ROS2学习（9）------ROS2动作

操作系统：ubuntu22.04
IDE:Visual Studio Code
编程语言：C++11
ROS版本：2

在 ROS 2 中，动作（Actions）是一种用于处理长时间运行的任务的通信机制。与服务（Services）相比，动作支持任务取消、进度反馈等特性，使其非常适合需要监控执行进度或可能耗时较长的操作。

创建和使用动作的基本步骤

定义动作消息：

动作消息由三个部分组成：目标（Goal）、结果（Result）和反馈（Feedback）。你需要为这三个部分定义 .action 文件。
生成接口代码：

使用 rosidl_default_generators 来自动生成基于你的 .action 文件的接口代码。
编写动作服务器和客户端：
- 动作服务器负责接收并处理来自客户端的动作请求。
- 动作客户端用于发送请求到服务器，并可以用来查询状态或取消请求。

示例

定义一个简单的计数器动作

假设我们要创建一个名为 Counter.action 的动作，它将从0开始计数直到达到目标值，并在此过程中提供进度反馈。

在 ROS 2 中，动作（Action）定义文件（.action 文件）通常放置在一个专门的 action 目录下，这个目录应该位于你的软件包的根目录中。这意味着如果你的软件包名称为 robot_nodes，那么动作定义文件应该放在 robot_nodes/action/ 目录下。

假设你正在开发一个名为 robot_nodes 的软件包，并且想要添加一个计数器动作（如之前的例子 Counter.action），你的目录结构看起来应该是这样的：

bash 复制代码

robot_nodes/
├── action/
│   └── Counter.action
├── CMakeLists.txt
├── package.xml
├── nodes/
│   ├── node1/
│   │   ├── CMakeLists.txt
│   │   └── node1.cpp
│   └── node2/
│       ├── CMakeLists.txt
│       └── node2.cpp
└── srv/
    └── MyService.srv

在 action/Counter.action 文件中定义：

bash 复制代码

# Goal
int32 goal
---
# Result
int32 result
---
# Feedback
int32 current_number

修改 package.xml

确保添加对 actionlib_msgs 和 rosidl_default_generators 的依赖：

bash 复制代码

<build_depend>rosidl_default_generators</build_depend>
<exec_depend>rosidl_default_runtime</exec_depend>
<depend>actionlib_msgs</depend>

更新 CMakeLists.txt

在主 CMakeLists.txt 中调用 rosidl_generate_interfaces 来生成动作接口：

bash 复制代码

find_package(rosidl_default_generators REQUIRED)
rosidl_generate_interfaces(${PROJECT_NAME}
  "action/Counter.action"
)

同时，确保在需要的地方链接相应的库，比如在定义节点可执行文件后：

bash 复制代码

ament_target_dependencies(your_node rclcpp ${PROJECT_NAME})

编写动作服务器和客户端

动作服务器示例 (nodes/server.cpp)：

cpp 复制代码

#include "../common/public.hpp"
#include "rclcpp/logging.hpp"
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "robot_nodes/action/counter.hpp"
#include "std_msgs/msg/string.hpp"
#include <future>

using Counter              = robot_nodes::action::Counter;
using GoalHandleCountUntil = rclcpp_action::ClientGoalHandle< Counter >;

class CountUntilClient : public rclcpp::Node {
public:
    explicit CountUntilClient() : Node( "count_until_client" )
    {
        client_ = rclcpp_action::create_client< Counter >( this, "Counter" );

        while ( !client_->wait_for_action_server( std::chrono::seconds( 1 ) ) )
        {
            if ( !rclcpp::ok() )
            {
                RCLCPP_ERROR( this->get_logger(), "Interrupted while waiting for the action server." );
                return;
            }
            RCLCPP_INFO( this->get_logger(), "Waiting for action server..." );
        }
        RCLCPP_INFO( this->get_logger(), "suceess" );
    }

    void send_goal( int goal_value )
    {
        auto goal_msg = Counter::Goal();
        goal_msg.goal = goal_value;

        auto send_goal_options = rclcpp_action::Client< Counter >::SendGoalOptions();

        send_goal_options.goal_response_callback = [ this ]( std::shared_ptr< GoalHandleCountUntil > 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" );
            }
        };

        send_goal_options.feedback_callback = [ this ]( std::shared_ptr< const GoalHandleCountUntil > goal_handle, const std::shared_ptr< const Counter::Feedback >& feedback ) {
            ( void )goal_handle;
            RCLCPP_INFO( this->get_logger(), "Feedback received: %d", feedback->current_number );
        };

        send_goal_options.result_callback = [ this ]( const GoalHandleCountUntil::WrappedResult& result ) {
            switch ( result.code )
            {
            case rclcpp_action::ResultCode::SUCCEEDED:
                RCLCPP_INFO( this->get_logger(), "Result: %ld", result.result->result );
                break;
            case rclcpp_action::ResultCode::ABORTED:
                RCLCPP_ERROR( this->get_logger(), "Goal was aborted" );
                break;
            case rclcpp_action::ResultCode::CANCELED:
                RCLCPP_ERROR( this->get_logger(), "Goal was canceled" );
                break;
            default:
                RCLCPP_ERROR( this->get_logger(), "Unknown result code" );
                break;
            }
        };

        client_->async_send_goal( goal_msg, send_goal_options );
    }

private:
    rclcpp_action::Client< Counter >::SharedPtr client_;
};

int main( int argc, char* argv[] )
{
    rclcpp::init( argc, argv );

    CCUTIL::set_logger_save_directory( "logs" );
    CCUTIL::set_log_level( CCUTIL::LogLevel::Debug );

    INFO( "==========main===========" );

    auto client = std::make_shared<CountUntilClient>();
    client->send_goal(12);

    rclcpp::shutdown();
    return 0;
}

动作客户端示例 (nodes/action_client.cpp)

cpp 复制代码

#include "../common/public.hpp"
#include "example_interfaces/srv/add_two_ints.hpp"  // 引入服务接口
#include "rclcpp/logging.hpp"
#include "rclcpp/rclcpp.hpp"
#include "robot_nodes/action/counter.hpp"
#include "std_msgs/msg/string.hpp"
#include <rclcpp_action/rclcpp_action.hpp>

using Counter           = robot_nodes::action::Counter;
using GoalHandleCountUntil = rclcpp_action::ServerGoalHandle< Counter >;

class CountUntilServer : public rclcpp::Node {
public:
    using SharedPtr = std::shared_ptr< CountUntilServer >;

    explicit CountUntilServer( const rclcpp::NodeOptions& options = {} ) : Node( "count_until_server", options )
    {

         this->action_server_ = rclcpp_action::create_server< Counter >(
             this, "Counter", [ this ]( const rclcpp_action::GoalUUID& uuid, std::shared_ptr< const Counter::Goal > goal ) { return handle_goal( uuid, goal ); },
             [ this ]( std::shared_ptr< GoalHandleCountUntil > goal_handle ) { return handle_cancel( goal_handle ); },
             [ this ]( std::shared_ptr< GoalHandleCountUntil > goal_handle ) { return handle_accepted( goal_handle ); } );
    }

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

    rclcpp_action::GoalResponse handle_goal( const rclcpp_action::GoalUUID& /*uuid*/, std::shared_ptr< const Counter::Goal > goal )
    {
        RCLCPP_INFO( this->get_logger(), "Received goal: %d", goal->goal );
        return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
    }

    rclcpp_action::CancelResponse handle_cancel( std::shared_ptr< GoalHandleCountUntil > goal_handle )
    {
        RCLCPP_INFO( this->get_logger(), "Received request to cancel goal" );
        ( void )goal_handle;
        return rclcpp_action::CancelResponse::ACCEPT;
    }

    void handle_accepted( std::shared_ptr< GoalHandleCountUntil > goal_handle )
    {
        std::thread( [ this, goal_handle ]() { this->execute( goal_handle ); } ).detach();
    }

    void execute( std::shared_ptr< GoalHandleCountUntil > goal_handle )
    {
        auto goal     = goal_handle->get_goal();
        auto feedback = std::make_shared< Counter::Feedback >();
        auto result   = std::make_shared< Counter::Result >();

        int number = 0;

        while ( number < goal->goal )
        {
            if ( goal_handle->is_canceling() )
            {
                result->result = number;
                goal_handle->canceled( result );
                RCLCPP_INFO( this->get_logger(), "Goal canceled" );
                return;
            }

            feedback->current_number = number;
            goal_handle->publish_feedback( feedback );

            std::this_thread::sleep_for( std::chrono::seconds( 1 ) );
            number++;
        }

        result->result = number;
        goal_handle->succeed( result );
        RCLCPP_INFO( this->get_logger(), "Goal succeeded with result: %d", number );
    }
};

int main( int argc, char* argv[] )
{
    rclcpp::init( argc, argv );

    CCUTIL::set_logger_save_directory( "logs" );
    CCUTIL::set_log_level( CCUTIL::LogLevel::Debug );

    INFO( "==========main===========" );
    auto node = std::make_shared<CountUntilServer>();
    rclcpp::spin(node);
    INFO("----end----");
    rclcpp::shutdown();
    return 0;
}

运行结果

客户端：

bash 复制代码

ros2 run robot_nodes node2
[2025-05-23 15:22:26:123][node2][info][node2.cpp:143]:==========main===========
[INFO] [1747984946.277645738] [count_until_client]: suceess

服务端：

bash 复制代码

ros2 run robot_nodes node1
[2025-05-23 15:20:52:745][node1][info][node1.cpp:127]:==========main===========
[INFO] [1747984857.242722174] [count_until_server]: Received goal: 10
[INFO] [1747984867.245015651] [count_until_server]: Goal succeeded with result: 10
[INFO] [1747984946.278083962] [count_until_server]: Received goal: 12
[INFO] [1747984958.280770786] [count_until_server]: Goal succeeded with result: 12
^C[INFO] [1747984976.058683878] [rclcpp]: signal_handler(signum=2)
[2025-05-23 15:22:56:059][node1][info][node1.cpp:137]:----end----

温馨提示：代码严重依赖环境，一般编译不会成功，不会成功，不会成功