Writing tf messages to bag using rosbag Python API

asked 2020-01-29 11:08:27 -0500

marcopaah
3 ●1 ●1 ●2

I have some simulated images and camera poses, which I would like to write to a bag using the Python rosbag API. I have successfully written the images and I am now trying to write the poses to the /tf topic. I can write the messages, but rosbag play and rviz doesn't recognize the frames in the tf tree.

These are my imports:

from std_msgs.msg import Header
from geometry_msgs.msg import TransformStamped
from tf2_msgs.msg import TFMessage

I use the following function to generate the message:

def to_pose_stamped(cam, header, frame_id, child_frame_id):
    camera_msg_config = TFMessage()
    camera_msg = TransformStamped()
    camera_msg_config.transforms = []
    camera_msg.header = header
    camera_msg.header.seq = 0
    camera_msg.header.frame_id = frame_id
    camera_msg.child_frame_id = child_frame_id
    camera_msg.transform.translation.x = cam.tx
    camera_msg.transform.translation.y = cam.ty
    camera_msg.transform.translation.z = cam.tx
    camera_msg.transform.rotation.x = cam.rx
    camera_msg.transform.rotation.y = cam.ry
    camera_msg.transform.rotation.z = cam.rz
    camera_msg.transform.rotation.w = 1
    camera_msg_config.transforms.append(camera_msg)
    return camera_msg_config

And I then write to the bag:

camera_pose = to_pose_stamped(obs.camera, std_header, "center_scene", "camera")
bag.write(
    '/tf', camera_pose, camera_pose.transforms[0].header.stamp
)

When I launch this in RViz I have the following in my launch file:

  <node name="static_transform_publisher" pkg="tf" type="static_transform_publisher" args="1 1 1 0 0 0 1  map center_scene 10"/>

Which populates the static transform between the map and center_scene.

Now RViz won't recognize these transforms.

If I inspect the bag:

path:        src/test.bag
version:     2.0
duration:    16.0s
start:       Jan 29 2020 17:50:26.11 (1580316626.11)
end:         Jan 29 2020 17:50:42.07 (1580316642.07)
size:        303.2 MB
messages:    606
compression: none [404/404 chunks]
types:       sensor_msgs/Image  [060021388200f6f0f447d0fcd9c64743]
             tf2_msgs/TFMessage [94810edda583a504dfda3829e70d7eec]
topics:      /stereo/left/image_rect_color    202 msgs    : sensor_msgs/Image 
             /stereo/right/image_rect_color   202 msgs    : sensor_msgs/Image 
             /tf                              202 msgs    : tf2_msgs/TFMessage

If I play the bag using rosbag CLI and echos the /tf topic I get:

WARNING: no messages received and simulated time is active.
Is /clock being published?
transforms: 
  - 
    header: 
      seq: 0
      stamp: 
        secs: 1580316626
        nsecs: 113162040
      frame_id: "center_scene"
    child_frame_id: "camera"
    transform: 
      translation: 
        x: 5.971
        y: 47.849
        z: 5.971
      rotation: 
        x: -1.3
        y: 15.6591
        z: -6.38888
        w: 1.0
---
transforms: 
  - 
    header: 
      seq: 0
      stamp: 
        secs: 1580316626
        nsecs: 113162040
      frame_id: "center_scene"
    child_frame_id: "projector"
    transform: 
      translation: 
        x: 23.801
        y: 83.23601
        z: 23.801
      rotation: 
        x: 35.03401
        y: 5.03358
        z: 9.73858
        w: 1.0
---

ie. the transforms are only published once and then nothing happens.

Any suggestions to how to fix this is much appreciated.

edit retag flag offensive close merge delete

add a comment

Comments

Thank you. My assumption was that the actual values didn't really matter and I would convert them to quaternions once I got it to publish the pose.

I have made a workaround where I write a StampedPose to the bag and during the launch and playback of the file, I listen to the StampedPose topic and publish it on the tf tree. That works.

marcopaah ( 2020-01-29 16:01:34 -0500 )edit

That should not be necessary. And it's a rather nasty workaround actually.