Pose_V to TFMessage conversion lacks the headers

[vatanaksoytezer]

Environment

• OS Version: Ubuntu 20.04
• Source or binary build? Edifice - Binary ros_ign - Latest Source

Description

• Expected behavior: TFMessage with child_frame_id and frame_id
• Actual behavior: child_frame_id and frame_id values are empty in TFMessage

Steps to reproduce

1. Spawn any world in Ignition and add at least 1 object to it. For eg: ign gazebo and add a sphere to the world via UI.
2. Convert /world/default/dynamic_pose/info or /world/default/pose/info topic to TFMessage via bridge: ros2 run ros_ign_bridge parameter_bridge /world/default/dynamic_pose/info@tf2_msgs/msg/TFMessage[ignition.msgs.Pose_V
3. Listen to the message from ROS2 ros2 topic echo /world/default/dynamic_pose/info

[vatanaksoytezer]

For context I am trying to expose ground truth poses of some of the models in Ignition through parameter bridge, but the error can be minimally reproduced as stated above. Any insights would be greatly appreciated @chapulina @mjcarroll

[chapulina]

Based on this piece of code, I'd expect those fields to be there:

https://github.com/ignitionrobotics/ros_ign/blob/540b9a21bffed1f2ab6216c6c5aa6d486b97c2d6/ros_ign_bridge/src/convert.cpp#L350-L363

I think what's probably happening is that those fields are not populated by ign-gazebo into the Ignition message by default. Have you tried using the PoisePublisher system?

[vatanaksoytezer]

I think what's probably happening is that those fields are not populated by ign-gazebo into the Ignition message by default. Have you tried using the PoisePublisher system?

Yep that's what I thought but ign_msg seems to be missing these fields (at least with my minimal debugging with some prints). I'll give pose publisher a go and update here, thanks for letting me know!

[blakermchale]

@vatanaksoytezer I had tried PosePublisher and only got it to publish the child links of a model, not the model itself. Did you find any different results?

[vatanaksoytezer]

@vatanaksoytezer I had tried PosePublisher and only got it to publish the child links of a model, not the model itself. Did you find any different results?

Same here, but there is OdometryPublisher plugin that seems to do what I want, but I haven't tried it yet. I will test it tomorrow and update here.

[vatanaksoytezer]

Same here, but there is OdometryPublisher plugin that seems to do what I want, but I haven't tried it yet. I will test it tomorrow and update here.

Forgot to update, but OdometryPublisher is super nice and does the job for me.

[chapulina]

Thanks for the update, @vatanaksoytezer , glad that worked. I'll close this for now because the headers should be there, it just depends on which Gazebo plugin you're using.

[Kakcalu13]

This new feature exists in Fortress+.

Citadel has /world/free_world/pose/infoonly, so is there anything alternate to it?

[Kakcalu13]

For anyone in future search the same thing using Citadel only:

I end up using this in python:

    pose = subprocess.Popen(["ign topic -e  -t world/free_world/pose/info -n1 | grep \"freenove_smart_car\" -A6"], shell=True, stdout=PIPE)
    pose_xyz = pose.communicate()[0].decode("utf-8")

[horverno]

Same error on Ignition Fortress, Ubuntu 22.04 and ROS 2 Humble.

My command is:

ros2 run ros_gz_bridge parameter_bridge /world/ackermann_steering/pose/info@tf2_msgs/msg/TFMessage[ignition.msgs.Pose_V  --ros-args -r /world/ackermann_steering/pose/info:=/tf

ign topic seems ok:

ign topic -et /world/ackermann_steering/pose/info

pose {
  name: "vehicle_blue"
  id: 8
  position {
    x: -1.6627091282644963
    y: 1.8613297571115033
    z: 0.32500721567664304
  }
  orientation {
    x: -3.4769474752805716e-10
    y: 1.0450316315873644e-09
    z: 0.016641913865699461
    w: 0.99986151376222432
  }
}

But in ROS 2, the frames are missing:

- header:
    stamp:
      sec: 0
      nanosec: 0
    frame_id: ''
  child_frame_id: ''
  transform:
    translation:
      x: -1.6627091282644963
      y: 1.8613297571115033
      z: 0.32500721567664304
    rotation:
      x: -3.4769474752805716e-10
      y: 1.0450316315873644e-09
      z: 0.01664191386569946
      w: 0.9998615137622243

Any suggestions are appreciated, thanks.

[slim71]

Just a remark for whoever stumbles on this and doesn't immediately see it... as I was doing while struggling to get this right! :sweatsmile: You need to add the PosePublisher/OdometryPublisher plugin to the <model> tag of the .sdf_ file, being that a world file or only an "object" file

<?xml version="1.0"?>
<sdf version="1.6">
    <model name="quadcopter">
        <pose>0 0 0 0 0 0</pose>

        <link name="base">
            <pose frame="">0 0 0 0 0 0</pose>
            <!-- ......... -->
        </link>

        <!-- ......... -->

        <plugin
            filename="gz-sim-pose-publisher-system"
            name="gz::sim::systems::PosePublisher"> <!-- or the odometry plugin -->
            <publish_link_pose>true</publish_link_pose>
            <publish_collision_pose>false</publish_collision_pose>
            <publish_visual_pose>false</publish_visual_pose>
            <publish_nested_model_pose>false</publish_nested_model_pose>
        </plugin>

    </model>
</sdf>

This will enable the related topic that you can then bridge to ROS2 via ros_gz_bridge.

[ns9]

@slim71 @azeey I have the PosePublisher added in the way you show (inside the model tag of the world sdf) and I just see the relative poses of each child frame of the model and not the models world pose. Is there something more I need to do? Is there a minimal example I can run where you expect to see world pose of the model?

[slim71]

@ns9 I later found out that in the PosePublisher doc that it publishes the transform of child entities:

Pose publisher system. Attach to an entity to publish the transform of its child entities

If you need the world pose, use the OdometryPublisher plugin, more or less in the same way (parameters change). I'm using that and it works fine, you just have to take into account which reference frame you need.

Odometry Publisher which can be attached to any entity in order to periodically publish 2D or 3D odometry

(Note that I've linked the docs of the version I'm using, but I don't think there's much difference apart from the naming)

[Rendok]

Hi @slim71, thanks for posting the doc. Could you explain how it solves the problem, though? From the doc, I see that the PosePublisher publishes gz::msgs::Pose_V messages, which still need bridging to ROS. For example, if I want to plot odometry in RViz.

[slim71]

@Rendok well, here we're talking about Gazebo stuff, the bridge to ROS2 is a given, I'd say... If you want stuff for Rviz, you may consider changing perspective and looking directly at docs or examples handling that

[Rendok]

@slim71, let me disagree; here we are talking about converting Pose_V to TFMessage, and Rviz is one of the examples where it may be needed.

[ns9]

@slim71 @azeey Perhaps I’m missing something but is there a way to get “true” world pose from the odometry publisher? For example, if the model doesn’t start at the origin I would like the odometry message to capture that, but it always starts at position (0,0,0) and I have to account for the offset manually by knowing where the model spawned which is less than ideal.

Seems like knowing true world frame position and orientation of each model in a sim environment should be a pretty straightforward feature to me so I feel like I must be missing something obvious.

[slim71]

@ns9 I agree, I too had to account for the offset in another way... Regarding the starting position at (0,0,0), I actually have opened a post in the forum to get some answers, but alas nothing major so far

[azeey]

I think the Odometry system is behaving as intended. I would expect anything called Odometry to have an output pose relative to some starting pose. I think the PosePublisher is probably the best system to use to get ground truth. The key parameter you want set to "true" is publish_nested_model_pose. https://github.com/gazebosim/gz-sim/blob/633ce72171e27e83bf2e0292c9998e036d5da3fc/src/systems/pose_publisher/PosePublisher.hh#L46-L49

[ns9]

@azeey I totally agree with you regarding the odometry system, I was confused why that was suggested as a source of truth in the first place. Any chance you could provide some insight regarding my original question, I feel like I'm missing something very obvious?

@slim71 @azeey I have the PosePublisher added in the way you show (inside the model tag of the world sdf) and I just see the relative poses of each child frame of the model and not the models world pose. Is there something more I need to do? Is there a minimal example I can run where you expect to see world pose of the model?

[azeey]

Here's an example for rolling_shapes.sdf:

1. Add PosePublisher plugin:

     <plugin
       filename="gz-sim-pose-publisher-system"
       name="gz::sim::systems::PosePublisher">
       <use_pose_vector_msg>true</use_pose_vector_msg>
       <publish_nested_model_pose>true</publish_nested_model_pose>
     </plugin>

2. Create tf_bridge.yaml:

   - ros_topic_name: "/tf"
   gz_topic_name: "model/sphere/pose"
   ros_type_name: "tf2_msgs/msg/TFMessage"
   gz_type_name: "gz.msgs.Pose_V"
   direction: GZ_TO_ROS

3. Start gazebo and bridge:

gz sim examples/worlds/rolling_shapes.sdf # Use `ign gazebo examples/worlds/rolling_shapes.sdf ` on Fortress

ros2 run ros_gz_bridge parameter_bridge --ros-args -p config_file:=examples/worlds/tf_bridge.yaml  # or wherever tf_bridge.yaml is

[ns9]

@azeey Thanks that example was helpful!

Just to confirm the only way to bridge the pose to ROS at the moment is using TF messages? Seems like trying to bridge the model poses as a ROS pose message causes the frame id's to be dropped on the ROS side?

[azeey]

Just to confirm the only way to bridge the pose to ROS at the moment is using TF messages? Seems like trying to bridge the model poses as a ROS pose message causes the frame id's to be dropped on the ROS side?

You have a few options that still preserve the frame id:

1. Set <use_pose_vector_msg>true</use_pose_vector_msg> in PosePublisher and bridge to tf2_msgs/msg/TFMessage
2. Set <use_pose_vector_msg>false</use_pose_vector_msg> and bridge to geometry_msgs/msg/PoseStamped or
3. Set <use_pose_vector_msg>false</use_pose_vector_msg> and bridge to geometry_msgs/msg/TransformStamped

The main point is that you have to use the PosePublisher and not rely on the output of /world/default/pose/info. That topic is not meant to be bridged.

[kalhansb]

I want to get the pose info of all the models in gazebo into ROS2. It would have been great if there was a way to bridge /world/default/pose/info. Any ideas on how to get the pose info from all models at once similar to this topic?