def _create_action_plan_for_agent(self, agent_id, train_run) -> ActionPlan:
action_plan = []
agent = self.env.agents[agent_id]
minimum_cell_time = int(np.ceil(1.0 / agent.speed_data['speed']))
for path_loop, train_run_way_point in enumerate(train_run):
train_run_way_point: TrainRunWayPoint = train_run_way_point
position = train_run_way_point.way_point.position
if Vec2d.is_equal(agent.target, position):
break
next_train_run_way_point: TrainRunWayPoint = train_run[path_loop +
1]
next_position = next_train_run_way_point.way_point.position
if path_loop == 0:
self._add_action_plan_elements_for_first_path_element_of_agent(
action_plan, train_run_way_point, next_train_run_way_point)
continue
just_before_target = Vec2d.is_equal(agent.target, next_position)
self._add_action_plan_elements_for_current_path_element(
action_plan, minimum_cell_time, train_run_way_point,
next_train_run_way_point)
# add a final element
if just_before_target:
self._add_action_plan_elements_for_target_at_path_element_just_before_target(
action_plan, minimum_cell_time, train_run_way_point,
next_train_run_way_point)
return action_plan
def test_vec2d_is_equal():
node_a = (1, 2)
node_b = (2, 4)
node_c = (1, 2)
res_1 = Vec2d.is_equal(node_a, node_b)
res_2 = Vec2d.is_equal(node_a, node_c)
assert not res_1
assert res_2
def check_path_exists(self, start: IntVector2DArray, direction: int,
end: IntVector2DArray):
"""
Breath first search for a possible path from one node with a certain orientation to a target node.
:param start: Start cell rom where we want to check the path
:param direction: Start direction for the path we are testing
:param end: Cell that we try to reach from the start cell
:return: True if a path exists, False otherwise
"""
visited = OrderedSet()
stack = [(start, direction)]
while stack:
node = stack.pop()
node_position = node[0]
node_direction = node[1]
if Vec2d.is_equal(node_position, end):
return True
if node not in visited:
visited.add(node)
moves = self.get_transitions(node_position[0],
node_position[1], node_direction)
for move_index in range(4):
if moves[move_index]:
stack.append(
(get_new_position(node_position,
move_index), move_index))
return False
def validate_new_transition(self, prev_pos: IntVector2D,
current_pos: IntVector2D, new_pos: IntVector2D,
end_pos: IntVector2D):
"""
Utility function to test that a path drawn by a-start algorithm uses valid transition objects.
We us this to quide a-star as there are many transition elements that are not allowed in RailEnv
:param prev_pos: The previous position we were checking
:param current_pos: The current position we are checking
:param new_pos: Possible child position we move into
:param end_pos: End cell of path we are drawing
:return: True if the transition is valid, False if transition element is illegal
"""
# start by getting direction used to get to current node
# and direction from current node to possible child node
new_dir = get_direction(current_pos, new_pos)
if prev_pos is not None:
current_dir = get_direction(prev_pos, current_pos)
else:
current_dir = new_dir
# create new transition that would go to child
new_trans = self.grid[current_pos]
if prev_pos is None:
if new_trans == 0:
# need to flip direction because of how end points are defined
new_trans = self.transitions.set_transition(
new_trans, mirror(current_dir), new_dir, 1)
else:
# check if matches existing layout
new_trans = self.transitions.set_transition(
new_trans, current_dir, new_dir, 1)
else:
# set the forward path
new_trans = self.transitions.set_transition(
new_trans, current_dir, new_dir, 1)
# set the backwards path
new_trans = self.transitions.set_transition(
new_trans, mirror(new_dir), mirror(current_dir), 1)
if Vec2d.is_equal(new_pos, end_pos):
# need to validate end pos setup as well
new_trans_e = self.grid[end_pos]
if new_trans_e == 0:
# need to flip direction because of how end points are defined
new_trans_e = self.transitions.set_transition(
new_trans_e, new_dir, mirror(new_dir), 1)
else:
# check if matches existing layout
new_trans_e = self.transitions.set_transition(
new_trans_e, new_dir, new_dir, 1)
if not self.transitions.is_valid(new_trans_e):
return False
# is transition is valid?
return self.transitions.is_valid(new_trans)
