feat(aeb): replace InterProcessPollingSubscriber (#6997)

Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>

Author: soblin

control/autonomous_emergency_braking/include/autonomous_emergency_braking/node.hpp

@@ -20,6 +20,7 @@
 #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>
@@ -224,18 +225,28 @@ 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
-  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_;
-
+  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"};
   // publisher
   rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub_obstacle_pointcloud_;
   rclcpp::Publisher<MarkerArray>::SharedPtr debug_ego_path_publisher_;  // debug
@@ -245,15 +256,12 @@ 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 isDataReady();
+  bool fetchLatestData();
 
   // main function
   void onCheckCollision(DiagnosticStatusWrapper & stat);

control/autonomous_emergency_braking/package.xml

@@ -7,6 +7,7 @@
   <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,27 +105,6 @@ 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_ =
@@ -229,11 +208,6 @@ 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
@@ -253,17 +227,6 @@ 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);
@@ -316,29 +279,42 @@ void AEB::onPointCloud(const PointCloud2::ConstSharedPtr input_msg)
   obstacle_ros_pointcloud_ptr_->header = input_msg->header;
 }
 
-bool AEB::isDataReady()
+bool AEB::fetchLatestData()
 {
   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");
   }
 
+  const auto imu_ptr = sub_imu_.takeData();
+  if (use_imu_path_ && !imu_ptr) {
+    return missing("imu message");
+  }
+  onImu(imu_ptr);
   if (use_imu_path_ && !angular_velocity_ptr_) {
     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");
   }
@@ -375,7 +351,7 @@ bool AEB::checkCollision(MarkerArray & debug_markers)
   using colorTuple = std::tuple<double, double, double, double>;
 
   // step1. check data
-  if (!isDataReady()) {
+  if (!fetchLatestData()) {
     return false;
   }