Drake
spline_lane.h
Go to the documentation of this file.
1 #pragma once
2 
3 #include <memory>
4 #include <tuple>
5 #include <vector>
6 
7 #include <Eigen/Core>
8 #include "ignition/math/Spline.hh"
9 #include "ignition/math/Vector3.hh"
10 
17 
18 namespace drake {
19 namespace maliput {
20 namespace rndf {
21 
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:
28 
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);
57 
58  ~SplineLane() override = default;
59 
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);
71 
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; }
77 
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;
84 
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;
91 
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;
97 
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;
105 
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;
110 
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;
115 
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;
119 
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;
127 
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;
132 
133  // Returns the length of the curve.
134  double do_length() const override {
135  return spline_->BaseSpline()->ArcLength();
136  }
137 
138  // TODO(@agalbachicar) Not implemented yet.
139  api::HBounds do_elevation_bounds(double, double) const override {
140  return api::HBounds(0., 20.);
141  }
142 
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;
147 
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;
159 
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;
175 
176  std::unique_ptr<ArcLengthParameterizedSpline> spline_;
177 
178  const double width_{};
179 
180  static const double kSplineErrorBound;
181 };
182 
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
This file contains abbreviated definitions for certain specializations of Eigen::Matrix that are comm...
Definition: automotive_demo.cc:88
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:266
int index() const
Returns the index of this Lane within the Segment which owns it.
Definition: lane.h:47
TypeSpecificIdentifier<T> represents an identifier specifically identifying an entity of type T...
Definition: type_specific_identifier.h:38
const Segment * segment() const
Returns the Segment to which this Lane belongs.
Definition: lane.h:44
This file includes conversions from roll-pitch-yaw representation, that do not depend on quaternion...
GeoPositionT< double > GeoPosition
Definition: lane_data.h:178
Bounds in the lateral dimension (r component) of a Lane-frame, consisting of a pair of minimum and ma...
Definition: lane_data.h:298
A 3-dimensional rotation.
Definition: lane_data.h:52
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:343
~SplineLane() override=default
A Segment represents a bundle of adjacent Lanes which share a continuously traversable road surface...
Definition: segment.h:27
#define DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(Classname)
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
Provides careful macros to selectively enable or disable the special member functions for copy-constr...