incl/reeds_shepp.h

   1 // Copyright (c) 2016, Guan-Horng Liu.
   2 // SPDX-FileCopyrightText: 2016 Guan-Horng Liu
   3 //
   4 // SPDX-License-Identifier: BSD-3-Clause
   5
   6 #ifndef SPACES_REEDS_SHEPP_STATE_SPACE_
   7 #define SPACES_REEDS_SHEPP_STATE_SPACE_
   8
   9 #include <boost/math/constants/constants.hpp>
  10 #include <cassert>
  11
  12 typedef int (*ReedsSheppPathSamplingCallback)(double q[3], void* user_data);
  13 typedef int (*ReedsSheppPathTypeCallback)(int t, void* user_data);
  14
  15 class ReedsSheppStateSpace
  16 {
  17 public:
  18
  19     /** \brief The Reeds-Shepp path segment types */
  20     enum ReedsSheppPathSegmentType { RS_NOP=0, RS_LEFT=1, RS_STRAIGHT=2, RS_RIGHT=3 };
  21
  22     /** \brief Reeds-Shepp path types */
  23     static const ReedsSheppPathSegmentType reedsSheppPathType[18][5];
  24
  25     /** \brief Complete description of a ReedsShepp path */
  26     class ReedsSheppPath
  27     {
  28     public:
  29         ReedsSheppPath(const ReedsSheppPathSegmentType* type=reedsSheppPathType[0],
  30             double t=std::numeric_limits<double>::max(), double u=0., double v=0.,
  31             double w=0., double x=0.);
  32
  33         double length() const { return totalLength_; }
  34
  35         /** Path segment types */
  36         const ReedsSheppPathSegmentType* type_;
  37         /** Path segment lengths */
  38         double length_[5];
  39         /** Total length */
  40         double totalLength_;
  41     };
  42
  43     ReedsSheppStateSpace(double turningRadius) : rho_(turningRadius) {}
  44
  45     double distance(double q0[3], double q1[3]);
  46
  47     void type(double q0[3], double q1[3], ReedsSheppPathTypeCallback cb, void* user_data);
  48
  49     void sample(double q0[3], double q1[3], double step_size, ReedsSheppPathSamplingCallback cb, void* user_data);
  50
  51     /** \brief Return the shortest Reeds-Shepp path from SE(2) state state1 to SE(2) state state2 */
  52     ReedsSheppPath reedsShepp(double q0[3], double q1[3]);
  53
  54 protected:
  55     void interpolate(double q0[3], ReedsSheppPath &path, double seg, double q[4]);
  56
  57     /** \brief Turning radius */
  58     double rho_;
  59 };
  60
  61 #endif