def convertToLocal(cur_vehicle: VehicleState, adj_vehicle: VehicleState):
"""
Converts the adj_vehicle position into cur_vehicle position
Expects VehicleState objects and Euclidean Coordinate Frame
Args
:param cur_vehicle: (VehicleState) the ego vehicle state
:param adj_vehicle: (VehicleState) the target vehicle state
Returns
the adj_vehicle pose in terms of the cur_vehicle
"""
# create a place holder vehicle object to represent the resultant relative pose
result_state = VehicleState()
# extract the pose and velocities of the adj_vehicle
x = adj_vehicle.vehicle_location.x
y = adj_vehicle.vehicle_location.y
theta = adj_vehicle.vehicle_location.theta
speed = adj_vehicle.vehicle_speed
# heading is in terms of the x-axis so vx is cos component
# vx = speed * np.cos(theta)
# vy = speed * np.sin(theta)
# get current_vehicle_speeds
# cvx = cur_vehicle.vehicle_speed * np.cos(cur_vehicle.vehicle_location.theta)
# cvy = cur_vehicle.vehicle_speed * np.sin(cur_vehicle.vehicle_location.theta)
# make homogeneous transform matrix to transform global coordinate into the frame
# of the ego vehicle state. Calculate as a product of two transformations. Multiply right.
H_Rot = np.eye(3)
H_Rot[-1, -1] = 1
H_Rot[0, -1] = 0
H_Rot[1, -1] = 0
H_Rot[0, 0] = np.cos(cur_vehicle.vehicle_location.theta)
H_Rot[0, 1] = -np.sin(cur_vehicle.vehicle_location.theta)
H_Rot[1, 0] = np.sin(cur_vehicle.vehicle_location.theta)
H_Rot[1, 1] = np.cos(cur_vehicle.vehicle_location.theta)
H_trans = np.eye(3)
H_trans[0, -1] = -cur_vehicle.vehicle_location.x
H_trans[1, -1] = -cur_vehicle.vehicle_location.y
H = np.matmul(H_Rot, H_trans)
# calculate and set relative position
res = np.matmul(H, np.array([x, y, 1]).reshape(3, 1))
result_state.vehicle_location.x = res[0, 0]
result_state.vehicle_location.y = res[1, 0]
# calculate and set relative orientation
result_state.vehicle_location.theta = theta - cur_vehicle.vehicle_location.theta
# calculate and set relative speed
# res_vel = np.array([vx - cvx, vy - cvy])
result_state.vehicle_speed = speed # np.linalg.norm(res_vel)
# time.sleep(5)
return result_state
def convert2ROS(self):
def speed_conversion(sim_speed):
return sim_speed * 3.6
vehicle_state = VehicleState()
vehicle_state.vehicle_location.x = self.x
vehicle_state.vehicle_location.y = self.y
vehicle_state.vehicle_location.theta = self.theta
vehicle_state.length = self.length
vehicle_state.width = self.width
vehicle_state.vehicle_speed = speed_conversion(self.speed)
return vehicle_state
def terminate(self, env_desc):
if self.event == Scenario.LANE_CHANGE:
return env_desc.reward.collision or \
env_desc.reward.path_planner_terminate or \
env_desc.reward.time_elapsed > self.eps_time
elif self.event == Scenario.PEDESTRIAN:
# return true if vehicle has moved past the vehicle
ped_vehicle = VehicleState()
relative_pose = ped_vehicle
if env_desc.nearest_pedestrian.exist:
ped_vehicle.vehicle_location = env_desc.nearest_pedestrian.pedestrian_location
ped_vehicle.vehicle_speed = env_desc.nearest_pedestrian.pedestrian_speed
relative_pose = convert_to_local(env_desc.cur_vehicle_state,
ped_vehicle)
if relative_pose.vehicle_location.x < -1:
return True
# usual conditions
return env_desc.reward.collision or \
env_desc.reward.path_planner_terminate or \
env_desc.reward.time_elapsed > self.eps_time
def getVehicleState(self, actor):
if actor == None:
return None
vehicle = VehicleState()
vehicle.vehicle_location.x = actor.get_transform().location.x
vehicle.vehicle_location.y = actor.get_transform().location.y
vehicle.vehicle_location.theta = (actor.get_transform().rotation.yaw *
np.pi / 180
) # CHECK : Changed this to radians.
vehicle.vehicle_speed = (
np.sqrt(actor.get_velocity().x**2 + actor.get_velocity().y**2 +
actor.get_velocity().z**2) * 3.6)
vehicle_bounding_box = actor.bounding_box.extent
vehicle.length = vehicle_bounding_box.x * 2
vehicle.width = vehicle_bounding_box.y * 2
return vehicle
def make_state_vector(self, data):
'''
create a state vector from the message recieved
'''
env_state = []
def append_vehicle_state(env_state, vehicle_state):
env_state.append(vehicle_state.vehicle_location.x)
env_state.append(vehicle_state.vehicle_location.y)
env_state.append(vehicle_state.vehicle_location.theta)
env_state.append(vehicle_state.vehicle_speed)
return
# items needed
# current vehicle velocity
append_vehicle_state(env_state, data.cur_vehicle_state)
#env_state.append(data.cur_vehicle_state.vehicle_speed)
# rear vehicle state
# position
# velocity
#append_vehicle_state(env_state, data.back_vehicle_state)
#append_vehicle_state(env_state, data.front_vehicle_state)
# adjacent vehicle state
# position
# velocity
i = 0
for _, veh_state in enumerate(data.adjacent_lane_vehicles):
if i < 5:
append_vehicle_state(env_state, veh_state)
else:
break
i += 1
dummy = VehicleState()
dummy.vehicle_location.x = data.cur_vehicle_state.vehicle_location.x + 100
dummy.vehicle_location.y = data.cur_vehicle_state.vehicle_location.y + 100
dummy.vehicle_location.theta = 0
dummy.vehicle_speed = 0
while i < 5:
append_vehicle_state(env_state, dummy)
i += 1
return env_state
def speed_reward(self, desc, action):
reward = 0
self.min_dist = desc.nearest_pedestrian.radius
ped_vehicle = VehicleState()
relative_pose = ped_vehicle
if desc.nearest_pedestrian.exist:
ped_vehicle.vehicle_location = desc.nearest_pedestrian.pedestrian_location
ped_vehicle.vehicle_speed = desc.nearest_pedestrian.pedestrian_speed
relative_pose = convertToLocal(desc.cur_vehicle_state, ped_vehicle)
# check if pedestrian collided
if desc.reward.collision:
return -self.max_reward
# check if pedestrian avoided
elif desc.nearest_pedestrian.exist and relative_pose.vehicle_location.x < -10:
reward = self.max_reward
# add costs of overspeeding
reward -= self.vel_cost()
reward -= self.position_cost()
self.reset()
return reward
def convert_to_local(cur_vehicle, adj_vehicle):
result_state = VehicleState()
x = adj_vehicle.vehicle_location.x
y = adj_vehicle.vehicle_location.y
theta = adj_vehicle.vehicle_location.theta
speed = adj_vehicle.vehicle_speed
vx = speed * np.cos(theta)
vy = speed * np.sin(theta)
# get current_vehicle_speeds
cvx = cur_vehicle.vehicle_speed * np.cos(
cur_vehicle.vehicle_location.theta)
cvy = cur_vehicle.vehicle_speed * np.sin(
cur_vehicle.vehicle_location.theta)
# make homogeneous transform
H_Rot = np.eye(3)
H_Rot[-1, -1] = 1
H_Rot[0, -1] = 0
H_Rot[1, -1] = 0
H_Rot[0, 0] = np.cos(cur_vehicle.vehicle_location.theta)
H_Rot[0, 1] = -np.sin(cur_vehicle.vehicle_location.theta)
H_Rot[1, 0] = np.sin(cur_vehicle.vehicle_location.theta)
H_Rot[1, 1] = np.cos(cur_vehicle.vehicle_location.theta)
H_trans = np.eye(3)
H_trans[0, -1] = -cur_vehicle.vehicle_location.x
H_trans[1, -1] = -cur_vehicle.vehicle_location.y
H = np.matmul(H_Rot, H_trans)
# calculate and set relative position
res = np.matmul(H, np.array([x, y, 1]).reshape(3, 1))
result_state.vehicle_location.x = res[0, 0]
result_state.vehicle_location.y = res[1, 0]
# calculate and set relative orientation
result_state.vehicle_location.theta = theta - cur_vehicle.vehicle_location.theta
# calculate and set relative speed
res_vel = np.array([vx - cvx, vy - cvy])
result_state.vehicle_speed = speed # np.linalg.norm(res_vel)
# time.sleep(5)
return result_state
def terminate(self, env_desc: EnvironmentState) -> bool:
"""
A function which returns true if the episode must terminate.
This is decided by the following conditions.
* Has the vehicle collinded?
* Is the episode time elapsed more than the threshold self.eps_time?
* Is the maneouver completed?
Args:
env_desc: (EnvironmentState) ROS Message for the environment
Returns:
true if episode must terminate
"""
if self.event == Scenario.LANE_CHANGE:
# return true if any of the conditions described in the description is true
return env_desc.reward.collision or \
env_desc.reward.path_planner_terminate or \
env_desc.reward.time_elapsed > self.eps_time
elif self.event == Scenario.PEDESTRIAN:
# return true if vehicle has moved past the pedestrian
# treat a pedestrian as a vehicle object
ped_vehicle = VehicleState()
relative_pose = VehicleState()
# if there is a pedestrian closeby then check if we have passed him
if env_desc.nearest_pedestrian.exist:
# populate the "pedestrian vehicle" parameters
ped_vehicle.vehicle_location = env_desc.nearest_pedestrian.pedestrian_location
ped_vehicle.vehicle_speed = env_desc.nearest_pedestrian.pedestrian_speed
relative_pose = convertToLocal(env_desc.cur_vehicle_state,ped_vehicle)
if relative_pose.vehicle_location.x < -10:
return True
# usual conditions
return env_desc.reward.collision or \
env_desc.reward.path_planner_terminate or \
env_desc.reward.time_elapsed > self.eps_time
def make_state_vector(self, data):
'''
create a state vector from the message recieved
'''
i = 0
env_state = []
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 _, veh_state in enumerate(data.adjacent_lane_vehicles):
if i < 5:
self.append_vehicle_state(env_state, veh_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
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
def talker():
# initialize publishers
pub = rospy.Publisher(SIM_TOPIC_NAME, EnvironmentState, queue_size=10)
pub_rl = rospy.Publisher(RL_TOPIC_NAME, RLCommand, queue_size=10)
rospy.init_node(NODE_NAME, anonymous=True)
rate = rospy.Rate(10) # 10hz
while not rospy.is_shutdown():
# current Lane
lane_cur = Lane()
lane_points = []
for i in range(100):
lane_point = LanePoint()
lane_point.pose.y = 0
lane_point.pose.x = i / 10.
lane_point.pose.theta = 0
lane_point.width = 3
lane_points.append(lane_point)
lane_cur.lane = lane_points
# next Lane
lane_next = Lane()
lane_points_next = []
for i in range(100):
lane_point = LanePoint()
lane_point.pose.y = 3
lane_point.pose.x = i / 10.
lane_point.pose.theta = 0
lane_point.width = 3
lane_points_next.append(lane_point)
lane_next.lane = lane_points_next
# current vehicle
vehicle = VehicleState()
vehicle.vehicle_location.x = 2
vehicle.vehicle_location.y = 0
vehicle.vehicle_location.theta = 0
vehicle.vehicle_speed = 10
vehicle.length = 5
vehicle.width = 2
# sample enviroment state
env_state = EnvironmentState()
env_state.cur_vehicle_state = copy.copy(vehicle)
env_state.front_vehicle_state = copy.copy(vehicle)
env_state.back_vehicle_state = copy.copy(vehicle)
env_state.adjacent_lane_vehicles = [
copy.copy(vehicle), copy.copy(vehicle)
]
env_state.max_num_vehicles = 2
env_state.speed_limit = 40
env_state.current_lane = copy.copy(lane_cur)
env_state.next_lane = copy.copy(lane_next)
# sample RL command
rl_command = RLCommand()
rl_command.change_lane = 1
rospy.loginfo("Publishing")
pub.publish(env_state)
pub_rl.publish(rl_command)
rate.sleep()
def initialize(self):
# initialize node
rospy.init_node(NODE_NAME, anonymous=True)
# initialize service
self.planner_service = rospy.Service(SIM_SERVICE_NAME, SimService,
self.pathRequest)
# Start Client. Make sure Carla server is running before starting.
client = carla.Client("localhost", 2000)
client.set_timeout(2.0)
print("Connection to CARLA server established!")
# Create a CarlaHandler object. CarlaHandler provides some cutom built APIs for the Carla Server.
self.carla_handler = CarlaHandler(client)
self.client = client
settings = self.carla_handler.world.get_settings()
settings.synchronous_mode = True
settings.fixed_delta_seconds = self.simulation_sync_timestep
self.carla_handler.world.apply_settings(settings)
self.tm = CustomScenario(self.client, self.carla_handler)
# Reset Environment
self.resetEnv()
# Initialize PID Controller
self.vehicle_controller = GRASPPIDController(
self.ego_vehicle,
args_lateral={
"K_P": 0.5,
"K_D": 0,
"K_I": 0,
"dt": self.simulation_sync_timestep,
},
args_longitudinal={
"K_P": 1,
"K_D": 0.0,
"K_I": 0.0,
"dt": self.simulation_sync_timestep,
},
)
state_information = self.carla_handler.get_state_information_new(
self.ego_vehicle, self.original_lane)
(
current_lane_waypoints,
left_lane_waypoints,
right_lane_waypoints,
front_vehicle,
rear_vehicle,
actors_in_current_lane,
actors_in_left_lane,
actors_in_right_lane,
) = state_information
self.current_lane = current_lane_waypoints
self.left_lane = left_lane_waypoints
# self.carla_handler.draw_waypoints(current_lane_waypoints, life_time=100)
# self.carla_handler.draw_waypoints(left_lane_waypoints, life_time=100, color=True)
pedestrians_on_current_road = self.carla_handler.get_pedestrian_information(
self.ego_vehicle)
##############################################################################################################
# publish the first frame
# Current Lane
self.lane_cur = self.getLanePoints(current_lane_waypoints)
# Left Lane
self.lane_left = self.getLanePoints(left_lane_waypoints)
# Right Lane
self.lane_right = self.getLanePoints(right_lane_waypoints)
vehicle_ego = self.getVehicleState(self.ego_vehicle)
# Front vehicle
if front_vehicle == None:
vehicle_front = VehicleState() # vehicle_ego
else:
vehicle_front = self.getVehicleState(front_vehicle)
# Rear vehicle
if rear_vehicle == None:
vehicle_rear = VehicleState() # vehicle_ego
else:
vehicle_rear = self.getVehicleState(rear_vehicle)
# Contruct enviroment state ROS message
env_state = EnvironmentState()
env_state.cur_vehicle_state = vehicle_ego
env_state.front_vehicle_state = vehicle_front
env_state.back_vehicle_state = vehicle_rear
env_state.adjacent_lane_vehicles, _ = self.getClosest(
[self.getVehicleState(actor) for actor in actors_in_left_lane],
vehicle_ego,
self.max_num_vehicles,
) # TODO : Only considering left lane for now. Need to make this more general
env_state.current_lane = self.lane_cur
env_state.next_lane = self.lane_left
env_state.max_num_vehicles = self.max_num_vehicles
env_state.speed_limit = 20
## Pedestrian
if self.pedestrian is not None:
env_state.nearest_pedestrian = self.getClosestPedestrian(
[
self.getPedestrianState(actor)
for actor in [self.pedestrian]
],
vehicle_ego,
1,
)[0]
else:
env_state.nearest_pedestrian = Pedestrian()
env_state.nearest_pedestrian.exist = False
reward_info = RewardInfo()
reward_info.time_elapsed = self.timestamp
reward_info.new_run = self.first_run
reward_info.collision = self.collision_marker
env_state.reward = reward_info
def updateMessages(self):
self.lock.acquire()
publish_msg = EnvironmentState()
# current lane
publish_msg.current_lane = self.lanes[self.cur_lane].convert2ROS()
# next lane
next_lane = self.cur_lane - 1
if next_lane >= 0:
publish_msg.next_lane = self.lanes[next_lane].convert2ROS()
# vehicles
closest_next_lane_vehicles = []
front_veh = False
back_veh = False
def sort_veh(veh_tup):
return veh_tup[1]
veh_dir_vec = np.array(np.cos(self.controlling_vehicle.theta),
-np.sin(self.controlling_vehicle.theta))
for veh in self.vehicles:
veh_dist = np.linalg.norm(
np.array(veh.x - self.controlling_vehicle.x,
veh.y - self.controlling_vehicle.y))
if veh.lane == next_lane:
# next lane vehicles
if len(closest_next_lane_vehicles) < NUM_NEXT_LANE_VEHICLES:
closest_next_lane_vehicles.append(tuple((veh, veh_dist)))
closest_next_lane_vehicles.sort(key=sort_veh)
elif veh_dist < closest_next_lane_vehicles[-1][1]:
closest_next_lane_vehicles[-1] = tuple((veh, veh_dist))
closest_next_lane_vehicles.sort(key=sort_veh)
if veh.lane == self.cur_lane:
x_diff = veh.x - self.controlling_vehicle.x
y_diff = veh.y - self.controlling_vehicle.y
pos_diff = np.array(x_diff, y_diff)
proj = np.dot(veh_dir_vec, pos_diff)
if proj > 0: # front vehicle
if front_veh:
if front_veh[1] > veh_dist:
front_veh = tuple((veh, veh_dist))
else:
front_veh = tuple((veh, veh_dist))
else: # back vehicle
if back_veh:
if back_veh[1] > veh_dist:
back_veh = tuple((veh, veh_dist))
else:
back_veh = tuple((veh, veh_dist))
# next lane vehicle documentation
for veh_tup in closest_next_lane_vehicles:
publish_msg.adjacent_lane_vehicles.append(veh_tup[0].convert2ROS())
# front vehicle
if front_veh:
publish_msg.front_vehicle_state = front_veh[0].convert2ROS()
else:
# ToDo: no front vehicle
publish_msg.front_vehicle_state = VehicleState()
# back vehicle
if back_veh:
publish_msg.back_vehicle_state = back_veh[0].convert2ROS()
else:
# ToDo: no back vehicle
publish_msg.back_vehicle_state = VehicleState()
# ego vehicle
publish_msg.cur_vehicle_state = self.controlling_vehicle.convert2ROS()
# max # vehicles
publish_msg.max_num_vehicles = NUM_NEXT_LANE_VEHICLES
# speed limit
publish_msg.speed_limit = 20
# reward info
reward_info = RewardInfo()
reward_info.time_elapsed = self.timestamp
reward_info.new_run = self.first_run
reward_info.collision = self.collisionCheck()
reward_info.action_progress = self.action_progress
reward_info.end_of_action = self.end_of_action
publish_msg.reward = reward_info
self.env_msg = publish_msg
self.lock.release()