From 1a6f528483106b48428049ffd39786417faaa5bd Mon Sep 17 00:00:00 2001 From: Fumiya Watanabe Date: Fri, 5 Jul 2024 16:37:00 +0900 Subject: [PATCH 1/2] 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); From 2a3e90f27a975055a8440b3dfbd54c65dee91908 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Wed, 11 Sep 2024 15:39:56 +0900 Subject: [PATCH 2/2] feat(mission_planner): add option to prevent rerouting in autonomous driving mode (#8757) Signed-off-by: kosuke55 --- planning/autoware_mission_planner/README.md | 23 ++++++++++--------- .../config/mission_planner.param.yaml | 1 + .../src/mission_planner/mission_planner.cpp | 6 +++++ .../src/mission_planner/mission_planner.hpp | 3 +++ 4 files changed, 22 insertions(+), 11 deletions(-) diff --git a/planning/autoware_mission_planner/README.md b/planning/autoware_mission_planner/README.md index 452d0a64d5c9c..b5993d0106add 100644 --- a/planning/autoware_mission_planner/README.md +++ b/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 diff --git a/planning/autoware_mission_planner/config/mission_planner.param.yaml b/planning/autoware_mission_planner/config/mission_planner.param.yaml index 9b7dcffbc687b..ecfbdab9e569f 100644 --- a/planning/autoware_mission_planner/config/mission_planner.param.yaml +++ b/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 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 da3e5411e818b..0f47938d479c7 100644 --- a/planning/autoware_mission_planner/src/mission_planner/mission_planner.cpp +++ b/planning/autoware_mission_planner/src/mission_planner/mission_planner.cpp @@ -47,6 +47,7 @@ MissionPlanner::MissionPlanner(const rclcpp::NodeOptions & options) map_frame_ = declare_parameter("map_frame"); reroute_time_threshold_ = declare_parameter("reroute_time_threshold"); minimum_reroute_length_ = declare_parameter("minimum_reroute_length"); + allow_reroute_in_autonomous_mode_ = declare_parameter("allow_reroute_in_autonomous_mode"); planner_ = plugin_loader_.createSharedInstance("autoware::mission_planner::lanelet2::DefaultPlanner"); @@ -240,6 +241,11 @@ void MissionPlanner::on_set_lanelet_route( 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, "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) { 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 1181ef54273ae..99384e9ddeb3b 100644 --- a/planning/autoware_mission_planner/src/mission_planner/mission_planner.hpp +++ b/planning/autoware_mission_planner/src/mission_planner/mission_planner.hpp @@ -135,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 logger_configure_;