ROS-Mehrfach-Master-Nachrichtenaustausch

Anforderungen

Manchmal benötigen wir mehrere verschiedene Master, und diese müssen Themeninhalte austauschen. Derzeit können wir nicht die Methode zum Festlegen desselben Masters verwenden, die mit Ros geliefert wird.

Unsere Verarbeitungsmethode besteht darin, einen Client und einen Server zu erstellen, die unter verschiedenen Mastern ausgeführt werden. Der Client abonniert Thema1 unter Master1 und sendet es dann über das TCP-Protokoll an den Server unter Master2 (definieren Sie selbst ein Nachrichtenprotokollformat). . Nachrichtenanalyse und anschließende Veröffentlichung von Topic1 unter Master2, damit wir die Verbreitung von Topic1-Nachrichten von Master1 an Master2 realisieren können, ohne das Topic-Framework von ROS zu ändern.

Was wir unten implementieren, ist die Konvertierung eines amcl_pose-Themas. Der Nachrichtentyp ist PoseWithCovarianceStamped und wird von Master1 an Master2 übertragen. Andere Themencodes ähneln

Client-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('>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-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('>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(&#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()

Schlussfolgerung

Der obige Code wurde im LAN getestet und kann sicherstellen, dass beim Senden von Bildern und Laserdaten keine Daten verloren gehen.