尝试一下如何用Python发布消息以控制机器人运动

0 前言

在前面的几篇博客中，我们介绍了七自由度机器人是如何运动的，解释了七自由度机器人的URDF模型描述文件，也讲解了用自带的控制器来控制七自由度机器人的运动。到目前为止，我们知道了如何通过发布topic来控制机器人运动，但是每次从终端里发布消息也未免太不方便了。因此，这篇博客我们尝试写一个Python脚本，通过这个脚本来发布topic控制机器人运动。

1 概述

在前面的博客中已经介绍了当在Gazebo中运行机器人仿真后，具有的topic列表如下：

我们知道通过发布topic控制机器人运动的方式如下(以joint4为例)：

2 用Python来控制机器人运动

2.1 Python脚本的编写

先来编写发布消息的Python脚本。打开seven_dof_arm_gazebo/scripts文件夹，并在该文件夹下新建 main.py 文件。文件内容的编写参考Writing a Simple Publisher and Subscriber (Python)。这里直接给出内容：

import rospy
from std_msgs.msg import Float64
import math

def talker():
    pub = rospy.Publisher('/seven_dof_arm/joint4_position_controller/command', Float64, queue_size=10)
    rospy.init_node('seven_dof_arm_talker', anonymous=True) # You can rename this node
    rate = rospy.Rate(10) # 10hz
    i = 0
    while not rospy.is_shutdown():
        # hello_str = "hello world %s" % rospy.get_time()
        # position = math.pi/2
        position = math.sin(i)
        rospy.loginfo(position) # print the position
        pub.publish(position)   # publish the position
        rate.sleep()
        i += 0.01

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

2.2 Python脚本的解释

在上述程序的第4行和第8行，涉及到了消息的类型，可以在终端中用rostopic info /xxx查看相应消息的类型。

从上图中，我们可以看到，/seven_dof_arm/joint4_position_controller/command 的类型是 std_msgs/Float64，所以在第4行修改import数据的类型，在第8行修改topic的名字及类型。

在程序的第9行，可以自行指定node的名字。

在while循环中，指定了让关节4做正弦运动，并将位置信息发布。

2.3 程序的运行

# 假设已经启动Gazebo仿真
# Open a new terminal
cd ~/catkin_ws
catkin_make
source devel/setup.bash
cd src/seven_dof_arm_gazebo/scripts/
ls -l
chmod a+x main.py
ls -l
rosrun seven_dof_arm_gazebo main.py

说明：在上述命令执行ls -l后，查看是否和下图的结果一致，如果不一致，是因为没有赋予文件权限。

程序运行的结果如下所示：

一点小小的疑问：
到目前为止，我们还没有介绍过七自由度机器人的两个ROS Packages中的 ROS wiki CMakeLists.txt 和 ROS wiki package.xml，这两个文件都位于ROS package下的根目录。CMakeLists.txt 是用来编译ROS package的，会包含一些依赖项，这个大家应该都很熟悉了；package.xml 是描述功能包的属性，包括功能包的名字、版本号、作者、维护者、通行证等。我理解的添加Python脚本文件后，应该在这两个文件中包含 rospy 库才对。可是没有，也能直接运行？

ROS仿真揭秘——七自由度机器人(五)