def reset(self, initial_distance, initial_speed):
if self.gui and self.display:
self.display.stop()
# Clean all vehicles before reset the environment
for i, _ in enumerate(self.actor):
if self.actor[i] is not None:
self.actor[i].destroy()
self.actor[i] = None
self.actor = []
self.spawn_vehicles()
self.dist_calc = DistanceCalculation(self.ego_vehicle, self.leading_vehicle, self.perception)
self.pid_controller = PID(P=3.0, I=0.0003, D=0.0)
if self.gui:
self.display = pygameViewer()
if self.collect["option"] != 0:
self.collect_data = collectData(os.path.join(self.collect["path"], "episode_" + str(self.episode)), self.perception)
self.step_count = 0
self.weather = DynamicPrecipitation(initial_precipitation=round(np.random.uniform(0.0,20.0), 2))
self.world.set_weather(self.weather.get_weather_parameters())
S0 = self.reset_episode(initial_distance, initial_speed)
self.episode_count += 1
return S0
def reset(self, initial_distance, initial_speed, patch_location,
friction_value, current_actor):
#def reset(self, initial_distance, initial_speed):
self.kickspd = initial_speed
if self.gui and self.display:
self.display.stop()
# Clean all vehicles before reset the environment
for i, _ in enumerate(self.actor):
if self.actor[i] is not None:
self.actor[i].destroy()
self.actor[i] = None
self.actor = []
self.spawn_vehicles(initial_speed)
self.spawn_friction(patch_location, friction_value)
self.dist_calc = DistanceCalculation(self.ego_vehicle,
self.leading_vehicle,
self.perception)
self.rewards = reward_calc(a=1.0, d=1.0, base=1.9)
self.pid_controller = PID(P=1, I=0.0003, D=0.0)
if self.gui:
self.display = pygameViewer()
if self.collect["option"] != 0 and self.flag != current_actor:
self.episode = 0
self.flag = current_actor
self.collect_data = collectData(self.collect["path"],
self.perception, current_actor)
self.step_count = 0
self.weather = DynamicPrecipitation(
initial_precipitation=round(np.random.uniform(0.0, 20.0), 2))
self.world.set_weather(self.weather.get_weather_parameters())
S0 = self.reset_episode(initial_distance, initial_speed)
return S0
class SetupWorld():
def __init__(self,
host='127.0.0.1',
port=2002,
town=1,
gui=False,
collect={
"option": 0,
"path": None
},
perception=None):
self.episode = 0
self.velocity_bfr_kicking_nn = 0.0
self.flag = 0
self.kickspd = 0.0
self.crossing_velocity = 0.0
self.location_of_patch = 0.0
self.friction_of_patch = 0.0
self.collect = collect
self.perception = perception
self.gui = gui
# try to import carla python API
self.client = carla.Client(host, port)
self.client.set_timeout(30.0)
self.world = self.client.load_world('Town0{}'.format(town))
settings = self.world.get_settings()
settings.fixed_delta_seconds = 0.05
settings.synchronous_mode = True
self.world.apply_settings(settings)
self.actor = []
self.display = None
def reset(self, initial_distance, initial_speed, patch_location,
friction_value):
#def reset(self, initial_distance, initial_speed):
self.kickspd = initial_speed
if self.gui and self.display:
self.display.stop()
# Clean all vehicles before reset the environment
for i, _ in enumerate(self.actor):
if self.actor[i] is not None:
self.actor[i].destroy()
self.actor[i] = None
self.actor = []
self.spawn_vehicles(initial_speed)
self.spawn_friction(patch_location, friction_value)
self.dist_calc = DistanceCalculation(self.ego_vehicle,
self.leading_vehicle,
self.perception)
self.rewards = reward_calc(a=1.0, d=1.0, base=1.9)
self.pid_controller = PID(P=1, I=0.0003, D=0.0)
if self.gui:
self.display = pygameViewer()
if self.collect["option"] != 0 and self.flag == 0:
self.flag = 1
self.collect_data = collectData(self.collect["path"],
self.perception)
self.step_count = 0
self.weather = DynamicPrecipitation(
initial_precipitation=round(np.random.uniform(0.0, 20.0), 2))
self.world.set_weather(self.weather.get_weather_parameters())
S0 = self.reset_episode(initial_distance, initial_speed)
return S0
def reset_episode(self, initial_distance, initial_speed):
while True:
weather_parameter = self.weather.get_weather_parameters()
self.world.set_weather(weather_parameter)
self.world.tick()
image = self.image_queue.get()
if not self.collision_queue.empty():
_ = self.collision_queue.get()
print("collision occurred during reset...")
distance = self.dist_calc.getTrueDistance()
velocity = self.ego_vehicle.get_velocity().y
vehicle_stop = velocity <= 0.0
if vehicle_stop == True:
break
if self.gui:
self.display.updateViewer(image)
if (distance <= initial_distance):
#print('Velocity before kicking NN:',velocity)
self.velocity_bfr_kicking_nn = velocity
break
regression_distance = 0
if self.perception:
regression_distance = self.dist_calc.getRegressionDistance(image)
if self.perception:
distance = regression_distance
return [distance, velocity, vehicle_stop]
def spawn_friction(self, location, set_friction):
self.friction_bp = self.world.get_blueprint_library().find(
'static.trigger.friction')
extent = carla.Location(400, 200, 0.001)
self.friction_bp.set_attribute('friction', str(set_friction))
self.friction_bp.set_attribute('extent_x', str(extent.x))
self.friction_bp.set_attribute('extent_y', str(extent.y))
self.friction_bp.set_attribute('extent_z', str(extent.z))
# Spawn Trigger Friction
transform = carla.Transform()
transform.location = carla.Location(x=392.1, y=(217 + location), z=0.0)
self.friction = self.world.spawn_actor(self.friction_bp, transform)
self.location_of_patch = location
self.friction_of_patch = set_friction
# Optional for visualizing trigger
self.world.debug.draw_box(box=carla.BoundingBox(transform.location, extent), rotation=transform.rotation, \
life_time=15, thickness=0.05, color=carla.Color(r=255,g=0,b=0))
self.actor.append(self.friction)
def spawn_vehicles(self, initial_speed):
self.bp_lib = self.world.get_blueprint_library()
ego_bp = self.bp_lib.filter('vehicle.tesla.model3')[0]
spawn_point = Transform(Location(x=392.1, y=217.0, z=0.02),
Rotation(yaw=89.6))
self.ego_vehicle = self.world.spawn_actor(ego_bp, spawn_point)
self.setup_sensors(self.ego_vehicle)
self.actor.append(self.ego_vehicle)
self.ego_vehicle.get_world()
self.ego_vehicle.set_velocity(carla.Vector3D(0.0, initial_speed, 0.0))
leading_bp = self.bp_lib.filter('vehicle.audi.a2')[0]
#leading_bp = self.bp_lib.filter('vehicle.audi.etron')[0]
#leading_bp = self.bp_lib.filter('vehicle.audi.tt')[0]
#leading_bp = random.choice(self.bp_lib.filter('vehicle.audi.*'))
spawn_point_leading = Transform(Location(x=392.1, y=320.0, z=0.0),
Rotation(yaw=90))
self.leading_vehicle = self.world.try_spawn_actor(
leading_bp, spawn_point_leading)
self.leading_vehicle.get_world()
self.actor.append(self.leading_vehicle)
def setup_sensors(self, ego_vehicle):
camera_bp = self.bp_lib.find('sensor.camera.rgb')
camera_bp.set_attribute('image_size_x', '800')
camera_bp.set_attribute('image_size_y', '600')
camera_bp.set_attribute('fov', '100')
self.camera = self.world.try_spawn_actor(camera_bp,
Transform(
Location(x=0.8,
y=0.0,
z=1.7),
Rotation(yaw=0.0)),
attach_to=ego_vehicle)
self.actor.append(self.camera)
collision_bp = self.bp_lib.find('sensor.other.collision')
self.collision = self.world.try_spawn_actor(collision_bp,
Transform(),
attach_to=self.ego_vehicle)
self.actor.append(self.collision)
self.image_queue = queue.Queue()
self.camera.listen(self.image_queue.put)
self.collision_queue = queue.Queue()
self.collision.listen(self.collision_queue.put)
def step(self, action):
self.step_count += 1
weather_parameter = self.weather.get_weather_parameters(
step=self.step_count)
self.world.set_weather(weather_parameter)
control = carla.VehicleControl(throttle=0.0, brake=action)
self.ego_vehicle.apply_control(control)
self.world.tick()
image = self.image_queue.get()
if not self.collision_queue.empty():
collision = self.collision_queue.get().normal_impulse
isCollision = True
else:
isCollision = False
groundtruth_distance = self.dist_calc.getTrueDistance()
if groundtruth_distance > 10 and groundtruth_distance < 15:
self.crossing_velocity = self.ego_vehicle.get_velocity().y
regression_distance = 0
distance = groundtruth_distance
if groundtruth_distance < 110.0 and self.perception is not None:
regression_distance = self.dist_calc.getRegressionDistance(image)
distance = regression_distance
#print("Groundtruth distance: {}, Regression distance: {}, Error: {}".format(groundtruth_distance, regression_distance, abs(regression_distance-groundtruth_distance)))
velocity = self.ego_vehicle.get_velocity().y
if self.gui:
self.display.updateViewer(image)
isStop = velocity <= 0.05
done = isStop or isCollision
self.image = image
if done:
if (isCollision):
reward = self.rewards.reward_total(groundtruth_distance,
self.crossing_velocity)
print(
"===> Collision & Episode: {},KickSpd:{},NN_spd: {},CRS_spd: {}, Reward: {}, Stop_Dist: {}"
.format(self.episode, self.kickspd,
self.velocity_bfr_kicking_nn,
self.crossing_velocity, reward,
groundtruth_distance))
elif (isStop):
reward = self.rewards.reward_total(groundtruth_distance,
self.crossing_velocity)
print(
"====> Stopped & Episode: {},,KickSpd:{},NN_spd: {}, CRS_spd: {},Reward: {}, Stop_Dist: {}"
.format(self.episode, self.kickspd,
self.velocity_bfr_kicking_nn,
self.crossing_velocity, reward,
groundtruth_distance))
if self.collect["option"] != 0:
self.collect_data(self.episode, self.kickspd,
self.velocity_bfr_kicking_nn,
self.crossing_velocity,
self.location_of_patch,
self.friction_of_patch, reward,
groundtruth_distance)
self.episode += 1
else:
reward = 0
return [[distance, velocity], reward, done]
def get_image(self):
img = np.frombuffer(self.image.raw_data, dtype=np.dtype("uint8"))
img = np.reshape(img, (self.image.height, self.image.width, 4))
img = img[:, :, :3]
img = cv2.resize(img, (224, 224))
img = np.asarray(img) / 255.
return img
def closefile(self):
for i, _ in enumerate(self.actor):
if self.actor[i] is not None:
self.actor[i].destroy()
self.collect_data.close_csv()
class SetupWorld():
def __init__(self, host='127.0.0.1', port=2000, town=1, gui=False, collect={"option":0, "path": None}, perception=None):
self.episode = 0
self.collect = collect
self.perception = perception
self.gui = gui
# try to import carla python API
self.client = carla.Client(host, port)
self.client.set_timeout(10.0)
self.world = self.client.load_world('Town0{}'.format(town))
self.episode_count = 0
settings = self.world.get_settings()
settings.fixed_delta_seconds = 0.05
settings.synchronous_mode = True
self.world.apply_settings(settings)
self.actor = []
self.display = None
def reset(self, initial_distance, initial_speed):
if self.gui and self.display:
self.display.stop()
# Clean all vehicles before reset the environment
for i, _ in enumerate(self.actor):
if self.actor[i] is not None:
self.actor[i].destroy()
self.actor[i] = None
self.actor = []
self.spawn_vehicles()
self.dist_calc = DistanceCalculation(self.ego_vehicle, self.leading_vehicle, self.perception)
self.pid_controller = PID(P=3.0, I=0.0003, D=0.0)
if self.gui:
self.display = pygameViewer()
if self.collect["option"] != 0:
self.collect_data = collectData(os.path.join(self.collect["path"], "episode_" + str(self.episode)), self.perception)
self.step_count = 0
self.weather = DynamicPrecipitation(initial_precipitation=round(np.random.uniform(0.0,20.0), 2))
self.world.set_weather(self.weather.get_weather_parameters())
S0 = self.reset_episode(initial_distance, initial_speed)
self.episode_count += 1
return S0
def reset_episode(self, initial_distance, initial_speed):
velocity = 0.0
while True:
weather_parameter = self.weather.get_weather_parameters()
self.world.set_weather(weather_parameter)
action = self.pid_controller.update(initial_speed, velocity)
control = carla.VehicleControl(
throttle = action + 0.4,
brake = 0.0,
steer = 0.0
)
self.ego_vehicle.apply_control(control)
self.world.tick()
image = self.image_queue.get()
if not self.collision_queue.empty():
_ = self.collision_queue.get()
print("collision occurred during reset...")
distance = self.dist_calc.getTrueDistance()
velocity = self.ego_vehicle.get_velocity().y
if self.gui:
self.display.updateViewer(image)
if (distance<=initial_distance):
break
if self.collect["option"] == 1 and distance <= 110:
self.collect_data(image, distance, velocity, -1, weather_parameter.precipitation, self.step_count)
# collect the data for s0
regression_distance = 0
if self.perception:
regression_distance = self.dist_calc.getRegressionDistance(image)
if self.collect["option"] != 0:
self.collect_data(image, distance, velocity, -0.1, weather_parameter.precipitation, self.step_count, regression_distance)
if self.perception:
distance = regression_distance
return [distance, velocity]
def spawn_vehicles(self):
self.bp_lib = self.world.get_blueprint_library()
ego_bp = self.bp_lib.filter('vehicle.tesla.model3')[0]
spawn_point = Transform(Location(x=392.1, y=10.0, z=0.02), Rotation(yaw=89.6))
self.ego_vehicle = self.world.spawn_actor(ego_bp, spawn_point)
self.setup_sensors(self.ego_vehicle)
self.actor.append(self.ego_vehicle)
self.ego_vehicle.get_world()
leading_bp = self.bp_lib.filter('vehicle.audi.a2')[0]
#leading_bp = self.bp_lib.filter('vehicle.audi.etron')[0]
#leading_bp = self.bp_lib.filter('vehicle.audi.tt')[0]
#leading_bp = random.choice(self.bp_lib.filter('vehicle.audi.*'))
spawn_point_leading = Transform(Location(x=392.1, y=320.0, z=0.0), Rotation(yaw=90))
self.leading_vehicle = self.world.try_spawn_actor(leading_bp, spawn_point_leading)
self.leading_vehicle.get_world()
self.actor.append(self.leading_vehicle)
def setup_sensors(self, ego_vehicle):
camera_bp = self.bp_lib.find('sensor.camera.rgb')
camera_bp.set_attribute('image_size_x', '800')
camera_bp.set_attribute('image_size_y', '600')
camera_bp.set_attribute('fov', '100')
self.camera = self.world.try_spawn_actor(
camera_bp,
Transform(Location(x=0.8,y=0.0,z=1.7), Rotation(yaw=0.0)),
attach_to=ego_vehicle)
self.actor.append(self.camera)
collision_bp = self.bp_lib.find('sensor.other.collision')
self.collision = self.world.try_spawn_actor(
collision_bp,
Transform(),
attach_to=self.ego_vehicle)
self.actor.append(self.collision)
self.image_queue = queue.Queue()
self.camera.listen(self.image_queue.put)
self.collision_queue = queue.Queue()
self.collision.listen(self.collision_queue.put)
def step(self, action):
self.step_count += 1
weather_parameter = self.weather.get_weather_parameters(step=self.step_count)
self.world.set_weather(weather_parameter)
control=carla.VehicleControl(
throttle = 0.0,
brake = action
)
self.ego_vehicle.apply_control(control)
self.world.tick()
image = self.image_queue.get()
if not self.collision_queue.empty():
collision = self.collision_queue.get().normal_impulse
isCollision =True
else:
isCollision = False
groundtruth_distance = self.dist_calc.getTrueDistance()
regression_distance = 0
distance = groundtruth_distance
if groundtruth_distance < 110.0 and self.perception is not None:
regression_distance = self.dist_calc.getRegressionDistance(image)
distance = regression_distance
print("Groundtruth distance: {}, Regression distance: {}, Error: {}".format(groundtruth_distance, regression_distance, abs(regression_distance-groundtruth_distance)))
velocity = self.ego_vehicle.get_velocity().y
if self.collect["option"] != 0:
self.collect_data(image, groundtruth_distance, velocity, action, weather_parameter.precipitation, self.step_count, regression_distance)
if self.gui:
self.display.updateViewer(image)
isStop = velocity <= 0.0
done = isStop or isCollision
self.image = image
if done:
if (isCollision):
#reward = -math.sqrt(collision.x**2+collision.y**2+collision.z**2)/100.0
reward = -200.0 - math.sqrt(collision.x**2+collision.y**2+collision.z**2)/100.0
print("Collision: {}".format(reward))
elif (isStop):
#too_far_reward = -(distance>5.0)*(distance-5)#-((distance-5)/250.0*400 + 20) * (distance>5.0)
too_far_reward = -((groundtruth_distance-3.0)/120.0*400+30) * (groundtruth_distance>3.0)
#too_close_reward = 0.0#-(20.0)*(distance<1.0)
too_close_reward = -(40.0)*(groundtruth_distance<1.0)
reward = too_far_reward + too_close_reward
print("Stop: {}, Distance: {}".format(reward, groundtruth_distance))
self.episode+=1
else:
reward = 0
return [[distance, velocity], reward, done]
def get_image(self):
img = np.frombuffer(self.image.raw_data, dtype=np.dtype("uint8"))
img = np.reshape(img, (self.image.height, self.image.width, 4))
img = img[:, :, :3]
img = cv2.resize(img, (224,224))
img = np.asarray(img) / 255.
return img