diff --git a/nav2_lidar_ground_segmentation_demo/CMakeLists.txt b/nav2_lidar_ground_segmentation_demo/CMakeLists.txt
new file mode 100644
index 0000000..c706e71
--- /dev/null
+++ b/nav2_lidar_ground_segmentation_demo/CMakeLists.txt
@@ -0,0 +1,14 @@
+cmake_minimum_required(VERSION 3.16)
+project(nav2_lidar_ground_segmentation_demo)
+
+find_package(ament_cmake REQUIRED)
+
+# Install launch files, configs, and model files
+install(
+ DIRECTORY launch config models
+ DESTINATION share/${PROJECT_NAME}
+ PATTERN "__pycache__" EXCLUDE
+ PATTERN "*.pyc" EXCLUDE
+)
+
+ament_package()
diff --git a/nav2_lidar_ground_segmentation_demo/README.md b/nav2_lidar_ground_segmentation_demo/README.md
new file mode 100644
index 0000000..11c4096
--- /dev/null
+++ b/nav2_lidar_ground_segmentation_demo/README.md
@@ -0,0 +1,2 @@
+# Nav2 Lidar Ground Segmentation Demo
+Tutorial code referenced in https://docs.nav2.org/tutorials/docs/navigation2_with_ground_consistency_layer.rst
\ No newline at end of file
diff --git a/nav2_lidar_ground_segmentation_demo/config/gazebo_gui.config b/nav2_lidar_ground_segmentation_demo/config/gazebo_gui.config
new file mode 100644
index 0000000..abd7826
--- /dev/null
+++ b/nav2_lidar_ground_segmentation_demo/config/gazebo_gui.config
@@ -0,0 +1,1126 @@
+
+
+
+ -1
+ -1
+
+ 1920
+ 1043
+
+
+
+
+
+
+
+
+ 3D View
+ 0
+ 0
+ 0
+ 1919
+ 995
+ 1
+ true
+ true
+ docked
+ 0
+ false
+ false
+ false
+ false
+ 0
+ 1
+ true
+ false
+ 0
+ 0
+ 1919
+ 995
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ false
+ false
+ false
+ true
+ false
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 50
+ true
+ true
+ true
+ false
+ true
+ false
+ ▁
+ ▴
+ ▾
+ □
+ ✕
+ #00000000
+ 50
+ 200
+ 200
+ #03a9f4
+ #fafafa
+
+ ogre2
+ scene
+ 0.4 0.4 0.4
+ 0.8 0.8 0.8
+ -222 -75 1.5 0 0.4 0.785
+
+
+
+ 0
+ 0
+ 0
+ 5
+ 5
+ 1
+ true
+ true
+ floating
+ 0
+ false
+ false
+ false
+ false
+ 0
+ 1
+ true
+ false
+ 0
+ 0
+ 5
+ 5
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ false
+ false
+ false
+ true
+ false
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 50
+ true
+ true
+ true
+ false
+ true
+ false
+ ▁
+ ▴
+ ▾
+ □
+ ✕
+ #00000000
+ 370
+ 350
+ 370
+ #03a9f4
+ #fafafa
+
+
+
+
+ 0
+ 0
+ 0
+ 5
+ 5
+ 1
+ true
+ true
+ floating
+ 0
+ false
+ false
+ false
+ false
+ 0
+ 1
+ true
+ false
+ 0
+ 0
+ 5
+ 5
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ false
+ false
+ false
+ true
+ false
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 50
+ true
+ true
+ true
+ false
+ false
+ false
+ ▁
+ ▴
+ ▾
+ □
+ ✕
+ #00000000
+ 130
+ 350
+ 130
+ #03a9f4
+ #fafafa
+
+
+
+
+ 0
+ 0
+ 0
+ 5
+ 5
+ 1
+ true
+ true
+ floating
+ 0
+ false
+ false
+ false
+ false
+ 0
+ 1
+ true
+ false
+ 0
+ 0
+ 5
+ 5
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ false
+ false
+ false
+ true
+ false
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 50
+ true
+ true
+ true
+ false
+ false
+ false
+ ▁
+ ▴
+ ▾
+ □
+ ✕
+ #00000000
+ 110
+ 350
+ 110
+ #03a9f4
+ #fafafa
+
+
+
+
+ 0
+ 0
+ 0
+ 5
+ 5
+ 1
+ true
+ true
+ floating
+ 0
+ false
+ false
+ false
+ false
+ 0
+ 1
+ true
+ false
+ 0
+ 0
+ 5
+ 5
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ false
+ false
+ false
+ true
+ false
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 50
+ true
+ true
+ true
+ false
+ false
+ false
+ ▁
+ ▴
+ ▾
+ □
+ ✕
+ #00000000
+ 260
+ 350
+ 260
+ #03a9f4
+ #fafafa
+
+
+
+
+ 0
+ 0
+ 0
+ 5
+ 5
+ 1
+ true
+ true
+ floating
+ 0
+ false
+ false
+ false
+ false
+ 0
+ 1
+ true
+ false
+ 0
+ 0
+ 5
+ 5
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ false
+ false
+ false
+ true
+ false
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 50
+ true
+ true
+ true
+ false
+ false
+ false
+ ▁
+ ▴
+ ▾
+ □
+ ✕
+ #00000000
+ 230
+ 350
+ 230
+ #03a9f4
+ #fafafa
+
+
+
+
+ 0
+ 0
+ 0
+ 5
+ 5
+ 1
+ true
+ true
+ floating
+ 0
+ false
+ false
+ false
+ false
+ 0
+ 1
+ true
+ false
+ 0
+ 0
+ 5
+ 5
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ false
+ false
+ false
+ true
+ false
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 50
+ true
+ true
+ true
+ false
+ false
+ false
+ ▁
+ ▴
+ ▾
+ □
+ ✕
+ #00000000
+ 90
+ 350
+ 90
+ #03a9f4
+ #fafafa
+
+
+
+
+ 0
+ 0
+ 0
+ 5
+ 5
+ 1
+ true
+ true
+ floating
+ 0
+ false
+ false
+ false
+ false
+ 0
+ 1
+ true
+ false
+ 0
+ 0
+ 5
+ 5
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ false
+ false
+ false
+ true
+ false
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 50
+ true
+ true
+ true
+ false
+ false
+ false
+ ▁
+ ▴
+ ▾
+ □
+ ✕
+ #00000000
+ 110
+ 350
+ 110
+ #03a9f4
+ #fafafa
+
+
+
+
+ 0
+ 0
+ 0
+ 5
+ 5
+ 1
+ true
+ true
+ floating
+ 0
+ false
+ false
+ false
+ false
+ 0
+ 1
+ true
+ false
+ 0
+ 0
+ 5
+ 5
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ false
+ false
+ false
+ true
+ false
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 50
+ true
+ true
+ true
+ false
+ false
+ false
+ ▁
+ ▴
+ ▾
+ □
+ ✕
+ #00000000
+ 130
+ 350
+ 130
+ #03a9f4
+ #fafafa
+
+
+
+
+ World control
+
+
+
+
+ 0
+ 923
+ 1
+ 1199
+ 72
+ 1
+ true
+ true
+ floating
+ 0
+ false
+ false
+ false
+ false
+ 0
+ 1
+ true
+ false
+ 0
+ 0
+ 1199
+ 72
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ false
+ false
+ false
+ true
+ false
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 50
+ true
+ true
+ true
+ false
+ false
+ false
+ ▁
+ ▴
+ ▾
+ □
+ ✕
+ #00000000
+ 100
+ 100
+ 100
+ #03a9f4
+ #fafafa
+
+ true
+ true
+ true
+ true
+
+
+
+ World stats
+
+
+
+
+ 1629
+ 885
+ 1
+ 290
+ 110
+ 1
+ true
+ true
+ floating
+ 0
+ false
+ false
+ false
+ false
+ 0
+ 1
+ true
+ false
+ 0
+ 0
+ 290
+ 110
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ false
+ false
+ false
+ true
+ false
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 50
+ true
+ true
+ true
+ false
+ false
+ false
+ ▁
+ ▴
+ ▾
+ □
+ ✕
+ #00000000
+ 110
+ 290
+ 110
+ #03a9f4
+ #fafafa
+
+ true
+ true
+ true
+ true
+
+
+
+ 0
+ 0
+ 0
+ 300
+ 50
+ 1
+ true
+ true
+ floating
+ 0
+ false
+ false
+ false
+ false
+ 0
+ 1
+ true
+ false
+ 0
+ 0
+ 300
+ 50
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ false
+ false
+ false
+ true
+ false
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 50
+ true
+ true
+ true
+ false
+ false
+ false
+ ▁
+ ▴
+ ▾
+ □
+ ✕
+ #666666
+ 100
+ 200
+ 100
+ #03a9f4
+ #fafafa
+
+
+
+
+ 300
+ 0
+ 0
+ 150
+ 50
+ 1
+ true
+ true
+ floating
+ 0
+ false
+ false
+ false
+ false
+ 0
+ 1
+ true
+ false
+ 0
+ 0
+ 150
+ 50
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ false
+ false
+ false
+ true
+ false
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 50
+ true
+ true
+ true
+ false
+ false
+ false
+ ▁
+ ▴
+ ▾
+ □
+ ✕
+ #666666
+ 100
+ 200
+ 100
+ #03a9f4
+ #fafafa
+
+
+
+
+ 0
+ 50
+ 0
+ 250
+ 50
+ 1
+ true
+ true
+ floating
+ 0
+ false
+ false
+ false
+ false
+ 0
+ 1
+ true
+ false
+ 0
+ 0
+ 250
+ 50
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ false
+ false
+ false
+ true
+ false
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 50
+ true
+ true
+ true
+ false
+ false
+ false
+ ▁
+ ▴
+ ▾
+ □
+ ✕
+ #777777
+ 100
+ 200
+ 100
+ #03a9f4
+ #fafafa
+
+
+
+
+ 250
+ 50
+ 0
+ 50
+ 50
+ 1
+ true
+ true
+ floating
+ 0
+ false
+ false
+ false
+ false
+ 0
+ 1
+ true
+ false
+ 0
+ 0
+ 50
+ 50
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ false
+ false
+ false
+ true
+ false
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 50
+ true
+ true
+ true
+ false
+ false
+ false
+ ▁
+ ▴
+ ▾
+ □
+ ✕
+ #777777
+ 150
+ 200
+ 150
+ #03a9f4
+ #fafafa
+
+
+
+
+ 300
+ 50
+ 0
+ 100
+ 50
+ 1
+ true
+ true
+ floating
+ 0
+ false
+ false
+ false
+ false
+ 0
+ 1
+ true
+ false
+ 0
+ 0
+ 100
+ 50
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ false
+ false
+ false
+ true
+ false
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 50
+ true
+ true
+ true
+ false
+ false
+ false
+ ▁
+ ▴
+ ▾
+ □
+ ✕
+ #777777
+ 100
+ 200
+ 100
+ #03a9f4
+ #fafafa
+
+
diff --git a/nav2_lidar_ground_segmentation_demo/config/gseg3d_config.yaml b/nav2_lidar_ground_segmentation_demo/config/gseg3d_config.yaml
new file mode 100644
index 0000000..baeecb0
--- /dev/null
+++ b/nav2_lidar_ground_segmentation_demo/config/gseg3d_config.yaml
@@ -0,0 +1,63 @@
+# Ground Segmentation Parameters for nav2_lidar_ground_segmentation_demo
+# Tuned for: Clearpath Husky + Velodyne VLP-16 in Gazebo
+#
+# Tuning Guide:
+# 1. Keep groundInlierThreshold < cellSizeZPhase2
+# 2. If small obstacles disappear → decrease groundInlierThreshold
+# 3. If ground becomes fragmented → increase groundInlierThreshold
+# 4. If slopes are obstacles → increase slopeThresholdDegrees
+# 5. If ramps become ground too easily → decrease slopeThresholdDegrees
+# 6. Larger cellSizeX/Y = smoother but less detail
+# 7. cellSizeZPhase2 controls vertical resolution (smaller = better obstacle separation)
+# 8. High LiDAR noise → enable downsample, increase groundInlierThreshold
+# 9. Steep terrain → enable use_imu_orientation
+
+ground_segmentation:
+ ros__parameters:
+ # Frame configuration
+ robot_frame: "husky/base_link"
+
+ # Processing bounds (relative to robot)
+ maxX: 50.0
+ minX: -50.0
+ maxY: 50.0
+ minY: -50.0
+ maxZ: 50.0
+ minZ: -50.0
+
+ # Optional preprocessing
+ downsample: false # Disable for simulation (clean data)
+ downsample_resolution: 0.1
+
+ # Lidar mounting height (negative = above ground)
+ lidar_to_ground: -0.92
+
+ # Transform tolerance for TF lookups
+ transform_tolerance: 0.5
+
+ # Ground Detection Parameters
+ # Cell dimensions for ground modeling
+ cellSizeX: 1.0 # Horizontal resolution (smaller = more detail)
+ cellSizeY: 1.0 # Horizontal resolution
+ cellSizeZ: 4.0 # Vertical bin height (phase 1)
+ cellSizeZPhase2: 1.0 # Ground layer thickness (phase 2)
+
+ # Slope threshold: classify steep surfaces as obstacles
+ slopeThresholdDegrees: 30.0 # Tune: <20° if missing ramps, >30° if false positives
+
+ # Point classification threshold (m)
+ groundInlierThreshold: 0.2 # Distance from fitted ground plane
+ # Smaller = stricter (fewer ground points)
+ # Larger = more forgiving (marks more as ground)
+
+ # Centroid search radius for ground clustering (cells)
+ centroidSearchRadius: 5.0
+
+ # Optional: Use IMU for gravity direction (if available)
+ use_imu_orientation: false
+
+ # Max vertical deviation from fitted ground plane
+ maxGroundHeightDeviation: 0.2
+
+ # Logging
+ show_benchmark: false # Set to true for performance debugging
diff --git a/nav2_lidar_ground_segmentation_demo/config/kiss_icp_config.yaml b/nav2_lidar_ground_segmentation_demo/config/kiss_icp_config.yaml
new file mode 100644
index 0000000..b3f3e02
--- /dev/null
+++ b/nav2_lidar_ground_segmentation_demo/config/kiss_icp_config.yaml
@@ -0,0 +1,42 @@
+# KISS-ICP Configuration for nav2_lidar_ground_segmentation_demo
+# Tuned for: Clearpath Husky + Velodyne VLP-16 in Gazebo
+#
+# Key tuning parameters:
+# - max_range: Lidar max range (Velodyne VLP-16 = 30m)
+# - max_num_iterations: More = slower but more accurate (500 is default for exploration)
+# - initial_threshold: Start value for adaptive threshold (higher = faster but less robust)
+# - max_points_per_voxel: Reduce for speed, increase for accuracy
+
+kiss_icp_node:
+ ros__parameters:
+ # NOTE: Launch arguments will override these ROS parameters if set
+
+ #### Core KISS-ICP parameters ####
+
+ data:
+ # Point cloud preprocessing
+ deskew: true # Remove motion distortion from rotating lidar
+ max_range: 30.0 # Velodyne VLP-16 max range
+ min_range: 0.0 # Minimum range threshold
+
+ mapping:
+ # Voxel grid parameters for the local map
+ # voxel_size is auto-calculated as max_range / 100.0 (= 0.3m here)
+ # Uncomment to override:
+ # voxel_size: 0.3
+ max_points_per_voxel: 20 # Points per grid cell (higher = better detail, slower)
+
+ adaptive_threshold:
+ # Dynamic thresholding for registration convergence
+ initial_threshold: 1.5 # Starting correspondence threshold (meters)
+ # Larger = faster but may lose accuracy
+ # Smaller = slower but more accurate
+ min_motion_th: 0.05 # Minimum motion threshold (meters)
+
+ registration:
+ # ICP registration parameters
+ max_num_iterations: 200 # Max iterations per scan (default 500, reduced for real-time)
+ # Lower = faster but may not converge
+ # Higher = more accurate but slower
+ convergence_criterion: 0.0001 # Registration error threshold
+ max_num_threads: 0 # 0 = auto-detect CPU cores
diff --git a/nav2_lidar_ground_segmentation_demo/config/nav2_config.yaml b/nav2_lidar_ground_segmentation_demo/config/nav2_config.yaml
new file mode 100644
index 0000000..7da70f0
--- /dev/null
+++ b/nav2_lidar_ground_segmentation_demo/config/nav2_config.yaml
@@ -0,0 +1,158 @@
+bt_navigator:
+ ros__parameters:
+ robot_base_frame: husky/base_link
+
+controller_server:
+ ros__parameters:
+ progress_checker_plugins: ["progress_checker"]
+ goal_checker_plugins: ["goal_checker"]
+ controller_plugins: ["FollowPath"]
+
+ progress_checker:
+ plugin: "nav2_controller::SimpleProgressChecker"
+
+ goal_checker:
+ plugin: "nav2_controller::SimpleGoalChecker"
+
+ FollowPath:
+ plugin: "nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController"
+ desired_linear_vel: 0.35
+ lookahead_dist: 2.0
+ min_lookahead_dist: 1.0
+ max_lookahead_dist: 2.5
+ lookahead_time: 3.0
+ use_velocity_scaled_lookahead_dist: true
+ use_rotate_to_heading: false
+ max_angular_accel: 1.5
+
+local_costmap:
+ local_costmap:
+ ros__parameters:
+ update_frequency: 10.0
+ publish_frequency: 10.0
+ global_frame: odom
+ robot_base_frame: husky/base_link
+ rolling_window: true
+ width: 50
+ height: 50
+ resolution: 0.3
+ footprint: "[[0.495, 0.349], [0.495, -0.349], [-0.495, -0.349], [-0.495, 0.349]]"
+ plugins: ["ground_consistency", "inflation_layer"]
+
+ ground_consistency:
+ plugin: "nav2_ground_consistency_costmap_plugin::GroundConsistencyLayer"
+ ground_points_topic: /ground_segmentation/ground_points
+ nonground_points_topic: /ground_segmentation/obstacle_points
+ tf_timeout: 0.1
+ min_clearance: 0.1
+ robot_height: 1.5
+ maximum_height_filter: 2.0
+ ground_inc: 1.0
+ nonground_inc: 1.5
+ nonground_decay: 0.93
+ ground_decay: 0.80
+ max_score: 5000.0
+ nonground_occ_thresh: 6.0
+ nonground_prob_thresh: 0.75
+ enable_kpi_logging: true
+ discretize_costs: true
+ max_data_range: 50.0
+ ground_neighbor_search_cells: 0
+
+ inflation_layer:
+ plugin: "nav2_costmap_2d::InflationLayer"
+ enabled: true
+ cost_scaling_factor: 1.0
+ inflation_radius: 1.2
+
+ always_send_full_costmap: false
+
+global_costmap:
+ global_costmap:
+ ros__parameters:
+ update_frequency: 1.0
+ publish_frequency: 1.0
+ global_frame: map
+ robot_base_frame: husky/base_link
+ resolution: 0.3
+ track_unknown_space: false
+ rolling_window: false
+ width: 100
+ height: 100
+ origin_x: -50.0
+ origin_y: -50.0
+ footprint: "[[0.495, 0.349], [0.495, -0.349], [-0.495, -0.349], [-0.495, 0.349]]"
+ plugins: ["obstacle_layer"]
+ obstacle_layer:
+ plugin: "nav2_costmap_2d::ObstacleLayer"
+ observation_sources: pointcloud
+ pointcloud:
+ topic: /ground_segmentation/obstacle_points
+ max_obstacle_height: 2.0
+ clearing: true
+ marking: true
+ data_type: "PointCloud2"
+ always_send_full_costmap: false
+
+behavior_server:
+ ros__parameters:
+ cycle_frequency: 10.0
+ behavior_plugins: ["backup", "drive_on_heading", "wait", "spin"]
+ backup:
+ plugin: "nav2_behaviors::BackUp"
+ drive_on_heading:
+ plugin: "nav2_behaviors::DriveOnHeading"
+ wait:
+ plugin: "nav2_behaviors::Wait"
+ spin:
+ plugin: "nav2_behaviors::Spin"
+ local_frame: odom
+ global_frame: map
+ robot_base_frame: husky/base_link
+ transform_tolerance: 0.1
+ simulate_ahead_time: 2.0
+ max_rotational_vel: 1.0
+ min_rotational_vel: 0.4
+ rotational_acc_lim: 3.2
+
+collision_monitor:
+ ros__parameters:
+ base_frame_id: "husky/base_link"
+ odom_frame_id: "odom"
+ cmd_vel_in_topic: "cmd_vel_smoothed"
+ cmd_vel_out_topic: "cmd_vel"
+ state_topic: "collision_monitor_state"
+ transform_tolerance: 0.5
+ source_timeout: 1.0
+ base_shift_correction: True
+ stop_pub_timeout: 2.0
+ polygons: ["HuskyFootprint"]
+ HuskyFootprint:
+ type: "polygon"
+ action_type: "stop"
+ points: "[[-0.5, -0.34], [-0.5, 0.34], [0.5, 0.34], [0.5, -0.34]]"
+ visualize: True
+ enabled: True
+ observation_sources: ["pointcloud"]
+ pointcloud:
+ type: "pointcloud"
+ topic: "/ground_segmentation/obstacle_points"
+ min_height: 0.1
+ max_height: 1.5
+ enabled: True
+
+docking_server:
+ ros__parameters:
+ base_frame: "husky/base_link"
+ fixed_frame: "odom"
+ dock_plugins: ['simple_charging_dock']
+ simple_charging_dock:
+ plugin: 'opennav_docking::SimpleChargingDock'
+ use_external_detection_pose: true
+ use_battery_status: false
+ use_stall_detection: false
+ controller:
+ use_collision_detection: true
+ projection_time: 5.0
+ simulation_step: 0.1
+ dock_collision_threshold: 0.3
\ No newline at end of file
diff --git a/nav2_lidar_ground_segmentation_demo/dependencies.repos b/nav2_lidar_ground_segmentation_demo/dependencies.repos
new file mode 100644
index 0000000..e9fd600
--- /dev/null
+++ b/nav2_lidar_ground_segmentation_demo/dependencies.repos
@@ -0,0 +1,17 @@
+repositories:
+ kiss_icp:
+ type: git
+ url: https://github.com/PRBonn/kiss-icp.git
+ version: main
+ ground_segmentation:
+ type: git
+ url: https://github.com/dfki-ric/ground_segmentation.git
+ version: master
+ ground_segmentation_ros2:
+ type: git
+ url: https://github.com/dfki-ric/ground_segmentation_ros2.git
+ version: master
+ nav2_ground_consistency_costmap_plugin:
+ type: git
+ url: https://github.com/dfki-ric/nav2_ground_consistency_costmap_plugin.git
+ version: main
diff --git a/nav2_lidar_ground_segmentation_demo/launch/lidar_ground_segmentation_demo.launch.py b/nav2_lidar_ground_segmentation_demo/launch/lidar_ground_segmentation_demo.launch.py
new file mode 100644
index 0000000..f9fc402
--- /dev/null
+++ b/nav2_lidar_ground_segmentation_demo/launch/lidar_ground_segmentation_demo.launch.py
@@ -0,0 +1,170 @@
+"""
+Nav2 navigation stack launch for nav2_lidar_ground_segmentation_demo
+
+Usage:
+ ros2 launch nav2_lidar_ground_segmentation_demo full_stack.launch.py
+"""
+
+import os
+from ament_index_python.packages import get_package_share_directory
+
+from launch import LaunchDescription
+from launch.actions import IncludeLaunchDescription, SetEnvironmentVariable
+from launch.substitutions import PathJoinSubstitution
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+DEMO_PKG_SHARE = get_package_share_directory('nav2_lidar_ground_segmentation_demo')
+WORLD_NAME = "baylands_terrain"
+
+def generate_gazebo_launch():
+ """Generate Gazebo simulation with Husky robot and terrain."""
+
+ # Gazebo world and GUI config paths'
+ world_sdf_path = os.path.join(DEMO_PKG_SHARE, 'models', WORLD_NAME + '.sdf')
+ gui_config_path = os.path.join(DEMO_PKG_SHARE, 'config', 'gazebo_gui.config')
+
+ # Construct Gazebo launch arguments
+ ign_args = '-v 4 -r ' + world_sdf_path
+ if os.path.exists(gui_config_path):
+ ign_args += ' --gui-config ' + gui_config_path
+
+ # Launch Gazebo
+ gazebo_launch = IncludeLaunchDescription(
+ PythonLaunchDescriptionSource(
+ os.path.join(
+ get_package_share_directory('ros_gz_sim'),
+ 'launch',
+ 'gz_sim.launch.py'
+ )
+ ),
+ launch_arguments={
+ 'gz_args': ign_args
+ }.items()
+ )
+
+ # ROS 2 <-> Gazebo bridge
+ bridge_args = [
+ '/model/husky/cmd_vel@geometry_msgs/msg/Twist@gz.msgs.Twist',
+ f'/world/{WORLD_NAME}/model/husky/link/base_link/sensor/front_laser/scan/points@sensor_msgs/msg/PointCloud2@gz.msgs.PointCloudPacked',
+ f'/world/{WORLD_NAME}/clock@rosgraph_msgs/msg/Clock@gz.msgs.Clock'
+ ]
+
+ ign_ros2_bridge = Node(
+ package='ros_gz_bridge',
+ executable='parameter_bridge',
+ arguments=bridge_args,
+ remappings=[
+ ('/model/husky/cmd_vel', '/cmd_vel'),
+ (f'/world/{WORLD_NAME}/clock', '/clock'),
+ (f'/world/{WORLD_NAME}/model/husky/link/base_link/sensor/front_laser/scan/points', '/husky/scan/points')
+ ],
+ output='both'
+ )
+
+ # Static transform: base_link -> front_laser
+ # We don't use tf from Gazebo because it interferes with kiss_icp's odometry frame associations.
+ # Instead, we publish a static transform with the same parameters as the one in Gazebo.
+ static_tf_front_laser = Node(
+ package='tf2_ros',
+ executable='static_transform_publisher',
+ arguments=['0.0012', '0', '0.716', '0', '0', '0', 'husky/base_link', 'husky/base_link/front_laser'],
+ parameters=[{'use_sim_time': True}],
+ )
+
+ return [gazebo_launch, ign_ros2_bridge, static_tf_front_laser]
+
+def generate_launch_description():
+ """Generate complete launch description."""
+
+ # Get package directories
+ nav2_bringup_dir = FindPackageShare("nav2_bringup")
+ kiss_icp_dir = FindPackageShare("kiss_icp")
+ ground_seg_dir = FindPackageShare("ground_segmentation_ros2")
+
+ # Set Gazebo resource path
+ models_dir = os.path.join(DEMO_PKG_SHARE, 'models')
+ set_gz_resource_path = SetEnvironmentVariable(
+ 'GZ_SIM_RESOURCE_PATH',
+ models_dir + ':$GZ_SIM_RESOURCE_PATH'
+ )
+
+ # KISS-ICP odometry
+ kiss_icp_launch = IncludeLaunchDescription(
+ PythonLaunchDescriptionSource(
+ [kiss_icp_dir, "/launch/odometry.launch.py"]
+ ),
+ launch_arguments={
+ "topic": "/husky/scan/points",
+ "base_frame": "husky/base_link",
+ "lidar_odom_frame": "odom",
+ "invert_odom_tf": "False",
+ "visualize": "False",
+ "config_file": PathJoinSubstitution(
+ [DEMO_PKG_SHARE, "config/kiss_icp_config.yaml"]
+ ),
+ "use_sim_time": "True",
+ }.items()
+ )
+
+ # Ground segmentation
+ ground_seg_launch = IncludeLaunchDescription(
+ PythonLaunchDescriptionSource(
+ [ground_seg_dir, "/launch/ground_segmentation.launch.py"]
+ ),
+ launch_arguments={
+ "pointcloud_topic": "/husky/scan/points",
+ "params_file": PathJoinSubstitution(
+ [DEMO_PKG_SHARE, "config/gseg3d_config.yaml"]
+ ),
+ "use_sim_time": "True",
+ }.items()
+ )
+
+ # Nav2 bringup
+ nav2_bringup = IncludeLaunchDescription(
+ PythonLaunchDescriptionSource(
+ PathJoinSubstitution([nav2_bringup_dir, "launch", "bringup_launch.py"])
+ ),
+ launch_arguments={
+ "use_sim_time": "True",
+ "slam": "False",
+ "use_localization": "False",
+ "autostart": "True",
+ "use_composition": "False",
+ "use_respawn": "False",
+ "params_file": PathJoinSubstitution([DEMO_PKG_SHARE, "config", "nav2_config.yaml"]),
+ }.items(),
+ )
+
+ # Static map -> odom transform
+ # We don't use a mapper in the demo so provide a static transform between map and odom.
+ map_to_odom_tf = Node(
+ package="tf2_ros",
+ output="screen",
+ executable="static_transform_publisher",
+ arguments=["0", "0", "0", "0", "0", "0", "map", "odom"],
+ parameters=[{"use_sim_time": True}],
+ )
+
+ # RViz2 visualization
+ rviz_launch = IncludeLaunchDescription(
+ PythonLaunchDescriptionSource(
+ PathJoinSubstitution([nav2_bringup_dir, "launch", "rviz_launch.py"])
+ ),
+ launch_arguments={
+ "use_sim_time": "True",
+ }.items()
+ )
+
+ return LaunchDescription([
+ set_gz_resource_path,
+
+ *generate_gazebo_launch(),
+ kiss_icp_launch,
+ ground_seg_launch,
+ map_to_odom_tf,
+ nav2_bringup,
+ rviz_launch,
+ ])
diff --git a/nav2_lidar_ground_segmentation_demo/models/baylands_terrain.sdf b/nav2_lidar_ground_segmentation_demo/models/baylands_terrain.sdf
new file mode 100644
index 0000000..432a33b
--- /dev/null
+++ b/nav2_lidar_ground_segmentation_demo/models/baylands_terrain.sdf
@@ -0,0 +1,39 @@
+
+
+
+ 0.004
+ 1
+ 1000
+
+
+ 1 1 1 1
+ 1 1 1 1
+ false
+ false
+ true
+
+
+
+
+ https://fuel.gazebosim.org/1.0/OpenRobotics/models/baylands
+
+
+
+
+ husky_diff_drive.sdf
+ -220 -70 0.5 0 0 0
+
+
+
+
+
+
+
+
+
+ ogre2
+
+ 0 0 -9.8
+
+
+
diff --git a/nav2_lidar_ground_segmentation_demo/models/husky_diff_drive.sdf b/nav2_lidar_ground_segmentation_demo/models/husky_diff_drive.sdf
new file mode 100644
index 0000000..8bb03de
--- /dev/null
+++ b/nav2_lidar_ground_segmentation_demo/models/husky_diff_drive.sdf
@@ -0,0 +1,32 @@
+
+
+
+
+
+ https://fuel.gazebosim.org/1.0/OpenRobotics/models/COSTAR_HUSKY_SENSOR_CONFIG_1
+
+
+
+
+ /model/husky/cmd_vel
+
+ odom
+ tf
+
+ rear_left_wheel_joint
+ front_left_wheel_joint
+
+ rear_right_wheel_joint
+ front_right_wheel_joint
+
+ 0.5708
+ 0.1651
+
+ 0
+
+
+
+
+
\ No newline at end of file
diff --git a/nav2_lidar_ground_segmentation_demo/package.xml b/nav2_lidar_ground_segmentation_demo/package.xml
new file mode 100644
index 0000000..2890c79
--- /dev/null
+++ b/nav2_lidar_ground_segmentation_demo/package.xml
@@ -0,0 +1,56 @@
+
+
+
+ nav2_lidar_ground_segmentation_demo
+ 1.0.0
+
+ Demo package for nav2_ground_consistency_costmap_plugin.
+ Provides Gazebo simulation with Husky robot equipped with Velodyne VLP-16 3D lidar
+ for testing and evaluating ground/obstacle classification in nav2 costmaps.
+
+
+ DFKI RIC
+ DFKI RIC
+
+ BSD-3-Clause
+
+ https://github.com/dfki-ric/nav2_ground_consistency_costmap_plugin
+ https://github.com/dfki-ric/nav2_ground_consistency_costmap_plugin
+ https://github.com/dfki-ric/nav2_ground_consistency_costmap_plugin/issues
+
+ ament_cmake
+
+
+ ros_gz_sim
+ ros_gz_bridge
+ ros_gz_interfaces
+
+
+ tf2_ros
+ tf2_tools
+
+
+ rviz2
+ rviz_common
+
+
+ nav2_planner
+ nav2_controller
+ nav2_behaviors
+ nav2_costmap_2d
+ nav2_bt_navigator
+
+
+ nav2_ground_consistency_costmap_plugin
+ ground_segmentation_ros2
+
+
+ kiss_icp
+
+
+ ament_cmake_pytest
+
+
+ ament_cmake
+
+