From 448c6c8d3853ee03c75754b61278e94a90577250 Mon Sep 17 00:00:00 2001 From: EnzoRap117 Date: Tue, 24 Feb 2026 21:37:09 +0000 Subject: [PATCH 01/11] Test Commit --- src/igvc_slam/launch/rtabmap.launch.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/igvc_slam/launch/rtabmap.launch.py b/src/igvc_slam/launch/rtabmap.launch.py index 958ad292..bdedac09 100644 --- a/src/igvc_slam/launch/rtabmap.launch.py +++ b/src/igvc_slam/launch/rtabmap.launch.py @@ -4,7 +4,7 @@ from launch import LaunchDescription from launch.actions import IncludeLaunchDescription -from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.launch_description_sources import PythonLaunchDescriptionSource #test from launch_ros.actions import Node import xacro From 0613b78116cf15146ab35fdc9bd719c83bc0f60c Mon Sep 17 00:00:00 2001 From: EnzoRap117 Date: Tue, 24 Feb 2026 23:59:10 +0000 Subject: [PATCH 02/11] added gps rtab launch parameters and began work on adding robot_localization --- src/igvc_slam/config/ekf.yaml | 49 ++++++++++++++++++++++ src/igvc_slam/launch/sim_rtabmap.launch.py | 7 +++- 2 files changed, 55 insertions(+), 1 deletion(-) create mode 100644 src/igvc_slam/config/ekf.yaml diff --git a/src/igvc_slam/config/ekf.yaml b/src/igvc_slam/config/ekf.yaml new file mode 100644 index 00000000..c7ee7dbb --- /dev/null +++ b/src/igvc_slam/config/ekf.yaml @@ -0,0 +1,49 @@ +# Note, this is copied stragith from + +### ekf config file ### +ekf_filter_node: + ros__parameters: +# The frequency, in Hz, at which the filter will output a position estimate. Note that the filter will not begin +# computation until it receives at least one message from one of the inputs. It will then run continuously at the +# frequency specified here, regardless of whether it receives more measurements. Defaults to 30 if unspecified. + frequency: 30.0 + +# ekf_localization_node and ukf_localization_node both use a 3D omnidirectional motion model. If this parameter is +# set to true, no 3D information will be used in your state estimate. Use this if you are operating in a planar +# environment and want to ignore the effect of small variations in the ground plane that might otherwise be detected +# by, for example, an IMU. Defaults to false if unspecified. + two_d_mode: false + +# Whether to publish the acceleration state. Defaults to false if unspecified. + publish_acceleration: true + +# Whether to broadcast the transformation over the /tf topic. Defaults to true if unspecified. + publish_tf: true + +# 1. Set the map_frame, odom_frame, and base_link frames to the appropriate frame names for your system. +# 1a. If your system does not have a map_frame, just remove it, and make sure "world_frame" is set to the value of odom_frame. +# 2. If you are fusing continuous position data such as wheel encoder odometry, visual odometry, or IMU data, set "world_frame" +# to your odom_frame value. This is the default behavior for robot_localization's state estimation nodes. +# 3. If you are fusing global absolute position data that is subject to discrete jumps (e.g., GPS or position updates from landmark +# observations) then: +# 3a. Set your "world_frame" to your map_frame value +# 3b. MAKE SURE something else is generating the odom->base_link transform. Note that this can even be another state estimation node +# from robot_localization! However, that instance should *not* fuse the global data. + map_frame: map # Defaults to "map" if unspecified + odom_frame: odom # Defaults to "odom" if unspecified + base_link_frame: base_link # Defaults to "base_link" if unspecified + world_frame: odom # Defaults to the value of odom_frame if unspecified + + odom0: demo/odom + odom0_config: [false, false, false, + false, false, false, + true, true, false, + false, false, true, + false, false, false] + + imu0: demo/imu + imu0_config: [false, false, false, + false, false, false, + false, false, false, + false, false, true, + false, false, false] \ No newline at end of file diff --git a/src/igvc_slam/launch/sim_rtabmap.launch.py b/src/igvc_slam/launch/sim_rtabmap.launch.py index 41b3991f..9c0114c4 100644 --- a/src/igvc_slam/launch/sim_rtabmap.launch.py +++ b/src/igvc_slam/launch/sim_rtabmap.launch.py @@ -1,3 +1,5 @@ +# WORK FILE + import os from ament_index_python.packages import get_package_share_directory @@ -38,11 +40,14 @@ def generate_launch_description(): 'rviz' : 'true', 'use_sim_time' : 'true', 'wait_for_transform' : '0.4', - 'imu_topic' : '/rtabmap/imu', + 'imu_topic' : '/rtabmap/imu', # Placeholder path + 'gps_topic' : '/rtabmap/gps', # Placeholder path 'wait_imu_to_init' : 'false', 'map_topic' : '/map' + # More odometry arguments can be called }.items() ) + return LaunchDescription([ rtabmap From 92896dc8ad74bde1cf85a23546b227c2aa974f9a Mon Sep 17 00:00:00 2001 From: EnzoRap117 Date: Fri, 27 Feb 2026 21:46:03 +0000 Subject: [PATCH 03/11] Changes to add robot_localization node --- src/igvc_slam/launch/sim_rtabmap.launch.py | 16 ++++++++++++++-- 1 file changed, 14 insertions(+), 2 deletions(-) diff --git a/src/igvc_slam/launch/sim_rtabmap.launch.py b/src/igvc_slam/launch/sim_rtabmap.launch.py index 9c0114c4..119127d7 100644 --- a/src/igvc_slam/launch/sim_rtabmap.launch.py +++ b/src/igvc_slam/launch/sim_rtabmap.launch.py @@ -7,11 +7,13 @@ from launch import LaunchDescription from launch.actions import IncludeLaunchDescription from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_ros.substitutions import FindPackageShare from launch_ros.actions import Node def generate_launch_description(): rtabmap_package = get_package_share_directory('rtabmap_launch') + package_slam = FindPackageShare(package='igvc_slam').find('igvc_slam') rtabmap_launch_path = os.path.join(rtabmap_package, 'launch', 'rtabmap.launch.py') @@ -48,7 +50,17 @@ def generate_launch_description(): }.items() ) + robot_localization_node = Node( + package='robot_localization', + executable='ekf_node', + name='ekf_filter_node', + output='screen', + parameters=[os.path.join(package_slam, 'config/ekf.yaml'), {'use_sim_time': 'true'}] + ) return LaunchDescription([ - rtabmap - ]) \ No newline at end of file + rtabmap, + robot_localization_node + ]) + + \ No newline at end of file From e4c80f6bd353f9c87e64296d779ae191f34a4e52 Mon Sep 17 00:00:00 2001 From: EnzoRap117 Date: Fri, 27 Feb 2026 22:36:49 +0000 Subject: [PATCH 04/11] Attempt to add robot_localization --- src/igvc_slam/CMakeLists.txt | 1 + src/igvc_slam/config/ekf.yaml | 2 +- src/igvc_slam/launch/sim_rtabmap.launch.py | 6 ++++++ src/igvc_slam/package.xml | 1 + 4 files changed, 9 insertions(+), 1 deletion(-) diff --git a/src/igvc_slam/CMakeLists.txt b/src/igvc_slam/CMakeLists.txt index 97eea4ba..7457c8c3 100644 --- a/src/igvc_slam/CMakeLists.txt +++ b/src/igvc_slam/CMakeLists.txt @@ -10,6 +10,7 @@ find_package(ament_cmake REQUIRED) install( DIRECTORY launch + # DIRECTORY src launch rviz config # From robot_localization setup guide, likely unecessary DESTINATION share/${PROJECT_NAME} ) diff --git a/src/igvc_slam/config/ekf.yaml b/src/igvc_slam/config/ekf.yaml index c7ee7dbb..61033720 100644 --- a/src/igvc_slam/config/ekf.yaml +++ b/src/igvc_slam/config/ekf.yaml @@ -1,4 +1,4 @@ -# Note, this is copied stragith from +# Note, this is copied straight from the robot_localization documentation ### ekf config file ### ekf_filter_node: diff --git a/src/igvc_slam/launch/sim_rtabmap.launch.py b/src/igvc_slam/launch/sim_rtabmap.launch.py index 119127d7..f739d1cb 100644 --- a/src/igvc_slam/launch/sim_rtabmap.launch.py +++ b/src/igvc_slam/launch/sim_rtabmap.launch.py @@ -57,8 +57,14 @@ def generate_launch_description(): output='screen', parameters=[os.path.join(package_slam, 'config/ekf.yaml'), {'use_sim_time': 'true'}] ) + return LaunchDescription([ + launch.actions.DeclareLaunchArgument( + name='use_sim_time', + default_value='True', + description='Flag to enable use_sim_time' + ), rtabmap, robot_localization_node ]) diff --git a/src/igvc_slam/package.xml b/src/igvc_slam/package.xml index 9dc74bc8..c62cb168 100644 --- a/src/igvc_slam/package.xml +++ b/src/igvc_slam/package.xml @@ -14,6 +14,7 @@ rtabmap_ros imu_filter_madgwick + robot_localization ament_cmake From bb11a04fd6cff6c29a31208bc5e8237ace3ea02d Mon Sep 17 00:00:00 2001 From: EnzoRap117 Date: Fri, 27 Feb 2026 23:04:41 +0000 Subject: [PATCH 05/11] Removed arguments causing sim errors from LaunchDescription() --- src/igvc_slam/launch/sim_rtabmap.launch.py | 5 ----- 1 file changed, 5 deletions(-) diff --git a/src/igvc_slam/launch/sim_rtabmap.launch.py b/src/igvc_slam/launch/sim_rtabmap.launch.py index f739d1cb..ac6d3073 100644 --- a/src/igvc_slam/launch/sim_rtabmap.launch.py +++ b/src/igvc_slam/launch/sim_rtabmap.launch.py @@ -60,11 +60,6 @@ def generate_launch_description(): return LaunchDescription([ - launch.actions.DeclareLaunchArgument( - name='use_sim_time', - default_value='True', - description='Flag to enable use_sim_time' - ), rtabmap, robot_localization_node ]) From b5e0e78a518a848c562a6db950c09c336f5085a2 Mon Sep 17 00:00:00 2001 From: EnzoRap117 Date: Fri, 27 Feb 2026 23:13:13 +0000 Subject: [PATCH 06/11] Removed unnecessary comments --- src/igvc_slam/CMakeLists.txt | 1 - src/igvc_slam/launch/rtabmap.launch.py | 2 +- 2 files changed, 1 insertion(+), 2 deletions(-) diff --git a/src/igvc_slam/CMakeLists.txt b/src/igvc_slam/CMakeLists.txt index 7457c8c3..97eea4ba 100644 --- a/src/igvc_slam/CMakeLists.txt +++ b/src/igvc_slam/CMakeLists.txt @@ -10,7 +10,6 @@ find_package(ament_cmake REQUIRED) install( DIRECTORY launch - # DIRECTORY src launch rviz config # From robot_localization setup guide, likely unecessary DESTINATION share/${PROJECT_NAME} ) diff --git a/src/igvc_slam/launch/rtabmap.launch.py b/src/igvc_slam/launch/rtabmap.launch.py index bdedac09..958ad292 100644 --- a/src/igvc_slam/launch/rtabmap.launch.py +++ b/src/igvc_slam/launch/rtabmap.launch.py @@ -4,7 +4,7 @@ from launch import LaunchDescription from launch.actions import IncludeLaunchDescription -from launch.launch_description_sources import PythonLaunchDescriptionSource #test +from launch.launch_description_sources import PythonLaunchDescriptionSource from launch_ros.actions import Node import xacro From d784b38ad864d938520434a5572494fc6e51766e Mon Sep 17 00:00:00 2001 From: EnzoRap117 Date: Tue, 10 Mar 2026 22:50:27 +0000 Subject: [PATCH 07/11] Ran into some troubles getting rviz to display properly. - Reorganized rtab launch arguments by category - Added the following arguments: 'icp_odometry', 'visual_odometry', 'subscribe_scan', 'scan_topic' --- src/igvc_slam/launch/sim_rtabmap.launch.py | 49 ++++++++++++++-------- 1 file changed, 31 insertions(+), 18 deletions(-) diff --git a/src/igvc_slam/launch/sim_rtabmap.launch.py b/src/igvc_slam/launch/sim_rtabmap.launch.py index ac6d3073..176e6a65 100644 --- a/src/igvc_slam/launch/sim_rtabmap.launch.py +++ b/src/igvc_slam/launch/sim_rtabmap.launch.py @@ -21,32 +21,45 @@ def generate_launch_description(): PythonLaunchDescriptionSource(rtabmap_launch_path), launch_arguments={ 'args' : '--delete_db_on_start', + # Topics 'depth_topic' : '/camera/camera/depth/image_rect_raw', 'rgb_topic' : '/camera/camera/color/image_raw', 'camera_info_topic' : '/camera/camera/depth/camera_info', - 'approx_sync' : 'true', - 'frame_id' : 'base_footprint', - 'log_level' : 'debug', - 'publish_tf_odom' : 'true', + 'imu_topic' : '/rtabmap/imu', # Placeholder path + 'gps_topic' : '/rtabmap/gps', # Placeholder path 'odom_topic' : '/odom', - 'odom_frame_id' : 'odom', - 'sync_queue_size' : '10', #TODO verify - 'topic_queue_size' : '10', #TODO verify + 'scan_topic' : '/scan', # LiDAR placeholder + + # Camera Arguments + 'frame_id' : 'base_footprint', 'rgbd_sync' : 'true', - 'approx_rgbd_sync' : 'true', - 'subscribe_depth' : 'false', - 'subscribe_rgbd' : 'true', - 'visual_odometry' : 'true', + 'approx_rgbd_sync' : 'true', 'approx_sync_max_interval' : '0.01', + 'subscribe_rgbd' : 'true', + 'subscribe_depth' : 'false', + + # Odometry Arguments + 'odom_frame_id' : 'odom', + 'publish_tf_odom' : 'true', + 'icp_odometry' : 'true', # Lidar Odom + 'visual_odometry' : 'true', # Visual Odom + 'imu_topic' : '/rtabmap/imu', + 'wait_imu_to_init' : 'false', + + # LiDAR Arguments + 'subscribe_scan' : 'true', + + + + # Misc. + 'log_level' : 'debug', + 'approx_sync' : 'true', 'qos' : '1', - 'rviz' : 'true', - 'use_sim_time' : 'true', + 'sync_queue_size' : '50', + 'topic_queue_size' : '50', 'wait_for_transform' : '0.4', - 'imu_topic' : '/rtabmap/imu', # Placeholder path - 'gps_topic' : '/rtabmap/gps', # Placeholder path - 'wait_imu_to_init' : 'false', - 'map_topic' : '/map' - # More odometry arguments can be called + 'rtabmap_viz' : 'true', + 'map_topic' : '/map', }.items() ) From b5a469bec7dec91930c39947119c6656e1b48814 Mon Sep 17 00:00:00 2001 From: EnzoRap117 Date: Fri, 13 Mar 2026 22:19:26 +0000 Subject: [PATCH 08/11] launch icp_odometry and stereo_odometry separately --- src/igvc_slam/config/ekf.yaml | 15 +++- src/igvc_slam/launch/sim_rtabmap.launch.py | 90 ++++++++++++++++++++-- 2 files changed, 95 insertions(+), 10 deletions(-) diff --git a/src/igvc_slam/config/ekf.yaml b/src/igvc_slam/config/ekf.yaml index 61033720..1b3da77d 100644 --- a/src/igvc_slam/config/ekf.yaml +++ b/src/igvc_slam/config/ekf.yaml @@ -1,4 +1,5 @@ # Note, this is copied straight from the robot_localization documentation +# This is a more detailed example of a ekf.yaml setup: https://github.com/cra-ros-pkg/robot_localization/blob/rolling-devel/params/ekf.yaml ### ekf config file ### ekf_filter_node: @@ -34,14 +35,24 @@ ekf_filter_node: base_link_frame: base_link # Defaults to "base_link" if unspecified world_frame: odom # Defaults to the value of odom_frame if unspecified - odom0: demo/odom + # Assumes icp_odometry is not publishing to odom + odom0: rtabmap/odom_icp odom0_config: [false, false, false, false, false, false, true, true, false, false, false, true, false, false, false] - imu0: demo/imu + # Assumes icp_odometry is not publishing to odom + odom1: rtabmap/odom_stereo + odom01_config: [false, false, false, + false, false, false, + true, true, false, + false, false, true, + false, false, false] + + # Note I'm not sure what topic our imu is + imu0: /imu imu0_config: [false, false, false, false, false, false, false, false, false, diff --git a/src/igvc_slam/launch/sim_rtabmap.launch.py b/src/igvc_slam/launch/sim_rtabmap.launch.py index 176e6a65..6155365d 100644 --- a/src/igvc_slam/launch/sim_rtabmap.launch.py +++ b/src/igvc_slam/launch/sim_rtabmap.launch.py @@ -27,8 +27,9 @@ def generate_launch_description(): 'camera_info_topic' : '/camera/camera/depth/camera_info', 'imu_topic' : '/rtabmap/imu', # Placeholder path 'gps_topic' : '/rtabmap/gps', # Placeholder path - 'odom_topic' : '/odom', + 'odom_topic' : '/odom', # Filtered odometry from robot_localization 'scan_topic' : '/scan', # LiDAR placeholder + 'map_topic' : '/map', # Camera Arguments 'frame_id' : 'base_footprint', @@ -41,15 +42,13 @@ def generate_launch_description(): # Odometry Arguments 'odom_frame_id' : 'odom', 'publish_tf_odom' : 'true', - 'icp_odometry' : 'true', # Lidar Odom - 'visual_odometry' : 'true', # Visual Odom + 'icp_odometry' : 'false', # Lidar Odom + 'visual_odometry' : 'false', # Visual Odom 'imu_topic' : '/rtabmap/imu', 'wait_imu_to_init' : 'false', # LiDAR Arguments - 'subscribe_scan' : 'true', - - + 'subscribe_scan' : 'true', # Misc. 'log_level' : 'debug', @@ -59,7 +58,6 @@ def generate_launch_description(): 'topic_queue_size' : '50', 'wait_for_transform' : '0.4', 'rtabmap_viz' : 'true', - 'map_topic' : '/map', }.items() ) @@ -69,10 +67,86 @@ def generate_launch_description(): name='ekf_filter_node', output='screen', parameters=[os.path.join(package_slam, 'config/ekf.yaml'), {'use_sim_time': 'true'}] - ) + ) + # Stereo odometry + + stereo_odom_node = Node( + package='rtabmap_odom', executable='stereo_odometry', name="stereo_odometry", output="screen", + emulate_tty=True, + condition=IfCondition(PythonExpression(["'", LaunchConfiguration('icp_odometry'), "' != 'true' and '", LaunchConfiguration('visual_odometry'), "' == 'true' and '", LaunchConfiguration('stereo'), "' == 'true'"])), + parameters=[{ + "frame_id": LaunchConfiguration('frame_id'), + "odom_frame_id": LaunchConfiguration('vo_frame_id'), + "publish_tf": LaunchConfiguration('publish_tf_odom'), + "ground_truth_frame_id": LaunchConfiguration('ground_truth_frame_id').perform(context), + "ground_truth_base_frame_id": LaunchConfiguration('ground_truth_base_frame_id').perform(context), + "wait_for_transform": LaunchConfiguration('wait_for_transform'), + "wait_imu_to_init": LaunchConfiguration('wait_imu_to_init'), + "always_check_imu_tf": LaunchConfiguration('always_check_imu_tf'), + "approx_sync": LaunchConfiguration('approx_sync'), + "approx_sync_max_interval": LaunchConfiguration('approx_sync_max_interval'), + "config_path": LaunchConfiguration('cfg').perform(context), + "topic_queue_size": LaunchConfiguration('topic_queue_size'), + "sync_queue_size": LaunchConfiguration('sync_queue_size'), + "qos": LaunchConfiguration('qos_image'), + "qos_camera_info": LaunchConfiguration('qos_camera_info'), + "qos_imu": LaunchConfiguration('qos_imu'), + "subscribe_rgbd": LaunchConfiguration('subscribe_rgbd'), + "guess_frame_id": LaunchConfiguration('odom_guess_frame_id').perform(context), + "guess_min_translation": LaunchConfiguration('odom_guess_min_translation'), + "guess_min_rotation": LaunchConfiguration('odom_guess_min_rotation')}], + remappings=[ + ("left/image_rect", LaunchConfiguration('left_image_topic_relay')), + ("right/image_rect", LaunchConfiguration('right_image_topic_relay')), + ("left/camera_info", LaunchConfiguration('left_camera_info_topic')), + ("right/camera_info", LaunchConfiguration('right_camera_info_topic')), + ("rgbd_image", LaunchConfiguration('rgbd_topic_relay')), + ("odom", LaunchConfiguration('odom_topic')), + ("imu", LaunchConfiguration('imu_topic'))], + arguments=[LaunchConfiguration("args"), LaunchConfiguration("odom_args"), "--ros-args", "--log-level", [LaunchConfiguration('namespace'), '.stereo_odometry:=', LaunchConfiguration('odom_log_level')], "--log-level", ['stereo_odometry:=', LaunchConfiguration('odom_log_level')]], + prefix=LaunchConfiguration('launch_prefix'), + namespace=LaunchConfiguration('namespace') + ), + + + # ICP odometry + icp_odom_node = Node( + package='rtabmap_odom', executable='icp_odometry', name="icp_odometry", output="screen", + emulate_tty=True, + condition=IfCondition(LaunchConfiguration('icp_odometry')), + parameters=[{ + "frame_id": LaunchConfiguration('frame_id'), + "odom_frame_id": LaunchConfiguration('vo_frame_id'), + "publish_tf": LaunchConfiguration('publish_tf_odom'), + "ground_truth_frame_id": LaunchConfiguration('ground_truth_frame_id').perform(context), + "ground_truth_base_frame_id": LaunchConfiguration('ground_truth_base_frame_id').perform(context), + "wait_for_transform": LaunchConfiguration('wait_for_transform'), + "wait_imu_to_init": LaunchConfiguration('wait_imu_to_init'), + "always_check_imu_tf": LaunchConfiguration('always_check_imu_tf'), + "approx_sync": LaunchConfiguration('approx_sync'), + "config_path": LaunchConfiguration('cfg').perform(context), + "topic_queue_size": LaunchConfiguration('topic_queue_size'), + "sync_queue_size": LaunchConfiguration('sync_queue_size'), + "qos": LaunchConfiguration('qos_image'), + "qos_imu": LaunchConfiguration('qos_imu'), + "guess_frame_id": LaunchConfiguration('odom_guess_frame_id').perform(context), + "guess_min_translation": LaunchConfiguration('odom_guess_min_translation'), + "guess_min_rotation": LaunchConfiguration('odom_guess_min_rotation')}], + remappings=[ + ("scan", LaunchConfiguration('scan_topic')), + ("scan_cloud", LaunchConfiguration('scan_cloud_topic')), + ("odom", LaunchConfiguration('odom_topic')), + ("imu", LaunchConfiguration('imu_topic'))], + arguments=[LaunchConfiguration("args"), LaunchConfiguration("odom_args"), "--ros-args", "--log-level", [LaunchConfiguration('namespace'), '.icp_odometry:=', LaunchConfiguration('odom_log_level')], "--log-level", ['icp_odometry:=', LaunchConfiguration('odom_log_level')]], + prefix=LaunchConfiguration('launch_prefix'), + namespace=LaunchConfiguration('namespace') + ), + return LaunchDescription([ + #stereo_odom_node, + #icp_odom_node, rtabmap, robot_localization_node ]) From fd36755c4bc92417fd3b368475a9fc8fe1665f45 Mon Sep 17 00:00:00 2001 From: EnzoRap117 Date: Tue, 17 Mar 2026 22:35:59 +0000 Subject: [PATCH 09/11] Changed icp_odom_node and icp_stereo_node - Still fails to launch simulation - Styled it off of rtabmap's launch arguments & robot localization - probably doesn't work at all --- src/igvc_slam/launch/sim_rtabmap.launch.py | 130 ++++++++++----------- 1 file changed, 63 insertions(+), 67 deletions(-) diff --git a/src/igvc_slam/launch/sim_rtabmap.launch.py b/src/igvc_slam/launch/sim_rtabmap.launch.py index 6155365d..04f9c65d 100644 --- a/src/igvc_slam/launch/sim_rtabmap.launch.py +++ b/src/igvc_slam/launch/sim_rtabmap.launch.py @@ -1,5 +1,3 @@ -# WORK FILE - import os from ament_index_python.packages import get_package_share_directory @@ -8,7 +6,6 @@ from launch.actions import IncludeLaunchDescription from launch.launch_description_sources import PythonLaunchDescriptionSource from launch_ros.substitutions import FindPackageShare - from launch_ros.actions import Node def generate_launch_description(): @@ -69,84 +66,83 @@ def generate_launch_description(): parameters=[os.path.join(package_slam, 'config/ekf.yaml'), {'use_sim_time': 'true'}] ) - # Stereo odometry - stereo_odom_node = Node( - package='rtabmap_odom', executable='stereo_odometry', name="stereo_odometry", output="screen", + package='rtabmap_odom', executable='stereo_odometry', name='stereo_odometry', output='screen', emulate_tty=True, - condition=IfCondition(PythonExpression(["'", LaunchConfiguration('icp_odometry'), "' != 'true' and '", LaunchConfiguration('visual_odometry'), "' == 'true' and '", LaunchConfiguration('stereo'), "' == 'true'"])), parameters=[{ - "frame_id": LaunchConfiguration('frame_id'), - "odom_frame_id": LaunchConfiguration('vo_frame_id'), - "publish_tf": LaunchConfiguration('publish_tf_odom'), - "ground_truth_frame_id": LaunchConfiguration('ground_truth_frame_id').perform(context), - "ground_truth_base_frame_id": LaunchConfiguration('ground_truth_base_frame_id').perform(context), - "wait_for_transform": LaunchConfiguration('wait_for_transform'), - "wait_imu_to_init": LaunchConfiguration('wait_imu_to_init'), - "always_check_imu_tf": LaunchConfiguration('always_check_imu_tf'), - "approx_sync": LaunchConfiguration('approx_sync'), - "approx_sync_max_interval": LaunchConfiguration('approx_sync_max_interval'), - "config_path": LaunchConfiguration('cfg').perform(context), - "topic_queue_size": LaunchConfiguration('topic_queue_size'), - "sync_queue_size": LaunchConfiguration('sync_queue_size'), - "qos": LaunchConfiguration('qos_image'), - "qos_camera_info": LaunchConfiguration('qos_camera_info'), - "qos_imu": LaunchConfiguration('qos_imu'), - "subscribe_rgbd": LaunchConfiguration('subscribe_rgbd'), - "guess_frame_id": LaunchConfiguration('odom_guess_frame_id').perform(context), - "guess_min_translation": LaunchConfiguration('odom_guess_min_translation'), - "guess_min_rotation": LaunchConfiguration('odom_guess_min_rotation')}], - remappings=[ - ("left/image_rect", LaunchConfiguration('left_image_topic_relay')), - ("right/image_rect", LaunchConfiguration('right_image_topic_relay')), - ("left/camera_info", LaunchConfiguration('left_camera_info_topic')), - ("right/camera_info", LaunchConfiguration('right_camera_info_topic')), - ("rgbd_image", LaunchConfiguration('rgbd_topic_relay')), - ("odom", LaunchConfiguration('odom_topic')), - ("imu", LaunchConfiguration('imu_topic'))], - arguments=[LaunchConfiguration("args"), LaunchConfiguration("odom_args"), "--ros-args", "--log-level", [LaunchConfiguration('namespace'), '.stereo_odometry:=', LaunchConfiguration('odom_log_level')], "--log-level", ['stereo_odometry:=', LaunchConfiguration('odom_log_level')]], - prefix=LaunchConfiguration('launch_prefix'), - namespace=LaunchConfiguration('namespace') + 'frame_id' : 'frame_id', + 'odom_frame_id' : 'vo_frame_id', + 'publish_tf' : 'publish_tf_odom', + 'ground_truth_frame_id' : 'ground_truth_frame_id', + 'ground_truth_base_frame_id' : 'ground_truth_base_frame_id', + 'wait_for_transform' : 'wait_for_transform', + 'wait_imu_to_init' : 'wait_imu_to_init', + 'always_check_imu_tf' : 'always_check_imu_tf', + 'approx_sync' : 'approx_sync', + 'approx_sync_max_interval' : 'approx_sync_max_interval', + 'config_path' : 'cfg', + 'topic_queue_size' : 'topic_queue_size', + 'sync_queue_size' : 'sync_queue_size', + 'qos' : 'qos_image', + 'qos_camera_info' : 'qos_camera_info', + 'qos_imu' : 'qos_imu', + 'subscribe_rgbd' : 'subscribe_rgbd', + 'guess_frame_id' : 'odom_guess_frame_id', + 'guess_min_translation' : 'odom_guess_min_translation', + 'guess_min_rotation' : 'odom_guess_min_rotation'}], + remappings=[{ + 'left/image_rect' : 'left_image_topic_relay', + 'right/image_rect' : 'right_image_topic_relay', + 'left/camera_info' : 'left_camera_info_topic', + 'right/camera_info' : 'right_camera_info_topic', + 'rgbd_image' : 'rgbd_topic_relay', + 'odom' : 'odom_stereo', + 'imu' : 'imu_topic'}], + arguments=['args', 'odom_args', "--ros-args", "--log-level", 'namespace', '.stereo_odometry:=', 'odom_log_level', "--log-level", 'stereo_odometry:=', 'odom_log_level'], + prefix= 'launch_prefix', + namespace= 'namespace' ), - # ICP odometry icp_odom_node = Node( package='rtabmap_odom', executable='icp_odometry', name="icp_odometry", output="screen", + name='icp_odometry_node', emulate_tty=True, - condition=IfCondition(LaunchConfiguration('icp_odometry')), parameters=[{ - "frame_id": LaunchConfiguration('frame_id'), - "odom_frame_id": LaunchConfiguration('vo_frame_id'), - "publish_tf": LaunchConfiguration('publish_tf_odom'), - "ground_truth_frame_id": LaunchConfiguration('ground_truth_frame_id').perform(context), - "ground_truth_base_frame_id": LaunchConfiguration('ground_truth_base_frame_id').perform(context), - "wait_for_transform": LaunchConfiguration('wait_for_transform'), - "wait_imu_to_init": LaunchConfiguration('wait_imu_to_init'), - "always_check_imu_tf": LaunchConfiguration('always_check_imu_tf'), - "approx_sync": LaunchConfiguration('approx_sync'), - "config_path": LaunchConfiguration('cfg').perform(context), - "topic_queue_size": LaunchConfiguration('topic_queue_size'), - "sync_queue_size": LaunchConfiguration('sync_queue_size'), - "qos": LaunchConfiguration('qos_image'), - "qos_imu": LaunchConfiguration('qos_imu'), - "guess_frame_id": LaunchConfiguration('odom_guess_frame_id').perform(context), - "guess_min_translation": LaunchConfiguration('odom_guess_min_translation'), - "guess_min_rotation": LaunchConfiguration('odom_guess_min_rotation')}], - remappings=[ - ("scan", LaunchConfiguration('scan_topic')), - ("scan_cloud", LaunchConfiguration('scan_cloud_topic')), - ("odom", LaunchConfiguration('odom_topic')), - ("imu", LaunchConfiguration('imu_topic'))], - arguments=[LaunchConfiguration("args"), LaunchConfiguration("odom_args"), "--ros-args", "--log-level", [LaunchConfiguration('namespace'), '.icp_odometry:=', LaunchConfiguration('odom_log_level')], "--log-level", ['icp_odometry:=', LaunchConfiguration('odom_log_level')]], - prefix=LaunchConfiguration('launch_prefix'), - namespace=LaunchConfiguration('namespace') + 'frame_id' : 'frame_id', + 'odom_frame_id' : 'vo_frame_id', + 'publish_tf' : 'publish_tf_odom', + 'ground_truth_frame_id' : 'ground_truth_frame_id', + 'ground_truth_base_frame_id' : 'ground_truth_base_frame_id', + 'wait_for_transform' : 'wait_for_transform', + 'wait_imu_to_init' : 'wait_imu_to_init', + 'always_check_imu_tf' : 'always_check_imu_tf', + 'approx_sync' : 'approx_sync', + 'approx_sync_max_interval' : 'approx_sync_max_interval', + 'config_path' : 'cfg', + 'topic_queue_size' : 'topic_queue_size', + 'sync_queue_size' : 'sync_queue_size', + 'qos' : 'qos_image', + 'qos_camera_info' : 'qos_camera_info', + 'qos_imu' : 'qos_imu', + 'subscribe_rgbd' : 'subscribe_rgbd', + 'guess_frame_id' : 'odom_guess_frame_id', + 'guess_min_translation' : 'odom_guess_min_translation', + 'guess_min_rotation' : 'odom_guess_min_rotation'}], + remappings=[{ + 'scan_topic' : '/scan', + 'scan_cloud_topic' : '/scan_cloud', + 'odom_topic' : '/odom_icp', + 'imu_topic' : '/imu'}], + arguments=['args', 'odom_args', "--ros-args", "--log-level", 'namespace', '.icp_odometry:=', 'odom_log_level'], + prefix= 'launch_prefix', + namespace= 'namespace' ), return LaunchDescription([ - #stereo_odom_node, - #icp_odom_node, + stereo_odom_node, + icp_odom_node, rtabmap, robot_localization_node ]) From bf1e022aeba544ac45012fdce314de52055e4a38 Mon Sep 17 00:00:00 2001 From: EnzoRap117 Date: Tue, 17 Mar 2026 22:42:39 +0000 Subject: [PATCH 10/11] Fixed inconsistency in stereo_odom_node --- src/igvc_slam/launch/sim_rtabmap.launch.py | 15 +++++++-------- 1 file changed, 7 insertions(+), 8 deletions(-) diff --git a/src/igvc_slam/launch/sim_rtabmap.launch.py b/src/igvc_slam/launch/sim_rtabmap.launch.py index 04f9c65d..de613667 100644 --- a/src/igvc_slam/launch/sim_rtabmap.launch.py +++ b/src/igvc_slam/launch/sim_rtabmap.launch.py @@ -1,7 +1,6 @@ import os from ament_index_python.packages import get_package_share_directory - from launch import LaunchDescription from launch.actions import IncludeLaunchDescription from launch.launch_description_sources import PythonLaunchDescriptionSource @@ -92,13 +91,13 @@ def generate_launch_description(): 'guess_min_translation' : 'odom_guess_min_translation', 'guess_min_rotation' : 'odom_guess_min_rotation'}], remappings=[{ - 'left/image_rect' : 'left_image_topic_relay', - 'right/image_rect' : 'right_image_topic_relay', - 'left/camera_info' : 'left_camera_info_topic', - 'right/camera_info' : 'right_camera_info_topic', - 'rgbd_image' : 'rgbd_topic_relay', - 'odom' : 'odom_stereo', - 'imu' : 'imu_topic'}], + 'left_image_topic_relay' : '/left/image_rect', + 'right_image_topic_relay' : '/right/image_rect', + 'left_camera_info_topic' : '/left/camera_info', + 'right_camera_info_topic' : '/right/camera_info', + 'rgbd_topic_relay' : '/rgbd_image', + 'odom_topic' : '/odom_stereo', + 'imu_topic' : '/imu'}], arguments=['args', 'odom_args', "--ros-args", "--log-level", 'namespace', '.stereo_odometry:=', 'odom_log_level', "--log-level", 'stereo_odometry:=', 'odom_log_level'], prefix= 'launch_prefix', namespace= 'namespace' From 6e1b34dc12e34a48198951e6f6d6fe1b4cb4b07f Mon Sep 17 00:00:00 2001 From: EnzoRap117 Date: Fri, 27 Mar 2026 21:36:40 +0000 Subject: [PATCH 11/11] Commit because git won't let me pull develop --- .devcontainer/Dockerfile | 2 +- src/igvc_slam/launch/sim_rtabmap.launch.py | 150 +++++++-------- .../launch/sim_rtabmap.launch_Mar16.py | 152 ++++++++++++++++ .../sim_rtabmap.launch_rtabmap_experiemnt.py | 171 ++++++++++++++++++ 4 files changed, 399 insertions(+), 76 deletions(-) create mode 100644 src/igvc_slam/launch/sim_rtabmap.launch_Mar16.py create mode 100644 src/igvc_slam/launch/sim_rtabmap.launch_rtabmap_experiemnt.py diff --git a/.devcontainer/Dockerfile b/.devcontainer/Dockerfile index ffe9e3f0..af94ea0f 100755 --- a/.devcontainer/Dockerfile +++ b/.devcontainer/Dockerfile @@ -31,7 +31,7 @@ ros-jazzy-rmw-cyclonedds-cpp \ # IMU Lib dependencies libxml2 libxml2-dev bison flex libcdk5-dev cmake libaio-dev libusb-1.0-0-dev libserialport-dev libxml2-dev libavahi-client-dev doxygen graphviz \ # External terminal -gnome-terminal dbus-x11 \ +gnome-terminal dbus-x11 # Camera stuff RUN if $DEPLOYABLE_BUILD ; then \ diff --git a/src/igvc_slam/launch/sim_rtabmap.launch.py b/src/igvc_slam/launch/sim_rtabmap.launch.py index de613667..42dca047 100644 --- a/src/igvc_slam/launch/sim_rtabmap.launch.py +++ b/src/igvc_slam/launch/sim_rtabmap.launch.py @@ -53,7 +53,7 @@ def generate_launch_description(): 'sync_queue_size' : '50', 'topic_queue_size' : '50', 'wait_for_transform' : '0.4', - 'rtabmap_viz' : 'true', + #'rtabmap_viz' : 'true', }.items() ) @@ -65,83 +65,83 @@ def generate_launch_description(): parameters=[os.path.join(package_slam, 'config/ekf.yaml'), {'use_sim_time': 'true'}] ) - # Stereo odometry - stereo_odom_node = Node( - package='rtabmap_odom', executable='stereo_odometry', name='stereo_odometry', output='screen', - emulate_tty=True, - parameters=[{ - 'frame_id' : 'frame_id', - 'odom_frame_id' : 'vo_frame_id', - 'publish_tf' : 'publish_tf_odom', - 'ground_truth_frame_id' : 'ground_truth_frame_id', - 'ground_truth_base_frame_id' : 'ground_truth_base_frame_id', - 'wait_for_transform' : 'wait_for_transform', - 'wait_imu_to_init' : 'wait_imu_to_init', - 'always_check_imu_tf' : 'always_check_imu_tf', - 'approx_sync' : 'approx_sync', - 'approx_sync_max_interval' : 'approx_sync_max_interval', - 'config_path' : 'cfg', - 'topic_queue_size' : 'topic_queue_size', - 'sync_queue_size' : 'sync_queue_size', - 'qos' : 'qos_image', - 'qos_camera_info' : 'qos_camera_info', - 'qos_imu' : 'qos_imu', - 'subscribe_rgbd' : 'subscribe_rgbd', - 'guess_frame_id' : 'odom_guess_frame_id', - 'guess_min_translation' : 'odom_guess_min_translation', - 'guess_min_rotation' : 'odom_guess_min_rotation'}], - remappings=[{ - 'left_image_topic_relay' : '/left/image_rect', - 'right_image_topic_relay' : '/right/image_rect', - 'left_camera_info_topic' : '/left/camera_info', - 'right_camera_info_topic' : '/right/camera_info', - 'rgbd_topic_relay' : '/rgbd_image', - 'odom_topic' : '/odom_stereo', - 'imu_topic' : '/imu'}], - arguments=['args', 'odom_args', "--ros-args", "--log-level", 'namespace', '.stereo_odometry:=', 'odom_log_level', "--log-level", 'stereo_odometry:=', 'odom_log_level'], - prefix= 'launch_prefix', - namespace= 'namespace' - ), + # # Stereo odometry + # stereo_odom_node = Node( + # package='rtabmap_odom', executable='stereo_odometry', name='stereo_odometry', output='screen', + # emulate_tty=True, + # parameters=[{ + # 'frame_id' : 'frame_id', + # 'odom_frame_id' : 'vo_frame_id', + # 'publish_tf' : 'publish_tf_odom', + # 'ground_truth_frame_id' : 'ground_truth_frame_id', + # 'ground_truth_base_frame_id' : 'ground_truth_base_frame_id', + # 'wait_for_transform' : 'wait_for_transform', + # 'wait_imu_to_init' : 'wait_imu_to_init', + # 'always_check_imu_tf' : 'always_check_imu_tf', + # 'approx_sync' : 'approx_sync', + # 'approx_sync_max_interval' : 'approx_sync_max_interval', + # 'config_path' : 'cfg', + # 'topic_queue_size' : 'topic_queue_size', + # 'sync_queue_size' : 'sync_queue_size', + # 'qos' : 'qos_image', + # 'qos_camera_info' : 'qos_camera_info', + # 'qos_imu' : 'qos_imu', + # 'subscribe_rgbd' : 'subscribe_rgbd', + # 'guess_frame_id' : 'odom_guess_frame_id', + # 'guess_min_translation' : 'odom_guess_min_translation', + # 'guess_min_rotation' : 'odom_guess_min_rotation'}], + # remappings=[{ + # 'left_image_topic_relay' : '/left/image_rect', + # 'right_image_topic_relay' : '/right/image_rect', + # 'left_camera_info_topic' : '/left/camera_info', + # 'right_camera_info_topic' : '/right/camera_info', + # 'rgbd_topic_relay' : '/rgbd_image', + # 'odom_topic' : '/odom_stereo', # Hopefully changes where its publishing + # 'imu_topic' : '/imu'}], + # arguments=['args', 'odom_args', "--ros-args", "--log-level", 'namespace', '.stereo_odometry:=', 'odom_log_level', "--log-level", 'stereo_odometry:=', 'odom_log_level'], + # prefix= 'launch_prefix', + # namespace= 'namespace' + # ), - # ICP odometry - icp_odom_node = Node( - package='rtabmap_odom', executable='icp_odometry', name="icp_odometry", output="screen", - name='icp_odometry_node', - emulate_tty=True, - parameters=[{ - 'frame_id' : 'frame_id', - 'odom_frame_id' : 'vo_frame_id', - 'publish_tf' : 'publish_tf_odom', - 'ground_truth_frame_id' : 'ground_truth_frame_id', - 'ground_truth_base_frame_id' : 'ground_truth_base_frame_id', - 'wait_for_transform' : 'wait_for_transform', - 'wait_imu_to_init' : 'wait_imu_to_init', - 'always_check_imu_tf' : 'always_check_imu_tf', - 'approx_sync' : 'approx_sync', - 'approx_sync_max_interval' : 'approx_sync_max_interval', - 'config_path' : 'cfg', - 'topic_queue_size' : 'topic_queue_size', - 'sync_queue_size' : 'sync_queue_size', - 'qos' : 'qos_image', - 'qos_camera_info' : 'qos_camera_info', - 'qos_imu' : 'qos_imu', - 'subscribe_rgbd' : 'subscribe_rgbd', - 'guess_frame_id' : 'odom_guess_frame_id', - 'guess_min_translation' : 'odom_guess_min_translation', - 'guess_min_rotation' : 'odom_guess_min_rotation'}], - remappings=[{ - 'scan_topic' : '/scan', - 'scan_cloud_topic' : '/scan_cloud', - 'odom_topic' : '/odom_icp', - 'imu_topic' : '/imu'}], - arguments=['args', 'odom_args', "--ros-args", "--log-level", 'namespace', '.icp_odometry:=', 'odom_log_level'], - prefix= 'launch_prefix', - namespace= 'namespace' - ), + # # ICP odometry + # icp_odom_node = Node( + # package='rtabmap_odom', executable='icp_odometry', name="icp_odometry", output="screen", + # name='icp_odometry_node', + # emulate_tty=True, + # parameters=[{ + # 'frame_id' : 'frame_id', + # 'odom_frame_id' : 'vo_frame_id', + # 'publish_tf' : 'publish_tf_odom', + # 'ground_truth_frame_id' : 'ground_truth_frame_id', + # 'ground_truth_base_frame_id' : 'ground_truth_base_frame_id', + # 'wait_for_transform' : 'wait_for_transform', + # 'wait_imu_to_init' : 'wait_imu_to_init', + # 'always_check_imu_tf' : 'always_check_imu_tf', + # 'approx_sync' : 'approx_sync', + # 'approx_sync_max_interval' : 'approx_sync_max_interval', + # 'config_path' : 'cfg', + # 'topic_queue_size' : 'topic_queue_size', + # 'sync_queue_size' : 'sync_queue_size', + # 'qos' : 'qos_image', + # 'qos_camera_info' : 'qos_camera_info', + # 'qos_imu' : 'qos_imu', + # 'subscribe_rgbd' : 'subscribe_rgbd', + # 'guess_frame_id' : 'odom_guess_frame_id', + # 'guess_min_translation' : 'odom_guess_min_translation', + # 'guess_min_rotation' : 'odom_guess_min_rotation'}], + # remappings=[{ + # 'scan_topic' : '/scan', + # 'scan_cloud_topic' : '/scan_cloud', + # 'odom_topic' : '/odom_icp', # Hopefully changes where its publishing + # 'imu_topic' : '/imu'}], + # arguments=['args', 'odom_args', "--ros-args", "--log-level", 'namespace', '.icp_odometry:=', 'odom_log_level'], + # prefix= 'launch_prefix', + # namespace= 'namespace' + # ), return LaunchDescription([ - stereo_odom_node, - icp_odom_node, + #stereo_odom_node, + #icp_odom_node, rtabmap, robot_localization_node ]) diff --git a/src/igvc_slam/launch/sim_rtabmap.launch_Mar16.py b/src/igvc_slam/launch/sim_rtabmap.launch_Mar16.py new file mode 100644 index 00000000..664ce56f --- /dev/null +++ b/src/igvc_slam/launch/sim_rtabmap.launch_Mar16.py @@ -0,0 +1,152 @@ +# WORK FILE + +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_ros.substitutions import FindPackageShare +from launch_ros.actions import Node + +def generate_launch_description(): + rtabmap_package = get_package_share_directory('rtabmap_launch') + package_slam = FindPackageShare(package='igvc_slam').find('igvc_slam') + + rtabmap_launch_path = os.path.join(rtabmap_package, 'launch', 'rtabmap.launch.py') + + rtabmap = IncludeLaunchDescription( + PythonLaunchDescriptionSource(rtabmap_launch_path), + launch_arguments={ + 'args' : '--delete_db_on_start', + # Topics + 'depth_topic' : '/camera/camera/depth/image_rect_raw', + 'rgb_topic' : '/camera/camera/color/image_raw', + 'camera_info_topic' : '/camera/camera/depth/camera_info', + 'imu_topic' : '/rtabmap/imu', # Placeholder path + 'gps_topic' : '/rtabmap/gps', # Placeholder path + 'odom_topic' : '/odom', # Filtered odometry from robot_localization + 'scan_topic' : '/scan', # LiDAR placeholder + 'map_topic' : '/map', + + # Camera Arguments + 'frame_id' : 'base_footprint', + 'rgbd_sync' : 'true', + 'approx_rgbd_sync' : 'true', + 'approx_sync_max_interval' : '0.01', + 'subscribe_rgbd' : 'true', + 'subscribe_depth' : 'false', + + # Odometry Arguments + 'odom_frame_id' : 'odom', + 'publish_tf_odom' : 'true', + 'icp_odometry' : 'false', # Lidar Odom + 'visual_odometry' : 'false', # Visual Odom + 'imu_topic' : '/rtabmap/imu', + 'wait_imu_to_init' : 'false', + + # LiDAR Arguments + 'subscribe_scan' : 'true', + + # Misc. + 'log_level' : 'debug', + 'approx_sync' : 'true', + 'qos' : '1', + 'sync_queue_size' : '50', + 'topic_queue_size' : '50', + 'wait_for_transform' : '0.4', + 'rtabmap_viz' : 'true', + }.items() + ) + + robot_localization_node = Node( + package='robot_localization', + executable='ekf_node', + name='ekf_filter_node', + output='screen', + parameters=[os.path.join(package_slam, 'config/ekf.yaml'), {'use_sim_time': 'true'}] + ) + + + # Stereo odometry + stereo_odom_node = Node( + package='rtabmap_odom', executable='stereo_odometry', name="stereo_odometry", output="screen", + emulate_tty=True, + condition=IfCondition(PythonExpression(["'", LaunchConfiguration('icp_odometry'), "' != 'true' and '", LaunchConfiguration('visual_odometry'), "' == 'true' and '", LaunchConfiguration('stereo'), "' == 'true'"])), + parameters=[{ + "frame_id": LaunchConfiguration('frame_id'), + "odom_frame_id": LaunchConfiguration('vo_frame_id'), + "publish_tf": LaunchConfiguration('publish_tf_odom'), + "ground_truth_frame_id": LaunchConfiguration('ground_truth_frame_id').perform(context), + "ground_truth_base_frame_id": LaunchConfiguration('ground_truth_base_frame_id').perform(context), + "wait_for_transform": LaunchConfiguration('wait_for_transform'), + "wait_imu_to_init": LaunchConfiguration('wait_imu_to_init'), + "always_check_imu_tf": LaunchConfiguration('always_check_imu_tf'), + "approx_sync": LaunchConfiguration('approx_sync'), + "approx_sync_max_interval": LaunchConfiguration('approx_sync_max_interval'), + "config_path": LaunchConfiguration('cfg').perform(context), + "topic_queue_size": LaunchConfiguration('topic_queue_size'), + "sync_queue_size": LaunchConfiguration('sync_queue_size'), + "qos": LaunchConfiguration('qos_image'), + "qos_camera_info": LaunchConfiguration('qos_camera_info'), + "qos_imu": LaunchConfiguration('qos_imu'), + "subscribe_rgbd": LaunchConfiguration('subscribe_rgbd'), + "guess_frame_id": LaunchConfiguration('odom_guess_frame_id').perform(context), + "guess_min_translation": LaunchConfiguration('odom_guess_min_translation'), + "guess_min_rotation": LaunchConfiguration('odom_guess_min_rotation')}], + remappings=[ + ("left/image_rect", LaunchConfiguration('left_image_topic_relay')), + ("right/image_rect", LaunchConfiguration('right_image_topic_relay')), + ("left/camera_info", LaunchConfiguration('left_camera_info_topic')), + ("right/camera_info", LaunchConfiguration('right_camera_info_topic')), + ("rgbd_image", LaunchConfiguration('rgbd_topic_relay')), + ("odom", LaunchConfiguration('odom_topic')), + ("imu", LaunchConfiguration('imu_topic'))], + arguments=[LaunchConfiguration("args"), LaunchConfiguration("odom_args"), "--ros-args", "--log-level", [LaunchConfiguration('namespace'), '.stereo_odometry:=', LaunchConfiguration('odom_log_level')], "--log-level", ['stereo_odometry:=', LaunchConfiguration('odom_log_level')]], + prefix=LaunchConfiguration('launch_prefix'), + namespace=LaunchConfiguration('namespace') + ), + + + # ICP odometry + icp_odom_node = Node( + package='rtabmap_odom', executable='icp_odometry', name="icp_odometry", output="screen", + emulate_tty=True, + condition=IfCondition(LaunchConfiguration('icp_odometry')), + parameters=[{ + "frame_id": LaunchConfiguration('frame_id'), + "odom_frame_id": LaunchConfiguration('vo_frame_id'), + "publish_tf": LaunchConfiguration('publish_tf_odom'), + "ground_truth_frame_id": LaunchConfiguration('ground_truth_frame_id').perform(context), + "ground_truth_base_frame_id": LaunchConfiguration('ground_truth_base_frame_id').perform(context), + "wait_for_transform": LaunchConfiguration('wait_for_transform'), + "wait_imu_to_init": LaunchConfiguration('wait_imu_to_init'), + "always_check_imu_tf": LaunchConfiguration('always_check_imu_tf'), + "approx_sync": LaunchConfiguration('approx_sync'), + "config_path": LaunchConfiguration('cfg').perform(context), + "topic_queue_size": LaunchConfiguration('topic_queue_size'), + "sync_queue_size": LaunchConfiguration('sync_queue_size'), + "qos": LaunchConfiguration('qos_image'), + "qos_imu": LaunchConfiguration('qos_imu'), + "guess_frame_id": LaunchConfiguration('odom_guess_frame_id').perform(context), + "guess_min_translation": LaunchConfiguration('odom_guess_min_translation'), + "guess_min_rotation": LaunchConfiguration('odom_guess_min_rotation')}], + remappings=[ + ("scan", LaunchConfiguration('scan_topic')), + ("scan_cloud", LaunchConfiguration('scan_cloud_topic')), + ("odom", LaunchConfiguration('odom_topic')), + ("imu", LaunchConfiguration('imu_topic'))], + arguments=[LaunchConfiguration("args"), LaunchConfiguration("odom_args"), "--ros-args", "--log-level", [LaunchConfiguration('namespace'), '.icp_odometry:=', LaunchConfiguration('odom_log_level')], "--log-level", ['icp_odometry:=', LaunchConfiguration('odom_log_level')]], + prefix=LaunchConfiguration('launch_prefix'), + namespace=LaunchConfiguration('namespace') + ), + + return LaunchDescription([ + #stereo_odom_node, + #icp_odom_node, + rtabmap, + robot_localization_node + ]) + + \ No newline at end of file diff --git a/src/igvc_slam/launch/sim_rtabmap.launch_rtabmap_experiemnt.py b/src/igvc_slam/launch/sim_rtabmap.launch_rtabmap_experiemnt.py new file mode 100644 index 00000000..08d9d67b --- /dev/null +++ b/src/igvc_slam/launch/sim_rtabmap.launch_rtabmap_experiemnt.py @@ -0,0 +1,171 @@ +# WORK FILE + +import os + +from ament_index_python.packages import get_package_share_directory + +#from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_ros.substitutions import FindPackageShare +from launch_ros.actions import Node + +# From rtabmap launch file + +from launch import LaunchDescription, Substitution, LaunchContext +from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable, LogInfo, OpaqueFunction +from launch.substitutions import LaunchConfiguration, ThisLaunchFileDir, PythonExpression +from launch.conditions import IfCondition, UnlessCondition +from launch_ros.actions import Node +from launch_ros.actions import SetParameter +from typing import Text +from ament_index_python.packages import get_package_share_directory + +def generate_launch_description(): + rtabmap_package = get_package_share_directory('rtabmap_launch') + package_slam = FindPackageShare(package='igvc_slam').find('igvc_slam') + + rtabmap_launch_path = os.path.join(rtabmap_package, 'launch', 'rtabmap.launch.py') + + rtabmap = IncludeLaunchDescription( + PythonLaunchDescriptionSource(rtabmap_launch_path), + launch_arguments={ + 'args' : '--delete_db_on_start', + # Topics + 'depth_topic' : '/camera/camera/depth/image_rect_raw', + 'rgb_topic' : '/camera/camera/color/image_raw', + 'camera_info_topic' : '/camera/camera/depth/camera_info', + 'imu_topic' : '/rtabmap/imu', # Placeholder path + 'gps_topic' : '/rtabmap/gps', # Placeholder path + 'odom_topic' : '/odom', # Filtered odometry from robot_localization + 'scan_topic' : '/scan', # LiDAR placeholder + 'map_topic' : '/map', + + # Camera Arguments + 'frame_id' : 'base_footprint', + 'rgbd_sync' : 'true', + 'approx_rgbd_sync' : 'true', + 'approx_sync_max_interval' : '0.01', + 'subscribe_rgbd' : 'true', + 'subscribe_depth' : 'false', + + # Odometry Arguments + 'odom_frame_id' : 'odom', + 'publish_tf_odom' : 'true', + 'icp_odometry' : 'false', # Lidar Odom, + 'visual_odometry' : 'false', # Visual Odom, published topic 'rgbd_odometry' + 'imu_topic' : '/rtabmap/imu', + 'wait_imu_to_init' : 'false', + + # LiDAR Arguments + 'subscribe_scan' : 'true', + + # Misc. + 'log_level' : 'debug', + 'approx_sync' : 'true', + 'qos' : '1', + 'sync_queue_size' : '50', + 'topic_queue_size' : '50', + 'wait_for_transform' : '0.4', + 'rtabmap_viz' : 'true', + }.items() + ) + + robot_localization_node = Node( + package='robot_localization', + executable='ekf_node', + name='ekf_filter_node', + output='screen', + parameters=[os.path.join(package_slam, 'config/ekf.yaml'), {'use_sim_time': 'true'}] + ) + + + # Stereo odometry + + # Stereo odometry + stereo_odom_node = Node( + package='rtabmap_odom', executable='stereo_odometry', name="stereo_odometry", output="screen", + emulate_tty=True, + condition=IfCondition(PythonExpression(["'", LaunchConfiguration('icp_odometry'), "' != 'true' and '", LaunchConfiguration('visual_odometry'), "' == 'true' and '", LaunchConfiguration('stereo'), "' == 'true'"])), + parameters=[{ + "frame_id": LaunchConfiguration('frame_id'), + "odom_frame_id": LaunchConfiguration('vo_frame_id'), + "publish_tf": LaunchConfiguration('publish_tf_odom'), + "ground_truth_frame_id": LaunchConfiguration('ground_truth_frame_id').perform(context), + "ground_truth_base_frame_id": LaunchConfiguration('ground_truth_base_frame_id').perform(context), + "wait_for_transform": LaunchConfiguration('wait_for_transform'), + "wait_imu_to_init": LaunchConfiguration('wait_imu_to_init'), + "always_check_imu_tf": LaunchConfiguration('always_check_imu_tf'), + "approx_sync": LaunchConfiguration('approx_sync'), + "approx_sync_max_interval": LaunchConfiguration('approx_sync_max_interval'), + "config_path": LaunchConfiguration('cfg').perform(context), + "topic_queue_size": LaunchConfiguration('topic_queue_size'), + "sync_queue_size": LaunchConfiguration('sync_queue_size'), + "qos": LaunchConfiguration('qos_image'), + "qos_camera_info": LaunchConfiguration('qos_camera_info'), + "qos_imu": LaunchConfiguration('qos_imu'), + "subscribe_rgbd": LaunchConfiguration('subscribe_rgbd'), + "guess_frame_id": LaunchConfiguration('odom_guess_frame_id').perform(context), + "guess_min_translation": LaunchConfiguration('odom_guess_min_translation'), + "guess_min_rotation": LaunchConfiguration('odom_guess_min_rotation')}], + remappings=[ + ("left/image_rect", LaunchConfiguration('left_image_topic_relay')), + ("right/image_rect", LaunchConfiguration('right_image_topic_relay')), + ("left/camera_info", LaunchConfiguration('left_camera_info_topic')), + ("right/camera_info", LaunchConfiguration('right_camera_info_topic')), + ("rgbd_image", LaunchConfiguration('rgbd_topic_relay')), + ("odom", LaunchConfiguration('odom_topic')), + ("imu", LaunchConfiguration('imu_topic'))], + arguments=[LaunchConfiguration("args"), LaunchConfiguration("odom_args"), "--ros-args", "--log-level", [LaunchConfiguration('namespace'), '.stereo_odometry:=', LaunchConfiguration('odom_log_level')], "--log-level", ['stereo_odometry:=', LaunchConfiguration('odom_log_level')]], + prefix=LaunchConfiguration('launch_prefix'), + namespace=LaunchConfiguration('namespace') + ), + + + # ICP odometry + icp_odom_node = Node( + package='rtabmap_odom', executable='icp_odometry', name="icp_odometry", output="screen", + emulate_tty=True, + condition=IfCondition(LaunchConfiguration('icp_odometry')), + parameters=[{ + "frame_id": LaunchConfiguration('frame_id'), + "odom_frame_id": LaunchConfiguration('vo_frame_id'), + "publish_tf": LaunchConfiguration('publish_tf_odom'), + "ground_truth_frame_id": LaunchConfiguration('ground_truth_frame_id').perform(context), + "ground_truth_base_frame_id": LaunchConfiguration('ground_truth_base_frame_id').perform(context), + "wait_for_transform": LaunchConfiguration('wait_for_transform'), + "wait_imu_to_init": LaunchConfiguration('wait_imu_to_init'), + "always_check_imu_tf": LaunchConfiguration('always_check_imu_tf'), + "approx_sync": LaunchConfiguration('approx_sync'), + "config_path": LaunchConfiguration('cfg').perform(context), + "topic_queue_size": LaunchConfiguration('topic_queue_size'), + "sync_queue_size": LaunchConfiguration('sync_queue_size'), + "qos": LaunchConfiguration('qos_image'), + "qos_imu": LaunchConfiguration('qos_imu'), + "guess_frame_id": LaunchConfiguration('odom_guess_frame_id').perform(context), + "guess_min_translation": LaunchConfiguration('odom_guess_min_translation'), + "guess_min_rotation": LaunchConfiguration('odom_guess_min_rotation')}], + remappings=[ + ("scan", LaunchConfiguration('scan_topic')), + ("scan_cloud", LaunchConfiguration('scan_cloud_topic')), + ("odom", LaunchConfiguration('odom_topic')), + ("imu", LaunchConfiguration('imu_topic'))], + arguments=[LaunchConfiguration("args"), LaunchConfiguration("odom_args"), "--ros-args", "--log-level", [LaunchConfiguration('namespace'), '.icp_odometry:=', LaunchConfiguration('odom_log_level')], "--log-level", ['icp_odometry:=', LaunchConfiguration('odom_log_level')]], + prefix=LaunchConfiguration('launch_prefix'), + namespace=LaunchConfiguration('namespace') + ), + + def perform(self, context: 'LaunchContext') -> Text: + if self.condition == True or self.condition == 'true' or self.condition == 'True': + return self.text_if + else: + return self.text_else + + return LaunchDescription([ + #stereo_odom_node, + #icp_odom_node, + rtabmap, + robot_localization_node + ]) + + \ No newline at end of file