AutoLabor-ROS-Python 学习记录——第二章 ROS通信机制

引言

该文档主要基于赵虚左老师的课程【Autolabor初级教程】ROS机器人入门，撰写一些 ROS Python 相关的重要知识点或者配置内容。

• 课程视频链接：【Autolabor初级教程】ROS机器人入门
• 课程电子书：ROS机器人入门课程《ROS理论与实践》零基础教程
• ROS noetic 配置文档：ROS配置

第二章 ROS通信机制

机器人是一种高度复杂的系统性实现，在机器人上可能集成各种传感器(雷达、摄像头、GPS…)以及运动控制实现，为了解耦合，在ROS中每一个功能点都是一个单独的进程，每一个进程都是独立运行的。更确切的讲，ROS是进程（也称为Nodes）的分布式框架。 因为这些进程甚至还可分布于不同主机，不同主机协同工作，从而分散计算压力。不过随之也有一个问题: 不同的进程是如何通信的？也即不同进程间如何实现数据交换的？在此我们就需要介绍一下ROS中的通信机制了。

ROS 中的基本通信机制主要有如下三种实现策略:

• 话题通信(发布订阅模式)
• 服务通信(请求响应模式)
• 参数服务器(参数共享模式)

本章的主要内容就是是介绍各个通信机制的应用场景、理论模型、代码实现以及相关操作命令。本章预期达成学习目标如下:

• 能够熟练介绍ROS中常用的通信机制
• 能够理解ROS中每种通信机制的理论模型
• 能够以代码的方式实现各种通信机制对应的案例
• 能够熟练使用ROS中的一些操作命令
• 能够独立完成相关实操案例

案例演示

1. 话题通信演示：
   控制小乌龟做圆周运动

获取乌龟位姿

2. 服务通信演示：
   在指定位置生成乌龟

3. 参数服务器演示：
   改变乌龟窗口的背景颜色

2.1 话题通信

话题通信基于发布/订阅模型，是一种异步通信模式。一个节点发布消息，另一个节点订阅该消息。这种方式将信息的产生和使用双方解耦，常用于不断更新的、含有较少逻辑处理的数据通信。例如，雷达数据、里程计数据等。

话题通信是ROS中使用频率最高的一种通信模式，话题通信是基于发布/订阅模式的，也即:一个节点发布消息，另一个节点订阅该消息。话题通信的应用场景也极其广泛，比如下面一个常见场景:

机器人在执行导航功能，使用的传感器是激光雷达，机器人会采集激光雷达感知到的信息并计算，然后生成运动控制信息驱动机器人底盘运动。

在上述场景中，就不止一次使用到了话题通信。

• 以激光雷达信息的采集处理为例，在 ROS 中有一个节点需要时时的发布当前雷达采集到的数据，导航模块中也有节点会订阅并解析雷达数据。
• 再以运动消息的发布为例，导航模块会根据传感器采集的数据时时的计算出运动控制信息并发布给底盘，底盘也可以有一个节点订阅运动信息并最终转换成控制电机的脉冲信号。

以此类推，像雷达、摄像头、GPS… 等等一些传感器数据的采集，也都是使用了话题通信，换言之，话题通信适用于不断更新的数据传输相关的应用场景。

概念：以发布订阅的方式实现不同节点之间数据交互的通信模式。
作用：用于不断更新的、少逻辑处理的数据传输场景。

2.1.1 理论模型

话题通信实现模型是比较复杂的，该模型如下图所示,该模型中涉及到三个角色：

• ROS Master
• Talker
• Listener

ROS Master 负责保管 Talker 和 Listener 注册的信息，并匹配话题相同的 Talker 与 Listener，帮助 Talker 与 Listener 建立连接，连接建立后，Talker 可以发布消息，且发布的消息会被 Listener 订阅。

1. Talker 注册：
   • Talker启动后，会通过RPC在 ROS Master 中注册自身信息，其中包含所发布消息的话题名称。ROS Master 会将节点的注册信息加入到注册表中。
2. Listener 注册：
   • Listener启动后，也会通过RPC在 ROS Master 中注册自身信息，包含需要订阅消息的话题名。ROS Master 会将节点的注册信息加入到注册表中。
3. ROS Master 实现信息匹配：
   • ROS Master 会根据注册表中的信息匹配Talker 和 Listener，并通过 RPC 向 Listener 发送 Talker 的 RPC 地址信息。
4. Listener向Talker发送请求：
   • Listener 根据接收到的 RPC 地址，通过 RPC 向 Talker 发送连接请求，传输订阅的话题名称、消息类型以及通信协议(TCP/UDP)。
5. Talker确认请求：
   • Talker 接收到 Listener 的请求后，也是通过 RPC 向 Listener 确认连接信息，并发送自身的 TCP 地址信息。
6. Listener与Talker件里连接：
   • Listener 根据步骤4 返回的消息使用 TCP 与 Talker 建立网络连接。
7. Talker向Listener发送消息
   • 连接建立后，Talker 开始向 Listener 发布消息。

注意1: 上述实现流程中，前五步使用的 RPC协议，最后两步使用的是 TCP 协议
注意2: Talker 与 Listener 的启动无先后顺序要求
注意3: Talker 与 Listener 都可以有多个
注意4: Talker 与 Listener 连接建立后，不再需要 ROS Master。也即，即便关闭ROS Master，Talker 与 Listern 照常通信。

2.1.2 话题通信基本操作

需求：

编写发布订阅实现，要求发布方以10HZ(每秒10次)的频率发布文本消息，订阅方订阅消息并将消息内容打印输出。

分析：
在模型实现中，ROS master 不需要实现，而连接的建立也已经被封装了，需要关注的关键点有三个：

• 发布方
• 订阅方
• 数据

流程：

1. 编写发布方实现；
2. 编写订阅方实现；
3. Python 文件添加可执行权限；
4. 编写配置文件；
5. 编译并后自行。

2.1.2.1 发布方

#! /usr/bin/env python"""
要求：编写发布订阅实现，要求发布方以10HZ(每秒10次)的频率发布文本消息，订阅方订阅消息并将消息内容打印输出。发布方实现：1. 导包2. 初始化 ROS 节点3. 创建发布者对象4. 编写发布逻辑并发布数据Author: Penry
Version: 1.0
"""import rospy
from std_msgs.msg import Stringdef talker():# 2. 初始化 ROS 节点rospy.init_node("publisher")# 3. 创建发布者对象publisher = rospy.Publisher("Car", String, queue_size= 10)# 4. 编写发布逻辑并发布数据# 创建数据msg = String()# 发布频率rate = rospy.Rate(1)# 设置计数器count = 0# 休眠3秒，防止订阅方少数据rospy.sleep(3)# 使用循环发布数据while rospy.is_shutdown() is False:count += 1msg.data = "Hello, I'm publisher, I have a car." + str(count)# 发布数据publisher.publish(msg)rospy.loginfo("发布的数据是:{}".format(msg.data))rate.sleep()if __name__ == "__main__":try:talker()except rospy.ROSInterruptException:pass

验证：

2.1.2.2 订阅方

#! /usr/bin/env python"""
要求：编写发布订阅实现，要求发布方以10HZ(每秒10次)的频率发布文本消息，订阅方订阅消息并将消息内容打印输出。订阅方实现：1. 导包2. 初始化 ROS 节点3. 创建订阅者对象4. 回调函数处理数据5. spin()Author: Penry
Version: 1.0
"""import rospy
from std_msgs.msg import Stringdef doMsg(msg):rospy.loginfo("订阅到的数据是:{}".format(msg.data))def listener():# 2. 初始化 ROS 节点rospy.init_node("subscriber")# 3. 创建订阅者对象subsrciber = rospy.Subscriber("Car", String, doMsg, queue_size = 10)# 4. 回调函数处理数据# 5. spin()rospy.spin()if __name__ == "__main__":listener()

验证：

注意1：添加可执行权限，终端下进入 scripts 执行:chmod +x *.py；
注意2：配置CMakeLists.txt文件

catkin_install_python(PROGRAMSscripts/talker_p.pyscripts/listener_p.pyDESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)

2.1.2.3 订阅费缺少消息的问题

如图所示，我将发布方发送频率设置为 $1Hz$，明显发现接收方缺少了第一条消息：

原因解释：因为发布方首先要在 ROS Master 中注册，可能在注册过程中就已经发布了第一条消息，导致这个时候订阅方无法接收。

解决方案：确保发布方注册完毕后，再开始发送数据，在发布之前加一个休眠。

#! /usr/bin/env python"""
要求：编写发布订阅实现，要求发布方以10HZ(每秒10次)的频率发布文本消息，订阅方订阅消息并将消息内容打印输出。发布方实现：1. 导包2. 初始化 ROS 节点3. 创建发布者对象4. 编写发布逻辑并发布数据Author: Penry
Version: 1.0
"""import rospy
from std_msgs.msg import Stringif __name__ == "__main__":# 2. 初始化 ROS 节点rospy.init_node("publisher")# 3. 创建发布者对象publisher = rospy.Publisher("Car", String, queue_size = 10)# 4. 编写发布逻辑并发布数据# 创建数据msg = String()# 发布频率rate = rospy.Rate(1)# 设置计数器count = 0# 休眠3秒，防止订阅方少数据rospy.sleep(3)# 使用循环发布数据while rospy.is_shutdown() is False:count += 1msg.data = "Hello, I'm publisher, I have a car." + str(count)# 发布数据publisher.publish(msg)rospy.loginfo("发布的数据是:{}".format(msg.data))rate.sleep()

2.1.2.4 计算图查看Python发布订阅模型

Ctrl+Alt+T快速创建四个终端：

1. 第一个终端：roscore
2. 第二个终端：rosrun 包名 demo01_sub.py
3. 第三个终端：rosrun 包名 demo01_pub.py
4. 第四个终端：rqt_graph

2.1.2.5 关键概念：解耦合

解耦是指系统中的组件或模块之间的依赖关系最小化，从而提高系统的灵活性、可维护性和可扩展性。在软件系统中，解耦通常通过引入中间件或消息队列等机制来实现。

解耦的实现方式：

1. 发布/订阅模式：发布者将消息发布到主题（Topic），订阅者从主题订阅消息。发布者和订阅者之间没有直接依赖关系，它们通过消息队列进行通信。
2. 服务/客户端模式：服务端提供服务，客户端请求服务。服务端和客户端之间也没有直接依赖关系，它们通过消息队列进行通信。

解耦合意味着即使我们的功能包中的两个源文件分别用 cpp 和 python 编写，也可以实现通信，这是因为它们通过消息队列进行解耦。

2.1.3 话题通信自定义msg

在 ROS 通信协议中，数据载体是一个较为重要组成部分，ROS 中通过 std_msgs 封装了一些原生的数据类型,比如:String、Int32、Int64、Char、Bool、Empty… 但是，这些数据一般只包含一个 data 字段，结构的单一意味着功能上的局限性，当传输一些复杂的数据，比如: 激光雷达的信息… std_msgs 由于描述性较差而显得力不从心，这种场景下可以使用自定义的消息类型。

msgs只是简单的文本文件，每行具有字段类型和字段名称，可以使用的字段类型有：

• int8, int16, int32, int64 (或者无符号类型: uint8, uint16, uint32, uint64)
• float32, float64
• string
• time
• duration
• other msg files
• variable-length array[] and fixed-length array[C]

ROS中还有一种特殊类型：Header，标头包含时间戳和ROS中常用的坐标帧信息。会经常看到msg文件的第一行具有Header标头。

需求：创建自定义消息，该消息包含人的信息:姓名、身高、年龄等。
流程：

1. 按照固定格式创建 msg 文件
2. 编辑配置文件
3. 编译生成可以被 Python 调用的中间文件

2.1.3.1 定义 msg 文件

功能包下新建 msg 目录，添加文件 Person.msg：

string name
uint32 age
float32 height

2.1.3.2 编辑配置文件

1. 在 package.xml 中添加编译依赖与执行依赖：

<build_depend>message_generation</build_depend><exec_depend>message_runtime</exec_depend>

2. 在 CMakeLists.txt 编辑 msg 相关配置：

find_package(catkin REQUIRED COMPONENTSroscpprospystd_msgsmessage_generation
)
# 需要加入 message_generation,必须有 std_msgs

## 配置 msg 源文件
add_message_files(FILESPerson.msg
)

# 生成消息时依赖于 std_msgs
generate_messages(DEPENDENCIESstd_msgs
)

#执行时依赖
catkin_package(
#  INCLUDE_DIRS include
#  LIBRARIES demo02_talker_listenerCATKIN_DEPENDS roscpp rospy std_msgs message_runtime
#  DEPENDS system_lib
)

2.1.3.3 编译并查看中间文件

Python 中间文件为：工作空间/devel/lib/python3/dist-packages/包名/msg/_Person.py

2.1.4 话题通信自定义msg调用

需求：

编写发布订阅实现，要求发布方以1HZ(每秒1次)的频率发布自定义消息，订阅方订阅自定义消息并将消息内容打印输出。

分析：
在模型实现中，ROS master 不需要实现，而连接的建立也已经被封装了，需要关注的关键点有三个：

1. 发布方
2. 订阅方
3. 自定义数据

流程：

1. 编写发布方实现
2. 编写订阅方实现
3. 为 Python 文件赋予可执行权限
4. 编辑配置文件
5. 编译并执行

2.1.4.1 VsCode配置

为了方便代码提示以及误抛异常，需要先配置 VsCode，将前面生成的 python 文件路径配置进 settings.json：

{"python.autoComplete.extraPaths": ["/opt/ros/noetic/lib/python3/dist-packages","/xxx/yyy工作空间/devel/lib/python3/dist-packages"]
}

我的填写示例：

{"python.autoComplete.extraPaths": ["/opt/ros/noetic/lib/python3/dist-packages","/home/mpy/workspace_learn_ros/autolabor_python/devel/lib/python3/dist-packages"],"python.analysis.extraPaths": ["/opt/ros/noetic/lib/python3/dist-packages","/home/mpy/workspace_learn_ros/autolabor_python/devel/lib/python3/dist-packages"]
}

2.1.4.2 发布方

#! /usr/bin/env python"""
需求：编写发布订阅实现，要求发布方以1HZ(每秒1次)的频率发布自定义消息，订阅方订阅自定义消息并将消息内容打印输出。发布方实现：1. 导包2. 初始化 ROS 节点3. 创建发布者对象4. 组织被发布的数据，并发布5. 设置发布频率6. 发布Author: Penry
Version: 1.0
"""import rospy
from base_learn.msg import Persondef talker():# 2. 初始化 ROS 节点rospy.init_node("Publisher")# 3. 创建发布者对象publisher = rospy.Publisher("Person", Person, queue_size = 10)# 4. 组织被发布的数据，并发布# 4-1. 创建 Person 类型消息person = Person()person.name = "Penry"person.age = 21person.height = 1.70# 4-2. 创建 Rate 对象rate = rospy.Rate(1)# 4-3. 循环发布数据while not rospy.is_shutdown():publisher.publish(person)rospy.loginfo("发布的消息：{0},{1},{2}".format(person.name, person.age, person.height))rate.sleep()if __name__ == "__main__":try:talker()except rospy.ROSInterruptException:pass

新建三个终端：

1. 第一个终端：roscore
2. 第二个终端：
   1. source ~/工作空间/devel/setup.bash
   2. rosrun 包名 可执行文件
3. 第三个终端：
   1. source ~/工作空间/devel/setup.bash
   2. rostopic echo 话题名

2.1.4.3 订阅方

#! /usr/bin/env python"""
需求：编写发布订阅实现，要求发布方以1HZ(每秒1次)的频率发布自定义消息，订阅方订阅自定义消息并将消息内容打印输出。订阅方实现：1. 导包2. 初始化 ROS 节点3. 创建订阅者对象4. 设置订阅话题5. 设置回调函数6. 订阅7. 处理数据8. 处理异常Author: Penry
Version: 1.0
"""import rospy
from base_learn.msg import Persondef doPerson(person):rospy.loginfo("订阅的消息：{0},{1},{2}".format(person.name, person.age, person.height))def listener():# 2. 初始化 ROS 节点rospy.init_node("Subscriber_Person")# 3. 创建订阅者对象subsriber = rospy.Subscriber("Person", Person, doPerson, queue_size= 10)# 4. 处理订阅消息# 5. spin()rospy.spin() if __name__ == "__main__":try:listener()except rospy.ROSInterruptException:pass

新建四个终端：

1. 第一个终端：roscore
2. 第二个终端发布方：
   1. source ~/工作空间/devel/setup.bash
   2. rosrun 包名 可执行文件
3. 第三个终端订阅方：
   1. source ~/工作空间/devel/setup.bash
   2. rostopic echo 话题名
4. 第四个终端：
   1. rosnode list

计算图查看：rqt_graph

2.2 服务通信

服务通信基于请求/响应模型，是一种同步通信模式。一个节点A向另一个节点B发送请求，B接收处理请求并产生响应结果返回给A。这种方式常用于数据量较小但有强逻辑处理的数据交换，例如拍照、语音识别等，如以下场景：

机器人巡逻过程中，控制系统分析传感器数据发现可疑物体或人… 此时需要拍摄照片并留存。

在上述场景中，就使用到了服务通信：一个节点需要向相机节点发送拍照请求，相机节点处理请求，并返回处理结果。

与上述应用类似的，服务通信更适用于对时时性有要求、具有一定逻辑处理的应用场景。

概念：以请求响应的方式实现不同节点之间数据交互的通信模式。
作用：用于偶然的、对时时性有要求、有一定逻辑处理需求的数据传输场景。
案例：实现两个数字的求和，客户端节点，运行会向服务器发送两个数字，服务器端节点接收两个数字求和并将结果响应回客户端。

2.2.1 服务通信理论模型

服务通信较之于话题通信更简单些，理论模型如下图所示，该模型中涉及到三个角色：

• ROS master（管理者）
• Server（服务端）
• Client（客户端）

ROS Master 负责保管 Server 和 Client 注册的信息，并匹配话题相同的 Server 与 Client ，帮助 Server 与 Client 建立连接，连接建立后，Client 发送请求信息，Server 返回响应信息。

2.2.1.1 Server注册

Server 启动后，会通过RPC在 ROS Master 中注册自身信息，其中包含提供的服务的名称。ROS Master 会将节点的注册信息加入到注册表中。

2.2.1.2 Client注册

Client 启动后，也会通过RPC在 ROS Master 中注册自身信息，包含需要请求的服务的名称。ROS Master 会将节点的注册信息加入到注册表中。

2.2.1.3 ROS Master实现信息匹配

ROS Master 会根据注册表中的信息匹配Server和 Client，并通过 RPC 向 Client 发送 Server 的 TCP 地址信息。

2.2.1.4 Client发送请求

Client 根据步骤3 响应的信息，使用 TCP 与 Server 建立网络连接，并发送请求数据。

2.2.1.5 Server发送响应

Server 收到请求后，处理请求并将结果返回给 Client。

注意：

1. 客户端请求被处理时，需要保证服务器已经启动，保证顺序；
2. 服务端和客户端都可以存在多个。

2.2.2 服务通信自定义srv

需求：

服务通信中，客户端提交两个整数至服务端，服务端求和并响应结果到客户端，请创建服务器与客户端通信的数据载体。

流程：
srv 文件内的可用数据类型与 msg 文件一致，且定义 srv 实现流程与自定义 msg 实现流程类似：

1. 按照固定格式创建 srv 文件
2. 编辑配置文件
3. 编译生成中间文件

2.2.2.1 定义 srv 文件

服务通信中，数据分成两部分，请求与响应，在 srv 文件中请求和响应使用---分割，具体实现如下:

1. 新建功能包：atkin_create_pkg plumbing_server_client rospy roscpp std_msgs
2. 在功能包下新建 srv 目录，添加 Addints.srv 文件，内容为：

# 客户端请求时发送的两个数字
int32 num1
int32 num2
---
# 服务器响应发送的数据
int32 sum

2.2.2.2 编辑配置文件

1. package.xml 中添加编译依赖于执行依赖：

<build_depend>message_generation</build_depend><exec_depend>message_runtime</exec_depend>

2. CMakeLists.txt 中编辑 srv 相关配置：

find_package(catkin REQUIRED COMPONENTSroscpprospystd_msgsmessage_generation
)
# 需要加入 message_generation,必须有 std_msgs

add_service_files(FILESAddInts.srv
)

generate_messages(DEPENDENCIESstd_msgs
)

catkin_package(
#  INCLUDE_DIRS include
#  LIBRARIES plumbing_server_clientCATKIN_DEPENDS roscpp rospy std_msgs message_runtime
#  DEPENDS system_lib
)

2.2.2.3 编译

1. 使用快捷键 Ctrl+Alt+B 编译；
2. 在路径 /home/mpy/workspace_learn_ros/autolabor_python/devel/lib/python3/dist-packages/plumbing_server_client/srv/_Addints.py 下可以看到 Python 要调用的中间文件：

2.2.3 服务通信自定义 srv 调用

需求：

编写服务通信，客户端提交两个整数至服务端，服务端求和并响应结果到客户端。

分析：
在模型实现中，ROS master 不需要实现，而连接的建立也已经被封装了，需要关注的关键点有三个：

1. 服务端
2. 客户端
3. 数据

流程：

1. 编写服务端实现；
2. 编写客户端实现；
3. 为 Python 文件添加可执行权限；
4. 编辑配置文件；
5. 编译并执行。

2.2.3.1 VsCode 配置

需要像之前自定义 msg 实现一样配置settings.json 文件，如果以前已经配置且没有变更工作空间，可以忽略，如果需要配置，配置方式与之前相同：

{"python.autoComplete.extraPaths": ["/opt/ros/noetic/lib/python3/dist-packages","/xxx/yyy工作空间/devel/lib/python3/dist-packages"]
}

2.2.3.2 服务端实现

#! usr/bin/env python"""
编写服务通信，客户端提交两个整数至服务端，服务端求和并响应结果到客户端。服务端实现：1. 导包；2. 初始化 ROS 节点；3. 创建服务端对象；4. 处理请求并响应（回调函数）；5. spin()Author: Penry
Version: 1.0
"""import rospy
from plumbing_server_client.srv import *def doNum(request):"""param: 封装了请求参数的 request 对象return: 封装了响应数据的 response 对象"""# 4. 处理请求并响应（回调函数）# 4-1. 获取请求数据num1 = request.num1num2 = request.num2# 4-2. 求和sum = num1 + num2# 4-3. 将结果封装到 response 对象中response = AddintsResponse()response.sum = sumrospy.loginfo("服务器解析得到的数据 num1 = %d, num2 = %d, sum = %d", num1, num2, sum)# 4-4. 返回响应return responsedef server():# 2. 初始化 ROS 节点rospy.init_node("Server_P")# 3. 创建服务端对象server = rospy.Service("Addints", Addints, handler = doNum)rospy.loginfo("服务器启动成功！")# 5. spin()rospy.spin()if __name__ == "__main__":try:server()except rospy.ROSInterruptException:pass

注意给 Python 文件赋权限，以及在 CMakeLists.txt 中添加配置。

启动三个终端：

1. 第一个终端：roscore
2. 第二个终端：rosrun plumbing_server_client server.py
3. 第三个终端：rosservice call Addints "{num1: 10, num2: 20}"

2.2.3.3 客户端实现

#! usr/bin/env python"""
编写服务通信，客户端提交两个整数至服务端，服务端求和并响应结果到客户端。客户端实现：1. 导包；2. 初始化 ROS 节点；3. 创建一个服务代理；4. 组织请求数据并发送请求；5. 处理响应数据；6. spin()Author: Penry
Version: 1.0
"""import rospy
from plumbing_server_client.srv import *def client():# 2. 初始化 ROS 节点rospy.init_node("Client_P")# 3. 创建一个服务代理client = rospy.ServiceProxy("Addints", Addints)# 4. 组织请求数据并发送请求response = client.call(num1 = 12, num2 = 34)# 5. 处理响应数据rospy.loginfo("客户端解析得到的数据 sum = %d", response.sum)if __name__ == "__main__":try:client()except rospy.ROSInterruptException:pass

注意给 Python 文件赋权限，以及在 CMakeLists.txt 中添加配置。

2.2.3.4 客户端优化

#! usr/bin/env python"""
编写服务通信，客户端提交两个整数至服务端，服务端求和并响应结果到客户端。客户端实现：1. 导包；2. 初始化 ROS 节点；3. 创建一个服务代理；4. 组织请求数据并发送请求；5. 处理响应数据；6. spin()优化实现：可以在执行节点的时候，动态传入参数Author: Penry
Version: 1.1
"""import rospy
from plumbing_server_client.srv import *
import sysdef client():# 判断传入参数长度if len(sys.argv) != 3:rospy.logerr("请输入两个整数！")sys.exit(1)# 2. 初始化 ROS 节点rospy.init_node("Client_P")# 3. 创建一个服务代理client = rospy.ServiceProxy("Addints", Addints)# 4. 组织请求数据并发送请求# 解析传入的参数num1 = int(sys.argv[1])num2 = int(sys.argv[2])response = client.call(num1, num2)# 5. 处理响应数据rospy.loginfo("客户端解析得到的数据 sum = %d", response.sum)if __name__ == "__main__":try:client()except rospy.ROSInterruptException:pass

注意给 Python 文件赋权限，以及在 CMakeLists.txt 中添加配置。

2.2.3.5 设置权限

chmod +x *.pyll

2.2.3.5 要求先启动客户端再启动服务端

问题：客户端优先于服务端启动，会抛出异常；
需要：客户端优先于服务端启动，不要抛出异常，而是挂起，等待服务端启动后再发送请求。
实现：ROS 中内置了相关函数，这些函数可以判断相关服务器状态，如果服务没启动，就让客户端挂起。
函数：rospy.wait_for_service("服务名") 或 client.wait_for_service()，这里的 client 是创建的客户端对象。
作用：等待服务启动，参数为服务名，返回值为 None。

#! usr/bin/env python"""
编写服务通信，客户端提交两个整数至服务端，服务端求和并响应结果到客户端。客户端实现：1. 导包；2. 初始化 ROS 节点；3. 创建一个服务代理；4. 组织请求数据并发送请求；5. 处理响应数据；6. spin()优化实现：可以在执行节点的时候，动态传入参数问题：客户端优先于服务端启动，会抛出异常；
需要：客户端优先于服务端启动，不要抛出异常，而是挂起，等待服务端启动后再发送请求。
实现：ROS 中内置了相关函数，这些函数可以判断相关服务器状态，如果服务没启动，就让客户端挂起。
函数：`rospy.wait_for_service("服务名")`
作用：等待服务启动，参数为服务名，返回值为 None。Author: Penry
Version: 1.2
"""import rospy
from plumbing_server_client.srv import *
import sysdef client():# 判断传入参数长度if len(sys.argv) != 3:rospy.logerr("请输入两个整数！")sys.exit(1)# 2. 初始化 ROS 节点rospy.init_node("Client_P")# 3. 创建一个服务代理client = rospy.ServiceProxy("Addints", Addints)# 4. 组织请求数据并发送请求# 解析传入的参数num1 = int(sys.argv[1])num2 = int(sys.argv[2])# 等待服务启动client.wait_for_service()# rospy.wait_for_service("Addints")response = client.call(num1, num2)# 5. 处理响应数据rospy.loginfo("客户端解析得到的数据 sum = %d", response.sum)if __name__ == "__main__":try:client()except rospy.ROSInterruptException:pass

如下图，先启动客户端，发现客户端已经挂起，没有报错：

再启动服务端，发现成功实现通信：

2.3 参数通信

参数服务器在 ROS 中主要用于实现不同节点之间的数据共享。参数服务器相当于是独立于所有节点的一个公共容器，可以将数据存储在该容器中，被不同的节点调用，当然不同的节点也可以往其中存储数据，关于参数服务器的典型应用场景如下：

导航实现时，会进行路径规划，比如: 全局路径规划，设计一个从出发点到目标点的大致路径。本地路径规划，会根据当前路况生成时时的行进路径。

上述场景中，全局路径规划和本地路径规划时，就会使用到参数服务器：

• 路径规划时，需要参考小车的尺寸，我们可以将这些尺寸信息存储到参数服务器，全局路径规划节点与本地路径规划节点都可以从参数服务器中调用这些参数。

参数服务器，一般适用于存在数据共享的一些应用场景。

概念：以共享的方式实现不同节点之间数据交互的通信模式。
作用：存储一些多节点共享的数据，类似于全局变量。
案例：实现参数增删改查操作。

2.3.1 参数服务器理论模型

参数服务器实现是最为简单的，该模型如下图所示,该模型中涉及到三个角色:

• ROS Master（管理者）
• Talker（参数设置者）
• Listener（参数调用者）

ROS Master 作为一个公共容器保存参数，Talker 可以向容器中设置参数，Listener 可以获取参数。

2.3.1.1 Talker 设置参数

Talker 通过 RPC 向参数服务器发送参数(包括参数名与参数值)，ROS Master 将参数保存到参数列表中。

2.3.1.2 Listener 获取参数

Listener 通过 RPC 向参数服务器发送参数查找请求，请求中包含要查找的参数名。

2.3.1.3 ROS Master 向 Listener 发送参数值

ROS Master 根据步骤2请求提供的参数名查找参数值，并将查询结果通过 RPC 发送给 Listener。

参数可使用数据类型:

• 32-bit integers
• booleans
• strings
• doubles
• iso8601 dates
• lists
• base64-encoded binary data
• 字典

注意:参数服务器不是为高性能而设计的，因此最好用于存储静态的非二进制的简单数据

2.3.2 参数操作

需求:实现参数服务器参数的增删改查操作。

2.3.2.1 参数服务器新增(修改)参数

1. 新增参数：

#! /usr/bin/env python"""描述：演示参数通信中参数的新增和修改需求：在参数服务器中设置机器人属性：型号、半径实现：rospy.set_param("属性名", 属性值)Author: PenryVersion: 1.0
"""import rospydef set_param():# 1. 初始化节点rospy.init_node("set_param")# 2. 新增参数rospy.set_param("robot_type", "Turtlebot3")rospy.set_param("radius", 0.15)if __name__ == "__main__":set_param()

新建四个终端：

• 第一个终端：roscore
• 第二个终端：rosrun plumbing_param_server demo01_param_set_p.py
• 第三个终端：rosparam list，查看参数列表
• 第四个终端：rosparam get 参数名，获取参数值

2. 修改参数：

#! /usr/bin/env python"""描述：演示参数通信中参数的新增和修改需求：在参数服务器中设置机器人属性：型号、半径实现：rospy.set_param("属性名", 属性值)Author: PenryVersion: 1.0
"""import rospydef set_param():# 1. 初始化节点rospy.init_node("set_param")# 2. 新增参数rospy.set_param("robot_type", "Turtlebot3")rospy.set_param("radius", 0.15)# 3. 修改参数rospy.set_param("radius", 1.11)if __name__ == "__main__":set_param()

很明显观察到参数 radius 被修改为 1.11：

2.3.2.2 参数服务器获取参数

这里提供了多个实现方式：

1. get_params(键,默认值)：获取指定键的参数值，如果键不存在，返回默认值。
2. get_param_cached(键,默认值)：获取指定键的参数值，如果键不存在，返回默认值。与 get_param 不同，该方法会缓存参数值，因此在循环中调用时效率更高。
3. get_param_names()：获取所有参数的键列表。
4. has_param(键)：检查参数服务器中是否存在指定键。
5. search_param(参数名)：搜索参数名，返回第一个匹配的参数名。

2.3.2.2.1 get_param

#! /usr/bin/env python"""描述：演示参数通信中参数的查询需求：在参数服务器中设置机器人属性：型号、半径实现：1. get_param(键,默认值)当键存在时，返回对应的值，如果不存在返回默认值2. get_param_cached3. get_param_names4. has_param5. search_paramAuthor: PenryVersion: 1.1
"""import rospydef get_param():rospy.init_node("get_param")# 1. get_paramradius = rospy.get_param("radius", 0.5)radius2 = rospy.get_param("radius_2", 0.5)rospy.loginfo("radius = {0:.2f}".format(radius))rospy.loginfo("radius2 = {0:.2f}".format(radius2))if __name__ == "__main__":get_param()

新建四个终端：

• 第一个终端：roscore
• 第二个终端：rosrun plumbing_param_server demo01_param_set_p.py
• 第三个终端：rosrun plumbing_param_server demo02_param_get_p.py
• 第四个终端：rosparam list，获取参数列表

2.3.2.2.2 get_params_cached

#! /usr/bin/env python"""描述：演示参数通信中参数的查询需求：在参数服务器中设置机器人属性：型号、半径实现：1. get_param(键,默认值)当键存在时，返回对应的值，如果不存在返回默认值2. get_param_cached3. get_param_names4. has_param5. search_paramAuthor: PenryVersion: 1.2
"""import rospydef get_param():rospy.init_node("get_param")# 1. get_paramradius = rospy.get_param("radius", 0.5)radius2 = rospy.get_param("radius2", 0.5)rospy.loginfo("radius = {0:.2f}".format(radius))rospy.loginfo("radius2 = {0:.2f}".format(radius2))# 2. get_param_cachedradius3 = rospy.get_param_cached("radius", 0.5)radius4 = rospy.get_param_cached("radius2", 0.5)rospy.loginfo("radius3 = {0:.2f}".format(radius3))rospy.loginfo("radius4 = {0:.2f}".format(radius4))if __name__ == "__main__":get_param()

2.3.2.2.3 get_param_names

#! /usr/bin/env python"""描述：演示参数通信中参数的查询需求：在参数服务器中设置机器人属性：型号、半径实现：1. get_param(键,默认值)当键存在时，返回对应的值，如果不存在返回默认值2. get_param_cached3. get_param_names4. has_param5. search_paramAuthor: PenryVersion: 1.3
"""import rospydef get_param():rospy.init_node("get_param")# 1. get_paramradius = rospy.get_param("radius", 0.5)radius2 = rospy.get_param("radius2", 0.5)rospy.loginfo("radius = {0:.2f}".format(radius))rospy.loginfo("radius2 = {0:.2f}".format(radius2))# 2. get_param_cachedradius3 = rospy.get_param_cached("radius", 0.5)radius4 = rospy.get_param_cached("radius2", 0.5)rospy.loginfo("radius3 = {0:.2f}".format(radius3))rospy.loginfo("radius4 = {0:.2f}".format(radius4))# 3. get_param_namesnames = rospy.get_param_names()for name in names:rospy.loginfo("name = %s", name)if __name__ == "__main__":get_param()

2.3.2.2.4 has_param

#! /usr/bin/env python"""描述：演示参数通信中参数的查询需求：在参数服务器中设置机器人属性：型号、半径实现：1. get_param(键,默认值)当键存在时，返回对应的值，如果不存在返回默认值2. get_param_cached3. get_param_names4. has_param5. search_paramAuthor: PenryVersion: 1.4
"""import rospydef get_param():rospy.init_node("get_param")# 1. get_paramradius = rospy.get_param("radius", 0.5)radius2 = rospy.get_param("radius2", 0.5)rospy.loginfo("radius = {0:.2f}".format(radius))rospy.loginfo("radius2 = {0:.2f}".format(radius2))# 2. get_param_cachedradius3 = rospy.get_param_cached("radius", 0.5)radius4 = rospy.get_param_cached("radius2", 0.5)rospy.loginfo("radius3 = {0:.2f}".format(radius3))rospy.loginfo("radius4 = {0:.2f}".format(radius4))# 3. get_param_namesnames = rospy.get_param_names()for name in names:rospy.loginfo("name = %s", name)# 4. has_paramflag1 = rospy.has_param("radius")if flag1:rospy.loginfo("has radius")else:rospy.loginfo("no radius")flag2 = rospy.has_param("radius2")if flag2:rospy.loginfo("has radius2")else:rospy.loginfo("no radius2")if __name__ == "__main__":get_param()

2.3.2.2.5 search_param

#! /usr/bin/env python"""描述：演示参数通信中参数的查询需求：在参数服务器中设置机器人属性：型号、半径实现：1. get_param(键,默认值)当键存在时，返回对应的值，如果不存在返回默认值2. get_param_cached3. get_param_names4. has_param5. search_paramAuthor: PenryVersion: 1.5
"""import rospydef get_param():rospy.init_node("get_param")# 1. get_paramradius = rospy.get_param("radius", 0.5)radius2 = rospy.get_param("radius2", 0.5)rospy.loginfo("radius = {0:.2f}".format(radius))rospy.loginfo("radius2 = {0:.2f}".format(radius2))# 2. get_param_cachedradius3 = rospy.get_param_cached("radius", 0.5)radius4 = rospy.get_param_cached("radius2", 0.5)rospy.loginfo("radius3 = {0:.2f}".format(radius3))rospy.loginfo("radius4 = {0:.2f}".format(radius4))# 3. get_param_namesnames = rospy.get_param_names()for name in names:rospy.loginfo("name = %s", name)# 4. has_paramflag1 = rospy.has_param("radius")if flag1:rospy.loginfo("has radius")else:rospy.loginfo("no radius")flag2 = rospy.has_param("radius2")if flag2:rospy.loginfo("has radius2")else:rospy.loginfo("no radius2")# 5. search_paramkey = rospy.search_param("radius")rospy.loginfo("key = %s", key)key1 = rospy.search_param("radius2")rospy.loginfo("key1 = %s", key1)if __name__ == "__main__":get_param()

2.3.2.3 参数服务器删除参数

使用 rospy.delete_param("键") 删除参数：

• 键不存在时，会抛出异常
• 键存在时，删除成功

#! /usr/bin/env python"""描述：演示参数通信中参数的删除需求：删除参数服务器中的参数实现：rospy.delete_param("参数名")Author: PenryVersion: 1.0
"""import rospydef del_param():rospy.init_node("del_param")# 删除参数rospy.delete_param("radius")if __name__ == "__main__":try:del_param()except Exception as e:rospy.logerr(e)

这里如果要删除的参数不存在，是会抛异常的，这里我们使用try...except结构打印出来异常，如下图所示：

2.4 常用命令

机器人系统中启动的节点少则几个，多则十几个、几十个，不同的节点名称各异，通信时使用话题、服务、消息、参数等等都各不相同，一个显而易见的问题是: 当需要自定义节点和其他某个已经存在的节点通信时，如何获取对方的话题、以及消息载体的格式呢？

在 ROS 同提供了一些实用的命令行工具，可以用于获取不同节点的各类信息，常用的命令如下:

• rosnode: 操作节点
• rostopic: 操作话题
• rosservice: 操作服务
• rosmsg: 操作消息
• rossrv: 操作 srv 消息
• rosparam: 操作参数

作用：和之前介绍的文件系统操作命令比较，文件操作命令是静态的，操作的是磁盘上的文件，而上述命令是动态的，在ROS程序启动后，可以动态的获取运行中的节点或参数的相关信息。

案例：本节将借助于 2.1、2.2 和 2.3 的通信实现介绍相关命令的基本使用，并通过练习ROS内置的小海龟例程来强化命令的应用。

参考文档：CommandLineTools

2.4.1 rosnode

rosnode 是用于获取节点信息的命令：

• rosnode ping 测试到节点的连接状态
• rosnode list 列出活动节点
• rosnode info 打印节点信息
• rosnode machine 列出指定设备上节点
• rosnode kill 杀死某个节点
• rosnode cleanup 清除不可连接的节点

1. rosnode list

2. rosnode ping /Publisher_Person

3. rosnode info /Subscriber_Person

4. rosnode machine mpy-vpc

5. rosnode kill /Publisher_Person

能看到右下角的进程被杀死了：

6. rosnode cleanup

能看到虽然 /turtlesim 已经被 Ctrl+C 关掉了，但是 rosnode list 显示该节点还活着，这时候需要用 rosnode cleanup清理干净

rosnode cleanup 清理僵尸节点：

2.4.2 rostopic

打开话题通信模型。

rostopic包含rostopic命令行工具，用于显示有关 ROS 主题的调试信息，包括发布者，订阅者，发布频率和 ROS 消息。它还包含一个实验性Python库，用于动态获取有关主题的信息并与之交互。

• rostopic bw 显示主题使用的带宽
• rostopic delay 显示带有 header 的主题延迟
• rostopic echo 打印消息到屏幕
• rostopic find 根据类型查找主题
• rostopic hz 显示主题的发布频率
• rostopic info 显示主题相关信息
• rostopic list 显示所有活动状态下的主题
• rostopic pub 将数据发布到主题
• rostopic type 打印主题类型

2.4.2.1 rostpoic list

2.4.2.2 rostopic echo 话题名

打开四个终端，在执行 rostopic echo Person 的终端中，我们要依次执行如下步骤：

1. cd ~/工作空间，即进入到该功能包所在的工作空间；
2. source ./devel/setup.bash，即加载该功能包下的所有环境变量；
3. rostopic echo 话题名，即可打印发布的话题消息。

2.4.2.3 rostopic pub

在执行 rostopic pub 话题名 消息类型 "消息内容" 之前，我们要依次执行如下步骤：

1. cd ~/工作空间，即进入到该功能包所在的工作空间；
2. source ./devel/setup.bash，即加载该功能包下的所有环境变量；
3. rostopic pub 话题名 消息类型 "消息内容"，即可发布消息。
   • 其中输入完消息类型后点击Tab可以自动补齐并产生“消息内容”。

也可以按照某种频率发送消息：

• rostopic pub -r 10 话题名 消息类型 "消息内容"，即按照10hz的频率发布消息。

2.4.2.4 rostopic info

查看当前话题具体信息：

• 类型；
• 发布者；
• 订阅者；

2.4.2.5 rostopic hz

显示话题发布频率：

2.4.3 rosservice

打开服务通信模型。

rosservice包含用于列出和查询ROSServices的rosservice命令行工具。

调用部分服务时，如果对相关工作空间没有配置 path，需要进入工作空间调用 source ./devel/setup.bash.

常用的命令：

• rosservice args 打印服务参数
• rosservice call 使用提供的参数调用服务
• rosservice find 按照服务类型查找服务
• rosservice info 打印有关服务的信息
• rosservice list 列出所有活动的服务
• rosservice type 打印服务类型
• rosservice uri 打印服务的 ROSRPC uri

2.4.3.1 rosservice list

查看所有服务：

2.4.3.2 rosservice call

调用服务，扮演客户端的角色：

• 语法：rosservice call 服务名 服务参数；
• 示例：rosservice call Addints 服务参数。
  • 服务参数可以直接使用 Tab 键补齐

可以看到我们并没有启动客户端，仅仅通过该命令行也实现了客户端的功能。

2.4.3.3 rosservice info

查看服务具体信息：

• Node：服务所在的节点；
• URI：服务的 ROSRPC uri；
• Type：服务的类型；
• Args：服务的参数；

2.4.3.4 rosservice type

查看服务类型：

• 语法：rosservice type 服务名；
• 示例：rosservice type Addints。

2.4.4 rosmsg

打开话题通信模型。

rosmsg是用于显示有关 ROS 消息类型的信息的命令行工具。

常用的命令：

• rosmsg show 显示消息描述
• rosmsg info 显示消息信息
• rosmsg list 列出所有消息
• rosmsg md5 显示 md5 加密后的消息
• rosmsg package 显示某个功能包下的所有消息
• rosmsg packages 列出包含消息的功能包

2.4.4.1 rosmsg list

1. rosmsg list 会列出 ROS 支持的所有消息列表：

2. rosmsg list | grep -i 消息名 可以根据消息名进行搜索：

2.4.4.2 rosmsg info 和 rosmsg show

二者均是展示消息的详细信息，展示内容一致：

2.4.5 rossrv

rossrv 是用于显示有关 ROS 服务类型的信息的命令行工具，与 rosmsg 使用语法高度雷同。

常用语法：

• rossrv show 显示服务消息详情
• rossrv info 显示服务消息相关信息
• rossrv list 列出所有服务信息
• rossrv md5 显示 md5 加密后的服务消息
• rossrv package 显示某个包下所有服务消息
• rossrv packages 显示包含服务消息的所有包

2.4.5.1 rossrv list

显示 ROS 中所有的服务消息：

rossrv list | grep -i 服务名 可以根据服务名进行搜索：

2.4.5.2 rossrv info

使用用法是：rossrv info 消息名称，而对应的消息名称我们已经使用rossrv list | grep -i 服务名抓取到了。

2.4.5.3 rossrv show

整体用法与 rossrv info 一致，显示内容也一致：

2.4.6 rosparam

rosparam 包含 rosparam 命令行工具，用于使用 YAML 编码文件在参数服务器上获取和设置 ROS 参数。

常用命令：

• rosparam list 列出所有参数
• rosparam get 获取参数值
• rosparam set 设置参数值
• rosparam delete 删除参数
• rosparam dump 导出所有参数为 YAML 文件
• rosparam load 从 YAML 文件导入参数

2.4.6.1 rosparam list

列举出所有参数：

• rosparam list 可以列出所有的参数；
• rosparam list | grep -i 参数名 可以根据参数名进行搜索。

2.4.6.2 rosparam set & rosparam get

用法：

• rosparam set 参数名 参数值

配合 rosparam list 以及 rosparam get 参数名 观察效果：

2.4.6.3 rosparam delete

1. 首先使用 rosparam set 多设置几个参数和对应的值；
2. 演示 rosparam delete 参数名 效果。

2.4.6.4 rosparam dump & rosparam load

描述：

• 执行rosparam dump params.yaml，将所有参数导出为 YAML 文件；
• 执行rosparam load params.yaml，将 YAML 文件中的参数导入参数服务器。

这里我们演示将参数序列化，即导出为 YAML 文件：

接着我们将 roscore 重启，会发现之前自定义的参数已经被释放了，此时我们选择将 params.yaml 中的参数导入参数服务器，进行反序列化：

2.5 通信机制实操

内容：本节主要是通过ROS内置的 turtlesim 案例，结合已经介绍ROS命令获取节点、话题、话题消息、服务、服务消息与参数的信息，最终再以编码的方式实现乌龟运动的控制、乌龟位姿的订阅、乌龟生成与乌龟窗体背景颜色的修改。

目的:熟悉、强化通信模式应用。

2.5.1 实操01_话题发布

需求描述：编码实现乌龟运动控制，让小乌龟做圆周运动。

结果演示：

实现分析：

1. 乌龟运动控制实现，关键节点有两个，一个是乌龟运动显示节点 turtlesim_node，另外一个是控制节点，二者是订阅发布模式实现通信的，乌龟运动显示节点直接调用即可，运动控制节点之前是使用的 turtle_teleop_key 通过键盘控制，现在需要自定义控制节点。
2. 控制节点自实现时，首先需要了解控制节点与显示节点通信使用的话题与消息，可以使用ros命令结合计算图来获取。
3. 了解了话题与消息之后，通过 C++ 或 Python 编写运动控制节点，通过指定的话题，按照一定的逻辑发布消息即可。

实现流程：

1. 通过计算图结合ros命令获取话题与消息信息。
2. 编码实现运动控制节点。
3. 启动 roscore、turtlesim_node 以及自定义的控制节点，查看运行结果。

2.5.1.1 话题与消息获取

准备: 先启动键盘控制乌龟运动案例。

2.5.1.1.1 话题获取

获取话题：/turtle/cmd_vel

使用命令rostopic list：

结合命令 rqt_graph：

我们得知控制话题是 /turtle1/cmd_vel。

2.5.1.1.2 消息获取

我们可以使用 rostopic info /tuetle1/cmd_vel 获取消息类型：

我们接着可以使用 rosmsg info geometry_msgs/Twist 获取消息的详细信息：

消息解释：

• linear：线速度，包含x、y、z三个坐标，单位是m/s；
• angular：角速度，包含x、y、z三个坐标，单位是rad/s。

详细请见补充资料。

对于乌龟案例而言，只需要控制其线速度的 x 参数，角速度的 z 参数即可实现圆周运动，以下我们先进行一个速度消息验证。

我们在终端中通过 rostopic echo /turtle1/cmd_vel 可以查看发布的速度消息：

可以通过打印数据观察到在整个运动过程中，只有线速度的 x 参数和角速度的 z 参数改变。

2.5.1.2 节点实现

1. 其实有一种极简的方法实现，就是使用以下命令：

rostopic pub 10 -r 1 /turtle1/cmd_vel geometry_msgs/Twist "linear:x: 1.0y: 0.0z: 0.0
angular:x: 0.0y: 0.0z: 1.0"

这条命令，其中 10 表示发布的频率，-r 表示重复发布，1 表示重复发布的次数，/turtle1/cmd_vel 表示发布的话题，geometry_msgs/Twist 表示发布的消息类型，后面的字符串是发布的消息内容。

2. 使用 Python 实现自定义节点：
   1. 我们首先新建功能包：catkin_create_pkg plumbing_test rospy roscpp std_msgs geometry_msgs；
   2. 我们在 plumbing_test/scripts 目录下新 test01_pub_twist_p.py 文件，编写发布方代码；
   3. 赋予可执行权限：chmod +x *.py；
   4. 编辑 CMakeLists.txt 配置文件；
   5. 运行：rosrun plumbing_test test01_pub_twist_p.py。

#！ /usr/bin/env python"""发布方：发布速度消息话题名称：/turtle1/cmd_vel消息类型：gemoetry_msgs/Twist实现：1. 导包2. 初始化节点3. 创建发布者对象4. 组织数据并发布Author: PenryVersion: 1.0
"""import rospy
from geometry_msgs.msg import Twistdef pub_twist():# 1. 初始化节点rospy.init_node("pub_twist")# 2. 创建发布者对象pub = rospy.Publisher("/turtle1/cmd_vel", Twist, queue_size=10)# 3. 组织数据并发布rate = rospy.Rate(10)while not rospy.is_shutdown():# 3.1 组织数据msg = Twist()msg.linear.x = 0.5msg.angular.z = 0.2# 3.2 发布数据pub.publish(msg)rate.sleep()if __name__ == "__main__":try:pub_twist()except rospy.ROSInterruptException:pass

2.5.1.3 运行

创建四个终端：

• 第一个终端 roscore；
• 第二个终端 rosrun turtlesim turtlesim_node；
• 第三个终端 rosrun plumbing_test test01_pub_twist_p.py；
• 第四个终端 rostopic echo /turtle1/cmd_vel。

2.5.1.4 补充资料

这里整体均参考为右手坐标系。

1. 弧度：单位弧度定义为圆弧长度等于半径时的圆心角。

• 圆周长计算公式：$C=2\pi r$

1. 偏航、翻滚与俯仰：

• 坐标系图解：

• 偏航：绕 z 轴旋转，单位是弧度；

• 俯仰：绕 y 轴旋转，单位是弧度；

• 翻滚：绕 x 轴旋转，单位是弧度。

2.5.2 实操02_话题订阅

需求描述: 已知turtlesim中的乌龟显示节点，会发布当前乌龟的位姿(窗体中乌龟的坐标以及朝向)，要求控制乌龟运动，并时时打印当前乌龟的位姿。

结果演示：

实现分析：

1. 首先，启动乌龟显示以及运动控制节点并控制乌龟运动；
2. 要通过 ROS 命令，来获取乌龟位姿发布的话题以及消息；
3. 编写订阅节点，订阅并打印乌龟的位姿。

实现流程：

1. 通过 ROS 命令获取话题与消息信息；
2. 编码实现位姿获取节点；
3. 启动 roscore、turtlesim_node、控制节点以及位姿订阅节点，控制乌龟运动并输出乌龟的位姿。

2.5.2.0 准备工作

准备工作：编写乌龟GUI界面和键盘控制的launch文件。

<!-- 启动乌龟GUI与键盘控制节点 --><launch><!-- 乌龟GUI --><node pkg = "turtlesim" type = "turtlesim_node" name = "turtle1" output = "screen" /><!-- 乌龟键盘控制 --><node pkg = "turtlesim" type = "turtle_teleop_key" name = "key" output = "screen" /></launch>

2.5.2.1 话题与消息获取

1. 话题获取：
   • 新建三个终端：
     • 第一个：roscore
     • 第二个：roslaunch plumbing_test start_turtle.launch
     • 第三个：rostopic list
       • /turtle1/pose

2. 消息类型获取：
   • 首先使用 rostopic info /turtle1/pose 命令获取话题的消息类型；
     • 消息类型：turtlesim/Pose
   • 再使用 rosmsg info /turtle1/Pose 命令获取消息的具体结构；
     • 消息结构：
       • float32 x
       • float32 y
       • float32 theta
       • float32 linear_velocity
       • float32 angular_velocity

2.5.2.2 实现订阅节点

最简单的方式，不需要通过自定义节点，只需要打印 /turtle1/pose 的信息即可：

rostopic echo /turtle1/pose

创建功能包需要依赖的功能包：roscpp rospy std_msgs turtlesim，我们这里仍然在功能包 plumbing_test 中追加 turtlesim 依赖即可：

1. 修改 package.xml：

<build_depend>turtlesim</build_depend><exec_depend>turtlesim</exec_depend>

2. 修改 CMakeLists.txt：

find_package(catkin REQUIRED COMPONENTSgeometry_msgsroscpprospystd_msgsturtlesim
)

3. 新建 test02_sub_pose_p.py：

#! /usr/bin/env python"""需求：订阅并输出乌龟位姿信息实现：1. 初始化ROS节点2. 创建订阅者对象3. 回调函数4. 订阅5. spinAuthor: PenryVersion: 1.0
"""import rospy
from turtlesim.msg import Posedef doPose(pose):rospy.loginfo("Python Turtle pose：坐标(%.2f, %.2f), 朝向%.2f, 线速度%.2f, 角速度%.2f",pose.x, pose.y, pose.theta, pose.linear_velocity, pose.angular_velocity)def sub_pose():# 2. 初始化ROS节点rospy.init_node("sub_pose")# 3. 创建订阅者对象sub = rospy.Subscriber("/turtle1/pose", Pose, doPose, queue_size= 10)# 4. 使用回调函数处理订阅到的信息# 5. spin()让节点一直保持运行rospy.spin()if __name__ == "__main__":sub_pose()

这里我们还需要配置 CMakeLists.txt：

catkin_install_python(PROGRAMSscripts/test01_pub_twist_p.pyscripts/test02_sub_pose_p.pyDESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)

注意：为 Python 文件赋予执行权限。

2.5.3 实操03_服务调用

需求描述：编码实现向 turtlesim 发送请求，在乌龟显示节点的窗体指定位置生成一乌龟，这是一个服务请求操作。

结果演示：

实现分析：

1. 首先，需要启动乌龟显示节点；
2. 要通过ROS命令，来获取乌龟生成服务的服务名称以及服务消息类；
3. 编写服务请求节点，生成新的乌龟。

实现流程：

1. 通过ros命令获取服务与服务消息信息；
2. 编码实现服务请求节点；
3. 启动 roscore、turtlesim_node 、乌龟生成节点，生成新的乌龟。

2.5.3.1 服务名称与服务消息获取

1. 获取服务名称：
   • 使用 rosservice list：
     • /spawn

2. 获取消息类型：
   • 使用 rosservice info /spawn
     • turtlesim/Spawn

3. 获取消息结构：
   • 使用 rossrv info turtlesim/Spawn
     • float32 x
     • float32 y
     • float32 theta
     • string name

2.5.3.2 服务客户端实现

最简单的方式是使用 rosservice call 命令来调用服务，具体如下：

rosservice call /spawn "x: 1.0
y: 1.1
theta: 0.5
name: 'turtle2'" 

注意：

1. 服务名称前需要添加 /；
2. 服务消息的 x、y、theta 数据类型为 float32，name 数据类型为 string，需要使用 '' 包裹；
3. 多个数据之间使用空格隔开。

创建功能包需要依赖的功能包: roscpp rospy std_msgs turtlesim

1. 创建 test03_srv_spawn_p.py：

#! /usr/bin/env python"""生成一只小乌龟准备工作:1.服务话题 /spawn2.服务消息类型 turtlesim/Spawn3.运行前先启动 turtlesim_node 节点实现流程:1.导包需要包含 turtlesim 包下资源，注意在 package.xml 配置2.初始化 ros 节点3.创建 service 客户端4.等待服务启动5.发送请求6.处理响应Author: PenryVersion: 1.0
"""import rospy
from turtlesim.srv import *
import sysdef client_spawn():# 2. 初始化 ros 节点rospy.init_node("client_spawn")# 3. 创建 service 客户端client = rospy.ServiceProxy("/spawn", Spawn)# 4. 等待服务启动rospy.wait_for_service("/spawn")# 5. 发送请求# 5.1 定义请求数据if len(sys.argv) != 5:rospy.logerr("请正确提交参数")sys.exit(1)req = SpawnRequest()req.x = float(sys.argv[1])req.y = float(sys.argv[2])req.theta = float(sys.argv[3])req.name = sys.argv[4]# 5.2 调用服务response = client.call(req)if __name__ == "__main__":try:client_spawn()except Exception as e:print(e)else:print("乌龟创建成功")

2. 编辑 CMakeLists.txt 文件，赋予 Python 文件可执行权限，并编译。
3. 运行，建立三个终端：
   • 第一个：roscore
   • 第二个：roslaunch plumbing_test start_turtle.launch
   • 第三个：rosrun plumbing_test test03_srv_spawn_p.py 3.0 3.0 0.5 "turtle3"

2.5.4 实操04_参数设置

需求描述：修改 turtlesim 乌龟显示节点窗体的背景色，已知背景色是通过参数服务器的方式以 rgb 方式设置的。

结果演示：

实现分析：

1. 首先，需要启动乌龟显示节点；
2. 要通过ROS命令，来获取参数服务器中设置背景色的参数；
3. 编写参数设置节点，修改参数服务器中的参数值。

实现流程：

1. 通过ros命令获取参数；
2. 编码实现服参数设置节点；
3. 启动 roscore、turtlesim_node 与参数设置节点，查看运行结果。

2.5.4.1 参数名获取

1. 获取参数列表：
   • /turtlesim/background_b
   • /turtlesim/background_g
   • /turtlesim/background_r

2.5.4.2 获取参数值

1. 获取参数值：
   • rosparam get /turtlesim/background_r
   • rosparam get /turtlesim/background_g
   • rosparam get /turtlesim/background_b

2.5.4.2 参数修改

最简单的方式是直接使用命令的方式修改：

• rosparam set /turtlesim/background_r 255
• rosparam set /turtlesim/background_g 0
• rosparam set /turtlesim/background_b 0

然后重新启动节点 rosrun turtlesim turtlesim_node，会发现背景变为亮红色了：

接下来我们编写参数设置节点，修改背景颜色为亮红色。

新建 test04_par_back_p.py 文件，并配置 CMakeLists.txt文件：

#! /usr/bin/env python"""需求：修改参数服务器中 turtlesim 背景色相关的参数实现：1. 初始化 ROS 节点2. 设置参数
"""import rospydef set_param_back():# 初始化 ROS 节点rospy.init_node('set_param_back')# 设置参数rospy.set_param("/turtlesim/background_r", 100)rospy.set_param("/turtlesim/background_g", 100)rospy.set_param("/turtlesim/background_b", 100)if __name__ == '__main__':try:set_param_back()except Exception as e:print(e)

注意：赋予 Python 文件可执行权限，之后在终端运行 rosrun plumbing_test test04_par_back_p.py，并重启 turtlesim_node 节点，查看运行结果。

2.5.4.3 补充其他设置方式

2.5.4.3.1 方式1：修改小乌龟节点的背景色(命令行实现)

rosparam set /turtlesim/background_b 自定义数值
rosparam set /turtlesim/background_g 自定义数值
rosparam set /turtlesim/background_r 自定义数值

修改相关参数后，重启 turtlesim_node 节点，背景色就会发生改变了。

2.5.4.3.2 方式2：启动节点时，直接设置参数

rosrun turtlesim turtlesim_node _background_r:=100 _background_g=0 _background_b=0

2.5.4.3.3 方式3：通过launch文件传参

<launch><node pkg="turtlesim" type="turtlesim_node" name="set_bg" output="screen"><!-- launch 传参策略1 --><!-- <param name="background_b" value="0" type="int" /><param name="background_g" value="0" type="int" /><param name="background_r" value="0" type="int" /> --><!-- launch 传参策略2 --><rosparam command="load" file="$(find demo03_test_parameter)/cfg/color.yaml" /></node></launch>

2.6 通信机制比较

三种通信机制中，参数服务器是一种数据共享机制，可以在不同的节点之间共享数据，话题通信与服务通信是在不同的节点之间传递数据的，三者是ROS中最基础也是应用最为广泛的通信机制。

这其中，话题通信和服务通信有一定的相似性也有本质上的差异，在此将二者做一下简单比较。

二者的实现流程是比较相似的，都是涉及到四个要素：

要素编号	要素内容	实现形式
要素1	消息的发布方/客户端	Publisher/Client
要素2	消息的订阅方/服务端	Subscriber/Server
要素3	话题名称	Topic/Service
要素4	数据载体	msg/srv

可以概括为: 两个节点通过话题关联到一起，并使用某种类型的数据载体实现数据传输。

二者的实现也是有本质差异的，具体比较如下：

通信模式	Topic(话题)	Service(服务)
同步性	异步	同步
底层协议	ROSTCP/ROSUDP	ROSTCP/ROSUDP
缓冲区	有	无
实时性	弱	强
节点关系	多对多	一对多(一个 Server)
通信数据	msg	srv
使用场景	连续高频的数据发布与接收:雷达、里程计	偶尔调用或执行某一项特定功能: 拍照、语音识别

2.7 本章小节

本章主要介绍了ROS中最基本的也是最核心的通信机制实现: 话题通信、服务通信、参数服务器。每种通信机制，都介绍了如下内容：

1. 伊始介绍了当前通信机制的应用场景；
2. 介绍了当前通信机制的理论模型；
3. 介绍了当前通信机制的 Python 实现。

除此之外，还介绍了：

1. ROS中的常用命令方便操作、调试节点以及通信信息；
2. 通过实操又将上述知识点加以整合；
3. 最后又着重比较了话题通信与服务通信的相同点以及差异。