ROS2 动作通信——自定义动作接口（C++）

实验目的：

         自定义动作接口，要求动作客户端发送一个整型数据给服务端，动作服务端进行累加求和，在累加过程中连续反馈进度，最后反馈结果给客户端。

1、创建工作空间和自定义接口功能包

        请参考：ROS2 话题通信——自定义消息类型（C++）-CSDN博客 

2、自定义动作接口

        在interface_pkg功能包中新建action文件夹，用于存放动作接口文件，在action目录中创建Progress.action文件 

mkdir -p ./text_w/src/interface_pkg/action
touch ./text_w/src/interface_pkg/action/Progress.action

       在Progress.action文件定义以下数据格式 

int32 num
---
int32 sum
---
float64 progress

        修改interface_pkg中的CMakeLists.txt文件

        修改interface_pkg中的package.xml文件 

  <!--action构建工具依赖-->
  <buildtool_depend>rosidl_default_generators</buildtool_depend>
  <depend>action_msgs</depend>

         保存然后编译

cd text_w/
colcon build --packages-select interface_pkg 

        在install目录下面生成了我们所需要的文件，即编译成功 

        创建动作功能包action_pkg，新建动作服务端和动作客户端节点

cd ./text_w/src/
ros2 pkg create action_pkg --build-type ament_cmake --dependencies rclcpp rclcpp_action interface_pkg --node-name action_server
touch ./action_pkg/src/action_client.cpp

        编写动作服务端节点

//1.包含头文件；
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "interface_pkg/action/progress.hpp"


using namespace std::placeholders;
using interface_pkg::action::Progress;
using GoalHandleProgress = rclcpp_action::ServerGoalHandle<Progress>;

//2.定义节点类；
class ProgressActionServer : public rclcpp::Node
{
    public:
        ProgressActionServer() : Node("action_server")
        {    
            // 3-1.创建动作服务端；  
            this->action_server_ = rclcpp_action::create_server<Progress>(      
                this,      
                "get_sum",      
                std::bind(&ProgressActionServer::handle_goal, this, _1, _2),      
                std::bind(&ProgressActionServer::handle_cancel, this, _1),      
                std::bind(&ProgressActionServer::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(), "接收到动作客户端请求，请求数字为 %d", goal->num);    
            if (goal->num < 1) {      
                RCLCPP_INFO(this->get_logger(), "提交的目标值必须大于等于0!");
                return rclcpp_action::GoalResponse::REJECT;    
            }   
            RCLCPP_INFO(this->get_logger(), "提交的目标值合法!");
            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;  
        }  

        // 3-4.生成连续反馈(回调函数)。  
        void handle_accepted(const std::shared_ptr<GoalHandleProgress> goal_handle)
        {
            //新建子线程处理耗时主逻辑
            std::thread{std::bind(&ProgressActionServer::execute, this, _1), goal_handle}.detach();
        }

        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(), "任务完成！");
            }
        }

};


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

        编写动作客户端节点

#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "interface_pkg/action/progress.hpp"


using interface_pkg::action::Progress;
using GoalHandleProgress = rclcpp_action::ClientGoalHandle<Progress>;
using namespace std::placeholders;

// 定义节点类；
class ProgressActionClient : public rclcpp::Node
{
    public:  
        ProgressActionClient() : Node("action_client")  
        {    
            // 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(&ProgressActionClient::goal_response_callback, this, _1);    
            send_goal_options.feedback_callback =std::bind(&ProgressActionClient::feedback_callback, this, _1, _2);    
            send_goal_options.result_callback =std::bind(&ProgressActionClient::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<ProgressActionClient>();  
    action_client->send_goal(10);  
    rclcpp::spin(action_client);
    // 5.释放资源。  
    rclcpp::shutdown();
    return 0;
} 

        修改CMakeLists.txt

        分别开两个终端先编译一下，然后运行服务端和客户端节点

colcon build --packages-select action_pkg
source install/setup.bash 
ros2 run action_pkg action_server
 
 
source install/setup.bash 
ros2 run action_pkg action_client

        测试成功。