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 socketimport structimport rospyimport timefrom geometry_msgs.msg import PoseWithCovarianceStamped,PoseStamped#message proto# id |length | datadef 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）） + msgsock.sendall（msg）def odomCallback（msg）:global odom_socketdata = ""id = msg.header.seqprint "send id: ",idx = msg.pose.pose.position.xy = msg.pose.pose.position.y#orientationorien_z = msg.pose.pose.orientation.zorien_w = msg.pose.pose.orientation.wdata += 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 socketimport time,os,fcntlimport structimport rospyfrom geometry_msgs.msg import PoseWithCovarianceStamped,PoseStamped#message proto# id | length | datadef recv_msg（sock）:try:# Read message length and unpack it into an integerraw_id = recvall（sock, 4）if not raw_id:return Noneid = struct.unpack（">I", raw_id）[0]print "receive id: ",idraw_msglen = recvall（sock, 4）if not raw_msglen:return Nonemsglen = struct.unpack（">I", raw_msglen）[0]# Read the message datareturn recvall（sock, msglen）except Exception ,e:return Nonedef recvall（sock, n）:# Helper function to recv n bytes or return None if EOF is hitdata = ""while len（data） < n:packet = sock.recv（n - len（data））if not packet:return Nonedata += packetreturn 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 = 0pose.pose.pose.orientation.x = 0pose.pose.pose.orientation.y = 0pose.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（）

结论

上面的代码在局域网内测试过，发送图像，激光的数据都可以保证不丢数据。本文永久更新链接地址：http://www.linuxidc.com/Linux/2016-11/136659.htm