mirror of
https://github.com/AtsushiSakai/PythonRobotics.git
synced 2026-04-22 03:00:22 -04:00
1308e76424
* speed up spacetime astar * forgot to include hash impl on Position * add condition to test on node expansions * remove heuristic from Node __hash__ impl * update rst with note about optimization
35 lines
835 B
Python
35 lines
835 B
Python
from PathPlanning.TimeBasedPathPlanning.GridWithDynamicObstacles import (
|
|
Grid,
|
|
ObstacleArrangement,
|
|
Position,
|
|
)
|
|
from PathPlanning.TimeBasedPathPlanning import SpaceTimeAStar as m
|
|
import numpy as np
|
|
import conftest
|
|
|
|
|
|
def test_1():
|
|
start = Position(1, 11)
|
|
goal = Position(19, 19)
|
|
grid_side_length = 21
|
|
grid = Grid(
|
|
np.array([grid_side_length, grid_side_length]),
|
|
obstacle_arrangement=ObstacleArrangement.ARRANGEMENT1,
|
|
)
|
|
|
|
m.show_animation = False
|
|
planner = m.SpaceTimeAStar(grid, start, goal)
|
|
|
|
path = planner.plan(False)
|
|
|
|
# path should have 28 entries
|
|
assert len(path.path) == 31
|
|
|
|
# path should end at the goal
|
|
assert path.path[-1].position == goal
|
|
|
|
assert planner.expanded_node_count < 1000
|
|
|
|
if __name__ == "__main__":
|
|
conftest.run_this_test(__file__)
|