incl/reeds_shepp.h

   1 /*********************************************************************
   2 *
   3 * Software License Agreement (BSD License)
   4 *
   5 *  Copyright (c) 2016, Guan-Horng Liu.
   6 *  All rights reserved.
   7 *
   8 *  Redistribution and use in source and binary forms, with or without
   9 *  modification, are permitted provided that the following conditions
  10 *  are met:
  11 *
  12 *   * Redistributions of source code must retain the above copyright
  13 *     notice, this list of conditions and the following disclaimer.
  14 *   * Redistributions in binary form must reproduce the above
  15 *     copyright notice, this list of conditions and the following
  16 *     disclaimer in the documentation and/or other materials provided
  17 *     with the distribution.
  18 *   * Neither the name of the the copyright holder nor the names of its
  19 *     contributors may be used to endorse or promote products derived
  20 *     from this software without specific prior written permission.
  21 *
  22 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
  23 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
  24 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
  25 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
  26 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
  27 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
  28 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
  29 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
  30 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
  31 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
  32 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  33 *  POSSIBILITY OF SUCH DAMAGE.
  34 *
  35 * Author:  Guan-Horng Liu
  36 *********************************************************************/
  37
  38 #ifndef SPACES_REEDS_SHEPP_STATE_SPACE_
  39 #define SPACES_REEDS_SHEPP_STATE_SPACE_
  40
  41 #include <boost/math/constants/constants.hpp>
  42 #include <cassert>
  43
  44 typedef int (*ReedsSheppPathSamplingCallback)(double q[3], void* user_data);
  45 typedef int (*ReedsSheppPathTypeCallback)(int t, void* user_data);
  46
  47 class ReedsSheppStateSpace
  48 {
  49 public:
  50
  51     /** \brief The Reeds-Shepp path segment types */
  52     enum ReedsSheppPathSegmentType { RS_NOP=0, RS_LEFT=1, RS_STRAIGHT=2, RS_RIGHT=3 };
  53
  54     /** \brief Reeds-Shepp path types */
  55     static const ReedsSheppPathSegmentType reedsSheppPathType[18][5];
  56
  57     /** \brief Complete description of a ReedsShepp path */
  58     class ReedsSheppPath
  59     {
  60     public:
  61         ReedsSheppPath(const ReedsSheppPathSegmentType* type=reedsSheppPathType[0],
  62             double t=std::numeric_limits<double>::max(), double u=0., double v=0.,
  63             double w=0., double x=0.);
  64
  65         double length() const { return totalLength_; }
  66
  67         /** Path segment types */
  68         const ReedsSheppPathSegmentType* type_;
  69         /** Path segment lengths */
  70         double length_[5];
  71         /** Total length */
  72         double totalLength_;
  73     };
  74
  75     ReedsSheppStateSpace(double turningRadius) : rho_(turningRadius) {}
  76
  77     double distance(double q0[3], double q1[3]);
  78
  79     void type(double q0[3], double q1[3], ReedsSheppPathTypeCallback cb, void* user_data);
  80
  81     void sample(double q0[3], double q1[3], double step_size, ReedsSheppPathSamplingCallback cb, void* user_data);
  82
  83     /** \brief Return the shortest Reeds-Shepp path from SE(2) state state1 to SE(2) state state2 */
  84     ReedsSheppPath reedsShepp(double q0[3], double q1[3]);
  85
  86 protected:
  87     void interpolate(double q0[3], ReedsSheppPath &path, double seg, double q[4]);
  88
  89     /** \brief Turning radius */
  90     double rho_;
  91 };
  92
  93 #endif