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()
