diff --git a/drake/automotive/maliput/rndf/BUILD b/drake/automotive/maliput/rndf/BUILD index eaca8ed2b946..a103c91ae992 100644 --- a/drake/automotive/maliput/rndf/BUILD +++ b/drake/automotive/maliput/rndf/BUILD @@ -67,8 +67,19 @@ 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", ], @@ -81,6 +92,14 @@ drake_cc_googletest( ], ) +drake_cc_googletest( + name = "spline_lane_test", + deps = [ + ":lanes", + "//drake/automotive/maliput/api:maliput_types_compare", + ], +) + drake_cc_googletest( name = "spline_helpers_test", deps = [ 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..7daca5761bbb 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 id This segment's ID. + /// @param junction The api::Junction that contains this Segment. It must + /// remain valid for the lifetime of this object. 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 id 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..4e5980e99d79 100644 --- a/drake/automotive/maliput/rndf/spline_lane.cc +++ b/drake/automotive/maliput/rndf/spline_lane.cc @@ -1,12 +1,202 @@ #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 Matrix3 rotation_matrix = Rabg_of_s(s); + // Rotate (0,r,h) and sum with mapped (s,0,h). + const Vector3 xyz = + rotation_matrix * Vector3(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()); + // Gets the rotation matrix at path length position s. As RNDF does not + // provide information of height, only yaw angle is set. + return api::Rotation::FromRpy(0.0, 0.0, heading_of_s(s)); +} + +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 { + // Let xy_of_s be called L, which is a function + // R --> R^2. We discard the 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 { + // Computes 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 + // takes the form: h(s) = R --> R or h(f(x, y)) where f is + // a function defined by y / x. y and x are the components + // of the first derivative of L. Thus, we get: 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 get its derivative by + // 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))} + // Since x(s) and y(s) are 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)); +} + +Matrix3 SplineLane::Rabg_of_s(double s) const { + return drake::math::ZRotation(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 the 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..97c1f06e8611 100644 --- a/drake/automotive/maliput/rndf/spline_lane.h +++ b/drake/automotive/maliput/rndf/spline_lane.h @@ -1,51 +1,184 @@ #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. +/// 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's first value is the knot point while its second value + /// is the tangent at the knot point. + /// @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. + /// @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 matrix 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. + Matrix3 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 if the lane referred by @p lane_id has no intersection with this + // lane's normal at @p s, or the two lines are 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/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 b5cbe46544d5..9d90b42899f7 100644 --- a/drake/automotive/maliput/rndf/test/spline_helpers_test.cc +++ b/drake/automotive/maliput/rndf/test/spline_helpers_test.cc @@ -46,11 +46,11 @@ GTEST_TEST(RNDFSplineHelperTest, ExceptionsInInverseFunctionInterpolator) { EXPECT_THROW( InverseFunctionInterpolator([](double t) { return t; }, 0., 1., -1.), std::runtime_error); - // xmin is equal to xmax + // xmin is equal to xmax. EXPECT_THROW( InverseFunctionInterpolator([](double t) { return t; }, 0., 0., -1.), std::runtime_error); - // xmin is greater than xmax + // xmin is greater than xmax. EXPECT_THROW( InverseFunctionInterpolator([](double t) { return t; }, 1., 0., -1.), std::runtime_error); @@ -71,7 +71,7 @@ GTEST_TEST(RNDFSplineHelperTest, ExceptionsInInverseFunctionInterpolator) { // Tests the ArcLengthParameterizedSpline exceptions. GTEST_TEST(RNDFSplineHelperTest, ExceptionsInArcLengthParameterizedSpline) { std::unique_ptr spline; - // Spline pointer set to nullptr + // Spline pointer set to nullptr. EXPECT_THROW( ArcLengthParameterizedSpline(std::move(spline), kLinearTolerance), std::runtime_error); @@ -135,7 +135,7 @@ GTEST_TEST(RNDFSplineHelperTest, StraightSplineFindClosesPointTo) { std::unique_ptr spline = CreateSpline(control_points); auto arc_length_param_spline = std::make_unique( std::move(spline), kLinearTolerance); - // Border checks + // Border checks. EXPECT_NEAR(arc_length_param_spline->FindClosestPointTo( ignition::math::Vector3d(0.0, 5.0, 0.0), kLinearStep), 0., kLinearTolerance); @@ -148,14 +148,14 @@ GTEST_TEST(RNDFSplineHelperTest, StraightSplineFindClosesPointTo) { EXPECT_NEAR(arc_length_param_spline->FindClosestPointTo( ignition::math::Vector3d(20.0, -5.0, 0.0), kLinearStep), 20., kLinearTolerance); - // Middle checks + // Middle checks. EXPECT_NEAR(arc_length_param_spline->FindClosestPointTo( ignition::math::Vector3d(10.0, 5.0, 0.0), kLinearStep), 10., kLinearTolerance); EXPECT_NEAR(arc_length_param_spline->FindClosestPointTo( ignition::math::Vector3d(10.0, -5.0, 0.0), kLinearStep), 10., kLinearTolerance); - // Before and after checks + // Before and after checks. EXPECT_NEAR(arc_length_param_spline->FindClosestPointTo( ignition::math::Vector3d(-5, -5.0, 0.0), kLinearStep), 0., kLinearTolerance); 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..c8fc08f7b1b6 --- /dev/null +++ b/drake/automotive/maliput/rndf/test/spline_lane_test.cc @@ -0,0 +1,323 @@ +#include "drake/automotive/maliput/rndf/spline_lane.h" + +#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/api/test/maliput_types_compare.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" + +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 control_points A vector of tuples consisting of two +// ignition::math::Vector3d objects. The tuple's first value is the knot point +// while its second value is the tangent at the knot point. +// @return a pointer to an ignition::math::Spline object. +std::unique_ptr CreateSpline( + const std::vector>& control_points) { + auto spline = std::make_unique(); + spline->AutoCalculate(true); + for (const auto& control_point : control_points) { + spline->AddPoint(std::get<0>(control_point), std::get<1>(control_point)); + } + return spline; +} + +// Performs geometric tests over a straight lane. +GTEST_TEST(RNDFSplineLanesTest, FlatLineLane) { + const double kWidth = 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))); + const Lane* l1 = s1->NewSplineLane({"l1"}, control_points, kWidth); + // Checks road geometry invariants. + EXPECT_EQ(rg.CheckInvariants(), std::vector()); + // Checks lane length. + EXPECT_NEAR(l1->length(), std::sqrt((20. * 20.) + (0. * 0.)), kPathTolerance); + // Checks bounds. + EXPECT_TRUE(api::test::IsRBoundsClose(l1->lane_bounds(0.), + api::RBounds(-kWidth / 2., kWidth / 2.), + kVeryExact)); + EXPECT_TRUE(api::test::IsRBoundsClose(l1->driveable_bounds(0.), + api::RBounds(-kWidth / 2., kWidth / 2.), + kVeryExact)); + + // ToGeoPosition. + // Reference line. + // At the beginning, end and middle. + EXPECT_TRUE(api::test::IsGeoPositionClose( + l1->ToGeoPosition(api::LanePosition(0., 0., 0.)), + api::GeoPosition(0., 0., 0.), + kPathTolerance)); + EXPECT_TRUE(api::test::IsGeoPositionClose( + l1->ToGeoPosition(api::LanePosition(20., 0., 0.)), + api::GeoPosition(20., 0., 0.), + kPathTolerance)); + EXPECT_TRUE(api::test::IsGeoPositionClose( + l1->ToGeoPosition(api::LanePosition(10., 0., 0.)), + api::GeoPosition(10., 0., 0.), + kPathTolerance)); + + // A couple of meters away of the reference baseline. + EXPECT_TRUE(api::test::IsGeoPositionClose( + l1->ToGeoPosition(api::LanePosition(5., 2., 0.)), + api::GeoPosition(5., 2., 0.), + kPathTolerance)); + EXPECT_TRUE(api::test::IsGeoPositionClose( + l1->ToGeoPosition(api::LanePosition(12., -2., 0.)), + api::GeoPosition(12., -2., 0.), + kPathTolerance)); + EXPECT_TRUE(api::test::IsGeoPositionClose( + l1->ToGeoPosition(api::LanePosition(18., 1.234567, 0.)), + api::GeoPosition(18., 1.234567, 0.), + kPathTolerance)); + + // Orientation. + EXPECT_TRUE(api::test::IsRotationClose( + l1->GetOrientation(api::LanePosition(0., 0., 0.)), + api::Rotation::FromRpy(0., 0., 0.), + kAngularTolerance)); + EXPECT_TRUE(api::test::IsRotationClose( + l1->GetOrientation(api::LanePosition(20., 0., 0.)), + api::Rotation::FromRpy(0., 0., 0.), + kAngularTolerance)); + EXPECT_TRUE(api::test::IsRotationClose( + l1->GetOrientation(api::LanePosition(10., 2., 0.)), + api::Rotation::FromRpy(0., 0., 0.), + kAngularTolerance)); + EXPECT_TRUE(api::test::IsRotationClose( + l1->GetOrientation(api::LanePosition(10., -2., 0.)), + api::Rotation::FromRpy(0., 0., 0.), + kAngularTolerance)); + + // EvalMotionDerivatives. + EXPECT_TRUE(api::test::IsLanePositionClose( + l1->EvalMotionDerivatives({0., 0., 0.}, {0., 0., 0.}), + api::LanePosition(0., 0., 0.), + kPathTolerance)); + EXPECT_TRUE(api::test::IsLanePositionClose( + l1->EvalMotionDerivatives({0., 0., 0.}, {1., 0., 0.}), + api::LanePosition(1., 0., 0.), + kPathTolerance)); + EXPECT_TRUE(api::test::IsLanePositionClose( + l1->EvalMotionDerivatives({0., 0., 0.}, {0., 1., 0.}), + api::LanePosition(0., 1., 0.), + kPathTolerance)); + EXPECT_TRUE(api::test::IsLanePositionClose( + l1->EvalMotionDerivatives({0., 0., 0.}, {0., 0., 1.}), + api::LanePosition(0., 0., 1.), + kPathTolerance)); + EXPECT_TRUE(api::test::IsLanePositionClose( + l1->EvalMotionDerivatives({10., 2., 5.}, {0., 0., 1.}), + api::LanePosition(0., 0., 1.), + kPathTolerance)); +} + +// Performs geometric tests over a curved lane. +GTEST_TEST(RNDFSplineLanesTest, CurvedLineLane) { + const double kWidth = 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))); + const Lane* l1 = s1->NewSplineLane({"l1"}, control_points, kWidth); + // Checks road geometry invariants. + EXPECT_EQ(rg.CheckInvariants(), std::vector()); + // Creates 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()); + // Checks length. + const double s_total = arc_length_interpolator->BaseSpline()->ArcLength(); + EXPECT_NEAR(l1->length(), s_total, kPathTolerance); + // Checks bounds. + EXPECT_TRUE(api::test::IsRBoundsClose(l1->lane_bounds(0.), + api::RBounds(-kWidth / 2., kWidth / 2.), + kVeryExact)); + EXPECT_TRUE(api::test::IsRBoundsClose(l1->driveable_bounds(0.), + api::RBounds(-kWidth / 2., kWidth / 2.), + kVeryExact)); + + // ToGeoPosition. + // Reference line. + // At the beginning, end and middle. + { + EXPECT_TRUE(api::test::IsGeoPositionClose( + l1->ToGeoPosition(api::LanePosition(0., 0., 0.)), + api::GeoPosition(0., 0., 0.), + kPathTolerance)); + EXPECT_TRUE(api::test::IsGeoPositionClose( + 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_TRUE(api::test::IsGeoPositionClose( + 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_TRUE(api::test::IsGeoPositionClose( + 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 = -kWidth / 2. * std::sin(yaw); + const double y_offset = kWidth / 2. * std::cos(yaw); + EXPECT_TRUE(api::test::IsGeoPositionClose( + l1->ToGeoPosition(api::LanePosition(s_total / 2., 15., 0.)), + api::GeoPosition(point.X() + x_offset, point.Y() + y_offset, 0.), + kPathTolerance)); + } + // Orientation. + { + EXPECT_TRUE(api::test::IsRotationClose( + l1->GetOrientation(api::LanePosition(0., 0., 0.)), + api::Rotation::FromRpy(0., 0., 0.), + kAngularTolerance)); + EXPECT_TRUE(api::test::IsRotationClose( + 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_TRUE(api::test::IsRotationClose( + l1->GetOrientation(api::LanePosition( + arc_length_interpolator->BaseSpline()->ArcLength() / 2., 0., 0.)), + api::Rotation::FromRpy(0., 0., yaw), + kAngularTolerance)); + EXPECT_TRUE(api::test::IsRotationClose( + l1->GetOrientation(api::LanePosition( + arc_length_interpolator->BaseSpline()->ArcLength() / 2., 2., 0.)), + api::Rotation::FromRpy(0., 0., yaw), + kAngularTolerance)); + EXPECT_TRUE(api::test::IsRotationClose( + l1->GetOrientation(api::LanePosition( + arc_length_interpolator->BaseSpline()->ArcLength() / 2., -2., 0.)), + api::Rotation::FromRpy(0., 0., yaw), + kAngularTolerance)); + } + // EvalMotionDerivatives. + { + EXPECT_TRUE(api::test::IsLanePositionClose( + l1->EvalMotionDerivatives({0., 0., 0.}, {0., 0., 0.}), + api::LanePosition(0., 0., 0.), + kPathTolerance)); + EXPECT_TRUE(api::test::IsLanePositionClose( + l1->EvalMotionDerivatives({0., 0., 0.}, {1., 0., 0.}), + api::LanePosition(1., 0., 0.), + kPathTolerance)); + EXPECT_TRUE(api::test::IsLanePositionClose( + l1->EvalMotionDerivatives({0., 0., 0.}, {0., 1., 0.}), + api::LanePosition(0., 1., 0.), + kPathTolerance)); + EXPECT_TRUE(api::test::IsLanePositionClose( + l1->EvalMotionDerivatives({0., 0., 0.}, {0., 0., 1.}), + api::LanePosition(0., 0., 1.), + kPathTolerance)); + EXPECT_TRUE(api::test::IsLanePositionClose( + 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