fix(behavior_path_planner): add isInLaneletWithYawThreshold() (autowa…

…refoundation#3726) fix(behavior_path_planner): add isInLaneletWithYawThreshold Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com>
kenji-miyake · May 16, 2023 · 14b183f · 14b183f
1 parent 8a7ebf6
commit 14b183f
Show file tree

Hide file tree

Showing 2 changed files with 18 additions and 3 deletions.
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp
@@ -301,6 +301,10 @@ BehaviorModuleOutput createGoalAroundPath(const std::shared_ptr<const PlannerDat
 
 bool isInLanelets(const Pose & pose, const lanelet::ConstLanelets & lanes);
 
+bool isInLaneletWithYawThreshold(
+  const Pose & current_pose, const lanelet::ConstLanelet & lanelet, const double yaw_threshold,
+  const double radius = 0.0);
+
 bool isEgoOutOfRoute(
   const Pose & self_pose, const std::optional<PoseWithUuidStamped> & modified_goal,
   const std::shared_ptr<RouteHandler> & route_handler);

diff --git a/planning/behavior_path_planner/src/utils/utils.cpp b/planning/behavior_path_planner/src/utils/utils.cpp
@@ -1035,6 +1035,19 @@ bool isInLanelets(const Pose & pose, const lanelet::ConstLanelets & lanes)
   return false;
 }
 
+bool isInLaneletWithYawThreshold(
+  const Pose & current_pose, const lanelet::ConstLanelet & lanelet, const double yaw_threshold,
+  const double radius)
+{
+  const double pose_yaw = tf2::getYaw(current_pose.orientation);
+  const double lanelet_angle = lanelet::utils::getLaneletAngle(lanelet, current_pose.position);
+  const double angle_diff =
+    std::abs(tier4_autoware_utils::normalizeRadian(lanelet_angle - pose_yaw));
+
+  return (angle_diff < std::abs(yaw_threshold)) &&
+         lanelet::utils::isInLanelet(current_pose, lanelet, radius);
+}
+
 bool isEgoOutOfRoute(
   const Pose & self_pose, const std::optional<PoseWithUuidStamped> & modified_goal,
   const std::shared_ptr<RouteHandler> & route_handler)
@@ -1058,10 +1071,8 @@ bool isEgoOutOfRoute(
   }
 
   // If ego vehicle is over goal on goal lane, return true
-  lanelet::ConstLanelet closest_lane;
   const double yaw_threshold = tier4_autoware_utils::deg2rad(90);
-  if (lanelet::utils::query::getClosestLaneletWithConstrains(
-        {goal_lane}, self_pose, &closest_lane, 0.0, yaw_threshold)) {
+  if (isInLaneletWithYawThreshold(self_pose, goal_lane, yaw_threshold)) {
     constexpr double buffer = 1.0;
     const auto ego_arc_coord = lanelet::utils::getArcCoordinates({goal_lane}, self_pose);
     const auto goal_arc_coord =