def __findNAOOnInit(self):
""" Assumes the NAO is positioned approximately in front of the laser scanner """
mincost = 1000
thenao = None
for robot in self.Robots:
cost = numpy.fabs(robot.Bearing) + robot.R / 100.0
if cost < mincost:
mincost = cost
thenao = robot
if thenao != None:
self.NaoX = thenao.X
self.NaoY = thenao.Y
self.NaoPreviousX = self.NaoX
self.NaoPreviousY = self.NaoY
self.NaoOrientation, sigmaOrientation = self.__determineOrientation(
thenao)
self.NaoPreviousO = self.NaoOrientation
self.PreviousTime = 0
self.NAO = NAO(thenao)
self.NAO.Orientation = self.NaoOrientation
self.NAO.VX = self.NaoVelocityX
self.NAO.VY = self.NaoVelocityY
self.NAO.VOrientation = self.NaoVelocityOrientation
self.NAO.V = self.NaoVelocity
else:
self._clearNAOLocation()
def __updateNAO(self, newnao):
""" """
self.NaoPreviousX = self.NaoX
self.NaoPreviousY = self.NaoY
self.NaoPreviousOrientation = self.NaoOrientation
self.NaoX = newnao.X
self.NaoY = newnao.Y
self.NaoVelocityX = (self.NaoX - self.NaoPreviousX) / (
self.Time - self.PreviousTime)
self.NaoVelocityY = (self.NaoY - self.NaoPreviousY) / (
self.Time - self.PreviousTime)
self.NaoVelocity = math.sqrt(self.NaoVelocityX**2 +
self.NaoVelocityY**2)
self.NaoOrientation, sigmaOrientation = self.__determineOrientation(
newnao)
# I need to be careful with the orientation velocity because it can wrap around 0->PI or PI->0, this produces a huge velocity and kills localisation
# case 1: previous is 0.1 this is 3.1 => diff = 3
if (self.NaoOrientation - self.NaoPreviousOrientation) > numpy.pi / 2:
deltaOrientation = self.NaoOrientation - numpy.pi - self.NaoPreviousOrientation
# case 2: previous is 3.1 this is 0.1 => diff = -3
elif (self.NaoOrientation -
self.NaoPreviousOrientation) < -numpy.pi / 2:
deltaOrientation = self.NaoOrientation - (
self.NaoPreviousOrientation - numpy.pi)
else:
deltaOrientation = self.NaoOrientation - self.NaoPreviousOrientation
self.NaoVelocityOrientation = deltaOrientation / (self.Time -
self.PreviousTime)
self.PreviousTime = self.Time
self.NAO = NAO(newnao)
self.NAO.Orientation = self.NaoOrientation
self.NAO.VX = self.NaoVelocityX
self.NAO.VY = self.NaoVelocityY
self.NAO.VOrientation = self.NaoVelocityOrientation
self.NAO.V = self.NaoVelocity
R = numpy.sqrt(self.NAO.X**2 + self.NAO.Y**2)
self.NAO.sigmaX = 0.015 * R
self.NAO.sigmaY = 0.015 * R
self.NAO.sigmaOrientation = sigmaOrientation
def _clearNAOLocation(self):
""" """
self.NaoX = 0
self.NaoY = 0
self.NaoOrientation = 0
self.NaoVelocityX = 0
self.NaoVelocityY = 0
self.NaoVelocityOrientation = 0
self.NaoVelocity = 0
self.NaoPreviousX = 0
self.NaoPreviousY = 0
self.NaoPreviousOrientation = 0
self.ShapeX = numpy.array([0, 0])
self.ShapeY = numpy.array([0, 0])
self.slope = 0
self.intercept = 0
self.NAO = NAO()
self.PreviousNAO = self.NAO
def __init__(self):
self.nao = NAO(self)
self.kinect = Kinect()
self.xaalproxy = xAALProxy()
class Controller:
def __init__(self):
self.nao = NAO(self)
self.kinect = Kinect()
self.xaalproxy = xAALProxy()
def run(self):
while raw_input("continu ? (Y/N)") == "Y":
if self.kinect.fallDetection():
self.nao.say("detecter la chute")
self.nao.say("detecter la chute")
self.nao.moveToPerson()
self.nao.verifyPersonState(5)
if not self.nao.getVerifyState() % 2:
self.nao.say("Ne t'inquiete pas, je vais demander a quelqu'un pour t'aider")
print "verifystate ", self.nao.getVerifyState()
self.scenario()
self.nao.response(0)
self.nao.say("merci d'etre venu")
else:
self.nao.say("tres bien")
def waitPersonResponse(self, timeout):
count = 0
self.nao.resetVerifyState()
while not self.nao.getVerifyState():
time.sleep(3)
count += 1
if (timeout != 0) and (count > timeout):
print "waitpersonresponse timeout!"
return False
print "getpersonresponse"
return self.nao.getVerifyState() % 2
def sendMail(self):
link = self.webRTCaddress()
topic = "nao_robot/camera/top/camera/image_raw"
text = "please see the video from this address " + link
text = text + " with topic name " + topic
mail = Mail(text)
mail.sendMail()
def webRTCaddress(self):
ni.ifaddresses("wlan0")
ip = ni.ifaddresses("wlan0")[2][0]["addr"] + ":8080"
print "HostIP : ", ip
return ip
def scenario(self):
self.smartDeviceAction("shutterleft", "up")
self.smartDeviceAction("shutterright", "up")
self.smartDeviceAction("lamp1", "on")
self.smartDeviceAction("lamp2", "on")
self.smartDeviceAction("lamp3", "on")
self.nao.say("porte ouverte, volet remonte")
self.smartDeviceAction("switch", "off")
self.nao.say("j'ai eteint l'electricite")
try:
self.sendMail()
except:
print "mail send failed"
self.nao.say("j'ai envoye le mail a vos proches")
self.smartDeviceAction(
"mobilephone", "inform", "msg", "J'ai detecte un probleme. Votre ami a fait un malaise. Venez l'aider."
)
self.nao.say("message vocal envoye")
def smartDeviceAction(self, device, action, key=None, value=None):
self.xaalproxy.sendmsg(device, action, key, value)
from time import sleep
import bufhelp
import FieldTrip
from NAO import NAO
import warnings
warnings.filterwarnings('ignore')
# Configuring NAO robot IP and its port, initializing NAO object and training 3 tasks
robotIP = "192.168.10.101"
robotPort = 9559
Nao = NAO(robotIP, robotPort)
for i in range(3):
Nao.trainTask()
# Buffer interfacing functions
def sendEvent(event_type, event_value=1, offset=0):
e = FieldTrip.Event()
e.type = event_type
e.value = event_value
if offset > 0:
sample, bla = ftc.poll(
) #TODO: replace this with a clock-sync based version
e.sample = sample + offset + 1
ftc.putEvents(e)
hostname = 'localhost'
port = 1972
(ftc, hdr) = bufhelp.connect(hostname, port)
# Wait until the buffer connects correctly and returns a valid header
