def update_information(self, world):
"""
This method updates the information regarding the ego
vehicle based on the surrounding world.
:param world: carla.world object
"""
self.speed = get_speed(self.vehicle)
self.speed_limit = world.player.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 = world.player.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 _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 run_step(self, target_speed, debug=False):
"""
Function used to execute one step of longitudinal control to reach a given target speed.
:param target_speed: target speed in Km/h
:param debug: debug control flag
:return:
"""
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
: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 car_following_manager(self,
vehicle,
distance,
extreme_weather,
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
"""
extreme_weather = debug
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 and not extreme_weather:
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 and not extreme_weather:
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 collision_and_car_avoid_manager(self, location, waypoint,
extreme_weather):
"""
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)
vehicle_list = [
v for v in vehicle_list if dist(v) < 45 and v.id != self.vehicle.id
]
if (extreme_weather and self.behavior.max_speed == 50
and random.randint(0, 2) < 1):
print("ignore vehicles")
vehicle_list = []
if self.direction == RoadOption.CHANGELANELEFT:
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:
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(self.behavior.min_proximity_threshold,
self.speed_limit / 3),
up_angle_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):
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