Exemple #1
0
    def direction(self, loc1, loc2):
        'determine the direction of the next step to take using A*'
        row1, col1 = loc1

        path = heuristic_search(self.graph, loc1, loc2, scott())
        try:
            row2, col2 = path[1]
        except IndexError:
            return False

        # translate into a direction SNEWSNEW!
        if row1 - row2 == -1:
            return 's'
        elif row1 - row2 == 1:
            return 'n'
        elif col1 - col2 == -1:
            return 'e'
        elif col1 - col2 == 1:
            return 'w'
        elif (row1 - row2) > 0:
            return 's'
        elif (row1 - row2) < 0:
            return 'n'
        elif (col1 - col2) > 0:
            return 'e'
        elif (col1 - col2) < 0:
            return 'w'
        else:
            return False
Exemple #2
0
 def _risk(self, _heuristic, board_with_heads, path, snakes):
     score = 0
     for node in path:
         for snake in snakes:
             opponent_path = heuristic_search(
                 board_with_heads, tuple(snake["coords"][0]), node, _heuristic)
             # distance between the opponent's head and path point
             score += len(opponent_path)
     return score
Exemple #3
0
 def way_between_areas(self,start,dest):
     way = []
     try :
         way = heuristic_search(self.areas_graph, start, dest, self.heuristic)
     except pygraph.classes.exceptions.NodeUnreachable :
         pass
     except KeyError :
         print 'E:way_between_areas: Key error', start, dest
     return way 
Exemple #4
0
    def search_A_star(self, sentence):
        tokens = nltk.word_tokenize(sentence)

        #if a node does not exist, it shall be added to the graph and connected to the previous node
        for i, token in enumerate(tokens):
            if not self.net.has_node(token):
                self.net.add_node(str(token))

                #print "adding node", token

            #if the edge does not exist between the current node and the previous one, an edge shall be added
            if i != 0:
                edge = (tokens[i - 1], token)
                if not self.net.has_edge(edge):
                    self.net.add_edge(edge,
                                      wt=self.EDGE_START_WEIGHT,
                                      label=self.START_OCCURRENCES_NUM)

        h = chow(
            tokens[0]
        )  #this optimization shall be performed chooosing a term which is connected: the choice of this term shall be performed accoding to some rule
        h.optimize(self.net)
        path = ''

        for k, token in enumerate(tokens):
            if k != 0:
                try:
                    intermediate_node_path = heuristic_search(
                        self.net, tokens[k - 1], token,
                        h)  #the path between each couple

                except NodeUnreachable, e:
                    print str(e)
                    print "NODE UREACHABLE"

                for j, node in enumerate(
                        intermediate_node_path
                ):  #increase the weight of the edges in the path
                    if j != 0:
                        edge = (intermediate_node_path[j - 1], node)
                        if not self.net.has_edge(edge):
                            raise Exception('edge not found')
                        else:  #if the edge exists, its weight is divided by the number of occurrences stored in the label
                            number_of_occurrences = self.net.edge_label(edge)
                            new_number_of_occurrences = number_of_occurrences + 1
                            self.net.set_edge_label(edge,
                                                    new_number_of_occurrences)
                            self.net.set_edge_weight(edge,
                                                     wt=1.0 /
                                                     new_number_of_occurrences)

                if k < len(tokens) - 1:
                    #we remove the last element to prevent repetitions of head and tail in the subsequences of the final path
                    path = path + ' ' + ' '.join(intermediate_node_path[0:-1])
                else:
                    path = path + ' ' + ' '.join(intermediate_node_path)
def pathgen(graph):
    """Calculates the shortest path from each node to each other node."""
    # grnodes = len(graph.nodes())
    h = chow(1)         # Needed for heuristic_search()
    h.optimize(graph)
    paths = [[] for x in range(len(graph.nodes()))]
    for j in graph.nodes():
        for i in graph.nodes():
            paths[j].append(heuristic_search(graph, j, i, h))
    return paths
 def run_benchmark(self):
     G = self.fn_create()
     self.generate(G, self.GRAPH_ORDER, self.GRAPH_EDGES)
     
     reference_nodes = sample( G.nodes(), self.CHOW_REFERENCE_POINTS )
     heuristic = chow( *reference_nodes )
     heuristic.optimize(G)
     
     source_nodes = sample( G.nodes(), self.NUMBER_OF_SEARCHES )
     dest_nodes = sample( G.nodes(), self.NUMBER_OF_SEARCHES )
     
     for s,d in zip( source_nodes, dest_nodes ):
         log.debug("Searching from %s to %s" % (s,d) )
         result = heuristic_search( G, s, d, heuristic )
Exemple #7
0
 def a_star(self, targets):
     paths = {}
     distances = {}
     heuristic = chow(*targets)
     heuristic.optimize(self.board)
     for target in targets:
         try:
             paths[target] = heuristic_search(self.board, tuple(self.snake["coords"][0]), target, heuristic)
             paths[target] = paths[target][1:]  # remove first (current position)
             distances[target] = len(paths[target])
         except Exception as e:
             distances[target] = sys.maxint
             print("ERROR %s" % e)
             pass
     return paths, distances
    def search_A_star(self, sentence):
        tokens = nltk.word_tokenize(sentence)
        
        #if a node does not exist, it shall be added to the graph and connected to the previous node
        for i, token in enumerate(tokens):
            if not self.net.has_node(token):
                self.net.add_node(str(token))

                #print "adding node", token
                
            #if the edge does not exist between the current node and the previous one, an edge shall be added
            if i!=0:
                edge = (tokens[i-1], token)
                if not self.net.has_edge(edge):             
                    self.net.add_edge(edge, wt=self.EDGE_START_WEIGHT, label = self.START_OCCURRENCES_NUM)


                    
        h = chow(tokens[0]) #this optimization shall be performed chooosing a term which is connected: the choice of this term shall be performed accoding to some rule
        h.optimize(self.net)
        path = ''
        
        for k, token in enumerate(tokens):
            if k!=0:
                try:                     
                    intermediate_node_path = heuristic_search(self.net, tokens[k-1], token, h) #the path between each couple
                    
                except NodeUnreachable, e:
                    print str(e)
                    print "NODE UREACHABLE"
                        
                for j, node in enumerate(intermediate_node_path): #increase the weight of the edges in the path
                    if j != 0:
                        edge = (intermediate_node_path[j-1], node)
                        if not self.net.has_edge(edge):
                            raise Exception('edge not found')
                        else: #if the edge exists, its weight is divided by the number of occurrences stored in the label
                            number_of_occurrences = self.net.edge_label(edge)
                            new_number_of_occurrences = number_of_occurrences + 1
                            self.net.set_edge_label(edge, new_number_of_occurrences)
                            self.net.set_edge_weight(edge, wt = 1.0/new_number_of_occurrences)
                
                if k < len(tokens)-1: 
                    #we remove the last element to prevent repetitions of head and tail in the subsequences of the final path
                    path = path + ' ' + ' '.join(intermediate_node_path[0:-1])
                else:
                    path = path + ' ' + ' '.join(intermediate_node_path)
Exemple #9
0
 def findpath(self, nodeone, nodetwo):
   return heuristic_search(self.graph, nodeone, nodetwo, self.heuristic)[1:-1]
    def plan(self, point1, point2):
        """
        Plan a path which connects the two given 2D points.

        The points are represented by tuples of two numbers (x, y).

        Return a list of tuples where each tuple represents a point in the
        planned path, the first point is the start point, and the last point is
        the end point. If the planning algorithm failed the returned list
        should be empty.
        """
        #reset number of trials
        self.number_of_trials = 50

        self.search_result = False
        self.output_path = list()
        start_node_id = self.next_id()
        end_node_id = self.next_id()

        if not self.is_valid_points(point1, point2):
            rospy.logwarn(self.__class__.__name__ + ": invalid points")
            return self.output_path #return empty list
        else:
            # add point1, point2 to the graph
            self.graph.add_node(start_node_id, attrs=[('position', point1)])
            self.graph.add_node(end_node_id, attrs=[('position', point2)])

            # connect point1, point2
            if self.line_free_cb(point1, point2):
                edge_weight = sqrt(pow(point1[0] - point2[0], 2) + pow(point1[1] - point2[1], 2))
                self.graph.add_edge((start_node_id, end_node_id), wt = edge_weight)

            # add more edges between point1, point2 and the rest of graph
            for temp_id, attr in self.graph.node_attr.iteritems():
                # 'temp_id' contains node temp_id, 'attr' contains array of attributes
                temp_position = attr[0][1]
                if point1 != temp_position and self.line_free_cb(point1, temp_position):
                    edge_weight = sqrt(pow(point1[0] - temp_position[0], 2) + pow(point1[1] - temp_position[1], 2))
                    self.graph.add_edge((start_node_id, temp_id), wt = edge_weight)

                if point2 != temp_position and self.line_free_cb(point2, temp_position):
                    edge_weight = sqrt(pow(point2[0] - temp_position[0], 2) + pow(point2[1] - temp_position[1], 2))
                    self.graph.add_edge((end_node_id, temp_id), wt = edge_weight)

        while not self.search_result and self.number_of_trials > 0:
            try:
                #find a path (sequence of node ids) between start and end
                self.search_algorithm.optimize(self.graph)
                node_ids = heuristic_search(self.graph, start_node_id, end_node_id, self.search_algorithm)

                for temp_id in node_ids:
                    node_position = self.graph.node_attributes(temp_id)
                    self.output_path.append(node_position[0][1])

                self.search_result = True

            except NodeUnreachable:
                # create a random point if there is no path
                x = uniform(self.dimensions[0][0] , self.dimensions[0][1])
                y = uniform(self.dimensions[1][0] , self.dimensions[1][1])
                new_point_position = (x , y)
                # add the new point to graph
                if self.point_free_cb(new_point_position):
                    new_point_id = self.next_id()
                    self.graph.add_node(new_point_id, attrs=[('position', new_point_position)])
                    # connect the new_point with older nodes
                    for temp_id, attr in self.graph.node_attr.iteritems():
                        temp_position = attr[0][1]
                        if new_point_position != temp_position and self.line_free_cb(new_point_position, temp_position):
                            edge_weight = sqrt(pow(new_point_position[0] - temp_position[0], 2) + pow(new_point_position[1] - temp_position[1], 2))
                            self.graph.add_edge((new_point_id, temp_id), wt = edge_weight)

            # decrease number of trials
            self.number_of_trials = self.number_of_trials-1

        return self.output_path
    def plan(self, point1, point2):
        """
        Plan a path which connects the two given 2D points.

        The points are represented by tuples of two numbers (x, y).

        Return a list of tuples where each tuple represents a point in the
        planned path, the first point is the start point, and the last point is
        the end point. If the planning algorithm failed the returned list
        should be empty.
        """
        path = []
        #Add start and end
        self.node_counter += 1
        start = self.node_counter
        self.graph.add_node(self.node_counter,
                            attrs=[('position', (point1[0], point1[1]))])
        self.node_counter += 1
        end = self.node_counter
        self.graph.add_node(self.node_counter,
                            attrs=[('position', (point2[0], point2[1]))])

        new_node_counter = 0
        #Create nodes until a solution is found, limit = self.node_limit
        while new_node_counter < self.node_limit:
            #Add 5 new random nodes each iteration
            for i in range(self.number_nodes):
                position_x = random.uniform(self.dimensions_x[0],
                                            self.dimensions_x[1])
                position_y = random.uniform(self.dimensions_y[0],
                                            self.dimensions_y[1])
                if self.point_free_cb((position_x, position_y)):
                    self.node_counter += 1
                    new_node_counter += 1
                    self.graph.add_node(self.node_counter,
                                        attrs=[('position', (position_x,
                                                             position_y))])

            #Add edges
            for nid1, attr in self.graph.node_attr.iteritems():
                position1 = attr[0][1]

                for nid2, attr2 in self.graph.node_attr.iteritems():
                    position2 = attr2[0][1]
                    distance = self.distance(position1, position2)
                    #Avoid self edges and repetitivity
                    if distance > 0.0 and self.graph.has_edge(
                        (nid1, nid2)) == False:
                        if self.line_free_cb(position1, position2):
                            self.graph.add_edge((nid1, nid2), wt=distance)

            #A* search
            try:
                heuristic = euclidean()
                heuristic.optimize(self.graph)
                id_path = heuristic_search(self.graph, start, end, heuristic)

                for member in id_path:
                    attributes = self.graph.node_attributes(member)
                    path.append(attributes[0][1])
            except NodeUnreachable:
                rospy.logwarn("Path unreachable, not enough nodes")
            else:
                return path

        #If node limit is passed, then no path is found
        rospy.logwarn("Node limit reached, path is not connected")
        return path
Exemple #12
0
    def plan(self, point1, point2):
        """
        Plan a path which connects the two given 2D points.

        The points are represented by tuples of two numbers (x, y).

        Return a list of tuples where each tuple represents a point in the
        planned path, the first point is the start point, and the last point is
        the end point. If the planning algorithm failed the returned list
        should be empty.
        """
        #reset number of trials
        self.number_of_trials = 50

        self.search_result = False
        self.output_path = list()
        start_node_id = self.next_id()
        end_node_id = self.next_id()

        if not self.is_valid_points(point1, point2):
            rospy.logwarn(self.__class__.__name__ + ": invalid points")
            return self.output_path  #return empty list
        else:
            # add point1, point2 to the graph
            self.graph.add_node(start_node_id, attrs=[('position', point1)])
            self.graph.add_node(end_node_id, attrs=[('position', point2)])

            # connect point1, point2
            if self.line_free_cb(point1, point2):
                edge_weight = sqrt(
                    pow(point1[0] - point2[0], 2) +
                    pow(point1[1] - point2[1], 2))
                self.graph.add_edge((start_node_id, end_node_id),
                                    wt=edge_weight)

            # add more edges between point1, point2 and the rest of graph
            for temp_id, attr in self.graph.node_attr.iteritems():
                # 'temp_id' contains node temp_id, 'attr' contains array of attributes
                temp_position = attr[0][1]
                if point1 != temp_position and self.line_free_cb(
                        point1, temp_position):
                    edge_weight = sqrt(
                        pow(point1[0] - temp_position[0], 2) +
                        pow(point1[1] - temp_position[1], 2))
                    self.graph.add_edge((start_node_id, temp_id),
                                        wt=edge_weight)

                if point2 != temp_position and self.line_free_cb(
                        point2, temp_position):
                    edge_weight = sqrt(
                        pow(point2[0] - temp_position[0], 2) +
                        pow(point2[1] - temp_position[1], 2))
                    self.graph.add_edge((end_node_id, temp_id), wt=edge_weight)

        while not self.search_result and self.number_of_trials > 0:
            try:
                #find a path (sequence of node ids) between start and end
                self.search_algorithm.optimize(self.graph)
                node_ids = heuristic_search(self.graph, start_node_id,
                                            end_node_id, self.search_algorithm)

                for temp_id in node_ids:
                    node_position = self.graph.node_attributes(temp_id)
                    self.output_path.append(node_position[0][1])

                self.search_result = True

            except NodeUnreachable:
                # create a random point if there is no path
                x = uniform(self.dimensions[0][0], self.dimensions[0][1])
                y = uniform(self.dimensions[1][0], self.dimensions[1][1])
                new_point_position = (x, y)
                # add the new point to graph
                if self.point_free_cb(new_point_position):
                    new_point_id = self.next_id()
                    self.graph.add_node(new_point_id,
                                        attrs=[('position', new_point_position)
                                               ])
                    # connect the new_point with older nodes
                    for temp_id, attr in self.graph.node_attr.iteritems():
                        temp_position = attr[0][1]
                        if new_point_position != temp_position and self.line_free_cb(
                                new_point_position, temp_position):
                            edge_weight = sqrt(
                                pow(new_point_position[0] -
                                    temp_position[0], 2) +
                                pow(new_point_position[1] -
                                    temp_position[1], 2))
                            self.graph.add_edge((new_point_id, temp_id),
                                                wt=edge_weight)

            # decrease number of trials
            self.number_of_trials = self.number_of_trials - 1

        return self.output_path
def shortestPathFromDataState(graph, node):

  def zeroHeuristic(neighbor, goal):
    return 0 

  return heuristic_search(graph, "dataState", node, zeroHeuristic)
    def plan(self, point1, point2):

        a = self.dimensions[0]
        rospy.logwarn(np.random.uniform(a[0], a[1], 30))
        rospy.logwarn(np.random.uniform(a[0], a[1], 30))

        self.node_list = []
        result_node_id = []
        result_path = []

        #Adding start node to the graph and local node list
        start_node_id = self.generate_id()
        self.graph.add_node(start_node_id, attrs=[('position', point1)])
        self.node_list.append(point1)

        #Adding goal node to the graph and local node list
        goal_node_id = self.generate_id()
        self.graph.add_node(goal_node_id, attrs=[('position', point2)])
        self.node_list.append(point2)

        #Checking is there is any direct path between the start and goal node
        if self.line_free_cb(point1, point2):
            rospy.loginfo('Direct path found between start and goal node')
            return self.node_list

        #Generating 30 random nodes and adding it to graph
        while (len(self.node_list) < 30):
            self.addNode()

        rospy.loginfo('Creating possible edges process started...')
        #Generating possible edges between the generated nodes
        for nid, attr in self.graph.node_attr.iteritems():
            temp_id = nid
            temp_position = attr[0][1]
            for nid_1, attr_1 in self.graph.node_attr.iteritems():
                if (nid_1 != temp_id):
                    #Checking the line free between two nodes
                    if self.line_free_cb(temp_position, attr_1[0][1]):
                        if not self.graph.has_edge((temp_id, nid_1)):
                            #Adding the edge between two nodes with euclidean distance
                            self.graph.add_edge((temp_id, nid_1),
                                                self.calculateEuclidean(
                                                    temp_position,
                                                    attr_1[0][1]))
        rospy.loginfo('Creating possible edges process finished...')

        #Optimizing the graph
        self.heuristic.optimize(self.graph)
        rospy.loginfo('Optimizing graph finished...')

        try:
            #Performing A* search to find the shortest path between the start node and goal node
            rospy.loginfo('Finding shorted path using A* algorithm started')
            result_node_id = heuristic_search(self.graph, start_node_id,
                                              goal_node_id, self.heuristic)
            rospy.loginfo('Shorteset path found...')
        except:
            rospy.loginfo('No Shorteset path found...')

        #Creating the list for the shortest path between the start and goal node
        for name in result_node_id:
            result_path.append(self.get_node(name))

        print result_path
        rospy.loginfo('Planning completed...')
        return result_path
        """
Exemple #15
0
 def distance(self, loc1, loc2):
     return len(heuristic_search(self.graph, loc1, loc2, scott()))
Exemple #16
0
 def findpath(self, nodeone, nodetwo):
     return heuristic_search(self.graph, nodeone, nodetwo,
                             self.heuristic)[1:-1]
    def plan(self, point1, point2):
        """
        Plan a path which connects the two given 2D points.

        The points are represented by tuples of two numbers (x, y).

        Return a list of tuples where each tuple represents a point in the
        planned path, the first point is the start point, and the last point is
        the end point. If the planning algorithm failed the returned list
        should be empty.
        """
        print "PLANNING METHOD CALLED"

        if not self.point_free_cb(point1):
            rospy.logwarn("START POINT UNAVAILIBLE")
            return []
        if not self.point_free_cb(point2):
            rospy.logwarn("END POINT UNAVAILIBLE")
            return []

        if self.line_free_cb(point1, point2):
            print "STRAIGHT LINE ROUTE FOUND"
            return [point1, point2]
        else:
            print "EXPANDING MAP"
            start_id = self.add_node_to_graph(point1, force=True)
            finish_id = self.add_node_to_graph(point2, force=True)

            h = euclidean()

            while True:
                # No DO..UNTIL loops in python. Condition check below
                path = []
                try:
                    h.optimize(self.graph)
                    path = heuristic_search(self.graph, start_id, finish_id, h)
                except NodeUnreachable:
                    # add another node
                    new_node_added = False
                    while not new_node_added:
                        randomPoint = (uniform(*self.dimensions[0]), uniform(*self.dimensions[1]))
                        if self.point_free_cb(randomPoint):
                            if self.add_node_to_graph(randomPoint):
                                new_node_added = True

                # DO..UNTIL emulation this way
                if len(path) > 0 or self.nodecount >= RandomizedRoadmapPlanner.MAX_NODES_NUM:
                    print "Break loop"
                    break

            # print path
            if len(path) > 0:
                found_path = []
                rospy.loginfo("PATH FOUND")
                for n in path:
                    found_path.append(self.graph.node_attr.get(n)[0][1])
                return found_path
            elif self.nodecount >= RandomizedRoadmapPlanner.MAX_NODES_NUM:
                rospy.logwarn("MAX NUM NODES ALLOWED EXCEEDED!")

        rospy.logwarn("PATH NOT FOUND!")
        return []