def run_step(self, vd, vehicle_ahead):
v = get_speed(self.vehicle)
if vehicle_ahead is None:
acc_cmd = self.a_max * (1 - (v / vd)**self.delta)
else:
loc1 = [
vehicle_ahead.get_location().x,
vehicle_ahead.get_location().y,
vehicle_ahead.get_location().z
]
loc2 = [
self.vehicle.get_location().x,
self.vehicle.get_location().y,
self.vehicle.get_location().z
]
d = euclidean_distance(loc1, loc2)
v2 = get_speed(vehicle_ahead)
dv = v - v2
d_star = self.d0 + max(
0, v * self.T + v * dv / (2 * math.sqrt(self.b * self.a_max)))
acc_cmd = self.a_max * (1 - (v / vd)**self.delta - (d_star / d)**2)
cmdSpeed = get_speed(self.vehicle) + acc_cmd * self.dt
# if self.vehicle.attributes['role_name'] == 'hero':
# print(v, cmdSpeed, acc_cmd)
# print([x * 3.6 for x in [cmdSpeed, acc_cmd, v, vd]])
return cmdSpeed
def _tailgating(self, location, waypoint, vehicle_list):
"""
This method is in charge of tailgating behaviors.
:param location: current location of the agent
:param waypoint: current waypoint of the agent
:param vehicle_list: list of all the nearby vehicles
"""
left_turn = waypoint.left_lane_marking.lane_change
right_turn = waypoint.right_lane_marking.lane_change
left_wpt = waypoint.get_left_lane()
right_wpt = waypoint.get_right_lane()
behind_vehicle_state, behind_vehicle, _ = self._bh_is_vehicle_hazard(waypoint, location, vehicle_list, max(
self.behavior.min_proximity_threshold, self.speed_limit / 2), up_angle_th=180, low_angle_th=160)
if behind_vehicle_state and self.speed < get_speed(behind_vehicle):
if (right_turn == carla.LaneChange.Right or right_turn ==
carla.LaneChange.Both) and waypoint.lane_id * right_wpt.lane_id > 0 and right_wpt.lane_type == carla.LaneType.Driving:
new_vehicle_state, _, _ = self._bh_is_vehicle_hazard(waypoint, location, vehicle_list, max(
self.behavior.min_proximity_threshold, self.speed_limit / 2), up_angle_th=180, lane_offset=1)
if not new_vehicle_state:
print("Tailgating, moving to the right!")
self.behavior.tailgate_counter = 200
self.set_destination(right_wpt.transform.location,
self.end_waypoint.transform.location, clean=True)
elif left_turn == carla.LaneChange.Left and waypoint.lane_id * left_wpt.lane_id > 0 and left_wpt.lane_type == carla.LaneType.Driving:
new_vehicle_state, _, _ = self._bh_is_vehicle_hazard(waypoint, location, vehicle_list, max(
self.behavior.min_proximity_threshold, self.speed_limit / 2), up_angle_th=180, lane_offset=-1)
if not new_vehicle_state:
print("Tailgating, moving to the left!")
self.behavior.tailgate_counter = 200
self.set_destination(left_wpt.transform.location,
self.end_waypoint.transform.location, clean=True)
def car_following_manager(self, vehicle, distance, debug=False):
"""
Module in charge of car-following behaviors when there's
someone in front of us.
:param vehicle: car to follow
:param distance: distance from vehicle
:param debug: boolean for debugging
:return control: carla.VehicleControl
"""
vehicle_speed = get_speed(vehicle)
delta_v = max(1, (self.speed - vehicle_speed) / 3.6)
ttc = distance / delta_v if delta_v != 0 else distance / np.nextafter(0., 1.)
# Under safety time distance, slow down.
if self.behavior.safety_time > ttc > 0.0:
control = self._local_planner.run_step(
target_speed=min(positive(vehicle_speed - self.behavior.speed_decrease),
min(self.behavior.max_speed, self.speed_limit - self.behavior.speed_lim_dist)), debug=debug)
# Actual safety distance area, try to follow the speed of the vehicle in front.
elif 2 * self.behavior.safety_time > ttc >= self.behavior.safety_time:
control = self._local_planner.run_step(
target_speed=min(max(self.min_speed, vehicle_speed),
min(self.behavior.max_speed, self.speed_limit - self.behavior.speed_lim_dist)), debug=debug)
# Normal behavior.
else:
control = self._local_planner.run_step(
target_speed= min(self.behavior.max_speed, self.speed_limit - self.behavior.speed_lim_dist), debug=debug)
return control
def state(self):
"""
frenet state of the ego vehicle.
:return: [X, Y, PSI, Velocity(m/s), Xi, Eta, Theta, last_u1, last_u2, kappa]
"""
vehicle_location = self._vehicle.get_location()
vehicle_transform = self._vehicle.get_transform()
angle_wp = wp.get_wp_angle(self._wp_current, self._wp_next)
angle_xy = wp.get_angle2wp_line(vehicle_transform, self._wp_current,
self._wp_next)
eta = -1 * np.sign(angle_xy) * wp.get_distance2wp(
vehicle_transform, self._wp_current, self._wp_next)
return np.array([
vehicle_location.x, # X
vehicle_location.y, # Y
wrap2pi(
np.deg2rad(round(self._vehicle.get_transform().rotation.yaw,
3))), # PSI [rad]
get_speed(self._vehicle) / 3.6, # Velocity [m/s]
0, # xi
eta, # eta
wrap2pi(
np.deg2rad(round(self._vehicle.get_transform().rotation.yaw,
3)) - angle_wp), # theta
self._last_control[0], # last u0
self._last_control[1], # last u1
# self._kr, # kappa
])
def run_step(self):
"""Execute one step of navigation."""
hazard_detected = False
# Retrieve all relevant actors
actor_list = self._world.get_actors()
vehicle_list = actor_list.filter("*vehicle*")
lights_list = actor_list.filter("*traffic_light*")
vehicle_speed = get_speed(self._vehicle) / 3.6
# Check for possible vehicle obstacles
max_vehicle_distance = self._base_vehicle_threshold + vehicle_speed
affected_by_vehicle, _, _ = self._vehicle_obstacle_detected(
vehicle_list, max_vehicle_distance)
if affected_by_vehicle:
hazard_detected = True
# Check if the vehicle is affected by a red traffic light
max_tlight_distance = self._base_tlight_threshold + vehicle_speed
affected_by_tlight, _ = self._affected_by_traffic_light(
lights_list, max_tlight_distance)
if affected_by_tlight:
hazard_detected = True
control = self._local_planner.run_step()
if hazard_detected:
control = self.add_emergency_stop(control)
return control
def _is_vehicle_close(self, vehicle_list):
"""
Check if a given vehicle is an obstacle in our way. To this end we take
into account the road and lane the target vehicle is on and run a
geometry test to check if the target vehicle is under a certain distance
in front of our ego vehicle.
WARNING: This method is an approximation that could fail for very large
vehicles, which center is actually on a different lane but their
extension falls within the ego vehicle lane.
:param vehicle_list: list of potential obstacle to check
:return: a tuple given by (bool_flag, vehicle), where
- bool_flag is True if there is a vehicle ahead blocking us
and False otherwise
- vehicle is the blocker object itself
"""
vehicle, distance = self.get_closest_vehicle_ahead(
vehicle_list,
max(10 * 1.17,
get_speed(self._vehicle) / 2.3) * 1.2)
if (vehicle != None):
return (True, vehicle)
return (False, None)
def update_information(self):
"""
This method updates the information regarding the ego
vehicle based on the surrounding world.
"""
self.speed = get_speed(self.vehicle)
self.speed_limit = self.vehicle.get_speed_limit()
self._local_planner.set_speed(self.speed_limit)
self.direction = self._local_planner.target_road_option
if self.direction is None:
self.direction = RoadOption.LANEFOLLOW
self.look_ahead_steps = int((self.speed_limit) / 10)
self.incoming_waypoint, self.incoming_direction = self._local_planner.get_incoming_waypoint_and_direction(
steps=self.look_ahead_steps)
if self.incoming_direction is None:
self.incoming_direction = RoadOption.LANEFOLLOW
self.is_at_traffic_light = self.vehicle.is_at_traffic_light()
if self.ignore_traffic_light:
self.light_state = "Green"
else:
# This method also includes stop signs and intersections.
self.light_state = str(self.vehicle.get_traffic_light_state())
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, target_speed):
"""
Execute one step of longitudinal control to reach a given target speed.
:param target_speed: target speed in m/s
:return: throttle control in the range [0, 1]
"""
current_speed = get_speed(self._vehicle)
return self._pid_control(target_speed, current_speed), current_speed
def run_step(self, target_speed, debug=False) -> np.ndarray:
"""
Execute one step of longitudinal control to reach a given target speed.
:param target_speed: target speed in Km/h
:return: throttle control in the range [0, 1]
"""
current_speed = get_speed(self._vehicle)
if debug:
print('Current speed = {}'.format(current_speed))
return self._pid_control(target_speed, current_speed)
def run_step(self, target_speed, debug=False):
"""
Execute one step of longitudinal control to reach a given target speed.
:param target_speed: target speed in Km/h
:param debug: boolean for debugging
:return: throttle control
"""
current_speed = get_speed(self._vehicle)
if debug:
print('Current speed = {}'.format(current_speed))
return self._pid_control(target_speed, current_speed)
def _take_action(self, action, sp):
mps_kmph_conversion = 3.6
target_speed = 0.0
# accelerate
if action == 0:
current_speed = get_speed(self.ego_vehicle) / mps_kmph_conversion
desired_speed = current_speed + 0.2
desired_speed *= 3.6
self.current_speed = desired_speed
self.planner.local_planner.set_speed(desired_speed)
control = self.planner.run_step()
control.brake = 0.0
self.ego_vehicle.apply_control(control)
# decelerate
elif action == 1:
current_speed = get_speed(self.ego_vehicle) / mps_kmph_conversion
desired_speed = current_speed - 0.2
desired_speed *= 3.6
self.current_speed = desired_speed
self.planner.local_planner.set_speed(desired_speed)
control = self.planner.run_step()
control.brake = 0.0
self.ego_vehicle.apply_control(control)
elif action == 2: # emergency stop
self.emergency_stop()
# speed tracking
elif action == 3:
self.planner.local_planner.set_speed(self.current_speed)
control = self.planner.run_step()
control.brake = 0.0
self.ego_vehicle.apply_control(control)
def speed_setting(self):
global target_vehicle
target_vehicle = None
ego_vehicle = self._vehicle
ego_vehicle_location = self._vehicle.get_location()
# ego_vehicle_waypoint = self._world.get_map().get_waypoint(ego_vehicle_location)
vehicle_list = self._world.get_actors().filter("*vehicle*")
car_number = len(vehicle_list)
if car_number > 1:
# if target_vehicle is None:
# for index in vehicle_list:
# if index.id != ego_vehicle.id:
# target_vehicle = index
# break
# else:
# pass
# else:
# target_vehicle_location = target_vehicle.get_location()
# # target_vehicle_waypoint = self._world.get_map().get_waypoint(target_vehicle_location)
# # if target_vehicle_waypoint.road_id == ego_vehicle_waypoint.road_id or\
# # target_vehicle_waypoint.lane_id == ego_vehicle_waypoint.lane_id:
# offset = abs(ego_vehicle_location.y - target_vehicle_location.y)
# if offset < 1.7:
# return get_speed(target_vehicle)
# else:
# return 60
# target_vehicle = vehicle_list[0]
for index in vehicle_list:
if index.id != ego_vehicle.id:
target_vehicle = index
break
else:
pass
# target_vehicle_waypoint = self._world.get_map().get_waypoint(target_vehicle_location)
# if target_vehicle_waypoint.road_id == ego_vehicle_waypoint.road_id or\
# target_vehicle_waypoint.lane_id == ego_vehicle_waypoint.lane_id:
target_vehicle_location = target_vehicle.get_location()
offset = abs(ego_vehicle_location.x - target_vehicle_location.x)
if offset < 1.77:
return get_speed(target_vehicle)
else:
return 20
else:
return 20
def state(self):
"""
xy-position state of the ego vehicle.
:return: [X, Y, PSI, Velocity(m/s), last_u1, last_u2]
"""
vehicle_location = self._vehicle.get_location()
return np.array([
vehicle_location.x, # X
vehicle_location.y, # Y
wrap2pi(
np.deg2rad(round(self._vehicle.get_transform().rotation.yaw,
3))), # PSI [rad]
get_speed(self._vehicle) / 3.6, # Velocity [m/s]
self._last_control[0], # last u0
self._last_control[1], # last u1
])
def collision_and_car_avoid_manager(self, location, waypoint):
"""
This module is in charge of warning in case of a collision
and managing possible overtaking or tailgating chances.
:param location: current location of the agent
:param waypoint: current waypoint of the agent
:return vehicle_state: True if there is a vehicle nearby, False if not
:return vehicle: nearby vehicle
:return distance: distance to nearby vehicle
"""
vehicle_list = self._world.get_actors().filter("*vehicle*")
def dist(v): return v.get_location().distance(waypoint.transform.location)
# og dist threshold is 45
vehicle_list = [v for v in vehicle_list if dist(v) < 25 and v.id != self.vehicle.id]
if self.direction == RoadOption.CHANGELANELEFT:
print('Change lane to left')
vehicle_state, vehicle, distance = self._bh_is_vehicle_hazard(
waypoint, location, vehicle_list, max(
self.behavior.min_proximity_threshold, self.speed_limit / 2), up_angle_th=180, lane_offset=-1)
elif self.direction == RoadOption.CHANGELANERIGHT:
print('Change lane to right')
vehicle_state, vehicle, distance = self._bh_is_vehicle_hazard(
waypoint, location, vehicle_list, max(
self.behavior.min_proximity_threshold, self.speed_limit / 2), up_angle_th=180, lane_offset=1)
else:
vehicle_state, vehicle, distance = self._bh_is_vehicle_hazard(
waypoint, location, vehicle_list, max( # increase up_angle_th to account for goal waypoint appear after turn
self.behavior.min_proximity_threshold, self.speed_limit / 3),up_angle_th=30) # up_angle_th=100) # og up th: 30)
# Check for overtaking
if vehicle_state and self.direction == RoadOption.LANEFOLLOW and \
not waypoint.is_junction and self.speed > 10 \
and self.behavior.overtake_counter == 0 and self.speed > get_speed(vehicle):
print("!! start over taking !!")
self._overtake(location, waypoint, vehicle_list)
# Check for tailgating
elif not vehicle_state and self.direction == RoadOption.LANEFOLLOW \
and not waypoint.is_junction and self.speed > 10 \
and self.behavior.tailgate_counter == 0:
self._tailgating(location, waypoint, vehicle_list)
return vehicle_state, vehicle, distance
def Ego_Perception(self):
"""
Currently, getting from the server directly
"""
self.ego_vehicle_location = self.ego_vehicle.get_location()
self.ego_vehicle_speed = get_speed(self.ego_vehicle)/3.6 ###### m/s
self.ego_vehicle_transform = self.ego_vehicle.get_transform()
self.dt = 0.05
self.speed_limit = self.ego_vehicle.get_speed_limit()
ego_vehicle_waypoint = self.map.get_waypoint(self.ego_vehicle_location)
if ego_vehicle_waypoint.is_junction:
self.in_intersection = True
else:
self.in_intersection = False
self.dt = self._clock.dt()
def _update_information(self):
"""
This method updates the information regarding the ego
vehicle based on the surrounding world.
"""
self._speed = get_speed(self._vehicle)
self._speed_limit = self._vehicle.get_speed_limit()
self._local_planner.set_speed(self._speed_limit)
self._direction = self._local_planner.target_road_option
if self._direction is None:
self._direction = RoadOption.LANEFOLLOW
self._look_ahead_steps = int((self._speed_limit) / 10)
self._incoming_waypoint, self._incoming_direction = self._local_planner.get_incoming_waypoint_and_direction(
steps=self._look_ahead_steps)
if self._incoming_direction is None:
self._incoming_direction = RoadOption.LANEFOLLOW
def run_step(self, target_speed, waypoint):
"""
Execute one step of control invoking both lateral and longitudinal PID controllers to reach a target waypoint
at a given target_speed.
:param target_speed: desired vehicle speed
:param waypoint: target location encoded as a waypoint
:return: distance (in meters) to the waypoint
"""
throttle = self._lon_controller.run_step(target_speed)
steering = self._lat_controller.run_step(waypoint)
control = carla.VehicleControl()
control.steer = steering
control.throttle = throttle
control.brake = 0.0 if target_speed + 10.0 > get_speed(self._vehicle) else 1.0
control.hand_brake = False
control.manual_gear_shift = False
return control
def run_step(self, target_speed, target_distance, debug=False):
"""
Execute one step of longitudinal control to reach a given target speed.
:param target_speed: target speed in Km/h
:return: throttle control in the range [0, 1]
"""
current_speed = get_speed(self._vehicle)
if self._front_vehicle is not None:
dx = self._vehicle.get_location().x - self._front_vehicle.get_location().x
dy = self._vehicle.get_location().y - self._front_vehicle.get_location().y
current_distance = math.sqrt(dx * dx + dy * dy)
if debug:
print('Current speed = {}'.format(current_speed))
return self._pid_control(target_speed, current_speed, target_distance, current_distance)
else:
return self._pid_control(target_speed, current_speed, 0.0, 0.0)
def _update_target_speed(self, hazard_detected, debug):
if hazard_detected:
self._set_target_speed(0)
return 0
MAX_PERCENTAGE_OF_SPEED_LIMIT = 0.75
speed_limit = self._vehicle.get_speed_limit() # km/h
current_speed = get_speed(self._vehicle)
new_target_speed = speed_limit * MAX_PERCENTAGE_OF_SPEED_LIMIT
use_custom_traffic_light_speed = False
if use_custom_traffic_light_speed:
TRAFFIC_LIGHT_SECONDS_AWAY = 3
METERS_TO_STOP_BEFORE_TRAFFIC_LIGHT = 8
get_traffic_light = self._vehicle.get_traffic_light(
) # type: carla.TrafficLight
nearest_traffic_light, distance = get_nearest_traffic_light(
self._vehicle) # type: carla.TrafficLight, float
distance_to_light = distance
distance -= METERS_TO_STOP_BEFORE_TRAFFIC_LIGHT
if nearest_traffic_light is None:
nearest_traffic_light = get_traffic_light
# Draw debug info
if debug and nearest_traffic_light is not None:
self._world.debug.draw_point(
nearest_traffic_light.get_transform().location,
size=1,
life_time=0.1,
color=carla.Color(255, 15, 15))
"""
if get_traffic_light is not None:
print("get_traffic_light: ", get_traffic_light.get_location() if get_traffic_light is not None else "None", " ", get_traffic_light.state if get_traffic_light is not None else "None")
if nearest_traffic_light is not None:
print("nearest_traffic_light: ", nearest_traffic_light.get_location() if nearest_traffic_light is not None else "None", " ", nearest_traffic_light.state if nearest_traffic_light is not None else "None")
"""
ego_vehicle_location = self._vehicle.get_location()
ego_vehicle_waypoint = self._map.get_waypoint(ego_vehicle_location)
self.is_affected_by_traffic_light = False
self.stopping_for_traffic_light = False
if ego_vehicle_waypoint.is_junction:
# It is too late. Do not block the intersection! Keep going!
pass
# Check if we should start braking
elif distance_to_light <= TRAFFIC_LIGHT_SECONDS_AWAY * new_target_speed / 3.6 and nearest_traffic_light is not None and nearest_traffic_light.state != carla.TrafficLightState.Green:
self.is_affected_by_traffic_light = True
brake_distance = current_speed / 3.6 * TRAFFIC_LIGHT_SECONDS_AWAY
print("TL distance: ", distance_to_light,
", distance (to stop): ", distance,
", distance travel 4 secs: ", brake_distance)
new_target_speed = self._target_speed
if distance <= 0:
new_target_speed = 0
self.stopping_for_traffic_light = True
print("Stopping before traffic light, distance ",
distance, "m")
elif brake_distance >= distance and brake_distance != 0:
percent_before_light = (brake_distance -
distance) / brake_distance
new_target_speed = speed_limit - max(
0, percent_before_light) * speed_limit
print("Slowing down before traffic light ",
percent_before_light * 100, "% ", new_target_speed,
" km/h")
self._set_target_speed(max(0, new_target_speed))
return new_target_speed
def run_step(self, debug=False):
"""
Execute one step of navigation.
:return: carla.VehicleControl
"""
# is there an obstacle in front of us?
hazard_detected = False
# retrieve relevant elements for safe navigation, i.e.: traffic lights
# and other vehicles
actor_list = self._world.get_actors() # type: ActorList
vehicle_list = actor_list.filter("*vehicle*") # type: List[Actor]
pedestrians_list = actor_list.filter("*walker.pedestrian*")
lights_list = actor_list.filter(
"*traffic_light*") # type: List[carla.TrafficLight]
if not self.drawn_lights and debug:
for light in lights_list:
self._world.debug.draw_box(
carla.BoundingBox(
light.trigger_volume.location +
light.get_transform().location,
light.trigger_volume.extent * 2),
carla.Rotation(0, 0, 0), 0.05, carla.Color(255, 128, 0, 0),
0)
self.drawn_lights = True
# check possible obstacles
vehicle_state, vehicle = self._is_vehicle_hazard(vehicle_list)
if vehicle_state:
if debug:
print('!!! VEHICLE BLOCKING AHEAD [{}])'.format(vehicle.id))
self._state = AgentState.BLOCKED_BY_VEHICLE
hazard_detected = True
# Check for pedestrians
pedestrian_state, pedestrian = self._is_pedestrian_hazard(
pedestrians_list)
if pedestrian_state:
if debug:
print('!!! PEDESTRIAN BLOCKING AHEAD [{}])'.format(
pedestrian.id))
self._state = AgentState.BLOCKED_BY_VEHICLE
hazard_detected = True
# check for the state of the traffic lights
light_state, traffic_light = self._is_light_red(lights_list)
if light_state:
if debug:
print('=== RED LIGHT AHEAD [{}])'.format(traffic_light.id))
self._state = AgentState.BLOCKED_RED_LIGHT
hazard_detected = True
new_target_speed = self._update_target_speed(hazard_detected, debug)
# if hazard_detected:
# control = self.emergency_stop()
# else:
# self._state = AgentState.NAVIGATING
# self.braking_intial_speed = None
# # standard local planner behavior
# control = self._local_planner.run_step(debug=debug)
# if self.stopping_for_traffic_light:
# control.steer = 0.0
self._state = AgentState.NAVIGATING
self.braking_intial_speed = None
# standard local planner behavior
control = self._local_planner.run_step(debug=debug)
if self.stopping_for_traffic_light:
control.steer = 0.0
# Prevent from steering randomly when stopped
if math.fabs(get_speed(self._vehicle)) < 0.1:
control.steer = 0
return control
def step(self, action=None):
state = [0, 0]
self.n_step += 1
track_finished = False
"""
**********************************************************************************************************************
*********************************************** Behavior Planner *****************************************************
**********************************************************************************************************************
"""
# self.MOBIL.run_step(self.f_state, self.traffic_module.actors_batch)
# change_lane = 0 # random.randint(-1, 1)
"""
**********************************************************************************************************************
*********************************************** Motion Planner *******************************************************
**********************************************************************************************************************
"""
temp = [self.ego.get_velocity(), self.ego.get_acceleration()]
speed = get_speed(self.ego)
acc_vec = self.ego.get_acceleration()
acc = math.sqrt(acc_vec.x**2 + acc_vec.y**2 + acc_vec.z**2)
psi = math.radians(self.ego.get_transform().rotation.yaw)
ego_state = [
self.ego.get_location().x,
self.ego.get_location().y, speed, acc, psi, temp, self.max_s
]
# fpath = self.motionPlanner.run_step_single_path(ego_state, self.f_idx, df_n=action[0], Tf=5, Vf_n=action[1])
fpath, fplist, best_path_idx = self.motionPlanner.run_step(
ego_state,
self.f_idx,
self.traffic_module.actors_batch,
target_speed=self.targetSpeed)
wps_to_go = len(
fpath.t
) - 3 # -2 bc len gives # of items not the idx of last item + 2wp controller is used
self.f_idx = 1
"""
**********************************************************************************************************************
************************************************* Controller *********************************************************
**********************************************************************************************************************
"""
# initialize flags
collision = track_finished = False
elapsed_time = lambda previous_time: time.time() - previous_time
path_start_time = time.time()
# follows path until end of WPs for max 1.8seconds
loop_counter = 0
while self.f_idx < wps_to_go:
loop_counter += 1
# for _ in range(wps_to_go):
# self.f_idx += 1
ego_location = [
self.ego.get_location().x,
self.ego.get_location().y,
math.radians(self.ego.get_transform().rotation.yaw)
]
self.f_idx = closest_wp_idx(ego_location, fpath, self.f_idx)
cmdSpeed = math.sqrt((fpath.s_d[self.f_idx])**2 +
(fpath.d_d[self.f_idx])**2)
cmdWP = [fpath.x[self.f_idx], fpath.y[self.f_idx]]
cmdWP2 = [fpath.x[self.f_idx + 1], fpath.y[self.f_idx + 1]]
# IDM for ego (Overwrite the desired speed)
vehicle_ahead = self.world_module.los_sensor.get_vehicle_ahead()
cmdSpeed = self.IDM.run_step(vd=cmdSpeed,
vehicle_ahead=vehicle_ahead)
# control = self.vehicleController.run_step(cmdSpeed, cmdWP) # calculate control
control = self.vehicleController.run_step_2_wp(
cmdSpeed, cmdWP, cmdWP2) # calculate control
self.ego.apply_control(control) # apply control
# print(fpath.s[self.f_idx], self.ego.get_transform().rotation.yaw)
"""
**********************************************************************************************************************
*********************************************** Draw Waypoints *******************************************************
**********************************************************************************************************************
"""
# three layer waypoints to draw:
self.world_module.points_to_draw = [{}, {}, {}]
if self.world_module.args.play_mode != 0:
for j, path in enumerate(fplist):
path_name = 'path {}'.format(j)
if j != best_path_idx:
layer = 0
color = 'COLOR_SKY_BLUE_0'
else:
layer = 1
color = 'COLOR_ALUMINIUM_0'
waypoints = []
for i in range(len(path.t)):
waypoints.append(
carla.Location(x=path.x[i], y=path.y[i]))
self.world_module.points_to_draw[layer][path_name] = {
'waypoints': waypoints,
'color': color
}
layer = 2
self.world_module.points_to_draw[layer]['ego'] = {
'waypoints': [self.ego.get_location()],
'color': 'COLOR_SCARLET_RED_0'
}
self.world_module.points_to_draw[layer]['waypoint ahead'] = {
'waypoints': [carla.Location(x=cmdWP[0], y=cmdWP[1])],
'color': 'COLOR_SCARLET_RED_0'
}
self.world_module.points_to_draw[layer]['waypoint ahead'] = {
'waypoints': [carla.Location(x=cmdWP2[0], y=cmdWP2[1])],
'color': 'COLOR_SCARLET_RED_0'
}
"""
**********************************************************************************************************************
************************************************ Update Carla ********************************************************
**********************************************************************************************************************
"""
ego_s, ego_d = fpath.s[self.f_idx], fpath.d[self.f_idx]
state = [ego_s, ego_d]
self.module_manager.tick() # Update carla world
if self.auto_render:
self.render()
collision_hist = self.world_module.get_collision_history()
if any(collision_hist):
collision = True
break
distance_traveled = ego_s - self.init_s
if distance_traveled < -5:
distance_traveled = self.max_s + distance_traveled
if distance_traveled >= self.track_length:
track_finished = True
break
if loop_counter >= self.loop_break:
break
self.f_state = [
fpath.s[self.f_idx], fpath.d[self.f_idx], fpath.s_d[self.f_idx],
fpath.d_d[self.f_idx], fpath.s_dd[self.f_idx],
fpath.d_dd[self.f_idx]
]
"""
**********************************************************************************************************************
********************************************* Episode Termination ****************************************************
**********************************************************************************************************************
"""
done = False
if collision:
# print('Collision happened!')
reward = -10
done = True
# print('eps rew: ', self.n_step, self.eps_rew)
return state, reward, done, {'reserved': 0}
if track_finished:
# print('Finished the race')
reward = 10
done = True
# print('eps rew: ', self.n_step, self.eps_rew)
return state, reward, done, {'reserved': 0}
reward = 1
return state, reward, done, {'reserved': 0}
def step(self, action=None):
self.n_step += 1
self.actor_enumerated_dict['EGO'] = {
'NORM_S': [],
'NORM_D': [],
'S': [],
'D': [],
'SPEED': []
}
if self.verbosity:
print(
'ACTION'.ljust(15),
'{:+8.6f}, {:+8.6f}'.format(float(action[0]),
float(action[1])))
if self.is_first_path: # Episode start is bypassed
action = [0, -1]
self.is_first_path = False
"""
**********************************************************************************************************************
*********************************************** Motion Planner *******************************************************
**********************************************************************************************************************
"""
temp = [self.ego.get_velocity(), self.ego.get_acceleration()]
init_speed = speed = get_speed(self.ego)
acc_vec = self.ego.get_acceleration()
acc = math.sqrt(acc_vec.x**2 + acc_vec.y**2 + acc_vec.z**2)
psi = math.radians(self.ego.get_transform().rotation.yaw)
ego_state = [
self.ego.get_location().x,
self.ego.get_location().y, speed, acc, psi, temp, self.max_s
]
fpath, self.lanechange, off_the_road = self.motionPlanner.run_step_single_path(
ego_state, self.f_idx, df_n=action[0], Tf=5, Vf_n=action[1])
wps_to_go = len(
fpath.t
) - 3 # -2 bc len gives # of items not the idx of last item + 2wp controller is used
self.f_idx = 1
"""
**********************************************************************************************************************
************************************************* Controller *********************************************************
**********************************************************************************************************************
"""
# initialize flags
collision = track_finished = False
elapsed_time = lambda previous_time: time.time() - previous_time
path_start_time = time.time()
ego_init_d, ego_target_d = fpath.d[0], fpath.d[-1]
# follows path until end of WPs for max 1.5 * path_time or loop counter breaks unless there is a langechange
loop_counter = 0
while self.f_idx < wps_to_go and (
elapsed_time(path_start_time) < self.motionPlanner.D_T * 1.5
or loop_counter < self.loop_break or self.lanechange):
loop_counter += 1
ego_state = [
self.ego.get_location().x,
self.ego.get_location().y,
math.radians(self.ego.get_transform().rotation.yaw), 0, 0,
temp, self.max_s
]
self.f_idx = closest_wp_idx(ego_state, fpath, self.f_idx)
cmdWP = [fpath.x[self.f_idx], fpath.y[self.f_idx]]
cmdWP2 = [fpath.x[self.f_idx + 1], fpath.y[self.f_idx + 1]]
# overwrite command speed using IDM
ego_s = self.motionPlanner.estimate_frenet_state(
ego_state, self.f_idx)[0] # estimated current ego_s
ego_d = fpath.d[self.f_idx]
vehicle_ahead = self.get_vehicle_ahead(ego_s, ego_d, ego_init_d,
ego_target_d)
cmdSpeed = self.IDM.run_step(vd=self.targetSpeed,
vehicle_ahead=vehicle_ahead)
# control = self.vehicleController.run_step(cmdSpeed, cmdWP) # calculate control
control = self.vehicleController.run_step_2_wp(
cmdSpeed, cmdWP, cmdWP2) # calculate control
self.ego.apply_control(control) # apply control
"""
**********************************************************************************************************************
*********************************************** Draw Waypoints *******************************************************
**********************************************************************************************************************
"""
if self.world_module.args.play_mode != 0:
for i in range(len(fpath.t)):
self.world_module.points_to_draw['path wp {}'.format(
i)] = [
carla.Location(x=fpath.x[i], y=fpath.y[i]),
'COLOR_ALUMINIUM_0'
]
self.world_module.points_to_draw['ego'] = [
self.ego.get_location(), 'COLOR_SCARLET_RED_0'
]
self.world_module.points_to_draw[
'waypoint ahead'] = carla.Location(x=cmdWP[0], y=cmdWP[1])
self.world_module.points_to_draw[
'waypoint ahead 2'] = carla.Location(x=cmdWP2[0],
y=cmdWP2[1])
"""
**********************************************************************************************************************
************************************************ Update Carla ********************************************************
**********************************************************************************************************************
"""
self.module_manager.tick() # Update carla world
if self.auto_render:
self.render()
collision_hist = self.world_module.get_collision_history()
self.actor_enumerated_dict['EGO']['S'].append(ego_s)
self.actor_enumerated_dict['EGO']['D'].append(ego_d)
self.actor_enumerated_dict['EGO']['NORM_S'].append(
(ego_s - self.init_s) / self.track_length)
self.actor_enumerated_dict['EGO']['NORM_D'].append(
round((ego_d + self.LANE_WIDTH) / (3 * self.LANE_WIDTH), 2))
last_speed = get_speed(self.ego)
self.actor_enumerated_dict['EGO']['SPEED'].append(last_speed /
self.maxSpeed)
# if ego off-the road or collided
if any(collision_hist):
collision = True
break
distance_traveled = ego_s - self.init_s
if distance_traveled < -5:
distance_traveled = self.max_s + distance_traveled
if distance_traveled >= self.track_length:
track_finished = True
"""
*********************************************************************************************************************
*********************************************** RL Observation ******************************************************
*********************************************************************************************************************
"""
if cfg.GYM_ENV.FIXED_REPRESENTATION:
self.state = self.fix_representation()
if self.verbosity == 2:
print(3 * '---EPS UPDATE---')
print(
TENSOR_ROW_NAMES[0].ljust(15),
# '{:+8.6f} {:+8.6f}'.format(self.state[-1][1], self.state[-1][0]))
'{:+8.6f}'.format(self.state[-1][0]))
for idx in range(1, self.state.shape[1]):
print(TENSOR_ROW_NAMES[idx].ljust(15),
'{:+8.6f}'.format(self.state[-1][idx]))
if self.verbosity == 3: print(self.state)
"""
**********************************************************************************************************************
********************************************* RL Reward Function *****************************************************
**********************************************************************************************************************
"""
e_speed = abs(self.targetSpeed - last_speed)
r_speed = self.w_r_speed * np.exp(
-e_speed**2 / self.maxSpeed *
self.w_speed) # 0<= r_speed <= self.w_r_speed
# first two path speed change increases regardless so we penalize it differently
spd_change_percentage = (
last_speed - init_speed) / init_speed if init_speed != 0 else -1
r_laneChange = 0
if self.lanechange and spd_change_percentage < self.min_speed_gain:
r_laneChange = -1 * r_speed * self.lane_change_penalty # <= 0
elif self.lanechange:
r_speed *= self.lane_change_reward
positives = r_speed
negatives = r_laneChange
reward = positives + negatives # r_speed * (1 - lane_change_penalty) <= reward <= r_speed * lane_change_reward
# print(self.n_step, self.eps_rew)
"""
**********************************************************************************************************************
********************************************* Episode Termination ****************************************************
**********************************************************************************************************************
"""
done = False
if collision:
# print('Collision happened!')
reward = self.collision_penalty
done = True
self.eps_rew += reward
# print('eps rew: ', self.n_step, self.eps_rew)
if self.verbosity:
print('REWARD'.ljust(15), '{:+8.6f}'.format(reward))
return self.state, reward, done, {'reserved': 0}
elif track_finished:
# print('Finished the race')
# reward = 10
done = True
if off_the_road:
reward = self.off_the_road_penalty
self.eps_rew += reward
# print('eps rew: ', self.n_step, self.eps_rew)
if self.verbosity:
print('REWARD'.ljust(15), '{:+8.6f}'.format(reward))
return self.state, reward, done, {'reserved': 0}
elif off_the_road:
# print('Collision happened!')
reward = self.off_the_road_penalty
# done = True
self.eps_rew += reward
# print('eps rew: ', self.n_step, self.eps_rew)
if self.verbosity:
print('REWARD'.ljust(15), '{:+8.6f}'.format(reward))
return self.state, reward, done, {'reserved': 0}
self.eps_rew += reward
# print(self.n_step, self.eps_rew)
if self.verbosity: print('REWARD'.ljust(15), '{:+8.6f}'.format(reward))
return self.state, reward, done, {'reserved': 0}
def _model_predictive_control(self, target_speed, waypoints,
vehicle_transform):
# Transform points from world frame to car frame
_x = vehicle_transform.location.x
_y = vehicle_transform.location.y
# _psi = vehicle_transform.rotation.yaw
_psi = self.get_psi(vehicle_transform)
R = np.array([[cos(-_psi), -sin(-_psi)], [sin(-_psi), cos(-_psi)]])
t = np.array([[_x], [_y]])
waypoints_world = np.array(waypoints)
waypoints_car = np.array(waypoints)
waypoints_car[:, 0:2] = np.dot(
R,
np.array([waypoints_world[:, 0], waypoints_world[:, 1]]) - t).T
N = 10
dt = 0.1
Lf = 2.67
ref_v = target_speed
# Define var start positions
x_start = 0
y_start = x_start + N
psi_start = y_start + N
v_start = psi_start + N
cte_start = v_start + N
epsi_start = cte_start + N
delta_start = epsi_start + N
a_start = delta_start + N - 1
# State
x = 0
y = 0
psi = 0
v = get_speed(self._vehicle)
cte = self.get_cross_track_error([x, y], waypoints_car)
epsi = self.get_epsi(vehicle_transform, waypoints_car)
coeffs = self.get_coeffs(waypoints_car)
# number of model variables
# For example: If the [state] is a 4 element vector, the [actuators] is a 2
# element vector and there are 10 timesteps. The number of variables is:
n_vars = N * 6 + (N - 1) * 2
# Set the number of constraints
n_constraints = N * 6
# NLP variable vector
vars = MX.sym('x', n_vars)
vars_init = np.zeros(n_vars)
# set initial variables values
vars_init[x_start] = x
vars_init[y_start] = y
vars_init[psi_start] = psi
vars_init[v_start] = v
vars_init[cte_start] = cte
vars_init[epsi_start] = epsi
# upperbound and lowerbound vectors for vars
vars_upperbound = np.zeros(n_vars)
vars_lowerbound = np.zeros(n_vars)
# Set all non-actuators upper and lowerlimits
# to the max negative and positive values.
vars_upperbound[:delta_start] = 1.0e9
vars_lowerbound[:delta_start] = -1.0e9
# Set the upper and lower limits of delta as -25 and 25 degrees
vars_upperbound[delta_start:a_start] = 0.7
vars_lowerbound[delta_start:a_start] = -0.7
# Set the upper and lower limits of accelerations as -1 and 1
vars_upperbound[a_start:] = 0.7
vars_lowerbound[a_start:] = -0.7
# Lower and upper limits for the constraints
# Should be 0 besides initial state.
constraints_upperbound = np.zeros(n_constraints)
constraints_lowerbound = np.zeros(n_constraints)
constraints_lowerbound[x_start] = x
constraints_lowerbound[y_start] = y
constraints_lowerbound[psi_start] = psi
constraints_lowerbound[v_start] = v
constraints_lowerbound[cte_start] = cte
constraints_lowerbound[epsi_start] = epsi
constraints_upperbound[x_start] = x
constraints_upperbound[y_start] = y
constraints_upperbound[psi_start] = psi
constraints_upperbound[v_start] = v
constraints_upperbound[cte_start] = cte
constraints_upperbound[epsi_start] = epsi
# Object for defining objective and constraints
f, g = self.operator(vars, coeffs, n_constraints, N, dt, ref_v, Lf)
# NLP
nlp = {'x': vars, 'f': f, 'g': vertcat(*g)}
# print("g shape:", vertcat(*g).shape)
## ----
## SOLVE THE NLP
## ----
# Set options
opts = {}
opts["expand"] = True
#opts["ipopt.max_iter"] = 4
opts["ipopt.linear_solver"] = 'ma27'
opts["ipopt.print_level"] = 0
opts["ipopt.sb"] = "yes"
opts["print_time"] = 0
# Allocate an NLP solver
solver = nlpsol("solver", "ipopt", nlp, opts)
arg = {}
# Initial condition
arg["x0"] = vars_init
# print("x0 shape: ", vars_init.shape)
# Bounds on x
arg["lbx"] = vars_lowerbound
arg["ubx"] = vars_upperbound
# print("lbx shape: ", vars_lowerbound.shape)
# Bounds on g
arg["lbg"] = constraints_lowerbound
arg["ubg"] = constraints_upperbound
# print("ubg: ", constraints_upperbound.shape)
# Solve the problem
res = solver(**arg)
vars_opt = np.array(res["x"])
x_mpc = vars_opt[x_start:y_start]
y_mpc = vars_opt[y_start:psi_start]
steering = vars_opt[delta_start:a_start]
accelerations = vars_opt[a_start:]
# print("steering: ", steering)
# print("accelerations: ", accelerations)
throttle_output = accelerations * 0
brake_output = accelerations * 0
for i in range(N - 1):
if accelerations[i] > 0:
throttle_output[i] = accelerations[i]
brake_output[i] = 0
else:
throttle_output[i] = 0
brake_output[i] = -accelerations[i]
steer_output = steering
'''
print("================= MPC ======================")
print("steer_output: ", steer_output[0])
print("throttle_output: ", throttle_output[0])
print("brake_output: ", brake_output[0])
print("============================================")
print(" ")
'''
return throttle_output[0], brake_output[0], steer_output[0]
def get_ego_speed(self):
return get_speed(self.ego_vehicle)
def run_step(self, debug):
"""
Execute one step of navigation.
:Check for lights, junction block and proximity block
:return: carla.VehicleControl
"""
global time_count
global lane_change
global next_lane_waypoint
# is there an obstacle in front of us?
hazard_detected = False
# retrieve relevant elements for safe navigation, i.e.: traffic lights
# and other vehicles, static props (occupancy grid maps)
actor_list = self._world.get_actors()
vehicle_list = actor_list.filter("*vehicle*")
lights_list = actor_list.filter("*traffic_light*")
prop_list = actor_list.filter("*static.prop.streetbarrier*")
if (len(prop_list) == 0
and len(actor_list.filter("*static.prop.container*")) != 0):
prop_list = actor_list.filter("*static.prop.container*")
#print(f'=============== prop list {prop_list}')
#Get location and current waypoint
location = self.vehicle.get_location()
waypoint = self.world_map.get_waypoint(location)
# check possible longitudinal and lateral obstacles
vehicle_state, vehicle = self._is_vehicle_hazard(vehicle_list)
if vehicle_state and not lane_change and not self.static_lane_change:
if debug:
print('!!! VEHICLE BLOCKING AHEAD [{}])'.format(vehicle.id))
time_count = time_count + 1
if (time_count > self._blocking_threshold):
print(
f'------- Blocked more than threshold Perform lane change manouver ---- {time_count}'
)
print(f'current location {location}')
lane_change = True
next_lane_waypoint = waypoint.get_left_lane()
print(
f'Left lane--------{waypoint.get_left_lane().transform.location.x}-------{waypoint.get_left_lane().transform.location.y}'
)
self._state = AgentState.BLOCKED_BY_VEHICLE
hazard_detected = True
# check for the state of the traffic lights
light_state, traffic_light = self._is_light_red(lights_list)
if light_state:
if debug:
print('=== RED LIGHT AHEAD [{}])'.format(traffic_light.id))
self._state = AgentState.BLOCKED_RED_LIGHT
hazard_detected = True
#Check the if there is a junction class between ego and another vehicle
#If there is a junction class check for priority
if (not self.vehicle_junction_hazard):
#Return True only if another vehicle has higher priority
self.vehicle_junction_hazard, self.vehicle_crossing = self._is_junction_hazard(
vehicle_list)
#Check if the priority vehicle is still in junction
if self.vehicle_junction_hazard and self.world_map.get_waypoint(
self.vehicle_crossing.get_location()).is_junction:
self._state = AgentState.BLOCKED_IN_JUNCTION
if debug:
print(
f'=== OTHER VEHICLE CROSSING JUNCTION {self.vehicle_crossing.id}'
)
control = self.emergency_stop()
return control
else:
self.vehicle_junction_hazard = False
self.vehicle_crossing = None
#Check for static obstacles on the route
if (len(prop_list) != 0) and not self.static_lane_change:
self.static_obstacle, prop = self._is_static_obstacle_ahead(
prop_list)
if self.static_obstacle:
print(
f'=== VEHICLE BLOCKED BY STATIC OBSTACLE======={prop.id}')
next_lane_waypoint = waypoint.get_left_lane()
print(f'next lane id ========== {next_lane_waypoint.lane_id}')
print(f'current lane id ========== {waypoint.lane_id}')
print(
f'Next waypoint left lane=========={next_lane_waypoint.transform}'
)
#Check if there is a vehicle within n meters
self.static_lane_change = self._is_next_lane_clear(
next_lane_waypoint, vehicle_list)
print(
f'Lane clearance value==========={self.static_lane_change}'
)
#In case lane change clearance is not available stay stopped
if not self.static_lane_change:
print(f'I am STOPPED')
control = self.emergency_stop()
return control
if hazard_detected and not lane_change:
control = self.emergency_stop()
elif lane_change:
print(f'current transform {self.vehicle.get_transform()}')
vehicle_transform = self.vehicle.get_transform()
print(
f'current location {next_lane_waypoint.lane_id}===={waypoint.lane_id}'
)
print(f'next lane ============= {next_lane_waypoint.transform}')
while (next_lane_waypoint.lane_id != waypoint.lane_id or abs(
abs(int(vehicle_transform.rotation.yaw)) -
abs(int(next_lane_waypoint.transform.rotation.yaw))) > 2):
#Observation variables
kmh = get_speed(self.vehicle)
vehicle_transform = self.vehicle.get_transform()
yaw_vehicle = vehicle_transform.rotation.yaw
delta_yaw = abs(
abs(int(vehicle_transform.rotation.yaw)) -
abs(int(next_lane_waypoint.transform.rotation.yaw)))
state = [kmh, yaw_vehicle, delta_yaw]
state = np.reshape(state, (1, 3))
choice = self.left_lane_model.predict(state)
action = self.choose_action_leftlanechange(np.argmax(choice))
print(f"action lane change ------------------> {action}")
control = carla.VehicleControl(throttle=action[0],
steer=action[1],
brake=action[2],
reverse=action[3])
return control
lane_change = False
control = self.nn_control()
time_count = 0
#For subsequent steer around lane change maneuver in case of static obstacle
elif self.static_lane_change:
while (waypoint.lane_id != next_lane_waypoint.lane_id
or self.vehicle.get_transform().rotation.yaw < -1.5
) and not self.first_lane_reached:
print(f'=======Left Lane Change=====')
kmh = get_speed(self.vehicle)
vehicle_transform = self.vehicle.get_transform()
yaw_vehicle = vehicle_transform.rotation.yaw
delta_yaw = abs(
abs(int(179 + vehicle_transform.rotation.yaw)) -
abs(int(next_lane_waypoint.transform.rotation.yaw)))
lane_id = waypoint.lane_id
state = [kmh, yaw_vehicle, lane_id, delta_yaw]
state = np.reshape(state, (1, 4))
choice = self.left_lane_static.predict(state)
action = self.choose_action_leftlanechange_static(
np.argmax(choice))
control = carla.VehicleControl(throttle=action[0],
steer=action[1],
brake=action[2],
reverse=action[3])
return control
self.first_lane_reached = True
next_lane_waypoint = waypoint.get_right_lane()
while (waypoint.lane_id != -1
or self.vehicle.get_transform().rotation.yaw > 1):
print(f'=======Right Lane Change=====')
kmh = get_speed(self.vehicle)
vehicle_transform = self.vehicle.get_transform()
yaw_vehicle = vehicle_transform.rotation.yaw
delta_yaw = abs(
abs(int(vehicle_transform.rotation.yaw)) -
abs(int(next_lane_waypoint.transform.rotation.yaw)))
lane_id = waypoint.lane_id
print(f'current transform {self.vehicle.get_transform()}')
state = [kmh, yaw_vehicle, lane_id, delta_yaw]
state = np.reshape(state, (1, 4))
choice = self.right_lane_static.predict(state)
action = self.choose_action_rightlanechange_static(
np.argmax(choice))
if (waypoint.lane_id == -1):
action[1] = -0.14
control = carla.VehicleControl(throttle=action[0],
steer=action[1],
brake=action[2],
reverse=action[3])
return control
self.static_obstacle = False
self.static_lane_change = False
control = self.nn_control()
else:
#print(f'current transform {self.vehicle.get_transform()}')
self._state = AgentState.NAVIGATING
# standard local planner behavior
control = self.nn_control()
return control
def nn_control(self):
#Retrieve the current option from
if self.route_count < len(self.route_list):
option = self.route_list[self.route_count][0]
target_loc = self.route_list[self.route_count][2]
curr_loc = (self.vehicle.get_location().x,
self.vehicle.get_location().y)
target_diff = target_magnitude(curr_loc, target_loc)
#print(f'target difference ======={option}=========== {round(target_diff)}========{self.prev_target_diff}')
if option == 'straight':
#get_speed(self.vehicle)
kmh = get_speed(self.vehicle)
target_location = carla.Location(x=float(target_loc[0]),
y=float(target_loc[1]))
vehicle_waypoint = self.world_map.get_waypoint(
self.vehicle.get_location())
current_location = vehicle_waypoint.transform.location
norm_target = self.compute_magnitude(target_location,
current_location)
obs = [kmh, norm_target, 0, 15]
obs = np.reshape(obs, (1, 4))
choice = self.straight_model.predict(obs)
action = self.choose_action_straight(np.argmax(choice[0]))
control = carla.VehicleControl(throttle=action[0],
steer=action[1],
brake=action[2],
reverse=action[3])
#control = carla.VehicleControl(throttle=0.6, steer=0.0, brake=0.0, reverse=False)
if option == 'right_turn':
kmh = get_speed(self.vehicle)
target_location = carla.Location(x=float(target_loc[0]),
y=float(target_loc[1]))
target_waypoint = self.world_map.get_waypoint(target_location)
target_yaw = target_waypoint.transform.rotation.yaw
vehicle_waypoint = self.world_map.get_waypoint(
self.vehicle.get_location())
current_location = vehicle_waypoint.transform.location
current_yaw = vehicle_waypoint.transform.rotation.yaw
norm_target, diff_angle = self.compute_magnitude_angle(
target_location, current_location, target_yaw, current_yaw)
state = [norm_target, diff_angle, 0, kmh]
state = np.reshape(state, (1, 4))
choice = self.right_turn_model.predict(state)
action = self.choose_action_rightturn(np.argmax(choice))
#print(f'action right turn ------> {action}--------target yaw==={target_yaw}--current yaw===={current_yaw}')
control = carla.VehicleControl(throttle=action[0],
steer=action[1],
brake=action[2],
reverse=action[3])
#control = carla.VehicleControl(throttle=0.5, steer=0.2, brake=0.0, reverse=False)
if option == 'left_turn':
kmh = get_speed(self.vehicle)
target_location = carla.Location(x=float(target_loc[0]),
y=float(target_loc[1]))
target_waypoint = self.world_map.get_waypoint(target_location)
target_yaw = target_waypoint.transform.rotation.yaw
vehicle_waypoint = self.world_map.get_waypoint(
self.vehicle.get_location())
current_location = vehicle_waypoint.transform.location
current_yaw = vehicle_waypoint.transform.rotation.yaw
norm_target, diff_angle = self.compute_magnitude_angle(
target_location, current_location, target_yaw, current_yaw)
state = [norm_target, diff_angle, 0, kmh]
state = np.reshape(state, (1, 4))
choice = self.left_turn_model.predict(state)
action = self.choose_action_leftturn(np.argmax(choice))
#print(f'action right turn ------> {action}--------target yaw==={target_yaw}--current yaw===={current_yaw}')
control = carla.VehicleControl(throttle=action[0],
steer=action[1],
brake=action[2],
reverse=action[3])
if (round(target_diff) <= 1
or round(target_diff) > self.prev_target_diff):
self.route_count = self.route_count + 1
self.prev_target_diff = 200
print(f'Count increased =====')
else:
self.prev_target_diff = round(target_diff)
else:
control = self.emergency_stop()
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 game_loop(options_dict):
world = None
try:
client = carla.Client('localhost', 2000)
client.set_timeout(60.0)
print('Changing world to Town 5')
client.load_world('Town05')
world = World(client.get_world())
world = World(client.get_world())
spawn_points = world.world.get_map().get_spawn_points()
vehicle_bp = 'model3'
vehicle_transform = spawn_points[options_dict['spawn_point_indices']
[0]]
vehicle = Car(vehicle_bp, vehicle_transform, world)
agent = agent = BasicAgent(vehicle.vehicle)
destination_point = spawn_points[options_dict['spawn_point_indices']
[1]].location
print('Going to ', destination_point)
agent.set_destination(
(destination_point.x, destination_point.y, destination_point.z))
camera_bp = [
'sensor.camera.rgb', 'sensor.camera.depth', 'sensor.lidar.ray_cast'
]
camera_transform = [
carla.Transform(carla.Location(x=1.5, z=2.4),
carla.Rotation(pitch=-15, yaw=40)),
carla.Transform(carla.Location(x=1.5, z=2.4),
carla.Rotation(pitch=-15, yaw=-40)),
carla.Transform(carla.Location(x=1.5, z=2.4))
]
cam1 = Camera(camera_bp[0], camera_transform[0], vehicle)
cam2 = Camera(camera_bp[0], camera_transform[1], vehicle)
# depth1 = Camera(camera_bp[1], camera_transform[0], vehicle)
# depth2 = Camera(camera_bp[1], camera_transform[1], vehicle)
lidar = Lidar(camera_bp[2], camera_transform[2], vehicle)
prev_location = vehicle.vehicle.get_location()
sp = 2
file = open("control_input.txt", "a")
while True:
world.world.tick()
world_snapshot = world.world.wait_for_tick(60.0)
if not world_snapshot:
continue
control = agent.run_step()
vehicle.vehicle.apply_control(control)
w = str(world_snapshot.frame_count) + ',' + str(
control.steer) + ',' + str(get_speed(vehicle.vehicle)) + '\n'
file.write(w)
current_location = vehicle.vehicle.get_location()
if current_location.distance(
prev_location) <= 0.0 and current_location.distance(
destination_point) <= 10:
print('distance from destination: ',
current_location.distance(destination_point))
if len(options_dict['spawn_point_indices']) <= sp:
break
else:
destination_point = spawn_points[
options_dict['spawn_point_indices'][sp]].location
print('Going to ', destination_point)
agent.set_destination(
(destination_point.x, destination_point.y,
destination_point.z))
sp += 1
prev_location = current_location
finally:
if world is not None:
world.destroy()
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
