From a5c087ecccc63598d234aa0b81e334265760a25f Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Tue, 7 Aug 2018 15:54:27 -0300 Subject: [PATCH] Improves explicit theta_dot treatment in Multilane's Builder. Better documentation on how theta_dot is handled, additional continuity checks in the Builder to cope with misuse and tests demonstrating proper and inappropriate usage. --- automotive/maliput/multilane/BUILD.bazel | 1 + automotive/maliput/multilane/builder.cc | 177 +++++-- automotive/maliput/multilane/builder.h | 45 +- automotive/maliput/multilane/circuit.yaml | 34 +- .../multilane/test/multilane_builder_test.cc | 444 +++++++++++++++++- 5 files changed, 628 insertions(+), 73 deletions(-) diff --git a/automotive/maliput/multilane/BUILD.bazel b/automotive/maliput/multilane/BUILD.bazel index 9caeb36f04b0..080b3d8ef8a6 100644 --- a/automotive/maliput/multilane/BUILD.bazel +++ b/automotive/maliput/multilane/BUILD.bazel @@ -112,6 +112,7 @@ drake_cc_googletest( ":builder", "//automotive/maliput/api/test_utilities", "//automotive/maliput/multilane/test_utilities", + "//common:essential", "//common/test_utilities:eigen_matrix_compare", ], ) diff --git a/automotive/maliput/multilane/builder.cc b/automotive/maliput/multilane/builder.cc index b0eccfadd97f..164c6b59f475 100644 --- a/automotive/maliput/multilane/builder.cc +++ b/automotive/maliput/multilane/builder.cc @@ -142,6 +142,14 @@ const Connection* Builder::Connect(const std::string& id, return connections_.back().get(); } +// TODO(hidmic): Support connections where the constant C in the +// continuity constraints theta_dotA - sin(betaA) = C +// and theta_dotB - sin(betaB) = C is non-zero. This +// is necessary to ensure continuity when arbitrary +// theta_dot values are provided by the user. Then, +// do not clear *explicitly* set theta_dot values +// upon Spec construction using a Connection anymore. + const Connection* Builder::Connect(const std::string& id, const LaneLayout& lane_layout, const StartLane::Spec& start_spec, @@ -164,16 +172,16 @@ const Connection* Builder::Connect(const std::string& id, -lane_layout.ref_r0() + lane_width_ * static_cast(start_spec.lane_id() - lane_layout.ref_lane()); - // Computes the displacement vector from lane point to the reference curve - // point. - const Vector3 start_lane_to_ref( - -start_lane_offset * std::cos(start_superelevation) * - std::sin(start_heading), - start_lane_offset * std::cos(start_superelevation) * - std::cos(start_heading), - start_lane_offset * std::sin(start_superelevation)); + + // Gets the rotation matrix at the start lane endpoint and translates the + // point to start_reference_position. + const api::Rotation start_rotation = api::Rotation::FromRpy( + start_superelevation, -std::atan(start_spec.endpoint().z().z_dot()), + start_heading); const Vector3 start_reference_position = - start_lane_position - start_lane_to_ref; + start_lane_position + + start_rotation.matrix() * Vector3(0., -start_lane_offset, 0.); + // Assigns the start endpoint. Endpoint start{ EndpointXy(start_reference_position.x(), start_reference_position.y(), @@ -217,14 +225,10 @@ const Connection* Builder::Connect(const std::string& id, const double curvature = std::copysign(1., arc_offset.d_theta()) / arc_offset.radius(); - // Fills arc related parameters, computes end Endpoint and creates the - // RoadCurve. - const double theta0 = start_spec.endpoint().xy().heading() - - std::copysign(M_PI / 2., arc_offset.d_theta()); // Gets the initial heading and superelevation. const double start_superelevation = start_spec.endpoint().z().theta(); - // Computes the center. + // Computes the start_lane_radius to scale z_dot. const double start_lane_offset = -lane_layout.ref_r0() + lane_width_ * @@ -233,20 +237,26 @@ const Connection* Builder::Connect(const std::string& id, start_lane_offset * std::cos(start_superelevation) * std::copysign(1., arc_offset.d_theta()); - const double cx = - start_spec.endpoint().xy().x() - std::cos(theta0) * start_lane_radius; - const double cy = - start_spec.endpoint().xy().y() - std::sin(theta0) * start_lane_radius; const double start_z_dot = start_spec.endpoint().z().z_dot() * start_lane_radius / arc_offset.radius(); + + // Gets the rotation matrix at the start lane endpoint and translates the + // point to start_reference_position. + const api::Rotation start_rotation = api::Rotation::FromRpy( + start_superelevation, -std::atan(start_z_dot), + start_spec.endpoint().xy().heading()); + const Vector3 start_lane_position{start_spec.endpoint().xy().x(), + start_spec.endpoint().xy().y(), + start_spec.endpoint().z().z()}; + const Vector3 start_reference_position = + start_lane_position + + start_rotation.matrix() * Vector3(0., -start_lane_offset, 0.); + // Assigns the start endpoint. Endpoint start{ - EndpointXy(cx + arc_offset.radius() * std::cos(theta0), - cy + arc_offset.radius() * std::sin(theta0), + EndpointXy(start_reference_position.x(), start_reference_position.y(), start_spec.endpoint().xy().heading()), - EndpointZ(start_spec.endpoint().z().z() - - start_lane_offset * std::sin(start_superelevation) * - std::copysign(1., arc_offset.d_theta()), + EndpointZ(start_reference_position.z(), start_z_dot, start_spec.endpoint().z().theta(), {})}; ComputeContinuityConstraint(curvature, &(start.get_mutable_z())); @@ -301,23 +311,108 @@ Group* Builder::MakeGroup(const std::string& id, namespace { -// Determine the heading (in xy-plane) along the centerline when -// travelling towards/into the lane, from the specified end. -double HeadingIntoLane(const api::Lane* const lane, - const api::LaneEnd::Which end) { + +// Determine the heading direction at an `r_offset` leftwards of the `lane` +// centerline when traveling outwards from the specified `end`. +Vector3 DirectionOutFromLane(const api::Lane* const lane, + const api::LaneEnd::Which end, + double r_offset) { + const Vector3 s_hat = Vector3::UnitX(); switch (end) { case api::LaneEnd::kStart: { - return lane->GetOrientation({0., 0., 0.}).yaw(); + return -lane->GetOrientation( + {0., -r_offset, 0.}).matrix() * s_hat; } case api::LaneEnd::kFinish: { - return lane->GetOrientation({lane->length(), 0., 0.}).yaw() + M_PI; + return lane->GetOrientation( + {lane->length(), r_offset, 0.}).matrix() * s_hat; } default: { DRAKE_ABORT(); } } } -} // namespace +// Checks if the heading direction of the all the given lanes at an `r_offset` +// leftwards of the centerlines when traveling outwards from their specified +// end in `set` matches the reference `direction_at_r_offset`, down to the +// given `angular_tolerance`. +bool AreLanesDirectionsWithinTolerance( + const Vector3& direction_at_r_offset, double r_offset, + const api::LaneEndSet* set, double angular_tolerance) { + DRAKE_DEMAND(set != nullptr); + + for (int i = 0; i < set->size(); ++i) { + const api::LaneEnd& le = set->get(i); + const Vector3 other_direction = + DirectionOutFromLane(le.lane, le.end, r_offset); + const double angular_deviation = + std::acos(direction_at_r_offset.dot(other_direction)); + if (angular_deviation > angular_tolerance) { + return false; + } + } + return true; +} + + +// Checks if the `lane` is G1 continuous along an `r_offset` leftwards of +// the `lane` centerline when traveling outwards from the given `end`, at the +// branch point and down to the given `angular_tolerance`. `lane` is assumed +// to have an out-of-the-lane tangent vector that is coincident with those of +// the lanes in the `parallel_set`, which in turn are anti-parallel to +// those of the lanes in the `antiparallel_set`. +bool IsLaneContinuousAtBranchPointAlongROffset( + const api::Lane* lane, const api::LaneEnd::Which end, + const api::LaneEndSet* parallel_set, + const api::LaneEndSet* antiparallel_set, + double r_offset, double angular_tolerance) { + DRAKE_DEMAND(lane != nullptr); + DRAKE_DEMAND(parallel_set != nullptr); + DRAKE_DEMAND(antiparallel_set != nullptr); + + const Vector3 direction = + DirectionOutFromLane(lane, end, r_offset); + return + (AreLanesDirectionsWithinTolerance( + direction, r_offset, + parallel_set, angular_tolerance) && + AreLanesDirectionsWithinTolerance( + -direction, r_offset, + antiparallel_set, angular_tolerance)); +} + + +// Checks if the `lane` is G1 continuous at the branchpoint on the given +// `end`, down to an `angular_tolerance`. `lane` is assumed to have an +// out-of-the-lane tangent vector that is coincident with those of the +// lanes in the `parallel_set`, which in turn are anti-parallel to those +// of the lanes in the `antiparallel_set`. +bool IsLaneContinuousAtBranchPoint( + const api::Lane* lane, const api::LaneEnd::Which end, + const api::LaneEndSet* parallel_set, + const api::LaneEndSet* antiparallel_set, + double angular_tolerance) { + DRAKE_DEMAND(lane != nullptr); + DRAKE_DEMAND(parallel_set != nullptr); + DRAKE_DEMAND(antiparallel_set != nullptr); + + constexpr double kZeroROffset{0.}; + const double s_at_branch_point = + (end == api::LaneEnd::kFinish) ? lane->length() : 0.; + const double max_r_offset = + lane->lane_bounds(s_at_branch_point).max(); + return + (IsLaneContinuousAtBranchPointAlongROffset( + lane, end, parallel_set, antiparallel_set, + kZeroROffset, angular_tolerance) && + IsLaneContinuousAtBranchPointAlongROffset( + lane, end, parallel_set, antiparallel_set, + max_r_offset, angular_tolerance)); +} + + +} // namespace + BranchPoint* Builder::FindOrCreateBranchPoint( const Endpoint& point, @@ -363,21 +458,33 @@ void Builder::AttachBranchPoint( bp->AddABranch({lane, end}); return; } + // Otherwise, assess if this new lane-end is parallel or anti-parallel to // the first lane-end. Parallel: go to same, A-side; anti-parallel: // other, B-side. Do this by examining the dot-product of the heading // vectors (rather than goofing around with cyclic angle arithmetic). - const double new_h = HeadingIntoLane(lane, end); - const api::LaneEnd old_le = bp->GetASide()->get(0); - const double old_h = HeadingIntoLane(old_le.lane, old_le.end); - if (((std::cos(new_h) * std::cos(old_h)) + - (std::sin(new_h) * std::sin(old_h))) > 0.) { + constexpr double kZeroROffset{0.}; + const Vector3 direction = + DirectionOutFromLane(lane, end, kZeroROffset); + const api::LaneEnd& old_le = bp->GetASide()->get(0); + const Vector3 old_direction = + DirectionOutFromLane(old_le.lane, old_le.end, kZeroROffset); + if (direction.dot(old_direction) > 0.) { + // Assert continuity before attaching the lane. + DRAKE_THROW_UNLESS(IsLaneContinuousAtBranchPoint( + lane, end, bp->GetASide(), bp->GetBSide(), + angular_tolerance_)); bp->AddABranch({lane, end}); } else { + // Assert continuity before attaching the lane. + DRAKE_THROW_UNLESS(IsLaneContinuousAtBranchPoint( + lane, end, bp->GetBSide(), bp->GetASide(), + angular_tolerance_)); bp->AddBBranch({lane, end}); } } + std::vector Builder::BuildConnection( const Connection* const conn, Junction* const junction, RoadGeometry* const road_geometry, diff --git a/automotive/maliput/multilane/builder.h b/automotive/maliput/multilane/builder.h index 7c0b9b6983fb..b8af8bb1e9cc 100644 --- a/automotive/maliput/multilane/builder.h +++ b/automotive/maliput/multilane/builder.h @@ -59,9 +59,9 @@ class StartReference { } /// Builds a Spec at `connection`'s `end` side with `direction` direction. - /// When `direction` == `Direction::kReverse`, `endpoint` is reversed. - /// Spec's theta_dot will be cleared so the Builder adjusts it to match - /// continuity constraints. + /// `connection`'s theta_dot at the given `end` will be ignored by the new + /// Spec so that the Builder can adjust it to match road continuity + /// constraints. Spec at(const Connection& connection, api::LaneEnd::Which end, Direction direction) const { Endpoint endpoint = end == api::LaneEnd::Which::kStart ? connection.start() @@ -121,7 +121,8 @@ class StartLane { /// Builds a Spec at `endpoint` with `direction` direction. When /// `direction` == `Direction::kReverse`, `endpoint` is reversed. - /// `endpoint`'s theta_dot must be nullopt. + /// Additionally, `endpoint`'s theta_dot must be nullopt. Otherwise + /// the Builder is unable to match road continuity constraints. Spec at(const Endpoint& endpoint, Direction direction) const { DRAKE_DEMAND(!endpoint.z().theta_dot().has_value()); return direction == Direction::kForward @@ -130,13 +131,12 @@ class StartLane { } /// Builds a Spec at `connection`'s `lane_id` lane at `end` side with - /// `direction` direction. When `direction` == `Direction::kReverse`, - /// `endpoint` is reversed. + /// `direction` direction. `connection`'s theta_dot at the given `end` + /// will be ignored by the new Spec so that the Builder can adjust it + /// to match road continuity constraints. /// /// `lane_id` must be non-negative and smaller than `connection`'s number of /// lanes. - /// Spec's theta_dot will be cleared so the Builder adjusts it to match - /// continuity constraints. Spec at(const Connection& connection, int lane_id, api::LaneEnd::Which end, Direction direction) const { DRAKE_DEMAND(lane_id >= 0 && lane_id < connection.num_lanes()); @@ -189,10 +189,9 @@ class EndReference { EndReference() = default; /// Builds a Spec at `connection`'s `end` side with `direction` direction. - /// When `direction` == `Direction::kReverse`, `end`-side endpoint's - /// EndpointZ is reversed. - /// Spec's theta_dot will be cleared so the Builder adjusts it to match - /// continuity constraints. + /// `connection`'s theta_dot at the given `end` will be ignored by the new + /// Spec so that the Builder can adjust it to match road continuity + /// constraints. Spec z_at(const Connection& connection, api::LaneEnd::Which end, Direction direction) const { EndpointZ endpoint_z = end == api::LaneEnd::Which::kStart @@ -258,13 +257,12 @@ class EndLane { } /// Builds a Spec at `connection`'s `end` side with `direction` direction. - /// When `direction` == `Direction::kReverse`, `end`-side endpoint's - /// EndpointZ is reversed. + /// `connection`'s theta_dot at the given `end` will be ignored by the new + /// Spec so that the Builder can adjust it to match road continuity + /// constraints. /// /// `lane_id` must be non-negative and smaller than `connection`'s number of /// lanes. - /// Spec's theta_dot will be cleared so the Builder adjusts it to match - /// continuity constraints. Spec z_at(const Connection& connection, int lane_id, api::LaneEnd::Which end, Direction direction) const { DRAKE_DEMAND(lane_id >= 0 && lane_id < connection.num_lanes()); @@ -279,7 +277,8 @@ class EndLane { /// Builds an Spec at `endpoint_z` with `direction` direction. /// When `direction` == `Direction::kReverse`, `endpoint_z` is reversed. - /// `endpoint_z`'s theta_dot must be nullopt. + /// Additionally, `endpoint`'s theta_dot must be nullopt. Otherwise + /// the Builder is unable to match road continuity constraints. Spec z_at(const EndpointZ& endpoint_z, Direction direction) const { DRAKE_DEMAND(!endpoint_z.theta_dot().has_value()); return direction == Direction::kForward @@ -471,6 +470,8 @@ class BuilderBase { const std::vector& connections) = 0; /// Produces a RoadGeometry, with the ID `id`. + /// @throws std::runtime_error if unable to produce a valid + /// (i.e. G1) RoadGeometry. virtual std::unique_ptr Build( const api::RoadGeometryId& id) const = 0; }; @@ -523,6 +524,16 @@ class BuilderFactoryBase { /// the Segments associated with the grouped Connections; ungrouped /// Connections each receive their own Junction. /// +/// At Endpoints where theta_dot (i.e. the rate of change of superelevation with +/// respect to an arc-length coordinate t that travels along the Connection +/// curve’s projection over the z = 0 plane) has not been explicitly specified +/// the Builder will automatically calculate theta_dot values to preserve G1 +/// continuity across Connections. theta_dot values can only be explicitly +/// specified for a Connection created from a reference curve with an explicit +/// start or end Endpoint. However, specifying an explicit theta_dot may cause +/// Builder::Build() to throw if it cannot assemble a G1 continuous +/// RoadGeometry. +/// /// Specific suffixes are used to name Maliput entities. The following list /// explains the naming convention: /// diff --git a/automotive/maliput/multilane/circuit.yaml b/automotive/maliput/multilane/circuit.yaml index 7cf7521649f5..62473102cf03 100644 --- a/automotive/maliput/multilane/circuit.yaml +++ b/automotive/maliput/multilane/circuit.yaml @@ -14,32 +14,32 @@ maliput_multilane_builder: points: a: xypoint: [0, 0, 0] - zpoint: [0, 0, 0, 0] + zpoint: [0, 0, 0] b: xypoint: [50, 5, 0] - zpoint: [0, 0, 0, 0] + zpoint: [0, 0, 0] connections: s1: lanes: [3, 0, 0] start: ["ref", "points.a.forward"] length: 50 - z_end: ["ref", [0, 0, 0, 0]] + z_end: ["ref", [0, 0, 0]] s2: lanes: [1, 0, 10] start: ["ref", "connections.s1.end.ref.forward"] arc: [20, 180] - z_end: ["ref", [0, 0, 0, 0]] + z_end: ["ref", [0, 0, 0]] s3: lanes: [1, 0, 10] start: ["ref", "connections.s2.end.ref.forward"] length: 40 - z_end: ["ref", [0, 0, 0, 0]] + z_end: ["ref", [0, 0, 0]] s4: lanes: [1, 0, 10] right_shoulder: 0 start: ["ref", "connections.s3.end.ref.forward"] length: 10 - z_end: ["ref", [0, 0, 0, 0]] + z_end: ["ref", [0, 0, 0]] s5: lanes: [2, 0, 5] start: ["ref", "connections.s4.end.ref.forward"] @@ -49,38 +49,40 @@ maliput_multilane_builder: lanes: [2, 0, -5] start: ["ref", "points.b.forward"] arc: [20, -180] - z_end: ["ref", [0, 0, 0, 0]] + z_end: ["ref", [0, 0, 0]] s7: lanes: [2, 0, -5] start: ["ref", "connections.s6.end.ref.forward"] length: 50 - z_end: ["ref", [0, 0, 0, 0]] + z_end: ["ref", [0, 0, 0]] s8: lanes: [2, 0, -5] start: ["ref", "connections.s7.end.ref.forward"] arc: [20, -180] - z_end: ["ref", [0, 0, 0, 0]] + z_end: ["ref", [0, 0, 0]] s9: lanes: [1, 0, 0] right_shoulder: 0 start: ["ref", "connections.s7.end.ref.forward"] arc: [20, 30.5] - z_end: ["ref", [0, 0, 0, 0]] + z_end: ["ref", [0, 0, 0]] s10: lanes: [1, 0, 0] start: ["ref", "connections.s9.end.ref.forward"] arc: [20, 59.5] - z_end: ["ref", [5, 0.5, -15, 0.7255]] + z_end: ["ref", [5, 0.5, -15]] # theta_dot is left unspecified for the + # Builder to properly adjust it to match + # continuity constraints. s11: lanes: [1, 0, 0] start: ["ref", "connections.s10.end.ref.forward"] arc: [20, 90] - z_end: ["ref", [10, 0, -15, 0]] + z_end: ["ref", [10, 0, -15]] s12: lanes: [1, 0, 0] start: ["ref", "connections.s11.end.ref.forward"] arc: [20, 90] - z_end: ["ref", [10, 0, 0, 0]] + z_end: ["ref", [10, 0, 0]] s13: lanes: [1, 0, 0] start: ["ref", "connections.s12.end.ref.forward"] @@ -90,18 +92,18 @@ maliput_multilane_builder: lanes: [1, 0, 0] start: ["ref", "connections.s13.end.ref.forward"] length: 30 - z_end: ["ref", [-10, 0, 0, 0]] + z_end: ["ref", [-10, 0, 0]] s15: lanes: [1, 0, 0] start: ["ref", "connections.s14.end.ref.forward"] length: 20 - z_end: ["ref", [-10, 0, 0, 0]] + z_end: ["ref", [-10, 0, 0]] s16: lanes: [1, 0, 0] left_shoulder: 0 start: ["ref", "connections.s15.end.ref.forward"] arc: [20, 90] - z_end: ["ref", [0, 0, 0, 0]] + z_end: ["ref", [0, 0, 0]] groups: g1: [s2, s6] g2: [s5, s9, s12] diff --git a/automotive/maliput/multilane/test/multilane_builder_test.cc b/automotive/maliput/multilane/test/multilane_builder_test.cc index f704119353ed..55f010b17ac2 100644 --- a/automotive/maliput/multilane/test/multilane_builder_test.cc +++ b/automotive/maliput/multilane/test/multilane_builder_test.cc @@ -5,6 +5,7 @@ #include #include #include +#include #include #include @@ -13,6 +14,7 @@ #include "drake/automotive/maliput/api/test_utilities/check_id_indexing.h" #include "drake/automotive/maliput/api/test_utilities/maliput_types_compare.h" #include "drake/automotive/maliput/multilane/test_utilities/multilane_types_compare.h" +#include "drake/common/drake_copyable.h" namespace drake { namespace maliput { @@ -21,11 +23,6 @@ namespace { using Which = api::LaneEnd::Which; -// TODO(maddog-tri) Test use of Endpoint::reverse() with non-zero theta and -// theta_dot, checking that the orientation of the resulting -// road surface is continuous, off the centerline, at the -// branch-point where two connections connect - // StartReference::Spec using an Endpoint. GTEST_TEST(StartReferenceSpecTest, Endpoint) { const Endpoint point{{1., 2., 3.}, {4., 5., 6., 7.}}; @@ -268,6 +265,7 @@ GTEST_TEST(MultilaneBuilderTest, ProperConnections) { EXPECT_EQ(arc_connection->computation_policy(), kComputationPolicy); } + GTEST_TEST(MultilaneBuilderTest, Fig8) { const double kLaneWidth = 4.; const api::HBounds kElevationBounds(0., 5.); @@ -538,6 +536,7 @@ GTEST_TEST(MultilaneBuilderTest, QuadRing) { EXPECT_TRUE(api::test::CheckIdIndexing(rg.get())); }; + // Holds common properties for reference-to-reference curve primitive tests. class MultilaneBuilderReferenceCurvePrimitivesTest : public ::testing::Test { protected: @@ -1044,6 +1043,441 @@ TEST_F(MultilaneBuilderLaneToLanePrimitivesTest, ElevatedEndArcSegment) { EXPECT_EQ(rg->num_branch_points(), 2 * kNumLanes); } +namespace { + +// An encapsulated Multilane road build procedure. +class BuildProcedure { + public: + DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(BuildProcedure); + + // Constructs a named build procedure. + explicit BuildProcedure(const std::string& name) + : name_(name) { + } + + virtual ~BuildProcedure() = default; + + // Applies procedure to the given @p builder. + virtual void ApplyTo(Builder* builder) const = 0; + + // Asserts that the procedure was properly applied + // to the given @p road_geometry. + virtual void AssertProperConstruction( + const api::RoadGeometry& road_geometry) const = 0; + + // Returns procedure's name. + const std::string& name() const { return name_; } + + private: + // Procedure name. + const std::string name_; +}; + +// An stream operator overload for shared references to +// BuildProcedure instances. Useful for gtest to print. +std::ostream& operator<<( + std::ostream& os, const std::shared_ptr& proc) { + return os << proc->name(); +} + +// An encapsulated Multilane road turn build procedure. +class TurnBuildProcedure : public BuildProcedure { + public: + DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(TurnBuildProcedure); + + // Constructs a named build procedure for a turn. + // @param name Name for the build procedure. + // @param start_endpoint Turn's start endpoint. + // @param straight_conn_length Turn straight section's length. + // @param curved_conn_radius Turn curved section's arc radius. + // @param curved_conn_angular_delta Turn curved section's arc + // subtended angle. + explicit TurnBuildProcedure( + const std::string& name, const Endpoint& start_endpoint, + double straight_conn_length, double curved_conn_radius, + double curved_conn_angular_delta) + : BuildProcedure(name), start_endpoint_(start_endpoint), + straight_conn_length_(straight_conn_length), + curved_conn_radius_(curved_conn_radius), + curved_conn_angular_delta_(curved_conn_angular_delta) {} + + void AssertProperConstruction( + const api::RoadGeometry& road_geometry) const override { + ASSERT_EQ(road_geometry.num_junctions(), 2); + const api::Junction* straight_junction = + road_geometry.ById().GetJunction(api::JunctionId{ + "j:" + std::string{kStraightConnName}}); + const api::Junction* curved_junction = + road_geometry.ById().GetJunction(api::JunctionId{ + "j:" + std::string{kCurvedConnName}}); + ASSERT_EQ(curved_junction->num_segments(), 1); + ASSERT_EQ(straight_junction->num_segments(), 1); + const api::Segment* straight_segment = straight_junction->segment(0); + const api::Segment* curved_segment = curved_junction->segment(0); + ASSERT_EQ(curved_segment->num_lanes(), kLaneNum); + ASSERT_EQ(straight_segment->num_lanes(), kLaneNum); + const api::Lane* straight_lane = straight_segment->lane(0); + const api::Lane* curved_lane = curved_segment->lane(kLaneNum - 1); + + const api::LaneEndSet* straight_lane_branches = + straight_lane->GetOngoingBranches(api::LaneEnd::kStart); + ASSERT_EQ(straight_lane_branches->size(), 1); + ASSERT_EQ(straight_lane_branches->get(0).lane, curved_lane); + ASSERT_EQ(straight_lane_branches->get(0).end, api::LaneEnd::kStart); + const api::LaneEndSet* curved_lane_branches = + curved_lane->GetOngoingBranches(api::LaneEnd::kStart); + ASSERT_EQ(curved_lane_branches->size(), 1); + ASSERT_EQ(curved_lane_branches->get(0).lane, straight_lane); + ASSERT_EQ(curved_lane_branches->get(0).end, api::LaneEnd::kStart); + + const double kS{0.}; + const double kH{0.}; + const api::RBounds lbounds = straight_lane->lane_bounds(kS); + const double lane_width = lbounds.max() - lbounds.min(); + for (double r = lbounds.min(); r <= lbounds.max(); r += lane_width/10) { + // Since the curved lane heading at the origin is opposite to that of + // the straight lane by construction, the r-offset sign is reversed. + EXPECT_TRUE(api::test::IsGeoPositionClose( + straight_lane->ToGeoPosition({kS, r, kH}), + curved_lane->ToGeoPosition({kS, -r, kH}), + road_geometry.linear_tolerance())) + << "Position discontinuity at r = " << r; + // Since the curved lane heading at the origin is opposite to that of + // the straight lane by construction, the resulting orientation is + // rotated pi radians about the h-axis. + const api::Rotation rrotation = curved_lane->GetOrientation({kS, -r, kH}); + const Quaternion pi_rotation( + AngleAxis(M_PI, Vector3::UnitZ())); + EXPECT_TRUE(api::test::IsRotationClose( + straight_lane->GetOrientation({kS, r, kH}), + // Applies a pi radians rotation around the h-axis to the curved + // lane orientation (i.e. apply an intrinsic pi radians rotation + // about the lane local z-axis, effectively post-multiplying). + api::Rotation::FromQuat(rrotation.quat() * pi_rotation), + road_geometry.angular_tolerance())) + << " Orientation discontinuity at r = " << r; + } + } + + protected: + // Returns straight connection line spec. + LineOffset line_offset() const { + return LineOffset(straight_conn_length_); + } + + // Returns curved connection arc spec. + ArcOffset arc_offset() const { + return ArcOffset(curved_conn_radius_, curved_conn_angular_delta_); + } + + // Returns turn start endpoint. + const Endpoint& start_endpoint() const { return start_endpoint_; } + + const int kLaneNum{2}; + const double kLeftShoulder{2.}; + const double kRightShoulder{2.}; + const EndpointZ kFlatZ{0., 0., 0., {}}; + const std::string kStraightConnName{"straight"}; + const std::string kCurvedConnName{"curved"}; + + private: + const Endpoint start_endpoint_; + const double straight_conn_length_; + const double curved_conn_radius_; + const double curved_conn_angular_delta_; +}; + +// An encapsulated Multilane turn build procedure that +// constructs it by specifying the straight connection by +// it's reference curve starting at the endpoint and the +// curved connection by reversing the former connection +// start endpoint. +class TurnUsingRefToRConn + : public TurnBuildProcedure { + public: + DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(TurnUsingRefToRConn); + using TurnBuildProcedure::TurnBuildProcedure; + + void ApplyTo(Builder* builder) const override { + const int ref_lane{0}; + const double lane_width{builder->get_lane_width()}; + const double total_width{kLaneNum * lane_width}; + // Locates the reference curve along the centerline of + // the Connection segment. + const LaneLayout lane_layout{kLeftShoulder, kRightShoulder, kLaneNum, + ref_lane, -(total_width - lane_width) / 2.}; + + const Connection* straight_conn = builder->Connect( + kStraightConnName, lane_layout, + StartReference().at(start_endpoint(), Direction::kForward), + line_offset(), EndReference().z_at(kFlatZ, Direction::kForward)); + ASSERT_TRUE(straight_conn != nullptr); + + const Connection* curved_conn = builder->Connect( + kCurvedConnName, lane_layout, StartReference().at( + *straight_conn, api::LaneEnd::kStart, Direction::kReverse), + arc_offset(), EndReference().z_at(kFlatZ, Direction::kForward)); + ASSERT_TRUE(curved_conn != nullptr); + } +}; + +// An encapsulated Multilane turn build procedure that +// constructs it by specifying the straight connection by +// it's reference curve starting at the endpoint and the +// curved connection by reversing the endpoint. +class TurnUsingRefToRRef + : public TurnBuildProcedure { + public: + DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(TurnUsingRefToRRef); + using TurnBuildProcedure::TurnBuildProcedure; + + void ApplyTo(Builder* builder) const override { + const int ref_lane{0}; + const double lane_width{builder->get_lane_width()}; + const double total_width{kLaneNum * lane_width}; + // Locates the reference curve along the centerline of + // the Connection segment. + const LaneLayout lane_layout{kLeftShoulder, kRightShoulder, kLaneNum, + ref_lane, -(total_width - lane_width) / 2.}; + + const Connection* straight_conn = builder->Connect( + kStraightConnName, lane_layout, + StartReference().at(start_endpoint(), Direction::kForward), + line_offset(), EndReference().z_at(kFlatZ, Direction::kForward)); + ASSERT_TRUE(straight_conn != nullptr); + + const Connection* curved_conn = builder->Connect( + kCurvedConnName, lane_layout, StartReference().at( + start_endpoint(), Direction::kReverse), + arc_offset(), EndReference().z_at(kFlatZ, Direction::kForward)); + ASSERT_TRUE(curved_conn != nullptr); + } +}; + +// An encapsulated Multilane turn build procedure that +// constructs it by specifying the straight connection by +// it's reference curve starting at the endpoint and the +// curved connection by reversing the former connection +// reference lane at its start end. +class TurnUsingStraightRefToCurvedRLane + : public TurnBuildProcedure { + public: + DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(TurnUsingStraightRefToCurvedRLane); + using TurnBuildProcedure::TurnBuildProcedure; + + void ApplyTo(Builder* builder) const override { + const int ref_lane{0}; + const double lane_width{builder->get_lane_width()}; + const double total_width{kLaneNum * lane_width}; + // Locates the reference curve on the left (for an observer + // looking in the direction of increasing s coordinates) of + // the Connection segment, half a lane width away from its + // centerline. + const LaneLayout lane_layout{ + kLeftShoulder, kRightShoulder, kLaneNum, + ref_lane, -(total_width + lane_width) / 2.}; + + const Connection* straight_conn = builder->Connect( + kStraightConnName, lane_layout, + StartReference().at(start_endpoint(), Direction::kForward), + line_offset(), EndReference().z_at(kFlatZ, Direction::kForward)); + ASSERT_TRUE(straight_conn != nullptr); + + const int other_lane{kLaneNum - 1}; + + const Connection* curved_conn = builder->Connect( + kCurvedConnName, lane_layout, StartLane(ref_lane).at( + *straight_conn, other_lane, api::LaneEnd::kStart, + Direction::kReverse), arc_offset(), + EndLane(ref_lane).z_at(kFlatZ, Direction::kForward)); + ASSERT_TRUE(curved_conn != nullptr); + } +}; + +// An encapsulated Multilane turn build procedure that +// constructs it by specifying the curved connection by +// it's reference curve starting at the endpoint and the +// straight connection by reversing the former connection +// reference lane at its start end. +class TurnUsingCurvedRefToStraightRLane + : public TurnBuildProcedure { + public: + DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(TurnUsingCurvedRefToStraightRLane); + using TurnBuildProcedure::TurnBuildProcedure; + + void ApplyTo(Builder* builder) const override { + const int ref_lane{0}; + const double lane_width{builder->get_lane_width()}; + const double total_width{kLaneNum * lane_width}; + // Locates the reference curve on the left (for an observer + // looking in the direction of increasing s coordinates) of + // the Connection segment, half a lane width away from its + // centerline. + const LaneLayout lane_layout{ + kLeftShoulder, kRightShoulder, kLaneNum, + ref_lane, -(total_width + lane_width) / 2.}; + + const Connection* curved_conn = builder->Connect( + kCurvedConnName, lane_layout, + StartReference().at(start_endpoint(), Direction::kForward), + arc_offset(), EndReference().z_at(kFlatZ, Direction::kForward)); + ASSERT_TRUE(curved_conn != nullptr); + + const int other_lane{kLaneNum - 1}; + + const Connection* straight_conn = builder->Connect( + kStraightConnName, lane_layout, StartLane(ref_lane).at( + *curved_conn, other_lane, api::LaneEnd::kStart, + Direction::kReverse), line_offset(), + EndLane(ref_lane).z_at(kFlatZ, Direction::kForward)); + ASSERT_TRUE(straight_conn != nullptr); + } +}; + + +} // namespace + +using TestWithBuildProcedure = + ::testing::TestWithParam>; + +// A test fixture for Multilane build procedures. +class MultilaneBuildProcedureTest : public TestWithBuildProcedure { + protected: + // Applies provided build procedure param to the given @p builder. + void ApplyProcedureTo(Builder* builder) { + ASSERT_TRUE(GetParam() != nullptr); + GetParam()->ApplyTo(builder); + } + + // Asserts that the given @p road_geometry was properly built by + // the provided build procedure param. + void AssertProcedureResult(const api::RoadGeometry& road_geometry) { + ASSERT_TRUE(GetParam() != nullptr); + GetParam()->AssertProperConstruction(road_geometry); + } + + const double kLaneWidth{4.}; + const api::HBounds kElevationBounds{0., 5.}; + const double kLinearTolerance{0.01}; + const double kAngularTolerance{0.01 * M_PI}; + const double kScaleLength{1.0}; + const ComputationPolicy kComputationPolicy{ + ComputationPolicy::kPreferAccuracy}; +}; + +// Test fixture parameterized by BuildProcedure instances that are +// expected to produce G1 continuity issues. See Builder class +// documentation for further reference. +class MultilaneDiscontinuousBuildProcedureTest + : public MultilaneBuildProcedureTest { +}; + +// Tests that the Builder refuses to issue G1 discontinuous road +// geometries. +TEST_P(MultilaneDiscontinuousBuildProcedureTest, ThrowingUponBuild) { + Builder builder(kLaneWidth, kElevationBounds, + kLinearTolerance, kAngularTolerance, + kScaleLength, kComputationPolicy, + std::make_unique()); + ApplyProcedureTo(&builder); + EXPECT_THROW({ + const std::unique_ptr road_geometry = + builder.Build(api::RoadGeometryId{"bad_road"}); + }, + std::runtime_error); +} + +// Returns a collection of BuildProcedure instances that make use +// of reversed semantics in a way that would introduce G1 continuity +// issues. +std::vector> +ListBadReverseBuildProcedures() { + constexpr double kStraightConnLength{50.}; + constexpr double kCurvedConnRadius{50.}; + constexpr double kCurvedConnAngularDelta{M_PI/2.}; + const Endpoint kBadStartEndpoint{{0., 0., M_PI/2.}, {0., 0., 0.3, 0.1}}; + std::vector> procedures; + procedures.push_back( + std::make_unique( + "BankedTurnUsingRefToRConnWithBadThetaDot", + kBadStartEndpoint, kStraightConnLength, + kCurvedConnRadius, kCurvedConnAngularDelta)); + procedures.push_back( + std::make_unique( + "BankedTurnUsingRefToRRefWithBadThetaDot", + kBadStartEndpoint, kStraightConnLength, + kCurvedConnRadius, kCurvedConnAngularDelta)); + procedures.push_back( + std::make_unique( + "BankedTurnUsingStraightRefToCurvedRLaneWithBadThetaDot", + kBadStartEndpoint, kStraightConnLength, + kCurvedConnRadius, kCurvedConnAngularDelta)); + procedures.push_back( + std::make_unique( + "BankedTurnUsingCurvedRefToStraightRLaneWithBadThetaDot", + kBadStartEndpoint, kStraightConnLength, + kCurvedConnRadius, kCurvedConnAngularDelta)); + return procedures; +} + +INSTANTIATE_TEST_CASE_P(BadReverseConnectionTest, + MultilaneDiscontinuousBuildProcedureTest, + ::testing::ValuesIn(ListBadReverseBuildProcedures())); + +// Test fixture parameterized by BuildProcedure instances, that +// are expected to produce G1 continuous roads. +class MultilaneContinuousBuildProcedureTest + : public MultilaneBuildProcedureTest { +}; + +// Tests (exhaustively) that the Builder achieves G1 continuity. +TEST_P(MultilaneContinuousBuildProcedureTest, Continuity) { + Builder builder(kLaneWidth, kElevationBounds, + kLinearTolerance, kAngularTolerance, + kScaleLength, kComputationPolicy, + std::make_unique()); + ApplyProcedureTo(&builder); + const std::unique_ptr road_geometry = + builder.Build(api::RoadGeometryId{"good_road"}); + AssertProcedureResult(*road_geometry); +} + +// Returns a collection of BuildProcedure instances that make use +// of reversed semantics in a way that produces G1 continuous roads. +std::vector> +ListGoodReverseBuildProcedures() { + constexpr double kStraightConnLength{50.}; + constexpr double kCurvedConnRadius{50.}; + constexpr double kCurvedConnAngularDelta{M_PI/2.}; + const Endpoint kStartEndpoint{{0., 0., M_PI/2.}, {0., 0., 0.3, {}}}; + std::vector> procedures; + procedures.push_back( + std::make_unique( + "BankedTurnUsingRefToRConnWithoutThetaDot", + kStartEndpoint, kStraightConnLength, + kCurvedConnRadius, kCurvedConnAngularDelta)); + procedures.push_back( + std::make_unique( + "BankedTurnUsingRefToRRefWithoutThetaDot", + kStartEndpoint, kStraightConnLength, + kCurvedConnRadius, kCurvedConnAngularDelta)); + procedures.push_back( + std::make_unique( + "BankedTurnUsingStraightRefToCurvedRLaneWithoutThetaDot", + kStartEndpoint, kStraightConnLength, + kCurvedConnRadius, kCurvedConnAngularDelta)); + procedures.push_back( + std::make_unique( + "BankedTurnUsingCurvedRefToStraightRLaneWithoutThetaDot", + kStartEndpoint, kStraightConnLength, + kCurvedConnRadius, kCurvedConnAngularDelta)); + return procedures; +} + +INSTANTIATE_TEST_CASE_P(GoodReverseConnectionTest, + MultilaneContinuousBuildProcedureTest, + ::testing::ValuesIn(ListGoodReverseBuildProcedures())); + // Holds Lane IDs for each Lane, both at the start and end of the Lane. struct BranchPointLaneIds { std::vector start_a_side;