From d37b1c6cfef42675628fce21a2e3e11b2b6628fd Mon Sep 17 00:00:00 2001
From: Jiri Vlasak <jiri.vlasak.2@cvut.cz>
Date: Tue, 24 Aug 2021 09:48:37 +0200
Subject: [PATCH] Use macros to comment out the code

---
 src/rrtext13.cc | 27 +++++++++++++++++++--------
 1 file changed, 19 insertions(+), 8 deletions(-)

diff --git a/src/rrtext13.cc b/src/rrtext13.cc
index 03ff714..8c5a451 100644
--- a/src/rrtext13.cc
+++ b/src/rrtext13.cc
@@ -1,7 +1,5 @@
 #include <queue>
 #include "rrtext.hh"
-#include <iostream>
-using namespace std;
 
 namespace rrts {
 
@@ -31,13 +29,16 @@ RRTExt13::pick_interesting()
 {
 	this->dn_.clear();
 	this->dn_.reserve(this->path_.size());
-	//for (auto n: this->opath_) {
-	//	this->dn_.push_back(DijkstraNode(n));
-	//	this->dn_.back().i = this->dn_.size() - 1;
-	//}
-	//return;
+#if 0 // all path poses are interesting
+	for (auto n: this->opath_) {
+		this->dn_.push_back(DijkstraNode(n));
+		this->dn_.back().i = this->dn_.size() - 1;
+	}
+	return;
+#endif
+#if 1 // cusp and 1st non-drivable path poses are interesting
 	for (unsigned int i = 0; i < this->opath_.size() - 1; i++) {
-	//unsigned int i = 0; {
+	//unsigned int i = 0; { // just for cusp, comment out drivable section
 		RRTNode& ni = *this->opath_[i];
 		this->bc_.set_pose(ni);
 		for (unsigned int j = i + 1; j < this->opath_.size(); j++) {
@@ -58,6 +59,7 @@ RRTExt13::pick_interesting()
 	}
 	this->dn_.push_back(DijkstraNode(this->opath_.back()));
 	this->dn_.back().i = this->dn_.size() - 1;
+#endif
 }
 
 void
@@ -136,6 +138,12 @@ void
 RRTExt13::compute_path()
 {
 	RRTS::compute_path();
+#if 0 // TODO 0.59 should work for sc4-1-0 only.
+	if (this->goal_.cc() * 0.59 > this->last_goal_cc_
+			&& this->last_goal_cc_ != 0.0) {
+		return;
+	}
+#endif
 	bool measure_time = false;
 	if (this->opath_.size() == 0) {
 		this->opath_ = this->path_;
@@ -147,8 +155,11 @@ RRTExt13::compute_path()
 	}
 	this->pick_interesting();
 	this->dijkstra_forward();
+#if 0 // TODO Fix as the code does not always finish.
+	RRTS::compute_path();
 	this->pick_interesting();
 	this->dijkstra_backward();
+#endif
 	RRTS::compute_path();
 	if (measure_time) {
 		this->otime_ += this->ter_.scnt();
-- 
2.39.2