feat(joy_controller): use polling subscriber (#7286)

Signed-off-by: Takayuki Murooka <[email protected]>

control/joy_controller/include/joy_controller/joy_controller.hpp

@@ -18,6 +18,7 @@
 #include "joy_controller/joy_converter/joy_converter_base.hpp"
 
 #include <rclcpp/rclcpp.hpp>
+#include <tier4_autoware_utils/ros/polling_subscriber.hpp>
 
 #include <autoware_control_msgs/msg/control.hpp>
 #include <autoware_vehicle_msgs/msg/engage.hpp>
@@ -66,19 +67,20 @@ class AutowareJoyControllerNode : public rclcpp::Node
   double backward_accel_ratio_;
 
   // CallbackGroups
-  rclcpp::CallbackGroup::SharedPtr callback_group_subscribers_;
   rclcpp::CallbackGroup::SharedPtr callback_group_services_;
 
   // Subscriber
-  rclcpp::Subscription<sensor_msgs::msg::Joy>::SharedPtr sub_joy_;
-  rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr sub_odom_;
+  tier4_autoware_utils::InterProcessPollingSubscriber<sensor_msgs::msg::Joy> sub_joy_{
+    this, "input/joy"};
+  tier4_autoware_utils::InterProcessPollingSubscriber<nav_msgs::msg::Odometry> sub_odom_{
+    this, "input/odometry"};
 
   rclcpp::Time last_joy_received_time_;
   std::shared_ptr<const JoyConverterBase> joy_;
   geometry_msgs::msg::TwistStamped::ConstSharedPtr twist_;
 
-  void onJoy(const sensor_msgs::msg::Joy::ConstSharedPtr msg);
-  void onOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg);
+  void onJoy();
+  void onOdometry();
 
   // Publisher
   rclcpp::Publisher<autoware_control_msgs::msg::Control>::SharedPtr pub_control_command_;

control/joy_controller/package.xml

@@ -26,6 +26,7 @@
   <depend>sensor_msgs</depend>
   <depend>std_srvs</depend>
   <depend>tier4_api_utils</depend>
+  <depend>tier4_autoware_utils</depend>
   <depend>tier4_control_msgs</depend>
   <depend>tier4_external_api_msgs</depend>
 

control/joy_controller/src/joy_controller/joy_controller_node.cpp

@@ -148,8 +148,9 @@ double calcMapping(const double input, const double sensitivity)
 
 namespace joy_controller
 {
-void AutowareJoyControllerNode::onJoy(const sensor_msgs::msg::Joy::ConstSharedPtr msg)
+void AutowareJoyControllerNode::onJoy()
 {
+  const auto msg = sub_joy_.takeData();
   last_joy_received_time_ = msg->header.stamp;
   if (joy_type_ == "G29") {
     joy_ = std::make_shared<const G29JoyConverter>(*msg);
@@ -190,8 +191,13 @@ void AutowareJoyControllerNode::onJoy(const sensor_msgs::msg::Joy::ConstSharedPt
   }
 }
 
-void AutowareJoyControllerNode::onOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg)
+void AutowareJoyControllerNode::onOdometry()
 {
+  if (raw_control_) {
+    return;
+  }
+
+  const auto msg = sub_odom_.takeData();
   auto twist = std::make_shared<geometry_msgs::msg::TwistStamped>();
   twist->header = msg->header;
   twist->twist = msg->twist.twist;
@@ -243,6 +249,9 @@ bool AutowareJoyControllerNode::isDataReady()
 
 void AutowareJoyControllerNode::onTimer()
 {
+  onOdometry();
+  onJoy();
+
   if (!isDataReady()) {
     return;
   }
@@ -470,23 +479,11 @@ AutowareJoyControllerNode::AutowareJoyControllerNode(const rclcpp::NodeOptions &
   RCLCPP_INFO(get_logger(), "Joy type: %s", joy_type_.c_str());
 
   // Callback Groups
-  callback_group_subscribers_ =
-    this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
   callback_group_services_ =
     this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
-  auto subscriber_option = rclcpp::SubscriptionOptions();
-  subscriber_option.callback_group = callback_group_subscribers_;
 
   // Subscriber
-  sub_joy_ = this->create_subscription<sensor_msgs::msg::Joy>(
-    "input/joy", 1, std::bind(&AutowareJoyControllerNode::onJoy, this, std::placeholders::_1),
-    subscriber_option);
-  if (!raw_control_) {
-    sub_odom_ = this->create_subscription<nav_msgs::msg::Odometry>(
-      "input/odometry", 1,
-      std::bind(&AutowareJoyControllerNode::onOdometry, this, std::placeholders::_1),
-      subscriber_option);
-  } else {
+  if (raw_control_) {
     twist_ = std::make_shared<geometry_msgs::msg::TwistStamped>();
   }