From 2a3e90f27a975055a8440b3dfbd54c65dee91908 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Wed, 11 Sep 2024 15:39:56 +0900 Subject: [PATCH] feat(mission_planner): add option to prevent rerouting in autonomous driving mode (#8757) Signed-off-by: kosuke55 --- planning/autoware_mission_planner/README.md | 23 ++++++++++--------- .../config/mission_planner.param.yaml | 1 + .../src/mission_planner/mission_planner.cpp | 6 +++++ .../src/mission_planner/mission_planner.hpp | 3 +++ 4 files changed, 22 insertions(+), 11 deletions(-) diff --git a/planning/autoware_mission_planner/README.md b/planning/autoware_mission_planner/README.md index 452d0a64d5c9c..b5993d0106add 100644 --- 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 index 9b7dcffbc687b..ecfbdab9e569f 100644 --- 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 index da3e5411e818b..0f47938d479c7 100644 --- 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("map_frame"); reroute_time_threshold_ = declare_parameter("reroute_time_threshold"); minimum_reroute_length_ = declare_parameter("minimum_reroute_length"); + allow_reroute_in_autonomous_mode_ = declare_parameter("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 index 1181ef54273ae..99384e9ddeb3b 100644 --- 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 logger_configure_;