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 perception objects and reference images 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.6.0 (2026-02-14)

• feat: add Jazzy support for rosbag APIs (#347)

• feat(perception_reproducer): enable compressed image visualization (#339)

  • feat(perception_reproducer): support also searching and outputting images
  • feat: update the default topic names in helping
  • doc: update README for reference image setting

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

• feat(perception_replayer, perception_reproducer): refactor code and publish occupancy_grid_map (#328)

  • fist commit
  • remove unused code
  • refactor code
  • pre-commit
  • fix noise bug
  • fix killer timer

  * refactor func ---------

• Contributors: Kem (TiankuiXian), Ryohsuke Mitsudome, Yuxuan Liu

0.5.0 (2025-12-30)

• feat(perception_replayer): cpp implementation of perception replayer (#321)

  • feat cpp version perception_replayer/reproducer
  • delete comment
  • fix pre-commit error
  • fix spell
  • fix package.xml depends
  • update readme and fix options
  • add error ouptut
  • add noise and pub_route option
  • fix option behavior
  • delete unnecessary depend
  • fix typo
  • throttling warn message

  * throttling warn message ---------

• Contributors: Kotakku

0.4.0 (2025-11-16)

• feat(trajectory_visualizer): sort topic list (#307)

  • sort topic list
  • fix pre-commit error
  • add README topic display ordering section
  • fix inital topics selection
  • config path
  • fix pre-commit error

  * add copylight ---------

• feat(planning_debug_tools): improve trajectory visualizer usability (#300)

  • feat equal aspect ratio when XY-plot
  • add unwrapped yaw plot

  * fix pre-commit ---------

• fix: change to detailed library reference (#295)

• feat(planning_debug_tools): support V2I (#292)

• fix(planning_debug_tools): fix InitializeLocalization msg import (#291)

• fix: show motion velocity planner output by default (#290)

• Contributors: Kotakku, Satoshi OTA, Xiaoyu WANG, Yukinari Hisaki

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)

File truncated at 100 lines see the full file