Add bicycle car model, fix collision check

diff --git a/CHANGELOG.md b/CHANGELOG.md
index 03ebb97561ec1444b1346c8ddcb773c4e4a5c672..74584861290691ac838b64e450ae040407423788 100644 (file)
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -21,3 +21,4 @@ The format is based on [Keep a Changelog][] and this project adheres to
 - Main program (input/output) with cmake file.
 - Obstacles class (circle obstacle, segment obstacle).
 - Collision check function to base RRT class.
+- Bicycle car model used for collision check with car frame.

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 592884c449b976c1c83cab27055beb9f6a4af84f..a055b10623a703c679e8c1e053a6f47d9c4c3f98 100644 (file)
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -16,6 +16,7 @@ add_executable(go_car_go
 
         perception/obstacle.cc
 
+        vehicle_platform/bcar.cc
         vehicle_platform/cost.cc
         vehicle_platform/steer.cc
 )

diff --git a/base/rrtbase.cc b/base/rrtbase.cc
index 9233a64a371869ebeefde115b953bc736f138f5c..0092db3caf257570f6f779f254fafca0e615ca9b 100644 (file)
--- a/base/rrtbase.cc
+++ b/base/rrtbase.cc
@@ -16,6 +16,7 @@ along with I am car. If not, see <http://www.gnu.org/licenses/>.
 */
 
 #include <cmath>
+#include "bcar.h"
 #include "rrtbase.h"
 
 RRTBase::RRTBase():
@@ -121,20 +122,20 @@ bool RRTBase::collide(RRTNode *init, RRTNode *goal)
         std::vector<RRTEdge *> edges;
         RRTNode *tmp = goal;
         while (tmp != init) {
-                //BicycleCar bc(tmp->x(), tmp->y(), tmp->h());
+                BicycleCar bc(tmp->x(), tmp->y(), tmp->h());
                 for (auto &o: *this->cobstacles_) {
                         if (o.collide(tmp)) {
                                 return true;
                         }
                         // TODO collide with car frame
                 }
-                //for (auto &o: *this->sobstacles_) {
-                //        for (auto &e: bc.frame()) {
-                //                if (o.collide(e)) {
-                //                        return true;
-                //                }
-                //        }
-                //}
+                for (auto &o: *this->sobstacles_) {
+                        for (auto &e: bc.frame()) {
+                                if (o.collide(e)) {
+                                        return true;
+                                }
+                        }
+                }
                 if (!tmp->parent()) {
                         break;
                 }

diff --git a/incl/bcar.h b/incl/bcar.h
new file mode 100644 (file)
index 0000000..8a9947e
--- /dev/null
+++ b/incl/bcar.h
@@ -0,0 +1,38 @@
+/*
+This file is part of I am car.
+
+I am car is free software: you can redistribute it and/or modify
+it under the terms of the GNU General Public License as published by
+the Free Software Foundation, either version 3 of the License, or
+(at your option) any later version.
+
+I am car is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+GNU General Public License for more details.
+
+You should have received a copy of the GNU General Public License
+along with I am car. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#ifndef BICYCLECAR_H
+#define BICYCLECAR_H
+
+#include <vector>
+#include "rrtnode.h"
+
+class BicycleCar: public RRTNode {
+        private:
+                // see https://en.wikipedia.org/wiki/Fiat_Punto
+                float height_ = 1.450;
+                float length_ = 3.760;
+                float safety_dist_ = 0;
+                float wheel_base_ = 2.450;
+                float width_ = 1.625;
+        public:
+                using RRTNode::RRTNode;
+
+                std::vector<RRTEdge *> frame();
+};
+
+#endif

diff --git a/vehicle_platform/bcar.cc b/vehicle_platform/bcar.cc
new file mode 100644 (file)
index 0000000..e57e5e9
--- /dev/null
+++ b/vehicle_platform/bcar.cc
@@ -0,0 +1,80 @@
+/*
+This file is part of I am car.
+
+I am car is free software: you can redistribute it and/or modify
+it under the terms of the GNU General Public License as published by
+the Free Software Foundation, either version 3 of the License, or
+(at your option) any later version.
+
+I am car is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+GNU General Public License for more details.
+
+You should have received a copy of the GNU General Public License
+along with I am car. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#include <cmath>
+#include "bcar.h"
+
+std::vector<RRTEdge *> BicycleCar::frame()
+{
+        std::vector<RRTEdge *> frame;
+        float dr = (this->length_ - this->wheel_base_) / 2;
+        float df = this->length_ - dr;
+
+       float lfx = this->x();
+        lfx += (this->width_ / 2) * cos(this->h() + M_PI / 2);
+        lfx += df * cos(this->h());
+        lfx += this->safety_dist_ * cos(this->h());
+
+        float lrx = this->x();
+        lrx += (this->width_ / 2) * cos(this->h() + M_PI / 2);
+        lrx += -dr * cos(this->h());
+        lrx += -this->safety_dist_ * cos(this->h());
+
+        float rrx = this->x();
+        rrx += (this->width_ / 2) * cos(this->h() - M_PI / 2);
+        rrx += -dr * cos(this->h());
+        rrx += -this->safety_dist_ * cos(this->h());
+
+        float rfx = this->x();
+        rfx += (this->width_ / 2) * cos(this->h() - M_PI / 2);
+        rfx += df * cos(this->h());
+        rfx += this->safety_dist_ * cos(this->h());
+
+        float lfy = this->y();
+        lfy += (this->width_ / 2) * sin(this->h() + M_PI / 2);
+        lfy += df * sin(this->h());
+        lfy += this->safety_dist_ * sin(this->h());
+
+        float lry = this->y();
+        lry += (this->width_ / 2) * sin(this->h() + M_PI / 2);
+        lry += -dr * sin(this->h());
+        lry += -this->safety_dist_ * sin(this->h());
+
+        float rry = this->y();
+        rry += (this->width_ / 2) * sin(this->h() - M_PI / 2);
+        rry += -dr * sin(this->h());
+        rry += -this->safety_dist_ * sin(this->h());
+
+        float rfy = this->y();
+        rfy += (this->width_ / 2) * sin(this->h() - M_PI / 2);
+        rfy += df * sin(this->h());
+        rfy += this->safety_dist_ * sin(this->h());
+
+       frame.push_back(new RRTEdge(
+                                new RRTNode(lfx, lfy),
+                                new RRTNode(lrx, lry)));
+       frame.push_back(new RRTEdge(
+                                new RRTNode(lrx, lry),
+                                new RRTNode(rrx, rry)));
+       frame.push_back(new RRTEdge(
+                                new RRTNode(rrx, rry),
+                                new RRTNode(rfx, rfy)));
+       frame.push_back(new RRTEdge(
+                                new RRTNode(rfx, rfy),
+                                new RRTNode(lfx, rfy)));
+       return frame;
+}