incl/dubins.h

   1 /*
   2  * Copyright (c) 2008-2018, Andrew Walker
   3  *
   4  * Permission is hereby granted, free of charge, to any person obtaining a copy
   5  * of this software and associated documentation files (the "Software"), to deal
   6  * in the Software without restriction, including without limitation the rights
   7  * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
   8  * copies of the Software, and to permit persons to whom the Software is
   9  * furnished to do so, subject to the following conditions:
  10  *
  11  * The above copyright notice and this permission notice shall be included in
  12  * all copies or substantial portions of the Software.
  13  *
  14  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
  15  * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
  16  * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
  17  * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
  18  * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
  19  * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
  20  * THE SOFTWARE.
  21  */
  22 #ifndef DUBINS_H
  23 #define DUBINS_H
  24
  25 typedef enum
  26 {
  27     LSL = 0,
  28     LSR = 1,
  29     RSL = 2,
  30     RSR = 3,
  31     RLR = 4,
  32     LRL = 5
  33 } DubinsPathType;
  34
  35 typedef struct
  36 {
  37     /* the initial configuration */
  38     double qi[3];
  39     /* the lengths of the three segments */
  40     double param[3];
  41     /* model forward velocity / model angular velocity */
  42     double rho;
  43     /* the path type described */
  44     DubinsPathType type;
  45 } DubinsPath;
  46
  47 #define EDUBOK        (0)   /* No error */
  48 #define EDUBCOCONFIGS (1)   /* Colocated configurations */
  49 #define EDUBPARAM     (2)   /* Path parameterisitation error */
  50 #define EDUBBADRHO    (3)   /* the rho value is invalid */
  51 #define EDUBNOPATH    (4)   /* no connection between configurations with this word */
  52
  53 /**
  54  * Callback function for path sampling
  55  *
  56  * @note the q parameter is a configuration
  57  * @note the t parameter is the distance along the path
  58  * @note the user_data parameter is forwarded from the caller
  59  * @note return non-zero to denote sampling should be stopped
  60  */
  61 typedef int (*DubinsPathSamplingCallback)(double q[3], double t, void* user_data);
  62
  63 /**
  64  * Generate a path from an initial configuration to
  65  * a target configuration, with a specified maximum turning
  66  * radii
  67  *
  68  * A configuration is (x, y, theta), where theta is in radians, with zero
  69  * along the line x = 0, and counter-clockwise is positive
  70  *
  71  * @param path  - the resultant path
  72  * @param q0    - a configuration specified as an array of x, y, theta
  73  * @param q1    - a configuration specified as an array of x, y, theta
  74  * @param rho   - turning radius of the vehicle (forward velocity divided by maximum angular velocity)
  75  * @return      - non-zero on error
  76  */
  77 int dubins_shortest_path(DubinsPath* path, double q0[3], double q1[3], double rho);
  78
  79 /**
  80  * Generate a path with a specified word from an initial configuration to
  81  * a target configuration, with a specified turning radius
  82  *
  83  * @param path     - the resultant path
  84  * @param q0       - a configuration specified as an array of x, y, theta
  85  * @param q1       - a configuration specified as an array of x, y, theta
  86  * @param rho      - turning radius of the vehicle (forward velocity divided by maximum angular velocity)
  87  * @param pathType - the specific path type to use
  88  * @return         - non-zero on error
  89  */
  90 int dubins_path(DubinsPath* path, double q0[3], double q1[3], double rho, DubinsPathType pathType);
  91
  92 /**
  93  * Calculate the length of an initialised path
  94  *
  95  * @param path - the path to find the length of
  96  */
  97 double dubins_path_length(DubinsPath* path);
  98
  99 /**
 100  * Return the length of a specific segment in an initialized path
 101  *
 102  * @param path - the path to find the length of
 103  * @param i    - the segment you to get the length of (0-2)
 104  */
 105 double dubins_segment_length(DubinsPath* path, int i);
 106
 107 /**
 108  * Return the normalized length of a specific segment in an initialized path
 109  *
 110  * @param path - the path to find the length of
 111  * @param i    - the segment you to get the length of (0-2)
 112  */
 113 double dubins_segment_length_normalized( DubinsPath* path, int i );
 114
 115 /**
 116  * Extract an integer that represents which path type was used
 117  *
 118  * @param path    - an initialised path
 119  * @return        - one of LSL, LSR, RSL, RSR, RLR or LRL
 120  */
 121 DubinsPathType dubins_path_type(DubinsPath* path);
 122
 123 /**
 124  * Calculate the configuration along the path, using the parameter t
 125  *
 126  * @param path - an initialised path
 127  * @param t    - a length measure, where 0 <= t < dubins_path_length(path)
 128  * @param q    - the configuration result
 129  * @returns    - non-zero if 't' is not in the correct range
 130  */
 131 int dubins_path_sample(DubinsPath* path, double t, double q[3]);
 132
 133 /**
 134  * Walk along the path at a fixed sampling interval, calling the
 135  * callback function at each interval
 136  *
 137  * The sampling process continues until the whole path is sampled, or the callback returns a non-zero value
 138  *
 139  * @param path      - the path to sample
 140  * @param stepSize  - the distance along the path for subsequent samples
 141  * @param cb        - the callback function to call for each sample
 142  * @param user_data - optional information to pass on to the callback
 143  *
 144  * @returns - zero on successful completion, or the result of the callback
 145  */
 146 int dubins_path_sample_many(DubinsPath* path,
 147                             double stepSize,
 148                             DubinsPathSamplingCallback cb,
 149                             void* user_data);
 150
 151 /**
 152  * Convenience function to identify the endpoint of a path
 153  *
 154  * @param path - an initialised path
 155  * @param q    - the configuration result
 156  */
 157 int dubins_path_endpoint(DubinsPath* path, double q[3]);
 158
 159 /**
 160  * Convenience function to extract a subset of a path
 161  *
 162  * @param path    - an initialised path
 163  * @param t       - a length measure, where 0 < t < dubins_path_length(path)
 164  * @param newpath - the resultant path
 165  */
 166 int dubins_extract_subpath(DubinsPath* path, double t, DubinsPath* newpath);
 167
 168
 169 #endif /* DUBINS_H */
 170