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
    No fields configured for issues without a type.

    Projects

    Status
    Inbox

    Milestone

    No milestone

    Relationships

    None yet

    Development

    No branches or pull requests

    Issue actions