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