def refuel(self, location): if self.state != 'refueling': nextWaypoint = Waypoint.Waypoint() nextWaypoint.set_Number(1) self.commandf.set_FirstWaypoint(1) nextWaypoint.set_NextWaypoint(3) nextWaypoint.set_Speed(20) nextWaypoint.set_SpeedType(self.speedType) nextWaypoint.set_ClimbRate(0) nextWaypoint.set_TurnType(self.turnType) nextWaypoint.set_Longitude(location.center_lon) nextWaypoint.set_Altitude(50) nextWaypoint.set_Latitude(location.center_lat) nextWaypoint.set_AltitudeType(self.altitudeType) self.commandf.get_WaypointList().append(nextWaypoint) self.commandf.set_VehicleID(self.id) self.commandf.set_CommandID(0) self.state = 'refueling' if self.refueling < 1 and vincenty((location.center_lat,location.center_lon),(self.getit('latitude',self.id)\ ,self.getit('longitude',self.id))).meters > location.height: self.sock.send(LMCPFactory.packMessage(self.commandf, True)) self.refueling += 1 if vincenty((location.center_lat,location.center_lon),(self.getit('latitude',self.id),self.getit('longitude'\ ,self.id))).meters < location.height: entity = AirVehicleState.AirVehicleState() entity.set_Location(self.stateMap.get(self.id).get_Location()) entity.set_EnergyAvailable(100) entity.set_ID(self.id) this = LMCPFactory.packMessage(entity, True) self.sock.send(this) self.refueling = 0
def unpack(self, buffer, _pos): """ Unpacks data from a bytearray and sets class members """ _pos = LMCPObject.LMCPObject.unpack(self, buffer, _pos) self.PayloadID = struct.unpack_from(">q", buffer, _pos)[0] _pos += 8 _strlen = struct.unpack_from(">H", buffer, _pos )[0] _pos += 2 if _strlen > 0: self.PayloadKind = struct.unpack_from( repr(_strlen) + "s", buffer, _pos )[0] _pos += _strlen else: self.PayloadKind = "" _arraylen = struct.unpack_from(">H", buffer, _pos )[0] _pos += 2 self.Parameters = [None] * _arraylen for x in range(_arraylen): _valid = struct.unpack_from("B", buffer, _pos )[0] _pos += 1 if _valid: _series = struct.unpack_from(">q", buffer, _pos)[0] _pos += 8 _type = struct.unpack_from(">I", buffer, _pos)[0] _pos += 4 _version = struct.unpack_from(">H", buffer, _pos)[0] _pos += 2 from lmcp import LMCPFactory self.Parameters[x] = LMCPFactory.LMCPFactory().createObject(_series, _version, _type ) _pos = self.Parameters[x].unpack(buffer, _pos) else: self.Parameters[x] = None return _pos
def unpack(self, buffer, _pos): """ Unpacks data from a bytearray and sets class members """ _pos = LMCPObject.LMCPObject.unpack(self, buffer, _pos) self.UniqueTrackingID = struct.unpack_from(">I", buffer, _pos)[0] _pos += 4 _valid = struct.unpack_from("B", buffer, _pos)[0] _pos += 1 if _valid: _series = struct.unpack_from(">q", buffer, _pos)[0] _pos += 8 _type = struct.unpack_from(">I", buffer, _pos)[0] _pos += 4 _version = struct.unpack_from(">H", buffer, _pos)[0] _pos += 2 from lmcp import LMCPFactory self.EstimatedZoneShape = LMCPFactory.LMCPFactory().createObject( _series, _version, _type) _pos = self.EstimatedZoneShape.unpack(buffer, _pos) else: self.EstimatedZoneShape = None self.EstimatedGrowthRate = struct.unpack_from(">f", buffer, _pos)[0] _pos += 4 self.PerceivedZoneType = struct.unpack_from(">i", buffer, _pos)[0] _pos += 4 self.EstimatedZoneDirection = struct.unpack_from(">f", buffer, _pos)[0] _pos += 4 self.EstimatedZoneSpeed = struct.unpack_from(">f", buffer, _pos)[0] _pos += 4 return _pos
def unpack(self, buffer, _pos): """ Unpacks data from a bytearray and sets class members """ _pos = AbstractGeometry.AbstractGeometry.unpack(self, buffer, _pos) _valid = struct.unpack_from("B", buffer, _pos )[0] _pos += 1 if _valid: _series = struct.unpack_from(">q", buffer, _pos)[0] _pos += 8 _type = struct.unpack_from(">I", buffer, _pos)[0] _pos += 4 _version = struct.unpack_from(">H", buffer, _pos)[0] _pos += 2 from lmcp import LMCPFactory self.CenterPoint = LMCPFactory.LMCPFactory().createObject(_series, _version, _type ) _pos = self.CenterPoint.unpack(buffer, _pos) else: self.CenterPoint = None self.Width = struct.unpack_from(">f", buffer, _pos)[0] _pos += 4 self.Height = struct.unpack_from(">f", buffer, _pos)[0] _pos += 4 self.Rotation = struct.unpack_from(">f", buffer, _pos)[0] _pos += 4 return _pos
def unpack(self, buffer, _pos): """ Unpacks data from a bytearray and sets class members """ _pos = NavigationAction.NavigationAction.unpack(self, buffer, _pos) self.LoiterType = struct.unpack_from(">i", buffer, _pos)[0] _pos += 4 self.Radius = struct.unpack_from(">f", buffer, _pos)[0] _pos += 4 self.Axis = struct.unpack_from(">f", buffer, _pos)[0] _pos += 4 self.Length = struct.unpack_from(">f", buffer, _pos)[0] _pos += 4 self.Direction = struct.unpack_from(">i", buffer, _pos)[0] _pos += 4 self.Duration = struct.unpack_from(">q", buffer, _pos)[0] _pos += 8 self.Airspeed = struct.unpack_from(">f", buffer, _pos)[0] _pos += 4 _valid = struct.unpack_from("B", buffer, _pos)[0] _pos += 1 if _valid: _series = struct.unpack_from(">q", buffer, _pos)[0] _pos += 8 _type = struct.unpack_from(">I", buffer, _pos)[0] _pos += 4 _version = struct.unpack_from(">H", buffer, _pos)[0] _pos += 2 from lmcp import LMCPFactory self.Location = LMCPFactory.LMCPFactory().createObject( _series, _version, _type) _pos = self.Location.unpack(buffer, _pos) else: self.Location = None return _pos
def unpack(self, buffer, _pos): """ Unpacks data from a bytearray and sets class members """ _pos = LMCPObject.LMCPObject.unpack(self, buffer, _pos) self.EntityID = struct.unpack_from(">q", buffer, _pos)[0] _pos += 8 _arraylen = struct.unpack_from(">H", buffer, _pos)[0] _pos += 2 self.Waypoints = [None] * _arraylen for x in range(_arraylen): _valid = struct.unpack_from("B", buffer, _pos)[0] _pos += 1 if _valid: _series = struct.unpack_from(">q", buffer, _pos)[0] _pos += 8 _type = struct.unpack_from(">I", buffer, _pos)[0] _pos += 4 _version = struct.unpack_from(">H", buffer, _pos)[0] _pos += 2 from lmcp import LMCPFactory self.Waypoints[x] = LMCPFactory.LMCPFactory().createObject( _series, _version, _type) _pos = self.Waypoints[x].unpack(buffer, _pos) else: self.Waypoints[x] = None self.TransferMode = struct.unpack_from(">i", buffer, _pos)[0] _pos += 4 return _pos
def unpack(self, buffer, _pos): """ Unpacks data from a bytearray and sets class members """ _pos = VehicleActionCommand.VehicleActionCommand.unpack( self, buffer, _pos) self.FirstWaypoint = struct.unpack_from(">q", buffer, _pos)[0] _pos += 8 _arraylen = struct.unpack_from(">H", buffer, _pos)[0] _pos += 2 self.WaypointList = [None] * _arraylen for x in range(_arraylen): _valid = struct.unpack_from("B", buffer, _pos)[0] _pos += 1 if _valid: _series = struct.unpack_from(">q", buffer, _pos)[0] _pos += 8 _type = struct.unpack_from(">I", buffer, _pos)[0] _pos += 4 _version = struct.unpack_from(">H", buffer, _pos)[0] _pos += 2 from lmcp import LMCPFactory self.WaypointList[x] = LMCPFactory.LMCPFactory().createObject( _series, _version, _type) _pos = self.WaypointList[x].unpack(buffer, _pos) else: self.WaypointList[x] = None self.StartTime = struct.unpack_from(">q", buffer, _pos)[0] _pos += 8 self.StopTime = struct.unpack_from(">q", buffer, _pos)[0] _pos += 8 self.RepeatMode = struct.unpack_from(">i", buffer, _pos)[0] _pos += 4 return _pos
def unpack(self, buffer, _pos): """ Unpacks data from a bytearray and sets class members """ _pos = AbstractGeometry.AbstractGeometry.unpack(self, buffer, _pos) _arraylen = struct.unpack_from(">H", buffer, _pos)[0] _pos += 2 self.BoundaryPoints = [None] * _arraylen for x in range(_arraylen): _valid = struct.unpack_from("B", buffer, _pos)[0] _pos += 1 if _valid: _series = struct.unpack_from(">q", buffer, _pos)[0] _pos += 8 _type = struct.unpack_from(">I", buffer, _pos)[0] _pos += 4 _version = struct.unpack_from(">H", buffer, _pos)[0] _pos += 2 from lmcp import LMCPFactory self.BoundaryPoints[x] = LMCPFactory.LMCPFactory( ).createObject(_series, _version, _type) _pos = self.BoundaryPoints[x].unpack(buffer, _pos) else: self.BoundaryPoints[x] = None return _pos
def send_to_amase(obj, socket, client_id): """ Send an LCMP object to AMASE. Keyword arguments: obj -- an LMCP message object socket -- a ZMQ socket connected to AMASE client_id -- the client id for the AMASE socket """ attributes = bytearray(str(obj.FULL_LMCP_TYPE_NAME) + "$lmcp|" + str(obj.FULL_LMCP_TYPE_NAME) + "||0|0$", 'ascii') smsg = LMCPFactory.packMessage(obj, True) sentinel = "+=+=+=+="; sentinel += str(len(attributes) + len(smsg)); sentinel += "#@#@#@#@"; addressedPayload = attributes; addressedPayload.extend(smsg); # sentinelized checksum val = 0; for i in range(0, len(addressedPayload)): val += int(addressedPayload[i] & 0xFF); footer = "!%!%!%!%" + str(val) + "?^?^?^?^"; totalmsg = bytearray(sentinel, 'ascii'); totalmsg.extend(addressedPayload); totalmsg.extend(bytearray(footer, 'ascii')); socket.send(client_id, flags=zmq.SNDMORE, copy=False) socket.send(totalmsg, copy=False) print(" Sent: " + obj.FULL_LMCP_TYPE_NAME)
def unpack(self, buffer, _pos): """ Unpacks data from a bytearray and sets class members """ _pos = LMCPObject.LMCPObject.unpack(self, buffer, _pos) self.State = struct.unpack_from(">i", buffer, _pos)[0] _pos += 4 self.StartTime = struct.unpack_from(">q", buffer, _pos)[0] _pos += 8 self.ScenarioTime = struct.unpack_from(">q", buffer, _pos)[0] _pos += 8 self.RealTimeMultiple = struct.unpack_from(">f", buffer, _pos)[0] _pos += 4 _arraylen = struct.unpack_from(">H", buffer, _pos )[0] _pos += 2 self.Parameters = [None] * _arraylen for x in range(_arraylen): _valid = struct.unpack_from("B", buffer, _pos )[0] _pos += 1 if _valid: _series = struct.unpack_from(">q", buffer, _pos)[0] _pos += 8 _type = struct.unpack_from(">I", buffer, _pos)[0] _pos += 4 _version = struct.unpack_from(">H", buffer, _pos)[0] _pos += 2 from lmcp import LMCPFactory self.Parameters[x] = LMCPFactory.LMCPFactory().createObject(_series, _version, _type ) _pos = self.Parameters[x].unpack(buffer, _pos) else: self.Parameters[x] = None return _pos
def unpack(self, buffer, _pos): """ Unpacks data from a bytearray and sets class members """ _pos = LMCPObject.LMCPObject.unpack(self, buffer, _pos) _valid = struct.unpack_from("B", buffer, _pos)[0] _pos += 1 if _valid: _series = struct.unpack_from(">q", buffer, _pos)[0] _pos += 8 _type = struct.unpack_from(">I", buffer, _pos)[0] _pos += 4 _version = struct.unpack_from(">H", buffer, _pos)[0] _pos += 2 from lmcp import LMCPFactory self.DetectedLocation = LMCPFactory.LMCPFactory().createObject( _series, _version, _type) _pos = self.DetectedLocation.unpack(buffer, _pos) else: self.DetectedLocation = None self.SensorPayloadID = struct.unpack_from(">I", buffer, _pos)[0] _pos += 4 self.DetectingEnitiyID = struct.unpack_from(">I", buffer, _pos)[0] _pos += 4 self.DetectedHazardZoneType = struct.unpack_from(">i", buffer, _pos)[0] _pos += 4 return _pos
def unpack(self, buffer, _pos): """ Unpacks data from a bytearray and sets class members """ _pos = LMCPObject.LMCPObject.unpack(self, buffer, _pos) _valid = struct.unpack_from("B", buffer, _pos)[0] _pos += 1 if _valid: _series = struct.unpack_from(">q", buffer, _pos)[0] _pos += 8 _type = struct.unpack_from(">I", buffer, _pos)[0] _pos += 4 _version = struct.unpack_from(">H", buffer, _pos)[0] _pos += 2 from lmcp import LMCPFactory self.Area = LMCPFactory.LMCPFactory().createObject( _series, _version, _type) _pos = self.Area.unpack(buffer, _pos) else: self.Area = None self.WindSpeed = struct.unpack_from(">f", buffer, _pos)[0] _pos += 4 self.WindDirection = struct.unpack_from(">f", buffer, _pos)[0] _pos += 4 self.Visibility = struct.unpack_from(">f", buffer, _pos)[0] _pos += 4 self.CloudCeiling = struct.unpack_from(">f", buffer, _pos)[0] _pos += 4 self.CloudCoverage = struct.unpack_from(">f", buffer, _pos)[0] _pos += 4 return _pos
def unpack(self, buffer, _pos): """ Unpacks data from a bytearray and sets class members """ _pos = LMCPObject.LMCPObject.unpack(self, buffer, _pos) self.PercentComplete = struct.unpack_from(">f", buffer, _pos)[0] _pos += 4 _arraylen = struct.unpack_from(">H", buffer, _pos)[0] _pos += 2 self.Info = [None] * _arraylen for x in range(_arraylen): _valid = struct.unpack_from("B", buffer, _pos)[0] _pos += 1 if _valid: _series = struct.unpack_from(">q", buffer, _pos)[0] _pos += 8 _type = struct.unpack_from(">I", buffer, _pos)[0] _pos += 4 _version = struct.unpack_from(">H", buffer, _pos)[0] _pos += 2 from lmcp import LMCPFactory self.Info[x] = LMCPFactory.LMCPFactory().createObject( _series, _version, _type) _pos = self.Info[x].unpack(buffer, _pos) else: self.Info[x] = None self.StatusType = struct.unpack_from(">i", buffer, _pos)[0] _pos += 4 return _pos
def unpack(self, buffer, _pos): """ Unpacks data from a bytearray and sets class members """ _pos = SearchTask.SearchTask.unpack(self, buffer, _pos) _arraylen = struct.unpack_from(">H", buffer, _pos)[0] _pos += 2 self.PointList = [None] * _arraylen for x in range(_arraylen): _valid = struct.unpack_from("B", buffer, _pos)[0] _pos += 1 if _valid: _series = struct.unpack_from(">q", buffer, _pos)[0] _pos += 8 _type = struct.unpack_from(">I", buffer, _pos)[0] _pos += 4 _version = struct.unpack_from(">H", buffer, _pos)[0] _pos += 2 from lmcp import LMCPFactory self.PointList[x] = LMCPFactory.LMCPFactory().createObject( _series, _version, _type) _pos = self.PointList[x].unpack(buffer, _pos) else: self.PointList[x] = None _arraylen = struct.unpack_from(">H", buffer, _pos)[0] _pos += 2 self.ViewAngleList = [None] * _arraylen for x in range(_arraylen): _valid = struct.unpack_from("B", buffer, _pos)[0] _pos += 1 if _valid: _series = struct.unpack_from(">q", buffer, _pos)[0] _pos += 8 _type = struct.unpack_from(">I", buffer, _pos)[0] _pos += 4 _version = struct.unpack_from(">H", buffer, _pos)[0] _pos += 2 from lmcp import LMCPFactory self.ViewAngleList[x] = LMCPFactory.LMCPFactory().createObject( _series, _version, _type) _pos = self.ViewAngleList[x].unpack(buffer, _pos) else: self.ViewAngleList[x] = None boolChar = struct.unpack_from(">B", buffer, _pos)[0] self.UseInertialViewAngles = True if boolChar == 1 else False _pos += 1 return _pos
def sendLMCPObject(self, lmcpObj): if isinstance(lmcpObj, LMCPObject.LMCPObject): buf = bytearray() buf.extend(LMCPFactory.packMessage(lmcpObj, True)) self.__socket.send(buf) else: raise ValueError( "Not an LMCP Object. Non-LMCP message to AMASE not supported")
def __init__(self, host, port): super().__init__() self.__socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM) self.__stop_reading = False self.__factory = LMCPFactory.LMCPFactory() self.__recvCallbacks = [] self.__host = host self.__port = port
def unpack(self, buffer, _pos): """ Unpacks data from a bytearray and sets class members """ _pos = GimballedPayloadState.GimballedPayloadState.unpack( self, buffer, _pos) self.HorizontalFieldOfView = struct.unpack_from(">f", buffer, _pos)[0] _pos += 4 self.VerticalFieldOfView = struct.unpack_from(">f", buffer, _pos)[0] _pos += 4 _arraylen = struct.unpack_from(">H", buffer, _pos)[0] _pos += 2 self.Footprint = [None] * _arraylen for x in range(_arraylen): _valid = struct.unpack_from("B", buffer, _pos)[0] _pos += 1 if _valid: _series = struct.unpack_from(">q", buffer, _pos)[0] _pos += 8 _type = struct.unpack_from(">I", buffer, _pos)[0] _pos += 4 _version = struct.unpack_from(">H", buffer, _pos)[0] _pos += 2 from lmcp import LMCPFactory self.Footprint[x] = LMCPFactory.LMCPFactory().createObject( _series, _version, _type) _pos = self.Footprint[x].unpack(buffer, _pos) else: self.Footprint[x] = None _valid = struct.unpack_from("B", buffer, _pos)[0] _pos += 1 if _valid: _series = struct.unpack_from(">q", buffer, _pos)[0] _pos += 8 _type = struct.unpack_from(">I", buffer, _pos)[0] _pos += 4 _version = struct.unpack_from(">H", buffer, _pos)[0] _pos += 2 from lmcp import LMCPFactory self.Centerpoint = LMCPFactory.LMCPFactory().createObject( _series, _version, _type) _pos = self.Centerpoint.unpack(buffer, _pos) else: self.Centerpoint = None return _pos
def track(tracker): factory = LMCPFactory.LMCPFactory() msg_obj = factory.createObjectByName("CMASI", "AutomationRequest") msg_obj.EntityList = [tracker] msg_obj.TaskList = [6] header = str(msg_obj.FULL_LMCP_TYPE_NAME) + "$lmcp|" +\ str(msg_obj.FULL_LMCP_TYPE_NAME) + "||0|0$" smsg = LMCPFactory.packMessage(msg_obj, True) socket_send.send(header + smsg)
def goto(uav, loc): factory = LMCPFactory.LMCPFactory() msg_obj = factory.createObjectByName("CMASI", "AutomationRequest") msg_obj.EntityList = [uav] msg_obj.TaskList = [loc] msg_obj.OperatingRegion = 100 header = str(msg_obj.FULL_LMCP_TYPE_NAME) + "$lmcp|" +\ str(msg_obj.FULL_LMCP_TYPE_NAME) + "||0|0$" smsg = LMCPFactory.packMessage(msg_obj, True) socket_send.send(header + smsg)
def unpack(self, buffer, _pos): """ Unpacks data from a bytearray and sets class members """ _pos = SearchTask.SearchTask.unpack(self, buffer, _pos) _valid = struct.unpack_from("B", buffer, _pos)[0] _pos += 1 if _valid: _series = struct.unpack_from(">q", buffer, _pos)[0] _pos += 8 _type = struct.unpack_from(">I", buffer, _pos)[0] _pos += 4 _version = struct.unpack_from(">H", buffer, _pos)[0] _pos += 2 from lmcp import LMCPFactory self.SearchLocation = LMCPFactory.LMCPFactory().createObject( _series, _version, _type) _pos = self.SearchLocation.unpack(buffer, _pos) else: self.SearchLocation = None self.StandoffDistance = struct.unpack_from(">f", buffer, _pos)[0] _pos += 4 _arraylen = struct.unpack_from(">H", buffer, _pos)[0] _pos += 2 self.ViewAngleList = [None] * _arraylen for x in range(_arraylen): _valid = struct.unpack_from("B", buffer, _pos)[0] _pos += 1 if _valid: _series = struct.unpack_from(">q", buffer, _pos)[0] _pos += 8 _type = struct.unpack_from(">I", buffer, _pos)[0] _pos += 4 _version = struct.unpack_from(">H", buffer, _pos)[0] _pos += 2 from lmcp import LMCPFactory self.ViewAngleList[x] = LMCPFactory.LMCPFactory().createObject( _series, _version, _type) _pos = self.ViewAngleList[x].unpack(buffer, _pos) else: self.ViewAngleList[x] = None return _pos
def unpack(self, buffer, _pos): """ Unpacks data from a bytearray and sets class members """ _pos = LMCPObject.LMCPObject.unpack(self, buffer, _pos) self.ZoneID = struct.unpack_from(">q", buffer, _pos)[0] _pos += 8 self.MinAltitude = struct.unpack_from(">f", buffer, _pos)[0] _pos += 4 self.MinAltitudeType = struct.unpack_from(">i", buffer, _pos)[0] _pos += 4 self.MaxAltitude = struct.unpack_from(">f", buffer, _pos)[0] _pos += 4 self.MaxAltitudeType = struct.unpack_from(">i", buffer, _pos)[0] _pos += 4 _arraylen = struct.unpack_from(">H", buffer, _pos)[0] _pos += 2 self.AffectedAircraft = [None] * _arraylen if _arraylen > 0: self.AffectedAircraft = struct.unpack_from( ">" + repr(_arraylen) + "q", buffer, _pos) _pos += 8 * _arraylen self.StartTime = struct.unpack_from(">q", buffer, _pos)[0] _pos += 8 self.EndTime = struct.unpack_from(">q", buffer, _pos)[0] _pos += 8 self.Padding = struct.unpack_from(">f", buffer, _pos)[0] _pos += 4 _strlen = struct.unpack_from(">H", buffer, _pos)[0] _pos += 2 if _strlen > 0: self.Label = struct.unpack_from(repr(_strlen) + "s", buffer, _pos)[0] _pos += _strlen else: self.Label = "" _valid = struct.unpack_from("B", buffer, _pos)[0] _pos += 1 if _valid: _series = struct.unpack_from(">q", buffer, _pos)[0] _pos += 8 _type = struct.unpack_from(">I", buffer, _pos)[0] _pos += 4 _version = struct.unpack_from(">H", buffer, _pos)[0] _pos += 2 from lmcp import LMCPFactory self.Boundary = LMCPFactory.LMCPFactory().createObject( _series, _version, _type) _pos = self.Boundary.unpack(buffer, _pos) else: self.Boundary = None return _pos
def handle(self): self.factory = LMCPFactory.LMCPFactory() print "Client address: %s" % (self.client_address, ) while True: try: data = [self.request.recv(LMCPFactory.HEADER_SIZE)] print "header size: %d" % (len(data[0]), ) size = LMCPFactory.getSize(data[0]) print "object size: %d" % (size, ) data.append(self.request.recv(size + 4)) # compensate for checksum data_str = "".join(data) print "%d bytes received" % (len(data_str), ) recv_obj = self.factory.getObject(data_str) print "%s received" % recv_obj.__class__ if recv_obj != None: print "Printing object XML..." print recv_obj.toXMLStr("") else: print "Invalid object received." except socket.error, msg: print msg self.stop = True
def __readLMCPDataFromSocket(self): data = bytearray(self.__socket.recv(LMCPFactory.HEADER_SIZE)) if (len(data) >= LMCPFactory.HEADER_SIZE): size = LMCPFactory.getSize(data) data.extend(bytearray( self.__socket.recv(size + 4))) # compensate for checksum recv_obj = self.__factory.getObject(data) if recv_obj != None: return recv_obj else: raise ValueError("Invalid object received.") if (len(data) == 0): return raise ValueError("Data read not enough for an LMCP header")
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 add_nofly_zone(): ################################################################################################################### # Prepare a ZeroMQ context and socket, then connect to 5555. This is the port AMASE uses by convention. # The actual port is set by an XML element in the file Plugins.xml, whose location is specified with the AMASE # option --config <config_directory> when starting up AMASE. This XML element has the form: # <Plugin Class="avtas.amase.network.TcpServer"> # <TCPServer Port="5555"/> # </Plugin> # ----------------------------------------------------------------------------------------------------------------- context = zmq.Context() socket = context.socket(zmq.STREAM) socket.connect("tcp://127.0.0.1:5555") # Store the client ID for this socket client_id, message = socket.recv_multipart() ################################################################################################################### # Initialize an LMCPFactory for creating LMCP messages # ----------------------------------------------------------------------------------------------------------------- factory = LMCPFactory.LMCPFactory() ################################################################################################################### # Add keepout zone from xml # ----------------------------------------------------------------------------------------------------------------- keepout_dom = parse("MessageLibrary/KeepOutZone_10.xml") obj_list = factory.unpackFromXMLNode(keepout_dom) # cc3_obj = obj_list[0] # cc3_obj.set_PayloadID(10003) # gc3_lmcp_dom = parse("MessageLibrary/GimbalConfiguration_ClampedFalse_PayloadID-1001.xml") # obj_list = factory.unpackFromXMLNode(gc3_lmcp_dom) # gc3_obj = obj_list[0] # gc3_obj.set_PayloadID(1003) # gc3_obj.ContainedPayloadList = [10003] # avc3_lmcp_dom = parse("MessageLibrary/AirVehicleConfiguration_FlightProfileAll_LoiterAll_ID-1.xml") # obj_list = factory.unpackFromXMLNode(avc3_lmcp_dom) # avc3_obj = obj_list[0] # avc3_obj.set_ID(3) # avc3_obj.set_Label("UAV3") # avc3_obj.PayloadConfigurationList = [cc3_obj, gc3_obj] send_to_amase(obj_list[0], socket, client_id) msg_time = 0 while msg_time <= 7000: msg_obj = get_from_amase(socket, factory) if msg_obj.FULL_LMCP_TYPE_NAME == 'afrl.cmasi.AirVehicleState': msg_time = msg_obj.get_Time()
def unpack(self, buffer, _pos): """ Unpacks data from a bytearray and sets class members """ _pos = LMCPObject.LMCPObject.unpack(self, buffer, _pos) self.TaskID = struct.unpack_from(">q", buffer, _pos)[0] _pos += 8 _strlen = struct.unpack_from(">H", buffer, _pos)[0] _pos += 2 if _strlen > 0: self.Label = struct.unpack_from(repr(_strlen) + "s", buffer, _pos)[0] _pos += _strlen else: self.Label = "" _arraylen = struct.unpack_from(">H", buffer, _pos)[0] _pos += 2 self.EligibleEntities = [None] * _arraylen if _arraylen > 0: self.EligibleEntities = struct.unpack_from( ">" + repr(_arraylen) + "q", buffer, _pos) _pos += 8 * _arraylen self.RevisitRate = struct.unpack_from(">f", buffer, _pos)[0] _pos += 4 _arraylen = struct.unpack_from(">H", buffer, _pos)[0] _pos += 2 self.Parameters = [None] * _arraylen for x in range(_arraylen): _valid = struct.unpack_from("B", buffer, _pos)[0] _pos += 1 if _valid: _series = struct.unpack_from(">q", buffer, _pos)[0] _pos += 8 _type = struct.unpack_from(">I", buffer, _pos)[0] _pos += 4 _version = struct.unpack_from(">H", buffer, _pos)[0] _pos += 2 from lmcp import LMCPFactory self.Parameters[x] = LMCPFactory.LMCPFactory().createObject( _series, _version, _type) _pos = self.Parameters[x].unpack(buffer, _pos) else: self.Parameters[x] = None self.Priority = struct.unpack_from(">B", buffer, _pos)[0] _pos += 1 boolChar = struct.unpack_from(">B", buffer, _pos)[0] self.Required = True if boolChar == 1 else False _pos += 1 return _pos
def handle(self): self.factory = LMCPFactory.LMCPFactory() print("Client address: %s" % (self.client_address, )) while True: try: data = bytearray(self.request.recv(LMCPFactory.HEADER_SIZE)) if (len(data) >= LMCPFactory.HEADER_SIZE): print("header size: %d" % (len(data), )) size = LMCPFactory.getSize(data) print("object size: %d" % (size, )) data.extend(bytearray( self.request.recv(size + 4))) # compensate for checksum print("%d bytes received" % (len(data), )) recv_obj = self.factory.getObject(data) print("%s received" % recv_obj.__class__) if recv_obj != None: print("Printing object XML...") print(recv_obj.toXMLStr("")) else: print("Invalid object received.") except socket.error: self.stop = True self.request.close()
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 unpack(self, buffer, _pos): """ Unpacks data from a bytearray and sets class members """ _pos = VehicleAction.VehicleAction.unpack(self, buffer, _pos) _valid = struct.unpack_from("B", buffer, _pos)[0] _pos += 1 if _valid: _series = struct.unpack_from(">q", buffer, _pos)[0] _pos += 8 _type = struct.unpack_from(">I", buffer, _pos)[0] _pos += 4 _version = struct.unpack_from(">H", buffer, _pos)[0] _pos += 2 from lmcp import LMCPFactory self.Location = LMCPFactory.LMCPFactory().createObject( _series, _version, _type) _pos = self.Location.unpack(buffer, _pos) else: self.Location = None return _pos
def handle(self): self.factory = LMCPFactory.LMCPFactory() print("Client address: %s" % (self.client_address,)) while True: try: data = bytearray(self.request.recv(LMCPFactory.HEADER_SIZE)) if(len(data) >= LMCPFactory.HEADER_SIZE): print("header size: %d" % (len(data),) ) size = LMCPFactory.getSize(data) print("object size: %d" % (size,)) data.extend(bytearray(self.request.recv(size+4))) # compensate for checksum print("%d bytes received" % (len(data),)) recv_obj = self.factory.getObject(data) print("%s received" % recv_obj.__class__) if recv_obj != None: print("Printing object XML...") print(recv_obj.toXMLStr("")) else: print("Invalid object received.") except socket.error: self.stop = True self.request.close()
def Track(av_state_1, socket_send): # If not defined, create storage in the vehicle's internal state for the vehicle's 'behavior' if 'behavior' not in av_state_1.internal_state: av_state_1.internal_state['behavior'] = 'undefined' # If not already tracking, put in an AutomationRequest to track (Task 3) if av_state_1.internal_state['behavior'].lower() != 'track': av_state_1.internal_state['behavior'] = 'track' # Create AutomationRequest to do Track, which is Task 3 factory = LMCPFactory.LMCPFactory() msg_obj = factory.createObjectByName("CMASI", "AutomationRequest") msg_obj.EntityList = [av_state_1.ID] msg_obj.TaskList = [3] header = bytearray(str(msg_obj.FULL_LMCP_TYPE_NAME) + "$lmcp|" + str(msg_obj.FULL_LMCP_TYPE_NAME) + "||0|0$", 'ascii') smsg = LMCPFactory.packMessage(msg_obj, True) socket_send.send(header + smsg) # Print status print('\nSending ' + msg_obj.FULL_LMCP_TYPE_NAME + ' for Air Vehicle ' + str(av_state_1.ID) + ' to perform Track task (TaskID 3)') print(msg_obj.toXMLStr("")) return
def unpack(self, buffer, _pos): """ Unpacks data from a bytearray and sets class members """ _pos = Task.Task.unpack(self, buffer, _pos) _valid = struct.unpack_from("B", buffer, _pos)[0] _pos += 1 if _valid: _series = struct.unpack_from(">q", buffer, _pos)[0] _pos += 8 _type = struct.unpack_from(">I", buffer, _pos)[0] _pos += 4 _version = struct.unpack_from(">H", buffer, _pos)[0] _pos += 2 from lmcp import LMCPFactory self.Position = LMCPFactory.LMCPFactory().createObject( _series, _version, _type) _pos = self.Position.unpack(buffer, _pos) else: self.Position = None boolChar = struct.unpack_from(">B", buffer, _pos)[0] self.UseAltitude = True if boolChar == 1 else False _pos += 1 return _pos
def Search1(av_state_1, socket_send): # If not defined, create storage in the vehicle's internal state for the vehicle's 'behavior' if 'behavior' not in av_state_1.internal_state: av_state_1.internal_state['behavior'] = 'undefined' # If not already searching area 1, put in an AutomationRequest for a Search task of area 1 (Task 1) if av_state_1.internal_state['behavior'].lower() != 'search1': av_state_1.internal_state['behavior'] = 'search1' # Create AutomationRequest to do Search1, which is Task 1 factory = LMCPFactory.LMCPFactory() msg_obj = factory.createObjectByName("CMASI", "AutomationRequest") msg_obj.EntityList = [av_state_1.ID] msg_obj.TaskList = [1] msg_obj.OperatingRegion = 100 header = bytearray(str(msg_obj.FULL_LMCP_TYPE_NAME) + "$lmcp|" + str(msg_obj.FULL_LMCP_TYPE_NAME) + "||0|0$", 'ascii') smsg = LMCPFactory.packMessage(msg_obj, True) socket_send.send(header + smsg) # Print status print('\nSending ' + msg_obj.FULL_LMCP_TYPE_NAME + ' for Air Vehicle ' + str(av_state_1.ID) + ' to perform Search1 task (TaskID 1)') print(msg_obj.toXMLStr("")) return
from afrl.vehicles import GroundVehicleState from afrl.vehicles import SurfaceVehicleConfiguration from afrl.vehicles import SurfaceVehicleState from afrl.vehicles import StationarySensorConfiguration from afrl.vehicles import StationarySensorState s = socket.socket() host = socket.gethostname() port = 11041 s.connect((host, port)) buf = bytearray() #Pack AbstractGeometry obj = AbstractGeometry.AbstractGeometry() buf.extend(LMCPFactory.packMessage(obj, True)) #Pack KeyValuePair obj = KeyValuePair.KeyValuePair() buf.extend(LMCPFactory.packMessage(obj, True)) #Pack Location3D obj = Location3D.Location3D() buf.extend(LMCPFactory.packMessage(obj, True)) #Pack PayloadAction obj = PayloadAction.PayloadAction() buf.extend(LMCPFactory.packMessage(obj, True)) #Pack PayloadConfiguration obj = PayloadConfiguration.PayloadConfiguration() buf.extend(LMCPFactory.packMessage(obj, True)) #Pack PayloadState obj = PayloadState.PayloadState() buf.extend(LMCPFactory.packMessage(obj, True))
def handle(self,obj): """ This function takes a LMCP.object and does the right thing with it obj can contain ServicePlannerConfiguration or ServicePlannerRequest if it is a Configuration we modify config and load a new region. This type of message will reset the infostate, so one must resend any outstanding requests. This type of message is typically only used once at the beginning. if it is a plan request, we update the infostate and return a plan. This is the normal usage. """ if isinstance(obj,ServicePlannerConfiguration.ServicePlannerConfiguration): #config.enableModification() nameprefix = '_%s__'%obj.__class__.__name__ for k,v in confnames.items(): attrname = nameprefix+v if hasattr(obj,attrname): setattr(config,k,getattr(obj,attrname)) for k,v in obj.__dict__.items(): if (k.startswith('__') and not k.endswith('_') and k not in notconfnames): print k setattr(config,k,v) #config.disableModification() try: region = lmcpio.createRegion(obj.get_Region()) except: region = None if region is None: print ("expected a region in Configuration message\n" "falling back to plannermodule.region") #pdb.set_trace() else: self.region = region self.planner = self.plannerfactory(region) self.infostate = InfoState( self.region, sensor_radius=config.sensor_radius, nagents=config.nagents, locations=None, speed=config.speed) networkregion = lmcpio.createNetwork(self.region) return LMCPFactory.packMessage(networkregion,True) elif isinstance(obj,ServicePlannerRequest.ServicePlannerRequest): if True:#try: lmcplocs = obj.get_AgentLocations() lmcpevent = obj.get_NewEvent() dvrpevent = lmcpio.lmcpEvent2dvrpEvent(lmcpevent) dvrplocs = [lmcpio.lmcpLoc2dvrpLoc(l) for l in lmcplocs] else:#except Exception, err: print err pdb.set_trace() event = dvrpevent self.infostate.update(event,dvrplocs) self.logStatus(event) requests,belief = self.infostate.getRequests(),self.infostate.getBelief() plan = self.planner.plan(belief,requests,dvrplocs) #pdb.set_trace() self.infostate.setPlan(plan) #pdb.set_trace() message = lmcpio.plansToMessage(plan)#,self.infostate.region) return message elif(isinstance(obj,RequestConfiguration.RequestConfiguration)): try: networkregion = lmcpio.createNetwork(self.region) return LMCPFactory.packMessage(networkregion,True) except Exception, err: print err pdb.set_trace()