From 2c1332e6c1badfa619a7638560b14145be208603 Mon Sep 17 00:00:00 2001 From: Jiri Hubacek Date: Tue, 3 Jul 2018 07:41:21 +0200 Subject: [PATCH] Add Reeds and Shepp steering procedure --- CHANGELOG.md | 1 + CMakeLists.txt | 1 + incl/cost.h | 1 + incl/reeds_shepp.h | 93 +++++ incl/steer.h | 1 + vehicle_platform/cost.cc | 9 + vehicle_platform/reeds_shepp.cpp | 620 +++++++++++++++++++++++++++++++ vehicle_platform/steer.cc | 18 + 8 files changed, 744 insertions(+) create mode 100644 incl/reeds_shepp.h create mode 100644 vehicle_platform/reeds_shepp.cpp diff --git a/CHANGELOG.md b/CHANGELOG.md index 5edb862..854f4af 100644 --- 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 f95ae75..df49c3b 100644 --- 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 46668df..8c8b8ed 100644 --- a/incl/cost.h +++ b/incl/cost.h @@ -21,5 +21,6 @@ along with I am car. If not, see . #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 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 +#include + +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::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 601578f..6584ecf 100644 --- a/incl/steer.h +++ b/incl/steer.h @@ -23,5 +23,6 @@ along with I am car. If not, see . std::vector st1(RRTNode *init, RRTNode *goal); std::vector st2(RRTNode *init, RRTNode *goal); +std::vector st3(RRTNode *init, RRTNode *goal); #endif diff --git a/vehicle_platform/cost.cc b/vehicle_platform/cost.cc index 27a1c60..81a3edb 100644 --- a/vehicle_platform/cost.cc +++ b/vehicle_platform/cost.cc @@ -19,6 +19,7 @@ along with I am car. If not, see . #define COST_H #include +#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(rsss.distance(q0, q1)); +} + #endif diff --git a/vehicle_platform/reeds_shepp.cpp b/vehicle_platform/reeds_shepp.cpp new file mode 100644 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 + + +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 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 942b2d1..c58b004 100644 --- a/vehicle_platform/steer.cc +++ b/vehicle_platform/steer.cc @@ -16,6 +16,7 @@ along with I am car. If not, see . */ #include +#include "reeds_shepp.h" #include "steer.h" #define ST2WHEELBASE 2.450 @@ -66,3 +67,20 @@ std::vector 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 *nodes = (std::vector *) user_data; + nodes->push_back(new RRTNode(q[0], q[1], q[2])); + return 0; +} + +std::vector st3(RRTNode *init, RRTNode *goal) +{ + std::vector 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; +} -- 2.39.2