Fix link obstacles for T3

diff --git a/decision_control/rrtplanner.cc b/decision_control/rrtplanner.cc
index 36c90f13e6f3ebc55df4d7068ee3668a572014eb..6ad2edfa83d6d5f62d8c3104959f8b17186a6ca1 100644 (file)
--- a/decision_control/rrtplanner.cc
+++ b/decision_control/rrtplanner.cc
@@ -600,3 +600,14 @@ bool T3::next()
 {
         return true;
 }
+
+bool T3::link_obstacles(
+        std::vector<CircleObstacle> *cobstacles,
+        std::vector<SegmentObstacle> *sobstacles)
+{
+        bool ret = false;
+        ret = RRTBase::link_obstacles(cobstacles, sobstacles);
+        ret &= this->p_root_.link_obstacles(cobstacles, sobstacles);
+        ret &= this->p_goal_.link_obstacles(cobstacles, sobstacles);
+        return ret;
+}

diff --git a/incl/rrtplanner.h b/incl/rrtplanner.h
index d80ce0dde32790eea81af1499007535ecdf485cf..a34705043c2105df8f15a90343c0c256a5e4f5ff 100644 (file)
--- a/incl/rrtplanner.h
+++ b/incl/rrtplanner.h
@@ -148,6 +148,9 @@ class T3: public RRTBase {
         public:
                 T3(RRTNode *init, RRTNode *goal);
                 bool next();
+                bool link_obstacles(
+                                std::vector<CircleObstacle> *cobstacles,
+                                std::vector<SegmentObstacle> *sobstacles);
 };
 
 #endif