feat(traffic_simulator): support backward motion in FollowTrajectoryAction

[dmoszynski]

Description

Regressions

No regressions confirmed

Abstract

Add backward motion support to FollowTrajectoryAction via the follow_backwards parameter.
This enables realistic reversing behavior instead of unrealistic 180° in-place rotations.

Background

During work on RJD-1921, scenarios were found where FollowTrajectoryAction needs to drive vehicles backward.
Before this PR, FollowTrajectoryAction did not support this correctly.

The problem comes from how FollowTrajectoryAction works: it always turns the entity to face the nearest waypoint. This is fine for forward motion. But when the entity and trajectory are on a lanelet and the waypoints are behind the entity, the behavior is wrong:

• The vehicle rotates 180° in place to face the waypoint
• It then drives forward against traffic
• This creates unrealistic turns and wrong-way driving

This breaks scenarios that need backward parking or reversing along a trajectory.

Details

This PR introduces the follow_backwards parameter to enable backward motion in FollowTrajectoryAction.
Scenarios can now explicitly specify when an entity should follow trajectory backward.

Core changes:

1. New follow_backwards parameter
   • Added to PolylineTrajectory message (ros2 msg, protobuf, conversions)
   • Read from OpenSCENARIO trajectory ParameterDeclaration named follow_backwards (boolean)
   • Defaults to false for backward compatibility
   • When true, the entity drives backward along the trajectory
   • Note:
     • To enable follow_backwards = true, the initial velocity of the entity must be non-positive.
     • In other words, follow_backwards = true requires initial_velocity <= 0.
   • Example:

  - PrivateAction:
      LongitudinalAction:
        SpeedAction:
          SpeedActionDynamics:
            dynamicsDimension: time
            dynamicsShape: step
            value: 0
          SpeedActionTarget:
            AbsoluteTargetSpeed:
              value: -1.3889
    name: ''
  - PrivateAction:
      RoutingAction:
        FollowTrajectoryAction:
          TimeReference:
            Timing:
              domainAbsoluteRelative: relative
              offset: 0
              scale: 1
          TrajectoryFollowingMode:
            followingMode: position
          TrajectoryRef:
            Trajectory:
              ParameterDeclarations:
                ParameterDeclaration:
                  - name: follow_backwards
                    parameterType: boolean
                    value: true
              Shape:
                Polyline:
[...]

2. Orientation handling (key fix)

   • When follow_backwards=true, desired_velocity is reversed 180° around z-axis
   • The vehicle front faces away from the waypoint (opposite to the direction of motion)
   • This matches real-world backward driving
   • No more unrealistic in-place 180° rotation before moving

3. Negative speed support in makeUpdatedStatus

   • twist.linear.x now uses signed values: positive for forward, negative for backward
   • When follow_backwards=true, twist.linear.x is negative throughout calculations
   • All kinematic calculations updated to handle negative speeds
   • Speed magnitude uses std::abs() where needed

4. FollowWaypointController adaptation

   • Problem: FollowWaypointController expects speed >= 0 and does not support negative speeds
   • Workaround: Transform values at the interface:
     • Input: multiply speed and acceleration by direction_sign to make negative values positive
     • Controller: computes acceleration in its expected domain (non-negative speeds)
     • Output: multiply acceleration by direction_sign to restore the sign for backward motion
   • Physical correctness is kept through consistent sign transformation
   • Documented with comments and TODO for future native support

Backward compatibility and usage:

When follow_backwards is not provided or set to false, FollowTrajectoryAction behaves exactly as before.
All existing scenarios continue to work without any modifications.

To enable realistic backward motion in scenarios:

• Set follow_backwards=true in the trajectory ParameterDeclaration
• Ensure the entity's initial speed is non-positive (zero or negative)
• The entity will then reverse along the trajectory with proper orientation

Example behavior:

Before (incorrect):

scenario_35fef270-3e48-4435-ab1f-5c194e2b8bf8_11.mp4

After (correct):

scenario_35fef270-3e48-4435-ab1f-5c194e2b8bf8_11_fix.mp4

Known Limitations

• FollowTrajectoryAction does not support positive initial velocities when follow_backwards = true.
• FollowWaypointController does not natively support negative speeds yet; workaround transformation is required (see code TODOs)

[github-actions]

Checklist for reviewers ☑️

All references to "You" in the following text refer to the code reviewer.

• Is this pull request written in a way that is easy to read from a third-party perspective?
• Is there sufficient information (background, purpose, specification, algorithm description, list of disruptive changes, and migration guide) in the description of this pull request?
• If this pull request contains a destructive change, does this pull request contain the migration guide?
• Labels of this pull request are valid?
• All unit tests/integration tests are included in this pull request? If you think adding test cases is unnecessary, please describe why and cross out this line.
• The documentation for this pull request is enough? If you think adding documents for this pull request is unnecessary, please describe why and cross out this line.

[github-actions]

This PR may breaks the interface to external services! Please see https://tier4.atlassian.net/wiki/spaces/S/pages/1984987364

[sonarqubecloud]

Quality Gate passed

Issues
6 New issues
0 Accepted issues

Measures
0 Security Hotspots
2.6% Coverage on New Code
0.0% Duplication on New Code

See analysis details on SonarQube Cloud