Пример #1
0
    def __init__(self):
        Thread.__init__(self)
        self.name = "WalleServer"

        self.number = 0
        self.azimuth = 0.0  # position sense, azimuth from north
        self.turning_to_object = False  # Are we turning to see an object
        self.hearing_angle = 0.0  # device hears sound from this angle, device looks to its front
        # to the azimuth direction
        self.observation_angle = 0.0  # turn until azimuth is this angle

        self.leftPower = 0.0  # moving
        self.rightPower = 0.0

        self.number = 0
        self.in_axon = Axon()  # global queue for senses to put sensations to walle
        self.out_axon = Axon()  # global queue for walle to put sensations to external senses

        # starting build in capabilities/senses
        # we have capability to move
        if MANUAL:
            self.romeo = ManualRomeo()
        else:
            self.romeo = Romeo()
        # we have hearing (positioning of object using sounds)
        self.hearing = Hearing(self.in_axon)

        # starting tcp server as nerve pathway to external senses to connect
        # we have azimuth sense (our own position detection)
        self.tcpServer = TCPServer(out_axon=self.in_axon, in_axon=self.out_axon, server_address=(HOST, PORT))

        self.running = False
        self.turnTimer = Timer(WalleServer.ACTION_TIME, self.stopTurn)

        self.calibrating = False  # are we calibrating
        self.calibrating_angle = 0.0  # calibrate device hears sound from this angle, device looks to its front
        # self.calibratingTimer = Timer(WalleServer.ACTION_TIME, self.stopCalibrating)
        print "WalleServer: Calibrate version"
Пример #2
0
class WalleServer(Thread):
    """
    Controls Walle-robot. Walle has capabilities like moving, hearing, seeing and position sense.
    Technically we use socket servers to communicate with external devices. Romeo board is controlled
    using library using USB. We use USB-microphones and Raspberry pi camera.
    
    Walle emulates sensorys (camera, microphone, mobile phone) that have emit sensations to "brain" that has state and memory and gives
    commands (technically Sensation class instances) to muscles (Romeo Board, mobile phone)
    
    Sensations from integrated sensorys are transferred By axons ad in real organs, implemented as Queue, which is thread safe.
    Every sensory runs in its own thread as real sensorys, independently.
    External Sensorys are handled using sockets.
    """

    TURN_ACCURACYFACTOR = math.pi * 10.0 / 180.0
    FULL_TURN_FACTOR = math.pi * 45.0 / 180.0

    DEFAULT_OBSERVATION_DISTANCE = 3.0

    ACTION_TIME = 1.0

    def __init__(self):
        Thread.__init__(self)
        self.name = "WalleServer"

        self.number = 0
        self.azimuth = 0.0  # position sense, azimuth from north
        self.turning_to_object = False  # Are we turning to see an object
        self.hearing_angle = 0.0  # device hears sound from this angle, device looks to its front
        # to the azimuth direction
        self.observation_angle = 0.0  # turn until azimuth is this angle

        self.leftPower = 0.0  # moving
        self.rightPower = 0.0

        self.number = 0
        self.in_axon = Axon()  # global queue for senses to put sensations to walle
        self.out_axon = Axon()  # global queue for walle to put sensations to external senses

        # starting build in capabilities/senses
        # we have capability to move
        if MANUAL:
            self.romeo = ManualRomeo()
        else:
            self.romeo = Romeo()
        # we have hearing (positioning of object using sounds)
        self.hearing = Hearing(self.in_axon)

        # starting tcp server as nerve pathway to external senses to connect
        # we have azimuth sense (our own position detection)
        self.tcpServer = TCPServer(out_axon=self.in_axon, in_axon=self.out_axon, server_address=(HOST, PORT))

        self.running = False
        self.turnTimer = Timer(WalleServer.ACTION_TIME, self.stopTurn)

        self.calibrating = False  # are we calibrating
        self.calibrating_angle = 0.0  # calibrate device hears sound from this angle, device looks to its front
        # self.calibratingTimer = Timer(WalleServer.ACTION_TIME, self.stopCalibrating)
        print "WalleServer: Calibrate version"

    def run(self):
        print "Starting " + self.name

        # starting other threads/senders/capabilities

        self.running = True
        self.hearing.start()
        print "WalleServer: starting TCPServer"
        self.tcpServer.start()

        while self.running:
            sensation = self.in_axon.get()
            print "WalleServer: got sensation from queue " + str(sensation)
            self.process(sensation)
            # as a test, echo everything to external device
            # self.out_axon.put(sensation)

        self.tcpServer.stop()
        self.hearing.stop()

    def stop(self):
        self.running = False

    def process(self, sensation):
        print "WalleServer.process: " + time.ctime(sensation.getTime()) + " " + str(sensation)
        if sensation.getSensationType() == Sensation.SensationType.Drive:
            print "Walleserver.process Sensation.SensationType.Drive"
        elif sensation.getSensationType() == Sensation.SensationType.Stop:
            print "Walleserver.process Sensation.SensationType.Stop"
            self.stop()
        elif sensation.getSensationType() == Sensation.SensationType.Who:
            print "Walleserver.process Sensation.SensationType.Who"
        elif sensation.getSensationType() == Sensation.SensationType.HearDirection:
            print "Walleserver.process Sensation.SensationType.HearDirection"
            # inform external senses that we remember now hearing
            self.out_axon.put(sensation)
            self.hearing_angle = sensation.getHearDirection()
            if self.calibrating:
                print "Walleserver.process Calibrating hearing_angle " + str(
                    self.hearing_angle
                ) + " calibrating_angle " + str(self.calibrating_angle)
            else:
                self.observation_angle = self.add_radian(
                    original_radian=self.azimuth, added_radian=self.hearing_angle
                )  # object in this angle
                print "Walleserver.process create Sensation.SensationType.Observation"
                self.in_axon.put(
                    Sensation(
                        number=++self.number,
                        sensationType=Sensation.SensationType.Observation,
                        observationDirection=self.observation_angle,
                        observationDistance=WalleServer.DEFAULT_OBSERVATION_DISTANCE,
                    )
                )
                # mark hearing sensation to be processed to set direction out of memory, we forget it
                sensation.setDirection(Sensation.Direction.Out)
                # inform external senses that we don't remember hearing any more
                self.out_axon.put(sensation)
        elif sensation.getSensationType() == Sensation.SensationType.Azimuth:
            if not self.calibrating:
                print "Walleserver.process Sensation.SensationType.Azimuth"
                # inform external senses that we remember now azimuth
                # self.out_axon.put(sensation)
                self.azimuth = sensation.getAzimuth()
                self.turn()
        elif sensation.getSensationType() == Sensation.SensationType.Observation:
            if not self.calibrating:
                print "Walleserver.process Sensation.SensationType.Observation"
                # inform external senses that we remember now observation
                self.out_axon.put(sensation)
                self.observation_angle = sensation.getObservationDirection()
                self.turn()
        elif sensation.getSensationType() == Sensation.SensationType.Picture:
            print "Walleserver.process Sensation.SensationType.Picture"
        elif sensation.getSensationType() == Sensation.SensationType.Calibrate:
            print "Walleserver.process Sensation.SensationType.Calibrate"
            if sensation.getMemory() == Sensation.Memory.Working:
                if sensation.getDirection() == Sensation.Direction.In:
                    print "Walleserver.process asked to start calibrating mode"
                    self.calibrating = True
                else:
                    print "Walleserver.process asked to stop calibrating mode"
                    self.calibrating = False
                # ask external senses to to set same calibrating mode
                self.out_axon.put(sensation)
            elif sensation.getMemory() == Sensation.Memory.Sensory:
                if self.calibrating:
                    if self.turning_to_object:
                        print "Walleserver.process turning_to_object, can't start calibrate activity yet"
                    else:
                        # allow requester to start calibration activaties
                        if sensation.getDirection() == Sensation.Direction.In:
                            print "Walleserver.process asked to start calibrating activity"
                            self.calibrating_angle = sensation.getHearDirection()
                            self.hearing.setCalibrating(calibrating=True, calibrating_angle=self.calibrating_angle)
                            sensation.setDirection(Sensation.Direction.In)
                            self.out_axon.put(sensation)
                            # self.calibratingTimer = Timer(WalleServer.ACTION_TIME, self.stopCalibrating)
                            # self.calibratingTimer.start()
                        else:
                            print "Walleserver.process asked to stop calibrating activity"
                            self.hearing.setCalibrating(calibrating=False, calibrating_angle=self.calibrating_angle)
                            # self.calibratingTimer.cancel()
                else:
                    print "Walleserver.process asked calibrating activity WITHOUT calibrate mode, IGNORED"

        elif sensation.getSensationType() == Sensation.SensationType.Capability:
            print "Walleserver.process Sensation.SensationType.Capability"
        elif sensation.getSensationType() == Sensation.SensationType.Unknown:
            print "Walleserver.process Sensation.SensationType.Unknown"

    def turn(self):
        # calculate new power to turn or continue turning
        self.leftPower, self.rightPower = self.getPower()
        if self.turning_to_object:
            print "WalleServer.turn: self.hearing_angle " + str(self.hearing_angle) + " self.azimuth " + str(
                self.azimuth
            )
            print "WalleServer.turn: turn to " + str(self.observation_angle)
            if math.fabs(self.leftPower) < Romeo.MINPOWER or math.fabs(self.rightPower) < Romeo.MINPOWER:
                self.stopTurn()
                print "WalleServer.turn: Turn is ended"
                self.turnTimer.cancel()
            else:
                print "WalleServer.turn: powers adjusted to " + str(self.leftPower) + " " + str(self.rightPower)
                self.number = self.number + 1
                sensation, picture = self.romeo.processSensation(
                    Sensation(
                        number=self.number, sensationType="D", leftPower=self.leftPower, rightPower=self.rightPower
                    )
                )
                self.leftPower = sensation.getLeftPower()  # set motors in opposite power to turn in place
                self.rightPower = sensation.getRightPower()  # set motors in opposite power to turn in place

        else:
            if math.fabs(self.leftPower) >= Romeo.MINPOWER or math.fabs(self.rightPower) >= Romeo.MINPOWER:
                self.turning_to_object = True
                # adjust hearing
                # if turn, don't hear sound, because we are giving moving sound
                # we want hear only sounds from other objects
                self.hearing.setOn(not self.turning_to_object)
                print "WalleServer.turn: powers initial to " + str(self.leftPower) + " " + str(self.rightPower)
                self.number = self.number + 1
                sensation, picture = self.romeo.processSensation(
                    Sensation(
                        number=self.number, sensationType="D", leftPower=self.leftPower, rightPower=self.rightPower
                    )
                )
                self.leftPower = sensation.getLeftPower()  # set motors in opposite power to turn in place
                self.rightPower = sensation.getRightPower()  # set motors in opposite power to turn in place
                self.turnTimer = Timer(WalleServer.ACTION_TIME, self.stopTurn)
                self.turnTimer.start()

    def stopTurn(self):
        self.turning_to_object = False
        self.leftPower = 0.0  # set motors in opposite power to turn in place
        self.rightPower = 0.0
        print "WalleServer.stopTurn: Turn is stopped/cancelled"

        print "WalleServer.stopTurn: powers to " + str(self.leftPower) + " " + str(self.rightPower)

        self.hearing.setOn(not self.turning_to_object)

        self.number = self.number + 1
        # test=Sensation.SensationType.Drive
        sensation, picture = self.romeo.processSensation(
            Sensation(number=self.number, sensationType="D", leftPower=self.leftPower, rightPower=self.rightPower)
        )
        self.leftPower = sensation.getLeftPower()  # set motors in opposite power to turn in place
        self.rightPower = sensation.getRightPower()
        print "WalleServer.stopTurn: powers set to " + str(self.leftPower) + " " + str(self.rightPower)

    #   def stopCalibrating(self):
    #       self.calibrating=False
    #       print "WalleServer.stopCalibrating: Calibrating mode is stopped/cancelled"

    def add_radian(self, original_radian, added_radian):
        result = original_radian + added_radian
        if result > math.pi:
            return -math.pi + (result - math.pi)
        if result < -math.pi:
            return math.pi - (result - math.pi)
        return result

    def getPower(self):
        leftPower = 0.0  # set motor in opposite power to turn in place
        rightPower = 0.0

        if math.fabs(self.observation_angle - self.azimuth) > WalleServer.TURN_ACCURACYFACTOR:
            power = (self.observation_angle - self.azimuth) / WalleServer.FULL_TURN_FACTOR
            if power > 1.0:
                power = 1.0
            if power < -1.0:
                power = -1.0
            if math.fabs(power) < Romeo.MINPOWER:
                power = 0.0
            leftPower = power  # set motorn in opposite power to turn in place
            rightPower = -power
        if math.fabs(leftPower) < Romeo.MINPOWER or math.fabs(rightPower) < Romeo.MINPOWER:
            leftPower = 0.0  # set motors in opposite power to turn in place
            rightPower = 0.0

        # test system has so little power, that we must run it at full speed
        #       if leftPower > Romeo.MINPOWER:
        #           leftPower = 1.0           # set motorn in opposite pover to turn in place
        #           rightPower = -1.0
        #       elif leftPower < -Romeo.MINPOWER:
        #           leftPower = -1.0           # set motorn in opposite pover to turn in place
        #           rightPower = 1.0

        return leftPower, rightPower