其他
开发者说丨ROS理论与实践③：ROS通信机制进阶
上一章《开发者说丨ROS理论与实践②：ROS通信机制》结尾介绍到：在ROS中话题通信、服务通信、参数服务器这三种通信协议并不能完全满足ROS中多样的应用场景，本章将介绍上述通信协议在一些特定场景下存在的缺陷，以及对应的优化策略：action和动态配置参数。本章预期达成的学习目标：
了解服务通信应用的局限性（action的应用场景）。
了解参数服务器应用的局限性（动态配置参数的应用场景）。
熟练掌握action的理论模型与实现流程。
独立完成action相关案例。
熟练掌握动态配置参数的实现流程。
独立完成动态配置参数的相关案例。
案例演示
1. action案例：乌龟自主运动到指定坐标点，运动过程中回传自身当前坐标。
2. 动态配置参数案例：通过图形化界面修改参数，节点可以实时获取修改后的参数数据。
下面是由社区开发者—赵虚左提供的文章，对ROS通信机制进阶进行详细讲解，希望这篇文章能给感兴趣的开发者带来更多帮助。
ENJOY THE FOLLOWING
关于action通信，我们先从应用场景开始介绍，场景描述如下：
机器人导航到某个目标点，此过程需要一个节点A发布目标信息，然后一个节点B接收到请求并控制移动，最终响应目标达成状态信息。
乍一看，这好像是服务通信实现，因为需求中要A发送目标，B执行并返回结果，这是一个典型的基于请求响应的应答模式，不过，如果只是使用基本的服务通信实现，存在一个问题：导航是一个过程，是耗时操作，如果使用服务通信，那么只有在导航结束时，才会产生响应结果，而在导航过程中，节点A是不会获取到任何反馈的，从而可能出现程序"假死"的现象，过程的不可控意味着不良的用户体验，以及逻辑处理的缺陷（比如：导航中止的需求无法实现）。更合理的方案应该是：导航过程中，可以连续反馈当前机器人状态信息，当导航终止时，再返回最终的执行结果。在ROS中，该实现策略称之为：action通信。
① 概念
action是一种类似于服务通信的实现，也是基于请求和响应的，但是不同的是，在请求和响应的过程中，服务端还可以连续的反馈当前任务进度，客户端可以接收连续反馈并且还可以取消任务。
② 作用
一般适用于耗时的请求响应场景，用以获取连续的状态反馈。
③案例
创建两个ROS节点，服务器和客户端，客户端可以向服务器发送目标数据N（一个整型数据）服务器会计算1到N之间所有整数的和，这是一个循环累加的过程，返回给客户端，这是基于请求响应模式的，又已知服务器从接收到请求到产生响应是一个耗时操作，每累加一次耗时0.1s，为了良好的用户体验，需要服务器在计算过程中，每累加一次，就给客户端响应一次百分比格式的执行进度，使用action实现。
action、srv、msg文件内的可用数据类型一致，且三者实现流程类似：
按照固定格式创建action文件
编辑配置文件
编译生成中间文件
1.定义action文件
action文件组成分为三部分：请求目标值，最终响应结果，连续反馈，三者之间使用---分割。
示例：首先功能包下新建action目录，新增Xxx.action，内容如下：
1#目标值
2int32 add_max_num
3---
4#最终结果
5int32 final_result
6---
7#连续反馈
8float64 progress_bar
<左右滑动以查看完整代码>
2.编译配置文件
package.xml
1....
2<build_depend>actionlibbuild_depend>
3<build_depend>actionlib_msgsbuild_depend>
4....
5<exec_depend>actionlibexec_depend>
6<exec_depend>actionlib_msgsexec_depend>
<左右滑动以查看完整代码>
CMakeLists.txt
1find_package(catkin REQUIRED COMPONENTS
2roscpp
3rospy
4std_msgs
5actionlib
6actionlib_msgs
7)
8add_action_files(
9FILES
10AddInts.action
11)
12generate_messages(
13DEPENDENCIES
14std_msgs
15actionlib_msgs
16)
17catkin_package(
18# INCLUDE_DIRS include
19# LIBRARIES demo04_action
20CATKIN_DEPENDS roscpp rospy std_msgs actionlib actionlib_msgs
21# DEPENDS system_lib
22)
<左右滑动以查看完整代码>
3.编译
编译后会生成一些中间文件：
C++ 需要调用的中间文件（.../工作空间/devel/include/包名/xxx.h）。
Python需要调用的中间文件（.../工作空间/devel/lib/python3/dist-packages/包名/msg）。
PS：
在C++中间文件中，生成了多个.h 文件，其中xxxAction.h包含了 xxxActionGoal.h、xxxActionResult.h 和xxxActionFeedback.h，后面三者又分别包含了对应的xxxGoal.h、xxxResult.h 和 xxx.Feedback.h，导包时只需要导入xxxAction.h即可。
0.vscode配置
需要像之前自定义msg实现一样配置c_cpp_properies.json文件，如果以前已经配置且没有变更工作空间，可以忽略，如果需要配置，配置方式与之前相同：
1{
2"configurations": [
3 {
4 "browse": {
5 "databaseFilename": "",
6 "limitSymbolsToIncludedHeaders": true
7 },
8 "includePath": [
9 "/opt/ros/noetic/include/**",
10 "/usr/include/**",
11 "/xxx/yyy工作空间/devel/include/**" //配置 head 文件的路径
12 ],
13 "name": "ROS",
14 "intelliSenseMode": "gcc-x64",
15 "compilerPath": "/usr/bin/gcc",
16 "cStandard": "c11",
17 "cppStandard": "c++17"
18 }
19],
20"version": 4
21}
<左右滑动以查看完整代码>
1.服务端
1/*
2需求:
3 ROS 节点，服务器和客户端，
4 客户端可以向服务器发送目标数据N(一个整形数据)
5 服务器会计算 1 到 N 之间所有整数的和，返回给客户端，
6 这是基于请求响应模式的，
7 又已知服务器从接收到请求到产生响应是一个耗时操作，
8 为了良好的用户体验，
9 需要服务器在循环累加过程中，每累加一次，
10 就给客户端响应一次执行进度，使用 action实现
11
12已经实现:
13 定义了 action 消息体
14
15服务器端实现:
16 1.包含头文件
17 此处无需包含 ros/ros.h 因为 actionlib 已经包含了
18 2.初始化 ROS 节点
19 3.创建 ROS 句柄
20 4.创建服务器对象
21 5.处理请求数据产生响应结果，中间还要连续反馈
22 6.spin
23
24*/
25#include "actionlib/server/simple_action_server.h"
26#include "demo04_action/AddIntsAction.h"
27
28typedef actionlib::SimpleActionServer<:addintsaction> Server;
29
30// void executeCb(const demo04_action::AddIntsGoal::ConstPtr& g, Server* s){
31void executeCb(const demo04_action::AddIntsGoalConstPtr& g, Server* s){
32//1.解析提交的目标值
33int goal = g->add_max_num;
34ROS_INFO("需要计算的目标值:%d --- ",goal);
35//2.循环累加数据，并连续反馈
36ros::Rate r(10);
37int sum = 0;
38for (int i = 1; i <= goal; i++)
39{
40 sum += i;
41 //响应进度
42 demo04_action::AddIntsFeedback fb_obj;
43 fb_obj.progress_bar = i / (double)goal;
44 s->publishFeedback(fb_obj);
45 r.sleep();
46}
47//3.响应最终结果
48//将最终结果封装进对象
49demo04_action::AddIntsResult result_obj;
50result_obj.final_result = sum;
51//响应
52s->setSucceeded(result_obj);
53ROS_INFO("服务器响应的结果:%d",result_obj.final_result);
54}
55
56int main(int argc, char *argv[])
57{
58setlocale(LC_ALL,"");
59// 2.初始化 ROS 节点
60ros::init(argc,argv,"action_server");
61// 3.创建 ROS 句柄
62ros::NodeHandle nh;
63// 4.创建服务器对象(类型冗余，可以重命名优化)
64// actionlib::SimpleActionServer<:addintsaction>
65/* SimpleActionServer(ros::NodeHandle n,
66 std::string name,
67 boost::function execute_callback,
68 bool auto_start)
69*/
70Server server(nh,"addInts",boost::bind(&executeCb,_1,&server),false);
71server.start();
72ROS_INFO("action服务已经启动....");
73// 5.处理请求数据产生响应结果，中间还要连续反馈
74// 6.spin
75ros::spin();
76return 0;
77}
<左右滑动以查看完整代码>
2.客户端
1/*
2需求:
3 ROS 节点，服务器和客户端，
4 客户端可以向服务器发送目标数据N(一个整形数据)
5 服务器会计算 1 到 N 之间所有整数的和，返回给客户端，
6 这是基于请求响应模式的，
7 又已知服务器从接收到请求到产生响应是一个耗时操作，
8 为了良好的用户体验，
9 需要服务器在循环累加过程中，每累加一次，
10 就给客户端响应一次执行进度，使用 action实现
11
12已经实现:
13 定义了 action 消息体
14
15服务器端实现:
16 1.包含头文件
17 此处无需包含 ros/ros.h 因为 actionlib 已经包含了
18 2.初始化 ROS 节点
19 3.创建 ROS 句柄(可以需要，也可以不需要)
20 4.创建客户端对象
21 5.等待服务启动
22 6.组织并发送目标值，回调函数处理连续反馈以及最终响应结果
23 7.spin()
24
25*/
26#include "actionlib/client/simple_action_client.h"
27#include "demo04_action/AddIntsAction.h"
28
29typedef actionlib::SimpleActionClient<:addintsaction> Client;
30
31void doneCb(const actionlib::SimpleClientGoalState &state, const demo04_action::AddIntsResult::ConstPtr &result){
32// void doneCb(const actionlib::SimpleClientGoalState &state, const demo04_action::AddIntsResultConstPtr &result){
33ROS_INFO("任务状态:%d",state.state_);
34if (state.state_ == state.SUCCEEDED)
35{
36 ROS_INFO("最终结果:%d",result->final_result);
37} else {
38 ROS_ERROR("任务失败");
39}
40
41}
42
43void activeCb(){
44ROS_INFO("服务被激活");
45}
46
47// void feedbackCb(const demo04_action::AddIntsFeedbackConstPtr &feedback){
48void feedbackCb(const demo04_action::AddIntsFeedback::ConstPtr &feedback){
49ROS_INFO("当前进度:%.1f %%",feedback->progress_bar * 100);
50}
51
52int main(int argc, char *argv[])
53{
54setlocale(LC_ALL,"");
55// 2.初始化 ROS 节点
56ros::init(argc,argv,"action_client");
57// 3.创建 ROS 句柄
58ros::NodeHandle nh;
59// 4.创建客户端对象(类型冗余，重命名优化)
60// actionlib::SimpleActionClient<:addintsaction>
61Client client(nh,"addInts",true);
62// 5.等待服务启动
63client.waitForServer();
64// ros::service::waitForService("addInts"); //不可以使用
65// 6.组织并发送目标值，回调函数处理连续反馈以及最终响应结果
66demo04_action::AddIntsGoal goal_obj;
67goal_obj.add_max_num = 100;
68
69/*
70 void sendGoal
71 (const demo04_action::AddIntsGoal &goal,
72 boost::function done_cb,
73 boost::function active_cb = actionlib::SimpleActionClient<:addintsaction>::SimpleActiveCallback(),
74 boost::function feedback_cb = actionlib::SimpleActionClient<:addintsaction>::SimpleFeedbackCallback())
75
76*/
77client.sendGoal(goal_obj,&doneCb,&activeCb,&feedbackCb);
78
79//设置等待时间
80bool flag = client.waitForResult(ros::Duration(5));
81if (flag)
82{
83 ROS_INFO("任务顺利结束");
84
85} else {
86 ROS_INFO("任务超时");
87 // client.cancelGoal();
88}
89
90// 7.spin() --- 设置等待时间时，不设置 spin()
91// ros::spin();
92
93return 0;
94}
<左右滑动以查看完整代码>
PS：
等待服务启动，只可以使用client.waitForServer();，之前服务中等待启动的另一种方式ros::service::waitForService("addInts");不适用。
参数替换： xxxConstPtr可以和 xxx::ConstPtr置换。
比如：demo04_action::AddIntsFeedbackConstPtr与demo04_action::AddIntsFeedback::ConstPtr。
3.编译配置文件
1add_executable(Action_Server src/Action_Server.cpp)
2add_executable(Action_Client src/Action_Client.cpp)
3
4...
5
6add_dependencies(Action_Server ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
7add_dependencies(Action_Client ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
8
9...
10
11target_link_libraries(Action_Server
12${catkin_LIBRARIES}
13)
14target_link_libraries(Action_Client
15${catkin_LIBRARIES}
16)
<左右滑动以查看完整代码>
4.执行
0.vscode配置
需要像之前自定义msg实现一样配置settings.json文件，如果以前已经配置且没有变更工作空间，可以忽略，如果需要配置，配置方式与之前相同：
1{
2"python.autoComplete.extraPaths": [
3 "/opt/ros/noetic/lib/python3/dist-packages",
4 "/xxx/yyy工作空间/devel/lib/python3/dist-packages"
5]
6}
<左右滑动以查看完整代码>
1.服务端
1#! /usr/bin/env python
2"""
3需求:
4 ROS 节点，服务器和客户端，
5 客户端可以向服务器发送目标数据N(一个整形数据)
6 服务器会计算 1 到 N 之间所有整数的和，返回给客户端，
7 这是基于请求响应模式的，
8 又已知服务器从接收到请求到产生响应是一个耗时操作，
9 为了良好的用户体验，
10 需要服务器在循环累加过程中，每累加一次，
11 就给客户端响应一次执行进度，使用 action实现
12
13已经实现:
14 定义了 action 消息体
15
16服务器端实现:
17 1.导包
18 2.初始化 ROS 节点
19 3.使用类封装，然后创建对象
20 4.创建服务器对象
21 5.处理请求数据产生响应结果，中间还要连续反馈
22 6.spin
23
24"""
25import rospy
26import actionlib
27from demo04_action.msg import *
28
29class MyActionServer:
30def __init__(self):
31 #创建服务对象
32 #SimpleActionServer(name, ActionSpec, execute_cb=None, auto_start=True)
33 self.server = actionlib.SimpleActionServer("addInts",AddIntsAction,self.executeCb,False)
34 #启动服务
35 self.server.start()
36
37def executeCb(self,goal):
38 #1.解析目标值
39 goal_num = goal.add_max_num
40 rospy.loginfo("提交数据是:%d",goal_num)
41 #2.循环累加，连续反馈
42 r = rospy.Rate(10)
43 sum = 0
44 for i in range(1,goal_num + 1):
45 sum += i
46 fb_obj = AddIntsFeedback()
47 fb_obj.progress_bar = i / goal_num
48 self.server.publish_feedback(fb_obj)
49 rospy.loginfo("当前进度:%.1f %%",fb_obj.progress_bar * 100)
50 r.sleep()
51 #3.响应最终结果
52 result = AddIntsResult()
53 result.final_result = sum
54 rospy.loginfo("最终结果:%d",sum)
55 self.server.set_succeeded(result)
56if __name__ == "__main__":
57rospy.init_node("action_server_p")
58server = MyActionServer()
59rospy.spin()
<左右滑动以查看完整代码>
2.客户端
1#! /usr/bin/env python
2"""
3需求:
4 ROS 节点，服务器和客户端，
5 客户端可以向服务器发送目标数据N(一个整形数据)
6 服务器会计算 1 到 N 之间所有整数的和，返回给客户端，
7 这是基于请求响应模式的，
8 又已知服务器从接收到请求到产生响应是一个耗时操作，
9 为了良好的用户体验，
10 需要服务器在循环累加过程中，每累加一次，
11 就给客户端响应一次执行进度，使用 action实现
12
13已经实现:
14 定义了 action 消息体
15
16服务器端实现:
17 1.导包
18 2.初始化 ROS 节点
19 3.创建 action Client 对象
20 4.等待服务
21 5.组织结果对象并发送
22 6.编写回调, 激活、连续反馈、最终响应
23 7.spin
24
25"""
26# 1.导包
27import rospy
28import actionlib
29from demo04_action.msg import *
30
31def activeCb():
32rospy.loginfo("服务被激活....")
33
34def feedbackCb(fb):
35rospy.loginfo("当前进度:%.1f %%",fb.progress_bar * 100)
36
37def doneCb(state,r):
38rospy.loginfo("状态:%d",state)
39rospy.loginfo("最终结果:%d",r.final_result)
40
41
42if __name__ == "__main__":
43# 2.初始化 ROS 节点
44rospy.init_node("action_client_p")
45# 3.创建 action Client 对象
46client = actionlib.SimpleActionClient("addInts",AddIntsAction)
47# 4.等待服务
48client.wait_for_server()
49# 5.组织目标对象并发送
50goal_obj = AddIntsGoal()
51goal_obj.add_max_num = 30
52# 6.编写回调, 激活、连续反馈、最终响应
53client.send_goal(goal_obj,doneCb,activeCb,feedbackCb)
54
55flag = client.wait_for_result(rospy.Duration(5))
56if flag:
57 rospy.loginfo("执行成功!")
58else:
59 rospy.loginfo("执行超时!")
60
61# 7.spin
62# rospy.spin()
<左右滑动以查看完整代码>
3.编辑配置文件
先为Python文件添加可执行权限：chmod+x *.py。
1catkin_install_python(PROGRAMS
2scripts/Action_Server_p.py
3scripts/Action_Client_p.py
4DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
5)
<左右滑动以查看完整代码>
4.执行
需求描述： 让小乌龟到达指定目标点，并且在运行过程中，实时返回当前位姿信息。
结果演示
实现分析：
需要启动乌龟显示节点。
需求是带连续反馈的请求，主体需要使用action实现。
客户端请求需要发送目标点，并接收反馈。
服务端需要接收请求坐标，然后控制乌龟运动，并连续反馈乌龟位姿（先订阅再反馈）。
必须了解乌龟控制与位姿使用的的话题和消息。
实现流程：
通过ros命令来获取乌龟运动控制、乌龟位姿相关的话题和消息。
定义action文件来设置请求数据、响应结果与连续反馈数据。
定义action客户端向服务器发送请求，并处理响应。
定义action服务端解析目标坐标，控制乌龟向坐标点运动，反馈位姿信息，响应最终执行结果。
执行。
1.准备
控制乌龟运动，那么首先需要了解：乌龟节点turtlesim_node订阅的话题以及使用的消息类型。
获取话题：/turtle1/cmd_vel。
rostopic list
<左右滑动以查看完整代码
获取消息类型：geometry_msgs/Twist。
rostopic type /turtle1/cmd_vel
<左右滑动以查看完整代码>
获取消息格式：
rosmsg info geometry_msgs/Twist
<左右滑动以查看完整代码>
响应结果：
1geometry_msgs/Vector3 linear
2float64 x
3float64 y
4float64 z
5geometry_msgs/Vector3 angular
6float64 x
7float64 y
8float64 z
<左右滑动以查看完整代码>
订阅乌龟在窗体中坐标（位姿），需要先了解乌龟节点turtlesim_node发布的话题以及消息类型。
获取话题：/turtle1/pose。
rostopic list
<左右滑动以查看完整代码>
获取消息类型：turtlesim/Pose。
rostopic type /turtle1/pose
<左右滑动以查看完整代码>
获取消息格式：
rosmsg info turtlesim/Pose
<左右滑动以查看完整代码>
响应结果：
1float32 x
2float32 y
3float32 theta
4float32 linear_velocity
5float32 angular_velocity
<左右滑动以查看完整代码>
2.定义action文件
action文件需要设置目标点的坐标点、最终到达位置的坐标点、连续反馈的乌龟的位姿。
1.action实现方案A（C++）
Server端：
1/*
2通过 action 控制乌龟运动到指定的目的地，移动过程中连续接收乌龟反馈
3
4分析:
5 1.使用action
6 2.需要订阅乌龟的位姿
7 3.需要发布乌龟运动的速度信息
8
9实现流程(阶段1:实现action通信):
10 1.包含头文件
11 2.初始化 ros 节点
12 3.创建 ros 句柄
13 4.创建 action server 对象
14 5.处理请求
15 6.spin
16实现流程(阶段2:实现位姿订阅与速度发布)
17 实现逻辑:
18 1.在回调函数中，需要解析目标值
19
20 2.根据目标值计算生成速度指令并发布
21 3.并且需要时时反馈乌龟位姿
22
23 4.到达目标点后生成最终响应结果
24
25*/
26//1.包含头文件
27#include "ros/ros.h"
28#include "actionlib/server/simple_action_server.h"
29#include "demo02_test_action/turtle_moveAction.h"
30#include "turtlesim/Pose.h"
31#include "geometry_msgs/Twist.h"
32
33typedef actionlib::SimpleActionServer<demo02_test_action::turtle_moveaction> Server;
34
35//定义位姿相关的结构体
36struct TurtlePose {
37double x;
38double y;
39double z;
40}final_pose,current_pose,goal_pose;
41ros::Publisher pub;
42
43
44//处理请求
45void doReq(const demo02_test_action::turtle_moveGoalConstPtr& goal, Server* s){
46ROS_INFO("目标值:x=%.2f, y=%.2f, theta=%.2f",goal->turtle_goal_x,goal->turtle_goal_y,goal->turtle_goal_z);
47goal_pose.x = goal->turtle_goal_x;
48goal_pose.y = goal->turtle_goal_y;
49goal_pose.z = goal->turtle_goal_z;
50
51demo02_test_action::turtle_moveFeedback fb;
52geometry_msgs::Twist vec_msg;
53demo02_test_action::turtle_moveResult result;
54
55ros::Rate r(10);
56while(true)
57{
58 //已有目标坐标信息:goal_pose, 和当前坐标信息: current_pose
59 //根据二者计算出角度与速度
60 // 线速度 = ((目标x - 当前x)平方 + (目标y - 当前y)平方)然后再开方 再 * 速度系数
61 double jianju = sqrt(pow(goal_pose.x - current_pose.x,2) + pow(goal_pose.y - current_pose.y,2));
62 vec_msg.linear.x = 0.5 * jianju;
63 // 角速度 = (atan2(目标y - 当前y, 目标x - 当前x) - 当前角度) * 系数
64 vec_msg.angular.z = 2 * (atan2(goal_pose.y - current_pose.y, goal_pose.x - current_pose.x) - current_pose.z);
65 //将计算后的结果通过 pub 发送
66 pub.publish(vec_msg);
67
68 //将当前坐标信息赋值给 fb
69 fb.turtle_current_x = current_pose.x;
70 fb.turtle_current_y = current_pose.y;
71 fb.turtle_current_z = current_pose.z;
72 s->publishFeedback(fb);
73
74 //还要设置循环结束的条件(比如:距离目标点 0.1m 之内时，即中止)
75 if (jianju < 0.1)
76 {
77 break;
78
79 }
80
81
82 r.sleep();
83
84}
85
86
87result.turtle_final_x = current_pose.x;
88result.turtle_final_y = current_pose.y;
89result.turtle_final_z = current_pose.z;
90
91s->setSucceeded(result);
92
93}
94
95//订阅位姿的回调函数，将订阅的坐标信息保存进结构体
96void doPose(const turtlesim::Pose::ConstPtr& pose){
97current_pose.x = pose->x;
98current_pose.y = pose->y;
99current_pose.z = pose->theta;
100}
101
102int main(int argc, char *argv[])
103{
104setlocale(LC_ALL,"");
105// 2.初始化 ros 节点
106ros::init(argc,argv,"move_server");
107// 3.创建 ros 句柄
108ros::NodeHandle nh;
109// 4.创建 action server 对象
110
111//创建 pose 的订阅节点
112ros::Subscriber sub = nh.subscribe<:pose>("/turtle1/pose",1000,doPose);
113
114//创建 cmd_vel 的发布节点
115pub = nh.advertise<:twist>("/turtle1/cmd_vel",1000);
116
117//boost::function execute_callback
118Server server(nh,"turtle_move",boost::bind(&doReq,_1,&server), false);
119//启动服务
120server.start();
121ROS_INFO("服务启动成功！！！！");
122// 5.处理请求
123// 6.spin
124ros::spin();
125return 0;
126}
</demo02_test_action::turtle_moveaction><左右滑动以查看完整代码>
Client端：
1/*
2通过 action 控制乌龟运动到指定的目的地，移动过程中连续接收乌龟反馈
3
4分析:
5 1.使用action
6 2.需要订阅乌龟的位姿
7 3.需要发布乌龟运动的速度信息
8
9实现流程(阶段1:实现action通信):
10 1.包含头文件
11 2.初始化 ros 节点
12 3.创建 ros 句柄
13 4.创建 action client 对象
14 5.发送请求
15 6.spin
16
17
18*/
19
20//1.包含头文件
21#include "ros/ros.h"
22#include "actionlib/client/simple_action_client.h"
23#include "demo02_test_action/turtle_moveAction.h"
24
25typedef actionlib::SimpleActionClient<demo02_test_action::turtle_moveaction> Client;
26
27
28//服务被激活函数
29void activeCb(){
30ROS_INFO("服务已经连接成功");
31}
32
33//连续反馈函数
34void fbCb(const demo02_test_action::turtle_moveFeedbackConstPtr &feedback){
35ROS_INFO("连续反馈:x=%.2f,y=%.2f,z=%.2f;",
36 feedback->turtle_current_x,
37 feedback->turtle_current_y,
38 feedback->turtle_current_z
39 );
40}
41
42//最终响应结果
43void doneCb(const actionlib::SimpleClientGoalState &state,
44 const demo02_test_action::turtle_moveResultConstPtr &result){
45ROS_INFO("响应状态:%d",state.state_);
46ROS_INFO("最终-------------反馈:x=%.2f,y=%.2f,z=%.2f;",
47 result->turtle_final_x,
48 result->turtle_final_y,
49 result->turtle_final_z
50 );
51}
52
53int main(int argc, char *argv[])
54{
55setlocale(LC_ALL,"");
56// 2.初始化 ros 节点
57ros::init(argc,argv,"move_client");
58// 3.创建 ros 句柄
59ros::NodeHandle nh;
60// 4.创建 action client 对象
61Client client(nh,"turtle_move",true);
62client.waitForServer();
63// 5.发送请求
64//void sendGoal(const demo02_test_action::turtle_moveGoal &goal,
65// boost::function done_cb,
66// boost::function active_cb,
67// boost::function feedback_cb)
68demo02_test_action::turtle_moveGoal goal;
69nh.getParam("x",goal.turtle_goal_x);
70nh.getParam("y",goal.turtle_goal_y);
71nh.getParam("z",goal.turtle_goal_z);
72
73client.sendGoal(goal,&doneCb, &activeCb, &fbCb);
74// 6.spin
75ros::spin();
76return 0;
77}
</demo02_test_action::turtle_moveaction><左右滑动以查看完整代码>
2.action实现方案B（Python）
Server端：
1#! /usr/bin/env python
2"""
3通过 action 控制乌龟运动到指定的目的地，移动过程中连续接收乌龟反馈
4
5分析:
6 1.使用action
7 2.需要订阅乌龟的位姿
8 3.需要发布乌龟运动的速度信息
9
10实现流程(阶段1:实现action通信):
11 1.包含头文件
12 2.初始化 ros 节点
13 3.创建 action server 对象
14 4.处理请求
15 5.spin
16实现流程(阶段2:实现位姿订阅与速度发布)
17
18
19"""
20import rospy
21from demo02_test_action.msg import turtle_moveAction, turtle_moveGoal, turtle_moveResult, turtle_moveFeedback
22from actionlib import SimpleActionServer
23from geometry_msgs.msg import Twist
24from turtlesim.msg import Pose
25import math
26#单独创建一个类来处理请求
27class MyServer:
28def __init__(self,topic):
29 self.pub = rospy.Publisher("/turtle1/cmd_vel",Twist,queue_size=1000)
30 self.sub = rospy.Subscriber("/turtle1/pose",Pose,self.doPose,queue_size=1000)
31 self.server = SimpleActionServer(topic,turtle_moveAction,self.exeCb,False)
32 self.server.start()
33 rospy.loginfo("服务已经启动....")
34
35def doPose(self,pose):
36 self.current_x = pose.x
37 self.current_y = pose.y
38 self.current_z = pose.theta
39
40def exeCb(self,goal):
41 #解析目标数据
42 goal_x = goal.turtle_goal_x
43 goal_y = goal.turtle_goal_y
44 goal_z = goal.turtle_goal_z
45 rospy.loginfo("目标点:%.2f,%.2f,%.2f",goal_x,goal_y,goal_z)
46
47 #根据目标数据与当前坐标计算速度与角速度，并发布
48 #连续回传当前坐标信息
49 rate = rospy.Rate(10)
50 while True:
51 fb = turtle_moveFeedback()
52 msg = Twist()
53 try:
54 # 订阅到的乌龟的位姿
55 fb.turtle_current_x = self.current_x
56 fb.turtle_current_y = self.current_y
57 fb.turtle_current_z = self.current_z
58 self.server.publish_feedback(fb)
59 # 计算并发布速度信息
60 jianju = math.sqrt(math.pow(goal_x - self.current_x,2) + math.pow(goal_y - self.current_y,2))
61 msg.linear.x = 0.5 * jianju
62 msg.linear.y = 0
63 msg.linear.z = 0
64
65 msg.angular.x = 0
66 msg.angular.y = 0
67 msg.angular.z = 2 * (math.atan2(goal_y - self.current_y, goal_x - self.current_x) - self.current_z);
68
69 self.pub.publish(msg)
70 if jianju < 0.1:
71 break
72 except Exception as e:
73 rospy.logwarn("还没有订阅到乌龟信息，请稍候...")
74 rate.sleep()
75
76
77
78
79 #响应最终信息
80 final_turtle = turtle_moveResult()
81 final_turtle.turtle_final_x = self.current_x
82 final_turtle.turtle_final_y = self.current_y
83 final_turtle.turtle_final_z = self.current_z
84
85 self.server.set_succeeded(final_turtle)
86
87if __name__ == "__main__":
88rospy.init_node("move_server_p")
89server = MyServer("turtle_move")
90rospy.spin()
<左右滑动以查看完整代码>
Client端：
1#! /usr/bin/env python
2"""
3通过 action 控制乌龟运动到指定的目的地，移动过程中连续接收乌龟反馈
4
5分析:
6 1.使用action
7 2.需要订阅乌龟的位姿
8 3.需要发布乌龟运动的速度信息
9
10实现流程(阶段1:实现action通信):
11 1.包含头文件
12 2.初始化 ros 节点
13 3.创建 action client 对象
14 4.发送请求
15 5.spin
16
17"""
18import rospy
19from demo02_test_action.msg import turtle_moveAction, turtle_moveFeedback, turtle_moveGoal, turtle_moveResult
20from actionlib import SimpleActionClient
21from actionlib import GoalStatus
22
23def activeCb():
24rospy.loginfo("服务已经激活....")
25
26def fbCb(fb):
27rospy.loginfo("连续反馈的坐标信息:x=%.2f,y=%.2f,z=%.2f",
28 fb.turtle_current_x,
29 fb.turtle_current_y,
30 fb.turtle_current_z
31 )
32
33def doneCb(state,result):
34rospy.loginfo("成功了吗?%d",(state == GoalStatus.SUCCEEDED))
35rospy.loginfo("最终终点坐标:x=%.2f,y=%.2f,z=%.2f",
36 result.turtle_final_x,
37 result.turtle_final_y,
38 result.turtle_final_z
39 )
40
41if __name__ == "__main__":
42# 2.初始化 ros 节点
43rospy.init_node("move_client_p")
44# 3.创建 action client 对象
45client = SimpleActionClient("turtle_move",turtle_moveAction)
46# 5.发送请求
47client.wait_for_server()
48goal = turtle_moveGoal()
49goal.turtle_goal_x = rospy.get_param("x")
50goal.turtle_goal_y = rospy.get_param("y")
51goal.turtle_goal_z = rospy.get_param("z")
52client.send_goal(goal,doneCb,activeCb,fbCb)
53# 6.spin
54rospy.spin()
<左右滑动以查看完整代码>
1.执行方案A：C++
1<launch>
2
3<param name="x" value="1.0" />
4<param name="y" value="2.0" />
5<param name="z" value="3.14" />
6
7<node pkg="turtlesim" type="turtlesim_node" name="turtle" output="screen" />
8<node pkg="demo02_test_action" type="turtle_move_server" name="turtle_server" output="screen" />
9<node pkg="demo02_test_action" type="turtle_move_client" name="turtle_client" output="screen" />
10launch>
<左右滑动以查看完整代码>
2.执行方案B：C++
1<launch>
2
3<param name="x" value="1.0" />
4<param name="y" value="2.0" />
5<param name="z" value="3.14" />
6
7<node pkg="turtlesim" type="turtlesim_node" name="turtle" output="screen" />
8
9<node pkg="demo02_test_action" type="turtle_move_server_p.py" name="turtle_server_p" output="screen" />
10
11<node pkg="demo02_test_action" type="turtle_move_client_p.py" name="turtle_client_p" output="screen" />
12launch>
<左右滑动以查看完整代码>
参数服务器的数据被修改时，如果节点不重新访问，那么就不能获取修改后的数据，例如在乌龟背景色修改的案例中，先启动乌龟显示节点，然后再修改参数服务器中关于背景色设置的参数，那么窗体的背景色是不会修改的，必须要重启乌龟显示节点才能生效。而一些特殊场景下，是要求要能做到动态获取的，也即，参数一旦修改，能够通知节点参数已经修改并读取修改后的数据，比如：
机器人调试时，需要修改机器人轮廓信息（长宽高）、传感器位姿信息等，如果这些信息存储在参数服务器中，那么意味着需要重启节点，才能使更新设置生效，但是希望修改完毕之后，节点能够即时更新内部的这些参数信息。
在ROS中针对这种场景已经给出的解决方案：dynamic reconfigure动态配置参数。
动态配置参数，之所以能够实现即时更新，因为被设计成CS架构，客户端修改参数就是向服务器发送请求，服务器接收到请求之后，读取修改后的是参数。
① 概念
一种可以在运行时更新参数而无需重启节点的参数配置策略。
② 作用
主要应用于需要动态更新参数的场景，比如参数条数、功能切换等。
③ 案例
编写两个节点，一个节点可以动态修改参数，另一个节点实时解析修改后的数据。
1.新建功能包，注意添加依赖包：dynamic_reconfigure。
package.xml如下：
1<buildtool_depend>catkinbuildtool_depend>
2<build_depend>roscppbuild_depend>
3<build_depend>rospybuild_depend>
4<build_depend>std_msgsbuild_depend>
5<build_depend>dynamic_reconfigurebuild_depend>
6<build_export_depend>roscppbuild_export_depend>
7<build_export_depend>rospybuild_export_depend>
8<build_export_depend>std_msgsbuild_export_depend>
9<exec_depend>roscppexec_depend>
10<exec_depend>rospyexec_depend>
11<exec_depend>std_msgsexec_depend>
12<exec_depend>dynamic_reconfigureexec_depend>
<左右滑动以查看完整代码>
可以创建功能包时直接声明roscpp rospy std_msgs dynamic_reconfigure，也可以在功能包创建完毕后，手动修改package.xml。
2.新建cfg文件夹，添加xxx.cfg文件（并添加可执行权限）。
cfg文件其实就是一个python文件,用于生成参数修改的客户端（GUI）。
1#! /usr/bin/env python
2
3"""
4生成动态参数 int,double,bool,string,列表
5实现流程:
6 1.导包
7 2.创建生成器
8 3.向生成器添加若干参数
9 4.生成中间文件并退出
10"""
11
12# 1.导包
13from dynamic_reconfigure.parameter_generator_catkin import *
14PACKAGE = "demo06_dynamic_reconfigure"
15
16# 2.创建生成器
17gen = ParameterGenerator()
18
19# 3.向生成器添加若干参数
20#add(name, paramtype, level, description, default=None, min=None, max=None, edit_method="")
21gen.add("int_param",int_t,0,"整型参数",50,0,100)
22gen.add("double_param",double_t,0,"浮点参数",1.57,0,3.14)
23gen.add("string_param",str_t,0,"字符串参数","hello world ")
24gen.add("bool_param",bool_t,0,"bool参数",True)
25
26many_enum = gen.enum([gen.const("small",int_t,0,"a small size"),
27 gen.const("mediun",int_t,1,"a medium size"),
28 gen.const("big",int_t,2,"a big size")
29 ],"a car size set")
30
31gen.add("list_param",int_t,0,"列表参数",0,0,2, edit_method=many_enum)
32
33# 4.生成中间文件并退出
34exit(gen.generate(PACKAGE,"dr_node","dr"))
<左右滑动以查看完整代码>
3.配置CMakeLists.txt。
1find_package(catkin REQUIRED COMPONENTS
2roscpp
3rospy
4std_msgs
5dynamic_reconfigure
6)
7generate_dynamic_reconfigure_options(
8cfg/my_car.cfg
9)
10catkin_package(
11# INCLUDE_DIRS include
12# LIBRARIES demo06_dynamic_reconfigure
13CATKIN_DEPENDS roscpp rospy std_msgs dynamic_reconfigure
14# DEPENDS system_lib
15)
<左右滑动以查看完整代码>
4.编译
编译后会生成中间文件。
C++需要调用的头文件。
Python需要调用的文件。
0.vscode配置
需要像之前自定义msg实现一样配置settings.json文件，如果以前已经配置且没有变更工作空间，可以忽略，如果需要配置，配置方式与之前相同：
1{
2"configurations": [
3 {
4 "browse": {
5 "databaseFilename": "",
6 "limitSymbolsToIncludedHeaders": true
7 },
8 "includePath": [
9 "/opt/ros/noetic/include/**",
10 "/usr/include/**",
11 "/xxx/yyy工作空间/devel/include/**" //配置 head 文件的路径
12 ],
13 "name": "ROS",
14 "intelliSenseMode": "gcc-x64",
15 "compilerPath": "/usr/bin/gcc",
16 "cStandard": "c11",
17 "cppStandard": "c++17"
18 }
19],
20"version": 4
21}
<左右滑动以查看完整代码>
1.服务器代码实现
1/*
2动态参数服务端: 参数被修改时直接打印
3实现流程:
4 1.包含头文件
5 2.初始化 ros 节点
6 3.创建服务器对象
7 4.创建回调对象(使用回调函数，打印修改后的参数)
8 5.服务器对象调用回调对象
9 6.spin()
10
11*/
12
13// 1.包含头文件
14#include "ros/ros.h"
15#include "dynamic_reconfigure/server.h" //动态参数服务
16#include "demo06_dynamic_reconfigure/drConfig.h" //配置头文件
17
18void cb(demo06_dynamic_reconfigure::drConfig& config, uint32_t level){
19ROS_INFO("动态参数服务器解析数据:%d,%.2f,%d,%s,%d",
20 config.int_param,
21 config.double_param,
22 config.bool_param,
23 config.string_param.c_str(),
24 config.list_param
25);
26}
27
28int main(int argc, char *argv[])
29{
30setlocale(LC_ALL,"");
31
32// 2.初始化 ros 节点
33ros::init(argc,argv,"dr");
34// 3.创建服务器对象
35dynamic_reconfigure::Server<demo06_dynamic_reconfigure::drconfig> server;
36// 4.创建回调对象(使用回调函数，打印修改后的参数)
37dynamic_reconfigure::Server<demo06_dynamic_reconfigure::drconfig>::CallbackType cbType;
38cbType = boost::bind(&cb,_1,_2);
39// 5.服务器对象调用回调对象
40server.setCallback(cbType);
41// 6.spin()
42ros::spin();
43return 0;
44}
</demo06_dynamic_reconfigure::drconfig></demo06_dynamic_reconfigure::drconfig><左右滑动以查看完整代码>
2.编译配置文件
1add_executable(DR_Server src/DR_Server.cpp)
2
3add_dependencies(DR_Server ${PROJECT_NAME}_gencfg ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
4
5target_link_libraries(DR_Server
6${catkin_LIBRARIES}
7)
<左右滑动以查看完整代码>
3.执行
先启动roscore。
启动服务端：rosrun功能包xxxx。
启动客户端：rosrun rqt_gui rqt_gui -s rqt_reconfigure或rosrun rqt_reconfigure rqt_reconfigure。
修改图形化界面数据，服务器将输出修改后的结果。
0.vscode配置
需要像之前自定义msg实现一样配置settings.json文件，如果以前已经配置且没有变更工作空间，可以忽略，如果需要配置，配置方式与之前相同：
1{
2"python.autoComplete.extraPaths": [
3 "/opt/ros/noetic/lib/python3/dist-packages",
4 "/xxx/yyy工作空间/devel/lib/python3/dist-packages"
5]
6}
<左右滑动以查看完整代码>
1.服务器代码实现
1#! /usr/bin/env python
2"""
3动态参数服务端: 参数被修改时直接打印
4实现流程:
5 1.导包
6 2.初始化 ros 节点
7 3.创建服务对象
8 4.回调函数处理
9 5.spin
10"""
11
12
13
14# 1.导包
15import rospy
16from dynamic_reconfigure.server import Server
17from demo06_dynamic_reconfigure.cfg import drConfig
18
19def cb(config,level):
20rospy.loginfo("python 动态参数服务解析:%d,%.2f,%d,%s,%d",
21 config.int_param,
22 config.double_param,
23 config.bool_param,
24 config.string_param,
25 config.list_param
26 )
27return config
28
29if __name__ == "__main__":
30
31# 2.初始化 ros 节点
32rospy.init_node("dr_p")
33
34# # 3.创建服务对象
35# # 4.回调函数处理
36server = Server(drConfig,cb)
37
38# # 5.spin
39rospy.spin()
40pass
<左右滑动以查看完整代码>
2.编辑配置文件
1catkin_install_python(PROGRAMS
2scripts/DR_Server_p.py
3DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
4)
<左右滑动以查看完整代码>
3.执行
先启动roscore。
启动服务端：rosrun功能包xxxx.py。
启动客户端：rosrun rqt_gui rqt_gui -s rqt_reconfigure或rosrun rqt_reconfigure rqt_reconfigure。
修改图形化界面数据，服务器将输出修改后的结果。
本章主要介绍了ROS通信机制中两种进阶实现：action通信与动态配置参数。主要内容如下：
action通信的应用场景。
action文件的定义。
action通信的C++以及Python实现。
动态配置参数的应用场景。
动态配置参数的C++以及Python实现。
这两种通信机制是对ROS中三种基本通信机制的补充与优化，到目前为止，ROS的通信机制模块就已经介绍完毕了，ROS的通信机制是ROS架构中的核心实现，该部分是需要重点关注的内容。下一章我们将进入一个新的阶段，该阶段主要介绍ROS运行时管理机制。
以上是"ROS通信机制进阶"的全部内容，更多话题讨论、技术交流可以扫描下方二维码添加『Apollo小哥哥』为好友，进开发者交流群。
* 以上内容为开发者原创，不代表百度官方言论，已获开发者授权。