Express characteristic scale length concept in api::RoadGeometry (Rob…

…otLocomotion#9306)

* Express characteristic scale length concept in api::RoadGeometry

automotive/maliput/api/road_geometry.h

@@ -126,6 +126,12 @@ class RoadGeometry {
     return do_angular_tolerance();
   }
 
+  // TODO([email protected]) Needs a precise mathematical definition.
+  /// Returns the characteristic scale length expressed by this RoadGeometry.
+  double scale_length() const {
+    return do_scale_length();
+  }
+
   /// Verifies certain invariants guaranteed by the API.
   ///
   /// Returns a vector of strings describing violations of invariants.
@@ -160,6 +166,8 @@ class RoadGeometry {
   virtual double do_linear_tolerance() const = 0;
 
   virtual double do_angular_tolerance() const = 0;
+
+  virtual double do_scale_length() const = 0;
   ///@}
 };
 

automotive/maliput/doc/maliput-design.org

@@ -123,6 +123,9 @@ surface.  The routing graph (=Lanes= are joined end-to-end via
 =BranchPoints=) describes how one can get from one region of the road
 network to another.
 
+*** Scale Length and Tolerances
+/TODO: Explain the concepts of linear tolerance, angular tolerance, and
+characteristic scale length./
 *** =World= Frame versus =Lane= Frame
 Two types of coordinate frames are used in this model: the (single)
 =World=-frame and the (multiple) =Lane=-frames.  In both, distances
@@ -413,10 +416,10 @@ joins those two end-to-end (resulting in three independent =Junctions=).
 *** =RoadGeometry=
     * accessors for component =Junctions=
     * accessors for component =BranchPoints=
-    * accessors for tolerances
+    * accessors for characteristic lengths and tolerances
       * =linear_tolerance=
       * =angular_tolerance=
-      * /TODO/ =scale_length=
+      * =scale_length=
 *** =Junction=
     * accessors for parent =RoadGeometry=, component =Junctions=
 *** =Segment=

automotive/maliput/dragway/road_geometry.cc

@@ -27,6 +27,13 @@ RoadGeometry::RoadGeometry(const api::RoadGeometryId& id,
   : id_(id),
     linear_tolerance_(linear_tolerance),
     angular_tolerance_(angular_tolerance),
+    // Dragway is completely flat and featureless, so the scale-length might
+    // as well be any value that characterizes its overall size.
+    // TODO([email protected])  If this code is ever re-arranged in a way that
+    //                          makes it possible to query the segment for its
+    //                          `road_width`, then use the max of length and
+    //                          width.
+    scale_length_(length),
     junction_(this, num_lanes, length,
               lane_width, shoulder_width, maximum_height) {
   DRAKE_DEMAND(length > 0);

automotive/maliput/dragway/road_geometry.h

@@ -82,6 +82,8 @@ class RoadGeometry final : public api::RoadGeometry {
 
   double do_angular_tolerance() const final { return angular_tolerance_; }
 
+  double do_scale_length() const final { return scale_length_; }
+
   // Returns true iff `geo_pos` is "on" the dragway. It is on the dragway iff
   // `geo_pos.x` and `geo_pos.y` fall within the dragway's driveable region.
   bool IsGeoPositionOnDragway(const api::GeoPosition& geo_pos) const;
@@ -94,6 +96,7 @@ class RoadGeometry final : public api::RoadGeometry {
   const api::RoadGeometryId id_;
   const double linear_tolerance_{};
   const double angular_tolerance_{};
+  const double scale_length_{};
   const Junction junction_;
   api::BasicIdIndex id_index_;
 };

automotive/maliput/geometry_base/road_geometry.h

@@ -50,14 +50,17 @@ class RoadGeometry : public api::RoadGeometry {
   /// @param angular_tolerance the angular tolerance
   ///
   /// @throws std::exception if either `linear_tolerance` or
-  ///         `angular_tolerance` is non-positive.
+  ///         `angular_tolerance` or `scale_length` is non-positive.
   RoadGeometry(const api::RoadGeometryId& id,
-               double linear_tolerance, double angular_tolerance)
+               double linear_tolerance, double angular_tolerance,
+               double scale_length)
       : id_(id),
         linear_tolerance_(linear_tolerance),
-        angular_tolerance_(angular_tolerance) {
+        angular_tolerance_(angular_tolerance),
+        scale_length_(scale_length) {
     DRAKE_THROW_UNLESS(linear_tolerance_ > 0.);
     DRAKE_THROW_UNLESS(angular_tolerance_ > 0.);
+    DRAKE_THROW_UNLESS(scale_length_ > 0.);
   }
 
   /// Adds @p junction to this RoadGeometry.
@@ -123,9 +126,12 @@ class RoadGeometry : public api::RoadGeometry {
 
   double do_angular_tolerance() const override { return angular_tolerance_; }
 
+  double do_scale_length() const override { return scale_length_; }
+
   api::RoadGeometryId id_;
   double linear_tolerance_{};
   double angular_tolerance_{};
+  double scale_length_{};
   std::vector<std::unique_ptr<Junction>> junctions_;
   std::vector<std::unique_ptr<BranchPoint>> branch_points_;
   api::BasicIdIndex id_index_;

automotive/maliput/geometry_base/test/geometry_base_test.cc

@@ -238,17 +238,25 @@ GTEST_TEST(GeometryBaseJunctionTest, AddingSegments) {
 GTEST_TEST(GeometryBaseRoadGeometryTest, BasicConstruction) {
   const double kValidLinearTolerance = 7.0;
   const double kValidAngularTolerance = 99.0;
-  // Tolerance values must be positive.
+  const double kValidScaleLength = 0.5;
+  // Tolerance/scale-length values must be positive.
   EXPECT_THROW(MockRoadGeometry(
-      api::RoadGeometryId("dut"), kValidLinearTolerance, 0.), std::exception);
+      api::RoadGeometryId("dut"),
+      0., kValidAngularTolerance, kValidScaleLength), std::exception);
   EXPECT_THROW(MockRoadGeometry(
-      api::RoadGeometryId("dut"), 0., kValidAngularTolerance), std::exception);
+      api::RoadGeometryId("dut"),
+      kValidLinearTolerance, 0., kValidScaleLength), std::exception);
+  EXPECT_THROW(MockRoadGeometry(
+      api::RoadGeometryId("dut"),
+      kValidLinearTolerance, kValidAngularTolerance, 0.), std::exception);
 
-  const MockRoadGeometry dut(api::RoadGeometryId("dut"),
-                             kValidLinearTolerance, kValidAngularTolerance);
+  const MockRoadGeometry dut(
+      api::RoadGeometryId("dut"),
+      kValidLinearTolerance, kValidAngularTolerance, kValidScaleLength);
   EXPECT_EQ(dut.id(), api::RoadGeometryId("dut"));
   EXPECT_EQ(dut.linear_tolerance(), kValidLinearTolerance);
   EXPECT_EQ(dut.angular_tolerance(), kValidAngularTolerance);
+  EXPECT_EQ(dut.scale_length(), kValidScaleLength);
 }
 
 
@@ -262,7 +270,8 @@ GTEST_TEST(GeometryBaseRoadGeometryTest, AddingBranchPoints) {
 
   const double kSomePositiveDouble = 7.0;
   MockRoadGeometry dut(
-      api::RoadGeometryId("dut"), kSomePositiveDouble, kSomePositiveDouble);
+      api::RoadGeometryId("dut"),
+      kSomePositiveDouble, kSomePositiveDouble, kSomePositiveDouble);
 
   // Test the empty dut.
   EXPECT_EQ(dut.num_branch_points(), 0);
@@ -298,7 +307,8 @@ GTEST_TEST(GeometryBaseRoadGeometryTest, AddingJunctions) {
 
   const double kSomePositiveDouble = 7.0;
   MockRoadGeometry dut(
-      api::RoadGeometryId("dut"), kSomePositiveDouble, kSomePositiveDouble);
+      api::RoadGeometryId("dut"),
+      kSomePositiveDouble, kSomePositiveDouble, kSomePositiveDouble);
 
   // Test the empty dut.
   EXPECT_EQ(dut.num_junctions(), 0);
@@ -337,7 +347,7 @@ class GeometryBaseRoadGeometryIndexingTest : public ::testing::Test {
   void SetUp() override {
     constexpr double kArbitrary{1.};
     road_geometry_ = std::make_unique<MockRoadGeometry>(
-        api::RoadGeometryId("dut"), kArbitrary, kArbitrary);
+        api::RoadGeometryId("dut"), kArbitrary, kArbitrary, kArbitrary);
   }
 
   std::unique_ptr<RoadGeometry> road_geometry_;
@@ -402,7 +412,7 @@ TEST_F(GeometryBaseRoadGeometryIndexingTest, Test) {
 
 
 GTEST_TEST(GeometryBaseRoadGeometryTest, UnimplementedMethods) {
-  MockRoadGeometry dut(api::RoadGeometryId("dut"), 1., 1.);
+  MockRoadGeometry dut(api::RoadGeometryId("dut"), 1., 1., 1.);
   // Ensure that the not-actually-implemented methods throw an exception.
   EXPECT_THROW(dut.ToRoadPosition(
       api::GeoPosition(), nullptr, nullptr, nullptr), std::exception);

automotive/maliput/geometry_base/test_utilities/mock_geometry.h

@@ -41,11 +41,13 @@ class MockRoadGeometry : public geometry_base::RoadGeometry {
   /// @param angular_tolerance the angular tolerance
   ///
   /// @throws std::exception if either `linear_tolerance` or
-  ///         `angular_tolerance` is non-positive.
+  ///         `angular_tolerance` or `scale_length` is non-positive.
   MockRoadGeometry(const api::RoadGeometryId& id,
-                   const double linear_tolerance,
-                   const double angular_tolerance)
-      : geometry_base::RoadGeometry(id, linear_tolerance, angular_tolerance) {}
+                   double linear_tolerance,
+                   double angular_tolerance,
+                   double scale_length)
+      : geometry_base::RoadGeometry(id, linear_tolerance, angular_tolerance,
+                                    scale_length) {}
 
  private:
   api::RoadPosition DoToRoadPosition(const api::GeoPosition& geo_position,

automotive/maliput/monolane/road_geometry.h

@@ -65,6 +65,12 @@ class RoadGeometry : public api::RoadGeometry {
 
   double do_angular_tolerance() const override { return angular_tolerance_; }
 
+  // TODO([email protected])  monolane is not scale_length aware in any way,
+  //                          and really this property should be consistent
+  //                          with the geometry of the curves themselves.
+  //                          This value of 1 has been picked arbitrarily.
+  double do_scale_length() const override { return 1.; }
+
   api::RoadGeometryId id_;
   double linear_tolerance_{};
   double angular_tolerance_{};

automotive/maliput/multilane/builder.cc

@@ -404,7 +404,7 @@ std::vector<Lane*> Builder::BuildConnection(
 std::unique_ptr<const api::RoadGeometry> Builder::Build(
     const api::RoadGeometryId& id) const {
   auto road_geometry = std::make_unique<RoadGeometry>(
-      id, linear_tolerance_, angular_tolerance_);
+      id, linear_tolerance_, angular_tolerance_, scale_length_);
   std::map<Endpoint, BranchPoint*, EndpointFuzzyOrder> bp_map(
       (EndpointFuzzyOrder(linear_tolerance_)));
   std::map<const Connection*, std::vector<Lane*>> lane_map;

automotive/maliput/multilane/road_geometry.h

@@ -22,13 +22,16 @@ class RoadGeometry : public api::RoadGeometry {
  public:
   DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(RoadGeometry)
 
-  /// Constructs an empty RoadGeometry with the specified tolerances.
+  /// Constructs an empty RoadGeometry with the specified tolerances and
+  /// scale-length.
   RoadGeometry(const api::RoadGeometryId& id,
-               const double linear_tolerance,
-               const double angular_tolerance)
+               double linear_tolerance,
+               double angular_tolerance,
+               double scale_length)
       : id_(id),
         linear_tolerance_(linear_tolerance),
-        angular_tolerance_(angular_tolerance) {}
+        angular_tolerance_(angular_tolerance),
+        scale_length_(scale_length) {}
 
   /// Creates and adds a new Junction with the specified @p id.
   Junction* NewJunction(api::JunctionId id);
@@ -69,9 +72,15 @@ class RoadGeometry : public api::RoadGeometry {
 
   double do_angular_tolerance() const override { return angular_tolerance_; }
 
+  // TODO([email protected])  scale_length should really be kept consistent
+  //                          with the geometry of the curves themselves,
+  //                          perhaps even derived from the curves directly.
+  double do_scale_length() const override { return scale_length_; }
+
   api::RoadGeometryId id_;
   double linear_tolerance_{};
   double angular_tolerance_{};
+  double scale_length_{};
   std::vector<std::unique_ptr<Junction>> junctions_;
   std::vector<std::unique_ptr<BranchPoint>> branch_points_;
   api::BasicIdIndex id_index_;

automotive/maliput/multilane/test/multilane_lanes_test.cc

@@ -55,7 +55,7 @@ class MultilaneLanesParamTest : public ::testing::TestWithParam<double> {
 
 TEST_P(MultilaneLanesParamTest, FlatLineLane) {
   RoadGeometry rg(api::RoadGeometryId{"apple"},
-                  kLinearTolerance, kAngularTolerance);
+                  kLinearTolerance, kAngularTolerance, kScaleLength);
   std::unique_ptr<RoadCurve> road_curve_1 = std::make_unique<LineRoadCurve>(
       Vector2<double>(100., -75.), Vector2<double>(100., 50.), zp, zp,
       kLinearTolerance, kScaleLength, kComputationPolicy);
@@ -382,7 +382,8 @@ TEST_P(MultilaneLanesParamTest, CorkScrewLane) {
 
   RoadGeometry rg(api::RoadGeometryId{"corkscrew"},
                   kLinearTolerance,
-                  kAngularTolerance);
+                  kAngularTolerance,
+                  kScaleLength);
   Segment* s1 =
       rg.NewJunction(api::JunctionId{"j1"})
           ->NewSegment(api::SegmentId{"s1"}, std::move(road_curve),
@@ -472,7 +473,7 @@ TEST_P(MultilaneLanesParamTest, CorkScrewLane) {
 
 TEST_P(MultilaneLanesParamTest, FlatArcLane) {
   RoadGeometry rg(api::RoadGeometryId{"apple"},
-                  kLinearTolerance, kAngularTolerance);
+                  kLinearTolerance, kAngularTolerance, kScaleLength);
   const double theta0 = 0.25 * M_PI;
   const double d_theta = 1.5 * M_PI;
   const double radius = 100.;
@@ -763,7 +764,7 @@ TEST_P(MultilaneLanesParamTest, HillIntegration) {
   // reference curve is a piece of an arc with cubic elevation and zero
   // superelevation.
   RoadGeometry rg(api::RoadGeometryId{"apple"},
-                  kLinearTolerance, kAngularTolerance);
+                  kLinearTolerance, kAngularTolerance, kScaleLength);
   const double theta0 = 0.25 * M_PI;
   const double d_theta = 0.5 * M_PI;
   const double theta1 = theta0 + d_theta;
@@ -870,7 +871,7 @@ GTEST_TEST(MultilaneLanesTest, ArcLaneWithConstantSuperelevation) {
   const double kMaxHeight = 5.;
 
   RoadGeometry rg(api::RoadGeometryId{"apple"},
-                  kLinearTolerance, kAngularTolerance);
+                  kLinearTolerance, kAngularTolerance, kScaleLength);
   std::unique_ptr<RoadCurve> road_curve_1 = std::make_unique<ArcRoadCurve>(
       Vector2<double>(100., -75.), 100.0, 0.25 * M_PI, 1.5 * M_PI, zp,
       CubicPolynomial((kTheta) / (100. * 1.5 * M_PI), 0., 0., 0.),
@@ -982,7 +983,7 @@ class MultilaneMultipleLanesTest : public ::testing::Test {
 
 TEST_F(MultilaneMultipleLanesTest, MultipleLineLanes) {
   RoadGeometry rg(api::RoadGeometryId{"apple"}, kLinearTolerance,
-                  kAngularTolerance);
+                  kAngularTolerance, kScaleLength);
   std::unique_ptr<RoadCurve> road_curve = std::make_unique<LineRoadCurve>(
       Vector2<double>(100., -75.), Vector2<double>(100., 50.), zp, zp,
       kLinearTolerance, kScaleLength, kComputationPolicy);
@@ -1118,7 +1119,7 @@ TEST_F(MultilaneMultipleLanesTest, MultipleArcLanes) {
   const Vector3<double> kGeoCenter{kCenter(0), kCenter(1), 0.};
 
   RoadGeometry rg(api::RoadGeometryId{"apple"}, kLinearTolerance,
-                  kAngularTolerance);
+                  kAngularTolerance, kScaleLength);
   std::unique_ptr<RoadCurve> road_curve = std::make_unique<ArcRoadCurve>(
       kCenter, kRadius, kTheta0, kDTheta, zp, zp, kLinearTolerance,
       kScaleLength, kComputationPolicy);

automotive/maliput/multilane/test/multilane_segments_test.cc

@@ -35,8 +35,8 @@ GTEST_TEST(MultilaneSegmentsTest, MultipleLanes) {
   const ComputationPolicy kComputationPolicy{
     ComputationPolicy::kPreferAccuracy};
 
-  RoadGeometry rg(api::RoadGeometryId{"apple"}, kLinearTolerance,
-                  kAngularTolerance);
+  RoadGeometry rg(api::RoadGeometryId{"apple"},
+                  kLinearTolerance, kAngularTolerance, kScaleLength);
   std::unique_ptr<RoadCurve> road_curve_1 = std::make_unique<LineRoadCurve>(
       Vector2<double>(100., -75.), Vector2<double>(100., 50.), zp, zp,
       kLinearTolerance, kScaleLength, kComputationPolicy);

automotive/maliput/rndf/road_geometry.h

@@ -74,6 +74,12 @@ class RoadGeometry : public api::RoadGeometry {
                                      api::GeoPosition* nearest_position,
                                      double* distance) const override;
 
+  // TODO([email protected])  rndf backend is not scale_length aware in any way,
+  //                          and really this property should be consistent
+  //                          with the geometry of the curves themselves.
+  //                          This value of 1 has been picked arbitrarily.
+  double do_scale_length() const override { return 1.; }
+
   double do_linear_tolerance() const override { return linear_tolerance_; }
 
   double do_angular_tolerance() const override { return angular_tolerance_; }