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-03-16
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 scaled linearly between max_speed (at safe_distance) and 0 (at stop_distance).

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
`safe_distance`	10.0 m	Distance to obstacle where deceleration begins.
`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.
`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: Increase safe_distance to lengthen the deceleration ramp.
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
	visualization_msgs
	lanelet_msgs
	wato_trajectory_msgs
	tf2_ros
	tf2_geometry_msgs
	wato_lifecycle_manager

System Dependencies

No direct system dependencies.

Dependant Packages

No known dependants.

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 behaviour_planning freeroam_planner global_planning lattice_planning lattice_planning_markers lattice_planning_msgs model_predictive_control 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 oscc_interfacing pid_control pid_msgs roscco_msg sensor_interfacing topic_healthchecker camera_object_detection depth_estimation feature_extraction patchworkpp patchworkpp_vendor perception_bringup perception_utils 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 lanelet_markers lanelet_msgs lidar_localization 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-03-16
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 scaled linearly between max_speed (at safe_distance) and 0 (at stop_distance).

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
`safe_distance`	10.0 m	Distance to obstacle where deceleration begins.
`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.
`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: Increase safe_distance to lengthen the deceleration ramp.
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
	visualization_msgs
	lanelet_msgs
	wato_trajectory_msgs
	tf2_ros
	tf2_geometry_msgs
	wato_lifecycle_manager

System Dependencies

No direct system dependencies.

Dependant Packages

No known dependants.

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 behaviour_planning freeroam_planner global_planning lattice_planning lattice_planning_markers lattice_planning_msgs model_predictive_control 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 oscc_interfacing pid_control pid_msgs roscco_msg sensor_interfacing topic_healthchecker camera_object_detection depth_estimation feature_extraction patchworkpp patchworkpp_vendor perception_bringup perception_utils 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 lanelet_markers lanelet_msgs lidar_localization 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-03-16
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 scaled linearly between max_speed (at safe_distance) and 0 (at stop_distance).

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
`safe_distance`	10.0 m	Distance to obstacle where deceleration begins.
`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.
`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: Increase safe_distance to lengthen the deceleration ramp.
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
	visualization_msgs
	lanelet_msgs
	wato_trajectory_msgs
	tf2_ros
	tf2_geometry_msgs
	wato_lifecycle_manager

System Dependencies

No direct system dependencies.

Dependant Packages

No known dependants.

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 behaviour_planning freeroam_planner global_planning lattice_planning lattice_planning_markers lattice_planning_msgs model_predictive_control 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 oscc_interfacing pid_control pid_msgs roscco_msg sensor_interfacing topic_healthchecker camera_object_detection depth_estimation feature_extraction patchworkpp patchworkpp_vendor perception_bringup perception_utils 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 lanelet_markers lanelet_msgs lidar_localization 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-03-16
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 scaled linearly between max_speed (at safe_distance) and 0 (at stop_distance).

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
`safe_distance`	10.0 m	Distance to obstacle where deceleration begins.
`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.
`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: Increase safe_distance to lengthen the deceleration ramp.
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
	visualization_msgs
	lanelet_msgs
	wato_trajectory_msgs
	tf2_ros
	tf2_geometry_msgs
	wato_lifecycle_manager

System Dependencies

No direct system dependencies.

Dependant Packages

No known dependants.

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 behaviour_planning freeroam_planner global_planning lattice_planning lattice_planning_markers lattice_planning_msgs model_predictive_control 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 oscc_interfacing pid_control pid_msgs roscco_msg sensor_interfacing topic_healthchecker camera_object_detection depth_estimation feature_extraction patchworkpp patchworkpp_vendor perception_bringup perception_utils 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 lanelet_markers lanelet_msgs lidar_localization 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-03-16
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 scaled linearly between max_speed (at safe_distance) and 0 (at stop_distance).

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
`safe_distance`	10.0 m	Distance to obstacle where deceleration begins.
`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.
`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: Increase safe_distance to lengthen the deceleration ramp.
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
	visualization_msgs
	lanelet_msgs
	wato_trajectory_msgs
	tf2_ros
	tf2_geometry_msgs
	wato_lifecycle_manager

System Dependencies

No direct system dependencies.

Dependant Packages

No known dependants.

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 behaviour_planning freeroam_planner global_planning lattice_planning lattice_planning_markers lattice_planning_msgs model_predictive_control 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 oscc_interfacing pid_control pid_msgs roscco_msg sensor_interfacing topic_healthchecker camera_object_detection depth_estimation feature_extraction patchworkpp patchworkpp_vendor perception_bringup perception_utils 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 lanelet_markers lanelet_msgs lidar_localization 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-03-16
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 scaled linearly between max_speed (at safe_distance) and 0 (at stop_distance).

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
`safe_distance`	10.0 m	Distance to obstacle where deceleration begins.
`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.
`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: Increase safe_distance to lengthen the deceleration ramp.
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
	visualization_msgs
	lanelet_msgs
	wato_trajectory_msgs
	tf2_ros
	tf2_geometry_msgs
	wato_lifecycle_manager

System Dependencies

No direct system dependencies.

Dependant Packages

No known dependants.

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 behaviour_planning freeroam_planner global_planning lattice_planning lattice_planning_markers lattice_planning_msgs model_predictive_control 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 oscc_interfacing pid_control pid_msgs roscco_msg sensor_interfacing topic_healthchecker camera_object_detection depth_estimation feature_extraction patchworkpp patchworkpp_vendor perception_bringup perception_utils 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 lanelet_markers lanelet_msgs lidar_localization 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-03-16
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 scaled linearly between max_speed (at safe_distance) and 0 (at stop_distance).

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
`safe_distance`	10.0 m	Distance to obstacle where deceleration begins.
`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.
`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: Increase safe_distance to lengthen the deceleration ramp.
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
	visualization_msgs
	lanelet_msgs
	wato_trajectory_msgs
	tf2_ros
	tf2_geometry_msgs
	wato_lifecycle_manager

System Dependencies

No direct system dependencies.

Dependant Packages

No known dependants.

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 behaviour_planning freeroam_planner global_planning lattice_planning lattice_planning_markers lattice_planning_msgs model_predictive_control 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 oscc_interfacing pid_control pid_msgs roscco_msg sensor_interfacing topic_healthchecker camera_object_detection depth_estimation feature_extraction patchworkpp patchworkpp_vendor perception_bringup perception_utils 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 lanelet_markers lanelet_msgs lidar_localization 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-03-16
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 scaled linearly between max_speed (at safe_distance) and 0 (at stop_distance).

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
`safe_distance`	10.0 m	Distance to obstacle where deceleration begins.
`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.
`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: Increase safe_distance to lengthen the deceleration ramp.
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
	visualization_msgs
	lanelet_msgs
	wato_trajectory_msgs
	tf2_ros
	tf2_geometry_msgs
	wato_lifecycle_manager

System Dependencies

No direct system dependencies.

Dependant Packages

No known dependants.

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 behaviour_planning freeroam_planner global_planning lattice_planning lattice_planning_markers lattice_planning_msgs model_predictive_control 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 oscc_interfacing pid_control pid_msgs roscco_msg sensor_interfacing topic_healthchecker camera_object_detection depth_estimation feature_extraction patchworkpp patchworkpp_vendor perception_bringup perception_utils 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 lanelet_markers lanelet_msgs lidar_localization 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-03-16
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.