Ros2_学习梳理_3_动作通信（赵虚左老师）

三、动作通信

动作通信适用于长时间运行任务，结构上由目标、反馈、结果三部分构成。目标发送和结果获取都是对服务通信的封装，连续反馈是对话题通信的封装。

1、自定义动作通信接口消息

1-1.编辑配置文件

package.xml:

action构建工具依赖

<buildtool_depend>rosidl_default_generators</buildtool_depend>

这个是<build_depend>、<exec_depend>、<export_depend>三者的集成

<depend>action_msgs</depend>

声明当前包所属的功能包组

<member_of_group>rosidl_interface_packages</member_of_group>

CMakeList.txt:

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

2、动作通信服务端C++核心实现
/*

1.包含头文件；
2.初始化 ROS2 客户端；
3.定义节点类；
3-1.创建动作服务端；
3-2.处理提交的目标值（回调函数）；
3-3.生成连续反馈与最终响应（回调函数）；
3-4.处理取消任务请求（回调函数）。
4.调用spin函数，并传入节点对象指针；
5.释放资源。

*/
1、包含头文件

#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"//动作通信需要用到的头文件
#include "base_interfaces_demo/action/progress.hpp"//消息类型头文件

3、定义节点类

3-1.创建动作服务端

rclcpp_action::create_server<Progress>(
this,
"get_sum",
std::bind(&ActionServer::handle_goal,this,_1,_2),
std::bind(&ActionServer::handle_cancel,this,_1),
std::bind(&ActionServer::handle_accepted,this,_1)
);

API:创建动作服务端
create_server()
/*
模板：动作服务接口消息类型
参数：
1、当前节点
2、动作服务话题名称
3、提交目标处理函数 （回调函数）
4、取消任务请求处理函数（回调函数）
5、生成连续反馈处理函数（回调函数）
返回值：
动作消息接口类型的指针对象
*/
3-2.处理提交的目标值

 rclcpp_action::GoalResponse handle_goal(const rclcpp_action::GoalUUID & uuid,std::shared_ptr<const Progress::Goal> goal)

参数：
1、rclcpp_action::GoalUUID ：一个全局唯一标识符（UUID），用于标识特定的目标请求。这可以用于在需要时查找或识别目标。
2、Progress::Goal类型的智能指针 ：一个指向目标请求的共享指针
返回值类型：rclcpp_action::GoalResponse类型
Tips：
1、没有用到的成员变量可以用void（）描述，例：void（uuid）
2、一般在rclcpp_action::GoalResponse下存在三个选项：

rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE
rclcpp_action::GoalResponse::ACCEPT_AND_DEFER
rclcpp_action::GoalResponse::REJECT

3-3.处理取消任务请求的函数

  rclcpp_action::CancelResponse handle_cancel(const std::shared_ptr<rclcpp_action::ServerGoalHandle<Progress>> goal_handle)

参数：
1、const std::shared_ptr<ServerGoalHandle> ：表示指向ServerGoalHandle对象的共享指针
返回值：rclcpp_action::CancelResponse

Tips:
1、一般在rclcpp_action::CancelResponse下存在两个选项：

rclcpp_action::CancelResponse::ACCEPT
rclcpp_action::CanceResponse::REJECT

3-4.生成连续反馈与最终响应

std::thread(std::bind(&ActionServer::execute,this,goal_handle)).detach()

一般处理主逻辑都比较耗时，所以此处用到了子线程
std::thread(执行体).detach()-------匿名线程
回调函数execute实现：

3-4-1.初始化goal、feedback、result

//从handle_goal上获取目标信息
const auto goal = goal_handle->get_goal();
//创建一个进度反馈信息的共享指针
auto feedback = std::make_shared<Progress::Feedback>();
//创建一个结果信息的共享指针
auto result = std::make_shared<Progress::Result>();

3-4-1.生成连续反馈返回给客户端

feedback->progress = (double_t)i / goal->num;
goal_handle->publish_feedback(feedback)

3-4-2.生成最终响应结果

result->sum = sum;
goal_andle->succeed(result);

API（扩展）：
1、goal_handle->get_goal();获取目标值
2、rclcpp::Rate rate(1.0);rate.sleep();设置休眠时间
3、goal_handle->is_canceling();判断是否正在取消请求
4、goal_handle->canceled();通知任务被取消并传递结果对象

3.动作通信客户端C++核心实现

/*
1.包含头文件；
2.初始化 ROS2 客户端；
3.定义节点类；
3-1.创建动作客户端；
3-2.发送请求；
3-3.处理目标发送后的反馈（回调函数）；
3-4.处理连续反馈（回调函数）；
3-5.处理最终响应（回调函数）。
4.调用spin函数，并传入节点对象指针；
5.释放资源。
*/
1.包含头文件

#include "rclcpp/rclcpp.hpp" 
#include "rclcpp_ation/rclcpp.hpp"
#include "base_interfaces_demo/action/progress.hpp"

3.定义节点类

3-1.创建动作客户端

rclcpp_action::Client<Progress>::SharedPtr client_ptr_;
this->client_ptr_ = rclcpp_action::create_client<Progress>(this,"get_sum");

API:创建动作客户端
create_client()
/*
模板：动作通信接口
参数：
1、当前节点对象
2、动作通信话题名称
返回值：动作通信客户端对象指针
*/
3-2.发送请求
主函数调用成员函数send_goal。
send_goal实现：

3-2-1.连接服务器

this->clent_ptr_->wait_for_action_server(std::chrono::seconds(10))

API：等待服务器连接
wait_for_action_server()
/*
参数：
1、超时时间
返回值：bool值,连接成功返回True，失败则返回False。
*/
3-2-2.发送具体请求

//设置goal_msg
auto goal_msg = Progress::Goal();
goal_msg.num = num;//提交目标值
//设置options
rclcpp_action::Client<Progress>::SendGoalOptions options;
options.goal_response_callback =std::bind(&MinimalActionClient::goal_response_callback, this, _1);
options.feedback_callback =std::bind(&MinimalActionClient::feedback_callback, this, _1, _2);
options.result_callback =std::bind(&MinimalActionClient::result_callback, this, _1);
//发送异步请求
client_->async_send_goal(goal_msg,options),

API：异步发送请求
asyn_send_goal(参数1，参数2)
/*
参数：
1、const base_interfaces-demo::action::Progress::Goal &goal;自定义的动作接口消息类型下的目标
2、const rclcpp_action::Client<base_interfaes_demo::action::Progress>::SendGoalOptions &options;发送目标选项，设置需要处理的回调函数（1、处理目标值的服务响应2、处理连续反馈3、处理最终响应）
*/

1.处理目标发送后的反馈（回调函数）
/*
参数：
1、指向ClientGoalHandle类型的实例的智能指针，它管理目标的生命周期，并提供接口来查询目标状态、取消目标、获取结果等。
*/

void goal_response_callback(rclcpp::action::ClientGoaHandle<Progress>::SharedPtr goal_handle)
{
//客户端首先发送一个目标值给服务端，服务端进行判断看目标值是否可以被接受，然后响应给客户端，如果可处理的话，goal_handle里面是由内容的，如果不可处理的话，goal_handle是个空指针。
if(!goal_handle)//goal_handle如果是个空指针
//目标请求被服务端拒绝，否则被接受
}

2.处理连续反馈（回调函数）
/*
参数：
1、这是个智能指针，用于管理特定目标（goal）的生命周期和状态。
2、指向反馈消息的智能指针。
*/

void feedback_callback(rclcpp_action::ClientGoalHandle<Progress>::SharedPtr goal_handle,const std::shared_ptr<Progress::Feedback> feedbak)
{
//通过feedback获取反馈内容
int_32_t progress = (int_32_t)(feedback->progress * 100)
}

3.处理最终响应（回调函数）
/*
参数：
1、通常包含一个布尔值表示目标是否成功完成，以及一个指向 Result 消息类型的共享指针，该消息包含了动作服务器返回的具体结果数据。
*/

void result_callback(const rclcpp_action::ClientGoalHandle<Progress>::WrappedResult &result)
{
//result.code
//通过状态码判断最终结果状态
if(result.code == rclcpp_action::ResultCode::SUCCEEDED)
{
//成功
}
else if(result.code == rclcpp_action::ResultCode::ABORTED)
{
//被中断
}
else if(result.code == rclcpp_action::ResultCode::CANCELED)
{
//被取消
}else
{
//未知异常
}
}

3、动作通信服务端Python核心实现
“”"
1.导包；
2.初始化 ROS2 客户端；
3.定义节点类；
3-1.创建动作服务端；
3-2.生成连续反馈；
3-3.生成最终响应。
4.调用spin函数，并传入节点对象；
5.释放资源。
“”"
1.导包

import rclpy
from rclpy.action import ActionServer
from rclpy.node import Node
from base_interfaces_demo.action  import Progress

3.定义节点类
Tips:
其中处理请求数据实现和处理取消任务请求实现都有默认函数，可根据需求进行重写实现。

3-1.创建动作服务端
ActionServer(node,action_type,action_name,execute_callback)
API:创建动作通信服务端
ActionServer()

/*
参数：
1、ActionServer创建时所依赖的节点（node）
2、动作通信接口类型(action_type)
3、将服务端和客户端关联在一起的话题名称(action_name)
4、进行主逻辑的回调函数(execute_callback)
返回值：
*/
self._action_server = ActionServer(
self,
Progress,
‘get_sum’,
self.execute_callback
)
3-2.生成连续反馈并生成最终响应
goal_handle的类型是ServerGoalHandle类型的，里面包含了各种方法。

def exeute_callback(self,goal_handle)
{
***3-2-1.生成连续反馈***
feedback_msg = Progress.Feedback()
feedback_msg.progress = i / goal_handle.request.num
goal_handle.publish_feedback(feedback_msg)
***3-2-2.响应最终结果***
goal_handle.succeed()
result = Progress.Result()
return result
}

4.动作通信客户端Python核心实现
“”"
1.导包；
2.初始化 ROS2 客户端；
3.定义节点类；
3-1.创建动作客户端；
3-2.发送请求；
3-3.处理目标发送后的反馈；
3-4.处理连续反馈；
3-5.处理最终响应。
4.调用spin函数，并传入节点对象；
5.释放资源。

“”"
1.导包

import rclpy
from rclpy.action import ActionClient
from rclpy.node import Node
from base_interfaces_demo.action import Progress

3.定义节点类

3-1.创建动作客户端

self._action_client = ActionClient(self,Progress,'get_sum')
API:创建动作客户端
ActionClient(node,action_type,action_name)
/*
参数：
1、构建ActionClient时所依赖的节点
2、动作通信服务接口
3、动作服务端和客户端关联的话题
*/

3-2.发送请求

goal = Progress.Goal()
goal.num = num
self.aaction_client.wait_for_server()
self.future = self.action_client.send_goal_async(goal,self.fb_callback)
self.future.add_done_callback(self.goal_response_callback)

3-2-1.等待连接服务器

self.action_client.wait_for_server()//一直等待服务端连接
API:等待服务端连接
wait_for_server(timeout)
/*
参数：
1、超时时间
*/

3-2-2.发送具体请求

API:异步发送目标请求
send_goal_async(goal_msg,feed_callback)
/*
1、目标数据goal
2、回调函数feed_callback
返回值：返回一个ClientGoalHandle类型的Future对象
/
API:添加一个回调函数
add_done_callback(callback)
/
这是future下的函数，也就是ClientGoalHandle类下的成员函数
参数：
1、回调函数
*/
3-3.处理目标发送后的反馈
def goal_response_callback(self,future)
3-3-1.获取目标句柄
API：用于异步获取并返回操作结果，有阻塞行为
future.result()

goal_handle = future.result()

3-3-2.判断目标是否被服务器正常接收
API:goal_handle对象的一个属性，用于检查目标是否被action server接受。
goal_handle.accepted()
API:用于异步获取目标的最终结果,没有阻塞行为，返回一个Future对象，判断结果是否可用
get_result_async()

if not goal_handle.accepted():
	return
self.result_future = goal_handle.get_result_async()
self.result_future.add_done_callback(self.get_result_callback)

3-5.处理最终响应

def get_result_callback(self.future):
	result = future.result().result

3-4.处理连续反馈

连续反馈的回调函数：

//通过fb_msg获取到连续反馈回来的数据
def fb_callback(self,fb_msg):
		progress = fb_msg.feedback.progress