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