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