Merge branch 'feature/perpendicular-slotplanner'

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 6427d4a246274552fc1fff570a1a0f692fa9fcda..9a102c96dae7938d01dac5ae544bb67aeebe22da 100644 (file)
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -10,6 +10,9 @@ ENDIF(TMAX)
 IF(USE_GL)
         ADD_DEFINITIONS(-DUSE_GL=1)
 ENDIF(USE_GL)
+IF(USE_SLOTPLANNER)
+        ADD_DEFINITIONS(-DUSE_SLOTPLANNER=1)
+ENDIF(USE_SLOTPLANNER)
 
 find_package(OpenMP)
 if (OPENMP_FOUND)

diff --git a/README.md b/README.md
index 3f9060cda585242bbf579da070478963926f178a..67255b3a399c90c56afe5fe54b2c4d88e04c64e8 100644 (file)
--- a/README.md
+++ b/README.md
@@ -60,6 +60,7 @@ The list of available macros with values:
         - `Klamm2015` - RRT*-Connect planner.
 - `TMAX` - Specify the upper time bound in seconds.
 - `USE_GL` - Specify if GL is going to be used to plot the algorithm.
+- `USE_SLOTPLANNER` - Specify if slot planner is going to be used.
 
 Implemented Steering procedures:
 - `st1` - Steer directly to goal, no constraints.

diff --git a/base/main.cc b/base/main.cc
index 64532824b0d0fa134e5243001103b878433dca37..7c5f41655bfc0cfbafbc94beadc72a67d5294dca 100644 (file)
--- a/base/main.cc
+++ b/base/main.cc
@@ -51,9 +51,6 @@ along with I am car. If not, see <http://www.gnu.org/licenses/>.
         //#define USE_PTHREAD
 #endif
 
-// enable
-//#define USE_SLOTPLANNER
-
 std::chrono::high_resolution_clock::time_point TSTART_;
 std::chrono::high_resolution_clock::time_point TEND_;
 float TELAPSED = 0;
@@ -161,16 +158,23 @@ int main()
         p.ocost(p.goal());
 
         ParallelSlot ps = ParallelSlot();
+        if (
+                jvi["slot"] != Json::nullValue &&
+                jvi["slot"]["polygon"] != Json::nullValue
+        ) {
+                for (auto xy: jvi["slot"]["polygon"]) {
+                        ps.slot().add_bnode(new RRTNode(
+                                xy[0].asFloat(),
+                                xy[1].asFloat()
+                        ));
+                }
+        }
+        if (ps.slot().bnodes().size() > 0) {
+                ps.setAll();
+                p.samplingInfo_ = ps.getSamplingInfo();
+        }
 #ifdef USE_SLOTPLANNER
         TSTART();
-        for (auto xy: jvi["slot"]["polygon"]) {
-                ps.slot().add_bnode(new RRTNode(
-                        xy[0].asFloat(),
-                        xy[1].asFloat()
-                ));
-        }
-        ps.setAll();
-        p.samplingInfo_ = ps.getSamplingInfo();
         if (ps.slot().bnodes().size() > 0)
                 ps.fipr(ps.getFP());
         TEND();

diff --git a/base/rrtbase.cc b/base/rrtbase.cc
index 1513b2bccf19a67e5bb3113f8d932cb2843882c2..ef0024a03305df14f3b6823d3abf3ad1d98f6582 100644 (file)
--- a/base/rrtbase.cc
+++ b/base/rrtbase.cc
@@ -873,38 +873,68 @@ std::vector<RRTNode *> RRTBase::findt(RRTNode *n)
 // RRT Framework
 RRTNode *RRTBase::sample()
 {
-#ifdef USE_SLOTPLANNER
-        float x = this->samplingInfo_.x;
-        float y = this->samplingInfo_.y;
-        std::normal_distribution<float> xdist(
-                0,
-                BCAR_WIDTH / 3
-        );
-        x += this->samplingInfo_.dx
-                * (BCAR_WIDTH / 2 + std::abs(xdist(this->gen_)))
-                * cos(this->samplingInfo_.sh + M_PI / 2);
-        y += this->samplingInfo_.dx
-                * (BCAR_WIDTH / 2 + std::abs(xdist(this->gen_)))
-                * sin(this->samplingInfo_.sh + M_PI / 2);
-        std::normal_distribution<float> ydist(
-                0,
-                BCAR_LENGTH / 3
-        );
-        x += ydist(this->gen_) * cos(this->samplingInfo_.sh);
-        y += ydist(this->gen_) * sin(this->samplingInfo_.sh);
-        std::normal_distribution<float> hdist(
-                0,
-                this->samplingInfo_.h / 3
-        );
-        float h = std::abs(hdist(this->gen_));
-        return new RRTNode(
-                x,
-                y,
-                this->samplingInfo_.sh + this->samplingInfo_.dh * h
-        );
-#else
-        return sa1();
-#endif
+        if (
+                this->samplingInfo_.dx &&
+                this->samplingInfo_.dy &&
+                this->samplingInfo_.dh
+        ) {
+                float x = this->samplingInfo_.x;
+                float y = this->samplingInfo_.y;
+                float h = 0;
+                std::normal_distribution<float> xdist(
+                        0,
+                        (this->samplingInfo_.r)
+                                ?this->samplingInfo_.r
+                                :BCAR_WIDTH * 2
+                );
+                std::normal_distribution<float> hdist(
+                        0,
+                        this->samplingInfo_.h
+                );
+                std::normal_distribution<float> ydist(
+                        0,
+                        (this->samplingInfo_.r)
+                                ?this->samplingInfo_.h / 2
+                                :BCAR_LENGTH / 3
+                );
+                if (!this->samplingInfo_.r) {
+                        float dx = BCAR_WIDTH / 2 +
+                                std::abs(xdist(this->gen_));
+                        x += dx * cos(
+                                this->samplingInfo_.sh +
+                                this->samplingInfo_.dx * M_PI / 2
+                        );
+                        y += dx * cos(
+                                this->samplingInfo_.sh +
+                                this->samplingInfo_.dx * M_PI / 2
+                        );
+                        float dy = ydist(this->gen_);
+                        x += dy * cos(this->samplingInfo_.sh);
+                        y += dy * sin(this->samplingInfo_.sh);
+                        h = std::abs(hdist(this->gen_));
+                } else {
+                        float dr = std::abs(xdist(this->gen_));
+                        float dh = hdist(this->gen_);
+                        x += dr * cos(
+                                this->samplingInfo_.sh +
+                                this->samplingInfo_.dh * M_PI / 2 +
+                                dh
+                        );
+                        y += dr * sin(
+                                this->samplingInfo_.sh +
+                                this->samplingInfo_.dh * M_PI / 2 +
+                                dh
+                        );
+                        h = std::abs(ydist(this->gen_));
+                }
+                return new RRTNode(
+                        x,
+                        y,
+                        this->samplingInfo_.sh + this->samplingInfo_.dh * h
+                );
+        } else {
+                return sa1();
+        }
 }
 
 float RRTBase::cost(RRTNode *init, RRTNode *goal)

diff --git a/base/rrtnode.cc b/base/rrtnode.cc
index cba4c5514abfe14aa51b3a43349cdc6665ae66f9..8127f3d4cfd9d9194985f2b3e366fa1873544ab7 100644 (file)
--- a/base/rrtnode.cc
+++ b/base/rrtnode.cc
@@ -71,7 +71,12 @@ float RRTNode::y() const
 
 float RRTNode::h() const
 {
-        return this->h_;
+        float h = this->h_;
+        while (h > M_PI)
+                h -= 2 * M_PI;
+        while (h <= -M_PI)
+                h += 2 * M_PI;
+        return h;
 }
 
 float RRTNode::t() const

diff --git a/decision_control/slotplanner.cc b/decision_control/slotplanner.cc
index ed1446de490c20a83eab26a9392b45d6ebe59f76..51219061dfbd8f9da58a8f9d0d8543c861b6ac4e 100644 (file)
--- a/decision_control/slotplanner.cc
+++ b/decision_control/slotplanner.cc
@@ -144,6 +144,11 @@ createcuspandfinish:
         std::swap(q, empty);
 }
 
+void ParallelSlot::fipr(RRTNode *n)
+{
+        return this->fipr(new BicycleCar(n->x(), n->y(), n->h()));
+}
+
 void ParallelSlot::fipr(BicycleCar *B)
 {
         std::vector<RRTNode *> cusp;
@@ -152,11 +157,36 @@ void ParallelSlot::fipr(BicycleCar *B)
         if (this->slotSide() == LEFT)
                 di = -1;
         if (this->slotType() == PERPENDICULAR) {
-                cusp.push_back(new RRTNode(
-                        B->x() - di * B->length(),
-                        B->y(),
-                        B->h()
-                ));
+                this->DH(di * 0.01 / B->out_radi()); // TODO car in slot h()
+                RRTNode *cc;
+                if (this->slotSide() == LEFT)
+                        cc = BicycleCar(B->x(), B->y(), B->h()).ccl();
+                else
+                        cc = BicycleCar(B->x(), B->y(), B->h()).ccr();
+                BicycleCar *p;
+                int i = 1;
+                p = B->move(cc, i * this->DH());
+                while (
+                        !this->slot().collide(p->frame())
+                        && this->slot().collide(p)
+                ) {
+                        delete p;
+                        i += 10;
+                        p = B->move(cc, i * this->DH());
+                }
+                i -= 10;
+                p = B->move(cc, i * this->DH());
+                while (
+                        !this->slot().collide(p->frame())
+                        && this->slot().collide(p)
+                ) {
+                        delete p;
+                        i += 1;
+                        p = B->move(cc, i * this->DH());
+                }
+                i -= 1;
+                p = B->move(cc, i * this->DH());
+                cusp.push_back(new RRTNode(p->x(), p->y(), p->h()));
                 std::reverse(cusp.begin(), cusp.end());
                 this->cusp().push_back(cusp);
                 return;
@@ -470,16 +500,16 @@ BicycleCar *ParallelSlot::getFP()
                                 * cos(h + M_PI);
                         ny = y + (BCAR_LENGTH + BCAR_WHEEL_BASE) / 2
                                 * sin(h + M_PI);
-                        x = nx + (BCAR_WIDTH + 0.01) * cos(h + M_PI / 2);
-                        y = ny + (BCAR_WIDTH + 0.01) * sin(h + M_PI / 2);
+                        x = nx + (BCAR_DIAG_RRADI) * cos(h + M_PI / 2);
+                        y = ny + (BCAR_DIAG_RRADI) * sin(h + M_PI / 2);
                 } else {
                         h += M_PI / 2;
                         nx = x + (BCAR_LENGTH + BCAR_WHEEL_BASE) / 2
                                 * cos(h - M_PI);
                         ny = y + (BCAR_LENGTH + BCAR_WHEEL_BASE) / 2
                                 * sin(h - M_PI);
-                        x = nx + (BCAR_WIDTH / 2 + 0.01) * cos(h - M_PI / 2);
-                        y = ny + (BCAR_WIDTH / 2 + 0.01) * sin(h - M_PI / 2);
+                        x = nx + (BCAR_DIAG_RRADI) * cos(h - M_PI / 2);
+                        y = ny + (BCAR_DIAG_RRADI) * sin(h - M_PI / 2);
                 }
         }
         return new BicycleCar(x, y, h);
@@ -511,6 +541,7 @@ bool ParallelSlot::isInside(BicycleCar *c)
 struct SamplingInfo ParallelSlot::getSamplingInfo()
 {
         struct SamplingInfo si;
+#ifdef USE_SLOTPLANNER
         BicycleCar *CC = this->getEPC();
         si.x = this->slot().bnodes()[0]->x();
         si.y = this->slot().bnodes()[0]->y();
@@ -523,7 +554,6 @@ struct SamplingInfo ParallelSlot::getSamplingInfo()
                 si.dy = -1;
                 si.dh = -1;
         }
-        si.r = CC->diag_radi();
         si.sh = this->slotHeading();
         if (this->slotType() == PARALLEL) {
                 si.h = this->slotHeading() - acos(EDIST(
@@ -531,7 +561,28 @@ struct SamplingInfo ParallelSlot::getSamplingInfo()
                         this->slot().bnodes()[1]
                 ) / BCAR_LENGTH);
         } else {
-                si.h = M_PI /2;
+                si.h = M_PI / 2 / 3;
+        }
+#else
+        si.x = this->slot().bnodes()[3]->x() - this->slot().bnodes()[0]->x();
+        si.x /= 2;
+        si.x += this->slot().bnodes()[0]->x();
+        si.y = this->slot().bnodes()[3]->y() - this->slot().bnodes()[0]->y();
+        si.y /= 2;
+        si.y += this->slot().bnodes()[0]->y();
+        if (this->slotSide() == RIGHT) {
+                si.dx = 1;
+                si.dy = 1;
+                si.dh = 1;
+        } else {
+                si.dx = -1;
+                si.dy = -1;
+                si.dh = -1;
         }
+        si.r = EDIST(this->slot().bnodes()[0], this->slot().bnodes()[3]) / 2;
+        si.r *= 2;
+        si.sh = this->slotHeading();
+        si.h = M_PI / 4;
+#endif
         return si;
 }

diff --git a/incl/bcar.h b/incl/bcar.h
index afac7c7b49d5379f8e1ba7e7113dba2d53bcb012..c09c65239749b00179accc3eab199493559e134c 100644 (file)
--- a/incl/bcar.h
+++ b/incl/bcar.h
@@ -27,6 +27,13 @@ along with I am car. If not, see <http://www.gnu.org/licenses/>.
 #define BCAR_TURNING_RADIUS 10.820
 #define BCAR_WHEEL_BASE 2.450
 #define BCAR_WIDTH 1.625
+#define BCAR_DIST_REAR ((BCAR_LENGTH - BCAR_WHEEL_BASE) / 2)
+#define BCAR_DIST_FRONT (BCAR_LENGTH - BCAR_DIST_REAR)
+#define BCAR_DIAG_RADI pow(pow(BCAR_DIST_FRONT, 2) + pow(BCAR_WIDTH/2, 2), 0.5)
+#define BCAR_DIAG_RRADI pow(pow(BCAR_DIST_REAR, 2) + pow(BCAR_WIDTH/2, 2), 0.5)
+#define BCAR_OUT_RADI pow( \
+        pow(BCAR_TURNING_RADIUS + BCAR_WIDTH/2, 2) + \
+        pow(BCAR_DIST_FRONT, 2), 0.5)
 
 #define MAXSTEER(wb, tr) ((wb) / (tr))
 
@@ -92,6 +99,8 @@ class BicycleCar: public RRTNode {
 
                 /** Return distance from wheelbase center to top corner */
                 float diag_radi();
+                /** Return distance from wheelbase center to rear corner */
+                float diag_rradi();
                 /** Outer radius of the farthest point */
                 float out_radi();
                 /** Angle between wheelbase line and outer radius */

diff --git a/incl/slotplanner.h b/incl/slotplanner.h
index 08246ecc229cdd836cf74a5e31cdf4b067d4bc56..f48e2a264a2573d5a86d49f4f19b06d090c0ec9d 100644 (file)
--- a/incl/slotplanner.h
+++ b/incl/slotplanner.h
@@ -73,6 +73,7 @@ class ParallelSlot {
 
                 @param B Last pose of vehicle when it is parked.
                 */
+                void fipr(RRTNode *n);
                 void fipr(BicycleCar *B);
                 /** _Find Last Not Colliding for Reverse_ BicycleCar pose
 

diff --git a/vehicle_platform/bcar.cc b/vehicle_platform/bcar.cc
index 3972c96e4c462df490ba9f3baae791ccc9bb0d0c..f1580a184597d64f2d2ad3bf4764b17fb4c3cd55 100644 (file)
--- a/vehicle_platform/bcar.cc
+++ b/vehicle_platform/bcar.cc
@@ -254,6 +254,13 @@ float BicycleCar::diag_radi()
         return pow(xx + yy, 0.5);
 }
 
+float BicycleCar::diag_rradi()
+{
+        float xx = pow(this->dr(), 2);
+        float yy = pow(this->width_ / 2, 2);
+        return pow(xx + yy, 0.5);
+}
+
 float BicycleCar::out_radi()
 {
         return pow((pow(this->turning_radius_ + this->width_ / 2, 2) +