update

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

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

@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef TIER4_AUTOWARE_UTILS__ROS__POLLING_RECEIVER_HPP_
-#define TIER4_AUTOWARE_UTILS__ROS__POLLING_RECEIVER_HPP_
+#ifndef TIER4_AUTOWARE_UTILS__ROS__POLLING_SUBSCRIBER_HPP_
+#define TIER4_AUTOWARE_UTILS__ROS__POLLING_SUBSCRIBER_HPP_
 
 #include <rclcpp/rclcpp.hpp>
 
@@ -39,16 +39,17 @@ class InterProcessPollingSubscriber
       topic_name, rclcpp::QoS{1}, [node](const typename T::ConstSharedPtr msg) { assert(false); },
       noexec_subscription_options);
   };
-  std::optional<T> takeLatestData()
+  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;
+    return data.has_value();
   };
+  const std::optional<T> & getData() const { return data; };
 };
 
 }  // namespace tier4_autoware_utils

planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp

@@ -145,10 +145,6 @@ class ObstacleCruisePlannerNode : public rclcpp::Node
     this, "~/input/objects"};
   tier4_autoware_utils::InterProcessPollingSubscriber<AccelWithCovarianceStamped> acc_sub_{
     this, "~/input/acceleration"};
-
-  std::optional<Odometry> ego_odom_opt_;
-  std::optional<PredictedObjects> objects_opt_;
-  std::optional<AccelWithCovarianceStamped> acc_opt_;
 
   // Vehicle Parameters
   VehicleInfo vehicle_info_;

planning/obstacle_cruise_planner/src/node.cpp

@@ -484,11 +484,9 @@ rcl_interfaces::msg::SetParametersResult ObstacleCruisePlannerNode::onParam(
 
 void ObstacleCruisePlannerNode::onTrajectory(const Trajectory::ConstSharedPtr msg)
 {
-  ego_odom_opt_ = ego_odom_sub_.takeLatestData();
-  objects_opt_ = objects_sub_.takeLatestData();
-  acc_opt_ = acc_sub_.takeLatestData();
-
-  if (!ego_odom_opt_ || !objects_opt_ || !acc_opt_) {
+  if (
+    !ego_odom_sub_.updateLatestData() || !objects_sub_.updateLatestData() ||
+    !acc_sub_.updateLatestData()) {
     return;
   }
 
@@ -607,11 +605,11 @@ std::vector<Obstacle> ObstacleCruisePlannerNode::convertToObstacles(
 {
   stop_watch_.tic(__func__);
 
-  const auto obj_stamp = rclcpp::Time(objects_opt_->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_opt_->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 +627,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_opt_->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 +724,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_opt_->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 +782,8 @@ ObstacleCruisePlannerNode::determineEgoBehaviorAgainstObstacles(
   slow_down_condition_counter_.removeCounterUnlessUpdated();
 
   // Check target obstacles' consistency
-  checkConsistency(objects_opt_->header.stamp, objects_opt_.value(), stop_obstacles);
+  checkConsistency(
+    objects_sub_.getData()->header.stamp, objects_sub_.getData().value(), stop_obstacles);
 
   // update previous obstacles
   prev_stop_obstacles_ = stop_obstacles;
@@ -805,7 +805,7 @@ std::vector<TrajectoryPoint> ObstacleCruisePlannerNode::decimateTrajectoryPoints
 
   // trim trajectory
   const size_t ego_seg_idx =
-    ego_nearest_param_.findSegmentIndex(traj_points, ego_odom_opt_->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 +1189,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_opt_->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 +1259,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_opt_->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 +1422,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_opt_->pose.pose;
-  const double ego_vel = ego_odom_opt_->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 +1455,9 @@ PlannerData ObstacleCruisePlannerNode::createPlannerData(
   PlannerData planner_data;
   planner_data.current_time = now();
   planner_data.traj_points = traj_points;
-  planner_data.ego_pose = ego_odom_opt_->pose.pose;
-  planner_data.ego_vel = ego_odom_opt_->twist.twist.linear.x;
-  planner_data.ego_acc = acc_opt_->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;
 }