Note: This tutorial assumes that you have completed the previous tutorials: 理解ROS服务和参数. |
Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags. |
编写简单的发布者和订阅者
Description: 这个教程将引导您创建一个ROS包和两个简单的rospy节点。talker节点将在chatter话题上广播一个消息,而listener节点将接收并输出该消息。Tutorial Level: BEGINNER
Next Tutorial: Writing a simple service and client
编写发布者节点
“节点”是连接到ROS网络的可执行文件。在这里,我们将创建talker(发布者)节点,该节点将不断广播消息。
将目录切换到之前的教程中创建的beginner_tutorials包中:
$ roscd beginner_tutorials
代码
首先让我们创建一个scripts目录来存放我们的Python脚本:
$ mkdir scripts $ cd scripts
然后下载示例脚本talker.py放到scripts目录中并给执行权限:
$ wget https://raw.github.com/ros/ros_tutorials/noetic-devel/rospy_tutorials/001_talker_listener/talker.py # 若遇到网络问题,请打开上面文件的链接并复制文本内容到talker.py文件中 $ chmod +x talker.py
先不要运行它。你可以通过下面的命令查看和编辑这个文件。
$ rosed beginner_tutorials talker.py
1 #!/usr/bin/env python 2 # license removed for brevity 3 import rospy 4 from std_msgs.msg import String 5 6 def talker(): 7 pub = rospy.Publisher('chatter', String, queue_size=10) 8 rospy.init_node('talker', anonymous=True) 9 rate = rospy.Rate(10) # 10hz 10 while not rospy.is_shutdown(): 11 hello_str = "hello world %s" % rospy.get_time() 12 rospy.loginfo(hello_str) 13 pub.publish(hello_str) 14 rate.sleep() 15 16 if __name__ == '__main__': 17 try: 18 talker() 19 except rospy.ROSInterruptException: 20 pass
然后将以下内容添加到CMakeLists.txt文件。这样可以确保正确安装Python脚本,并使用合适的Python解释器。
catkin_install_python(PROGRAMS scripts/talker.py DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} )
解释
现在,让我们把代码分解。
1 #!/usr/bin/env python
每个Python ROS节点的最开头都有这个声明。第一行确保脚本作为Python脚本执行。
如果要编写ROS节点,则需要导入rospy。std_msgs.msg的导入则是为了使我们能够重用std_msgs/String消息类型(即一个简单的字符串容器)来发布。
这部分代码定义了talker与其他ROS部分的接口。pub = rospy.Publisher("chatter", String, queue_size=10)声明该节点正在使用String消息类型发布到chatter话题。这里的String实际上是std_msgs.msg.String类。queue_size参数是在ROS Hydro及更新版本中新增的,用于在订阅者接收消息的速度不够快的情况下,限制排队的消息数量。对于ROS Groovy及早期版本来说,只需省略即可。
下一行的rospy.init_node(NAME, ...)非常重要,因为它把该节点的名称告诉了rospy——只有rospy掌握了这一信息后,才会开始与ROS主节点进行通信。在本例中,你的节点将使用talker名称。注意:名称必须是基本名称,例如不能包含任何斜杠/。
anonymous = True会让名称末尾添加随机数,来确保节点具有唯一的名称。请参考从启动到关机——初始化ROS节点。
9 rate = rospy.Rate(10) # 10hz
此行创建一个Rate对象rate。借助其方法sleep(),它提供了一种方便的方法,来以你想要的速率循环。它的参数是10,即表示希望它每秒循环10次(只要我们的处理时间不超过十分之一秒)!
这个循环是一个相当标准的rospy结构:检查rospy.is_shutdown()标志,然后执行代码逻辑。你必须查看is_shutdown()以检查程序是否应该退出(例如有Ctrl+C或其他)。在本例中,代码逻辑即对public .publish(hello_str)的调用,它将一个字符串发布到chatter话题。循环的部分还调用了rate.sleep(),它在循环中可以用刚刚好的睡眠时间维持期望的速率。
你可能也见过rospy.sleep(),它和time.sleep()类似,不同的是前者还能用于模拟时间(参见Clock)。
此循环还调用了rospy.loginfo(str),它有3个任务:打印消息到屏幕上;把消息写入节点的日志文件中;写入rosout。rosout是一个方便的调试工具:您可以使用rqt_console来拉取消息,而不必在控制台窗口找你节点的输出。
std_msgs.msg.String是一个非常简单的消息类型,那更复杂的类型怎么发布呢?一般的经验法则是构造函数参数的顺序与.msg文件中的顺序相同。也可以不传入任何参数,直接初始化字段,例如:
msg = String() msg.data = str
或者可以只初始化某些字段,并将其余字段保留为默认值:
String(data=str)
你可能会想知道最后一点:
除了标准的Python __main__检查,它还会捕获一个rospy.ROSInterruptException异常,当按下Ctrl+C或节点因其他原因关闭时,这一异常就会被rospy.sleep()和rospy.Rate.sleep()抛出。引发此异常的原因是你不会意外地在sleep()之后继续执行代码。
现在我们需要编写一个节点来接收消息。
编写订阅者节点
代码
下载示例脚本listener.py放到scripts目录中并给执行权限:
$ roscd beginner_tutorials/scripts/ $ wget https://raw.github.com/ros/ros_tutorials/noetic-devel/rospy_tutorials/001_talker_listener/listener.py # 若遇到网络问题,请打开上面文件的链接并复制文本内容到listener.py文件中 $ chmod +x listener.py
文件内容类似于:
1 #!/usr/bin/env python
2 import rospy
3 from std_msgs.msg import String
4
5 def callback(data):
6 rospy.loginfo(rospy.get_caller_id() + "I heard %s", data.data)
7
8 def listener():
9
10 # In ROS, nodes are uniquely named. If two nodes with the same
11 # name are launched, the previous one is kicked off. The
12 # anonymous=True flag means that rospy will choose a unique
13 # name for our 'listener' node so that multiple listeners can
14 # run simultaneously.
15 rospy.init_node('listener', anonymous=True)
16
17 rospy.Subscriber("chatter", String, callback)
18
19 # spin() simply keeps python from exiting until this node is stopped
20 rospy.spin()
21
22 if __name__ == '__main__':
23 listener()
然后,编辑你CMakeLists.txt中的catkin_install_python()调用,如下所示:
catkin_install_python(PROGRAMS scripts/talker.py scripts/listener.py DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} )
解释
listener.py的代码类似于talker.py,只不过我们为订阅消息引入了一种新的基于回调的机制。
这声明你的节点订阅了chatter话题,类型是std_msgs.msgs.String。当接收到新消息时,callback函数被调用,消息作为第一个参数。
我们还稍微更改了对rospy.init_node()的调用。我们添加了anonymous=True关键字参数。ROS要求每个节点都有一个唯一的名称,如果出现具有相同名称的节点,则会与前一个节点发生冲突,这样一来,出现故障的节点很容易地被踢出网络。anonymous=True标志会告诉rospy为节点生成唯一的名称,这样就很容易可以有多个listener.py一起运行。
最后再补充一下,rospy.spin()只是不让你的节点退出,直到节点被明确关闭。与roscpp不同,rospy.spin()不影响订阅者回调函数,因为它们有自己的线程。
构建节点
我们使用CMake作为构建系统。是的,即使是Python节点也必须使用它。这是为了确保能为创建的消息和服务自动生成Python代码。
We also use a Makefile for a bit of convenience. roscreate-pkg automatically created a Makefile, so you don't have to edit it.
Now run make:
$ make
回到catkin工作空间,然后运行catkin_make:
$ cd ~/catkin_ws $ catkin_make
运行节点
运行节点前需要启动roscore。ROS核心是节点和程序的集合,这些程序是基于ROS的系统的先决条件。为了让ROS节点进行通信,必须运行一个roscore。打开一个新终端,输入:
$ roscore
roscore将输出类似这样的内容: roscore will output something similar to this:
... logging to /u/nkoenig/ros-jaunty/ros/log/d92b213a-90d4-11de-9344-00301b8246bf/roslaunch-ncq-11315.log ... loading XML file [/u/nkoenig/ros-jaunty/ros/tools/roslaunch/roscore.xml] Added core node of type [rosout/rosout] in namespace [/] started roslaunch server http://ncq:60287/ SUMMARY ====== NODES starting new master (master configured for auto start) process[master]: started with pid [11338] ROS_MASTER_URI=http://ncq:11311/ setting /run_id to d92b213a-90d4-11de-9344-00301b8246bf +PARAM [/run_id] by /roslaunch +PARAM [/roslaunch/uris/ncq:60287] by /roslaunch process[rosout-1]: started with pid [11353] started core service [/rosout] +SERVICE [/rosout/get_loggers] /rosout http://ncq:36277/ +SERVICE [/rosout/set_logger_level] /rosout http://ncq:36277/ +SUB [/time] /rosout http://ncq:36277/ +PUB [/rosout_agg] /rosout http://ncq:36277/ +SUB [/rosout] /rosout http://ncq:36277/
现在运行talker/listener的一切都准备好了。在新终端中输入:
$ rosrun beginner_tutorials talker.py
现在回到开始的终端中输入:
$ rosrun beginner_tutorials listener.py
rosrun只是一个方便的脚本。你想输入./talker.py也没问题。
Talker的输出应类似于:
[INFO] [WallTime: 1394915011.927728] hello world 1394915011.93 [INFO] [WallTime: 1394915012.027887] hello world 1394915012.03 [INFO] [WallTime: 1394915012.127884] hello world 1394915012.13 [INFO] [WallTime: 1394915012.227858] hello world 1394915012.23 ...
而listener应类似于:
[INFO] [WallTime: 1394915043.555022] /listener_9056_1394915043253I heard hello world 1394915043.55 [INFO] [WallTime: 1394915043.654982] /listener_9056_1394915043253I heard hello world 1394915043.65 [INFO] [WallTime: 1394915043.754936] /listener_9056_1394915043253I heard hello world 1394915043.75 [INFO] [WallTime: 1394915043.854918] /listener_9056_1394915043253I heard hello world 1394915043.85 ...
现在已经编写了第一个侦听器节点。您还应该知道,ROS为任何话题都附送了他们自己的通用listener,名为rostopic。如果你运行rostopic echo topic_name,你会得到与你写的listener.py类似的输出:
$ rostopic echo chatter
data: hello world 1394915083.35 --- data: hello world 1394915083.45 --- data: hello world 1394915083.55 --- data: hello world 1394915083.65 --- data: hello world 1394915083.75 --- ...
恭喜!你刚刚运行了第一个Python ROS节点。要了解更多示例代码,请参阅rospy_tutorials包,或者转到下一个教程Writing a Simple Service and Client。