Drake

Defines an interface for a path in a Segment object surface. More...
#include <drake/automotive/maliput/multilane/road_curve.h>
Public Member Functions  
virtual  ~RoadCurve ()=default 
const CubicPolynomial &  elevation () const 
const CubicPolynomial &  superelevation () const 
const double &  linear_tolerance () const 
const double &  scale_length () const 
const ComputationPolicy &  computation_policy () const 
std::function< double(double)>  OptimizeCalcPFromS (double r) const 
Optimizes the computation of the parametric position p along the reference curve from the longitudinal position (in pathlength) s along a parallel curve laterally offset by r from the reference curve. More...  
std::function< double(double)>  OptimizeCalcSFromP (double r) const 
Optimizes the computation of path length integral in the interval of the parameter [0; p] and along a parallel curve laterally offset by r the planar reference curve. More...  
virtual Vector2< double >  xy_of_p (double p) const =0 
Computes the reference curve. More...  
virtual Vector2< double >  xy_dot_of_p (double p) const =0 
Computes the first derivative of the reference curve. More...  
virtual double  heading_of_p (double p) const =0 
Computes the heading of the reference curve. More...  
virtual double  heading_dot_of_p (double p) const =0 
Computes the first derivative heading of the reference curve. More...  
virtual double  l_max () const =0 
Computes the path length integral of the reference curve for the whole [0; 1] interval of p, formally l_max = ∫₀¹ G'(p) dp where G' = dG/dp. More...  
virtual Vector3< double >  ToCurveFrame (const Vector3< double > &geo_coordinate, double r_min, double r_max, const api::HBounds &height_bounds) const =0 
Converts a geo_coordinate in the world frame to the composed curve frame, i.e., the superposition of the reference curve, elevation and superelevation polynomials. More...  
virtual bool  IsValid (double r_min, double r_max, const api::HBounds &height_bounds) const =0 
Checks that there are no selfintersections (singularities) in the volume created by applying the constant r_min , r_max and height_bounds to the RoadCurve. More...  
Vector3< double >  W_of_prh (double p, double r, double h) const 
Returns W, the world function evaluated at p , r , h . More...  
Vector3< double >  W_prime_of_prh (double p, double r, double h, const Rot3 &Rabg, double g_prime) const 
Returns W' = ∂W/∂p, the partial differential of W with respect to p, evaluated at p , r , h . More...  
Rot3  Rabg_of_p (double p) const 
Returns the rotation R_αβγ, evaluated at p along the reference curve. More...  
Rot3  Orientation (double p, double r, double h) const 
Returns the rotation R_αβγ, evaluated at p , r and h . More...  
Vector3< double >  s_hat_of_prh (double p, double r, double h, const Rot3 &Rabg, double g_prime) const 
Returns the saxis unitvector, expressed in the world frame, of the (s,r,h) Lane frame (with respect to the world frame). More...  
Vector3< double >  r_hat_of_Rabg (const Rot3 &Rabg) const 
Returns the raxis unitvector, expressed in the world frame, of the (s,r,h) Lane frame (with respect to the world frame). More...  
double  CalcGPrimeAsUsedForCalcSFromP (double p) const 
Computes the most appropriate value for the elevation derivative g' at p , that accounts for the limitations of the arc length parameterization being used. More...  
Does not allow copy, move, or assignment  
RoadCurve (const RoadCurve &)=delete  
RoadCurve &  operator= (const RoadCurve &)=delete 
RoadCurve (RoadCurve &&)=delete  
RoadCurve &  operator= (RoadCurve &&)=delete 
Protected Member Functions  
RoadCurve (double linear_tolerance, double scale_length, const CubicPolynomial &elevation, const CubicPolynomial &superelevation, ComputationPolicy computation_policy)  
Constructs a road curve given elevation and superelevation curves. More...  
Defines an interface for a path in a Segment object surface.
The path is defined by an elevation and superelevation CubicPolynomial objects and a reference curve. This reference curve is a C1 function in the z=0 plane. Its domain is constrained in [0;1] interval and it should map a ℝ² curve. As per notation, p is the parameter of the reference curve, not necessarily arc length s, and function interpolations and function derivatives as well as headings and heading derivatives are expressed in world coordinates, which is the same frame as api::GeoPosition. By implementing this interface the road curve is defined and complete.
The geometry here revolves around an abstract "world function"
W: (p,r,h) –> (x,y,z)
which maps a Lane
frame position to its corresponding representation in world coordinates (with the caveat that instead of the lane's native longitudinal coordinate 's', the reference curve parameter 'p' is used).
W is derived from the three functions which define the lane:
G: p –> (x,y) = the reference curve, a.k.a. xy_of_p() Z: p –> z / l_max = the elevation function, a.k.a. elevation_ Θ: p –> θ / l_max = the superelevation function, a.k.a. superelevation_
as:
(x,y,z) = W(p,r,h) = (G(p), Z(p)) + R_αβγ*(0,r,h)
where R_αβγ is the roll/pitch/yaw rotation given by angles:
α = Θ(p) β = atan(dZ/dp) at p γ = atan2(dG_y/dp, dG_x/dp) at p
(R_αβγ is essentially the orientation of the (s,r,h) Lane
frame at a location (s,0,0) on the referenceline of the lane. However, it is not necessarily the correct orientation at r != 0 or h != 0.)
The W(p,r,h) "world function" is defined by the RoadCurve referenced by a Lane's Segment. A Lane is also defined by a r0 lateral offset with respect to the reference curve of the RoadCurve. Thus, a mapping from the local (s,r,h) laneframe of the Lane becomes:
(x,y,z) = L(s,r,h) = W(P(s, r0), r + r0, h),
where P:(s, r0) –> (p) is a (potentially nonlinear) function dependent on the RoadCurve's referencecurve, elevation, and superelevation functions.
TODO(maddogtri) Add support for Lanes with both nonzero r0 and superelevation polynomial.

virtualdefault 

protected 
Constructs a road curve given elevation and superelevation curves.
linear_tolerance  The linear tolerance, in meters, for all computations. It is understood in the the absolute error sense i.e. linear error must lie in the 0 ± linear tolerance interval, for scale_length long features at most. 
scale_length  The minimum spatial period of variation in the curve, in meters. This imposes an upper limit to the spatial frequency (i.e. the Nyquist limit), which indicates the maximum level of detail expressed by the curve. 
elevation  CubicPolynomial object that represents the elevation function (see below for more details). 
superelevation  CubicPolynomial object that represents the superelevation function (see below for more details). 
computation_policy  Policy to guide computations in terms of speed and accuracy. Actual behavior may vary across implementations. 
scale_length
is a positive number. linear_tolerance
is a positive number. std::runtime_error  if any of the preconditions is not met. 
elevation
and superelevation
are cubicpolynomial functions which define the elevation and superelevation as a function of position along the planar reference curve. elevation
specifies the zcomponent of the surface at (r,h) = (0,0). superelevation
specifies the angle of the raxis with respect to the horizon, i.e., how the road twists. Thus, nonzero superelevation
contributes to the zcomponent at r != 0.
These two functions (elevation
and superelevation
) must be isotropically scaled to operate over the domain p in [0, 1], where p is linear in the pathlength of the planar reference curve, p = 0 corresponds to the start and p = 1 to the end. l_max() is the length of the reference curve. In other words...
Given:
then:
elevation
is E_scaled = (1 / l_max) * E_true(l_max * p);superelevation
is S_scaled = (1 / l_max) * S_true(l_max * p). Computes the most appropriate value for the elevation derivative g' at p
, that accounts for the limitations of the arc length parameterization being used.
p  The reference curve parameter. 
p
) value.

inline 

inline 
Computes the first derivative heading of the reference curve.
p  The reference curve parameter. 
p
. Implemented in ArcRoadCurve, and LineRoadCurve.
Computes the heading of the reference curve.
p  The reference curve parameter. 
p
, i.e., the angle of the tangent vector (with respect to xaxis) in the increasingp direction. Implemented in ArcRoadCurve, and LineRoadCurve.

pure virtual 
Checks that there are no selfintersections (singularities) in the volume created by applying the constant r_min
, r_max
and height_bounds
to the RoadCurve.
r_min  Minimum lateral distance from the composed curve to evaluate the validity of the geometry. 
r_max  Maximum lateral distance from the composed curve to evaluate the validity of the geometry. 
height_bounds  An api::HBounds object that represents the elevation bounds of the surface mapping. 
Implemented in ArcRoadCurve, and LineRoadCurve.

pure virtual 
Computes the path length integral of the reference curve for the whole [0; 1] interval of p, formally l_max = ∫₀¹ G'(p) dp where G' = dG/dp.
Implemented in ArcRoadCurve, and LineRoadCurve.

inline 
Optimizes the computation of the parametric position p along the reference curve from the longitudinal position (in pathlength) s
along a parallel curve laterally offset by r
from the reference curve.
s
at the specified parallel curve to parametric position p along the reference curve, defined for all s
values between 0 and the total path length of the parallel curve (and throwing for any given value outside this interval). std::runtime_error  When r makes the radius of curvature be a non positive number. 
Optimizes the computation of path length integral in the interval of the parameter [0; p] and along a parallel curve laterally offset by r
the planar reference curve.
std::runtime_error  When r makes the radius of curvature be a non positive number. 
Returns the rotation R_αβγ, evaluated at p
, r
and h
.
Returns the raxis unitvector, expressed in the world frame, of the (s,r,h) Lane
frame (with respect to the world frame).
(Rabg
must be the result of Rabg_of_p(p) — passed in here to avoid recomputing it.)
Returns the rotation R_αβγ, evaluated at p
along the reference curve.
Vector3< double > s_hat_of_prh  (  double  p, 
double  r,  
double  h,  
const Rot3 &  Rabg,  
double  g_prime  
)  const 
Returns the saxis unitvector, expressed in the world frame, of the (s,r,h) Lane
frame (with respect to the world frame).
(Rabg
must be the result of Rabg_of_p(p) — passed in here to avoid recomputing it.) (g_prime
must be the result of elevation().f_dot_p(p) — passed in here to avoid recomputing it.)

inline 

inline 

pure virtual 
Converts a geo_coordinate
in the world frame to the composed curve frame, i.e., the superposition of the reference curve, elevation and superelevation polynomials.
The resulting coordinates [p, r, h] are saturated in the following domain ranges.
r_min
, r_max
]height_bounds
] geo_coordinate  A 3D vector in the world frame to be converted to the composed curve frame. 
r_min  Minimum lateral distance from the composed curve to saturate, if it is necessary, the result in the given direction. 
r_max  Maximum lateral distance from the composed curve to evaluate, if it is necessary, the result in the given direction 
height_bounds  An api::HBounds object that represents the elevation bounds of the surface mapping. 
geo_cooridnate
. Implemented in ArcRoadCurve, and LineRoadCurve.
Returns W, the world function evaluated at p
, r
, h
.
Vector3< double > W_prime_of_prh  (  double  p, 
double  r,  
double  h,  
const Rot3 &  Rabg,  
double  g_prime  
)  const 
Returns W' = ∂W/∂p, the partial differential of W with respect to p, evaluated at p
, r
, h
.
(Rabg
must be the result of Rabg_of_p(p) — passed in here to avoid recomputing it.) (g_prime
must be the result of elevation().f_dot_p(p) — passed in here to avoid recomputing it.)
Computes the first derivative of the reference curve.
p  The reference curve parameter. 
p
, i.e., F'(p0) = (dx/dp, dy/dp) at p0. Implemented in ArcRoadCurve, and LineRoadCurve.
Computes the reference curve.
p  The reference curve parameter. 
Implemented in ArcRoadCurve, and LineRoadCurve.