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