【ROS】ROS2编程示例：动作

1、准备

1）安装ROS2
【ROS】Ubuntu22.04安装ROS2（Humble Hawksbill）

2）ROS2命令
【ROS】ROS2命令行工具详解

3）配置工作空间
【ROS】ROS2中的概念和名词解释中第一节：工作空间 workspace

4）ROS2源码及官方示例：

https://github.com/ros2
https://github.com/ros2/examples

2、编辑

2.1 创建功能包

cd ~/ros/src
mkdir laoerExample
cd laoerExample
ros2 pkg create --build-type ament_cmake cAction --dependencies  rclcpp rclcpp_action example_interfaces

注意：创建功能包时一定要先创建好目录，进入相应的目录后创建，不要使用下面的方法，否则会在编译时报错

cd ~/ros/src
ros2 pkg create --build-type ament_cmake laoerExample/cAction

2.2 编辑源码

2.2.1 动作消息定义

动作消息引用ros源码自带的example_interfaces中的Fibonacci.action，内容如下：
cat /opt/ros/humble/share/example_interfaces/action$ cat Fibonacci.action

# Goal
int32 order
---
# Result
int32[] sequence
---
# Feedback
int32[] sequence

2.2.2 动作服务器

~/ros/src/laoerExample/cAction/src$ vi member_functions_srv.cpp 

#include 
#include 
#include 

#include "example_interfaces/action/fibonacci.hpp"
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"

# a）自定义类MinimalActionServer，继承自节点类rclcpp::Node
class MinimalActionServer : public rclcpp::Node
{
public:
  # b）using重命名Fibonacci动作、GoalHandleFibonacci目标交互类
  using Fibonacci = example_interfaces::action::Fibonacci;
  using GoalHandleFibonacci = rclcpp_action::ServerGoalHandle<Fibonacci>;

  # c）构造函数：构造时给节点命名为"minimal_action_server"
  explicit MinimalActionServer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions())
  : Node("minimal_action_server", options)
  {
    # d）std::placeholders占为符，用于std::bind函数最后的参数 std::placeholders::_1
    using namespace std::placeholders;
	
	# e）创建服务，参数有好多个，以后用多了会慢慢习惯，
	# 前四个基本为固定的（设置时钟、日志等），
	# 第五个为动作名称
	# 后三个为回调函数：接受、取消、执行
    this->action_server_ = rclcpp_action::create_server<Fibonacci>(
      this->get_node_base_interface(),
      this->get_node_clock_interface(),
      this->get_node_logging_interface(),
      this->get_node_waitables_interface(),
      "fibonacci",
      # f）std::bind就是封装下函数，成为函数对象，这样就可以作为参数传递了
      std::bind(&MinimalActionServer::handle_goal, this, _1, _2),
      std::bind(&MinimalActionServer::handle_cancel, this, _1),
      std::bind(&MinimalActionServer::handle_accepted, this, _1));
  }

private:
  rclcpp_action::Server<Fibonacci>::SharedPtr action_server_;

  # g）每当有客户端接入时，先调同这个，表示收到命令，返回是否执行
  rclcpp_action::GoalResponse handle_goal(
    const rclcpp_action::GoalUUID & uuid,
    std::shared_ptr<const Fibonacci::Goal> goal)
  {
    RCLCPP_INFO(this->get_logger(), "Received goal request with order %d", goal->order);
    (void)uuid;
    # h） 限制斐波那契数列计算次数9000
    if (goal->order > 9000) {
      return rclcpp_action::GoalResponse::REJECT;
    }
    return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
  }

  # i）如果客户端主动取消，调用这个函数
  rclcpp_action::CancelResponse handle_cancel(
    const std::shared_ptr<GoalHandleFibonacci> goal_handle)
  {
    RCLCPP_INFO(this->get_logger(), "Received request to cancel goal");
    (void)goal_handle;
    return rclcpp_action::CancelResponse::ACCEPT;
  }

 # j）执行函数，需要将这个函数放入单独的线程中
  void execute(const std::shared_ptr<GoalHandleFibonacci> goal_handle)
  {
    RCLCPP_INFO(this->get_logger(), "Executing goal");
    rclcpp::Rate loop_rate(1);
    const auto goal = goal_handle->get_goal();
    auto feedback = std::make_shared<Fibonacci::Feedback>();
    auto & sequence = feedback->sequence;
    sequence.push_back(0);
    sequence.push_back(1);
    auto result = std::make_shared<Fibonacci::Result>();

    for (int i = 1; (i < goal->order) && rclcpp::ok(); ++i) {
      if (goal_handle->is_canceling()) {
        result->sequence = sequence;
        goal_handle->canceled(result);
        RCLCPP_INFO(this->get_logger(), "Goal Canceled");
        return;
      }
      
      sequence.push_back(sequence[i] + sequence[i - 1]);
      // Publish feedback
      goal_handle->publish_feedback(feedback);
      RCLCPP_INFO(this->get_logger(), "Publish Feedback");

      loop_rate.sleep();
    }

    if (rclcpp::ok()) {
      result->sequence = sequence;
      goal_handle->succeed(result);
      RCLCPP_INFO(this->get_logger(), "Goal Succeeded");
    }
  }

  void handle_accepted(const std::shared_ptr<GoalHandleFibonacci> goal_handle)
  {
    using namespace std::placeholders;
    # k）这需要快速返回以避免阻塞执行器，因此启动一个新线程
    std::thread{std::bind(&MinimalActionServer::execute, this, _1), goal_handle}.detach();
  }
};  // class MinimalActionServer

int main(int argc, char ** argv)
{
  # A）初始化
  rclcpp::init(argc, argv);
  # B）创建服务并启动
  auto action_server = std::make_shared<MinimalActionServer>();
  # C）spin()创建一个单线程执行器，处理回调任务，服务于指定的节点。它生效后，周期运行。无限阻塞状态。
  rclcpp::spin(action_server);
  # D）关闭
  rclcpp::shutdown();
  return 0;
}

2.2.2 动作客户端

~/ros/src/laoerExample/cAction/src$ cat member_functions_cli.cpp

#include 
#include 
#include 
#include 
#include 
#include 

#include "example_interfaces/action/fibonacci.hpp"
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"

class MinimalActionClient : public rclcpp::Node
{
public:
  # a）重命名
  using Fibonacci = example_interfaces::action::Fibonacci;
  using GoalHandleFibonacci = rclcpp_action::ClientGoalHandle<Fibonacci>;

 # b）构造函数：设置节点名为"minimal_action_client"
  explicit MinimalActionClient(const rclcpp::NodeOptions & node_options = rclcpp::NodeOptions())
  : Node("minimal_action_client", node_options), goal_done_(false)
  {
    # c）创建动作客户端
    this->client_ptr_ = rclcpp_action::create_client<Fibonacci>(
      this->get_node_base_interface(),
      this->get_node_graph_interface(),
      this->get_node_logging_interface(),
      this->get_node_waitables_interface(),
      "fibonacci");

	# d）create_wall_timer：定时器，定时500毫米，执行send_goal函数
    this->timer_ = this->create_wall_timer(
      std::chrono::milliseconds(500),
      std::bind(&MinimalActionClient::send_goal, this));
  }

  bool is_goal_done() const
  {
    return this->goal_done_;
  }

  void send_goal()
  {
    using namespace std::placeholders;
	
	# e）先定制定时，再执行下面的动作
    this->timer_->cancel();

    this->goal_done_ = false;
    if (!this->client_ptr_) {
      RCLCPP_ERROR(this->get_logger(), "Action client not initialized");
    }
	
	# f）等待服务端启动
    if (!this->client_ptr_->wait_for_action_server(std::chrono::seconds(10))) {
      RCLCPP_ERROR(this->get_logger(), "Action server not available after waiting");
      this->goal_done_ = true;
      return;
    }

    auto goal_msg = Fibonacci::Goal();
    goal_msg.order = 10;

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

    # g）设置回调函数：goal_response_callback、feedback_callback、result_callback
    # action 由三个部分组成：目标(goal)、反馈（feedback）和结果（result）。
    auto send_goal_options = rclcpp_action::Client<Fibonacci>::SendGoalOptions();
    send_goal_options.goal_response_callback =
      std::bind(&MinimalActionClient::goal_response_callback, this, _1);
    send_goal_options.feedback_callback =
      std::bind(&MinimalActionClient::feedback_callback, this, _1, _2);
    send_goal_options.result_callback =
      std::bind(&MinimalActionClient::result_callback, this, _1);
    auto goal_handle_future = this->client_ptr_->async_send_goal(goal_msg, send_goal_options);
  }

private:
  rclcpp_action::Client<Fibonacci>::SharedPtr client_ptr_;
  rclcpp::TimerBase::SharedPtr timer_;
  bool goal_done_;

  void goal_response_callback(GoalHandleFibonacci::SharedPtr goal_handle)
  {
    if (!goal_handle) {
      RCLCPP_ERROR(this->get_logger(), "Goal was rejected by server");
    } else {
      RCLCPP_INFO(this->get_logger(), "Goal accepted by server, waiting for result");
    }
  }

  void feedback_callback(
    GoalHandleFibonacci::SharedPtr,
    const std::shared_ptr<const Fibonacci::Feedback> feedback)
  {
    RCLCPP_INFO(
      this->get_logger(),
      "Next number in sequence received: %" PRId32,
      feedback->sequence.back());
  }

  void result_callback(const GoalHandleFibonacci::WrappedResult & result)
  {
    this->goal_done_ = 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->sequence) {
      RCLCPP_INFO(this->get_logger(), "%" PRId32, number);
    }
  }
};  // class MinimalActionClient

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

  while (!action_client->is_goal_done()) {
    # h）rclcpp::spin_some在线程中只执行一次？
    rclcpp::spin_some(action_client);
  }

  rclcpp::shutdown();
  return 0;
}

2.3 添加依赖配置

如果在创建包时已经指定：

ros2 pkg create --build-type ament_cmake cAction --dependencies  rclcpp rclcpp_action example_interfaces

则下面的内容会自动添加在package.xml中

  <depend>rclcppdepend>
  <depend>rclcpp_actiondepend>
  <depend>example_interfacesdepend>

完整的内容如下

~/ros/src/laoerExample/cAction$ cat package.xml 

<package format="3">
  <name>cActionname>
  <version>0.0.0version>
  <description>TODO: Package descriptiondescription>
  <maintainer email="[email protected]">lesenmaintainer>
  <license>TODO: License declarationlicense>

  <buildtool_depend>ament_cmakebuildtool_depend>

  <depend>rclcppdepend>
  <depend>example_interfacesdepend>
  <depend>rclcpp_actiondepend>

  <test_depend>ament_lint_autotest_depend>
  <test_depend>ament_lint_commontest_depend>

  <export>
    <build_type>ament_cmakebuild_type>
  export>
package>

2.4 修改编译配置

1）添加依赖包

find_package(rclcpp REQUIRED)
find_package(rclcpp_action REQUIRED)
find_package(example_interfaces REQUIRED)

2）添加编译可执行程序及对应的源码

add_executable(action_client_member_functions src/member_functions_cli.cpp)
ament_target_dependencies(action_client_member_functions
  "rclcpp"
  "rclcpp_action"
  "example_interfaces")
  
add_executable(action_server_member_functions src/member_functions_srv.cpp)
ament_target_dependencies(action_server_member_functions
  "rclcpp"
  "rclcpp_action"
  "example_interfaces")

3）添加安装规则

install(TARGETS
  action_client_member_functions
  action_server_member_functions
  DESTINATION lib/${PROJECT_NAME})

4）完整的 CMakeLists.txt 内容如下

~/ros/src/laoerExample/cAction$ cat CMakeLists.txt 

cmake_minimum_required(VERSION 3.8)
project(cAction)

# Default to C++17
if(NOT CMAKE_CXX_STANDARD)
  set(CMAKE_CXX_STANDARD 17)
  set(CMAKE_CXX_STANDARD_REQUIRED ON)
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(rclcpp REQUIRED)
find_package(rclcpp_action REQUIRED)
find_package(example_interfaces REQUIRED)

if(BUILD_TESTING)
  find_package(ament_lint_auto REQUIRED)
  set(ament_cmake_copyright_FOUND TRUE)
  set(ament_cmake_cpplint_FOUND TRUE)
  ament_lint_auto_find_test_dependencies()
endif()

add_executable(action_client_member_functions src/member_functions_cli.cpp)
ament_target_dependencies(action_client_member_functions
  "rclcpp"
  "rclcpp_action"
  "example_interfaces")

add_executable(action_server_member_functions src/member_functions_srv.cpp)
ament_target_dependencies(action_server_member_functions
  "rclcpp"
  "rclcpp_action"
  "example_interfaces")

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

install(TARGETS
  action_client_member_functions
  action_server_member_functions
  DESTINATION lib/${PROJECT_NAME})

ament_package()

3、编译

在工作空间根目录中编译：

cd ~/ros
~/ros$ colcon build

4、运行

在终端1中运行服务端：

source install/setup.sh 
ros2 run  cAction action_server_member_functions 

在终端2中运行客户端：

source install/setup.sh 
ros2 run  cAction action_client_member_functions

终端1服务端打印信息如下：

~$ ros2 run  cAction action_server_member_functions 
[INFO] [1686017247.134106327] [minimal_action_server]: Received goal request with order 10
[INFO] [1686017247.134329634] [minimal_action_server]: Executing goal
[INFO] [1686017247.134416312] [minimal_action_server]: Publish Feedback
……
[INFO] [1686017255.134476702] [minimal_action_server]: Publish Feedback
[INFO] [1686017256.134540042] [minimal_action_server]: Goal Succeeded

终端2客户端打印信息如下：

~$ ros2 run cAction action_client_member_functions 
[INFO] [1686017247.133888720] [minimal_action_client]: Sending goal
[INFO] [1686017247.134327098] [minimal_action_client]: Goal accepted by server, waiting for result
[INFO] [1686017247.134478858] [minimal_action_client]: Next number in sequence received: 1
……
[INFO] [1686017255.134584821] [minimal_action_client]: Next number in sequence received: 55
[INFO] [1686017256.134662635] [minimal_action_client]: Result received
[INFO] [1686017256.134692702] [minimal_action_client]: 0
……
[INFO] [1686017256.134740934] [minimal_action_client]: 55

5、测试

1）查看节点信息
在终端3中查看节点信息，运行命令：ros2 node list

$ ros2 node list 
/minimal_action_client
/minimal_action_server

2）查看动作信息
在终端3中查看话题信息，运行命令：ros2 action list

$ ros2 action list 
/fibonacci