feat(mission_planner): reroute with current route start pose when tri…

…ggered by modifed goal (#9136) * feat(mission_planner): reroute with current route start pose when triggered by modifed goal Signed-off-by: kosuke55 <[email protected]> * check new ego goal is in original preffered lane as much as possible Signed-off-by: kosuke55 <[email protected]> * check goal is in goal_lane Signed-off-by: kosuke55 <[email protected]> --------- Signed-off-by: kosuke55 <[email protected]>
autowarefoundation · Oct 30, 2024 · e58a158 · e58a158
1 parent 77b1181
commit e58a158
Show file tree

Hide file tree

Showing 3 changed files with 47 additions and 14 deletions.
diff --git a/planning/autoware_mission_planner/src/mission_planner/mission_planner.cpp b/planning/autoware_mission_planner/src/mission_planner/mission_planner.cpp
@@ -396,7 +396,8 @@ LaneletRoute MissionPlanner::create_route(const SetWaypointRoute::Request & req)
   const auto & uuid = req.uuid;
   const auto & allow_goal_modification = req.allow_modification;
 
-  return create_route(header, waypoints, goal_pose, uuid, allow_goal_modification);
+  return create_route(
+    header, waypoints, odometry_->pose.pose, goal_pose, uuid, allow_goal_modification);
 }
 
 LaneletRoute MissionPlanner::create_route(const PoseWithUuidStamped & msg)
@@ -406,7 +407,21 @@ LaneletRoute MissionPlanner::create_route(const PoseWithUuidStamped & msg)
   const auto & uuid = msg.uuid;
   const auto & allow_goal_modification = current_route_->allow_modification;
 
-  return create_route(header, std::vector<Pose>(), goal_pose, uuid, allow_goal_modification);
+  // NOTE: Reroute by modifed goal is assumed to be a slight modification near the goal lane.
+  //       It is assumed that ego and goal are on the extension of the current route at least.
+  //       Therefore, the start pose is the start pose of the current route if it exists.
+  //       This prevents the route from becoming shorter due to reroute.
+  //       Also, use start pose and waypoints that are on the preferred lanelet of the current route
+  //       as much as possible.
+  //       For this process, refer to RouteHandler::planPathLaneletsBetweenCheckpoints() or
+  //       https://github.com/autowarefoundation/autoware.universe/pull/8238 too.
+  const auto & start_pose = current_route_ ? current_route_->start_pose : odometry_->pose.pose;
+  std::vector<Pose> waypoints{};
+  if (current_route_) {
+    waypoints.push_back(odometry_->pose.pose);
+  }
+
+  return create_route(header, waypoints, start_pose, goal_pose, uuid, allow_goal_modification);
 }
 
 LaneletRoute MissionPlanner::create_route(
@@ -425,11 +440,11 @@ LaneletRoute MissionPlanner::create_route(
 }
 
 LaneletRoute MissionPlanner::create_route(
-  const Header & header, const std::vector<Pose> & waypoints, const Pose & goal_pose,
-  const UUID & uuid, const bool allow_goal_modification)
+  const Header & header, const std::vector<Pose> & waypoints, const Pose & start_pose,
+  const Pose & goal_pose, const UUID & uuid, const bool allow_goal_modification)
 {
   PlannerPlugin::RoutePoints points;
-  points.push_back(odometry_->pose.pose);
+  points.push_back(start_pose);
   for (const auto & waypoint : waypoints) {
     points.push_back(transform_pose(waypoint, header));
   }

diff --git a/planning/autoware_mission_planner/src/mission_planner/mission_planner.hpp b/planning/autoware_mission_planner/src/mission_planner/mission_planner.hpp
@@ -125,8 +125,8 @@ class MissionPlanner : public rclcpp::Node
     const Header & header, const std::vector<LaneletSegment> & segments, const Pose & goal_pose,
     const UUID & uuid, const bool allow_goal_modification);
   LaneletRoute create_route(
-    const Header & header, const std::vector<Pose> & waypoints, const Pose & goal_pose,
-    const UUID & uuid, const bool allow_goal_modification);
+    const Header & header, const std::vector<Pose> & waypoints, const Pose & start_pose,
+    const Pose & goal_pose, const UUID & uuid, const bool allow_goal_modification);
 
   void publish_pose_log(const Pose & pose, const std::string & pose_type);
 

diff --git a/planning/autoware_route_handler/src/route_handler.cpp b/planning/autoware_route_handler/src/route_handler.cpp
@@ -1898,13 +1898,31 @@ bool RouteHandler::planPathLaneletsBetweenCheckpoints(
     std::remove_if(
       candidates.begin(), candidates.end(), [&](const auto & l) { return !isRoadLanelet(l); }),
     candidates.end());
-  if (!lanelet::utils::query::getClosestLanelet(candidates, goal_checkpoint, &goal_lanelet)) {
-    RCLCPP_WARN_STREAM(
-      logger_, "Failed to find closest lanelet."
-                 << std::endl
-                 << " - start checkpoint: " << toString(start_checkpoint) << std::endl
-                 << " - goal checkpoint: " << toString(goal_checkpoint) << std::endl);
-    return false;
+  // if there is a lanelet in candidates that is included in previous preferred lanelets,
+  // set it as goal_lanelet.
+  // this is to select the same lane as much as possible when rerouting with waypoints.
+  const auto findGoalClosestPreferredLanelet = [&]() -> std::optional<lanelet::ConstLanelet> {
+    lanelet::ConstLanelet closest_lanelet;
+    if (getClosestPreferredLaneletWithinRoute(goal_checkpoint, &closest_lanelet)) {
+      if (std::find(candidates.begin(), candidates.end(), closest_lanelet) != candidates.end()) {
+        if (lanelet::utils::isInLanelet(goal_checkpoint, closest_lanelet)) {
+          return closest_lanelet;
+        }
+      }
+    }
+    return std::nullopt;
+  };
+  if (auto closest_lanelet = findGoalClosestPreferredLanelet()) {
+    goal_lanelet = closest_lanelet.value();
+  } else {
+    if (!lanelet::utils::query::getClosestLanelet(candidates, goal_checkpoint, &goal_lanelet)) {
+      RCLCPP_WARN_STREAM(
+        logger_, "Failed to find closest lanelet."
+                   << std::endl
+                   << " - start checkpoint: " << toString(start_checkpoint) << std::endl
+                   << " - goal checkpoint: " << toString(goal_checkpoint) << std::endl);
+      return false;
+    }
   }
 
   lanelet::Optional<lanelet::routing::Route> optional_route;