ROS2 通信三大件之动作 -- Action

news2024/11/24 15:50:33

通信最后一个，也是不太容易理解的方式action，复杂且重要

1、创建action数据结构

创建工作空间和模块就不多说了

在模块 src/action_moudle/action/Counter.action 下创建文件 Counter.action

int32 target  # Goal: 目标 
---
int32 current_value  # Result: 结果
---
int32[] sequence  # Feedback: 中间状态反馈

需要注意的是这几个值不要搞混，结果和中间状态容易搞错

添加依赖：src/action_moudle/package.xml

  <depend>rclcpp_action</depend> 

  <depend>rosidl_default_generators</depend>
  <depend>rosidl_default_runtime</depend>

  <member_of_group>rosidl_interface_packages</member_of_group>

src/action_moudle/CMakeLists.txt

find_package(rosidl_default_generators REQUIRED)
find_package(rclcpp_action REQUIRED)


# 设置消息和服务文件路径
set(action_FILES
  "action/Counter.action"
)

# 添加消息和服务生成目标
rosidl_generate_interfaces(${PROJECT_NAME}
  ${action_FILES}
  DEPENDENCIES std_msgs
)

# 安装 Action 文件
install(DIRECTORY action/
  DESTINATION share/${PROJECT_NAME}/action
)

编译后生成install/action_moudle/include/action_moudle/action_moudle/action/counter.hpp

至此action文件生成

2、编写Count计时的Action服务端和客户端代码

AI生成的代码靠不住，改了很多才编译过

src/action_moudle/src/action_server.cpp

#include "rclcpp/rclcpp.hpp"
#include "action_moudle/action/counter.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include <memory>
#include <vector>

using Counter = action_moudle::action::Counter;
using namespace std::placeholders;

class CounterActionServer : public rclcpp::Node
{
public:
    CounterActionServer() : Node("counter_action_server")
    {
        action_server_ =
            rclcpp_action::create_server<Counter>(this, "counter", std::bind(&CounterActionServer::handle_goal, this, _1, _2),
                                                  std::bind(&CounterActionServer::handle_cancel, this, _1), std::bind(&CounterActionServer::handle_accepted, this, _1));
    }

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)
    {
        (void)uuid;
        RCLCPP_INFO(this->get_logger(), "Received goal request with target: %ld", goal->target);
        return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
    }

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

    void handle_accepted(const std::shared_ptr<rclcpp_action::ServerGoalHandle<Counter>> goal_handle)
    {
        using namespace std::placeholders;
        std::thread{std::bind(&CounterActionServer::execute, this, _1), goal_handle}.detach();
    }

    void execute(const std::shared_ptr<rclcpp_action::ServerGoalHandle<Counter>> 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<Counter::Feedback>();
        auto         result   = std::make_shared<Counter::Result>();

        result->current_value = 0;
        feedback->sequence.push_back(0);

        if (goal_handle->is_canceling())
        {
            goal_handle->canceled(result);
            RCLCPP_INFO(this->get_logger(), "Goal canceled");
            return;
        }
        goal_handle->publish_feedback(feedback);

        for (int i = 1; i <= goal->target; ++i)
        {
            result->current_value = i;
            feedback->sequence.push_back(i);
            if (goal_handle->is_canceling())
            {
                goal_handle->canceled(result);
                RCLCPP_INFO(this->get_logger(), "Goal canceled");
                return;
            }
            goal_handle->publish_feedback(feedback);
            loop_rate.sleep();
        }

        goal_handle->succeed(result);
        RCLCPP_INFO(this->get_logger(), "Returning result");
    }
};

int main(int argc, char* argv[])
{
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<CounterActionServer>());
    rclcpp::shutdown();
    return 0;
}

src/action_moudle/src/action_client.cpp

#include "rclcpp/rclcpp.hpp"
#include "action_moudle/action/counter.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include <memory>

using Counter = action_moudle::action::Counter;
using namespace std::placeholders;

class CounterActionClient : public rclcpp::Node
{
public:
    CounterActionClient() : Node("counter_action_client")
    {
        action_client_ = rclcpp_action::create_client<Counter>(this, "counter");

        while (!action_client_->wait_for_action_server())
        {
            if (!rclcpp::ok())
            {
                RCLCPP_ERROR(this->get_logger(), "Interrupted while waiting for the action server. Exiting.");
                return;
            }
            RCLCPP_INFO(this->get_logger(), "Action server not available, waiting again...");
        }

        auto goal   = Counter::Goal();
        goal.target = 10;

        auto send_goal_options                   = rclcpp_action::Client<Counter>::SendGoalOptions();
        send_goal_options.goal_response_callback = std::bind(&CounterActionClient::goal_response_callback, this, std::placeholders::_1);
        send_goal_options.feedback_callback      = std::bind(&CounterActionClient::feedback_callback, this, std::placeholders::_1, std::placeholders::_2);
        send_goal_options.result_callback        = std::bind(&CounterActionClient::result_callback, this, std::placeholders::_1);

        action_client_->async_send_goal(goal, send_goal_options);
    }

private:
    rclcpp_action::Client<Counter>::SharedPtr action_client_;

    void goal_response_callback(rclcpp_action::ClientGoalHandle<Counter>::SharedPtr goal_handle)
    {
        if (!goal_handle)
        {
            RCLCPP_INFO(this->get_logger(), "Goal rejected");
        }
        else
        {
            RCLCPP_INFO(this->get_logger(), "Goal accepted");
        }
    }
    // void goal_response_callback(std::shared_future<std::shared_ptr<rclcpp_action::ClientGoalHandle<Counter>>> future)
    // {
    //     auto goal_handle = future.get();
    //     if (!goal_handle)
    //     {
    //         RCLCPP_INFO(this->get_logger(), "Goal rejected");
    //     }
    //     else
    //     {
    //         RCLCPP_INFO(this->get_logger(), "Goal accepted");
    //     }
    // }

    void feedback_callback(std::shared_ptr<rclcpp_action::ClientGoalHandle<Counter>>, const std::shared_ptr<const Counter::Feedback> feedback)
    {
        // RCLCPP_INFO(this->get_logger(), "Current value: %ld", feedback->current_value);
        RCLCPP_INFO(this->get_logger(), "Sequence value: %s", print_sequence(feedback->sequence).c_str());
    }

    void result_callback(const rclcpp_action::ClientGoalHandle<Counter>::WrappedResult& result)
    {
        switch (result.code)
        {
            case rclcpp_action::ResultCode::SUCCEEDED:
                RCLCPP_INFO(this->get_logger(), "Result: %d", result.result->current_value);
                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;
        }
        rclcpp::shutdown();
    }

    std::string print_sequence(const std::vector<int32_t>& sequence)
    {
        std::stringstream ss;
        ss << "[";
        for (size_t i = 0; i < sequence.size(); ++i)
        {
            if (i > 0)
            {
                ss << ", ";
            }
            ss << sequence[i];
        }
        ss << "]";
        return ss.str();
    }
};

int main(int argc, char* argv[])
{
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<CounterActionClient>());
    rclcpp::shutdown();
    return 0;
}

src/action_moudle/CMakeLists.txt 添加

find_package(action_moudle REQUIRED)
find_package(rclcpp_action REQUIRED)

# 创建可执行文件
add_executable(action_server src/action_server.cpp)
ament_target_dependencies(action_server rclcpp std_msgs action_moudle rclcpp_action)

add_executable(action_client src/action_client.cpp)
ament_target_dependencies(action_client rclcpp std_msgs action_moudle rclcpp_action)

# 安装目标
install(TARGETS
  action_server action_client
  DESTINATION lib/${PROJECT_NAME}
)