def __init__(self, slam, mapconfig, ROBOT_SIZE_METERS, offset_in_scan,
min_distance, commands):
"""
MAP_SIXE_PIXELS: map size in pixel
MAP_SIZE_METERS: map size in meters
ROBOT_SZIE_METERS: robot size in meters
offset_in_scan: values to check in a scan for obstacles from the center
min_distance: minimum distance to keep to obstacles
commans: costants for commands
"""
self.commands = commands
threading.Thread.__init__(self)
self.slam = slam
self.mapconfig = mapconfig
self.ROBOT_SIZE_METERS = ROBOT_SIZE_METERS
self.mapbytes = self.createMap()
self.recalculate = False
self.offset_in_scan = offset_in_scan
self.min_distance = min_distance
self.router = TentacleRouter(mapconfig, ROBOT_SIZE_METERS,
min_distance)
def __init__(self, slam, mapconfig, ROBOT_SIZE_METERS, offset_in_scan, min_distance, commands):
"""
MAP_SIXE_PIXELS: map size in pixel
MAP_SIZE_METERS: map size in meters
ROBOT_SZIE_METERS: robot size in meters
offset_in_scan: values to check in a scan for obstacles from the center
min_distance: minimum distance to keep to obstacles
commans: costants for commands
"""
self.commands = commands
threading.Thread.__init__(self)
self.slam = slam
self.mapconfig = mapconfig
self.ROBOT_SIZE_METERS = ROBOT_SIZE_METERS
self.mapbytes = self.createMap()
self.recalculate = False
self.offset_in_scan = offset_in_scan
self.min_distance = min_distance
self.router = TentacleRouter(mapconfig, ROBOT_SIZE_METERS, min_distance)
class Navigation(threading.Thread):
route_lock = threading.Condition()
route = None
target = None
terminated = False
def __init__(self, slam, mapconfig, ROBOT_SIZE_METERS, offset_in_scan, min_distance, commands):
"""
MAP_SIXE_PIXELS: map size in pixel
MAP_SIZE_METERS: map size in meters
ROBOT_SZIE_METERS: robot size in meters
offset_in_scan: values to check in a scan for obstacles from the center
min_distance: minimum distance to keep to obstacles
commans: costants for commands
"""
self.commands = commands
threading.Thread.__init__(self)
self.slam = slam
self.mapconfig = mapconfig
self.ROBOT_SIZE_METERS = ROBOT_SIZE_METERS
self.mapbytes = self.createMap()
self.recalculate = False
self.offset_in_scan = offset_in_scan
self.min_distance = min_distance
self.router = TentacleRouter(mapconfig, ROBOT_SIZE_METERS, min_distance)
def run(self):
'''
Recalcualtes a new route if necessary
'''
self.running = True
time.sleep(15)
self.recalculate = True
while(self.running):
if(self.recalculate):
self.mapbytes = self.createMap()
self.position = self.slam.getpos()
start = time.time()
print "Searching at: ", self.position
route = self.router.getRoute(self.position, self.mapbytes)
if(route == None): self.terminated = True
else: print "Target found in %fs: %f,%f" % (time.time()-start,route[0][0], route[0][1])
self.route_lock.acquire()
self.route = route
self.recalculate = False
self.route_lock.release()
else:
self.route_lock.acquire()
self.route_lock.wait()
self.route_lock.release()
print "Navigation stoped"
def update(self, scan):
"""
return a move command based on the navigation and the scan
or None if the navigationis not able to find a new target
"""
if(self.terminated): return None
command = self.getCommand()
if(command == self.commands.MOVE_FORWARD):
#check scan for obstacles in front
if(self.checkTrajectory(scan, self.offset_in_scan, self.min_distance)== False):
#recalcualte route
self.route_lock.acquire()
self.recalculate = True
self.target = None
self.route_lock.notify()
self.route_lock.release()
return self.commands.WAIT
#turn and stay should be always possible
return command
def getCommand(self):
"""
returns the next movement to perform
"""
#check if currently recalculating ==> wait
if(self.recalculate): return self.commands.WAIT
#check if no route is available ==> turn around to expand map
if(self.route == None): return self.commands.TURN_RIGHT
position = self.slam.getpos()
#check if target is reached and if new targets are available
while(self.reachedTarget(self.target, position)):
if(len(self.route)==0):
#recalcualte route
self.recalculate = True
self.route_lock.acquire()
self.route_lock.notify()
self.route_lock.release()
return self.commands.WAIT
else:
self.target = self.route.popleft()
#chek if it is not necessary to turn
destangle = self.getAngle(self.target, position)%360
current = position[2] % 360
angle = destangle - current
if(angle>180):angle = angle - 360
if(angle<-180):angle = angle + 360
if(math.fabs(angle) < ANGLE_TOLERANCE_DEGREE):
return self.commands.MOVE_FORWARD
#turn
if(angle > 0):
return self.commands.TURN_RIGHT
else:
return self.commands.TURN_LEFT
def reachedTarget(self, target, position):
"""
Checks if the position is in the target range
"""
if(target == None): return True
xd = target[0]-position[0]
yd = target[1]-position[1]
dist = math.sqrt(xd*xd+yd*yd)
return dist < TARGET_TOLERANCE_MM
def getAngle(self, target, position):
"""
Calcualtes the angel of the line between the position and the target
target: target position
position: robot position
return: anngle of the trajectory
"""
x = float(target[0] - position[0])
y = float(target[1] - position[1])
if(x == 0 and y == 0):
return 0
angle = math.degrees(math.acos(x/math.sqrt(x*x+y*y)))
if(y<0):return -angle
return angle
def checkTrajectory(self, scan, offset, min_distance):
"""
Checks if the Trajectory in front is free.
scan: current scan with center on the trajectory
offset: values to check in scan for obstacles
min_distance: minimum distance to obstacles in the trajectory
return: True if Trajecotry is free
"""
center = len(scan)/2
for i in range(center-offset, center+offset):
if(scan[i] < min_distance and scan[i] > 0):
return False
return True
def createMap(self):
"""
creates the map and stores it a byte array.
"""
# Create a byte array to receive the computed maps
mapbytes = bytearray(self.mapconfig.SIZE_PIXELS * self.mapconfig.SIZE_PIXELS)
# Get final map
self.slam.getmap(mapbytes)
return mapbytes
def stop(self):
"""
Stops navigation.
"""
self.running = False
self.recalculate = False
self.route_lock.acquire()
self.route_lock.notify()
self.route_lock.release()
def getmapbytes(self):
"""
Returns the last received mapbytes
"""
if(self.mapbytes == None):
self.mapbytes = self.createMap()
return self.mapbytes
class Navigation(threading.Thread):
route_lock = threading.Condition()
route = None
target = None
terminated = False
def __init__(self, slam, mapconfig, ROBOT_SIZE_METERS, offset_in_scan,
min_distance, commands):
"""
MAP_SIXE_PIXELS: map size in pixel
MAP_SIZE_METERS: map size in meters
ROBOT_SZIE_METERS: robot size in meters
offset_in_scan: values to check in a scan for obstacles from the center
min_distance: minimum distance to keep to obstacles
commans: costants for commands
"""
self.commands = commands
threading.Thread.__init__(self)
self.slam = slam
self.mapconfig = mapconfig
self.ROBOT_SIZE_METERS = ROBOT_SIZE_METERS
self.mapbytes = self.createMap()
self.recalculate = False
self.offset_in_scan = offset_in_scan
self.min_distance = min_distance
self.router = TentacleRouter(mapconfig, ROBOT_SIZE_METERS,
min_distance)
def run(self):
'''
Recalcualtes a new route if necessary
'''
self.running = True
time.sleep(15)
self.recalculate = True
while (self.running):
if (self.recalculate):
self.mapbytes = self.createMap()
self.position = self.slam.getpos()
start = time.time()
print "Searching at: ", self.position
route = self.router.getRoute(self.position, self.mapbytes)
if (route == None): self.terminated = True
else:
print "Target found in %fs: %f,%f" % (
time.time() - start, route[0][0], route[0][1])
self.route_lock.acquire()
self.route = route
self.recalculate = False
self.route_lock.release()
else:
self.route_lock.acquire()
self.route_lock.wait()
self.route_lock.release()
print "Navigation stoped"
def update(self, scan):
"""
return a move command based on the navigation and the scan
or None if the navigationis not able to find a new target
"""
if (self.terminated): return None
command = self.getCommand()
if (command == self.commands.MOVE_FORWARD):
#check scan for obstacles in front
if (self.checkTrajectory(scan, self.offset_in_scan,
self.min_distance) == False):
#recalcualte route
self.route_lock.acquire()
self.recalculate = True
self.target = None
self.route_lock.notify()
self.route_lock.release()
return self.commands.WAIT
#turn and stay should be always possible
return command
def getCommand(self):
"""
returns the next movement to perform
"""
#check if currently recalculating ==> wait
if (self.recalculate): return self.commands.WAIT
#check if no route is available ==> turn around to expand map
if (self.route == None): return self.commands.TURN_RIGHT
position = self.slam.getpos()
#check if target is reached and if new targets are available
while (self.reachedTarget(self.target, position)):
if (len(self.route) == 0):
#recalcualte route
self.recalculate = True
self.route_lock.acquire()
self.route_lock.notify()
self.route_lock.release()
return self.commands.WAIT
else:
self.target = self.route.popleft()
#chek if it is not necessary to turn
destangle = self.getAngle(self.target, position) % 360
current = position[2] % 360
angle = destangle - current
if (angle > 180): angle = angle - 360
if (angle < -180): angle = angle + 360
if (math.fabs(angle) < ANGLE_TOLERANCE_DEGREE):
return self.commands.MOVE_FORWARD
#turn
if (angle > 0):
return self.commands.TURN_RIGHT
else:
return self.commands.TURN_LEFT
def reachedTarget(self, target, position):
"""
Checks if the position is in the target range
"""
if (target == None): return True
xd = target[0] - position[0]
yd = target[1] - position[1]
dist = math.sqrt(xd * xd + yd * yd)
return dist < TARGET_TOLERANCE_MM
def getAngle(self, target, position):
"""
Calcualtes the angel of the line between the position and the target
target: target position
position: robot position
return: anngle of the trajectory
"""
x = float(target[0] - position[0])
y = float(target[1] - position[1])
if (x == 0 and y == 0):
return 0
angle = math.degrees(math.acos(x / math.sqrt(x * x + y * y)))
if (y < 0): return -angle
return angle
def checkTrajectory(self, scan, offset, min_distance):
"""
Checks if the Trajectory in front is free.
scan: current scan with center on the trajectory
offset: values to check in scan for obstacles
min_distance: minimum distance to obstacles in the trajectory
return: True if Trajecotry is free
"""
center = len(scan) / 2
for i in range(center - offset, center + offset):
if (scan[i] < min_distance and scan[i] > 0):
return False
return True
def createMap(self):
"""
creates the map and stores it a byte array.
"""
# Create a byte array to receive the computed maps
mapbytes = bytearray(self.mapconfig.SIZE_PIXELS *
self.mapconfig.SIZE_PIXELS)
# Get final map
self.slam.getmap(mapbytes)
return mapbytes
def stop(self):
"""
Stops navigation.
"""
self.running = False
self.recalculate = False
self.route_lock.acquire()
self.route_lock.notify()
self.route_lock.release()
def getmapbytes(self):
"""
Returns the last received mapbytes
"""
if (self.mapbytes == None):
self.mapbytes = self.createMap()
return self.mapbytes