Apollo  6.0
Open source self driving car software
Public Member Functions | Static Public Member Functions | List of all members
apollo::common::math::CartesianFrenetConverter Class Reference

#include <cartesian_frenet_conversion.h>

Collaboration diagram for apollo::common::math::CartesianFrenetConverter:
Collaboration graph

Public Member Functions

 CartesianFrenetConverter ()=delete
 

Static Public Member Functions

static void cartesian_to_frenet (const double rs, const double rx, const double ry, const double rtheta, const double rkappa, const double rdkappa, const double x, const double y, const double v, const double a, const double theta, const double kappa, std::array< double, 3 > *const ptr_s_condition, std::array< double, 3 > *const ptr_d_condition)
 
static void cartesian_to_frenet (const double rs, const double rx, const double ry, const double rtheta, const double x, const double y, double *ptr_s, double *ptr_d)
 
static void frenet_to_cartesian (const double rs, const double rx, const double ry, const double rtheta, const double rkappa, const double rdkappa, const std::array< double, 3 > &s_condition, const std::array< double, 3 > &d_condition, double *const ptr_x, double *const ptr_y, double *const ptr_theta, double *const ptr_kappa, double *const ptr_v, double *const ptr_a)
 
static double CalculateTheta (const double rtheta, const double rkappa, const double l, const double dl)
 
static double CalculateKappa (const double rkappa, const double rdkappa, const double l, const double dl, const double ddl)
 
static Vec2d CalculateCartesianPoint (const double rtheta, const Vec2d &rpoint, const double l)
 
static double CalculateLateralDerivative (const double theta_ref, const double theta, const double l, const double kappa_ref)
 : given sl, theta, and road's theta, kappa, extract derivative l, second order derivative l: More...
 
static double CalculateSecondOrderLateralDerivative (const double theta_ref, const double theta, const double kappa_ref, const double kappa, const double dkappa_ref, const double l)
 

Constructor & Destructor Documentation

◆ CartesianFrenetConverter()

apollo::common::math::CartesianFrenetConverter::CartesianFrenetConverter ( )
delete

Member Function Documentation

◆ CalculateCartesianPoint()

static Vec2d apollo::common::math::CartesianFrenetConverter::CalculateCartesianPoint ( const double  rtheta,
const Vec2d rpoint,
const double  l 
)
static

◆ CalculateKappa()

static double apollo::common::math::CartesianFrenetConverter::CalculateKappa ( const double  rkappa,
const double  rdkappa,
const double  l,
const double  dl,
const double  ddl 
)
static

◆ CalculateLateralDerivative()

static double apollo::common::math::CartesianFrenetConverter::CalculateLateralDerivative ( const double  theta_ref,
const double  theta,
const double  l,
const double  kappa_ref 
)
static

: given sl, theta, and road's theta, kappa, extract derivative l, second order derivative l:

◆ CalculateSecondOrderLateralDerivative()

static double apollo::common::math::CartesianFrenetConverter::CalculateSecondOrderLateralDerivative ( const double  theta_ref,
const double  theta,
const double  kappa_ref,
const double  kappa,
const double  dkappa_ref,
const double  l 
)
static

◆ CalculateTheta()

static double apollo::common::math::CartesianFrenetConverter::CalculateTheta ( const double  rtheta,
const double  rkappa,
const double  l,
const double  dl 
)
static

◆ cartesian_to_frenet() [1/2]

static void apollo::common::math::CartesianFrenetConverter::cartesian_to_frenet ( const double  rs,
const double  rx,
const double  ry,
const double  rtheta,
const double  rkappa,
const double  rdkappa,
const double  x,
const double  y,
const double  v,
const double  a,
const double  theta,
const double  kappa,
std::array< double, 3 > *const  ptr_s_condition,
std::array< double, 3 > *const  ptr_d_condition 
)
static

Convert a vehicle state in Cartesian frame to Frenet frame. Decouple a 2d movement to two independent 1d movement w.r.t. reference line. The lateral movement is a function of longitudinal accumulated distance s to achieve better satisfaction of nonholonomic constraints.

◆ cartesian_to_frenet() [2/2]

static void apollo::common::math::CartesianFrenetConverter::cartesian_to_frenet ( const double  rs,
const double  rx,
const double  ry,
const double  rtheta,
const double  x,
const double  y,
double *  ptr_s,
double *  ptr_d 
)
static

◆ frenet_to_cartesian()

static void apollo::common::math::CartesianFrenetConverter::frenet_to_cartesian ( const double  rs,
const double  rx,
const double  ry,
const double  rtheta,
const double  rkappa,
const double  rdkappa,
const std::array< double, 3 > &  s_condition,
const std::array< double, 3 > &  d_condition,
double *const  ptr_x,
double *const  ptr_y,
double *const  ptr_theta,
double *const  ptr_kappa,
double *const  ptr_v,
double *const  ptr_a 
)
static

Convert a vehicle state in Frenet frame to Cartesian frame. Combine two independent 1d movement w.r.t. reference line to a 2d movement.


The documentation for this class was generated from the following file: