feat: add planning debug tools

[TakaHoribe]

Description

Now there are many tools for planning debug, visualization, and analysis.
Here add a pkg to manage these debugging tools.

This PR has also a new feature to visualize the trajectory information on PlotJugglar.

TODO

• add readme
• make a config file to autmatically load the script

Related links

Tests performed

trajectory_analyzer-2022-07-17_00.16.22.mp4

with the following code on plotjuggler reactive scripts

in Global code

behavior_path = '/planning/scenario_planning/lane_driving/behavior_planning/path_with_lane_id/debug_info'
behavior_velocity = '/planning/scenario_planning/lane_driving/behavior_planning/path/debug_info'
motion_avoid = '/planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/trajectory/debug_info'
motion_smoother_latacc = '/planning/scenario_planning/motion_velocity_smoother/debug/trajectory_lateral_acc_filtered/debug_info'
motion_smoother = '/planning/scenario_planning/trajectory/debug_info'

in function(tracker_time)

PlotCurvatureOverArclength('k_behavior_path', behavior_path, tracker_time)
PlotCurvatureOverArclength('k_behavior_velocity', behavior_velocity, tracker_time)
PlotCurvatureOverArclength('k_motion_avoid', motion_avoid, tracker_time)
PlotCurvatureOverArclength('k_motion_smoother', motion_smoother, tracker_time)

PlotVelocityOverArclength('v_behavior_path', behavior_path, tracker_time)
PlotVelocityOverArclength('v_behavior_velocity', behavior_velocity, tracker_time)
PlotVelocityOverArclength('v_motion_avoid', motion_avoid, tracker_time)
PlotVelocityOverArclength('v_motion_smoother_latacc', motion_smoother_latacc, tracker_time)
PlotVelocityOverArclength('v_motion_smoother', motion_smoother, tracker_time)

PlotCurrentVelocity('localization_kinematic_state', '/localization/kinematic_state', tracker_time)

in Function Library

function PlotCurvatureOverArclength(name, path, timestamp)
  new_series = ScatterXY.new(name)
  index = 0
  while(true) do
    series_k = TimeseriesView.find( string.format( "%s/curvature.%d", path, index) )
    series_s = TimeseriesView.find( string.format( "%s/arclength.%d", path, index) )
    series_size = TimeseriesView.find( string.format( "%s/size", path) )

    if series_k == nil or series_s == nil then break end
    
    k = series_k:atTime(timestamp)	
    s = series_s:atTime(timestamp)
    size = series_size:atTime(timestamp)

    if index >= size then break end
  
    new_series:push_back(s,k)
    index = index+1
  end
end

function PlotVelocityOverArclength(name, path, timestamp)
  new_series = ScatterXY.new(name)
  index = 0
  while(true) do
    series_v = TimeseriesView.find( string.format( "%s/velocity.%d", path, index) )
    series_s = TimeseriesView.find( string.format( "%s/arclength.%d", path, index) )
    series_size = TimeseriesView.find( string.format( "%s/size", path) )

    if series_v == nil or series_s == nil then break end

    v = series_v:atTime(timestamp)	
    s = series_s:atTime(timestamp)
    size = series_size:atTime(timestamp)

    if index >= size then break end
    
    new_series:push_back(s,v)
    index = index+1
  end
end


function PlotCurrentVelocity(name, kinematics_name, timestamp)
  new_series = ScatterXY.new(name)
  series_v = TimeseriesView.find( string.format( "%s/twist/twist/linear/x", kinematics_name))
  if series_v == nil then 
    print("error")
    return
  end
  v = series_v:atTime(timestamp)	
  new_series:push_back(0.0, v)
end

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 #1362 (eeb6e81) into main (f603772) will decrease coverage by 0.06%.
The diff coverage is 0.00%.

@@           Coverage Diff            @@
##            main   #1362      +/-   ##
========================================
- Coverage   9.64%   9.58%   -0.07%     
========================================
  Files       1096    1099       +3     
  Lines      77147   77644     +497     
  Branches   17719   18041     +322     
========================================
+ Hits        7441    7442       +1     
- Misses     62465   62936     +471     
- Partials    7241    7266      +25     

Flag	Coverage Δ		*Carryforward flag
differential	3.82% <0.00%> (?)
total	9.62% <ø> (ø)		Carriedforward from ef3a045

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

Impacted Files	Coverage Δ
...clude/planning_debug_tools/trajectory_analyzer.hpp	0.00% <0.00%> (ø)
..._debug_tools/include/planning_debug_tools/util.hpp	0.00% <0.00%> (ø)
...g/planning_debug_tools/src/trajectory_analyzer.cpp	0.00% <0.00%> (ø)
...ity_planner/src/scene_module/intersection/util.cpp	0.00% <0.00%> (ø)
..._planner/src/scene_module/intersection/manager.cpp	0.00% <0.00%> (ø)
...ner/src/scene_module/crosswalk/scene_crosswalk.cpp	0.00% <0.00%> (ø)
...c/scene_module/intersection/scene_intersection.cpp	0.00% <0.00%> (ø)
planning/behavior_velocity_planner/src/node.cpp	0.84% <0.00%> (+0.38%)	⬆️

---

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 f603772...eeb6e81. Read the comment docs.

[taikitanaka3]

@TakaHoribe
Can you remove or move trajectory visualizer to this package,? Because purpose of the package is the same.
https://github.com/autowarefoundation/autoware.universe/tree/main/planning/motion_velocity_smoother/scripts

[TakaHoribe]

It seems this code causes an error in the latest version of the plotjugler (3.4.4-1focal.20220517.013511).

new_series = MutableScatterXY.new("xxx")

It worked in the old version: 3.4.2-1focal.20220430.111249.

[TakaHoribe]

Now the error problem was fixed.
facontidavide/PlotJuggler#699

[taikitanaka3]

LGTM