feat(mission_planner): use polling subscriber (#7447)

Signed-off-by: kosuke55 <[email protected]>

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<RerouteAvailability>();
-  reroute_availability->availability = false;
-  reroute_availability_ = reroute_availability;
-
   const auto durable_qos = rclcpp::QoS(1).transient_local();
   sub_odometry_ = create_subscription<Odometry>(
     "~/input/odometry", rclcpp::QoS(1), std::bind(&MissionPlanner::on_odometry, this, _1));
   sub_vector_map_ = create_subscription<LaneletMapBin>(
     "~/input/vector_map", durable_qos, std::bind(&MissionPlanner::on_map, this, _1));
-  sub_reroute_availability_ = create_subscription<RerouteAvailability>(
-    "~/input/reroute_availability", rclcpp::QoS(1),
-    std::bind(&MissionPlanner::on_reroute_availability, this, _1));
   pub_marker_ = create_publisher<MarkerArray>("~/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.");
   }

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 <autoware_route_handler/route_handler.hpp>
 #include <mission_planner/mission_planner_plugin.hpp>
@@ -84,13 +85,13 @@ class MissionPlanner : public rclcpp::Node
 
   rclcpp::Subscription<PoseWithUuidStamped>::SharedPtr sub_modified_goal_;
   rclcpp::Subscription<Odometry>::SharedPtr sub_odometry_;
+  tier4_autoware_utils::InterProcessPollingSubscriber<RerouteAvailability>
+    sub_reroute_availability_{this, "~/input/reroute_availability"};
+
   rclcpp::Subscription<LaneletMapBin>::SharedPtr sub_vector_map_;
-  rclcpp::Subscription<RerouteAvailability>::SharedPtr sub_reroute_availability_;
   rclcpp::Publisher<MarkerArray>::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};