clearpathrobotics / clearpath_simulator

BSD 3-Clause "New" or "Revised" License
8 stars 6 forks source link

Fix gazebo auto generated topics / frames #37

Open SteveMacenski opened 3 weeks ago

SteveMacenski commented 3 weeks ago

The reason you have odd things like the prefix pointing at the model through the world name is because those are the autogenerated values from the SDF if you do not specify topic names or frame IDs.

For example, I had to copy paste the intel realsense xacro into my package so I could do:


    <gazebo reference="${name}_link">
      <sensor name="${name}" type="rgbd_camera">
        <camera>
          <horizontal_fov>1.25</horizontal_fov>
          <image>
            <width>${image_width}</width>
            <height>${image_height}</height>
          </image>
          <clip>
            <near>0.3</near>
            <far>100</far>
          </clip>
          <ignition_frame_id>${name}_link</ignition_frame_id>  <!-- Steve: mod from _color_optical_frame to make work -->
        </camera>
        <always_on>1</always_on>
        <update_rate>${update_rate}</update_rate>
        <visualize>true</visualize>
        <ignition_frame_id>${name}_link</ignition_frame_id>
        <topic>sensors/camera0</topic>  <!-- Steve mod: added to make topic sane for bridge -->
      </sensor>
    </gazebo>

Note that I set the topic to what the hardware does (or close, I haven't been able to get to hardware validation of any of the topics yet due to ethernet misconfiguration issues on delivery) and I set the frame ID to something that the URDF knows about. Now it shows up as /sensors/camera0/image in the GZ topic list & I don't need to set a TF static transform broadcaster to convert from the URDF standard frames into the autogenerated model frame.

That buys us the ability to use the ros_gz_image image transport bridge:

    camera_bridge_depth = Node(
        package='ros_gz_image',
        executable='image_bridge',
        name='bridge_gz_ros_camera_depth',
        output='screen',
        parameters=[{
            'use_sim_time': True,
        }],
        arguments=['/sensors/camera0/depth_image'])

And pops up on the ROS side as /sensors/camera0/depth_image and so on with the compression options provided by image_transport. I'm having to copy+paste in all my sensors to my package from the clearpath sensor descriptions package so I can make these modifications so that my GZ bridge doesn't depend on a fragile path that includes the specific launched world name with the prefix.


Another example for a 3D lidar:

    <gazebo reference="${name}">
      <sensor name="${name}" type="gpu_lidar">
        <update_rate>${hz}</update_rate>
        <visualize>false</visualize>
        <always_on>true</always_on>
        <ignition_frame_id>${name}</ignition_frame_id> <!-- Steve mod: fix frame to something actually in URDF -->
        <topic>sensors/lidar0</topic> <!-- Steve mod: set topic to same as hardware -->
        <lidar>
          <scan>
            <horizontal>
              <samples>${samples}</samples>
              <resolution>1</resolution>
              <min_angle>${min_angle}</min_angle>
              <max_angle>${max_angle}</max_angle>
            </horizontal>
            <vertical>
              <samples>${lasers}</samples>
              <resolution>1</resolution>
              <min_angle>${vfov_min}</min_angle>
              <max_angle>${vfov_max}</max_angle>
            </vertical>
          </scan>
          <range>
            <min>${min_range}</min>
            <max>${max_range}</max>
            <resolution>0.003</resolution>
          </range>
          <noise>
            <type>gaussian</type>
            <mean>0.0</mean>
            <stddev>0.003</stddev>
          </noise>
        </lidar>
      </sensor>
    </gazebo>

And now my bridge configuration looks standard without the long prefix that depends on the model

# 3D Lidar
- ros_topic_name: "sensors/lidar0/points"
  gz_topic_name: "/sensors/lidar0/points"
  ros_type_name: "sensor_msgs/msg/PointCloud2"
  gz_type_name: "gz.msgs.PointCloudPacked"
  direction: GZ_TO_ROS