Fixing topic sync lagging and delay issues, more examples (depthai, z…

…ed) (#1206)

* Fixed odometry latency

* Fixed node ID=0 issues as msgs may not have seq. (#1202)

(cherry picked from commit 097cab0)

* Odom: removed long processing from ros callbacks (decreasing delay, also making delay independent of the message filters topic_queue_size and sync_queue_size parameters)

* Changed a log from info->debug

* Changed a log from info->debug

* Updated default topic and sync queue_size inside nodes. Exposing topic and sync queue size params in warning when cannot synchronize. Added zed and depthai examples. Odom: Fixed imu callback group, added multi-threaded executors for all odometry nodes.

* fixed merge

* Include everything needed in example launch files for simple launch. Small fixes.

---------

Co-authored-by: Borong Yuan <yuanborong@hotmail.com>

rtabmap_conversions/src/MsgConversion.cpp

@@ -1988,12 +1988,6 @@ rtabmap::Transform getTransform(
 {
 	// TF ready?
 	rtabmap::Transform transform;
-	std::string errString;
-	if(!tfBuffer.canTransform(fromFrameId, toFrameId,  tf2_ros::fromMsg(stamp), tf2::durationFromSec(waitForTransform), &errString))
-	{
-		UWARN("(can transform %s -> %s?) %s (wait_for_transform=%f)", fromFrameId.c_str(), toFrameId.c_str(), errString.c_str(), waitForTransform);
-		return rtabmap::Transform();
-	}
 	try
 	{
 		geometry_msgs::msg::TransformStamped tmp;

rtabmap_examples/launch/depthai.launch.py

@@ -0,0 +1,72 @@
+# Requirements:
+#   A OAK-D camera
+#   Install depthai-ros package (https://github.com/luxonis/depthai-ros)
+# Example:
+#   $ ros2 launch rtabmap_examples depthai.launch.py camera_model:=OAK-D
+
+import os
+
+from ament_index_python.packages import get_package_share_directory
+
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from launch.actions import IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+
+
+def generate_launch_description():
+    parameters=[{'frame_id':'oak-d-base-frame',
+                 'subscribe_rgbd':True,
+                 'subscribe_odom_info':True,
+                 'approx_sync':False,
+                 'wait_imu_to_init':True}]
+
+    remappings=[('imu', '/imu/data')]
+
+    return LaunchDescription([
+
+        # Launch camera driver
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource([os.path.join(
+                get_package_share_directory('depthai_examples'), 'launch'),
+                '/stereo_inertial_node.launch.py']),
+                launch_arguments={'depth_aligned': 'false',
+                                  'enableRviz': 'false',
+                                  'monoResolution': '400p'}.items(),
+        ),
+
+        # Sync right/depth/camera_info together
+        Node(   
+            package='rtabmap_sync', executable='rgbd_sync', output='screen',
+            parameters=parameters,
+            remappings=[('rgb/image', '/right/image_rect'),
+                        ('rgb/camera_info', '/right/camera_info'),
+                        ('depth/image', '/stereo/depth')]),
+
+        # Compute quaternion of the IMU
+        Node(
+            package='imu_filter_madgwick', executable='imu_filter_madgwick_node', output='screen',
+            parameters=[{'use_mag': False, 
+                         'world_frame':'enu', 
+                         'publish_tf':False}],
+            remappings=[('imu/data_raw', '/imu')]),
+
+        # Visual odometry
+        Node(
+            package='rtabmap_odom', executable='rgbd_odometry', output='screen',
+            parameters=parameters,
+            remappings=remappings),
+
+        # VSLAM
+        Node(
+            package='rtabmap_slam', executable='rtabmap', output='screen',
+            parameters=parameters,
+            remappings=remappings,
+            arguments=['-d']),
+
+        # Visualization
+        Node(
+            package='rtabmap_viz', executable='rtabmap_viz', output='screen',
+            parameters=parameters,
+            remappings=remappings)
+    ])

rtabmap_examples/launch/realsense_d400.launch.py

@@ -2,16 +2,16 @@
 #   A realsense D400 series
 #   Install realsense2 ros2 package (make sure you have this patch: https://github.com/IntelRealSense/realsense-ros/issues/2564#issuecomment-1336288238)
 # Example:
-#   $ ros2 launch realsense2_camera rs_launch.py align_depth.enable:=true
-#
 #   $ ros2 launch rtabmap_examples realsense_d400.launch.py
-#   OR
-#   $ ros2 launch rtabmap_launch rtabmap.launch.py frame_id:=camera_link args:="-d" rgb_topic:=/camera/color/image_raw depth_topic:=/camera/aligned_depth_to_color/image_raw camera_info_topic:=/camera/color/camera_info approx_sync:=false
+
+import os
+
+from ament_index_python.packages import get_package_share_directory
 
 from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable
-from launch.substitutions import LaunchConfiguration
-from launch_ros.actions import Node
+from launch_ros.actions import Node, SetParameter
+from launch.actions import IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
 
 def generate_launch_description():
     parameters=[{
@@ -27,7 +27,18 @@ def generate_launch_description():
 
     return LaunchDescription([
 
-        # Nodes to launch
+        # Make sure IR emitter is enabled
+        SetParameter(name='depth_module.emitter_enabled', value=1),
+
+        # Launch camera driver
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource([os.path.join(
+                get_package_share_directory('realsense2_camera'), 'launch'),
+                '/rs_launch.py']),
+                launch_arguments={'align_depth.enable': 'true',
+                                  'rgb_camera.profile': '640x360x30'}.items(),
+        ),
+
         Node(
             package='rtabmap_odom', executable='rgbd_odometry', output='screen',
             parameters=parameters,

rtabmap_examples/launch/realsense_d435i_color.launch.py

@@ -6,10 +6,14 @@
 #
 #   $ ros2 launch rtabmap_examples realsense_d435i_color.launch.py
 
+import os
+
+from ament_index_python.packages import get_package_share_directory
+
 from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable
-from launch.substitutions import LaunchConfiguration
-from launch_ros.actions import Node
+from launch_ros.actions import Node, SetParameter
+from launch.actions import IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
 
 def generate_launch_description():
     parameters=[{
@@ -23,11 +27,26 @@ def generate_launch_description():
           ('imu', '/imu/data'),
           ('rgb/image', '/camera/color/image_raw'),
           ('rgb/camera_info', '/camera/color/camera_info'),
-          ('depth/image', '/camera/realigned_depth_to_color/image_raw')]
+          ('depth/image', '/camera/aligned_depth_to_color/image_raw')]
 
     return LaunchDescription([
 
-        # Nodes to launch       
+        # Make sure IR emitter is enabled
+        SetParameter(name='depth_module.emitter_enabled', value=1),
+
+        # Launch camera driver
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource([os.path.join(
+                get_package_share_directory('realsense2_camera'), 'launch'),
+                '/rs_launch.py']),
+                launch_arguments={'enable_gyro': 'true',
+                                  'enable_accel': 'true',
+                                  'unite_imu_method': '1',
+                                  'align_depth.enable': 'true',
+                                  'enable_sync': 'true',
+                                  'rgb_camera.profile': '640x360x30'}.items(),
+        ),
+
         Node(
             package='rtabmap_odom', executable='rgbd_odometry', output='screen',
             parameters=parameters,
@@ -43,26 +62,7 @@ def generate_launch_description():
             package='rtabmap_viz', executable='rtabmap_viz', output='screen',
             parameters=parameters,
             remappings=remappings),
-
-        # Because of this issue: https://github.com/IntelRealSense/realsense-ros/issues/2564
-        # Generate point cloud from not aligned depth
-        Node(
-            package='rtabmap_util', executable='point_cloud_xyz', output='screen',
-            parameters=[{'approx_sync':False}],
-            remappings=[('depth/image',       '/camera/depth/image_rect_raw'),
-                        ('depth/camera_info', '/camera/depth/camera_info'),
-                        ('cloud',             '/camera/cloud_from_depth')]),
-
-        # Generate aligned depth to color camera from the point cloud above       
-        Node(
-            package='rtabmap_util', executable='pointcloud_to_depthimage', output='screen',
-            parameters=[{ 'decimation':2,
-                          'fixed_frame_id':'camera_link',
-                          'fill_holes_size':1}],
-            remappings=[('camera_info', '/camera/color/camera_info'),
-                        ('cloud',       '/camera/cloud_from_depth'),
-                        ('image_raw',   '/camera/realigned_depth_to_color/image_raw')]),
-
+
         # Compute quaternion of the IMU
         Node(
             package='imu_filter_madgwick', executable='imu_filter_madgwick_node', output='screen',

rtabmap_examples/launch/realsense_d435i_infra.launch.py

@@ -2,15 +2,16 @@
 #   A realsense D435i
 #   Install realsense2 ros2 package (ros-$ROS_DISTRO-realsense2-camera)
 # Example:
-#   $ ros2 launch realsense2_camera rs_launch.py enable_gyro:=true enable_accel:=true unite_imu_method:=1 enable_infra1:=true enable_infra2:=true enable_sync:=true
-#   $ ros2 param set /camera/camera depth_module.emitter_enabled 0
-#
 #   $ ros2 launch rtabmap_examples realsense_d435i_infra.launch.py
 
+import os
+
+from ament_index_python.packages import get_package_share_directory
+
 from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable
-from launch.substitutions import LaunchConfiguration
-from launch_ros.actions import Node
+from launch_ros.actions import Node, SetParameter
+from launch.actions import IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
 
 def generate_launch_description():
     parameters=[{
@@ -28,7 +29,22 @@ def generate_launch_description():
 
     return LaunchDescription([
 
-        # Nodes to launch       
+        #Hack to disable IR emitter
+        SetParameter(name='depth_module.emitter_enabled', value=0),
+
+        # Launch camera driver
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource([os.path.join(
+                get_package_share_directory('realsense2_camera'), 'launch'),
+                '/rs_launch.py']),
+                launch_arguments={'enable_gyro': 'true',
+                                  'enable_accel': 'true',
+                                  'unite_imu_method': '1',
+                                  'enable_infra1': 'true',
+                                  'enable_infra2': 'true',
+                                  'enable_sync': 'true'}.items(),
+        ),
+
         Node(
             package='rtabmap_odom', executable='rgbd_odometry', output='screen',
             parameters=parameters,

rtabmap_examples/launch/realsense_d435i_stereo.launch.py

@@ -2,15 +2,16 @@
 #   A realsense D435i
 #   Install realsense2 ros2 package (ros-$ROS_DISTRO-realsense2-camera)
 # Example:
-#   $ ros2 launch realsense2_camera rs_launch.py enable_gyro:=true enable_accel:=true unite_imu_method:=1 enable_infra1:=true enable_infra2:=true enable_sync:=true
-#   $ ros2 param set /camera/camera depth_module.emitter_enabled 0
-#
 #   $ ros2 launch rtabmap_examples realsense_d435i_stereo.launch.py
 
+import os
+
+from ament_index_python.packages import get_package_share_directory
+
 from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable
-from launch.substitutions import LaunchConfiguration
-from launch_ros.actions import Node
+from launch_ros.actions import Node, SetParameter
+from launch.actions import IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
 
 def generate_launch_description():
     parameters=[{
@@ -28,7 +29,22 @@ def generate_launch_description():
 
     return LaunchDescription([
 
-        # Nodes to launch       
+        #Hack to disable IR emitter
+        SetParameter(name='depth_module.emitter_enabled', value=0),
+
+        # Launch camera driver
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource([os.path.join(
+                get_package_share_directory('realsense2_camera'), 'launch'),
+                '/rs_launch.py']),
+                launch_arguments={'enable_gyro': 'true',
+                                  'enable_accel': 'true',
+                                  'unite_imu_method': '1',
+                                  'enable_infra1': 'true',
+                                  'enable_infra2': 'true',
+                                  'enable_sync': 'true'}.items(),
+        ),
+
         Node(
             package='rtabmap_odom', executable='stereo_odometry', output='screen',
             parameters=parameters,

rtabmap_examples/launch/vlp16.launch.py

@@ -1,15 +1,16 @@
 # Example:
-#   $ ros2 launch velodyne_driver velodyne_driver_node-VLP16-launch.py
-#   $ ros2 launch velodyne_pointcloud velodyne_transform_node-VLP16-launch.py
-#
-#   SLAM:
 #   $ ros2 launch rtabmap_examples vlp16.launch.py
 
+import os
+
+from ament_index_python.packages import get_package_share_directory
 
 from launch import LaunchDescription
 from launch.actions import DeclareLaunchArgument
 from launch.substitutions import LaunchConfiguration
 from launch_ros.actions import Node
+from launch.actions import IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
 
 def generate_launch_description():
 
@@ -28,6 +29,17 @@ def generate_launch_description():
             description='Enable lidar deskewing'),
 
         # Nodes to launch
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource([os.path.join(
+                get_package_share_directory('velodyne_driver'), 'launch'),
+                '/velodyne_driver_node-VLP16-launch.py']),
+        ),
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource([os.path.join(
+                get_package_share_directory('velodyne_pointcloud'), 'launch'),
+                '/velodyne_transform_node-VLP16-launch.py']),
+        ),
+
         Node(
             package='rtabmap_odom', executable='icp_odometry', output='screen',
             parameters=[{
@@ -57,18 +69,7 @@ def generate_launch_description():
             remappings=[
               ('scan_cloud', '/velodyne_points')
             ]),
-
-        Node(
-            package='rtabmap_util', executable='point_cloud_assembler', output='screen',
-            parameters=[{
-              'max_clouds':10,
-              'fixed_frame_id':'',
-              'use_sim_time':use_sim_time,
-            }],
-            remappings=[
-              ('cloud', 'odom_filtered_input_scan')
-            ]),
-
+
         Node(
             package='rtabmap_slam', executable='rtabmap', output='screen',
             parameters=[{
@@ -102,7 +103,7 @@ def generate_launch_description():
               'Icp/CorrespondenceRatio': '0.2'
             }],
             remappings=[
-              ('scan_cloud', 'assembled_cloud')
+              ('scan_cloud', 'odom_filtered_input_scan')
             ],
             arguments=[
               '-d' # This will delete the previous database (~/.ros/rtabmap.db)