def verify_waypoint_progress(simdata, params=DEFAULT_VALIDATION_PARAMS):
'''Check simulation data for progress towards goal'''
condition_name = "Waypoint Progress"
wp_radius = params["wp_radius"]
origin = simdata["ownship"]["position"][0]
lla2ned = lambda x: GS.LLA2NED(origin, x)
waypoints = [lla2ned(wp[0:3])+[i] for i, wp in enumerate(simdata["waypoints"])]
pos_local = [lla2ned(pos) for pos in simdata["ownship"]["position"]]
reached = []
for pos in pos_local:
# Check for reached waypoints
for wp_seq, wp_pos in enumerate(waypoints):
# If within wp_radius, add to reached
dist = np.sqrt((pos[0] - wp_pos[0])**2 + (pos[1] - wp_pos[1])**2)
if dist < wp_radius and wp_seq not in reached:
#print("reached", wp[3], pos)
reached.append(wp_seq)
reached_expected = get_planned_waypoints(simdata)
#print("reached %s" % reached)
#print("expected %s" % reached_expected)
if set(reached) == set(reached_expected):
return True, "Reached all planned waypoints", condition_name
else:
return False, "Did not reach all planned waypoints", condition_name
def verify_geofence(simdata):
'''Check simulation data for geofence violations'''
condition_name = "Geofencing"
geofences = simdata["geofences"]
waypoints = simdata["waypoints"]
origin = [waypoints[0][0], waypoints[0][1]]
lla2ned = lambda x: GS.LLA2NED(origin, x)
if len(geofences) == 0:
return True, "No Geofences", condition_name
for i, pos in enumerate(simdata["ownship"]["position"]):
pos = lla2ned(pos)
# Check for keep in fence violations
keep_in_fence = [Vector(*vertex) for vertex in geofences[0]]
s = Vector(pos[1], pos[0])
if not definitely_inside(keep_in_fence, s, 0.01):
t = simdata["ownship"]["t"][i]
msg = "Keep In Geofence Violation at time = %fs" % t
return False, msg, condition_name
# Check for keep out fence violations
for fence in geofences[1:]:
fencePoly = [Vector(*vertex) for vertex in fence]
if definitely_inside(fencePoly, s, 0.01):
t = simdata["ownship"]["t"][i]
msg = "Keep Out Geofence Violation at time = %fs" % t
return False, msg, condition_name
return True, "No Geofence Violation", condition_name
def get_planned_waypoints(simdata):
""" Return the list of wps that are reachable without violating a geofence """
if not simdata.get("geofences"):
# All waypoints should be reachable
return range(len(simdata["waypoints"]))
planned_wps = []
origin = simdata["ownship"]["position"][0]
geofences = GetPolygons(origin, simdata["geofences"])
keep_in_fence = [Vector(*vertex) for vertex in geofences[0]]
keep_out_fences = [[Vector(*vertex) for vertex in fence]
for fence in geofences[1:]]
lla2ned = lambda x: GS.LLA2NED(origin, x)
waypoints = [lla2ned(wp[0:3]) for wp in simdata["waypoints"]]
for i, wp in enumerate(waypoints):
reachable = True
s = Vector(wp[1], wp[0])
# Check for keep in fence violations
if not definitely_inside(keep_in_fence, s, 0.01):
reachable = False
# Check for keep out fence violations
for fence in keep_out_fences:
if definitely_inside(fence, s, 0.01):
reachable = False
if reachable:
planned_wps.append(i)
return planned_wps
def verify_traffic(simdata, params=DEFAULT_VALIDATION_PARAMS):
'''Check simulation data for traffic well clear violations'''
condition_name = "Traffic Avoidance"
DMOD = simdata["parameters"]["DET_1_WCV_DTHR"]*0.3048 # Convert ft to m
ZTHR = simdata["parameters"]["DET_1_WCV_ZTHR"]*0.3048 # Convert ft to m
h_allow = params["h_allow"]
v_allow = params["v_allow"]
waypoints = simdata["waypoints"]
origin = simdata["ownship"]["position"][0]
lla2ned = lambda x: GS.LLA2NED(origin, x)
ownship_position = [lla2ned(pos) for pos in simdata["ownship"]["position"]]
for i, o_pos in enumerate(ownship_position):
o_alt = o_pos[2]
for traffic_id in simdata["traffic"].keys():
traffic = simdata["traffic"][traffic_id]
if i > len(traffic["t"]) - 1:
continue
t_pos = lla2ned(traffic["position"][i])
t_alt = t_pos[2]
dist = np.sqrt((o_pos[0] - t_pos[0])**2 + (o_pos[1] - t_pos[1])**2)
horiz_violation = (dist < DMOD*h_allow)
vert_violation = (abs(o_alt - t_alt) < ZTHR*v_allow)
if horiz_violation and vert_violation:
t = simdata["ownship"]["t"][i]
msg = "Well Clear Violation at t = %fs" % t
return False, msg, condition_name
return True, "No Well Clear Violation", condition_name
def verify_geofence(simdata, params=DEFAULT_VALIDATION_PARAMS):
'''Check simulation data for geofence violations'''
condition_name = "Geofencing"
origin = simdata["ownship"]["position"][0]
geofences = GetPolygons(origin, simdata["geofences"])
if len(geofences) == 0:
return True, "No Geofences", condition_name
keep_in_fence = [Vector(*vertex) for vertex in geofences[0]]
keep_out_fences = [[Vector(*vertex) for vertex in fence]
for fence in geofences[1:]]
lla2ned = lambda x: GS.LLA2NED(origin, x)
pos_local = [lla2ned(pos) for pos in simdata["ownship"]["position"]]
pos_vector = [Vector(pos[1], pos[0]) for pos in pos_local]
for i, s in enumerate(pos_vector):
# Check for keep in fence violations
if not definitely_inside(keep_in_fence, s, 0.01):
t = simdata["ownship"]["t"][i]
msg = "Keep In Geofence Violation at time = %fs" % t
return False, msg, condition_name
# Check for keep out fence violations
for fence in keep_out_fences:
if definitely_inside(fence, s, 0.01):
t = simdata["ownship"]["t"][i]
msg = "Keep Out Geofence Violation at time = %fs" % t
return False, msg, condition_name
return True, "No Geofence Violation", condition_name
def verify_traffic(simdata):
'''Check simulation data for traffic well clear violations'''
condition_name = "Traffic Avoidance"
DMOD = simdata["params"]["DET_1_WCV_DTHR"] * 0.3048 # Convert ft to m
ZTHR = simdata["params"]["DET_1_WCV_ZTHR"] * 0.3048 # Convert ft to m
waypoints = simdata["waypoints"]
origin = [waypoints[0][0], waypoints[0][1], waypoints[0][2]]
lla2ned = lambda x: GS.LLA2NED(origin, x)
for i, o_pos in enumerate(simdata["ownship"]["position"]):
o_pos = lla2ned(o_pos)
o_alt = o_pos[2]
for traffic_id in simdata["traffic"].keys():
traffic = simdata["traffic"][traffic_id]
t_pos = lla2ned(traffic["position"][i])
t_alt = t_pos[2]
dist = np.sqrt((o_pos[0] - t_pos[0])**2 + (o_pos[1] - t_pos[1])**2)
horiz_violation = (dist < DMOD * 0.85)
vert_violation = (abs(o_alt - t_alt) < ZTHR)
if horiz_violation and vert_violation:
t = simdata["ownship"]["t"][i]
msg = "Well Clear Violation at t = %fs" % t
return False, msg, condition_name
return True, "No Well Clear Violation", condition_name
def verify_waypoint_progress(simdata):
'''Check simulation data for progress towards goal'''
condition_name = "Waypoint Progress"
waypoints = simdata["waypoints"]
origin = [waypoints[0][0], waypoints[0][1]]
lla2ned = lambda x: GS.LLA2NED(origin, x)
reached = []
for i, pos in enumerate(simdata["ownship"]["position"]):
pos = lla2ned(pos)
# Check for reached waypoints
for wp in waypoints:
wp_pos = lla2ned([wp[0], wp[1]])
wp_seq = wp[3]
# If within 5m, add to reached
dist = np.sqrt((pos[0] - wp_pos[0])**2 + (pos[1] - wp_pos[1])**2)
if dist < 5 and wp_seq not in reached:
reached.append(wp[3])
default = [wp[3] for wp in waypoints]
reached_expected = simdata["scenario"].get("planned_wps", default)
# print("reached %s" % reached)
# print("expected %s" % reached_expected)
if set(reached) == set(reached_expected):
return True, "Reached all planned waypoints", condition_name
else:
return False, "Did not reach all planned waypoints", condition_name
def plot_scenario(simdata, save=False, output_dir=""):
'''Plot the result of the scenario'''
from matplotlib import pyplot as plt
fig = plt.figure()
WP = simdata["waypoints"]
origin = [WP[0][0], WP[0][1]]
lla2ned = lambda x: GS.LLA2NED(origin, x)
# Plot waypoints
waypoints = [[wp[0], wp[1]] for wp in WP]
waypoints_local = list(map(lla2ned, waypoints))
wp_x = [val[0] for val in waypoints_local]
wp_y = [val[1] for val in waypoints_local]
plt.plot(wp_y, wp_x, 'k*:', label="Flight Plan")
# Plot ownship path
ownship_poslocal = list(map(lla2ned, simdata["ownship"]["position"]))
ownpos_x = [val[0] for val in ownship_poslocal]
ownpos_y = [val[1] for val in ownship_poslocal]
plt.plot(ownpos_y, ownpos_x, label="Ownship Path")
# Plot traffic path
for traf in simdata["traffic"].values():
traffic_poslocal = list(map(lla2ned, traf["position"]))
traffic_x = [val[0] for val in traffic_poslocal]
traffic_y = [val[1] for val in traffic_poslocal]
plt.plot(traffic_y, traffic_x, label="Traffic Path")
# Plot geofences
for i, fence in enumerate(simdata["geofences"]):
geopos_x = [val[0] for val in fence]
geopos_y = [val[1] for val in fence]
geopos_x.append(geopos_x[0])
geopos_y.append(geopos_y[0])
if i == 0:
plt.plot(geopos_x, geopos_y, 'orange', label="Keep In Geofence")
else:
plt.plot(geopos_x,
geopos_y,
'r',
label="Keep Out Geofence" + str(i))
# Set up figure
plt.title(simdata["scenario"]["name"] + " Scenario - Simulation Results")
plt.xlabel("X (m)")
plt.ylabel("Y (m)")
plt.legend()
plt.axis('equal')
if save:
if not os.path.isdir(output_dir):
os.makedirs(output_dir)
output_file = os.path.join(output_dir, "simplot.png")
plt.savefig(output_file)
plt.close(fig)
else:
plt.show()
def VerifySimOutput(data):
ownship = data["ownship_pos"]
traffic = data["traffic_pos"]
waypoints = data["waypoints"]
geofences = data["geofences"]
numWaypoints = len(waypoints)
origin = [waypoints[0][0], waypoints[0][1], 0.0]
lla2ned = lambda x: GS.LLA2NED(origin, x)
ownship_poslocal = map(lla2ned, ownship)
traffic_poslocal = map(lla2ned, traffic)
waypoints_poslocal = map(lla2ned, waypoints)
progress = {}
keepInViolation = False
keepOutViolation = False
trafficViolation = False
if (len(traffic_poslocal) > 0):
print("found traffic\n")
for i, elem in enumerate(waypoints):
progress["wp" + str(i)] = False
# Check for progress to waypoints. Let progress be a list of booleans (one
# for each waypoint, True if its reached)
for j, pos in enumerate(ownship_poslocal):
for i, wp in enumerate(waypoints_poslocal):
dist = np.sqrt((pos[0] - wp[0])**2 + (pos[1] - wp[1])**2)
val = True if dist < 10 else False
progress["wp" + str(i)] = progress["wp" + str(i)] or val
if len(geofences) > 0:
# Check for keep in fence violations
keep_in_fence = [Vector(*vertex) for vertex in geofences[0]]
s = Vector(pos[1], pos[0])
if not definitely_inside(keep_in_fence, s, 0.01):
keepInViolation = True
# Check for keep out fence violations
for fence in geofences[1:]:
fencePoly = [Vector(*vertex) for vertex in fence]
if definitely_inside(fencePoly, s, 0.01):
keepOutViolation = True
# Check for traffic violations
if (len(traffic_poslocal) > 0):
tfpos = traffic_poslocal[j]
dist = np.sqrt((pos[0] - tfpos[0])**2 + (pos[1] - tfpos[1])**2)
if dist < 5:
trafficViolation = True
return (progress, keepInViolation, keepOutViolation, trafficViolation)
def GetPolygons(origin, fenceList):
"""
Constructs a list of Polygon (for use with PolyCARP) given a list of fences
@param fenceList list of fence dictionaries
@return list of polygons (a polygon here is a list of Vectors)
"""
Polygons = []
for fence in fenceList:
vertices = fence["Vertices"]
vertices_ned = [list(reversed(GS.LLA2NED(origin[0:2], position)))
for position in vertices]
polygon = [[vertex[0], vertex[1]] for vertex in vertices_ned]
Polygons.append(polygon)
return Polygons
def plot_scenario(simdata, output_dir="", save=False):
'''Plot the result of the scenario'''
from matplotlib import pyplot as plt
scenario_name = os.path.basename(output_dir)
fig1 = plt.figure()
WP = simdata["waypoints"]
origin = simdata["ownship"]["position"][0]
geofences = GetPolygons(origin, simdata["geofences"])
lla2ned = lambda x: GS.LLA2NED(origin, x)
# Plot waypoints
waypoints = [wp[0:3] for wp in WP]
waypoints_local = np.array([lla2ned(wp) for wp in waypoints])
plt.plot(waypoints_local[:, 1], waypoints_local[:, 0], 'k*:', label="Flight Plan")
# Plot ownship path
ownpos_local = np.array([lla2ned(ownpos) for ownpos in simdata["ownship"]["position"]])
plt.plot(ownpos_local[:, 1], ownpos_local[:, 0], label="Ownship Path")
# Plot traffic path
for traf_id, traf in simdata["traffic"].items():
trafpos_local = np.array([lla2ned(t_pos) for t_pos in traf["position"]])
plt.plot(trafpos_local[:, 1], trafpos_local[:, 0], label=str(traf_id)+" Path")
# Plot geofences
for i, vertices in enumerate(geofences):
vertices.append(vertices[0])
vertices = np.array(vertices)
if i == 0:
plt.plot(vertices[:, 0], vertices[:, 1], 'orange', label="Keep In Geofence")
else:
plt.plot(vertices[:, 0], vertices[:, 1], 'r', label="Keep Out Geofence"+str(i))
# Set up figure
plt.title(scenario_name + " - Ground track")
plt.xlabel("X (m)")
plt.ylabel("Y (m)")
plt.legend()
plt.axis('equal')
if save:
output_file = os.path.join(output_dir, "simplot.png")
plt.savefig(output_file)
plt.close(fig1)
# Plot ownship and traffic altitudes
fig2 = plt.figure()
ownship_alt = np.array(simdata['ownship']['position'])[:,2]
t = np.array(simdata['ownship']['t'])
plt.plot(t,ownship_alt,label='ownship')
plt.title(scenario_name + " - Altitude data")
plt.xlabel("t (s)")
plt.ylabel("Alt (m)")
plt.legend()
# Plot traffic path
for traf_id, traf in simdata["traffic"].items():
trafpos_alt = np.array(traf["position"])[:,2]
plt.plot(t, trafpos_alt, label=str(traf_id)+" Path")
if save:
output_file = os.path.join(output_dir, "alt.png")
plt.savefig(output_file)
plt.close(fig2)
# Plot onwship speed
fig3 = plt.figure()
ownship_vel = simdata['ownship']['velocityNED']
speed = [np.sqrt(v[0]**2 + v[1]**2 + v[2]**2) for v in ownship_vel]
plt.plot(t,speed,label='ownship')
plt.title(scenario_name + " - Speed data")
plt.xlabel("t (s)")
plt.ylabel("speed (m/s)")
plt.legend()
if save:
output_file = os.path.join(output_dir, "speed.png")
plt.savefig(output_file)
plt.close(fig3)
if not save:
plt.show()
plan.solutionData = sorted(plan.solutionData, key=lambda x: x[2])
plan.plotSolutionPath(anima=False)
input('Start MavLink Connection?')
# Open a mavlink UDP port
master = None
try:
master = mavutil.mavlink_connection("udp:127.0.0.1:14553", source_system=1)
#master = mavutil.mavlink_connection("udp:127.0.0.1:14553", source_system=255)
#print (master)
except Exception as msg:
print("Error opening mavlink connection " + str(msg))
master.wait_heartbeat()
GS = BatchGSModule(master, 1, 0)
input('Load In Geofence?')
GS.loadGeofence([polygon], 1)
input('Load Out Geofence?')
GS.loadGeofence(polygon_list[:5], 0)
input('Load WayPoints?')
GS.loadWaypoint(plan.solutionData[0][1])
input('Start Mission?')
GS.StartMission()
input('End?')
# Start ICAROUS
#sitl = subprocess.Popen(['cd','/home/josuehfa/System/icarous/exe/cpu1/','&&','./core-cpu1', '-C', '1', '-I', '0'], shell=True, stdout=subprocess.PIPE)
#time.sleep(30)
#Start MAVProxy
f2 = open('sitl_output.txt', 'w')
# Start sim vehicle simulation script
#sim = subprocess.Popen(["sim_vehicle.py","-v","ArduCopter","-l","37.1021769,-76.3872069,5,0","-S","1"],stdout=f1)
# Start ICAROUS
#sitl = subprocess.Popen(["java","-cp","lib/icarous.jar:lib/jssc-2.8.0.jar","launch","-v",\
# "--sitl","localhost","14551",\
# "--com","localhost","14552","14553",\
# "--mode", "active"],stdout=f2)
# Open a mavlink UDP port
master = None
try:
master = mavutil.mavlink_connection("udp:127.0.0.1:14553",
dialect="icarous",
source_system=1)
except Exception as msg:
print "Error opening mavlink connection"
master.wait_heartbeat()
GS = BatchGSModule(master, 1, 0)
GS.loadWaypoint("../../Java/params/flightplan.txt")
GS.loadGeofence("../../Java/params/geofence.xml")
GS.StartMission()
#time.sleep(120)
#os.kill(sim.pid,signal.SIGTERM)
#subprocess.Popen(["pkill","xterm"])
#sitl.kill()
#gs.kill()
#os.kill(os.getpid(),signal.SIGTERM)
#f1 = open('sim_output.txt', 'w')
#f2 = open('sitl_output.txt','w')
# Start sim vehicle simulation script
#sim = subprocess.Popen(["sim_vehicle.py","-v","ArduCopter","-l","37.1021769,-76.3872069,5,0","-S","1"],stdout=f1)
# Start ICAROUS
#sitl = subprocess.Popen(["java","-cp","lib/icarous.jar:lib/jssc-2.8.0.jar","launch","-v",\
# "--sitl","localhost","14551",\
# "--com","localhost","14552","14553",\
# "--mode", "active"],stdout=f2)
# Open a mavlink UDP port
master = None
try:
master = mavutil.mavlink_connection("udp:127.0.0.1:14553", source_system=1)
#print (master)
except Exception as msg:
print ("Error opening mavlink connection " + str(msg))
master.wait_heartbeat()
GS = BatchGSModule(master,1,0)
GS.loadWaypoint("/home/josuehfa/System/icarous/Scripts/flightplan4.txt")
GS.loadGeofence("/home/josuehfa/System/icarous/Scripts/geofence2.xml")
GS.StartMission()
#time.sleep(120)
#os.kill(sim.pid,signal.SIGTERM)
#subprocess.Popen(["pkill","xterm"])
#sitl.kill()
#gs.kill()
#os.kill(os.getpid(),signal.SIGTERM)
def setup(self):
scenario = self.scenario
self.cpu_id = scenario.get("cpu_id", 1)
self.spacecraft_id = self.cpu_id - 1
self.callsign = scenario.get("name", "vehicle%d" % self.spacecraft_id)
# Set up mavlink connections
icarous_port = 14553 + (self.cpu_id - 1)*10
gs_port = icarous_port
if self.out is not None:
# Use mavproxy to forward mavlink stream (for visualization)
gs_port = icarous_port + 1
logname = "telemetry-%d-%f" % (self.spacecraft_id, time.time())
logfile = os.path.join(self.output_dir, logname + ".tlog")
self.mav_forwarding = subprocess.Popen(["mavproxy.py",
"--master=127.0.0.1:"+str(icarous_port),
"--out=127.0.0.1:"+str(gs_port),
"--out=127.0.0.1:"+str(self.out),
"--target-system=1",
"--target-component=5",
"--logfile="+logfile],
stdout=subprocess.DEVNULL)
# Open connection for virtual ground station
master = mavutil.mavlink_connection("127.0.0.1:"+str(gs_port))
# Start the ICAROUS process
os.chdir(icarous_exe)
fpic = open("icout-%d-%f.txt" % (self.spacecraft_id, time.time()), 'w')
self.ic = subprocess.Popen(["./core-cpu1",
"-I "+str(self.spacecraft_id),
"-C "+str(self.cpu_id)],
stdout=fpic)
os.chdir(sim_home)
# Pause for a couple of seconds here so that ICAROUS can boot up
if self.verbose and self.out is not None:
print("Telemetry for %s is on 127.0.0.1:%d" % (self.callsign, self.out))
if self.verbose:
print("Waiting for heartbeat...")
master.wait_heartbeat()
self.gs = GS.BatchGSModule(master, 1, 0)
# Launch SITL simulator
if self.sitl:
self.launch_arducopter()
# Set up the scenario (flight plan, geofence, parameters, traffic)
self.gs.loadWaypoint(os.path.join(icarous_home, scenario["waypoint_file"]))
wp_log = "flightplan-%d.waypoints" % self.spacecraft_id
shutil.copy(os.path.join(icarous_home, scenario["waypoint_file"]),
os.path.join(self.output_dir, wp_log))
if scenario.get("geofence_file"):
self.gs.loadGeofence(os.path.join(icarous_home, scenario["geofence_file"]))
gf_log = "geofence-%d.xml" % self.spacecraft_id
shutil.copy(os.path.join(icarous_home, scenario["geofence_file"]),
os.path.join(self.output_dir, gf_log))
if scenario.get("parameter_file"):
self.gs.loadParams(os.path.join(icarous_home, scenario["parameter_file"]))
if scenario.get("param_adjustments"):
for param_id, param_value in scenario["param_adjustments"].items():
self.gs.setParam(param_id, param_value)
if scenario.get("traffic"):
for traf in scenario["traffic"]:
self.gs.load_traffic([0]+traf)
self.waypoints = self.get_waypoints()
self.params = self.gs.getParams()
self.gs.setParam("RESSPEED", self.params["DEF_WP_SPEED"])
self.params = self.gs.getParams()
self.geofences = self.gs.fenceList
self.ownship_log = vehicle_log()
self.traffic_log = {}
self.delay = self.scenario.get("delay", 0)
self.duration = 0
# Wait for GPS fix before starting mission
if self.verbose:
print("Waiting for GPS fix...")
while True:
m = master.recv_match(type="GLOBAL_POSITION_INT", blocking=False)
if m is None:
continue
if abs(m.lat) > 1e-5:
break
def __init__(self,
home_pos,
callsign="SPEEDBIRD",
vehicleID=0,
verbose=1,
logRateHz=5,
apps="default",
sitl=False,
out=None):
"""
Initialize an instance of ICAROUS running in cFS
:param apps: List of the apps to run, or "default" to use default apps
:param sitl: When True, launch arducopter for vehicle simulation
:param out: port number to forward MAVLink data for visualization
(use out=None to turn off MAVLink output)
Other parameters are defined in parent class, see IcarousInterface.py
"""
super().__init__(home_pos, callsign, vehicleID, verbose)
self.SetApps(apps=apps)
self.sitl = sitl
self.out = out
self.cpu_id = self.vehicleID + 1
self.spacecraft_id = self.vehicleID
if "rotorsim" in self.apps:
self.simType = "cFS/rotorsim"
elif "arducopter" in self.apps:
self.simType = "cFS/SITL"
else:
self.simType = "cFS"
# Set up mavlink connections
icarous_port = 14553 + 10 * self.spacecraft_id
gs_port = icarous_port
if self.out is not None:
# Use mavproxy to forward mavlink stream (for visualization)
gs_port = icarous_port + 1
logname = "%s-%f" % (self.callsign, time.time())
self.mav_forwarding = subprocess.Popen([
"mavproxy.py", "--master=127.0.0.1:" + str(icarous_port),
"--out=127.0.0.1:" + str(gs_port),
"--out=127.0.0.1:" + str(self.out), "--target-system=1",
"--target-component=5", "--logfile=" + logname
],
stdout=subprocess.DEVNULL)
if self.verbose:
print("%s : Telemetry for %s is on 127.0.0.1:%d" %
(self.callsign, self.callsign, self.out))
# Open connection for virtual ground station
master = mavutil.mavlink_connection("127.0.0.1:" + str(gs_port))
# Start the ICAROUS process
os.chdir(icarous_exe)
fpic = open("icout-%s-%f.txt" % (self.callsign, time.time()), 'w')
self.ic = subprocess.Popen([
"./core-cpu1", "-I " + str(self.spacecraft_id),
"-C " + str(self.cpu_id)
],
stdout=fpic)
os.chdir(sim_home)
# Pause for a couple of seconds here so that ICAROUS can boot up
if self.verbose:
print("%s : Waiting for heartbeat..." % self.callsign)
master.wait_heartbeat()
self.gs = GS.BatchGSModule(master, target_system=1, target_component=0)
# Launch SITL
if self.sitl:
self.launch_arducopter()
def RunScenario(scenario,
watch=False,
save=False,
verbose=True,
out="14557",
output_dir=""):
'''run an icarous simulation of the given scenario'''
ownship = vehicle()
traffic = {}
name = scenario["name"].replace(' ', '-')
# Clear message queue to avoid icarous problems
messages = os.listdir("/dev/mqueue")
for m in messages:
os.remove(os.path.join("/dev/mqueue", m))
# Set up mavlink connections
gs_port = "14553"
# Use mavproxy to forward mavlink stream (for visualization)
if watch and not out:
out = "14557"
if out:
gs_port = "14554"
icarous_port = "14553"
mav_forwarding = subprocess.Popen([
"mavproxy.py", "--master=127.0.0.1:" + icarous_port,
"--out=127.0.0.1:" + gs_port, "--out=127.0.0.1:" + out
],
stdout=subprocess.DEVNULL)
# Optionally open up mavproxy with a map window to watch simulation
if watch:
logfile = os.path.join(output_dir, name + ".tlog")
mapwindow = subprocess.Popen([
"mavproxy.py", "--master=127.0.0.1:" + out, "--target-component=5",
"--load-module", "map,console", "--load-module",
"traffic,geofence", "--logfile", logfile
])
# Open connection for virtual ground station
try:
master = mavutil.mavlink_connection("127.0.0.1:" + gs_port)
except Exception as msg:
print("Error opening mavlink connection")
# Start the ICAROUS process
os.chdir(icarous_exe)
ic = subprocess.Popen(["./core-cpu1", "-I 0", "-C 1"],
stdout=subprocess.DEVNULL)
# Pause for a couple of seconds here so that ICAROUS can boot up
if verbose:
print("Waiting for heartbeat...")
master.wait_heartbeat()
gs = GS.BatchGSModule(master, 1, 0)
# Upload the flight plan
gs.loadWaypoint(os.path.join(icarous_home, scenario["waypoint_file"]))
# Read the flight plan
WP = GetWaypoints(gs.wploader)
origin = [WP[0][0], WP[0][1]]
final = [WP[-1][0], WP[-1][1]]
# Upload the geofence
if scenario.get("geofence_file"):
gs.loadGeofence(os.path.join(icarous_home, scenario["geofence_file"]))
# Read the geofences
GF = GetPolygons(origin, gs.fenceList)
# Upload the icarous parameters
if scenario.get("parameter_file"):
gs.loadParams(os.path.join(icarous_home, scenario["parameter_file"]))
if scenario.get("param_adjustments"):
for param_id, param_value in scenario["param_adjustments"].items():
gs.setParam(param_id, param_value)
# Get the parameters
params = gs.getParams()
# Load traffic vehicles
if scenario.get("traffic"):
for traf in scenario["traffic"]:
gs.load_traffic([0] + traf)
# Wait for GPS fix before starting mission
time.sleep(1)
if verbose:
print("Waiting for GPS fix...")
master.recv_match(type="GLOBAL_POSITION_INT", blocking=True)
if verbose:
print("Starting mission")
gs.StartMission()
# Run simulation for specified duration
simTimeLimit = scenario["time_limit"]
startT = time.time()
duration = 0
alt = 0
dist2final = 1000
while duration < simTimeLimit:
if verbose:
print("Sim Duration: %.1fs\t Dist to Final: %.1fm\t Alt: %.1fm%s" %
(duration, dist2final, alt, " " * 10),
end="\r")
time.sleep(0.01)
currentT = time.time()
duration = currentT - startT
gs.Update_traffic()
msg = master.recv_match(blocking=False, type=["GLOBAL_POSITION_INT"])
if msg is None:
continue
# Store ownship position/velocity information
ownship["t"].append(duration)
ownship["position"].append(
[msg.lat / 1E7, msg.lon / 1E7, msg.relative_alt / 1E3])
ownship["velocity"].append([msg.vx / 1E2, msg.vy / 1E2, msg.vz / 1E2])
# Store traffic position/velocity information
for i, traf in enumerate(gs.traffic_list):
if i not in traffic.keys():
traffic[i] = vehicle()
traffic[i]["t"].append(duration)
traffic[i]["position"].append([traf.lat, traf.lon, traf.alt])
traffic[i]["velocity"].append([traf.vx0, traf.vy0, traf.vz0])
# Check end conditions
dist2final = GS.gps_distance(msg.lat / 1E7, msg.lon / 1E7, final[0],
final[1])
alt = ownship["position"][-1][2]
if duration > 20 and dist2final < 5:
if verbose:
print("\nReached final waypoint")
break
# Once simulation is finished, kill the icarous process
ic.kill()
if out:
subprocess.call(["kill", "-9", str(mav_forwarding.pid)])
if watch:
mapwindow.send_signal(signal.SIGINT)
subprocess.call(["pkill", "-9", "mavproxy"])
# Construct the sim data for verification
simdata = {
"geofences": GF,
"waypoints": WP,
"scenario": scenario,
"params": params,
"ownship": ownship,
"traffic": traffic
}
# Save the sim data
if save:
if not os.path.isdir(output_dir):
os.makedirs(output_dir)
output_file = os.path.join(output_dir, "simoutput.json")
f = open(output_file, 'w')
json.dump(simdata, f)
print("\nWrote sim data")
f.close()
return simdata
ownship = vehicle()
traffic = vehicle()
try:
master = mavutil.mavlink_connection("127.0.0.1:14553")
except Exception as msg:
print("Error opening mavlink connection")
# Start the ICAROUS process
ic = subprocess.Popen(["core-cpu1", "-I 0", "-C 1"])
time.sleep(20)
# Pause for a couple of seconds here so that ICAROUS can boot up
master.wait_heartbeat()
gs = GS.BatchGSModule(master, 1, 0)
# Upload the test flight plan
gs.loadWaypoint("../../../Examples/InputData/flightplan.txt")
wpcount = gs.wploader.count()
# Get the waypoints
WP = []
for i in range(wpcount):
WP.append([gs.wploader.wp(i).x, gs.wploader.wp(i).y, gs.wploader.wp(i).z])
origin = [WP[0][0], WP[0][1]]
final = [WP[-1][0], WP[-1][0]]
time.sleep(1)
if not trafficAvailable: