def test_daemon_can_receive_event_get_new_path(self):
""" NOTE: As this test is async, and NoseTest does not
support async tests, please check this manually.
"""
logging.info('')
logging.info('---started event test---')
## Setup Nav daemon
nav = Nav()
nav.start()
## Setup client and send info
client = DispatcherClient(port=9002)
client.start()
client.send(9001, 'newPath', {"from" : "124", "to" : "1210"})
logging.info('Requested for new Path from 9002.')
while(True):
pass
## Keep daemons alive for 5 seconds.
time.sleep(5)
client.stop()
nav.stop()
logging.info('---finished event test---')
def test_can_communicate(self):
d = VoiceDaemon()
d.start()
time.sleep(3)
# send test stuff
sender = DispatcherClient(port=9001)
sender.start()
## Dependency injection here
sender.send(d.VOICE_DAEMON_ADDR, 'onData', {"count" : "counting numbers"})
logging.info('finished sending.')
sender.stop()
# stop daemon
d.stop()
def test_daemon_update_pos_from_cruncher_event(self):
logging.info('')
logging.info('---started event test---')
## Setup Nav daemon
nav = Nav()
nav.start()
loc = nav.loc() # get current location as a point
x,y,z,angle = loc.getLocTuple()
expect(x).to_equal(0)
expect(y).to_equal(0)
expect(z).to_equal(0)
expect(angle).to_equal(0)
# Wait for 2 seconds to set-up
time.sleep(2)
## Setup mock cruncher
client = DispatcherClient(port=9004)
client.start()
client.send(9001, 'point', {
"x": 10,
"y": 123,
"z": 456,
"ang": 1,
})
# Wait for 5 seconds to propagate
startTime = time.time()
while(time.time() - startTime < 5):
time.sleep(0.01)
loc = nav.loc() # get current location as a point
x,y,z,angle = loc.getLocTuple()
expect(x).to_equal(10)
expect(y).to_equal(123)
expect(z).to_equal(456)
expect(angle).to_equal(1)
client.stop()
nav.stop()
logging.info('---finished event test---')
def test_generic(self):
## Event handler. Note the use of smokesignal label.
@smokesignal.on('update')
def doSomething(args):
"""This is an event callback. Payload data is passed
as args, and is a JSON object
"""
logging.info('Event triggered: Update!')
logging.info('JSON output: %s' % args)
@smokesignal.on('fun event')
def doSomething(args):
"""This is an event callback. Payload data is passed
as args, and is a JSON object
"""
logging.info('Event triggered: A fun event!')
logging.info('JSON output: %s' % args)
## Create two clients. One at 9001, the other 9002
client1 = DispatcherClient(port=9001)
client2 = DispatcherClient(port=9002)
client1.start()
client2.start()
client1.send(9002, 'update', {"hello" : "world"})
client1.send(9002, 'update', {"something" : "here"})
client1.send(9002, 'fun event', {"count" : "counting numbers"})
logging.info('finished sending.')
## Keep clients alive for 5 seconds. Note how both are unaffected by block.
time.sleep(5)
client1.stop()
client2.stop()
logging.info('Ended. Goodbye!')
pass
def test_generic(self):
## Event handler. Note the use of smokesignal label.
@smokesignal.on('update')
def doSomething(args):
"""This is an event callback. Payload data is passed
as args, and is a JSON object
"""
logging.info('Event triggered: Update!')
logging.info('JSON output: %s' % args)
@smokesignal.on('fun event')
def doSomething(args):
"""This is an event callback. Payload data is passed
as args, and is a JSON object
"""
logging.info('Event triggered: A fun event!')
logging.info('JSON output: %s' % args)
## Create two clients. One at 9001, the other 9002
client1 = DispatcherClient(port=9001)
client2 = DispatcherClient(port=9002)
client1.start()
client2.start()
client1.send(9002, 'update', {"hello": "world"})
client1.send(9002, 'update', {"something": "here"})
client1.send(9002, 'fun event', {"count": "counting numbers"})
logging.info('finished sending.')
## Keep clients alive for 5 seconds. Note how both are unaffected by block.
time.sleep(5)
client1.stop()
client2.stop()
logging.info('Ended. Goodbye!')
pass
class VoiceDaemon:
""" The Voice Daemon is the voice component for easyNav.
Example Code:
daemon = VoiceDaemon()
daemon.start() # Starts the daemon
daemon.stop() # Stops the daemon
"""
def __init__(self):
"""Initializes the daemon.
"""
self.VOICE_DAEMON_ADDR = 9010
self._active = False
self._dispatcherClient = DispatcherClient(port=self.VOICE_DAEMON_ADDR)
self._attachHandlers() ## Attach event handlers here
### Start declare variables here ###
###
###
### End declare variables here ###
logging.info('Voice Daemon instantiated.')
def start(self):
"""Starts the daemon.
"""
self._active = True
## Run tick thread
def runThread():
while(self._active):
self._tick()
self._threadListen = threading.Thread(target=runThread)
self._dispatcherClient.start() # start comms
self._threadListen.start()
logging.info('Voice Daemon: Thread started.')
def stop(self):
self._dispatcherClient.stop() # Stop comms
self._active = False
self._threadListen.join()
### Start De-initialize variables here ###
###
###
### End De-initialize variables here ###
logging.info('Serial Daemon: Thread stopped.')
def _tick(self):
"""Tick function run when daemon is active
"""
#########################################
# <<FILL IN>> #
#########################################
pass
def _attachHandlers(self):
"""Attach event handlers here
"""
###################################################
####### Define event handlers here #######
###################################################
#########################################
# <<FILL IN>> #
#########################################
### Sample handler
@smokesignal.on('onData')
def onDataHandler(messageObj):
""" Event callback for serial data
"""
logging.debug('Oh yay! I got triggered!')
pass
class Nav(object):
""" This is the Nav class, which handles navigation on the Raspberry Pi.
It retrieves remote location information and navigates to the point. In addition,
this class implements the REST API endpoints from the server.
"""
HOST_ADDR = "http://*****:*****@smokesignal.on('newPath')
def onNewPath(args):
logging.debug('Event triggered: Request for new path.')
nodeFrom = json.loads(args.get('payload')).get('from')
nodeTo = json.loads(args.get('payload')).get('to')
if ((nodeTo == None) or (nodeFrom == None)):
logging.error('Received no start / end nodes')
return
# DEPRECATED ------
## reset current location
# self.setPosBySUID(str(nodeFrom))
# /DEPRECATED ------
## Get new path
self.getPathTo(nodeFrom, nodeTo)
# @smokesignal.on('obstacle')
# def onObstacle(args):
# response = int(json.loads(args.get('payload')).get('status'))
# if (response == 0):
# self.obstacle = None
# return # Do not set collision locked
# elif (response == 1):
# self.obstacle = 'FRONT'
# elif (response == 2):
# self.obstacle = 'LEFT'
# elif (response == 3):
# self.obstacle = 'RIGHT'
# # If collision, set to true
# self.collisionLocked = True
@smokesignal.on('point')
def onPoint(args):
"""Update location based on interprocess posting
by cruncher
"""
locInfo = json.loads(args.get('payload'))
x = locInfo.get('x')
y = locInfo.get('y')
z = locInfo.get('z')
angle = locInfo.get('ang')
self.__model['currLoc'] = Point.fromParam(
x=x,
y=y,
z=z,
orientation=angle)
# logging.info(
# '[ NAV ] Internal pos is currently: (x=%s y=%s z=%s ang=%s)' %
# (x,y,z,angle) )
@smokesignal.on('reset')
def onReset(args):
"""If navigation needs to be reset during routing,
use this
"""
self._dispatcherClient.send(9002, 'say', {'text': 'Nav reset.'})
self._resetNavParams() # Reset all navigation params and await new path
logging.debug('Nav has been RESET.')
@smokesignal.on('pause')
def onPause(args):
""" Pause navigation. Useful for stairs / etc.
"""
self.toPause = True
self._dispatcherClient.send(9002, 'say', {'text': 'Nav paused.'})
logging.debug('Nav is PAUSED.')
@smokesignal.on('unpause')
def onPause(args):
""" Unpause navigation. Useful for stairs / etc.
"""
self.toPause = False
self._dispatcherClient.send(9002, 'say', {'text': 'Nav resumed.'})
logging.debug('Nav is RESUMED.')
@smokesignal.on('checkNavigation')
def onPause(args):
""" Unpause navigation. Useful for stairs / etc.
"""
isNavigating = self.isNavigating()
self._dispatcherClient.send(9002, 'checkNavigation', {'isNavigating': isNavigating })
logging.debug('Nav state checked.')
def setPosByXYZ(self, x=0, y=0, z=0, orientation=0):
""" Set position by XYZ
"""
_x = str(x)
_y = str(y)
_z = str(z)
_orientation = str(orientation)
try:
r = requests.post(Nav.HOST_ADDR
+ '/heartbeat/location/?x=' + _x
+ '&y=' + _y
+ '&z=' + _z
+ '&orientation=' + _orientation)
except requests.exceptions.RequestException as e:
logging.error('Oops! Failed to set position by XYZ. Error: %s' % e)
def setPosBySUID(self, suid=0):
"""Set position on server, by SUID
"""
## Get the current old values (orientation workaround)
# self.getPos()
orientation = 0
# orientation = (self.__model['currLoc'])['orientation']
### TODO: Fix orientation bug
## Get the SUID coordinates
try:
r = requests.get(Nav.HOST_ADDR + '/node/?SUID=' + str(suid))
except requests.exceptions.RequestException as e:
logging.error('Oops! Failed to set position by SUID. Error: %s' % e)
response = json.loads(r.text)
## Invalid point exception
if (response == []):
logging.error('Oops. No such SUID on server.')
return
response = response[0]
loc = json.loads(response.get('loc'))
## Set coordinates
x,y,z = loc.get('x'), loc.get('y'), loc.get('z')
self.setPosByXYZ(x,y,z, orientation)
def resetMap(self):
"""Resets the map.
"""
try:
r = requests.delete(Nav.HOST_ADDR + '/map')
logging.info('Map deleted.')
except requests.exceptions.RequestException as e:
logging.error('Oops! Failed to delete map. Is server connected?')
def updateMap(self):
"""Updates the map on server.
"""
try:
r = requests.get(Nav.HOST_ADDR + '/map/update')
# self._dispatcherClient.send(9002, 'say', {'text': 'Map updated.'})
logging.info('Map updated.')
except requests.exceptions.RequestException as e:
logging.error('Oops! Failed to update map. Is server connected?')
def updateMapCustom(self, building, floor):
"""Updates the map on server, with floor and building name.
"""
try:
r = requests.get(Nav.HOST_ADDR + '/map/update?floor=' + floor + '&building=' + building)
# self._dispatcherClient.send(9002, 'say', {'text': 'Map updated.'})
logging.info('Map updated with building ' + building + ' at floor ' + floor)
except requests.exceptions.RequestException as e:
logging.error('Oops! Failed to update map building=%s floor=%s. Is server connected?' % (building, floor))
def getPathTo(self, nodeFrom, nodeTo):
"""Gets shortest path from point from current location, and updates internal
path accordingly.
"""
try:
self._resetNavParams()
r = requests.get(Nav.HOST_ADDR + '/map/shortest/' + str(nodeFrom) + '/' + str(nodeTo) )
self.__model['path'] = Path.fromString(r.text)
self._dispatcherClient.send(9002, 'say', {'text': 'Retrieved new path.'})
logging.info('Retrieved new path.')
except requests.exceptions.RequestException as e:
logging.info('Oops! Failed to retrieve shortest path. Is server connected?')
def _resetNavParams(self):
"""Reset nav flags and relevant info.
"""
self._hasPassedStart = False # Variable to test if start pt has passed
self.achievedNode = -1 # Variable to test if previous point was the SAME point!!
self.__model['path'] = None # Reset path
def path(self):
"""Returns the current path of nav, as a Path instance.
"""
return self.__model['path']
def loc(self):
""" Returns the current location of user, as a Point instance.
"""
return self.__model['currLoc']
def isNavigating(self):
""" Check for existence of path, and use that to detect
isNavigating state
"""
return not (path == None)
def exeLevelNorm(self):
""" Called for run level RUNLVL_NORMAL. Do not call externally.
"""
path = self.path()
pt = self.loc()
## Return if no path set!
if path == None:
logging.debug('No path set.')
return
logging.debug('Point: %s' % pt)
logging.debug('>>>>>> Current target: %s' % path.get())
feedback = path.isOnPath(pt, self.THRESHOLD_DIST, self.THRESHOLD_ANGLE)
status = feedback['status']
angleCorrection = int(angles.r2d(float(feedback['angleCorrection'])))
## Collision detection first
if (self.collisionLocked):
if (self.obstacle == None):
# Unlock collisionLocked
self._dispatcherClient.send(9002, 'say', {'text': 'Obstacle cleared. Move forward!'})
logging.debug('Obstacle cleared. Move forward!')
self.collisionLocked = False
time.sleep(5)
logging.debug('Time to clear obstacle has passed.')
elif (self.obstacle == 'FRONT'):
self._dispatcherClient.send(9002, 'say', {'text': 'Obstacle ahead. Turn left or right!'})
logging.debug('Obstacle ahead. Turn left or right!')
elif (self.obstacle == 'LEFT'):
self._dispatcherClient.send(9002, 'say', {'text': 'Obstacle on the left!'})
logging.debug('Obstacle on the left!')
elif (self.obstacle == 'RIGHT'):
self._dispatcherClient.send(9002, 'say', {'text': 'Obstacle on the right!'})
logging.debug('Obstacle on the right!')
# Do not execute below
return
if (status is Point.REACHED):
##TODO: Implement print to voice
checkpointName = '' ## initialize it first
if (path.isAtDest() is False):
if ((self._hasPassedStart == False) and (self.__model['path'].ref == 0)):
self.__model['path'].next()
self._hasPassedStart = True # So this does not trigger again at start
self.achievedNode = 0
# elif ( (self.__model['path'].ref != 0) and (self.achievedNode == int(self.__model['path'].ref - 1)) ):
else:
if (self.achievedNode == (self.__model['path'].ref - 1)):
self.achievedNode = self.__model['path'].ref # Update to current node before incrementing
##TODO: Fix this tomorrow
try:
currNode = self.__model['path'].get() ## Get current node
checkpointName = currNode.name()
except:
logging.error('Oops. Invalid name?')
self.__model['path'].next()
## Store the last value, i.e. 'delay'
if checkpointName == '':
self._dispatcherClient.send(9002, 'say', {'text': 'Checkpoint reached!'})
else:
self._dispatcherClient.send(9002, 'say', {'text': 'Checkpoint ' + checkpointName + ' reached!'})
logging.debug('checkpoint reached!')
else:
self._dispatcherClient.send(9002, 'say', {'text': 'Destination ' + checkpointName + ' reached!'})
self._resetNavParams() # Reset all navigation params and await new path
logging.debug('Reached destination, done!')
pass
elif (status is Point.MOVE_FORWARD):
self._dispatcherClient.send(9002, 'say', {'text': 'Move forward!'})
logging.debug('move forward!')
pass
elif (status is Point.OUT_OF_PATH):
self._dispatcherClient.send(9002, 'say', {'text': 'Out of path!'})
logging.debug('Out of path!')
## DEPRECATED --------------------------------
# self.getPathTo( self.path().dest().name() )
## /DEPRECATED --------------------------------
pass
elif (status is Point.ALIGNED):
##TODO: Implement print to voice
# self._dispatcherClient.send(9002, 'say', {'text': 'Out of path!'})
logging.debug('Point aligned!')
pass
elif (status is Point.TURN_LEFT):
self._dispatcherClient.send(9002, 'say', {'text': 'Turn left ' + str(angleCorrection) + ' degrees' })
logging.debug('Turn left ' + str(angleCorrection) + ' degrees')
pass
elif (status is Point.TURN_RIGHT):
self._dispatcherClient.send(9002, 'say', {'text': 'Turn right ' + str(angleCorrection) + ' degrees' })
logging.debug('Turn right ' + str(angleCorrection) + ' degrees')
pass
else:
## Oops uncaught feedback
logging.warn('Oops, did we account for all feedback flags?')
pass
pass
# print ('--------------curr node--------', self.__model['path'].ref)
# print 'VAL ACHIEVED--------------------', self.achievedNode
# print 'VAL TARGETED--------------------', self.__model['path'].ref
def exeLevelWarnObstacle(self):
""" Called for run level RUNLVL_WARN_OBSTACLE. Do not call externally.
"""
# TODO: Code warn obstacle stuff
self._dispatcherClient.send(9002, 'say', {'text': 'Near obstacle!'})
logging.warn('Near obstacle!!')
pass
def feedbackCorrection(self):
"""Feedback correction control, called by Nav daemon automatically.
"""
if (self.runLevel == Nav.RUNLVL_NORMAL):
self.exeLevelNorm()
elif (self.runLevel == Nav.RUNLVL_WARNING_OBSTACLE):
self.exeLevelWarnObstacle()
pass
def tick(self, debug=False):
"""Tick function executed every run cycle, called by #run
Param:
debug: If False, do not not update position from server
Instead do dependency injection manually.
"""
if (self.toPause == False):
self.feedbackCorrection()
class Nav(object):
""" This is the Nav class, which handles navigation on the Raspberry Pi.
It retrieves remote location information and navigates to the point. In addition,
this class implements the REST API endpoints from the server.
"""
HOST_ADDR = "http://*****:*****@smokesignal.on('newPath')
def onNewPath(args):
logging.debug('Event triggered: Request for new path.')
nodeTo = args.get('to')
if ((nodeTo == None)):
logging.error('Received no start / end nodes')
return
self.getPathTo(nodeTo)
@smokesignal.on('obstacle')
def onObstacle(args):
##TODO: Implement obstacle detection
pass
def resetMap(self):
"""Resets the map.
"""
r = requests.delete(Nav.HOST_ADDR + '/map')
logging.info('Map deleted.')
def updateMap(self):
"""Updates the map on server.
"""
r = requests.get(Nav.HOST_ADDR + '/map/update')
# self._dispatcherClient.send(9002, 'say', {'text': 'Map updated.'})
logging.info('Map updated.')
def getPos(self):
"""Gets the position from server, and updates internal
coordinates in Nav module.
"""
r = requests.get(Nav.HOST_ADDR + '/heartbeat/location')
self.__model['currLoc'] = Point.fromJson(r.text)
def getPathTo(self, pointId):
"""Gets shortest path from point from current location, and updates internal
path accordingly.
"""
r = requests.get(Nav.HOST_ADDR + '/map/goto/' + str(pointId))
self.__model['path'] = Path.fromString(r.text)
self._dispatcherClient.send(9002, 'say', {'text': 'Retrieved new path.'})
logging.info('Retrieved new path.')
def path(self):
"""Returns the current path of nav, as a Path instance.
"""
return self.__model['path']
def loc(self):
""" Returns the current location of user, as a Point instance.
"""
return self.__model['currLoc']
def exeLevelNorm(self):
""" Called for run level RUNLVL_NORMAL. Do not call externally.
"""
path = self.path()
pt = self.loc()
## Return if no path set!
if path == None:
return
logging.debug('Point: %s' % pt)
logging.debug('Current target: %s' % path.get())
feedback = path.isOnPath(pt, self.THRESHOLD_DIST, self.THRESHOLD_ANGLE)
status = feedback['status']
if (status is Point.REACHED):
##TODO: Implement print to voice
if (path.isAtDest() is False):
self.__model['path'].next()
self._dispatcherClient.send(9002, 'say', {'text': 'Checkpoint reached!'})
logging.debug('checkpoint reached!')
else:
self._dispatcherClient.send(9002, 'say', {'text': 'You have reached your destination!'})
logging.debug('Reached destination, done!')
pass
elif (status is Point.MOVE_FORWARD):
self._dispatcherClient.send(9002, 'say', {'text': 'Move forward!'})
logging.debug('move forward!')
pass
elif (status is Point.OUT_OF_PATH):
self._dispatcherClient.send(9002, 'say', {'text': 'Out of path!'})
logging.debug('Out of path!')
self.getPathTo( self.path().dest().name() )
pass
elif (status is Point.ALIGNED):
##TODO: Implement print to voice
# self._dispatcherClient.send(9002, 'say', {'text': 'Out of path!'})
logging.debug('Point aligned!')
pass
elif (status is Point.TURN_LEFT):
self._dispatcherClient.send(9002, 'say', {'text': 'Turn left!'})
logging.debug('Turn left!')
pass
elif (status is Point.TURN_RIGHT):
self._dispatcherClient.send(9002, 'say', {'text': 'Turn right!'})
logging.debug('Turn right!')
pass
else:
## Oops uncaught feedback
logging.warn('Oops, did we account for all feedback flags?')
pass
pass
def exeLevelWarnObstacle(self):
""" Called for run level RUNLVL_WARN_OBSTACLE. Do not call externally.
"""
# TODO: Code warn obstacle stuff
self._dispatcherClient.send(9002, 'say', {'text': 'Near obstacle!'})
logging.warn('Near obstacle!!')
pass
def feedbackCorrection(self):
"""Feedback correction control, called by Nav daemon automatically.
"""
if (self.runLevel == Nav.RUNLVL_NORMAL):
self.exeLevelNorm()
elif (self.runLevel == Nav.RUNLVL_WARNING_OBSTACLE):
self.exeLevelWarnObstacle()
pass
def tick(self, debug=False):
"""Tick function executed every run cycle, called by #run
Param:
debug: If False, do not not update position from server
Instead do dependency injection manually.
"""
self.getPos() if (debug is False) else True
self.feedbackCorrection()
pass
