change API

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

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,7 +29,7 @@ class InterProcessPollingSubscriber
 {
 private:
   typename rclcpp::Subscription<T>::SharedPtr subscriber_;
-  std::optional<T> data_;
+  typename T::SharedPtr data_;
 
 public:
   explicit InterProcessPollingSubscriber(
@@ -43,33 +45,21 @@ class InterProcessPollingSubscriber
       [node]([[maybe_unused]] const typename T::ConstSharedPtr msg) { assert(false); },
       noexec_subscription_options);
     if (qos.get_rmw_qos_profile().depth > 1) {
-      RCLCPP_WARN(
-        node->get_logger(),
+      throw std::invalid_argument(
         "InterProcessPollingSubscriber will be used with depth > 1, which may cause inefficient "
         "serialization while updateLatestData()");
     }
   };
-  bool updateLatestData()
+  typename T::ConstSharedPtr takeData()
   {
+    auto new_data = std::make_shared<T>();
     rclcpp::MessageInfo message_info;
-    T tmp;
-    // The queue size (QoS) must be 1 to get the last message data.
-    bool is_latest_message_consumed = false;
-    // pop the queue until latest data
-    while (subscriber_->take(tmp, message_info)) {
-      is_latest_message_consumed = true;
+    const bool success = subscriber_->take(*new_data, message_info);
+    if (success) {
+      data_ = new_data;
     }
-    if (is_latest_message_consumed) {
-      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();
+
+    return data_;
   };
 };
 

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,

planning/obstacle_cruise_planner/src/node.cpp

@@ -484,12 +484,17 @@ rcl_interfaces::msg::SetParametersResult ObstacleCruisePlannerNode::onParam(
 
 void ObstacleCruisePlannerNode::onTrajectory(const Trajectory::ConstSharedPtr msg)
 {
-  if (
-    !ego_odom_sub_.updateLatestData() || !objects_sub_.updateLatestData() ||
-    !acc_sub_.updateLatestData()) {
+  const auto ego_odom_ptr = ego_odom_sub_.takeData();
+  const auto objects_ptr = objects_sub_.takeData();
+  const auto acc_ptr = acc_sub_.takeData();
+  if (!ego_odom_ptr || !objects_ptr || !acc_ptr) {
     return;
   }
 
+  const auto & ego_odom = *ego_odom_ptr;
+  const auto & objects = *objects_ptr;
+  const auto & acc = *acc_ptr;
+
   const auto traj_points = motion_utils::convertToTrajectoryPointArray(*msg);
   // check if subscribed variables are ready
   if (traj_points.empty()) {
@@ -506,14 +511,14 @@ void ObstacleCruisePlannerNode::onTrajectory(const Trajectory::ConstSharedPtr ms
   //    (1) with a proper label
   //    (2) in front of ego
   //    (3) not too far from trajectory
-  const auto target_obstacles = convertToObstacles(traj_points);
+  const auto target_obstacles = convertToObstacles(ego_odom, objects, traj_points);
 
   //  2. Determine ego's behavior against each obstacle from stop, cruise and slow down.
   const auto & [stop_obstacles, cruise_obstacles, slow_down_obstacles] =
-    determineEgoBehaviorAgainstObstacles(traj_points, target_obstacles);
+    determineEgoBehaviorAgainstObstacles(ego_odom, objects, traj_points, target_obstacles);
 
   // 3. Create data for planning
-  const auto planner_data = createPlannerData(traj_points);
+  const auto planner_data = createPlannerData(ego_odom, acc, traj_points);
 
   // 4. Stop planning
   const auto stop_traj_points = planner_ptr_->generateStopTrajectory(planner_data, stop_obstacles);
@@ -629,15 +634,16 @@ std::vector<Polygon2d> ObstacleCruisePlannerNode::createOneStepPolygons(
 }
 
 std::vector<Obstacle> ObstacleCruisePlannerNode::convertToObstacles(
+  const Odometry & odometry, const PredictedObjects & objects,
   const std::vector<TrajectoryPoint> & traj_points) const
 {
   stop_watch_.tic(__func__);
 
-  const auto obj_stamp = rclcpp::Time(objects_sub_.getData().header.stamp);
+  const auto obj_stamp = rclcpp::Time(objects.header.stamp);
   const auto & p = behavior_determination_param_;
 
   std::vector<Obstacle> target_obstacles;
-  for (const auto & predicted_object : objects_sub_.getData().objects) {
+  for (const auto & predicted_object : objects.objects) {
     const auto & object_id =
       tier4_autoware_utils::toHexString(predicted_object.object_id).substr(0, 4);
     const auto & current_obstacle_pose =
@@ -655,8 +661,7 @@ 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_sub_.getData().pose.pose);
+    const size_t ego_idx = ego_nearest_param_.findIndex(traj_points, odometry.pose.pose);
     const auto ego_to_obstacle_distance =
       calcDistanceToFrontVehicle(traj_points, ego_idx, current_obstacle_pose.pose.position);
     if (!ego_to_obstacle_distance) {
@@ -745,14 +750,15 @@ bool ObstacleCruisePlannerNode::isFrontCollideObstacle(
 
 std::tuple<std::vector<StopObstacle>, std::vector<CruiseObstacle>, std::vector<SlowDownObstacle>>
 ObstacleCruisePlannerNode::determineEgoBehaviorAgainstObstacles(
+  const Odometry & odometry, const PredictedObjects & objects,
   const std::vector<TrajectoryPoint> & traj_points, const std::vector<Obstacle> & obstacles)
 {
   stop_watch_.tic(__func__);
 
   // calculated decimated trajectory points and trajectory polygon
-  const auto decimated_traj_points = decimateTrajectoryPoints(traj_points);
+  const auto decimated_traj_points = decimateTrajectoryPoints(odometry, traj_points);
   const auto decimated_traj_polys =
-    createOneStepPolygons(decimated_traj_points, vehicle_info_, ego_odom_sub_.getData().pose.pose);
+    createOneStepPolygons(decimated_traj_points, vehicle_info_, odometry.pose.pose);
   debug_data_ptr_->detection_polygons = decimated_traj_polys;
 
   // determine ego's behavior from stop, cruise and slow down
@@ -777,14 +783,14 @@ ObstacleCruisePlannerNode::determineEgoBehaviorAgainstObstacles(
       cruise_obstacles.push_back(*cruise_obstacle);
       continue;
     }
-    const auto stop_obstacle =
-      createStopObstacle(decimated_traj_points, decimated_traj_polys, obstacle, precise_lat_dist);
+    const auto stop_obstacle = createStopObstacle(
+      odometry, decimated_traj_points, decimated_traj_polys, obstacle, precise_lat_dist);
     if (stop_obstacle) {
       stop_obstacles.push_back(*stop_obstacle);
       continue;
     }
     const auto slow_down_obstacle =
-      createSlowDownObstacle(decimated_traj_points, obstacle, precise_lat_dist);
+      createSlowDownObstacle(odometry, decimated_traj_points, obstacle, precise_lat_dist);
     if (slow_down_obstacle) {
       slow_down_obstacles.push_back(*slow_down_obstacle);
       continue;
@@ -810,7 +816,7 @@ ObstacleCruisePlannerNode::determineEgoBehaviorAgainstObstacles(
   slow_down_condition_counter_.removeCounterUnlessUpdated();
 
   // Check target obstacles' consistency
-  checkConsistency(objects_sub_.getData().header.stamp, objects_sub_.getData(), stop_obstacles);
+  checkConsistency(objects.header.stamp, objects, stop_obstacles);
 
   // update previous obstacles
   prev_stop_obstacles_ = stop_obstacles;
@@ -826,13 +832,12 @@ ObstacleCruisePlannerNode::determineEgoBehaviorAgainstObstacles(
 }
 
 std::vector<TrajectoryPoint> ObstacleCruisePlannerNode::decimateTrajectoryPoints(
-  const std::vector<TrajectoryPoint> & traj_points) const
+  const Odometry & odometry, const std::vector<TrajectoryPoint> & traj_points) const
 {
   const auto & p = behavior_determination_param_;
 
   // trim trajectory
-  const size_t ego_seg_idx =
-    ego_nearest_param_.findSegmentIndex(traj_points, ego_odom_sub_.getData().pose.pose);
+  const size_t ego_seg_idx = ego_nearest_param_.findSegmentIndex(traj_points, odometry.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());
@@ -1151,8 +1156,9 @@ ObstacleCruisePlannerNode::createCollisionPointsForOutsideCruiseObstacle(
 }
 
 std::optional<StopObstacle> ObstacleCruisePlannerNode::createStopObstacle(
-  const std::vector<TrajectoryPoint> & traj_points, const std::vector<Polygon2d> & traj_polys,
-  const Obstacle & obstacle, const double precise_lat_dist) const
+  const Odometry & odometry, const std::vector<TrajectoryPoint> & traj_points,
+  const std::vector<Polygon2d> & traj_polys, const Obstacle & obstacle,
+  const double precise_lat_dist) const
 {
   const auto & p = behavior_determination_param_;
   const auto & object_id = obstacle.uuid.substr(0, 4);
@@ -1203,7 +1209,7 @@ std::optional<StopObstacle> ObstacleCruisePlannerNode::createStopObstacle(
     }
 
     const double collision_time_margin =
-      calcCollisionTimeMargin(collision_points, traj_points, is_driving_forward_);
+      calcCollisionTimeMargin(odometry, collision_points, traj_points, is_driving_forward_);
     if (p.collision_time_margin < collision_time_margin) {
       RCLCPP_INFO_EXPRESSION(
         get_logger(), enable_debug_info_,
@@ -1216,7 +1222,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_sub_.getData().pose.pose, p.max_lat_margin_for_stop);
+    traj_points, vehicle_info_, odometry.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_);
@@ -1231,8 +1237,8 @@ std::optional<StopObstacle> ObstacleCruisePlannerNode::createStopObstacle(
 }
 
 std::optional<SlowDownObstacle> ObstacleCruisePlannerNode::createSlowDownObstacle(
-  const std::vector<TrajectoryPoint> & traj_points, const Obstacle & obstacle,
-  const double precise_lat_dist)
+  const Odometry & odometry, const std::vector<TrajectoryPoint> & traj_points,
+  const Obstacle & obstacle, const double precise_lat_dist)
 {
   const auto & object_id = obstacle.uuid.substr(0, 4);
   const auto & p = behavior_determination_param_;
@@ -1286,7 +1292,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_sub_.getData().pose.pose,
+    traj_points, vehicle_info_, odometry.pose.pose,
     p.max_lat_margin_for_slow_down + p.lat_hysteresis_margin_for_slow_down);
 
   std::vector<Polygon2d> front_collision_polygons;
@@ -1446,11 +1452,11 @@ bool ObstacleCruisePlannerNode::isObstacleCrossing(
 }
 
 double ObstacleCruisePlannerNode::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
 {
-  const auto & ego_pose = ego_odom_sub_.getData().pose.pose;
-  const double ego_vel = ego_odom_sub_.getData().twist.twist.linear.x;
+  const auto & ego_pose = odometry.pose.pose;
+  const double ego_vel = odometry.twist.twist.linear.x;
 
   const double time_to_reach_collision_point = [&]() {
     const double abs_ego_offset = is_driving_forward
@@ -1477,14 +1483,15 @@ double ObstacleCruisePlannerNode::calcCollisionTimeMargin(
 }
 
 PlannerData ObstacleCruisePlannerNode::createPlannerData(
+  const Odometry & odometry, const AccelWithCovarianceStamped & acc,
   const std::vector<TrajectoryPoint> & traj_points) const
 {
   PlannerData planner_data;
   planner_data.current_time = now();
   planner_data.traj_points = traj_points;
-  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.ego_pose = odometry.pose.pose;
+  planner_data.ego_vel = odometry.twist.twist.linear.x;
+  planner_data.ego_acc = acc.accel.accel.linear.x;
   planner_data.is_driving_forward = is_driving_forward_;
   return planner_data;
 }