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()