feat(default_adapi): use polling subscription

common/autoware_component_interface_utils/include/autoware/component_interface_utils/rclcpp/create_interface.hpp

@@ -65,11 +65,23 @@ template <class SpecT, class NodeT, class CallbackT>
 typename Subscription<SpecT>::SharedPtr create_subscription_impl(
   NodeT * node, CallbackT && callback)
 {
-  // This function is a wrapper for the following.
-  // https://github.com/ros2/rclcpp/blob/48068130edbb43cdd61076dc1851672ff1a80408/rclcpp/include/rclcpp/node.hpp#L207-L238
-  auto subscription = node->template create_subscription<typename SpecT::Message>(
-    SpecT::name, get_qos<SpecT>(), std::forward<CallbackT>(callback));
-  return Subscription<SpecT>::make_shared(subscription);
+  if constexpr (!std::is_null_pointer_v<CallbackT>) {
+    // This function is a wrapper for the following.
+    // https://github.com/ros2/rclcpp/blob/48068130edbb43cdd61076dc1851672ff1a80408/rclcpp/include/rclcpp/node.hpp#L207-L238
+    auto subscription = node->template create_subscription<typename SpecT::Message>(
+      SpecT::name, get_qos<SpecT>(), std::forward<CallbackT>(callback));
+    return Subscription<SpecT>::make_shared(subscription);
+  } else {
+    // If the callback is nullptr, create a subscription for polling.
+    // https://github.com/autowarefoundation/autoware.universe/tree/main/common/autoware_universe_utils/include/autoware/universe_utils/ros/polling_subscriber.hpp
+    auto group = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive, false);
+    auto options = rclcpp::SubscriptionOptions();
+    options.callback_group = group;
+
+    auto subscription = node->template create_subscription<typename SpecT::Message>(
+      SpecT::name, get_qos<SpecT>(), [](const typename SpecT::Message) {}, options);
+    return Subscription<SpecT>::make_shared(subscription);
+  }
 }
 
 }  // namespace autoware::component_interface_utils

common/autoware_component_interface_utils/include/autoware/component_interface_utils/rclcpp/topic_subscription.hpp

@@ -17,6 +17,8 @@
 
 #include <rclcpp/subscription.hpp>
 
+#include <memory>
+
 namespace autoware::component_interface_utils
 {
 
@@ -35,6 +37,40 @@
     subscription_ = subscription;  // to keep the reference count
   }
 
+  typename SpecType::Message::ConstSharedPtr takeLatestData()
+  {
+    rclcpp::MessageInfo info;
+    auto data = std::make_shared<typename SpecType::Message>();
+    bool flag = false;
+    for (size_t i = 0; i < subscription_->get_actual_qos().depth(); ++i) {
+      if (!subscription_->take(*data, info)) {
+        break;
+      }
+      flag = true;  // Whether there is at least one data.
+    }
+    return flag ? data : nullptr;
+  }
+
+  bool updateWithLatestData(typename SpecType::Message::ConstSharedPtr & ptr)
+  {
+    const auto msg = takeLatestData();
+    if (!msg) {
+      return false;
+    }
+    ptr = msg;
+    return true;
+  }
+
+  bool updateWithLatestData(typename SpecType::Message & ref)
+  {
+    const auto msg = takeLatestData();
+    if (!msg) {
+      return false;
+    }
+    ref = *msg;
+    return true;
+  }
+
 private:
   RCLCPP_DISABLE_COPY(Subscription)
   typename WrapType::SharedPtr subscription_;

system/autoware_default_adapi/src/compatibility/autoware_state.cpp

@@ -42,9 +42,9 @@ AutowareStateNode::AutowareStateNode(const rclcpp::NodeOptions & options)
     std::bind(&AutowareStateNode::on_shutdown, this, std::placeholders::_1, std::placeholders::_2));
 
   const auto adaptor = autoware::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);
+  adaptor.init_sub(sub_localization_, nullptr);
+  adaptor.init_sub(sub_routing_, nullptr);
+  adaptor.init_sub(sub_operation_mode_, nullptr);
 
   const auto rate = rclcpp::Rate(declare_parameter<double>("update_rate"));
   timer_ = rclcpp::create_timer(this, get_clock(), rate.period(), [this]() { on_timer(); });
@@ -56,19 +56,6 @@ AutowareStateNode::AutowareStateNode(const rclcpp::NodeOptions & options)
   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)
 {
@@ -115,6 +102,11 @@ void AutowareStateNode::on_timer()
     return AutowareState::PLANNING;
   };
 
+  // Update each state
+  sub_localization_->updateWithLatestData(localization_state_);
+  sub_routing_->updateWithLatestData(routing_state_);
+  sub_operation_mode_->updateWithLatestData(operation_mode_state_);
+
   // Update launch state.
   if (launch_state_ == LaunchState::Initializing) {
     bool is_initialized = true;

system/autoware_default_adapi/src/compatibility/autoware_state.hpp

@@ -61,9 +61,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/autoware_default_adapi/src/planning.cpp

@@ -115,17 +115,12 @@ PlanningNode::PlanningNode(const rclcpp::NodeOptions & options) : Node("planning
   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);
+  adaptor.init_sub(sub_trajectory_, nullptr);
 
   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;
@@ -142,6 +137,9 @@ void PlanningNode::on_timer()
   auto velocity = merge_factors<VelocityFactorArray>(now(), velocity_factors_);
   auto steering = merge_factors<SteeringFactorArray>(now(), steering_factors_);
 
+  // Take latest trajectory.
+  sub_trajectory_->updateWithLatestData(trajectory_);
+
   // Set the distance if it is nan.
   if (trajectory_ && kinematic_state_) {
     for (auto & factor : velocity.factors) {

system/autoware_default_adapi/src/planning.hpp

@@ -51,7 +51,6 @@ class PlanningNode : public rclcpp::Node
   using VehicleStopChecker = autoware::motion_utils::VehicleStopCheckerBase;
   using Trajectory = autoware::component_interface_specs::planning::Trajectory::Message;
   using KinematicState = autoware::component_interface_specs::localization::KinematicState::Message;
-  void on_trajectory(const Trajectory::ConstSharedPtr msg);
   void on_kinematic_state(const KinematicState::ConstSharedPtr msg);
   void on_timer();
 

system/autoware_default_adapi/src/vehicle.cpp

@@ -67,14 +67,14 @@ VehicleNode::VehicleNode(const rclcpp::NodeOptions & options) : Node("vehicle",
   group_cli_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
   adaptor.init_pub(pub_kinematics_);
   adaptor.init_pub(pub_status_);
-  adaptor.init_sub(sub_kinematic_state_, this, &VehicleNode::kinematic_state);
-  adaptor.init_sub(sub_acceleration_, this, &VehicleNode::acceleration_status);
-  adaptor.init_sub(sub_steering_, this, &VehicleNode::steering_status);
-  adaptor.init_sub(sub_gear_state_, this, &VehicleNode::gear_status);
-  adaptor.init_sub(sub_turn_indicator_, this, &VehicleNode::turn_indicator_status);
-  adaptor.init_sub(sub_map_projector_info_, this, &VehicleNode::map_projector_info);
-  adaptor.init_sub(sub_hazard_light_, this, &VehicleNode::hazard_light_status);
-  adaptor.init_sub(sub_energy_level_, this, &VehicleNode::energy_status);
+  adaptor.init_sub(sub_kinematic_state_, nullptr);
+  adaptor.init_sub(sub_acceleration_, nullptr);
+  adaptor.init_sub(sub_steering_, nullptr);
+  adaptor.init_sub(sub_gear_state_, nullptr);
+  adaptor.init_sub(sub_turn_indicator_, nullptr);
+  adaptor.init_sub(sub_map_projector_info_, nullptr);
+  adaptor.init_sub(sub_hazard_light_, nullptr);
+  adaptor.init_sub(sub_energy_level_, nullptr);
 
   const auto rate = rclcpp::Rate(10);
   timer_ = rclcpp::create_timer(this, get_clock(), rate.period(), [this]() { on_timer(); });
@@ -90,53 +90,6 @@ uint8_t VehicleNode::mapping(
   }
 }
 
-void VehicleNode::kinematic_state(
-  const autoware::component_interface_specs::localization::KinematicState::Message::ConstSharedPtr
-    msg_ptr)
-{
-  kinematic_state_msgs_ = msg_ptr;
-}
-
-void VehicleNode::acceleration_status(
-  const autoware::component_interface_specs::localization::Acceleration::Message::ConstSharedPtr
-    msg_ptr)
-{
-  acceleration_msgs_ = msg_ptr;
-}
-
-void VehicleNode::steering_status(
-  const autoware::component_interface_specs::vehicle::SteeringStatus::Message::ConstSharedPtr
-    msg_ptr)
-{
-  steering_status_msgs_ = msg_ptr;
-}
-
-void VehicleNode::gear_status(const GearReport::ConstSharedPtr msg_ptr)
-{
-  gear_status_msgs_ = msg_ptr;
-}
-
-void VehicleNode::turn_indicator_status(const TurnIndicatorsReport::ConstSharedPtr msg_ptr)
-{
-  turn_indicator_status_msgs_ = msg_ptr;
-}
-
-void VehicleNode::hazard_light_status(const HazardLightsReport::ConstSharedPtr msg_ptr)
-{
-  hazard_light_status_msgs_ = msg_ptr;
-}
-
-void VehicleNode::energy_status(
-  const autoware::component_interface_specs::vehicle::EnergyStatus::Message::ConstSharedPtr msg_ptr)
-{
-  energy_status_msgs_ = msg_ptr;
-}
-
-void VehicleNode::map_projector_info(const MapProjectorInfo::ConstSharedPtr msg_ptr)
-{
-  map_projector_info_ = msg_ptr;
-}
-
 void VehicleNode::publish_kinematics()
 {
   if (!kinematic_state_msgs_ || !acceleration_msgs_ || !map_projector_info_) return;
@@ -190,6 +143,15 @@ void VehicleNode::publish_status()
 
 void VehicleNode::on_timer()
 {
+  sub_kinematic_state_->updateWithLatestData(kinematic_state_msgs_);
+  sub_acceleration_->updateWithLatestData(acceleration_msgs_);
+  sub_steering_->updateWithLatestData(steering_status_msgs_);
+  sub_gear_state_->updateWithLatestData(gear_status_msgs_);
+  sub_turn_indicator_->updateWithLatestData(turn_indicator_status_msgs_);
+  sub_hazard_light_->updateWithLatestData(hazard_light_status_msgs_);
+  sub_energy_level_->updateWithLatestData(energy_status_msgs_);
+  sub_map_projector_info_->updateWithLatestData(map_projector_info_);
+
   publish_kinematics();
   publish_status();
 }

system/autoware_default_adapi/src/vehicle.hpp

@@ -69,30 +69,6 @@ class VehicleNode : public rclcpp::Node
   autoware::component_interface_specs::map::MapProjectorInfo::Message::ConstSharedPtr
     map_projector_info_;
 
-  void kinematic_state(
-    const autoware::component_interface_specs::localization::KinematicState::Message::ConstSharedPtr
-      msg_ptr);
-  void acceleration_status(
-    const autoware::component_interface_specs::localization::Acceleration::Message::ConstSharedPtr
-      msg_ptr);
-  void steering_status(
-    const autoware::component_interface_specs::vehicle::SteeringStatus::Message::ConstSharedPtr
-      msg_ptr);
-  void gear_status(
-    const autoware::component_interface_specs::vehicle::GearStatus::Message::ConstSharedPtr
-      msg_ptr);
-  void turn_indicator_status(
-    const autoware::component_interface_specs::vehicle::TurnIndicatorStatus::Message::ConstSharedPtr
-      msg_ptr);
-  void map_projector_info(
-    const autoware::component_interface_specs::map::MapProjectorInfo::Message::ConstSharedPtr
-      msg_ptr);
-  void hazard_light_status(
-    const autoware::component_interface_specs::vehicle::HazardLightStatus::Message::ConstSharedPtr
-      msg_ptr);
-  void energy_status(
-    const autoware::component_interface_specs::vehicle::EnergyStatus::Message::ConstSharedPtr
-      msg_ptr);
   uint8_t mapping(
     std::unordered_map<uint8_t, uint8_t> hash_map, uint8_t input, uint8_t default_value);
   void publish_kinematics();