fix: return false to manual lane change request when lane change is working

planning/autoware_manual_lane_change_handler/README.md

@@ -20,10 +20,11 @@ ros2 service call /planning/mission_planning/manual_lane_change_handler/set_pref
 
 ### Subscriptions
 
-| Name                               | Type                                    | Description           |
-| ---------------------------------- | --------------------------------------- | --------------------- |
-| `input/odometry`                   | nav_msgs/msg/Odometry                   | vehicle odometry      |
-| `/planning/mission_planning/route` | autoware_planning_msgs/msg/LaneletRoute | current lanelet route |
+| Name                                                                                                           | Type                                        | Description                                                                                                                                           |
+| -------------------------------------------------------------------------------------------------------------- | ------------------------------------------- | ----------------------------------------------------------------------------------------------------------------------------------------------------- |
+| `input/odometry`                                                                                               | nav_msgs/msg/Odometry                       | vehicle odometry                                                                                                                                      |
+| `/planning/mission_planning/route`                                                                             | autoware_planning_msgs/msg/LaneletRoute     | current lanelet route                                                                                                                                 |
+| `/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/output/is_reroute_available` | tier4_planning_msgs/msg/RerouteAvailability | Availability to reroute generally published by the behavior_path_planner. The `set_preferred_lane` service will be denied when this topic is `false`. |
 
 ### Publications
 
@@ -54,6 +55,7 @@ Based on certain criteria, shifting may be rejected, as in the following cases:
 
 1. Left or Right shift is not available due to no lane being present to shift to
 2. The next segment is a turn or the very last lane - this is to ensure that we can navigate the enter path and end up at the goal
+3. The reroute availability provided by the behavior_path_planner is `false`
 
 ```plantuml
 @startuml

planning/autoware_manual_lane_change_handler/launch/manual_lane_change_handler.launch.xml

@@ -6,5 +6,6 @@
     <param from="$(var manual_lane_change_handler_param_path)"/>
     <remap from="~/input/vector_map" to="$(var map_topic_name)"/>
     <remap from="~/input/odometry" to="/localization/kinematic_state"/>
+    <remap from="~/input/reroute_availability" to="/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/output/is_reroute_available"/>
   </node>
 </launch>

planning/autoware_manual_lane_change_handler/src/manual_lane_change_handler.cpp

@@ -148,6 +148,13 @@ void ManualLaneChangeHandler::set_preferred_lane(
     return;
   }
 
+  const auto reroute_availability = sub_reroute_availability_.take_data();
+  if (!reroute_availability || !reroute_availability->availability) {
+    res->status.success = false;
+    res->status.message = "Not in lane driving state. Wait for the current scenario to end.";
+    return;
+  }
+
   lanelet::ConstLanelet closest_lanelet =
     get_lanelet_by_id(current_route_->segments.front().preferred_primitive.id);
   const bool found_closest_lane = planner_->getRouteHandler().getClosestLaneletWithinRoute(

planning/autoware_manual_lane_change_handler/src/manual_lane_change_handler.hpp

@@ -19,6 +19,7 @@
 #include <autoware/mission_planner_universe/service_utils.hpp>
 #include <autoware_lanelet2_extension/utility/query.hpp>
 #include <autoware_lanelet2_extension/utility/utilities.hpp>
+#include <autoware_utils/ros/polling_subscriber.hpp>
 #include <pluginlib/class_loader.hpp>
 #include <rclcpp/rclcpp.hpp>
 
@@ -29,6 +30,7 @@
 #include <autoware_planning_msgs/srv/set_preferred_primitive.hpp>
 #include <nav_msgs/msg/odometry.hpp>
 #include <tier4_external_api_msgs/srv/set_preferred_lane.hpp>
+#include <tier4_planning_msgs/msg/reroute_availability.hpp>
 
 #include <boost/uuid/uuid.hpp>
 #include <boost/uuid/uuid_generators.hpp>
@@ -94,6 +96,8 @@ class ManualLaneChangeHandler : public rclcpp::Node
   rclcpp::Service<SetPreferredLane>::SharedPtr srv_set_preferred_lane;
   rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr sub_odometry_;
   rclcpp::Subscription<LaneletRoute>::SharedPtr sub_route_;
+  autoware_utils::InterProcessPollingSubscriber<tier4_planning_msgs::msg::RerouteAvailability>
+    sub_reroute_availability_{this, "~/input/reroute_availability"};
   rclcpp::Publisher<autoware_internal_debug_msgs::msg::Float64Stamped>::SharedPtr
     pub_processing_time_;
   rclcpp::Publisher<autoware_internal_debug_msgs::msg::Int32Stamped>::SharedPtr pub_shift_number_;

planning/autoware_mission_planner_universe/src/mission_planner/mission_planner.cpp

@@ -333,6 +333,7 @@ void MissionPlanner::on_set_preferred_primitive(
   const autoware_planning_msgs::srv::SetPreferredPrimitive::Response::SharedPtr res)
 {
   using ResponseCode = autoware_adapi_v1_msgs::msg::ResponseStatus;
+  const auto is_reroute = state_.state == RouteState::SET;
 
   if (!current_route_) {
     res->status.success = false;
@@ -366,6 +367,20 @@ void MissionPlanner::on_set_preferred_primitive(
       "There is no saved original route to reset to.");
   }
 
+  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_.take_data();
+    if (!reroute_availability || !reroute_availability->availability) {
+      throw service_utils::ServiceException(
+        autoware_adapi_v1_msgs::srv::SetRoute::Response::ERROR_INVALID_STATE,
+        "Cannot reroute as the planner is not in lane following.");
+    }
+  }
+
   if (req->reset) {
     RCLCPP_INFO(get_logger(), "Cleared the saved original route after reset.");