[ROS2] --- action

1 action介绍

ROS通信机制也会被常常用到——那就是动作。从这个名字上就可以很好理解这个概念的含义，这种通信机制的目的就是便于对机器人某一完整行为的流程进行管理。

1.1 客户端/服务器模型

动作和服务类似，使用的也是客户端和服务器模型，客户端发送动作的目标，想让机器人干什么，服务器端执行动作过程， 控制机器人达到运动的目标，同时周期反馈动作执行过程中的状态。

客户端发送一个运动的目标，想让机器人动起来，服务器端收到之后，就开始控制机器人运动，一边运动，一边反馈当前的状态，如果是一个导航动作，这个反馈可能是当前所处的坐标，如果是机械臂抓取，这个反馈可能又是机械臂的实时姿态。当运动执行结束后，服务器再反馈一个动作结束的信息。整个通信过程就此结束。

1.2 action通信特点

• 一对多通信
  和服务一样，动作通信中的客户端可以有多个，大家都可以发送运动命令，但是服务器端只能有一个，毕竟只有一个机器人，先执行完成一个动作，才能执行下一个动作。

• 同步通信
  既然有反馈，那动作也是一种同步通信机制，之前我们也介绍过，动作过程中的数据通信接口，使用.action文件进行定义。

• 由服务和话题合成
  大家再仔细看下上边的动图，是不是还会发现一个隐藏的秘密。
  动作的三个通信模块，竟然有两个是服务，一个是话题，当客户端发送运动目标时，使用的是服务的请求调用，服务器端也会反馈一个应带，表示收到命令。动作的反馈过程，其实就是一个话题的周期发布，服务器端是发布者，客户端是订阅者。
  没错，动作是一种应用层的通信机制，其底层就是基于话题和服务来实现的。

2 action自定义通信接口

延续上一讲[ROS2] — action，中创建的自定义接口功能包，在src目录下创建action/Concatenate.action文件
Concatenate.action

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

3 action编码示例

这里创建功能包名为，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 编译运行

# 编译
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常用指令

# 查看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