Add collide with car frame procedure

diff --git a/base/rrtbase.cc b/base/rrtbase.cc
index 4aafe7f388702b6e6f593fef08165b8e4e303b80..7d6f8038550e53a261d9a7b6c44c82440528f02b 100644 (file)
--- a/base/rrtbase.cc
+++ b/base/rrtbase.cc
@@ -358,7 +358,11 @@ bool RRTBase::collide(RRTNode *init, RRTNode *goal)
                         if ((*this->cobstacles_)[i].collide(tmp)) {
                                 col = true;
                         }
-                        // TODO collide with car frame
+                        for (auto &e: bcframe) {
+                                if ((*this->cobstacles_)[i].collide(e)) {
+                                        col = true;
+                                }
+                        }
                 }
                 if (col) {
                         for (auto e: bcframe) {