About | Support | answers.ros.org	Search:

Attaching a ROS Ground Truth Pose Broadcaster to Objects in Simulation

Description: Attaching a ROS ground truth pose broadcaster to objects in simulation

Tutorial Level:

Attaching a Ground Truth Plugin

1. Update the URDF from this tutorial by adding a Gazebo extension for ros_p3d plugin:

   <?xml version="1.0"?>
   <robot xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller" name="simple_box">
     <gazebo>
       <controller:ros_p3d name="my_box_controller" plugin="libros_p3d.so">
         <alwaysOn>true</alwaysOn>
         <updateRate>100.0</updateRate>
         <bodyName>my_box</bodyName>
         <topicName>my_box_pose_ground_truth</topicName>
         <gaussianNoise>0.0</gaussianNoise>
         <frameName>my_box</frameName>
         <interface:position name="my_box_p3d_position_iface" />
       </controller:ros_p3d>
     </gazebo>
   
     <joint name="my_box_joint" type="revolute" >
       <!-- axis is in the parent link frame coordintates -->
       <axis xyz="0 1 0" />
       <parent link="world" />
       <child link="my_box" />
       <!-- initial pose of my_box joint/link in the parent frame coordiantes -->
       <origin xyz="0 0 2" rpy="0 0 0" />
     </joint>
     <link name="my_box">
       <inertial>
         <mass value="1.0" />
         <!-- center of mass (com) is defined w.r.t. link local coordinate system -->
         <origin xyz="1 0 0" /> 
         <inertia  ixx="1.0" ixy="0.0"  ixz="0.0"  iyy="100.0"  iyz="0.0"  izz="1.0" />
       </inertial>
       <visual>
         <!-- visual origin is defined w.r.t. link local coordinate system -->
         <origin xyz="1 0 0" rpy="0 0 0" />
         <geometry name="my_box_visual_geom">
           <box size="0.05 0.05 0.10" />
         </geometry>
       </visual>
       <collision>
         <!-- collision origin is defined w.r.t. link local coordinate system -->
         <origin xyz="1 0 0" rpy="0 0 0 " />
         <geometry name="my_box_collision_geom">
           <box size="0.05 0.05 0.10" />
         </geometry>
       </collision>
     </link>
     <gazebo reference="my_box">
       <material>Gazebo/Blue</material>
       <turnGravityOff>false</turnGravityOff>
     </gazebo>
   </robot>

The additional Gazebo extension block below:

  <gazebo>
    <controller:ros_p3d name="my_box_controller" plugin="libros_p3d.so">
      <alwaysOn>true</alwaysOn>
      <updateRate>100.0</updateRate>
      <bodyName>my_box</bodyName>
      <topicName>my_box_pose_ground_truth</topicName>
      <gaussianNoise>0.0</gaussianNoise>
      <frameName>my_box</frameName>
      <interface:position name="my_box_p3d_position_iface" />
    </controller:ros_p3d>
  </gazebo>

instantiates a ros_p3d plugin that broadcasts a geometry_msgs/Pose message over ROS topic my_box_pose_ground_truth. To see pose information, run

rostopic echo my_box_pose_ground_truth

after simulation has started.