help='Simulate constant wind: SOURCE (deg, 0=North), SPEED (m/s)')
parser.add_argument("--daalog", action="store_true", help='Enable daa logs')
args = parser.parse_args()
if args.cfs:
args.fasttime = False
# Create log folder for module logs
try:
os.mkdir(os.path.join(os.getcwd(), 'log'))
except FileExistsError:
pass
except:
raise
# Initialize simulation environment
sim = SimEnvironment(fasttime=args.fasttime, verbose=args.verbosity)
sim.AddWind([args.wind])
# Set the home position for the simulation
HomePos = GetHomePosition(args.flightplan)
# Add traffic inputs
if args.traffic != '':
tfinputs = ReadTrafficInput(args.traffic)
for tf in tfinputs:
sim.AddTraffic(tf[0], HomePos, *tf[1:-1], delay=tf[-1])
# Initialize Icarous class
if args.cfs:
ic = IcarousRunner(HomePos, verbose=args.verbosity)
else:
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)
vector = vector.normalize()
next_point = p1 + vector * delta_d
fp_start = []
#Startup Waypoint
fp_start.append([
scenario.start_real[0], scenario.start_real[1], 0.0, speed_aircraft,
[0, 0, 0], [0.0, 0, 0]
])
#First Calculeted Waypoint
lat = next_point[1] * scenario.lat_range + min(scenario.lat_region)
lon = next_point[0] * scenario.lon_range + min(scenario.lon_region)
fp_start.append([lat, lon, 5.0, speed_aircraft, [0, 0, 0], [0.0, 0, 0]])
# Initialize simulation environment
sim = SimEnvironment(fasttime=args.fasttime, verbose=args.verbosity)
# Set the home position for the simulation
HomePos = list(scenario.start_real)
# Add traffic inputs
if args.traffic != '':
#tfinputs = ReadTrafficInput(args.traffic)
#for tf in tfinputs:
# sim.AddTraffic(tf[0], fp_start[1][:3], *tf[1:])
#[id, range [m], bearing [deg], altitude [m], speed [m/s], heading [deg], climb rate [m/s]]
idx = 1
tfHomePos = fp_start[idx][:3]
bearing = get_bearing(fp_start[idx - 1][:2], fp_start[idx][:2])
tf = [1, 70, bearing, 5, 5, 180 + bearing, 0]
sim.AddTraffic(tf[0], tfHomePos, *tf[1:])
)
parser.add_argument("-v",
"--verbosity",
type=int,
choices=[1, 2],
default=1,
help='Set print verbosity level')
parser.add_argument("-u",
"--uncertainty",
type=bool,
default=False,
help='Enable uncertainty')
args = parser.parse_args()
# Initialize simulation environment
sim = SimEnvironment()
# Set the home position for the simulation
HomePos = GetHomePosition(args.flightplan)
# Add traffic inputs
if args.traffic != '':
tfinputs = ReadTrafficInput(args.traffic)
for tf in tfinputs:
sim.AddTraffic(tf[0], HomePos, *tf[1:])
# Initialize Icarous class
ic = Icarous(HomePos,
simtype="UAM_VTOL",
monitor=args.daaType,
verbose=args.verbosity,
from ups import MasterAnchorNode, AnchorNode, SensorNode
from rls import RLSNode
from lst import LSTNode
from PositionCalculator import UPSCalculator, TOACalculator, TDOACalculator
from random import uniform, gauss
import matplotlib.pyplot as plt
import numpy as np
# RLS TEST
D = 4000 # side of the simulation area
L = 400 # distance between two node
sim = SimEnvironment((D, D, 500))
# add initial anchors
anchors = [(D / 2 - L, D / 2 - L, 0), (D / 2 + L, D / 2 - L, 0),
(D / 2, D / 2 + L, 0)
# , (D/2, D/2 + L, -300)
]
for n, p in enumerate(anchors):
node = LSTNode(n, p, True)
sim.addNode(node)
n = len(sim.nodes)
R = D / L
from matplotlib import pyplot as plt
from SimEnvironment import SimEnvironment
from Icarous import *
# Initialize simulation environment
sim = SimEnvironment()
sim.InputMergeFixes('../TestRunner/tcl4+/merging_scenario/merge_fixes.txt')
# Initialize Icarous class
HomePos1 = [37.415756, -122.056634, 0]
HomePos2 = [37.416407, -122.056657, 0]
ic1 = Icarous(HomePos1,
simtype="UAM_VTOL",
vehicleID=0,
verbose=1,
callsign="sc0")
ic2 = Icarous(HomePos2,
simtype="UAM_VTOL",
vehicleID=1,
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(
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)
from matplotlib import pyplot as plt
from SimEnvironment import SimEnvironment
from Icarous import *
# Initialize simulation environment
sim = SimEnvironment()
# Set the home position for the simulation
HomePos = [37.55290000,-122.27250000,0.000000]
# Start a single traffic vehicle from Home at specified
# range,brg,alt,track,speed,climb rate
# Call this function again to start multiple traffic vehicles
#StartTraffic(1,HomePos,15000,309,5,50,127,0,tfList)
sim.AddTraffic(1,HomePos,26000,309,1000,50,127,0)
# Initialize Icarous class
ic = Icarous(HomePos, simtype = 'UAM_VTOL')
# Read params from file and input params
params = LoadIcarousParams('data/icarous_default2.parm')
ic.daafile = "data/DaidalusQuadConfig.txt"
ic.SetParameters(params)
# 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],
from matplotlib import pyplot as plt
from SimEnvironment import SimEnvironment
from Icarous import *
# Initialize simulation environment
sim = SimEnvironment()
# Initialize Icarous class
HomePos1 = [37.102177, -76.387207, 0.000000]
HomePos2 = [37.102177, -76.387937, 0.000000]
ic1 = Icarous(HomePos1,
simtype="UAM_VTOL",
vehicleID=0,
verbose=1,
callsign="sc0")
ic2 = Icarous(HomePos2,
simtype="UAM_VTOL",
vehicleID=1,
verbose=1,
callsign="sc1")
# Read params from file and input params
params = LoadIcarousParams('data/spacing_default.parm')
ic1.SetParameters(params)
ic2.SetParameters(params)
# Input flight plans
flightplan1 = [[37.102177, -76.387207, 5.000000, 5.0],
[37.102177, -76.287207, 5.000000, 5.0]]
flightplan2 = [[37.102177, -76.387207, 5.000000, 10.0],