文章

ROS1 程序编写

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

会看到输入窗口,海龟展示窗口。我录制了一个视频给大家看看效果:

my_project

程序模板

这里我们提供了两个样板,可以根据具体的需求简要改进。

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()
本文由作者按照 CC BY 4.0 进行授权

热门标签