Added the implementation of the spline lanes.

drake/automotive/maliput/rndf/BUILD

@@ -57,19 +57,39 @@ drake_cc_googletest(
     ],
 )
 
+drake_cc_library(
+    name = "rndf_test_utils",
+    testonly = 1,
+    hdrs = ["test/rndf_test_utils.h"],
+    deps = [
+        "//drake/automotive/maliput/api",
+        "@ignition_rndf",
+    ],
+)
+
+drake_cc_googletest(
+    name = "road_geometry_test",
+    size = "small",
+    deps = [
+        ":lanes",
+    ],
+)
+
 drake_cc_googletest(
     name = "spline_helpers_test",
     size = "small",
     deps = [
         ":lanes",
+        ":rndf_test_utils",
     ],
 )
 
 drake_cc_googletest(
-    name = "road_geometry_test",
+    name = "spline_lane_test",
     size = "small",
     deps = [
         ":lanes",
+        ":rndf_test_utils",
     ],
 )
 

drake/automotive/maliput/rndf/lane.h

@@ -32,14 +32,10 @@ class Lane : public api::Lane {
   /// @param id is the ID of the api::Lane.
   /// @param segment is a pointer that refers to its parent, which must remain
   /// valid for the lifetime of this class.
-  /// @param width is the value of the width of the lane based on the RNDF
-  /// lane_width parameter. It will be used to set a constant lane_bound
-  /// value along the road.
   /// @param index is the index that can be used to reference this Lane from
   /// api::Segment::lane() call.
-  Lane(const api::LaneId& id, const api::Segment* segment, double width,
-       int index)
-      : id_(id), segment_(segment), width_(width), index_(index) {}
+  Lane(const api::LaneId& id, const api::Segment* segment, int index)
+      : id_(id), segment_(segment), index_(index) {}
 
   /// Sets the pointer of the BranchPoint that contains a
   /// api::LaneEnd::Which::kStart value attached to this lane pointer.
@@ -61,7 +57,7 @@ class Lane : public api::Lane {
 
   ~Lane() override = default;
 
- protected:
+ private:
   const api::LaneId do_id() const override { return id_; }
 
   const api::Segment* do_segment() const override;
@@ -98,7 +94,6 @@ class Lane : public api::Lane {
   const api::Segment* segment_{};
   BranchPoint* start_bp_{};
   BranchPoint* end_bp_{};
-  const double width_{};
   const int index_{};
 };
 

drake/automotive/maliput/rndf/segment.cc

@@ -8,9 +8,13 @@ namespace drake {
 namespace maliput {
 namespace rndf {
 
-SplineLane* Segment::NewSplineLane(const api::LaneId& id, double width) {
-  std::unique_ptr<SplineLane> lane =
-      std::make_unique<SplineLane>(id, this, width, lanes_.size());
+SplineLane* Segment::NewSplineLane(
+    const api::LaneId& id,
+    const std::vector<std::tuple<ignition::math::Vector3d,
+                                 ignition::math::Vector3d>>& control_points,
+    double width) {
+  std::unique_ptr<SplineLane> lane = std::make_unique<SplineLane>(
+      id, this, control_points, width, lanes_.size());
   SplineLane* spline_lane = lane.get();
   lanes_.push_back(std::move(lane));
   return spline_lane;

drake/automotive/maliput/rndf/segment.h

@@ -1,8 +1,11 @@
 #pragma once
 
 #include <memory>
+#include <tuple>
 #include <vector>
 
+#include "ignition/math/Vector3.hh"
+
 #include "drake/automotive/maliput/api/junction.h"
 #include "drake/automotive/maliput/api/lane.h"
 #include "drake/automotive/maliput/api/segment.h"
@@ -22,20 +25,28 @@ class Segment : public api::Segment {
   DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(Segment)
 
   /// Constructs a new Segment.
-  /// @param id to name this segment
-  /// @param junction must remain valid for the lifetime of this class.
+  /// @param This segment's ID.
+  /// @param The api::Junction that contains this Segment. It must remain valid
+  /// for the lifetime of this class.
   Segment(const api::SegmentId& id, api::Junction* junction)
       : id_(id), junction_(junction) {}
 
   /// Gives the segment a newly constructed SplineLane.
   ///
-  /// @param id is the id of the lane.
-  /// @param width is the width specified by the RNDF lane_width
-  /// parameter, or the default assigned value by this code. Later, this value
-  /// will be used to construct the api::Lane::lane_bounds() and the
-  /// api::Lane::driveable_bounds() result.
-  /// @return a pointer to a valid SplineLane.
-  SplineLane* NewSplineLane(const api::LaneId& id, double width);
+  /// @param The lane's ID.
+  /// @param control_points A vector of tuples that hold the point (first
+  /// element) and the tangent (second element) at that point to construct the
+  /// spline based lane. The size should be at least two pairs.
+  /// @param width The width specified by the RNDF lane_width
+  /// parameter. Later, this value will be used to construct the
+  /// api::Lane::lane_bounds() and the api::Lane::driveable_bounds() result.
+  /// @return a pointer to a valid SplineLane that was added to this Segment.
+  /// @throws std::runtime_error When @p control_points' size is less than 2.
+  SplineLane* NewSplineLane(
+      const api::LaneId& id,
+      const std::vector<std::tuple<ignition::math::Vector3d,
+                                   ignition::math::Vector3d>>& control_points,
+      double width);
 
   ~Segment() override = default;
 

drake/automotive/maliput/rndf/spline_helpers.h

@@ -34,12 +34,12 @@ class InverseFunctionInterpolator {
   /// @param[in] xmax @p function 's domain interval upper bound.
   /// @param[in] error_boundary a positive constraint on the maximum error
   /// allowed when approximating the inverse function.
-  /// @throws when @p error_boundary is not positive.
-  /// @throws when @p xmin is equal or greater than @p xmax.
-  /// @throws when evaluating @p function throws.
-  explicit InverseFunctionInterpolator(
-      std::function<double(double)> function, double xmin, double xmax,
-      double error_boundary);
+  /// @throws std::runtime_error When @p error_boundary is not positive.
+  /// @throws std::runtime_error When @p xmin is equal or greater than @p xmax.
+  /// @throws std::runtime_error When evaluating @p function throws.
+  explicit InverseFunctionInterpolator(std::function<double(double)> function,
+                                       double xmin, double xmax,
+                                       double error_boundary);
 
   /// Interpolates @f$ x^{(derivative_order)}(y) @f$, that is, the inverse of
   /// the given function.
@@ -54,8 +54,8 @@ class InverseFunctionInterpolator {
   /// @throws when @p derivative_order is a negative integer.
   /// @throws when @p y is larger than the maximum range of the function's
   /// codomain as described in the constructor's documentation.
-  /// @throws when @p y is smaller than the minimum range of the function's
-  /// codomain as described in the constructor's documentation.
+  /// @throws std::runtime_error When @p y is smaller than the minimum range of
+  /// the function's codomain as described in the constructor's documentation.
   double InterpolateMthDerivative(int derivative_order, double y);
 
  private:
@@ -96,8 +96,9 @@ class ArcLengthParameterizedSpline {
   /// @param[in] error_boundary a positive constraint on the
   /// maximum error allowed when approximating the path length
   /// parameterization.
-  /// @throws when @p spline is nullptr.
-  /// @throws when @p error_boundary is not a positive number.
+  /// @throws std::runtime_error When @p spline is nullptr.
+  /// @throws std::runtime_error When @p error_boundary is not a positive
+  /// number.
   explicit ArcLengthParameterizedSpline(
       std::unique_ptr<ignition::math::Spline> spline, double error_boundary);
 
@@ -109,7 +110,7 @@ class ArcLengthParameterizedSpline {
   /// @param[in] s path length to interpolate at, constrained
   /// by the curve dimensions [0, path_length].
   /// @return the @p derivative_order derivative @f$ Q^{(derivative_order)}(s) .
-  /// @throws if @p derivative_order is a negative integer.
+  /// @throws std::runtime_error If @p derivative_order is a negative integer.
   ignition::math::Vector3d InterpolateMthDerivative(int derivative_order,
                                                     double s);
 

drake/automotive/maliput/rndf/spline_lane.cc

@@ -1,12 +1,199 @@
 #include "drake/automotive/maliput/rndf/spline_lane.h"
 
+#include <utility>
+
+#include "drake/common/drake_assert.h"
+#include "drake/common/drake_throw.h"
+#include "drake/math/saturate.h"
+
 namespace drake {
 namespace maliput {
 namespace rndf {
 
-SplineLane::SplineLane(const api::LaneId& id, const api::Segment* segment,
-                       double width, int index)
-    : Lane(id, segment, width, index) {}
+// This constant specifies the maximum tolerable error to
+// ArcLengthParameterizedSpline. It will be used to provide a close
+// approximation of the path length s coordinate to the spline's t parameter.
+const double SplineLane::kSplineErrorBound = 1e-6;
+
+SplineLane::SplineLane(
+    const api::LaneId& id, const api::Segment* segment,
+    const std::vector<std::tuple<ignition::math::Vector3d,
+                                 ignition::math::Vector3d>>& control_points,
+    double width, int index)
+    : Lane(id, segment, index), width_(width) {
+  DRAKE_THROW_UNLESS(control_points.size() >= 2);
+  // Creates a spline based on the positions and their tangents.
+  auto spline = std::make_unique<ignition::math::Spline>();
+  spline->AutoCalculate(true);
+  for (const auto& point : control_points) {
+    spline->AddPoint(std::get<0>(point), std::get<1>(point));
+  }
+  // Wraps the ignition::math::Spline within an ArcLengthParamterizedSpline for
+  // compatibility with Maliput's s parameter.
+  spline_ = std::make_unique<ArcLengthParameterizedSpline>(std::move(spline),
+                                                           kSplineErrorBound);
+}
+
+api::LanePosition SplineLane::DoToLanePosition(const api::GeoPosition&,
+                                               api::GeoPosition*,
+                                               double*) const {
+  // TODO(@agalbachicar) We need to find a way to implement it.
+  DRAKE_ABORT();
+}
+
+api::GeoPosition SplineLane::DoToGeoPosition(
+    const api::LanePosition& lane_pos) const {
+  const double s = drake::math::saturate(lane_pos.s(), 0., do_length());
+  const api::RBounds driveable_bounds = do_driveable_bounds(s);
+  const double r = drake::math::saturate(lane_pos.r(), driveable_bounds.r_min,
+                                         driveable_bounds.r_max);
+  // Calculate x,y of (s,0,0).
+  const Vector2<double> xy = xy_of_s(s);
+  // Calculate orientation of (s,r,h) basis at (s,0,0).
+  const Rot3 ypr = Rabg_of_s(s);
+  // Rotate (0,r,h) and sum with mapped (s,0,h).
+  const Vector3<double> xyz = ypr.apply({0., r, lane_pos.h()}) +
+                              Vector3<double>(xy.x(), xy.y(), lane_pos.h());
+  return {xyz.x(), xyz.y(), xyz.z()};
+}
+
+api::Rotation SplineLane::DoGetOrientation(
+    const api::LanePosition& lane_pos) const {
+  const double s = math::saturate(lane_pos.s(), 0., this->do_length());
+  // Recover linear parameter p from path length position s.
+  const Rot3 Rabg = Rabg_of_s(s);
+  return api::Rotation::FromRpy(0.0, 0.0, Rabg.yaw());
+}
+
+api::LanePosition SplineLane::DoEvalMotionDerivatives(
+    const api::LanePosition&, const api::IsoLaneVelocity& velocity) const {
+  return api::LanePosition(velocity.sigma_v, velocity.rho_v, velocity.eta_v);
+}
+
+Vector2<double> SplineLane::xy_of_s(double s) const {
+  // xy_of_s it's called L which is a function
+  // R --> R^2. We discard z component right now. We can say
+  // L = f(s) = (x(s) ; y(s))
+  const ignition::math::Vector3d point =
+      spline_->InterpolateMthDerivative(0, s);
+  return {point.X(), point.Y()};
+}
+Vector2<double> SplineLane::xy_dot_of_s(double s) const {
+  // We get here the tangent, which is the first derivative of
+  // L --> dL(s) / ds
+  const ignition::math::Vector3d point =
+      spline_->InterpolateMthDerivative(1, s);
+  return {point.X(), point.Y()};
+}
+double SplineLane::heading_of_s(double s) const {
+  // The tangent of the heading is the function of y(s) / x(s).
+  // So, we can say that h(s) = arctg (y(s) / x(s)). This function
+  // is a function like: h(s) = R --> R or h(f(x, y)) where f it's
+  // a function defined like y / x. y and x are the components
+  // of the first derivative of L. Then, we got: f: R^2 --> R
+  const Vector2<double> tangent = xy_dot_of_s(s);
+  return std::atan2(tangent.y(), tangent.x());
+}
+
+double SplineLane::heading_dot_of_s(double s) const {
+  // Based on the explanation of heading_of_s, we got it applying the chain
+  // rule:
+  // dh / ds = d/ds {arctg (f(x(s), y(s)))}
+  //         = 1 / (1 + f(x(s), y(s))^2) * d/ds {f(x(s), y(s))}
+  // As x(s) and y(s) and independent polynomials, we can say that:
+  // df(x(s), y(s)) / dp = (y' * x - y * x') / x^2
+  // Where y and x are the components of the L' and, x' and y' are
+  // the components of L'' as they are independent.
+  const double heading = heading_of_s(s);
+  const ignition::math::Vector3d first_derivative =
+      spline_->InterpolateMthDerivative(1, s);
+  const ignition::math::Vector3d second_derivative =
+      spline_->InterpolateMthDerivative(2, s);
+  const double m = (second_derivative.Y() * first_derivative.X() -
+                    first_derivative.Y() * second_derivative.X()) /
+                   (first_derivative.X() * first_derivative.X());
+  return (m / (1.0 + heading * heading));
+}
+
+Rot3 SplineLane::Rabg_of_s(double s) const {
+  return Rot3(0.0, 0.0, heading_of_s(s));
+}
+
+api::RBounds SplineLane::do_lane_bounds(double) const {
+  return api::RBounds(-width_ / 2., width_ / 2.);
+}
+
+api::RBounds SplineLane::do_driveable_bounds(double s) const {
+  if (segment()->num_lanes() == 1) {
+    return api::RBounds(-width_ / 2., width_ / 2.);
+  }
+  // Get the position to the first lane.
+  const ignition::math::Vector3d position_first_lane = GetPositionToLane(s, 0);
+  const ignition::math::Vector3d position_last_lane =
+      GetPositionToLane(s, segment()->num_lanes() - 1);
+  const double r_min = -std::abs(
+      (position_first_lane - spline_->InterpolateMthDerivative(0, s)).Length() +
+      segment()->lane(0)->lane_bounds(0.).r_max);
+  const double r_max = std::abs(
+      (position_last_lane - spline_->InterpolateMthDerivative(0, s)).Length() +
+      segment()->lane(segment()->num_lanes() - 1)->lane_bounds(0.).r_max);
+  return api::RBounds(r_min, r_max);
+}
+
+ignition::math::Vector3d SplineLane::GetPositionToLane(double s,
+                                                       int lane_id) const {
+  // Computes the geo position in the current lane at LanePosition(s, 0., 0.).
+  const ignition::math::Vector3d p = spline_->InterpolateMthDerivative(0, s);
+  // If lane_id points to myself I just need to return my position.
+  if (lane_id == index()) {
+    return p;
+  }
+  // This is the tangent of the current position.
+  ignition::math::Vector3d t_p = spline_->InterpolateMthDerivative(1, s);
+  t_p.Normalize();
+  // Computes the normal with a right-handed frame. The normal is just a 90°
+  // rotation over the z-axis.
+  const ignition::math::Vector3d r(-t_p.Y(), t_p.X(), 0.);
+  // Gets the beginning and ending of the other lane, and then computes the
+  // respective GeoPositions.
+  const api::Lane* other_lane = segment()->lane(lane_id);
+  DRAKE_DEMAND(other_lane != nullptr);
+  const api::GeoPosition other_lane_beginning =
+      other_lane->ToGeoPosition(api::LanePosition(0., 0., 0.));
+  const api::GeoPosition other_lane_ending = other_lane->ToGeoPosition(
+      api::LanePosition(other_lane->length(), 0., 0.));
+  // Converts the beginning and ending positions of the other lane into
+  // ignition::math::Vector3d objects.
+  const ignition::math::Vector3d q(other_lane_beginning.x(),
+                                   other_lane_beginning.y(), 0.);
+  const ignition::math::Vector3d q_ending(other_lane_ending.x(),
+                                          other_lane_ending.y(), 0.);
+  // As the lane is approximated as a line, we get the normalized tangent as:
+  ignition::math::Vector3d t_q = (q_ending - q);
+  t_q.Normalize();
+  // Checks if lines are collinear.
+  const ignition::math::Vector3d r_cross_t_q = r.Cross(t_q);
+  const ignition::math::Vector3d p_to_q_cross_r = (p - q).Cross(r);
+  const double kAlmostZero = 1e-6;
+  // Lines are parallel or non-intersecting. We cannot handle correctly that
+  // case for the purpose of this function.
+  DRAKE_DEMAND(r_cross_t_q.Length() > kAlmostZero);
+  // Computes the intersection point between r and the other_lane's line
+  // approximation.
+  return p + r * (p_to_q_cross_r.Length() / r_cross_t_q.Length());
+}
+
+double SplineLane::ComputeLength(
+    const std::vector<std::tuple<ignition::math::Vector3d,
+                                 ignition::math::Vector3d>>& control_points) {
+  DRAKE_THROW_UNLESS(control_points.size() >= 2);
+  ignition::math::Spline spline;
+  spline.AutoCalculate(true);
+  for (const auto& control_point : control_points) {
+    spline.AddPoint(std::get<0>(control_point), std::get<1>(control_point));
+  }
+  return spline.ArcLength();
+}
 
 }  // namespace rndf
 }  // namespace maliput