diff --git a/flatland/core/grid/grid4_astar.py b/flatland/core/grid/grid4_astar.py
index 9022329565a093a4c60336680a98ba4d8d7a0a83..a049ae260860e946ac8b27f9e83ab11bf4ed2920 100644
--- a/flatland/core/grid/grid4_astar.py
+++ b/flatland/core/grid/grid4_astar.py
@@ -4,6 +4,7 @@ from flatland.core.grid.grid_utils import IntVector2D, IntVector2DDistance
from flatland.core.grid.grid_utils import IntVector2DArray
from flatland.core.grid.grid_utils import Vec2dOperations as Vec2d
from flatland.core.transition_map import GridTransitionMap
+from flatland.utils.ordered_set import OrderedSet
class AStarNode:
@@ -49,8 +50,8 @@ def a_star(grid_map: GridTransitionMap,
start_node = AStarNode(start, None)
end_node = AStarNode(end, None)
- open_nodes = set()
- closed_nodes = set()
+ open_nodes = OrderedSet()
+ closed_nodes = OrderedSet()
open_nodes.add(start_node)
while len(open_nodes) > 0:
diff --git a/tests/test_flatland_core_grid4_generators_util.py b/tests/test_flatland_core_grid4_generators_util.py
index 9e85d522cea11ef1897b2bcb8eecbf305108d468..72deddc66eacc71a5aa840b49225e5ba056a8b84 100644
--- a/tests/test_flatland_core_grid4_generators_util.py
+++ b/tests/test_flatland_core_grid4_generators_util.py
@@ -15,8 +15,8 @@ def test_build_railway_infrastructure():
start_point = (2, 2)
end_point = (8, 8)
connection_001 = connect_rail(rail_trans, grid_map, start_point, end_point, Vec2d.get_manhattan_distance)
- connection_001_expected = [(2, 2), (3, 2), (3, 3), (4, 3), (4, 4), (5, 4), (5, 5), (5, 6), (6, 6), (6, 7), (7, 7),
- (8, 7), (8, 8)]
+ connection_001_expected = [(2, 2), (2, 3), (2, 4), (2, 5), (2, 6), (2, 7), (2, 8), (3, 8), (4, 8), (5, 8), (6, 8),
+ (7, 8), (8, 8)]
start_point = (1, 3)
end_point = (1, 7)
@@ -33,30 +33,37 @@ def test_build_railway_infrastructure():
connection_004 = connect_to_nodes(rail_trans, grid_map, start_point, end_point, Vec2d.get_manhattan_distance)
connection_004_expected = [(7, 5), (7, 6), (7, 7), (7, 8), (7, 9), (8, 9)]
- assert connection_001 == connection_001_expected
- assert connection_002 == connection_002_expected
- assert connection_003 == connection_003_expected
- assert connection_004 == connection_004_expected
-
- grid_map_grid_expected = [[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
- [0, 0, 0, 0, 1025, 1025, 1025, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
- [0, 0, 8192, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
- [0, 0, 72, 4608, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
- [0, 0, 0, 72, 4608, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
- [0, 0, 0, 0, 72, 1025, 4608, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
- [0, 0, 0, 1025, 1025, 256, 72, 4608, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
- [0, 0, 0, 0, 0, 4, 1025, 33825, 1025, 4608, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
- [0, 0, 0, 0, 0, 0, 0, 72, 256, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
- [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
- [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
- [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
- [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
- [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
- [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
- [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
- [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
- [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
- [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
- [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]]
-
- assert np.all(grid_map.grid == grid_map_grid_expected)
+ assert connection_001 == connection_001_expected, \
+ "actual={}, expected={}".format(connection_001, connection_001_expected)
+ assert connection_002 == connection_002_expected, \
+ "actual={}, expected={}".format(connection_002, connection_002_expected)
+ assert connection_003 == connection_003_expected, \
+ "actual={}, expected={}".format(connection_003, connection_003_expected)
+ assert connection_004 == connection_004_expected, \
+ "actual={}, expected={}".format(connection_004, connection_004_expected)
+
+ grid_map_grid_expected = [
+ [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
+ [0, 0, 0, 0, 1025, 1025, 1025, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
+ [0, 0, 4, 1025, 1025, 1025, 1025, 1025, 4608, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
+ [0, 0, 0, 0, 0, 0, 0, 0, 32800, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
+ [0, 0, 0, 0, 0, 0, 0, 0, 32800, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
+ [0, 0, 0, 0, 0, 0, 0, 0, 32800, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
+ [0, 0, 0, 1025, 1025, 256, 0, 0, 32800, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
+ [0, 0, 0, 0, 0, 4, 1025, 1025, 33825, 4608, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
+ [0, 0, 0, 0, 0, 0, 0, 0, 128, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
+ [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
+ [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
+ [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
+ [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
+ [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
+ [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
+ [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
+ [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
+ [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
+ [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
+ [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
+ ]
+
+ assert np.all(grid_map.grid == grid_map_grid_expected), \
+ "actual={}, expected={}".format(grid_map.grid, grid_map_grid_expected)
diff --git a/tests/test_flatland_envs_city_generator.py b/tests/test_flatland_envs_city_generator.py
index 353bb1d414e5fe2ca57831ada73cdaa1851be391..4a96a8fbf282fee79ccd778ba7d097042fbf31c3 100644
--- a/tests/test_flatland_envs_city_generator.py
+++ b/tests/test_flatland_envs_city_generator.py
@@ -27,11 +27,11 @@ def test_city_generator():
obs_builder_object=GlobalObsForRailEnv())
# approximative test (replace image comparison)
- assert (np.sum(env.rail.grid) == 2718122)
+ assert np.sum(env.rail.grid) == 3642337
s0 = 0
s1 = 0
for a in range(env.get_num_agents()):
s0 = Vec2d.get_manhattan_distance(env.agents[a].position, (0, 0))
s1 = Vec2d.get_chebyshev_distance(env.agents[a].position, (0, 0))
- assert s0 == 49
- assert s1 == 27
+ assert s0 == 58, "actual={}".format(s0)
+ assert s1 == 38, "actual={}".format(s1)
diff --git a/tests/test_flatland_envs_sparse_rail_generator.py b/tests/test_flatland_envs_sparse_rail_generator.py
index 2d22e39a925ad0e3bc35e7094b4bbb711ae852e2..47cb174f6938fddccae8b3374cbaeb73f31e2c67 100644
--- a/tests/test_flatland_envs_sparse_rail_generator.py
+++ b/tests/test_flatland_envs_sparse_rail_generator.py
@@ -27,14 +27,15 @@ def test_sparse_rail_generator():
obs_builder_object=GlobalObsForRailEnv())
# approximative test (replace image comparison)
- assert (np.sum(env.rail.grid) == 10746925)
+ assert (np.sum(env.rail.grid) == 12054975)
+ print(env.rail.grid)
s0 = 0
s1 = 0
for a in range(env.get_num_agents()):
s0 = Vec2d.get_manhattan_distance(env.agents[a].position, (0, 0))
s1 = Vec2d.get_chebyshev_distance(env.agents[a].position, (0, 0))
- assert s0 == 33
- assert s1 == 31
+ assert s0 == 53, "actual={}".format(s0)
+ assert s1 == 36, "actual={}".format(s1)
def test_sparse_rail_generator_deterministic():
diff --git a/tests/test_flatland_malfunction.py b/tests/test_flatland_malfunction.py
index fde9df58663993ae170c4c1e3fea55637feb4282..884a2a51f84a40a45acced32e7310dcf4d497944 100644
--- a/tests/test_flatland_malfunction.py
+++ b/tests/test_flatland_malfunction.py
@@ -48,7 +48,8 @@ class SingleAgentNavigationObs(TreeObsForRailEnv):
for direction in [(agent.direction + i) % 4 for i in range(-1, 2)]:
if possible_transitions[direction]:
new_position = get_new_position(agent.position, direction)
- min_distances.append(self.distance_map[handle, new_position[0], new_position[1], direction])
+ min_distances.append(
+ self.env.distance_map.get()[handle, new_position[0], new_position[1], direction])
else:
min_distances.append(np.inf)