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..dd1ef382
--- /dev/null
+++ b/src/Nav/localization/config/ekf/lidar_2dslam_ekf_global.yaml
@@ -0,0 +1,159 @@
+global_ekf:
+ ros__parameters:
+ frequency: 20.0
+ # sensor_timeout: 20.0
+ use_sim_time: false
+
+ two_d_mode: true
+
+ # transform_time_offset: 0.25
+ # transform_timeout: 3.0
+
+ print_diagnostics: true
+ debug: false
+
+ permit_corrected_publication: false
+ publish_acceleration: true
+ publish_tf: false
+
+ map_frame: map
+ odom_frame: odom
+ base_link_frame: base_link
+ world_frame: map
+
+ # Initial pose to bootstrap the system (high covariance, only used at startup)
+ pose0: /ekf_initial_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_differential: false
+ pose0_relative: false
+ 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: [
+ # 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..1399aa57
--- /dev/null
+++ b/src/Nav/localization/config/ekf/lidar_2dslam_ekf_local.yaml
@@ -0,0 +1,159 @@
+local_ekf:
+ ros__parameters:
+ frequency: 20.0
+ # sensor_timeout: 20.0
+ use_sim_time: false
+
+ two_d_mode: true
+
+ # 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
+
+ # Initial pose to bootstrap the system (high covariance, only used at startup)
+ pose0: /ekf_initial_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_differential: false
+ pose0_relative: false
+ 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
+ false, false, false,
+ # orientation
+ false, false, false,
+ # 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: [
+ # 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..4f8c1d7e 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")
@@ -97,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(
[
@@ -104,9 +133,11 @@ def generate_launch_description():
launch_rviz_cmd,
launch_gps_cmd,
launch_desc_cmd,
+ ekf_profile_cmd,
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..c3d4623c 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: ''
@@ -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
new file mode 100644
index 00000000..04991e08
--- /dev/null
+++ b/src/Nav/navigation/config/slam_toolbox/slam_toolbox_params.yaml
@@ -0,0 +1,103 @@
+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 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
+ #map_start_pose: [0.0, 0.0, 0.0]
+ #map_start_at_dock: true
+
+ # ========================================
+ # PERFORMANCE & TIMING PARAMETERS
+ # ========================================
+ 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.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.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
+
+ # ========================================
+ # MOTION & KEYFRAME PARAMETERS
+ # ========================================
+ # These control when new keyframes are created
+ use_scan_matching: true
+ use_scan_barycenter: true
+ 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
+
+ # ========================================
+ # 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..b58ec966
--- /dev/null
+++ b/src/Nav/navigation/launch/lidar_2dslam.launch.py
@@ -0,0 +1,136 @@
+import os
+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_ros.actions import ComposableNodeContainer, Node
+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 pointcloud to laserscan conversion
+ 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", "/ouster_2d_scan"),
+ ],
+ parameters=[
+ {
+ "target_frame": "lidar_link",
+ "transform_tolerance": 0.01,
+ "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.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,
+ "use_inf": True,
+ "inf_epsilon": 1.0,
+ "use_sim_time": use_sim_time,
+ }
+ ],
+ ),
+ ],
+ 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": True, # 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(
+ 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",
+ "launch_gps": "false",
+ }.items(),
+ )
+
+ return LaunchDescription(
+ [
+ declare_use_sim_time,
+ declare_slam_params_file,
+ slam_container,
+ ouster_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 4cc89bc7..cd7e2bc8 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
+from launch.conditions import IfCondition
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",
@@ -73,20 +69,20 @@ def generate_launch_description():
params_file,
{
"auto_start": auto_start,
- "mask_path": mask_file,
+ # "mask_path": mask_file,
"use_intra_process_comms": True,
},
],
)
- # 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/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,
+ ]
+ )
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