Simplifying the Astar Code

Author: Shaswat2001

python/utils/make_graph.py

@@ -15,13 +15,14 @@ def __init__(self,start,goal,obs_node,RPM,grid_size):
         self.start = start
         self.goal = goal
         self.obs_node = obs_node
-        self.drive = NonHolonomicDrive(RPM,grid_size,send_int_nodes=True)
+
+        self.drive = NonHolonomicDrive(RPM,grid_size)
 
         self.fig, self.ax = plt.subplots()
 
-    def animate(self,algorithm,visited,path):
+    def animate(self,algorithm,path):
         self.plot_canvas(algorithm)
-        # self.plot_visited(visited)
+        self.plot_visited(path)
         self.shortest_path(path)
         plt.show()
 
@@ -80,27 +81,48 @@ def plot_canvas(self,algorithm):
     def shortest_path(self,path):
 
         for i in reversed(range(1,len(path))):
-            start = path[i][0]
-            end = path[i-1][0]
-            theta = path[i][1]
+            start = path[i]["vertex"]
+            end = path[i-1]["vertex"]
+            theta = path[i]["orientation"]
             # theta = np.rad2deg(math.atan2(end.y-start.y,end.x-start.x))%360
-            neighbours = self.drive.get_neighbours(start,theta)
+            int_node_x = []
+            int_node_y = []
+            for x,y in path[i]["intermediate_nodes"]:
+    
+                int_node_x.append(round(x,2))
+                int_node_y.append(round(y,2))
+            plt.plot(int_node_x, int_node_y,color="r")
+            plt.pause(0.01)
 
-            for key,value in neighbours.items():
+        plt.scatter(self.start.x,self.start.y,color="magenta")
+        plt.scatter(self.goal.x,self.goal.y,color="blue")
+        plt.pause(0.01)
 
-                (_,_,int_nodes) = value
+    def plot_visited(self,path):
+
+        for i in reversed(range(1,len(path))):
+            start = path[i]["vertex"]
+            end = path[i-1]["vertex"]
+            theta = path[i]["orientation"]
+
+            neighbours = self.drive.get_neighbours(start,theta)
+            # theta = np.rad2deg(math.atan2(end.y-start.y,end.x-start.x))%360
+            for _,value in neighbours.items():
+
+                int_nodes = value["intermediate_nodes"]
 
                 int_node_x = []
                 int_node_y = []
                 for i in range(len(int_nodes)):
                     int_node_x.append(round(int_nodes[i][0],2))
                     int_node_y.append(round(int_nodes[i][1],2))
-                plt.plot(int_node_x, int_node_y,color="r")
+                plt.plot(int_node_x, int_node_y,color="g")
                 plt.pause(0.01)
 
         plt.scatter(self.start.x,self.start.y,color="magenta")
         plt.scatter(self.goal.x,self.goal.y,color="blue")
         plt.pause(0.01)
 
 
+
 

python/utils/non_holonomic.py

@@ -4,15 +4,14 @@
 
 class NonHolonomicDrive:
 
-    def __init__(self,RPM,grid_size,r = 3.8,L = 35,time_run = 1,send_int_nodes=False):
+    def __init__(self,RPM,grid_size,r = 3.8,L = 35,time_run = 1):
 
         self.RPM_L = RPM[0]
         self.RPM_R = RPM[1]
         self.grid_size = grid_size
         self.r = r
         self.L = L
         self.time_run = time_run
-        self.send_int_nodes = send_int_nodes
 
     def get_neighbours(self,current_node,current_orientation):
 
@@ -26,13 +25,13 @@ def get_neighbours(self,current_node,current_orientation):
 
             nbr,_,int_nodes = self.MoveRobot(current_node,current_orientation,RPM[0],RPM[1])
             robot_twist = self.convert_rpm_to_robospeed(RPM,self.r,self.L)
+            
             if self.grid_size[0][0] <= nbr[2].x <= self.grid_size[0][1] and self.grid_size[1][0] <= nbr[2].y <= self.grid_size[1][1]:
-                
-                if not self.send_int_nodes:
-                    all_neighbours[nbr[2]] = (nbr[0],nbr[1],robot_twist)
-                else:
-                    all_neighbours[nbr[2]] = (nbr[0],nbr[1],int_nodes)
-
+                all_neighbours[nbr[2]] = {"cost":nbr[0],
+                                          "orientation":nbr[1],
+                                          "twist":robot_twist,
+                                          "intermediate_nodes":int_nodes}
+        
         return all_neighbours
 
     def MoveRobot(self,current_node,current_orientation,RPM_L,RPM_R):

scripts/SearchBased/HeuristicSearch/Astar.py

@@ -73,8 +73,11 @@ def plan(self):
 
                 # If the neighbour is not already visited and if not in Obstacle space
                 if not self.graph.checkObstacleSpace(nbr_node) and not check_NodeIn_list(nbr_node,self.CLOSED):
-                    movement_cost, new_orientation,turtlebot_twist = node_values
-                    new_orientation = new_orientation%360
+                    
+                    movement_cost = node_values["cost"]
+                    new_orientation = node_values["orientation"]%360
+                    turtlebot_twist = node_values["twist"]
+
                     if self.V[int(2*nbr_node.x)][int(2*nbr_node.y)][self.orientation[new_orientation]] == 0:
 
                         self.V[int(2*nbr_node.x)][int(2*nbr_node.y)][self.orientation[new_orientation]] = 1
@@ -89,7 +92,10 @@ def plan(self):
 
                         self.OPEN.insert_pq(self.f[int(2*nbr_node.x)][int(2*nbr_node.y)][self.orientation[new_orientation]],(nbr_node,new_orientation))
                         self.backtrack_node[nbr_node]={}
-                        self.backtrack_node[nbr_node][self.f[int(2*nbr_node.x)][int(2*nbr_node.y)][self.orientation[new_orientation]]] = (current_vtx[0],current_vtx[1],turtlebot_twist)
+                        self.backtrack_node[nbr_node][self.f[int(2*nbr_node.x)][int(2*nbr_node.y)][self.orientation[new_orientation]]] = {"vertex":current_vtx[0],
+                                                "orientation":current_vtx[1],
+                                                "twist":turtlebot_twist,
+                                                "intermediate_nodes":node_values["intermediate_nodes"]}
 
                         if euclidean_heuristic(nbr_node,self.goal) <= 1.5: # Check if goal node is reached
                             print("The goal node is found")
@@ -99,7 +105,10 @@ def plan(self):
                         if self.f[int(2*nbr_node.x)][int(2*nbr_node.y)][self.orientation[new_orientation]] > self.f[int(2*current_vtx[0].x)][int(2*current_vtx[0].y)][self.orientation[current_vtx[1]]]:
                             self.f[int(2*nbr_node.x)][int(2*nbr_node.y)][self.orientation[new_orientation]] = self.f[int(2*current_vtx[0].x)][int(2*current_vtx[0].y)][self.orientation[current_vtx[1]]]
                             same_nbr_node = getSameNode(nbr_node,list(self.backtrack_node.keys()))
-                            self.backtrack_node[same_nbr_node][self.f[int(2*nbr_node.x)][int(2*nbr_node.y)][self.orientation[new_orientation]]] = (current_vtx[0],current_vtx[1],turtlebot_twist)
+                            self.backtrack_node[same_nbr_node][self.f[int(2*nbr_node.x)][int(2*nbr_node.y)][self.orientation[new_orientation]]] = {"vertex":current_vtx[0],
+                                                            "orientation":current_vtx[1],
+                                                            "twist":turtlebot_twist,
+                                                            "intermediate_nodes":node_values["intermediate_nodes"]}
 
 
         # If a path Doesn't exit
@@ -114,23 +123,25 @@ def extract_path(self,vertex):
         bkt_list -- List of nodes in the shortest path
         '''
 
-        bkt_list=[]
-        angle_node_list = []
-        bkt_list.append(self.goal)
+        goal_node = {"vertex":self.goal,
+                     "orientation":None,
+                     "twist":None,
+                     "intermediate_nodes":None}
+        
+        angle_node_list = [goal_node]
         node = vertex
         # loops till goal is not equal to zero
         while node!=0:
             for nbr,values in reversed(list(self.backtrack_node.items())):
                 # if nbr and goal are same
-                for cost,parent in values.items():
+                for _,parent in values.items():
 
                     if check_nodes(nbr,node):
-                        if not check_NodeIn_list(parent[0],bkt_list):
-                            bkt_list.append(parent[0])
+                        if not check_NodeIn_list(parent["vertex"],[node["vertex"] for node in angle_node_list]):
                             angle_node_list.append(parent)
-                        node=parent[0]
+                        node=parent["vertex"]
 
-                        if check_nodes(parent[0],self.start[0]):
+                        if check_nodes(parent["vertex"],self.start[0]):
                             node=0
                             return angle_node_list
 
@@ -189,28 +200,28 @@ def get_grid_size(obstacle_list):
 
     mp_data = motion_planning()
 
-    for i in shortest_path:
+    for i in shortest_path[1:]:
 
         path_pt = Point()
-        path_pt.x = i[0].x
-        path_pt.y = i[0].y
+        path_pt.x = i["vertex"].x
+        path_pt.y = i["vertex"].y
         path_pt.z = 0
         mp_data.path.append(path_pt)
 
-        mp_data.orientation.append(i[1])
+        mp_data.orientation.append(i["orientation"])
 
         twist_pt = Twist()
-        twist_pt.angular.z = i[-1][0]
-        twist_pt.linear.x = i[-1][1]
+        twist_pt.angular.z = i["twist"][0]
+        twist_pt.linear.x = i["twist"][1]
 
         mp_data.turtlebot_twist.append(twist_pt)
 
     mp_pub.publish(mp_data)
 
-    # plot_result = Visualize(start[0],goal,obs_locations,RPM,grid_size)
-    # plot_result.animate("Astar",None,shortest_path)
+    plot_result = Visualize(start[0],goal,obs_locations,RPM,grid_size)
+    plot_result.animate("Astar",shortest_path)
 
     for i in shortest_path:
-        rospy.loginfo(f"The x : {i[0].x} and y : {i[0].y} and orientation {i[1]}")
+        rospy.loginfo(f"The x coordinate : {i['vertex'].x} and y coordinate : {i['vertex'].y} and orientation : {i['orientation']}")