drone & d435 issue

[Samim-17]

Hello! For a quite a few days I'm facing issue of using the rtab map drone simulations example with simple modification of using d435 camera provided by PAL robotics. I added the camera to the drone. Below is the sensor added to urdf file for the drone

<!-- Import macro for realsense-RS200 camera-->
   <xacro:include filename="$(find rtabmap_drone_example)/urdf/_d435.urdf.xacro"/> 
   <!-- <xacro:include filename="$(find rtabmap_drone_example)/urdf/realsense-RS200.macro.xacro"/> -->
  <joint name="base_camera_joint" type="fixed">
    <origin xyz="${scale*0.1} 0 0" rpy="0 0 0" /> 
    <parent link="base_link"/>
    <child link="base_camera" />
  </joint>

  <link name='base_camera'/>

  <!-- Create camera instance -->
     <xacro:sensor_d435 prefix="" parent="base_camera">
        <origin xyz="0 0 0" rpy="0 0 0" />   
  </xacro:sensor_d435> 

Now, I edited the slam.launch file to integrate the the camera topics which I'm publishing. Here is the code below

<?xml version="1.0"?>
<launch>
   
    <arg name="localization" default="false"/>
    <arg name="rtabmap_viz"   default="true"/>
    <arg name="ground_truth" default="false"/>
    
    <arg     if="$(arg localization)" name="pre_args" default=""/>
    <arg unless="$(arg localization)" name="pre_args" default="-d"/>
   
    <node pkg="nodelet" type="nodelet" name="imu_to_tf" args="standalone rtabmap_util/imu_to_tf">
      <remap from="imu/data" to="/mavros/imu/data"/>
      <param name="fixed_frame_id" value="base_link_stabilized"/>
      <param name="base_frame_id" value="base_link"/>
    </node>

    <!-- To connect rtabmap planning stuff with move_base below -->
    <param name="/rtabmap/rtabmap/use_action_for_goal" value="true"/>
    <remap from="/rtabmap/move_base" to="/move_base"/>

    <!-- VSLAM -->
    <param name="/rtabmap/rtabmap/latch" value="false"/> <!-- For some reason, if we latch grid_map, the global costmap inflation layer will create holes on robot path. To avoid holes, republish grid_map on each update (latching disabled). -->
    <include file="$(find rtabmap_launch)/launch/rtabmap.launch">
      <arg name="localization"      value="$(arg localization)"/>
      <arg name="args"              value="$(arg pre_args) --Optimizer/GravitySigma 0.1 --Vis/FeatureType 10 --Kp/DetectorStrategy 10 --Grid/MapFrameProjection true --NormalsSegmentation false --Grid/MaxGroundHeight 1 --Grid/MaxObstacleHeight 1.6 --RGBD/StartAtOrigin true" />
      <arg name="rtabmap_viz"        value="$(arg rtabmap_viz)" />
      <arg name="frame_id"          value="base_link" />
      <arg name="odom_guess_frame_id" value="base_link_stabilized" />
      <arg name="rgb_topic"         value="/camera/camera/color/image_raw" />
      <arg name="depth_topic"       value="/camera/camera/depth/image_raw" />
      <arg name="camera_info_topic" value="/camera/camera/color/camera_info" /> 

      <arg name="imu_topic"         value="/mavros/imu/data"/>
      <arg name="wait_imu_to_init"  value="true"/>
      <arg name="approx_sync"       value="true"/>
      <arg if="$(arg ground_truth)" name="ground_truth_frame_id" value="world"/>
      <arg if="$(arg ground_truth)" name="ground_truth_base_frame_id" value="base_link_gt"/>
    </include>
    
    <!-- Costmap -->
    <node pkg="nodelet" type="nodelet" name="camera_points_xyz" args="standalone rtabmap_util/point_cloud_xyz">
      <remap from="depth/image"       to="/camera/camera/depth/image_raw"/>
      <remap from="depth/camera_info" to="/camera/camera/depth/camera_info"/>
      <remap from="cloud"             to="camera_cloud" />

      <param name="decimation"  type="double" value="4"/>
      <param name="voxel_size"  type="double" value="0.0"/>
      <param name="approx_sync" type="bool"   value="true"/>
    </node> 

    <!-- navigation -->  
    <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
       <remap from="map" to="/rtabmap/grid_map"/>
       <remap from="odom" to="/rtabmap/odom"/>
       <param name="base_local_planner" value="dwa_local_planner/DWAPlannerROS" />  
       <rosparam file="$(find rtabmap_drone_example)/param/costmap_common_params.yaml" command="load" ns="global_costmap" />
       <rosparam file="$(find rtabmap_drone_example)/param/costmap_common_params.yaml" command="load" ns="local_costmap" />
       <rosparam file="$(find rtabmap_drone_example)/param/global_costmap_params.yaml" command="load" />
       <rosparam file="$(find rtabmap_drone_example)/param/local_costmap_params.yaml" command="load" />
       <rosparam file="$(find rtabmap_drone_example)/param/base_local_planner_params.yaml" command="load" />
    </node>
   <node name="empty_voxels_markers" pkg="rtabmap_costmap_plugins" type="voxel_markers" args="voxel_grid:=/move_base/local_costmap/voxel_layer/voxel_grid visualization_marker:=/voxels_empty">
       <param name="cell_type" value="0"/>
   </node>
   <node name="marked_voxels_markers" pkg="rtabmap_costmap_plugins" type="voxel_markers" args="voxel_grid:=/move_base/local_costmap/voxel_layer/voxel_grid visualization_marker:=/voxels_marked" />

    <!-- joystick -->
    <rosparam file="$(find rtabmap_drone_example)/config/joy_config.yaml" />
    <node pkg="joy" type="joy_node" name="joy_node">
        <param name="autorepeat_rate" value="5"/>
    </node>
    <node pkg="teleop_twist_joy" type="teleop_node" name="teleop_node" output="screen">
        <param name="autorepeat_rate" value="5"/>
    </node>

    <!-- Ground truth -->
  <node if="$(arg ground_truth)" pkg="tf" type="static_transform_publisher" name="worldtomap_broadcaster" args="0 0 0 0 0 0 1 world map 100" />
  <node if="$(arg ground_truth)" pkg="rtabmap_util" type="gazebo_ground_truth.py" name="gazebo_ground_truth" output="screen">
    <param name="frame_id"        value="world"/>
    <param name="child_frame_id"  value="base_link_gt"/>
    <param name="gazebo_frame_id" value="drone::base_link"/>
  </node>
</launch>

The issue starts now.After launching gazebo.launch and slam.launch, My tf tree is compelely changed than the tutorial provied in the github

The error in the slam.launch terminal is as follow :

I tried to workaround by adding TF between all the frames like the one below.

After doing this my RTAB map is able to visualize pointcloud but the offboard and rviz doesnot work correctly. I would say both of them throws error and the drone crashes in simulation. I think I know these are due to sensor fusion and TF error.

I request any one to look at this problem and help me.
Thank You

[matlabbe]

Did you base your work on this example https://github.com/matlabbe/rtabmap_drone_example ?

In that example, there is already a simulated "realsense" like RGB-D camera (though with very accurate depth). You may try to compare what is different.