From f48213aa11f4d99068cf5204f9976da1a4dd5549 Mon Sep 17 00:00:00 2001 From: Agustin Alba Chicar Date: Wed, 3 Mar 2021 16:30:38 -0300 Subject: [PATCH 01/10] Adds RoadGeometry::internal_inertial_frame_translation() A translation vector between a backend specific Inertial Frame and the maliput Inertial Frame. It is used to set the maliput Inertial Frame at a specific position, such as the one that a lat-long-elev ECEF conversion can provide. - Modifies the api - Modifies geometry_base to enable easy to implement backend traslation. --- maliput/include/maliput/api/road_geometry.h | 8 ++++ ...brute_force_find_road_positions_strategy.h | 4 +- maliput/include/maliput/geometry_base/lane.h | 45 +++++++++++++++++++ .../maliput/geometry_base/road_geometry.h | 12 ++++- maliput/include/maliput/test_utilities/mock.h | 11 ++++- .../maliput/test_utilities/mock_geometry.h | 8 +++- maliput/src/geometry_base/lane.cc | 24 ++++++++++ maliput/src/test_utilities/mock.cc | 8 +++- maliput/test/api/lane_test.cc | 13 +++--- maliput/test/api/road_network_test.cc | 4 +- .../brute_force_find_road_positions_test.cc | 27 ++++++----- .../test/geometry_base/geometry_base_test.cc | 30 +++++++++---- 12 files changed, 160 insertions(+), 34 deletions(-) diff --git a/maliput/include/maliput/api/road_geometry.h b/maliput/include/maliput/api/road_geometry.h index 248f3ea26..5d459c3c6 100644 --- a/maliput/include/maliput/api/road_geometry.h +++ b/maliput/include/maliput/api/road_geometry.h @@ -14,6 +14,7 @@ #include "maliput/api/type_specific_identifier.h" #include "maliput/common/maliput_copyable.h" #include "maliput/common/maliput_throw.h" +#include "maliput/math/vector.h" namespace maliput { namespace api { @@ -176,6 +177,11 @@ class RoadGeometry { return DoSampleAheadWaypoints(lane_s_route, path_length_sampling_rate); } + /// @return maliput's Internal Inertial Frame translation with respect to + /// another arbitrary Inertial Frame with the same exact characteristics of + /// maliput's. + math::Vector3 internal_inertial_frame_translation() const { return do_internal_inertial_frame_translation(); } + protected: RoadGeometry() = default; @@ -210,6 +216,8 @@ class RoadGeometry { virtual std::vector DoSampleAheadWaypoints(const LaneSRoute&, double path_length_sampling_rate) const; + + virtual math::Vector3 do_internal_inertial_frame_translation() const = 0; ///@} }; diff --git a/maliput/include/maliput/geometry_base/brute_force_find_road_positions_strategy.h b/maliput/include/maliput/geometry_base/brute_force_find_road_positions_strategy.h index 44d33ec0c..c01782710 100644 --- a/maliput/include/maliput/geometry_base/brute_force_find_road_positions_strategy.h +++ b/maliput/include/maliput/geometry_base/brute_force_find_road_positions_strategy.h @@ -12,8 +12,8 @@ namespace geometry_base { /// that exhaustively calls `ToLanePosition()` on all lanes and checks /// _distance_ with `radius`. /// -/// Maliput backends could avoid implementing a custom implementation that knows -/// about the geometry internals by forwarding calls to this function. +/// @note Maliput backends could avoid implementing a custom implementation that +/// knows about the geometry internals by forwarding calls to this function. /// /// @param rg The RoadGeometry over all these operations are performed. It /// must not be nullptr. diff --git a/maliput/include/maliput/geometry_base/lane.h b/maliput/include/maliput/geometry_base/lane.h index e5b64de93..dcc68520c 100644 --- a/maliput/include/maliput/geometry_base/lane.h +++ b/maliput/include/maliput/geometry_base/lane.h @@ -4,6 +4,7 @@ #include "maliput/api/branch_point.h" #include "maliput/api/lane.h" +#include "maliput/api/lane_data.h" #include "maliput/api/segment.h" #include "maliput/common/maliput_copyable.h" #include "maliput/common/passkey.h" @@ -87,6 +88,50 @@ class Lane : public api::Lane { std::optional DoGetDefaultBranch(const api::LaneEnd::Which which_end) const override; + // @{ + // Forwards the call to DoToBackendInertialPosition(lane_pos) and translates + // the result to account for api::RoadGeometry::internal_inertial_frame_translation(). + // + // @note Backend implementations should not override this method and should + // override DoToBackendInertialPosition() when making use of this frame + // conversion. + // @note Backend implementations should override this method and should not + // override DoToBackendInertialPostion() when they prefer to handle the + // entire mapping process. + virtual api::InertialPosition DoToInertialPosition(const api::LanePosition& lane_pos) const override; + + // Maps @p lane_pos into the backend (maliput's) Inertial Frame which does not + // include api::RoadGeometry::internal_inertial_frame_translation() + // translation. + // + // This method implementation @throws maliput::common::assertion_error as it + // must never be called as is. @see DoToInertialPosition() docstring to + // understand when to override each method accordingly. + virtual api::InertialPosition DoToBackendInertialPosition(const api::LanePosition& lane_pos) const; + // @} + + // @{ + // Translates @p inertial_pos with api::RoadGeometry::internal_inertial_frame_translation() + // and forwards the result to DoToBackendLanePosition(). + // + // @note Backend implementations should not override this method and should + // override DoToBackendLanePosition() when making use of this frame + // conversion. + // @note Backend implementations should override this method and should not + // override DoToBackendLanePosition() when they prefer to handle the + // entire mapping process. + virtual api::LanePositionResult DoToLanePosition(const api::InertialPosition& inertial_pos) const override; + + // Maps @p inertial_pos (measured in the backend (maliput's) Inertial Frame + // which does not include api::RoadGeometry::internal_inertial_frame_translation() + // translation) into this Lane Frame. + // + // This method implementation @throws maliput::common::assertion_error as it + // must never be called as is. @see DoToLanePosition() docstring to + // understand when to override each method accordingly. + virtual api::LanePositionResult DoToBackendLanePosition(const api::InertialPosition& inertial_pos) const; + // @} + const api::LaneId id_; const api::Segment* segment_{}; int index_{-1}; diff --git a/maliput/include/maliput/geometry_base/road_geometry.h b/maliput/include/maliput/geometry_base/road_geometry.h index 1fc6ac92d..4022443ca 100644 --- a/maliput/include/maliput/geometry_base/road_geometry.h +++ b/maliput/include/maliput/geometry_base/road_geometry.h @@ -13,6 +13,7 @@ #include "maliput/common/maliput_throw.h" #include "maliput/geometry_base/branch_point.h" #include "maliput/geometry_base/junction.h" +#include "maliput/math/vector.h" namespace maliput { namespace geometry_base { @@ -46,14 +47,18 @@ class RoadGeometry : public api::RoadGeometry { /// @param linear_tolerance the linear tolerance /// @param angular_tolerance the angular tolerance /// @param scale_length the scale length + /// @param internal_inertial_frame_translation the internal inertial frame + /// origin translation /// /// @throws maliput::common::assertion_error if either `linear_tolerance` or /// `angular_tolerance` or `scale_length` is non-positive. - RoadGeometry(const api::RoadGeometryId& id, double linear_tolerance, double angular_tolerance, double scale_length) + RoadGeometry(const api::RoadGeometryId& id, double linear_tolerance, double angular_tolerance, double scale_length, + const math::Vector3& internal_inertial_frame_translation) : id_(id), linear_tolerance_(linear_tolerance), angular_tolerance_(angular_tolerance), - scale_length_(scale_length) { + scale_length_(scale_length), + internal_inertial_frame_translation_(internal_inertial_frame_translation) { MALIPUT_THROW_UNLESS(linear_tolerance_ > 0.); MALIPUT_THROW_UNLESS(angular_tolerance_ > 0.); MALIPUT_THROW_UNLESS(scale_length_ > 0.); @@ -122,10 +127,13 @@ class RoadGeometry : public api::RoadGeometry { double do_scale_length() const override { return scale_length_; } + math::Vector3 do_internal_inertial_frame_translation() const override { return internal_inertial_frame_translation_; } + api::RoadGeometryId id_; double linear_tolerance_{}; double angular_tolerance_{}; double scale_length_{}; + math::Vector3 internal_inertial_frame_translation_{}; std::vector> junctions_; std::vector> branch_points_; api::BasicIdIndex id_index_; diff --git a/maliput/include/maliput/test_utilities/mock.h b/maliput/include/maliput/test_utilities/mock.h index 329c5bbe3..d286a39c4 100644 --- a/maliput/include/maliput/test_utilities/mock.h +++ b/maliput/include/maliput/test_utilities/mock.h @@ -51,6 +51,7 @@ struct RoadGeometryContiguityBuildFlags { bool add_angular_mismatch{false}; double linear_tolerance{0}; double angular_tolerance{0}; + math::Vector3 internal_inertial_frame_translation{0., 0., 0.}; }; /// Holds RoadRulebook contiguity build configuration. @@ -301,8 +302,12 @@ class MockRoadGeometry : public RoadGeometry { public: MALIPUT_NO_COPY_NO_MOVE_NO_ASSIGN(MockRoadGeometry) MockRoadGeometry(const RoadGeometryId& id) : id_(id) {} - MockRoadGeometry(const RoadGeometryId& id, const double& linear_tolerance, const double& angular_tolerance) - : id_(id), linear_tolerance_(linear_tolerance), angular_tolerance_(angular_tolerance) {} + MockRoadGeometry(const RoadGeometryId& id, const double& linear_tolerance, const double& angular_tolerance, + const math::Vector3& internal_inertial_frame_translation) + : id_(id), + linear_tolerance_(linear_tolerance), + angular_tolerance_(angular_tolerance), + internal_inertial_frame_translation_(internal_inertial_frame_translation) {} void add_junction(std::unique_ptr junction) { junctions_.push_back(std::move(junction)); } void set_start_bp(std::unique_ptr start_bp) { start_bp_ = std::move(start_bp); } @@ -341,11 +346,13 @@ class MockRoadGeometry : public RoadGeometry { double do_linear_tolerance() const override { return linear_tolerance_; } double do_angular_tolerance() const override { return angular_tolerance_; } double do_scale_length() const override { return 0; } + math::Vector3 do_internal_inertial_frame_translation() const override { return internal_inertial_frame_translation_; } MockIdIndex mock_id_index_; RoadGeometryId id_; double linear_tolerance_{0}; double angular_tolerance_{0}; + math::Vector3 internal_inertial_frame_translation_{}; std::vector> junctions_; std::unique_ptr start_bp_; std::unique_ptr end_bp_; diff --git a/maliput/include/maliput/test_utilities/mock_geometry.h b/maliput/include/maliput/test_utilities/mock_geometry.h index b4896a659..f8b95baa9 100644 --- a/maliput/include/maliput/test_utilities/mock_geometry.h +++ b/maliput/include/maliput/test_utilities/mock_geometry.h @@ -9,6 +9,7 @@ #include "maliput/geometry_base/lane.h" #include "maliput/geometry_base/road_geometry.h" #include "maliput/geometry_base/segment.h" +#include "maliput/math/vector.h" namespace maliput { namespace geometry_base { @@ -43,12 +44,15 @@ class MockRoadGeometry : public geometry_base::RoadGeometry { /// @param linear_tolerance the linear tolerance /// @param angular_tolerance the angular tolerance /// @param scale_length the scale length + /// @param internal_inertial_frame_translation the internal inertial frame + /// origin translation /// /// @throws std::exception if either `linear_tolerance` or /// `angular_tolerance` or `scale_length` is non-positive. MockRoadGeometry(const api::RoadGeometryId& id, double linear_tolerance, double angular_tolerance, - double scale_length) - : geometry_base::RoadGeometry(id, linear_tolerance, angular_tolerance, scale_length) {} + double scale_length, const math::Vector3& internal_inertial_frame_translation) + : geometry_base::RoadGeometry(id, linear_tolerance, angular_tolerance, scale_length, + internal_inertial_frame_translation) {} private: api::RoadPositionResult DoToRoadPosition(const api::InertialPosition& inertial_position, diff --git a/maliput/src/geometry_base/lane.cc b/maliput/src/geometry_base/lane.cc index abd1c3f3b..57ad4d08c 100644 --- a/maliput/src/geometry_base/lane.cc +++ b/maliput/src/geometry_base/lane.cc @@ -83,5 +83,29 @@ std::optional Lane::DoGetDefaultBranch(api::LaneEnd::Which which_e return GetBranchPoint(which_end)->GetDefaultBranch({this, which_end}); } +api::InertialPosition Lane::DoToInertialPosition(const api::LanePosition& lane_pos) const { + const api::InertialPosition backend_inertial_position = DoToBackendInertialPosition(lane_pos); + return api::InertialPosition::FromXyz(backend_inertial_position.xyz() + + segment()->junction()->road_geometry()->internal_inertial_frame_translation()); +} + +api::InertialPosition Lane::DoToBackendInertialPosition(const api::LanePosition& lane_pos) const { + MALIPUT_THROW_MESSAGE( + "Unimplemented method. Please check the documentation of " + "maliput::geometry_base::Lane::DoToInertialPosition()."); +} + +api::LanePositionResult Lane::DoToLanePosition(const api::InertialPosition& inertial_pos) const { + const api::InertialPosition backend_inertial_position = api::InertialPosition::FromXyz( + inertial_pos.xyz() - segment()->junction()->road_geometry()->internal_inertial_frame_translation()); + return DoToBackendLanePosition(backend_inertial_position); +} + +api::LanePositionResult Lane::DoToBackendLanePosition(const api::InertialPosition& inertial_pos) const { + MALIPUT_THROW_MESSAGE( + "Unimplemented method. Please check the documentation of " + "maliput::geometry_base::Lane::DoToLanePosition()."); +} + } // namespace geometry_base } // namespace maliput diff --git a/maliput/src/test_utilities/mock.cc b/maliput/src/test_utilities/mock.cc index 449d676a6..03bff0661 100644 --- a/maliput/src/test_utilities/mock.cc +++ b/maliput/src/test_utilities/mock.cc @@ -72,6 +72,8 @@ class MockOneLaneRoadGeometry final : public RoadGeometry { double do_linear_tolerance() const override { return 0; } double do_angular_tolerance() const override { return 0; } double do_scale_length() const override { return 0; } + math::Vector3 do_internal_inertial_frame_translation() const override { return math::Vector3{0., 0., 0.}; } + MockOneLaneIdIndex mock_id_index_; }; @@ -533,9 +535,10 @@ std::unique_ptr CreateRoadGeometry(const RoadGeometryBuildFlags& b // Return values when ToLanePosition method is called are hardcoded. std::unique_ptr CreateTwoLanesRoadGeometry() { constexpr double kArbitrary{1.}; + const math::Vector3 kZero{0., 0., 0.}; const LanePositionResult lane_position_result_a{{10., 20., 30.}, {12., 89., 1.}, 0.5}; const LanePositionResult lane_position_result_b{{40., 50., 60.}, {50., 1., 45.}, 30.}; - auto rg = std::make_unique(RoadGeometryId("road_geometry"), kArbitrary, kArbitrary); + auto rg = std::make_unique(RoadGeometryId("road_geometry"), kArbitrary, kArbitrary, kZero); auto junction_a = std::make_unique(JunctionId("junction_a")); auto junction_b = std::make_unique(JunctionId("junction_b")); auto segment_a = std::make_unique(SegmentId("segment_a")); @@ -564,7 +567,8 @@ std::unique_ptr CreateTwoLanesRoadGeometry() { std::unique_ptr CreateMockContiguousRoadGeometry(const RoadGeometryContiguityBuildFlags& build_flags) { // Creates the road geometry. auto rg = std::make_unique(RoadGeometryId("mock"), build_flags.linear_tolerance, - build_flags.angular_tolerance); + build_flags.angular_tolerance, + build_flags.internal_inertial_frame_translation); // Creates the Junctions. auto junction_a = std::make_unique(JunctionId("mock_a")); junction_a->set_road_geometry(rg.get()); diff --git a/maliput/test/api/lane_test.cc b/maliput/test/api/lane_test.cc index 53fd498f7..a45a4404a 100644 --- a/maliput/test/api/lane_test.cc +++ b/maliput/test/api/lane_test.cc @@ -30,6 +30,7 @@ class LaneTest : public ::testing::Test { const double s{1}; const double r{2}; const double h{3}; + const math::Vector3 internal_inertial_frame_translation{0., 0., 0.}; }; class LaneMock final : public geometry_base::test::MockLane { @@ -55,8 +56,8 @@ class LaneMock final : public geometry_base::test::MockLane { class RoadGeometryMock final : public geometry_base::test::MockRoadGeometry { public: explicit RoadGeometryMock(const api::RoadGeometryId& id, double linear_tolerance, double angular_tolerance, - double scale_length) - : MockRoadGeometry(id, linear_tolerance, angular_tolerance, scale_length) {} + double scale_length, const math::Vector3& internal_inertial_frame_translation) + : MockRoadGeometry(id, linear_tolerance, angular_tolerance, scale_length, internal_inertial_frame_translation) {} void set_lanes(std::vector lanes) { lanes_.assign(lanes.begin(), lanes.end()); } std::vector get_lanes() { return lanes_; } @@ -65,8 +66,10 @@ class RoadGeometryMock final : public geometry_base::test::MockRoadGeometry { }; std::unique_ptr MakeFullRoadGeometry(const api::RoadGeometryId& id, double linear_tolerance, - double angular_tolerance, double scale_length) { - auto road_geometry = std::make_unique(id, linear_tolerance, angular_tolerance, scale_length); + double angular_tolerance, double scale_length, + const math::Vector3& internal_inertial_frame_translation) { + auto road_geometry = std::make_unique(id, linear_tolerance, angular_tolerance, scale_length, + internal_inertial_frame_translation); std::vector lanes; auto lane0 = std::make_unique(api::LaneId("lane0")); @@ -99,7 +102,7 @@ TEST_F(LaneTest, Contains) { const LanePosition false_lane_position = LanePosition(s + kLaneLength + linear_tolerance, r, h); auto rg = MakeFullRoadGeometry(api::RoadGeometryId("mock_road_geometry"), linear_tolerance, angular_tolerance, - scale_length); + scale_length, internal_inertial_frame_translation); const std::vector lanes = rg.get()->get_lanes(); diff --git a/maliput/test/api/road_network_test.cc b/maliput/test/api/road_network_test.cc index b8cae3e01..021c93d3b 100644 --- a/maliput/test/api/road_network_test.cc +++ b/maliput/test/api/road_network_test.cc @@ -32,6 +32,7 @@ class RoadNetworkTest : public ::testing::Test { const double linear_tolerance{1.}; const double angular_tolerance{1.}; const double scale_length{1.}; + const math::Vector3 internal_inertial_frame_translation{0., 0., 0.}; virtual void SetUp() { road_geometry_ = test::CreateRoadGeometry(); @@ -175,7 +176,8 @@ TEST_F(RoadNetworkTest, TestMemberMethodAccess) { TEST_F(RoadNetworkTest, Contains) { auto mock_road_geometry = std::make_unique( - api::RoadGeometryId{"mock_road_geometry"}, linear_tolerance, angular_tolerance, scale_length); + api::RoadGeometryId{"mock_road_geometry"}, linear_tolerance, angular_tolerance, scale_length, + internal_inertial_frame_translation); auto mock_lane = std::make_unique(api::LaneId{"mock_lane"}); auto mock_segment = std::make_unique(api::SegmentId{"mock_segment"}); auto mock_junction = std::make_unique(api::JunctionId{"mock_junction"}); diff --git a/maliput/test/geometry_base/brute_force_find_road_positions_test.cc b/maliput/test/geometry_base/brute_force_find_road_positions_test.cc index 42cdcb887..24f67727f 100644 --- a/maliput/test/geometry_base/brute_force_find_road_positions_test.cc +++ b/maliput/test/geometry_base/brute_force_find_road_positions_test.cc @@ -34,6 +34,7 @@ class BruteForceTest : public ::testing::Test { const double linear_tolerance{1.}; const double angular_tolerance{1.}; const double scale_length{1.}; + const math::Vector3 internal_inertial_frame_translation{0., 0., 0.}; const double kZeroTolerance{0.}; }; @@ -80,8 +81,8 @@ class LaneMock final : public MockLane { class RoadGeometryMock final : public MockRoadGeometry { public: explicit RoadGeometryMock(const api::RoadGeometryId& id, double linear_tolerance, double angular_tolerance, - double scale_length) - : MockRoadGeometry(id, linear_tolerance, angular_tolerance, scale_length) {} + double scale_length, const math::Vector3& internal_inertial_frame_translation) + : MockRoadGeometry(id, linear_tolerance, angular_tolerance, scale_length, internal_inertial_frame_translation) {} void set_lanes(std::vector lanes) { lanes_.assign(lanes.begin(), lanes.end()); } std::vector get_lanes() { return lanes_; } @@ -91,9 +92,11 @@ class RoadGeometryMock final : public MockRoadGeometry { std::unique_ptr MakeOneLaneRoadGeometry(const api::RoadGeometryId& id, double linear_tolerance, double angular_tolerance, double scale_length, + const math::Vector3& internal_inertial_frame_translation, double lane_distance) { std::vector lanes; - auto road_geometry = std::make_unique(id, linear_tolerance, angular_tolerance, scale_length); + auto road_geometry = std::make_unique(id, linear_tolerance, angular_tolerance, scale_length, + internal_inertial_frame_translation); auto lane0 = std::make_unique(api::LaneId("lane0"), lane_distance); auto segment0 = std::make_unique(api::SegmentId("segment0")); auto junction0 = std::make_unique(api::JunctionId("junction0")); @@ -108,8 +111,10 @@ std::unique_ptr MakeOneLaneRoadGeometry(const api::RoadGeometr } std::unique_ptr MakeFullRoadGeometry(const api::RoadGeometryId& id, double linear_tolerance, - double angular_tolerance, double scale_length) { - auto road_geometry = std::make_unique(id, linear_tolerance, angular_tolerance, scale_length); + double angular_tolerance, double scale_length, + const math::Vector3& internal_inertial_frame_translation) { + auto road_geometry = std::make_unique(id, linear_tolerance, angular_tolerance, scale_length, + internal_inertial_frame_translation); std::vector lanes; auto lane0 = std::make_unique(api::LaneId("lane0"), kDistance); @@ -138,8 +143,8 @@ std::unique_ptr MakeFullRoadGeometry(const api::RoadGeometryId } TEST_F(BruteForceTest, LaneInAndOutRadius) { - auto rg = - MakeOneLaneRoadGeometry(api::RoadGeometryId("dut"), linear_tolerance, angular_tolerance, scale_length, kDistance); + auto rg = MakeOneLaneRoadGeometry(api::RoadGeometryId("dut"), linear_tolerance, angular_tolerance, scale_length, + internal_inertial_frame_translation, kDistance); std::vector results = BruteForceFindRoadPositionsStrategy(rg.get(), api::InertialPosition(1., 2., 3.), kRadius); @@ -158,7 +163,7 @@ TEST_F(BruteForceTest, LaneInAndOutRadius) { EXPECT_NEAR(results.front().distance, kExpectedDistance, kZeroTolerance); rg = MakeOneLaneRoadGeometry(api::RoadGeometryId("dut"), linear_tolerance, angular_tolerance, scale_length, - 2. * kDistance); + internal_inertial_frame_translation, 2. * kDistance); results = BruteForceFindRoadPositionsStrategy(rg.get(), api::InertialPosition(1., 2., 3.), kRadius); EXPECT_TRUE(results.empty()); @@ -171,13 +176,15 @@ TEST_F(BruteForceTest, NullRoadGeometry) { TEST_F(BruteForceTest, NegativeRadius) { std::unique_ptr rg = - MakeFullRoadGeometry(api::RoadGeometryId("dut"), linear_tolerance, angular_tolerance, scale_length); + MakeFullRoadGeometry(api::RoadGeometryId("dut"), linear_tolerance, angular_tolerance, scale_length, + internal_inertial_frame_translation); EXPECT_THROW(BruteForceFindRoadPositionsStrategy(rg.get(), api::InertialPosition(0., 0., 0.), -1.), common::assertion_error); } TEST_F(BruteForceTest, AllLanesCalled) { - auto rg = MakeFullRoadGeometry(api::RoadGeometryId("dut"), linear_tolerance, angular_tolerance, scale_length); + auto rg = MakeFullRoadGeometry(api::RoadGeometryId("dut"), linear_tolerance, angular_tolerance, scale_length, + internal_inertial_frame_translation); const std::vector lanes = rg.get()->get_lanes(); diff --git a/maliput/test/geometry_base/geometry_base_test.cc b/maliput/test/geometry_base/geometry_base_test.cc index a82fa3c2d..d53786d59 100644 --- a/maliput/test/geometry_base/geometry_base_test.cc +++ b/maliput/test/geometry_base/geometry_base_test.cc @@ -209,20 +209,26 @@ GTEST_TEST(GeometryBaseRoadGeometryTest, BasicConstruction) { const double kValidLinearTolerance = 7.0; const double kValidAngularTolerance = 99.0; const double kValidScaleLength = 0.5; + const math::Vector3 kInternalInertialFrameTranslation{1., 2., 3.}; + // Tolerance/scale-length values must be positive. - EXPECT_THROW(MockRoadGeometry(api::RoadGeometryId("dut"), 0., kValidAngularTolerance, kValidScaleLength), + EXPECT_THROW(MockRoadGeometry(api::RoadGeometryId("dut"), 0., kValidAngularTolerance, kValidScaleLength, + kInternalInertialFrameTranslation), std::exception); - EXPECT_THROW(MockRoadGeometry(api::RoadGeometryId("dut"), kValidLinearTolerance, 0., kValidScaleLength), + EXPECT_THROW(MockRoadGeometry(api::RoadGeometryId("dut"), kValidLinearTolerance, 0., kValidScaleLength, + kInternalInertialFrameTranslation), std::exception); - EXPECT_THROW(MockRoadGeometry(api::RoadGeometryId("dut"), kValidLinearTolerance, kValidAngularTolerance, 0.), + EXPECT_THROW(MockRoadGeometry(api::RoadGeometryId("dut"), kValidLinearTolerance, kValidAngularTolerance, 0., + kInternalInertialFrameTranslation), std::exception); const MockRoadGeometry dut(api::RoadGeometryId("dut"), kValidLinearTolerance, kValidAngularTolerance, - kValidScaleLength); + kValidScaleLength, kInternalInertialFrameTranslation); 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); + EXPECT_EQ(dut.internal_inertial_frame_translation(), kInternalInertialFrameTranslation); } GTEST_TEST(GeometryBaseRoadGeometryTest, AddingBranchPoints) { @@ -232,7 +238,10 @@ GTEST_TEST(GeometryBaseRoadGeometryTest, AddingBranchPoints) { MockBranchPoint* raw_branch_point1 = branch_point1.get(); const double kSomePositiveDouble = 7.0; - MockRoadGeometry dut(api::RoadGeometryId("dut"), kSomePositiveDouble, kSomePositiveDouble, kSomePositiveDouble); + const math::Vector3 kInternalInertialFrameTranslation{0., 0., 0.}; + + MockRoadGeometry dut(api::RoadGeometryId("dut"), kSomePositiveDouble, kSomePositiveDouble, kSomePositiveDouble, + kInternalInertialFrameTranslation); // Test the empty dut. EXPECT_EQ(dut.num_branch_points(), 0); @@ -264,7 +273,10 @@ GTEST_TEST(GeometryBaseRoadGeometryTest, AddingJunctions) { MockJunction* raw_junction1 = junction1.get(); const double kSomePositiveDouble = 7.0; - MockRoadGeometry dut(api::RoadGeometryId("dut"), kSomePositiveDouble, kSomePositiveDouble, kSomePositiveDouble); + const math::Vector3 kInternalInertialFrameTranslation{0., 0., 0.}; + + MockRoadGeometry dut(api::RoadGeometryId("dut"), kSomePositiveDouble, kSomePositiveDouble, kSomePositiveDouble, + kInternalInertialFrameTranslation); // Test the empty dut. EXPECT_EQ(dut.num_junctions(), 0); @@ -301,7 +313,9 @@ class GeometryBaseRoadGeometryIndexingTest : public ::testing::Test { void SetUp() override { constexpr double kArbitrary{1.}; - road_geometry_ = std::make_unique(api::RoadGeometryId("dut"), kArbitrary, kArbitrary, kArbitrary); + const math::Vector3 kInternalInertialFrameTranslation{0., 0., 0.}; + road_geometry_ = std::make_unique(api::RoadGeometryId("dut"), kArbitrary, kArbitrary, kArbitrary, + kInternalInertialFrameTranslation); } std::unique_ptr road_geometry_; @@ -360,7 +374,7 @@ TEST_F(GeometryBaseRoadGeometryIndexingTest, Test) { } GTEST_TEST(GeometryBaseRoadGeometryTest, UnimplementedMethods) { - const MockRoadGeometry dut(api::RoadGeometryId("dut"), 1., 1., 1.); + const MockRoadGeometry dut(api::RoadGeometryId("dut"), 1., 1., 1., {0, 0, 0}); // Ensure that the not-actually-implemented methods throw an exception. EXPECT_THROW(dut.ToRoadPosition(api::InertialPosition(), std::nullopt), std::exception); EXPECT_THROW(dut.FindRoadPositions(api::InertialPosition(), 1.), std::exception); From bfb79ca10dfcbf6d03f03d2ab49fc836048b6166 Mon Sep 17 00:00:00 2001 From: Agustin Alba Chicar Date: Fri, 5 Mar 2021 09:12:28 -0300 Subject: [PATCH 02/10] Updates the API to use Inertial Frame and Backend Frame. --- maliput/include/maliput/api/road_geometry.h | 14 ++-- maliput/include/maliput/geometry_base/lane.h | 77 ++++++++++++------- .../maliput/geometry_base/road_geometry.h | 14 ++-- maliput/include/maliput/test_utilities/mock.h | 12 +-- .../maliput/test_utilities/mock_geometry.h | 8 +- maliput/src/geometry_base/lane.cc | 24 ++++-- maliput/src/test_utilities/mock.cc | 4 +- maliput/test/api/lane_test.cc | 13 ++-- maliput/test/api/road_network_test.cc | 4 +- .../brute_force_find_road_positions_test.cc | 23 +++--- .../test/geometry_base/geometry_base_test.cc | 24 +++--- 11 files changed, 127 insertions(+), 90 deletions(-) diff --git a/maliput/include/maliput/api/road_geometry.h b/maliput/include/maliput/api/road_geometry.h index 5d459c3c6..98d08889d 100644 --- a/maliput/include/maliput/api/road_geometry.h +++ b/maliput/include/maliput/api/road_geometry.h @@ -177,10 +177,14 @@ class RoadGeometry { return DoSampleAheadWaypoints(lane_s_route, path_length_sampling_rate); } - /// @return maliput's Internal Inertial Frame translation with respect to - /// another arbitrary Inertial Frame with the same exact characteristics of - /// maliput's. - math::Vector3 internal_inertial_frame_translation() const { return do_internal_inertial_frame_translation(); } + /// The Backend Frame is an inertial frame similar to the Inertial Frame that + /// differ one from another by an isometric transformation. This method + /// returns the translation vector between both frames. + /// + /// @return maliput's Inertial Frame to Backend Frame translation vector. + math::Vector3 inertial_to_backend_frame_translation() const { return do_inertial_to_backend_frame_translation(); } + + // TODO(#400): Add RollPitchYaw inertial_to_backend_frame_rotation() const. protected: RoadGeometry() = default; @@ -217,7 +221,7 @@ class RoadGeometry { virtual std::vector DoSampleAheadWaypoints(const LaneSRoute&, double path_length_sampling_rate) const; - virtual math::Vector3 do_internal_inertial_frame_translation() const = 0; + virtual math::Vector3 do_inertial_to_backend_frame_translation() const = 0; ///@} }; diff --git a/maliput/include/maliput/geometry_base/lane.h b/maliput/include/maliput/geometry_base/lane.h index dcc68520c..60ef119ad 100644 --- a/maliput/include/maliput/geometry_base/lane.h +++ b/maliput/include/maliput/geometry_base/lane.h @@ -89,47 +89,66 @@ class Lane : public api::Lane { std::optional DoGetDefaultBranch(const api::LaneEnd::Which which_end) const override; // @{ - // Forwards the call to DoToBackendInertialPosition(lane_pos) and translates - // the result to account for api::RoadGeometry::internal_inertial_frame_translation(). + // Forwards the call to DoToBackendPosition(lane_pos) and translates + // the result to account for api::RoadGeometry::inertial_to_backend_frame_translation(). // - // @note Backend implementations should not override this method and should - // override DoToBackendInertialPosition() when making use of this frame - // conversion. - // @note Backend implementations should override this method and should not - // override DoToBackendInertialPostion() when they prefer to handle the - // entire mapping process. + // @note When the Inertial and Backend Frames differ, the backend + // implementation should (1) not override this method and (2) override + // DoToBackendPosition(). + // @note When the Inertial and Backend Frames do not differ, the backend + // implementation should (1) override this method and (2) do not + // override DoToBackendPosition(). virtual api::InertialPosition DoToInertialPosition(const api::LanePosition& lane_pos) const override; - // Maps @p lane_pos into the backend (maliput's) Inertial Frame which does not - // include api::RoadGeometry::internal_inertial_frame_translation() - // translation. + // Maps @p lane_pos into the Backend Frame. // - // This method implementation @throws maliput::common::assertion_error as it - // must never be called as is. @see DoToInertialPosition() docstring to - // understand when to override each method accordingly. - virtual api::InertialPosition DoToBackendInertialPosition(const api::LanePosition& lane_pos) const; + // @note This method implementation @throws maliput::common::assertion_error + // as it should never be called. @see DoToInertialPosition() + // docstring to understand when to override each method accordingly. + // + // @return The mapped @p lane_pos into the Backend Frame. + virtual math::Vector3 DoToBackendPosition(const api::LanePosition& lane_pos) const; // @} // @{ // Translates @p inertial_pos with api::RoadGeometry::internal_inertial_frame_translation() - // and forwards the result to DoToBackendLanePosition(). + // and calls DoToLanePositionFromBackendPosition(). The returned values are + // used to build an api::LanePositionResult. Note that `nearest_backend_pos` + // is converted back to the Inertial Frame before returning the final result. // - // @note Backend implementations should not override this method and should - // override DoToBackendLanePosition() when making use of this frame - // conversion. - // @note Backend implementations should override this method and should not - // override DoToBackendLanePosition() when they prefer to handle the - // entire mapping process. + // @note When the Inertial and Backend Frames differ, the backend + // implementation should (1) not override this method and (2) override + // DoToLanePositionFromBackendPosition(). + // @note When the Inertial and Backend Frames do not differ, the backend + // implementation should (1) override this method and (2) do not + // override DoToLanePositionFromBackendPosition(). virtual api::LanePositionResult DoToLanePosition(const api::InertialPosition& inertial_pos) const override; - // Maps @p inertial_pos (measured in the backend (maliput's) Inertial Frame - // which does not include api::RoadGeometry::internal_inertial_frame_translation() - // translation) into this Lane Frame. + // Maps @p backend_pos (measured in the Backend Frame) into this Lane Frame. + // + // @note This method implementation @throws maliput::common::assertion_error + // as it should never be called. @see DoToLanePosition() + // docstring to understand when to override each method accordingly. + // + // @param backend_pos The Backend Frame coordinate to map into this Lane + // Frame. + // @param lane_pos The candidate LanePosition within the Lane' segment- + // bounds which is closest to an Inertial Frame position supplied to + // api::Lane::ToLanePosition() and then converted into the Backend + // Frame (measured by the Cartesian metric in the world frame). It must + // not be nullptr. + // @param nearest_backend_pos The position that exactly corresponds to + // @p lane_pos. It must not be nullptr. + // @param distance The Cartesian distance between `nearest_position` and the + // Inertial Frame position supplied to Lane::ToLanePosition() and then + // converted into the Backend Frame. It must not be nullptr. + // + // @throws When any of @p backend_pos, @p lane_pos, or @p distance is + // nullptr. // - // This method implementation @throws maliput::common::assertion_error as it - // must never be called as is. @see DoToLanePosition() docstring to - // understand when to override each method accordingly. - virtual api::LanePositionResult DoToBackendLanePosition(const api::InertialPosition& inertial_pos) const; + // @return The @p lane_pos, @p nearest_backend_pos and @p distance. + virtual void DoToLanePositionFromBackendPosition(const math::Vector3& backend_pos, api::LanePosition* lane_pos, + math::Vector3* nearest_backend_pos, double* distance) const; // @} const api::LaneId id_; diff --git a/maliput/include/maliput/geometry_base/road_geometry.h b/maliput/include/maliput/geometry_base/road_geometry.h index 4022443ca..d61c48174 100644 --- a/maliput/include/maliput/geometry_base/road_geometry.h +++ b/maliput/include/maliput/geometry_base/road_geometry.h @@ -47,18 +47,18 @@ class RoadGeometry : public api::RoadGeometry { /// @param linear_tolerance the linear tolerance /// @param angular_tolerance the angular tolerance /// @param scale_length the scale length - /// @param internal_inertial_frame_translation the internal inertial frame - /// origin translation + /// @param inertial_to_backend_frame_translation the Inertial Frame to Backend + /// Frame translation vector /// /// @throws maliput::common::assertion_error if either `linear_tolerance` or /// `angular_tolerance` or `scale_length` is non-positive. RoadGeometry(const api::RoadGeometryId& id, double linear_tolerance, double angular_tolerance, double scale_length, - const math::Vector3& internal_inertial_frame_translation) + const math::Vector3& inertial_to_backend_frame_translation) : id_(id), linear_tolerance_(linear_tolerance), angular_tolerance_(angular_tolerance), scale_length_(scale_length), - internal_inertial_frame_translation_(internal_inertial_frame_translation) { + inertial_to_backend_frame_translation_(inertial_to_backend_frame_translation) { MALIPUT_THROW_UNLESS(linear_tolerance_ > 0.); MALIPUT_THROW_UNLESS(angular_tolerance_ > 0.); MALIPUT_THROW_UNLESS(scale_length_ > 0.); @@ -127,13 +127,15 @@ class RoadGeometry : public api::RoadGeometry { double do_scale_length() const override { return scale_length_; } - math::Vector3 do_internal_inertial_frame_translation() const override { return internal_inertial_frame_translation_; } + math::Vector3 do_inertial_to_backend_frame_translation() const override { + return inertial_to_backend_frame_translation_; + } api::RoadGeometryId id_; double linear_tolerance_{}; double angular_tolerance_{}; double scale_length_{}; - math::Vector3 internal_inertial_frame_translation_{}; + math::Vector3 inertial_to_backend_frame_translation_{}; std::vector> junctions_; std::vector> branch_points_; api::BasicIdIndex id_index_; diff --git a/maliput/include/maliput/test_utilities/mock.h b/maliput/include/maliput/test_utilities/mock.h index d286a39c4..1b6f0b266 100644 --- a/maliput/include/maliput/test_utilities/mock.h +++ b/maliput/include/maliput/test_utilities/mock.h @@ -51,7 +51,7 @@ struct RoadGeometryContiguityBuildFlags { bool add_angular_mismatch{false}; double linear_tolerance{0}; double angular_tolerance{0}; - math::Vector3 internal_inertial_frame_translation{0., 0., 0.}; + math::Vector3 inertial_to_backend_frame_translation{0., 0., 0.}; }; /// Holds RoadRulebook contiguity build configuration. @@ -303,11 +303,11 @@ class MockRoadGeometry : public RoadGeometry { MALIPUT_NO_COPY_NO_MOVE_NO_ASSIGN(MockRoadGeometry) MockRoadGeometry(const RoadGeometryId& id) : id_(id) {} MockRoadGeometry(const RoadGeometryId& id, const double& linear_tolerance, const double& angular_tolerance, - const math::Vector3& internal_inertial_frame_translation) + const math::Vector3& inertial_to_backend_frame_translation) : id_(id), linear_tolerance_(linear_tolerance), angular_tolerance_(angular_tolerance), - internal_inertial_frame_translation_(internal_inertial_frame_translation) {} + inertial_to_backend_frame_translation_(inertial_to_backend_frame_translation) {} void add_junction(std::unique_ptr junction) { junctions_.push_back(std::move(junction)); } void set_start_bp(std::unique_ptr start_bp) { start_bp_ = std::move(start_bp); } @@ -346,13 +346,15 @@ class MockRoadGeometry : public RoadGeometry { double do_linear_tolerance() const override { return linear_tolerance_; } double do_angular_tolerance() const override { return angular_tolerance_; } double do_scale_length() const override { return 0; } - math::Vector3 do_internal_inertial_frame_translation() const override { return internal_inertial_frame_translation_; } + math::Vector3 do_inertial_to_backend_frame_translation() const override { + return inertial_to_backend_frame_translation_; + } MockIdIndex mock_id_index_; RoadGeometryId id_; double linear_tolerance_{0}; double angular_tolerance_{0}; - math::Vector3 internal_inertial_frame_translation_{}; + math::Vector3 inertial_to_backend_frame_translation_{}; std::vector> junctions_; std::unique_ptr start_bp_; std::unique_ptr end_bp_; diff --git a/maliput/include/maliput/test_utilities/mock_geometry.h b/maliput/include/maliput/test_utilities/mock_geometry.h index f8b95baa9..849627f50 100644 --- a/maliput/include/maliput/test_utilities/mock_geometry.h +++ b/maliput/include/maliput/test_utilities/mock_geometry.h @@ -44,15 +44,15 @@ class MockRoadGeometry : public geometry_base::RoadGeometry { /// @param linear_tolerance the linear tolerance /// @param angular_tolerance the angular tolerance /// @param scale_length the scale length - /// @param internal_inertial_frame_translation the internal inertial frame - /// origin translation + /// @param inertial_to_backend_frame_translation the Inertial to Backend Frame + /// translation /// /// @throws std::exception if either `linear_tolerance` or /// `angular_tolerance` or `scale_length` is non-positive. MockRoadGeometry(const api::RoadGeometryId& id, double linear_tolerance, double angular_tolerance, - double scale_length, const math::Vector3& internal_inertial_frame_translation) + double scale_length, const math::Vector3& inertial_to_backend_frame_translation) : geometry_base::RoadGeometry(id, linear_tolerance, angular_tolerance, scale_length, - internal_inertial_frame_translation) {} + inertial_to_backend_frame_translation) {} private: api::RoadPositionResult DoToRoadPosition(const api::InertialPosition& inertial_position, diff --git a/maliput/src/geometry_base/lane.cc b/maliput/src/geometry_base/lane.cc index 57ad4d08c..9f770834d 100644 --- a/maliput/src/geometry_base/lane.cc +++ b/maliput/src/geometry_base/lane.cc @@ -84,24 +84,32 @@ std::optional Lane::DoGetDefaultBranch(api::LaneEnd::Which which_e } api::InertialPosition Lane::DoToInertialPosition(const api::LanePosition& lane_pos) const { - const api::InertialPosition backend_inertial_position = DoToBackendInertialPosition(lane_pos); - return api::InertialPosition::FromXyz(backend_inertial_position.xyz() + - segment()->junction()->road_geometry()->internal_inertial_frame_translation()); + const math::Vector3 backend_pos = DoToBackendPosition(lane_pos); + return api::InertialPosition::FromXyz( + backend_pos + segment()->junction()->road_geometry()->inertial_to_backend_frame_translation()); } -api::InertialPosition Lane::DoToBackendInertialPosition(const api::LanePosition& lane_pos) const { +math::Vector3 Lane::DoToBackendPosition(const api::LanePosition& lane_pos) const { MALIPUT_THROW_MESSAGE( "Unimplemented method. Please check the documentation of " "maliput::geometry_base::Lane::DoToInertialPosition()."); } api::LanePositionResult Lane::DoToLanePosition(const api::InertialPosition& inertial_pos) const { - const api::InertialPosition backend_inertial_position = api::InertialPosition::FromXyz( - inertial_pos.xyz() - segment()->junction()->road_geometry()->internal_inertial_frame_translation()); - return DoToBackendLanePosition(backend_inertial_position); + const math::Vector3 inertial_to_backend_frame_translation = + segment()->junction()->road_geometry()->inertial_to_backend_frame_translation(); + const math::Vector3 backend_pos = inertial_pos.xyz() - inertial_to_backend_frame_translation; + + api::LanePosition lane_pos{}; + math::Vector3 nearest_backend_pos{}; + double distance{}; + DoToLanePositionFromBackendPosition(backend_pos, &lane_pos, &nearest_backend_pos, &distance); + return {lane_pos, api::InertialPosition::FromXyz(nearest_backend_pos + inertial_to_backend_frame_translation), + distance}; } -api::LanePositionResult Lane::DoToBackendLanePosition(const api::InertialPosition& inertial_pos) const { +void Lane::DoToLanePositionFromBackendPosition(const math::Vector3& backend_pos, api::LanePosition* lane_position, + math::Vector3* nearest_backend_pos, double* distance) const { MALIPUT_THROW_MESSAGE( "Unimplemented method. Please check the documentation of " "maliput::geometry_base::Lane::DoToLanePosition()."); diff --git a/maliput/src/test_utilities/mock.cc b/maliput/src/test_utilities/mock.cc index 03bff0661..6ed2d1e84 100644 --- a/maliput/src/test_utilities/mock.cc +++ b/maliput/src/test_utilities/mock.cc @@ -72,7 +72,7 @@ class MockOneLaneRoadGeometry final : public RoadGeometry { double do_linear_tolerance() const override { return 0; } double do_angular_tolerance() const override { return 0; } double do_scale_length() const override { return 0; } - math::Vector3 do_internal_inertial_frame_translation() const override { return math::Vector3{0., 0., 0.}; } + math::Vector3 do_inertial_to_backend_frame_translation() const override { return math::Vector3{0., 0., 0.}; } MockOneLaneIdIndex mock_id_index_; }; @@ -568,7 +568,7 @@ std::unique_ptr CreateMockContiguousRoadGeometry(const RoadGeometr // Creates the road geometry. auto rg = std::make_unique(RoadGeometryId("mock"), build_flags.linear_tolerance, build_flags.angular_tolerance, - build_flags.internal_inertial_frame_translation); + build_flags.inertial_to_backend_frame_translation); // Creates the Junctions. auto junction_a = std::make_unique(JunctionId("mock_a")); junction_a->set_road_geometry(rg.get()); diff --git a/maliput/test/api/lane_test.cc b/maliput/test/api/lane_test.cc index a45a4404a..9b6bfadfc 100644 --- a/maliput/test/api/lane_test.cc +++ b/maliput/test/api/lane_test.cc @@ -30,7 +30,7 @@ class LaneTest : public ::testing::Test { const double s{1}; const double r{2}; const double h{3}; - const math::Vector3 internal_inertial_frame_translation{0., 0., 0.}; + const math::Vector3 inertial_to_backend_frame_translation{0., 0., 0.}; }; class LaneMock final : public geometry_base::test::MockLane { @@ -56,8 +56,9 @@ class LaneMock final : public geometry_base::test::MockLane { class RoadGeometryMock final : public geometry_base::test::MockRoadGeometry { public: explicit RoadGeometryMock(const api::RoadGeometryId& id, double linear_tolerance, double angular_tolerance, - double scale_length, const math::Vector3& internal_inertial_frame_translation) - : MockRoadGeometry(id, linear_tolerance, angular_tolerance, scale_length, internal_inertial_frame_translation) {} + double scale_length, const math::Vector3& inertial_to_backend_frame_translation) + : MockRoadGeometry(id, linear_tolerance, angular_tolerance, scale_length, inertial_to_backend_frame_translation) { + } void set_lanes(std::vector lanes) { lanes_.assign(lanes.begin(), lanes.end()); } std::vector get_lanes() { return lanes_; } @@ -67,9 +68,9 @@ class RoadGeometryMock final : public geometry_base::test::MockRoadGeometry { std::unique_ptr MakeFullRoadGeometry(const api::RoadGeometryId& id, double linear_tolerance, double angular_tolerance, double scale_length, - const math::Vector3& internal_inertial_frame_translation) { + const math::Vector3& inertial_to_backend_frame_translation) { auto road_geometry = std::make_unique(id, linear_tolerance, angular_tolerance, scale_length, - internal_inertial_frame_translation); + inertial_to_backend_frame_translation); std::vector lanes; auto lane0 = std::make_unique(api::LaneId("lane0")); @@ -102,7 +103,7 @@ TEST_F(LaneTest, Contains) { const LanePosition false_lane_position = LanePosition(s + kLaneLength + linear_tolerance, r, h); auto rg = MakeFullRoadGeometry(api::RoadGeometryId("mock_road_geometry"), linear_tolerance, angular_tolerance, - scale_length, internal_inertial_frame_translation); + scale_length, inertial_to_backend_frame_translation); const std::vector lanes = rg.get()->get_lanes(); diff --git a/maliput/test/api/road_network_test.cc b/maliput/test/api/road_network_test.cc index 021c93d3b..0aecdb6ca 100644 --- a/maliput/test/api/road_network_test.cc +++ b/maliput/test/api/road_network_test.cc @@ -32,7 +32,7 @@ class RoadNetworkTest : public ::testing::Test { const double linear_tolerance{1.}; const double angular_tolerance{1.}; const double scale_length{1.}; - const math::Vector3 internal_inertial_frame_translation{0., 0., 0.}; + const math::Vector3 inertial_to_backend_frame_translation{0., 0., 0.}; virtual void SetUp() { road_geometry_ = test::CreateRoadGeometry(); @@ -177,7 +177,7 @@ TEST_F(RoadNetworkTest, TestMemberMethodAccess) { TEST_F(RoadNetworkTest, Contains) { auto mock_road_geometry = std::make_unique( api::RoadGeometryId{"mock_road_geometry"}, linear_tolerance, angular_tolerance, scale_length, - internal_inertial_frame_translation); + inertial_to_backend_frame_translation); auto mock_lane = std::make_unique(api::LaneId{"mock_lane"}); auto mock_segment = std::make_unique(api::SegmentId{"mock_segment"}); auto mock_junction = std::make_unique(api::JunctionId{"mock_junction"}); diff --git a/maliput/test/geometry_base/brute_force_find_road_positions_test.cc b/maliput/test/geometry_base/brute_force_find_road_positions_test.cc index 24f67727f..42aecfadc 100644 --- a/maliput/test/geometry_base/brute_force_find_road_positions_test.cc +++ b/maliput/test/geometry_base/brute_force_find_road_positions_test.cc @@ -34,7 +34,7 @@ class BruteForceTest : public ::testing::Test { const double linear_tolerance{1.}; const double angular_tolerance{1.}; const double scale_length{1.}; - const math::Vector3 internal_inertial_frame_translation{0., 0., 0.}; + const math::Vector3 inertial_to_backend_frame_translation{0., 0., 0.}; const double kZeroTolerance{0.}; }; @@ -81,8 +81,9 @@ class LaneMock final : public MockLane { class RoadGeometryMock final : public MockRoadGeometry { public: explicit RoadGeometryMock(const api::RoadGeometryId& id, double linear_tolerance, double angular_tolerance, - double scale_length, const math::Vector3& internal_inertial_frame_translation) - : MockRoadGeometry(id, linear_tolerance, angular_tolerance, scale_length, internal_inertial_frame_translation) {} + double scale_length, const math::Vector3& inertial_to_backend_frame_translation) + : MockRoadGeometry(id, linear_tolerance, angular_tolerance, scale_length, inertial_to_backend_frame_translation) { + } void set_lanes(std::vector lanes) { lanes_.assign(lanes.begin(), lanes.end()); } std::vector get_lanes() { return lanes_; } @@ -92,11 +93,11 @@ class RoadGeometryMock final : public MockRoadGeometry { std::unique_ptr MakeOneLaneRoadGeometry(const api::RoadGeometryId& id, double linear_tolerance, double angular_tolerance, double scale_length, - const math::Vector3& internal_inertial_frame_translation, + const math::Vector3& inertial_to_backend_frame_translation, double lane_distance) { std::vector lanes; auto road_geometry = std::make_unique(id, linear_tolerance, angular_tolerance, scale_length, - internal_inertial_frame_translation); + inertial_to_backend_frame_translation); auto lane0 = std::make_unique(api::LaneId("lane0"), lane_distance); auto segment0 = std::make_unique(api::SegmentId("segment0")); auto junction0 = std::make_unique(api::JunctionId("junction0")); @@ -112,9 +113,9 @@ std::unique_ptr MakeOneLaneRoadGeometry(const api::RoadGeometr std::unique_ptr MakeFullRoadGeometry(const api::RoadGeometryId& id, double linear_tolerance, double angular_tolerance, double scale_length, - const math::Vector3& internal_inertial_frame_translation) { + const math::Vector3& inertial_to_backend_frame_translation) { auto road_geometry = std::make_unique(id, linear_tolerance, angular_tolerance, scale_length, - internal_inertial_frame_translation); + inertial_to_backend_frame_translation); std::vector lanes; auto lane0 = std::make_unique(api::LaneId("lane0"), kDistance); @@ -144,7 +145,7 @@ std::unique_ptr MakeFullRoadGeometry(const api::RoadGeometryId TEST_F(BruteForceTest, LaneInAndOutRadius) { auto rg = MakeOneLaneRoadGeometry(api::RoadGeometryId("dut"), linear_tolerance, angular_tolerance, scale_length, - internal_inertial_frame_translation, kDistance); + inertial_to_backend_frame_translation, kDistance); std::vector results = BruteForceFindRoadPositionsStrategy(rg.get(), api::InertialPosition(1., 2., 3.), kRadius); @@ -163,7 +164,7 @@ TEST_F(BruteForceTest, LaneInAndOutRadius) { EXPECT_NEAR(results.front().distance, kExpectedDistance, kZeroTolerance); rg = MakeOneLaneRoadGeometry(api::RoadGeometryId("dut"), linear_tolerance, angular_tolerance, scale_length, - internal_inertial_frame_translation, 2. * kDistance); + inertial_to_backend_frame_translation, 2. * kDistance); results = BruteForceFindRoadPositionsStrategy(rg.get(), api::InertialPosition(1., 2., 3.), kRadius); EXPECT_TRUE(results.empty()); @@ -177,14 +178,14 @@ TEST_F(BruteForceTest, NullRoadGeometry) { TEST_F(BruteForceTest, NegativeRadius) { std::unique_ptr rg = MakeFullRoadGeometry(api::RoadGeometryId("dut"), linear_tolerance, angular_tolerance, scale_length, - internal_inertial_frame_translation); + inertial_to_backend_frame_translation); EXPECT_THROW(BruteForceFindRoadPositionsStrategy(rg.get(), api::InertialPosition(0., 0., 0.), -1.), common::assertion_error); } TEST_F(BruteForceTest, AllLanesCalled) { auto rg = MakeFullRoadGeometry(api::RoadGeometryId("dut"), linear_tolerance, angular_tolerance, scale_length, - internal_inertial_frame_translation); + inertial_to_backend_frame_translation); const std::vector lanes = rg.get()->get_lanes(); diff --git a/maliput/test/geometry_base/geometry_base_test.cc b/maliput/test/geometry_base/geometry_base_test.cc index d53786d59..74887dd50 100644 --- a/maliput/test/geometry_base/geometry_base_test.cc +++ b/maliput/test/geometry_base/geometry_base_test.cc @@ -209,26 +209,26 @@ GTEST_TEST(GeometryBaseRoadGeometryTest, BasicConstruction) { const double kValidLinearTolerance = 7.0; const double kValidAngularTolerance = 99.0; const double kValidScaleLength = 0.5; - const math::Vector3 kInternalInertialFrameTranslation{1., 2., 3.}; + const math::Vector3 kInertialToBackendFrameTranslation{1., 2., 3.}; // Tolerance/scale-length values must be positive. EXPECT_THROW(MockRoadGeometry(api::RoadGeometryId("dut"), 0., kValidAngularTolerance, kValidScaleLength, - kInternalInertialFrameTranslation), + kInertialToBackendFrameTranslation), std::exception); EXPECT_THROW(MockRoadGeometry(api::RoadGeometryId("dut"), kValidLinearTolerance, 0., kValidScaleLength, - kInternalInertialFrameTranslation), + kInertialToBackendFrameTranslation), std::exception); EXPECT_THROW(MockRoadGeometry(api::RoadGeometryId("dut"), kValidLinearTolerance, kValidAngularTolerance, 0., - kInternalInertialFrameTranslation), + kInertialToBackendFrameTranslation), std::exception); const MockRoadGeometry dut(api::RoadGeometryId("dut"), kValidLinearTolerance, kValidAngularTolerance, - kValidScaleLength, kInternalInertialFrameTranslation); + kValidScaleLength, kInertialToBackendFrameTranslation); 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); - EXPECT_EQ(dut.internal_inertial_frame_translation(), kInternalInertialFrameTranslation); + EXPECT_EQ(dut.inertial_to_backend_frame_translation(), kInertialToBackendFrameTranslation); } GTEST_TEST(GeometryBaseRoadGeometryTest, AddingBranchPoints) { @@ -238,10 +238,10 @@ GTEST_TEST(GeometryBaseRoadGeometryTest, AddingBranchPoints) { MockBranchPoint* raw_branch_point1 = branch_point1.get(); const double kSomePositiveDouble = 7.0; - const math::Vector3 kInternalInertialFrameTranslation{0., 0., 0.}; + const math::Vector3 kInertialToBackendFrameTranslation{0., 0., 0.}; MockRoadGeometry dut(api::RoadGeometryId("dut"), kSomePositiveDouble, kSomePositiveDouble, kSomePositiveDouble, - kInternalInertialFrameTranslation); + kInertialToBackendFrameTranslation); // Test the empty dut. EXPECT_EQ(dut.num_branch_points(), 0); @@ -273,10 +273,10 @@ GTEST_TEST(GeometryBaseRoadGeometryTest, AddingJunctions) { MockJunction* raw_junction1 = junction1.get(); const double kSomePositiveDouble = 7.0; - const math::Vector3 kInternalInertialFrameTranslation{0., 0., 0.}; + const math::Vector3 kInertialToBackendFrameTranslation{0., 0., 0.}; MockRoadGeometry dut(api::RoadGeometryId("dut"), kSomePositiveDouble, kSomePositiveDouble, kSomePositiveDouble, - kInternalInertialFrameTranslation); + kInertialToBackendFrameTranslation); // Test the empty dut. EXPECT_EQ(dut.num_junctions(), 0); @@ -313,9 +313,9 @@ class GeometryBaseRoadGeometryIndexingTest : public ::testing::Test { void SetUp() override { constexpr double kArbitrary{1.}; - const math::Vector3 kInternalInertialFrameTranslation{0., 0., 0.}; + const math::Vector3 kInertialToBackendFrameTranslation{0., 0., 0.}; road_geometry_ = std::make_unique(api::RoadGeometryId("dut"), kArbitrary, kArbitrary, kArbitrary, - kInternalInertialFrameTranslation); + kInertialToBackendFrameTranslation); } std::unique_ptr road_geometry_; From 6d2a77dfacc2f899aaf0b6b55a477ce833ffb439 Mon Sep 17 00:00:00 2001 From: Agustin Alba Chicar Date: Mon, 8 Mar 2021 12:01:52 -0300 Subject: [PATCH 03/10] Updates maliput_design.h to differentiate the Inertial Frame from the Backend Frame. --- maliput/include/maliput/api/maliput_design.h | 19 ++++++++++++++----- 1 file changed, 14 insertions(+), 5 deletions(-) diff --git a/maliput/include/maliput/api/maliput_design.h b/maliput/include/maliput/api/maliput_design.h index d8ccddbb0..feff514c0 100644 --- a/maliput/include/maliput/api/maliput_design.h +++ b/maliput/include/maliput/api/maliput_design.h @@ -85,6 +85,13 @@ /// * The world containing the road network is approximated by an inertial, /// locally (if not globally) flat, 3-dimensional Cartesian coordinate /// system referred to as the *world frame*. +/// * The API exposed frame is called `Inertial Frame` and is identical to +/// the *world frame*. `maliput` implementations (_backends_) may require +/// an isometric transform to another frame with the same characteristics +/// due to geometry definition or world positioning of the road surface. +/// This second frame will be called `Backend Frame` and they both use +/// different names to *world frame* because they are both embedded in the +/// _world_ via the following means. /// * The road surface is a bounded compact orientable 2-dimensional manifold /// embedded in the @f$ \mathbb{R}^3 @f$ world frame via a @f$ G^1 @f$ /// continuous map from @f$ \mathbb{R}^2 \to \mathbb{R}^3 @f$. @@ -155,11 +162,13 @@ /// > altitude or elevation. @f$\hat{x}@f$ and @f$\hat{y}@f$ span the horizontal /// > plane. Typically, the "ENU" convention is used: @f$\hat{x}@f$ points *East* /// > and @f$\hat{y}@f$ points *North*. -/// > -/// > *In the future:* the `maliput` API will be extended to provide a -/// > description of the geographic coordinate system (if any) used by a -/// > `RoadGeometry`, as well as a local gravity vector as a function of -/// > position. +/// +/// As differentiated before, the `World`-frame is identical to the +/// `Inertial Frame` and another frame with the same characteristics is used for +/// `maliput` implementations. This second frame is the `Backend Frame` which +/// differs from the `Inertial Frame` by an isometric transform. The +/// `Backend Frame` coordinates should use `Vector3` to properly differentiate +/// from `InertialPositions` and must be part of backend implementations only. /// /// A `Lane`-frame is a right-handed orthonormal curvilinear coordinate system, with /// positions expressed as coordinates @f$(s,r,h)@f$. Each `Lane` in a `RoadGeometry` From d9134668ab70dd1086aa13b742230c25884dfac8 Mon Sep 17 00:00:00 2001 From: Agustin Alba Chicar Date: Mon, 8 Mar 2021 12:19:08 -0300 Subject: [PATCH 04/10] Updated backend frame definition based on Liang' suggestion. --- maliput/include/maliput/api/maliput_design.h | 26 ++++++++++---------- 1 file changed, 13 insertions(+), 13 deletions(-) diff --git a/maliput/include/maliput/api/maliput_design.h b/maliput/include/maliput/api/maliput_design.h index feff514c0..12a56802f 100644 --- a/maliput/include/maliput/api/maliput_design.h +++ b/maliput/include/maliput/api/maliput_design.h @@ -85,13 +85,6 @@ /// * The world containing the road network is approximated by an inertial, /// locally (if not globally) flat, 3-dimensional Cartesian coordinate /// system referred to as the *world frame*. -/// * The API exposed frame is called `Inertial Frame` and is identical to -/// the *world frame*. `maliput` implementations (_backends_) may require -/// an isometric transform to another frame with the same characteristics -/// due to geometry definition or world positioning of the road surface. -/// This second frame will be called `Backend Frame` and they both use -/// different names to *world frame* because they are both embedded in the -/// _world_ via the following means. /// * The road surface is a bounded compact orientable 2-dimensional manifold /// embedded in the @f$ \mathbb{R}^3 @f$ world frame via a @f$ G^1 @f$ /// continuous map from @f$ \mathbb{R}^2 \to \mathbb{R}^3 @f$. @@ -163,12 +156,19 @@ /// > plane. Typically, the "ENU" convention is used: @f$\hat{x}@f$ points *East* /// > and @f$\hat{y}@f$ points *North*. /// -/// As differentiated before, the `World`-frame is identical to the -/// `Inertial Frame` and another frame with the same characteristics is used for -/// `maliput` implementations. This second frame is the `Backend Frame` which -/// differs from the `Inertial Frame` by an isometric transform. The -/// `Backend Frame` coordinates should use `Vector3` to properly differentiate -/// from `InertialPositions` and must be part of backend implementations only. +/// At the API level, we will refer to `Inertial Frame` as a synonym of +/// `World`-frame. +/// +/// Another frame will be defined, the `Backend Frame` which is +/// different from the `World`-frame by an isometric transform. This frame is +/// also right-handed 3D inertial Cartesian coordinate system with an +/// orthonormal basis. It exists and potentially differs from the +/// `Inertial Frame` because of differing contexts. Typically, the +/// `Inertial Frame` chosen matches that of the client, e.g., a +/// simulator that uses Maliput. Meanwhile, the `Backend Frame` typically +/// matches the underlying data used by a particular backend, e.g., an +/// OpenDRIVE file. The `Backend Frame` will use `Vector3` for coordinates to +/// properly differentiate from the `World`-frame type, i.e. `InertialPositions`. /// /// A `Lane`-frame is a right-handed orthonormal curvilinear coordinate system, with /// positions expressed as coordinates @f$(s,r,h)@f$. Each `Lane` in a `RoadGeometry` From 2a5bfc953a6efe6aff74a4e6359aad62a16b04c6 Mon Sep 17 00:00:00 2001 From: Agustin Alba Chicar Date: Mon, 8 Mar 2021 12:19:54 -0300 Subject: [PATCH 05/10] Updates geometry_base docstrings based on Liang's comments. --- ...brute_force_find_road_positions_strategy.h | 4 ++- maliput/include/maliput/geometry_base/lane.h | 26 +++++++++---------- maliput/src/geometry_base/lane.cc | 6 ++--- 3 files changed, 19 insertions(+), 17 deletions(-) diff --git a/maliput/include/maliput/geometry_base/brute_force_find_road_positions_strategy.h b/maliput/include/maliput/geometry_base/brute_force_find_road_positions_strategy.h index c01782710..929ac9600 100644 --- a/maliput/include/maliput/geometry_base/brute_force_find_road_positions_strategy.h +++ b/maliput/include/maliput/geometry_base/brute_force_find_road_positions_strategy.h @@ -13,7 +13,9 @@ namespace geometry_base { /// _distance_ with `radius`. /// /// @note Maliput backends could avoid implementing a custom implementation that -/// knows about the geometry internals by forwarding calls to this function. +/// knows about the geometry internals by forwarding calls to this function. On +/// the contrary, backends might decide not to use this function because, for +/// example, time complexity. /// /// @param rg The RoadGeometry over all these operations are performed. It /// must not be nullptr. diff --git a/maliput/include/maliput/geometry_base/lane.h b/maliput/include/maliput/geometry_base/lane.h index 60ef119ad..e3e3f3006 100644 --- a/maliput/include/maliput/geometry_base/lane.h +++ b/maliput/include/maliput/geometry_base/lane.h @@ -103,8 +103,9 @@ class Lane : public api::Lane { // Maps @p lane_pos into the Backend Frame. // // @note This method implementation @throws maliput::common::assertion_error - // as it should never be called. @see DoToInertialPosition() - // docstring to understand when to override each method accordingly. + // as it should only be called when it is overridden by a derived class. + // @see DoToInertialPosition() docstring to understand when to override + // each method accordingly. // // @return The mapped @p lane_pos into the Backend Frame. virtual math::Vector3 DoToBackendPosition(const api::LanePosition& lane_pos) const; @@ -112,31 +113,30 @@ class Lane : public api::Lane { // @{ // Translates @p inertial_pos with api::RoadGeometry::internal_inertial_frame_translation() - // and calls DoToLanePositionFromBackendPosition(). The returned values are + // and calls DoToLanePositionBackend(). The returned values are // used to build an api::LanePositionResult. Note that `nearest_backend_pos` // is converted back to the Inertial Frame before returning the final result. // // @note When the Inertial and Backend Frames differ, the backend // implementation should (1) not override this method and (2) override - // DoToLanePositionFromBackendPosition(). + // DoToLanePositionBackend(). // @note When the Inertial and Backend Frames do not differ, the backend // implementation should (1) override this method and (2) do not - // override DoToLanePositionFromBackendPosition(). + // override DoToLanePositionBackend(). virtual api::LanePositionResult DoToLanePosition(const api::InertialPosition& inertial_pos) const override; // Maps @p backend_pos (measured in the Backend Frame) into this Lane Frame. // // @note This method implementation @throws maliput::common::assertion_error - // as it should never be called. @see DoToLanePosition() - // docstring to understand when to override each method accordingly. + // as it should only be called when it is overridden by a derived class. + // @see DoToLanePosition() docstring to understand when to override each + // method accordingly. // // @param backend_pos The Backend Frame coordinate to map into this Lane // Frame. // @param lane_pos The candidate LanePosition within the Lane' segment- - // bounds which is closest to an Inertial Frame position supplied to - // api::Lane::ToLanePosition() and then converted into the Backend - // Frame (measured by the Cartesian metric in the world frame). It must - // not be nullptr. + // bounds which is closest closest to @p backend_pos. It must not be + // nullptr. // @param nearest_backend_pos The position that exactly corresponds to // @p lane_pos. It must not be nullptr. // @param distance The Cartesian distance between `nearest_position` and the @@ -147,8 +147,8 @@ class Lane : public api::Lane { // nullptr. // // @return The @p lane_pos, @p nearest_backend_pos and @p distance. - virtual void DoToLanePositionFromBackendPosition(const math::Vector3& backend_pos, api::LanePosition* lane_pos, - math::Vector3* nearest_backend_pos, double* distance) const; + virtual void DoToLanePositionBackend(const math::Vector3& backend_pos, api::LanePosition* lane_pos, + math::Vector3* nearest_backend_pos, double* distance) const; // @} const api::LaneId id_; diff --git a/maliput/src/geometry_base/lane.cc b/maliput/src/geometry_base/lane.cc index 9f770834d..962533fcd 100644 --- a/maliput/src/geometry_base/lane.cc +++ b/maliput/src/geometry_base/lane.cc @@ -103,13 +103,13 @@ api::LanePositionResult Lane::DoToLanePosition(const api::InertialPosition& iner api::LanePosition lane_pos{}; math::Vector3 nearest_backend_pos{}; double distance{}; - DoToLanePositionFromBackendPosition(backend_pos, &lane_pos, &nearest_backend_pos, &distance); + DoToLanePositionBackend(backend_pos, &lane_pos, &nearest_backend_pos, &distance); return {lane_pos, api::InertialPosition::FromXyz(nearest_backend_pos + inertial_to_backend_frame_translation), distance}; } -void Lane::DoToLanePositionFromBackendPosition(const math::Vector3& backend_pos, api::LanePosition* lane_position, - math::Vector3* nearest_backend_pos, double* distance) const { +void Lane::DoToLanePositionBackend(const math::Vector3& backend_pos, api::LanePosition* lane_position, + math::Vector3* nearest_backend_pos, double* distance) const { MALIPUT_THROW_MESSAGE( "Unimplemented method. Please check the documentation of " "maliput::geometry_base::Lane::DoToLanePosition()."); From 7d176485685bb5f0e5b0f623d1b92b193b9a596b Mon Sep 17 00:00:00 2001 From: Agustin Alba Chicar Date: Mon, 8 Mar 2021 12:37:44 -0300 Subject: [PATCH 06/10] References maliput_desing.h from api/road_geometry.h for a frame description. --- maliput/include/maliput/api/road_geometry.h | 3 +++ 1 file changed, 3 insertions(+) diff --git a/maliput/include/maliput/api/road_geometry.h b/maliput/include/maliput/api/road_geometry.h index 98d08889d..801f3a7e3 100644 --- a/maliput/include/maliput/api/road_geometry.h +++ b/maliput/include/maliput/api/road_geometry.h @@ -181,6 +181,9 @@ class RoadGeometry { /// differ one from another by an isometric transformation. This method /// returns the translation vector between both frames. /// + /// For an explanation on the two different frames, see the explanation in + /// maliput_design.h. + /// /// @return maliput's Inertial Frame to Backend Frame translation vector. math::Vector3 inertial_to_backend_frame_translation() const { return do_inertial_to_backend_frame_translation(); } From 33ea1dfca4228325adca5be80b516bb309fde9b5 Mon Sep 17 00:00:00 2001 From: Agustin Alba Chicar Date: Mon, 8 Mar 2021 12:39:45 -0300 Subject: [PATCH 07/10] BruteForceFindRoadPositionsStrategy() docstring with the minimum time complexity. --- .../geometry_base/brute_force_find_road_positions_strategy.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/maliput/include/maliput/geometry_base/brute_force_find_road_positions_strategy.h b/maliput/include/maliput/geometry_base/brute_force_find_road_positions_strategy.h index 929ac9600..f2172fff2 100644 --- a/maliput/include/maliput/geometry_base/brute_force_find_road_positions_strategy.h +++ b/maliput/include/maliput/geometry_base/brute_force_find_road_positions_strategy.h @@ -15,7 +15,7 @@ namespace geometry_base { /// @note Maliput backends could avoid implementing a custom implementation that /// knows about the geometry internals by forwarding calls to this function. On /// the contrary, backends might decide not to use this function because, for -/// example, time complexity. +/// example, time complexity which is at least O(n^3). /// /// @param rg The RoadGeometry over all these operations are performed. It /// must not be nullptr. From cf6ce814ace9fe1ef21e795140852951830a1c87 Mon Sep 17 00:00:00 2001 From: Agustin Alba Chicar Date: Mon, 8 Mar 2021 14:05:40 -0300 Subject: [PATCH 08/10] Clarifies note in BruteForceFindRoadPositionsStrategy() docstring. --- .../brute_force_find_road_positions_strategy.h | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/maliput/include/maliput/geometry_base/brute_force_find_road_positions_strategy.h b/maliput/include/maliput/geometry_base/brute_force_find_road_positions_strategy.h index f2172fff2..b9be2cb97 100644 --- a/maliput/include/maliput/geometry_base/brute_force_find_road_positions_strategy.h +++ b/maliput/include/maliput/geometry_base/brute_force_find_road_positions_strategy.h @@ -12,10 +12,11 @@ namespace geometry_base { /// that exhaustively calls `ToLanePosition()` on all lanes and checks /// _distance_ with `radius`. /// -/// @note Maliput backends could avoid implementing a custom implementation that -/// knows about the geometry internals by forwarding calls to this function. On -/// the contrary, backends might decide not to use this function because, for -/// example, time complexity which is at least O(n^3). +/// @note Maliput backends could avoid implementing a custom implementation of +/// RoadGeometry::FindRoadPositions() that knows about the geometry internals by +/// forwarding calls to this function. On the contrary, backends might decide +/// not to use this function because, for example, time complexity which is at +/// least O(n^3). /// /// @param rg The RoadGeometry over all these operations are performed. It /// must not be nullptr. From 4bbd3d58921e9759ff94434d9b54ffb40ecd9707 Mon Sep 17 00:00:00 2001 From: Agustin Alba Chicar Date: Wed, 10 Mar 2021 16:53:26 -0300 Subject: [PATCH 09/10] Clarified notes in API. Changed the focus to performance over differences in transform. --- maliput/include/maliput/api/maliput_design.h | 4 ++-- maliput/include/maliput/geometry_base/lane.h | 20 ++++++++------------ 2 files changed, 10 insertions(+), 14 deletions(-) diff --git a/maliput/include/maliput/api/maliput_design.h b/maliput/include/maliput/api/maliput_design.h index 12a56802f..6f57952ce 100644 --- a/maliput/include/maliput/api/maliput_design.h +++ b/maliput/include/maliput/api/maliput_design.h @@ -161,14 +161,14 @@ /// /// Another frame will be defined, the `Backend Frame` which is /// different from the `World`-frame by an isometric transform. This frame is -/// also right-handed 3D inertial Cartesian coordinate system with an +/// also a right-handed 3D inertial Cartesian coordinate system with an /// orthonormal basis. It exists and potentially differs from the /// `Inertial Frame` because of differing contexts. Typically, the /// `Inertial Frame` chosen matches that of the client, e.g., a /// simulator that uses Maliput. Meanwhile, the `Backend Frame` typically /// matches the underlying data used by a particular backend, e.g., an /// OpenDRIVE file. The `Backend Frame` will use `Vector3` for coordinates to -/// properly differentiate from the `World`-frame type, i.e. `InertialPositions`. +/// properly differentiate from the `World`-frame type, i.e. `InertialPosition`. /// /// A `Lane`-frame is a right-handed orthonormal curvilinear coordinate system, with /// positions expressed as coordinates @f$(s,r,h)@f$. Each `Lane` in a `RoadGeometry` diff --git a/maliput/include/maliput/geometry_base/lane.h b/maliput/include/maliput/geometry_base/lane.h index e3e3f3006..9136f1560 100644 --- a/maliput/include/maliput/geometry_base/lane.h +++ b/maliput/include/maliput/geometry_base/lane.h @@ -92,12 +92,10 @@ class Lane : public api::Lane { // Forwards the call to DoToBackendPosition(lane_pos) and translates // the result to account for api::RoadGeometry::inertial_to_backend_frame_translation(). // - // @note When the Inertial and Backend Frames differ, the backend - // implementation should (1) not override this method and (2) override - // DoToBackendPosition(). - // @note When the Inertial and Backend Frames do not differ, the backend - // implementation should (1) override this method and (2) do not - // override DoToBackendPosition(). + // @note Because of performance constraints, backends must decide whether to + // exclusively override DoToInertialPosition() or DoToBackendPosition(). + // When overriding DoToInertialPosition(), a better performance is + // expected than the generic transform applied here. virtual api::InertialPosition DoToInertialPosition(const api::LanePosition& lane_pos) const override; // Maps @p lane_pos into the Backend Frame. @@ -117,12 +115,10 @@ class Lane : public api::Lane { // used to build an api::LanePositionResult. Note that `nearest_backend_pos` // is converted back to the Inertial Frame before returning the final result. // - // @note When the Inertial and Backend Frames differ, the backend - // implementation should (1) not override this method and (2) override - // DoToLanePositionBackend(). - // @note When the Inertial and Backend Frames do not differ, the backend - // implementation should (1) override this method and (2) do not - // override DoToLanePositionBackend(). + // @note Because of performance constraints, backends must decide whether to + // exclusively override DoToLanePosition() or DoToLanePositionBackend(). + // When overriding DoToLanePosition(), a better performance is + // expected than the generic transform applied here. virtual api::LanePositionResult DoToLanePosition(const api::InertialPosition& inertial_pos) const override; // Maps @p backend_pos (measured in the Backend Frame) into this Lane Frame. From efdb80c00b2c8c868f4ea4f2550e620bca22b319 Mon Sep 17 00:00:00 2001 From: Agustin Alba Chicar Date: Thu, 11 Mar 2021 15:13:38 -0300 Subject: [PATCH 10/10] Improves docstring style. --- maliput/include/maliput/geometry_base/lane.h | 17 +++++++++++------ 1 file changed, 11 insertions(+), 6 deletions(-) diff --git a/maliput/include/maliput/geometry_base/lane.h b/maliput/include/maliput/geometry_base/lane.h index 9136f1560..d185a4195 100644 --- a/maliput/include/maliput/geometry_base/lane.h +++ b/maliput/include/maliput/geometry_base/lane.h @@ -89,8 +89,10 @@ class Lane : public api::Lane { std::optional DoGetDefaultBranch(const api::LaneEnd::Which which_end) const override; // @{ - // Forwards the call to DoToBackendPosition(lane_pos) and translates - // the result to account for api::RoadGeometry::inertial_to_backend_frame_translation(). + // Maps @p lane_pos into the Inertial Frame. + // + // @details Forwards the call to DoToBackendPosition(lane_pos) and translates + // the result to account for api::RoadGeometry::inertial_to_backend_frame_translation(). // // @note Because of performance constraints, backends must decide whether to // exclusively override DoToInertialPosition() or DoToBackendPosition(). @@ -110,10 +112,13 @@ class Lane : public api::Lane { // @} // @{ - // Translates @p inertial_pos with api::RoadGeometry::internal_inertial_frame_translation() - // and calls DoToLanePositionBackend(). The returned values are - // used to build an api::LanePositionResult. Note that `nearest_backend_pos` - // is converted back to the Inertial Frame before returning the final result. + // Maps @p inertial_pos (measured in the Inertial Frame) into this Lane Frame. + // + // @details Translates @p inertial_pos with api::RoadGeometry::internal_inertial_frame_translation() + // and calls DoToLanePositionBackend(). The returned values are + // used to build an api::LanePositionResult. Note that + // `nearest_backend_pos` is converted back to the Inertial Frame + // before returning the final result. // // @note Because of performance constraints, backends must decide whether to // exclusively override DoToLanePosition() or DoToLanePositionBackend().