Add plot steered1/2

diff --git a/scripts/plot_json_objects_scenario.py b/scripts/plot_json_objects_scenario.py
index 651cb8d73988720941d33945c4b70601c48cbe6f..56e2fc5d1ecbb5aacdb8be545006010814b2d830 100644 (file)
--- a/scripts/plot_json_objects_scenario.py
+++ b/scripts/plot_json_objects_scenario.py
@@ -174,7 +174,7 @@ if __name__ == "__main__":
                 if n[1] < MINY:
                     MINY = n[1]
 
-    # Plot all the nodes (if exists), obstacles, and slot.
+    # Plot all the nodes (if exists.)
     if "nodes_x" in scenario and "nodes_y" in scenario:
         plt.plot(
             [x - MINX for x in scenario["nodes_x"]],
@@ -184,6 +184,27 @@ if __name__ == "__main__":
             ms=2,
             lw=0,
         )
+    # Plot all the steered2 nodes (if exists.)
+    if "steered2_x" in scenario and "steered2_y" in scenario:
+        plt.plot(
+            [x - MINX for x in scenario["steered2_x"]],
+            [y - MINY for y in scenario["steered2_y"]],
+            color="orange",
+            marker="o",
+            ms=2,
+            lw=0,
+        )
+    # Plot all the steered1 nodes (if exists.)
+    if "steered1_x" in scenario and "steered1_y" in scenario:
+        plt.plot(
+            [x - MINX for x in scenario["steered1_x"]],
+            [y - MINY for y in scenario["steered1_y"]],
+            color="blue",
+            marker="o",
+            ms=2,
+            lw=0,
+        )
+    # Plot obstacles, slot.
     if "obst" in scenario and len(scenario["obst"]) > 0:
         for o in scenario["obst"]:
             if not o: