From 51fd4c57bacb5cdb4ceb560d9378d104bf253e77 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Tue, 11 Jul 2023 12:06:42 +0900 Subject: [PATCH] feat(avoidance): use intersection area (#4206) * feat(avoidance): use intersection areas Signed-off-by: satoshi-ota * feat(utils): use intersection areas Signed-off-by: satoshi-ota * refactor(behavior_path_planner): clean up Signed-off-by: satoshi-ota --------- Signed-off-by: satoshi-ota --- .../config/avoidance/avoidance.param.yaml | 1 + .../behavior_path_planner/data_manager.hpp | 1 + .../utils/avoidance/avoidance_module_data.hpp | 3 + .../utils/avoidance/utils.hpp | 5 +- .../behavior_path_planner/utils/utils.hpp | 8 +- .../src/planner_manager.cpp | 4 +- .../avoidance/avoidance_module.cpp | 3 + .../src/scene_module/avoidance/manager.cpp | 1 + .../src/utils/avoidance/utils.cpp | 58 +++--- .../behavior_path_planner/src/utils/utils.cpp | 174 ++++++++++++------ .../static_centerline_optimizer/src/utils.cpp | 2 +- 11 files changed, 176 insertions(+), 84 deletions(-) diff --git a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml index f73520e8d7c40..3fd9b0ed9968b 100644 --- a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml +++ b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml @@ -20,6 +20,7 @@ enable_yield_maneuver_during_shifting: false disable_path_update: false use_hatched_road_markings: false + use_intersection_areas: false # for debug publish_debug_marker: false diff --git a/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp index 639f495a74598..5b252cef92714 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp @@ -92,6 +92,7 @@ struct DrivableAreaInfo std::vector drivable_lanes{}; std::vector obstacles{}; // obstacles to extract from the drivable area bool enable_expanding_hatched_road_markings{false}; + bool enable_expanding_intersection_areas{false}; // temporary only for pull over's freespace planning double drivable_margin{0.0}; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp index 63166b0cbf7df..4130a5a207d13 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp @@ -108,6 +108,9 @@ struct AvoidanceParameters // use hatched road markings for avoidance bool use_hatched_road_markings{false}; + // use intersection area for avoidance + bool use_intersection_areas{false}; + // constrains bool use_constraints_for_decel{false}; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp index 811587ea1c276..f63a0105d1efa 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp @@ -124,8 +124,9 @@ void filterTargetObjects( double extendToRoadShoulderDistanceWithPolygon( const std::shared_ptr & rh, const lanelet::ConstLineString3d & target_line, const double to_road_shoulder_distance, - const geometry_msgs::msg::Point & overhang_pos, - const lanelet::BasicPoint3d & overhang_basic_pose); + const lanelet::ConstLanelet & overhang_lanelet, const geometry_msgs::msg::Point & overhang_pos, + const lanelet::BasicPoint3d & overhang_basic_pose, const bool use_hatched_road_markings, + const bool use_intersection_areas); void fillAdditionalInfoFromPoint(const AvoidancePlanningData & data, AvoidLineArray & lines); diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp index ff9b340a54616..bdf4b0da6a110 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp @@ -226,7 +226,8 @@ std::vector generateDrivableLanesWithShoulderLanes( const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & shoulder_lanes); std::vector calcBound( const std::shared_ptr route_handler, - const std::vector & drivable_lanes, const bool enable_expanding_polygon, + const std::vector & drivable_lanes, + const bool enable_expanding_hatched_road_markings, const bool enable_expanding_intersection_areas, const bool is_left); boost::optional getOverlappedLaneletId(const std::vector & lanes); @@ -235,8 +236,9 @@ std::vector cutOverlappedLanes( void generateDrivableArea( PathWithLaneId & path, const std::vector & lanes, - const bool enable_expanding_polygon, const double vehicle_length, - const std::shared_ptr planner_data, const bool is_driving_forward = true); + const bool enable_expanding_hatched_road_markings, const bool enable_expanding_intersection_areas, + const double vehicle_length, const std::shared_ptr planner_data, + const bool is_driving_forward = true); void generateDrivableArea( PathWithLaneId & path, const double vehicle_length, const double offset, diff --git a/planning/behavior_path_planner/src/planner_manager.cpp b/planning/behavior_path_planner/src/planner_manager.cpp index b09bba73d9128..94d5bef5a31e9 100644 --- a/planning/behavior_path_planner/src/planner_manager.cpp +++ b/planning/behavior_path_planner/src/planner_manager.cpp @@ -154,7 +154,7 @@ void PlannerManager::generateCombinedDrivableArea( } else if (di.is_already_expanded) { // for single side shift utils::generateDrivableArea( - *output.path, di.drivable_lanes, false, data->parameters.vehicle_length, data); + *output.path, di.drivable_lanes, false, false, data->parameters.vehicle_length, data); } else { const auto shorten_lanes = utils::cutOverlappedLanes(*output.path, di.drivable_lanes); @@ -166,7 +166,7 @@ void PlannerManager::generateCombinedDrivableArea( // for other modules where multiple modules may be launched utils::generateDrivableArea( *output.path, expanded_lanes, di.enable_expanding_hatched_road_markings, - data->parameters.vehicle_length, data); + di.enable_expanding_intersection_areas, data->parameters.vehicle_length, data); } // extract obstacles from drivable area diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp index 58ef8f5fa5b3e..59552fd545ec5 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp @@ -2365,6 +2365,9 @@ void AvoidanceModule::generateExtendedDrivableArea(BehaviorModuleOutput & output // expand hatched road markings current_drivable_area_info.enable_expanding_hatched_road_markings = parameters_->use_hatched_road_markings; + // expand intersection areas + current_drivable_area_info.enable_expanding_intersection_areas = + parameters_->use_intersection_areas; output.drivable_area_info = utils::combineDrivableAreaInfo( current_drivable_area_info, getPreviousModuleOutput().drivable_area_info); diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp index 9b7a8b68b969d..179bc90adafc9 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp @@ -69,6 +69,7 @@ AvoidanceModuleManager::AvoidanceModuleManager( get_parameter(node, ns + "enable_yield_maneuver_during_shifting"); p.disable_path_update = get_parameter(node, ns + "disable_path_update"); p.use_hatched_road_markings = get_parameter(node, ns + "use_hatched_road_markings"); + p.use_intersection_areas = get_parameter(node, ns + "use_intersection_areas"); p.publish_debug_marker = get_parameter(node, ns + "publish_debug_marker"); p.print_debug_info = get_parameter(node, ns + "print_debug_info"); } diff --git a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp index 8ffd4b52e74af..bd5912ddfe425 100644 --- a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp +++ b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp @@ -908,11 +908,11 @@ void filterTargetObjects( } debug.bounds.push_back(target_line); - // update to_road_shoulder_distance with expandable polygons - if (parameters->use_hatched_road_markings) { + { o.to_road_shoulder_distance = extendToRoadShoulderDistanceWithPolygon( - rh, target_line, o.to_road_shoulder_distance, o.overhang_pose.position, - overhang_basic_pose); + rh, target_line, o.to_road_shoulder_distance, overhang_lanelet, o.overhang_pose.position, + overhang_basic_pose, parameters->use_hatched_road_markings, + parameters->use_intersection_areas); } } @@ -1137,29 +1137,43 @@ void filterTargetObjects( double extendToRoadShoulderDistanceWithPolygon( const std::shared_ptr & rh, const lanelet::ConstLineString3d & target_line, const double to_road_shoulder_distance, - const geometry_msgs::msg::Point & overhang_pos, const lanelet::BasicPoint3d & overhang_basic_pose) + const lanelet::ConstLanelet & overhang_lanelet, const geometry_msgs::msg::Point & overhang_pos, + const lanelet::BasicPoint3d & overhang_basic_pose, const bool use_hatched_road_markings, + const bool use_intersection_areas) { // get expandable polygons for avoidance (e.g. hatched road markings) std::vector expandable_polygons; - for (const auto & point : target_line) { - const auto new_polygon_candidate = utils::getPolygonByPoint(rh, point, "hatched_road_markings"); - if (!new_polygon_candidate) { - continue; - } - bool is_new_polygon{true}; - for (const auto & polygon : expandable_polygons) { - if (polygon.id() == new_polygon_candidate->id()) { - is_new_polygon = false; - break; + const auto exist_polygon = [&](const auto & candidate_polygon) { + return std::any_of( + expandable_polygons.begin(), expandable_polygons.end(), + [&](const auto & polygon) { return polygon.id() == candidate_polygon.id(); }); + }; + + if (use_hatched_road_markings) { + for (const auto & point : target_line) { + const auto new_polygon_candidate = + utils::getPolygonByPoint(rh, point, "hatched_road_markings"); + + if (!!new_polygon_candidate && !exist_polygon(*new_polygon_candidate)) { + expandable_polygons.push_back(*new_polygon_candidate); } } + } - if (is_new_polygon) { - expandable_polygons.push_back(*new_polygon_candidate); + if (use_intersection_areas) { + const std::string area_id_str = overhang_lanelet.attributeOr("intersection_area", "else"); + + if (area_id_str != "else") { + expandable_polygons.push_back( + rh->getLaneletMapPtr()->polygonLayer.get(std::atoi(area_id_str.c_str()))); } } + if (expandable_polygons.empty()) { + return to_road_shoulder_distance; + } + // calculate point laterally offset from overhang position to calculate intersection with // polygon Point lat_offset_overhang_pos; @@ -1170,7 +1184,7 @@ double extendToRoadShoulderDistanceWithPolygon( const auto closest_target_line_point = lanelet::geometry::fromArcCoordinates(target_line, arc_coordinates); - const double ratio = 10.0 / to_road_shoulder_distance; + const double ratio = 100.0 / to_road_shoulder_distance; lat_offset_overhang_pos.x = closest_target_line_point.x() + (closest_target_line_point.x() - overhang_pos.x) * ratio; lat_offset_overhang_pos.y = @@ -1197,12 +1211,8 @@ double extendToRoadShoulderDistanceWithPolygon( } std::sort(intersect_dist_vec.begin(), intersect_dist_vec.end()); - if (1 < intersect_dist_vec.size()) { - if (std::abs(updated_to_road_shoulder_distance - intersect_dist_vec.at(0)) < 1e-3) { - updated_to_road_shoulder_distance = - std::max(updated_to_road_shoulder_distance, intersect_dist_vec.at(1)); - } - } + updated_to_road_shoulder_distance = + std::max(updated_to_road_shoulder_distance, intersect_dist_vec.back()); } return updated_to_road_shoulder_distance; } diff --git a/planning/behavior_path_planner/src/utils/utils.cpp b/planning/behavior_path_planner/src/utils/utils.cpp index 6b8264cf986ad..34684a32d6edb 100644 --- a/planning/behavior_path_planner/src/utils/utils.cpp +++ b/planning/behavior_path_planner/src/utils/utils.cpp @@ -1343,8 +1343,9 @@ geometry_msgs::msg::Point calcLongitudinalOffsetGoalPoint( void generateDrivableArea( PathWithLaneId & path, const std::vector & lanes, - const bool enable_expanding_polygon, const double vehicle_length, - const std::shared_ptr planner_data, const bool is_driving_forward) + const bool enable_expanding_hatched_road_markings, const bool enable_expanding_intersection_areas, + const double vehicle_length, const std::shared_ptr planner_data, + const bool is_driving_forward) { // extract data const auto transformed_lanes = utils::transformToLanelets(lanes); @@ -1379,8 +1380,12 @@ void generateDrivableArea( }; // Insert Position - auto left_bound = calcBound(route_handler, lanes, enable_expanding_polygon, true); - auto right_bound = calcBound(route_handler, lanes, enable_expanding_polygon, false); + auto left_bound = calcBound( + route_handler, lanes, enable_expanding_hatched_road_markings, + enable_expanding_intersection_areas, true); + auto right_bound = calcBound( + route_handler, lanes, enable_expanding_hatched_road_markings, + enable_expanding_intersection_areas, false); if (left_bound.empty() || right_bound.empty()) { auto clock{rclcpp::Clock{RCL_ROS_TIME}}; @@ -1515,7 +1520,7 @@ void generateDrivableArea( // make bound longitudinally monotonic // TODO(Murooka) Fix makeBoundLongitudinallyMonotonic - if (enable_expanding_polygon) { + if (enable_expanding_hatched_road_markings || enable_expanding_intersection_areas) { makeBoundLongitudinallyMonotonic(path, true); // for left bound makeBoundLongitudinallyMonotonic(path, false); // for right bound } @@ -1524,7 +1529,8 @@ void generateDrivableArea( // calculate bounds from drivable lanes and hatched road markings std::vector calcBound( const std::shared_ptr route_handler, - const std::vector & drivable_lanes, const bool enable_expanding_polygon, + const std::vector & drivable_lanes, + const bool enable_expanding_hatched_road_markings, const bool enable_expanding_intersection_areas, const bool is_left) { // a function to convert drivable lanes to points without duplicated points @@ -1560,11 +1566,29 @@ std::vector calcBound( const auto mod = [&](const int a, const int b) { return (a + b) % b; // NOTE: consider negative value }; + const auto extract_bound_from_polygon = + [](const auto & polygon, const auto start_idx, const auto end_idx, const auto clockwise) { + std::vector ret; + for (size_t i = start_idx; i != end_idx; i = clockwise ? i + 1 : i - 1) { + ret.push_back(lanelet::utils::conversion::toGeomMsgPt(polygon[i])); + + if (i + 1 == polygon.size() && clockwise) { + i = 0; + continue; + } + + if (i == 0 && !clockwise) { + i = polygon.size() - 1; + } + } + + return ret; + }; // If no need to expand with polygons, return here. std::vector output_points; const auto bound_points = convert_to_points(drivable_lanes); - if (!enable_expanding_polygon) { + if (!enable_expanding_hatched_road_markings && !enable_expanding_intersection_areas) { for (const auto & point : bound_points) { output_points.push_back(lanelet::utils::conversion::toGeomMsgPt(point)); } @@ -1573,60 +1597,100 @@ std::vector calcBound( std::optional current_polygon{std::nullopt}; std::vector current_polygon_border_indices; - for (size_t bound_point_idx = 0; bound_point_idx < bound_points.size(); ++bound_point_idx) { - const auto & bound_point = bound_points.at(bound_point_idx); - const auto polygon = getPolygonByPoint(route_handler, bound_point, "hatched_road_markings"); - - bool will_close_polygon{false}; - if (!current_polygon) { - if (!polygon) { - output_points.push_back(lanelet::utils::conversion::toGeomMsgPt(bound_point)); - } else { - // There is a new additional polygon to expand - current_polygon = polygon; - current_polygon_border_indices.push_back( - get_corresponding_polygon_index(*current_polygon, bound_point.id())); + for (const auto & drivable_lane : drivable_lanes) { + // extract target lane and bound. + const auto bound_lane = is_left ? drivable_lane.left_lane : drivable_lane.right_lane; + const auto bound = is_left ? bound_lane.leftBound3d() : bound_lane.rightBound3d(); + + if (bound.size() < 2) { + continue; + } + + // expand drivable area by intersection areas. + const std::string id = bound_lane.attributeOr("intersection_area", "else"); + const auto use_intersection_area = enable_expanding_intersection_areas && id != "else"; + if (use_intersection_area) { + const auto polygon = + route_handler->getLaneletMapPtr()->polygonLayer.get(std::atoi(id.c_str())); + + const auto start_itr = std::find_if(polygon.begin(), polygon.end(), [&bound](const auto & p) { + return p.id() == bound.front().id(); + }); + + const auto end_itr = std::find_if(polygon.begin(), polygon.end(), [&bound](const auto & p) { + return p.id() == bound.back().id(); + }); + + if (start_itr == polygon.end() || end_itr == polygon.end()) { + continue; } - } else { - if (!polygon) { - will_close_polygon = true; - } else { - current_polygon_border_indices.push_back( - get_corresponding_polygon_index(*current_polygon, bound_point.id())); + + // extract line strings between start_idx and end_idx. + const size_t start_idx = std::distance(polygon.begin(), start_itr); + const size_t end_idx = std::distance(polygon.begin(), end_itr); + for (const auto & point : extract_bound_from_polygon(polygon, start_idx, end_idx, is_left)) { + output_points.push_back(point); } - } - if (bound_point_idx == bound_points.size() - 1 && current_polygon) { - // If drivable lanes ends earlier than polygon, close the polygon - will_close_polygon = true; + continue; } - if (will_close_polygon) { - // The current additional polygon ends to expand - if (current_polygon_border_indices.size() == 1) { - output_points.push_back(lanelet::utils::conversion::toGeomMsgPt( - (*current_polygon)[current_polygon_border_indices.front()])); + // expand drivable area by hatched road markings. + for (size_t bound_point_idx = 0; bound_point_idx < bound.size(); ++bound_point_idx) { + const auto & bound_point = bound[bound_point_idx]; + const auto polygon = getPolygonByPoint(route_handler, bound_point, "hatched_road_markings"); + + bool will_close_polygon{false}; + if (!current_polygon) { + if (!polygon) { + output_points.push_back(lanelet::utils::conversion::toGeomMsgPt(bound_point)); + } else { + // There is a new additional polygon to expand + current_polygon = polygon; + current_polygon_border_indices.push_back( + get_corresponding_polygon_index(*current_polygon, bound_point.id())); + } } else { - const size_t current_polygon_points_num = current_polygon->size(); - const bool is_polygon_opposite_direction = [&]() { - const size_t modulo_diff = mod( - static_cast(current_polygon_border_indices[1]) - - static_cast(current_polygon_border_indices[0]), - current_polygon_points_num); - return modulo_diff == 1; - }(); + if (!polygon) { + will_close_polygon = true; + } else { + current_polygon_border_indices.push_back( + get_corresponding_polygon_index(*current_polygon, bound_point.id())); + } + } + + if (bound_point_idx == bound_points.size() - 1 && current_polygon) { + // If drivable lanes ends earlier than polygon, close the polygon + will_close_polygon = true; + } - const int target_points_num = - current_polygon_points_num - current_polygon_border_indices.size() + 1; - for (int poly_idx = 0; poly_idx <= target_points_num; ++poly_idx) { - const int target_poly_idx = current_polygon_border_indices.front() + - poly_idx * (is_polygon_opposite_direction ? -1 : 1); + if (will_close_polygon) { + // The current additional polygon ends to expand + if (current_polygon_border_indices.size() == 1) { output_points.push_back(lanelet::utils::conversion::toGeomMsgPt( - (*current_polygon)[mod(target_poly_idx, current_polygon_points_num)])); + (*current_polygon)[current_polygon_border_indices.front()])); + } else { + const size_t current_polygon_points_num = current_polygon->size(); + const bool is_polygon_opposite_direction = [&]() { + const size_t modulo_diff = mod( + static_cast(current_polygon_border_indices[1]) - + static_cast(current_polygon_border_indices[0]), + current_polygon_points_num); + return modulo_diff == 1; + }(); + + const int target_points_num = + current_polygon_points_num - current_polygon_border_indices.size() + 1; + for (int poly_idx = 0; poly_idx <= target_points_num; ++poly_idx) { + const int target_poly_idx = current_polygon_border_indices.front() + + poly_idx * (is_polygon_opposite_direction ? -1 : 1); + output_points.push_back(lanelet::utils::conversion::toGeomMsgPt( + (*current_polygon)[mod(target_poly_idx, current_polygon_points_num)])); + } } + current_polygon = std::nullopt; + current_polygon_border_indices.clear(); } - current_polygon = std::nullopt; - current_polygon_border_indices.clear(); } } @@ -2523,7 +2587,8 @@ BehaviorModuleOutput getReferencePath( dp.drivable_area_types_to_skip); // for old architecture - generateDrivableArea(reference_path, expanded_lanes, false, p.vehicle_length, planner_data); + generateDrivableArea( + reference_path, expanded_lanes, false, false, p.vehicle_length, planner_data); BehaviorModuleOutput output; output.path = std::make_shared(reference_path); @@ -2917,6 +2982,11 @@ DrivableAreaInfo combineDrivableAreaInfo( drivable_area_info1.enable_expanding_hatched_road_markings || drivable_area_info2.enable_expanding_hatched_road_markings; + // enable expanding intersection areas + combined_drivable_area_info.enable_expanding_intersection_areas = + drivable_area_info1.enable_expanding_intersection_areas || + drivable_area_info2.enable_expanding_intersection_areas; + return combined_drivable_area_info; } diff --git a/planning/static_centerline_optimizer/src/utils.cpp b/planning/static_centerline_optimizer/src/utils.cpp index 3fe544c9b561e..691ae7960968c 100644 --- a/planning/static_centerline_optimizer/src/utils.cpp +++ b/planning/static_centerline_optimizer/src/utils.cpp @@ -91,7 +91,7 @@ PathWithLaneId get_path_with_lane_id( constexpr double vehicle_length = 0.0; const auto drivable_lanes = behavior_path_planner::utils::generateDrivableLanes(lanelets); behavior_path_planner::utils::generateDrivableArea( - path_with_lane_id, drivable_lanes, false, vehicle_length, planner_data); + path_with_lane_id, drivable_lanes, false, false, vehicle_length, planner_data); return path_with_lane_id; }