def resolveLocation(self, curPos, maxDistance=None):
dao = Locations()
try:
locations = dao.findLocations()
except:
return ('', curPos)
if len(locations) == 0:
return ('', curPos)
name = None
diff = None
for loc in locations:
dist = 0
dist += math.pow(curPos[0] - loc['xCoord'], 2)
dist += math.pow(curPos[1] - loc['yCoord'], 2)
#We're not worried about orientation for location matching
#use a fraction of the orientation in case we get two named
# locations with the same x/y and different orientations
# needs to be a very small fraction since rotation uses a different unit measurement
# than location does (degrees vs. meters)
dist += math.pow(curPos[2] - loc['orientation'], 2) / 100000
dist = math.sqrt(dist)
if name == None or dist < diff:
name = loc['name']
diff = dist
if maxDistance == None or diff <= maxDistance:
return (name, curPos)
else:
return ('', curPos)
def resolveLocation(self, curPos, maxDistance=None):
dao = Locations()
try:
locations = dao.findLocations()
except:
return ('', curPos)
if len(locations) == 0:
return ('', curPos)
name = None
diff = None
for loc in locations:
dist = 0
dist += math.pow(curPos[0] - loc['xCoord'], 2)
dist += math.pow(curPos[1] - loc['yCoord'], 2)
#We're not worried about orientation for location matching
#use a fraction of the orientation in case we get two named
# locations with the same x/y and different orientations
# needs to be a very small fraction since rotation uses a different unit measurement
# than location does (degrees vs. meters)
dist += math.pow(curPos[2] - loc['orientation'], 2) / 100000
dist = math.sqrt(dist)
if name == None or dist < diff:
name = loc['name']
diff = dist
if maxDistance == None or diff <= maxDistance:
return (name, curPos)
else:
return ('', curPos)
def _getCurrentLoc(self):
locs = self._ros.getSingleMessage(self._topic)
if locs == None:
print "No message recieved from %s" % self._topic
return ('', (None, None, None))
loc = None
for human in locs.trackedHumans:
if human.specialFlag == 1:
loc = human
break
if loc == None:
print "No human returned from location tracker"
return ('', (None, None, None))
#tm = self._transformMatrix
#loc.header.frame_id is 'usually' /camera_frame
(x, y, _) = self._transform.transformPoint(loc.location, toTopic='/map', fromTopic=loc.location.header.frame_id)
if x == None or y == None:
print "Error getting transform"
return ('', (None, None, None))
#else:
# angle = 0 #TODO: UVA Orientation
# print "Using human location from id: %s" % loc.id
pos = (round(x, 3), round(y, 3), 0)
return Locations.resolveLocation(pos)
def __init__(self):
super(ROSLocationProcessor, self).__init__()
self._lastSensors = {}
self._lastLocations = {}
self._sensors = Sensors()
self._locations = Locations()
self._ros = ROS()
def _getCurrentLoc(self):
locs = self._ros.getSingleMessage(self._topic)
if locs == None:
print "No message recieved from %s" % self._topic
return ('', (None, None, None))
loc = None
for human in locs.trackedHumans:
if human.specialFlag == 1:
loc = human
break
if loc == None:
print "No human returned from location tracker"
return ('', (None, None, None))
#tm = self._transformMatrix
#loc.header.frame_id is 'usually' /camera_frame
(x, y, _) = self._transform.transformPoint(loc.location, toTopic='/map', fromTopic=loc.location.header.frame_id)
if x == None or y == None:
print "Error getting transform"
return ('', (None, None, None))
#else:
# angle = 0 #TODO: UVA Orientation
# print "Using human location from id: %s" % loc.id
pos = (round(x, 3), round(y, 3), 0)
return Locations.resolveLocation(pos)
def _getCurrentLoc(self):
locs = self._ros.getSingleMessage(self._topic)
if len(locs.trackedHumans) > 0:
loc = locs.trackedHumans[0]
else:
return ('', (None, None, None))
x = loc.location.point.x + self._transformMatrix[0]
y = loc.location.point.y + self._transformMatrix[1]
angle = 0 + self._transformMatrix[2]
print "Using human location from id: %s" % loc.id
pos = (round(x, 3), round(y, 3), angle)
return Locations.resolveLocation(pos)
def getCurrentRobot():
""" Retrieves the current Robot type from the database, given the active experiment location from the """
""" session control table and constructs the appropriate class """
activeLocation = Locations().getActiveExperimentLocation()
if activeLocation == None:
print "No experiment location set"
elif activeLocation['activeRobot'] != None and activeLocation['activeRobot'] != '':
robotData = Robots().getRobot(activeLocation['activeRobot'])
if robotData != None and robotData['robotName'] != '':
return Factory.getRobot(robotData['robotName'])
else:
print >> sys.stderr, "No data retrieved for robot ID %s" % activeLocation['activeRobot']
else:
print >> sys.stderr, "No robots set for experiment location %s" % activeLocation['location']
return None
def _getCurrentLoc(self):
locs = self._ros.getSingleMessage(self._topic)
if locs == None:
print "No message recieved from %s" % self._topic
return ('', (None, None, None))
loc = None
for human in locs.trackedHumans:
if human.specialFlag == 1:
loc = human
break
if loc == None:
print "No human returned from location tracker"
return ('', (None, None, None))
x = loc.location.point.x + self._transformMatrix[0]
y = loc.location.point.y + self._transformMatrix[1]
angle = 0 + self._transformMatrix[2]
print "Using human location from id: %s" % loc.id
pos = (round(x, 3), round(y, 3), angle)
return Locations.resolveLocation(pos)
(_, _, orientation) = self._tf.transformations.euler_from_quaternion(heading)
return (xyPos, orientation)
except (self._tf.LookupException, self._tf.ConnectivityException, self._tf.ExtrapolationException) as e:
print >> sys.stderr, "Error while looking up transform: " + str(e)
return ((None, None, None), None)
else:
# this takes significantly less processing time, but requires ipa_navigation
poseMsg = self._ros.getSingleMessage('/base_pose')
if poseMsg == None:
print >> sys.stderr, "No message recieved from /base_pose"
return ((None, None, None), None)
pose = poseMsg.pose
xyPos = (pose.position.x, pose.position.y, pose.position.z)
(_, _, orientation) = self._tf.transformations.euler_from_quaternion((pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w))
return (xyPos, orientation)
if __name__ == '__main__':
ROS.configureROS(version='electric', packagePath=None, packageName=None, rosMaster='http://sf1-1-pc1:11311', overlayPath=None)
r = ROS()
t = Transform(r)
from Data.dataAccess import Locations
from Data.dataAccess import DataAccess
dao = DataAccess()
while True:
loc = t.getRobotPose()
print Locations.resolveLocation(loc, None, dao)
time.sleep(0.25)
self._channels[row['ID']] = {
'id': _id,
'room': _device,
'channel': _name,
'value': _value,
'status': _status
}
if __name__ == '__main__':
""" Run sensor updaters for all sensor types configured in the locations_config for the currently """
""" active experimental location """
import config
from history import SensorLog
activeLocation = Locations().getActiveExperimentLocation()
if activeLocation == None:
print "Unable to determine active experiment Location"
exit
sensorPollers = []
dataUpdaters = []
for sensorType in config.locations_config[
activeLocation['location']]['sensors']:
sensor = None
if sensorType == 'ZWaveHomeController':
sensor = ZWaveHomeController(config.server_config['zwave_ip'])
elif sensorType == 'ZWaveVeraLite':
sensor = ZWaveVeraLite(config.server_config['zwave_ip'],
config.server_config['zwave_port'])
class ROSLocationProcessor(PollingProcessor):
"""Abstract location processor, current concrete implementations are Human and Robot processors"""
def __init__(self):
super(ROSLocationProcessor, self).__init__()
self._lastSensors = {}
self._lastLocations = {}
self._sensors = Sensors()
self._locations = Locations()
self._ros = ROS()
def start(self):
print "Started polling uh locations"
self._addPollingProcessor('location_publish', self.checkUpdateLocations, (), 0.1)
self._addPollingProcessor('sensor_publish', self.checkUpdateSensors, (), 0.1)
def stop(self):
print "Stopped polling uh locations"
self._removePollingProcessor('location_publish')
self._removePollingProcessor('sensor_publish')
def checkUpdateLocations(self):
locationData = self._locations.findLocations()
locations = {}
for location in locationData:
locationParam = {
'id': location['locationId'],
'text': location['name'] or '',
'color': {'r': float(not location['ValidRobotLocation'] and not location['ValidUserLocation']),
'g': float(location['ValidRobotLocation'] or 0),
'b': float(location['ValidUserLocation'] or 0),
'a': 1.0}
}
if location.get('xCoord', None) is not None:
locationParam.setdefault('position', {})
locationParam['position']['x'] = location.get('xCoord')
if location.get('yCoord', None) is not None:
locationParam.setdefault('position', {})
locationParam['position']['y'] = location.get('yCoord')
if location.get('zCoord', None) is not None:
locationParam.setdefault('position', {})
locationParam['position']['z'] = location.get('zCoord')
if location.get('orientation', None) is not None:
locationParam.setdefault('orientation', {})
locationParam['orientation']['theta'] = math.radians(location['orientation'])
locations[locationParam['id']] = locationParam
if self._lastLocations != locations:
self._lastLocations = locations
self._ros.setParam('location_data', locations.values())
def checkUpdateSensors(self):
sensorData = self._sensors.findSensors()
sensors = {}
for sensor in sensorData:
sensorParam = {
'id': sensor['sensorId'],
'text': sensor['name'] or '',
'model': sensor['iconName'] or '',
'color': {'r': 0,
'g': 0,
'b': 0,
'a': 0}
}
if sensor.get('xCoord', None) is not None:
sensorParam.setdefault('position', {})
sensorParam['position']['x'] = sensor.get('xCoord')
if sensor.get('yCoord', None) is not None:
sensorParam.setdefault('position', {})
sensorParam['position']['y'] = sensor.get('yCoord')
if sensor.get('zCoord', None) is not None:
sensorParam.setdefault('position', {})
sensorParam['position']['z'] = sensor.get('zCoord')
if sensor['orientation'] is not None:
sensorParam.setdefault('orientation', {})
sensorParam['orientation']['theta'] = math.radians(sensor['orientation'])
if sensor['isActive']:
sensorParam['color']['r'] = 0
sensorParam['color']['g'] = 1.0
sensorParam['color']['b'] = 0
sensorParam['color']['a'] = 1.0
if sensor['xCoord'] is None and sensor['yCoord'] is None:
sensorParam['position']['z'] = -1
sensors[sensorParam['id']] = sensorParam
if self._lastSensors != sensors:
self._lastSensors = sensors
self._ros.setParam('sensor_data', sensors.values())
