add error handling code

Author: AtsushiSakai

PathPlanning/BatchInformedRRTStar/batch_informed_rrtstar.py

@@ -311,7 +311,11 @@ def find_final_path(self):
         currId = self.goalId
         while (currId != self.startId):
             plan.append(self.tree.nodeIdToRealWorldCoord(currId))
-            currId = self.nodes[currId]
+            try:
+                currId = self.nodes[currId]
+            except(KeyError):
+                print("Path key error")
+                return []
 
         plan.append(self.start)
         plan = plan[::-1]  # reverse the plan