Issue with Mapping Using 2D Lidar and RealSense

[Joel_A]

Hey, I've been trying to use this rplidar with the realsense camera but I've been having trouble deciding if I should try ICP odometry, rgbd, or rgbdicp odometry. Before I get to that though, I can't seem to get a map.



This has me believing there isn't an issue, I get no error and even rviz has some results.



However, when I terminate, no map is saved in the database.



Here is my launch file (Sorry if it's messy):

<launch>

<arg name="rgbd"    default="false"/>
<arg name="pm"      default="false"/>
<arg name="nodelet" default="false"/>

<group ns="camera"> 
    <node pkg="nodelet" type="nodelet" name="points_xyz" args="load rtabmap_ros/point_cloud_xyz camera_nodelet_manager">
      <remap from="depth/image"       to="/aligned_depth_to_color/image_raw"/>
      <remap from="depth/camera_info" to="/aligned_depth_to_color/camera_info"/>
      <remap from="cloud"             to="/cloud" />
      <remap from="scan"	      to="/scan1" />

      <param name="voxel_size" type="double" value="0.05"/>
      <param name="decimation" type="int" value="8"/>
      
      <param name="Odom/AlignWithGround"  type="string" value="true"/>
    </node>

<group if="$(arg nodelet)">
      <node unless="$(arg rgbd)" pkg="nodelet" type="nodelet" name="rgbd_odometry" args="load rtabmap_ros/rbgd_odometry camera_nodelet_manager" output="screen">
        <remap from="depth/image"       to="/camera/aligned_depth_to_color/image_raw"/>
        <remap from="rgb/camera_info"   to="/camera/color/camera_info"/>
        <remap from="rgb/image"         to="/camera/color/image_raw"/>

	  
        <param name="frame_id"        type="string" value="base_link"/>   

        <param name="Odom/GuessMotion"  type="string" value="true"/>
        <param name="Odom/ResetCountdown"  type="string" value="1"/>
      </node>
</group>

<group unless="$(arg nodelet)">
      <node unless="$(arg rgbd)" pkg="rtabmap_ros" type="rgbd_odometry" name="rgbd_odometry" output="screen">
	<remap from="depth/image"       to="/camera/aligned_depth_to_color/image_raw"/>
        <remap from="rgb/camera_info"   to="/camera/color/camera_info"/>
        <remap from="rgb/image"         to="/camera/color/image_raw"/>

	  
        <param name="frame_id"        type="string" value="base_link"/>   

        <param name="Odom/GuessMotion"  type="string" value="true"/>
        <param name="Odom/ResetCountdown"  type="string" value="1"/>
      </node>
</group>
</group>

<node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">
          <param name="frame_id"        type="string" value="base_link"/>
          <param name="subscribe_depth" type="bool"   value="false"/>
          <param name="subscribe_rgbd"  type="bool"   value="true"/>
          <param name="subscribe_scan"  type="bool"   value="true"/>

          <remap from="scan"		  to="/scan1"/>
          <remap from="odom"      	  to="odom"/>
          <remap from="scan_cloud"        to="/cloud"/>
          <remap from="depth/image"       to="/camera/aligned_depth_to_color/image_raw"/>
          <remap from="rgb/camera_info"   to="/camera/color/camera_info"/>
          <remap from="rgb/image"         to="/camera/color/image_raw"/>

          <param name="queue_size"  type="int"  value="10"/>
          <param name="approx_sync" type="bool" value="false"/>

          <!-- RTAB-Map's parameters -->
          <param name="RGBD/AngularUpdate"        type="string" value="0.01"/>
          <param name="RGBD/LinearUpdate"         type="string" value="0.01"/>
          <param name="RGBD/OptimizeFromGraphEnd" type="string" value="false"/>
    </node>

</launch>

How can I get this working right?

[matlabbe]

Hi,

Are your camera and lidar really looking towards the ceiling? We expect configurations more like this one (lidar parallel to ground, camera looking forward):


Not sure you need point_cloud_xyz nodelet, as I would rater use the scan from rplidar than the fake scan created from realsense.

Odometry seems working, the mapping part seems waiting on input topics. Look if there are warnings appearing in terminal telling that some topics may not be received (rtabmap node). When "subscribe_rgbd" is used, you should have the nodelet rgbd_sync running. See this example if you have also access to wheel odometry (in that case you won't need rgbd_odometry nodelet) or this example if you still need to compute odometry (in this example, I would rather use icp_odometry with lidar instead of rgbd_odometry node to be more robust to featureless surfaces, which are common indoor).

cheers,
Mathieu

[Joel_A]

Lol I know it seems strange, but It's pointing to the ceiling since I'm intending on mapping inside a concrete pipe which also causes issue if I'm using wheel odometry. I'm thinking because in a round pipe the wheels aren't flat on the ground and sliding could be messing it up. The odometry is working pretty decently however, but right now rtabmap is only functioning as a odometry node.
I'm not getting the error where it says waiting on certain topics. It's charging right now so I will look into that example and post here again after.

Edit: I feel I should mention that the lidar is 2D