feat(default_ad_api): change to read topic by polling

system/default_ad_api/src/compatibility/autoware_state.cpp

@@ -14,6 +14,8 @@
 
 #include "autoware_state.hpp"
 
+#include "../utils/interface_subscriber.hpp"
+
 #include <string>
 #include <vector>
 
@@ -42,9 +44,12 @@
     std::bind(&AutowareStateNode::on_shutdown, this, std::placeholders::_1, std::placeholders::_2));
 
   const auto adaptor = component_interface_utils::NodeAdaptor(this);
-  adaptor.init_sub(sub_localization_, this, &AutowareStateNode::on_localization);
-  adaptor.init_sub(sub_routing_, this, &AutowareStateNode::on_routing);
-  adaptor.init_sub(sub_operation_mode_, this, &AutowareStateNode::on_operation_mode);
+
+  localization_sub_ =
+    create_polling_subscriber<autoware_ad_api::localization::InitializationState>(this);
+  routing_sub_ = create_polling_subscriber<autoware_ad_api::routing::RouteState>(this);
+  operation_mode_sub_ =
+    create_polling_subscriber<autoware_ad_api::operation_mode::OperationModeState>(this);
 
   const auto rate = rclcpp::Rate(declare_parameter<double>("update_rate"));
   timer_ = rclcpp::create_timer(this, get_clock(), rate.period(), [this]() { on_timer(); });
@@ -56,19 +61,6 @@
   operation_mode_state_.mode = OperationModeState::UNKNOWN;
 }
 
-void AutowareStateNode::on_localization(const LocalizationState::ConstSharedPtr msg)
-{
-  localization_state_ = *msg;
-}
-void AutowareStateNode::on_routing(const RoutingState::ConstSharedPtr msg)
-{
-  routing_state_ = *msg;
-}
-void AutowareStateNode::on_operation_mode(const OperationModeState::ConstSharedPtr msg)
-{
-  operation_mode_state_ = *msg;
-}
-
 void AutowareStateNode::on_shutdown(
   const Trigger::Request::SharedPtr, const Trigger::Response::SharedPtr res)
 {
@@ -79,6 +71,21 @@
 
 void AutowareStateNode::on_timer()
 {
+  auto localization_msg = localization_sub_->takeData();
+  if (localization_msg) {
+    localization_state_ = *localization_msg;
+  }
+
+  auto routing_msg = routing_sub_->takeData();
+  if (routing_msg) {
+    routing_state_ = *routing_msg;
+  }
+
+  auto operation_mode_msg = operation_mode_sub_->takeData();
+  if (operation_mode_msg) {
+    operation_mode_state_ = *operation_mode_msg;
+  }
+
   const auto convert_state = [this]() {
     if (launch_state_ == LaunchState::Finalizing) {
       return AutowareState::FINALIZING;

system/default_ad_api/src/compatibility/autoware_state.hpp

@@ -23,9 +23,11 @@
 #include <std_srvs/srv/trigger.hpp>
 #include <tier4_system_msgs/msg/mode_change_available.hpp>
 
+#include <memory>
 #include <vector>
 
 // This file should be included after messages.
+#include "../utils/interface_subscriber.hpp"
 #include "../utils/types.hpp"
 
 namespace default_ad_api
@@ -40,9 +42,15 @@ class AutowareStateNode : public rclcpp::Node
   using ModeChangeAvailable = tier4_system_msgs::msg::ModeChangeAvailable;
   rclcpp::TimerBase::SharedPtr timer_;
   // emergency
-  Sub<autoware_ad_api::localization::InitializationState> sub_localization_;
-  Sub<autoware_ad_api::routing::RouteState> sub_routing_;
-  Sub<autoware_ad_api::operation_mode::OperationModeState> sub_operation_mode_;
+  std::shared_ptr<tier4_autoware_utils::InterProcessPollingSubscriber<
+    autoware_ad_api::localization::InitializationState::Message>>
+    localization_sub_;
+  std::shared_ptr<tier4_autoware_utils::InterProcessPollingSubscriber<
+    autoware_ad_api::routing::RouteState::Message>>
+    routing_sub_;
+  std::shared_ptr<tier4_autoware_utils::InterProcessPollingSubscriber<
+    autoware_ad_api::operation_mode::OperationModeState::Message>>
+    operation_mode_sub_;
 
   using AutowareState = autoware_system_msgs::msg::AutowareState;
   using LocalizationState = autoware_ad_api::localization::InitializationState::Message;
@@ -61,9 +69,6 @@ class AutowareStateNode : public rclcpp::Node
   OperationModeState operation_mode_state_;
 
   void on_timer();
-  void on_localization(const LocalizationState::ConstSharedPtr msg);
-  void on_routing(const RoutingState::ConstSharedPtr msg);
-  void on_operation_mode(const OperationModeState::ConstSharedPtr msg);
   void on_shutdown(const Trigger::Request::SharedPtr, const Trigger::Response::SharedPtr);
 };
 

system/default_ad_api/src/motion.cpp

@@ -14,6 +14,8 @@
 
 #include "motion.hpp"
 
+#include "utils/interface_subscriber.hpp"
+
 #include <memory>
 #include <unordered_map>
 
@@ -35,6 +37,9 @@ MotionNode::MotionNode(const rclcpp::NodeOptions & options)
   adaptor.init_sub(sub_is_paused_, this, &MotionNode::on_is_paused);
   adaptor.init_sub(sub_is_start_requested_, this, &MotionNode::on_is_start_requested);
 
+  is_paused_sub_ = create_polling_subscriber<control_interface::IsPaused>(this);
+  is_start_requested_sub_ = create_polling_subscriber<control_interface::IsStartRequested>(this);
+
   rclcpp::Rate rate(10);
   timer_ = rclcpp::create_timer(this, get_clock(), rate.period(), [this]() { on_timer(); });
   state_ = State::Unknown;

system/default_ad_api/src/motion.hpp

@@ -22,7 +22,10 @@
 #include <motion_utils/vehicle/vehicle_state_checker.hpp>
 #include <rclcpp/rclcpp.hpp>
 
+#include <memory>
+
 // This file should be included after messages.
+#include "utils/interface_subscriber.hpp"
 #include "utils/types.hpp"
 
 namespace default_ad_api
@@ -42,6 +45,12 @@ class MotionNode : public rclcpp::Node
   Cli<control_interface::SetPause> cli_set_pause_;
   Sub<control_interface::IsPaused> sub_is_paused_;
   Sub<control_interface::IsStartRequested> sub_is_start_requested_;
+  std::shared_ptr<
+    tier4_autoware_utils::InterProcessPollingSubscriber<control_interface::IsPaused::Message>>
+    is_paused_sub_;
+  std::shared_ptr<tier4_autoware_utils::InterProcessPollingSubscriber<
+    control_interface::IsStartRequested::Message>>
+    is_start_requested_sub_;
 
   enum class State { Unknown, Pausing, Paused, Starting, Resuming, Resumed, Moving };
   State state_;

system/default_ad_api/src/planning.cpp

@@ -11,9 +11,10 @@
 // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 // See the License for the specific language governing permissions and
 // limitations under the License.
-
 #include "planning.hpp"
 
+#include "utils/interface_subscriber.hpp"
+
 #include <motion_utils/trajectory/trajectory.hpp>
 
 #include <autoware_adapi_v1_msgs/msg/planning_behavior.hpp>
@@ -101,57 +102,58 @@
   const auto adaptor = component_interface_utils::NodeAdaptor(this);
   adaptor.init_pub(pub_velocity_factors_);
   adaptor.init_pub(pub_steering_factors_);
-  adaptor.init_sub(sub_kinematic_state_, this, &PlanningNode::on_kinematic_state);
-  adaptor.init_sub(sub_trajectory_, this, &PlanningNode::on_trajectory);
+
+  trajectory_sub_ = create_polling_subscriber<planning_interface::Trajectory>(this);
+  kinematic_state_sub_ = create_polling_subscriber<localization_interface::KinematicState>(this);
 
   const auto rate = rclcpp::Rate(5);
   timer_ = rclcpp::create_timer(this, get_clock(), rate.period(), [this]() { on_timer(); });
 }
 
-void PlanningNode::on_trajectory(const Trajectory::ConstSharedPtr msg)
-{
-  trajectory_ = msg;
-}
-
-void PlanningNode::on_kinematic_state(const KinematicState::ConstSharedPtr msg)
-{
-  kinematic_state_ = msg;
-
-  geometry_msgs::msg::TwistStamped twist;
-  twist.header = msg->header;
-  twist.twist = msg->twist.twist;
-  stop_checker_->addTwist(twist);
-}
-
 void PlanningNode::on_timer()
 {
   using autoware_adapi_v1_msgs::msg::VelocityFactor;
   auto velocity = merge_factors<VelocityFactorArray>(now(), velocity_factors_);
   auto steering = merge_factors<SteeringFactorArray>(now(), steering_factors_);
 
+  auto trajectory_msg = trajectory_sub_->takeData();
+  if (trajectory_msg) {
+    trajectory_ = trajectory_msg;
+  }
+
+  auto kinematic_state_msg = kinematic_state_sub_->takeData();
+  if (kinematic_state_msg) {
+    kinematic_state_ = kinematic_state_msg;
+
+    geometry_msgs::msg::TwistStamped twist;
+    twist.header = kinematic_state_msg->header;
+    twist.twist = kinematic_state_msg->twist.twist;
+    stop_checker_->addTwist(twist);
+  }
+
   // Set the distance if it is nan.
   if (trajectory_ && kinematic_state_) {
     for (auto & factor : velocity.factors) {
       if (std::isnan(factor.distance)) {
         const auto & curr_point = kinematic_state_->pose.pose.position;
         const auto & stop_point = factor.pose.position;
         const auto & points = trajectory_->points;
         factor.distance = motion_utils::calcSignedArcLength(points, curr_point, stop_point);
       }
     }
-  }
 
-  // Set the status if it is unknown.
-  const auto is_vehicle_stopped = stop_checker_->isVehicleStopped(stop_duration_);
-  for (auto & factor : velocity.factors) {
-    if ((factor.status == VelocityFactor::UNKNOWN) && (!std::isnan(factor.distance))) {
-      const auto is_stopped = is_vehicle_stopped && (factor.distance < stop_distance_);
-      factor.status = is_stopped ? VelocityFactor::STOPPED : VelocityFactor::APPROACHING;
+    // Set the status if it is unknown.
+    const auto is_vehicle_stopped = stop_checker_->isVehicleStopped(stop_duration_);
+    for (auto & factor : velocity.factors) {
+      if ((factor.status == VelocityFactor::UNKNOWN) && (!std::isnan(factor.distance))) {
+        const auto is_stopped = is_vehicle_stopped && (factor.distance < stop_distance_);
+        factor.status = is_stopped ? VelocityFactor::STOPPED : VelocityFactor::APPROACHING;
+      }
     }
-  }
 
-  pub_velocity_factors_->publish(velocity);
-  pub_steering_factors_->publish(steering);
+    pub_velocity_factors_->publish(velocity);
+    pub_steering_factors_->publish(steering);
+  }
 }
 
 }  // namespace default_ad_api

system/default_ad_api/src/planning.hpp

@@ -25,6 +25,7 @@
 #include <vector>
 
 // This file should be included after messages.
+#include "utils/interface_subscriber.hpp"
 #include "utils/types.hpp"
 
 namespace default_ad_api
@@ -40,8 +41,14 @@ class PlanningNode : public rclcpp::Node
   using SteeringFactorArray = autoware_adapi_v1_msgs::msg::SteeringFactorArray;
   Pub<autoware_ad_api::planning::VelocityFactors> pub_velocity_factors_;
   Pub<autoware_ad_api::planning::SteeringFactors> pub_steering_factors_;
-  Sub<planning_interface::Trajectory> sub_trajectory_;
-  Sub<localization_interface::KinematicState> sub_kinematic_state_;
+
+  std::shared_ptr<
+    tier4_autoware_utils::InterProcessPollingSubscriber<planning_interface::Trajectory::Message>>
+    trajectory_sub_;
+  std::shared_ptr<tier4_autoware_utils::InterProcessPollingSubscriber<
+    localization_interface::KinematicState::Message>>
+    kinematic_state_sub_;
+
   std::vector<rclcpp::Subscription<VelocityFactorArray>::SharedPtr> sub_velocity_factors_;
   std::vector<rclcpp::Subscription<SteeringFactorArray>::SharedPtr> sub_steering_factors_;
   std::vector<VelocityFactorArray::ConstSharedPtr> velocity_factors_;
@@ -51,8 +58,6 @@ class PlanningNode : public rclcpp::Node
   using VehicleStopChecker = motion_utils::VehicleStopCheckerBase;
   using Trajectory = planning_interface::Trajectory::Message;
   using KinematicState = localization_interface::KinematicState::Message;
-  void on_trajectory(const Trajectory::ConstSharedPtr msg);
-  void on_kinematic_state(const KinematicState::ConstSharedPtr msg);
   void on_timer();
 
   double stop_distance_;

system/default_ad_api/src/routing.cpp

@@ -14,6 +14,7 @@
 
 #include "routing.hpp"
 
+#include "utils/interface_subscriber.hpp"
 #include "utils/route_conversion.hpp"
 
 #include <memory>
@@ -55,7 +56,6 @@ RoutingNode::RoutingNode(const rclcpp::NodeOptions & options) : Node("routing",
   adaptor.init_pub(pub_state_);
   adaptor.init_pub(pub_route_);
   adaptor.init_sub(sub_state_, this, &RoutingNode::on_state);
-  adaptor.init_sub(sub_route_, this, &RoutingNode::on_route);
   adaptor.init_cli(cli_clear_route_, group_cli_);
   adaptor.init_cli(cli_set_waypoint_route_, group_cli_);
   adaptor.init_cli(cli_set_lanelet_route_, group_cli_);
@@ -65,8 +65,10 @@ RoutingNode::RoutingNode(const rclcpp::NodeOptions & options) : Node("routing",
   adaptor.init_srv(srv_change_route_, this, &RoutingNode::on_change_route);
   adaptor.init_srv(srv_change_route_points_, this, &RoutingNode::on_change_route_points);
 
+  route_sub_ = create_polling_subscriber<planning_interface::LaneletRoute>(this);
+  operation_mode_sub_ = create_polling_subscriber<system_interface::OperationModeState>(this);
+
   adaptor.init_cli(cli_operation_mode_, group_cli_);
-  adaptor.init_sub(sub_operation_mode_, this, &RoutingNode::on_operation_mode);
 
   is_auto_mode_ = false;
   state_.state = State::Message::UNKNOWN;
@@ -82,13 +84,18 @@ void RoutingNode::change_stop_mode()
   }
 }
 
-void RoutingNode::on_operation_mode(const OperationModeState::Message::ConstSharedPtr msg)
-{
-  is_auto_mode_ = msg->mode == OperationModeState::Message::AUTONOMOUS;
-}
-
 void RoutingNode::on_state(const State::Message::ConstSharedPtr msg)
 {
+  auto operation_mode_msg = operation_mode_sub_->takeData();
+  if (operation_mode_msg) {
+    is_auto_mode_ = operation_mode_msg->mode == OperationModeState::Message::AUTONOMOUS;
+  }
+
+  auto route_msg = route_sub_->takeData();
+  if (route_msg) {
+    pub_route_->publish(conversion::convert_route(*route_msg));
+  }
+
   // TODO(Takagi, Isamu): Add adapi initializing state.
   // Represent initializing state by not publishing the topic for now.
   if (msg->state == State::Message::INITIALIZING) {
@@ -109,11 +116,6 @@ void RoutingNode::on_state(const State::Message::ConstSharedPtr msg)
   }
 }
 
-void RoutingNode::on_route(const Route::Message::ConstSharedPtr msg)
-{
-  pub_route_->publish(conversion::convert_route(*msg));
-}
-
 void RoutingNode::on_clear_route(
   const autoware_ad_api::routing::ClearRoute::Service::Request::SharedPtr req,
   const autoware_ad_api::routing::ClearRoute::Service::Response::SharedPtr res)

system/default_ad_api/src/routing.hpp

@@ -21,7 +21,10 @@
 #include <component_interface_utils/status.hpp>
 #include <rclcpp/rclcpp.hpp>
 
+#include <memory>
+
 // This file should be included after messages.
+#include "utils/interface_subscriber.hpp"
 #include "utils/types.hpp"
 
 namespace default_ad_api
@@ -46,19 +49,27 @@ class RoutingNode : public rclcpp::Node
   Srv<autoware_ad_api::routing::ChangeRoute> srv_change_route_;
   Srv<autoware_ad_api::routing::ClearRoute> srv_clear_route_;
   Sub<planning_interface::RouteState> sub_state_;
-  Sub<planning_interface::LaneletRoute> sub_route_;
+  // Sub<planning_interface::LaneletRoute> sub_route_;
   Cli<planning_interface::SetWaypointRoute> cli_set_waypoint_route_;
   Cli<planning_interface::SetLaneletRoute> cli_set_lanelet_route_;
   Cli<planning_interface::ClearRoute> cli_clear_route_;
   Cli<system_interface::ChangeOperationMode> cli_operation_mode_;
-  Sub<system_interface::OperationModeState> sub_operation_mode_;
+  // Sub<system_interface::OperationModeState> sub_operation_mode_;
+
+  // std::shared_ptr<tier4_autoware_utils::InterProcessPollingSubscriber<planning_interface::RouteState::Message>>
+  //   state_sub_;
+  std::shared_ptr<
+    tier4_autoware_utils::InterProcessPollingSubscriber<planning_interface::LaneletRoute::Message>>
+    route_sub_;
+  std::shared_ptr<tier4_autoware_utils::InterProcessPollingSubscriber<
+    system_interface::OperationModeState::Message>>
+    operation_mode_sub_;
+
   bool is_auto_mode_;
   State::Message state_;
 
   void change_stop_mode();
-  void on_operation_mode(const OperationModeState::Message::ConstSharedPtr msg);
   void on_state(const State::Message::ConstSharedPtr msg);
-  void on_route(const Route::Message::ConstSharedPtr msg);
   void on_clear_route(
     const autoware_ad_api::routing::ClearRoute::Service::Request::SharedPtr req,
     const autoware_ad_api::routing::ClearRoute::Service::Response::SharedPtr res);