From b0bfba2c777009c590985761333c91cd3826d15d Mon Sep 17 00:00:00 2001 From: Erik Caldwell Date: Fri, 14 Nov 2025 07:43:46 +0000 Subject: [PATCH 1/5] Add Lidar 2D SLAM EKF configuration and launch files --- .../config/ekf/lidar_2dslam_ekf_global.yaml | 143 ++++++++++++++++++ .../config/ekf/lidar_2dslam_ekf_local.yaml | 143 ++++++++++++++++++ src/Nav/localization/launch/ekf.launch.py | 28 ++-- .../launch/localization.launch.py | 12 +- .../slam_toolbox/slam_toolbox_params.yaml | 100 ++++++++++++ .../navigation/launch/lidar_2dslam.launch.py | 134 ++++++++++++++++ .../launch/ouster_composable.launch.py | 51 +++---- src/Nav/navigation/package.xml | 2 + 8 files changed, 575 insertions(+), 38 deletions(-) create mode 100644 src/Nav/localization/config/ekf/lidar_2dslam_ekf_global.yaml create mode 100644 src/Nav/localization/config/ekf/lidar_2dslam_ekf_local.yaml create mode 100644 src/Nav/navigation/config/slam_toolbox/slam_toolbox_params.yaml create mode 100644 src/Nav/navigation/launch/lidar_2dslam.launch.py diff --git a/src/Nav/localization/config/ekf/lidar_2dslam_ekf_global.yaml b/src/Nav/localization/config/ekf/lidar_2dslam_ekf_global.yaml new file mode 100644 index 00000000..550b2229 --- /dev/null +++ b/src/Nav/localization/config/ekf/lidar_2dslam_ekf_global.yaml @@ -0,0 +1,143 @@ +global_ekf: + ros__parameters: + frequency: 20.0 + # sensor_timeout: 20.0 + use_sim_time: false + + two_d_mode: false + + # transform_time_offset: 0.25 + # transform_timeout: 3.0 + + print_diagnostics: true + debug: false + + permit_corrected_publication: false + publish_acceleration: true + publish_tf: true + + map_frame: map + odom_frame: odom + base_link_frame: base_link + world_frame: map + + pose0: /2dslam_pose + pose0_config: [ + # position + true, true, false, + # orientation + false, false, true, + # lineal vel + false, false, false, + # angular vel + false, false, false, + # lineal acc + false, false, false, + ] + # pose0_queue_size: 2 + # pose0_nodelay: false + pose0_differential: false + pose0_relative: false + pose0_pose_rejection_threshold: 5.0 # Mahalanobis Distance + # pose0_twist_rejection_threshold: 1.0 + + odom0: /drive/odom + odom0_config: [ + # position + false, false, false, + # orientation + false, false, false, + # lineal vel + true, false, false, + # angular vel + false, false, false, + # lineal acc + false, false, false, + ] + # odom0_queue_size: 2 + # odom0_nodelay: false + odom0_differential: false + odom0_relative: false + # odom0_pose_rejection_threshold: 5.0 + # odom0_twist_rejection_threshold: 1.0 + + imu0: /zed/zed_node/imu/data + imu0_config: [ + # position + false, false, false, + # orientation + true, true, false, + # lineal vel + false, false, false, + # angular vel + false, false, true, + # lineal acc + false, false, false, + ] + # imu0_queue_size: 2 + # imu0_nodelay: false + imu0_differential: false + imu0_relative: false + imu0_pose_rejection_threshold: 5.0 # Mahalanobis Distance + # imu0_twist_rejection_threshold: 1.0 + + imu1: /zed/zed_node/imu/data + imu1_config: [ + # position + false, false, false, + # orientation + false, false, false, + # lineal vel + false, false, false, + # angular vel + false, false, false, + # lineal acc + false, false, false, + ] + # imu1_queue_size: 2 + # imu1_nodelay: false + imu1_differential: false + imu1_relative: true + imu1_pose_rejection_threshold: 5.0 # Mahalanobis Distance + # imu1_twist_rejection_threshold: 1.0 + + # use_control: false + # stamped_control: false + # control_timeout: 0.2 + # control_config: [true, false, false, false, false, true] + # acceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 3.4] + # deceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 4.5] + # acceleration_gains: [0.8, 0.0, 0.0, 0.0, 0.0, 0.9] + # deceleration_gains: [1.0, 0.0, 0.0, 0.0, 0.0, 1.0] + + process_noise_covariance: [0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.04, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.02, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.015] + + initial_estimate_covariance: [1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9] diff --git a/src/Nav/localization/config/ekf/lidar_2dslam_ekf_local.yaml b/src/Nav/localization/config/ekf/lidar_2dslam_ekf_local.yaml new file mode 100644 index 00000000..3a7876eb --- /dev/null +++ b/src/Nav/localization/config/ekf/lidar_2dslam_ekf_local.yaml @@ -0,0 +1,143 @@ +local_ekf: + ros__parameters: + frequency: 20.0 + # sensor_timeout: 20.0 + use_sim_time: false + + two_d_mode: false + + # transform_time_offset: 0.25 + # transform_timeout: 3.0 + + print_diagnostics: true + debug: false + + permit_corrected_publication: false + publish_acceleration: true + publish_tf: true + + map_frame: map + odom_frame: odom + base_link_frame: base_link + world_frame: odom + + pose0: /2dslam_pose + pose0_config: [ + # position + true, true, false, + # orientation + false, false, true, + # lineal vel + false, false, false, + # angular vel + false, false, false, + # lineal acc + false, false, false, + ] + # pose0_queue_size: 2 + # pose0_nodelay: false + pose0_differential: false + pose0_relative: false + pose0_pose_rejection_threshold: 5.0 # Mahalanobis Distance + # pose0_twist_rejection_threshold: 1.0 + + odom0: /drive/odom + odom0_config: [ + # position + false, false, false, + # orientation + false, false, false, + # lineal vel + true, false, false, + # angular vel + false, false, false, + # lineal acc + false, false, false, + ] + # odom0_queue_size: 2 + # odom0_nodelay: false + odom0_differential: false + odom0_relative: false + # odom0_pose_rejection_threshold: 5.0 + # odom0_twist_rejection_threshold: 1.0 + + imu0: /zed/zed_node/imu/data + imu0_config: [ + # position + false, false, false, + # orientation + true, true, false, + # lineal vel + false, false, false, + # angular vel + false, false, true, + # lineal acc + false, false, false, + ] + # imu0_queue_size: 2 + # imu0_nodelay: false + imu0_differential: false + imu0_relative: false + imu0_pose_rejection_threshold: 5.0 # Mahalanobis Distance + # imu0_twist_rejection_threshold: 1.0 + + imu1: /zed/zed_node/imu/data + imu1_config: [ + # position + false, false, false, + # orientation + false, false, false, + # lineal vel + false, false, false, + # angular vel + false, false, false, + # lineal acc + false, false, false, + ] + # imu1_queue_size: 2 + # imu1_nodelay: false + imu1_differential: false + imu1_relative: true + imu1_pose_rejection_threshold: 5.0 # Mahalanobis Distance + # imu1_twist_rejection_threshold: 1.0 + + # use_control: false + # stamped_control: false + # control_timeout: 0.2 + # control_config: [true, false, false, false, false, true] + # acceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 3.4] + # deceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 4.5] + # acceleration_gains: [0.8, 0.0, 0.0, 0.0, 0.0, 0.9] + # deceleration_gains: [1.0, 0.0, 0.0, 0.0, 0.0, 1.0] + + process_noise_covariance: [0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.04, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.02, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.015] + + initial_estimate_covariance: [1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9] diff --git a/src/Nav/localization/launch/ekf.launch.py b/src/Nav/localization/launch/ekf.launch.py index a2d53797..f838cc27 100644 --- a/src/Nav/localization/launch/ekf.launch.py +++ b/src/Nav/localization/launch/ekf.launch.py @@ -23,24 +23,32 @@ from launch.actions import DeclareLaunchArgument, OpaqueFunction from nav2_common.launch import RewrittenYaml -ekf_profiles = { +EKF_PROFILES = { "gps": { "local": "gps_ekf_local.yaml", "global": "gps_ekf_global.yaml", }, + "lidar_2dslam": { + "local": "lidar_2dslam_ekf_local.yaml", + "global": "lidar_2dslam_ekf_global.yaml", + }, } -default_profile = "gps" +DEFAULT_PROFILE = "gps" def launch_setup(context): - # This function is executed at launch time - + # Retrieve launch configuration values use_sim_time = LaunchConfiguration("use_sim_time").perform(context) profile = LaunchConfiguration("ekf_profile").perform(context) - if profile not in ekf_profiles: + # Set default profile if 'default' is selected + if profile in ("default", ""): + profile = DEFAULT_PROFILE + + # Check if the selected profile exists + if profile not in EKF_PROFILES: raise ValueError( - f"Unknown EKF profile: '{profile}'. Available profiles are: {list(ekf_profiles.keys())}" + f"Unknown EKF profile: '{profile}'. Available profiles are: {list(EKF_PROFILES.keys())}" ) # Load the EKF parameters based on the profile @@ -48,13 +56,13 @@ def launch_setup(context): get_package_share_directory("localization"), "config", "ekf", - ekf_profiles[profile]["local"], + EKF_PROFILES[profile]["local"], ) global_params_file = os.path.join( get_package_share_directory("localization"), "config", "ekf", - ekf_profiles[profile]["global"], + EKF_PROFILES[profile]["global"], ) print(f"Using EKF profile: {profile}") @@ -107,8 +115,8 @@ def generate_launch_description(): profile_cmd = DeclareLaunchArgument( "ekf_profile", - default_value=default_profile, - description="Select the EKF profile to use (gps, lidar)", + default_value=DEFAULT_PROFILE, + description="Select the EKF profile to use", ) return LaunchDescription( diff --git a/src/Nav/localization/launch/localization.launch.py b/src/Nav/localization/launch/localization.launch.py index be1c0721..e84ac0ef 100644 --- a/src/Nav/localization/launch/localization.launch.py +++ b/src/Nav/localization/launch/localization.launch.py @@ -20,6 +20,7 @@ def generate_launch_description(): launch_rviz = LaunchConfiguration("launch_rviz", default="False") launch_gps = LaunchConfiguration("launch_gps", default="True") launch_desc = LaunchConfiguration("launch_desc", default="True") + ekf_profile = LaunchConfiguration("ekf_profile", default="default") # Launch argument declarations use_sim_time_cmd = DeclareLaunchArgument( @@ -36,6 +37,11 @@ def generate_launch_description(): launch_desc_cmd = DeclareLaunchArgument( "launch_desc", description="Launch description if True", default_value="True" ) + ekf_profile_cmd = DeclareLaunchArgument( + "ekf_profile", + description="Select the EKF profile to use", + default_value="default", + ) rviz_config = os.path.join( pkg_localization, "config", "rviz.views", "basicNavMap.rviz" @@ -74,7 +80,10 @@ def generate_launch_description(): PythonLaunchDescriptionSource( os.path.join(pkg_localization, "launch", "ekf.launch.py") ), - launch_arguments={"use_sim_time": use_sim_time}.items(), + launch_arguments={ + "use_sim_time": use_sim_time, + "ekf_profile": ekf_profile, + }.items(), ) config_dir = os.path.join(get_package_share_directory("localization"), "config") @@ -104,6 +113,7 @@ def generate_launch_description(): launch_rviz_cmd, launch_gps_cmd, launch_desc_cmd, + ekf_profile_cmd, rviz_cmd, gps_cmd, # slam_cmd, diff --git a/src/Nav/navigation/config/slam_toolbox/slam_toolbox_params.yaml b/src/Nav/navigation/config/slam_toolbox/slam_toolbox_params.yaml new file mode 100644 index 00000000..a9d0f9cb --- /dev/null +++ b/src/Nav/navigation/config/slam_toolbox/slam_toolbox_params.yaml @@ -0,0 +1,100 @@ +slam_toolbox: + ros__parameters: + # If these values don't work try the values here: https://github.com/medhansh-rath/rock_e/blob/438e4ae3d668c1247f21d733da635a91530f0656/config/mapper_params_online_async.yaml#L5 + + # ======================================== + # SOLVER CONFIGURATION + # ======================================== + # These control the optimization backend + solver_plugin: solver_plugins::CeresSolver + ceres_linear_solver: SPARSE_NORMAL_CHOLESKY # Good for medium-sized problems + ceres_preconditioner: SCHUR_JACOBI # Faster convergence for SLAM + ceres_trust_strategy: LEVENBERG_MARQUARDT # Robust optimization strategy + ceres_dogleg_type: TRADITIONAL_DOGLEG # Classical approach + ceres_loss_function: None # No robust loss function + + # ======================================== + # BASIC ROS PARAMETERS + # ======================================== + odom_frame: odom + map_frame: map + base_frame: base_link + scan_topic: /2d_ouster_scan + mode: mapping + + # Optional: For continuing existing maps + #map_file_name: your_map_name + #map_start_pose: [0.0, 0.0, 0.0] + #map_start_at_dock: true + + # ======================================== + # PERFORMANCE & TIMING PARAMETERS + # ======================================== + debug_logging: false + throttle_scans: 1 # Process every scan (10Hz is manageable) + transform_publish_period: 0.02 # 50Hz transform publishing + map_update_interval: 0.5 # Update map every 0.5 seconds + resolution: 0.03 # Fine resolution + max_laser_range: 20.0 # Match your sensor's max range + minimum_time_interval: 0.1 # Fast updates for 10Hz scans + transform_timeout: 0.2 + tf_buffer_duration: 30.0 + stack_size_to_use: 40000000 + enable_interactive_mode: true + + # ======================================== + # MOTION & KEYFRAME PARAMETERS + # ======================================== + # These control when new keyframes are created + use_scan_matching: true + use_scan_barycenter: true + minimum_travel_distance: 0.3 # Smaller for better detail (was 0.5) + minimum_travel_heading: 0.3 # Smaller angle threshold (was 0.5) + scan_buffer_size: 20 # Larger buffer for 10Hz (was 10) + scan_buffer_maximum_scan_distance: 20.0 # Match max range + + # ======================================== + # SCAN MATCHING PARAMETERS + # ======================================== + # Controls local scan-to-scan alignment + link_match_minimum_response_fine: 0.15 # Slightly higher for reliability + link_scan_maximum_distance: 2.0 # Increased for rover speeds + + # ======================================== + # LOOP CLOSURE PARAMETERS + # ======================================== + # Controls global loop detection and closure + loop_search_maximum_distance: 4.0 + do_loop_closing: true + loop_match_minimum_chain_size: 15 # More conservative (was 10) + loop_match_maximum_variance_coarse: 2.5 # Tighter constraint (was 3.0) + loop_match_minimum_response_coarse: 0.4 # Higher threshold (was 0.35) + loop_match_minimum_response_fine: 0.5 # Higher threshold (was 0.45) + + # ======================================== + # CORRELATION SEARCH PARAMETERS + # ======================================== + # Local scan matching search space + correlation_search_space_dimension: 0.8 # Larger search for rover movement + correlation_search_space_resolution: 0.01 # Fine resolution + correlation_search_space_smear_deviation: 0.1 + + # Loop closure search space + loop_search_space_dimension: 10.0 + loop_search_space_resolution: 0.05 + loop_search_space_smear_deviation: 0.03 + + # ======================================== + # PENALTY PARAMETERS + # ======================================== + # Control optimization weighting + distance_variance_penalty: 0.4 + angle_variance_penalty: 1.2 + + # Angular search parameters + fine_search_angle_offset: 0.012 # Match your angular resolution + coarse_search_angle_offset: 0.24 # 20x fine resolution + coarse_angle_resolution: 0.024 # 2x fine resolution + minimum_angle_penalty: 0.9 + minimum_distance_penalty: 0.5 + use_response_expansion: true \ No newline at end of file diff --git a/src/Nav/navigation/launch/lidar_2dslam.launch.py b/src/Nav/navigation/launch/lidar_2dslam.launch.py new file mode 100644 index 00000000..65d8e452 --- /dev/null +++ b/src/Nav/navigation/launch/lidar_2dslam.launch.py @@ -0,0 +1,134 @@ +import os +from pathlib import Path +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration +from launch.conditions import IfCondition +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode + + +def generate_launch_description(): + # Get package directories + pkg_navigation = get_package_share_directory("navigation") + pkg_localization = get_package_share_directory("localization") + + # Launch configurations + use_sim_time = LaunchConfiguration("use_sim_time") + slam_params_file = LaunchConfiguration("slam_params_file") + + # Declare launch arguments + declare_use_sim_time = DeclareLaunchArgument( + "use_sim_time", default_value="false", description="Use simulation time if true" + ) + + declare_slam_params_file = DeclareLaunchArgument( + "slam_params_file", + default_value=os.path.join( + pkg_navigation, "config", "slam_toolbox", "slam_toolbox_params.yaml" + ), + description="Path to slam_toolbox parameters file", + ) + + # Create a composable container for SLAM components + slam_container = ComposableNodeContainer( + name="slam_container", + namespace="", + package="rclcpp_components", + executable="component_container", + composable_node_descriptions=[ + # Pointcloud to LaserScan conversion + ComposableNode( + package="pointcloud_to_laserscan", + plugin="pointcloud_to_laserscan::PointCloudToLaserScanNode", + name="pointcloud_to_laserscan", + remappings=[ + ("cloud_in", "/ouster/points"), + ("scan", "/2d_ouster_scan"), + ], + parameters=[ + { + "target_frame": "lidar_link", + "transform_tolerance": 0.01, + "min_height": -0.3, # ouster is 0.425m above ground + "max_height": 0.5, + "angle_min": -3.14159, + "angle_max": 3.14159, + "angle_increment": 0.01227184630308513, # ouster in 512x10 mode so use 2*pi/512 + "scan_time": 0.1, # matches 10 Hz + "range_min": 1.5, # avoids accidently picking up the rover itself + "range_max": 20.0, + "use_inf": True, + "inf_epsilon": 1.0, + "use_sim_time": use_sim_time, + } + ], + ), + # SLAM Toolbox + ComposableNode( + package="slam_toolbox", + plugin="slam_toolbox::OnlineAsyncSlamToolbox", + name="slam_toolbox", + parameters=[ + slam_params_file, + { + "use_sim_time": use_sim_time, + "publish_tf": False, # False: Disable slam_toolbox publishing odom and map tf frames + }, + ], + remappings=[ + ("scan", "/2d_ouster_scan"), + ("pose", "/2dslam_pose"), + ], + ), + ], + output="screen", + ) + + # Launch Ouster with the SLAM container as target + ouster_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(pkg_navigation, "launch", "ouster_composable.launch.py") + ), + launch_arguments={ + "target_container": "/slam_container", + "independent_container": "False", + }.items(), + ) + + # Launch SLAM launch file with localization disabled + slam_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(pkg_navigation, "launch", "slam.launch.py") + ), + launch_arguments={ + "use_sim_time": use_sim_time, + "launch_localization": "false", + "launch_ouster": "false", + }.items(), + ) + + # Launch localization with lidar_2dslam EKF profile + localization_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(pkg_localization, "launch", "localization.launch.py") + ), + launch_arguments={ + "use_sim_time": use_sim_time, + "ekf_profile": "lidar_2dslam", + }.items(), + ) + + return LaunchDescription( + [ + declare_use_sim_time, + declare_slam_params_file, + slam_container, + ouster_cmd, + slam_cmd, + localization_cmd, + ] + ) diff --git a/src/Nav/navigation/launch/ouster_composable.launch.py b/src/Nav/navigation/launch/ouster_composable.launch.py index 4cc89bc7..25fe47e5 100644 --- a/src/Nav/navigation/launch/ouster_composable.launch.py +++ b/src/Nav/navigation/launch/ouster_composable.launch.py @@ -1,38 +1,27 @@ # Copyright 2023 Ouster, Inc. -# -"""Launch ouster nodes using a composite container""" +"""Launch ouster nodes using a composite container.""" from pathlib import Path import launch from ament_index_python.packages import get_package_share_directory from launch_ros.actions import ComposableNodeContainer, LoadComposableNodes from launch_ros.descriptions import ComposableNode -from launch.actions import ( - DeclareLaunchArgument, - IncludeLaunchDescription, - LogInfo, -) -from launch.conditions import ( - IfCondition, - UnlessCondition, -) # Import UnlessCondition for clarity if preferred +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, LogInfo +from launch.conditions import IfCondition, UnlessCondition from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import ( - LaunchConfiguration, - PythonExpression, -) # Import PythonExpression +from launch.substitutions import LaunchConfiguration, PythonExpression import os def generate_launch_description(): """ - Generate launch description for running ouster_ros components in a single - process/container. + Generate a launch description for running ouster_ros components either + inside a specified target container or in an independent component container. """ ouster_ros_pkg_dir = get_package_share_directory("ouster_ros") - # Set params_file directly as requested, no longer a launch argument + # Parameter and mask file paths params_file = os.path.join( get_package_share_directory("navigation"), "config", @@ -46,6 +35,7 @@ def generate_launch_description(): "ouster_mask_combined_shifted.png", ) + # Launch arguments ouster_ns = LaunchConfiguration("ouster_ns") ouster_ns_arg = DeclareLaunchArgument("ouster_ns", default_value="ouster") @@ -55,15 +45,21 @@ def generate_launch_description(): auto_start = LaunchConfiguration("auto_start") auto_start_arg = DeclareLaunchArgument("auto_start", default_value="True") - # NEW: Declare launch argument for independent container independent_container = LaunchConfiguration("independent_container") independent_container_arg = DeclareLaunchArgument( "independent_container", default_value="False", - description="If true, launch Ouster nodes in their own container. Otherwise, load into /zed/zed_container.", + description="If true, launch Ouster nodes in their own container.", ) - # Define Composable Nodes for Ouster + target_container = LaunchConfiguration("target_container") + target_container_arg = DeclareLaunchArgument( + "target_container", + default_value="/zed/zed_container", + description="Container name to load Ouster composable nodes into when not launched independently.", + ) + + # Define the Ouster driver as a composable node os_driver = ComposableNode( package="ouster_ros", plugin="ouster_ros::OusterDriver", @@ -79,14 +75,14 @@ def generate_launch_description(): ], ) - # Load into an existing container (conditional) + # Load the composable node into an existing container when not independent load_ouster_nodes_into_zed_container = LoadComposableNodes( composable_node_descriptions=[os_driver], - target_container="/zed/zed_container", + target_container=target_container, condition=IfCondition(PythonExpression(["not ", independent_container])), ) - # Create a new container for Ouster (conditional) + # Create an independent component container for Ouster when requested ouster_container = ComposableNodeContainer( name="ouster_container", namespace="", @@ -97,7 +93,7 @@ def generate_launch_description(): output="screen", ) - # RViz Launch file inclusion + # Optional RViz launch inclusion rviz_launch_file_path = Path(ouster_ros_pkg_dir) / "launch" / "rviz.launch.py" rviz_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource([str(rviz_launch_file_path)]), @@ -109,9 +105,10 @@ def generate_launch_description(): ouster_ns_arg, rviz_enable_arg, auto_start_arg, - independent_container_arg, # Add the new argument + independent_container_arg, + target_container_arg, rviz_launch, load_ouster_nodes_into_zed_container, - ouster_container, # Add the new container + ouster_container, ] ) diff --git a/src/Nav/navigation/package.xml b/src/Nav/navigation/package.xml index 93e920ce..50798696 100644 --- a/src/Nav/navigation/package.xml +++ b/src/Nav/navigation/package.xml @@ -20,6 +20,8 @@ navigation2 nav2_smac_planner nav2_regulated_pure_pursuit_controller + pointcloud_to_laserscan + slam_toolbox ament_copyright ament_flake8 From 5976d720fed8697e926291ed12a41143690a0df8 Mon Sep 17 00:00:00 2001 From: Erik Caldwell Date: Fri, 14 Nov 2025 07:51:44 +0000 Subject: [PATCH 2/5] fix: dont launch gps indoors --- src/Nav/navigation/launch/lidar_2dslam.launch.py | 1 + 1 file changed, 1 insertion(+) diff --git a/src/Nav/navigation/launch/lidar_2dslam.launch.py b/src/Nav/navigation/launch/lidar_2dslam.launch.py index 65d8e452..95de3b18 100644 --- a/src/Nav/navigation/launch/lidar_2dslam.launch.py +++ b/src/Nav/navigation/launch/lidar_2dslam.launch.py @@ -119,6 +119,7 @@ def generate_launch_description(): launch_arguments={ "use_sim_time": use_sim_time, "ekf_profile": "lidar_2dslam", + "launch_gps": "false", }.items(), ) From 2eaad1aec1cde356eee9137c01d56922342f9481 Mon Sep 17 00:00:00 2001 From: Erik Caldwell Date: Fri, 14 Nov 2025 08:00:02 +0000 Subject: [PATCH 3/5] address copilot pr comments --- src/Nav/navigation/launch/lidar_2dslam.launch.py | 4 +--- src/Nav/navigation/launch/ouster_composable.launch.py | 4 ++-- 2 files changed, 3 insertions(+), 5 deletions(-) diff --git a/src/Nav/navigation/launch/lidar_2dslam.launch.py b/src/Nav/navigation/launch/lidar_2dslam.launch.py index 95de3b18..91646b7f 100644 --- a/src/Nav/navigation/launch/lidar_2dslam.launch.py +++ b/src/Nav/navigation/launch/lidar_2dslam.launch.py @@ -1,12 +1,10 @@ import os -from pathlib import Path from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration -from launch.conditions import IfCondition from launch_ros.actions import ComposableNodeContainer from launch_ros.descriptions import ComposableNode @@ -59,7 +57,7 @@ def generate_launch_description(): "angle_max": 3.14159, "angle_increment": 0.01227184630308513, # ouster in 512x10 mode so use 2*pi/512 "scan_time": 0.1, # matches 10 Hz - "range_min": 1.5, # avoids accidently picking up the rover itself + "range_min": 1.5, # avoids accidentally picking up the rover itself "range_max": 20.0, "use_inf": True, "inf_epsilon": 1.0, diff --git a/src/Nav/navigation/launch/ouster_composable.launch.py b/src/Nav/navigation/launch/ouster_composable.launch.py index 25fe47e5..b35737d7 100644 --- a/src/Nav/navigation/launch/ouster_composable.launch.py +++ b/src/Nav/navigation/launch/ouster_composable.launch.py @@ -7,8 +7,8 @@ from ament_index_python.packages import get_package_share_directory from launch_ros.actions import ComposableNodeContainer, LoadComposableNodes from launch_ros.descriptions import ComposableNode -from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, LogInfo -from launch.conditions import IfCondition, UnlessCondition +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.conditions import IfCondition from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration, PythonExpression import os From b5358a954f3fdb481cd7783565642979c222dbaa Mon Sep 17 00:00:00 2001 From: Jetson Date: Sat, 22 Nov 2025 23:46:37 +0000 Subject: [PATCH 4/5] Work on 2d lidar slam --- .../config/ekf/lidar_2dslam_ekf_global.yaml | 26 +++- .../config/ekf/lidar_2dslam_ekf_local.yaml | 26 +++- .../launch/localization.launch.py | 21 ++++ .../localization/initial_pose_publisher.py | 119 ++++++++++++++++++ src/Nav/localization/setup.py | 1 + .../config/ouster/driver_params.yaml | 2 +- .../slam_toolbox/slam_toolbox_params.yaml | 7 +- .../navigation/launch/lidar_2dslam.launch.py | 49 ++++---- .../launch/ouster_composable.launch.py | 2 +- 9 files changed, 215 insertions(+), 38 deletions(-) create mode 100644 src/Nav/localization/localization/initial_pose_publisher.py diff --git a/src/Nav/localization/config/ekf/lidar_2dslam_ekf_global.yaml b/src/Nav/localization/config/ekf/lidar_2dslam_ekf_global.yaml index 550b2229..f07984e8 100644 --- a/src/Nav/localization/config/ekf/lidar_2dslam_ekf_global.yaml +++ b/src/Nav/localization/config/ekf/lidar_2dslam_ekf_global.yaml @@ -21,7 +21,8 @@ global_ekf: base_link_frame: base_link world_frame: map - pose0: /2dslam_pose + # Initial pose to bootstrap the system (high covariance, only used at startup) + pose0: /ekf_initial_pose pose0_config: [ # position true, true, false, @@ -34,12 +35,27 @@ global_ekf: # lineal acc false, false, false, ] - # pose0_queue_size: 2 - # pose0_nodelay: false pose0_differential: false pose0_relative: false - pose0_pose_rejection_threshold: 5.0 # Mahalanobis Distance - # pose0_twist_rejection_threshold: 1.0 + pose0_pose_rejection_threshold: 100.0 # Very high - accept anything at startup + + # SLAM pose (will override initial pose once available) + pose1: /slam_2d_pose + pose1_config: [ + # position + true, true, false, + # orientation + false, false, true, + # lineal vel + false, false, false, + # angular vel + false, false, false, + # lineal acc + false, false, false, + ] + pose1_differential: false + pose1_relative: false + pose1_pose_rejection_threshold: 5.0 # Normal threshold for SLAM odom0: /drive/odom odom0_config: [ diff --git a/src/Nav/localization/config/ekf/lidar_2dslam_ekf_local.yaml b/src/Nav/localization/config/ekf/lidar_2dslam_ekf_local.yaml index 3a7876eb..d4ed6351 100644 --- a/src/Nav/localization/config/ekf/lidar_2dslam_ekf_local.yaml +++ b/src/Nav/localization/config/ekf/lidar_2dslam_ekf_local.yaml @@ -21,7 +21,8 @@ local_ekf: base_link_frame: base_link world_frame: odom - pose0: /2dslam_pose + # Initial pose to bootstrap the system (high covariance, only used at startup) + pose0: /ekf_initial_pose pose0_config: [ # position true, true, false, @@ -34,12 +35,27 @@ local_ekf: # lineal acc false, false, false, ] - # pose0_queue_size: 2 - # pose0_nodelay: false pose0_differential: false pose0_relative: false - pose0_pose_rejection_threshold: 5.0 # Mahalanobis Distance - # pose0_twist_rejection_threshold: 1.0 + pose0_pose_rejection_threshold: 100.0 # Very high - accept anything at startup + + # SLAM pose (will override initial pose once available) + pose1: /slam_2d_pose + pose1_config: [ + # position + true, true, false, + # orientation + false, false, true, + # lineal vel + false, false, false, + # angular vel + false, false, false, + # lineal acc + false, false, false, + ] + pose1_differential: false + pose1_relative: false + pose1_pose_rejection_threshold: 5.0 # Normal threshold for SLAM odom0: /drive/odom odom0_config: [ diff --git a/src/Nav/localization/launch/localization.launch.py b/src/Nav/localization/launch/localization.launch.py index e84ac0ef..d7c08139 100644 --- a/src/Nav/localization/launch/localization.launch.py +++ b/src/Nav/localization/launch/localization.launch.py @@ -106,6 +106,26 @@ def generate_launch_description(): arguments=["--ros-args", "--log-level", "Warn"], ) + # Initial pose publisher to bootstrap EKF and break circular dependency + initial_pose_node = launch_ros.actions.Node( + package="localization", + executable="initial_pose_publisher", + name="initial_pose_publisher", + output="screen", + parameters=[ + { + "use_sim_time": use_sim_time, + "frame_id": "map", + "publish_count": 5, + "publish_rate": 2.0, + "x": 0.0, + "y": 0.0, + "z": 0.0, + "yaw": 0.0, + } + ], + ) + # Assemble the LaunchDescription and return return LaunchDescription( [ @@ -117,6 +137,7 @@ def generate_launch_description(): rviz_cmd, gps_cmd, # slam_cmd, + initial_pose_node, # Publish initial pose first ekf_cmd, desc_cmd, navsat_node, diff --git a/src/Nav/localization/localization/initial_pose_publisher.py b/src/Nav/localization/localization/initial_pose_publisher.py new file mode 100644 index 00000000..afc80a5c --- /dev/null +++ b/src/Nav/localization/localization/initial_pose_publisher.py @@ -0,0 +1,119 @@ +#!/usr/bin/env python3 +""" +Initial Pose Publisher for breaking EKF circular dependency. + +This node publishes an initial origin pose at startup to allow the EKF to +begin publishing transforms. Once SLAM Toolbox starts providing poses, +it will naturally take over as the more accurate pose source. + +The pose is published only once (or a few times) at startup to bootstrap the system. +""" + +import rclpy +from rclpy.node import Node +from geometry_msgs.msg import PoseWithCovarianceStamped +from rclpy.qos import QoSProfile, DurabilityPolicy + + +class InitialPosePublisher(Node): + def __init__(self): + super().__init__("initial_pose_publisher") + + # Parameters + self.declare_parameter("frame_id", "map") + self.declare_parameter( + "publish_count", 5 + ) # Publish a few times to ensure receipt + self.declare_parameter("publish_rate", 1.0) # Hz + self.declare_parameter("x", 0.0) + self.declare_parameter("y", 0.0) + self.declare_parameter("z", 0.0) + self.declare_parameter("yaw", 0.0) # radians + + self.frame_id = self.get_parameter("frame_id").value + self.publish_count = self.get_parameter("publish_count").value + self.publish_rate = self.get_parameter("publish_rate").value + self.x = self.get_parameter("x").value + self.y = self.get_parameter("y").value + self.z = self.get_parameter("z").value + self.yaw = self.get_parameter("yaw").value + + # Publisher with latching-like behavior using transient local durability + qos = QoSProfile(depth=1, durability=DurabilityPolicy.TRANSIENT_LOCAL) + + self.pose_pub = self.create_publisher( + PoseWithCovarianceStamped, "/ekf_initial_pose", qos + ) + + self.count = 0 + self.timer = self.create_timer( + 1.0 / self.publish_rate, self.publish_initial_pose + ) + + self.get_logger().info( + f"Initial Pose Publisher started. Will publish origin pose {self.publish_count} times." + ) + + def publish_initial_pose(self): + """Publish an initial pose at the origin to bootstrap the EKF.""" + if self.count >= self.publish_count: + self.get_logger().info( + "Finished publishing initial poses. Node will continue running." + ) + self.timer.cancel() + return + + msg = PoseWithCovarianceStamped() + msg.header.stamp = self.get_clock().now().to_msg() + msg.header.frame_id = self.frame_id + + # Set position + msg.pose.pose.position.x = self.x + msg.pose.pose.position.y = self.y + msg.pose.pose.position.z = self.z + + # Set orientation (yaw only, roll and pitch = 0) + # Convert yaw to quaternion: qz = sin(yaw/2), qw = cos(yaw/2) + import math + + msg.pose.pose.orientation.x = 0.0 + msg.pose.pose.orientation.y = 0.0 + msg.pose.pose.orientation.z = math.sin(self.yaw / 2.0) + msg.pose.pose.orientation.w = math.cos(self.yaw / 2.0) + + # Set a high covariance to indicate low confidence + # This ensures SLAM will quickly override it with better estimates + # Format: [x, y, z, rot_x, rot_y, rot_z] + covariance = [0.0] * 36 + covariance[0] = 10.0 # x variance - high uncertainty + covariance[7] = 10.0 # y variance - high uncertainty + covariance[14] = 10.0 # z variance - high uncertainty + covariance[21] = 1.0 # roll variance + covariance[28] = 1.0 # pitch variance + covariance[35] = 1.0 # yaw variance - moderate uncertainty + msg.pose.covariance = covariance + + self.pose_pub.publish(msg) + self.count += 1 + + self.get_logger().info( + f"Published initial pose ({self.count}/{self.publish_count}): " + f"x={self.x:.2f}, y={self.y:.2f}, yaw={self.yaw:.2f} rad" + ) + + +def main(args=None): + rclpy.init(args=args) + node = InitialPosePublisher() + + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/src/Nav/localization/setup.py b/src/Nav/localization/setup.py index 5392dd42..34904b07 100644 --- a/src/Nav/localization/setup.py +++ b/src/Nav/localization/setup.py @@ -34,6 +34,7 @@ "imu_filter = localization.imu_filter:main", "repub_odom = localization.republish_odometry:main", "lidar_mask_tool = localization.lidar_mask_tool:main", + "initial_pose_publisher = localization.initial_pose_publisher:main", ], }, ) diff --git a/src/Nav/navigation/config/ouster/driver_params.yaml b/src/Nav/navigation/config/ouster/driver_params.yaml index 9fe9f304..3962c842 100644 --- a/src/Nav/navigation/config/ouster/driver_params.yaml +++ b/src/Nav/navigation/config/ouster/driver_params.yaml @@ -2,7 +2,7 @@ ros__parameters: # sensor_hostname[required]: hostname or IP address of the sensor (IP4 or # IP6). - sensor_hostname: 'os-122335000064.local' + sensor_hostname: '192.168.0.32' # mDNS lookups are bad within docker so 'os-122335000064.local' doesn't work # udp_dest[optional]: hostname or multicast group IP where the sensor will # send UDP data packets. udp_dest: '' diff --git a/src/Nav/navigation/config/slam_toolbox/slam_toolbox_params.yaml b/src/Nav/navigation/config/slam_toolbox/slam_toolbox_params.yaml index a9d0f9cb..a9ad6180 100644 --- a/src/Nav/navigation/config/slam_toolbox/slam_toolbox_params.yaml +++ b/src/Nav/navigation/config/slam_toolbox/slam_toolbox_params.yaml @@ -19,7 +19,7 @@ slam_toolbox: odom_frame: odom map_frame: map base_frame: base_link - scan_topic: /2d_ouster_scan + # scan_topic is handled by remapping in launch file mode: mapping # Optional: For continuing existing maps @@ -30,14 +30,15 @@ slam_toolbox: # ======================================== # PERFORMANCE & TIMING PARAMETERS # ======================================== - debug_logging: false + debug_logging: true # Enable to see what's happening throttle_scans: 1 # Process every scan (10Hz is manageable) transform_publish_period: 0.02 # 50Hz transform publishing map_update_interval: 0.5 # Update map every 0.5 seconds resolution: 0.03 # Fine resolution max_laser_range: 20.0 # Match your sensor's max range + min_laser_range: 1.5 # Match pointcloud_to_laserscan range_min minimum_time_interval: 0.1 # Fast updates for 10Hz scans - transform_timeout: 0.2 + transform_timeout: 2.0 # Further increased timeout for transform lookups tf_buffer_duration: 30.0 stack_size_to_use: 40000000 enable_interactive_mode: true diff --git a/src/Nav/navigation/launch/lidar_2dslam.launch.py b/src/Nav/navigation/launch/lidar_2dslam.launch.py index 91646b7f..1d26dcd4 100644 --- a/src/Nav/navigation/launch/lidar_2dslam.launch.py +++ b/src/Nav/navigation/launch/lidar_2dslam.launch.py @@ -5,7 +5,7 @@ from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration -from launch_ros.actions import ComposableNodeContainer +from launch_ros.actions import ComposableNodeContainer, Node from launch_ros.descriptions import ComposableNode @@ -31,7 +31,7 @@ def generate_launch_description(): description="Path to slam_toolbox parameters file", ) - # Create a composable container for SLAM components + # Create a composable container for pointcloud to laserscan conversion slam_container = ComposableNodeContainer( name="slam_container", namespace="", @@ -45,14 +45,14 @@ def generate_launch_description(): name="pointcloud_to_laserscan", remappings=[ ("cloud_in", "/ouster/points"), - ("scan", "/2d_ouster_scan"), + ("scan", "/ouster_2d_scan"), ], parameters=[ { "target_frame": "lidar_link", "transform_tolerance": 0.01, "min_height": -0.3, # ouster is 0.425m above ground - "max_height": 0.5, + "max_height": 0.1, "angle_min": -3.14159, "angle_max": 3.14159, "angle_increment": 0.01227184630308513, # ouster in 512x10 mode so use 2*pi/512 @@ -65,27 +65,29 @@ def generate_launch_description(): } ], ), - # SLAM Toolbox - ComposableNode( - package="slam_toolbox", - plugin="slam_toolbox::OnlineAsyncSlamToolbox", - name="slam_toolbox", - parameters=[ - slam_params_file, - { - "use_sim_time": use_sim_time, - "publish_tf": False, # False: Disable slam_toolbox publishing odom and map tf frames - }, - ], - remappings=[ - ("scan", "/2d_ouster_scan"), - ("pose", "/2dslam_pose"), - ], - ), ], output="screen", ) + # SLAM Toolbox as standalone node (easier to debug) + slam_toolbox_node = Node( + package="slam_toolbox", + executable="async_slam_toolbox_node", + name="slam_toolbox", + output="screen", + parameters=[ + slam_params_file, + { + "use_sim_time": use_sim_time, + "publish_tf": False, # False: Disable slam_toolbox publishing odom and map tf frames + }, + ], + remappings=[ + ("scan", "/ouster_2d_scan"), + ("pose", "/slam_2d_pose"), + ], + ) + # Launch Ouster with the SLAM container as target ouster_cmd = IncludeLaunchDescription( PythonLaunchDescriptionSource( @@ -127,7 +129,8 @@ def generate_launch_description(): declare_slam_params_file, slam_container, ouster_cmd, - slam_cmd, - localization_cmd, + slam_toolbox_node, + # slam_cmd, + # localization_cmd, ] ) diff --git a/src/Nav/navigation/launch/ouster_composable.launch.py b/src/Nav/navigation/launch/ouster_composable.launch.py index b35737d7..cd7e2bc8 100644 --- a/src/Nav/navigation/launch/ouster_composable.launch.py +++ b/src/Nav/navigation/launch/ouster_composable.launch.py @@ -69,7 +69,7 @@ def generate_launch_description(): params_file, { "auto_start": auto_start, - "mask_path": mask_file, + # "mask_path": mask_file, "use_intra_process_comms": True, }, ], From baa16badaa593a68a48956cf87179cf1ca28f3bd Mon Sep 17 00:00:00 2001 From: Jetson Date: Wed, 26 Nov 2025 03:24:11 +0000 Subject: [PATCH 5/5] Somewhat working slam toolbox, needs more testing --- .../config/ekf/lidar_2dslam_ekf_global.yaml | 4 +- .../config/ekf/lidar_2dslam_ekf_local.yaml | 6 +-- .../launch/localization.launch.py | 2 +- .../config/ouster/driver_params.yaml | 2 +- .../slam_toolbox/slam_toolbox_params.yaml | 16 +++--- .../navigation/launch/lidar_2dslam.launch.py | 12 ++--- .../navigation/launch/slam_toolbox.launch.py | 52 +++++++++++++++++++ 7 files changed, 74 insertions(+), 20 deletions(-) create mode 100644 src/Nav/navigation/launch/slam_toolbox.launch.py diff --git a/src/Nav/localization/config/ekf/lidar_2dslam_ekf_global.yaml b/src/Nav/localization/config/ekf/lidar_2dslam_ekf_global.yaml index f07984e8..dd1ef382 100644 --- a/src/Nav/localization/config/ekf/lidar_2dslam_ekf_global.yaml +++ b/src/Nav/localization/config/ekf/lidar_2dslam_ekf_global.yaml @@ -4,7 +4,7 @@ global_ekf: # sensor_timeout: 20.0 use_sim_time: false - two_d_mode: false + two_d_mode: true # transform_time_offset: 0.25 # transform_timeout: 3.0 @@ -14,7 +14,7 @@ global_ekf: permit_corrected_publication: false publish_acceleration: true - publish_tf: true + publish_tf: false map_frame: map odom_frame: odom diff --git a/src/Nav/localization/config/ekf/lidar_2dslam_ekf_local.yaml b/src/Nav/localization/config/ekf/lidar_2dslam_ekf_local.yaml index d4ed6351..1399aa57 100644 --- a/src/Nav/localization/config/ekf/lidar_2dslam_ekf_local.yaml +++ b/src/Nav/localization/config/ekf/lidar_2dslam_ekf_local.yaml @@ -4,7 +4,7 @@ local_ekf: # sensor_timeout: 20.0 use_sim_time: false - two_d_mode: false + two_d_mode: true # transform_time_offset: 0.25 # transform_timeout: 3.0 @@ -43,9 +43,9 @@ local_ekf: pose1: /slam_2d_pose pose1_config: [ # position - true, true, false, + false, false, false, # orientation - false, false, true, + false, false, false, # lineal vel false, false, false, # angular vel diff --git a/src/Nav/localization/launch/localization.launch.py b/src/Nav/localization/launch/localization.launch.py index d7c08139..4f8c1d7e 100644 --- a/src/Nav/localization/launch/localization.launch.py +++ b/src/Nav/localization/launch/localization.launch.py @@ -137,7 +137,7 @@ def generate_launch_description(): rviz_cmd, gps_cmd, # slam_cmd, - initial_pose_node, # Publish initial pose first + # initial_pose_node, # Publish initial pose first ekf_cmd, desc_cmd, navsat_node, diff --git a/src/Nav/navigation/config/ouster/driver_params.yaml b/src/Nav/navigation/config/ouster/driver_params.yaml index 3962c842..c3d4623c 100644 --- a/src/Nav/navigation/config/ouster/driver_params.yaml +++ b/src/Nav/navigation/config/ouster/driver_params.yaml @@ -16,7 +16,7 @@ # lidar_mode[optional]: resolution and rate; possible values: { 512x10, # 512x20, 1024x10, 1024x20, 2048x10, 4096x5 }. Leave empty to remain on # current the lidar mode. - lidar_mode: '512x10' + lidar_mode: '1024x10' # timestamp_mode[optional]: method used to timestamp measurements; possible # values: # - TIME_FROM_INTERNAL_OSC diff --git a/src/Nav/navigation/config/slam_toolbox/slam_toolbox_params.yaml b/src/Nav/navigation/config/slam_toolbox/slam_toolbox_params.yaml index a9ad6180..04991e08 100644 --- a/src/Nav/navigation/config/slam_toolbox/slam_toolbox_params.yaml +++ b/src/Nav/navigation/config/slam_toolbox/slam_toolbox_params.yaml @@ -21,6 +21,8 @@ slam_toolbox: base_frame: base_link # scan_topic is handled by remapping in launch file mode: mapping + publish_tf: true + use_sim_time: false # Optional: For continuing existing maps #map_file_name: your_map_name @@ -30,15 +32,15 @@ slam_toolbox: # ======================================== # PERFORMANCE & TIMING PARAMETERS # ======================================== - debug_logging: true # Enable to see what's happening + debug_logging: false # Enable to see what's happening throttle_scans: 1 # Process every scan (10Hz is manageable) transform_publish_period: 0.02 # 50Hz transform publishing map_update_interval: 0.5 # Update map every 0.5 seconds - resolution: 0.03 # Fine resolution + resolution: 0.05 # Fine resolution max_laser_range: 20.0 # Match your sensor's max range min_laser_range: 1.5 # Match pointcloud_to_laserscan range_min - minimum_time_interval: 0.1 # Fast updates for 10Hz scans - transform_timeout: 2.0 # Further increased timeout for transform lookups + minimum_time_interval: 0.2 # Fast updates for 10Hz scans + transform_timeout: 0.2 # Further increased timeout for transform lookups tf_buffer_duration: 30.0 stack_size_to_use: 40000000 enable_interactive_mode: true @@ -49,9 +51,9 @@ slam_toolbox: # These control when new keyframes are created use_scan_matching: true use_scan_barycenter: true - minimum_travel_distance: 0.3 # Smaller for better detail (was 0.5) - minimum_travel_heading: 0.3 # Smaller angle threshold (was 0.5) - scan_buffer_size: 20 # Larger buffer for 10Hz (was 10) + minimum_travel_distance: 0.1 # Smaller for better detail (was 0.5) + minimum_travel_heading: 0.1 # Smaller angle threshold (was 0.5) + scan_buffer_size: 30 # Larger buffer for 10Hz (was 10) scan_buffer_maximum_scan_distance: 20.0 # Match max range # ======================================== diff --git a/src/Nav/navigation/launch/lidar_2dslam.launch.py b/src/Nav/navigation/launch/lidar_2dslam.launch.py index 1d26dcd4..b58ec966 100644 --- a/src/Nav/navigation/launch/lidar_2dslam.launch.py +++ b/src/Nav/navigation/launch/lidar_2dslam.launch.py @@ -51,11 +51,11 @@ def generate_launch_description(): { "target_frame": "lidar_link", "transform_tolerance": 0.01, - "min_height": -0.3, # ouster is 0.425m above ground + "min_height": -0.1, # ouster is 0.425m above ground "max_height": 0.1, "angle_min": -3.14159, "angle_max": 3.14159, - "angle_increment": 0.01227184630308513, # ouster in 512x10 mode so use 2*pi/512 + "angle_increment": 0.006135923, # ouster in 1024x10 mode so use 2*pi/1024 "scan_time": 0.1, # matches 10 Hz "range_min": 1.5, # avoids accidentally picking up the rover itself "range_max": 20.0, @@ -79,7 +79,7 @@ def generate_launch_description(): slam_params_file, { "use_sim_time": use_sim_time, - "publish_tf": False, # False: Disable slam_toolbox publishing odom and map tf frames + "publish_tf": True, # False: Disable slam_toolbox publishing odom and map tf frames }, ], remappings=[ @@ -129,8 +129,8 @@ def generate_launch_description(): declare_slam_params_file, slam_container, ouster_cmd, - slam_toolbox_node, - # slam_cmd, - # localization_cmd, + # slam_toolbox_node, + slam_cmd, + localization_cmd, ] ) diff --git a/src/Nav/navigation/launch/slam_toolbox.launch.py b/src/Nav/navigation/launch/slam_toolbox.launch.py new file mode 100644 index 00000000..46d81e79 --- /dev/null +++ b/src/Nav/navigation/launch/slam_toolbox.launch.py @@ -0,0 +1,52 @@ +import os +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + pkg_navigation = get_package_share_directory("navigation") + + use_sim_time = LaunchConfiguration("use_sim_time") + slam_params_file = LaunchConfiguration("slam_params_file") + + declare_use_sim_time = DeclareLaunchArgument( + "use_sim_time", default_value="false", description="Use simulation time if true" + ) + + declare_slam_params_file = DeclareLaunchArgument( + "slam_params_file", + default_value=os.path.join( + pkg_navigation, "config", "slam_toolbox", "slam_toolbox_params.yaml" + ), + description="Path to slam_toolbox parameters file", + ) + + slam_toolbox_node = Node( + package="slam_toolbox", + executable="async_slam_toolbox_node", + name="slam_toolbox", + output="screen", + parameters=[ + slam_params_file, + { + "use_sim_time": use_sim_time, + "publish_tf": True, + }, + ], + remappings=[ + ("scan", "/ouster_2d_scan"), + ("pose", "/slam_2d_pose"), + ], + ) + + return LaunchDescription( + [ + declare_use_sim_time, + declare_slam_params_file, + slam_toolbox_node, + ] + )