Revert "feat(aeb): replace InterProcessPollingSubscriber (autowarefoundation#6997)"

This reverts commit 74da4c1.

Author: beyza

control/autonomous_emergency_braking/include/autonomous_emergency_braking/node.hpp

@@ -20,7 +20,6 @@
 #include <pcl_ros/transforms.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <tier4_autoware_utils/geometry/geometry.hpp>
-#include <tier4_autoware_utils/ros/polling_subscriber.hpp>
 #include <vehicle_info_util/vehicle_info_util.hpp>
 
 #include <autoware_auto_planning_msgs/msg/trajectory.hpp>
@@ -225,28 +224,18 @@ class CollisionDataKeeper
   rclcpp::Clock::SharedPtr clock_;
 };
 
-static rclcpp::SensorDataQoS SingleDepthSensorQoS()
-{
-  rclcpp::SensorDataQoS qos;
-  qos.get_rmw_qos_profile().depth = 1;
-  return qos;
-}
-
 class AEB : public rclcpp::Node
 {
 public:
   explicit AEB(const rclcpp::NodeOptions & node_options);
 
   // subscriber
-  tier4_autoware_utils::InterProcessPollingSubscriber<PointCloud2> sub_point_cloud_{
-    this, "~/input/pointcloud", SingleDepthSensorQoS()};
-  tier4_autoware_utils::InterProcessPollingSubscriber<VelocityReport> sub_velocity_{
-    this, "~/input/velocity"};
-  tier4_autoware_utils::InterProcessPollingSubscriber<Imu> sub_imu_{this, "~/input/imu"};
-  tier4_autoware_utils::InterProcessPollingSubscriber<Trajectory> sub_predicted_traj_{
-    this, "~/input/predicted_trajectory"};
-  tier4_autoware_utils::InterProcessPollingSubscriber<AutowareState> sub_autoware_state_{
-    this, "/autoware/state"};
+  rclcpp::Subscription<PointCloud2>::SharedPtr sub_point_cloud_;
+  rclcpp::Subscription<VelocityReport>::SharedPtr sub_velocity_;
+  rclcpp::Subscription<Imu>::SharedPtr sub_imu_;
+  rclcpp::Subscription<Trajectory>::SharedPtr sub_predicted_traj_;
+  rclcpp::Subscription<AutowareState>::SharedPtr sub_autoware_state_;
+
   // publisher
   rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub_obstacle_pointcloud_;
   rclcpp::Publisher<MarkerArray>::SharedPtr debug_ego_path_publisher_;  // debug
@@ -256,12 +245,15 @@ class AEB : public rclcpp::Node
 
   // callback
   void onPointCloud(const PointCloud2::ConstSharedPtr input_msg);
+  void onVelocity(const VelocityReport::ConstSharedPtr input_msg);
   void onImu(const Imu::ConstSharedPtr input_msg);
   void onTimer();
+  void onPredictedTrajectory(const Trajectory::ConstSharedPtr input_msg);
+  void onAutowareState(const AutowareState::ConstSharedPtr input_msg);
   rcl_interfaces::msg::SetParametersResult onParameter(
     const std::vector<rclcpp::Parameter> & parameters);
 
-  bool fetchLatestData();
+  bool isDataReady();
 
   // main function
   void onCheckCollision(DiagnosticStatusWrapper & stat);

control/autonomous_emergency_braking/package.xml

@@ -7,7 +7,6 @@
   <maintainer email="takamasa.horibe@tier4.jp">Takamasa Horibe</maintainer>
   <maintainer email="tomoya.kimura@tier4.jp">Tomoya Kimura</maintainer>
   <maintainer email="mamoru.sobue@tier4.jp">Mamoru Sobue</maintainer>
-  <maintainer email="daniel.sanchez@tier4.jp">Daniel Sanchez</maintainer>
 
   <license>Apache License 2.0</license>
 

control/autonomous_emergency_braking/src/node.cpp

@@ -105,6 +105,27 @@ AEB::AEB(const rclcpp::NodeOptions & node_options)
   vehicle_info_(vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo()),
   collision_data_keeper_(this->get_clock())
 {
+  // Subscribers
+  {
+    sub_point_cloud_ = this->create_subscription<PointCloud2>(
+      "~/input/pointcloud", rclcpp::SensorDataQoS(),
+      std::bind(&AEB::onPointCloud, this, std::placeholders::_1));
+
+    sub_velocity_ = this->create_subscription<VelocityReport>(
+      "~/input/velocity", rclcpp::QoS{1}, std::bind(&AEB::onVelocity, this, std::placeholders::_1));
+
+    sub_imu_ = this->create_subscription<Imu>(
+      "~/input/imu", rclcpp::QoS{1}, std::bind(&AEB::onImu, this, std::placeholders::_1));
+
+    sub_predicted_traj_ = this->create_subscription<Trajectory>(
+      "~/input/predicted_trajectory", rclcpp::QoS{1},
+      std::bind(&AEB::onPredictedTrajectory, this, std::placeholders::_1));
+
+    sub_autoware_state_ = this->create_subscription<AutowareState>(
+      "/autoware/state", rclcpp::QoS{1},
+      std::bind(&AEB::onAutowareState, this, std::placeholders::_1));
+  }
+
   // Publisher
   {
     pub_obstacle_pointcloud_ =
@@ -208,6 +229,11 @@ void AEB::onTimer()
   updater_.force_update();
 }
 
+void AEB::onVelocity(const VelocityReport::ConstSharedPtr input_msg)
+{
+  current_velocity_ptr_ = input_msg;
+}
+
 void AEB::onImu(const Imu::ConstSharedPtr input_msg)
 {
   // transform imu
@@ -227,6 +253,17 @@ void AEB::onImu(const Imu::ConstSharedPtr input_msg)
   tf2::doTransform(input_msg->angular_velocity, *angular_velocity_ptr_, transform_stamped);
 }
 
+void AEB::onPredictedTrajectory(
+  const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr input_msg)
+{
+  predicted_traj_ptr_ = input_msg;
+}
+
+void AEB::onAutowareState(const AutowareState::ConstSharedPtr input_msg)
+{
+  autoware_state_ = input_msg;
+}
+
 void AEB::onPointCloud(const PointCloud2::ConstSharedPtr input_msg)
 {
   PointCloud::Ptr pointcloud_ptr(new PointCloud);
@@ -279,23 +316,17 @@ void AEB::onPointCloud(const PointCloud2::ConstSharedPtr input_msg)
   obstacle_ros_pointcloud_ptr_->header = input_msg->header;
 }
 
-bool AEB::fetchLatestData()
+bool AEB::isDataReady()
 {
   const auto missing = [this](const auto & name) {
     RCLCPP_INFO_SKIPFIRST_THROTTLE(get_logger(), *get_clock(), 5000, "[AEB] waiting for %s", name);
     return false;
   };
 
-  current_velocity_ptr_ = sub_velocity_.takeData();
   if (!current_velocity_ptr_) {
     return missing("ego velocity");
   }
 
-  const auto pointcloud_ptr = sub_point_cloud_.takeData();
-  if (!pointcloud_ptr) {
-    return missing("object pointcloud message");
-  }
-  onPointCloud(pointcloud_ptr);
   if (!obstacle_ros_pointcloud_ptr_) {
     return missing("object pointcloud");
   }
@@ -312,12 +343,10 @@ bool AEB::fetchLatestData()
     return missing("imu");
   }
 
-  predicted_traj_ptr_ = sub_predicted_traj_.takeData();
   if (use_predicted_trajectory_ && !predicted_traj_ptr_) {
     return missing("control predicted trajectory");
   }
 
-  autoware_state_ = sub_autoware_state_.takeData();
   if (!autoware_state_) {
     return missing("autoware_state");
   }
@@ -354,7 +383,7 @@ bool AEB::checkCollision(MarkerArray & debug_markers)
   using colorTuple = std::tuple<double, double, double, double>;
 
   // step1. check data
-  if (!fetchLatestData()) {
+  if (!isDataReady()) {
     return false;
   }