From ffd7174e871ddc7a236842ed1aaa69564716fd3f Mon Sep 17 00:00:00 2001 From: Agustin Alba Chicar Date: Tue, 27 Jun 2017 13:43:07 -0300 Subject: [PATCH] Added the implementation of the spline lanes. --- drake/automotive/maliput/rndf/BUILD | 22 +- drake/automotive/maliput/rndf/lane.h | 11 +- drake/automotive/maliput/rndf/segment.cc | 10 +- drake/automotive/maliput/rndf/segment.h | 29 +- .../automotive/maliput/rndf/spline_helpers.h | 23 +- drake/automotive/maliput/rndf/spline_lane.cc | 193 ++++++++++++- drake/automotive/maliput/rndf/spline_lane.h | 209 ++++++++++++-- .../maliput/rndf/test/CMakeLists.txt | 24 +- .../maliput/rndf/test/rndf_test_utils.h | 55 ++++ .../maliput/rndf/test/segment_test.cc | 19 +- .../maliput/rndf/test/spline_helpers_test.cc | 12 +- .../maliput/rndf/test/spline_lane_test.cc | 266 ++++++++++++++++++ 12 files changed, 790 insertions(+), 83 deletions(-) create mode 100644 drake/automotive/maliput/rndf/test/rndf_test_utils.h create mode 100644 drake/automotive/maliput/rndf/test/spline_lane_test.cc diff --git a/drake/automotive/maliput/rndf/BUILD b/drake/automotive/maliput/rndf/BUILD index 952e86a6bcbf..4f5656084e88 100644 --- a/drake/automotive/maliput/rndf/BUILD +++ b/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", ], ) diff --git a/drake/automotive/maliput/rndf/lane.h b/drake/automotive/maliput/rndf/lane.h index ac7cfe5e4827..ea4e11f69f77 100644 --- a/drake/automotive/maliput/rndf/lane.h +++ b/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_{}; }; diff --git a/drake/automotive/maliput/rndf/segment.cc b/drake/automotive/maliput/rndf/segment.cc index 98de1c77b3eb..8c0fd81ab0a1 100644 --- a/drake/automotive/maliput/rndf/segment.cc +++ b/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 lane = - std::make_unique(id, this, width, lanes_.size()); +SplineLane* Segment::NewSplineLane( + const api::LaneId& id, + const std::vector>& control_points, + double width) { + std::unique_ptr lane = std::make_unique( + id, this, control_points, width, lanes_.size()); SplineLane* spline_lane = lane.get(); lanes_.push_back(std::move(lane)); return spline_lane; diff --git a/drake/automotive/maliput/rndf/segment.h b/drake/automotive/maliput/rndf/segment.h index c3394c85bd7c..65e2ecaddd96 100644 --- a/drake/automotive/maliput/rndf/segment.h +++ b/drake/automotive/maliput/rndf/segment.h @@ -1,8 +1,11 @@ #pragma once #include +#include #include +#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>& control_points, + double width); ~Segment() override = default; diff --git a/drake/automotive/maliput/rndf/spline_helpers.h b/drake/automotive/maliput/rndf/spline_helpers.h index 577b4b0fc433..d71d4b987b4a 100644 --- a/drake/automotive/maliput/rndf/spline_helpers.h +++ b/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 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 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 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); diff --git a/drake/automotive/maliput/rndf/spline_lane.cc b/drake/automotive/maliput/rndf/spline_lane.cc index c40eae710be3..adf4e5f341fe 100644 --- a/drake/automotive/maliput/rndf/spline_lane.cc +++ b/drake/automotive/maliput/rndf/spline_lane.cc @@ -1,12 +1,199 @@ #include "drake/automotive/maliput/rndf/spline_lane.h" +#include + +#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>& 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(); + 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(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 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 xyz = ypr.apply({0., r, lane_pos.h()}) + + Vector3(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 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 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 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>& 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 diff --git a/drake/automotive/maliput/rndf/spline_lane.h b/drake/automotive/maliput/rndf/spline_lane.h index e9138e6295c0..7b2c4e603076 100644 --- a/drake/automotive/maliput/rndf/spline_lane.h +++ b/drake/automotive/maliput/rndf/spline_lane.h @@ -1,51 +1,210 @@ #pragma once -#include "drake/automotive/maliput/api/lane.h" +#include +#include +#include + +#include + +#include "ignition/math/Spline.hh" +#include "ignition/math/Vector3.hh" + #include "drake/automotive/maliput/api/lane_data.h" -#include "drake/automotive/maliput/api/segment.h" #include "drake/automotive/maliput/rndf/lane.h" +#include "drake/automotive/maliput/rndf/spline_helpers.h" #include "drake/common/drake_copyable.h" +#include "drake/common/eigen_types.h" +#include "drake/math/roll_pitch_yaw_not_using_quaternion.h" namespace drake { namespace maliput { namespace rndf { -// TODO(@agalbachicar) Implement me. -/// This class is a stub and will be completed in a subsequent PR. +/// An R^3 rotation parameterized by roll, pitch, yaw. +/// +/// This effects a compound rotation around space-fixed x-y-z axes: +/// +/// Rot3(yaw,pitch,roll) * V = RotZ(yaw) * RotY(pitch) * RotX(roll) * V +class Rot3 { + public: + DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(Rot3) + + Rot3(double roll, double pitch, double yaw) : rpy_(roll, pitch, yaw) {} + + /// Applies the rotation to a 3-vector. + Vector3 apply(const Vector3& in) const { + return math::rpy2rotmat(rpy_) * in; + } + + double yaw() const { return rpy_(2); } + double pitch() const { return rpy_(1); } + double roll() const { return rpy_(0); } + + private: + Eigen::Matrix rpy_; +}; + +/// Specialization of drake::maliput::rndf::Lane with a spline curve as its +/// reference path. RNDF specification lacks elevation and superelevation so +/// all the Lanes will depict a flat path just over the ground. class SplineLane : public Lane { public: DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(SplineLane) - SplineLane(const api::LaneId& id, const api::Segment* segment, double width, - int index); + /// Constructs a SplineLane, a lane specified by a spline segment defined in + /// the xy-plane (the ground plane) of the world frame. + /// + /// @param id The ID of the api::Lane. + /// @param segment A pointer to the segment that contains this lane, which + /// must remain valid for the lifetime of this class. + /// @param control_points A collection of knots and their tangents where + /// the interpolated curve will pass. There must be at least two control + /// points. The tuple will be interpreted as with the first value as the knot + /// and the second as the tangent. + /// @param width 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 The index that can be used to reference this Lane from + /// api::Segment::lane() call. + /// + /// This implementation uses ignition::math::Spline and + /// ArcLengthParameterizedSpline which is used as the inverse function + /// that maps the s parameter of s,r,h frame to ignition::math's Spline t + /// parameter. + /// + /// @throws std::runtime_error When the size of @p control_points is less than + /// 2. + SplineLane( + const api::LaneId& id, const api::Segment* segment, + const std::vector>& control_points, + double width, int index); ~SplineLane() override = default; + /// Computes the length of a SplineLane created using @p control_points. + /// The first value are the points and the second is the + /// value at that point. + /// @param control_points A collection of knots and their tangents where + /// the interpolated curve will pass. There must be at least two control + /// points. The tuple's first value is the knot point while its second value + /// is the tangent at the knot point. + /// @return The length of the baseline computed as a spline. + /// @throws std::runtime_error When the size of @p control_points is less than + /// 2. + static double ComputeLength( + const std::vector>& control_points); + + /// @return the error bound that the path length interpolator will + /// attempt to attain when approximating the inverse function that maps + /// the s coordinate of api::LanePosition frame into the t parameter that + /// ignition::math::Spline uses to evaluate the function. + static double SplineErrorBound() { return kSplineErrorBound; } + private: - double do_length() const override { return 0.0; } - api::RBounds do_lane_bounds(double) const override { - return api::RBounds(0., 0.); - } - api::RBounds do_driveable_bounds(double) const override { - return api::RBounds(0., 0.); - } - api::GeoPosition DoToGeoPosition(const api::LanePosition&) const override { - return api::GeoPosition(); - } - api::LanePosition DoToLanePosition(const api::GeoPosition&, api::GeoPosition*, - double*) const override { - return api::LanePosition(); - } - api::Rotation DoGetOrientation(const api::LanePosition&) const override { - return api::Rotation(); - } + // This function is not implemented and will abort if called. + // TODO(@agalbachicar) We need to find a way to implement it. + api::LanePosition DoToLanePosition(const api::GeoPosition& geo_pos, + api::GeoPosition* nearest_point, + double* distance) const override; + + // Provides the api::GeoPosition at the @p lane_pos over the lane. + // s coordinate of @p lane_pos will be saturated to be processed in the range + // of 0.0 to the lane length. The same is applied to r coordinate which is + // saturated to the driveable_bounds at s coordinate (after the saturation). + api::GeoPosition DoToGeoPosition( + const api::LanePosition& lane_pos) const override; + + // Provides the api::Rotation matrix against the origin of the world frame at + // @p lane_pos coordinate over the lane. The s coordinate of @p lane_pose will + // be saturated to be in the range of 0.0 to the lane length. + api::Rotation DoGetOrientation( + const api::LanePosition& lane_pos) const override; + + // As we are moving along the lane with path length coordinates we will + // return the velocity as is and will not scale through the chain rule. This + // is because the returned api::LanePosition is isotropic, i.e., it contains + // real physical values. api::LanePosition DoEvalMotionDerivatives( - const api::LanePosition&, const api::IsoLaneVelocity&) const override { - return api::LanePosition(); + const api::LanePosition& position, + const api::IsoLaneVelocity& velocity) const override; + + // Provides the x,y coordinates based on the + // ArcLengthParameterizedSpline that will provide the interpolation + // image at a path length s from the beginning of the SplineLane. + Vector2 xy_of_s(double s) const; + + // Provides the x,y tangent values based on the ArcLengthParameterizedSpline + // that will provide the interpolation image at a path length s from the + // beginning of the SplineLane. + Vector2 xy_dot_of_s(double s) const; + + // Provides the angle of the tangent vector evaluated at a path length + // s distance from the beginning of the SplineLane. + double heading_of_s(double s) const; + + // Provides the derivative of the angle at a path length s from the beginning + // of the SplineLane. Given that x, y and z components of + // ignition::math::Spline are independent from each other and a function of s + // (given the inverse function approximation provided by + // ArcLengthParameterizedSpline) we can apply the chain rule and + // the obtain derivative of the heading. + double heading_dot_of_s(double s) const; + + // Provides a rotation vector in terms of Euler angles, with only + // yaw angle set as RNDF is defined without any change in elevation. + // See heading_of_s for more information. + Rot3 Rabg_of_s(double s) const; + + // Returns the length of the curve. + double do_length() const override { + return spline_->BaseSpline()->ArcLength(); } + + // TODO(@agalbachicar) Not implemented yet. api::HBounds do_elevation_bounds(double, double) const override { return api::HBounds(0., 0.); } + + // Computes the lane_bounds taking into account the Lane::width. Based + // on it, it constructs an api::RBounds object whose r_min variable is + // half the width but negative, and the r_max variable is half the width. + api::RBounds do_lane_bounds(double s) const override; + + // If the segment has only one lane, this one, the return value will be + // identical to do_lane_bounds(s). However, for the general case, each lane + // is approximated by a straight line from the beginning to the end of it. + // Then, we compute the intersection point of the current normal (following + // the s direction it will be coincident with the r direction) at s with that + // line approximation. Based on the intersection points with the lanes set as + // extents of the segment (the first and last ones) we can determine the + // distance to those limits and create the api::RBounds set as result. + // Here, GetPositionToLane() is used to compute the line approximation and + // intersection point. + api::RBounds do_driveable_bounds(double s) const override; + + // Computes the intersection point of the normal line that intersects the + // current lane at @p s (r = 0, h = 0) with the lane whose index is + // lane_id. The function of the lane with @p lane_id is approximated by a line + // defined by the api::GeoPosition values of both the beginning and ending + // of it. + // It returns the intersection point on the lane whose index is lane_id. + // If @p lane_id is equal to the Lane::index, then it returns the geo position + // at (s, 0, 0). + // Aborts if lane_id does not refer to a valid lane ID for the lane's segment. + // Aborts the lane referred by @p lane_id has no intersection with this lane, + // or it is collinear and coincident. + // The implementation is a transcription of this comment: + // https://stackoverflow.com/a/565282. It was adapted for the purposes of this + // code (no support for collinear and non intersecting lines). + ignition::math::Vector3d GetPositionToLane(double s, int lane_id) const; + + std::unique_ptr spline_; + + const double width_{}; + + static const double kSplineErrorBound; }; } // namespace rndf diff --git a/drake/automotive/maliput/rndf/test/CMakeLists.txt b/drake/automotive/maliput/rndf/test/CMakeLists.txt index f5db059c8185..90114486cbf0 100644 --- a/drake/automotive/maliput/rndf/test/CMakeLists.txt +++ b/drake/automotive/maliput/rndf/test/CMakeLists.txt @@ -13,14 +13,6 @@ if(ignition-rndf0_FOUND) drakeMaliputRndf "${IGNITION-RNDF_LIBRARIES}") - drake_add_cc_test(spline_helpers_test) - target_include_directories(spline_helpers_test PRIVATE - "${IGNITION-RNDF_INCLUDE_DIRS}" - ) - target_link_libraries(spline_helpers_test - drakeMaliputRndf - "${IGNITION-RNDF_LIBRARIES}") - drake_add_cc_test(road_geometry_test) target_include_directories(road_geometry_test PRIVATE "${IGNITION-RNDF_INCLUDE_DIRS}" @@ -36,4 +28,20 @@ if(ignition-rndf0_FOUND) target_link_libraries(segment_test drakeMaliputRndf "${IGNITION-RNDF_LIBRARIES}") + + drake_add_cc_test(spline_helpers_test) + target_include_directories(spline_helpers_test PRIVATE + "${IGNITION-RNDF_INCLUDE_DIRS}" + ) + target_link_libraries(spline_helpers_test + drakeMaliputRndf + "${IGNITION-RNDF_LIBRARIES}") + + drake_add_cc_test(spline_lane_test) + target_include_directories(spline_lane_test PRIVATE + "${IGNITION-RNDF_INCLUDE_DIRS}" + ) + target_link_libraries(spline_lane_test + drakeMaliputRndf + "${IGNITION-RNDF_LIBRARIES}") endif() diff --git a/drake/automotive/maliput/rndf/test/rndf_test_utils.h b/drake/automotive/maliput/rndf/test/rndf_test_utils.h new file mode 100644 index 000000000000..4bc3a8732fd2 --- /dev/null +++ b/drake/automotive/maliput/rndf/test/rndf_test_utils.h @@ -0,0 +1,55 @@ +#pragma once + +#include + +#include "ignition/math/Vector3.hh" + +#include "drake/automotive/maliput/api/lane_data.h" + +namespace drake { +namespace maliput { +namespace rndf { + +#define EXPECT_GEO_NEAR(actual_arg, expected_arg, tolerance_arg) \ + do { \ + const api::GeoPosition actual(actual_arg); \ + const api::GeoPosition expected(expected_arg); \ + const double tolerance(tolerance_arg); \ + EXPECT_NEAR(actual.x(), expected.x(), tolerance); \ + EXPECT_NEAR(actual.y(), expected.y(), tolerance); \ + EXPECT_NEAR(actual.z(), expected.z(), tolerance); \ + } while (0) + +#define EXPECT_LANE_NEAR(actual_arg, expected_arg, tolerance_arg) \ + do { \ + const api::LanePosition actual(actual_arg); \ + const api::LanePosition expected(expected_arg); \ + const double tolerance(tolerance_arg); \ + EXPECT_NEAR(actual.s(), expected.s(), tolerance); \ + EXPECT_NEAR(actual.r(), expected.r(), tolerance); \ + EXPECT_NEAR(actual.h(), expected.h(), tolerance); \ + } while (0) + +#define EXPECT_ROT_NEAR(actual_arg, expected_arg, tolerance_arg) \ + do { \ + const api::Rotation actual(actual_arg); \ + const api::Rotation expected(expected_arg); \ + const double tolerance(tolerance_arg); \ + EXPECT_NEAR(actual.yaw(), expected.yaw(), tolerance); \ + EXPECT_NEAR(actual.pitch(), expected.pitch(), tolerance); \ + EXPECT_NEAR(actual.roll(), expected.roll(), tolerance); \ + } while (0) + +#define EXPECT_IGN_VECTOR_NEAR(actual_arg, expected_arg, tolerance_arg) \ + do { \ + const ignition::math::Vector3d actual(actual_arg); \ + const ignition::math::Vector3d expected(expected_arg); \ + const double tolerance(tolerance_arg); \ + EXPECT_NEAR(actual.X(), expected.X(), tolerance); \ + EXPECT_NEAR(actual.Y(), expected.Y(), tolerance); \ + EXPECT_NEAR(actual.Z(), expected.Z(), tolerance); \ + } while (0) + +} // namespace rndf +} // namespace maliput +} // namespace drake diff --git a/drake/automotive/maliput/rndf/test/segment_test.cc b/drake/automotive/maliput/rndf/test/segment_test.cc index b13e7c30fa1c..0a7a0a9c6c58 100644 --- a/drake/automotive/maliput/rndf/test/segment_test.cc +++ b/drake/automotive/maliput/rndf/test/segment_test.cc @@ -1,7 +1,5 @@ #include "drake/automotive/maliput/rndf/segment.h" -#include -#include #include #include @@ -28,11 +26,22 @@ GTEST_TEST(RNDFSegmentTest, MetadataLane) { EXPECT_EQ(s1->junction(), rg.junction(0)); EXPECT_EQ(s1->num_lanes(), 0); - s1->NewSplineLane({"l1"}, 5.); - + std::vector> + control_points; + control_points.push_back( + std::make_tuple(ignition::math::Vector3d(0.0, 0.0, 0.0), + ignition::math::Vector3d(10.0, 0.0, 0.0))); + // As I add only one control_point, I expect the creator to throw. + EXPECT_THROW(s1->NewSplineLane({"l1"}, control_points, 5.), + std::runtime_error); + // Now I add the second control_point and expect it to not throw. + control_points.push_back( + std::make_tuple(ignition::math::Vector3d(20.0, 0.0, 0.0), + ignition::math::Vector3d(10.0, 0.0, 0.0))); + EXPECT_NO_THROW(s1->NewSplineLane({"l1"}, control_points, 5.)); EXPECT_EQ(s1->num_lanes(), 1); - EXPECT_NO_THROW(s1->NewSplineLane({"l2"}, 5.)); + EXPECT_NO_THROW(s1->NewSplineLane({"l2"}, control_points, 5.)); EXPECT_EQ(s1->num_lanes(), 2); diff --git a/drake/automotive/maliput/rndf/test/spline_helpers_test.cc b/drake/automotive/maliput/rndf/test/spline_helpers_test.cc index 807e5b0fb56e..a0e04910833f 100644 --- a/drake/automotive/maliput/rndf/test/spline_helpers_test.cc +++ b/drake/automotive/maliput/rndf/test/spline_helpers_test.cc @@ -10,6 +10,8 @@ #include "ignition/math/Spline.hh" #include "ignition/math/Vector3.hh" +#include "drake/automotive/maliput/rndf/test/rndf_test_utils.h" + namespace drake { namespace maliput { namespace rndf { @@ -21,16 +23,6 @@ const double kLinearTolerance = 1e-4; // length coordinate to a point in space. const double kLinearStep = 1e-2; -#define EXPECT_IGN_VECTOR_NEAR(actual_arg, expected_arg, tolerance_arg) \ - do { \ - const ignition::math::Vector3d actual(actual_arg); \ - const ignition::math::Vector3d expected(expected_arg); \ - const double tolerance(tolerance_arg); \ - EXPECT_NEAR(actual.X(), expected.X(), tolerance); \ - EXPECT_NEAR(actual.Y(), expected.Y(), tolerance); \ - EXPECT_NEAR(actual.Z(), expected.Z(), tolerance); \ - } while (0) - // This is a wrapper to easily create an ignition::math::Spline. // @param points a tuple consisting of two ignition::math::Vector3d. The // first one is the position of the point and the second one is the tangent diff --git a/drake/automotive/maliput/rndf/test/spline_lane_test.cc b/drake/automotive/maliput/rndf/test/spline_lane_test.cc new file mode 100644 index 000000000000..60d6f6c9dc2a --- /dev/null +++ b/drake/automotive/maliput/rndf/test/spline_lane_test.cc @@ -0,0 +1,266 @@ +#include +#include +#include +#include +#include +#include + +#include + +#include "ignition/math/Spline.hh" +#include "ignition/math/Vector3.hh" + +#include "drake/automotive/maliput/api/lane_data.h" +#include "drake/automotive/maliput/rndf/junction.h" +#include "drake/automotive/maliput/rndf/lane.h" +#include "drake/automotive/maliput/rndf/road_geometry.h" +#include "drake/automotive/maliput/rndf/segment.h" +#include "drake/automotive/maliput/rndf/spline_helpers.h" +#include "drake/automotive/maliput/rndf/spline_lane.h" +#include "drake/automotive/maliput/rndf/test/rndf_test_utils.h" + +namespace drake { +namespace maliput { +namespace rndf { + +// Angular tolerance will be used to match the angle values from the orientation +// matrices. +const double kAngularTolerance = 1e-6; +// For path length computations we expect to have this error or less as the +// SplineLane class provides this accuracy. +const double kPathTolerance = 1e-6; +// Use this tolerance when the API call should answer the same exact result as +// the expected value without adding any new operation in the middle. +const double kVeryExact = 1e-12; + +// This is a wrapper to easily create an ignition::math::Spline. +// @param points a tuple consisting of two ignition::math::Vector3d. The +// first one is the position of the point and the second one is the tangent +// vector or the spline first derivative at that point. +// @return a pointer to an ignition::math::Spline object. +std::unique_ptr CreateSpline( + const std::vector>& points) { + auto spline = std::make_unique(); + spline->AutoCalculate(true); + for (const auto& point : points) { + spline->AddPoint(std::get<0>(point), std::get<1>(point)); + } + return spline; +} + +// Makes geometric tests over a straight line lane. +GTEST_TEST(RNDFSplineLanesTest, FlatLineLane) { + const double width = 5.; + RoadGeometry rg({"FlatLineLane"}, kPathTolerance, kAngularTolerance); + Segment* s1 = rg.NewJunction({"j1"})->NewSegment({"s1"}); + std::vector> + control_points; + control_points.push_back( + std::make_tuple(ignition::math::Vector3d(0.0, 0.0, 0.0), + ignition::math::Vector3d(10.0, 0.0, 0.0))); + control_points.push_back( + std::make_tuple(ignition::math::Vector3d(20.0, 0.0, 0.0), + ignition::math::Vector3d(10.0, 0.0, 0.0))); + Lane* l1 = s1->NewSplineLane({"l1"}, control_points, width); + // Check road geometry invariants + EXPECT_EQ(rg.CheckInvariants(), std::vector()); + // Check lane length + EXPECT_NEAR(l1->length(), std::sqrt((20. * 20.) + (0. * 0.)), kPathTolerance); + // Check bounds + EXPECT_NEAR(l1->lane_bounds(0.).r_min, -width / 2., kVeryExact); + EXPECT_NEAR(l1->lane_bounds(0.).r_max, width / 2., kVeryExact); + EXPECT_NEAR(l1->driveable_bounds(0.).r_min, -width / 2., kVeryExact); + EXPECT_NEAR(l1->driveable_bounds(0.).r_max, width / 2., kVeryExact); + + // ToGeoPosition. + // Reference line. + // At the beginning, end and middle. + EXPECT_GEO_NEAR(l1->ToGeoPosition(api::LanePosition(0., 0., 0.)), + api::GeoPosition(0., 0., 0.), kPathTolerance); + EXPECT_GEO_NEAR(l1->ToGeoPosition(api::LanePosition(20., 0., 0.)), + api::GeoPosition(20., 0., 0.), kPathTolerance); + EXPECT_GEO_NEAR(l1->ToGeoPosition(api::LanePosition(10., 0., 0.)), + api::GeoPosition(10., 0., 0.), kPathTolerance); + + // A couple of meters away of the reference baseline. + EXPECT_GEO_NEAR(l1->ToGeoPosition(api::LanePosition(5., 2., 0.)), + api::GeoPosition(5., 2., 0.), kPathTolerance); + EXPECT_GEO_NEAR(l1->ToGeoPosition(api::LanePosition(12., -2., 0.)), + api::GeoPosition(12., -2., 0.), kPathTolerance); + EXPECT_GEO_NEAR(l1->ToGeoPosition(api::LanePosition(18., 1.234567, 0.)), + api::GeoPosition(18., 1.234567, 0.), kPathTolerance); + + // Orientation. + EXPECT_ROT_NEAR(l1->GetOrientation(api::LanePosition(0., 0., 0.)), + api::Rotation::FromRpy(0., 0., 0.), kAngularTolerance); + EXPECT_ROT_NEAR(l1->GetOrientation(api::LanePosition(20., 0., 0.)), + api::Rotation::FromRpy(0., 0., 0.), kAngularTolerance); + EXPECT_ROT_NEAR(l1->GetOrientation(api::LanePosition(10., 2., 0.)), + api::Rotation::FromRpy(0., 0., 0.), kAngularTolerance); + EXPECT_ROT_NEAR(l1->GetOrientation(api::LanePosition(10., -2., 0.)), + api::Rotation::FromRpy(0., 0., 0.), kAngularTolerance); + + // EvalMotionDerivatives. + EXPECT_LANE_NEAR(l1->EvalMotionDerivatives({0., 0., 0.}, {0., 0., 0.}), + api::LanePosition(0., 0., 0.), kPathTolerance); + EXPECT_LANE_NEAR(l1->EvalMotionDerivatives({0., 0., 0.}, {1., 0., 0.}), + api::LanePosition(1., 0., 0.), kPathTolerance); + EXPECT_LANE_NEAR(l1->EvalMotionDerivatives({0., 0., 0.}, {0., 1., 0.}), + api::LanePosition(0., 1., 0.), kPathTolerance); + EXPECT_LANE_NEAR(l1->EvalMotionDerivatives({0., 0., 0.}, {0., 0., 1.}), + api::LanePosition(0., 0., 1.), kPathTolerance); + EXPECT_LANE_NEAR(l1->EvalMotionDerivatives({10., 2., 5.}, {0., 0., 1.}), + api::LanePosition(0., 0., 1.), kPathTolerance); +} + +// Makes geometric tests over a curved spline. +GTEST_TEST(RNDFSplineLanesTest, CurvedLineLane) { + const double width = 5.; + RoadGeometry rg({"CurvedLineLane"}, kPathTolerance, kAngularTolerance); + Segment* s1 = rg.NewJunction({"j1"})->NewSegment({"s1"}); + std::vector> + control_points; + control_points.push_back( + std::make_tuple(ignition::math::Vector3d(0.0, 0.0, 0.0), + ignition::math::Vector3d(10.0, 0.0, 0.0))); + control_points.push_back( + std::make_tuple(ignition::math::Vector3d(20.0, 20.0, 0.0), + ignition::math::Vector3d(0, 10.0, 0.0))); + Lane* l1 = s1->NewSplineLane({"l1"}, control_points, width); + // Check road geometry invariants. + EXPECT_EQ(rg.CheckInvariants(), std::vector()); + // Create a spline to check the internal values. + std::unique_ptr spline = CreateSpline(control_points); + auto arc_length_interpolator = std::make_unique( + std::move(spline), SplineLane::SplineErrorBound()); + // Check length. + const double s_total = arc_length_interpolator->BaseSpline()->ArcLength(); + EXPECT_NEAR(l1->length(), s_total, kPathTolerance); + // Check bounds. + EXPECT_NEAR(l1->lane_bounds(0.).r_min, -width / 2., kVeryExact); + EXPECT_NEAR(l1->lane_bounds(0.).r_max, width / 2., kVeryExact); + EXPECT_NEAR(l1->driveable_bounds(0.).r_min, -width / 2., kVeryExact); + EXPECT_NEAR(l1->driveable_bounds(0.).r_max, width / 2., kVeryExact); + + // ToGeoPosition. + // Reference line. + // At the beginning, end and middle. + { + EXPECT_GEO_NEAR(l1->ToGeoPosition(api::LanePosition(0., 0., 0.)), + api::GeoPosition(0., 0., 0.), kPathTolerance); + EXPECT_GEO_NEAR(l1->ToGeoPosition(api::LanePosition(s_total, 0., 0.)), + api::GeoPosition(20., 20.0, 0.), kPathTolerance); + const ignition::math::Vector3d point = + arc_length_interpolator->InterpolateMthDerivative(0, s_total / 2.); + EXPECT_GEO_NEAR(l1->ToGeoPosition(api::LanePosition(s_total / 2., 0., 0.)), + api::GeoPosition(point.X(), point.Y(), 0.), kPathTolerance); + } + + // On the plane but at any point over the road. + { + const ignition::math::Vector3d point = + arc_length_interpolator->InterpolateMthDerivative(0, s_total / 2.); + const ignition::math::Vector3d tangent = + arc_length_interpolator->InterpolateMthDerivative(1, s_total / 2.); + const double yaw = std::atan2(tangent.Y(), tangent.X()); + const double x_offset = -2. * std::sin(yaw); + const double y_offset = 2. * std::cos(yaw); + EXPECT_GEO_NEAR( + l1->ToGeoPosition(api::LanePosition(s_total / 2., 2., 0.)), + api::GeoPosition(point.X() + x_offset, point.Y() + y_offset, 0.), + kPathTolerance); + } + // Outside the lane constraints. + { + const ignition::math::Vector3d point = + arc_length_interpolator->InterpolateMthDerivative(0, s_total / 2.); + const ignition::math::Vector3d tangent = + arc_length_interpolator->InterpolateMthDerivative(1, s_total / 2.); + const double yaw = std::atan2(tangent.Y(), tangent.X()); + const double x_offset = -width / 2. * std::sin(yaw); + const double y_offset = width / 2. * std::cos(yaw); + EXPECT_GEO_NEAR( + l1->ToGeoPosition(api::LanePosition(s_total / 2., 15., 0.)), + api::GeoPosition(point.X() + x_offset, point.Y() + y_offset, 0.), + kPathTolerance); + } + // Orientation. + { + EXPECT_ROT_NEAR(l1->GetOrientation(api::LanePosition(0., 0., 0.)), + api::Rotation::FromRpy(0., 0., 0.), kAngularTolerance); + EXPECT_ROT_NEAR( + l1->GetOrientation(api::LanePosition( + arc_length_interpolator->BaseSpline()->ArcLength(), 0., 0.)), + api::Rotation::FromRpy(0., 0., M_PI / 2.), kAngularTolerance); + const ignition::math::Vector3d point = + arc_length_interpolator->InterpolateMthDerivative( + 1, arc_length_interpolator->BaseSpline()->ArcLength() / 2.); + const double yaw = std::atan2(point.Y(), point.X()); + EXPECT_ROT_NEAR( + l1->GetOrientation(api::LanePosition( + arc_length_interpolator->BaseSpline()->ArcLength() / 2., 0., 0.)), + api::Rotation::FromRpy(0., 0., yaw), kAngularTolerance); + EXPECT_ROT_NEAR( + l1->GetOrientation(api::LanePosition( + arc_length_interpolator->BaseSpline()->ArcLength() / 2., 2., 0.)), + api::Rotation::FromRpy(0., 0., yaw), kAngularTolerance); + EXPECT_ROT_NEAR( + l1->GetOrientation(api::LanePosition( + arc_length_interpolator->BaseSpline()->ArcLength() / 2., -2., 0.)), + api::Rotation::FromRpy(0., 0., yaw), kAngularTolerance); + } + // EvalMotionDerivatives. + { + EXPECT_LANE_NEAR(l1->EvalMotionDerivatives({0., 0., 0.}, {0., 0., 0.}), + api::LanePosition(0., 0., 0.), kPathTolerance); + EXPECT_LANE_NEAR(l1->EvalMotionDerivatives({0., 0., 0.}, {1., 0., 0.}), + api::LanePosition(1., 0., 0.), kPathTolerance); + EXPECT_LANE_NEAR(l1->EvalMotionDerivatives({0., 0., 0.}, {0., 1., 0.}), + api::LanePosition(0., 1., 0.), kPathTolerance); + EXPECT_LANE_NEAR(l1->EvalMotionDerivatives({0., 0., 0.}, {0., 0., 1.}), + api::LanePosition(0., 0., 1.), kPathTolerance); + EXPECT_LANE_NEAR(l1->EvalMotionDerivatives({10., 2., 5.}, {0., 0., 1.}), + api::LanePosition(0., 0., 1.), kPathTolerance); + } +} + +// Constructor constraints. +GTEST_TEST(RNDFSplineLanesTest, Constructor) { + std::vector> + control_points; + control_points.push_back( + std::make_tuple(ignition::math::Vector3d(0.0, 0.0, 0.0), + ignition::math::Vector3d(10.0, 0.0, 0.0))); + // Check the constraint in size of control_points. + EXPECT_THROW(SplineLane({"l:1"}, nullptr, control_points, 5.0, 1), + std::runtime_error); + // Adding a second control_point should pass. + control_points.push_back( + std::make_tuple(ignition::math::Vector3d(20.0, 20.0, 0.0), + ignition::math::Vector3d(0, 10.0, 0.0))); + EXPECT_NO_THROW(SplineLane({"l:1"}, nullptr, control_points, 5.0, 1)); +} + +// Tests the ComputeLength computation. +GTEST_TEST(RNDFSplineLanesTest, ComputeLength) { + std::vector> + control_points; + control_points.push_back( + std::make_tuple(ignition::math::Vector3d(0.0, 0.0, 0.0), + ignition::math::Vector3d(10.0, 0.0, 0.0))); + // Check the constraint in size of control_points. + EXPECT_THROW(SplineLane::ComputeLength(control_points), std::runtime_error); + + control_points.push_back( + std::make_tuple(ignition::math::Vector3d(20.0, 20.0, 0.0), + ignition::math::Vector3d(0, 10.0, 0.0))); + auto spline = CreateSpline(control_points); + // Check that length is being correctly computed. + EXPECT_NEAR(spline->ArcLength(), SplineLane::ComputeLength(control_points), + kVeryExact); +} + +} // namespace rndf +} // namespace maliput +} // namespace drake