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