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.8.0 (2026-10-19)

• feat(perception_replayer, perception_reproducer): add object filter options around the recorded ego pose (#468)

  * feat(perception_replayer, perception_reproducer): add object filter options around the recorded ego pose Add CLI options to limit the published objects by their position relative to the recorded ego pose, which helps to find the objects that affect the planning behavior.

  • --filter-radius <max> | <min>,<max>: 2D distance range [m]

  - --filter-angle <min>,<max>: angle range [deg] in the ego frame (0 is front, counterclockwise is positive, wraps through the rear if min > max)

  • --filter-class <class,...>: classes judged by the highest probability

  - --filter-invert: remove the matching objects instead of keeping them The filter logic is in utils.hpp and the option parsing is shared via help_utils.hpp.

  * fix(planning_debug_tools): use autoware_utils_math::normalize_radian utils::normalize_radian does not exist on main, so use the one in autoware_utils_math and add it to package.xml. ---------Co-authored-by: kyo0221 <<kyo.yamashita@tier4.jp>>

• fix: replay-route does not work in DLR test (#446) fix replay-route bug in dlr test

• Contributors: Kem (TiankuiXian), KyoYamashita

0.7.0 (2026-05-01)

• feat(planning_debug_tools): replace autoware_universe_utils with specific autoware_utils sub-packages (#413)

• fix(planning_debug_tools): migrate to rclcpp typesupport helpers (#403)

• fix: reprodocuer route bug (#374)

  • fix: reprodocuer route bug

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

• feat(perception_reproducer): publish route from rosbag (#369)

  • change pub-route to set-goal and make the new pub-route really publishing route
  • pre-commit

  * refactor argment names ---------

• Contributors: Kem (TiankuiXian), Mete Fatih Cırıt, Vishal Chauhan

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)

File truncated at 100 lines see the full file