def manual_mode():
# Prepare command
cmd_allo = yarp.Bottle()
cmd_allo.clear()
cmd_allo.addString('manual')
cmd_allo.addString('on')
# Send command to allostatic controller
if not yarp.Network.isConnected(alloPortName,alloRPC):
print yarp.Network.connect(alloPortName,alloRPC)
time.sleep(0.1)
toAllo.write(cmd_allo)
# Send command to planner
cmd_planner = yarp.Bottle()
cmd_planner.clear()
cmd_planner.addString('manual')
cmd_planner.addString('on')
if not yarp.Network.isConnected(plannerPortName,plannerRPC):
print yarp.Network.connect(plannerPortName,plannerRPC)
time.sleep(0.1)
toPlanner.write(cmd_planner)
time.sleep(0.1)
freeze_all()
time.sleep(0.1)
reset_all()
def createPorts(self):
self.imageDataInputPort = yarp.BufferedPortImageRgb()
self.outputFacePrection = yarp.Port()
self.speakStatusPort = yarp.RpcClient()
self.speakStatusOutBottle = yarp.Bottle()
self.speakStatusInBottle = yarp.Bottle()
self.imageInputBottle = yarp.Bottle()
def wait(self):
miOutput = yarp.Bottle()
miInput = yarp.Bottle()
miOutput.clear()
miOutput.addVocab(VOCAB_WAIT)
self.p.write(miOutput, miInput)
return True
def in_progress(self):
print "@in_progress Gettign WS"
ws = self.get_sensors()
wsc = ws.copy(); #self.memory.remove_ignored_preconditions(wsc)
new_change = self.excitedState.complement(wsc)
if len(new_change.state) == 0:
new_change = wsc.complement(self.excitedState)
if len(new_change.state) > 0:
print "@in_progress Getting excitation for new WS:\n",new_change.to_string()
agent = self.memory.get_excited_agent(new_change)
if agent[0].second > self.current_excitation:
print "@in_progress New state is more excited, executing it"
self.excitedState = ws; self.memory.current_schema = None; self.memory.current_chain = None
self.memory.ws = ws
self.memory.execute_excited_agent(self.excitedState)
print "Current Schema with new excitation is:",self.memory.current_schema.id
else:
print "@in_progress Last schema is still most excited"
query = yarp.Bottle()
query.addString("proceed")
self.portactsender.write(query)
response = yarp.Bottle()
response = self.portactstatusreceiver.read(True)
self.action_status_check(response.toString())
else:
print "@in_progress No significant change detected; Proceed with last action"
query = yarp.Bottle()
query.addString("proceed")
self.portactsender.write(query)
response = yarp.Bottle()
response = self.portactstatusreceiver.read(True)
self.action_status_check(response.toString())
return
def automatic_mode():
# Prepare command
cmd = yarp.Bottle()
cmd.clear()
cmd.addString('manual')
cmd.addString('off')
# Send command
if not yarp.Network.isConnected(alloPortName,alloRPC):
print yarp.Network.connect(alloPortName,alloRPC)
time.sleep(0.1)
toAllo.write(cmd)
# Send command to planner
cmd_planner = yarp.Bottle()
cmd_planner.clear()
cmd_planner.addString('manual')
cmd_planner.addString('off')
if not yarp.Network.isConnected(plannerPortName,plannerRPC):
print yarp.Network.connect(plannerPortName,plannerRPC)
time.sleep(0.1)
toPlanner.write(cmd_planner)
time.sleep(0.1)
reset_all()
time.sleep(0.1)
unfreeze_all()
def getActualPose(self):
"""
Returns the actual pose of the EE
:return: a kdl.Frame with the actual pose of the EE
"""
reply = yarp.Bottle()
reply.clear()
request = yarp.Bottle()
request.clear()
request.addString('get actual_pose')
self.rpc.write(request, reply)
if reply.size() != 16:
raise Exception('Unexpected reply size')
frame = kdl.Frame()
for i in xrange(3):
frame.p[i] = reply.get(i * 4 + 3).asDouble()
for r in xrange(3):
for c in xrange(3):
frame.M[r, c] = reply.get(r * 4 + c).asDouble()
return frame
def get_3d_stereo_angles(self, u_left, v_left, u_right, v_right):
""" returns the 3D point whose projected pixel coordinates (u,v)
in the image plane <type> ["left"|"right"] along with third
component <z> in the eye's reference frame are given.
It requires iKinGaze to run. In the simulator mode should
be activated with: ./iKinGazeCtrl --from configSim.ini
WARNING: has been hard to find the way for adding a list
in a bottle, the icub documentation should be improved.
The trick is: tmp_var = bottle.addList()
@param type: 'let' or 'right' camera
@param u: pixel coordinate x
@param v: pixel coordinate y
@return: the 3D point (x,y,z) coordinates
"""
bottle = yarp.Bottle()
result = yarp.Bottle()
bottle.clear()
bottle.addString('get')
bottle.addString('3D')
bottle.addString('stereo')
tmp0 = bottle.addList()
tmp0.addInt(u_left)
tmp0.addInt(v_left)
tmp0.addInt(u_right)
tmp0.addInt(v_right)
self.rpc_client_head_ikin.write(bottle, result)
list_bottle = result.get(1).asList()
list_return = []
for i in range(list_bottle.size()):
list_return.append(list_bottle.get(i).asDouble())
return list_return
def abstract_action(self, caller, A): #*args) #print "@abstract_action :", A.name, A.props, A.coords.concrete_coords query = yarp.Bottle() response = yarp.Bottle() query.addString(A.name) for a in A.props.keys(): query.addString(a) if type(A.props[a]) == type(str()): query.addString(A.props[a]) elif type(A.props[a]) == type(int()): query.addInt(A.props[a]) elif type(A.props[a]) == type(float()): query.addDouble(A.props[a]) for c in A.coords.concrete_coords.keys(): query.addString(c) if type(A.coords.concrete_coords[c]) == type(str()): query.addString(A.coords.concrete_coords[c]) elif type(A.coords.concrete_coords[c]) == type(int()): query.addInt(A.coords.concrete_coords[c]) elif type(A.coords.concrete_coords[c]) == type(float()): query.addDouble(A.coords.concrete_coords[c]) print "\nSending query:\n", query.toString() self.portactsender.write(query) print "Query sent, waiting for response:", datetime.datetime.now() response = self.portactstatusreceiver.read(True) self.action_status_check(response.toString()) return
def stop(self):
miOutput = yarp.Bottle()
miInput = yarp.Bottle()
miOutput.clear()
miOutput.addVocab(VOCAB_MY_STOP)
self.p.write(miOutput, miInput)
return True
def _move_head_random(self, delay=1.0):
t = threading.currentThread()
while getattr(t, "do_run", True):
roll = 0
pitch = random.randint(a=-30, b=+30)
yaw = random.randint(a=-20, b=+20)
bottle = yarp.Bottle()
result = yarp.Bottle()
# Set ROLL
bottle.clear()
bottle.addString("set")
bottle.addString("pos")
bottle.addInt(1) # Joint
bottle.addInt(roll) # Angle
self.rpc_client_head.write(bottle, result) # Send
# Set PITCH
bottle.clear()
bottle.addString("set")
bottle.addString("pos")
bottle.addInt(0) # Joint
bottle.addInt(pitch) # Angle
self.rpc_client_head.write(bottle, result) # Send
# Set YAW
bottle.clear()
bottle.addString("set")
bottle.addString("pos")
bottle.addInt(2) # Joint
bottle.addInt(yaw) # Angle
self.rpc_client_head.write(bottle, result) # Send
time.sleep(delay)
def set_icub_head_pose(rpc_client, roll, pitch, yaw):
bottle = yarp.Bottle()
result = yarp.Bottle()
return_list = list()
#Set ROLL
bottle.clear()
bottle.addString("set")
bottle.addString("pos")
bottle.addInt(1) #Joint
bottle.addInt(roll) #Angle
rpc_client.write(bottle, result) #Send
if(result == "[OK]"): return_list.append(True)
else: return_list.append(False)
#Set PITCH
bottle.clear()
bottle.addString("set")
bottle.addString("pos")
bottle.addInt(0) #Joint
bottle.addInt(pitch) #Angle
rpc_client.write(bottle, result) #Send
if(result == "[OK]"): return_list.append(True)
else: return_list.append(False)
#Set YAW
bottle.clear()
bottle.addString("set")
bottle.addString("pos")
bottle.addInt(2) #Joint
bottle.addInt(yaw) #Angle
rpc_client.write(bottle, result) #Send
if(result == "[OK]"): return_list.append(True)
else: return_list.append(False)
def __init__(self):
"""
Initialisation of the interaction function
"""
yarp.RFModule.__init__(self)
self.mm = None
self.dataPath = None
self.configPath = None
self.modelPath = None
self.driverName = ''
self.model_type = None
self.model_mode = None
self.textLabels = None
self.classifiers = None
self.classif_thresh = None
self.verbose = None
self.Quser = None
self.listOfModels = None
self.portsList = []
self.svPort = None
self.labelPort = None
self.instancePort = None
self.callSignList = []
self.inputBottle = yarp.Bottle()
self.outputBottle = yarp.Bottle()
self.portNameList = []
self.rpcConnected = False
self.dataInConnected = False
self.dataOutConnected = False
self.collectionMethod = ''
self.bufferSize = None
self.falseCount = 0
self.noDataCount = 0
self.inputType = None
self.outputType = None
self.errorRate = 50
self.dataList = []
self.classificationList = []
self.closeFlag = False
self.instancePortName = ''
self.labelPortName = ''
self.verboseSetting = False
self.exitFlag = False
self.recordingFile = ''
self.additionalInfoDict = dict()
self.modelLoaded = False
self.attentionMode = 'continue'
self.baseLogFileName = 'interactionErrorLog'
self.windowedMode = True
self.modelRoot = None
self.eventPort = None
self.eventPortName = None
self.classTimestamps = None
self.probClassList = None
self.recency = None
self.useRecentClassTime = True
self.my_mutex = thread.allocate_lock()
def __init__(self):
yarp.RFModule.__init__(self)
self.xtionResolution = (640, 480)
# Define port variable to recive an image from Teo's camera
self.xtionImagePort = yarp.BufferedPortImageRgb()
# Create numpy array to receive the image
self.xtionImageArray = np.zeros(
(self.xtionResolution[1], self.xtionResolution[0], 3),
dtype=np.uint8)
self.xtionObjectDetectionsPort = yarp.BufferedPortBottle()
self.glassesImagePort = yarp.BufferedPortImageRgb()
self.glassesResolution = (1080, 1080)
self.glassesImageArray = np.zeros(
(self.glassesResolution[1], self.glassesResolution[0], 3),
dtype=np.uint8)
self.minConfidenceDetection = 0.9
self.triggerProb = 0.7
self.cropsResolution = (120, 120)
self.glassesDataPort = yarp.BufferedPortBottle()
self.tripletModel = None
self.embeddingModel = None
# Output Ports
self.fixationObjectPort = yarp.Port()
self.fixationObjectImagePort = yarp.BufferedPortImageRgb()
self.glassesCropImagePort = yarp.BufferedPortImageRgb()
self.detections = yarp.Bottle()
self.bottleData = yarp.Bottle()
self.count_not_detections = 0
self.max_count_not_detections = 10
self.rightObject = False
if robot == '/teo':
self.xtionImagePortName = "/xtion/rgbImage"
elif robot == '/teoSim':
self.xtionImagePortName = "/teoSim/camera/rgbImage"
self.xtionObjectDetectionPortName = "/rgbdObjectDetection/state"
self.glassesImagePortName = "/glassesServer/images"
self.glassesDataPortName = "/glassesServer/data"
self.fixationObjectPortName = "/object"
self.fixationObjectImagePortName = "/rgbImage"
self.glassesCropImagePortName = "/Glassescrop"
# Categories
self.categories = None
self.previous_category = 0
self.glassesCropImages = []
self.allDetectionsInWindowFrames = []
self.allDetectionsLabelsInWindowFrames = []
self.period = 0.01
self.timeToGrop = 0
self.numberOfFramesJustOneObject = 0
def send_stop(rpc):
cmd = yarp.Bottle()
reply = yarp.Bottle()
cmd.addString('stop')
rpc.write(cmd, reply)
def checkRecall(rpcPort, askCommand):
# check correct response
cmBottle = yarp.Bottle()
rpBottle = yarp.Bottle()
cmBottle.addString(askCommand)
rpcPort.write(cmBottle, rpBottle)
results.append(rpBottle.toString())
return rpBottle
def updateBehaviorList():
cmd = yarp.Bottle()
rply = yarp.Bottle()
cmd.clear()
cmd.addString('names')
toBM.write(cmd,rply)
behList = rply.toString().strip('()').split(' ')
print behList
return behList
def updateDriveList():
cmd = yarp.Bottle()
rply = yarp.Bottle()
cmd.clear()
cmd.addString('names')
toHomeo.write(cmd,rply)
driveList = rply.toString().strip('()').split(' ')
print driveList
return driveList
def setLambda(self, l):
reply = yarp.Bottle()
reply.clear()
request = yarp.Bottle()
request.clear()
request.addString('set lambda')
request.addDouble(l)
self.rpc.write(request, reply)
def toggle_tokenizer(self):
cmd = yarp.Bottle()
cmd.clear()
rep = yarp.Bottle()
if self.tokenizer_state:
cmd.addString("pause")
else:
cmd.addString("resume")
self.portsList["tokenizer_control"].write(cmd, rep)
self.tokenizer_state = not self.tokenizer_state
def getLambda(self):
reply = yarp.Bottle()
reply.clear()
request = yarp.Bottle()
request.clear()
request.addString('get lambda')
self.rpc.write(request, reply)
return reply.get(0).asDouble()
def set_home(self):
"""Sets manipulator to Home"""
query = yarp.Bottle()
response = yarp.Bottle()
query.clear()
response.clear()
query.addString("home")
#print("sending get_sensors")
self.portactsender.write(query)
return
def movl(self, xd):
miOutput = yarp.Bottle()
miInput = yarp.Bottle()
miOutput.clear()
miOutput.addVocab(VOCAB_MOVL)
dBottle = yarp.Bottle()
dBottle = miOutput.addList()
for elem in range(0, len(xd)):
dBottle.addFloat64(xd[elem])
self.p.write(miOutput, miInput)
return True
def help(self):
reply = yarp.Bottle()
reply.clear()
request = yarp.Bottle()
request.clear()
request.addString('help')
self.rpc.write(request, reply)
return reply
def send_command(self, command):
"""Send a command to the RPC controller."""
query = yarp.Bottle()
query.clear()
response = yarp.Bottle()
response.clear()
print ("sending %s in send_command"%command)
query.addString(command)
self.portactsender.write(query)
response = self.portactstatusreceiver.read(True)
return response.toString == "SUCCESS"
def stat(self, res):
miOutput = yarp.Bottle()
miInput = yarp.Bottle()
miOutput.clear()
miOutput.addVocab(VOCAB_STAT)
self.p.write(miOutput, miInput)
data = miInput.get(0).asList()
del res[:]
for elem in range(0, data.size()):
res.append(data.get(elem).asFloat64())
return True
def send_position_reference(rpc, x, y, z):
cmd = yarp.Bottle()
reply = yarp.Bottle()
cmd.addString('set_position')
cmd.addDouble(x)
cmd.addDouble(y)
cmd.addDouble(z)
rpc.write(cmd, reply)
def format_names_to_bottle(list_names):
import yarp
list_objects_bottle = yarp.Bottle()
list_objects_bottle.clear()
for name in list_names:
yarp_object_bottle = yarp.Bottle()
yarp_object_bottle.addString(name[0])
yarp_object_bottle.addDouble(round(float(name[1]), 2))
list_objects_bottle.addList().read(yarp_object_bottle)
return list_objects_bottle
def inv(self, xd, res):
miOutput = yarp.Bottle()
miInput = yarp.Bottle()
miOutput.clear()
miOutput.addVocab(VOCAB_INV)
dBottle = yarp.Bottle()
dBottle = miOutput.addList()
for elem in range(0, len(xd)):
dBottle.addFloat64(xd[elem])
self.p.write(miOutput, miInput)
data = miInput.get(0).asList()
del res[:]
for elem in range(0, data.size()):
res.append(data.get(elem).asFloat64())
return True
def write_text(self, text):
if self.speech_output.getOutputCount():
action_bottle = yarp.Bottle()
action_bottle.clear()
action_bottle.addString(text)
self.speech_output.write(action_bottle)
def readCommands(supPort, inBottle, replyBool, replyStr, exception,
recActions):
while (1):
supPort.read(inBottle, replyBool)
message = inBottle.get(0).asString()
print(message + ' received')
print 'responding to ' + message + ' request'
if (message == 'EXIT'):
exception[0] = 'keyInterupt'
replyStr = 'ack'
elif ('label' in message):
if (len(recActions) > 0):
print 'label'
print recActions
replyStr = '__'.join(recActions)
replyStr = 'ack ' + replyStr
else:
replyStr = 'ack no_actions_recognised'
elif ('instance' in message):
# parse all remaining Bottle contents
if (portsList[instancePort].getInputCount() != 0):
replyStr = 'ack'
else:
replyStr = 'nack'
print 'No connections to ' + portsList[instancePort].getName()
else:
replyStr = 'nack'
print message + ' is not a valid request'
print
supPort.reply(yarp.Bottle(replyStr))