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)