ROS2 various QOL updates (preparing new binary release) (#1225)

* Updating examples and README

* updated main readme

* odom: Added always_check_imu_tf parameter. sync: increased default sync_queue_size from 2 to 5 (to be more flexible to different hardware), added input and output diagnostics. rtabmap_viz: fixed node with same name redeclared warning.

* update readme

* Added husky demo

* converted demo_robot_mapping.launch to ros2

* Converted stereo_outdoor demo from ros1 to ros2

* Converted multi-session demo to ros2

* Converted demo_find_object launch to ros2

* renamed files

* uniformized default qos, increased sync_queue_size default to 10

* Added vlp16 + imu example

* Added husky 3D lidar demos

* moved demos in subdir

* Added vlp16+zed example

* rtabmap_viz: ignore odom update if tf not ready when not using topic (to avoid showing red screen). Added champ VSLAM demo.

* forwarded camera_model arg for zed examples, removed qos for turtlebot4 demo

* pointcloud_to_depthimage: added more error logs, removed output camera_info published twice and match ros1 namespaces

* Created two base general examples for 3D lidar usage, then create hardware specific examples on top of them.

* Added isaac sim demo

* Final update of all demos. Fixed MapCloud rviz plugin crashing when floor/ceiling filtering is used and resulting cloud is empty.

* Fixed 2d scan deskewing output frame, moved nav2 params under "params" folder, added number of topics processed/dropped for odometry, added icp_odometry option for turtlebot3 scan-only demo.

* rtabmap_demos: added README with examples

* Added TOC

* bump version to 0.21.9

.gitignore

@@ -1,2 +1,3 @@
 .pydevproject
 .settings
+__pycache__

README.md

@@ -1,7 +1,7 @@
 rtabmap_ros
 ===========
 
-RTAB-Map's ROS2 package (branch `ros2`). **ROS2 Foxy minimum required**: currently most nodes are ported to ROS2, however they are not all tested yet. The interface is the same than on ROS1 (parameters and topic names should still match ROS1 documentation on [rtabmap_ros](http://wiki.ros.org/rtabmap_ros)). 
+RTAB-Map's ROS2 package (branch `ros2`). **ROS2 Foxy minimum required**: currently most nodes are ported to ROS2. The interface is the same than on ROS1 (parameters and topic names should still match ROS1 documentation on [rtabmap_ros](http://wiki.ros.org/rtabmap_ros)). 
 
 #### CI Latest
 
@@ -58,41 +58,24 @@ RTAB-Map's ROS2 package (branch `ros2`). **ROS2 Foxy minimum required**: current
 
 # Usage
 
-`rtabmap.launch` is also ported to ROS2 with same arguments. If you see [ROS1 examples](http://wiki.ros.org/rtabmap_ros/Tutorials/HandHeldMapping) like this:
+* For sensor integration examples (stereo and RGB-D cameras, 3D LiDAR), see [rtabmap_examples](https://github.com/introlab/rtabmap_ros/tree/ros2/rtabmap_examples/launch) sub-folder.
 
-```bash
-roslaunch zed_wrapper zed_no_tf.launch
-
-roslaunch rtabmap_ros rtabmap.launch \
-    rtabmap_args:="--delete_db_on_start" \
-    rgb_topic:=/zed/zed_node/rgb/image_rect_color \
-    depth_topic:=/zed/zed_node/depth/depth_registered \
-    camera_info_topic:=/zed/zed_node/rgb/camera_info \
-    frame_id:=base_link \
-    approx_sync:=false \
-    wait_imu_to_init:=true \
-    imu_topic:=/zed_node/imu/data
+* For robot integration examples (turtlebot3 and turtlebot4, nav2 integration), see [rtabmap_demos](https://github.com/introlab/rtabmap_ros/tree/ros2/rtabmap_demos) sub-folder.
 
+## Logging
+To make RTAB-Map's logs appear ordered with RCLCPP's logs, set the following environment variables in your `.bashrc` (see official "[About Logging](https://docs.ros.org/en/humble/Concepts/Intermediate/About-Logging.html)" documentation for more info):
+```bash
+export RCUTILS_LOGGING_USE_STDOUT=1
+export RCUTILS_LOGGING_BUFFERED_STREAM=1
+# Optional, but if you like colored logs:
+export RCUTILS_COLORIZED_OUTPUT=1
 ```
 
-The ROS2 equivalent is (with those [lines](https://github.com/stereolabs/zed-ros2-wrapper/blob/b512dce6ad4565f4770273995b147122e735ca0f/zed_wrapper/config/common.yaml#L58-L60) set to false to avoid TF conflicts):
-
+## Recommended DDS
+If RTAB-Map's GUI or topic frequency feel laggy (even if processing time looks fast enough), it may be caused by the DDS. I recommend to use [Cyclone DDS](https://docs.ros.org/en/foxy/Installation/DDS-Implementations/Working-with-Eclipse-CycloneDDS.html), you can try it by adding this before launching any nodes/launch files:
 ```bash
-ros2 launch zed_wrapper zed.launch.py
-
-ros2 launch rtabmap_launch rtabmap.launch.py \
-    rtabmap_args:="--delete_db_on_start" \
-    rgb_topic:=/zed/zed_node/rgb/image_rect_color \
-    depth_topic:=/zed/zed_node/depth/depth_registered \
-    camera_info_topic:=/zed/zed_node/rgb/camera_info \
-    frame_id:=base_link \
-    approx_sync:=false \
-    wait_imu_to_init:=true \
-    imu_topic:=/zed/zed_node/imu/data \
-    qos:=1 \
-    rviz:=true
+export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp
 ```
-`qos` (Quality of Service) argument should match the published topics QoS (1=RELIABLE, 2=BEST EFFORT). ROS1 was always RELIABLE.
 
 # Installation 
 
@@ -121,39 +104,3 @@ sudo apt install ros-$ROS_DISTRO-rtabmap-ros
     colcon build --symlink-install --cmake-args -DRTABMAP_SYNC_MULTI_RGBD=ON -DRTABMAP_SYNC_USER_DATA=ON -DCMAKE_BUILD_TYPE=Release
     ```
 
-# Example with Turtlebot3
-
-1. Launch Turtlebot3 simulator:
-    ```bash
-    export TURTLEBOT3_MODEL=waffle
-    ros2 launch turtlebot3_gazebo turtlebot3_world.launch.py
-    
-    export TURTLEBOT3_MODEL=waffle
-    ros2 run turtlebot3_teleop teleop_keyboard
-    ```
-
-2. Launch RTAB-Map:
-    ```
-    ros2 launch rtabmap_demos turtlebot3_scan.launch.py
-    
-    # OR with rtabmap.launch.py
-    ros2 launch rtabmap_launch rtabmap.launch.py \
-       visual_odometry:=false \
-       frame_id:=base_footprint \
-       subscribe_scan:=true depth:=false \
-       approx_sync:=true \
-       odom_topic:=/odom \
-       scan_topic:=/scan \
-       qos:=2 \
-       args:="-d --RGBD/NeighborLinkRefining true --Reg/Strategy 1" \
-       use_sim_time:=true \
-       rviz:=true
-    ```
-
-3. Launch navigation (`nav2_bringup` package should be installed):
-    ```
-    ros2 launch nav2_bringup navigation_launch.py use_sim_time:=True
-    ros2 launch nav2_bringup rviz_launch.py
-    ```
-    
-See [rtabmap_demos/launch](https://github.com/introlab/rtabmap_ros/tree/ros2/rtabmap_demos/launch) and [rtabmap_examples/launch](https://github.com/introlab/rtabmap_ros/tree/ros2/rtabmap_examples/launch) subfolders for some other ROS2 examples with turtlebot3 in simulation and a RGB-D camera.

rtabmap_conversions/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>rtabmap_conversions</name>
-  <version>0.21.5</version>
+  <version>0.21.9</version>
   <description>RTAB-Map's conversions package. This package can be used to convert rtabmap_msgs's msgs into RTAB-Map's library objects.</description>
   <maintainer email="[email protected]">Mathieu Labbe</maintainer>
   <author>Mathieu Labbe</author>

rtabmap_conversions/src/MsgConversion.cpp

@@ -923,7 +923,8 @@ void cameraModelToROS(
 	UASSERT(model.R().empty() || model.R().total() == 9);
 	if(model.R().empty())
 	{
-		memset(camInfo.r.data(), 0.0, 9*sizeof(double));
+		cv::Mat eye = cv::Mat::eye(3,3,CV_64FC1);
+		memcpy(camInfo.r.data(), eye.data, 9*sizeof(double));
 	}
 	else
 	{
@@ -934,6 +935,10 @@ void cameraModelToROS(
 	if(model.P().empty())
 	{
 		memset(camInfo.p.data(), 0.0, 12*sizeof(double));
+		if(!model.K_raw().empty()) {
+			model.K_raw().copyTo(cv::Mat(3,4,CV_64FC1, camInfo.p.data()).colRange(0,3));
+			camInfo.p.back() = 1.0;
+		}
 	}
 	else
 	{
@@ -2169,7 +2174,7 @@ bool convertRGBDMsgs(
 		rtabmap::Transform localTransform = rtabmap_conversions::getTransform(frameId, !imageMsgs.empty()?imageMsgs[i]->header.frame_id:cameraInfoMsgs[i].header.frame_id, stamp, listener, waitForTransform);
 		if(localTransform.isNull())
 		{
-			UERROR("TF of received image %d at time %fs is not set!", i, stamp.seconds());
+			UERROR("TF of received image for camera %d at time %fs is not set!", i, stamp.seconds());
 			return false;
 		}
 		// sync with odometry stamp

rtabmap_demos/CMakeLists.txt

@@ -3,7 +3,7 @@ project(rtabmap_demos)
 
 find_package(ament_cmake REQUIRED)
 
-install(DIRECTORY launch
+install(DIRECTORY launch config params data
    DESTINATION share/${PROJECT_NAME}
 )
 

rtabmap_demos/README.md

@@ -0,0 +1,101 @@
+# rtabmap_demos
+
+- [rtabmap_demos](#rtabmap-demos)
+    + [Outdoor Stereo VSLAM](#outdoor-stereo-vslam)
+    + [Indoor 2D LiDAR and RGB-D SLAM](#indoor-2d-lidar-and-rgb-d-slam)
+    + [Multi-Session Indoor 2D LiDAR and RGB-D SLAM](#multi-session-indoor-2d-lidar-and-rgb-d-slam)
+    + [Find-Object with SLAM](#find-object-with-slam)
+    + [Turtlebot4 Nav2, 2D LiDAR and RGB-D SLAM](#turtlebot4-nav2--2d-lidar-and-rgb-d-slam)
+    + [Turtlebot3 Nav2 and 2D LiDAR SLAM](#turtlebot3-nav2-and-2d-lidar-slam)
+    + [Turtlebot3 Nav2 and RGB-D SLAM](#turtlebot3-nav2-and-rgb-d-slam)
+    + [Turtlebot3 Nav2, 2D LiDAR and RGB-D SLAM](#turtlebot3-nav2--2d-lidar-and-rgb-d-slam)
+    + [Champ Quadruped Nav2, Elevation Map and VSLAM](#champ-quadruped-nav2--elevation-map-and-vslam)
+    + [Clearpath Husky Nav2, 2D LiDAR and RGB-D SLAM](#clearpath-husky-nav2--2d-lidar-and-rgb-d-slam)
+    + [Clearpath Husky Nav2, 3D LiDAR and RGB-D SLAM](#clearpath-husky-nav2--3d-lidar-and-rgb-d-slam)
+    + [Clearpath Husky Nav2, 3D LiDAR Assembling and RGB-D SLAM](#clearpath-husky-nav2--3d-lidar-assembling-and-rgb-d-slam)
+    + [Isaac Sim Nav2 and Stereo SLAM](#isaac-sim-nav2-and-stereo-slam)
+    + [Isaac Sim Nav2 and RGB-D VSLAM](#isaac-sim-nav2-and-rgb-d-vslam)
+
+### Outdoor Stereo VSLAM
+```
+ros2 launch rtabmap_demos stereo_outdoor_demo.launch.py
+```
+![Peek 2024-11-29 10-52](https://github.com/user-attachments/assets/b6dd4a1c-5bd5-4cfa-936d-e8e707bbcb23)
+
+### Indoor 2D LiDAR and RGB-D SLAM
+```
+ros2 launch rtabmap_demos robot_mapping_demo.launch.py rviz:=true rtabmap_viz:=true
+```
+![Peek 2024-11-29 11-07](https://github.com/user-attachments/assets/b02beeea-28ed-4fde-932d-c89bef1a046d)
+
+### Multi-Session Indoor 2D LiDAR and RGB-D SLAM
+```
+ros2 launch rtabmap_demos multisession_mapping_demo.launch.py
+```
+![Peek 2024-11-29 11-48](https://github.com/user-attachments/assets/b130e5ab-618f-4c8b-840f-f926b65ab53b)
+
+### Find-Object with SLAM
+```
+ros2 launch rtabmap_demos find_object_demo.launch.py
+```
+![Peek 2024-11-29 12-01](https://github.com/user-attachments/assets/b3cc0c67-517a-4f69-b4cc-35d288e96165)
+
+### Turtlebot4 Nav2, 2D LiDAR and RGB-D SLAM
+```
+ros2 launch rtabmap_demos turtlebot4_sim_demo.launch.py
+```
+![Peek 2024-11-29 12-19](https://github.com/user-attachments/assets/5914e34c-19f1-4b7c-b4df-2e7084946888)
+
+### Turtlebot3 Nav2 and 2D LiDAR SLAM
+```
+ros2 launch rtabmap_demos turtlebot3_sim_scan_demo.launch.py
+```
+![Peek 2024-11-29 12-23](https://github.com/user-attachments/assets/e3c31c5a-5c46-4370-ad17-38c795db7917)
+
+### Turtlebot3 Nav2 and RGB-D SLAM
+```
+ros2 launch rtabmap_demos turtlebot3_sim_rgbd_demo.launch.py
+```
+![Peek 2024-11-29 14-22](https://github.com/user-attachments/assets/5088be17-0875-42cc-b863-d14468c67f26)
+
+### Turtlebot3 Nav2, 2D LiDAR and RGB-D SLAM
+```
+ros2 launch rtabmap_demos turtlebot3_sim_rgbd_scan_demo.launch.py
+```
+![Peek 2024-11-29 13-41](https://github.com/user-attachments/assets/2e878158-b1b6-48a4-801c-72cdb41b4783)
+
+### Champ Quadruped Nav2, Elevation Map and VSLAM
+```
+ros2 launch rtabmap_demos champ_sim_vslam.launch.py
+```
+![Peek 2024-11-29 15-00](https://github.com/user-attachments/assets/d1a27c78-27bc-4901-82a7-59b5d24e6454)
+
+### Clearpath Husky Nav2, 2D LiDAR and RGB-D SLAM
+```
+ros2 launch rtabmap_demos husky_sim_scan2d_demo.launch.py
+```
+![Peek 2024-11-29 15-30](https://github.com/user-attachments/assets/c8f79b86-253e-4c8e-ac7a-c26584f43fa4)
+
+### Clearpath Husky Nav2, 3D LiDAR and RGB-D SLAM
+```
+ros2 launch rtabmap_demos husky_sim_scan3d_demo.launch.py
+```
+![Peek 2024-11-29 15-36](https://github.com/user-attachments/assets/a4b6e6ae-38ed-44da-bbfb-d3c30a301f9c)
+
+### Clearpath Husky Nav2, 3D LiDAR Assembling and RGB-D SLAM
+```
+ros2 launch rtabmap_demos husky_sim_scan3d_assemble_demo.launch.py
+```
+![Peek 2024-11-29 16-16](https://github.com/user-attachments/assets/b2235bd2-33d2-4c44-b6e9-9923a524632b)
+
+### Isaac Sim Nav2 and Stereo SLAM
+```
+ros2 launch rtabmap_demos isaac_sim_vslam_demo.launch.py
+```
+![Peek 2024-11-29 17-49](https://github.com/user-attachments/assets/54cd0c82-aaed-47e5-911a-f286b6d2cc17)
+
+### Isaac Sim Nav2 and RGB-D VSLAM
+```
+ros2 launch rtabmap_demos isaac_sim_vslam_demo.launch.py stereo:=false vo:=rtabmap
+```
+![Peek 2024-11-30 13-22](https://github.com/user-attachments/assets/240820c6-4dea-4cbf-9431-b4b3af695d51)