def _find_path_internal(self) -> None:
model: BasicLSTMModule = self._services.resources.model_dir.load(self._load_name)
model.init_running_algorithm(self._get_grid())
history_frequency: Dict[Point, int] = {}
last_agent_pos: Point = self._get_grid().agent.position
stuck_threshold = 5
it = 0
while it < self._max_it:
# goal reached if radius intersects
if self._get_grid().is_agent_in_goal_radius():
self.move_agent(self._get_grid().goal.position)
break
next_move: Point = Point.from_tensor(model.forward_features(self._get_grid()))
self.move_agent(self._get_grid().apply_move(next_move, self._get_grid().agent.position))
last_agent_pos = self._get_grid().agent.position
new_freq: int = history_frequency.get(last_agent_pos, 0) + 1
history_frequency[last_agent_pos] = new_freq
# fail safe
if new_freq >= stuck_threshold:
break
it += 1
self.key_frame()
def _get_new_vertex(self, q_near: Vertex, q_sample: Point, max_dist) -> Vertex:
dir = q_sample.to_tensor() - q_near.position.to_tensor()
if torch.norm(dir) <= max_dist:
return Vertex(q_sample)
dir_normalized = dir / torch.norm(dir)
q_new = Point.from_tensor(q_near.position.to_tensor() + max_dist * dir_normalized)
return Vertex(q_new)
def render(self, *discarded) -> None:
self._services.graphics.renderer.push_line_thickness(50)
features = MapProcessing.extract_features(self._map, [
"distance_to_goal_normalized", "raycast_8_normalized",
"direction_to_goal_normalized", "agent_goal_angle"
])
rc = self.ray_colour()
for (ray, dir) in zip(features["raycast_8_normalized"],
self._map.ALL_POINTS_MOVE_VECTOR):
ray = 50.0 * ray - self._map.get_movement_cost(Point(0, 0), dir)
dir = dir.to_tensor() / torch.norm(dir.to_tensor())
vec = Point.from_tensor(self._map.agent.position.to_tensor() +
ray * dir)
self.__render_line(self._map.agent.position, vec, rc)
bc = self.bearing_colour()
self.__render_line(self._map.agent.position, self._map.goal.position,
bc)
self.__render_arc(self._map.agent.position, self._map.goal.position,
bc)
self._services.graphics.renderer.pop_line_thickness()
