Stop Python script running on the robot itself, touching its head

My problem is as follows. I have a script in Python that runs so that the robot performs various actions without stopping until execution stops, however, for security reasons (in case the robot goes crazy and wants to kill us all). I need to add this instruction to stop it with the touch sensor of his head in case of pressing.

I got a little familiar with the ALTouch module, with which you can create the TouchChanged () module, but it acts on all sensors (including movements), and not just on the touch sensor on the head.

Any ideas or related documentation would be welcome.

Here is my code:

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

RMS, , , ( ).

+1
1

, TouchChanged, ( 3 ):

  • FrontTactilTouched
  • MiddleTactilTouched
  • RearTactilTouched

1, , 0, . , , 1.

+1

Source: https://habr.com/ru/post/1688945/


All Articles