Add tutorial for ground consistency

nav2_ground_consistency_demo/CMakeLists.txt

@@ -0,0 +1,14 @@
+cmake_minimum_required(VERSION 3.16)
+project(nav2_ground_consistency_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_ground_consistency_demo/README.md

@@ -0,0 +1,2 @@
+# NAV2 Ground Consistency Demo
+Tutorial code referenced in https://docs.nav2.org/tutorials/docs/navigation2_with_ground_consistency_layer.rst

nav2_ground_consistency_demo/config/gseg3d_config.yaml

@@ -0,0 +1,63 @@
+# Ground Segmentation Parameters for nav2_ground_consistency_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_ground_consistency_demo/config/kiss_icp_config.yaml

@@ -0,0 +1,42 @@
+# KISS-ICP Configuration for nav2_ground_consistency_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_ground_consistency_demo/config/nav2_config.yaml

@@ -0,0 +1,183 @@
+bt_navigator:
+  ros__parameters:
+    robot_base_frame: husky/base_link
+
+controller_server:
+  ros__parameters:
+    controller_frequency: 20.0
+    min_x_velocity_threshold: 0.001
+    min_y_velocity_threshold: 0.5
+    min_theta_velocity_threshold: 0.001
+    progress_checker_plugins: ["progress_checker"]
+    goal_checker_plugins: ["goal_checker"]
+    controller_plugins: ["FollowPath"]
+
+    progress_checker:
+      plugin: "nav2_controller::SimpleProgressChecker"
+      required_movement_radius: 0.5
+      movement_time_allowance: 10.0
+    goal_checker:
+      plugin: "nav2_controller::SimpleGoalChecker"
+      xy_goal_tolerance: 0.25
+      yaw_goal_tolerance: 0.25
+      stateful: True
+    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
+      rotate_to_heading_angular_vel: 1.0
+      transform_tolerance: 0.1
+      use_velocity_scaled_lookahead_dist: true
+      min_approach_linear_velocity: 0.05
+      approach_velocity_scaling_dist: 1.0
+      use_collision_detection: true
+      max_allowed_time_to_collision_up_to_carrot: 1.0
+      use_regulated_linear_velocity_scaling: true
+      use_cost_regulated_linear_velocity_scaling: false
+      regulated_linear_scaling_min_radius: 0.9
+      regulated_linear_scaling_min_speed: 0.25
+      use_fixed_curvature_lookahead: false
+      curvature_lookahead_dist: 1.0
+      use_rotate_to_heading: false
+      rotate_to_heading_min_angle: 0.785
+      max_angular_accel: 1.5
+      max_robot_pose_search_dist: 10.0
+      interpolate_curvature_after_goal: false
+      cost_scaling_dist: 0.3
+      cost_scaling_gain: 1.0
+      inflation_cost_scaling_factor: 3.0
+
+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_ground_consistency_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