SLAM with D435i tutorial: map not showing

[naoya-kumagai]

I have a question on running the SLAM with D435i tutorial. I am following the instructions on the page.
When running

roslaunch realsense2_camera opensource_tracking.launch

There are a few problems occuring in Rviz.

1. The pointcloud is not shown. I put a check in PointCloud2 and the Topic is /voxelcloud
2. The camera_link TF is moving around wildly despite keeping the camera absolutely still. The camera has been calibrated.
3. In the console, Failed to meet update rate! Warning is constantly showing, which may be indicative of something wrong.

The color image is shown so this is not a problem of the camera not being found.

[MartyG-RealSense]

Hi @naoya-kumagai Thank you very much for your questions.

1. Do you have Fixed Frame set to map in RViz, as shown in the enlarged view below of an image from the guide?

Also, do you only have 'map' and 'camera_link' ticked in the Frames list of tick-boxes, and everything else in that list unticked?

The color image topic should be set to /camera/color/image_raw

2. If the above settings are applied, does the camera_link TF still move around wildly when the camera is stationary?

3. There is little documentation available about the Failed to meet update rate! error. When ROS does generate this error though, it sometimes provides this advice: "Try decreasing the rate, limiting sensor output frequency, or limiting the number of sensors".

[naoya-kumagai]

@MartyG-RealSense

1. Although I had already done what you suggested, I found I had not installed the octomap_rviz_plugins package. Installing this produced a pointcloud somewhat(*) successfully.
2. Yes, the camera_link TF is moving around after these changes.
3. From the messages, I presume that maybe the frequency of the accel/gyro sensors are too high for the software to keep up with. Is this correct? And if so, is there a known cause for this?

Additional Issues.
4. I said somewhat above because I see another weird behavior. About half of the time, Rviz successfully shows a pointcloud. But the other times, 1. the pointcloud is not shown and 2. the map TF is missing (not an option in TF)
5. Even when the point cloud is showing, I do not see a map forming in Rviz.

[MartyG-RealSense]

I'm pleased to hear that you made progress, @naoya-kumagai

3. The IMU can be very sensitive because of the accelerometer. In the librealsense SDK there is the ability to reduce this sensitivity with program scripting to improve stability by weighting calculations towards the gyro instead of the accelerometer. The RealSense ROS wrapper does not provide this function though.

4. Are there any log messages generating in the ROS terminal that might indicate a communication problem with the camera, such as control_transfer returned

5. The D435i SLAM guide states that a map is created by using the command below, which would be input into the ROS terminal:

rosrun map_server map_saver map:=/rtabmap/proj_map –f my_map_1

If you would like to expand the capabilities of the SLAM guide to make use of features such as localization and path plotting, a RealSense user created an adaptation of Intel's guide in the link below that explains how to set up these additional features.

https://shinkansan.github.io/2019-UGRP-DPoom/SLAM.html

[naoya-kumagai]

@MartyG-RealSense
Thank you for the additional resource! It is very helpful.

I have checked again and see that control_transfer error only occurs for at the very beginning after launch so this is not a problem.

Before I get to any mapping or other high level action, I would like to make sure the pointcloud generation and IMU filtering is consistently working. Let me summarize the 3 errors/warnings that I am having. In addition, I have the problem of the TF in Rviz moving around very wildly.

   [ WARN] [1631039665.860917063]: Failed to meet update rate! Took 0.0080772589999999998278

    [rviz-7] process has died [pid 16141, exit code -11, cmd /opt/ros/noetic/lib/rviz/rviz -d /opt/ros/noetic/share/rtabmap_ros/launch/config/rgbd.rviz __name:=rviz __log:=/home/naoya/.ros/log/38763406-100a-11ec-9603-cd127680ddf0/rviz-7.log].

    [ERROR] (2021-09-07 11:28:40.147) Rtabmap.cpp:1155::process() RGB-D SLAM mode is enabled, memory is incremental but no odometry is provided. Image 694 is ignored!

The second process died error seems to have disappeared after setting inital_reset:=true, but if there is any other known solution, I would like to know.

What would be an example of controlling the gyro/accel? I am aware that the librealsense SDK is available from many languages, but would this involve for example adding a node to my launch file that becomes a substitute for imu_filter_madgwick?

My wrapper and librealsense versions are as below.
[ INFO] [1631034714.285349816]: RealSense ROS v2.3.1
[ INFO] [1631034714.288835293]: Built with LibRealSense v2.48.0

For installation, I did

 sudo apt-get install ros-$ROS_DISTRO-realsense2-camera

as well as the steps in
https://github.com/IntelRealSense/librealsense/blob/master/doc/distribution_linux.md#installing-the-packages

[MartyG-RealSense]

Does the RGB-D SLAM mode is enabled, memory is incremental but no odometry is provided message disappear if you add the commands enable_gyro:=true enable_accel:=true to the end of your roslaunch instruction? For example:

realsense2_camera opensource_tracking.launch enable_gyro:=true enable_accel:=true

If you wish to define custom frequencies for gyro and accel in ROS then you specify the frequency as an FPS speed with gyro_fps and accel_fps. For example, inserting the commands below into your roslaunch instruction:

accel_fps:=63 gyro_fps:=200

[naoya-kumagai]

@MartyG-RealSense
I added the what you suggested, so now my roslaunch instruction is:

realsense2_camera opensource_tracking.launch enable_gyro:=true enable_accel:=true accel_fps:=63 gyro_fps:=200

but the no odometry error still shows.

Since there are so many errors, I think it would be good to focus on the most crucial error right now, which is the pointcloud not showing in Rviz. I thought I solved this earlier, but it came up again. I have followed your instructions above, except for ticking map in TF because that tick doesn't show up. What are some ways to solve this?

[MartyG-RealSense]

The installation process diverged from what I know as the normal procedure for D435i SLAM when the octomap_rviz_plugins package was installed earlier in #2067 (comment) so that could be affecting the settings that are available.

It may be useful to go back to the start of Intel's D435i SLAM guide and check the prerequisites list again. Can you confirm please that the 4 modules below are installed.

realsense2_camera
imu_filter_madgwick
rtabmap_ros
robot_localization

[naoya-kumagai]

@MartyG-RealSense
Ah! After removing octomap_rviz_plugins, I now see a point cloud. I have tried launching multiple times, but it seems consistent.

If anyone has the same problem, the error message about octomap_rviz_plugins probably is okay to ignore.

I have some other questions, but the main focus of this issue is solved, so I will close it.
Thank you!

[MartyG-RealSense]

You are very welcome, @naoya-kumagai - I'm pleased that you achieved a solution! Please do feel free to ask further questions in a new issue.