feat(mission_planner): add option to prevent rerouting in autonomous …

…driving mode (autowarefoundation#8757) Signed-off-by: kosuke55 <[email protected]>
tier4 · Oct 10, 2024 · 2a3e90f · 2a3e90f
1 parent 1a6f528
commit 2a3e90f
Show file tree

Hide file tree

Showing 4 changed files with 22 additions and 11 deletions.
diff --git a/planning/autoware_mission_planner/README.md b/planning/autoware_mission_planner/README.md
@@ -19,17 +19,18 @@ It distributes route requests and planning results according to current MRM oper
 
 ### Parameters
 
-| Name                         | Type   | Description                                                                                                      |
-| ---------------------------- | ------ | ---------------------------------------------------------------------------------------------------------------- |
-| `map_frame`                  | string | The frame name for map                                                                                           |
-| `arrival_check_angle_deg`    | double | Angle threshold for goal check                                                                                   |
-| `arrival_check_distance`     | double | Distance threshold for goal check                                                                                |
-| `arrival_check_duration`     | double | Duration threshold for goal check                                                                                |
-| `goal_angle_threshold`       | double | Max goal pose angle for goal approve                                                                             |
-| `enable_correct_goal_pose`   | bool   | Enabling correction of goal pose according to the closest lanelet orientation                                    |
-| `reroute_time_threshold`     | double | If the time to the rerouting point at the current velocity is greater than this threshold, rerouting is possible |
-| `minimum_reroute_length`     | double | Minimum Length for publishing a new route                                                                        |
-| `consider_no_drivable_lanes` | bool   | This flag is for considering no_drivable_lanes in planning or not.                                               |
+| Name                               | Type   | Description                                                                                                                |
+| ---------------------------------- | ------ | -------------------------------------------------------------------------------------------------------------------------- |
+| `map_frame`                        | string | The frame name for map                                                                                                     |
+| `arrival_check_angle_deg`          | double | Angle threshold for goal check                                                                                             |
+| `arrival_check_distance`           | double | Distance threshold for goal check                                                                                          |
+| `arrival_check_duration`           | double | Duration threshold for goal check                                                                                          |
+| `goal_angle_threshold`             | double | Max goal pose angle for goal approve                                                                                       |
+| `enable_correct_goal_pose`         | bool   | Enabling correction of goal pose according to the closest lanelet orientation                                              |
+| `reroute_time_threshold`           | double | If the time to the rerouting point at the current velocity is greater than this threshold, rerouting is possible           |
+| `minimum_reroute_length`           | double | Minimum Length for publishing a new route                                                                                  |
+| `consider_no_drivable_lanes`       | bool   | This flag is for considering no_drivable_lanes in planning or not.                                                         |
+| `allow_reroute_in_autonomous_mode` | bool   | This is a flag to allow reroute in autonomous driving mode. If false, reroute fails. If true, only safe reroute is allowed |
 
 ### Services
 

diff --git a/planning/autoware_mission_planner/config/mission_planner.param.yaml b/planning/autoware_mission_planner/config/mission_planner.param.yaml
@@ -10,3 +10,4 @@
     minimum_reroute_length: 30.0
     consider_no_drivable_lanes: false # This flag is for considering no_drivable_lanes in planning or not.
     check_footprint_inside_lanes: true
+    allow_reroute_in_autonomous_mode: true
diff --git a/planning/autoware_mission_planner/src/mission_planner/mission_planner.cpp b/planning/autoware_mission_planner/src/mission_planner/mission_planner.cpp
@@ -47,6 +47,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");
+  allow_reroute_in_autonomous_mode_ = declare_parameter<bool>("allow_reroute_in_autonomous_mode");
 
   planner_ =
     plugin_loader_.createSharedInstance("autoware::mission_planner::lanelet2::DefaultPlanner");
@@ -240,6 +241,11 @@ void MissionPlanner::on_set_lanelet_route(
                               operation_mode_state_->is_autoware_control_enabled
                           : false;
 
+  if (is_reroute && !allow_reroute_in_autonomous_mode_ && is_autonomous_driving) {
+    throw service_utils::ServiceException(
+      ResponseCode::ERROR_INVALID_STATE, "Reroute is not allowed in autonomous mode.");
+  }
+
   if (is_reroute && is_autonomous_driving) {
     const auto reroute_availability = sub_reroute_availability_.takeData();
     if (!reroute_availability || !reroute_availability->availability) {

diff --git a/planning/autoware_mission_planner/src/mission_planner/mission_planner.hpp b/planning/autoware_mission_planner/src/mission_planner/mission_planner.hpp
@@ -135,6 +135,9 @@ class MissionPlanner : public rclcpp::Node
 
   double reroute_time_threshold_;
   double minimum_reroute_length_;
+  // flag to allow reroute in autonomous driving mode.
+  // if false, reroute fails. if true, only safe reroute is allowed.
+  bool allow_reroute_in_autonomous_mode_;
   bool check_reroute_safety(const LaneletRoute & original_route, const LaneletRoute & target_route);
 
   std::unique_ptr<autoware::universe_utils::LoggerLevelConfigure> logger_configure_;