Add Reeds and Shepp steering procedure

diff --git a/CHANGELOG.md b/CHANGELOG.md
index 5edb862c8b2de70043a488bbdd9610db01a9e7c0..854f4afe62f204bb8143b3d896abe5d75ef31729 100644 (file)
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -25,6 +25,7 @@ The format is based on [Keep a Changelog][] and this project adheres to
 - Near vertices procedure.
 - RRT\* planner based on [Karaman2011] paper.
 - Alternative steering procedure.
+- Reeds and Shepp cost, steering procedures.
 
 ### Changed
 - Adding JSON ouput for edges, samples.

diff --git a/CMakeLists.txt b/CMakeLists.txt
index f95ae75ef21b0ad6fb78653d9e5b2e0b386c1069..df49c3b5ae5b54e5c105420954ad6b4b8b4b94b1 100644 (file)
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -19,6 +19,7 @@ add_executable(go_car_go
 
         vehicle_platform/bcar.cc
         vehicle_platform/cost.cc
+        vehicle_platform/reeds_shepp.cpp
         vehicle_platform/steer.cc
 )
 

diff --git a/incl/cost.h b/incl/cost.h
index 46668dfa6aedd479afbc31bf99b02871694b61f5..8c8b8ed430c73312b5281e0cd5daf879e059d266 100644 (file)
--- a/incl/cost.h
+++ b/incl/cost.h
@@ -21,5 +21,6 @@ along with I am car. If not, see <http://www.gnu.org/licenses/>.
 #include "rrtnode.h"
 
 float co1(RRTNode *init, RRTNode *goal);
+float co2(RRTNode *init, RRTNode *goal);
 
 #endif

diff --git a/incl/reeds_shepp.h b/incl/reeds_shepp.h
new file mode 100644 (file)
index 0000000..a038a2a
--- /dev/null
+++ b/incl/reeds_shepp.h
@@ -0,0 +1,93 @@
+/*********************************************************************
+*
+* Software License Agreement (BSD License)
+*
+*  Copyright (c) 2016, Guan-Horng Liu.
+*  All rights reserved.
+*
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions
+*  are met:
+*
+*   * Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   * Redistributions in binary form must reproduce the above
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * Neither the name of the the copyright holder nor the names of its
+*     contributors may be used to endorse or promote products derived
+*     from this software without specific prior written permission.
+*
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*
+* Author:  Guan-Horng Liu
+*********************************************************************/
+
+#ifndef SPACES_REEDS_SHEPP_STATE_SPACE_
+#define SPACES_REEDS_SHEPP_STATE_SPACE_
+
+#include <boost/math/constants/constants.hpp>
+#include <cassert>
+
+typedef int (*ReedsSheppPathSamplingCallback)(double q[3], void* user_data);
+typedef int (*ReedsSheppPathTypeCallback)(int t, void* user_data);
+
+class ReedsSheppStateSpace
+{
+public:
+
+    /** \brief The Reeds-Shepp path segment types */
+    enum ReedsSheppPathSegmentType { RS_NOP=0, RS_LEFT=1, RS_STRAIGHT=2, RS_RIGHT=3 };
+
+    /** \brief Reeds-Shepp path types */
+    static const ReedsSheppPathSegmentType reedsSheppPathType[18][5];
+
+    /** \brief Complete description of a ReedsShepp path */
+    class ReedsSheppPath
+    {
+    public:
+        ReedsSheppPath(const ReedsSheppPathSegmentType* type=reedsSheppPathType[0],
+            double t=std::numeric_limits<double>::max(), double u=0., double v=0.,
+            double w=0., double x=0.);
+
+        double length() const { return totalLength_; }
+
+        /** Path segment types */
+        const ReedsSheppPathSegmentType* type_;
+        /** Path segment lengths */
+        double length_[5];
+        /** Total length */
+        double totalLength_;
+    };
+
+    ReedsSheppStateSpace(double turningRadius) : rho_(turningRadius) {}
+
+    double distance(double q0[3], double q1[3]);
+
+    void type(double q0[3], double q1[3], ReedsSheppPathTypeCallback cb, void* user_data);
+
+    void sample(double q0[3], double q1[3], double step_size, ReedsSheppPathSamplingCallback cb, void* user_data);
+
+    /** \brief Return the shortest Reeds-Shepp path from SE(2) state state1 to SE(2) state state2 */
+    ReedsSheppPath reedsShepp(double q0[3], double q1[3]);
+
+protected:
+    void interpolate(double q0[3], ReedsSheppPath &path, double seg, double q[3]);
+
+    /** \brief Turning radius */
+    double rho_;
+};
+
+#endif

diff --git a/incl/steer.h b/incl/steer.h
index 601578f36cb308bea28d9c55eaffb7c04b0d2bab..6584ecf7098e5b2247ae39f62414f86c24eed42c 100644 (file)
--- a/incl/steer.h
+++ b/incl/steer.h
@@ -23,5 +23,6 @@ along with I am car. If not, see <http://www.gnu.org/licenses/>.
 
 std::vector<RRTNode *> st1(RRTNode *init, RRTNode *goal);
 std::vector<RRTNode *> st2(RRTNode *init, RRTNode *goal);
+std::vector<RRTNode *> st3(RRTNode *init, RRTNode *goal);
 
 #endif

diff --git a/vehicle_platform/cost.cc b/vehicle_platform/cost.cc
index 27a1c60894e491ef155b72680f5b63e86868d8b5..81a3edb08415f4f55eb3d9a8f1759a37ee4c86b7 100644 (file)
--- a/vehicle_platform/cost.cc
+++ b/vehicle_platform/cost.cc
@@ -19,6 +19,7 @@ along with I am car. If not, see <http://www.gnu.org/licenses/>.
 #define COST_H
 
 #include <cmath>
+#include "reeds_shepp.h"
 #include "rrtnode.h"
 
 float co1(RRTNode *init, RRTNode *goal)
@@ -28,4 +29,12 @@ float co1(RRTNode *init, RRTNode *goal)
         return pow(pow(dx, 2) + pow(dy, 2), 0.5);
 }
 
+float co2(RRTNode *init, RRTNode *goal)
+{
+        double q0[] = {init->x(), init->y(), init->h()};
+        double q1[] = {goal->x(), goal->y(), goal->h()};
+        ReedsSheppStateSpace rsss(10.82); // TODO const param
+        return static_cast<float>(rsss.distance(q0, q1));
+}
+
 #endif

diff --git a/vehicle_platform/reeds_shepp.cpp b/vehicle_platform/reeds_shepp.cpp
new file mode 100644 (file)
index 0000000..575d428
--- /dev/null
+++ b/vehicle_platform/reeds_shepp.cpp
@@ -0,0 +1,620 @@
+/*********************************************************************
+*
+* Software License Agreement (BSD License)
+*
+*  Copyright (c) 2016, Guan-Horng Liu.
+*  All rights reserved.
+*
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions
+*  are met:
+*
+*   * Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   * Redistributions in binary form must reproduce the above
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * Neither the name of the the copyright holder nor the names of its
+*     contributors may be used to endorse or promote products derived
+*     from this software without specific prior written permission.
+*
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*
+* Author:  Guan-Horng Liu
+*********************************************************************/
+
+
+#include "reeds_shepp.h"
+#include <boost/math/constants/constants.hpp>
+
+
+namespace
+{
+    // The comments, variable names, etc. use the nomenclature from the Reeds & Shepp paper.
+
+    const double pi = boost::math::constants::pi<double>();
+    const double twopi = 2. * pi;
+    const double RS_EPS = 1e-6;
+    const double ZERO = 10*std::numeric_limits<double>::epsilon();
+
+    inline double mod2pi(double x)
+    {
+        double v = fmod(x, twopi);
+        if (v < -pi)
+            v += twopi;
+        else
+            if (v > pi)
+                v -= twopi;
+        return v;
+    }
+    inline void polar(double x, double y, double &r, double &theta)
+    {
+        r = sqrt(x*x + y*y);
+        theta = atan2(y, x);
+    }
+    inline void tauOmega(double u, double v, double xi, double eta, double phi, double &tau, double &omega)
+    {
+        double delta = mod2pi(u-v), A = sin(u) - sin(delta), B = cos(u) - cos(delta) - 1.;
+        double t1 = atan2(eta*A - xi*B, xi*A + eta*B), t2 = 2. * (cos(delta) - cos(v) - cos(u)) + 3;
+        tau = (t2<0) ? mod2pi(t1+pi) : mod2pi(t1);
+        omega = mod2pi(tau - u + v - phi) ;
+    }
+
+    // formula 8.1 in Reeds-Shepp paper
+    inline bool LpSpLp(double x, double y, double phi, double &t, double &u, double &v)
+    {
+        polar(x - sin(phi), y - 1. + cos(phi), u, t);
+        if (t >= -ZERO)
+        {
+            v = mod2pi(phi - t);
+            if (v >= -ZERO)
+            {
+                assert(fabs(u*cos(t) + sin(phi) - x) < RS_EPS);
+                assert(fabs(u*sin(t) - cos(phi) + 1 - y) < RS_EPS);
+                assert(fabs(mod2pi(t+v - phi)) < RS_EPS);
+                return true;
+            }
+        }
+        return false;
+    }
+    // formula 8.2
+    inline bool LpSpRp(double x, double y, double phi, double &t, double &u, double &v)
+    {
+        double t1, u1;
+        polar(x + sin(phi), y - 1. - cos(phi), u1, t1);
+        u1 = u1*u1;
+        if (u1 >= 4.)
+        {
+            double theta;
+            u = sqrt(u1 - 4.);
+            theta = atan2(2., u);
+            t = mod2pi(t1 + theta);
+            v = mod2pi(t - phi);
+            assert(fabs(2*sin(t) + u*cos(t) - sin(phi) - x) < RS_EPS);
+            assert(fabs(-2*cos(t) + u*sin(t) + cos(phi) + 1 - y) < RS_EPS);
+            assert(fabs(mod2pi(t-v - phi)) < RS_EPS);
+            return t>=-ZERO && v>=-ZERO;
+        }
+        return false;
+    }
+    void CSC(double x, double y, double phi, ReedsSheppStateSpace::ReedsSheppPath &path)
+    {
+        double t, u, v, Lmin = path.length(), L;
+        if (LpSpLp(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
+        {
+            path = ReedsSheppStateSpace::ReedsSheppPath(
+                ReedsSheppStateSpace::reedsSheppPathType[14], t, u, v);
+            Lmin = L;
+        }
+        if (LpSpLp(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
+        {
+            path = ReedsSheppStateSpace::ReedsSheppPath(
+                ReedsSheppStateSpace::reedsSheppPathType[14], -t, -u, -v);
+            Lmin = L;
+        }
+        if (LpSpLp(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
+        {
+            path = ReedsSheppStateSpace::ReedsSheppPath(
+                ReedsSheppStateSpace::reedsSheppPathType[15], t, u, v);
+            Lmin = L;
+        }
+        if (LpSpLp(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
+        {
+            path = ReedsSheppStateSpace::ReedsSheppPath(
+                ReedsSheppStateSpace::reedsSheppPathType[15], -t, -u, -v);
+            Lmin = L;
+        }
+        if (LpSpRp(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
+        {
+            path = ReedsSheppStateSpace::ReedsSheppPath(
+                ReedsSheppStateSpace::reedsSheppPathType[12], t, u, v);
+            Lmin = L;
+        }
+        if (LpSpRp(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
+        {
+            path = ReedsSheppStateSpace::ReedsSheppPath(
+                ReedsSheppStateSpace::reedsSheppPathType[12], -t, -u, -v);
+            Lmin = L;
+        }
+        if (LpSpRp(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
+        {
+            path = ReedsSheppStateSpace::ReedsSheppPath(
+                ReedsSheppStateSpace::reedsSheppPathType[13], t, u, v);
+            Lmin = L;
+        }
+        if (LpSpRp(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
+            path = ReedsSheppStateSpace::ReedsSheppPath(
+                ReedsSheppStateSpace::reedsSheppPathType[13], -t, -u, -v);
+    }
+    // formula 8.3 / 8.4  *** TYPO IN PAPER ***
+    inline bool LpRmL(double x, double y, double phi, double &t, double &u, double &v)
+    {
+        double xi = x - sin(phi), eta = y - 1. + cos(phi), u1, theta;
+        polar(xi, eta, u1, theta);
+        if (u1 <= 4.)
+        {
+            u = -2.*asin(.25 * u1);
+            t = mod2pi(theta + .5 * u + pi);
+            v = mod2pi(phi - t + u);
+            assert(fabs(2*(sin(t) - sin(t-u)) + sin(phi) - x) < RS_EPS);
+            assert(fabs(2*(-cos(t) + cos(t-u)) - cos(phi) + 1 - y) < RS_EPS);
+            assert(fabs(mod2pi(t-u+v - phi)) < RS_EPS);
+            return t>=-ZERO && u<=ZERO;
+        }
+        return false;
+    }
+    void CCC(double x, double y, double phi, ReedsSheppStateSpace::ReedsSheppPath &path)
+    {
+        double t, u, v, Lmin = path.length(), L;
+        if (LpRmL(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
+        {
+            path = ReedsSheppStateSpace::ReedsSheppPath(
+                ReedsSheppStateSpace::reedsSheppPathType[0], t, u, v);
+            Lmin = L;
+        }
+        if (LpRmL(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
+        {
+            path = ReedsSheppStateSpace::ReedsSheppPath(
+                ReedsSheppStateSpace::reedsSheppPathType[0], -t, -u, -v);
+            Lmin = L;
+        }
+        if (LpRmL(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
+        {
+            path = ReedsSheppStateSpace::ReedsSheppPath(
+                ReedsSheppStateSpace::reedsSheppPathType[1], t, u, v);
+            Lmin = L;
+        }
+        if (LpRmL(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
+        {
+            path = ReedsSheppStateSpace::ReedsSheppPath(
+                ReedsSheppStateSpace::reedsSheppPathType[1], -t, -u, -v);
+            Lmin = L;
+        }
+
+        // backwards
+        double xb = x*cos(phi) + y*sin(phi), yb = x*sin(phi) - y*cos(phi);
+        if (LpRmL(xb, yb, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
+        {
+            path = ReedsSheppStateSpace::ReedsSheppPath(
+                ReedsSheppStateSpace::reedsSheppPathType[0], v, u, t);
+            Lmin = L;
+        }
+        if (LpRmL(-xb, yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
+        {
+            path = ReedsSheppStateSpace::ReedsSheppPath(
+                ReedsSheppStateSpace::reedsSheppPathType[0], -v, -u, -t);
+            Lmin = L;
+        }
+        if (LpRmL(xb, -yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
+        {
+            path = ReedsSheppStateSpace::ReedsSheppPath(
+                ReedsSheppStateSpace::reedsSheppPathType[1], v, u, t);
+            Lmin = L;
+        }
+        if (LpRmL(-xb, -yb, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
+            path = ReedsSheppStateSpace::ReedsSheppPath(
+                ReedsSheppStateSpace::reedsSheppPathType[1], -v, -u, -t);
+    }
+    // formula 8.7
+    inline bool LpRupLumRm(double x, double y, double phi, double &t, double &u, double &v)
+    {
+        double xi = x + sin(phi), eta = y - 1. - cos(phi), rho = .25 * (2. + sqrt(xi*xi + eta*eta));
+        if (rho <= 1.)
+        {
+            u = acos(rho);
+            tauOmega(u, -u, xi, eta, phi, t, v);
+            assert(fabs(2*(sin(t)-sin(t-u)+sin(t-2*u))-sin(phi) - x) < RS_EPS);
+            assert(fabs(2*(-cos(t)+cos(t-u)-cos(t-2*u))+cos(phi)+1 - y) < RS_EPS);
+            assert(fabs(mod2pi(t-2*u-v - phi)) < RS_EPS);
+            return t>=-ZERO && v<=ZERO;
+        }
+        return false;
+    }
+    // formula 8.8
+    inline bool LpRumLumRp(double x, double y, double phi, double &t, double &u, double &v)
+    {
+        double xi = x + sin(phi), eta = y - 1. - cos(phi), rho = (20. - xi*xi - eta*eta) / 16.;
+        if (rho>=0 && rho<=1)
+        {
+            u = -acos(rho);
+            if (u >= -.5 * pi)
+            {
+                tauOmega(u, u, xi, eta, phi, t, v);
+                assert(fabs(4*sin(t)-2*sin(t-u)-sin(phi) - x) < RS_EPS);
+                assert(fabs(-4*cos(t)+2*cos(t-u)+cos(phi)+1 - y) < RS_EPS);
+                assert(fabs(mod2pi(t-v - phi)) < RS_EPS);
+                return t>=-ZERO && v>=-ZERO;
+            }
+        }
+        return false;
+    }
+    void CCCC(double x, double y, double phi, ReedsSheppStateSpace::ReedsSheppPath &path)
+    {
+        double t, u, v, Lmin = path.length(), L;
+        if (LpRupLumRm(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + 2.*fabs(u) + fabs(v)))
+        {
+            path = ReedsSheppStateSpace::ReedsSheppPath(
+                ReedsSheppStateSpace::reedsSheppPathType[2], t, u, -u, v);
+            Lmin = L;
+        }
+        if (LpRupLumRm(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + 2.*fabs(u) + fabs(v))) // timeflip
+        {
+            path = ReedsSheppStateSpace::ReedsSheppPath(
+                ReedsSheppStateSpace::reedsSheppPathType[2], -t, -u, u, -v);
+            Lmin = L;
+        }
+        if (LpRupLumRm(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + 2.*fabs(u) + fabs(v))) // reflect
+        {
+            path = ReedsSheppStateSpace::ReedsSheppPath(
+                ReedsSheppStateSpace::reedsSheppPathType[3], t, u, -u, v);
+            Lmin = L;
+        }
+        if (LpRupLumRm(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + 2.*fabs(u) + fabs(v))) // timeflip + reflect
+        {
+            path = ReedsSheppStateSpace::ReedsSheppPath(
+                ReedsSheppStateSpace::reedsSheppPathType[3], -t, -u, u, -v);
+            Lmin = L;
+        }
+
+        if (LpRumLumRp(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + 2.*fabs(u) + fabs(v)))
+        {
+            path = ReedsSheppStateSpace::ReedsSheppPath(
+                ReedsSheppStateSpace::reedsSheppPathType[2], t, u, u, v);
+            Lmin = L;
+        }
+        if (LpRumLumRp(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + 2.*fabs(u) + fabs(v))) // timeflip
+        {
+            path = ReedsSheppStateSpace::ReedsSheppPath(
+                ReedsSheppStateSpace::reedsSheppPathType[2], -t, -u, -u, -v);
+            Lmin = L;
+        }
+        if (LpRumLumRp(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + 2.*fabs(u) + fabs(v))) // reflect
+        {
+            path = ReedsSheppStateSpace::ReedsSheppPath(
+                ReedsSheppStateSpace::reedsSheppPathType[3], t, u, u, v);
+            Lmin = L;
+        }
+        if (LpRumLumRp(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + 2.*fabs(u) + fabs(v))) // timeflip + reflect
+            path = ReedsSheppStateSpace::ReedsSheppPath(
+                ReedsSheppStateSpace::reedsSheppPathType[3], -t, -u, -u, -v);
+    }
+    // formula 8.9
+    inline bool LpRmSmLm(double x, double y, double phi, double &t, double &u, double &v)
+    {
+        double xi = x - sin(phi), eta = y - 1. + cos(phi), rho, theta;
+        polar(xi, eta, rho, theta);
+        if (rho >= 2.)
+        {
+            double r = sqrt(rho*rho - 4.);
+            u = 2. - r;
+            t = mod2pi(theta + atan2(r, -2.));
+            v = mod2pi(phi - .5*pi - t);
+            assert(fabs(2*(sin(t)-cos(t))-u*sin(t)+sin(phi) - x) < RS_EPS);
+            assert(fabs(-2*(sin(t)+cos(t))+u*cos(t)-cos(phi)+1 - y) < RS_EPS);
+            assert(fabs(mod2pi(t+pi/2+v-phi)) < RS_EPS);
+            return t>=-ZERO && u<=ZERO && v<=ZERO;
+        }
+        return false;
+    }
+    // formula 8.10
+    inline bool LpRmSmRm(double x, double y, double phi, double &t, double &u, double &v)
+    {
+        double xi = x + sin(phi), eta = y - 1. - cos(phi), rho, theta;
+        polar(-eta, xi, rho, theta);
+        if (rho >= 2.)
+        {
+            t = theta;
+            u = 2. - rho;
+            v = mod2pi(t + .5*pi - phi);
+            assert(fabs(2*sin(t)-cos(t-v)-u*sin(t) - x) < RS_EPS);
+            assert(fabs(-2*cos(t)-sin(t-v)+u*cos(t)+1 - y) < RS_EPS);
+            assert(fabs(mod2pi(t+pi/2-v-phi)) < RS_EPS);
+            return t>=-ZERO && u<=ZERO && v<=ZERO;
+        }
+        return false;
+    }
+    void CCSC(double x, double y, double phi, ReedsSheppStateSpace::ReedsSheppPath &path)
+    {
+        double t, u, v, Lmin = path.length() - .5*pi, L;
+        if (LpRmSmLm(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
+        {
+            path = ReedsSheppStateSpace::ReedsSheppPath(
+                ReedsSheppStateSpace::reedsSheppPathType[4], t, -.5*pi, u, v);
+            Lmin = L;
+        }
+        if (LpRmSmLm(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
+        {
+            path = ReedsSheppStateSpace::ReedsSheppPath(
+                ReedsSheppStateSpace::reedsSheppPathType[4], -t, .5*pi, -u, -v);
+            Lmin = L;
+        }
+        if (LpRmSmLm(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
+        {
+            path = ReedsSheppStateSpace::ReedsSheppPath(
+                ReedsSheppStateSpace::reedsSheppPathType[5], t, -.5*pi, u, v);
+            Lmin = L;
+        }
+        if (LpRmSmLm(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
+        {
+            path = ReedsSheppStateSpace::ReedsSheppPath(
+                ReedsSheppStateSpace::reedsSheppPathType[5], -t, .5*pi, -u, -v);
+            Lmin = L;
+        }
+
+        if (LpRmSmRm(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
+        {
+            path = ReedsSheppStateSpace::ReedsSheppPath(
+                ReedsSheppStateSpace::reedsSheppPathType[8], t, -.5*pi, u, v);
+            Lmin = L;
+        }
+        if (LpRmSmRm(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
+        {
+            path = ReedsSheppStateSpace::ReedsSheppPath(
+                ReedsSheppStateSpace::reedsSheppPathType[8], -t, .5*pi, -u, -v);
+            Lmin = L;
+        }
+        if (LpRmSmRm(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
+        {
+            path = ReedsSheppStateSpace::ReedsSheppPath(
+                ReedsSheppStateSpace::reedsSheppPathType[9], t, -.5*pi, u, v);
+            Lmin = L;
+        }
+        if (LpRmSmRm(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
+        {
+            path = ReedsSheppStateSpace::ReedsSheppPath(
+                ReedsSheppStateSpace::reedsSheppPathType[9], -t, .5*pi, -u, -v);
+            Lmin = L;
+        }
+
+        // backwards
+        double xb = x*cos(phi) + y*sin(phi), yb = x*sin(phi) - y*cos(phi);
+        if (LpRmSmLm(xb, yb, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
+        {
+            path = ReedsSheppStateSpace::ReedsSheppPath(
+                ReedsSheppStateSpace::reedsSheppPathType[6], v, u, -.5*pi, t);
+            Lmin = L;
+        }
+        if (LpRmSmLm(-xb, yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
+        {
+            path = ReedsSheppStateSpace::ReedsSheppPath(
+                ReedsSheppStateSpace::reedsSheppPathType[6], -v, -u, .5*pi, -t);
+            Lmin = L;
+        }
+        if (LpRmSmLm(xb, -yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
+        {
+            path = ReedsSheppStateSpace::ReedsSheppPath(
+                ReedsSheppStateSpace::reedsSheppPathType[7], v, u, -.5*pi, t);
+            Lmin = L;
+        }
+        if (LpRmSmLm(-xb, -yb, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
+        {
+            path = ReedsSheppStateSpace::ReedsSheppPath(
+                ReedsSheppStateSpace::reedsSheppPathType[7], -v, -u, .5*pi, -t);
+            Lmin = L;
+        }
+
+        if (LpRmSmRm(xb, yb, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
+        {
+            path = ReedsSheppStateSpace::ReedsSheppPath(
+                ReedsSheppStateSpace::reedsSheppPathType[10], v, u, -.5*pi, t);
+            Lmin = L;
+        }
+        if (LpRmSmRm(-xb, yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
+        {
+            path = ReedsSheppStateSpace::ReedsSheppPath(
+                ReedsSheppStateSpace::reedsSheppPathType[10], -v, -u, .5*pi, -t);
+            Lmin = L;
+        }
+        if (LpRmSmRm(xb, -yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
+        {
+            path = ReedsSheppStateSpace::ReedsSheppPath(
+                ReedsSheppStateSpace::reedsSheppPathType[11], v, u, -.5*pi, t);
+            Lmin = L;
+        }
+        if (LpRmSmRm(-xb, -yb, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
+            path = ReedsSheppStateSpace::ReedsSheppPath(
+                ReedsSheppStateSpace::reedsSheppPathType[11], -v, -u, .5*pi, -t);
+    }
+    // formula 8.11 *** TYPO IN PAPER ***
+    inline bool LpRmSLmRp(double x, double y, double phi, double &t, double &u, double &v)
+    {
+        double xi = x + sin(phi), eta = y - 1. - cos(phi), rho, theta;
+        polar(xi, eta, rho, theta);
+        if (rho >= 2.)
+        {
+            u = 4. - sqrt(rho*rho - 4.);
+            if (u <= ZERO)
+            {
+                t = mod2pi(atan2((4-u)*xi -2*eta, -2*xi + (u-4)*eta));
+                v = mod2pi(t - phi);
+                assert(fabs(4*sin(t)-2*cos(t)-u*sin(t)-sin(phi) - x) < RS_EPS);
+                assert(fabs(-4*cos(t)-2*sin(t)+u*cos(t)+cos(phi)+1 - y) < RS_EPS);
+                assert(fabs(mod2pi(t-v-phi)) < RS_EPS);
+                return t>=-ZERO && v>=-ZERO;
+            }
+        }
+        return false;
+    }
+    void CCSCC(double x, double y, double phi, ReedsSheppStateSpace::ReedsSheppPath &path)
+    {
+        double t, u, v, Lmin = path.length() - pi, L;
+        if (LpRmSLmRp(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
+        {
+            path = ReedsSheppStateSpace::ReedsSheppPath(
+                ReedsSheppStateSpace::reedsSheppPathType[16], t, -.5*pi, u, -.5*pi, v);
+            Lmin = L;
+        }
+        if (LpRmSLmRp(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
+        {
+            path = ReedsSheppStateSpace::ReedsSheppPath(
+                ReedsSheppStateSpace::reedsSheppPathType[16], -t, .5*pi, -u, .5*pi, -v);
+            Lmin = L;
+        }
+        if (LpRmSLmRp(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
+        {
+            path = ReedsSheppStateSpace::ReedsSheppPath(
+                ReedsSheppStateSpace::reedsSheppPathType[17], t, -.5*pi, u, -.5*pi, v);
+            Lmin = L;
+        }
+        if (LpRmSLmRp(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
+            path = ReedsSheppStateSpace::ReedsSheppPath(
+                ReedsSheppStateSpace::reedsSheppPathType[17], -t, .5*pi, -u, .5*pi, -v);
+    }
+
+    ReedsSheppStateSpace::ReedsSheppPath reedsShepp(double x, double y, double phi)
+    {
+        ReedsSheppStateSpace::ReedsSheppPath path;
+        CSC(x, y, phi, path);
+        CCC(x, y, phi, path);
+        CCCC(x, y, phi, path);
+        CCSC(x, y, phi, path);
+        CCSCC(x, y, phi, path);
+        return path;
+    }
+}
+
+const ReedsSheppStateSpace::ReedsSheppPathSegmentType
+ReedsSheppStateSpace::reedsSheppPathType[18][5] = {
+    { RS_LEFT, RS_RIGHT, RS_LEFT, RS_NOP, RS_NOP },             // 0
+    { RS_RIGHT, RS_LEFT, RS_RIGHT, RS_NOP, RS_NOP },            // 1
+    { RS_LEFT, RS_RIGHT, RS_LEFT, RS_RIGHT, RS_NOP },           // 2
+    { RS_RIGHT, RS_LEFT, RS_RIGHT, RS_LEFT, RS_NOP },           // 3
+    { RS_LEFT, RS_RIGHT, RS_STRAIGHT, RS_LEFT, RS_NOP },        // 4
+    { RS_RIGHT, RS_LEFT, RS_STRAIGHT, RS_RIGHT, RS_NOP },       // 5
+    { RS_LEFT, RS_STRAIGHT, RS_RIGHT, RS_LEFT, RS_NOP },        // 6
+    { RS_RIGHT, RS_STRAIGHT, RS_LEFT, RS_RIGHT, RS_NOP },       // 7
+    { RS_LEFT, RS_RIGHT, RS_STRAIGHT, RS_RIGHT, RS_NOP },       // 8
+    { RS_RIGHT, RS_LEFT, RS_STRAIGHT, RS_LEFT, RS_NOP },        // 9
+    { RS_RIGHT, RS_STRAIGHT, RS_RIGHT, RS_LEFT, RS_NOP },       // 10
+    { RS_LEFT, RS_STRAIGHT, RS_LEFT, RS_RIGHT, RS_NOP },        // 11
+    { RS_LEFT, RS_STRAIGHT, RS_RIGHT, RS_NOP, RS_NOP },         // 12
+    { RS_RIGHT, RS_STRAIGHT, RS_LEFT, RS_NOP, RS_NOP },         // 13
+    { RS_LEFT, RS_STRAIGHT, RS_LEFT, RS_NOP, RS_NOP },          // 14
+    { RS_RIGHT, RS_STRAIGHT, RS_RIGHT, RS_NOP, RS_NOP },        // 15
+    { RS_LEFT, RS_RIGHT, RS_STRAIGHT, RS_LEFT, RS_RIGHT },      // 16
+    { RS_RIGHT, RS_LEFT, RS_STRAIGHT, RS_RIGHT, RS_LEFT }       // 17
+};
+
+ReedsSheppStateSpace::ReedsSheppPath::ReedsSheppPath(const ReedsSheppPathSegmentType* type,
+    double t, double u, double v, double w, double x)
+    : type_(type)
+{
+    length_[0] = t; length_[1] = u; length_[2] = v; length_[3] = w; length_[4] = x;
+    totalLength_ = fabs(t) + fabs(u) + fabs(v) + fabs(w) + fabs(x);
+}
+
+
+double ReedsSheppStateSpace::distance(double q0[3], double q1[3])
+{
+    return rho_ * reedsShepp(q0, q1).length();
+}
+
+ReedsSheppStateSpace::ReedsSheppPath ReedsSheppStateSpace::reedsShepp(double q0[3], double q1[3])
+{
+    double dx = q1[0] - q0[0], dy = q1[1] - q0[1], dth = q1[2] - q0[2];
+    double c = cos(q0[2]), s = sin(q0[2]);
+    double x = c*dx + s*dy, y = -s*dx + c*dy;
+    return ::reedsShepp(x/rho_, y/rho_, dth);
+}
+
+void ReedsSheppStateSpace::type(double q0[3], double q1[3], ReedsSheppPathTypeCallback cb, void* user_data)
+{
+    ReedsSheppPath path = reedsShepp(q0, q1);
+    for (int i=0;i<5;++i)
+        cb(path.type_[i], user_data);
+    return;
+}
+
+void ReedsSheppStateSpace::sample(double q0[3], double q1[3], double step_size, ReedsSheppPathSamplingCallback cb, void* user_data)
+{
+    ReedsSheppPath path = reedsShepp(q0, q1);
+    double dist = rho_ * path.length(); // TODO shorten path here
+
+    for (double seg=0.0; seg<=dist; seg+=step_size){
+        double qnew[3] = {};
+        interpolate(q0, path, seg/rho_, qnew);
+        cb( qnew, user_data);
+    }
+    return;
+}
+
+void ReedsSheppStateSpace::interpolate(double q0[3], ReedsSheppPath &path, double seg, double s[3])
+{
+
+    if (seg < 0.0) seg = 0.0;
+    if (seg > path.length()) seg = path.length();
+
+    double phi, v;
+
+    s[0] = s[1] = 0.0;
+    s[2] = q0[2];
+
+    for (unsigned int i=0; i<5 && seg>0; ++i)
+    {
+        if (path.length_[i]<0)
+        {
+            v = std::max(-seg, path.length_[i]);
+            seg += v;
+        }
+        else
+        {
+            v = std::min(seg, path.length_[i]);
+            seg -= v;
+        }
+        phi = s[2];
+        switch(path.type_[i])
+        {
+            case RS_LEFT:
+                s[0] += ( sin(phi+v) - sin(phi));
+                s[1] += (-cos(phi+v) + cos(phi));
+                s[2] = phi + v;
+                break;
+            case RS_RIGHT:
+                s[0] += (-sin(phi-v) + sin(phi));
+                s[1] += ( cos(phi-v) - cos(phi));
+                s[2] = phi - v;
+                break;
+            case RS_STRAIGHT:
+                s[0] += (v * cos(phi));
+                s[1] += (v * sin(phi));
+                break;
+            case RS_NOP:
+                break;
+        }
+    }
+
+    s[0] = s[0] * rho_ + q0[0];
+    s[1] = s[1] * rho_ + q0[1];
+}

diff --git a/vehicle_platform/steer.cc b/vehicle_platform/steer.cc
index 942b2d1c59a0dfc99502c957d28ae67201e3b271..c58b0048347b95879bd0cded4bb41518a6a3ffeb 100644 (file)
--- a/vehicle_platform/steer.cc
+++ b/vehicle_platform/steer.cc
@@ -16,6 +16,7 @@ along with I am car. If not, see <http://www.gnu.org/licenses/>.
 */
 
 #include <cmath>
+#include "reeds_shepp.h"
 #include "steer.h"
 
 #define ST2WHEELBASE 2.450
@@ -66,3 +67,20 @@ std::vector<RRTNode *> st2(RRTNode *init, RRTNode *goal)
         nodes.push_back(new RRTNode(new_x, new_y, new_h));
         return nodes;
 }
+
+int cb_st3(double q[3], void *user_data)
+{
+        std::vector<RRTNode *> *nodes = (std::vector<RRTNode *> *) user_data;
+        nodes->push_back(new RRTNode(q[0], q[1], q[2]));
+        return 0;
+}
+
+std::vector<RRTNode *> st3(RRTNode *init, RRTNode *goal)
+{
+        std::vector<RRTNode *> nodes;
+        double q0[] = {init->x(), init->y(), init->h()};
+        double q1[] = {goal->x(), goal->y(), goal->h()};
+        ReedsSheppStateSpace rsss(10.82); // TODO const param
+        rsss.sample(q0, q1, 1, cb_st3, &nodes); // TODO const
+        return nodes;
+}