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']
