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]
Exemple #3
0
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)
Exemple #5
0
    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()
Exemple #6
0
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)
Exemple #10
0
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