Help with realsense gazebo plugin

[Rudresh172]

Hello,

I have been trying to use the Realsense plugin for Gazebo Classic on ROS2 Humble 22.04. I do not have a sdf world file. Instead I am trying to add the gazebo plugin to the original Realsense description URDF file by Intel.

<robot name="sensor_d435" xmlns:xacro="http://ros.org/wiki/xacro">
  <!-- Includes -->
  <xacro:include filename="$(find realsense2_description)/urdf/_materials.urdf.xacro" />
  <xacro:include filename="$(find realsense2_description)/urdf/_usb_plug.urdf.xacro" />

  <xacro:macro name="sensor_d435" params="parent *origin name:=camera use_nominal_extrinsics:=false add_plug:=false use_mesh:=true">
    <xacro:property name="M_PI" value="3.1415926535897931" />

    <!-- The following values are approximate, and the camera node
     publishing TF values with actual calibrated camera extrinsic values -->
    <xacro:property name="d435_cam_depth_to_infra1_offset" value="0.0"/>
    <xacro:property name="d435_cam_depth_to_infra2_offset" value="-0.050"/>
    <xacro:property name="d435_cam_depth_to_color_offset" value="0.015"/>

    <!-- The following values model the aluminum peripherial case for the
    D435 camera, with the camera joint represented by the actual
    peripherial camera tripod mount -->
    <xacro:property name="d435_cam_width" value="0.090"/>
    <xacro:property name="d435_cam_height" value="0.025"/>
    <xacro:property name="d435_cam_depth" value="0.02505"/>
    <xacro:property name="d435_cam_mount_from_center_offset" value="0.0149"/>
    <!-- glass cover is 0.1 mm inwards from front aluminium plate -->
    <xacro:property name="d435_glass_to_front" value="0.1e-3"/>
    <!-- see datasheet Revision 007, Fig. 4-4 page 65 -->
    <xacro:property name="d435_zero_depth_to_glass" value="4.2e-3"/>
    <!-- convenience precomputation to avoid clutter-->
    <xacro:property name="d435_mesh_x_offset" value="${d435_cam_mount_from_center_offset-d435_glass_to_front-d435_zero_depth_to_glass}"/>

    <!-- The following offset is relative the the physical D435 camera peripherial
    camera tripod mount -->
    <xacro:property name="d435_cam_depth_px" value="${d435_cam_mount_from_center_offset}"/>
    <xacro:property name="d435_cam_depth_py" value="0.0175"/>
    <xacro:property name="d435_cam_depth_pz" value="${d435_cam_height/2}"/>

    <!-- camera body, with origin at bottom screw mount -->
    <joint name="${name}_joint" type="fixed">
      <xacro:insert_block name="origin" />
      <parent link="${parent}"/>
      <child link="${name}_bottom_screw_frame" />
    </joint>
    <link name="${name}_bottom_screw_frame"/>

    <joint name="${name}_link_joint" type="fixed">
      <origin xyz="${d435_mesh_x_offset} ${d435_cam_depth_py} ${d435_cam_depth_pz}" rpy="0 0 0"/>
      <parent link="${name}_bottom_screw_frame"/>
      <child link="${name}_link" />
    </joint>

    <link name="${name}_link">
      <visual>
        <xacro:if value="${use_mesh}">
          <!-- the mesh origin is at front plate in between the two infrared camera axes -->
          <origin xyz="${d435_zero_depth_to_glass + d435_glass_to_front} ${-d435_cam_depth_py} 0" rpy="${M_PI/2} 0 ${M_PI/2}"/>
          <geometry>
            <mesh filename="file://$(find realsense2_description)/meshes/d435.dae" />
          </geometry>
        </xacro:if>
        <xacro:unless value="${use_mesh}">
          <origin xyz="0 ${-d435_cam_depth_py} 0" rpy="0 0 0"/>
          <geometry>
            <box size="${d435_cam_depth} ${d435_cam_width} ${d435_cam_height}"/>
          </geometry>
          <material name="aluminum"/>
        </xacro:unless>     
      </visual>
      <collision>
        <origin xyz="0 ${-d435_cam_depth_py} 0" rpy="0 0 0"/>
        <geometry>
          <box size="${d435_cam_depth} ${d435_cam_width} ${d435_cam_height}"/>
        </geometry>
      </collision>
      <inertial>
        <!-- The following are not reliable values, and should not be used for modeling -->
        <mass value="0.072" /> <!-- 0.072-->
        <origin xyz="0 0 0" />
        <inertia ixx="0.000003881243" ixy="0.0" ixz="0.0" iyy="0.000498940" iyz="0.0" izz="0.000003879257" />
      </inertial>
    </link>

    <!-- Use the nominal extrinsics between camera frames if the calibrated extrinsics aren't being published. e.g. running the device in simulation  -->
    <xacro:if value="${use_nominal_extrinsics}">
      <!-- camera depth joints and links -->
      <joint name="${name}_depth_joint" type="fixed">
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <parent link="${name}_link"/>
        <child link="${name}_depth_frame" />
      </joint>
      <link name="${name}_depth_frame"/>

      <joint name="${name}_depth_optical_joint" type="fixed">
        <origin xyz="0 0 0" rpy="${-M_PI/2} 0 ${-M_PI/2}" />
        <parent link="${name}_depth_frame" />
        <child link="${name}_depth_optical_frame" />
      </joint>
      <link name="${name}_depth_optical_frame"/>

      <!-- camera left IR joints and links -->
      <joint name="${name}_infra1_joint" type="fixed">
        <origin xyz="0 ${d435_cam_depth_to_infra1_offset} 0" rpy="0 0 0" />
        <parent link="${name}_link" />
        <child link="${name}_infra1_frame" />
      </joint>
      <link name="${name}_infra1_frame"/>

      <joint name="${name}_infra1_optical_joint" type="fixed">
        <origin xyz="0 0 0" rpy="${-M_PI/2} 0 ${-M_PI/2}" />
        <parent link="${name}_infra1_frame" />
        <child link="${name}_infra1_optical_frame" />
      </joint>
      <link name="${name}_infra1_optical_frame"/>

      <!-- camera right IR joints and links -->
      <joint name="${name}_infra2_joint" type="fixed">
        <origin xyz="0 ${d435_cam_depth_to_infra2_offset} 0" rpy="0 0 0" />
        <parent link="${name}_link" />
        <child link="${name}_infra2_frame" />
      </joint>
      <link name="${name}_infra2_frame"/>

      <joint name="${name}_infra2_optical_joint" type="fixed">
        <origin xyz="0 0 0" rpy="${-M_PI/2} 0 ${-M_PI/2}" />
        <parent link="${name}_infra2_frame" />
        <child link="${name}_infra2_optical_frame" />
      </joint>
      <link name="${name}_infra2_optical_frame"/>

      <!-- camera color joints and links -->
      <joint name="${name}_color_joint" type="fixed">
        <origin xyz="0 ${d435_cam_depth_to_color_offset} 0" rpy="0 0 0" />
        <parent link="${name}_link" />
        <child link="${name}_color_frame" />
      </joint>
      <link name="${name}_color_frame"/>

      <joint name="${name}_color_optical_joint" type="fixed">
        <origin xyz="0 0 0" rpy="${-M_PI/2} 0 ${-M_PI/2}" />
        <parent link="${name}_color_frame" />
        <child link="${name}_color_optical_frame" />
      </joint>
      <link name="${name}_color_optical_frame"/>
    </xacro:if>

    <xacro:if value="${add_plug}">
      <xacro:usb_plug parent="${name}_link" name="${name}_usb_plug">
        <origin xyz="${d435_cam_mount_from_center_offset - 0.02095} ${-d435_cam_depth_py - 0.0353} 0" rpy="0 0 0"/>
      </xacro:usb_plug>
    </xacro:if>

    <!-- gazebo plugin -->

    <xacro:property name="deg_to_rad" value="0.01745329251994329577" />
    <xacro:property name="sensor_name" value="d435" />
    <xacro:property name="rate" value="10" />

    <gazebo reference="${sensor_name}_link">
      <self_collide>0</self_collide>
      <enable_wind>0</enable_wind>
      <kinematic>0</kinematic>
      <gravity>1</gravity>
      <mu2>1</mu2>
      <fdir1>0 0 0</fdir1>
      <kp>1e+13</kp>
      <kd>1</kd>

      <sensor name="${sensor_name}_color" type="camera">
        <camera name="${sensor_name}">
          <horizontal_fov>${69.4*deg_to_rad}</horizontal_fov>
          <image>
            <width>640</width>
            <height>480</height>
            <format>RGB_INT8</format>
          </image>
          <clip>
            <near>0.1</near>
            <far>100</far>
          </clip>
          <noise>
            <type>gaussian</type>
            <mean>0.0</mean>
            <stddev>0.007</stddev>
          </noise>
        </camera>
        <always_on>1</always_on>
        <update_rate>30</update_rate>
        <visualize>1</visualize>
      </sensor>
      <sensor name="${sensor_name}_ired1" type="camera">
        <camera name="${sensor_name}">
          <horizontal_fov>${85.2*deg_to_rad}</horizontal_fov>
          <image>
            <width>1280</width>
            <height>720</height>
            <format>L_INT8</format>
          </image>
          <clip>
            <near>0.1</near>
            <far>100</far>
          </clip>
          <noise>
            <type>gaussian</type>
            <mean>0.0</mean>
            <stddev>0.05</stddev>
          </noise>
        </camera>
        <always_on>1</always_on>
        <update_rate>90</update_rate>
        <visualize>0</visualize>
      </sensor>
      <sensor name="${sensor_name}_ired2" type="camera">
        <camera name="${sensor_name}">
          <horizontal_fov>${85.2*deg_to_rad}</horizontal_fov>
          <image>
            <width>1280</width>
            <height>720</height>
            <format>L_INT8</format>
          </image>
          <clip>
            <near>0.1</near>
            <far>100</far>
          </clip>
          <noise>
            <type>gaussian</type>
            <mean>0.0</mean>
            <stddev>0.05</stddev>
          </noise>
        </camera>
        <always_on>1</always_on>
        <update_rate>90</update_rate>
        <visualize>0</visualize>
      </sensor>
      <sensor name="${sensor_name}_depth" type="depth">
        <camera name="${sensor_name}">
          <horizontal_fov>${85.2*deg_to_rad}</horizontal_fov>
          <image>
            <width>1280</width>
            <height>720</height>
          </image>
          <clip>
            <near>0.1</near>
            <far>100</far>
          </clip>
          <noise>
            <type>gaussian</type>
            <mean>0.0</mean>
            <stddev>0.100</stddev>
          </noise>
        </camera>
        <always_on>1</always_on>
        <update_rate>90</update_rate>
        <visualize>0</visualize>
      </sensor>
    </gazebo>

    <gazebo>
      <plugin name="${sensor_name}" filename="librealsense_gazebo_plugin.so">
        <prefix>${sensor_name}_</prefix>
        <depthUpdateRate>${rate}</depthUpdateRate>
        <colorUpdateRate>${rate}</colorUpdateRate>
        <infraredUpdateRate>${rate}</infraredUpdateRate>
        <depthTopicName>depth/image_raw</depthTopicName>
        <depthCameraInfoTopicName>depth/camera_info</depthCameraInfoTopicName>
        <colorTopicName>color/image_raw</colorTopicName>
        <colorCameraInfoTopicName>color/camera_info</colorCameraInfoTopicName>
        <infrared1TopicName>infra1/image_raw</infrared1TopicName>
        <infrared1CameraInfoTopicName>infra1/camera_info</infrared1CameraInfoTopicName>
        <infrared2TopicName>infra2/image_raw</infrared2TopicName>
        <infrared2CameraInfoTopicName>infra2/camera_info</infrared2CameraInfoTopicName>
        <colorOpticalframeName>${sensor_name}_color_optical_frame</colorOpticalframeName>
        <depthOpticalframeName>${sensor_name}_depth_optical_frame</depthOpticalframeName>
        <infrared1OpticalframeName>${sensor_name}_infrared1_optical_frame</infrared1OpticalframeName>
        <infrared2OpticalframeName>${sensor_name}_infrared2_optical_frame</infrared2OpticalframeName>
        <rangeMinDepth>0.2</rangeMinDepth>
        <rangeMaxDepth>10.0</rangeMaxDepth>
        <pointCloud>false</pointCloud>
        <pointCloudTopicName>depth/color/points</pointCloudTopicName>
        <pointCloudCutoff>0.25</pointCloudCutoff>
        <pointCloudCutoffMax>9.0</pointCloudCutoffMax>
      </plugin>
    </gazebo>
  </xacro:macro>
</robot>

But when I include this to my robot URDF file and try to run Gazebo, the whole robot fails to spawn and throws this error -

[ERROR] [gzserver-3]: process has died [pid 88660, exit code 255, cmd 'gzserver /home/rudresh/Downloads/relffok_mir_30.1/install/mir_gazebo/share/mir_gazebo/worlds/maze.world -slibgazebo_ros_init.so -slibgazebo_ros_factory.so -slibgazebo_ros_force_system.so'].

I also noticed that I do not have the librealsense_gazebo_plugin.so in either of my /opt/ros/humble/lib or usr/lib/x86_64-linux-gnu/gazebo-11/plugins folders

So do I have to make any changes to package.xml or CMakeLists.txt to add that? Sorry I"m new to ROS!

Thanks!

[yuan-0816]

you can see this page this is pre-build realsense plugin package, clone into your src folder, and run colcon build

and you can create your own robot description by using xacro, example here

if you want to use realsense plugin, you need this two file: urdf and gazebo_plugin you just use include into your xacro file. like this

Also you can see this https://github.com/koichirokato/gazebo_sensor_ros2

[Rudresh172]

Hello,

Apparantly, I did not setup the plugin properly. Your instructions helped and it works now.

Thanks for your prompt response!

[Rudresh172]

@yuan-0816 Also, do you know any open source grippers which can be simulated in Gazebo Classic and controlled by MoveIt2?

[yuan-0816]

I'm glad you succeeded. I'm not very familiar with MoveIt 2. I suggest you can inquire about it on their GitHub. https://moveit.picknik.ai/main/index.html

[Rudresh172]

Okay thanks!