Fix unstructured vehicle curvature (#86)

* Update unstructured constellation polygon creation

Fix vehicle curvature calculation after response time, make DebugDrawing accessible from python

* Update version to 4.3.0

* Cleanup

Change-Id: I2b185f59b8545d150e8049534d2eeb0dbdc447b5

* Cleanup unit tests

Change-Id: I910c03f1d0aaa259a5c1d45529b9927386e0b8ca

Author: fpasch

ad_rss/generated/CMakeLists.txt

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

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

@@ -108,7 +108,6 @@ struct UnstructuredSettings
       && (vehicleTrajectoryCalculationStep == other.vehicleTrajectoryCalculationStep)
       && (vehicleFrontIntermediateRatioSteps == other.vehicleFrontIntermediateRatioSteps)
       && (vehicleBackIntermediateRatioSteps == other.vehicleBackIntermediateRatioSteps)
-      && (vehicleResponseTimeIntermediateAccelerationSteps == other.vehicleResponseTimeIntermediateAccelerationSteps)
       && (vehicleBrakeIntermediateAccelerationSteps == other.vehicleBrakeIntermediateAccelerationSteps)
       && (vehicleContinueForwardIntermediateAccelerationSteps
           == other.vehicleContinueForwardIntermediateAccelerationSteps);
@@ -175,12 +174,6 @@ struct UnstructuredSettings
    */
   uint32_t vehicleBackIntermediateRatioSteps{0};
 
-  /*!
-   * Specifies the intermediate acceleration steps (between brakeMax and accelMax) used while calculating the states at
-   * response time. These are later used for calculating the trajectory sets.
-   */
-  uint32_t vehicleResponseTimeIntermediateAccelerationSteps{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
@@ -251,9 +244,6 @@ inline std::ostream &operator<<(std::ostream &os, UnstructuredSettings const &_v
   os << "vehicleBackIntermediateRatioSteps:";
   os << _value.vehicleBackIntermediateRatioSteps;
   os << ",";
-  os << "vehicleResponseTimeIntermediateAccelerationSteps:";
-  os << _value.vehicleResponseTimeIntermediateAccelerationSteps;
-  os << ",";
   os << "vehicleBrakeIntermediateAccelerationSteps:";
   os << _value.vehicleBrakeIntermediateAccelerationSteps;
   os << ",";

ad_rss/generated/include/ad_rss/Version.hpp

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

ad_rss/impl/include/ad/rss/unstructured/DebugDrawing.hpp

@@ -18,6 +18,7 @@
 
 #define DEBUG_DRAWING_POLYGON(polygon, color, ns) (DebugDrawing::getInstance()->drawPolygon(polygon, color, ns))
 #define DEBUG_DRAWING_LINE(line, color, ns) (DebugDrawing::getInstance()->drawLine(line, color, ns))
+#define DEBUG_DRAWING_IS_ENABLED() (DebugDrawing::getInstance()->isEnabled())
 
 /*!
  * @brief namespace ad
@@ -35,6 +36,21 @@ namespace unstructured {
 class DebugDrawing
 {
 public:
+  struct NullDeleter
+  {
+    void operator()(const void *)
+    {
+    }
+  };
+  struct DebugPoint
+  {
+    DebugPoint(double inX, double inY)
+      : x(inX)
+      , y(inY){};
+    double x;
+    double y;
+  };
+
   struct DebugLine
   {
     DebugLine(Line const &iLine, std::string const &iColor, std::string const &iNs)
@@ -46,6 +62,16 @@ class DebugDrawing
     Line line;
     std::string color{"white"};
     std::string ns;
+
+    std::vector<DebugPoint> getVector()
+    {
+      std::vector<DebugPoint> vectorLine;
+      for (auto const &pt : line)
+      {
+        vectorLine.push_back(DebugPoint(pt.x(), pt.y()));
+      }
+      return vectorLine;
+    }
   };
 
   struct DebugPolygon
@@ -59,28 +85,58 @@ class DebugDrawing
     Polygon polygon;
     std::string color{"white"};
     std::string ns;
+
+    std::vector<DebugPoint> getVector()
+    {
+      std::vector<DebugPoint> vectorPolygon;
+      for (auto const &pt : polygon.outer())
+      {
+        vectorPolygon.push_back(DebugPoint(pt.x(), pt.y()));
+      }
+      return vectorPolygon;
+    }
   };
 
   explicit DebugDrawing() = default;
 
   /**
    * @brief singelton instance getter
    */
-  static DebugDrawing *getInstance()
+  static std::shared_ptr<DebugDrawing> getInstance()
   {
     static DebugDrawing *mSingleton = new DebugDrawing();
-    return mSingleton;
+    return std::shared_ptr<DebugDrawing>(mSingleton, NullDeleter());
   }
 
   /**
    * @brief clean up all geometries to draw (before a new cycle)
    */
   void reset()
   {
+    if (!mEnabled)
+    {
+      return;
+    }
     mLines.clear();
     mPolygons.clear();
   }
 
+  /**
+   * @brief enable/disable debug drawing
+   */
+  void enable(bool value)
+  {
+    mEnabled = value;
+  }
+
+  /**
+   * @brief enable/disable debug drawing
+   */
+  bool isEnabled()
+  {
+    return mEnabled;
+  }
+
   /**
    * @brief draw a line
    *
@@ -90,6 +146,10 @@ class DebugDrawing
    */
   void drawLine(Line const &line, std::string const &color = "white", std::string const &ns = "")
   {
+    if (!mEnabled)
+    {
+      return;
+    }
     mLines.push_back(DebugLine(line, color, ns));
 
     spdlog::trace("DRAW;{};{};{};LINE", ns, color, std::to_string(line));
@@ -104,13 +164,16 @@ class DebugDrawing
    */
   void drawPolygon(Polygon const &polygon, std::string const &color = "white", std::string const &ns = "")
   {
+    if (!mEnabled)
+    {
+      return;
+    }
     mPolygons.push_back(DebugPolygon(polygon, color, ns));
-
-    spdlog::trace("DRAW;{};{};{};POLYGON", ns, color, std::to_string(polygon));
   }
 
   std::vector<DebugLine> mLines;
   std::vector<DebugPolygon> mPolygons;
+  bool mEnabled{false};
 };
 
 } // namespace unstructured

ad_rss/impl/src/unstructured/TrajectoryCommon.hpp

@@ -17,12 +17,6 @@
 #include "ad/rss/situation/VehicleState.hpp"
 #include "ad/rss/unstructured/Geometry.hpp"
 
-#define DEBUG_DRAWING 0
-
-#if DEBUG_DRAWING
-#include "ad/rss/unstructured/DebugDrawing.hpp"
-#endif
-
 /*!
  * @brief namespace ad
  */

ad_rss/impl/src/unstructured/TrajectoryPedestrian.cpp

@@ -12,6 +12,7 @@
 #include "TrajectoryPedestrian.hpp"
 #include <ad/physics/Operation.hpp>
 #include "ad/rss/situation/Physics.hpp"
+#include "ad/rss/unstructured/DebugDrawing.hpp"
 
 /*!
  * @brief namespace ad
@@ -52,10 +53,8 @@ bool TrajectoryPedestrian::calculateTrajectorySets(situation::VehicleState const
       result = calculateTrajectorySetsMoving(vehicleState, timeToStop, brakePolygon, continueForwardPolygon);
     }
   }
-#if DEBUG_DRAWING
-  DEBUG_DRAWING_POLYGON(brakePolygon, "red", "brake_pedestrian");
-  DEBUG_DRAWING_POLYGON(continueForwardPolygon, "green", "continueForward_pedestrian");
-#endif
+  DEBUG_DRAWING_POLYGON(brakePolygon, "red", "pedestrian_brake");
+  DEBUG_DRAWING_POLYGON(continueForwardPolygon, "green", "pedestrian_continueForward");
   return result;
 }
 
@@ -170,11 +169,8 @@ bool TrajectoryPedestrian::calculateTrajectorySetsMoving(situation::VehicleState
   //-------------
   if (result)
   {
-    result = calculateStepPolygon(vehicleState,
-                                  minBrakeDistance,
-                                  responseTimeFrontSide,
-                                  "", //"brake_front",
-                                  brakePolygon);
+    result = calculateStepPolygon(
+      vehicleState, minBrakeDistance, responseTimeFrontSide, "pedestrian_brake_front", brakePolygon);
     if (!result)
     {
       spdlog::debug("TrajectoryPedestrian::calculateTrajectorySets>> Could not calculate brake max step polygon.");
@@ -216,9 +212,7 @@ bool TrajectoryPedestrian::calculateTrajectorySetsMoving(situation::VehicleState
     boost::geometry::append(
       back, getVehicleCorner(finalLeftMaxBrakeDistance, vehicleState.objectState.dimension, VehicleCorner::frontLeft));
     boost::geometry::convex_hull(back, maxBrakePolygon);
-#if DEBUG_DRAWING
-    DEBUG_DRAWING_POLYGON(maxBrakePolygon, "red", "brake_back");
-#endif
+    DEBUG_DRAWING_POLYGON(maxBrakePolygon, "red", "pedestrian_brake_back");
     combinePolygon(maxBrakePolygon, brakePolygon, brakePolygon);
   }
 
@@ -230,8 +224,11 @@ bool TrajectoryPedestrian::calculateTrajectorySetsMoving(situation::VehicleState
   //-------------
   if (result)
   {
-    result = calculateStepPolygon(
-      vehicleState, accelMaxDistance, responseTimeFrontSide, "continueForward_front", continueForwardPolygon);
+    result = calculateStepPolygon(vehicleState,
+                                  accelMaxDistance,
+                                  responseTimeFrontSide,
+                                  "pedestrian_continueForward_front",
+                                  continueForwardPolygon);
     if (!result)
     {
       spdlog::debug(
@@ -399,21 +396,18 @@ bool TrajectoryPedestrian::calculateStepPolygon(situation::VehicleState const &v
   auto finalCenterFrontRight
     = getVehicleCorner(finalStep.center, vehicleState.objectState.dimension, VehicleCorner::frontRight);
 
-//----
-// left
-//----
-#if DEBUG_DRAWING
+  //----
+  // left
+  //----
   int idx = 0;
-#endif
   for (auto it = finalStep.left.begin(); (it != finalStep.left.end()) && result; ++it)
   {
-#if DEBUG_DRAWING
-    auto vehicleLocation = TrafficParticipantLocation(*it, vehicleState);
-    DEBUG_DRAWING_POLYGON(vehicleLocation.toPolygon(), "black", debugNamespace + "_left_" + std::to_string(idx));
+    if (DEBUG_DRAWING_IS_ENABLED())
+    {
+      auto vehicleLocation = TrafficParticipantLocation(*it, vehicleState);
+      DEBUG_DRAWING_POLYGON(vehicleLocation.toPolygon(), "black", debugNamespace + "_left_" + std::to_string(idx));
+    }
     ++idx;
-#else
-    (void)debugNamespace;
-#endif
     boost::geometry::append(ptsLeft,
                             getVehicleCorner(*it, vehicleState.objectState.dimension, VehicleCorner::frontRight));
     boost::geometry::append(ptsLeft,
@@ -422,21 +416,18 @@ bool TrajectoryPedestrian::calculateStepPolygon(situation::VehicleState const &v
   boost::geometry::append(ptsLeft, finalCenterFrontLeft);
   boost::geometry::append(ptsLeft, finalCenterFrontRight);
 
-//-----
-// right
-//-----
-#if DEBUG_DRAWING
+  //-----
+  // right
+  //-----
   idx = 0;
-#endif
   for (auto it = finalStep.right.begin(); (it != finalStep.right.end()) && result; ++it)
   {
-#if DEBUG_DRAWING
-    auto vehicleLocation = TrafficParticipantLocation(*it, vehicleState);
-    DEBUG_DRAWING_POLYGON(vehicleLocation.toPolygon(), "black", debugNamespace + "_right_" + std::to_string(idx));
+    if (DEBUG_DRAWING_IS_ENABLED())
+    {
+      auto vehicleLocation = TrafficParticipantLocation(*it, vehicleState);
+      DEBUG_DRAWING_POLYGON(vehicleLocation.toPolygon(), "black", debugNamespace + "_right_" + std::to_string(idx));
+    }
     ++idx;
-#else
-    (void)debugNamespace;
-#endif
     boost::geometry::append(ptsRight,
                             getVehicleCorner(*it, vehicleState.objectState.dimension, VehicleCorner::frontLeft));
     boost::geometry::append(ptsRight,
@@ -529,10 +520,8 @@ bool TrajectoryPedestrian::getResponseTimeTrajectoryPoint(situation::VehicleStat
                                              speed,
                                              maxDistance);
 
-#if DEBUG_DRAWING
   Line linePts;
   boost::geometry::append(linePts, startingPoint);
-#endif
 
   TrajectoryPoint currentPoint(vehicleState);
 
@@ -564,16 +553,12 @@ bool TrajectoryPedestrian::getResponseTimeTrajectoryPoint(situation::VehicleStat
     pointAfterResponseTime
       = startingPoint + toPoint(-std::sin(startingAngle) * maxDistance, std::cos(startingAngle) * maxDistance);
   }
-#if DEBUG_DRAWING
   boost::geometry::append(linePts, pointAfterResponseTime);
-#endif
 
   resultTrajectoryPoint = TrajectoryPoint(
     pointAfterResponseTime, vehicleState.objectState.yaw + angleChange, speed, physics::AngularVelocity(0.));
-#if DEBUG_DRAWING
   DEBUG_DRAWING_LINE(
     linePts, "orange", std::to_string(aUntilResponseTime) + "_" + std::to_string(angleChangeRatio) + "_trajectory");
-#endif
   return result;
 }