diff --git a/planning/behavior_path_planner/src/planner_manager.cpp b/planning/behavior_path_planner/src/planner_manager.cpp index 026999790299c..692a5ce0798c4 100644 --- a/planning/behavior_path_planner/src/planner_manager.cpp +++ b/planning/behavior_path_planner/src/planner_manager.cpp @@ -636,15 +636,21 @@ void PlannerManager::resetRootLanelet(const std::shared_ptr & data) const auto root_lanelet = updateRootLanelet(data); + // if root_lanelet is not route lanelets, reset root lanelet. + // this can be caused by rerouting. + const auto & route_handler = data->route_handler; + if (!route_handler->isRouteLanelet(root_lanelet_.get())) { + root_lanelet_ = root_lanelet; + return; + } + // check ego is in same lane if (root_lanelet_.get().id() == root_lanelet.id()) { return; } - const auto route_handler = data->route_handler; - const auto next_lanelets = route_handler->getRoutingGraphPtr()->following(root_lanelet_.get()); - // check ego is in next lane + const auto next_lanelets = route_handler->getRoutingGraphPtr()->following(root_lanelet_.get()); for (const auto & next : next_lanelets) { if (next.id() == root_lanelet.id()) { return; diff --git a/planning/route_handler/include/route_handler/route_handler.hpp b/planning/route_handler/include/route_handler/route_handler.hpp index 6ad42e1f8b03c..c60c0e431108e 100644 --- a/planning/route_handler/include/route_handler/route_handler.hpp +++ b/planning/route_handler/include/route_handler/route_handler.hpp @@ -325,6 +325,7 @@ class RouteHandler const lanelet::ConstLanelet & prev_lane, const lanelet::ConstLanelet & next_lane) const; lanelet::ConstLanelets getShoulderLanelets() const; bool isShoulderLanelet(const lanelet::ConstLanelet & lanelet) const; + bool isRouteLanelet(const lanelet::ConstLanelet & lanelet) const; // for path PathWithLaneId getCenterLinePath( diff --git a/planning/route_handler/src/route_handler.cpp b/planning/route_handler/src/route_handler.cpp index e1fbf7f1dccc6..3027ca0b96560 100644 --- a/planning/route_handler/src/route_handler.cpp +++ b/planning/route_handler/src/route_handler.cpp @@ -1800,6 +1800,11 @@ bool RouteHandler::isShoulderLanelet(const lanelet::ConstLanelet & lanelet) cons return lanelet::utils::contains(shoulder_lanelets_, lanelet); } +bool RouteHandler::isRouteLanelet(const lanelet::ConstLanelet & lanelet) const +{ + return lanelet::utils::contains(route_lanelets_, lanelet); +} + lanelet::ConstLanelets RouteHandler::getPreviousLaneletSequence( const lanelet::ConstLanelets & lanelet_sequence) const {