Robotics StackExchange | Archived questions

Realsense D435 in Gazebo

I am trying to simulate a Realsense D435 in Gazebo 9, running ROS Melodic in Ubuntu 18.04.

I am using the ROS Wrapper from Realsense and the plugin from here.

Importing the _d435.urdf.xacro and _d435.gazebo.xacro files into my own xacro, I should be able to use the camera in Gazebo and see several topics being published. My file is

<?xml version="1.0"?>
<robot name="realsense2_camera" xmlns:xacro="http://ros.org/wiki/xacro">
  <xacro:arg name="use_nominal_extrinsics" default="false"/>
  <xacro:include filename="$(find realsense2_description)/urdf/_d435.urdf.xacro" />
  <xacro:include filename="$(find realsense2_description)/urdf/_d435.gazebo.xacro" />

  <link name="base_link" />
  <xacro:sensor_d435 parent="base_link" use_nominal_extrinsics="$(arg use_nominal_extrinsics)">
    <origin xyz="0 0 0" rpy="0 0 0"/>
  </xacro:sensor_d435>
  <xacro:gazebo_d435 camera_name="camera" reference_link="camera" topics_ns="rs" depth_optical_frame="depth" color_optical_frame="color" infrared1_optical_frame="ired1" infrared2_optical_frame="ired2" >
  </xacro:gazebo_d435>
</robot>

However, running like this causes Gazebo to crash at startup, while loading the plugin (at least it's after some text messages from the plugin). Looking at the _d435.gazebo.xacro file, I changed it slightly to include the gazebo reference when entering the plugin:

<?xml version="1.0"?>

<robot xmlns:xacro="http://ros.org/wiki/xacro">

  <xacro:macro name="gazebo_d435" params="camera_name reference_link topics_ns depth_optical_frame color_optical_frame infrared1_optical_frame infrared2_optical_frame" >

    <!-- Load parameters to model's main link-->
    <xacro:property name="deg_to_rad" value="0.01745329251994329577" />
    <gazebo reference="${reference_link}">
      <self_collide>0</self_collide>
      <enable_wind>0</enable_wind>
      <kinematic>0</kinematic>
      <gravity>1</gravity>
      <!--<mu>1</mu>-->
      <mu2>1</mu2>
      <fdir1>0 0 0</fdir1>
      <!--<slip1>0</slip1>
      <slip2>0</slip2>-->
      <kp>1e+13</kp>
      <kd>1</kd>
      <!--<max_vel>0.01</max_vel>
      <min_depth>0</min_depth>-->
      <sensor name="${camera_name}color" type="camera">
        <camera name="${camera_name}">
          <horizontal_fov>${69.4*deg_to_rad}</horizontal_fov>
          <image>
            <width>1920</width>
            <height>1080</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="${camera_name}ired1" type="camera">
          <camera name="${camera_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>30</update_rate>
        <visualize>0</visualize>
      </sensor>
      <sensor name="${camera_name}ired2" type="camera">
        <camera name="${camera_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>30</update_rate>
        <visualize>0</visualize>
      </sensor>
      <sensor name="${camera_name}depth" type="depth">
        <camera name="${camera_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>30</update_rate>
        <visualize>0</visualize>
      </sensor>
<!--    </gazebo>

    <gazebo reference="${reference_link}"> -->
      <plugin name="${topics_ns}" filename="librealsense_gazebo_plugin.so">
        <prefix>${camera_name}</prefix>
        <depthUpdateRate>30.0</depthUpdateRate>
        <colorUpdateRate>30.0</colorUpdateRate>
        <infraredUpdateRate>30.0</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>${color_optical_frame}</colorOpticalframeName>
        <depthOpticalframeName>${depth_optical_frame}</depthOpticalframeName>
        <infrared1OpticalframeName>${infrared1_optical_frame}</infrared1OpticalframeName>
        <infrared2OpticalframeName>${infrared2_optical_frame}</infrared2OpticalframeName>
        <rangeMinDepth>0.2</rangeMinDepth>
        <rangeMaxDepth>10.0</rangeMaxDepth>
        <pointCloud>false</pointCloud>
        <pointCloudTopicName>depth/points</pointCloudTopicName>
        <pointCloudCutoff>0.5</pointCloudCutoff>
      </plugin>
    </gazebo>

  </xacro:macro>
</robot>

Now Gazebo runs, the camera is visible, but there are no topics being published and no indication that the plugin is being loaded. I have also tried adding the plugin under each tag, with similar results.

What am I doing wrong here? Any tips would be appreciated, any questions will be answered. Thank you!

-- EDIT 1 -- Running Gazebo with debug argument to true resulted in:

[INFO] [1601627990.163632, 0.000000]: Calling service /gazebo/spawn_urdf_model
[INFO] [1601627990.292581, 0.000000]: Spawn status: SpawnModel: Successfully spawned entity
[tcsetpgrp failed in terminal_inferior: Inappropriate ioctl for device]
[ INFO] [1601627990.532084369]: Realsense Gazebo ROS plugin loading.

RealSensePlugin: The realsense_camera plugin is attach to model realsense2_camera

Thread 54 "gzserver" received signal SIGSEGV, Segmentation fault.
[Switching to Thread 0x7fff0a7fb700 (LWP 15179)]
[urdf_spawner-5] process has finished cleanly
log file: /home/cneves/.ros/log/ccfb807c-048a-11eb-80e9-40f02fe63d1c/urdf_spawner-5*.log
0x00007fff40143ff9 in gazebo::RealSensePlugin::Load(boost::shared_ptr<gazebo::physics::Model>, std::shared_ptr<sdf::Element>) ()
   from /home/cneves/catkin_add/devel/lib/librealsense_gazebo_plugin.so

Asked by clusher on 2020-09-30 13:06:55 UTC

Comments

Answers

Please do check the implementation of realsense done by Neobotix for their robots

Asked by pmuthu2s on 2020-10-01 03:18:06 UTC

Comments

Thank you, they are a great resource for simulation.

However, they are using the libgazebo_ros_openni_kinect.so plugin (which is a great alternative), while I was trying to get the realsense plugin to work.

Asked by clusher on 2020-10-01 04:27:48 UTC

realsense_gazebo_plugin is a model plugin. Move it out of

<gazebo reference="${reference_link}">
  ...
</gazebo>

and into

<gazebo>
  ...
</gazebo>

Asked by nlamprian on 2020-10-01 15:17:56 UTC

Comments

That is what I did first, as that is the original implementation in the repository. Gazebo window opens, but hangs when starting the plugin and ends up launching a "segmentation fault".

Asked by clusher on 2020-10-01 16:43:13 UTC

Start gazebo with the debug argument to true and check the backtrace to see where it failed.

Asked by nlamprian on 2020-10-02 02:40:21 UTC

Great suggestion. I added the output to the original question. It doesn't say much, just that a SIGSEV signal was received when running the plugin.

Asked by clusher on 2020-10-02 03:43:51 UTC

Do you find the solution cause i get the same issue

Asked by alexje2k on 2022-07-09 08:59:42 UTC

Comments