From 96d22a677756163662705d83e1b50074722f28ea Mon Sep 17 00:00:00 2001 From: yutaka Date: Thu, 25 May 2023 20:35:17 +0900 Subject: [PATCH 1/3] feat(mission_planner): add mrm reroute Signed-off-by: yutaka --- .../src/mission_planner/mission_planner.cpp | 188 ++++++++++++++++-- .../src/mission_planner/mission_planner.hpp | 13 +- 2 files changed, 178 insertions(+), 23 deletions(-) diff --git a/planning/mission_planner/src/mission_planner/mission_planner.cpp b/planning/mission_planner/src/mission_planner/mission_planner.cpp index b3df2d0a0010a..d4434f761445b 100644 --- a/planning/mission_planner/src/mission_planner/mission_planner.cpp +++ b/planning/mission_planner/src/mission_planner/mission_planner.cpp @@ -75,8 +75,10 @@ MissionPlanner::MissionPlanner(const rclcpp::NodeOptions & options) plugin_loader_("mission_planner", "mission_planner::PlannerPlugin"), tf_buffer_(get_clock()), tf_listener_(tf_buffer_), + is_emergency_{false}, original_route_(nullptr), - normal_route_(nullptr) + normal_route_(nullptr), + mrm_route_{nullptr} { map_frame_ = declare_parameter("map_frame"); reroute_time_threshold_ = declare_parameter("reroute_time_threshold"); @@ -154,6 +156,11 @@ void MissionPlanner::clear_route() // TODO(Takagi, Isamu): publish an empty route here } +void MissionPlanner::clear_mrm_route() +{ + mrm_route_ = nullptr; +} + void MissionPlanner::change_route(const LaneletRoute & route) { PoseWithUuidStamped goal; @@ -170,43 +177,64 @@ void MissionPlanner::change_route(const LaneletRoute & route) normal_route_ = std::make_shared(route); } -LaneletRoute MissionPlanner::create_route(const SetRoute::Service::Request::SharedPtr req) +void MissionPlanner::change_mrm_route(const LaneletRoute & route) { - PoseStamped goal_pose; - goal_pose.header = req->header; - goal_pose.pose = req->goal; + PoseWithUuidStamped goal; + goal.header = route.header; + goal.pose = route.goal_pose; + goal.uuid = route.uuid; + + arrival_checker_.set_goal(goal); + pub_route_->publish(route); + pub_marker_->publish(planner_->visualize(route)); + planner_->updateRoute(route); + + // update emergency route + mrm_route_ = std::make_shared(route); +} + +LaneletRoute MissionPlanner::create_route( + const std_msgs::msg::Header & header, + const std::vector & route_segments, + const geometry_msgs::msg::Pose & goal_pose, const bool allow_goal_modification) +{ + PoseStamped goal_pose_stamped; + goal_pose_stamped.header = header; + goal_pose_stamped.pose = goal_pose; // Convert route. LaneletRoute route; route.start_pose = odometry_->pose.pose; - route.goal_pose = transform_pose(goal_pose).pose; - for (const auto & segment : req->segments) { + route.goal_pose = transform_pose(goal_pose_stamped).pose; + for (const auto & segment : route_segments) { route.segments.push_back(convert(segment)); } - route.header.stamp = req->header.stamp; + route.header.stamp = header.stamp; route.header.frame_id = map_frame_; route.uuid.uuid = generate_random_id(); - route.allow_modification = req->option.allow_goal_modification; + route.allow_modification = allow_goal_modification; return route; } -LaneletRoute MissionPlanner::create_route(const SetRoutePoints::Service::Request::SharedPtr req) +LaneletRoute MissionPlanner::create_route( + const std_msgs::msg::Header & header, const std::vector & waypoints, + const geometry_msgs::msg::Pose & goal_pose, const bool allow_goal_modification) { using ResponseCode = autoware_adapi_v1_msgs::srv::SetRoute::Response; // Use temporary pose stamped for transform. PoseStamped pose; - pose.header = req->header; + pose.header = header; // Convert route points. PlannerPlugin::RoutePoints points; points.push_back(odometry_->pose.pose); - for (const auto & waypoint : req->waypoints) { + for (const auto & waypoint : waypoints) { pose.pose = waypoint; points.push_back(transform_pose(pose).pose); } - pose.pose = req->goal; + pose.pose = goal_pose; points.push_back(transform_pose(pose).pose); // Plan route. @@ -215,14 +243,34 @@ LaneletRoute MissionPlanner::create_route(const SetRoutePoints::Service::Request throw component_interface_utils::ServiceException( ResponseCode::ERROR_PLANNER_FAILED, "The planned route is empty."); } - route.header.stamp = req->header.stamp; + route.header.stamp = header.stamp; route.header.frame_id = map_frame_; route.uuid.uuid = generate_random_id(); - route.allow_modification = req->option.allow_goal_modification; + route.allow_modification = allow_goal_modification; return route; } +LaneletRoute MissionPlanner::create_route(const SetRoute::Service::Request::SharedPtr req) +{ + const auto & header = req->header; + const auto & route_segments = req->segments; + const auto & goal_pose = req->goal; + const auto & allow_goal_modification = req->option.allow_goal_modification; + + return create_route(header, route_segments, goal_pose, allow_goal_modification); +} + +LaneletRoute MissionPlanner::create_route(const SetRoutePoints::Service::Request::SharedPtr req) +{ + const auto & header = req->header; + const auto & waypoints = req->waypoints; + const auto & goal_pose = req->goal; + const auto & allow_goal_modification = req->option.allow_goal_modification; + + return create_route(header, waypoints, goal_pose, allow_goal_modification); +} + void MissionPlanner::change_state(RouteState::Message::_state_type state) { state_.stamp = now(); @@ -253,6 +301,11 @@ void MissionPlanner::on_set_route( throw component_interface_utils::ServiceException( ResponseCode::ERROR_PLANNER_UNREADY, "The vehicle pose is not received."); } + if (is_emergency_) { + RCLCPP_ERROR_THROTTLE( + get_logger(), *get_clock(), 5000, "Cannot set route because of the emergency state"); + return; + } // Convert request to a new route. const auto route = create_route(req); @@ -283,6 +336,11 @@ void MissionPlanner::on_set_route_points( throw component_interface_utils::ServiceException( ResponseCode::ERROR_PLANNER_UNREADY, "The vehicle pose is not received."); } + if (is_emergency_) { + RCLCPP_ERROR_THROTTLE( + get_logger(), *get_clock(), 5000, "Cannot set route points because of the emergency state"); + return; + } // Plan route. const auto route = create_route(req); @@ -299,19 +357,105 @@ void MissionPlanner::on_set_mrm_route( const SetMrmRoute::Service::Request::SharedPtr req, const SetMrmRoute::Service::Response::SharedPtr res) { - // TODO(Yutaka Shimizu): reroute for MRM - (void)req; - (void)res; + using ResponseCode = autoware_adapi_v1_msgs::srv::SetRoutePoints::Response; + change_state(RouteState::Message::CHANGING); + + if (!planner_->ready()) { + change_state(RouteState::Message::SET); + throw component_interface_utils::ServiceException( + ResponseCode::ERROR_PLANNER_UNREADY, "The planner is not ready."); + } + if (!odometry_) { + change_state(RouteState::Message::SET); + throw component_interface_utils::ServiceException( + ResponseCode::ERROR_PLANNER_UNREADY, "The vehicle pose is not received."); + } + if (!normal_route_) { + change_state(RouteState::Message::SET); + throw component_interface_utils::ServiceException( + ResponseCode::ERROR_PLANNER_UNREADY, "Normal route is not set."); + } + + // Plan route. + const auto new_route = create_route(req); + + // check route safety + if (checkRerouteSafety(*normal_route_, new_route)) { + // sucess to reroute + change_mrm_route(new_route); + change_state(RouteState::Message::SET); + res->status.success = true; + is_emergency_ = true; + } else { + // failed to reroute + change_route(*normal_route_); + change_state(RouteState::Message::SET); + res->status.success = false; + is_emergency_ = false; + } } // NOTE: The route interface should be mutually exclusive by callback group. void MissionPlanner::on_clear_mrm_route( - const ClearMrmRoute::Service::Request::SharedPtr req, + [[maybe_unused]] const ClearMrmRoute::Service::Request::SharedPtr req, const ClearMrmRoute::Service::Response::SharedPtr res) { - // TODO(Yutaka Shimizu): reroute for MRM - (void)req; - (void)res; + using ResponseCode = autoware_adapi_v1_msgs::srv::SetRoutePoints::Response; + change_state(RouteState::Message::CHANGING); + + if (!planner_->ready()) { + change_state(RouteState::Message::SET); + throw component_interface_utils::ServiceException( + ResponseCode::ERROR_PLANNER_UNREADY, "The planner is not ready."); + } + if (!odometry_) { + change_state(RouteState::Message::SET); + throw component_interface_utils::ServiceException( + ResponseCode::ERROR_PLANNER_UNREADY, "The vehicle pose is not received."); + } + if (!normal_route_) { + change_state(RouteState::Message::SET); + throw component_interface_utils::ServiceException( + ResponseCode::ERROR_PLANNER_UNREADY, "Normal route is not set."); + } + if (!mrm_route_) { + change_state(RouteState::Message::SET); + throw component_interface_utils::ServiceException( + ResponseCode::ERROR_PLANNER_UNREADY, "Mrm route is not set."); + } + + // check route safety + if (checkRerouteSafety(*mrm_route_, *normal_route_)) { + clear_mrm_route(); + change_route(*normal_route_); + change_state(RouteState::Message::SET); + res->success = true; + is_emergency_ = false; + return; + } + + // reroute with normal goal + const std::vector empty_waypoints; + const auto new_route = create_route( + odometry_->header, empty_waypoints, normal_route_->goal_pose, + normal_route_->allow_modification); + + // check new route safety + if (new_route.segments.empty() || !checkRerouteSafety(*mrm_route_, new_route)) { + // failed to create a new route + RCLCPP_ERROR_THROTTLE(get_logger(), *get_clock(), 5000, "Reroute with normal goal failed."); + change_mrm_route(*mrm_route_); + change_route(*normal_route_); + res->success = false; + is_emergency_ = true; + } else { + clear_mrm_route(); + change_route(new_route); + res->success = true; + is_emergency_ = false; + } + + change_state(RouteState::Message::SET); } void MissionPlanner::on_modified_goal(const ModifiedGoal::Message::ConstSharedPtr msg) diff --git a/planning/mission_planner/src/mission_planner/mission_planner.hpp b/planning/mission_planner/src/mission_planner/mission_planner.hpp index f108e2d99496c..9068d4bb10047 100644 --- a/planning/mission_planner/src/mission_planner/mission_planner.hpp +++ b/planning/mission_planner/src/mission_planner/mission_planner.hpp @@ -75,9 +75,18 @@ class MissionPlanner : public rclcpp::Node rclcpp::Publisher::SharedPtr pub_marker_; void clear_route(); + void clear_mrm_route(); void change_route(const LaneletRoute & route); + void change_mrm_route(const LaneletRoute & route); LaneletRoute create_route(const SetRoute::Service::Request::SharedPtr req); LaneletRoute create_route(const SetRoutePoints::Service::Request::SharedPtr req); + LaneletRoute create_route( + const std_msgs::msg::Header & header, + const std::vector & route_segments, + const geometry_msgs::msg::Pose & goal_pose, const bool allow_goal_modification); + LaneletRoute create_route( + const std_msgs::msg::Header & header, const std::vector & waypoints, + const geometry_msgs::msg::Pose & goal_pose, const bool allow_goal_modification); RouteState::Message state_; component_interface_utils::Publisher::SharedPtr pub_state_; @@ -105,7 +114,7 @@ class MissionPlanner : public rclcpp::Node const SetMrmRoute::Service::Request::SharedPtr req, const SetMrmRoute::Service::Response::SharedPtr res); void on_clear_mrm_route( - const ClearMrmRoute::Service::Request::SharedPtr req, + [[maybe_unused]] const ClearMrmRoute::Service::Request::SharedPtr req, const ClearMrmRoute::Service::Response::SharedPtr res); HADMapBin::ConstSharedPtr map_ptr_{nullptr}; @@ -124,8 +133,10 @@ class MissionPlanner : public rclcpp::Node double minimum_reroute_length_{30.0}; bool checkRerouteSafety(const LaneletRoute & original_route, const LaneletRoute & target_route); + bool is_emergency_{false}; std::shared_ptr original_route_{nullptr}; std::shared_ptr normal_route_{nullptr}; + std::shared_ptr mrm_route_{nullptr}; }; } // namespace mission_planner From ec8961ae0726f6fd85799c35179deaa72e08c915 Mon Sep 17 00:00:00 2001 From: yutaka Date: Fri, 26 May 2023 01:23:26 +0900 Subject: [PATCH 2/3] update Signed-off-by: yutaka --- .../src/mission_planner/mission_planner.cpp | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/planning/mission_planner/src/mission_planner/mission_planner.cpp b/planning/mission_planner/src/mission_planner/mission_planner.cpp index d4434f761445b..c2b010fb909f6 100644 --- a/planning/mission_planner/src/mission_planner/mission_planner.cpp +++ b/planning/mission_planner/src/mission_planner/mission_planner.cpp @@ -477,6 +477,11 @@ void MissionPlanner::on_change_route( throw component_interface_utils::ServiceException( ResponseCode::ERROR_PLANNER_UNREADY, "The vehicle pose is not received."); } + if (is_emergency_) { + RCLCPP_ERROR_THROTTLE( + get_logger(), *get_clock(), 5000, "Cannot reroute because of the emergency state"); + return; + } // set to changing state change_state(RouteState::Message::CHANGING); @@ -519,6 +524,11 @@ void MissionPlanner::on_change_route_points( throw component_interface_utils::ServiceException( ResponseCode::ERROR_PLANNER_UNREADY, "The vehicle pose is not received."); } + if (is_emergency_) { + RCLCPP_ERROR_THROTTLE( + get_logger(), *get_clock(), 5000, "Cannot reroute because of the emergency state"); + return; + } change_state(RouteState::Message::CHANGING); From 0669c0aff7c5704de42b025eda0ed981e6db1584 Mon Sep 17 00:00:00 2001 From: yutaka Date: Fri, 26 May 2023 01:54:01 +0900 Subject: [PATCH 3/3] feat(mission_planner): reroute for modified goal Signed-off-by: yutaka --- .../src/mission_planner/mission_planner.cpp | 108 ++++++++++++++++-- .../src/mission_planner/mission_planner.hpp | 3 + 2 files changed, 99 insertions(+), 12 deletions(-) diff --git a/planning/mission_planner/src/mission_planner/mission_planner.cpp b/planning/mission_planner/src/mission_planner/mission_planner.cpp index c2b010fb909f6..9d085e505cb25 100644 --- a/planning/mission_planner/src/mission_planner/mission_planner.cpp +++ b/planning/mission_planner/src/mission_planner/mission_planner.cpp @@ -221,8 +221,6 @@ LaneletRoute MissionPlanner::create_route( const std_msgs::msg::Header & header, const std::vector & waypoints, const geometry_msgs::msg::Pose & goal_pose, const bool allow_goal_modification) { - using ResponseCode = autoware_adapi_v1_msgs::srv::SetRoute::Response; - // Use temporary pose stamped for transform. PoseStamped pose; pose.header = header; @@ -239,10 +237,6 @@ LaneletRoute MissionPlanner::create_route( // Plan route. LaneletRoute route = planner_->plan(points); - if (route.segments.empty()) { - throw component_interface_utils::ServiceException( - ResponseCode::ERROR_PLANNER_FAILED, "The planned route is empty."); - } route.header.stamp = header.stamp; route.header.frame_id = map_frame_; route.uuid.uuid = generate_random_id(); @@ -251,6 +245,14 @@ LaneletRoute MissionPlanner::create_route( return route; } +LaneletRoute MissionPlanner::create_route( + const std_msgs::msg::Header & header, const geometry_msgs::msg::Pose & goal_pose, + const bool allow_goal_modification) +{ + const std::vector empty_waypoints; + return create_route(header, empty_waypoints, goal_pose, allow_goal_modification); +} + LaneletRoute MissionPlanner::create_route(const SetRoute::Service::Request::SharedPtr req) { const auto & header = req->header; @@ -342,9 +344,17 @@ void MissionPlanner::on_set_route_points( return; } + change_state(RouteState::Message::CHANGING); + // Plan route. const auto route = create_route(req); + if (route.segments.empty()) { + change_state(RouteState::Message::SET); + throw component_interface_utils::ServiceException( + ResponseCode::ERROR_PLANNER_FAILED, "The planned route is empty."); + } + // Update route. change_route(route); change_state(RouteState::Message::SET); @@ -379,6 +389,13 @@ void MissionPlanner::on_set_mrm_route( // Plan route. const auto new_route = create_route(req); + if (new_route.segments.empty()) { + change_route(*normal_route_); + change_state(RouteState::Message::SET); + throw component_interface_utils::ServiceException( + ResponseCode::ERROR_PLANNER_FAILED, "The planned route is empty."); + } + // check route safety if (checkRerouteSafety(*normal_route_, new_route)) { // sucess to reroute @@ -435,10 +452,15 @@ void MissionPlanner::on_clear_mrm_route( } // reroute with normal goal - const std::vector empty_waypoints; - const auto new_route = create_route( - odometry_->header, empty_waypoints, normal_route_->goal_pose, - normal_route_->allow_modification); + const auto new_route = + create_route(odometry_->header, normal_route_->goal_pose, normal_route_->allow_modification); + + if (new_route.segments.empty()) { + change_route(*normal_route_); + change_state(RouteState::Message::SET); + throw component_interface_utils::ServiceException( + ResponseCode::ERROR_PLANNER_FAILED, "The planned route is empty."); + } // check new route safety if (new_route.segments.empty() || !checkRerouteSafety(*mrm_route_, new_route)) { @@ -460,8 +482,56 @@ void MissionPlanner::on_clear_mrm_route( void MissionPlanner::on_modified_goal(const ModifiedGoal::Message::ConstSharedPtr msg) { - // TODO(Yutaka Shimizu): reroute if the goal is outside the lane. - arrival_checker_.modify_goal(*msg); + using ResponseCode = autoware_adapi_v1_msgs::srv::SetRoute::Response; + + if (state_.state != RouteState::Message::SET) { + throw component_interface_utils::ServiceException( + ResponseCode::ERROR_INVALID_STATE, "The route hasn't set yet. Cannot reroute."); + } + if (!odometry_) { + throw component_interface_utils::ServiceException( + ResponseCode::ERROR_PLANNER_UNREADY, "The vehicle pose is not received."); + } + if (!normal_route_) { + throw component_interface_utils::ServiceException( + ResponseCode::ERROR_PLANNER_UNREADY, "Normal route is not set."); + } + + // set to changing state + change_state(RouteState::Message::CHANGING); + + if (normal_route_->uuid == msg->uuid) { + const auto new_route = create_route(msg->header, msg->pose, normal_route_->allow_modification); + if (new_route.segments.empty()) { + change_route(*normal_route_); + change_state(RouteState::Message::SET); + throw component_interface_utils::ServiceException( + ResponseCode::ERROR_PLANNER_FAILED, "The planned route is empty."); + } + change_route(new_route); + change_state(RouteState::Message::SET); + arrival_checker_.modify_goal(*msg); + return; + } + + if (mrm_route_ && mrm_route_->uuid == msg->uuid) { + const auto new_route = create_route(msg->header, msg->pose, mrm_route_->allow_modification); + if (new_route.segments.empty()) { + change_mrm_route(*mrm_route_); + change_state(RouteState::Message::SET); + throw component_interface_utils::ServiceException( + ResponseCode::ERROR_PLANNER_FAILED, "The planned route is empty."); + } + change_mrm_route(new_route); + change_state(RouteState::Message::SET); + arrival_checker_.modify_goal(*msg); + return; + } + + change_state(RouteState::Message::SET); + throw component_interface_utils::ServiceException( + ResponseCode::ERROR_REROUTE_FAILED, "UUID is incorrect."); + return; } void MissionPlanner::on_change_route( @@ -489,6 +559,13 @@ void MissionPlanner::on_change_route( // Convert request to a new route. const auto new_route = create_route(req); + if (new_route.segments.empty()) { + change_route(*normal_route_); + change_state(RouteState::Message::SET); + throw component_interface_utils::ServiceException( + ResponseCode::ERROR_PLANNER_FAILED, "The planned route is empty."); + } + // check route safety if (checkRerouteSafety(*normal_route_, new_route)) { // sucess to reroute @@ -535,6 +612,13 @@ void MissionPlanner::on_change_route_points( // Plan route. const auto new_route = create_route(req); + if (new_route.segments.empty()) { + change_route(*normal_route_); + change_state(RouteState::Message::SET); + throw component_interface_utils::ServiceException( + ResponseCode::ERROR_PLANNER_FAILED, "The planned route is empty."); + } + // check route safety if (checkRerouteSafety(*normal_route_, new_route)) { // sucess to reroute diff --git a/planning/mission_planner/src/mission_planner/mission_planner.hpp b/planning/mission_planner/src/mission_planner/mission_planner.hpp index 9068d4bb10047..43f6243d09f1e 100644 --- a/planning/mission_planner/src/mission_planner/mission_planner.hpp +++ b/planning/mission_planner/src/mission_planner/mission_planner.hpp @@ -87,6 +87,9 @@ class MissionPlanner : public rclcpp::Node LaneletRoute create_route( const std_msgs::msg::Header & header, const std::vector & waypoints, const geometry_msgs::msg::Pose & goal_pose, const bool allow_goal_modification); + LaneletRoute create_route( + const std_msgs::msg::Header & header, const geometry_msgs::msg::Pose & goal_pose, + const bool allow_goal_modification); RouteState::Message state_; component_interface_utils::Publisher::SharedPtr pub_state_;