diff --git a/README.md b/README.md index fe4c65f..22212bf 100644 --- a/README.md +++ b/README.md @@ -156,6 +156,11 @@ Launching Carla CUDA_VISIBLE_DEVICES=0 ./CarlaUE4.sh -quality-level=Low -prefernvidia -ResX=10 -ResY=10 ``` +Generate the simple trajectory templates +```bash +python3 -m drivellava.scripts.generate_trajectory_templates_simple +``` + Running the carla client ```bash python3 -m drivellava.scripts.carla_run @@ -179,6 +184,12 @@ Read the [CONTRIBUTING.md](CONTRIBUTING.md) file. - [x] Quantize the trajectory - [x] Visualize the trajectory on the image - [x] Generate JSON dataset +- [ ] Carla + - [x] Build a Carla Client + - [x] Connect Vision Model to Carla + - [ ] Pause sim when thinking + - [ ] Feed in multi-cam input + - [ ] Feed in top down map view - [ ] CI and Features - [ ] There's a lot of issues with the sanity of the dataset since the trajectory was derived from accelerometer data. Find out how to filter out bad data without affecting the distribution of data. Write test cases to enforce - [ ] Write test cases for the following scripts diff --git a/drivellava/carla/client.py b/drivellava/carla/client.py index f4bc643..b566fad 100644 --- a/drivellava/carla/client.py +++ b/drivellava/carla/client.py @@ -14,6 +14,7 @@ from drivellava.carla.helpers import HUD, KeyboardControl, World from drivellava.schema.carla import DroneControls, DroneState from drivellava.schema.image import Image +from drivellava.trajectory_encoder import TrajectoryEncoder class CarlaClient: @@ -24,8 +25,8 @@ def __init__( port=2000, sync=False, autopilot=False, - width=512, - height=256, + width=256, + height=128, rolename="hero", filter="vehicle.tesla.model3", generation="2", @@ -120,8 +121,32 @@ def get_car_state(self, default=None) -> DroneState: steering_angle=controls.steer, ) - def set_car_controls(self, controls: DroneControls): + def set_car_controls( + self, controls: DroneControls, trajectory_encoder: TrajectoryEncoder + ): """ Set the car controls """ - pass + if controls.speed_index == 0: + self.world.player.enable_constant_velocity( + carla.Vector3D(0, 0, 0) # 30 Km/h + ) + self.world.constant_velocity_enabled = True + elif controls.speed_index == 1: + self.world.player.enable_constant_velocity( + carla.Vector3D(1, 0, 0) # 5 Km/h + ) + self.world.constant_velocity_enabled = True + + steering_angle = ( + 2.0 + * ( + float(controls.trajectory_index) + / trajectory_encoder.num_trajectory_templates + ) + - 1.0 + ) + + assert -1 <= steering_angle <= 1 + + self.controller.set_steering_angle(steering_angle) diff --git a/drivellava/carla/helpers.py b/drivellava/carla/helpers.py index 582b93f..e03ee22 100644 --- a/drivellava/carla/helpers.py +++ b/drivellava/carla/helpers.py @@ -1524,6 +1524,12 @@ def _parse_vehicle_keys(self, keys, milliseconds): else: self._ackermann_control.steer = round(self._steer_cache, 1) + def set_steering_angle(self, steering_angle): + if not self._ackermann_enabled: + self._control.steer = round(steering_angle, 1) + else: + self._ackermann_control.steer = round(steering_angle, 1) + def _parse_walker_keys(self, keys, milliseconds, world): self._control.speed = 0.0 if keys[K_DOWN] or keys[K_s]: @@ -1749,7 +1755,9 @@ def render(self, display): ) display.blit(surface, (8, v_offset)) v_offset += 18 - self._notifications.render(display) + + # self._notifications.render(display) + self.help.render(display) diff --git a/drivellava/gpt/gpt_vision.py b/drivellava/gpt/gpt_vision.py index db0fdd3..24242d8 100644 --- a/drivellava/gpt/gpt_vision.py +++ b/drivellava/gpt/gpt_vision.py @@ -2,6 +2,8 @@ GPT Vision to make Control Decisions """ +from textwrap import dedent + import instructor import numpy as np from openai import OpenAI @@ -46,12 +48,19 @@ def llm_client_factory(llm_provider: str): class GPTVision: - def __init__(self, max_history=10): + def __init__(self, max_history=1): self.client = llm_client_factory(settings.system.SYSTEM_LLM_PROVIDER) self.previous_messages = GPTState() self.max_history = max_history - self.trajectory_encoder = TrajectoryEncoder() + self.num_trajectory_templates = 9 + TRAJECTORY_TEMPLATES_NPY = f"./trajectory_templates/proposed_trajectory_templates_simple_{self.num_trajectory_templates}.npy" # noqa + TRAJECTORY_TEMPLATES_KMEANS_PKL = f"./trajectory_templates/kmeans_simple_{self.num_trajectory_templates}.pkl" # noqa + self.trajectory_encoder = TrajectoryEncoder( + num_trajectory_templates=self.num_trajectory_templates, + trajectory_templates_npy=TRAJECTORY_TEMPLATES_NPY, + trajectory_templates_kmeans_pkl=TRAJECTORY_TEMPLATES_KMEANS_PKL, + ) def step( self, @@ -60,7 +69,20 @@ def step( mission: str, ) -> DroneControls: # base64_image = encode_opencv_image(image) - traj_str = self.trajectory_encoder.get_traj_str() + # traj_str = self.trajectory_encoder.get_traj_str() + traj_str = dedent( + f""" + The trajectories are numbered from: + [0,{self.num_trajectory_templates-1}] + + They are labelled from left to right. + Select trajectory 0 to get the left most. + Select trajectory {(self.num_trajectory_templates-1)//2} to get a + more centered trajectory + Select trajectory {self.num_trajectory_templates-1} to get the + right most. + """ + ) new_messages = [ # User GPTMessage( diff --git a/drivellava/gpt/prompts.py b/drivellava/gpt/prompts.py index 4d327b0..e7f2921 100644 --- a/drivellava/gpt/prompts.py +++ b/drivellava/gpt/prompts.py @@ -3,7 +3,7 @@ """ GPT_SYSTEM = """ -You are an autonomous vehicle. You are given the drone's environment state \ +You are an autonomous vehicle. You are given the vehicle's environment state \ and you must control the vehicle to complete the mission. """ @@ -20,12 +20,12 @@ Trajectories: {traj_str} Note that translation happens first, followed by rotation. -What are your next actions? Let's think step by step. +What are your next actions? Be short and brief with your thoughts """ GPT_PROMPT_UPDATE = """MISSION: {mission} Updated visual is provided -What are your next actions? Let's think step by step. +What are your next actions? Be short and brief with your thoughts """ GPT_PROMPT_CONTROLS = """ @@ -36,15 +36,19 @@ Trajectories: {traj_str} +Speed: Select 0 to stop the vehicle and 1 to move the vehicle at the constant +speed + Based on the description, what are the next actions the pilot should take. You will provide the next actions in the form of a JSON object: - "trajectory_index": trajectory_index (str), - "speed_index": speed_index (str), + "trajectory_index": trajectory_index (int), + "speed_index": speed_index (int), You can select one or more of the following actions as your next immediate \ action: - trajectory_index: select one of the trajectories from those drawn - - speed_index: select a speed index from one of those provided + - speed_index: Select 0 to stop the vehicle and 1 to move the vehicle at + the constant speed """ diff --git a/drivellava/scripts/carla_run.py b/drivellava/scripts/carla_run.py index 862fbdc..896c3d2 100644 --- a/drivellava/scripts/carla_run.py +++ b/drivellava/scripts/carla_run.py @@ -2,15 +2,17 @@ GPT Vision to make Control Decisions """ +import time import traceback import cv2 import numpy as np +from matplotlib import colormaps from drivellava.carla.client import CarlaClient - -# from drivellava.gpt.gpt_vision import GPTVision +from drivellava.gpt.gpt_vision import GPTVision from drivellava.settings import settings +from drivellava.utils import plot_steering_traj def main(): # pragma: no cover @@ -23,37 +25,78 @@ def main(): # pragma: no cover print("mission", mission) - # gpt = GPTVision() + gpt = GPTVision() client = CarlaClient() + trajectory_templates = gpt.trajectory_encoder.left_to_right_traj() + NUM_TEMLATES = gpt.num_trajectory_templates + + # Select colors based on templates + COLORS = [ + (255 * colormaps["gist_rainbow"]([float(i + 1) / NUM_TEMLATES])[0]) + for i in range(NUM_TEMLATES) + ] + drone_state = client.get_car_state() # image = drone_state.image.cv_image() - # last_update = drone_state.timestamp + last_update = drone_state.timestamp try: while True: client.game_loop() - # # Get GPT Controls - # if ( - # drone_state.timestamp > last_update - # ): - # # last_update = drone_state.timestamp - # last_update = int(time.time() * 1000) - # # gpt_controls = gpt.step(image, drone_state, mission) - - # # client.set_car_controls(gpt_controls) - # gpt.previous_messages.timestamp = last_update - drone_state = client.get_car_state(default=drone_state) + image_raw = np.array( + drone_state.image.cv_image(), + ) image = np.array( drone_state.image.cv_image(), ) + # Draw all template trajectories + for index in range(NUM_TEMLATES): + template_trajectory = trajectory_templates[index] + template_trajectory_3d = np.zeros( + (settings.system.TRAJECTORY_SIZE, 3) + ) + template_trajectory_3d[:, 0] = template_trajectory[:, 0] + template_trajectory_3d[:, 2] = template_trajectory[:, 1] + color = COLORS[index] + plot_steering_traj( + image, template_trajectory_3d, color=color, track=False + ) + # print(type(image), image.dtype) - cv2.imshow("carla", image) + image_vis = image_raw.copy() + # Get GPT Controls + if drone_state.timestamp > last_update: + # last_update = drone_state.timestamp + last_update = int(time.time() * 1000) + gpt_controls = gpt.step(image, drone_state, mission) + + client.set_car_controls(gpt_controls, gpt.trajectory_encoder) + gpt.previous_messages.timestamp = last_update + + template_trajectory = trajectory_templates[ + gpt_controls.trajectory_index + ] + template_trajectory_3d = np.zeros( + (settings.system.TRAJECTORY_SIZE, 3) + ) + template_trajectory_3d[:, 0] = template_trajectory[:, 0] + template_trajectory_3d[:, 2] = template_trajectory[:, 1] + + plot_steering_traj( + image_vis, + template_trajectory_3d, + color=(255, 0, 0), + track=True, + ) + + cv2.imshow("carla", image_vis) + key = cv2.waitKey(10) & 0xFF if key == ord("q"): break diff --git a/drivellava/scripts/generate_trajectory_templates_simple.py b/drivellava/scripts/generate_trajectory_templates_simple.py new file mode 100644 index 0000000..59f6a39 --- /dev/null +++ b/drivellava/scripts/generate_trajectory_templates_simple.py @@ -0,0 +1,210 @@ +""" +Generates image frames for the commavq dataset +""" + +import pickle + +import cv2 +import matplotlib.pyplot as plt +import numpy as np +from sklearn.cluster import KMeans +from tqdm import tqdm + +from drivellava.trajectory_encoder import TRAJECTORY_SIZE +from drivellava.utils import plot_bev_trajectory, plot_steering_traj + + +def generate_trajectory_dataset( + num_templates: int, + num_frames: int, + total_angle: float = np.pi / 2, # radians + trajectory_distance: float = 40.0, # meters +) -> list: + """ + Generate a list of trajectories. + The list must be of length num_templates + Each trajectory is of the following shape: (num_frames, 2) + Each trajectory element consists of a 2D point (x, y) + + The list of trajectories must be generated such that the trajectories + sweep in terms of curvature from left to right. + i.e. the 0th trajectiry curves to the left, + the (num_templates//2)th trajectory is straight, and + the last trajectiry curves to the right. + + The maximum radius of curvature is max_radius_of_curvature. + The distance traversed along any trajectory must be trajectory_distance. + All the points on the trajectory must be roughly equidistant from their + neighbors. + """ + trajectory_dataset = [] + + # Calculate the total distance and angle covered by each trajectory + total_distance = trajectory_distance + distance_per_frame = total_distance / num_frames + + for i in range(num_templates): + # Calculate the starting point and orientation of the current + # trajectory + start_point = (0, 0, 0) + + # Initialize the current trajectory with the starting point + trajectory = [start_point] + + traj_total_angle = total_angle / (i + 1) + angle_step = traj_total_angle / num_frames + + for j in range(1, num_frames): + # Calculate the new angle and position based on the previous one + angle = (j - 1) * angle_step + # angle = (num_frames - j -1) * angle_step + # angle = np.pi - angle + + # Calculate the new position based on the previous one and the + # curvature + x = trajectory[-1][0] + distance_per_frame * np.sin(angle) + y = trajectory[-1][2] + distance_per_frame * np.cos(angle) + + # Append the new point to the current trajectory + trajectory.append((x, 0, y)) + # trajectory.append((y, 0, -x)) + + # Add the current trajectory to the dataset + trajectory_dataset.append(np.array(trajectory)) + print( + "x", + trajectory_dataset[-1][:, 0].min(), + trajectory_dataset[-1][:, 0].max(), + ) + print( + "y", + trajectory_dataset[-1][:, 2].min(), + trajectory_dataset[-1][:, 2].max(), + ) + + return trajectory_dataset + + +def main(): + + NUM_FRAMES = TRAJECTORY_SIZE + K = 9 + PLOT_TRAJ = True + + trajectories = [] + + trajectory_dataset = generate_trajectory_dataset(K * 10, NUM_FRAMES) + + # Iterate over the embeddings in batches and decode the images + for i in tqdm( + range(len(trajectory_dataset)), + desc="Video", + disable=True, + ): + + trajectory = trajectory_dataset[i] + + trajectory_2d = trajectory[:, [0, 2]] + + trajectories.append(trajectory_2d.flatten()) + + trajectory_2d_flipped = trajectory_2d.copy() + trajectory_2d_flipped[:, 0] *= -1 + + trajectories.append(trajectory_2d_flipped.flatten()) + + if PLOT_TRAJ: + img = np.zeros((128, 256, 3), dtype=np.uint8) + + dx = trajectory[1:, 2] - trajectory[:-1, 2] + speed = dx / (1.0 / 20.0) + # m/s to km/h + speed_kmph = speed * 3.6 + # speed mph + speed_mph = speed_kmph * 0.621371 + + img = plot_steering_traj( + img, + trajectory, + ) + + img_bev = plot_bev_trajectory(trajectory, img) + + # Write speed on img + font = cv2.FONT_HERSHEY_SIMPLEX + bottomLeftCornerOfText = (10, 50) + fontScale = 0.5 + fontColor = (255, 255, 255) + lineType = 2 + + img = cv2.resize(img, (0, 0), fx=2, fy=2) + + cv2.putText( + img, + f"Speed: {speed_mph.mean():.2f} mph", + bottomLeftCornerOfText, + font, + fontScale, + fontColor, + lineType, + ) + + cv2.imshow("frame_path", img) + + cv2.imshow( + "frame_path_bev", cv2.resize(img_bev, (0, 0), fx=2, fy=2) + ) + + key = cv2.waitKey(1) + + if key == ord("q"): + exit() + elif key == ord("n"): + break + + print("Running K-MEANs on the trajectories", len(trajectories)) + kmeans = KMeans(n_clusters=K, random_state=0).fit(trajectories) + proposed_trajectory_templates = [ + centroid.reshape((NUM_FRAMES, 2)) + for centroid in kmeans.cluster_centers_ + ] + + # Save the kmeans object as trajectory_templates/kmeans.pkl + with open(f"trajectory_templates/kmeans_simple_{str(K)}.pkl", "wb") as f: + pickle.dump(kmeans, f) + + # Plotting the trajectory templates + plt.figure(figsize=(10, 6)) + for template in proposed_trajectory_templates: + plt.plot(template[:, 0], template[:, 1], alpha=0.5) + plt.xlabel("X") + plt.ylabel("Y") + + # X lim + plt.xlim(-10, 10) + # Y lim + plt.ylim(-1, 40) + + plt.title("Proposed Trajectory Templates") + plt.savefig( + f"trajectory_templates/trajectory_templates_simple_{str(K)}.png" + ) # Saving the plot + plt.show() + + # Saving the templates as a NumPy file + proposed_trajectory_templates_np = np.array( + proposed_trajectory_templates, dtype=np.float32 + ) + print( + "proposed_trajectory_templates_np.shape", + proposed_trajectory_templates_np.shape, + ) + np.save( + f"trajectory_templates/proposed_trajectory_templates_simple_{str(K)}.npy", # noqa + proposed_trajectory_templates_np, + allow_pickle=False, + ) + + +if __name__ == "__main__": + main() diff --git a/drivellava/trajectory_encoder.py b/drivellava/trajectory_encoder.py index a7d0fa8..7f9d441 100644 --- a/drivellava/trajectory_encoder.py +++ b/drivellava/trajectory_encoder.py @@ -137,11 +137,6 @@ def left_to_right( +ve x is right """ x = self.trajectory_templates[:, :, 0] - # x_mean, x_std = x.mean(), x.std() - # y_mean, y_std = ( - # self.trajectory_templates[:, :, 1].mean(), - # self.trajectory_templates[:, :, 1].std(), - # ) # x_mean is ~0 # Sort the trajectory_templates by the mean of the x axis @@ -161,16 +156,24 @@ def left_to_right( center_tokens = [self.trajectory_index_2_token[i] for i in center] right_tokens = [self.trajectory_index_2_token[i] for i in right] - # print("self.trajectory_templates", self.trajectory_templates.shape) - # print("left", left.shape) - # print("center", center.shape) - # print("right", right.shape) + return left_tokens, center_tokens, right_tokens + + def left_to_right_traj( + self, + ): + """Arrange the tokens from left to right + -ve x is left + +ve x is right + """ + x = self.trajectory_templates[:, :, 0] - # print("left_tokens", left_tokens) - # print("center_tokens", center_tokens) - # print("right_tokens", right_tokens) + # x_mean is ~0 + # Sort the trajectory_templates by the mean of the x axis + sorted_indices = np.argsort(x.mean(axis=1)) - return left_tokens, center_tokens, right_tokens + sorted_traj = [self.trajectory_templates[i] for i in sorted_indices] + + return sorted_traj def get_traj_str(self) -> str: left_tokens, center_tokens, right_tokens = self.left_to_right() diff --git a/drivellava/utils.py b/drivellava/utils.py index ee645da..84cd6d7 100644 --- a/drivellava/utils.py +++ b/drivellava/utils.py @@ -121,9 +121,14 @@ def plot_steering_traj( DistCoef=None, # offsets=[0.0, 1.5, 1.0], offsets=[0.0, -0.75, 0.0], + # offsets=[0.0, -1.5, 0.0], method="add_weighted", + track=True, ): assert method in ("overlay", "mask", "add_weighted") + + h, w = frame_center.shape[:2] + if intrinsic_matrix is None: # intrinsic_matrix = np.array([ # [525.5030, 0, 333.4724], @@ -132,8 +137,8 @@ def plot_steering_traj( # ]) intrinsic_matrix = np.array( [ - [525.5030, 0, 256.0 / 2], - [0, 531.1660, 128.0 / 2], + [525.5030, 0, w / 2], + [0, 531.1660, h / 2], [0, 0, 1.0], ] ) @@ -147,7 +152,6 @@ def plot_steering_traj( -0.6398, # Radial Distortion ] ) - h, w = frame_center.shape[:2] newcameramtx, roi = cv2.getOptimalNewCameraMatrix( intrinsic_matrix, DistCoef, (w, h), 1, (w, h) ) @@ -184,13 +188,14 @@ def plot_steering_traj( px_p, py_p = prev_point dist = ((px_p - px) ** 2 + (py_p - py) ** 2) ** 0.5 if dist < 20: - rect_coords_3D = get_rect_coords_3D(p4d, prev_point_3D) - rect_coords = convert_3D_points_to_2D( - rect_coords_3D, homo_cam_mat - ) - rect_frame = cv2.fillPoly( - rect_frame, pts=[rect_coords], color=color - ) + if track: + rect_coords_3D = get_rect_coords_3D(p4d, prev_point_3D) + rect_coords = convert_3D_points_to_2D( + rect_coords_3D, homo_cam_mat + ) + rect_frame = cv2.fillPoly( + rect_frame, pts=[rect_coords], color=color + ) frame_center = cv2.line( frame_center, (px_p, py_p), (px, py), color, 2 diff --git a/trajectory_templates/kmeans_simple_16.pkl b/trajectory_templates/kmeans_simple_16.pkl new file mode 100644 index 0000000..37ef664 Binary files /dev/null and b/trajectory_templates/kmeans_simple_16.pkl differ diff --git a/trajectory_templates/kmeans_simple_5.pkl b/trajectory_templates/kmeans_simple_5.pkl new file mode 100644 index 0000000..9f6588f Binary files /dev/null and b/trajectory_templates/kmeans_simple_5.pkl differ diff --git a/trajectory_templates/kmeans_simple_9.pkl b/trajectory_templates/kmeans_simple_9.pkl new file mode 100644 index 0000000..2d30358 Binary files /dev/null and b/trajectory_templates/kmeans_simple_9.pkl differ diff --git a/trajectory_templates/proposed_trajectory_templates_simple_16.npy b/trajectory_templates/proposed_trajectory_templates_simple_16.npy new file mode 100644 index 0000000..18ea6b8 Binary files /dev/null and b/trajectory_templates/proposed_trajectory_templates_simple_16.npy differ diff --git a/trajectory_templates/proposed_trajectory_templates_simple_5.npy b/trajectory_templates/proposed_trajectory_templates_simple_5.npy new file mode 100644 index 0000000..476c805 Binary files /dev/null and b/trajectory_templates/proposed_trajectory_templates_simple_5.npy differ diff --git a/trajectory_templates/proposed_trajectory_templates_simple_9.npy b/trajectory_templates/proposed_trajectory_templates_simple_9.npy new file mode 100644 index 0000000..447dbdc Binary files /dev/null and b/trajectory_templates/proposed_trajectory_templates_simple_9.npy differ diff --git a/trajectory_templates/trajectory_templates_simple_16.png b/trajectory_templates/trajectory_templates_simple_16.png new file mode 100644 index 0000000..5f55a90 Binary files /dev/null and b/trajectory_templates/trajectory_templates_simple_16.png differ diff --git a/trajectory_templates/trajectory_templates_simple_5.png b/trajectory_templates/trajectory_templates_simple_5.png new file mode 100644 index 0000000..32f7b8c Binary files /dev/null and b/trajectory_templates/trajectory_templates_simple_5.png differ diff --git a/trajectory_templates/trajectory_templates_simple_9.png b/trajectory_templates/trajectory_templates_simple_9.png new file mode 100644 index 0000000..0042ffc Binary files /dev/null and b/trajectory_templates/trajectory_templates_simple_9.png differ