fix(motion_velocity_smoother): reverse velocity handling

[TakaHoribe]

Description

Fixed the negative velocity handling.

The correct process must be

flipSpeedIfNegative();
planVelocity();
re-flipSpeedIfNegative();

Before this PR:

planVelocity_1();
flipSpeedIfNegative();
planVelocity_2();
re-flipSpeedIfNegative();
planVelocity_3();

where, the negative velocity was not considered in the process 1 and 3.

The old process causes unintentional sudden stop or low speed for backward motion.

Additionaly, the calcInitialMotion() function was fixed.
before:

  const auto prev_output_closest_point =
    trajectory_utils::calcInterpolatedTrajectoryPoint(prev_traj, input_traj.at(input_closest).pose);

after:

  const auto prev_output_closest_point =
    trajectory_utils::calcInterpolatedTrajectoryPoint(prev_traj, current_pose_ptr_->pose);

which was critical to driving at a slow speed.

Related links

Tests performed

Run in the parking scenario (or something that requires backward motion) and check if it runs correctly.

Notes for reviewers

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.

[codecov]

Codecov Report

Merging #1465 (da714f7) into main (141b7a9) will decrease coverage by 0.02%.
The diff coverage is 0.00%.

@@            Coverage Diff             @@
##             main    #1465      +/-   ##
==========================================
- Coverage   10.29%   10.27%   -0.03%     
==========================================
  Files        1119     1119              
  Lines       77647    77816     +169     
  Branches    17876    17876              
==========================================
  Hits         7997     7997              
- Misses      62155    62324     +169     
  Partials     7495     7495              

Flag	Coverage Δ		*Carryforward flag
differential	3.96% <0.00%> (?)
total	10.28% <0.00%> (ø)		Carriedforward from 141b7a9

*This pull request uses carry forward flags. Click here to find out more.

Impacted Files	Coverage Δ
...ity_smoother/src/motion_velocity_smoother_node.cpp	0.00% <0.00%> (ø)

---

Continue to review full report at Codecov.

Legend - Click here to learn more
Δ = absolute <relative> (impact), ø = not affected, ? = missing data
Powered by Codecov. Last update 9f4c833...da714f7. Read the comment docs.

[kosuke55]

thanks very much. It also works fine with pull_out.

fix_smoother_back-2022-07-29_19.53.13.mp4