def hd_lookat_caller(command):
"""
BB Command callback which:
1. Attends a BB Command Invocation.
2. Parses the parameters list comming from BlackBoard to the hd_lookat format.
3. Invoke the correponding service (which has the same name of the bridged command) with the parsed parameters and waits for his response.
4. Send the response received from the ROS service and send it to Blackboard.
Receives:
command: A command (pyrobotics) object
Return:
The response to the command caller
"""
rospy.logdebug('Call to service: "' + str(command.name) +
'" received from BB.')
try:
#parse the parameters comming from BB to the service specific request format
head_pan = float(command.params.split(' ')[0])
head_tilt = float(command.params.split(' ')[1])
#send the parameters to the ROS service
rospy.wait_for_service(
command.name, timeout=4) #wait for the service to be available
hd_lookat_service = rospy.ServiceProxy(command.name, hd_lookat)
service_response = hd_lookat_service(head_pan, head_tilt)
#parse the response obtained from the ros service
command_response = str(service_response.currentPan) + str(
service_response.currentTilt) + str(service_response.errors)
return Response.FromCommandObject(command, True, command_response)
except rospy.ServiceException, e:
rospy.logfatal('Service call ' + command.name + ' failed: ' +
str(e))
def slowFunction(c):
print "slow function called..."
time.sleep(3)
r = BB.SendAndWait(Command('tst_testfunction', "This is another test."),
3000)
success = r and r.successful
return Response.FromCommandObject(c, success, 'Slow Command response')
def cmd_task(c):
args = ''
if q.es_vacia() == False:
res = q.popC()
for i in res:
temp = i.split(" ")
if len(temp) == 1:
args = args + " " + "person"
if len(temp)>1:
for j in range(1,len(temp)):
args = args + " " + temp[j]
print args
return Response.FromCommandObject(c, True, args )
#print "verdadero"
else:
#print "falso"
return Response.FromCommandObject(c, False, "No_Task" )
def cmd_int(c):
#cadena = cmdQ.popC()
#cadena2 = str(cadena)
#try:
# content = cadena2.split("',")
#except:
# print 'Error the Command format is incorrect'
# args = 'No_Interpretation'
# return Response.FromCommandObject(c, False, args)
#temp = content[0]
#temp1 = temp.lstrip("[('")
#agregamos el comando mediante un archivo, despues usaremos el gpsr
filename = sys.argv[1]
with open(filename) as f:
content = f.read()
interpreted_command = egprs_interpreter.interpret_command(content)
print " "
print "Comando Interpretado:"
print interpreted_command
try:
cabecera = interpreted_command.split(' ')
except:
print 'Error the Interpreted Command format is incorrect'
args = 'No_Interpretation'
return Response.FromCommandObject(c, False, args)
if interpreted_command == 'False' or cabecera[0] == '(task_to' :
args = 'No_Interpretation'
return Response.FromCommandObject(c, False, args)
else:
q.empty()
separaTask(interpreted_command)
args = temp1.replace(' ','_')
#steps = q.lenC()
#plan_name_id = randrange(10000)
#args = "plan-" + str(plan_name_id) + " " + str(steps)
#print '<-------------->'
#print args
#print '<-------------->'
return Response.FromCommandObject(c, True, args)
def cmd_int(c):
#agregamos el comando mediante un archivo, despues usaremos el gpsr
filename = sys.argv[1]
with open(filename) as f:
content = f.read()
interpreted_command = egprs_interpreter.interpret_command(content)
print " "
print "Comando Interpretado:"
print interpreted_command
separaTask(interpreted_command)
return Response.FromCommandObject(c, True, "Completed")
def process_string(c): interpreted_command = egprs_interpreter.interpret_command(c.params) return Response.FromCommandObject(c, True, interpreted_command)
def testFunction(c):
print "testFunction called..."
print 'Params: ' + c.params
return Response.FromCommandObject(c, True, 'Command response')
def cmd_one(c):
time.sleep(1)
return Response.FromCommandObject(c, True, 'cmd_one response')
def cmd_two(c):
time.sleep(5)
return Response.FromCommandObject(c, True, 'cmd_two response')
def cmd_speech(c): print 'ReadyForReciveCommands' cmdQR.empty() cmdHQ.empty() print 'Give a COMMAND' return Response.FromCommandObject(c, True, "ReadyToDevelopCommands")
def cmd_conf(c):
print ' '
print 'Request CONFIRMATION '# + c.params
spg = 'Do you want I make the command ' + c.params
spg_say = spg.replace('_', ' ')
print 'Sending command say...' + spg_say
#print BB.SendAndWait(Command('say', spg_say), 5000, 1)
#print 'cmdQR'
#cmdQR.empty()
#print 'cmdHQ'
#cmdHQ.empty()
#print 'Ingresar un comando Test'
#while cmdQR.es_vacia():
# j = 0
#if cmdHQ.es_vacia() == False:
# temp = cmdHQ.popPile()
#print 'temp'
#print temp
#else:
# if cmdQR.es_vacia()== False:
# temp = cmdQR.popC()
#print 'temp'
#print temp
# else:
# print 'False confirmation'
# return Response.FromCommandObject(c, False, args)
#cadena2 = str(temp)
#content = cadena2.split("',")
#temp2 = content[0]
#temp1 = temp2.lstrip("[('")
#print 'Resp ' + temp1
#if temp1 == 'robot yes':
# steps = q.lenC()
# plan_name_id = randrange(10000)
# args = "plan-" + str(plan_name_id) + " " + str(steps)
# print '<-------------->'
# print args
# print '<-------------->'
# return Response.FromCommandObject(c, True, args)
#elif temp1 == 'robot no':
# q.empty()
# args = 'confirmation_no'
# return Response.FromCommandObject(c, False, args)
#else:
# q.empty()
# args = 'confirmation_no'
# return Response.FromCommandObject(c, False, args)
n = raw_input('[Y/N]: ')
if n == 'y' or n == 'Y':
steps = q.lenC()
plan_name_id = randrange(10000)
args = "plan-" + str(plan_name_id) + " " + str(steps)
print '<-------------->'
print args
print '<-------------->'
return Response.FromCommandObject(c, True, args)
else:
q.empty()
args = 'confirmation_no'
return Response.FromCommandObject(c, False, args)
command_response = str(service_response.currentPan) + str(
service_response.currentTilt) + str(service_response.errors)
return Response.FromCommandObject(command, True, command_response)
except rospy.ServiceException, e:
rospy.logfatal('Service call ' + command.name + ' failed: ' +
str(e))
except rospy.ROSException:
rospy.logfatal('Timeout exceeded while waiting for the service ' +
command.name + ' to be available.')
except:
rospy.logfatal(
'An error ocurred when trying to parse the params: "' +
command.params + '" for the ' + command.name + ' service')
#send the response to BB
return Response.FromCommandObject(command, False, '')
@staticmethod
def hd_torque_caller(command):
"""
BB Command callback which:
1. Attends a BB Command Invocation.
2. Parses the parameters list comming from BlackBoard to the hd_torque format.
3. Invoke the correponding service (which has the same name of the bridged command) with the parsed parameters and waits for his response.
4. Send the response received from the ROS service and send it to Blackboard.
Receives:
command: A command (pyrobotics) object
Return:
The response to the command caller
"""
rospy.logdebug('Call to service: "' + str(command.name) +
def SendResponse(cmdName, cmd_id, result, response):
result = str(result).lower() not in ['false', '0']
r = Response(cmdName, result, response)
r._id = cmd_id
BB.Send(r)
