def __init__(self,
id,
sock,
stateMap,
d_w=0.005,
d_t=0,
d_l=0,
turning_radius=.003):
self.id = id
self.commandr = MissionCommand.MissionCommand()
self.commandt = MissionCommand.MissionCommand()
self.commandf = MissionCommand.MissionCommand()
self.commands = MissionCommand.MissionCommand()
self.rloc = []
self.sloc = []
self.roam_log = 0
self.refueling = 0
self.searching = 0
self.search_log = 0
self.fuel_sensor = 1
self.target_sensor = 0
self.altitudeType = AltitudeType.AltitudeType.MSL
self.turnType = TurnType.TurnType.TurnShort
self.speedType = SpeedType.SpeedType.Airspeed
self.sock = sock
self.stateMap = stateMap
self.state = 'idle'
self.d_w = d_w
self.d_t = d_t
self.d_l = d_l
self.turning_radius = turning_radius
self.last_wp = None
self.current_wp = None
def spiral_search(self,
location,
extent=.03,
alpha=0.9,
step=10,
step_size=0.003):
if self.state != 'spiral_search' + location.name:
self.state = 'spiral_search' + location.name
heading = self.stateMap.get(self.id).get_Heading()
longtitude = self.stateMap.get(
self.id).get_Location().get_Longitude()
latitude = self.stateMap.get(self.id).get_Location().get_Latitude()
pre_ve = (longtitude, latitude, heading)
p = (location.center_lon, location.center_lat, 0)
paths = search.spiral_search(pre_ve = pre_ve, p = p,d_w = self.d_w, d_l = self.d_l\
, alpha = alpha, extent = extent, step = deg2rad(step), turning_radius = .001\
, step_size = step_size)
self.rloc = []
self.commandr = MissionCommand.MissionCommand()
self.commandr.set_FirstWaypoint(0)
self.commandr.set_VehicleID(self.id)
self.commandr.set_CommandID(0)
for path in paths:
self.rloc.append(Location(path[1], path[0], 0, 0))
nextWaypoint = Waypoint.Waypoint()
for i in range(len(self.rloc)):
nextWaypoint = Waypoint.Waypoint()
nextWaypoint.set_Number(i)
nextWaypoint.set_NextWaypoint(i + 1)
nextWaypoint.set_Speed(20)
nextWaypoint.set_SpeedType(self.speedType)
nextWaypoint.set_ClimbRate(0)
nextWaypoint.set_TurnType(self.turnType)
nextWaypoint.set_Longitude(self.rloc[i].center_lon)
nextWaypoint.set_Altitude(50)
nextWaypoint.set_Latitude(self.rloc[i].center_lat)
nextWaypoint.set_AltitudeType(self.altitudeType)
self.commandr.get_WaypointList().append(nextWaypoint)
self.sock.send(LMCPFactory.packMessage(self.commandr, True))
return i
def line_search(self,
line,
angles=None,
smoothing=10,
step_size=0.5,
b=False):
if self.state != 'line_search':
self.state = 'line_search'
heading = self.stateMap.get(self.id).get_Heading()
longtitude = self.stateMap.get(
self.id).get_Location().get_Longitude()
latitude = self.stateMap.get(self.id).get_Location().get_Latitude()
pre_ve = (longtitude, latitude, heading)
nextWaypoint = Waypoint.Waypoint()
paths = search.line_search(smoothing = smoothing, pre_ve = pre_ve, line = line\
, angles = deg2rad(angles), turning_radius = self.turning_radius, step_size = step_size, b = False)
self.commandr = MissionCommand.MissionCommand()
self.commandr.set_FirstWaypoint(0)
self.commandr.set_VehicleID(self.id)
self.commandr.set_CommandID(0)
self.rloc = []
for path in paths:
self.rloc.append(Location(path[1], path[0], 0, 0))
for i in range(len(self.rloc)):
nextWaypoint = Waypoint.Waypoint()
nextWaypoint.set_Number(i)
nextWaypoint.set_NextWaypoint(i + 1)
nextWaypoint.set_Speed(20)
nextWaypoint.set_SpeedType(self.speedType)
nextWaypoint.set_ClimbRate(0)
nextWaypoint.set_TurnType(self.turnType)
nextWaypoint.set_Longitude(self.rloc[i].center_lon)
nextWaypoint.set_Altitude(50)
nextWaypoint.set_Latitude(self.rloc[i].center_lat)
nextWaypoint.set_AltitudeType(self.altitudeType)
self.commandr.get_WaypointList().append(nextWaypoint)
self.sock.send(LMCPFactory.packMessage(self.commandr, True))
return i
def point_search(self, location, X=0, xc=0, xr=0, s=0, step_size=0.003):
if self.state != 'point_search' or True:
self.state = 'point_search'
heading = self.stateMap.get(self.id).get_Heading()
longtitude = self.stateMap.get(
self.id).get_Location().get_Longitude()
latitude = self.stateMap.get(self.id).get_Location().get_Latitude()
pre_ve = (longtitude, latitude, heading)
p = (location.center_lon, location.center_lat, deg2rad(X))
pre_ve = (pre_ve[0], pre_ve[1],
deg2rad(convert_heading(pre_ve[2])))
paths = search.point_search(pre_ve = pre_ve, p = p,xc=deg2rad(xc),xr=deg2rad(xr),s=s,d_t=self.d_t\
, d_l = self.d_l, turning_radius = self.turning_radius, step_size = step_size)
self.commandr = MissionCommand.MissionCommand()
self.commandr.set_FirstWaypoint(0)
self.commandr.set_VehicleID(self.id)
self.commandr.set_CommandID(0)
self.rloc = []
for path in paths:
self.rloc.append(Location(path[1], path[0], 0, 0))
nextWaypoint = Waypoint.Waypoint()
for i in range(len(self.rloc)):
nextWaypoint = Waypoint.Waypoint()
nextWaypoint.set_Number(i)
nextWaypoint.set_NextWaypoint(i + 1)
nextWaypoint.set_Speed(20)
nextWaypoint.set_SpeedType(self.speedType)
nextWaypoint.set_ClimbRate(0)
nextWaypoint.set_TurnType(self.turnType)
nextWaypoint.set_Longitude(self.rloc[i].center_lon)
nextWaypoint.set_Altitude(50)
nextWaypoint.set_Latitude(self.rloc[i].center_lat)
nextWaypoint.set_AltitudeType(self.altitudeType)
self.commandr.get_WaypointList().append(nextWaypoint)
self.sock.send(LMCPFactory.packMessage(self.commandr, True))
return i
def trackTarget(self, id2):
self.state = 'tracking'
counter = len(self.commandt.get_WaypointList())
if counter % 5 == 0:
self.commandt = MissionCommand.MissionCommand()
nextWaypoint = Waypoint.Waypoint()
nextWaypoint.set_Number(counter)
self.commandt.set_FirstWaypoint(counter)
nextWaypoint.set_NextWaypoint(counter + 1)
nextWaypoint.set_Speed(20)
nextWaypoint.set_SpeedType(self.speedType)
nextWaypoint.set_ClimbRate(0)
nextWaypoint.set_TurnType(self.turnType)
nextWaypoint.set_Longitude(self.getit('longitude', id2))
nextWaypoint.set_Altitude(50)
nextWaypoint.set_Latitude(self.getit('latitude', id2))
nextWaypoint.set_AltitudeType(self.altitudeType)
if len(self.commandt.get_WaypointList()) <= 1:
#self.commandt.WaypointList = []
self.commandt.get_WaypointList().append(nextWaypoint)
self.commandt.set_VehicleID(self.id)
self.commandt.set_CommandID(0)
self.sock.send(LMCPFactory.packMessage(self.commandt, True))
elif len(self.commandt.get_WaypointList()) > 1:
if (abs(self.commandt.get_WaypointList()[-1].get_Latitude() - nextWaypoint.get_Latitude())>0.00001\
or abs(self.commandt.get_WaypointList()[-1].get_Longitude() - nextWaypoint.get_Longitude()) >0.00001)\
and vincenty((self.commandt.get_WaypointList()[-1].get_Latitude(),self.commandt.get_WaypointList()[-1]\
.get_Longitude()), (nextWaypoint.get_Latitude(),nextWaypoint.get_Longitude())).meters > 300:
self.commandt.get_WaypointList().append(nextWaypoint)
self.commandt.set_VehicleID(self.id)
self.commandt.set_CommandID(0)
self.sock.send(LMCPFactory.packMessage(self.commandt, True))
def roam(self, location):
nextWaypoint = Waypoint.Waypoint()
if self.state != 'roaming':
counter = 0
self.commandr = MissionCommand.MissionCommand()
self.roam_log = 0
else:
counter = len(self.commandr.get_WaypointList())
self.state = 'roaming'
if counter % 5 == 0:
self.commandr = MissionCommand.MissionCommand()
if self.roam_log == 0:
self.rloc.append(Location(location.center_lat+float(location.height)/261538.0\
,location.center_lon+float(location.height)/261538.0,0,0))
self.rloc.append(Location(location.center_lat+float(location.height)/261538.0\
,location.center_lon-float(location.height)/261538.0,0,0))
self.rloc.append(Location(location.center_lat-float(location.height)/261538.0\
,location.center_lon+float(location.height)/261538.0,0,0))
self.rloc.append(
Location(location.center_lat, location.center_lon, 0, 0))
self.rloc.append(Location(location.center_lat-float(location.height)/261538.0\
,location.center_lon-float(location.height)/261538.0,0,0))
if self.roam_log == 0:
nextWaypoint = Waypoint.Waypoint()
self.commandr.set_FirstWaypoint(counter)
nextWaypoint.set_Number(counter)
nextWaypoint.set_NextWaypoint(counter + 1)
nextWaypoint.set_Speed(20)
nextWaypoint.set_SpeedType(self.speedType)
nextWaypoint.set_ClimbRate(0)
nextWaypoint.set_TurnType(self.turnType)
nextWaypoint.set_Longitude(self.rloc[self.roam_log % 5].center_lon)
nextWaypoint.set_Altitude(50)
nextWaypoint.set_Latitude(self.rloc[self.roam_log % 5].center_lat)
nextWaypoint.set_AltitudeType(self.altitudeType)
self.commandr.get_WaypointList().append(nextWaypoint)
self.commandr.set_VehicleID(self.id)
self.commandr.set_CommandID(0)
self.sock.send(LMCPFactory.packMessage(self.commandr, True))
self.roam_log += 1
if vincenty((self.getit('latitude',self.id),self.getit('longitude',self.id))\
, (self.rloc[(self.roam_log-1)%5].center_lat,self.rloc[(self.roam_log-1)%5].center_lon)).meters < 50:
nextWaypoint = Waypoint.Waypoint()
self.commandr.set_FirstWaypoint(counter)
nextWaypoint.set_Number(counter)
nextWaypoint.set_NextWaypoint(counter + 1)
nextWaypoint.set_Speed(20)
nextWaypoint.set_SpeedType(self.speedType)
nextWaypoint.set_ClimbRate(0)
nextWaypoint.set_TurnType(self.turnType)
nextWaypoint.set_Longitude(self.rloc[self.roam_log % 5].center_lon)
nextWaypoint.set_Altitude(50)
nextWaypoint.set_Latitude(self.rloc[self.roam_log % 5].center_lat)
nextWaypoint.set_AltitudeType(self.altitudeType)
self.commandr.get_WaypointList().append(nextWaypoint)
self.commandr.set_VehicleID(self.id)
self.commandr.set_CommandID(0)
self.sock.send(LMCPFactory.packMessage(self.commandr, True))
self.roam_log += 1
def loiter(self,
location=None,
pre_ve=None,
angle=0,
alpha=0.9,
p=None,
xc=0,
xr=0,
s=0,
step_size=0.003):
if self.state != 'loiter':
self.state = 'loiter'
nextWaypoint = Waypoint.Waypoint()
heading = self.stateMap.get(self.id).get_Heading()
longtitude = self.stateMap.get(
self.id).get_Location().get_Longitude()
latitude = self.stateMap.get(self.id).get_Location().get_Latitude()
pre_ve = (longtitude, latitude, heading)
poly = [
(location.center_lon - ((9 * (10**(-6)) * location.width) / 2),
location.center_lat - ((9 *
(10**(-6)) * location.height) / 2)),
(location.center_lon - ((9 * (10**(-6)) * location.width) / 2),
location.center_lat + ((9 *
(10**(-6)) * location.height) / 2)),
(location.center_lon + ((9 * (10**(-6)) * location.width) / 2),
location.center_lat - ((9 *
(10**(-6)) * location.height) / 2)),
(location.center_lon + ((9 * (10**(-6)) * location.width) / 2),
location.center_lat + ((9 *
(10**(-6)) * location.height) / 2))
]
paths = search.area_search(angle = deg2rad(angle) ,pre_ve = pre_ve, poly = poly,xc=deg2rad(xc),xr=deg2rad(xr),
s=s,d_t=self.d_t, p = p, d_w = self.d_w, d_l = self.d_l,alpha = alpha\
, turning_radius = self.turning_radius, step_size = step_size)
self.commandr = MissionCommand.MissionCommand()
self.commandr.set_VehicleID(self.id)
self.commandr.set_CommandID(0)
self.commandr.set_FirstWaypoint(0)
self.rloc = []
for path in paths:
self.rloc.append(Location(path[1], path[0], 0, 0))
for i in range(len(self.rloc)):
nextWaypoint = Waypoint.Waypoint()
nextWaypoint.set_Number(i)
if i + 1 == len(self.rloc):
nextWaypoint.set_NextWaypoint(1)
else:
nextWaypoint.set_NextWaypoint(i + 1)
nextWaypoint.set_Speed(20)
nextWaypoint.set_SpeedType(self.speedType)
nextWaypoint.set_ClimbRate(0)
nextWaypoint.set_TurnType(self.turnType)
nextWaypoint.set_Longitude(self.rloc[i].center_lon)
nextWaypoint.set_Altitude(50)
nextWaypoint.set_Latitude(self.rloc[i].center_lat)
nextWaypoint.set_AltitudeType(self.altitudeType)
self.commandr.get_WaypointList().append(nextWaypoint)
self.sock.send(LMCPFactory.packMessage(self.commandr, True))
return i
def getInstance(self, type_):
if (type_ == 1): return AbstractGeometry.AbstractGeometry()
if (type_ == 2): return KeyValuePair.KeyValuePair()
if (type_ == 3): return Location3D.Location3D()
if (type_ == 4): return PayloadAction.PayloadAction()
if (type_ == 5): return PayloadConfiguration.PayloadConfiguration()
if (type_ == 6): return PayloadState.PayloadState()
if (type_ == 7): return VehicleAction.VehicleAction()
if (type_ == 8): return Task.Task()
if (type_ == 9): return SearchTask.SearchTask()
if (type_ == 10): return AbstractZone.AbstractZone()
if (type_ == 11): return EntityConfiguration.EntityConfiguration()
if (type_ == 12): return FlightProfile.FlightProfile()
if (type_ == 13):
return AirVehicleConfiguration.AirVehicleConfiguration()
if (type_ == 14): return EntityState.EntityState()
if (type_ == 15): return AirVehicleState.AirVehicleState()
if (type_ == 16): return Wedge.Wedge()
if (type_ == 17): return AreaSearchTask.AreaSearchTask()
if (type_ == 18): return CameraAction.CameraAction()
if (type_ == 19): return CameraConfiguration.CameraConfiguration()
if (type_ == 20): return GimballedPayloadState.GimballedPayloadState()
if (type_ == 21): return CameraState.CameraState()
if (type_ == 22): return Circle.Circle()
if (type_ == 23): return GimbalAngleAction.GimbalAngleAction()
if (type_ == 24): return GimbalConfiguration.GimbalConfiguration()
if (type_ == 25): return GimbalScanAction.GimbalScanAction()
if (type_ == 26): return GimbalStareAction.GimbalStareAction()
if (type_ == 27): return GimbalState.GimbalState()
if (type_ == 28): return GoToWaypointAction.GoToWaypointAction()
if (type_ == 29): return KeepInZone.KeepInZone()
if (type_ == 30): return KeepOutZone.KeepOutZone()
if (type_ == 31): return LineSearchTask.LineSearchTask()
if (type_ == 32): return NavigationAction.NavigationAction()
if (type_ == 33): return LoiterAction.LoiterAction()
if (type_ == 34): return LoiterTask.LoiterTask()
if (type_ == 35): return Waypoint.Waypoint()
if (type_ == 36): return MissionCommand.MissionCommand()
if (type_ == 37): return MustFlyTask.MustFlyTask()
if (type_ == 38): return OperatorSignal.OperatorSignal()
if (type_ == 39): return OperatingRegion.OperatingRegion()
if (type_ == 40): return AutomationRequest.AutomationRequest()
if (type_ == 41): return PointSearchTask.PointSearchTask()
if (type_ == 42): return Polygon.Polygon()
if (type_ == 43): return Rectangle.Rectangle()
if (type_ == 44): return RemoveTasks.RemoveTasks()
if (type_ == 45): return ServiceStatus.ServiceStatus()
if (type_ == 46): return SessionStatus.SessionStatus()
if (type_ == 47): return VehicleActionCommand.VehicleActionCommand()
if (type_ == 48): return VideoStreamAction.VideoStreamAction()
if (type_ == 49):
return VideoStreamConfiguration.VideoStreamConfiguration()
if (type_ == 50): return VideoStreamState.VideoStreamState()
if (type_ == 51): return AutomationResponse.AutomationResponse()
if (type_ == 52): return RemoveZones.RemoveZones()
if (type_ == 53): return RemoveEntities.RemoveEntities()
if (type_ == 54): return FlightDirectorAction.FlightDirectorAction()
if (type_ == 55): return WeatherReport.WeatherReport()
if (type_ == 56): return FollowPathCommand.FollowPathCommand()
if (type_ == 57): return PathWaypoint.PathWaypoint()
if (type_ == 58): return StopMovementAction.StopMovementAction()
if (type_ == 59): return WaypointTransfer.WaypointTransfer()
if (type_ == 60): return PayloadStowAction.PayloadStowAction()
return None