feat(mission_planner): reroute in manual driving (autowarefoundation#…

…7842) * feat(mission_planner): reroute in manual driving Signed-off-by: Fumiya Watanabe <[email protected]> * docs(mission_planner): update document Signed-off-by: Fumiya Watanabe <[email protected]> * feat(mission_planner): fix operation mode state receiving check Signed-off-by: Fumiya Watanabe <[email protected]> --------- Signed-off-by: Fumiya Watanabe <[email protected]>
NorahXiong · Jul 5, 2024 · 5be7627 · 5be7627
1 parent 3627afc
commit 5be7627
Show file tree

Hide file tree

Showing 4 changed files with 54 additions and 12 deletions.
diff --git a/planning/autoware_mission_planner/README.md b/planning/autoware_mission_planner/README.md
@@ -47,10 +47,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 +171,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)
 

diff --git a/planning/autoware_mission_planner/launch/mission_planner.launch.xml b/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"/>

diff --git a/planning/autoware_mission_planner/src/mission_planner/mission_planner.cpp b/planning/autoware_mission_planner/src/mission_planner/mission_planner.cpp
@@ -55,6 +55,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 +133,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 +230,23 @@ 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_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);
@@ -238,7 +259,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 +292,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 +321,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(

diff --git a/planning/autoware_mission_planner/src/mission_planner/mission_planner.hpp b/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);