def test_insert(self): # Verify the value which is smaller than the root of min heap # is new root after it is inserted test_input = [ [2], [3, 4], [3, 5, 4], [16, 14, 7, 2, 8, 1], [12, 11, 6, 8, 10, 2, 3, 1], [8, 3, 1, 5, 6, 4, 2] ] for i in range(len(test_input)): pq = priority_queue.PriorityQueue(test_input[i]) pq.build_heap() new_min = min(test_input[i])-1 pq.insert(new_min) self.assertEqual(pq.find_top(), new_min) # Verify the value which is larger than the root of min heap # is the leaf node after it is inserted test_input = [ [2], [3, 4], [3, 5, 4], [16, 14, 7, 2, 8, 1], [12, 11, 6, 8, 10, 2, 3, 1], [8, 3, 1, 5, 6, 4, 2] ] for i in range(len(test_input)): pq = priority_queue.PriorityQueue(test_input[i]) pq.build_heap() new_max = max(test_input[i])+1 pq.insert(new_max) self.assertEqual(pq.queue[pq.queue_size-1], new_max)
def test_PriorityQueue_parameters(self): with self.assertRaises(Exception): priority_queue.PriorityQueue(supplied_iterable=423515342282) ''' Just call do_something(), and if an exception gets raised, the test automatically fails. This is also the reason why assertDoesNotRaise() does not exist. Reference: http://stackoverflow.com/questions/6181555/ pass-a-unit-test-if-an-exception-isnt-thrown ''' priority_queue.PriorityQueue() with self.assertRaises(Exception): priority_queue.PriorityQueue( supplied_iterable="Test string 12()3)\45")
def setUp(self): self.empty_priority_queue = priority_queue.PriorityQueue() # Test the supplied_iterable kwarg: def craft_iterable(): # 10, 20, 30, 40, 50, 60, 70, 80 for each_number in range(10, 81, 10): import random random_value = random.randint(400, 500) yield priority_queue.Node(random_value, each_number) self.populated_priority_queue = priority_queue.PriorityQueue( supplied_iterable=craft_iterable())
def __init__(self): self.nodes, self.connections, self.level, self.index = [], [], 0, 0 self.last_node = None # iterator stuff self.current_node = None self.queue = priority_queue.PriorityQueue() self.seen = [] self.directed = -1
def astar_search(waternet_graph, start_vertex, goal_vertex): current_node = AStarNode(start_vertex, None, None, 0) # Check if the start is the goal state if is_goal_state(current_node, goal_vertex): return construct_route(current_node) # Priority queue initialised with node and empty explored list unexplored_nodes = priority_queue.PriorityQueue() #priority doesn't matter, right? unexplored_nodes.push(current_node, 0) explored_nodes = [] # Perform AStar search until the unexplored nodes is empty. while not unexplored_nodes.isEmpty(): # Pop the current_node of the unexplored node list. current_node = unexplored_nodes.pop() explored_nodes.append(current_node) # Check if this current node is the goal node if is_goal_state(current_node, goal_vertex): return construct_route(current_node) # Get the neighbours of the current_node for current_neighbour, edge_id in waternet_graph.neighbours(current_node.vertex): #value = graph_helper.euclidean_distance(node.state, current_neighbour)*1000 current_edge = waternet_graph.edge(edge_id) if isinstance(edge_id, basestring): edge_id = int(edge_id.split("_")[0]) current_neighbour = AStarNode(current_neighbour, edge_id, current_node, (current_node.route_cost + current_node.vertex.euclidean_distance(current_neighbour) * current_edge.cost_function())) #initialise node with actual distance, successor pathcost + entire path cost # Check if the neighbour is in the explored explored nodes if is_explored_node(current_neighbour, explored_nodes): continue # Check if the neighbour already exists in the explored nodes list. is_unexplored_neighbour, current_unexplored_route_cost = is_unexplored_node(current_neighbour, goal_vertex, unexplored_nodes) # + amount(child.state, child.parent.state) distance = current_neighbour.route_cost # Check if it is not an unexplored node if not is_unexplored_neighbour: unexplored_nodes.push(current_neighbour, distance) elif current_neighbour.route_cost < current_unexplored_route_cost: # Replace in frontier heap only if the for unexplored_node in unexplored_nodes.heap: # Only check if the state of the unexplored node is equal to the current neighbour state. if(unexplored_node[2].vertex == current_neighbour.vertex): unexplored_nodes.heap.remove(unexplored_node) unexplored_nodes.push(current_neighbour, distance) break return False
def __init__(self): self.setup_options_and_constraints() self.queue = priority_queue.PriorityQueue() self.selector = motif_state_selector.default_selector() self.scorer = motif_state_search_scorer.MTSS_Astar() self.solutions = [] self.lookup = None self.test_node = None self.no_more_solutions = 0
def generic_a_star(self, moves, start, end, max_nodes=MAX_VALID_NODES): """ Performs an a* search on the map with the given set of moves queue implementation stores the state which consists of current position and estimated cost to end point. Based on the estimated cost the queue is sorted. The store is indexed on position and stores actual cost of reaching the point along with parent point. Note: end should be a valid point on the map for path to be found Args: moves: list of allowed moves at each point start: x and y coordinates of start point end: x and y coordinates of end point max_nodes: max number of valid nodes to process Returns: store: contains the all the states encountered with links to parent states it can be used to generate the path and the next step """ def total_cost(cur_pos, next_pos, end): return self.movement_cost(cur_pos, next_pos) + self.heuristic_cost( next_pos, end) queue = priority_queue.PriorityQueue() queue.push((self.heuristic_cost(start, end), start)) store = {start: (0, start)} while not queue.is_empty(): (_, cur_pos) = queue.pop() (cur_score, _) = store[cur_pos] if cur_pos == end or max_nodes <= 0: return store # return store on reaching end or when exhausted nodes next_moves = [cur_pos + move for move in moves] valid_moves = [ move for move in next_moves if self.is_valid_move(move) ] max_nodes -= len(valid_moves) possible_state = [(total_cost(cur_pos, move, end), move) for move in valid_moves] valid_state = [ (next_score, next_pos) for (next_score, next_pos) in possible_state if next_score <= cur_score + self.heuristic_cost(cur_pos, end) ] update_state = [ state for state in valid_state if state[1] not in store ] for (next_score, next_pos) in update_state: store[next_pos] = (cur_score + self.movement_cost(cur_pos, next_pos), cur_pos) queue.push((next_score, next_pos)) return {}
def testQueue(self): queue_a = priority_queue.PriorityQueue() queue_a.push(11) queue_a.push(2) queue_a.push(13) first_item_removed = queue_a.pop() queue_a.push(29) self.assertEqual(first_item_removed, 13) self.assertFalse(queue_a.is_empty()) self.assertEqual(queue_a.pop(), 29) self.assertEqual(queue_a.pop(), 11) self.assertEqual(queue_a.pop(), 2)
def test_delete(self): # Verify each node of the result keeps proper position # by checking heap property test_input = [ [1, 2, 3, 4, 5, 6, 7], [3, 4, 5], [4, 9, 8, 11, 12, 13, 15], ] for i in range(len(test_input)): pq = priority_queue.PriorityQueue(test_input[i]) pq.delete(1) check_priority_order(pq)
def test_heapify(self): # Verify each node of the result keeps proper position # by checking heap property test_input = [ [2], [5, 11, 8], [7, 5, 6, 11, 8, 9, 10], [8, 3, 1, 5, 6, 4, 2], ] for i in range(len(test_input)): pq = priority_queue.PriorityQueue(test_input[i]) pq.heapify(0) check_priority_order(pq)
def search_shortest_dist(graph, start, end): # Check if the graph is valid. If it is not, return error string. if graph is None: return 'graph is null: Function Failed' # Declare a queue and enqueue the starting node. q = priority_queue.PriorityQueue() q.put(start, 0) curr_cost = {} curr_cost[start] = 0 # Declare a string to count factory stops and int to count miles traversed # Set it to blank space. solution = '' miles_traveled = 0 nodes_expanded = 0 # While the q is not empty, get the next node on the queue # and check for the goal. If at the goal, return the # string of stops and the nodes expanded. Else, # expand the node to the queue. prev = None while not q.empty(): cur = q.get() if prev == cur: prev = cur cur = q.get() nodes_expanded = nodes_expanded + 1 end[1].add_comp(cur) end[2].add_comp(cur) end[3].add_comp(cur) end[4].add_comp(cur) end[5].add_comp(cur) if prev is not None and prev != cur: miles_traveled = miles_traveled + graph.cost(prev, cur) if cur != start: solution = solution + cur #the widgets are complete, return if end[1].done and end[2].done and end[3].done and end[4].done and end[ 5].done: return (solution, miles_traveled, nodes_expanded) #add the neighbors to the queue in order of their cost plus current cost for next in graph.neighbors(cur): update_cost = curr_cost[cur] + graph.cost(cur, next) #if the nighbor isn't in the queue or the new cost is less than it's original cost if next not in curr_cost or update_cost < curr_cost[next]: curr_cost[next] = update_cost prior = update_cost + dist_hist(graph.weights[cur], end, next, 5) q.put(next, prior) prev = cur # Return the failed because solution was not found solution = 'FAILED' miles_traveled = -1 return (solution, miles_traveled, nodes_expanded)
def search_smallest_stops(graph, start, end): # Check if the graph is valid. If it is not, return error string. if graph is None: return 'graph is null: Function Failed' # Declare a queue and enqueue the starting node. q = priority_queue.PriorityQueue() q.put(start, 0) curr_cost = {} curr_cost[start] = 0 # Declare a string to count factory stops # Set it to blank space. solution = '' nodes_expanded = 0 # While the q is not empty, get the next node on the queue # and check for the goal. If at the goal, copy the successful # path to the array of mazedata and then return 1. Else, # expand the node to the queue. prev = None while not q.empty(): cur = q.get() if prev == cur: prev = cur cur = q.get() nodes_expanded = nodes_expanded + 1 end[1].add_comp(cur) end[2].add_comp(cur) end[3].add_comp(cur) end[4].add_comp(cur) end[5].add_comp(cur) if cur != start: solution = solution + cur if end[1].done and end[2].done and end[3].done and end[4].done and end[ 5].done: return (solution, nodes_expanded) for next in graph.neighbors(cur): update_cost = curr_cost[cur] + graph.cost(cur, next) if next not in curr_cost or update_cost < curr_cost[next]: curr_cost[next] = update_cost prior = curr_cost[next] + stop_hist(end, next, 5) q.put(next, prior) prev = cur # Return the failed because solution was not found solution = 'FAILED: ' + solution return (solution, nodes_expanded)
def __iter__(self): if len(self.nodes) != 0: for n in self.nodes: avail_pos = n.available_children_pos() if len(avail_pos) != 0: self.current_node = n break if self.current_node is None: self.current_node = self.nodes[0] else: self.current_node = None self.seen = [self.current_node] self.queue = priority_queue.PriorityQueue() self.directed = -1 return self
def test_build_heap(self): # Verify each node of the result keeps proper position # by checking heap property test_input = [ [2], [3, 4], [3, 5, 4], [16, 14, 7, 2, 8, 1], [12, 11, 6, 8, 10, 2, 3, 1], [8, 3, 1, 5, 6, 4, 2] ] for i in range(len(test_input)): pq = priority_queue.PriorityQueue(test_input[i]) pq.build_heap() check_priority_order(pq)
def test_find_top(self): # Verify the result of find_top method in min heap is # same as the minimum of the array test_input = [ [2], [3, 4], [3, 5, 4], [16, 14, 7, 2, 8, 1], [12, 11, 6, 8, 10, 2, 3, 1], [8, 3, 1, 5, 6, 4, 2] ] for i in range(len(test_input)): pq = priority_queue.PriorityQueue(test_input[i]) pq.build_heap() self.assertEqual(pq.find_top(), min(test_input[i]))
def __init__(self, circuit): """Creates a simulation that will run on a pre-built circuit. The Circuit instance does not need to be completely built before it is given to the class constructor. However, it does need to be complete before the run method is called. Args: circuit: The circuit whose state transitions will be simulated. """ self.circuit = circuit self.in_transitions = [] self.queue = pq.PriorityQueue() self.probes = [] self.probe_all_undo_log = []
def test_extract_top(self): # Verify the result of find_top method in min heap is # same as the minimum of the array and it's properly removed test_input = [ [2], [3, 4], [3, 5, 4], [16, 14, 7, 2, 8, 1], [12, 11, 6, 8, 10, 2, 3, 1], [8, 3, 1, 5, 6, 4, 2] ] for i in range(len(test_input)): pq = priority_queue.PriorityQueue(test_input[i]) pq.build_heap() min_value = min(test_input[i]) self.assertEqual(pq.extract_top(), min_value) assert min_value not in pq.queue
def solve(self, problem): done = set() start = problem.startState() goal_state = None cache = {start:(0, start)} pq = priority_queue.PriorityQueue() pq.push(0, start) while not pq.is_empty(): cost_to, s = pq.pop() # once we pop a state skip it if s in done: continue done.add(s) # goal then must have traversed shortest path if problem.isGoal(s): self.cost, _ = cache[s] goal_state = s break # add each ns to pq if cost lower now for (a,ns,c) in problem.succAndCost(s): total = cost_to + c if ns not in cache: cache[ns] = (total, s) pq.push(total, ns) elif cache[ns][0] > total: cache[ns] = (total, s) pq.push(total, ns) # collect actions with backpointers self.actions = [goal_state] s = goal_state while s != start: ps = cache[s][1] self.actions.append(ps) s = ps self.actions.reverse()
def short_path(graph, start, end): queue = priority_queue.PriorityQueue( lambda x, y: x['distance'] < y['distance']) queue.enqueue({"name": start, "distance": 0, 'index': 0, 'path': ['A']}) seen = {} while len(queue): value = queue.dequeue() seen[value['name']] = True if value['name'] == end: value.pop('name') return value for node in graph[value['name']]: if node in seen: continue queue.enqueue({ 'name': node, 'distance': value['distance'] + graph[value['name']][node], 'path': value['path'] + [node] }) return None
def __init__(self): self.scheduling_queue = priority_queue.PriorityQueue() self.priority_level = {"high": 3, "mid": 2, "low": 1} self.flag_set = ["--once", "--daily", "--weekly", "--monthly"]
#!/usr/bin/env python """ A first template for a PID controller """ import sys sys.path.insert(0,'..') import priority_queue as pq ### Initializing queue # key and cost in range(0,5) print('Initializing queue...') queue = pq.PriorityQueue() for vals in range(0,5): queue.insert(vals,vals) # Print queue for elems in queue.queue: print('%3d %3d'%(elems.key,elems.cost)) ### Pop element print('\nExtracting min key...') min_val,idx = queue.min_extract() print('Min key: {}, Min cost: {}'.format(min_val,idx)) # Print queue for elems in queue.queue: print('%3d %3d'%(elems.key,elems.cost)) ### Check if is element print('\nChecking if 5 is a key: {}'.format(queue.is_member(5))) print('Checking if 3 is a key: {}'.format(queue.is_member(3))) ### Insert element
def test_compare(self): pq = priority_queue.PriorityQueue([3, 4, 2]) self.assertEqual(pq.compare(pq.queue[0], pq.queue[1]), True) self.assertEqual(pq.compare(pq.queue[1], pq.queue[2]), False)
def test_swap(self): pq = priority_queue.PriorityQueue([3, 4]) pq.swap(0,1) self.assertEqual(pq.queue, [4, 3])
def setUp(self): self.pq = priority_queue.PriorityQueue( ['T', 'N', 'R', 'P', 'H', 'O', 'A', 'E', 'I', 'G'])
#os.system('cls') print('\n') print(str(round(((time.clock() - StartTime) / 60.0), 2)), 'minutes elapsed') print(str(PMIDReadCount), 'results fetched,', str(round(((PMIDReadCount / float(NumOfPMIDs)) * 100.0), 2)), '% done') print('URL Fetch rate', str(URLFetchCount - dFetchRate), 'per', str(UpdateTimeInSec), 'sec,', str(URLFetchCount), 'total URLs fetched') print('Total Proxies:', str(len(ProxyQueue.ItemList))) ProxyQueue = pq.PriorityQueue() ListCount = 0 # Python list, each item is string 'facebook_count|twitter_count' skip_index = 0 # number of inputs PMIDs to ignore ListPMID = [] # Python list, each entry is a PMID as a string PMIDReadCount = skip_index # Number of PMIDs that have their count done so far URLFetchCount = 0 # Number of successful URLs requests so far NumOfPMIDs = 0 # Number of PMIDs StartTime = 0 # start time of our program on clock dFetchRate = 0 # delta fetch rate, to be used to trigger refresh of program performance stats after a specified time interval NameInputFile = 'pubmed_test.txt' NameOutputFile = NameInputFile[0:-4] + '_stats.' + NameInputFile[-3:] BaseURLPubmed = 'https://www.ncbi.nlm.nih.gov/pubmed/'
def solve(state): """ solves the 8-torus problem in the least amount of moves possible """ # run A* search algorithm goal_state = [1, 2, 3, 4, 5, 6, 7, 8, 0] # this is the desired state we wish to reach pq = priority_queue.PriorityQueue() # stores the given state as a dictionary and stores it in our priority queue state_dict = { 'state': state, 'h': calculate_heuristic(state), 'parent': None, 'g': 0, 'f': calculate_heuristic(state) } pq.enqueue(state_dict) found = False closed = [] # stores all 'closed' nodes goal_dict = None # will store the dictionary of the desired goal state # finds the goal state through the A* algorithm while not pq.is_empty(): # pops the min-cost element from the queue, adds it to closed, and stores the state curr_dict = pq.pop() curr_state = curr_dict['state'] add_to_closed(closed, curr_dict) # checks if goal_state is reached if curr_state == goal_state: goal_dict = curr_dict # stores the goal dictionary break # generates and enqueues the successor states as dictionaries succ_states = generate_succ(curr_state) for succ in succ_states: enqueue = True succ_dict = { 'state': succ, 'h': calculate_heuristic(succ), 'parent': curr_dict['state'], 'g': curr_dict['g'] + 1, 'f': calculate_heuristic(succ) + curr_dict['g'] + 1 } for closed_dict in closed: if (closed_dict['state'] == succ_dict['state']): if (closed_dict['f'] < succ_dict['f']): enqueue = False if enqueue: pq.enqueue(succ_dict) prev = goal_dict # this is our goal that we have reached moves = [prev] # stores the list of moves requires to solve the puzzle # performs the second half of the A* algorithm by going through the closed list to produce a a list of moves # required to reach the goal state num_moves = int(goal_dict['f']) for i in range(num_moves): for curr in closed: if curr['state'] == prev[ 'parent']: # finds the parent state within the of the current state prev = curr # sets the previous state to the parent (current) state moves.append( curr) # adds the current (parent) to the moves list break # reverses the list of moves into the correct order moves = reversed(moves) # prints out the desired results for x in moves: print str(x['state']) + " h= " + str(x['h']) + " moves: " + str(x['g']) print("Max queue length: " + str(pq.max_len))
self.name = name self.age = age def __eq__(self, other): return self.age == other.age def __lt__(self, other): return self.age < other.age def __repr__(self): return '(' + str(self.name) + '; ' + str(self.age) + ')' print("int MaxHeap:") lst = [] q_int = q.PriorityQueue(lst, key=max, seq_type=int) print("Checking is empty before assignment:", q_int.is_empty()) q_int.push(1) q_int.push(2) q_int.push(3) q_int.push(4) q_int.push(0) q_int.push(100) print("Checking is empty after assignment:", q_int.is_empty()) print("Heap size is", q_int.heap_size()) print(q_int, "is heap") peeked = q_int.peek() print(peeked, "is peeked element")
def a_star(self, mapdata, start, goal): ### REQUIRED CREDIT rospy.loginfo("Executing A* from (%d,%d) to (%d,%d)" % (start[0], start[1], goal[0], goal[1])) afrontier = priority_queue.PriorityQueue() afrontier.put(start, 0) came_from = {} cost_so_far = {} came_from[start] = None cost_so_far[start] = 0 path = [] the_frontier = GridCells() the_frontier.header.frame_id = "map" the_frontier.cell_width = mapdata.info.resolution the_frontier.cell_height = mapdata.info.resolution the_frontier.cells = [] while not afrontier.empty(): current = afrontier.get() #rospy.loginfo("at point" + str(current)) if current == goal: rospy.loginfo("found the goal") break current_x = current[0] current_y = current[1] for this_next in PathPlanner.neighbors_of_8(mapdata, current_x, current_y): new_cost = cost_so_far[current] + PathPlanner.euclidean_distance(current_x, current_y, this_next[0], this_next[1]) if this_next not in cost_so_far or new_cost < cost_so_far[this_next]: cost_so_far[this_next] = new_cost priority = new_cost + PathPlanner.euclidean_distance(this_next[0], this_next[1], goal[0], goal[1]) afrontier.put(this_next, priority) came_from[this_next] = current msg_Point = Point() world_point = PathPlanner.grid_to_world(mapdata, this_next[0], this_next[1]) msg_Point.x = world_point.x msg_Point.y = world_point.y msg_Point.z = 0 the_frontier.cells.append(msg_Point) self.frontier.publish(the_frontier) rospy.sleep(0.025) the_path = GridCells() the_path.header.frame_id = "map" the_path.cell_width = mapdata.info.resolution the_path.cell_height = mapdata.info.resolution the_path.cells = [] rospy.loginfo("shit's goin down") while 1 == 1: msg_Point = Point() world_point = PathPlanner.grid_to_world(mapdata, current[0], current[1]) msg_Point.x = world_point.x msg_Point.y = world_point.y msg_Point.z = 0 the_path.cells.append(msg_Point) path.append(current) previous_node = came_from[current] if previous_node == None: break current = previous_node rospy.loginfo(current) self.path.publish(the_path) return path
def a_star(self, mapdata, start, goal): """ Calculates the astar path. Publishes the list of cells that were added to the path. :param mapdata [OccupancyGrid] The map data. :param start [int8,int8] The x,y grid location of the starting location :param goal [int8,int8] The x,y grid location of the goal location :return [[int8]] The C-Space as a list of path values. """ rospy.loginfo("Executing A* from (%d,%d) to (%d,%d)" % (start[0], start[1], goal[0], goal[1])) frontier_cells = [] unoptimized_path = [] visited_cells = [] #initializes dictionaries and lists frontier = priority_queue.PriorityQueue() frontier.put(start, 0) came_from = {} came_from[start] = None cost_so_far = {} cost_so_far[start] = 0 #only when not empty while not frontier.empty(): current = frontier.get() #have we reached? if current == goal: break #current cell is turned into a Point class and added to the visited_cells list visited_cell = Point() world_pos_astar = PathPlanner.grid_to_world( mapdata, current[0], current[1]) visited_cell.x = world_pos_astar[0] visited_cell.y = world_pos_astar[1] visited_cell.z = 0 visited_cells.append(visited_cell) #finds neighbor list in order to start A* algorithm list_of_8_neighbors = PathPlanner.neighbors_of_8( mapdata, current[0], current[1]) #iterates through all 8 neighbors to determine cost at each node (f(n)) for next in list_of_8_neighbors: next_x = next[0] next_y = next[1] #h(n) cost from neighbor. Heuristic is euclidiean distance new_cost = cost_so_far[ current] + PathPlanner.euclidean_distance( next_x, next_y, current[0], current[1]) #checks to see how the cost of the next neighbor compares to the visited_cell if next not in cost_so_far or new_cost < cost_so_far[next]: #updates cost_so_far[] dictionary to include new neighbor cost_so_far[next] = new_cost priority = new_cost + PathPlanner.euclidean_distance( next_x, next_y, goal[0], goal[1]) frontier.put(next, priority) #generates list of frontier_cell Points to use in rviz frontier_cell = Point() world_pos_astar = PathPlanner.grid_to_world( mapdata, next[0], next[1]) frontier_cell.x = world_pos_astar[0] frontier_cell.y = world_pos_astar[1] frontier_cell.z = 0 frontier_cells.append(frontier_cell) came_from[next] = current #publishes frontier in rviz frontier_cells_message = GridCells() frontier_cells_message.header.frame_id = 'map' frontier_cells_message.cell_width = mapdata.info.resolution frontier_cells_message.cell_height = mapdata.info.resolution frontier_cells_message.cells = frontier_cells ## Return the C-space self.frontier.publish(frontier_cells_message) #generates list of visited cells and publishes to rviz visited_cells_message = GridCells() visited_cells_message.header.frame_id = 'map' visited_cells_message.cell_width = mapdata.info.resolution visited_cells_message.cell_height = mapdata.info.resolution visited_cells_message.cells = visited_cells ## Return the C-space self.expanded_cells.publish(visited_cells_message) node = goal #Iterates through came_from[] dictionary backwards to find A* path while node != start: node = came_from[node] unoptimized_path_cell = Point() world_pos_unoptimized_path = PathPlanner.grid_to_world( mapdata, node[0], node[1]) unoptimized_path_cell.x = world_pos_unoptimized_path[0] unoptimized_path_cell.y = world_pos_unoptimized_path[1] unoptimized_path_cell.z = 0 unoptimized_path.append(unoptimized_path_cell) # message is published and displayed in rviz unoptimized_path_message = GridCells() unoptimized_path_message.header.frame_id = 'map' unoptimized_path_message.cell_width = mapdata.info.resolution unoptimized_path_message.cell_height = mapdata.info.resolution unoptimized_path_message.cells = unoptimized_path ## Return the C-space return unoptimized_path