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 369bc03144ef..0ba143ac988b 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,7 +102,8 @@ class LaneDepartureChecker vehicle_info_ptr_ = std::make_shared(vehicle_info); } - bool checkPathWillLeaveLane(const lanelet::ConstLanelets & lanelets, const PathWithLaneId & path); + bool checkPathWillLeaveLane( + const lanelet::ConstLanelets & lanelets, const PathWithLaneId & path) const; vehicle_info_util::VehicleInfo vehicle_info_public_; @@ -122,7 +123,7 @@ class LaneDepartureChecker std::vector createVehicleFootprints( const geometry_msgs::msg::PoseWithCovariance & covariance, const TrajectoryPoints & trajectory, const Param & param); - std::vector createVehicleFootprints(const PathWithLaneId & path); + std::vector createVehicleFootprints(const PathWithLaneId & path) const; 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 8cee01856e15..ce36b394ae54 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 lanelet::ConstLanelets & lanelets, const PathWithLaneId & path) const { std::vector vehicle_footprints = createVehicleFootprints(path); lanelet::ConstLanelets candidate_lanelets = getCandidateLanelets(lanelets, vehicle_footprints); @@ -242,7 +242,8 @@ std::vector LaneDepartureChecker::createVehicleFootprints( return vehicle_footprints; } -std::vector LaneDepartureChecker::createVehicleFootprints(const PathWithLaneId & path) +std::vector LaneDepartureChecker::createVehicleFootprints( + const PathWithLaneId & path) const { // 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 3ed737807134..fc1a47087257 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,6 +61,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/lane_change_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module.hpp index 9688f0426a00..f3bdd89079ff 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 @@ -185,6 +185,7 @@ 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 9846e36e3aae..351e619a48f9 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,6 +75,9 @@ 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 1fa2d5b1edef..aae4601fd032 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp @@ -184,6 +184,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); @@ -196,6 +205,10 @@ 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 @@ -292,9 +305,14 @@ 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 892d6952031a..f2d194fc0f22 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 @@ -52,6 +52,12 @@ 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); @@ -77,7 +83,7 @@ void LaneChangeModule::onExit() { clearWaitingApproval(); removeRTCStatus(); - current_state_ = BT::NodeStatus::IDLE; + current_state_ = BT::NodeStatus::SUCCESS; RCLCPP_DEBUG(getLogger(), "LANE_CHANGE onExit"); } @@ -122,6 +128,11 @@ 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; @@ -143,6 +154,10 @@ 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; @@ -397,6 +412,44 @@ 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); + + // check path points are in any lanelets + for (const auto & point : path.points) { + bool is_in_lanelet = false; + for (const auto & lanelet : lanelets) { + if (lanelet::utils::isInLanelet(point.point.pose, lanelet)) { + is_in_lanelet = true; + break; + } + } + if (!is_in_lanelet) { + 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 250390eafdd6..c97cdd8c4340 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 @@ -172,6 +172,11 @@ std::vector getLaneChangePaths( target_lanelets, reference_path1.points.back().point.pose); double s_start = lane_change_start_arc_position.length; double s_end = s_start + straight_distance + lane_change_distance + forward_path_length; + if (route_handler.isInGoalRouteSection(target_lanelets.back())) { + const auto goal_arc_coordinates = + lanelet::utils::getArcCoordinates(target_lanelets, route_handler.getGoalPose()); + s_end = std::min(s_end, goal_arc_coordinates.length); + } target_lane_reference_path = route_handler.getCenterLinePath(target_lanelets, s_start, s_end); } @@ -498,5 +503,49 @@ 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 6bfea188fe89..81cdc2c03281 100644 --- a/planning/behavior_path_planner/src/utilities.cpp +++ b/planning/behavior_path_planner/src/utilities.cpp @@ -1066,6 +1066,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. @@ -1649,6 +1750,44 @@ 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); @@ -1934,12 +2073,33 @@ 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)) { @@ -1950,25 +2110,56 @@ lanelet::ConstLanelets getExtendedCurrentLanes( } // For current_lanes with desired length - auto current_lanes = route_handler->getLaneletSequence( + const auto current_lanes = route_handler->getLaneletSequence( current_lane, current_pose, common_parameters.backward_path_length, common_parameters.forward_path_length); - // 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()); + 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 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()); + 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; + } } - return current_lanes; + return true; } } // 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 85aaf9c303ea..82633d771d7f 100644 --- a/planning/route_handler/include/route_handler/route_handler.hpp +++ b/planning/route_handler/include/route_handler/route_handler.hpp @@ -123,6 +123,7 @@ 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 607e3c7b590a..77037d6e66da 100644 --- a/planning/route_handler/src/route_handler.cpp +++ b/planning/route_handler/src/route_handler.cpp @@ -771,6 +771,12 @@ 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 {