Auto Sim

auto_sim/.gitignore

@@ -0,0 +1,5 @@
+__pycache__
+build
+*.egg-info
+labels
+.idea

auto_sim/.vscode/settings.json

@@ -0,0 +1,5 @@
+{
+	"python-envs.defaultEnvManager": "ms-python.python:pipenv",
+	"cmake.sourceDirectory": "C:/Users/jessi/Documents/School/fsae/sim/auto_sim/test_controller_2",
+	"editor.defaultFormatter": null
+}

auto_sim/Pipfile

@@ -0,0 +1,18 @@
+[[source]]
+url = "https://pypi.org/simple"
+verify_ssl = true
+name = "pypi"
+
+[packages]
+pygame = "*"
+numpy = "*"
+scipy = "*"
+setuptools = "*"
+pybind11 = "*"
+requests = "*"
+casadi = "==3.6.7"
+
+[dev-packages]
+
+[requires]
+python_version = "3.13"

auto_sim/README.md

@@ -0,0 +1,6 @@
+# TODO
+
+* basic boundary checking， triangulation
+* basic controller?
+* full MPC
+

auto_sim/constants.py

@@ -0,0 +1 @@
+VEHICLE_WIDTH_M = 1.2

auto_sim/main.py

@@ -0,0 +1,77 @@
+import pygame
+from Controller import compute, sim_step, Cone, ConeColor, VehicleState, mock_perception, compute_path
+from render import init, render_world
+import ctypes
+import platform
+
+# dpi awareness for windows - prevents blurry rendering on high dpi displays
+if platform.system() == "Windows":
+    try:
+        # Set DPI Awareness (Windows 10/8)
+        ctypes.windll.shcore.SetProcessDpiAwareness(2)
+    except AttributeError:
+        # Windows 7/Vista
+        ctypes.windll.user32.SetProcessDPIAware()
+
+CONE_POSITIONS: list[Cone] = []
+with open("labels/0000001.txt", "r") as f:
+    lines = f.readlines()
+    for line in lines:
+        x, y, _, _, _, _, _, color = line.strip().split()
+        cone_color = ConeColor.YELLOW if color == "Cone_Yellow" else ConeColor.BLUE
+        CONE_POSITIONS.append(Cone(float(x), float(y), cone_color))
+
+# pygame setup
+pygame.init()
+screen = pygame.display.set_mode((1900, 1200), pygame.RESIZABLE)
+pygame.display.set_caption('Autonomous Sim')
+clock = pygame.time.Clock()
+running = True
+time: float = 0.0
+init()
+
+vehicle_state: VehicleState = VehicleState()
+# vehicle_state.v_y = 2.0
+# vehicle_state.theta = math.radians(23.54)
+# vehicle_state.omega = -0.2
+
+def handle_key(key: int, state: VehicleState):
+    match key:
+        case pygame.K_d: state.y -= 1
+        case pygame.K_a: state.y += 1
+        case pygame.K_w: state.x += 1
+        case pygame.K_s: state.x -= 1
+
+ran = False
+def simulate_cone_detection(state: VehicleState):
+    global ran
+    if not ran:
+        mock_perception(CONE_POSITIONS)
+        compute_path()
+        ran = True
+    return CONE_POSITIONS
+
+while running:
+    # handle events
+    # pygame.QUIT event means the user clicked X to close your window
+    for event in pygame.event.get():
+        if event.type == pygame.QUIT:
+            running = False
+        elif event.type == pygame.KEYDOWN:
+            handle_key(event.key, vehicle_state)
+
+    # this is what is running on the 
+    detected_cones = simulate_cone_detection(vehicle_state)
+    controls = compute(vehicle_state)
+
+    # closing the SIL loop
+    dt = clock.get_time()
+    screen.fill("black")
+    sim_step(vehicle_state, controls, dt)
+    render_world(vehicle_state, CONE_POSITIONS, screen)
+
+    # flip() the display to put your work on screen
+    pygame.display.flip()
+    clock.tick(60)  # limits FPS to 60
+
+pygame.quit()

auto_sim/render.py

@@ -0,0 +1,175 @@
+from typing import Tuple
+
+import pygame
+from scipy.spatial.transform import Rotation as R
+from constants import VEHICLE_WIDTH_M
+from Controller import VehicleState, Cone, ConeColor, spline_t, project, get_center_line_length, get_center_points
+from math import ceil, degrees
+import numpy as np
+from colorsys import hsv_to_rgb
+
+PIXELS_PER_M = 	20.0
+w: int
+h: int
+
+car: pygame.Surface
+vignette: pygame.Surface
+
+def init():
+	global car, vignette
+	car = pygame.image.load('fsae.png').convert_alpha()
+	vignette_image = pygame.image.load('vignette.png').convert_alpha()
+	vignette_image = pygame.transform.scale(vignette_image, (30 * PIXELS_PER_M,30 * PIXELS_PER_M))
+	vignette = pygame.Surface((3000,3000), pygame.SRCALPHA)
+	vignette.blit(vignette_image, vignette_image.get_rect(center=(1500,1500)))
+
+def transform(x: float, y: float, vehicle_state: VehicleState) -> tuple[int, int]:
+	"""
+	Transforms from global coordinates to screen space coordinates
+	"""
+	global w, h
+	r = R.from_euler('z', -vehicle_state.theta + np.pi/2, degrees=False)
+	rel_x, rel_y = x - vehicle_state.x, y - vehicle_state.y
+	rotated_x, rotated_y,_ = r.apply([rel_x, rel_y, 0])
+	screen_x = int(w / 2 + rotated_x * PIXELS_PER_M)
+	screen_y = int((h * 0.67) - rotated_y * PIXELS_PER_M)
+	# Simple transformation - replace with actual coordinate transformation logic
+	return (screen_x, screen_y)
+
+def create_grid_surface(l: int, spacing_px: int, color: str) -> pygame.Surface:
+	grid_surf = pygame.Surface((2*l,2*l), pygame.SRCALPHA)
+	for x in range(0, 2*l, spacing_px):
+		pygame.draw.line(grid_surf, color, (x, 0), (x, 2*l), 2)
+	for y in range(0, 2*l, spacing_px):
+		pygame.draw.line(grid_surf, color, (0, y), (2*l, y), 2)
+	return grid_surf
+
+old_state = None, None, None
+old_grid_surf = None
+def drawGrid(screen: pygame.Surface, state: VehicleState, color=(45, 45, 45), spacing_m=1):
+	global h, w, old_state, old_grid_surf, vignette
+	# create grid, cached
+	spacing_px =  int(spacing_m * PIXELS_PER_M)
+	l: int= ceil(np.hypot(w/2, h/2) * 0.4 / spacing_m) * spacing_m
+	if (spacing_m, color, l) != old_state or old_grid_surf is None:
+		print("RERENDERING GRID")
+		old_state = (spacing_m, color, l)
+		old_grid_surf = create_grid_surface(2*l, spacing_px, color)
+	grid_surf = old_grid_surf
+
+	# grid
+	screen_grid = pygame.surface.Surface((w, h), pygame.SRCALPHA)
+
+	# Calculate grid offset to align with world coordinates
+	r = R.from_euler('z', -state.theta + np.pi/2, degrees=False)
+	x, y = r.apply([state.x % spacing_m, state.y % spacing_m, 0])[:2]
+	rotated_surf: pygame.Surface = pygame.transform.rotate(grid_surf, degrees(-state.theta - np.pi/2))
+	screen_grid.blit(rotated_surf, rotated_surf.get_rect(center=(
+		w/2 - x * PIXELS_PER_M,
+		0.67 * h + y * PIXELS_PER_M
+	)).topleft)
+
+	# vignette
+	screen_grid.blit(vignette, vignette.get_rect(center=(w/2, 0.67*h)), special_flags=pygame.BLEND_RGBA_MULT)
+
+	screen.blit(screen_grid, (0, 0))
+
+
+# def create_spline_surface():
+# 	surf = pygame.Surface((77 * PIXELS_PER_M, 47 * PIXELS_PER_M), pygame.SRCALPHA)
+# 	length = get_center_line_length()
+# 	for x in range(int(77 * PIXELS_PER_M)):
+# 		for y in range(int(47 * PIXELS_PER_M)):
+# 			k = hsv_to_rgb(project(22-x/PIXELS_PER_M, -39+y/PIXELS_PER_M)/length, 1, 1)
+# 			surf.set_at((x, y), (int(k[0]*255), int(k[1]*255), int(k[2]*255)))
+# 	return surf
+
+# spline_surface = None
+# def drawSplintSurface(screen: pygame.Surface, vehicle_state: VehicleState):
+# 	global spline_surface	
+# 	if spline_surface is None:
+# 		spline_surface = create_spline_surface()
+# 	rotated_surf: pygame.Surface = pygame.transform.rotate(spline_surface, degrees(-vehicle_state.theta - np.pi/2))
+# 	screen.blit(rotated_surf, rotated_surf.get_rect(center=transform(-16.5, -15.5, vehicle_state)))
+
+
+def int_to_color(value: ConeColor) -> str:
+	match value:
+		case ConeColor.BLUE:
+			return "blue"
+		case ConeColor.YELLOW:
+			return "yellow"
+		case _:
+			return "white"
+
+at_t = None
+def render_world(vehicle_state: VehicleState, cones: list[Cone], screen: pygame.Surface):
+	global w, h, at_t
+	w, h = screen.get_width(), screen.get_height()
+
+	# grid for context
+	drawGrid(screen, vehicle_state)
+	# drawSplintSurface(screen, vehicle_state)
+
+	# pygame.draw.circle(screen, "white", transform(0, 0, vehicle_state), 5)
+
+	# DRAW TRIANGULATION
+	# for edge in get_offline_edges():
+	# 	v1, v2 = cones[edge.v1()], cones[edge.v2()]
+	# 	default_colour = "#676767"
+	# 	pygame.draw.line(screen, default_colour,
+	# 		transform(v1.x, v1.y, vehicle_state),
+	# 		transform(v2.x, v2.y, vehicle_state), 2
+	# 	)
+	# for edge in get_boundary_edges():
+	# 	v1, v2 = cones[edge.v1()], cones[edge.v2()]
+	# 	default_colour = "#FF0000"
+	# 	if v1.c == v2.c:
+	# 		match v1.c:
+	# 			case ConeColor.BLUE:
+	# 				default_colour = "blue"
+	# 			case ConeColor.YELLOW:
+	# 				default_colour = "yellow"
+	# 	pygame.draw.line(screen, default_colour,
+	# 		transform(v1.x, v1.y, vehicle_state),
+	# 		transform(v2.x, v2.y, vehicle_state), 1
+	# 	)
+
+	# DRAW CENTER LINE
+	# center_points = get_center_points()
+	# for i in range(len(center_points)):
+	# 	v1, v2 = center_points[i], center_points[(i+1) % len(center_points)] # returns cone objects
+	# 	default_colour = "#676767"
+	# 	pygame.draw.line(screen, default_colour,
+	# 		transform(v1.x, v1.y, vehicle_state),
+	# 		transform(v2.x, v2.y, vehicle_state), 2
+	# 	)
+
+	# DRAW PATH PROJECTION
+	# at_t = project(vehicle_state.x, vehicle_state.y)
+	# x = spline_t(at_t)
+	# pygame.draw.circle(screen, "red", transform(x.x, x.y, vehicle_state), 5)
+
+	# DRAW SPLINE
+	# steps = 100
+	# dt = get_center_line_length() / steps
+	# for t in range(steps):
+	# 	x1 = spline_t(t * dt)
+	# 	x2= spline_t((t + 1) * dt)
+	# 	pygame.draw.line(screen, "#676767",
+	# 			   transform(x1.x, x1.y, vehicle_state),
+	# 			   transform(x2.x, x2.y, vehicle_state), 2)
+
+	for cone in cones:
+		pygame.draw.circle(
+			screen, int_to_color(cone.c),
+			transform(cone.x, cone.y, vehicle_state), 0.2 * PIXELS_PER_M
+		)
+
+	car_rotated = pygame.transform.rotate(car,-90)
+	scale_factor = VEHICLE_WIDTH_M / car_rotated.get_width() * PIXELS_PER_M
+	car_scaled = pygame.transform.scale(car_rotated, (
+		car_rotated.get_width() * scale_factor, car_rotated.get_height() * scale_factor
+	))
+	car_rect = car_scaled.get_rect(center=transform(vehicle_state.x, vehicle_state.y, vehicle_state))
+	screen.blit(car_scaled, car_rect)

auto_sim/test_controller_2/.clang-format

@@ -0,0 +1,3 @@
+BasedOnStyle: WebKit
+BreakBeforeBraces: Attach
+PackConstructorInitializers: NextLine

auto_sim/test_controller_2/CMakeLists.txt

@@ -0,0 +1,53 @@
+cmake_minimum_required(VERSION 3.10)
+set(CMAKE_CXX_STANDARD 23)
+project(high_level_controller)
+
+file(
+        DOWNLOAD
+        https://github.com/cpm-cmake/CPM.cmake/releases/download/v0.40.8/CPM.cmake
+        ${CMAKE_CURRENT_BINARY_DIR}/cmake/CPM.cmake
+        EXPECTED_HASH SHA256=78ba32abdf798bc616bab7c73aac32a17bbd7b06ad9e26a6add69de8f3ae4791
+)
+include(${CMAKE_CURRENT_BINARY_DIR}/cmake/CPM.cmake)
+
+CPMAddPackage(
+        NAME pybind11
+        GIT_REPOSITORY https://github.com/pybind/pybind11.git
+        VERSION 3.0.2
+        DOWNLOAD_ONLY TRUE
+)
+add_subdirectory(${pybind11_SOURCE_DIR})
+
+CPMAddPackage(
+        NAME CDT
+        GIT_REPOSITORY https://github.com/artem-ogre/CDT.git
+        GIT_TAG 1.4.4
+        DOWNLOAD_ONLY TRUE
+)
+CPMAddPackage(
+        NAME EIGEN
+        GIT_REPOSITORY https://gitlab.com/libeigen/eigen
+        GIT_TAG 3147391d946bb4b6c68edd901f2add6ac1f31f8c
+        GIT_SHALLOW TRUE
+        DOWNLOAD_ONLY TRUE
+)
+set(EIGEN_INCLUDE_DIRS ${EIGEN_SOURCE_DIR})
+add_library(eigen_interface INTERFACE)
+target_include_directories(eigen_interface SYSTEM INTERFACE ${EIGEN_INCLUDE_DIRS})
+target_compile_definitions(eigen_interface INTERFACE
+        EIGEN_STACK_ALLOCATION_LIMIT=0
+        EIGEN_MAX_STATIC_ALIGN_BYTES=0
+)
+
+CPMAddPackage(
+        NAME alglib
+        URL https://www.alglib.net/translator/re/alglib-4.07.0.cpp.gpl.zip
+        DOWNLOAD_ONLY TRUE
+)
+add_library(alglib_interface INTERFACE)
+target_include_directories(alglib_interface INTERFACE ${alglib_SOURCE_DIR}/src)
+
+file(GLOB SOURCES *.cpp ${alglib_SOURCE_DIR}/src/*.cpp)
+pybind11_add_module(controller ${SOURCES})
+target_include_directories(controller PRIVATE ${CDT_SOURCE_DIR}/CDT/include)
+target_link_libraries(controller PRIVATE eigen_interface alglib_interface)

auto_sim/test_controller_2/CMakePresets.json

@@ -0,0 +1,19 @@
+{
+  "version": 10,
+  "configurePresets": [
+    {
+      "name": "default_win",
+      "generator": "Visual Studio 17 2022",
+      "binaryDir": "${sourceDir}/build",
+      "cacheVariables": {
+      }
+    },
+    {
+      "name": "default_unix",
+      "generator": "Ninja",
+      "binaryDir": "${sourceDir}/build",
+      "cacheVariables": {
+      }
+    }
+  ]
+}

auto_sim/test_controller_2/README.md

@@ -0,0 +1,9 @@
+# Development
+To develop, set up as per usual as a CMake project.
+
+# Installation
+```bash
+pip install ./test_controller_2
+```
+
+This will trigger setup.py which will automatically install the python package into the environment of your choosing

auto_sim/test_controller_2/controller.cpp

@@ -0,0 +1,6 @@
+#include "controller.hpp"
+
+ControlOutput compute(const VehicleState& ve) {
+    // t.adj will contain the adj list
+    return { 0, 0 };
+}