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)