客户端：
/*
需求：简单的模拟导航功能，向机器人发送一个前进N米的请求，机器人就会以 0.1m/s的速度前进，
当与目标点的距离小于0.05m时，机器人就会停止运动，返回机器人的停止坐标，并且在此过程中，
会连续反馈机器人与目标点之间的剩余距离。
流程：
1.包含头文件；
2.初始化ROS2客户端；
3.自定义节点类；
3-1.创建参数服务客户端，连接到速度发布节点；
3-2.创建订阅方，订阅机器人的里程计以获取机器人坐标；
3-3.创建动作服务端，解析客户端的相关并生成响应。
4.调用spin函数，并传入节点对象指针；
5.资源释放。
*/
// 1.包含头文件；
// 包含头文件；
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "myexer_interfaces/action/nav.hpp"
#include "nav_msgs/msg/odometry.hpp"
using myexer_interfaces::action::Nav;
using namespace std::placeholders;
using namespace std::chrono_literals;
// 自定义节点类；
class NavServer : public rclcpp::Node{
public:
NavServer(): Node("nav_server_node_cpp"){
RCLCPP_INFO(this->get_logger(), "NavServer服务端创建！}");
last_x = last_y = 0.0;
//创建参数客户端，连接小车的速度发布节点
paramClient = std::make_shared<rclcpp::AsyncParametersClient>(this,"pub_vel_node_cpp");
while(!paramClient->wait_for_service(5s))
{
if(!rclcpp::ok())
{
return;
}
RCLCPP_ERROR(this->get_logger(),"服务超时未连接");
}
RCLCPP_INFO(this->get_logger(),"已经成功连接速度发布节点了，可以进rqt动态设置线速度和角速度了!");
sub_odom_ = this->create_subscription<nav_msgs::msg::Odometry>("odom",10,std::bind(&NavServer::on_timer,this,_1));
//创建动作服务端
nav_action_server_ = rclcpp_action::create_server<Nav>(
/*NodeT node,
const std::string &name,
rclcpp_action::Server<ActionT>::GoalCallback handle_goal,
rclcpp_action::Server<ActionT>::CancelCallback handle_cancel,
rclcpp_action::Server<ActionT>::AcceptedCallback handle_accepted,*/
this,
"nav",
std::bind(&NavServer::handle_goal,this,_1,_2),
std::bind(&NavServer::handle_cancel,this,_1),
std::bind(&NavServer::handle_accepted,this,_1)
);
}
private:
rclcpp_action::Server<Nav>::SharedPtr nav_action_server_;
rclcpp::AsyncParametersClient::SharedPtr paramClient;
rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr sub_odom_;
double x,y;
double last_x,last_y; // 每次导航时，机器人起点坐标
//处理订阅到的里程计数据
void on_timer(const nav_msgs::msg::Odometry &odom)
{
x = odom.pose.pose.position.x;
y = odom.pose.pose.position.y;
}
//using GoalCallback = std::function<GoalResponse(const GoalUUID &, std::shared_ptr<const typename ActionT::Goal>)>;
//处理动作客户端请求
rclcpp_action::GoalResponse handle_goal(const rclcpp_action::GoalUUID &goal_uuid,std::shared_ptr<const Nav::Goal> goal)
{
(void)goal_uuid;
double x = goal->goal;
if(x > 0)
{
RCLCPP_INFO(this->get_logger(),"小车将前进 %.2f 米",x);
}else{
RCLCPP_INFO(this->get_logger(),"输入数据小于0，不符合要求，请重新输入");
return rclcpp_action::GoalResponse::REJECT; //表示拒绝
}
paramClient->set_parameters({rclcpp::Parameter("linear",0.2),rclcpp::Parameter("angular",0.0)});
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; //这里代表同意
}
// using CancelCallback = std::function<CancelResponse(std::shared_ptr<ServerGoalHandle<ActionT>>)>;
//处理取消请求
rclcpp_action::CancelResponse handle_cancel(std::shared_ptr<rclcpp_action::ServerGoalHandle<Nav>> goal_handle)
{
(void)goal_handle;
RCLCPP_INFO(this->get_logger(),"任务取消中......");
paramClient->set_parameters({rclcpp::Parameter("linear",0.0),rclcpp::Parameter("angular",0.0)});
return rclcpp_action::CancelResponse::REJECT; //表示拒绝
}
void execute(std::shared_ptr<rclcpp_action::ServerGoalHandle<Nav>> goal_handle)
{
RCLCPP_INFO(this->get_logger(),"开始执行任务");
//获取目标点
float goal = goal_handle->get_goal()->goal;
//连续反馈
auto feedback = std::make_shared<Nav::Feedback>();
//最终结果
auto result = std::make_shared<Nav::Result>();
//最终结果
//1、设置连续反馈
rclcpp::Rate rate(1.0);
while(rclcpp::ok())
{
//检测任务是否被取消
if(goal_handle->is_canceling())
{
result->x = x; //最终结果x = 里程计x
result->y = y; //最终结y = 里程计y
goal_handle->canceled(result);
RCLCPP_INFO(this->get_logger(),"任务取消");
last_x = x; //机器人起点 = 里程计x
last_y = y; //机器人起点 = 里程计y
paramClient->set_parameters({rclcpp::Parameter("linear",0.0),rclcpp::Parameter("angular",0.0)});
return;
}
double distance = goal - (x-last_x); //x代表里程计 last_x代表初始距离，x-last_x等于走的距离，goal是客户端设置过来走多少米
feedback->distance = distance;
goal_handle->publish_feedback(feedback);
if(distance <= 0.05)
{
break;
}
rate.sleep();
}
//2、设置最终反馈
if(rclcpp::ok())
{
result->x = x;
result->y = y;
last_x = x;
last_y = y;
goal_handle->succeed(result);
paramClient->set_parameters({rclcpp::Parameter("linear",0.0),rclcpp::Parameter("angular",0.0)});
RCLCPP_INFO(this->get_logger(),"任务完成！");
}
}
// using AcceptedCallback = std::function<void (std::shared_ptr<ServerGoalHandle<ActionT>>)>;
//处理连续反馈、最终响应
void handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<Nav>> goal_handle) //这里做的是一个多线程
{
std::thread{std::bind(&NavServer::execute,this,_1),goal_handle}.detach();
}
};
int main(int argc, char * argv[]){
// 初始化 ROS2 客户端；
rclcpp::init(argc, argv);
// 调用 spin 函数，并传入节点对象指针。
rclcpp::spin(std::make_shared<NavServer>());
// 释放资源；
rclcpp::shutdown();
return 0;
}
服务端：
/*
需求：向导航动作服务端发送目标点数据，并处理服务端的响应数据。 需要像服务端提交数据，前进距离是浮点类型
步骤：
1.包含头文件；
2.初始化 ROS2 客户端；
3.定义节点类； （处理终端提交的数据）
3-1.创建动作客户端；
3-2.发送请求数据，并处理服务端响应； //提供一个函数
3-3.处理目标响应； //处理服务端返回的数据是否合法
3-4.处理响应的连续反馈； //处理连续反馈
3-5.处理最终响应。 //处理最终响应
3-6 特定条件下发送取消请求 客户端按下取消 rclcpp::ok()
发送取消任务请求 action_client_ ->async_cancel_goal
4.调用spin函数，并传入节点对象指针；
5.释放资源。
*/
// 1.包含头文件；
// 包含头文件；
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp" //动作通信的头文件
#include "myexer_interfaces/action/nav.hpp" //自定义类型
using namespace std::chrono_literals;
using namespace std::placeholders;
using myexer_interfaces::action::Nav;
// 包含头文件；
#include "rclcpp/rclcpp.hpp"
// 自定义节点类；
class NavClient : public rclcpp::Node{
public:
NavClient(): Node("exe_nav_action_client"){
RCLCPP_INFO(this->get_logger(), "NavClient动作客户端创建！}");
//创建动作客户端
nav_clienr = rclcpp_action::create_client<Nav>(this,"nav");
}
//发送数据请求
void send_goal(float x)
{
//连接服务端，超时退出
if(!nav_clienr->wait_for_action_server(5s))
{
RCLCPP_INFO(this->get_logger(),"服务连接失败");
return;
}
//组织请求数据
auto goal_msg = myexer_interfaces::action::Nav::Goal();
goal_msg.goal = x;
rclcpp_action::Client<Nav>::SendGoalOptions options;
options.goal_response_callback = std::bind(&NavClient::goal_response_callback, this, _1);
options.feedback_callback = std::bind(&NavClient::feedback_callback, this, _1, _2);
options.result_callback = std::bind(&NavClient::result_callback, this, _1);
//发送数据
nav_clienr->async_send_goal(goal_msg,options);
}
private:
rclcpp_action::Client<Nav>::SharedPtr nav_clienr;
//处理目标响应
void goal_response_callback(rclcpp_action::ClientGoalHandle<myexer_interfaces::action::Nav>::SharedPtr goal_handle)
{
if(!goal_handle)
{
RCLCPP_INFO(this->get_logger(),"目标请求被服务器拒绝");
}else{
RCLCPP_INFO(this->get_logger(),"目标请求被接收");
std::thread(&NavClient::cancel_goals,this,goal_handle).detach();
}
}
void cancel_goals(rclcpp_action::ClientGoalHandle<myexer_interfaces::action::Nav>::SharedPtr goal_handle)
{
while(rclcpp::ok()){}
nav_clienr->async_cancel_goal(goal_handle); //处理取消请求
}
// 3-4.处理响应的连续反馈；
void feedback_callback(rclcpp_action::ClientGoalHandle<myexer_interfaces::action::Nav>::SharedPtr goal_handle,
const std::shared_ptr<const myexer_interfaces::action::Nav::Feedback> feedback)
{
(void)goal_handle;
RCLCPP_INFO(this->get_logger(),"距离目标还有 %.2f 米",feedback->distance);
}
// 3-5.处理最终响应。
void result_callback(const rclcpp_action::ClientGoalHandle<myexer_interfaces::action::Nav>::WrappedResult & result)
{
if(result.code == rclcpp_action::ResultCode::SUCCEEDED)
{
RCLCPP_INFO(this->get_logger(),"小车的最终坐为：(%.2f,%.2f)",result.result->x,result.result->y);
}
else{
RCLCPP_INFO(this->get_logger(),"异常");
}
rclcpp::shutdown();
}
};
int main(int argc, char * argv[]){
if(argc != 2)
{
RCLCPP_ERROR(rclcpp::get_logger("rclcpp"),"输入错误，请输入一个数");
}
// 初始化 ROS2 客户端；
rclcpp::init(argc, argv);
// 调用 spin 函数，并传入节点对象指针。
auto client = std::make_shared<NavClient>();
client ->send_goal(atof(argv[1]));
rclcpp::spin(client);
rclcpp::shutdown();
return 0;
}