1from typing import TYPE_CHECKING
2
3import carla
4import numpy as np
5
6if TYPE_CHECKING:
7 from agents.tools.config_creation import BehaviorAgentSettings, LunaticAgentSettings
8 from classes.type_protocols import HasPlannerWithConfig
9
10
11from agents.tools.misc import get_speed
12
13__epsilon = np.nextafter(0., 1.) # to not divide by 0
14
15
[docs]
16def car_following_manager(self: "HasPlannerWithConfig[BehaviorAgentSettings | LunaticAgentSettings]",
17 vehicle: carla.Vehicle,
18 distance: float,
19 debug: bool = False) -> carla.VehicleControl:
20 """
21 Module in charge of car-following behaviors when there's
22 someone in front of us.
23
24 Parameters:
25 vehicle: car to follow
26 distance: distance from vehicle
27 debug: boolean for debugging
28
29 Returns:
30 carla.VehicleControl:
31 Calculates the control for the vehicle
32
33 Assumes:
34 No control has been calculated in this tick.
35 """
36
37 vehicle_speed = get_speed(vehicle)
38 delta_v = max(1, (self.config.live_info.current_speed - vehicle_speed) / 3.6)
39 ttc = (distance / delta_v if delta_v != 0 # TimeTillCollision
40 else distance / __epsilon # do not divide by 0,
41 )
42
43 # Under safety time distance, slow down.
44 if self.config.speed.safety_time > ttc > 0.0:
45 target_speed = min([
46 max(0.0, vehicle_speed - self.config.speed.speed_decrease),
47 self.config.speed.max_speed,
48 self.config.live_info.current_speed_limit - self.config.speed.speed_lim_dist])
49 self._local_planner.set_speed(target_speed)
50 control = self._calculate_control(debug=debug)
51
52 # Actual safety distance area, try to follow the speed of the vehicle in front.
53 elif 2 * self.config.speed.safety_time > ttc >= self.config.speed.safety_time:
54 target_speed = min([
55 max(self.config.speed.min_speed, vehicle_speed),
56 self.config.speed.max_speed,
57 self.config.live_info.current_speed_limit - self.config.speed.speed_lim_dist])
58 self._local_planner.set_speed(target_speed)
59 control = self._calculate_control(debug=debug)
60
61 # Normal behavior.
62 else:
63 target_speed = min([
64 self.config.speed.max_speed,
65 self.config.live_info.current_speed_limit - self.config.speed.speed_lim_dist])
66 self._local_planner.set_speed(target_speed)
67 control = self._calculate_control(debug=debug)
68
69 return control