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]