tfは遅い？

現在、grSimとROSの結合作業をしています。

とりあえず、grSimからマルチキャストで送信される各ロボットの座標を、tfでブロードキャストしようと思ったのですが、問題発生。

おおよそ60Hzで座標データが飛んでくるので、その周期に合わせてブロードキャストしようと思い、コードを書きました。 しかし、2つ以上のデータを60Hzでブロードキャストしようとすると、一番最後にブロードキャストした以外のフレームの更新頻度が極端に落ちます。 一番最後にブロードキャストしたフレームは60Hz近くで更新されるのですが、それ以外が10Hzとか3Hzとか。

いろいろと原因を探ってみたのですが、いまいちよく分からず。 帯域が逼迫しているわけでもなさそうですし、tf.sendTransform()自体もそんな時間は食っていないようです。

一つのノードからは、高い周波数で複数のフレームをブロードキャストできないのかもしれません…。困ったな。 何か打開策を考えます。

#!/usr/bin/env  python
import  rospy
import  tf
import  socket
import  messages_robocup_ssl_wrapper_pb2

##################

multicast_if_addr='192.168.0.10'
multicast_group='224.5.23.2'
multicast_port=10020

my_addr='0.0.0.0'
server_address=(my_addr, multicast_port)

sock=socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
sock.bind(server_address)

mreq=socket.inet_aton(multicast_group)+socket.inet_aton(multicast_if_addr)
sock.setsockopt(socket.IPPROTO_IP, socket.IP_ADD_MEMBERSHIP, mreq)

#make protobuf instance
ssl_wrapper = messages_robocup_ssl_wrapper_pb2.SSL_WrapperPacket()



def loadGeometry():
    br = tf.TransformBroadcaster()
    
    while not rospy.is_shutdown():   
        data    = sock.recv(1024)
        ssl_wrapper.ParseFromString(data)   #Parse sent data
        
        for i in range(0, 3):
            x   = ssl_wrapper.detection.robots_blue[i].x / 1000.0
            y   = ssl_wrapper.detection.robots_blue[i].y / 1000.0
            th  = ssl_wrapper.detection.robots_blue[i].orientation
                 
            br.sendTransform((x, y, 0),
                   tf.transformations.quaternion_from_euler(0, 0, th),
                   rospy.Time.now(),
                   "blue" + str(i),
                   "world")
        
        
        
if  __name__ == '__main__':
    rospy.init_node('grsim_geometry_loader')
    
    try:
        loadGeometry()
    except  rospy.ROSInterruptException:
        rospy.loginfo('Exception occured')
        pass