def plan_path(self):
self.flight_state = States.PLANNING
print("Searching for a path ...")
# TODO: read lat0, lon0 from colliders into floating point values
coord = read_global_home("colliders.csv")
# TODO: set home position to (lat0, lon0, 0)
self.set_home_position(*coord)
# TODO: retrieve current global position
# TODO: convert to current local position using global_to_local()
current_local_pos = 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))
print("current local position {}".format(current_local_pos))
start = current_local_pos
goal = global_to_local(self.global_goal, self.global_home)
self.planner.plan_path(start, goal)
self.send_raw_waypoints()
self.get_waypoints()
def plan_path(self):
self.flight_state = States.PLANNING
print("Searching for a path ...")
TARGET_ALTITUDE = 5
SAFETY_DISTANCE = 8
self.target_position[2] = TARGET_ALTITUDE
# TODO: read lat0, lon0 from colliders into floating point values
with open('colliders.csv') as f:
home_pos_data = f.readline().split(",")
lat0 = float(home_pos_data[0].strip().split(" ")[1])
lon0 = float(home_pos_data[1].strip().split(" ")[1])
print(lat0, lon0)
# TODO: set home position to (lon0, lat0, 0)
self.set_home_position(lon0, lat0, 0)
# TODO: retrieve current global position
global_pos = [self._longitude, self._latitude, self._altitude]
# TODO: convert to current local position using global_to_local()
local_pos = global_to_local(global_pos, self.global_home)
print('global home {0}, position {1}, local position {2}'.format(self.global_home, self.global_position,
self.local_position))
# Read in obstacle map
data = np.loadtxt('colliders.csv', delimiter=',', dtype='Float64', skiprows=2)
# Define a grid for a particular altitude and safety margin around obstacles
grid, north_offset, east_offset = create_grid(data, TARGET_ALTITUDE, SAFETY_DISTANCE)
print("North offset = {0}, east offset = {1}".format(north_offset, east_offset))
# Define starting point on the grid (this is just grid center)
grid_start = (-north_offset, -east_offset)
# TODO: convert start position to current position rather than map center
grid_start = (int(local_pos[0]-north_offset), int(local_pos[1]- east_offset))
# Set goal as some arbitrary position on the grid
grid_goal = (-north_offset + 10, -east_offset + 10)
# TODO: adapt to set goal as latitude / longitude position and convert
goal_pos = global_to_local([-122.400886, 37.795174, self._altitude], self.global_home)
grid_goal = (int(goal_pos[0]-north_offset), int(goal_pos[1]-east_offset))
# Run A* to find a path from start to goal
# TODO: add diagonal motions with a cost of sqrt(2) to your A* implementation
# or move to a different search space such as a graph (not done here)
print('Local Start and Goal: ', grid_start, grid_goal)
path, _ = a_star(grid, heuristic, grid_start, grid_goal)
# TODO: prune path to minimize number of waypoints
path = prune_path(path)
# TODO (if you're feeling ambitious): Try a different approach altogether!
# Convert path to waypoints
waypoints = [[p[0] + north_offset, p[1] + east_offset, TARGET_ALTITUDE, 0] for p in path]
# Set self.waypoints
self.waypoints = waypoints
# TODO: send waypoints to sim (this is just for visualization of waypoints)
self.send_waypoints()
def calculate_waypoints(global_start, global_goal, global_home, data,
drone_altitude, safety_distance, nSamples, k):
# Calculate graph and offsets
grid, north_offset, east_offset = create_grid(data, drone_altitude,
safety_distance)
collision_free_nodes = create_nodes(data, nSamples, drone_altitude,
safety_distance)
graph = create_graph(data, collision_free_nodes, k, safety_distance)
local_position = global_to_local(global_start, global_home)
graph_start = closest_point(graph, local_position)
local_goal = global_to_local(global_goal, global_home)
graph_goal = closest_point(graph, local_goal)
# Find path
a = time.time()
path, path_cost = a_star(graph, graph_start, graph_goal)
b = time.time()
print('A* search time:', b - a)
#print('path: ', path)
print('path cost: ', path_cost)
print('path length: ', len(path))
pruned_path = collinearity_prune(path, epsilon=1e-4)
return [[int(p[0]), int(p[1]), math.ceil(p[2]), 0]
for p in path], path, pruned_path, graph, grid
def calculate_waypoints(global_start, global_goal, global_home, data, drone_altitude, safety_distance):
"""
Calculates the waypoints for the trajectory from `global_start` to `global_goal`.
Using `global_home` as home and colliders `data`.
"""
# Calculate graph and offsets
graph, north_offset, east_offset = create_graph(data, drone_altitude, safety_distance)
map_offset = np.array([north_offset, east_offset, .0])
# Convert start position from global to local.
local_position = global_to_local(global_start, global_home) - map_offset
# Find closest point to the graph for start
graph_start = closest_point(graph, local_position)
# Convert goal postion from global to local
local_goal = global_to_local(global_goal, global_home) - map_offset
# Find closest point to the graph for goal
graph_goal = closest_point(graph, local_goal)
# Find path
path, _ = a_star(graph, graph_start, graph_goal)
path.append(local_goal)
# Prune path
path = collinearity_prune(path, epsilon=1e-3)
# Calculate waypoints
return [[int(p[0] + north_offset), int(p[1] + east_offset), drone_altitude, 0] for p in path]
def plan_path(self):
self.flight_state = States.PLANNING
print("Searching for a path ...")
TARGET_ALTITUDE = 5
SAFETY_DISTANCE = 5
self.target_position[2] = TARGET_ALTITUDE
# Read lat0, lon0 from colliders into floating point values
lat0, lon0 = read_home('colliders.csv')
# Set home position to (lon0, lat0, 0)
self.set_home_position(lon0, lat0, 0)
# Convert to current local position using global_to_local()
current_position = [self._longitude, self._latitude, self._altitude]
self._north, self._east, self._down = global_to_local(
current_position, self.global_home)
print('global home: {0}, global pos: {1}, local pos: {2}'.format(
self.global_home, self.global_position, self.local_position))
# Read in obstacle map
data = np.loadtxt('colliders.csv',
delimiter=',',
dtype='Float64',
skiprows=2)
# Define a grid for a particular altitude and safety margin around obstacles
grid, north_offset, east_offset = create_grid(data, TARGET_ALTITUDE,
SAFETY_DISTANCE)
print("North offset: {0}, East offset: {1}".format(
north_offset, east_offset))
# Define starting point on the grid
grid_start = (int(np.ceil(-north_offset + self.local_position[0])),
int(np.ceil(-east_offset + self.local_position[1])))
# Set goal as some arbitrary position on the grid
local_target_position = global_to_local(self.global_target_position,
self.global_home)
grid_goal = (int(np.ceil(-north_offset + local_target_position[0])),
int(np.ceil(-east_offset + local_target_position[1])))
# Run A* to find a path from start to goal
print('Local Start and Goal: ', grid_start, grid_goal)
path, _ = a_star(grid, heuristic, grid_start, grid_goal)
# Prune path to minimize number of waypoints
pruned_path = prune_path(path)
print('Length of pruned path: {}'.format(len(pruned_path)))
# Convert path to waypoints
waypoints = [[
p[0] + north_offset, p[1] + east_offset, TARGET_ALTITUDE, 0
] for p in path]
# Set self.waypoints
self.waypoints = waypoints
# Send waypoints to sim (this is just for visualization of waypoints)
self.send_waypoints()
def plan_path(self):
self.flight_state = States.PLANNING
print("Searching for a path ...")
TARGET_ALTITUDE = 10
SAFETY_DISTANCE = 5
GOAL_LAT = 37.797638
GOAL_LON = -122.394808
self.target_position[2] = TARGET_ALTITUDE
# TODO: read lat0, lon0 from colliders into floating point values
data = np.loadtxt('colliders.csv', delimiter=',', dtype='U16', usecols = (0, 1))
lat0 = np.float64(data[0, 0].split(' ')[1])
lon0 = np.float64(data[0, 1].split(' ')[2])
# TODO: set home position to (lon0, lat0, 0)
self.set_home_position(lon0, lat0, 0)
# TODO: retrieve current global position
global_pos = self.global_position
# TODO: convert to current local position using global_to_local()
local_pos = global_to_local(global_pos, self.global_home)
print('global home {0}, position {1}, local position {2}'.format(self.global_home, self.global_position,
self.local_position))
# Read in obstacle map
data = np.loadtxt('colliders.csv', delimiter=',', dtype='Float64', skiprows = 2)
# Define a grid for a particular altitude and safety margin around obstacles
grid, north_offset, east_offset = create_grid(data, TARGET_ALTITUDE, SAFETY_DISTANCE)
print("North offset = {0}, east offset = {1}".format(north_offset, east_offset))
# Define starting point on the grid (this is just grid center)
# TODO: convert start position to current position rather than map center
grid_start = (int(-north_offset + local_pos[0]), int(-east_offset + local_pos[1]))
# Set goal as some arbitrary position on the grid
# TODO: adapt to set goal as latitude / longitude position and convert
local_goal = global_to_local([GOAL_LON, GOAL_LAT, 0], self.global_home)
grid_goal = (int(-north_offset + local_goal[0]), int(-east_offset + local_goal[1]))
# Run A* to find a path from start to goal
# TODO: add diagonal motions with a cost of sqrt(2) to your A* implementation
# or move to a different search space such as a graph (not done here)
print('Local Start and Goal: ', grid_start, grid_goal)
path, _ = a_star(grid, heuristic, grid_start, grid_goal)
# TODO: prune path to minimize number of waypoints
# TODO (if you're feeling ambitious): Try a different approach altogether!
pruned_path = copy.deepcopy(path)
for i in range(1, len(path) - 1):
p1 = path[i-1]
p2 = path[i]
p3 = path[i+1]
det = p1[0]*(p2[1] - p3[1]) + p2[0]*(p3[1] - p1[1]) + p3[0]*(p1[1] - p2[1])
if det == 0:
pruned_path.remove(p2)
path = pruned_path
# Convert path to waypoints
waypoints = [[p[0] + north_offset, p[1] + east_offset, TARGET_ALTITUDE, 0] for p in path]
# Set self.waypoints
self.waypoints = waypoints
# TODO: send waypoints to sim (this is just for visualization of waypoints)
self.send_waypoints()
def plan_path(self):
self.flight_state = States.PLANNING
print("Searching for a path ...")
TARGET_ALTITUDE = 5
SAFETY_DISTANCE = 7
self.target_position[2] = TARGET_ALTITUDE
# TODO: read lat0, lon0 from colliders into floating point values
fileptr = open('colliders.csv')
latitudeLongitudeArr = fileptr.readline().rstrip().replace('lat0','').replace('lon0','').split(',')
lat0 = float(latitudeLongitudeArr[0])
lon0 = float(latitudeLongitudeArr[1])
# TODO: set home position to (lon0, lat0, 0)
self.set_home_position(lon0, lat0, 0)
# TODO: retrieve current global position
global_position = self.global_position
# TODO: convert to current local position using global_to_local()
north, east, down = global_to_local(global_position,self.global_home)
print('global home {0}, position {1}, local position {2}'.format(self.global_home, self.global_position,
self.local_position))
# Read in obstacle map
data = np.loadtxt('colliders.csv', delimiter=',', dtype='Float64', skiprows=2)
# Define a grid for a particular altitude and safety margin around obstacles
grid, north_offset, east_offset = create_grid(data, TARGET_ALTITUDE, SAFETY_DISTANCE)
print("North offset = {0}, east offset = {1}".format(north_offset, east_offset))
# Define starting point on the grid (this is just grid center)
grid_start = (int(np.ceil(north - north_offset)), int(np.ceil(east - east_offset)))
# TODO: convert start position to current position rather than map center
if len(self.target_global_position) == 0:
lon0t = (lon0 + self.targetlongitudeDelta)
lat0t = (lat0 + self.targetlatitudeDelta)
print('Target Longitude and latitude: ', lon0t, lat0t)
northg, eastg, downg = global_to_local([lon0t,lat0t,self.global_home[2]],self.global_home)
else:
self.target_global_position[2] = self.global_home[2]
northg, eastg, downg = global_to_local(self.target_global_position,self.global_home)
# Set goal as some arbitrary position on the grid
grid_goal = (int(np.ceil(northg - north_offset)),int(np.ceil(eastg - east_offset)))
# TODO: adapt to set goal as latitude / longitude position and convert
# Run A* to find a path from start to goal
# TODO: add diagonal motions with a cost of sqrt(2) to your A* implementation
# or move to a different search space such as a graph (not done here)
print('Local Start and Goal: ', grid_start, grid_goal)
path, _ = a_star(grid, heuristic, grid_start, grid_goal)
# TODO: prune path to minimize number of waypoints
# TODO (if you're feeling ambitious): Try a different approach altogether!
path = collinearity_prune(path)
# Convert path to waypoints
waypoints = [[p[0] + north_offset, p[1] + east_offset, TARGET_ALTITUDE, 0] for p in path]
# Set self.waypoints
self.waypoints = waypoints
# TODO: send waypoints to sim (this is just for visualization of waypoints)
self.send_waypoints()
def planning_transition(self):
print('planning_transition')
self.flight_state = States.PLANNING
print('Loading map...')
self.planner.load_map()
pos = self.planner.home_gps_pos
self.set_home_position(pos.lon, pos.lat, pos.altitude)
print("Setting home to : ", pos)
print("global_home: ", self.global_home)
# local position of the drone
current_global = self.global_position
current_local = global_to_local(current_global, self.global_home)
start = current_local
print('Local pos: ', current_local)
#goal = (-self.planner.north_min + 10, -self.planner.east_min + 10)
goal_gps = GpsLocation(37.79489867, -122.39687191, 0)
goal = global_to_local(goal_gps, self.global_home)
goal = np.array([int(goal[0]), int(goal[1]), 0])
print('Drone will fly {} to {}'.format(start, goal))
# get a pruned, a_starred path from the planner
print('Finding a plan...')
self.path2d_with_cost, self.landing_waypoint_index = self.planner.plan_route(
start, goal)
# fig = plt.figure()
# grid = self.planner.create_grid()
# plt.imshow(grid, cmap='Greys', origin='lower')
# plt.scatter(start[1] - self.planner.east_min, start[0] - self.planner.east_min, color='blue')
# plt.scatter(goal[1] - self.planner.east_min, goal[0] - self.planner.east_min, color='green')
# plt.show()
if len(self.path2d_with_cost) > 0:
print('Plan found')
self.all_waypoints = np.array(self.path2d_with_cost)[:, :3]
self.send_waypoints()
self.plan_status = PlanResult.PLAN_SUCCESS
else:
print("Could not plan a path for the given start and goal state")
self.plan_status = PlanResult.PLAN_FAILED
self.waypoint_hit_radius = self.WAYPOINT_HIT_RADIUS_DURING
def plan_path(self):
self.flight_state = States.PLANNING
print("Searching for a path ...")
TARGET_ALTITUDE = 5
SAFETY_DISTANCE = 5
self.target_position[2] = TARGET_ALTITUDE
colliders_file = 'colliders.csv'
# TODO: read lat0, lon0 from colliders into floating point values
lat0, lon0 = read_home(colliders_file)
print(f'Home lat : {lat0}, lon : {lon0}')
# # # TODO: set home position to (lat0, lon0, 0)
self.set_home_position(lon0, lat0, 0)
# TODO: retrieve current global position
local_north, local_east, local_down = global_to_local(self.global_position, self.global_home)
print(f'Local => north : {local_north}, east : {local_east}, down : {local_down}')
# TODO: convert to current local position using global_to_local()
print('global home {0}, position {1}, local position {2}'.format(self.global_home, self.global_position,
self.local_position))
# Read in obstacle map
data = np.loadtxt(colliders_file, delimiter=',', dtype='Float64', skiprows=3)
# Define a grid for a particular altitude and safety margin around obstacles
grid, north_offset, east_offset = create_grid(data, TARGET_ALTITUDE, SAFETY_DISTANCE)
print("North offset = {0}, east offset = {1}".format(north_offset, east_offset))
# Define starting point on the grid (this is just grid center)
grid_start_north = int(np.ceil(local_north - north_offset))
grid_start_east = int(np.ceil(local_east - east_offset))
grid_start = (grid_start_north, grid_start_east)
# TODO: convert start position to current position rather than map center
# Set goal as some arbitrary position on the grid
goal_north, goal_east, goal_alt = global_to_local(self.goal_global_position, self.global_home)
grid_goal = (int(np.ceil(goal_north - north_offset)), int(np.ceil(goal_east - east_offset)))
# TODO: adapt to set goal as latitude / longitude position and convert
# Run A* to find a path from start to goal
# TODO: add diagonal motions with a cost of sqrt(2) to your A* implementation
# or move to a different search space such as a graph (not done here)
print('Local Start and Goal: ', grid_start, grid_goal)
path, _ = a_star(grid, heuristic, grid_start, grid_goal)
# TODO: prune path to minimize number of waypoints
path = collinearity_prune(path)
# TODO (if you're feeling ambitious): Try a different approach altogether!
# Convert path to waypoints
waypoints = [[int(p[0] + north_offset), int(p[1] + east_offset), TARGET_ALTITUDE, 0] for p in path]
# Set self.waypoints
self.waypoints = waypoints
# TODO: send waypoints to sim
self.send_waypoints()
def plan_path(self):
self.flight_state = States.PLANNING
print("Searching for a path ...")
TARGET_ALTITUDE = 5
SAFETY_DISTANCE = 5
self.target_position[2] = TARGET_ALTITUDE
lat_lon_data = np.loadtxt('colliders.csv', usecols=(0, 1), delimiter=',', dtype='str')
# read lat0, lon0 from colliders into floating point values
lat0 = float(lat_lon_data[0,0][5:])
lon0 = float(lat_lon_data[0,1][5:])
# set home position to (lon0, lat0, 0)
self.set_home_position(lon0, lat0, 0)
# retrieve current global position
current_global_position = self.global_position
# convert to current local position using global_to_local()
current_local_position = global_to_local(current_global_position, self.global_home)
print("current local position: ", current_local_position[0], " ", current_local_position[1])
print('global home {0}, position {1}, local position {2}'.format(self.global_home, self.global_position,
self.local_position))
# Read in obstacle map
data = np.loadtxt('colliders.csv', delimiter=',', dtype='Float64', skiprows=2)
# Define a grid for a particular altitude and safety margin around obstacles
grid, north_offset, east_offset = create_grid(data, TARGET_ALTITUDE, SAFETY_DISTANCE)
print("North offset = {0}, east offset = {1}".format(north_offset, east_offset))
# convert start position to current position rather than map center
grid_start = (int(current_local_position[0])-north_offset, int(current_local_position[1])-east_offset)
# set goal as latitude / longitude position and convert
lon_goal = -122.3961
lat_goal = 37.7940
goal_global = np.array([lon_goal, lat_goal, 0])
goal_local = global_to_local(goal_global, self.global_home)
grid_goal = (int(goal_local[0])-north_offset, int(goal_local[1])-east_offset)
# Run A* to find a path from start to goal
print('Local Start and Goal: ', grid_start, grid_goal)
path, _ = a_star(grid, heuristic, grid_start, grid_goal)
# prune path to minimize number of waypoints
pruned_path = prune_path(path)
# Convert path to waypoints
waypoints = [[p[0] + north_offset, p[1] + east_offset, TARGET_ALTITUDE, 0] for p in pruned_path]
# Set self.waypoints
self.waypoints = waypoints
# send waypoints to sim (this is just for visualization of waypoints)
self.send_waypoints()
def plan_path(self):
print("Searching for a path ...")
self.flight_state = States.PLANNING
self.target_position[2] = self.TARGET_ALTITUDE
# Read lat0, lon0 from colliders into floating point values, and set home position
with open('colliders.csv', 'r') as f:
latlon_string = f.readline()
f.close()
m = re.search('lat0 (?P<lat>-*\d+.\d+), lon0 (?P<lon>-*\d+.\d+)\n',
latlon_string)
lat0, lon0 = [m.group('lat'), m.group('lon')]
self.set_home_position(lon0, lat0, 0)
# Convert current posiiton to local frame
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))
# Read in obstacle map
data = np.loadtxt('colliders.csv',
delimiter=',',
dtype='Float64',
skiprows=2)
# Define a grid for a particular altitude and safety margin around obstacles
grid, north_offset, east_offset = create_grid(data,
self.TARGET_ALTITUDE,
self.SAFETY_DISTANCE)
print("North offset = {0}, east offset = {1}".format(
north_offset, east_offset))
# Define starting point on the grid (this is just grid center)
grid_start = local_to_grid(local_position, north_offset, east_offset)
# Set goal as some arbitrary position on the grid
local_goal = global_to_local(self.GOAL_LATLON, self.global_home)
grid_goal = local_to_grid(local_goal, north_offset, east_offset)
# Run A* to find a path from start to goal
print('Local Start and Goal: ', grid_start, grid_goal)
path, _ = a_star(grid, heuristic, grid_start, grid_goal)
pruned_path = prune(path)
# Convert path to waypoints
print("Path computed, publishing waypoints...")
waypoints = [[
p[0] + north_offset, p[1] + east_offset, self.TARGET_ALTITUDE, 0
] for p in pruned_path]
# Set self.waypoints
self.waypoints = waypoints
self.send_waypoints()
def plan_path(self):
self.flight_state = States.PLANNING
print("Searching for a path ...")
TARGET_ALTITUDE = 5
SAFETY_DISTANCE = 5
self.target_position[2] = TARGET_ALTITUDE
# TODO: read lat0, lon0 from colliders into floating point values
file = open('colliders.csv', 'r')
reader = csv.reader(file)
header = next(reader)
lat0, lon0 = [float(header[i].split()[1]) for i, j in enumerate(header)]
# TODO: set home position to (lon0, lat0, 0)
global_home = (lon0, lat0, 0)
print("global home is ", global_home)
self.set_home_position(lon0, lat0, 0)
# TODO: retrieve current global position
global_position = (self._longitude, self._latitude, self._altitude)
# print(global_position)
# TODO: convert to current local position using global_to_local()
local_position = global_to_local(global_position, global_home)
local_north, local_east, local_down = local_position
print(local_position)
print('global home {0}, position {1}, local position {2}'.format(self.global_home, self.global_position,
self.local_position))
# Read in obstacle map
data = np.loadtxt('colliders.csv', delimiter=',', dtype='Float64', skiprows=2)
# Define a grid for a particular altitude and safety margin around obstacles
grid, north_offset, east_offset = create_grid(data, TARGET_ALTITUDE, SAFETY_DISTANCE)
print("North offset = {0}, east offset = {1}".format(north_offset, east_offset))
# Define starting point on the grid (this is just grid center)
# TODO: convert start position to current position rather than map center
grid_start = (-north_offset + int(local_north), -east_offset + int(local_east))
# Set goal as some arbitrary position on the grid
# TODO: adapt to set goal as latitude / longitude position and convert
goal_north, goal_east, goal_alt = global_to_local([-122.400150, 37.796005, TARGET_ALTITUDE], self.global_home)
grid_goal = (int(np.ceil((-north_offset + goal_north))), int(np.ceil(-east_offset + goal_east)))
# Run A* to find a path from start to goal
# TODO: add diagonal motions with a cost of sqrt(2) to your A* implementation
# or move to a different search space such as a graph (not done here)
print('Local Start and Goal: ', grid_start, grid_goal)
path, _ = a_star(grid, heuristic, grid_start, grid_goal)
# TODO: prune path to minimize number of waypoints
pruned_path = prune_path(path)
# TODO (if you're feeling ambitious): Try a different approach altogether!
# Convert path to waypoints
waypoints = [[p[0] + north_offset, p[1] + east_offset, TARGET_ALTITUDE, 0] for p in pruned_path]
# Set self.waypoints
self.waypoints = waypoints
# TODO: send waypoints to sim (this is just for visualization of waypoints)
self.send_waypoints()
def plan_path(self):
self.flight_state = States.PLANNING
print("Searching for a path ...")
TARGET_ALTITUDE = 5
SAFETY_DISTANCE = 5
self.target_position[2] = TARGET_ALTITUDE
# read lat0, lon0 from colliders into floating point values
with open('colliders.csv') as f:
first_line = f.readline()
items = first_line.split(',')
lat0 = float(items[0].split()[1])
lon0 = float(items[1].split()[1])
# set home position to (lon0, lat0, 0)
self.set_home_position(lon0, lat0, 0)
# retrieve current global position
print ("current global position:", self.global_position)
local_pos = global_to_local(self.global_position, self.global_home)
print("current local position", local_pos)
print('global home {0}, position {1}, local position {2}'.format(self.global_home, self.global_position,
self.local_position))
# Read in obstacle map
data = np.loadtxt('colliders.csv', delimiter=',', dtype='Float64', skiprows=2)
# Define a grid for a particular altitude and safety margin around obstacles
grid, north_offset, east_offset = create_grid(data, TARGET_ALTITUDE, SAFETY_DISTANCE)
print("North offset = {0}, east offset = {1}".format(north_offset, east_offset))
# Set goal: latitude / longitude
goal_lon = -122.398470
goal_lat = 37.793445
local_goal = global_to_local((goal_lon, goal_lat, 0), self.global_home)
grid_start = (int(local_pos[0])-north_offset, int(local_pos[1])-east_offset)
grid_goal = (int(local_goal[0]-north_offset), int(local_goal[1]-east_offset))
# Adapt to set goal as position and convert
# Run A* to find a path from start to goal
print('Local Start and Goal: ', grid_start, grid_goal)
if grid_start[0] != grid_goal[0] or grid_start[1] != grid_goal[1]:
path, _ = a_star(grid, heuristic, grid_start, grid_goal)
# prune path to minimize number of waypoints
pruned_path = self.prune_path(path)
print ("prunned path is :", pruned_path)
if len(pruned_path):
# Convert path to waypoints
waypoints = [[p[0] + north_offset, p[1] + east_offset, TARGET_ALTITUDE, 0] for p in pruned_path]
# Set self.waypoints
self.waypoints = waypoints
# send waypoints to sim (this is just for visualization of waypoints)
self.send_waypoints()
def plan_path(self):
self.flight_state = States.PLANNING
print("Searching for a path ...")
self.target_position[2] = pu.TARGET_ALTITUDE
# TODO: read lat0, lon0 from colliders into floating point values
lat0, lon0 = pu.GetLat0Lon0()
# TODO: set home position to (lon0, lat0, 0)
self.set_home_position(lon0, lat0, 0)
# TODO: retrieve current global position
global_position = self.global_position
# TODO: convert to current local position using global_to_local()
local_position = global_to_local(global_position, self.global_home)
print('global home {0}, position {1}, local position {2}'.format(
self.global_home, self.global_position, self.local_position))
grid, north_offset, east_offset = pu.GetGridAndOffsets()
graph = pu.GetVoronoyGraph()
# Define starting point on the grid (this is just grid center)
# grid_start = (-north_offset, -east_offset)
# TODO: convert start position to current position rather than map center
start = (int(local_position[0] - north_offset),
int(local_position[1]) - east_offset)
# Set goal as some arbitrary position on the grid
goal = (250, 720) # (-north_offset + 10, -east_offset + 10)
# TODO: adapt to set goal as latitude / longitude position and convert
goal_position = global_to_local([-122.3962, 37.7950, 0.0],
self.global_home)
goal = (int(goal_position[0]) - north_offset,
int(goal_position[1]) - east_offset)
if (not self.goal_North is None and not self.goal_East is None):
goal = (self.goal_North, self.goal_East)
# Run A* to find a path from start to goal
# TODO: add diagonal motions with a cost of sqrt(2) to your A* implementation
# or move to a different search space such as a graph (not done here)
waypointsPath = self.GetPath(north_offset, east_offset, grid, graph,
start, goal)
if (waypointsPath is None):
print("Path was not found")
waypointsPath = []
self.waypoints = waypointsPath
print(self.waypoints)
self.send_waypoints()
def plan_path(self):
self.flight_state = States.PLANNING
print("Searching for a path ...")
TARGET_ALTITUDE = 5
SAFETY_DISTANCE = 5
self.target_position[2] = TARGET_ALTITUDE
# TODO: read lat0, lon0 from colliders into floating point values
data = np.genfromtxt('colliders.csv',
delimiter=',',
dtype='str',
max_rows=1)
_, lat0 = data[0].strip().split(' ')
_, lon0 = data[1].strip().split(' ')
# TODO: set home position to (lon0, lat0, 0)
self.set_home_position(float(lon0), float(lat0), 0)
# TODO: retrieve current global position
curr_glb_pos = [self._longitude, self._latitude, self._altitude]
# TODO: convert to current local position using global_to_local()
self._north, self._east, self._down = global_to_local(
curr_glb_pos, self.global_home)
print('global home {0}, position {1}, local position {2}'.format(
self.global_home, self.global_position, self.local_position))
# Read in obstacle map
data = np.loadtxt('colliders.csv',
delimiter=',',
dtype='Float64',
skiprows=2)
sampler = Sampler(data, SAFETY_DISTANCE)
self.polygons = sampler._polygons
nodes = sampler.sample(300)
print('nodes_len: ', len(nodes))
g = self.create_graph(nodes, 10)
print('graph_edgs: ', len(g.edges))
start = self.local_position
goal = global_to_local([-122.396428, 37.795128, TARGET_ALTITUDE],
self.global_home)
start = self.find_closest_node(g.nodes, start)
goal = self.find_closest_node(g.nodes, goal)
path, cost = a_star_for_graph(g, heuristic, start, goal)
print('a_star_path: ', path)
path = self.prune_path(path)
print('prune_path: ', path)
if len(path) > 0:
# Convert path to waypoints
waypoints = [[p[0], p[1], TARGET_ALTITUDE, 0] for p in path]
# Set self.waypoints
self.waypoints = waypoints
# TODO: send waypoints to sim (this is just for visualization of waypoints)
self.send_waypoints()
def plan_path(self):
self.flight_state = States.PLANNING
print("Searching for a path ...")
TARGET_ALTITUDE = int(np.ceil(self.global_goal[2]))
SAFETY_DISTANCE = 5
self.target_position[2] = TARGET_ALTITUDE
obstacle_file = 'data/colliders.csv'
lat0, lon0 = getOrigin(obstacle_file)
self.set_home_position(lon0, lat0, 0)
local_home_ned = global_to_local(self.global_position, self.global_home)
print('home position set to [N : {:2f}, E : {:2f} D : {:2f}]'.format(
local_home_ned[0], local_home_ned[1], local_home_ned[2]))
print('global home {0}, position {1}, local position {2}'.format(self.global_home, self.global_position,
self.local_position))
# Read in obstacle map
data = np.loadtxt('data/colliders.csv', delimiter=',', dtype='Float64', skiprows=2)
# Define a grid for a particular altitude and safety margin around obstacles
grid, north_offset, east_offset = create_grid(data, TARGET_ALTITUDE, SAFETY_DISTANCE)
print("North offset = {0}, east offset = {1}".format(north_offset, east_offset))
# Define starting point on the grid (this is just grid center)
global_pos = (self._longitude, self._latitude, self._altitude)
global_home = self.global_home
local_pos = global_to_local(global_pos, global_home)
grid_start = (int(local_pos[0] - north_offset), int(local_pos[1] - east_offset))
# Set goal as some arbitrary position on the grid
local_goal_ned = global_to_local(self.global_goal, self.global_home)
grid_goal = (int(local_goal_ned[0] - north_offset), int(local_goal_ned[1] - east_offset))
skel = create_medial_axis(grid)
medial_start, medial_goal = find_start_goal(skel, grid_start, grid_goal)
medial_grid = back_to_grid(skel)
# Run A* to find a path from start to goal
# TODO: add diagonal motions with a cost of sqrt(2) to your A* implementation
# or move to a different search space such as a graph (not done here)
print('Local Start and Goal: ', grid_start, grid_goal)
path, _ = a_star(medial_grid, heuristic, tuple(medial_start), tuple(medial_goal), self.diagonal_search)
# Prune path
path = prunePath(path)
# Convert path to waypoints
waypoints = [[p[0] + north_offset, p[1] + east_offset, TARGET_ALTITUDE, 0] for p in path]
# Set self.waypoints
self.waypoints = waypoints
# TODO: send waypoints to sim (this is just for visualization of waypoints)
self.send_waypoints()
def plan_path(self):
self.flight_state = States.PLANNING
print("Searching for a path ...")
TARGET_ALTITUDE = 6
SAFETY_DISTANCE = 5
self.target_position[2] = TARGET_ALTITUDE
#posX,posY,posZ,halfSizeX,halfSizeY,halfSizeZ
# TODO: read lat0, lon0 from colliders into floating point values
lat_lon = pd.read_csv('colliders.csv')
lat = float(lat_lon.columns[0][5:])
lon = float(lat_lon.columns[1][6:])
# TODO: set home position to (lon0, lat0, 0)
self.set_home_position = (lon, lat, 0)
print("home position set to {0}, {1}".format(lon,lat))
# TODO: retrieve current global position
local_north, local_east, local_down = global_to_local(self.global_position, self.global_home)
print("local position set to {0}, {1}, {2}".format(local_north,local_east,local_down))
# TODO: convert to current local position using global_to_local()
local_coordinates_NED = global_to_local( self.global_position, self.global_home)
print('local_coordinates_NED {0}'.format(local_coordinates_NED))
print('global home {0}, position {1}, local position {2}'.format(self.global_home, self.global_position,
self.local_position))
# Read in obstacle map
data = np.loadtxt('colliders.csv', delimiter=',', dtype='Float64',skiprows=2)
# Define a grid for a particular altitude and safety margin around obstacles
grid, north_offset, east_offset = create_grid(data, TARGET_ALTITUDE, SAFETY_DISTANCE)
print("North offset = {0}, east offset = {1}".format(north_offset, east_offset))
# Define starting point on the grid (this is just grid center)
#grid_start = (-north_offset, -east_offset)
# TODO: convert start position to current position rather than map center
grid_start = (int(np.ceil(local_north - north_offset)), int(np.ceil(local_east - east_offset)))
# Set goal as some arbitrary position on the grid
goal = global_to_local(self.goal_position,self.global_home)
grid_goal = ( int(np.ceil(goal[0]-north_offset)), int(np.ceil(goal[1]-east_offset)))
# TODO: adapt to set goal as latitude / longitude position and convert
# Run A* to find a path from start to goal
# TODO: add diagonal motions with a cost of sqrt(2) to your A* implementation
# or move to a different search space such as a graph (not done here)
print('Local Start and Goal: ', grid_start, grid_goal)
path, _ = a_star(grid, heuristic, grid_start, grid_goal)
# TODO: prune path to minimize number of waypoints
print('path: ',path)
pruned_path = prune_path(path)
print('pruned path: ',pruned_path)
# TODO (if you're feeling ambitious): Try a different approach altogether!
# Convert path to waypoints
waypoints = [[p[0] + north_offset, p[1] + east_offset, TARGET_ALTITUDE, 0] for p in pruned_path]
# Set self.waypoints
self.waypoints = waypoints
# TODO: send waypoints to sim (this is just for visualization of waypoints)
self.send_waypoints()
def plan_path(self):
self.flight_state = States.PLANNING
print("Searching for a path ...")
TARGET_ALTITUDE = 20
SAFETY_DISTANCE = 5
self.target_position[2] = TARGET_ALTITUDE
# Setting the home position based on the latitude and longitude given
# on the first line of colliders.csv
file = 'colliders.csv'
latlon = []
latlon = open(file).readline().split()
lat_h, lon_h = float(latlon[1].replace(',', '')), float(latlon[3])
self.set_home_position(lon_h, lat_h, 0.0)
global_position = self.global_position
# creating a 2D grid from the colliders.csv
data = np.loadtxt('colliders.csv',
delimiter=',',
dtype='Float64',
skiprows=2)
# Define a grid for a particular altitude and safety margin around obstacles
grid, north_offset, east_offset = create_grid(data, TARGET_ALTITUDE,
SAFETY_DISTANCE)
# Acquiring start and goal position
grid_start_latlon = (self._longitude, self._latitude, self._altitude)
grid_start_l = global_to_local(grid_start_latlon, self.global_home)
grid_start = (int(grid_start_l[0]) - north_offset,
int(grid_start_l[1]) - east_offset)
goal_lon = input('Goal longitude: ') #goal_lon = -122.396071
goal_lat = input('Goal latitude: ') #goal_lat = 37.793077
goal_position = (float(goal_lon), float(goal_lat), 0.0)
grid_goal_l = global_to_local(goal_position, self.global_home)
grid_goal = (int(grid_goal_l[0]) - north_offset,
int(grid_goal_l[1]) - east_offset)
print('Local Start: ', grid_start)
print('Local Goal: ', grid_goal)
# path planning and pruning using collinearity check and bresenham
path, _ = a_star(grid, heuristic, grid_start, grid_goal)
pruned_path = prune_path(path, grid)
print('Path length: ', len(pruned_path))
print(pruned_path)
# Convert path to waypoints
waypoints = [[
p[0] + north_offset, p[1] + east_offset, TARGET_ALTITUDE, 0
] for p in pruned_path]
# Set self.waypoints
self.waypoints = waypoints
# TODO: send waypoints to sim (this is just for visualization of waypoints)
self.send_waypoints()
def plan_path(self):
self.flight_state = States.PLANNING
print("Searching for a path ...")
TARGET_ALTITUDE = 5
SAFETY_DISTANCE = 5
self.target_position[2] = TARGET_ALTITUDE
# TODO: read lat0, lon0 from colliders into floating point values
lat0, lon0 = self.get_global_home_from_file()
# TODO: set home position to (lon0, lat0, 0)
self.set_home_position(lon0, lat0, 0)
# TODO: retrieve current global position
# TODO: convert to current local position 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,
local_position))
# Read in obstacle map
data = np.loadtxt('colliders.csv', delimiter=',', dtype='Float64', skiprows=2)
# Define a grid for a particular altitude and safety margin around obstacles
grid, north_offset, east_offset = create_grid(data, TARGET_ALTITUDE, SAFETY_DISTANCE)
print("North offset = {0}, east offset = {1}".format(north_offset, east_offset))
# Define starting point on the grid (this is just grid center)
# TODO: convert start position to current position rather than map center
grid_start = (int(local_position[0])-north_offset, int(local_position[1])-east_offset)
# Set goal as some arbitrary position on the grid
# grid_goal = (grid_start[0] + 10, grid_start[1] + 10)
# TODO: adapt to set goal as latitude / longitude position and convert
goal_geo = np.array([-122.401763, 37.795809, 0])
local_goal = global_to_local(goal_geo, self.global_home)
grid_goal = (int(local_goal[0])-north_offset, int(local_goal[1])-east_offset)
# Run A* to find a path from start to goal
# TODO: add diagonal motions with a cost of sqrt(2) to your A* implementation
# or move to a different search space such as a graph (not done here)
print('Local Start and Goal: ', grid_start, grid_goal)
path, _ = a_star(grid, heuristic, grid_start, grid_goal)
print('Path length: {0}'.format(len(path)))
# TODO: prune path to minimize number of waypoints
# TODO (if you're feeling ambitious): Try a different approach altogether!
"""
I have implemented both Collinearity and Bresenham methods to prune the path.
You can call either below to test
"""
# pruned_path = prune_coll(path)
pruned_path = prune_bres(path, grid)
print('Pruned-path length: {0}'.format(len(pruned_path)))
# Convert path to waypoints
waypoints = [[p[0] + north_offset, p[1] + east_offset, TARGET_ALTITUDE, 0] for p in pruned_path]
# Set self.waypoints
self.waypoints = waypoints
# TODO: send waypoints to sim (this is just for visualization of waypoints)
self.send_waypoints()
def global_to_map(self, global_pos, current_pos, shape, n_offset, e_offset,
altitude):
# convert global position to position in map, which includes north and east offset (bottom left corner is [0,0])
position = global_to_local(global_pos, self.global_home)
# quick check if the global position falls outside of given map.
# If it falls outside, set the position to current drone position
if position[0] < n_offset or position[0] >= shape[0] or position[
1] < e_offset or position[1] >= shape[1]:
position = global_to_local(current_pos, self.global_home)
print(
'Goal coordinate exceeds current map area, goal is set to current drone position...'
)
return (int(position[0]) - n_offset, int(position[1]) - e_offset,
altitude)
def plan_path(self):
self.flight_state = States.PLANNING
print("Searching for a path ...")
TARGET_ALTITUDE = 5
SAFETY_DISTANCE = 5
self.target_position[2] = TARGET_ALTITUDE
# read lat0, lon0 from colliders into floating point values
filename = 'colliders.csv'
home = np.genfromtxt(filename, dtype=str, max_rows=1, delimiter=',')
lat0 = float(str.split(home[0])[1])
lon0 = float(str.split(home[1])[1])
# set home position to (lon0, lat0, 0)
self.set_home_position( float(lon0), float(lat0), 0)
# convert to current global position to local position using global_to_local()
local_start = 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, local_start))
# Set goal using provided global position
goal_lat_lon = ( args.goal_lon, args.goal_lat, args.goal_alt )
local_goal = global_to_local( goal_lat_lon, self.global_home )
# Read in obstacle map
data = np.loadtxt(filename, delimiter=',', dtype='Float64', skiprows=2)
# select planning algorithm
waypoints = []
if(args.plan == 'medial-axis'):
# use medial-axis planner with a "graph" using the road network calculated between obstacles
waypoints = self.plan_medial_axis(data, local_start, local_goal, TARGET_ALTITUDE, SAFETY_DISTANCE)
else:
# use a simple grid-based planner
waypoints = self.grid_plan(data, local_start, local_goal, TARGET_ALTITUDE, SAFETY_DISTANCE)
# Assign heading to the waypoints so the craft flies pointed towards the waypoint
for i in range(1, len(waypoints)):
p1 = waypoints[i-1]
p2 = waypoints[i]
waypoints[i][3] = np.arctan2((p2[1]-p1[1]), (p2[0]-p1[0]))
# Set self.waypoints
self.waypoints = waypoints
# send waypoints to sim (this is just for visualization of waypoints)
print("Waypoints:", len(self.waypoints))
self.send_waypoints()
def plan_path(self):
print("Searching for a path ...")
TARGET_ALTITUDE = 5
SAFETY_DISTANCE = 5
self.target_position[2] = TARGET_ALTITUDE
# TODO: read lat0, lon0 from colliders into floating point values
data = np.genfromtxt('colliders.csv', delimiter=',', dtype='str', max_rows=1)
_, lat0 = data[0].strip().split(' ')
_, lon0 = data[1].strip().split(' ')
# TODO: set home position to (lon0, lat0, 0)
self.set_home_position(float(lon0), float(lat0), 0)
# TODO: retrieve current global position
curr_glb_pos = [self._longitude, self._latitude, self._altitude]
# TODO: convert to current local position using global_to_local()
self._north, self._east, self._down = global_to_local(curr_glb_pos, self.global_home)
print('global home {0}, position {1}, local position {2}'.format(self.global_home, self.global_position,
self.local_position))
# Read in obstacle map
data = np.loadtxt('colliders.csv', delimiter=',', dtype='Float64', skiprows=2)
sampler = Sampler(data, SAFETY_DISTANCE)
self.polygons = sampler._polygons
nodes = sampler.sample(3)
print('nodes_len: ', len(nodes))
xx = [[p[0], p[1], TARGET_ALTITUDE, 0] for p in nodes]
data = msgpack.dumps(xx)
print(data)
self.connection._master.write(data)
def plan_path(self):
self.flight_state = States.PLANNING
print("Searching for a path ...")
TARGET_ALTITUDE = 5
SAFETY_DISTANCE = 3
self.target_position[2] = TARGET_ALTITUDE
filename = 'colliders.csv'
# TODO: read lat0, lon0 from colliders into floating point values
global_home = planning_engines.get_global_home(filename)
# TODO: set home position to (lon0, lat0, 0)
self.set_home_position(global_home[0], global_home[1], global_home[2])
# TODO: retrieve current global position
# TODO: convert to current local position using global_to_local()
current_local_pos = global_to_local(self.global_position, global_home)
print('global home {0}, position {1}, local position {2}'.format(self.global_home, self.global_position,
self.local_position))
print('local position from drone: {0}, local_position (global_to_local): {1}'.format(self.local_position,
np.array(current_local_pos)))
# TODO: adapt to set goal as latitude / longitude position and convert
#goal_lon, goal_lat = -122.39441123, 37.79141968 # main str between mission and howard
goal_lon, goal_lat = -122.4081522, 37.7942581
#goal_lon, goal_lat = -122.398721, 37.7931154
waypoints = planning_engines.get_main_plan(global_home,goal_lat, goal_lon, current_local_pos,
SAFETY_DISTANCE, TARGET_ALTITUDE, filename)
# Set self.waypoints
self.waypoints = waypoints
print(waypoints)
def medial_axis_Astar(self, data, global_goal, TARGET_ALTITUDE,
SAFETY_DISTANCE):
grid, north_offset, east_offset = create_grid_bfs(
data, TARGET_ALTITUDE, SAFETY_DISTANCE)
skeleton = medial_axis(invert(grid))
print("North offset = {0}, east offset = {1}".format(
north_offset, east_offset))
grid_start = (int(self.local_position[0]) - north_offset,
int(self.local_position[1]) - east_offset)
local_goal = global_to_local(global_goal, self.global_home)
grid_goal = (int(local_goal[0]) - north_offset,
int(local_goal[1]) - east_offset)
skel_start, skel_goal = find_start_goal(skeleton, grid_start,
grid_goal)
mapa = breadth_first(
invert(skeleton).astype(np.int), tuple(skel_goal),
tuple(skel_start))
print('Local Start and Goal: ', grid_start, grid_goal)
path, _ = a_star_bfs(
invert(skeleton).astype(np.int), mapa, tuple(skel_start),
tuple(skel_goal))
path.append(grid_goal)
pruned_path = prune_path_bres(path, grid)
waypoints = [[
int(p[0] + north_offset),
int(p[1] + east_offset), TARGET_ALTITUDE, 0
] for p in pruned_path]
for i in range(1, len(waypoints)):
heading = np.arctan2(
waypoints[i][0] - waypoints[i - 1][0],
waypoints[i][1] - waypoints[i - 1][1]) - pi / 2
waypoints[i][3] = -heading
return waypoints
def _plan_helix(self):
TARGET_ALTITUDE = 5
SAFETY_DISTANCE = 5
# Starting at flying altitude
# self.target_position[2] = 10
with open('colliders.csv') as f:
_, lat0, _, lon0 = f.readline().replace(",", " ").replace("\n", "").split()
# Set home position to (lon0, lat0, 0)
self.set_home_position(float(lon0), float(lat0), 0)
# To NED
local_pos = global_to_local(self.global_position, self.global_home)
data = np.loadtxt('colliders.csv', delimiter=',', dtype='Float64', skiprows=2)
# Is TARGET_ALTITUDE necessary?
grid, north_offset, east_offset = create_grid(data, TARGET_ALTITUDE, SAFETY_DISTANCE)
# Define starting point on the grid, grid center
grid_start = (int(-north_offset + local_pos[0]), int(-east_offset + local_pos[1]))
# wayps = generate_helix(3, (-north_offset, -east_offset), (0, 0, 200))
wayps = generate_helix(3, (0, 0), (0, 0, 20))
print("wayps", wayps)
# if waypoint list is empty don't do it
# self.waypoints = []
self.waypoints = wayps
def sample_goal(self, nodes, lat, lon):
zvals = np.random.uniform(self._zmin, self._zmax)
lp = global_to_local((lon, lat, 0),
self._global_home) # (north, east, down)
goal = (lp[0], lp[1], zvals)
in_collision = False
idxs = list(
self._tree.query_radius(np.array([goal[0],
goal[1]]).reshape(1, -1),
r=self._max_poly_xy)[0])
if len(idxs) > 0:
for ind in idxs:
p = self._polygons[int(ind)]
if p.contains([goal[0], goal[1], goal[2]
]) and p.height >= goal[2]:
in_collision = True
if not in_collision:
return goal
# If the lat and lon given by parameters is collide with obstacles,
# Set it to the closest point
nodes = np.array(nodes)
min_dist_idx = np.linalg.norm(np.array(goal) - nodes, axis=1).argmin()
return nodes[min_dist_idx][0], nodes[min_dist_idx][1], zvals
def plan_path(self):
self.flight_state = States.PLANNING
print("Searching for a path ...")
TARGET_ALTITUDE = 4
SAFETY_DISTANCE = 5
self.target_position[2] = TARGET_ALTITUDE
# TODO: read lat0, lon0 from colliders into floating point values
pos = open("colliders.csv").readline().split(",")
lat0 = float(pos[0].replace("lat0 ",""))
lon0 = float(pos[1].replace("lon0 ",""))
# TODO: set home position to (lon0, lat0, 0)
self.set_home_position(lon0, lat0, 0.0)
# TODO: retrieve current global position
#print('global position {}'.format(self.global_position))
# TODO: convert to current local position using global_to_local()
local_pos = global_to_local(self.global_position, self.global_home)
print('global home {0}, \n position {1}, \n local position {2}'.format(self.global_home, self.global_position,
self.local_position))
# Read in obstacle map
data = np.loadtxt('colliders.csv', delimiter=',', dtype='Float64', skiprows=2)
# Define a grid for a particular altitude and safety margin around obstacles
grid, north_offset, east_offset = create_grid(data, TARGET_ALTITUDE, SAFETY_DISTANCE)
print("North offset = {0}, east offset = {1}".format(north_offset, east_offset))
# convert start position to current position rather than map center
grid_start = [int(np.ceil(local_pos[0] - north_offset)), int(np.ceil(local_pos[1] - east_offset))]
# Set goal as some arbitrary position on the grid
#grid_goal = (-north_offset + 300, -east_offset + 300)
# adapt to set goal as latitude / longitude position and convert
local_goal_pos = global_to_local(self.global_goal_position, self.global_home)
grid_goal = (int(np.ceil(local_goal_pos[0] - north_offset)), int(np.ceil(local_goal_pos[1] - east_offset)))
print(grid_goal)
# Run A* to find a path from start to goal
print('Local Start and Goal: ', grid_start, grid_goal)
path, _ = a_star(grid, heuristic, grid_start, grid_goal)
# prune path to minimize number of waypoints
reduced_path = prune_path(path)
# Convert path to waypoints
waypoints = [[p[0] + north_offset, p[1] + east_offset, TARGET_ALTITUDE, 0] for p in reduced_path]
# Set self.waypoints
self.waypoints = waypoints
# TODO: send waypoints to sim (this is just for visualization of waypoints)
self.send_waypoints()
def randomPoint(self, grid, north_offset, east_offset, TARGET_ALTITUDE,
global_home):
local_position_ne = np.array(
[north_offset, east_offset, TARGET_ALTITUDE])
local_position_sw = np.array(
[-north_offset, -east_offset, TARGET_ALTITUDE])
lat_ne, lon_ne, alt = local_to_global(local_position_ne, global_home)
lat_sw, lon_sw, alt = local_to_global(local_position_sw, global_home)
random_lat = random.uniform(lat_sw, lat_ne)
random_lon = random.uniform(lon_ne, lon_sw)
global_position = np.array([random_lat, random_lon, TARGET_ALTITUDE])
n, e, d = global_to_local(global_position, global_home)
while grid[int(n) - north_offset, int(e) - east_offset] == 1:
random_lat = random.uniform(lat_sw, lat_ne)
random_lon = random.uniform(lon_ne, lon_sw)
global_position = np.array(
[random_lat, random_lon, TARGET_ALTITUDE])
n, e, d = global_to_local(global_position, global_home)
return global_position
def plan_path(self):
def local_to_grid(local_coord):
"""
Convert a local frame to a grid frame location
"""
return (int(np.ceil(local_coord[0] - north_offset)),
int(np.ceil(local_coord[1] - east_offset)))
self.flight_state = States.PLANNING
print("Searching for a path ...")
TARGET_ALTITUDE = 5
SAFETY_DISTANCE = 5
self.target_position[2] = TARGET_ALTITUDE
self.load_home('colliders.csv')
local = 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))
# Read in obstacle map
data = np.loadtxt('colliders.csv',
delimiter=',',
dtype='Float64',
skiprows=2)
# Define a grid for a particular altitude and safety margin around obstacles
grid, north_offset, east_offset = create_grid(data, TARGET_ALTITUDE,
SAFETY_DISTANCE)
print("North offset = {0}, east offset = {1}".format(
north_offset, east_offset))
# Define starting point on the grid
grid_start = local_to_grid(local)
# Set goal as some arbitrary position on the grid
grid_goal = (grid_start[0] + 10, grid_start[1] - 10
) #(-north_offset + 10, -east_offset + 10)
# TODO: adapt to set goal as latitude / longitude position and convert
# Run A* to find a path from start to goal
# TODO: add diagonal motions with a cost of sqrt(2) to your A* implementation
# or move to a different search space such as a graph (not done here)
print('Local Start and Goal: ', grid_start, grid_goal)
path, _ = a_star(grid, heuristic, grid_start, grid_goal)
# TODO: prune path to minimize number of waypoints
# TODO (if you're feeling ambitious): Try a different approach altogether!
# Convert path to waypoints
waypoints = [[
p[0] + north_offset, p[1] + east_offset, TARGET_ALTITUDE, 0
] for p in path]
# Set self.waypoints
self.waypoints = waypoints
# TODO: send waypoints to sim (this is just for visualization of waypoints)
self.send_waypoints()
def plan_path(self):
self.flight_state = States.PLANNING
print("Searching for a path ...")
TARGET_ALTITUDE = 5
SAFETY_DISTANCE = 5
self.target_position[2] = TARGET_ALTITUDE
lat0, lon0 = open('colliders.csv').readline().replace("lat0", "").replace("lon0", "").strip().split(",")
lat0 = float(lat0)
lon0 = float(lon0)
self.set_home_position(lon0, lat0, 0)
current_local_position = global_to_local(self.global_position, self.global_home)
goal_local_position = global_to_local(self.global_goal, self.global_home)
print('global home {0}, position {1}, local position {2}'.format(self.global_home, self.global_position,
self.local_position))
# Read in obstacle map
data = np.loadtxt('colliders.csv', delimiter=',', dtype='Float64', skiprows=2)
# Define a grid for a particular altitude and safety margin around obstacles
grid, north_offset, east_offset = create_grid(data, TARGET_ALTITUDE, SAFETY_DISTANCE)
print("North offset = {0}, east offset = {1}".format(north_offset, east_offset))
# Define starting point on the grid (this is just grid center)
grid_start = (int(current_local_position[0]-north_offset), int(current_local_position[1]-east_offset))
# Set goal as some arbitrary position on the grid
grid_goal = (int(goal_local_position[0]-north_offset), int(goal_local_position[1]-east_offset))
# Run A* to find a path from start to goal
print('Local Start and Goal: ', grid_start, grid_goal)
path, _ = a_star(grid, heuristic, grid_start, grid_goal)
path = prune_path(path)
# Convert path to waypoints
waypoints = [[p[0] + north_offset, p[1] + east_offset, TARGET_ALTITUDE, 0] for p in path]
# Set self.waypoints
self.waypoints = waypoints
# TODO: send waypoints to sim (this is just for visualization of waypoints)
self.send_waypoints()
def get_grid_start_and_goal_position(
self,
input_data,
target_altitude,
safety_distance,
use_gedoetic_frame_for_goal_position=True):
# Define a grid for a particular altitude and safety margin around obstacles
grid, north_offset, east_offset = plu.create_grid(
input_data, target_altitude, safety_distance)
# retrieve current global position
current_global_position = [
self._longitude, self._latitude, self._altitude
]
# convert to current local position relative to new global home based on colliders.csv using global_to_local()
curr_local_position_rel_new_global_home = global_to_local(
current_global_position, self.global_home)
# Define starting point on the grid
grid_start = (int(np.floor(
curr_local_position_rel_new_global_home[0])),
int(np.floor(
curr_local_position_rel_new_global_home[1])),
curr_local_position_rel_new_global_home[2])
if use_gedoetic_frame_for_goal_position:
goal_position_valid = False
count = 0
max_proposals_allowed = 30
while not goal_position_valid or count < max_proposals_allowed:
goal_lon = np.random.uniform(-122.42, -122.3)
goal_lat = np.random.uniform(37.7, 37.82)
goal_position_valid, local_goal_position = self.is_goal_position_within_grid_boundaries(
goal_lon, goal_lat, 0.0, north_offset, east_offset)
if (goal_position_valid):
grid_goal = (int(np.floor(local_goal_position[0])),
int(np.floor(local_goal_position[1])),
-target_altitude)
break
else:
count += 1
if not goal_position_valid:
print("no valid goal position found")
else:
# Set goal as some arbitrary position on the grid
goal_north_position = np.random.randint(0, 2 * abs(north_offset))
goal_east_position = np.random.randint(0, 2 * abs(east_offset))
grid_goal = (goal_north_position, goal_east_position,
-target_altitude)
return grid, north_offset, east_offset, grid_start, grid_goal
