feat: check pointcloud within detection area

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

Author: satoshi-ota

planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/object_manager.cpp

@@ -79,13 +79,27 @@ void ObjectInfo::initialize(
   std::optional<lanelet::ConstLanelet> attention_lanelet_opt,
   std::optional<lanelet::ConstLineString3d> stopline_opt)
 {
+  type = ObstacleType::OBJECT;
   predicted_object_ = object;
   attention_lanelet_opt_ = attention_lanelet_opt;
   stopline_opt_ = stopline_opt;
   unsafe_interval_ = std::nullopt;
   calc_dist_to_stopline();
 }
 
+void ObjectInfo::initialize(
+  const geometry_msgs::msg::Point & point,
+  std::optional<lanelet::ConstLanelet> attention_lanelet_opt,
+  std::optional<lanelet::ConstLineString3d> stopline_opt)
+{
+  type = ObstacleType::POINTCLOUD;
+  point_ = point;
+  attention_lanelet_opt_ = attention_lanelet_opt;
+  stopline_opt_ = stopline_opt;
+  unsafe_interval_ = std::nullopt;
+  calc_dist_to_stopline();
+}
+
 void ObjectInfo::update_safety(
   const std::optional<CollisionInterval> & unsafe_interval,
   const std::optional<CollisionInterval> & safe_interval, const bool safe_under_traffic_control)

planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/object_manager.hpp

@@ -130,6 +130,11 @@ class ObjectInfo
     std::optional<lanelet::ConstLanelet> attention_lanelet_opt,
     std::optional<lanelet::ConstLineString3d> stopline_opt);
 
+  void initialize(
+    const geometry_msgs::msg::Point & point,
+    std::optional<lanelet::ConstLanelet> attention_lanelet_opt,
+    std::optional<lanelet::ConstLineString3d> stopline_opt);
+
   /**
    * @brief update unsafe_knowledge
    */
@@ -191,9 +196,17 @@ class ObjectInfo
 
   const std::string uuid_str;
 
+  enum ObstacleType {
+    OBJECT,
+    POINTCLOUD,
+  };
+  ObstacleType type;
+
 private:
   autoware_perception_msgs::msg::PredictedObject predicted_object_;
 
+  geometry_msgs::msg::Point point_;
+
   //! null if the object in intersection_area but not in attention_area
   std::optional<lanelet::ConstLanelet> attention_lanelet_opt_{std::nullopt};
 
@@ -265,6 +278,8 @@ class ObjectInfoManager
 private:
   std::unordered_map<unique_identifier_msgs::msg::UUID, std::shared_ptr<ObjectInfo>> objects_info_;
 
+  std::vector<std::shared_ptr<geometry_msgs::msg::Point>> obstacle_pointcloud_info_;
+
   //! belong to attention area
   std::vector<std::shared_ptr<ObjectInfo>> attention_area_objects_;
 

planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.hpp

@@ -806,6 +806,9 @@ class IntersectionModule : public SceneModuleInterfaceWithRTC
     const bool is_over_1st_pass_judge_line,
     const std::optional<bool> is_over_2nd_pass_judge_line) const;
 
+  std::optional<size_t> isWithinTargetLanelets(
+    const geometry_msgs::msg::Point & point, const lanelet::ConstLanelets & target_lanelets) const;
+
   std::optional<size_t> checkAngleForTargetLanelets(
     const geometry_msgs::msg::Pose & pose, const lanelet::ConstLanelets & target_lanelets,
     const bool is_parked_vehicle) const;