From 75f7e49112c435e8997311a81ec4136e8c353a9e Mon Sep 17 00:00:00 2001
From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com>
Date: Tue, 17 Oct 2023 12:06:21 +0900
Subject: [PATCH] feat(avoidance): add another envelope polygon update logic
 (#5323)

feat(avoidance): add other envelope polygon update logic

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>
---
 .../utils/avoidance/utils.hpp                 |  3 ++
 .../src/utils/avoidance/utils.cpp             | 34 +++++++++++++------
 2 files changed, 27 insertions(+), 10 deletions(-)

diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp
index ede0e5bfb728c..f17d07f3da30e 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp
@@ -79,6 +79,9 @@ void setStartData(
   AvoidLine & al, const double start_shift_length, const geometry_msgs::msg::Pose & start,
   const size_t start_idx, const double start_dist);
 
+Polygon2d createEnvelopePolygon(
+  const Polygon2d & object_polygon, const Pose & closest_pose, const double envelope_buffer);
+
 Polygon2d createEnvelopePolygon(
   const ObjectData & object_data, const Pose & closest_pose, const double envelope_buffer);
 
diff --git a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp
index 10766b7aec416..ef8dfbeb7fbab 100644
--- a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp
+++ b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp
@@ -328,15 +328,14 @@ void setStartData(
 }
 
 Polygon2d createEnvelopePolygon(
-  const ObjectData & object_data, const Pose & closest_pose, const double envelope_buffer)
+  const Polygon2d & object_polygon, const Pose & closest_pose, const double envelope_buffer)
 {
   namespace bg = boost::geometry;
+  using tier4_autoware_utils::expandPolygon;
   using tier4_autoware_utils::Point2d;
   using tier4_autoware_utils::Polygon2d;
   using Box = bg::model::box<Point2d>;
 
-  const auto object_polygon = tier4_autoware_utils::toPolygon2d(object_data.object);
-
   const auto toPolygon2d = [](const geometry_msgs::msg::Polygon & polygon) {
     Polygon2d ret{};
 
@@ -371,11 +370,17 @@ Polygon2d createEnvelopePolygon(
   tf2::doTransform(
     toMsg(envelope_poly, closest_pose.position.z), envelope_ros_polygon, geometry_tf);
 
-  const auto expanded_polygon =
-    tier4_autoware_utils::expandPolygon(toPolygon2d(envelope_ros_polygon), envelope_buffer);
+  const auto expanded_polygon = expandPolygon(toPolygon2d(envelope_ros_polygon), envelope_buffer);
   return expanded_polygon;
 }
 
+Polygon2d createEnvelopePolygon(
+  const ObjectData & object_data, const Pose & closest_pose, const double envelope_buffer)
+{
+  const auto object_polygon = tier4_autoware_utils::toPolygon2d(object_data.object);
+  return createEnvelopePolygon(object_polygon, closest_pose, envelope_buffer);
+}
+
 std::vector<DrivableAreaInfo::Obstacle> generateObstaclePolygonsForDrivableArea(
   const ObjectDataArray & objects, const std::shared_ptr<AvoidanceParameters> & parameters,
   const double vehicle_width)
@@ -530,8 +535,6 @@ void fillObjectEnvelopePolygon(
   ObjectData & object_data, const ObjectDataArray & registered_objects, const Pose & closest_pose,
   const std::shared_ptr<AvoidanceParameters> & parameters)
 {
-  using boost::geometry::within;
-
   const auto object_type = utils::getHighestProbLabel(object_data.object.classification);
   const auto object_parameter = parameters->object_parameters.at(object_type);
 
@@ -549,15 +552,26 @@ void fillObjectEnvelopePolygon(
     return;
   }
 
-  const auto object_polygon = tier4_autoware_utils::toPolygon2d(object_data.object);
+  const auto envelope_poly =
+    createEnvelopePolygon(object_data, closest_pose, envelope_buffer_margin);
+
+  if (boost::geometry::within(envelope_poly, same_id_obj->envelope_poly)) {
+    object_data.envelope_poly = same_id_obj->envelope_poly;
+    return;
+  }
 
-  if (!within(object_polygon, same_id_obj->envelope_poly)) {
+  std::vector<Polygon2d> unions;
+  boost::geometry::union_(envelope_poly, same_id_obj->envelope_poly, unions);
+
+  if (unions.empty()) {
     object_data.envelope_poly =
       createEnvelopePolygon(object_data, closest_pose, envelope_buffer_margin);
     return;
   }
 
-  object_data.envelope_poly = same_id_obj->envelope_poly;
+  boost::geometry::correct(unions.front());
+
+  object_data.envelope_poly = createEnvelopePolygon(unions.front(), closest_pose, 0.0);
 }
 
 void fillObjectMovingTime(