Add tutorial for ground consistency

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()

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.html

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

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

nav2_lidar_ground_segmentation_demo/config/nav2_config.yaml

@@ -0,0 +1,193 @@
+# Nav2 Configuration for LiDAR Ground Segmentation Demo
+# ========================================================
+# 
+# Key customizations from Nav2 defaults:
+#
+# 1. Updated frame names to match Husky + Velodyne setup (e.g., robot_base_frame: husky/base_link)
+#
+# 2. controller_server: Regulated Pure Pursuit (RPP) instead of default MPPI
+#    - Parameters tuned for terrain: lookahead_dist=2.0, lookahead_time=3.0, use_velocity_scaled_lookahead_dist=true
+#    - Disabled turn on spot (use_rotate_to_heading: false)
+#    - allow_reversing: true 
+#    - velocity tuning: desired_linear_vel=0.35 m/s
+#      Reason: Reduced speed allows smoother terrain adaptation
+#      max_angular_accel=1.5 (lowered from default for gentler steering)
+#
+# 3. local_costmap: Ground Consistency Layer + Inflation Layer
+#    - Changed update/publish frequency to 10 Hz (matches LiDAR rate)
+#    - Changed resolution to 0.3m and size to 50x50m
+#    - Plugins: ["ground_consistency", "inflation_layer"]
+#    - Reason: Demo-specific - uses ground segmentation for terrain-aware costmap
+#    - Update frequency: 10 Hz (matches LiDAR publish rate)
+#
+# 4. global_costmap: Obstacle Layer with ground segmentation obstacles
+#    - Updated resolution to 0.3m and size to 100x100m  
+#
+# 5. collision_monitor: Relaxed timeouts
+#    - transform_tolerance: 0.5s, source_timeout: 1.0s
+#    - Reason: KISS-ICP odometry publishes at ~10Hz with timestamp jitter
+#
+# 6. robot dimensions: Husky footprint with external Dimensions (L X W), 990 x 698 
+#    - robot_height: 1.5m (approximate height with mounted LiDAR for height-based obstacle filtering)
+# 
+
+
+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
+      allow_reversing: true
+
+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

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