From d02e34ed90a1238d1a5d57a8a579d3fabecc8a39 Mon Sep 17 00:00:00 2001 From: zbrotherton Date: Fri, 3 Apr 2026 18:13:57 +0000 Subject: [PATCH 01/14] 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 02/14] 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 03/14] 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 04/14] 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 05/14] 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 06/14] 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 07/14] 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 08/14] 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 09/14] 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')) + ) ]) From fcb1b1b6cdbcd86e45aa1ce1094db64f0bc4a40d Mon Sep 17 00:00:00 2001 From: EnzoRap117 Date: Tue, 7 Apr 2026 21:41:34 +0000 Subject: [PATCH 10/14] Update ROS1 xmls to work with ROS2 Co-authored-by: Zane Brotherton --- src/igvc_slam/config/rtabmap.yaml | 4 +++ src/igvc_slam/launch/igvc_slam.launch | 24 ++++++++------- src/igvc_slam/launch/include/rtabmap.launch | 34 ++++++++------------- 3 files changed, 29 insertions(+), 33 deletions(-) diff --git a/src/igvc_slam/config/rtabmap.yaml b/src/igvc_slam/config/rtabmap.yaml index 3c6d2523..27cfbe04 100644 --- a/src/igvc_slam/config/rtabmap.yaml +++ b/src/igvc_slam/config/rtabmap.yaml @@ -28,6 +28,10 @@ queue_size: 10 scan_normal_k: 0 +Mem: + IncrementalMemory: true + InitWithAllNodes: false + Grid: 3D: true FlatObstacleDetected: true diff --git a/src/igvc_slam/launch/igvc_slam.launch b/src/igvc_slam/launch/igvc_slam.launch index 48628ef5..bb0d58d2 100644 --- a/src/igvc_slam/launch/igvc_slam.launch +++ b/src/igvc_slam/launch/igvc_slam.launch @@ -17,6 +17,7 @@ THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. --> + @@ -28,9 +29,10 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - + + - + - - - - - - - + + + + + + + - + \ No newline at end of file diff --git a/src/igvc_slam/launch/include/rtabmap.launch b/src/igvc_slam/launch/include/rtabmap.launch index 1f5b5140..06603fc5 100644 --- a/src/igvc_slam/launch/include/rtabmap.launch +++ b/src/igvc_slam/launch/include/rtabmap.launch @@ -1,12 +1,6 @@ - - - - - - @@ -16,7 +10,7 @@ - + @@ -24,29 +18,25 @@ - - - - + + + + - + - - + + - - - + + + - + - - - - From 16475db0a1810612b1b17399e25d4c7d0c78ee0c Mon Sep 17 00:00:00 2001 From: EnzoRap117 Date: Tue, 7 Apr 2026 22:12:41 +0000 Subject: [PATCH 11/14] Separated left and right cameras --- .../urdf/sensor/camera.urdf.xacro | 62 +++++++++++++++++++ src/igvc_gazebo/config/gz_bridge.yaml | 26 ++++++++ src/igvc_slam/launch/include/rtabmap.launch | 8 +-- 3 files changed, 92 insertions(+), 4 deletions(-) diff --git a/src/igvc_description/urdf/sensor/camera.urdf.xacro b/src/igvc_description/urdf/sensor/camera.urdf.xacro index adb11647..4bf04ee8 100644 --- a/src/igvc_description/urdf/sensor/camera.urdf.xacro +++ b/src/igvc_description/urdf/sensor/camera.urdf.xacro @@ -72,4 +72,66 @@ oak/color + + + + + 1.51843645 + + 1280 + 720 + R8G8B8 + + + 0.05 + 20 + + + gaussian + + 0.0 + 0.007 + + oak_infra1_optical_frame + oak/camera_info + + 1 + 20 + true + oak/right + + + + + + + 1.51843645 + + 1280 + 720 + R8G8B8 + + + 0.05 + 20 + + + gaussian + + 0.0 + 0.007 + + oak_infra2_optical_frame + oak/camera_info + + 1 + 20 + true + oak/left + + \ No newline at end of file diff --git a/src/igvc_gazebo/config/gz_bridge.yaml b/src/igvc_gazebo/config/gz_bridge.yaml index c7aae016..2cae9438 100644 --- a/src/igvc_gazebo/config/gz_bridge.yaml +++ b/src/igvc_gazebo/config/gz_bridge.yaml @@ -15,6 +15,32 @@ # Depth Camera +- ros_topic_name: "zed/zed_node/left/image_rect_color" + gz_topic_name: "oak/left" + ros_type_name: "sensor_msgs/msg/Image" + gz_type_name: "gz.msgs.Image" + direction: "GZ_TO_ROS" + qos_profile: SENSOR_DATA + +- ros_topic_name: "zed/zed_node/right/image_rect_color" + gz_topic_name: "oak/right" + ros_type_name: "sensor_msgs/msg/Image" + gz_type_name: "gz.msgs.Image" + direction: "GZ_TO_ROS" + qos_profile: SENSOR_DATA + +- ros_topic_name: "zed/zed_node/left/camera_info" #TODO: verify this matches real ros topic + gz_topic_name: "oak/camera_info" + ros_type_name: "sensor_msgs/msg/CameraInfo" + gz_type_name: "gz.msgs.CameraInfo" + direction: GZ_TO_ROS + +- ros_topic_name: "zed/zed_node/right/camera_info" #TODO: verify this matches real ros topic + gz_topic_name: "oak/camera_info" + ros_type_name: "sensor_msgs/msg/CameraInfo" + gz_type_name: "gz.msgs.CameraInfo" + direction: GZ_TO_ROS + - ros_topic_name: "camera/camera/color/image_raw" gz_topic_name: "oak/color" ros_type_name: "sensor_msgs/msg/Image" diff --git a/src/igvc_slam/launch/include/rtabmap.launch b/src/igvc_slam/launch/include/rtabmap.launch index 06603fc5..3aed330b 100644 --- a/src/igvc_slam/launch/include/rtabmap.launch +++ b/src/igvc_slam/launch/include/rtabmap.launch @@ -11,10 +11,10 @@ - - - - + + + + From e1dc5f138c328fafce24f954450de5feaa178c38 Mon Sep 17 00:00:00 2001 From: EnzoRap117 Date: Tue, 7 Apr 2026 22:59:30 +0000 Subject: [PATCH 12/14] Format rtabmap.yaml so simulation uses it Co-authored-by: Zane Brotherton --- src/igvc_slam/config/rtabmap.yaml | 129 +++++++++++++++--------------- 1 file changed, 65 insertions(+), 64 deletions(-) diff --git a/src/igvc_slam/config/rtabmap.yaml b/src/igvc_slam/config/rtabmap.yaml index 27cfbe04..87ef263f 100644 --- a/src/igvc_slam/config/rtabmap.yaml +++ b/src/igvc_slam/config/rtabmap.yaml @@ -1,73 +1,74 @@ # RTAB-Map configuration +rtabmap: + ros__parameters: + 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 -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" -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 -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 -odom_sensor_sync: true -wait_for_transform_duration: 0.2 -approx_sync: true + queue_size: 10 -queue_size: 10 + scan_normal_k: 0 -scan_normal_k: 0 + Mem: + IncrementalMemory: true + InitWithAllNodes: false -Mem: - IncrementalMemory: true - InitWithAllNodes: false + 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 -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 + 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 11b10f0c47b093af3a996e74a3b6e050b6e49b61 Mon Sep 17 00:00:00 2001 From: EnzoRap117 Date: Fri, 10 Apr 2026 21:38:12 +0000 Subject: [PATCH 13/14] Changed parameters & task file --- .vscode/tasks.json | 2 +- src/igvc_slam/config/rtabmap.yaml | 3 +++ 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/.vscode/tasks.json b/.vscode/tasks.json index 0e9227e6..f0234b4d 100644 --- a/.vscode/tasks.json +++ b/.vscode/tasks.json @@ -74,7 +74,7 @@ { "label": "IGVC: Open RTAB", "type": "shell", - "command": "source install/setup.bash; ros2 launch igvc_slam sim_rtabmap.launch.py", + "command": "source install/setup.bash; ros2 launch igvc_slam igvc_slam.launch", "problemMatcher": [] }, { diff --git a/src/igvc_slam/config/rtabmap.yaml b/src/igvc_slam/config/rtabmap.yaml index 87ef263f..4fe429b1 100644 --- a/src/igvc_slam/config/rtabmap.yaml +++ b/src/igvc_slam/config/rtabmap.yaml @@ -1,6 +1,9 @@ # RTAB-Map configuration rtabmap: ros__parameters: + use_sim_time: true + + subscribe_depth: true subscribe_rgbd: false subscribe_stereo: false From 667d9b36e87076437b91c300a96f7a79f60802da Mon Sep 17 00:00:00 2001 From: EnzoRap117 Date: Sat, 11 Apr 2026 00:38:07 +0000 Subject: [PATCH 14/14] Some reformatting and stereo odom param file --- src/igvc_slam/config/rtabmap.yaml | 2 +- src/igvc_slam/config/stereo_odometry.yaml | 26 +++++++++++++++++++++ src/igvc_slam/launch/include/rtabmap.launch | 2 ++ 3 files changed, 29 insertions(+), 1 deletion(-) create mode 100644 src/igvc_slam/config/stereo_odometry.yaml diff --git a/src/igvc_slam/config/rtabmap.yaml b/src/igvc_slam/config/rtabmap.yaml index 4fe429b1..9421c7c5 100644 --- a/src/igvc_slam/config/rtabmap.yaml +++ b/src/igvc_slam/config/rtabmap.yaml @@ -1,8 +1,8 @@ # RTAB-Map configuration rtabmap: ros__parameters: + use_sim_time: true - subscribe_depth: true subscribe_rgbd: false diff --git a/src/igvc_slam/config/stereo_odometry.yaml b/src/igvc_slam/config/stereo_odometry.yaml new file mode 100644 index 00000000..de3cfc69 --- /dev/null +++ b/src/igvc_slam/config/stereo_odometry.yaml @@ -0,0 +1,26 @@ +stereo_odometry: + ros__parameters: + + subscribe_rgbd: false + + publish_tf: true + wait_imu_to_init: false + always_check_imu_tf: false + + frame_id: "base_link" + odom_frame_id: "odom" + ground_truth_frame_id: "" + ground_truth_base_frame_id: "" + guess_frame_id: "" + config_path: "~/.ros/rtabmap.cfg" #unverified, copied from rtabmap.yaml + + wait_for_transform: 0.2 + approx_sync: false + approx_sync_max_interval: 0.0 + topic_queue_size: 10 + sync_queue_size: 10 + qos: 1 + qos_camera_info: 1 + qos_imu: 1 + guess_min_translation: 0.0 + guess_min_rotation: 0.0 \ No newline at end of file diff --git a/src/igvc_slam/launch/include/rtabmap.launch b/src/igvc_slam/launch/include/rtabmap.launch index 3aed330b..dd422cf4 100644 --- a/src/igvc_slam/launch/include/rtabmap.launch +++ b/src/igvc_slam/launch/include/rtabmap.launch @@ -11,6 +11,8 @@ + +