ROS2核心概念之代码示例三

上一节写到service，这一节写Action。

ROS2核心概念代码

Action

class MoveCircleActionServer : public rclcpp::Node
{
    public:
        // 定义一个自定义的动作接口类，便于后续使用
        using CustomAction = learning_interface::action::MoveCircle;
        // 定义一个处理动作请求、取消、执行的服务器端
        using GoalHandle = rclcpp_action::ServerGoalHandle<CustomAction>;

        explicit MoveCircleActionServer(const rclcpp::NodeOptions & action_server_options = rclcpp::NodeOptions())
        : Node("action_move_server", action_server_options)                                     // ROS2节点父类初始化
        {
            using namespace std::placeholders;

            this->action_server_ = rclcpp_action::create_server<CustomAction>(                  // 创建动作服务器（接口类型、动作名、回调函数）
                this->get_node_base_interface(),
                this->get_node_clock_interface(),
                this->get_node_logging_interface(),
                this->get_node_waitables_interface(),
                "move_circle",
                std::bind(&MoveCircleActionServer::handle_goal, this, _1, _2),
                std::bind(&MoveCircleActionServer::handle_cancel, this, _1),
                std::bind(&MoveCircleActionServer::handle_accepted, this, _1));
        }

    private:
        rclcpp_action::Server<CustomAction>::SharedPtr action_server_;  // 动作服务器

        // 响应动作目标的请求
        rclcpp_action::GoalResponse handle_goal(
            const rclcpp_action::GoalUUID & uuid,
            std::shared_ptr<const CustomAction::Goal> goal_request)                             
        {
            RCLCPP_INFO(this->get_logger(), "Server: Received goal request: %d", goal_request->enable);
            (void)uuid;

            // 如请求为enable则接受运动请求，否则就拒绝
            if (goal_request->enable)                                                           
            {
                return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
            }
            else
            {
                return rclcpp_action::GoalResponse::REJECT;
            }
        }

        // 响应动作取消的请求
        rclcpp_action::CancelResponse handle_cancel(
            const std::shared_ptr<GoalHandle> goal_handle_canceled_)                            
        {
            RCLCPP_INFO(this->get_logger(), "Server: Received request to cancel action");
            (void) goal_handle_canceled_; 
            return rclcpp_action::CancelResponse::ACCEPT;
        }

        // 处理动作接受后具体执行的过程
        void handle_accepted(const std::shared_ptr<GoalHandle> goal_handle_accepted_)          
        {
            using namespace std::placeholders;
            // 在线程中执行动作过程
            std::thread{std::bind(&MoveCircleActionServer::execute, this, _1), goal_handle_accepted_}.detach(); 
        }


        void execute(const std::shared_ptr<GoalHandle> goal_handle_)
        {
            const auto requested_goal = goal_handle_->get_goal();       // 动作目标
            auto feedback = std::make_shared<CustomAction::Feedback>(); // 动作反馈
            auto result = std::make_shared<CustomAction::Result>();     // 动作结果

            RCLCPP_INFO(this->get_logger(), "Server: Executing goal");
            rclcpp::Rate loop_rate(1);

            // 动作执行的过程
            for (int i = 0; (i < 361) && rclcpp::ok(); i=i+30)
            {
                // 检查是否取消动作
                if (goal_handle_->is_canceling())
                {
                    result->finish = false;
                    goal_handle_->canceled(result);
                    RCLCPP_INFO(this->get_logger(), "Server: Goal canceled");
                    return;
                }

                // 更新反馈状态
                feedback->state = i;
                // 发布反馈状态
                goal_handle_->publish_feedback(feedback);
                RCLCPP_INFO(this->get_logger(), "Server: Publish feedback");

                loop_rate.sleep();
            }

            // 动作执行完成
            if (rclcpp::ok())
            {
                result->finish = true;
                goal_handle_->succeed(result);
                RCLCPP_INFO(this->get_logger(), "Server: Goal succeeded");
            }
        }
};

// ROS2节点主入口main函数
int main(int argc, char * argv[])                                
{
    // ROS2 C++接口初始化
    rclcpp::init(argc, argv);                        
    
    // 创建ROS2节点对象并进行初始化             
    rclcpp::spin(std::make_shared<MoveCircleActionServer>());   
    
    // 关闭ROS2 C++接口  
    rclcpp::shutdown();                                           
    
    return 0;
}

上边的是server端，看看下边的client端；代码如下：

class MoveCircleActionClient : public rclcpp::Node
{
    public:
        // 定义一个自定义的动作接口类，便于后续使用
        using CustomAction = learning_interface::action::MoveCircle;
        // 定义一个处理动作请求、取消、执行的客户端类
        using GoalHandle = rclcpp_action::ClientGoalHandle<CustomAction>;

        explicit MoveCircleActionClient(const rclcpp::NodeOptions & node_options = rclcpp::NodeOptions())
        : Node("action_move_client", node_options)                            // ROS2节点父类初始化
        {
            this->client_ptr_ = rclcpp_action::create_client<CustomAction>(   // 创建动作客户端（接口类型、动作名）
                this->get_node_base_interface(),
                this->get_node_graph_interface(),
                this->get_node_logging_interface(),
                this->get_node_waitables_interface(),
                "move_circle");
        }

        // 创建一个发送动作目标的函数
        void send_goal(bool enable)
        {
            // 检查动作服务器是否可以使用
            if (!this->client_ptr_->wait_for_action_server(std::chrono::seconds(10))) 
            {
                RCLCPP_ERROR(this->get_logger(), "Client: Action server not available after waiting");
                rclcpp::shutdown();
                return;
            }

            // 绑定动作请求、取消、执行的回调函数
            auto send_goal_options = rclcpp_action::Client<CustomAction>::SendGoalOptions();
            using namespace std::placeholders;
            send_goal_options.goal_response_callback =
                std::bind(&MoveCircleActionClient::goal_response_callback, this, _1);
            send_goal_options.feedback_callback =
                std::bind(&MoveCircleActionClient::feedback_callback, this, _1, _2);
            send_goal_options.result_callback =
                std::bind(&MoveCircleActionClient::result_callback, this, _1);

            // 创建一个动作目标的消息
            auto goal_msg = CustomAction::Goal();
            goal_msg.enable = enable;
            // 异步方式发送动作的目标
            RCLCPP_INFO(this->get_logger(), "Client: Sending goal");
                this->client_ptr_->async_send_goal(goal_msg, send_goal_options);
        }

    private:
        rclcpp_action::Client<CustomAction>::SharedPtr client_ptr_;

        // 创建一个服务器收到目标之后反馈时的回调函数
        void goal_response_callback(GoalHandle::SharedPtr goal_message)
		{
			if (!goal_message)
			{
				RCLCPP_ERROR(this->get_logger(), "Client: Goal was rejected by server");
				rclcpp::shutdown(); // Shut down client node
			}
			else
			{
				RCLCPP_INFO(this->get_logger(), "Client: Goal accepted by server, waiting for result");
			}
		}

        // 创建处理周期反馈消息的回调函数
        void feedback_callback(
            GoalHandle::SharedPtr,
            const std::shared_ptr<const CustomAction::Feedback> feedback_message)
        {
            std::stringstream ss;
            ss << "Client: Received feedback: "<< feedback_message->state;
            RCLCPP_INFO(this->get_logger(), "%s", ss.str().c_str());
        }

        // 创建一个收到最终结果的回调函数
        void result_callback(const GoalHandle::WrappedResult & result_message)
        {
            switch (result_message.code)
            {
                case rclcpp_action::ResultCode::SUCCEEDED:
                    break;
                case rclcpp_action::ResultCode::ABORTED:
                    RCLCPP_ERROR(this->get_logger(), "Client: Goal was aborted");
                    rclcpp::shutdown(); // 关闭客户端节点
                    return;
                case rclcpp_action::ResultCode::CANCELED:
                    RCLCPP_ERROR(this->get_logger(), "Client: Goal was canceled");
                    rclcpp::shutdown(); // 关闭客户端节点
                    return;
                default:
                    RCLCPP_ERROR(this->get_logger(), "Client: Unknown result code");
                    rclcpp::shutdown(); // 关闭客户端节点
                    return;
            }
            RCLCPP_INFO(this->get_logger(), "Client: Result received: %s", (result_message.result->finish ? "true" : "false"));
            rclcpp::shutdown();         // 关闭客户端节点
        }
};

// ROS2节点主入口main函数
int main(int argc, char * argv[])                                    
{
    // ROS2 C++接口初始化
    rclcpp::init(argc, argv);   
    
    // 创建一个客户端指针                                     
    auto action_client = std::make_shared<MoveCircleActionClient>(); 
    
    // 发送动作目标
    action_client->send_goal(true);     
    
    // 创建ROS2节点对象并进行初始化                            
    rclcpp::spin(action_client);  
    
    // 关闭ROS2 C++接口                                   
    rclcpp::shutdown();                                              
    
    return 0;
}

这段代码，就是客户端发起一个动作，服务器接收到，然后反馈，最后收到结果，然后重新调整。通过这样的例子，可以看懂ROS2的action是怎么用的，以及如何自定义动作，自定义消息。

这篇就写到这里，代码没什么复杂的，关键是要学会c++，c++不熟悉的，可以看看这里。