diff --git a/planning/route_handler/include/route_handler/route_handler.hpp b/planning/route_handler/include/route_handler/route_handler.hpp index 73bf99962ea82..9ea9a5544ca2b 100644 --- a/planning/route_handler/include/route_handler/route_handler.hpp +++ b/planning/route_handler/include/route_handler/route_handler.hpp @@ -277,7 +277,8 @@ class RouteHandler std::optional getPullOverTarget(const Pose & goal_pose) const; std::optional getPullOutStartLane( const Pose & pose, const double vehicle_width) const; - lanelet::ConstLanelets getRoadLaneletsAtPose(const Pose & pose) const; + lanelet::ConstLanelets getRoadLaneletsAtPose( + const Pose & pose, const std::optional & yaw_threshold = std::nullopt) const; std::optional getLeftShoulderLanelet( const lanelet::ConstLanelet & lanelet) const; std::optional getRightShoulderLanelet( diff --git a/planning/route_handler/src/route_handler.cpp b/planning/route_handler/src/route_handler.cpp index 4dd559b9bbc98..eb7a1530c6ac1 100644 --- a/planning/route_handler/src/route_handler.cpp +++ b/planning/route_handler/src/route_handler.cpp @@ -689,7 +689,8 @@ lanelet::ConstLanelets RouteHandler::getLaneletSequence( return lanelet_sequence; } -lanelet::ConstLanelets RouteHandler::getRoadLaneletsAtPose(const Pose & pose) const +lanelet::ConstLanelets RouteHandler::getRoadLaneletsAtPose( + const Pose & pose, const std::optional & yaw_threshold) const { lanelet::ConstLanelets road_lanelets_at_pose; const lanelet::BasicPoint2d p{pose.position.x, pose.position.y}; @@ -697,6 +698,15 @@ lanelet::ConstLanelets RouteHandler::getRoadLaneletsAtPose(const Pose & pose) co for (const auto & lanelet_at_pose : lanelets_at_pose) { // confirm that the pose is inside the lanelet since "search" does an approximation with boxes const auto is_pose_inside_lanelet = lanelet::geometry::inside(lanelet_at_pose, p); + + if (yaw_threshold) { + const auto lane_yaw = lanelet::utils::getLaneletAngle(lanelet_at_pose, pose.position); + const double yaw = tf2::getYaw(pose.orientation); + if (*yaw_threshold < std::abs(tier4_autoware_utils::normalizeRadian(lane_yaw - yaw))) { + continue; + } + } + if (is_pose_inside_lanelet && isRoadLanelet(lanelet_at_pose)) road_lanelets_at_pose.push_back(lanelet_at_pose); }