replace polling subscriber

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

Author: soblin

control/autonomous_emergency_braking/include/autonomous_emergency_braking/node.hpp

@@ -20,7 +20,7 @@
 #include <pcl_ros/transforms.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <tier4_autoware_utils/geometry/geometry.hpp>
-#include <tier4_autoware_utils/ros/polling_subsriber.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>
@@ -240,7 +240,7 @@ class AEB : public rclcpp::Node
   // subscriber
   tier4_autoware_utils::InterProcessPollingSubscriber<PointCloud2> sub_point_cloud_{
     this, "~/input/pointcloud", getSingleDepthSensorQoS()};
-  tier4_autoware_utils::InterProcessPollingSubscriber<VelocityReport> sub_veloclty_{
+  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_{
@@ -256,12 +256,12 @@ class AEB : public rclcpp::Node
   rclcpp::TimerBase::SharedPtr timer_;
 
   // callback
-  void onPointCloud(const PointCloud2::ConstSharedPtr input_msg);
-  void onVelocity(const VelocityReport::ConstSharedPtr input_msg);
-  void onImu(const Imu::ConstSharedPtr input_msg);
+  void fetchPointCloud();
+  void fetchVelocity();
+  void fetchImu();
+  void fetchPredictedTrajectory();
+  void fetchAutowareState();
   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);
 
@@ -301,10 +301,10 @@ class AEB : public rclcpp::Node
     const ObjectData & closest_object, const Path & path, const double current_ego_speed);
 
   PointCloud2::SharedPtr obstacle_ros_pointcloud_ptr_{nullptr};
-  VelocityReport::ConstSharedPtr current_velocity_ptr_{nullptr};
+  VelocityReport::SharedPtr current_velocity_ptr_{nullptr};
   Vector3::SharedPtr angular_velocity_ptr_{nullptr};
-  Trajectory::ConstSharedPtr predicted_traj_ptr_{nullptr};
-  AutowareState::ConstSharedPtr autoware_state_{nullptr};
+  Trajectory::SharedPtr predicted_traj_ptr_{nullptr};
+  AutowareState::SharedPtr autoware_state_{nullptr};
 
   tf2_ros::Buffer tf_buffer_{get_clock()};
   tf2_ros::TransformListener tf_listener_{tf_buffer_};

control/autonomous_emergency_braking/src/node.cpp

@@ -208,68 +208,83 @@ void AEB::onTimer()
   updater_.force_update();
 }
 
-void AEB::onVelocity(const VelocityReport::ConstSharedPtr input_msg)
+void AEB::fetchVelocity()
 {
-  current_velocity_ptr_ = input_msg;
+  if (sub_velocity_.updateLatestData()) {
+    current_velocity_ptr_ = std::make_shared<VelocityReport>(sub_velocity_.getData());
+  }
 }
 
-void AEB::onImu(const Imu::ConstSharedPtr input_msg)
+void AEB::fetchImu()
 {
+  if (!sub_imu_.updateLatestData()) {
+    return;
+  }
+
+  const auto & input_msg = sub_imu_.getData();
   // transform imu
   geometry_msgs::msg::TransformStamped transform_stamped{};
   try {
     transform_stamped = tf_buffer_.lookupTransform(
-      "base_link", input_msg->header.frame_id, input_msg->header.stamp,
+      "base_link", input_msg.header.frame_id, input_msg.header.stamp,
       rclcpp::Duration::from_seconds(0.5));
   } catch (tf2::TransformException & ex) {
     RCLCPP_ERROR_STREAM(
       get_logger(),
-      "[AEB] Failed to look up transform from base_link to" << input_msg->header.frame_id);
+      "[AEB] Failed to look up transform from base_link to" << input_msg.header.frame_id);
     return;
   }
 
   angular_velocity_ptr_ = std::make_shared<Vector3>();
-  tf2::doTransform(input_msg->angular_velocity, *angular_velocity_ptr_, transform_stamped);
+  tf2::doTransform(input_msg.angular_velocity, *angular_velocity_ptr_, transform_stamped);
 }
 
-void AEB::onPredictedTrajectory(
-  const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr input_msg)
+void AEB::fetchPredictedTrajectory()
 {
-  predicted_traj_ptr_ = input_msg;
+  if (sub_predicted_traj_.updateLatestData()) {
+    predicted_traj_ptr_ = std::make_shared<Trajectory>(sub_predicted_traj_.getData());
+  }
 }
 
-void AEB::onAutowareState(const AutowareState::ConstSharedPtr input_msg)
+void AEB::fetchAutowareState()
 {
-  autoware_state_ = input_msg;
+  if (sub_autoware_state_.updateLatestData()) {
+    autoware_state_ = std::make_shared<AutowareState>(sub_autoware_state_.getData());
+  }
 }
 
-void AEB::onPointCloud(const PointCloud2::ConstSharedPtr input_msg)
+void AEB::fetchPointCloud()
 {
+  if (!sub_point_cloud_.updateLatestData()) {
+    return;
+  }
+
+  const auto & input_msg = sub_point_cloud_.getData();
   PointCloud::Ptr pointcloud_ptr(new PointCloud);
-  pcl::fromROSMsg(*input_msg, *pointcloud_ptr);
+  pcl::fromROSMsg(input_msg, *pointcloud_ptr);
 
-  if (input_msg->header.frame_id != "base_link") {
+  if (input_msg.header.frame_id != "base_link") {
     RCLCPP_ERROR_STREAM(
       get_logger(),
-      "[AEB]: Input point cloud frame is not base_link and it is " << input_msg->header.frame_id);
+      "[AEB]: Input point cloud frame is not base_link and it is " << input_msg.header.frame_id);
     // transform pointcloud
     geometry_msgs::msg::TransformStamped transform_stamped{};
     try {
       transform_stamped = tf_buffer_.lookupTransform(
-        "base_link", input_msg->header.frame_id, input_msg->header.stamp,
+        "base_link", input_msg.header.frame_id, input_msg.header.stamp,
         rclcpp::Duration::from_seconds(0.5));
     } catch (tf2::TransformException & ex) {
       RCLCPP_ERROR_STREAM(
         get_logger(),
-        "[AEB] Failed to look up transform from base_link to" << input_msg->header.frame_id);
+        "[AEB] Failed to look up transform from base_link to" << input_msg.header.frame_id);
       return;
     }
 
     // transform by using eigen matrix
     PointCloud2 transformed_points{};
     const Eigen::Matrix4f affine_matrix =
       tf2::transformToEigen(transform_stamped.transform).matrix().cast<float>();
-    pcl_ros::transformPointCloud(affine_matrix, *input_msg, transformed_points);
+    pcl_ros::transformPointCloud(affine_matrix, input_msg, transformed_points);
     pcl::fromROSMsg(transformed_points, *pointcloud_ptr);
   }
 
@@ -292,7 +307,7 @@ void AEB::onPointCloud(const PointCloud2::ConstSharedPtr input_msg)
   obstacle_ros_pointcloud_ptr_ = std::make_shared<PointCloud2>();
 
   pcl::toROSMsg(*no_height_filtered_pointcloud_ptr, *obstacle_ros_pointcloud_ptr_);
-  obstacle_ros_pointcloud_ptr_->header = input_msg->header;
+  obstacle_ros_pointcloud_ptr_->header = input_msg.header;
 }
 
 bool AEB::isDataReady()
@@ -302,22 +317,27 @@ bool AEB::isDataReady()
     return false;
   };
 
+  fetchVelocity();
   if (!current_velocity_ptr_) {
     return missing("ego velocity");
   }
 
+  fetchPointCloud();
   if (!obstacle_ros_pointcloud_ptr_) {
-    return missing("object pointcloud");
+    return missing("filtered object pointcloud");
   }
 
+  fetchImu();
   if (use_imu_path_ && !angular_velocity_ptr_) {
-    return missing("imu");
+    return missing("filtered imu");
   }
 
+  fetchPredictedTrajectory();
   if (use_predicted_trajectory_ && !predicted_traj_ptr_) {
     return missing("control predicted trajectory");
   }
 
+  fetchAutowareState();
   if (!autoware_state_) {
     return missing("autoware_state");
   }