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"
18 namespace drake {
19 namespace maliput {
20 namespace rndf {
25 class SplineLane : public Lane {
26  public:
30  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;
68  static double ComputeLength(
69  const std::vector<std::tuple<ignition::math::Vector3d,
70  ignition::math::Vector3d>>& control_points);
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., 0.);
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
This file contains abbreviated definitions for certain specializations of Eigen::Matrix that are comm...
A position in 3-dimensional geographical Cartesian space, i.e., in the world frame, consisting of three components x, y, and z.
Definition: lane_data.h:128
double position
Definition: system_identification_test.cc:205
Definition: automotive_demo.cc:88
Eigen::Matrix< Scalar, 2, 1 > Vector2
A column vector of size 2, templated on scalar type.
Definition: eigen_types.h:30
std::vector< double > vector
Definition: translator_test.cc:20
Base class for the RNDF implementation of api::Lane.
Definition: lane.h:27
Isometric velocity vector in a Lane-frame.
Definition: lane_data.h:242
int index() const
Returns the index of this Lane within the Segment which owns it.
Definition: lane.h:45
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:42
This file includes conversions from roll-pitch-yaw representation, that do not depend on quaternion...
Bounds in the lateral dimension (r component) of a Lane-frame, consisting of a pair of minimum and ma...
Definition: lane_data.h:274
A 3-dimensional rotation.
Definition: lane_data.h:53
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:309
~SplineLane() override=default
A Segment represents a bundle of adjacent Lanes which share a continuously traversable road surface...
Definition: segment.h:27
A 3-dimensional position in a Lane-frame, consisting of three components:
Definition: lane_data.h:187
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:35
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...