Add drivable method to bcar

[hubacji1/iamcar.git] / vehicle_platform / bcar.cc

diff --git a/vehicle_platform/bcar.cc b/vehicle_platform/bcar.cc
index 57ebee0d2da5c667b8e12d6c1e0dd0e32fe1bcc2..bb34e4ceb201ce7d9e0acfddf5be1277b88b2dcd 100644 (file)
--- a/vehicle_platform/bcar.cc
+++ b/vehicle_platform/bcar.cc
@@ -230,3 +230,17 @@ float BicycleCar::alfa()
         return asin((this->length_ + this->wheel_base_) /
                         (2 * this->out_radi()));
 }
+
+bool BicycleCar::drivable(RRTNode *n)
+{
+        bool drivable = true;
+        RRTNode *ccl = this->ccl();
+        RRTNode *ccr = this->ccr();
+        if (pow(ccl->x() - n->x(), 2) + pow(ccl->y() - n->y(), 2) <=
+                        pow(this->turning_radius_, 2))
+                drivable = false;
+        if (pow(ccr->x() - n->x(), 2) + pow(ccr->y() - n->y(), 2) <=
+                        pow(this->turning_radius_, 2))
+                drivable = false;
+        return drivable;
+}