From af0c545791e12f4fd9779fab9aa973346cfa3f47 Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" <43976882+isamu-takagi@users.noreply.github.com> Date: Thu, 28 Nov 2024 14:12:45 +0900 Subject: [PATCH] fix(mission_planner): fix initialization after route set (#9457) Signed-off-by: Takagi, Isamu --- .../src/mission_planner/mission_planner.cpp | 32 ++++++++----------- .../src/mission_planner/mission_planner.hpp | 1 + 2 files changed, 15 insertions(+), 18 deletions(-) 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 2b54160a81038..95695498e4450 100644 --- a/planning/autoware_mission_planner/src/mission_planner/mission_planner.cpp +++ b/planning/autoware_mission_planner/src/mission_planner/mission_planner.cpp @@ -81,6 +81,7 @@ MissionPlanner::MissionPlanner(const rclcpp::NodeOptions & options) // otherwise the mission planner rejects the request for the API. const auto period = rclcpp::Rate(10).period(); data_check_timer_ = create_wall_timer(period, [this] { check_initialization(); }); + is_mission_planner_ready_ = false; logger_configure_ = std::make_unique(this); } @@ -111,6 +112,7 @@ void MissionPlanner::check_initialization() } // All data is ready. Now API is available. + is_mission_planner_ready_ = true; RCLCPP_DEBUG(logger, "Route API is ready."); change_state(RouteState::UNSET); @@ -174,12 +176,8 @@ void MissionPlanner::on_modified_goal(const PoseWithUuidStamped::ConstSharedPtr RCLCPP_ERROR(get_logger(), "The route hasn't set yet. Cannot reroute."); return; } - if (!planner_->ready()) { - RCLCPP_ERROR(get_logger(), "The planner is not ready."); - return; - } - if (!odometry_) { - RCLCPP_ERROR(get_logger(), "The vehicle pose is not received."); + if (!is_mission_planner_ready_) { + RCLCPP_ERROR(get_logger(), "The mission planner is not ready."); return; } if (!current_route_) { @@ -209,6 +207,12 @@ void MissionPlanner::on_modified_goal(const PoseWithUuidStamped::ConstSharedPtr void MissionPlanner::on_clear_route( const ClearRoute::Request::SharedPtr, const ClearRoute::Response::SharedPtr res) { + if (!is_mission_planner_ready_) { + using ResponseCode = autoware_adapi_v1_msgs::msg::ResponseStatus; + throw service_utils::ServiceException( + ResponseCode::NO_EFFECT, "The mission planner is not ready.", true); + } + change_route(); change_state(RouteState::UNSET); res->status.success = true; @@ -224,13 +228,9 @@ void MissionPlanner::on_set_lanelet_route( throw service_utils::ServiceException( ResponseCode::ERROR_INVALID_STATE, "The route cannot be set in the current state."); } - if (!planner_->ready()) { + if (!is_mission_planner_ready_) { throw service_utils::ServiceException( - ResponseCode::ERROR_PLANNER_UNREADY, "The planner is not ready."); - } - if (!odometry_) { - throw service_utils::ServiceException( - ResponseCode::ERROR_PLANNER_UNREADY, "The vehicle pose is not received."); + ResponseCode::ERROR_PLANNER_UNREADY, "The mission planner is not ready."); } if (is_reroute && !operation_mode_state_) { throw service_utils::ServiceException( @@ -291,13 +291,9 @@ void MissionPlanner::on_set_waypoint_route( throw service_utils::ServiceException( ResponseCode::ERROR_INVALID_STATE, "The route cannot be set in the current state."); } - if (!planner_->ready()) { - throw service_utils::ServiceException( - ResponseCode::ERROR_PLANNER_UNREADY, "The planner is not ready."); - } - if (!odometry_) { + if (!is_mission_planner_ready_) { throw service_utils::ServiceException( - ResponseCode::ERROR_PLANNER_UNREADY, "The vehicle pose is not received."); + ResponseCode::ERROR_PLANNER_UNREADY, "The mission planner is not ready."); } if (is_reroute && !operation_mode_state_) { 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 9342bc7840641..3a838888d6d71 100644 --- a/planning/autoware_mission_planner/src/mission_planner/mission_planner.hpp +++ b/planning/autoware_mission_planner/src/mission_planner/mission_planner.hpp @@ -132,6 +132,7 @@ class MissionPlanner : public rclcpp::Node rclcpp::TimerBase::SharedPtr data_check_timer_; void check_initialization(); + bool is_mission_planner_ready_; double reroute_time_threshold_; double minimum_reroute_length_;