From f01ad49748600395d49ee40d2cf02ec4856d2a66 Mon Sep 17 00:00:00 2001 From: berndgassmann Date: Tue, 20 Jul 2021 12:33:58 +0200 Subject: [PATCH] FIX: consider lateral fluctuation margin (#108) Updated ad_map_access to v2.4.5 Switched to clang-format-10 --- .clang-format | 1 + .github/workflows/code_format_check.yml | 6 +- .../src/ad/rss/state/RssStateEvaluator.cpp | 19 +++--- .../ad/rss/core/RssResponseResolving.hpp | 60 +++++++++---------- .../impl/include/ad/rss/situation/Physics.hpp | 2 +- .../include/ad/rss/unstructured/Geometry.hpp | 2 +- .../impl/src/core/RssSituationExtraction.cpp | 2 +- ad_rss/impl/src/situation/RssFormulas.cpp | 28 +++++---- .../RssStructuredSceneIntersectionChecker.cpp | 49 ++++++++------- .../src/unstructured/TrajectoryCommon.hpp | 4 +- .../src/unstructured/TrajectoryVehicle.cpp | 8 +-- .../src/world/RssObjectPositionExtractor.hpp | 22 +++---- .../impl/test/core/RssCheckLateralTests.cpp | 8 ++- .../PhysicsUnitTestsTimeToCoverDistance.cpp | 4 +- ...rmulaTestsCalculateSafeLateralDistance.cpp | 34 ++++++++++- ...nCheckingTestsUnstructuredSceneChecker.cpp | 2 +- ad_rss/impl/test/test_support/TestSupport.cpp | 4 +- ad_rss/python/tests/interface_test.py | 2 - .../ad/rss/map/RssObjectConversion.hpp | 4 +- .../impl/include/ad/rss/map/RssObjectData.hpp | 2 +- ...eationTestEgoSouth2EastOtherEast2North.cpp | 14 ++--- ...eationTestEgoSouth2EastOtherEast2South.cpp | 18 ++---- .../python/tests/interface_test.py | 2 +- dependencies/map | 2 +- doc/CHANGELOG.md | 7 ++- 25 files changed, 170 insertions(+), 136 deletions(-) diff --git a/.clang-format b/.clang-format index 3a2be14e3c..ed7e3996ee 100644 --- 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 index bfb407d163..97381c6711 100644 --- a/.github/workflows/code_format_check.yml +++ b/.github/workflows/code_format_check.yml @@ -11,7 +11,7 @@ jobs: name: Check Code Formatting - runs-on: ubuntu-16.04 + runs-on: ubuntu-20.04 steps: - uses: actions/checkout@v2 @@ -19,7 +19,7 @@ jobs: 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 "::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,8 +113,9 @@ 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; } @@ -121,8 +123,9 @@ template <>::ad::rss::state::RssStateEvaluator fromString(std::string const &str { 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 index eb5d896a25..5e51acd9e7 100644 --- 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 index 64ed1d6a6f..09cb4401e4 100644 --- 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 index ac911cdad8..e59e6ed20d 100644 --- 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 index f28c05ffbb..6129812b6f 100644 --- 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 index 42e4260a81..b1c5b08c4a 100644 --- a/ad_rss/impl/src/situation/RssFormulas.cpp +++ b/ad_rss/impl/src/situation/RssFormulas.cpp @@ -74,8 +74,9 @@ bool calculateLateralDistanceOffsetAfterStatedBrakingPattern(Speed const ¤ 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(resultingSpeed)) != std::signbit(static_cast(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 index f7b86d4013..3b8c03017c 100644 --- 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 index 91cb5407fc..c461cbc570 100644 --- 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; struct TrajectorySetStepVehicleLocation diff --git a/ad_rss/impl/src/unstructured/TrajectoryVehicle.cpp b/ad_rss/impl/src/unstructured/TrajectoryVehicle.cpp index c00ba7aeb0..f1f499e9e2 100644 --- 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 index 725474daae..38f7576421 100644 --- 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; }; diff --git a/ad_rss/impl/test/core/RssCheckLateralTests.cpp b/ad_rss/impl/test/core/RssCheckLateralTests.cpp index 571b1c6c45..54bc2372af 100644 --- a/ad_rss/impl/test/core/RssCheckLateralTests.cpp +++ b/ad_rss/impl/test/core/RssCheckLateralTests.cpp @@ -148,6 +148,9 @@ TEST_F(RssCheckLateralEgoLeftTest, Lateral_Velocity_Towards_Each_Other) state::ProperResponse properResponse; core::RssCheck rssCheck; + // increase lateral speed to enforce unsafe cases + worldModel.scenes[0].egoVehicle.velocity.speedLatMax += ad::physics::Speed(0.5); + worldModel.scenes[0].object.velocity.speedLatMin -= ad::physics::Speed(0.5); for (uint32_t i = 0; i <= 90; i++) { worldModel.scenes[0].egoVehicle.occupiedRegions[0].latRange.minimum = ParametricValue(0.01 * i); @@ -156,7 +159,7 @@ TEST_F(RssCheckLateralEgoLeftTest, Lateral_Velocity_Towards_Each_Other) Distance const dMin = calculateLateralMinSafeDistance(worldModel.scenes[0].egoVehicle.velocity.speedLatMax, worldModel.scenes[0].egoVehicleRssDynamics, - worldModel.scenes[0].object.velocity.speedLatMax, + worldModel.scenes[0].object.velocity.speedLatMin, worldModel.scenes[0].objectRssDynamics); ASSERT_TRUE(rssCheck.calculateProperResponse(worldModel, properResponse)); @@ -167,7 +170,6 @@ TEST_F(RssCheckLateralEgoLeftTest, Lateral_Velocity_Towards_Each_Other) } else { - // TODO: never reached testRestrictions(properResponse.accelerationRestrictions, state::LongitudinalResponse::None, state::LateralResponse::None, @@ -179,8 +181,8 @@ TEST_F(RssCheckLateralEgoLeftTest, Lateral_Velocity_Towards_Each_Other) template class RssCheckLateralEgoInTheMiddleTestBase : public TESTBASE { protected: - using TESTBASE::worldModel; using TESTBASE::testRestrictions; + using TESTBASE::worldModel; void SetUp() override { diff --git a/ad_rss/impl/test/situation/PhysicsUnitTestsTimeToCoverDistance.cpp b/ad_rss/impl/test/situation/PhysicsUnitTestsTimeToCoverDistance.cpp index fa46f4122e..22cc10ffd7 100644 --- a/ad_rss/impl/test/situation/PhysicsUnitTestsTimeToCoverDistance.cpp +++ b/ad_rss/impl/test/situation/PhysicsUnitTestsTimeToCoverDistance.cpp @@ -38,8 +38,8 @@ TEST(PhysicsUnitTestsTimeToCoverDistance, no_brake_required_zero_acceleration_is { for (size_t i = 0; i < 10u; ++i) { - Acceleration acceleration( - (static_cast(i) - 5.) * static_cast(std::numeric_limits::epsilon()) * 0.9); + Acceleration acceleration((static_cast(i) - 5.) + * static_cast(std::numeric_limits::epsilon()) * 0.9); Duration requiredTime(0.); EXPECT_TRUE(calculateTimeToCoverDistance( Speed(2.), cMaxSpeedOnAcceleration, Duration(1.), acceleration, Acceleration(-1.), Distance(1.), requiredTime)); diff --git a/ad_rss/impl/test/situation/RssFormulaTestsCalculateSafeLateralDistance.cpp b/ad_rss/impl/test/situation/RssFormulaTestsCalculateSafeLateralDistance.cpp index 1d8b87e9dc..d2313dd78b 100644 --- a/ad_rss/impl/test/situation/RssFormulaTestsCalculateSafeLateralDistance.cpp +++ b/ad_rss/impl/test/situation/RssFormulaTestsCalculateSafeLateralDistance.cpp @@ -31,7 +31,7 @@ TEST(RssFormulaTestsCalculateSafeLateralDistance, same_lateral_speed) { Distance safeDistance(0.); - std::vector expectedSafeDistance = {1., 1.09, 1.09, 2.8, 2.8}; + std::vector expectedSafeDistance = {1.1, 1.19, 1.19, 2.9, 2.9}; uint32_t expectedSafeDistanceIndex = 0u; for (auto lateralSpeed : {0., 1., -1., 5., -5.}) { @@ -52,8 +52,8 @@ TEST(RssFormulaTestsCalculateSafeLateralDistance, one_zero_lateral_speed) { Distance safeDistance(0.); - std::vector expectedSafeDistanceLeft = {1.74, 0.35, 5.67, 0.}; - std::vector expectedSafeDistanceRight = {0.35, 1.74, 0., 5.67}; + std::vector expectedSafeDistanceLeft = {1.84, 0.45, 5.77, 0.}; + std::vector expectedSafeDistanceRight = {0.45, 1.84, 0., 5.77}; uint32_t expectedSafeDistanceIndex = 0u; for (auto lateralSpeed : {1., -1., 5., -5.}) { @@ -70,6 +70,34 @@ TEST(RssFormulaTestsCalculateSafeLateralDistance, one_zero_lateral_speed) } } +TEST(RssFormulaTestsCalculateSafeLateralDistance, lateral_fluctuation_margin_is_considered) +{ + Distance safeDistance(0.); + + std::vector expectedSafeDistance = {1.1, 1.19, 1.19, 2.9, 2.9}; + uint32_t expectedSafeDistanceIndex = 0u; + for (auto lateralSpeed : {0., 1., -1., 5., -5.}) + { + VehicleState leftVehicle = createVehicleStateForLateralMotion(lateralSpeed); + VehicleState rightVehicle = createVehicleStateForLateralMotion(lateralSpeed); + + ASSERT_TRUE(calculateSafeLateralDistance(leftVehicle, rightVehicle, safeDistance)); + ASSERT_NEAR(static_cast(safeDistance), expectedSafeDistance[expectedSafeDistanceIndex], 0.01); + + leftVehicle.dynamics.lateralFluctuationMargin += ad::physics::Distance(0.5); + + ASSERT_TRUE(calculateSafeLateralDistance(rightVehicle, leftVehicle, safeDistance)); + ASSERT_NEAR(static_cast(safeDistance), expectedSafeDistance[expectedSafeDistanceIndex] + 0.25, 0.01); + + rightVehicle.dynamics.lateralFluctuationMargin += ad::physics::Distance(0.5); + + ASSERT_TRUE(calculateSafeLateralDistance(rightVehicle, leftVehicle, safeDistance)); + ASSERT_NEAR(static_cast(safeDistance), expectedSafeDistance[expectedSafeDistanceIndex] + 0.5, 0.01); + + expectedSafeDistanceIndex++; + } +} + } // namespace situation } // namespace rss } // namespace ad diff --git a/ad_rss/impl/test/situation/RssSituationCheckingTestsUnstructuredSceneChecker.cpp b/ad_rss/impl/test/situation/RssSituationCheckingTestsUnstructuredSceneChecker.cpp index cb2516b507..327e7b7dd9 100644 --- a/ad_rss/impl/test/situation/RssSituationCheckingTestsUnstructuredSceneChecker.cpp +++ b/ad_rss/impl/test/situation/RssSituationCheckingTestsUnstructuredSceneChecker.cpp @@ -240,6 +240,6 @@ TEST(RssSituationCheckingTestsUnstructuredSceneChecker, calculateState_other_in_ ASSERT_EQ(rssState.unstructuredSceneState.response, state::UnstructuredSceneResponse::Brake); } -} // namespace unstructured +} // namespace situation } // namespace rss } // namespace ad diff --git a/ad_rss/impl/test/test_support/TestSupport.cpp b/ad_rss/impl/test/test_support/TestSupport.cpp index cb6f90787f..c298d52cb2 100644 --- a/ad_rss/impl/test/test_support/TestSupport.cpp +++ b/ad_rss/impl/test/test_support/TestSupport.cpp @@ -84,6 +84,7 @@ world::RssDynamics getObjectRssDynamics() rssDynamics.alphaLat.accelMax = cMaximumLateralAcceleration; rssDynamics.alphaLat.brakeMin = cMinimumLateralBrakingDeceleleration; + rssDynamics.lateralFluctuationMargin = ad::physics::Distance(0.1); rssDynamics.responseTime = cResponseTimeOtherVehicles; rssDynamics.unstructuredSettings.pedestrianTurningRadius = ad::physics::Distance(2.0); @@ -118,6 +119,7 @@ world::RssDynamics getEgoRssDynamics() rssDynamics.alphaLat.accelMax = cMaximumLateralAcceleration; rssDynamics.alphaLat.brakeMin = cMinimumLateralBrakingDeceleleration; + rssDynamics.lateralFluctuationMargin = ad::physics::Distance(0.1); rssDynamics.responseTime = cResponseTimeEgoVehicle; rssDynamics.unstructuredSettings.pedestrianTurningRadius = ad::physics::Distance(2.0); @@ -296,7 +298,7 @@ Distance calculateLateralMinSafeDistance(physics::Speed const &leftObjectSpeed, { dMin += rObjectVelAfterResTime * rObjectVelAfterResTime / (2. * -rightObjectRssDynamics.alphaLat.brakeMin); } - + dMin += 0.5 * (rightObjectRssDynamics.lateralFluctuationMargin + leftObjectRssDynamics.lateralFluctuationMargin); return std::max(dMin, Distance(0.)); } TestSupport::TestSupport() diff --git a/ad_rss/python/tests/interface_test.py b/ad_rss/python/tests/interface_test.py index 2ad7f7bc9d..54de5ac077 100644 --- a/ad_rss/python/tests/interface_test.py +++ b/ad_rss/python/tests/interface_test.py @@ -164,8 +164,6 @@ def test_getHeadingOverlap(self): self.assertEqual(1. / 4. * ad.physics.cPI, overlap_ranges[1].begin) self.assertEqual(1. / 2. * ad.physics.cPI, overlap_ranges[1].end) - - def test_interface(self): """ Main test part diff --git a/ad_rss_map_integration/impl/include/ad/rss/map/RssObjectConversion.hpp b/ad_rss_map_integration/impl/include/ad/rss/map/RssObjectConversion.hpp index 3df6a11138..de1e623a53 100644 --- a/ad_rss_map_integration/impl/include/ad/rss/map/RssObjectConversion.hpp +++ b/ad_rss_map_integration/impl/include/ad/rss/map/RssObjectConversion.hpp @@ -139,10 +139,10 @@ class RssObjectConversion * for later analysis. * This function can be used to check for an acceptable speed. * - * @param[in] acceptableNegativeSpeed a small negative speed value that should be allowed to be mapped to zero + * @param[in] acceptableNegativeSpeed a small negative speed value that should be allowed to be mapped to zero * without error (default -0.5m/s). * - * @returns \c true if the original speed equals the current internal object speed. + * @returns \c true if the original speed equals the current internal object speed. * It also returns \c true if the original speed is equal or larger than the provided acceptableNegativeSpeed. */ bool isOriginalSpeedAcceptable(::ad::physics::Speed const acceptableNegativeSpeed = ::ad::physics::Speed(-0.5)) const; diff --git a/ad_rss_map_integration/impl/include/ad/rss/map/RssObjectData.hpp b/ad_rss_map_integration/impl/include/ad/rss/map/RssObjectData.hpp index 628e24e617..63332c85dc 100644 --- a/ad_rss_map_integration/impl/include/ad/rss/map/RssObjectData.hpp +++ b/ad_rss_map_integration/impl/include/ad/rss/map/RssObjectData.hpp @@ -29,7 +29,7 @@ namespace rss { namespace map { /** - * @brief struct describing the object input data + * @brief struct describing the object input data */ struct RssObjectData { diff --git a/ad_rss_map_integration/impl/tests/RssSceneCreationTestEgoSouth2EastOtherEast2North.cpp b/ad_rss_map_integration/impl/tests/RssSceneCreationTestEgoSouth2EastOtherEast2North.cpp index 4ef6f2b657..b1534c36b0 100644 --- a/ad_rss_map_integration/impl/tests/RssSceneCreationTestEgoSouth2EastOtherEast2North.cpp +++ b/ad_rss_map_integration/impl/tests/RssSceneCreationTestEgoSouth2EastOtherEast2North.cpp @@ -201,9 +201,10 @@ TEST_F(RssSceneCreationTestWithRouteEgoSouth2EastOtherEast2North, e3_o1) std::initializer_list{ // since the other is besides us, we still find the route from within the intersection // as it is the shortest one - // we get still 2 object predictions + // we get 3 object predictions // route length is zero, but consists of 2 segments here std::make_tuple(::ad::rss::situation::SituationType::OppositeDirection, 2u, 0u, ::ad::physics::Speed(15.2778)), + std::make_tuple(::ad::rss::situation::SituationType::OppositeDirection, 2u, 0u, ::ad::physics::Speed(15.2778)), std::make_tuple(::ad::rss::situation::SituationType::OppositeDirection, 2u, 0u, ::ad::physics::Speed(15.2778))}); } @@ -316,19 +317,12 @@ TEST_F(RssSceneCreationTestWithoutRouteEgoSouth2EastOtherEast2North, e0_o0) // ego-turn-right: all 2 object predictions lead to the opposite direction case std::make_tuple(::ad::rss::situation::SituationType::OppositeDirection, 3u, 0u, ::ad::physics::Speed(100.)), std::make_tuple(::ad::rss::situation::SituationType::OppositeDirection, 3u, 0u, ::ad::physics::Speed(100.)), - // ego-straight-then-right: object prediction 1 - // object coming from right has prio - std::make_tuple( - ::ad::rss::situation::SituationType::IntersectionObjectHasPriority, 3u, 2u, ::ad::physics::Speed(100.)), - // ego-straight-then-right: object prediction 2 + // ego-straight: object prediction 1 // object coming from right has prio std::make_tuple( ::ad::rss::situation::SituationType::IntersectionObjectHasPriority, 3u, 2u, ::ad::physics::Speed(100.)), - // ego-straight-then-straight: object prediction 1 + // ego-straight: object prediction 2 // object coming from right has prio - std::make_tuple( - ::ad::rss::situation::SituationType::IntersectionObjectHasPriority, 3u, 2u, ::ad::physics::Speed(100.)), - // ego-straight-then-straight: object prediction 2 std::make_tuple( ::ad::rss::situation::SituationType::IntersectionObjectHasPriority, 3u, 2u, ::ad::physics::Speed(100.))}); } diff --git a/ad_rss_map_integration/impl/tests/RssSceneCreationTestEgoSouth2EastOtherEast2South.cpp b/ad_rss_map_integration/impl/tests/RssSceneCreationTestEgoSouth2EastOtherEast2South.cpp index 72b8f13901..f0ad594367 100644 --- a/ad_rss_map_integration/impl/tests/RssSceneCreationTestEgoSouth2EastOtherEast2South.cpp +++ b/ad_rss_map_integration/impl/tests/RssSceneCreationTestEgoSouth2EastOtherEast2South.cpp @@ -208,9 +208,10 @@ TEST_F(RssSceneCreationTestWithRouteEgoSouth2EastOtherEast2South, e3_o1) std::initializer_list{ // since the other is besides us, we still find the route from within the intersection // as it is the shortest one - // we get still 2 object predictions + // we get still 3 object predictions // route length is zero, but consists of 2 segments here std::make_tuple(::ad::rss::situation::SituationType::OppositeDirection, 2u, 0u, ::ad::physics::Speed(15.2778)), + std::make_tuple(::ad::rss::situation::SituationType::OppositeDirection, 2u, 0u, ::ad::physics::Speed(15.2778)), std::make_tuple(::ad::rss::situation::SituationType::OppositeDirection, 2u, 0u, ::ad::physics::Speed(15.2778))}); } @@ -323,19 +324,12 @@ TEST_F(RssSceneCreationTestWithoutRouteEgoSouth2EastOtherEast2South, e0_o0) // ego-turn-right: all 2 object predictions lead to the opposite direction case std::make_tuple(::ad::rss::situation::SituationType::OppositeDirection, 3u, 0u, ::ad::physics::Speed(100.)), std::make_tuple(::ad::rss::situation::SituationType::OppositeDirection, 3u, 0u, ::ad::physics::Speed(100.)), - // ego-straight-then-right: object prediction 1 - // object coming form right has prio - std::make_tuple( - ::ad::rss::situation::SituationType::IntersectionObjectHasPriority, 3u, 2u, ::ad::physics::Speed(100.)), - // ego-straight-then-right: object prediction 2 - // object coming form right has prio - std::make_tuple( - ::ad::rss::situation::SituationType::IntersectionObjectHasPriority, 3u, 2u, ::ad::physics::Speed(100.)), - // ego-straight-then-straight: object prediction 1 - // object coming form right has prio + // ego-straight: object prediction 1 + // object coming from right has prio std::make_tuple( ::ad::rss::situation::SituationType::IntersectionObjectHasPriority, 3u, 2u, ::ad::physics::Speed(100.)), - // ego-straight-then-straight: object prediction 2 + // ego-straight: object prediction 2 + // object coming from right has prio std::make_tuple( ::ad::rss::situation::SituationType::IntersectionObjectHasPriority, 3u, 2u, ::ad::physics::Speed(100.))}); } diff --git a/ad_rss_map_integration/python/tests/interface_test.py b/ad_rss_map_integration/python/tests/interface_test.py index 325569aa80..8c7aa737f9 100644 --- a/ad_rss_map_integration/python/tests/interface_test.py +++ b/ad_rss_map_integration/python/tests/interface_test.py @@ -189,7 +189,7 @@ def test_interface(self): longitudinal_distance = rss_situation_snapshot.situations[0].relativePosition.longitudinalDistance self.assertTrue(rss_proper_response.isSafe) - self.assertEqual(longitudinal_distance, ad.physics.Distance(104.413)) + self.assertEqual(longitudinal_distance, ad.physics.Distance(104.415)) self.world_model.timeIndex += 1 diff --git a/dependencies/map b/dependencies/map index 5d0e7c49da..bd7843df30 160000 --- a/dependencies/map +++ b/dependencies/map @@ -1 +1 @@ -Subproject commit 5d0e7c49da2a20e0253c0bc3dde37094a6b7dcc5 +Subproject commit bd7843df3041ac5936cc3e49669f59cb8328aac7 diff --git a/doc/CHANGELOG.md b/doc/CHANGELOG.md index 118effd0c4..96ba3bb45a 100644 --- a/doc/CHANGELOG.md +++ b/doc/CHANGELOG.md @@ -1,5 +1,10 @@ - ## Latest changes + +## Release 4.4.1 + +#### :ghost: Maintenance +* Fix: Consider lateral fluctuation margin correctly. +* Updated ad_map_access to v2.4.5 * Use target python version for build * Fix: Ensure maxSpeedOnAcceleration only limits the actual acceleration while reponse time. * `python_wrapper_helper.py` is now generated by cmake so that it uses the