Store all paths found and their costs

diff --git a/rrts/incl/rrts.hh b/rrts/incl/rrts.hh
index 6e6574b2fe6350d822e54fcaddb3cf2da5bc3d95..97d03bbbed51a81170cdeea73b537ad18c4bf238 100644 (file)
--- a/rrts/incl/rrts.hh
+++ b/rrts/incl/rrts.hh
@@ -97,7 +97,7 @@ protected:
        double _cost = 0.0;
        double _eta = 0.5;
        double _time = 0.0;
-       std::vector<RRTNode> _last_path;
+       std::vector<std::vector<RRTNode>> _logged_paths;
 protected:
        double min_gamma_eta(void) const;
        bool should_continue(void) const;

diff --git a/rrts/src/rrts.cc b/rrts/src/rrts.cc
index 289a7713af58be298d62f4470f1a958acfccf8e2..4c016b2f1620c5bd6b653fb58029e8f09bb1d7c0 100644 (file)
--- a/rrts/src/rrts.cc
+++ b/rrts/src/rrts.cc
@@ -354,11 +354,11 @@ RRTS::path_cost() const
 double
 RRTS::last_path_cost(void) const
 {
-       if (this->_last_path.size() == 0) {
-               return 999.9;
-       } else {
-               return this->_last_path.back().cc();
+       if (this->_logged_paths.size() == 0) {
+               return 0.0;
        }
+       assert(this->_logged_paths.back().size() > 0);
+       return this->_logged_paths.back().back().cc();
 }
 
 double
@@ -377,8 +377,27 @@ Json::Value
 RRTS::json() const
 {
        Json::Value jvo;
-       unsigned int i = 0;
-       for (auto n: this->_path) { // TODO because path() has just x, y, h
+       unsigned int i = 0, j = 0;
+       for (auto path: this->_logged_paths) {
+               i = 0;
+               for (auto n: path) {
+                       jvo["paths"][j][i][0] = n.x();
+                       jvo["paths"][j][i][1] = n.y();
+                       jvo["paths"][j][i][2] = n.h();
+                       jvo["paths"][j][i][3] = n.sp();
+                       jvo["paths"][j][i][4] = n.st();
+                       i++;
+               }
+               jvo["costs"][j] = path.back().cc();
+               j++;
+       }
+       i = 0;
+       for (auto n: this->_path) {
+               jvo["paths"][j][i][0] = n->x();
+               jvo["paths"][j][i][1] = n->y();
+               jvo["paths"][j][i][2] = n->h();
+               jvo["paths"][j][i][3] = n->sp();
+               jvo["paths"][j][i][4] = n->st();
                jvo["path"][i][0] = n->x();
                jvo["path"][i][1] = n->y();
                jvo["path"][i][2] = n->h();
@@ -386,7 +405,10 @@ RRTS::json() const
                jvo["path"][i][4] = n->st();
                i++;
        }
-       jvo["goal_cc"] = this->_goal.cc();
+       jvo["costs"][j] = this->_path.back()->cc();
+       j++;
+       jvo["goal_cc"] = this->_goal.cc(); // TODO remove, use the following
+       jvo["cost"] = this->path_cost();
        jvo["time"] = this->scnt();
        return jvo;
 }
@@ -427,7 +449,8 @@ RRTS::next()
        double d1 = this->cost_search(this->_nodes.front(), rs);
        double d2 = this->cost_search(rs, this->_goal);
        if (this->last_path_cost() != 0.0 && d1 + d2 > this->last_path_cost()) {
-               rs = this->_last_path[rand() % this->_last_path.size()];
+               auto& last_path = this->_logged_paths.back();
+               rs = last_path[rand() % last_path.size()];
        }
 }
 #endif
@@ -487,11 +510,25 @@ RRTS::reset()
 {
        if (this->path_cost() != 0.0
                        && this->path_cost() < this->last_path_cost()) {
-               this->_last_path.clear();
+               this->_logged_paths.push_back(std::vector<RRTNode>());
+               auto& last_path = this->_logged_paths.back();
+               last_path.reserve(this->_path.size());
+               RRTNode* p = nullptr;
                for (auto n: this->_path) {
-                       this->_last_path.push_back(*n);
-                       // FIXME _last_path nodes' pointers to parents
+                       last_path.push_back(*n);
+                       if (p != nullptr) {
+                               last_path.back().p(*p);
+                       }
+                       p = &last_path.back();
+               }
+               // Test that last path cost matches.
+               auto last_path_cost = last_path.back().cc();
+               for (unsigned int i = 1; i < last_path.size(); i++) {
+                       last_path[i].c(this->cost_build(
+                               last_path[i - 1],
+                               last_path[i]));
                }
+               assert(last_path_cost == last_path.back().cc());
        }
        this->_goal = RRTGoal(
                this->_goal.x(),