An implementation of continuous curvature (CC) steer for a Dubins car with zero curvature at the start and goal configuration as described in: T. Fraichard and A. Scheuer, "From Reeds and Shepp's to continuous-curvature paths," IEEE Transactions on Robotics (Volume 20, Issue: 6, Dec. 2004). It evaluates all Dubins families and returns the shortest path. More...
#include <cc00_dubins_state_space.hpp>
Classes | |
class | CC00_Dubins |
Public Member Functions | |
CC_Dubins_Path * | cc00_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 * | cc00_dubins (const State &state1, const State &state2) const |
Returns a sequence of turns and straight lines connecting a start and an end configuration. More... | |
CC00_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... | |
~CC00_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< CC00_Dubins > | cc00_dubins_ |
Pimpl Idiom: unique pointer on class with families More... | |
bool | forwards_ |
Driving direction. 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 zero curvature at the start and goal configuration as described in: T. Fraichard and A. Scheuer, "From Reeds and Shepp's to continuous-curvature paths," IEEE Transactions on Robotics (Volume 20, Issue: 6, Dec. 2004). It evaluates all Dubins families and returns the shortest path.
Definition at line 70 of file cc00_dubins_state_space.hpp.
steering::CC00_Dubins_State_Space::CC00_Dubins_State_Space | ( | double | kappa, |
double | sigma, | ||
double | discretization = 0.1 , |
||
bool | forwards = true |
||
) |
Constructor.
Definition at line 314 of file cc00_dubins_state_space.cpp.
|
default |
Destructor.
CC_Dubins_Path * steering::CC00_Dubins_State_Space::cc00_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 323 of file cc00_dubins_state_space.cpp.
CC_Dubins_Path * steering::CC00_Dubins_State_Space::cc00_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 414 of file cc00_dubins_state_space.cpp.
|
virtual |
Returns controls of the shortest path from state1 to state2.
Implements steering::HC_CC_State_Space.
Definition at line 496 of file cc00_dubins_state_space.cpp.
double steering::CC00_Dubins_State_Space::get_distance | ( | const State & | state1, |
const State & | state2 | ||
) | const |
Returns shortest path length from state1 to state2.
Definition at line 488 of file cc00_dubins_state_space.cpp.
|
private |
Pimpl Idiom: unique pointer on class with families
Definition at line 119 of file cc00_dubins_state_space.hpp.
|
private |
Driving direction.
Definition at line 116 of file cc00_dubins_state_space.hpp.