Plot cusps

diff --git a/base/main.cc b/base/main.cc
index 1bcd55839b972c1b186f02dc450aaf002aa86c4f..b0e559368ee5ca9975db9d69e617b6af5d53e049 100644 (file)
--- a/base/main.cc
+++ b/base/main.cc
@@ -176,6 +176,8 @@ int main()
                         jvo["traj"][j][i][0] = n->x();
                         jvo["traj"][j][i][1] = n->y();
                         jvo["traj"][j][i][2] = n->h();
+                        jvo["traj"][j][i][3] = n->t();
+                        jvo["traj"][j][i][4] = n->s();
                         i++;
                 }
                 j++;

diff --git a/base/rrtbase.cc b/base/rrtbase.cc
index c17bc467cbca3e60ce687ad51d5fea2fc2a5f585..5c5d4d9084516fb837a57831e617d429dbba08a6 100644 (file)
--- a/base/rrtbase.cc
+++ b/base/rrtbase.cc
@@ -262,6 +262,7 @@ bool RRTBase::glplot()
                 }
         }
         glEnd();
+        std::vector<RRTNode *> cusps;
         // Plot last trajectory
         if (this->tlog().size() > 0) {
                 glLineWidth(2);
@@ -271,10 +272,20 @@ bool RRTBase::glplot()
                                 glColor3f(0, 0, 1);
                                 glVertex2f(GLVERTEX(n));
                                 glVertex2f(GLVERTEX(n->parent()));
+                                if (sgn(n->s()) != sgn(n->parent()->s()))
+                                        cusps.push_back(n);
                         }
                 }
                 glEnd();
         }
+        // Plot cusps
+        glPointSize(8);
+        glBegin(GL_POINTS);
+        for (auto n: cusps) {
+                glColor3f(0, 0, 1);
+                glVertex2f(GLVERTEX(n));
+        }
+        glEnd();
         SDL_GL_SwapWindow(gw);
         for (auto n: r)
                 n->visit(false);

diff --git a/plot.py b/plot.py
index 05aa9c33e583813e8cae802e5aca8da0bbc652c9..44ba684c4f48c2a71b3516a9ad29ba7105878cda 100644 (file)
--- a/plot.py
+++ b/plot.py
@@ -1,10 +1,12 @@
 # -*- coding: utf-8 -*-
 """This scipt loads scenario and result trajectory from files and plots it."""
 from json import loads
-from math import cos, pi, sin
+from math import copysign, cos, pi, sin
 from matplotlib import pyplot as plt
 from sys import argv
 
+sign = lambda x: copysign(1, x)
+
 HEIGHT = 1.418
 LENGTH = 4.970
 SAFETY_DIST = 0
@@ -195,6 +197,11 @@ if __name__ == "__main__":
                                 label=t["cost"][traj])
         except:
             print("No trajectory")
+        for i in range(len(t["traj"][-1]) - 1):
+            n1 = t["traj"][-1][i]
+            n2 = t["traj"][-1][i + 1]
+            if (sign(n1[4]) != sign(n2[4])):
+                plt.plot(n2[0], n2[1], color=COLOR["log"][1], marker=".")
     plt.plot(*plot_nodes([s["init"]]), color=COLOR["start"], marker=".")
     plt.plot(*plot_nodes([s["goal"]]), color=COLOR["goal"], marker=".")
     # end plot here