No version for distro humble. Known supported distros are highlighted in the buttons above.
No version for distro jazzy. Known supported distros are highlighted in the buttons above.
No version for distro rolling. Known supported distros are highlighted in the buttons above.

autoware_obstacle_cruise_planner package from autoware_universe repo

autoware_adapi_specs autoware_agnocast_wrapper autoware_auto_common autoware_component_interface_specs_universe autoware_component_interface_tools autoware_component_interface_utils autoware_cuda_dependency_meta autoware_fake_test_node autoware_glog_component autoware_goal_distance_calculator autoware_grid_map_utils autoware_path_distance_calculator autoware_polar_grid autoware_time_utils autoware_traffic_light_recognition_marker_publisher autoware_traffic_light_utils autoware_universe_utils tier4_api_utils autoware_autonomous_emergency_braking autoware_collision_detector autoware_control_performance_analysis autoware_control_validator autoware_external_cmd_selector autoware_joy_controller autoware_lane_departure_checker autoware_mpc_lateral_controller autoware_obstacle_collision_checker autoware_operation_mode_transition_manager autoware_pid_longitudinal_controller autoware_predicted_path_checker autoware_pure_pursuit autoware_shift_decider autoware_smart_mpc_trajectory_follower autoware_trajectory_follower_base autoware_trajectory_follower_node autoware_vehicle_cmd_gate autoware_control_evaluator autoware_kinematic_evaluator autoware_localization_evaluator autoware_perception_online_evaluator autoware_planning_evaluator autoware_scenario_simulator_v2_adapter tier4_autoware_api_launch tier4_control_launch tier4_localization_launch tier4_map_launch tier4_perception_launch tier4_planning_launch tier4_sensing_launch tier4_simulator_launch tier4_system_launch tier4_vehicle_launch autoware_geo_pose_projector autoware_gyro_odometer autoware_ar_tag_based_localizer autoware_landmark_manager autoware_lidar_marker_localizer autoware_localization_error_monitor autoware_ndt_scan_matcher autoware_pose2twist autoware_pose_covariance_modifier autoware_pose_estimator_arbiter autoware_pose_initializer autoware_pose_instability_detector yabloc_common yabloc_image_processing yabloc_monitor yabloc_particle_filter yabloc_pose_initializer autoware_lanelet2_map_visualizer autoware_map_height_fitter autoware_map_tf_generator autoware_bytetrack autoware_cluster_merger autoware_compare_map_segmentation autoware_crosswalk_traffic_light_estimator autoware_detected_object_feature_remover autoware_detected_object_validation autoware_detection_by_tracker autoware_elevation_map_loader autoware_euclidean_cluster autoware_ground_segmentation autoware_image_projection_based_fusion autoware_lidar_apollo_instance_segmentation autoware_lidar_centerpoint autoware_lidar_transfusion autoware_map_based_prediction autoware_multi_object_tracker autoware_object_merger autoware_object_range_splitter autoware_object_velocity_splitter autoware_occupancy_grid_map_outlier_filter autoware_probabilistic_occupancy_grid_map autoware_radar_crossing_objects_noise_filter autoware_radar_fusion_to_detected_object autoware_radar_object_clustering autoware_radar_object_tracker autoware_radar_tracks_msgs_converter autoware_raindrop_cluster_filter autoware_shape_estimation autoware_simple_object_merger autoware_tensorrt_classifier autoware_tensorrt_common autoware_tensorrt_yolox autoware_tracking_object_merger autoware_traffic_light_arbiter autoware_traffic_light_category_merger autoware_traffic_light_classifier autoware_traffic_light_fine_detector autoware_traffic_light_map_based_detector autoware_traffic_light_multi_camera_fusion autoware_traffic_light_occlusion_predictor autoware_traffic_light_selector autoware_traffic_light_visualization perception_utils autoware_costmap_generator autoware_external_velocity_limit_selector autoware_freespace_planner autoware_freespace_planning_algorithms autoware_mission_planner_universe autoware_obstacle_cruise_planner autoware_obstacle_stop_planner autoware_path_optimizer autoware_path_smoother autoware_planning_validator autoware_remaining_distance_time_calculator autoware_rtc_interface autoware_scenario_selector autoware_surround_obstacle_checker autoware_behavior_path_avoidance_by_lane_change_module autoware_behavior_path_dynamic_obstacle_avoidance_module autoware_behavior_path_external_request_lane_change_module autoware_behavior_path_goal_planner_module autoware_behavior_path_lane_change_module autoware_behavior_path_planner autoware_behavior_path_planner_common autoware_behavior_path_sampling_planner_module autoware_behavior_path_side_shift_module autoware_behavior_path_start_planner_module autoware_behavior_path_static_obstacle_avoidance_module autoware_behavior_velocity_blind_spot_module autoware_behavior_velocity_crosswalk_module autoware_behavior_velocity_detection_area_module autoware_behavior_velocity_intersection_module autoware_behavior_velocity_no_drivable_lane_module autoware_behavior_velocity_no_stopping_area_module autoware_behavior_velocity_occlusion_spot_module autoware_behavior_velocity_rtc_interface autoware_behavior_velocity_run_out_module autoware_behavior_velocity_speed_bump_module autoware_behavior_velocity_template_module autoware_behavior_velocity_traffic_light_module autoware_behavior_velocity_virtual_traffic_light_module autoware_behavior_velocity_walkway_module autoware_motion_velocity_dynamic_obstacle_stop_module autoware_motion_velocity_obstacle_cruise_module autoware_motion_velocity_obstacle_slow_down_module autoware_motion_velocity_obstacle_velocity_limiter_module autoware_motion_velocity_out_of_lane_module autoware_bezier_sampler autoware_frenet_planner autoware_path_sampler autoware_sampler_common autoware_cuda_pointcloud_preprocessor autoware_cuda_utils autoware_image_diagnostics autoware_image_transport_decompressor autoware_imu_corrector autoware_pcl_extensions autoware_pointcloud_preprocessor autoware_radar_scan_to_pointcloud2 autoware_radar_static_pointcloud_filter autoware_radar_threshold_filter autoware_radar_tracks_noise_filter autoware_livox_tag_filter autoware_carla_interface autoware_dummy_perception_publisher autoware_fault_injection autoware_learning_based_vehicle_model autoware_simple_planning_simulator autoware_vehicle_door_simulator tier4_dummy_object_rviz_plugin autoware_bluetooth_monitor autoware_component_monitor autoware_component_state_monitor autoware_default_adapi autoware_adapi_adaptors autoware_adapi_visualizers autoware_automatic_pose_initializer autoware_diagnostic_graph_aggregator autoware_diagnostic_graph_utils autoware_dummy_diag_publisher autoware_dummy_infrastructure autoware_duplicated_node_checker autoware_hazard_status_converter autoware_mrm_comfortable_stop_operator autoware_mrm_emergency_stop_operator autoware_mrm_handler autoware_processing_time_checker autoware_system_diagnostic_monitor autoware_system_monitor autoware_topic_relay_controller autoware_topic_state_monitor autoware_velodyne_monitor reaction_analyzer autoware_accel_brake_map_calibrator autoware_external_cmd_converter autoware_raw_vehicle_cmd_converter autoware_steer_offset_estimator autoware_bag_time_manager_rviz_plugin autoware_mission_details_overlay_rviz_plugin autoware_overlay_rviz_plugin autoware_string_stamped_rviz_plugin autoware_perception_rviz_plugin tier4_adapi_rviz_plugin tier4_camera_view_rviz_plugin tier4_datetime_rviz_plugin tier4_localization_rviz_plugin tier4_planning_factor_rviz_plugin tier4_planning_rviz_plugin tier4_state_rviz_plugin tier4_system_rviz_plugin tier4_traffic_light_rviz_plugin tier4_vehicle_rviz_plugin

Package Summary

Tags No category tags.
Version 0.43.0
License Apache License 2.0
Build type AMENT_CMAKE
Use RECOMMENDED

Repository Summary

Checkout URI https://github.com/autowarefoundation/autoware_universe.git
VCS Type git
VCS Version main
Last Updated 2025-04-04
Dev Status UNMAINTAINED
CI status No Continuous Integration
Released UNRELEASED
Tags No category tags.
Contributing Help Wanted (0)
Good First Issues (0)
Pull Requests to Review (0)

Package Description

The autoware_obstacle_cruise_planner package

Additional Links

No additional links.

Maintainers

  • Takayuki Murooka
  • Kosuke Takeuchi
  • Satoshi Ota
  • Yuki Takagi
  • Berkay Karaman

Authors

  • Takayuki Murooka
  • Yutaka Shimizu

Obstacle Cruise Planner

Overview

The autoware_obstacle_cruise_planner package has following modules.

  • Stop planning
    • stop when there is a static obstacle near the trajectory.
  • Cruise planning
    • cruise a dynamic obstacle in front of the ego.
  • Slow down planning
    • slow down when there is a static/dynamic obstacle near the trajectory.

Interfaces

Input topics

Name Type Description
~/input/trajectory autoware_planning_msgs::Trajectory input trajectory
~/input/objects autoware_perception_msgs::PredictedObjects dynamic objects
~/input/odometry nav_msgs::msg::Odometry ego odometry

Output topics

Name Type Description
~/output/trajectory autoware_planning_msgs::Trajectory output trajectory
~/output/velocity_limit autoware_internal_planning_msgs::msg::VelocityLimit velocity limit for cruising
~/output/clear_velocity_limit autoware_internal_planning_msgs::msg::VelocityLimitClearCommand clear command for velocity limit

Design

Design for the following functions is defined here.

  • Behavior determination against obstacles
  • Stop planning
  • Cruise planning
  • Slow down planning

A data structure for cruise and stop planning is as follows. This planner data is created first, and then sent to the planning algorithm.

struct PlannerData
{
  rclcpp::Time current_time;
  autoware_planning_msgs::msg::Trajectory traj;
  geometry_msgs::msg::Pose current_pose;
  double ego_vel;
  double current_acc;
  std::vector<Obstacle> target_obstacles;
};

struct Obstacle
{
  rclcpp::Time stamp;  // This is not the current stamp, but when the object was observed.
  geometry_msgs::msg::Pose pose;  // interpolated with the current stamp
  bool orientation_reliable;
  Twist twist;
  bool twist_reliable;
  ObjectClassification classification;
  std::string uuid;
  Shape shape;
  std::vector<PredictedPath> predicted_paths;
};

Behavior determination against obstacles

Obstacles for cruising, stopping and slowing down are selected in this order based on their pose and velocity. The obstacles not in front of the ego will be ignored.

determine_cruise_stop_slow_down

The behavior determination flowchart is shown below.

behavior_determination_flowchart

Determine cruise vehicles

The obstacles meeting the following condition are determined as obstacles for cruising.

  • The lateral distance from the object to the ego’s trajectory is smaller than behavior_determination.cruise.max_lat_margin.

  • The object type is for cruising according to common.cruise_obstacle_type.*.
  • The object is not crossing the ego’s trajectory (*1).
  • If the object is inside the trajectory.
    • The object type is for inside cruising according to common.cruise_obstacle_type.inside.*.
    • The object velocity is larger than behavior_determination.obstacle_velocity_threshold_from_cruise_to_stop.
  • If the object is outside the trajectory.
    • The object type is for outside cruising according to common.cruise_obstacle_type.outside.*.
    • The object velocity is larger than behavior_determination.cruise.outside_obstacle.obstacle_velocity_threshold.
    • The highest confident predicted path collides with the ego’s trajectory.
    • Its collision’s period is larger than behavior_determination.cruise.outside_obstacle.ego_obstacle_overlap_time_threshold.
Parameter Type Description
common.cruise_obstacle_type.inside.unknown bool flag to consider unknown objects for cruising
common.cruise_obstacle_type.inside.car bool flag to consider unknown objects for cruising
common.cruise_obstacle_type.inside.truck bool flag to consider unknown objects for cruising
bool
common.cruise_obstacle_type.outside.unknown bool flag to consider unknown objects for cruising
common.cruise_obstacle_type.outside.car bool flag to consider unknown objects for cruising
common.cruise_obstacle_type.outside.truck bool flag to consider unknown objects for cruising
bool
behavior_determination.cruise.max_lat_margin double maximum lateral margin for cruise obstacles
behavior_determination.obstacle_velocity_threshold_from_cruise_to_stop double maximum obstacle velocity for cruise obstacle inside the trajectory
behavior_determination.cruise.outside_obstacle.obstacle_velocity_threshold double maximum obstacle velocity for cruise obstacle outside the trajectory
behavior_determination.cruise.outside_obstacle.ego_obstacle_overlap_time_threshold double maximum overlap time of the collision between the ego and obstacle
Yield for vehicles that might cut in into the ego’s lane

It is also possible to yield (cruise) behind vehicles in neighbor lanes if said vehicles might cut in the ego vehicle’s current lane.

The obstacles meeting the following condition are determined as obstacles for yielding (cruising).

  • The object type is for cruising according to common.cruise_obstacle_type.* and it is moving with a speed greater than behavior_determination.cruise.yield.stopped_obstacle_velocity_threshold.
  • The object is not crossing the ego’s trajectory (*1).
  • There is another object of type common.cruise_obstacle_type.* stopped in front of the moving obstacle.
  • The lateral distance (using the ego’s trajectory as reference) between both obstacles is less than behavior_determination.cruise.yield.max_lat_dist_between_obstacles
  • Both obstacles, moving and stopped, are within behavior_determination.cruise.yield.lat_distance_threshold and behavior_determination.cruise.yield.lat_distance_threshold + behavior_determination.cruise.yield.max_lat_dist_between_obstacles lateral distance from the ego’s trajectory respectively.

If the above conditions are met, the ego vehicle will cruise behind the moving obstacle, yielding to it so it can cut in into the ego’s lane to avoid the stopped obstacle.

Determine stop vehicles

Among obstacles which are not for cruising, the obstacles meeting the following condition are determined as obstacles for stopping.

  • The object type is for stopping according to common.stop_obstacle_type.*.
  • The lateral distance from the object to the ego’s trajectory is smaller than behavior_determination.stop.max_lat_margin.
  • The object velocity along the ego’s trajectory is smaller than behavior_determination.obstacle_velocity_threshold_from_stop_to_cruise.
  • The object
    • does not cross the ego’s trajectory (*1)
    • with the velocity smaller than behavior_determination.crossing_obstacle.obstacle_velocity_threshold
    • and its collision time margin is large enough (*2).
Parameter Type Description
common.stop_obstacle_type.unknown bool flag to consider unknown objects for stopping
common.stop_obstacle_type.car bool flag to consider unknown objects for stopping
common.stop_obstacle_type.truck bool flag to consider unknown objects for stopping
bool
behavior_determination.stop.max_lat_margin double maximum lateral margin for stop obstacles
behavior_determination.crossing_obstacle.obstacle_velocity_threshold double maximum crossing obstacle velocity to ignore
behavior_determination.obstacle_velocity_threshold_from_stop_to_cruise double maximum obstacle velocity for stop

Determine slow down vehicles

Among obstacles which are not for cruising and stopping, the obstacles meeting the following condition are determined as obstacles for slowing down.

  • The object type is for slowing down according to common.slow_down_obstacle_type.*.
  • The lateral distance from the object to the ego’s trajectory is smaller than behavior_determination.slow_down.max_lat_margin.
Parameter Type Description
common.slow_down_obstacle_type.unknown bool flag to consider unknown objects for slowing down
common.slow_down_obstacle_type.car bool flag to consider unknown objects for slowing down
common.slow_down_obstacle_type.truck bool flag to consider unknown objects for slowing down
bool
behavior_determination.slow_down.max_lat_margin double maximum lateral margin for slow down obstacles

NOTE

*1: Crossing obstacles

Crossing obstacle is the object whose orientation’s yaw angle against the ego’s trajectory is smaller than behavior_determination.crossing_obstacle.obstacle_traj_angle_threshold.

Parameter Type Description
behavior_determination.crossing_obstacle.obstacle_traj_angle_threshold double maximum angle against the ego’s trajectory to judge the obstacle is crossing the trajectory [rad]
*2: Enough collision time margin

We predict the collision area and its time by the ego with a constant velocity motion and the obstacle with its predicted path. Then, we calculate a collision time margin which is the difference of the time when the ego will be inside the collision area and the obstacle will be inside the collision area. When this time margin is smaller than behavior_determination.stop.crossing_obstacle.collision_time_margin, the margin is not enough.

Parameter Type Description
behavior_determination.stop.crossing_obstacle.collision_time_margin double maximum collision time margin of the ego and obstacle

Stop planning

Parameter Type Description
common.min_strong_accel double ego’s minimum acceleration to stop [m/ss]
common.safe_distance_margin double distance with obstacles for stop [m]
common.terminal_safe_distance_margin double terminal_distance with obstacles for stop, which cannot be exceed safe distance margin [m]

The role of the stop planning is keeping a safe distance with static vehicle objects or dynamic/static non vehicle objects.

The stop planning just inserts the stop point in the trajectory to keep a distance with obstacles. The safe distance is parameterized as common.safe_distance_margin. When it stops at the end of the trajectory, and obstacle is on the same point, the safe distance becomes terminal_safe_distance_margin.

When inserting the stop point, the required acceleration for the ego to stop in front of the stop point is calculated. If the acceleration is less than common.min_strong_accel, the stop planning will be cancelled since this package does not assume a strong sudden brake for emergency.

Cruise planning

Parameter Type Description
common.safe_distance_margin double minimum distance with obstacles for cruise [m]

The role of the cruise planning is keeping a safe distance with dynamic vehicle objects with smoothed velocity transition. This includes not only cruising a front vehicle, but also reacting a cut-in and cut-out vehicle.

The safe distance is calculated dynamically based on the Responsibility-Sensitive Safety (RSS) by the following equation.

\[d_{rss} = v_{ego} t_{idling} + \frac{1}{2} a_{ego} t_{idling}^2 + \frac{v_{ego}^2}{2 a_{ego}} - \frac{v_{obstacle}^2}{2 a_{obstacle}},\]

assuming that $d_{rss}$ is the calculated safe distance, $t_{idling}$ is the idling time for the ego to detect the front vehicle’s deceleration, $v_{ego}$ is the ego’s current velocity, $v_{obstacle}$ is the front obstacle’s current velocity, $a_{ego}$ is the ego’s acceleration, and $a_{obstacle}$ is the obstacle’s acceleration. These values are parameterized as follows. Other common values such as ego’s minimum acceleration is defined in common.param.yaml.

Parameter Type Description
common.idling_time double idling time for the ego to detect the front vehicle starting deceleration [s]
common.min_ego_accel_for_rss double ego’s acceleration for RSS [m/ss]
common.min_object_accel_for_rss double front obstacle’s acceleration for RSS [m/ss]

The detailed formulation is as follows.

\[\begin{align} d_{error} & = d - d_{rss} \\ d_{normalized} & = lpf(d_{error} / d_{obstacle}) \\ d_{quad, normalized} & = sign(d_{normalized}) *d_{normalized}*d_{normalized} \\ v_{pid} & = pid(d_{quad, normalized}) \\ v_{add} & = v_{pid} > 0 ? v_{pid}* w_{acc} : v_{pid} \\ v_{target} & = max(v_{ego} + v_{add}, v_{min, cruise}) \end{align}\]
Variable Description
d actual distance to obstacle
d_{rss} ideal distance to obstacle based on RSS
v_{min, cruise} min_cruise_target_vel
w_{acc} output_ratio_during_accel
lpf(val) apply low-pass filter to val
pid(val) apply pid to val

Block diagram

cruise_planning_block_diagram

Slow down planning

Parameter Type Description
slow_down.labels vector(string) A vector of labels for customizing obstacle-label-based slow down behavior. Each label represents an obstacle type that will be treated differently when applying slow down. The possible labels are (“default” (Mandatory), “unknown”,”car”,”truck”,”bus”,”trailer”,”motorcycle”,”bicycle” or “pedestrian”)
slow_down.default.static.min_lat_velocity double minimum velocity to linearly calculate slow down velocity [m]. Note: This default value will be used when the detected obstacle label does not match any of the slow_down.labels and the obstacle is considered to be static, or not moving
slow_down.default.static.max_lat_velocity double maximum velocity to linearly calculate slow down velocity [m]. Note: This default value will be used when the detected obstacle label does not match any of the slow_down.labels and the obstacle is considered to be static, or not moving
slow_down.default.static.min_lat_margin double minimum lateral margin to linearly calculate slow down velocity [m]. Note: This default value will be used when the detected obstacle label does not match any of the slow_down.labels and the obstacle is considered to be static, or not moving
slow_down.default.static.max_lat_margin double maximum lateral margin to linearly calculate slow down velocity [m]. Note: This default value will be used when the detected obstacle label does not match any of the slow_down.labels and the obstacle is considered to be static, or not moving
slow_down.default.moving.min_lat_velocity double minimum velocity to linearly calculate slow down velocity [m]. Note: This default value will be used when the detected obstacle label does not match any of the slow_down.labels and the obstacle is considered to be moving
slow_down.default.moving.max_lat_velocity double maximum velocity to linearly calculate slow down velocity [m]. Note: This default value will be used when the detected obstacle label does not match any of the slow_down.labels and the obstacle is considered to be moving
slow_down.default.moving.min_lat_margin double minimum lateral margin to linearly calculate slow down velocity [m]. Note: This default value will be used when the detected obstacle label does not match any of the slow_down.labels and the obstacle is considered to be moving
slow_down.default.moving.max_lat_margin double maximum lateral margin to linearly calculate slow down velocity [m]. Note: This default value will be used when the detected obstacle label does not match any of the slow_down.labels and the obstacle is considered to be moving
(optional) slow_down."label".(static & moving).min_lat_velocity double minimum velocity to linearly calculate slow down velocity [m]. Note: only for obstacles specified in slow_down.labels. Requires a static and a moving value
(optional) slow_down."label".(static & moving).max_lat_velocity double maximum velocity to linearly calculate slow down velocity [m]. Note: only for obstacles specified in slow_down.labels. Requires a static and a moving value
(optional) slow_down."label".(static & moving).min_lat_margin double minimum lateral margin to linearly calculate slow down velocity [m]. Note: only for obstacles specified in slow_down.labels. Requires a static and a moving value
(optional) slow_down."label".(static & moving).max_lat_margin double maximum lateral margin to linearly calculate slow down velocity [m]. Note: only for obstacles specified in slow_down.labels. Requires a static and a moving value

The role of the slow down planning is inserting slow down velocity in the trajectory where the trajectory points are close to the obstacles. The parameters can be customized depending on the obstacle type (see slow_down.labels), making it possible to adjust the slow down behavior depending if the obstacle is a pedestrian, bicycle, car, etc. Each obstacle type has a static and a moving parameter set, so it is possible to customize the slow down response of the ego vehicle according to the obstacle type and if it is moving or not. If an obstacle is determined to be moving, the corresponding moving set of parameters will be used to compute the vehicle slow down, otherwise, the static parameters will be used. The static and moving separation is useful for customizing the ego vehicle slow down behavior to, for example, slow down more significantly when passing stopped vehicles that might cause occlusion or that might suddenly open its doors.

An obstacle is classified as static if its total speed is less than the moving_object_speed_threshold parameter. Furthermore, a hysteresis based approach is used to avoid chattering, it uses the moving_object_hysteresis_range parameter range and the obstacle’s previous state (moving or static) to determine if the obstacle is moving or not. In other words, if an obstacle was previously classified as static, it will not change its classification to moving unless its total speed is greater than moving_object_speed_threshold + moving_object_hysteresis_range. Likewise, an obstacle previously classified as moving, will only change to static if its speed is lower than moving_object_speed_threshold - moving_object_hysteresis_range.

The closest point on the obstacle to the ego’s trajectory is calculated. Then, the slow down velocity is calculated by linear interpolation with the distance between the point and trajectory as follows.

slow_down_velocity_calculation

Variable Description
v_{out} calculated velocity for slow down
v_{min} slow_down.min_lat_velocity
v_{max} slow_down.max_lat_velocity
l_{min} slow_down.min_lat_margin
l_{max} slow_down.max_lat_margin
l'_{max} behavior_determination.slow_down.max_lat_margin

The calculated velocity is inserted in the trajectory where the obstacle is inside the area with behavior_determination.slow_down.max_lat_margin.

slow_down_planning

Implementation

Flowchart

Successive functions consist of autoware_obstacle_cruise_planner as follows.

Various algorithms for stop and cruise planning will be implemented, and one of them is designated depending on the use cases. The core algorithm implementation generateTrajectory depends on the designated algorithm.

@startuml
title onTrajectory
start

group convertToObstacles
  :check obstacle's label;
  :check obstacle is in front of ego;
  :check obstacle's lateral deviation to trajectory;
  :create obstacle instance;
end group

group determineEgoBehaviorAgainstObstacles
  :resampleExtendedTrajectory;
  group for each obstacle
    :createCruiseObstacle;
    :createStopObstacle;
    :createSlowDownObstacle;
  end group
  :update previous obstacles;
end group

:createPlannerData;

:generateStopTrajectory;

:generateCruiseTrajectory;

:generateSlowDownTrajectory;

:publish trajectory;

:publishDebugData;

:publish and print calculation time;

stop
@enduml

Algorithm selection for cruise planner

Currently, only a PID-based planner is supported. Each planner will be explained in the following.

Parameter Type Description
common.planning_method string cruise and stop planning algorithm, selected from “pid_base”

PID-based planner

Stop planning

In the pid_based_planner namespace,

Parameter Type Description
obstacle_velocity_threshold_from_cruise_to_stop double obstacle velocity threshold to be stopped from cruised [m/s]

Only one obstacle is targeted for the stop planning. It is the obstacle among obstacle candidates whose velocity is less than obstacle_velocity_threshold_from_cruise_to_stop, and which is the nearest to the ego along the trajectory. A stop point is inserted keepingcommon.safe_distance_margin distance between the ego and obstacle.

Note that, as explained in the stop planning design, a stop planning which requires a strong acceleration (less than common.min_strong_accel) will be canceled.

Cruise planning

In the pid_based_planner namespace,

Parameter Type Description
kp double p gain for pid control [-]
ki double i gain for pid control [-]
kd double d gain for pid control [-]
output_ratio_during_accel double The output velocity will be multiplied by the ratio during acceleration to follow the front vehicle. [-]
vel_to_acc_weight double target acceleration is target velocity * vel_to_acc_weight [-]
min_cruise_target_vel double minimum target velocity during cruise [m/s]

In order to keep the safe distance, the target velocity and acceleration is calculated and sent as an external velocity limit to the velocity smoothing package (motion_velocity_smoother by default). The target velocity and acceleration is respectively calculated with the PID controller according to the error between the reference safe distance and the actual distance.

Optimization-based planner

under construction

Minor functions

Prioritization of behavior module’s stop point

When stopping for a pedestrian walking on the crosswalk, the behavior module inserts the zero velocity in the trajectory in front of the crosswalk. Also autoware_obstacle_cruise_planner’s stop planning also works, and the ego may not reach the behavior module’s stop point since the safe distance defined in autoware_obstacle_cruise_planner may be longer than the behavior module’s safe distance. To resolve this non-alignment of the stop point between the behavior module and autoware_obstacle_cruise_planner, common.min_behavior_stop_margin is defined. In the case of the crosswalk described above, autoware_obstacle_cruise_planner inserts the stop point with a distance common.min_behavior_stop_margin at minimum between the ego and obstacle.

Parameter Type Description
common.min_behavior_stop_margin double minimum stop margin when stopping with the behavior module enabled [m]

A function to keep the closest stop obstacle in target obstacles

In order to keep the closest stop obstacle in the target obstacles, we check whether it is disappeared or not from the target obstacles in the checkConsistency function. If the previous closest stop obstacle is remove from the lists, we keep it in the lists for stop_obstacle_hold_time_threshold seconds. Note that if a new stop obstacle appears and the previous closest obstacle removes from the lists, we do not add it to the target obstacles again.

Parameter Type Description
behavior_determination.stop_obstacle_hold_time_threshold double maximum time for holding closest stop obstacle [s]

How To Debug

How to debug can be seen here.

Known Limits

  • Common
    • When the obstacle pose or velocity estimation has a delay, the ego sometimes will go close to the front vehicle keeping deceleration.
    • Current implementation only uses predicted objects message for static/dynamic obstacles and does not use pointcloud. Therefore, if object recognition is lost, the ego cannot deal with the lost obstacle.
    • The current predicted paths for obstacle’s lane change does not have enough precision for obstacle_cruise_planner. Therefore, we set rough_detection_area a small value.
  • PID-based planner
    • The algorithm strongly depends on the velocity smoothing package (motion_velocity_smoother by default) whether or not the ego realizes the designated target speed. If the velocity smoothing package is updated, please take care of the vehicle’s behavior as much as possible.
CHANGELOG

Changelog for package autoware_obstacle_cruise_planner

0.43.0 (2025-03-21)

  • Merge remote-tracking branch 'origin/main' into chore/bump-version-0.43
  • chore: rename from [autoware.universe]{.title-ref} to [autoware_universe]{.title-ref} (#10306)
  • feat(path_optimizer): additional failure logging and failure mode handling (#10276) MRM when MPT fails
  • feat!: replace VelocityLimit messages with autoware_internal_planning_msgs (#10273)
  • feat: adaption to ROS nodes guidelines about directory structure (#10268)
  • feat(Autoware_planning_factor_interface): replace tier4_msgs with autoware_internal_msgs (#10204)
  • Contributors: Arjun Jagdish Ram, Hayato Mizushima, NorahXiong, Ryohsuke Mitsudome, Yutaka Kondo, 心刚

0.42.0 (2025-03-03)

  • Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base
  • feat(autoware_utils): replace autoware_universe_utils with autoware_utils (#10191)
  • feat(autoware_vehicle_info_utils): replace autoware_universe_utils with autoware_utils (#10167)
  • feat(planning_test_manager): abstract message-specific functions (#9882)
    • abstract message-specific functions
    • include necessary header
    • adapt velocity_smoother to new test manager
    • adapt behavior_velocity_planner to new test manager
    • adapt path_optimizer to new test manager
    • fix output subscription
    • adapt behavior_path_planner to new test manager
    • adapt scenario_selector to new test manager
    • adapt freespace_planner to new test manager
    • adapt planning_validator to new test manager
    • adapt obstacle_stop_planner to new test manager
    • adapt obstacle_cruise_planner to new test manager
    • disable test for freespace_planner
    • adapt behavior_velocity_crosswalk_module to new test manager
    • adapt behavior_path_lane_change_module to new test manager
    • adapt behavior_path_avoidance_by_lane_change_module to new test manager
    • adapt behavior_path_dynamic_obstacle_avoidance_module to new test manager
    • adapt behavior_path_external_request_lane_change_module to new test manager
    • adapt behavior_path_side_shift_module to new test manager
    • adapt behavior_path_static_obstacle_avoidance_module to new test manager
    • adapt path_smoother to new test manager
    • adapt behavior_velocity_blind_spot_module to new test manager
    • adapt behavior_velocity_detection_area_module to new test manager
    • adapt behavior_velocity_intersection_module to new test manager
    • adapt behavior_velocity_no_stopping_area_module to new test manager
    • adapt behavior_velocity_run_out_module to new test manager
    • adapt behavior_velocity_stop_line_module to new test manager
    • adapt behavior_velocity_traffic_light_module to new test manager
    • adapt behavior_velocity_virtual_traffic_light_module to new test manager
    • adapt behavior_velocity_walkway_module to new test manager
    • adapt motion_velocity_planner_node_universe to new test manager
    • include necessary headers

    * Odometries -> Odometry ---------Co-authored-by: Takayuki Murooka <<takayuki5168@gmail.com>>

  • Contributors: Fumiya Watanabe, Mitsuhiro Sakamoto, Ryohsuke Mitsudome, 心刚

0.41.2 (2025-02-19)

  • chore: bump version to 0.41.1 (#10088)
  • Contributors: Ryohsuke Mitsudome

0.41.1 (2025-02-10)

0.41.0 (2025-01-29)

  • Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base
  • feat(autoware_obstacle_cruise_planner)!: tier4_debug_msgs changed to autoware_internal_debug_msgs in autoware_obstacle_cruise_planner (#9905) feat: tier4_debug_msgs changed to autoware_internal_debug_msgs in files planning/autoware_obstacle_cruise_planner
  • chore(planning): move package directory for planning factor interface (#9948)
    • chore: add new package for planning factor interface
    • chore(surround_obstacle_checker): update include file
    • chore(obstacle_stop_planner): update include file
    • chore(obstacle_cruise_planner): update include file
    • chore(motion_velocity_planner): update include file
    • chore(bpp): update include file
    • chore(bvp-common): update include file
    • chore(blind_spot): update include file
    • chore(crosswalk): update include file
    • chore(detection_area): update include file
    • chore(intersection): update include file
    • chore(no_drivable_area): update include file
    • chore(no_stopping_area): update include file
    • chore(occlusion_spot): update include file
    • chore(run_out): update include file
    • chore(speed_bump): update include file
    • chore(stop_line): update include file
    • chore(template_module): update include file
    • chore(traffic_light): update include file
    • chore(vtl): update include file
    • chore(walkway): update include file

    * chore(motion_utils): remove factor interface ---------

  • feat(planning_factor)!: remove velocity_factor, steering_factor and introduce planning_factor (#9927) Co-authored-by: Satoshi OTA <<44889564+satoshi-ota@users.noreply.github.com>> Co-authored-by: Ryohsuke Mitsudome <<43976834+mitsudome-r@users.noreply.github.com>> Co-authored-by: satoshi-ota <<satoshi.ota928@gmail.com>>
  • docs(obstacle_cruise_planner): add supplemental figures (#9154)
    • add behavior determination flowchart
    • add cruise planning block diagram

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

  • Contributors: Fumiya Watanabe, Mamoru Sobue, Mitsuhiro Sakamoto, Satoshi OTA, Vishal Chauhan

0.40.0 (2024-12-12)

  • Merge branch 'main' into release-0.40.0
  • Revert "chore(package.xml): bump version to 0.39.0 (#9587)" This reverts commit c9f0f2688c57b0f657f5c1f28f036a970682e7f5.
  • refactor(obstacle_cruise_planner)!: refactor rviz and terminal info (#9594)
  • fix: fix ticket links in CHANGELOG.rst (#9588)
  • chore(package.xml): bump version to 0.39.0 (#9587)
    • chore(package.xml): bump version to 0.39.0
    • fix: fix ticket links in CHANGELOG.rst

    * fix: remove unnecessary diff ---------Co-authored-by: Yutaka Kondo <<yutaka.kondo@youtalk.jp>>

  • fix: fix ticket links in CHANGELOG.rst (#9588)
  • fix(cpplint): include what you use - planning (#9570)
  • fix(obstacle_cruise_planner)!: remove stop reason (#9464) fix(obstacle_cruise_planner): remove stop reason
  • 0.39.0
  • update changelog
  • Merge commit '6a1ddbd08bd' into release-0.39.0
  • fix: fix ticket links to point to https://github.com/autowarefoundation/autoware_universe (#9304)
  • feat(obstacle_cruise_planner): outputs velocity factor when the ego follows front vehicle. (#9359)
    • feat(obstacle_cruise_planner): outputs velocity factor when the ego follows front vehicle.

    * fix: cppcheck ---------

  • fix(autoware_obstacle_cruise_planner): fix clang-diagnostic-delete-abstract-non-virtual-dtor (#9419) fix: clang-diagnostic-delete-abstract-non-virtual-dtor
  • fix(autoware_obstacle_cruise_planner): fix clang-diagnostic-defaulted-function-deleted (#9398) fix: clang-diagnostic-defaulted-function-deleted
  • fix: fix ticket links to point to https://github.com/autowarefoundation/autoware_universe (#9304)
  • feat(tier4_metric_msgs): apply tier4_metric_msgs for scenario_simulator_v2_adapter, control_evaluator, planning_evaluator, autonomous_emergency_braking, obstacle_cruise_planner, motion_velocity_planner, processing_time_checker (#9180)
    • first commit
    • fix building errs.
    • change diagnostic messages to metric messages for publishing decision.
    • fix bug about motion_velocity_planner
    • change the diagnostic msg to metric msg in autoware_obstacle_cruise_planner.
    • tmp save for planning_evaluator
    • change the topic to which metrics published to.
    • fix typo.
    • remove unnesessary publishing of metrics.
    • mke planning_evaluator publish msg of MetricArray instead of Diags.
    • update aeb with metric type for decision.
    • fix some bug
    • remove autoware_evaluator_utils package.
    • remove diagnostic_msgs dependency of planning_evaluator
    • use metric_msgs for autoware_processing_time_checker.
    • rewrite diagnostic_convertor to scenario_simulator_v2_adapter, supporting metric_msgs.
    • pre-commit and fix typo
    • publish metrics even if there is no metric in the MetricArray.
    • modify the metric name of processing_time.
    • update unit test for test_planning/control_evaluator

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

  • chore(package.xml): bump version to 0.38.0 (#9266) (#9284)
    • unify package.xml version to 0.37.0
    • remove system_monitor/CHANGELOG.rst
    • add changelog

    * 0.38.0

  • chore(obstacle_cruise_planner): add function tests for a utils function (#9206) * add utils test ---------

  • Contributors: Esteve Fernandez, Fumiya Watanabe, Kem (TiankuiXian), M. Fatih Cırıt, Ryohsuke Mitsudome, Satoshi OTA, Yuki TAKAGI, Yutaka Kondo, kobayu858

0.39.0 (2024-11-25)

  • Merge commit '6a1ddbd08bd' into release-0.39.0
  • fix: fix ticket links to point to https://github.com/autowarefoundation/autoware_universe (#9304)
  • fix: fix ticket links to point to https://github.com/autowarefoundation/autoware_universe (#9304)
  • feat(tier4_metric_msgs): apply tier4_metric_msgs for scenario_simulator_v2_adapter, control_evaluator, planning_evaluator, autonomous_emergency_braking, obstacle_cruise_planner, motion_velocity_planner, processing_time_checker (#9180)
    • first commit
    • fix building errs.
    • change diagnostic messages to metric messages for publishing decision.
    • fix bug about motion_velocity_planner
    • change the diagnostic msg to metric msg in autoware_obstacle_cruise_planner.
    • tmp save for planning_evaluator
    • change the topic to which metrics published to.
    • fix typo.
    • remove unnesessary publishing of metrics.
    • mke planning_evaluator publish msg of MetricArray instead of Diags.
    • update aeb with metric type for decision.
    • fix some bug
    • remove autoware_evaluator_utils package.
    • remove diagnostic_msgs dependency of planning_evaluator
    • use metric_msgs for autoware_processing_time_checker.
    • rewrite diagnostic_convertor to scenario_simulator_v2_adapter, supporting metric_msgs.
    • pre-commit and fix typo
    • publish metrics even if there is no metric in the MetricArray.
    • modify the metric name of processing_time.
    • update unit test for test_planning/control_evaluator

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

  • chore(package.xml): bump version to 0.38.0 (#9266) (#9284)
    • unify package.xml version to 0.37.0
    • remove system_monitor/CHANGELOG.rst
    • add changelog

    * 0.38.0

  • chore(obstacle_cruise_planner): add function tests for a utils function (#9206) * add utils test ---------

  • Contributors: Esteve Fernandez, Kem (TiankuiXian), Yuki TAKAGI, Yutaka Kondo

0.38.0 (2024-11-08)

  • unify package.xml version to 0.37.0
  • refactor(object_recognition_utils): add autoware prefix to object_recognition_utils (#8946)
  • refactor(osqp_interface): added autoware prefix to osqp_interface (#8958)
  • chore(obstacle_cruise_planner): add maintainer (#9077)
  • feat(obstacle_cruise_planner): improve stop and cruise behavior for cut-in & out (#8072)
    • feat(obstacle_cruise_planner): improve stop and cruise behavior for cut-in & out

    * cleanup, add stop safety margin for transient objects style(pre-commit): autofix

    • fix: debug
    • fix: precommit error
    • fix: unused-variable
    • feat: improve cruise behavior for outside obstacles
    • fix projected velocity, improve transient obstacle behavior
    • feat: add predefined deceleration rate for VRUs

    * feat: update ---------

  • refactor(autoware_interpolation): prefix package and namespace with autoware (#8088) Co-authored-by: kosuke55 <<kosuke.tnp@gmail.com>>
  • refactor(signal_processing): prefix package and namespace with autoware (#8541)
  • fix(motion_planning): align the parameters with launcher (#8792) parameters in motion_planning aligned
  • fix(velocity_smoother, obstacle_cruise_planner ): float type of processing time was wrong (#8161) fix(velocity_smoother): float type of processing time was wrong
  • feat(cruise_planner,planning_evaluator): add cruise and slow down diags (#7960)
    • add cruise and slow down diags to cruise planner
    • add cruise types

    * adjust planning eval ---------

  • feat(obstacle_cruise_planner): prevent chattering when using point cloud (#7861)
    • prevent chattering of stop planning

    * Update planning/autoware_obstacle_cruise_planner/src/node.cpp Co-authored-by: Satoshi OTA <<44889564+satoshi-ota@users.noreply.github.com>> * fix stop position oscillation ---------Co-authored-by: Satoshi OTA <<44889564+satoshi-ota@users.noreply.github.com>>

  • feat(obstacle_cruise_planner): add diagnostics publishing to cruise planner (#7836) add diagnostics publishing to cruise planner
  • feat(obstacle_cruise_planner): support pointcloud-based obstacles (#6907)
    • add pointcloud to obstacle properties
    • add tf listener & pointcloud subscriber
    • add parameters for pointcloud obstacle
    • add type aliases
    • convert pointcloud to obstacle
    • add type alias
    • add polygon conversion for pointcloud obstacle
    • initialize twist & pose of pointcloud obstacle
    • overload to handle both obstacle & predicted path
    • implement ego behavior determination against pointcloud obstacles
    • generate obstacle from point
    • revert getCollisionIndex()
    • generate obstacle from each point in cloud
    • set pointcloud obstacle velocity to 0
    • use tf buffer & listener with pointers
    • update latest pointcloud data
    • add topic remap
    • remove unnecessary includes
    • set slow down obstacle velocity to 0
    • add flag to consider pointcloud obstacle for stopping & slowing down
    • style(pre-commit): autofix
    • downsample pointcloud using voxel grid
    • change shape type of pointcloud obstacle to polygon
    • convert pointcloud to obstacle by clustering
    • add parameters for clustering
    • add max_num_points parameter to dummy object
    • downsample pointcloud when the number of points is larger than max_num_points
    • add max_num_points property to dummy bus
    • add parameters for pointcloud based obstacles
    • store pointcloud in obstacle struct
    • change obstacle conversion method
    • migrate previous changes to new package
    • store necessary points only
    • move use_pointcloud to common parameter
    • extract necessary points from pointcloud
    • add use_pointcloud parameter to planner interface
    • fix obstacle conversion
    • fix collision point determination
    • simplify pointcloud transformation
    • style(pre-commit): autofix
    • fix collision point determination
    • pick nearest stop collision point
    • check collision for every point in cluster
    • migrate previous changes to new files
    • reduce diff
    • remove use_pointcloud parameter
    • add parameters for pointcloud filtering
    • add autoware namespace

    * Revert "add max_num_points parameter to dummy object" This reverts commit 98bcd0856f861d23c9f7989d8128939ec0b3e27c. * Revert "downsample pointcloud when the number of points is larger than max_num_points" This reverts commit fb00b59d8f14cec6810e7fab12bc34d8a0c617c7. * Revert "add max_num_points property to dummy bus" This reverts commit 5f9e4ab5ae7d8d46521c736b1d259040121f3bc5.

    • feat(diagnostic_graph_utils): add logging tool
    • fix all OK
    • feat(default_ad_api): add log when operation mode change fails
    • get only the necessary one of object or pointcloud data
    • addfield for obstacle source type
    • enable simultaneous use of PredictedObjects and PointCloud
    • separate convertToObstacles() by source type
    • avoid using pointer
    • reduce diff
    • make nest shallower
    • define vector concatenate function
    • shorten variable names

    * fix redundant condition ---------Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]\@users.noreply.github.com> Co-authored-by: Takagi, Isamu <<isamu.takagi@tier4.jp>> Co-authored-by: Satoshi OTA <<44889564+satoshi-ota@users.noreply.github.com>>

  • feat: add [autoware_]{.title-ref} prefix to [lanelet2_extension]{.title-ref} (#7640)
  • fix(autoware_obstacle_cruise_planner): fix shadowVariable warning in generateSlowDownTrajectory (#7659)
  • fix(autoware_obstacle_cruise_planner): fix shadowVariable warning (#7656)
    • fix(autoware_obstacle_cruise_planner): fix shadowVariable warning
    • fix

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

  • fix(autoware_obstacle_cruise_planner): fix knownConditionTrueFalse warnings (#7620)
    • fix(autoware_obstacle_cruise_planner): fix knownConditionTrueFalse warnings

    * fix

  • fix(autoware_obstacle_cruise_planner): fix unreadVariable warning (#7627)
  • refactor(obstacle_cruise_planner): apply clang-tidy check (#7553) obstacle_cruise
  • refactor(universe_utils/motion_utils)!: add autoware namespace (#7594)
  • fix(autoware_obstacle_cruise_planner): fix assignBoolToFloat warning (#7541)
    • fix(autoware_obstacle_cruise_planner): fix assignBoolToFloat warning
    • delete unnecessary file

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

  • fix(autoware_obstacle_cruise_planner): fix unusedScopedObject bug (#7569)
  • refactor(motion_utils)!: add autoware prefix and include dir (#7539) refactor(motion_utils): add autoware prefix and include dir
  • feat(autoware_universe_utils)!: rename from tier4_autoware_utils (#7538) Co-authored-by: kosuke55 <<kosuke.tnp@gmail.com>>
  • feat(obstacle_cruise_planner): rename to include/autoware/{package_name} (#7510)
  • refactor(test_utils): move to common folder (#7158)
    • Move autoware planning test manager to autoware namespace
    • fix package share directory for behavior path planner
    • renaming files and directory
    • rename variables that has planning_test_utils in its name.
    • use autoware namespace for test utils
    • move folder to common
    • update .pages file
    • fix test error
    • removed obstacle velocity limiter test artifact

    * remove namespace from planning validator, it has using keyword ---------

  • refactor(obstacle_cruise_planner)!: add autoware_ prefix (#7419)
  • Contributors: Berkay Karaman, Esteve Fernandez, Koichi98, Kosuke Takeuchi, Mamoru Sobue, Mitsuhiro Sakamoto, Ryuta Kambe, Takayuki Murooka, Yuki TAKAGI, Yutaka Kondo, Zhe Shen, Zulfaqar Azmi, danielsanchezaran

0.26.0 (2024-04-03)

Wiki Tutorials

This package does not provide any links to tutorials in it's rosindex metadata. You can check on the ROS Wiki Tutorials page for the package.

Launch files

  • launch/obstacle_cruise_planner.launch.xml
      • obstacle_cruise_planner_param_path [default: $(find-pkg-share autoware_obstacle_cruise_planner)/config/obstacle_cruise_planner.param.yaml]
      • vehicle_info_param_path [default: $(find-pkg-share autoware_vehicle_info_utils)/config/vehicle_info.param.yaml]
      • input_trajectory [default: /planning/scenario_planning/lane_driving/trajectory]
      • input_odometry [default: /localization/kinematic_state]
      • input_map [default: /map/vector_map]
      • input_objects [default: /perception/object_recognition/objects]
      • output_trajectory [default: /planning/scenario_planning/lane_driving/trajectory]
      • output_velocity_limit [default: /planning/scenario_planning/max_velocity_candidates]
      • output_clear_velocity_limit [default: /planning/scenario_planning/clear_velocity_limit]

Messages

No message files found.

Services

No service files found

Plugins

No plugins found.

Recent questions tagged autoware_obstacle_cruise_planner at Robotics Stack Exchange

No version for distro noetic. Known supported distros are highlighted in the buttons above.
No version for distro ardent. Known supported distros are highlighted in the buttons above.
No version for distro bouncy. Known supported distros are highlighted in the buttons above.
No version for distro crystal. Known supported distros are highlighted in the buttons above.
No version for distro eloquent. Known supported distros are highlighted in the buttons above.
No version for distro dashing. Known supported distros are highlighted in the buttons above.
No version for distro galactic. Known supported distros are highlighted in the buttons above.
No version for distro foxy. Known supported distros are highlighted in the buttons above.
No version for distro iron. Known supported distros are highlighted in the buttons above.
No version for distro lunar. Known supported distros are highlighted in the buttons above.
No version for distro jade. Known supported distros are highlighted in the buttons above.
No version for distro indigo. Known supported distros are highlighted in the buttons above.
No version for distro hydro. Known supported distros are highlighted in the buttons above.
No version for distro kinetic. Known supported distros are highlighted in the buttons above.
No version for distro melodic. Known supported distros are highlighted in the buttons above.