Skip to content

Cruise Control Env

cbfpy.envs.car_env

Adaptive Cruise Control Simulation Environment

Simple simulation of a leader-follower vehicle system for adaptive cruise control testing

VehicleEnv

Bases: BaseEnv

Leader/follower vehicle simulation environment for adaptive cruise control testing

This will bring up an interactive Pygame window where you can control the speed of the leader car, and the follower car will be controlled by a simple adaptive cruise control algorithm.

Parameters:

Name Type Description Default
controller_name str

Name of the controller being tested

required
mass float

Mass of the follower vehicle. Defaults to 1650.0 (kg)

1650.0
drag_coeffs Tuple[float, float, float]

Coefficients of a simple polynomial friction model, as described in the Ames CBF paper. Defaults to (0.1, 5.0, 0.25)

(0.1, 5.0, 0.25)
v_des float

Desired velocity of the follower car. Defaults to 24.0 (m/s)

24.0
init_leader_pos float

Initial position of the leader car. Defaults to 0.0 (meters)

0.0
init_leader_vel float

Initial velocity of the leader car. Defaults to 14.0 (m/s)

14.0
init_follower_pos float

Initial position of the follower car. Defaults to -20.0 (meters)

-20.0
init_follower_vel float

Initial velocity of the follower car. Defaults to 10.0 (m/s)

10.0
u_min float

Minimum control input, in Newtons. Defaults to -np.inf (unconstrained)

-inf
u_max float

Maximum control input, in Newtons. Defaults to np.inf (unconstrained)

inf
Source code in cbfpy/envs/car_env.py
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
class VehicleEnv(BaseEnv):
    """Leader/follower vehicle simulation environment for adaptive cruise control testing

    This will bring up an interactive Pygame window where you can control the speed of the leader car,
    and the follower car will be controlled by a simple adaptive cruise control algorithm.

    Args:
        controller_name (str): Name of the controller being tested
        mass (float, optional): Mass of the follower vehicle. Defaults to 1650.0 (kg)
        drag_coeffs (Tuple[float, float, float]): Coefficients of a simple polynomial friction model, as
            described in the Ames CBF paper. Defaults to (0.1, 5.0, 0.25)
        v_des (float, optional): Desired velocity of the follower car. Defaults to 24.0 (m/s)
        init_leader_pos (float, optional): Initial position of the leader car. Defaults to 0.0 (meters)
        init_leader_vel (float, optional): Initial velocity of the leader car. Defaults to 14.0 (m/s)
        init_follower_pos (float, optional): Initial position of the follower car. Defaults to -20.0 (meters)
        init_follower_vel (float, optional): Initial velocity of the follower car. Defaults to 10.0 (m/s)
        u_min (float, optional): Minimum control input, in Newtons. Defaults to -np.inf (unconstrained)
        u_max (float, optional): Maximum control input, in Newtons. Defaults to np.inf (unconstrained)
    """

    # Constants
    PIXELS_PER_METER = 15
    MIN_FOLLOW_DIST_M = 1
    MIN_FOLLOW_DIST_PX = PIXELS_PER_METER * MIN_FOLLOW_DIST_M

    # Screen dimensions
    SCREEN_WIDTH = 600
    SCREEN_HEIGHT = 800

    # Colors
    WHITE = (255, 255, 255)
    GRAY = (100, 100, 100)
    BLACK = (0, 0, 0)
    BLUE = (0, 0, 255)
    RED = (255, 0, 0)
    YELLOW = (255, 255, 0)

    # Road properties
    DASH_HEIGHT_M = 2
    DASH_WIDTH_M = 0.3
    DASH_GAP_M = 1
    DASH_HEIGHT_PX = PIXELS_PER_METER * DASH_HEIGHT_M
    DASH_WIDTH_PX = PIXELS_PER_METER * DASH_WIDTH_M
    DASH_GAP_PX = PIXELS_PER_METER * DASH_GAP_M

    # Car properties
    CAR_WIDTH_M = 2
    CAR_HEIGHT_M = 4
    CAR_WIDTH_PX = PIXELS_PER_METER * CAR_WIDTH_M
    CAR_HEIGHT_PX = PIXELS_PER_METER * CAR_HEIGHT_M

    def __init__(
        self,
        controller_name: str,
        mass: float = 1650.0,
        drag_coeffs: Tuple[float, float, float] = (0.1, 5.0, 0.25),
        v_des: float = 24.0,
        init_leader_pos: float = 0.0,
        init_leader_vel: float = 14.0,
        init_follower_pos: float = -20.0,
        init_follower_vel: float = 10.0,
        u_min: float = -np.inf,
        u_max: float = np.inf,
    ):
        self.mass = mass
        self.v_des = v_des  # Desired velocity of the follower car
        assert len(drag_coeffs) == 3
        self.f0, self.f1, self.f2 = drag_coeffs
        # Initialize Pygame
        pygame.init()
        # Set up the display
        self.screen = pygame.display.set_mode(
            (VehicleEnv.SCREEN_WIDTH, VehicleEnv.SCREEN_HEIGHT)
        )
        pygame.display.set_caption(f"Adaptive Cruise Control: {controller_name}")
        # Create car sprites
        self.leader_sprite = pygame.Surface(
            (VehicleEnv.CAR_WIDTH_PX, VehicleEnv.CAR_HEIGHT_PX)
        )
        self.leader_sprite.fill(VehicleEnv.RED)
        self.follower_sprite = pygame.Surface(
            (VehicleEnv.CAR_WIDTH_PX, VehicleEnv.CAR_HEIGHT_PX)
        )
        self.follower_sprite.fill(VehicleEnv.BLUE)
        # Positioning the cars in the display
        self.leader_x = int(VehicleEnv.SCREEN_WIDTH / 4 - VehicleEnv.CAR_WIDTH_PX / 2)
        self.leader_y = VehicleEnv.SCREEN_HEIGHT // 5
        self.follower_x = self.leader_x
        self.follower_y = self.leader_y + VehicleEnv.MIN_FOLLOW_DIST_PX

        # Init dynamics
        self.leader_pos = init_leader_pos
        self.leader_vel = init_leader_vel
        self.follower_pos = init_follower_pos
        self.follower_vel = init_follower_vel

        self.leader_vel_des = init_leader_vel

        # Initial position of dashed lines
        self.dash_offset = 0

        self.font = pygame.font.SysFont("Arial", 20)

        self.fps = 60
        self.dt = 1 / self.fps
        self.running = True

        self.last_control = 0.0

        self.u_min = u_min
        self.u_max = u_max

        # Print instructions
        print(CAR_ASCII)
        print("Beginning Vehicle Simulation")
        print("Press UP and DOWN to control the speed of the leader car")
        print("Press ESC to quit")

    def pixels_to_meters(self, n_pixels: int) -> float:
        """Helper function: Converts pixels to meters

        Args:
            n_pixels (int): Number of pixels

        Returns:
            float: Distance in meters
        """
        return n_pixels / self.PIXELS_PER_METER

    def meters_to_pixels(self, n_meters: float) -> float:
        """Helper function: Converts meters to pixels

        Args:
            n_meters (float): Distance in meters

        Returns:
            float: Number of pixels (Note: Not rounded to an integer value)
        """
        return n_meters * self.PIXELS_PER_METER

    def friction(self, v: float) -> float:
        """Computes the drag force on the vehicle with the simple model presented in the Ames CBF paper

        Args:
            v (float): Car speed, in m/s

        Returns:
            float: Drag force on the car, in Newtons
        """
        return self.f0 + self.f1 * v + self.f2 * v**2

    @property
    def follow_distance(self):
        """Distance between the leader and follower cars, in meters"""
        return self.leader_pos - self.follower_pos - self.CAR_HEIGHT_M

    def get_state(self):
        return np.array(
            [
                self.follower_vel,
                self.leader_vel,
                self.follow_distance,
            ]
        )

    def get_desired_state(self):
        leader_speed_kmh = 3.6 * self.leader_vel
        safe_follow_distance = leader_speed_kmh / 2 + self.MIN_FOLLOW_DIST_M
        # Note: This desired state is primarily for the simple nominal controller and NOT the CLF-CBF
        return np.array(
            [
                self.v_des,
                self.leader_vel,
                safe_follow_distance,
            ]
        )

    def apply_control(self, u) -> None:
        u = np.clip(u, self.u_min, self.u_max).item()
        if np.isnan(u):
            print("Infeasible. Using last safe control")
            u = self.last_control
        force = u - self.friction(self.follower_vel)
        follower_accel = force / self.mass
        self.follower_vel += follower_accel * self.dt
        self.follower_pos += self.follower_vel * self.dt
        self.last_control = u

    def leader_controller(self) -> float:
        """Simple controller for the leader car

        The desired leader velocity will be set by the user, and this controller will try to track that velocity

        Returns:
            float: Control input to the leader car: Acceleration force, in Newtons
        """
        kv = 1000
        return kv * (self.leader_vel_des - self.leader_vel)

    def step(self):
        # Handle events
        # This includes where the speed of the main controlled by the user
        for event in pygame.event.get():
            if event.type == pygame.QUIT:
                self.running = False
                return
            elif event.type == pygame.KEYDOWN:
                if event.key == pygame.K_UP:
                    self.leader_vel_des += 1
                elif event.key == pygame.K_DOWN:
                    self.leader_vel_des -= 1
                elif event.key == pygame.K_ESCAPE:
                    self.running = False
                    return

        leader_u = self.leader_controller()
        leader_force = leader_u - self.friction(self.leader_vel)
        # Assume both vehicles have the same mass
        leader_accel = leader_force / self.mass
        self.leader_vel += leader_accel * self.dt
        self.leader_pos += self.leader_vel * self.dt
        follow_dist = self.follow_distance

        # Update locations of the cars in pixel frame
        self.follower_y = (
            self.leader_y + self.meters_to_pixels(follow_dist) + self.CAR_HEIGHT_PX
        )

        # Clear the screen
        self.screen.fill(VehicleEnv.WHITE)

        # Draw the road
        pygame.draw.rect(
            self.screen,
            VehicleEnv.GRAY,
            (
                0,
                0,
                VehicleEnv.SCREEN_WIDTH // 2,
                VehicleEnv.SCREEN_HEIGHT,
            ),
        )

        # Draw dashed lines on the road
        dash_y = self.dash_offset
        while dash_y < VehicleEnv.SCREEN_HEIGHT:
            pygame.draw.rect(
                self.screen,
                VehicleEnv.YELLOW,
                (
                    VehicleEnv.SCREEN_WIDTH // 4 - VehicleEnv.DASH_WIDTH_PX // 2,
                    dash_y,
                    VehicleEnv.DASH_WIDTH_PX,
                    VehicleEnv.DASH_HEIGHT_PX,
                ),
            )
            dash_y += VehicleEnv.DASH_HEIGHT_PX + VehicleEnv.DASH_GAP_PX

        # Move the dashed lines
        self.dash_offset += self.leader_vel
        if self.dash_offset >= VehicleEnv.DASH_HEIGHT_PX + VehicleEnv.DASH_GAP_PX:
            self.dash_offset = 0

        # Draw the cars
        self.screen.blit(self.leader_sprite, (self.leader_x, self.leader_y))
        self.screen.blit(self.follower_sprite, (self.follower_x, self.follower_y))

        # Print info to the pygame window
        info_1 = f"Leader desired speed: {self.leader_vel_des:.2f}"
        info_2 = f"Leader speed: {self.leader_vel:.2f}"
        info_3 = f"Follower speed: {self.follower_vel:.2f}"
        info_4 = f"Follow distance: {follow_dist:.2f}"
        info_5 = f"Control: {self.last_control:.2f}"
        text = [info_1, info_2, info_3, info_4, info_5]
        for i, line in enumerate(text):
            self.screen.blit(
                self.font.render(line, True, VehicleEnv.BLACK),
                (VehicleEnv.SCREEN_WIDTH // 2 + 10, 10 + 30 * i),
            )

        # Update the display
        pygame.display.flip()

        # Cap the frame rate
        pygame.time.Clock().tick(self.fps)

follow_distance property

Distance between the leader and follower cars, in meters

pixels_to_meters(n_pixels)

Helper function: Converts pixels to meters

Parameters:

Name Type Description Default
n_pixels int

Number of pixels

required

Returns:

Name Type Description
float float

Distance in meters

Source code in cbfpy/envs/car_env.py
142
143
144
145
146
147
148
149
150
151
def pixels_to_meters(self, n_pixels: int) -> float:
    """Helper function: Converts pixels to meters

    Args:
        n_pixels (int): Number of pixels

    Returns:
        float: Distance in meters
    """
    return n_pixels / self.PIXELS_PER_METER

meters_to_pixels(n_meters)

Helper function: Converts meters to pixels

Parameters:

Name Type Description Default
n_meters float

Distance in meters

required

Returns:

Name Type Description
float float

Number of pixels (Note: Not rounded to an integer value)

Source code in cbfpy/envs/car_env.py
153
154
155
156
157
158
159
160
161
162
def meters_to_pixels(self, n_meters: float) -> float:
    """Helper function: Converts meters to pixels

    Args:
        n_meters (float): Distance in meters

    Returns:
        float: Number of pixels (Note: Not rounded to an integer value)
    """
    return n_meters * self.PIXELS_PER_METER

friction(v)

Computes the drag force on the vehicle with the simple model presented in the Ames CBF paper

Parameters:

Name Type Description Default
v float

Car speed, in m/s

required

Returns:

Name Type Description
float float

Drag force on the car, in Newtons

Source code in cbfpy/envs/car_env.py
164
165
166
167
168
169
170
171
172
173
def friction(self, v: float) -> float:
    """Computes the drag force on the vehicle with the simple model presented in the Ames CBF paper

    Args:
        v (float): Car speed, in m/s

    Returns:
        float: Drag force on the car, in Newtons
    """
    return self.f0 + self.f1 * v + self.f2 * v**2

leader_controller()

Simple controller for the leader car

The desired leader velocity will be set by the user, and this controller will try to track that velocity

Returns:

Name Type Description
float float

Control input to the leader car: Acceleration force, in Newtons

Source code in cbfpy/envs/car_env.py
212
213
214
215
216
217
218
219
220
221
def leader_controller(self) -> float:
    """Simple controller for the leader car

    The desired leader velocity will be set by the user, and this controller will try to track that velocity

    Returns:
        float: Control input to the leader car: Acceleration force, in Newtons
    """
    kv = 1000
    return kv * (self.leader_vel_des - self.leader_vel)