Fixing topic sync lagging and delay issues, more examples (depthai, zed) (#1206)

* Fixed odometry latency

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

(cherry picked from commit 097cab0667)

* 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 <[email protected]>

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,