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 = 5
self.target_position[2] = TARGET_ALTITUDE
colliders_data = 'colliders.csv'
# TODO: read lat0, lon0 from colliders into floating point values
lat0, lon0 = read_global_home(colliders_data)
# TODO: set home position to (lat0, lon0, 0)
self.set_home_position(lon0, lat0, 0)
# TODO: retrieve current global position
local_N, local_E, local_D = global_to_local(self.global_position,
self.global_home)
# 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_data,
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_SN = int(np.ceil(local_N - north_offset))
grid_SE = int(np.ceil(local_E - east_offset))
grid_start = (grid_SN, grid_SE)
# TODO: convert start position to current position rather than map center
# Set goal as some arbitrary position on the grid
goal_N, goal_E, goal_alt = global_to_local(self.goal_global_position,
self.global_home)
grid_goal = (int(np.ceil(goal_N - north_offset)),
int(np.ceil(goal_E - 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 = collinearityPrune(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
# TODO: read lat0, lon0 from colliders into floating point values
csv_name = 'colliders.csv'
lat0, lon0 = read_global_home(csv_name)
# 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()
print('global positon {}'.format(self.global_position))
local_north, local_east, _ = 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 (this is just grid center)
# TODO: convert start position to current position rather than map 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)
# Set goal as some arbitrary position on the grid
lon_goal = -122.397745
lat_goal = 37.793837
# TODO: adapt to set goal as latitude / longitude position and convert
pos_global_goal = [lon_goal, lat_goal, 0]
pos_local_goal = global_to_local(pos_global_goal, self.global_home)
north_goal = int(pos_local_goal[0])
easth_goal = int(pos_local_goal[1])
grid_goal = ((north_goal - north_offset), (easth_goal - 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
# TODO (if you're feeling ambitious): Try a different approach altogether!
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
# 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
filename = "colliders.csv"
lon0, lat0 = read_global_home(filename)
# print('Global Home Lat: {0}, Lon: {1}'.format(lat0, lon0))
# set home position to (lat0, lon0, 0)
self.set_home_position(lon0, lat0, 0)
# retrieve current global position
local_position_g = [self._longitude, self._latitude, self._altitude]
# print('current global position: longitude={}, latitude={}'.format(self._longitude, self._latitude))
# convert to current local position using global_to_local()
self._north, self._east, self._down = global_to_local(
local_position_g, self.global_home)
print('global home {}, global position {}, local position {}'.format(
self.global_home, self.global_position, self.local_position))
# Read in obstacle map
data = np.loadtxt(filename, 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
# Set goal as some arbitrary position on the grid
# Define starting point on the grid (this is just grid center)
start_position_l = self.local_position[:2]
grid_start = (int(np.ceil(-north_offset + start_position_l[0])),
int(np.ceil(-east_offset + start_position_l[1])))
# adapt to set goal as latitude / longitude position and convert
print("goal_position_g: {}".format(goal_position_g))
goal_position_l = global_to_local(goal_position_g, self.global_home)
grid_goal = (int(np.ceil(-north_offset + goal_position_l[0])),
int(np.ceil(-east_offset + goal_position_l[1])))
print('Local Start and Goal: ', grid_start, grid_goal)
# Run A* to find a path from start to goal
#Compute the lowest cost path with a_star
path, _ = a_star(grid, heuristic, grid_start, grid_goal)
print('Length of raw path: {}'.format(len(path)))
# print(f'Path cost: {cost}')
#prune path to minimize number of waypoints
pruned_path = prune_path(path)
print('Length of pruned path: {}'.format(len(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]
print('Waypoint count : {}'.format(len(waypoints)))
# Set self.waypoints
self.waypoints = waypoints
# send waypoints to sim
self.send_waypoints()