1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
| #ifndef DUBINS_H
#define DUBINS_H
// Path types
#define LSL (0)
#define LSR (1)
#define RSL (2)
#define RSR (3)
#define RLR (4)
#define LRL (5)
// Error codes
#define EDUBOK (0) // No error
#define EDUBCOCONFIGS (1) // Colocated configurations
#define EDUBPARAM (2) // Path parameterisitation error
#define EDUBBADRHO (3) // the rho value is invalid
#define EDUBNOPATH (4) // no connection between configurations with this word
// The various types of solvers for each of the path types
typedef int (*DubinsWord)(double, double, double, double* );
// A complete list of the possible solvers that could give optimal paths
extern DubinsWord dubins_words[];
typedef struct
{
double qi[3]; // the initial configuration
double param[3]; // the lengths of the three segments
double rho; // model forward velocity / model angular velocity
int type; // path type. one of LSL, LSR, ...
} DubinsPath;
/**
* Callback function for path sampling
*
* @note the q parameter is a configuration
* @note the t parameter is the distance along the path
* @note the user_data parameter is forwarded from the caller
* @note return non-zero to denote sampling should be stopped
*/
typedef int (*DubinsPathSamplingCallback)(double q[3], double t, void* user_data);
/**
* Generate a path from an initial configuration to
* a target configuration, with a specified maximum turning
* radii
*
* A configuration is (x, y, theta), where theta is in radians, with zero
* along the line x = 0, and counter-clockwise is positive
*
* @param q0 - a configuration specified as an array of x, y, theta
* @param q1 - a configuration specified as an array of x, y, theta
* @param rho - turning radius of the vehicle (forward velocity divided by maximum angular velocity)
* @param path - the resultant path
* @return - non-zero on error
*/
int dubins_init( double q0[3], double q1[3], double rho, DubinsPath* path);
/**
* Calculate the length of an initialised path
*
* @param path - the path to find the length of
*/
double dubins_path_length( DubinsPath* path );
/**
* Extract an integer that represents which path type was used
*
* @param path - an initialised path
* @return - one of LSL, LSR, RSL, RSR, RLR or LRL (ie/ 0-5 inclusive)
*/
int dubins_path_type( DubinsPath * path );
/**
* Calculate the configuration along the path, using the parameter t
*
* @param path - an initialised path
* @param t - a length measure, where 0 <= t < dubins_path_length(path)
* @param q - the configuration result
* @returns - non-zero if 't' is not in the correct range
*/
int dubins_path_sample( DubinsPath* path, double t, double q[3]);
/**
* Walk along the path at a fixed sampling interval, calling the
* callback function at each interval
*
* @param path - the path to sample
* @param cb - the callback function to call for each sample
* @param user_data - optional information to pass on to the callback
* @param stepSize - the distance along the path for subsequent samples
*/
int dubins_path_sample_many( DubinsPath* path, DubinsPathSamplingCallback cb, double stepSize, void* user_data );
/**
* Convenience function to identify the endpoint of a path
*
* @param path - an initialised path
* @param q - the configuration result
*/
int dubins_path_endpoint( DubinsPath* path, double q[3] );
/**
* Convenience function to extract a subset of a path
*
* @param path - an initialised path
* @param t - a length measure, where 0 < t < dubins_path_length(path)
* @param newpath - the resultant path
*/
int dubins_extract_subpath( DubinsPath* path, double t, DubinsPath* newpath );
// Only exposed for testing purposes
int dubins_LSL( double alpha, double beta, double d, double* outputs );
int dubins_RSR( double alpha, double beta, double d, double* outputs );
int dubins_LSR( double alpha, double beta, double d, double* outputs );
int dubins_RSL( double alpha, double beta, double d, double* outputs );
int dubins_LRL( double alpha, double beta, double d, double* outputs );
int dubins_RLR( double alpha, double beta, double d, double* outputs );
#endif // DUBINS_H |
