ROS 2 动作通信

ROS 2 动作通信

概述

ROS2（Robot Operating System 2）是一种广泛应用于机器人领域的开源框架。动作（Action）是ROS2中用于处理长时间异步任务的通信机制。本文将介绍如何在ROS2中使用C++实现动作通信。

动作通信的基本概念

动作通信由三部分组成：

1. 目标（Goal）：要执行的任务。
2. 反馈（Feedback）：任务执行过程中的进展信息。
3. 结果（Result）：任务完成后的最终状态。

实现步骤

1. 创建自定义动作文件

在ROS2中，动作文件通常位于action目录中。假设我们创建一个名为Progress.action的动作文件。

# Progress.action

int64 num
---
int64 sum
---
float64 progress

2. 生成C++接口

在CMakeLists.txt中添加以下内容以生成C++接口：

rosidl_generate_interfaces(${PROJECT_NAME}
  "msg/PositionSpeed.msg"
  "srv/ComputeArea.srv"
  "srv/AddTwoInts.srv"
  "action/Factorial.action"
  "action/Count.action"
  "action/Move.action"
  //添加下面一行
  "action/Progress.action"
  DEPENDENCIES # Add packages that above messages depend on
)

运行colcon build以生成对应的代码。

3. 创建动作服务器

#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "my_custom_msgs/action/progress.hpp"
using my_custom_msgs::action::Progress;
using GoalHandleProgress = rclcpp_action::ClientGoalHandle<Progress>;
using namespace std::placeholders;

// 3.定义节点类；
class MinimalActionClient : public rclcpp::Node
{
    
    
public:  
explicit MinimalActionClient(const rclcpp::NodeOptions & node_options = rclcpp::NodeOptions())  
: Node("minimal_action_client", node_options)  
{
    
        
 // 3-1.创建动作客户端；    
 this->client_ptr_ = rclcpp_action::create_client<Progress>(this,"get_sum");    }  
 // 3-2.发送请求；  
 void send_goal(int64_t num)  
 {
    
          

   if (!this->client_ptr_) {
    
        
    RCLCPP_ERROR(this->get_logger(), "动作客户端未被初始化。");    
   }    

   if (!this->client_ptr_->wait_for_action_server(std::chrono::seconds(10))) {
    
          
     RCLCPP_ERROR(this->get_logger(), "服务连接失败！");      
     return;    
    }    
     auto goal_msg = Progress::Goal();    
     goal_msg.num = num;    
     RCLCPP_INFO(this->get_logger(), "发送请求数据！");    

      auto send_goal_options = rclcpp_action::Client<Progress>::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<Progress>::SharedPtr client_ptr_;

// 3-3.处理目标发送后的反馈；  
void goal_response_callback(GoalHandleProgress::SharedPtr goal_handle)  
{
    
        
if (!goal_handle) 
{
    
        
  RCLCPP_ERROR(this->get_logger(), "目标请求被服务器拒绝！");    
  } 
  else 
  {
    
         
    RCLCPP_INFO(this->get_logger(), "目标被接收，等待结果中");    
  }  
}  

// 3-4.处理连续反馈；  
void feedback_callback(GoalHandleProgress::SharedPtr,const std::shared_ptr<const Progress::Feedback> feedback)  
{
    
        
  int32_t progress = (int32_t)(feedback->progress * 100);      
  RCLCPP_INFO(this->get_logger(), "当前进度: %d%%", progress);  
 }

// 3-5.处理最终响应。  
void result_callback(const GoalHandleProgress::WrappedResult & result)  
{
    
        
 switch (result.code) {
    
      
    case rclcpp_action::ResultCode::SUCCEEDED:        
       break;      
    case rclcpp_action::ResultCode::ABORTED:      
       RCLCPP_ERROR(this->get_logger(), "任务被中止");        
       return;      
    case rclcpp_action::ResultCode::CANCELED:        
       RCLCPP_ERROR(this->get_logger(), "任务被取消");        
       return;      
     default:        
       RCLCPP_ERROR(this->get_logger(), "未知异常");        
       return;  
  }    
   RCLCPP_INFO(this->get_logger(), "任务执行完毕，最终结果: %d", result.result->sum);  
  }
};

int main(int argc, char ** argv)
{
    
      
// 2.初始化 ROS2 客户端；  
rclcpp::init(argc, argv);  
// 4.调用spin函数，并传入节点对象指针；  
auto action_client = std::make_shared<MinimalActionClient>();  
action_client->send_goal(10);  
rclcpp::spin(action_client);
// 5.释放资源。  
rclcpp::shutdown();
return 0;
} 

4. 创建动作客户端

// 1.包含头文件；
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "my_custom_msgs/action/progress.hpp"
using namespace std::placeholders;
using my_custom_msgs::action::Progress;
using GoalHandleProgress = rclcpp_action::ServerGoalHandle<Progress>;

// 3.定义节点类；
class MinimalActionServer : public rclcpp::Node
{
    
    
public:
explicit MinimalActionServer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions())  
: Node("minimal_action_server", options)
{
    
        
// 3-1.创建动作服务端；  
this->action_server_ = rclcpp_action::create_server<Progress>(      
 this,      
 "get_sum",      
 std::bind(&MinimalActionServer::handle_goal, this, _1, _2),      
 std::bind(&MinimalActionServer::handle_cancel, this, _1),      
 std::bind(&MinimalActionServer::handle_accepted, this, _1));    
RCLCPP_INFO(this->get_logger(),"动作服务端创建，等待请求...");  
}

private:  
rclcpp_action::Server<Progress>::SharedPtr action_server_;  
// 3-2.处理请求数据；  
rclcpp_action::GoalResponse handle_goal(const rclcpp_action::GoalUUID & uuid,std::shared_ptr<const Progress::Goal> goal)  
{
    
        
(void)uuid;    
RCLCPP_INFO(this->get_logger(), "接收到动作客户端请求，请求数字为 %ld", goal->num);    
if (goal->num < 1) {
    
          
 return rclcpp_action::GoalResponse::REJECT;    
}   
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;  
}  

// 3-3.处理取消任务请求；  
rclcpp_action::CancelResponse handle_cancel(    
 const std::shared_ptr<GoalHandleProgress> goal_handle)  
{
    
        
 (void)goal_handle;    
  RCLCPP_INFO(this->get_logger(), "接收到任务取消请求");    
  return rclcpp_action::CancelResponse::ACCEPT;  
}  

void execute(const std::shared_ptr<GoalHandleProgress> goal_handle)  
{
    
        
RCLCPP_INFO(this->get_logger(), "开始执行任务");    
rclcpp::Rate loop_rate(10.0);    
const auto goal = goal_handle->get_goal();    
auto feedback = std::make_shared<Progress::Feedback>();    
auto result = std::make_shared<Progress::Result>();    
int64_t sum= 0;    

for (int i = 1; (i <= goal->num) && rclcpp::ok(); i++) {
    
          
   sum += i;      
   // Check if there is a cancel request      
   if (goal_handle->is_canceling()) {
    
            
      result->sum = sum;        
      goal_handle->canceled(result);        
      RCLCPP_INFO(this->get_logger(), "任务取消");        
      return;      
      }      
      feedback->progress = (double_t)i / goal->num;      
      goal_handle->publish_feedback(feedback);      
      RCLCPP_INFO(this->get_logger(), "连续反馈中，进度：%.2f", feedback->progress);      
      loop_rate.sleep();    
     } 
     
if (rclcpp::ok()) {
    
     
result->sum = sum; 
goal_handle->succeed(result);
RCLCPP_INFO(this->get_logger(), "任务完成！");
}
}

// 3-4.生成连续反馈。  

void handle_accepted(const std::shared_ptr<GoalHandleProgress> goal_handle)
{
    
    
std::thread{
    
    std::bind(&MinimalActionServer::execute, this, _1), goal_handle}.detach();
}
};

int main(int argc, char ** argv)
{
    
    
// 2.初始化 ROS2 客户端；  
rclcpp::init(argc, argv);
// 4.调用spin函数，并传入节点对象指针；  
auto action_server = std::make_shared<MinimalActionServer>();
rclcpp::spin(action_server);
// 5.释放资源。  
rclcpp::shutdown();
return 0;
} 

结论

通过以上步骤，我们演示了如何在ROS2中使用C++实现动作通信。这种机制对于处理机器人复杂任务尤其有用，因为它提供了实时反馈和取消功能。希望这篇文章对你理解ROS2的动作通信有所帮助。