fix(mission_planner): subtract goal distance from the accumulated length

planning/mission_planner/config/mission_planner.param.yaml

@@ -7,3 +7,4 @@
     goal_angle_threshold_deg: 45.0
     enable_correct_goal_pose: false
     reroute_time_threshold: 10.0
+    minimum_reroute_length: 30.0

planning/mission_planner/src/mission_planner/mission_planner.cpp

@@ -80,6 +80,7 @@ MissionPlanner::MissionPlanner(const rclcpp::NodeOptions & options)
 {
   map_frame_ = declare_parameter<std::string>("map_frame");
   reroute_time_threshold_ = declare_parameter<double>("reroute_time_threshold");
+  minimum_reroute_length_ = declare_parameter<double>("minimum_reroute_length");
 
   planner_ = plugin_loader_.createSharedInstance("mission_planner::lanelet2::DefaultPlanner");
   planner_->initialize(this);
@@ -169,6 +170,59 @@ void MissionPlanner::change_route(const LaneletRoute & route)
   normal_route_ = std::make_shared<LaneletRoute>(route);
 }
 
+LaneletRoute MissionPlanner::create_route(const SetRoute::Service::Request::SharedPtr req)
+{
+  PoseStamped goal_pose;
+  goal_pose.header = req->header;
+  goal_pose.pose = req->goal;
+
+  // Convert route.
+  LaneletRoute route;
+  route.start_pose = odometry_->pose.pose;
+  route.goal_pose = transform_pose(goal_pose).pose;
+  for (const auto & segment : req->segments) {
+    route.segments.push_back(convert(segment));
+  }
+  route.header.stamp = req->header.stamp;
+  route.header.frame_id = map_frame_;
+  route.uuid.uuid = generate_random_id();
+  route.allow_modification = req->option.allow_goal_modification;
+
+  return route;
+}
+
+LaneletRoute MissionPlanner::create_route(const SetRoutePoints::Service::Request::SharedPtr req)
+{
+  using ResponseCode = autoware_adapi_v1_msgs::srv::SetRoute::Response;
+
+  // Use temporary pose stamped for transform.
+  PoseStamped pose;
+  pose.header = req->header;
+
+  // Convert route points.
+  PlannerPlugin::RoutePoints points;
+  points.push_back(odometry_->pose.pose);
+  for (const auto & waypoint : req->waypoints) {
+    pose.pose = waypoint;
+    points.push_back(transform_pose(pose).pose);
+  }
+  pose.pose = req->goal;
+  points.push_back(transform_pose(pose).pose);
+
+  // Plan route.
+  LaneletRoute route = planner_->plan(points);
+  if (route.segments.empty()) {
+    throw component_interface_utils::ServiceException(
+      ResponseCode::ERROR_PLANNER_FAILED, "The planned route is empty.");
+  }
+  route.header.stamp = req->header.stamp;
+  route.header.frame_id = map_frame_;
+  route.uuid.uuid = generate_random_id();
+  route.allow_modification = req->option.allow_goal_modification;
+
+  return route;
+}
+
 void MissionPlanner::change_state(RouteState::Message::_state_type state)
 {
   state_.stamp = now();
@@ -200,22 +254,8 @@ void MissionPlanner::on_set_route(
       ResponseCode::ERROR_PLANNER_UNREADY, "The vehicle pose is not received.");
   }
 
-  // Use temporary pose stamped for transform.
-  PoseStamped pose;
-  pose.header = req->header;
-  pose.pose = req->goal;
-
-  // Convert route.
-  LaneletRoute route;
-  route.start_pose = odometry_->pose.pose;
-  route.goal_pose = transform_pose(pose).pose;
-  for (const auto & segment : req->segments) {
-    route.segments.push_back(convert(segment));
-  }
-  route.header.stamp = req->header.stamp;
-  route.header.frame_id = map_frame_;
-  route.uuid.uuid = generate_random_id();
-  route.allow_modification = req->option.allow_goal_modification;
+  // Convert request to a new route.
+  const auto route = create_route(req);
 
   // Update route.
   change_route(route);
@@ -244,30 +284,8 @@ void MissionPlanner::on_set_route_points(
       ResponseCode::ERROR_PLANNER_UNREADY, "The vehicle pose is not received.");
   }
 
-  // Use temporary pose stamped for transform.
-  PoseStamped pose;
-  pose.header = req->header;
-
-  // Convert route points.
-  PlannerPlugin::RoutePoints points;
-  points.push_back(odometry_->pose.pose);
-  for (const auto & waypoint : req->waypoints) {
-    pose.pose = waypoint;
-    points.push_back(transform_pose(pose).pose);
-  }
-  pose.pose = req->goal;
-  points.push_back(transform_pose(pose).pose);
-
   // Plan route.
-  LaneletRoute route = planner_->plan(points);
-  if (route.segments.empty()) {
-    throw component_interface_utils::ServiceException(
-      ResponseCode::ERROR_PLANNER_FAILED, "The planned route is empty.");
-  }
-  route.header.stamp = req->header.stamp;
-  route.header.frame_id = map_frame_;
-  route.uuid.uuid = generate_random_id();
-  route.allow_modification = req->option.allow_goal_modification;
+  const auto route = create_route(req);
 
   // Update route.
   change_route(route);
@@ -316,24 +334,11 @@ void MissionPlanner::on_change_route(
       ResponseCode::ERROR_PLANNER_UNREADY, "The vehicle pose is not received.");
   }
 
+  // set to changing state
   change_state(RouteState::Message::CHANGING);
 
-  // Use temporary pose stamped for transform.
-  PoseStamped pose;
-  pose.header = req->header;
-  pose.pose = req->goal;
-
-  // Convert route.
-  LaneletRoute new_route;
-  new_route.start_pose = odometry_->pose.pose;
-  new_route.goal_pose = transform_pose(pose).pose;
-  for (const auto & segment : req->segments) {
-    new_route.segments.push_back(convert(segment));
-  }
-  new_route.header.stamp = req->header.stamp;
-  new_route.header.frame_id = map_frame_;
-  new_route.uuid.uuid = generate_random_id();
-  new_route.allow_modification = req->option.allow_goal_modification;
+  // Convert request to a new route.
+  const auto new_route = create_route(req);
 
   // check route safety
   if (checkRerouteSafety(*normal_route_, new_route)) {
@@ -373,31 +378,8 @@ void MissionPlanner::on_change_route_points(
 
   change_state(RouteState::Message::CHANGING);
 
-  // Use temporary pose stamped for transform.
-  PoseStamped pose;
-  pose.header = req->header;
-
-  // Convert route points.
-  PlannerPlugin::RoutePoints points;
-  points.push_back(odometry_->pose.pose);
-  for (const auto & waypoint : req->waypoints) {
-    pose.pose = waypoint;
-    points.push_back(transform_pose(pose).pose);
-  }
-  pose.pose = req->goal;
-  points.push_back(transform_pose(pose).pose);
-
   // Plan route.
-  LaneletRoute new_route = planner_->plan(points);
-  if (new_route.segments.empty()) {
-    change_state(RouteState::Message::SET);
-    throw component_interface_utils::ServiceException(
-      ResponseCode::ERROR_PLANNER_FAILED, "The planned route is empty.");
-  }
-  new_route.header.stamp = req->header.stamp;
-  new_route.header.frame_id = map_frame_;
-  new_route.uuid.uuid = generate_random_id();
-  new_route.allow_modification = req->option.allow_goal_modification;
+  const auto new_route = create_route(req);
 
   // check route safety
   if (checkRerouteSafety(*normal_route_, new_route)) {
@@ -509,9 +491,30 @@ bool MissionPlanner::checkRerouteSafety(
     accumulated_length += *std::min_element(lanelets_length.begin(), lanelets_length.end());
   }
 
+  // check if the goal is inside of the target terminal lanelet
+  const auto & target_end_primitives = target_route.segments.at(end_idx - start_idx).primitives;
+  const auto & target_goal = target_route.goal_pose;
+  for (const auto & target_end_primitive : target_end_primitives) {
+    const auto lanelet = lanelet_map_ptr_->laneletLayer.get(target_end_primitive.id);
+    if (lanelet::utils::isInLanelet(target_goal, lanelet)) {
+      const auto target_goal_position =
+        lanelet::utils::conversion::toLaneletPoint(target_goal.position);
+      const double dist_to_goal = lanelet::geometry::toArcCoordinates(
+                                    lanelet::utils::to2D(lanelet.centerline()),
+                                    lanelet::utils::to2D(target_goal_position).basicPoint())
+                                    .length;
+      const double target_lanelet_length = lanelet::utils::getLaneletLength2d(lanelet);
+      const double dist = target_lanelet_length - dist_to_goal;
+      accumulated_length = std::max(accumulated_length - dist, 0.0);
+      break;
+    }
+  }
+
   // check safety
   const auto current_velocity = odometry_->twist.twist.linear.x;
-  if (accumulated_length > current_velocity * reroute_time_threshold_) {
+  const double safety_length =
+    std::max(current_velocity * reroute_time_threshold_, minimum_reroute_length_);
+  if (accumulated_length > safety_length) {
     return true;
   }
 

planning/mission_planner/src/mission_planner/mission_planner.hpp

@@ -76,6 +76,8 @@ class MissionPlanner : public rclcpp::Node
   rclcpp::Publisher<MarkerArray>::SharedPtr pub_marker_;
   void clear_route();
   void change_route(const LaneletRoute & route);
+  LaneletRoute create_route(const SetRoute::Service::Request::SharedPtr req);
+  LaneletRoute create_route(const SetRoutePoints::Service::Request::SharedPtr req);
 
   RouteState::Message state_;
   component_interface_utils::Publisher<RouteState>::SharedPtr pub_state_;
@@ -119,6 +121,7 @@ class MissionPlanner : public rclcpp::Node
     const SetRoutePoints::Service::Response::SharedPtr res);
 
   double reroute_time_threshold_{10.0};
+  double minimum_reroute_length_{30.0};
   bool checkRerouteSafety(const LaneletRoute & original_route, const LaneletRoute & target_route);
 
   std::shared_ptr<LaneletRoute> original_route_{nullptr};