# person_follower_params.yaml
# Configuration for person_follower_node — proportional person-following controller.
#
# Run with:
#   ros2 launch saltybot_follower person_follower.launch.py
# Or override individual params:
#   ros2 launch saltybot_follower person_follower.launch.py follow_distance:=1.2
#
# IMPORTANT: This node publishes raw /cmd_vel. The cmd_vel_bridge_node (PR #46)
# applies the ESC ramp, deadman switch, and STM32 AUTONOMOUS mode gate.
# Do not run this node without the cmd_vel bridge running on the same robot.

# ── Follow geometry ────────────────────────────────────────────────────────────
# The target distance to maintain behind the person (metres).
# Decrease for tighter following; increase for more standoff.
follow_distance:  1.5         # metres

# ±dead_zone around follow_distance — no linear velocity command in this band.
# Prevents jitter when the robot is approximately at target distance.
# Must be > position noise of /person/target detector.
dead_zone:        0.3         # metres

# ── Proportional gains ────────────────────────────────────────────────────────
# linear.x  = clamp(kp_linear  * (distance - follow_distance), ±max_linear_vel)
# angular.z = clamp(kp_angular * bearing,                       ±max_angular_vel)
#
# kp_linear:  higher → faster approach/retreat but more oscillation
# kp_angular: higher → tighter heading tracking but may cause weaving
kp_linear:        0.6
kp_angular:       1.2

# ── Velocity limits ────────────────────────────────────────────────────────────
# Hard caps on the cmd_vel output of this node.
# The cmd_vel bridge ALSO clamps (max_linear_vel=0.5, max_angular_vel=2.0),
# so these limits should be ≤ bridge limits for this node to have effect.
#
# Balance robot: keep max_linear_vel ≤ 0.5 m/s until balance is well-tuned.
max_linear_vel:   0.5         # m/s
max_angular_vel:  1.0         # rad/s  (conservative — avoid spinning out)

# ── Lost-target timeouts ──────────────────────────────────────────────────────
# lost_timeout:   seconds after last /person/target before entering STOPPING
#   (publishing zero cmd_vel — cmd_vel bridge deadman will also kick in).
lost_timeout:     2.0         # seconds

# search_timeout: seconds after last /person/target before entering SEARCHING
#   (slow rotation to re-acquire person).
search_timeout:   5.0         # seconds

# Angular velocity while in SEARCHING state (rad/s, positive = CCW/left).
# Keep small — robot is on a balance platform.
search_angular_vel: 0.3       # rad/s

# ── Obstacle avoidance ────────────────────────────────────────────────────────
# The node checks a forward corridor in the Nav2 local costmap.
# If any cell >= obstacle_threshold, forward cmd_vel is zeroed (turning allowed).
#
# obstacle_check_dist: how far ahead to check (metres)
# robot_width:         corridor width for obstacle check (metres)
# obstacle_threshold:  costmap cell value treated as obstacle
#   Nav2 conventions: free=0, inscribed=99, lethal=100, unknown=-1
#   70 = "probably occupied" — conservative; raise to 99 for less cautious
obstacle_check_dist:  0.5     # metres
robot_width:          0.3     # metres (corridor half-width = 0.15m each side)
obstacle_threshold:   70      # costmap value ≥ this → obstacle

# ── Control loop ──────────────────────────────────────────────────────────────
control_rate:     20.0        # Hz — lower than cmd_vel bridge (50Hz) by design

# ── Mode integration ──────────────────────────────────────────────────────────
# Master enable for the follow controller. When false, node publishes zero cmd_vel.
# Toggle at runtime: ros2 param set /person_follower follow_enabled false
# The cmd_vel bridge independently gates on STM32 AUTONOMOUS mode (md=2).
follow_enabled:   true
