args.dest_global_pos_lon,
args.dest_global_pos_lat,
args.dest_global_pos_alt,
]),
np.array([lon0,lat0,0]))
data = np.loadtxt('colliders.csv',
delimiter=',',
dtype='Float64',
skiprows=2)
probabilistic_path, _ = optimal_probabilistic(data,
args.dest_global_pos_alt,
heuristic,
local_position_ned,
local_goal)
else:
probabilistic_path = None
conn = MavlinkConnection('tcp:{0}:{1}'.format(args.host, args.port),
timeout=60)
drone = MotionPlanning(
conn,
np.array([
args.dest_global_pos_lon, args.dest_global_pos_lat,
args.dest_global_pos_alt
]), args.mode, probabilistic_path)
time.sleep(1)
drone.start()
"""This method is provided
1. Open a log file
2. Start the drone connection
3. Close the log file
"""
print("Creating log file")
self.start_log("Logs", "NavLog.txt")
print("starting connection")
self.connection.start()
print("Closing log file")
self.stop_log()
if __name__ == "__main__":
parser = argparse.ArgumentParser()
parser.add_argument('--port', type=int, default=5760, help='Port number')
parser.add_argument('--host',
type=str,
default='127.0.0.1',
help="host address, i.e. '127.0.0.1'")
args = parser.parse_args()
conn = MavlinkConnection('tcp:{0}:{1}'.format(args.host, args.port),
threaded=False,
PX4=False)
#conn = WebSocketConnection('ws://{0}:{1}'.format(args.host, args.port))
drone = BackyardFlyer(conn)
time.sleep(2)
drone.start()
def manual_transition(self):
print("manual transition")
self.stop()
self.in_mission = False
self.flight_state = States.MANUAL
def start(self):
self.start_log("Logs", "NavLog.txt")
# self.connect()
print("starting connection")
# self.connection.start()
super().start()
# Only required if they do threaded
# while self.in_mission:
# pass
self.stop_log()
if __name__ == "__main__":
conn = MavlinkConnection('tcp:127.0.0.1:5760', threaded=False, PX4=False)
#conn = WebSocketConnection('ws://127.0.0.1:5760')
drone = ControlsFlyer(conn)
time.sleep(2)
drone.start()
drone.print_mission_score()
self.connection.start()
# Only required if they do threaded
# while self.in_mission:
# pass
self.stop_log()
if __name__ == "__main__":
parser = argparse.ArgumentParser()
parser.add_argument("-lat",
"--latitude",
help="enter latitude ",
action="store_true")
parser.add_argument("-lon",
"--longitude",
help="enter longitude ",
action="store_true")
#parser.add_argument('--port', type=int, default=5760, help='Port number')
#parser.add_argument('--host', type=str, default='127.0.0.1', help="host address, i.e. '127.0.0.1'")
#args = parser.parse_args()
#conn = MavlinkConnection('tcp:{0}:{1}'.format(args.host, args.port), timeout=60)
conn = MavlinkConnection('tcp:127.0.0.1:5760', timeout=60)
drone = MotionPlanning(conn)
time.sleep(1)
drone.start()
# TODO: add code to visualize the path
path_pairs = zip(path[:-1], path[1:])
for (n1, n2) in path_pairs:
plt.plot([n1[1] - emin, n2[1] - emin], [n1[0] - nmin, n2[0] - nmin], 'green')
plt.xlabel('NORTH')
plt.ylabel('EAST')
plt.show()
"""
# Convert path to waypoints
waypoints = [[p[0] + north_offset, p[1] + east_offset, p[2], 0]
for p in pruned_path]
# waypoints.insert(0, [grid_start[0] + north_offset, grid_start[1] + east_offset, 10.0, 0])
waypoints.pop(0)
waypoints.append([
grid_goal[0] + north_offset, grid_goal[1] + east_offset, grid_goal[2],
0
])
conn = MavlinkConnection('tcp:{0}:{1}'.format(args.host, args.port),
threaded=False,
send_rate=5,
timeout=60000)
drone = MotionPlanning(conn)
time.sleep(2)
drone.start()
import argparse
import time
from enum import Enum
from udacidrone import Drone
from udacidrone.connection import MavlinkConnection
conn = MavlinkConnection('tcp:127.0.0.1:5760', threaded=True)
drone = Drone(conn)
time.sleep(2)
print('starting drone...')
drone.start()
print('taking control...')
drone.take_control()
print('arming...')
drone.arm()
drone.set_home_position(drone.global_position[0], drone.global_position[1],
drone.global_position[2])
drone.takeoff(1)
time.sleep(5)
drone.cmd_position(0, 0, 3, 0)
time.sleep(5)
drone.cmd_position(0, 10, 3, 0)
time.sleep(5)
drone.cmd_position(10, 10, 3, 0)
time.sleep(5)
drone.cmd_position(10, 0, 3, 0)
try_count = 5
try_i = 0
is_timeout = True
GRD = None
waypoints = None
landing_altitude = 0
t0 = time.time()
while (is_timeout and try_i < try_count):
if GRD is None:
print("GRD is None")
else:
print("GRD is not None")
conn = MavlinkConnection('tcp:{0}:{1}'.format('127.0.0.1', 5760),
timeout=1000)
drone = MotionPlanning(conn,
GRD,
waypoints=waypoints,
landing_altitude=landing_altitude,
global_goal=global_goal,
local_goal=local_goal,
grid_goal=grid_goal)
time.sleep(1)
drone.start()
try_i += 1
if drone.GRD is not None:
print("GRD = drone.GRD, not none")
GRD = drone.GRD
else:
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
if len(self.waypoints) > 0:
self.send_waypoints()
time.sleep(1)
else:
filename = 'colliders.csv'
# Reading in the data skipping the first two lines.
# reading lat0, lon0 from colliders into floating point values
f = open(filename, "r")
temp = f.read().split('\n')
lat, lon = temp[0].split(",")
lat0 = float(lat.strip('lat0'))
lon0 = float(lon.strip(' lon0 '))
f.close()
print(lat0, lon0)
# setting home position to (lon0, lat0, 0)
self.set_home_position(lon0, lat0, 0.0)
# retrieving current global position
global_position = (self._longitude, self._latitude, self._altitude)
# converting 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))
self.landing_transition()
self.disarming_transition()
self.manual_transition()
# Reading in obstacle map
data = np.loadtxt('colliders.csv',
delimiter=',',
dtype='Float64',
skiprows=2)
# Defining a grid for a particular altitude and safety margin around obstacles
#grid, north_offset, east_offset = create_grid(data, TARGET_ALTITUDE, SAFETY_DISTANCE)
drone_altitude = 5
safety_distance = 3
# This is now the routine using Voronoi
grid, edges, north_offset, east_offset = create_grid_and_edges(
data, drone_altitude, safety_distance)
grid_start = (-north_offset + int(self.local_position[0]),
-east_offset + int(self.local_position[1]))
# Setting goal as some arbitrary position on the grid
# adapting to set goal as latitude / longitude position and convert
goal_local = global_to_local([-122.395914, 37.795267, 0],
self.global_home)
#goal_local = global_to_local ([-122.398993,37.792522,0],self.global_home)
grid_goal = ((-north_offset + int(goal_local[0])),
(-east_offset + int(goal_local[1])))
#grid_goal = (-north_offset + 10, -east_offset + 10)
G = nx.Graph()
for e in edges:
p1 = e[0]
p2 = e[1]
dist = LA.norm(np.array(p2) - np.array(p1))
G.add_edge(p1, p2, weight=dist)
# Running A* to find a path from start to goal
print('Local Start and Goal: ', grid_start, grid_goal)
start_ne_g = closest_point(G, grid_start)
goal_ne_g = closest_point(G, grid_goal)
path_, cost = a_star(G, heuristic, start_ne_g, goal_ne_g)
path = prune_path(grid, path_)
# Converting path to waypoints
waypoints = [[
int(p[0] + north_offset),
int(p[1] + east_offset), TARGET_ALTITUDE, 0
] for p in path]
print(waypoints)
# Reconnecting to drone and send calculated waypoints
conn = MavlinkConnection('tcp:{0}:{1}'.format('127.0.0.1', 5760),
timeout=600)
drone = MotionPlanning(conn, waypoints=waypoints)
time.sleep(1)
drone.start()
def plan_path(self):
t0 = time.time()
self.flight_state = States.PLANNING
print("Searching for a path ...")
TARGET_ALTITUDE = args.altitude
SAFETY_DISTANCE = args.safety
self.target_position[2] = TARGET_ALTITUDE
if len(self.waypoints) > 0:
self.send_waypoints()
time.sleep(1)
else:
# TODO: read lat0, lon0 from colliders into floating point values
with open('colliders.csv', newline='') as f:
reader = csv.reader(f)
row1 = next(reader)
f.close()
lat = float(
re.findall(
"[-+]?[.]?[\d]+(?:,\d\d\d)*[\.]?\d*(?:[eE][-+]?\d+)?",
row1[0])[1])
lon = float(
re.findall(
"[-+]?[.]?[\d]+(?:,\d\d\d)*[\.]?\d*(?:[eE][-+]?\d+)?",
row1[1])[1])
# TODO: set home position to (lon0, lat0, 0)
self.set_home_position(lon, lat, 0.0)
# TODO: retrieve current global position
global_position = [self._longitude, self._latitude, self._altitude]
# TODO: convert to current local position using global_to_local()
local_position = global_to_local(global_position, self.global_home)
print('global home {0}, global position {1}, local position {2}'.
format(self.global_home, self.global_position,
self.local_position))
# Close connection, proceed with planning and then open a new connection
# with determined waypoints
self.disarming_transition()
self.manual_transition()
# 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, edges, north_offset, east_offset = create_grid_and_edges(
data, TARGET_ALTITUDE, SAFETY_DISTANCE)
print("Found {0} edges".format(len(edges)))
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_position[0] - north_offset),
int(local_position[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
if args.goal_lon and args.goal_lat:
if args.goal_alt:
goal = [
float(args.goal_lon),
float(args.goal_lat),
float(args.goal_alt)
]
else:
goal = [float(args.goal_lon), float(args.goal_lat), 0.0]
local_goal = global_to_local(goal, self.global_home)
grid_goal = (int(local_goal[0] - north_offset),
int(local_goal[1] - east_offset))
else:
grid_goal = (-north_offset + 50, -east_offset + 50)
'''
if grid[grid_goal[0], grid_goal[1]] > 0:
print('Goal will result in collision, '
'resetting goal to initial goal')
grid_goal = (-north_offset + 50, -east_offset + 50)
'''
# 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)
plot_chart_edges(grid, edges, grid_start, grid_goal)
print('Saved chart of graph')
# TODO (if you're feeling ambitious): Try a different approach altogether!
print('Building graph...')
t1 = time.time()
graph = create_graph(edges)
print('Graph built in {:.2f} secs'.format(time.time() - t1))
graph_start = closest_point(graph, grid_start)
graph_goal = closest_point(graph, grid_goal)
print('Graph Start and Goal: ', graph_start, graph_goal)
path, cost = a_star_graph(graph, heuristic, graph_start,
graph_goal)
# path.append(grid_goal)
if len(path) == 0:
self.landing_transition()
print('Length of original path: {}\tCost of path: {}'.format(
len(path), cost))
# prune path
pruned_path = bres_prune(grid, path)
if pruned_path:
path = pruned_path
print('Length of pruned path: {}'.format(len(path)))
plot_chart_edges_route(grid, edges, path, grid_start, graph_start,
grid_goal, graph_goal)
print('Saved chart of graph with route')
# Convert path to waypoints
waypoints = [[
int(p[0] + north_offset),
int(p[1] + east_offset), TARGET_ALTITUDE, 0
] for p in path]
print(waypoints)
print('Time to complete planning: {:.2f}'.format(time.time() - t0))
# Reconnect to drone and send calculated waypoints
conn = MavlinkConnection('tcp:{0}:{1}'.format('127.0.0.1', 5760),
timeout=600)
drone = MotionPlanning(conn, waypoints=waypoints)
time.sleep(1)
drone.start()
class MotionPlanning(Drone):
def __init__(self, connection):
super().__init__(connection)
self.target_position = np.array([0.0, 0.0, 0.0])
self.waypoints = []
self.in_mission = True
self.check_state = {}
# initial state
self.flight_state = States.MANUAL
# register all your callbacks here
self.register_callback(MsgID.LOCAL_POSITION, self.local_position_callback)
self.register_callback(MsgID.LOCAL_VELOCITY, self.velocity_callback)
self.register_callback(MsgID.STATE, self.state_callback)
def local_position_callback(self):
if self.flight_state == States.TAKEOFF:
if -1.0 * self.local_position[2] > 0.95 * self.target_position[2]:
self.waypoint_transition()
elif self.flight_state == States.WAYPOINT:
if np.linalg.norm(self.target_position[0:2] - self.local_position[0:2]) < 1.0:
if len(self.waypoints) > 0:
self.waypoint_transition()
else:
if np.linalg.norm(self.local_velocity[0:2]) < 1.0:
self.landing_transition()
def velocity_callback(self):
if self.flight_state == States.LANDING:
if self.global_position[2] - self.global_home[2] < 0.1:
if abs(self.local_position[2]) < 0.01:
self.disarming_transition()
def state_callback(self):
if self.in_mission:
if self.flight_state == States.MANUAL:
self.plan_path()
self.arming_transition()
elif self.flight_state == States.ARMING:
if self.armed:
self.plan_path()
elif self.flight_state == States.PLANNING:
self.takeoff_transition()
elif self.flight_state == States.DISARMING:
if ~self.armed & ~self.guided:
self.manual_transition()
def arming_transition(self):
self.flight_state = States.ARMING
print("arming transition")
self.arm()
self.take_control()
def takeoff_transition(self):
self.flight_state = States.TAKEOFF
print("takeoff transition")
self.takeoff(self.target_position[2])
def waypoint_transition(self):
self.flight_state = States.WAYPOINT
print("waypoint transition")
self.target_position = self.waypoints.pop(0)
print('target position', self.target_position)
self.cmd_position(self.target_position[0], self.target_position[1], self.target_position[2], self.target_position[3])
def landing_transition(self):
self.flight_state = States.LANDING
print("landing transition")
self.land()
def disarming_transition(self):
self.flight_state = States.DISARMING
print("disarm transition")
self.disarm()
self.release_control()
def manual_transition(self):
self.flight_state = States.MANUAL
print("manual transition")
self.stop()
self.in_mission = False
def send_waypoints(self):
print("Sending waypoints to simulator ...")
data = msgpack.dumps(self.waypoints)
self.connection._master.write(data)
def plan_path(self):
def start(self):
self.start_log("Logs", "NavLog.txt")
print("starting connection")
self.connection.start()
# Only required if they do threaded
# while self.in_mission:
# pass
self.stop_log()
if __name__ == "__main__":
parser = argparse.ArgumentParser()
parser.add_argument('--port', type=int, default=5760, help='Port number')
parser.add_argument('--host', type=str, default='127.0.0.1', help="host address, i.e. '127.0.0.1'")
args = parser.parse_args()
conn = MavlinkConnection('tcp:{0}:{1}'.format(args.host, args.port), timeout=60)
drone = MotionPlanning(conn)
time.sleep(1)
drone.start()
class BackyardFlyer(Drone):
def __init__(self, connection):
super().__init__(connection)
self.target_position = [0.0, 0.0, 0.0]
self.all_waypoints = self.calculate_box()
self.in_mission = True
self.check_state = {}
# initial state
self.flight_state = States.MANUAL
# Register all your callbacks here
self.register_callback(MsgID.LOCAL_POSITION, self.local_position_callback)
self.register_callback(MsgID.LOCAL_VELOCITY, self.velocity_callback)
self.register_callback(MsgID.STATE, self.state_callback)
def local_position_callback(self):
"""
This triggers when `MsgID.LOCAL_POSITION` is received and self.local_position contains new data
"""
if self.flight_state == States.TAKEOFF:
# check if altitude is within 95% of target
if abs(self.local_position[2] - self.target_position[2]) < abs(0.05*self.target_position[2]):
self.waypoint_transition()
if self.flight_state == States.WAYPOINT:
# check if is within 0.1 m box
if abs(self.local_position[0] - self.target_position[0]) < 0.2 and \
abs(self.local_position[1] - self.target_position[1]) < 0.2 and \
abs(self.local_position[2] - self.target_position[2]) < 0.2:
if len(self.all_waypoints) > 0:
self.waypoint_transition()
else:
self.landing_transition()
if self.flight_state == States.LANDING:
if ((self.global_position[2] - self.global_home[2] < 0.1) and
abs(self.local_position[2]) < 0.01):
self.disarming_transition()
def velocity_callback(self):
"""
This triggers when `MsgID.LOCAL_VELOCITY` is received and self.local_velocity contains new data
"""
pass
def state_callback(self):
if self.in_mission:
if self.flight_state == States.MANUAL:
# now just passively waiting for the pilot to change these attributes
# once the pilot changes, need to update our internal state
if self.guided:
self.flight_state = States.ARMING
elif self.flight_state == States.ARMING:
if self.armed:
self.takeoff_transition()
elif self.flight_state == States.LANDING:
# check if the pilot has changed the armed and control modes
# if so (and the script no longer in control) stop the mission
if not self.armed and not self.guided:
self.stop()
self.in_mission = False
elif self.flight_state == States.DISARMING:
# no longer want the vehicle to handle the disarming and releasing control
# that will be done by the pilot
pass
def calculate_box(self, target_side=1.0, target_altitude=3.0):
"""
1. Return waypoints to fly a box
"""
print("Setting Home")
# get the current local position -> note we need to change the sign of the down coordinate to be altitude
cp = np.array([self.local_position[0], self.local_position[1], -self.local_position[2]])
target_altitude = 10.0
target_side = 3.0
waypoints = [cp + [target_side, 0, -target_altitude, 0],
cp + [target_side, target_side, -target_altitude, 0],
cp + [0, target_side, -target_altitude, 0],
cp + [0, 0, -target_altitude, 0]]
return waypoints
def arming_transition(self):
"""
1. Take control of the drone
2. Pass an arming command
3. Set the home location to current position
4. Transition to the ARMING state
"""
print("arming transition")
self.take_control()
self.arm()
# set the current location to be the home position
self.set_home_position(self.global_position[0],
self.global_position[1],
self.global_position[2])
self.flight_state = States.ARMING
def takeoff_transition(self):
"""
1. Set target_position altitude to 3.0m
2. Command a takeoff to 3.0m
3. Transition to the TAKEOFF state
"""
print("takeoff transition")
target_altitude = 3.0
self.target_position[2] = -target_altitude
self.takeoff(target_altitude) # super
self.flight_state = States.TAKEOFF
def waypoint_transition(self):
"""
1. Command the next waypoint position
2. Transition to WAYPOINT state
"""
print("waypoint transition")
self.target_position = self.all_waypoints.pop(0)
north, east, down, heading = self.target_position
self.cmd_position(north, east, -down, heading)
self.flight_state = States.WAYPOINT
def landing_transition(self):
"""
1. Command the drone to land
2. Transition to the LANDING state
"""
print("landing transition")
self.land()
self.flight_state = States.LANDING
def disarming_transition(self):
"""
1. Command the drone to disarm
2. Transition to the DISARMING state
"""
print("disarm transition")
self.disarm()
self.flight_state = States.DISARMING
def manual_transition(self):
"""
1. Release control of the drone
2. Stop the connection (and telemetry log)
3. End the mission
4. Transition to the MANUAL state
"""
print("manual transition")
self.release_control()
self.stop()
self.in_mission = False
self.flight_state = States.MANUAL
def start(self):
"""
1. Open a log file
2. Start the drone connection
3. Close the log file
"""
print("Creating log file")
self.start_log("Logs", "NavLog.txt")
print("starting connection")
self.connection.start()
print("Closing log file")
self.stop_log()
if __name__ == "__main__":
parser = argparse.ArgumentParser()
parser.add_argument('--port', type=int, default=5760, help='Port number')
parser.add_argument('--host', type=str, default='127.0.0.1', help="host address, i.e. '127.0.0.1'")
args = parser.parse_args()
conn = MavlinkConnection('udp:192.168.1.2:14550', PX4=True, threaded=False)
#conn = WebSocketConnection('ws://{0}:{1}'.format(args.host, args.port))
drone = BackyardFlyer(conn)
time.sleep(2)
drone.start()
def plan_path(self):
t0 = time.time()
self.flight_state = States.PLANNING
print("Searching for a path ...")
TARGET_ALTITUDE = 5
SAFETY_DISTANCE = 5
self.target_position[2] = TARGET_ALTITUDE
# If waypoints are supplied as arguments, then send them to the sim
# for visualisation and carry out the mission, or otherwise continue
# with planning
if len(self.waypoints) > 0:
# TODO: send waypoints to sim (this is just for visualization of waypoints)
self.send_waypoints()
time.sleep(1)
else:
# TODO: read lat0, lon0 from colliders into floating point values
lat0, lon0 = get_long_lat()
# TODO: set home position to (lon0, lat0, 0)
self.set_home_position(lon0, lat0, 0.0)
# TODO: retrieve current global position
global_position = [self._longitude, self._latitude, self._altitude]
# TODO: convert to current local position using global_to_local()
local_position = global_to_local(global_position, self.global_home)
print('global home {0}, global position {1}, local position {2}'.
format(self.global_home, self.global_position,
self.local_position))
# Close connection proceed with planning and then open a new connection
# with determined waypoints
#self.disarming_transition()
#self.manual_transition()
# 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_position[0] - north_offset)),
int(np.ceil(local_position[1] - east_offset)))
# Set goal as some arbitrary position on the grid
# TODO: adapt to set goal as latitude / longitude position and convert
if args.goal_lon and args.goal_lat:
if args.goal_alt:
goal = [
float(args.goal_lon),
float(args.goal_lat),
float(args.goal_alt)
]
else:
goal = [
float(args.goal_lon),
float(args.goal_lat), TARGET_ALTITUDE
]
local_goal = global_to_local(goal, self.global_home)
grid_goal = (int(np.ceil(local_goal[0] - north_offset)),
int(np.ceil(local_goal[1] - east_offset)))
else:
north_goal = np.random.choice(np.arange(10, 50), 1)[0]
east_goal = np.random.choice(np.arange(10, 50), 1)[0]
print('north_goal : ', north_goal)
print('east_goal : ', east_goal)
grid_goal = (-north_offset + north_goal,
-east_offset + east_goal)
# 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('Actual path length: {}'.format(len(path)))
# TODO: prune path to minimize number of waypoints
print('Original length of path: {}'.format(len(path)))
# TODO (if you're feeling ambitious): Try a different approach altogether!
pruned_path = bres_prune(grid, path)
print('Pruned path length: {}'.format(len(path)))
plot_route(path, pruned_path, grid_start, grid_goal)
print('Plotted chart of pruned path')
# Convert path to waypoints
waypoints = [[
int(p[0] + north_offset),
int(p[1] + east_offset), TARGET_ALTITUDE, 0
] for p in pruned_path]
# Set self.waypoints
self.waypoints = waypoints
self.send_waypoints()
print('Time to complete planning: {:.2f}'.format(time.time() - t0))
# Reconnect to drone and send calculated waypoints
conn = MavlinkConnection('tcp:{0}:{1}'.format('127.0.0.1', 5760),
timeout=600)
drone = MotionPlanning(conn, waypoints=waypoints)
time.sleep(1)
drone.start()
print("Creating log file")
self.start_log("Logs", "NavLog.txt")
print("starting connection")
# self.connection.start()
super().start()
# note that the rest of the program executes here and you don't proceed until the connection is closed
print("Closing log file")
self.stop_log()
if __name__ == "__main__":
# conn = MavlinkConnection('tcp:127.0.0.1:5760', threaded=False, PX4=False)
conn = MavlinkConnection(
'udp:192.168.1.2:14550', PX4=True,
threaded=False) # insert actual IP address of wireless adapter
drone = BackyardFlyer(conn)
time.sleep(2)
# intialize the waypoints list
drone.all_waypoints = drone.calculate_box()
drone.start()
