def RunOwnship(self):
trkgsvs_command = ConvertVnedToTrkGsVs(*self.controlInput)
self.ownship.InputCommand(*trkgsvs_command)
self.ownship.Run(windFrom=self.windFrom,windSpeed=self.windSpeed)
opos = self.ownship.GetOutputPositionNED()
ovel = self.ownship.GetOutputVelocityNED()
self.localPos = opos
(ogx, ogy) = gps_offset(self.home_pos[0], self.home_pos[1], opos[1], opos[0])
self.position = [ogx, ogy, opos[2]]
self.velocity = ovel
self.trkgsvs = ConvertVnedToTrkGsVs(ovel[0],ovel[1],ovel[2])
self.RecordOwnship()
def RunOwnship(self):
self.controlInput = self.core.GetOutput()
self.ownship.InputCommand(*self.controlInput)
self.ownship.Run(windFrom=self.windFrom, windSpeed=self.windSpeed)
opos = self.ownship.GetOutputPositionNED()
ovel = self.ownship.GetOutputVelocityNED()
self.localPos = opos
(ogx, ogy) = gps_offset(self.home_pos[0], self.home_pos[1], opos[1],
opos[0])
self.position = [ogx, ogy, opos[2]]
self.velocity = ovel
self.trkgsvs = ConvertVnedToTrkGsVs(ovel[0], ovel[1], ovel[2])
self.RecordOwnship()
sigmaPos = [
self.ownship.sigma_pos[0][0], self.ownship.sigma_pos[1][1],
self.ownship.sigma_pos[2][2], self.ownship.sigma_pos[0][1],
self.ownship.sigma_pos[0][2], self.ownship.sigma_pos[1][2]
]
sigmaVel = [
self.ownship.sigma_vel[0][0], self.ownship.sigma_vel[1][1],
self.ownship.sigma_vel[2][2], self.ownship.sigma_vel[0][1],
self.ownship.sigma_vel[0][2], self.ownship.sigma_vel[1][2]
]
self.core.InputOwnshipState(self.currTime, self.position, self.trkgsvs,
sigmaPos, sigmaVel)
def __init__(self,
idx,
home_gps,
x=0.0,
y=0.0,
z=0.0,
vx=0.0,
vy=0.0,
vz=0.0,
dt=0.05):
"""
Initialize a UAM VTOL vehicle simulation
:param x: initial x position (meters east from home_gps)
:param y: initial y position (meters north from home_gps)
:param z: initial altitude (meters)
:param vx: initial east velocity (m/s)
:param vy: initial north velocity (m/s)
:param vz: initial upward velocity (m/s)
Other arguments are defined in VehicleSimInterface
"""
super().__init__(idx, home_gps, dt)
g = 9.8
self.U = np.array([0.0, 0.0, 0.0])
self.pos0 = np.array([x, y, z])
self.vel0 = np.array([vx, vy, vz])
self.pos = np.array([x, y, z])
self.vel = np.array([vx, vy, vz])
self.turnRate = 20
self.accel = 0.5 * g
self.daccel = -0.5 * g
self.vaccel = 0.15 * g
self.trk, self.gs, self.vs = ConvertVnedToTrkGsVs(vy, vx, vz)
def InputTraffic(self,idx,position,velocity):
if idx is not self.vehicleID and 0 not in position:
trkgsvs = ConvertVnedToTrkGsVs(velocity[0],velocity[1],velocity[2])
self.tfMonitor.input_traffic(idx,position,trkgsvs,self.currTime)
self.Trajectory.InputTrafficData(idx,position,trkgsvs,self.currTime)
localPos = self.ConvertToLocalCoordinates(position)
self.RecordTraffic(idx, position, velocity, localPos)
def InputTraffic(self, callsign, position, velocity):
if callsign != self.callsign and np.abs(np.sum(position)) > 0:
trkgsvs = ConvertVnedToTrkGsVs(velocity[0], velocity[1],
velocity[2])
self.core.InputIntruderState(self.currTime, callsign, position,
trkgsvs)
localPos = self.ConvertToLocalCoordinates(position)
self.RecordTraffic(callsign, position, velocity, localPos)
def InputTraffic(self, callsign, position, velocity):
if callsign != self.callsign and 0 not in position:
trkgsvs = ConvertVnedToTrkGsVs(velocity[0], velocity[1],
velocity[2])
self.tfMonitor.input_traffic(callsign, position, trkgsvs,
self.currTime)
self.Trajectory.InputTrafficData(callsign, position, trkgsvs,
self.currTime)
localPos = self.ConvertToLocalCoordinates(position)
self.RecordTraffic(callsign, position, velocity, localPos)
def RunOwnship(self):
self.ownship.inputNED(*self.controlInput)
self.ownship.step(windFrom=self.windFrom,windSpeed=self.windSpeed)
opos = self.ownship.getOutputPositionNED()
ovel = self.ownship.getOutputVelocityNED()
self.localPos = opos
(ogx, ogy) = gps_offset(self.home_pos[0], self.home_pos[1], opos[1], opos[0])
self.position = [ogx, ogy, opos[2]]
self.velocity = ovel
self.trkgsvs = ConvertVnedToTrkGsVs(ovel[0],ovel[1],-ovel[2])
self.trkband = None
self.gsband = None
self.altband = None
self.vsband = None
self.RecordOwnship()
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 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 SetInitialConditions(self, x=0, y=0, z=0, vx=0, vy=0, vz=0):
self.pos0 = np.array([x, y, z])
self.vel0 = np.array([vx, vy, vz])
self.trk, self.gs, self.vs = ConvertVnedToTrkGsVs(vy, vx, vz)
def Run(self):
time_prev = 0
while (self.dist > 10):
time_now = time.time()
if not self.fasttime:
if time_now - time_prev >= 0.05:
time_prev = time_now
else:
continue
self.count += 1
if (self.count > 2000) and (self.dist >= 1000):
print("terminating simulation")
break
U = (0, 0, 0)
self.dist = np.linalg.norm(self.targetPos - self.currentOwnshipPos)
if self.conflict == 0:
U = ComputeControl(self.simSpeed, self.currentOwnshipPos,
self.targetPos)
self.resObtained = False
else:
(ve, vn, vd) = (0, 0, 0)
if self.trackResolution:
#print("executing track resolution")
(ve, vn,
vd) = ConvertTrkGsVsToVned(self.ptrack, self.simSpeed, 0)
elif self.speedResolution:
#print("executing speed resolution")
(ve, vn,
vd) = ConvertTrkGsVsToVned(self.heading2target,
self.pspeed, 0)
elif self.altResolution:
#print("executing alt resolution")
(ve, vn,
vd) = ConvertTrkGsVsToVned(self.heading2target,
self.simSpeed, self.pvs)
U = np.array([ve, vn, vd])
self.ownship.input(U[0], U[1], U[2])
self.ownship.step()
opos = self.ownship.getOutputPosition()
ovel = self.ownship.getOutputVelocity()
self.currentOwnshipPos = (opos[0], opos[1],
opos[2] + self.home_pos[2])
self.currentOwnshipVel = (ovel[0], ovel[1], ovel[2])
self.ownshipPosLog.append(self.currentOwnshipPos)
self.ownshipVelLog.append(self.currentOwnshipVel)
home_pos = self.home_pos
targetPos = self.targetPos
(ogx, ogy) = gps_offset(home_pos[0], home_pos[1],
self.currentOwnshipPos[0],
self.currentOwnshipPos[1])
(wgx, wgy) = gps_offset(home_pos[0], home_pos[1], targetPos[0],
targetPos[1])
ownship_pos_gx = [ogx, ogy, self.currentOwnshipPos[2]]
ownship_vel = [
self.currentOwnshipVel[0], self.currentOwnshipVel[1],
self.currentOwnshipVel[2]
]
ovelTrkGsVs = ConvertVnedToTrkGsVs(*ownship_vel)
self.heading2target = ComputeHeading(ownship_pos_gx, targetPos)
traffic_pos_gx = []
traffic_vel = []
for i in range(len(self.traffic)):
traffic = self.traffic[i]
oldvel = traffic.getOutputVelocity()
traffic.input(oldvel[0], oldvel[1], oldvel[2])
traffic.step()
newpos = traffic.getOutputPosition()
newvel = traffic.getOutputVelocity()
self.trafficPosLog[i].append(newpos)
self.trafficVelLog[i].append(newvel)
(tgx, tgy) = gps_offset(home_pos[0], home_pos[1],
traffic.pos[0], traffic.pos[1])
traffic_pos_gx = [tgx, tgy, traffic.pos[2]]
traffic_vel = [traffic.vel[0], traffic.vel[1], traffic.vel[2]]
self.tfMonitor.input_traffic(i, traffic_pos_gx, traffic_vel,
self.count * 0.05)
self.tfMonitor.monitor_traffic(ownship_pos_gx, ovelTrkGsVs,
self.count * 0.05)
(conflict1, self.ptrack) = self.tfMonitor.GetTrackBands()
(conflict2, self.pspeed) = self.tfMonitor.GetGSBands()
(conflict3, pdown, pup, self.palt) = self.tfMonitor.GetAltBands()
self.conflict = (conflict1 == 1) or (conflict2 == 1) or (conflict3
== 1)
if (self.conflict is True) and not self.resObtained:
self.confcount = 1
print("conflict detected")
self.resObtained = True
# Place holder for to use functions to obtain resolutions
wpFeasibility = False
if self.resObtained:
if self.trackResolution:
if not math.isfinite(self.ptrack) or self.ptrack == -1:
if len(self.preferredTrack) > 0:
self.ptrack = self.preferredTrack[-1]
else:
self.ptrack = -1
currentHeading = ovelTrkGsVs[0]
safe2Turn = self.tfMonitor.check_safe_to_turn(
ownship_pos_gx, ovelTrkGsVs, currentHeading,
self.heading2target)
newOvelTrkGsVs = ovelTrkGsVs
newOvelTrkGsVs = (self.heading2target, ovelTrkGsVs[1],
ovelTrkGsVs[2])
wpFeasibility = self.tfMonitor.monitor_wp_feasibility(
ownship_pos_gx, ovelTrkGsVs, [wgx, wgy, targetPos[2]])
if not wpFeasibility:
self.conflict = 1
if self.ptrack == -1:
self.ptrack = self.preferredTrack[-1]
self.preferredTrack.append(self.ptrack)
elif self.speedResolution:
if not math.isfinite(self.pspeed):
if len(self.preferredSpeed) > 0:
self.pspeed = self.preferredSpeed[-1]
else:
self.pspeed = -100
self.vehicleSpeed.append(ovelTrkGsVs[1])
ovelTrkGsVs = (ovelTrkGsVs[0], self.simSpeed,
ovelTrkGsVs[2])
wpFeasibility = self.tfMonitor.monitor_wp_feasibility(
ownship_pos_gx, ovelTrkGsVs, [wgx, wgy, targetPos[2]])
if not wpFeasibility:
self.conflict = 1
if len(self.preferredSpeed) > 0:
self.pSpeed = self.preferredSpeed[-1]
else:
self.pSpeed = -100
self.preferredSpeed.append(self.pspeed)
if self.pspeed == -100:
self.pspeed = self.simSpeed
elif self.altResolution:
self.palt = pup + 2
if not math.isfinite(self.palt):
if len(self.preferredAlt) > 0:
self.palt = self.preferredAlt[-1]
else:
self.palt = -1000
wpFeasibility = self.tfMonitor.monitor_wp_feasibility(
ownship_pos_gx, ovelTrkGsVs, [wgx, wgy, targetPos[2]])
if not wpFeasibility:
self.conflict = 1
if self.conflict:
diffAlt = self.palt - ownship_pos_gx[2]
self.pvs = 0.1 * diffAlt
self.preferredAlt.append(self.palt)
if self.palt == -1000:
self.palt = 5