FIX: consider lateral fluctuation margin (#108)

Updated ad_map_access to v2.4.5 Switched to clang-format-10
intel · Jul 20, 2021 · f01ad49 · f01ad49
1 parent b7ea114
commit f01ad49
Show file tree

Hide file tree

Showing 25 changed files with 170 additions and 136 deletions.
diff --git a/.clang-format b/.clang-format
@@ -23,6 +23,7 @@ AlwaysBreakTemplateDeclarations: false
 BinPackArguments: false
 BinPackParameters: false
 BraceWrapping:
+  AfterCaseLabel:  true
   AfterClass:      true
   AfterControlStatement: true
   AfterEnum:       true

diff --git a/.github/workflows/code_format_check.yml b/.github/workflows/code_format_check.yml
@@ -11,15 +11,15 @@ jobs:
 
     name: Check Code Formatting
 
-    runs-on: ubuntu-16.04
+    runs-on: ubuntu-20.04
 
     steps:
     - uses: actions/checkout@v2
       with:
           submodules: true
 
     - name: Install Dependencies
-      run: sudo apt-get update && sudo apt-get install clang-format-3.9
+      run: sudo apt-get update && sudo apt-get install clang-format-10
 
     - name: Check Formatting
-      run: failed=0; for file in `find . -path ./dependencies -prune -false -o -iname *.cpp -o -iname *.hpp`; do if [ `clang-format-3.9 $file -output-replacements-xml | grep -c "<replacement "` -ne 0 ]; then echo "$file does not match codeing style. Please run clang-format-3.9"; failed=1; fi; if [ $failed -ne 0 ]; then exit 1; fi; done
+      run: failed=0; for file in `find . -path ./dependencies -prune -false -o -iname *.cpp -o -iname *.hpp`; do if [ `clang-format-10 $file -output-replacements-xml | grep -c "<replacement "` -ne 0 ]; then echo "$file does not match codeing style. Please run clang-format-10"; failed=1; fi; if [ $failed -ne 0 ]; then exit 1; fi; done
diff --git a/ad_rss/generated/src/ad/rss/state/RssStateEvaluator.cpp b/ad_rss/generated/src/ad/rss/state/RssStateEvaluator.cpp
@@ -68,7 +68,7 @@ template <>::ad::rss::state::RssStateEvaluator fromString(std::string const &str
   if (
     str
     == std::string(
-         "::ad::rss::state::RssStateEvaluator::LongitudinalDistanceOppositeDirectionEgoCorrectLane")) // LCOV_EXCL_BR_LINE
+      "::ad::rss::state::RssStateEvaluator::LongitudinalDistanceOppositeDirectionEgoCorrectLane")) // LCOV_EXCL_BR_LINE
   {
     return ::ad::rss::state::RssStateEvaluator::LongitudinalDistanceOppositeDirectionEgoCorrectLane;
   }
@@ -85,8 +85,9 @@ template <>::ad::rss::state::RssStateEvaluator fromString(std::string const &str
   {
     return ::ad::rss::state::RssStateEvaluator::LongitudinalDistanceOppositeDirection;
   }
-  if (str == std::string(
-               "::ad::rss::state::RssStateEvaluator::LongitudinalDistanceSameDirectionEgoFront")) // LCOV_EXCL_BR_LINE
+  if (str
+      == std::string(
+        "::ad::rss::state::RssStateEvaluator::LongitudinalDistanceSameDirectionEgoFront")) // LCOV_EXCL_BR_LINE
   {
     return ::ad::rss::state::RssStateEvaluator::LongitudinalDistanceSameDirectionEgoFront;
   }
@@ -96,7 +97,7 @@ template <>::ad::rss::state::RssStateEvaluator fromString(std::string const &str
   }
   if (str
       == std::string(
-           "::ad::rss::state::RssStateEvaluator::LongitudinalDistanceSameDirectionOtherInFront")) // LCOV_EXCL_BR_LINE
+        "::ad::rss::state::RssStateEvaluator::LongitudinalDistanceSameDirectionOtherInFront")) // LCOV_EXCL_BR_LINE
   {
     return ::ad::rss::state::RssStateEvaluator::LongitudinalDistanceSameDirectionOtherInFront;
   }
@@ -112,17 +113,19 @@ template <>::ad::rss::state::RssStateEvaluator fromString(std::string const &str
   {
     return ::ad::rss::state::RssStateEvaluator::LateralDistance;
   }
-  if (str == std::string(
-               "::ad::rss::state::RssStateEvaluator::IntersectionOtherPriorityEgoAbleToStop")) // LCOV_EXCL_BR_LINE
+  if (str
+      == std::string(
+        "::ad::rss::state::RssStateEvaluator::IntersectionOtherPriorityEgoAbleToStop")) // LCOV_EXCL_BR_LINE
   {
     return ::ad::rss::state::RssStateEvaluator::IntersectionOtherPriorityEgoAbleToStop;
   }
   if (str == std::string("IntersectionOtherPriorityEgoAbleToStop")) // LCOV_EXCL_BR_LINE
   {
     return ::ad::rss::state::RssStateEvaluator::IntersectionOtherPriorityEgoAbleToStop;
   }
-  if (str == std::string(
-               "::ad::rss::state::RssStateEvaluator::IntersectionEgoPriorityOtherAbleToStop")) // LCOV_EXCL_BR_LINE
+  if (str
+      == std::string(
+        "::ad::rss::state::RssStateEvaluator::IntersectionEgoPriorityOtherAbleToStop")) // LCOV_EXCL_BR_LINE
   {
     return ::ad::rss::state::RssStateEvaluator::IntersectionEgoPriorityOtherAbleToStop;
   }

diff --git a/ad_rss/impl/include/ad/rss/core/RssResponseResolving.hpp b/ad_rss/impl/include/ad/rss/core/RssResponseResolving.hpp
@@ -57,18 +57,18 @@ class RssResponseResolving
 
 private:
   /*!
-  * @brief updateAccelerationRestriction
-  *
-  * Updates the longitudinal accelerationRestriction
-  *
-  * @param[in] state - The unstructured scene state to update the acceleration restrictions with
- *  @param[inout] driveAwayBrakeMin -- the minimal brakeMin of all drive-away states
- *  @param[inout] driveAwayToBrakeTransition -- a transition from drive-Away to Brake happened
- *  @param[inout] response -- the combined RSS response to become RSS safe.
- *  @param[inout] responseHeadingRanges -- the combined RSS response heading ranges (for driving away)
-  * @param[inout] accelerationRange - The combined restrictions on the vehicle acceleration to become RSS safe.
-  *
-  */
+   * @brief updateAccelerationRestriction
+   *
+   * Updates the longitudinal accelerationRestriction
+   *
+   * @param[in] state - The unstructured scene state to update the acceleration restrictions with
+   * @param[inout] driveAwayBrakeMin -- the minimal brakeMin of all drive-away states
+   * @param[inout] driveAwayToBrakeTransition -- a transition from drive-Away to Brake happened
+   * @param[inout] response -- the combined RSS response to become RSS safe.
+   * @param[inout] responseHeadingRanges -- the combined RSS response heading ranges (for driving away)
+   * @param[inout] accelerationRange - The combined restrictions on the vehicle acceleration to become RSS safe.
+   *
+   */
   void combineState(state::UnstructuredSceneRssState const &state,
                     physics::Acceleration &driveAwayBrakeMin,
                     bool &driveAwayToBrakeTransition,
@@ -77,29 +77,29 @@ class RssResponseResolving
                     physics::AccelerationRange &accelerationRange);
 
   /*!
-  * @brief updateAccelerationRestriction
-  *
-  * Updates the longitudinal accelerationRestriction
-  *
-  * @param[in] state - The longitudinal state to update the acceleration restrictions with
- *  @param[inout] response -- the combined RSS response to become RSS safe.
-  * @param[inout] accelerationRange - The combined restrictions on the vehicle acceleration to become RSS safe.
-  *
-  */
+   * @brief updateAccelerationRestriction
+   *
+   * Updates the longitudinal accelerationRestriction
+   *
+   * @param[in] state - The longitudinal state to update the acceleration restrictions with
+   * @param[inout] response -- the combined RSS response to become RSS safe.
+   * @param[inout] accelerationRange - The combined restrictions on the vehicle acceleration to become RSS safe.
+   *
+   */
   void combineState(state::LongitudinalRssState const &state,
                     state::LongitudinalResponse &response,
                     physics::AccelerationRange &accelerationRange);
 
   /*!
-  * @brief updateAccelerationRestriction
-  *
-  * Updates the lateral accelerationRestrictions
-  *
-  * @param[in] state - The lateral state to update the acceleration restrictions with
-  * @param[inout] response -- the combined RSS response to become RSS safe.
-  * @param[inout] accelerationRange - The restrictions on the vehicle acceleration to become RSS safe.
-  *
-  */
+   * @brief updateAccelerationRestriction
+   *
+   * Updates the lateral accelerationRestrictions
+   *
+   * @param[in] state - The lateral state to update the acceleration restrictions with
+   * @param[inout] response -- the combined RSS response to become RSS safe.
+   * @param[inout] accelerationRange - The restrictions on the vehicle acceleration to become RSS safe.
+   *
+   */
   void combineState(state::LateralRssState const &state,
                     state::LateralResponse &response,
                     physics::AccelerationRange &accelerationRange);

diff --git a/ad_rss/impl/include/ad/rss/situation/Physics.hpp b/ad_rss/impl/include/ad/rss/situation/Physics.hpp
@@ -39,7 +39,7 @@ namespace situation {
  * @param[in]  speed           is the current speed
  * @param[in]  acceleration    is the acceleration value to be considered
  * @param[in]  duration        is the (positive) period of time the acceleration is performed
-*  @param[out] distanceOffset  is the distance offset from the current position.
+ * @param[out] distanceOffset  is the distance offset from the current position.
  *
  * @return true on success, false otherwise
  */

diff --git a/ad_rss/impl/include/ad/rss/unstructured/Geometry.hpp b/ad_rss/impl/include/ad/rss/unstructured/Geometry.hpp
@@ -331,4 +331,4 @@ inline std::string to_string(ad::rss::unstructured::Line value)
   stream << "]";
   return stream.str();
 }
-}
+} // namespace std
diff --git a/ad_rss/impl/src/core/RssSituationExtraction.cpp b/ad_rss/impl/src/core/RssSituationExtraction.cpp
@@ -435,7 +435,7 @@ bool RssSituationExtraction::mergeSituations(situation::Situation const &otherSi
     || (otherSituation.situationType != mergedSituation.situationType)
     || !mergeVehicleStates(MergeMode::EgoVehicle, otherSituation.egoVehicleState, mergedSituation.egoVehicleState)
     || !mergeVehicleStates(
-         MergeMode::OtherVehicle, otherSituation.otherVehicleState, mergedSituation.otherVehicleState))
+      MergeMode::OtherVehicle, otherSituation.otherVehicleState, mergedSituation.otherVehicleState))
   {
     return false;
   }

diff --git a/ad_rss/impl/src/situation/RssFormulas.cpp b/ad_rss/impl/src/situation/RssFormulas.cpp
@@ -74,8 +74,9 @@ bool calculateLateralDistanceOffsetAfterStatedBrakingPattern(Speed const &curren
   Distance distanceOffsetAfterResponseTime = Distance(0.);
 
   bool result = calculateSpeedInAcceleratedMovement(currentSpeed, acceleration, responseTime, resultingSpeed);
-  result = result && calculateDistanceOffsetInAcceleratedMovement(
-                       currentSpeed, acceleration, responseTime, distanceOffsetAfterResponseTime);
+  result = result
+    && calculateDistanceOffsetInAcceleratedMovement(
+             currentSpeed, acceleration, responseTime, distanceOffsetAfterResponseTime);
 
   Distance distanceToStop = Distance(0.);
   if (std::signbit(static_cast<double>(resultingSpeed)) != std::signbit(static_cast<double>(deceleration)))
@@ -112,9 +113,9 @@ bool calculateSafeLongitudinalDistanceSameDirection(VehicleState const &leadingV
     followingVehicle.dynamics.alphaLon.brakeMin,
     distanceStatedBraking);
   Distance distanceMaxBrake = Distance(0.);
-  result = result && calculateStoppingDistance(leadingVehicle.velocity.speedLon.minimum,
-                                               leadingVehicle.dynamics.alphaLon.brakeMax,
-                                               distanceMaxBrake);
+  result = result
+    && calculateStoppingDistance(
+             leadingVehicle.velocity.speedLon.minimum, leadingVehicle.dynamics.alphaLon.brakeMax, distanceMaxBrake);
 
   if (result)
   {
@@ -256,18 +257,21 @@ bool calculateSafeLateralDistance(VehicleState const &leftVehicle,
     leftVehicle.dynamics.alphaLat.brakeMin,
     distanceOffsetStatedBrakingLeft);
 
-  result = result && calculateLateralDistanceOffsetAfterStatedBrakingPattern( // LCOV_EXCL_LINE: wrong detection
-                       rightVehicle.velocity.speedLat.minimum,
-                       rightVehicle.dynamics.responseTime,
-                       -rightVehicle.dynamics.alphaLat.accelMax,
-                       -rightVehicle.dynamics.alphaLat.brakeMin,
-                       distanceOffsetStatedBrakingRight);
+  result = result
+    && calculateLateralDistanceOffsetAfterStatedBrakingPattern( // LCOV_EXCL_LINE: wrong detection
+             rightVehicle.velocity.speedLat.minimum,
+             rightVehicle.dynamics.responseTime,
+             -rightVehicle.dynamics.alphaLat.accelMax,
+             -rightVehicle.dynamics.alphaLat.brakeMin,
+             distanceOffsetStatedBrakingRight);
 
   if (result)
   {
     // safe distance is the difference of both distances
-    // Note: The fluctuation margin is already considered in the vehicle bounding boxes
     safeDistance = distanceOffsetStatedBrakingLeft - distanceOffsetStatedBrakingRight;
+    //  plus the lateral fluctuation margin: here we use the 0.5*my of both
+    safeDistance
+      += 0.5 * (leftVehicle.dynamics.lateralFluctuationMargin + rightVehicle.dynamics.lateralFluctuationMargin);
     safeDistance = std::max(safeDistance, Distance(0.));
   }
   return result;

diff --git a/ad_rss/impl/src/situation/RssStructuredSceneIntersectionChecker.cpp b/ad_rss/impl/src/situation/RssStructuredSceneIntersectionChecker.cpp
@@ -43,29 +43,32 @@ bool RssStructuredSceneIntersectionChecker::checkLateralIntersect(Situation cons
                                              situation.egoVehicleState.distanceToEnterIntersection,
                                              timeToReachEgo);
 
-  result = result && calculateTimeToCoverDistance(situation.otherVehicleState.velocity.speedLon.maximum,
-                                                  situation.egoVehicleState.dynamics.maxSpeedOnAcceleration,
-                                                  situation.otherVehicleState.dynamics.responseTime,
-                                                  situation.otherVehicleState.dynamics.alphaLon.accelMax,
-                                                  situation.otherVehicleState.dynamics.alphaLon.brakeMin,
-                                                  situation.otherVehicleState.distanceToEnterIntersection,
-                                                  timeToReachOther);
-
-  result = result && calculateTimeToCoverDistance(situation.egoVehicleState.velocity.speedLon.minimum,
-                                                  situation.egoVehicleState.dynamics.maxSpeedOnAcceleration,
-                                                  situation.egoVehicleState.dynamics.responseTime,
-                                                  situation.egoVehicleState.dynamics.alphaLon.brakeMax,
-                                                  situation.egoVehicleState.dynamics.alphaLon.brakeMax,
-                                                  situation.egoVehicleState.distanceToLeaveIntersection,
-                                                  timeToLeaveEgo);
-
-  result = result && calculateTimeToCoverDistance(situation.otherVehicleState.velocity.speedLon.minimum,
-                                                  situation.egoVehicleState.dynamics.maxSpeedOnAcceleration,
-                                                  situation.otherVehicleState.dynamics.responseTime,
-                                                  situation.otherVehicleState.dynamics.alphaLon.brakeMax,
-                                                  situation.otherVehicleState.dynamics.alphaLon.brakeMax,
-                                                  situation.otherVehicleState.distanceToLeaveIntersection,
-                                                  timeToLeaveOther);
+  result = result
+    && calculateTimeToCoverDistance(situation.otherVehicleState.velocity.speedLon.maximum,
+                                    situation.egoVehicleState.dynamics.maxSpeedOnAcceleration,
+                                    situation.otherVehicleState.dynamics.responseTime,
+                                    situation.otherVehicleState.dynamics.alphaLon.accelMax,
+                                    situation.otherVehicleState.dynamics.alphaLon.brakeMin,
+                                    situation.otherVehicleState.distanceToEnterIntersection,
+                                    timeToReachOther);
+
+  result = result
+    && calculateTimeToCoverDistance(situation.egoVehicleState.velocity.speedLon.minimum,
+                                    situation.egoVehicleState.dynamics.maxSpeedOnAcceleration,
+                                    situation.egoVehicleState.dynamics.responseTime,
+                                    situation.egoVehicleState.dynamics.alphaLon.brakeMax,
+                                    situation.egoVehicleState.dynamics.alphaLon.brakeMax,
+                                    situation.egoVehicleState.distanceToLeaveIntersection,
+                                    timeToLeaveEgo);
+
+  result = result
+    && calculateTimeToCoverDistance(situation.otherVehicleState.velocity.speedLon.minimum,
+                                    situation.egoVehicleState.dynamics.maxSpeedOnAcceleration,
+                                    situation.otherVehicleState.dynamics.responseTime,
+                                    situation.otherVehicleState.dynamics.alphaLon.brakeMax,
+                                    situation.otherVehicleState.dynamics.alphaLon.brakeMax,
+                                    situation.otherVehicleState.distanceToLeaveIntersection,
+                                    timeToLeaveOther);
 
   if (result)
   {

diff --git a/ad_rss/impl/src/unstructured/TrajectoryCommon.hpp b/ad_rss/impl/src/unstructured/TrajectoryCommon.hpp
@@ -172,8 +172,8 @@ struct TrafficParticipantLocation
 };
 
 /*!
-  * a trajectory
-  */
+ * a trajectory
+ */
 using Trajectory = std::vector<TrajectoryPoint>;
 
 struct TrajectorySetStepVehicleLocation

diff --git a/ad_rss/impl/src/unstructured/TrajectoryVehicle.cpp b/ad_rss/impl/src/unstructured/TrajectoryVehicle.cpp
@@ -410,8 +410,9 @@ bool TrajectoryVehicle::calculateContinueForward(situation::VehicleState const &
   // Front
   //-----------
   auto ratioDiffFront = physics::RatioValue(
-    2.0 / (2.0 * vehicleState.dynamics.unstructuredSettings.vehicleContinueForwardIntermediateYawRateChangeRatioSteps
-           + 2.0));
+    2.0
+    / (2.0 * vehicleState.dynamics.unstructuredSettings.vehicleContinueForwardIntermediateYawRateChangeRatioSteps
+       + 2.0));
   auto front = responseTimeFrontSide;
   // center-front, with no change of the current yaw rate
   auto result = calculateTrajectorySetStepOnCircle(
@@ -442,8 +443,7 @@ bool TrajectoryVehicle::calculateContinueForward(situation::VehicleState const &
   // center-left, with maximum changing of the yaw rate
   front.left.reserve(
     front.left.size()
-    + vehicleState.dynamics.unstructuredSettings.vehicleContinueForwardIntermediateYawRateChangeRatioSteps
-    + 1);
+    + vehicleState.dynamics.unstructuredSettings.vehicleContinueForwardIntermediateYawRateChangeRatioSteps + 1);
   for (auto ratioValue = ratioDiffFront; (ratioValue <= physics::RatioValue(1.0)) && result;
        ratioValue += ratioDiffFront)
   {

diff --git a/ad_rss/impl/src/world/RssObjectPositionExtractor.hpp b/ad_rss/impl/src/world/RssObjectPositionExtractor.hpp
@@ -49,26 +49,26 @@ class ObjectDimensions
   physics::MetricRange longitudinalDimensions;
 
   /**
-    * @brief range of lateral object position
-    */
+   * @brief range of lateral object position
+   */
   physics::MetricRange lateralDimensions;
 
   /**
-    * @brief flag to indicate if the object is on the positive driving lane
-    */
+   * @brief flag to indicate if the object is on the positive driving lane
+   */
   bool onPositiveLane{false};
 
   /**
-    * @brief flag to indicate if the object is on the negative driving lane
-    */
+   * @brief flag to indicate if the object is on the negative driving lane
+   */
   bool onNegativeLane{false};
 
   /**
-      * @brief range of intersection position
-      *
-      * minimum: minimal distance to intersection entry
-      * maximum: maximum distance to intersection exit
-      */
+   * @brief range of intersection position
+   *
+   * minimum: minimal distance to intersection entry
+   * maximum: maximum distance to intersection exit
+   */
   physics::MetricRange intersectionPosition;
 };