Drake
lane.h
Go to the documentation of this file.
1 #pragma once
2 
3 #include <cmath>
4 #include <memory>
5 
6 #include <Eigen/Dense>
7 
13 #include "drake/common/unused.h"
15 
16 namespace drake {
17 namespace maliput {
18 namespace monolane {
19 
21 
22 typedef Vector2<double> V2;
24 
30 class Rot3 {
31  public:
33 
34  Rot3(double roll, double pitch, double yaw) : rpy_(roll, pitch, yaw) {}
35 
37  V3 apply(const V3& in) const { return math::rpy2rotmat(rpy_) * in; }
38 
39  double yaw() const { return rpy_(2); }
40  double pitch() const { return rpy_(1); }
41  double roll() const { return rpy_(0); }
42 
43  private:
44  Eigen::Matrix<double, 3, 1, Eigen::DontAlign> rpy_;
45 };
46 
47 
50  public:
52 
53 
54  CubicPolynomial() : CubicPolynomial(0., 0., 0., 0.) {}
55 
57  CubicPolynomial(double a, double b, double c, double d)
58  : a_(a), b_(b), c_(c), d_(d) {
59  const double df = f_p(1.) - f_p(0.);
60  s_1_ = std::sqrt(1. + (df * df));
61  }
62 
63  // Returns the a coefficient.
64  double a() const { return a_; }
65 
66  // Returns the b coefficient.
67  double b() const { return b_; }
68 
69  // Returns the c coefficient.
70  double c() const { return c_; }
71 
72  // Returns the d coefficient.
73  double d() const { return d_; }
74 
76  double f_p(double p) const {
77  return a_ + (b_ * p) + (c_ * p * p) + (d_ * p * p * p);
78  }
79 
81  double f_dot_p(double p) const {
82  return b_ + (2. * c_ * p) + (3. * d_ * p * p);
83  }
84 
86  double f_ddot_p(double p) const {
87  return (2. * c_) + (6. * d_ * p);
88  }
89 
90  // TODO(maddog@tri.global) s_p() and p_s() need to be replaced with a
91  // properly integrated path-length parameterization.
92  // For the moment, we are calculating the length by
93  // approximating the curve with a single linear
94  // segment from (0, f(0)) to (1, f(1)), which is
95  // not entirely awful for relatively flat curves.
97  double s_p(double p) const {
98  return s_1_ * p;
99  }
100 
102  double p_s(double s) const {
103  return s / s_1_;
104  }
105 
106  // TODO(maddog@tri.global) Until s(p) is a properly integrated path-length
107  // parameterization, we have a need to calculate the
108  // derivative of the actual linear function
109  // involved in our bogus path-length approximation.
110  double fake_gprime(double p) const {
111  unused(p);
112  // return df; which is...
113  return f_p(1.) - f_p(0.);
114  }
115 
116  private:
117  double a_{};
118  double b_{};
119  double c_{};
120  double d_{};
121  double s_1_{};
122 };
123 
124 
126 class Lane : public api::Lane {
127  public:
129 
130 
131  Lane(const api::LaneId& id, const api::Segment* segment,
181  const api::RBounds& lane_bounds,
182  const api::RBounds& driveable_bounds,
183  const api::HBounds& elevation_bounds,
184  double p_scale,
185  const CubicPolynomial& elevation,
186  const CubicPolynomial& superelevation)
187  : id_(id), segment_(segment),
188  lane_bounds_(lane_bounds),
189  driveable_bounds_(driveable_bounds),
190  elevation_bounds_(elevation_bounds),
191  p_scale_(p_scale),
192  elevation_(elevation),
193  superelevation_(superelevation) {
194  DRAKE_DEMAND(lane_bounds_.min() >= driveable_bounds_.min());
195  DRAKE_DEMAND(lane_bounds_.max() <= driveable_bounds_.max());
196  }
197 
198  // TODO(maddog@tri.global) Allow superelevation to have a center-of-rotation
199  // which is different from r = 0.
200 
201  const CubicPolynomial& elevation() const { return elevation_; }
202 
203  const CubicPolynomial& superelevation() const { return superelevation_; }
204 
205  void SetStartBp(BranchPoint* bp) { start_bp_ = bp; }
206  void SetEndBp(BranchPoint* bp) { end_bp_ = bp; }
207 
208  BranchPoint* start_bp() { return start_bp_; }
209 
210  BranchPoint* end_bp() { return end_bp_; }
211 
212  ~Lane() override = default;
213 
214  private:
215  const api::LaneId do_id() const override { return id_; }
216 
217  const api::Segment* do_segment() const override;
218 
219  int do_index() const override { return 0; } // Only one lane per segment!
220 
221  const api::Lane* do_to_left() const override { return nullptr; }
222 
223  const api::Lane* do_to_right() const override { return nullptr; }
224 
225  const api::BranchPoint* DoGetBranchPoint(
226  const api::LaneEnd::Which which_end) const override;
227 
228  const api::LaneEndSet* DoGetConfluentBranches(
229  const api::LaneEnd::Which which_end) const override;
230 
231  const api::LaneEndSet* DoGetOngoingBranches(
232  const api::LaneEnd::Which which_end) const override;
233 
234  std::unique_ptr<api::LaneEnd> DoGetDefaultBranch(
235  const api::LaneEnd::Which which_end) const override;
236 
237  double do_length() const override;
238 
239  api::RBounds do_lane_bounds(double) const override { return lane_bounds_; }
240 
241  api::RBounds do_driveable_bounds(double) const override {
242  return driveable_bounds_;
243  }
244 
245  api::HBounds do_elevation_bounds(double, double) const override {
246  return elevation_bounds_;
247  }
248 
249  api::GeoPosition DoToGeoPosition(
250  const api::LanePosition& lane_pos) const override;
251 
252  api::Rotation DoGetOrientation(
253  const api::LanePosition& lane_pos) const override;
254 
255  api::LanePosition DoEvalMotionDerivatives(
257  const api::IsoLaneVelocity& velocity) const override;
258 
259  // The following virtual methods define a reference curve in the xy-plane
260  // of the world frame (i.e., the Earth ground plane). The curve is a
261  // parametric curve:
262  //
263  // F: p --> (x, y) for p in [0, 1]
264  //
265  // defined such that parameter p is linear in the path-length of the curve.
266 
267  // Returns the reference curve itself, F(p).
268  virtual V2 xy_of_p(const double p) const = 0;
269 
270  // Returns the derivative of the curve with respect to p, at p,
271  // i.e., F'(p0) = (dx/dp, dy/dp) at p0
272  virtual V2 xy_dot_of_p(const double p) const = 0;
273 
274  // Returns the heading of the curve at p, i.e., the angle of the tangent
275  // vector (with respect to x-axis) in the increasing-p direction.
276  virtual double heading_of_p(const double p) const = 0;
277 
278  // Returns the derivative of the heading with respect to p,
279  // i.e., d_heading/dp evaluated at p.
280  virtual double heading_dot_of_p(const double p) const = 0;
281 
282 
283  // The geometry here revolves around an abstract "world function"
284  //
285  // W: (p,r,h) --> (x,y,z)
286  //
287  // which maps a `Lane`-frame position to its corresponding representation in
288  // world coordinates (with the caveat that instead of the lane's native
289  // longitudinal coordinate 's', the reference curve parameter 'p' is used).
290  //
291  // W is derived from the three functions which define the lane:
292  //
293  // G: p --> (x,y) = the reference curve, a.k.a. xy_of_p()
294  // Z: p --> z / q_max = the elevation function, a.k.a. elevation_
295  // Θ: p --> θ / q_max = the superelevation function, a.k.a. superelevation_
296  //
297  // as:
298  //
299  // (x,y,z) = W(p,r,h) = (G(p), Z(p)) + R_αβγ*(0,r,h)
300  //
301  // where R_αβγ is the roll/pitch/yaw rotation given by angles:
302  //
303  // α = Θ(p)
304  // β = -atan(dZ/dp) at p
305  // γ = atan2(dG_y/dp, dG_x/dp) at p
306  //
307  // (R_αβγ is essentially the orientation of the (s,r,h) `Lane`-frame
308  // at a location (s,0,0) on the reference-line of the lane. However, it
309  // is *not* necessarily the correct orientation at r != 0 or h != 0.)
310  //
311  // The following methods compute various terms derived from the above which
312  // see repeated use.
313 
314  // Returns the parametric position p along the reference curve corresponding
315  // to longitudinal position @p s along the lane.
316  double p_from_s(const double s) const;
317 
318  // Returns the rotation R_αβγ, evaluated at @p p along the reference curve.
319  Rot3 Rabg_of_p(const double p) const;
320 
321  // Returns W' = ∂W/∂p, the partial differential of W with respect to p,
322  // evaluated at @p p, @p r, @p h.
323  //
324  // (@p Rabg must be the result of Rabg_of_p(p) --- passed in here to
325  // avoid recomputing it.)
326  V3 W_prime_of_prh(const double p, const double r, const double h,
327  const Rot3& Rabg) const;
328 
329  // Returns the s-axis unit-vector, expressed in the world frame,
330  // of the (s,r,h) `Lane`-frame (with respect to the world frame).
331  //
332  // (@p Rabg must be the result of Rabg_of_p(p) --- passed in here to
333  // avoid recomputing it.)
334  V3 s_hat_of_prh(const double p, const double r, const double h,
335  const Rot3& Rabg) const;
336 
337  // Returns the r-axis unit-vector, expressed in the world frame,
338  // of the (s,r,h) `Lane`-frame (with respect to the world frame).
339  //
340  // (@p Rabg must be the result of Rabg_of_p(p) --- passed in here to
341  // avoid recomputing it.)
342  V3 r_hat_of_Rabg(const Rot3& Rabg) const;
343 
344  const api::LaneId id_;
345  const api::Segment* segment_{};
346  BranchPoint* start_bp_{};
347  BranchPoint* end_bp_{};
348 
349  const api::RBounds lane_bounds_;
350  const api::RBounds driveable_bounds_;
351  const api::HBounds elevation_bounds_;
352  const double p_scale_{};
353  const CubicPolynomial elevation_;
354  const CubicPolynomial superelevation_;
355 };
356 
357 
358 } // namespace monolane
359 } // namespace maliput
360 } // namespace drake
double b() const
Definition: lane.h:67
BranchPoint * start_bp()
Definition: lane.h:208
double f_dot_p(double p) const
Evaluates the derivative df/dp at p.
Definition: lane.h:81
CubicPolynomial(double a, double b, double c, double d)
Constructs a cubic polynomial given all four coefficients.
Definition: lane.h:57
This file contains abbreviated definitions for certain specializations of Eigen::Matrix that are comm...
A cubic polynomial, f(p) = a + b*p + c*p^2 + d*p^3.
Definition: lane.h:49
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
A BranchPoint is a node in the network of a RoadGeometry at which Lanes connect to one another...
Definition: branch_point.h:66
const CubicPolynomial & superelevation() const
Definition: lane.h:203
double position
Definition: system_identification_test.cc:205
Definition: automotive_demo.cc:88
double d() const
Definition: lane.h:73
const Variable a_
Definition: symbolic_decompose_test.cc:35
Eigen::Matrix< Scalar, 2, 1 > Vector2
A column vector of size 2, templated on scalar type.
Definition: eigen_types.h:30
double pitch() const
Definition: lane.h:40
Vector2< double > V2
Definition: lane.h:20
A set of LaneEnds.
Definition: branch_point.h:22
A Lane represents a lane of travel in a road network.
Definition: lane.h:32
int b
Definition: rgbd_camera.cc:91
double p_s(double s) const
Returns the inverse of the path-length parameterization s_p(p).
Definition: lane.h:102
Isometric velocity vector in a Lane-frame.
Definition: lane_data.h:242
double yaw() const
Definition: lane.h:39
const Variable b_
Definition: symbolic_decompose_test.cc:36
TypeSpecificIdentifier<T> represents an identifier specifically identifying an entity of type T...
Definition: type_specific_identifier.h:38
#define DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(Classname)
DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN defaults the special member functions for copy-construction, copy-assignment, move-construction, and move-assignment.
Definition: drake_copyable.h:59
double s_p(double p) const
Returns the path-length s along the curve (p, f(p)) from p = 0 to p.
Definition: lane.h:97
const CubicPolynomial & elevation() const
Definition: lane.h:201
void SetEndBp(BranchPoint *bp)
Definition: lane.h:206
#define DRAKE_DEMAND(condition)
Evaluates condition and iff the value is false will trigger an assertion failure with a message showi...
Definition: drake_assert.h:47
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
Base class for the monolane implementation of api::Lane.
Definition: lane.h:126
Vector3< double > V3
Definition: lane.h:23
double c() const
Definition: lane.h:70
An R^3 rotation parameterized by roll, pitch, yaw.
Definition: lane.h:30
void SetStartBp(BranchPoint *bp)
Definition: lane.h:205
Bounds in the elevation dimension (h component) of a Lane-frame, consisting of a pair of minimum and ...
Definition: lane_data.h:309
A Segment represents a bundle of adjacent Lanes which share a continuously traversable road surface...
Definition: segment.h:27
Which
Labels for the endpoints of a Lane.
Definition: lane_data.h:25
A 3-dimensional position in a Lane-frame, consisting of three components:
Definition: lane_data.h:187
An implementation of api::BranchPoint.
Definition: branch_point.h:41
BranchPoint * end_bp()
Definition: lane.h:210
double a() const
Definition: lane.h:64
double f_ddot_p(double p) const
Evaluates the double-derivative d^2f/dp^2 at p.
Definition: lane.h:86
Expression sqrt(const Expression &e)
Definition: symbolic_expression.cc:553
double f_p(double p) const
Evaluates the polynomial f at p.
Definition: lane.h:76
V3 apply(const V3 &in) const
Applies the rotation to a 3-vector.
Definition: lane.h:37
double fake_gprime(double p) const
Definition: lane.h:110
Matrix3< typename Derived::Scalar > rpy2rotmat(const Eigen::MatrixBase< Derived > &rpy)
We use an extrinsic rotation about Space-fixed x-y-z axes by angles [rpy(0), rpy(1), rpy(2)].
Definition: roll_pitch_yaw_not_using_quaternion.h:67
int r
Definition: rgbd_camera.cc:89
#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:35
const maliput::api::Segment * segment_
Definition: mobil_planner_test.cc:134
Vector6< double > velocity
Definition: pose_smoother.cc:29
double roll() const
Definition: lane.h:41
void unused(const Args &...)
Documents the argument(s) as unused, placating GCC&#39;s -Wunused-parameter warning.
Definition: unused.h:53
const Variable c_
Definition: symbolic_decompose_test.cc:37
Provides careful macros to selectively enable or disable the special member functions for copy-constr...