def makeStateVector(self, data, local=False):
'''
create a state vector from the message recieved
'''
if self.event == Scenario.LANE_CHANGE:
i = 0
env_state = []
if not local:
self.append_vehicle_state(env_state, data.cur_vehicle_state)
# self.append_vehicle_state(env_state, data.back_vehicle_state)
# self.append_vehicle_state(env_state, data.front_vehicle_state)
for _, vehicle in enumerate(data.adjacent_lane_vehicles):
if i < 5:
self.append_vehicle_state(env_state, vehicle)
else:
break
i += 1
else:
cur_vehicle_state = VehicleState()
cur_vehicle_state.vehicle_location.x = 0
cur_vehicle_state.vehicle_location.y = 0
cur_vehicle_state.vehicle_location.theta = 0
cur_vehicle_state.vehicle_speed = data.cur_vehicle_state.vehicle_speed
self.append_vehicle_state(env_state, cur_vehicle_state)
for _, vehicle in enumerate(data.adjacent_lane_vehicles):
converted_state = convert_to_local(data.cur_vehicle_state,
vehicle)
if i < 5:
self.append_vehicle_state(env_state, converted_state)
else:
break
i += 1
dummy = VehicleState()
dummy.vehicle_location.x = 100
dummy.vehicle_location.y = 100
dummy.vehicle_location.theta = 0
dummy.vehicle_speed = 0
while i < 5:
self.append_vehicle_state(env_state, dummy)
i += 1
return env_state
elif self.event == Scenario.PEDESTRIAN:
env_state = []
if not local:
self.append_vehicle_state(env_state, data.cur_vehicle_state)
ped_vehicle = VehicleState()
ped_vehicle.vehicle_location = data.nearest_pedestrian.pedestrian_location
ped_vehicle.pedestrian_speed = data.nearest_pedestrian.pedestrian_speed
self.append_vehicle_state(env_state, ped_vehicle)
else:
cur_vehicle_state = VehicleState()
cur_vehicle_state.vehicle_location.x = 0
cur_vehicle_state.vehicle_location.y = 0
cur_vehicle_state.vehicle_location.theta = 0
cur_vehicle_state.vehicle_speed = data.cur_vehicle_state.vehicle_speed
self.append_vehicle_state(env_state, cur_vehicle_state)
ped_vehicle = VehicleState()
ped_vehicle.vehicle_location = data.nearest_pedestrian.pedestrian_location
ped_vehicle.vehicle_speed = data.nearest_pedestrian.pedestrian_speed
converted_state = convert_to_local(data.cur_vehicle_state,
ped_vehicle)
self.append_vehicle_state(env_state, converted_state)
return env_state
def embedState(self, env_desc: EnvironmentState, scenario: Scenario, local=False) -> np.ndarray:
"""
Create a state embedding vector from message recieved
Args:
:param env_des: (EnvironmentState) a ROS message describing the environment
"""
# TODO: Convert the embedding to Frenet coordinate system
# based on scenario create the embedding accordingly
if self.event == Scenario.LANE_CHANGE:
# initialize count of vehicles. We will add 5 vehicles
# The lane change scenario contains the current vehicle and 5 adjacent vehicles in the state
count_of_vehicles = 0
env_state = []
# if local flag is not set then use absolute coordinates
if not local:
self.append_vehicle_state(env_state, env_desc.cur_vehicle_state)
# TODO: Currently not adding vehicles in the front and back. Might need to add these later.
# self.append_vehicle_state(env_state, env_desc.back_vehicle_state)
# self.append_vehicle_state(env_state, env_desc.front_vehicle_state)
# count += 2
# add vehicles in the adjacent lane
for _, vehicle in enumerate(env_desc.adjacent_lane_vehicles):
if count_of_vehicles < 5:
self.append_vehicle_state(env_state, vehicle)
else:
break
count_of_vehicles += 1
else:
# use relative coordinates. Current vehicle is origin
cur_vehicle_state = VehicleState()
cur_vehicle_state.vehicle_location.x = 0
cur_vehicle_state.vehicle_location.y = 0
cur_vehicle_state.vehicle_location.theta = 0
# Still choose absolute current_vehicle speed wrt global frame
cur_vehicle_state.vehicle_speed = env_desc.cur_vehicle_state.vehicle_speed
self.append_vehicle_state(env_state, cur_vehicle_state)
# TODO: Currently not adding vehicles in the front and back. Might need to add these later.
# add vehicles in the adjacent lane
for _, vehicle in enumerate(env_desc.adjacent_lane_vehicles):
converted_state = convertToLocal(env_desc.cur_vehicle_state, vehicle)
if count_of_vehicles < 5:
self.append_vehicle_state(env_state, converted_state)
else:
break
count_of_vehicles += 1
# add dummy vehicles into the state if not reached 5
dummy = VehicleState()
dummy.vehicle_location.x = 100
dummy.vehicle_location.y = 100
dummy.vehicle_location.theta = 0
dummy.vehicle_speed = 0
while count_of_vehicles < 5:
self.append_vehicle_state(env_state, dummy)
count_of_vehicles += 1
return env_state
elif self.event == Scenario.PEDESTRIAN:
# initialize an empty list for the state
env_state = []
# choose between local and absolute coordinate system
# the state embedding contains the vehicle and the pedestrian
if not local:
self.append_vehicle_state(env_state, env_desc.cur_vehicle_state)
ped_vehicle = VehicleState()
ped_vehicle.vehicle_location = env_desc.nearest_pedestrian.pedestrian_location
ped_vehicle.pedestrian_speed = env_desc.nearest_pedestrian.pedestrian_speed
self.append_vehicle_state(env_state, ped_vehicle)
else:
cur_vehicle_state = VehicleState()
cur_vehicle_state.vehicle_location.x = 0
cur_vehicle_state.vehicle_location.y = 0
cur_vehicle_state.vehicle_location.theta = 0
cur_vehicle_state.vehicle_speed = env_desc.cur_vehicle_state.vehicle_speed
self.append_vehicle_state(env_state, cur_vehicle_state)
ped_vehicle = VehicleState()
ped_vehicle.vehicle_location = env_desc.nearest_pedestrian.pedestrian_location
ped_vehicle.vehicle_speed = env_desc.nearest_pedestrian.pedestrian_speed
converted_state = convertToLocal(env_desc.cur_vehicle_state, ped_vehicle)
self.append_vehicle_state(env_state, converted_state)
return env_state