Остановить скрипт Python, работающий на роботе нао, касающемся его головы

Моя проблема заключается в следующем. У меня есть скрипт на Python, который выполняется так, что робот выполняет различные действия без остановки, пока выполнение не остановится, однако из соображений безопасности (на случай, если робот сойдет с ума и захочет нас всех убить) мне нужно добавить эту инструкцию, чтобы остановить это с помощью сенсорного датчика его головы в случае нажатия.

Я немного читал о модуле ALTouch, с помощью которого можно сгенерировать модуль TouchChanged(), но он действует на все датчики (включая движения), а не только на датчик касания на голове.

Любые идеи или соответствующая документация будут приветствоваться.

Вот часть моего кода:

class SoundProcessingModule(object):

    def __init__( self, app):

        ttsProxy.say("Touch my head at any moment to stop me")

        super(SoundProcessingModule, self).__init__()
        app.start()
        session = app.session

        self.audio_service = session.service("ALAudioDevice")
        self.isProcessingDone = False
        self.nbOfFramesToProcess = 100
        self.framesCount=0
        self.micFront = []
        self.module_name = "SoundProcessingModule"

    def startProcessing(self):
        self.audio_service.setClientPreferences(self.module_name, 16000, 3, 0)
        self.audio_service.subscribe(self.module_name)

        while self.isProcessingDone == False:
            time.sleep(1)

        self.audio_service.unsubscribe(self.module_name)

    def processRemote(self, nbOfChannels, nbOfSamplesByChannel, timeStamp, inputBuffer):
        #ReadyToDance
        postureProxy.goToPosture("StandInit", 0.5)

        self.framesCount = self.framesCount + 1
        if (self.framesCount <= self.nbOfFramesToProcess):
            print(self.framesCount)
            self.micFront=self.convertStr2SignedInt(inputBuffer)
            rmsMicFront = self.calcRMSLevel(self.micFront)
            print ("Nivel RMS del microfono frontal = " + str(rmsMicFront))
            rmsArray.insert(self.framesCount-1,rmsMicFront)

            #-40 y -30
            if promedio >= -40 and promedio <= -30 :
                #Some dance moves    

            #-29 y -20
            elif promedio >= -29 and promedio <= -20:
                #Some dance moves

            #-19 y -11
            elif promedio >= -19 and promedio <= -11:
                #Some dance moves

        else :
            self.isProcessingDone=True
            #Plot RMS signal
            plt.plot(rmsArray)
            plt.ylabel('RMS')
            plt.xlabel('Frames')
            plt.text(np.argmin(rmsArray), np.min(rmsArray) - 0.1, u'Mínimo', fontsize=10, horizontalalignment='center',
                     verticalalignment='center')  
            plt.text(np.argmax(rmsArray), np.max(rmsArray) + 0.1, u'Máximo', fontsize=10, horizontalalignment='center',
                     verticalalignment='center')  
            print("")
            print ("El promedio total del sonido es: " + str(np.mean(rmsArray)))
            print("El maximo total del sonido es: " + str(np.max(rmsArray)))
            print("El minimo total del sonido es: " + str(np.min(rmsArray)))
            plt.show()
            postureProxy.goToPosture("Sit", 1.0)

    def calcRMSLevel(self,data) :
        rms = 20 * np.log10( np.sqrt( np.sum( np.power(data,2) / len(data)  )))
        return rms

    def convertStr2SignedInt(self, data) :
        signedData=[]
        ind=0;
        for i in range (0,len(data)/2) :
            signedData.append(data[ind]+data[ind+1]*256)
            ind=ind+2

        for i in range (0,len(signedData)) :
            if signedData[i]>=32768 :
                signedData[i]=signedData[i]-65536

        for i in range (0,len(signedData)) :
            signedData[i]=signedData[i]/32768.0

        return signedData


def StiffnessOn(proxy):
    # We use the "Body" name to signify the collection of all joints
    pNames = "Body"
    pStiffnessLists = 1.0
    pTimeLists = 1.0
    proxy.stiffnessInterpolation(pNames, pStiffnessLists, pTimeLists)

if __name__ == "__main__":
    parser = argparse.ArgumentParser()
    #Es necesario estar al pendiente de la IP del robot para moficarla
    parser.add_argument("--ip", type=str, default="nao.local",
                        help="Robot IP address. On robot or Local Naoqi: use '127.0.0.1'.")
    parser.add_argument("--port", type=int, default=9559,
                        help="Naoqi port number")

    args = parser.parse_args()
        # Inicializamos proxys.
    try:
        proxy = ALProxy("ALMotion", "nao.local", 9559)
    except Exception, e:
        print "Could not create proxy to ALMotion"
        print "Error was: ", e

    try:
        postureProxy = ALProxy("ALRobotPosture", "nao.local", 9559)
    except Exception, e:
        print "Could not create proxy to ALRobotPosture"
        print "Error was: ", e

    try:
        ttsProxy = ALProxy("ALTextToSpeech" , "nao.local", 9559)
    except Exception, e:
        print "Could not create proxy to ALTextToSpeech"
        print "Error was: ", e

    try:
        memory = ALProxy("ALMemory" , "nao.local", 9559)
    except Exception, e:
        print "Could not create proxy to ALMemory"
        print "Error was: ", e

    try:
        connection_url = "tcp://" + args.ip + ":" + str(args.port)
        app = qi.Application(["SoundProcessingModule", "--qi-url=" + connection_url])
    except RuntimeError:
        print ("Can't connect to Naoqi at ip \"" + args.ip + "\" on port " + str(args.port) +".\n"
               "Please check your script arguments. Run with -h option for help.")
        sys.exit(1)

    MySoundProcessingModule = SoundProcessingModule(app)
    app.session.registerService("SoundProcessingModule", MySoundProcessingModule)
    MySoundProcessingModule.startProcessing()

Робот танцует в соответствии со среднеквадратичным значением, полученным от переднего микрофона, но мне нужно остановить его в любой момент, когда коснется датчика головы (или любого другого датчика).


person Luis Hongo    schedule 06.09.2017    source источник


Ответы (1)


Вместо подписки на TouchChanged можно подписаться на голову только с 3-мя событиями (для 3-х тактильных кнопок):

  • ФронтTactilTouched
  • СреднийTactilTouched
  • ЗадняяTactilTouched

Они будут повышаться со значением 1, когда начинается касание, и 0, когда касание заканчивается. Таким образом, вы захотите отфильтровать и остановить свой танец только тогда, когда значение равно 1.

person JLS    schedule 13.09.2017