ROS多個master消息互通

需求

有時候我們需要有幾個不同的master, 他們之間要交換topic的內容，這時候就不能使用ros自帶的設置同一個master的方法.

我們的處理方法是，構造一個client和一個server，他們運行在不同的master下面, client在master1下訂閱topic1，然後通過tcp協議(自己定義一個消息協議格式)發到master2下面的server,進行消息解析，再發布出master2下面的topic1，這樣我們不改變ros自帶的topic框架，就實現topic1的消息從master1到master2的傳播.

下面我們實現的是將一個amcl_pose的topic，消息類型是PoseWithCovarianceStamped從master1傳到master2，其他的topic代碼類似

client 的代碼

#! /usr/bin/env python
# -*- coding=utf-8 -*-
import socket
import struct
import rospy
import time
from geometry_msgs.msg import PoseWithCovarianceStamped,PoseStamped


#message proto
# id |  length | data
def send_msg(sock, msg ,id):
    # Prefix each message with a 4-byte id and length (network byte order)
    msg = struct.pack('>I',int(id)) + struct.pack('>I', len(msg)) + msg
    sock.sendall(msg)


def odomCallback(msg):
    global odom_socket

    data = ""

    id = msg.header.seq
    print "send id: ",id
    x = msg.pose.pose.position.x
    y = msg.pose.pose.position.y
    #orientation
    orien_z = msg.pose.pose.orientation.z
    orien_w = msg.pose.pose.orientation.w

    data += str(x) + "," + str(y)+ "," + str(orien_z)+ "," + str(orien_w)

    send_msg(odom_socket,data,id)


odom_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
odom_socket.connect(('127.0.0.1',8000))

rospy.init_node('server_node')

rospy.Subscriber('/amcl_pose',PoseWithCovarianceStamped,odomCallback)

rospy.spin()

server 的代碼

#! /usr/bin/env python
# -*- coding=utf-8 -*-
import socket
import time,os,fcntl
import struct
import rospy
from geometry_msgs.msg import PoseWithCovarianceStamped,PoseStamped

#message proto
# id | length | data
def recv_msg(sock):
    try:
        # Read message length and unpack it into an integer
        raw_id = recvall(sock, 4)
        if not raw_id:
            return None
        id = struct.unpack('>I', raw_id)[0]
        print "receive id: ",id
        raw_msglen = recvall(sock, 4)
        if not raw_msglen:
            return None
        msglen = struct.unpack('>I', raw_msglen)[0]
        # Read the message data
        return recvall(sock, msglen)
    except Exception ,e:
        return None



def recvall(sock, n):
    # Helper function to recv n bytes or return None if EOF is hit
    data = ''
    while len(data) < n:
        packet = sock.recv(n - len(data))
        if not packet:
            return None
        data += packet
    return data

##把接受的數據重新打包成ros topic發出去
def msg_construct(data):

    list = data.split(',')

    pose = PoseWithCovarianceStamped()
    pose.header.stamp = rospy.Time.now()
    pose.header.frame_id = "/map"
    pose.pose.pose.position.x = float(list[0])
    pose.pose.pose.position.y = float(list[1])
    pose.pose.pose.position.z = 0
    pose.pose.pose.orientation.x = 0
    pose.pose.pose.orientation.y = 0
    pose.pose.pose.orientation.z = float(list[2])
    pose.pose.pose.orientation.w = float(list[3])
    pose.pose.covariance = [0.25, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.25, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.06853891945200942]

    return pose


#初始化socket，監聽8000端口
odom_socket = socket.socket(socket.AF_INET,socket.SOCK_STREAM)
odom_socket.bind(('',8000))
odom_socket.listen(10)

(client,address) = odom_socket.accept()

rospy.init_node("client_node")
odom_pub = rospy.Publisher("/amcl_pose",PoseWithCovarianceStamped,queue_size=30)
r = rospy.Rate(1000)

#設置noblock，否則會阻塞在接聽，下面while不會一直循環，只有在有數據才進行下一次循環
fcntl.fcntl(client, fcntl.F_SETFL, os.O_NONBLOCK)


while not rospy.is_shutdown():
    data = recv_msg(client)
    if data:
        odom_pub.publish(msg_construct(data))
    r.sleep()

結論

上面的代碼在局域網內測試過，發送圖像，激光的數據都可以保證不丟數據。