Merge remote-tracking branch 'origin/sl-perception/issue-478-costmaps'

# Conflicts:
#	jetson/ros2_ws/src/saltybot_bringup/config/nav2_params.yaml

jetson/ros2_ws/src/saltybot_bringup/config/nav2_params.yaml

@@ -1,18 +1,18 @@
 # Nav2 parameters — SaltyBot (Jetson Orin Nano Super / ROS2 Humble)
 #
 # Robot: differential-drive self-balancing two-wheeler
-#   robot_radius:   0.22 m (0.4m x 0.4m footprint)
+#   robot_radius:   0.15 m  (~0.2m with margin)
 #   footprint:      0.4 x 0.4 m
-#   max_vel_x:      0.3  m/s (conservative for FC + hoverboard ESC, Issue #475)
-#   max_vel_theta:  0.5  rad/s (conservative for FC + hoverboard ESC, Issue #475)
+#   max_vel_x:      1.0  m/s
+#   max_vel_theta:  1.5  rad/s
 #
 # Localization: RTAB-Map (publishes /map + map→odom TF + /rtabmap/odom)
 #   → No AMCL, no map_server needed.
 #
 # Sensors (Issue #478 — Costmap configuration):
-#   /scan                    — RPLIDAR A1M8 360° LaserScan (obstacle layer)
-#   /depth_scan              — RealSense D435i depth_to_laserscan (obstacle layer)
-#   /camera/depth/color/points — RealSense D435i PointCloud2 (voxel layer, local only)
+#   /scan                    — RPLIDAR A1M8 360° LaserScan (obstacle)
+#   /depth_scan              — RealSense D435i depth_to_laserscan (obstacle)
+#   /camera/depth/color/points — RealSense D435i PointCloud2 (voxel)
 #
 # Inflation:
 #   inflation_radius: 0.3m (robot_radius 0.15m + 0.15m padding)
@@ -108,8 +108,8 @@ controller_server:
 
     progress_checker:
       plugin: "nav2_controller::SimpleProgressChecker"
-      required_movement_radius: 0.2        # Issue #479: Minimum 20cm movement to detect progress
-      movement_time_allowance: 10.0        # 10 second window to detect movement, then attempt recovery
+      required_movement_radius: 0.5
+      movement_time_allowance: 10.0
 
     general_goal_checker:
       stateful: true
@@ -120,14 +120,14 @@ controller_server:
     FollowPath:
       plugin: "dwb_core::DWBLocalPlanner"
       debug_trajectory_details: false
-      # Velocity limits (conservative for FC + hoverboard ESC, Issue #475)
-      min_vel_x: -0.15          # allow limited reverse (half of max forward)
+      # Velocity limits
+      min_vel_x: -0.25          # allow limited reverse
       min_vel_y: 0.0
-      max_vel_x: 0.3            # conservative: 0.3 m/s linear
+      max_vel_x: 1.0
       max_vel_y: 0.0
-      max_vel_theta: 0.5         # conservative: 0.5 rad/s angular
+      max_vel_theta: 1.5
       min_speed_xy: 0.0
-      max_speed_xy: 0.3          # match max_vel_x
+      max_speed_xy: 1.0
       min_speed_theta: 0.0
       # Acceleration limits (differential drive)
       acc_lim_x: 2.5
@@ -185,8 +185,6 @@ smoother_server:
       do_refinement: true
 
 # ── Behavior Server (recovery behaviors) ────────────────────────────────────
-# Issue #479: Recovery behaviors with conservative speed/distance limits for FC+Hoverboard
-# ESC priority: E-stop (Issue #459) takes priority over any recovery behavior.
 behavior_server:
   ros__parameters:
     use_sim_time: false
@@ -195,40 +193,25 @@ behavior_server:
     local_footprint_topic: local_costmap/published_footprint
     global_footprint_topic: global_costmap/published_footprint
     cycle_frequency: 10.0
-    behavior_plugins: ["spin", "backup", "wait"]
-
-    # Spin recovery: In-place 90° rotation to escape local deadlock
+    behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"]
     spin:
       plugin: "nav2_behaviors/Spin"
-      spin_dist: 1.5708                    # 90 degrees (π/2 radians)
-      max_rotational_vel: 0.5              # Conservative: 0.5 rad/s for self-balancer stability
-      min_rotational_vel: 0.25             # Minimum angular velocity
-      rotational_acc_lim: 1.6              # Half of max (3.2) for smooth accel on self-balancer
-      time_allowance: 10.0                 # max 10 seconds to complete 90° rotation
-
-    # Backup recovery: Reverse 0.3m at 0.1 m/s to escape obstacles
-    # Conservative for FC (Flux Capacitor) + Hoverboard ESC drivetrain
     backup:
       plugin: "nav2_behaviors/BackUp"
-      backup_dist: 0.3                     # Reverse 0.3 meters (safe distance for deadlock escape)
-      backup_speed: 0.1                    # Very conservative: 0.1 m/s reverse
-      max_backup_vel: 0.15                 # Absolute max reverse velocity
-      min_backup_vel: 0.05                 # Minimum backup velocity threshold
-      time_allowance: 5.0                  # max 5 seconds (0.3m at 0.1m/s = 3s normal)
-
-    # Wait recovery: Pause 5 seconds to let obstacles move away
+    drive_on_heading:
+      plugin: "nav2_behaviors/DriveOnHeading"
     wait:
       plugin: "nav2_behaviors/Wait"
-      wait_duration: 5.0                   # 5 second pause
-
+    assisted_teleop:
+      plugin: "nav2_behaviors/AssistedTeleop"
     local_frame: odom
     global_frame: map
     robot_base_frame: base_link
     transform_tolerance: 0.1
     simulate_ahead_time: 2.0
-    max_rotational_vel: 0.5                # Conservative: self-balancer stability limit
-    min_rotational_vel: 0.25
-    rotational_acc_lim: 1.6                # Half of main limit for recovery behaviors
+    max_rotational_vel: 1.0
+    min_rotational_vel: 0.4
+    rotational_acc_lim: 3.2
 
 # ── Waypoint Follower ────────────────────────────────────────────────────────
 waypoint_follower:
@@ -243,15 +226,14 @@ waypoint_follower:
       waypoint_pause_duration: 200
 
 # ── Velocity Smoother ────────────────────────────────────────────────────────
-# Conservative speeds for FC + hoverboard ESC (Issue #475)
 velocity_smoother:
   ros__parameters:
     use_sim_time: false
     smoothing_frequency: 20.0
     scale_velocities: false
     feedback: "OPEN_LOOP"
-    max_velocity: [0.3, 0.0, 0.5]           # conservative: 0.3 m/s linear, 0.5 rad/s angular
-    min_velocity: [-0.15, 0.0, -0.5]
+    max_velocity: [1.0, 0.0, 1.5]
+    min_velocity: [-0.25, 0.0, -1.5]
     max_accel: [2.5, 0.0, 3.2]
     max_decel: [-2.5, 0.0, -3.2]
     odom_topic: /rtabmap/odom
@@ -269,13 +251,11 @@ local_costmap:
       global_frame: odom
       robot_base_frame: base_link
       rolling_window: true
-      width: 3
+      width: 3                          # 3m x 3m rolling window
       height: 3
       resolution: 0.05
-      robot_radius: 0.22
-      # Footprint: [x, y] in base_link frame, in counterclockwise order
-      # Robot footprint ~0.4m x 0.4m, with 2m lookahead buffer for controller stability
-      footprint: "[[-0.2, -0.2], [-0.2, 0.2], [0.2, 0.2], [0.2, -0.2]]"
+      robot_radius: 0.15
+      footprint: "[[-0.2, -0.2], [-0.2, 0.2], [0.2, 0.2], [0.2, -0.2]]"  # 0.4m x 0.4m
       plugins: ["obstacle_layer", "voxel_layer", "inflation_layer"]
 
       obstacle_layer:
@@ -330,7 +310,7 @@ local_costmap:
       inflation_layer:
         plugin: "nav2_costmap_2d::InflationLayer"
         cost_scaling_factor: 3.0
-        inflation_radius: 0.3           # robot_radius (0.15) + 0.15m padding for safety
+        inflation_radius: 0.3           # robot_radius (0.15) + 0.15m padding
 
       always_send_full_costmap: false
 
@@ -343,9 +323,8 @@ global_costmap:
       publish_frequency: 1.0
       global_frame: map
       robot_base_frame: base_link
-      robot_radius: 0.22
-      # Footprint: [x, y] in base_link frame, in counterclockwise order
-      footprint: "[[-0.2, -0.2], [-0.2, 0.2], [0.2, 0.2], [0.2, -0.2]]"
+      robot_radius: 0.15
+      footprint: "[[-0.2, -0.2], [-0.2, 0.2], [0.2, 0.2], [0.2, -0.2]]"  # 0.4m x 0.4m
       resolution: 0.05
       track_unknown_space: true
       plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
@@ -395,7 +374,7 @@ global_costmap:
       inflation_layer:
         plugin: "nav2_costmap_2d::InflationLayer"
         cost_scaling_factor: 3.0
-        inflation_radius: 0.3           # robot_radius (0.15) + 0.15m padding for safety
+        inflation_radius: 0.3           # robot_radius (0.15) + 0.15m padding
 
       always_send_full_costmap: false