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.

obstacle_stop_planner package from autodrrt repo

autonomous_emergency_braking control_performance_analysis control_validator external_cmd_selector joy_controller lane_departure_checker mpc_lateral_controller obstacle_collision_checker operation_mode_transition_manager pid_longitudinal_controller predicted_path_checker pure_pursuit shift_decider trajectory_follower_base trajectory_follower_node vehicle_cmd_gate diagnostic_converter kinematic_evaluator localization_evaluator planning_evaluator ekf_localizer geo_pose_projector gyro_odometer ar_tag_based_localizer landmark_manager localization_error_monitor localization_util ndt_scan_matcher pose2twist pose_initializer pose_instability_detector stop_filter tree_structured_parzen_estimator twist2accel yabloc_common yabloc_image_processing yabloc_monitor yabloc_particle_filter yabloc_pose_initializer map_height_fitter map_loader map_projection_loader map_tf_generator lanelet2_map_preprocessor ros2_bevdet ros2_bevformer bevfusion bytetrack cluster_merger compare_map_segmentation crosswalk_traffic_light_estimator detected_object_feature_remover detected_object_validation detection_by_tracker elevation_map_loader euclidean_cluster front_vehicle_velocity_estimator ground_segmentation heatmap_visualizer image_projection_based_fusion lidar_apollo_instance_segmentation lidar_apollo_segmentation_tvm lidar_apollo_segmentation_tvm_nodes lidar_centerpoint lidar_centerpoint_tvm map_based_prediction multi_object_tracker object_merger object_range_splitter object_velocity_splitter occupancy_grid_map_outlier_filter probabilistic_occupancy_grid_map radar_crossing_objects_noise_filter radar_fusion_to_detected_object radar_object_clustering radar_object_tracker radar_tracks_msgs_converter shape_estimation simple_object_merger tensorrt_classifier tensorrt_yolo tensorrt_yolox tracking_object_merger traffic_light_arbiter traffic_light_classifier traffic_light_fine_detector traffic_light_map_based_detector traffic_light_multi_camera_fusion traffic_light_occlusion_predictor traffic_light_ssd_fine_detector traffic_light_visualization behavior_path_avoidance_by_lane_change_module behavior_path_avoidance_module behavior_path_external_request_lane_change_module behavior_path_goal_planner_module behavior_path_lane_change_module behavior_path_planner behavior_path_planner_common behavior_path_side_shift_module behavior_path_start_planner_module behavior_velocity_blind_spot_module behavior_velocity_crosswalk_module behavior_velocity_detection_area_module behavior_velocity_intersection_module behavior_velocity_no_drivable_lane_module behavior_velocity_no_stopping_area_module behavior_velocity_occlusion_spot_module behavior_velocity_out_of_lane_module behavior_velocity_planner behavior_velocity_planner_common behavior_velocity_run_out_module behavior_velocity_speed_bump_module behavior_velocity_stop_line_module behavior_velocity_template_module behavior_velocity_traffic_light_module behavior_velocity_virtual_traffic_light_module behavior_velocity_walkway_module costmap_generator external_velocity_limit_selector freespace_planner freespace_planning_algorithms mission_planner motion_velocity_smoother objects_of_interest_marker_interface obstacle_avoidance_planner obstacle_cruise_planner obstacle_stop_planner obstacle_velocity_limiter path_smoother planning_debug_tools planning_test_utils planning_topic_converter planning_validator route_handler rtc_interface rtc_replayer bezier_sampler frenet_planner path_sampler sampler_common scenario_selector static_centerline_optimizer surround_obstacle_checker gnss_poser image_diagnostics image_transport_decompressor imu_corrector livox_tag_filter pointcloud_preprocessor radar_scan_to_pointcloud2 radar_static_pointcloud_filter radar_threshold_filter radar_tracks_noise_filter tier4_pcl_extensions vehicle_velocity_converter autoware_auto_msgs_adapter bluetooth_monitor component_state_monitor default_ad_api ad_api_adaptors ad_api_visualizers automatic_pose_initializer diagnostic_graph_aggregator dummy_diag_publisher dummy_infrastructure duplicated_node_checker emergency_handler mrm_comfortable_stop_operator mrm_emergency_stop_operator system_error_monitor system_monitor topic_state_monitor velodyne_monitor accel_brake_map_calibrator external_cmd_converter raw_vehicle_cmd_converter steer_offset_estimator vehicle_info_util launch launch_ros autoware_ad_api_specs autoware_adapi_v1_msgs autoware_adapi_version_msgs autoware_auto_common autoware_auto_geometry autoware_auto_control_msgs autoware_auto_geometry_msgs autoware_auto_mapping_msgs autoware_auto_msgs autoware_auto_perception_msgs autoware_auto_planning_msgs autoware_auto_system_msgs autoware_auto_vehicle_msgs autoware_auto_perception_rviz_plugin autoware_auto_tf2 autoware_cmake autoware_lint_common autoware_utils lanelet2_extension autoware_common_msgs autoware_control_msgs autoware_localization_msgs autoware_map_msgs autoware_perception_msgs autoware_planning_msgs autoware_sensing_msgs autoware_system_msgs autoware_vehicle_msgs autoware_point_types autoware_testing bag_time_manager_rviz_plugin component_interface_specs component_interface_tools component_interface_utils cuda_utils fake_test_node geography_utils global_parameter_loader glog_component goal_distance_calculator grid_map_utils interpolation kalman_filter motion_utils object_recognition_utils osqp_interface path_distance_calculator perception_utils polar_grid qp_interface rtc_manager_rviz_plugin signal_processing tensorrt_common tier4_adapi_rviz_plugin tier4_api_utils tier4_automatic_goal_rviz_plugin tier4_autoware_utils tier4_calibration_rviz_plugin tier4_camera_view_rviz_plugin tier4_control_rviz_plugin tier4_datetime_rviz_plugin tier4_debug_rviz_plugin tier4_debug_tools tier4_localization_rviz_plugin tier4_perception_rviz_plugin tier4_planning_rviz_plugin tier4_screen_capture_rviz_plugin tier4_simulated_clock_rviz_plugin tier4_state_rviz_plugin tier4_system_rviz_plugin tier4_target_object_type_rviz_plugin tier4_traffic_light_rviz_plugin tier4_vehicle_rviz_plugin time_utils simulator_compatibility_test traffic_light_recognition_marker_publisher traffic_light_utils tvm_utility dma_customer_msg dma_transfer eagleye_coordinate eagleye_navigation eagleye_msgs eagleye_rt eagleye_can_velocity_converter eagleye_fix2kml eagleye_geo_pose_converter eagleye_geo_pose_fusion eagleye_gnss_converter eagleye_tf llh_converter morai_msgs mussp ndt_omp orocos_kdl python_orocos_kdl pointcloud_to_laserscan rtklib_bridge rtklib_msgs autoware_external_api_msgs autoware_iv_external_api_adaptor autoware_iv_internal_api_adaptor awapi_awiv_adapter tier4_api_msgs tier4_auto_msgs_converter tier4_control_msgs tier4_debug_msgs tier4_external_api_msgs tier4_hmi_msgs tier4_localization_msgs tier4_map_msgs tier4_perception_msgs tier4_planning_msgs tier4_rtc_msgs tier4_simulation_msgs tier4_system_msgs tier4_v2x_msgs tier4_vehicle_msgs io_opt 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 fastrtps cyclonedds lanelet2 lanelet2_core lanelet2_examples lanelet2_io lanelet2_maps lanelet2_matching lanelet2_projection lanelet2_python lanelet2_routing lanelet2_traffic_rules lanelet2_validation sophus angles behaviortree_cpp_v3 bond bond_core bondcpp bondpy smclib test_bond cudnn_cmake_module diagnostic_aggregator diagnostic_common_diagnostics diagnostic_updater diagnostics self_test filters geodesy geographic_info geographic_msgs grid_map grid_map_cmake_helpers grid_map_core grid_map_costmap_2d grid_map_cv grid_map_demos grid_map_filters grid_map_loader grid_map_msgs grid_map_octomap grid_map_pcl grid_map_ros grid_map_rviz_plugin grid_map_sdf grid_map_visualization mrt_cmake_modules nav2_amcl nav2_behavior_tree nav2_behaviors nav2_bringup nav2_bt_navigator nav2_collision_monitor nav2_common nav2_controller nav2_core nav2_costmap_2d costmap_queue dwb_core dwb_critics dwb_msgs dwb_plugins nav2_dwb_controller nav_2d_msgs nav_2d_utils nav2_lifecycle_manager nav2_map_server nav2_msgs nav2_navfn_planner nav2_planner nav2_regulated_pure_pursuit_controller nav2_rotation_shim_controller nav2_rviz_plugins nav2_simple_commander nav2_smac_planner nav2_smoother nav2_system_tests nav2_theta_star_planner nav2_util nav2_velocity_smoother nav2_voxel_grid nav2_waypoint_follower navigation2 dynamic_edt_3d octomap octovis octomap_msgs osqp_vendor pacmod3_msgs pcl_msgs pcl_conversions pcl_ros perception_pcl point_cloud_msg_wrapper radar_msgs can_msgs rqt_tf_tree tensorrt_cmake_module topic_tools topic_tools_interfaces tvm_vendor cv_bridge image_geometry opencv_tests vision_opencv xacro rviz2 rviz_assimp_vendor rviz_common rviz_default_plugins rviz_ogre_vendor rviz_rendering rviz_rendering_tests rviz_visual_testing_framework dummy_perception_publisher fault_injection simple_planning_simulator classformsg node_v2x image_view v4l2_camera can_interface_custom cgi430_can_driver cgi610_driver ARS408_driver data_format_dump data_preprocess_launch lidar_centerpoint_collect lidar_saver message_sync time_cal camera_calibration direct_visual_lidar_calibration multi_lidar_calibration

Package Summary

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

Repository Summary

Checkout URI https://github.com/ieiauto/autodrrt.git
VCS Type git
VCS Version main
Last Updated 2024-09-19
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 obstacle_stop_planner package

Additional Links

No additional links.

Maintainers

  • Satoshi Ota
  • Taiki Tanaka
  • Tomoya Kimura
  • Shumpei Wakabayashi
  • Berkay Karaman

Authors

  • Satoshi Ota
  • Yukihiro Saito

Obstacle Stop Planner

Overview

obstacle_stop_planner has following modules

  • Obstacle Stop Planner
    • inserts a stop point in trajectory when there is a static point cloud on the trajectory.
  • Slow Down Planner
    • inserts a deceleration section in trajectory when there is a point cloud near the trajectory.
  • Adaptive Cruise Controller (ACC)
    • embeds target velocity in trajectory when there is a dynamic point cloud on the trajectory.

Input topics

Name Type Description
~/input/pointcloud sensor_msgs::PointCloud2 obstacle pointcloud
~/input/trajectory autoware_auto_planning_msgs::Trajectory trajectory
~/input/vector_map autoware_auto_mapping_msgs::HADMapBin vector map
~/input/odometry nav_msgs::Odometry vehicle velocity
~/input/dynamic_objects autoware_auto_perception_msgs::PredictedObjects dynamic objects
~/input/expand_stop_range tier4_planning_msgs::msg::ExpandStopRange expand stop range

Output topics

Name Type Description
~output/trajectory autoware_auto_planning_msgs::Trajectory trajectory to be followed
~output/stop_reasons tier4_planning_msgs::StopReasonArray reasons that cause the vehicle to stop

Common Parameter

Parameter Type Description
enable_slow_down bool enable slow down planner [-]
max_velocity double max velocity [m/s]
chattering_threshold double even if the obstacle disappears, the stop judgment continues for chattering_threshold [s]
enable_z_axis_obstacle_filtering bool filter obstacles in z axis (height) [-]
z_axis_filtering_buffer double additional buffer for z axis filtering [m]
use_predicted_objects bool whether to use predicted objects for collision and slowdown detection [-]
predicted_object_filtering_threshold double threshold for filtering predicted objects [valid only publish_obstacle_polygon true] [m]
publish_obstacle_polygon bool if use_predicted_objects is true, node publishes collision polygon [-]

Obstacle Stop Planner

Role

This module inserts the stop point before the obstacle with margin. In nominal case, the margin is the sum of baselink_to_front and max_longitudinal_margin. The baselink_to_front means the distance between baselink( center of rear-wheel axis) and front of the car. The detection area is generated along the processed trajectory as following figure. (This module cut off the input trajectory behind the ego position and decimates the trajectory points for reducing computational costs.)

![example](./docs/collision_parameters.svg){width=1000}
parameters for obstacle stop planner
![example](./docs/stop_target.svg){width=1000}
target for obstacle stop planner

If another stop point has already been inserted by other modules within max_longitudinal_margin, the margin is the sum of baselink_to_front and min_longitudinal_margin. This feature exists to avoid stopping unnaturally position. (For example, the ego stops unnaturally far away from stop line of crosswalk that pedestrians cross to without this feature.)

![example](./docs/min_longitudinal_margin.svg){width=1000}
minimum longitudinal margin

The module searches the obstacle pointcloud within detection area. When the pointcloud is found, Adaptive Cruise Controller modules starts to work. only when Adaptive Cruise Controller modules does not insert target velocity, the stop point is inserted to the trajectory. The stop point means the point with 0 velocity.

Restart prevention

If it needs X meters (e.g. 0.5 meters) to stop once the vehicle starts moving due to the poor vehicle control performance, the vehicle goes over the stopping position that should be strictly observed when the vehicle starts to moving in order to approach the near stop point (e.g. 0.3 meters away).

This module has parameter hold_stop_margin_distance in order to prevent from these redundant restart. If the vehicle is stopped within hold_stop_margin_distance meters from stop point of the module, the module judges that the vehicle has already stopped for the module’s stop point and plans to keep stopping current position even if the vehicle is stopped due to other factors.

![example](./docs/restart_prevention.svg){width=1000}
parameters
![example](./docs/restart.svg){width=1000}
outside the hold_stop_margin_distance
![example](./docs/keep_stopping.svg){width=1000}
inside the hold_stop_margin_distance

Parameters

Stop position

Parameter Type Description
max_longitudinal_margin double margin between obstacle and the ego’s front [m]
max_longitudinal_margin_behind_goal double margin between obstacle and the ego’s front when the stop point is behind the goal[m]
min_longitudinal_margin double if any obstacle exists within max_longitudinal_margin, this module set margin as the value of stop margin to min_longitudinal_margin [m]
hold_stop_margin_distance double parameter for restart prevention (See above section) [m]

Obstacle detection area

Parameter Type Description
lateral_margin double lateral margin from the vehicle footprint for collision obstacle detection area [m]
step_length double step length for pointcloud search range [m]
enable_stop_behind_goal_for_obstacle bool enabling extend trajectory after goal lane for obstacle detection

Flowchart

@startuml
title insertStopPoint
start


:callback trajectory;

:trim trajectory from self-pose;

:decimate trajectory;

:generate detection area;

if ( search obstacle pointcloud in detection area?) then (yes(find))
else (no(not find))
  stop
endif

if ( insert target velocity by adaptive cruise module?) then (yes)
  stop
else (no)
endif

:insert stop point;


stop
@enduml

Slow Down Planner

Role

This module inserts the slow down section before the obstacle with forward margin and backward margin. The forward margin is the sum of baselink_to_front and longitudinal_forward_margin, and the backward margin is the sum of baselink_to_front and longitudinal_backward_margin. The ego keeps slow down velocity in slow down section. The velocity is calculated the following equation.

$v_{target} = v_{min} + \frac{l_{ld} - l_{vw}/2}{l_{margin}} (v_{max} - v_{min} )$

  • $v_{target}$ : slow down target velocity [m/s]
  • $v_{min}$ : min_slow_down_velocity [m/s]
  • $v_{max}$ : max_slow_down_velocity [m/s]
  • $l_{ld}$ : lateral deviation between the obstacle and the ego footprint [m]
  • $l_{margin}$ : lateral_margin [m]
  • $l_{vw}$ : width of the ego footprint [m]

The above equation means that the smaller the lateral deviation of the pointcloud, the lower the velocity of the slow down section.

![example](./docs/slow_down_parameters.svg){width=1000}
parameters for slow down planner
![example](./docs/slow_down_target.svg){width=1000}
target for slow down planner

Parameters

Slow down section

Parameter Type Description
longitudinal_forward_margin double margin between obstacle and the ego’s front [m]
longitudinal_backward_margin double margin between obstacle and the ego’s rear [m]

Obstacle detection area

Parameter Type Description
lateral_margin double lateral margin from the vehicle footprint for slow down obstacle detection area [m]

Slow down target velocity

Parameter Type Description
max_slow_down_velocity double max slow down velocity [m/s]
min_slow_down_velocity double min slow down velocity [m/s]

Flowchart

@startuml
title insertDecelerationPoint
start


:callback trajectory;

:trim trajectory from self-pose;

:decimate trajectory;

:generate detection area;

if ( search obstacle pointcloud in detection area?) then (yes(find))
else (no(not find))
  stop
endif

:insert deceleration point;


stop
@enduml

Adaptive Cruise Controller

Role

Adaptive Cruise Controller module embeds maximum velocity in trajectory when there is a dynamic point cloud on the trajectory. The value of maximum velocity depends on the own velocity, the velocity of the point cloud ( = velocity of the front car), and the distance to the point cloud (= distance to the front car).

Parameter Type Description
adaptive_cruise_control.use_object_to_estimate_vel bool use dynamic objects for estimating object velocity or not (valid only if osp.use_predicted_objects false)
adaptive_cruise_control.use_pcl_to_estimate_vel bool use raw pointclouds for estimating object velocity or not (valid only if osp.use_predicted_objects false)
adaptive_cruise_control.consider_obj_velocity bool consider forward vehicle velocity to calculate target velocity in adaptive cruise or not
adaptive_cruise_control.obstacle_velocity_thresh_to_start_acc double start adaptive cruise control when the velocity of the forward obstacle exceeds this value [m/s]
adaptive_cruise_control.obstacle_velocity_thresh_to_stop_acc double stop acc when the velocity of the forward obstacle falls below this value [m/s]
adaptive_cruise_control.emergency_stop_acceleration double supposed minimum acceleration (deceleration) in emergency stop [m/ss]
adaptive_cruise_control.emergency_stop_idling_time double supposed idling time to start emergency stop [s]
adaptive_cruise_control.min_dist_stop double minimum distance of emergency stop [m]
adaptive_cruise_control.obstacle_emergency_stop_acceleration double supposed minimum acceleration (deceleration) in emergency stop [m/ss]
adaptive_cruise_control.max_standard_acceleration double supposed maximum acceleration in active cruise control [m/ss]
adaptive_cruise_control.min_standard_acceleration double supposed minimum acceleration (deceleration) in active cruise control [m/ss]
adaptive_cruise_control.standard_idling_time double supposed idling time to react object in active cruise control [s]
adaptive_cruise_control.min_dist_standard double minimum distance in active cruise control [m]
adaptive_cruise_control.obstacle_min_standard_acceleration double supposed minimum acceleration of forward obstacle [m/ss]
adaptive_cruise_control.margin_rate_to_change_vel double rate of margin distance to insert target velocity [-]
adaptive_cruise_control.use_time_compensation_to_calc_distance bool use time-compensation to calculate distance to forward vehicle
adaptive_cruise_control.p_coefficient_positive double coefficient P in PID control (used when target dist -current_dist >=0) [-]
adaptive_cruise_control.p_coefficient_negative double coefficient P in PID control (used when target dist -current_dist <0) [-]
adaptive_cruise_control.d_coefficient_positive double coefficient D in PID control (used when delta_dist >=0) [-]
adaptive_cruise_control.d_coefficient_negative double coefficient D in PID control (used when delta_dist <0) [-]
adaptive_cruise_control.object_polygon_length_margin double The distance to extend the polygon length the object in pointcloud-object matching [m]
adaptive_cruise_control.object_polygon_width_margin double The distance to extend the polygon width the object in pointcloud-object matching [m]
adaptive_cruise_control.valid_estimated_vel_diff_time double Maximum time difference treated as continuous points in speed estimation using a point cloud [s]
adaptive_cruise_control.valid_vel_que_time double Time width of information used for speed estimation in speed estimation using a point cloud [s]
adaptive_cruise_control.valid_estimated_vel_max double Maximum value of valid speed estimation results in speed estimation using a point cloud [m/s]
adaptive_cruise_control.valid_estimated_vel_min double Minimum value of valid speed estimation results in speed estimation using a point cloud [m/s]
adaptive_cruise_control.thresh_vel_to_stop double Embed a stop line if the maximum speed calculated by ACC is lower than this speed [m/s]
adaptive_cruise_control.lowpass_gain_of_upper_velocity double Lowpass-gain of target velocity
adaptive_cruise_control.use_rough_velocity_estimation: bool Use rough estimated velocity if the velocity estimation is failed (valid only if osp.use_predicted_objects false)
adaptive_cruise_control.rough_velocity_rate double In the rough velocity estimation, the velocity of front car is estimated as self current velocity * this value

Flowchart

@startuml
title insertTargetVelocity()
start

:get target vehicle point (*1) in detection area ;

partition Estimate-Target-Velocity {

if (Is there a DynamicObject on the target vehicle point?) then (yes)
:use the DynamicObject velocity\nas a target vehicle point velocity (*2);
else (no)

  if (The target vehicle point is found in the previous step?) then (yes)
  else (no (estimation failed))
  stop
  endif

  :estimate the target vehicle point velocity \nby the travel distance from the previous step;

  if (The estimated velocity is valid?) then (yes)
  else (no (estimation failed))
  stop
  endif

  :use the estimated velocity\nas a target vehicle point velocity (*2);

endif
}

if (Is the target vehicle point velocity fast enough?) then (yes)
else (no)
  stop
endif

if (calculate distance to the pointcloud from self-position?) then (yes)
else (no)
  stop
endif

:calculate target velocity to be inserted in the trajectory;

if (the target velocity is fast enough?) then (yes)
else (no)
  stop
endif

:insert the target velocity;

stop
@enduml

(*1) The target vehicle point is calculated as a closest obstacle PointCloud from ego along the trajectory.

(*2) The sources of velocity estimation can be changed by the following ROS parameters.

  • adaptive_cruise_control.use_object_to_estimate_vel
  • adaptive_cruise_control.use_pcl_to_estimate_vel

This module works only when the target point is found in the detection area of the Obstacle stop planner module.

The first process of this module is to estimate the velocity of the target vehicle point. The velocity estimation uses the velocity information of dynamic objects or the travel distance of the target vehicle point from the previous step. The dynamic object information is primal, and the travel distance estimation is used as a backup in case of the perception failure. If the target vehicle point is contained in the bounding box of a dynamic object geometrically, the velocity of the dynamic object is used as the target point velocity. Otherwise, the target point velocity is calculated by the travel distance of the target point from the previous step; that is (current_position - previous_position) / dt. Note that this travel distance based estimation fails when the target point is detected in the first time (it mainly happens in the cut-in situation). To improve the stability of the estimation, the median of the calculation result for several steps is used.

If the calculated velocity is within the threshold range, it is used as the target point velocity.

Only when the estimation is succeeded and the estimated velocity exceeds the value of obstacle_stop_velocity_thresh_*, the distance to the pointcloud from self-position is calculated. For prevent chattering in the mode transition, obstacle_velocity_thresh_to_start_acc is used for the threshold to start adaptive cruise, and obstacle_velocity_thresh_to_stop_acc is used for the threshold to stop adaptive cruise. When the calculated distance value exceeds the emergency distance $d_{emergency}$ calculated by emergency_stop parameters, target velocity to insert is calculated.

The emergency distance $d_{emergency}$ is calculated as follows.

$d_{emergency} = d_{margin_{emergency}} + t_{idling_{emergency}} \cdot v_{ego} + (-\frac{v_{ego}^2}{2 \cdot a_{ego_ {emergency}}}) - (-\frac{v_{obj}^2}{2 \cdot a_{obj_{emergency}}})$

  • $d_{margin_{emergency}}$ is a minimum margin to the obstacle pointcloud. The value of $d_{margin_{emergency}}$ depends on the parameter min_dist_stop
  • $t_{idling_{emergency}}$ is a supposed idling time. The value of $t_{idling_{emergency}}$ depends on the parameter emergency_stop_idling_time
  • $v_{ego}$ is a current velocity of own vehicle
  • $a_{ego_{{emergency}}}$ is a minimum acceleration (maximum deceleration) of own vehicle. The value of $a{ego_{_ {emergency}}}$ depends on the parameter emergency_stop_acceleration
  • $v_{obj}$ is a current velocity of obstacle pointcloud.
  • $a_{obj_{{emergency}}}$ is a supposed minimum acceleration of obstacle pointcloud. The value of $a{obj_{_ {emergency}}}$ depends on the parameter obstacle_emergency_stop_acceleration
  • *Above $X_{_{emergency}}$ parameters are used only in emergency situation.

The target velocity is determined to keep the distance to the obstacle pointcloud from own vehicle at the standard distance $d_{standard}$ calculated as following. Therefore, if the distance to the obstacle pointcloud is longer than standard distance, The target velocity becomes higher than the current velocity, and vice versa. For keeping the distance, a PID controller is used.

$d_{standard} = d_{margin_{standard}} + t_{idling_{standard}} \cdot v_{ego} + (-\frac{v_{ego}^2}{2 \cdot a_{ego_ {standard}}}) - (-\frac{v_{obj}^2}{2 \cdot a_{obj_{standard}}})$

  • $d_{margin_{standard}}$ is a minimum margin to the obstacle pointcloud. The value of $d_{margin_{standard}}$ depends on the parameter min_dist_stop
  • $t_{idling_{standard}}$ is a supposed idling time. The value of $t_{idling_{standard}}$ depends on the parameter standard_stop_idling_time
  • $v_{ego}$ is a current velocity of own vehicle
  • $a_{ego_{{standard}}}$ is a minimum acceleration (maximum deceleration) of own vehicle. The value of $a{ego_{_ {standard}}}$ depends on the parameter min_standard_acceleration
  • $v_{obj}$ is a current velocity of obstacle pointcloud.
  • $a_{obj_{{standard}}}$ is a supposed minimum acceleration of obstacle pointcloud. The value of $a{obj_{_ {standard}}}$ depends on the parameter obstacle_min_standard_acceleration
  • *Above $X_{_{standard}}$ parameters are used only in non-emergency situation.

adaptive_cruise

If the target velocity exceeds the value of thresh_vel_to_stop, the target velocity is embedded in the trajectory.

Known Limits

  • It is strongly depends on velocity planning module whether or not it moves according to the target speed embedded by Adaptive Cruise Controller module. If the velocity planning module is updated, please take care of the vehicle’s behavior as much as possible and always be ready for overriding.

  • The velocity estimation algorithm in Adaptive Cruise Controller is depend on object tracking module. Please note that if the object-tracking fails or the tracking result is incorrect, it the possibility that the vehicle behaves dangerously.

  • It does not work for backward driving, but publishes the path of the input as it is. Please use obstacle_cruise_planner if you want to stop against an obstacle when backward driving.

CHANGELOG
No CHANGELOG found.

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_stop_planner.launch.xml
      • common_param_path [default: $(find-pkg-share obstacle_stop_planner)/config/common.param.yaml]
      • adaptive_cruise_control_param_path [default: $(find-pkg-share obstacle_stop_planner)/config/adaptive_cruise_control.param.yaml]
      • obstacle_stop_planner_param_path [default: $(find-pkg-share obstacle_stop_planner)/config/obstacle_stop_planner.param.yaml]
      • vehicle_info_param_file [default: $(find-pkg-share vehicle_info_util)/config/vehicle_info.param.yaml]
      • enable_slow_down [default: false]
      • input_objects [default: /perception/object_recognition/objects]
      • input_pointcloud [default: input/pointcloud]
      • input_trajectory [default: input/trajectory]
      • input_odometry [default: /localization/kinematic_state]
      • output_trajectory [default: output/trajectory]
      • output_velocity_limit [default: /planning/scenario_planning/max_velocity_candidates]
      • output_velocity_limit_clear_command [default: /planning/scenario_planning/clear_velocity_limit]

Messages

No message files found.

Services

No service files found

Plugins

No plugins found.

Recent questions tagged obstacle_stop_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.