实验目的:
自定义动作接口,要求动作客户端发送一个整型数据给服务端,动作服务端进行累加求和,在累加过程中连续反馈进度,最后反馈结果给客户端。
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
测试成功。