Output final path

diff --git a/src/test7.cc b/src/test7.cc
index c544a356a4bc7e0521ffe9707d4ddd004e8532c1..30865327d9adf41d0b5f069ec49d10aafd077405 100644 (file)
--- a/src/test7.cc
+++ b/src/test7.cc
@@ -208,6 +208,17 @@ for (int i = 1; i < 5; i++) {
         std::cerr << std::endl;
         jvo[rcnt++] = rrts2.json();
 }
+        {
+                jvo[rcnt]["obst"] = jvo[rcnt - 1]["obst"];
+                jvo[rcnt]["cost"] = final_path_cost;
+                unsigned int ncnt = 0;
+                for (auto n: final_path) {
+                        jvo[rcnt]["path"][ncnt][0] = n.x();
+                        jvo[rcnt]["path"][ncnt][1] = n.y();
+                        jvo[rcnt]["path"][ncnt][2] = n.h();
+                        ncnt++;
+                }
+        }
         std::cout << jvo << std::endl;
         return 0;
 }