diff --git a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml index 942f4e8a5c2fd..ac0426073c9d0 100644 --- a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml +++ b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml @@ -54,6 +54,7 @@ maximum_peeking_distance: 6.0 # [m] attention_lane_crop_curvature_threshold: 0.25 attention_lane_curvature_calculation_ds: 0.5 + static_occlusion_with_traffic_light_timeout: 3.5 enable_rtc: intersection: true # If set to true, the scene modules require approval from the rtc (request to cooperate) function. If set to false, the modules can be executed without requiring rtc approval diff --git a/planning/behavior_velocity_intersection_module/src/debug.cpp b/planning/behavior_velocity_intersection_module/src/debug.cpp index d0bda3447eec4..625be513e3cad 100644 --- a/planning/behavior_velocity_intersection_module/src/debug.cpp +++ b/planning/behavior_velocity_intersection_module/src/debug.cpp @@ -179,14 +179,6 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( &debug_marker_array); } - if (debug_data_.intersection_area) { - appendMarkerArray( - debug::createPolygonMarkerArray( - debug_data_.intersection_area.value(), "intersection_area", lane_id_, now, 0.3, 0.0, 0.0, - 0.0, 1.0, 0.0), - &debug_marker_array); - } - if (debug_data_.stuck_vehicle_detect_area) { appendMarkerArray( debug::createPolygonMarkerArray( @@ -224,6 +216,11 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( debug_data_.conflicting_targets, "conflicting_targets", module_id_, now, 0.99, 0.4, 0.0), &debug_marker_array, now); + appendMarkerArray( + debug::createObjectsMarkerArray( + debug_data_.amber_ignore_targets, "amber_ignore_targets", module_id_, now, 0.0, 1.0, 0.0), + &debug_marker_array, now); + appendMarkerArray( debug::createObjectsMarkerArray( debug_data_.stuck_targets, "stuck_targets", module_id_, now, 0.99, 0.99, 0.2), @@ -290,6 +287,12 @@ motion_utils::VirtualWalls IntersectionModule::createVirtualWalls() if (debug_data_.occlusion_stop_wall_pose) { wall.style = motion_utils::VirtualWallType::stop; wall.text = "intersection_occlusion"; + if (debug_data_.static_occlusion_with_traffic_light_timeout) { + std::stringstream timeout; + timeout << std::setprecision(2) + << debug_data_.static_occlusion_with_traffic_light_timeout.value(); + wall.text += "(" + timeout.str() + ")"; + } wall.ns = "intersection_occlusion" + std::to_string(module_id_) + "_"; wall.pose = debug_data_.occlusion_stop_wall_pose.value(); virtual_walls.push_back(wall); diff --git a/planning/behavior_velocity_intersection_module/src/manager.cpp b/planning/behavior_velocity_intersection_module/src/manager.cpp index 4aabe66c79dda..4fae43c2cc3fc 100644 --- a/planning/behavior_velocity_intersection_module/src/manager.cpp +++ b/planning/behavior_velocity_intersection_module/src/manager.cpp @@ -125,6 +125,8 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) node.declare_parameter(ns + ".occlusion.attention_lane_crop_curvature_threshold"); ip.occlusion.attention_lane_curvature_calculation_ds = node.declare_parameter(ns + ".occlusion.attention_lane_curvature_calculation_ds"); + ip.occlusion.static_occlusion_with_traffic_light_timeout = + node.declare_parameter(ns + ".occlusion.static_occlusion_with_traffic_light_timeout"); } void IntersectionModuleManager::launchNewModules( diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index f230edc7d46e1..42619d8e3d6a7 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -30,10 +30,12 @@ #include #include +#include #include #include #include +#include #include #include #include @@ -101,6 +103,11 @@ IntersectionModule::IntersectionModule( planner_param_.occlusion.before_creep_stop_time); temporal_stop_before_attention_state_machine_.setState(StateMachine::State::STOP); } + { + static_occlusion_timeout_state_machine_.setMarginTime( + planner_param_.occlusion.static_occlusion_with_traffic_light_timeout); + static_occlusion_timeout_state_machine_.setState(StateMachine::State::STOP); + } decision_state_pub_ = node_.create_publisher("~/debug/intersection/decision_state", 1); @@ -514,6 +521,8 @@ void reactRTCApprovalByDecisionResult( planning_utils::setVelocityFromIndex(occlusion_peeking_stop_line, 0.0, path); debug_data->occlusion_stop_wall_pose = planning_utils::getAheadPose(occlusion_peeking_stop_line, baselink2front, *path); + debug_data->static_occlusion_with_traffic_light_timeout = + decision_result.static_occlusion_timeout; { tier4_planning_msgs::msg::StopFactor stop_factor; stop_factor.stop_pose = path->points.at(occlusion_peeking_stop_line).point.pose; @@ -573,6 +582,8 @@ void reactRTCApprovalByDecisionResult( planning_utils::setVelocityFromIndex(stop_line_idx, 0.0, path); debug_data->occlusion_stop_wall_pose = planning_utils::getAheadPose(stop_line_idx, baselink2front, *path); + debug_data->static_occlusion_with_traffic_light_timeout = + decision_result.static_occlusion_timeout; { tier4_planning_msgs::msg::StopFactor stop_factor; stop_factor.stop_pose = path->points.at(stop_line_idx).point.pose; @@ -838,10 +849,14 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( planner_param_.occlusion.occlusion_attention_area_length, planner_param_.common.consider_wrong_direction_vehicle); } - const auto & intersection_lanelets = intersection_lanelets_.value(); + auto & intersection_lanelets = intersection_lanelets_.value(); + + // at the very first time of registration of this module, the path may not be conflicting with the + // attention area, so update() is called to update the internal data as well as traffic light info const bool tl_arrow_solid_on = util::isTrafficLightArrowActivated(assigned_lanelet, planner_data_->traffic_light_id_map); - intersection_lanelets_.value().update(tl_arrow_solid_on, interpolated_path_info); + const auto footprint = planner_data_->vehicle_info_.createFootprint(0.0, 0.0); + intersection_lanelets.update(tl_arrow_solid_on, interpolated_path_info, footprint); // this is abnormal const auto & conflicting_lanelets = intersection_lanelets.conflicting(); @@ -903,6 +918,19 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( } return state_machine.getState() == StateMachine::State::GO; }; + auto stoppedAtPosition = [&](const size_t pos, const double duration) { + const double dist_stopline = fromEgoDist(pos); + const bool approached_dist_stopline = + (std::fabs(dist_stopline) < planner_param_.common.stop_overshoot_margin); + const bool over_stopline = (dist_stopline < -planner_param_.common.stop_overshoot_margin); + const bool is_stopped = planner_data_->isVehicleStopped(duration); + if (over_stopline) { + return true; + } else if (is_stopped && approached_dist_stopline) { + return true; + } + return false; + }; // stuck vehicle detection is viable even if attention area is empty // so this needs to be checked before attention area validation @@ -972,7 +1000,6 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( const auto first_attention_stop_line_idx = first_attention_stop_line_idx_opt.value(); const auto occlusion_stop_line_idx = occlusion_peeking_stop_line_idx_opt.value(); - const auto & attention_lanelets = intersection_lanelets.attention(); const auto & adjacent_lanelets = intersection_lanelets.adjacent(); const auto & occlusion_attention_lanelets = intersection_lanelets.occlusion_attention(); const auto & occlusion_attention_area = intersection_lanelets.occlusion_attention_area(); @@ -981,24 +1008,18 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( debug_data_.adjacent_area = intersection_lanelets.adjacent_area(); // get intersection area - const auto intersection_area = planner_param_.common.use_intersection_area - ? util::getIntersectionArea(assigned_lanelet, lanelet_map_ptr) - : std::nullopt; - if (intersection_area) { - const auto intersection_area_2d = intersection_area.value(); - debug_data_.intersection_area = toGeomPoly(intersection_area_2d); - } + const auto intersection_area = util::getIntersectionArea(assigned_lanelet, lanelet_map_ptr); + + auto target_objects = generateTargetObjects(intersection_lanelets, intersection_area); // calculate dynamic collision around attention area const double time_to_restart = (is_go_out_ || tl_arrow_solid_on) ? 0.0 : (planner_param_.collision_detection.state_transit_margin_time - collision_state_machine_.getDuration()); - const auto target_objects = - filterTargetObjects(attention_lanelets, adjacent_lanelets, intersection_area); const bool has_collision = checkCollision( - *path, target_objects, path_lanelets, closest_idx, time_to_restart, tl_arrow_solid_on); + *path, &target_objects, path_lanelets, closest_idx, time_to_restart, tl_arrow_solid_on); collision_state_machine_.setStateWithMarginTime( has_collision ? StateMachine::State::STOP : StateMachine::State::GO, logger_.get_child("collision state_machine"), *clock_); @@ -1023,30 +1044,30 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( const double occlusion_dist_thr = std::fabs( std::pow(planner_param_.occlusion.max_vehicle_velocity_for_rss, 2) / (2 * planner_param_.occlusion.min_vehicle_brake_for_rss)); - std::vector blocking_attention_objects; - std::copy_if( - target_objects.objects.begin(), target_objects.objects.end(), - std::back_inserter(blocking_attention_objects), - [thresh = planner_param_.occlusion.ignore_parked_vehicle_speed_threshold](const auto & object) { - return std::fabs(object.kinematics.initial_twist_with_covariance.twist.linear.x) <= thresh; - }); - debug_data_.blocking_attention_objects.objects = blocking_attention_objects; - const bool is_occlusion_cleared = + auto occlusion_status = (enable_occlusion_detection_ && !occlusion_attention_lanelets.empty() && !tl_arrow_solid_on) - ? isOcclusionCleared( + ? getOcclusionStatus( *planner_data_->occupancy_grid, occlusion_attention_area, adjacent_lanelets, first_attention_area, interpolated_path_info, occlusion_attention_divisions, - blocking_attention_objects, occlusion_dist_thr) - : true; + target_objects, current_pose, occlusion_dist_thr) + : OcclusionType::NOT_OCCLUDED; occlusion_stop_state_machine_.setStateWithMarginTime( - is_occlusion_cleared ? StateMachine::State::GO : StateMachine::STOP, + occlusion_status == OcclusionType::NOT_OCCLUDED ? StateMachine::State::GO : StateMachine::STOP, logger_.get_child("occlusion_stop"), *clock_); const bool is_occlusion_cleared_with_margin = (occlusion_stop_state_machine_.getState() == StateMachine::State::GO); - // distinguish if ego detected occlusion or RTC detects occlusion const bool ext_occlusion_requested = (is_occlusion_cleared_with_margin && !occlusion_activated_); + if (ext_occlusion_requested) { + occlusion_status = OcclusionType::RTC_OCCLUDED; + } const bool is_occlusion_state = (!is_occlusion_cleared_with_margin || ext_occlusion_requested); + if (is_occlusion_state && occlusion_status == OcclusionType::NOT_OCCLUDED) { + occlusion_status = prev_occlusion_status_; + } else { + prev_occlusion_status_ = occlusion_status; + } + // Safe if (!is_occlusion_state && !has_collision_with_margin) { return IntersectionModule::Safe{closest_idx, collision_stop_line_idx, occlusion_stop_line_idx}; @@ -1057,11 +1078,13 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( closest_idx, collision_stop_line_idx, occlusion_stop_line_idx}; } // Occluded + // occlusion_status is assured to be not NOT_OCCLUDED const bool stopped_at_default_line = stoppedForDuration( default_stop_line_idx, planner_param_.occlusion.before_creep_stop_time, before_creep_state_machine_); if (stopped_at_default_line) { - // in this case ego will temporarily stop before entering attention area + // if specified the parameter occlusion.temporal_stop_before_attention_area OR + // has_no_traffic_light_, ego will temporarily stop before entering attention area const bool temporal_stop_before_attention_required = (planner_param_.occlusion.temporal_stop_before_attention_area || !has_traffic_light_) ? !stoppedForDuration( @@ -1078,20 +1101,51 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( temporal_stop_before_attention_required, closest_idx, first_attention_stop_line_idx, occlusion_stop_line_idx}; } + // following remaining block is "has_traffic_light_" + // if ego is stuck by static occlusion in the presence of traffic light, start timeout count + const bool is_static_occlusion = occlusion_status == OcclusionType::STATICALLY_OCCLUDED; + const bool is_stuck_by_static_occlusion = + stoppedAtPosition(occlusion_stop_line_idx, planner_param_.occlusion.before_creep_stop_time) && + is_static_occlusion; + if (has_collision_with_margin) { + // if collision is detected, timeout is reset + static_occlusion_timeout_state_machine_.setState(StateMachine::State::STOP); + } else if (is_stuck_by_static_occlusion) { + static_occlusion_timeout_state_machine_.setStateWithMarginTime( + StateMachine::State::GO, logger_.get_child("static_occlusion"), *clock_); + } + const bool release_static_occlusion_stuck = + (static_occlusion_timeout_state_machine_.getState() == StateMachine::State::GO); + if (!has_collision_with_margin && release_static_occlusion_stuck) { + return IntersectionModule::Safe{ + closest_idx, collision_stop_line_idx, occlusion_stop_line_idx}; + } + // occlusion_status is either STATICALLY_OCCLUDED or DYNAMICALLY_OCCLUDED + const double max_timeout = + planner_param_.occlusion.static_occlusion_with_traffic_light_timeout + + planner_param_.occlusion.stop_release_margin_time; + const std::optional static_occlusion_timeout = + is_stuck_by_static_occlusion + ? std::make_optional( + max_timeout - static_occlusion_timeout_state_machine_.getDuration() - + occlusion_stop_state_machine_.getDuration()) + : (is_static_occlusion ? std::make_optional(max_timeout) : std::nullopt); if (has_collision_with_margin) { return IntersectionModule::OccludedCollisionStop{is_occlusion_cleared_with_margin, temporal_stop_before_attention_required, closest_idx, collision_stop_line_idx, first_attention_stop_line_idx, - occlusion_stop_line_idx}; + occlusion_stop_line_idx, + static_occlusion_timeout}; } else { return IntersectionModule::PeekingTowardOcclusion{is_occlusion_cleared_with_margin, temporal_stop_before_attention_required, closest_idx, collision_stop_line_idx, first_attention_stop_line_idx, - occlusion_stop_line_idx}; + occlusion_stop_line_idx, + static_occlusion_timeout}; } } else { const auto occlusion_stop_line = @@ -1118,18 +1172,17 @@ bool IntersectionModule::checkStuckVehicle( &debug_data_); } -autoware_auto_perception_msgs::msg::PredictedObjects IntersectionModule::filterTargetObjects( - const lanelet::ConstLanelets & attention_area_lanelets, - const lanelet::ConstLanelets & adjacent_lanelets, +util::TargetObjects IntersectionModule::generateTargetObjects( + const util::IntersectionLanelets & intersection_lanelets, const std::optional & intersection_area) const { - using lanelet::utils::getArcCoordinates; - using lanelet::utils::getPolygonFromArcLength; - const auto & objects_ptr = planner_data_->predicted_objects; // extract target objects - autoware_auto_perception_msgs::msg::PredictedObjects target_objects; + util::TargetObjects target_objects; target_objects.header = objects_ptr->header; + const auto & attention_lanelets = intersection_lanelets.attention(); + const auto & attention_lanelet_stoplines = intersection_lanelets.attention_stop_lines(); + const auto & adjacent_lanelets = intersection_lanelets.adjacent(); for (const auto & object : objects_ptr->objects) { // ignore non-vehicle type objects, such as pedestrian. if (!isTargetCollisionVehicleType(object)) { @@ -1138,48 +1191,72 @@ autoware_auto_perception_msgs::msg::PredictedObjects IntersectionModule::filterT // check direction of objects const auto object_direction = util::getObjectPoseWithVelocityDirection(object.kinematics); - const auto is_in_adjacent_lanelets = util::checkAngleForTargetLanelets( - object_direction, object.kinematics.initial_twist_with_covariance.twist.linear.x, - adjacent_lanelets, planner_param_.common.attention_area_angle_thr, + const auto belong_adjacent_lanelet_id = util::checkAngleForTargetLanelets( + object_direction, adjacent_lanelets, planner_param_.common.attention_area_angle_thr, planner_param_.common.consider_wrong_direction_vehicle, - planner_param_.common.attention_area_margin, - planner_param_.occlusion.ignore_parked_vehicle_speed_threshold); - if (is_in_adjacent_lanelets) { + planner_param_.common.attention_area_margin, false); + if (belong_adjacent_lanelet_id) { continue; } + const auto is_parked_vehicle = + std::fabs(object.kinematics.initial_twist_with_covariance.twist.linear.x) < + planner_param_.occlusion.ignore_parked_vehicle_speed_threshold; + auto & container = is_parked_vehicle ? target_objects.parked_attention_objects + : target_objects.attention_objects; if (intersection_area) { + const auto & obj_pos = object.kinematics.initial_pose_with_covariance.pose.position; const auto obj_poly = tier4_autoware_utils::toPolygon2d(object); const auto intersection_area_2d = intersection_area.value(); - const auto is_in_intersection_area = bg::within(obj_poly, intersection_area_2d); - if (is_in_intersection_area) { - target_objects.objects.push_back(object); - } else if (util::checkAngleForTargetLanelets( - object_direction, object.kinematics.initial_twist_with_covariance.twist.linear.x, - attention_area_lanelets, planner_param_.common.attention_area_angle_thr, - planner_param_.common.consider_wrong_direction_vehicle, - planner_param_.common.attention_area_margin, - planner_param_.occlusion.ignore_parked_vehicle_speed_threshold)) { - target_objects.objects.push_back(object); + const auto belong_attention_lanelet_id = util::checkAngleForTargetLanelets( + object_direction, attention_lanelets, planner_param_.common.attention_area_angle_thr, + planner_param_.common.consider_wrong_direction_vehicle, + planner_param_.common.attention_area_margin, is_parked_vehicle); + if (belong_attention_lanelet_id) { + const auto id = belong_attention_lanelet_id.value(); + util::TargetObject target_object; + target_object.object = object; + target_object.attention_lanelet = attention_lanelets.at(id); + target_object.stop_line = attention_lanelet_stoplines.at(id); + container.push_back(target_object); + } else if (bg::within(Point2d{obj_pos.x, obj_pos.y}, intersection_area_2d)) { + util::TargetObject target_object; + target_object.object = object; + target_object.attention_lanelet = std::nullopt; + target_object.stop_line = std::nullopt; + target_objects.intersection_area_objects.push_back(target_object); } - } else if (util::checkAngleForTargetLanelets( - object_direction, object.kinematics.initial_twist_with_covariance.twist.linear.x, - attention_area_lanelets, planner_param_.common.attention_area_angle_thr, + } else if (const auto belong_attention_lanelet_id = util::checkAngleForTargetLanelets( + object_direction, attention_lanelets, + planner_param_.common.attention_area_angle_thr, planner_param_.common.consider_wrong_direction_vehicle, - planner_param_.common.attention_area_margin, - planner_param_.occlusion.ignore_parked_vehicle_speed_threshold)) { + planner_param_.common.attention_area_margin, is_parked_vehicle); + belong_attention_lanelet_id.has_value()) { // intersection_area is not available, use detection_area_with_margin as before - target_objects.objects.push_back(object); + const auto id = belong_attention_lanelet_id.value(); + util::TargetObject target_object; + target_object.object = object; + target_object.attention_lanelet = attention_lanelets.at(id); + target_object.stop_line = attention_lanelet_stoplines.at(id); + container.push_back(target_object); } } + for (const auto & object : target_objects.attention_objects) { + target_objects.all_attention_objects.push_back(object); + } + for (const auto & object : target_objects.parked_attention_objects) { + target_objects.all_attention_objects.push_back(object); + } + for (auto & object : target_objects.all_attention_objects) { + object.calc_dist_to_stop_line(); + } return target_objects; } bool IntersectionModule::checkCollision( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const autoware_auto_perception_msgs::msg::PredictedObjects & objects, - const util::PathLanelets & path_lanelets, const int closest_idx, const double time_delay, - const bool tl_arrow_solid_on) + util::TargetObjects * target_objects, const util::PathLanelets & path_lanelets, + const int closest_idx, const double time_delay, const bool tl_arrow_solid_on) { using lanelet::utils::getArcCoordinates; using lanelet::utils::getPolygonFromArcLength; @@ -1191,15 +1268,13 @@ bool IntersectionModule::checkCollision( planner_param_.common.intersection_velocity, planner_param_.collision_detection.minimum_ego_predicted_velocity); const double passing_time = time_distance_array.back().first; - auto target_objects = objects; - util::cutPredictPathWithDuration(&target_objects, clock_, passing_time); + util::cutPredictPathWithDuration(target_objects, clock_, passing_time); const auto & concat_lanelets = path_lanelets.all; const auto closest_arc_coords = getArcCoordinates( concat_lanelets, tier4_autoware_utils::getPose(path.points.at(closest_idx).point)); const auto & ego_lane = path_lanelets.ego_or_entry2exit; debug_data_.ego_lane = ego_lane.polygon3d(); - const auto ego_poly = ego_lane.polygon2d().basicPolygon(); // check collision between predicted_path and ego_area const double collision_start_margin_time = @@ -1209,7 +1284,8 @@ bool IntersectionModule::checkCollision( tl_arrow_solid_on ? planner_param_.collision_detection.relaxed.collision_end_margin_time : planner_param_.collision_detection.normal.collision_end_margin_time; bool collision_detected = false; - for (const auto & object : target_objects.objects) { + for (const auto & target_object : target_objects->all_attention_objects) { + const auto & object = target_object.object; for (const auto & predicted_path : object.kinematics.predicted_paths) { if ( predicted_path.confidence < @@ -1300,15 +1376,14 @@ bool IntersectionModule::checkCollision( return collision_detected; } -bool IntersectionModule::isOcclusionCleared( +IntersectionModule::OcclusionType IntersectionModule::getOcclusionStatus( const nav_msgs::msg::OccupancyGrid & occ_grid, const std::vector & attention_areas, const lanelet::ConstLanelets & adjacent_lanelets, const lanelet::CompoundPolygon3d & first_attention_area, const util::InterpolatedPathInfo & interpolated_path_info, - const std::vector & lane_divisions, - const std::vector & - blocking_attention_objects, + const std::vector & lane_divisions, + const util::TargetObjects & target_objects, const geometry_msgs::msg::Pose & current_pose, const double occlusion_dist_thr) { const auto & path_ip = interpolated_path_info.path; @@ -1317,7 +1392,7 @@ bool IntersectionModule::isOcclusionCleared( const auto first_attention_area_idx = util::getFirstPointInsidePolygon(path_ip, lane_interval_ip, first_attention_area); if (!first_attention_area_idx) { - return false; + return OcclusionType::NOT_OCCLUDED; } const auto first_inside_attention_idx_ip_opt = @@ -1432,29 +1507,30 @@ bool IntersectionModule::isOcclusionCleared( // re-use attention_mask attention_mask = cv::Mat(width, height, CV_8UC1, cv::Scalar(0)); // (3.1) draw all cells on attention_mask behind blocking vehicles as not occluded + const auto & blocking_attention_objects = target_objects.parked_attention_objects; + for (const auto & blocking_attention_object : blocking_attention_objects) { + debug_data_.blocking_attention_objects.objects.push_back(blocking_attention_object.object); + } std::vector> blocking_polygons; for (const auto & blocking_attention_object : blocking_attention_objects) { - const Polygon2d obj_poly = tier4_autoware_utils::toPolygon2d(blocking_attention_object); + const Polygon2d obj_poly = tier4_autoware_utils::toPolygon2d(blocking_attention_object.object); findCommonCvPolygons(obj_poly.outer(), blocking_polygons); } for (const auto & blocking_polygon : blocking_polygons) { cv::fillPoly(attention_mask, blocking_polygon, cv::Scalar(BLOCKED), cv::LINE_AA); } - for (const auto & lane_division : lane_divisions) { - const auto & divisions = lane_division.divisions; - for (const auto & division : divisions) { - bool blocking_vehicle_found = false; - for (const auto & point_it : division) { - const auto [valid, idx_x, idx_y] = coord2index(point_it.x(), point_it.y()); - if (!valid) continue; - if (blocking_vehicle_found) { - occlusion_mask.at(height - 1 - idx_y, idx_x) = 0; - continue; - } - if (attention_mask.at(height - 1 - idx_y, idx_x) == BLOCKED) { - blocking_vehicle_found = true; - occlusion_mask.at(height - 1 - idx_y, idx_x) = 0; - } + for (const auto & division : lane_divisions) { + bool blocking_vehicle_found = false; + for (const auto & point_it : division) { + const auto [valid, idx_x, idx_y] = coord2index(point_it.x(), point_it.y()); + if (!valid) continue; + if (blocking_vehicle_found) { + occlusion_mask.at(height - 1 - idx_y, idx_x) = 0; + continue; + } + if (attention_mask.at(height - 1 - idx_y, idx_x) == BLOCKED) { + blocking_vehicle_found = true; + occlusion_mask.at(height - 1 - idx_y, idx_x) = 0; } } } @@ -1527,95 +1603,105 @@ bool IntersectionModule::isOcclusionCleared( } } - auto findNearestPointToProjection = - [](lanelet::ConstLineString2d division, const Point2d & projection, const double dist_thresh) { - double min_dist = std::numeric_limits::infinity(); - auto nearest = division.end(); - for (auto it = division.begin(); it != division.end(); it++) { - const double dist = std::hypot(it->x() - projection.x(), it->y() - projection.y()); - if (dist < min_dist) { - min_dist = dist; - nearest = it; - } - if (dist < dist_thresh) { - break; - } + auto findNearestPointToProjection = []( + const lanelet::ConstLineString3d & division, + const Point2d & projection, const double dist_thresh) { + double min_dist = std::numeric_limits::infinity(); + auto nearest = division.end(); + for (auto it = division.begin(); it != division.end(); it++) { + const double dist = std::hypot(it->x() - projection.x(), it->y() - projection.y()); + if (dist < min_dist) { + min_dist = dist; + nearest = it; } - return nearest; - }; + if (dist < dist_thresh) { + break; + } + } + return nearest; + }; struct NearestOcclusionPoint { - int lane_id; - int64 division_index; - double dist; + int64 division_index{0}; + int64 point_index{0}; + double dist{0.0}; geometry_msgs::msg::Point point; geometry_msgs::msg::Point projection; } nearest_occlusion_point; double min_dist = std::numeric_limits::infinity(); - for (const auto & lane_division : lane_divisions) { - const auto & divisions = lane_division.divisions; - const auto lane_id = lane_division.lane_id; - for (unsigned division_index = 0; division_index < divisions.size(); ++division_index) { - const auto & division = divisions.at(division_index); - LineString2d division_linestring; - auto division_point_it = division.begin(); - division_linestring.emplace_back(division_point_it->x(), division_point_it->y()); - for (auto point_it = division.begin(); point_it != division.end(); point_it++) { - if ( - std::hypot( - point_it->x() - division_point_it->x(), point_it->y() - division_point_it->y()) < - 3.0 /* rough tick for computational cost */) { - continue; - } - division_linestring.emplace_back(point_it->x(), point_it->y()); - division_point_it = point_it; - } - - // find the intersection point of lane_line and path - std::vector intersection_points; - boost::geometry::intersection(division_linestring, path_linestring, intersection_points); - if (intersection_points.empty()) { + for (unsigned division_index = 0; division_index < lane_divisions.size(); ++division_index) { + const auto & division = lane_divisions.at(division_index); + LineString2d division_linestring; + auto division_point_it = division.begin(); + division_linestring.emplace_back(division_point_it->x(), division_point_it->y()); + for (auto point_it = division.begin(); point_it != division.end(); point_it++) { + if ( + std::hypot(point_it->x() - division_point_it->x(), point_it->y() - division_point_it->y()) < + 3.0 /* rough tick for computational cost */) { continue; } - const auto & projection_point = intersection_points.at(0); - const auto projection_it = - findNearestPointToProjection(division, projection_point, resolution); - if (projection_it == division.end()) { - continue; + division_linestring.emplace_back(point_it->x(), point_it->y()); + division_point_it = point_it; + } + + // find the intersection point of lane_line and path + std::vector intersection_points; + boost::geometry::intersection(division_linestring, path_linestring, intersection_points); + if (intersection_points.empty()) { + continue; + } + const auto & projection_point = intersection_points.at(0); + const auto projection_it = findNearestPointToProjection(division, projection_point, resolution); + if (projection_it == division.end()) { + continue; + } + double acc_dist = 0.0; + auto acc_dist_it = projection_it; + for (auto point_it = projection_it; point_it != division.end(); point_it++) { + const double dist = + std::hypot(point_it->x() - acc_dist_it->x(), point_it->y() - acc_dist_it->y()); + acc_dist += dist; + acc_dist_it = point_it; + const auto [valid, idx_x, idx_y] = coord2index(point_it->x(), point_it->y()); + if (!valid) continue; + const auto pixel = occlusion_mask.at(height - 1 - idx_y, idx_x); + if (pixel == BLOCKED) { + break; } - double acc_dist = 0.0; - auto acc_dist_it = projection_it; - for (auto point_it = projection_it; point_it != division.end(); point_it++) { - const double dist = - std::hypot(point_it->x() - acc_dist_it->x(), point_it->y() - acc_dist_it->y()); - acc_dist += dist; - acc_dist_it = point_it; - const auto [valid, idx_x, idx_y] = coord2index(point_it->x(), point_it->y()); - // TODO(Mamoru Sobue): add handling for blocking vehicles - if (!valid) continue; - const auto pixel = occlusion_mask.at(height - 1 - idx_y, idx_x); - if (pixel == BLOCKED) { - break; - } - if (pixel == OCCLUDED) { - if (acc_dist < min_dist) { - min_dist = acc_dist; - nearest_occlusion_point = { - lane_id, std::distance(division.begin(), point_it), acc_dist, - tier4_autoware_utils::createPoint(point_it->x(), point_it->y(), origin.z), - tier4_autoware_utils::createPoint(projection_it->x(), projection_it->y(), origin.z)}; - } + if (pixel == OCCLUDED) { + if (acc_dist < min_dist) { + min_dist = acc_dist; + nearest_occlusion_point = { + division_index, std::distance(division.begin(), point_it), acc_dist, + tier4_autoware_utils::createPoint(point_it->x(), point_it->y(), origin.z), + tier4_autoware_utils::createPoint(projection_it->x(), projection_it->y(), origin.z)}; } } } } if (min_dist == std::numeric_limits::infinity() || min_dist > occlusion_dist_thr) { - return true; + return OcclusionType::NOT_OCCLUDED; } + debug_data_.nearest_occlusion_projection = std::make_pair(nearest_occlusion_point.point, nearest_occlusion_point.projection); - return false; + LineString2d ego_occlusion_line; + ego_occlusion_line.emplace_back(current_pose.position.x, current_pose.position.y); + ego_occlusion_line.emplace_back(nearest_occlusion_point.point.x, nearest_occlusion_point.point.y); + for (const auto & attention_object : target_objects.all_attention_objects) { + const auto obj_poly = tier4_autoware_utils::toPolygon2d(attention_object.object); + if (bg::intersects(obj_poly, ego_occlusion_line)) { + return OcclusionType::DYNAMICALLY_OCCLUDED; + } + } + for (const auto & attention_object : target_objects.intersection_area_objects) { + const auto obj_poly = tier4_autoware_utils::toPolygon2d(attention_object.object); + if (bg::intersects(obj_poly, ego_occlusion_line)) { + return OcclusionType::DYNAMICALLY_OCCLUDED; + } + } + return OcclusionType::STATICALLY_OCCLUDED; } /* diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp index ac10f6b4cfd8e..7e08e29eadfac 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp @@ -115,9 +115,17 @@ class IntersectionModule : public SceneModuleInterface } absence_traffic_light; double attention_lane_crop_curvature_threshold; double attention_lane_curvature_calculation_ds; + double static_occlusion_with_traffic_light_timeout; } occlusion; }; + enum OcclusionType { + NOT_OCCLUDED, + STATICALLY_OCCLUDED, + DYNAMICALLY_OCCLUDED, + RTC_OCCLUDED, // actual occlusion does not exist, only disapproved by RTC + }; + struct Indecisive { std::string error; @@ -141,6 +149,7 @@ class IntersectionModule : public SceneModuleInterface size_t first_stop_line_idx{0}; size_t occlusion_stop_line_idx{0}; }; + // A state peeking to occlusion limit line in the presence of traffic light struct PeekingTowardOcclusion { // NOTE: if intersection_occlusion is disapproved externally through RTC, @@ -151,7 +160,12 @@ class IntersectionModule : public SceneModuleInterface size_t collision_stop_line_idx{0}; size_t first_attention_stop_line_idx{0}; size_t occlusion_stop_line_idx{0}; + // if null, it is dynamic occlusion and shows up intersection_occlusion(dyn) + // if valid, it contains the remaining time to release the static occlusion stuck and shows up + // intersection_occlusion(x.y) + std::optional static_occlusion_timeout{std::nullopt}; }; + // A state detecting both collision and occlusion in the presence of traffic light struct OccludedCollisionStop { bool is_actually_occlusion_cleared{false}; @@ -160,6 +174,9 @@ class IntersectionModule : public SceneModuleInterface size_t collision_stop_line_idx{0}; size_t first_attention_stop_line_idx{0}; size_t occlusion_stop_line_idx{0}; + // if null, it is dynamic occlusion and shows up intersection_occlusion(dyn) + // if valid, it contains the remaining time to release the static occlusion stuck + std::optional static_occlusion_timeout{std::nullopt}; }; struct OccludedAbsenceTrafficLight { @@ -227,23 +244,25 @@ class IntersectionModule : public SceneModuleInterface const std::string turn_direction_; const bool has_traffic_light_; - bool is_go_out_ = false; - bool is_permanent_go_ = false; - DecisionResult prev_decision_result_; + bool is_go_out_{false}; + bool is_permanent_go_{false}; + DecisionResult prev_decision_result_{Indecisive{""}}; + OcclusionType prev_occlusion_status_; // Parameter PlannerParam planner_param_; - std::optional intersection_lanelets_; + std::optional intersection_lanelets_{std::nullopt}; // for occlusion detection const bool enable_occlusion_detection_; - std::optional> occlusion_attention_divisions_; - // OcclusionState prev_occlusion_state_ = OcclusionState::NONE; + std::optional> occlusion_attention_divisions_{ + std::nullopt}; StateMachine collision_state_machine_; //! for stable collision checking StateMachine before_creep_state_machine_; //! for two phase stop StateMachine occlusion_stop_state_machine_; StateMachine temporal_stop_before_attention_state_machine_; + StateMachine static_occlusion_timeout_state_machine_; // NOTE: uuid_ is base member @@ -253,11 +272,11 @@ class IntersectionModule : public SceneModuleInterface // for RTC const UUID occlusion_uuid_; - bool occlusion_safety_ = true; - double occlusion_stop_distance_; - bool occlusion_activated_ = true; + bool occlusion_safety_{true}; + double occlusion_stop_distance_{0.0}; + bool occlusion_activated_{true}; // for first stop in two-phase stop - bool occlusion_first_stop_required_ = false; + bool occlusion_first_stop_required_{false}; void initializeRTCStatus(); void prepareRTCStatus( @@ -269,26 +288,23 @@ class IntersectionModule : public SceneModuleInterface const std::shared_ptr & planner_data, const util::PathLanelets & path_lanelets); - autoware_auto_perception_msgs::msg::PredictedObjects filterTargetObjects( - const lanelet::ConstLanelets & attention_area_lanelets, - const lanelet::ConstLanelets & adjacent_lanelets, + util::TargetObjects generateTargetObjects( + const util::IntersectionLanelets & intersection_lanelets, const std::optional & intersection_area) const; bool checkCollision( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const autoware_auto_perception_msgs::msg::PredictedObjects & target_objects, - const util::PathLanelets & path_lanelets, const int closest_idx, const double time_delay, - const bool tl_arrow_solid_on); + util::TargetObjects * target_objects, const util::PathLanelets & path_lanelets, + const int closest_idx, const double time_delay, const bool tl_arrow_solid_on); - bool isOcclusionCleared( + OcclusionType getOcclusionStatus( const nav_msgs::msg::OccupancyGrid & occ_grid, const std::vector & attention_areas, const lanelet::ConstLanelets & adjacent_lanelets, const lanelet::CompoundPolygon3d & first_attention_area, const util::InterpolatedPathInfo & interpolated_path_info, - const std::vector & lane_divisions, - const std::vector & - parked_attention_objects, + const std::vector & lane_divisions, + const util::TargetObjects & target_objects, const geometry_msgs::msg::Pose & current_pose, const double occlusion_dist_thr); /* diff --git a/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp b/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp index 10ad395953476..1553345dfc827 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp @@ -93,8 +93,10 @@ bool MergeFromPrivateRoadModule::modifyPathVelocity(PathWithLaneId * path, StopR planner_param_.attention_area_length, planner_param_.occlusion_attention_area_length, planner_param_.consider_wrong_direction_vehicle); } - intersection_lanelets_.value().update(false, interpolated_path_info); - const auto & first_conflicting_area = intersection_lanelets_.value().first_conflicting_area(); + auto & intersection_lanelets = intersection_lanelets_.value(); + const auto local_footprint = planner_data_->vehicle_info_.createFootprint(0.0, 0.0); + intersection_lanelets.update(false, interpolated_path_info, local_footprint); + const auto & first_conflicting_area = intersection_lanelets.first_conflicting_area(); if (!first_conflicting_area) { return false; } diff --git a/planning/behavior_velocity_intersection_module/src/util.cpp b/planning/behavior_velocity_intersection_module/src/util.cpp index e118b49781c15..3cf6719c0f07e 100644 --- a/planning/behavior_velocity_intersection_module/src/util.cpp +++ b/planning/behavior_velocity_intersection_module/src/util.cpp @@ -217,6 +217,50 @@ static std::optional getStopLineIndexFromMap( return stop_idx_ip_opt.get(); } +static std::optional getFirstPointInsidePolygonByFootprint( + const lanelet::CompoundPolygon3d & polygon, const InterpolatedPathInfo & interpolated_path_info, + const tier4_autoware_utils::LinearRing2d & footprint) +{ + const auto & path_ip = interpolated_path_info.path; + const auto [lane_start, lane_end] = interpolated_path_info.lane_id_interval.value(); + + const auto area_2d = lanelet::utils::to2D(polygon).basicPolygon(); + for (auto i = lane_start; i <= lane_end; ++i) { + const auto & base_pose = path_ip.points.at(i).point.pose; + const auto path_footprint = tier4_autoware_utils::transformVector( + footprint, tier4_autoware_utils::pose2transform(base_pose)); + if (bg::intersects(path_footprint, area_2d)) { + return std::make_optional(i); + } + } + return std::nullopt; +} + +static std::optional> +getFirstPointInsidePolygonsByFootprint( + const std::vector & polygons, + const InterpolatedPathInfo & interpolated_path_info, + const tier4_autoware_utils::LinearRing2d & footprint) +{ + const auto & path_ip = interpolated_path_info.path; + const auto [lane_start, lane_end] = interpolated_path_info.lane_id_interval.value(); + + for (size_t i = lane_start; i <= lane_end; ++i) { + const auto & pose = path_ip.points.at(i).point.pose; + const auto path_footprint = + tier4_autoware_utils::transformVector(footprint, tier4_autoware_utils::pose2transform(pose)); + for (size_t j = 0; j < polygons.size(); ++j) { + const auto area_2d = lanelet::utils::to2D(polygons.at(j)).basicPolygon(); + const bool is_in_polygon = bg::intersects(area_2d, path_footprint); + if (is_in_polygon) { + return std::make_optional>(i, j); + } + } + } + return std::nullopt; +} + std::optional generateIntersectionStopLines( const lanelet::CompoundPolygon3d & first_conflicting_area, const lanelet::CompoundPolygon3d & first_detection_area, @@ -233,6 +277,7 @@ std::optional generateIntersectionStopLines( const int base2front_idx_dist = std::ceil(planner_data->vehicle_info_.max_longitudinal_offset_m / ds); + /* // find the index of the first point that intersects with detection_areas const auto first_inside_detection_idx_ip_opt = getFirstPointInsidePolygon(path_ip, lane_interval_ip, first_detection_area); @@ -241,6 +286,16 @@ std::optional generateIntersectionStopLines( return std::nullopt; } const auto first_inside_detection_ip = first_inside_detection_idx_ip_opt.value(); + */ + // find the index of the first point whose vehicle footprint on it intersects with detection_area + const auto local_footprint = planner_data->vehicle_info_.createFootprint(0.0, 0.0); + std::optional first_footprint_inside_detection_ip_opt = + getFirstPointInsidePolygonByFootprint( + first_detection_area, interpolated_path_info, local_footprint); + if (!first_footprint_inside_detection_ip_opt) { + return std::nullopt; + } + const auto first_footprint_inside_detection_ip = first_footprint_inside_detection_ip_opt.value(); // (1) default stop line position on interpolated path bool default_stop_line_valid = true; @@ -251,8 +306,7 @@ std::optional generateIntersectionStopLines( stop_idx_ip_int = static_cast(map_stop_idx_ip.value()) - base2front_idx_dist; } if (stop_idx_ip_int < 0) { - stop_idx_ip_int = static_cast(first_inside_detection_ip) - stop_line_margin_idx_dist - - base2front_idx_dist; + stop_idx_ip_int = first_footprint_inside_detection_ip - stop_line_margin_idx_dist; } if (stop_idx_ip_int < 0) { default_stop_line_valid = false; @@ -269,8 +323,6 @@ std::optional generateIntersectionStopLines( const auto closest_idx_ip = closest_idx_ip_opt.value(); // (3) occlusion peeking stop line position on interpolated path - const auto local_footprint = planner_data->vehicle_info_.createFootprint(0.0, 0.0); - const auto area_2d = lanelet::utils::to2D(first_detection_area).basicPolygon(); int occlusion_peeking_line_ip_int = static_cast(default_stop_line_ip); bool occlusion_peeking_line_valid = true; { @@ -278,20 +330,13 @@ std::optional generateIntersectionStopLines( const auto & base_pose0 = path_ip.points.at(default_stop_line_ip).point.pose; const auto path_footprint0 = tier4_autoware_utils::transformVector( local_footprint, tier4_autoware_utils::pose2transform(base_pose0)); - if (bg::intersects(path_footprint0, area_2d)) { + if (bg::intersects( + path_footprint0, lanelet::utils::to2D(first_detection_area).basicPolygon())) { occlusion_peeking_line_valid = false; } } if (occlusion_peeking_line_valid) { - for (size_t i = default_stop_line_ip + 1; i <= std::get<1>(lane_interval_ip); ++i) { - const auto & base_pose = path_ip.points.at(i).point.pose; - const auto path_footprint = tier4_autoware_utils::transformVector( - local_footprint, tier4_autoware_utils::pose2transform(base_pose)); - if (bg::intersects(path_footprint, area_2d)) { - occlusion_peeking_line_ip_int = i; - break; - } - } + occlusion_peeking_line_ip_int = first_footprint_inside_detection_ip; } const auto first_attention_stop_line_ip = static_cast(occlusion_peeking_line_ip_int); const bool first_attention_stop_line_valid = occlusion_peeking_line_valid; @@ -307,8 +352,8 @@ std::optional generateIntersectionStopLines( const double delay_response_time = planner_data->delay_response_time; const double braking_dist = planning_utils::calcJudgeLineDistWithJerkLimit( velocity, acceleration, max_stop_acceleration, max_stop_jerk, delay_response_time); - int pass_judge_ip_int = static_cast(first_inside_detection_ip) - base2front_idx_dist - - std::ceil(braking_dist / ds); + int pass_judge_ip_int = + static_cast(first_footprint_inside_detection_ip) - std::ceil(braking_dist / ds); const auto pass_judge_line_ip = static_cast( std::clamp(pass_judge_ip_int, 0, static_cast(path_ip.points.size()) - 1)); @@ -318,18 +363,18 @@ std::optional generateIntersectionStopLines( if (use_stuck_stopline) { // NOTE: when ego vehicle is approaching detection area and already passed // first_conflicting_area, this could be null. - const auto stuck_stop_line_idx_ip_opt = - getFirstPointInsidePolygon(path_ip, lane_interval_ip, first_conflicting_area); + const auto stuck_stop_line_idx_ip_opt = getFirstPointInsidePolygonByFootprint( + first_conflicting_area, interpolated_path_info, local_footprint); if (!stuck_stop_line_idx_ip_opt) { stuck_stop_line_valid = false; stuck_stop_line_ip_int = 0; } else { - stuck_stop_line_ip_int = stuck_stop_line_idx_ip_opt.value(); + stuck_stop_line_ip_int = stuck_stop_line_idx_ip_opt.value() - stop_line_margin_idx_dist; } } else { - stuck_stop_line_ip_int = std::get<0>(lane_interval_ip); + stuck_stop_line_ip_int = + std::get<0>(lane_interval_ip) - (stop_line_margin_idx_dist + base2front_idx_dist); } - stuck_stop_line_ip_int -= (stop_line_margin_idx_dist + base2front_idx_dist); if (stuck_stop_line_ip_int < 0) { stuck_stop_line_valid = false; } @@ -427,6 +472,9 @@ std::optional getFirstPointInsidePolygon( if (is_in_lanelet) { return std::make_optional(i); } + if (i == 0) { + break; + } } } return std::nullopt; @@ -469,6 +517,9 @@ getFirstPointInsidePolygons( if (is_in_lanelet) { break; } + if (i == 0) { + break; + } } } return std::nullopt; @@ -516,6 +567,110 @@ static std::vector getPolygon3dFromLanelets( return polys; } +static std::string getTurnDirection(lanelet::ConstLanelet lane) +{ + return lane.attributeOr("turn_direction", "else"); +} + +/** + * @param pair lanelets and the vector of original lanelets in topological order (not reversed as + *in generateDetectionLaneDivisions()) + **/ +static std::pair> +mergeLaneletsByTopologicalSort( + const lanelet::ConstLanelets & lanelets, + const lanelet::routing::RoutingGraphPtr routing_graph_ptr) +{ + const int n_node = lanelets.size(); + std::vector> adjacency(n_node); + for (int dst = 0; dst < n_node; ++dst) { + adjacency[dst].resize(n_node); + for (int src = 0; src < n_node; ++src) { + adjacency[dst][src] = false; + } + } + std::set lanelet_ids; + std::unordered_map id2ind; + std::unordered_map ind2id; + std::unordered_map id2lanelet; + int ind = 0; + for (const auto & lanelet : lanelets) { + lanelet_ids.insert(lanelet.id()); + const auto id = lanelet.id(); + id2ind[id] = ind; + ind2id[ind] = id; + id2lanelet[id] = lanelet; + ind++; + } + // NOTE: this function aims to traverse the detection lanelet backward from ego side to farthest + // side, so if lane B follows lane A on the routing_graph, adj[B][A] = true + for (const auto & lanelet : lanelets) { + const auto & followings = routing_graph_ptr->following(lanelet); + const int dst = lanelet.id(); + for (const auto & following : followings) { + if (const int src = following.id(); lanelet_ids.find(src) != lanelet_ids.end()) { + adjacency[(id2ind[src])][(id2ind[dst])] = true; + } + } + } + // terminal node + std::map> branches; + auto has_no_previous = [&](const int node) { + for (int dst = 0; dst < n_node; dst++) { + if (adjacency[dst][node]) { + return false; + } + } + return true; + }; + for (int src = 0; src < n_node; src++) { + if (!has_no_previous(src)) { + continue; + } + // So `src` has no previous lanelets + branches[(ind2id[src])] = std::vector{}; + auto & branch = branches[(ind2id[src])]; + lanelet::Id node_iter = ind2id[src]; + std::set visited_ids; + while (true) { + const auto & destinations = adjacency[(id2ind[node_iter])]; + // NOTE: assuming detection lanelets have only one "previous"(on the routing_graph) lanelet + const auto next = std::find(destinations.begin(), destinations.end(), true); + if (next == destinations.end()) { + branch.push_back(node_iter); + break; + } + if (visited_ids.find(node_iter) != visited_ids.end()) { + // loop detected + break; + } + branch.push_back(node_iter); + visited_ids.insert(node_iter); + node_iter = ind2id[std::distance(destinations.begin(), next)]; + } + } + for (decltype(branches)::iterator it = branches.begin(); it != branches.end(); it++) { + auto & branch = it->second; + std::reverse(branch.begin(), branch.end()); + } + lanelet::ConstLanelets merged; + std::vector originals; + for (const auto & [id, sub_ids] : branches) { + if (sub_ids.size() == 0) { + continue; + } + lanelet::ConstLanelets merge; + originals.push_back(lanelet::ConstLanelets({})); + auto & original = originals.back(); + for (const auto sub_id : sub_ids) { + merge.push_back(id2lanelet[sub_id]); + original.push_back(id2lanelet[sub_id]); + } + merged.push_back(lanelet::utils::combineLaneletsShape(merge)); + } + return {merged, originals}; +} + IntersectionLanelets getObjectiveLanelets( lanelet::LaneletMapConstPtr lanelet_map_ptr, lanelet::routing::RoutingGraphPtr routing_graph_ptr, const lanelet::ConstLanelet assigned_lanelet, const lanelet::ConstLanelets & lanelets_on_path, @@ -653,14 +808,52 @@ IntersectionLanelets getObjectiveLanelets( } } } + lanelet::ConstLanelets occlusion_detection_and_preceding_lanelets_wo_turn_direction; + for (const auto & ll : occlusion_detection_and_preceding_lanelets) { + const auto turn_direction = getTurnDirection(ll); + if (turn_direction == "left" || turn_direction == "right") { + continue; + } + occlusion_detection_and_preceding_lanelets_wo_turn_direction.push_back(ll); + } + + auto [attention_lanelets, original_attention_lanelet_sequences] = + mergeLaneletsByTopologicalSort(detection_and_preceding_lanelets, routing_graph_ptr); IntersectionLanelets result; - result.attention_ = std::move(detection_and_preceding_lanelets); + result.attention_ = std::move(attention_lanelets); + for (const auto & original_attention_lanelet_seq : original_attention_lanelet_sequences) { + // NOTE: in mergeLaneletsByTopologicalSort(), sub_ids are empty checked, so it is ensured that + // back() exists. + std::optional stop_line{std::nullopt}; + for (auto it = original_attention_lanelet_seq.rbegin(); + it != original_attention_lanelet_seq.rend(); ++it) { + const auto traffic_lights = it->regulatoryElementsAs(); + for (const auto & traffic_light : traffic_lights) { + const auto stop_line_opt = traffic_light->stopLine(); + if (!stop_line_opt) continue; + stop_line = stop_line_opt.get(); + break; + } + if (stop_line) break; + } + result.attention_stop_lines_.push_back(stop_line); + } result.attention_non_preceding_ = std::move(detection_lanelets); + // TODO(Mamoru Sobue): find stop lines for attention_non_preceding_ if needed + for (unsigned i = 0; i < result.attention_non_preceding_.size(); ++i) { + result.attention_non_preceding_stop_lines_.push_back(std::nullopt); + } result.conflicting_ = std::move(conflicting_ex_ego_lanelets); result.adjacent_ = planning_utils::getConstLaneletsFromIds(lanelet_map_ptr, associative_ids); - result.occlusion_attention_ = std::move(occlusion_detection_and_preceding_lanelets); - // compoundPolygon3d + // NOTE: occlusion_attention is not inverted here + // TODO(Mamoru Sobue): apply mergeLaneletsByTopologicalSort for occlusion lanelets as well and + // then trim part of them based on curvature threshold + result.occlusion_attention_ = + std::move(occlusion_detection_and_preceding_lanelets_wo_turn_direction); + + // NOTE: to properly update(), each element in conflicting_/conflicting_area_, + // attention_non_preceding_/attention_non_preceding_area_ need to be matched result.attention_area_ = getPolygon3dFromLanelets(result.attention_); result.attention_non_preceding_area_ = getPolygon3dFromLanelets(result.attention_non_preceding_); result.conflicting_area_ = getPolygon3dFromLanelets(result.conflicting_); @@ -669,11 +862,6 @@ IntersectionLanelets getObjectiveLanelets( return result; } -static std::string getTurnDirection(lanelet::ConstLanelet lane) -{ - return lane.attributeOr("turn_direction", "else"); -} - bool isOverTargetIndex( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int closest_idx, const geometry_msgs::msg::Pose & current_pose, const int target_idx) @@ -836,21 +1024,17 @@ double getHighestCurvature(const lanelet::ConstLineString3d & centerline) return *std::max_element(curvatures_positive.begin(), curvatures_positive.end()); } -std::vector generateDetectionLaneDivisions( +std::vector generateDetectionLaneDivisions( lanelet::ConstLanelets detection_lanelets_all, const lanelet::routing::RoutingGraphPtr routing_graph_ptr, const double resolution, const double curvature_threshold, const double curvature_calculation_ds) { using lanelet::utils::getCenterlineWithOffset; - using lanelet::utils::to2D; // (0) remove left/right lanelet lanelet::ConstLanelets detection_lanelets; for (const auto & detection_lanelet : detection_lanelets_all) { - const auto turn_direction = getTurnDirection(detection_lanelet); - if (turn_direction.compare("left") == 0 || turn_direction.compare("right") == 0) { - continue; - } + // TODO(Mamoru Sobue): instead of ignoring, only trim straight part of lanelet const auto fine_centerline = lanelet::utils::generateFineCenterline(detection_lanelet, curvature_calculation_ds); const double highest_curvature = getHighestCurvature(fine_centerline); @@ -861,111 +1045,34 @@ std::vector generateDetectionLaneDivisions( } // (1) tsort detection_lanelets - // generate adjacency matrix - // if lanelet1 => lanelet2; then adjacency[lanelet2][lanelet1] = true - const int n_node = detection_lanelets.size(); - std::vector> adjacency(n_node); - for (int dst = 0; dst < n_node; ++dst) { - adjacency[dst].resize(n_node); - for (int src = 0; src < n_node; ++src) { - adjacency[dst][src] = false; - } - } - std::set detection_lanelet_ids; - std::unordered_map id2ind, ind2id; - std::unordered_map id2lanelet; - int ind = 0; - for (const auto & detection_lanelet : detection_lanelets) { - detection_lanelet_ids.insert(detection_lanelet.id()); - const int id = detection_lanelet.id(); - id2ind[id] = ind; - ind2id[ind] = id; - id2lanelet[id] = detection_lanelet; - ind++; - } - for (const auto & detection_lanelet : detection_lanelets) { - const auto & followings = routing_graph_ptr->following(detection_lanelet); - const int dst = detection_lanelet.id(); - for (const auto & following : followings) { - if (const int src = following.id(); - detection_lanelet_ids.find(src) != detection_lanelet_ids.end()) { - adjacency[(id2ind[src])][(id2ind[dst])] = true; - } - } - } - // terminal node - std::map> branches; - auto has_no_previous = [&](const int node) { - for (int dst = 0; dst < n_node; dst++) { - if (adjacency[dst][node]) { - return false; - } - } - return true; - }; - for (int src = 0; src < n_node; src++) { - if (!has_no_previous(src)) { - continue; - } - branches[(ind2id[src])] = std::vector{}; - auto & branch = branches[(ind2id[src])]; - int node_iter = ind2id[src]; - while (true) { - const auto & dsts = adjacency[(id2ind[node_iter])]; - // NOTE: assuming detection lanelets have only one previous lanelet - const auto next = std::find(dsts.begin(), dsts.end(), true); - if (next == dsts.end()) { - branch.push_back(node_iter); - break; - } - branch.push_back(node_iter); - node_iter = ind2id[std::distance(dsts.begin(), next)]; - } - } - for (decltype(branches)::iterator it = branches.begin(); it != branches.end(); it++) { - auto & branch = it->second; - std::reverse(branch.begin(), branch.end()); - } + const auto [merged_detection_lanelets, originals] = + mergeLaneletsByTopologicalSort(detection_lanelets, routing_graph_ptr); // (2) merge each branch to one lanelet // NOTE: somehow bg::area() for merged lanelet does not work, so calculate it here - std::unordered_map> merged_branches; - for (const auto & [src, branch] : branches) { - lanelet::Points3d lefts; - lanelet::Points3d rights; + std::vector> merged_lanelet_with_area; + for (unsigned i = 0; i < merged_detection_lanelets.size(); ++i) { + const auto & merged_detection_lanelet = merged_detection_lanelets.at(i); + const auto & original = originals.at(i); double area = 0; - for (const auto & lane_id : branch) { - const auto lane = id2lanelet[lane_id]; - for (const auto & left_point : lane.leftBound()) { - lefts.push_back(lanelet::Point3d(left_point)); - } - for (const auto & right_point : lane.rightBound()) { - rights.push_back(lanelet::Point3d(right_point)); - } - area += bg::area(lane.polygon2d().basicPolygon()); + for (const auto & partition : original) { + area += bg::area(partition.polygon2d().basicPolygon()); } - lanelet::LineString3d right = lanelet::LineString3d(lanelet::InvalId, lefts).invert(); - lanelet::LineString3d left = lanelet::LineString3d(lanelet::InvalId, rights).invert(); - lanelet::Lanelet merged = lanelet::Lanelet(lanelet::InvalId, left, right); - merged_branches[src] = std::make_pair(merged, area); + merged_lanelet_with_area.emplace_back(merged_detection_lanelet, area); } // (3) discretize each merged lanelet - std::vector detection_divisions; - for (const auto & [last_lane_id, branch] : merged_branches) { - DescritizedLane detection_division; - detection_division.lane_id = last_lane_id; - const auto detection_lanelet = branch.first; - const double area = branch.second; - const double length = bg::length(detection_lanelet.centerline()); + std::vector detection_divisions; + for (const auto & [merged_lanelet, area] : merged_lanelet_with_area) { + const double length = bg::length(merged_lanelet.centerline()); const double width = area / length; - auto & divisions = detection_division.divisions; for (int i = 0; i < static_cast(width / resolution); ++i) { const double offset = resolution * i - width / 2; - divisions.push_back(to2D(getCenterlineWithOffset(detection_lanelet, offset, resolution))); + detection_divisions.push_back( + getCenterlineWithOffset(merged_lanelet, offset, resolution).invert()); } - divisions.push_back(to2D(getCenterlineWithOffset(detection_lanelet, width / 2, resolution))); - detection_divisions.push_back(detection_division); + detection_divisions.push_back( + getCenterlineWithOffset(merged_lanelet, width / 2, resolution).invert()); } return detection_divisions; } @@ -1088,13 +1195,13 @@ Polygon2d generateStuckVehicleDetectAreaPolygon( return polygon; } -bool checkAngleForTargetLanelets( - const geometry_msgs::msg::Pose & pose, const double longitudinal_velocity, - const lanelet::ConstLanelets & target_lanelets, const double detection_area_angle_thr, - const bool consider_wrong_direction_vehicle, const double dist_margin, - const double parked_vehicle_speed_threshold) +std::optional checkAngleForTargetLanelets( + const geometry_msgs::msg::Pose & pose, const lanelet::ConstLanelets & target_lanelets, + const double detection_area_angle_thr, const bool consider_wrong_direction_vehicle, + const double dist_margin, const bool is_parked_vehicle) { - for (const auto & ll : target_lanelets) { + for (unsigned i = 0; i < target_lanelets.size(); ++i) { + const auto & ll = target_lanelets.at(i); if (!lanelet::utils::isInLanelet(pose, ll, dist_margin)) { continue; } @@ -1103,39 +1210,38 @@ bool checkAngleForTargetLanelets( const double angle_diff = tier4_autoware_utils::normalizeRadian(ll_angle - pose_angle, -M_PI); if (consider_wrong_direction_vehicle) { if (std::fabs(angle_diff) > 1.57 || std::fabs(angle_diff) < detection_area_angle_thr) { - return true; + return std::make_optional(i); } } else { if (std::fabs(angle_diff) < detection_area_angle_thr) { - return true; + return std::make_optional(i); } // NOTE: sometimes parked vehicle direction is reversed even if its longitudinal velocity is // positive if ( - std::fabs(longitudinal_velocity) < parked_vehicle_speed_threshold && - (std::fabs(angle_diff) < detection_area_angle_thr || - (std::fabs(angle_diff + M_PI) < detection_area_angle_thr))) { - return true; + is_parked_vehicle && (std::fabs(angle_diff) < detection_area_angle_thr || + (std::fabs(angle_diff + M_PI) < detection_area_angle_thr))) { + return std::make_optional(i); } } } - return false; + return std::nullopt; } void cutPredictPathWithDuration( - autoware_auto_perception_msgs::msg::PredictedObjects * objects_ptr, - const rclcpp::Clock::SharedPtr clock, const double time_thr) + util::TargetObjects * target_objects, const rclcpp::Clock::SharedPtr clock, const double time_thr) { const rclcpp::Time current_time = clock->now(); - for (auto & object : objects_ptr->objects) { // each objects - for (auto & predicted_path : object.kinematics.predicted_paths) { // each predicted paths + for (auto & target_object : target_objects->all_attention_objects) { // each objects + for (auto & predicted_path : + target_object.object.kinematics.predicted_paths) { // each predicted paths const auto origin_path = predicted_path; predicted_path.path.clear(); for (size_t k = 0; k < origin_path.path.size(); ++k) { // each path points const auto & predicted_pose = origin_path.path.at(k); const auto predicted_time = - rclcpp::Time(objects_ptr->header.stamp) + + rclcpp::Time(target_objects->header.stamp) + rclcpp::Duration(origin_path.time_step) * static_cast(k); if ((predicted_time - current_time).seconds() < time_thr) { predicted_path.path.push_back(predicted_pose); @@ -1229,22 +1335,25 @@ double calcDistanceUntilIntersectionLanelet( } void IntersectionLanelets::update( - const bool tl_arrow_solid_on, const InterpolatedPathInfo & interpolated_path_info) + const bool tl_arrow_solid_on, const InterpolatedPathInfo & interpolated_path_info, + const tier4_autoware_utils::LinearRing2d & footprint) { tl_arrow_solid_on_ = tl_arrow_solid_on; // find the first conflicting/detection area polygon intersecting the path - const auto & path = interpolated_path_info.path; - const auto & lane_interval = interpolated_path_info.lane_id_interval.value(); - { - auto first = getFirstPointInsidePolygons(path, lane_interval, conflicting_area_); - if (first && !first_conflicting_area_) { - first_conflicting_area_ = first.value().second; + if (!first_conflicting_area_) { + auto first = + getFirstPointInsidePolygonsByFootprint(conflicting_area_, interpolated_path_info, footprint); + if (first) { + first_conflicting_lane_ = conflicting_.at(first.value().second); + first_conflicting_area_ = conflicting_area_.at(first.value().second); } } - { - auto first = getFirstPointInsidePolygons(path, lane_interval, attention_area_); - if (first && !first_attention_area_) { - first_attention_area_ = first.value().second; + if (!first_attention_area_) { + auto first = getFirstPointInsidePolygonsByFootprint( + attention_non_preceding_area_, interpolated_path_info, footprint); + if (first) { + first_attention_lane_ = attention_non_preceding_.at(first.value().second); + first_attention_area_ = attention_non_preceding_area_.at(first.value().second); } } } @@ -1330,5 +1439,23 @@ std::optional generatePathLanelets( return path_lanelets; } +void TargetObject::calc_dist_to_stop_line() +{ + if (!attention_lanelet || !stop_line) { + return; + } + const auto attention_lanelet_val = attention_lanelet.value(); + const auto object_arc_coords = lanelet::utils::getArcCoordinates( + {attention_lanelet_val}, object.kinematics.initial_pose_with_covariance.pose); + const auto stop_line_val = stop_line.value(); + geometry_msgs::msg::Pose stopline_center; + stopline_center.position.x = (stop_line_val.front().x() + stop_line_val.back().x()) / 2.0; + stopline_center.position.y = (stop_line_val.front().y() + stop_line_val.back().y()) / 2.0; + stopline_center.position.z = (stop_line_val.front().z() + stop_line_val.back().z()) / 2.0; + const auto stopline_arc_coords = + lanelet::utils::getArcCoordinates({attention_lanelet_val}, stopline_center); + dist_to_stop_line = (stopline_arc_coords.length - object_arc_coords.length); +} + } // namespace util } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_intersection_module/src/util.hpp b/planning/behavior_velocity_intersection_module/src/util.hpp index c61771ef1e603..f9e47e1567163 100644 --- a/planning/behavior_velocity_intersection_module/src/util.hpp +++ b/planning/behavior_velocity_intersection_module/src/util.hpp @@ -106,7 +106,7 @@ bool hasAssociatedTrafficLight(lanelet::ConstLanelet lane); bool isTrafficLightArrowActivated( lanelet::ConstLanelet lane, const std::map & tl_infos); -std::vector generateDetectionLaneDivisions( +std::vector generateDetectionLaneDivisions( lanelet::ConstLanelets detection_lanelets, const lanelet::routing::RoutingGraphPtr routing_graph_ptr, const double resolution, const double curvature_threshold, const double curvature_calculation_ds); @@ -127,15 +127,14 @@ bool checkStuckVehicleInIntersection( Polygon2d generateStuckVehicleDetectAreaPolygon( const util::PathLanelets & path_lanelets, const double stuck_vehicle_detect_dist); -bool checkAngleForTargetLanelets( - const geometry_msgs::msg::Pose & pose, const double longitudinal_velocity, - const lanelet::ConstLanelets & target_lanelets, const double detection_area_angle_thr, - const bool consider_wrong_direction_vehicle, const double dist_margin, - const double parked_vehicle_speed_threshold); +std::optional checkAngleForTargetLanelets( + const geometry_msgs::msg::Pose & pose, const lanelet::ConstLanelets & target_lanelets, + const double detection_area_angle_thr, const bool consider_wrong_direction_vehicle, + const double dist_margin, const bool is_parked_vehicle); void cutPredictPathWithDuration( - autoware_auto_perception_msgs::msg::PredictedObjects * objects_ptr, - const rclcpp::Clock::SharedPtr clock, const double time_thr); + util::TargetObjects * target_objects, const rclcpp::Clock::SharedPtr clock, + const double time_thr); TimeDistanceArray calcIntersectionPassingTime( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, diff --git a/planning/behavior_velocity_intersection_module/src/util_type.hpp b/planning/behavior_velocity_intersection_module/src/util_type.hpp index 20dd489552066..4d58016bded3f 100644 --- a/planning/behavior_velocity_intersection_module/src/util_type.hpp +++ b/planning/behavior_velocity_intersection_module/src/util_type.hpp @@ -15,6 +15,8 @@ #ifndef UTIL_TYPE_HPP_ #define UTIL_TYPE_HPP_ +#include + #include #include #include @@ -40,19 +42,20 @@ struct DebugData std::optional pass_judge_wall_pose{std::nullopt}; std::optional> attention_area{std::nullopt}; std::optional> occlusion_attention_area{std::nullopt}; - std::optional intersection_area{std::nullopt}; std::optional ego_lane{std::nullopt}; std::optional> adjacent_area{std::nullopt}; std::optional stuck_vehicle_detect_area{std::nullopt}; std::optional candidate_collision_ego_lane_polygon{std::nullopt}; std::vector candidate_collision_object_polygons; autoware_auto_perception_msgs::msg::PredictedObjects conflicting_targets; + autoware_auto_perception_msgs::msg::PredictedObjects amber_ignore_targets; autoware_auto_perception_msgs::msg::PredictedObjects stuck_targets; std::vector occlusion_polygons; std::optional> nearest_occlusion_projection{std::nullopt}; autoware_auto_perception_msgs::msg::PredictedObjects blocking_attention_objects; std::optional absence_traffic_light_creep_wall{std::nullopt}; + std::optional static_occlusion_with_traffic_light_timeout{std::nullopt}; }; struct InterpolatedPathInfo @@ -67,11 +70,17 @@ struct InterpolatedPathInfo struct IntersectionLanelets { public: - void update(const bool tl_arrow_solid_on, const InterpolatedPathInfo & interpolated_path_info); + void update( + const bool tl_arrow_solid_on, const InterpolatedPathInfo & interpolated_path_info, + const tier4_autoware_utils::LinearRing2d & footprint); const lanelet::ConstLanelets & attention() const { return tl_arrow_solid_on_ ? attention_non_preceding_ : attention_; } + const std::vector> & attention_stop_lines() const + { + return tl_arrow_solid_on_ ? attention_non_preceding_stop_lines_ : attention_stop_lines_; + } const lanelet::ConstLanelets & conflicting() const { return conflicting_; } const lanelet::ConstLanelets & adjacent() const { return adjacent_; } const lanelet::ConstLanelets & occlusion_attention() const @@ -91,38 +100,48 @@ struct IntersectionLanelets { return occlusion_attention_area_; } + const std::optional & first_conflicting_lane() const + { + return first_conflicting_lane_; + } const std::optional & first_conflicting_area() const { return first_conflicting_area_; } + const std::optional & first_attention_lane() const + { + return first_attention_lane_; + } const std::optional & first_attention_area() const { return first_attention_area_; } - lanelet::ConstLanelets attention_; + lanelet::ConstLanelets attention_; // topologically merged lanelets + std::vector> + attention_stop_lines_; // the stop lines for each attention_ lanelets lanelet::ConstLanelets attention_non_preceding_; + std::vector> + attention_non_preceding_stop_lines_; // the stop lines for each attention_non_preceding_ + // lanelets lanelet::ConstLanelets conflicting_; lanelet::ConstLanelets adjacent_; - lanelet::ConstLanelets occlusion_attention_; // for occlusion detection - std::vector attention_area_; + lanelet::ConstLanelets occlusion_attention_; // topologically merged lanelets + std::vector occlusion_attention_size_; // the area() of each occlusion attention lanelets + std::vector attention_area_; // topologically merged lanelets std::vector attention_non_preceding_area_; std::vector conflicting_area_; std::vector adjacent_area_; - std::vector occlusion_attention_area_; + std::vector + occlusion_attention_area_; // topologically merged lanelets // the first area intersecting with the path // even if lane change/re-routing happened on the intersection, these areas area are supposed to // be invariant under the 'associative' lanes. + std::optional first_conflicting_lane_{std::nullopt}; + std::optional first_conflicting_area_{std::nullopt}; + std::optional first_attention_lane_{std::nullopt}; + std::optional first_attention_area_{std::nullopt}; bool tl_arrow_solid_on_ = false; - std::optional first_conflicting_area_ = std::nullopt; - std::optional first_attention_area_ = std::nullopt; -}; - -struct DescritizedLane -{ - int lane_id{0}; - // discrete fine lines from left to right - std::vector divisions{}; }; struct IntersectionStopLines @@ -156,6 +175,23 @@ struct PathLanelets // conflicting lanelets plus the next lane part of the path }; +struct TargetObject +{ + autoware_auto_perception_msgs::msg::PredictedObject object; + std::optional attention_lanelet{std::nullopt}; + std::optional stop_line{std::nullopt}; + std::optional dist_to_stop_line{std::nullopt}; + void calc_dist_to_stop_line(); +}; + +struct TargetObjects +{ + std_msgs::msg::Header header; + std::vector attention_objects; + std::vector parked_attention_objects; + std::vector intersection_area_objects; + std::vector all_attention_objects; // TODO(Mamoru Sobue): avoid copy +}; } // namespace behavior_velocity_planner::util #endif // UTIL_TYPE_HPP_