def test_sitl():
sitl = SITL()
sitl.download('copter', '3.3')
sitl.launch(copter_args)
sitl.block_until_ready()
assert_equals(sitl.poll(), None, 'SITL should still be running.')
assert_equals(sitl.using_sim, False, 'SITL for copter-3.3 should not be using pysim')
sitl.stop()
assert sitl.poll() != None, 'SITL should stop running after kill.'
# Test "relaunch"
sitl.launch(copter_args)
try:
sitl.launch(copter_args)
assert False, 'SITL should fail to launch() again when running'
except:
pass
try:
sitl.launch(copter_args, restart=True)
except:
assert False, 'SITL should succeed in launch() when restart=True'
sitl.stop()
def connection(self, args):
args = str(args)
if args is 'sitl' or args is None:
self._log("Starting copter simulator (SITL)")
from dronekit_sitl import SITL
sitl = SITL()
self.sitl = sitl
sitl.download('copter', '3.3', verbose=True)
sitl_args = [
'-I0', '--model', 'quad', '--home=-35.363261,149.165230,584,0'
]
sitl.launch(sitl_args, await_ready=True, restart=True)
connection_string = 'tcp:127.0.0.1:5760'
elif args == '0':
connection_string = '/dev/ttyACM0'
elif args == '1':
connection_string = '/dev/ttyACM1'
else:
connection_string = str(args)
self._log('Connecting to vehicle on: %s' % connection_string)
vehicle = connect(connection_string, wait_ready=True)
# Register observers
vehicle.add_attribute_listener('location', self.location_callback)
#vehicle.add_attribute_listener('battery',self.battery_callback)
#vehicle.add_attribute_listener('heading',self.heading_callback)
return vehicle
def connectMyCopter():
parser = argparse.ArgumentParser(description='commands')
parser.add_argument('--connect')
args = parser.parse_args()
connection_string = args.connect
'''
if not connection_string:
import dronekit_sitl
sitl = dronekit_sitl.start_default()
connection_string = sitl.connection_string()
vehicle = connect(connection_string,wait_ready=True, baud=57600)
'''
if not connection_string:
import dronekit_sitl
from dronekit_sitl import SITL
sitl = SITL()
sitl.download('copter', '3.3', verbose=True)
sitl_args = [
'-I0', '--model', 'quad',
'--home=28.468772344967554,77.53801532669809,200,353'
]
sitl.launch(sitl_args, verbose=True, await_ready=False, restart=True)
#vehicle = connect(connection_string,wait_ready=True, baud=57600)
vehicle = connect('tcp:127.0.0.1:5760', wait_ready=True)
return vehicle
def copter_sitl_guided_to():
"""
Returns a copter SITL instance for use in test cases
Status: hovering at 10m AGL
Mode: Guided
"""
# Launch a SITL using local copy of Copter 4
sitl = SITL(sitl_filename, instance=0)
sitl.launch(['--home=51.454531,-2.629158,589,353'])
# Wait for sitl to be ready before passing to test cases
sitl.block_until_ready()
# Connect to UAV to setup base parameters
veh = connect(sitl.connection_string(), vehicle_class=DroneTreeVehicle)
veh.parameters['SIM_SONAR_SCALE'] = 0.00001
veh.parameters['RNGFND2_ORIENT'] = 0
veh.parameters['RNGFND2_SCALING'] = 10
veh.parameters['RNGFND2_PIN'] = 0
veh.parameters['RNGFND2_TYPE'] = 1
veh.parameters['RNGFND2_MAX_CM'] = 5000
veh.parameters['RNGFND2_MIN_CM'] = 5000
veh.parameters['SIM_BATT_VOLTAGE'] = 12.59
veh.parameters['BATT_MONITOR'] = 4
veh.parameters['TERRAIN_ENABLE'] = 0
veh.parameters['ARMING_CHECK'] = 16384 # mission only
veh.parameters['SIM_SPEEDUP'] = 10 # speed up SITL for rapid startup
veh.parameters['FRAME_CLASS'] = 2 # I'm a hex
veh.close() # close vehicle instance after setting base parameters
# Stop and relaunch SITL to enable distance sensor and battery monitor
sitl.stop()
sitl.launch(['--home=51.454531,-2.629158,589,353'],
use_saved_data=True,
verbose=True)
# Wait for sitl to be ready before passing to test cases
sitl.block_until_ready()
# Connect again
veh = connect(sitl.connection_string(), vehicle_class=DroneTreeVehicle)
# wait until armable
while not veh.is_armable:
sleep(0.5)
veh.parameters['SIM_SPEEDUP'] = 5 # set sim speed for rapid take-off
veh.arm() # arm veh
veh.mode = VehicleMode('GUIDED') # set veh mode to GUIDED
veh.wait_simple_takeoff(10) # take-off to 10m AGL
veh.close() # close vehicle after guided takeoff
yield sitl
# Shut down simulator if it was started.
if sitl is not None:
sitl.stop()
def __init__(self, config):
self.default_webserver_ip = config.ip
self.default_port = config.port
self.default_webserver_port = config.webport
self.id = config.id
self.ip = config.ip
self.home_location = LocationGlobalRelative(config.lat, config.lon,
config.alt)
self.sitl = config.sitl
# Follow instructions @ https://github.com/abearman/sparrow-dev/wiki/How-to-use-the-DroneKit-SITL-simulator
self.instance = self.id - 1
self.sitlInstance = SITL(instance=self.instance)
self.sitlInstance.download('copter', '3.3', verbose=True)
# system (e.g. "copter", "solo"), version (e.g. "3.3"), verbose
if self.sitl:
sitl_args = [
'--simin=127.0.0.1:4000', '--simout=127.0.0.1:4001', '--model',
'quad', '--home=' + str(self.home_location.lat) + "," +
str(self.home_location.lon) + "," +
str(self.home_location.alt) + ",360"
]
print(str(sitl_args))
self.sitlInstance.launch(sitl_args,
verbose=False,
await_ready=False,
restart=True)
self.connection_string = self.sitlInstance.connection_string()
else:
self.connection_string = '/dev/tty/ACM0'
self.formation = None
self.webserver = str(self.ip) + ":" + str(self.default_webserver_port)
print('\nConnecting to vehicle on: %s\n' % self.connection_string)
#print(site.USER_SITE)
self.vehicle = connect(self.connection_string)
print("Drone: " + str(self.id) + " connected!")
self.drone_data = self.get_drone_data()
# print(self.drone_data.__dict__.get(str(self.id)))
# self.print_drone_data()
self.mission = Mission(self.vehicle)
# logger config
self.logger = logging.getLogger("/logs/drone" + str(self.id) + "_log")
self.logger.setLevel(logging.DEBUG)
fh = logging.FileHandler("drone" + str(self.id) + "_log")
fh.setLevel(logging.DEBUG)
ch = logging.StreamHandler()
ch.setLevel(logging.DEBUG)
formatter = logging.Formatter(
'%(asctime)s - %(name)s - %(levelname)s - %(message)s')
fh.setFormatter(formatter)
self.logger.addHandler(fh)
self.logger.addHandler(ch)
def __init__(self,
iNum,
extraArgs,
rate,
speedup,
vType="copter",
version="3.3"):
# type: (int, list[str], int, int, str, str) -> None
"""
:param iNum: instance number, affect SITL system ID
:param home: (lat,lng,alt,yaw)
"""
self.iNum = iNum
self.rate = rate
self.args = extraArgs + ['-I' + str(iNum)] + [
'--speedup', str(speedup)
] + ['-r', str(rate)]
self.vType = vType
self.version = version
sitl = SITL()
self.sitl = sitl
@retry(5)
def download():
sitl.download(vType, version)
download()
@retry(2)
def launch():
try:
sitl.launch(self.args, await_ready=True, restart=True)
print(
"launching APM SITL:",
*(self.args + ["URI=" + self.connStr] +
["PID=" + str(sitl.p.pid)]))
self.setParamAndRelaunch('SYSID_THISMAV', self.iNum + 1)
@self.withVehicle()
def set(vehicle):
vehicle.parameters['FS_GCS_ENABLE'] = 0
vehicle.parameters['FS_EKF_THRESH'] = 100
set()
except:
self.close()
raise
launch()
print("APM SITL on", self.connStr, "is up and running")
def __init__(self, index):
self.iNum = index
self.args = sitl_args + ['-I' + str(index)]
self.sitl = SITL()
self.sitl.download('copter', '3.3')
self.sitl.launch(self.args, await_ready=True, restart=True)
self.connStr = tcp_master(index)
self.setParamAndRelaunch('SYSID_THISMAV', index + 1)
APMSim.existing.append(self)
def start_sitl_rover():
'''start a SITL session using sensible defaults.
This should be the simplest way to start a sitl session'''
print("Starting copter simulator (SITL)")
sitl = SITL()
sitl.download('rover', '2.50', verbose=True)
sitl_args = [
'-I0', '--model', 'rover', '--home=42.2278287,-8.72184010,584,353'
]
sitl.launch(sitl_args, await_ready=False, restart=True, verbose=True)
sitl.block_until_ready(verbose=True)
return sitl
def __init__(self, sample_id, core_id, initial_profile, config):
ardupilot_dir = config['root_dir'] + 'experiment/elf/'
out_dir = config['root_dir'] + 'experiment/output/'
self.elf_dir = "%s%d/" % (ardupilot_dir, 0)
self.exp_out_dir = "%s%s/%d/" % (out_dir, 'PA', 0)
self.ready = True
self.states = []
self.profiles = []
self.sim_id = "%d_%d" % (sample_id, core_id)
copter_file = self.elf_dir + "ArduCopter.elf"
self.delta = 0.1
self.sitl = SITL(path=copter_file)
home_str = "%.6f,%.6f,%.2f,%d" % (
initial_profile.lat, initial_profile.lon, initial_profile.alt,
initial_profile.yaw)
sitl_args = [
'-S',
'-I%d' % bug_id, '--home=' + home_str, '--model', '+',
'--speedup=1', '--defaults=' + self.elf_dir + 'copter.parm'
]
self.sitl.launch(sitl_args,
await_ready=True,
restart=True,
wd=self.elf_dir)
port_number = 5760 + bug_id * 10
self.missions = [
Mission1(),
Mission2(),
Mission3(),
Mission4(),
Mission5()
]
try:
self.vehicle = connect('tcp:127.0.0.1:%d' % port_number,
wait_ready=True,
rate=10)
except APIException:
print("Cannot connect")
self.sitl.stop()
self.sitl.p.stdout.close()
self.sitl.p.stderr.close()
self.ready = False
return
for k, v in initial_profile.params.items():
self.vehicle.parameters[k] = v
while not self.vehicle.is_armable:
print('initializing...')
time.sleep(1)
self.arm_vehicle()
time.sleep(2)
print(self.vehicle.version)
def sitlUp(instance=0):
global sitls
args = sitl_args + ['-I' + str(instance)]
sitl = SITL()
sitl.download('copter', '3.3')
sitl.launch(args, await_ready=True, restart=True)
key = 'SYSID_THISMAV'
value = instance + 1
setParamAndRelaunch(sitl, args, tcp_master(instance), key, value)
sitls.append(sitl)
def copter_sitl_ground():
"""
Returns a copter SITL instance for use in test cases
Status: on ground
Mode: Stabilize
"""
# Launch a SITL using local copy of Copter 4
sitl = SITL(sitl_filename, instance=0)
sitl.launch(['--home=51.454531,-2.629158,589,353'])
# Wait for sitl to be ready before passing to test cases
sitl.block_until_ready()
print(sitl.connection_string())
# Connect to UAV to setup base parameters
veh = connect(sitl.connection_string(), vehicle_class=DroneTreeVehicle)
veh.parameters['SIM_SONAR_SCALE'] = 0.00001
veh.parameters['RNGFND2_ORIENT'] = 0
veh.parameters['RNGFND2_SCALING'] = 10
veh.parameters['RNGFND2_PIN'] = 0
veh.parameters['RNGFND2_TYPE'] = 1
veh.parameters['RNGFND2_MAX_CM'] = 5000
veh.parameters['RNGFND2_MIN_CM'] = 5000
veh.parameters['SIM_BATT_VOLTAGE'] = 12.59
veh.parameters['BATT_MONITOR'] = 4
veh.parameters['TERRAIN_ENABLE'] = 0
veh.parameters['ARMING_CHECK'] = 16384 # mission only
veh.parameters['SIM_SPEEDUP'] = 10 # speed up SITL for rapid startup
veh.parameters['FRAME_CLASS'] = 2 # I'm a hex
veh.close() # close veh instance after setting base parameters
# Stop and relaunch SITL to enable distance sensor and battery monitor
sitl.stop()
sitl.launch(['--home=51.454531,-2.629158,589,353'],
use_saved_data=True,
verbose=True)
# Wait for sitl to be ready before passing to test cases
sitl.block_until_ready()
yield sitl
# Shut down simulator if it was started.
if sitl is not None:
sitl.stop()
def _start_SITL(self):
from dronekit_sitl import SITL
sitl = SITL()
sitl.download(
system="copter", version="3.3", verbose=False
) # ...or download system (e.g. "copter") and version (e.g. "3.3")
sitl.launch(["--home=52.52.512593, 13.321893,0,90"],
verbose=False,
await_ready=False,
restart=False)
sitl.block_until_ready(
verbose=False) # explicitly wait until receiving commands
connection_string = sitl.connection_string()
return connection_string
def start_default_sitl(lat=None, lon=None):
print "Starting copter simulator (SITL)"
from dronekit_sitl import SITL
sitl = SITL()
sitl.download('copter', '3.3', verbose=True)
if ((lat is not None and lon is None) or
(lat is None and lon is not None)):
print("Supply both lat and lon, or neither")
exit(1)
sitl_args = ['-I0', '--model', 'quad', ]
if lat is not None:
sitl_args.append('--home=%f,%f,584,353' % (lat,lon,))
sitl.launch(sitl_args, await_ready=True, restart=True)
connection_string='tcp:127.0.0.1:5760'
return (sitl, connection_string)
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 from_config(cls, config_path: str, sitl_path: str):
"""
Create new Drone from a configuration file. Ignores connection_string to use SITL.
"""
# pylint: disable=arguments-differ
with open(config_path, "r") as file:
configuration = json.load(file)
sitl = SITL(path=sitl_path)
sitl.launch([])
vehicle_mode = configuration["vehicle_mode"]
broker_ip = configuration["broker"]["ip"]
broker_port = configuration["broker"]["port"]
node_descriptor = configuration["node"]
return cls(
broker_ip,
broker_port,
node_descriptor,
sitl,
vehicle_mode,
)
def __init__(self, config):
self.default_webserver_ip = config.ip
self.default_port = config.port
self.default_webserver_port = config.webport
self.id = config.id
self.ip = config.ip
self.sitl = config.sitl
self.sitlInstance = SITL('/home/dino/.dronekit/sitl/copter-3.3/apm')
if self.sitl:
sitl_args = [
'--instance ' + str(self.id - 1), '-I0', '--model', 'quad',
'--home=-35.363261,149.165230,584,353'
]
self.sitlInstance.launch(sitl_args,
verbose=False,
await_ready=False,
restart=True)
self.connection_string = self.sitlInstance.connection_string()
else:
self.connection_string = '/dev/tty/ACM0'
self.formation = None
self.webserver = str(self.ip) + ":" + str(self.default_webserver_port)
print('\nConnecting to vehicle on: %s\n' % self.connection_string)
self.vehicle = connect(self.connection_string)
print("Drone: " + str(self.id) + " connected!")
self.drone_data = self.get_drone_data()
self.print_drone_data()
self.mission = Mission(self.vehicle)
# logger config
self.logger = logging.getLogger("drone" + (str)(self.id) + "_log")
self.logger.setLevel(logging.DEBUG)
fh = logging.FileHandler("drone" + (str)(self.id) + "_log")
fh.setLevel(logging.DEBUG)
ch = logging.StreamHandler()
ch.setLevel(logging.DEBUG)
formatter = logging.Formatter(
'%(asctime)s - %(name)s - %(levelname)s - %(message)s')
fh.setFormatter(formatter)
self.logger.addHandler(fh)
self.logger.addHandler(ch)
def start_sitl():
"""Launch a SITL using local copy of Copter 4,
then set up the simulator for a rangefinder.
Only works for Windows or Linux. Including
binary in the project is ugly, but only used
for testing."""
sitl_path = sitl_file_path()
sitl = SITL(sitl_path)
sitl.launch(['--home=51.454531,-2.629158,589,353'])
veh = connect(sitl.connection_string(), vehicle_class=DroneTreeVehicle)
veh.parameters['SIM_SONAR_SCALE'] = 0.00001
veh.parameters['RNGFND2_ORIENT'] = 0
veh.parameters['RNGFND2_SCALING'] = 10
veh.parameters['RNGFND2_PIN'] = 0
veh.parameters['RNGFND2_TYPE'] = 1
veh.parameters['RNGFND2_MAX_CM'] = 5000
veh.parameters['RNGFND2_MIN_CM'] = 5000
veh.parameters['SIM_BATT_VOLTAGE'] = 12.59
veh.parameters['BATT_MONITOR'] = 4
veh.parameters['TERRAIN_ENABLE'] = 0
veh.parameters['ARMING_CHECK'] = 16384 # mission only
veh.parameters['FRAME_CLASS'] = 2 # I'm a hex
veh.close()
sitl.stop()
sitl.launch(['--home=51.454531,-2.629158,589,353'],
use_saved_data=True,
verbose=True)
sitl.block_until_ready()
return sitl
def test_preserve_eeprom():
# Start an SITL instance and change COMPASS_USE
sitl = SITL()
sitl.download('copter', '3.3')
sitl.launch(copter_args, verbose=True, await_ready=True, use_saved_data=False)
connection_string = sitl.connection_string()
vehicle = dronekit.connect(connection_string, wait_ready=True)
new_compass_use = 10
print("Changing COMPASS_USE to {0}".format(new_compass_use))
while vehicle.parameters["COMPASS_USE"] != new_compass_use:
vehicle.parameters["COMPASS_USE"] = new_compass_use
time.sleep(0.1)
print("Changed COMPASS_USE to {0}".format(new_compass_use))
time.sleep(5) # give parameters time to write
sitl.stop()
vehicle.close()
# Now see if it persisted
sitl.launch(copter_args, await_ready=True, use_saved_data=True)
vehicle = dronekit.connect(connection_string, wait_ready=True)
assert_equals(new_compass_use, vehicle.parameters["COMPASS_USE"])
vehicle.close()
sitl.stop()
import os
from nose.tools import assert_equals, with_setup
from dronekit_sitl import SITL
sitl = SITL('copter', '3.3-rc5')
sitl_args = ['-I0', '--model', 'quad', '--home=-35.363261,149.165230,584,353']
if 'SITL_SPEEDUP' in os.environ:
sitl_args += ['--speedup', str(os.environ['SITL_SPEEDUP'])]
if 'SITL_RATE' in os.environ:
sitl_args += ['-r', str(os.environ['SITL_RATE'])]
def setup_sitl():
sitl.launch(sitl_args, await_ready=True, restart=True)
def teardown_sitl():
sitl.stop()
def with_sitl(fn):
@with_setup(setup_sitl, teardown_sitl)
def test(*args, **kargs):
return fn('tcp:127.0.0.1:5760', *args, **kargs)
return test
import argparse
parser = argparse.ArgumentParser(description='Commands vehicle using vehicle.basic_takeoff.')
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
connection_string1 = None
connection_string2 = None
if not connection_string:
from dronekit_sitl import SITL
print(site.USER_SITE)
sitl1 = SITL()
sitl1.download('copter', '3.3', verbose=True)
sitl2 = SITL()
sitl2.download('copter', '3.3', verbose=True)
sitl_args1 = ['-I0', '--simin=127.0.0.1:4000', '--simout=127.0.0.1:4001', '--model',
'quad']
sitl_args2 = ['-I1', '--simin=127.0.0.1:4000', '--simout=127.0.0.1:4001', '--model',
'quad']
sitlInstance1 = sitl1.launch(sitl_args1, verbose=False, await_ready=False, restart=True)
sitlInstance2 = sitl2.launch(sitl_args2, verbose=False, await_ready=False, restart=True)
# sitl = dronekit_sitl.start_default()
connection_string1 = sitl1.connection_string()
connection_string2 = sitl2.connection_string()
print('Connecting to vehicle1 on: %s' % connection_string1)
vehicle1 = connect(connection_string1, wait_ready=True)
def execute_mission(mission, time_limit):
# TODO: allow 'binary' and 'speedup' to be passed as arguments
home = [40.071374969556928, -105.22978898137808, 1583.702759, 246]
speedup = 1
binary = '/experiment/source/build/sitl/bin/ardurover'
vehicle = sitl = None
try:
model_arg = '--model=rover'
home_arg = '--home={}'.format(','.join(map(str, home)))
defaults_arg = "--defaults=/experiment/source/Tools/autotest/default_params/rover.parm"
sitl = SITL(binary)
sitl.launch([home_arg, model_arg, defaults_arg],
verbose=True,
await_ready=True,
restart=False,
speedup=speedup)
# launch mavproxy -- ports aren't being forwarded!
# sim_uri = sitl.connection_string()
# mavproxy_cmd = \
# "mavproxy.py --console --master tcp:127.0.0.1:5760 --sitl 127.0.0.1:5501 --out localhost:14550 & /bin/bash"
# subprocess.call(mavproxy_cmd, shell=True)
# time.sleep(1)
# dronekit is broken! it always tries to connect to 127.0.0.1:5760
print("try to connect to vehicle...")
dronekit_connects_to = 'tcp:127.0.0.1:5760'
vehicle = dronekit.connect(dronekit_connects_to, wait_ready=True)
# TODO: add time limit
while not vehicle.is_armable:
time.sleep(0.2)
# TODO: add time limit
# arm the rover
vehicle.armed = True
while not vehicle.armed:
print("waiting for the rover to be armed...")
time.sleep(0.1)
vehicle.armed = True
# TODO: add time limit
issue_mission(vehicle, mission)
# trigger the mission by switching the vehicle's mode to "AUTO"
vehicle.mode = VehicleMode("AUTO")
# monitor the mission
last_wp = vehicle.commands.count
mission_complete = [False]
waypoints_visited = []
try:
def on_waypoint(self, name, message):
wp = int(message.seq)
# print("Reached WP #{}".format(wp))
# record the (relevant) state of the vehicle
waypoints_visited.append((wp, snapshot(vehicle)))
if wp == last_wp:
mission_complete[0] = True
vehicle.add_message_listener('MISSION_ITEM_REACHED', on_waypoint)
# wait until the last waypoint is reached or a time limit has expired
time_started = timer()
while not mission_complete[0]:
time_elapsed = timer() - time_started
if time_limit and time_elapsed > time_limit:
return waypoints_visited[:]
time.sleep(0.2)
return waypoints_visited[:]
# remove the listener
finally:
vehicle.remove_message_listener('MISSION_ITEM_REACHED',
on_waypoint)
# close the connection and shutdown the simulator
finally:
if vehicle:
vehicle.close()
if sitl:
sitl.stop()
def setup_solo_sitl():
global sitl
sitl = SITL()
sitl.download('solo', '1.2.0')
sitl.launch(sitl_args, await_ready=True, restart=True)
def test_download_404(self):
sitl = SITL()
sitl.download('rocket', '1.123.098.123')
def main():
parser = argparse.ArgumentParser(description='Gps navigation of a single UAV.')
parser.add_argument("x",help ="+x -> EAST and -x -> WEST",type=float,metavar="X")
parser.add_argument("y",help="+y -> NORTH and -y -> SOUTH",type=float,metavar="Y")
parser.add_argument("d",help="total distance by single uav",type=int,metavar="distance")
parser.add_argument('--connect',
help="Vehicle connection target string. If not specified, SITL automatically started and used.")
parser.add_argument('--baudrate',type=int,
help="Vehicle baudrate settings specify 57600 when connecting with radio. If not specified, SITL automatically started and used.",default=115200)
args = parser.parse_args()
connection_string = args.connect
sitl = None
#Start SITL if no connection string specified
if not connection_string or not args.baudrate:
from dronekit_sitl import SITL
sitl = SITL()
sitl.download('copter', '3.3', verbose=True)
sitl_args = ['-I0', '--model', 'quad', '--home=53.367063,-3.109593,0,0']
sitl.launch(sitl_args, await_ready=True, restart=True)
connection_string = 'tcp:127.0.0.1:5762'
# Connect to the Vehicle
print 'Connecting to vehicle on: %s' % connection_string
vehicle = connect(connection_string, wait_ready=True,baud=args.baudrate)
quad = QuadController(vehicle)
home_location = quad.home_position()
init_pos = altered_position(home_location,args.x,args.y)
start = init_pos
end = altered_position(start,0,args.d)
print "Basic pre-arm checks"
# Don't try to arm until autopilot is ready
while not quad.is_armable:
print " Waiting for vehicle to initialise..."
time.sleep(1)
print "Arming motors"
# Copter should arm in GUIDED mode
quad.mode = "GUIDED"
quad.arm = True
# Confirm vehicle armed before attempting to take off
while not quad.arm:
print " Waiting for arming..."
time.sleep(1)
print "Vehicl armed status: " + str(quad.arm)
print "taking off .."
print quad.mode
quad.takeoff()
print "going to first way point..."
quad.goto(init_pos)
if not quad.goto(init_pos,groundspeed=1):
print "some event has gone horribly wrong"
return
while not QuadController.get_distance((quad.current_location.lat,quad.current_location.lon),init_pos):
time.sleep(0.001)#to avoid consuming much CPU
print "arrived at first way point"
#print "settting yaw"
#quad.set_yaw(90)
#time.sleep(5)
#print quad.heading
print "following path..."
path = QuadController.path(start,end)
for p in path[1:]:
print "going to Lat:%.6f, Lon:%.6f" %(p[0],p[1])
if not quad.goto((p[0],p[1])):
print "some event has gone horribly wrong"
return
while not QuadController.get_distance((quad.current_location.lat,quad.current_location.lon),(p[0],p[1])):
time.sleep(0.01)
print quad.current_location
print "vehicle returning to launch..."
quad.mode = "RTL"
"target-lon": 0,
"target-alt": 50,
"expected-heading": 90,
}
],
},
]
# ready data for many dronekit-sitl processes
simulation_count = 2
for i in range(0,simulation_count):
print("Creating simulator (SITL) %d" % (i,))
from dronekit_sitl import SITL
sitl = SITL(instance=i, path=args.binary, defaults_filepath=args.defaults, gdb=True)
# sitl.download('copter', '3.3', verbose=True)
sitls.append(sitl)
hub_connection_strings.append(sitl.connection_string())
# start the SITLs one at a time, giving each a unique SYSID_THISMAV
def set_params_target(i, new_sysid, connection_string):
lat = -35.363261
lon = 149.165230
sitl_args = ['--model', 'quad', '--home=%s,%s,584,353' % (lat,lon,) ]
print("%d: Launching SITL (%s)" % (i,str(sitl_args)))
sitls[i].launch(sitl_args, await_ready=True, verbose=True, speedup=50,)
print("Sleeping a little here")
time.sleep(5)
print("%d: Connecting to its vehicle 1" % (i,))
vehicle = dronekit.connect(connection_string, wait_ready=True, target_system=1, heartbeat_timeout=60)
def __init__(self):
# SITL is the system in the loop simulator from dronekit
self.sitl = SITL()
self.sitl.download('solo', '1.2.0', verbose=True)
sitl_args = ['solo', '--home=51.011447,3.711648,5,0']
self.sitl.launch(sitl_args,
await_ready=True,
restart=True,
verbose=True)
#############################################################################################################
# This is old code from when the onboard and simulator code were no modules yet
# It is used to find the correct directories and start the correct processes, and might prove useful in the future
# # This simulator can be invoked from the /simulator directory and the /tests directory
# # Hardcoding relative paths should be avoided
# current_dir = os.path.abspath(os.curdir)
# parent_dir = os.path.dirname(current_dir)
# drone_dir = os.path.join(parent_dir, "drone")
# simulator_dir = os.path.join(parent_dir, "simulator")
# onboard_dir = os.path.join(parent_dir, "onboard")
# # Search for the drone directory containing the /simulator and /onboard directories
# # Or stop if we find the /simulator and /onboard directories
# while not (os.path.exists(drone_dir) or (os.path.exists(simulator_dir) and os.path.exists(onboard_dir))):
# current_dir = parent_dir
# parent_dir = os.path.dirname(current_dir)
# if parent_dir == current_dir:
# print "Could not find files"
# sys.exit(1) # we did not find one of the necessary files
# drone_dir = os.path.join(parent_dir, "drone")
# simulator_dir = os.path.join(parent_dir, "simulator")
# onboard_dir = os.path.join(parent_dir, "onboard")
# if os.path.exists(drone_dir):
# # we found the drone directory
# simulator_dir = os.path.join(drone_dir, "simulator")
# onboard_dir = os.path.join(drone_dir, "onboard")
# video_dir = os.path.join(simulator_dir, "videos")
# video_footage = os.path.join(video_dir, "testfootage.h264")
# server = os.path.join(onboard_dir, "server.py")
# control_module = os.path.join(onboard_dir, "control_module.py")
# if not (os.path.exists(simulator_dir) and
# os.path.exists(onboard_dir) and
# os.path.exists(video_footage) and
# os.path.exists(server) and
# os.path.exists(control_module)):
# print "Could not find files"
# sys.exit(1) # we did not find one of the necessary files
# The server and control modules were started as seperate processes
# This gave issues when trying to measure the code coverage, hence we changed it to threads
# env_vars = os.environ.copy()
# env_vars["COVERAGE_PROCESS_START"] = ".coveragerc"
# self.server_process = Popen(['python2', server, '--level', 'debug', '--simulate'], env=env_vars)
# self.control_process = Popen(['python2', control_module, '--level', 'debug', '--simulate'], env=env_vars)
#############################################################################################################
# Now that we have modules, we can just do this
current_dir = os.path.dirname(
os.path.realpath(__file__)) # this is the directory of the file
video_footage = os.path.join(current_dir, 'videos', 'testfootage.h264')
self.stream_simulator = StreamSimulator(video_footage)
self.stream_simulator.start()
log_level = logging.DEBUG
simulation_logger = logging.getLogger("Simulator")
formatter = logging.Formatter(logformat, datefmt=dateformat)
handler = logging.StreamHandler(stream=sys.stdout)
# To log to a file, this could be used
# handler = logging.FileHandler(filename=log_file)
handler.setFormatter(formatter)
handler.setLevel(log_level)
simulation_logger.addHandler(handler)
simulation_logger.setLevel(log_level)
self.logger = simulation_logger
self.server_thread = ServerSimulator(logger=simulation_logger)
self.control_thread = ControlModuleSimulator(logger=simulation_logger,
log_lvl=log_level)
self.server_thread.start()
self.control_thread.start()
# capture kill signals to send it to the subprocesses
signal.signal(signal.SIGINT, self.signal_handler)
signal.signal(signal.SIGTERM, self.signal_handler)
def setup_sitl():
global sitl
sitl = SITL()
sitl.download('copter', '3.3')
sitl.launch(sitl_args, await_ready=True, restart=True)
'''Runs simulator using python'''
import os
from time import sleep
from dronekit_sitl import SITL
SIM = {}
BIN = os.getenv("SITL_BIN")
DO_DOWNLOAD = True
if BIN is not None:
DO_DOWNLOAD = False
SIM["path"] = BIN
DEFAULTS = os.getenv("SITL_DEFAULTS_FILEPATH")
if DEFAULTS is not None:
SIM["defaults_filepath"] = DEFAULTS
SIM = SITL(**SIM)
if DO_DOWNLOAD:
SIM.download('copter', '3.3', verbose=False)
SIM.launch([
'--model',
'quad',
], await_ready=True, restart=False)
print("Simulator lanched")
while True:
sleep(5)
i = i - 1
elif list[rand] == 'b':
j = j - 1
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 = SITL()
# Start SITL if no connection string specified
if not connection_string:
import dronekit_sitl
sitl = dronekit_sitl.start_default()
connection_string = 'tcp:127.0.0.1:5762'
# Connect to the Vehicle
print('Connecting to vehicle on: %s' % connection_string)
vehicle = connect(connection_string, wait_ready=True)
def arm_and_takeoff(aTargetAltitude):
print("Basic pre-arm checks")
},
]
# ready data for many dronekit-sitl processes
simulation_count = 2
for i in range(0, simulation_count):
print("Creating simulator (SITL) %d" % (i, ))
from dronekit_sitl import SITL
breakpoints = []
# breakpoints = ["GCS_Common.cpp:1031"]
# breakpoints = ["GCS_MAVLINK::handle_mission_item"]
# breakpoints = ["GCS_MAVLINK::handle_mission_count"]
# breakpoints = ["Plane::auto_takeoff_check"]
sitl = SITL(instance=i,
path=args.binary,
defaults_filepath=args.defaults,
gdb=True,
valgrind=False,
gdb_breakpoints=breakpoints)
# sitl.download('copter', '3.3', verbose=True)
sitls.append(sitl)
hub_connection_strings.append(sitl.connection_string())
def load_mission(vehicle):
# load a simple mission so we can take off
cmds = vehicle.commands
print(" Clear any existing commands")
cmds.clear()
def check_mission_commands(conn, name, m):
