# speed_profiles.yaml
# Outdoor adaptive speed profiles for SaltyBot follow-me mode.
#
# Run with:
#   ros2 launch saltybot_speed_controller outdoor_speed.launch.py
#
# IMPORTANT: This controller outputs /cmd_vel which feeds into cmd_vel_bridge
# (PR #46). The bridge applies its own hard caps (max_linear_vel=0.5 m/s).
# For outdoor/ride profiles you MUST re-launch cmd_vel_bridge with higher limits:
#   ros2 launch saltybot_bridge cmd_vel_bridge.launch.py max_linear_vel:=8.0
#
# Data flow:
<<<<<<< HEAD
#   person_follower → /cmd_vel_raw → [speed_controller] → /cmd_vel → cmd_vel_bridge → ESP32 BALANCE
=======
#   person_follower → /cmd_vel_raw → [speed_controller] → /cmd_vel → cmd_vel_bridge → ESP32-S3
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)

# ── Controller ─────────────────────────────────────────────────────────────────
control_rate:   50.0           # Hz — 50ms tick, same as cmd_vel_bridge
input_topic:    /cmd_vel_raw   # Upstream cmd_vel source
output_topic:   /cmd_vel       # Output to cmd_vel_bridge
initial_profile: walk          # Start in safest profile until target velocity known

# ── Profile selection ──────────────────────────────────────────────────────────
# Hysteresis prevents rapid walk↔jog↔ride oscillation.
# up_ticks: consecutive ticks above threshold before upgrading profile (50Hz → 5 = 100ms)
# down_ticks: ticks below threshold before downgrading (50Hz → 15 = 300ms)
#   Downgrade is slower (safety: prefer lower profile when uncertain)
hysteresis_up_ticks:   5
hysteresis_down_ticks: 15

# ── Emergency stop ─────────────────────────────────────────────────────────────
# Triggered when target stops suddenly:
#   (a) target velocity drops below stop_vel_threshold, OR
#   (b) target deceleration exceeds hard_stop_decel_threshold m/s²
#
# emergency_decel_mps2: how fast the robot decelerates to zero.
# Conservative (0.5-1.0 m/s²) is safer for balance; too fast = tip-over.
# At ride speed (8 m/s), 2.0 m/s² = 4 second stop distance ≈ 16m — acceptable.
emergency_decel_mps2:       2.0    # m/s² — hard brake limit
stop_vel_threshold:         0.4    # m/s  — target speed below this = stopped
hard_stop_decel_threshold:  3.0    # m/s² — target decel above this = hard stop

# ── GPS runaway protection ─────────────────────────────────────────────────────
# If GPS speed > commanded_speed × runaway_factor AND GPS speed > 50% of
# profile max, trigger emergency decel (catches downhill runaways).
gps_runaway_factor:  1.5    # 1.5× commanded = runaway
gps_timeout:         3.0    # s — ignore GPS if stale (tunnel, bridge)

# ── UWB range ─────────────────────────────────────────────────────────────────
uwb_range_field:  range    # JSON field name in /uwb/ranges messages
uwb_timeout:      2.0      # s — use GPS-only profile if UWB stale

# ── Speed profiles ─────────────────────────────────────────────────────────────
# Each profile applies when estimated target velocity is in [0, target_vel_max].
# Velocity limits are BEFORE cmd_vel_bridge caps (bridge must also be set high).
#
# Acceleration notes:
#   - Balance robot needs time to adjust lean angle when speed changes.
#   - accel_limit at RIDE (0.3 m/s²) means 0→8 m/s takes ~27s — intentionally slow.
#   - Increase only after validating balance PID tuning at speed.
#   - decel_limit can be slightly higher than accel_limit (braking > accelerating).

walk:
  # Person walking at 0–2 m/s (≈ 0–7 km/h)
  max_linear_vel:   1.5     # m/s  (= 5.4 km/h)
  max_angular_vel:  1.2     # rad/s
  accel_limit:      0.6     # m/s²  — moderate, balance handles it
  decel_limit:      1.0     # m/s²
  target_vel_max:   2.0     # m/s  — target faster than this → upgrade to jog

jog:
  # Person jogging at 2–5 m/s (≈ 7–18 km/h)
  max_linear_vel:   3.0     # m/s  (= 10.8 km/h)
  max_angular_vel:  1.5     # rad/s
  accel_limit:      0.5     # m/s²  — a bit gentler at higher speed
  decel_limit:      0.8     # m/s²
  target_vel_max:   5.0     # m/s  — target faster than this → upgrade to ride

ride:
  # Person on EUC at 5–15 m/s (≈ 18–54 km/h / 20–30 km/h typical follow-me)
  max_linear_vel:   8.0     # m/s  (= 28.8 km/h)
  max_angular_vel:  1.5     # rad/s — conservative for balance at speed
  accel_limit:      0.3     # m/s²  — very gentle: balance dynamics change at 8 m/s
  decel_limit:      0.5     # m/s²  — gradual stop from 8 m/s ≈ 16s / ~64m
  target_vel_max:   15.0    # m/s  — cap; EUC max ~30 km/h = 8.3 m/s typical

# ── Notes ─────────────────────────────────────────────────────────────────────
<<<<<<< HEAD
# 1. To enable ride profile, the Jetson → ESP32 BALANCE cmd_vel_bridge must also be
#    reconfigured: max_linear_vel=8.0, ramp_rate=500 → consider ramp_rate=150
#    at ride speed (slower ramp = smoother balance).
#
# 2. The ESP32 BALANCE balance PID gains likely need retuning for ride speed. Expect
=======
# 1. To enable ride profile, the Jetson → ESP32-S3 cmd_vel_bridge must also be
#    reconfigured: max_linear_vel=8.0, ramp_rate=500 → consider ramp_rate=150
#    at ride speed (slower ramp = smoother balance).
#
# 2. The ESP32-S3 balance PID gains likely need retuning for ride speed. Expect
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
#    increased sensitivity to pitch angle errors at 8 m/s vs 0.5 m/s.
#
# 3. Test sequence recommendation:
#    - Validate walk profile on flat indoor surface first
#    - Test jog profile in controlled outdoor space
#    - Only enable ride profile after verified balance at jog speeds
