diff --git a/.gitignore b/.gitignore
index 0c36d682f..3da224293 100644
--- a/.gitignore
+++ b/.gitignore
@@ -165,3 +165,13 @@ cython_debug/
 #  and can be added to the global gitignore or merged into this file.  For a more nuclear
 #  option (not recommended) you can uncomment the following to ignore the entire idea folder.
 #.idea/
+
+# ZED run files
+**/*.run
+.vscode/
+setup/zed_sdk.run
+cuda/
+GEMstack/knowledge/detection/yolov8n.pt
+GEMstack/knowledge/detection/yolo11n.pt
+yolov8n.pt
+yolo11n.pt
\ No newline at end of file
diff --git a/GEMstack/knowledge/defaults/e2.yaml b/GEMstack/knowledge/defaults/e2.yaml
new file mode 100644
index 000000000..ed8d26d19
--- /dev/null
+++ b/GEMstack/knowledge/defaults/e2.yaml
@@ -0,0 +1,34 @@
+# ********* Main settings entry point for behavior stack ***********
+
+# Configure settings for the vehicle / vehicle model
+vehicle:  !include ../vehicle/gem_e2.yaml
+
+#arguments for algorithm components here
+model_predictive_controller:
+    dt: 0.1
+    lookahead: 20
+control:
+    recovery:
+        brake_amount : 0.5
+        brake_speed : 2.0
+    pure_pursuit:
+        lookahead: 2.0
+        lookahead_scale: 3.0
+        crosstrack_gain: 1.0
+        desired_speed: trajectory
+    longitudinal_control:
+        pid_p: 1.0
+        pid_i: 0.1
+        pid_d: 0.0
+
+#configure the simulator, if using
+simulator:
+    dt: 0.01
+    real_time_multiplier: 1.0    # make the simulator run faster than real time by making this > 1
+    gnss_emulator: 
+        dt: 0.1    #10Hz
+        #position_noise: 0.1  #10cm noise
+        #orientation_noise: 0.04  #2.3 degrees noise
+        #velocity_noise:
+        #    constant: 0.04  #4cm/s noise
+        #    linear: 0.02    #2% noise
\ No newline at end of file
diff --git a/GEMstack/onboard/interface/gem_gazebo.py b/GEMstack/onboard/interface/gem_gazebo.py
new file mode 100644
index 000000000..11d97736e
--- /dev/null
+++ b/GEMstack/onboard/interface/gem_gazebo.py
@@ -0,0 +1,257 @@
+from .gem import *
+from ...utils import settings
+import math
+import time
+
+# ROS Headers
+import rospy
+from sensor_msgs.msg import Image, PointCloud2, Imu, NavSatFix
+from septentrio_gnss_driver.msg import INSNavGeod
+from geometry_msgs.msg import Vector3Stamped
+from sensor_msgs.msg import JointState  # For reading joint states from Gazebo
+# Changed from AckermannDriveStamped
+from ackermann_msgs.msg import AckermannDrive
+from rosgraph_msgs.msg import Clock
+from tf.transformations import euler_from_quaternion
+
+
+from ...state import ObjectPose,ObjectFrameEnum
+from ...knowledge.vehicle.geometry import steer2front
+from ...knowledge.vehicle.dynamics import pedal_positions_to_acceleration
+
+# OpenCV and cv2 bridge
+import cv2
+import numpy as np
+from ...utils import conversions
+from ...mathutils import transforms
+
+
+@dataclass
+class GNSSReading:
+    pose: ObjectPose
+    speed: float
+    status: str
+
+
+class GEMGazeboInterface(GEMInterface):
+    """Interface for connecting to the GEM e2 vehicle in Gazebo simulation."""
+
+    def __init__(self):
+        GEMInterface.__init__(self)
+        self.max_send_rate = settings.get('vehicle.max_command_rate', 10.0)
+        self.ros_sensor_topics = settings.get('vehicle.sensors.ros_topics')
+        self.last_command_time = 0.0
+        self.last_reading = GEMVehicleReading()
+        self.last_reading.speed = 0.0
+        self.last_reading.steering_wheel_angle = 0.0
+        self.last_reading.accelerator_pedal_position = 0.0
+        self.last_reading.brake_pedal_position = 0.0
+        self.last_reading.gear = 1
+        self.last_reading.left_turn_signal = False
+        self.last_reading.right_turn_signal = False
+        self.last_reading.horn_on = False
+        self.last_reading.wiper_level = 0
+        self.last_reading.headlights_on = False
+
+
+
+
+
+        # GNSS data subscriber
+        self.gnss_sub = None
+
+        # Other sensors
+        self.front_camera_sub = None
+        self.front_depth_sub = None
+        self.top_lidar_sub = None
+        self.front_radar_sub = None
+
+        self.faults = []
+
+        # Gazebo vehicle control
+        self.ackermann_pub = rospy.Publisher(
+            '/ackermann_cmd', AckermannDrive, queue_size=1)
+        self.ackermann_cmd = AckermannDrive()
+        self.last_command = None  # Store the last command
+
+        # Add clock subscription for simulation time
+        self.sim_time = rospy.Time(0)
+        self.clock_sub = rospy.Subscriber('/clock', Clock, self.clock_callback)
+
+    def start(self):
+        print("Starting GEM Gazebo Interface")
+
+    def clock_callback(self, msg):
+        self.sim_time = msg.clock
+
+    def time(self):
+        # Return Gazebo simulation time
+        return self.sim_time.to_sec()
+
+    def get_reading(self) -> GEMVehicleReading:
+        return self.last_reading
+
+
+    def subscribe_sensor(self, name, callback, type=None):
+        if name == 'gnss':
+            topic = self.ros_sensor_topics['gnss']
+            def gnss_callback_wrapper(gnss_msg: INSNavGeod):
+                roll, pitch, yaw = gnss_msg.roll, gnss_msg.pitch, gnss_msg.heading
+                # Convert from degrees to radians
+                roll, pitch, yaw = math.radians(roll), math.radians(pitch), math.radians(yaw)
+
+                # Transform yaw to correct frame - Gazebo typically uses ROS standard frame (x-forward)
+                # while navigation uses x-east reference frame
+                # Need to convert from Gazebo's frame to navigation heading, then to navigation yaw
+
+                # Assuming Gazebo's yaw is 0 when facing east (ROS REP 103 convention)
+                # Convert IMU's yaw to heading (CW from North), then to navigation yaw (CCW from East)
+                # This handles the coordinate frame differences between Gazebo and the navigation frame
+                # Negate yaw to convert from ROS to heading
+                heading = transforms.yaw_to_heading(-yaw - np.pi/2, degrees=False)
+                navigation_yaw = transforms.heading_to_yaw(
+                    heading, degrees=False)
+
+                # Create fused pose with transformed yaw
+                pose = ObjectPose(
+                    frame=ObjectFrameEnum.GLOBAL,
+                    t=gnss_msg.header.stamp,
+                    x=gnss_msg.longitude,
+                    y=gnss_msg.latitude,
+                    z=gnss_msg.height,
+                    roll=roll,
+                    pitch=pitch,
+                    yaw=navigation_yaw
+                )
+
+                # Calculate speed from GNSS
+                self.last_reading.speed = np.linalg.norm([gnss_msg.ve, gnss_msg.vn])
+
+                # Create GNSS reading with fused data
+                reading = GNSSReading(
+                    pose=pose,
+                    speed=self.last_reading.speed,
+                    status='error' if gnss_msg.error else 'ok'
+                )
+                # Added debug
+                print(
+                    f"[GNSS] Raw coordinates: Lat={gnss_msg.latitude:.6f}, Lon={gnss_msg.longitude:.6f}")
+                # Added debug
+                print(
+                    f"[GNSS-FUSED] Orientation: Roll={roll:.2f}, Pitch={pitch:.2f}, Yaw={yaw:.2f} rad")
+                # Added debug
+                print(f"[GNSS-FUSED] Speed: {self.last_reading.speed:.2f} m/s")
+
+                callback(reading)
+
+            self.gnss_sub = rospy.Subscriber(topic, INSNavGeod, gnss_callback_wrapper)
+
+        elif name == 'top_lidar':
+            topic = self.ros_sensor_topics[name]
+            if type is not None and (type is not PointCloud2 and type is not np.ndarray):
+                raise ValueError("GEMGazeboInterface only supports PointCloud2 or numpy array for top lidar")
+            if type is None or type is PointCloud2:
+                self.top_lidar_sub = rospy.Subscriber(topic, PointCloud2, callback)
+            else:
+                def callback_with_numpy(msg: PointCloud2):
+                    points = conversions.ros_PointCloud2_to_numpy(msg, want_rgb=False)
+                    callback(points)
+                self.top_lidar_sub = rospy.Subscriber(topic, PointCloud2, callback_with_numpy)
+
+        elif name == 'front_camera':
+            topic = self.ros_sensor_topics[name]
+            if type is not None and (type is not Image and type is not cv2.Mat):
+                raise ValueError("GEMGazeboInterface only supports Image or OpenCV for front camera")
+            if type is None or type is Image:
+                self.front_camera_sub = rospy.Subscriber(topic, Image, callback)
+            else:
+                def callback_with_cv2(msg: Image):
+                    cv_image = conversions.ros_Image_to_cv2(msg, desired_encoding="bgr8")
+                    callback(cv_image)
+                self.front_camera_sub = rospy.Subscriber(topic, Image, callback_with_cv2)
+        # Front depth sensor has not been added to gazebo yet.
+        # This code is placeholder until we add front depth sensor.
+        elif name == 'front_depth':
+            topic = self.ros_sensor_topics[name]
+            if type is not None and (type is not Image and type is not cv2.Mat):
+                raise ValueError("GEMGazeboInterface only supports Image or OpenCV for front depth")
+            if type is None or type is Image:
+                self.front_depth_sub = rospy.Subscriber(topic, Image, callback)
+            else:
+                def callback_with_cv2(msg: Image):
+                    cv_image = conversions.ros_Image_to_cv2(msg, desired_encoding="passthrough")
+                    callback(cv_image)
+                self.front_depth_sub = rospy.Subscriber(topic, Image, callback_with_cv2)
+
+    def hardware_faults(self) -> List[str]:
+        # In simulation, we don't have real hardware faults
+        return self.faults
+
+    def send_command(self, command : GEMVehicleCommand):
+        # Throttle rate at which we send commands
+        t = self.time()
+        if t < self.last_command_time + 1.0/self.max_send_rate:
+            # Skip command, similar to hardware interface
+            return
+        self.last_command_time = t
+
+        # Get current speed
+        v = self.last_reading.speed
+
+
+        #update last reading
+        self.last_reading.accelerator_pedal_position = command.accelerator_pedal_position
+        self.last_reading.brake_pedal_position = command.brake_pedal_position
+        self.last_reading.steering_wheel_angle = command.steering_wheel_angle
+
+        # Convert pedal to acceleration
+        accelerator_pedal_position = np.clip(command.accelerator_pedal_position, 0.0, 1.0)
+        brake_pedal_position = np.clip(command.brake_pedal_position, 0.0, 1.0)
+
+        # Zero out accelerator if brake is active (just like hardware interface)
+        if brake_pedal_position > 0.0:
+            accelerator_pedal_position = 0.0
+
+        # Calculate acceleration from pedal positions
+        acceleration = pedal_positions_to_acceleration(accelerator_pedal_position, brake_pedal_position, v, 0, 1)
+
+        # Apply reasonable limits to acceleration
+        max_accel = settings.get('vehicle.limits.max_acceleration', 1.0)
+        max_decel = settings.get('vehicle.limits.max_deceleration', -2.0)
+        acceleration = np.clip(acceleration, max_decel, max_accel)
+
+        # Convert wheel angle to steering angle (front wheel angle)
+        phides = steer2front(command.steering_wheel_angle)
+
+        # Apply steering angle limits
+        min_wheel_angle = settings.get('vehicle.geometry.min_wheel_angle', -0.6)
+        max_wheel_angle = settings.get('vehicle.geometry.max_wheel_angle', 0.6)
+        phides = np.clip(phides, min_wheel_angle, max_wheel_angle)
+
+        # Calculate target speed based on acceleration
+        # Don't use infinite speed, instead calculate a reasonable target speed
+        current_speed = v
+        target_speed = current_speed
+
+        if acceleration > 0:
+            # Accelerating - set target speed to current speed plus some increment
+            # This is more realistic than infinite speed
+            max_speed = settings.get('vehicle.limits.max_speed', 10.0)
+            target_speed = min(current_speed + acceleration * 0.5, max_speed)
+        elif acceleration < 0:
+            # Braking - set target speed to zero if deceleration is significant
+            if brake_pedal_position > 0.1:
+                target_speed = 0.0
+
+        # Create and publish drive message
+        msg = AckermannDrive()
+        msg.acceleration = acceleration
+        msg.speed = target_speed
+        msg.steering_angle = phides
+        msg.steering_angle_velocity = command.steering_wheel_speed  # Respect steering velocity limit
+
+        # Debug output
+        print(f"[ACKERMANN] Speed: {msg.speed:.2f}, Accel: {msg.acceleration:.2f}, Steer: {msg.steering_angle:.2f}")
+
+        self.ackermann_pub.publish(msg)
+        self.last_command = command
diff --git a/GEMstack/onboard/perception/test_yolo_gazebo_simulation.py b/GEMstack/onboard/perception/test_yolo_gazebo_simulation.py
new file mode 100644
index 000000000..2bd124781
--- /dev/null
+++ b/GEMstack/onboard/perception/test_yolo_gazebo_simulation.py
@@ -0,0 +1,166 @@
+"""
+Demo script to run YOLO object detection on a Gazebo simulation.
+
+This code subscribes to the front camera feed from the GEM e2/e4 model and applies YOLO-based object detection to the incoming images.
+
+Visualization:
+- Use RViz or rqt to monitor the topics.
+
+ROS Topics:
+- Raw camera feed: /gem/debug
+- YOLO detection output: /gem/image_detection
+"""
+
+
+# Python 
+import os
+from typing import List, Dict
+from collections import defaultdict
+from datetime import datetime
+import copy
+# ROS, CV
+import rospy
+import message_filters
+import cv2
+import numpy as np
+import tf
+from cv_bridge import CvBridge
+from sensor_msgs.msg import Image, PointCloud2
+from visualization_msgs.msg import MarkerArray
+# YOLO
+from ultralytics import YOLO
+from ultralytics.engine.results import Results, Boxes
+# GEMStack
+from ...state import AllState,VehicleState,ObjectPose,ObjectFrameEnum,AgentState,AgentEnum,AgentActivityEnum,ObjectFrameEnum
+from ..interface.gem import GEMInterface, GNSSReading
+from ..component import Component
+from scipy.spatial.transform import Rotation as R
+
+
+
+class ObjectDetection(Component):
+
+    def __init__(self, vehicle_interface : GEMInterface) -> None:
+        """
+        Initializes essential functions required to run the YOLO model.
+        """
+
+        # vehicle interface
+        self.vehicle_interface = vehicle_interface
+        # cv2 to ros converter
+        self.bridge = CvBridge()
+
+        # yolo model
+        self.detector = YOLO(os.getcwd()+'/GEMstack/knowledge/detection/yolov8n.pt')
+        self.confidence = 0.1
+        self.classes_to_detect = 0
+
+   
+    def initialize(self):
+        """
+        Defines callback functions for subscribing to the front camera image stream and sets up publishers for debugging purposes.
+        """
+
+        super().initialize()
+        self.vehicle_interface.subscribe_sensor('front_camera',self.front_camera_callback, type = cv2.Mat)
+        self.pub_image = rospy.Publisher("/gem/debug", Image, queue_size=1)
+        self.pub_detimage = rospy.Publisher("/gem/image_detection", Image, queue_size=1)
+
+
+    def update(self, vehicle : VehicleState) -> Dict[str,AgentState]:
+
+        """
+        Displays vehicle statistics in the GLOBAL reference frame.
+
+        This function also allows switching between coordinate frames using the `VehicleState -> pose` method.
+
+        Returning an `AgentState` will automatically log detected objects.
+        """
+
+
+        print(f"VEHICLE State at time: {vehicle.pose.t}")
+
+        print(f"x: {vehicle.pose.x}")
+        print(f"y: {vehicle.pose.y}")
+
+        print(f"z: {vehicle.pose.z}")
+        print(f"roll: {vehicle.pose.roll}")
+        print(f"pitch: {vehicle.pose.pitch}")
+        print(f"yaw: {vehicle.pose.yaw}")
+        print(f"speed: {vehicle.v}")
+
+
+        return {}
+
+
+
+    def front_camera_callback(self, image: cv2.Mat):
+        
+        """
+        A simple callback function that processes incoming images, runs them through a YOLO model, and visualizes the detections.
+        """
+
+
+        ros_img = self.bridge.cv2_to_imgmsg(image, 'bgr8')
+        self.pub_image.publish(ros_img)
+
+        track_result = self.detector.track(source=image, persist=True, conf=self.confidence)
+
+        class_names = self.detector.names
+        label_text = "Object "
+        font = cv2.FONT_HERSHEY_SIMPLEX
+        font_scale = 0.5
+        font_color = (255, 255, 255)  # White text
+        outline_color = (0, 0, 0)  # Black outline
+        line_type = 2
+        text_thickness = 1 # Text thickness
+        outline_thickness = 1  # Thickness of the text outline
+
+        boxes = track_result[0].boxes
+        for box in boxes:
+
+            
+            class_id = int(box.cls.item())
+            label_text = class_names[class_id]
+            xywh = box.xywh[0].tolist()
+            x, y, w, h = xywh
+            id = box.id.item()
+
+            # Draw bounding box
+            cv2.rectangle(image, (int(x - w / 2), int(y - h / 2)), (int(x + w / 2), int(y + h / 2)), (255, 0, 255), 3)
+
+            # Define text label
+            x = int(x - w / 2)
+            y = int(y - h / 2)
+            label = label_text + str(id) + " : " + str(round(box.conf.item(), 2))
+
+            # Get text size
+            text_size, baseline = cv2.getTextSize(label, font, font_scale, line_type)
+            text_w, text_h = text_size
+
+            # Position text above the bounding box
+            text_x = x
+            text_y = y - 10 if y - 10 > 10 else y + h + text_h
+
+            # Draw text outline for better visibility
+            for dx, dy in [(-1, -1), (-1, 1), (1, -1), (1, 1)]:  
+                cv2.putText(image, label, (text_x + dx, text_y - baseline + dy), font, font_scale, outline_color, outline_thickness)
+
+            # Draw main text on top of the outline
+            cv2.putText(image, label, (text_x, text_y - baseline), font, font_scale, font_color, text_thickness)
+
+
+            
+        ros_img = self.bridge.cv2_to_imgmsg(image, 'bgr8')
+        self.pub_detimage.publish(ros_img)
+
+
+    def rate(self):
+        return 4.0
+    
+    def state_inputs(self):
+        return ['vehicle']
+    
+    def state_outputs(self):
+        return ['agents']
+
diff --git a/GEMstack/onboard/visualization/mpl_visualization.py b/GEMstack/onboard/visualization/mpl_visualization.py
index ef9a24851..08392e9ed 100644
--- a/GEMstack/onboard/visualization/mpl_visualization.py
+++ b/GEMstack/onboard/visualization/mpl_visualization.py
@@ -5,6 +5,8 @@
 import matplotlib.animation as animation
 import time
 from collections import deque
+from ...state.agent import AgentEnum
+import numpy as np
 
 class MPLVisualization(Component):
     """Runs a matplotlib visualization at 10Hz. 
@@ -21,8 +23,12 @@ def __init__(self, rate : float = 10.0, save_as : str = None):
         self.axs = None
         self.tstart = 0
         self.plot_t_range = 10
-        self.plot_values = {}
-        self.plot_events = {}
+
+        # Separate vehicle and pedestrian tracking
+        self.vehicle_plot_values = {}
+        self.pedestrian_plot_values = {}
+        self.vehicle_plot_events = {}
+        self.pedestrian_plot_events = {}
 
     def rate(self) -> float:
         return self._rate
@@ -41,7 +47,7 @@ def initialize(self):
             self.writer.setup(plt.gcf(), self.save_as, dpi=100)
         plt.ion()
         # to run GUI event loop
-        self.fig,self.axs = plt.subplots(1,2,figsize=(12,6))
+        self.fig,self.axs = plt.subplots(1,3,figsize=(18,6))
         self.fig.canvas.mpl_connect('close_event', self.on_close)
         plt.show(block=False)
         self.tstart = time.time()
@@ -53,30 +59,48 @@ def on_close(self,event):
     def debug(self, source, item, value):
         t = time.time() - self.tstart
         item = source+'.'+item
-        if item not in self.plot_values:
-            self.plot_values[item] = deque()
-        plot = self.plot_values[item]
-        self.plot_values[item].append((t,value))
+        # Determine which plot dict to use based on source
+        if source.startswith('ped_'):
+            target_dict = self.pedestrian_plot_values
+        else:
+            target_dict = self.vehicle_plot_values
+            
+        if item not in target_dict:
+            target_dict[item] = deque()
+        plot = target_dict[item]
+        plot.append((t,value))
         while t - plot[0][0] > self.plot_t_range:
             plot.popleft()
 
     def debug_event(self, source, event):
         t = time.time() - self.tstart
         event = source+'.'+event
-        if event not in self.plot_events:
-            self.plot_events[event] = deque()
-        plot = self.plot_events[event]
+        target_dict = self.pedestrian_plot_events if source.startswith('ped_') else self.vehicle_plot_events
+        
+        if event not in target_dict:
+            target_dict[event] = deque()
+        plot = target_dict[event]
         plot.append(t)
         while t - plot[0] > self.plot_t_range:
             plot.popleft()
 
     def update(self, state):
         if not plt.fignum_exists(self.fig.number):
-            #plot closed
             return
         self.num_updates += 1
-        self.debug("vehicle","velocity",state.vehicle.v)
-        self.debug("vehicle","front wheel angle",state.vehicle.front_wheel_angle)
+        
+        # Vehicle metrics
+        self.debug("vehicle", "velocity", state.vehicle.v)
+        self.debug("vehicle", "front_wheel_angle", state.vehicle.front_wheel_angle)
+        
+        # Pedestrian metrics
+        for agent_id, agent in state.agents.items():
+            if agent.type == AgentEnum.PEDESTRIAN:
+                # self.debug(f"ped_{agent_id}", "x", agent.pose.x)
+                # self.debug(f"ped_{agent_id}", "y", agent.pose.y)
+                self.debug(f"ped_{agent_id}", "velocity", np.linalg.norm(agent.velocity))  # Magnitude of resultant velocity
+                self.debug(f"ped_{agent_id}", "yaw_rate", agent.yaw_rate)
+        
         time_str = time.strftime("%Y-%m-%d %H:%M:%S", time.localtime(state.t))
         #frame=ObjectFrameEnum.CURRENT
         #state = state.to_frame(frame)
@@ -84,18 +108,27 @@ def update(self, state):
         yrange = [state.vehicle.pose.y - 10, state.vehicle.pose.y + 10]
         #plot main visualization
         mpl_visualization.plot(state,title="Scene %d at %s"%(self.num_updates,time_str),xrange=xrange,yrange=yrange,show=False,ax=self.axs[0])
-        #plot figure on axs[1]
+        
+        # Vehicle plot (axs[1])
         self.axs[1].clear()
-        for k,v in self.plot_values.items():
+        for k,v in self.vehicle_plot_values.items():
             t = [x[0] for x in v]
             y = [x[1] for x in v]
             self.axs[1].plot(t,y,label=k)
-        for i,(k,v) in enumerate(self.plot_events.items()):
-            for t in v:
-                self.axs[1].axvline(x=t,linestyle='--',color='C'+str(i),label=k)
+        self.axs[1].set_title('Vehicle Metrics')
         self.axs[1].set_xlabel('Time (s)')
         self.axs[1].legend()
 
+        # Pedestrian plot (axs[2])
+        self.axs[2].clear()
+        for k,v in self.pedestrian_plot_values.items():
+            t = [x[0] for x in v]
+            y = [x[1] for x in v]
+            self.axs[2].plot(t,y,label=k)
+        self.axs[2].set_title('Pedestrian Metrics')
+        self.axs[2].set_xlabel('Time (s)')
+        self.axs[2].legend()
+
         self.fig.canvas.draw_idle()
         self.fig.canvas.flush_events()
 
diff --git a/README.md b/README.md
index 51f26e290..b66f78c3c 100644
--- a/README.md
+++ b/README.md
@@ -11,25 +11,103 @@
 GEMstack uses Python 3.7+ and ROS Noetic.  (It is possible to do some offline and simulation work without ROS, but it is highly recommended to install it if you are working on any onboard behavior or training for rosbag files.)  
 
 You should also have the following Python dependencies installed, which you can install from this folder using `pip install -r requirements.txt`:
+- GEMstack Dependencies
+  - numpy
+  - scipy
+  - matplotlib
+  - opencv-python
+  - torch
+  - klampt==0.9.2
+  - shapely
+  - dacite
+  - pyyaml
+- Perception Dependencies
+  - ultralytics
 
-- numpy
-- scipy
-- matplotlib
-- opencv-python
-- torch
-- klampt
-- shapely
-- dacite
-- pyyaml
+In order to interface with the actual GEM e2 vehicle, you will need [PACMOD2](https://github.com/astuff/pacmod2) - Autonomoustuff's low level interface to vehicle. You will also need Autonomoustuff's [sensor message packages](https://github.com/astuff/astuff_sensor_msgs).  The onboard computer uses Ubuntu 20.04 with Python 3.8, CUDA 11.6, and NVIDIA driver 515, so to minimize compatibility issues you should ensure that these are installed on your development system.
 
+## Running the stack on Ubuntu 20.04 without Docker
+### Checking CUDA Version
 
-In order to interface with the actual GEM e2 vehicle, you will need [PACMOD2](https://github.com/astuff/pacmod2) - Autonomoustuff's low level interface to vehicle. You will also need Autonomoustuff's [sensor message packages](https://github.com/astuff/astuff_sensor_msgs).  The onboard computer uses Ubuntu 20.04 with Python 3.8, CUDA 11.6, and NVIDIA driver 515, so to minimize compatibility issues you should ensure that these are installed on your development system.
+Before proceeding, check your Nvidia Driver and supported CUDA version:
+```bash
+nvidia-smi
+```
+This will show your NVIDIA driver version and the maximum supported CUDA version. Make sure you have CUDA 11.8 or 12+ installed.
+
+From Ubuntu 20.04 install [CUDA 11.6](https://gist.github.com/ksopyla/bf74e8ce2683460d8de6e0dc389fc7f5) or [CUDA 12+](https://gist.github.com/ksopyla/ee744bf013c83e4aa3fc525634d893c9) based on your current Nvidia Driver versio.
+
+To check the currently installed CUDA version:
+```bash
+nvcc --version
+```
+you can install the dependencies or GEMstack by running `setup/setup_this_machine.sh` from the top-level GEMstack folder.
+
+## Running the stack on Ubuntu 20.04 or 22.04 with Docker
+> [!NOTE]
+> Make sure to check the Nvidia Driver and supported CUDA version before proceeding by following the steps in the previous section.
 
-From a fresh Ubuntu 20.04 with ROS Noetic and [CUDA 11.6 installed](https://gist.github.com/ksopyla/bf74e8ce2683460d8de6e0dc389fc7f5), you can install these dependencies by running `setup/setup_this_machine.sh` from the top-level GEMstack folder.
+## Prerequisites
+- Docker (In Linux - Make sure to follow the post-installation steps from [here](https://docs.docker.com/engine/install/linux-postinstall/))
+- Nvidia Container Toolkit
 
-To build a Docker container with all these prerequisites, you can use the provided Dockerfile by running `docker build -t gem_stack setup/`.  For GPU support you will need the NVidia Container Runtime (run `setup/get_nvidia_container.sh` from this directory to install, or see [this tutorial](https://collabnix.com/introducing-new-docker-cli-api-support-for-nvidia-gpus-under-docker-engine-19-03-0-beta-release/) to install) and run `docker run -it --gpus all gem_stack /bin/bash`.
+Try running the sample workload from the [NVIDIA Container Toolkit](https://docs.nvidia.com/datacenter/cloud-native/container-toolkit/latest/sample-workload.html) to check if your system is compatible.
 
+```bash
+sudo docker run --rm --runtime=nvidia --gpus all ubuntu nvidia-smi
+```
+You should see the nvidia-smi output similar to [this](https://docs.nvidia.com/datacenter/cloud-native/container-toolkit/latest/sample-workload.html#:~:text=all%20ubuntu%20nvidia%2Dsmi-,Your%20output%20should%20resemble%20the%20following%20output%3A,-%2B%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2B%0A%7C%20NVIDIA%2DSMI%20535.86.10).
+
+If you see the output, you are good to go. Otherwise, you will need to install the Docker and NVidia Container Toolkit by following the instructions. 
+- For **Docker**, follow the instructions [here](https://docs.docker.com/engine/install/ubuntu/#install-using-the-repository).
+  
+- For **Nvidia Container Toolkit**, run `setup/get_nvidia_container.sh` from this directory to install, or see [this](https://docs.nvidia.com/datacenter/cloud-native/container-toolkit/latest/install-guide.html) for more details.
+
+## Building the Docker image
+
+To build a Docker image with all these prerequisites, you can use the provided Dockerfile by running.
+
+```bash
+bash setup/build_docker_image.sh
+```
+
+## Running the Docker container
+
+To run the container, you can use the provided Docker Compose file by running.
+> [!NOTE]
+> If you want to open multiple terminals to run the container, you can use the same command. It will automatically start a new terminal inside the same container.
+```bash
+bash run_docker_container.sh
+```
+## Usage Tips and Instructions
+
+### Using Host Volume
+
+You can use the host volume under the container's home directory inside the `<username>` folder. This allows you to build and run files that are on the host machine. For example, if you have a file on the host machine at `/home/<username>/project`, you can access it inside the container at `/home/<username>/host/project`.
+
+### Using Dev Containers Extension in VSCode
+
+To have a good developer environment inside the Docker container, you can use the Dev Containers extension in VSCode. Follow these steps:
+
+1. Install the Dev Containers extension in VSCode.
+2. Open the cloned repository in VSCode.
+3. Press `ctrl+shift+p`(or select the remote explorer icon from the left bar) and select `Dev-Containers: Attach to Running Container...`.
+4. Select the container name `gem_stack-container`.
+5. Once attached, Select `File->Open Folder...`.
+6. Select the folder/workspace you want to open in the container.
+
+This will set up the development environment inside the Docker container, allowing you to use VSCode features seamlessly.
+
+## Stopping the Docker container
+
+To stop the container, you can use the provided stop script by running.
+
+```bash
+bash stop_docker_container.sh
+```
 
+## Installing for Mac
+To install Ubuntu and setup ROS for Mac, follow this [link](https://doc.clickup.com/9011960452/d/h/8cjf6m4-11191/e694fcfb47a015e) for in-depth instructions and troubleshooting.
 
 ## In this folder
 
diff --git a/docs/Gazebo Simulation Documentation.md b/docs/Gazebo Simulation Documentation.md
new file mode 100644
index 000000000..a09d9616a
--- /dev/null
+++ b/docs/Gazebo Simulation Documentation.md	
@@ -0,0 +1,101 @@
+# Gazebo Simulation Documentation
+
+## Available Sensors in Gazebo
+
+The following sensors are currently available in the Gazebo simulation environment:
+
+- **Front Camera**
+- **GNSS**
+- **Lidar**
+
+---
+
+## Gazebo Simulation Setup
+
+To run your code within the Gazebo simulator, you'll first need to set up and install necessary dependencies.
+
+### 1. Set Up the Simulator
+
+Go to this [POLARIS GEM Simulator](https://github.com/harishkumarbalaji/POLARIS_GEM_Simulator/tree/main) repository to set up the Gazebo Docker container and environment for simulation.
+
+Follow the instructions in the linked repo to build and run the Docker container. It also provides a simulation environment of highbay.
+
+### 2. Install Dependencies
+
+Install the required ROS packages:
+
+```bash
+sudo apt-get install -y ros-noetic-ackermann-msgs
+```
+
+---
+
+## Configuring Your Code for Simulation
+
+To run your code with the Gazebo simulation, modify your launch file according to the following steps:
+
+1. **Add a new variant:**
+   Under the `variants` section, create a new variant called `"gazebo"`.
+
+2. **Set the vehicle interface:**
+   In the `"gazebo"` variant, set the `vehicle_interface` to use the Gazebo interface:
+   ```yaml
+   vehicle_interface:
+     type: gem_gazebo.GEMGazeboInterface
+   ```
+
+3. **Add modifications (optional):**
+   You can add any other configuration or parameters needed for your testing under the `"gazebo"` variant.
+
+Setting the `vehicle_interface` to `gem_gazebo.GEMGazeboInterface` will automatically link the **GEMStack** sensor topics with the corresponding **Gazebo vehicle sensors**, allowing you to test your code in the simulation environment.
+
+## Example Launch File
+
+For a full example, refer to the [`launch/fixed_route.yaml`](launch/fixed_route.yaml) file, which includes a sample `gazebo` variant configuration.
+
+---
+
+## Running Gazebo
+
+Follow these steps to run your GEMStack code with Gazebo:
+
+### 1. Launch the Gazebo Simulator
+
+In one terminal, run the Gazebo simulator (using the instructions provided in the [POLARIS GEM Simulator](https://github.com/harishkumarbalaji/POLARIS_GEM_Simulator/tree/main) repository). This will load the simulation environment with the vehicle and sensors.
+
+### 2. Launch the GEM Stack
+
+Open a second terminal and launch GEMStack with your configured launch file.
+
+```bash
+python3 main.py --variant=gazebo --settings={your_file}.yaml launch/{your_file}.yaml
+
+```
+- Make sure to set the variant to `gazebo`.
+- You can specify the settings file to  GEMstack/knowledge/defaults/e4_gazebo.yaml or e2_gazebo.yaml till we work on closing the sim to real gap.
+
+Example command launching the fixed route with e4 vehicle:
+
+```bash
+python3 main.py --variant=gazebo --settings=GEMstack/knowledge/defaults/e2.yaml launch/fixed_route.yaml
+or
+python3 main.py --variant=gazebo --settings=GEMstack/knowledge/defaults/current.yaml launch/fixed_route.yaml
+```
+**Variants:**
+ - sim
+ - gazebo
+
+**Vehicle types:**
+- e2
+- e4
+
+**Setting:**
+
+By default it takes GEMstack/knowledge/defaults/current.yaml which is GEM E4 Vehicle configuration settings.
+
+Other available configuration files:
+
+GEMstack/knowledge/defaults/
+- e2.yaml
+
+---
diff --git a/launch/fixed_route.yaml b/launch/fixed_route.yaml
index c05de8ff7..40e6b9a73 100644
--- a/launch/fixed_route.yaml
+++ b/launch/fixed_route.yaml
@@ -74,6 +74,15 @@ variants:
                     agent_detection : OmniscientAgentDetector
             visualization: !include "klampt_visualization.yaml"
             #visualization: !include "mpl_visualization.yaml"
+    gazebo:
+        run:
+            mode: simulation
+            vehicle_interface:
+                type: gem_gazebo.GEMGazeboInterface
+            drive:
+                perception:
+                    state_estimation: GNSSStateEstimator  # Matches your Gazebo GNSS implementation
+            # visualization: !include "mpl_visualization.yaml"
     log_ros:
         log:
             ros_topics : !include "../GEMstack/knowledge/defaults/standard_ros_topics.yaml"
\ No newline at end of file
diff --git a/launch/gazebo_simulation.yaml b/launch/gazebo_simulation.yaml
new file mode 100644
index 000000000..be77df511
--- /dev/null
+++ b/launch/gazebo_simulation.yaml
@@ -0,0 +1,96 @@
+description: "Drive the GEM vehicle along a fixed route (currently xyhead_highbay_backlot_p.csv)"
+mode: hardware
+vehicle_interface: gem_hardware.GEMHardwareInterface
+# !include "../GEMstack/knowledge/defaults/computation_graph.yaml"
+mission_execution: StandardExecutor
+# Recovery behavior after a component failure
+recovery: 
+    planning: 
+        trajectory_tracking:
+            type: recovery.StopTrajectoryTracker
+            print: False
+# Driving behavior for the GEM vehicle following a fixed route
+
+# Default settings
+drive: 
+    perception:
+        state_estimation : GNSSStateEstimator
+        perception_normalization : StandardPerceptionNormalizer
+        
+    planning:
+        route_planning:
+            type: StaticRoutePlanner
+            args: [!relative_path '../GEMstack/knowledge/routes/forward_15m.csv','start']
+        motion_planning: longitudinal_planning.YieldTrajectoryPlanner
+        trajectory_tracking:
+            type: pure_pursuit.PurePursuitTrajectoryTracker
+            print: False
+log:
+    # Specify the top-level folder to save the log files.  Default is 'logs'
+    #folder : 'logs' 
+    # If prefix is specified, then the log folder will be named with the prefix followed by the date and time. Default no prefix
+    #prefix : 'fixed_route_'  
+    # If suffix is specified, then logs will output to folder/prefix+suffix. Default uses date and time as the suffix
+    #suffix : 'test3'  
+    # Specify which ros topics to record to vehicle.bag. Default records nothing. This records the "standard" ROS topics.
+    ros_topics : []
+    # Specify options to pass to rosbag record. Default is no options.
+    #rosbag_options : '--split --size=1024' 
+    # If True, then record all readings / commands of the vehicle interface. Default False
+    vehicle_interface : True
+    # Specify which components to record to behavior.json. Default records nothing
+    components : ['state_estimation','trajectory_tracking']
+    # Specify which components of state to record to state.json. Default records nothing 
+    #state: ['all']
+    # Specify the rate in Hz at which to record state to state.json. Default records at the pipeline's rate
+    #state_rate: 10
+replay:  # Add items here to set certain topics / inputs to be replayed from logs
+    # Specify which log folder to replay from
+    log: 
+    # For replaying sensor data, try !include "../knowledge/defaults/standard_sensor_ros_topics.yaml"
+    ros_topics : []
+    components : []
+
+#usually can keep this constant
+computation_graph: !include "../GEMstack/knowledge/defaults/computation_graph.yaml"
+
+after:
+    show_log_folder: True  #set to false to avoid showing the log folder
+
+#on load, variants will overload the settings structure
+variants:
+    #sim variant doesn't execute on the real vehicle 
+    #real variant executes on the real robot
+    gazebo:
+        run:
+            mode: simulation
+            vehicle_interface:
+                type: gem_gazebo.GEMGazeboInterface
+                # args:
+                #     scene: !relative_path '../scenes/gazebo.yaml'
+
+            drive: 
+                perception:
+                    state_estimation : GNSSStateEstimator
+                    agent_detection : test_yolo_gazebo_simulation.ObjectDetection
+                    perception_normalization : StandardPerceptionNormalizer
+
+                planning:
+                    # Adding your custom relation estimation
+                    # relations_estimation: pedestrian_yield_logic.PedestrianYielder
+                    # Fixed route with pure pursuit
+                    route_planning:
+                        type: StaticRoutePlanner
+                        args: [!relative_path '../GEMstack/knowledge/routes/xyhead_highbay_backlot_p.csv'] # Fixed path
+                    motion_planning:
+                        type: RouteToTrajectoryPlanner
+                        args: [null]  #desired speed in m/s.  If null, this will keep the route untimed for the trajectory tracker
+                    trajectory_tracking:
+                        type: pure_pursuit.PurePursuitTrajectoryTracker
+                        args: {desired_speed: 2.5}  #approximately 5mph
+                        print: True
+            # visualization: !include "klampt_visualization.yaml"
+            # visualization: !include "mpl_visualization.yaml"
+    log_ros:
+        log:
+            ros_topics : !include "../GEMstack/knowledge/defaults/standard_ros_topics.yaml"
\ No newline at end of file
diff --git a/requirements.txt b/requirements.txt
index 94db8ba43..f8e808d90 100644
--- a/requirements.txt
+++ b/requirements.txt
@@ -4,6 +4,11 @@ scipy
 matplotlib
 torch
 shapely
-klampt
+klampt==0.9.2
 pyyaml
 dacite
+
+# Perception
+ultralytics
+lap==0.5.12
+open3d
\ No newline at end of file
diff --git a/run_docker_container.sh b/run_docker_container.sh
new file mode 100644
index 000000000..ac0f998b3
--- /dev/null
+++ b/run_docker_container.sh
@@ -0,0 +1,8 @@
+#!/bin/bash
+# Check if container is already running
+if [ "$(docker ps -q -f name=gem_stack-container)" ]; then
+    docker exec -it gem_stack-container bash
+else
+    UID=$(id -u) GID=$(id -g) docker compose -f setup/docker-compose.yaml up -d
+    docker exec -it gem_stack-container bash
+fi
\ No newline at end of file
diff --git a/setup/Dockerfile b/setup/Dockerfile
deleted file mode 100644
index b35c3b85d..000000000
--- a/setup/Dockerfile
+++ /dev/null
@@ -1,49 +0,0 @@
-FROM nvidia/cuda:11.8.0-cudnn8-devel-ubuntu20.04
-#use bash instead of sh
-RUN rm /bin/sh && ln -s /bin/bash /bin/sh
-RUN apt-get update && apt-get install -y git python3 python3-pip wget zstd
-# Install ROS Noetic
-RUN apt-get update && apt-get install -y lsb-release gnupg2
-RUN sh -c 'echo "deb http://packages.ros.org/ros/ubuntu focal main" > /etc/apt/sources.list.d/ros-latest.list'
-RUN wget https://raw.githubusercontent.com/ros/rosdistro/master/ros.asc
-RUN apt-key add ros.asc
-RUN apt-get update
-RUN DEBIAN_FRONTEND=noninteractive apt-get install -y ros-noetic-desktop
-RUN apt-get install -y python3-rosdep python3-rosinstall python3-rosinstall-generator python3-wstool build-essential python3-catkin-tools
-RUN rosdep init
-RUN rosdep update
-
-#Install Cuda 11.8
-#RUN wget https://developer.download.nvidia.com/compute/cuda/repos/ubuntu2004/x86_64/cuda-ubuntu2004.pin
-#RUN sudo mv cuda-ubuntu2004.pin /etc/apt/preferences.d/cuda-repository-pin-600
-##add public keys
-#RUN sudo apt-key adv --fetch-keys https://developer.download.nvidia.com/compute/cuda/repos/ubuntu2004/x86_64/3bf863cc.pub
-#RUN sudo add-apt-repository "deb http://developer.download.nvidia.com/compute/cuda/repos/ubuntu2004/x86_64/ /"
-#RUN install cuda-toolkit-11-8
-
-
-# install Zed SDK
-RUN wget https://download.stereolabs.com/zedsdk/4.0/cu118/ubuntu20 -O zed_sdk.run
-RUN chmod +x zed_sdk.run
-RUN ./zed_sdk.run -- silent
-
-# create ROS Catkin workspace
-RUN mkdir -p /catkin_ws/src
-
-# install ROS dependencies and packages
-RUN cd /catkin_ws/src && git clone https://github.com/krishauser/POLARIS_GEM_e2.git
-RUN cd /catkin_ws/src && git clone --recurse-submodules https://github.com/stereolabs/zed-ros-wrapper.git
-RUN cd /catkin_ws/src && git clone https://github.com/astuff/pacmod2.git
-   #for some reason the ibeo messages don't work?
-RUN cd /catkin_ws/src && git clone https://github.com/astuff/astuff_sensor_msgs.git && rm -rf astuff_sensor_msgs/ibeo_msgs
-RUN cd /catkin_ws/src && git clone https://github.com/ros-perception/radar_msgs.git \
-   && cd radar_msgs && git checkout noetic 
-
-RUN source /opt/ros/noetic/setup.bash && cd /catkin_ws && rosdep install --from-paths src --ignore-src -r -y
-RUN source /opt/ros/noetic/setup.bash && cd /catkin_ws && catkin_make -DCMAKE_BUILD_TYPE=Release
-
-# install GEMstack Python dependencies
-RUN git clone https://github.com/krishauser/GEMstack.git
-RUN cd GEMstack && pip3 install -r requirements.txt
-
-RUN echo /catkin_ws/devel/setup.sh
diff --git a/setup/Dockerfile.cuda11.8 b/setup/Dockerfile.cuda11.8
new file mode 100644
index 000000000..459e7dca0
--- /dev/null
+++ b/setup/Dockerfile.cuda11.8
@@ -0,0 +1,82 @@
+FROM nvidia/cuda:11.8.0-cudnn8-devel-ubuntu20.04
+
+ARG USER_UID=1000
+ARG USER_GID=1000
+ARG USER=$(whoami)
+
+ENV DEBIAN_FRONTEND=noninteractive
+
+#use bash instead of sh
+SHELL ["/bin/bash", "-c"]
+
+RUN apt-get update && apt-get install -y git python3 python3-pip wget zstd
+
+# Set time zone non-interactively
+ENV TZ=America/Chicago
+RUN ln -fs /usr/share/zoneinfo/$TZ /etc/localtime \
+    && echo $TZ > /etc/timezone \
+    && apt-get update && apt-get install -y tzdata \
+    && rm -rf /var/lib/apt/lists/*
+
+RUN wget https://stereolabs.sfo2.cdn.digitaloceanspaces.com/zedsdk/4.2/ZED_SDK_Ubuntu20_cuda11.8_v4.2.4.zstd.run -O zed_sdk.run
+RUN chmod +x zed_sdk.run
+RUN ./zed_sdk.run -- silent
+
+# Install ROS Noetic
+RUN apt-get update && apt-get install -y lsb-release gnupg2
+RUN sh -c 'echo "deb http://packages.ros.org/ros/ubuntu focal main" > /etc/apt/sources.list.d/ros-latest.list'
+RUN wget https://raw.githubusercontent.com/ros/rosdistro/master/ros.asc
+RUN apt-key add ros.asc
+RUN apt-get update
+RUN DEBIAN_FRONTEND=noninteractive apt-get install -y ros-noetic-desktop
+RUN apt-get install -y python3-rosdep python3-rosinstall python3-rosinstall-generator python3-wstool build-essential python3-catkin-tools
+RUN rosdep init
+RUN rosdep update
+
+ARG USER
+ARG USER_UID
+ARG USER_GID
+
+# Create user more safely
+RUN groupadd -g ${USER_GID} ${USER} || groupmod -n ${USER} $(getent group ${USER_GID} | cut -d: -f1)
+RUN useradd -l -m -u ${USER_UID} -g ${USER_GID} ${USER} || usermod -l ${USER} -m -u ${USER_UID} -g ${USER_GID} $(getent passwd ${USER_UID} | cut -d: -f1)
+RUN echo "${USER} ALL=(ALL) NOPASSWD:ALL" >> /etc/sudoers
+
+# Fix permissions for Python packages
+RUN chown -R ${USER}:${USER} /usr/local/lib/python3.8/dist-packages/ \
+    && chmod -R u+rw /usr/local/lib/python3.8/dist-packages/
+
+# create ROS Catkin workspace
+RUN mkdir -p /catkin_ws/src
+
+# install ROS dependencies and packages
+RUN cd /catkin_ws/src && git clone https://github.com/krishauser/POLARIS_GEM_e2.git
+RUN cd /catkin_ws/src && git clone --recurse-submodules https://github.com/stereolabs/zed-ros-wrapper.git
+RUN cd /catkin_ws/src && git clone https://github.com/astuff/pacmod2.git
+   #for some reason the ibeo messages don't work?
+RUN cd /catkin_ws/src && git clone https://github.com/astuff/astuff_sensor_msgs.git && rm -rf astuff_sensor_msgs/ibeo_msgs
+RUN cd /catkin_ws/src && git clone https://github.com/ros-perception/radar_msgs.git \
+   && cd radar_msgs && git checkout noetic 
+
+RUN source /opt/ros/noetic/setup.bash && cd /catkin_ws && rosdep install --from-paths src --ignore-src -r -y
+RUN source /opt/ros/noetic/setup.bash && cd /catkin_ws && catkin_make -DCMAKE_BUILD_TYPE=Release -j1
+
+# Copy requirements.txt from host (now relative to parent directory)
+COPY requirements.txt /tmp/requirements.txt
+
+# Install Python dependencies
+RUN pip3 install -r /tmp/requirements.txt
+
+# Install other Dependencies
+RUN apt-get install -y ros-noetic-septentrio-gnss-driver ros-noetic-ackermann-msgs
+
+USER ${USER}
+
+# Add ROS and GEMstack paths to bashrc
+RUN echo "source /opt/ros/noetic/setup.bash" >> /home/${USER}/.bashrc
+RUN echo "source /catkin_ws/devel/setup.bash" >> /home/${USER}/.bashrc
+
+# BASE END CONFIG
+WORKDIR /home/${USER}
+
+ENTRYPOINT [ "/bin/bash", "-l" ]
diff --git a/setup/Dockerfile.cuda12 b/setup/Dockerfile.cuda12
new file mode 100644
index 000000000..a7ad71e82
--- /dev/null
+++ b/setup/Dockerfile.cuda12
@@ -0,0 +1,83 @@
+FROM nvidia/cuda:12.0.0-cudnn8-devel-ubuntu20.04
+
+ARG USER_UID=1000
+ARG USER_GID=1000
+ARG USER=$(whoami)
+
+ENV DEBIAN_FRONTEND=noninteractive
+
+#use bash instead of sh
+SHELL ["/bin/bash", "-c"]
+
+RUN apt-get update && apt-get install -y git python3 python3-pip wget zstd
+
+# Set time zone non-interactively
+ENV TZ=America/Chicago
+RUN ln -fs /usr/share/zoneinfo/$TZ /etc/localtime \
+    && echo $TZ > /etc/timezone \
+    && apt-get update && apt-get install -y tzdata \
+    && rm -rf /var/lib/apt/lists/*
+
+# install Zed SDK. If you want to install a different version, change to this https://download.stereolabs.com/zedsdk/4.2/cu12/ubuntu20
+RUN wget https://stereolabs.sfo2.cdn.digitaloceanspaces.com/zedsdk/4.2/ZED_SDK_Ubuntu20_cuda12.1_v4.2.4.zstd.run -O zed_sdk.run
+RUN chmod +x zed_sdk.run
+RUN ./zed_sdk.run -- silent
+
+# Install ROS Noetic
+RUN apt-get update && apt-get install -y lsb-release gnupg2
+RUN sh -c 'echo "deb http://packages.ros.org/ros/ubuntu focal main" > /etc/apt/sources.list.d/ros-latest.list'
+RUN wget https://raw.githubusercontent.com/ros/rosdistro/master/ros.asc
+RUN apt-key add ros.asc
+RUN apt-get update
+RUN DEBIAN_FRONTEND=noninteractive apt-get install -y ros-noetic-desktop
+RUN apt-get install -y python3-rosdep python3-rosinstall python3-rosinstall-generator python3-wstool build-essential python3-catkin-tools
+RUN rosdep init
+RUN rosdep update
+
+ARG USER
+ARG USER_UID
+ARG USER_GID
+
+# Create user more safely
+RUN groupadd -g ${USER_GID} ${USER} || groupmod -n ${USER} $(getent group ${USER_GID} | cut -d: -f1)
+RUN useradd -l -m -u ${USER_UID} -g ${USER_GID} ${USER} || usermod -l ${USER} -m -u ${USER_UID} -g ${USER_GID} $(getent passwd ${USER_UID} | cut -d: -f1)
+RUN echo "${USER} ALL=(ALL) NOPASSWD:ALL" >> /etc/sudoers
+
+# Fix permissions for Python packages
+RUN chown -R ${USER}:${USER} /usr/local/lib/python3.8/dist-packages/ \
+    && chmod -R u+rw /usr/local/lib/python3.8/dist-packages/
+
+# create ROS Catkin workspace
+RUN mkdir -p /catkin_ws/src
+
+# install ROS dependencies and packages
+RUN cd /catkin_ws/src && git clone https://github.com/krishauser/POLARIS_GEM_e2.git
+RUN cd /catkin_ws/src && git clone --recurse-submodules https://github.com/stereolabs/zed-ros-wrapper.git
+RUN cd /catkin_ws/src && git clone https://github.com/astuff/pacmod2.git
+   #for some reason the ibeo messages don't work?
+RUN cd /catkin_ws/src && git clone https://github.com/astuff/astuff_sensor_msgs.git && rm -rf astuff_sensor_msgs/ibeo_msgs
+RUN cd /catkin_ws/src && git clone https://github.com/ros-perception/radar_msgs.git \
+   && cd radar_msgs && git checkout noetic 
+
+RUN source /opt/ros/noetic/setup.bash && cd /catkin_ws && rosdep install --from-paths src --ignore-src -r -y
+RUN source /opt/ros/noetic/setup.bash && cd /catkin_ws && catkin_make -DCMAKE_BUILD_TYPE=Release
+
+# Copy requirements.txt from host (now relative to parent directory)
+COPY requirements.txt /tmp/requirements.txt
+
+# Install Python dependencies
+RUN pip3 install -r /tmp/requirements.txt
+
+# Install other Dependencies
+RUN apt-get install -y ros-noetic-septentrio-gnss-driver ros-noetic-ackermann-msgs
+
+USER ${USER}
+
+# Add ROS and GEMstack paths to bashrc
+RUN echo "source /opt/ros/noetic/setup.bash" >> /home/${USER}/.bashrc
+RUN echo "source /catkin_ws/devel/setup.bash" >> /home/${USER}/.bashrc
+
+# BASE END CONFIG
+WORKDIR /home/${USER}
+
+ENTRYPOINT ["/bin/bash"]
\ No newline at end of file
diff --git a/setup/build_docker_image.sh b/setup/build_docker_image.sh
new file mode 100644
index 000000000..0852f27ab
--- /dev/null
+++ b/setup/build_docker_image.sh
@@ -0,0 +1,22 @@
+#!/bin/bash
+
+echo "Select CUDA version:"
+echo "1) CUDA 11.8"
+echo "2) CUDA 12+"
+read -p "Enter choice [1-2]: " choice
+
+case $choice in
+    1)
+        DOCKERFILE=setup/Dockerfile.cuda11.8
+        ;;
+    2)
+        DOCKERFILE=setup/Dockerfile.cuda12
+        ;;
+    *)
+        echo "Invalid choice"
+        exit 1
+        ;;
+esac
+
+export DOCKERFILE
+UID=$(id -u) GID=$(id -g) docker compose -f setup/docker-compose.yaml build
\ No newline at end of file
diff --git a/setup/docker-compose.yaml b/setup/docker-compose.yaml
new file mode 100644
index 000000000..f0c7703b8
--- /dev/null
+++ b/setup/docker-compose.yaml
@@ -0,0 +1,41 @@
+version: '3.9'
+
+services:
+  gem-stack-ubuntu-20.04-CUDA:
+    image: gem_stack
+    container_name: gem_stack-container
+    build:
+      context: ..
+      dockerfile: ${DOCKERFILE:-setup/Dockerfile.cuda11.8}  # Default to cuda11.8 if not specified
+      args:
+        USER: ${USER}
+        USER_UID: ${UID:-1000}  # Pass host UID
+        USER_GID: ${GID:-1000}  # Pass host GID
+    stdin_open: true
+    tty: true
+    volumes:
+      # - "/etc/group:/etc/group:ro"
+      # - "/etc/passwd:/etc/passwd:ro"
+      # - "/etc/shadow:/etc/shadow:ro"
+      # - "/etc/sudoers.d:/etc/sudoers.d:ro"
+      - "~:/home/${USER}/host"
+      - "/tmp/.X11-unix:/tmp/.X11-unix:rw"
+    environment:
+      - DISPLAY=${DISPLAY}
+      - XDG_RUNTIME_DIR=/tmp/runtime-${USER}
+      - DBUS_SYSTEM_BUS_ADDRESS=unix:path=/var/run/dbus/system_bus_socket
+      - DBUS_SESSION_BUS_ADDRESS=unix:path=/run/user/${UID}/bus
+      - NVIDIA_DRIVER_CAPABILITIES=all
+      - NVIDIA_VISIBLE_DEVICES=all
+      # - LIBGL_ALWAYS_SOFTWARE=1 # Uncomment if you want to use software rendering (No GPU)
+    network_mode: host
+    ipc: host
+    user: "${USER}:${USER}"
+    # Un-Comment the following lines if you want to use Nvidia GPU
+    deploy:
+      resources:
+        reservations:
+          devices:
+            - driver: nvidia
+              count: all # alternatively, use `count: all` for all GPUs
+              capabilities: [gpu]
diff --git a/setup/get_nvidia_container.sh b/setup/get_nvidia_container.sh
index 466b1989e..4f738e2c8 100755
--- a/setup/get_nvidia_container.sh
+++ b/setup/get_nvidia_container.sh
@@ -1,9 +1,14 @@
 #!/bin/bash
 
-curl -s -L https://nvidia.github.io/nvidia-container-runtime/gpgkey | \
-  sudo apt-key add -
-distribution=$(. /etc/os-release;echo $ID$VERSION_ID)
-curl -s -L https://nvidia.github.io/nvidia-container-runtime/$distribution/nvidia-container-runtime.list | \
-  sudo tee /etc/apt/sources.list.d/nvidia-container-runtime.list
+curl -fsSL https://nvidia.github.io/libnvidia-container/gpgkey | sudo gpg --dearmor -o /usr/share/keyrings/nvidia-container-toolkit-keyring.gpg \
+  && curl -s -L https://nvidia.github.io/libnvidia-container/stable/deb/nvidia-container-toolkit.list | \
+    sed 's#deb https://#deb [signed-by=/usr/share/keyrings/nvidia-container-toolkit-keyring.gpg] https://#g' | \
+    sudo tee /etc/apt/sources.list.d/nvidia-container-toolkit.list
+
+sed -i -e '/experimental/ s/^#//g' /etc/apt/sources.list.d/nvidia-container-toolkit.list
+
 sudo apt-get update
-sudo apt-get install nvidia-container-runtime
\ No newline at end of file
+sudo apt-get install -y nvidia-container-toolkit
+
+sudo nvidia-ctk runtime configure --runtime=docker
+sudo systemctl restart docker
\ No newline at end of file
diff --git a/setup/setup_this_machine.sh b/setup/setup_this_machine.sh
index e30b3aa23..36690f1d4 100755
--- a/setup/setup_this_machine.sh
+++ b/setup/setup_this_machine.sh
@@ -1,30 +1,88 @@
 #!/bin/bash
 sudo apt update
-sudo apt-get install git python3 python3-pip wget zstd
+sudo apt-get install -y git python3 python3-pip wget zstd
+
+if [ ! -f /opt/ros/noetic/setup.bash ]; then
+    echo "ROS Noetic not found. Installing ROS Noetic..."
+    sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list'
+    wget https://raw.githubusercontent.com/ros/rosdistro/master/ros.asc
+    sudo apt-key add ros.asc
+    sudo apt update
+    sudo DEBIAN_FRONTEND=noninteractive apt install -y ros-noetic-desktop
+    sudo apt install -y python3-rosdep python3-rosinstall python3-rosinstall-generator python3-wstool build-essential python3-catkin-tools
+    sudo rosdep init
+    rosdep update
+fi
 source /opt/ros/noetic/setup.bash
 
 #install Zed SDK
-wget https://download.stereolabs.com/zedsdk/4.0/cu121/ubuntu20 -O ZED_SDK_Ubuntu20_cuda11.8_v4.0.8.zstd.run
-chmod +x ZED_SDK_Ubuntu20_cuda11.8_v4.0.8.zstd.run
-./ZED_SDK_Ubuntu20_cuda11.8_v4.0.8.zstd.run -- silent
+echo "To install the ZED SDK, select the CUDA version:"
+echo "1) CUDA 11.8"
+echo "2) CUDA 12+"
+echo "3) No GPU (Skip ZED SDK installation)"
+read -p "Enter choice [1-3]: " choice
+
+case $choice in
+    1)
+        wget https://download.stereolabs.com/zedsdk/4.0/cu118/ubuntu20 -O zed_sdk.run
+        chmod +x zed_sdk.run
+        ./zed_sdk.run -- silent
+        ;;
+    2)
+        wget https://stereolabs.sfo2.cdn.digitaloceanspaces.com/zedsdk/4.2/ZED_SDK_Ubuntu20_cuda12.1_v4.2.4.zstd.run -O zed_sdk.run
+        chmod +x zed_sdk.run
+        ./zed_sdk.run -- silent
+        ;;
+    3)
+        echo "Skipping ZED SDK installation..."
+        ;;
+    *)
+        echo "Invalid choice"
+        exit 1
+        ;;
+esac
 
 #create ROS Catkin workspace
-mkdir catkin_ws
-mkdir catkin_ws/src
+mkdir -p ~/catkin_ws/src
+
+# Store current working directory
+CURRENT_DIR=$(pwd)
+echo "CURRENT_DIR: $CURRENT_DIR"
 
 #install ROS dependencies and packages
-cd catkin_ws/src
+cd ~/catkin_ws/src
 git clone https://github.com/krishauser/POLARIS_GEM_e2.git
 git clone https://github.com/astuff/pacmod2.git
-git clone https://github.com/astuff/astuff_sensor_msgs.git
-git clone https://github.com/ros-perception/radar_msgs.git
-cd radar_msgs; git checkout noetic; cd ..
+git clone https://github.com/astuff/astuff_sensor_msgs.git && rm -rf astuff_sensor_msgs/ibeo_msgs
+git clone https://github.com/ros-perception/radar_msgs.git && cd radar_msgs; git checkout noetic; cd ..
 
 cd ..   #back to catkin_ws
+rosdep update
 rosdep install --from-paths src --ignore-src -r -y
 catkin_make -DCMAKE_BUILD_TYPE=Release
 source devel/setup.bash
-cd .. #back to GEMstack
 
+cd $CURRENT_DIR
 #install GEMstack Python dependencies
-python3 -m pip install -r requirements.txt
\ No newline at end of file
+
+# Ask the user if they want to install ultralytics
+read -p "Do you want to install ultralytics? (y/n): " install_ultralytics
+
+# Create a temporary requirements file
+temp_requirements="temp_requirements.txt"
+
+# Copy all lines except ultralytics if the user chooses to skip it
+if [ "$install_ultralytics" == "y" ]; then
+    cp requirements.txt $temp_requirements
+else
+    grep -v "ultralytics" requirements.txt > $temp_requirements
+fi
+
+# Install the packages from the temporary requirements file
+pip3 install -r $temp_requirements
+
+# Clean up the temporary file
+rm $temp_requirements
+
+#install other dependencies
+sudo apt-get install -y ros-noetic-septentrio-gnss-driver
\ No newline at end of file
diff --git a/stop_docker_container.sh b/stop_docker_container.sh
new file mode 100644
index 000000000..1b1bad2c8
--- /dev/null
+++ b/stop_docker_container.sh
@@ -0,0 +1,3 @@
+#! /bin/bash
+
+docker compose -f setup/docker-compose.yaml down
\ No newline at end of file