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 07acd61fb40db..1fa2d5b1edef0 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp @@ -193,8 +193,8 @@ cv::Point toCVPoint( const Point & geom_point, const double width_m, const double height_m, const double resolution); OccupancyGrid generateDrivableArea( - const lanelet::ConstLanelets & lanes, const double resolution, const double vehicle_length, - const std::shared_ptr planner_data); + const PathWithLaneId & path, const lanelet::ConstLanelets & lanes, const double resolution, + const double vehicle_length, const std::shared_ptr planner_data); lanelet::ConstLineStrings3d getDrivableAreaForAllSharedLinestringLanelets( const std::shared_ptr & planner_data); 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 9221961015fe3..91b26839d4a8b 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 @@ -1891,7 +1891,8 @@ void AvoidanceModule::generateExtendedDrivableArea(ShiftedPath * shifted_path) c { const auto & p = planner_data_->parameters; shifted_path->path.drivable_area = util::generateDrivableArea( - extended_lanelets, p.drivable_area_resolution, p.vehicle_length, planner_data_); + shifted_path->path, extended_lanelets, 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 fba45e56619d5..892d6952031a1 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 @@ -152,7 +152,7 @@ BehaviorModuleOutput LaneChangeModule::plan() const double resolution = common_parameters.drivable_area_resolution; path.drivable_area = util::generateDrivableArea( - lanes, resolution, common_parameters.vehicle_length, planner_data_); + path, lanes, resolution, common_parameters.vehicle_length, planner_data_); } if (isAbortConditionSatisfied()) { @@ -288,8 +288,8 @@ PathWithLaneId LaneChangeModule::getReferencePath() const lane_change_buffer); reference_path.drivable_area = util::generateDrivableArea( - current_lanes, common_parameters.drivable_area_resolution, common_parameters.vehicle_length, - planner_data_); + reference_path, current_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_following/lane_following_module.cpp b/planning/behavior_path_planner/src/scene_module/lane_following/lane_following_module.cpp index d3edf66a6693f..a5e2688e75c7b 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 @@ -142,7 +142,7 @@ PathWithLaneId LaneFollowingModule::getReferencePath() const } reference_path.drivable_area = util::generateDrivableArea( - current_lanes, p.drivable_area_resolution, p.vehicle_length, planner_data_); + reference_path, current_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/pull_out_module.cpp b/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp index 5d3ab62dca23b..9e012c195f2dd 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 @@ -232,7 +232,7 @@ BehaviorModuleOutput PullOutModule::planWaitingApproval() lanes.insert(lanes.end(), shoulder_lanes.begin(), shoulder_lanes.end()); const double resolution = common_parameters.drivable_area_resolution; candidate_path.drivable_area = util::generateDrivableArea( - lanes, resolution, common_parameters.vehicle_length, planner_data_); + candidate_path, lanes, resolution, common_parameters.vehicle_length, planner_data_); updateRTCStatus(candidate.distance_to_path_change); } @@ -309,7 +309,8 @@ void PullOutModule::updatePullOutStatus() const double resolution = common_parameters.drivable_area_resolution; status_.pull_out_path.path.drivable_area = util::generateDrivableArea( - lanes, resolution, common_parameters.vehicle_length, planner_data_); + status_.pull_out_path.path, lanes, resolution, common_parameters.vehicle_length, + planner_data_); } const auto arclength_start = @@ -348,8 +349,8 @@ PathWithLaneId PullOutModule::getReferencePath() const parameters_.deceleration_interval, goal_pose); reference_path.drivable_area = util::generateDrivableArea( - pull_out_lanes, common_parameters.drivable_area_resolution, common_parameters.vehicle_length, - planner_data_); + reference_path, pull_out_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/pull_over/pull_over_module.cpp b/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp index f3bde34014ead..db4abcda2910c 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 @@ -648,8 +648,8 @@ bool PullOverModule::planShiftPath() std::tie(found_valid_path, found_safe_path) = getSafePath(shift_parking_path_); shift_parking_path_.path.drivable_area = util::generateDrivableArea( - status_.lanes, common_parameters.drivable_area_resolution, common_parameters.vehicle_length, - planner_data_); + shift_parking_path_.path, status_.lanes, common_parameters.drivable_area_resolution, + common_parameters.vehicle_length, planner_data_); shift_parking_path_.path.header = planner_data_->route_handler->getRouteHeader(); return found_safe_path; @@ -694,7 +694,7 @@ PathWithLaneId PullOverModule::getReferencePath() const } reference_path.drivable_area = util::generateDrivableArea( - status_.current_lanes, common_parameters.drivable_area_resolution, + reference_path, status_.current_lanes, common_parameters.drivable_area_resolution, common_parameters.vehicle_length, planner_data_); return reference_path; @@ -735,7 +735,7 @@ PathWithLaneId PullOverModule::getStopPath() } reference_path.drivable_area = util::generateDrivableArea( - status_.current_lanes, common_parameters.drivable_area_resolution, + reference_path, status_.current_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/side_shift/side_shift_module.cpp b/planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp index a42c825622de4..ba5a5345b8923 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 @@ -425,7 +425,7 @@ void SideShiftModule::adjustDrivableArea(ShiftedPath * path) const { const auto & p = planner_data_->parameters; path->path.drivable_area = util::generateDrivableArea( - extended_lanelets, p.drivable_area_resolution, p.vehicle_length, planner_data_); + path->path, extended_lanelets, p.drivable_area_resolution, p.vehicle_length, planner_data_); } } diff --git a/planning/behavior_path_planner/src/scene_module/utils/geometric_parallel_parking.cpp b/planning/behavior_path_planner/src/scene_module/utils/geometric_parallel_parking.cpp index bdac6fe5029cc..e6325dc2e042b 100644 --- a/planning/behavior_path_planner/src/scene_module/utils/geometric_parallel_parking.cpp +++ b/planning/behavior_path_planner/src/scene_module/utils/geometric_parallel_parking.cpp @@ -149,7 +149,7 @@ void GeometricParallelParking::generateStraightPath(const Pose start_pose) const auto common_params = planner_data_->parameters; path.drivable_area = util::generateDrivableArea( - current_lanes, common_params.drivable_area_resolution, common_params.vehicle_length, + path, current_lanes, common_params.drivable_area_resolution, common_params.vehicle_length, planner_data_); path.points.back().point.longitudinal_velocity_mps = 0; @@ -255,12 +255,14 @@ bool GeometricParallelParking::planOneTraial( path_turn_left.header = planner_data_->route_handler->getRouteHeader(); path_turn_left.drivable_area = util::generateDrivableArea( - lanes, common_params.drivable_area_resolution, common_params.vehicle_length, planner_data_); + path_turn_left, lanes, common_params.drivable_area_resolution, common_params.vehicle_length, + planner_data_); paths_.push_back(path_turn_left); path_turn_right.header = planner_data_->route_handler->getRouteHeader(); path_turn_right.drivable_area = util::generateDrivableArea( - lanes, common_params.drivable_area_resolution, common_params.vehicle_length, planner_data_); + path_turn_right, lanes, common_params.drivable_area_resolution, common_params.vehicle_length, + planner_data_); paths_.push_back(path_turn_right); Cr_.pose = Cr; diff --git a/planning/behavior_path_planner/src/utilities.cpp b/planning/behavior_path_planner/src/utilities.cpp index 0b0b307586fc9..b66e67c342567 100644 --- a/planning/behavior_path_planner/src/utilities.cpp +++ b/planning/behavior_path_planner/src/utilities.cpp @@ -65,11 +65,47 @@ bool sumLengthFromTwoPoints( return is_end; } -std::array getLaneletScope( - const lanelet::ConstLanelets & lanes, const size_t nearest_lane_idx, +std::array getPathScope( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const std::shared_ptr route_handler, const geometry_msgs::msg::Pose & current_pose, const double forward_lane_length, const double backward_lane_length, const double lane_margin) { + // extract lanes from path_with_lane_id + lanelet::ConstLanelets path_lanes = [&]() { + // extract "unique" lane ids from path_with_lane_id + std::vector path_lane_ids; + for (const auto & path_point : path.points) { + for (const size_t lane_id : path_point.lane_ids) { + if (std::find(path_lane_ids.begin(), path_lane_ids.end(), lane_id) == path_lane_ids.end()) { + path_lane_ids.push_back(lane_id); + } + } + } + + // get lanes according to lane ids + lanelet::ConstLanelets path_lanes; + for (const auto path_lane_id : path_lane_ids) { + const auto & lane = route_handler->getLaneletsFromId(path_lane_id); + path_lanes.push_back(lane); + } + + return path_lanes; + }(); + + // claculate nearest lane idx + const int nearest_lane_idx = [&]() -> int { + lanelet::ConstLanelet closest_lanelet; + if (lanelet::utils::query::getClosestLanelet(path_lanes, current_pose, &closest_lanelet)) { + for (size_t i = 0; i < path_lanes.size(); ++i) { + if (path_lanes.at(i).id() == closest_lanelet.id()) { + return i; + } + } + } + return 0; + }(); + // define functions to get right/left bounds as a vector const auto get_bound_funcs = std::vector>{ @@ -88,7 +124,7 @@ std::array getLaneletScope( for (const auto & get_bound_func : get_bound_funcs) { // search nearest point index to current pose - const auto & nearest_bound = get_bound_func(lanes.at(nearest_lane_idx)); + const auto & nearest_bound = get_bound_func(path_lanes.at(nearest_lane_idx)); if (nearest_bound.empty()) { continue; } @@ -108,7 +144,7 @@ std::array getLaneletScope( motion_utils::calcSignedArcLength(points, current_pose.position, nearest_segment_idx); double sum_length = std::min(forward_offset_length, 0.0); size_t current_lane_idx = nearest_lane_idx; - auto current_lane = lanes.at(current_lane_idx); + auto current_lane = path_lanes.at(current_lane_idx); size_t current_point_idx = nearest_segment_idx; while (true) { const auto & bound = get_bound_func(current_lane); @@ -130,12 +166,12 @@ std::array getLaneletScope( drivable_area_utils::updateMinMaxPosition( previous_bound[previous_point_idx].basicPoint(), min_x, min_y, max_x, max_y); - if (current_lane_idx == lanes.size() - 1) { + if (current_lane_idx == path_lanes.size() - 1) { break; } current_lane_idx += 1; - current_lane = lanes.at(current_lane_idx); + current_lane = path_lanes.at(current_lane_idx); current_point_idx = 0; const auto & current_bound = get_bound_func(current_lane); @@ -156,7 +192,7 @@ std::array getLaneletScope( motion_utils::calcSignedArcLength(points, nearest_segment_idx + 1, current_pose.position); sum_length = std::min(backward_offset_length, 0.0); current_lane_idx = nearest_lane_idx; - current_lane = lanes.at(current_lane_idx); + current_lane = path_lanes.at(current_lane_idx); while (true) { const auto & bound = get_bound_func(current_lane); if (current_point_idx != 0) { @@ -182,7 +218,7 @@ std::array getLaneletScope( } current_lane_idx -= 1; - current_lane = lanes.at(current_lane_idx); + current_lane = path_lanes.at(current_lane_idx); const auto & current_bound = get_bound_func(current_lane); current_point_idx = current_bound.size() - 1; @@ -1031,40 +1067,32 @@ bool containsGoal(const lanelet::ConstLanelets & lanes, const lanelet::Id & goal } // 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. +// lanes argument is used to determinte (= draw) the drivable area. +// This is because lanes argument has multiple parallel lanes which makes hard to calculate +// the size of the drivable area OccupancyGrid generateDrivableArea( - const lanelet::ConstLanelets & lanes, const double resolution, const double vehicle_length, - const std::shared_ptr planner_data) + const PathWithLaneId & path, const lanelet::ConstLanelets & lanes, const double resolution, + const double vehicle_length, const std::shared_ptr planner_data) { const auto & params = planner_data->parameters; const auto route_handler = planner_data->route_handler; const auto current_pose = planner_data->self_pose; - // search closest lanelet to current pose from given lanelets - const int nearest_lane_idx = [&]() -> int { - lanelet::ConstLanelet closest_lanelet; - if (lanelet::utils::query::getClosestLanelet(lanes, current_pose->pose, &closest_lanelet)) { - for (size_t i = 0; i < lanes.size(); ++i) { - if (lanes.at(i).id() == closest_lanelet.id()) { - return i; - } - } - } - return 0; - }(); - - // calculate min/max x and y - const auto lanelet_scope = drivable_area_utils::getLaneletScope( - lanes, nearest_lane_idx, current_pose->pose, params.drivable_lane_forward_length, + // calculate min/max x and y from lanes in path argument (not from lanes argument) + const auto path_scope = drivable_area_utils::getPathScope( + path, route_handler, current_pose->pose, params.drivable_lane_forward_length, params.drivable_lane_backward_length, params.drivable_lane_margin); const double min_x = - drivable_area_utils::quantize(lanelet_scope.at(0) - params.drivable_area_margin, resolution); + drivable_area_utils::quantize(path_scope.at(0) - params.drivable_area_margin, resolution); const double min_y = - drivable_area_utils::quantize(lanelet_scope.at(1) - params.drivable_area_margin, resolution); + drivable_area_utils::quantize(path_scope.at(1) - params.drivable_area_margin, resolution); const double max_x = - drivable_area_utils::quantize(lanelet_scope.at(2) + params.drivable_area_margin, resolution); + drivable_area_utils::quantize(path_scope.at(2) + params.drivable_area_margin, resolution); const double max_y = - drivable_area_utils::quantize(lanelet_scope.at(3) + params.drivable_area_margin, resolution); + drivable_area_utils::quantize(path_scope.at(3) + params.drivable_area_margin, resolution); const double width = max_x - min_x; const double height = max_y - min_y; @@ -1586,7 +1614,7 @@ std::shared_ptr generateCenterLinePath( centerline_path->header = route_handler->getRouteHeader(); centerline_path->drivable_area = util::generateDrivableArea( - lanelet_sequence, p.drivable_area_resolution, p.vehicle_length, planner_data); + *centerline_path, lanelet_sequence, p.drivable_area_resolution, p.vehicle_length, planner_data); return centerline_path; }