Lane::DoEvalMotionDerivatives.

Signed-off-by: Franco Cipollone <franco.c@ekumenlabs.com>
maliput · Oct 3, 2022 · 20f3241 · 20f3241
1 parent f9b4a08
commit 20f3241
Show file tree

Hide file tree

Showing 2 changed files with 74 additions and 2 deletions.
diff --git a/src/base/lane.cc b/src/base/lane.cc
@@ -101,8 +101,10 @@ maliput::api::Rotation Lane::DoGetOrientation(const maliput::api::LanePosition&
 
 maliput::api::LanePosition Lane::DoEvalMotionDerivatives(const maliput::api::LanePosition& position,
                                                          const maliput::api::IsoLaneVelocity& velocity) const {
-  // TODO: Implement
-  MALIPUT_THROW_MESSAGE("Not implemented");
+  // The definition of path-length of a path along σ yields dσ = |∂W/∂s| ds
+  // evaluated at (s, r, h).
+  const double ds_dsigma = lane_geometry_->WDot(position.s()).norm() / lane_geometry_->WDot(position.srh()).norm();
+  return {ds_dsigma * velocity.sigma_v, velocity.rho_v, velocity.eta_v};
 }
 
 }  // namespace maliput_sparse
diff --git a/test/base/lane_test.cc b/test/base/lane_test.cc
@@ -45,6 +45,7 @@ namespace {
 
 using geometry::LineString3d;
 using maliput::api::InertialPosition;
+using maliput::api::IsoLaneVelocity;
 using maliput::api::LanePosition;
 using maliput::api::LanePositionResult;
 using maliput::api::Rotation;
@@ -67,6 +68,10 @@ struct LaneTestCase {
 
 std::vector<LaneTestCase> LaneTestCases() {
   return {{
+              // Straight lane:
+              //   ______
+              //  0______100m
+              //
               LineString3d{{0., 2., 0.}, {100., 2., 0.}} /* left*/,
               LineString3d{{0., -2., 0.}, {100., -2., 0.}} /* right*/,
               {{0., 0., 0.}} /* srh */,
@@ -235,6 +240,71 @@ TEST_P(ToLaneSegmentPositionTest, Test) {
 INSTANTIATE_TEST_CASE_P(ToLaneSegmentPositionTestGroup, ToLaneSegmentPositionTest,
                         ::testing::ValuesIn(ToLaneSegmentPositionTestCases()));
 
+struct EvalMotionDerivativesTestCase {
+  LineString3d left{};
+  LineString3d right{};
+  IsoLaneVelocity velocity{};
+  std::vector<LanePosition> srh{};
+  std::vector<LanePosition> expected_motion_derivatives{};
+};
+
+std::vector<EvalMotionDerivativesTestCase> EvalMotionDerivativesTestCases() {
+  return {{
+              // Straight lane:
+              //   ______
+              //  0______100m
+              //
+              LineString3d{{0., 2., 0.}, {100., 2., 0.}} /* left*/,
+              LineString3d{{0., -2., 0.}, {100., -2., 0.}} /* right*/,
+              {3., 2., 1.}, /* velocity */
+              {{0., 0., 0.}, {50., 2., 0.}, {50., -2., 0.}} /* srh */,
+              {{3., 2., 1.}, {3., 2., 1.}, {3., 2., 1.}} /* expected_motion_derivatives */
+          },
+          {
+              // Arc-like no elevated lane:
+              //    | |
+              //  __/ /
+              //  __ /
+              LineString3d{{0., 2., 0.}, {100., 2., 0.}, {200., 102., 0.}, {200., 200., 0.}} /* left*/,
+              LineString3d{{0., -2., 0.}, {100., -2., 0.}, {204., 102., 0.}, {204., 200., 0.}} /* right*/,
+              // Centerline : {0., 0., 0.}, {100., 0., 0.}, {202., 102, 0.}, {202., 200., 0.}
+              {3., 2., 1.}, /* velocity */
+              {{50., 0., 0.},
+               {50., -2., 0.},
+               {100., 0., 0.},
+               {100. + 50 * std::sqrt(2.), 0., 0.},
+               {100. + 50 * std::sqrt(2.), 2., 0.}} /* srh */,
+              {{3, 2., 1.}, {3, 2., 1.}, {3, 2., 1.}, {3, 2., 1.}, {3, 2., 1.}} /* expected_motion_derivatives */
+          }};
+}
+
+class LaneEvalMotionDerivativesTestTest : public ::testing::TestWithParam<EvalMotionDerivativesTestCase> {
+ public:
+  static constexpr double kTolerance{1.e-5};
+  static constexpr double kScaleLength{1.};
+
+  void SetUp() override {
+    ASSERT_EQ(case_.srh.size(), case_.expected_motion_derivatives.size()) << ">>>>> Test case is ill-formed.";
+  }
+
+  const maliput::api::LaneId kLaneId{"dut id"};
+  const maliput::api::HBounds kHBounds{-5., 5.};
+  EvalMotionDerivativesTestCase case_ = GetParam();
+  std::unique_ptr<geometry::LaneGeometry> lane_geometry_ =
+      std::make_unique<geometry::LaneGeometry>(case_.left, case_.right, kTolerance, kScaleLength);
+};
+
+TEST_P(LaneEvalMotionDerivativesTestTest, Test) {
+  std::unique_ptr<Lane> dut = std::make_unique<Lane>(kLaneId, kHBounds, std::move(lane_geometry_));
+  for (std::size_t i = 0; i < case_.srh.size(); ++i) {
+    IsLanePositionClose(case_.expected_motion_derivatives[i], dut->EvalMotionDerivatives(case_.srh[i], case_.velocity),
+                        kTolerance);
+  }
+}
+
+INSTANTIATE_TEST_CASE_P(LaneEvalMotionDerivativesTestTestGroup, LaneEvalMotionDerivativesTestTest,
+                        ::testing::ValuesIn(EvalMotionDerivativesTestCases()));
+
 }  // namespace
 }  // namespace test
 }  // namespace maliput_sparse