两个ROS-master间的通信

事情是这样的，

有一套定位系统，运行在主机A上，启动了第一个ros-master。

又有一辆小车，上面载着树莓派（称为主机B），运行着第二个ros-master,它有着自己的SLAM。

但是现在要用精准的定位系统来给小车定位并做控制，用主机A上的matlab给小车做控制程序设计。通过matlab的ros模块可以很容易获取小车的位置姿态信息，但如何把matlab的控制指令发给小车是一个问题。

因为不在同一ros系统管理下。

so , 怎么把masterA下的控制信息发给masterB下的cmd_vel接收节点呢？

-----------------------------------------------------------------------------------------------------------------------------------

首先，将两者连在同一局域网下，然后通过udp端口通信来发送控制信息包。

server :  订阅matlab的控制指令发布节点信息，把信息打包udp发送。

client :  接收udp端口通信消息，转换信息城Twist消息格式，发布在cmd_vel节点上。

++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

思路就是这么个思路，但是你以为就这样完了吗，并没有！

控制指令要求是实时的，但udp通信接受消息的client会有缓冲区，会造成指令延迟严重，没法使用！

百度不得其解，大多是c++的函数解决方案，没找到python的，但思路还是可以借鉴的。

起两个线程，一个线程不停的调用recvfrom（）函数，榨干缓冲区，

另一个线程则按照上文思路正常解包发布ros节点信息。

As is known to all ,'Talk is cheap , show me the code ',

so ,

代码如下：

'''------client-------'''
'''接受udp端口消息并发布在ros节点上'''

from socket import *
import rospy
from geometry_msgs.msg import Twist
import threading

# HOST = '192.168.207.80'
HOST = '127.0.0.1'

PORT = 21567

BUFSIZ = 64

ADDR = (HOST, PORT)
udpserverSocket = socket(AF_INET, SOCK_DGRAM)
udpserverSocket.bind(ADDR)
rospy.init_node('B1', anonymous=True)
pub = rospy.Publisher('cmd_vel', Twist, queue_size=1)
rate = rospy.Rate(10)  # 10hz

data = '....'
def get_last_data():
    global data
    while True:
        a = udpserverSocket.recvfrom(BUFSIZ)
        if a:
            data = a


def work():
    global data
    while not rospy.is_shutdown():

        mydata = data
        rec = mydata[0]
        list = rec.split(',')
        if len(list) == 6:

            msg = Twist()
            msg.linear.x = float(list[0])
            msg.linear.y = float(list[1])
            msg.linear.z = float(list[2])
            msg.angular.x = float(list[3])
            msg.angular.y = float(list[4])
            msg.angular.z = float(list[5])

            print(rec)
            pub.publish(msg)
        rate.sleep()

if __name__ == '__main__':
    th1=threading.Thread(target=get_last_data)
    th2 = threading.Thread(target=work)
    th1.start()
    th2.start()

'''接受matlab发布的节点信息并通过udp转发'''

import rospy
from geometry_msgs.msg import Twist
from socket import *
import time

HOST = '192.168.207.80'
PORT = 21567
BUSIZ = 0
ADDR = (HOST,PORT)

udpclientSock = socket(AF_INET,SOCK_DGRAM)


def callback(msg):
    lx=msg.linear.x
    ly = msg.linear.y
    lz = msg.linear.z
    ax = msg.angular.x
    ay = msg.angular.y
    az = msg.angular.z
    data = str(lx) +','+str(ly)+','+str(lz)+','+str(ax)+','+str(ay)+','+str(az)
    udpclientSock.sendto(data,ADDR)
    print(data)

rospy.init_node('A2', anonymous=True)
rospy.Subscriber("chatter1", Twist, callback)
rospy.spin()

代码就是这样的代码了，希望能帮到你吧！