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 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 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 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 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 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 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 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 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
#print("type(obj): %r" % type(obj2))
#print("obj unpacked from dict: '%r'\n\n" % obj2)
print("JSON conversion successful? %r" % (d == d2, ))
if not (obj == obj2):
#print("obj = '%s'" % obj.toString())
#print("obj2 = '%s'" % obj2.toString())
dd1 = obj.toDict()
dd2 = obj2.toDict()
# exporting back to dicts, to see whether the DATA really is different (not just listed as diff b/c diff subobjects)
print("Dictionary conversion successful? %r" % (dd1 == dd2, ))
if not (dd1 == dd2):
print(
"obj2 not same as original obj and dicts show different data, too (yikes!!)"
)
print("dict obj = '%s'" % str(dd1))
print("dict obj2 = '%s'" % str(dd2))
# syntax to create an object from scratch
obj = factory.createObjectByName("CMASI", "KeyValuePair")
obj.set_Key("Hello")
obj.set_Value("World")
# syntax to send back to UxAS
smsg = bytearray(
str(obj.FULL_LMCP_TYPE_NAME) + "$lmcp|" +
str(obj.FULL_LMCP_TYPE_NAME) + "||0|0$", 'ascii')
smsg.extend(LMCPFactory.packMessage(obj, True))
socket_send.send(smsg)
print(" Sent: " + obj.FULL_LMCP_TYPE_NAME)
def searchA(self, location):
self.state = 'searching'
counter = len(self.commands.get_WaypointList())
if self.searching == 0:
self.sloc.append(
Location(location.center_lat, location.center_lon, 0, 0))
self.sloc.append(
Location(location.center_lat + 0.01,
location.center_lon + 0.0155, 0, 0))
self.sloc.append(
Location(location.center_lat - 0.01,
location.center_lon + 0.0155, 0, 0))
self.sloc.append(
Location(location.center_lat + 0.01,
location.center_lon - 0.0155, 0, 0))
self.sloc.append(
Location(location.center_lat - 0.01,
location.center_lon - 0.0155, 0, 0))
nextWaypoint = Waypoint.Waypoint()
self.commands.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.sloc[self.search_log %
4].center_lon)
nextWaypoint.set_Altitude(50)
nextWaypoint.set_Latitude(self.sloc[self.search_log %
4].center_lat)
nextWaypoint.set_AltitudeType(self.altitudeType)
self.commands.get_WaypointList().append(nextWaypoint)
self.commands.set_VehicleID(self.id)
self.commands.set_CommandID(0)
self.sock.send(LMCPFactory.packMessage(self.commands, True))
self.search_log += 1
if vincenty((self.getit('latitude',self.id),self.getit('longitude',self.id)),\
(self.sloc[self.search_log-1%4].center_lat,self.sloc[self.search_log-1%4].center_lon)).meters < 50:
nextWaypoint = Waypoint.Waypoint()
self.commands.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.sloc[self.search_log %
4].center_lon)
nextWaypoint.set_Altitude(50)
nextWaypoint.set_Latitude(self.sloc[self.search_log %
4].center_lat)
nextWaypoint.set_AltitudeType(self.altitudeType)
self.commands.get_WaypointList().append(nextWaypoint)
self.commands.set_VehicleID(self.id)
self.commands.set_CommandID(0)
self.sock.send(LMCPFactory.packMessage(self.commands, True))
self.search_log += 1
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()
def sendObj(obj):
header = str(obj.FULL_LMCP_TYPE_NAME) + '$lmcp|' + str(obj.FULL_LMCP_TYPE_NAME) + '||0|0$'
msg = LMCPFactory.packMessage(obj, True)
socket_send.send(header + msg)
print("dict loaded from json: '%r'\n" % d2)
print("type(obj): %r" % type(obj2))
print("obj unpacked from dict: '%r'\n\n" % obj2)
print("d2 same as original d? %r" % (d == d2, ))
if not (obj == obj2):
#print("obj = '%s'" % obj.toString())
#print("obj2 = '%s'" % obj2.toString())
dd1 = obj.toDict()
dd2 = obj2.toDict()
# exporting back to dicts, to see whether the DATA really is different (not just listed as diff b/c diff subobjects)
print("dicts-from-class-objects the same? %r" % (dd1 == dd2, ))
if not (dd1 == dd2):
print(
"obj2 not same as original obj and dicts show different data, too (yikes!!)"
)
print("dict obj = '%s'" % str(dd1))
print("dict obj2 = '%s'" % str(dd2))
# syntax to create an object from scratch
obj = factory.createObjectByName("CMASI", "KeyValuePair")
obj.set_Key("Hello")
obj.set_Value("World")
# syntax to send back to UxAS
header = str(obj.FULL_LMCP_TYPE_NAME) + "$lmcp|" + str(
obj.FULL_LMCP_TYPE_NAME) + "||0|0$"
smsg = LMCPFactory.packMessage(obj, True)
socket_send.send(header + smsg)
print(" Sent: " + obj.FULL_LMCP_TYPE_NAME)
