Go to the documentation of this file.
1 #pragma once
3 #include <memory>
4 #include <tuple>
5 #include <vector>
7 #include <Eigen/Core>
8 #include "ignition/math/Spline.hh"
9 #include "ignition/math/Vector3.hh"
11 #include "drake/automotive/maliput/api/lane_data.h"
12 #include "drake/automotive/maliput/rndf/lane.h"
13 #include "drake/automotive/maliput/rndf/spline_helpers.h"
14 #include "drake/common/drake_copyable.h"
15 #include "drake/common/eigen_types.h"
16 #include "drake/math/roll_pitch_yaw_not_using_quaternion.h"
18 namespace drake {
19 namespace maliput {
20 namespace rndf {
22 /// Specialization of drake::maliput::rndf::Lane with a spline curve as its
23 /// reference path. RNDF specification lacks elevation and superelevation so
24 /// all the Lanes will depict a flat path just over the ground.
25 class SplineLane : public Lane {
26  public:
29  /// Constructs a SplineLane, a lane specified by a spline segment defined in
30  /// the xy-plane (the ground plane) of the world frame.
31  ///
32  /// @param id The ID of the api::Lane.
33  /// @param segment A pointer to the segment that contains this lane, which
34  /// must remain valid for the lifetime of this class.
35  /// @param control_points A collection of knots and their tangents where
36  /// the interpolated curve will pass. There must be at least two control
37  /// points. The tuple's first value is the knot point while its second value
38  /// is the tangent at the knot point.
39  /// @param width The width of the lane based on the RNDF
40  /// lane_width parameter. It will be used to set a constant lane_bound
41  /// value along the road.
42  /// @param index The index that can be used to reference this Lane from
43  /// api::Segment::lane() call.
44  ///
45  /// This implementation uses ignition::math::Spline and
46  /// ArcLengthParameterizedSpline which is used as the inverse function
47  /// that maps the s parameter of s,r,h frame to ignition::math's Spline t
48  /// parameter.
49  ///
50  /// @throws std::runtime_error When the size of @p control_points is less than
51  /// 2.
52  SplineLane(
53  const api::LaneId& id, const api::Segment* segment,
54  const std::vector<std::tuple<ignition::math::Vector3d,
55  ignition::math::Vector3d>>& control_points,
56  double width, int index);
58  ~SplineLane() override = default;
60  /// Computes the length of a SplineLane created using @p control_points.
61  /// @param control_points A collection of knots and their tangents where
62  /// the interpolated curve will pass. There must be at least two control
63  /// points. The tuple's first value is the knot point while its second value
64  /// is the tangent at the knot point.
65  /// @return The length of the baseline computed as a spline.
66  /// @throws std::runtime_error When the size of @p control_points is less than
67  /// 2.
68  static double ComputeLength(
69  const std::vector<std::tuple<ignition::math::Vector3d,
70  ignition::math::Vector3d>>& control_points);
72  /// @return the error bound that the path length interpolator will
73  /// attempt to attain when approximating the inverse function that maps
74  /// the s coordinate of api::LanePosition frame into the t parameter that
75  /// ignition::math::Spline uses to evaluate the function.
76  static double SplineErrorBound() { return kSplineErrorBound; }
78  private:
79  // This function is not implemented and will abort if called.
80  // TODO(@agalbachicar) We need to find a way to implement it.
81  api::LanePosition DoToLanePosition(const api::GeoPosition& geo_pos,
82  api::GeoPosition* nearest_point,
83  double* distance) const override;
85  // Provides the api::GeoPosition at the @p lane_pos over the lane.
86  // s coordinate of @p lane_pos will be saturated to be processed in the range
87  // of 0.0 to the lane length. The same is applied to r coordinate which is
88  // saturated to the driveable_bounds at s coordinate (after the saturation).
89  api::GeoPosition DoToGeoPosition(
90  const api::LanePosition& lane_pos) const override;
92  // Provides the api::Rotation matrix against the origin of the world frame at
93  // @p lane_pos coordinate over the lane. The s coordinate of @p lane_pose will
94  // be saturated to be in the range of 0.0 to the lane length.
95  api::Rotation DoGetOrientation(
96  const api::LanePosition& lane_pos) const override;
98  // As we are moving along the lane with path length coordinates we will
99  // return the velocity as is and will not scale through the chain rule. This
100  // is because the returned api::LanePosition is isotropic, i.e., it contains
101  // real physical values.
102  api::LanePosition DoEvalMotionDerivatives(
104  const api::IsoLaneVelocity& velocity) const override;
106  // Provides the x,y coordinates based on the
107  // ArcLengthParameterizedSpline that will provide the interpolation
108  // image at a path length s from the beginning of the SplineLane.
109  Vector2<double> xy_of_s(double s) const;
111  // Provides the x,y tangent values based on the ArcLengthParameterizedSpline
112  // that will provide the interpolation image at a path length s from the
113  // beginning of the SplineLane.
114  Vector2<double> xy_dot_of_s(double s) const;
116  // Provides the angle of the tangent vector evaluated at a path length
117  // s distance from the beginning of the SplineLane.
118  double heading_of_s(double s) const;
120  // Provides the derivative of the angle at a path length s from the beginning
121  // of the SplineLane. Given that x, y and z components of
122  // ignition::math::Spline are independent from each other and a function of s
123  // (given the inverse function approximation provided by
124  // ArcLengthParameterizedSpline) we can apply the chain rule and
125  // the obtain derivative of the heading.
126  double heading_dot_of_s(double s) const;
128  // Provides a rotation matrix in terms of Euler angles, with only
129  // yaw angle set as RNDF is defined without any change in elevation.
130  // See heading_of_s for more information.
131  Matrix3<double> Rabg_of_s(double s) const;
133  // Returns the length of the curve.
134  double do_length() const override {
135  return spline_->BaseSpline()->ArcLength();
136  }
138  // TODO(@agalbachicar) Not implemented yet.
139  api::HBounds do_elevation_bounds(double, double) const override {
140  return api::HBounds(0., 20.);
141  }
143  // Computes the lane_bounds taking into account the Lane::width. Based
144  // on it, it constructs an api::RBounds object whose r_min variable is
145  // half the width but negative, and the r_max variable is half the width.
146  api::RBounds do_lane_bounds(double s) const override;
148  // If the segment has only one lane, this one, the return value will be
149  // identical to do_lane_bounds(s). However, for the general case, each lane
150  // is approximated by a straight line from the beginning to the end of it.
151  // Then, we compute the intersection point of the current normal (following
152  // the s direction it will be coincident with the r direction) at s with that
153  // line approximation. Based on the intersection points with the lanes set as
154  // extents of the segment (the first and last ones) we can determine the
155  // distance to those limits and create the api::RBounds set as result.
156  // Here, GetPositionToLane() is used to compute the line approximation and
157  // intersection point.
158  api::RBounds do_driveable_bounds(double s) const override;
160  // Computes the intersection point of the normal line that intersects the
161  // current lane at @p s (r = 0, h = 0) with the lane whose index is
162  // lane_id. The function of the lane with @p lane_id is approximated by a line
163  // defined by the api::GeoPosition values of both the beginning and ending
164  // of it.
165  // It returns the intersection point on the lane whose index is lane_id.
166  // If @p lane_id is equal to the Lane::index, then it returns the geo position
167  // at (s, 0, 0).
168  // Aborts if lane_id does not refer to a valid lane ID for the lane's segment.
169  // Aborts if the lane referred by @p lane_id has no intersection with this
170  // lane's normal at @p s, or the two lines are collinear and coincident.
171  // The implementation is a transcription of this comment:
172  // https://stackoverflow.com/a/565282. It was adapted for the purposes of this
173  // code (no support for collinear and non intersecting lines).
174  ignition::math::Vector3d GetPositionToLane(double s, int lane_id) const;
176  std::unique_ptr<ArcLengthParameterizedSpline> spline_;
178  const double width_{};
180  static const double kSplineErrorBound;
181 };
183 } // namespace rndf
184 } // namespace maliput
185 } // namespace drake
static double SplineErrorBound()
Definition: spline_lane.h:76
Specialization of drake::maliput::rndf::Lane with a spline curve as its reference path...
Definition: spline_lane.h:25
Definition: automotive_demo.cc:89
const double position
Definition: robot_plan_interpolator_test.cc:65
Eigen::Matrix< Scalar, 2, 1 > Vector2
A column vector of size 2, templated on scalar type.
Definition: eigen_types.h:30
Definition: branch_point.cc:7
std::vector< double > vector
Definition: translator_test.cc:20
Base class for the RNDF implementation of api::Lane.
Definition: lane.h:28
Isometric velocity vector in a Lane-frame.
Definition: lane_data.h:286
int index() const
Returns the index of this Lane within the Segment which owns it.
Definition: lane.h:48
TypeSpecificIdentifier<T> represents an identifier specifically identifying an entity of type T...
Definition: type_specific_identifier.h:39
const Segment * segment() const
Returns the Segment to which this Lane belongs.
Definition: lane.h:45
GeoPositionT< double > GeoPosition
Definition: lane_data.h:189
Bounds in the lateral dimension (r component) of a Lane-frame, consisting of a pair of minimum and ma...
Definition: lane_data.h:318
A 3-dimensional rotation.
Definition: lane_data.h:54
static double ComputeLength(const std::vector< std::tuple< ignition::math::Vector3d, ignition::math::Vector3d >> &control_points)
Computes the length of a SplineLane created using control_points.
Definition: spline_lane.cc:191
Bounds in the elevation dimension (h component) of a Lane-frame, consisting of a pair of minimum and ...
Definition: lane_data.h:363
~SplineLane() override=default
A Segment represents a bundle of adjacent Lanes which share a continuously traversable road surface...
Definition: segment.h:27
DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN deletes the special member functions for copy-construction, copy-assignment, move-construction, and move-assignment.
Definition: drake_copyable.h:33
SplineLane(const SplineLane &)=delete
Vector6< double > velocity
Definition: pose_smoother.cc:29