diff --git a/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp b/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp index 0ba143ac988bb..369bc03144efe 100644 --- a/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp +++ b/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp @@ -102,8 +102,7 @@ class LaneDepartureChecker vehicle_info_ptr_ = std::make_shared(vehicle_info); } - bool checkPathWillLeaveLane( - const lanelet::ConstLanelets & lanelets, const PathWithLaneId & path) const; + bool checkPathWillLeaveLane(const lanelet::ConstLanelets & lanelets, const PathWithLaneId & path); vehicle_info_util::VehicleInfo vehicle_info_public_; @@ -123,7 +122,7 @@ class LaneDepartureChecker std::vector createVehicleFootprints( const geometry_msgs::msg::PoseWithCovariance & covariance, const TrajectoryPoints & trajectory, const Param & param); - std::vector createVehicleFootprints(const PathWithLaneId & path) const; + std::vector createVehicleFootprints(const PathWithLaneId & path); static std::vector createVehiclePassingAreas( const std::vector & vehicle_footprints); diff --git a/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp b/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp index ce36b394ae548..8cee01856e154 100644 --- a/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp +++ b/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp @@ -145,7 +145,7 @@ Output LaneDepartureChecker::update(const Input & input) } bool LaneDepartureChecker::checkPathWillLeaveLane( - const lanelet::ConstLanelets & lanelets, const PathWithLaneId & path) const + const lanelet::ConstLanelets & lanelets, const PathWithLaneId & path) { std::vector vehicle_footprints = createVehicleFootprints(path); lanelet::ConstLanelets candidate_lanelets = getCandidateLanelets(lanelets, vehicle_footprints); @@ -242,8 +242,7 @@ std::vector LaneDepartureChecker::createVehicleFootprints( return vehicle_footprints; } -std::vector LaneDepartureChecker::createVehicleFootprints( - const PathWithLaneId & path) const +std::vector LaneDepartureChecker::createVehicleFootprints(const PathWithLaneId & path) { // Create vehicle footprint in base_link coordinate const auto local_vehicle_footprint = createVehicleFootprint(*vehicle_info_ptr_); 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 fc1a470872577..3ed737807134d 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 @@ -61,13 +61,6 @@ 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/lane_change_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module.hpp index 8dbbd452e0446..9688f0426a004 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module.hpp @@ -18,7 +18,6 @@ #include "behavior_path_planner/scene_module/lane_change/lane_change_path.hpp" #include "behavior_path_planner/scene_module/scene_module_interface.hpp" #include "behavior_path_planner/utilities.hpp" -#include "lane_departure_checker/lane_departure_checker.hpp" #include #include @@ -38,7 +37,6 @@ namespace behavior_path_planner { using autoware_auto_planning_msgs::msg::PathWithLaneId; -using lane_departure_checker::LaneDepartureChecker; struct LaneChangeParameters { @@ -127,7 +125,6 @@ class LaneChangeModule : public SceneModuleInterface LaneChangeParameters parameters_; LaneChangeStatus status_; PathShifter path_shifter_; - LaneDepartureChecker lane_departure_checker_; double lane_change_lane_length_{200.0}; double check_distance_{100.0}; @@ -188,7 +185,6 @@ class LaneChangeModule : public SceneModuleInterface bool isSafe() const; bool isLaneBlocked(const lanelet::ConstLanelets & lanes) const; - bool isValidPath(const PathWithLaneId & path) const; bool isNearEndOfLane() const; bool isCurrentSpeedLow() const; bool isAbortConditionSatisfied() const; 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 351e619a48f90..9846e36e3aaef 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 @@ -75,9 +75,6 @@ bool hasEnoughDistance( const bool isInGoalRouteSection, const Pose & goal_pose, const lanelet::routing::RoutingGraphContainer & overall_graphs); bool isObjectFront(const Pose & ego_pose, const Pose & obj_pose); -std::vector generateDrivableLanes( - const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes, - const lanelet::ConstLanelets & lane_change_lanes); } // namespace lane_change_utils } // namespace behavior_path_planner 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 aae4601fd032d..1fa2d5b1edef0 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp @@ -184,15 +184,6 @@ 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); @@ -205,10 +196,6 @@ OccupancyGrid generateDrivableArea( const PathWithLaneId & path, const lanelet::ConstLanelets & lanes, const double resolution, const double vehicle_length, const std::shared_ptr planner_data); -std::vector expandLanelets( - const std::vector & drivable_lanes, const double left_bound_offset, - const double right_bound_offset, const std::vector & types_to_skip = {}); - lanelet::ConstLineStrings3d getDrivableAreaForAllSharedLinestringLanelets( const std::shared_ptr & planner_data); // goal management @@ -305,14 +292,9 @@ std::uint8_t getHighestProbLabel(const std::vector & class lanelet::ConstLanelets getCurrentLanes(const std::shared_ptr & planner_data); -lanelet::ConstLanelets extendLanes( - const std::shared_ptr route_handler, const lanelet::ConstLanelets & lanes); - lanelet::ConstLanelets getExtendedCurrentLanes( const std::shared_ptr & planner_data); -bool checkPathRelativeAngle(const PathWithLaneId & path, const double angle_threshold); - } // namespace util } // namespace behavior_path_planner 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 1140b19eb1b18..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 @@ -44,7 +44,6 @@ LaneChangeModule::LaneChangeModule( uuid_left_{generateUUID()}, uuid_right_{generateUUID()} { - lane_departure_checker_.setVehicleInfo(vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo()); } BehaviorModuleOutput LaneChangeModule::run() @@ -53,12 +52,6 @@ BehaviorModuleOutput LaneChangeModule::run() current_state_ = BT::NodeStatus::RUNNING; is_activated_ = isActivated(); const auto output = plan(); - - if (!isSafe()) { - current_state_ = BT::NodeStatus::SUCCESS; // for breaking loop - return output; - } - const auto turn_signal_info = output.turn_signal_info; if (turn_signal_info.turn_signal.command == TurnIndicatorsCommand::ENABLE_LEFT) { waitApprovalLeft(turn_signal_info.signal_distance); @@ -84,7 +77,7 @@ void LaneChangeModule::onExit() { clearWaitingApproval(); removeRTCStatus(); - current_state_ = BT::NodeStatus::SUCCESS; + current_state_ = BT::NodeStatus::IDLE; RCLCPP_DEBUG(getLogger(), "LANE_CHANGE onExit"); } @@ -129,11 +122,6 @@ bool LaneChangeModule::isExecutionReady() const BT::NodeStatus LaneChangeModule::updateState() { RCLCPP_DEBUG(getLogger(), "LANE_CHANGE updateState"); - if (!isSafe()) { - current_state_ = BT::NodeStatus::SUCCESS; - return current_state_; - } - if (isAbortConditionSatisfied()) { if (isNearEndOfLane() && isCurrentSpeedLow()) { current_state_ = BT::NodeStatus::RUNNING; @@ -155,10 +143,6 @@ BehaviorModuleOutput LaneChangeModule::plan() { constexpr double RESAMPLE_INTERVAL = 1.0; auto path = util::resamplePathWithSpline(status_.lane_change_path.path, RESAMPLE_INTERVAL); - if (!isValidPath(path)) { - status_.is_safe = false; - return BehaviorModuleOutput{}; - } // Generate drivable area { const auto common_parameters = planner_data_->parameters; @@ -413,33 +397,6 @@ std::pair LaneChangeModule::getSafePath( bool LaneChangeModule::isSafe() const { return status_.is_safe; } -bool LaneChangeModule::isValidPath(const PathWithLaneId & path) const -{ - const auto & route_handler = planner_data_->route_handler; - const auto drivable_area_left_bound_offset = 0.5; - const auto drivable_area_right_bound_offset = 0.5; - - // check lane departure - const auto drivable_lanes = lane_change_utils::generateDrivableLanes( - *route_handler, util::extendLanes(route_handler, status_.current_lanes), - util::extendLanes(route_handler, status_.lane_change_lanes)); - const auto expanded_lanes = util::expandLanelets( - drivable_lanes, drivable_area_left_bound_offset, drivable_area_right_bound_offset); - const auto lanelets = util::transformToLanelets(expanded_lanes); - if (lane_departure_checker_.checkPathWillLeaveLane(lanelets, path)) { - RCLCPP_WARN_STREAM_THROTTLE(getLogger(), *clock_, 1000, "path is out of lanes"); - return false; - } - - // check relative angle - if (!util::checkPathRelativeAngle(path, M_PI)) { - RCLCPP_WARN_STREAM_THROTTLE(getLogger(), *clock_, 1000, "path relative angle is invalid"); - return false; - } - - return true; -} - bool LaneChangeModule::isNearEndOfLane() const { const auto current_pose = planner_data_->self_pose->pose; 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 fe5b81f6fe7c2..250390eafdd67 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 @@ -498,49 +498,5 @@ bool isObjectFront(const Pose & ego_pose, const Pose & obj_pose) return obj_from_ego.position.x > 0; } -std::vector generateDrivableLanes( - const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes, - const lanelet::ConstLanelets & lane_change_lanes) -{ - size_t current_lc_idx = 0; - 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 (size_t lc_idx = current_lc_idx; lc_idx < lane_change_lanes.size(); ++lc_idx) { - const auto & lc_lane = lane_change_lanes.at(lc_idx); - if (left_lane && lc_lane.id() == left_lane->id()) { - drivable_lanes.at(i).left_lane = lc_lane; - current_lc_idx = lc_idx; - break; - } - - if (right_lane && lc_lane.id() == right_lane->id()) { - drivable_lanes.at(i).right_lane = lc_lane; - current_lc_idx = lc_idx; - break; - } - } - } - - for (size_t i = current_lc_idx + 1; i < lane_change_lanes.size(); ++i) { - const auto & lc_lane = lane_change_lanes.at(i); - DrivableLanes drivable_lane; - drivable_lane.left_lane = lc_lane; - drivable_lane.right_lane = lc_lane; - drivable_lanes.push_back(drivable_lane); - } - - return drivable_lanes; -} - } // namespace lane_change_utils } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/src/utilities.cpp b/planning/behavior_path_planner/src/utilities.cpp index 81cdc2c032819..6bfea188fe894 100644 --- a/planning/behavior_path_planner/src/utilities.cpp +++ b/planning/behavior_path_planner/src/utilities.cpp @@ -1066,107 +1066,6 @@ 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. @@ -1750,44 +1649,6 @@ lanelet::ConstLineStrings3d getDrivableAreaForAllSharedLinestringLanelets( return linestring_shared; } -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 drivable_lanes; - - std::vector expanded_drivable_lanes{}; - expanded_drivable_lanes.reserve(drivable_lanes.size()); - for (const auto & lanes : drivable_lanes) { - const std::string l_type = - lanes.left_lane.leftBound().attributeOr(lanelet::AttributeName::Type, "none"); - const std::string r_type = - 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(); - const bool r_skip = - std::find(types_to_skip.begin(), types_to_skip.end(), r_type) != types_to_skip.end(); - const double l_offset = l_skip ? 0.0 : left_bound_offset; - const double r_offset = r_skip ? 0.0 : -right_bound_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.right_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.right_lane, 0.0, r_offset); - } - expanded_lanes.middle_lanes = lanes.middle_lanes; - expanded_drivable_lanes.push_back(expanded_lanes); - } - return expanded_drivable_lanes; -} - PredictedObjects filterObjectsByVelocity(const PredictedObjects & objects, double lim_v) { return filterObjectsByVelocity(objects, -lim_v, lim_v); @@ -2073,33 +1934,12 @@ lanelet::ConstLanelets getCurrentLanes(const std::shared_ptr common_parameters.forward_path_length); } -lanelet::ConstLanelets extendLanes( - const std::shared_ptr route_handler, const lanelet::ConstLanelets & lanes) -{ - auto extended_lanes = lanes; - - // Add next lane - const auto next_lanes = route_handler->getNextLanelets(extended_lanes.back()); - if (!next_lanes.empty()) { - extended_lanes.push_back(next_lanes.front()); - } - - // Add previous lane - const auto prev_lanes = route_handler->getPreviousLanelets(extended_lanes.front()); - if (!prev_lanes.empty()) { - extended_lanes.insert(extended_lanes.begin(), prev_lanes.front()); - } - - return extended_lanes; -} - lanelet::ConstLanelets getExtendedCurrentLanes( const std::shared_ptr & planner_data) { const auto & route_handler = planner_data->route_handler; const auto current_pose = planner_data->self_pose->pose; const auto common_parameters = planner_data->parameters; - const auto routing_graph_ptr = route_handler->getRoutingGraphPtr(); lanelet::ConstLanelet current_lane; if (!route_handler->getClosestLaneletWithinRoute(current_pose, ¤t_lane)) { @@ -2110,56 +1950,25 @@ lanelet::ConstLanelets getExtendedCurrentLanes( } // For current_lanes with desired length - const auto current_lanes = route_handler->getLaneletSequence( + auto current_lanes = route_handler->getLaneletSequence( current_lane, current_pose, common_parameters.backward_path_length, common_parameters.forward_path_length); - return extendLanes(route_handler, current_lanes); -} - -bool checkPathRelativeAngle(const PathWithLaneId & path, const double angle_threshold) -{ - // We need at least three points to compute relative angle - constexpr size_t relative_angle_points_num = 3; - if (path.points.size() < relative_angle_points_num) { - return true; + // Add next_lanes + const auto next_lanes = route_handler->getNextLanelets(current_lanes.back()); + if (!next_lanes.empty()) { + // TODO(kosuke55) which lane should be added? + current_lanes.push_back(next_lanes.front()); } - for (size_t p1_id = 0; p1_id <= path.points.size() - relative_angle_points_num; ++p1_id) { - // Get Point1 - const auto & p1 = path.points.at(p1_id).point.pose.position; - - // Get Point2 - const auto & p2 = path.points.at(p1_id + 1).point.pose.position; - - // Get Point3 - const auto & p3 = path.points.at(p1_id + 2).point.pose.position; - - // ignore invert driving direction - if ( - path.points.at(p1_id).point.longitudinal_velocity_mps < 0 || - path.points.at(p1_id + 1).point.longitudinal_velocity_mps < 0 || - path.points.at(p1_id + 2).point.longitudinal_velocity_mps < 0) { - continue; - } - - // convert to p1 coordinate - const double x3 = p3.x - p1.x; - const double x2 = p2.x - p1.x; - const double y3 = p3.y - p1.y; - const double y2 = p2.y - p1.y; - - // calculate relative angle of vector p3 based on p1p2 vector - const double th = std::atan2(y2, x2); - const double th2 = - std::atan2(-x3 * std::sin(th) + y3 * std::cos(th), x3 * std::cos(th) + y3 * std::sin(th)); - if (std::abs(th2) > angle_threshold) { - // invalid angle - return false; - } + // Add prev_lane + lanelet::ConstLanelets prev_lanes; + if (route_handler->getPreviousLaneletsWithinRoute(current_lanes.front(), &prev_lanes)) { + // TODO(kosuke55) which lane should be added? + current_lanes.insert(current_lanes.begin(), 0, prev_lanes.front()); } - return true; + return current_lanes; } } // namespace util diff --git a/planning/route_handler/include/route_handler/route_handler.hpp b/planning/route_handler/include/route_handler/route_handler.hpp index 82633d771d7f5..85aaf9c303ea3 100644 --- a/planning/route_handler/include/route_handler/route_handler.hpp +++ b/planning/route_handler/include/route_handler/route_handler.hpp @@ -123,7 +123,6 @@ class RouteHandler */ boost::optional getLeftLanelet( const lanelet::ConstLanelet & lanelet) const; - lanelet::ConstLanelets getPreviousLanelets(const lanelet::ConstLanelet & lanelet) const; lanelet::ConstLanelets getNextLanelets(const lanelet::ConstLanelet & lanelet) const; /** diff --git a/planning/route_handler/src/route_handler.cpp b/planning/route_handler/src/route_handler.cpp index 77037d6e66da1..607e3c7b590aa 100644 --- a/planning/route_handler/src/route_handler.cpp +++ b/planning/route_handler/src/route_handler.cpp @@ -771,12 +771,6 @@ bool RouteHandler::getClosestLaneletWithinRoute( return lanelet::utils::query::getClosestLanelet(route_lanelets_, search_pose, closest_lanelet); } -lanelet::ConstLanelets RouteHandler::getPreviousLanelets( - const lanelet::ConstLanelet & lanelet) const -{ - return routing_graph_ptr_->previous(lanelet); -} - bool RouteHandler::getNextLaneletWithinRoute( const lanelet::ConstLanelet & lanelet, lanelet::ConstLanelet * next_lanelet) const {