Ros Multiple Master message interworking

Demand

Sometimes we need to have a few different master, they want to exchange topic content, this time cannot use Ros to set the same master method.

Our approach is to construct a client and a server, they run under different master, the client subscribes to Topic1 under Master1, and then through the TCP protocol (it defines a message protocol format) Send to the server below Master2, for message parsing, and then release the topic1 below Master2, so that we do not change the topic framework of the ROS, the transmission of TOPIC1 messages from Master1 to Master2 is realized.

The following is the implementation of a amcl_pose topic, the message type is posewithcovariancestamped from Master1 to Master2, the other topic code similar

Client's Code

#! /usr/bin/env python#-*-coding=utf-8-*-import socketimport structimport rospyimport timefrom geometry_msgs.msg Import P  Osewithcovariancestamped,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_so Cket data = "" id = msg.header.seq print "Send ID:", id x = msg.pose.pose.position.x y = Msg.pose.pose.posi  TION.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 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:", I D 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:retu RN Nonedef Recvall (sock, N): # Helper function to recv n bytes or return None if EOF was hit data = "while Len (d    ATA) < N:packet = Sock.recv (N-len (data)) if not packet:return None data + = Packet Return data# #把接受的数据重新打包成ros topic send out 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 = f    Loat (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# initialize socket, monitor 8000 port Odom_socket = Socket.socket (Socket.af_inet,socket. Sock_stream) Odom_socket.bind ((", 8000)) Odom_socket.listen (client,address) = Odom_socket.accept () rospy.init_ Node ("Client_node") Odom_pub = Rospy. Publisher ("/amcl_pose", posewithcovariancestamped,queue_size=30) R = Rospy. Rate (#设置noblock), otherwise it will block in the answer, while the lower while will not cycle, onlyThe next cycle fcntl.fcntl (client, FCNTL) is only available with data. F_SETFL, OS. O_nonblock) while not Rospy.is_shutdown (): data = recv_msg (client) if Data:odom_pub.publish (msg_construct (DAT A)) R.sleep ()

Conclusion

The above code in the LAN test, send images, laser data can be guaranteed not to lose data.

