From 9b0db18a41256218039f6f5f1a2b35a8adaacace Mon Sep 17 00:00:00 2001 From: Unknown Date: Wed, 3 Oct 2018 13:18:44 -0400 Subject: [PATCH 01/16] getting started project added for dronekit / python and SITL tests --- gettingstarted/GCS/GCS_Runner.py | 73 +++++++++++ gettingstarted/GCS/__init__.py | 0 gettingstarted/GCS/simplegcs.py | 94 ++++++++++++++ gettingstarted/GCS/util.py | 173 +++++++++++++++++++++++++ gettingstarted/hello_drone.py | 40 ++++++ gettingstarted/simple_connect_DRONE.py | 14 ++ gettingstarted/simple_connect_SITL.py | 25 ++++ 7 files changed, 419 insertions(+) create mode 100644 gettingstarted/GCS/GCS_Runner.py create mode 100644 gettingstarted/GCS/__init__.py create mode 100644 gettingstarted/GCS/simplegcs.py create mode 100644 gettingstarted/GCS/util.py create mode 100644 gettingstarted/hello_drone.py create mode 100644 gettingstarted/simple_connect_DRONE.py create mode 100644 gettingstarted/simple_connect_SITL.py diff --git a/gettingstarted/GCS/GCS_Runner.py b/gettingstarted/GCS/GCS_Runner.py new file mode 100644 index 00000000..da148cac --- /dev/null +++ b/gettingstarted/GCS/GCS_Runner.py @@ -0,0 +1,73 @@ +import dronekit_sitl +import dronekit +import json +import argparse +import os +import threading +import time +import signal +import util +import logging +import simplegcs +from dronekit import connect, VehicleMode, LocationGlobalRelative + +# make sure you change this so that it's correct for your system +ARDUPATH = os.path.join('/', 'home', 'bayley', 'git', 'ardupilot') + + +def main(ardupath=None): + if ardupath is not None: + global ARDUPATH + ARDUPATH = ardupath + + + connect() + print("Registering Drones...") + vehicle1=addDrone([41.715446209367,-86.242847096132,0],"Frank") + + print("Taking off...") + vehicle1.simple_takeoff(10) + time.sleep(10) + print("Going somewhere...") + + gotoWaypoint(vehicle1,41.515446209367,-86.342847096132, 40,3) + + # point1 = LocationGlobalRelative(41.515446209367,-86.342847096132, 40) + # vehicle1.simple_goto(point1) + + # point2 = LocationGlobalRelative(41.515446209367, -86.342847096132, 40) + # vehicle2.simple_goto(point2) + +def connect(): + print("Connecting to Dronology...") + print("SITL path: "+ARDUPATH) + global GCS + GCS = simplegcs.SimpleGCS(ARDUPATH,"simplegcs") + GCS.connect() + time.sleep(10) + +def gotoWaypoint(vehicle,xcoord,ycoord,zcoord, airspeed=10): + vehicle.airspeed = airspeed + point = LocationGlobalRelative(xcoord,ycoord,zcoord) + vehicle.simple_goto(point) + +def addDrone(home, name=None): + vehicle = GCS.registerDrone(home,name) + time.sleep(5) + while not vehicle.is_armable: + print(" Waiting for vehicle to initialise...") + time.sleep(3) + print("Arming motors") + vehicle.mode = VehicleMode("GUIDED") + vehicle.armed = True + while not vehicle.armed: + print(" Waiting for arming...") + time.sleep(1) + return vehicle + +if __name__ == '__main__': + ap = argparse.ArgumentParser() + #ap.add_argument('path_to_config', type=str, help='the path to the drone configuration file.') + ap.add_argument('--ardupath', type=str, default=ARDUPATH) + args = ap.parse_args() + main(ardupath=args.ardupath) diff --git a/gettingstarted/GCS/__init__.py b/gettingstarted/GCS/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/gettingstarted/GCS/simplegcs.py b/gettingstarted/GCS/simplegcs.py new file mode 100644 index 00000000..8a12bde7 --- /dev/null +++ b/gettingstarted/GCS/simplegcs.py @@ -0,0 +1,94 @@ +import dronekit_sitl +import dronekit +import json +import argparse +import os +import threading +import time +import signal +import util +import logging + +_LOG = logging.getLogger(__name__) +_LOG.setLevel(logging.INFO) + +fh = logging.FileHandler('main.log', mode='w') +fh.setLevel(logging.INFO) +formatter = logging.Formatter('| %(levelname)6s | %(funcName)8s:%(lineno)2d | %(message)s |') +fh.setFormatter(formatter) +_LOG.addHandler(fh) + +DO_CONT = False +MESSAGE_FREQUENCY=1.0 + +class SimpleGCS: + sitls = [] + vehicles = {} + + def __init__(self,ardupath,g_id="default_groundstation"): + self.g_id=g_id + self.ardupath=ardupath + + def registerDrone(self, home,name,virtual=True): + if name is None: + name = get_vehicle_id(len(self.vehicles)) + + if virtual: + vehicle, sitl = self.connect_virtual_vehicle(len(self.vehicles), home) + self.sitls.append(sitl) + else: + vehicle = self.connect_physical_vehicle(home) + handshake = util.DroneHandshakeMessage.from_vehicle(vehicle, self.dronology._g_id,name) + + self.vehicles[name]=vehicle + self.dronology.send(str(handshake)) + print("New drone registered.."+handshake.__str__()) + return vehicle + + def connect(self): + self.dronology = util.Connection(None, "localhost", 1234,self.g_id) + self.dronology.start() + global DO_CONT + DO_CONT = True + w0 = threading.Thread(target=state_out_work, args=(self.dronology, self.vehicles)) + w0.start() + + def connect_virtual_vehicle(self,instance, home): + home_ = tuple(home) + (0,) + home_ = ','.join(map(str, home_)) + sitl_defaults = os.path.join(self.ardupath, 'Tools', 'autotest', 'default_params', 'copter.parm') + sitl_args = ['-I{}'.format(instance), '--home', home_, '--model', '+', '--defaults', sitl_defaults] + sitl = dronekit_sitl.SITL(path=os.path.join(self.ardupath, 'build', 'sitl', 'bin', 'arducopter')) + sitl.launch(sitl_args, await_ready=True) + + tcp, ip, port = sitl.connection_string().split(':') + port = str(int(port) + instance * 10) + conn_string = ':'.join([tcp, ip, port]) + + vehicle = dronekit.connect(conn_string) + vehicle.wait_ready(timeout=120) + + return vehicle, sitl + + def connect_physical_vehicle(self, home): + + vehicle = dronekit.connect(home, wait_ready=True) + vehicle.wait_ready(timeout=120) + + return vehicle + + +def get_vehicle_id(i): + return 'drone{}'.format(i) + + +def state_out_work(dronology, vehicles): + while DO_CONT: + # for i, v in enumerate(vehicles): + for name, v in vehicles.iteritems(): + state = util.StateMessage.from_vehicle(v,dronology._g_id,name) + state_str = str(state) + _LOG.info(state_str) + dronology.send(state_str) + + time.sleep(MESSAGE_FREQUENCY) diff --git a/gettingstarted/GCS/util.py b/gettingstarted/GCS/util.py new file mode 100644 index 00000000..c213cc6c --- /dev/null +++ b/gettingstarted/GCS/util.py @@ -0,0 +1,173 @@ +import json +import os +import threading +import socket +import time +from boltons import socketutils + + +class DronologyMessage(object): + def __init__(self, m_type,g_id, uav_id, data): + self.m_type = m_type + self.g_id=g_id + self.uav_id = uav_id + self.data = data + + def __str__(self): + return json.dumps({'type': self.m_type, + 'sendtimestamp': long(round(time.time() * 1000)), + 'uavid': str(self.uav_id), + 'groundstationid': str(self.g_id), + 'data': self.data}) + + def __repr__(self): + return str(self) + + +class DroneHandshakeMessage(DronologyMessage): + def __init__(self, g_id,uav_id, data, p2sac='../cfg/sac.json'): + super(DroneHandshakeMessage, self).__init__('handshake',g_id, uav_id, data) + self.p2sac = p2sac + + @classmethod + def from_vehicle(cls, vehicle,g_id, v_id, p2sac='../cfg/sac.json'): + battery = { + 'voltage': vehicle.battery.voltage, + 'current': vehicle.battery.current, + 'level': vehicle.battery.level, + } + + lla = vehicle.location.global_relative_frame + data = { + 'home': {'x': lla.lat, + 'y': lla.lon, + 'z': lla.alt}, + 'safetycase': json.dumps({})} + return cls(g_id,v_id, data) + + +class StateMessage(DronologyMessage): + def __init__(self, g_id, uav_id, data): + super(StateMessage, self).__init__('state',g_id, uav_id, data) + + @classmethod + def from_vehicle(cls, vehicle,g_id, v_id, **kwargs): + lla = vehicle.location.global_relative_frame + att = vehicle.attitude + vel = vehicle.velocity + battery = { + 'voltage': vehicle.battery.voltage, + 'current': vehicle.battery.current, + 'level': vehicle.battery.level, + } + data = { + 'location': {'x': lla.lat, 'y': lla.lon, 'z': lla.alt}, + 'attitude': {'x': att.roll, 'y': att.pitch, 'z': att.yaw}, + 'velocity': {'x': vel[0], 'y': vel[1], 'z': vel[2]}, + 'status': vehicle.system_status.state, + 'heading': vehicle.heading, + 'armable': vehicle.is_armable, + 'airspeed': vehicle.airspeed, + 'groundspeed': vehicle.airspeed, + 'armed': vehicle.armed, + 'mode': vehicle.mode.name, + 'batterystatus': battery + } + + return cls(g_id,v_id, data) + +class Connection: + _WAITING = 1 + _CONNECTED = 2 + _DEAD = -1 + + def __init__(self, msg_queue=None, addr='localhost', port=1234, g_id='default_groundstation'): + self._g_id = g_id + self._msgs = msg_queue + self._addr = addr + self._port = port + self._sock = None + self._conn_lock = threading.Lock() + self._status = Connection._WAITING + self._status_lock = threading.Lock() + self._msg_buffer = '' + + def get_status(self): + with self._status_lock: + return self._status + + def set_status(self, status): + with self._status_lock: + self._status = status + + def is_connected(self): + return self.get_status() == Connection._CONNECTED + + def start(self): + threading.Thread(target=self._work).start() + + def stop(self): + self.set_status(Connection._DEAD) + + def send(self, msg): + success = False + with self._conn_lock: + if self._status == Connection._CONNECTED: + try: + self._sock.send(msg) + self._sock.send(os.linesep) + success = True + except Exception as e: + print('failed to send message! ({})'.format(e)) + + return success + + def _work(self): + """ + Main loop. + 1. Wait for a connection + 2. Once connected, wait for commands from dronology + 3. If connection interrupted, wait for another connection again. + 4. Shut down when status is set to DEAD + :return: + """ + cont = True + while cont: + status = self.get_status() + if status == Connection._DEAD: + # Shut down + cont = False + elif status == Connection._WAITING: + # Try to connect, timeout after 10 seconds. + try: + sock = socket.create_connection((self._addr, self._port), timeout=5.0) + self._sock = socketutils.BufferedSocket(sock) + handshake = json.dumps({'type': 'connect', 'groundstationid': self._g_id}) + self._sock.send(handshake) + self._sock.send(os.linesep) + self.set_status(Connection._CONNECTED) + except socket.error as e: + print('Socket error ({})'.format(e)) + time.sleep(10.0) + else: + # Receive messages + try: + msg = self._sock.recv_until(os.linesep, timeout=0.1) + if self._msgs is not None: + self._msgs.append(msg) + except socket.timeout: + pass + except socket.error as e: + print('connection interrupted! ({})'.format(e)) + self._sock.shutdown(socket.SHUT_RDWR) + self._sock.close() + self._sock = None + self.set_status(Connection._WAITING) + time.sleep(20.0) + + if self._sock is not None: + print('Shutting down socket.') + self._sock.shutdown(socket.SHUT_WR) + print('Closing socket.') + self._sock.close() + return \ No newline at end of file diff --git a/gettingstarted/hello_drone.py b/gettingstarted/hello_drone.py new file mode 100644 index 00000000..4a956c4f --- /dev/null +++ b/gettingstarted/hello_drone.py @@ -0,0 +1,40 @@ +# Import DroneKit-Python +from dronekit import connect, VehicleMode, time + +#Setup option parsing to get connection string +import argparse +parser = argparse.ArgumentParser(description='Print out vehicle state information') +parser.add_argument('--connect',help="vehicle connection target string.") +args=parser.parse_args() + +connection_string = args.connect +sitl = None + + +#Start SITL if no connection string specified +if not connection_string: + import dronekit_sitl + sitl = dronekit_sitl.start_default() + connection_string = sitl.connection_string() + +# Connect to the Vehicle. +# Set `wait_ready=True` to ensure default attributes are populated before `connect()` returns. +print "\nConnecting to vehicle on: %s" % connection_string +vehicle = connect(connection_string, wait_ready=True) + +#vehicle.wait_ready('autopilot_version') + +#Get some vehicle attributes (state) +print "Get some vehicle attribute values:" +print "GPS: %s" % vehicle.gps_0 +print "Battery: %s" % vehicle.battery +print "Last Heartbeat: %s" % vehicle.last_heartbeat +print "Is Armable?: %s" % vehicle.system_status.state +print "Mode: %s" % vehicle.mode.name # settable + +# Close vehicle object before exiting script +vehicle.close() + +time.sleep(5) + +print("Completed") \ No newline at end of file diff --git a/gettingstarted/simple_connect_DRONE.py b/gettingstarted/simple_connect_DRONE.py new file mode 100644 index 00000000..dab1bba5 --- /dev/null +++ b/gettingstarted/simple_connect_DRONE.py @@ -0,0 +1,14 @@ +from dronekit import connect, VehicleMode, time +import dronekit +import os + + +vehicle = dronekit.connect("/dev/ttyUSB0",wait_ready=True) +vehicle.wait_ready(timeout=120) + +while not vehicle.is_armable: + print(" Waiting for vehicle to initialise...") + time.sleep(3) +print("Arming motors") +vehicle.mode = VehicleMode("GUIDED") +vehicle.armed = True \ No newline at end of file diff --git a/gettingstarted/simple_connect_SITL.py b/gettingstarted/simple_connect_SITL.py new file mode 100644 index 00000000..ace55fed --- /dev/null +++ b/gettingstarted/simple_connect_SITL.py @@ -0,0 +1,25 @@ +from dronekit import connect, VehicleMode, time +import dronekit_sitl +import dronekit +import os + +ardupath ="/win/Work/git/ardupilot" +home="41.715446209367,-86.242847096132" +sitl_defaults = os.path.join(ardupath, 'Tools', 'autotest', 'default_params', 'copter.parm') +sitl_args = ['-I{}'.format(0), '--home', home, '--model', '+', '--defaults', sitl_defaults] +sitl = dronekit_sitl.SITL(path=os.path.join(ardupath, 'build', 'sitl', 'bin', 'arducopter')) +sitl.launch(sitl_args, await_ready=True) + +tcp, ip, port = sitl.connection_string().split(':') +port = str(int(port) + 0 * 10) +conn_string = ':'.join([tcp, ip, port]) + +vehicle = dronekit.connect(conn_string) +vehicle.wait_ready(timeout=120) + +while not vehicle.is_armable: + print(" Waiting for vehicle to initialise...") + time.sleep(3) +print("Arming motors") +vehicle.mode = VehicleMode("GUIDED") +vehicle.armed = True \ No newline at end of file From 53b97a373aee8884137cb3d55e07ebc3cb57756d Mon Sep 17 00:00:00 2001 From: Unknown Date: Wed, 3 Oct 2018 14:15:48 -0400 Subject: [PATCH 02/16] startup & install scripts added --- gettingstarted/run_sitl.sh | 10 ++++++ gettingstarted/setup_dronekit.sh | 60 ++++++++++++++++++++++++++++++++ 2 files changed, 70 insertions(+) create mode 100644 gettingstarted/run_sitl.sh create mode 100644 gettingstarted/setup_dronekit.sh diff --git a/gettingstarted/run_sitl.sh b/gettingstarted/run_sitl.sh new file mode 100644 index 00000000..c8f8ec53 --- /dev/null +++ b/gettingstarted/run_sitl.sh @@ -0,0 +1,10 @@ +#cd "$(dirname "$(realpath "$0")")" +REPO=/home/uav/git/ardupilot/ArduCopter + +pkill -9 python + +gnome-terminal -x bash -c "dronekit-sitl copter --home=41.71480,-86.24187" +gnome-terminal --working-directory=$REPO -x bash -c "python3 ../Tools/autotest/sim_vehicle.py -j4 --map --console" + + + diff --git a/gettingstarted/setup_dronekit.sh b/gettingstarted/setup_dronekit.sh new file mode 100644 index 00000000..89734854 --- /dev/null +++ b/gettingstarted/setup_dronekit.sh @@ -0,0 +1,60 @@ +#! /bin/bash +################################ MODIFY REPO PARAMETERS HERE ############################## +# Clone the Dronology Repository +# modify these two pointers if you want to change where the repos should be cloned +# defaults to $HOME/git (e.g., /home/bayley/git) +# will fail and/or result in unexpected behavior if parent directory does not exist or is not an absolute path! +export REPOS_PARENT_DIR=$HOME +export REPOS_DIR=git +################################ MODIFY REPO PARAMETERS HERE ############################## + +# prereqs +export CURR_VER=`lsb_release -rs` +function version_ge() { test "$(echo "$@" | tr " " "\n" | sort -rV | head -n 1)" == "$1"; } + +# install python +sudo apt-get install software-properties-common +sudo add-apt-repository ppa:deadsnakes/ppa +sudo apt-get update +sudo apt-get install python2.7 +# install pip +sudo apt-get install python-pip +# install git +sudo apt-get install git + + +cd $REPOS_PARENT_DIR +# makes the directory if it doesn't already exist +mkdir $REPOS_DIR +cd $REPOS_DIR + +# Clone the ArduPilot repository. +git clone git://github.com/ArduPilot/ardupilot.git +cd ardupilot +git submodule update --init --recursive + +# Install ardupilot dependencies +if version_ge $CURR_VER '16.0'; then +export WXGTK_VERSION=3.0 +else +export WXGTK_VERSION=2.8 +fi +echo "Linux Version is: $CURR_VER -- wxgtk $WXGTK_VERSION needs to be installed" +sudo apt-get install python-dev python-opencv python-wxgtk$WXGTK_VERSION python-pip python-matplotlib python-pygame python-lxml +sudo pip install future pymavlink MAVProxy + +sudo pip install pyudev +sudo pip install PyYAML +sudo pip install nvector +sudo pip install boltons +sudo pip install dronekit +sudo pip install dronekit-sitl + + +# Build SITL +export PATH=$PATH:$REPOS_PARENT_DIR/$REPOS_DIR/ardupilot/Tools/autotest +export PATH=/usr/lib/ccache:$PATH +cd ArduCopter +sim_vehicle.py -w -j4 --map + + From 5a1ceff1b1b024f35ec32c169ad076a9ce0be615 Mon Sep 17 00:00:00 2001 From: Jane Cleland Huang Date: Tue, 8 Sep 2020 22:31:02 -0400 Subject: [PATCH 03/16] Fixed WXGTK Version to 3.0 always --- gettingstarted/setup_dronekit.sh | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/gettingstarted/setup_dronekit.sh b/gettingstarted/setup_dronekit.sh index 89734854..59d3b5aa 100644 --- a/gettingstarted/setup_dronekit.sh +++ b/gettingstarted/setup_dronekit.sh @@ -34,11 +34,11 @@ cd ardupilot git submodule update --init --recursive # Install ardupilot dependencies -if version_ge $CURR_VER '16.0'; then +#if version_ge $CURR_VER '16.0'; then export WXGTK_VERSION=3.0 -else -export WXGTK_VERSION=2.8 -fi +#else +#export WXGTK_VERSION=2.8 +#fi echo "Linux Version is: $CURR_VER -- wxgtk $WXGTK_VERSION needs to be installed" sudo apt-get install python-dev python-opencv python-wxgtk$WXGTK_VERSION python-pip python-matplotlib python-pygame python-lxml sudo pip install future pymavlink MAVProxy From da36e0fa066b900cc0d8292613e5d90bb26f5b0a Mon Sep 17 00:00:00 2001 From: Jane Cleland Huang Date: Thu, 10 Sep 2020 07:20:58 -0400 Subject: [PATCH 04/16] Hard coded in dronekit version numbers dronekit==2.9.1 dronekit-sitl==3.2.0 --- gettingstarted/setup_dronekit.sh | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gettingstarted/setup_dronekit.sh b/gettingstarted/setup_dronekit.sh index 59d3b5aa..5ce51447 100644 --- a/gettingstarted/setup_dronekit.sh +++ b/gettingstarted/setup_dronekit.sh @@ -47,8 +47,8 @@ sudo pip install pyudev sudo pip install PyYAML sudo pip install nvector sudo pip install boltons -sudo pip install dronekit -sudo pip install dronekit-sitl +sudo pip install dronekit==2.9.1 +sudo pip install dronekit-sitl==3.2.0 # Build SITL From 58ab5f54f3335d629fdc38f4cac07edf0b213457 Mon Sep 17 00:00:00 2001 From: Jane Cleland Huang Date: Thu, 10 Sep 2020 20:49:17 -0700 Subject: [PATCH 05/16] Create simple_goto.py --- gettingstarted/simple_goto.py | 101 ++++++++++++++++++++++++++++++++++ 1 file changed, 101 insertions(+) create mode 100644 gettingstarted/simple_goto.py diff --git a/gettingstarted/simple_goto.py b/gettingstarted/simple_goto.py new file mode 100644 index 00000000..b7c00b76 --- /dev/null +++ b/gettingstarted/simple_goto.py @@ -0,0 +1,101 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + +""" +© Copyright 2015-2016, 3D Robotics. +simple_goto.py: GUIDED mode "simple goto" example (Copter Only) +Demonstrates how to arm and takeoff in Copter and how to navigate to points using Vehicle.simple_goto. +Full documentation is provided at http://python.dronekit.io/examples/simple_goto.html +""" + +from __future__ import print_function +import time +from dronekit import connect, VehicleMode, LocationGlobalRelative + + +# Set up option parsing to get connection string +import argparse +parser = argparse.ArgumentParser(description='Commands vehicle using vehicle.simple_goto.') +parser.add_argument('--connect', + help="Vehicle connection target string. If not specified, SITL automatically started and used.") +args = parser.parse_args() + +connection_string = args.connect +sitl = None + + +# Start SITL if no connection string specified +if not connection_string: + import dronekit_sitl + sitl = dronekit_sitl.start_default() + connection_string = sitl.connection_string() + + +# Connect to the Vehicle +print('Connecting to vehicle on: %s' % connection_string) +vehicle = connect(connection_string, wait_ready=True) + + +def arm_and_takeoff(aTargetAltitude): + """ + Arms vehicle and fly to aTargetAltitude. + """ + + print("Basic pre-arm checks") + # Don't try to arm until autopilot is ready + while not vehicle.is_armable: + print(" Waiting for vehicle to initialise...") + time.sleep(3) + print("Arming motors") + vehicle.mode = VehicleMode("GUIDED") + vehicle.armed = True + + while not vehicle.armed: + print(" Waiting for arming...") + time.sleep(1) + + print("Vehicle armed!") + print("Taking off!") + vehicle.simple_takeoff(aTargetAltitude) # Take off to target altitude + + # Wait until the vehicle reaches a safe height before processing the goto + # (otherwise the command after Vehicle.simple_takeoff will execute + # immediately). + while True: + print(" Altitude: ", vehicle.location.global_relative_frame.alt) + # Break and return from function just below target altitude. + if vehicle.location.global_relative_frame.alt >= aTargetAltitude * 0.95: + print("Reached target altitude") + break + time.sleep(1) + + +arm_and_takeoff(10) + +print("Set default/target airspeed to 3") +vehicle.airspeed = 3 + +print("Going towards first point for 30 seconds ...") +point1 = LocationGlobalRelative(-35.361354, 149.165218, 20) +vehicle.simple_goto(point1) + +# sleep so we can see the change in map +time.sleep(30) + +print("Going towards second point for 30 seconds (groundspeed set to 10 m/s) ...") +point2 = LocationGlobalRelative(-35.363244, 149.168801, 20) +vehicle.simple_goto(point2, groundspeed=10) + +# sleep so we can see the change in map +time.sleep(30) + +print("Returning to Launch") +vehicle.mode = VehicleMode("RTL") + +# Close vehicle object before exiting script +print("Close vehicle object") +vehicle.close() + +# Shut down simulator if it was started. +if sitl: + sitl.stop() From f36a43db28b638108703c1b547d118157d294596 Mon Sep 17 00:00:00 2001 From: Jane Cleland Huang Date: Thu, 10 Sep 2020 20:51:59 -0700 Subject: [PATCH 06/16] Update simple_goto.py --- gettingstarted/simple_goto.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gettingstarted/simple_goto.py b/gettingstarted/simple_goto.py index b7c00b76..93bfc7e9 100644 --- a/gettingstarted/simple_goto.py +++ b/gettingstarted/simple_goto.py @@ -98,4 +98,4 @@ def arm_and_takeoff(aTargetAltitude): # Shut down simulator if it was started. if sitl: - sitl.stop() + sitl.stop() From d7917bb3346b3f9ccb68344b4b40e34ba16a96c9 Mon Sep 17 00:00:00 2001 From: Jane Cleland Huang Date: Sun, 13 Sep 2020 21:01:13 -0700 Subject: [PATCH 07/16] Added home coordinates to startup --- gettingstarted/simple_goto.py | 22 ++++++++++++---------- 1 file changed, 12 insertions(+), 10 deletions(-) diff --git a/gettingstarted/simple_goto.py b/gettingstarted/simple_goto.py index 93bfc7e9..e45b6cb1 100644 --- a/gettingstarted/simple_goto.py +++ b/gettingstarted/simple_goto.py @@ -10,8 +10,8 @@ from __future__ import print_function import time -from dronekit import connect, VehicleMode, LocationGlobalRelative - +from dronekit_sitl import SITL +from dronekit import Vehicle, VehicleMode, connect, LocationGlobalRelative # Set up option parsing to get connection string import argparse @@ -19,22 +19,24 @@ parser.add_argument('--connect', help="Vehicle connection target string. If not specified, SITL automatically started and used.") args = parser.parse_args() - connection_string = args.connect sitl = None # Start SITL if no connection string specified +# This technique for starting SITL allows us to specify defffaults if not connection_string: - import dronekit_sitl - sitl = dronekit_sitl.start_default() - connection_string = sitl.connection_string() - + sitl_defaults = '~/git/ardupilot/tools/autotest/default_params/copter.parm' + sitl = SITL() + sitl.download('copter', '3.3', verbose=True) + sitl_args = ['-I0', '--model', 'quad', '--home=35.361350, 149.165210,0,180'] + sitl.launch(sitl_args, await_ready=True, restart=True) + connection_string = 'tcp:127.0.0.1:5760' # Connect to the Vehicle print('Connecting to vehicle on: %s' % connection_string) -vehicle = connect(connection_string, wait_ready=True) - +vehicle = connect(connection_string, wait_ready=True, baud=57600) +print ('Current position of vehicle is: %s' % vehicle.location.global_frame) def arm_and_takeoff(aTargetAltitude): """ @@ -98,4 +100,4 @@ def arm_and_takeoff(aTargetAltitude): # Shut down simulator if it was started. if sitl: - sitl.stop() + sitl.stop() From 8ddec1e32295a9a2a78f03620f11909aac7f795f Mon Sep 17 00:00:00 2001 From: Jane Cleland Huang Date: Mon, 14 Sep 2020 00:14:46 -0400 Subject: [PATCH 08/16] fixed indent --- gettingstarted/simple_goto.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gettingstarted/simple_goto.py b/gettingstarted/simple_goto.py index e45b6cb1..ff726d7a 100644 --- a/gettingstarted/simple_goto.py +++ b/gettingstarted/simple_goto.py @@ -100,4 +100,4 @@ def arm_and_takeoff(aTargetAltitude): # Shut down simulator if it was started. if sitl: - sitl.stop() + sitl.stop() From a96d46321df21f8644e1af29f690d6f9e3912bd8 Mon Sep 17 00:00:00 2001 From: Jane Cleland Huang Date: Wed, 16 Sep 2020 13:53:08 -0400 Subject: [PATCH 09/16] Multiple Drone program examples --- .../multiple_drones/multiple_drones1.py | 106 ++++++++++++++ .../multiple_drones/multiple_drones2.py | 81 +++++++++++ .../multiple_drones/multiple_drones3.py | 134 ++++++++++++++++++ 3 files changed, 321 insertions(+) create mode 100644 gettingstarted/multiple_drones/multiple_drones1.py create mode 100644 gettingstarted/multiple_drones/multiple_drones2.py create mode 100644 gettingstarted/multiple_drones/multiple_drones3.py diff --git a/gettingstarted/multiple_drones/multiple_drones1.py b/gettingstarted/multiple_drones/multiple_drones1.py new file mode 100644 index 00000000..2b7286a6 --- /dev/null +++ b/gettingstarted/multiple_drones/multiple_drones1.py @@ -0,0 +1,106 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + +""" +© Copyright 2015-2016, 3D Robotics. +simple_goto.py: GUIDED mode "simple goto" example (Copter Only) +Demonstrates how to arm and takeoff in Copter and how to navigate to points using Vehicle.simple_goto. +Full documentation is provided at http://python.dronekit.io/examples/simple_goto.html +""" + +import time +from dronekit_sitl import SITL +from dronekit import Vehicle, VehicleMode, connect, LocationGlobalRelative + +def connect_virtual_vehicle(instance, home): + sitl = SITL() + sitl.download('copter', '3.3', verbose=True) + instance_arg = '-I%s' %(str(instance)) + home_arg = '--home=%s, %s,%s,180' % (str(home[0]), str(home[1]), str(home[2])) + sitl_args = [instance_arg, '--model', 'quad', home_arg] + sitl.launch(sitl_args, await_ready=True) + tcp, ip, port = sitl.connection_string().split(':') + port = str(int(port) + instance * 10) + conn_string = ':'.join([tcp, ip, port]) + print('Connecting to vehicle on: %s' % conn_string) + + vehicle = connect(conn_string) + vehicle.wait_ready(timeout=120) + print("Reached here") + return vehicle, sitl + +def arm_and_takeoff(aTargetAltitude, vehicle): + """ + Arms vehicle and fly to aTargetAltitude. + """ + + print("Basic pre-arm checks") + # Don't try to arm until autopilot is ready + while not vehicle.is_armable: + print(" Waiting for vehicle to initialise...") + time.sleep(3) + print("Arming motors") + vehicle.mode = VehicleMode("GUIDED") + vehicle.armed = True + + while not vehicle.armed: + print(" Waiting for arming...") + time.sleep(1) + + print("Vehicle armed!") + print("Taking off!") + vehicle.simple_takeoff(aTargetAltitude) # Take off to target altitude + + # Wait until the vehicle reaches a safe height before processing the goto + # (otherwise the command after Vehicle.simple_takeoff will execute + # immediately). + while True: + print(" Altitude: ", vehicle.location.global_relative_frame.alt) + # Break and return from function just below target altitude. + if vehicle.location.global_relative_frame.alt >= aTargetAltitude * 0.95: + print("Reached target altitude") + break + time.sleep(1) + +def fly_drone(vehicle): + arm_and_takeoff(10, vehicle) + print("Set default/target airspeed to 3") + vehicle.airspeed = 3 + + print("Going towards first point for 30 seconds ...") + point1 = LocationGlobalRelative(-35.361354, 149.165218, 20) + print (point1) + vehicle.simple_goto(point1) + + # sleep so we can see the change in map + time.sleep(10) + + print("Going towards second point for 30 seconds (groundspeed set to 10 m/s) ...") + point2 = LocationGlobalRelative(-35.363244, 149.168801, 20) + vehicle.simple_goto(point2, groundspeed=10) + + # sleep so we can see the change in map + time.sleep(10) + + print("Returning to Launch") + vehicle.mode = VehicleMode("LAND") + print (vehicle.location.global_frame) + + # Close vehicle object before exiting script + print("Close vehicle object") + vehicle.close() + +vehicle, sitl = connect_virtual_vehicle(0,([41.715446209367,-86.242847096132,0])) +vehicle2, sitl2 = connect_virtual_vehicle(1,([41.715469, -86.242543,0])) + +print("flying drone 1") +arm_and_takeoff(10, vehicle) + +print("flying drone 2") +arm_and_takeoff(10, vehicle2) + + +# Shut down simulator if it was started. +if sitl: + sitl.stop() + sitl2.stop() diff --git a/gettingstarted/multiple_drones/multiple_drones2.py b/gettingstarted/multiple_drones/multiple_drones2.py new file mode 100644 index 00000000..a60871f5 --- /dev/null +++ b/gettingstarted/multiple_drones/multiple_drones2.py @@ -0,0 +1,81 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + +""" +© Copyright 2015-2016, 3D Robotics. +simple_goto.py: GUIDED mode "simple goto" example (Copter Only) +Demonstrates how to arm and takeoff in Copter and how to navigate to points using Vehicle.simple_goto. +Full documentation is provided at http://python.dronekit.io/examples/simple_goto.html +""" + +#from __future__ import print_function +import time +from dronekit_sitl import SITL +from dronekit import Vehicle, VehicleMode, connect, LocationGlobalRelative + +def connect_virtual_vehicle(instance, home): + sitlx = SITL() + sitlx.download('copter', '3.3', verbose=True) + instance_arg = '-I%s' %(str(instance)) + print("Drone instance is: %s" % instance_arg) + home_arg = '--home=%s, %s,%s,180' % (str(home[0]), str(home[1]), str(home[2])) + sitl_args = [instance_arg, '--model', 'quad', home_arg] + sitlx.launch(sitl_args, await_ready=True) + tcp, ip, port = sitlx.connection_string().split(':') + port = str(int(port) + instance * 10) + conn_string = ':'.join([tcp, ip, port]) + print('Connecting to vehicle on: %s' % conn_string) + + vehicle = connect(conn_string) + vehicle.wait_ready(timeout=120) + print("Reached here") + return vehicle, sitlx + +def arm_and_takeoff(aTargetAltitude): + """ + Arms vehicle and fly to aTargetAltitude. + """ + print("Basic pre-arm checks") + # Don't try to arm until autopilot is ready + while not (vehicle.is_armable and vehicle2.is_armable): + if (not vehicle.is_armable): + print(" Waiting for vehicle 1 to initialise...") + if (not vehicle2.is_armable): + print(" Waiting for vehicle 2 to initialise...") + time.sleep(3) + + print("Arming motors") + vehicle.mode = VehicleMode("GUIDED") + vehicle.armed = True + vehicle2.mode = VehicleMode("GUIDED") + vehicle2.armed = True + + while not (vehicle.armed and vehicle2.armed): + print(" Waiting for arming...") + time.sleep(1) + + print("Vehicle armed!") + print("Both drones are now Taking off!") + vehicle.simple_takeoff(aTargetAltitude) # Take off to target altitude + vehicle2.simple_takeoff(aTargetAltitude) + + # Wait for both drones + while True: + print(" Altitude V1: ", vehicle.location.global_relative_frame.alt) + print(" Altitude V2: ", vehicle2.location.global_relative_frame.alt) + + # Break and return from function just below target altitude. + if vehicle.location.global_relative_frame.alt >= aTargetAltitude * 0.95 and vehicle.location.global_relative_frame.alt >= aTargetAltitude * 0.95: + print("Both Drones Reached target altitude") + break + time.sleep(1) + +vehicle, sitl = connect_virtual_vehicle(0,([41.715446209367,-86.242847096132,0])) +vehicle2, sitl2 = connect_virtual_vehicle(1,([41.715469, -86.242543,0])) + +arm_and_takeoff(10) + +# Shut down simulator if it was started. +if sitl: + sitl.stop() + sitl2.stop() diff --git a/gettingstarted/multiple_drones/multiple_drones3.py b/gettingstarted/multiple_drones/multiple_drones3.py new file mode 100644 index 00000000..d78a3728 --- /dev/null +++ b/gettingstarted/multiple_drones/multiple_drones3.py @@ -0,0 +1,134 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + +""" +© Copyright 2015-2016, 3D Robotics. +simple_goto.py: GUIDED mode "simple goto" example (Copter Only) +Demonstrates how to arm and takeoff in Copter and how to navigate to points using Vehicle.simple_goto. +Full documentation is provided at http://python.dronekit.io/examples/simple_goto.html +""" + +import time +from dronekit_sitl import SITL +from dronekit import Vehicle, VehicleMode, connect, LocationGlobalRelative + +copters = [] +sitls = [] + +def connect_virtual_vehicle(instance, home): + sitl = SITL() + sitl.download('copter', '3.3', verbose=True) + instance_arg = '-I%s' %(str(instance)) + print("Drone instance is: %s" % instance_arg) + home_arg = '--home=%s, %s,%s,180' % (str(home[0]), str(home[1]), str(home[2])) + sitl_args = [instance_arg, '--model', 'quad', home_arg] + sitl.launch(sitl_args, await_ready=True) + tcp, ip, port = sitl.connection_string().split(':') + port = str(int(port) + instance * 10) + conn_string = ':'.join([tcp, ip, port]) + print('Connecting to vehicle on: %s' % conn_string) + + vehicle = connect(conn_string) + vehicle.wait_ready(timeout=120) + + # Collections + copters.append(vehicle) + sitls.append(sitl) + +def copters_armable(): + unarmed = False + for c in copters: + print ("Trying one") + if (not c.is_armable): + unarmed = True + else: + print ("Copter armed") + time.sleep(3) + return not unarmed + +def copters_at_altitude(aTargetAltitude): + while True: + at_altitude = True + ctr=1 + for c in copters: + print ('Copter ID: {} at altitude {} '.format(ctr,str(c.location.global_relative_frame.alt))) + ctr = ctr + 1 + if (not c.location.global_relative_frame.alt >= aTargetAltitude * 0.95): + at_altitude = False + time.sleep(3) + + if at_altitude == True: + print("All drones have reached their target altitudes") + break + +def copters_arm(): + for c in copters: + c.mode = VehicleMode("GUIDED") + c.armed = True + + for c in copters: + while not (c.armed): + time.sleep(1) + +def land_drones(): + for c in copters: + c.mode = VehicleMode("LAND") + print ("LANDING....") + time.sleep(30) + +def copters_armable(): + + while True: + unarmable = False + for c in copters: + if (not c.is_armable): + unarmable = True + time.sleep(3) + + if unarmable == False: + break + + +def arm_and_takeoff(aTargetAltitude): + """ + Arms vehicle and fly to aTargetAltitude. + """ + print("Basic pre-arm checks") + # Don't try to arm until autopilot is ready + copters_armable() + + print("Arming motors") + copters_arm() + + print("Vehicle armed!") + + print("All drones are now Taking off!") + aTargetAltitude = 10 + for c in copters: + c.simple_takeoff(aTargetAltitude) # Take off to target altitude + + print("Waiting for copters to ascend") + copters_at_altitude(aTargetAltitude) + +# Starting coordinates +coordinates = [41.714469, -86.241786,0] + +# Copter list +for n in range(5): + coordinates = [coordinates[0],coordinates[1]-(0.00005*n),coordinates[2]] + connect_virtual_vehicle(n,coordinates) + +# Arm and takeoff to 10 meters +arm_and_takeoff(10) + +# Land them +land_drones() + +# Close all vehicles +for c in copters: + c.close() + +# Shut down simulators +for s in sitls: + s.stop() + From 3f60292abf1c0de0b38533617e903ebffda03d64 Mon Sep 17 00:00:00 2001 From: Jane Cleland Huang Date: Wed, 16 Sep 2020 16:48:05 -0400 Subject: [PATCH 10/16] Removed extra armable function! --- gettingstarted/multiple_drones/multiple_drones3.py | 11 ----------- 1 file changed, 11 deletions(-) diff --git a/gettingstarted/multiple_drones/multiple_drones3.py b/gettingstarted/multiple_drones/multiple_drones3.py index d78a3728..fc16e33b 100644 --- a/gettingstarted/multiple_drones/multiple_drones3.py +++ b/gettingstarted/multiple_drones/multiple_drones3.py @@ -35,17 +35,6 @@ def connect_virtual_vehicle(instance, home): copters.append(vehicle) sitls.append(sitl) -def copters_armable(): - unarmed = False - for c in copters: - print ("Trying one") - if (not c.is_armable): - unarmed = True - else: - print ("Copter armed") - time.sleep(3) - return not unarmed - def copters_at_altitude(aTargetAltitude): while True: at_altitude = True From 1fcf0bcf262d68573fd4e89163d6f7f4b0545f52 Mon Sep 17 00:00:00 2001 From: uav Date: Mon, 21 Sep 2020 04:05:23 +0530 Subject: [PATCH 11/16] added websocker server and google maps to visualize drone movements on the map --- gettingstarted/drone_model.py | 17 ++++++ gettingstarted/server.py | 33 ++++++++++++ gettingstarted/simple_goto.py | 46 ++++++++++++++++- gettingstarted/utility.py | 11 ++++ gettingstarted/webApp/map.css | 5 ++ gettingstarted/webApp/map.html | 19 +++++++ gettingstarted/webApp/map.js | 94 ++++++++++++++++++++++++++++++++++ 7 files changed, 223 insertions(+), 2 deletions(-) create mode 100644 gettingstarted/drone_model.py create mode 100644 gettingstarted/server.py create mode 100644 gettingstarted/utility.py create mode 100644 gettingstarted/webApp/map.css create mode 100644 gettingstarted/webApp/map.html create mode 100644 gettingstarted/webApp/map.js diff --git a/gettingstarted/drone_model.py b/gettingstarted/drone_model.py new file mode 100644 index 00000000..8db59343 --- /dev/null +++ b/gettingstarted/drone_model.py @@ -0,0 +1,17 @@ +import json +class Drone_Model: + lat=0 + lon=0 + def __init__(self, id, lat, lon): + self.id = id + self.lat = lat + self.lon = lon + + def update_status(self, lat, lon): + self.lat = lat + self.lon = lon + + + def toJSON(self): + return json.dumps(self, default=lambda o: o.__dict__,sort_keys=True, indent=4) + \ No newline at end of file diff --git a/gettingstarted/server.py b/gettingstarted/server.py new file mode 100644 index 00000000..f639b2b0 --- /dev/null +++ b/gettingstarted/server.py @@ -0,0 +1,33 @@ +from simple_websocket_server import WebSocketServer, WebSocket + + +class WebServer(WebSocket): + def handle(self): + for client in clients: + if client != self: + client.send_message(self.data) + + def connected(self): + print(self.address, 'connected') + for client in clients: + client.send_message(self.address[0] + u' - connected') + clients.append(self) + + def handle_close(self): + clients.remove(self) + print(self.address, 'closed') + for client in clients: + client.send_message(self.address[0] + u' - disconnected') + + # def send(self, data): + # for client in clients: + # if client !=self: + # client.send_message(data) + + + + +clients = [] + +server = WebSocketServer('', 8000, WebServer) +server.serve_forever() \ No newline at end of file diff --git a/gettingstarted/simple_goto.py b/gettingstarted/simple_goto.py index ff726d7a..2b63b4b1 100644 --- a/gettingstarted/simple_goto.py +++ b/gettingstarted/simple_goto.py @@ -13,6 +13,14 @@ from dronekit_sitl import SITL from dronekit import Vehicle, VehicleMode, connect, LocationGlobalRelative + +import json +from websocket import create_connection +from drone_model import Drone_Model +ws = create_connection("ws://localhost:8000") + +drone_model_object = Drone_Model(1,0,0) + # Set up option parsing to get connection string import argparse parser = argparse.ArgumentParser(description='Commands vehicle using vehicle.simple_goto.') @@ -28,6 +36,7 @@ if not connection_string: sitl_defaults = '~/git/ardupilot/tools/autotest/default_params/copter.parm' sitl = SITL() + sitl.download('copter', '3.3', verbose=True) sitl_args = ['-I0', '--model', 'quad', '--home=35.361350, 149.165210,0,180'] sitl.launch(sitl_args, await_ready=True, restart=True) @@ -38,6 +47,15 @@ vehicle = connect(connection_string, wait_ready=True, baud=57600) print ('Current position of vehicle is: %s' % vehicle.location.global_frame) + +def custom_sleep(drone_model, sleep_time): + current_time = 0 + while(current_time= aTargetAltitude * 0.95: print("Reached target altitude") @@ -72,8 +92,13 @@ def arm_and_takeoff(aTargetAltitude): time.sleep(1) + + arm_and_takeoff(10) + + + print("Set default/target airspeed to 3") vehicle.airspeed = 3 @@ -81,15 +106,21 @@ def arm_and_takeoff(aTargetAltitude): point1 = LocationGlobalRelative(-35.361354, 149.165218, 20) vehicle.simple_goto(point1) + + + # sleep so we can see the change in map -time.sleep(30) +#time.sleep(30) +custom_sleep(drone_model_object,30) + print("Going towards second point for 30 seconds (groundspeed set to 10 m/s) ...") point2 = LocationGlobalRelative(-35.363244, 149.168801, 20) vehicle.simple_goto(point2, groundspeed=10) # sleep so we can see the change in map -time.sleep(30) +#time.sleep(30) +custom_sleep(drone_model_object,30) print("Returning to Launch") vehicle.mode = VehicleMode("RTL") @@ -101,3 +132,14 @@ def arm_and_takeoff(aTargetAltitude): # Shut down simulator if it was started. if sitl: sitl.stop() + + + + + + + + + + + diff --git a/gettingstarted/utility.py b/gettingstarted/utility.py new file mode 100644 index 00000000..9d3ee539 --- /dev/null +++ b/gettingstarted/utility.py @@ -0,0 +1,11 @@ +class Utility: + + def __init__(self): + pass + def custom_sleep(self, vehicle,drone_model, sleep_time): + current_time = 0 + while(current_time + + + + + + + + + +
+ + + + + diff --git a/gettingstarted/webApp/map.js b/gettingstarted/webApp/map.js new file mode 100644 index 00000000..4dea67a0 --- /dev/null +++ b/gettingstarted/webApp/map.js @@ -0,0 +1,94 @@ +marker_count=0; +markers = {} +number_drones = 1 +function initMap() { + + // map center on the screen - 50,50 + // change it according to the home location of the drone. + var myLatLng = new google.maps.LatLng( 41.714469, -86.241786 ) + myOptions = { + zoom: 4, + center: myLatLng, + mapTypeId: google.maps.MapTypeId.ROADMAP + } + map = new google.maps.Map( document.getElementById( 'map-canvas' ), myOptions ) + + add_markers(myLatLng, map) + + + + marker.setMap( map ); + //markers[1] = marker1; + + //marker2.setMap(map); + //marker3.setMap(map); + +} +function get_marker_for_drone(drone_id){ + return markers[drone_id] + +} +function add_markers(position,map){ + while(marker_count!=number_drones){ + marker_count+=1; + marker = new google.maps.Marker( {position: position, map: map} ); + markers[marker_count] = marker; + + } + console.log(markers); +} + +function move(id,lat,lon) { + console.log("moving", id, lat, lon); + marker_obj = get_marker_for_drone(id) + console.log(marker_obj); + marker_obj.setPosition( new google.maps.LatLng( lat, lon ) ); + + + // map.panTo( new google.maps.LatLng( 0, 0 ) ); + +}; + +initMap(); +doConnect(); + + + +function doConnect() +{ + websocket = new WebSocket('ws://localhost:8000/'); + websocket.onopen = function(evt) { onOpen(evt) }; + websocket.onclose = function(evt) { onClose(evt) }; + websocket.onmessage = function(evt) { onMessage(evt) }; + websocket.onerror = function(evt) { onError(evt) }; +} + +function onOpen(evt) +{ +console.log("connected\n"); + +} + +function onClose(evt) +{ +console.log("disconnected\n"); + +} + +function onMessage(evt) +{ + drone = JSON.parse(evt.data); + + console.log(drone+'\n'); + move(drone.id, drone.lat,drone.lon) + +} + +function onError(evt) +{ + console.log('error: ' + evt.data + '\n'); + +websocket.close(); + +} + From 29821f2a563cca8fce1662370163dcd6d5bec823 Mon Sep 17 00:00:00 2001 From: uav Date: Wed, 23 Sep 2020 00:05:24 +0530 Subject: [PATCH 12/16] move multiple drones on the map --- gettingstarted/multiple_drones/drone_model.py | 16 ++++++ .../multiple_drones/multiple_drones1.py | 57 +++++++++++++++---- .../multiple_drones/multiple_drones3.py | 4 ++ gettingstarted/webApp/map.js | 5 +- 4 files changed, 68 insertions(+), 14 deletions(-) create mode 100644 gettingstarted/multiple_drones/drone_model.py diff --git a/gettingstarted/multiple_drones/drone_model.py b/gettingstarted/multiple_drones/drone_model.py new file mode 100644 index 00000000..fb597cf8 --- /dev/null +++ b/gettingstarted/multiple_drones/drone_model.py @@ -0,0 +1,16 @@ +import json +class Drone_Model: + lat=0 + lon=0 + def __init__(self, id, lat, lon): + self.id = id + self.lat = lat + self.lon = lon + + def update_status(self, lat, lon): + self.lat = lat + self.lon = lon + + + def toJSON(self): + return json.dumps(self, default=lambda o: o.__dict__,sort_keys=True, indent=4) \ No newline at end of file diff --git a/gettingstarted/multiple_drones/multiple_drones1.py b/gettingstarted/multiple_drones/multiple_drones1.py index 2b7286a6..686fb368 100644 --- a/gettingstarted/multiple_drones/multiple_drones1.py +++ b/gettingstarted/multiple_drones/multiple_drones1.py @@ -12,6 +12,16 @@ from dronekit_sitl import SITL from dronekit import Vehicle, VehicleMode, connect, LocationGlobalRelative + +from websocket import create_connection +from drone_model import Drone_Model + +ws = create_connection("ws://localhost:8000") + +# drone_count = 1 + +# drone_models={} + def connect_virtual_vehicle(instance, home): sitl = SITL() sitl.download('copter', '3.3', verbose=True) @@ -27,9 +37,27 @@ def connect_virtual_vehicle(instance, home): vehicle = connect(conn_string) vehicle.wait_ready(timeout=120) print("Reached here") - return vehicle, sitl -def arm_and_takeoff(aTargetAltitude, vehicle): + data_model = Drone_Model(instance+1,home[0],home[1]) + # drone_model[drone_count] = drone_model + + + + return vehicle, sitl, data_model + + +def custom_sleep(vehicle,drone_model, sleep_time): + current_time = 0 + while(current_time= aTargetAltitude * 0.95: print("Reached target altitude") break - time.sleep(1) + # time.sleep(1) + custom_sleep(vehicle,data_model,1) -def fly_drone(vehicle): - arm_and_takeoff(10, vehicle) +def fly_drone(vehicle, data_model): + arm_and_takeoff(10, vehicle, data_model) print("Set default/target airspeed to 3") vehicle.airspeed = 3 @@ -73,14 +103,15 @@ def fly_drone(vehicle): vehicle.simple_goto(point1) # sleep so we can see the change in map - time.sleep(10) - + #time.sleep(10) + custom_sleep(vehicle,data_model,10) print("Going towards second point for 30 seconds (groundspeed set to 10 m/s) ...") point2 = LocationGlobalRelative(-35.363244, 149.168801, 20) vehicle.simple_goto(point2, groundspeed=10) # sleep so we can see the change in map - time.sleep(10) + #time.sleep(10) + custom_sleep(vehicle,data_model,10) print("Returning to Launch") vehicle.mode = VehicleMode("LAND") @@ -90,14 +121,16 @@ def fly_drone(vehicle): print("Close vehicle object") vehicle.close() -vehicle, sitl = connect_virtual_vehicle(0,([41.715446209367,-86.242847096132,0])) -vehicle2, sitl2 = connect_virtual_vehicle(1,([41.715469, -86.242543,0])) +vehicle, sitl, data_model_1 = connect_virtual_vehicle(0,([41.715446209367,-86.242847096132,0])) +vehicle2, sitl2, data_model_2 = connect_virtual_vehicle(1,([41.715469, -86.242543,0])) print("flying drone 1") -arm_and_takeoff(10, vehicle) +#arm_and_takeoff(10, vehicle, data_model_1) +fly_drone(vehicle, data_model_1) print("flying drone 2") -arm_and_takeoff(10, vehicle2) +#arm_and_takeoff(10, vehicle2, data_model_2) +fly_drone(vehicle2, data_model_2) # Shut down simulator if it was started. diff --git a/gettingstarted/multiple_drones/multiple_drones3.py b/gettingstarted/multiple_drones/multiple_drones3.py index fc16e33b..2c1264bb 100644 --- a/gettingstarted/multiple_drones/multiple_drones3.py +++ b/gettingstarted/multiple_drones/multiple_drones3.py @@ -12,8 +12,12 @@ from dronekit_sitl import SITL from dronekit import Vehicle, VehicleMode, connect, LocationGlobalRelative + + copters = [] sitls = [] +drone_models = [] + def connect_virtual_vehicle(instance, home): sitl = SITL() diff --git a/gettingstarted/webApp/map.js b/gettingstarted/webApp/map.js index 4dea67a0..df39e948 100644 --- a/gettingstarted/webApp/map.js +++ b/gettingstarted/webApp/map.js @@ -1,6 +1,6 @@ marker_count=0; markers = {} -number_drones = 1 +number_drones = 2 function initMap() { // map center on the screen - 50,50 @@ -17,7 +17,7 @@ function initMap() { - marker.setMap( map ); + //markers[1] = marker1; //marker2.setMap(map); @@ -32,6 +32,7 @@ function add_markers(position,map){ while(marker_count!=number_drones){ marker_count+=1; marker = new google.maps.Marker( {position: position, map: map} ); + marker.setMap( map ); markers[marker_count] = marker; } From a1c0ea6cfdb4717e2e4630d0c4fc3bdaecc21b3b Mon Sep 17 00:00:00 2001 From: Jane Cleland Huang Date: Wed, 23 Sep 2020 07:39:04 -0400 Subject: [PATCH 13/16] NED code --- gettingstarted/NED/__init__.py | 0 gettingstarted/NED/flight_plotter.py | 64 ++++++ gettingstarted/NED/fly_a_circle.py | 239 +++++++++++++++++++++++ gettingstarted/NED/ned_utilities.py | 140 +++++++++++++ gettingstarted/NED/simple_ned_example.py | 152 ++++++++++++++ 5 files changed, 595 insertions(+) create mode 100644 gettingstarted/NED/__init__.py create mode 100644 gettingstarted/NED/flight_plotter.py create mode 100644 gettingstarted/NED/fly_a_circle.py create mode 100644 gettingstarted/NED/ned_utilities.py create mode 100644 gettingstarted/NED/simple_ned_example.py diff --git a/gettingstarted/NED/__init__.py b/gettingstarted/NED/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/gettingstarted/NED/flight_plotter.py b/gettingstarted/NED/flight_plotter.py new file mode 100644 index 00000000..620a9f90 --- /dev/null +++ b/gettingstarted/NED/flight_plotter.py @@ -0,0 +1,64 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + +""" +Graph Utilities for plotting longitude and latitude of flight results +""" +from pymavlink import mavutil +import math +import re +import numpy as np +import matplotlib.pyplot as plt + + +class Location: + def __init__(self,lat=0.0,lon=0.0): + """ Create a new point at the origin """ + self.lat = lat + self.lon = lon + +class CoordinateLogger: + def __init__(self): + self.lat_array = [] + self.lon_array = [] + + def add_data(self,latitude,longitude): + """ + + :rtype: object + """ + self.lat_array.append([]) + self.lon_array.append([]) + self.lat_array[-1].append(latitude) + self.lon_array[-1].append(longitude) + + +############################################################################################## +# Provides Graph Plotting functionality +############################################################################################## +class GraphPlotter: + def __init__(self,lat1_array,lon1_array,lat2_array,lon2_array,xlabel="",ylabel="",title=""): + self.lat1_array=lat1_array + self.lon1_array=lon1_array + self.lat2_array=lat2_array + self.lon2_array=lon2_array + self.xlegend = xlabel + self.ylegend=ylabel + self.title=title + self.marker_lat = 0 + self.marker_lon = 0 + + def add_marker(self,markerlat,markerlon): + self.marker_lat = markerlat + self.marker_lon = markerlon + + def scatter_plot(self): + plt.plot(self.lat1_array,self.lon1_array,linewidth=10,color='gray') + plt.plot(self.lat2_array,self.lon2_array,linewidth=4,color='blue') + plt.xlabel(self.xlegend) + plt.ylabel(self.ylegend) + plt.title(self.title) + if self.marker_lat != 0: + plt.plot([self.marker_lat], [self.marker_lon], "ro", markersize=22) + plt.show() + diff --git a/gettingstarted/NED/fly_a_circle.py b/gettingstarted/NED/fly_a_circle.py new file mode 100644 index 00000000..c6516230 --- /dev/null +++ b/gettingstarted/NED/fly_a_circle.py @@ -0,0 +1,239 @@ +##!/ usr / bin / env python +# -*- coding: utf-8 -*- + +""" +Modified from 3DR simple_goto.py +Added code for flying using NED Velocity Vectors - Jane Cleland-Huang 1/15 +""" +import math +import os +import time +from math import sin, cos, atan2, radians, sqrt + +from dronekit import Vehicle, connect, VehicleMode, LocationGlobalRelative +from dronekit_sitl import SITL + +from flight_plotter import Location, CoordinateLogger, GraphPlotter +from ned_utilities import ned_controller + +def connect_virtual_vehicle(instance, home): + sitl = SITL() + sitl.download('copter', '3.3', verbose=True) + instance_arg = '-I%s' %(str(instance)) + home_arg = '--home=%s, %s,%s,180' % (str(home[0]), str(home[1]), str(home[2])) + speedup_arg = '--speedup=4' + sitl_args = [instance_arg, '--model', 'quad', home_arg, speedup_arg] + #sitl_args = [instance_arg, '--model', 'quad', home_arg] + sitl.launch(sitl_args, await_ready=True) + tcp, ip, port = sitl.connection_string().split(':') + port = str(int(port) + instance * 10) + conn_string = ':'.join([tcp, ip, port]) + print('Connecting to vehicle on: %s' % conn_string) + + vehicle = connect(conn_string) + vehicle.wait_ready(timeout=120) + print("Reached here") + return vehicle, sitl + + +################################################################################################ +# ARM and TAKEOFF +################################################################################################ + +# function: arm and takeoff +# parameters: target altitude (e.g., 10, 20) +# returns: n/a + +def arm_and_takeoff(aTargetAltitude): + """ + Arms vehicle and fly to aTargetAltitude. + """ + + print("Basic pre-arm checks") + # Don't try to arm until autopilot is ready + while not vehicle.is_armable: + print(" Waiting for vehicle to initialise...") + time.sleep(1) + + print("home: " + str(vehicle.location.global_relative_frame.lat)) + + print("Arming motors") + # Copter should arm in GUIDED mode + vehicle.mode = VehicleMode("GUIDED") + vehicle.armed = True + + # Confirm vehicle armed before attempting to take off + while not vehicle.armed: + print(" Waiting for arming...") + time.sleep(1) + + print("Taking off!") + vehicle.simple_takeoff(aTargetAltitude) # Take off to target altitude + + while True: + # Break and return from function just below target altitude. + if vehicle.location.global_relative_frame.alt >= aTargetAltitude * .95: + print("Reached target altitude") + break + time.sleep(1) + + +################################################################################################ +# function: Get distance in meters +# parameters: Two global relative locations +# returns: Distance in meters +################################################################################################ +def get_distance_meters(locationA, locationB): + # approximate radius of earth in km + R = 6373.0 + + lat1 = radians(locationA.lat) + lon1 = radians(locationA.lon) + lat2 = radians(locationB.lat) + lon2 = radians(locationB.lon) + + dlon = lon2 - lon1 + dlat = lat2 - lat1 + + a = sin(dlat / 2) ** 2 + cos(lat1) * cos(lat2) * sin(dlon / 2) ** 2 + c = 2 * atan2(sqrt(a), sqrt(1 - a)) + + distance = (R * c) * 1000 + + # print("Distance (meters):", distance) + return distance + + +################################################################################################ +# Fly to a target location using a waypoint +################################################################################################ +def fly_to(vehicle, targetLocation, groundspeed): + print("Flying from: " + str(vehicle.location.global_frame.lat) + "," + str( + vehicle.location.global_frame.lon) + " to " + str(targetLocation.lat) + "," + str(targetLocation.lon)) + vehicle.groundspeed = groundspeed + currentTargetLocation = targetLocation + vehicle.simple_goto(currentTargetLocation) + remainingDistance = get_distance_meters(currentTargetLocation, vehicle.location.global_frame) + + while vehicle.mode.name == "GUIDED": + remainingDistance = get_distance_meters(currentTargetLocation, vehicle.location.global_frame) + print(remainingDistance) + if remainingDistance < 1: + print("Reached target") + break + time.sleep(1) + +################################################################################################ +# Given the GPS coordinates of the center of a circle, and a degree (0-360) and radius +# return a point on the circumference. +################################################################################################ +def point_on_circle(radius, angle_indegrees, latitude, longitude): + # Convert from degrees to radians + lon = (radius * math.cos(angle_indegrees * math.pi / 180)) + longitude + lat = (radius * math.sin(angle_indegrees * math.pi / 180)) + latitude + return Location(lat, lon) + + +############################################################################################ +# Main functionality +############################################################################################ + +vehicle, sitl = connect_virtual_vehicle(1,([41.7144367,-86.2417136,0])) #"41.7144367,-86.2417136,221,0" +start = time.time() + +arm_and_takeoff(10) + +# Get current location of vehicle and establish a conceptual circle around it for flying +center = Location(vehicle.location.global_relative_frame.lat, + vehicle.location.global_relative_frame.lon) +radius = .0001 +angle = 0 #Starting angle + +# Fly to starting position using waypoint +currentLocation = center +startingPos = point_on_circle(radius*.95, angle+1, center.lat, center.lon) # Close to first position on circle perimeter +firstTargetPosition = LocationGlobalRelative(startingPos.lat, startingPos.lon, 10) +fly_to(vehicle, firstTargetPosition, 10) + +# Establish a starting angle to compute next position on circle +angle = 0 + +# Establish an instance of CoordinateLogger +log1 = CoordinateLogger() + +# Create a NedController +nedcontroller = ned_controller() +waypoint_goto = False + +# Fly from one point on the circumference to the next +while angle <= 360: + + # For NED flying compute the NED Velocity vector + print("\nNew target " + str(angle)) + nextTarget = (point_on_circle(radius, angle, center.lat, center.lon)) + currentLocation = Location(vehicle.location.global_relative_frame.lat, vehicle.location.global_relative_frame.lon) + distance_to_target = get_distance_meters(currentLocation, nextTarget) + closestDistance=distance_to_target + ned = nedcontroller.setNed(currentLocation, nextTarget) + + # Keep going until target is reached + while distance_to_target > 1: + currentLocation = Location(vehicle.location.global_relative_frame.lat, + vehicle.location.global_relative_frame.lon) + distance_to_target = get_distance_meters(currentLocation, nextTarget) + print ('Current Pos: (' + str(currentLocation.lat) + "," + str(currentLocation.lon) + + ') Target Pos: ' + str(nextTarget.lat) + ' Target lon: ' + str(nextTarget.lon) + ' Distance: ' + str( + distance_to_target) + " NED: " + str(ned.north) + " " + str(ned.east)) + + if distance_to_target > closestDistance: #Prevent unwanted fly-by + break + else: + closestDistance = distance_to_target + + # Needed if the ned vectors don't take you to the final target i.e., + # You'll need to create another NED. You could reuse the previous one + # but recomputing it can mitigate small errors which otherwise build up. + currentLocation = Location(vehicle.location.global_relative_frame.lat, + vehicle.location.global_relative_frame.lon) + ned = nedcontroller.setNed(currentLocation, nextTarget) + + # Either fly using waypoints or NEDs + if waypoint_goto == True: + firstTargetPosition = LocationGlobalRelative(nextTarget.lat, nextTarget.lon, 10) + fly_to(vehicle, firstTargetPosition, 10) + else: + nedcontroller.send_ned_velocity(ned.north, ned.east, ned.down, 1, vehicle) + + #Log data + log1.add_data(currentLocation.lat,currentLocation.lon) + + angle = angle + 2 + +print("Returning to Launch") +vehicle.mode = VehicleMode("RTL") +# Close vehicle object before exiting script +print("Close vehicle object") +vehicle.close() + +# Shut down simulator if it was started. +if sitl is not None: + sitl.stop() + +end = time.time() +print(end - start) + +########################################################################### +# Create the target coordinates +########################################################################### +log2 = CoordinateLogger() +angle = 0 +while angle <= 360: + point = (point_on_circle(radius, angle, center.lat, center.lon)) + log2.add_data(point.lat,point.lon) + angle = angle + 1 + +########################################################################### +# Plot graph +########################################################################### +plotter = GraphPlotter(log1.lat_array,log1.lon_array,log2.lat_array,log2.lon_array,"Longitude","Latitude","NED vs. target Coordinates") +plotter.scatter_plot() diff --git a/gettingstarted/NED/ned_utilities.py b/gettingstarted/NED/ned_utilities.py new file mode 100644 index 00000000..bfd9ad0e --- /dev/null +++ b/gettingstarted/NED/ned_utilities.py @@ -0,0 +1,140 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + +""" +NED Utilities +""" +from pymavlink import mavutil +import time +import numpy as np +import nvector as nv +from nvector import rad, deg +from math import sin, cos, atan2, radians, sqrt +from flight_plotter import Location + +wgs84 = nv.FrameE(name='WGS84') + + +class Nedvalues: + def __init__(self, north=0.0, east=0.0, down=0.0): + """ Create a new point at the origin """ + self.north = north + self.east = east + self.down = down + + +class ned_controller: + + ################################################################################################ + # function: Get distance in meters + # parameters: Two global relative locations + # returns: Distance in meters + ################################################################################################ + def get_distance_meters(self, locationA, locationB): + # approximate radius of earth in km + R = 6373.0 + + lat1 = radians(locationA.lat) + lon1 = radians(locationA.lon) + lat2 = radians(locationB.lat) + lon2 = radians(locationB.lon) + + dlon = lon2 - lon1 + dlat = lat2 - lat1 + + a = sin(dlat / 2) ** 2 + cos(lat1) * cos(lat2) * sin(dlon / 2) ** 2 + c = 2 * atan2(sqrt(a), sqrt(1 - a)) + + distance = (R * c) * 1000 + + # print("Distance (meters):", distance) + return distance + + # Sends velocity vector message to UAV vehicle + def send_ned_velocity(self, velocity_x, velocity_y, velocity_z, duration, vehicle): + """ + Move vehicle in a direction based on specified velocity vectors. + """ + msg = vehicle.message_factory.set_position_target_local_ned_encode( + 0, # time_boot_ms (not used) + 0, 0, # target system, target component + mavutil.mavlink.MAV_FRAME_LOCAL_NED, # frame + 0b0000111111000111, # type_mask (only speeds enabled) + 0, 0, 0, # x, y, z positions (not used) + velocity_x, velocity_y, velocity_z, # x, y, z velocity in m/s + 0, 0, 0, # x, y, z acceleration (not supported yet, ignored in GCS_Mavlink) + 0, 0) # yaw, yaw_rate (not supported yet, ignored in GCS_Mavlink) + + #for x in range(0, duration): + vehicle.send_mavlink(msg) + + time.sleep(0.1) + + # Sends velocity vector message to UAV vehicle + + def send_ned_stop(self, vehicle): + count = 1 + outerloopcounter = 1 + + currentVelocity = vehicle.velocity + north = currentVelocity[0] + south = currentVelocity[1] + msg = vehicle.message_factory.set_position_target_local_ned_encode( + 0, # time_boot_ms (not used) + 0, 0, # target system, target component + mavutil.mavlink.MAV_FRAME_LOCAL_NED, # frame + 0b0000111111000111, # type_mask (only speeds enabled) + 0, 0, 0, # x, y, z positions (not used) + -north*100,-south*100,0, # x, y, z velocity in m/s + 0, 0, 0, # x, y, z acceleration (not supported yet, ignored in GCS_Mavlink) + 0, 0) # yaw, yaw_rate (not supported yet, ignored in GCS_Mavlink) + + while True: + if vehicle.groundspeed > 1 and outerloopcounter <= 100: + if count == 100: + count = 1 + print(vehicle.groundspeed) + outerloopcounter = outerloopcounter + 1 + else: + count = count + 1 + + vehicle.send_mavlink(msg) + + if outerloopcounter == 100: + break + + msg = vehicle.message_factory.set_position_target_local_ned_encode( + 0, # time_boot_ms (not used) + 0, 0, # target system, target component + mavutil.mavlink.MAV_FRAME_LOCAL_NED, # frame + 0b0000111111000111, # type_mask (only speeds enabled) + 0, 0, 0, # x, y, z positions (not used) + 0,0, 0, # x, y, z velocity in m/s + 0, 0, 0, # x, y, z acceleration (not supported yet, ignored in GCS_Mavlink) + 0, 0) # yaw, yaw_rate (not supported yet, ignored in GCS_Mavlink) + + print ("MESSAGE HERE: ") + print (msg) + print ("++++++++++++++++++++++++++++++++++++++") + + time.sleep(0.1) + + # Sets NED given a current and target location + def setNed(self, current, target): + lat_C, lon_C = rad(current.lat), rad(current.lon) + lat_T, lon_T = rad(target.lat), rad(target.lon) + # create an n-vector for current and target + nvecC = nv.lat_lon2n_E(lat_C, lon_C) + nvecT = nv.lat_lon2n_E(lat_T, lon_T) + # create a p-vec from C to T in the Earth's frame + # the zeros are for the depth (depth = -1 * altitude) + p_CT_E = nv.n_EA_E_and_n_EB_E2p_AB_E(nvecC, nvecT, 0, 0) + # create a rotation matrix + # this rotates points from the NED frame to the Earth's frame + R_EN = nv.n_E2R_EN(nvecC) + # rotate p_CT_E so it lines up with current's NED frame + # we use the transpose so we can go from the Earth's frame to the NED frame + n, e, d = np.dot(R_EN.T, p_CT_E).ravel() + + return Nedvalues(n,e,d) + diff --git a/gettingstarted/NED/simple_ned_example.py b/gettingstarted/NED/simple_ned_example.py new file mode 100644 index 00000000..c9516905 --- /dev/null +++ b/gettingstarted/NED/simple_ned_example.py @@ -0,0 +1,152 @@ +#!/ usr / bin / env python +# -*- coding: utf-8 -*- + +""" +Modified from 3DR simple_goto.py +Added code for flying using NED Velocity Vectors - Jane Cleland-Huang 1/15/18 +""" +import math +import os +import time +from math import sin, cos, atan2, radians, sqrt + +from dronekit import Vehicle, connect, VehicleMode, LocationGlobalRelative +from dronekit_sitl import SITL + +from flight_plotter import Location, CoordinateLogger, GraphPlotter +from ned_utilities import ned_controller + +################################################################################################ +# Standard Connect +################################################################################################ +def connect_virtual_vehicle(instance, home): + sitl = SITL() + sitl.download('copter', '3.3', verbose=True) + instance_arg = '-I%s' %(str(instance)) + home_arg = '--home=%s, %s,%s,180' % (str(home[0]), str(home[1]), str(home[2])) + sitl_args = [instance_arg, '--model', 'quad', home_arg] + sitl.launch(sitl_args, await_ready=True) + tcp, ip, port = sitl.connection_string().split(':') + port = str(int(port) + instance * 10) + conn_string = ':'.join([tcp, ip, port]) + print('Connecting to vehicle on: %s' % conn_string) + + vehicle = connect(conn_string) + vehicle.wait_ready(timeout=120) + print("Reached here") + return vehicle, sitl + + +################################################################################################ +# ARM and TAKEOFF +################################################################################################ + +# function: arm and takeoff +# parameters: target altitude (e.g., 10, 20) +# returns: n/a + +def arm_and_takeoff(aTargetAltitude): + """ + Arms vehicle and fly to aTargetAltitude. + """ + + print("Basic pre-arm checks") + # Don't try to arm until autopilot is ready + while not vehicle.is_armable: + print(" Waiting for vehicle to initialise...") + time.sleep(1) + + print("home: " + str(vehicle.location.global_relative_frame.lat)) + + print("Arming motors") + # Copter should arm in GUIDED mode + vehicle.mode = VehicleMode("GUIDED") + vehicle.armed = True + + # Confirm vehicle armed before attempting to take off + while not vehicle.armed: + print(" Waiting for arming...") + time.sleep(1) + + print("Taking off!") + vehicle.simple_takeoff(aTargetAltitude) # Take off to target altitude + + while True: + # Break and return from function just below target altitude. + if vehicle.location.global_relative_frame.alt >= aTargetAltitude * .95: + print("Reached target altitude") + break + time.sleep(1) + + +################################################################################################ +# function: Get distance in meters +# parameters: Two global relative locations +# returns: Distance in meters +################################################################################################ +def get_distance_meters(locationA, locationB): + # approximate radius of earth in km + R = 6373.0 + + lat1 = radians(locationA.lat) + lon1 = radians(locationA.lon) + lat2 = radians(locationB.lat) + lon2 = radians(locationB.lon) + + dlon = lon2 - lon1 + dlat = lat2 - lat1 + + a = sin(dlat / 2) ** 2 + cos(lat1) * cos(lat2) * sin(dlon / 2) ** 2 + c = 2 * atan2(sqrt(a), sqrt(1 - a)) + + distance = (R * c) * 1000 + + # print("Distance (meters):", distance) + return distance + + +############################################################################################ +# Main functionality: Example of one NED command +############################################################################################ + +vehicle, sitl = connect_virtual_vehicle(1,([41.714436,-86.241713,0])) +arm_and_takeoff(10) +startingLocation = Location(vehicle.location.global_relative_frame.lat, vehicle.location.global_relative_frame.lon) +targetLocation = LocationGlobalRelative(41.714500,-86.241600,0) +totalDistance = get_distance_meters(startingLocation,targetLocation) +print("Distance to travel " + str(totalDistance)) + +# Establish an instance of ned_controller +nedcontroller = ned_controller() + +# Get the NED velocity vector mavlink message +ned = nedcontroller.setNed(startingLocation, targetLocation) +nedcontroller.send_ned_velocity(ned.north, ned.east, ned.down, 1, vehicle) + +# Just wait for it to get there! +print("ARE WE THERE YET?") +currentLocation = Location(vehicle.location.global_relative_frame.lat, vehicle.location.global_relative_frame.lon) +while get_distance_meters(currentLocation,targetLocation) > .05: + currentLocation = Location(vehicle.location.global_relative_frame.lat, vehicle.location.global_relative_frame.lon) + distance_to_target = get_distance_meters(currentLocation, targetLocation) + print ('Distance: {0} Ground speed: {1} '.format(distance_to_target,vehicle.groundspeed)) + #print (distance_to_target) + +# How close did it get? +endLocation = Location(vehicle.location.global_relative_frame.lat, vehicle.location.global_relative_frame.lon) +print("Starting position: " + str(startingLocation.lat) + ", " + str(startingLocation.lon)) +print("Target position: " + str(targetLocation.lat) + ", " + str(targetLocation.lon)) +print("End position: " + str(endLocation.lat) + ", " + str(endLocation.lon)) + +print("Returning to Launch") +vehicle.mode = VehicleMode("RTL") + +# Close vehicle object before exiting script +print("Close vehicle object") +vehicle.close() + +# Shut down simulator if it was started. +if sitl is not None: + sitl.stop() + + From 5fa9c6e37485f33439306d20228627db1567cd9c Mon Sep 17 00:00:00 2001 From: Michael Murphy Date: Fri, 25 Sep 2020 11:59:50 -0700 Subject: [PATCH 14/16] Add pip dependencies Add geographiclib for ned_utilities.py Add numpy 1.16.6 (the latest version for python 2.7) --- gettingstarted/setup_dronekit.sh | 2 ++ 1 file changed, 2 insertions(+) diff --git a/gettingstarted/setup_dronekit.sh b/gettingstarted/setup_dronekit.sh index 5ce51447..3c8abd66 100644 --- a/gettingstarted/setup_dronekit.sh +++ b/gettingstarted/setup_dronekit.sh @@ -46,6 +46,8 @@ sudo pip install future pymavlink MAVProxy sudo pip install pyudev sudo pip install PyYAML sudo pip install nvector +sudo pip install geographiclib +sudo pip install numpy==1.16.6 sudo pip install boltons sudo pip install dronekit==2.9.1 sudo pip install dronekit-sitl==3.2.0 From facf570dcad068fb9c32e12e644674bef48f8dda Mon Sep 17 00:00:00 2001 From: Michael Murphy Date: Sat, 10 Oct 2020 11:12:27 -0700 Subject: [PATCH 15/16] Add rotate_velocity, and get_velocity rotate_velocity takes a velocity and an angle. It rotates the velocity clockwise. get_velocity takes a vehicle and returns its current velocity. --- gettingstarted/NED/ned_utilities.py | 71 +++++++++++++++++++++++++++++ 1 file changed, 71 insertions(+) diff --git a/gettingstarted/NED/ned_utilities.py b/gettingstarted/NED/ned_utilities.py index bfd9ad0e..7c5d40e8 100644 --- a/gettingstarted/NED/ned_utilities.py +++ b/gettingstarted/NED/ned_utilities.py @@ -4,6 +4,7 @@ """ NED Utilities """ +import math from pymavlink import mavutil import time import numpy as np @@ -138,3 +139,73 @@ def setNed(self, current, target): return Nedvalues(n,e,d) +def get_velocity(vehicle): + v = vehicle.velocity + return Nedvalues(v[0], v[1], v[2]) + +def rotate_velocity(ned_velocity, degrees_clockwise, preserve_down_component=True): + """ + Returns a Nedvalues object holding the velocity vector that is rotated + clockwise as seen from above. + + Parameters: + ned_velocity: a Nedvalues object representing a velocity. Must not be + Nedvalues(0.0, 0.0, 0.0), pure up or pure down. + degrees_clockwise: a float + preserve_down_component: a boolean + + When preserve_down_component is true, the down component of the velocity is + not changed. When it is false, the down component is changed, as the new velocity + is found by rotating around an axis that is orthogonal to ned_velocity but + as close to the down axis as possible. + + Here is the use case for this function: + A drone is traveling in any direction and at any speed. We want it to turn + at an X degree angle and send it a new NED. + + Example 0: + Our vehicle is flying and we want to turn 42 degrees clockwise (as seen from above). + + rotate_velocity(get_velocity(vehicle), 42.0) + + This would return a Nedvalues object. Its value would depend on the vehicle's + current velocity + + Example 1: + We have a velocity of north and 45 degrees up. We want to rotate this velocity + 180 degrees while not changing the down component. We can do so by calling: + + rotate_velocity(Nedvalues(3.0, 0.0, -3.0), 180.0) + + This would return Nedvalues(-3.0, 0.0, -3.0) + + Example 2: + We are flying north and up at 45 degrees. We want to rotate the velocity 180 + degrees while changing down component of velocity: + + rotate_velocity(Nedvalues(3.0, 0.0, -3.0), 180.0, False) + Would return: Nedvalues(-3.0, 0.0, 3.0) + + Example 3: + We are flying north west at 1.4 meters per second. We want to rotate 90 degrees + counter-clockwise: + + rotate_velocity(Nedvalues(1.0, -1.0, 0.0), -90.0) + would return Nedvalues(-1.0, -1.0, 0.0) + """ + # This uses Rodrigues' rotation formula. + # https://en.wikipedia.org/wiki/Rodrigues%27_rotation_formula + v = np.array([ned_velocity.north, ned_velocity.east, ned_velocity.down]) + theta = math.radians(degrees_clockwise) + down_axis = np.array([0.0, 0.0, 1.0]) + if preserve_down_component: + k = down_axis + else: + left = np.cross(v, down_axis) + k = np.cross(left, v) + magnitude = math.sqrt(np.dot(k, k)) + k = k * (1.0 / magnitude) + + # https://en.wikipedia.org/wiki/Rodrigues%27_rotation_formula#Statement + v_rot = v * math.cos(theta) + np.cross(k, v) * math.sin(theta) + k * (np.dot(k, v)) * (1.0 - math.cos(theta)) + return Nedvalues(v_rot[0], v_rot[1], v_rot[2]) From aefe64cbdaeb342bb9490e1df09251abb3eefe0f Mon Sep 17 00:00:00 2001 From: Michael Murphy Date: Sat, 10 Oct 2020 13:11:09 -0700 Subject: [PATCH 16/16] Add __repr__ method to Nedvalues This makes working with Nedvalues a little easier from the REPL. --- gettingstarted/NED/ned_utilities.py | 3 +++ 1 file changed, 3 insertions(+) diff --git a/gettingstarted/NED/ned_utilities.py b/gettingstarted/NED/ned_utilities.py index 7c5d40e8..cd0a53d9 100644 --- a/gettingstarted/NED/ned_utilities.py +++ b/gettingstarted/NED/ned_utilities.py @@ -22,6 +22,9 @@ def __init__(self, north=0.0, east=0.0, down=0.0): self.north = north self.east = east self.down = down + + def __repr__(self): + return 'Nedvalues({self.north}, {self.east}, {self.down})'.format(self=self) class ned_controller: