Skip to content

[DRAFT] Add Lidar 2D SLAM EKF for indoor autonomy testing #25

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Draft
ErikCald wants to merge 5 commits into
base: main
Choose a base branch
from
Draft

[DRAFT] Add Lidar 2D SLAM EKF for indoor autonomy testing #25

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
b0bfba2
Add Lidar 2D SLAM EKF configuration and launch files
ErikCald Nov 14, 2025
5976d72
fix: dont launch gps indoors
ErikCald Nov 14, 2025
2eaad1a
address copilot pr comments
ErikCald Nov 14, 2025
b5358a9
Work on 2d lidar slam
CPRTSoftwareLead Nov 22, 2025
baa16ba
Somewhat working slam toolbox, needs more testing
CPRTSoftwareLead Nov 26, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
159 changes: 159 additions & 0 deletions src/Nav/localization/config/ekf/lidar_2dslam_ekf_global.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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]
159 changes: 159 additions & 0 deletions src/Nav/localization/config/ekf/lidar_2dslam_ekf_local.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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]
28 changes: 18 additions & 10 deletions src/Nav/localization/launch/ekf.launch.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -23,38 +23,46 @@
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
local_params_file = os.path.join(
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}")

Expand Down Expand Up @@ -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(
Expand Down
Loading