rtabmap/rgbd_odometry: Did not receive data since 5 seconds! (/camera/color/image_raw \ + /camera/aligned_depth_to_color/image_raw \ + /camera/color/camera_info)

[lalondelalonde]

Hello guys,

I've been stock on this for days now...

I'm currently trying to build a mobile 3d mapping kit using:

• Ubuntu 20.04 Focal Fossa
• Ros Noetic
• Realsense D435i (pre-build packages X86/AMD64-based Debian)
• Slamtec M2M2 Mapper (slamware_ros_sdk & slamware_ros)
• Rviz
• Rtabmap (rtabmap & rtabmap_ros)

My rtabmap won't work with neither D435i or M2M2 Mapper (zero success so far).

I've been able to make my sensor work in Rviz. M2M2 is doing a real-time 2d map (nice), IMU seems to be working (I'm no expert), pointcloud, odometry and so on.
D435i is also being detected as you see in the screen shot:

D435i is also working in Realsense Viewer:

Please note that my coding skill is not high.

I've been trying to see what is the issue. /camera/color/image_raw \ + /camera/aligned_depth_to_color/image_raw \ + /camera/color/camera_info are all published under : rostopic list.

If I run rostopic hz, all topic will display : No new message.
I've update dependencies and package and everything seems to be up-to-date.

I can't achieve the RGB-D Handheld Mapping tutorial.

Thank you all
Peace & Blessings

[matlabbe]

If I run rostopic hz, all topic will display : No new message.

You are on something, rostopic hz should at least show some frame rate for sensor topics. If you can record/share a rosbag of the camera and scan topics, that would be useful, e.g. (note that I guessed the scan topic name):

rosbag record /tf /tf_static /camera/color/image_raw /camera/aligned_depth_to_color/image_raw /camera/color/camera_info /scan

You may also show which commands you tried to start realsense, the lidar and rtabmap.

[lalondelalonde]

Hello Mat, thank you for the input.

I went for :
rosbag record /tf /tf_static /camera/color/image_raw /camera/aligned_depth_to_color/image_raw /camera/color/camera_info /camera/aligned_depth_to_color/camera_info /slamware_ros_sdk_server_node/scan

I've done 2 rosbag for 2 realsense commands:

ROSBAG1: Commands used:

M2M2 = roslaunch slamware_ros_sdk slamware_ros_sdk_server_node.launch ip_address:=xxx.xxx.xx.x roslaunch slamware_ros_sdk view_slamware_ros_sdk_server_node.launch
Rtabmap = roslaunch rtabmap_ros rtabmap.launch rtabmap_args:="--delete_db_on_start" rviz:=true
D435i = roslaunch realsense2_camera rs_camera.launch

ROSBAG2: Commands used:

M2M2 = roslaunch slamware_ros_sdk slamware_ros_sdk_server_node.launch ip_address:=xxx.xxx.xx.x roslaunch slamware_ros_sdk view_slamware_ros_sdk_server_node.launch
Rtabmap = roslaunch rtabmap_ros rtabmap.launch rtabmap_args:="--delete_db_on_start" rviz:=true
D435i =

 # ISSUE: Use unite_imu_method:="copy" if imu topics keep stopping
 roslaunch realsense2_camera rs_camera.launch \
    align_depth:=true \
    unite_imu_method:="linear_interpolation" \
    enable_gyro:=true \
     enable_accel:=true

 rosrun imu_filter_madgwick imu_filter_node \
    _use_mag:=false \
    _publish_tf:=false \
    _world_frame:="enu" \
    /imu/data_raw:=/camera/imu \
    /imu/data:=/rtabmap/imu

Seems like the command #2 for the D435i is failing to work properly, but the #1 seems to be working fine.

Still, with both, rtabmap is still giving the error message.

Peace.

[matlabbe]

In rosbag 1, the depth image topic is missing. In rosbag 2, only the scan topic is there.

[lalondelalonde]

How would you go about this?

Any idea what might cause such issue?

Thanks

[matlabbe]

Can you do this:

roslaunch realsense2_camera rs_camera.launch \
    align_depth:=true \
    unite_imu_method:="linear_interpolation" \
    enable_gyro:=true \
     enable_accel:=true

roslaunch slamware_ros_sdk slamware_ros_sdk_server_node.launch ip_address:=xxx.xxx.xx.x 

roslaunch slamware_ros_sdk view_slamware_ros_sdk_server_node.launch

rosbag record /tf /tf_static \
   /camera/imu \
   /camera/color/image_raw \
   /camera/aligned_depth_to_color/image_raw \
   /camera/color/camera_info \
   /slamware_ros_sdk_server_node/scan

Record 10 sec, then verify that all those topics are in the rosbag. If one or more are missing, you should debug why the corresponding nodes are not publishing them. If all topics are there, can you share the rosbag on a dropbox/googledrive?

[lalondelalonde]

Hello Mat, I really appreciate the time, sincerely thank you.

As discussed, I ran:

• roslaunch realsense2_camera rs_camera.launch
  align_depth:=true
  unite_imu_method:="linear_interpolation"
  enable_gyro:=true
  enable_accel:=true
• roslaunch slamware_ros_sdk slamware_ros_sdk_server_node.launch ip_address:=xxx.xxx.xx.x
• roslaunch slamware_ros_sdk view_slamware_ros_sdk_server_node.launch

followed by :

• _rosbag record /tf /tf_static \

  /camera/imu
  /camera/color/image_raw
  /camera/aligned_depth_to_color/image_raw
  /camera/color/camera_info
  /slamware_ros_sdk_server_node/scan_

Please note I didn't run any rtabmap command.

Here is the result:

I also ran rostopic list (all camera topics are there, even IMU that doesn't show up on the image):

I assume I will have to try to debug those nodes since none (camera related) are actually showing up.

Peace.

[lalondelalonde]

Just want to add a rqt_graph:

[matlabbe]

Yes, you would need to debug why the camera is not publishing anything. For convenience, instead of recording a bag, you can debug with:

rostopic hz \
   /camera/imu \
   /camera/color/image_raw \
   /camera/aligned_depth_to_color/image_raw \
   /camera/color/camera_info \
   /slamware_ros_sdk_server_node/scan

[lalondelalonde]

Quick update:

I'm making progress but still having issues.

I created a URDF and it seems that my RVIZ is doing great now and that all topics are being published under base_link:

I also succeeded to save a rosbag and have it play inside Rviz.

That being said... this rtabmap.. I can't make it work.

I can open it with a .launch file while my rviz is opening with the right topics and .config but still have nothing being publish in rtabmap.

So I will keep on working on this but wanted to update you all.

PEACE

[lalondelalonde]

Improving, after 5 weeks! Finally some response in RTABMAP!

Nothing perfect but thats a milestone for non-programmer like me:

[matlabbe]

If you can record a bag, can you share one?

[lalondelalonde]

Hello Mat, yes I can share one with you, no problem.

I just want to show you guys, that I did indeed capture... something! We are making progress here! My rtabmap_GUI is still very buggy flashing red and yellow. I think my slamware nodes/topics weren't running (lidar module 2d) but I still got the d435i realsense.

I still need to work on my URDF and rtabmap.launch (it's like 500 lines) (I've tried to make a custom .launch by I keep on running into more problems... anyways!). I will 3D print a frame so it will probably help too.

Look at this beauty

It's supposed to be a single wall! hahaha

You can see that I have some frames duplicate clearly, we can see it by the light source and the photo frames. BUT! after like.... over a month of trials I would say.. this is quite an achievement!

Small wins count!

[lalondelalonde]

Hello guys!
Still progressing.

Do you guys have an idea on why the map is not keep the structure? We can see in the middle where I was spinning.

Here is my RQT TF tree. Not sure if slamware and robot_pose should be directly link to map or lidar_link??

If you see anything that might cause the problem, let me know!

Also, my time? is set to 17,000,000 is there a way to reset this?

PEACE

[matlabbe]

Do you guys have an idea on why the map is not keep the structure? We can see in the middle where I was spinning.

It looks like bad visual odometry, or odometry done in wrong frame.

I don't know what kind of data slamware is producing (it is like producing 3 frames), if you have a bag, I could guess what they should represent.

The time looks fine, that format is epoch time. You can convert it to human-readable here https://www.epochconverter.com/

[lalondelalonde]

Currently working on odometry. Seems to be the problem.

(it is like producing 3 frames)

Yes exactly, without moving the kit at all, I start with 3 frames right away that are close but not align.

Visual odometry & ICP are set to false (I might play around with the Visual Odometry to use the IMU of D435i)

Structure is getting better but still some misalignment or frame duplication. When I run the D435i realsense alone in rtabmap I don't have this issue so I assume it's the 2D lidar.

Thanks Mat ✌️

[lalondelalonde]

Just to give a general idea, this is right on start without any movement:

[lalondelalonde]

Just want to share that I'm making progress once again.

Odometry from the 2D lidar seems to have improved and wall texture also. I still have some misalignment occuring that I will need to fine tuned.

[matlabbe]

Can you share that database? Having a look at the data could help to give more tips or see if there are issues with the data.