def test_set_current(self):
sitl = SITL()
sitl.download('copter', '3.3', verbose=True)
sitl_args = ['-I0', '--model', 'quad', '--home=-35.363261,149.165230,584,353']
sitl.launch(sitl_args, await_ready=True, restart=True)
print "Connecting to vehicle on: 'tcp:127.0.0.1:5760'"
vehicle = connect('tcp:127.0.0.1:5760', wait_ready=True)
print " Battery: %s" % vehicle.battery
battery_test = Battery(vehicle.battery)
# check number negative the limit
battery_test.set_current(NEGATIVE_NUMBER)
self.assertEqual(battery_test.current, 0)
# check number positive the limit
battery_test.set_current(POSITIVE_NUMBER)
self.assertEqual(battery_test.get_current(), POSITIVE_NUMBER)
# check ZERO
battery_test.set_current(0)
self.assertEqual(battery_test.get_current(), 0)
#check char that non number
battery_test.set_current(CHARACTER)
self.assertEqual(battery_test.get_current(), 0)
#check char that number
battery_test.set_current(CHAR_NUMBER)
self.assertEqual(battery_test.get_current(), int(CHAR_NUMBER))
sitl.stop()
def test_can_arm():
sitl = SITL()
sitl.download('copter', '3.3')
sitl.launch(copter_args, verbose=True, await_ready=True)
vehicle = dronekit.connect("tcp:127.0.0.1:5760", wait_ready=True)
# Wait for vehicle to be armable
timeout = time.time() + 10
while not vehicle.is_armable and time.time() < timeout:
time.sleep(0.35)
timed_out = time.time() >= timeout
if timed_out:
sitl.stop()
assert_false(timed_out, "Vehicle init timed out")
# Arm it
vehicle.mode = dronekit.VehicleMode("GUIDED")
vehicle.armed = True
# Confirm that it armed
timeout = time.time() + 10
while not vehicle.armed and time.time() < timeout:
time.sleep(0.35)
timed_out = time.time() >= timeout
vehicle.close()
sitl.stop()
assert_false(timed_out, "Vehicle arming timed out")
def test_land(self):
sitl = SITL()
sitl.download('copter', '3.3', verbose=True)
sitl_args = ['-I0', '--model', 'quad', '--home=31.768923,35.193595,0,0']
sitl.launch(sitl_args, await_ready=True, restart=True)
drone = Drone('tcp:127.0.0.1:5760')
headBefor =drone.vehicle.heading
locBefor = drone.vehicle.location.global_relative_frame
drone.take_off(25)
print drone.vehicle.location.global_relative_frame
drone.land()
print drone.vehicle.location.global_relative_frame
drone.take_off(4)
print drone.vehicle.location.global_relative_frame
drone.land()
print drone.vehicle.location.global_relative_frame
drone.take_off(2)
print drone.vehicle.location.global_relative_frame
drone.land()
print drone.vehicle.location.global_relative_frame
drone.take_off(10)
print drone.vehicle.location.global_relative_frame
drone.land()
print drone.vehicle.location.global_relative_frame
drone.vehicle.close()
sitl.stop()
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 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 test_take_off(self):
sitl = SITL()
sitl.download('copter', '3.3', verbose=True)
sitl_args = ['-I0', '--model', 'quad', '--home=31.768923,35.193595,0,0']
sitl.launch(sitl_args, await_ready=True, restart=True)
drone = Drone('tcp:127.0.0.1:5760')
headBefor =drone.vehicle.heading
locBefor = drone.vehicle.location.global_relative_frame
drone.take_off(3)
headAfter = drone.vehicle.heading
locAfter = drone.vehicle.location.global_relative_frame
self.assertTrue( round(locAfter.alt-locBefor.alt)==3 and ((headBefor+5)%360<headAfter or(headBefor-5)%360 >headAfter))
drone.vehicle.close()
sitl.stop()
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():
"""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 SYSID_THISMAV
print "test"
sitl = SITL()
sitl.download('copter', '3.3')
new_sysid = 10
working_dir = tempfile.mkdtemp()
sitl.launch(copter_args, verbose=True, await_ready=True, use_saved_data=True, wd=working_dir)
vehicle = dronekit.connect("tcp:127.0.0.1:5760", wait_ready=True)
print "Changing SYSID_THISMAV to {0}".format(new_sysid)
while vehicle.parameters["SYSID_THISMAV"] != new_sysid:
vehicle.parameters["SYSID_THISMAV"] = new_sysid
time.sleep(0.1)
time.sleep(6) # Allow time for the parameter to go back to EEPROM
print "Changed SYSID_THISMAV to {0}".format(new_sysid)
vehicle.close()
sitl.stop()
# Now see if it persisted
sitl.launch(copter_args, await_ready=True, use_saved_data=True, wd=working_dir)
vehicle = dronekit.connect("tcp:127.0.0.1:5760", wait_ready=True)
assert_equals(new_sysid, vehicle.parameters["SYSID_THISMAV"])
vehicle.close()
sitl.stop()
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()
class SimRunner:
def arm_vehicle(self):
arm_time = 0
self.vehicle.armed = True
while not self.vehicle.armed:
time.sleep(0.2)
print('waiting for armed...')
arm_time += 0.2
if arm_time > 30:
print("Arm fail")
self.vehicle.close()
self.sitl.stop()
self.sitl.p.stdout.close()
self.sitl.p.stderr.close()
self.ready = False
return
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 run(self):
takeoff_mission = self.missions[0]
takeoff_mission.run(self)
time.sleep(10)
temp_state = []
waypoint_num = 3
T = 2
### 4 missions. Each mission has 3 waypoints. For each waypoint, the vehicle runs for 2 seconds
for i in range(0, 4):
current_location = self.vehicle.location.global_frame
if i % 2 == 0:
target_delta = [
random.uniform(0.0002, 0.0003) / waypoint_num,
random.uniform(0.0002, 0.0003) / waypoint_num,
random.uniform(20, 30) / waypoint_num
]
else:
target_delta = [
random.uniform(0.0002, 0.0003) / waypoint_num,
random.uniform(-0.0003, -0.0002) / waypoint_num,
random.uniform(-30, -20) / waypoint_num
]
# count = 0
for j in range(1, waypoint_num + 1):
profile = LocationGlobal(
current_location.lat + target_delta[0] * j,
current_location.lon + target_delta[1] * j,
current_location.alt + target_delta[2] * j)
self.profiles.append([profile.lat, profile.lon, profile.alt])
self.vehicle.simple_goto(profile)
current_t = 0
temp_state = []
while current_t < T:
### record the [lat,lon,alt,pitch,yaw,roll,velocity] every 0.1 seconds
temp_state.append([
self.vehicle.location.global_frame.lat,
self.vehicle.location.global_frame.lon,
self.vehicle.location.global_frame.alt,
self.vehicle.attitude.pitch, self.vehicle.attitude.yaw,
self.vehicle.attitude.roll, self.vehicle.velocity[0],
self.vehicle.velocity[1], self.vehicle.velocity[2]
])
time.sleep(0.1)
current_t += 0.1
# count += 1
# print(count)
# print_info(self.vehicle)
self.states.append(temp_state)
self.vehicle.close()
self.sitl.stop()
np.save(self.exp_out_dir + "states_np_%s" % self.sim_id,
np.array(self.states))
np.save(self.exp_out_dir + "profiles_np_%s" % self.sim_id,
np.array(self.profiles))
print("Output Execution Path...")
with open(self.exp_out_dir + "raw_%s.txt" % self.sim_id,
"w") as execution_file:
ep_line = self.sitl.stdout.readline(0.01)
while ep_line is not None:
execution_file.write(ep_line)
ep_line = self.sitl.stdout.readline(0.01)
self.sitl.p.stdout.close()
self.sitl.p.stderr.close()
print("Simulation %s completed." % self.sim_id)
def run1(self):
takeoff_mission = self.missions[0]
takeoff_mission.run(self)
time.sleep(10)
home_location = self.vehicle.location.global_frame
temp_state = []
waypoint_num = 3
T = 2
## first mission : guided mode
for i in range(0, 5):
current_location = self.vehicle.location.global_frame
if i % 2 == 0:
target_delta = [
random.uniform(0.0002, 0.0003) / waypoint_num,
random.uniform(0.0002, 0.0003) / waypoint_num,
random.uniform(20, 30) / waypoint_num
]
else:
target_delta = [
random.uniform(0.0002, 0.0003) / waypoint_num,
random.uniform(-0.0003, -0.0002) / waypoint_num,
random.uniform(-30, -20) / waypoint_num
]
for j in range(1, waypoint_num + 1):
profile = LocationGlobal(
current_location.lat + target_delta[0] * j,
current_location.lon + target_delta[1] * j,
current_location.alt + target_delta[2] * j)
self.profiles.append([profile.lat, profile.lon, profile.alt])
self.vehicle.simple_goto(profile)
current_t = 0
temp_state = []
while current_t < T:
temp_state.append([
self.vehicle.location.global_frame.lat,
self.vehicle.location.global_frame.lon,
self.vehicle.location.global_frame.alt,
self.vehicle.attitude.pitch, self.vehicle.attitude.yaw,
self.vehicle.attitude.roll, self.vehicle.velocity[0],
self.vehicle.velocity[1], self.vehicle.velocity[2]
])
time.sleep(0.1)
current_t += 0.1
self.states.append(temp_state)
## second mission : acro mode
self.vehicle.channels.overrides['1'] = 1400
self.vehicle.channels.overrides['2'] = 1400
self.vehicle.channels.overrides['3'] = 1500
self.vehicle.channels.overrides['4'] = 1500
self.vehicle.mode = VehicleMode('ACRO')
for i in range(0, waypoint_num):
current_location = self.vehicle.location.global_frame
self.profiles.append([
current_location.lat, current_location.lon,
current_location.alt
])
current_t = 0
temp_state = []
while current_t < T:
self.vehicle.channels.overrides['3'] = 1500
temp_state.append([
self.vehicle.location.global_frame.lat,
self.vehicle.location.global_frame.lon,
self.vehicle.location.global_frame.alt,
self.vehicle.attitude.pitch, self.vehicle.attitude.yaw,
self.vehicle.attitude.roll, self.vehicle.velocity[0],
self.vehicle.velocity[1], self.vehicle.velocity[2]
])
time.sleep(0.1)
current_t += 0.1
# print_info(self.vehicle)
self.states.append(temp_state)
# ## third mission : auto mode
# cmds = self.vehicle.commands
# cmds.clear()
# waypoint_in_auto = [random.uniform(0.0002,0.0003)/waypoint_num,random.uniform(0.0002,0.0003)/waypoint_num,random.uniform(20,30)/waypoint_num]
# for j in range(1,waypoint_num+1):
# profile = LocationGlobal(current_location.lat+target_delta[0]*j,current_location.lon+target_delta[1]*j,current_location.alt+target_delta[2]*j)
# self.profiles.append([profile.lat,profile.lon,profile.alt])
# cmds.add(Command(0,0,0,mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,mavutil.mavlink.MAV_CMD_NAV_WAYPOINT,0,0,0,0,0,0,profile.lat,profile.lon,profile.alt))
# cmds.upload()
# self.vehicle.commands.next = 0
# self.vehicle.mode = VehicleMode('AUTO')
# for j in range(0,waypoint_num):
# current_t = 0
# temp_state = []
# while current_t < T:
# temp_state.append([self.vehicle.location.global_frame.lat,self.vehicle.location.global_frame.lon,self.vehicle.location.global_frame.alt
# ,self.vehicle.attitude.pitch,self.vehicle.attitude.yaw,self.vehicle.attitude.roll,self.vehicle.velocity[0],self.vehicle.velocity[1],self.vehicle.velocity[2]])
# time.sleep(0.1)
# current_t += 0.1
# self.vehicle.commands.next += 1
# self.states.append(temp_state)
#### fourth mission : rtl mode
# self.vehicle.mode = VehicleMode('RTL')
# for j in range(0,waypoint_num):
# self.profiles.append([home_location.lat,home_location.lon,home_location.alt])
# current_t = 0
# temp_state = []
# while current_t < T:
# temp_state.append([self.vehicle.location.global_frame.lat,self.vehicle.location.global_frame.lon,self.vehicle.location.global_frame.alt
# ,self.vehicle.attitude.pitch,self.vehicle.attitude.yaw,self.vehicle.attitude.roll,self.vehicle.velocity[0],self.vehicle.velocity[1],self.vehicle.velocity[2]])
# time.sleep(0.1)
# current_t += 0.1
# self.states.append(temp_state)
self.vehicle.close()
self.sitl.stop()
np.save(self.exp_out_dir + "states_np_%s" % self.sim_id,
np.array(self.states))
np.save(self.exp_out_dir + "profiles_np_%s" % self.sim_id,
np.array(self.profiles))
print("Output Execution Path...")
with open(self.exp_out_dir + "raw_%s.txt" % self.sim_id,
"w") as execution_file:
ep_line = self.sitl.stdout.readline(0.01)
while ep_line is not None:
execution_file.write(ep_line)
ep_line = self.sitl.stdout.readline(0.01)
self.sitl.p.stdout.close()
self.sitl.p.stderr.close()
print("Simulation %s completed." % self.sim_id)
class Simulator:
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 signal_handler(self, signal, frame):
self.logger.debug("Exiting simulator")
self.stop()
sys.exit(0)
def stop(self):
self.stream_simulator.stop_thread()
self.logger.debug("Stopping server and control module")
self.server_thread.stop_thread()
self.control_thread.stop_thread()
self.logger.debug("Sleeping")
time.sleep(5.0)
self.logger.debug("Stopping SITL")
self.sitl.stop()
def copter_sitl_auto_to():
"""
Returns a copter SITL instance for use in test cases
Status: starting auto takeoff(to 20m AGL), 13 waypoints
Mode: Auto
"""
# 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(),
wait_ready=True,
vehicle_class=DroneTreeVehicle)
# upload an arbitrary mission
mission = MAVWPLoader()
filename = join(abspath(sys.path[0]), 'executable_mission.txt')
mission.load(filename)
# bit of a hack: fix the sequence number
for w in mission.wpoints:
w.seq = w.seq + 1
cmds = veh.commands
cmds.clear()
wplist = mission.wpoints[:]
# add a dummy command on the front
# just the first WP used as a placeholder
# upload seems to skip the first WP
cmds.add(wplist[0])
for wp in wplist:
cmds.add(wp)
cmds.upload()
# wait until armable
while not veh.is_armable:
sleep(0.5)
veh.parameters['SIM_SPEEDUP'] = 5 # set sim speed for rapid take-off
sleep(0.5) # wait for parameter change to take place
veh.arm() # arm vehicle
# Perform an auto take-off
veh.mode = VehicleMode('AUTO') # change mode to auto
veh.channels.overrides['3'] = 1700 # raise throttle to confirm T/O
# wait for take-off
while veh.location.global_relative_frame.alt < 0.5:
sleep(0.5)
veh.close() # close vehicle instance after auto takeoff
yield sitl
# Shut down simulator if it was started.
if sitl is not None:
sitl.stop()
def test_move_left(self):
""""
sitl = SITL()
sitl.download('copter', '3.3', verbose=True)
sitl_args = ['-I0', '--model', 'quad', '--home=31.768923,35.193595,0,0']
sitl.launch(sitl_args, await_ready=True, restart=True)
drone = Drone('tcp:127.0.0.1:5760')
drone.take_off(2)
print drone.vehicle.location.global_relative_frame
drone.move_left(2)
print drone.vehicle.location.global_relative_frame
drone.land()
drone.vehicle.close()
sitl.stop()
"""""
""""
sitl = SITL()
sitl.download('copter', '3.3', verbose=True)
sitl_args = ['-I0', '--model', 'quad', '--home=31.768923,35.193595,0,45']
sitl.launch(sitl_args, await_ready=True, restart=True)
drone = Drone('tcp:127.0.0.1:5760')
drone.take_off(2)
print drone.vehicle.location.global_relative_frame
drone.move_left(2)
print drone.vehicle.location.global_relative_frame
drone.land()
drone.vehicle.close()
sitl.stop()
#self.assertTrue(True)
"""
""""
sitl = SITL()
sitl.download('copter', '3.3', verbose=True)
sitl_args = ['-I0', '--model', 'quad', '--home=31.768923,35.193595,0,90']
sitl.launch(sitl_args, await_ready=True, restart=True)
drone = Drone('tcp:127.0.0.1:5760')
drone.take_off(2)
print drone.vehicle.location.global_relative_frame
drone.move_left(2)
print drone.vehicle.location.global_relative_frame
drone.land()
drone.vehicle.close()
sitl.stop()
#self.assertTrue(True)
"""
""""
sitl = SITL()
sitl.download('copter', '3.3', verbose=True)
sitl_args = ['-I0', '--model', 'quad', '--home=31.768923,35.193595,0,135']
sitl.launch(sitl_args, await_ready=True, restart=True)
drone = Drone('tcp:127.0.0.1:5760')
drone.take_off(2)
print drone.vehicle.location.global_relative_frame
drone.move_left(2)
print drone.vehicle.location.global_relative_frame
drone.land()
drone.vehicle.close()
sitl.stop()
#self.assertTrue(True)
"""
""""
sitl = SITL()
sitl.download('copter', '3.3', verbose=True)
sitl_args = ['-I0', '--model', 'quad', '--home=31.768923,35.193595,0,180']
sitl.launch(sitl_args, await_ready=True, restart=True)
drone = Drone('tcp:127.0.0.1:5760')
drone.take_off(2)
print drone.vehicle.location.global_relative_frame
drone.move_left(2)
print drone.vehicle.location.global_relative_frame
drone.land()
drone.vehicle.close()
sitl.stop()
#self.assertTrue(True)
"""
""""
sitl = SITL()
sitl.download('copter', '3.3', verbose=True)
sitl_args = ['-I0', '--model', 'quad', '--home=31.768923,35.193595,0,225']
sitl.launch(sitl_args, await_ready=True, restart=True)
drone = Drone('tcp:127.0.0.1:5760')
drone.take_off(2)
print drone.vehicle.location.global_relative_frame
drone.move_left(2)
print drone.vehicle.location.global_relative_frame
drone.land()
drone.vehicle.close()
sitl.stop()
#self.assertTrue(True)
"""
sitl = SITL()
sitl.download('copter', '3.3', verbose=True)
sitl_args = ['-I0', '--model', 'quad', '--home=31.768923,35.193595,0,270']
sitl.launch(sitl_args, await_ready=True, restart=True)
drone = Drone('tcp:127.0.0.1:5760')
drone.take_off(2)
print drone.vehicle.location.global_relative_frame
drone.move_left(2)
print drone.vehicle.location.global_relative_frame
drone.land()
drone.vehicle.close()
sitl.stop()
#self.assertTrue(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()
break
time.sleep(1)
def countdown(amtTime):
i = 0
while i <= amtTime:
print("COUNTDOWN: "+str(amtTime-i))
time.sleep(1)
i = i+1
def countdownAlt(amTime):
o = 0
while o <= amTime:
print("COUNTDOWN: "+str(amTime-o))
print " Altitude: ", vehicle.location.global_relative_frame.alt
time.sleep(1)
o = o+1
countdown(5)
cmds = vehicle.commands
cmds.download()
cmds.wait_ready()
home = vehicle.home_location
print home.lat
print home.lon
arm_and_takeoff(2);
vehicle.mode = VehicleMode("RTL")
countdownAlt(30);
sitl.stop();
print("Done")
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 is not None:
sitl.stop()
class APMSim(object):
existing = []
usedINum = mav.manager.list()
@staticmethod
def nextINum():
port = mav.nextUnused(APMSim.usedINum, range(0, 254))
return port
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)
@staticmethod
def create():
index = APMSim.nextINum()
try:
result = APMSim(index)
return result
except Exception as ee:
APMSim.usedINum.remove(index)
raise ee
def setParamAndRelaunch(self, key, value):
wd = self.sitl.wd
v = connect(self.connStr, wait_ready=True)
v.parameters.set(key, value, wait_ready=True)
v.close()
self.sitl.stop()
self.sitl.launch(self.args,
await_ready=True,
restart=True,
wd=wd,
use_saved_data=True)
v = connect(self.connStr, wait_ready=True)
# This fn actually rate limits itself to every 2s.
# Just retry with persistence to get our first param stream.
v._master.param_fetch_all()
v.wait_ready()
actualValue = v._params_map[key]
assert actualValue == value
v.close()
def close(self):
self.sitl.stop()
APMSim.usedINum.remove(self.iNum)
APMSim.existing.remove(self)
@staticmethod
def clean():
for sitl in APMSim.existing:
sitl.stop()
APMSim.existing = []
print("Yaw 90 relative (to previous yaw heading)")
condition_yaw(90, relative=True)
print("Velocity North and East")
send_global_velocity(NORTH, EAST, 0, DURATION)
send_global_velocity(0, 0, 0, 1)
print("Yaw 90 relative (to previous yaw heading)")
condition_yaw(90, relative=True)
print("Velocity South and East")
send_global_velocity(SOUTH, EAST, 0, DURATION)
send_global_velocity(0, 0, 0, 1)
"""
The example is completing. LAND at current location.
"""
print("Setting LAND mode...")
vehicle.mode = VehicleMode("LAND")
#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()
print("Completed")
def main():
"""
The Main function of this script.
"""
args = _parse_arguments()
util.log_init(
"sitl_A%s_%s.txt" % (args.id, util.get_latest_log("latest_sitl.txt")),
util.log_level[args.level])
shared.AGENT_ID = 'A%s' % args.id
shared.AGENT_COUNT = args.n
shared.CURRENT_ALGORITHM = args.algorithm
shared.AGENT_CHARACTER = args.character
shared.des_alt = args.alt
util.log_info("AGENT_ID = %s" % shared.AGENT_ID)
util.log_info("Algorithm: %s" % shared.CURRENT_ALGORITHM)
util.log_info("Agent type: %s" % shared.AGENT_CHARACTER)
print "Start simulator (SITL)"
sitl = SITL(args.pix) # initialize SITL with firmware path
if shared.AGENT_ID in start_loc:
sitl_args = ['--home=%s' % start_loc[shared.AGENT_ID]]
else:
sitl_args = ['--home=%s' % start_loc['FFF']]
# Pre-recorded coordinates.
#sitl_args = ['-I0', '--model', 'quad', '--home=31.301201,121.498192,9,353']
sitl.launch(sitl_args, await_ready=True, restart=True)
# Connect to the vehicle. (Spawn an instance of Vehicle named "vehicle")
# connection port is coded in the file name of the firmware like "ac3.4.5_port5760"
# use regular expression to search the string and extract port number
port = re.search(r'port\d{4}', args.pix)
port = re.search(r'\d{4}', port.group()).group()
print "Connecting to copter on: TCP: 127.0.0.1:%s" % port
copter = nav.connect('tcp:127.0.0.1:%s' % port, wait_ready=True, rate=20)
util.log_info("Copter connected. Firmware: %s" % copter.version)
if not args.xbee: # simulate XBee using ZeroMQ
[pub, sub] = comm.zmq_init(comm_port_list[shared.AGENT_ID],
comm_port_list)
subscriber_thread = comm.Subscriber(shared.AGENT_ID, sub)
subscriber_thread.start()
xbee = pub # make xbee the publisher
util.log_info("ZeroMQ initialzied.")
else: # use actual xbee ports
ser = serial.Serial(args.xbee, 57600)
xbee = comm.xbee_init(ser)
util.log_info("Xbee initialzed.")
info = "IFO,%s connected with firmware %s" % (shared.AGENT_ID,
copter.version)
comm.xbee_broadcast(xbee, info)
_add_listeners(copter)
takeoff_thread = nav.Takeoff(copter, xbee, shared.des_alt, 3)
purge_thread = comm.Purge(shared.neighbors)
broadcast_thread = comm.Broadcast(shared.AGENT_ID, copter, xbee)
flocking_thread = _choose_algorithm(copter, xbee, shared.neighbors)
takeoff_thread.start()
takeoff_thread.join() # wait until takeoff procedure completed
if shared.status['airborne']: # only execute the threads when airborne
util.log_info("Copter is airborne, starting threads.")
broadcast_thread.start()
purge_thread.start()
flocking_thread.start()
# main loop
while True:
try:
time.sleep(.2)
except KeyboardInterrupt:
break
if shared.status['airborne']:
# echo exiting status
if shared.status['exiting']:
info = "IFO,%s %s-ing." % (shared.AGENT_ID,
shared.status['command'])
comm.xbee_broadcast(xbee, info)
util.log_info(info)
# if an rtl or land command is received, kill flocking and set the `exiting` flag
elif shared.status['command'] == 'RTL' or shared.status[
'command'] == 'LAND':
shared.status['thread_flag'] |= shared.FLOCKING_FLAG
nav.set_mode(copter, shared.status['command'])
shared.status['exiting'] = True
if not flocking_thread.is_alive(): # break the loop if finished
break
nav.wait_for_disarm(copter) # wait for disarm
comm.xbee_broadcast(xbee, 'IFO,%s terminated.' % shared.AGENT_ID)
# clean up
purge_thread.stop()
while purge_thread.is_alive():
util.log_info('Waiting for purge to shutdown')
purge_thread.join(3)
util.log_info('Purge killed.')
broadcast_thread.stop()
while broadcast_thread.is_alive():
util.log_info('Waiting for broadcast to shutdown')
broadcast_thread.join(3)
util.log_info('Broadcast killed.')
copter.close()
util.log_info("Copter shutdown.")
if args.xbee:
xbee.halt()
ser.close()
util.log_info("Xbee and serial closed.")
else:
subscriber_thread.stop()
while subscriber_thread.is_alive():
util.log_info('Waiting for Subscriber to shutdown')
subscriber_thread.join(3)
util.log_info('Subscriber killed.')
sitl.stop()
util.log_info("SITL shutdown.")
