def __init__(self, name, init_xy, init_dir, init_battery, \
noise_vel, noise_turn, noise_radio, noise_mag, fail_prob, controller, case=None):
'''
Params: length in no. of cells
breadth in no. of cells
'''
self.name = name
self.xy = [float(i) for i in init_xy] # In meters
self.dir = float(init_dir)
self.mag_dir = float(init_dir)
self.is_alive = True
self.is_moving = False
self.is_backing_off = False
self.has_turned = False
self.command = Command(0,0,0)
self.last_command = Command(0,0,0)
self.__real_command = Command(0,0,0)
self.command_time_cnt = 0
self.noise_velocity = float(noise_vel)
self.noise_turn = float(noise_turn)
self.noise_radio = float(noise_radio)
self.noise_mag = float(noise_mag)
self.noise_fingerprint = [[self.noise_radio/2,0],[0,self.noise_radio/2]]
self.odometer = Odometer(self.noise_velocity, self.noise_turn)
self.radio = Radio(self)
self.mag = Magnetometer(self)
self.has_collided = False
self.is_goal_reached = False
self.fail_prob = fail_prob
self.collision_count = 0
# Records estimated location
self.pf_estimated_xy = np.zeros([1, 2], dtype=np.float32)
self.dr_estimated_xy = np.zeros([1, 2], dtype=np.float32)
self.pf_particles_xy = np.ones([100, 2], dtype=float) * self.xy
self.sig_match_cnt = 0
# Add references to the central controller
self.controller = controller
self.last_goal_dir = np.zeros([1, 2], dtype=np.float32)
self.backoff_time_cnt = 0
if case is not None:
self.sig_db = case.rf_signature_db
class SensorFly(object):
'''
SensorFly node
'''
def __init__(self, name, init_xy, init_dir, init_battery, \
noise_vel, noise_turn, noise_radio, noise_mag, fail_prob, controller, case=None):
'''
Params: length in no. of cells
breadth in no. of cells
'''
self.name = name
self.xy = [float(i) for i in init_xy] # In meters
self.dir = float(init_dir)
self.mag_dir = float(init_dir)
self.is_alive = True
self.is_moving = False
self.is_backing_off = False
self.has_turned = False
self.command = Command(0,0,0)
self.last_command = Command(0,0,0)
self.__real_command = Command(0,0,0)
self.command_time_cnt = 0
self.noise_velocity = float(noise_vel)
self.noise_turn = float(noise_turn)
self.noise_radio = float(noise_radio)
self.noise_mag = float(noise_mag)
self.noise_fingerprint = [[self.noise_radio/2,0],[0,self.noise_radio/2]]
self.odometer = Odometer(self.noise_velocity, self.noise_turn)
self.radio = Radio(self)
self.mag = Magnetometer(self)
self.has_collided = False
self.is_goal_reached = False
self.fail_prob = fail_prob
self.collision_count = 0
# Records estimated location
self.pf_estimated_xy = np.zeros([1, 2], dtype=np.float32)
self.dr_estimated_xy = np.zeros([1, 2], dtype=np.float32)
self.pf_particles_xy = np.ones([100, 2], dtype=float) * self.xy
self.sig_match_cnt = 0
# Add references to the central controller
self.controller = controller
self.last_goal_dir = np.zeros([1, 2], dtype=np.float32)
self.backoff_time_cnt = 0
if case is not None:
self.sig_db = case.rf_signature_db
def setMoveCommand(self, command_params):
'''
Params: turn - the angle to turn in degrees
time - the time to setMoveCommand in seconds
'''
self.is_moving = True
[turn, time, velocity] = command_params
# Set the command_params to be executed now
self.command.time = float(time)
self.command.turn = float(turn)
self.command.velocity = float(velocity)
self.__real_command = copy.deepcopy(self.command)
self.__real_command.velocity = self.odometer.velocity(velocity)
self.__real_command.turn = self.odometer.turn(turn)
self.command_time_cnt = self.__real_command.time
# Store the last executed command_params
self.last_command = copy.deepcopy(self.__real_command)
def update(self, deltick, arena):
'''
Params: deltick - the time tick in seconds
'''
# Move
if self.is_moving:
# Turn instantly in first tick
if (self.command_time_cnt == self.__real_command.time):
self.dir = (self.dir + self.__real_command.turn) % 360
self.has_turned = True
else:
self.has_turned = False
# Get the magnetometer reading for direction
self.mag_dir = self.mag.getDirection()
# Set the time taking into account the tick size
if (self.command_time_cnt - deltick <= 0):
deltime = self.command_time_cnt
self.command_time_cnt = 0
else:
self.command_time_cnt = self.command_time_cnt - deltick
deltime = deltick
oldpos = self.xy
newx = self.xy[0] + ((self.__real_command.velocity * deltime) * \
math.cos(math.radians(self.dir)))
newy = self.xy[1] + ((self.__real_command.velocity * deltime) * \
math.sin(math.radians(self.dir)))
newpos = [newx, newy]
is_collision = self.isCollision(oldpos,newpos,arena)
# If no collision setMoveCommand to new point
if is_collision == False:
self.xy = [newx, newy]
else: # Else stop at object boundary
self.is_moving = False
self.command_time_cnt = 0
self.is_backing_off = False
# If the movement is complete
if (self.command_time_cnt == 0):
self.is_moving = False
self.is_backing_off = False
else:
deltime = 0
def isCollision(self, oldpos, newpos, arena):
'''
Compute if collision
'''
[oldX, oldY] = arena.gridmap.xytocell(oldpos)
[newX, newY] = arena.gridmap.xytocell(newpos)
covCells = self.bresenhamLine(oldX, oldY, newX, newY)
for c in covCells:
if arena.gridmap.map[c[0]][c[1]] == arena.gridmap.v_obstacle: # hit obstacle
self.has_collided = True
self.collision_count += 1
p = random.random()
if (p < self.fail_prob):
self.is_alive = False
return True
self.has_collided = False
return False
def bresenhamLine(self,x,y,x2,y2):
# Brensenham line algorithm
steep = 0
coords = []
dx = abs(x2 - x)
if (x2 - x) > 0: sx = 1
else: sx = -1
dy = abs(y2 - y)
if (y2 - y) > 0: sy = 1
else: sy = -1
if dy > dx:
steep = 1
x,y = y,x
dx,dy = dy,dx
sx,sy = sy,sx
d = (2 * dy) - dx
for _ in range(0,dx):
if steep: coords.append((y,x))
else: coords.append((x,y))
while d >= 0:
y = y + sy
d = d - (2 * dx)
x = x + sx
d = d + (2 * dy)
coords.append((x2,y2))
return coords
def getRadioSignature(self):
# anchors = [0,1,2,3,5,6]
# signature = np.zeros(6,np.float32)
# for i,a in enumerate(anchors):
# [x,y] = self.xytocell(self.xy)
# signature[i] = np.random.choice(self.sig_db[(y,x,a)])
signature = np.random.multivariate_normal(self.xy, self.noise_fingerprint)
# self.radio.rss(self.controller.anchors)
return signature
def xytocell(self, xy):
X = int(round(xy[0]))
Y = int(round(xy[1]))
if X < 0:
X = 0
elif X >= 10:
X = 10-1
if Y < 0:
Y=0
elif Y >= 7:
Y=7-1
return [X,Y]
def testDiff():
o3 = Odometer(3)
o4 = Odometer(3)
o3.next_reading(4)
o3.prev_reading(6)
assert o3.diff(o4) == 2
assert o4.diff(o3) == 82
def test_diff(self):
odo = Odometer(2)
self.o2.next_reading(3)
odo.next_reading(5)
self.assertEqual(self.o2.diff(odo), 2)
self.assertEqual(odo.diff(self.o2), 34)
def __init__(self, a_star=None):
self.a_star = a_star or AStar()
self.encoders = Encoders(self.a_star)
self.odometer = Odometer(self.encoders)
self.motors = Motors(self.a_star, self.odometer)
self.camera = Camera()
self.log = logging.getLogger('romi')
self.motors.stop()
from odometer import Odometer
o1 = Odometer(5)
o2 = Odometer(2)
def testLength():
length1 = o1.LENGTH
length2 = o2.LENGTH
assert length1 == 126
assert length2 == 36
def testNextReading():
o1.next_reading(10)
assert o1.readings[o1.position] == 12368
o2.next_reading(4)
assert o2.readings[o2.position] == 16
def textPrevReading():
o1.prev_reading(6)
assert o1.readings[o1.position] == 14
o2.prev_reading(2)
assert o2.readings[o2.position] == 12349
def testDiff():
o3 = Odometer(3)
def test_ascending(self):
self.assertEqual(Odometer.is_ascending(11), False)
self.assertEqual(Odometer.is_ascending(4689), True)
self.assertEqual(Odometer.is_ascending(2), True)
self.assertEqual(Odometer.is_ascending(123456789), True)
def setUp(self):
self.o1 = Odometer(1)
self.o2 = Odometer(2)
class TestOdometer(unittest.TestCase):
def setUp(self):
self.o1 = Odometer(1)
self.o2 = Odometer(2)
def test_ascending(self):
self.assertEqual(Odometer.is_ascending(11), False)
self.assertEqual(Odometer.is_ascending(4689), True)
self.assertEqual(Odometer.is_ascending(2), True)
self.assertEqual(Odometer.is_ascending(123456789), True)
def test_length(self):
length = self.o1.LENGTH
self.assertEqual(length, 0)
length = self.o2.LENGTH
self.assertEqual(length, 36)
def test_next_reading(self):
#getting back the starting pos after adding length
length = self.o2.LENGTH
self.o2.next_reading(length)
self.assertEqual(self.o2.readings[self.o2.position], 12)
#adding 1 to the above obtained reading
self.o2.next_reading(1)
self.assertEqual(self.o2.readings[self.o2.position], 13)
#adding 7 to the above reading
self.o2.next_reading(7)
self.assertEqual(self.o2.readings[self.o2.position], 23)
def test_prev_reading(self):
length = self.o2.LENGTH
self.o2.prev_reading(length)
self.assertEqual(self.o2.readings[self.o2.position], 12)
self.o2.prev_reading(1)
self.assertEqual(self.o2.readings[self.o2.position], 89)
self.o2.prev_reading(10)
self.assertEqual(self.o2.readings[self.o2.position], 49)
def test_diff(self):
odo = Odometer(2)
self.o2.next_reading(3)
odo.next_reading(5)
self.assertEqual(self.o2.diff(odo), 2)
self.assertEqual(odo.diff(self.o2), 34)
class Robot(object):
def __init__(self, a_star=None):
self.a_star = a_star or AStar()
self.encoders = Encoders(self.a_star)
self.odometer = Odometer(self.encoders)
self.motors = Motors(self.a_star, self.odometer)
self.camera = Camera()
self.log = logging.getLogger('romi')
self.motors.stop()
def set_speed_target(self, left, right):
self.motors.set_speed_target(left, right)
def stop(self):
if self.is_romi_board_connected():
self.motors.stop()
def get_camera_frame(self):
return self.camera.get_camera_frame()
def read_buttons(self):
return self.a_star.read_buttons()
def play_welcome_message(self):
pattern = (LED1, LED2, LED3, LED1, LED2, LED3, NO_LED, ALL_LEDS, NO_LED, ALL_LEDS, NO_LED)
for leds in pattern:
self.a_star.leds(*leds)
time.sleep(0.2)
def play_goodbye_message(self):
pattern = (ALL_LEDS, (1, 1, 0), (1, 0, 0), NO_LED)
for leds in pattern:
self.a_star.leds(*leds)
time.sleep(0.4)
def get_position_xy(self):
situation = self.odometer.get_situation()
return situation.x, situation.y
def get_encoders(self):
return self.encoders.read_encoders()
def get_yaw(self):
return self.odometer.get_situation().yaw
def get_distance(self):
return self.odometer.get_situation().dist
def get_speed(self):
return self.odometer.get_situation().dist
def set_leds(self, red, yellow, green):
self.a_star.leds(red, yellow, green)
def reset_odometry(self):
self.odometer.reset_odometry()
def play_notes(self, notes):
self.a_star.play_notes(notes)
def get_battery(self):
try:
mv, = self.a_star.read_battery_millivolts()
return mv
except IOError:
return None
def is_romi_board_connected(self):
try:
self.a_star.read_battery_millivolts()
return True
except IOError:
return False
def rotate(self, angle, speed):
speed = self._cap(speed)
self.motors.rotate(angle, speed)
def move_forward(self, distance, speed):
speed = self._cap(speed)
self.motors.move_forward(distance, speed)
def _cap(self, x):
if x > 1:
return 1
if x < -1:
return -1
return x
