fix(motion_velocity_smoother): improve backward trajectory handling

[VRichardJP]

Description

Fixes #5976

Current support for backward trajectories is fairly limited and sometimes just happen to work "by accident" (e.g. some calculated data is wrong but mostly unused). In particular, I have found many backward driving issues with the motion_velocity_smoother node while implementing a new freespace planner.

Most functions in motion_velocity_smoother have been implemented with forward trajectories in mind and don't apply to backward trajectories. The "trick" currently used to handle backward trajectories is to "flip" the trajectory points velocity before processing it, to do as if the vehicle was driving forward and not backward, and then flip the output back. I think the idea is valid, however it is not properly implemented: for example it causes steering values to be incorrect on backward trajectories.

The "proper" way to implement the flipping trick is not just to flip the velocity but to flip everything: trajectory pose orientation, velocity, steering... but also the ego pose and velocity when we need to place ego vehicle on the flipped trajectory.

Note that the way the logic is implemented at the moment (local trajectory but global is_reverse_ parameter) is error prone and difficult to maintain. It would be worth packing trajectory data and ego state inside a single ("flippable") struct so that sub functions could just assume everything goes forward.

Related links

#5976

Tests performed

With the freespace planner in the planning simulator:

• curved forward/backward trajectories values for longitudinal_velocity_mps, acceleration_mps2, front_wheel_angle_rad have correct signs.
• same for single point turn trajectories
• external speed limit on above trajectories works fine.
• lanelet based driving still looks ok

Notes for reviewers

Interface changes

None.

Effects on system behavior

On all trajectories:

• motion_velocity_smoother now ignores trajectories mixing forward/backward driving. AFAIK this is not supposed to be supported and would most likely give strange result. But now it is checked.

On backward trajectory:

• motion_velocity_smoother output trajectory points values are now signed properly

Except for these, there should be no other visible behavior change.

Pre-review checklist for the PR author

The PR author must check the checkboxes below when creating the PR.

• I've confirmed the contribution guidelines.
• The PR follows the pull request guidelines.

In-review checklist for the PR reviewers

The PR reviewers must check the checkboxes below before approval.

• The PR follows the pull request guidelines.
• The PR has been properly tested.
• The PR has been reviewed by the code owners.

Post-review checklist for the PR author

The PR author must check the checkboxes below before merging.

• There are no open discussions or they are tracked via tickets.
• The PR is ready for merge.

After all checkboxes are checked, anyone who has write access can merge the PR.

[rej55]

LGTM.
Thank you for your contribution.

[stale]

This pull request has been automatically marked as stale because it has not had recent activity.

[xmfcx]

@takayuki5168 -san, could you assign someone from TIER IV for this issue? @VRichardJP is probably busy and this seems like an important feature.

[stale]

This pull request has been automatically marked as stale because it has not had recent activity.