fix(motion_utils/path_shifter): modify the sampling method

[zulfaqar-azmi-t4]

Signed-off-by: Muhammad Zulfaqar Azmi zulfaqar.azmi@tier4.jp

Description

The lane change candidate path performance is currently reduced, which might caused by the resampling of the candidate path.

Presently, the resamplingPathWithSpline method employs, const double s = motion_utils::calcSignedArcLength(transformed_path, 0, i) to compute the sum between path's 0th and ith index. This causes additional interation to be made.

To simplify the computation, a new function is introduce to return a vector of the interval sum. Therefore, the sum between path's 0th and ith index only computed once, reducing the need to compute them at each iteration.

Slight performance gain is observed (0.15 ~ 0.2 hz improvement observed)
Laptop specs

CPU: i7-11800H @ 2.30gHz x 16
RAM: 32 GB
Ubuntu: 22.04

Related links

Tests performed

1. Open planning_simulator and add RTCManagerPanel.
2. Set lane_change_right and lane_change_left to manual mode.
3. Test lane change scenario.
4. Publish constant twist as follow

   ros2 topic pub /simulation/input/initialtwist geometry_msgs/msg/TwistStamped "{twist: 
   {linear: {x: 10.0}}}" -r 100`
   

5. Echo the following topic

   ros2 topic hz /planning/path_candidate/lane_change
   

Before PR
Before approval, at 36km/h

ros2 topic hz /planning/path_candidate/lane_change
average rate: 8.384
min: 0.113s max: 0.170s std dev: 0.00584s

ros2 topic hz /planning/scenario_planning/lane_driving/behavior_planning/path_with_lane_id
average rate: 8.457
min: 0.111s max: 0.140s std dev: 0.00420s

After approval, at 36km/h

ros2 topic hz /planning/path_candidate/lane_change
average rate: 10.00
min: 0.090s max: 0.110s std dev: 0.00381s

ros2 topic hz /planning/scenario_planning/lane_driving/behavior_planning/path_with_lane_id
average rate: 10.00
min: 0.089s max: 0.113s std dev: 0.00427s

After PR
Before approval, at 36km/h

ros2 topic hz /planning/path_candidate/lane_change
average rate: 8.654
min: 0.110s max: 0.208s std dev: 0.00499s

ros2 topic hz /planning/scenario_planning/lane_driving/behavior_planning/path_with_lane_id
average rate: 8.675
min: 0.105s max: 0.145s std dev: 0.00403s

After approval, at 36km/h

ros2 topic hz /planning/path_candidate/lane_change
average rate: 10.00
min: 0.090s max: 0.110s std dev: 0.00392s

ros2 topic hz /planning/scenario_planning/lane_driving/behavior_planning/path_with_lane_id
average rate: 10.00
min: 0.086s max: 0.116s std dev: 0.00453s

Notes for reviewers

I named the motion_utils::calcSignedArcLengthPartialSum following stl's partial_sum. But if there is better naming, please let me know.

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

Base: 11.38% // Head: 11.54% // Increases project coverage by +0.15% 🎉

Coverage data is based on head (ce640dd) compared to base (1d39c6c).
Patch coverage: 46.15% of modified lines in pull request are covered.

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #2658      +/-   ##
==========================================
+ Coverage   11.38%   11.54%   +0.15%     
==========================================
  Files        1277     1277              
  Lines       89231    89473     +242     
  Branches    23619    23761     +142     
==========================================
+ Hits        10163    10328     +165     
- Misses      68297    68358      +61     
- Partials    10771    10787      +16     

Flag	Coverage Δ		*Carryforward flag
differential	12.52% <0.00%> (?)
total	11.38% <46.15%> (ø)		Carriedforward from 1d39c6c

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

Impacted Files	Coverage Δ
...nning/behavior_path_planner/src/path_utilities.cpp	0.00% <0.00%> (ø)
...ils/include/motion_utils/trajectory/trajectory.hpp	78.93% <66.66%> (+3.81%)	⬆️

Help us with your feedback. Take ten seconds to tell us how you rate us. Have a feature suggestion? Share it here.

☔ View full report at Codecov.
📢 Do you have feedback about the report comment? Let us know in this issue.

[TakaHoribe]

motion_utils also have a way to calculate a partial sum. Is this function different from that?
https://github.com/autowarefoundation/autoware.universe/blob/main/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp/#L420

[zulfaqar-azmi-t4]

yes, that is true, however, this function returns the current partial sum, instead of std::vector of partial sums.

What happen is that because here, each time i increases, the calcSignedArcLength is recalled, it will start computing partial sum from 0 -> new i again. With vector of partial sums, we only need to compute the partial sum once.

[TakaHoribe]

thanks!