Skip to content

21.2.0

Choose a tag to compare

View all tags
@github-actions github-actions released this 16 Dec 10:51
21.2.0
f84a264

Description

Abstract

This PR refactors the FollowTrajectoryAction implementation. The new code is simpler and easier to maintain. It uses a sliding window model and unified arrival detection. The changes remove complex overshoot detection logic. Final and intermediate waypoints now use separate state machines. The code is reorganized into clear logical sections.

Background

The old follow_trajectory.cpp code became too complex over time. It had many problems:

  1. Two different arrival checks: Final waypoints used strict 0.01m tolerance. Overshoot cases used 0.1m tolerance. This created two separate code paths with different behavior.

  2. Complex overshoot detection: The code used inner product calculations to detect when the entity passed waypoints. This required complicated edge case handling.

  3. Mixed concerns: Validation, distance calculations, time checks, and waypoint logic were mixed together in a single 600+ line if-else chain.

  4. Unclear control flow: It was hard to trace execution and understand which conditions led to which actions.

  5. Limited reusability: Calculations were written inline instead of using reusable functions.

After discussions (see RJD-1921), we found ways to simplify the code while keeping it correct and making it more robust.

Details

Core changes:

1. Sliding Window Model

  • Before: Processed waypoints one by one from the queue. Entity moved directly from current position to target.
  • After: Uses a two-waypoint sliding window. vertices[0] = previous waypoint. vertices[1] = target waypoint.
  • Benefit: Gives clear direction context. Makes orientation calculation more stable. Trajectory following is smoother.

2. Orientation Calculation

  • Before: Calculated orientation from entity's current position → target waypoint.
  • After: Calculates orientation from previous waypoint → target waypoint.
  • Benefit: Orientation stays stable. It follows the trajectory path better. Less jitter when entity moves slightly off path. Not affected by entity's current position.

3. Unified Arrival Tolerance

  • Before: Two different tolerances. Strict 0.01m for final waypoints. Lenient 0.1m for overshoot cases.
  • After: One tolerance for all cases (~0.1m).
  • Benefit: Simpler logic. More forgiving of small errors. Fewer false failures from strict tolerances.

4. Removal of Overshoot Detection

  • Before: Used complex inner product checks. Detected when entity passed waypoint and velocity reversed direction.
  • After: Simple distance check: this_step_distance >= distance → advance waypoint.
  • Benefit: Removes complex edge cases. Distance-based method is more robust and easier to understand.

5. Separate State Machines

  • Before: One big if-else chain. Mixed all waypoint types and conditions together.
  • After: Two separate functions:
    • handle_final_waypoint() - Handles the last waypoint (requires vehicle to stop)
    • handle_intermediate_waypoint() - Handles waypoints before the last one (allows pass-through)
  • Benefit: Clear separation. Easier to understand. State transitions are explicit. Behavior is documented in tables.

Final Waypoint State Table:

┌──────────────────────────────┬─────────────────────────────────┬──────────────────────┐
│ Entity State                 │ Timing Condition                │ Return               │
├──────────────────────────────┼─────────────────────────────────┼──────────────────────┤
│ Distance ≤ threshold         │ No arrival time specified       │ nullopt              │
│ Velocity = 0                 │ (remaining_time = NaN)          │ (trajectory done)    │
│ Acceleration = 0             │                                 │                      │
├──────────────────────────────┼─────────────────────────────────┼──────────────────────┤
│ Distance ≤ threshold         │ Arrival time reached            │ nullopt              │
│ Velocity = 0                 │ (remaining_time < step_time/2)  │ (trajectory done)    │
│ Acceleration = 0             │                                 │                      │
├──────────────────────────────┼─────────────────────────────────┼──────────────────────┤
│ Distance ≤ threshold         │ Arrival time not reached        │ EntityStatus         │
│ Velocity = 0                 │ (remaining_time ≥ step_time/2)  │ velocity = 0 (wait)  │
│ Acceleration = 0             │                                 │                      │
├──────────────────────────────┼─────────────────────────────────┼──────────────────────┤
│ Distance > threshold         │ Any                             │ throw Error          │
│ Velocity = 0                 │                                 │                      │
│ Acceleration = 0             │                                 │                      │
├──────────────────────────────┼─────────────────────────────────┼──────────────────────┤
│ Distance > threshold         │ Any                             │ EntityStatus         │
│ Velocity > 0                 │                                 │ move toward waypoint │
└──────────────────────────────┴─────────────────────────────────┴──────────────────────┘

Note: The case where distance ≤ threshold but velocity > 0 (braking to stop) is marked as TODO in code and currently falls through to the general movement case.

Intermediate Waypoint State Table:

┌───────────────────────────────────┬──────────────────────────────┬──────────────────────┐
│ Distance Check                    │ Timing Condition             │ Return               │
├───────────────────────────────────┼──────────────────────────────┼──────────────────────┤
│ step_distance < remaining_distance│ No arrival time specified    │ EntityStatus         │
│ (waypoint not reached yet)        │ (remaining_time = NaN)       │ move toward waypoint │
├───────────────────────────────────┼──────────────────────────────┼──────────────────────┤
│ step_distance < remaining_distance│ Arrival time not reached     │ EntityStatus         │
│ (waypoint not reached yet)        │ (remaining_time ≥ step/2)    │ move toward waypoint │
├───────────────────────────────────┼──────────────────────────────┼──────────────────────┤
│ step_distance ≥ remaining_distance│ No arrival time specified    │ Recurse              │
│ (waypoint reached in this step)   │ (remaining_time = NaN)       │ (advance waypoint)   │
├───────────────────────────────────┼──────────────────────────────┼──────────────────────┤
│ step_distance ≥ remaining_distance│ Arrival time reached         │ Recurse              │
│ (waypoint reached in this step)   │ (remaining_time < step/2)    │ (advance waypoint)   │
├───────────────────────────────────┼──────────────────────────────┼──────────────────────┤
│ step_distance < remaining_distance│ Arrival time reached         │ throw Error          │
│ (waypoint not reached yet)        │ (remaining_time < step/2)    │                      │
└───────────────────────────────────┴──────────────────────────────┴──────────────────────┘

6. Code Organization

The code is now organized into clear sections:

  • WAYPOINT QUERIES: Functions to get waypoint information
  • DISTANCE: Functions to calculate distances
  • TIME: Functions to calculate remaining time
  • VALIDATION: Functions to check for errors
  • ENTITY UPDATE: Logic to update position and velocity
  • VELOCITY: Functions to calculate velocity
  • WAYPOINT HANDLING: State machine functions and logging
  • EXECUTION: Main control logic

Benefit: Easy to navigate. Logical grouping. Self-documenting structure.

7. Query Functions

  • Before: Calculations written directly in the code wherever needed.
  • After: Separate query functions with clear names. Examples:
    • distance_to_target_waypoint() - Gets distance to target waypoint
    • remaining_time_to_waypoint() - Gets remaining time to any waypoint
    • is_target_waypoint_final() - Checks if this is the last waypoint
    • And more (11 functions total)
  • Benefit: Functions can be reused. Clear naming. One place for each calculation.

8. Validation Improvements

  • Before: Validation checks mixed with control flow. Used complex template code.
  • After: Separate validation functions. They run at the start before main logic. Examples:
    • validate_entity_status() - Checks position, velocity, acceleration for errors
    • validate_trajectory() - Checks waypoint count and positions
    • validate_arrival_time() - Checks if timing is possible
    • And more (5 validation functions total)
  • Benefit: Errors found quickly. Clearer error messages. Easy to add new checks.

9. Better Documentation

Added detailed documentation:

  • Function purpose and behavior
  • Return value meaning
  • All error conditions
  • State machine tables (shown above)
  • Clear mapping: (entity state, timing) → action

10. Debug Logging

Added three debug flags:

  • verbose_input_state - Logs entity state at start
  • verbose_action - Logs every waypoint action
  • verbose_move_action - Controls "Move" action logging (reduces noise)

Log output shows: entity name, time, distance, remaining time, waypoint type, action, and reason.

What Was Removed:

  1. Complex template code → replaced with simple isfinite() function
  2. Inner product overshoot detection → replaced with distance-based logic
  3. Complex arrival conditions → replaced with single threshold check
  4. Nested if-else chains → replaced with state machine functions
  5. Unused code: CatmullRomSpline, innerProduct, normalize, truncate

Summary Table:

Aspect Before After Benefit
Waypoint Model Single target Sliding window (previous + target) Directional context
Orientation Entity→target Previous→target Path-aligned orientation
Arrival Tolerance Dual path (0.01m/0.1m) Unified (0.1m) Simpler, more forgiving
Overshoot Detection Inner product check Removed Simplified logic
Control Flow Monolithic if-else Separate state machines Clear separation
Code Organization Linear Sectioned Easy navigation
Query Functions Inline calculations Dedicated functions Reusable, composable
Validation Scattered Dedicated functions Fail-fast
Documentation Minimal Comprehensive with tables Self-documenting

Known Limitations

  1. Trajectory Requirement: Now requires minimum 2 waypoints (previous + target). Single-waypoint trajectories are not supported. This matches OpenSCENARIO standard requirements.

  2. Relaxed Final Tolerance: Final waypoint tolerance changed from 0.01m to 0.1m. This is more forgiving. It may allow slightly less precise stops. In practice, this change should not affect scenario quality. It improves robustness.

These limitations are acceptable. The improvements in code quality are significant. Testing confirms the refactored code works correctly with existing scenarios.

Related Issues

Assets 2
Loading