Improve behavior_velocity_planner's latency

[soblin]

Checklist

• I've read the contribution guidelines.
• I've searched other issues and no duplicate issues were found.
• I've agreed with the maintainers that I can plan this task.

Description

Report from @veqcc.

Currently the latency of onTrigger function in behavior_velocity_planner has a big influence in Autoware's entire latency. Below figures show (1) the latency of behavior_path_planner and (2) behavior_velocity_planner respectively, measured in planning_simulation tutorial.

Especially the latency of behavior_velocity_planner is constantly not negligible (about 200ms ) given the behavior_path_planner's publication rate of 10Hz, which indicates its latency is twice the subscription rate.

@veqcc and @soblin are working to lower this computation time as much as possible. I'm not sure about the lower bound of this latency, but it should be at least below the behavior_path_planner's publication rate of 10Hz.

Possible approaches

• get statistics on the functions calls
  • measure elapsed time by binary search
  • get FlameGraph
• refactor current implementation
  • check the logic flow and return instantly as much as possible
  • avoid and delay heavy computation
  • cache lanelet stuff because this data is expected to be almost the same in each unit of behavior_velocity module.
    • define a cache class for lanelet that has CompoundPolygon3d and tier4_autowawe_utils::Polygon2d in it
• do calculation in-place

Definition of done

• measure the performance in realistic cases with a massive map, various & numerous obstacles, with multiple lane elements and characteristics (turn_direction, walkway, turn_signal, occlusion spot, blind spot, etc.)
• 2x ~ 3x performance improvement would be good.

[soblin]

Refactor/Performance PR:

• refactor(intersection): code clean #2071
• perf(crosswalk): reduce processing time #2137
• perf(intersection): reduce bg::within call #2190
• fix(behavior_velocity_planner): int/uint for path index #2221
• perf(behavior_velocity_planner): remove unnecessary debug data #2462

[VRichardJP]

Which tools do you use for profiling? I have used gperftools in the past but is there any tool recommended for autoware?

Out of curiosity, I went on and tried profiling behavior_path_planner/behavior_velocity_planner.
I did the following:

• added prefix="gnome-terminal -- env LD_PRELOAD=/lib/x86_64-linux-gnu/libprofiler.so CPUPROFILESIGNAL=12 CPUPROFILE_FREQUENCY=1000 CPUPROFILE=/tmp/behavior_velocity_planner.profile ", to behavior_planning_container in behavior_planning.launch.py
• launched planning_simulator, and prepared a simple environment with a few obstacles.
• started and stopped profiling with kill -12 <CONTAINER_PID> (see https://gperftools.github.io/gperftools/cpuprofile.html)
• generated the call tree below with: google-pprof --web install/behavior_velocity_planner/lib/behavior_velocity_planner/behavior_velocity_planner_node /tmp/behavior_velocity_planner.profile (can also use --cachegrind option instead of --web to explore the logs with kcachegrind)

For example, my sample profile shows that most of the computing time is spent in the 4 functions:

• behavior_path_planner::util::resamplePathWithSpline()
• behavior_path_planner::util::imageToOccupancyGrid()
• cv::Mat::operator=() (in behavior_path_planner::util::generateDrivableArea())
• behavior_path_planner::AvoidanceModule::calcShiftLines()

Do you have any better way to track down the performance issues?

[veqcc]

We have used CARET (https://github.com/tier4/caret), which is an open source performance analysis tool developed by TIER IV.
Please read the CARET documentatiion!! (https://tier4.github.io/CARET_doc/main/)

[veqcc]

The performance improvement result is in the following figure.
At the peaks of the graph, 2~2.5x performance optimizations have been realized.