def __init__(self, map: OccupancyGridMap, s_start: (int, int, int),
s_goal: (int, int, int)):
"""
:param map: the ground truth map of the environment provided by gui
:param s_start: start location
:param s_goal: end location
"""
## we need to also update our occupancy grid map
self.new_edges_and_old_costs = None
# algorithm start
self.s_start = s_start # now takes in an x, y and z
self.s_goal = s_goal # now takes in an x, y and z
self.s_last = s_start # now takes in an x, y and z
self.k_m = 0 # accumulation
self.U = PriorityQueue()
self.rhs = np.ones((map.x_dim, map.y_dim, map.z_dim)) * np.inf
self.g = self.rhs.copy()
self.sensed_map = OccupancyGridMap(x_dim=map.x_dim,
y_dim=map.y_dim,
z_dim=map.z_dim,
exploration_setting='26N')
self.rhs[self.s_goal] = 0
self.U.insert(self.s_goal,
Priority(heuristic(self.s_start, self.s_goal), 0))
def calculate_key(self, s: (int, int)):
"""
:param s: the vertex we want to calculate key
:return: Priority class of the two keys
"""
k1 = min(self.g[s], self.rhs[s]) + heuristic(self.s_start, s) + self.k_m
k2 = min(self.g[s], self.rhs[s])
return Priority(k1, k2)
def __init__(self, map: OccupancyGridMap, s_start: (int, int),
s_goal: (int, int)):
self.new_edges_and_old_costs = None
self.s_start = s_start
self.s_goal = s_goal
self.s_last = s_start
self.k_m = 0
self.U = PriorityQueue()
self.rhs = np.ones((map.x_dim, map.y_dim)) * np.inf
self.g = self.rhs.copy()
self.sensed_map = OccupancyGridMap(x_dim=map.x_dim,
y_dim=map.y_dim,
exploration_setting='8N')
self.rhs[self.s_goal] = 0
self.U.insert(self.s_goal,
Priority(heuristic(self.s_start, self.s_goal), 0))
def calculate_key(self, s: (int, int)):
k1 = min(self.g[s], self.rhs[s]) + heuristic(self.s_start,
s) + self.k_m
k2 = min(self.g[s], self.rhs[s])
return Priority(k1, k2)