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 89f329508516f..79039e606252d 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 @@ -59,6 +59,13 @@ struct Approval ModuleNameStamped is_force_approved{}; }; +struct DrivableLanes +{ + lanelet::ConstLanelet right_lane; + lanelet::ConstLanelet left_lane; + lanelet::ConstLanelets middle_lanes; +}; + struct PlannerData { PoseStamped::ConstSharedPtr self_pose{}; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/util.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/util.hpp index 36aad5d6393fd..705b0564247b1 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/util.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/util.hpp @@ -135,6 +135,10 @@ TurnSignalInfo calc_turn_signal_info( void get_turn_signal_info( const LaneChangePath & lane_change_path, TurnSignalInfo * turn_signal_info); + +std::vector generateDrivableLanes( + const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes, + const lanelet::ConstLanelets & lane_change_lanes); } // namespace behavior_path_planner::lane_change_utils #endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__UTIL_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_module.hpp index 1a911ea29df70..92cd395e159dd 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_module.hpp @@ -52,7 +52,7 @@ struct PullOutStatus PathWithLaneId backward_path; lanelet::ConstLanelets current_lanes; lanelet::ConstLanelets pull_out_lanes; - lanelet::ConstLanelets lanes; + std::vector lanes; std::vector lane_follow_lane_ids; std::vector pull_out_lane_ids; bool is_safe = false; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/util.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/util.hpp index 5107d875bcdd1..4bdefc2d7a09a 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/util.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/util.hpp @@ -40,9 +40,7 @@ PathWithLaneId combineReferencePath(const PathWithLaneId path1, const PathWithLa PathWithLaneId getBackwardPath( const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes, const Pose & current_pose, const Pose & backed_pose, const double velocity); -lanelet::ConstLanelets getPullOutLanes( - const lanelet::ConstLanelets & current_lanes, - const std::shared_ptr & planner_data); +lanelet::ConstLanelets getPullOutLanes(const std::shared_ptr & planner_data); Pose getBackedPose( const Pose & current_pose, const double & yaw_shoulder_lane, const double & back_distance); } // namespace behavior_path_planner::pull_out_utils diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_module.hpp index 66e2dac960bac..38637766bf1e9 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_module.hpp @@ -59,7 +59,7 @@ struct PUllOverStatus std::shared_ptr prev_stop_path{nullptr}; lanelet::ConstLanelets current_lanes{}; lanelet::ConstLanelets pull_over_lanes{}; - lanelet::ConstLanelets lanes{}; // current + pull_over + std::vector lanes{}; // current + pull_over bool has_decided_path{false}; bool is_safe{false}; bool prev_is_safe{false}; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp index 8e865da478126..478f436e0135b 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp @@ -276,6 +276,15 @@ PredictedObjects filterObjectsByVelocity( const PredictedObjects & objects, double min_v, double max_v); // drivable area generation +lanelet::ConstLanelets transformToLanelets(const DrivableLanes & drivable_lanes); +lanelet::ConstLanelets transformToLanelets(const std::vector & drivable_lanes); +boost::optional getRightLanelet( + const lanelet::ConstLanelet & current_lane, const lanelet::ConstLanelets & shoulder_lanes); +boost::optional getLeftLanelet( + const lanelet::ConstLanelet & current_lane, const lanelet::ConstLanelets & shoulder_lanes); +std::vector generateDrivableLanes(const lanelet::ConstLanelets & current_lanes); +std::vector generateDrivableLanesWithShoulderLanes( + const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & shoulder_lanes); void occupancyGridToImage(const OccupancyGrid & occupancy_grid, cv::Mat * cv_image); @@ -285,7 +294,7 @@ cv::Point toCVPoint( const Point & geom_point, const double width_m, const double height_m, const double resolution); OccupancyGrid generateDrivableArea( - const PathWithLaneId & path, const lanelet::ConstLanelets & lanes, const double resolution, + const PathWithLaneId & path, const std::vector & lanes, const double resolution, const double vehicle_length, const std::shared_ptr planner_data); lanelet::ConstLineStrings3d getDrivableAreaForAllSharedLinestringLanelets( @@ -293,14 +302,14 @@ lanelet::ConstLineStrings3d getDrivableAreaForAllSharedLinestringLanelets( /** * @brief Expand the borders of the given lanelets - * @param [in] lanelets lanelets to expand + * @param [in] drivable_lanes lanelets to expand * @param [in] left_bound_offset [m] expansion distance of the left bound * @param [in] right_bound_offset [m] expansion distance of the right bound * @param [in] types_to_skip linestring types that will not be expanded * @return expanded lanelets */ -lanelet::ConstLanelets expandLanelets( - const lanelet::ConstLanelets & lanelets, const double left_bound_offset, +std::vector expandLanelets( + const std::vector & drivable_lanes, const double left_bound_offset, const double right_bound_offset, const std::vector & types_to_skip = {}); // goal management 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 e62c07d80cc6e..1de0d23c5505e 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 @@ -1791,24 +1791,52 @@ double AvoidanceModule::getLeftShiftBound() const // two lanes since which way to avoid is not obvious void AvoidanceModule::generateExtendedDrivableArea(ShiftedPath * shifted_path) const { + const auto has_same_lane = + [](const lanelet::ConstLanelets lanes, const lanelet::ConstLanelet & lane) { + if (lanes.empty()) return false; + const auto has_same = [&](const auto & ll) { return ll.id() == lane.id(); }; + return std::find_if(lanes.begin(), lanes.end(), has_same) != lanes.end(); + }; + const auto & route_handler = planner_data_->route_handler; const auto & current_lanes = avoidance_data_.current_lanelets; - lanelet::ConstLanelets extended_lanelets = current_lanes; + const auto & enable_opposite = parameters_->enable_avoidance_over_opposite_direction; + std::vector drivable_lanes; for (const auto & current_lane : current_lanes) { + DrivableLanes current_drivable_lanes; + current_drivable_lanes.left_lane = current_lane; + current_drivable_lanes.right_lane = current_lane; + if (!parameters_->enable_avoidance_over_same_direction) { - break; + drivable_lanes.push_back(current_drivable_lanes); + continue; } - const auto extend_from_current_lane = std::invoke( - [this, &route_handler](const lanelet::ConstLanelet & lane) { - const auto enable_opposite = parameters_->enable_avoidance_over_opposite_direction; - return route_handler->getAllSharedLineStringLanelets(lane, true, true, enable_opposite); - }, - current_lane); - extended_lanelets.reserve(extended_lanelets.size() + extend_from_current_lane.size()); - extended_lanelets.insert( - extended_lanelets.end(), extend_from_current_lane.begin(), extend_from_current_lane.end()); + // get left side lane + const lanelet::ConstLanelets all_left_lanelets = + route_handler->getAllLeftSharedLinestringLanelets(current_lane, enable_opposite); + if (!all_left_lanelets.empty()) { + current_drivable_lanes.left_lane = all_left_lanelets.back(); // leftmost lanelet + + for (int i = all_left_lanelets.size() - 2; i >= 0; --i) { + current_drivable_lanes.middle_lanes.push_back(all_left_lanelets.at(i)); + } + } + + // get right side lane + const lanelet::ConstLanelets all_right_lanelets = + route_handler->getAllRightSharedLinestringLanelets(current_lane, enable_opposite); + if (!all_right_lanelets.empty()) { + current_drivable_lanes.right_lane = all_right_lanelets.back(); // rightmost lanelet + if (current_drivable_lanes.left_lane.id() != current_lane.id()) { + current_drivable_lanes.middle_lanes.push_back(current_lane); + } + + for (size_t i = 0; i < all_right_lanelets.size() - 1; ++i) { + current_drivable_lanes.middle_lanes.push_back(all_right_lanelets.at(i)); + } + } // 2. when there are multiple turning lanes whose previous lanelet is the same in // intersection @@ -1837,31 +1865,43 @@ void AvoidanceModule::generateExtendedDrivableArea(ShiftedPath * shifted_path) c // 2.1 look for neighbour lane, where end line of the lane is connected to end line of the // original lane - std::copy_if( - next_lanes_from_intersection.begin(), next_lanes_from_intersection.end(), - std::back_inserter(extended_lanelets), - [¤t_lane](const lanelet::ConstLanelet & neighbor_lane) { - const auto & next_left_back_point_2d = neighbor_lane.leftBound2d().back().basicPoint(); - const auto & next_right_back_point_2d = neighbor_lane.rightBound2d().back().basicPoint(); - - const auto & orig_left_back_point_2d = current_lane.leftBound2d().back().basicPoint(); - const auto & orig_right_back_point_2d = current_lane.rightBound2d().back().basicPoint(); - constexpr double epsilon = 1e-5; - const bool is_neighbour_lane = - (next_left_back_point_2d - orig_right_back_point_2d).norm() < epsilon || - (next_right_back_point_2d - orig_left_back_point_2d).norm() < epsilon; - return (current_lane.id() != neighbor_lane.id() && is_neighbour_lane); - }); - } - - extended_lanelets = util::expandLanelets( - extended_lanelets, parameters_->drivable_area_left_bound_offset, + for (const auto & next_lane : next_lanes_from_intersection) { + if (current_lane.id() == next_lane.id()) { + continue; + } + constexpr double epsilon = 1e-5; + const auto & next_left_back_point_2d = next_lane.leftBound2d().back().basicPoint(); + const auto & next_right_back_point_2d = next_lane.rightBound2d().back().basicPoint(); + const auto & orig_left_back_point_2d = current_lane.leftBound2d().back().basicPoint(); + const auto & orig_right_back_point_2d = current_lane.rightBound2d().back().basicPoint(); + + if ((next_right_back_point_2d - orig_left_back_point_2d).norm() < epsilon) { + current_drivable_lanes.left_lane = next_lane; + if ( + current_drivable_lanes.right_lane.id() != current_lane.id() && + !has_same_lane(current_drivable_lanes.middle_lanes, current_lane)) { + current_drivable_lanes.middle_lanes.push_back(current_lane); + } + } else if ( + (next_left_back_point_2d - orig_right_back_point_2d).norm() < epsilon && + !has_same_lane(current_drivable_lanes.middle_lanes, current_lane)) { + current_drivable_lanes.right_lane = next_lane; + if (current_drivable_lanes.left_lane.id() != current_lane.id()) { + current_drivable_lanes.middle_lanes.push_back(current_lane); + } + } + } + drivable_lanes.push_back(current_drivable_lanes); + } + + drivable_lanes = util::expandLanelets( + drivable_lanes, parameters_->drivable_area_left_bound_offset, parameters_->drivable_area_right_bound_offset, {"road_border"}); { const auto & p = planner_data_->parameters; shifted_path->path.drivable_area = util::generateDrivableArea( - shifted_path->path, extended_lanelets, p.drivable_area_resolution, p.vehicle_length, + shifted_path->path, drivable_lanes, p.drivable_area_resolution, p.vehicle_length, planner_data_); } } diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp index 9fb964b4905ba..0f9dddfdda779 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp @@ -170,17 +170,16 @@ BehaviorModuleOutput LaneChangeModule::plan() // Generate drivable area { const auto & common_parameters = planner_data_->parameters; - lanelet::ConstLanelets lanes; - lanes.reserve(status_.current_lanes.size() + status_.lane_change_lanes.size()); - lanes.insert(lanes.end(), status_.current_lanes.begin(), status_.current_lanes.end()); - lanes.insert(lanes.end(), status_.lane_change_lanes.begin(), status_.lane_change_lanes.end()); - lanes = util::expandLanelets( - lanes, parameters_->drivable_area_left_bound_offset, + const auto & route_handler = planner_data_->route_handler; + const auto drivable_lanes = lane_change_utils::generateDrivableLanes( + *route_handler, status_.current_lanes, status_.lane_change_lanes); + const auto expanded_lanes = util::expandLanelets( + drivable_lanes, parameters_->drivable_area_left_bound_offset, parameters_->drivable_area_right_bound_offset); const double & resolution = common_parameters.drivable_area_resolution; path.drivable_area = util::generateDrivableArea( - path, lanes, resolution, common_parameters.vehicle_length, planner_data_); + path, expanded_lanes, resolution, common_parameters.vehicle_length, planner_data_); } if (isAbortConditionSatisfied()) { @@ -329,12 +328,13 @@ PathWithLaneId LaneChangeModule::getReferencePath() const *route_handler, reference_path, current_lanes, parameters_->lane_change_prepare_duration, lane_change_buffer); - const auto current_extended_lanes = util::expandLanelets( - current_lanes, parameters_->drivable_area_left_bound_offset, + const auto drivable_lanes = util::generateDrivableLanes(current_lanes); + const auto expanded_lanes = util::expandLanelets( + drivable_lanes, parameters_->drivable_area_left_bound_offset, parameters_->drivable_area_right_bound_offset); reference_path.drivable_area = util::generateDrivableArea( - reference_path, current_extended_lanes, common_parameters.drivable_area_resolution, + reference_path, expanded_lanes, common_parameters.drivable_area_resolution, common_parameters.vehicle_length, planner_data_); return reference_path; diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp index cdad57e466c88..5beaa0a39bb9a 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp @@ -627,4 +627,36 @@ void get_turn_signal_info( turn_signal_info->desired_end_point = lane_change_path.turn_signal_info.desired_end_point; } +std::vector generateDrivableLanes( + const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes, + const lanelet::ConstLanelets & lane_change_lanes) +{ + std::vector drivable_lanes(current_lanes.size()); + for (size_t i = 0; i < current_lanes.size(); ++i) { + const auto & current_lane = current_lanes.at(i); + drivable_lanes.at(i).left_lane = current_lane; + drivable_lanes.at(i).right_lane = current_lane; + + const auto left_lane = route_handler.getLeftLanelet(current_lane); + const auto right_lane = route_handler.getRightLanelet(current_lane); + if (!left_lane && !right_lane) { + continue; + } + + for (const auto & lc_lane : lane_change_lanes) { + if (left_lane && lc_lane.id() == left_lane->id()) { + drivable_lanes.at(i).left_lane = lc_lane; + break; + } + + if (right_lane && lc_lane.id() == right_lane->id()) { + drivable_lanes.at(i).right_lane = lc_lane; + break; + } + } + } + + return drivable_lanes; +} + } // namespace behavior_path_planner::lane_change_utils diff --git a/planning/behavior_path_planner/src/scene_module/lane_following/lane_following_module.cpp b/planning/behavior_path_planner/src/scene_module/lane_following/lane_following_module.cpp index d9863fe72293f..c05c06d893d45 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_following/lane_following_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_following/lane_following_module.cpp @@ -93,8 +93,9 @@ PathWithLaneId LaneFollowingModule::getReferencePath() const } // For current_lanes with desired length - lanelet::ConstLanelets current_lanes = planner_data_->route_handler->getLaneletSequence( + const auto current_lanes = planner_data_->route_handler->getLaneletSequence( current_lane, current_pose, p.backward_path_length, p.forward_path_length); + const auto drivable_lanes = util::generateDrivableLanes(current_lanes); if (current_lanes.empty()) { return reference_path; @@ -136,12 +137,12 @@ PathWithLaneId LaneFollowingModule::getReferencePath() const lane_change_buffer); } - current_lanes = util::expandLanelets( - current_lanes, parameters_.drivable_area_left_bound_offset, + const auto expanded_lanes = util::expandLanelets( + drivable_lanes, parameters_.drivable_area_left_bound_offset, parameters_.drivable_area_right_bound_offset); reference_path.drivable_area = util::generateDrivableArea( - reference_path, current_lanes, p.drivable_area_resolution, p.vehicle_length, planner_data_); + reference_path, expanded_lanes, p.drivable_area_resolution, p.vehicle_length, planner_data_); return reference_path; } diff --git a/planning/behavior_path_planner/src/scene_module/pull_out/geometric_pull_out.cpp b/planning/behavior_path_planner/src/scene_module/pull_out/geometric_pull_out.cpp index b8dde0300de16..76ccead98268e 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_out/geometric_pull_out.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_out/geometric_pull_out.cpp @@ -38,7 +38,7 @@ boost::optional GeometricPullOut::plan(Pose start_pose, Pose goal_p // combine road lane and shoulder lane const auto road_lanes = util::getExtendedCurrentLanes(planner_data_); - const auto shoulder_lanes = getPullOutLanes(road_lanes, planner_data_); + const auto shoulder_lanes = getPullOutLanes(planner_data_); auto lanes = road_lanes; lanes.insert(lanes.end(), shoulder_lanes.begin(), shoulder_lanes.end()); 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 e67c20577b0f5..fc0317a0a8125 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 @@ -123,7 +123,7 @@ bool PullOutModule::isExecutionRequested() const // Check if ego is not out of lanes const auto current_lanes = util::getExtendedCurrentLanes(planner_data_); - const auto pull_out_lanes = pull_out_utils::getPullOutLanes(current_lanes, planner_data_); + const auto pull_out_lanes = pull_out_utils::getPullOutLanes(planner_data_); auto lanes = current_lanes; lanes.insert(lanes.end(), pull_out_lanes.begin(), pull_out_lanes.end()); if (LaneDepartureChecker::isOutOfLane(lanes, vehicle_footprint)) { @@ -184,6 +184,7 @@ BehaviorModuleOutput PullOutModule::plan() } else { path = status_.backward_path; } + const auto expanded_lanes = util::expandLanelets( status_.lanes, parameters_.drivable_area_left_bound_offset, parameters_.drivable_area_right_bound_offset); @@ -276,17 +277,17 @@ BehaviorModuleOutput PullOutModule::planWaitingApproval() BehaviorModuleOutput output; const auto current_lanes = util::getExtendedCurrentLanes(planner_data_); - const auto pull_out_lanes = pull_out_utils::getPullOutLanes(current_lanes, planner_data_); - auto lanes = current_lanes; - lanes.insert(lanes.end(), pull_out_lanes.begin(), pull_out_lanes.end()); + const auto pull_out_lanes = pull_out_utils::getPullOutLanes(planner_data_); + const auto drivable_lanes = + util::generateDrivableLanesWithShoulderLanes(current_lanes, pull_out_lanes); - lanes = util::expandLanelets( - lanes, parameters_.drivable_area_left_bound_offset, + const auto expanded_lanes = util::expandLanelets( + drivable_lanes, parameters_.drivable_area_left_bound_offset, parameters_.drivable_area_right_bound_offset); auto candidate_path = status_.back_finished ? getCurrentPath() : status_.backward_path; candidate_path.drivable_area = util::generateDrivableArea( - candidate_path, lanes, planner_data_->parameters.drivable_area_resolution, + candidate_path, expanded_lanes, planner_data_->parameters.drivable_area_resolution, planner_data_->parameters.vehicle_length, planner_data_); auto stop_path = candidate_path; for (auto & p : stop_path.points) { @@ -433,12 +434,11 @@ void PullOutModule::updatePullOutStatus() const auto & goal_pose = planner_data_->route_handler->getGoalPose(); status_.current_lanes = util::getExtendedCurrentLanes(planner_data_); - status_.pull_out_lanes = pull_out_utils::getPullOutLanes(status_.current_lanes, planner_data_); + status_.pull_out_lanes = pull_out_utils::getPullOutLanes(planner_data_); // combine road and shoulder lanes - status_.lanes = status_.current_lanes; - status_.lanes.insert( - status_.lanes.end(), status_.pull_out_lanes.begin(), status_.pull_out_lanes.end()); + status_.lanes = + util::generateDrivableLanesWithShoulderLanes(status_.current_lanes, status_.pull_out_lanes); // search pull out start candidates backward std::vector start_pose_candidates; diff --git a/planning/behavior_path_planner/src/scene_module/pull_out/shift_pull_out.cpp b/planning/behavior_path_planner/src/scene_module/pull_out/shift_pull_out.cpp index d8849e9bc2b0c..77e6899609711 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_out/shift_pull_out.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_out/shift_pull_out.cpp @@ -42,14 +42,14 @@ boost::optional ShiftPullOut::plan(Pose start_pose, Pose goal_pose) const auto & common_parameters = planner_data_->parameters; const auto & dynamic_objects = planner_data_->dynamic_object; const auto & road_lanes = util::getExtendedCurrentLanes(planner_data_); - const auto & shoulder_lanes = getPullOutLanes(road_lanes, planner_data_); + const auto & shoulder_lanes = getPullOutLanes(planner_data_); if (shoulder_lanes.empty()) { return boost::none; } - lanelet::ConstLanelets lanes; - lanes.insert(lanes.end(), road_lanes.begin(), road_lanes.end()); - lanes.insert(lanes.end(), shoulder_lanes.begin(), shoulder_lanes.end()); + const auto drivable_lanes = + util::generateDrivableLanesWithShoulderLanes(road_lanes, shoulder_lanes); + const auto lanes = util::transformToLanelets(drivable_lanes); // find candidate paths auto pull_out_paths = calcPullOutPaths( @@ -93,7 +93,7 @@ boost::optional ShiftPullOut::plan(Pose start_pose, Pose goal_pose) // Generate drivable area const double resolution = common_parameters.drivable_area_resolution; shift_path.drivable_area = util::generateDrivableArea( - shift_path, lanes, resolution, common_parameters.vehicle_length, planner_data_); + shift_path, drivable_lanes, resolution, common_parameters.vehicle_length, planner_data_); shift_path.header = planner_data_->route_handler->getRouteHeader(); diff --git a/planning/behavior_path_planner/src/scene_module/pull_out/util.cpp b/planning/behavior_path_planner/src/scene_module/pull_out/util.cpp index 8d3858a495a98..9ec243d2484de 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_out/util.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_out/util.cpp @@ -94,28 +94,22 @@ Pose getBackedPose( } // getShoulderLanesOnCurrentPose -lanelet::ConstLanelets getPullOutLanes( - const lanelet::ConstLanelets & current_lanes, - const std::shared_ptr & planner_data) +lanelet::ConstLanelets getPullOutLanes(const std::shared_ptr & planner_data) { const double pull_out_lane_length = 200.0; - lanelet::ConstLanelets shoulder_lanes; + const double & vehicle_width = planner_data->parameters.vehicle_width; const auto & route_handler = planner_data->route_handler; const auto current_pose = planner_data->self_pose->pose; - lanelet::ConstLanelet shoulder_lane; - - if (current_lanes.empty()) { - return shoulder_lanes; - } + lanelet::ConstLanelet current_shoulder_lane; + lanelet::ConstLanelets shoulder_lanes; if (route_handler->getPullOutStartLane( - route_handler->getShoulderLanelets(), current_pose, planner_data->parameters.vehicle_width, - &shoulder_lane)) { + route_handler->getShoulderLanelets(), current_pose, vehicle_width, + ¤t_shoulder_lane)) { shoulder_lanes = route_handler->getShoulderLaneletSequence( - shoulder_lane, current_pose, pull_out_lane_length, pull_out_lane_length); + current_shoulder_lane, current_pose, pull_out_lane_length, pull_out_lane_length); } return shoulder_lanes; } - } // namespace behavior_path_planner::pull_out_utils diff --git a/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp b/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp index ae24d40754835..80d18b7e85c72 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp @@ -349,11 +349,8 @@ BehaviorModuleOutput PullOverModule::plan() status_.current_lanes = util::getExtendedCurrentLanes(planner_data_); status_.pull_over_lanes = pull_over_utils::getPullOverLanes(*(planner_data_->route_handler)); - status_.lanes = lanelet::ConstLanelets{}; - status_.lanes.insert( - status_.lanes.end(), status_.current_lanes.begin(), status_.current_lanes.end()); - status_.lanes.insert( - status_.lanes.end(), status_.pull_over_lanes.begin(), status_.pull_over_lanes.end()); + status_.lanes = + util::generateDrivableLanesWithShoulderLanes(status_.current_lanes, status_.pull_over_lanes); // Check if it needs to decide path if (status_.is_safe) { @@ -613,8 +610,9 @@ PathWithLaneId PullOverModule::getReferencePath() const reference_path, parameters_.pull_over_velocity, search_start_pose, -calcMinimumShiftPathDistance(), parameters_.deceleration_interval); + const auto drivable_lanes = util::generateDrivableLanes(status_.current_lanes); const auto lanes = util::expandLanelets( - status_.current_lanes, parameters_.drivable_area_left_bound_offset, + drivable_lanes, parameters_.drivable_area_left_bound_offset, parameters_.drivable_area_right_bound_offset); reference_path.drivable_area = util::generateDrivableArea( @@ -661,8 +659,9 @@ PathWithLaneId PullOverModule::generateStopPath() const } } + const auto drivable_lanes = util::generateDrivableLanes(status_.current_lanes); const auto lanes = util::expandLanelets( - status_.current_lanes, parameters_.drivable_area_left_bound_offset, + drivable_lanes, parameters_.drivable_area_left_bound_offset, parameters_.drivable_area_right_bound_offset); stop_path.drivable_area = util::generateDrivableArea( 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 1da538089f46b..77315ba631ac8 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 @@ -415,12 +415,13 @@ void SideShiftModule::adjustDrivableArea(ShiftedPath * path) const *itr.first - (*itr.first < -threshold ? margin : 0.0), -parameters_.drivable_area_right_bound_offset); - const auto extended_lanelets = util::expandLanelets(current_lanelets_, left_offset, right_offset); + const auto drivable_lanes = util::generateDrivableLanes(current_lanelets_); + const auto expanded_lanes = util::expandLanelets(drivable_lanes, left_offset, right_offset); { const auto & p = planner_data_->parameters; path->path.drivable_area = util::generateDrivableArea( - path->path, extended_lanelets, p.drivable_area_resolution, p.vehicle_length, planner_data_); + path->path, expanded_lanes, p.drivable_area_resolution, p.vehicle_length, planner_data_); } } diff --git a/planning/behavior_path_planner/src/utilities.cpp b/planning/behavior_path_planner/src/utilities.cpp index 133111cc75738..3e430a7c8284a 100644 --- a/planning/behavior_path_planner/src/utilities.cpp +++ b/planning/behavior_path_planner/src/utilities.cpp @@ -1102,6 +1102,107 @@ bool containsGoal(const lanelet::ConstLanelets & lanes, const lanelet::Id & goal return false; } +lanelet::ConstLanelets transformToLanelets(const DrivableLanes & drivable_lanes) +{ + lanelet::ConstLanelets lanes; + + const auto has_same_lane = [&](const auto & lane) { + if (lanes.empty()) return false; + const auto has_same = [&](const auto & ll) { return ll.id() == lane.id(); }; + return std::find_if(lanes.begin(), lanes.end(), has_same) != lanes.end(); + }; + + lanes.push_back(drivable_lanes.right_lane); + if (!has_same_lane(drivable_lanes.left_lane)) { + lanes.push_back(drivable_lanes.left_lane); + } + + for (const auto & ml : drivable_lanes.middle_lanes) { + if (!has_same_lane(ml)) { + lanes.push_back(ml); + } + } + + return lanes; +} + +lanelet::ConstLanelets transformToLanelets(const std::vector & drivable_lanes) +{ + lanelet::ConstLanelets lanes; + + for (const auto & drivable_lane : drivable_lanes) { + const auto transformed_lane = transformToLanelets(drivable_lane); + lanes.insert(lanes.end(), transformed_lane.begin(), transformed_lane.end()); + } + + return lanes; +} + +boost::optional getRightLanelet( + const lanelet::ConstLanelet & current_lane, const lanelet::ConstLanelets & shoulder_lanes) +{ + for (const auto & shoulder_lane : shoulder_lanes) { + if (shoulder_lane.leftBound().id() == current_lane.rightBound().id()) { + return shoulder_lane; + } + } + + return {}; +} + +boost::optional getLeftLanelet( + const lanelet::ConstLanelet & current_lane, const lanelet::ConstLanelets & shoulder_lanes) +{ + for (const auto & shoulder_lane : shoulder_lanes) { + if (shoulder_lane.rightBound().id() == current_lane.leftBound().id()) { + return shoulder_lane; + } + } + + return {}; +} + +std::vector generateDrivableLanes(const lanelet::ConstLanelets & lanes) +{ + std::vector drivable_lanes(lanes.size()); + for (size_t i = 0; i < lanes.size(); ++i) { + drivable_lanes.at(i).left_lane = lanes.at(i); + drivable_lanes.at(i).right_lane = lanes.at(i); + } + return drivable_lanes; +} + +std::vector generateDrivableLanesWithShoulderLanes( + const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & shoulder_lanes) +{ + std::vector drivable_lanes; + for (const auto & current_lane : current_lanes) { + DrivableLanes drivable_lane; + + const auto right_lane = getRightLanelet(current_lane, shoulder_lanes); + const auto left_lane = getLeftLanelet(current_lane, shoulder_lanes); + + if (right_lane && left_lane) { + drivable_lane.right_lane = *right_lane; + drivable_lane.left_lane = *left_lane; + drivable_lane.middle_lanes.push_back(current_lane); + } else if (right_lane) { + drivable_lane.right_lane = *right_lane; + drivable_lane.left_lane = current_lane; + } else if (left_lane) { + drivable_lane.right_lane = current_lane; + drivable_lane.left_lane = *left_lane; + } else { + drivable_lane.right_lane = current_lane; + drivable_lane.left_lane = current_lane; + } + + drivable_lanes.push_back(drivable_lane); + } + + return drivable_lanes; +} + // input lanes must be in sequence // NOTE: lanes in the path argument is used to calculate the size of the drivable area to cover // designated forward and backward length by getPathScope function. @@ -1109,9 +1210,10 @@ bool containsGoal(const lanelet::ConstLanelets & lanes, const lanelet::Id & goal // This is because lanes argument has multiple parallel lanes which makes hard to calculate // the size of the drivable area OccupancyGrid generateDrivableArea( - const PathWithLaneId & path, const lanelet::ConstLanelets & lanes, const double resolution, + const PathWithLaneId & path, const std::vector & lanes, const double resolution, const double vehicle_length, const std::shared_ptr planner_data) { + const auto transformed_lanes = util::transformToLanelets(lanes); const auto & params = planner_data->parameters; const auto route_handler = planner_data->route_handler; const auto current_pose = planner_data->self_pose; @@ -1143,10 +1245,10 @@ OccupancyGrid generateDrivableArea( drivable_lanes.end(), lanes_before_current_pose.begin(), lanes_before_current_pose.end()); // 2. add lanes - drivable_lanes.insert(drivable_lanes.end(), lanes.begin(), lanes.end()); + drivable_lanes.insert(drivable_lanes.end(), transformed_lanes.begin(), transformed_lanes.end()); // 3. add succeeding lanes after goal - if (containsGoal(lanes, route_handler->getGoalLaneId())) { + if (containsGoal(transformed_lanes, route_handler->getGoalLaneId())) { const auto lanes_after_goal = route_handler->getLanesAfterGoal(vehicle_length); drivable_lanes.insert(drivable_lanes.end(), lanes_after_goal.begin(), lanes_after_goal.end()); } @@ -1656,13 +1758,19 @@ std::shared_ptr generateCenterLinePath( lanelet::ConstLanelets lanelet_sequence = route_handler->getLaneletSequence( current_lane, pose->pose, p.backward_path_length, p.forward_path_length); + std::vector drivable_lanes(lanelet_sequence.size()); + for (size_t i = 0; i < lanelet_sequence.size(); ++i) { + drivable_lanes.at(i).left_lane = lanelet_sequence.at(i); + drivable_lanes.at(i).right_lane = lanelet_sequence.at(i); + } + *centerline_path = getCenterLinePath( *route_handler, lanelet_sequence, pose->pose, p.backward_path_length, p.forward_path_length, p); centerline_path->header = route_handler->getRouteHeader(); centerline_path->drivable_area = util::generateDrivableArea( - *centerline_path, lanelet_sequence, p.drivable_area_resolution, p.vehicle_length, planner_data); + *centerline_path, drivable_lanes, p.drivable_area_resolution, p.vehicle_length, planner_data); return centerline_path; } @@ -1695,19 +1803,19 @@ lanelet::ConstLineStrings3d getDrivableAreaForAllSharedLinestringLanelets( return linestring_shared; } -lanelet::ConstLanelets expandLanelets( - const lanelet::ConstLanelets & lanelets, const double left_bound_offset, +std::vector expandLanelets( + const std::vector & drivable_lanes, const double left_bound_offset, const double right_bound_offset, const std::vector & types_to_skip) { - if (left_bound_offset == 0.0 && right_bound_offset == 0.0) return lanelets; + if (left_bound_offset == 0.0 && right_bound_offset == 0.0) return drivable_lanes; - lanelet::ConstLanelets expanded_lanelets{}; - expanded_lanelets.reserve(lanelets.size()); - for (const auto & lanelet : lanelets) { + std::vector expanded_drivable_lanes{}; + expanded_drivable_lanes.reserve(drivable_lanes.size()); + for (const auto & lanes : drivable_lanes) { const std::string l_type = - lanelet.leftBound().attributeOr(lanelet::AttributeName::Type, "none"); + lanes.left_lane.leftBound().attributeOr(lanelet::AttributeName::Type, "none"); const std::string r_type = - lanelet.rightBound().attributeOr(lanelet::AttributeName::Type, "none"); + lanes.right_lane.rightBound().attributeOr(lanelet::AttributeName::Type, "none"); const bool l_skip = std::find(types_to_skip.begin(), types_to_skip.end(), l_type) != types_to_skip.end(); @@ -1716,9 +1824,21 @@ lanelet::ConstLanelets expandLanelets( const double l_offset = l_skip ? 0.0 : left_bound_offset; const double r_offset = r_skip ? 0.0 : -right_bound_offset; - expanded_lanelets.push_back(lanelet::utils::getExpandedLanelet(lanelet, l_offset, r_offset)); + DrivableLanes expanded_lanes; + if (lanes.left_lane.id() == lanes.right_lane.id()) { + expanded_lanes.left_lane = + lanelet::utils::getExpandedLanelet(lanes.left_lane, l_offset, r_offset); + expanded_lanes.right_lane = + lanelet::utils::getExpandedLanelet(lanes.left_lane, l_offset, r_offset); + } else { + expanded_lanes.left_lane = lanelet::utils::getExpandedLanelet(lanes.left_lane, l_offset, 0.0); + expanded_lanes.right_lane = + lanelet::utils::getExpandedLanelet(lanes.left_lane, 0.0, r_offset); + } + expanded_lanes.middle_lanes = lanes.middle_lanes; + expanded_drivable_lanes.push_back(expanded_lanes); } - return expanded_lanelets; + return expanded_drivable_lanes; } PredictedObjects filterObjectsByVelocity(const PredictedObjects & objects, double lim_v) diff --git a/planning/behavior_path_planner/test/test_utilities.cpp b/planning/behavior_path_planner/test/test_utilities.cpp index da66c60c74e00..2b7cc8b13b57b 100644 --- a/planning/behavior_path_planner/test/test_utilities.cpp +++ b/planning/behavior_path_planner/test/test_utilities.cpp @@ -84,8 +84,9 @@ TEST(BehaviorPathPlanningUtilitiesBehaviorTest, setGoal) TEST(BehaviorPathPlanningUtilitiesBehaviorTest, expandLanelets) { + using behavior_path_planner::DrivableLanes; using behavior_path_planner::util::expandLanelets; - lanelet::ConstLanelets original_lanelets; + std::vector original_lanelets; { // empty list of lanelets, empty output const auto expanded_lanelets = expandLanelets(original_lanelets, 0.0, 0.0); ASSERT_TRUE(expanded_lanelets.empty()); @@ -101,19 +102,34 @@ TEST(BehaviorPathPlanningUtilitiesBehaviorTest, expandLanelets) lanelet.rightBound().push_back(lanelet::Point3d(lanelet::Id(), 1.0, 1.0)); lanelet.rightBound().push_back(lanelet::Point3d(lanelet::Id(), 1.0, 2.0)); lanelet.rightBound().push_back(lanelet::Point3d(lanelet::Id(), 1.0, 3.0)); - original_lanelets.push_back(lanelet); + DrivableLanes drivable_lane; + drivable_lane.left_lane = lanelet; + drivable_lane.right_lane = lanelet; + original_lanelets.push_back(drivable_lane); { // no offsets, unchanged output const auto expanded_lanelets = expandLanelets(original_lanelets, left_bound, right_bound); ASSERT_EQ(expanded_lanelets.size(), original_lanelets.size()); - ASSERT_EQ(expanded_lanelets[0].leftBound().size(), original_lanelets[0].leftBound().size()); - ASSERT_EQ(expanded_lanelets[0].rightBound().size(), original_lanelets[0].rightBound().size()); - for (size_t i = 0; i < expanded_lanelets[0].leftBound().size(); ++i) { - ASSERT_EQ(expanded_lanelets[0].leftBound()[i].x(), original_lanelets[0].leftBound()[i].x()); - ASSERT_EQ(expanded_lanelets[0].leftBound()[i].y(), original_lanelets[0].leftBound()[i].y()); + ASSERT_EQ( + expanded_lanelets[0].left_lane.leftBound().size(), + original_lanelets[0].left_lane.leftBound().size()); + ASSERT_EQ( + expanded_lanelets[0].right_lane.rightBound().size(), + original_lanelets[0].right_lane.rightBound().size()); + for (size_t i = 0; i < expanded_lanelets[0].left_lane.leftBound().size(); ++i) { + ASSERT_EQ( + expanded_lanelets[0].left_lane.leftBound()[i].x(), + original_lanelets[0].left_lane.leftBound()[i].x()); + ASSERT_EQ( + expanded_lanelets[0].left_lane.leftBound()[i].y(), + original_lanelets[0].left_lane.leftBound()[i].y()); } - for (size_t i = 0; i < expanded_lanelets[0].rightBound().size(); ++i) { - ASSERT_EQ(expanded_lanelets[0].rightBound()[i].x(), original_lanelets[0].rightBound()[i].x()); - ASSERT_EQ(expanded_lanelets[0].rightBound()[i].y(), original_lanelets[0].rightBound()[i].y()); + for (size_t i = 0; i < expanded_lanelets[0].right_lane.rightBound().size(); ++i) { + ASSERT_EQ( + expanded_lanelets[0].right_lane.rightBound()[i].x(), + original_lanelets[0].right_lane.rightBound()[i].x()); + ASSERT_EQ( + expanded_lanelets[0].right_lane.rightBound()[i].y(), + original_lanelets[0].right_lane.rightBound()[i].y()); } } left_bound = 0.5; @@ -123,9 +139,10 @@ TEST(BehaviorPathPlanningUtilitiesBehaviorTest, expandLanelets) expandLanelets(original_lanelets, left_bound, right_bound, {"road_border"}); ASSERT_EQ(expanded_lanelets.size(), original_lanelets.size()); const auto l_dist = lanelet::geometry::distance2d( - expanded_lanelets[0].leftBound2d(), original_lanelets[0].leftBound2d()); + expanded_lanelets[0].left_lane.leftBound2d(), original_lanelets[0].left_lane.leftBound2d()); const auto r_dist = lanelet::geometry::distance2d( - expanded_lanelets[0].rightBound2d(), original_lanelets[0].rightBound2d()); + expanded_lanelets[0].right_lane.rightBound2d(), + original_lanelets[0].right_lane.rightBound2d()); EXPECT_NEAR(l_dist, 0.0, 1E-03); EXPECT_NEAR(r_dist, 0.0, 1E-03); } @@ -133,9 +150,10 @@ TEST(BehaviorPathPlanningUtilitiesBehaviorTest, expandLanelets) const auto expanded_lanelets = expandLanelets(original_lanelets, left_bound, right_bound); ASSERT_EQ(expanded_lanelets.size(), original_lanelets.size()); const auto l_dist = lanelet::geometry::distance2d( - expanded_lanelets[0].leftBound2d(), original_lanelets[0].leftBound2d()); + expanded_lanelets[0].left_lane.leftBound2d(), original_lanelets[0].left_lane.leftBound2d()); const auto r_dist = lanelet::geometry::distance2d( - expanded_lanelets[0].rightBound2d(), original_lanelets[0].rightBound2d()); + expanded_lanelets[0].right_lane.rightBound2d(), + original_lanelets[0].right_lane.rightBound2d()); EXPECT_NEAR(l_dist, left_bound, 1E-03); EXPECT_NEAR(r_dist, right_bound, 1E-03); } diff --git a/planning/static_centerline_optimizer/src/utils.cpp b/planning/static_centerline_optimizer/src/utils.cpp index 692b369fac1fe..bd01977734019 100644 --- a/planning/static_centerline_optimizer/src/utils.cpp +++ b/planning/static_centerline_optimizer/src/utils.cpp @@ -94,8 +94,9 @@ PathWithLaneId get_path_with_lane_id( // generate drivable area and store it in path with lane id constexpr double drivable_area_resolution = 0.1; constexpr double vehicle_length = 0.0; + const auto drivable_lanes = behavior_path_planner::util::generateDrivableLanes(lanelets); path_with_lane_id.drivable_area = behavior_path_planner::util::generateDrivableArea( - path_with_lane_id, lanelets, drivable_area_resolution, vehicle_length, planner_data); + path_with_lane_id, drivable_lanes, drivable_area_resolution, vehicle_length, planner_data); return path_with_lane_id; }