Implemented dijsktra to return shortest odom path

Graph_mapping/graph_mapping.py

@@ -1,26 +1,31 @@
 from collections import defaultdict
 import math
+from shortest_path import dijsktra
 
 class Graph():
     def __init__(self):
+
         """
         self.edges is a dict of all possible next nodes
         e.g. {'X': ['A', 'B', 'C', 'E'], ...}
         self.weights has all the weights between two nodes,
         with the two nodes as a tuple as the key
         e.g. {('X', 'A'): 7, ('X', 'B'): 2, ...}
         """
+
         self.edges = defaultdict(list)
         self.weights = {}                                   # this is the distance between two nodes
         self.co_ordinates = [(0,0) for i in range(15)]     # this is the odom values of a node
 
 
     def add(self, from_node, to_node, odom_x, odom_y):      # odom_x = odom_from = (a,b)
+
         '''
         Note: assumes edges are bi-directional
         odom_from = (1,2)
         odom_to = (1,3) i.e in a tuple format containing (x,y) cartesian co-ordinate
         '''
+
         if to_node not in self.edges[from_node]: 
             self.edges[from_node].append(to_node)
             self.edges[to_node].append(from_node)
@@ -35,6 +40,15 @@ def add(self, from_node, to_node, odom_x, odom_y):      # odom_x = odom_from = (
         self.weights[(to_node, from_node)] = weight               
 
     def check_repeated_node(self,odom_x): 
+
+        '''
+        Func to check if the node in a graph is repeated, if not create a new node
+        
+        Parameter : co-ordinates of the current node
+        Return value : current node position of the bot
+
+        '''
+
         threshold = 1    #Error allowed 
         counter=0        # To store the node number
         flag = 0         # 0 means create new node, 1 means exisiting node found
@@ -59,20 +73,38 @@ def print_graph(self):
         print('Co-ordinates = ',self.co_ordinates)
         print('-----------------')
 
+    def shortest_path(self,path):
+
+        '''
+        Func to convert the shortest node path to shortest cartesian path that the urdf bot has to follow
+
+        parameter : shortest path corresponding to nodes
+        return value : shortest path corresponding to cartesian co-ordinates
+
+        '''
+
+        co_ordinate_path = [self.co_ordinates[i] for i in path]
+        return co_ordinate_path
+
 g = Graph()
 
 more_than_2_exits=True
 took_a_turn=True
 dead_end=True
 
-start=1
-prev_node=start       # References thoroughout the code
+start_node=1
+prev_node=start_node       # References thoroughout the code
 curr_node=2
 
 prev_odom = (0,0)
 
+
+# Set of hardcoded values. Input these values continuously from urdf model in the run 
+
 nodes=2
 odom = [(3,0),(3,3),(10,3),(3,3.5),(3,0.5),(7,0),(7,-2),(9,-2)]
+
+
 for i in range(len(odom)):
     curr_node = nodes
     curr_odom = odom[i]
@@ -89,5 +121,11 @@ def print_graph(self):
     prev_node = curr_node
     prev_odom = curr_odom
     g.print_graph()
-
-
+
+end_node = curr_node
+
+node_shortest_path = dijsktra(g,start_node,end_node)
+co_orindate_shortest_path = g.shortest_path(node_shortest_path)
+
+print('The ideal shortest node path to follow = ',node_shortest_path)
+print('The ideal shortest co-ordinate path to follow = ',co_orindate_shortest_path)

Graph_mapping/shortest_path.py

@@ -0,0 +1,38 @@
+def dijsktra(graph, initial, end):
+    # shortest paths is a dict of nodes
+    # whose value is a tuple of (previous node, weight)
+    shortest_paths = {initial: (None, 0)}
+    current_node = initial
+    visited = set()
+
+    while current_node != end:
+        visited.add(current_node)
+        destinations = graph.edges[current_node]
+        weight_to_current_node = shortest_paths[current_node][1]
+
+        for next_node in destinations:
+            weight = graph.weights[(current_node, next_node)] + weight_to_current_node
+            if next_node not in shortest_paths:
+                shortest_paths[next_node] = (current_node, weight)
+            else:
+                current_shortest_weight = shortest_paths[next_node][1]
+                if current_shortest_weight > weight:
+                    shortest_paths[next_node] = (current_node, weight)
+
+        next_destinations = {node: shortest_paths[node] for node in shortest_paths if node not in visited}
+        if not next_destinations:
+            return []  #if no path possible, return empty list
+
+        # next node is the destination with the lowest weight
+        current_node = min(next_destinations, key=lambda k: next_destinations[k][1])
+
+    # Work back through destinations in shortest path
+    path = []
+    while current_node is not None:
+        path.append(current_node)
+        next_node = shortest_paths[current_node][0]
+        current_node = next_node
+    # Reverse path
+    path = path[::-1]
+    return path
+