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"
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
