Drake

Defines an interface for a path in a Segment object surface. More...

#include <drake/automotive/maliput/multilane/road_curve.h>

## Public Member Functions

const CubicPolynomialelevation () const

const CubicPolynomialsuperelevation () const

const doublelinear_tolerance () const

const doublescale_length () const

const ComputationPolicycomputation_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 path-length) 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< doublexy_of_p (double p) const =0
Computes the reference curve. More...

virtual Vector2< doublexy_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< doubleToCurveFrame (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 self-intersections (singularities) in the volume created by applying the constant r_min, r_max and height_bounds to the RoadCurve. More...

Vector3< doubleW_of_prh (double p, double r, double h) const
Returns W, the world function evaluated at p, r, h. More...

Vector3< doubleW_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< doubles_hat_of_prh (double p, double r, double h, const Rot3 &Rabg, double g_prime) const
Returns the s-axis unit-vector, expressed in the world frame, of the (s,r,h) Lane-frame (with respect to the world frame). More...

Vector3< doubler_hat_of_Rabg (const Rot3 &Rabg) const
Returns the r-axis unit-vector, 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

## 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...

## Detailed Description

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 reference-line 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) lane-frame 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 non-linear) function dependent on the RoadCurve's reference-curve, elevation, and superelevation functions.

## Constructor & Destructor Documentation

delete

delete

virtualdefault

 RoadCurve ( double linear_tolerance, double scale_length, const CubicPolynomial & elevation, const CubicPolynomial & superelevation, ComputationPolicy computation_policy )
protected

Constructs a road curve given elevation and superelevation curves.

Parameters
 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.
Precondition
The given scale_length is a positive number.
The given linear_tolerance is a positive number.
Exceptions
 std::runtime_error if any of the preconditions is not met.

elevation and superelevation are cubic-polynomial functions which define the elevation and superelevation as a function of position along the planar reference curve. elevation specifies the z-component of the surface at (r,h) = (0,0). superelevation specifies the angle of the r-axis with respect to the horizon, i.e., how the road twists. Thus, non-zero superelevation contributes to the z-component 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 path-length 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:

• a reference curve R(p) parameterized by p in domain [0, 1], which has a path-length ℓ(p) in range [0, l_max], linearly related to p, where l_max is the total path-length of R (in real-world units);
• the true elevation function E_true(ℓ), parameterized by the path-length ℓ of R;
• the true superelevation function S_true(ℓ), parameterized by the path-length ℓ of R;

then:

• p = ℓ / l_max;
• elevation is E_scaled = (1 / l_max) * E_true(l_max * p);
• superelevation is S_scaled = (1 / l_max) * S_true(l_max * p).

## ◆ CalcGPrimeAsUsedForCalcSFromP()

 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.

Parameters
 p The reference curve parameter.
Returns
The elevation derivative g'(p) value.
W_prime_of_prh()

## ◆ computation_policy()

 const ComputationPolicy& computation_policy ( ) const
inline

## ◆ elevation()

 const CubicPolynomial& elevation ( ) const
inline

 virtual double heading_dot_of_p ( double p ) const
pure virtual

Computes the first derivative heading of the reference curve.

Parameters
 p The reference curve parameter.
Returns
The derivative of the heading with respect to p, i.e., d_heading/dp evaluated at p.

 virtual double heading_of_p ( double p ) const
pure virtual

Computes the heading of the reference curve.

Parameters
 p The reference curve parameter.
Returns
The heading of the curve at p, i.e., the angle of the tangent vector (with respect to x-axis) in the increasing-p direction.

## ◆ IsValid()

 virtual bool IsValid ( double r_min, double r_max, const api::HBounds & height_bounds ) const
pure virtual

Checks that there are no self-intersections (singularities) in the volume created by applying the constant r_min, r_max and height_bounds to the RoadCurve.

Parameters
 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.
Returns
True when there are no self-intersections.

## ◆ l_max()

 virtual double l_max ( ) const
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.

Returns
The total path length of the reference curve.

## ◆ linear_tolerance()

 const double& linear_tolerance ( ) const
inline

delete

delete

## ◆ OptimizeCalcPFromS()

 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 path-length) s along a parallel curve laterally offset by r from the reference curve.

Returns
A function that relates longitudinal position 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).
Exceptions
 std::runtime_error When r makes the radius of curvature be a non positive number.

## ◆ OptimizeCalcSFromP()

 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.

Returns
A function that relates parametric position p along the reference curve to longitudinal position s at the specified parallel curve, defined for all p between 0 and 1 (and throwing for any given value outside this interval).
Exceptions
 std::runtime_error When r makes the radius of curvature be a non positive number.

## ◆ Orientation()

 Rot3 Orientation ( double p, double r, double h ) const

Returns the rotation R_αβγ, evaluated at p, r and h.

## ◆ r_hat_of_Rabg()

 Vector3< double > r_hat_of_Rabg ( const Rot3 & Rabg ) const

Returns the r-axis unit-vector, 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.)

## ◆ Rabg_of_p()

 Rot3 Rabg_of_p ( double p ) const

Returns the rotation R_αβγ, evaluated at p along the reference curve.

## ◆ s_hat_of_prh()

 Vector3< double > s_hat_of_prh ( double p, double r, double h, const Rot3 & Rabg, double g_prime ) const

Returns the s-axis unit-vector, 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.)

## ◆ scale_length()

 const double& scale_length ( ) const
inline

## ◆ superelevation()

 const CubicPolynomial& superelevation ( ) const
inline

## ◆ ToCurveFrame()

 virtual Vector3 ToCurveFrame ( const Vector3< double > & geo_coordinate, double r_min, double r_max, const api::HBounds & height_bounds ) const
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.

• p: [0, 1]
• r: [r_min, r_max]
• h: [height_bounds]
Parameters
 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.
Returns
A 3D vector [p, r, h], that represent the domain coordinates of the world function, that gives as world function output geo_cooridnate.

## ◆ W_of_prh()

 Vector3< double > W_of_prh ( double p, double r, double h ) const

Returns W, the world function evaluated at p, r, h.

## ◆ W_prime_of_prh()

 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.)

## ◆ xy_dot_of_p()

 virtual Vector2 xy_dot_of_p ( double p ) const
pure virtual

Computes the first derivative of the reference curve.

Parameters
 p The reference curve parameter.
Returns
The derivative of the curve with respect to p, at p, i.e., F'(p0) = (dx/dp, dy/dp) at p0.

## ◆ xy_of_p()

 virtual Vector2 xy_of_p ( double p ) const
pure virtual

Computes the reference curve.

Parameters
 p The reference curve parameter.
Returns
The reference curve itself, F(p).