ROS multiple master message exchange-Python Tutorial-php.cn

ROS multiple master message exchange

高洛峰

Release： 2016-11-02 11:02:40

Original

2100 people have browsed it

Requirements

Sometimes we need to have several different masters, and they need to exchange topic content. At this time, we cannot use the method of setting the same master that comes with ros.

Our approach is to construct a The client and a server run under different masters. The client subscribes to topic1 under master1, and then sends it to the server under master2 through the tcp protocol (defines a message protocol format by itself), performs message parsing, and then publishes topic1 under master2. , so that we can achieve the propagation of topic1 messages from master1 to master2 without changing the topic framework that comes with ros.

What we implement below is to pass an amcl_pose topic, the message type is PoseWithCovarianceStamped from master1 to master2, and other topics The code is similar to

client’s code

#! /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(&#39;>I&#39;,int(id)) + struct.pack(&#39;>I&#39;, 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((&#39;127.0.0.1&#39;,8000))

rospy.init_node(&#39;server_node&#39;)

rospy.Subscriber(&#39;/amcl_pose&#39;,PoseWithCovarianceStamped,odomCallback)

rospy.spin()

Copy after login

server’s code

#! /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(&#39;>I&#39;, raw_id)[0]        print "receive id: ",id
        raw_msglen = recvall(sock, 4)        if not raw_msglen:            return None
        msglen = struct.unpack(&#39;>I&#39;, 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 = &#39;&#39;
    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(&#39;,&#39;)

    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((&#39;&#39;,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()

Copy after login

Conclusion

The above code has been tested in the local area network, and it can ensure that no data is lost when sending images and laser data.