-
Notifications
You must be signed in to change notification settings - Fork 209
Description
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_idfield is emtpy - Or
positionandorientationare 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
Labels
Type
Projects
Status