def choose_next_state(self, predictions): """ Нужно сделать выбор лучшего следующего состояния по предсказанным траекториям окружающих автомобилей (predictions). Функции, которые можно (и нужно) использовать: 1. successor_states(): Возвращает вектор состояний, в который можно перейти из текущего. Состояния: Keep Lane (KL), Lane Change Left (LCL), Lane Change Right (LCR), Prepare Lane Change Left (PLCL), Prepare Lane Chang Right. 2. generate_trajectory(self, state, predictions): Возвращает вектор предсказанной траектории по состоянию и предсказанным траекториям других автомобилей. 3. calculate_cost(vehicle, trajectory, predictions): Считает цену траектории. """ best_trajectory = None best_cost = None for state in self.successor_states(): trajectory = self.generate_trajectory(state, predictions) cost = calculate_cost(self, trajectory, predictions) if best_trajectory is None or cost < best_cost: best_trajectory = trajectory best_cost = cost return best_trajectory
def choose_next_state(self, predictions): """ Implement the transition_function code from the Behavior Planning Pseudocode classroom concept. INPUTS: A predictions dictionary. This is a dictionary of vehicle id keys with predicted vehicle trajectories as values. Trajectories are a list of Vehicle objects representing the vehicle at the current timestep and one timestep in the future. OUTPUT: The the best (lowest cost) trajectory corresponding to the next ego vehicle state. """ states = self.successor_states() costs = [] for state in states: trajectory = self.generate_trajectory(state, predictions) if trajectory: cost = calculate_cost(self, trajectory, predictions) costs.append({ "cost": cost, "state": state, "trajectory": trajectory }) best = min(costs, key=lambda s: s['cost']) return best["trajectory"]
def _get_next_state(self, predictions): states = ["KL", "LCL", "LCR"] #remove impossible successor states if self.lane == 0: states.remove("LCL") if self.lane == (self.lanes_available - 1): states.remove("LCR") costs = [] #go through each possible state, #find rough trajectory to that state and cost for that trajectory for state in states: #make a deep copy of predictions as #self._trajectory_for_state(state, predictions) method #updates predictions dictionary and we don't want it to be updated predictions_copy = deepcopy(predictions) #find trajectory for this state trajectory = self._trajectory_for_state(state, predictions_copy) print("Trajectory for state {} is {}".format(state, trajectory)) #calculate cost for moving to current state using found trajectory cost = calculate_cost(self, trajectory, predictions) costs.append({"state": state, "cost": cost}) #find the state with min cost trajectory best = min(costs, key=lambda s: s['cost']) return best['state']
def choose_next_state(self, predictions): possible_state = self.successor_states() cost_List = [] cost_min = 9999 ind = 0 for state_tgt in possible_state : trajectory_tgt = self.generate_trajectory(state_tgt, predictions) cost_tgt = calculate_cost(self, trajectory_tgt, predictions) cost_List.append({'state':state_tgt, 'cost':cost_tgt}) if (ind == 0) : cost_min = cost_tgt sate_min = state_tgt trajectory_min = trajectory_tgt else : if (cost_tgt < cost_min) : cost_min = cost_tgt sate_min = state_tgt trajectory_min = trajectory_tgt ind += 1 print(sate_min) return trajectory_min '''
def _get_next_state(self, predictions): states = ["KL", "LCL", "LCR"] if self.lane == 0: states.remove("LCL") if self.lane == (self.lanes_available - 1): states.remove("LCR") cprint('All possible states: {}'.format(states), 'blue') costs = [] for state in states: predictions_copy = deepcopy(predictions) # pretend ego has accepted the state `state` # increment the ego in 5 time-steps and return the new dict of prediction trajectory = self._trajectory_for_state(state, predictions_copy) cprint('trajectory for state {}'.format(state), 'blue', 'on_white') for xx in trajectory: cprint(xx, 'blue', 'on_white') cost = calculate_cost(self, trajectory, predictions, verbose=True) costs.append({"state": state, "cost": cost}) cprint('All costs', 'yellow') for ix in costs: cprint(ix, 'yellow') best = min(costs, key=lambda s: s['cost']) return best['state']
def choose_next_state(self, predictions): ''' Implement the transition function code for the vehicle's behaviour planning finite state machine, which operates based on the cost function (defined in a separate module cost_functions.py). INPUTS: A predictions dictionary with vehicle id keys and predicted vehicle trajectories as values. Trajectories are a list of Vehicle objects representing the vehicle at the current timestep and one timestep in the future. OUTPUT: The the best (lowest cost) trajectory corresponding to the next ego vehicle state. Functions that will be useful: 1. successor_states(): Returns a vector of possible successor states for the finite state machine. 2. generate_trajectory(self, state, predictions): Returns a vector of Vehicle objects representing a vehicle trajectory, given a state and predictions. Note that trajectories might be empty if no possible trajectory exists for the state; for example, if the state is LCR, but a vehicle is occupying the space to the ego vehicle's right, then there is no possible trajectory without first transitioning to another state. 3. calculate_cost(vehicle, trajectory, predictions): Imported from cost_functions.py, computes the cost for a trajectory. ''' v_states = self.successor_states() # print("predictions : ", predictions) print("state : ", v_states) # state : ['KL', 'PLCL', 'PLCR'] for i in range(len(v_states)): traj = self.generate_trajectory(v_states[i], predictions) cost = calculate_cost(self, traj, predictions) # print("traj : ", traj) # print("i : ", i) # print("cost : ", cost) if i == 0: minimum_cost = cost minimum_cost_trajectory = traj else: if cost < minimum_cost: minimum_cost = cost minimum_cost_trajectory = traj # TODO: implement state transition function based on the cost # associated with each transition. # Note that the return value is a trajectory, where a trajectory # is a list of Vehicle objects with two elements. return minimum_cost_trajectory
def choose_next_state(self, predictions): ''' Implement the transition function code for the vehicle's behaviour planning finite state machine, which operates based on the cost function (defined in a separate module cost_functions.py). INPUTS: A predictions dictionary with vehicle id keys and predicted vehicle trajectories as values. Trajectories are a list of Vehicle objects representing the vehicle at the current timestep and one timestep in the future. OUTPUT: The the best (lowest cost) trajectory corresponding to the next ego vehicle state. Functions that will be useful: 1. successor_states(): Returns a vector of possible successor states for the finite state machine. 2. generate_trajectory(self, state, predictions): Returns a vector of Vehicle objects representing a vehicle trajectory, given a state and predictions. Note that trajectories might be empty if no possible trajectory exists for the state; for example, if the state is LCR, but a vehicle is occupying the space to the ego vehicle's right, then there is no possible trajectory without first transitioning to another state. 3. calculate_cost(vehicle, trajectory, predictions): Imported from cost_functions.py, computes the cost for a trajectory. ''' # TODO: implement state transition function based on the cost # associated with each transition. # road.advance()에서 generate_predictions()를 가지고 다른 차량에 대한 # predictions를 먼저 구하고 나서 여기에 들어온다. # print(predictions) possible_successor_states = self.successor_states() costs = [] for state in possible_successor_states: print("possibles:", state) trajectory = self.generate_trajectory(state, predictions) print("trajectory state:", trajectory[1].state) cost_for_state = calculate_cost(self, trajectory, predictions) costs.append({'state': state, 'cost': cost_for_state}) best_next_state = None min_cost = 9999999 for c in costs: if c['cost'] < min_cost: min_cost = c['cost'] best_next_state = c['state'] next_trajectory = self.generate_trajectory(best_next_state, predictions) # Note that the return value is a trajectory, where a trajectory # is a list of Vehicle objects with two elements. return next_trajectory
def choose_next_state(self, predictions): ''' Implement the transition function code for the vehicle's behaviour planning finite state machine, which operates based on the cost function (defined in a separate module cost_functions.py). INPUTS: A predictions dictionary with vehicle id keys and predicted vehicle trajectories as values. Trajectories are a list of Vehicle objects representing the vehicle at the current timestep and one timestep in the future. OUTPUT: The the best (lowest cost) trajectory corresponding to the next ego vehicle state. Functions that will be useful: 1. successor_states(): Returns a vector of possible successor states for the finite state machine. 2. generate_trajectory(self, state, predictions): Returns a vector of Vehicle objects representing a vehicle trajectory, given a state and predictions. Note that trajectories might be empty if no possible trajectory exists for the state; for example, if the state is LCR, but a vehicle is occupying the space to the ego vehicle's right, then there is no possible trajectory without first transitioning to another state. 3. calculate_cost(vehicle, trajectory, predictions): Imported from cost_functions.py, computes the cost for a trajectory. ''' # TODO: implement state transition function based on the cost # associated with each transition. # Note that the return value is a trajectory, where a trajectory # is a list of Vehicle objects with two elements. states = self.successor_states() cost = 0 costs = [] final_states = [] final_trajectories = [] for s in states: trajectory = self.generate_trajectory(s, predictions) if len(trajectory) != 0: cost = calculate_cost(self, trajectory, predictions) costs.append(cost) final_trajectories.append(trajectory) return [ Vehicle(self.lane, self.s, self.v, self.a, self.state), Vehicle(self.lane, self.position_at(1), self.v, 0, self.state) ]
def choose_next_state(self, predictions): ''' Implement the transition function code for the vehicle's behaviour planning finite state machine, which operates based on the cost function (defined in a separate module cost_functions.py). INPUTS: A predictions dictionary with vehicle id keys and predicted vehicle trajectories as values. Trajectories are a list of Vehicle objects representing the vehicle at the current timestep and one timestep in the future. OUTPUT: The the best (lowest cost) trajectory corresponding to the next ego vehicle state. Functions that will be useful: 1. successor_states(): Returns a vector of possible successor states for the finite state machine. 2. generate_trajectory(self, state, predictions): Returns a vector of Vehicle objects representing a vehicle trajectory, given a state and predictions. Note that trajectories might be empty if no possible trajectory exists for the state; for example, if the state is LCR, but a vehicle is occupying the space to the ego vehicle's right, then there is no possible trajectory without first transitioning to another state. 3. calculate_cost(vehicle, trajectory, predictions): Imported from cost_functions.py, computes the cost for a trajectory. ''' state = self.successor_states() min_cost = 9999999 best_next_state = None cost_array = [] for st in state: t = self.generate_trajectory(st, predictions) cost = calculate_cost(self, t, predictions) cost_array.append({'cost': cost, 'state': st, 'trajectory': t}) for k in cost_array: if k['cost'] < min_cost: min_cost = k['cost'] best_next_state = k['state'] best_trajectory = self.generate_trajectory(best_next_state, predictions) return best_trajectory
def _get_next_state(self, predictions): states = ["KL", "LCL", "LCR"] if self.lane == 0: states.remove("LCL") if self.lane == (self.lanes_available - 1): states.remove("LCR") costs = [] for state in states: predictions_copy = deepcopy(predictions) trajectory = self._trajectory_for_state(state, predictions_copy) cost = calculate_cost(self, trajectory, predictions) costs.append({"state": state, "cost": cost}) best = min(costs, key=lambda s: s['cost']) return best['state']
def choose_next_state(self, predictions): """ Implement the transition_function code from the Behavior Planning Pseudocode classroom concept. INPUTS: A predictions dictionary. This is a dictionary of vehicle id keys with predicted vehicle trajectories as values. Trajectories are a list of Vehicle objects representing the vehicle at the current timestep and one timestep in the future. OUTPUT: The the best (lowest cost) trajectory corresponding to the next ego vehicle state. Functions that will be useful: 1. successor_states(): Returns a vector of possible successor states for the finite state machine. 2. generate_trajectory(self, state, predictions): Returns a vector of Vehicle objects representing a vehicle trajectory, given a state and predictions. Note that trajectories might be empty if no possible trajectory exists for the state; for example, if the state is LCR, but a vehicle is occupying the space to the ego vehicle's right, then there is no possible trajectory without first transitioning to another state. 3. calculate_cost(vehicle, trajectory, predictions): Included from cost.cpp, computes the cost for a trajectory. """ #debug print("----------------choose_next_state-----------------------") print("successor_states: ") states = self.successor_states() for onestate in states: print(onestate) costs = [] for state in states: trajectory = self.generate_trajectory(state, predictions) if trajectory: cost = calculate_cost(self, trajectory, predictions) print("cost: ", cost, " ,state: ", state) costs.append({ "cost": cost, "state": state, "trajectory": trajectory }) best = min(costs, key=lambda s: s['cost']) # if equal cost ? print("min cost: ", best["cost"], " , at state: ", best["state"]) return best["trajectory"]
def choose_next_state(self, predictions): """ Implement the transition_function code from the Behavior Planning Pseudocode classroom concept. INPUTS: A predictions dictionary. This is a dictionary of vehicle id keys with predicted vehicle trajectories as values. Trajectories are a list of Vehicle objects representing the vehicle at the current timestep and one timestep in the future. OUTPUT: The the best (lowest cost) trajectory corresponding to the next ego vehicle state. Functions that will be useful: 1. successor_states(): Returns a vector of possible successor states for the finite state machine. 2. generate_trajectory(self, state, predictions): Returns a vector of Vehicle objects representing a vehicle trajectory, given a state and predictions. Note that trajectories might be empty if no possible trajectory exists for the state; for example, if the state is LCR, but a vehicle is occupying the space to the ego vehicle's right, then there is no possible trajectory without first transitioning to another state. 3. calculate_cost(vehicle, trajectory, predictions): Included from cost.cpp, computes the cost for a trajectory. """ #Your code here states = self.successor_states() costs = [] for state in states: print(state, ':', end='') trajectory = self.generate_trajectory(state, predictions) #print ('Num states in trajectory' , len(trajectory)) assert (len(trajectory) == 2) for vehicle_state in trajectory: print(vehicle_state.lane, end='') print('->', end='') cost_for_state = calculate_cost(self, trajectory, predictions) print(' {', cost_for_state, ' }') sys.exit() #Change return value here return [ Vehicle(self.lane, self.s, self.v, self.a, self.state), Vehicle(self.lane, self.position_at(1), self.v, 0, self.state) ]
def _get_next_state(self, predictions): states = ["KL", "LCL", "LCR"] # no PLCL or PLCR if self.lane == 0: states.remove("LCL") if self.lane == (self.lanes_available - 1): states.remove("LCR") costs = [] ## For each state -- find trajectory, and cost of trajectory for state in states: predictions_copy = deepcopy(predictions) trajectory = self._trajectory_for_state(state, predictions_copy) cost = calculate_cost(self, trajectory, predictions) costs.append({"state": state, "cost": cost}) # Choose the trajectory with least cost best = min(costs, key=lambda s: s['cost']) return best['state']
def _get_next_state(self, predictions): # initially, define all states as possible states = ["KL", "LCL", "LCR"] # evaluate possible lanes to go if self.lane == 0: states.remove("LCL") if self.lane == (self.lanes_available - 1): states.remove("LCR") costs = [] for state in states: predictions_copy = deepcopy(predictions) # get a possible trajectory for each state trajectory = self._trajectory_for_state(state, predictions_copy) # calculate the cost for that trajectory cost = calculate_cost(self, trajectory, predictions) costs.append({"state": state, "cost": cost}) # choose trajectory with minimal cost best = min(costs, key=lambda s: s['cost']) return best['state']