RGB_D camera + octomap_server + rtabmap

[ahmadkh1995]

Hello
I want to implement below block diagram with a recorded rosbag , but when I want to visualize octomap(produced by octomap_server) nothing visualizes in Rviz and it's output doesn't publish.
This is my octomap_server launch file;

<node pkg="octomap_server" type="octomap_color_server_node" name="octomap_server">
 <remap from="cloud_in" to="/camera/depth/points" />   
</node>

and also my "frame_id" is "map" which given by rtabmap.

would you please help me where is my mistake ? I understand that some users use;
 <remap from="cloud_in" to="/rtabmap/cloud_map" />

but I want to use pointcloud2 directly from my RGB-D sensor.

[matlabbe]

First, can you visualize the rtabmap output? (no warnings in terminal) What is the rtabmap config (which odometry is used?)? You could also visualize the octomap created by rtabmap in rviz.

[ahmadkh1995]

Thanks for your reply , I can visualize RTabmap output but I want visuilize also output of octomap_server (the idea is to investigate and compare produced octomap in both case(RTabnap's octomap output and also octomap_server output) on the same dataset.

This is error of launching octomap_server ;


and this the log of Debug for octomap_server;


[ INFO] [1614479710.367599425]: Using RGB color registration (if information available)
[ INFO] [1614479710.372157328]: Publishing latched (single publish will take longer, all topics are prepared)
[DEBUG] [1614479741.032376537]: MessageFilter [target=/map ]: Removed oldest message because buffer is full, count now 5 (frame_id=camera_rgb_optical_frame, stamp=1614168934.887957)
[DEBUG] [1614479741.032463667]: MessageFilter [target=/map ]: Added message in frame camera_rgb_optical_frame at time 1614168935.057, count now 5
[DEBUG] [1614479741.095177594]: MessageFilter [target=/map ]: Removed oldest message because buffer is full, count now 5 (frame_id=camera_rgb_optical_frame, stamp=1614168934.924031)
[DEBUG] [1614479741.095265584]: MessageFilter [target=/map ]: Added message in frame camera_rgb_optical_frame at time 1614168935.089, count now 5
[DEBUG] [1614479741.133174963]: MessageFilter [target=/map ]: Removed oldest message because buffer is full, count now 5 (frame_id=camera_rgb_optical_frame, stamp=1614168934.957006)
[DEBUG] [1614479741.133306858]: MessageFilter [target=/map ]: Added message in frame camera_rgb_optical_frame at time 1614168935.124, count now 5
[DEBUG] [1614479741.178926381]: MessageFilter [target=/map ]: Removed oldest message because buffer is full, count now 5 (frame_id=camera_rgb_optical_frame, stamp=1614168934.988682)
[DEBUG] [1614479741.179071806]: MessageFilter [target=/map ]: Added message in frame camera_rgb_optical_frame at time 1614168935.156, count now 5
[DEBUG] [1614479741.241546980]: MessageFilter [target=/map ]: Removed oldest message because buffer is full, count now 5 (frame_id=camera_rgb_optical_frame, stamp=1614168935.024630)




I attached my roslaunch files both for RTabmap and also for octomap_server in this post

test_octomap.launch

test_rtabmap.launch

this is launch files that I run ;

        $ roscore
        $ rosbag play mybag.bag
        $ roslaunch rtabmap_test test_rtabmap.launch      (rtabmap launch file)
        $ roslaunch octomap_project test_octomap.launch      (octomap_server launch file)


These are my topics in rosbag file;

/camera/depth_registered/image
/camera/depth_registered/image_raw
/camera/depth_registered/points
/camera/rgb/camera_info
/camera/rgb/image_color
/camera/rgb/image_raw
/camera/rgb/image_rect_color
/clock
/rosout
/rosout_agg
/tf

[matlabbe]

Launching octomap alone you don't have TF between map and the camera frame. You can debug your tf tree with

rosrun tf view_frames

What happens when you launch both octomap_server and rtabmap launch files?

[ahmadkh1995]

Thanks for your reply , I attached my tf file here:
frames.pdf

This is the output of launching RTabmap , output of the Rtabmap (" /rtabmap/octomap_full ") works fine and I can visualize it in Rviz but output of octomap_server(" /octomap_full ") not visualizes in Rviz.



here is Rviz when I want to see output of octomap_server : (" /octomap_full ")

[matlabbe]

Hi,

I tried octomap_server and it seems to work. I tested with demo_robot_mapping.launch with this line set to true to produce a 3D map.

$ roslaunch rtabmap_ros demo_robot_mapping.launch rviz:=true rtabmapviz:=false
$ rosrun octomap_server octomap_color_server_node cloud_in:=/voxel_cloud
$ rosbag play --clock demo_mapping.bag

What I see comparing the OctoMaps, is that on loop closure, the OctoMap cannot correct the previous cells. Rtabmap updates the Octomap on loop closure correction.

octomap_server:


rtabmap:


EDIT: the rqt_graph


cheers,
Mathieu