An implementation of continuous curvature (CC) steer for a Dubins car with either positive (p) or negative (n) max. curvature at the start and zero curvature at the goal configuration. It evaluates all Dubins families and returns the shortest path. More...
#include <ccpm0_dubins_state_space.hpp>
Classes | |
class | CCpm0_Dubins |
Public Member Functions | |
CC_Dubins_Path * | ccpm0_circles_dubins_path (const HC_CC_Circle &c1, const HC_CC_Circle &c2) const |
Returns a sequence of turns and straight lines connecting the two circles c1 and c2. More... | |
CC_Dubins_Path * | ccpm0_dubins (const State &state1, const State &state2) const |
Returns a sequence of turns and straight lines connecting a start and an end configuration. More... | |
CCpm0_Dubins_State_Space (double kappa, double sigma, double discretization=0.1, bool forwards=true) | |
Constructor. More... | |
std::vector< Control > | get_controls (const State &state1, const State &state2) const |
Returns controls of the shortest path from state1 to state2. More... | |
double | get_distance (const State &state1, const State &state2) const |
Returns shortest path length from state1 to state2. More... | |
~CCpm0_Dubins_State_Space () | |
Destructor. More... | |
![]() | |
std::vector< State > | get_path (const State &state1, const State &state2) const |
Returns path from state1 to state2. More... | |
std::vector< State_With_Covariance > | get_path_with_covariance (const State_With_Covariance &state1, const State &state2) const |
Returns path including covariances from state1 to state2. More... | |
HC_CC_State_Space (double kappa, double sigma, double discretization) | |
Constructor. More... | |
std::vector< State > | integrate (const State &state, const std::vector< Control > &controls) const |
Returns integrated states given a start state and controls. More... | |
State | integrate_ODE (const State &state, const Control &control, double integration_step) const |
Returns integrated state given a start state, a control, and an integration step. More... | |
std::vector< State_With_Covariance > | integrate_with_covariance (const State_With_Covariance &state, const std::vector< Control > &controls) const |
Returns integrated states including covariance given a start state and controls. More... | |
State | interpolate (const State &state, const std::vector< Control > &controls, double t) const |
Returns interpolated state at distance t in [0,1] (percentage of total path length) More... | |
void | set_filter_parameters (const Motion_Noise &motion_noise, const Measurement_Noise &measurement_noise, const Controller &controller) |
Sets the parameters required by the filter. More... | |
Private Attributes | |
std::unique_ptr< CCpm0_Dubins > | ccpm0_dubins_ |
Pimpl Idiom: unique pointer on class with families More... | |
bool | forwards_ |
Driving direction. More... | |
double | mu_ |
Angle between a configuration on the hc-/cc-circle and the tangent to the circle at that position. More... | |
double | radius_ |
Outer radius of a hc-/cc-circle. More... | |
HC_CC_Circle_Param | rs_circle_param_ |
Parameter of a rs-circle. More... | |
Additional Inherited Members | |
![]() | |
double | discretization_ |
Discretization of path. More... | |
EKF | ekf_ |
Extended Kalman Filter for uncertainty propagation. More... | |
HC_CC_Circle_Param | hc_cc_circle_param_ |
Parameters of a hc-/cc-circle. More... | |
double | kappa_ |
Curvature, sharpness of clothoid. More... | |
double | sigma_ |
An implementation of continuous curvature (CC) steer for a Dubins car with either positive (p) or negative (n) max. curvature at the start and zero curvature at the goal configuration. It evaluates all Dubins families and returns the shortest path.
Definition at line 69 of file ccpm0_dubins_state_space.hpp.
steering::CCpm0_Dubins_State_Space::CCpm0_Dubins_State_Space | ( | double | kappa, |
double | sigma, | ||
double | discretization = 0.1 , |
||
bool | forwards = true |
||
) |
Constructor.
Definition at line 336 of file ccpm0_dubins_state_space.cpp.
|
default |
Destructor.
CC_Dubins_Path * steering::CCpm0_Dubins_State_Space::ccpm0_circles_dubins_path | ( | const HC_CC_Circle & | c1, |
const HC_CC_Circle & | c2 | ||
) | const |
Returns a sequence of turns and straight lines connecting the two circles c1 and c2.
Definition at line 348 of file ccpm0_dubins_state_space.cpp.
CC_Dubins_Path * steering::CCpm0_Dubins_State_Space::ccpm0_dubins | ( | const State & | state1, |
const State & | state2 | ||
) | const |
Returns a sequence of turns and straight lines connecting a start and an end configuration.
Definition at line 427 of file ccpm0_dubins_state_space.cpp.
|
virtual |
Returns controls of the shortest path from state1 to state2.
Implements steering::HC_CC_State_Space.
Definition at line 515 of file ccpm0_dubins_state_space.cpp.
double steering::CCpm0_Dubins_State_Space::get_distance | ( | const State & | state1, |
const State & | state2 | ||
) | const |
Returns shortest path length from state1 to state2.
Definition at line 507 of file ccpm0_dubins_state_space.cpp.
|
private |
Pimpl Idiom: unique pointer on class with families
Definition at line 118 of file ccpm0_dubins_state_space.hpp.
|
private |
Driving direction.
Definition at line 115 of file ccpm0_dubins_state_space.hpp.
|
private |
Angle between a configuration on the hc-/cc-circle and the tangent to the circle at that position.
Definition at line 130 of file ccpm0_dubins_state_space.hpp.
|
private |
Outer radius of a hc-/cc-circle.
Definition at line 127 of file ccpm0_dubins_state_space.hpp.
|
private |
Parameter of a rs-circle.
Definition at line 124 of file ccpm0_dubins_state_space.hpp.