def RunScenario(scenario,
verbose=0,
fasttime=True,
use_python=False,
out=None,
output_dir="sim_output"):
"""
Run an ICAROUS scenario
:param scenario: dictionary defining scenario inputs
:param verbose: control printouts (0: none, 1: some, 2+: more)
:param fasttime: run simulation in fasttime (pycarous only)
:param use_python: run simulation with pycarous (override scenario inputs)
:param out: port number to forward MAVLink data for visualization
(use out=None to turn off MAVLink output)
:param output_dir: location to save log files
"""
# Clear out existing logs
ClearLogs(pycarous_log_dir)
ClearLogs(cFS_log_dir)
# Set python option for each vehicle
for v in scenario["vehicles"]:
if use_python:
v["python"] = True
if not v.get("python", scenario.get("python", False)):
fasttime = False
# Create simulation environment
sim_time_limit = scenario.get("time_limit", 1000)
sim = SimEnvironment(verbose=verbose,
fasttime=fasttime,
time_limit=sim_time_limit)
if "merge_fixes" in scenario:
sim.InputMergeFixes(os.path.join(icarous_home,
scenario["merge_fixes"]))
sim.AddWind(scenario.get("wind", [(0, 0)]))
sitl_running = False
# Add Icarous instances to simulation environment
num_vehicles = len(scenario["vehicles"])
for v in scenario["vehicles"]:
v = dict(list(scenario.items()) + list(v.items()))
cpu_id = v.get("cpu_id", len(sim.icInstances) + 1)
spacecraft_id = cpu_id - 1
callsign = v.get("name", "vehicle%d" % spacecraft_id)
fp_file = os.path.join(icarous_home, v["waypoint_file"])
HomePos = GetHomePosition(fp_file)
daa_file = os.path.join(icarous_home,
v.get("daa_file", default_daa_file))
# Initialize Icarous class
python = v.get("python", False)
if python:
os.chdir("../pycarous")
ic = Icarous(HomePos,
simtype="UAM_VTOL",
vehicleID=spacecraft_id,
callsign=callsign,
verbose=verbose,
fasttime=fasttime,
daaConfig=daa_file)
else:
sim.fasttime = False
try:
shutil.copyfile(daa_file,
icarous_exe + "/../ram/DaidalusQuadConfig.txt")
except shutil.SameFileError:
pass
apps = v.get("apps", default_apps)
sitl = v.get("sitl", False)
if sitl:
if "arducopter" not in apps:
apps.append("arducopter")
if "rotorsim" in apps:
apps.remove("rotorsim")
sitl_running |= sitl
ic = IcarousRunner(HomePos,
vehicleID=spacecraft_id,
callsign=callsign,
verbose=verbose,
apps=apps,
sitl=sitl,
out=out)
# Set parameters
param_file = os.path.join(icarous_home, v["parameter_file"])
params = LoadIcarousParams(param_file)
if v.get("param_adjustments"):
params.update(v["param_adjustments"])
params["RESSPEED"] = params["DEF_WP_SPEED"]
ic.SetParameters(params)
# Input flight plan
eta = v.get("eta", False)
ic.InputFlightplanFromFile(fp_file, eta=eta)
# Input geofences
if v.get("geofence_file"):
gf_file = os.path.join(icarous_home, v["geofence_file"])
ic.InputGeofence(gf_file)
# Input simulated traffic
for tf in v["traffic"]:
traf_id = num_vehicles + len(sim.tfList)
sim.AddTraffic(traf_id, HomePos, *tf)
delay = v.get("delay", 0)
time_limit = min(sim_time_limit, v.get("time_limit", sim_time_limit))
sim.AddIcarousInstance(ic, delay, time_limit)
# Run the simulation
sim.RunSimulation()
# Shut down SITL if necessary
if sitl_running:
subprocess.call(["pkill", "-9", "arducopter"])
subprocess.call(["pkill", "-9", "mavproxy"])
# Collect the log files
os.chdir(output_dir)
sim.WriteLog()
os.chdir(sim_home)
CollectLogs(pycarous_log_dir, output_dir)
CollectLogs(cFS_log_dir, output_dir)
fasttime=args.fasttime)
if args.daalog:
# Dirty hack to silently update the daa logging parameter from commandline
os.system(
"sed -Ein -e \'s/(LOGDAADATA)(\\ *)([0-1])(\\.0*)/\\1\\21\\4/\' " +
args.params)
# Read params from file and input params
ic.SetParametersFromFile(args.params)
# Input flightplan
ic.InputFlightplanFromFile(args.flightplan, eta=args.eta, repair=args.repair)
# Input geofences from file
if args.geofence != '':
ic.InputGeofence(args.geofence)
# Add icarous instance to sim environment
sim.AddIcarousInstance(ic, time_limit=args.tlimit)
# Set position uncertainty for vehicles in the simulation
if args.uncertainty:
sim.SetPosUncertainty(0.1, 0.1, 0, 0, 0, 0)
# Run the Simulation
sim.RunSimulation()
# Save json log outputs
sim.WriteLog()
def RunScenarioPy(scenario, verbose=False, eta=False, output_dir="sim_output"):
""" Run an ICAROUS scenario using pyIcarous """
os.chdir(output_dir)
# Create fasttime simulation environment
sim = SimEnvironment()
if "merge_fixes" in scenario:
sim.InputMergeFixes(os.path.join(icarous_home,
scenario["merge_fixes"]))
sim.AddWind(scenario.get("wind", [(0, 0)]))
# Add Icarous instances to simulation environment
num_vehicles = len(scenario["vehicles"])
time_limit = scenario.get("time_limit", 1000)
for v_scenario in scenario["vehicles"]:
cpu_id = v_scenario.get("cpu_id", len(sim.icInstances) + 1)
spacecraft_id = cpu_id - 1
callsign = v_scenario.get("name", "vehicle%d" % spacecraft_id)
waypoints, _, _ = ReadFlightplanFile(
os.path.join(icarous_home, v_scenario["waypoint_file"]))
HomePos = waypoints[0][0:3]
ic = Icarous(HomePos,
simtype="UAM_VTOL",
vehicleID=spacecraft_id,
callsign=callsign,
verbose=1)
# Set parameters
param_file = os.path.join(icarous_home, v_scenario["parameter_file"])
params = LoadIcarousParams(param_file)
if v_scenario.get("param_adjustments"):
params.update(v_scenario["param_adjustments"])
params["RESSPEED"] = params["DEF_WP_SPEED"]
ic.SetParameters(params)
# Input flight plan
fp_file = os.path.join(icarous_home, v_scenario["waypoint_file"])
ic.InputFlightplanFromFile(fp_file, eta=eta)
# Input geofences
if v_scenario.get("geofence_file"):
gf_file = os.path.join(icarous_home, v_scenario["geofence_file"])
ic.InputGeofence(gf_file)
# Input simulated traffic
for tf in v_scenario["traffic"]:
traf_id = num_vehicles + len(sim.tfList)
sim.AddTraffic(traf_id, HomePos, *tf)
delay = v_scenario.get("delay", 0)
sim.AddIcarousInstance(ic, delay, time_limit)
sim.RunSimulation()
sim.WriteLog()
os.chdir(sim_home)
# Collect log files
for ic in sim.icInstances:
logname = "simlog-%s-%f.json" % (ic.callsign, time.time())
dest = os.path.join(output_dir, logname)
print("writing log: %s" % dest)
log_data = {
"scenario": scenario,
"ownship_id": ic.vehicleID,
"ownship": ic.ownshipLog,
"traffic": ic.trafficLog,
"waypoints": ic.flightplan1,
"geofences": ic.fenceList,
"parameters": ic.params,
"sim_type": "pyIcarous"
}
with open(dest, 'w') as f:
json.dump(log_data, f)
verbose=1,
callsign="sc1")
# Read params from file and input params
params = LoadIcarousParams(
'../TestRunner/tcl4+/merging_scenario/merging_default.parm')
ic1.SetParameters(params)
ic2.SetParameters(params)
# Input flight plans
ic1.InputFlightplanFromFile(
'../TestRunner/tcl4+/merging_scenario/vehicle0_waypoints.txt')
ic2.InputFlightplanFromFile(
'../TestRunner/tcl4+/merging_scenario/vehicle1_waypoints.txt')
sim.AddIcarousInstance(ic1, delay=5, time_limit=100)
sim.AddIcarousInstance(ic2, delay=5, time_limit=100)
sim.SetPosUncertainty(0.01, 0.01, 0, 0, 0, 0)
# Run the simulation
sim.RunSimulation()
sim.WriteLog()
# Plot data for visualization
#plt.figure(1)
#for ic in sim.icInstances:
#plt.plot(np.array(ic.ownshipLog["positionNED"])[:, 1],
#np.array(ic.ownshipLog["positionNED"])[:, 0],
#label=ic.vehicleID)
#plt.legend()
#plt.savefig("merging_example.png")
# Input flightplan
flightplan = [[37.55290000,-122.27250000,0.00000000, 0],
[37.55569196,-122.27680163,403.34964813, 10],
[37.56363069,-122.28903606,787.79770992, 10],
[37.56790032,-122.29561791,989.20925446, 10],
[37.56812906,-122.29597056,997.30231350, 50],
[37.56836248,-122.29633044,999.99999956, 50] ,
[37.57193913,-122.30184516,1000.00000000, 50],
[37.63472832,-122.39880873,1000.00000000, 50],
[37.63509610,-122.39937753,997.04321204, 50],
[37.63545875,-122.39993840,988.17284609, 50],
[37.63890105,-122.40526271,851.70136857, 50]]
ic.InputFlightplan(flightplan,0)
sim.AddIcarousInstance(ic)
sim.SetPosUncertainty(0.01, 0.01, 0, 0, 0, 0)
# Run the simulation
sim.RunSimulation()
sim.WriteLog()
# Plot data for visualization
#plt.figure(1)
#plt.plot(np.array(ic.positionLog)[:,0],np.array(ic.positionLog)[:,1],'r')
#plt.plot(np.array(ic.localPlans[0])[:,1],np.array(ic.localPlans[0])[:,0],'g--')
#plt.scatter(np.array(ic.localPlans[0])[:,1],np.array(ic.localPlans[0])[:,0])
#for tf in tfList:
#plt.plot(np.array(tf.log['pos'])[:,0],np.array(tf.log['pos'])[:,1],'b')
#plt.figure(2)
#plt.plot([i for i in range(len(ic.positionLog))],np.array(ic.positionLog)[:,2])