fix(mission_planner): set goal in shoulder lane (autowarefoundation#2548

) (#223)

* fix(mission_planner): set goal in shoulder lane

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

* fix map without shoulder lane

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>
Co-authored-by: Kosuke Takeuchi <kosuke.tnp@gmail.com>

planning/mission_planner/src/lanelet2_plugins/default_planner.cpp

@@ -272,6 +272,25 @@ bool DefaultPlanner::is_goal_valid(
   const geometry_msgs::msg::Pose & goal, lanelet::ConstLanelets path_lanelets)
 {
   const auto logger = node_->get_logger();
+
+  const auto goal_lanelet_pt = lanelet::utils::conversion::toLaneletPoint(goal.position);
+
+  // check if goal is in shoulder lanelet
+  lanelet::Lanelet closest_shoulder_lanelet;
+  if (lanelet::utils::query::getClosestLanelet(
+        shoulder_lanelets_, goal, &closest_shoulder_lanelet)) {
+    if (is_in_lane(closest_shoulder_lanelet, goal_lanelet_pt)) {
+      const auto lane_yaw =
+        lanelet::utils::getLaneletAngle(closest_shoulder_lanelet, goal.position);
+      const auto goal_yaw = tf2::getYaw(goal.orientation);
+      const auto angle_diff = tier4_autoware_utils::normalizeRadian(lane_yaw - goal_yaw);
+      const double th_angle = tier4_autoware_utils::deg2rad(param_.goal_angle_threshold_deg);
+      if (std::abs(angle_diff) < th_angle) {
+        return true;
+      }
+    }
+  }
+
   lanelet::Lanelet closest_lanelet;
   if (!lanelet::utils::query::getClosestLanelet(road_lanelets_, goal, &closest_lanelet)) {
     return false;
@@ -298,8 +317,6 @@ bool DefaultPlanner::is_goal_valid(
     return false;
   }
 
-  const auto goal_lanelet_pt = lanelet::utils::conversion::toLaneletPoint(goal.position);
-
   if (is_in_lane(closest_lanelet, goal_lanelet_pt)) {
     const auto lane_yaw = lanelet::utils::getLaneletAngle(closest_lanelet, goal.position);
     const auto goal_yaw = tf2::getYaw(goal.orientation);
@@ -323,23 +340,6 @@ bool DefaultPlanner::is_goal_valid(
     return true;
   }
 
-  // check if goal is in shoulder lanelet
-  lanelet::Lanelet closest_shoulder_lanelet;
-  if (!lanelet::utils::query::getClosestLanelet(
-        shoulder_lanelets_, goal, &closest_shoulder_lanelet)) {
-    return false;
-  }
-  // check if goal pose is in shoulder lane
-  if (is_in_lane(closest_shoulder_lanelet, goal_lanelet_pt)) {
-    const auto lane_yaw = lanelet::utils::getLaneletAngle(closest_shoulder_lanelet, goal.position);
-    const auto goal_yaw = tf2::getYaw(goal.orientation);
-    const auto angle_diff = tier4_autoware_utils::normalizeRadian(lane_yaw - goal_yaw);
-
-    const double th_angle = tier4_autoware_utils::deg2rad(param_.goal_angle_threshold_deg);
-    if (std::abs(angle_diff) < th_angle) {
-      return true;
-    }
-  }
   return false;
 }