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 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, 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: control to be applied
"""
if self._follow_speed_limits:
self._target_speed = self._vehicle.get_speed_limit()
# Add more waypoints too few in the horizon
if not self._stop_waypoint_creation and len(
self._waypoints_queue) < self._min_waypoint_queue_length:
self._compute_next_waypoints(k=self._min_waypoint_queue_length)
# Purge the queue of obsolete waypoints
veh_location = self._vehicle.get_location()
vehicle_speed = get_speed(self._vehicle) / 3.6
self._min_distance = self._base_min_distance + 0.5 * vehicle_speed
num_waypoint_removed = 0
for waypoint, _ in self._waypoints_queue:
if len(self._waypoints_queue) - num_waypoint_removed == 1:
min_distance = 1 # Don't remove the last waypoint until very close by
else:
min_distance = self._min_distance
if veh_location.distance(
waypoint.transform.location) < min_distance:
num_waypoint_removed += 1
else:
break
if num_waypoint_removed > 0:
for _ in range(num_waypoint_removed):
self._waypoints_queue.popleft()
# Get the target waypoint and move using the PID controllers. Stop if no target waypoint
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
else:
self.target_waypoint, self.target_road_option = self._waypoints_queue[
0]
control = self._vehicle_controller.run_step(
self._target_speed, self.target_waypoint)
if debug:
draw_waypoints(self._vehicle.get_world(), [self.target_waypoint],
1.0)
return control
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, debug=True, target_speed=None):
"""
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 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)
# Empty queue
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 len(self.waypoint_buffer) < self._buffer_size:
for i in range(self._buffer_size - len(self.waypoint_buffer)):
if self._waypoints_queue:
self.waypoint_buffer.append(
self._waypoints_queue.popleft())
else:
break
# Control Vehicle
# current vehicle waypoint
vehicle_transform = self._vehicle.get_transform()
self._current_waypoint = self._map.get_waypoint(
vehicle_transform.location)
# target waypoint
self.target_waypoint, self._target_road_option = self.waypoint_buffer[
0]
# move using PID controllers
_waypoints = [i for i, _ in self.waypoint_buffer]
waypoints = [[
points.transform.location.x, points.transform.location.y,
points.transform.rotation.yaw
] for points in _waypoints]
# print("waypoints: ", waypoints)
if target_speed is None:
target_speed = self._target_speed
control = self._vehicle_controller.run_step(target_speed, waypoints,
self.target_waypoint,
self._current_waypoint)
self.update_buffer()
# Draw waypoints
if debug:
draw_waypoints(self._vehicle.get_world(), [self.target_waypoint],
0.8)
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 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 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
