feat(autoware_behavior_velocity_run_out_module): check collision with polygon type

Signed-off-by: jkoronczok <jkoronczok@autonomous-systems.pl>

Author: jakor97

planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/README.md

@@ -244,6 +244,5 @@ You can choose whether to use this feature by parameter of `slow_down_limit.enab
 ### Future extensions / Unimplemented parts
 
 - Calculate obstacle's min velocity and max velocity from covariance
-- Detect collisions with polygon object
 - Handle the case when the predicted path of obstacles are not straight line
   - Currently collision check is calculated based on the assumption that the predicted path of the obstacle is a straight line

planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.cpp

@@ -24,6 +24,7 @@
 #include <autoware/universe_utils/geometry/geometry.hpp>
 #include <autoware/universe_utils/ros/uuid_helper.hpp>
 
+#include <boost/geometry/algorithms/append.hpp>
 #include <boost/geometry/algorithms/intersection.hpp>
 #include <boost/geometry/algorithms/within.hpp>
 
@@ -517,7 +518,8 @@ bool RunOutModule::checkCollisionWithShape(
       break;
 
     case Shape::POLYGON:
-      collision_detected = checkCollisionWithPolygon();
+      collision_detected =
+        checkCollisionWithPolygon(vehicle_polygon, pose_with_range, shape, collision_points);
       break;
 
     default:
@@ -640,11 +642,60 @@ bool RunOutModule::checkCollisionWithBoundingBox(
   return true;
 }
 
-bool RunOutModule::checkCollisionWithPolygon() const
+Polygon2d RunOutModule::createPolygonForRangedPoints(
+  const PoseWithRange & pose_with_range, const Shape & shape) const
 {
-  RCLCPP_WARN_STREAM(logger_, "detection for POLYGON type is not implemented yet.");
+  // Convert the obstacle's pose to a min and max polygon
+  const auto pose_min_polygon =
+    autoware::universe_utils::toPolygon2d(pose_with_range.pose_min, shape);
+  const auto pose_max_polygon =
+    autoware::universe_utils::toPolygon2d(pose_with_range.pose_max, shape);
+
+  // Compute the convex hull of the min and max polygons
+  auto ranged_polygon = pose_min_polygon;
+  bg::append(ranged_polygon.outer(), pose_max_polygon.outer());
+  Polygon2d convex_polygon;
+  bg::convex_hull(ranged_polygon, convex_polygon);
+
+  return convex_polygon;
+}
+
+bool RunOutModule::checkCollisionWithPolygon(
+  const Polygon2d & vehicle_polygon, const PoseWithRange pose_with_range, const Shape & shape,
+  std::vector<geometry_msgs::msg::Point> & collision_points) const
+{
+  const auto bg_ranged_polygon = createPolygonForRangedPoints(pose_with_range, shape);
+
+  if (bg_ranged_polygon.outer().empty()) {
+    RCLCPP_WARN_STREAM_THROTTLE(logger_, *clock_, 1000, "failed to check collision with polygon");
+    return false;
+  }
+
+  std::vector<geometry_msgs::msg::Point> ranged_polygon;
+  for (const auto & point : bg_ranged_polygon.outer()) {
+    const auto p_msg = autoware::universe_utils::createPoint(
+      point.x(), point.y(), pose_with_range.pose_min.position.z);
+    ranged_polygon.emplace_back(p_msg);
+  }
+
+  // Check for collision between the vehicle and the obstacle polygon
+  std::vector<Point2d> collision_points_bg;
+  bg::intersection(vehicle_polygon, bg_ranged_polygon, collision_points_bg);
 
-  return false;
+  debug_ptr_->pushPredictedObstaclePolygons(ranged_polygon);
+
+  // No collision detected
+  if (collision_points_bg.empty()) {
+    return false;
+  }
+
+  // Collision detected
+  for (const auto & p : collision_points_bg) {
+    const auto p_msg =
+      autoware::universe_utils::createPoint(p.x(), p.y(), pose_with_range.pose_min.position.z);
+    collision_points.emplace_back(p_msg);
+  }
+  return true;
 }
 
 std::optional<geometry_msgs::msg::Pose> RunOutModule::calcStopPoint(

planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.hpp

@@ -112,11 +112,18 @@ class RunOutModule : public SceneModuleInterface
     const geometry_msgs::msg::Vector3 & dimension,
     std::vector<geometry_msgs::msg::Point> & collision_points) const;
 
-  bool checkCollisionWithPolygon() const;
+  bool checkCollisionWithPolygon(
+    const Polygon2d & vehicle_polygon,
+    const PoseWithRange pose_with_range,
+    const Shape & shape,
+    std::vector<geometry_msgs::msg::Point> & collision_points) const;
 
   std::vector<geometry_msgs::msg::Point> createBoundingBoxForRangedPoints(
     const PoseWithRange & pose_with_range, const float x_offset, const float y_offset) const;
 
+  Polygon2d createPolygonForRangedPoints(
+    const PoseWithRange & pose_with_range, const Shape & shape) const;
+
   std::optional<geometry_msgs::msg::Pose> calcStopPoint(
     const std::optional<DynamicObstacle> & dynamic_obstacle, const PathWithLaneId & path,
     const geometry_msgs::msg::Pose & current_pose, const float current_vel,