diff --git a/planning/autoware_mission_planner/README.md b/planning/autoware_mission_planner/README.md
index 2f912e30b8246..452d0a64d5c9c 100644
--- 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
index 655662c392213..8d77e417a6379 100644
--- a/planning/autoware_mission_planner/launch/mission_planner.launch.xml
+++ b/planning/autoware_mission_planner/launch/mission_planner.launch.xml
@@ -10,6 +10,7 @@
+
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 e484b9915aa87..da3e5411e818b 100644
--- 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(
"~/input/odometry", rclcpp::QoS(1), std::bind(&MissionPlanner::on_odometry, this, _1));
+ sub_operation_mode_state_ = create_subscription(
+ "~/input/operation_mode_state", rclcpp::QoS(1),
+ std::bind(&MissionPlanner::on_operation_mode_state, this, _1));
sub_vector_map_ = create_subscription(
"~/input/vector_map", durable_qos, std::bind(&MissionPlanner::on_map, this, _1));
pub_marker_ = create_publisher("~/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
index 1a04a91c14ba3..1181ef54273ae 100644
--- 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
#include
+#include
#include
#include
#include
@@ -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::SharedPtr sub_modified_goal_;
rclcpp::Subscription::SharedPtr sub_odometry_;
+ rclcpp::Subscription::SharedPtr sub_operation_mode_state_;
autoware::universe_utils::InterProcessPollingSubscriber
sub_reroute_availability_{this, "~/input/reroute_availability"};
rclcpp::Subscription::SharedPtr sub_vector_map_;
rclcpp::Publisher::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);