From d02e34ed90a1238d1a5d57a8a579d3fabecc8a39 Mon Sep 17 00:00:00 2001 From: zbrotherton Date: Fri, 3 Apr 2026 18:13:57 +0000 Subject: [PATCH 1/9] Add rtabmap yaml from ZED defaults --- src/igvc_slam/config/rtabmap.yaml | 69 +++++++++++++++++++++++++++++++ 1 file changed, 69 insertions(+) create mode 100644 src/igvc_slam/config/rtabmap.yaml diff --git a/src/igvc_slam/config/rtabmap.yaml b/src/igvc_slam/config/rtabmap.yaml new file mode 100644 index 00000000..3c6d2523 --- /dev/null +++ b/src/igvc_slam/config/rtabmap.yaml @@ -0,0 +1,69 @@ +# RTAB-Map configuration + +subscribe_depth: true +subscribe_rgbd: false +subscribe_stereo: false +subscribe_stereo: false +subscribe_scan: false +subscribe_scan_cloud: false +subscribe_user_data: false +subscribe_odom_info: false + +database_path: "~/.ros/rtabmap.db" +config_path: "~/.ros/rtabmap.cfg" + +frame_id: "base_link" +map_frame_id: "map" +odom_frame_id: "odom" # odometry from odom msg to have covariance - Remapped by launch file +odom_tf_angular_variance: 0.001 # If TF is used to get odometry, this is the default angular variance +odom_tf_linear_variance: 0.001 # If TF is used to get odometry, this is the default linear variance +tf_delay: 0.02 +publish_tf: false # Set to false if fusing different poses (map->odom) with UKF + +odom_sensor_sync: true +wait_for_transform_duration: 0.2 +approx_sync: true + +queue_size: 10 + +scan_normal_k: 0 + +Grid: + 3D: true + FlatObstacleDetected: true + MapFrameProjection: false + GroundIsObstacle: false + PreVoxelFiltering: true + RayTracing: true + FromDepth: true + NormalsSegmentation: false + CellSize: 0.05 + ClusterRadius: 0.1 + MinClusterSize: 3 + DepthDecimation: 1 + DepthRoiRatios: [0.0, 0.0, 0.0, 0.0] + FootprintHeight: 2.0 + FootprintLength: 0.18 + FootprintWidth: 0.18 + MaxGroundAngle: 30.0 + MinGroundHeight: -0.5 + MaxGroundHeight: -0.4 + MaxObstacleHeight: 0.1 + NoiseFilteringMinNeighbors: 5 + NoiseFilteringRadius: 0.1 + NormalK: 20 + RangeMin: 0.7 + RangeMax: 3.0 + +GridGlobal: + Eroded: false # Erode obstacle cells + FootprintRadius: 0.18 # Footprint radius (m) used to clear all obstacles under the graph + FullUpdate: true # When the graph is changed, the whole map will be reconstructed instead of moving individually each cells of the map. Also, data added to cache won't be released after updating the map. This process is longer but more robust to drift that would erase some parts of the map when it should not + MaxNodes: 0 # Maximum nodes assembled in the map starting from the last node (0=unlimited) + MinSize: 1.0 # Minimum map size (m) + OccupancyThr: 0.55 # Occupancy threshold (value between 0 and 1) + ProbClampingMax: 0.971 # Probability clamping maximum (value between 0 and 1) + ProbClampingMin: 0.1192 # Probability clamping minimum (value between 0 and 1) + ProbHit: 0.7 # Probability of a hit (value between 0.5 and 1) + ProbMiss: 0.4 # Probability of a miss (value between 0 and 0.5) + UpdateError: 0.01 # Graph changed detection error (m). Update map only if poses in new optimized graph have moved more than this value \ No newline at end of file From afe059eeb424dd346d84e293e87d430e56e9a554 Mon Sep 17 00:00:00 2001 From: zbrotherton Date: Fri, 3 Apr 2026 20:23:52 +0000 Subject: [PATCH 2/9] Add example files from ZED --- src/igvc_slam/launch/rtabmap.launch.py | 22 +------- src/igvc_slam/launch/sl_rtabmap.launch.xml | 61 ++++++++++++++++++++++ src/igvc_slam/launch/zed_rtabmap.launch | 56 ++++++++++++++++++++ 3 files changed, 118 insertions(+), 21 deletions(-) create mode 100644 src/igvc_slam/launch/sl_rtabmap.launch.xml create mode 100644 src/igvc_slam/launch/zed_rtabmap.launch diff --git a/src/igvc_slam/launch/rtabmap.launch.py b/src/igvc_slam/launch/rtabmap.launch.py index 958ad292..49984f29 100644 --- a/src/igvc_slam/launch/rtabmap.launch.py +++ b/src/igvc_slam/launch/rtabmap.launch.py @@ -59,27 +59,7 @@ def generate_launch_description(): }.items() ) - realsense = IncludeLaunchDescription( - PythonLaunchDescriptionSource(realsense_launch_path), - launch_arguments={ - 'enable_color' : 'true', - 'enable_depth' : 'true', - 'align_depth.enable' : 'true', - 'pointcloud.enable' : 'false', - 'enable_sync' : 'false', - 'unite_imu_method' : '2', - 'enable_gyro' : 'true', - 'enable_accel' : 'true', - 'color_fps' : '60', - 'depth_fps' : '60', - 'gyro_fps' : '200', - 'accel_fps' : '63', - 'publish_tf' : 'true' - }.items() - ) return LaunchDescription([ - imu_filter, - rtabmap, - realsense + rtabmap ]) diff --git a/src/igvc_slam/launch/sl_rtabmap.launch.xml b/src/igvc_slam/launch/sl_rtabmap.launch.xml new file mode 100644 index 00000000..d9971716 --- /dev/null +++ b/src/igvc_slam/launch/sl_rtabmap.launch.xml @@ -0,0 +1,61 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/igvc_slam/launch/zed_rtabmap.launch b/src/igvc_slam/launch/zed_rtabmap.launch new file mode 100644 index 00000000..3b6486ad --- /dev/null +++ b/src/igvc_slam/launch/zed_rtabmap.launch @@ -0,0 +1,56 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file From abce7dda34f05fcbe76e61992a7ab30826bb33ae Mon Sep 17 00:00:00 2001 From: EnzoRap117 Date: Fri, 3 Apr 2026 20:59:35 +0000 Subject: [PATCH 3/9] Include config in CMakeLists --- src/igvc_slam/CMakeLists.txt | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/igvc_slam/CMakeLists.txt b/src/igvc_slam/CMakeLists.txt index 97eea4ba..044be5fa 100644 --- a/src/igvc_slam/CMakeLists.txt +++ b/src/igvc_slam/CMakeLists.txt @@ -13,6 +13,11 @@ install( DESTINATION share/${PROJECT_NAME} ) +install( + DIRECTORY config + DESTINATION share/${PROJECT_NAME} +) + if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) # the following line skips the linter which checks for copyrights From 6a52e5b425a92382d9594a94d043cff9ce91f0b8 Mon Sep 17 00:00:00 2001 From: zbrotherton Date: Fri, 3 Apr 2026 20:59:52 +0000 Subject: [PATCH 4/9] Rename and relocate default launch files --- .../launch/{zed_rtabmap.launch => igvc_slam.launch} | 2 +- .../{sl_rtabmap.launch.xml => include/rtabmap.launch} | 9 --------- 2 files changed, 1 insertion(+), 10 deletions(-) rename src/igvc_slam/launch/{zed_rtabmap.launch => igvc_slam.launch} (97%) rename src/igvc_slam/launch/{sl_rtabmap.launch.xml => include/rtabmap.launch} (96%) diff --git a/src/igvc_slam/launch/zed_rtabmap.launch b/src/igvc_slam/launch/igvc_slam.launch similarity index 97% rename from src/igvc_slam/launch/zed_rtabmap.launch rename to src/igvc_slam/launch/igvc_slam.launch index 3b6486ad..48628ef5 100644 --- a/src/igvc_slam/launch/zed_rtabmap.launch +++ b/src/igvc_slam/launch/igvc_slam.launch @@ -41,7 +41,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - + diff --git a/src/igvc_slam/launch/sl_rtabmap.launch.xml b/src/igvc_slam/launch/include/rtabmap.launch similarity index 96% rename from src/igvc_slam/launch/sl_rtabmap.launch.xml rename to src/igvc_slam/launch/include/rtabmap.launch index d9971716..1f5b5140 100644 --- a/src/igvc_slam/launch/sl_rtabmap.launch.xml +++ b/src/igvc_slam/launch/include/rtabmap.launch @@ -14,17 +14,14 @@ - - @@ -36,7 +33,6 @@ - @@ -53,9 +49,4 @@ - - - - - From 53878661d2cf9ab8549b2becd2ffa494964c2ba5 Mon Sep 17 00:00:00 2001 From: zbrotherton Date: Fri, 3 Apr 2026 21:01:33 +0000 Subject: [PATCH 5/9] Delete existing rtab launches --- src/igvc_slam/launch/rtabmap.launch.py | 65 ---------------------- src/igvc_slam/launch/sim_rtabmap.launch.py | 49 ---------------- 2 files changed, 114 deletions(-) delete mode 100644 src/igvc_slam/launch/rtabmap.launch.py delete mode 100644 src/igvc_slam/launch/sim_rtabmap.launch.py diff --git a/src/igvc_slam/launch/rtabmap.launch.py b/src/igvc_slam/launch/rtabmap.launch.py deleted file mode 100644 index 49984f29..00000000 --- a/src/igvc_slam/launch/rtabmap.launch.py +++ /dev/null @@ -1,65 +0,0 @@ -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.actions import Node - -import xacro - -def generate_launch_description(): - rtabmap_package = get_package_share_directory('rtabmap_launch') - realsense_package = get_package_share_directory('realsense2_camera') - - rtabmap_launch_path = os.path.join(rtabmap_package, 'launch', 'rtabmap.launch.py') - realsense_launch_path = os.path.join(realsense_package, 'launch', 'rs_launch.py') - - imu_filter = Node( - package='imu_filter_madgwick', - executable='imu_filter_madgwick_node', - output='screen', - remappings=[ - ('/imu/data_raw', '/camera/camera/imu'), - ('imu/data', '/rtabmap/imu') - ], - parameters=[ - {'use_mag' : False}, - {'publish_tf' : True}, - {'fixed_frame' : 'camera_link'} - ] - ) - - rtabmap = IncludeLaunchDescription( - PythonLaunchDescriptionSource(rtabmap_launch_path), - launch_arguments={ - 'args' : '--delete_db_on_start', - 'depth_topic' : '/camera/camera/aligned_depth_to_color/image_raw', - 'rgb_topic' : '/camera/camera/color/image_raw', - 'camera_info_topic' : '/camera/camera/color/camera_info', - 'frame_id' : 'base_footprint', - 'publish_tf_odom' : 'true', - 'odom_topic' : '/odom', - 'odom_frame_id' : 'odom', - 'approx_sync' : 'true', - 'rgbd_sync' : 'true', - 'approx_rgbd_sync' : 'true', - 'subscribe_rgbd' : 'true', - 'visual_odometry' : 'true', - 'qos' : '1', - 'approx_sync_max_interval' : '0.01', - 'sync_queue_size' : '50', - 'topic_queue_size' : '50', - 'wait_for_transform' : '0.4', - 'imu_topic' : '/rtabmap/imu', - 'wait_imu_to_init' : 'true', - 'rtabmap_viz' : 'true', - 'map_topic' : '/map' - }.items() - ) - - - return LaunchDescription([ - rtabmap - ]) diff --git a/src/igvc_slam/launch/sim_rtabmap.launch.py b/src/igvc_slam/launch/sim_rtabmap.launch.py deleted file mode 100644 index 41b3991f..00000000 --- a/src/igvc_slam/launch/sim_rtabmap.launch.py +++ /dev/null @@ -1,49 +0,0 @@ -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.actions import Node - -def generate_launch_description(): - rtabmap_package = get_package_share_directory('rtabmap_launch') - - 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', - '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', - 'odom_topic' : '/odom', - 'odom_frame_id' : 'odom', - 'sync_queue_size' : '10', #TODO verify - 'topic_queue_size' : '10', #TODO verify - 'rgbd_sync' : 'true', - 'approx_rgbd_sync' : 'true', - 'subscribe_depth' : 'false', - 'subscribe_rgbd' : 'true', - 'visual_odometry' : 'true', - 'approx_sync_max_interval' : '0.01', - 'qos' : '1', - 'rviz' : 'true', - 'use_sim_time' : 'true', - 'wait_for_transform' : '0.4', - 'imu_topic' : '/rtabmap/imu', - 'wait_imu_to_init' : 'false', - 'map_topic' : '/map' - }.items() - ) - - return LaunchDescription([ - rtabmap - ]) \ No newline at end of file From 5a27ecb0cef7458b4cf3d372ca133683e353ae31 Mon Sep 17 00:00:00 2001 From: EnzoRap117 Date: Fri, 3 Apr 2026 21:09:22 +0000 Subject: [PATCH 6/9] regex skip fix --- .vscode/tasks.json | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/.vscode/tasks.json b/.vscode/tasks.json index cf9adc71..9648a074 100644 --- a/.vscode/tasks.json +++ b/.vscode/tasks.json @@ -6,7 +6,8 @@ "args": [ "build", "--symlink-install", - "--packages-skip-regex zed_*" + "--packages-skip-regex", + "zed_*" ], "problemMatcher": [ "$colcon-gcc" @@ -17,12 +18,14 @@ }, "label": "colcon: Colcon Build Release" }, + { "type": "colcon", "args": [ "build", "--symlink-install", - "--packages-skip-regex zed_*", + "--packages-skip-regex", + "zed_*", "--cmake-args", "-DCMAKE_BUILD_TYPE=RelWithDebInfo" ], @@ -38,7 +41,8 @@ "args": [ "build", "--symlink-install", - "--packages-skip-regex zed_*", + "--packages-skip-regex", + "zed_*", "--cmake-args", "-DCMAKE_BUILD_TYPE=Debug" ], From 650c718e3458fe3e395e6b61c6ebc1bf348648c6 Mon Sep 17 00:00:00 2001 From: EnzoRap117 Date: Fri, 3 Apr 2026 21:32:15 +0000 Subject: [PATCH 7/9] Fix the bringup file to use the new xml launch file, not working --- src/igvc_bringup/launch/bringup.launch.py | 25 ++++++++++++++--------- 1 file changed, 15 insertions(+), 10 deletions(-) diff --git a/src/igvc_bringup/launch/bringup.launch.py b/src/igvc_bringup/launch/bringup.launch.py index f2c046be..6d5bca44 100644 --- a/src/igvc_bringup/launch/bringup.launch.py +++ b/src/igvc_bringup/launch/bringup.launch.py @@ -1,8 +1,11 @@ -from launch import LaunchDescription +import os + +from ament_index_python import get_package_share_directory +from launch import LaunchDescription, LaunchDescriptionSource from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription from launch.conditions import IfCondition, UnlessCondition from launch.substitutions import LaunchConfiguration -from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.launch_description_sources import FrontendLaunchDescriptionSource, PythonLaunchDescriptionSource from launch_ros.substitutions import FindPackageShare @@ -10,6 +13,10 @@ def generate_launch_description(): use_sim = LaunchConfiguration('use_sim') use_mock_hardware = LaunchConfiguration('use_mock_hardware') + igvc_slam_package = get_package_share_directory('igvc_slam') + + igvc_slam_path = os.path.join(igvc_slam_package, 'launch', 'igvc_slam.launch') + return LaunchDescription([ # Launch Arguments DeclareLaunchArgument( @@ -70,22 +77,20 @@ def generate_launch_description(): # Real hardware IncludeLaunchDescription( - PythonLaunchDescriptionSource( + LaunchDescriptionSource( [FindPackageShare('igvc_hardware'), '/launch', '/hardware.launch.py'] ), condition = UnlessCondition(use_mock_hardware) ), - + + #---------------------------------------------------- + # SLAM IncludeLaunchDescription( - PythonLaunchDescriptionSource( - [FindPackageShare('igvc_slam'), - '/launch', - '/sim_rtabmap.launch.py'] - ), - condition = IfCondition(LaunchConfiguration('use_slam')) + FrontendLaunchDescriptionSource(igvc_slam_path) + # condition = IfCondition(LaunchConfiguration('use_slam')) ), # TODO Nav From 25f1ee3250a872d249a19aa0e43cac8b069609c4 Mon Sep 17 00:00:00 2001 From: EnzoRap117 Date: Fri, 3 Apr 2026 17:44:33 -0400 Subject: [PATCH 8/9] Removed comment --- src/igvc_bringup/launch/bringup.launch.py | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/igvc_bringup/launch/bringup.launch.py b/src/igvc_bringup/launch/bringup.launch.py index 6d5bca44..4f587042 100644 --- a/src/igvc_bringup/launch/bringup.launch.py +++ b/src/igvc_bringup/launch/bringup.launch.py @@ -85,8 +85,6 @@ def generate_launch_description(): condition = UnlessCondition(use_mock_hardware) ), - #---------------------------------------------------- - # SLAM IncludeLaunchDescription( FrontendLaunchDescriptionSource(igvc_slam_path) From 3971b396faca1fef1abd8359ac37da4b1e72529d Mon Sep 17 00:00:00 2001 From: zbrotherton Date: Fri, 3 Apr 2026 22:04:50 +0000 Subject: [PATCH 9/9] Modify slam bringup to call new XML launch --- src/igvc_bringup/launch/bringup.launch.py | 24 ++++++++++------------- 1 file changed, 10 insertions(+), 14 deletions(-) diff --git a/src/igvc_bringup/launch/bringup.launch.py b/src/igvc_bringup/launch/bringup.launch.py index 6d5bca44..4abb00c6 100644 --- a/src/igvc_bringup/launch/bringup.launch.py +++ b/src/igvc_bringup/launch/bringup.launch.py @@ -4,7 +4,7 @@ from launch import LaunchDescription, LaunchDescriptionSource from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription from launch.conditions import IfCondition, UnlessCondition -from launch.substitutions import LaunchConfiguration +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution from launch.launch_description_sources import FrontendLaunchDescriptionSource, PythonLaunchDescriptionSource from launch_ros.substitutions import FindPackageShare @@ -12,10 +12,6 @@ def generate_launch_description(): use_sim = LaunchConfiguration('use_sim') use_mock_hardware = LaunchConfiguration('use_mock_hardware') - - igvc_slam_package = get_package_share_directory('igvc_slam') - - igvc_slam_path = os.path.join(igvc_slam_package, 'launch', 'igvc_slam.launch') return LaunchDescription([ # Launch Arguments @@ -77,7 +73,7 @@ def generate_launch_description(): # Real hardware IncludeLaunchDescription( - LaunchDescriptionSource( + PythonLaunchDescriptionSource( [FindPackageShare('igvc_hardware'), '/launch', '/hardware.launch.py'] @@ -85,13 +81,13 @@ def generate_launch_description(): condition = UnlessCondition(use_mock_hardware) ), - #---------------------------------------------------- - # SLAM - IncludeLaunchDescription( - FrontendLaunchDescriptionSource(igvc_slam_path) - # condition = IfCondition(LaunchConfiguration('use_slam')) - ), - - # TODO Nav + IncludeLaunchDescription( + FrontendLaunchDescriptionSource( + [FindPackageShare('igvc_slam'), + '/launch', + '/igvc_slam.launch'] + ), + condition = IfCondition(LaunchConfiguration('use_slam')) + ) ])