Use macros to comment out the code

diff --git a/src/rrtext13.cc b/src/rrtext13.cc
index 03ff7148425b03e247f9bc96a7218ce9ee8726c1..8c5a45108df68589fcc406befe52416ef57a0fa7 100644 (file)
--- 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();