diff --git a/PathPlanning/ReedsSheppPath/ReedsSheppStateSpace.cpp b/PathPlanning/ReedsSheppPath/ReedsSheppStateSpace.cpp deleted file mode 100755 index aa114168..00000000 --- a/PathPlanning/ReedsSheppPath/ReedsSheppStateSpace.cpp +++ /dev/null @@ -1,709 +0,0 @@ -/********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2010, Rice University -* 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 Rice University 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: Mark Moll */ - -#include "ompl/base/spaces/ReedsSheppStateSpace.h" -#include "ompl/base/SpaceInformation.h" -#include "ompl/util/Exception.h" -#include -#include - -using namespace ompl::base; - -namespace -{ - // The comments, variable names, etc. use the nomenclature from the Reeds & Shepp paper. - - const double pi = boost::math::constants::pi(); - const double twopi = 2. * pi; - const double RS_EPS = 1e-6; - const double ZERO = 10 * std::numeric_limits::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 ompl::base::ReedsSheppStateSpace::ReedsSheppPathSegmentType - ompl::base::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 -}; - -ompl::base::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 ompl::base::ReedsSheppStateSpace::distance(const State *state1, const State *state2) const -{ - return rho_ * reedsShepp(state1, state2).length(); -} - -void ompl::base::ReedsSheppStateSpace::interpolate(const State *from, const State *to, const double t, - State *state) const -{ - bool firstTime = true; - ReedsSheppPath path; - interpolate(from, to, t, firstTime, path, state); -} - -void ompl::base::ReedsSheppStateSpace::interpolate(const State *from, const State *to, const double t, bool &firstTime, - ReedsSheppPath &path, State *state) const -{ - if (firstTime) - { - if (t >= 1.) - { - if (to != state) - copyState(state, to); - return; - } - if (t <= 0.) - { - if (from != state) - copyState(state, from); - return; - } - path = reedsShepp(from, to); - firstTime = false; - } - interpolate(from, path, t, state); -} - -void ompl::base::ReedsSheppStateSpace::interpolate(const State *from, const ReedsSheppPath &path, double t, - State *state) const -{ - auto *s = allocState()->as(); - double seg = t * path.length(), phi, v; - - s->setXY(0., 0.); - s->setYaw(from->as()->getYaw()); - 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->getYaw(); - switch (path.type_[i]) - { - case RS_LEFT: - s->setXY(s->getX() + sin(phi + v) - sin(phi), s->getY() - cos(phi + v) + cos(phi)); - s->setYaw(phi + v); - break; - case RS_RIGHT: - s->setXY(s->getX() - sin(phi - v) + sin(phi), s->getY() + cos(phi - v) - cos(phi)); - s->setYaw(phi - v); - break; - case RS_STRAIGHT: - s->setXY(s->getX() + v * cos(phi), s->getY() + v * sin(phi)); - break; - case RS_NOP: - break; - } - } - state->as()->setX(s->getX() * rho_ + from->as()->getX()); - state->as()->setY(s->getY() * rho_ + from->as()->getY()); - getSubspace(1)->enforceBounds(s->as(1)); - state->as()->setYaw(s->getYaw()); - freeState(s); -} - -ompl::base::ReedsSheppStateSpace::ReedsSheppPath ompl::base::ReedsSheppStateSpace::reedsShepp(const State *state1, - const State *state2) const -{ - const auto *s1 = static_cast(state1); - const auto *s2 = static_cast(state2); - double x1 = s1->getX(), y1 = s1->getY(), th1 = s1->getYaw(); - double x2 = s2->getX(), y2 = s2->getY(), th2 = s2->getYaw(); - double dx = x2 - x1, dy = y2 - y1, c = cos(th1), s = sin(th1); - double x = c * dx + s * dy, y = -s * dx + c * dy, phi = th2 - th1; - return ::reedsShepp(x / rho_, y / rho_, phi); -} - -void ompl::base::ReedsSheppMotionValidator::defaultSettings() -{ - stateSpace_ = dynamic_cast(si_->getStateSpace().get()); - if (stateSpace_ == nullptr) - throw Exception("No state space for motion validator"); -} - -bool ompl::base::ReedsSheppMotionValidator::checkMotion(const State *s1, const State *s2, - std::pair &lastValid) const -{ - /* assume motion starts in a valid configuration so s1 is valid */ - - bool result = true, firstTime = true; - ReedsSheppStateSpace::ReedsSheppPath path; - int nd = stateSpace_->validSegmentCount(s1, s2); - - if (nd > 1) - { - /* temporary storage for the checked state */ - State *test = si_->allocState(); - - for (int j = 1; j < nd; ++j) - { - stateSpace_->interpolate(s1, s2, (double)j / (double)nd, firstTime, path, test); - if (!si_->isValid(test)) - { - lastValid.second = (double)(j - 1) / (double)nd; - if (lastValid.first != nullptr) - stateSpace_->interpolate(s1, s2, lastValid.second, firstTime, path, lastValid.first); - result = false; - break; - } - } - si_->freeState(test); - } - - if (result) - if (!si_->isValid(s2)) - { - lastValid.second = (double)(nd - 1) / (double)nd; - if (lastValid.first != nullptr) - stateSpace_->interpolate(s1, s2, lastValid.second, firstTime, path, lastValid.first); - result = false; - } - - if (result) - valid_++; - else - invalid_++; - - return result; -} - -bool ompl::base::ReedsSheppMotionValidator::checkMotion(const State *s1, const State *s2) const -{ - /* assume motion starts in a valid configuration so s1 is valid */ - if (!si_->isValid(s2)) - return false; - - bool result = true, firstTime = true; - ReedsSheppStateSpace::ReedsSheppPath path; - int nd = stateSpace_->validSegmentCount(s1, s2); - - /* initialize the queue of test positions */ - std::queue> pos; - if (nd >= 2) - { - pos.push(std::make_pair(1, nd - 1)); - - /* temporary storage for the checked state */ - State *test = si_->allocState(); - - /* repeatedly subdivide the path segment in the middle (and check the middle) */ - while (!pos.empty()) - { - std::pair x = pos.front(); - - int mid = (x.first + x.second) / 2; - stateSpace_->interpolate(s1, s2, (double)mid / (double)nd, firstTime, path, test); - - if (!si_->isValid(test)) - { - result = false; - break; - } - - pos.pop(); - - if (x.first < mid) - pos.push(std::make_pair(x.first, mid - 1)); - if (x.second > mid) - pos.push(std::make_pair(mid + 1, x.second)); - } - - si_->freeState(test); - } - - if (result) - valid_++; - else - invalid_++; - - return result; -} diff --git a/PathPlanning/ReedsSheppPath/ReedsSheppStateSpace.h b/PathPlanning/ReedsSheppPath/ReedsSheppStateSpace.h deleted file mode 100755 index 40e66e4f..00000000 --- a/PathPlanning/ReedsSheppPath/ReedsSheppStateSpace.h +++ /dev/null @@ -1,151 +0,0 @@ -/********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2010, Rice University -* 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 Rice University 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: Mark Moll */ - -#ifndef OMPL_BASE_SPACES_REEDS_SHEPP_STATE_SPACE_ -#define OMPL_BASE_SPACES_REEDS_SHEPP_STATE_SPACE_ - -#include "ompl/base/spaces/SE2StateSpace.h" -#include "ompl/base/MotionValidator.h" -#include - -namespace ompl -{ - namespace base - { - /** \brief An SE(2) state space where distance is measured by the - length of Reeds-Shepp curves. - - The notation and solutions are taken from: - J.A. Reeds and L.A. Shepp, “Optimal paths for a car that goes both - forwards and backwards,” Pacific Journal of Mathematics, - 145(2):367–393, 1990. - - This implementation explicitly computes all 48 Reeds-Shepp curves - and returns the shortest valid solution. This can be improved by - using the configuration space partition described in: - P. Souères and J.-P. Laumond, “Shortest paths synthesis for a - car-like robot,” IEEE Trans. on Automatic Control, 41(5):672–688, - May 1996. - */ - class ReedsSheppStateSpace : public SE2StateSpace - { - 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::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 = 1.0) : rho_(turningRadius) - { - } - - double distance(const State *state1, const State *state2) const override; - - void interpolate(const State *from, const State *to, double t, State *state) const override; - virtual void interpolate(const State *from, const State *to, double t, bool &firstTime, - ReedsSheppPath &path, State *state) const; - - void sanityChecks() const override - { - double zero = std::numeric_limits::epsilon(); - double eps = .1; // rarely such a large error will occur - StateSpace::sanityChecks(zero, eps, ~STATESPACE_INTERPOLATION); - } - - /** \brief Return the shortest Reeds-Shepp path from SE(2) state state1 to SE(2) state state2 */ - ReedsSheppPath reedsShepp(const State *state1, const State *state2) const; - - protected: - virtual void interpolate(const State *from, const ReedsSheppPath &path, double t, State *state) const; - - /** \brief Turning radius */ - double rho_; - }; - - /** \brief A Reeds-Shepp motion validator that only uses the state validity checker. - Motions are checked for validity at a specified resolution. - - This motion validator is almost identical to the DiscreteMotionValidator - except that it remembers the optimal ReedsSheppPath between different calls to - interpolate. */ - class ReedsSheppMotionValidator : public MotionValidator - { - public: - ReedsSheppMotionValidator(SpaceInformation *si) : MotionValidator(si) - { - defaultSettings(); - } - ReedsSheppMotionValidator(const SpaceInformationPtr &si) : MotionValidator(si) - { - defaultSettings(); - } - ~ReedsSheppMotionValidator() override = default; - bool checkMotion(const State *s1, const State *s2) const override; - bool checkMotion(const State *s1, const State *s2, std::pair &lastValid) const override; - - private: - ReedsSheppStateSpace *stateSpace_; - void defaultSettings(); - }; - } -} - -#endif diff --git a/PathPlanning/ReedsSheppPath/pyReedsShepp b/PathPlanning/ReedsSheppPath/pyReedsShepp deleted file mode 160000 index 69aebbb6..00000000 --- a/PathPlanning/ReedsSheppPath/pyReedsShepp +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 69aebbb6ad98f38b60e192999a7302d3b74bb7ad diff --git a/README.md b/README.md index 290788f7..37add091 100644 --- a/README.md +++ b/README.md @@ -52,6 +52,12 @@ This code uses the model predictive trajectory generator to solve boundary probl ![PythonRobotics/figure_1.png at master · AtsushiSakai/PythonRobotics](https://github.com/AtsushiSakai/PythonRobotics/blob/master/PathPlanning/StateLatticePlanner/Figure_4.png) +### Lane sampling results: + +![PythonRobotics/figure_1.png at master · AtsushiSakai/PythonRobotics](https://github.com/AtsushiSakai/PythonRobotics/blob/master/PathPlanning/StateLatticePlanner/Figure_5.png) + +![PythonRobotics/figure_1.png at master · AtsushiSakai/PythonRobotics](https://github.com/AtsushiSakai/PythonRobotics/blob/master/PathPlanning/StateLatticePlanner/Figure_6.png) + ## RRT