Remove optimize part procedure

diff --git a/CHANGELOG.md b/CHANGELOG.md
index d2392768a8f5b628dfceeb3b18f49f2584884005..5cfb4c8266eea79eec3f38dc2f9331db097778c0 100644 (file)
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -16,6 +16,9 @@ The format is based on [Keep a Changelog][] and this project adheres to
 - Test script.
 - `nn4` use Euclidean distance while independently on RRT framework cost used.
 
+### Removed
+- `opt_part` procedure (unused).
+
 ### Fixed
 - Dijkstra path optimization only if goal found.
 - Workaround used for optimizing part of path. There is a bug that causes

diff --git a/decision_control/rrtplanner.cc b/decision_control/rrtplanner.cc
index 0cecd74ebfa698062aad8bd1f0e404e868a98516..16cad429f2a8e8a1693edb228280dc619a313e6d 100644 (file)
--- a/decision_control/rrtplanner.cc
+++ b/decision_control/rrtplanner.cc
@@ -587,29 +587,3 @@ float T2::goal_cost()
         }
         return this->goal()->ccost();
 }
-
-bool T2::opt_part(RRTNode *init, RRTNode *goal)
-{
-        std::vector<RRTNode *> steered;
-        steered = this->steer(init, goal);
-        unsigned int ss = steered.size();
-        for (unsigned int i = 0; i < ss - 1; i++) {
-                steered[i]->add_child(
-                                steered[i + 1],
-                                this->cost(
-                                        steered[0], // FIXME
-                                        steered[ss - 1])); // FIXME
-        }
-        if (this->collide(steered[0], steered[ss - 1])) {
-                for (auto n: steered)
-                        delete n;
-                return false;
-        }
-        RRTNode *op;
-        op = init->parent();
-        if (!op)
-                op = init;
-        op->add_child(steered[0], this->cost(op, steered[0]));
-        steered[ss - 1]->add_child(goal, this->cost(steered[ss - 1], goal));
-        return true;
-}

diff --git a/incl/rrtplanner.h b/incl/rrtplanner.h
index cd40f8591b973496d5820ccdc8352f0fc306a1ef..13c0747e38a958368d3a5b765af60f39d346416c 100644 (file)
--- a/incl/rrtplanner.h
+++ b/incl/rrtplanner.h
@@ -139,7 +139,6 @@ class T2: public Karaman2011 {
 
                 bool next();
                 float goal_cost();
-                bool opt_part(RRTNode *init, RRTNode *goal);
 };
 
 #endif