PR6976

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

Author: soblin

common/tier4_autoware_utils/include/tier4_autoware_utils/ros/polling_subscriber.hpp

@@ -17,6 +17,8 @@
 
 #include <rclcpp/rclcpp.hpp>
 
+#include <memory>
+#include <stdexcept>
 #include <string>
 
 namespace tier4_autoware_utils
@@ -27,37 +29,37 @@ class InterProcessPollingSubscriber
 {
 private:
   typename rclcpp::Subscription<T>::SharedPtr subscriber_;
-  std::optional<T> data_;
+  typename T::SharedPtr data_;
 
 public:
-  explicit InterProcessPollingSubscriber(rclcpp::Node * node, const std::string & topic_name)
+  explicit InterProcessPollingSubscriber(
+    rclcpp::Node * node, const std::string & topic_name, const rclcpp::QoS & qos = rclcpp::QoS{1})
   {
     auto noexec_callback_group =
       node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive, false);
     auto noexec_subscription_options = rclcpp::SubscriptionOptions();
     noexec_subscription_options.callback_group = noexec_callback_group;
 
     subscriber_ = node->create_subscription<T>(
-      topic_name, rclcpp::QoS{1},
+      topic_name, qos,
       [node]([[maybe_unused]] const typename T::ConstSharedPtr msg) { assert(false); },
       noexec_subscription_options);
-  };
-  bool updateLatestData()
-  {
-    rclcpp::MessageInfo message_info;
-    T tmp;
-    // The queue size (QoS) must be 1 to get the last message data.
-    if (subscriber_->take(tmp, message_info)) {
-      data_ = tmp;
+    if (qos.get_rmw_qos_profile().depth > 1) {
+      throw std::invalid_argument(
+        "InterProcessPollingSubscriber will be used with depth > 1, which may cause inefficient "
+        "serialization while updateLatestData()");
     }
-    return data_.has_value();
   };
-  const T & getData() const
+  typename T::ConstSharedPtr takeData()
   {
-    if (!data_.has_value()) {
-      throw std::runtime_error("Bad_optional_access in class InterProcessPollingSubscriber");
+    auto new_data = std::make_shared<T>();
+    rclcpp::MessageInfo message_info;
+    const bool success = subscriber_->take(*new_data, message_info);
+    if (success) {
+      data_ = new_data;
     }
-    return data_.value();
+
+    return data_;
   };
 };
 

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/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;
   }
 

planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp

@@ -55,15 +55,19 @@ class ObstacleCruisePlannerNode : public rclcpp::Node
     const std::vector<TrajectoryPoint> & traj_points,
     const vehicle_info_util::VehicleInfo & vehicle_info,
     const geometry_msgs::msg::Pose & current_ego_pose, const double lat_margin = 0.0) const;
-  std::vector<Obstacle> convertToObstacles(const std::vector<TrajectoryPoint> & traj_points) const;
+  std::vector<Obstacle> convertToObstacles(
+    const Odometry & odometry, const PredictedObjects & objects,
+    const std::vector<TrajectoryPoint> & traj_points) const;
   std::tuple<std::vector<StopObstacle>, std::vector<CruiseObstacle>, std::vector<SlowDownObstacle>>
   determineEgoBehaviorAgainstObstacles(
+    const Odometry & odometry, const PredictedObjects & objecrts,
     const std::vector<TrajectoryPoint> & traj_points, const std::vector<Obstacle> & obstacles);
   std::vector<TrajectoryPoint> decimateTrajectoryPoints(
-    const std::vector<TrajectoryPoint> & traj_points) const;
+    const Odometry & odometry, const std::vector<TrajectoryPoint> & traj_points) const;
   std::optional<StopObstacle> createStopObstacle(
-    const std::vector<TrajectoryPoint> & traj_points, const std::vector<Polygon2d> & traj_polys,
-    const Obstacle & obstacle, const double precise_lateral_dist) const;
+    const Odometry & odometry, const std::vector<TrajectoryPoint> & traj_points,
+    const std::vector<Polygon2d> & traj_polys, const Obstacle & obstacle,
+    const double precise_lateral_dist) const;
   bool isStopObstacle(const uint8_t label) const;
   bool isInsideCruiseObstacle(const uint8_t label) const;
   bool isOutsideCruiseObstacle(const uint8_t label) const;
@@ -88,12 +92,14 @@ class ObstacleCruisePlannerNode : public rclcpp::Node
   bool isObstacleCrossing(
     const std::vector<TrajectoryPoint> & traj_points, const Obstacle & obstacle) const;
   double calcCollisionTimeMargin(
-    const std::vector<PointWithStamp> & collision_points,
+    const Odometry & odometry, const std::vector<PointWithStamp> & collision_points,
     const std::vector<TrajectoryPoint> & traj_points, const bool is_driving_forward) const;
   std::optional<SlowDownObstacle> createSlowDownObstacle(
-    const std::vector<TrajectoryPoint> & traj_points, const Obstacle & obstacle,
-    const double precise_lat_dist);
-  PlannerData createPlannerData(const std::vector<TrajectoryPoint> & traj_points) const;
+    const Odometry & odometry, const std::vector<TrajectoryPoint> & traj_points,
+    const Obstacle & obstacle, const double precise_lat_dist);
+  PlannerData createPlannerData(
+    const Odometry & odometry, const AccelWithCovarianceStamped & acc,
+    const std::vector<TrajectoryPoint> & traj_points) const;
 
   void checkConsistency(
     const rclcpp::Time & current_time, const PredictedObjects & predicted_objects,