Merge pull request #1583 from tier4/feat/reroute

* feat(mission_planner): reroute in manual driving (autowarefoundation#7842)

* feat(mission_planner): reroute in manual driving

Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com>

* docs(mission_planner): update document

Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com>

* feat(mission_planner): fix operation mode state receiving check

Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com>

---------

Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com>

* feat(mission_planner): add option to prevent rerouting in autonomous driving mode (autowarefoundation#8757)

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

---------

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

Author: 3 people

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
 
@@ -47,10 +48,11 @@ It distributes route requests and planning results according to current MRM oper
 
 ### Subscriptions
 
-| Name                  | Type                                | Description            |
-| --------------------- | ----------------------------------- | ---------------------- |
-| `input/vector_map`    | autoware_map_msgs/msg/LaneletMapBin | vector map of Lanelet2 |
-| `input/modified_goal` | geometry_msgs/PoseWithUuidStamped   | modified goal pose     |
+| Name                         | Type                                      | Description            |
+| ---------------------------- | ----------------------------------------- | ---------------------- |
+| `input/vector_map`           | autoware_map_msgs/msg/LaneletMapBin       | vector map of Lanelet2 |
+| `input/modified_goal`        | geometry_msgs/PoseWithUuidStamped         | modified goal pose     |
+| `input/operation_mode_state` | autoware_adapi_v1_msgs/OperationModeState | operation mode state   |
 
 ### Publications
 
@@ -170,6 +172,7 @@ To calculate `route_lanelets`,
 ### Rerouting
 
 Reroute here means changing the route while driving. Unlike route setting, it is required to keep a certain distance from vehicle to the point where the route is changed.
+If the ego vehicle is not on autonomous driving state, the safety checking process will be skipped.
 
 ![rerouting_safety](./media/rerouting_safety.svg)
 

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

planning/autoware_mission_planner/launch/mission_planner.launch.xml

@@ -10,6 +10,7 @@
       <remap from="~/input/modified_goal" to="$(var modified_goal_topic_name)"/>
       <remap from="~/input/vector_map" to="$(var map_topic_name)"/>
       <remap from="~/input/odometry" to="/localization/kinematic_state"/>
+      <remap from="~/input/operation_mode_state" to="/system/operation_mode/state"/>
       <remap from="~/input/reroute_availability" to="/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/output/is_reroute_available"/>
       <remap from="~/route" to="route"/>
       <remap from="~/state" to="state"/>

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");
@@ -55,6 +56,9 @@ MissionPlanner::MissionPlanner(const rclcpp::NodeOptions & options)
   const auto durable_qos = rclcpp::QoS(1).transient_local();
   sub_odometry_ = create_subscription<Odometry>(
     "~/input/odometry", rclcpp::QoS(1), std::bind(&MissionPlanner::on_odometry, this, _1));
+  sub_operation_mode_state_ = create_subscription<OperationModeState>(
+    "~/input/operation_mode_state", rclcpp::QoS(1),
+    std::bind(&MissionPlanner::on_operation_mode_state, this, _1));
   sub_vector_map_ = create_subscription<LaneletMapBin>(
     "~/input/vector_map", durable_qos, std::bind(&MissionPlanner::on_map, this, _1));
   pub_marker_ = create_publisher<MarkerArray>("~/debug/route_marker", durable_qos);
@@ -130,6 +134,11 @@ void MissionPlanner::on_odometry(const Odometry::ConstSharedPtr msg)
   }
 }
 
+void MissionPlanner::on_operation_mode_state(const OperationModeState::ConstSharedPtr msg)
+{
+  operation_mode_state_ = msg;
+}
+
 void MissionPlanner::on_map(const LaneletMapBin::ConstSharedPtr msg)
 {
   map_ptr_ = msg;
@@ -222,10 +231,28 @@ void MissionPlanner::on_set_lanelet_route(
     throw service_utils::ServiceException(
       ResponseCode::ERROR_PLANNER_UNREADY, "The vehicle pose is not received.");
   }
-  const auto reroute_availability = sub_reroute_availability_.takeData();
-  if (is_reroute && (!reroute_availability || !reroute_availability->availability)) {
+  if (is_reroute && !operation_mode_state_) {
+    throw service_utils::ServiceException(
+      ResponseCode::ERROR_PLANNER_UNREADY, "Operation mode state is not received.");
+  }
+
+  const bool is_autonomous_driving =
+    operation_mode_state_ ? operation_mode_state_->mode == OperationModeState::AUTONOMOUS &&
+                              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, "Cannot reroute as the planner is not in lane following.");
+      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) {
+      throw service_utils::ServiceException(
+        ResponseCode::ERROR_INVALID_STATE,
+        "Cannot reroute as the planner is not in lane following.");
+    }
   }
 
   change_state(is_reroute ? RouteState::REROUTING : RouteState::ROUTING);
@@ -238,7 +265,7 @@ void MissionPlanner::on_set_lanelet_route(
       ResponseCode::ERROR_PLANNER_FAILED, "The planned route is empty.");
   }
 
-  if (is_reroute && !check_reroute_safety(*current_route_, route)) {
+  if (is_reroute && is_autonomous_driving && !check_reroute_safety(*current_route_, route)) {
     cancel_route();
     change_state(RouteState::SET);
     throw service_utils::ServiceException(
@@ -271,10 +298,23 @@ void MissionPlanner::on_set_waypoint_route(
     throw service_utils::ServiceException(
       ResponseCode::ERROR_PLANNER_UNREADY, "The vehicle pose is not received.");
   }
-  const auto reroute_availability = sub_reroute_availability_.takeData();
-  if (is_reroute && (!reroute_availability || !reroute_availability->availability)) {
+  if (is_reroute && !operation_mode_state_) {
     throw service_utils::ServiceException(
-      ResponseCode::ERROR_INVALID_STATE, "Cannot reroute as the planner is not in lane following.");
+      ResponseCode::ERROR_PLANNER_UNREADY, "Operation mode state is not received.");
+  }
+
+  const bool is_autonomous_driving =
+    operation_mode_state_ ? operation_mode_state_->mode == OperationModeState::AUTONOMOUS &&
+                              operation_mode_state_->is_autoware_control_enabled
+                          : false;
+
+  if (is_reroute && is_autonomous_driving) {
+    const auto reroute_availability = sub_reroute_availability_.takeData();
+    if (!reroute_availability || !reroute_availability->availability) {
+      throw service_utils::ServiceException(
+        ResponseCode::ERROR_INVALID_STATE,
+        "Cannot reroute as the planner is not in lane following.");
+    }
   }
 
   change_state(is_reroute ? RouteState::REROUTING : RouteState::ROUTING);
@@ -287,7 +327,7 @@ void MissionPlanner::on_set_waypoint_route(
       ResponseCode::ERROR_PLANNER_FAILED, "The planned route is empty.");
   }
 
-  if (is_reroute && !check_reroute_safety(*current_route_, route)) {
+  if (is_reroute && is_autonomous_driving && !check_reroute_safety(*current_route_, route)) {
     cancel_route();
     change_state(RouteState::SET);
     throw service_utils::ServiceException(

planning/autoware_mission_planner/src/mission_planner/mission_planner.hpp

@@ -24,6 +24,7 @@
 #include <pluginlib/class_loader.hpp>
 #include <rclcpp/rclcpp.hpp>
 
+#include <autoware_adapi_v1_msgs/msg/operation_mode_state.hpp>
 #include <autoware_adapi_v1_msgs/srv/set_route.hpp>
 #include <autoware_adapi_v1_msgs/srv/set_route_points.hpp>
 #include <autoware_planning_msgs/msg/lanelet_route.hpp>
@@ -45,6 +46,7 @@
 namespace autoware::mission_planner
 {
 
+using autoware_adapi_v1_msgs::msg::OperationModeState;
 using autoware_map_msgs::msg::LaneletMapBin;
 using autoware_planning_msgs::msg::LaneletPrimitive;
 using autoware_planning_msgs::msg::LaneletRoute;
@@ -85,18 +87,21 @@ class MissionPlanner : public rclcpp::Node
 
   rclcpp::Subscription<PoseWithUuidStamped>::SharedPtr sub_modified_goal_;
   rclcpp::Subscription<Odometry>::SharedPtr sub_odometry_;
+  rclcpp::Subscription<OperationModeState>::SharedPtr sub_operation_mode_state_;
   autoware::universe_utils::InterProcessPollingSubscriber<RerouteAvailability>
     sub_reroute_availability_{this, "~/input/reroute_availability"};
 
   rclcpp::Subscription<LaneletMapBin>::SharedPtr sub_vector_map_;
   rclcpp::Publisher<MarkerArray>::SharedPtr pub_marker_;
   Odometry::ConstSharedPtr odometry_;
+  OperationModeState::ConstSharedPtr operation_mode_state_;
   LaneletMapBin::ConstSharedPtr map_ptr_;
   RouteState state_;
   LaneletRoute::ConstSharedPtr current_route_;
   lanelet::LaneletMapPtr lanelet_map_ptr_{nullptr};
 
   void on_odometry(const Odometry::ConstSharedPtr msg);
+  void on_operation_mode_state(const OperationModeState::ConstSharedPtr msg);
   void on_map(const LaneletMapBin::ConstSharedPtr msg);
   void on_reroute_availability(const RerouteAvailability::ConstSharedPtr msg);
   void on_modified_goal(const PoseWithUuidStamped::ConstSharedPtr msg);
@@ -130,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_;