ROS1 程序编写
(注意:本站所有博客均是记录本人学习过程中的理解,可能存在错误和不完善之处,请仔细甄别)
ROS的程序编写
本章会结合我的一个小的项目设计,来介绍我们在ROS1系统上部署代码的一般流程。如果想要在自己的电脑上复现,可以通过以下链接下载代码并在本机训练好我们后面会用的模型:
1
2
3
4
https://bhpan.buaa.edu.cn/link/AA64A6CA6A316C48289F9DCB2EC3C7CD10
文件夹名:Fast-text
有效期限:2030-04-07 19:48
提取码:uU2Y
上一章我们讲到工作区的初始化,现在我们开始着手包中的src内的代码编写。ROS的代码可以用python或者C++来编写,我使用的是更好理解的python,目前还没用过C++,如果有明显的区别会后续更新。
我们先看项目内容:我打算在一个窗口输入一段话,导入模型判断这句话是积极还是消极,并由此指导我们的小海龟做出不同的移动策略。
这个项目看起来很简单,为了展示在ROS上的部署,我们给这个任务做一下分工:
- 获取用户输入的text(text_input_node)
- 调用训练好的模型,输出判断结果(decision_maker_node)
- 依据判断结果,生成海龟移动的参数(drawing_node)
- 显示海龟的移动(turtlesim_node)
由此,我们需要四个节点,它们的名称我写在了上面的括号中。其中,text_input_node要向decision_maker_node传递String类型的信息,decision_maker_node要向drawing_node传递String类型的信息,drawing_node要向turtlesim_node传递Twist类型的信息(这个类型是控制机器人移动的参数,海龟的运动也需要这个类型的信息)。这个例子比较简单,所以我们可以直接分析出来关系。每一组信息传递,我们设置一个话题来连接即可。我们先编写text_input_node.py:
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
import rospy
from std_msgs.msg import String
def text_inputter():
# 创建一个发布者对象,发布到名为'/user_text'的话题,消息类型为String,队列大小为10
pub = rospy.Publisher('/user_text', String, queue_size=10)
# 初始化一个名为'text_input_node'的节点,anonymous=True确保节点名唯一
rospy.init_node('text_input_node', anonymous=True)
# 设置一个发布速率,这里是1Hz(每秒1次)
rate = rospy.Rate(1)
# 在终端上打印一条日志信息,表示节点已准备好
rospy.loginfo("Text input node is ready. Please enter a sentence...")
# rospy.is_shutdown()会检查ROS是否已被关闭(如Ctrl+C)
while not rospy.is_shutdown():
try:
# 等待用户在终端输入
user_input = input("Enter text: ")
# 如果用户有输入
if user_input:
# 调用publish()方法发布消息
pub.publish(user_input)
# 在终端上打印发布的文本
rospy.loginfo("Sent text: %s", user_input)
except EOFError:
# 处理Ctrl+D等输入结束的情况
rospy.loginfo("Input node shutting down.")
break
# 依照之前定义的速率(1Hz)进行休眠,以控制循环频率
rate.sleep()
if __name__ == '__main__':
try:
text_inputter()
except rospy.ROSInterruptException:
# 捕获ROS关闭时(如Ctrl+C)的异常,防止报错
pass
前面的第一行注释不可省略:因为是在linux环境下运行,我们要使用python3来运行。我们分步来解析一下这份代码:
导入:
第一行导入了rospy库,这个库是连接Python和ROS的必要库。我们初始化节点,设置订阅和发布都要用到这个库。
from std_msgs.msg import String这个事导入数据类型的代码,一般节点编写都是必要的,根据需要导入对应的类型即可。常用的类型有:- std_msgs: 包含所有基本数据类型,如 String (字符串), Int32 (32位整数), Float64 (64位浮点数), Bool (布尔值), Header (带时间戳和坐标系ID的头部信息)。
- geometry_msgs: 用于表示几何学概念,如 Point (点), Vector3 (向量), Pose (位姿,包含位置和姿态), Twist (速度,包含线速度和角速度)。
- sensor_msgs: 用于传感器数据,如 Image (图像), LaserScan (激光雷达扫描), Imu (惯性测量单元)。
初始化:
我们在开头初始化了发布者对象和节点,可以观察到使用的函数:
rospy.init_node('node_name', anonymous=True)- 功能: 初始化一个节点,并告诉
roscore它的存在。这是任何ROS节点的第一步。 anonymous=True确保了每次启动节点时,它的名字都是独一无二的,避免重名冲突。
- 功能: 初始化一个节点,并告诉
rospy.Publisher('topic_name', MessageType, queue_size=10)- 功能: 创建一个发布者对象。
- 参数: 话题名,消息的数据类型,消息队列大小(如果订阅者接收不过来,最多缓存多少条)。
一般在函数开始就先初始化节点和话题,如果要订阅也是先订阅话题。
循环主体:
由于我们的节点需要一直运行,所以它的主体部分要设计成一个循环。展示的代码引入了错误处理,是比较规范的写法。我们获取信息,发布信息,都应该写在循环内。发布信息的函数为:
pub.publish(message_object)- 功能: 通过一个发布者对象
pub,将一个消息message_object发送出去。
- 功能: 通过一个发布者对象
同时我们要配置rate来控制循环的速率。我们可以在循环前初始化一个rate,然后循环内用
rate.sleep()控制即可rospy.Rate(hz)&rate.sleep()- 功能: 配合使用,用于精确控制一个循环的执行频率。
rate = rospy.Rate(10)创建一个10Hz的频率控制器。- 在
while循环的末尾调用rate.sleep(),它会自动计算需要休眠多久,以保证循环恰好以10Hz的频率运行。这对于发布者节点来说非常重要。
至此我们完成了第一个节点的代码编写,这个节点作为发布者,初始化了话题user_text,后续订阅的时候会用到。接着我们看第二个节点decision_maker_node.py:
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
import rospy
from std_msgs.msg import String
import fasttext
import os
import rospkg # 导入rospkg库,用于查找ROS包的路径
# --- 全局变量定义 ---
# 将发布者对象定义为全局变量,以便在回调函数和主函数中都能访问
command_pub = None
# 将模型对象定义为全局变量,避免每次回调都重新加载模型
model = None
# --- 回调函数定义 ---
# 当'/user_text'话题收到消息时,此函数会被自动调用
def text_callback(message):
global command_pub, model
# 如果模型没有成功加载,则直接返回
if not model: return
# 从消息中获取文本数据,并进行简单的预处理(转小写、去首尾空格)
text = message.data.lower().strip()
rospy.loginfo("Sentiment brain received text: '%s'", text)
# 使用加载好的fasttext模型进行预测
predictions = model.predict(text)
# 提取预测结果中的标签
predicted_label = predictions[0][0]
rospy.loginfo("Model prediction: %s", predicted_label)
# 根据预测的标签,决定要发送的指令
if predicted_label == '__label__positive':
command = "draw_check" # 积极情绪 -> 画圈
rospy.loginfo("Judged as POSITIVE, sending 'draw circle' command!")
else:
command = "draw_cross" # 消极情绪 -> 旋转
rospy.loginfo("Judged as NEGATIVE, sending 'spin' command!")
# 发布最终的指令
command_pub.publish(command)
# --- 主函数定义 ---
def decision_maker():
global command_pub, model
# 初始化节点
rospy.init_node('decision_maker_node', anonymous=True)
try:
# 创建一个RosPack对象,这是一个包路径查找工具
rospack = rospkg.RosPack()
# 获取我们功能包('beginner_tutorials')的绝对路径
package_path = rospack.get_path('beginner_tutorials')
# 定义模型文件名
model_name = "sentiment_classifier.bin"
# 使用os.path.join安全地拼接出模型的完整路径
# 这样做可以确保代码在任何机器上都能正确找到模型文件
model_path = os.path.join(package_path, 'model', model_name)
rospy.loginfo("Loading sentiment brain from: %s", model_path)
# 加载fasttext模型
model = fasttext.load_model(model_path)
rospy.loginfo("Model loaded successfully! The turtle now has sentiment analysis ability!")
except Exception as e:
# 如果加载模型失败,打印错误日志并退出
rospy.logerr("Failed to load model: %s", e)
return
# 创建一个订阅者,订阅'/user_text'话题,并指定'text_callback'为回调函数
rospy.Subscriber('/user_text', String, text_callback)
# 创建一个发布者,发布指令到'/drawing_command'话题
command_pub = rospy.Publisher('/drawing_command', String, queue_size=10)
rospy.loginfo("AI Decision (Sentiment) node is ready, waiting for text...")
# rospy.spin()使节点进入循环等待状态,持续监听话题并处理回调函数
rospy.spin()
if __name__ == '__main__':
decision_maker()
这个比第一个要复杂一点,一方面是我们要调用我用fasttext训练的模型,另一方面是订阅比发布的操作要复杂一点。我们一起来看:
- 导入:相比于之前,ros相关库多了一个rospkg。这个库是用来定义我们的包的位置的。为什么要多这个库呢?因为我们要调用的模型是放在包目录的model文件夹下的,我们要使用外部文件了,用这个库来指定路径会更加规范。
- 初始化:初始化decision_maker_node节点,以及订阅话题/user_text,发布话题/drawing_command。订阅的函数如下:
rospy.Subscriber('topic_name', MessageType, callback_function)- 功能: 创建一个订阅者对象。
- 参数: 话题名,消息的数据类型,以及一个回调函数。当收到消息时,ROS会自动调用这个函数。
比起发布话题,订阅话题需要提供一个回调函数,这也是为什么我们这个文件下写了两个函数。每当有数据被订阅者接收,都会 调用一次回调函数,将接受到的数据作为参数传入回调函数,所以我们在回调函数里编写消息处理的代码。这里我们在回调函数 里调用模型解析消息,并直接由发布者发布。
- 循环主体:我们发现这里没有再用while。ros内置的
rospy.spin()可以保持节点处于运行状态,并在终端终止时停止。这里它保持了订阅者处于活跃状态,和while循环起到了类似的作用。rospy.spin()- 功能: 让你的节点进入“挂起”状态,持续等待并处理回调函数。它会阻止你的程序退出,这对于订阅者节点来说是必需的。
第二个节点也编写好了。再看第三个drawing_node.py:
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
import rospy
from std_msgs.msg import String
from geometry_msgs.msg import Twist # 导入Twist消息类型,用于发布速度指令
from std_srvs.srv import Empty # 导入Empty服务类型,用于调用turtlesim的reset服务
import math
# --- 全局变量定义 ---
# 小海龟速度指令的发布者
turtle_pub = None
# 调用/reset服务的客户端代理
reset_proxy = None
# --- 辅助函数:控制海龟移动 ---
def move_turtle(linear_vel, angular_vel, duration):
global turtle_pub
# 创建一个Twist消息对象
cmd = Twist()
# 设置线速度(前进/后退)
cmd.linear.x = linear_vel
# 设置角速度(左转/右转)
cmd.angular.z = angular_vel
# 记录开始时间
start_time = rospy.Time.now().to_sec()
# 在指定的时间段内,持续发布速度指令
while (rospy.Time.now().to_sec() - start_time) < duration and not rospy.is_shutdown():
turtle_pub.publish(cmd)
rospy.sleep(0.01) # 短暂休眠,避免占用过多CPU
# 循环结束后,发布一个全为0的Twist消息,让海龟停下来
turtle_pub.publish(Twist())
# --- 绘图动作函数 ---
def draw_circle():
global reset_proxy
rospy.loginfo("Command received, drawing a perfect circle O")
# 调用服务,重置海龟的位置和姿态
reset_proxy()
# 定义画圆的线速度和角速度
linear_speed = 2.0
angular_speed = 2.0
# 计算画一个完整的圆所需的时间 (时间 = 2π / 角速度)
duration = (2 * math.pi) / angular_speed
# 调用移动函数执行画圆动作
move_turtle(linear_vel=linear_speed, angular_vel=angular_speed, duration=duration)
def draw_spin():
global reset_proxy
rospy.loginfo("Command received, spinning to show confusion...")
reset_proxy()
# 调用移动函数,只设置角速度,让海龟原地旋转
move_turtle(linear_vel=0, angular_vel=6.0, duration=2.0)
# --- 回调函数定义 ---
# 当'/drawing_command'话题收到消息时,此函数会被调用
def command_callback(message):
command = message.data
# 根据收到的指令,调用对应的绘图函数
if command == "draw_check":
draw_circle()
elif command == "draw_cross":
draw_spin()
# --- 主函数定义 ---
def drawer():
global turtle_pub, reset_proxy
rospy.init_node('drawing_node', anonymous=True)
# 等待名为'/reset'的服务上线。这是一个阻塞函数,直到服务可用才会继续执行
rospy.loginfo("Waiting for /reset service...")
rospy.wait_for_service('/reset')
# 创建一个服务客户端代理,用于后续调用'/reset'服务
reset_proxy = rospy.ServiceProxy('/reset', Empty)
rospy.loginfo("Service connected!")
# 创建订阅者,订阅指令
rospy.Subscriber('/drawing_command', String, command_callback)
# 创建发布者,向小海龟发布速度指令
turtle_pub = rospy.Publisher('/turtle1/cmd_vel', Twist, queue_size=10)
rospy.loginfo("Drawing node is ready, waiting for commands...")
rospy.spin()
if __name__ == '__main__':
drawer()
这个也和先前的类似,我们只解读一下新出现的内容:
- 消息类型 geometry_msgs/Twist: 这是ROS中用于表示物体速度的标准消息类型。它包含两个部分:
- linear: 一个Vector3对象,表示线速度,有x, y, z三个分量。对于小海龟这种在2D平面上移动的机器人,我们通常只使用linear.x来表示前进或后退。
- angular: 也是一个Vector3对象,表示角速度。对于小海龟,我们通常只使用angular.z来表示绕自身中心逆时针(正值)或顺时针(负值)旋转。
- rospy.wait_for_service 和 rospy.ServiceProxy: 除了话题(Topic)这种异步通信方式,ROS还有服务(Service)*这种*同步的、请求/响应式的通信方式。
- rospy.wait_for_service(‘/reset’): 这是一个阻塞函数。它会暂停你节点的执行,直到名为/reset的服务上线可用。这非常重要,可以防止你的节点在turtlesim节点还没准备好时就去调用它的服务,从而避免了程序出错。
- rospy.ServiceProxy(‘/reset’, Empty): 创建一个服务的“客户端”或“代理”。之后你就可以像调用普通函数一样调用这个服务了,比如代码中的 reset_proxy()。
海龟节点是内置的,它自动订阅了/turtle1/cmd_vel话题,我们直接启动就可以了。
配置launch文件
把上述代码放在src文件夹下,为了一次启动多个节点,我们按照我们上一章讲述的方法配置launch文件,文件名设置为sentiment_turtle.launch,内容如下:
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
<launch>
<!-- 1. 启动小海龟仿真器 -->
<node pkg="turtlesim" type="turtlesim_node" name="sim"/>
<!-- 2. 启动我们的文字输入节点 -->
<!-- launch-prefix="xterm -e" 会为这个节点弹出一个独立的新终端,方便我们输入 -->
<node pkg="beginner_tutorials" type="text_input_node.py" name="text_input_node"
output="screen" launch-prefix="xterm -e"/>
<!-- 3. 启动我们的AI决策节点 -->
<node pkg="beginner_tutorials" type="decision_maker_node.py" name="decision_maker_node" output="screen"/>
<!-- 4. 启动我们的乌龟绘画节点 -->
<node pkg="beginner_tutorials" type="drawing_node.py" name="drawing_node" output="screen"/>
</launch>
至此我们完成了全部的程序编写。注意编写完成后要使用chmod +x *.py赋予代码文件执行权限,还有上一章提到过的更新配置文件。我们打开一个终端,输入指令:
1
roslaunch beginner_tutorials sentiment_turtle.launch
会看到输入窗口,海龟展示窗口。我录制了一个视频给大家看看效果:
程序模板
这里我们提供了两个样板,可以根据具体的需求简要改进。
1. 发布者节点 (Publisher) 模板
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
# 1. 导入必要的库
import rospy
from std_msgs.msg import String # 导入你需要发布的消息类型
def publisher_node():
# 2. 初始化ROS节点
rospy.init_node('my_publisher_node', anonymous=True)
# 3. 创建一个发布者对象
# 发布到'my_topic'话题,消息类型为String
pub = rospy.Publisher('my_topic', String, queue_size=10)
# 4. 设置循环频率 (例如:10Hz)
rate = rospy.Rate(10)
# 5. 主循环,直到ROS被关闭
while not rospy.is_shutdown():
# --- 在这里编写你的核心逻辑 ---
# 6. 创建并填充消息对象
my_message = String()
my_message.data = "Hello from ROS publisher at %s" % rospy.get_time()
# 7. 打印日志并发布消息
rospy.loginfo("Publishing: %s", my_message.data)
pub.publish(my_message)
# 8. 按照设定的频率休眠
rate.sleep()
if __name__ == '__main__':
try:
publisher_node()
except rospy.ROSInterruptException:
pass
2. 订阅者节点 (Subscriber) 模板
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
# 1. 导入必要的库
import rospy
from std_msgs.msg import String # 导入你需要订阅的消息类型
# 2. 定义回调函数 (Callback Function)
# 这是整个订阅者的核心!当收到消息时,这个函数会被自动调用。
# 收到的消息会作为参数'message'传入。
def message_callback(message):
# --- 在这里编写你的核心逻辑 ---
# 例如,处理数据、驱动硬件、或者再发布一个新话题
# 3. 打印收到的消息内容
rospy.loginfo("I heard: %s", message.data)
def subscriber_node():
# 4. 初始化ROS节点
rospy.init_node('my_subscriber_node', anonymous=True)
# 5. 创建一个订阅者对象
# 订阅'my_topic'话题,并指定'message_callback'作为回调函数
rospy.Subscriber('my_topic', String, message_callback)
# 6. 让节点保持运行,等待消息
# rospy.spin()会一直阻塞程序,直到节点被关闭
rospy.loginfo("Subscriber node is running and waiting for messages...")
rospy.spin()
if __name__ == '__main__':
subscriber_node()
进阶模板:使用类(Class)封装节点
在前面的例子中,我们使用了全局变量(global)来在主函数和回调函数之间共享发布者(Publisher)等对象。对于简单的节点,这样做是可行的。但当节点逻辑变得复杂时,大量的全局变量会使代码难以阅读和维护。
一个更优雅、更专业的解决方案是使用Python的类(Class)来封装整个节点。这样做有几个核心优势:
- 告别全局变量:所有的发布者、订阅者、变量都成为类的属性(self.xxx),作用域被清晰地限定在对象内部。
- 状态管理:可以方便地在类的属性中存储和管理节点的状态。
- 代码复用:可以轻松地继承和扩展你的节点类。
- 结构清晰:所有与该节点相关的逻辑都被组织在一个独立的类中,可读性大大提高。
下面是使用类封装的发布者和订阅者节点的标准模板。
3. 发布者节点 (Class-based Publisher) 模板
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
import rospy
from std_msgs.msg import String
class MyPublisher:
def __init__(self):
"""
类的构造函数,在这里进行所有的初始化工作
"""
# 1. 创建一个发布者对象
# 将发布者存为类的属性 self.pub
self.pub = rospy.Publisher('my_topic', String, queue_size=10)
# 2. 设置循环频率
self.rate = rospy.Rate(10) # 10hz
# 3. 初始化一个计数器作为我们要发布的数据
self.counter = 0
rospy.loginfo("Class-based Publisher node has been initialized.")
def run(self):
"""
包含节点主循环的函数
"""
# 4. 主循环,直到ROS被关闭
while not rospy.is_shutdown():
# 5. 创建并填充消息
my_message = String()
my_message.data = "Hello from Class! Count: %d" % self.counter
# 6. 打印日志并发布消息
rospy.loginfo("Publishing: %s", my_message.data)
self.pub.publish(my_message)
# 7. 计数器自增
self.counter += 1
# 8. 按照设定的频率休眠
self.rate.sleep()
if __name__ == '__main__':
# a. 初始化ROS节点
rospy.init_node('my_class_publisher_node', anonymous=True)
try:
# b. 创建类的实例
publisher_instance = MyPublisher()
# c. 调用run方法,启动主循环
publisher_instance.run()
except rospy.ROSInterruptException:
pass
4. 订阅者节点 (Class-based Subscriber) 模板
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
import rospy
from std_msgs.msg import String
class MySubscriber:
def __init__(self):
"""
类的构造函数,在这里进行所有的初始化工作
"""
# 1. 创建一个订阅者对象
# 回调函数是类的另一个方法 self.message_callback
self.sub = rospy.Subscriber('my_topic', String, self.message_callback)
# 2. 初始化一个变量,用于存储最新收到的消息
self.latest_message = "No message received yet."
rospy.loginfo("Class-based Subscriber node has been initialized.")
def message_callback(self, message):
"""
回调函数。当收到消息时,这个函数会被ROS自动调用。
"""
# 3. 在回调函数中处理接收到的数据
rospy.loginfo("I heard: [%s]", message.data)
# 4. 将接收到的数据存储在类的属性中
self.latest_message = message.data
# 你可以在这里加入更复杂的处理逻辑...
if __name__ == '__main__':
# a. 初始化ROS节点
rospy.init_node('my_class_subscriber_node', anonymous=True)
# b. 创建类的实例。一旦创建,订阅者就开始工作了。
subscriber_instance = MySubscriber()
# c. rospy.spin() 让程序保持运行,等待回调函数被触发
rospy.spin()
