ROS1中使用Python发布和订阅主题

ROS是开发机器人应用的极具生产力的工具，最常用于概念开发、算法验证。在算法开发工具中，Python是一个极佳的选择，因为Python的工具包十分丰富，脚本语言的特性也使得无需编译，算法修改方便。

本文以ROS1为开发环境，以经典的Turtlesim的发布和订阅为例子，介绍用Python开发ROS机器人应用的步骤。例子中将实现用Python控制小乌龟的运动。

1. 操作系统Ubuntu18.04，安装ROS1、Python，网上安装教程很多，不再赘述。安装成功的标志是：终端中运行 roscore，不报错；终端中运行 python，不报错。

2. 通常安装ROS1后，会附带turtlesim功能包，在终端中运行 rosrun turtlesim turtlesim_node，会有一只很可爱的小乌龟出现在窗口中。

3. ROS1实际上就是实现了一个通信机制，发布/订阅是其中主要的一个通信形式。如果想查看有哪些主题在发布和订阅，可以通过命令，当然还有更直接的方式，就是用ROS1自带的rqt工具，在终端中运行 rqt，即出现可视化工具了。如果turtulesim还在运行的话，可以看到Topic中看到turtle1/cmd_vel这一主题，以及它的类型、刷新频率，这一主题实际上就是小乌龟的线速度、角速度指令了，如果去改一下数值，是可以在实际的Turtlesim窗口中观察到小乌龟的运动的。

4. 了解了ROS1的通信机制，那小乌龟的运动控制思路就很简单了：只要给相应的主题发指令就行了。我们的Python代码的目的也很明确了：新建一个节点 -> 连接到一个话题 -> 向话题发布信息。

就这么简单，下面就构建ROS1中的Python开发环境，用到caktin指令。

5. 在Ubuntu中新建文件夹，作为工作空间，文件夹下新建src文件夹，这很重要，编译时要找这个文件夹的。

6. 定位到src文件夹下，运行终端，输入命令 catkin_init_workspace，初始化工作空间。再输入命令 catkin_create_pkg demo rospy std_msgs，建立文件包，这里的demo是起的包的名字，请修改成你自己的包的名字。不出意外的话，src文件夹下自动出现了demo文件夹，下面有自动建立的一系列文件和文件夹。

7. 定位到src/demo文件夹下，新建scripts文件夹，这是python文件的存放位置，当然也可以放在别的位置，后面可以在CMakeLists文件中修改。在scripts文件夹下新建python文件，比如 talker.py。右键修改其属性为 Allow executing file as program，这一步很关键，否则后面运行会提示找不到可执行文件。

8. 双击打开，用文本编辑该文件。输入你的python程序，比如下面，注意检查语法中的缩进字符：

#! /usr/bin/env python

import rospy
from geometry_msgs.msg import Twist
import numpy as np

def talker():
    rospy.init_node('talker', anonymous=True)
    pub = rospy.Publisher('turtle1/cmd_vel', Twist, queue_size=10)
    rate = rospy.Rate(1) # 1hz
    while not rospy.is_shutdown():
        vel_msg = Twist()
        vel_msg.linear.x = 0.5 * np.random.uniform(-1, 1)
        vel_msg.angular.z = 0.2 * np.random.uniform(-1, 1)
        pub.publish(vel_msg)
        rate.sleep()

if __name__ == '__main__':
    try:
        talker()
    except rospy.ROSInterruptException:
        pass
 

9. 修改CMakeList.txt，主要是找到 catkin_install_python 这一重要信息，将它从注释中释放出来，并把my_python_script修改为你的python文件的地址。（看到第7步中建立scripts文件夹的意义了吧）

## Mark executable scripts (Python etc.) for installation
## in contrast to setup.py, you can choose the destination
 catkin_install_python(PROGRAMS
   scripts/my_python_script
   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
 )

10. 定位到工作空间根目录，在终端中运行命令 catkin_make demo，这里demo是包的名字。可以看到自动生成了一系列文件，build中存放的是可执行文件，devel中存放的是安装包。

11. 在终端中运行 source devel/setup.bash，这一步是将包的名字放到环境变量中，否则后面ROS会提示无法找到这个包。

12. 万事具备，来检验一下我们的环境是否搭建成功吧。首先运行 roscore，启动基础通信服务；然后运行 rosrun turtlesim turtlesim_node，启动小乌龟程序，该程序订阅了turtle1/cmd_vel主题；最后运行 rosrun demo talker.py，启动我们的python程序，程序将按照1Hz的频率向主题turtle1/cmd_vel发布数据。可以看到我们的小乌龟按照预期运动起来了！

13. python程序可以随时修改，但修改后要先 ctrl+c 结束运行，然后重新输入 rosrun demo talker.py，才可以执行新的程序。

其实gazebo仿真算法的思路也是类似的，主要就是用rqt找到相应的主题，然后订阅或发布数据给该主题。剩下就是python算法的设计了。