def find_next_nodes(self, cspace, goal):
available = []
for direction in self.walk_dirs:
loc = self.loc + direction
#check whether node is inside the frame
# and it is not obstacle
if (not check_in_area(loc,
self.space_boundary)) or is_exist_same_loc(
loc, cspace):
# print("OverStep or exist same loc")
continue
else:
n = Node(loc, self.space_boundary, self.walk_dirs)
n.parent = self
# g = g_parent + g_move
n.g = self.g + self.cost(loc)
n.h = cal_Manhattan_dist(n.loc, goal)
n.f = n.g + n.h
available.append(n)
return available
def cost(self, other):
if self.is_obstacle or other.is_obstacle:
return np.inf
return cal_Manhattan_dist(self.loc, other.loc)
def cost(self, new_loc):
return cal_Manhattan_dist(self.loc, new_loc)