Store and plot "parent is cusp" boolean

diff --git a/rrts/src/rrts.cc b/rrts/src/rrts.cc
index da4ae36f6fcbb6a78164113a2f0f82f0ac27a7c8..d6f8f002c2a750a8f17527bac7f944fa93e8b46e 100644 (file)
--- a/rrts/src/rrts.cc
+++ b/rrts/src/rrts.cc
@@ -435,6 +435,7 @@ RRTS::json(void) const
                        jvo["paths"][j][i][2] = n.h();
                        jvo["paths"][j][i][3] = n.sp();
                        jvo["paths"][j][i][4] = n.st();
+                       jvo["paths"][j][i][5] = n.p_is_cusp();
                        i++;
                }
                jvo["costs"][j] = path.back().cc();
@@ -447,11 +448,13 @@ RRTS::json(void) const
                jvo["paths"][j][i][2] = n->h();
                jvo["paths"][j][i][3] = n->sp();
                jvo["paths"][j][i][4] = n->st();
+               jvo["paths"][j][i][5] = n->p_is_cusp();
                jvo["path"][i][0] = n->x();
                jvo["path"][i][1] = n->y();
                jvo["path"][i][2] = n->h();
                jvo["path"][i][3] = n->sp();
                jvo["path"][i][4] = n->st();
+               jvo["path"][i][5] = n->p_is_cusp();
                i++;
        }
        jvo["costs"][j] = this->_path.back()->cc();

diff --git a/scripts/plot_scenario.py b/scripts/plot_scenario.py
index 0d10501ae2680a7fa01d2169e2b2924000b1b221..4d8761ef654397215eaab79c5b17b0cf19e22501 100755 (executable)
--- a/scripts/plot_scenario.py
+++ b/scripts/plot_scenario.py
@@ -738,7 +738,10 @@ if __name__ == "__main__":
                 else:
                     plt.plot(p[0] - MINX, p[1] - MINY, color="blue", marker=".")
             else:
-                plt.plot(p[0] - MINX, p[1] - MINY, color="blue", marker="+")
+                if len(p) > 5 and p[5]:  # parent is cusp
+                    plt.plot(p[0] - MINX, p[1] - MINY, color="blue", marker="o")
+                else:
+                    plt.plot(p[0] - MINX, p[1] - MINY, color="blue", marker="+")
             #plt.plot(*plot_car(p), color="blue")
             pass
             #cc = plot_car_corners(p)