Skip to content

Surround Obstacle Checker#

Purpose#

This module subscribes required data (ego-pose, obstacles, etc), and publishes zero velocity limit to keep stopping if any of stop conditions are satisfied.

Inner-workings / Algorithms#

Flow chart#

uml diagram

Algorithms#

Check data#

Check that surround_obstacle_checker receives no ground pointcloud, dynamic objects and current velocity data.

Get distance to nearest object#

Calculate distance between ego vehicle and the nearest object. In this function, it calculates the minimum distance between the polygon of ego vehicle and all points in pointclouds and the polygons of dynamic objects.

Stop requirement#

If it satisfies all following conditions, it plans stopping.

  • Ego vehicle is stopped
  • It satisfies any following conditions
    1. The distance to nearest obstacle satisfies following conditions
      • If state is State::PASS, the distance is less than surround_check_distance
      • If state is State::STOP, the distance is less than surround_check_recover_distance
    2. If it does not satisfies the condition in 1, elapsed time from the time it satisfies the condition in 1 is less than state_clear_time

States#

To prevent chattering, surround_obstacle_checker manages two states. As mentioned in stop condition section, it prevents chattering by changing threshold to find surround obstacle depending on the states.

  • State::PASS : Stop planning is released
  • State::STOP :While stop planning

Inputs / Outputs#

Input#

Name Type Description
/perception/obstacle_segmentation/pointcloud sensor_msgs::msg::PointCloud2 Pointcloud of obstacles which the ego-vehicle should stop or avoid
/perception/object_recognition/objects autoware_auto_perception_msgs::msg::PredictedObjects Dynamic objects
/localization/kinematic_state nav_msgs::msg::Odometry Current twist
/tf tf2_msgs::msg::TFMessage TF
/tf_static tf2_msgs::msg::TFMessage TF static

Output#

Name Type Description
~/output/velocity_limit_clear_command tier4_planning_msgs::msg::VelocityLimitClearCommand Velocity limit clear command
~/output/max_velocity tier4_planning_msgs::msg::VelocityLimit Velocity limit command
~/output/no_start_reason diagnostic_msgs::msg::DiagnosticStatus No start reason
~/output/stop_reasons tier4_planning_msgs::msg::StopReasonArray Stop reasons
~/debug/marker visualization_msgs::msg::MarkerArray Marker for visualization
~/debug/footprint geometry_msgs::msg::PolygonStamped Ego vehicle base footprint for visualization
~/debug/footprint_offset geometry_msgs::msg::PolygonStamped Ego vehicle footprint with surround_check_distance offset for visualization
~/debug/footprint_recover_offset geometry_msgs::msg::PolygonStamped Ego vehicle footprint with surround_check_recover_distance offset for visualization

Parameters#

Name Type Description Default Range
pointcloud.enable_check boolean enable to check surrounding pointcloud false N/A
pointcloud.surround_check_front_distance float If objects exist in this distance, transit to "exist-surrounding-obstacle" status. [m] 0.5 ≥0.0
pointcloud.surround_check_side_distance float If objects exist in this distance, transit to "exist-surrounding-obstacle" status. [m] 0.5 ≥0.0
pointcloud.surround_check_back_distance float If objects exist in this distance, transit to "exist-surrounding-obstacle" status. [m] 0.5 ≥0.0
unknown.enable_check boolean enable to check surrounding unknown objects true N/A
unknown.surround_check_front_distance float If objects exist in this distance, transit to "exist-surrounding-obstacle" status. [m] 0.5 ≥0.0
unknown.surround_check_side_distance float If objects exist in this distance, transit to "exist-surrounding-obstacle" status. [m] 0.5 ≥0.0
unknown.surround_check_back_distance float If objects exist in this distance, transit to "exist-surrounding-obstacle" status. [m] 0.5 ≥0.0
car.enable_check boolean enable to check surrounding car true N/A
car.surround_check_front_distance float If objects exist in this distance, transit to "exist-surrounding-obstacle" status. [m] 0.5 ≥0.0
car.surround_check_side_distance float If objects exist in this distance, transit to "exist-surrounding-obstacle" status. [m] 0.5 ≥0.0
car.surround_check_back_distance float If objects exist in this distance, transit to "exist-surrounding-obstacle" status. [m] 0.5 ≥0.0
truck.enable_check boolean enable to check surrounding truck true N/A
truck.surround_check_front_distance float If objects exist in this distance, transit to "exist-surrounding-obstacle" status. [m] 0.5 ≥0.0
truck.surround_check_side_distance float If objects exist in this distance, transit to "exist-surrounding-obstacle" status. [m] 0.5 ≥0.0
truck.surround_check_back_distance float If objects exist in this distance, transit to "exist-surrounding-obstacle" status. [m] 0.5 ≥0.0
bus.enable_check boolean enable to check surrounding bus true N/A
bus.surround_check_front_distance float If objects exist in this distance, transit to "exist-surrounding-obstacle" status. [m] 0.5 ≥0.0
bus.surround_check_side_distance float If objects exist in this distance, transit to "exist-surrounding-obstacle" status. [m] 0.5 ≥0.0
bus.surround_check_back_distance float If objects exist in this distance, transit to "exist-surrounding-obstacle" status. [m] 0.5 ≥0.0
trailer.enable_check boolean enable to check surrounding trailer true N/A
trailer.surround_check_front_distance float If objects exist in this distance, transit to "exist-surrounding-obstacle" status. [m] 0.5 ≥0.0
trailer.surround_check_side_distance float If objects exist in this distance, transit to "exist-surrounding-obstacle" status. [m] 0.5 ≥0.0
trailer.surround_check_back_distance float If objects exist in this distance, transit to "exist-surrounding-obstacle" status. [m] 0.5 ≥0.0
motorcycle.enable_check boolean enable to check surrounding motorcycle true N/A
motorcycle.surround_check_front_distance float If objects exist in this distance, transit to "exist-surrounding-obstacle" status. [m] 0.5 ≥0.0
motorcycle.surround_check_side_distance float If objects exist in this distance, transit to "exist-surrounding-obstacle" status. [m] 0.5 ≥0.0
motorcycle.surround_check_back_distance float If objects exist in this distance, transit to "exist-surrounding-obstacle" status. [m] 0.5 ≥0.0
bicycle.enable_check boolean enable to check surrounding bicycle true N/A
bicycle.surround_check_front_distance float If objects exist in this distance, transit to "exist-surrounding-obstacle" status. [m] 0.5 ≥0.0
bicycle.surround_check_side_distance float f objects exist in this distance, transit to "exist-surrounding-obstacle" status. [m] 0.5 ≥0.0
bicycle.surround_check_back_distance float If objects exist in this distance, transit to "exist-surrounding-obstacle" status. [m] 0.5 ≥0.0
pedestrian.enable_check boolean enable to check surrounding pedestrian true N/A
pedestrian.surround_check_front_distance float If objects exist in this distance, transit to "exist-surrounding-obstacle" status. [m] 0.5 ≥0.0
pedestrian.surround_check_side_distance float If objects exist in this distance, transit to "exist-surrounding-obstacle" status. [m] 0.5 ≥0.0
pedestrian.surround_check_back_distance float If objects exist in this distance, transit to "exist-surrounding-obstacle" status. [m] 0.5 ≥0.0
surround_check_hysteresis_distance float If no object exists in this hysteresis distance added to the above distance, transit to "non-surrounding-obstacle" status [m] 0.3 ≥0.0
state_clear_time float Threshold to clear stop state [s] 2.0 ≥0.0
stop_state_ego_speed float Threshold to check ego vehicle stopped [m/s] 0.1 ≥0.0
publish_debug_footprints boolean Publish vehicle footprint & footprints with surround_check_distance and surround_check_recover_distance offsets. true N/A
debug_footprint_label string select the label for debug footprint car ['pointcloud', 'unknown', 'car', 'truck', 'bus', 'trailer', 'motorcycle', 'bicycle', 'pedestrian']
Name Type Description Default value
enable_check bool Indicates whether each object is considered in the obstacle check target. true for objects; false for point clouds
surround_check_front_distance bool If there are objects or point clouds within this distance in front, transition to the "exist-surrounding-obstacle" status [m]. 0.5
surround_check_side_distance double If there are objects or point clouds within this side distance, transition to the "exist-surrounding-obstacle" status [m]. 0.5
surround_check_back_distance double If there are objects or point clouds within this back distance, transition to the "exist-surrounding-obstacle" status [m]. 0.5
surround_check_hysteresis_distance double If no object exists within surround_check_xxx_distance plus this additional distance, transition to the "non-surrounding-obstacle" status [m]. 0.3
state_clear_time double Threshold to clear stop state [s] 2.0
stop_state_ego_speed double Threshold to check ego vehicle stopped [m/s] 0.1
stop_state_entry_duration_time double Threshold to check ego vehicle stopped [s] 0.1
publish_debug_footprints bool Publish vehicle footprint with/without offsets true

Assumptions / Known limits#

To perform stop planning, it is necessary to get obstacle pointclouds data. Hence, it does not plan stopping if the obstacle is in blind spot.