replace TargetObject with ObjectInfoManager

Signed-off-by: Mamoru Sobue <[email protected]>

planning/behavior_velocity_intersection_module/src/object_manager.cpp

@@ -16,9 +16,13 @@
 
 #include <lanelet2_extension/utility/utilities.hpp>
 #include <tier4_autoware_utils/geometry/boost_geometry.hpp>
+#include <tier4_autoware_utils/geometry/boost_polygon_utils.hpp>  // for toPolygon2d
 
+#include <boost/geometry/algorithms/convex_hull.hpp>
+#include <boost/geometry/algorithms/correct.hpp>
 #include <boost/geometry/algorithms/intersection.hpp>
 
+#include <lanelet2_core/geometry/Lanelet.h>
 #include <lanelet2_core/geometry/LineString.h>
 
 #include <vector>
@@ -33,6 +37,31 @@ std::string to_string(const unique_identifier_msgs::msg::UUID & uuid)
   }
   return ss.str();
 }
+
+tier4_autoware_utils::Polygon2d createOneStepPolygon(
+  const geometry_msgs::msg::Pose & prev_pose, const geometry_msgs::msg::Pose & next_pose,
+  const autoware_auto_perception_msgs::msg::Shape & shape)
+{
+  namespace bg = boost::geometry;
+  const auto prev_poly = tier4_autoware_utils::toPolygon2d(prev_pose, shape);
+  const auto next_poly = tier4_autoware_utils::toPolygon2d(next_pose, shape);
+
+  tier4_autoware_utils::Polygon2d one_step_poly;
+  for (const auto & point : prev_poly.outer()) {
+    one_step_poly.outer().push_back(point);
+  }
+  for (const auto & point : next_poly.outer()) {
+    one_step_poly.outer().push_back(point);
+  }
+
+  bg::correct(one_step_poly);
+
+  tier4_autoware_utils::Polygon2d convex_one_step_poly;
+  bg::convex_hull(one_step_poly, convex_one_step_poly);
+
+  return convex_one_step_poly;
+}
+
 }  // namespace
 
 namespace behavior_velocity_planner::intersection
@@ -134,9 +163,18 @@ bool ObjectInfo::can_stop_before_ego_lane(
   return object_to_ego_path > tolerable_overshoot;
 }
 
+bool ObjectInfo::before_stopline_by(const double margin) const
+{
+  if (!dist_to_stopline_opt) {
+    return false;
+  }
+  const double dist_to_stopline = dist_to_stopline_opt.value();
+  return dist_to_stopline < margin;
+}
+
 std::shared_ptr<ObjectInfo> ObjectInfoManager::registerObject(
   const unique_identifier_msgs::msg::UUID & uuid, const bool belong_attention_area,
-  const bool belong_intersection_area)
+  const bool belong_intersection_area, const bool is_parked_vehicle)
 {
   if (objects_info_.count(uuid) == 0) {
     auto object = std::make_shared<intersection::ObjectInfo>(uuid);
@@ -148,7 +186,88 @@ std::shared_ptr<ObjectInfo> ObjectInfoManager::registerObject(
   } else if (belong_intersection_area) {
     intersection_area_objects_.push_back(object);
   }
+  if (is_parked_vehicle) {
+    parked_objects_.push_back(object);
+  }
   return object;
 }
 
+void ObjectInfoManager::registerExistingObject(
+  const unique_identifier_msgs::msg::UUID & uuid, const bool belong_attention_area,
+  const bool belong_intersection_area, const bool is_parked_vehicle,
+  std::shared_ptr<intersection::ObjectInfo> object)
+{
+  objects_info_[uuid] = object;
+  if (belong_attention_area) {
+    attention_area_objects_.push_back(object);
+  } else if (belong_intersection_area) {
+    intersection_area_objects_.push_back(object);
+  }
+  if (is_parked_vehicle) {
+    parked_objects_.push_back(object);
+  }
+}
+
+std::optional<intersection::CollisionInterval> findPassageInterval(
+  const autoware_auto_perception_msgs::msg::PredictedPath & predicted_path,
+  const autoware_auto_perception_msgs::msg::Shape & shape,
+  const lanelet::BasicPolygon2d & ego_lane_poly,
+  const std::optional<lanelet::ConstLanelet> & first_attention_lane_opt,
+  const std::optional<lanelet::ConstLanelet> & second_attention_lane_opt)
+{
+  const auto first_itr = std::adjacent_find(
+    predicted_path.path.cbegin(), predicted_path.path.cend(), [&](const auto & a, const auto & b) {
+      return bg::intersects(ego_lane_poly, ::createOneStepPolygon(a, b, shape));
+    });
+  if (first_itr == predicted_path.path.cend()) {
+    // even the predicted path end does not collide with the beginning of ego_lane_poly
+    return std::nullopt;
+  }
+  const auto last_itr = std::adjacent_find(
+    predicted_path.path.crbegin(), predicted_path.path.crend(),
+    [&](const auto & a, const auto & b) {
+      return bg::intersects(ego_lane_poly, ::createOneStepPolygon(a, b, shape));
+    });
+  if (last_itr == predicted_path.path.crend()) {
+    // even the predicted path start does not collide with the end of ego_lane_poly
+    return std::nullopt;
+  }
+
+  const size_t enter_idx = static_cast<size_t>(first_itr - predicted_path.path.begin());
+  const double object_enter_time =
+    static_cast<double>(enter_idx) * rclcpp::Duration(predicted_path.time_step).seconds();
+  const size_t exit_idx = static_cast<size_t>(last_itr.base() - predicted_path.path.begin());
+  const double object_exit_time =
+    static_cast<double>(exit_idx) * rclcpp::Duration(predicted_path.time_step).seconds();
+  const auto [lane_position, lane_id] =
+    [&]() -> std::pair<intersection::CollisionInterval::LanePosition, lanelet::Id> {
+    if (second_attention_lane_opt) {
+      if (lanelet::geometry::inside(
+            second_attention_lane_opt.value(),
+            lanelet::BasicPoint2d(first_itr->position.x, first_itr->position.y))) {
+        return std::make_pair(
+          intersection::CollisionInterval::LanePosition::SECOND,
+          second_attention_lane_opt.value().id());
+      }
+    }
+    if (first_attention_lane_opt) {
+      if (lanelet::geometry::inside(
+            first_attention_lane_opt.value(),
+            lanelet::BasicPoint2d(first_itr->position.x, first_itr->position.y))) {
+        return std::make_pair(
+          intersection::CollisionInterval::LanePosition::FIRST,
+          first_attention_lane_opt.value().id());
+      }
+    }
+    return std::make_pair(intersection::CollisionInterval::LanePosition::ELSE, lanelet::InvalId);
+  }();
+
+  std::vector<geometry_msgs::msg::Pose> path;
+  for (const auto & pose : predicted_path.path) {
+    path.push_back(pose);
+  }
+  return intersection::CollisionInterval{
+    lane_position, lane_id, path, {enter_idx, exit_idx}, {object_enter_time, object_exit_time}};
+}
+
 }  // namespace behavior_velocity_planner::intersection

planning/behavior_velocity_intersection_module/src/object_manager.hpp

@@ -134,6 +134,11 @@ class ObjectInfo
     const double brake_deceleration, const double tolerable_overshoot,
     lanelet::ConstLanelet ego_lane) const;
 
+  /**
+   * @brief check if the object is before the stopline within the specified margin
+   */
+  bool before_stopline_by(const double margin) const;
+
 private:
   const std::string uuid_str;
   autoware_auto_perception_msgs::msg::PredictedObject predicted_object_;
@@ -165,30 +170,62 @@ class ObjectInfoManager
 public:
   std::shared_ptr<ObjectInfo> registerObject(
     const unique_identifier_msgs::msg::UUID & uuid, const bool belong_attention_area,
-    const bool belong_intersection_area);
-  void clearAreaObjects()
+    const bool belong_intersection_area, const bool is_parked);
+
+  void registerExistingObject(
+    const unique_identifier_msgs::msg::UUID & uuid, const bool belong_attention_area,
+    const bool belong_intersection_area, const bool is_parked,
+    std::shared_ptr<intersection::ObjectInfo> object);
+
+  void clearObjects()
   {
+    objects_info_.clear();
     attention_area_objects_.clear();
     intersection_area_objects_.clear();
+    parked_objects_.clear();
   };
+
   const std::vector<std::shared_ptr<ObjectInfo>> & attentionObjects() const
   {
     return attention_area_objects_;
   }
+
+  const std::vector<std::shared_ptr<ObjectInfo>> & parkedObjects() const { return parked_objects_; }
+
   std::vector<std::shared_ptr<ObjectInfo>> allObjects()
   {
     std::vector<std::shared_ptr<ObjectInfo>> all_objects = attention_area_objects_;
     all_objects.insert(
       all_objects.end(), intersection_area_objects_.begin(), intersection_area_objects_.end());
+    all_objects.insert(all_objects.end(), parked_objects_.begin(), parked_objects_.end());
     return all_objects;
   }
 
+  const std::unordered_map<unique_identifier_msgs::msg::UUID, std::shared_ptr<ObjectInfo>> &
+  getObjectsMap()
+  {
+    return objects_info_;
+  }
+
 private:
   std::unordered_map<unique_identifier_msgs::msg::UUID, std::shared_ptr<ObjectInfo>> objects_info_;
   std::vector<std::shared_ptr<ObjectInfo>> attention_area_objects_;  //! belong to attention area
   std::vector<std::shared_ptr<ObjectInfo>>
     intersection_area_objects_;  //! does not belong to attention area but to intersection area
+  std::vector<std::shared_ptr<ObjectInfo>>
+    parked_objects_;  //! parked objects on attention_area/intersection_area
 };
+
+/**
+ * @brief return the CollisionKnowledge struct if the predicted path collides ego path spatially
+ */
+std::optional<intersection::CollisionInterval> findPassageInterval(
+  const autoware_auto_perception_msgs::msg::PredictedPath & predicted_path,
+  const autoware_auto_perception_msgs::msg::Shape & shape,
+  const lanelet::BasicPolygon2d & ego_lane_poly,
+  const std::optional<lanelet::ConstLanelet> & first_attention_lane_opt,
+  const std::optional<lanelet::ConstLanelet> & second_attention_lane_opt);
+
 }  // namespace behavior_velocity_planner::intersection
 
 #endif  // OBJECT_MANAGER_HPP_