需求

有时候我们需要有几个不同的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()

结论

上面的代码在局域网内测试过，发送图像，激光的数据都可以保证不丢数据。