def a_star(grid,heuristic,grid_start,grid_goal):
path=[]
queue=PriorityQueue()
queue.put((0,grid_start))
visited=set(grid_start)
branch={}
found=False
while not queue.empty():
item=queue.get()
#print('item=',item[1])
current_cost=item[0]
current_node=item[1]
#print('cu=',current_node,' goal=',grid_goal)
if current_node==grid_goal:
print('Found a path.')
found=True
break
else:
for action in valid_actions(grid,current_node):
#get the tuple representation
da = action.delta
cost=action.cost
next_node=(current_node[0]+da[0],current_node[1]+da[1])
new_cost=current_cost+cost+heuristic(next_node,grid_goal)
if next_node not in visited:
visited.add(next_node)
queue.put((new_cost,next_node))
branch[next_node]=(new_cost,current_node,action)
n=next_node
#path=[]
#path_cost=0
if found:
#retrace steps
#path=[]
n=grid_goal
path_cost=branch[n][0]
path.append(grid_goal)
while branch[n][1] != grid_start:
path.append(branch[n][1])
n=branch[n][1]
#print('cu=',branch[n][1])
path.append(branch[n][1])
print('path=',path)
return path[::-1],path_cost
def a_star(graph, start, goal):
"""Modified A* to work with NetworkX graphs."""
path = []
queue = PriorityQueue()
queue.put((0, start))
visited = set(start)
branch = {}
found = False
while not queue.empty():
item = queue.get()
current_cost = item[0]
current_node = item[1]
if current_node == goal:
print('Found a path.')
found = True
break
else:
for next_node in graph[current_node]:
cost = graph.edges[current_node, next_node]['weight']
new_cost = current_cost + cost + heuristic(next_node, goal)
if next_node not in visited:
visited.add(next_node)
queue.put((new_cost, next_node))
branch[next_node] = (new_cost, current_node)
path = []
path_cost = 0
if found:
# retrace steps
path = []
n = goal
path_cost = branch[n][0]
path.append(goal)
while branch[n][1] != start:
path.append(branch[n][1])
n = branch[n][1]
path.append(branch[n][1])
return path[::-1], path_cost
def plan_path(self, id):
self.flight_state = States.PLANNING
print("Planning...")
TARGET_ALTITUDE = 6
SAFETY_DISTANCE = 7.0
if id == 0:
# Initial
# Read lat0, lon0 from colliders into floating point values
with open('colliders.csv', newline='') as file:
line = csv.reader(file)
(lat0, lon0) = next(line)
lat0, lon0 = lat0[5:], lon0[5:]
# Set home position to (lat0, lon0, 0)
print("Setting home position:", lat0, lon0, 0.)
self.set_home_position(float(lon0), float(lat0),
0.) # NOTE THE ORDER!!!
# Retrieve current global position
print("Current global position:", self.global_position)
print("Verify:", self._longitude, self._latitude, self._altitude)
print('global home:', self.global_home)
print('global position:', self.global_position)
print('local position:', self.local_position)
if self.last_TARGET_ALTITUDE is None:
if self.local_position[2] < 0.0:
self.last_TARGET_ALTITUDE = int(-self.local_position[2] +
SAFETY_DISTANCE + 1)
else:
self.last_TARGET_ALTITUDE = TARGET_ALTITUDE
# Read in obstacle map
self.data = np.loadtxt('colliders.csv',
delimiter=',',
dtype='Float64',
skiprows=2)
# Create a 2D grid object
self.grid = Grid(self.data)
self.north_offset, self.east_offset = self.grid.get_offset()
self.grid.update_obstacles(SAFETY_DISTANCE)
print("Offset north: {0}, east: {1}".format(
self.north_offset, self.east_offset))
# Read in graph (map of city intersections)
self.graph = np.loadtxt('graph.csv',
delimiter=',',
dtype='Float64',
skiprows=0)
# Read in itinerary (list of points to visit)
self.itinerary = np.loadtxt('itinerary.csv',
delimiter=',',
dtype='Float64',
skiprows=1)
self.number_of_stops = len(self.itinerary)
# Note: to set a specific goal, visit the top of this class
if self.goal is not None:
# Itinerary override: targeting specified goal instead
# For safety, snap to closest itinerary item
if self.snap:
self.itinerary = self.snap_to_closest(self.goal)
else:
self.itinerary = np.array([[self.goal[0], self.goal[1]]])
self.number_of_stops = 1
self.visit_first = 0
elif self.visit_first is None and not self.randomize_itin:
# no priority on itin item
self.last_id = -1
# Set flight altitude to be current height + TARGET_ALTITUDE
# in case drone is on top of a building
self.target_position[2] = int(-self.local_position[2] +
TARGET_ALTITUDE)
print("Take off target altitude:", self.target_position[2])
if id == 0 and self.visit_first is not None:
next_id = self.visit_first
self.last_id = next_id
else:
if self.randomize_itin:
next_id = randint(0,
len(self.itinerary) -
1) # get another itinerary destination
while next_id == self.last_id:
next_id = randint(0,
len(self.itinerary) -
1) # oops, same one, try again
self.last_id = next_id
else:
next_id = self.last_id + 1
if next_id > self.number_of_stops - 1: # loop back to first item
next_id = 0
self.last_id = next_id
print(" ")
print(
"###############################################################################"
)
print("Time stamp:", strftime("%Y%m%d.%H%M%S", gmtime()))
print("Stop number: {0} of {1}".format(id + 1, self.number_of_stops))
print("Itinerary index number:", next_id)
# set up the target
local_target = global_to_local(
np.array(
[self.itinerary[next_id, 1], self.itinerary[next_id, 0], 0.0]),
self.global_home)
landing_altitude = self.grid.get_landing_altitude(
int(local_target[0]) - self.north_offset,
int(local_target[1]) - self.east_offset)
target = [
self.itinerary[next_id, 1], self.itinerary[next_id, 0],
-landing_altitude
]
print("Global target:", target)
target_distance = heuristic(self.local_position, local_target)
print("Distance to target:", target_distance)
if self.graph_mode and target_distance > GRAPH_THRESHOLD:
print("Attempting compound path...")
waypoints = self.plan_compound_path(target, TARGET_ALTITUDE,
SAFETY_DISTANCE)
self.last_TARGET_ALTITUDE = TARGET_ALTITUDE
else:
print("Planning direct path...")
waypoints = self.plan_simple_path(target, TARGET_ALTITUDE,
SAFETY_DISTANCE)
# Set self.waypoints for a queue of points to follow toward goal
self.waypoints = waypoints
# Send waypoints to sim for visualization
self.send_waypoints()
def plan_path(self):
print("Searching for a path ...")
TARGET_ALTITUDE = 5.0
# read lat0, lon0 from colliders into floating point values
with open('colliders.csv') as f:
first_line = f.readline().split(',')
lat0, lon0 = float(first_line[0].split(' ')[-1]), float(
first_line[1].split(' ')[-1])
# set home position to (lon0, lat0, 0)
self.set_home_position(
lon0, lat0,
0) # set the current location to be the home position
# Convert drone global position to local position (relative to global home) using global_to_local()
local_position = global_to_local(self.global_position,
self.global_home)
print('global home {0}, position {1}, local position {2}'.format(
self.global_home, self.global_position, self.local_position))
# Load precomputed graph
print('Loading graph {0}'.format(len(self.graph.nodes)))
# Set grid start position at our current drone global position
drone_location = (self.local_position[0] - self.north_offset,
self.local_position[1] - self.east_offset,
self.local_position[2])
print('Drone location local: {0}, Drone location map: {1}'.format(
self.local_position, drone_location))
# Select the nearest node closest to the drone location
nearest_start = None
closest_distance = sys.float_info.max
for n in self.graph.nodes:
# heuristic is the Euclidean distance:
distance = heuristic(drone_location, n)
if distance < closest_distance:
closest_distance = distance
nearest_start = n
if nearest_start == None:
print('Error while getting closest starting node')
return
print('Found starting node = {0}'.format(nearest_start))
# Select a random goal node
rnd_goal = np.random.randint(len(self.graph.nodes))
goal = list(self.graph.nodes)[rnd_goal]
print('Selecting random goal[{0}]: {1}'.format(rnd_goal, goal))
print('****')
path, cost = a_star_graph(self.graph, heuristic, nearest_start, goal)
print('A* from {0} to {1} with a length of: {2}'.format(
nearest_start, goal, len(path)))
print('A* path: ', path)
# First waypoint of path
curr = path[0]
# Save height for Takeoff
self.target_position[2] = curr[2]
# Add first waypoint
waypoints = [[curr[0], curr[1], curr[2], 0]]
idx = 1
while idx < len(path):
prev_wp = waypoints[len(waypoints) - 1]
# idx indicate a good waypoint candidate to be inserted
# path[idx] is supposed to be a good candidate already for the A* construction
# Check following waypoints
while idx + 1 < len(path):
candidate_wp = path[idx + 1]
hit = False
cells = list(
bresenham(int(prev_wp[0]), int(prev_wp[1]),
int(candidate_wp[0]), int(candidate_wp[1])))
for c in cells:
# Check if we're in collision
if self.collision_grid[
c[0], c[1]] >= FLYING_ALTITUDE + SAFETY_DISTANCE:
hit = True
break
# If the path does not hit on obstacle, add it to the list
if not hit:
# It's a good candidate. Update idx
idx = idx + 1
else:
break
# Add the candidate using idx
good_candidate = path[idx]
curr_wp = [
good_candidate[0], good_candidate[1], good_candidate[2], 0
]
# Set heading of curr_wp based on relative position to prev_wp
heading = np.arctan2((curr_wp[1] - prev_wp[1]),
(curr_wp[0] - prev_wp[0]))
curr_wp[3] = heading
# Append it to waypoints
waypoints.append(curr_wp)
idx = idx + 1
# Set self.waypoints
self.waypoints = waypoints
#self.send_waypoints()
self.flight_state = States.PLANNING
def perform_astar(Gr, Cg, nmin=0, emin=0):
#drone_location = (-emin, -nmin, 5.0) # map coordinates
drone_location = (445.04762260615826, 315.94609723985195, 5.0)
print('Find Start node from {0}'.format(drone_location))
nearest_start = None
closest_distance = sys.float_info.max
for n in Gr.nodes:
# heuristic is the Euclidean distance:
distance = heuristic(drone_location, n)
if distance < closest_distance:
closest_distance = distance
nearest_start = n
if nearest_start == None:
print('Error while getting closest starting node')
return
print('Found starting node = {0}'.format(nearest_start))
##########
goal_location = (240.7685, 360.76114, 5.0) # map coordinates
print('Find Goal node from {0}'.format(goal_location))
nearest_goal = None
closest_distance = sys.float_info.max
for n in Gr.nodes:
# heuristic is the Euclidean distance:
distance = heuristic(goal_location, n)
if distance < closest_distance:
closest_distance = distance
nearest_goal = n
################
start = nearest_start
print('Start: ', start)
goal = nearest_goal
print('Goal: ', goal)
path, cost = a_star_graph(Gr, heuristic, start, goal)
print(len(path), path)
if len(path) == 0:
return
waypoints = [[p[0], p[1], p[2], 0] for p in path]
print("start")
fig = plt.figure(figsize=(10,10))
plt.imshow(Cg, cmap='Greys', origin='lower')
path_pairs = zip(waypoints[:-1], waypoints[1:])
for (n1, n2) in path_pairs:
plt.plot([n1[1], n2[1]], [n1[0], n2[0]], 'green')
plt.scatter(drone_location[0], drone_location[1], c='blue') # (0,0)
plt.scatter(emin - emin, nmin - nmin , c='green') # Lowest point
plt.scatter(100, 0, c='purple') # (0,0)
plt.xlabel('EAST')
plt.ylabel('NORTH')
plt.show()