Add drivable to forward dijkstra algorithm

diff --git a/src/rrtext13.cc b/src/rrtext13.cc
index 5be98881726a428952e24f8f00312102ad546597..dada6a375ea1416c1c26f8a6d251af31ce22b518 100644 (file)
--- a/src/rrtext13.cc
+++ b/src/rrtext13.cc
@@ -113,8 +113,12 @@ RRTExt13::dijkstra_forward()
                DijkstraNode fd = pq.top();
                RRTNode& f = *fd.node;
                pq.pop();
+               this->bc_.set_pose(f);
                for (unsigned int i = fd.i + 1; i < this->dn_.size(); i++) {
                        RRTNode& t = *this->dn_[i].node;
+                       if (!this->bc_.drivable(t)) {
+                               continue;
+                       }
                        double cost = f.cc() + this->cost_build(f, t);
                        this->steer(f, t);
                        if (this->steered_.size() == 0) {