RT1-6697 use polling subscriber

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

Author: zulfaqar-azmi-t4

control/shift_decider/include/shift_decider/shift_decider.hpp

@@ -15,15 +15,15 @@
 #ifndef SHIFT_DECIDER__SHIFT_DECIDER_HPP_
 #define SHIFT_DECIDER__SHIFT_DECIDER_HPP_
 
+#include "tier4_autoware_utils/ros/polling_subscriber.hpp"
+
 #include <rclcpp/rclcpp.hpp>
 
 #include <autoware_control_msgs/msg/control.hpp>
 #include <autoware_system_msgs/msg/autoware_state.hpp>
 #include <autoware_vehicle_msgs/msg/gear_command.hpp>
 #include <autoware_vehicle_msgs/msg/gear_report.hpp>
 
-#include <memory>
-
 class ShiftDecider : public rclcpp::Node
 {
 public:
@@ -38,16 +38,19 @@ class ShiftDecider : public rclcpp::Node
   void initTimer(double period_s);
 
   rclcpp::Publisher<autoware_vehicle_msgs::msg::GearCommand>::SharedPtr pub_shift_cmd_;
-  rclcpp::Subscription<autoware_control_msgs::msg::Control>::SharedPtr sub_control_cmd_;
-  rclcpp::Subscription<autoware_system_msgs::msg::AutowareState>::SharedPtr sub_autoware_state_;
-  rclcpp::Subscription<autoware_vehicle_msgs::msg::GearReport>::SharedPtr sub_current_gear_;
+  tier4_autoware_utils::InterProcessPollingSubscriber<autoware_control_msgs::msg::Control>
+    sub_control_cmd_{this, "input/control_cmd"};
+  tier4_autoware_utils::InterProcessPollingSubscriber<autoware_system_msgs::msg::AutowareState>
+    sub_autoware_state_{this, "input/state"};
+  tier4_autoware_utils::InterProcessPollingSubscriber<autoware_vehicle_msgs::msg::GearReport>
+    sub_current_gear_{this, "input/current_gear"};
 
   rclcpp::TimerBase::SharedPtr timer_;
 
-  autoware_control_msgs::msg::Control::SharedPtr control_cmd_;
-  autoware_system_msgs::msg::AutowareState::SharedPtr autoware_state_;
+  autoware_control_msgs::msg::Control::ConstSharedPtr control_cmd_;
+  autoware_system_msgs::msg::AutowareState::ConstSharedPtr autoware_state_;
   autoware_vehicle_msgs::msg::GearCommand shift_cmd_;
-  autoware_vehicle_msgs::msg::GearReport::SharedPtr current_gear_ptr_;
+  autoware_vehicle_msgs::msg::GearReport::ConstSharedPtr current_gear_ptr_;
   uint8_t prev_shift_command = autoware_vehicle_msgs::msg::GearCommand::PARK;
 
   bool park_on_goal_;

control/shift_decider/package.xml

@@ -17,6 +17,7 @@
   <depend>autoware_vehicle_msgs</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_components</depend>
+  <depend>tier4_autoware_utils</depend>
 
   <test_depend>ament_cmake_cppcheck</test_depend>
   <test_depend>ament_cmake_cpplint</test_depend>

control/shift_decider/src/shift_decider.cpp

@@ -18,8 +18,6 @@
 
 #include <cstddef>
 #include <functional>
-#include <memory>
-#include <utility>
 
 ShiftDecider::ShiftDecider(const rclcpp::NodeOptions & node_options)
 : Node("shift_decider", node_options)
@@ -34,33 +32,15 @@ ShiftDecider::ShiftDecider(const rclcpp::NodeOptions & node_options)
 
   pub_shift_cmd_ =
     create_publisher<autoware_vehicle_msgs::msg::GearCommand>("output/gear_cmd", durable_qos);
-  sub_control_cmd_ = create_subscription<autoware_control_msgs::msg::Control>(
-    "input/control_cmd", queue_size, std::bind(&ShiftDecider::onControlCmd, this, _1));
-  sub_autoware_state_ = create_subscription<autoware_system_msgs::msg::AutowareState>(
-    "input/state", queue_size, std::bind(&ShiftDecider::onAutowareState, this, _1));
-  sub_current_gear_ = create_subscription<autoware_vehicle_msgs::msg::GearReport>(
-    "input/current_gear", queue_size, std::bind(&ShiftDecider::onCurrentGear, this, _1));
 
   initTimer(0.1);
 }
 
-void ShiftDecider::onControlCmd(autoware_control_msgs::msg::Control::SharedPtr msg)
-{
-  control_cmd_ = msg;
-}
-
-void ShiftDecider::onAutowareState(autoware_system_msgs::msg::AutowareState::SharedPtr msg)
-{
-  autoware_state_ = msg;
-}
-
-void ShiftDecider::onCurrentGear(autoware_vehicle_msgs::msg::GearReport::SharedPtr msg)
-{
-  current_gear_ptr_ = msg;
-}
-
 void ShiftDecider::onTimer()
 {
+  control_cmd_ = sub_control_cmd_.takeData();
+  autoware_state_ = sub_autoware_state_.takeData();
+  current_gear_ptr_ = sub_current_gear_.takeData();
   if (!autoware_state_ || !control_cmd_ || !current_gear_ptr_) {
     return;
   }