Dynamic Rival Safety Distance

[2006wu]

Description

Currently, teb_controller uses fixed rival distance thresholds:

rival_slowdown_dist: 1.1
rival_close_distance: 0.8
rival_stop_distance: 0.55

The controller already uses three stages:

Slow → Close → Stop

This feature keeps the existing three-stage behavior, but adds a dynamic safety margin based on:

• Our robot's linear velocity
• Our robot's angular velocity
• Rival robot's velocity
• Relative / closing velocity between both robots

The goal is to make the controller react earlier in high-risk situations without dynamically changing all three thresholds.

---

Expected Behavior

• Low-speed motion → small safety margin
• High-speed straight motion → slightly larger safety margin
• High-speed turning → larger safety margin
• Rival moving away → little or no additional safety margin
• Rival approaching quickly → larger safety margin
• Both robots approaching quickly → significantly larger safety margin

The main value should be closing speed, instead of only using the rival's absolute speed.

---

Dynamic Rival Distance

Calculate an additional safety margin:

dynamic_safety_margin =
    self_motion_factor
    + relative_motion_factor

Initial idea:

self_motion_factor =
    k_turn * |v_self * w_self|

relative_motion_factor =
    k_relative * closing_speed

Where:

• v_self = our robot's linear velocity
• w_self = our robot's angular velocity
• closing_speed = how quickly the distance between both robots is decreasing

Then calculate:

effective_rival_distance =
    measured_rival_distance
    - dynamic_safety_margin

Example:

Measured distance = 1.00 m
Safety margin     = 0.25 m

Effective distance = 0.75 m

The existing Slow / Close / Stop logic can continue using effective_rival_distance.

The exact formula can be adjusted after testing.

---

Slow / Close / Stop Thresholds

Keep rival_stop_distance as the base value.

Only rival_stop_distance needs to be loaded from:

/home/share/data/rival_params.yaml

Calculate the other thresholds using offsets:

rival_close_distance = rival_stop_distance + rival_close_distance_offset

rival_slowdown_dist = rival_stop_distance + rival_slowdown_distance_offset

For example:

rival_stop_distance            = 0.55
rival_close_distance_offset    = 0.25
rival_slowdown_distance_offset = 0.55

Result:

Stop  = 0.55 m
Close = 0.80 m
Slow  = 1.10 m

If rival_stop_distance changes to 0.65:

Stop  = 0.65 m
Close = 0.90 m
Slow  = 1.20 m

This keeps the relative relationship between the three stages consistent.

---

Proposed Parameters

Fast:
  plugin: "teb_controller::TebController"

  external_rival_data_path: "/home/share/data/rival_params.yaml"

  rival_min_speed_scale: 0.5

  # Loaded from rival_params.yaml
  rival_stop_distance: 0.55

  # Slow / Close offsets
  rival_close_distance_offset: 0.25
  rival_slowdown_distance_offset: 0.55

  # Dynamic rival safety
  rival_turn_safety_gain: 0.0
  rival_closing_speed_gain: 0.0
  rival_max_safety_margin: 0.0

  rival_escape_arc_angle_deg: 270.0
  rival_escape_distance: 0.2

The exact parameter names and values can be adjusted during implementation and testing.

---

Flow

Our Linear Velocity ─────┐
Our Angular Velocity ────┤
                         ├──→ Dynamic Safety Margin
Rival Velocity ──────────┤             │
Closing Speed ───────────┘             ▼
                              effective_rival_distance
                                         │
                                         ▼
                                Existing TEB Logic
                                         │
                                  Slow → Close → Stop

Thresholds:

/home/share/data/rival_params.yaml
                │
                ▼
       rival_stop_distance
                │
        ┌───────┴────────┐
        │                │
        ▼                ▼
 + close_offset     + slowdown_offset
        │                │
        ▼                ▼
      Close             Slow

---

Tasks

• Check current rival distance logic in teb_controller
• Implement dynamic safety margin using self and rival motion
• Add maximum safety margin and filtering if necessary
• Keep the existing Slow / Close / Stop logic
• Generate Close and Slow thresholds using offsets
• Tune parameters and test locally
• Test and tune on the actual machines
• Update README