Skip to content

State of tf publishing from gazebo -> ros2 #848

@mamueluth

Description

@mamueluth

Goal: Get complete transformation/pose of an dynamically added model from gazebo to ros2 including frame_id, position and orientation.
Problem: No complete Pose/Transform msg in ros2:

  • Either frame_id field is emtpy
  • Or position and orientation are set to default (0,0,0) and (0,0,0,1)

I found the following related issues:
#172 , #410

Question:
What is the state of this?
Does the issue of missing headers/default pose indeed still persists or do i just mix things up and there is a way how to achieve this. I would be thankful for a clarification on how to get the full pose/transformation information for a dynamically added model out of gazebo and publish into ros2.

What i tried:
Description: Ubuntu 24.04.4 LTS
Release: 24.04
Codename: noble
Gazebo: Ionic version 9.5
ROS2: Kilted

I create an empty world. I add a cube like this to the empty world:

ros2 service call /gz_server/spawn_entity simulation_interfaces/srv/SpawnEntity "{
  name: 'cube_5cm',
  allow_renaming: true,
  uri: '',
  resource_string: '<sdf version=\"1.9\">
<model name=\"cube_5cm\">
  <static>false</static>
  <link name=\"link\">
    <inertial>
      <mass>0.2</mass>
      <inertia>
        <ixx>0.0000833</ixx>
        <iyy>0.0000833</iyy>
        <izz>0.0000833</izz>
        <ixy>0</ixy>
        <ixz>0</ixz>
        <iyz>0</iyz>
      </inertia>
    </inertial>

    <collision name=\"collision\">
      <geometry>
        <box>
          <size>0.05 0.05 0.05</size>
        </box>
      </geometry>
    </collision>

    <visual name=\"visual\">
      <geometry>
        <box>
          <size>0.05 0.05 0.05</size>
        </box>
      </geometry>
    </visual>
  </link>

  <plugin filename=\"gz-sim-pose-publisher-system\" name=\"gz::sim::systems::PosePublisher\">
    <publish_link_pose>true</publish_link_pose>
    <publish_collision_pose>true</publish_collision_pose>
    <publish_visual_pose>true</publish_visual_pose>
    <publish_nested_model_pose>true</publish_nested_model_pose>
  </plugin>
</model>
</sdf>',
  entity_namespace: '',
  initial_pose: {
    header: { frame_id: 'world' },
    pose: {
      position: { x: 1.5, y: 1.8, z: 1.5 },
      orientation: { x: 0.0, y: 0.0, z: 0.0, w: 1.0 }
    }
  }
}"

If i check /world/empty/pose/info or /world/empty/dynamic_pose/info the object is there:

pose {
  name: "cube_5cm"
  id: 77
  position {
    x: 1.5
    y: 1.8
    z: 0.827499882536041
  }
  orientation {
    x: -1.0207687115592842e-17
    y: -2.4794898127535062e-18
    z: -8.6042680501730887e-19
    w: 1
  }
}

1. Bridging /world/empty/pose/info or /world/empty/dynamic_pose/info

- ros_topic_name: "/gazebo/world/dynamic_pose"
  gz_topic_name: "/world/empty/dynamic_pose/info"
  ros_type_name: "tf2_msgs/msg/TFMessage"
  gz_type_name: "gz.msgs.Pose_V"
  direction: GZ_TO_ROS
  lazy: true

- ros_topic_name: "/gazebo/world/pose"
  gz_topic_name: "/world/empty/pose/info"
  ros_type_name: "tf2_msgs/msg/TFMessage"
  gz_type_name: "gz.msgs.Pose_V"
  direction: GZ_TO_ROS
  lazy: true

yields the correct pose but no frame_id:

- header:
    stamp:
      sec: 0
      nanosec: 0
    frame_id: ''
  child_frame_id: ''
  transform:
    translation:
      x: 1.5
      y: 1.8
      z: 0.827499882536041
    rotation:
      x: -1.0207687115592842e-17
      y: -2.479489812753506e-18
      z: -8.604268050173089e-19
      w: 1.0

2. Using gz-sim-pose-publisher-system
create /cube_5cm/pose/ topic on gz side:
Messages for:

    <publish_link_pose>true</publish_link_pose>
    <publish_collision_pose>true</publish_collision_pose>
    <publish_visual_pose>true</publish_visual_pose>

are empty:

header {
  stamp {
    sec: 1482
    nsec: 973000000
  }
  data {
    key: "frame_id"
    value: "cube_5cm::link"
  }
  data {
    key: "child_frame_id"
    value: "cube_5cm::link::collision"
  }
}
name: "cube_5cm::link::collision"
position {
}
orientation {
  w: 1
}

header {
  stamp {
    sec: 1482
    nsec: 973000000
  }
  data {
    key: "frame_id"
    value: "cube_5cm::link"
  }
  data {
    key: "child_frame_id"
    value: "cube_5cm::link::visual"
  }
}
name: "cube_5cm::link::visual"
position {
}
orientation {
  w: 1
}

But for <publish_nested_model_pose>true</publish_nested_model_pose> the correct pose is published:

name: "cube_5cm"
position {
  x: 1.5
  y: 1.8
  z: 0.827499882536041
}
orientation {
  x: -1.0207687115592842e-17
  y: -2.4794898127535062e-18
  z: -8.6042680501730887e-19
  w: 1
}

Checking the ros2 side however: either the frame_id is empty or pose is set to default:

---
header:
  stamp:
    sec: 1664
    nanosec: 95000000
  frame_id: empty
pose:
  position:
    x: 1.5
    y: 1.8
    z: 0.827499882536041
  orientation:
    x: -1.0207687115592842e-17
    y: -2.479489812753506e-18
    z: -8.604268050173089e-19
    w: 1.0
---
header:
  stamp:
    sec: 1664
    nanosec: 96000000
  frame_id: cube_5cm/link
pose:
  position:
    x: 0.0
    y: 0.0
    z: 0.0
  orientation:
    x: 0.0
    y: 0.0
    z: 0.0
    w: 1.0
---

Metadata

Metadata

Assignees

No one assigned

    Labels

    No labels
    No labels

    Type

    No type

    Projects

    Status

    Inbox

    Milestone

    No milestone

    Relationships

    None yet

    Development

    No branches or pull requests

    Issue actions