From 1a6f528483106b48428049ffd39786417faaa5bd Mon Sep 17 00:00:00 2001 From: Fumiya Watanabe Date: Fri, 5 Jul 2024 16:37:00 +0900 Subject: [PATCH] feat(mission_planner): reroute in manual driving (#7842) * feat(mission_planner): reroute in manual driving Signed-off-by: Fumiya Watanabe * docs(mission_planner): update document Signed-off-by: Fumiya Watanabe * feat(mission_planner): fix operation mode state receiving check Signed-off-by: Fumiya Watanabe --------- Signed-off-by: Fumiya Watanabe --- planning/autoware_mission_planner/README.md | 10 ++-- .../launch/mission_planner.launch.xml | 1 + .../src/mission_planner/mission_planner.cpp | 50 ++++++++++++++++--- .../src/mission_planner/mission_planner.hpp | 5 ++ 4 files changed, 54 insertions(+), 12 deletions(-) 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);