feat(tier4_autoware_utils, obstacle_cruise): change to read topic by polling (autowarefoundation#6702)

change to read topic by polling

Signed-off-by: Yuki Takagi <yuki.takagi@tier4.jp>

Author: yuki-takagi-66

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

@@ -0,0 +1,65 @@
+// Copyright 2024 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef TIER4_AUTOWARE_UTILS__ROS__POLLING_SUBSCRIBER_HPP_
+#define TIER4_AUTOWARE_UTILS__ROS__POLLING_SUBSCRIBER_HPP_
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <string>
+
+namespace tier4_autoware_utils
+{
+
+template <typename T>
+class InterProcessPollingSubscriber
+{
+private:
+  typename rclcpp::Subscription<T>::SharedPtr subscriber_;
+  std::optional<T> data_;
+
+public:
+  explicit InterProcessPollingSubscriber(rclcpp::Node * node, const std::string & topic_name)
+  {
+    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}, [node](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;
+    }
+    return data_.has_value();
+  };
+  const T & getData() const
+  {
+    if (!data_.has_value()) {
+      throw std::runtime_error("Bad_optional_access in class InterProcessPollingSubscriber");
+    }
+    return data_.value();
+  };
+};
+
+}  // namespace tier4_autoware_utils
+
+#endif  // TIER4_AUTOWARE_UTILS__ROS__POLLING_SUBSCRIBER_HPP_

planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp

@@ -21,6 +21,7 @@
 #include "obstacle_cruise_planner/type_alias.hpp"
 #include "signal_processing/lowpass_filter_1d.hpp"
 #include "tier4_autoware_utils/ros/logger_level_configure.hpp"
+#include "tier4_autoware_utils/ros/polling_subscriber.hpp"
 #include "tier4_autoware_utils/system/stop_watch.hpp"
 
 #include <rclcpp/rclcpp.hpp>
@@ -137,14 +138,12 @@ class ObstacleCruisePlannerNode : public rclcpp::Node
 
   // subscriber
   rclcpp::Subscription<Trajectory>::SharedPtr traj_sub_;
-  rclcpp::Subscription<PredictedObjects>::SharedPtr objects_sub_;
-  rclcpp::Subscription<Odometry>::SharedPtr odom_sub_;
-  rclcpp::Subscription<AccelWithCovarianceStamped>::SharedPtr acc_sub_;
-
-  // data for callback functions
-  PredictedObjects::ConstSharedPtr objects_ptr_{nullptr};
-  Odometry::ConstSharedPtr ego_odom_ptr_{nullptr};
-  AccelWithCovarianceStamped::ConstSharedPtr ego_accel_ptr_{nullptr};
+  tier4_autoware_utils::InterProcessPollingSubscriber<Odometry> ego_odom_sub_{
+    this, "~/input/odometry"};
+  tier4_autoware_utils::InterProcessPollingSubscriber<PredictedObjects> objects_sub_{
+    this, "~/input/objects"};
+  tier4_autoware_utils::InterProcessPollingSubscriber<AccelWithCovarianceStamped> acc_sub_{
+    this, "~/input/acceleration"};
 
   // Vehicle Parameters
   VehicleInfo vehicle_info_;

planning/obstacle_cruise_planner/src/node.cpp

@@ -357,15 +357,6 @@ ObstacleCruisePlannerNode::ObstacleCruisePlannerNode(const rclcpp::NodeOptions &
   traj_sub_ = create_subscription<Trajectory>(
     "~/input/trajectory", rclcpp::QoS{1},
     std::bind(&ObstacleCruisePlannerNode::onTrajectory, this, _1));
-  objects_sub_ = create_subscription<PredictedObjects>(
-    "~/input/objects", rclcpp::QoS{1},
-    [this](const PredictedObjects::ConstSharedPtr msg) { objects_ptr_ = msg; });
-  odom_sub_ = create_subscription<Odometry>(
-    "~/input/odometry", rclcpp::QoS{1},
-    [this](const Odometry::ConstSharedPtr msg) { ego_odom_ptr_ = msg; });
-  acc_sub_ = create_subscription<AccelWithCovarianceStamped>(
-    "~/input/acceleration", rclcpp::QoS{1},
-    [this](const AccelWithCovarianceStamped::ConstSharedPtr msg) { ego_accel_ptr_ = msg; });
 
   // publisher
   trajectory_pub_ = create_publisher<Trajectory>("~/output/trajectory", 1);
@@ -492,10 +483,15 @@ rcl_interfaces::msg::SetParametersResult ObstacleCruisePlannerNode::onParam(
 
 void ObstacleCruisePlannerNode::onTrajectory(const Trajectory::ConstSharedPtr msg)
 {
-  const auto traj_points = motion_utils::convertToTrajectoryPointArray(*msg);
+  if (
+    !ego_odom_sub_.updateLatestData() || !objects_sub_.updateLatestData() ||
+    !acc_sub_.updateLatestData()) {
+    return;
+  }
 
+  const auto traj_points = motion_utils::convertToTrajectoryPointArray(*msg);
   // check if subscribed variables are ready
-  if (traj_points.empty() || !ego_odom_ptr_ || !ego_accel_ptr_ || !objects_ptr_) {
+  if (traj_points.empty()) {
     return;
   }
 
@@ -607,11 +603,11 @@ std::vector<Obstacle> ObstacleCruisePlannerNode::convertToObstacles(
 {
   stop_watch_.tic(__func__);
 
-  const auto obj_stamp = rclcpp::Time(objects_ptr_->header.stamp);
+  const auto obj_stamp = rclcpp::Time(objects_sub_.getData().header.stamp);
   const auto & p = behavior_determination_param_;
 
   std::vector<Obstacle> target_obstacles;
-  for (const auto & predicted_object : objects_ptr_->objects) {
+  for (const auto & predicted_object : objects_sub_.getData().objects) {
     const auto & object_id =
       tier4_autoware_utils::toHexString(predicted_object.object_id).substr(0, 4);
     const auto & current_obstacle_pose =
@@ -629,7 +625,8 @@ std::vector<Obstacle> ObstacleCruisePlannerNode::convertToObstacles(
     }
 
     // 2. Check if the obstacle is in front of the ego.
-    const size_t ego_idx = ego_nearest_param_.findIndex(traj_points, ego_odom_ptr_->pose.pose);
+    const size_t ego_idx =
+      ego_nearest_param_.findIndex(traj_points, ego_odom_sub_.getData().pose.pose);
     const auto ego_to_obstacle_distance =
       calcDistanceToFrontVehicle(traj_points, ego_idx, current_obstacle_pose.pose.position);
     if (!ego_to_obstacle_distance) {
@@ -725,7 +722,7 @@ ObstacleCruisePlannerNode::determineEgoBehaviorAgainstObstacles(
   // calculated decimated trajectory points and trajectory polygon
   const auto decimated_traj_points = decimateTrajectoryPoints(traj_points);
   const auto decimated_traj_polys =
-    createOneStepPolygons(decimated_traj_points, vehicle_info_, ego_odom_ptr_->pose.pose);
+    createOneStepPolygons(decimated_traj_points, vehicle_info_, ego_odom_sub_.getData().pose.pose);
   debug_data_ptr_->detection_polygons = decimated_traj_polys;
 
   // determine ego's behavior from stop, cruise and slow down
@@ -783,7 +780,7 @@ ObstacleCruisePlannerNode::determineEgoBehaviorAgainstObstacles(
   slow_down_condition_counter_.removeCounterUnlessUpdated();
 
   // Check target obstacles' consistency
-  checkConsistency(objects_ptr_->header.stamp, *objects_ptr_, stop_obstacles);
+  checkConsistency(objects_sub_.getData().header.stamp, objects_sub_.getData(), stop_obstacles);
 
   // update previous obstacles
   prev_stop_obstacles_ = stop_obstacles;
@@ -805,7 +802,7 @@ std::vector<TrajectoryPoint> ObstacleCruisePlannerNode::decimateTrajectoryPoints
 
   // trim trajectory
   const size_t ego_seg_idx =
-    ego_nearest_param_.findSegmentIndex(traj_points, ego_odom_ptr_->pose.pose);
+    ego_nearest_param_.findSegmentIndex(traj_points, ego_odom_sub_.getData().pose.pose);
   const size_t traj_start_point_idx = ego_seg_idx;
   const auto trimmed_traj_points =
     std::vector<TrajectoryPoint>(traj_points.begin() + traj_start_point_idx, traj_points.end());
@@ -1189,7 +1186,7 @@ std::optional<StopObstacle> ObstacleCruisePlannerNode::createStopObstacle(
 
   // calculate collision points with trajectory with lateral stop margin
   const auto traj_polys_with_lat_margin = createOneStepPolygons(
-    traj_points, vehicle_info_, ego_odom_ptr_->pose.pose, p.max_lat_margin_for_stop);
+    traj_points, vehicle_info_, ego_odom_sub_.getData().pose.pose, p.max_lat_margin_for_stop);
 
   const auto collision_point = polygon_utils::getCollisionPoint(
     traj_points, traj_polys_with_lat_margin, obstacle, is_driving_forward_, vehicle_info_);
@@ -1259,7 +1256,7 @@ std::optional<SlowDownObstacle> ObstacleCruisePlannerNode::createSlowDownObstacl
   // calculate collision points with trajectory with lateral stop margin
   // NOTE: For additional margin, hysteresis is not divided by two.
   const auto traj_polys_with_lat_margin = createOneStepPolygons(
-    traj_points, vehicle_info_, ego_odom_ptr_->pose.pose,
+    traj_points, vehicle_info_, ego_odom_sub_.getData().pose.pose,
     p.max_lat_margin_for_slow_down + p.lat_hysteresis_margin_for_slow_down);
 
   std::vector<Polygon2d> front_collision_polygons;
@@ -1422,8 +1419,8 @@ double ObstacleCruisePlannerNode::calcCollisionTimeMargin(
   const std::vector<PointWithStamp> & collision_points,
   const std::vector<TrajectoryPoint> & traj_points, const bool is_driving_forward) const
 {
-  const auto & ego_pose = ego_odom_ptr_->pose.pose;
-  const double ego_vel = ego_odom_ptr_->twist.twist.linear.x;
+  const auto & ego_pose = ego_odom_sub_.getData().pose.pose;
+  const double ego_vel = ego_odom_sub_.getData().twist.twist.linear.x;
 
   const double time_to_reach_collision_point = [&]() {
     const double abs_ego_offset = is_driving_forward
@@ -1455,9 +1452,9 @@ PlannerData ObstacleCruisePlannerNode::createPlannerData(
   PlannerData planner_data;
   planner_data.current_time = now();
   planner_data.traj_points = traj_points;
-  planner_data.ego_pose = ego_odom_ptr_->pose.pose;
-  planner_data.ego_vel = ego_odom_ptr_->twist.twist.linear.x;
-  planner_data.ego_acc = ego_accel_ptr_->accel.accel.linear.x;
+  planner_data.ego_pose = ego_odom_sub_.getData().pose.pose;
+  planner_data.ego_vel = ego_odom_sub_.getData().twist.twist.linear.x;
+  planner_data.ego_acc = acc_sub_.getData().accel.accel.linear.x;
   planner_data.is_driving_forward = is_driving_forward_;
   return planner_data;
 }