rrts/incl/dubins.h

   1 /*
   2  * Copyright (c) 2008-2018, Andrew Walker
   3  * SPDX-FileCopyrightText: 2008-2018 Andrew Walker
   4  *
   5  * SPDX-License-Identifier: MIT
   6  */
   7
   8 #ifndef DUBINS_H
   9 #define DUBINS_H
  10
  11 #ifdef __cplusplus
  12 extern "C" {
  13 #endif
  14
  15 typedef enum
  16 {
  17     LSL = 0,
  18     LSR = 1,
  19     RSL = 2,
  20     RSR = 3,
  21     RLR = 4,
  22     LRL = 5
  23 } DubinsPathType;
  24
  25 typedef struct
  26 {
  27     /* the initial configuration */
  28     double qi[3];
  29     /* the lengths of the three segments */
  30     double param[3];
  31     /* model forward velocity / model angular velocity */
  32     double rho;
  33     /* the path type described */
  34     DubinsPathType type;
  35 } DubinsPath;
  36
  37 #define EDUBOK        (0)   /* No error */
  38 #define EDUBCOCONFIGS (1)   /* Colocated configurations */
  39 #define EDUBPARAM     (2)   /* Path parameterisitation error */
  40 #define EDUBBADRHO    (3)   /* the rho value is invalid */
  41 #define EDUBNOPATH    (4)   /* no connection between configurations with this word */
  42
  43 /**
  44  * Callback function for path sampling
  45  *
  46  * @note the q parameter is a configuration
  47  * @note the t parameter is the distance along the path
  48  * @note the user_data parameter is forwarded from the caller
  49  * @note return non-zero to denote sampling should be stopped
  50  */
  51 typedef int (*DubinsPathSamplingCallback)(double q[3], double t, void* user_data);
  52
  53 /**
  54  * Generate a path from an initial configuration to
  55  * a target configuration, with a specified maximum turning
  56  * radii
  57  *
  58  * A configuration is (x, y, theta), where theta is in radians, with zero
  59  * along the line x = 0, and counter-clockwise is positive
  60  *
  61  * @param path  - the resultant path
  62  * @param q0    - a configuration specified as an array of x, y, theta
  63  * @param q1    - a configuration specified as an array of x, y, theta
  64  * @param rho   - turning radius of the vehicle (forward velocity divided by maximum angular velocity)
  65  * @return      - non-zero on error
  66  */
  67 int dubins_shortest_path(DubinsPath* path, double q0[3], double q1[3], double rho);
  68
  69 /**
  70  * Generate a path with a specified word from an initial configuration to
  71  * a target configuration, with a specified turning radius
  72  *
  73  * @param path     - the resultant path
  74  * @param q0       - a configuration specified as an array of x, y, theta
  75  * @param q1       - a configuration specified as an array of x, y, theta
  76  * @param rho      - turning radius of the vehicle (forward velocity divided by maximum angular velocity)
  77  * @param pathType - the specific path type to use
  78  * @return         - non-zero on error
  79  */
  80 int dubins_path(DubinsPath* path, double q0[3], double q1[3], double rho, DubinsPathType pathType);
  81
  82 /**
  83  * Calculate the length of an initialised path
  84  *
  85  * @param path - the path to find the length of
  86  */
  87 double dubins_path_length(DubinsPath* path);
  88
  89 /**
  90  * Return the length of a specific segment in an initialized path
  91  *
  92  * @param path - the path to find the length of
  93  * @param i    - the segment you to get the length of (0-2)
  94  */
  95 double dubins_segment_length(DubinsPath* path, int i);
  96
  97 /**
  98  * Return the normalized length of a specific segment in an initialized path
  99  *
 100  * @param path - the path to find the length of
 101  * @param i    - the segment you to get the length of (0-2)
 102  */
 103 double dubins_segment_length_normalized( DubinsPath* path, int i );
 104
 105 /**
 106  * Extract an integer that represents which path type was used
 107  *
 108  * @param path    - an initialised path
 109  * @return        - one of LSL, LSR, RSL, RSR, RLR or LRL
 110  */
 111 DubinsPathType dubins_path_type(DubinsPath* path);
 112
 113 /**
 114  * Calculate the configuration along the path, using the parameter t
 115  *
 116  * @param path - an initialised path
 117  * @param t    - a length measure, where 0 <= t < dubins_path_length(path)
 118  * @param q    - the configuration result
 119  * @returns    - non-zero if 't' is not in the correct range
 120  */
 121 int dubins_path_sample(DubinsPath* path, double t, double q[3]);
 122
 123 /**
 124  * Walk along the path at a fixed sampling interval, calling the
 125  * callback function at each interval
 126  *
 127  * The sampling process continues until the whole path is sampled, or the callback returns a non-zero value
 128  *
 129  * @param path      - the path to sample
 130  * @param stepSize  - the distance along the path for subsequent samples
 131  * @param cb        - the callback function to call for each sample
 132  * @param user_data - optional information to pass on to the callback
 133  *
 134  * @returns - zero on successful completion, or the result of the callback
 135  */
 136 int dubins_path_sample_many(DubinsPath* path,
 137                             double stepSize,
 138                             DubinsPathSamplingCallback cb,
 139                             void* user_data);
 140
 141 /**
 142  * Convenience function to identify the endpoint of a path
 143  *
 144  * @param path - an initialised path
 145  * @param q    - the configuration result
 146  */
 147 int dubins_path_endpoint(DubinsPath* path, double q[3]);
 148
 149 /**
 150  * Convenience function to extract a subset of a path
 151  *
 152  * @param path    - an initialised path
 153  * @param t       - a length measure, where 0 < t < dubins_path_length(path)
 154  * @param newpath - the resultant path
 155  */
 156 int dubins_extract_subpath(DubinsPath* path, double t, DubinsPath* newpath);
 157
 158 #ifdef __cplusplus
 159 }
 160 #endif
 161
 162 #endif /* DUBINS_H */