diff --git a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml index cbd4d4879753b..ce49163006d99 100644 --- a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml +++ b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml @@ -47,6 +47,7 @@ max_vehicle_velocity_for_rss: 16.66 # [m/s] == 60kmph denoise_kernel: 1.0 # [m] possible_object_bbox: [1.0, 2.0] # [m x m] + ignore_parked_vehicle_speed_threshold: 0.333 # == 1.2km/h enable_rtc: # 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 intersection: true diff --git a/planning/behavior_velocity_intersection_module/src/manager.cpp b/planning/behavior_velocity_intersection_module/src/manager.cpp index 336087751d9aa..75c282d117e97 100644 --- a/planning/behavior_velocity_intersection_module/src/manager.cpp +++ b/planning/behavior_velocity_intersection_module/src/manager.cpp @@ -115,6 +115,8 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) ip.occlusion.denoise_kernel = node.declare_parameter(ns + ".occlusion.denoise_kernel"); ip.occlusion.possible_object_bbox = node.declare_parameter>(ns + ".occlusion.possible_object_bbox"); + ip.occlusion.ignore_parked_vehicle_speed_threshold = + node.declare_parameter(ns + ".occlusion.ignore_parked_vehicle_speed_threshold"); } 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 b283a757dc8cb..713df530b43d1 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -830,9 +830,10 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( ? 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, attention_lanelets, adjacent_lanelets, intersection_area, ego_lane_with_next_lane, - closest_idx, time_delay, tl_arrow_solid_on); + *path, target_objects, ego_lane_with_next_lane, closest_idx, time_delay, tl_arrow_solid_on); collision_state_machine_.setStateWithMarginTime( has_collision ? StateMachine::State::STOP : StateMachine::State::GO, logger_.get_child("collision state_machine"), *clock_); @@ -853,12 +854,19 @@ 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 parked_attention_objects; + std::copy_if( + target_objects.objects.begin(), target_objects.objects.end(), + std::back_inserter(parked_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; + }); const bool is_occlusion_cleared = (enable_occlusion_detection_ && !occlusion_attention_lanelets.empty() && !tl_arrow_solid_on) ? isOcclusionCleared( *planner_data_->occupancy_grid, occlusion_attention_area, adjacent_lanelets, first_attention_area.value(), interpolated_path_info, - occlusion_attention_divisions_.value(), occlusion_dist_thr) + occlusion_attention_divisions_.value(), parked_attention_objects, occlusion_dist_thr) : true; // check safety @@ -940,13 +948,10 @@ bool IntersectionModule::checkStuckVehicle( return is_stuck; } -bool IntersectionModule::checkCollision( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, +autoware_auto_perception_msgs::msg::PredictedObjects IntersectionModule::filterTargetObjects( const lanelet::ConstLanelets & attention_area_lanelets, const lanelet::ConstLanelets & adjacent_lanelets, - const std::optional & intersection_area, - const lanelet::ConstLanelets & ego_lane_with_next_lane, const int closest_idx, - const double time_delay, const bool tl_arrow_solid_on) + const std::optional & intersection_area) const { using lanelet::utils::getArcCoordinates; using lanelet::utils::getPolygonFromArcLength; @@ -993,6 +998,17 @@ bool IntersectionModule::checkCollision( target_objects.objects.push_back(object); } } + return target_objects; +} + +bool IntersectionModule::checkCollision( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const autoware_auto_perception_msgs::msg::PredictedObjects & objects, + const lanelet::ConstLanelets & ego_lane_with_next_lane, const int closest_idx, + const double time_delay, const bool tl_arrow_solid_on) +{ + using lanelet::utils::getArcCoordinates; + using lanelet::utils::getPolygonFromArcLength; // check collision between target_objects predicted path and ego lane // cut the predicted path at passing_time @@ -1001,6 +1017,7 @@ 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); const auto closest_arc_coords = getArcCoordinates( @@ -1128,7 +1145,9 @@ bool IntersectionModule::isOcclusionCleared( const lanelet::ConstLanelets & adjacent_lanelets, const lanelet::CompoundPolygon3d & first_attention_area, const util::InterpolatedPathInfo & interpolated_path_info, - const std::vector & lane_divisions, const double occlusion_dist_thr) + const std::vector & lane_divisions, + const std::vector & parked_attention_objects, + const double occlusion_dist_thr) { const auto & path_ip = interpolated_path_info.path; const auto & lane_interval_ip = interpolated_path_info.lane_id_interval.value(); @@ -1156,6 +1175,8 @@ bool IntersectionModule::isOcclusionCleared( cv::Mat unknown_mask(width, height, CV_8UC1, cv::Scalar(0)); // (1) prepare detection area mask + // attention: 255 + // non-attention: 0 Polygon2d grid_poly; grid_poly.outer().emplace_back(origin.x, origin.y); grid_poly.outer().emplace_back(origin.x + (width - 1) * resolution, origin.y); @@ -1194,7 +1215,7 @@ bool IntersectionModule::isOcclusionCleared( } } for (const auto & poly : attention_area_cv_polygons) { - cv::fillPoly(attention_mask, poly, cv::Scalar(255), cv::LINE_4); + cv::fillPoly(attention_mask, poly, cv::Scalar(255), cv::LINE_AA); } // (1.1) // reset adjacent_lanelets area to 0 on attention_mask @@ -1261,7 +1282,6 @@ bool IntersectionModule::isOcclusionCleared( const double distance_min = -distance_max; const int undef_pixel = 255; const int max_cost_pixel = 254; - auto dist2pixel = [=](const double dist) { return std::min( max_cost_pixel, @@ -1270,8 +1290,8 @@ bool IntersectionModule::isOcclusionCleared( auto pixel2dist = [=](const int pixel) { return pixel * 1.0 / max_cost_pixel * (distance_max - distance_min) + distance_min; }; - const int zero_dist_pixel = dist2pixel(0.0); + const int parked_vehicle_pixel = zero_dist_pixel - 1; // magic auto coord2index = [&](const double x, const double y) { const int idx_x = (x - origin.x) / resolution; @@ -1284,6 +1304,7 @@ bool IntersectionModule::isOcclusionCleared( cv::Mat distance_grid(width, height, CV_8UC1, cv::Scalar(undef_pixel)); cv::Mat projection_ind_grid(width, height, CV_32S, cv::Scalar(-1)); + // (4.1) fill zero_dist_pixel on the path const auto [lane_start, lane_end] = lane_attention_interval_ip; for (int i = static_cast(lane_end); i >= static_cast(lane_start); i--) { const auto & path_pos = path_ip.points.at(i).point.pose.position; @@ -1295,6 +1316,31 @@ bool IntersectionModule::isOcclusionCleared( projection_ind_grid.at(height - 1 - idx_y, idx_x) = i; } + // (4.2) fill parked_vehicle_pixel to parked_vehicles (both positive and negative) + for (const auto & parked_attention_object : parked_attention_objects) { + const auto obj_poly = tier4_autoware_utils::toPolygon2d(parked_attention_object); + std::vector common_areas; + bg::intersection(obj_poly, grid_poly, common_areas); + if (common_areas.empty()) continue; + for (size_t i = 0; i < common_areas.size(); ++i) { + common_areas[i].outer().push_back(common_areas[i].outer().front()); + bg::correct(common_areas[i]); + } + std::vector> parked_attention_object_area_cv_polygons; + for (const auto & common_area : common_areas) { + std::vector parked_attention_object_area_cv_polygon; + for (const auto & p : common_area.outer()) { + const int idx_x = static_cast((p.x() - origin.x) / resolution); + const int idx_y = static_cast((p.y() - origin.y) / resolution); + parked_attention_object_area_cv_polygon.emplace_back(idx_x, height - 1 - idx_y); + } + parked_attention_object_area_cv_polygons.push_back(parked_attention_object_area_cv_polygon); + } + for (const auto & poly : parked_attention_object_area_cv_polygons) { + cv::fillPoly(distance_grid, poly, cv::Scalar(parked_vehicle_pixel), cv::LINE_AA); + } + } + for (const auto & lane_division : lane_divisions) { const auto & divisions = lane_division.divisions; for (const auto & division : divisions) { @@ -1303,8 +1349,9 @@ bool IntersectionModule::isOcclusionCleared( int projection_ind = -1; std::optional> cost_prev_checkpoint = std::nullopt; // cost, x, y, projection_ind - for (const auto & point : division) { - const auto [valid, idx_x, idx_y] = coord2index(point.x(), point.y()); + for (auto point = division.begin(); point != division.end(); point++) { + const double x = point->x(), y = point->y(); + const auto [valid, idx_x, idx_y] = coord2index(x, y); // exited grid just now if (is_in_grid && !valid) break; @@ -1327,16 +1374,27 @@ bool IntersectionModule::isOcclusionCleared( zero_dist_cell_found = true; projection_ind = projection_ind_grid.at(height - 1 - idx_y, idx_x); assert(projection_ind >= 0); - cost_prev_checkpoint = std::make_optional>( - 0.0, point.x(), point.y(), projection_ind); + cost_prev_checkpoint = + std::make_optional>(0.0, x, y, projection_ind); continue; } + // hit positive parked vehicle + if (zero_dist_cell_found && pixel == parked_vehicle_pixel) { + while (point != division.end()) { + const double x = point->x(), y = point->y(); + const auto [valid, idx_x, idx_y] = coord2index(x, y); + if (valid) occlusion_mask.at(height - 1 - idx_y, idx_x) = 0; + point++; + } + break; + } + if (zero_dist_cell_found) { // finally traversed to defined cell (first half) const auto [prev_cost, prev_checkpoint_x, prev_checkpoint_y, prev_projection_ind] = cost_prev_checkpoint.value(); - const double dy = point.y() - prev_checkpoint_y, dx = point.x() - prev_checkpoint_x; + const double dy = y - prev_checkpoint_y, dx = x - prev_checkpoint_x; double new_dist = prev_cost + std::hypot(dy, dx); const int new_projection_ind = projection_ind_grid.at(height - 1 - idx_y, idx_x); const double cur_dist = pixel2dist(pixel); @@ -1349,7 +1407,7 @@ bool IntersectionModule::isOcclusionCleared( projection_ind_grid.at(height - 1 - idx_y, idx_x) = projection_ind; distance_grid.at(height - 1 - idx_y, idx_x) = dist2pixel(new_dist); cost_prev_checkpoint = std::make_optional>( - new_dist, point.x(), point.y(), projection_ind); + new_dist, x, y, projection_ind); } } } @@ -1360,7 +1418,7 @@ bool IntersectionModule::isOcclusionCleared( const double possible_object_bbox_y = possible_object_bbox.at(1) / resolution; const double possible_object_area = possible_object_bbox_x * possible_object_bbox_y; std::vector> contours; - cv::findContours(occlusion_mask, contours, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_SIMPLE); + cv::findContours(occlusion_mask, contours, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_NONE); std::vector> valid_contours; for (const auto & contour : contours) { std::vector valid_contour; @@ -1377,11 +1435,18 @@ bool IntersectionModule::isOcclusionCleared( cv::approxPolyDP( valid_contour, approx_contour, std::round(std::min(possible_object_bbox_x, possible_object_bbox_y) / std::sqrt(2.0)), true); - if (approx_contour.size() <= 1) continue; + if (approx_contour.size() <= 2) continue; // check area const double poly_area = cv::contourArea(approx_contour); if (poly_area < possible_object_area) continue; - + // check bounding box size + const auto bbox = cv::minAreaRect(approx_contour); + if (const auto size = bbox.size; std::min(size.height, size.width) < + std::min(possible_object_bbox_x, possible_object_bbox_y) || + std::max(size.height, size.width) < + std::max(possible_object_bbox_x, possible_object_bbox_y)) { + continue; + } valid_contours.push_back(approx_contour); geometry_msgs::msg::Polygon polygon_msg; geometry_msgs::msg::Point32 point_msg; diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp index d4ef1442bb27a..5c6222b31bf54 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp @@ -108,6 +108,7 @@ class IntersectionModule : public SceneModuleInterface double max_vehicle_velocity_for_rss; double denoise_kernel; std::vector possible_object_bbox; + double ignore_parked_vehicle_speed_threshold; } occlusion; }; @@ -248,11 +249,14 @@ class IntersectionModule : public SceneModuleInterface const autoware_auto_planning_msgs::msg::PathWithLaneId & input_path, const util::IntersectionStopLines & intersection_stop_lines); - bool checkCollision( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + autoware_auto_perception_msgs::msg::PredictedObjects filterTargetObjects( const lanelet::ConstLanelets & attention_area_lanelets, const lanelet::ConstLanelets & adjacent_lanelets, - const std::optional & intersection_area, + 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 lanelet::ConstLanelets & ego_lane_with_next_lane, const int closest_idx, const double time_delay, const bool tl_arrow_solid_on); @@ -262,7 +266,10 @@ class IntersectionModule : public SceneModuleInterface const lanelet::ConstLanelets & adjacent_lanelets, const lanelet::CompoundPolygon3d & first_attention_area, const util::InterpolatedPathInfo & interpolated_path_info, - const std::vector & lane_divisions, const double occlusion_dist_thr); + const std::vector & lane_divisions, + const std::vector & + parked_attention_objects, + const double occlusion_dist_thr); /* bool IntersectionModule::checkFrontVehicleDeceleration(