From: Jiri Vlasak <jiri.vlasak.2@cvut.cz>
Date: Mon, 30 Aug 2021 11:00:42 +0000 (+0200)
Subject: Add drivable to forward dijkstra algorithm
X-Git-Tag: v0.9.0~5^2~1
X-Git-Url: https://rtime.felk.cvut.cz/gitweb/hubacji1/rrts.git/commitdiff_plain/d1ca17761644e177599781621b219ff7b509ec65

Add drivable to forward dijkstra algorithm
---

diff --git a/src/rrtext13.cc b/src/rrtext13.cc
index 5be9888..dada6a3 100644
--- 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) {