Python写ROS话题

导入ROS模块

用python编写ROS的程序有很多有点，Python的numpy模块可以方便快速的完成机器人规划、正逆运动学的开发，如果需要完成更复杂的计算功能，可以使用scipy模块完成科学计算，对采集数据的时间系列分析可以采用pandas做数据分析，最重要的是Python的matplotlib模块可以完成绝大部分的数据绘图，可以与pyqt5结合完成数据的可视化显示。当然Python还可以方便的调用机器人学习、深度学习等模块完成人工智能的开发，用最短的时间完成机器人的智能化。
Python中使用ROS首先要导入rospy模块

import rospy

完成机械臂的控制还需要导入其他数据模块，ROS在C++中的数据类型在Python也可以找到，其中最常用的数据模块是std_msgs、sensor_msgs、geometry_msgs模块。

from std_msgs.msg import Float64MultiArray
from sensor_msgs.msg import JointState
from geometry_msgs.msg import WrenchStamped

发送话题

ros话题是最常用的通讯方式，通过话题实现数据传输，下面将介绍话题发布。
首先，需要建立一个一个节点，并发起一个或多个话题

#建立节点
rospy.init_node("joint_position_command", anonymous=True)
#在该节点上发起话题
pub = rospy.Publisher("/all_joints_position_group_controller/command", Float64MultiArray, queue_size=1)

其次，设定发布的频率，每秒发送的次数

rate = rospy.Rate(100) # 100hz

然后，发送数据

pub.publish(send_data)

最后，需要休眠等待

rate.sleep()

完整的程序如下

#!/usr/bin/env python
# -*-coding:utf-8-*-
# 本文档用于发送关节角度
# 程序员：CYT
# 版权：哈尔滨工业大学(深圳）
# 日期：初稿：2019.11.6

import rospy
from std_msgs.msg import Float64MultiArray

import os
import numpy as np

def talker():
    # 建立节点
    rospy.init_node("joint_position_command", anonymous=True)
    #建立话题
    pub = rospy.Publisher("joint_command", Float64MultiArray, queue_size=1)
    #设置发送频率
    rate = rospy.Rate(100)  # 100hz

    #假设数据
    command_pos = np.zeros([1000,7])

    # 重写数据
    kk = len(command_pos[:, 0])
    n = len(command_pos[0, :])
    command_data = np.zeros([kk, n])
    for i in range(kk):
        for j in range(n):
            command_data[i, j] = command_pos[i, j]

    #主循环中发送数据
    k = 0
    while not rospy.is_shutdown():
        if k == kk:
            break
        tip_str = "第 %s 次命令：" % k
        rospy.loginfo(tip_str)

        send_data = Float64MultiArray()
        send_data.data = command_data[k, :]
        print send_data.data
        #发送数据
        pub.publish(send_data)
        #休眠
        rate.sleep()
        k = k + 1

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

接收话题

Python接受话题有两种方式，但是与C++相比，其只有rospy.spin()，没有ros::spinOnce，所以想要在循环中处理需要特别注意。

第一种方式：rospy.Subscriber

第一种方式与c++类似，调用回调函数来处理，但是因为只有rospy.spin()来调用回调函数，所以程序会阻塞在回调函数中，接受到一个数据，调用一次。

#建立节点
rospy.init_node('listener', anonymous=True)
#订阅话题
rospy.Subscriber('/joint_states', JointState, callback)
#调用回调函数，并阻塞，直到程序结束
rospy.spin()

第二种方式：rospy.wait_for_message

该方法无需节点，也无需回调函数，其与一个函数类似，等待话题发布消息，当接收到一个消息时，返回数据，继续执行后面的程序。相对于第一种，我们把它称为半阻塞，接收的话题如果没有发布消息，它会一直等待，但是接收到一个消息后，等待结束，会继续执行后面的程序。

msg2 = rospy.wait_for_message('/joint_states', JointState, timeout=None)

完整程序

通过两个函数演示两种不同的就收方法，当上文中话题启动后，下文中的程序就可接受到数据。

#!/usr/bin/env python
# -*-coding:utf-8-*-
# 本文档用于接收信息
# 程序员：CYT
# 版权：哈尔滨工业大学(深圳）
# 日期：初稿：2019.12.12

import numpy as np

import rospy
from std_msgs.msg import Float64MultiArray

def callback(data):
    print "msg:", data

def listener1():
    #建立节点
    rospy.init_node('listener', anonymous=True)
    #订阅话题
    rospy.Subscriber('joint_command', Float64MultiArray, callback)
    #调用回调函数，并阻塞
    rospy.spin()

def listener2():
    #用循环来订阅所有数据
    while not rospy.is_shutdown():
        #订阅话题
        msg = rospy.wait_for_message('joint_command', Float64MultiArray, timeout=None)
        print "msg: %s" % msg

if __name__ == '__main__':
    #运行程序1
    #listener1()
    #运行程序2
    listener2()

多线程处理同时接受多个话题

在实际运用中，一个节点需要发送多个话题和接受多个话题，如机器人阻抗控制，需要同时接受关节角状态和末端六维力的数据，由于Python中没有ros::spinOnce，用rospy.spin()会阻塞程序，主程序无法执行其他模块，所以需要引入线程来处理。
单独建立一个函数，存放rospy.spin(),用于线程调用

def thread_spin(self):
    rospy.spin()

调用线程处理回调函数，使其在子线程中阻塞，主程序正常运行

# 运行线程1,收话题线程
t1 = threading.Thread(target=self.thread_spin)
t1.start()

如果采用第二中方式，程序可以类似于C++的ros::spinOnce处理话题接受，当并不相同，且存在一个缺点：rospy.wait_for_message会一直等待话题，直到接收到一个数据，其等待时间与所接受的话题有关，所以其主程序循环周期不稳定。

def listener2(self):
        # 用循环来订阅所有数据
        while not rospy.is_shutdown():
            # 订阅话题
            msg1 = rospy.wait_for_message('joint_command1', Float64MultiArray, timeout=None)
            print "msg1: %s" % msg1
            msg2 = rospy.wait_for_message('joint_command2', Float64MultiArray, timeout=None)
            print "msg2: %s" % msg2
            #处理其他函数
            print "处理其他函数！"

多个回调函数的数据可以通过全局变量供主函数调用，也可以采用类来写该函数，通过类变量共享的特点实现数据调用。

#!/usr/bin/env python
# -*-coding:utf-8-*-
# 本文档用于接收信息
# 程序员：CYT
# 版权：哈尔滨工业大学(深圳）
# 日期：初稿：2019.12.12

import numpy as np
#ros模块
import rospy
from std_msgs.msg import Float64MultiArray
#线程
import threading

class SubTopic(object):
    #定义全局变量
    qq = np.zeros(7)
    F = np.zeros(6)
    def __init__(self,flag = True):
        self.init()
        self.flag = flag
    def init(self):
        #调用第一个函数
        if(self.flag==True):
            print "第一种方式，多线程处理"
            self.listener1()
        else:
            print "第二中方式处理"
            self.listener2()

    def thread_spin(self):
        rospy.spin()

    def callback1(self, data):
        print "msg1:", data

    def callback2(self, data):
        print "msg2:", data

    def listener1(self):
        # 建立节点
        rospy.init_node('listener', anonymous=True)
        # 订阅话题
        rospy.Subscriber('joint_command1', Float64MultiArray, self.callback1)
        rospy.Subscriber('joint_command2', Float64MultiArray, self.callback2)
        # 运行线程1,收话题线程
        t1 = threading.Thread(target=self.thread_spin)  # 末端位置订阅线程
        t1.start()
        #处理其他函数
        while not rospy.is_shutdown():
            #处理其他函数
            print "处理其他函数！"

    def listener2(self):
        # 用循环来订阅所有数据
        while not rospy.is_shutdown():
            # 订阅话题
            msg1 = rospy.wait_for_message('joint_command1', Float64MultiArray, timeout=None)
            print "msg1: %s" % msg1
            msg2 = rospy.wait_for_message('joint_command2', Float64MultiArray, timeout=None)
            print "msg2: %s" % msg2
            #处理其他函数
            print "处理其他函数！"