def getRobot(robotName=None): """ Retrieves the named robot type from the database and constructs the appropriate class """ """ if robotName is None or Empty, acts like getCurrentRobot() """ if robotName == None or robotName == '': return Factory.getCurrentRobot() print "Building class for robot named: %s" % robotName robot = None if robotName.lower().startswith('care-o-bot'): import careobot if robot_config.has_key(robotName) and robot_config[robotName].has_key('hostname'): rosMaster = 'http://%s:11311' else: cobVersion = robotName[11:].replace('.', '-') rosMaster = "http://cob%s-pc1:11311" % cobVersion robot = careobot.CareOBot(robotName, rosMaster) elif robotName.lower().startswith('sunflower'): import sunflower if robot_config.has_key(robotName) and robot_config[robotName].has_key('hostname'): rosMaster = 'http://%s:11311' else: cobVersion = robotName[10:].replace('.', '-') rosMaster = "http://sunflower%s-pc1:11311" % cobVersion robot = sunflower.Sunflower(robotName, rosMaster) elif robotName.lower().startswith('dummy'): import dummy robot = dummy.DummyRobot(robotName) else: print >> sys.stderr, "Unknown robot %s" % robotName return None print "Finished building class %s" % robot.__class__.__name__ return robot
def getRobot(robotName=None, spawn=True): """ Retrieves the named robot type from the database and constructs the appropriate class """ """ if robotName is None or Empty, acts like getCurrentRobot() """ if robotName == None or robotName == '': return Factory.getCurrentRobot() print "Building class for robot named: %s" % robotName # care-o-bot shortened to make 'in' easier to handle (only check first 9 characters) rosRobots = ['care-o-bo', 'sunflower'] if robotName.lower()[:9] in rosRobots: if robotName.lower().startswith('care-o-bot'): if robot_config.has_key(robotName) and robot_config[robotName].has_key('hostname'): rosMaster = 'http://%s:11311' else: cobVersion = robotName[11:].replace('.', '-') rosMaster = "http://cob%s-pc1:11311" % cobVersion imports = ['from careobot import CareOBot'] args = (robotName, rosMaster, ) kwargs = {} className = 'CareOBot' elif robotName.lower().startswith('sunflower'): if robot_config.has_key(robotName) and robot_config[robotName].has_key('hostname'): rosMaster = 'http://%s:11311' else: cobVersion = robotName[10:].replace('.', '-') rosMaster = "http://sunflower%s-pc1:11311" % cobVersion imports = ['from sunflower import Sunflower'] args = (robotName, rosMaster, ) kwargs = {} className = 'Sunflower' # Set the LD_LIBRARY_PATH before forking from rosHelper import ROS rosENV = ROS._parseBashEnviron() rosLD = rosENV.get('LD_LIBRARY_PATH', '') curLD = os.environ.get('LD_LIBRARY_PATH') os.environ['LD_LIBRARY_PATH'] = curLD + ',' + rosLD if spawn: # Create the wrapper (runs the named class in a sub-process) from subProcessWrapper import SubProcessWrapper robot = SubProcessWrapper(imports, className, *args, **kwargs) # Now revert LD_LIBRARY_PATH for this process os.environ['LD_LIBRARY_PATH'] = curLD else: for import_ in imports: exec(import_, globals()) robot = globals()[className](*args, **kwargs) elif robotName.lower().startswith('dummy'): import dummy robot = dummy.DummyRobot(robotName) else: print >> sys.stderr, "Unknown robot %s" % robotName return None print "Finished building class %s" % className return robot
def getRobot(robotName=None): """ Retrieves the named robot type from the database and constructs the appropriate class """ """ if robotName is None or Empty, acts like getCurrentRobot() """ if robotName == None or robotName == "": return Factory.getCurrentRobot() print "Building class for robot named: %s" % robotName robot = None if robotName.lower().startswith("care-o-bot"): import careobot if robot_config.has_key(robotName) and robot_config[robotName].has_key("hostname"): rosMaster = "http://%s:11311" % robot_config[robotName]["hostname"] else: cobVersion = robotName[11:].replace(".", "-") rosMaster = "http://cob%s-pc1:11311" % cobVersion robot = careobot.CareOBot(robotName, rosMaster) elif robotName.lower().startswith("sunflower"): import sunflower if robot_config.has_key(robotName) and robot_config[robotName].has_key("hostname"): rosMaster = "http://%s:11311" % robot_config[robotName]["hostname"] else: sfVersion = robotName[10:].replace(".", "-") rosMaster = "http://sf%s-pc1:11311" % sfVersion robot = sunflower.Sunflower(robotName, rosMaster) elif robotName.lower().startswith("uh sunflower"): import sunflower if robot_config.has_key(robotName) and robot_config[robotName].has_key("hostname"): rosMaster = "http://%s:11311" % robot_config[robotName]["hostname"] else: rosMaster = "http://sf1-1-pc1:11311" robot = sunflower.Sunflower(robotName, rosMaster) elif robotName.lower().startswith("roomba"): import roomba if robot_config.has_key(robotName) and robot_config[robotName].has_key("hostname"): uri = "http://%s" % robot_config[robotName]["hostname"] else: uri = "roomba" robot = roomba.Roomba(robotName, uri) elif robotName.lower().startswith("dummy"): import dummy robot = dummy.DummyRobot(robotName) else: print >> sys.stderr, "Unknown robot %s" % robotName return None print "Finished building class %s" % robot.__class__.__name__ return robot
def setComponentState(self, name, value, blocking=True): """ Set the named component to the given value value can be a string for a named position, or an array of floats for specific joint values The length of the array must match the length of the named joints for the component If a named position is defined in the robot config, the config value will be substituted before sending. This allows for a generic top level command to work on multiple robots. I.E. 'Tray' to 'Raised' will change to 'Up', 'Open', 'DeliverUp' depending on the model of robot (COB3.2, Sunflower, COB3.5, in this example) if name is base and value is userLocation, this will cause the robot to go to the current user location. set blocking to true(default) for this function to block until completed if blocking is set to false, this function will return immediately with value 'ACTIVE' """ if ( robot_config.has_key(self.name) and robot_config[self.name].has_key(name) and robot_config[self.name][name].has_key("positions") and robot_config[self.name][name]["positions"].has_key(value) ): value = robot_config[self.name][name]["positions"][value] if name == "base" and value == "userLocation": user = Users().getActiveUser() if user["xCoord"] != None and user["yCoord"] != None and user["orientation"] != None: value = [user["xCoord"], user["yCoord"], math.radians(user["orientation"])] print "Going to userLocation location (%s, %s, %s)" % ( user["xCoord"], user["yCoord"], user["orientation"], ) status = self._robInt.runComponent(name, value, None, blocking) return _states[status]
def setComponentState(self, name, value, blocking=True): """ Set the named component to the given value value can be a string for a named position, or an array of floats for specific joint values The length of the array must match the length of the named joints for the component If a named position is defined in the robot config, the config value will be substituted before sending. This allows for a generic top level command to work on multiple robots. I.E. 'Tray' to 'Raised' will change to 'Up', 'Open', 'DeliverUp' depending on the model of robot (COB3.2, Sunflower, COB3.5, in this example) if name is base and value is userLocation, this will cause the robot to go to the current user location. set blocking to true(default) for this function to block until completed if blocking is set to false, this function will return immediately with value 'ACTIVE' """ if type(value) == str and \ robot_config.has_key(self.name) and \ robot_config[self.name].has_key(name) and \ robot_config[self.name][name].has_key('positions') and \ robot_config[self.name][name]['positions'].has_key(value): value = robot_config[self.name][name]['positions'][value] if name == "base" and value == "userLocation": user = Users().getActiveUser() if user['xCoord'] != None and user['yCoord'] != None and user[ 'orientation'] != None: value = [ user['xCoord'], user['yCoord'], math.radians(user['orientation']) ] print "Going to userLocation location (%s, %s, %s)" % ( user['xCoord'], user['yCoord'], user['orientation']) status = self._robInt.runComponent(name, value, None, blocking) return _states[status]
def getRobot(robotName=None): """ Retrieves the named robot type from the database and constructs the appropriate class """ """ if robotName is None or Empty, acts like getCurrentRobot() """ if robotName == None or robotName == '': return Factory.getCurrentRobot() print "Building class for robot named: %s" % robotName robot = None if robotName.lower().startswith('care-o-bot'): import careobot if robot_config.has_key(robotName) and robot_config[robotName].has_key('hostname'): rosMaster = 'http://%s:11311' else: cobVersion = robotName[11:].replace('.', '-') rosMaster = "http://cob%s-pc1:11311" % cobVersion robot = careobot.CareOBot(robotName, rosMaster) elif robotName.lower().startswith('sunflower'): import sunflower if robot_config.has_key(robotName) and robot_config[robotName].has_key('hostname'): rosMaster = 'http://%s:11311' % robot_config[robotName]['hostname'] else: sfVersion = robotName[10:].replace('.', '-') rosMaster = "http://sf%s-pc1:11311" % sfVersion robot = sunflower.Sunflower(robotName, rosMaster) elif robotName.lower().startswith('uh sunflower'): import sunflower if robot_config.has_key(robotName) and robot_config[robotName].has_key('hostname'): rosMaster = 'http://%s:11311' % robot_config[robotName]['hostname'] else: rosMaster = "http://sf1-1-pc1:11311" robot = sunflower.Sunflower(robotName, rosMaster) elif robotName.lower().startswith('dummy'): import dummy robot = dummy.DummyRobot(robotName) else: print >> sys.stderr, "Unknown robot %s" % robotName return None print "Finished building class %s" % robot.__class__.__name__ return robot
def resolveComponentState(self, componentName, state, tolerance=0.5): """ Attempt to resolve a given state to it's closes named position, given the stated tolerance value""" """ This method was initally written for locations, and can struggle with higher dimensional values, """ """ I.E, a named arm position. There are also problems around '0'. This method will likely need to """ """ eventually be replaced with something more robust. """ """ If the named position exists in the robot_config, the config key will be substituted before returning """ """ This is the reverse of what happens in setComponentState, using the same example, ('Up', 'Open', 'DeliverUp') """ """ will all be changed to 'Raised' before returning """ if state == None: return (None, None) curPos = state["positions"] positions = self.getComponentPositions(componentName) if len(positions) == 0: # print >> sys.stderr, "Unable to retrieve named positions. Name resolution will now abort" return (None, state) name = None diff = None for positionName in positions: positionValue = self._getValue(positions[positionName]) if type(positionValue) is not list: # we don't currently handle nested types continue if len(positionValue) != len(curPos): # raise Exception("Argument lengths don't match") continue dist = 0 for index in range(len(positionValue)): dist += math.pow(curPos[index] - positionValue[index], 2) dist = math.sqrt(dist) if name == None or dist < diff: name = positionName diff = dist if diff <= tolerance: if ( robot_config.has_key(self.name) and robot_config[self.name].has_key(componentName) and robot_config[self.name][componentName].has_key("positions") ): positions = robot_config[self.name][componentName]["positions"] for key, value in positions.items(): if value == name: return (key, state) return (name, state) else: return ("", state)
def resolveComponentState(self, componentName, state, tolerance=0.5): """ Attempt to resolve a given state to it's closes named position, given the stated tolerance value""" """ This method was initally written for locations, and can struggle with higher dimensional values, """ """ I.E, a named arm position. There are also problems around '0'. This method will likely need to """ """ eventually be replaced with something more robust. """ """ If the named position exists in the robot_config, the config key will be substituted before returning """ """ This is the reverse of what happens in setComponentState, using the same example, ('Up', 'Open', 'DeliverUp') """ """ will all be changed to 'Raised' before returning """ if state == None: return (None, None) curPos = state['positions'] positions = self.getComponentPositions(componentName) if len(positions) == 0: #print >> sys.stderr, "Unable to retrieve named positions. Name resolution will now abort" return (None, state) name = None diff = None for positionName in positions: positionValue = self._getValue(positions[positionName]) if type(positionValue) is not list: # we don't currently handle nested types continue if len(positionValue) != len(curPos): # raise Exception("Argument lengths don't match") continue dist = 0 for index in range(len(positionValue)): dist += math.pow(curPos[index] - positionValue[index], 2) dist = math.sqrt(dist) if name == None or dist < diff: name = positionName diff = dist if diff <= tolerance: if robot_config.has_key(self.name) and robot_config[ self.name].has_key(componentName) and robot_config[ self.name][componentName].has_key('positions'): positions = robot_config[self.name][componentName]['positions'] for key, value in positions.items(): if value == name: return (key, state) return (name, state) else: return ('', state)
def getImage(self, retFormat='PNG'): """ Returns the image from the robots camera (topic specified in robot_config) or None if no camera """ """ While retFormat is taken into account, PIL appers to always return a JPG file, issue has not been investigated """ if not robot_config.has_key(self.name) or not robot_config[ self.name]['head'].has_key('camera'): return None img_msg = self._ros.getSingleMessage(self._imageTopic) if img_msg == None: return None from PIL import Image imgBytes = io.BytesIO() imgBytes.write(img_msg.data) imgBytes.seek(0) img = Image.open(imgBytes) if robot_config[self._name]['head']['camera'].has_key('rotate'): angle = self.getCameraAngle() or 0 a = abs( angle - robot_config[self._name]['head']['camera']['rotate']['angle']) if a > 180: a = abs(a - 360) # 0=back, 180=front, 270=top, 90=bottom. rotate if not front (0-180 are invalid angles, only included for 'buffer') # if angle <= 90 and angle >= 270: if a <= robot_config[ self._name]['head']['camera']['rotate']['distance']: img = img.rotate(robot_config[self._name]['head']['camera'] ['rotate']['amount']) retFormat = retFormat.upper() if retFormat == 'JPG': retFormat = 'JPEG' if retFormat not in Robot._imageFormats: retFormat = 'PNG' imgBytes.seek(0) img.save(imgBytes, retFormat) return imgBytes.getvalue()
def getImage(self, retFormat="PNG"): """ Returns the image from the robots camera (topic specified in robot_config) or None if no camera """ """ While retFormat is taken into account, PIL appers to always return a JPG file, issue has not been investigated """ if not robot_config.has_key(self.name) or not robot_config[self.name]["head"].has_key("camera"): return None img_msg = self._ros.getSingleMessage(self._imageTopic) if img_msg == None: return None from PIL import Image imgBytes = io.BytesIO() imgBytes.write(img_msg.data) imgBytes.seek(0) img = Image.open(imgBytes) if robot_config[self._name]["head"]["camera"].has_key("rotate"): angle = self.getCameraAngle() or 0 a = abs(angle - robot_config[self._name]["head"]["camera"]["rotate"]["angle"]) if a > 180: a = abs(a - 360) # 0=back, 180=front, 270=top, 90=bottom. rotate if not front (0-180 are invalid angles, only included for 'buffer') # if angle <= 90 and angle >= 270: if a <= robot_config[self._name]["head"]["camera"]["rotate"]["distance"]: img = img.rotate(robot_config[self._name]["head"]["camera"]["rotate"]["amount"]) retFormat = retFormat.upper() if retFormat == "JPG": retFormat = "JPEG" if retFormat not in Robot._imageFormats: retFormat = "PNG" imgBytes.seek(0) img.save(imgBytes, retFormat) return imgBytes.getvalue()