From 992d82b96bcfdb95fd4641871ace9fc483a72f37 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Tue, 9 May 2023 12:48:21 +0900 Subject: [PATCH 1/3] feat(map_loader): visualize hatched road markings (#3639) * feat(map_loader): visualize hatched road markings Signed-off-by: Takayuki Murooka * update Signed-off-by: Takayuki Murooka --------- Signed-off-by: Takayuki Murooka --- .../lanelet2_map_visualization_node.cpp | 12 +++++++++++- 1 file changed, 11 insertions(+), 1 deletion(-) diff --git a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp index 6e94294c0900f..fad15948c368f 100644 --- a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp +++ b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp @@ -121,12 +121,15 @@ void Lanelet2MapVisualizationNode::onMapBin( lanelet::ConstPolygons3d no_obstacle_segmentation_area_for_run_out = lanelet::utils::query::getAllPolygonsByType( viz_lanelet_map, "no_obstacle_segmentation_area_for_run_out"); + lanelet::ConstPolygons3d hatched_road_markings_area = + lanelet::utils::query::getAllPolygonsByType(viz_lanelet_map, "hatched_road_markings"); std_msgs::msg::ColorRGBA cl_road, cl_shoulder, cl_cross, cl_partitions, cl_pedestrian_markings, cl_ll_borders, cl_shoulder_borders, cl_stoplines, cl_trafficlights, cl_detection_areas, cl_speed_bumps, cl_parking_lots, cl_parking_spaces, cl_lanelet_id, cl_obstacle_polygons, cl_no_stopping_areas, cl_no_obstacle_segmentation_area, - cl_no_obstacle_segmentation_area_for_run_out; + cl_no_obstacle_segmentation_area_for_run_out, cl_hatched_road_markings_area, + cl_hatched_road_markings_line; setColor(&cl_road, 0.27, 0.27, 0.27, 0.999); setColor(&cl_shoulder, 0.15, 0.15, 0.15, 0.999); setColor(&cl_cross, 0.27, 0.3, 0.27, 0.5); @@ -145,6 +148,8 @@ void Lanelet2MapVisualizationNode::onMapBin( setColor(&cl_lanelet_id, 0.5, 0.5, 0.5, 0.999); setColor(&cl_no_obstacle_segmentation_area, 0.37, 0.37, 0.27, 0.5); setColor(&cl_no_obstacle_segmentation_area_for_run_out, 0.37, 0.7, 0.27, 0.5); + setColor(&cl_hatched_road_markings_area, 0.3, 0.3, 0.3, 0.5); + setColor(&cl_hatched_road_markings_line, 0.5, 0.5, 0.5, 0.999); visualization_msgs::msg::MarkerArray map_marker_array; @@ -219,6 +224,11 @@ void Lanelet2MapVisualizationNode::onMapBin( lanelet::visualization::noObstacleSegmentationAreaForRunOutAsMarkerArray( no_obstacle_segmentation_area_for_run_out, cl_no_obstacle_segmentation_area_for_run_out)); + insertMarkerArray( + &map_marker_array, + lanelet::visualization::hatchedRoadMarkingsAreaAsMarkerArray( + hatched_road_markings_area, cl_hatched_road_markings_area, cl_hatched_road_markings_line)); + pub_marker_->publish(map_marker_array); } From 6781a5a6ee4dd42c1bb78a0d85aa3ee1ac18d202 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Tue, 9 May 2023 15:06:32 +0900 Subject: [PATCH 2/3] feat(behavior_path_planner): expand hatched road markings (#3604) * expand hatched road markings Signed-off-by: Takayuki Murooka * update Signed-off-by: Takayuki Murooka * add a flag Signed-off-by: Takayuki Murooka * update Signed-off-by: Takayuki Murooka * fix Signed-off-by: Takayuki Murooka * fix static_centerline_optimizer Signed-off-by: Takayuki Murooka * fix Signed-off-by: Takayuki Murooka * fix Signed-off-by: Takayuki Murooka --------- Signed-off-by: Takayuki Murooka --- .../config/avoidance/avoidance.param.yaml | 1 + .../behavior_path_planner/data_manager.hpp | 1 + .../utils/avoidance/avoidance_module_data.hpp | 3 + .../behavior_path_planner/utils/utils.hpp | 13 +- .../src/behavior_path_planner_node.cpp | 1 + .../src/planner_manager.cpp | 20 +- .../avoidance/avoidance_module.cpp | 5 +- .../goal_planner/goal_planner_module.cpp | 7 +- .../src/scene_module/lane_change/normal.cpp | 4 +- .../scene_module/pull_out/pull_out_module.cpp | 6 +- .../side_shift/side_shift_module.cpp | 3 +- .../src/utils/pull_out/shift_pull_out.cpp | 2 +- .../behavior_path_planner/src/utils/utils.cpp | 220 +++++++++++++++++- .../static_centerline_optimizer/src/utils.cpp | 2 +- 14 files changed, 255 insertions(+), 33 deletions(-) diff --git a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml index 71a98a65baf89..cd11d85d702e5 100644 --- a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml +++ b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml @@ -18,6 +18,7 @@ enable_safety_check: true enable_yield_maneuver: true disable_path_update: false + use_hatched_road_markings: 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 f48e82a9e2af6..37bb66376798e 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 @@ -89,6 +89,7 @@ struct DrivableAreaInfo }; std::vector drivable_lanes; std::vector obstacles; // obstacles to extract from the drivable area + bool enable_expanding_hatched_road_markings{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 e5c25081474f4..e502fb2aaa3e4 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 @@ -94,6 +94,9 @@ struct AvoidanceParameters // disable path update bool disable_path_update{false}; + // use hatched road markings for avoidance + bool use_hatched_road_markings{false}; + // constrains bool use_constraints_for_decel{false}; 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 edb30eb883737..a3b371d746154 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 @@ -229,13 +229,18 @@ boost::optional getLeftLanelet( std::vector generateDrivableLanes(const lanelet::ConstLanelets & current_lanes); 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 bool is_left); boost::optional getOverlappedLaneletId(const std::vector & lanes); std::vector cutOverlappedLanes( PathWithLaneId & path, const std::vector & lanes); void generateDrivableArea( - PathWithLaneId & path, const std::vector & lanes, const double vehicle_length, + 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); void generateDrivableArea( @@ -388,6 +393,12 @@ std::vector combineDrivableLanes( void extractObstaclesFromDrivableArea( PathWithLaneId & path, const std::vector & obstacles); + +void makeBoundLongitudinallyMonotonic(PathWithLaneId & path, const bool is_bound_left); + +std::optional getPolygonByPoint( + const std::shared_ptr & route_handler, const lanelet::ConstPoint3d & point, + const std::string & polygon_name); } // namespace behavior_path_planner::utils #endif // BEHAVIOR_PATH_PLANNER__UTILS__UTILS_HPP_ diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index 7adb135da3f61..899e0a8b58480 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -525,6 +525,7 @@ AvoidanceParameters BehaviorPathPlannerNode::getAvoidanceParam() p.enable_safety_check = declare_parameter(ns + "enable_safety_check"); p.enable_yield_maneuver = declare_parameter(ns + "enable_yield_maneuver"); p.disable_path_update = declare_parameter(ns + "disable_path_update"); + p.use_hatched_road_markings = declare_parameter(ns + "use_hatched_road_markings"); p.publish_debug_marker = declare_parameter(ns + "publish_debug_marker"); p.print_debug_info = declare_parameter(ns + "print_debug_info"); } diff --git a/planning/behavior_path_planner/src/planner_manager.cpp b/planning/behavior_path_planner/src/planner_manager.cpp index a718f97f20d82..a765e9f21e3a6 100644 --- a/planning/behavior_path_planner/src/planner_manager.cpp +++ b/planning/behavior_path_planner/src/planner_manager.cpp @@ -129,23 +129,22 @@ BehaviorModuleOutput PlannerManager::run(const std::shared_ptr & da void PlannerManager::generateCombinedDrivableArea( BehaviorModuleOutput & output, const std::shared_ptr & data) const { + const auto & di = output.drivable_area_info; constexpr double epsilon = 1e-3; - if (epsilon < std::abs(output.drivable_area_info.drivable_margin)) { + + if (epsilon < std::abs(di.drivable_margin)) { // for single free space pull over const auto is_driving_forward_opt = motion_utils::isDrivingForward(output.path->points); const bool is_driving_forward = is_driving_forward_opt ? *is_driving_forward_opt : true; utils::generateDrivableArea( - *output.path, data->parameters.vehicle_length, output.drivable_area_info.drivable_margin, - is_driving_forward); - } else if (output.drivable_area_info.is_already_expanded) { + *output.path, data->parameters.vehicle_length, di.drivable_margin, is_driving_forward); + } else if (di.is_already_expanded) { // for single side shift utils::generateDrivableArea( - *output.path, output.drivable_area_info.drivable_lanes, data->parameters.vehicle_length, - data); + *output.path, di.drivable_lanes, false, data->parameters.vehicle_length, data); } else { - const auto shorten_lanes = - utils::cutOverlappedLanes(*output.path, output.drivable_area_info.drivable_lanes); + const auto shorten_lanes = utils::cutOverlappedLanes(*output.path, di.drivable_lanes); const auto & dp = data->drivable_area_expansion_parameters; const auto expanded_lanes = utils::expandLanelets( @@ -154,11 +153,12 @@ void PlannerManager::generateCombinedDrivableArea( // for other modules where multiple modules may be launched utils::generateDrivableArea( - *output.path, expanded_lanes, data->parameters.vehicle_length, data); + *output.path, expanded_lanes, di.enable_expanding_hatched_road_markings, + data->parameters.vehicle_length, data); } // extract obstacles from drivable area - utils::extractObstaclesFromDrivableArea(*output.path, output.drivable_area_info.obstacles); + utils::extractObstaclesFromDrivableArea(*output.path, di.obstacles); } std::vector PlannerManager::getRequestModules( 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 e15c5294fd5cf..7576709ee0d7a 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 @@ -2529,12 +2529,15 @@ void AvoidanceModule::generateExtendedDrivableArea(BehaviorModuleOutput & output // generate obstacle polygons output.drivable_area_info.obstacles = utils::avoidance::generateObstaclePolygonsForDrivableArea( avoidance_data_.target_objects, parameters_, planner_data_->parameters.vehicle_width / 2.0); + // expand hatched road markings + output.drivable_area_info.enable_expanding_hatched_road_markings = + parameters_->use_hatched_road_markings; } { // for old architecture // NOTE: Obstacles to avoid are not extracted from the drivable area with an old architecture. utils::generateDrivableArea( - *output.path, drivable_lanes, planner_data_->parameters.vehicle_length, planner_data_); + *output.path, drivable_lanes, false, planner_data_->parameters.vehicle_length, planner_data_); } } diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp index f2a3610c7d56f..d83b2056e6e91 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp @@ -639,7 +639,8 @@ BehaviorModuleOutput GoalPlannerModule::planWithGoalModification() utils::clipPathLength(path, ego_idx, planner_data_->parameters); const auto target_drivable_lanes = getNonOverlappingExpandedLanes(path, status_.lanes); utils::generateDrivableArea( - path, target_drivable_lanes, planner_data_->parameters.vehicle_length, planner_data_); + path, target_drivable_lanes, false, planner_data_->parameters.vehicle_length, + planner_data_); } } @@ -918,7 +919,7 @@ PathWithLaneId GoalPlannerModule::generateStopPath() const auto drivable_lanes = utils::generateDrivableLanes(status_.current_lanes); const auto target_drivable_lanes = getNonOverlappingExpandedLanes(reference_path, drivable_lanes); utils::generateDrivableArea( - reference_path, target_drivable_lanes, common_parameters.vehicle_length, planner_data_); + reference_path, target_drivable_lanes, false, common_parameters.vehicle_length, planner_data_); return reference_path; } @@ -953,7 +954,7 @@ PathWithLaneId GoalPlannerModule::generateFeasibleStopPath() const auto drivable_lanes = utils::generateDrivableLanes(status_.current_lanes); const auto target_drivable_lanes = getNonOverlappingExpandedLanes(stop_path, drivable_lanes); utils::generateDrivableArea( - stop_path, target_drivable_lanes, common_parameters.vehicle_length, planner_data_); + stop_path, target_drivable_lanes, false, common_parameters.vehicle_length, planner_data_); return stop_path; } diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp index ac81db8552492..3d14c52fb475c 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp @@ -134,7 +134,7 @@ void NormalLaneChange::extendOutputDrivableArea(BehaviorModuleOutput & output) // for old architecture utils::generateDrivableArea( - *output.path, expanded_lanes, common_parameters.vehicle_length, planner_data_); + *output.path, expanded_lanes, false, common_parameters.vehicle_length, planner_data_); } bool NormalLaneChange::hasFinishedLaneChange() const @@ -688,7 +688,7 @@ PathWithLaneId NormalLaneChangeBT::getReferencePath() const shorten_lanes, dp.drivable_area_left_bound_offset, dp.drivable_area_right_bound_offset, dp.drivable_area_types_to_skip); utils::generateDrivableArea( - reference_path, expanded_lanes, common_parameters.vehicle_length, planner_data_); + reference_path, expanded_lanes, false, common_parameters.vehicle_length, planner_data_); return reference_path; } diff --git a/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp b/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp index 1ace845a18c5d..e3b08f40a6386 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp @@ -199,7 +199,7 @@ BehaviorModuleOutput PullOutModule::plan() const auto target_drivable_lanes = getNonOverlappingExpandedLanes(path, status_.lanes); utils::generateDrivableArea( - path, target_drivable_lanes, planner_data_->parameters.vehicle_length, planner_data_); + path, target_drivable_lanes, false, planner_data_->parameters.vehicle_length, planner_data_); output.drivable_area_info.drivable_lanes = utils::combineDrivableLanes( getPreviousModuleOutput().drivable_area_info.drivable_lanes, target_drivable_lanes); @@ -317,7 +317,7 @@ BehaviorModuleOutput PullOutModule::planWaitingApproval() drivable_lanes, dp.drivable_area_left_bound_offset, dp.drivable_area_right_bound_offset, dp.drivable_area_types_to_skip); utils::generateDrivableArea( - stop_path, expanded_lanes, planner_data_->parameters.vehicle_length, planner_data_); + stop_path, expanded_lanes, false, planner_data_->parameters.vehicle_length, planner_data_); for (auto & p : stop_path.points) { p.point.longitudinal_velocity_mps = 0.0; } @@ -517,7 +517,7 @@ PathWithLaneId PullOutModule::generateStopPath() const // for old architecture utils::generateDrivableArea( - path, target_drivable_lanes, planner_data_->parameters.vehicle_length, planner_data_); + path, target_drivable_lanes, false, planner_data_->parameters.vehicle_length, planner_data_); return path; } diff --git a/planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp b/planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp index 43892e0a19821..6cf7f04e6c06e 100644 --- a/planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp @@ -442,7 +442,8 @@ BehaviorModuleOutput SideShiftModule::adjustDrivableArea(const ShiftedPath & pat utils::expandLanelets(shorten_lanes, left_offset, right_offset, dp.drivable_area_types_to_skip); { // for old architecture - utils::generateDrivableArea(output_path, expanded_lanes, p.vehicle_length, planner_data_); + utils::generateDrivableArea( + output_path, expanded_lanes, false, p.vehicle_length, planner_data_); out.path = std::make_shared(output_path); } diff --git a/planning/behavior_path_planner/src/utils/pull_out/shift_pull_out.cpp b/planning/behavior_path_planner/src/utils/pull_out/shift_pull_out.cpp index f47f0ed1f631c..0a8866a6978ef 100644 --- a/planning/behavior_path_planner/src/utils/pull_out/shift_pull_out.cpp +++ b/planning/behavior_path_planner/src/utils/pull_out/shift_pull_out.cpp @@ -113,7 +113,7 @@ boost::optional ShiftPullOut::plan(Pose start_pose, Pose goal_pose) // NOTE: drivable_area_info is assigned outside this function. const auto shorten_lanes = utils::cutOverlappedLanes(shift_path, drivable_lanes); utils::generateDrivableArea( - shift_path, shorten_lanes, common_parameters.vehicle_length, planner_data_); + shift_path, shorten_lanes, false, common_parameters.vehicle_length, planner_data_); shift_path.header = planner_data_->route_handler->getRouteHeader(); diff --git a/planning/behavior_path_planner/src/utils/utils.cpp b/planning/behavior_path_planner/src/utils/utils.cpp index 0b8236bb61739..51de220f7fe42 100644 --- a/planning/behavior_path_planner/src/utils/utils.cpp +++ b/planning/behavior_path_planner/src/utils/utils.cpp @@ -443,6 +443,21 @@ std::vector updateBoundary( } } // namespace drivable_area_processing +std::optional getPolygonByPoint( + const std::shared_ptr & route_handler, const lanelet::ConstPoint3d & point, + const std::string & polygon_name) +{ + const auto polygons = route_handler->getLaneletMapPtr()->polygonLayer.findUsages(point); + for (const auto & polygon : polygons) { + const std::string type = polygon.attributeOr(lanelet::AttributeName::Type, "none"); + if (type == polygon_name) { + // NOTE: If there are multiple polygons on a point, only the front one is used. + return polygon; + } + } + return std::nullopt; +} + double l2Norm(const Vector3 vector) { return std::sqrt(std::pow(vector.x, 2) + std::pow(vector.y, 2) + std::pow(vector.z, 2)); @@ -1228,12 +1243,10 @@ geometry_msgs::msg::Point calcLongitudinalOffsetGoalPoint( } void generateDrivableArea( - PathWithLaneId & path, const std::vector & lanes, const double vehicle_length, + 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) { - std::vector left_bound; - std::vector right_bound; - // extract data const auto transformed_lanes = utils::transformToLanelets(lanes); const auto current_pose = planner_data->self_odometry->pose.pose; @@ -1267,10 +1280,8 @@ void generateDrivableArea( }; // Insert Position - for (const auto & lane : lanes) { - addPoints(lane.left_lane.leftBound3d(), left_bound); - addPoints(lane.right_lane.rightBound3d(), right_bound); - } + auto left_bound = calcBound(route_handler, lanes, enable_expanding_polygon, true); + auto right_bound = calcBound(route_handler, lanes, enable_expanding_polygon, false); // Insert points after goal lanelet::ConstLanelet goal_lanelet; @@ -1385,6 +1396,194 @@ void generateDrivableArea( path, expansion_params, *planner_data->dynamic_object, *planner_data->route_handler, transformed_lanes); } + + // make bound longitudinally monotonic + makeBoundLongitudinallyMonotonic(path, true); // for left bound + makeBoundLongitudinallyMonotonic(path, false); // for right bound +} + +// 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 bool is_left) +{ + // a function to convert drivable lanes to points without duplicated points + const auto convert_to_points = [&](const std::vector & drivable_lanes) { + constexpr double overlap_threshold = 0.01; + + std::vector points; + for (const auto & drivable_lane : drivable_lanes) { + const auto bound = + is_left ? drivable_lane.left_lane.leftBound3d() : drivable_lane.right_lane.rightBound3d(); + for (const auto & point : bound) { + if ( + points.empty() || + overlap_threshold < (points.back().basicPoint2d() - point.basicPoint2d()).norm()) { + points.push_back(point); + } + } + } + return points; + }; + // a function to get polygon with a designated id + const auto get_corresponding_polygon_index = [&](const auto & polygon, const auto & target_id) { + for (size_t poly_idx = 0; poly_idx < polygon.size(); ++poly_idx) { + if (polygon[poly_idx].id() == target_id) { + // NOTE: If there are duplicated points in polygon, the early one will be returned. + return poly_idx; + } + } + // This means calculation has some errors. + return polygon.size() - 1; + }; + const auto mod = [&](const int a, const int b) { + return (a + b) % b; // NOTE: consider negative value + }; + + // 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) { + for (const auto & point : bound_points) { + output_points.push_back(lanelet::utils::conversion::toGeomMsgPt(point)); + } + return motion_utils::removeOverlapPoints(output_points); + } + + std::optional current_polygon{std::nullopt}; + std::vector current_polygon_border_indices; + for (size_t point_idx = 0; point_idx < bound_points.size(); ++point_idx) { + const auto & point = bound_points.at(point_idx); + const auto polygon = getPolygonByPoint(route_handler, point, "hatched_road_markings"); + + bool will_close_polygon{false}; + if (!current_polygon) { + if (!polygon) { + output_points.push_back(lanelet::utils::conversion::toGeomMsgPt(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, point.id())); + } + } else { + if (!polygon) { + will_close_polygon = true; + } else { + current_polygon_border_indices.push_back( + get_corresponding_polygon_index(*current_polygon, point.id())); + } + } + + if (point_idx == bound_points.size() - 1 && current_polygon) { + // If drivable lanes ends earlier than polygon, close the polygon + will_close_polygon = true; + } + + if (will_close_polygon) { + // The current additional polygon ends to expand + 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(); + } + } + + return motion_utils::removeOverlapPoints(output_points); +} + +void makeBoundLongitudinallyMonotonic(PathWithLaneId & path, const bool is_bound_left) +{ + if (path.points.empty()) { + return; + } + + // define a function to remove non monotonic point on bound + const auto remove_non_monotonic_point = + [&](std::vector original_bound, const bool is_reversed) { + if (is_reversed) { + std::reverse(original_bound.begin(), original_bound.end()); + } + + const bool is_points_left = is_reversed ? !is_bound_left : is_bound_left; + + std::vector monotonic_bound; + size_t b_idx = 0; + while (true) { + if (original_bound.size() <= b_idx) { + break; + } + monotonic_bound.push_back(original_bound.at(b_idx)); + + // caculate bound pose and its laterally offset pose. + const auto bound_pose = [&]() { + geometry_msgs::msg::Pose pose; + pose.position = original_bound.at(b_idx); + const size_t nearest_idx = + motion_utils::findNearestIndex(path.points, original_bound.at(b_idx)); + pose.orientation = path.points.at(nearest_idx).point.pose.orientation; + return pose; + }(); + // NOTE: is_bound_left is used instead of is_points_left since orientation of path point is + // opposite. + const double lat_offset = is_bound_left ? 20.0 : -20.0; + const auto bound_pose_with_lat_offset = + tier4_autoware_utils::calcOffsetPose(bound_pose, 0.0, lat_offset, 0.0); + + // skip non monotonic points + for (size_t candidate_idx = b_idx + 1; candidate_idx < original_bound.size() - 1; + ++candidate_idx) { + const auto intersect_point = drivable_area_processing::intersect( + bound_pose.position, bound_pose_with_lat_offset.position, + original_bound.at(candidate_idx), original_bound.at(candidate_idx + 1)); + + if (intersect_point) { + const double theta = tier4_autoware_utils::normalizeRadian( + tier4_autoware_utils::calcAzimuthAngle( + bound_pose.position, original_bound.at(candidate_idx)) - + tier4_autoware_utils::calcAzimuthAngle( + bound_pose.position, bound_pose_with_lat_offset.position)); + if ((is_points_left && 0 < theta) || (!is_points_left && theta < 0)) { + monotonic_bound.push_back(*intersect_point); + b_idx = candidate_idx; + break; + } + } + } + + ++b_idx; + } + + if (is_reversed) { + std::reverse(monotonic_bound.begin(), monotonic_bound.end()); + } + return monotonic_bound; + }; + + auto & bound = is_bound_left ? path.left_bound : path.right_bound; + const auto original_bound = bound; + const auto half_monotonic_bound = + remove_non_monotonic_point(original_bound, true); // for reverse + const auto full_monotonic_bound = + remove_non_monotonic_point(half_monotonic_bound, false); // for not reverse + + bound = full_monotonic_bound; } // generate drivable area by expanding path for freespace @@ -1967,7 +2166,8 @@ std::shared_ptr generateCenterLinePath( centerline_path->header = route_handler->getRouteHeader(); - utils::generateDrivableArea(*centerline_path, drivable_lanes, p.vehicle_length, planner_data); + utils::generateDrivableArea( + *centerline_path, drivable_lanes, false, p.vehicle_length, planner_data); return centerline_path; } @@ -2217,7 +2417,7 @@ BehaviorModuleOutput getReferencePath( dp.drivable_area_types_to_skip); // for old architecture - generateDrivableArea(reference_path, expanded_lanes, p.vehicle_length, planner_data); + generateDrivableArea(reference_path, expanded_lanes, false, p.vehicle_length, planner_data); BehaviorModuleOutput output; output.path = std::make_shared(reference_path); diff --git a/planning/static_centerline_optimizer/src/utils.cpp b/planning/static_centerline_optimizer/src/utils.cpp index 22fcb924597cf..3fe544c9b561e 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, vehicle_length, planner_data); + path_with_lane_id, drivable_lanes, false, vehicle_length, planner_data); return path_with_lane_id; } From f127f2e3074c4f1eae3a3958ff7ad8170d45a51f Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Tue, 9 May 2023 16:26:41 +0900 Subject: [PATCH 3/3] feat(behavior_path_planner): combine drivable area info from multiple modules (#3647) * feat(behavior_path_planner): combine drivable area info from multiple modules Signed-off-by: Takayuki Murooka * fix Signed-off-by: Takayuki Murooka --------- Signed-off-by: Takayuki Murooka --- .../behavior_path_planner/utils/utils.hpp | 3 +++ .../avoidance/avoidance_module.cpp | 14 +++++++---- .../goal_planner/goal_planner_module.cpp | 14 ++++++++--- .../src/scene_module/lane_change/normal.cpp | 9 ++++++- .../scene_module/pull_out/pull_out_module.cpp | 21 +++++++++++----- .../behavior_path_planner/src/utils/utils.cpp | 25 +++++++++++++++++++ 6 files changed, 70 insertions(+), 16 deletions(-) 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 a3b371d746154..baad95096dd91 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 @@ -391,6 +391,9 @@ std::vector combineDrivableLanes( const std::vector & original_drivable_lanes_vec, const std::vector & new_drivable_lanes_vec); +DrivableAreaInfo combineDrivableAreaInfo( + const DrivableAreaInfo & drivable_area_info1, const DrivableAreaInfo & drivable_area_info2); + void extractObstaclesFromDrivableArea( PathWithLaneId & path, const std::vector & obstacles); 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 7576709ee0d7a..aa64e2741e52d 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 @@ -2523,15 +2523,19 @@ void AvoidanceModule::generateExtendedDrivableArea(BehaviorModuleOutput & output } { // for new architecture + DrivableAreaInfo current_drivable_area_info; // generate drivable lanes - output.drivable_area_info.drivable_lanes = utils::combineDrivableLanes( - getPreviousModuleOutput().drivable_area_info.drivable_lanes, drivable_lanes); + current_drivable_area_info.drivable_lanes = drivable_lanes; // generate obstacle polygons - output.drivable_area_info.obstacles = utils::avoidance::generateObstaclePolygonsForDrivableArea( - avoidance_data_.target_objects, parameters_, planner_data_->parameters.vehicle_width / 2.0); + current_drivable_area_info.obstacles = + utils::avoidance::generateObstaclePolygonsForDrivableArea( + avoidance_data_.target_objects, parameters_, planner_data_->parameters.vehicle_width / 2.0); // expand hatched road markings - output.drivable_area_info.enable_expanding_hatched_road_markings = + current_drivable_area_info.enable_expanding_hatched_road_markings = parameters_->use_hatched_road_markings; + + output.drivable_area_info = utils::combineDrivableAreaInfo( + current_drivable_area_info, getPreviousModuleOutput().drivable_area_info); } { // for old architecture diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp index d83b2056e6e91..59cf23562582d 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp @@ -692,8 +692,11 @@ BehaviorModuleOutput GoalPlannerModule::planWithGoalModification() planner_data_->parameters.vehicle_width / 2.0 + drivable_area_margin; } else { const auto target_drivable_lanes = getNonOverlappingExpandedLanes(*output.path, status_.lanes); - output.drivable_area_info.drivable_lanes = utils::combineDrivableLanes( - getPreviousModuleOutput().drivable_area_info.drivable_lanes, target_drivable_lanes); + + DrivableAreaInfo current_drivable_area_info; + current_drivable_area_info.drivable_lanes = target_drivable_lanes; + output.drivable_area_info = utils::combineDrivableAreaInfo( + current_drivable_area_info, getPreviousModuleOutput().drivable_area_info); } // return to lane parking if it is possible @@ -819,8 +822,11 @@ BehaviorModuleOutput GoalPlannerModule::planWaitingApprovalWithGoalModification( planner_data_->parameters.vehicle_width / 2.0 + drivable_area_margin; } else { const auto target_drivable_lanes = getNonOverlappingExpandedLanes(*out.path, status_.lanes); - out.drivable_area_info.drivable_lanes = utils::combineDrivableLanes( - getPreviousModuleOutput().drivable_area_info.drivable_lanes, target_drivable_lanes); + + DrivableAreaInfo current_drivable_area_info; + current_drivable_area_info.drivable_lanes = target_drivable_lanes; + out.drivable_area_info = utils::combineDrivableAreaInfo( + current_drivable_area_info, getPreviousModuleOutput().drivable_area_info); } return out; diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp index 3d14c52fb475c..19d612f2bc671 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp @@ -130,7 +130,14 @@ void NormalLaneChange::extendOutputDrivableArea(BehaviorModuleOutput & output) dp.drivable_area_types_to_skip); // for new architecture - output.drivable_area_info.drivable_lanes = expanded_lanes; + DrivableAreaInfo current_drivable_area_info; + current_drivable_area_info.drivable_lanes = expanded_lanes; + if (prev_drivable_area_info_) { + output.drivable_area_info = + utils::combineDrivableAreaInfo(current_drivable_area_info, *prev_drivable_area_info_); + } else { + output.drivable_area_info = current_drivable_area_info; + } // for old architecture utils::generateDrivableArea( diff --git a/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp b/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp index e3b08f40a6386..e1ad7da1781af 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp @@ -179,7 +179,12 @@ BehaviorModuleOutput PullOutModule::plan() // the path of getCurrent() is generated by generateStopPath() const PathWithLaneId stop_path = getCurrentPath(); output.path = std::make_shared(stop_path); - output.drivable_area_info.drivable_lanes = status_.lanes; + + DrivableAreaInfo current_drivable_area_info; + current_drivable_area_info.drivable_lanes = status_.lanes; + output.drivable_area_info = utils::combineDrivableAreaInfo( + current_drivable_area_info, getPreviousModuleOutput().drivable_area_info); + output.reference_path = getPreviousModuleOutput().reference_path; path_candidate_ = std::make_shared(stop_path); path_reference_ = getPreviousModuleOutput().reference_path; @@ -200,11 +205,13 @@ BehaviorModuleOutput PullOutModule::plan() const auto target_drivable_lanes = getNonOverlappingExpandedLanes(path, status_.lanes); utils::generateDrivableArea( path, target_drivable_lanes, false, planner_data_->parameters.vehicle_length, planner_data_); - output.drivable_area_info.drivable_lanes = utils::combineDrivableLanes( - getPreviousModuleOutput().drivable_area_info.drivable_lanes, target_drivable_lanes); + + DrivableAreaInfo current_drivable_area_info; + current_drivable_area_info.drivable_lanes = target_drivable_lanes; + output.drivable_area_info = utils::combineDrivableAreaInfo( + current_drivable_area_info, getPreviousModuleOutput().drivable_area_info); output.path = std::make_shared(path); - output.drivable_area_info.drivable_lanes = status_.lanes; output.reference_path = getPreviousModuleOutput().reference_path; output.turn_signal_info = calcTurnSignalInfo(); path_candidate_ = std::make_shared(getFullPath()); @@ -322,8 +329,10 @@ BehaviorModuleOutput PullOutModule::planWaitingApproval() p.point.longitudinal_velocity_mps = 0.0; } - output.drivable_area_info.drivable_lanes = utils::combineDrivableLanes( - getPreviousModuleOutput().drivable_area_info.drivable_lanes, expanded_lanes); + DrivableAreaInfo current_drivable_area_info; + current_drivable_area_info.drivable_lanes = expanded_lanes; + output.drivable_area_info = utils::combineDrivableAreaInfo( + current_drivable_area_info, getPreviousModuleOutput().drivable_area_info); output.path = std::make_shared(stop_path); output.drivable_area_info.drivable_lanes = status_.lanes; diff --git a/planning/behavior_path_planner/src/utils/utils.cpp b/planning/behavior_path_planner/src/utils/utils.cpp index 51de220f7fe42..3750c11f38bff 100644 --- a/planning/behavior_path_planner/src/utils/utils.cpp +++ b/planning/behavior_path_planner/src/utils/utils.cpp @@ -2762,6 +2762,31 @@ std::vector combineDrivableLanes( return updated_drivable_lanes_vec; } +DrivableAreaInfo combineDrivableAreaInfo( + const DrivableAreaInfo & drivable_area_info1, const DrivableAreaInfo & drivable_area_info2) +{ + DrivableAreaInfo combined_drivable_area_info; + + // drivable lanes + combined_drivable_area_info.drivable_lanes = + combineDrivableLanes(drivable_area_info1.drivable_lanes, drivable_area_info2.drivable_lanes); + + // obstacles + for (const auto & obstacle : drivable_area_info1.obstacles) { + combined_drivable_area_info.obstacles.push_back(obstacle); + } + for (const auto & obstacle : drivable_area_info2.obstacles) { + combined_drivable_area_info.obstacles.push_back(obstacle); + } + + // enable expanding hatched road markings + combined_drivable_area_info.enable_expanding_hatched_road_markings = + drivable_area_info1.enable_expanding_hatched_road_markings || + drivable_area_info2.enable_expanding_hatched_road_markings; + + return combined_drivable_area_info; +} + // NOTE: Assuming that path.right/left_bound is already created. void extractObstaclesFromDrivableArea( PathWithLaneId & path, const std::vector & obstacles)