replace rclcpp::Subscription to tier4_autoware_utils::InterProcessPollingSubscriber

Signed-off-by: Autumn60 <akiro.harada@tier4.jp>

Author: Autumn60

system/emergency_handler/include/emergency_handler/emergency_handler_core.hpp

@@ -26,6 +26,7 @@
 #include <autoware_auto_vehicle_msgs/msg/control_mode_report.hpp>
 #include <autoware_auto_vehicle_msgs/msg/gear_command.hpp>
 #include <autoware_auto_vehicle_msgs/msg/hazard_lights_command.hpp>
+#include <tier4_autoware_utils/ros/polling_subscriber.hpp>
 #include <tier4_system_msgs/msg/mrm_behavior_status.hpp>
 #include <tier4_system_msgs/srv/operate_mrm.hpp>
 
@@ -56,18 +57,20 @@ class EmergencyHandler : public rclcpp::Node
   EmergencyHandler();
 
 private:
-  // Subscribers
+  // Subscribers with Callback
   rclcpp::Subscription<autoware_auto_system_msgs::msg::HazardStatusStamped>::SharedPtr
     sub_hazard_status_stamped_;
   rclcpp::Subscription<autoware_auto_control_msgs::msg::AckermannControlCommand>::SharedPtr
     sub_prev_control_command_;
-  rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr sub_odom_;
-  rclcpp::Subscription<autoware_auto_vehicle_msgs::msg::ControlModeReport>::SharedPtr
-    sub_control_mode_;
-  rclcpp::Subscription<tier4_system_msgs::msg::MrmBehaviorStatus>::SharedPtr
-    sub_mrm_comfortable_stop_status_;
-  rclcpp::Subscription<tier4_system_msgs::msg::MrmBehaviorStatus>::SharedPtr
-    sub_mrm_emergency_stop_status_;
+  // Subscribers without Callback
+  tier4_autoware_utils::InterProcessPollingSubscriber<nav_msgs::msg::Odometry> sub_odom_{
+    this, "~/input/odometry"};
+  tier4_autoware_utils::InterProcessPollingSubscriber<autoware_auto_vehicle_msgs::msg::ControlModeReport>
+    sub_control_mode_{this, "~/input/control_mode"};
+  tier4_autoware_utils::InterProcessPollingSubscriber<tier4_system_msgs::msg::MrmBehaviorStatus>
+    sub_mrm_comfortable_stop_status_{this, "~/input/mrm/comfortable_stop/status"};
+  tier4_autoware_utils::InterProcessPollingSubscriber<tier4_system_msgs::msg::MrmBehaviorStatus>
+    sub_mrm_emergency_stop_status_{this, "~/input/mrm/emergency_stop/status"};
 
   autoware_auto_system_msgs::msg::HazardStatusStamped::ConstSharedPtr hazard_status_stamped_;
   autoware_auto_control_msgs::msg::AckermannControlCommand::ConstSharedPtr prev_control_command_;

system/emergency_handler/src/emergency_handler/emergency_handler_core.cpp

@@ -38,41 +38,6 @@ EmergencyHandler::EmergencyHandler() : Node("emergency_handler")
       "~/input/prev_control_command", rclcpp::QoS{1},
       std::bind(&EmergencyHandler::onPrevControlCommand, this, _1));
 
-  // Subscriber without callback
-  rclcpp::CallbackGroup::SharedPtr cb_group_noexec = this->create_callback_group(
-        rclcpp::CallbackGroupType::MutuallyExclusive, false);
-  auto subscription_options = rclcpp::SubscriptionOptions();
-  subscription_options.callback_group = cb_group_noexec;
-
-  sub_odom_ = create_subscription<nav_msgs::msg::Odometry>(
-    "~/input/odometry", rclcpp::QoS{1},
-    [this]([[maybe_unused]] nav_msgs::msg::Odometry::SharedPtr) -> void
-    {
-      assert(false);
-    }, subscription_options);
-
-  sub_control_mode_ = create_subscription<autoware_auto_vehicle_msgs::msg::ControlModeReport>(
-    "~/input/control_mode", rclcpp::QoS{1},
-    [this]([[maybe_unused]] autoware_auto_vehicle_msgs::msg::ControlModeReport::SharedPtr) -> void
-    {
-      assert(false);
-    }, subscription_options);
-
-  // subscribe control mode
-  sub_mrm_comfortable_stop_status_ = create_subscription<tier4_system_msgs::msg::MrmBehaviorStatus>(
-    "~/input/mrm/comfortable_stop/status", rclcpp::QoS{1},
-    [this]([[maybe_unused]] tier4_system_msgs::msg::MrmBehaviorStatus::SharedPtr) -> void
-    {
-      assert(false);
-    }, subscription_options);
-
-  sub_mrm_emergency_stop_status_ = create_subscription<tier4_system_msgs::msg::MrmBehaviorStatus>(
-    "~/input/mrm/emergency_stop/status", rclcpp::QoS{1},
-    [this]([[maybe_unused]] tier4_system_msgs::msg::MrmBehaviorStatus::SharedPtr) -> void
-    {
-      assert(false);
-    }, subscription_options);
-
   // Publisher
   pub_control_command_ = create_publisher<autoware_auto_control_msgs::msg::AckermannControlCommand>(
     "~/output/control_command", rclcpp::QoS{1});
@@ -442,36 +407,31 @@ bool EmergencyHandler::isEmergency()
 
 bool EmergencyHandler::isStopped()
 {
-  auto odom = std::make_shared<nav_msgs::msg::Odometry>();
-  rclcpp::MessageInfo msg_info;
-  if(!sub_odom_->take(*odom, msg_info)) return false;
-
+  if(!sub_odom_.updateLatestData()) return false;
+  auto odom = sub_odom_.getData();
   constexpr auto th_stopped_velocity = 0.001;
-  return (odom->twist.twist.linear.x < th_stopped_velocity);
+  return (odom.twist.twist.linear.x < th_stopped_velocity);
 }
 
 bool EmergencyHandler::isAutonomous()
 {
   using autoware_auto_vehicle_msgs::msg::ControlModeReport;
 
-  auto control_mode = std::make_shared<autoware_auto_vehicle_msgs::msg::ControlModeReport>();
-  rclcpp::MessageInfo msg_info;
-  if(!sub_control_mode_->take(*control_mode, msg_info)) return false;
-  return control_mode->mode == ControlModeReport::AUTONOMOUS;
+  if(!sub_control_mode_.updateLatestData()) return false;
+  auto mode = sub_control_mode_.getData();
+  return mode.mode == ControlModeReport::AUTONOMOUS;
 }
 
 bool EmergencyHandler::isComfortableStopStatusAvailable()
 {
-  auto status = std::make_shared<tier4_system_msgs::msg::MrmBehaviorStatus>();
-  rclcpp::MessageInfo msg_info;
-  if(!sub_mrm_comfortable_stop_status_->take(*status, msg_info)) return false;
-  return status->state != tier4_system_msgs::msg::MrmBehaviorStatus::NOT_AVAILABLE;
+  if(!sub_mrm_comfortable_stop_status_.updateLatestData()) return false;
+  auto status = sub_mrm_comfortable_stop_status_.getData();
+  return status.state != tier4_system_msgs::msg::MrmBehaviorStatus::NOT_AVAILABLE;
 }
 
 bool EmergencyHandler::isEmergencyStopStatusAvailable()
 {
-  auto status = std::make_shared<tier4_system_msgs::msg::MrmBehaviorStatus>();
-  rclcpp::MessageInfo msg_info;
-  if(!sub_mrm_emergency_stop_status_->take(*status, msg_info)) return false;
-  return status->state != tier4_system_msgs::msg::MrmBehaviorStatus::NOT_AVAILABLE;
+  if(!sub_mrm_emergency_stop_status_.updateLatestData()) return false;
+  auto status = sub_mrm_emergency_stop_status_.getData();
+  return status.state != tier4_system_msgs::msg::MrmBehaviorStatus::NOT_AVAILABLE;
 }