RTAB-Map Octomap for ROS MoveIt with dynamic objects and self-filtering

[robertzickler]

Hello everybody,
I'm working with an UR10 with an Intel Realsense D435i mounted on the endeffector. I'm using ROS Melodic.
What I want: Getting the (awesome!!) rtabmap pointcloud/octomap in ROS MoveIt for the path planning of the robot.
What is the problem: I either get dynamic objects removed or the self-filtering of the robot done never both at the same time.
What I tried:
1) Using the setup Assistant for MoveIt to get the Octomap in the planning scene (3D perception).
Result: Working with self-filtering but dynamic Objects are not updated, even when I use /rtabmap/octomap_occupied_space

2) Using an Octomap Server
Result: No self-filtering. Maybe I could go deeper in here but not sure how to do the self-filtering.

3) Using the package Moveit-External-Octomap-Updater (https://github.com/Notou/Moveit-External-Octomap-Updater)
Result: Dynamic Octomap but no self-filtering.

One more (little) problem: When using the "Clear octomap"-button in RViz nothing is happening.

I would appreciate if someone could point me in the right direction where to go next. Or is there a param I can tweak in the rtabmap settings to get a good map with dynamic objects?

Thank you very much
Robert

[matlabbe]

I didn't use all tools you mentioned, but for rtabmap, to solve the two problems:
1- Self-filtering: I would go with http://wiki.ros.org/camera_self_filter package to mask the depth image before sending it to rtabmap. That way, the octomap produced by rtabmap won't contain the robot itself.
2- Continuous update of the octomap: set parameter map_always_update to true for rtabmap node. Make sure Grid/RayTracing is also true to clear dynamic obstacles in the field of view of the camera.

The "clear octomap" button in rviz doesn't see standard. Is it coming with MoveIT plugin?

cheers,
Mathieu

[robertzickler]

Thank you very much. Took me a while to get my head around this problem but finally managed to get it working.
The linked camera_self_filter is very old (ROS groovy) and I couldn't make it working (rosbuild -> catkin ).

I now use the realtime_URDF_filter (https://github.com/blodow/realtime_urdf_filter). This one is using the GPU to mask the visual (not collision) STL-files from the robot description to the Depth Image. The filtered image then goes as input to RTAB-map.
Big problem: no padding of the robot parts! But you can inflate the STL-files (doggy...) or have a realy good setup with perfect model to real robot position and sensor data but that is kind of imposible. (*Tipp: read at the bottom)
The RTAB-map pointcloud is then published to 'move_group/monitored_planning_scene' and used for the MoveIt path planing.

I still have to tweek some params and will try to get you a working example for everyone to use. But up till now it is still kind of messy :D

Another package I tried was http://wiki.ros.org/robot_self_filter and http://wiki.ros.org/robot_body_filter. The first one is not maintained very well. The later (a fork from the robot_self_filter) seems to be very powerful but I couldn't get the filtering mechanism working on ROS. Especially I was missing a tutorial on pointcloud filtering... Would be awesome if someone could point me in the right direction

To your question:
Yes the button comes with MoveIt and clears the the 'move_it/monitored_planning_scene'. This is working again with my solution,.

*Note mentioned above for everyone interested:
A kind of interesting way I found out to still have the "ideal" robot in RViz while using the inflated STL-files for the URDF filtering:
My set-up: A seperat Jetson Nano (A) (or any other mashine with a GPU) with the camera pluged in and the filtering package and the RTAB-map running. The ROS core and MoveIt running on the main mashine (B). The Jetson connected over a switch.
Now the realtime_URDF_filter still uses the lokal path (on the Jetson A) to the STL-files. So you can have the "perfect" STL-models on the main mashine (B) (for RViz,...) and the inflated models on the mashine running the filter. The file names must match so dont get confused or override them when syncing your folders
Importend: time syncing the mashines but that is an other topic (https://wiki.ros.org/ROS/NetworkSetup)...

Robert

[matlabbe]

Thank you Robert for the detailed results.

rzett wrote

Especially I was missing a tutorial on pointcloud filtering... Would be awesome if someone could point me in the right direction

For general point cloud filtering, I am using Point CLoud Library (which is compatible with ROS and sensor_msgs::PointCloud2, through pcl_ros package). There are some filtering tutorials: https://pcl.readthedocs.io/projects/tutorials/en/latest/#filtering

cheers,
Mathieu