Interfacing TOF With RTABMAP

Interface Connection

This section provides information about how RTAB-Map works with ToF data without having AMR. Here, RTAB-Map relies on external odometry which is provided from the ICP odometry source using the point cloud.

Block Diagram of Interfacing ToF with RTAB-Map

---

• The ToF camera node publishes three topics ir_image, depth_image and camera_info.
• The rectification and synchronization node rectifies ir_image and depth_image to remove distortions and then synchronizes all three input topics ir_image, depth_image and camera_info. It publishes ir_image_sync, depth_image_sync and camera_info_sync.
• The depth_image_sync and camera_info_sync is used by point cloud node to generate point cloud.
• RTAB-Map subscribes to cloud and odometry to generate occupancy grid map and optimized odometry.
• ir_image_sync, occupancy grid map(grid) and odometry(odom) can be visualized in the Rviz.

Step by Step Instructions

These steps will guide you to realize that RTAB-Map is working fine with ToF. Once the RTAB-Map is tested OK the AMR Navigation can be run as given instructions in further sections.

1. Here, the first step will be to verify that ROS2 topics are coming from ToF. Please follow the guide given in section 4 to complete this if it is pending.
2. Note that RTAB-Map is already cloned and built along with docker container. So, start a new ROS2 humble docker container in new terminal.

Check ‘$ docker ps’ if container named ros2-humble-jammy is running run:

      docker exec -it ros2-humble-jammy bash

If container is not running, then refer Humble Docker:

3. Here, ToF topics needs to be rectified first before giving it to RTAB-Map. It will improve the quality of the data. Start a new ROS2 humble docker container (refer Humble Docker)

          docker exec -it ros2-humble-jammy bash
          cd ros2_ws/
          colcon build 
          //(Optional, if not done)
          source install/setup.bash
          ros2 launch eic_bringup_utils image_proc.launch.py

After running the above commands two new topics /cam1/ir_image_rect and /cam1/depth_image_rect will be visible.

---

ros2 topic list after rectification

---

4. After rectification, synchronizing topics in their timestamps is necessary. Follow the given steps to synchronize rectified topics to the timestamps of AGX Orin kit. Start and new ROS2 docker container and follow the steps

          docker exec -it ros2-humble-jammy bash
          python3 scripts/rect_sync_node.py

• After running the above commands, synchronized topics will be visible.

---

ros2 topic list after rectification and synchronization

---

Example and Visualizations

Once synchronization and rectification are done, start RTAB-Map to see its working. Go to eic_bringup_utils, and start a RTAB-Map launch file.

          docker exec -it ros2-humble-jammy bash
          source ~/rtabmap_ws/install/setup.bash
          cd ~/ros2_ws/
          colcon build (optional if not done)
          source install/setup.bash
          ros2 launch eic_bringup_utils rtabmap_icp_odom.launch.py

After running above commands and moving AMR little on the ground, you will see following types of map building process on RTAB-Map Visualizer node. For moving AMR, use ROS2’s teleop twist keyboard while running the following command.

          ros2 run teleop_twist_keyboard teleop_twist_keyboard

Visualization of Point cloud map and occupancy grid map in rtabmap visualizer

Similar maps can also be visualized in RVIZ2 by running the commands below on AGX Orin terminal.

          source /opt/ros/galactic/setup.bash
          ros2 run rviz2 rviz2 -d ~/Robotics_AMR-devel/eic_bringup_utils/rviz/rtabmap.rviz

Visualization of IR image and Occupancy grid map in rviz

Instructions/Errors

Note: If anyone working with the ROS2 Galactic system is necessary, follow the instructions in Galactic Docker and redo all the above procedures in an equivalent way using the ROS2 Galactic Docker.