class CarlaEnv(gym.Env):
"""An OpenAI gym wrapper for CARLA simulator."""
def __init__(self, params):
# parameters
self.display_size = params['display_size'] # rendering screen size
self.max_past_step = params['max_past_step']
self.number_of_vehicles = params['number_of_vehicles']
self.number_of_walkers = params['number_of_walkers']
self.dt = params['dt']
self.task_mode = params['task_mode']
self.max_time_episode = params['max_time_episode']
self.max_waypt = params['max_waypt']
self.obs_range = params['obs_range']
self.lidar_bin = params['lidar_bin']
self.d_behind = params['d_behind']
self.obs_size = int(self.obs_range / self.lidar_bin)
self.out_lane_thres = params['out_lane_thres']
self.desired_speed = params['desired_speed']
self.max_ego_spawn_times = params['max_ego_spawn_times']
self.display_route = params['display_route']
if 'pixor' in params.keys():
self.pixor = params['pixor']
self.pixor_size = params['pixor_size']
else:
self.pixor = False
# Terminal condition
if 'terminal_condition' in params:
self.terminal_condition = params['terminal_condition']
else:
self.terminal_condition = 'all'
# Destination
if params['task_mode'] == 'roundabout':
self.dests = [[4.46, -61.46, 0], [-49.53, -2.89, 0],
[-6.48, 55.47, 0], [35.96, 3.33, 0]]
else:
self.dests = None
# action and observation spaces
self.discrete = params['discrete']
self.discrete_act = [params['discrete_acc'],
params['discrete_steer']] # acc, steer
self.n_acc = len(self.discrete_act[0])
self.n_steer = len(self.discrete_act[1])
if self.discrete:
self.action_space = spaces.Discrete(self.n_acc * self.n_steer)
else:
self.action_space = spaces.Box(
np.array([
params['continuous_accel_range'][0],
params['continuous_steer_range'][0]
]),
np.array([
params['continuous_accel_range'][1],
params['continuous_steer_range'][1]
]),
dtype=np.float32) # acc, steer
observation_space_dict = {
'camera':
spaces.Box(low=0,
high=255,
shape=(self.obs_size, self.obs_size, 3),
dtype=np.uint8),
'lidar':
spaces.Box(low=0,
high=255,
shape=(self.obs_size, self.obs_size, 3),
dtype=np.uint8),
'birdeye':
spaces.Box(low=0,
high=255,
shape=(self.obs_size, self.obs_size, 3),
dtype=np.uint8),
'state':
spaces.Box(np.array([-2, -1, -5, 0]),
np.array([2, 1, 30, 1]),
dtype=np.float32)
}
if self.pixor:
observation_space_dict.update({
'roadmap':
spaces.Box(low=0,
high=255,
shape=(self.obs_size, self.obs_size, 3),
dtype=np.uint8),
'vh_clas':
spaces.Box(low=0,
high=1,
shape=(self.pixor_size, self.pixor_size, 1),
dtype=np.float32),
'vh_regr':
spaces.Box(low=-5,
high=5,
shape=(self.pixor_size, self.pixor_size, 6),
dtype=np.float32),
'pixor_state':
spaces.Box(np.array([-1000, -1000, -1, -1, -5]),
np.array([1000, 1000, 1, 1, 20]),
dtype=np.float32)
})
self.observation_space = spaces.Dict(observation_space_dict)
# Connect to carla server and get world object
print('connecting to Carla server...')
client = carla.Client('localhost', params['port'])
client.set_timeout(10.0)
self.world = client.load_world(params['town'])
print('Carla server connected!')
# Set weather
self.world.set_weather(carla.WeatherParameters.ClearNoon)
# Get spawn points
self.vehicle_spawn_points = list(
self.world.get_map().get_spawn_points())
self.walker_spawn_points = []
for i in range(self.number_of_walkers):
spawn_point = carla.Transform()
loc = self.world.get_random_location_from_navigation()
if (loc != None):
spawn_point.location = loc
self.walker_spawn_points.append(spawn_point)
# Create the ego vehicle blueprint
self.ego_bp = self._create_vehicle_bluepprint(
params['ego_vehicle_filter'], color='49,8,8')
# Collision sensor
self.collision_hist = [] # The collision history
self.collision_hist_l = 1 # collision history length
self.collision_bp = self.world.get_blueprint_library().find(
'sensor.other.collision')
# Lidar sensor
self.lidar_data = None
self.lidar_height = 2.1
self.lidar_trans = carla.Transform(
carla.Location(x=0.0, z=self.lidar_height))
self.lidar_bp = self.world.get_blueprint_library().find(
'sensor.lidar.ray_cast')
self.lidar_bp.set_attribute('channels', '32')
self.lidar_bp.set_attribute('range', '5000')
# Camera sensor
self.camera_img = np.zeros((self.obs_size, self.obs_size, 3),
dtype=np.uint8)
self.camera_trans = carla.Transform(carla.Location(x=0.8, z=1.7))
self.camera_bp = self.world.get_blueprint_library().find(
'sensor.camera.rgb')
# Modify the attributes of the blueprint to set image resolution and field of view.
self.camera_bp.set_attribute('image_size_x', str(self.obs_size))
self.camera_bp.set_attribute('image_size_y', str(self.obs_size))
self.camera_bp.set_attribute('fov', '110')
# Set the time in seconds between sensor captures
self.camera_bp.set_attribute('sensor_tick', '0.02')
# Set fixed simulation step for synchronous mode
self.settings = self.world.get_settings()
self.settings.fixed_delta_seconds = self.dt
# Record the time of total steps and resetting steps
self.reset_step = 0
self.total_step = 0
# Initialize the renderer
self._init_renderer()
# Get pixel grid points
if self.pixor:
x, y = np.meshgrid(
np.arange(self.pixor_size),
np.arange(self.pixor_size)) # make a canvas with coordinates
x, y = x.flatten(), y.flatten()
self.pixel_grid = np.vstack((x, y)).T
def reset(self):
# Clear sensor objects
self.collision_sensor = None
self.lidar_sensor = None
self.camera_sensor = None
# Delete sensors, vehicles and walkers
self._clear_all_actors([
'sensor.other.collision', 'sensor.lidar.ray_cast',
'sensor.camera.rgb', 'vehicle.*', 'controller.ai.walker',
'walker.*'
])
# Disable sync mode
self._set_synchronous_mode(False)
# Spawn surrounding vehicles
random.shuffle(self.vehicle_spawn_points)
count = self.number_of_vehicles
if count > 0:
for spawn_point in self.vehicle_spawn_points:
if self._try_spawn_random_vehicle_at(spawn_point,
number_of_wheels=[4]):
count -= 1
if count <= 0:
break
while count > 0:
if self._try_spawn_random_vehicle_at(random.choice(
self.vehicle_spawn_points),
number_of_wheels=[4]):
count -= 1
# Spawn pedestrians
random.shuffle(self.walker_spawn_points)
count = self.number_of_walkers
if count > 0:
for spawn_point in self.walker_spawn_points:
if self._try_spawn_random_walker_at(spawn_point):
count -= 1
if count <= 0:
break
while count > 0:
if self._try_spawn_random_walker_at(
random.choice(self.walker_spawn_points)):
count -= 1
# Get actors polygon list
self.vehicle_polygons = []
vehicle_poly_dict = self._get_actor_polygons('vehicle.*')
self.vehicle_polygons.append(vehicle_poly_dict)
self.walker_polygons = []
walker_poly_dict = self._get_actor_polygons('walker.*')
self.walker_polygons.append(walker_poly_dict)
# Spawn the ego vehicle
ego_spawn_times = 0
while True:
if ego_spawn_times > self.max_ego_spawn_times:
self.reset()
if self.task_mode == 'random':
transform = random.choice(self.vehicle_spawn_points)
if self.task_mode == 'roundabout':
self.start = [52.1 + np.random.uniform(-5, 5), -4.2,
178.66] # random
# self.start=[52.1,-4.2, 178.66] # static
transform = set_carla_transform(self.start)
if self._try_spawn_ego_vehicle_at(transform):
break
else:
ego_spawn_times += 1
time.sleep(0.1)
# Add collision sensor
self.collision_sensor = self.world.spawn_actor(self.collision_bp,
carla.Transform(),
attach_to=self.ego)
self.collision_sensor.listen(lambda event: get_collision_hist(event))
def get_collision_hist(event):
impulse = event.normal_impulse
intensity = np.sqrt(impulse.x**2 + impulse.y**2 + impulse.z**2)
self.collision_hist.append(intensity)
if len(self.collision_hist) > self.collision_hist_l:
self.collision_hist.pop(0)
self.collision_hist = []
# Add lidar sensor
self.lidar_sensor = self.world.spawn_actor(self.lidar_bp,
self.lidar_trans,
attach_to=self.ego)
self.lidar_sensor.listen(lambda data: get_lidar_data(data))
def get_lidar_data(data):
self.lidar_data = data
# Add camera sensor
self.camera_sensor = self.world.spawn_actor(self.camera_bp,
self.camera_trans,
attach_to=self.ego)
self.camera_sensor.listen(lambda data: get_camera_img(data))
def get_camera_img(data):
array = np.frombuffer(data.raw_data, dtype=np.dtype("uint8"))
array = np.reshape(array, (data.height, data.width, 4))
array = array[:, :, :3]
array = array[:, :, ::-1]
self.camera_img = array
# Update timesteps
self.time_step = 0
self.reset_step += 1
# Enable sync mode
self.settings.synchronous_mode = True
self.world.apply_settings(self.settings)
self.routeplanner = RoutePlanner(self.ego, self.max_waypt)
self.waypoints, _, self.vehicle_front = self.routeplanner.run_step()
# Set ego information for render
self.birdeye_render.set_hero(self.ego, self.ego.id)
return self._get_obs()
def step(self, action):
# Calculate acceleration and steering
if self.discrete:
acc = self.discrete_act[0][action // self.n_steer]
steer = self.discrete_act[1][action % self.n_steer]
else:
acc = action[0]
steer = action[1]
# Convert acceleration to throttle and brake
if acc > 0:
throttle = np.clip(acc / 3, 0, 1)
brake = 0
else:
throttle = 0
brake = np.clip(-acc / 8, 0, 1)
# Apply control
act = carla.VehicleControl(throttle=float(throttle),
steer=float(-steer),
brake=float(brake))
self.ego.apply_control(act)
self.world.tick()
# Append actors polygon list
vehicle_poly_dict = self._get_actor_polygons('vehicle.*')
self.vehicle_polygons.append(vehicle_poly_dict)
while len(self.vehicle_polygons) > self.max_past_step:
self.vehicle_polygons.pop(0)
walker_poly_dict = self._get_actor_polygons('walker.*')
self.walker_polygons.append(walker_poly_dict)
while len(self.walker_polygons) > self.max_past_step:
self.walker_polygons.pop(0)
# route planner
self.waypoints, _, self.vehicle_front = self.routeplanner.run_step()
# state information
info = {
'waypoints': self.waypoints,
'vehicle_front': self.vehicle_front
}
# Update timesteps
self.time_step += 1
self.total_step += 1
return (self._get_obs(), self._get_reward(), self._terminal(),
copy.deepcopy(info))
def seed(self, seed=None):
self.np_random, seed = seeding.np_random(seed)
return [seed]
def render(self, mode):
pass
def _create_vehicle_bluepprint(self,
actor_filter,
color=None,
number_of_wheels=[4]):
"""Create the blueprint for a specific actor type.
Args:
actor_filter: a string indicating the actor type, e.g, 'vehicle.lincoln*'.
Returns:
bp: the blueprint object of carla.
"""
blueprints = self.world.get_blueprint_library().filter(actor_filter)
blueprint_library = []
for nw in number_of_wheels:
blueprint_library = blueprint_library + [
x for x in blueprints
if int(x.get_attribute('number_of_wheels')) == nw
]
bp = random.choice(blueprint_library)
if bp.has_attribute('color'):
if not color:
color = random.choice(
bp.get_attribute('color').recommended_values)
bp.set_attribute('color', color)
return bp
def _init_renderer(self):
"""Initialize the birdeye view renderer.
"""
pygame.init()
self.display = pygame.display.set_mode(
(self.display_size * 3, self.display_size),
pygame.HWSURFACE | pygame.DOUBLEBUF)
pixels_per_meter = self.display_size / self.obs_range
pixels_ahead_vehicle = (self.obs_range / 2 -
self.d_behind) * pixels_per_meter
birdeye_params = {
'screen_size': [self.display_size, self.display_size],
'pixels_per_meter': pixels_per_meter,
'pixels_ahead_vehicle': pixels_ahead_vehicle
}
self.birdeye_render = BirdeyeRender(self.world, birdeye_params)
def _set_synchronous_mode(self, synchronous=True):
"""Set whether to use the synchronous mode.
"""
self.settings.synchronous_mode = synchronous
self.world.apply_settings(self.settings)
def _try_spawn_random_vehicle_at(self, transform, number_of_wheels=[4]):
"""Try to spawn a surrounding vehicle at specific transform with random bluprint.
Args:
transform: the carla transform object.
Returns:
Bool indicating whether the spawn is successful.
"""
blueprint = self._create_vehicle_bluepprint(
'vehicle.*', number_of_wheels=number_of_wheels)
blueprint.set_attribute('role_name', 'autopilot')
vehicle = self.world.try_spawn_actor(blueprint, transform)
if vehicle is not None:
vehicle.set_autopilot()
return True
return False
def _try_spawn_random_walker_at(self, transform):
"""Try to spawn a walker at specific transform with random bluprint.
Args:
transform: the carla transform object.
Returns:
Bool indicating whether the spawn is successful.
"""
walker_bp = random.choice(
self.world.get_blueprint_library().filter('walker.*'))
# set as not invencible
if walker_bp.has_attribute('is_invincible'):
walker_bp.set_attribute('is_invincible', 'false')
walker_actor = self.world.try_spawn_actor(walker_bp, transform)
if walker_actor is not None:
walker_controller_bp = self.world.get_blueprint_library().find(
'controller.ai.walker')
walker_controller_actor = self.world.spawn_actor(
walker_controller_bp, carla.Transform(), walker_actor)
# start walker
walker_controller_actor.start()
# set walk to random point
walker_controller_actor.go_to_location(
self.world.get_random_location_from_navigation())
# random max speed
walker_controller_actor.set_max_speed(
1 + random.random()
) # max speed between 1 and 2 (default is 1.4 m/s)
return True
return False
def _try_spawn_ego_vehicle_at(self, transform):
"""Try to spawn the ego vehicle at specific transform.
Args:
transform: the carla transform object.
Returns:
Bool indicating whether the spawn is successful.
"""
vehicle = None
# Check if ego position overlaps with surrounding vehicles
overlap = False
for idx, poly in self.vehicle_polygons[-1].items():
poly_center = np.mean(poly, axis=0)
ego_center = np.array([transform.location.x, transform.location.y])
dis = np.linalg.norm(poly_center - ego_center)
if dis > 8:
continue
else:
overlap = True
break
if not overlap:
vehicle = self.world.try_spawn_actor(self.ego_bp, transform)
if vehicle is not None:
self.ego = vehicle
return True
return False
def _get_actor_polygons(self, filt):
"""Get the bounding box polygon of actors.
Args:
filt: the filter indicating what type of actors we'll look at.
Returns:
actor_poly_dict: a dictionary containing the bounding boxes of specific actors.
"""
actor_poly_dict = {}
for actor in self.world.get_actors().filter(filt):
# Get x, y and yaw of the actor
trans = actor.get_transform()
x = trans.location.x
y = trans.location.y
yaw = trans.rotation.yaw / 180 * np.pi
# Get length and width
bb = actor.bounding_box
l = bb.extent.x
w = bb.extent.y
# Get bounding box polygon in the actor's local coordinate
poly_local = np.array([[l, w], [l, -w], [-l, -w],
[-l, w]]).transpose()
# Get rotation matrix to transform to global coordinate
R = np.array([[np.cos(yaw), -np.sin(yaw)],
[np.sin(yaw), np.cos(yaw)]])
# Get global bounding box polygon
poly = np.matmul(R, poly_local).transpose() + np.repeat(
[[x, y]], 4, axis=0)
actor_poly_dict[actor.id] = poly
return actor_poly_dict
def _get_obs(self):
"""Get the observations."""
## Birdeye rendering
self.birdeye_render.vehicle_polygons = self.vehicle_polygons
self.birdeye_render.walker_polygons = self.walker_polygons
self.birdeye_render.waypoints = self.waypoints
# birdeye view with roadmap and actors
birdeye_render_types = ['roadmap', 'actors']
if self.display_route:
birdeye_render_types.append('waypoints')
self.birdeye_render.render(self.display, birdeye_render_types)
birdeye = pygame.surfarray.array3d(self.display)
birdeye = birdeye[0:self.display_size, :, :]
birdeye = display_to_rgb(birdeye, self.obs_size)
# Roadmap
if self.pixor:
roadmap_render_types = ['roadmap']
if self.display_route:
roadmap_render_types.append('waypoints')
self.birdeye_render.render(self.display, roadmap_render_types)
roadmap = pygame.surfarray.array3d(self.display)
roadmap = roadmap[0:self.display_size, :, :]
roadmap = display_to_rgb(roadmap, self.obs_size)
# Add ego vehicle
for i in range(self.obs_size):
for j in range(self.obs_size):
if abs(birdeye[i, j, 0] - 255) < 20 and abs(
birdeye[i, j, 1] -
0) < 20 and abs(birdeye[i, j, 0] - 255) < 20:
roadmap[i, j, :] = birdeye[i, j, :]
# Display birdeye image
birdeye_surface = rgb_to_display_surface(birdeye, self.display_size)
self.display.blit(birdeye_surface, (0, 0))
## Lidar image generation
point_cloud = []
# Get point cloud data
for location in self.lidar_data:
point_cloud.append([location.x, location.y, -location.z])
point_cloud = np.array(point_cloud)
# Separate the 3D space to bins for point cloud, x and y is set according to self.lidar_bin,
# and z is set to be two bins.
y_bins = np.arange(-(self.obs_range - self.d_behind),
self.d_behind + self.lidar_bin, self.lidar_bin)
x_bins = np.arange(-self.obs_range / 2,
self.obs_range / 2 + self.lidar_bin, self.lidar_bin)
z_bins = [-self.lidar_height - 1, -self.lidar_height + 0.25, 1]
# Get lidar image according to the bins
lidar, _ = np.histogramdd(point_cloud, bins=(x_bins, y_bins, z_bins))
lidar[:, :, 0] = np.array(lidar[:, :, 0] > 0, dtype=np.uint8)
lidar[:, :, 1] = np.array(lidar[:, :, 1] > 0, dtype=np.uint8)
# Add the waypoints to lidar image
if self.display_route:
wayptimg = (birdeye[:, :, 0] <= 10) * (birdeye[:, :, 1] <= 10) * (
birdeye[:, :, 2] >= 240)
else:
wayptimg = birdeye[:, :, 0] < 0 # Equal to a zero matrix
wayptimg = np.expand_dims(wayptimg, axis=2)
wayptimg = np.fliplr(np.rot90(wayptimg, 3))
# Get the final lidar image
lidar = np.concatenate((lidar, wayptimg), axis=2)
lidar = np.flip(lidar, axis=1)
lidar = np.rot90(lidar, 1)
lidar = lidar * 255
# Display lidar image
lidar_surface = rgb_to_display_surface(lidar, self.display_size)
self.display.blit(lidar_surface, (self.display_size, 0))
## Display camera image
camera = resize(self.camera_img, (self.obs_size, self.obs_size)) * 255
camera_surface = rgb_to_display_surface(camera, self.display_size)
self.display.blit(camera_surface, (self.display_size * 2, 0))
# Display on pygame
pygame.display.flip()
# State observation
ego_trans = self.ego.get_transform()
ego_x = ego_trans.location.x
ego_y = ego_trans.location.y
ego_yaw = ego_trans.rotation.yaw / 180 * np.pi
lateral_dis, w = get_preview_lane_dis(self.waypoints, ego_x, ego_y)
delta_yaw = np.arcsin(
np.cross(w, np.array(np.array([np.cos(ego_yaw),
np.sin(ego_yaw)]))))
v = self.ego.get_velocity()
speed = np.sqrt(v.x**2 + v.y**2)
state = np.array([lateral_dis, -delta_yaw, speed, self.vehicle_front])
if self.pixor:
## Vehicle classification and regression maps (requires further normalization)
vh_clas = np.zeros((self.pixor_size, self.pixor_size))
vh_regr = np.zeros((self.pixor_size, self.pixor_size, 6))
# Generate the PIXOR image. Note in CARLA it is using left-hand coordinate
# Get the 6-dim geom parametrization in PIXOR, here we use pixel coordinate
for actor in self.world.get_actors().filter('vehicle.*'):
x, y, yaw, l, w = get_info(actor)
x_local, y_local, yaw_local = get_local_pose(
(x, y, yaw), (ego_x, ego_y, ego_yaw))
if actor.id != self.ego.id:
if abs(
y_local
) < self.obs_range / 2 + 1 and x_local < self.obs_range - self.d_behind + 1 and x_local > -self.d_behind - 1:
x_pixel, y_pixel, yaw_pixel, l_pixel, w_pixel = get_pixel_info(
local_info=(x_local, y_local, yaw_local, l, w),
d_behind=self.d_behind,
obs_range=self.obs_range,
image_size=self.pixor_size)
cos_t = np.cos(yaw_pixel)
sin_t = np.sin(yaw_pixel)
logw = np.log(w_pixel)
logl = np.log(l_pixel)
pixels = get_pixels_inside_vehicle(
pixel_info=(x_pixel, y_pixel, yaw_pixel, l_pixel,
w_pixel),
pixel_grid=self.pixel_grid)
for pixel in pixels:
vh_clas[pixel[0], pixel[1]] = 1
dx = x_pixel - pixel[0]
dy = y_pixel - pixel[1]
vh_regr[pixel[0], pixel[1], :] = np.array(
[cos_t, sin_t, dx, dy, logw, logl])
# Flip the image matrix so that the origin is at the left-bottom
vh_clas = np.flip(vh_clas, axis=0)
vh_regr = np.flip(vh_regr, axis=0)
# Pixor state, [x, y, cos(yaw), sin(yaw), speed]
pixor_state = [
ego_x, ego_y,
np.cos(ego_yaw),
np.sin(ego_yaw), speed
]
obs = {
'camera': camera.astype(np.uint8),
'lidar': lidar.astype(np.uint8),
'birdeye': birdeye.astype(np.uint8),
'state': state,
}
if self.pixor:
obs.update({
'roadmap':
roadmap.astype(np.uint8),
'vh_clas':
np.expand_dims(vh_clas, -1).astype(np.float32),
'vh_regr':
vh_regr.astype(np.float32),
'pixor_state':
pixor_state,
})
return obs
def _get_reward(self):
"""Calculate the step reward."""
# reward for speed tracking
v = self.ego.get_velocity()
speed = np.sqrt(v.x**2 + v.y**2)
r_speed = -abs(speed - self.desired_speed)
# reward for collision
r_collision = 0
if len(self.collision_hist) > 0:
r_collision = -1
# reward for steering:
r_steer = -self.ego.get_control().steer**2
# reward for out of lane
ego_x, ego_y = get_pos(self.ego)
dis, w = get_lane_dis(self.waypoints, ego_x, ego_y)
r_out = 0
if abs(dis) > self.out_lane_thres:
r_out = -1
# longitudinal speed
lspeed = np.array([v.x, v.y])
lspeed_lon = np.dot(lspeed, w)
# cost for too fast
r_fast = 0
if lspeed_lon > self.desired_speed:
r_fast = -1
# cost for lateral acceleration
r_lat = -abs(self.ego.get_control().steer) * lspeed_lon**2
r = 200 * r_collision + 1 * lspeed_lon + 10 * r_fast + 1 * r_out + r_steer * 5 + 0.2 * r_lat - 0.1
return r
# def _terminal(self):
# """Calculate whether to terminate the current episode."""
# # Get ego state
# ego_x, ego_y = get_pos(self.ego)
# # If collides
# if len(self.collision_hist)>0:
# return True
# # If reach maximum timestep
# if self.time_step>self.max_time_episode:
# return True
# # If at destination
# if self.dests is not None: # If at destination
# for dest in self.dests:
# if np.sqrt((ego_x-dest[0])**2+(ego_y-dest[1])**2)<4:
# return True
# # If out of lane
# dis, _ = get_lane_dis(self.waypoints, ego_x, ego_y)
# if abs(dis) > self.out_lane_thres:
# return True
# return False
def _terminal(self):
"""
Calculate whether to terminate the current episode with own condition
Add the terminal condition into 'terminal_condition' parameters:
Collision: if Collision happen
Step: maximum step
Destination: if ego reach the destination
Distence: if ego is out of lane
"""
# get ego state
ego_x, ego_y = get_pos(self.ego)
# If collides
if len(self.collision_hist) > 0 and (
'Collision' in self.terminal_condition
or self.terminal_condition == 'all'):
return True
# If reach maximum timestep
if self.time_step > self.max_time_episode and (
'Step' in self.terminal_condition
or self.terminal_condition == 'all'):
return True
# If at destination
if self.dests is not None and ('Destination' in self.terminal_condition
or self.terminal_condition
== 'all'): # If at destination
for dest in self.dests:
if np.sqrt((ego_x - dest[0])**2 + (ego_y - dest[1])**2) < 4:
return True
# If out of lane
dis, _ = get_lane_dis(self.waypoints, ego_x, ego_y)
if abs(dis) > self.out_lane_thres and (
'Distence' in self.terminal_condition
or self.terminal_condition == 'all'):
return True
return False
def _clear_all_actors(self, actor_filters):
"""Clear specific actors."""
for actor_filter in actor_filters:
for actor in self.world.get_actors().filter(actor_filter):
if actor.is_alive:
if actor.type_id == 'controller.ai.walker':
actor.stop()
actor.destroy()
class CarlaEnv(gym.Env):
"""An OpenAI gym wrapper for CARLA simulator."""
def __init__(self, params):
# parameters
self.display_size = params['display_size'] # rendering screen size
self.max_past_step = params['max_past_step']
self.number_of_vehicles = params['number_of_vehicles']
self.number_of_walkers = params['number_of_walkers']
self.dt = params['dt']
self.task_mode = params['task_mode']
self.max_time_episode = params['max_time_episode']
self.max_waypt = params['max_waypt']
self.obs_range = params['obs_range']
self.lidar_bin = params['lidar_bin']
self.d_behind = params['d_behind']
self.obs_size = int(self.obs_range / self.lidar_bin)
self.image_height = params['image_height']
self.image_width = params['image_width']
self.out_lane_thres = params['out_lane_thres']
self.desired_speed = params['desired_speed']
self.max_ego_spawn_times = params['max_ego_spawn_times']
self.display_route = params['display_route']
if 'pixor' in params.keys():
self.pixor = params['pixor']
self.pixor_size = params['pixor_size']
else:
self.pixor = False
if 'image_xform' in params.keys():
self.xform = True
else:
self.xform = False
self.step_rewards = []
self.step_actions = []
self.step_steering = []
self.max_distance = 2.0
# Destination
if params['task_mode'] == 'roundabout':
self.dests = [[4.46, -61.46, 0], [-49.53, -2.89, 0],
[-6.48, 55.47, 0], [35.96, 3.33, 0]]
else:
self.dests = None
# action and observation spaces
self.discrete = params['discrete']
self.discrete_act = [params['discrete_acc'],
params['discrete_steer']] # acc, steer
self.n_acc = len(self.discrete_act[0])
self.n_steer = len(self.discrete_act[1])
if self.discrete:
self.action_space = spaces.Discrete(self.n_acc * self.n_steer)
else:
self.action_space = spaces.Box(
np.array([
params['continuous_accel_range'][0],
params['continuous_steer_range'][0]
]),
np.array([
params['continuous_accel_range'][1],
params['continuous_steer_range'][1]
]),
dtype=np.float32) # acc, steer
observation_space_dict = {
'orig_camera':
spaces.Box(low=0,
high=255,
shape=(self.image_height, self.image_width, 3),
dtype=np.uint8),
'camera':
spaces.Box(low=0,
high=255,
shape=(self.obs_size, self.obs_size, 3),
dtype=np.uint8),
'lidar':
spaces.Box(low=0,
high=255,
shape=(self.obs_size, self.obs_size, 3),
dtype=np.uint8),
'birdeye':
spaces.Box(low=0,
high=255,
shape=(self.obs_size, self.obs_size, 3),
dtype=np.uint8),
'state':
spaces.Box(np.array([-2, -1, -5, 0]),
np.array([2, 1, 30, 1]),
dtype=np.float32)
}
if self.pixor:
observation_space_dict.update({
'roadmap':
spaces.Box(low=0,
high=255,
shape=(self.obs_size, self.obs_size, 3),
dtype=np.uint8),
'vh_clas':
spaces.Box(low=0,
high=1,
shape=(self.pixor_size, self.pixor_size, 1),
dtype=np.float32),
'vh_regr':
spaces.Box(low=-5,
high=5,
shape=(self.pixor_size, self.pixor_size, 6),
dtype=np.float32),
'pixor_state':
spaces.Box(np.array([-1000, -1000, -1, -1, -5]),
np.array([1000, 1000, 1, 1, 20]),
dtype=np.float32)
})
self.observation_space = spaces.Dict(observation_space_dict)
# Connect to carla server and get world object
print('connecting to Carla server...')
client = carla.Client('localhost', params['port'])
client.set_timeout(10.0)
self.client = client
self.world = client.load_world(params['town'])
print('Carla server connected!')
self.birdview_producer = BirdViewProducer(
self.client, # carla.Client
target_size=PixelDimensions(width=self.image_width,
height=self.image_height),
pixels_per_meter=4,
crop_type=BirdViewCropType.FRONT_AND_REAR_AREA)
# Set weather
self.world.set_weather(carla.WeatherParameters.ClearNoon)
# Get spawn points
self.vehicle_spawn_points = list(
self.world.get_map().get_spawn_points())
self.walker_spawn_points = []
for i in range(self.number_of_walkers):
spawn_point = carla.Transform()
loc = self.world.get_random_location_from_navigation()
if (loc != None):
spawn_point.location = loc
self.walker_spawn_points.append(spawn_point)
# Create the ego vehicle blueprint
self.ego_bp = self._create_vehicle_bluepprint(
params['ego_vehicle_filter'], color='49,8,8')
# Collision sensor
self.collision_hist = [] # The collision history
self.collision_hist_l = 1 # collision history length
self.collision_bp = self.world.get_blueprint_library().find(
'sensor.other.collision')
# Lidar sensor
self.lidar_data = None
self.lidar_height = 2.1
self.lidar_trans = carla.Transform(
carla.Location(x=0.0, z=self.lidar_height))
self.lidar_bp = self.world.get_blueprint_library().find(
'sensor.lidar.ray_cast')
self.lidar_bp.set_attribute('channels', '32')
self.lidar_bp.set_attribute('range', '5000')
# Camera sensor
self.camera_img = np.zeros((self.obs_size, self.obs_size, 3),
dtype=np.uint8)
self.original_camera_image = np.zeros(
(self.image_height, self.image_width, 3), dtype=np.uint8)
self.speed = np.zeros((1), dtype=np.uint8)
self.camera_trans = carla.Transform(carla.Location(x=0.8, z=1.7))
self.camera_bp = self.world.get_blueprint_library().find(
'sensor.camera.rgb')
# Modify the attributes of the blueprint to set image resolution and field of view.
self.camera_bp.set_attribute('image_size_x', str(self.obs_size))
self.camera_bp.set_attribute('image_size_y', str(self.obs_size))
self.camera_bp.set_attribute('fov', '110')
# Set the time in seconds between sensor captures
self.camera_bp.set_attribute('sensor_tick', '0.02')
# Set fixed simulation step for synchronous mode
self.settings = self.world.get_settings()
self.settings.fixed_delta_seconds = self.dt
# Record the time of total steps and resetting steps
self.reset_step = 0
self.total_step = 0
# Initialize the renderer
self._init_renderer()
self.current_waypoint_index = 0
self.auto_pilot_mode = False
# Get pixel grid points
if self.pixor:
x, y = np.meshgrid(
np.arange(self.pixor_size),
np.arange(self.pixor_size)) # make a canvas with coordinates
x, y = x.flatten(), y.flatten()
self.pixel_grid = np.vstack((x, y)).T
def get_client(self):
return self.client
def get_car(self):
return self.ego
def get_speed(self):
v = self.ego.get_velocity()
speed_kmh = 3.6 * np.sqrt(v.x**2 + v.y**2 + v.z**2)
return speed_kmh
def reset(self):
# Clear sensor objects
self.collision_sensor = None
self.lidar_sensor = None
self.camera_sensor = None
self.step_rewards = []
self.step_actions = []
self.step_steering = []
self.birdview_producer = BirdViewProducer(
self.client, # carla.Client
target_size=PixelDimensions(width=self.image_width,
height=self.image_height),
pixels_per_meter=4,
crop_type=BirdViewCropType.FRONT_AND_REAR_AREA)
# Delete sensors, vehicles and walkers
self._clear_all_actors([
'sensor.other.collision', 'sensor.lidar.ray_cast',
'sensor.camera.rgb', 'vehicle.*', 'controller.ai.walker',
'walker.*'
])
# Disable sync mode
self._set_synchronous_mode(False)
# Spawn surrounding vehicles
random.shuffle(self.vehicle_spawn_points)
count = self.number_of_vehicles
if count > 0:
for spawn_point in self.vehicle_spawn_points:
if self._try_spawn_random_vehicle_at(spawn_point,
number_of_wheels=[4]):
count -= 1
if count <= 0:
break
while count > 0:
if self._try_spawn_random_vehicle_at(random.choice(
self.vehicle_spawn_points),
number_of_wheels=[4]):
count -= 1
# Spawn pedestrians
random.shuffle(self.walker_spawn_points)
count = self.number_of_walkers
if count > 0:
for spawn_point in self.walker_spawn_points:
if self._try_spawn_random_walker_at(spawn_point):
count -= 1
if count <= 0:
break
while count > 0:
if self._try_spawn_random_walker_at(
random.choice(self.walker_spawn_points)):
count -= 1
# Get actors polygon list
self.vehicle_polygons = []
vehicle_poly_dict = self._get_actor_polygons('vehicle.*')
self.vehicle_polygons.append(vehicle_poly_dict)
self.walker_polygons = []
walker_poly_dict = self._get_actor_polygons('walker.*')
self.walker_polygons.append(walker_poly_dict)
# Spawn the ego vehicle
ego_spawn_times = 0
while True:
if ego_spawn_times > self.max_ego_spawn_times:
self.reset()
if self.task_mode == 'random':
transform = random.choice(self.vehicle_spawn_points)
if self.task_mode == 'roundabout':
self.start = [52.1 + np.random.uniform(-5, 5), -4.2,
178.66] # random
# self.start=[52.1,-4.2, 178.66] # static
transform = set_carla_transform(self.start)
if self._try_spawn_ego_vehicle_at(transform):
break
else:
ego_spawn_times += 1
time.sleep(0.1)
# Add collision sensor
self.collision_sensor = self.world.spawn_actor(self.collision_bp,
carla.Transform(),
attach_to=self.ego)
self.collision_sensor.listen(lambda event: get_collision_hist(event))
def get_collision_hist(event):
impulse = event.normal_impulse
intensity = np.sqrt(impulse.x**2 + impulse.y**2 + impulse.z**2)
self.collision_hist.append(intensity)
if len(self.collision_hist) > self.collision_hist_l:
self.collision_hist.pop(0)
self.collision_hist = []
# Add lidar sensor
self.lidar_sensor = self.world.spawn_actor(self.lidar_bp,
self.lidar_trans,
attach_to=self.ego)
self.lidar_sensor.listen(lambda data: get_lidar_data(data))
def get_lidar_data(data):
self.lidar_data = data
# Add camera sensor
self.camera_sensor = self.world.spawn_actor(self.camera_bp,
self.camera_trans,
attach_to=self.ego)
self.camera_sensor.listen(lambda data: get_camera_img(data))
def get_camera_img(data):
array = np.frombuffer(data.raw_data, dtype=np.dtype("uint8"))
array = np.reshape(array, (data.height, data.width, 4))
array = array[:, :, :3]
i3 = (array) / 255.
array = array[:, :, ::-1]
self.camera_img = array
self.original_camera_image = i3
# Update timesteps
self.time_step = 0
self.reset_step += 1
# Enable sync mode
self.settings.synchronous_mode = True
self.world.apply_settings(self.settings)
self.routeplanner = RoutePlanner(self.ego, self.max_waypt)
self.waypoints, _, self.vehicle_front = self.routeplanner.run_step()
# get all the route waypoints
self.route_waypoints = self.routeplanner._get_waypoints_data()
#print(" Route waypoints : {} ".format(len(self.route_waypoints)))
# Set ego information for render
self.birdeye_render.set_hero(self.ego, self.ego.id)
self.current_waypoint_index = 0
self.step_start_location = self.ego.get_location()
self.step_last_location = self.step_start_location
return self._get_obs()
def dump(self):
print("Step throttle : {} ".format(self.step_actions))
print("Step steering : {} ".format(self.step_steering))
print("Step rewards : {} ".format(self.step_rewards))
def get_steering_angle(self):
physics_control = self.ego.get_physics_control()
for wheel in physics_control.wheels:
print(wheel.max_steer_angle)
def auto(self, value):
self.auto_pilot_mode = value
self.ego.set_autopilot(value)
def isauto(self):
return self.auto_pilot_mode
def move(self):
self.world.tick()
return self._get_obs()
def get_action_auto(self):
control = self.ego.get_control()
return control.throttle, control.steer
def move_auto(self):
self.world.tick()
# Keep track of closest waypoint on the route
transform = self.ego.get_transform()
waypoint_index = self.current_waypoint_index
for _ in range(len(self.waypoints)):
# Check if we passed the next waypoint along the route
next_waypoint_index = waypoint_index + 1
wp = self.route_waypoints[next_waypoint_index %
len(self.waypoints)]
dot = np.dot(
vector(wp.transform.get_forward_vector())[:2],
vector(transform.location - wp.transform.location)[:2])
if dot > 0.0: # Did we pass the waypoint?
waypoint_index += 1 # Go to next waypoint
self.current_waypoint_index = waypoint_index
v_transform = self.ego.get_transform()
current_waypoint = self.route_waypoints[self.current_waypoint_index %
len(self.waypoints)]
next_waypoint = self.route_waypoints[(self.current_waypoint_index + 1)
% len(self.waypoints)]
self.distance_from_center = distance_to_line(
vector(current_waypoint.transform.location),
vector(next_waypoint.transform.location),
vector(v_transform.location))
self.current_waypoint = current_waypoint
self.next_waypoint = next_waypoint
control = self.ego.get_control()
isdone, isout = self._terminal()
reward = self.get_reward_speed(isdone, control.steer, isout)
return self._get_obs(), control.throttle, control.steer, reward, isdone
def step(self, action):
# Calculate acceleration and steering
self.throttle = 0
self.steer = 0
if self.discrete:
#acc = self.discrete_act[0][action//self.n_steer]
#steer = self.discrete_act[1][action%self.n_steer]
#acc = self.discrete_act[0][action]
#steer = self.discrete_act[1][action%self.n_steer]
acc = action[0]
steer = action[1]
else:
acc = action[0]
steer = action[1]
# Keep track of closest waypoint on the route
transform = self.ego.get_transform()
waypoint_index = self.current_waypoint_index
for _ in range(len(self.waypoints)):
# Check if we passed the next waypoint along the route
next_waypoint_index = waypoint_index + 1
wp = self.route_waypoints[next_waypoint_index %
len(self.waypoints)]
dot = np.dot(
vector(wp.transform.get_forward_vector())[:2],
vector(transform.location - wp.transform.location)[:2])
if dot > 0.0: # Did we pass the waypoint?
waypoint_index += 1 # Go to next waypoint
self.current_waypoint_index = waypoint_index
v_transform = self.ego.get_transform()
current_waypoint = self.route_waypoints[self.current_waypoint_index %
len(self.waypoints)]
next_waypoint = self.route_waypoints[(self.current_waypoint_index + 1)
% len(self.waypoints)]
self.distance_from_center = distance_to_line(
vector(current_waypoint.transform.location),
vector(next_waypoint.transform.location),
vector(v_transform.location))
self.current_waypoint = current_waypoint
self.next_waypoint = next_waypoint
#print("current way point idx {} ".format(self.current_waypoint_index))
# Convert acceleration to throttle and brake
if acc > 0:
#throttle = np.clip(acc/3,0,1)
#throttle = np.clip(acc,0,1)
#throttle = acc
#brake = 0
throttle = acc
brake = 0
else:
throttle = 0
brake = 0
#brake = np.clip(-acc/8,0,1)
#brake = np.clip(-acc,0,1)
self.throttle = throttle
self.steer = steer
# Apply control
if self.auto_pilot_mode:
self.world.tick()
else:
act = carla.VehicleControl(throttle=float(throttle),
steer=float(steer),
brake=float(brake))
self.ego.apply_control(act)
self.world.tick()
# Append actors polygon list
vehicle_poly_dict = self._get_actor_polygons('vehicle.*')
self.vehicle_polygons.append(vehicle_poly_dict)
while len(self.vehicle_polygons) > self.max_past_step:
self.vehicle_polygons.pop(0)
walker_poly_dict = self._get_actor_polygons('walker.*')
self.walker_polygons.append(walker_poly_dict)
while len(self.walker_polygons) > self.max_past_step:
self.walker_polygons.pop(0)
# route planner
self.waypoints, _, self.vehicle_front = self.routeplanner.run_step()
# get all the route waypoints
self.route_waypoints = self.routeplanner._get_waypoints_data()
# state information
info = {
'waypoints': self.waypoints,
'vehicle_front': self.vehicle_front
}
# Update timesteps
self.time_step += 1
self.total_step += 1
isdone, isout = self._terminal()
self.step_end_location = self.ego.get_location()
#reward , centre , coll, out = self._get_reward_speed_centering(isdone)
reward = self.get_reward_speed(isdone, steer, isout)
self.step_actions.append(throttle)
self.step_steering.append(steer)
self.step_rewards.append(reward)
if isdone:
print("Final Speed reward : {}".format(reward))
return (self._get_obs(), reward, isdone, copy.deepcopy(info))
def seed(self, seed=None):
self.np_random, seed = seeding.np_random(seed)
return [seed]
def render(self, mode):
pass
def _create_vehicle_bluepprint(self,
actor_filter,
color=None,
number_of_wheels=[4]):
"""Create the blueprint for a specific actor type.
Args:
actor_filter: a string indicating the actor type, e.g, 'vehicle.lincoln*'.
Returns:
bp: the blueprint object of carla.
"""
blueprints = self.world.get_blueprint_library().filter(actor_filter)
blueprint_library = []
for nw in number_of_wheels:
blueprint_library = blueprint_library + [
x for x in blueprints
if int(x.get_attribute('number_of_wheels')) == nw
]
bp = random.choice(blueprint_library)
if bp.has_attribute('color'):
if not color:
color = random.choice(
bp.get_attribute('color').recommended_values)
bp.set_attribute('color', color)
return bp
def _init_renderer(self):
"""Initialize the birdeye view renderer.
"""
pygame.init()
self.display = pygame.display.set_mode(
(self.display_size * 3, self.display_size),
pygame.HWSURFACE | pygame.DOUBLEBUF)
pixels_per_meter = self.display_size / self.obs_range
pixels_ahead_vehicle = (self.obs_range / 2 -
self.d_behind) * pixels_per_meter
birdeye_params = {
'screen_size': [self.display_size, self.display_size],
'pixels_per_meter': pixels_per_meter,
'pixels_ahead_vehicle': pixels_ahead_vehicle
}
self.birdeye_render = BirdeyeRender(self.world, birdeye_params)
def _set_synchronous_mode(self, synchronous=True):
"""Set whether to use the synchronous mode.
"""
self.settings.synchronous_mode = synchronous
self.world.apply_settings(self.settings)
def _try_spawn_random_vehicle_at(self, transform, number_of_wheels=[4]):
"""Try to spawn a surrounding vehicle at specific transform with random bluprint.
Args:
transform: the carla transform object.
Returns:
Bool indicating whether the spawn is successful.
"""
blueprint = self._create_vehicle_bluepprint(
'vehicle.*', number_of_wheels=number_of_wheels)
blueprint.set_attribute('role_name', 'autopilot')
vehicle = self.world.try_spawn_actor(blueprint, transform)
if vehicle is not None:
vehicle.set_autopilot()
return True
return False
def _try_spawn_random_walker_at(self, transform):
"""Try to spawn a walker at specific transform with random bluprint.
Args:
transform: the carla transform object.
Returns:
Bool indicating whether the spawn is successful.
"""
walker_bp = random.choice(
self.world.get_blueprint_library().filter('walker.*'))
# set as not invencible
if walker_bp.has_attribute('is_invincible'):
walker_bp.set_attribute('is_invincible', 'false')
walker_actor = self.world.try_spawn_actor(walker_bp, transform)
if walker_actor is not None:
walker_controller_bp = self.world.get_blueprint_library().find(
'controller.ai.walker')
walker_controller_actor = self.world.spawn_actor(
walker_controller_bp, carla.Transform(), walker_actor)
# start walker
walker_controller_actor.start()
# set walk to random point
walker_controller_actor.go_to_location(
self.world.get_random_location_from_navigation())
# random max speed
walker_controller_actor.set_max_speed(
1 + random.random()
) # max speed between 1 and 2 (default is 1.4 m/s)
return True
return False
def _try_spawn_ego_vehicle_at(self, transform):
"""Try to spawn the ego vehicle at specific transform.
Args:
transform: the carla transform object.
Returns:
Bool indicating whether the spawn is successful.
"""
vehicle = None
# Check if ego position overlaps with surrounding vehicles
overlap = False
for idx, poly in self.vehicle_polygons[-1].items():
poly_center = np.mean(poly, axis=0)
ego_center = np.array([transform.location.x, transform.location.y])
dis = np.linalg.norm(poly_center - ego_center)
if dis > 8:
continue
else:
overlap = True
break
if not overlap:
vehicle = self.world.try_spawn_actor(self.ego_bp, transform)
if vehicle is not None:
vehicle.set_simulate_physics(True)
self.ego = vehicle
return True
return False
def _get_actor_polygons(self, filt):
"""Get the bounding box polygon of actors.
Args:
filt: the filter indicating what type of actors we'll look at.
Returns:
actor_poly_dict: a dictionary containing the bounding boxes of specific actors.
"""
actor_poly_dict = {}
for actor in self.world.get_actors().filter(filt):
# Get x, y and yaw of the actor
trans = actor.get_transform()
x = trans.location.x
y = trans.location.y
yaw = trans.rotation.yaw / 180 * np.pi
# Get length and width
bb = actor.bounding_box
l = bb.extent.x
w = bb.extent.y
# Get bounding box polygon in the actor's local coordinate
poly_local = np.array([[l, w], [l, -w], [-l, -w],
[-l, w]]).transpose()
# Get rotation matrix to transform to global coordinate
R = np.array([[np.cos(yaw), -np.sin(yaw)],
[np.sin(yaw), np.cos(yaw)]])
# Get global bounding box polygon
poly = np.matmul(R, poly_local).transpose() + np.repeat(
[[x, y]], 4, axis=0)
actor_poly_dict[actor.id] = poly
return actor_poly_dict
def _get_obs(self):
"""Get the observations."""
## Birdeye rendering
self.birdeye_render.vehicle_polygons = self.vehicle_polygons
self.birdeye_render.walker_polygons = self.walker_polygons
self.birdeye_render.waypoints = self.waypoints
# birdeye view with roadmap and actors
birdeye_render_types = ['roadmap', 'actors']
if self.display_route:
birdeye_render_types.append('waypoints')
self.birdeye_render.render(self.display, birdeye_render_types)
birdeye = pygame.surfarray.array3d(self.display)
birdeye = birdeye[0:self.display_size, :, :]
birdeye = display_to_rgb(birdeye, self.obs_size)
# Roadmap
if self.pixor:
roadmap_render_types = ['roadmap']
if self.display_route:
roadmap_render_types.append('waypoints')
self.birdeye_render.render(self.display, roadmap_render_types)
roadmap = pygame.surfarray.array3d(self.display)
roadmap = roadmap[0:self.display_size, :, :]
roadmap = display_to_rgb(roadmap, self.obs_size)
# Add ego vehicle
for i in range(self.obs_size):
for j in range(self.obs_size):
if abs(birdeye[i, j, 0] - 255) < 20 and abs(
birdeye[i, j, 1] -
0) < 20 and abs(birdeye[i, j, 0] - 255) < 20:
roadmap[i, j, :] = birdeye[i, j, :]
# Display birdeye image
birdeye_surface = rgb_to_display_surface(birdeye, self.display_size)
self.display.blit(birdeye_surface, (0, 0))
## Lidar image generation
point_cloud = []
# Get point cloud data
for location in self.lidar_data:
point_cloud.append([location.x, location.y, -location.z])
point_cloud = np.array(point_cloud)
# Separate the 3D space to bins for point cloud, x and y is set according to self.lidar_bin,
# and z is set to be two bins.
y_bins = np.arange(-(self.obs_range - self.d_behind),
self.d_behind + self.lidar_bin, self.lidar_bin)
x_bins = np.arange(-self.obs_range / 2,
self.obs_range / 2 + self.lidar_bin, self.lidar_bin)
z_bins = [-self.lidar_height - 1, -self.lidar_height + 0.25, 1]
# Get lidar image according to the bins
lidar, _ = np.histogramdd(point_cloud, bins=(x_bins, y_bins, z_bins))
lidar[:, :, 0] = np.array(lidar[:, :, 0] > 0, dtype=np.uint8)
lidar[:, :, 1] = np.array(lidar[:, :, 1] > 0, dtype=np.uint8)
# Add the waypoints to lidar image
if self.display_route:
wayptimg = (birdeye[:, :, 0] <= 10) * (birdeye[:, :, 1] <= 10) * (
birdeye[:, :, 2] >= 240)
else:
wayptimg = birdeye[:, :, 0] < 0 # Equal to a zero matrix
wayptimg = np.expand_dims(wayptimg, axis=2)
wayptimg = np.fliplr(np.rot90(wayptimg, 3))
# Get the final lidar image
lidar = np.concatenate((lidar, wayptimg), axis=2)
lidar = np.flip(lidar, axis=1)
lidar = np.rot90(lidar, 1)
lidar = lidar * 255
# Display lidar image
lidar_surface = rgb_to_display_surface(lidar, self.display_size)
self.display.blit(lidar_surface, (self.display_size, 0))
## Display camera image
camera = resize(self.camera_img, (self.obs_size, self.obs_size)) * 255
camera_surface = rgb_to_display_surface(camera, self.display_size)
self.display.blit(camera_surface, (self.display_size * 2, 0))
## Display processed camera image
original_camera = resize(self.original_camera_image,
(self.image_height, self.image_width))
#print("original camera : {}".format(original_camera))
o_camera_surface = rgb_to_display_surface(camera, self.display_size)
self.display.blit(o_camera_surface, (self.display_size * 2, 0))
# Display on pygame
pygame.display.flip()
# State observation
ego_trans = self.ego.get_transform()
ego_x = ego_trans.location.x
ego_y = ego_trans.location.y
ego_yaw = ego_trans.rotation.yaw / 180 * np.pi
lateral_dis, w = get_preview_lane_dis(self.waypoints, ego_x, ego_y)
delta_yaw = np.arcsin(
np.cross(w, np.array(np.array([np.cos(ego_yaw),
np.sin(ego_yaw)]))))
v = self.ego.get_velocity()
speed = np.sqrt(v.x**2 + v.y**2)
state = np.array([lateral_dis, -delta_yaw, speed, self.vehicle_front])
if self.pixor:
## Vehicle classification and regression maps (requires further normalization)
vh_clas = np.zeros((self.pixor_size, self.pixor_size))
vh_regr = np.zeros((self.pixor_size, self.pixor_size, 6))
# Generate the PIXOR image. Note in CARLA it is using left-hand coordinate
# Get the 6-dim geom parametrization in PIXOR, here we use pixel coordinate
for actor in self.world.get_actors().filter('vehicle.*'):
x, y, yaw, l, w = get_info(actor)
x_local, y_local, yaw_local = get_local_pose(
(x, y, yaw), (ego_x, ego_y, ego_yaw))
if actor.id != self.ego.id:
if abs(
y_local
) < self.obs_range / 2 + 1 and x_local < self.obs_range - self.d_behind + 1 and x_local > -self.d_behind - 1:
x_pixel, y_pixel, yaw_pixel, l_pixel, w_pixel = get_pixel_info(
local_info=(x_local, y_local, yaw_local, l, w),
d_behind=self.d_behind,
obs_range=self.obs_range,
image_size=self.pixor_size)
cos_t = np.cos(yaw_pixel)
sin_t = np.sin(yaw_pixel)
logw = np.log(w_pixel)
logl = np.log(l_pixel)
pixels = get_pixels_inside_vehicle(
pixel_info=(x_pixel, y_pixel, yaw_pixel, l_pixel,
w_pixel),
pixel_grid=self.pixel_grid)
for pixel in pixels:
vh_clas[pixel[0], pixel[1]] = 1
dx = x_pixel - pixel[0]
dy = y_pixel - pixel[1]
vh_regr[pixel[0], pixel[1], :] = np.array(
[cos_t, sin_t, dx, dy, logw, logl])
# Flip the image matrix so that the origin is at the left-bottom
vh_clas = np.flip(vh_clas, axis=0)
vh_regr = np.flip(vh_regr, axis=0)
# Pixor state, [x, y, cos(yaw), sin(yaw), speed]
pixor_state = [
ego_x, ego_y,
np.cos(ego_yaw),
np.sin(ego_yaw), speed
]
birdview: BirdView = self.birdview_producer.produce(
agent_vehicle=self.ego)
bgr = cv2.cvtColor(BirdViewProducer.as_rgb(birdview), cv.COLOR_BGR2RGB)
v = self.ego.get_velocity()
speed_kmh = 3.6 * np.sqrt(v.x**2 + v.y**2 + v.z**2)
speed_kmh = np.array([speed_kmh])
obs = {
'original_camera': original_camera.astype(np.uint8),
'speed': speed_kmh.astype(np.uint8),
'camera': camera.astype(np.uint8),
'lidar': lidar.astype(np.uint8),
'birdeye': birdeye.astype(np.uint8),
'driving_image': bgr.astype(np.uint8),
'state': state,
}
if self.pixor:
obs.update({
'roadmap':
roadmap.astype(np.uint8),
'vh_clas':
np.expand_dims(vh_clas, -1).astype(np.float32),
'vh_regr':
vh_regr.astype(np.float32),
'pixor_state':
pixor_state,
})
return obs
def get_reward_speed(self, isdone, steer, isout):
v = self.ego.get_velocity()
speed_kmh = 3.6 * np.sqrt(v.x**2 + v.y**2 + v.z**2)
if speed_kmh <= min_speed:
r_speed = speed_kmh
elif speed_kmh > max_speed:
r_speed = max_speed - speed_kmh
else:
r_speed = speed_kmh
r_steer = -(steer * steer)
r_collision = 0
if len(self.collision_hist) > 0:
r_collision = -10.0
r_out = 0
if isout:
r_out = -1
reward_t = r_speed + r_steer + r_collision + r_out - 0.1
if math.isnan(reward_t):
print("Reward is Nan")
print("r_speed : {} ".format(r_speed))
print("r_steer : {} ".format(r_steer))
print("steer : {} ".format(steer))
if reward_t == 0:
print("Reward is 0")
return reward_t
def _get_reward_speed_centering(self, isdone):
"""
reward = Positive speed reward for being close to target speed,
however, quick decline in reward beyond target speed
* centering factor (1 when centered, 0 when not)
* angle factor (1 when aligned with the road, 0 when more than 20 degress off)
"""
v = self.ego.get_velocity()
fwd = vector(v)
#wp = self.world.get_map().get_waypoint(self.ego.get_location())
#wp_fwd = vector(wp.transform.rotation.get_forward_vector())
wp_fwd = vector(
self.current_waypoint.transform.rotation.get_forward_vector())
angle = angle_diff(fwd, wp_fwd)
speed_kmh = 3.6 * np.sqrt(v.x**2 + v.y**2 + v.z**2)
if self.throttle <= 0:
#if throttle is negative we can not move
speed_reward = -10.0
else:
if speed_kmh < min_speed: # When speed is in [0, min_speed] range
speed_reward = speed_kmh
elif speed_kmh >= min_speed and speed_kmh <= target_speed:
speed_reward = speed_kmh
elif speed_kmh > target_speed:
speed_reward = min_speed # only give min speed reward
collision_reward = 0
if len(self.collision_hist) > 0:
collision_reward = -1
current_location = self.ego.get_location()
start = self.step_last_location
dist = current_location.distance(start) # in meteres
dist_reward = dist
print("Distance reward = {} * Speed rewards {} , throttle {} ".format(
dist_reward, speed_reward, self.throttle))
# update last location
self.step_last_location = current_location
# Interpolated from 1 when centered to 0 when 3 m from center
centering_factor = max(
1.0 - self.distance_from_center / self.max_distance, 0.0)
centering_reward = max(self.distance_from_center / self.max_distance,
0.0)
# Interpolated from 1 when aligned with the road to 0 when +/- 20 degress of road
angle_factor = max(1.0 - abs(angle / np.deg2rad(20)), 0.0)
# reward for out of lane
ego_x, ego_y = get_pos(self.ego)
dis, w = get_lane_dis(self.waypoints, ego_x, ego_y)
r_out = 0
if abs(dis) > self.out_lane_thres:
r_out = -1
# Final reward
reward = dist_reward + centering_factor + 10 * collision_reward
#print("Dist reward {} * centering_factor {} ".format(dist_reward, centering_factor))
return reward, centering_factor, 10 * collision_reward, r_out
def _get_reward(self):
"""Calculate the step reward."""
# reward for speed tracking
v = self.ego.get_velocity()
speed = np.sqrt(v.x**2 + v.y**2)
r_speed = -abs(speed - self.desired_speed)
# reward for collision
r_collision = 0
if len(self.collision_hist) > 0:
r_collision = -1
# reward for steering:
r_steer = -self.ego.get_control().steer**2
# reward for out of lane
ego_x, ego_y = get_pos(self.ego)
dis, w = get_lane_dis(self.waypoints, ego_x, ego_y)
r_out = 0
if abs(dis) > self.out_lane_thres:
r_out = -1
# reward for not being in the centre of the lane
# get the way point to the centre of the road
waypoint = self.world.get_map().get_waypoint(self.ego.get_location(),
project_to_road=True)
ways = np.array([[
waypoint.transform.location.x, waypoint.transform.location.y,
waypoint.transform.rotation.yaw
]])
dis, w = get_preview_lane_dis(ways, ego_x, ego_y, 0)
if (np.isnan(dis)):
r_centre = 2.383e-07
print("Car centre distance is nan , rcenter = {}", r_centre)
else:
r_centre = abs(dis)
#print("Car centre distance : {}".format(r_centre))
# longitudinal speed
lspeed = np.array([v.x, v.y])
lspeed_lon = np.dot(lspeed, w)
# cost for too fast
r_fast = 0
if lspeed_lon > self.desired_speed:
r_fast = -1
# cost for lateral acceleration
r_lat = -abs(self.ego.get_control().steer) * lspeed_lon**2
#r = 200*r_collision + 1*lspeed_lon + 10*r_fast + 1*r_out + r_steer*5 + 0.2*r_lat - 0.1
#r = 300*r_collision + 1*lspeed_lon + 10*r_fast + 200*r_out + r_steer*5 + 0.2*r_lat - 0.1
r = 200 * r_collision + 1 * lspeed_lon + 10 * r_fast + 70 * r_out + r_steer * 5 + 0.2 * r_lat - 120 * r_centre - 0.1
return r, (-120 * r_centre), (200 * r_collision), (70 * r_out)
def _terminal(self):
"""Calculate whether to terminate the current episode."""
# Get ego state
ego_x, ego_y = get_pos(self.ego)
# If collides
if len(self.collision_hist) > 0:
return True, False
# If reach maximum timestep
if self.time_step > self.max_time_episode:
return True, False
# If at destination
if self.dests is not None: # If at destination
for dest in self.dests:
if np.sqrt((ego_x - dest[0])**2 + (ego_y - dest[1])**2) < 4:
return True, False
# Stop if distance from center > max distance
if self.distance_from_center > self.max_distance:
print("End : Distance from center more than max distance : {} ".
format(self.distance_from_center))
return True, True
# If out of lane
dis, _ = get_lane_dis(self.waypoints, ego_x, ego_y)
if abs(dis) > self.out_lane_thres:
print("End : Out of Lane , distance : {} ".format(dis))
return True, True
# Speed is too fast
v = self.ego.get_velocity()
speed_kmh = 3.6 * np.sqrt(v.x**2 + v.y**2 + v.z**2)
if max_speed > 0 and speed_kmh > max_speed:
print("End : Too fast {} ".format(speed_kmh))
return True, False
return False, False
def _clear_all_actors(self, actor_filters):
"""Clear specific actors."""
for actor_filter in actor_filters:
for actor in self.world.get_actors().filter(actor_filter):
if actor.is_alive:
if actor.type_id == 'controller.ai.walker':
actor.stop()
actor.destroy()
print("Destroy : {} ".format(actor.type_id))
else:
print("Not Destroyed : {} ".format(actor.type_id))
class CarlaEnv(gym.Env):
"""An OpenAI gym wrapper for CARLA simulator."""
def __init__(self, params):
# parameters
self.display_size = params['display_size'] # rendering screen size
self.max_past_step = params['max_past_step']
self.number_of_vehicles = params['number_of_vehicles']
self.number_of_walkers = params['number_of_walkers']
self.dt = params['dt']
self.task_mode = params['task_mode']
self.max_time_episode = params['max_time_episode']
self.max_waypt = params['max_waypt']
self.obs_range = params['obs_range']
self.lidar_bin = params['lidar_bin']
self.d_behind = params['d_behind']
self.obs_size = int(self.obs_range/self.lidar_bin)
self.out_lane_thres = params['out_lane_thres']
self.desired_speed = params['desired_speed']
self.max_ego_spawn_times = params['max_ego_spawn_times']
self.display_route = params['display_route']
self.pixor_size = params['pixor_size']
# Destination
if params['task_mode'] == 'roundabout':
self.dests = [[4.46, -61.46, 0], [-49.53, -2.89, 0], [-6.48, 55.47, 0], [35.96, 3.33, 0]]
else:
self.dests = None
# action and observation spaces
self.discrete = params['discrete']
self.discrete_act = [params['discrete_acc'], params['discrete_steer']] # acc, steer
self.n_acc = len(self.discrete_act[0])
self.n_steer = len(self.discrete_act[1])
if self.discrete:
self.action_space = spaces.Discrete(self.n_acc*self.n_steer)
else:
self.action_space = spaces.Box(np.array([params['continuous_accel_range'][0],
params['continuous_steer_range'][0]]), np.array([params['continuous_accel_range'][1],
params['continuous_steer_range'][1]]), dtype=np.float32) # acc, steer
self.observation_space = spaces.Dict({'birdeye': spaces.Box(low=0, high=255, shape=(self.obs_size, self.obs_size, 3), dtype=np.uint8),
'lidar': spaces.Box(low=0, high=255, shape=(self.obs_size, self.obs_size, 3), dtype=np.uint8),
'camera': spaces.Box(low=0, high=255, shape=(self.obs_size, self.obs_size, 3), dtype=np.uint8),
'roadmap': spaces.Box(low=0, high=255, shape=(self.obs_size, self.obs_size, 3), dtype=np.uint8),
'vh_clas': spaces.Box(low=0, high=1, shape=(self.pixor_size, self.pixor_size, 1), dtype=np.float32),
'vh_regr': spaces.Box(low=-5, high=5, shape=(self.pixor_size, self.pixor_size, 6), dtype=np.float32),
'state': spaces.Box(np.array([-2, -1, -5, 0]), np.array([2, 1, 30, 1]), dtype=np.float32),
'pixor_state': spaces.Box(np.array([-1000, -1000, -1, -1, -5]), np.array([1000, 1000, 1, 1, 20]), dtype=np.float32),
'costmap': spaces.Box(low=0, high=255, shape=(self.obs_size, self.obs_size, 1), dtype=np.uint8)}) #costmap should be a 2d array
# Connect to carla server and get world object
print('connecting to Carla server...')
client = carla.Client('localhost', params['port'])
client.set_timeout(10.0)
self.world = client.load_world(params['town'])
print('Carla server connected!')
# Set weather
self.world.set_weather(carla.WeatherParameters.ClearNoon)
# Get spawn points
self.vehicle_spawn_points = list(self.world.get_map().get_spawn_points())
self.walker_spawn_points = []
for i in range(self.number_of_walkers):
spawn_point = carla.Transform()
loc = self.world.get_random_location_from_navigation()
if (loc != None):
spawn_point.location = loc
self.walker_spawn_points.append(spawn_point)
# Create the ego vehicle blueprint
self.ego_bp = self._create_vehicle_bluepprint(params['ego_vehicle_filter'], color='49,8,8')
# Collision sensor
self.collision_hist = [] # The collision history
self.collision_hist_l = 1 # collision history length
self.collision_bp = self.world.get_blueprint_library().find('sensor.other.collision')
# Lidar sensor
self.lidar_data = None
self.lidar_height = 2.1
self.lidar_trans = carla.Transform(carla.Location(x=0.0, z=self.lidar_height))
self.lidar_bp = self.world.get_blueprint_library().find('sensor.lidar.ray_cast')
self.lidar_bp.set_attribute('channels', '32')
self.lidar_bp.set_attribute('range', '5000')
# Camera sensor
self.camera_img = np.zeros((self.obs_size, self.obs_size, 3), dtype=np.uint8)
self.camera_trans = carla.Transform(carla.Location(x=0.8, z=1.7))
self.camera_bp = self.world.get_blueprint_library().find('sensor.camera.rgb')
# Modify the attributes of the blueprint to set image resolution and field of view.
self.camera_bp.set_attribute('image_size_x', str(self.obs_size))
self.camera_bp.set_attribute('image_size_y', str(self.obs_size))
self.camera_bp.set_attribute('fov', '110')
# Set the time in seconds between sensor captures
self.camera_bp.set_attribute('sensor_tick', '0.02')
# Set fixed simulation step for synchronous mode
self.settings = self.world.get_settings()
self.settings.fixed_delta_seconds = self.dt
# Record the time of total steps and resetting steps
self.reset_step = 0
self.total_step = 0
# Initialize the renderer
self._init_renderer()
# Get pixel grid points
x, y = np.meshgrid(np.arange(self.pixor_size), np.arange(self.pixor_size)) # make a canvas with coordinates
x, y = x.flatten(), y.flatten()
self.pixel_grid = np.vstack((x,y)).T
def reset(self):
# Clear sensor objects
self.collision_sensor = None
self.lidar_sensor = None
self.camera_sensor = None
# Delete sensors, vehicles and walkers
self._clear_all_actors(['sensor.other.collision', 'sensor.lidar.ray_cast', 'sensor.camera.rgb', 'vehicle.*', 'controller.ai.walker', 'walker.*'])
# Disable sync mode
self._set_synchronous_mode(False)
# Spawn surrounding vehicles
random.shuffle(self.vehicle_spawn_points)
count = self.number_of_vehicles
if count > 0:
for spawn_point in self.vehicle_spawn_points:
if self._try_spawn_random_vehicle_at(spawn_point, number_of_wheels=[4]):
count -= 1
if count <= 0:
break
while count > 0:
if self._try_spawn_random_vehicle_at(random.choice(self.vehicle_spawn_points), number_of_wheels=[4]):
count -= 1
# Spawn pedestrians
random.shuffle(self.walker_spawn_points)
count = self.number_of_walkers
if count > 0:
for spawn_point in self.walker_spawn_points:
if self._try_spawn_random_walker_at(spawn_point):
count -= 1
if count <= 0:
break
while count > 0:
if self._try_spawn_random_walker_at(random.choice(self.walker_spawn_points)):
count -= 1
# Get actors polygon list
self.vehicle_polygons = []
vehicle_poly_dict = self._get_actor_polygons('vehicle.*')
self.vehicle_polygons.append(vehicle_poly_dict)
self.walker_polygons = []
walker_poly_dict = self._get_actor_polygons('walker.*')
self.walker_polygons.append(walker_poly_dict)
# Spawn the ego vehicle
ego_spawn_times = 0
while True:
if ego_spawn_times > self.max_ego_spawn_times:
self.reset()
if self.task_mode == 'random':
#transform = random.choice(self.vehicle_spawn_points)
transform = self.vehicle_spawn_points[0]
#transform.rotation.yaw = 0
tup = (transform.location.x, transform.location.y, transform.rotation.yaw)
print("Transform: " + str(tup))
if self.task_mode == 'roundabout':
self.start=[52.1+np.random.uniform(-5,5),-4.2, 178.66] # random
# self.start=[52.1,-4.2, 178.66] # static
transform = self._set_carla_transform(self.start)
if self._try_spawn_ego_vehicle_at(transform):
break
else:
ego_spawn_times += 1
time.sleep(0.1)
# Add collision sensor
self.collision_sensor = self.world.spawn_actor(self.collision_bp, carla.Transform(), attach_to=self.ego)
self.collision_sensor.listen(lambda event: get_collision_hist(event))
def get_collision_hist(event):
impulse = event.normal_impulse
intensity = np.sqrt(impulse.x**2 + impulse.y**2 + impulse.z**2)
self.collision_hist.append(intensity)
if len(self.collision_hist)>self.collision_hist_l:
self.collision_hist.pop(0)
self.collision_hist = []
# Add lidar sensor
self.lidar_sensor = self.world.spawn_actor(self.lidar_bp, self.lidar_trans, attach_to=self.ego)
self.lidar_sensor.listen(lambda data: get_lidar_data(data))
def get_lidar_data(data):
self.lidar_data = data
# Add camera sensor
self.camera_sensor = self.world.spawn_actor(self.camera_bp, self.camera_trans, attach_to=self.ego)
self.camera_sensor.listen(lambda data: get_camera_img(data))
def get_camera_img(data):
array = np.frombuffer(data.raw_data, dtype = np.dtype("uint8"))
array = np.reshape(array, (data.height, data.width, 4))
array = array[:, :, :3]
array = array[:, :, ::-1]
self.camera_img = array
# Update timesteps
self.time_step=0
self.reset_step+=1
# Enable sync mode
self.settings.synchronous_mode = True
self.world.apply_settings(self.settings)
self.routeplanner = RoutePlanner(self.ego, self.max_waypt)
self.waypoints, _, self.vehicle_front = self.routeplanner.run_step()
# Set ego information for render
self.birdeye_render.set_hero(self.ego, self.ego.id)
return self._get_obs()
def step(self, action):
# Calculate acceleration and steering
if self.discrete:
acc = self.discrete_act[0][action//self.n_steer]
steer = self.discrete_act[1][action%self.n_steer]
else:
acc = action[0]
steer = action[1]
# Convert acceleration to throttle and brake
if acc > 0:
throttle = np.clip(acc/3,0,1)
brake = 0
else:
throttle = 0
brake = np.clip(-acc/8,0,1)
# Apply control
act = carla.VehicleControl(throttle=float(throttle), steer=float(-steer), brake=float(brake))
self.ego.apply_control(act)
self.world.tick()
# Append actors polygon list
vehicle_poly_dict = self._get_actor_polygons('vehicle.*')
self.vehicle_polygons.append(vehicle_poly_dict)
while len(self.vehicle_polygons) > self.max_past_step:
self.vehicle_polygons.pop(0)
walker_poly_dict = self._get_actor_polygons('walker.*')
self.walker_polygons.append(walker_poly_dict)
while len(self.walker_polygons) > self.max_past_step:
self.walker_polygons.pop(0)
# route planner
self.waypoints, _, self.vehicle_front = self.routeplanner.run_step()
# state information
info = {
'waypoints': self.waypoints,
'vehicle_front': self.vehicle_front
}
# Update timesteps
self.time_step += 1
self.total_step += 1
return (self._get_obs(), self._get_reward(), self._terminal(), copy.deepcopy(info))
def seed(self, seed=None):
self.np_random, seed = seeding.np_random(seed)
return [seed]
def render(self, mode):
pass
def _create_vehicle_bluepprint(self, actor_filter, color=None, number_of_wheels=[4]):
"""Create the blueprint for a specific actor type.
Args:
actor_filter: a string indicating the actor type, e.g, 'vehicle.lincoln*'.
Returns:
bp: the blueprint object of carla.
"""
blueprints = self.world.get_blueprint_library().filter(actor_filter)
blueprint_library = []
for nw in number_of_wheels:
blueprint_library = blueprint_library + [x for x in blueprints if int(x.get_attribute('number_of_wheels')) == nw]
bp = random.choice(blueprint_library)
if bp.has_attribute('color'):
if not color:
color = random.choice(bp.get_attribute('color').recommended_values)
bp.set_attribute('color', color)
return bp
def _init_renderer(self):
"""Initialize the birdeye view renderer.
"""
pygame.init()
self.display = pygame.display.set_mode(
(self.display_size * 4, self.display_size),
pygame.HWSURFACE | pygame.DOUBLEBUF)
pixels_per_meter = self.display_size / self.obs_range
pixels_ahead_vehicle = (self.obs_range/2 - self.d_behind) * pixels_per_meter
birdeye_params = {
'screen_size': [self.display_size, self.display_size],
'pixels_per_meter': pixels_per_meter,
'pixels_ahead_vehicle': pixels_ahead_vehicle
}
self.birdeye_render = BirdeyeRender(self.world, birdeye_params)
def _set_synchronous_mode(self, synchronous = True):
"""Set whether to use the synchronous mode.
"""
self.settings.synchronous_mode = synchronous
self.world.apply_settings(self.settings)
def _try_spawn_random_vehicle_at(self, transform, number_of_wheels=[4]):
"""Try to spawn a surrounding vehicle at specific transform with random bluprint.
Args:
transform: the carla transform object.
Returns:
Bool indicating whether the spawn is successful.
"""
blueprint = self._create_vehicle_bluepprint('vehicle.*', number_of_wheels=number_of_wheels)
blueprint.set_attribute('role_name', 'autopilot')
vehicle = self.world.try_spawn_actor(blueprint, transform)
if vehicle is not None:
vehicle.set_autopilot()
return True
return False
def _try_spawn_random_walker_at(self, transform):
"""Try to spawn a walker at specific transform with random bluprint.
Args:
transform: the carla transform object.
Returns:
Bool indicating whether the spawn is successful.
"""
walker_bp = random.choice(self.world.get_blueprint_library().filter('walker.*'))
# set as not invencible
if walker_bp.has_attribute('is_invincible'):
walker_bp.set_attribute('is_invincible', 'false')
walker_actor = self.world.try_spawn_actor(walker_bp, transform)
if walker_actor is not None:
walker_controller_bp = self.world.get_blueprint_library().find('controller.ai.walker')
walker_controller_actor = self.world.spawn_actor(walker_controller_bp, carla.Transform(), walker_actor)
# start walker
walker_controller_actor.start()
# set walk to random point
walker_controller_actor.go_to_location(self.world.get_random_location_from_navigation())
# random max speed
walker_controller_actor.set_max_speed(1 + random.random()) # max speed between 1 and 2 (default is 1.4 m/s)
return True
return False
def _try_spawn_ego_vehicle_at(self, transform):
"""Try to spawn the ego vehicle at specific transform.
Args:
transform: the carla transform object.
Returns:
Bool indicating whether the spawn is successful.
"""
vehicle = None
# Check if ego position overlaps with surrounding vehicles
overlap = False
for idx, poly in self.vehicle_polygons[0].items():
poly_center = np.mean(poly, axis=0)
ego_center = np.array([transform.location.x, transform.location.y])
dis = np.linalg.norm(poly_center - ego_center)
if dis > 8:
continue
else:
overlap = True
break
if not overlap:
vehicle = self.world.try_spawn_actor(self.ego_bp, transform)
if vehicle is not None:
self.ego=vehicle
return True
return False
def _set_carla_transform(self, pose):
"""Get a carla tranform object given pose.
Args:
pose: [x, y, yaw].
Returns:
transform: the carla transform object
"""
transform = carla.Transform()
transform.location.x = pose[0]
transform.location.y = pose[1]
transform.rotation.yaw = pose[2]
return transform
def _get_actor_polygons(self, filt):
"""Get the bounding box polygon of actors.
Args:
filt: the filter indicating what type of actors we'll look at.
Returns:
actor_poly_dict: a dictionary containing the bounding boxes of specific actors.
"""
actor_poly_dict={}
for actor in self.world.get_actors().filter(filt):
# Get x, y and yaw of the actor
trans=actor.get_transform()
x=trans.location.x
y=trans.location.y
yaw=trans.rotation.yaw/180*np.pi
# Get length and width
bb=actor.bounding_box
l=bb.extent.x
w=bb.extent.y
# Get bounding box polygon in the actor's local coordinate
poly_local=np.array([[l,w],[l,-w],[-l,-w],[-l,w]]).transpose()
# Get rotation matrix to transform to global coordinate
R=np.array([[np.cos(yaw),-np.sin(yaw)],[np.sin(yaw),np.cos(yaw)]])
# Get global bounding box polygon
poly=np.matmul(R,poly_local).transpose()+np.repeat([[x,y]],4,axis=0)
actor_poly_dict[actor.id]=poly
return actor_poly_dict
def _get_ego(self):
""" Get the ego vehicle object
"""
return self.ego
def _display_to_rgb(self, display):
""" Transform image grabbed from pygame display to an rgb image uint8 matrix
"""
rgb = np.fliplr(np.rot90(display, 3)) # flip to regular view
rgb = resize(rgb, (self.obs_size, self.obs_size)) # resize
rgb = rgb * 255
return rgb
def _rgb_to_display_surface(self, rgb):
""" Generate pygame surface given an rgb image uint8 matrix
"""
surface = pygame.Surface((self.display_size, self.display_size)).convert()
display = resize(rgb, (self.display_size, self.display_size))
display = np.flip(display, axis=1)
display = np.rot90(display, 1)
pygame.surfarray.blit_array(surface, display)
return surface
def _get_obs(self):
"""Get the observations."""
## Birdeye rendering
self.birdeye_render.vehicle_polygons = self.vehicle_polygons
self.birdeye_render.walker_polygons = self.walker_polygons
self.birdeye_render.waypoints = self.waypoints
# birdeye view with roadmap and actors
birdeye_render_types = ['roadmap', 'actors']
if self.display_route:
birdeye_render_types.append('waypoints')
self.birdeye_render.render(self.display, birdeye_render_types)
birdeye = pygame.surfarray.array3d(self.display)
birdeye = birdeye[0:self.display_size, :, :]
birdeye = self._display_to_rgb(birdeye)
# Roadmap
roadmap_render_types = ['roadmap']
if self.display_route:
roadmap_render_types.append('waypoints')
self.birdeye_render.render(self.display, roadmap_render_types)
roadmap = pygame.surfarray.array3d(self.display)
roadmap = roadmap[0:self.display_size, :, :]
roadmap = self._display_to_rgb(roadmap)
# Add ego vehicle
for i in range(self.obs_size):
for j in range(self.obs_size):
if abs(birdeye[i, j, 0] - 255)<20 and abs(birdeye[i, j, 1] - 0)<20 and abs(birdeye[i, j, 0] - 255)<20:
roadmap[i, j, :] = birdeye[i, j, :]
# Display birdeye image
birdeye_surface = self._rgb_to_display_surface(birdeye)
self.display.blit(birdeye_surface, (0, 0))
## Lidar image generation
point_cloud = []
# Get point cloud data
for location in self.lidar_data:
point_cloud.append([location.x, location.y, -location.z])
point_cloud = np.array(point_cloud)
# Separate the 3D space to bins for point cloud, x and y is set according to self.lidar_bin,
# and z is set to be two bins.
y_bins = np.arange(-(self.obs_range - self.d_behind), self.d_behind+self.lidar_bin, self.lidar_bin)
x_bins = np.arange(-self.obs_range/2, self.obs_range/2+self.lidar_bin, self.lidar_bin)
z_bins = [-self.lidar_height-1, -self.lidar_height+0.25, 1]
# Get lidar image according to the bins
lidar, _ = np.histogramdd(point_cloud, bins=(x_bins, y_bins, z_bins))
lidar[:,:,0] = np.array(lidar[:,:,0]>0, dtype=np.uint8)
lidar[:,:,1] = np.array(lidar[:,:,1]>0, dtype=np.uint8)
# Add the waypoints to lidar image
if self.display_route:
wayptimg = (birdeye[:,:,0] <= 10) * (birdeye[:,:,1] <= 10) * (birdeye[:,:,2] >= 240)
else:
wayptimg = birdeye[:,:,0] < 0 # Equal to a zero matrix
wayptimg = np.expand_dims(wayptimg, axis=2)
wayptimg = np.fliplr(np.rot90(wayptimg, 3))
# Get the final lidar image
lidar = np.concatenate((lidar, wayptimg), axis=2)
lidar = np.flip(lidar, axis=1)
lidar = np.rot90(lidar, 1)
lidar = lidar * 255
# Display lidar image
lidar_surface = self._rgb_to_display_surface(lidar)
self.display.blit(lidar_surface, (self.display_size, 0))
## Display camera image
camera = resize(self.camera_img, (self.obs_size, self.obs_size)) * 255
camera_surface = self._rgb_to_display_surface(camera)
self.display.blit(camera_surface, (self.display_size * 2, 0))
## Display roadmap image
# roadmap_surface = self._rgb_to_display_surface(roadmap)
# self.display.blit(roadmap_surface, (self.display_size * 3, 0))
## Vehicle classification and regression maps (requires further normalization)
vh_clas = np.zeros((self.pixor_size, self.pixor_size))
vh_regr = np.zeros((self.pixor_size, self.pixor_size, 6))
# Generate the PIXOR image. Note in CARLA it is using left-hand coordinate
def get_actor_info(actor):
trans=actor.get_transform()
x=trans.location.x
y=trans.location.y
yaw=trans.rotation.yaw/180*np.pi
# Get length and width
bb=actor.bounding_box
l=bb.extent.x # half the length
w=bb.extent.y # half the width
return (x, y, yaw, l, w)
def global_to_local_pose(pose, ego_pose):
x, y, yaw = pose
ego_x, ego_y, ego_yaw = ego_pose
R = np.array([[np.cos(ego_yaw), np.sin(ego_yaw)],
[-np.sin(ego_yaw), np.cos(ego_yaw)]])
vec_local = R.dot(np.array([x - ego_x, y - ego_y]))
yaw_local = yaw - ego_yaw
return (vec_local[0], vec_local[1], yaw_local)
def local_to_pixel_info(info):
"""Here the ego local coordinate is left-handed, the pixel
coordinate is also left-handed, with its origin at the left bottom.
"""
x, y, yaw, l, w = info
x_pixel = (x + self.d_behind)/self.obs_range*self.pixor_size
y_pixel = y/self.obs_range*self.pixor_size + self.pixor_size/2
yaw_pixel = yaw
l_pixel = l/self.obs_range*self.pixor_size
w_pixel = w/self.obs_range*self.pixor_size
return (x_pixel, y_pixel, yaw_pixel, l_pixel, w_pixel)
def get_pixels_from_info(info):
"""Get pixels from information in pixel coordinate,
which the origin is at the left bottom.
"""
poly = get_poly_from_info(info)
p = Path(poly) # make a polygon
grid = p.contains_points(self.pixel_grid)
isinPoly = np.where(grid==True)
pixels = np.take(self.pixel_grid, isinPoly, axis=0)[0]
return pixels
def get_poly_from_info(info):
x, y, yaw, l, w = info
poly_local=np.array([[l,w],[l,-w],[-l,-w],[-l,w]]).transpose()
# Get rotation matrix to transform to the coordinate
R=np.array([[np.cos(yaw),-np.sin(yaw)],[np.sin(yaw),np.cos(yaw)]])
# Get bounding box polygon
poly=np.matmul(R,poly_local).transpose()+np.repeat([[x,y]],4,axis=0)
return poly
# Get the 6-dim geom parametrization in PIXOR, here we use pixel coordinate
# for convenience
ego_trans = self.ego.get_transform()
ego_x = ego_trans.location.x
ego_y = ego_trans.location.y
ego_yaw = ego_trans.rotation.yaw/180*np.pi
for actor in self.world.get_actors().filter('vehicle.*'):
x, y, yaw, l, w = get_actor_info(actor)
x_local, y_local, yaw_local = global_to_local_pose(
(x, y, yaw), (ego_x, ego_y, ego_yaw))
if actor.id != self.ego.id and np.sqrt(x_local**2 + y_local**2) < self.obs_range**1.5:
x_pixel, y_pixel, yaw_pixel, l_pixel, w_pixel = local_to_pixel_info(
(x_local, y_local, yaw_local, l, w))
cos_t = np.cos(yaw_pixel)
sin_t = np.sin(yaw_pixel)
logw = np.log(w_pixel)
logl = np.log(l_pixel)
pixels = get_pixels_from_info((x_pixel, y_pixel, yaw_pixel, l_pixel, w_pixel))
for pixel in pixels:
vh_clas[pixel[0], pixel[1]] = 1
dx = x_pixel - pixel[0]
dy = y_pixel - pixel[1]
# dx = (x_pixel - pixel[0])/self.obs_size*self.obs_range
# dy = (y_pixel - pixel[1])/self.obs_size*self.obs_range
vh_regr[pixel[0], pixel[1], :] = np.array(
[cos_t, sin_t, dx, dy, logw, logl])
# Flip the image matrix so that the origin is at the left-bottom
vh_clas = np.flip(vh_clas, axis=0)
vh_regr = np.flip(vh_regr, axis=0)
## Display pixor images
# vh_clas_display = np.stack([vh_clas, vh_clas, vh_clas], axis=2) * 255
# vh_clas_surface = self._rgb_to_display_surface(vh_clas_display)
# self.display.blit(vh_clas_surface, (self.display_size * 4, 0))
# vh_regr1 = vh_regr[:, :, 0:3]
# vh_regr2 = vh_regr[:, :, 3:6]
# vh_regr1_surface = self._rgb_to_display_surface(vh_regr1)
# self.display.blit(vh_regr1_surface, (self.display_size * 5, 0))
# vh_regr2_surface = self._rgb_to_display_surface(vh_regr2)
# self.display.blit(vh_regr2_surface, (self.display_size * 6, 0))
# Display on pygame
pygame.display.flip()
# State observation
lateral_dis, w = self._get_preview_lane_dis(self.waypoints, ego_x, ego_y)
delta_yaw = np.arcsin(np.cross(w,
np.array(np.array([np.cos(ego_yaw), np.sin(ego_yaw)]))))
v = self.ego.get_velocity()
speed = np.sqrt(v.x**2 + v.y**2)
state = np.array([lateral_dis, - delta_yaw, speed, self.vehicle_front])
# Pixor state, [x, y, cos(yaw), sin(yaw), speed]
pixor_state = [ego_x, ego_y, np.cos(ego_yaw), np.sin(ego_yaw), speed]
"""
Explanation of how my costmap implementation works:
First we get a list of all of the waypoints from the current position. We iterate through this list in pairs so that there is a current
waypoint and a previous waypoint. These along with parameter cost are passed into _get_costmap which returns a costmap only relevant to the
lane defined by the line between the two points. This costmap is summed with the global costmap. This profess is repeated for the left and right
lanes of the current waypoint if they exist and are in the same direction.
"""
def _get_perp_dis(x1, y1, x2, y2, x3, y3):
""" Computes the perpindicular distance between a point (x3,y3) and a line segment defined by points
(x1, y1) and (x2, y2). If the point doesn't have a perpincicular line within the line segment, the
distance is inf
"""
x = np.array([x3, y3])
p = np.array([x1, y1])
q = np.array([x2, y2])
lamb = np.dot((x - p), (q - p)) / np.dot((q - p), (q - p)) #lamb checks if point is within line segment
if lamb <= 1 and lamb >= 0:
s = p + (lamb * (q - p))
return np.linalg.norm(x - s)
return float('inf')
ego_y = ego_y - 2 #shift the cost map down because originally it was too high
def _get_costmap(pWaypoint, cWaypoint, cost):
""" Generates a costmap for a current waypoint cWaypoint and its preceding waypoint pWaypoint using cost.
I refer a lot to global vs local frame. Global means the xy coordinate in the Carla coordinates
Local is the coordinate in the costmap matrix.
Also the letters x and y are swapped when referring to the local frame but it works
"""
single_costmap = np.zeros((self.obs_size, self.obs_size))
#Definitions for the waypoints' x and y coordinates in the global frame
laneWidth = pWaypoint.lane_width
cX = cWaypoint.transform.location.x
cY = cWaypoint.transform.location.y
pX = pWaypoint.transform.location.x
pY = pWaypoint.transform.location.y
#If we draw a square around the center of the ego vehicle (length is determined by range), this is the bottom left corner in global coords
corner_x = ego_x - (self.obs_range / 2)
corner_y = ego_y - (self.obs_range / 2)
#Here we create two matrices with the same dimensions as the costmap. One represents the x coordinate and one represents the y coordinate in the local frame.
y_array, x_array = np.meshgrid(np.arange(0, self.obs_size), np.arange(0, self.obs_size))
#y_array is [[0 1 2 ... 255] [0 1 2 ... 255] ...]
#x_array is [[0 0 0 .... 0] [1 1 1 .... 1]... [255 255 ... 255]]
rotated_x_array = (2 * ego_x) - ((x_array * self.lidar_bin) + corner_x)
rotated_y_array = (y_array * self.lidar_bin) + corner_y
c = np.cos(ego_yaw)
s = np.sin(ego_yaw)
global_x_array = (c * (rotated_x_array - ego_x)) - (s * (rotated_y_array - ego_y)) + ego_x #for each point in our matrix, we have their global coordinates
global_y_array = (s * (rotated_x_array - ego_x)) + (c * (rotated_y_array - ego_y)) + ego_y
p = np.array([pX, pY])
q = np.array([cX, cY])
q_dif = q - p
lamb_array= (((global_x_array - pX) * (cX - pX)) + ((global_y_array - pY) * (cY - pY ))) / np.dot((q_dif), (q_dif))
sX = pX + (lamb_array * (cX - pX))
sY = pY + (lamb_array * (cY - pY))
distanceMap = np.sqrt(np.square(global_x_array - sX) + np.square(global_y_array - sY))
penal = (laneWidth / 2) * (-cost) / abs(cost)
distanceMap = np.where((lamb_array <=1) & (lamb_array >= 0) & (distanceMap <= laneWidth / 2), distanceMap, penal) #will have perpDistance in the spot if its within the lane
single_costmap = distanceMap * (abs(cost / (laneWidth / 2))) + cost
return single_costmap
#Generate list of waypoints. Previously, we relied on self.routeplanner._actualWaypoints
#there is way to save space for this. Instead of recalculating all waypoints, we can reuse most of them.
#we can do this for each neighboring lane
ego_waypoints = [self.world.get_map().get_waypoint(self.ego.get_location())] #make current ego position into a waypoint
current_dir= ego_waypoints[0].lane_id #positive or negative integer depending on which direction the lane is going in
left_lane = ego_waypoints[0].get_left_lane()
right_lane = ego_waypoints[0].get_right_lane()
waypoints_threshold = 5 #this is how many waypoints to keep
sampling_radius = 5 #how many meters away to sample for the next waypoint
if left_lane and left_lane.lane_type == carla.LaneType.Driving and left_lane.lane_id * current_dir >= 0: #check if neighboring lane exists, is drivable, and in same direction
ego_waypoints.append(left_lane)
if right_lane and right_lane.lane_type == carla.LaneType.Driving and right_lane.lane_id * current_dir >= 0:
ego_waypoints.append(right_lane)
dir_list = [] #list of lists of waypoints. each list inside represents a possible path
#we loop through ego_waypoints for the center, left, and right lanes
for current_waypoint in ego_waypoints:
#current_waypoint = self.world.get_map().get_waypoint(self.ego.get_location())
current_waypoints = [current_waypoint]
lane_end = False #did we reach the end of the lane
ctr = 0 #counter to make sure we don't pass the waypoints threshold
next_waypoints = [] #these represents all the other directions we have to go in after the current waypoint. TODO contains a tuple of starting waypoint and list of lists
while (not lane_end) and ctr < waypoints_threshold:
sample_waypoint = current_waypoints[-1]
ctr += 1
next_waypoint = sample_waypoint.next(sampling_radius)
# if (sample_waypoint.is_junction):
# junc = sample_waypoint.get_junction().get_waypoints(carla.LaneType.Driving)
# print("Sample", sample_waypoint)
# print("Junc", junc)
# for tup in junc:
# if tup[0] == sample_waypoint:
# next_waypoints.append([tup[0], tup[1]])
#TODO so that we don't just stop when there's multiple directions but instead stop when the lane ends
if (len(next_waypoint) != 1): #if there is more than 1 waypoint to go to we stop because we have to explore those directions
lane_end = True
#print("Testing if the last means in junction", sample_waypoint.is_junction)
else:
current_waypoints.append(next_waypoint[0]) #if there's only one direction just append it to the current array
last_waypoint = current_waypoints[-1] #this will be the starting waypoint for all the diverging lanes
dir_list.append(current_waypoints)
if lane_end:
#this means the lane changes direction so we have to compute the new lanes for the junction
#print("last waypoint coming up")
next_waypoints = last_waypoint.next(sampling_radius)
#print(next_waypoints)
for new_direction in next_waypoints: #we append some points
new_waypoints = [last_waypoint, new_direction]
ctr = 0
lane_end = False
while not lane_end and ctr < waypoints_threshold + 5:
ctr += 1
sample_waypoint = new_waypoints[-1]
next_waypoint = sample_waypoint.next(sampling_radius)
if (len(next_waypoint) > 1):
lane_end = True
else:
new_waypoints.append(next_waypoint[0])
#print('ctr', ctr)
#print("new_waypoints", new_waypoints)
dir_list.append(new_waypoints)
#listofWaypoints = self.routeplanner._actualWaypoints
cost = -10
costmap = np.zeros((self.obs_size, self.obs_size))
for listofWaypoints in dir_list:
if len(listofWaypoints) < 1:
print("Not enough waypoints to form costmap")
else:
pWaypoint = listofWaypoints[0]
for cWaypoint in listofWaypoints[1:]:
currentDirection = cWaypoint.lane_id #positive or negative integer depending on which direction the lane is going in
costmap = costmap + _get_costmap(pWaypoint, cWaypoint, cost)
#The current implementation of left and right lanes is contingent on whether the current lane has a left/right lane AND the previous lane has a left/right lane
pleftWaypoint = pWaypoint.get_left_lane()
prightWaypoint = pWaypoint.get_right_lane()
cleftWaypoint = cWaypoint.get_left_lane()
crightWaypoint = cWaypoint.get_right_lane()
pWaypoint = cWaypoint
# if pleftWaypoint and (pleftWaypoint.lane_id * currentDirection >= 0): #check if left waypoint exists for the previous waypoint and it goes in the same direction
# if cleftWaypoint and (cleftWaypoint.lane_id * currentDirection >= 0): #check if the left waypoint exists for the current waypoint and it goes in the same direction
# costmap = costmap + _get_costmap(pleftWaypoint, cleftWaypoint, cost)
# if prightWaypoint and (prightWaypoint.lane_id * currentDirection >= 0):
# if crightWaypoint and (crightWaypoint.lane_id * currentDirection >= 0):
# costmap = costmap + _get_costmap(prightWaypoint, crightWaypoint, cost)
#Here we convert the cost map which ranges from -cost to 0 (low cost to high cost) to a displayable costmap that has values from 0 to 255
costmap = np.clip(costmap, cost, 0)
costmap = (costmap - cost) * 255 / abs(cost)
costmap_surface = self._rgb_to_display_surface(np.moveaxis(np.array([costmap, costmap, costmap]), 0, -1))
self.display.blit(costmap_surface, (self.display_size * 3, 0))
obs = {}
obs.update({
'birdeye':birdeye.astype(np.uint8),
'lidar':lidar.astype(np.uint8),
'camera':camera.astype(np.uint8),
'roadmap':roadmap.astype(np.uint8),
'vh_clas':np.expand_dims(vh_clas, -1).astype(np.float32),
'vh_regr':vh_regr.astype(np.float32),
'state': state,
'pixor_state': pixor_state,
'costmap' : costmap
})
return obs
def _get_reward(self):
"""Calculate the step reward."""
# reward for speed tracking
v = self.ego.get_velocity()
speed = np.sqrt(v.x**2 + v.y**2)
r_speed = -abs(speed - self.desired_speed)
# reward for collision
r_collision = 0
if len(self.collision_hist) > 0:
r_collision = -1
# reward for steering:
r_steer = -self.ego.get_control().steer**2
# reward for out of lane
ego_x, ego_y = self._get_ego_pos()
dis, w = self._get_lane_dis(self.waypoints, ego_x, ego_y)
r_out = 0
if abs(dis) > self.out_lane_thres:
r_out = -1
# longitudinal speed
lspeed = np.array([v.x, v.y])
lspeed_lon = np.dot(lspeed, w)
# cost for too fast
r_fast = 0
if lspeed_lon > self.desired_speed:
r_fast = -1
# cost for lateral acceleration
r_lat = - abs(self.ego.get_control().steer) * lspeed_lon**2
r = 200*r_collision + 1*lspeed_lon + 10*r_fast + 1*r_out + r_steer*5 + 0.2*r_lat - 0.1
return r
def _get_ego_pos(self):
"""Get the ego vehicle pose (x, y)."""
ego_trans = self.ego.get_transform()
ego_x = ego_trans.location.x
ego_y = ego_trans.location.y
return ego_x, ego_y
def _get_lane_dis(self, waypoints, x, y):
"""Calculate distance from (x, y) to waypoints."""
dis_min = 1000
for pt in waypoints:
d = np.sqrt((x-pt[0])**2 + (y-pt[1])**2)
if d < dis_min:
dis_min = d
waypt=pt
vec = np.array([x - waypt[0],y - waypt[1]])
lv = np.linalg.norm(np.array(vec))
w = np.array([np.cos(waypt[2]/180*np.pi), np.sin(waypt[2]/180*np.pi)])
cross = np.cross(w, vec/lv)
dis = - lv * cross
return dis, w
def _get_preview_lane_dis(self, waypoints, x, y, idx=2):
"""Calculate distance from (x, y) to waypoints."""
dis_min = 1000
waypt = waypoints[idx]
vec = np.array([x - waypt[0],y - waypt[1]])
lv = np.linalg.norm(np.array(vec))
w = np.array([np.cos(waypt[2]/180*np.pi), np.sin(waypt[2]/180*np.pi)])
cross = np.cross(w, vec/lv)
dis = - lv * cross
return dis, w
def _terminal(self):
"""Calculate whether to terminate the current episode."""
# Get ego state
ego_x, ego_y = self._get_ego_pos()
# If collides
if len(self.collision_hist)>0:
return True
# If reach maximum timestep
if self.time_step>self.max_time_episode:
return True
# If at destination
if self.dests is not None: # If at destination
for dest in self.dests:
if np.sqrt((ego_x-dest[0])**2+(ego_y-dest[1])**2)<4:
return True
# If out of lane
dis, _ = self._get_lane_dis(self.waypoints, ego_x, ego_y)
if abs(dis) > self.out_lane_thres:
return True
return False
def _clear_all_actors(self, actor_filters):
"""Clear specific actors."""
for actor_filter in actor_filters:
for actor in self.world.get_actors().filter(actor_filter):
if actor.is_alive:
if actor.type_id == 'controller.ai.walker':
actor.stop()
actor.destroy()
class CarlaEnv(gym.Env):
"""An OpenAI gym wrapper for CARLA simulator."""
def __init__(self, params):
# parameters
self.display_size = params['display_size'] # rendering screen size
self.max_past_step = params['max_past_step']
self.number_of_vehicles = params['number_of_vehicles']
self.number_of_walkers = params['number_of_walkers']
self.dt = params['dt']
self.task_mode = params['task_mode']
self.max_time_episode = params['max_time_episode']
self.max_waypt = params['max_waypt']
self.obs_range = params['obs_range']
self.lidar_bin = params['lidar_bin']
self.d_behind = params['d_behind']
self.obs_size = int(self.obs_range / self.lidar_bin)
self.out_lane_thres = params['out_lane_thres']
self.desired_speed = params['desired_speed']
# Destination
if params['task_mode'] == 'roundabout':
self.dests = [[4.46, -61.46, 0], [-49.53, -2.89, 0],
[-6.48, 55.47, 0], [35.96, 3.33, 0]]
else:
self.dests = None
# action and observation spaces
self.discrete = params['discrete']
self.discrete_act = [params['discrete_acc'],
params['discrete_steer']] # acc, steer
self.n_acc = len(self.discrete_act[0])
self.n_steer = len(self.discrete_act[1])
if self.discrete:
self.action_space = spaces.Discrete(self.n_acc * self.n_steer)
else:
self.action_space = spaces.Box(
np.array([
params['continuous_accel_range'][0],
params['continuous_steer_range'][0]
]),
np.array([
params['continuous_accel_range'][1],
params['continuous_steer_range'][1]
]),
dtype=np.float32) # acc, steer
self.observation_space = spaces.Dict({
'birdeye':
spaces.Box(low=0,
high=255,
shape=(self.obs_size, self.obs_size, 3),
dtype=np.uint8),
'lidar':
spaces.Box(low=0,
high=255,
shape=(self.obs_size, self.obs_size, 3),
dtype=np.uint8),
'camera':
spaces.Box(low=0,
high=255,
shape=(self.obs_size, self.obs_size, 3),
dtype=np.uint8)
})
# Connect to carla server and get world object
print('connecting to Carla server...')
client = carla.Client('localhost', params['port'])
client.set_timeout(10.0)
self.world = client.load_world(params['town'])
print('Carla server connected!')
# Set weather
self.world.set_weather(carla.WeatherParameters.ClearNoon)
# Get spawn points
self.vehicle_spawn_points = list(
self.world.get_map().get_spawn_points())
self.walker_spawn_points = []
for i in range(self.number_of_walkers):
spawn_point = carla.Transform()
loc = self.world.get_random_location_from_navigation()
if (loc != None):
spawn_point.location = loc
self.walker_spawn_points.append(spawn_point)
# Create the ego vehicle blueprint
self.ego_bp = self._create_vehicle_bluepprint(
params['ego_vehicle_filter'], color='49,8,8')
# Collision sensor
self.collision_hist = [] # The collision history
self.collision_hist_l = 1 # collision history length
self.collision_bp = self.world.get_blueprint_library().find(
'sensor.other.collision')
# Lidar sensor
self.lidar_data = None
self.lidar_height = 2.1
self.lidar_trans = carla.Transform(
carla.Location(x=0.0, z=self.lidar_height))
self.lidar_bp = self.world.get_blueprint_library().find(
'sensor.lidar.ray_cast')
self.lidar_bp.set_attribute('channels', '32')
self.lidar_bp.set_attribute('range', '5000')
# Camera sensor
self.camera_img = np.zeros((self.obs_size, self.obs_size, 3),
dtype=np.uint8)
self.camera_trans = carla.Transform(carla.Location(x=0.8, z=1.7))
self.camera_bp = self.world.get_blueprint_library().find(
'sensor.camera.rgb')
# Modify the attributes of the blueprint to set image resolution and field of view.
self.camera_bp.set_attribute('image_size_x', str(self.obs_size))
self.camera_bp.set_attribute('image_size_y', str(self.obs_size))
self.camera_bp.set_attribute('fov', '110')
# Set the time in seconds between sensor captures
self.camera_bp.set_attribute('sensor_tick', '0.02')
# Set fixed simulation step for synchronous mode
self.settings = self.world.get_settings()
self.settings.fixed_delta_seconds = self.dt
# Record the time of total steps and resetting steps
self.reset_step = 0
self.total_step = 0
# Initialize the renderer
self._init_renderer()
def reset(self):
# Clear sensor objects
self.collision_sensor = None
self.lidar_sensor = None
self.camera_sensor = None
# Delete sensors, vehicles and walkers
self._clear_all_actors([
'sensor.other.collision', 'sensor.lidar.ray_cast',
'sensor.camera.rgb', 'vehicle.*', 'controller.ai.walker',
'walker.*'
])
# Disable sync mode
self._set_synchronous_mode(False)
# Spawn surrounding vehicles
random.shuffle(self.vehicle_spawn_points)
count = self.number_of_vehicles
if count > 0:
for spawn_point in self.vehicle_spawn_points:
if self._try_spawn_random_vehicle_at(spawn_point,
number_of_wheels=[4]):
count -= 1
if count <= 0:
break
while count > 0:
if self._try_spawn_random_vehicle_at(random.choice(
self.vehicle_spawn_points),
number_of_wheels=[4]):
count -= 1
# Spawn the ego vehicle
while True:
if self.task_mode == 'random':
transform = random.choice(self.vehicle_spawn_points)
if self.task_mode == 'roundabout':
self.start = [52.1 + np.random.uniform(-5, 5), -4.2,
178.66] # random
transform = self._set_carla_transform(self.start)
if self._try_spawn_ego_vehicle_at(transform):
break
# Spawn pedestrians
random.shuffle(self.walker_spawn_points)
count = self.number_of_walkers
if count > 0:
for spawn_point in self.walker_spawn_points:
if self._try_spawn_random_walker_at(spawn_point):
count -= 1
if count <= 0:
break
while count > 0:
if self._try_spawn_random_walker_at(
random.choice(self.walker_spawn_points)):
count -= 1
# Get actors polygon list
self.vehicle_polygons = []
vehicle_poly_dict = self._get_actor_polygons('vehicle.*')
self.vehicle_polygons.append(vehicle_poly_dict)
self.walker_polygons = []
walker_poly_dict = self._get_actor_polygons('walker.*')
self.walker_polygons.append(walker_poly_dict)
# Add collision sensor
self.collision_sensor = self.world.spawn_actor(self.collision_bp,
carla.Transform(),
attach_to=self.ego)
self.collision_sensor.listen(lambda event: get_collision_hist(event))
def get_collision_hist(event):
impulse = event.normal_impulse
intensity = np.sqrt(impulse.x**2 + impulse.y**2 + impulse.z**2)
self.collision_hist.append(intensity)
if len(self.collision_hist) > self.collision_hist_l:
self.collision_hist.pop(0)
self.collision_hist = []
# Add lidar sensor
self.lidar_sensor = self.world.spawn_actor(self.lidar_bp,
self.lidar_trans,
attach_to=self.ego)
self.lidar_sensor.listen(lambda data: get_lidar_data(data))
def get_lidar_data(data):
self.lidar_data = data
# Add camera sensor
self.camera_sensor = self.world.spawn_actor(self.camera_bp,
self.camera_trans,
attach_to=self.ego)
self.camera_sensor.listen(lambda data: get_camera_img(data))
def get_camera_img(data):
array = np.frombuffer(data.raw_data, dtype=np.dtype("uint8"))
array = np.reshape(array, (data.height, data.width, 4))
array = array[:, :, :3]
array = array[:, :, ::-1]
self.camera_img = array
# Update timesteps
self.time_step = 0
self.reset_step += 1
# Enable sync mode
self.settings.synchronous_mode = True
self.world.apply_settings(self.settings)
self.routeplanner = RoutePlanner(self.ego, self.max_waypt)
self.waypoints, _, self.vehicle_front = self.routeplanner.run_step()
# Set ego information for render
self.birdeye_render.set_hero(self.ego, self.ego.id)
return self._get_obs()
def step(self, action):
# Calculate acceleration and steering
if self.discrete:
acc = self.discrete_act[0][action // self.n_steer]
steer = self.discrete_act[1][action % self.n_steer]
else:
acc = action[0]
steer = action[1]
# Convert acceleration to throttle and brake
if acc > 0:
throttle = np.clip(acc / 3, 0, 1)
brake = 0
else:
throttle = 0
brake = np.clip(-acc / 8, 0, 1)
# Apply control
act = carla.VehicleControl(throttle=float(throttle),
steer=float(steer),
brake=float(brake))
self.ego.apply_control(act)
self.world.tick()
# Append actors polygon list
vehicle_poly_dict = self._get_actor_polygons('vehicle.*')
self.vehicle_polygons.append(vehicle_poly_dict)
while len(self.vehicle_polygons) > self.max_past_step:
self.vehicle_polygons.pop(0)
walker_poly_dict = self._get_actor_polygons('walker.*')
self.walker_polygons.append(walker_poly_dict)
while len(self.walker_polygons) > self.max_past_step:
self.walker_polygons.pop(0)
# route planner
self.waypoints, _, self.vehicle_front = self.routeplanner.run_step()
# state information
info = {
'waypoints': self.waypoints,
'vehicle_front': self.vehicle_front
}
# Update timesteps
self.time_step += 1
self.total_step += 1
return (self._get_obs(), self._get_reward(), self._terminal(),
copy.deepcopy(info))
def seed(self, seed=None):
self.np_random, seed = seeding.np_random(seed)
return [seed]
def render(self, mode):
pass
def _create_vehicle_bluepprint(self,
actor_filter,
color=None,
number_of_wheels=[4]):
"""Create the blueprint for a specific actor type.
Args:
actor_filter: a string indicating the actor type, e.g, 'vehicle.lincoln*'.
Returns:
bp: the blueprint object of carla.
"""
blueprints = self.world.get_blueprint_library().filter(actor_filter)
blueprint_library = []
for nw in number_of_wheels:
blueprint_library = blueprint_library + [
x for x in blueprints
if int(x.get_attribute('number_of_wheels')) == nw
]
bp = random.choice(blueprint_library)
if bp.has_attribute('color'):
if not color:
color = random.choice(
bp.get_attribute('color').recommended_values)
bp.set_attribute('color', color)
return bp
def _init_renderer(self):
"""Initialize the birdeye view renderer.
"""
pygame.init()
self.display = pygame.display.set_mode(
(self.display_size * 3, self.display_size),
pygame.HWSURFACE | pygame.DOUBLEBUF)
pixels_per_meter = self.display_size / self.obs_range
pixels_ahead_vehicle = (self.obs_range / 2 -
self.d_behind) * pixels_per_meter
birdeye_params = {
'screen_size': [self.display_size, self.display_size],
'pixels_per_meter': pixels_per_meter,
'pixels_ahead_vehicle': pixels_ahead_vehicle
}
self.birdeye_render = BirdeyeRender(self.world, birdeye_params)
def _set_synchronous_mode(self, synchronous=True):
"""Set whether to use the synchronous mode.
"""
self.settings.synchronous_mode = synchronous
self.world.apply_settings(self.settings)
def _try_spawn_random_vehicle_at(self, transform, number_of_wheels=[4]):
"""Try to spawn a surrounding vehicle at specific transform with random bluprint.
Args:
transform: the carla transform object.
Returns:
Bool indicating whether the spawn is successful.
"""
blueprint = self._create_vehicle_bluepprint(
'vehicle.*', number_of_wheels=number_of_wheels)
blueprint.set_attribute('role_name', 'autopilot')
vehicle = self.world.try_spawn_actor(blueprint, transform)
if vehicle is not None:
vehicle.set_autopilot()
return True
return False
def _try_spawn_random_walker_at(self, transform):
"""Try to spawn a walker at specific transform with random bluprint.
Args:
transform: the carla transform object.
Returns:
Bool indicating whether the spawn is successful.
"""
walker_bp = random.choice(
self.world.get_blueprint_library().filter('walker.*'))
# set as not invencible
if walker_bp.has_attribute('is_invincible'):
walker_bp.set_attribute('is_invincible', 'false')
walker_actor = self.world.try_spawn_actor(walker_bp, transform)
if walker_actor is not None:
walker_controller_bp = self.world.get_blueprint_library().find(
'controller.ai.walker')
walker_controller_actor = self.world.spawn_actor(
walker_controller_bp, carla.Transform(), walker_actor)
# start walker
walker_controller_actor.start()
# set walk to random point
walker_controller_actor.go_to_location(
self.world.get_random_location_from_navigation())
# random max speed
walker_controller_actor.set_max_speed(
1 + random.random()
) # max speed between 1 and 2 (default is 1.4 m/s)
return True
return False
def _try_spawn_ego_vehicle_at(self, transform):
"""Try to spawn the ego vehicle at specific transform.
Args:
transform: the carla transform object.
Returns:
Bool indicating whether the spawn is successful.
"""
vehicle = self.world.try_spawn_actor(self.ego_bp, transform)
if vehicle is not None:
self.ego = vehicle
return True
return False
def _set_carla_transform(self, pose):
"""Get a carla tranform object given pose.
Args:
pose: [x, y, yaw].
Returns:
transform: the carla transform object
"""
transform = carla.Transform()
transform.location.x = pose[0]
transform.location.y = pose[1]
transform.rotation.yaw = pose[2]
return transform
def _get_actor_polygons(self, filt):
"""Get the bounding box polygon of actors.
Args:
filt: the filter indicating what type of actors we'll look at.
Returns:
actor_poly_dict: a dictionary containing the bounding boxes of specific actors.
"""
actor_poly_dict = {}
for actor in self.world.get_actors().filter(filt):
# Get x, y and yaw of the actor
trans = actor.get_transform()
x = trans.location.x
y = trans.location.y
yaw = trans.rotation.yaw / 180 * np.pi
# Get length and width
bb = actor.bounding_box
l = bb.extent.x
w = bb.extent.y
# Get bounding box polygon in the actor's local coordinate
poly_local = np.array([[l, w], [l, -w], [-l, -w],
[-l, w]]).transpose()
# Get rotation matrix to transform to global coordinate
R = np.array([[np.cos(yaw), -np.sin(yaw)],
[np.sin(yaw), np.cos(yaw)]])
# Get global bounding box polygon
poly = np.matmul(R, poly_local).transpose() + np.repeat(
[[x, y]], 4, axis=0)
actor_poly_dict[actor.id] = poly
return actor_poly_dict
def _get_ego(self):
""" Get the ego vehicle object
"""
return self.ego
def _get_obs(self):
"""Get the observations."""
## Birdeye rendering
self.birdeye_render.vehicle_polygons = self.vehicle_polygons
self.birdeye_render.walker_polygons = self.walker_polygons
self.birdeye_render.waypoints = self.waypoints
self.birdeye_render.render(self.display)
# pygame.display.flip()
birdeye = pygame.surfarray.array3d(self.display)
birdeye = birdeye[0:self.display_size, :, :]
birdeye = np.fliplr(np.rot90(birdeye, 3)) # flip to regular view
birdeye = resize(birdeye, (self.obs_size, self.obs_size)) # resize
birdeye = birdeye * 255
birdeye.astype(np.uint8)
## Lidar image generation
point_cloud = []
# Get point cloud data
for location in self.lidar_data:
point_cloud.append([location.x, location.y, -location.z])
point_cloud = np.array(point_cloud)
# Separate the 3D space to bins for point cloud, x and y is set according to self.lidar_bin,
# and z is set to be two bins.
y_bins = np.arange(-(self.obs_range - self.d_behind),
self.d_behind + self.lidar_bin, self.lidar_bin)
x_bins = np.arange(-self.obs_range / 2,
self.obs_range / 2 + self.lidar_bin, self.lidar_bin)
z_bins = [-self.lidar_height - 1, -self.lidar_height + 0.25, 1]
# Get lidar image according to the bins
lidar, _ = np.histogramdd(point_cloud, bins=(x_bins, y_bins, z_bins))
lidar[:, :, 0] = np.array(lidar[:, :, 0] > 0, dtype=np.uint8)
lidar[:, :, 1] = np.array(lidar[:, :, 1] > 0, dtype=np.uint8)
# Add the waypoints to lidar image
wayptimg = (birdeye[:, :, 0] == 0) * (birdeye[:, :, 1]
== 0) * (birdeye[:, :, 2] == 255)
wayptimg = np.expand_dims(wayptimg, axis=2)
wayptimg = np.fliplr(np.rot90(wayptimg, 3))
wayptimg.astype(np.uint8)
# Get the final lidar image
lidar = np.concatenate((lidar, wayptimg), axis=2)
lidar = np.flip(lidar, axis=1)
lidar = np.rot90(lidar, 1)
lidar = lidar * 255
# Display lidar image
lidar_surface = pygame.Surface(
(self.display_size, self.display_size)).convert()
lidar_display = resize(lidar, (self.display_size, self.display_size))
lidar_display = np.flip(lidar_display, axis=1)
lidar_display = np.rot90(lidar_display, 1)
pygame.surfarray.blit_array(lidar_surface, lidar_display)
self.display.blit(lidar_surface, (self.display_size, 0))
# Display camera image
camera_surface = pygame.Surface(
(self.display_size, self.display_size)).convert()
camera_display = resize(self.camera_img,
(self.display_size, self.display_size))
camera_display = np.flip(camera_display, axis=1)
camera_display = np.rot90(camera_display, 1)
camera_display = camera_display * 255
pygame.surfarray.blit_array(camera_surface, camera_display)
self.display.blit(camera_surface, (self.display_size * 2, 0))
camera = pygame.surfarray.array3d(self.display)
camera = camera[2 * self.display_size:, :, :]
camera = np.fliplr(np.rot90(camera, 3)) # flip to regular view
camera = resize(camera, (self.obs_size, self.obs_size)) # resize
camera = camera * 255
camera.astype(np.uint8)
# Display on pygame
pygame.display.flip()
obs = {'birdeye': birdeye, 'lidar': lidar, 'camera': camera}
return obs
def _get_reward(self):
"""Calculate the step reward."""
# reward for speed tracking
v = self.ego.get_velocity()
speed = np.sqrt(v.x**2 + v.y**2)
r_speed = -abs(speed - self.desired_speed)
# reward for collision
r_collision = 0
if len(self.collision_hist) > 0:
r_collision = -1
# reward for steering:
r_steer = -self.ego.get_control().steer**2
# reward for out of lane
ego_x, ego_y = self._get_ego_pos()
dis, w = self._get_lane_dis(self.waypoints, ego_x, ego_y)
r_out = 0
if dis > self.out_lane_thres:
r_out = -1
# longitudinal speed
lspeed = np.array([v.x, v.y])
lspeed_lon = np.dot(lspeed, w)
# cost for too fast
r_fast = 0
if lspeed_lon > self.desired_speed:
r_fast = -1
# cost for lateral acceleration
r_lat = -abs(self.ego.get_control().steer) * lspeed_lon**2
r = 200 * r_collision + 1 * lspeed_lon + 10 * r_fast + 1 * r_out + r_steer * 5 + 0.2 * r_lat - 0.1
return r
def _get_ego_pos(self):
"""Get the ego vehicle pose (x, y)."""
ego_trans = self.ego.get_transform()
ego_x = ego_trans.location.x
ego_y = ego_trans.location.y
return ego_x, ego_y
def _get_lane_dis(self, waypoints, x, y):
"""Calculate distance from (x, y) to waypoints."""
dis_min = 1000
for pt in waypoints:
d = np.sqrt((x - pt[0])**2 + (y - pt[1])**2)
if d < dis_min:
dis_min = d
waypt = pt
vec = np.array([x - waypt[0], y - waypt[1]])
lv = np.linalg.norm(np.array(vec))
w = np.array(
[np.cos(waypt[2] / 180 * np.pi),
np.sin(waypt[2] / 180 * np.pi)])
costh = np.dot(vec / lv, w)
sinth = np.sqrt(1 - costh**2)
dis = lv * sinth
return dis, w
def _terminal(self):
"""Calculate whether to terminate the current episode."""
# Get ego state
ego_x, ego_y = self._get_ego_pos()
# If collides
if len(self.collision_hist) > 0:
return True
# If reach maximum timestep
if self.time_step > self.max_time_episode:
return True
# If at destination
if self.dests is not None: # If at destination
for dest in self.dests:
if np.sqrt((ego_x - dest[0])**2 + (ego_y - dest[1])**2) < 4:
return True
# If out of lane
dis, _ = self._get_lane_dis(self.waypoints, ego_x, ego_y)
if dis > self.out_lane_thres:
return True
return False
def _clear_all_actors(self, actor_filters):
"""Clear specific actors."""
for actor_filter in actor_filters:
for actor in self.world.get_actors().filter(actor_filter):
if actor.is_alive:
if actor.type_id == 'controller.ai.walker':
actor.stop()
actor.destroy()
class CarlaEnv(gym.Env):
"""An OpenAI gym wrapper for CARLA simulator."""
def __init__(self, params):
# parameters
print('init mai hai ham')
action_params = params['action_params']
self.path_params = params['path_params']
self.display_size = params['display_size'] # rendering screen size
self.max_past_step = params['max_past_step']
self.number_of_vehicles = params['number_of_vehicles']
self.number_of_walkers = params['number_of_walkers']
self.task_mode = params['task_mode']
self.max_time_episode = params['max_time_episode']
self.max_waypt = params['max_waypt']
self.obs_range = params['obs_range']
self.lidar_bin = params['lidar_bin']
self.d_behind = params['d_behind']
self.obs_size = int(self.obs_range / self.lidar_bin)
self.out_lane_thres = params['out_lane_thres']
self.desired_speed = params['desired_speed']
self.max_ego_spawn_times = params['max_ego_spawn_times']
self.display_route = params['display_route']
control_params = params['control_params']
self.args_lateral_dict = control_params['args_lateral_dict']
self.args_longitudinal_dict = control_params['args_longitudinal_dict']
D_T_S = action_params['d_t_s']
N_S_SAMPLE = action_params['n_s_sample']
MINT = action_params['mint']
MAXT = action_params['maxt']
MAX_ROAD_WIDTH = action_params['max_road_width']
MAX_LAT_VEL = action_params['max_lat_vel']
TARGET_SPEED = self.path_params['TARGET_SPEED']
self.dt = self.path_params['DT'] # time interval between 2 frames
if 'pixor' in params.keys():
self.pixor = params['pixor']
self.pixor_size = params['pixor_size']
else:
self.pixor = False
# Destination
if params['task_mode'] == 'roundabout':
self.dests = [[4.46, -61.46, 0], [-49.53, -2.89, 0],
[-6.48, 55.47, 0], [35.96, 3.33, 0]]
else:
self.dests = None
# action and observation spaces
self.discrete = params['discrete']
self.discrete_act = [params['discrete_acc'],
params['discrete_steer']] # acc, steer
self.n_acc = len(self.discrete_act[0])
self.n_steer = len(self.discrete_act[1])
if self.discrete:
self.action_space = spaces.Discrete(self.n_acc * self.n_steer)
else:
self.action_space = spaces.Box(
low=np.array([
TARGET_SPEED - D_T_S * N_S_SAMPLE, MINT, -MAX_ROAD_WIDTH,
-MAX_LAT_VEL
]),
high=np.array([
TARGET_SPEED + D_T_S * N_S_SAMPLE, MAXT, MAX_ROAD_WIDTH,
MAX_LAT_VEL
]),
dtype=np.float32)
observation_space_dict = {
'camera':
spaces.Box(low=0,
high=255,
shape=(self.obs_size, self.obs_size, 3),
dtype=np.uint8),
'lidar':
spaces.Box(low=0,
high=255,
shape=(self.obs_size, self.obs_size, 3),
dtype=np.uint8),
'birdeye':
spaces.Box(low=0,
high=255,
shape=(self.obs_size, self.obs_size, 3),
dtype=np.uint8),
'state':
spaces.Box(np.array([-2, -1, -5, 0]),
np.array([2, 1, 30, 1]),
dtype=np.float32)
}
if self.pixor: # dont change
observation_space_dict.update({
'roadmap':
spaces.Box(low=0,
high=255,
shape=(self.obs_size, self.obs_size, 3),
dtype=np.uint8),
'vh_clas':
spaces.Box(low=0,
high=1,
shape=(self.pixor_size, self.pixor_size, 1),
dtype=np.float32),
'vh_regr':
spaces.Box(low=-5,
high=5,
shape=(self.pixor_size, self.pixor_size, 6),
dtype=np.float32),
'pixor_state':
spaces.Box(np.array([-1000, -1000, -1, -1, -5]),
np.array([1000, 1000, 1, 1, 20]),
dtype=np.float32)
})
self.observation_space = spaces.Dict(observation_space_dict)
self.observation_space = spaces.Box(
low=-1,
high=1,
shape=(self.obs_size * self.obs_size * 3 + 4, 1),
dtype=np.float32)
# Connect to carla server and get world object
print('connecting to Carla server...')
client = carla.Client('localhost', params['port'])
client.set_timeout(10.0)
self.world = client.load_world(params['town'])
print('Carla server connected!')
# Set weather
self.world.set_weather(carla.WeatherParameters.ClearNoon)
# Get spawn points
self.vehicle_spawn_points = list(
self.world.get_map().get_spawn_points())
self.walker_spawn_points = []
for i in range(self.number_of_walkers):
spawn_point = carla.Transform()
loc = self.world.get_random_location_from_navigation()
if (loc != None):
spawn_point.location = loc
self.walker_spawn_points.append(spawn_point)
# Create the ego vehicle blueprint
self.ego_bp = self._create_vehicle_bluepprint(
params['ego_vehicle_filter'], color='49,8,8')
# Collision sensor
self.collision_hist = [] # The collision history
self.collision_hist_l = 1 # collision history length
self.collision_bp = self.world.get_blueprint_library().find(
'sensor.other.collision')
# Lidar sensor
self.lidar_data = None
self.lidar_height = 2.1 # Note : HEIGHT OF LIDAR
self.lidar_trans = carla.Transform(
carla.Location(x=0.0, z=self.lidar_height))
self.lidar_bp = self.world.get_blueprint_library().find(
'sensor.lidar.ray_cast')
self.lidar_bp.set_attribute('channels', '32')
self.lidar_bp.set_attribute('range', '5000')
# Camera sensor
self.camera_img = np.zeros((self.obs_size, self.obs_size, 3),
dtype=np.uint8)
self.camera_trans = carla.Transform(carla.Location(x=0.8, z=1.7))
self.camera_bp = self.world.get_blueprint_library().find(
'sensor.camera.rgb')
# Modify the attributes of the blueprint to set image resolution and field of view.
self.camera_bp.set_attribute('image_size_x', str(self.obs_size))
self.camera_bp.set_attribute('image_size_y', str(self.obs_size))
self.camera_bp.set_attribute('fov', '110')
# Set the time in seconds between sensor captures
self.camera_bp.set_attribute('sensor_tick', '0.02')
# Set fixed simulation step for synchronous mode
self.settings = self.world.get_settings()
self.settings.fixed_delta_seconds = self.dt
# Record the time of total steps and resetting steps
self.reset_step = 0
self.total_step = 0
# Initialize the renderer
self._init_renderer()
# Get pixel grid points
if self.pixor:
x, y = np.meshgrid(
np.arange(self.pixor_size),
np.arange(self.pixor_size)) # make a canvas with coordinates
x, y = x.flatten(), y.flatten()
self.pixel_grid = np.vstack((x, y)).T
def reset(self):
# Clear sensor objects
print('reset')
self.collision_sensor = None
self.lidar_sensor = None
self.camera_sensor = None
# Delete sensors, vehicles and walkers
self._clear_all_actors([
'sensor.other.collision', 'sensor.lidar.ray_cast',
'sensor.camera.rgb', 'vehicle.*', 'controller.ai.walker',
'walker.*'
])
# Disable sync mode
self._set_synchronous_mode(False)
# Spawn surrounding vehicles
random.shuffle(self.vehicle_spawn_points)
count = self.number_of_vehicles
if count > 0:
for spawn_point in self.vehicle_spawn_points:
if self._try_spawn_random_vehicle_at(spawn_point,
number_of_wheels=[4]):
count -= 1
if count <= 0:
break
while count > 0:
if self._try_spawn_random_vehicle_at(random.choice(
self.vehicle_spawn_points),
number_of_wheels=[4]):
count -= 1
# Spawn pedestrians
random.shuffle(self.walker_spawn_points)
count = self.number_of_walkers
if count > 0:
for spawn_point in self.walker_spawn_points:
if self._try_spawn_random_walker_at(spawn_point):
count -= 1
if count <= 0:
break
while count > 0:
if self._try_spawn_random_walker_at(
random.choice(self.walker_spawn_points)):
count -= 1
# Get actors polygon list
self.vehicle_polygons = []
vehicle_poly_dict = self._get_actor_polygons('vehicle.*')
self.vehicle_polygons.append(vehicle_poly_dict)
self.walker_polygons = []
walker_poly_dict = self._get_actor_polygons('walker.*')
self.walker_polygons.append(walker_poly_dict)
# Spawn the ego vehicle
ego_spawn_times = 0
while True:
if ego_spawn_times > self.max_ego_spawn_times:
self.reset()
if self.task_mode == 'random':
transform = random.choice(self.vehicle_spawn_points)
if self.task_mode == 'roundabout':
self.start = [52.1 + np.random.uniform(-5, 5), -4.2,
178.66] # random
# self.start=[52.1,-4.2, 178.66] # static
transform = set_carla_transform(self.start)
if self.task_mode == 'debug':
self.start = [88.71, -237, 4] # static
transform = set_carla_transform(self.start)
if self._try_spawn_ego_vehicle_at(transform):
break
else:
ego_spawn_times += 1
time.sleep(0.1)
# Initializing Controller (Requires ego vehicle spawn)
self._vehicle_controller = VehiclePIDController(
self.ego,
args_lateral=self.args_lateral_dict,
args_longitudinal=self.args_longitudinal_dict)
# Add collision sensor
self.collision_sensor = self.world.spawn_actor(self.collision_bp,
carla.Transform(),
attach_to=self.ego)
self.collision_sensor.listen(lambda event: get_collision_hist(event))
def get_collision_hist(event):
impulse = event.normal_impulse
intensity = np.sqrt(impulse.x**2 + impulse.y**2 + impulse.z**2)
self.collision_hist.append(intensity)
if len(self.collision_hist) > self.collision_hist_l:
self.collision_hist.pop(0)
self.collision_hist = []
# Add lidar sensor
self.lidar_sensor = self.world.spawn_actor(self.lidar_bp,
self.lidar_trans,
attach_to=self.ego)
self.lidar_sensor.listen(lambda data: get_lidar_data(data))
def get_lidar_data(data):
self.lidar_data = data
# Add camera sensor
self.camera_sensor = self.world.spawn_actor(self.camera_bp,
self.camera_trans,
attach_to=self.ego)
self.camera_sensor.listen(lambda data: get_camera_img(data))
def get_camera_img(data):
array = np.frombuffer(data.raw_data, dtype=np.dtype("uint8"))
array = np.reshape(array, (data.height, data.width, 4))
array = array[:, :, :3]
array = array[:, :, ::-1]
self.camera_img = array
# Update timesteps
self.time_step = 0
self.reset_step += 1
# Enable sync mode
self.settings.synchronous_mode = True
self.world.apply_settings(self.settings)
# ===================================================================================
self.routeplanner = RoutePlanner(self.ego, self.max_waypt)
self.waypoints, _, self.vehicle_front = self.routeplanner.run_step()
# ===================================================================================
W_X_0 = [t[0] for t in self.waypoints]
W_Y_0 = [t[1] for t in self.waypoints]
self.tx, self.ty, self.tyaw, self.tc, self.csp = frenet_optimal_trajectory.generate_target_course(
W_X_0, W_Y_0)
self.birdeye_render.set_hero(self.ego, self.ego.id)
return self._get_obs()
def step(self, action):
# Get target speed, target_waypoint
print('step mai hai ham')
ego_trans = self.ego.get_transform()
ego_z = ego_trans.location.z
# act = carla.VehicleControl(throttle=float(throttle), steer=float(-steer), brake=float(brake))
Ti = action[1]
di = action[2]
di_d = action[3]
tv = action[0]
# Initial Conditions
s0, c_speed, c_d, c_d_d, c_d_dd = self.initial_conditions()
# Generate Frenet Path
path = frenet_optimal_trajectory.frenet_optimal_planning(
self.csp, s0, c_speed, c_d, c_d_d, c_d_dd, Ti, di, di_d, tv,
self.path_params)
self.target_waypoint = [path.x[1], path.y[1], ego_z]
# sqrt(pow(1 - rk[1]*c_d, 2)*pow(c_speed, 2) + pow(c_d_d, 2))
# To be given in Km/h
self._target_speed = ((((1 - self.tc[1])**2) * ((path.s_d[1])**2) +
((path.d_d[1])**2))**0.5) * 3.6
# Apply control
control = self._vehicle_controller.run_step(self._target_speed,
self.target_waypoint)
self.ego.apply_control(control)
self.world.tick()
# Append actors polygon list
vehicle_poly_dict = self._get_actor_polygons('vehicle.*')
self.vehicle_polygons.append(vehicle_poly_dict)
while len(self.vehicle_polygons) > self.max_past_step:
self.vehicle_polygons.pop(0)
walker_poly_dict = self._get_actor_polygons('walker.*')
self.walker_polygons.append(walker_poly_dict)
while len(self.walker_polygons) > self.max_past_step:
self.walker_polygons.pop(0)
# route planner
self.waypoints, _, self.vehicle_front = self.routeplanner.run_step()
# state information
info = {
'waypoints': self.waypoints,
'vehicle_front': self.vehicle_front
}
# Update timesteps
self.time_step += 1
self.total_step += 1
return (self._get_obs(), self._get_reward(), self._terminal(),
copy.deepcopy(info))
def initial_conditions(self):
print('initial_conditions mai hai ham')
vel = self.ego.get_velocity()
ego_trans = self.ego.get_transform()
ego_x, ego_y = get_pos(self.ego)
v = ((vel.x**2) + (vel.y**2))**0.5
min_id, c_d = self.find_nearest_in_global_path()
vec1 = [ego_x - self.tx[min_id], ego_y - self.ty[min_id]]
vec2 = [
self.tx[min_id] - self.tx[min_id + 1],
self.tx[min_id] - self.ty[min_id + 1]
]
curl = vec1[0] * vec2[1] - vec1[1] * vec2[0]
if (curl < 0):
c_d = c_d * (-1)
s0 = self.calc_s(min_id)
ego_yaw = ego_trans.rotation.yaw / 180 * np.pi
global_path_yaw = self.tyaw[min_id]
delta_theta = ego_yaw - global_path_yaw
c_d_d = v * math.sin(delta_theta)
k_r = self.tc[min_id]
c_speed = v * math.cos(delta_theta) / (1 - k_r * c_d)
c_d_dd = 0
return s0, c_speed, c_d, c_d_d, c_d_dd
def find_nearest_in_global_path(self):
print('find nearest in global path')
ego_x, ego_y = get_pos(self.ego)
tx = np.asarray(self.tx, dtype=np.float32)
ty = np.asarray(self.ty, dtype=np.float32)
temp = (tx - ego_x)**2 + (ty - ego_y)**2
temp = np.sqrt(temp)
min_id = np.argmin(temp)
min_dist = np.min(temp)
return min_id, min_dist
def calc_s(self, min_id):
print('calcs mai hai ham')
tx = np.asarray(self.tx, dtype=np.float32)
ty = np.asarray(self.ty, dtype=np.float32)
to_stop = min_id + 1
tx_cut = tx[:to_stop]
ty_cut = ty[:to_stop]
tx_cut_2 = np.hstack((tx_cut[0], tx_cut))
ty_cut_2 = np.hstack((ty_cut[0], ty_cut))
tx_cut_2 = tx_cut_2[:to_stop]
ty_cut_2 = ty_cut_2[:to_stop]
s = np.sum(np.sqrt((tx_cut - tx_cut_2)**2 + (ty_cut - ty_cut_2)**2))
return s
def seed(self, seed=None):
print('seed')
self.np_random, seed = seeding.np_random(seed)
return [seed]
def render(self, mode):
pass
def _create_vehicle_bluepprint(self,
actor_filter,
color=None,
number_of_wheels=[4]):
print('create vehicale blyeprint')
"""Create the blueprint for a specific actor type.
Args:
actor_filter: a string indicating the actor type, e.g, 'vehicle.lincoln*'.
Returns:
bp: the blueprint object of carla.
"""
blueprints = self.world.get_blueprint_library().filter(actor_filter)
blueprint_library = []
for nw in number_of_wheels:
blueprint_library = blueprint_library + \
[x for x in blueprints if int(
x.get_attribute('number_of_wheels')) == nw]
bp = random.choice(blueprint_library)
if bp.has_attribute('color'):
if not color:
color = random.choice(
bp.get_attribute('color').recommended_values)
bp.set_attribute('color', color)
return bp
def _init_renderer(self):
"""Initialize the birdeye view renderer.
"""
print('init renderer mai hai ham')
pygame.init()
self.display = pygame.display.set_mode(
(self.display_size * 3, self.display_size),
pygame.HWSURFACE | pygame.DOUBLEBUF)
pixels_per_meter = self.display_size / self.obs_range
pixels_ahead_vehicle = (self.obs_range / 2 -
self.d_behind) * pixels_per_meter
birdeye_params = {
'screen_size': [self.display_size, self.display_size],
'pixels_per_meter': pixels_per_meter,
'pixels_ahead_vehicle': pixels_ahead_vehicle
}
self.birdeye_render = BirdeyeRender(self.world, birdeye_params)
def _set_synchronous_mode(self, synchronous=True):
"""Set whether to use the synchronous mode.
"""
print('set synchrounous mode mai hai ham')
self.settings.synchronous_mode = synchronous
self.world.apply_settings(self.settings)
def _try_spawn_random_vehicle_at(self, transform, number_of_wheels=[4]):
"""Try to spawn a surrounding vehicle at specific transform with random bluprint.
Args:
transform: the carla transform object.
Returns:
Bool indicating whether the spawn is successful.
"""
print('try spawn rnadom vehicle mai hai ham')
blueprint = self._create_vehicle_bluepprint(
'vehicle.*', number_of_wheels=number_of_wheels)
blueprint.set_attribute('role_name', 'autopilot')
vehicle = self.world.try_spawn_actor(blueprint, transform)
if vehicle is not None:
vehicle.set_autopilot()
return True
return False
def _try_spawn_random_walker_at(self, transform):
"""Try to spawn a walker at specific transform with random bluprint.
Args:
transform: the carla transform object.
Returns:
Bool indicating whether the spawn is successful.
"""
print('try spawn random walker mai hai ham')
walker_bp = random.choice(
self.world.get_blueprint_library().filter('walker.*'))
# set as not invencible
if walker_bp.has_attribute('is_invincible'):
walker_bp.set_attribute('is_invincible', 'false')
walker_actor = self.world.try_spawn_actor(walker_bp, transform)
if walker_actor is not None:
walker_controller_bp = self.world.get_blueprint_library().find(
'controller.ai.walker')
walker_controller_actor = self.world.spawn_actor(
walker_controller_bp, carla.Transform(), walker_actor)
# start walker
walker_controller_actor.start()
# set walk to random point
walker_controller_actor.go_to_location(
self.world.get_random_location_from_navigation())
# random max speed
# max speed between 1 and 2 (default is 1.4 m/s)
walker_controller_actor.set_max_speed(1 + random.random())
return True
return False
def _try_spawn_ego_vehicle_at(self, transform):
"""Try to spawn the ego vehicle at specific transform.
Args:
transform: the carla transform object.
Returns:
Bool indicating whether the spawn is successful.
"""
print('try spawn ego vehicle at mai hai ham')
vehicle = None
# Check if ego position overlaps with surrounding vehicles
overlap = False
for idx, poly in self.vehicle_polygons[-1].items():
poly_center = np.mean(poly, axis=0)
ego_center = np.array([transform.location.x, transform.location.y])
dis = np.linalg.norm(poly_center - ego_center)
if dis > 8:
continue
else:
overlap = True
break
if not overlap:
vehicle = self.world.try_spawn_actor(self.ego_bp, transform)
if vehicle is not None:
self.ego = vehicle
return True
return False
def _get_actor_polygons(self, filt):
"""Get the bounding box polygon of actors.
Args:
filt: the filter indicating what type of actors we'll look at.
Returns:
actor_poly_dict: a dictionary containing the bounding boxes of specific actors.
"""
print('get actor polygons mai hai ham')
actor_poly_dict = {}
for actor in self.world.get_actors().filter(filt):
# Get x, y and yaw of the actor
trans = actor.get_transform()
x = trans.location.x
y = trans.location.y
yaw = trans.rotation.yaw / 180 * np.pi
# Get length and width
bb = actor.bounding_box
l = bb.extent.x
w = bb.extent.y
# Get bounding box polygon in the actor's local coordinate
poly_local = np.array([[l, w], [l, -w], [-l, -w],
[-l, w]]).transpose()
# Get rotation matrix to transform to global coordinate
R = np.array([[np.cos(yaw), -np.sin(yaw)],
[np.sin(yaw), np.cos(yaw)]])
# Get global bounding box polygon
poly = np.matmul(R, poly_local).transpose() + \
np.repeat([[x, y]], 4, axis=0)
actor_poly_dict[actor.id] = poly
return actor_poly_dict
def _get_obs(self):
"""Get the observations."""
# Birdeye rendering
print('get obs mai hai ham')
self.birdeye_render.vehicle_polygons = self.vehicle_polygons
self.birdeye_render.walker_polygons = self.walker_polygons
self.birdeye_render.waypoints = self.waypoints
# birdeye view with roadmap and actors
birdeye_render_types = ['roadmap', 'actors']
if self.display_route:
birdeye_render_types.append('waypoints')
self.birdeye_render.render(self.display, birdeye_render_types)
birdeye = pygame.surfarray.array3d(self.display)
birdeye = birdeye[0:self.display_size, :, :]
birdeye = display_to_rgb(birdeye, self.obs_size)
# Roadmap
if self.pixor:
roadmap_render_types = ['roadmap']
if self.display_route:
roadmap_render_types.append('waypoints')
self.birdeye_render.render(self.display, roadmap_render_types)
roadmap = pygame.surfarray.array3d(self.display)
roadmap = roadmap[0:self.display_size, :, :]
roadmap = display_to_rgb(roadmap, self.obs_size)
# Add ego vehicle
for i in range(self.obs_size):
for j in range(self.obs_size):
if abs(birdeye[i, j, 0] - 255) < 20 and abs(
birdeye[i, j, 1] -
0) < 20 and abs(birdeye[i, j, 0] - 255) < 20:
roadmap[i, j, :] = birdeye[i, j, :]
# Display birdeye image
birdeye_surface = rgb_to_display_surface(birdeye, self.display_size)
self.display.blit(birdeye_surface, (0, 0))
# Lidar image generation
point_cloud = []
# Get point cloud data
for location in self.lidar_data:
point_cloud.append([location.x, location.y, -location.z])
point_cloud = np.array(point_cloud)
# Separate the 3D space to bins for point cloud, x and y is set according to self.lidar_bin,
# and z is set to be two bins.
y_bins = np.arange(-(self.obs_range - self.d_behind),
self.d_behind + self.lidar_bin, self.lidar_bin)
x_bins = np.arange(-self.obs_range / 2,
self.obs_range / 2 + self.lidar_bin, self.lidar_bin)
z_bins = [-self.lidar_height - 1, -self.lidar_height + 0.25, 1]
# Get lidar image according to the bins
lidar, _ = np.histogramdd(point_cloud, bins=(x_bins, y_bins, z_bins))
# Only 2 bins in z-direction. Taking positive values.
lidar[:, :, 0] = np.array(lidar[:, :, 0] > 0, dtype=np.uint8)
lidar[:, :, 1] = np.array(lidar[:, :, 1] > 0, dtype=np.uint8)
# Add the waypoints to lidar image
if self.display_route:
wayptimg = (birdeye[:, :, 0] <= 10) * \
(birdeye[:, :, 1] <= 10) * (birdeye[:, :, 2] >= 240)
else:
wayptimg = birdeye[:, :, 0] < 0 # Equal to a zero matrix
wayptimg = np.expand_dims(wayptimg, axis=2)
wayptimg = np.fliplr(np.rot90(wayptimg, 3))
# Get the final lidar image
lidar = np.concatenate((lidar, wayptimg), axis=2)
lidar = np.flip(lidar, axis=1)
lidar = np.rot90(lidar, 1)
lidar = lidar * 255
# Display lidar image
lidar_surface = rgb_to_display_surface(lidar, self.display_size)
self.display.blit(lidar_surface, (self.display_size, 0))
# Display camera image
camera = resize(self.camera_img, (self.obs_size, self.obs_size)) * 255
camera_surface = rgb_to_display_surface(camera, self.display_size)
self.display.blit(camera_surface, (self.display_size * 2, 0))
# Display on pygame
pygame.display.flip()
# State observation
ego_trans = self.ego.get_transform()
ego_x = ego_trans.location.x
ego_y = ego_trans.location.y
ego_yaw = ego_trans.rotation.yaw / 180 * np.pi # in Radians
lateral_dis, w = get_preview_lane_dis(self.waypoints, ego_x, ego_y)
delta_yaw = np.arcsin(
np.cross(w, np.array(np.array([np.cos(ego_yaw),
np.sin(ego_yaw)]))))
v = self.ego.get_velocity()
ang_v = self.ego.get_angular_velocity()
speed = np.sqrt(v.x**2 + v.y**2)
# state = np.array([lateral_dis, - delta_yaw, speed,ang_v.z, self.vehicle_front])
state = np.array([lateral_dis, -delta_yaw, speed, ang_v.z])
# state.reshape((4, 1))
if self.pixor:
# Vehicle classification and regression maps (requires further normalization)
vh_clas = np.zeros((self.pixor_size, self.pixor_size))
vh_regr = np.zeros((self.pixor_size, self.pixor_size, 6))
# Generate the PIXOR image. Note in CARLA it is using left-hand coordinate
# Get the 6-dim geom parametrization in PIXOR, here we use pixel coordinate
for actor in self.world.get_actors().filter('vehicle.*'):
x, y, yaw, l, w = get_info(actor)
x_local, y_local, yaw_local = get_local_pose(
(x, y, yaw), (ego_x, ego_y, ego_yaw))
if actor.id != self.ego.id:
if abs(
y_local
) < self.obs_range / 2 + 1 and x_local < self.obs_range - self.d_behind + 1 and x_local > -self.d_behind - 1:
x_pixel, y_pixel, yaw_pixel, l_pixel, w_pixel = get_pixel_info(
local_info=(x_local, y_local, yaw_local, l, w),
d_behind=self.d_behind,
obs_range=self.obs_range,
image_size=self.pixor_size)
cos_t = np.cos(yaw_pixel)
sin_t = np.sin(yaw_pixel)
logw = np.log(w_pixel)
logl = np.log(l_pixel)
pixels = get_pixels_inside_vehicle(
pixel_info=(x_pixel, y_pixel, yaw_pixel, l_pixel,
w_pixel),
pixel_grid=self.pixel_grid)
for pixel in pixels:
vh_clas[pixel[0], pixel[1]] = 1
dx = x_pixel - pixel[0]
dy = y_pixel - pixel[1]
vh_regr[pixel[0], pixel[1], :] = np.array(
[cos_t, sin_t, dx, dy, logw, logl])
# Flip the image matrix so that the origin is at the left-bottom
vh_clas = np.flip(vh_clas, axis=0)
vh_regr = np.flip(vh_regr, axis=0)
# Pixor state, [x, y, cos(yaw), sin(yaw), speed]
pixor_state = [
ego_x, ego_y,
np.cos(ego_yaw),
np.sin(ego_yaw), speed
]
obs = {
'camera': camera.astype(np.uint8),
'lidar': lidar.astype(np.uint8),
'birdeye': birdeye.astype(np.uint8),
'state': state
}
if self.pixor:
obs.update({
'roadmap':
roadmap.astype(np.uint8),
'vh_clas':
np.expand_dims(vh_clas, -1).astype(np.float32),
'vh_regr':
vh_regr.astype(np.float32),
'pixor_state':
pixor_state,
})
print(camera.shape)
print(lidar.shape)
print(birdeye.shape)
print(state.shape)
# float32 mai likhna hai
temp = lidar.flatten()
print(temp.shape)
print("hello")
temp = np.concatenate((temp, state))
temp = temp.reshape((temp.shape[0], 1))
temp = scale(temp, -1, 1)
print(temp.shape)
return temp
def _get_reward(self):
"""Calculate the step reward."""
print('get reward mai hai ham')
# reward for speed tracking
v = self.ego.get_velocity()
speed = np.sqrt(v.x**2 + v.y**2)
r_speed = -abs(speed - self.desired_speed)
# reward for collision
r_collision = 0
if len(self.collision_hist) > 0:
r_collision = -1
# reward for steering:
r_steer = -self.ego.get_control().steer**2
# reward for out of lane
ego_x, ego_y = get_pos(self.ego)
dis, w = get_lane_dis(self.waypoints, ego_x, ego_y)
r_out = 0
if abs(dis) > self.out_lane_thres:
r_out = -1
# longitudinal speed
lspeed = np.array([v.x, v.y])
lspeed_lon = np.dot(lspeed, w)
# cost for too fast
r_fast = 0
if lspeed_lon > self.desired_speed:
r_fast = -1
# cost for lateral acceleration
r_lat = -abs(self.ego.get_control().steer) * lspeed_lon**2
r = 200*r_collision + 1*lspeed_lon + 10 * \
r_fast + 1*r_out + r_steer*5 + 0.2*r_lat - 0.1
return r
def _terminal(self):
"""Calculate whether to terminate the current episode."""
print('terminal mai hai ham')
# Get ego state
ego_x, ego_y = get_pos(self.ego)
# If collides
if len(self.collision_hist) > 0:
return True
# If reach maximum timestep
if self.time_step > self.max_time_episode:
return True
# If at destination
if self.dests is not None: # If at destination
for dest in self.dests:
if np.sqrt((ego_x - dest[0])**2 + (ego_y - dest[1])**2) < 4:
return True
# If out of lane
dis, _ = get_lane_dis(self.waypoints, ego_x, ego_y)
if abs(dis) > self.out_lane_thres:
return True
return False
def _clear_all_actors(self, actor_filters):
"""Clear specific actors."""
print('clear all mai hai ham')
for actor_filter in actor_filters:
for actor in self.world.get_actors().filter(actor_filter):
if actor.is_alive:
if actor.type_id == 'controller.ai.walker':
actor.stop()
actor.destroy()
class CarlaEnv(gym.Env):
"""An OpenAI gym wrapper for CARLA simulator."""
def __init__(self, params):
# parameters
self.display_size = params['display_size'] # rendering screen size
self.max_past_step = params['max_past_step']
self.number_of_vehicles = params['number_of_vehicles']
self.number_of_walkers = params['number_of_walkers']
self.dt = params['dt']
self.task_mode = params['task_mode']
self.max_time_episode = params['max_time_episode']
self.max_waypt = params['max_waypt']
self.obs_range = params['obs_range']
self.lidar_bin = params['lidar_bin']
self.d_behind = params['d_behind']
self.obs_size = int(self.obs_range / self.lidar_bin)
self.out_lane_thres = params['out_lane_thres']
self.desired_speed = params['desired_speed']
self.max_ego_spawn_times = params['max_ego_spawn_times']
self.display_route = params['display_route']
self.use_rgb_camera = params['RGB_cam']
self.traffic_vehicles = []
#self.discrete_acc = [-1,-0.5,0.0,0.5,1.0] # discrete actions for throttle
self.discrete_vel = [-5.0, -1.0, 0.0, 1.0,
5.0] # discrete actions for velocity
self.discrete_actions = params[
'discrete'] # boolean to use discrete or continoius action space
self.cur_action = None
self.pedal_pid = PID(0.7, 0.01, 0.0)
self.pedal_pid.output_limits = (-1, 1)
self.rl_speed = 0.0
self.pedal_pid.setpoint = 0.0
self.dist_to_lead = -1
# Destination
if params['task_mode'] == 'acc_1':
self.dests = [[592.1, 244.7, 0]] # stopping condition in Town 06
else:
self.dests = None
self.idle_timesteps = 0
# action and observation spaces
# self.action_space = spaces.Box(np.array([params['continuous_accel_range'][0]], dtype=np.float32), np.array([params['continuous_accel_range'][1]], dtype=np.float32)) # acc
#self.action_space = spaces.Discrete(len(self.discrete_acc))
if self.discrete_actions:
self.action_space = spaces.Discrete(
5) # slow down -5, slowdown -1 m/s, do nothing, speed up 1 m/s
else:
self.action_space = spaces.Box(np.array([0.0]), np.array(
[30.0])) # speed is continous from 0 m.s to 30 m.s
self.observation_space = spaces.Box(np.array([0, 0, -1.0]),
np.array([40, 40, 100]),
dtype=np.float32)
# Connect to carla server and get world object
print('connecting to Carla server...')
self.client = carla.Client('localhost', params['port'])
self.client.set_timeout(10.0)
self.world = self.client.load_world(params['town'])
print('Carla server connected!')
self.map = self.world.get_map()
self.tm = self.client.get_trafficmanager(int(8000))
self.tm_port = self.tm.get_port()
#print('Traffic Manager Port ' + self.tm_port)
# Set weather
self.world.set_weather(carla.WeatherParameters.ClearNoon)
# Get spawn points
self.vehicle_spawn_points = list(
self.world.get_map().get_spawn_points())
self.walker_spawn_points = []
for i in range(self.number_of_walkers):
spawn_point = carla.Transform()
loc = self.world.get_random_location_from_navigation()
if (loc != None):
spawn_point.location = loc
self.walker_spawn_points.append(spawn_point)
# Create the ego vehicle blueprint
self.ego_bp = self._create_vehicle_bluepprint(
params['ego_vehicle_filter'], color='49,8,8')
# Collision sensor
self.collision_hist = [] # The collision history
self.collision_hist_l = 1 # collision history length
self.collision_bp = self.world.get_blueprint_library().find(
'sensor.other.collision')
# Camera sensor
self.camera_img = np.zeros((self.obs_size, self.obs_size, 3),
dtype=np.uint8)
self.camera_trans = carla.Transform(carla.Location(x=0.8, z=1.7))
self.camera_bp = self.world.get_blueprint_library().find(
'sensor.camera.rgb')
# Modify the attributes of the blueprint to set image resolution and field of view.
self.camera_bp.set_attribute('image_size_x', str(self.obs_size))
self.camera_bp.set_attribute('image_size_y', str(self.obs_size))
self.camera_bp.set_attribute('fov', '110')
# Set the time in seconds between sensor captures
self.camera_bp.set_attribute('sensor_tick', '0.02')
# Set fixed simulation step for synchronous mode
self.settings = self.world.get_settings()
self.settings.fixed_delta_seconds = self.dt
# Record the time of total steps and resetting steps
self.reset_step = 0
self.total_step = 0
# Initialize the renderer
self._init_renderer()
def reset(self):
# Clear sensor objects
self._clear_all_sensors()
self.collision_sensor = None
self.camera_sensor = None
self.idle_timesteps = 0
# Delete sensors, vehicles and walkers
self._clear_all_actors([
'sensor.other.collision', 'sensor.lidar.ray_cast',
'sensor.camera.rgb', 'vehicle.*', 'controller.ai.walker',
'walker.*'
])
# Disable sync mode
self._set_synchronous_mode(False)
# Spawn vehicles in same lane as ego vehicle and ahead
ego_vehicle_traffic_spawns_1 = get_spawn_points_for_traffic(
40, [-7, -6, -5, -4], self.map, self.number_of_vehicles)
ego_vehicle_traffic_spawns_2 = get_spawn_points_for_traffic(
39, [-7, -6, -5, -4], self.map, self.number_of_vehicles, start=15)
ego_vehicle_traffic_spawns = ego_vehicle_traffic_spawns_1 + ego_vehicle_traffic_spawns_2
random.shuffle(ego_vehicle_traffic_spawns)
count = self.number_of_vehicles
if count > 0:
for spawn_point in ego_vehicle_traffic_spawns:
if self._try_spawn_random_vehicle_at(spawn_point,
number_of_wheels=[4]):
count -= 1
if count <= 0:
break
while count > 0:
if self._try_spawn_random_vehicle_at(
random.choice(ego_vehicle_traffic_spawns),
number_of_wheels=[4]):
count -= 1
# set autopilot for all traffic vehicles:
batch = []
for vehicle in self.traffic_vehicles:
batch.append(carla.command.SetAutopilot(vehicle, True))
self.client.apply_batch_sync(batch)
# Get actors polygon list
self.vehicle_polygons = []
vehicle_poly_dict = self._get_actor_polygons('vehicle.*')
self.vehicle_polygons.append(vehicle_poly_dict)
self.walker_polygons = []
walker_poly_dict = self._get_actor_polygons('walker.*')
self.walker_polygons.append(walker_poly_dict)
# Spawn the ego vehicle
ego_spawn_times = 0
while True:
if ego_spawn_times > self.max_ego_spawn_times:
self.reset()
if self.task_mode == 'acc_1':
wpt1 = get_waypoint_for_ego_spawn(road_id=39,
lane_id=-5,
s=0,
map=self.map)
transform = wpt1.transform
transform.location.z += 2.0
if self._try_spawn_ego_vehicle_at(transform):
break
else:
print('trying to spawn %d' % ego_spawn_times)
ego_spawn_times += 1
time.sleep(0.1)
# Add collision sensor
self.collision_sensor = self.world.spawn_actor(self.collision_bp,
carla.Transform(),
attach_to=self.ego)
self.collision_sensor.listen(lambda event: get_collision_hist(event))
def get_collision_hist(event):
impulse = event.normal_impulse
intensity = np.sqrt(impulse.x**2 + impulse.y**2 + impulse.z**2)
self.collision_hist.append(intensity)
if len(self.collision_hist) > self.collision_hist_l:
self.collision_hist.pop(0)
self.collision_hist = []
# Add camera sensor
if self.use_rgb_camera:
self.camera_sensor = self.world.spawn_actor(self.camera_bp,
self.camera_trans,
attach_to=self.ego)
self.camera_sensor.listen(lambda data: get_camera_img(data))
def get_camera_img(data):
array = np.frombuffer(data.raw_data, dtype=np.dtype("uint8"))
array = np.reshape(array, (data.height, data.width, 4))
array = array[:, :, :3]
array = array[:, :, ::-1]
self.camera_img = array
# Update timesteps
self.time_step = 0
self.reset_step += 1
# Enable sync mode
self.settings.synchronous_mode = False
if not self.use_rgb_camera:
self.settings.no_rendering_mode = True
self.world.apply_settings(self.settings)
self.routeplanner = RoutePlanner(self.ego, self.max_waypt)
self.waypoints, _, self.vehicle_hazards = self.routeplanner.run_step()
# Set ego information for render
self.birdeye_render.set_hero(self.ego, self.ego.id)
return self._get_obs()
def step(self, action):
# Calculate acceleration and steering
#acc = self.discrete_acc[action]
v = self.ego.get_velocity()
speed = np.sqrt(v.x**2 + v.y**2)
if speed < 1e-1:
self.idle_timesteps += 1
else:
self.idle_timesteps = 0
if self.discrete_actions:
self.rl_speed += self.discrete_vel[action] # [-1.0 0.0, 1.0]
else:
self.rl_speed = action # range from 0 to 30
self.rl_speed = np.clip(self.rl_speed, 0.0, 30.0)
pid = self.pedal_pid(-(self.rl_speed - speed))
acc = pid
# acc = self.pedal_pid(speed)
# Convert acceleration to throttle and brake
if acc > 0:
throttle = np.clip(acc, 0, 1)
brake = 0
else:
throttle = 0
brake = np.clip(-acc, 0, 1)
#print(acc,speed,self.desired_speed-speed)
#print("rl-speed %.2f, pid %.2f, acc %.2f" % (self.rl_speed , pid, acc) )
# Apply control
act = carla.VehicleControl(throttle=float(throttle),
steer=0.0,
brake=float(brake))
self.ego.apply_control(act)
self.world.tick()
# Append actors polygon list
vehicle_poly_dict = self._get_actor_polygons('vehicle.*')
self.vehicle_polygons.append(vehicle_poly_dict)
while len(self.vehicle_polygons) > self.max_past_step:
self.vehicle_polygons.pop(0)
walker_poly_dict = self._get_actor_polygons('walker.*')
self.walker_polygons.append(walker_poly_dict)
while len(self.walker_polygons) > self.max_past_step:
self.walker_polygons.pop(0)
# route planner
self.waypoints, _, self.vehicle_hazards = self.routeplanner.run_step()
#print(self.vehicle_hazards)
# state information
info = {
'waypoints': self.waypoints,
}
# Update timesteps
self.time_step += 1
self.total_step += 1
return (self._get_obs(), self._get_reward(), self._terminal(),
copy.deepcopy(info))
def seed(self, seed=None):
self.np_random, seed = seeding.np_random(seed)
return [seed]
def render(self, mode):
pass
def _create_vehicle_bluepprint(self,
actor_filter,
color=None,
number_of_wheels=[4]):
"""Create the blueprint for a specific actor type.
Args:
actor_filter: a string indicating the actor type, e.g, 'vehicle.lincoln*'.
Returns:
bp: the blueprint object of carla.
"""
blueprints = self.world.get_blueprint_library().filter(actor_filter)
blueprint_library = []
for nw in number_of_wheels:
blueprint_library = blueprint_library + [
x for x in blueprints
if int(x.get_attribute('number_of_wheels')) == nw
]
bp = random.choice(blueprint_library)
if bp.has_attribute('color'):
if not color:
color = random.choice(
bp.get_attribute('color').recommended_values)
bp.set_attribute('color', color)
return bp
def _init_renderer(self):
"""Initialize the birdeye view renderer.
"""
pygame.init()
self.pyfont = pygame.font.SysFont(None, 22)
self.display = pygame.display.set_mode(
(self.display_size * 3, self.display_size),
pygame.HWSURFACE | pygame.DOUBLEBUF)
pixels_per_meter = self.display_size / self.obs_range
pixels_ahead_vehicle = (self.obs_range / 2 -
self.d_behind) * pixels_per_meter
birdeye_params = {
'screen_size': [self.display_size, self.display_size],
'pixels_per_meter': pixels_per_meter,
'pixels_ahead_vehicle': pixels_ahead_vehicle
}
self.birdeye_render = BirdeyeRender(self.world, birdeye_params)
def _set_synchronous_mode(self, synchronous=True):
"""Set whether to use the synchronous mode.
"""
self.settings.synchronous_mode = synchronous
self.world.apply_settings(self.settings)
def _try_spawn_random_vehicle_at(self, transform, number_of_wheels=[4]):
"""Try to spawn a surrounding vehicle at specific transform with random bluprint.
Args:
transform: the carla transform object.
Returns:
Bool indicating whether the spawn is successful.
"""
blueprint = self._create_vehicle_bluepprint(
'vehicle.*', number_of_wheels=number_of_wheels)
blueprint.set_attribute('role_name', 'autopilot')
vehicle = self.world.try_spawn_actor(blueprint, transform)
if vehicle is not None:
self.traffic_vehicles.append(vehicle)
#vehicle.set_autopilot()
# batch = []
# batch.append(carla.command.SetAutopilot(vehicle,True))
# self.client.apply_batch_sync(batch) # not how this is supposed to be done but oh well
#vehicle.enable_constant_velocity(np.random.uniform(low=18.0,high=30.0))
vehicle.set_autopilot(True, self.tm_port)
self.tm.auto_lane_change(vehicle, False)
high = np.random.uniform(low=-20, high=0)
low = np.random.uniform(low=-20, high=0)
self.tm.vehicle_percentage_speed_difference(
vehicle, random.choice([high, low])
) # percentage difference between posted speed and vehicle speed. Negative is greater
return True
return False
def _try_spawn_ego_vehicle_at(self, transform):
"""Try to spawn the ego vehicle at specific transform.
Args:
transform: the carla transform object.
Returns:
Bool indicating whether the spawn is successful.
"""
vehicle = None
# Check if ego position overlaps with surrounding vehicles
overlap = False
for idx, poly in self.vehicle_polygons[-1].items():
poly_center = np.mean(poly, axis=0)
ego_center = np.array([transform.location.x, transform.location.y])
dis = np.linalg.norm(poly_center - ego_center)
if dis > 8:
continue
else:
overlap = True
print('overlapping vehicle when trying to spawn')
break
if not overlap:
vehicle = self.world.try_spawn_actor(self.ego_bp, transform)
if vehicle is not None:
self.ego = vehicle
# batch = []
# batch.append(carla.command.SetAutopilot(self.ego,False))
# self.client.apply_batch_sync(batch)
# self.tm.vehicle_percentage_speed_difference(self.ego,-30)
#self.tm.distance_to_leading_vehicle(self.ego,20.0)
#self.tm.set_global_distance_to_leading_vehicle(0.0)
# self.tm.auto_lane_change(self.ego,False)
return True
print('could not spawn vehicle')
return False
def _get_actor_polygons(self, filt):
"""Get the bounding box polygon of actors.
Args:
filt: the filter indicating what type of actors we'll look at.
Returns:
actor_poly_dict: a dictionary containing the bounding boxes of specific actors.
"""
actor_poly_dict = {}
for actor in self.world.get_actors().filter(filt):
# Get x, y and yaw of the actor
trans = actor.get_transform()
x = trans.location.x
y = trans.location.y
yaw = trans.rotation.yaw / 180 * np.pi
# Get length and width
bb = actor.bounding_box
l = bb.extent.x
w = bb.extent.y
# Get bounding box polygon in the actor's local coordinate
poly_local = np.array([[l, w], [l, -w], [-l, -w],
[-l, w]]).transpose()
# Get rotation matrix to transform to global coordinate
R = np.array([[np.cos(yaw), -np.sin(yaw)],
[np.sin(yaw), np.cos(yaw)]])
# Get global bounding box polygon
poly = np.matmul(R, poly_local).transpose() + np.repeat(
[[x, y]], 4, axis=0)
actor_poly_dict[actor.id] = poly
return actor_poly_dict
def _get_obs(self):
"""Get the observations."""
## Birdeye rendering
self.birdeye_render.vehicle_polygons = self.vehicle_polygons
self.birdeye_render.walker_polygons = self.walker_polygons
self.birdeye_render.waypoints = self.waypoints
# birdeye view with roadmap and actors
birdeye_render_types = ['roadmap', 'actors']
if self.display_route:
birdeye_render_types.append('waypoints')
self.birdeye_render.render(self.display, birdeye_render_types)
birdeye = pygame.surfarray.array3d(self.display)
birdeye = birdeye[0:self.display_size, :, :]
birdeye = display_to_rgb(birdeye, self.obs_size)
# State observation
ego_trans = self.ego.get_transform()
ego_x = ego_trans.location.x
ego_y = ego_trans.location.y
ego_yaw = ego_trans.rotation.yaw / 180 * np.pi
lateral_dis, w = get_preview_lane_dis(self.waypoints, ego_x, ego_y)
delta_yaw = np.arcsin(
np.cross(w, np.array(np.array([np.cos(ego_yaw),
np.sin(ego_yaw)]))))
v = self.ego.get_velocity()
speed = np.sqrt(v.x**2 + v.y**2)
lead_vehicle = None
if len(self.vehicle_hazards) > 0:
dist_to_lead = 100
for i in range(len(self.vehicle_hazards)):
hazard_loc = self.vehicle_hazards[i].get_transform()
dist_to_lead_ = ((hazard_loc.location.x - ego_x)**2 +
(hazard_loc.location.y - ego_y)**2)**(1 / 2)
if dist_to_lead_ < dist_to_lead:
dist_to_lead = dist_to_lead_
lead_vehicle = self.vehicle_hazards[i]
else:
dist_to_lead = -1
self.dist_to_lead = dist_to_lead
if lead_vehicle is not None:
lead_v = lead_vehicle.get_velocity()
lead_speed = np.sqrt(lead_v.x**2 + lead_v.y**2)
else:
lead_speed = -1
state = np.array([speed, lead_speed, dist_to_lead])
## Get leading vehicle info
## Display camera image
if self.use_rgb_camera:
camera = resize(self.camera_img,
(self.obs_size, self.obs_size)) * 255
camera_surface = rgb_to_display_surface(camera, self.display_size)
self.display.blit(camera_surface, (self.display_size * 2, 0))
else:
camera = np.zeros((self.obs_size, self.obs_size, 3),
dtype=np.uint8)
## Display info statistics
self.display.blit(
self.pyfont.render('speed ' + str(round(speed, 1)) + ' m/s', True,
(255, 0, 0)), (self.display_size * 1, 80))
self.display.blit(
self.pyfont.render(
'lead_dist ' + str(round(dist_to_lead, 1)) + ' m', True,
(255, 255, 0)), (self.display_size * 1, 120))
self.display.blit(
self.pyfont.render('lead_v ' + str(round(lead_speed, 1)) + ' m/s',
True, (255, 255, 0)),
(self.display_size * 1, 100))
# Display on pygame
pygame.display.flip()
return state
def _get_reward(self):
"""Calculate the step reward."""
# reward for speed tracking
v = self.ego.get_velocity()
speed = np.sqrt(v.x**2 + v.y**2)
r_speed = -abs(speed - self.desired_speed)
# reward for collision
r_collision = 0
if len(self.collision_hist) > 0:
r_collision = -1
# reward for steering:
# reward for out of lane
ego_x, ego_y = get_pos(self.ego)
dis, w = get_lane_dis(self.waypoints, ego_x, ego_y)
r_out = 0
# if abs(dis) > self.out_lane_thres:
# r_out = -1
# longitudinal speed
lspeed = np.array([v.x, v.y])
lspeed_lon = np.dot(lspeed, w)
# cost for too fast
r_fast = 0
if lspeed_lon > self.desired_speed:
r_fast = -1
# cost for too slow
# cost for too fast
r_slow = 0
if lspeed_lon < self.desired_speed:
r_slow = -1
# cost for lateral acceleration
r_lat = -abs(self.ego.get_control().steer) * lspeed_lon**2
# cost for idling
r_idle = -1 * self.idle_timesteps
# cost for too close following distance:
r_space = 0
if self.dist_to_lead > 0 and self.dist_to_lead <= 35.0:
r_space = -(45.0 - self.dist_to_lead) * 15
r = 200 * r_collision + 1 * lspeed_lon + 10 * r_fast + 5 * r_slow + r_idle + 12 * r_speed + r_space
print(
'reward [collision %.2f] [distance %.2f] [overspeed %.2f] [underspeed %.2f] [idle %f] [speed mismatch %.2f] [too close %.2f]'
% (200 * r_collision, 1 * lspeed_lon, 10 * r_fast, 5 * r_slow,
r_idle, 12 * r_speed, r_space))
return r
def _terminal(self):
"""Calculate whether to terminate the current episode."""
# Get ego state
ego_x, ego_y = get_pos(self.ego)
# If collides
if len(self.collision_hist) > 0:
return True
# If reach maximum timestep
if self.time_step > self.max_time_episode:
return True
# If at destination
if self.dests is not None: # If at destination
for dest in self.dests:
if np.sqrt((ego_x - dest[0])**2 + (ego_y - dest[1])**2) < 4:
return True
# If out of lane
dis, _ = get_lane_dis(self.waypoints, ego_x, ego_y)
if abs(dis) > self.out_lane_thres:
return True
# if stopped for a vehicle ahead
# TODO
if self.idle_timesteps > 500:
print('terminal due to too many idle timesteps')
return True
return False
def _clear_all_actors(self, actor_filters):
"""Clear specific actors."""
for actor_filter in actor_filters:
for actor in self.world.get_actors().filter(actor_filter):
if actor.is_alive:
if actor.type_id == 'controller.ai.walker':
actor.stop()
actor.destroy()
def _clear_all_sensors(self):
try:
self.collision_sensor.destroy()
self.camera_sensor.destroy()
except:
pass