当前位置：首页 > news >正文

ros2 action server示例、拓展、练习

news 文章来源：https://blog.csdn.net/lucust/article/details/143035715 2025/5/4 20:46:15

注意：以下代码全部由ai生成，没有大问题，运用时需根据报错逐步调试

action server示例

将 goal、result 和 feedback 作为类的成员变量的 C++ 示例代码：

示例代码

#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "your_package/action/my_action.hpp"
#include "your_package/srv/my_service.hpp"class MyActionServer : public rclcpp::Node
{
public:using MyAction = your_package::action::MyAction;using GoalHandleMyAction = rclcpp_action::ServerGoalHandle<MyAction>;MyActionServer(): Node("my_action_server"),feedback_(std::make_shared<MyAction::Feedback>()),result_(std::make_shared<MyAction::Result>()){// 创建 Action 服务器action_server_ = rclcpp_action::create_server<MyAction>(this,"my_action",std::bind(&MyActionServer::handle_goal, this, std::placeholders::_1, std::placeholders::_2),std::bind(&MyActionServer::handle_cancel, this, std::placeholders::_1),std::bind(&MyActionServer::handle_accepted, this, std::placeholders::_1));// 创建服务service_ = this->create_service<your_package::srv::MyService>("my_service",std::bind(&MyActionServer::handle_service, this, std::placeholders::_1, std::placeholders::_2));}private:rclcpp_action::Server<MyAction>::SharedPtr action_server_;rclcpp::Service<your_package::srv::MyService>::SharedPtr service_;std::shared_ptr<const MyAction::Goal> goal_;std::shared_ptr<MyAction::Result> result_;std::shared_ptr<MyAction::Feedback> feedback_;// 处理目标rclcpp_action::GoalResponse handle_goal(const rclcpp_action::GoalUUID & uuid,std::shared_ptr<const MyAction::Goal> goal){RCLCPP_INFO(this->get_logger(), "Received goal: %f", goal->goal_value);goal_ = goal;  // 存储目标return rclcpp_action::GoalResponse::YES;}// 处理取消请求rclcpp_action::CancelResponse handle_cancel(const std::shared_ptr<GoalHandleMyAction> goal_handle){RCLCPP_INFO(this->get_logger(), "Canceling goal");return rclcpp_action::CancelResponse::ACCEPT;}// 处理已接受的目标void handle_accepted(const std::shared_ptr<GoalHandleMyAction> goal_handle){std::thread([this, goal_handle]() {this->execute(goal_handle);}).detach();}// 执行目标void execute(const std::shared_ptr<GoalHandleMyAction> goal_handle){for (int i = 1; i <= 5; ++i) {feedback_->current_value = i * goal_->goal_value;goal_handle->publish_feedback(feedback_);RCLCPP_INFO(this->get_logger(), "Feedback: %f", feedback_->current_value);std::this_thread::sleep_for(std::chrono::seconds(1));}// 设置结果result_->result_value = goal_->goal_value * 10;goal_handle->succeed(result_);RCLCPP_INFO(this->get_logger(), "Action succeeded with result: %f", result_->result_value);}// 处理服务void handle_service(const std::shared_ptr<your_package::srv::MyService::Request> request,std::shared_ptr<your_package::srv::MyService::Response> response){response->response_value = request->request_value * 2;  // 示例处理RCLCPP_INFO(this->get_logger(), "Service called with value: %f, response: %f",request->request_value, response->response_value);}
};int main(int argc, char ** argv)
{rclcpp::init(argc, argv);rclcpp::spin(std::make_shared<MyActionServer>());rclcpp::shutdown();return 0;
}

代码说明

成员变量：
- goal_: 存储接收到的目标。
- result_: 存储结果信息。
- feedback_: 存储反馈信息。
在 handle_goal 方法中，接收到的目标被存储到 goal_ 中。
在 execute 方法中，根据 goal_ 的值生成反馈，并发布反馈和结果。

拓展1 在execute中创建一个导航action client，并发布。

要在 execute 方法中创建一个导航目标点的 Action 客户端并发布一个固定点，你需要确保在代码中添加相关的导航 Action 依赖。以下是更新后的 C++ 示例代码：

示例代码

#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "your_package/action/my_action.hpp"
#include "your_package/srv/my_service.hpp"
#include "nav2_msgs/action/navigate_to_pose.hpp" // 导航 Actionclass MyActionServer : public rclcpp::Node
{
public:using MyAction = your_package::action::MyAction;using GoalHandleMyAction = rclcpp_action::ServerGoalHandle<MyAction>;using NavigateToPose = nav2_msgs::action::NavigateToPose;using NavigateToPoseClient = rclcpp_action::Client<NavigateToPose>;MyActionServer(): Node("my_action_server"),feedback_(std::make_shared<MyAction::Feedback>()),result_(std::make_shared<MyAction::Result>()),navigate_client_(rclcpp_action::create_client<NavigateToPose>(this, "navigate_to_pose")){action_server_ = rclcpp_action::create_server<MyAction>(this,"my_action",std::bind(&MyActionServer::handle_goal, this, std::placeholders::_1, std::placeholders::_2),std::bind(&MyActionServer::handle_cancel, this, std::placeholders::_1),std::bind(&MyActionServer::handle_accepted, this, std::placeholders::_1));service_ = this->create_service<your_package::srv::MyService>("my_service",std::bind(&MyActionServer::handle_service, this, std::placeholders::_1, std::placeholders::_2));}private:rclcpp_action::Server<MyAction>::SharedPtr action_server_;rclcpp::Service<your_package::srv::MyService>::SharedPtr service_;std::shared_ptr<const MyAction::Goal> goal_;std::shared_ptr<MyAction::Result> result_;std::shared_ptr<MyAction::Feedback> feedback_;rclcpp_action::Client<NavigateToPose>::SharedPtr navigate_client_;rclcpp_action::GoalResponse handle_goal(const rclcpp_action::GoalUUID & uuid,std::shared_ptr<const MyAction::Goal> goal){RCLCPP_INFO(this->get_logger(), "Received goal: %f", goal->goal_value);goal_ = goal;return rclcpp_action::GoalResponse::YES;}rclcpp_action::CancelResponse handle_cancel(const std::shared_ptr<GoalHandleMyAction> goal_handle){RCLCPP_INFO(this->get_logger(), "Canceling goal");return rclcpp_action::CancelResponse::ACCEPT;}void handle_accepted(const std::shared_ptr<GoalHandleMyAction> goal_handle){std::thread([this, goal_handle]() {this->execute(goal_handle);}).detach();}void execute(const std::shared_ptr<GoalHandleMyAction> goal_handle){// 创建导航目标点auto navigation_goal = NavigateToPose::Goal();navigation_goal.pose.header.frame_id = "map"; // 或者其他适当的坐标框架navigation_goal.pose.header.stamp = this->now();navigation_goal.pose.pose.position.x = 1.0; // 固定点的坐标navigation_goal.pose.pose.position.y = 1.0;navigation_goal.pose.pose.orientation.w = 1.0; // 方向// 调用导航客户端if (!navigate_client_->wait_for_action_server(std::chrono::seconds(5))) {RCLCPP_ERROR(this->get_logger(), "Navigation action server not available");goal_handle->abort(result_);return;}auto send_goal_options = rclcpp_action::Client<NavigateToPose>::SendGoalOptions();send_goal_options.result_callback = [](const rclcpp_action::ClientGoalHandle<NavigateToPose>::SharedPtr &) {RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Navigation goal accepted");};auto goal_handle_navigation = navigate_client_->async_send_goal(navigation_goal, send_goal_options);// 处理导航反馈while (rclcpp::ok()) {// 这里可以添加导航反馈处理逻辑std::this_thread::sleep_for(std::chrono::milliseconds(500));}// 返回结果result_->result_value = goal_->goal_value * 10;goal_handle->succeed(result_);RCLCPP_INFO(this->get_logger(), "Action succeeded with result: %f", result_->result_value);}void handle_service(const std::shared_ptr<your_package::srv::MyService::Request> request,std::shared_ptr<your_package::srv::MyService::Response> response){response->response_value = request->request_value * 2;RCLCPP_INFO(this->get_logger(), "Service called with value: %f, response: %f",request->request_value, response->response_value);}
};int main(int argc, char ** argv)
{rclcpp::init(argc, argv);rclcpp::spin(std::make_shared<MyActionServer>());rclcpp::shutdown();return 0;
}

代码说明

导航 Action 客户端：
- 创建 NavigateToPose 的 Action 客户端。
- 使用 navigate_client_ 来发送导航目标。
导航目标点：
- 在 execute 方法中定义固定的导航目标点（例如 (1.0, 1.0)）。
- 设置导航目标的坐标和方向。
异步发送目标：
- 使用 async_send_goal 方法发送导航目标，处理结果和反馈。
- 可以在循环中添加对导航状态的反馈处理逻辑。

使用

确保导航 Action 服务器（例如 Nav2）正在运行。
启动此 Action 服务器节点。
在客户端调用 Action 和服务，观察服务器的导航行为。

拓展2 针对send_goal_options配置结构，创建一个新成员函数，替代上一段的lambda函数，并使用send_goal_options的所有成员

好的，下面是更新后的代码示例，其中我们创建了一个新的成员函数来处理 send_goal_options 的配置，替代了之前使用的 Lambda 函数。这一新函数将使用 send_goal_options 的所有成员。

更新后的示例代码

#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "your_package/action/my_action.hpp"
#include "your_package/srv/my_service.hpp"
#include "nav2_msgs/action/navigate_to_pose.hpp" // 导航 Actionclass MyActionServer : public rclcpp::Node
{
public:using MyAction = your_package::action::MyAction;using GoalHandleMyAction = rclcpp_action::ServerGoalHandle<MyAction>;using NavigateToPose = nav2_msgs::action::NavigateToPose;using NavigateToPoseClient = rclcpp_action::Client<NavigateToPose>;MyActionServer(): Node("my_action_server"),feedback_(std::make_shared<MyAction::Feedback>()),result_(std::make_shared<MyAction::Result>()),navigate_client_(rclcpp_action::create_client<NavigateToPose>(this, "navigate_to_pose")){action_server_ = rclcpp_action::create_server<MyAction>(this,"my_action",std::bind(&MyActionServer::handle_goal, this, std::placeholders::_1, std::placeholders::_2),std::bind(&MyActionServer::handle_cancel, this, std::placeholders::_1),std::bind(&MyActionServer::handle_accepted, this, std::placeholders::_1));service_ = this->create_service<your_package::srv::MyService>("my_service",std::bind(&MyActionServer::handle_service, this, std::placeholders::_1, std::placeholders::_2));}private:rclcpp_action::Server<MyAction>::SharedPtr action_server_;rclcpp::Service<your_package::srv::MyService>::SharedPtr service_;std::shared_ptr<const MyAction::Goal> goal_;std::shared_ptr<MyAction::Result> result_;std::shared_ptr<MyAction::Feedback> feedback_;rclcpp_action::Client<NavigateToPose>::SharedPtr navigate_client_;rclcpp_action::GoalResponse handle_goal(const rclcpp_action::GoalUUID & uuid,std::shared_ptr<const MyAction::Goal> goal){RCLCPP_INFO(this->get_logger(), "Received goal: %f", goal->goal_value);goal_ = goal;return rclcpp_action::GoalResponse::YES;}rclcpp_action::CancelResponse handle_cancel(const std::shared_ptr<GoalHandleMyAction> goal_handle){RCLCPP_INFO(this->get_logger(), "Canceling goal");return rclcpp_action::CancelResponse::ACCEPT;}void handle_accepted(const std::shared_ptr<GoalHandleMyAction> goal_handle){std::thread([this, goal_handle]() {this->execute(goal_handle);}).detach();}void execute(const std::shared_ptr<GoalHandleMyAction> goal_handle){// 创建导航目标点auto navigation_goal = NavigateToPose::Goal();navigation_goal.pose.header.frame_id = "map";navigation_goal.pose.header.stamp = this->now();navigation_goal.pose.pose.position.x = 1.0;navigation_goal.pose.pose.position.y = 1.0;navigation_goal.pose.pose.orientation.w = 1.0;// 调用导航客户端if (!navigate_client_->wait_for_action_server(std::chrono::seconds(5))) {RCLCPP_ERROR(this->get_logger(), "Navigation action server not available");goal_handle->abort(result_);return;}// 使用新成员函数配置 send_goal_optionsconfigure_send_goal_options(goal_handle, navigation_goal);}void configure_send_goal_options(const std::shared_ptr<GoalHandleMyAction> goal_handle, const NavigateToPose::Goal &navigation_goal){auto send_goal_options = rclcpp_action::Client<NavigateToPose>::SendGoalOptions();// 设置 result_callbacksend_goal_options.result_callback = [this](const rclcpp_action::ClientGoalHandle<NavigateToPose>::SharedPtr &goal_handle) {if (goal_handle->get_status() == rclcpp_action::GoalStatus::SUCCEEDED) {RCLCPP_INFO(this->get_logger(), "Navigation goal succeeded");} else {RCLCPP_ERROR(this->get_logger(), "Navigation goal failed");}};// 设置 feedback_callbacksend_goal_options.feedback_callback = [this](const rclcpp_action::ClientGoalHandle<NavigateToPose>::SharedPtr &goal_handle, const std::shared_ptr<const NavigateToPose::Feedback> feedback) {RCLCPP_INFO(this->get_logger(), "Navigation feedback: current position %f, %f",feedback->current_pose.position.x, feedback->current_pose.position.y);};// 设置 goal_response_callbacksend_goal_options.goal_response_callback = [](const rclcpp_action::ClientGoalHandle<NavigateToPose>::SharedPtr &goal_handle) {if (goal_handle) {RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Navigation goal accepted");} else {RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Navigation goal rejected");}};// 发送导航目标auto goal_handle_navigation = navigate_client_->async_send_goal(navigation_goal, send_goal_options);}void handle_service(const std::shared_ptr<your_package::srv::MyService::Request> request,std::shared_ptr<your_package::srv::MyService::Response> response){response->response_value = request->request_value * 2;RCLCPP_INFO(this->get_logger(), "Service called with value: %f, response: %f",request->request_value, response->response_value);}
};int main(int argc, char ** argv)
{rclcpp::init(argc, argv);rclcpp::spin(std::make_shared<MyActionServer>());rclcpp::shutdown();return 0;
}

代码说明

新成员函数 configure_send_goal_options：
- 此函数负责配置 send_goal_options 的所有成员，包括 result_callback、feedback_callback 和 goal_response_callback。
- 通过捕获 this，可以在回调中访问类的成员函数和变量。
回调函数的实现：
- result_callback: 检查导航目标的状态，并根据成功或失败打印日志信息。
- feedback_callback: 输出当前导航位置的反馈信息。
- goal_response_callback: 检查目标是否被接受，并输出相应的日志信息。
调用导航目标：
- 在 execute 方法中，调用 configure_send_goal_options 来配置并发送导航目标。

这种方式将发送目标的逻辑与配置的回调逻辑分离，使代码更清晰、可维护。如果你有其他问题或者需要进一步的解释，请随时告诉我！

注意：只要设置了 send_goal_options.feedback_callback，回调函数就会在导航服务器发送反馈时被调用，可以在函数内部获取并处理这些反馈信息。
示例：
将其改写为

send_goal_options.feedback_callback = 
[this](const rclcpp_action::ClientGoalHandle<NavigateToPose>::SharedPtr &goal_handle
, const std::shared_ptr<const NavigateToPose::Feedback> feedback) 
{};

根据定义

#goal definition
geometry_msgs/PoseStamped pose
string behavior_tree
---
#result definition
std_msgs/Empty result
---
#feedback definition
geometry_msgs/PoseStamped current_pose
builtin_interfaces/Duration navigation_time
builtin_interfaces/Duration estimated_time_remaining
int16 number_of_recoveries
float32 distance_remaining
】

action的客户端也能获取
【geometry_msgs/PoseStamped current_pose
builtin_interfaces/Duration navigation_time
builtin_interfaces/Duration estimated_time_remaining
int16 number_of_recoveries
float32 distance_remaining】
这些信息

练习1

1.实现拓展中创建的navigation的导航action与自定义的action通信
2.找到isaac_mission_client实现其中自定义action与isaac_mission_client的通信，注意参考mqtt协议。

action server示例

示例代码

代码说明

拓展1 在execute中创建一个导航action client，并发布。

示例代码

代码说明

使用

拓展2 针对send_goal_options配置结构，创建一个新成员函数，替代上一段的lambda函数， 并使用send_goal_options的所有成员

更新后的示例代码

代码说明

练习1

相关文章：

拓展2 针对send_goal_options配置结构，创建一个新成员函数，替代上一段的lambda函数，并使用send_goal_options的所有成员