def __update_spectactor_pose(self):
# Set the world simulation view with respect to the vehicle.
v_pose = self._ego_vehicle.get_transform()
v_pose.location -= 10 * carla.Location(v_pose.get_forward_vector())
v_pose.location.z = 5
self._spectator.set_transform(v_pose)
def set_destination(self, location):
start_waypoint = self._map.get_waypoint(self._vehicle.get_location())
end_waypoint = self._map.get_waypoint(
carla.Location(location[0], location[1], location[2]))
solution = []
# Setting up global router
dao = GlobalRoutePlannerDAO(self._vehicle.get_world().get_map())
grp = GlobalRoutePlanner(dao)
grp.setup()
# Obtain route plan
x1 = start_waypoint.transform.location.x
y1 = start_waypoint.transform.location.y
x2 = end_waypoint.transform.location.x
y2 = end_waypoint.transform.location.y
route = grp.plan_route((x1, y1), (x2, y2))
current_waypoint = start_waypoint
route.append(RoadOption.VOID)
for action in route:
# Generate waypoints to next junction
wp_choice = current_waypoint.next(self._hop_resolution)
while len(wp_choice) == 1:
current_waypoint = wp_choice[0]
solution.append((current_waypoint, RoadOption.LANEFOLLOW))
wp_choice = current_waypoint.next(self._hop_resolution)
# Stop at destination
if current_waypoint.transform.location.distance(
end_waypoint.transform.location) < self._hop_resolution: break
if action == RoadOption.VOID: break
# Select appropriate path at the junction
if len(wp_choice) > 1:
# Current heading vector
current_transform = current_waypoint.transform
current_location = current_transform.location
projected_location = current_location + \
carla.Location(
x=math.cos(math.radians(current_transform.rotation.yaw)),
y=math.sin(math.radians(current_transform.rotation.yaw)))
v_current = vector(current_location, projected_location)
direction = 0
if action == RoadOption.LEFT:
direction = 1
elif action == RoadOption.RIGHT:
direction = -1
elif action == RoadOption.STRAIGHT:
direction = 0
select_criteria = float('inf')
# Choose correct path
# Choose the wp_select waypoint whose direction is the most similar to the action direction.
for wp_select in wp_choice:
v_select = vector(
current_location, wp_select.transform.location)
cross = float('inf')
if direction == 0:
cross = abs(np.cross(v_current, v_select)[-1])
else:
cross = direction*np.cross(v_current, v_select)[-1]
if cross < select_criteria:
select_criteria = cross
current_waypoint = wp_select
# Generate all waypoints within the junction
# along selected path
solution.append((current_waypoint, action))
current_waypoint = current_waypoint.next(self._hop_resolution)[0]
while current_waypoint.is_intersection:
solution.append((current_waypoint, action))
current_waypoint = current_waypoint.next(self._hop_resolution)[0]
assert solution
self._current_plan = solution
self._local_planner.set_global_plan(self._current_plan)
vehicle.apply_control(carla.VehicleControl(throttle=0.3, steer=0.0))
vehicle.set_autopilot(False) # if you just wanted some NPCs to drive.
actor_list.append(vehicle)
# https://carla.readthedocs.io/en/latest/cameras_and_sensors
# get the blueprint for this sensor
blueprint = blueprint_library.find('sensor.camera.rgb')
# change the dimensions of the image
blueprint.set_attribute('image_size_x', f'{IM_WIDTH}')
blueprint.set_attribute('image_size_y', f'{IM_HEIGHT}')
blueprint.set_attribute('fov', '110')
#blueprint.set_attribute('sensor_tick', '0.2')
# Adjust sensor relative to vehicle
spawn_point = carla.Transform(carla.Location(x=2.5, z=0.7))
# spawn the sensor and attach to vehicle.
sensor = world.spawn_actor(blueprint, spawn_point, attach_to=vehicle)
# add sensor to list of actors
actor_list.append(sensor)
# Init display
pygame.init()
display_surface = pygame.display.set_mode(
(1280, 960), pygame.HWSURFACE | pygame.DOUBLEBUF)
pygame.display.set_caption('CARLA image')
# do something with this sensor
sensor.listen(lambda data: frame_process(data))
def get_transform(vehicle_location, angle, d=6.4):
a = math.radians(angle)
location = carla.Location(d * math.cos(a), d * math.sin(a),
2.0) + vehicle_location
return carla.Transform(location, carla.Rotation(yaw=180 + angle,
pitch=-15))
def __init__(self, parent_actor, hud, gamma_correction):
self.sensor = None
self.surface = None
self._parent = parent_actor
self.hud = hud
self.recording = False
bound_y = 0.5 + self._parent.bounding_box.extent.y
Attachment = carla.AttachmentType
self._camera_transforms = [
(
carla.Transform(
carla.Location(x=-16.0, z=2.5), carla.Rotation(pitch=8.0)
),
Attachment.SpringArm,
),
(carla.Transform(carla.Location(x=1.6, z=1.7)), Attachment.Rigid),
(
carla.Transform(carla.Location(x=5.5, y=1.5, z=1.5)),
Attachment.SpringArm,
),
(
carla.Transform(
carla.Location(x=-8.0, z=6.0), carla.Rotation(pitch=6.0)
),
Attachment.SpringArm,
),
(
carla.Transform(carla.Location(x=-1, y=-bound_y, z=0.5)),
Attachment.Rigid,
),
]
self.transform_index = 1
self.sensors = [
["sensor.camera.rgb", cc.Raw, "Camera RGB"],
["sensor.camera.depth", cc.Raw, "Camera Depth (Raw)"],
["sensor.camera.depth", cc.Depth, "Camera Depth (Gray Scale)"],
[
"sensor.camera.depth",
cc.LogarithmicDepth,
"Camera Depth (Logarithmic Gray Scale)",
],
[
"sensor.camera.semantic_segmentation",
cc.Raw,
"Camera Semantic Segmentation (Raw)",
],
[
"sensor.camera.semantic_segmentation",
cc.CityScapesPalette,
"Camera Semantic Segmentation (CityScapes Palette)",
],
["sensor.lidar.ray_cast", None, "Lidar (Ray-Cast)"],
]
world = self._parent.get_world()
bp_library = world.get_blueprint_library()
for item in self.sensors:
bp = bp_library.find(item[0])
if item[0].startswith("sensor.camera"):
bp.set_attribute("image_size_x", str(hud.dim[0]))
bp.set_attribute("image_size_y", str(hud.dim[1]))
if bp.has_attribute("gamma"):
bp.set_attribute("gamma", str(gamma_correction))
elif item[0].startswith("sensor.lidar"):
bp.set_attribute("range", "5000")
item.append(bp)
self.index = None
def main():
actor_list = []
try:
# Connect to Carla server
client = carla.Client('localhost', 2000)
client.set_timeout(10.0)
world = client.load_world('Town01')
settings = world.get_settings()
settings.synchronous_mode = True
# settings.no_rendering_mode = True
world.apply_settings(settings)
blueprint_library = world.get_blueprint_library()
bp = blueprint_library.find('vehicle.tesla.model3')
colors = bp.get_attribute('color').recommended_values
bp.set_attribute('color', '140,0,0')
transform = random.choice(world.get_map().get_spawn_points())
# print(transform.location)
# print(transform.rotation)
vehicle = world.spawn_actor(bp, transform)
actor_list.append(vehicle)
print('created %s' % vehicle.type_id)
camera_bp = blueprint_library.find("sensor.camera.rgb")
camera_bp.set_attribute('image_size_x', "%s" % str(imageSize[1]))
camera_bp.set_attribute('image_size_y', "%s" % str(imageSize[0]))
camera_bp.set_attribute('fov', "%s" % str(100.0))
camera_transform = carla.Transform(carla.Location(x=2.0, z=1.4),
carla.Rotation(-15.0, 0, 0))
camera = world.spawn_actor(camera_bp,
camera_transform,
attach_to=vehicle)
actor_list.append(camera)
print('created %s' % camera.type_id)
image_queue = queue.Queue()
camera.listen(image_queue.put)
# TURN ON AUTOPILOT FOR TESTING
# vehicle.set_autopilot(True)
# print("VEHICLE DIR")
# print(dir(vehicle))
# INITIALISE CIL MODEL
agent = ImitationLearningAgent(vehicle=vehicle,
city_name='Town01',
avoid_stopping=True)
frame = None
display = pygame.display.set_mode((imageSize[1], imageSize[0]),
pygame.HWSURFACE | pygame.DOUBLEBUF)
# font = get_font()
clock = pygame.time.Clock()
while True:
if should_quit():
return
clock.tick()
world.tick()
ts = world.wait_for_tick()
if frame is not None:
if ts.frame_count != frame + 1:
logging.warning('frame skip!')
frame = ts.frame_count
while True:
image = image_queue.get()
if image.frame_number == ts.frame_count:
break
im = np.array(image.raw_data).reshape(imageSize)[:, :, :3]
speed_vec = vehicle.get_velocity()
forward_speed = np.sqrt(speed_vec.x**2 + speed_vec.y**2 +
speed_vec.z**2)
# RUN MODEL
# control = agent.run_step(measurements, image, directions=2, target=None) # 2: follow straight
control = agent._compute_action(im, forward_speed, direction=2)
vehicle.apply_control(control)
showImage(display, image)
pygame.display.flip()
except KeyboardInterrupt:
pass
finally:
print('\nDisabling synchronous mode...')
settings = world.get_settings()
settings.synchronous_mode = False
world.apply_settings(settings)
print("Destroying actors...")
for actor in actor_list:
actor.destroy()
print("Done.")
def tick_scenario(self):
spectator = self._world.get_spectator()
spectator.set_transform(
carla.Transform(self._player.get_location() + carla.Location(z=50),
carla.Rotation(pitch=-90)))
def visualize_enu_edge(self, edge, color, z_offset):
for point in edge:
carla_point = carla.Location(x=float(point.x),
y=float(-1 * point.y),
z=float(point.z + z_offset))
self._world.debug.draw_point(carla_point, 0.1, color, 0.1, False)
def go(q):
# setup CARLA
client = carla.Client("127.0.0.1", 2000)
client.set_timeout(10.0)
world = client.load_world(args.town)
blueprint_library = world.get_blueprint_library()
world_map = world.get_map()
vehicle_bp = blueprint_library.filter('vehicle.tesla.*')[1]
spawn_points = world_map.get_spawn_points()
assert len(spawn_points) > args.num_selected_spawn_point, \
f'''No spawn point {args.num_selected_spawn_point}, try a value between 0 and
{len(spawn_points)} for this town.'''
spawn_point = spawn_points[args.num_selected_spawn_point]
vehicle = world.spawn_actor(vehicle_bp, spawn_point)
max_steer_angle = vehicle.get_physics_control().wheels[0].max_steer_angle
# make tires less slippery
# wheel_control = carla.WheelPhysicsControl(tire_friction=5)
physics_control = vehicle.get_physics_control()
physics_control.mass = 2326
# physics_control.wheels = [wheel_control]*4
physics_control.torque_curve = [[20.0, 500.0], [5000.0, 500.0]]
physics_control.gear_switch_time = 0.0
vehicle.apply_physics_control(physics_control)
blueprint = blueprint_library.find('sensor.camera.rgb')
blueprint.set_attribute('image_size_x', str(W))
blueprint.set_attribute('image_size_y', str(H))
blueprint.set_attribute('fov', '70')
blueprint.set_attribute('sensor_tick', '0.05')
transform = carla.Transform(carla.Location(x=0.8, z=1.13))
camera = world.spawn_actor(blueprint, transform, attach_to=vehicle)
camera.listen(cam_callback)
world.set_weather(
carla.WeatherParameters(
cloudiness=args.cloudiness,
precipitation=args.precipitation,
precipitation_deposits=args.precipitation_deposits,
wind_intensity=args.wind_intensity,
sun_azimuth_angle=args.sun_azimuth_angle,
sun_altitude_angle=args.sun_altitude_angle))
# reenable IMU
imu_bp = blueprint_library.find('sensor.other.imu')
imu = world.spawn_actor(imu_bp, transform, attach_to=vehicle)
imu.listen(imu_callback)
def destroy():
print("clean exit")
imu.destroy()
camera.destroy()
vehicle.destroy()
print("done")
atexit.register(destroy)
vehicle_state = VehicleState()
# launch fake car threads
threading.Thread(target=panda_state_function).start()
threading.Thread(target=fake_driver_monitoring).start()
threading.Thread(target=fake_gps).start()
threading.Thread(target=can_function_runner,
args=(vehicle_state, )).start()
# can loop
rk = Ratekeeper(100, print_delay_threshold=0.05)
# init
throttle_ease_out_counter = REPEAT_COUNTER
brake_ease_out_counter = REPEAT_COUNTER
steer_ease_out_counter = REPEAT_COUNTER
vc = carla.VehicleControl(throttle=0, steer=0, brake=0, reverse=False)
is_openpilot_engaged = False
throttle_out = steer_out = brake_out = 0
throttle_op = steer_op = brake_op = 0
throttle_manual = steer_manual = brake_manual = 0
old_steer = old_brake = old_throttle = 0
throttle_manual_multiplier = 0.7 #keyboard signal is always 1
brake_manual_multiplier = 0.7 #keyboard signal is always 1
steer_manual_multiplier = 45 * STEER_RATIO #keyboard signal is always 1
while 1:
# 1. Read the throttle, steer and brake from op or manual controls
# 2. Set instructions in Carla
# 3. Send current carstate to op via can
cruise_button = 0
throttle_out = steer_out = brake_out = 0
throttle_op = steer_op = brake_op = 0
throttle_manual = steer_manual = brake_manual = 0
# --------------Step 1-------------------------------
if not q.empty():
message = q.get()
m = message.split('_')
if m[0] == "steer":
steer_manual = float(m[1])
is_openpilot_engaged = False
if m[0] == "throttle":
throttle_manual = float(m[1])
is_openpilot_engaged = False
if m[0] == "brake":
brake_manual = float(m[1])
is_openpilot_engaged = False
if m[0] == "reverse":
#in_reverse = not in_reverse
cruise_button = CruiseButtons.CANCEL
is_openpilot_engaged = False
if m[0] == "cruise":
if m[1] == "down":
cruise_button = CruiseButtons.DECEL_SET
is_openpilot_engaged = True
if m[1] == "up":
cruise_button = CruiseButtons.RES_ACCEL
is_openpilot_engaged = True
if m[1] == "cancel":
cruise_button = CruiseButtons.CANCEL
is_openpilot_engaged = False
throttle_out = throttle_manual * throttle_manual_multiplier
steer_out = steer_manual * steer_manual_multiplier
brake_out = brake_manual * brake_manual_multiplier
#steer_out = steer_out
# steer_out = steer_rate_limit(old_steer, steer_out)
old_steer = steer_out
old_throttle = throttle_out
old_brake = brake_out
# print('message',old_throttle, old_steer, old_brake)
if is_openpilot_engaged:
sm.update(0)
throttle_op = sm['carControl'].actuators.gas #[0,1]
brake_op = sm['carControl'].actuators.brake #[0,1]
steer_op = sm[
'controlsState'].steeringAngleDesiredDeg # degrees [-180,180]
throttle_out = throttle_op
steer_out = steer_op
brake_out = brake_op
steer_out = steer_rate_limit(old_steer, steer_out)
old_steer = steer_out
# OP Exit conditions
# if throttle_out > 0.3:
# cruise_button = CruiseButtons.CANCEL
# is_openpilot_engaged = False
# if brake_out > 0.3:
# cruise_button = CruiseButtons.CANCEL
# is_openpilot_engaged = False
# if steer_out > 0.3:
# cruise_button = CruiseButtons.CANCEL
# is_openpilot_engaged = False
else:
if throttle_out == 0 and old_throttle > 0:
if throttle_ease_out_counter > 0:
throttle_out = old_throttle
throttle_ease_out_counter += -1
else:
throttle_ease_out_counter = REPEAT_COUNTER
old_throttle = 0
if brake_out == 0 and old_brake > 0:
if brake_ease_out_counter > 0:
brake_out = old_brake
brake_ease_out_counter += -1
else:
brake_ease_out_counter = REPEAT_COUNTER
old_brake = 0
if steer_out == 0 and old_steer != 0:
if steer_ease_out_counter > 0:
steer_out = old_steer
steer_ease_out_counter += -1
else:
steer_ease_out_counter = REPEAT_COUNTER
old_steer = 0
# --------------Step 2-------------------------------
steer_carla = steer_out / (max_steer_angle * STEER_RATIO * -1)
steer_carla = np.clip(steer_carla, -1, 1)
steer_out = steer_carla * (max_steer_angle * STEER_RATIO * -1)
old_steer = steer_carla * (max_steer_angle * STEER_RATIO * -1)
vc.throttle = throttle_out / 0.6
vc.steer = steer_carla
vc.brake = brake_out
vehicle.apply_control(vc)
# --------------Step 3-------------------------------
vel = vehicle.get_velocity()
speed = math.sqrt(vel.x**2 + vel.y**2 + vel.z**2) # in m/s
vehicle_state.speed = speed
vehicle_state.angle = steer_out
vehicle_state.cruise_button = cruise_button
vehicle_state.is_engaged = is_openpilot_engaged
if rk.frame % PRINT_DECIMATION == 0:
print("frame: ", "engaged:", is_openpilot_engaged, "; throttle: ",
round(vc.throttle, 3), "; steer(c/deg): ",
round(vc.steer, 3), round(steer_out, 3), "; brake: ",
round(vc.brake, 3))
rk.keep_time()
def __create_segmentation_lidar_sensor(self):
return self.__world.spawn_actor(
create_semantic_lidar_blueprint(self.__world),
carla.Transform(carla.Location(z=self.Z_SENSOR_REL)),
attach_to=self.__ego_vehicle,
attachment_type=carla.AttachmentType.Rigid)