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