def InputFlightplan(self, fp, eta=False, repair=False):
waypoints = ConstructWaypointsFromList(fp, eta)
self.etaFP1 = eta
self.localPlans.append(self.GetLocalFlightPlan(waypoints))
self.Cog.InputFlightplanData("Plan0", waypoints, repair, self.turnRate)
self.Guidance.InputFlightplanData("Plan0", waypoints, repair,
self.turnRate)
self.Trajectory.InputFlightplanData("Plan0", waypoints, repair,
self.turnRate)
if repair:
waypoints = self.Guidance.GetKinematicPlan("Plan0")
self.plans.append(waypoints)
self.flightplan1 = waypoints
self.flightplan2 = []
# Set initial conditions of model based on flightplan waypoints
dist = distance(waypoints[1].latitude, waypoints[1].longitude,
waypoints[0].latitude, waypoints[0].longitude)
speed = dist / (waypoints[1].time - waypoints[0].time)
hdg = ComputeHeading(
[waypoints[0].latitude, waypoints[0].longitude, 0],
[waypoints[1].latitude, waypoints[1].longitude, 0])
vn, ve, vd = ConvertTrkGsVsToVned(hdg, speed, 0)
self.ownship.SetInitialConditions(z=waypoints[0].altitude,
vx=ve,
vy=vn)
def InputFlightplan(self, waypoints, eta=False, repair=False):
self.etaFP1 = eta
self.repair = repair
self.localPlans.append(self.GetLocalFlightPlan(waypoints))
self.core.InputMissionFlightPlan(waypoints, repair, eta)
#if repair:
# waypoints = self.Guidance.GetKinematicPlan("Plan0")
self.plans.append(waypoints)
self.flightplan1 = waypoints
self.flightplan2 = []
# Set initial conditions of model based on flightplan waypoints
dist = distance(waypoints[1].latitude, waypoints[1].longitude,
waypoints[0].latitude, waypoints[0].longitude)
speed = dist / (waypoints[1].time - waypoints[0].time)
hdg = ComputeHeading(
[waypoints[0].latitude, waypoints[0].longitude, 0],
[waypoints[1].latitude, waypoints[1].longitude, 0])
vn, ve, vd = ConvertTrkGsVsToVned(hdg, speed, 0)
if not eta:
self.ownship.SetInitialConditions(z=waypoints[0].altitude,
vx=ve,
vy=vn)
def ComputeClosesetPoint(self, wpA, wpB, position):
offset = self.ComputeXtrackDistance(wpA, wpB, position)
dist = distance(*wpA[:2], *wpB[:2])
n = 1 - (dist - offset[1]) / dist
C = [0.0, 0.0, 0.0]
for i in range(3):
C[i] = wpA[i] + n * (wpB[i] - wpA[i])
return C
def ConvertToLocalCoordinates(self,pos):
dh = lambda x, y: abs(distance(self.home_pos[0],self.home_pos[1],x,y))
if pos[1] > self.home_pos[1]:
sgnX = 1
else:
sgnX = -1
if pos[0] > self.home_pos[0]:
sgnY = 1
else:
sgnY = -1
dx = dh(self.home_pos[0],pos[1])
dy = dh(pos[0],self.home_pos[1])
return [dy*sgnY,dx*sgnX,pos[2]]
def InputFlightplan(self, planID, fp, eta=False):
wpTime = 0
for i in range(len(fp)):
if not eta:
if i > 0:
speed = fp[i][3]
prevWP = fp[i - 1][:3]
nextWP = fp[i][:3]
dist = distance(prevWP[0], prevWP[1], nextWP[0], nextWP[1])
wpTime += dist / speed
else:
wpTime = fp[i][3]
wp = Pos(fp[i][0], fp[i][1], fp[i][2])
self.lib.PathPlanner_InputFlightPlan(
self.module, c_char_p(planID.encode('utf-8')), c_int(i), wp,
c_double(wpTime))
def RunCognition(self):
self.Cog.InputVehicleState(self.position,self.trkgsvs,self.trkgsvs[0])
nextWP1 = self.nextWP1
if self.nextWP1 >= len(self.flightplan1):
nextWP1 = len(self.flightplan1) - 1
dist = distance(self.position[0],self.position[1],self.flightplan1[nextWP1][0],self.flightplan1[nextWP1][1])
if self.verbose > 1:
if self.activePlan == 'Plan0':
print("%s: %s, Distance to wp %d: %f" %(self.callsign,self.activePlan,self.nextWP1,dist))
else:
print("%s: %s, Distance to wp %d: %f" %(self.callsign,self.activePlan,self.nextWP2,dist))
self.fphase = self.Cog.RunFlightPhases(self.currTime)
if self.fphase == 8:
self.land = True
n, cmd = self.Cog.GetOutput()
if n > 0:
if cmd.commandType == CommandTypes.FP_CHANGE:
self.guidanceMode = GuidanceMode.FLIGHTPLAN
self.activePlan = cmd.commandU.fpChange.name.decode('utf-8')
nextWP = cmd.commandU.fpChange.wpIndex
if self.activePlan == "Plan0":
self.flightplan2 = []
self.nextWP1 = nextWP
else:
self.nextWP2 = nextWP
self.Guidance.SetGuidanceMode(self.guidanceMode,self.activePlan,nextWP)
if self.verbose > 0:
print(self.callsign, ": active plan = ",self.activePlan)
elif cmd.commandType == CommandTypes.P2P:
self.guidanceMode = GuidanceMode.POINT2POINT
point = cmd.commandU.p2pCommand.point
speed = cmd.commandU.p2pCommand.speed
fp = [(*self.position,0),(*point,speed)]
self.Guidance.InputFlightplanData("P2P",0,fp)
self.activePlan = "P2P"
self.nextWP2 = 1
self.Guidance.SetGuidanceMode(self.guidanceMode,self.activePlan,1)
if self.verbose > 0:
print(self.callsign, ": active plan = ",self.activePlan)
elif cmd.commandType == CommandTypes.VELOCITY:
self.guidanceMode = GuidanceMode.VECTOR
vn = cmd.commandU.velocityCommand.vn
ve = cmd.commandU.velocityCommand.ve
vu = cmd.commandU.velocityCommand.vu
trkGsVs = ConvertVnedToTrkGsVs(vn,ve,vu)
self.Guidance.InputVelocityCmd(trkGsVs)
self.Guidance.SetGuidanceMode(self.guidanceMode,"",0)
elif cmd.commandType == CommandTypes.TAKEOFF:
self.guidanceMode = GuidanceMode.TAKEOFF
elif cmd.commandType == CommandTypes.SPEED_CHANGE:
self.etaFP1 = False
speedChange = cmd.commandU.speedChange.speed
planID= cmd.commandU.speedChange.name.decode('utf-8')
nextWP = self.nextWP1 if planID == "Plan0" else self.nextWP2
self.Guidance.ChangeWaypointSpeed(planID,nextWP,speedChange,False)
elif cmd.commandType == CommandTypes.STATUS_MESSAGE:
if self.verbose > 0:
message = cmd.commandU.statusMessage.buffer
print(self.callsign,":",message.decode('utf-8'))
elif cmd.commandType == CommandTypes.FP_REQUEST:
self.flightplan2 = []
self.numSecPlan += 1
searchType = cmd.commandU.fpRequest.searchType
startPos = cmd.commandU.fpRequest.fromPosition
stopPos = cmd.commandU.fpRequest.toPosition
startVel = cmd.commandU.fpRequest.startVelocity
planID = cmd.commandU.fpRequest.name.decode('utf-8')
numWP = self.Trajectory.FindPath(
searchType,
planID,
startPos,
stopPos,
startVel)
if numWP > 0:
if self.verbose > 0:
print("%s : At %f s, Computed flightplan %s with %d waypoints" % (self.callsign,self.currTime,planID,numWP))
for i in range(numWP):
wp = self.Trajectory.GetWaypoint(planID,i)
self.flightplan2.append([wp[0],wp[1],wp[2],self.resSpeed])
self.Cog.InputFlightplanData(planID,0,self.flightplan2)
self.Guidance.InputFlightplanData(planID,0,self.flightplan2)
self.plans.append(self.flightplan2)
self.localPlans.append(self.GetLocalFlightPlan(self.flightplan2))
else:
if self.verbose > 0:
print(self.callsign,"Error finding path")
def RunSimulationOptimal(self,scenario, plans, path_x, path_y, delta_d,processing_time, planner, dimension, ims, axis, time_res, speed_aircraft):
""" Run simulation with optimal planning until mission complete or time limit reached """
simComplete = False
if self.mergeFixFile is not None:
for ic in self.icInstances:
ic.InputMergeFixes(self.mergeFixFile)
if self.fasttime:
t0 = 0
else:
t0 = time.time()
self.current_time = t0
min_dist = distance(scenario.start_real[0],scenario.start_real[1],scenario.start_real[0]+scenario.lat_range*delta_d,scenario.start_real[1]+scenario.lon_range*delta_d)
min_time = min_dist/(speed_aircraft*2)
time_ompl = min_time
cont = 0
run = True
while not simComplete:
status = False
# Advance time
duration = self.current_time - t0
if self.fasttime:
self.count += 1
self.current_time += self.dT
self.RunSimulatedTraffic()
if self.verbose > 0:
print("Sim Duration: %.1fs" % (duration), end="\r")
else:
time_now = time.time()
if time_now - self.current_time >= self.dT:
dT = time_now - self.current_time
self.current_time = time_now
self.count += 1
self.RunSimulatedTraffic(dT=dT)
if self.verbose > 0:
print("Sim Duration: %.1fs" % (duration), end="\r")
self.windFrom, self.windSpeed = self.GetWind()
# Update Icarous instances
for i, ic in enumerate(self.icInstances):
ic.InputWind(self.windFrom, self.windSpeed)
#Time to the last point
if round(self.current_time*1.05,1) == round(ic.localPlans[0][-1][0],1):
tried = 0
max_try = 0
while tried <= max_try:
plan_aux = []
cost_aut = []
if tried <= max_try:
#Linear algegra to return the next point in a line
p1 = Vector2(plans[-1].solutionSampled[0][0][0], plans[-1].solutionSampled[0][1][0])
p2 = Vector2(plans[-1].solutionSampled[0][0][1], plans[-1].solutionSampled[0][1][1])
vector = p2-p1
vector = vector.normalize()
next_point = p1 + vector*delta_d
else:
p1 = Vector2(plans[-1].solutionSampled[0][0][1], plans[-1].solutionSampled[0][1][1])
p2 = Vector2(plans[-1].solutionSampled[0][0][2], plans[-1].solutionSampled[0][1][2])
vector = p2-p1
vector = vector.normalize()
next_point = p1 + vector*delta_d
for idx, alg in enumerate(['RRTstar']):
costmap = {'pop':scenario.mapgen.z_pop_time[round(cont)],'met':scenario.mapgen.z_met_time[round(cont)]}
plan_aux.append(OptimalPlanning((next_point[0],next_point[1]), scenario.goal, scenario.region, scenario.obstacle, planner, dimension, costmap))
result = plan_aux[idx].plan(processing_time+tried, alg, 'WeightedLengthAndClearanceCombo',delta_d)
if plan_aux[idx].solution != []:
cost_aut.append(plan_aux[idx].solution[0][3])
else:
cost_aut.append(np.inf)
lower_cost = cost_aut.index(min(cost_aut))
#Se existir solução
if plan_aux[lower_cost].solution != []:
#Se o ultimo costmap é do mesmo periodo que o atual
if time_ompl == time_ompl:
#Tentar encontrar um valor melhor que o ultimo
if plan_aux[lower_cost].solution[0][3] < plans[-1].solution[0][3] * 1.05 and (plan_aux[lower_cost].solution != plans[-1].solution):
plans.append(plan_aux[lower_cost])
PlanningStatus(scenario,plans)
print('Add, Better Solution: ' + str(time_ompl) + " : " + str(cont) + " : " + str(min_time) + " Pnt: " + str(next_point[0])+','+str(next_point[1]))
path_x.append(plans[-1].solution[0][0][0])
path_y.append(plans[-1].solution[0][1][0])
time_res.append(int(round(cont)))
ims.append(plotResult(plans[-1],axis, scenario, path_x, path_y, round(cont)))
tried = 0
pnt = 1
break
elif tried >= max_try:
plans.append(plan_aux[lower_cost])
PlanningStatus(scenario,plans)
print('Add, tried Solution: ' + str(time_ompl) + " : " + str(cont) + " : " + str(min_time) + " Pnt: " + str(next_point[0])+','+str(next_point[1]))
path_x.append(plans[-1].solution[0][0][0])
path_y.append(plans[-1].solution[0][1][0])
time_res.append(int(round(cont)))
ims.append(plotResult(plans[-1],axis, scenario, path_x, path_y, round(cont)))
tried = 0
pnt = 1
break
else:
print('Tried: '+ str(tried) + " - (" + str(next_point[0])+','+str(next_point[1])+")" )
if tried >= max_try :
pnt = pnt +1
tried = tried + 1
else:
plans.append(plan_aux[lower_cost])
PlanningStatus(scenario,plans)
print('Add, Other time: ' + str(time_ompl) + " : " + str(cont) + " : " + str(min_time)+ " Pnt: " + str(next_point[0])+','+str(next_point[1]))
path_x.append(plans[-1].solution[0][0][0])
path_y.append(plans[-1].solution[0][1][0])
time_res.append(int(round(cont)))
ims.append(plotResult(plans[-1],axis, scenario, path_x, path_y, round(cont)))
else:
print('Pop Solution - Time: ' + str(round(cont)) + " : " + str(cont))
plans.pop()
continue
goal_sampled = (round(scenario.goal[0]*(scenario.mapgen.z.shape[0]-1))*delta_d, round(scenario.goal[1]*(scenario.mapgen.z.shape[0]-1))*delta_d)
if (plans[-1].solutionSampled[0][0][0], plans[-1].solutionSampled[0][1][0]) == goal_sampled:
path_x.append(plans[-1].solution[0][0][0])
path_y.append(plans[-1].solution[0][1][0])
time_res.append(int(round(cont)))
path_x.append(scenario.goal[0])
path_y.append(scenario.goal[1])
time_res.append(int(round(cont)))
run = False
break
#Tempo para reprocessar a rota
time_ompl = time_ompl + min_time
cont = cont + 0.1
if cont >= len(scenario.mapgen.z_time)-1:
cont = len(scenario.mapgen.z_time)-1
fp_start = []
#First Calculeted Waypoint
p1 = Vector2(plans[-1].solutionSampled[0][0][0], plans[-1].solutionSampled[0][1][0])
p2 = Vector2(plans[-1].solutionSampled[0][0][1], plans[-1].solutionSampled[0][1][1])
vector = p2-p1
vector = vector.normalize()
next_point = p1 + vector*delta_d
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]])
#Input flightplan
#Use the first plan of OMPL
if run:
ic.InputFlightplan(fp_start[:1],eta=False,repair=False)
if ic.CheckMissionComplete():
ic.Terminate()
# Send mission start command
if not ic.missionStarted and duration >= self.icStartDelay[i]:
ic.StartMission()
if self.verbose > 0:
print("%s : Start command sent at %f" %
(ic.callsign, self.current_time))
# Run Icarous
status |= ic.Run()
# Check if time limit has been met
if ic.missionStarted and not ic.missionComplete:
if duration >= self.icTimeLimit[i]:
ic.missionComplete = True
ic.Terminate()
if self.verbose > 0:
print("%s : Time limit reached at %f" %
(ic.callsign, self.current_time))
if self.time_limit is not None and duration >= self.time_limit:
for ic in self.icInstances:
ic.missionComplete = True
ic.Terminate()
if self.verbose > 0:
print("Time limit reached at %f" % self.current_time)
# Exchange all V2V data between vehicles in the environment
self.ExchangeV2VData()
simComplete = all(ic.missionComplete for ic in self.icInstances)
self.ConvertLogsToLocalCoordinates()
def RunCognition(self):
self.Cog.InputVehicleState(self.position,self.trkgsvs,self.trkgsvs[0])
nextWP1 = self.nextWP1
if self.nextWP1 >= len(self.flightplan1):
nextWP1 = len(self.flightplan1) - 1
dist = distance(self.position[0],self.position[1],self.flightplan1[nextWP1].latitude,self.flightplan1[nextWP1].longitude)
if self.verbose > 1:
if self.activePlan == 'Plan0':
print("%s: %s, Distance to wp %d: %f" %(self.callsign,self.activePlan,self.nextWP1,dist))
else:
print("%s: %s, Distance to wp %d: %f" %(self.callsign,self.activePlan,self.nextWP2,dist))
self.fphase = self.Cog.RunCognition(self.currTime)
if self.fphase == -2:
self.land = True
n, cmd = self.Cog.GetOutput()
if n > 0:
if cmd.commandType == CommandTypes.FP_CHANGE:
self.guidanceMode = GuidanceMode.FLIGHTPLAN
self.activePlan = cmd.commandU.fpChange.name.decode('utf-8')
nextWP = cmd.commandU.fpChange.wpIndex
eta = False
if self.activePlan == "Plan0":
self.flightplan2 = []
self.nextWP1 = nextWP
eta = self.etaFP1
else:
self.nextWP2 = nextWP
eta = self.etaFP2
self.Guidance.SetGuidanceMode(self.guidanceMode,self.activePlan,nextWP,eta)
if self.verbose > 0:
print(self.callsign, ": active plan = ",self.activePlan)
elif cmd.commandType == CommandTypes.P2P:
self.guidanceMode = GuidanceMode.POINT2POINT
point = cmd.commandU.p2pCommand.point
speed = cmd.commandU.p2pCommand.speed
fp = [(*self.position,0),(*point,speed)]
waypoints = ConstructWaypointsFromList(fp)
self.Guidance.InputFlightplanData("P2P",waypoints)
self.activePlan = "P2P"
self.nextWP2 = 1
self.Guidance.SetGuidanceMode(self.guidanceMode,self.activePlan,1,False)
if self.verbose > 0:
print(self.callsign, ": active plan = ",self.activePlan)
elif cmd.commandType == CommandTypes.VELOCITY:
self.guidanceMode = GuidanceMode.VECTOR
vn = cmd.commandU.velocityCommand.vn
ve = cmd.commandU.velocityCommand.ve
vu = -cmd.commandU.velocityCommand.vu
trkGsVs = ConvertVnedToTrkGsVs(vn,ve,vu)
self.Guidance.InputVelocityCmd(trkGsVs)
self.Guidance.SetGuidanceMode(self.guidanceMode,"",0)
elif cmd.commandType == CommandTypes.TAKEOFF:
self.guidanceMode = GuidanceMode.TAKEOFF
elif cmd.commandType == CommandTypes.SPEED_CHANGE:
self.etaFP1 = False
speedChange = cmd.commandU.speedChange.speed
planID= cmd.commandU.speedChange.name.decode('utf-8')
nextWP = self.nextWP1 if planID == "Plan0" else self.nextWP2
self.Guidance.ChangeWaypointSpeed(planID,nextWP,speedChange,False)
elif cmd.commandType == CommandTypes.ALT_CHANGE:
altChange = cmd.commandU.altChange.altitude
planID= cmd.commandU.altChange.name.decode('utf-8')
nextWP = self.nextWP1 if planID == "Plan0" else self.nextWP2
self.Guidance.ChangeWaypointAlt(planID,nextWP,altChange,cmd.commandU.altChange.hold)
elif cmd.commandType == CommandTypes.STATUS_MESSAGE:
if self.verbose > 0:
message = cmd.commandU.statusMessage.buffer
print(self.callsign,":",message.decode('utf-8'))
elif cmd.commandType == CommandTypes.FP_REQUEST:
self.flightplan2 = []
self.numSecPlan += 1
startPos = cmd.commandU.fpRequest.fromPosition
stopPos = cmd.commandU.fpRequest.toPosition
startVel = cmd.commandU.fpRequest.startVelocity
endVel = cmd.commandU.fpRequest.endVelocity
planID = cmd.commandU.fpRequest.name.decode('utf-8')
# Find the new path
numWP = self.Trajectory.FindPath(
planID,
startPos,
stopPos,
startVel,
endVel)
if numWP > 0:
if self.verbose > 0:
print("%s : At %f s, Computed flightplan %s with %d waypoints" % (self.callsign,self.currTime,planID,numWP))
# offset the new path with current time (because new paths start with t=0)
self.Trajectory.SetPlanOffset(planID,0,self.currTime)
# Combine new plan with rest of old plan
self.Trajectory.CombinePlan(planID,"Plan0",-1)
self.flightplan2 = self.Trajectory.GetFlightPlan(planID)
self.Cog.InputFlightplanData(planID,self.flightplan2)
self.Guidance.InputFlightplanData(planID,self.flightplan2)
# Set guidance mode with new flightplan and wp 0 to offset plan times
self.Guidance.SetGuidanceMode(GuidanceMode.FLIGHTPLAN,planID,0,False)
self.plans.append(self.flightplan2)
self.localPlans.append(self.GetLocalFlightPlan(self.flightplan2))
else:
if self.verbose > 0:
print(self.callsign,"Error finding path")
def __init__(self,
initialPos,
vehicleSpeed,
targetPosLLA=None,
targetPosNED=None,
simtype="UAS_ROTOR",
fasttime=True):
"""
@initialPos (lat,lon) tuple containing starting
latitude, longitude of simulation
@targetPos (rangeN,rangeE,rangeD) tuple containing
N,E,D range to target position
"""
self.fasttime = fasttime
self.home_pos = [initialPos[0], initialPos[1], initialPos[2]]
self.traffic = []
if simtype == "UAM_VTOL":
self.ownship = VehicleSim(0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
else:
self.ownship = QuadSim()
if targetPosNED is not None:
self.targetPos = np.array(
[targetPosNED[1], targetPosNED[0], targetPosNED[2]])
else:
targetN = distance(self.home_pos[0], self.home_pos[1],
targetPosLLA[0], self.home_pos[1])
targetE = distance(self.home_pos[0], self.home_pos[1],
self.home_pos[0], targetPosLLA[1])
if (targetPosLLA[0] > self.home_pos[0]):
signN = 1
else:
signN = -1
if (targetPosLLA[1] > self.home_pos[1]):
signE = 1
else:
signE = -1
self.targetPos = np.array(
[signE * targetE, signN * targetN, targetPosLLA[2]])
self.simSpeed = vehicleSpeed
self.currentOwnshipPos = (0, 0, initialPos[2])
self.currentOwnshipVel = (0, 0, 0)
self.ownshipPosLog = []
self.ownshipVelLog = []
self.trafficPosLog = []
self.trafficVelLog = []
self.tfMonitor = TrafficMonitor(0)
self.preferredTrack = []
self.preferredSpeed = []
self.preferredAlt = []
self.dist = 200
self.count = 0
self.ptrack = 0
self.pspeed = 0
self.palt = 0
self.pvs = 0
self.conflict = 0
self.cleared = False
self.heading2target = 0
self.trackResolution = True
self.speedResolution = False
self.altResolution = False
self.resObtained = False
self.resType = 0
self.relpos = []
self.relvel = []
self.confcount = 0
