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 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()

结论

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

上一篇:C陷阱与缺陷 1


下一篇:Linux/Ubuntu sudo不用输入密码的方法