def __init__(self, map_grid: np.ndarray, max_history=10, start_direction=None, error: float = 0.001): self.__map = map_grid self.__max_history = max_history self.__y = np.random.randint(low=0, high=self.__map.shape[0] - 1) self.__x = np.random.randint(low=0, high=self.__map.shape[1] - 1) self.__rel_y = 0 self.__rel_x = 0 self.error = error if start_direction is None: self.__direction = direction.Direction( np.random.randint(low=0, high=3)) elif 0 <= start_direction <= 3: self.__direction = direction.Direction(start_direction) else: raise ValueError("Invalid start direction") self.__start_direction = self.__direction log.info("Starting at direction", self.__direction) self.__history = collections.deque(maxlen=self.__max_history) self.__history_error = collections.deque(maxlen=self.__max_history) self.__history.append( (direction.RelativeDirection.FRONT, self.get_color(), 0, 0)) self.__history_error.append( (direction.RelativeDirection.FRONT, self.change_measurement(self.get_color()), 0, 0))
def plot_vehicle(self): location = self.__vehicle.location() arrow_dir = direction.Direction(location[2]).xy() plt.arrow(location[0] - arrow_dir[0] * 0.7, location[1] - arrow_dir[1] * 0.7, arrow_dir[0] * 0.2, arrow_dir[1] * 0.2, head_width=0.3, head_length=0.3)
def history_to_digraph(history: np.ndarray, start_direction: tp.Union[direction.Direction, int]): """ Convert the history of a vehicle to a graph :param history: vehicle history (numpy array) :param start_direction: vehicle orientation (guess) :return: DiGraph """ graph = nx.DiGraph() for hist_ind in range(history.shape[0]): graph.add_node(hist_ind, color=color.Color(history[hist_ind, 1])) for hist_ind in range(1, history.shape[0]): relative_dir = direction.RelativeDirection(history[hist_ind, 0]) abs_dir = direction.Direction((relative_dir + start_direction) % 4) graph.add_edge(hist_ind - 1, hist_ind, direction=abs_dir) return graph
def move_unbound(self): """ Move in a way not bounded by the map edges :return: - """ d = direction.Direction(np.random.randint(low=0, high=3)) self.__direction = d dx, dy = d.xy() self.__x = (self.__x + dx) % self.__map.shape[ 0] # modulus works with negative numbers -1%10 = 9 self.__y = (self.__y + dy) % self.__map.shape[1] rel_dir = direction.RelativeDirection( (self.__direction - self.__start_direction) % 4) rel_dx, rel_dy = rel_dir.xy() self.__rel_x += rel_dx self.__rel_y += rel_dy self.__history.append( (rel_dir, self.get_color(), self.__rel_x, self.__rel_y)) self.__history_error.append( (rel_dir, self.change_measurement(self.get_color()), self.__rel_x, self.__rel_y))
def __init__(self, raw_media, parent_sdp=None): assert type(raw_media) is str or type(raw_media) is raw.media.Media assert parent_sdp is None or type( parent_sdp) is session_description.SessionDescription if type(raw_media) == str: self.__raw_media = raw.media.Media(raw_media) else: self.__raw_media = raw_media self.__parent_sdp = parent_sdp #self.__pt_reserver = self.__parent_sdp.pt_reserver if self.__parent_sdp else PayloadTypeReserver() self.__ice = ice.Ice(self.__raw_media.attributes) self.__direction = direction.Direction(self.__raw_media.attributes) self.__sdp_codec_collection = sdp_codec.SdpCodecCollection( self.__raw_media) self.__mid = mid.Mid(self.__raw_media.attributes) self.__crypto = crypto.Crypto(self.__raw_media.attributes) self.__streams = stream.StreamCollection(self.__raw_media.attributes) self.__rtcp_mux = rtcp.RtcpMux(self.__raw_media.attributes) self.__rtcp = rtcp.Rtcp(self.__raw_media.attributes) self.__ptime = ptime.Ptime(self.__raw_media.attributes) self.__silence_supp = silence_supp.SilenceSupp( self.__raw_media.attributes)
def locate(m: np.ndarray, history: np.ndarray, v: vehicle.Vehicle = None, skip: tp.Union[int, tp.List[int]] = None) \ -> tp.Tuple[int, int, int, np.ndarray]: """ Vehicle locator algorithm :param m: map (numpy array) :param history: vehicle history (numpy array) :param v: vehicle object for debugging :param skip: skip these indexes :return: number of matches, located x, located y """ start_time = time.perf_counter() skip_list = [] # Check the skip values if skip is None: pass elif type(skip) is int: skip_list.append(skip) elif type(skip) is list: skip_list.extend(skip) else: raise ValueError("Invalid skip input") for index in skip_list: if not 0 <= index < history.shape[0]: raise ValueError("Invalid skip index", index) # Enable debug output if the vehicle is given debug = (v is not None) if debug: end_x, end_y, end_dir = v.location() else: end_x = -9000 end_y = -9000 if debug: log.debug("History:") for y in range(history.shape[0]): log.debug(direction.RelativeDirection(history[y, 0]), color.Color(history[y, 1]), history[y, 2], history[y, 3]) if (skip_list is not None) and (0 in skip_list): possible_start_loc = np.ones(m.shape, dtype=bool) else: possible_start_loc = np.equal(m, history[0, 1]) possible_loc = np.zeros(m.shape, dtype=bool) # Iterate map for y in range(m.shape[0]): for x in range(m.shape[1]): # If start location is possible if possible_start_loc[y, x]: if debug: log.debug("Checking start location (x, y):", x, y) if x == end_x and y == end_y: log.debug("----") log.debug("Checking the correct location", x, y) # Iterate orientations for start_direction in range(0, 4): if debug: log.debug("Checking with start direction", direction.Direction(start_direction)) if start_direction == v.start_direction(): log.debug("Checking correct start direction") check_x = x check_y = y failed = False # Iterate history for hist_ind in range(1, history.shape[0]): relative_dir = direction.RelativeDirection( history[hist_ind, 0]) abs_dir = direction.Direction( (relative_dir + start_direction) % 4) if debug: log.debug("Converting history dir", hist_ind, ":", relative_dir, "to", abs_dir) dx, dy = abs_dir.xy() check_x -= dx check_y -= dy check_x = check_x % m.shape[1] check_y = check_y % m.shape[0] # No longer a problem since the vehicle is allowed to go outside the map """ if not (0 <= check_x < m.shape[1] and 0 <= check_y < m.shape[0]): log.debug("Locator went outside the map") failed = True break """ # Skip color checking if the history index is in the skip list if hist_ind in skip_list: if debug: log.debug("Skipped history color", hist_ind, color.Color(history[hist_ind, 1]), "of", check_x, check_y, "which would correctly be", color.Color(m[check_y, check_x])) # Check the color matching (the normal situation) elif m[check_y, check_x] != history[hist_ind, 1]: if debug: log.debug("History color", hist_ind, color.Color(history[hist_ind, 1]), "of", check_x, check_y, "didn't match to", color.Color(m[check_y, check_x])) failed = True break if debug: log.debug("Color match", color.Color(m[check_x, check_y]), check_x, check_y) if not failed: log.debug("Found possible loc", x, y) possible_loc[check_y, check_x] = True log.debug("Locator results:\n", possible_loc.astype(int)) possible_y, possible_x = possible_loc.nonzero() num_matches = possible_y.size log.debug("Location took", time.perf_counter() - start_time) if num_matches == 0: log.debug("No suitable places found") loc_x = -9999 loc_y = -9999 elif num_matches == 1 and possible_x.size == 1: log.debug("Found location", possible_x[0], possible_y[0]) loc_x = possible_x[0] loc_y = possible_y[0] else: log.debug("Multiple possible locations found") loc_x = -9999 loc_y = -9999 return num_matches, loc_x, loc_y, possible_loc