WATO Trajectory Planner

Package Summary

Version	1.0.0
License	Apache-2.0
Build type	AMENT_CMAKE
Use	RECOMMENDED

Repository Summary

Description
Checkout URI	https://github.com/watonomous/wato_monorepo.git
VCS Type	git
VCS Version	main
Last Updated	2026-04-03
Dev Status	UNKNOWN
Released	UNRELEASED
Contributing	Help Wanted (-) Good First Issues (-) Pull Requests to Review (-)

Package Description

Longitudinal trajectory planner for velocity profiling based on costmaps

Maintainers

WATonomous

Authors

No additional authors.

WATO Trajectory Planner

The trajectory_planner package refines a geometric path into a velocity-profiled trajectory. It checks for obstacles along the path using a vehicle footprint and smoothly decelerates when a collision is imminent.

Overview

The node subscribes to a path (from local_planning) and a costmap (from world_modeling). It interpolates the path at a fixed resolution, sweeps the vehicle footprint at each point, and finds the distance to the first lethal obstacle. Velocity at each point is then limited by kinematics-based braking: v = sqrt(2 * max_tangential_accel * braking_distance), ensuring the vehicle can always stop in time given its current speed and max deceleration.

The lane speed limit from /world_modeling/lanelet/lane_context further caps velocity when available.

Usage

ros2 launch trajectory_planner trajectory_planner.launch.yaml

Topic remappings:

input_path → /action/local_planning/path
costmap → /world_modeling/costmap
trajectory → /action/trajectory_planning/trajectory
lane_context → /world_modeling/lanelet/lane_context

Visualization

Publishes visualization_msgs/MarkerArray on ~trajectory_markers. Each point is rendered as a purple sphere whose diameter scales with target speed (larger = faster).

Configuration

Parameters are defined in config/trajectory_planner_params.yaml.

Parameter	Default	Description
`stop_distance`	2.0 m	Distance to obstacle where vehicle must be fully stopped.
`max_speed`	20.0 m/s	Maximum speed when no lanelet limit is available.
`max_tangential_accel`	1.0 m/s^2	Comfort braking deceleration for normal obstacle avoidance.
`max_emergency_accel`	5.0 m/s^2	Emergency braking deceleration when comfort braking is insufficient.
`max_lateral_accel`	0.5 m/s^2	Maximum lateral acceleration to slow down in curves.
`interpolation_resolution`	0.1 m	Point spacing along path for collision checking.
`footprint_frame`	`base_link`	Frame in which the footprint is defined.
`footprint_x_min`	-0.5 m	Rear extent of vehicle.
`footprint_x_max`	3.5 m	Front extent of vehicle (front bumper).
`footprint_y_min`	-1.2 m	Right extent of vehicle.
`footprint_y_max`	1.2 m	Left extent of vehicle.

Tuning Guide

Car stops too early/late: Adjust stop_distance or the footprint_x_max to match actual front bumper position.
Car is too jerky: Decrease max_tangential_accel for gentler braking.
Car clips obstacles on the sides: Increase footprint_y_min/footprint_y_max to widen the safety corridor.
High CPU usage: Increase interpolation_resolution (e.g. 0.2 m), but avoid missing narrow obstacles.

Troubleshooting

No trajectory output: Verify input_path and costmap topics are publishing and remapped correctly.
“TrajectoryCore: Empty path”: The upstream local planner is not producing a path.
Collisions not detected: Confirm the costmap contains lethal cells (cost > 100) and that TF transforms between the path frame and costmap frame are available.

CHANGELOG

No CHANGELOG found.

Package Dependencies

Deps	Name
	ament_cmake
	ament_cmake_gtest
	wato_test
	rclcpp
	rclcpp_components
	rclcpp_lifecycle
	nav_msgs
	geometry_msgs
	behaviour_msgs
	visualization_msgs
	lanelet_msgs
	wato_trajectory_msgs
	tf2_ros
	tf2_geometry_msgs
	wato_lifecycle_manager

System Dependencies

No direct system dependencies.

Dependant Packages

Name	Deps
action_bringup

Launch files

No launch files found

Messages

No message files found.

Services

No service files found

Plugins

No plugins found.

Recent questions tagged `trajectory_planner` at Robotics Stack Exchange

No version for distro jazzy showing github. Known supported distros are highlighted in the buttons above.

trajectory_planner package from wato_monorepo repo

ackermann_pure_pursuit action_bringup freeroam_planner global_planning lattice_planning lattice_planning_markers lattice_planning_msgs model_predictive_control mpc_controller osqp_eigen_vendor trajectory_planner wato_trajectory_msgs infrastructure_bringup tf_static_relay vision_msgs_markers wato_lifecycle_manager ackermann_mux camera_calib lidar_imu_calib lidar_lidar_calib can_state_estimator dummy_ackermann eve_description interfacing_bringup joystick_interfacing lidar_aggregator oscc_interfacing oscc_mux pid_control pid_msgs roscco_msg sensor_interfacing topic_healthchecker attribute_assigner depth_estimation feature_extraction patchworkpp patchworkpp_vendor perception_bringup perception_utils spatial_association bytetrack_cpp_vendor track_viz_2d tracking traffic_light_detection carla_bringup carla_common carla_control carla_lifecycle carla_localization carla_msgs carla_perception carla_pygame carla_ros_bridge carla_sample_description carla_scenarios carla_teleop simulation_bringup wato_test behaviortree_ros2_vendor behaviour behaviour_markers behaviour_msgs costmap costmap_markers costmap_msgs eidos eidos_msgs eidos_tools eidos_transform lanelet_markers lanelet_msgs lidar_localization prediction world_model world_model_markers world_model_msgs world_modeling_bringup

ROS Distro
github

Package Summary

Version	1.0.0
License	Apache-2.0
Build type	AMENT_CMAKE
Use	RECOMMENDED

Repository Summary

Description
Checkout URI	https://github.com/watonomous/wato_monorepo.git
VCS Type	git
VCS Version	main
Last Updated	2026-04-03
Dev Status	UNKNOWN
Released	UNRELEASED
Contributing	Help Wanted (-) Good First Issues (-) Pull Requests to Review (-)

Package Description

Longitudinal trajectory planner for velocity profiling based on costmaps

Maintainers

WATonomous

Authors

No additional authors.

WATO Trajectory Planner

The trajectory_planner package refines a geometric path into a velocity-profiled trajectory. It checks for obstacles along the path using a vehicle footprint and smoothly decelerates when a collision is imminent.

Overview

The node subscribes to a path (from local_planning) and a costmap (from world_modeling). It interpolates the path at a fixed resolution, sweeps the vehicle footprint at each point, and finds the distance to the first lethal obstacle. Velocity at each point is then limited by kinematics-based braking: v = sqrt(2 * max_tangential_accel * braking_distance), ensuring the vehicle can always stop in time given its current speed and max deceleration.

The lane speed limit from /world_modeling/lanelet/lane_context further caps velocity when available.

Usage

ros2 launch trajectory_planner trajectory_planner.launch.yaml

Topic remappings:

input_path → /action/local_planning/path
costmap → /world_modeling/costmap
trajectory → /action/trajectory_planning/trajectory
lane_context → /world_modeling/lanelet/lane_context

Visualization

Publishes visualization_msgs/MarkerArray on ~trajectory_markers. Each point is rendered as a purple sphere whose diameter scales with target speed (larger = faster).

Configuration

Parameters are defined in config/trajectory_planner_params.yaml.

Parameter	Default	Description
`stop_distance`	2.0 m	Distance to obstacle where vehicle must be fully stopped.
`max_speed`	20.0 m/s	Maximum speed when no lanelet limit is available.
`max_tangential_accel`	1.0 m/s^2	Comfort braking deceleration for normal obstacle avoidance.
`max_emergency_accel`	5.0 m/s^2	Emergency braking deceleration when comfort braking is insufficient.
`max_lateral_accel`	0.5 m/s^2	Maximum lateral acceleration to slow down in curves.
`interpolation_resolution`	0.1 m	Point spacing along path for collision checking.
`footprint_frame`	`base_link`	Frame in which the footprint is defined.
`footprint_x_min`	-0.5 m	Rear extent of vehicle.
`footprint_x_max`	3.5 m	Front extent of vehicle (front bumper).
`footprint_y_min`	-1.2 m	Right extent of vehicle.
`footprint_y_max`	1.2 m	Left extent of vehicle.

Tuning Guide

Car stops too early/late: Adjust stop_distance or the footprint_x_max to match actual front bumper position.
Car is too jerky: Decrease max_tangential_accel for gentler braking.
Car clips obstacles on the sides: Increase footprint_y_min/footprint_y_max to widen the safety corridor.
High CPU usage: Increase interpolation_resolution (e.g. 0.2 m), but avoid missing narrow obstacles.

Troubleshooting

No trajectory output: Verify input_path and costmap topics are publishing and remapped correctly.
“TrajectoryCore: Empty path”: The upstream local planner is not producing a path.
Collisions not detected: Confirm the costmap contains lethal cells (cost > 100) and that TF transforms between the path frame and costmap frame are available.

CHANGELOG

No CHANGELOG found.

Package Dependencies

Deps	Name
	ament_cmake
	ament_cmake_gtest
	wato_test
	rclcpp
	rclcpp_components
	rclcpp_lifecycle
	nav_msgs
	geometry_msgs
	behaviour_msgs
	visualization_msgs
	lanelet_msgs
	wato_trajectory_msgs
	tf2_ros
	tf2_geometry_msgs
	wato_lifecycle_manager

System Dependencies

No direct system dependencies.

Dependant Packages

Name	Deps
action_bringup

Launch files

No launch files found

Messages

No message files found.

Services

No service files found

Plugins

No plugins found.

Recent questions tagged `trajectory_planner` at Robotics Stack Exchange

No version for distro kilted showing github. Known supported distros are highlighted in the buttons above.

trajectory_planner package from wato_monorepo repo

ackermann_pure_pursuit action_bringup freeroam_planner global_planning lattice_planning lattice_planning_markers lattice_planning_msgs model_predictive_control mpc_controller osqp_eigen_vendor trajectory_planner wato_trajectory_msgs infrastructure_bringup tf_static_relay vision_msgs_markers wato_lifecycle_manager ackermann_mux camera_calib lidar_imu_calib lidar_lidar_calib can_state_estimator dummy_ackermann eve_description interfacing_bringup joystick_interfacing lidar_aggregator oscc_interfacing oscc_mux pid_control pid_msgs roscco_msg sensor_interfacing topic_healthchecker attribute_assigner depth_estimation feature_extraction patchworkpp patchworkpp_vendor perception_bringup perception_utils spatial_association bytetrack_cpp_vendor track_viz_2d tracking traffic_light_detection carla_bringup carla_common carla_control carla_lifecycle carla_localization carla_msgs carla_perception carla_pygame carla_ros_bridge carla_sample_description carla_scenarios carla_teleop simulation_bringup wato_test behaviortree_ros2_vendor behaviour behaviour_markers behaviour_msgs costmap costmap_markers costmap_msgs eidos eidos_msgs eidos_tools eidos_transform lanelet_markers lanelet_msgs lidar_localization prediction world_model world_model_markers world_model_msgs world_modeling_bringup

ROS Distro
github

Package Summary

Version	1.0.0
License	Apache-2.0
Build type	AMENT_CMAKE
Use	RECOMMENDED

Repository Summary

Description
Checkout URI	https://github.com/watonomous/wato_monorepo.git
VCS Type	git
VCS Version	main
Last Updated	2026-04-03
Dev Status	UNKNOWN
Released	UNRELEASED
Contributing	Help Wanted (-) Good First Issues (-) Pull Requests to Review (-)

Package Description

Longitudinal trajectory planner for velocity profiling based on costmaps

Maintainers

WATonomous

Authors

No additional authors.

WATO Trajectory Planner

The trajectory_planner package refines a geometric path into a velocity-profiled trajectory. It checks for obstacles along the path using a vehicle footprint and smoothly decelerates when a collision is imminent.

Overview

The node subscribes to a path (from local_planning) and a costmap (from world_modeling). It interpolates the path at a fixed resolution, sweeps the vehicle footprint at each point, and finds the distance to the first lethal obstacle. Velocity at each point is then limited by kinematics-based braking: v = sqrt(2 * max_tangential_accel * braking_distance), ensuring the vehicle can always stop in time given its current speed and max deceleration.

The lane speed limit from /world_modeling/lanelet/lane_context further caps velocity when available.

Usage

ros2 launch trajectory_planner trajectory_planner.launch.yaml

Topic remappings:

input_path → /action/local_planning/path
costmap → /world_modeling/costmap
trajectory → /action/trajectory_planning/trajectory
lane_context → /world_modeling/lanelet/lane_context

Visualization

Publishes visualization_msgs/MarkerArray on ~trajectory_markers. Each point is rendered as a purple sphere whose diameter scales with target speed (larger = faster).

Configuration

Parameters are defined in config/trajectory_planner_params.yaml.

Parameter	Default	Description
`stop_distance`	2.0 m	Distance to obstacle where vehicle must be fully stopped.
`max_speed`	20.0 m/s	Maximum speed when no lanelet limit is available.
`max_tangential_accel`	1.0 m/s^2	Comfort braking deceleration for normal obstacle avoidance.
`max_emergency_accel`	5.0 m/s^2	Emergency braking deceleration when comfort braking is insufficient.
`max_lateral_accel`	0.5 m/s^2	Maximum lateral acceleration to slow down in curves.
`interpolation_resolution`	0.1 m	Point spacing along path for collision checking.
`footprint_frame`	`base_link`	Frame in which the footprint is defined.
`footprint_x_min`	-0.5 m	Rear extent of vehicle.
`footprint_x_max`	3.5 m	Front extent of vehicle (front bumper).
`footprint_y_min`	-1.2 m	Right extent of vehicle.
`footprint_y_max`	1.2 m	Left extent of vehicle.

Tuning Guide

Car stops too early/late: Adjust stop_distance or the footprint_x_max to match actual front bumper position.
Car is too jerky: Decrease max_tangential_accel for gentler braking.
Car clips obstacles on the sides: Increase footprint_y_min/footprint_y_max to widen the safety corridor.
High CPU usage: Increase interpolation_resolution (e.g. 0.2 m), but avoid missing narrow obstacles.

Troubleshooting

No trajectory output: Verify input_path and costmap topics are publishing and remapped correctly.
“TrajectoryCore: Empty path”: The upstream local planner is not producing a path.
Collisions not detected: Confirm the costmap contains lethal cells (cost > 100) and that TF transforms between the path frame and costmap frame are available.

CHANGELOG

No CHANGELOG found.

Package Dependencies

Deps	Name
	ament_cmake
	ament_cmake_gtest
	wato_test
	rclcpp
	rclcpp_components
	rclcpp_lifecycle
	nav_msgs
	geometry_msgs
	behaviour_msgs
	visualization_msgs
	lanelet_msgs
	wato_trajectory_msgs
	tf2_ros
	tf2_geometry_msgs
	wato_lifecycle_manager

System Dependencies

No direct system dependencies.

Dependant Packages

Name	Deps
action_bringup

Launch files

No launch files found

Messages

No message files found.

Services

No service files found

Plugins

No plugins found.

Recent questions tagged `trajectory_planner` at Robotics Stack Exchange

No version for distro rolling showing github. Known supported distros are highlighted in the buttons above.

trajectory_planner package from wato_monorepo repo

ackermann_pure_pursuit action_bringup freeroam_planner global_planning lattice_planning lattice_planning_markers lattice_planning_msgs model_predictive_control mpc_controller osqp_eigen_vendor trajectory_planner wato_trajectory_msgs infrastructure_bringup tf_static_relay vision_msgs_markers wato_lifecycle_manager ackermann_mux camera_calib lidar_imu_calib lidar_lidar_calib can_state_estimator dummy_ackermann eve_description interfacing_bringup joystick_interfacing lidar_aggregator oscc_interfacing oscc_mux pid_control pid_msgs roscco_msg sensor_interfacing topic_healthchecker attribute_assigner depth_estimation feature_extraction patchworkpp patchworkpp_vendor perception_bringup perception_utils spatial_association bytetrack_cpp_vendor track_viz_2d tracking traffic_light_detection carla_bringup carla_common carla_control carla_lifecycle carla_localization carla_msgs carla_perception carla_pygame carla_ros_bridge carla_sample_description carla_scenarios carla_teleop simulation_bringup wato_test behaviortree_ros2_vendor behaviour behaviour_markers behaviour_msgs costmap costmap_markers costmap_msgs eidos eidos_msgs eidos_tools eidos_transform lanelet_markers lanelet_msgs lidar_localization prediction world_model world_model_markers world_model_msgs world_modeling_bringup

ROS Distro
github

Package Summary

Version	1.0.0
License	Apache-2.0
Build type	AMENT_CMAKE
Use	RECOMMENDED

Repository Summary

Description
Checkout URI	https://github.com/watonomous/wato_monorepo.git
VCS Type	git
VCS Version	main
Last Updated	2026-04-03
Dev Status	UNKNOWN
Released	UNRELEASED
Contributing	Help Wanted (-) Good First Issues (-) Pull Requests to Review (-)

Package Description

Longitudinal trajectory planner for velocity profiling based on costmaps

Maintainers

WATonomous

Authors

No additional authors.

WATO Trajectory Planner

The trajectory_planner package refines a geometric path into a velocity-profiled trajectory. It checks for obstacles along the path using a vehicle footprint and smoothly decelerates when a collision is imminent.

Overview

The node subscribes to a path (from local_planning) and a costmap (from world_modeling). It interpolates the path at a fixed resolution, sweeps the vehicle footprint at each point, and finds the distance to the first lethal obstacle. Velocity at each point is then limited by kinematics-based braking: v = sqrt(2 * max_tangential_accel * braking_distance), ensuring the vehicle can always stop in time given its current speed and max deceleration.

The lane speed limit from /world_modeling/lanelet/lane_context further caps velocity when available.

Usage

ros2 launch trajectory_planner trajectory_planner.launch.yaml

Topic remappings:

input_path → /action/local_planning/path
costmap → /world_modeling/costmap
trajectory → /action/trajectory_planning/trajectory
lane_context → /world_modeling/lanelet/lane_context

Visualization

Publishes visualization_msgs/MarkerArray on ~trajectory_markers. Each point is rendered as a purple sphere whose diameter scales with target speed (larger = faster).

Configuration

Parameters are defined in config/trajectory_planner_params.yaml.

Parameter	Default	Description
`stop_distance`	2.0 m	Distance to obstacle where vehicle must be fully stopped.
`max_speed`	20.0 m/s	Maximum speed when no lanelet limit is available.
`max_tangential_accel`	1.0 m/s^2	Comfort braking deceleration for normal obstacle avoidance.
`max_emergency_accel`	5.0 m/s^2	Emergency braking deceleration when comfort braking is insufficient.
`max_lateral_accel`	0.5 m/s^2	Maximum lateral acceleration to slow down in curves.
`interpolation_resolution`	0.1 m	Point spacing along path for collision checking.
`footprint_frame`	`base_link`	Frame in which the footprint is defined.
`footprint_x_min`	-0.5 m	Rear extent of vehicle.
`footprint_x_max`	3.5 m	Front extent of vehicle (front bumper).
`footprint_y_min`	-1.2 m	Right extent of vehicle.
`footprint_y_max`	1.2 m	Left extent of vehicle.

Tuning Guide

Car stops too early/late: Adjust stop_distance or the footprint_x_max to match actual front bumper position.
Car is too jerky: Decrease max_tangential_accel for gentler braking.
Car clips obstacles on the sides: Increase footprint_y_min/footprint_y_max to widen the safety corridor.
High CPU usage: Increase interpolation_resolution (e.g. 0.2 m), but avoid missing narrow obstacles.

Troubleshooting

No trajectory output: Verify input_path and costmap topics are publishing and remapped correctly.
“TrajectoryCore: Empty path”: The upstream local planner is not producing a path.
Collisions not detected: Confirm the costmap contains lethal cells (cost > 100) and that TF transforms between the path frame and costmap frame are available.

CHANGELOG

No CHANGELOG found.

Package Dependencies

Deps	Name
	ament_cmake
	ament_cmake_gtest
	wato_test
	rclcpp
	rclcpp_components
	rclcpp_lifecycle
	nav_msgs
	geometry_msgs
	behaviour_msgs
	visualization_msgs
	lanelet_msgs
	wato_trajectory_msgs
	tf2_ros
	tf2_geometry_msgs
	wato_lifecycle_manager

System Dependencies

No direct system dependencies.

Dependant Packages

Name	Deps
action_bringup

Launch files

No launch files found

Messages

No message files found.

Services

No service files found

Plugins

No plugins found.

Recent questions tagged `trajectory_planner` at Robotics Stack Exchange

trajectory_planner package from wato_monorepo repo

ackermann_pure_pursuit action_bringup freeroam_planner global_planning lattice_planning lattice_planning_markers lattice_planning_msgs model_predictive_control mpc_controller osqp_eigen_vendor trajectory_planner wato_trajectory_msgs infrastructure_bringup tf_static_relay vision_msgs_markers wato_lifecycle_manager ackermann_mux camera_calib lidar_imu_calib lidar_lidar_calib can_state_estimator dummy_ackermann eve_description interfacing_bringup joystick_interfacing lidar_aggregator oscc_interfacing oscc_mux pid_control pid_msgs roscco_msg sensor_interfacing topic_healthchecker attribute_assigner depth_estimation feature_extraction patchworkpp patchworkpp_vendor perception_bringup perception_utils spatial_association bytetrack_cpp_vendor track_viz_2d tracking traffic_light_detection carla_bringup carla_common carla_control carla_lifecycle carla_localization carla_msgs carla_perception carla_pygame carla_ros_bridge carla_sample_description carla_scenarios carla_teleop simulation_bringup wato_test behaviortree_ros2_vendor behaviour behaviour_markers behaviour_msgs costmap costmap_markers costmap_msgs eidos eidos_msgs eidos_tools eidos_transform lanelet_markers lanelet_msgs lidar_localization prediction world_model world_model_markers world_model_msgs world_modeling_bringup

ROS Distro
github

Package Summary

Version	1.0.0
License	Apache-2.0
Build type	AMENT_CMAKE
Use	RECOMMENDED

Repository Summary

Description
Checkout URI	https://github.com/watonomous/wato_monorepo.git
VCS Type	git
VCS Version	main
Last Updated	2026-04-03
Dev Status	UNKNOWN
Released	UNRELEASED
Contributing	Help Wanted (-) Good First Issues (-) Pull Requests to Review (-)

Package Description

Longitudinal trajectory planner for velocity profiling based on costmaps

Maintainers

WATonomous

Authors

No additional authors.

WATO Trajectory Planner

The trajectory_planner package refines a geometric path into a velocity-profiled trajectory. It checks for obstacles along the path using a vehicle footprint and smoothly decelerates when a collision is imminent.

Overview

The node subscribes to a path (from local_planning) and a costmap (from world_modeling). It interpolates the path at a fixed resolution, sweeps the vehicle footprint at each point, and finds the distance to the first lethal obstacle. Velocity at each point is then limited by kinematics-based braking: v = sqrt(2 * max_tangential_accel * braking_distance), ensuring the vehicle can always stop in time given its current speed and max deceleration.

The lane speed limit from /world_modeling/lanelet/lane_context further caps velocity when available.

Usage

ros2 launch trajectory_planner trajectory_planner.launch.yaml

Topic remappings:

input_path → /action/local_planning/path
costmap → /world_modeling/costmap
trajectory → /action/trajectory_planning/trajectory
lane_context → /world_modeling/lanelet/lane_context

Visualization

Publishes visualization_msgs/MarkerArray on ~trajectory_markers. Each point is rendered as a purple sphere whose diameter scales with target speed (larger = faster).

Configuration

Parameters are defined in config/trajectory_planner_params.yaml.

Parameter	Default	Description
`stop_distance`	2.0 m	Distance to obstacle where vehicle must be fully stopped.
`max_speed`	20.0 m/s	Maximum speed when no lanelet limit is available.
`max_tangential_accel`	1.0 m/s^2	Comfort braking deceleration for normal obstacle avoidance.
`max_emergency_accel`	5.0 m/s^2	Emergency braking deceleration when comfort braking is insufficient.
`max_lateral_accel`	0.5 m/s^2	Maximum lateral acceleration to slow down in curves.
`interpolation_resolution`	0.1 m	Point spacing along path for collision checking.
`footprint_frame`	`base_link`	Frame in which the footprint is defined.
`footprint_x_min`	-0.5 m	Rear extent of vehicle.
`footprint_x_max`	3.5 m	Front extent of vehicle (front bumper).
`footprint_y_min`	-1.2 m	Right extent of vehicle.
`footprint_y_max`	1.2 m	Left extent of vehicle.

Tuning Guide

Car stops too early/late: Adjust stop_distance or the footprint_x_max to match actual front bumper position.
Car is too jerky: Decrease max_tangential_accel for gentler braking.
Car clips obstacles on the sides: Increase footprint_y_min/footprint_y_max to widen the safety corridor.
High CPU usage: Increase interpolation_resolution (e.g. 0.2 m), but avoid missing narrow obstacles.

Troubleshooting

No trajectory output: Verify input_path and costmap topics are publishing and remapped correctly.
“TrajectoryCore: Empty path”: The upstream local planner is not producing a path.
Collisions not detected: Confirm the costmap contains lethal cells (cost > 100) and that TF transforms between the path frame and costmap frame are available.

CHANGELOG

No CHANGELOG found.

Package Dependencies

Deps	Name
	ament_cmake
	ament_cmake_gtest
	wato_test
	rclcpp
	rclcpp_components
	rclcpp_lifecycle
	nav_msgs
	geometry_msgs
	behaviour_msgs
	visualization_msgs
	lanelet_msgs
	wato_trajectory_msgs
	tf2_ros
	tf2_geometry_msgs
	wato_lifecycle_manager

System Dependencies

No direct system dependencies.

Dependant Packages

Name	Deps
action_bringup

Launch files

No launch files found

Messages

No message files found.

Services

No service files found

Plugins

No plugins found.

Recent questions tagged `trajectory_planner` at Robotics Stack Exchange

No version for distro galactic showing github. Known supported distros are highlighted in the buttons above.

trajectory_planner package from wato_monorepo repo

ackermann_pure_pursuit action_bringup freeroam_planner global_planning lattice_planning lattice_planning_markers lattice_planning_msgs model_predictive_control mpc_controller osqp_eigen_vendor trajectory_planner wato_trajectory_msgs infrastructure_bringup tf_static_relay vision_msgs_markers wato_lifecycle_manager ackermann_mux camera_calib lidar_imu_calib lidar_lidar_calib can_state_estimator dummy_ackermann eve_description interfacing_bringup joystick_interfacing lidar_aggregator oscc_interfacing oscc_mux pid_control pid_msgs roscco_msg sensor_interfacing topic_healthchecker attribute_assigner depth_estimation feature_extraction patchworkpp patchworkpp_vendor perception_bringup perception_utils spatial_association bytetrack_cpp_vendor track_viz_2d tracking traffic_light_detection carla_bringup carla_common carla_control carla_lifecycle carla_localization carla_msgs carla_perception carla_pygame carla_ros_bridge carla_sample_description carla_scenarios carla_teleop simulation_bringup wato_test behaviortree_ros2_vendor behaviour behaviour_markers behaviour_msgs costmap costmap_markers costmap_msgs eidos eidos_msgs eidos_tools eidos_transform lanelet_markers lanelet_msgs lidar_localization prediction world_model world_model_markers world_model_msgs world_modeling_bringup

ROS Distro
github

Package Summary

Version	1.0.0
License	Apache-2.0
Build type	AMENT_CMAKE
Use	RECOMMENDED

Repository Summary

Description
Checkout URI	https://github.com/watonomous/wato_monorepo.git
VCS Type	git
VCS Version	main
Last Updated	2026-04-03
Dev Status	UNKNOWN
Released	UNRELEASED
Contributing	Help Wanted (-) Good First Issues (-) Pull Requests to Review (-)

Package Description

Longitudinal trajectory planner for velocity profiling based on costmaps

Maintainers

WATonomous

Authors

No additional authors.

WATO Trajectory Planner

The trajectory_planner package refines a geometric path into a velocity-profiled trajectory. It checks for obstacles along the path using a vehicle footprint and smoothly decelerates when a collision is imminent.

Overview

The node subscribes to a path (from local_planning) and a costmap (from world_modeling). It interpolates the path at a fixed resolution, sweeps the vehicle footprint at each point, and finds the distance to the first lethal obstacle. Velocity at each point is then limited by kinematics-based braking: v = sqrt(2 * max_tangential_accel * braking_distance), ensuring the vehicle can always stop in time given its current speed and max deceleration.

The lane speed limit from /world_modeling/lanelet/lane_context further caps velocity when available.

Usage

ros2 launch trajectory_planner trajectory_planner.launch.yaml

Topic remappings:

input_path → /action/local_planning/path
costmap → /world_modeling/costmap
trajectory → /action/trajectory_planning/trajectory
lane_context → /world_modeling/lanelet/lane_context

Visualization

Publishes visualization_msgs/MarkerArray on ~trajectory_markers. Each point is rendered as a purple sphere whose diameter scales with target speed (larger = faster).

Configuration

Parameters are defined in config/trajectory_planner_params.yaml.

Parameter	Default	Description
`stop_distance`	2.0 m	Distance to obstacle where vehicle must be fully stopped.
`max_speed`	20.0 m/s	Maximum speed when no lanelet limit is available.
`max_tangential_accel`	1.0 m/s^2	Comfort braking deceleration for normal obstacle avoidance.
`max_emergency_accel`	5.0 m/s^2	Emergency braking deceleration when comfort braking is insufficient.
`max_lateral_accel`	0.5 m/s^2	Maximum lateral acceleration to slow down in curves.
`interpolation_resolution`	0.1 m	Point spacing along path for collision checking.
`footprint_frame`	`base_link`	Frame in which the footprint is defined.
`footprint_x_min`	-0.5 m	Rear extent of vehicle.
`footprint_x_max`	3.5 m	Front extent of vehicle (front bumper).
`footprint_y_min`	-1.2 m	Right extent of vehicle.
`footprint_y_max`	1.2 m	Left extent of vehicle.

Tuning Guide

Car stops too early/late: Adjust stop_distance or the footprint_x_max to match actual front bumper position.
Car is too jerky: Decrease max_tangential_accel for gentler braking.
Car clips obstacles on the sides: Increase footprint_y_min/footprint_y_max to widen the safety corridor.
High CPU usage: Increase interpolation_resolution (e.g. 0.2 m), but avoid missing narrow obstacles.

Troubleshooting

No trajectory output: Verify input_path and costmap topics are publishing and remapped correctly.
“TrajectoryCore: Empty path”: The upstream local planner is not producing a path.
Collisions not detected: Confirm the costmap contains lethal cells (cost > 100) and that TF transforms between the path frame and costmap frame are available.

CHANGELOG

No CHANGELOG found.

Package Dependencies

Deps	Name
	ament_cmake
	ament_cmake_gtest
	wato_test
	rclcpp
	rclcpp_components
	rclcpp_lifecycle
	nav_msgs
	geometry_msgs
	behaviour_msgs
	visualization_msgs
	lanelet_msgs
	wato_trajectory_msgs
	tf2_ros
	tf2_geometry_msgs
	wato_lifecycle_manager

System Dependencies

No direct system dependencies.

Dependant Packages

Name	Deps
action_bringup

Launch files

No launch files found

Messages

No message files found.

Services

No service files found

Plugins

No plugins found.

Recent questions tagged `trajectory_planner` at Robotics Stack Exchange

No version for distro iron showing github. Known supported distros are highlighted in the buttons above.

trajectory_planner package from wato_monorepo repo

ackermann_pure_pursuit action_bringup freeroam_planner global_planning lattice_planning lattice_planning_markers lattice_planning_msgs model_predictive_control mpc_controller osqp_eigen_vendor trajectory_planner wato_trajectory_msgs infrastructure_bringup tf_static_relay vision_msgs_markers wato_lifecycle_manager ackermann_mux camera_calib lidar_imu_calib lidar_lidar_calib can_state_estimator dummy_ackermann eve_description interfacing_bringup joystick_interfacing lidar_aggregator oscc_interfacing oscc_mux pid_control pid_msgs roscco_msg sensor_interfacing topic_healthchecker attribute_assigner depth_estimation feature_extraction patchworkpp patchworkpp_vendor perception_bringup perception_utils spatial_association bytetrack_cpp_vendor track_viz_2d tracking traffic_light_detection carla_bringup carla_common carla_control carla_lifecycle carla_localization carla_msgs carla_perception carla_pygame carla_ros_bridge carla_sample_description carla_scenarios carla_teleop simulation_bringup wato_test behaviortree_ros2_vendor behaviour behaviour_markers behaviour_msgs costmap costmap_markers costmap_msgs eidos eidos_msgs eidos_tools eidos_transform lanelet_markers lanelet_msgs lidar_localization prediction world_model world_model_markers world_model_msgs world_modeling_bringup

ROS Distro
github

Package Summary

Version	1.0.0
License	Apache-2.0
Build type	AMENT_CMAKE
Use	RECOMMENDED

Repository Summary

Description
Checkout URI	https://github.com/watonomous/wato_monorepo.git
VCS Type	git
VCS Version	main
Last Updated	2026-04-03
Dev Status	UNKNOWN
Released	UNRELEASED
Contributing	Help Wanted (-) Good First Issues (-) Pull Requests to Review (-)

Package Description

Longitudinal trajectory planner for velocity profiling based on costmaps

Maintainers

WATonomous

Authors

No additional authors.

WATO Trajectory Planner

The trajectory_planner package refines a geometric path into a velocity-profiled trajectory. It checks for obstacles along the path using a vehicle footprint and smoothly decelerates when a collision is imminent.

Overview

The node subscribes to a path (from local_planning) and a costmap (from world_modeling). It interpolates the path at a fixed resolution, sweeps the vehicle footprint at each point, and finds the distance to the first lethal obstacle. Velocity at each point is then limited by kinematics-based braking: v = sqrt(2 * max_tangential_accel * braking_distance), ensuring the vehicle can always stop in time given its current speed and max deceleration.

The lane speed limit from /world_modeling/lanelet/lane_context further caps velocity when available.

Usage

ros2 launch trajectory_planner trajectory_planner.launch.yaml

Topic remappings:

input_path → /action/local_planning/path
costmap → /world_modeling/costmap
trajectory → /action/trajectory_planning/trajectory
lane_context → /world_modeling/lanelet/lane_context

Visualization

Publishes visualization_msgs/MarkerArray on ~trajectory_markers. Each point is rendered as a purple sphere whose diameter scales with target speed (larger = faster).

Configuration

Parameters are defined in config/trajectory_planner_params.yaml.

Parameter	Default	Description
`stop_distance`	2.0 m	Distance to obstacle where vehicle must be fully stopped.
`max_speed`	20.0 m/s	Maximum speed when no lanelet limit is available.
`max_tangential_accel`	1.0 m/s^2	Comfort braking deceleration for normal obstacle avoidance.
`max_emergency_accel`	5.0 m/s^2	Emergency braking deceleration when comfort braking is insufficient.
`max_lateral_accel`	0.5 m/s^2	Maximum lateral acceleration to slow down in curves.
`interpolation_resolution`	0.1 m	Point spacing along path for collision checking.
`footprint_frame`	`base_link`	Frame in which the footprint is defined.
`footprint_x_min`	-0.5 m	Rear extent of vehicle.
`footprint_x_max`	3.5 m	Front extent of vehicle (front bumper).
`footprint_y_min`	-1.2 m	Right extent of vehicle.
`footprint_y_max`	1.2 m	Left extent of vehicle.

Tuning Guide

Car stops too early/late: Adjust stop_distance or the footprint_x_max to match actual front bumper position.
Car is too jerky: Decrease max_tangential_accel for gentler braking.
Car clips obstacles on the sides: Increase footprint_y_min/footprint_y_max to widen the safety corridor.
High CPU usage: Increase interpolation_resolution (e.g. 0.2 m), but avoid missing narrow obstacles.

Troubleshooting

No trajectory output: Verify input_path and costmap topics are publishing and remapped correctly.
“TrajectoryCore: Empty path”: The upstream local planner is not producing a path.
Collisions not detected: Confirm the costmap contains lethal cells (cost > 100) and that TF transforms between the path frame and costmap frame are available.

CHANGELOG

No CHANGELOG found.

Package Dependencies

Deps	Name
	ament_cmake
	ament_cmake_gtest
	wato_test
	rclcpp
	rclcpp_components
	rclcpp_lifecycle
	nav_msgs
	geometry_msgs
	behaviour_msgs
	visualization_msgs
	lanelet_msgs
	wato_trajectory_msgs
	tf2_ros
	tf2_geometry_msgs
	wato_lifecycle_manager

System Dependencies

No direct system dependencies.

Dependant Packages

Name	Deps
action_bringup

Launch files

No launch files found

Messages

No message files found.

Services

No service files found

Plugins

No plugins found.

Recent questions tagged `trajectory_planner` at Robotics Stack Exchange

No version for distro melodic showing github. Known supported distros are highlighted in the buttons above.

trajectory_planner package from wato_monorepo repo

ackermann_pure_pursuit action_bringup freeroam_planner global_planning lattice_planning lattice_planning_markers lattice_planning_msgs model_predictive_control mpc_controller osqp_eigen_vendor trajectory_planner wato_trajectory_msgs infrastructure_bringup tf_static_relay vision_msgs_markers wato_lifecycle_manager ackermann_mux camera_calib lidar_imu_calib lidar_lidar_calib can_state_estimator dummy_ackermann eve_description interfacing_bringup joystick_interfacing lidar_aggregator oscc_interfacing oscc_mux pid_control pid_msgs roscco_msg sensor_interfacing topic_healthchecker attribute_assigner depth_estimation feature_extraction patchworkpp patchworkpp_vendor perception_bringup perception_utils spatial_association bytetrack_cpp_vendor track_viz_2d tracking traffic_light_detection carla_bringup carla_common carla_control carla_lifecycle carla_localization carla_msgs carla_perception carla_pygame carla_ros_bridge carla_sample_description carla_scenarios carla_teleop simulation_bringup wato_test behaviortree_ros2_vendor behaviour behaviour_markers behaviour_msgs costmap costmap_markers costmap_msgs eidos eidos_msgs eidos_tools eidos_transform lanelet_markers lanelet_msgs lidar_localization prediction world_model world_model_markers world_model_msgs world_modeling_bringup

ROS Distro
github

API Docs
Browse Code
Wiki

Package Summary

Version	1.0.0
License	Apache-2.0
Build type	AMENT_CMAKE
Use	RECOMMENDED

Repository Summary

Description
Checkout URI	https://github.com/watonomous/wato_monorepo.git
VCS Type	git
VCS Version	main
Last Updated	2026-04-03
Dev Status	UNKNOWN
Released	UNRELEASED
Contributing	Help Wanted (-) Good First Issues (-) Pull Requests to Review (-)

Package Description

Longitudinal trajectory planner for velocity profiling based on costmaps

Maintainers

WATonomous

Authors

No additional authors.

WATO Trajectory Planner

The trajectory_planner package refines a geometric path into a velocity-profiled trajectory. It checks for obstacles along the path using a vehicle footprint and smoothly decelerates when a collision is imminent.

Overview

The node subscribes to a path (from local_planning) and a costmap (from world_modeling). It interpolates the path at a fixed resolution, sweeps the vehicle footprint at each point, and finds the distance to the first lethal obstacle. Velocity at each point is then limited by kinematics-based braking: v = sqrt(2 * max_tangential_accel * braking_distance), ensuring the vehicle can always stop in time given its current speed and max deceleration.

The lane speed limit from /world_modeling/lanelet/lane_context further caps velocity when available.

Usage

ros2 launch trajectory_planner trajectory_planner.launch.yaml

Topic remappings:

input_path → /action/local_planning/path
costmap → /world_modeling/costmap
trajectory → /action/trajectory_planning/trajectory
lane_context → /world_modeling/lanelet/lane_context

Visualization

Publishes visualization_msgs/MarkerArray on ~trajectory_markers. Each point is rendered as a purple sphere whose diameter scales with target speed (larger = faster).

Configuration

Parameters are defined in config/trajectory_planner_params.yaml.

Parameter	Default	Description
`stop_distance`	2.0 m	Distance to obstacle where vehicle must be fully stopped.
`max_speed`	20.0 m/s	Maximum speed when no lanelet limit is available.
`max_tangential_accel`	1.0 m/s^2	Comfort braking deceleration for normal obstacle avoidance.
`max_emergency_accel`	5.0 m/s^2	Emergency braking deceleration when comfort braking is insufficient.
`max_lateral_accel`	0.5 m/s^2	Maximum lateral acceleration to slow down in curves.
`interpolation_resolution`	0.1 m	Point spacing along path for collision checking.
`footprint_frame`	`base_link`	Frame in which the footprint is defined.
`footprint_x_min`	-0.5 m	Rear extent of vehicle.
`footprint_x_max`	3.5 m	Front extent of vehicle (front bumper).
`footprint_y_min`	-1.2 m	Right extent of vehicle.
`footprint_y_max`	1.2 m	Left extent of vehicle.

Tuning Guide

Car stops too early/late: Adjust stop_distance or the footprint_x_max to match actual front bumper position.
Car is too jerky: Decrease max_tangential_accel for gentler braking.
Car clips obstacles on the sides: Increase footprint_y_min/footprint_y_max to widen the safety corridor.
High CPU usage: Increase interpolation_resolution (e.g. 0.2 m), but avoid missing narrow obstacles.

Troubleshooting

No trajectory output: Verify input_path and costmap topics are publishing and remapped correctly.
“TrajectoryCore: Empty path”: The upstream local planner is not producing a path.
Collisions not detected: Confirm the costmap contains lethal cells (cost > 100) and that TF transforms between the path frame and costmap frame are available.

CHANGELOG

No CHANGELOG found.

Package Dependencies

Deps	Name
	ament_cmake
	ament_cmake_gtest
	wato_test
	rclcpp
	rclcpp_components
	rclcpp_lifecycle
	nav_msgs
	geometry_msgs
	behaviour_msgs
	visualization_msgs
	lanelet_msgs
	wato_trajectory_msgs
	tf2_ros
	tf2_geometry_msgs
	wato_lifecycle_manager

System Dependencies

No direct system dependencies.

Dependant Packages

Name	Deps
action_bringup

Launch files

No launch files found

Messages

No message files found.

Services

No service files found

Plugins

No plugins found.

Recent questions tagged `trajectory_planner` at Robotics Stack Exchange

API Docs Browse Code Wiki

No version for distro noetic showing github. Known supported distros are highlighted in the buttons above.

trajectory_planner package from wato_monorepo repo

ackermann_pure_pursuit action_bringup freeroam_planner global_planning lattice_planning lattice_planning_markers lattice_planning_msgs model_predictive_control mpc_controller osqp_eigen_vendor trajectory_planner wato_trajectory_msgs infrastructure_bringup tf_static_relay vision_msgs_markers wato_lifecycle_manager ackermann_mux camera_calib lidar_imu_calib lidar_lidar_calib can_state_estimator dummy_ackermann eve_description interfacing_bringup joystick_interfacing lidar_aggregator oscc_interfacing oscc_mux pid_control pid_msgs roscco_msg sensor_interfacing topic_healthchecker attribute_assigner depth_estimation feature_extraction patchworkpp patchworkpp_vendor perception_bringup perception_utils spatial_association bytetrack_cpp_vendor track_viz_2d tracking traffic_light_detection carla_bringup carla_common carla_control carla_lifecycle carla_localization carla_msgs carla_perception carla_pygame carla_ros_bridge carla_sample_description carla_scenarios carla_teleop simulation_bringup wato_test behaviortree_ros2_vendor behaviour behaviour_markers behaviour_msgs costmap costmap_markers costmap_msgs eidos eidos_msgs eidos_tools eidos_transform lanelet_markers lanelet_msgs lidar_localization prediction world_model world_model_markers world_model_msgs world_modeling_bringup

ROS Distro
github

API Docs
Browse Code
Wiki

Package Summary

Version	1.0.0
License	Apache-2.0
Build type	AMENT_CMAKE
Use	RECOMMENDED

Repository Summary

Description
Checkout URI	https://github.com/watonomous/wato_monorepo.git
VCS Type	git
VCS Version	main
Last Updated	2026-04-03
Dev Status	UNKNOWN
Released	UNRELEASED
Contributing	Help Wanted (-) Good First Issues (-) Pull Requests to Review (-)

Package Description

Longitudinal trajectory planner for velocity profiling based on costmaps

Maintainers

WATonomous

Authors

No additional authors.