Planning Debug Tools

This package contains several planning-related debug tools.

• Trajectory analyzer: visualizes the information (speed, curvature, yaw, etc) along the trajectory
• Closest velocity checker: prints the velocity information indicated by each modules
• Perception reproducer: generates detected objects from rosbag data in planning simulator environment
• processing time checker: displays processing_time of modules on the terminal
• logging level updater: updates the logging level of the planning modules.

Trajectory analyzer

The trajectory_analyzer visualizes the information (speed, curvature, yaw, etc) along the trajectory. This feature would be helpful for purposes such as “investigating the reason why the vehicle decelerates here”. This feature employs the OSS PlotJuggler.

Stop reason visualizer

This is to visualize stop factor and reason. see the details

How to use

please launch the analyzer node

ros2 launch planning_debug_tools trajectory_analyzer.launch.xml

and visualize the analyzed data on the plot juggler following below.

setup PlotJuggler

For the first time, please add the following code to reactive script and save it as the picture below! (Looking for the way to automatically load the configuration file…)

You can customize what you plot by editing this code.

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/path_optimizer/trajectory/debug_info'
motion_smoother_latacc = '/planning/scenario_planning/motion_velocity_smoother/debug/trajectory_lateral_acc_filtered/debug_info'
motion_smoother = '/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)

PlotAccelerationOverArclength('a_behavior_path', behavior_path, tracker_time)
PlotAccelerationOverArclength('a_behavior_velocity', behavior_velocity, tracker_time)
PlotAccelerationOverArclength('a_motion_avoid', motion_avoid, tracker_time)
PlotAccelerationOverArclength('a_motion_smoother_latacc', motion_smoother_latacc, tracker_time)
PlotAccelerationOverArclength('a_motion_smoother', motion_smoother, tracker_time)

PlotYawOverArclength('yaw_behavior_path', behavior_path, tracker_time)
PlotYawOverArclength('yaw_behavior_velocity', behavior_velocity, tracker_time)
PlotYawOverArclength('yaw_motion_avoid', motion_avoid, tracker_time)
PlotYawOverArclength('yaw_motion_smoother_latacc', motion_smoother_latacc, tracker_time)
PlotYawOverArclength('yaw_motion_smoother', motion_smoother, tracker_time)

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

in Function Library

```lua

function PlotValue(name, path, timestamp, value) new_series = ScatterXY.new(name) index = 0 while(true) do series_k = TimeseriesView.find( string.format( “%s/”..value..”[%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)

File truncated at 100 lines see the full file

Changelog for package planning_debug_tools

0.3.0 (2025-08-11)

• feat(trajectory_visualizer): add initial config and better layout (#273)

• feat(trajectory_visualizer): support for new message types (#272)

  • fix(trajectory_data): handle early return for empty or single element trajectory in get_arc_lengths
  • feat(trajectory_visualizer): support multiple message types in trajectory visualization and data processing
  • feat(trajectory_data): add index_values function to return indices of trajectory points
  • apply pre-commit
  • remove unused imports

  * fix(package.xml): add missing maintainer entry for Maxime CLEMENT ---------

• style(pre-commit): autofix (#275)

  • apply pre-commit

  * fix: add maintainer in planning_debug tools package.xml ---------

• feat(trajectory_visualizer): add visualization of X, Y, and Yaw values (#262)

• fix(planning): bug of loading the db3 rosbag for the perception replayer (#269)

  • fix loading rosbag bug

  * remove formatting chanes ---------

• feat(planning/perception_reproducer): add mcap bag format support (#246)

• fix: skip ureadable msg (#259) fix error check

• feat(planning_debug_tools): add trajectory visualizer tool (#251)

  • implement initial script
  • improvements
  • add todo
  • improve script and add arc length shift on ego pose
  • remove "plot" button (now we replot when topics selection is changed)
  • Add README
  • disable cspell for python code

  * add copyright ---------

• Contributors: Kem (TiankuiXian), Kyoichi Sugahara, Maxime CLEMENT, yukage-oya

0.2.0 (2025-03-24)

• feat(planning_debug_tools)!: replace VelocityLimit messages to autoware_internal_planning_msgs (#223)
• feat!: replace tier4_planning_msgs/PathWithLaneId with autoware_internal_planning_msgs/PathWithLaneId (#206)
  • feat!: replace tier4_planning_msgs/PathWithLaneId with autoware_internal_planning_msgs/PathWithLaneId
• Contributors: Ryohsuke Mitsudome

0.1.0 (2025-01-28)

• unify version to 0.0.0

• feat(planning_debug_tools)!: replace tier4_debug_msgs with tier4_internal_debug_msgs (#200)

• chore: sync files (#11) Co-authored-by: github-actions <<github-actions@github.com>> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

• fix(perception_reproducer): fix a small bug (#155)

  • fix bug.

  * style(pre-commit): autofix ---------Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

File truncated at 100 lines see the full file