Class Dubins_State_Space

Nested Relationships

Nested Types

Class Documentation

class Dubins_State_Space

An SE(2) state space where distance is measured by the length of Dubins curves. Note that this Dubins distance is not

a proper distance metric, so nearest neighbor methods that rely on distance() being a metric (such as ompl::NearestNeighborsGNAT) will not always return the true nearest neighbors or get stuck in an infinite loop. The notation and solutions in the code are taken from:

A.M. Shkel and V. Lumelsky, “Classification of the Dubins set,” Robotics and Autonomous Systems, 34(4):179-202, 2001. DOI:

10.1016/S0921-8890(00)00127-5 The classification scheme described there is not actually used, since it only applies to “long” paths.

Public Types

enum Dubins_Path_Segment_Type

The Dubins path segment type.

Values:

enumerator DUBINS_LEFT
enumerator DUBINS_STRAIGHT
enumerator DUBINS_RIGHT

Public Functions

inline Dubins_State_Space(double kappa, double discretization = 0.1, bool forwards = true)

Constructor of the Dubins_State_Space.

void set_filter_parameters(const Motion_Noise &motion_noise, const Measurement_Noise &measurement_noise, const Controller &controller)

Sets the parameters required by the filter.

Dubins_Path dubins(const State &state1, const State &state2) const

Returns type and length of segments of path from state1 to state2 with curvature = 1.0.

double get_distance(const State &state1, const State &state2) const

Returns shortest path length from state1 to state2 with curvature = kappa_.

std::vector<Control> get_controls(const State &state1, const State &state2) const

Returns controls of the shortest path from state1 to state2 with curvature = kappa_.

std::vector<State> get_path(const State &state1, const State &state2) const

Returns shortest path from state1 to state2 with curvature = kappa_.

std::vector<State_With_Covariance> get_path_with_covariance(const State_With_Covariance &state1, const State &state2) const

Returns shortest path including covariances from state1 to state2 with curvature = kappa_.

std::vector<State> integrate(const State &state, const std::vector<Control> &controls) const

Returns integrated states given a start state and controls with curvature = kappa_.

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 with curvature = kappa_.

State interpolate(const State &state, const std::vector<Control> &controls, double t) const

Returns interpolated state at distance t in [0,1] (percent of total path length with curvature = kappa_)

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

Public Static Attributes

static const Dubins_Path_Segment_Type dubins_path_type[6][3]

Dubins path types.

class Dubins_Path

Complete description of a Dubins path.

Public Functions

inline Dubins_Path(const Dubins_Path_Segment_Type *type = dubins_path_type[0], double t = 0., double p = std::numeric_limits<double>::max(), double q = 0.)

Constructor of the Dubins_Path.

inline double length() const