需求
有時候我們需要有幾個不同的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)) + 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 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##把接受的数据重新打包成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()
結論
上面的程式碼在區域網路內測試過,傳送影像,雷射的資料都可以保證不遺失資料。