perf(path_smoother): add ReplanChecker to decrease computation cost

[maxime-clem]

Description

Copy the ReplanChecker from the obstacle_avoidance_planner in order to improve the performance of the path_smoother.

Also needs launch PR: autowarefoundation/autoware_launch#482

Tests performed

Psim.
CPU usage before: 200-300%.
CPU usage after: 30-60%.

Effects on system behavior

None

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.

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.

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

[tkimura4]

Before:
ros2 topic echo /planning/scenario_planning/lane_driving/motion_planning/elastic_band_smoother/debug/calculation_time -f --field data

        insertFixedPoint:= 0.006 [ms]
        getPaddedTrajectoryPoints:= 0.009 [ms]
        updateConstraint:= 33.534 [ms]
        calcSmoothedTrajectory:= 1.355 [ms]
        convertOptimizedPointsToTrajectory:= 0.021 [ms]
      smoothTrajectory:= 35.182 [ms]
    onPath:= 35.198 [ms]
    applyInputVelocity:= 0.21 [ms]
  extendTrajectory:= 0.114 [ms]
onPath:= 35.563 [ms]
========================================

---

        insertFixedPoint:= 0.005 [ms]
        getPaddedTrajectoryPoints:= 0.004 [ms]
        updateConstraint:= 40.113 [ms]
        calcSmoothedTrajectory:= 0.168 [ms]
        convertOptimizedPointsToTrajectory:= 0.008 [ms]
      smoothTrajectory:= 40.416 [ms]
    onPath:= 40.422 [ms]
    applyInputVelocity:= 0.044 [ms]
  extendTrajectory:= 0.032 [ms]
onPath:= 40.509 [ms]
========================================

---

        insertFixedPoint:= 0.002 [ms]
        getPaddedTrajectoryPoints:= 0.003 [ms]
        updateConstraint:= 27.409 [ms]
        calcSmoothedTrajectory:= 0.108 [ms]
        convertOptimizedPointsToTrajectory:= 0.004 [ms]
      smoothTrajectory:= 27.596 [ms]
    onPath:= 27.599 [ms]
    applyInputVelocity:= 0.028 [ms]
  extendTrajectory:= 0.022 [ms]
onPath:= 27.655 [ms]
========================================

---

        insertFixedPoint:= 0.002 [ms]
        getPaddedTrajectoryPoints:= 0.002 [ms]
        updateConstraint:= 18.86 [ms]
        calcSmoothedTrajectory:= 0.116 [ms]
        convertOptimizedPointsToTrajectory:= 0.005 [ms]
      smoothTrajectory:= 19.052 [ms]
    onPath:= 19.055 [ms]
    applyInputVelocity:= 0.03 [ms]
  extendTrajectory:= 0.023 [ms]
onPath:= 19.113 [ms]
========================================

---

        insertFixedPoint:= 0.002 [ms]
        getPaddedTrajectoryPoints:= 0.002 [ms]
        updateConstraint:= 9.735 [ms]
        calcSmoothedTrajectory:= 0.113 [ms]
        convertOptimizedPointsToTrajectory:= 0.004 [ms]
      smoothTrajectory:= 9.924 [ms]
    onPath:= 9.927 [ms]
    applyInputVelocity:= 0.03 [ms]
  extendTrajectory:= 0.023 [ms]
onPath:= 9.985 [ms]
========================================

---

        insertFixedPoint:= 0.002 [ms]
        getPaddedTrajectoryPoints:= 0.003 [ms]
        updateConstraint:= 15.611 [ms]
        calcSmoothedTrajectory:= 0.163 [ms]
        convertOptimizedPointsToTrajectory:= 0.007 [ms]
      smoothTrajectory:= 15.87 [ms]
    onPath:= 15.876 [ms]
    applyInputVelocity:= 0.046 [ms]
  extendTrajectory:= 0.034 [ms]
onPath:= 15.966 [ms]
========================================

After:

    onPath:= 0.003 [ms]
    applyInputVelocity:= 0.193 [ms]
  extendTrajectory:= 0.128 [ms]
onPath:= 0.381 [ms]
========================================

---

    onPath:= 0 [ms]
    applyInputVelocity:= 0.05 [ms]
  extendTrajectory:= 0.039 [ms]
onPath:= 0.109 [ms]
========================================

---

        insertFixedPoint:= 0.003 [ms]
        getPaddedTrajectoryPoints:= 0.002 [ms]
        updateConstraint:= 22.217 [ms]
        calcSmoothedTrajectory:= 0.12 [ms]
        convertOptimizedPointsToTrajectory:= 0.004 [ms]
      smoothTrajectory:= 22.493 [ms]
    onPath:= 22.496 [ms]
    applyInputVelocity:= 0.029 [ms]
  extendTrajectory:= 0.024 [ms]
onPath:= 22.556 [ms]
========================================

---

    onPath:= 0.001 [ms]
    applyInputVelocity:= 0.071 [ms]
  extendTrajectory:= 0.051 [ms]
onPath:= 0.149 [ms]
========================================

---

    onPath:= 0.001 [ms]
    applyInputVelocity:= 0.048 [ms]
  extendTrajectory:= 0.037 [ms]
onPath:= 0.105 [ms]
========================================

---

    onPath:= 0.002 [ms]
    applyInputVelocity:= 0.12 [ms]
  extendTrajectory:= 0.084 [ms]
onPath:= 0.251 [ms]
========================================

[tkimura4]

Merging this PR has resulted in the optimization (smoothTrajectory()) in the path_smoother no longer being executed every frame.
I have also confirmed a significant decrease in CPU usage of path_smoother node.

cc. @takayuki5168

[codecov]

Codecov Report

Patch coverage: 16.66% and no project coverage change.

Comparison is base (13b96ad) 14.93% compared to head (5b6cd12) 14.93%.
Report is 16 commits behind head on main.

Additional details and impacted files

@@           Coverage Diff           @@
##             main    #4416   +/-   ##
=======================================
  Coverage   14.93%   14.93%           
=======================================
  Files        1515     1516    +1     
  Lines      104545   104635   +90     
  Branches    31777    31826   +49     
=======================================
+ Hits        15613    15628   +15     
- Misses      71895    71949   +54     
- Partials    17037    17058   +21     

Flag	Coverage Δ		*Carryforward flag
differential	35.24% <16.66%> (?)
total	14.93% <ø> (+<0.01%)	⬆️	Carriedforward from 13b96ad

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

Files Changed	Coverage Δ
..._smoother/include/path_smoother/common_structs.hpp	42.85% <0.00%> (-2.60%)	⬇️
...er/include/path_smoother/elastic_band_smoother.hpp	40.00% <ø> (ø)
planning/path_smoother/src/replan_checker.cpp	14.66% <14.66%> (ø)
...anning/path_smoother/src/elastic_band_smoother.cpp	35.91% <28.57%> (-0.52%)	⬇️

☔ View full report in Codecov by Sentry.
📢 Have feedback on the report? Share it here.

[ahmeddesokyebrahim]

Thanks @maxime-clem for this interesting PR.

We - planning & control team in Leo Drive - are curious about the change in this PR as we are facing a specific behavior in a specific scenario while testing autoware which we are trying to understand. And the reason we are curious about this PR is that the behavior before it has changed after.

Before using this PR, we have tested autoware and faced this weird ego vehicle stop as shown in this video demo before the stop line (please watch carefully starting from 0:26).

The obvious reason for me (not totally sure), there is a replan (optimization) happening that is why it stopped and replanned the trajectory. The log message during this stop was Replan since path forward shape changed.

After using this PR, this weird stop is not longer happening except in very minor trials - giving me the impressions out of the comments above that optimization is no longer happening in each frame. And when it happens, it stops and replan again.

Knowing that ...

• This weird stop does not happen if we disable stop_line module with exact same scenrio
• This weird stop does not happen if we start the vehicle somehow near to the stop line so that the speed is less that 15 km/h (or with same exact scenario with speed_bump module enabled so that it slows down before approaching the stop line)

From these observations and checking the code change, I am curious to understand some questions and would be very grateful to you if you can support us in that ...

1. There is a parameter added in replan section in elastic_band_smoother.param.yaml. The comment associated with this parameter # if true, only perform smoothing when the input changes significantly. I am curious to understand how this is used to perform smoothing when input changed significantly in the time that the parameter is only used inside isReplanRequired() to return true if it is false.
2. Is smoothing mentioned in the associated comment above equivalent to replanning/optimization in this context ?
3. Are we facing this weird stop with way more less frequency due to replanning/optmization is not happening now in every frame ?
4. What is the relation here between replanning/optimization and the path forward shape change ?
5. Do you have any ideas in mind to connect the dots why this happening with stop_line module enabled or not happening in lower speeds ? Does that relate somehow to the nature of replanning/optimization happening ?
6. As replan_checker is now copied to path_smoother and I have seen both isReplanRequired() is being called by path_smoother and obstacle_avoidance_planner , is that intended ? or is it planned in future that replan_checker to be only in path_smoother ?

Thanks in advance for your efforts and support, and please let me know if you would need any further questions/clarifications before answering the questions.

cc: @mehmetdogru - @beyzanurkaya

[maxime-clem]

Hello and thank you for your question. I will try to answer as best as I can but I may need to investigate further to provide you with a satisfactory reply. Do not hesitate to ask more questions if things are not clear.

1. I am curious to understand how this is used to perform smoothing when input changed significantly in the time that the parameter is only used inside isReplanRequired() to return true if it is false.

As you noted, if replan.enable is set to false, then isReplanRequired() always returns true.
This is correct but maybe the parameter naming is confusing and replan should be renamed to replan_checker. By default, the elastic_band_smoother always smooths the inputs it receives, which is computationally quite expensive. For this reason we use the replan_checker to check whether a new smoothing is required.
When enable is true, we will try to reuse the previous result instead of performing a new smoothing.

2. Is smoothing mentioned in the associated comment above equivalent to replanning/optimization in this context ?

It is equivalent to redoing the elastic band optimization. There is no real planning involved as no decisions are made and the input path is just made smoother.

3. Are we facing this weird stop with way more less frequency due to replanning/optmization is not happening now in every frame ?

This is a strange issue. If there is a stop line it seems correct if the vehicle stops. However if the behavior is impacted by the path_smoother then there could be a bug.

4. What is the relation here between replanning/optimization and the path forward shape change ?

If the input path received is very different from the previous one, we want to update the output as well and will redo the elastic band optimization.

5. Do you have any ideas in mind to connect the dots why this happening with stop_line module enabled or not happening in lower speeds ? Does that relate somehow to the nature of replanning/optimization happening ?

There could be a bug with the resampling of the velocities such that the stop point (a single point with 0.0 velocity) is somehow "lost" during resampling.

6. As replan_checker is now copied to path_smoother and I have seen both isReplanRequired() is being called by path_smoother and obstacle_avoidance_planner , is that intended ? or is it planned in future that replan_checker to be only in path_smoother ?

I think there will be some refactoring in the future, but both the path_smoother and the obstacle_avoidance_planner use the replan_checker logic to determine whether they need to perform a new optimization or not. The goal is the same and the implementation is very similar, but they are both needed.

[ahmeddesokyebrahim]

Thanks a lot @maxime-clem for the valuable answers from your side and sorry for my delayed response.
Actually, the interesting observation we have till now for this weird stop, that it does not happen when the first step of cropping trajectory in smoothTrajectory() in elastic_band.cpp is skipped. I mean by skipping here by depending on the following steps using traj_points instead of cropped_traj_points. I am not sure till now why cropping is introducing the stop point in this case.
As well, feeding path to obstacle_avoidance_planner directly from behavior_velocity_planner instead of path_smoother make the problem not to happen. That means looks like the stop point is coming from the path updated by path_smoother. This stop point I can not see in eb_traj nor in mpt_traj but it is visible in obstacle_avoidance_planner/trajectory.
If we have the time to investigate more on this, we will update you for sure.
Thanks again for the answers.

[maxime-clem]

@ahmeddesokyebrahim Sorry I should have discussed this during the working group today.

It sounds like a significant issue which should be addressed as soon as possible.
Can I ask you to create an issue ? Ideally with some instructions or rosbag to reproduce the problem. This way we can also investigate on our side.