introlab / rtabmap_ros

RTAB-Map's ROS package.
http://wiki.ros.org/rtabmap_ros
BSD 3-Clause "New" or "Revised" License
916 stars 549 forks source link

'No map received' when using rtabmap with RPI4 & Realsense D435i #1129

Open HG1017 opened 4 months ago

HG1017 commented 4 months ago

hi, I have installed Rtabmap_ros on my latop and interfacing with Realsense D435i mounted on Turtlebot burger. It has Raspberry pi4 chip for sending the images from realsense to the laptop. Currently i am receiving the images correctly at a approx rate of 2 frames per second, showing in Rviz, but when i am using the command roslaunch realsense2_camera opensource_tracking.launch , we are getting the error of 'No Mapr Recieved' in the map section of Rviz. image

We have created a static transform between odom & camera_link as well as map and camera_link, still not able to solve the issue. Any help related to this is highly appreciated. Thank you

matlabbe commented 3 months ago

You may look for warnings/errors in terminal of where rtabmap is launched. At 2Hz, I suspect that input frames cannot be synchronized and rtabmap is waiting on input topics.

Ideally, you would need at least 10 Hz to properly do VSLAM. Are images available at 30 Hz on the RPI4? Open a terminal on the RPI4 and do rostopic hz /camera/topic/name on all topics that sent to rtabmap:

rostopic hz /camera/color/image_raw \
    /camera/aligned_depth_to_color/image_raw \
   /camera/color/camera_info \
   /camera/depth/camera_info

If it is still 2Hz, there is an issue with realsense driver on the RPI4. I hope you can see 30 Hz, that would mean your issue is a networking issue. If so, try connecting through ethernet instead of wireless.

Ideally when going wireless, you would need to compress some images:

<arg name="rgb_topic" value="/camera/color/image_raw"/>
<arg name="compressed" value="true"/> <!-- subscribe to compressed and compressedDepth topics -->
<!-- <arg name="depth_compressed" value="raw"/> --> <!-- uncomment if depth rate is worst -->
<arg name="depth_topic" value="/camera/aligned_depth_to_color/image_raw"/>
<arg name="camera_info_topic" value="/camera/color/camera_info"/>
<arg name="depth_camera_info_topic" value="/camera/depth/camera_info"/>