Plot root/goal

diff --git a/base/rrtbase.cc b/base/rrtbase.cc
index df76683778f4dfe80add0c4efaae925595b24595..865eef4e052ff3c7c53e35cbcf90289fdd3eb617 100644 (file)
--- a/base/rrtbase.cc
+++ b/base/rrtbase.cc
@@ -227,6 +227,13 @@ bool RRTBase::glplot()
                 glVertex2f(GLVERTEX(o.goal()));
         }
         glEnd();
+        // Plot root, goal
+        glPointSize(4);
+        glBegin(GL_POINTS);
+        glColor3f(1, 0, 0);
+        glVertex2f(GLVERTEX(this->root_));
+        glVertex2f(GLVERTEX(this->goal_));
+        glEnd();
         // Plot nodes
         std::vector<RRTNode *> s; // DFS stack
         std::vector<RRTNode *> r; // reset visited_