def run_step(self):
transform = self._vehicle.get_transform()
if self.waypoints:
# If too far off course, reset waypoint queue.
if distance_vehicle(self.waypoints[0],
transform) > 5.0 * self.radius:
self.waypoints = []
# Get more waypoints.
if len(self.waypoints) < self.max_waypoints // 2:
self.add_next_waypoints()
# If no more waypoints, stop.
if not self.waypoints:
print('Ran out of waypoints; stopping.')
control = carla.VehicleControl()
control.throttle = 0.0
return control
# Remove waypoints we've reached.
while distance_vehicle(self.waypoints[0], transform) < self.min_dist:
self.waypoints = self.waypoints[1:]
# Draw next waypoint
draw_waypoints(self._vehicle.get_world(), self.waypoints[:1],
self._vehicle.get_location().z + 1.0)
return self.controller.run_step(self.target_speed, self.waypoints[0])
def query_target_waypoints(current_waypoint,
target_speed,
number_waypoints,
target_waypoints,
road_option=None,
**kwargs):
# Method 1. query the target-waypoints based on current location of each frame
# # query target_waypoints based on current location
if 'measurements' not in kwargs.keys():
target_waypoints.clear()
else:
# Method 2. use the buffer target_waypoints to store the waypoints
# Once a waypoint is reached pop one waypoint out and push a new waypoint in
m = kwargs['m']
measurements = kwargs['measurements']
min_distance = kwargs['min_distance']
from_waypoint = m.get_waypoint(measurements.t.location)
max_index = -1
for num, waypoint in enumerate(list(target_waypoints)):
if distance_vehicle(waypoint,
from_waypoint.transform) < min_distance:
max_index = num
if max_index >= 0:
for num in range(1 + max_index):
target_waypoints.popleft()
if len(target_waypoints) > 0:
from_waypoint = target_waypoints[-1]
else:
from_waypoint = current_waypoint
for k in range(number_waypoints - len(target_waypoints)):
target_waypoint, road_option = compute_target_waypoint(
from_waypoint, target_speed)
target_waypoints.append(target_waypoint)
from_waypoint = target_waypoint
def run_step(self, debug=True):
"""
Execute one step of local planning which involves running the longitudinal and lateral PID controllers to
follow the waypoints trajectory.
:param debug: boolean flag to activate waypoints debugging
:return:
"""
# not enough waypoints in the horizon? => add more!
if not self._global_plan and len(self._waypoints_queue) < int(
self._waypoints_queue.maxlen * 0.5):
self._compute_next_waypoints(k=100)
if len(self._waypoints_queue) == 0:
control = carla.VehicleControl()
control.steer = 0.0
control.throttle = 0.0
control.brake = 1.0
control.hand_brake = False
control.manual_gear_shift = False
return control
# Buffering the waypoints
if not self._waypoint_buffer:
for i in range(self._buffer_size):
if self._waypoints_queue:
self._waypoint_buffer.append(
self._waypoints_queue.popleft())
else:
break
# current vehicle waypoint
self._current_waypoint = self._map.get_waypoint(
self._vehicle.get_location())
# target waypoint
self.target_waypoint, self._target_road_option = self._waypoint_buffer[
0]
# move using PID controllers
control = self._vehicle_controller.run_step(self._target_speed,
self.target_waypoint)
# purge the queue of obsolete waypoints
vehicle_transform = self._vehicle.get_transform()
max_index = -1
for i, (waypoint, _) in enumerate(self._waypoint_buffer):
if distance_vehicle(waypoint,
vehicle_transform) <= self._min_distance:
max_index = i
if max_index >= 0:
for i in range(max_index + 1):
self._waypoint_buffer.popleft()
if debug:
draw_waypoints(self._vehicle.get_world(), [self.target_waypoint],
self._vehicle.get_location().z + 1.0)
return control
def run_step(self):
assert self._route is not None
vehicle_transform = self._vehicle.get_transform()
max_index = -1
for i, (waypoint, _) in enumerate(self._waypoints_queue):
if distance_vehicle(waypoint, vehicle_transform) < self._min_distance:
max_index = i
if max_index >= 0:
for i in range(max_index + 1):
if self.distances:
self.distance_to_goal -= self.distances[0]
self.distances.popleft()
self._waypoints_queue.popleft()
if len(self._waypoints_queue) > 0:
self.target = self._waypoints_queue[0]
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.')
def run_step(self, debug=True):
"""
Execute one step of local planning which involves running the longitudinal and lateral PID controllers to
follow the waypoints trajectory.
:param debug: boolean flag to activate waypoints debugging
:return:
"""
# print("current queue len: ", len(self._waypoints_queue))
# print("maxlen: ", int(self._waypoints_queue.maxlen * 0.5))
# not enough waypoints in the horizon? => add more!
# if len(self._waypoints_queue) < int(self._waypoints_queue.maxlen * 0.5):
# print("no points here!")
# if not self._global_plan:
# self._compute_next_waypoints(k=100)
# # 队列为空时切换到手动操作模式，待修改
# if len(self._waypoints_queue) == 0:
#control = carla.VehicleControl()
#control.steer = 0.0
#control.throttle = 0.0
#control.brake = 0.0
#control.hand_brake = False
#control.manual_gear_shift = False
# return control
# Buffering the waypoints
# print("queue length inside run_step: ", len(self._waypoints_queue))
if not self._waypoint_buffer:
for i in range(self._buffer_size):
if self._waypoints_queue:
# print("queue length: ", len(self._waypoints_queue))
left_point, left_road = self._waypoints_queue.popleft()
self._waypoint_buffer.append((left_point, left_road))
# right_point, _ = self._waypoint_buffer.popleft()
# print("right point is ", right_point)
else:
break
# target_waypoint, _ = self._waypoint_buffer[0]
# print("target_waypoint:", target_waypoint)
# for i, (waypoint, _) in enumerate(self._waypoint_buffer):
# print("waypoint is ", waypoint)
# time.sleep(2)
# current vehicle waypoint
self._current_waypoint = self._map.get_waypoint(
self._vehicle.get_location())
if not self._waypoint_buffer:
# control = self._vehicle_controller.run_step(0, self._current_waypoint)
# return control
return None
self._target_waypoint, self._target_road_option = self._waypoint_buffer[
0]
# for i in range(len(self._waypoint_buffer)):
# wp, _ = self._waypoint_buffer[i]
# print("waypoint in buffer is ", wp)
# move using PID controllers
# print("target_waypoint: ", self._target_waypoint)
control = self._vehicle_controller.run_step(self._target_speed,
self._target_waypoint)
# purge the queue of obsolete waypoints
vehicle_transform = self._vehicle.get_transform()
max_index = -1
# print("len of buffer: ", len(self._waypoint_buffer))
# for i in range(len(self._waypoint_buffer)):
# waypoint = self
for i, (waypoint, _) in enumerate(self._waypoint_buffer):
distance = distance_vehicle(waypoint, vehicle_transform)
# print("min distance: ", self._min_distance, "distance: ", distance)
# time.sleep(2)
# 当前车辆和路点的距离小于最小距离，认为已经行驶完成
if distance < self._min_distance:
# print("waypoint in enumerate is ", waypoint)
max_index = i
if max_index >= 0:
for i in range(max_index + 1):
self._waypoint_buffer.popleft()
self.finished_waypoints += 1
print("current finished waypoints is ",
self.finished_waypoints)
if debug:
draw_waypoints(self._vehicle.get_world(),
[self._target_waypoint],
self._vehicle.get_location().z + 1.0)
# if len(self._waypoint_buffer) == 0:
# self.finished_waypoints = self.message_waypoints
return control
def run_step(self, debug=False):
"""
Execute one step of local planning which involves running the longitudinal and lateral PID controllers to
follow the waypoints trajectory.
:param debug: boolean flag to activate waypoints debugging
:return:
"""
##718 remove waypoints until the closest one
vehicle_transform = self._vehicle.get_transform()
min_index = -1
min_dist = 99999
for i, (waypoint, _) in enumerate(self._waypoint_buffer):
dist = distance_vehicle(waypoint, vehicle_transform)
if (dist < min_dist):
min_dist = dist
min_index = i
#max_index = i
if min_index >= 0:
for i in range(min_index):
self._waypoint_buffer.popleft()
# not enough waypoints in the horizon? => add more!
if not self._global_plan and len(self._waypoints_queue) < int(
self._waypoints_queue.maxlen * 0.5):
self._compute_next_waypoints(k=100)
if len(self._waypoints_queue) == 0:
##
#print('_waypoints_queue empty')
control = carla.VehicleControl()
control.steer = 0.0
control.throttle = 0.0
control.brake = 1.0
control.hand_brake = False
control.manual_gear_shift = False
return control
##
#print(self._waypoint_buffer)
# Buffering the waypoints
if not self._waypoint_buffer:
for i in range(self._buffer_size):
if self._waypoints_queue:
self._waypoint_buffer.append(
self._waypoints_queue.popleft())
else:
break
# current vehicle waypoint
self._current_waypoint = self._map.get_waypoint(
self._vehicle.get_location())
# target waypoint
self.target_waypoint, self._target_road_option = self._waypoint_buffer[
0]
# move using PID controllers
control = self._vehicle_controller.run_step(self._target_speed,
self.target_waypoint)
##testing waypoints
#if self.target_waypoint:
#print('target_waypoint',self.target_waypoint)
# purge the queue of obsolete waypoints
vehicle_transform = self._vehicle.get_transform()
max_index = -1
# find the waypoint in waypt buffer that have min distance
# and pop anything before that
for i, (waypoint, _) in enumerate(self._waypoint_buffer):
if distance_vehicle(waypoint,
vehicle_transform) < self._min_distance:
max_index = i
if max_index >= 0:
for i in range(max_index + 1):
self._waypoint_buffer.popleft()
if debug:
#sett = self._vehicle.get_world().get_settings()
#print(sett)
draw_waypoints(self._vehicle.get_world(), [self.target_waypoint],
self._vehicle.get_location().z + 1.0)
#draw_waypoints(self._vehicle.get_world(), [self.target_waypoint], self._vehicle.get_location().z + 1.0)
return control
def run_step(self, target_speed=None, debug=False):
"""
Execute one step of local planning which involves
running the longitudinal and lateral PID controllers to
follow the waypoints trajectory.
:param target_speed: desired speed
:param debug: boolean flag to activate waypoints debugging
:return: control
"""
if target_speed is not None:
self._target_speed = target_speed
else:
self._target_speed = self._vehicle.get_speed_limit()
# Buffer waypoints
self._buffer_waypoints()
if len(self._waypoint_buffer) == 0:
control = carla.VehicleControl()
control.steer = 0.0
control.throttle = 0.0
control.brake = 1.0
control.hand_brake = False
control.manual_gear_shift = False
return control
# Current vehicle waypoint
self._current_waypoint = self._map.get_waypoint(
self._vehicle.get_location())
speed = get_speed(self._vehicle) # kph
look_ahead = max(2, speed / 4.5)
# Target waypoint
self.target_waypoint, self.target_road_option = self._waypoint_buffer[
0]
look_ahead_loc = self._get_look_ahead_location(look_ahead)
if target_speed > 50:
args_lat = self.args_lat_hw_dict
args_long = self.args_long_hw_dict
else:
args_lat = self.args_lat_city_dict
args_long = self.args_long_city_dict
if not self._pid_controller:
self._pid_controller = VehiclePIDController(
self._vehicle,
args_lateral=args_lat,
args_longitudinal=args_long)
else:
self._pid_controller.set_lon_controller_params(**args_long)
self._pid_controller.set_lat_controller_params(**args_lat)
control = self._pid_controller.run_step(self._target_speed,
look_ahead_loc)
# Purge the queue of obsolete waypoints
vehicle_transform = self._vehicle.get_transform()
max_index = -1
for i, (waypoint, _) in enumerate(self._waypoint_buffer):
if distance_vehicle(waypoint,
vehicle_transform) < self._min_distance:
max_index = i
if max_index >= 0:
for i in range(max_index + 1):
if i == max_index:
self._prev_waypoint = self._waypoint_buffer.popleft()[0]
else:
self._waypoint_buffer.popleft()
if debug:
carla_world = self._vehicle.get_world()
# Draw current buffered waypoint
buffered_waypts = [elem[0] for elem in self._waypoint_buffer]
draw_waypoints(carla_world, buffered_waypts)
# Draw current look ahead point
look_ahead_loc
carla_world.debug.draw_line(look_ahead_loc,
look_ahead_loc + carla.Location(z=0.2),
color=carla.Color(255, 255, 0),
thickness=0.2,
life_time=1.0)
return control
def run_step(self, target_speed=None, debug=False):
"""
Execute one step of local planning which involves
running the longitudinal and lateral PID controllers to
follow the waypoints trajectory.
:param target_speed: desired speed
:param debug: boolean flag to activate waypoints debugging
:return: control
"""
if target_speed is not None:
self._target_speed = target_speed
else:
self._target_speed = self._vehicle.get_speed_limit()
if len(self.waypoints_queue) == 0:
control = carla.VehicleControl()
control.steer = 0.0
control.throttle = 0.0
control.brake = 1.0
control.hand_brake = False
control.manual_gear_shift = False
return control
# Buffering the waypoints
if not self._waypoint_buffer:
for i in range(self._buffer_size):
if self.waypoints_queue:
self._waypoint_buffer.append(
self.waypoints_queue.popleft())
else:
break
# Current vehicle waypoint
self._current_waypoint = self._map.get_waypoint(
self._vehicle.get_location())
# Target waypoint
self.target_waypoint, self.target_road_option = self._waypoint_buffer[
0]
if target_speed > 50:
args_lat = self.args_lat_hw_dict
args_long = self.args_long_hw_dict
else:
args_lat = self.args_lat_city_dict
args_long = self.args_long_city_dict
self._pid_controller = VehiclePIDController(
self._vehicle, args_lateral=args_lat, args_longitudinal=args_long)
control = self._pid_controller.run_step(self._target_speed,
self.target_waypoint)
# Purge the queue of obsolete waypoints
vehicle_transform = self._vehicle.get_transform()
max_index = -1
for i, (waypoint, _) in enumerate(self._waypoint_buffer):
if distance_vehicle(waypoint,
vehicle_transform) < self._min_distance:
max_index = i
if max_index >= 0:
for i in range(max_index + 1):
self._waypoint_buffer.popleft()
if debug:
draw_waypoints(self._vehicle.get_world(), [self.target_waypoint],
1.0)
return control
def done(self):
vehicle_transform = self._vehicle.get_transform()
return len(self._waypoints_queue) == 0 and all([
distance_vehicle(wp, vehicle_transform) < self._min_distance
for wp in self._waypoints_queue
])
def run_step(self, dest, lane_change, world, debug=False):
"""
Execute one step of local planning which involves running the longitudinal and lateral PID controllers to
follow the waypoints trajectory.
:param debug: boolean flag to activate waypoints debugging
:return:
"""
# if(lane_change == "right"):
# print('switch into the right lane')
# # change target waypoint to a point on the right lane
# right_lane = self._current_waypoint.get_right_lane()
# new_waypoint = right_lane.next(5)[0]
# # self.target_waypoint = new_waypoint
# self._waypoints_queue.clear()
# self._waypoint_buffer.clear()
# self._waypoints_queue.append((new_waypoint, RoadOption.LANEFOLLOW))
# elif(lane_change == "left"):
# print('switch into the left lane')
# # change target waypoint to a point on the right lane
# left_lane = self._current_waypoint.get_left_lane()
# new_waypoint = left_lane.next(5)[0]
# # self.target_waypoint = new_waypoint
# self._waypoints_queue.clear()
# self._waypoint_buffer.clear()
# print(len(self._waypoints_queue))
# # print("new waypoint at " + str(new_waypoint))
# self._waypoints_queue.append((new_waypoint, RoadOption.LANEFOLLOW))
# not enough waypoints in the horizon? => add more!
if not self._global_plan and len(self._waypoints_queue) < int(
self._waypoints_queue.maxlen * 0.5):
# print("current waypoint: " + str(self._current_waypoint))
# print("add 100 additional waypoints")
self._compute_next_waypoints(dest, k=100)
current_waypoint = self._map.get_waypoint(self._vehicle.get_location())
if len(self._waypoints_queue) == 0:
if not current_waypoint.is_intersection:
self._target_road_option = RoadOption.LANEFOLLOW
print("way points queue is empty")
control = carla.VehicleControl()
control.steer = 0.0
control.throttle = 0.0
control.brake = 1.0
control.hand_brake = False
control.manual_gear_shift = False
return control
# Buffering the waypoints
if not self._waypoint_buffer:
for i in range(self._buffer_size):
if self._waypoints_queue:
self._waypoint_buffer.append(
self._waypoints_queue.popleft())
else:
break
# current vehicle waypoint
self._current_waypoint = self._map.get_waypoint(
self._vehicle.get_location())
# target waypoint
self.target_waypoint, self._target_road_option = self._waypoint_buffer[
0]
# print("target waypoint at " + str(self.target_waypoint))
if (lane_change == "right"):
# print('switch into the right lane')
# change target waypoint to a point on the right lane
right_lane = self._current_waypoint.get_right_lane()
if (not right_lane):
print("cannot switch into the right lane")
else:
self.target_waypoint = right_lane.next(5)[-1]
# self.target_waypoint = new_waypoint
elif (lane_change == "left"):
# print('switch into the left lane')
# change target waypoint to a point on the right lane
left_lane = self._current_waypoint.get_left_lane()
if (not left_lane):
print("cannot switch into the right lane")
else:
self.target_waypoint = left_lane.next(5)[-1]
# # set the target speed
# speed_limit = self._vehicle.get_speed_limit()
# #set highway driving speed to 40 kmh
# if(speed_limit > 30):
# self.set_speed(30)
# # otherwise, set driving speed to 20 kmh
# else:
# self.set_speed(20)
# move using PID controllers
#print("target_speed: " + str(self._target_speed))
control = self._vehicle_controller.run_step(self._target_speed,
self.target_waypoint)
# purge the queue of obsolete waypoints
vehicle_transform = self._vehicle.get_transform()
max_index = -1
for i, (waypoint, _) in enumerate(self._waypoint_buffer):
if distance_vehicle(waypoint,
vehicle_transform) < self._min_distance:
max_index = i
if max_index >= 0:
for i in range(max_index + 1):
self._waypoint_buffer.popleft()
# if debug:
draw_waypoints(self._vehicle.get_world(), [self.target_waypoint],
self._vehicle.get_location().z + 1.0)
# if self._target_road_option != RoadOption.LANEFOLLOW not current_waypoint.is_intersection:
# self._target_road_option = RoadOption.LANEFOLLOW
return control
def run_step(self, timestep: int, debug=True, log=False):
"""
Execute one step of classic mpc controller which follow the waypoints trajectory.
:param debug: boolean flag to activate waypoints debugging
:return:
"""
start_time = time.time()
# not enough waypoints in the horizon? => Sample new ones
self._sampling_radius = calculate_step_distance(
get_speed(self._vehicle), 0.2,
factor=2) # factor 11 --> prediction horizon 10 steps
if self._sampling_radius < 2:
self._sampling_radius = 3
# Getting future waypoints
self._current_waypoint = self._map.get_waypoint(
self._vehicle.get_location())
self._next_wp_queue = compute_next_waypoints(self._current_waypoint,
d=self._sampling_radius,
k=20)
# Getting waypoint history --> history somehow starts at last wp of future wp
self._current_waypoint = self._map.get_waypoint(
self._vehicle.get_location())
self._previous_wp_queue = compute_previous_waypoints(
self._current_waypoint, d=self._sampling_radius, k=5)
# concentrate history, current waypoint, future
self._waypoint_buffer = deque(maxlen=self._buffer_size)
self._waypoint_buffer.extendleft(self._previous_wp_queue)
self._waypoint_buffer.append(
(self._map.get_waypoint(self._vehicle.get_location()),
RoadOption.LANEFOLLOW))
self._waypoint_buffer.extend(self._next_wp_queue)
self._waypoints_queue = self._next_wp_queue
# If no more waypoints in queue, returning emergency braking control
if len(self._waypoints_queue) == 0:
control = carla.VehicleControl()
control.steer = 0.0
control.throttle = 0.0
control.brake = 1.0
control.hand_brake = False
control.manual_gear_shift = False
print("Applied emergency control !!!")
return control
# target waypoint for Frenet calculation
self.wp1 = self._map.get_waypoint(self._vehicle.get_location())
self.wp2, _ = self._next_wp_queue[0]
# current vehicle waypoint
self.kappa_log = dict()
self._current_waypoint = self._map.get_waypoint(
self._vehicle.get_location())
self.curv_args, self.kappa_log = self.calculate_curvature_func_args(
log=True)
self.curv_x0 = func_kappa(0, self.curv_args)
# input for MPC controller --> wp_current, wp_next, kappa
target_wps = [self.wp1, self.wp2, self.curv_x0]
# move using MPC controller
if log:
if timestep / 30 > 8 and timestep / 30 < 9:
# Setting manual control
manual_control = [self.data_log.get('u_acceleration'), 0.01]
target_wps.append(manual_control)
print(manual_control)
# apply manual control
control, state, u, x_log, u_log, _ = self._vehicle_controller.mpc_control(
target_wps,
self._target_speed,
solve_nmpc=False,
manual=True,
log=log)
self.data_log = {
'X': state[0],
'Y': state[1],
'PSI': state[2],
'Velocity': state[3],
'Xi': state[4],
'Eta': state[5],
'Theta': state[6],
'u_acceleration': u[0],
'u_steering_angle': u[1],
'pred_states': [x_log],
'pred_control': [u_log],
'computation_time': time.time() - start_time,
"kappa": self.curv_x0,
"curvature_radius": 1 / self.curv_x0
}
elif timestep / 30 > 14 and timestep / 30 < 14.6:
# Setting manual control
manual_control = [self.data_log.get('u_acceleration'), -0.02]
target_wps.append(manual_control)
# apply manual control
control, state, u, x_log, u_log, _ = self._vehicle_controller.mpc_control(
target_wps,
self._target_speed,
solve_nmpc=False,
manual=True,
log=log)
self.data_log = {
'X': state[0],
'Y': state[1],
'PSI': state[2],
'Velocity': state[3],
'Xi': state[4],
'Eta': state[5],
'Theta': state[6],
'u_acceleration': u[0],
'u_steering_angle': u[1],
'pred_states': [x_log],
'pred_control': [u_log],
'computation_time': time.time() - start_time,
"kappa": self.curv_x0,
"curvature_radius": 1 / self.curv_x0
}
else:
if timestep % 6 == 0:
control, state, u, u_log, x_log, _ = self._vehicle_controller.mpc_control(
target_wps,
self._target_speed,
solve_nmpc=True,
log=log)
# Updating logging information of the logger
self.data_log = {
'X': state[0],
'Y': state[1],
'PSI': state[2],
'Velocity': state[3],
'Xi': state[4],
'Eta': state[5],
'Theta': state[6],
'u_acceleration': u[0],
'u_steering_angle': u[1],
'pred_states': [x_log],
'pred_control': [u_log],
'computation_time': time.time() - start_time,
"kappa": self.curv_x0,
"curvature_radius": 1 / self.curv_x0
}
else:
control, state, prediction, u = self._vehicle_controller.mpc_control(
target_wps,
self._target_speed,
solve_nmpc=False,
log=log)
# Updating logging information of the logger
self.data_log = {
'X': state[0],
'Y': state[1],
'PSI': state[2],
'Velocity': state[3],
'Xi': state[4],
'Eta': state[5],
'Theta': state[6],
'u_acceleration': u[0],
'u_steering_angle': u[1],
'pred_states': [prediction],
'pred_control': self.data_log.get("pred_control"),
'computation_time': time.time() - start_time,
"kappa": self.curv_x0,
"curvature_radius": 1 / self.curv_x0
}
else:
if timestep % 6 == 0:
control = self._vehicle_controller.mpc_control(
target_wps, self._target_speed, solve_nmpc=True, log=False)
else:
control, _, _, _ = self._vehicle_controller.mpc_control(
target_wps, self._target_speed, solve_nmpc=False, log=log)
# purge the queue of obsolete waypoints
vehicle_transform = self._vehicle.get_transform()
max_index = -1
for i, (waypoint, _) in enumerate(self._waypoint_buffer):
if distance_vehicle(waypoint,
vehicle_transform) < self._min_distance:
max_index = i
if max_index >= 0:
for i in range(max_index + 1):
self._waypoint_buffer.popleft()
if debug:
last_wp, _ = self._waypoint_buffer[len(self._waypoint_buffer) - 1]
draw_waypoints(self._vehicle.get_world(), [self.wp1, self.wp2],
self._vehicle.get_location().z + 1.0)
draw_waypoints_debug(self._vehicle.get_world(), [last_wp],
self._vehicle.get_location().z + 1.0,
color=(0, 255, 0))
return control, self.data_log, self.kappa_log if log else control
def is_valid_lane_change(road_option, world, proximity_threshold=5.9):
if road_option != RoadOption.CHANGELANELEFT and road_option != RoadOption.CHANGELANERIGHT:
print("ERROR: road option is not a lane change")
return False
player_waypoint = world.map.get_waypoint(world.player.get_location())
player_lane_id = player_waypoint.lane_id
player_transform = world.player.get_transform()
player_yaw = player_transform.rotation.yaw
vehicle_list = world.world.get_actors().filter('vehicle.*')
# Lane change left
if road_option == RoadOption.CHANGELANELEFT:
# check if lane change is valid
if not player_waypoint.lane_change & carla.LaneChange.Left:
#print("Traffic rules does not allow left lane change here")
return False
# Only look at vehicles in left adjecent lane
for vehicle in vehicle_list:
#vehicle = actorvehicle_list_list.find(vehicle_id)
vehicle_lane_id = world.map.get_waypoint(
vehicle.get_location()).lane_id
# Check if lane_id is in the same driving direction
if (player_lane_id < 0
and vehicle_lane_id < 0) or (player_lane_id > 0
and vehicle_lane_id > 0):
if abs(player_lane_id) - abs(vehicle_lane_id) == 1:
# check the vehicle|s proximity to the player
vehicle_waypoint = world.map.get_waypoint(
vehicle.get_location())
if distance_vehicle(
vehicle_waypoint,
player_transform) < proximity_threshold:
return False
# Lane change right
else:
# check if lane change is valid
if not player_waypoint.lane_change & carla.LaneChange.Right:
#print("Traffic rules does not allow right lane change here")
return False
# Only look for vehicles in right adjencent lane
for vehicle in vehicle_list:
#vehicle = vehicle_list.find(vehicle_id)
vehicle_lane_id = world.map.get_waypoint(
vehicle.get_location()).lane_id
# Check if lane_id is in the same driving direction
if (player_lane_id < 0
and vehicle_lane_id < 0) or (player_lane_id > 0
and vehicle_lane_id > 0):
if abs(player_lane_id) - abs(vehicle_lane_id) == -1:
# check the vehicle|s proximity to the player
vehicle_waypoint = world.map.get_waypoint(
vehicle.get_location())
if distance_vehicle(
vehicle_waypoint,
player_transform) < proximity_threshold:
return False
return True
def run_step(self, debug=True):
# not enough waypoints in the horizon? => add more!
if len(self._waypoints_queue) < int(
self._waypoints_queue.maxlen * 0.5):
if not self._global_plan:
self._compute_next_waypoints(k=100)
if len(self._waypoints_queue) == 0:
control = carla.VehicleControl()
control.steer = 0.0
control.throttle = 0.0
control.brake = 0.0
control.hand_brake = False
control.manual_gear_shift = False
return control
veh_location = self._vehicle.get_location() # type: carla.Location
veh_waypoint = self._map.get_waypoint(
veh_location) # type: carla.Waypoint
veh_yaw = self._vehicle.get_transform(
).rotation.yaw # TODO type: float, range TODO
local_plan = self.get_filled_waypoint_buffer(
) # type: List[carla.Waypoint]
# Calculate best waypoint to follow considering current yaw
L = 2.9
fx = veh_location.x + L * np.cos(veh_yaw)
fy = veh_location.y + L * np.sin(veh_yaw)
distances = []
for waypoint in local_plan:
wp = waypoint.transform.location
dx = fx - wp.x
dy = fy - wp.y
distance = np.sqrt(dx**2 + dy**2)
distances.append(distance)
target_idx = np.argmin(distances)
closest_error = distances[target_idx]
self._target_waypoint = local_plan[target_idx]
# Calculate path curvature
waypoints_to_look_ahead = 6
reference_waypoint = local_plan[target_idx + waypoints_to_look_ahead]
ref_location = reference_waypoint.transform.location
# delta_x = ref_location.x - veh_location.x
# delta_y = ref_location.y - veh_location.y
# theta_radians = math.atan2(delta_y, delta_x)
# FIXME Sometimes yaw oscilates from 179 to -179 which leads to temporarily bad calculations
distance, relative_angle = compute_magnitude_angle(
ref_location, veh_location,
veh_yaw) #np.rad2deg(theta_radians) - veh_yaw
# plt.cla()
# plt.plot([self._vehicle.get_transform().location.x, lookahead_waypoint.transform.location.x], [self._vehicle.get_transform().location.y, lookahead_waypoint.transform.location.y], "-r", label="debug")
# # plt.plot(, , ".b", label="lookahead")
# plt.axis("equal")
# plt.legend()
# plt.grid(True)
# plt.title("Rel angle: {}, yaw {}".format(str(angle), yaw))
# plt.pause(0.0001)
if abs(relative_angle) < 15:
target_speed = 50
elif abs(relative_angle) < 20:
target_speed = 40
elif abs(relative_angle) < 30:
target_speed = 30
else:
target_speed = 20
print(
'Relative angle to reference waypoint: {:3d} | Vehicle yaw angle: {:3d} | Target speed {} km/h'
.format(int(relative_angle), int(veh_yaw), target_speed))
control = self._vehicle_controller.run_step(target_speed,
self._target_waypoint)
# purge the queue of obsolete waypoints
vehicle_transform = self._vehicle.get_transform()
max_index = -1
# i, (waypoint, _)
for i, waypoint in enumerate(self._waypoint_buffer):
if distance_vehicle(waypoint,
vehicle_transform) < self._min_distance:
max_index = i
if max_index >= 0:
for i in range(max_index + 1):
self._waypoint_buffer.popleft()
if debug:
#draw_waypoints(self._vehicle.get_world(), [self._target_waypoint], self._vehicle.get_location().z + 1.0, color=carla.Color(0, 255, 0))
draw_waypoints(self._vehicle.get_world(), [reference_waypoint],
veh_location.z + 1.0)
return control
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.')
