From eb9bbbb25dd606daa7bc10a87f783169625948cd Mon Sep 17 00:00:00 2001
From: pmusau17 <pmusau13ster@gmail.com>
Date: Mon, 17 May 2021 13:38:49 -0500
Subject: [PATCH] ok it works sort of

---
 kinodynamic_rrt/kinodynamic_rrt.py | 41 ++++++++++++++++++++++++------
 kinodynamic_rrt/plotting.py        | 16 ++++++++++--
 2 files changed, 47 insertions(+), 10 deletions(-)

diff --git a/kinodynamic_rrt/kinodynamic_rrt.py b/kinodynamic_rrt/kinodynamic_rrt.py
index 4976fe8..c7459e9 100644
--- a/kinodynamic_rrt/kinodynamic_rrt.py
+++ b/kinodynamic_rrt/kinodynamic_rrt.py
@@ -264,7 +264,7 @@ def extract_path(self, node_end):
         newest node.
 
     """
-    def search_goal_parent(self):
+    def search_goal_parent(self,all_paths=False):
         # create a list of distances to the goal
         # random sampling allows you to add a node to the tree more than once (need to figure out how to prevent that)
         # well we will only consider 
@@ -294,13 +294,16 @@ def search_goal_parent(self):
             # get the dist
             dist = math.hypot(node_new.x - self.s_goal.x, node_new.y - self.s_goal.y)
             dists.append(dist)
-        index = np.asarray(dists).argmin()
-        node = candidates[index]
-
-        if(len(candidates)>0):
+        print(dists)
+        
+        
+        path =[]
+        if(len(candidates)>0 and not all_paths):
+            index = np.asarray(dists).argmin()
+            node = candidates[index]
             path = self.extract_path(node)
         
-        return path
+        return path,candidates
 
 
     """
@@ -309,7 +312,7 @@ def search_goal_parent(self):
     def plot_final(self):
         
         # get the index of the closest point to the goal
-        self.path = self.search_goal_parent()
+        self.path,_ = self.search_goal_parent()
 
 
         # get the path
@@ -318,6 +321,26 @@ def plot_final(self):
         # get the index of the minimum cost vertex within a step length of the goal 
         self.plotting.animation(self.list_of_vertices, self.path, "Porto Final Solution, N={}".format(self.iter_max),True,True)
 
+    """ 
+        Function that plots each of the paths
+    """
+    """
+        Function that plots final solution obtained
+    """
+    def plot_final_all(self):
+        
+        # get the index of the closest point to the goal
+        self.path,candidates = self.search_goal_parent(all_paths=True)
+        paths = []
+        for node in candidates:
+            path = self.extract_path(node)
+            paths.append(path)
+        # get the path
+        # self.path = self.extract_path([])
+
+        # get the index of the minimum cost vertex within a step length of the goal 
+        self.plotting.animation_all(self.list_of_vertices, paths, "All paths Porto Final Solution, N={}".format(self.iter_max),True,True)
+
 
 
 if __name__ == "__main__":
@@ -325,7 +348,7 @@ def plot_final(self):
     x_goal = (1.077466, 0.921832,0.750663, 0.1)
     grid = 'porto_grid.npy'
     time_forward = 0.2
-    n_samples = 1000
+    n_samples = 100
     goal_sample_rate = 0.10
     throttle_speed = 0.3
     number_of_motion_primitives = 5
@@ -333,3 +356,5 @@ def plot_final(self):
     kinodynamic_rrt = KinodynamicRRTStar(x_start, x_goal, time_forward,goal_sample_rate, throttle_speed, number_of_motion_primitives, n_samples,grid,min_speed=0.1)
     kinodynamic_rrt.planning()
     kinodynamic_rrt.plot_final()
+    #kinodynamic_rrt.plot_final_all()
+
diff --git a/kinodynamic_rrt/plotting.py b/kinodynamic_rrt/plotting.py
index 6a996af..6f72cd7 100644
--- a/kinodynamic_rrt/plotting.py
+++ b/kinodynamic_rrt/plotting.py
@@ -54,6 +54,18 @@ def animation(self,nodelist,path,name, animation=False,block=False):
         # plot the final path 
         self.plot_path(path,block=block)
 
+
+    def animation_all(self,nodelist,paths,name, animation=False,block=False):
+        # plot the grid
+        #self.ax.clear()
+        self.plot_grid(name)
+        # plot visited nodes
+        self.plot_visited(nodelist, animation)
+        # plot the final path 
+
+        for path in paths[:-1]:
+            self.plot_path(path,block=False,marker='--')
+        self.plot_path(paths[-1],block=block,marker='--')
     """
         I'm a visual learner so this method just shows me how points are sampled
     """
@@ -117,9 +129,9 @@ def plot_visited(self,nodelist, animation):
 
     # plotting a path via list comprehensions
     #@staticmethod
-    def plot_path(self,path,block=False):
+    def plot_path(self,path,block=False,marker='-r'):
         if len(path) != 0:
-            self.ax.plot([x[0] for x in path], [x[1] for x in path], '-r', linewidth=2)
+            self.ax.plot([x[0] for x in path], [x[1] for x in path], marker, linewidth=2)
             plt.pause(0.01)
         plt.show(block=block)