def main():
target_waypoints_bak = []
# Initialize pygame
pygame.init()
# Initialize actors, display configuration and clock
actor_list = []
display = pygame.display.set_mode((800, 600),
pygame.HWSURFACE | pygame.DOUBLEBUF)
font = get_font()
clock = pygame.time.Clock()
# Build connection between server and client
client = carla.Client('localhost', 2000)
client.set_timeout(2.0)
# Get the world class
world = client.get_world()
# actor_list = world.get_actors() # can get actors like traffic lights, stop signs, and spectator
#carla.Transform(carla.Location(x=64, y = 70, z=15), carla.Rotation(pitch=-80, yaw = 0)))
# set a clear weather
# weather = carla.WeatherParameters(cloudyness=0.0, precipitation=0.0, sun_altitude_angle=90.0)
# world.set_weather(weather)
world.set_weather(carla.WeatherParameters.ClearNoon)
# Some global variables
global should_auto
should_auto = False
global should_save
should_save = False
global should_slip
should_slip = False
global should_quit
should_quit = False
try:
m = world.get_map()
spawn_points = m.get_spawn_points()
print("total number of spawn_points", len(spawn_points))
# Initialize spawn configuration
spawn_config = 1
start_pose, destination = choose_spawn_destination(
m, spawn_config=spawn_config)
# Manually choose spawn location
#start_pose = carla.Transform(location=carla.Location(x=-6.446170, y=-79.055023))
print("start_pose")
print(start_pose)
# Manually choose destination
destination = carla.Transform(
location=carla.Location(x=121.678581, y=61.944809, z=-0.010011))
#?????destination = carla.Transform(location=carla.Location(x=-2.419357, y=204.005676, z=1.843104))
print("destination")
print(destination)
# Find the first waypoint equal to spawn location
print("Start waypoint", start_pose.location)
start_waypoint = m.get_waypoint(start_pose.location)
print("End waypoint", destination.location)
end_waypoint = m.get_waypoint(destination.location)
# Get blueprint library
blueprint_library = world.get_blueprint_library()
# set a constant vehicle
vehicle_temp = random.choice(
blueprint_library.filter('vehicle.lincoln.mkz2017'))
vehicle = world.spawn_actor(vehicle_temp, start_pose)
actor_list.append(vehicle)
vehicle.set_simulate_physics(True)
# Set a on-vehicle rgb camera
"""
camera_rgb = world.spawn_actor(
blueprint_library.find('sensor.camera.rgb'),
carla.Transform(carla.Location(x=-5.5, z=5.8), carla.Rotation(pitch=-35)),
attach_to=vehicle)
"""
camera_rgb = world.spawn_actor(
blueprint_library.find('sensor.camera.rgb'),
#starting position
#carla.Transform(carla.Location(x=110, y =55, z=15), carla.Rotation(pitch=-45, yaw = 180)))
#friction
# Angle
#carla.Transform(carla.Location(x=58, y = 72, z=15), carla.Rotation(pitch=-45, yaw = -40)))
# bird's view
#carla.Transform(carla.Location(x=64, y = 70, z=15), carla.Rotation(pitch=-80, yaw = 0)))
# closer
carla.Transform(carla.Location(x=73, y=73, z=15),
carla.Rotation(pitch=-90, yaw=0)))
actor_list.append(camera_rgb)
# Set a on-vehicle perceptron module
camera_semseg = world.spawn_actor(
blueprint_library.find('sensor.camera.semantic_segmentation'),
carla.Transform(carla.Location(x=-5.5, z=2.8),
carla.Rotation(pitch=-15)),
attach_to=vehicle)
actor_list.append(camera_semseg)
# Create a basic agent in the vehicle
target_speed = 30
vehicle_agent = BasicAgent(vehicle, target_speed=target_speed)
destination_loc = destination.location
vehicle_agent.set_destination(
(destination_loc.x, destination_loc.y, destination_loc.z))
# Initialize an NN controller from file
nn_number_waypoints = 5
model = mlp(nx=(5 + 3 * nn_number_waypoints), ny=2)
checkpoint = torch.load(model_path)
model.load_state_dict(checkpoint['model_state_dict'])
model = model.to(dtype).to(device)
model.eval()
print("Model loaded")
# Initialize an ILQR controller
controller = ILQRController(target_speed,
steps_ahead=100,
dt=0.05,
l=1.0,
half_width=1.5)
ilqr_number_waypoints = controller.steps_ahead
""" Collect data to file"""
csv_dir = build_file_base(
epoch,
timestr,
spawn_config,
nn_number_waypoints,
target_speed,
info='friction_safe_150cm_run') if save_data else None
# Create a synchronous mode context.
MIN_DISTANCE_PERCENTAGE = 0.9
MIN_DISTANCE = target_speed / 3.6 * MIN_DISTANCE_PERCENTAGE
nn_min_distance = MIN_DISTANCE
nn_target_waypoints = deque(maxlen=nn_number_waypoints)
ilqr_min_distance = 0.0
ilqr_target_waypoints = deque(maxlen=ilqr_number_waypoints)
visual_number_waypoints = 1
visual_target_waypoints = deque(maxlen=visual_number_waypoints)
max_distance = 1.5
#vehicle.set_autopilot(True)
# Start simulation
with CarlaSyncMode(world, camera_rgb, camera_semseg,
fps=30) as sync_mode:
nn_trajectory = []
ilqr_trajectory = []
tot_episodes = 0
tot_time = 0
top_spd = 0
avg_spd = 0
tot_unsafe_time = 0
tot_unsafe_episodes = 0
tot_interference_episodes = 0
tot_interference_time = 0
while True:
get_event()
# Quit the game once ESC is pressed
if should_quit:
return
# Spawn Trigger Friction once f key is pressed
avoidances = [carla.Location(64, 62, 0)]
#avoidances = [carla.Location(83, 50, 0)]
if should_slip:
for avoidance in avoidances:
friction_bp = world.get_blueprint_library().find(
'static.trigger.friction')
friction_bp, friction_transform, friction_box = config_friction(friction_bp, \
location = avoidance, \
extent = carla.Location(6., 2., 0.2),\
color = (255, 127, 0))
world.debug.draw_box(**friction_box)
frictioner = world.spawn_actor(friction_bp,
friction_transform)
actor_list.append(frictioner)
should_slip = False
clock.tick(30)
# Get the current measurements
t = vehicle.get_transform()
v = vehicle.get_velocity()
measurements_dict = {"v": v, "t": t}
measurements = AttrDict(measurements_dict)
print("Location:", measurements.t.location)
spd = np.linalg.norm([v.x, v.y], ord=2)
print("Velocity:", spd)
if top_spd <= spd:
top_spd = spd
print("New top speed: {}".format(top_spd))
nn_min_distance = MIN_DISTANCE
ilqr_min_distance = spd * controller.dt
# get last waypoint
current_waypoint = m.get_waypoint(t.location)
if distance_vehicle(current_waypoint, t) > max_distance:
tot_unsafe_episodes += 1
tot_unsafe_time += controller.dt
""" (visualize) Query ground true future waypoints
"""
query_target_waypoints(current_waypoint, \
target_speed, visual_number_waypoints, visual_target_waypoints, \
m = m, measurements = measurements, min_distance = nn_min_distance * 0.1)
for visual_target_waypoint in visual_target_waypoints:
world.debug.draw_box(**config_waypoint_box(
visual_target_waypoint, color=(0, 255, 0)))
""" Collect true waypoints with PD controller
"""
# Run PD controllerr
control_auto, target_waypoint = vehicle_agent.run_step(
debug=False)
# Draw PD controller target waypoints
#world.debug.draw_box(**config_waypoint_box(target_waypoint, color = (0, 0, 0)))
if should_save:
# Start saving target waypoint to waypoint file
print("Stored target waypoint {}".format(\
[target_waypoint.transform.location.x, target_waypoint.transform.location.y]))
target_waypoints_bak.append([
target_waypoint.transform.location.x,
target_waypoint.transform.location.y
])
# Run other controllers
if not should_auto:
# Draw nn controller waypoints
#for horizon_waypoint in target_waypoints:
# world.debug.draw_box(**config_waypoint_box(target_waypoint, color = (255, 0, 0)))
# Run constant control
# control = carla.VehicleControl(throttle=1, steer=0)
""" Predict future nn trajectory and draw
"""
nn_trajectory_pred = get_nn_prediction(m, measurements, controller, model, \
ilqr_number_waypoints, target_speed, nn_number_waypoints)
state = nn_trajectory_pred.states[0]
# Draw predcted measurements
#for i_measurements in nn_trajectory_pred.measurements:
# world.debug.draw_box(**config_measurements_box(i_measurements,\
# color = (255, 0, 0)))
us_init = np.array(
nn_trajectory_pred.us[:ilqr_number_waypoints - 1])
print("Verify predicted nn trajectory")
unsafe = verify_in_lane(nn_trajectory_pred,
ilqr_number_waypoints,
max_distance)
#unsafe = verify_avoidance(nn_trajectory_pred, number_waypoints, avoidances, 1.0)
if unsafe < ilqr_number_waypoints or no_interference:
""" Get output from neural controller
"""
# Visualize
world.debug.draw_box(**config_measurements_box(
measurements, color=(255, 0, 0)))
if unsafe:
print(
"!!!!!!!!!!!!!!\nUNSAFE NN operation number {}"
.format(unsafe))
print("!!!!!!!!!!!!!!")
# Query future waypoints
query_target_waypoints(current_waypoint, \
target_speed, nn_number_waypoints, nn_target_waypoints, \
m = m, measurements = measurements, min_distance = nn_min_distance)
# Draw future waypoints
#for nn_target_waypoint in nn_target_waypoints:
# world.debug.draw_box(**config_waypoint_box(nn_target_waypoint, color = (0, 255, 0)))
# Build state for neural controller
unnormalized_state, state = build_nn_state(measurements, \
target_speed, nn_number_waypoints, nn_target_waypoints)
# Get nn control
control_nn = get_nn_controller(state, model, device)
control = control_nn
nn_trajectory.append([
tot_episodes, control.throttle, control.steer, spd
])
else:
print("!!!!!!!!!!!!!!\nUNSAFE NN operation number {}".
format(unsafe))
print("!!!!!!!!!!!!!!")
""" Run ilqr controller
"""
# Visualize
world.debug.draw_box(**config_measurements_box(
measurements, color=(0, 0, 255)))
us_init = 0.0 * us_init
# Query future waypoints
query_target_waypoints(current_waypoint, \
target_speed, ilqr_number_waypoints, ilqr_target_waypoints, \
m = m, measurements = measurements, min_distance = ilqr_min_distance)
# Draw future waypoints
#for ilqr_target_waypoint in ilqr_target_waypoints:
# world.debug.draw_box(**config_waypoint_box(ilqr_target_waypoint, color = (0, 0, 255)))
# Get ilqr control
control_ilqr, ilqr_trajectory_pred = get_ilqr_control(measurements, \
controller, ilqr_target_waypoints, avoidances = avoidances, us_init = us_init)
control = control_ilqr
print("Verify predicted ilqr trajectory")
unsafe = verify_in_lane(ilqr_trajectory_pred,
ilqr_number_waypoints,
max_distance)
#unsafe = verify_avoidance(ilqr_trajectory_pred, number_waypoints, avoidances, 2.0)
if unsafe:
print(
"!!!!!!!!!!!!!!!!!!!!!!!!!\nUNSAFE ilqr operation number {}"
.format(unsafe))
print("!!!!!!!!!!!!!!!!!!!!!!!!!")
tot_interference_episodes += 1
tot_interference_time += controller.dt
ilqr_trajectory.append([
tot_episodes, control.throttle, control.steer, spd
])
else:
print("Auto pilot takes it from here")
print("output from PD_control", control_auto.throttle,
control_auto.steer)
control = control_auto
# Apply control to the actuator
vehicle.apply_control(control)
print("[Apply control]", control.throttle, control.steer)
# Store vehicle information
if leave_start_position(start_pose.location,
vehicle) >= 2 or tot_episodes >= 10:
# If having left the start position
tot_episodes += 1
tot_time += clock.get_time() / 1e3
#controller.dt = clock.get_time()/1e3
#controller.steps_ahead = int(5./controller.dt)
#ilqr_number_waypoints = controller.steps_ahead
#print("Update ILQR steps ahead {}, step length {}".format(controller.steps_ahead,\
# controller.dt))
avg_spd += spd
#nn_trajectory.append([control.throttle, control.steer, spd])
# Collect state-control training data
# world.debug.draw_box(**config_measurements_box(i_measurements,\
# color = (255, 0, 0)))
if csv_dir is not None:
y_x = np.hstack(
(np.array([control.throttle,
control.steer]), state))
with open(csv_dir, 'a+') as csvFile:
writer = csv.writer(csvFile)
writer.writerow(y_x)
# If reaching destination
if reach_destination(destination_loc, vehicle) < 2.0 or (
control_auto.throttle == 0
and control_auto.steer == 0.0):
if tot_episodes >= 10:
raise Exception("Endgame")
print("It is not the end!!!??")
return
print(">>>>Episode: {}".format(tot_episodes))
# Advance the simulation and wait for the data.
snapshot, image_rgb, image_semseg = sync_mode.tick(
timeout=2.0, vehicle=vehicle, control=control)
image_semseg.convert(carla.ColorConverter.CityScapesPalette)
fps = round(1.0 / snapshot.timestamp.delta_seconds)
# Draw the display.
draw_image(display, image_rgb)
#draw_image(display, image_semseg, blend=True)
display.blit(
font.render('% 5d FPS (real)' % clock.get_fps(), True,
(255, 255, 255)), (8, 10))
display.blit(
font.render('% 5d FPS (simulated)' % fps, True,
(255, 255, 255)), (8, 28))
pygame.display.flip()
# raise ValueError("stop here")
print('destroying local actors.')
for actor in actor_list:
actor.destroy()
except Exception as e:
print(e)
#if tot_episodes <= 10:
# os.remove(csv_dir)
traceback.print_exc()
finally:
""" Store waypoints to file
if len(target_waypoints_bak) is not 0:
print("Store {} waypoints".format(len(target_waypoints_bak)))
pickle.dump(target_waypoints_bak, open('./wps_at_plant_rotary_02.pt', 'wb'))
else:
print("Did not store target waypoints (length={})".format(len(target_waypoints_bak)))
"""
if should_quit:
os.remove(csv_dir)
print('destroying actors.')
for actor in actor_list:
actor.destroy()
avg_spd /= tot_episodes
print(">>>>>>>>>>> Total episodes: {} episodes||{}s <<<<<<<<<<<<<<".
format(tot_episodes, tot_time))
print(">>>>>>>>>>>>>Total unsafe episodes: {}||{}s<<<<<<<<<<<<<<<<".format(tot_unsafe_episodes,\
tot_unsafe_time))
print(">>>>>>>>>>>>>>Total interference episodes: {}||{}s<<<<<<<<<".format(tot_interference_episodes,\
tot_unsafe_time))
print(">>>>>>>>>>>>>>Top speed: {} <<<<<<<<<<<<<<<<<<<<<<".format(
top_spd))
print(">>>>>>>>>>>>>>Average speed: {}<<<<<<<<<<<<<<<<<<<".format(
avg_spd))
pygame.quit()
print('done.')
import numpy as np
import matplotlib
import matplotlib.pyplot as plt
import torch
import torch.nn.functional as F
from torch.autograd import Variable
# from torchviz import make_dot
from torchsummary import summary
from NN_controller import mlp
torch.set_default_dtype(torch.float32)
num_wps = 10
model = mlp(nx=(5 + 3 * num_wps), ny=2)
sample_state = np.array([0.00414619210919023, -0.15624896240234376, -0.19012335205078126, -0.5009140014648438, 0.75, -0.15629389953613282, -0.20678996276855469, 1.4991319444444444, -0.15633935546875, -0.22345657348632814, \
1.4991319444444444, -0.15638481140136717, -0.24012318420410156, 1.4991319444444444, -0.15643026733398438, -0.25678976440429685, 1.4991319444444444, -0.15645565795898436, -0.2734342346191406, 1.5011489868164063, \
-0.15625730895996093, -0.29004119873046874, 1.5064541286892361, -0.1557822265625, -0.3066424865722656, 1.511759270562066, -0.15503054809570313, -0.323233642578125, \
1.5170644124348958, -0.1540864562988281, -0.33986141967773437, 1.5181676228841146, -0.152741455078125, -0.356009033203125, -0.44086286756727433]).astype(np.float32).reshape(1, -1)
print("sample_state", sample_state.shape)
state = torch.from_numpy(sample_state)
MLP_dict_path = "/home/ruihan/UnrealEngine_4.22/carla/Dist/CARLA_0.9.5-428-g0ce908db/LinuxNoEditor/PythonAPI/synchronous_mode/models/mlp/dest_start_merge_models/mlp_dict_nx=8_wps10_lr0.001_bs32_optimSGD_ep100_ep200_ep300_ep400_ep500_ep600_ep700_ep800_ep900_ep1000.pth"
device = torch.device("cuda:0" if torch.cuda.is_available() else "cpu")
checkpoint = torch.load(MLP_dict_path)
model.load_state_dict(checkpoint['model_state_dict'])
model = model.to(device)
def main():
target_waypoints_bak = []
# Initialize pygame
pygame.init()
# Initialize actors, display configuration and clock
actor_list = []
display = pygame.display.set_mode((800, 600),
pygame.HWSURFACE | pygame.DOUBLEBUF)
font = get_font()
clock = pygame.time.Clock()
# Build connection between server and client
client = carla.Client('localhost', 2000)
client.set_timeout(2.0)
# Get the world class
world = client.get_world()
# actor_list = world.get_actors() # can get actors like traffic lights, stop signs, and spectator
# set a clear weather
# weather = carla.WeatherParameters(cloudyness=0.0, precipitation=0.0, sun_altitude_angle=90.0)
# world.set_weather(weather)
world.set_weather(carla.WeatherParameters.ClearNoon)
# Some global variables
global should_auto
should_auto = False
global should_save
should_save = False
global should_slip
should_slip = False
global should_quit
should_quit = False
try:
m = world.get_map()
spawn_points = m.get_spawn_points()
print("total number of spawn_points", len(spawn_points))
# Initialize spawn configuration
spawn_config = 1
start_pose, destination = choose_spawn_destination(
m, spawn_config=spawn_config)
# Manually choose spawn location
#start_pose = carla.Transform(location=carla.Location(x=-6.446170, y=-79.055023))
print("start_pose")
print(start_pose)
# Manually choose destination
destination = carla.Transform(
location=carla.Location(x=121.678581, y=61.944809, z=-0.010011))
#?????destination = carla.Transform(location=carla.Location(x=-2.419357, y=204.005676, z=1.843104))
print("destination")
print(destination)
# Find the first waypoint equal to spawn location
print("Start waypoint", start_pose.location)
start_waypoint = m.get_waypoint(start_pose.location)
print("End waypoint", destination.location)
end_waypoint = m.get_waypoint(destination.location)
# Get blueprint library
blueprint_library = world.get_blueprint_library()
# set a constant vehicle
vehicle_temp = random.choice(
blueprint_library.filter('vehicle.lincoln.mkz2017'))
vehicle = world.spawn_actor(vehicle_temp, start_pose)
actor_list.append(vehicle)
vehicle.set_simulate_physics(True)
# Set a on-vehicle rgb camera
camera_rgb = world.spawn_actor(
blueprint_library.find('sensor.camera.rgb'),
carla.Transform(carla.Location(x=-5.5, z=2.8),
carla.Rotation(pitch=-15)),
attach_to=vehicle)
actor_list.append(camera_rgb)
# Set a on-vehicle perceptron module
camera_semseg = world.spawn_actor(
blueprint_library.find('sensor.camera.semantic_segmentation'),
carla.Transform(carla.Location(x=-5.5, z=2.8),
carla.Rotation(pitch=-15)),
attach_to=vehicle)
actor_list.append(camera_semseg)
# Create a basic agent in the vehicle
target_speed = 30
vehicle_agent = BasicAgent(vehicle, target_speed=target_speed)
destination_loc = destination.location
vehicle_agent.set_destination(
(destination_loc.x, destination_loc.y, destination_loc.z))
# Initialize an NN controller from file
number_waypoints = 5
model = mlp(nx=(5 + 3 * number_waypoints), ny=2)
checkpoint = torch.load(model_path)
model.load_state_dict(checkpoint['model_state_dict'])
model = model.to(dtype).to(device)
model.eval()
print("Model loaded")
# Initialize an ILQR controller
controller = ILQRController(target_speed, steps_ahead=20)
number_waypoints = controller.steps_ahead
""" Collect data to file"""
csv_dir = None
#csv_dir = build_file_base(timestr, spawn_config, number_waypoints, target_speed)
# Create a synchronous mode context.
MIN_DISTANCE_PERCENTAGE = 0.9
min_distance = target_speed / 3.6 * MIN_DISTANCE_PERCENTAGE
target_waypoints = deque(maxlen=number_waypoints)
#vehicle.set_autopilot(True)
# Start simulation
with CarlaSyncMode(world, camera_rgb, camera_semseg,
fps=30) as sync_mode:
i_episode = 0
i_time = 0
top_spd = 0
avg_spd = 0
while True:
get_event()
# Quit the game once ESC is pressed
if should_quit:
return
# Spawn Trigger Friction once f key is pressed
if should_slip:
friction_bp = world.get_blueprint_library().find(
'static.trigger.friction')
friction_transform, friction_box = config_friction(friction_bp, \
location = carla.Location(62, 61, 0), \
extent = carla.Location(700., 700., 700.))
frictioner = world.spawn_actor(friction_bp,
friction_transform)
actor_list.append(frictioner)
world.debug.draw_box(**friction_box)
should_slip = False
clock.tick(30)
# Get the current measurements
t = vehicle.get_transform()
v = vehicle.get_velocity()
measurements_dict = {"v": v, "t": t}
measurements = AttrDict(measurements_dict)
print("Location:", measurements.t.location)
spd = np.linalg.norm([measurements.v.x, measurements.v.y],
ord=2)
print("Velocity:", spd)
if top_spd <= spd:
top_spd = spd
print("New top speed: {}".format(top_spd))
# get last waypoint
current_waypoint = m.get_waypoint(t.location)
query_target_waypoints(current_waypoint, \
target_speed, number_waypoints, target_waypoints, \
measurements = measurements, min_distance = min_distance)
# Draw future waypoints
for target_waypoint in target_waypoints:
world.debug.draw_box(**config_waypoint_box(
target_waypoint, color=(0, 255, 0)))
# Run PD controllerr
control_auto, target_waypoint = vehicle_agent.run_step(
debug=False)
# Draw PD controller target waypoints
world.debug.draw_box(
**config_waypoint_box(target_waypoint, color=(0, 0, 255)))
if should_save:
# Start saving target waypoint to waypoint file
print("Stored target waypoint {}".format(\
[target_waypoint.transform.location.x, target_waypoint.transform.location.y]))
target_waypoints_bak.append([
target_waypoint.transform.location.x,
target_waypoint.transform.location.y
])
# Run other controllers
if not should_auto:
""" Run ilqr controller
"""
control_ilqr = get_ilqr_control(measurements, controller,
target_waypoints)
print("output from ilqr_control", control_ilqr.throttle,
control_ilqr.steer)
control = control_ilqr
# Draw nn controller waypoints
#for horizon_waypoint in target_waypoints:
# world.debug.draw_box(**config_waypoint_box(target_waypoint, color = (255, 0, 0)))
# Run constant control
# control = carla.VehicleControl(throttle=1, steer=0)
""" Get output from neural controller
# Build state for neural controller
unnormalized_state, state = build_nn_state(measurements, \
target_speed, number_waypoints, target_waypoints)
# Get nn control
control_nn = get_nn_controller(state, model, device)
control = control_nn
"""
else:
print("Auto pilot takes it from here")
print("output from PD_control", control_auto.throttle,
control_auto.steer)
control = control_auto
# Apply control to the actuator
vehicle.apply_control(control)
# Store vehicle information
if leave_start_position(start_pose.location,
vehicle) >= 2 or i_episode >= 10:
# If having left the start position
i_episode += 1
i_time += clock.get_time()
print("Episode: {}".format(i_episode))
avg_spd += spd
# Collect state-PDcontrol training data
y_x = np.hstack(
(np.array([control.throttle, control.steer]), state))
if csv_dir is not None:
with open(csv_dir, 'a+') as csvFile:
writer = csv.writer(csvFile)
writer.writerow(y_x)
# If reaching destination
if reach_destination(destination_loc, vehicle) < 2.0 or (
control_auto.throttle == 0
and control_auto.steer == 0.0):
if i_episode >= 10:
raise Exception("Endgame")
print("It is not the end!!!??")
return
# Advance the simulation and wait for the data.
snapshot, image_rgb, image_semseg = sync_mode.tick(
timeout=2.0, vehicle=vehicle, control=control)
image_semseg.convert(carla.ColorConverter.CityScapesPalette)
fps = round(1.0 / snapshot.timestamp.delta_seconds)
# Draw the display.
draw_image(display, image_rgb)
#draw_image(display, image_semseg, blend=True)
display.blit(
font.render('% 5d FPS (real)' % clock.get_fps(), True,
(255, 255, 255)), (8, 10))
display.blit(
font.render('% 5d FPS (simulated)' % fps, True,
(255, 255, 255)), (8, 28))
pygame.display.flip()
# raise ValueError("stop here")
print('destroying local actors.')
for actor in actor_list:
actor.destroy()
except Exception as e:
print(e)
#if i_episode <= 10:
# os.remove(csv_dir)
traceback.print_exc()
finally:
""" Store waypoints to file
if len(target_waypoints_bak) is not 0:
print("Store {} waypoints".format(len(target_waypoints_bak)))
pickle.dump(target_waypoints_bak, open('./wps_at_plant_rotary_02.pt', 'wb'))
else:
print("Did not store target waypoints (length={})".format(len(target_waypoints_bak)))
"""
print('destroying actors.')
for actor in actor_list:
actor.destroy()
avg_spd /= i_episode
print(">>>>>>>>>>> Total Time: {} episodes||{}s <<<<<<<<<<<<<<".format(
i_episode, i_time / 1e3))
print(">>>>>>>>>>>>>>Top speed: {} <<<<<<<<<<<<<<<<<<<<<<".format(
top_spd))
print(">>>>>>>>>>>>>>Average speed: {}<<<<<<<<<<<<<<<<<<<".format(
avg_spd))
pygame.quit()
print('done.')
def main():
pygame.init()
# init font for text message
pygame.font.init()
# myfont = pygame.font.SysFont('Comic Sans MS', 30) # see below
hud_dim = [800, 600] # default: 800, 600 # collect data: 200, 88
display = pygame.display.set_mode((hud_dim[0], hud_dim[1]),
pygame.HWSURFACE | pygame.DOUBLEBUF)
font = get_font()
clock = pygame.time.Clock()
client = carla.Client('localhost', 2000)
client.set_timeout(2.0)
world = client.get_world()
world.set_weather(carla.WeatherParameters.ClearNoon)
blueprint_library = world.get_blueprint_library()
# actor_list = world.get_actors() # can get actors like traffic lights, stop signs, and spectator
global_actor_list = []
save_dir_base = 'data/testing/'
#MLP_model_path = "/home/depend/workspace/carla/PythonAPI/synchronous_mode/models/mlp/dest_start_SR0.5_models/mlp_model_nx=8_wps10_lr0.001_bs32_optimSGD_ep1000.pth"
MLP_model_path = "/home/depend/workspace/carla/PythonAPI/synchronous_mode/models/mlp/dest_start_merge_models/mlp_dict_nx=8_wps5_lr0.001_bs32_optimSGD_ep100_ep200_ep300_ep400_ep500_ep600_ep700_ep800_ep900_ep1000.pth"
# "/home/ruihan/UnrealEngine_4.22/carla/Dist/CARLA_0.9.5-428-g0ce908db/LinuxNoEditor/PythonAPI/synchronous_mode/models/mlp/dest_start_merge_models/mlp_model_nx=8_wps10_lr0.001_bs32_optimSGD_ep100_ep200_ep300_ep400_ep500_ep600_ep700_ep800_ep900_ep1000.pth"
# "/home/ruihan/UnrealEngine_4.22/carla/Dist/CARLA_0.9.5-428-g0ce908db/LinuxNoEditor/PythonAPI/synchronous_mode/models/mlp/dest_start_merge_models/mlp_model_nx=8_wps5_lr0.001_bs32_optimSGD_ep100_ep200_ep300_ep400_ep500_ep600_ep700_ep800_ep900_ep1000.pth"
# "/home/ruihan/UnrealEngine_4.22/carla/Dist/CARLA_0.9.5-428-g0ce908db/LinuxNoEditor/PythonAPI/synchronous_mode/models/mlp/dest_start_merge_models/mlp_model_nx=8_wps10_lr0.001_bs32_optimSGD_ep100_ep200_ep300_ep400_ep500_ep600_ep700_ep800_ep900_ep1000.pth"
# "/home/ruihan/UnrealEngine_4.22/carla/Dist/CARLA_0.9.5-428-g0ce908db/LinuxNoEditor/PythonAPI/synchronous_mode/models/mlp/mlp_img_model_nx=8_wps10_lr0.001_bs64_optimAdam_ep1000.pth"
model = torch.load(MLP_model_path)
checkpoint = torch.load(MLP_model_path)
model = mlp(nx=5 + 3 * 5, ny=2)
model.load_state_dict(checkpoint['model_state_dict'])
model.eval()
device = torch.device("cuda:0" if torch.cuda.is_available() else "cpu")
try:
m = world.get_map()
spawn_points = m.get_spawn_points()
print("total number of spawn_points", len(spawn_points))
# TODO: randomly choose spawn_point and destiny to test performance on new routes
i = random.randint(0, len(spawn_points))
j = random.randint(0, len(spawn_points))
while j == i:
j = random.randint(0, len(spawn_points))
i = 3
j = 0 # 2
save_dir = os.path.join(save_dir_base, 'data_{}_{}/'.format(i, j))
destiny = spawn_points[i]
print(j, "car destiny", destiny.location)
start_pose = spawn_points[j]
print(i, "car start_pose", start_pose.location)
# TODO: consider create a class of agent and use planner as in my_local_planner
# waypoints_queue = deque(maxlen=20000)
# use record and replay API
# check /home/ruihan/.config/Epic/CarlaUE4/Saved path
# client.start_recorder("record_testing_dest_{}_start_{}_wps10_1000.log".format(i, j))
# set and spawn actors
actor_list = []
vehicle_bp = random.choice(
blueprint_library.filter('vehicle.lincoln.mkz2017'))
vehicle = world.spawn_actor(vehicle_bp, start_pose)
vehicle.set_simulate_physics(True)
actor_list.append(vehicle)
# common camera locations:
# x=1.6, z=1.7 for front
# x=-5.5, z=2.8 for back
camera_rgb_bp = blueprint_library.find('sensor.camera.rgb')
camera_rgb_bp.set_attribute('image_size_x', str(hud_dim[0]))
camera_rgb_bp.set_attribute('image_size_y', str(hud_dim[1]))
camera_rgb = world.spawn_actor(camera_rgb_bp,
carla.Transform(
carla.Location(x=-5.5, z=2.8),
carla.Rotation(pitch=-15)),
attach_to=vehicle)
actor_list.append(camera_rgb)
camera_semseg_bp = blueprint_library.find(
'sensor.camera.semantic_segmentation')
camera_semseg_bp.set_attribute('image_size_x', str(hud_dim[0]))
camera_semseg_bp.set_attribute('image_size_y', str(hud_dim[1]))
camera_semseg = world.spawn_actor(camera_semseg_bp,
carla.Transform(
carla.Location(x=-5.5, z=2.8),
carla.Rotation(pitch=-15)),
attach_to=vehicle)
actor_list.append(camera_semseg)
# set PD control to the vehicle, fine here as long as you don't use vehicle_agent.run_step
target_speed = 30
vehicle_agent = BasicAgent(vehicle, target_speed=target_speed)
destiny_loc = destiny.location
vehicle_agent.set_destination(
(destiny_loc.x, destiny_loc.y, destiny_loc.z))
# vehicle.set_autopilot(True)
print("local actor list", actor_list)
img_width = 200
img_height = 88
num_wps = 5
MIN_DISTANCE_PERCENTAGE = 0.9
min_distance = target_speed / 3.6 * MIN_DISTANCE_PERCENTAGE
target_waypoints = deque(maxlen=num_wps)
safety_checking = False
# Create a synchronous mode context.
with CarlaSyncMode(world, camera_rgb, camera_semseg,
fps=20) as sync_mode:
control = vehicle.get_control()
while reach_destiny(destiny_loc, vehicle) > 10:
if should_quit():
print('destroying local actors.')
for actor in actor_list:
actor.destroy()
return
clock.tick(20)
t = vehicle.get_transform()
v = vehicle.get_velocity()
# Instead of using BasicAgent, query target_waypoint and feed in NN controller
last_waypoint = m.get_waypoint(t.location)
# Method 1. query the target-waypoints based on current location of each frame
# # query target_waypoints based on current location
# target_waypoints = []
# for k in range(num_wps):
# target_waypoint, road_option = compute_target_waypoint(last_waypoint, target_speed)
# target_waypoints.append([target_waypoint, road_option])
# last_waypoint = target_waypoint
# target_waypoints_np = []
# for wp_ro in target_waypoints:
# target_waypoints_np.append(transform_to_arr(wp_ro[0].transform))
# target_waypoints_np = np.array([target_waypoints_np]).flatten()
# Method 2. Use target_waypoints buffer and pop out once reached
# check if reach, pop out from the list (purge the queue of obsolete waypoints)
print("before purge", len(target_waypoints))
max_index = -1
# print(list(enumerate(target_waypoints)))
for num, waypoint in enumerate(target_waypoints):
if distance_vehicle(waypoint, t) < min_distance:
max_index = num
if max_index >= 0:
for num in range(max_index + 1):
target_waypoints.popleft()
print("after purge", len(target_waypoints))
for k in range(len(target_waypoints), num_wps):
# append waypoint one step ahead
target_waypoint, road_option = compute_target_waypoint(
last_waypoint, target_speed)
target_waypoints.append(target_waypoint)
last_waypoint = target_waypoint
# spawn a trolley to visualilze target_waypoint
# actor_list.append(spawn_trolley(world, blueprint_library, x=target_waypoint.transform.location.x, y=target_waypoint.transform.location.y))
print("after refill", len(target_waypoints))
target_waypoints_np = []
for wp_ro in target_waypoints:
target_waypoints_np.append(
transform_to_arr(wp_ro.transform))
target_waypoints_np = np.array([target_waypoints_np]).flatten()
cur_speed = np.linalg.norm(np.array([v.x, v.y]))
# save the long state for data collection
# state = np.hstack((cur_speed, transform_to_arr(t), target_speed, transform_to_arr(target_waypoint.transform)))
full_state = np.hstack(
(cur_speed, transform_to_arr(t), target_speed,
target_waypoints_np)).flatten() # shape(188,)
# print("full state")
# print(full_state)
# parse the state for NN testing
unnormalized = np.hstack(
(full_state[0:3], full_state[5], full_state[7]))
state = np.hstack(
(full_state[0] / max_speed_val,
full_state[1] / max_pos_val, full_state[2] / max_pos_val,
full_state[5] / max_yaw_val,
full_state[7] / max_speed_val))
for j in range(num_wps): # concatenate x, y, yaw of future_wps
unnormalized = np.hstack(
(unnormalized, full_state[8 + j * 6],
full_state[9 + j * 6], full_state[12 + j * 6]))
state = np.hstack(
(state, full_state[8 + j * 6] / max_pos_val,
full_state[9 + j * 6] / max_pos_val,
full_state[12 + j * 6] / max_yaw_val))
# actor_list.append(spawn_trolley(world, blueprint_library, x=full_state[8+j*6], y=full_state[9+j*6], z=10))
# print("unnormalized")
# print(unnormalized)
# # calculate the relative coordinates
# state[1] = state[1] + spawn_points[0].location.x - start_pose.location.x
# state[5] = state[5] + spawn_points[0].location.x - start_pose.location.x
# state[2] = state[2] + spawn_points[0].location.y - start_pose.location.y
# state[6] = state[6] + spawn_points[0].location.y - start_pose.location.y
# Advance the simulation and wait for the data.
snapshot, image_rgb, image_semseg = sync_mode.tick(
timeout=5.0) # extend timeout=2.0
image_semseg.convert(carla.ColorConverter.CityScapesPalette)
fps = round(1.0 / snapshot.timestamp.delta_seconds)
frame_number = image_semseg.frame_number
# save the data
# image_semseg.save_to_disk(save_dir+'{:08d}'.format(frame_number))
if '_img_' in MLP_model_path:
# process the img data as a tensor (without saving and reading in again), see _parse_image in manual_control.py
print("process img online")
array = np.frombuffer(image_semseg.raw_data,
dtype=np.dtype("uint8"))
array = np.reshape(
array, (image_semseg.height, image_semseg.width, 4))
array = array[:, :, :3]
array = array[:, :, ::-1]
print(array.shape)
control = get_nn_img_controller(array, state, model,
device, img_height,
img_width)
else:
control = get_nn_controller(state, model, device)
pred_traj = []
if safety_checking:
# check if the traj is safe
# safe, unsafe_time, pred_traj = mpc_verify(world, blueprint_library, model, device, m, full_state, control, pred_traj)
result = mpc_verify_2(world, blueprint_library, model,
device, m, full_state, control,
pred_traj)
print("message")
print(result.message)
print("output")
print(result.x)
if result.success:
print("NN control is safe", control.throttle,
control.steer)
else:
print("NN is unsafe, use MPC instead")
#TODO
# control = compute_mpc_safe_control()
# if not safe:
# print("mpc predicts unsafe traj", unsafe_time)
# textsurface = font.render('mpc predicts unsafe traj at {} step'.format(unsafe_time), False, (255, 255, 255))
# # output most conservative action
# DISCOUNTED_RATIO = 1.5
# control.throttle, control.steer = control.throttle/DISCOUNTED_RATIO, control.steer/DISCOUNTED_RATIO
# # raise ValueError("stop here")
# else:
# print("safe continue")
# textsurface = font.render('Safe. Continue', False, (255, 255, 255))
print("control", control.throttle, control.steer)
vehicle.apply_control(control)
# save the data
# path = save_dir+'{:08d}_ctv'.format(frame_number)
# x = np.hstack((np.array([control.throttle, control.steer]), state))
# print("control and measurement", x)
# save in two formats, one separate to work with img, one appended to csv file
# np.save(path, x)
# with open(csv_dir, 'a+') as csvFile:
# writer = csv.writer(csvFile)
# writer.writerow(x)
# Draw the display.
draw_image(display, image_rgb)
draw_image(display, image_semseg, blend=True)
# Render the text messafes
display.blit(
font.render('% 5d FPS (real)' % clock.get_fps(), True,
(255, 255, 255)), (8, 10))
display.blit(
font.render('% 5d FPS (simulated)' % fps, True,
(255, 255, 255)), (8, 28))
display.blit(
font.render(
'control: [{}, {}]'.format(control.throttle,
control.steer), True,
(255, 255, 255)), (8, 46))
if safety_checking:
display.blit(textsurface, (8, 64))
pygame.display.flip()
# wait()
if len(pred_traj):
# print("destroy pred_traj")
for actor in pred_traj:
actor.destroy()
print('destroying local actors.')
for actor in actor_list:
actor.destroy()
finally:
print('destroying global actors.')
for actor in global_actor_list:
actor.destroy()
pygame.quit()
# record API
# client.stop_recorder()
print('done.')