From dac04faef0d1cec6ad907916e4c87bb4d91818b5 Mon Sep 17 00:00:00 2001
From: matlabbe <matlabbe@gmail.com>
Date: Wed, 4 Sep 2024 19:38:05 -0700
Subject: [PATCH] zed example: added use_zed_odometry option

---
 rtabmap_examples/launch/zed.launch.py | 22 ++++++++++++++++++----
 1 file changed, 18 insertions(+), 4 deletions(-)

diff --git a/rtabmap_examples/launch/zed.launch.py b/rtabmap_examples/launch/zed.launch.py
index 0b89cec5..9344867e 100644
--- a/rtabmap_examples/launch/zed.launch.py
+++ b/rtabmap_examples/launch/zed.launch.py
@@ -9,10 +9,12 @@ import os
 from ament_index_python.packages import get_package_share_directory
 
 from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
 from launch_ros.actions import Node
 from launch.actions import IncludeLaunchDescription
 from launch.substitutions import LaunchConfiguration
 from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.conditions import UnlessCondition
 
 import tempfile
 
@@ -26,25 +28,36 @@ def generate_launch_description():
                   "        general:\n"+
                   "            grab_resolution: 'VGA'")
 
-    camera_model = LaunchConfiguration('camera_model'),
+    camera_model = LaunchConfiguration('camera_model')
+    use_zed_odometry = LaunchConfiguration('use_zed_odometry')
 
     parameters=[{'frame_id':'zed_camera_link',
                  'subscribe_rgbd':True,
-                 'subscribe_odom_info':True,
+                 'subscribe_odom_info': not use_zed_odometry,
                  'approx_sync':False,
                  'wait_imu_to_init':True}]
 
     remappings=[('imu', '/zed/zed_node/imu/data')]
 
+    if use_zed_odometry:
+        remappings.append(('odom', '/zed/zed_node/odom'))
+        
     return LaunchDescription([
 
+        # Launch arguments
+        DeclareLaunchArgument(
+            'use_zed_odometry', default_value='false',
+            description='Use zed\'s computed odometry instead of using rtabmap\'s odometry.'),
+
         # Launch camera driver
         IncludeLaunchDescription(
             PythonLaunchDescriptionSource([os.path.join(
                 get_package_share_directory('zed_wrapper'), 'launch'),
                 '/zed_camera.launch.py']),
                 launch_arguments={'camera_model': camera_model,
-                                  'ros_params_override_path': zed_override_file.name}.items(),
+                                  'ros_params_override_path': zed_override_file.name,
+                                  'publish_tf': use_zed_odometry,
+                                  'publish_map_tf': 'false'}.items(),
         ),
 
         # Sync right/depth/camera_info together
@@ -58,8 +71,9 @@ def generate_launch_description():
         # Visual odometry
         Node(
             package='rtabmap_odom', executable='rgbd_odometry', output='screen',
+            condition=UnlessCondition(use_zed_odometry),
             parameters=parameters,
-            remappings=remappings),
+            remappings=remappings,),
 
         # VSLAM
         Node(