[ROS2] --- action

1 action introduction

The ROS communication mechanism is also often used - that is, actions. The meaning of this concept can be easily understood from the name. The purpose of this communication mechanism is to facilitate the management of a complete behavior process of the robot.

1.1 Client/Server Model

Actions are similar to services. They also use client and server models. The client sends the target of the action and what it wants the robot to do. The server executes the action process and controls the robot to achieve the target of movement. At the same time, it periodically feeds back the status during the execution of the action.

The client sends a moving target and wants the robot to move. After the server receives it, it starts to control the robot's movement. While moving, it feeds back the current status. If it is a navigation action, this feedback may be the current coordinates. , if it is a robotic arm grabbing, this feedback may be the real-time posture of the robotic arm. When the movement execution is completed, the server feeds back a message indicating the end of the action. The entire communication process ends here.

1.2 Action communication characteristics

• One-to-many communication
  is the same as service. There can be multiple clients in action communication. Everyone can send motion commands, but there can only be one server. After all, there is only one robot. One action must be completed before the next action can be executed. .

• Synchronous communication
  Since there is feedback, the action is also a synchronous communication mechanism. We have also introduced before that the data communication interface during the action is defined using the .action file.

• Synthesized by services and topics.
  If you take a closer look at the above animation, will you find a hidden secret?
  Among the three communication modules of the action, two are services and one is topic. When the client sends a moving target, it uses the service request call, and the server will also feedback a response to indicate that the command has been received. The action feedback process is actually the periodic publishing of a topic. The server is the publisher and the client is the subscriber.
  That's right, action is an application layer communication mechanism, and its bottom layer is implemented based on topics and services.

2 action custom communication interface

Continuing the previous lecture [ROS2] - action, create a custom interface function package in the src directory and create the action/Concatenate.action file
Concatenate.action in the src directory.

int16 num_concatenations
---
string final_concatenation
---
string partial_concatenation

3 action coding examples

The function package created here is named, learning04_action

3.1 class_action_client.cpp

/**
 * @file class_action_client.cpp
 *
 * @brief A class defined ROS2 action client node that sends as goal the number of string
 *        concatenation the action server should perform. 
 *        The server will send back feedbacks and the final result
 *
 * @author Antonio Mauro Galiano
 * Contact: https://www.linkedin.com/in/antoniomaurogaliano/
 *
 */


#include "custom_interface/action/concatenate.hpp"
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"


class ConcatenateActionClient : public rclcpp::Node
{
public:
  using Concatenate = custom_interface::action::Concatenate;
  using GoalHandleConcatenate = rclcpp_action::ClientGoalHandle<Concatenate>;

  explicit ConcatenateActionClient(const rclcpp::NodeOptions & node_options = rclcpp::NodeOptions())
    : Node("class_action_client", node_options), goalDone_(false)
  {
    this->clientPtr_ = rclcpp_action::create_client<Concatenate>(
      this->get_node_base_interface(),
      this->get_node_graph_interface(),
      this->get_node_logging_interface(),
      this->get_node_waitables_interface(),
      "concatenation");

    this->timer_ = this->create_wall_timer(
      std::chrono::milliseconds(500),
      std::bind(&ConcatenateActionClient::SendGoal, this));
  }

  bool GoalDone() const;
  void SendGoal();

private:
  rclcpp_action::Client<Concatenate>::SharedPtr clientPtr_;
  rclcpp::TimerBase::SharedPtr timer_;
  bool goalDone_;

  void FeedbackCallback(
    GoalHandleConcatenate::SharedPtr,
    const std::shared_ptr<const Concatenate::Feedback> feedback);
  void ResultCallback(const GoalHandleConcatenate::WrappedResult & result);
  // be sure to define the parameter as it's here
  // more info at the declaration
  void GoalResponseCallback(const GoalHandleConcatenate::SharedPtr &goalHandle);
};


bool ConcatenateActionClient::GoalDone() const
{
  return this->goalDone_;
}


void ConcatenateActionClient::SendGoal()
{
  using namespace std::placeholders;

  this->timer_->cancel();

  this->goalDone_ = false;

  if (!this->clientPtr_)
  {
    RCLCPP_ERROR(this->get_logger(), "Action client not initialized");
  }

  if (!this->clientPtr_->wait_for_action_server(std::chrono::seconds(10)))
  {
    RCLCPP_ERROR(this->get_logger(), "!!ATTENTION!! Action server not available");
    this->goalDone_ = true;
    return;
  }

  auto goalMsg = Concatenate::Goal();
  goalMsg.num_concatenations = 9;

  RCLCPP_INFO(this->get_logger(), "Sending goal");

  auto send_goal_options = rclcpp_action::Client<Concatenate>::SendGoalOptions();
  send_goal_options.feedback_callback =
    std::bind(&ConcatenateActionClient::FeedbackCallback, this, _1, _2);
  send_goal_options.result_callback =
    std::bind(&ConcatenateActionClient::ResultCallback, this, _1);
  // send_goal_options.goal_response_callback =
  //   std::bind(&ConcatenateActionClient::GoalResponseCallback, this, _1);
  auto goal_handle_future = this->clientPtr_->async_send_goal(goalMsg, send_goal_options);
}


void ConcatenateActionClient::FeedbackCallback(
  rclcpp_action::ClientGoalHandle<Concatenate>::SharedPtr,
  const std::shared_ptr<const Concatenate::Feedback> feedback)
{
  RCLCPP_INFO(this->get_logger(),
    "Feedback received: %s",
    feedback->partial_concatenation.c_str());
}


void ConcatenateActionClient::ResultCallback(const GoalHandleConcatenate::WrappedResult & result)
{
  this->goalDone_ = true;
  switch (result.code) {
    case rclcpp_action::ResultCode::SUCCEEDED:
      break;
    case rclcpp_action::ResultCode::ABORTED:
      RCLCPP_ERROR(this->get_logger(), "Goal was aborted");
      return;
    case rclcpp_action::ResultCode::CANCELED:
      RCLCPP_ERROR(this->get_logger(), "Goal was canceled");
      return;
    default:
      RCLCPP_ERROR(this->get_logger(), "Unknown result code");
      return;
  }

  RCLCPP_INFO(this->get_logger(), "Result received");
  for (auto number : result.result->final_concatenation)
  {
    RCLCPP_INFO(this->get_logger(), "%d", number);
  }
}

// defining the parameter directly as a GoalHandleConcatenate::SharedPtr goalHandle
// it's wrong for the send_goal_options.goal_response_callback
// so it doesnt compile
void ConcatenateActionClient::GoalResponseCallback(
  const GoalHandleConcatenate::SharedPtr &goalHandle)
  {
    if (!goalHandle)
    {
      RCLCPP_ERROR(this->get_logger(), "Goal was rejected by server");
    } else
    {
      RCLCPP_INFO(this->get_logger(), "Goal accepted by server, waiting for result");
    }
  }


int main(int argc, char ** argv)
{
  rclcpp::init(argc, argv);
  auto action_client = std::make_shared<ConcatenateActionClient>();

  while (!action_client->GoalDone())
  {
    rclcpp::spin_some(action_client);
  }

  rclcpp::shutdown();
  return 0;
}

3.2 class_action_server.cpp

/**
 * @file class_action_server.cpp
 *
 * @brief A class defined ROS2 action server node that concatenates a string 
 *         based on the number of string concatenation sent by a client within a goal request
 *
 * @author Antonio Mauro Galiano
 * Contact: https://www.linkedin.com/in/antoniomaurogaliano/
 *
 */


#include "custom_interface/action/concatenate.hpp"
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"


class ConcatenateActionServer : public rclcpp::Node
{
public:
  using Concatenate = custom_interface::action::Concatenate;
  using GoalHandleConcatenate = rclcpp_action::ServerGoalHandle<Concatenate>;

  explicit ConcatenateActionServer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions())
    : Node("class_action_server", options)
  {
    using namespace std::placeholders;

    this->actionServer_ = rclcpp_action::create_server<Concatenate>(
      this->get_node_base_interface(),
      this->get_node_clock_interface(),
      this->get_node_logging_interface(),
      this->get_node_waitables_interface(),
      "concatenation",
      std::bind(&ConcatenateActionServer::HandleGoal, this, _1, _2),
      std::bind(&ConcatenateActionServer::HandleCancel, this, _1),
      std::bind(&ConcatenateActionServer::HandleAccepted, this, _1));
  }

private:
  rclcpp_action::Server<Concatenate>::SharedPtr actionServer_;
  rclcpp_action::GoalResponse HandleGoal(
    const rclcpp_action::GoalUUID & uuid,
    std::shared_ptr<const Concatenate::Goal> goal);
  rclcpp_action::CancelResponse HandleCancel(
    const std::shared_ptr<GoalHandleConcatenate> goalHandle);
  void execute(const std::shared_ptr<GoalHandleConcatenate> goalHandle);
  void HandleAccepted(const std::shared_ptr<ConcatenateActionServer::GoalHandleConcatenate> goalHandle);
};


rclcpp_action::GoalResponse ConcatenateActionServer::HandleGoal(
    const rclcpp_action::GoalUUID & uuid,
    std::shared_ptr<const Concatenate::Goal> goal)
{
  RCLCPP_INFO(rclcpp::get_logger("server"),
              "Got goal request with %d string concatenations",
              goal->num_concatenations);
  (void)uuid;
  // conditional to reject numbers of concatenations
  if ((goal->num_concatenations > 10) && (goal->num_concatenations < 2)) 
  {
    return rclcpp_action::GoalResponse::REJECT;
  }
  return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
}


rclcpp_action::CancelResponse ConcatenateActionServer::HandleCancel(
    const std::shared_ptr<GoalHandleConcatenate> goalHandle)
{
  RCLCPP_INFO(rclcpp::get_logger("server"), "Got request to cancel goal");
  (void)goalHandle;
  return rclcpp_action::CancelResponse::ACCEPT;
}


void ConcatenateActionServer::execute(
  const std::shared_ptr<GoalHandleConcatenate> goalHandle)
{
  RCLCPP_INFO(rclcpp::get_logger("server"), "Executing the concatenation");
  rclcpp::Rate loop_rate(1);
  const auto goal = goalHandle->get_goal();
  auto feedback = std::make_shared<Concatenate::Feedback>();
  std::string myString = "HELLOWORLD";
  auto &concatenation = feedback->partial_concatenation;
  concatenation = myString;
  concatenation = concatenation + " " + myString;
  auto result = std::make_shared<Concatenate::Result>();

  for (int i = 1; (i < goal->num_concatenations) && rclcpp::ok(); ++i)
  {
    // check if there is a cancel request
    if (goalHandle->is_canceling())
    {
      result->final_concatenation = concatenation;
      goalHandle->canceled(result);
      RCLCPP_INFO(rclcpp::get_logger("server"), "Goal Canceled");
      return;
    }
    // update the final concatenation
    concatenation = concatenation + " " + myString;
    // update and publish feedback of the partial concatenation
    goalHandle->publish_feedback(feedback);
    RCLCPP_INFO(rclcpp::get_logger("server"), "Publish Feedback");

    loop_rate.sleep();
  }

  // check if goal is done
  if (rclcpp::ok())
  {
    result->final_concatenation = concatenation;
    goalHandle->succeed(result);
    RCLCPP_INFO(rclcpp::get_logger("server"), "Goal Succeeded");
  }
}


void ConcatenateActionServer::HandleAccepted(
  const std::shared_ptr<GoalHandleConcatenate> goal_handle)
{
  using namespace std::placeholders;
  // this needs to return quickly to avoid blocking the executor, so spin up a new thread
  std::thread{std::bind(&ConcatenateActionServer::execute, this, _1), goal_handle}.detach();
}


int main(int argc, char ** argv)
{
  rclcpp::init(argc, argv);

  auto action_server = std::make_shared<ConcatenateActionServer>();

  rclcpp::spin(action_server);
  rclcpp::shutdown();
  return 0;
}

3.3 CMakeLists.txt

cmake_minimum_required(VERSION 3.5)
project(learning04_action)

# Default to C99
if(NOT CMAKE_C_STANDARD)
  set(CMAKE_C_STANDARD 99)
endif()

# Default to C++14
if(NOT CMAKE_CXX_STANDARD)
  set(CMAKE_CXX_STANDARD 14)
endif()

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic)
endif()

# find dependencies
find_package(ament_cmake REQUIRED)
find_package(custom_interface REQUIRED)
find_package(rclcpp REQUIRED)
find_package(rclcpp_action REQUIRED)
find_package(rclcpp_components REQUIRED)

# add_executable(simple_action_server src/simple_action_server.cpp)
# ament_target_dependencies(simple_action_server
#   "rclcpp"
#   "rclcpp_action"
#   "custom_interface")

add_executable(class_action_server src/class_action_server.cpp)
ament_target_dependencies(class_action_server
  "rclcpp"
  "rclcpp_action"
  "custom_interface")

# add_executable(simple_action_client src/simple_action_client.cpp)
# ament_target_dependencies(simple_action_client
#   "rclcpp"
#   "rclcpp_action"
#   "custom_interface")

add_executable(class_action_client src/class_action_client.cpp)
ament_target_dependencies(class_action_client
  "rclcpp"
  "rclcpp_action"
  "custom_interface")

if(BUILD_TESTING)
  find_package(ament_lint_auto REQUIRED)
  ament_lint_auto_find_test_dependencies()
endif()

install(TARGETS
  # simple_action_server
	class_action_server
	# simple_action_client
  class_action_client
  DESTINATION lib/${PROJECT_NAME})

ament_package()

3.4 package.xml

<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
  <name>learning04_action</name>
  <version>0.0.0</version>
  <description>Action based tutorial</description>
  <maintainer email="[email protected]">Antonio Mauro Galiano</maintainer>
  <license>TODO: License declaration</license>

  <buildtool_depend>ament_cmake</buildtool_depend>

  <depend>custom_interface</depend>
  <depend>rclcpp</depend>
  <depend>rclcpp_action</depend>
  <depend>rclcpp_components</depend>

  <test_depend>ament_lint_auto</test_depend>
  <test_depend>ament_lint_common</test_depend>

  <export>
    <build_type>ament_cmake</build_type>
  </export>
</package>

4 Compile and run

# 编译
colcon build

# source环境变量
source install/setup.sh

# 运行publisher
ros2 run learning04_action class_action_client

# 运行subsriber
ros2 run learning04_action class_action_server

5 action common instructions

# 查看action信息
ros2 action info /turtle/roate_absolute

# 设置action通信结构
ros2 interface show /turtle/roate_absolute

# 发送action请求
ros2 action send_goal <action_name> <action_type> <values>
ros2 action send_goal /turtle/roate_absolute turtlesim/action/RotateAbsolute "{theta: 1.57}"
ros2 action send_goal /turtle/roate_absolute turtlesim/action/RotateAbsolute "{theta: 1.57}" --feedback