ROS中编写Publisher和Subscriber的方法（Python版）

参考：http://wiki.ros.org/ROS/Tutorials/WritingPublisherSubscriber(python)

1 编写Publisher节点

节点是连接到ROS网络的可执行文件的ROS术语。在这里，我们创建一个不断广播消息的发布者（“talker”）节点。创建一个ros包，也可以用现有的ros包，比如：

$ roscd beginner_tutorials

1.1 代码

首先创建一个‘script’的路径来保存python代码

$ mkdir scripts
$ cd scripts

然后，新建一个文件，命名为talker.py，复制以下代码进去：

#!/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

1.2 代码解释

让我们来一行一行地看代码意义

   1 #!/usr/bin/env python

每个python版本的ROS节点在开头都有这样一个声明，表示这个文件是python类型

   3 import rospy
   4 from std_msgs.msg import String

如果要写ROS节点，需要导入rospy。std_msgs.msg的目的是可以使用std_msgs/String消息类型来发布

   7     pub = rospy.Publisher('chatter', String, queue_size=10)
   8     rospy.init_node('talker', anonymous=True)

这部分代码定义了talker与其它ROS节点的通讯。
pub = rospy.Publisher("chatter", String, queue_size=10) 表示你正在使用String类型的消息来发布你的节点到chatter。String就是std_msgs.msg.String类。如果任何订阅者都没有足够快地接收到消息，queue_size将会限制队列消息的数量。在旧的ROS版本中，忽略掉了这一点。

下一行是非常重要的，因为它告诉rospy你的ros节点名字，直到rospy有了这个信息，它不能够和ROS Master开始通讯。在这个例子中，你的节点名字是talker
anoymous=True 通过在你名字的后边添加一个随机数，来保证你的节点独一无二。

   9     rate = rospy.Rate(10) # 10hz

这一行创建速率对象rate.在其方法sleep()的帮助下，它提供了一个以一定速率循环的方便的方法。参数10表示我们期望以每秒循环10次（只要我们的处理时间不超过1/10秒）

  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()

这个循环是一个相当标准的rospy结构：检查rospy.is_shutdown标志位然后开始工作（‘work’）。你必须检查is_shutdown()来确定你的成熟是否应该退出（例如有Ctrl-C操作或其它）。在这个例子中，‘work’是调用pub.publish(hello_str)来发布一个字符串到chatter话题。循环调用rate.sleep()，睡眠足够的时间，以便通过循环来保持所需的速率。
（你可以运行rospy.sleep()和time.sleep()来达到相同的定时效果）
循环中海油rospy.loginfo(str)，这条执行三重任务：消息打印到屏幕上，写入到节点的日志文件中，并且被写入rosout。rosout可以方便的进行调试：你可以使用rqt_console来提取消息，而不必使用节点的输出找到控制台窗口。
std_msgs.msg.String是一个非常简单的消息类型，所以你可能会想知道发布更复杂的类型是什么样子。一般的经验是构造函数args与.msg文件中的顺序相同。你也可以传递任何参数，也可以直接初始化字段。

msg = String()
msg.data = str

或者可以初始化一些值，剩余的采用默认值：

String(data=str)

你可能会好奇剩余的几行代码：

  17     try:
  18         talker()
  19     except rospy.ROSInterruptException:
  20         pass

除了标准的Python_main_检查之外，他会捕获一个rospy.ROSInterruptException异常，当Ctrl-C被按下或者Node被关闭时，它将以rospy.sleep()和rospy.Rate.sleep()的方法抛出。引发这个异常的原因是因为在sleep()之后不会再继续执行代码。
现在，让我们来写一个节点来接收这条消息。

2.写一个Subscriber节点

2.1 代码

还是在上一节文件夹中，建立一个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     # node 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()

2.2 代码解释

listener.py与talker.py文件类似，在listener中会引入一种新的基于回调机制callback来订阅消息。

  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()

这个声明表示你的节点订阅消息类型为std_msgs.msgs.String的chatter主题。当接收到新的消息时，回调callback将作为第一个参数被调用。
我们也改变了对rospy.init_node()的调用，我们添加了anonymous=True关键字参数。ROS要求每个节点都有唯一的名称，如果有相同名称的节点出现，则会突破前一个节点。这样就可以很容易地从网络上启动故障的节点。anonymous=True标志高速rospy为节点生成唯一的名称，以便可以轻松地运行多个listener.py节点。
最后添加rospy.spin()只是为了让你的节点退出，直到节点已经关闭。与roscpp不同，rospy.spin()不影响用户回调函数，因为它们有自己的线程。

3 构建自己的节点

我们使用CMake作为我们的构建系统，是的，即使对于python节点也必须使用它。这是为了确保自动生成的消息和服务的python代码被创建。
运行到catkin工作空间，然后运行catkin_make:

$ cd ~/catkin_ws
$ catkin_make

运行节点的方法参考http://wiki.ros.org/ROS/Tutorials/ExaminingPublisherSubscriber