unit6_pp/scripts/planner_solution.py
def dijkstra_geo_planner(self, start_segment, goal_segment):
        """ 
        Plan route from start to goal. The actual search algorithm to find a path is executed here.   
        start_segment: The nearest segment to the start point, as type 'geographic_msgs/RouteSegment'
        goal_segment: The nearest segment to the goal point, as type 'geographic_msgs/RouteSegment'
        return: The planned path between start and goal, as type 'geographic_msgs/RoutePath'
        """
        
        # request parameters
        start_idx = self.points.index(start_segment.start.uuid)
        goal_idx = self.points.index(goal_segment.start.uuid)

        # initialize the object that holds the path to the goal
        plan = RoutePath(network=self.graph.id)

        # Dijkstra shortest path algorithm

        # create an open list
        # Important:
        # add nodes to open_list as [distance, index] pairs
        # where the first element is travel costs to the node
        # and the second element is the node's index
        open_list = []

        # add start node to open list as an [distance, index] pair
        open_list.append([0, start_idx])

        # boolean list used to check for closed nodes
        closed_list = [False for wp_id in xrange(len(self.points))]
        closed_list[start_idx] = True

        # dict for mapping children to parent nodes
        # Important:
        # dictionary key is current_neighbour_node
        # first element in value is current_node
        # second element is valie is the edge connecting both nodes
        parents = dict()

        # boolean flag indicating whether the goal was reached or not
        reached_goal = False

        rospy.loginfo('Dijkstra: Done with initialization')

        # Main loop, executes as long as there are still nodes inside open_list
        while open_list:

            # sort open_list according to first element of each sublist (the travel cost value)
            open_list.sort()
            # get 'current node', the first element in the sorted list
            current_travel_cost, current_node = open_list.pop(0)

            # Close node to prevent from travelling it again
            closed_list[current_node] = True

            # If current_node is the goal, exit the main loop
            if (current_node == goal_idx):
              reached_goal = True
              break

            # Loop neighbours
            for current_edge in self.edges[current_node]:
                # get index value of neighbour
                current_neighbour_node = current_edge.end

                # Check if the neighbor has already been visited
                if closed_list[current_neighbour_node]:
                  continue

                # calculate travel cost of neighbour considering it is reached through current_node
                new_travel_cost = current_travel_cost + current_edge.step_cost

                # Check if the neighbor is in open_list
                in_open_list = False
                for idx, open_list_item in enumerate(open_list):
                  # compare to current neighbour node
                  if open_list_item[1] == current_neighbour_node:
                    in_open_list = True
                    previous_cost = open_list_item[0]
                    break
                
                # CASE 1: neighbor already in open_list
                if in_open_list:
                  if new_travel_cost < previous_cost:
                    # Update the node's parent node inside 'parents'
                    parents[current_neighbour_node] = [current_node, current_edge]
                    # Update the node's g_cost inside the open list
                    open_list[idx] = [new_travel_cost, current_neighbour_node]

                # CASE 2: neighbor not in open_list
                else:
                  # Add neighbor and g cost to open_list
                  open_list.append([new_travel_cost, current_neighbour_node])
                  # Update the node's parent node inside 'parents'
                  parents[current_neighbour_node] = [current_node, current_edge]

        rospy.loginfo('Dijkstra: Done traversing nodes in open_list')

        if not reached_goal:
            rospy.logwarn('Dijkstra: No path found!')
            plan.segments = []
            return plan
            
        else:
          # reconstruct path by working backwards from target
          plan.segments = []

          while current_node != start_idx:
              plan.segments.append(parents[current_node][1].seg)
              # get next node to be processed
              current_node = parents[current_node][0]

          # reverse list
          plan.segments.reverse()

          rospy.loginfo('Dijkstra: Done reconstructing path')

          return plan
 
        Plan route from start to goal. The actual search algorithm to find a path is executed here.   
        start_segment: The nearest segment to the start point, as type 'geographic_msgs/RouteSegment'
        goal_segment: The nearest segment to the goal point, as type 'geographic_msgs/RouteSegment'
        return: The planned path between start and goal, as type 'geographic_msgs/RoutePath'
        
Plan route from start to goal. The actual search algorithm to find a path is executed here.
Plan route from start to goal . The actual search algorithm to find a path is executed here .
def dijkstra_geo_planner(self, start_segment, goal_segment):
        start_idx = self.points.index(start_segment.start.uuid)
        goal_idx = self.points.index(goal_segment.start.uuid)
        plan = RoutePath(network=self.graph.id)
        open_list = []
        open_list.append([0, start_idx])
        closed_list = [False for wp_id in xrange(len(self.points))]
        closed_list[start_idx] = True
        parents = dict()
        reached_goal = False
        rospy.loginfo('Dijkstra: Done with initialization')
        while open_list:
            open_list.sort()
            current_travel_cost, current_node = open_list.pop(0)
            closed_list[current_node] = True
            if (current_node == goal_idx):
              reached_goal = True
              break
            for current_edge in self.edges[current_node]:
                current_neighbour_node = current_edge.end
                if closed_list[current_neighbour_node]:
                  continue
                new_travel_cost = current_travel_cost + current_edge.step_cost
                in_open_list = False
                for idx, open_list_item in enumerate(open_list):
                  if open_list_item[1] == current_neighbour_node:
                    in_open_list = True
                    previous_cost = open_list_item[0]
                    break
                if in_open_list:
                  if new_travel_cost < previous_cost:
                    parents[current_neighbour_node] = [current_node, current_edge]
                    open_list[idx] = [new_travel_cost, current_neighbour_node]
                else:
                  open_list.append([new_travel_cost, current_neighbour_node])
                  parents[current_neighbour_node] = [current_node, current_edge]
        rospy.loginfo('Dijkstra: Done traversing nodes in open_list')
        if not reached_goal:
            rospy.logwarn('Dijkstra: No path found!')
            plan.segments = []
            return plan
        else:
          plan.segments = []
          while current_node != start_idx:
              plan.segments.append(parents[current_node][1].seg)
              current_node = parents[current_node][0]
          plan.segments.reverse()
          rospy.loginfo('Dijkstra: Done reconstructing path')
          return plan
def dijkstra_geo_planner ( self , start_segment , goal_segment ) : start_idx = self . points . index ( start_segment . start . uuid ) goal_idx = self . points . index ( goal_segment . start . uuid ) plan = RoutePath ( network = self . graph . id ) open_list = [ ] open_list . append ( [ 0 , start_idx ] ) closed_list = [ False for wp_id in xrange ( len ( self . points ) ) ] closed_list [ start_idx ] = True parents = dict ( ) reached_goal = False rospy . loginfo ( 'Dijkstra: Done with initialization' ) while open_list : open_list . sort ( ) current_travel_cost , current_node = open_list . pop ( 0 ) closed_list [ current_node ] = True if ( current_node == goal_idx ) : reached_goal = True break for current_edge in self . edges [ current_node ] : current_neighbour_node = current_edge . end if closed_list [ current_neighbour_node ] : continue new_travel_cost = current_travel_cost + current_edge . step_cost in_open_list = False for idx , open_list_item in enumerate ( open_list ) : if open_list_item [ 1 ] == current_neighbour_node : in_open_list = True previous_cost = open_list_item [ 0 ] break if in_open_list : if new_travel_cost < previous_cost : parents [ current_neighbour_node ] = [ current_node , current_edge ] open_list [ idx ] = [ new_travel_cost , current_neighbour_node ] else : open_list . append ( [ new_travel_cost , current_neighbour_node ] ) parents [ current_neighbour_node ] = [ current_node , current_edge ] rospy . loginfo ( 'Dijkstra: Done traversing nodes in open_list' ) if not reached_goal : rospy . logwarn ( 'Dijkstra: No path found!' ) plan . segments = [ ] return plan else : plan . segments = [ ] while current_node != start_idx : plan . segments . append ( parents [ current_node ] [ 1 ] . seg ) current_node = parents [ current_node ] [ 0 ] plan . segments . reverse ( ) rospy . loginfo ( 'Dijkstra: Done reconstructing path' ) return plan
Plan route from start to goal.
Plan route from start to goal .
""" 
        Plan route from start to goal. The actual search algorithm to find a path is executed here.   
        start_segment: The nearest segment to the start point, as type 'geographic_msgs/RouteSegment'
        goal_segment: The nearest segment to the goal point, as type 'geographic_msgs/RouteSegment'
        return: The planned path between start and goal, as type 'geographic_msgs/RoutePath'
        """ # request parameters # initialize the object that holds the path to the goal # Dijkstra shortest path algorithm # create an open list # Important: # add nodes to open_list as [distance, index] pairs # where the first element is travel costs to the node # and the second element is the node's index # add start node to open list as an [distance, index] pair # boolean list used to check for closed nodes # dict for mapping children to parent nodes # Important: # dictionary key is current_neighbour_node # first element in value is current_node # second element is valie is the edge connecting both nodes # boolean flag indicating whether the goal was reached or not # Main loop, executes as long as there are still nodes inside open_list # sort open_list according to first element of each sublist (the travel cost value) # get 'current node', the first element in the sorted list # Close node to prevent from travelling it again # If current_node is the goal, exit the main loop # Loop neighbours # get index value of neighbour # Check if the neighbor has already been visited # calculate travel cost of neighbour considering it is reached through current_node # Check if the neighbor is in open_list # compare to current neighbour node # CASE 1: neighbor already in open_list # Update the node's parent node inside 'parents' # Update the node's g_cost inside the open list # CASE 2: neighbor not in open_list # Add neighbor and g cost to open_list # Update the node's parent node inside 'parents' # reconstruct path by working backwards from target # get next node to be processed # reverse list
self
start_segment
goal_segment