Fixes in unstructured constellation polygon creation

ad_rss/generated/CMakeLists.txt

@@ -14,7 +14,7 @@
 ##
 
 cmake_minimum_required(VERSION 3.5)
-project(ad_rss VERSION 4.3.0)
+project(ad_rss VERSION 4.4.0)
 
 include(GNUInstallDirs)
 include(CMakePackageConfigHelpers)

ad_rss/generated/include/ad/rss/world/UnstructuredSettings.hpp

@@ -106,11 +106,21 @@ struct UnstructuredSettings
     return (pedestrianTurningRadius == other.pedestrianTurningRadius) && (driveAwayMaxAngle == other.driveAwayMaxAngle)
       && (vehicleYawRateChange == other.vehicleYawRateChange) && (vehicleMinRadius == other.vehicleMinRadius)
       && (vehicleTrajectoryCalculationStep == other.vehicleTrajectoryCalculationStep)
-      && (vehicleFrontIntermediateRatioSteps == other.vehicleFrontIntermediateRatioSteps)
-      && (vehicleBackIntermediateRatioSteps == other.vehicleBackIntermediateRatioSteps)
+      && (vehicleFrontIntermediateYawRateChangeRatioSteps == other.vehicleFrontIntermediateYawRateChangeRatioSteps)
+      && (vehicleBackIntermediateYawRateChangeRatioSteps == other.vehicleBackIntermediateYawRateChangeRatioSteps)
       && (vehicleBrakeIntermediateAccelerationSteps == other.vehicleBrakeIntermediateAccelerationSteps)
       && (vehicleContinueForwardIntermediateAccelerationSteps
-          == other.vehicleContinueForwardIntermediateAccelerationSteps);
+          == other.vehicleContinueForwardIntermediateAccelerationSteps)
+      && (vehicleContinueForwardIntermediateYawRateChangeRatioSteps
+          == other.vehicleContinueForwardIntermediateYawRateChangeRatioSteps)
+      && (pedestrianContinueForwardIntermediateHeadingChangeRatioSteps
+          == other.pedestrianContinueForwardIntermediateHeadingChangeRatioSteps)
+      && (pedestrianContinueForwardIntermediateAccelerationSteps
+          == other.pedestrianContinueForwardIntermediateAccelerationSteps)
+      && (pedestrianBrakeIntermediateAccelerationSteps == other.pedestrianBrakeIntermediateAccelerationSteps)
+      && (pedestrianFrontIntermediateHeadingChangeRatioSteps
+          == other.pedestrianFrontIntermediateHeadingChangeRatioSteps)
+      && (pedestrianBackIntermediateHeadingChangeRatioSteps == other.pedestrianBackIntermediateHeadingChangeRatioSteps);
   }
 
   /**
@@ -164,29 +174,63 @@ struct UnstructuredSettings
    * specifying the steps on one side, therefore the resulting intermedate steps are twice this value. This value is
    * used for the front of the trajectory set.
    */
-  uint32_t vehicleFrontIntermediateRatioSteps{0};
+  uint32_t vehicleFrontIntermediateYawRateChangeRatioSteps{0};
 
   /*!
    * During calculation of the trajectory set, multiple yaw rate ratios are used. The default is max left, max right and
    * no yaw rate change. By specifying a value larger than zero more intermediate steps are used. The value is
    * specifying the steps on one side, therefore the resulting intermedate steps are twice this value. This value is
    * used for the back of the trajectory set.
    */
-  uint32_t vehicleBackIntermediateRatioSteps{0};
+  uint32_t vehicleBackIntermediateYawRateChangeRatioSteps{0};
 
   /*!
    * Specifies the intermediate acceleration steps (between brakeMax and brakeMin) used while calculating the cbrake
-   * trajectory set. This is applied to all vehicleResponseIntermediateAccelerationSteps, therefore it has only an
-   * effect if that value is >0.
+   * trajectory set.
    */
   uint32_t vehicleBrakeIntermediateAccelerationSteps{0};
 
   /*!
    * Specifies the intermediate acceleration steps (between brakeMin and accelMax) used while calculating the continue
-   * forward trajectory set. This is applied to all vehicleResponseIntermediateAccelerationSteps, therefore it has only
-   * an effect if that value is >0.
+   * forward trajectory set.
    */
   uint32_t vehicleContinueForwardIntermediateAccelerationSteps{0};
+
+  /*!
+   * Specifies the intermediate yaw rate change ratio steps used while calculating the continue forward trajectory set.
+   */
+  uint32_t vehicleContinueForwardIntermediateYawRateChangeRatioSteps{0};
+
+  /*!
+   * Specifies the intermediate heading change ratio steps used while calculating the continue forward trajectory set.
+   */
+  uint32_t pedestrianContinueForwardIntermediateHeadingChangeRatioSteps{0};
+
+  /*!
+   * Specifies the intermediate steps used while calculating the continue forward trajectory set.
+   */
+  uint32_t pedestrianContinueForwardIntermediateAccelerationSteps{0};
+
+  /*!
+   * Specifies the intermediate steps used while calculating the brake trajectory set.
+   */
+  uint32_t pedestrianBrakeIntermediateAccelerationSteps{0};
+
+  /*!
+   * During calculation of the trajectory set, multiple heading change ratios are used. The default is max left, max
+   * right and no heading change. By specifying a value larger than zero more intermediate steps are used. The value is
+   * specifying the steps on one side, therefore the resulting intermedate steps are twice this value. This value is
+   * used for the front of the trajectory set.
+   */
+  uint32_t pedestrianFrontIntermediateHeadingChangeRatioSteps{0};
+
+  /*!
+   * During calculation of the trajectory set, multiple heading change ratios are used. The default is max left, max
+   * right and no yaw rate change. By specifying a value larger than zero more intermediate steps are used. The value is
+   * specifying the steps on one side, therefore the resulting intermedate steps are twice this value. This value is
+   * used for the back of the trajectory set.
+   */
+  uint32_t pedestrianBackIntermediateHeadingChangeRatioSteps{0};
 };
 
 } // namespace world
@@ -238,17 +282,35 @@ inline std::ostream &operator<<(std::ostream &os, UnstructuredSettings const &_v
   os << "vehicleTrajectoryCalculationStep:";
   os << _value.vehicleTrajectoryCalculationStep;
   os << ",";
-  os << "vehicleFrontIntermediateRatioSteps:";
-  os << _value.vehicleFrontIntermediateRatioSteps;
+  os << "vehicleFrontIntermediateYawRateChangeRatioSteps:";
+  os << _value.vehicleFrontIntermediateYawRateChangeRatioSteps;
   os << ",";
-  os << "vehicleBackIntermediateRatioSteps:";
-  os << _value.vehicleBackIntermediateRatioSteps;
+  os << "vehicleBackIntermediateYawRateChangeRatioSteps:";
+  os << _value.vehicleBackIntermediateYawRateChangeRatioSteps;
   os << ",";
   os << "vehicleBrakeIntermediateAccelerationSteps:";
   os << _value.vehicleBrakeIntermediateAccelerationSteps;
   os << ",";
   os << "vehicleContinueForwardIntermediateAccelerationSteps:";
   os << _value.vehicleContinueForwardIntermediateAccelerationSteps;
+  os << ",";
+  os << "vehicleContinueForwardIntermediateYawRateChangeRatioSteps:";
+  os << _value.vehicleContinueForwardIntermediateYawRateChangeRatioSteps;
+  os << ",";
+  os << "pedestrianContinueForwardIntermediateHeadingChangeRatioSteps:";
+  os << _value.pedestrianContinueForwardIntermediateHeadingChangeRatioSteps;
+  os << ",";
+  os << "pedestrianContinueForwardIntermediateAccelerationSteps:";
+  os << _value.pedestrianContinueForwardIntermediateAccelerationSteps;
+  os << ",";
+  os << "pedestrianBrakeIntermediateAccelerationSteps:";
+  os << _value.pedestrianBrakeIntermediateAccelerationSteps;
+  os << ",";
+  os << "pedestrianFrontIntermediateHeadingChangeRatioSteps:";
+  os << _value.pedestrianFrontIntermediateHeadingChangeRatioSteps;
+  os << ",";
+  os << "pedestrianBackIntermediateHeadingChangeRatioSteps:";
+  os << _value.pedestrianBackIntermediateHeadingChangeRatioSteps;
   os << ")";
   return os;
 }

ad_rss/generated/include/ad_rss/Version.hpp

@@ -25,7 +25,7 @@
 /*!
  * The minor version of ad_rss
  */
-#define AD_RSS_VERSION_MINOR 3
+#define AD_RSS_VERSION_MINOR 4
 
 /*!
  * The revision of ad_rss
@@ -35,4 +35,4 @@
 /*!
  * The version of ad_rss as string
  */
-#define AD_RSS_VERSION_STRING "4.3.0"
+#define AD_RSS_VERSION_STRING "4.4.0"

ad_rss/impl/src/unstructured/Geometry.cpp

@@ -188,9 +188,42 @@ bool combinePolygon(Polygon const &a, Polygon const &b, Polygon &result)
     boost::geometry::union_(a.outer(), b.outer(), unionPolygons);
     if (unionPolygons.size() != 1)
     {
-      spdlog::debug("Could not calculate combined polygon. Expected 1 polygon after union, found {}",
-                    unionPolygons.size());
-      return false;
+      auto fixed = false;
+      if (unionPolygons.size() == 0)
+      {
+        // if union reports zero polygons, it might happen that one polygon is covered by the other
+        auto aCoveredByB = true;
+        for (auto it = a.outer().cbegin(); (it != a.outer().cend()) && aCoveredByB; ++it)
+        {
+          aCoveredByB &= boost::geometry::covered_by(*it, b.outer());
+        }
+        if (aCoveredByB)
+        {
+          fixed = true;
+          result = b;
+        }
+        if (!fixed)
+        {
+          auto bCoveredByA = true;
+          for (auto it = b.outer().cbegin(); (it != b.outer().cend()) && bCoveredByA; ++it)
+          {
+            bCoveredByA &= boost::geometry::covered_by(*it, a.outer());
+          }
+          if (bCoveredByA)
+          {
+            fixed = true;
+            result = a;
+          }
+        }
+      }
+      if (!fixed)
+      {
+        spdlog::warn("Could not calculate combined polygon. Expected 1 polygon after union, found {}. A:\n{}\nB:\n{}",
+                     unionPolygons.size(),
+                     std::to_string(a),
+                     std::to_string(b));
+        return false;
+      }
     }
     else
     {

ad_rss/impl/src/unstructured/TrajectoryCommon.cpp

@@ -12,6 +12,7 @@
 
 #include "TrajectoryCommon.hpp"
 #include <ad/physics/Operation.hpp>
+#include "ad/rss/unstructured/DebugDrawing.hpp"
 
 /*!
  * @brief namespace ad
@@ -57,6 +58,180 @@ Point getVehicleCorner(TrajectoryPoint const &point,
   return resultPoint;
 }
 
+bool calculateStepPolygon(situation::VehicleState const &vehicleState,
+                          TrajectorySetStep const &step,
+                          std::string const &debugNamespace,
+                          Polygon &polygon,
+                          TrajectorySetStepVehicleLocation &stepVehicleLocation)
+{
+  MultiPoint frontPtsLeft;
+  MultiPoint frontPtsRight;
+
+  int idx = 0;
+  for (auto it = step.left.cbegin(); it != step.left.cend(); ++it)
+  {
+    auto vehicleLocationLeft = TrafficParticipantLocation(*it, vehicleState);
+    DEBUG_DRAWING_POLYGON(vehicleLocationLeft.toPolygon(), "blue", debugNamespace + "_left_" + std::to_string(idx));
+    boost::geometry::append(frontPtsLeft, vehicleLocationLeft.toMultiPoint());
+    if (it == step.left.end() - 1)
+    {
+      stepVehicleLocation.left = vehicleLocationLeft;
+    }
+    ++idx;
+  }
+
+  // center
+  auto vehicleLocationCenter = TrafficParticipantLocation(step.center, vehicleState);
+  DEBUG_DRAWING_POLYGON(vehicleLocationCenter.toPolygon(), "blue", debugNamespace + "_center");
+  boost::geometry::append(frontPtsLeft, vehicleLocationCenter.toMultiPoint());
+  stepVehicleLocation.center = vehicleLocationCenter;
+  boost::geometry::append(frontPtsRight, vehicleLocationCenter.toMultiPoint());
+
+  idx = 0;
+  for (auto it = step.right.cbegin(); it != step.right.cend(); ++it)
+  {
+    auto vehicleLocationRight = TrafficParticipantLocation(*it, vehicleState);
+    DEBUG_DRAWING_POLYGON(vehicleLocationRight.toPolygon(), "blue", debugNamespace + "_right_" + std::to_string(idx));
+    boost::geometry::append(frontPtsRight, vehicleLocationRight.toMultiPoint());
+    if (it == step.right.begin())
+    {
+      stepVehicleLocation.right = vehicleLocationRight;
+    }
+    ++idx;
+  }
+
+  Polygon hullLeft;
+  Polygon hullRight;
+  boost::geometry::convex_hull(frontPtsLeft, hullLeft);
+  boost::geometry::convex_hull(frontPtsRight, hullRight);
+  auto result = combinePolygon(hullLeft, hullRight, polygon);
+  return result;
+}
+
+bool calculateEstimationBetweenSteps(Polygon &polygon,
+                                     TrajectorySetStepVehicleLocation const &previousStepVehicleLocation,
+                                     TrajectorySetStepVehicleLocation const &currentStepVehicleLocation,
+                                     std::string const &debugNamespace)
+{
+  // Fill potential gap between two calculation steps by using the previous and current step
+  if (previousStepVehicleLocation == currentStepVehicleLocation)
+  {
+    return true;
+  }
+
+  Polygon hull;
+  MultiPoint interimPtsLeft;
+  boost::geometry::append(interimPtsLeft, previousStepVehicleLocation.left.toMultiPoint());
+  boost::geometry::append(interimPtsLeft, previousStepVehicleLocation.center.toMultiPoint());
+  boost::geometry::append(interimPtsLeft, currentStepVehicleLocation.left.toMultiPoint());
+  boost::geometry::append(interimPtsLeft, currentStepVehicleLocation.center.toMultiPoint());
+  Polygon hullLeft;
+  boost::geometry::convex_hull(interimPtsLeft, hullLeft);
+
+  MultiPoint interimPtsRight;
+  boost::geometry::append(interimPtsRight, previousStepVehicleLocation.right.toMultiPoint());
+  boost::geometry::append(interimPtsRight, previousStepVehicleLocation.center.toMultiPoint());
+  boost::geometry::append(interimPtsRight, currentStepVehicleLocation.right.toMultiPoint());
+  boost::geometry::append(interimPtsRight, currentStepVehicleLocation.center.toMultiPoint());
+  Polygon hullRight;
+  boost::geometry::convex_hull(interimPtsRight, hullRight);
+  auto result = combinePolygon(hullRight, hullLeft, hull);
+  if (!result)
+  {
+    spdlog::debug("unstructured::calculateEstimationBetweenSteps>> Could not create estimation polygon ({})."
+                  "left {}, right {}",
+                  debugNamespace,
+                  std::to_string(hullLeft),
+                  std::to_string(hullRight));
+  }
+
+  if (result)
+  {
+    DEBUG_DRAWING_POLYGON(hull, "yellow", debugNamespace + "_hull_front");
+    result = combinePolygon(polygon, hull, polygon);
+    if (!result)
+    {
+      spdlog::warn("unstructured::calculateEstimationBetweenSteps>> Could not combine front estimation polygon ({}) "
+                   "with final polygon."
+                   "polygon {}, hull {}",
+                   debugNamespace,
+                   std::to_string(polygon),
+                   std::to_string(hull));
+    }
+  }
+  return result;
+}
+
+bool calculateFrontAndSidePolygon(situation::VehicleState const &vehicleState,
+                                  TrajectorySetStepVehicleLocation const &initialStepVehicleLocation,
+                                  std::vector<TrajectorySetStep> const &sideSteps,
+                                  TrajectorySetStep const &front,
+                                  std::string const &debugNamespace,
+                                  Polygon &resultPolygon,
+                                  TrajectorySetStepVehicleLocation &frontSideStepVehicleLocation)
+{
+  auto result = true;
+  auto previousStepVehicleLocation = initialStepVehicleLocation;
+  //-------------
+  // sides
+  //-------------
+  auto idx = 0;
+  for (auto sideStepIt = sideSteps.cbegin(); (sideStepIt != sideSteps.cend()) && result; ++sideStepIt)
+  {
+    Polygon stepPolygon;
+    TrajectorySetStepVehicleLocation currentStepVehicleLocation;
+    result = calculateStepPolygon(
+      vehicleState, *sideStepIt, debugNamespace + "_" + std::to_string(idx), stepPolygon, currentStepVehicleLocation);
+    if (!result)
+    {
+      spdlog::debug("unstructured::calculateFrontAndSidePolygon>> Could not calculate step polygon");
+    }
+    if (result)
+    {
+      result = calculateEstimationBetweenSteps(resultPolygon,
+                                               previousStepVehicleLocation,
+                                               currentStepVehicleLocation,
+                                               debugNamespace + "_step_estimation_" + std::to_string(idx));
+      if (!result)
+      {
+        spdlog::debug("unstructured::calculateFrontAndSidePolygon>> Could not calculate between steps");
+      }
+      previousStepVehicleLocation = currentStepVehicleLocation;
+    }
+
+    if (result)
+    {
+      result = combinePolygon(resultPolygon, stepPolygon, resultPolygon);
+    }
+    idx++;
+  }
+
+  //-------------
+  // front
+  //-------------
+  Polygon frontPolygon;
+  if (result)
+  {
+    result = calculateStepPolygon(
+      vehicleState, front, debugNamespace + "_front", frontPolygon, frontSideStepVehicleLocation);
+  }
+  if (result)
+  {
+    result = calculateEstimationBetweenSteps(
+      resultPolygon, previousStepVehicleLocation, frontSideStepVehicleLocation, debugNamespace + "_front_estimation");
+    if (!result)
+    {
+      spdlog::debug("unstructured::calculateFrontAndSidePolygon>> Could not calculate between last step and "
+                    "front polygon.");
+    }
+  }
+  if (result)
+  {
+    result = combinePolygon(resultPolygon, frontPolygon, resultPolygon);
+  }
+  return result;
+}
+
 } // namespace unstructured
 } // namespace rss
 } // namespace ad