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 52f6e4ca191f5..0500fa07ec31f 100644 --- a/planning/autoware_mission_planner/src/mission_planner/mission_planner.cpp +++ b/planning/autoware_mission_planner/src/mission_planner/mission_planner.cpp @@ -52,18 +52,11 @@ MissionPlanner::MissionPlanner(const rclcpp::NodeOptions & options) plugin_loader_.createSharedInstance("autoware::mission_planner::lanelet2::DefaultPlanner"); planner_->initialize(this); - const auto reroute_availability = std::make_shared(); - reroute_availability->availability = false; - reroute_availability_ = reroute_availability; - 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_vector_map_ = create_subscription( "~/input/vector_map", durable_qos, std::bind(&MissionPlanner::on_map, this, _1)); - sub_reroute_availability_ = create_subscription( - "~/input/reroute_availability", rclcpp::QoS(1), - std::bind(&MissionPlanner::on_reroute_availability, this, _1)); pub_marker_ = create_publisher("~/debug/route_marker", durable_qos); // NOTE: The route interface should be mutually exclusive by callback group. @@ -137,11 +130,6 @@ void MissionPlanner::on_odometry(const Odometry::ConstSharedPtr msg) } } -void MissionPlanner::on_reroute_availability(const RerouteAvailability::ConstSharedPtr msg) -{ - reroute_availability_ = msg; -} - void MissionPlanner::on_map(const LaneletMapBin::ConstSharedPtr msg) { map_ptr_ = msg; @@ -234,7 +222,8 @@ void MissionPlanner::on_set_lanelet_route( throw service_utils::ServiceException( ResponseCode::ERROR_PLANNER_UNREADY, "The vehicle pose is not received."); } - if (is_reroute && !reroute_availability_->availability) { + const auto reroute_availability = sub_reroute_availability_.takeData(); + if (is_reroute && (!reroute_availability || !reroute_availability->availability)) { throw service_utils::ServiceException( ResponseCode::ERROR_INVALID_STATE, "Cannot reroute as the planner is not in lane following."); } @@ -282,7 +271,8 @@ void MissionPlanner::on_set_waypoint_route( throw service_utils::ServiceException( ResponseCode::ERROR_PLANNER_UNREADY, "The vehicle pose is not received."); } - if (is_reroute && !reroute_availability_->availability) { + const auto reroute_availability = sub_reroute_availability_.takeData(); + if (is_reroute && (!reroute_availability || !reroute_availability->availability)) { throw service_utils::ServiceException( ResponseCode::ERROR_INVALID_STATE, "Cannot reroute as the planner is not in lane following."); } 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 9f3dc7d98c41c..6a694b5485431 100644 --- a/planning/autoware_mission_planner/src/mission_planner/mission_planner.hpp +++ b/planning/autoware_mission_planner/src/mission_planner/mission_planner.hpp @@ -16,6 +16,7 @@ #define MISSION_PLANNER__MISSION_PLANNER_HPP_ #include "arrival_checker.hpp" +#include "tier4_autoware_utils/ros/polling_subscriber.hpp" #include #include @@ -84,13 +85,13 @@ class MissionPlanner : public rclcpp::Node rclcpp::Subscription::SharedPtr sub_modified_goal_; rclcpp::Subscription::SharedPtr sub_odometry_; + tier4_autoware_utils::InterProcessPollingSubscriber + sub_reroute_availability_{this, "~/input/reroute_availability"}; + rclcpp::Subscription::SharedPtr sub_vector_map_; - rclcpp::Subscription::SharedPtr sub_reroute_availability_; rclcpp::Publisher::SharedPtr pub_marker_; - Odometry::ConstSharedPtr odometry_; LaneletMapBin::ConstSharedPtr map_ptr_; - RerouteAvailability::ConstSharedPtr reroute_availability_; RouteState state_; LaneletRoute::ConstSharedPtr current_route_; lanelet::LaneletMapPtr lanelet_map_ptr_{nullptr};