ROS多個(gè)master消息互通

#! /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)) + 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()

#! /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 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 Nonedef 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##把接受的數(shù)據(jù)重新打包成ros topic發(fā)出去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，監(jiān)聽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)#設(shè)置noblock，否則會(huì)阻塞在接聽，下面while不會(huì)一直循環(huán)，只有在有數(shù)據(jù)才進(jìn)行下一次循環(huán)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()