diff --git a/maliput_malidrive/src/maliput_malidrive/road_curve/road_curve_offset.cc b/maliput_malidrive/src/maliput_malidrive/road_curve/road_curve_offset.cc index 940222d4..4f3704e9 100644 --- a/maliput_malidrive/src/maliput_malidrive/road_curve/road_curve_offset.cc +++ b/maliput_malidrive/src/maliput_malidrive/road_curve/road_curve_offset.cc @@ -169,8 +169,9 @@ RoadCurveOffset::RoadCurveOffset(const RoadCurve* road_curve, const Function* la // the same period and amplitude equal to the specified tolerance. The // difference in path length is bounded by 4e and the relative error is thus // bounded by 4e/L. - relative_tolerance_ = road_curve_->linear_tolerance() / road_curve_->LMax(); - + // Relative tolerance is clamped to kMinRelativeTolerance to avoid setting + // accuracy of the integrator that goes beyond the limit of the integrator. + relative_tolerance_ = std::max(road_curve_->linear_tolerance() / road_curve_->LMax(), kMinRelativeTolerance); // Sets `s_from_p`'s integration accuracy and step sizes. Said steps // should not be too large, because that could make accuracy control // fail, nor too small to avoid wasting cycles. The nature of the @@ -186,7 +187,8 @@ RoadCurveOffset::RoadCurveOffset(const RoadCurve* road_curve, const Function* la // road geometry invariants (i.e., CheckInvariants()) in Builder::Build(). // Consider modifying this accuracy if other tolerances are modified // elsewhere. - s_from_p_integrator.set_target_accuracy(relative_tolerance_ * 1e-2); + const double integrator_accuracy{relative_tolerance_ * kAccuracyMultiplier}; + s_from_p_integrator.set_target_accuracy(integrator_accuracy); // Sets `p_from_s`'s integration accuracy and step sizes. Said steps // should not be too large, because that could make accuracy control @@ -198,7 +200,7 @@ RoadCurveOffset::RoadCurveOffset(const RoadCurve* road_curve, const Function* la drake::systems::IntegratorBase& p_from_s_integrator = p_from_s_ivp_->get_mutable_integrator(); p_from_s_integrator.request_initial_step_size_target(road_curve_->scale_length() * 0.1); p_from_s_integrator.set_maximum_step_size(road_curve_->scale_length()); - p_from_s_integrator.set_target_accuracy(relative_tolerance_ * 1e-2); + p_from_s_integrator.set_target_accuracy(integrator_accuracy); } double RoadCurveOffset::CalcSFromP(double p) const { diff --git a/maliput_malidrive/src/maliput_malidrive/road_curve/road_curve_offset.h b/maliput_malidrive/src/maliput_malidrive/road_curve/road_curve_offset.h index 183876fb..35e67f13 100644 --- a/maliput_malidrive/src/maliput_malidrive/road_curve/road_curve_offset.h +++ b/maliput_malidrive/src/maliput_malidrive/road_curve/road_curve_offset.h @@ -78,6 +78,17 @@ class RoadCurveOffset { double p1() const { return p1_; } private: + // Minimum value that #relative_tolerance_ could take. + // Given that the accuracy of the integrator is the product + // of #relative_tolerance_ and #kAccuracyMultiplier this minimum value + // guarantees that the accuracy isn't set beyond the integrator limits. + static constexpr double kMinRelativeTolerance{1e-8}; + + // The result of multiplying it with #relative_tolerance_ is the accuracy of the integrator. + // The value of this multiplier was empirically obtained out of all the maps + // this package provides. + static constexpr double kAccuracyMultiplier{1e-4}; + // Holds the RoadCurve. const RoadCurve* road_curve_{}; diff --git a/maliput_malidrive/test/regression/base/lane_test.cc b/maliput_malidrive/test/regression/base/lane_test.cc index 337e64e6..753ad173 100644 --- a/maliput_malidrive/test/regression/base/lane_test.cc +++ b/maliput_malidrive/test/regression/base/lane_test.cc @@ -125,7 +125,7 @@ class LaneTest : public ::testing::Test { const maliput::api::HBounds kElevationBounds{0., 5.}; const double kP0{0.}; const double kP1{100.}; - const double kLinearTolerance{1e-12}; + const double kLinearTolerance{1e-11}; const double kScaleLength{1.}; const Vector2 kXy0{10., 12.}; const Vector2 kDXy{(kP1 - kP0) * std::sqrt(2.) / 2., (kP1 - kP0) * std::sqrt(2.) / 2.}; diff --git a/maliput_malidrive/test/regression/road_curve/road_curve_offset_test.cc b/maliput_malidrive/test/regression/road_curve/road_curve_offset_test.cc index a9a4ec91..4b92ae06 100644 --- a/maliput_malidrive/test/regression/road_curve/road_curve_offset_test.cc +++ b/maliput_malidrive/test/regression/road_curve/road_curve_offset_test.cc @@ -84,9 +84,21 @@ TEST_F(FlatLineRoadCurveTest, RoadCurve) { } TEST_F(FlatLineRoadCurveTest, RelativeTolerance) { + const double kTolerance{1e-3}; + auto ground_curve = std::make_unique(kTolerance, kXy0, kDXy, kP0, kP1); + auto elevation = std::make_unique(kZero, kZero, kZero, kZero, kP0, kP1, kTolerance); + auto superelevation = std::make_unique(kZero, kZero, kZero, kZero, kP0, kP1, kTolerance); + road_curve_ = std::make_unique(kTolerance, kScaleLength, std::move(ground_curve), std::move(elevation), + std::move(superelevation), kAssertContiguity); const RoadCurveOffset dut(road_curve_.get(), lane_offset_0.get(), kP0, kP1); - EXPECT_DOUBLE_EQ(/*kLinearTolerance / kDXy.norm()*/ 2.0000000000000000607e-13, dut.relative_tolerance()); + EXPECT_DOUBLE_EQ(kTolerance / kDXy.norm(), dut.relative_tolerance()); +} + +TEST_F(FlatLineRoadCurveTest, RelativeToleranceClamped) { + const RoadCurveOffset dut(road_curve_.get(), lane_offset_0.get(), kP0, kP1); + + EXPECT_DOUBLE_EQ(/* Minimum allowed value */ 1e-8, dut.relative_tolerance()); } TEST_F(FlatLineRoadCurveTest, CalcSFromP) { @@ -220,9 +232,22 @@ TEST_F(FlatArcRoadCurveTest, RoadCurve) { } TEST_F(FlatArcRoadCurveTest, RelativeTolerance) { + const double kTolerance{1e-3}; + auto ground_curve = + std::make_unique(kTolerance, kXy0, kStartHeading, kCurvature, kArcLength, kP0, kP1); + auto elevation = std::make_unique(kZero, kZero, kZero, kZero, kP0, kP1, kTolerance); + auto superelevation = std::make_unique(kZero, kZero, kZero, kZero, kP0, kP1, kTolerance); + road_curve_ = std::make_unique(kTolerance, kScaleLength, std::move(ground_curve), std::move(elevation), + std::move(superelevation), kAssertContiguity); + const RoadCurveOffset dut(road_curve_.get(), lane_offset_0.get(), kP0, kP1); + + EXPECT_DOUBLE_EQ(kTolerance / kArcLength, dut.relative_tolerance()); +} + +TEST_F(FlatArcRoadCurveTest, RelativeToleranceClamped) { const RoadCurveOffset dut(road_curve_.get(), lane_offset_0.get(), kP0, kP1); - EXPECT_DOUBLE_EQ(/*kLinearTolerance / kArcLength*/ 1.0000000000000000607e-14, dut.relative_tolerance()); + EXPECT_DOUBLE_EQ(/* Minimum allowed value */ 1e-8, dut.relative_tolerance()); } TEST_F(FlatArcRoadCurveTest, CalcSFromP) {