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