Repository: uwsbel/autonomy-research-testbed (language: python)

Issue:
Pre-commit code formatting
Add a pre-commit code formatting check so we all use the same format.


Discussion / hints:

Fix patch:
diff --git a/.github/workflows/pre-commit.yml b/.github/workflows/pre-commit.yml
new file mode 100644
index 00000000..f46e01d4
--- /dev/null
+++ b/.github/workflows/pre-commit.yml
@@ -0,0 +1,14 @@
+name: pre-commit
+
+on:
+  pull_request:
+  push:
+    branches: [master]
+
+jobs:
+  pre-commit:
+    runs-on: ubuntu-latest
+    steps:
+    - uses: actions/checkout@v3
+    - uses: actions/setup-python@v3
+    - uses: pre-commit/action@v3.0.0
diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml
new file mode 100644
index 00000000..fba52100
--- /dev/null
+++ b/.pre-commit-config.yaml
@@ -0,0 +1,23 @@
+exclude: ^sim/data
+repos:
+  - repo: https://github.com/pre-commit/pre-commit-hooks
+    rev: v3.2.0
+    hooks:
+      - id: trailing-whitespace
+      - id: check-ast # check that python can parse files
+      - id: check-case-conflict # check for files with names that would conflict on a case-insensitive filesystem
+      - id: end-of-file-fixer
+      - id: check-yaml
+      - id: check-symlinks
+      - id: check-added-large-files
+      - id: detect-private-key
+      - id: no-commit-to-branch
+        args: ['-b', 'master']
+  - repo: https://github.com/psf/black
+    rev: 23.7.0
+    hooks:
+      - id: black
+  - repo: https://github.com/pre-commit/mirrors-clang-format
+    rev: v17.0.2
+    hooks:
+      - id: clang-format
diff --git a/README.md b/README.md
index d8d14c58..37d809e5 100644
--- a/README.md
+++ b/README.md
@@ -1,6 +1,6 @@
 # Autonomy Research Testbed (ART)
 
-ART is meant to be a testbed for automated driving algorithm development and is to be used as a mechanism for validating the simulation platform [Chrono](https://projectchrono.org). The ART platform is under activate development by the [Simulation Based Engineering Laboratory](https://sbel.wisc.edu) at the [University of Wisconsin-Madison](https://wisc.edu). 
+ART is meant to be a testbed for automated driving algorithm development and is to be used as a mechanism for validating the simulation platform [Chrono](https://projectchrono.org). The ART platform is under activate development by the [Simulation Based Engineering Laboratory](https://sbel.wisc.edu) at the [University of Wisconsin-Madison](https://wisc.edu).
 
 ## Background
 
diff --git a/atk.yml b/atk.yml
index f0dbc588..b8cbe79b 100644
--- a/atk.yml
+++ b/atk.yml
@@ -27,12 +27,12 @@ x-optionals:
 name: art
 services:
   common:
-    env_file: 
+    env_file:
       - atk.env
     build:
       context: "./"
       network: "host"
-      args: 
+      args:
         PROJECT: "${COMPOSE_PROJECT_NAME}"
         ROS_DISTRO: "${ROS_DISTRO}"
         USER_UID: "${USER_UID:-1000}"
diff --git a/docker/chrono.dockerfile b/docker/chrono.dockerfile
index 3ed2176f..386c22fb 100644
--- a/docker/chrono.dockerfile
+++ b/docker/chrono.dockerfile
@@ -2,7 +2,7 @@
 # SPDX-License-Identifier: MIT
 
 # Syntax provided by the devthefuture/dockerfile-x project
-# Will copy in the base configuration for the build 
+# Will copy in the base configuration for the build
 INCLUDE ./docker/common/base.dockerfile
 
 # Snippets
diff --git a/docker/common/base.dockerfile b/docker/common/base.dockerfile
index 952e9543..c6312021 100644
--- a/docker/common/base.dockerfile
+++ b/docker/common/base.dockerfile
@@ -39,4 +39,4 @@ RUN adduser --shell /bin/${USERSHELL} --disabled-password --gecos "" ${USERNAME}
             getent group $g || groupadd $g; \
             usermod -aG $g ${USERNAME}; \
         done; \
-    fi
\ No newline at end of file
+    fi
diff --git a/docker/common/common.dockerfile b/docker/common/common.dockerfile
index b0cd0c2a..0c224501 100644
--- a/docker/common/common.dockerfile
+++ b/docker/common/common.dockerfile
@@ -17,4 +17,4 @@ ARG DEFAULT_SHELL_ADD_ONS="export TERM=xterm-256color"
 ARG USER_SHELL_ADD_ONS=""
 RUN echo "${DEFAULT_SHELL_ADD_ONS}" >> ${USERSHELLPROFILE} && \
         sed -i 's/#force_color_prompt=yes/force_color_prompt=yes/' ${USERSHELLPROFILE} && \
-        [ -z "${USER_SHELL_ADD_ONS}" ] || echo "${USER_SHELL_ADD_ONS}" >> ${USERSHELLPROFILE}
\ No newline at end of file
+        [ -z "${USER_SHELL_ADD_ONS}" ] || echo "${USER_SHELL_ADD_ONS}" >> ${USERSHELLPROFILE}
diff --git a/docker/common/final.dockerfile b/docker/common/final.dockerfile
index 1ce8a2e6..8fe732f4 100644
--- a/docker/common/final.dockerfile
+++ b/docker/common/final.dockerfile
@@ -7,4 +7,4 @@ ENV HOME=${USERHOME}
 ENV USERSHELLPATH=${USERSHELLPATH}
 ENV USERSHELLPROFILE=${USERSHELLPROFILE}
 
-CMD ${USERSHELLPATH}
\ No newline at end of file
+CMD ${USERSHELLPATH}
diff --git a/docker/dev.dockerfile b/docker/dev.dockerfile
index 25a0fac8..72b88fcc 100644
--- a/docker/dev.dockerfile
+++ b/docker/dev.dockerfile
@@ -2,7 +2,7 @@
 # SPDX-License-Identifier: MIT
 
 # Syntax provided by the devthefuture/dockerfile-x project
-# Will copy in the base configuration for the build 
+# Will copy in the base configuration for the build
 INCLUDE ./docker/common/base.dockerfile
 
 # Snippets
diff --git a/docker/snippets/rosdep.dockerfile b/docker/snippets/rosdep.dockerfile
index 95798c3e..2fef7628 100644
--- a/docker/snippets/rosdep.dockerfile
+++ b/docker/snippets/rosdep.dockerfile
@@ -17,4 +17,4 @@ RUN . /opt/ros/${ROS_DISTRO}/setup.sh && \
     apt-get update && \
     rosdep install --from-paths src --ignore-src -r -y && \
     rm -rf /tmp/workspace && \
-    apt-get clean && apt-get autoremove -y && rm -rf /var/lib/apt/lists/*
\ No newline at end of file
+    apt-get clean && apt-get autoremove -y && rm -rf /var/lib/apt/lists/*
diff --git a/docker/vnc.dockerfile b/docker/vnc.dockerfile
index 8ac6d15f..b4bcf4fd 100644
--- a/docker/vnc.dockerfile
+++ b/docker/vnc.dockerfile
@@ -59,5 +59,3 @@ RUN echo "\n\
     autorestart=true" > /opt/supervisord.conf
 
 CMD ["set -ex; exec supervisord -c /opt/supervisord.conf"]
-
-
diff --git a/requirements.txt b/requirements.txt
index 504e69a1..bb30e34e 100644
--- a/requirements.txt
+++ b/requirements.txt
@@ -1,1 +1,2 @@
 autonomy-toolkit
+pre-commit
diff --git a/sim/cpp/.gitignore b/sim/cpp/.gitignore
index d1638636..567609b1 100644
--- a/sim/cpp/.gitignore
+++ b/sim/cpp/.gitignore
@@ -1,1 +1,1 @@
-build/
\ No newline at end of file
+build/
diff --git a/sim/cpp/CMakeLists.txt b/sim/cpp/CMakeLists.txt
index 0d7c1093..f85e949e 100644
--- a/sim/cpp/CMakeLists.txt
+++ b/sim/cpp/CMakeLists.txt
@@ -70,4 +70,4 @@ foreach(PROGRAM ${DEMOS})
     target_link_libraries(${PROGRAM} ${CHRONO_LIBRARIES})
 endforeach()
 
-add_DLL_copy_command() 
+add_DLL_copy_command()
diff --git a/sim/python/Flat_Sim.py b/sim/python/Flat_Sim.py
index 3554480c..18d14b3c 100644
--- a/sim/python/Flat_Sim.py
+++ b/sim/python/Flat_Sim.py
@@ -39,7 +39,8 @@
 import pychrono.irrlicht as irr
 import pychrono.sensor as sens
 
-#// =============================================================================
+# // =============================================================================
+
 
 class ChSystem_DataGeneratorFunctor(veh.ChExternalDriver_DataGeneratorFunctor):
     def __init__(self, id: str, system: chrono.ChSystem):
@@ -50,6 +51,7 @@ def __init__(self, id: str, system: chrono.ChSystem):
     def Serialize(self, writer):
         writer.Key("time") << self.system.GetChTime()
 
+
 class ChCameraSensor_DataGeneratorFunctor(veh.ChExternalDriver_DataGeneratorFunctor):
     def __init__(self, id: str, cam: sens.ChCameraSensor):
         super().__init__("ChCameraSensor", id)
@@ -60,18 +62,23 @@ def Serialize(self, writer):
         rgba8_buffer = self.cam.GetMostRecentRGBA8Buffer()
         if rgba8_buffer.HasData():
             rgba8_data = rgba8_buffer.GetRGBA8Data()
-            rgba8_data = np.ascontiguousarray(np.flipud(rgba8_data)) # Needs to be contigious
+            rgba8_data = np.ascontiguousarray(
+                np.flipud(rgba8_data)
+            )  # Needs to be contigious
             shape = rgba8_data.shape
             writer.Key("width") << shape[1]
-            writer.Key("height") << shape[0] 
+            writer.Key("height") << shape[0]
             writer.Key("size") << shape[2]
             writer.Key("encoding") << "rgba8"
-            writer.Key("image").PointerAsString(rgba8_data.ctypes.data, int(np.prod(shape)))
+            writer.Key("image").PointerAsString(
+                rgba8_data.ctypes.data, int(np.prod(shape))
+            )
 
     def HasData(self) -> bool:
         rgba8_buffer = self.cam.GetMostRecentRGBA8Buffer()
         return rgba8_buffer.HasData()
 
+
 class ChVehicle_DataGeneratorFunctor(veh.ChExternalDriver_DataGeneratorFunctor):
     def __init__(self, id: str, vehicle: veh.ChVehicle):
         super().__init__("ChVehicle", id)
@@ -88,7 +95,10 @@ def Serialize(self, writer):
         writer.Key("lin_acc") << body.GetPos_dtdt()
         writer.Key("ang_acc") << body.GetWacc_loc()
 
-class ChAccelerometerSensor_DataGeneratorFunctor(veh.ChExternalDriver_DataGeneratorFunctor):
+
+class ChAccelerometerSensor_DataGeneratorFunctor(
+    veh.ChExternalDriver_DataGeneratorFunctor
+):
     def __init__(self, id: str, acc: sens.ChAccelerometerSensor):
         super().__init__("ChAccelerometerSensor", id)
 
@@ -106,6 +116,7 @@ def HasData(self) -> bool:
         buffer = self.acc.GetMostRecentAccelBuffer()
         return buffer.HasData()
 
+
 class ChGyroscopeSensor_DataGeneratorFunctor(veh.ChExternalDriver_DataGeneratorFunctor):
     def __init__(self, id: str, gyro: sens.ChGyroscopeSensor):
         super().__init__("ChGyroscopeSensor", id)
@@ -124,7 +135,10 @@ def HasData(self) -> bool:
         buffer = self.gyro.GetMostRecentGyroBuffer()
         return buffer.HasData()
 
-class ChMagnetometerSensor_DataGeneratorFunctor(veh.ChExternalDriver_DataGeneratorFunctor):
+
+class ChMagnetometerSensor_DataGeneratorFunctor(
+    veh.ChExternalDriver_DataGeneratorFunctor
+):
     def __init__(self, id: str, mag: sens.ChMagnetometerSensor):
         super().__init__("ChMagnetometerSensor", id)
 
@@ -142,6 +156,7 @@ def HasData(self) -> bool:
         buffer = self.mag.GetMostRecentMagnetBuffer()
         return buffer.HasData()
 
+
 class ChGPSSensor_DataGeneratorFunctor(veh.ChExternalDriver_DataGeneratorFunctor):
     def __init__(self, id: str, gps: sens.ChGPSSensor):
         super().__init__("ChGPSSensor", id)
@@ -160,14 +175,13 @@ def HasData(self) -> bool:
         buffer = self.gps.GetMostRecentGPSBuffer()
         return buffer.HasData()
 
+
 class ChDriverInputs_DataParserFunctor(veh.ChExternalDriver_DataParserFunctor):
     def __init__(self, driver: veh.ChDriver):
         super().__init__("ChDriverInputs")
 
         self.driver = driver
 
-
-
     def Deserialize(self, reader):
         steering = reader.GetFloat()
         throttle = reader.GetFloat()
@@ -178,19 +192,19 @@ def Deserialize(self, reader):
         self.driver.SetBraking(braking)
 
 
-#// =============================================================================
+# // =============================================================================
+
 
 def main():
-    
     def AddRandomCones(count, filename, class_id=1):
         mmesh = chrono.ChTriangleMeshConnected()
-        mmesh.SetLoadWavefrontMesh(filename, False, True)  
-        mmesh.SetTransform(chrono.ChVectorD(0, 0, 0), chrono.ChMatrix33(1)) 
+        mmesh.SetLoadWavefrontMesh(filename, False, True)
+        mmesh.SetTransform(chrono.ChVectorD(0, 0, 0), chrono.ChMatrix33(1))
         for i in range(count):
             # Generate a random position
-            x = (chrono.ChRandom() - .5) * cone_spread_x + cone_offset_x
-            y = (chrono.ChRandom() - .5) * cone_spread_y + cone_offset_y
-            z = terrain.GetHeight(chrono.ChVectorD(x, y, 1000)) # get the terrain z
+            x = (chrono.ChRandom() - 0.5) * cone_spread_x + cone_offset_x
+            y = (chrono.ChRandom() - 0.5) * cone_spread_y + cone_offset_y
+            z = terrain.GetHeight(chrono.ChVectorD(x, y, 1000))  # get the terrain z
             pos = chrono.ChVectorD(x, y, z)
 
             trimesh_shape = chrono.ChTriangleMeshShape()
@@ -206,7 +220,7 @@ def AddRandomCones(count, filename, class_id=1):
             mesh_body.SetBodyFixed(True)
             vehicle.GetSystem().Add(mesh_body)
 
-            if (class_id == 1):
+            if class_id == 1:
                 red_cone_assets.append(trimesh_shape)
                 cones.append(mesh_body)
             else:
@@ -215,11 +229,15 @@ def AddRandomCones(count, filename, class_id=1):
 
     def AddConesFromFile():
         green_cone_mesh = chrono.ChTriangleMeshConnected()
-        green_cone_mesh.LoadWavefrontMesh(chrono.GetChronoDataFile("sensor/cones/green_cone.obj"), False, True) 
+        green_cone_mesh.LoadWavefrontMesh(
+            chrono.GetChronoDataFile("sensor/cones/green_cone.obj"), False, True
+        )
         green_cone_mesh.Transform(chrono.ChVectorD(0, 0, 0), chrono.ChMatrix33D(1))
 
         red_cone_mesh = chrono.ChTriangleMeshConnected()
-        red_cone_mesh.LoadWavefrontMesh(chrono.GetChronoDataFile("sensor/cones/red_cone.obj"), False, True)
+        red_cone_mesh.LoadWavefrontMesh(
+            chrono.GetChronoDataFile("sensor/cones/red_cone.obj"), False, True
+        )
         red_cone_mesh.Transform(chrono.ChVectorD(0, 0, 0), chrono.ChMatrix33D(1))
 
         with open(chrono.GetChronoDataFile(cone_path_file)) as cone_file:
@@ -227,7 +245,7 @@ def AddConesFromFile():
                 line = cone_file.readline()
                 if not line:
                     break
-                id, color, x, y = map(float, line.split('\t')) 
+                id, color, x, y = map(float, line.split("\t"))
 
                 pos_x = x + cone_offset_x
                 pos_y = y + cone_offset_y
@@ -237,7 +255,7 @@ def AddConesFromFile():
                 rot = chrono.ChQuaternionD(1, 0, 0, 0)
 
                 cone_shape = chrono.ChTriangleMeshShape()
-                if (color == 0):
+                if color == 0:
                     cone_shape.SetMesh(red_cone_mesh)
                     red_cone_assets.append(cone_shape)
                 else:
@@ -254,23 +272,20 @@ def AddConesFromFile():
                 cones.append(cone_body)
                 vehicle.GetSystem().Add(cone_body)
 
-            
     def LabelConeAssets():
-            cone_id = 0
-            for cone in red_cone_assets:
-                cone_id += 1
-                for mat in cone.material_list:
-                    mat.SetClassID(1)
-                    mat.SetInstanceID(cone_id)
-
-
-            cone_id = 0
-            for cone in green_cone_assets:
-                cone_id += 1
-                for mat in cone.material_list:
-                    mat.SetClassID(2)
-                    mat.SetInstanceID(cone_id)
-
+        cone_id = 0
+        for cone in red_cone_assets:
+            cone_id += 1
+            for mat in cone.material_list:
+                mat.SetClassID(1)
+                mat.SetInstanceID(cone_id)
+
+        cone_id = 0
+        for cone in green_cone_assets:
+            cone_id += 1
+            for mat in cone.material_list:
+                mat.SetClassID(2)
+                mat.SetInstanceID(cone_id)
 
     # Create the RCCar vehicle, set parameters, and initialize
     vehicle = veh.RCCar()
@@ -282,7 +297,6 @@ def LabelConeAssets():
     vehicle.SetTireStepSize(tire_step_size)
     vehicle.Initialize()
 
-
     vehicle.SetChassisVisualizationType(chassis_vis_type)
     vehicle.SetSuspensionVisualizationType(suspension_vis_type)
     vehicle.SetSteeringVisualizationType(steering_vis_type)
@@ -294,58 +308,67 @@ def LabelConeAssets():
 
     patch_mat = chrono.ChMaterialSurfaceNSC()
     patch_mat.SetFriction(0.9)
-    patch_mat.SetRestitution(0.01) #-----check if mu, cr, and y is set similar to minfo
+    patch_mat.SetRestitution(
+        0.01
+    )  # -----check if mu, cr, and y is set similar to minfo
 
     # not right need to introduce point cloud -- not sure how to access this using the package_share_directory
-    patch = terrain.AddPatch(patch_mat,
-                             chrono.ChVectorD(
-                                 0, 0, 0), chrono.ChVectorD(0, 0, 1),
-                             terrainLength, terrainWidth)
-    patch.SetTexture(veh.GetDataFile("autonomy-toolkit/terrain/textures/tile4.jpg"), 200, 200)
-
+    patch = terrain.AddPatch(
+        patch_mat,
+        chrono.ChVectorD(0, 0, 0),
+        chrono.ChVectorD(0, 0, 1),
+        terrainLength,
+        terrainWidth,
+    )
+    patch.SetTexture(
+        veh.GetDataFile("autonomy-toolkit/terrain/textures/tile4.jpg"), 200, 200
+    )
 
     patch.SetColor(chrono.ChColor(0.8, 0.8, 0.5))
-    #patch = terrain.AddPatch(patch_mat, chrono.CSYSNORM, chrono.GetChronoDataFile("autonomy-toolkit/me3038/me3038_stereo_10cm_2.obj"))  #chrono.ChCoordsysD(chrono.ChVectorD(0, 0, 0), chrono.ChQuaternionD(1, 0, 0, 0))
+    # patch = terrain.AddPatch(patch_mat, chrono.CSYSNORM, chrono.GetChronoDataFile("autonomy-toolkit/me3038/me3038_stereo_10cm_2.obj"))  #chrono.ChCoordsysD(chrono.ChVectorD(0, 0, 0), chrono.ChQuaternionD(1, 0, 0, 0))
     # patch = terrain.AddPatch(patch_mat, chrono.CSYSNORM, chrono.GetChronoDataFile("autonomy-toolkit/me3038/rm3038_pt_cloud_2.obj"))
     # patch.SetTexture(veh.GetDataFile("terrain/textures/tile4.jpg"), 200, 200)
     # patch.SetColor(chrono.ChColor(0.8, 0.8, 0.5))
     terrain.Initialize()
 
     # add in ME3038 room mesh
-    #room_mmesh = chrono.ChTriangleMeshConnected()
-    #room_mmesh.LoadWavefrontMesh(chrono.GetChronoDataFile("autonomy-toolkit/me3038/rm3038_pt_cloud.obj"), False, True)
-    #room_mmesh.Transform(chrono.ChVectorD(0, 0, 0), chrono.ChMatrix33D(1))
-
-    #room_trimesh_shape = chrono.ChTriangleMeshShape()
-    #room_trimesh_shape.SetMesh(room_mmesh)
-    #room_trimesh_shape.SetName("ME3038")
-    #room_trimesh_shape.SetStatic(True)
-    #room_mesh_body = chrono.ChBody()
-    #room_mesh_body.SetPos(chrono.ChVectorD(0, 0, 0))
-    #room_mesh_body.AddAsset(room_trimesh_shape)
-    #room_mesh_body.SetBodyFixed(True)
+    # room_mmesh = chrono.ChTriangleMeshConnected()
+    # room_mmesh.LoadWavefrontMesh(chrono.GetChronoDataFile("autonomy-toolkit/me3038/rm3038_pt_cloud.obj"), False, True)
+    # room_mmesh.Transform(chrono.ChVectorD(0, 0, 0), chrono.ChMatrix33D(1))
 
-    #vehicle.GetSystem().Add(room_mesh_body)
+    # room_trimesh_shape = chrono.ChTriangleMeshShape()
+    # room_trimesh_shape.SetMesh(room_mmesh)
+    # room_trimesh_shape.SetName("ME3038")
+    # room_trimesh_shape.SetStatic(True)
+    # room_mesh_body = chrono.ChBody()
+    # room_mesh_body.SetPos(chrono.ChVectorD(0, 0, 0))
+    # room_mesh_body.AddAsset(room_trimesh_shape)
+    # room_mesh_body.SetBodyFixed(True)
 
+    # vehicle.GetSystem().Add(room_mesh_body)
 
     if cones_from_file:
         AddConesFromFile()
-    #else:
+    # else:
     #    AddRandomCones(num_cones / 2, chrono.GetChronoDataFile("sensor/cones/green_cone.obj"), 2)
     #    AddRandomCones(num_cones / 2, chrono.GetChronoDataFile("sensor/cones/red_cone.obj"), 1)
 
-
     # === create sensors ===
     manager = sens.ChSensorManager(vehicle.GetSystem())
-    manager.scene.AddPointLight(chrono.ChVectorF(0, 0, 100), chrono.ChVectorF(1, 1, 1), 5000)
-    
+    manager.scene.AddPointLight(
+        chrono.ChVectorF(0, 0, 100), chrono.ChVectorF(1, 1, 1), 5000
+    )
+
     b = sens.Background()
-    b.color_horizon = chrono.ChVectorF(.6, .7, .8)
-    b.color_zenith = chrono.ChVectorF(.4, .5, .6)
+    b.color_horizon = chrono.ChVectorF(0.6, 0.7, 0.8)
+    b.color_zenith = chrono.ChVectorF(0.4, 0.5, 0.6)
     b.mode = sens.BackgroundMode_GRADIENT
     manager.scene.SetBackground(b)
 
-    camera_pose = chrono.ChFrameD(chrono.ChVectorD(0.204, 0, 0.10018), chrono.Q_from_AngAxis(.1, chrono.ChVectorD(0, 1, 0)))
+    camera_pose = chrono.ChFrameD(
+        chrono.ChVectorD(0.204, 0, 0.10018),
+        chrono.Q_from_AngAxis(0.1, chrono.ChVectorD(0, 1, 0)),
+    )
     width = 1280
     height = 720
     frame_rate = 30.0
@@ -353,14 +376,15 @@ def LabelConeAssets():
     # fov = 1.59
 
     camera = sens.ChCameraSensor(
-                vehicle.GetChassisBody(), # body camera is attached to
-                frame_rate,                 # update rate in Hz
-                camera_pose,                # offset pose
-                width,                      # image width
-                height,                     # image height
-                fov,                        # FOV
-                2,
-                sens.RADIAL)  # super sample diameter
+        vehicle.GetChassisBody(),  # body camera is attached to
+        frame_rate,  # update rate in Hz
+        camera_pose,  # offset pose
+        width,  # image width
+        height,  # image height
+        fov,  # FOV
+        2,
+        sens.RADIAL,
+    )  # super sample diameter
     camera.SetRadialLensParameters(chrono.ChVectorF(-0.369, 0.1257, -0.0194))
     camera.SetName("Camera Sensor")
     c_window = float(0)
@@ -368,14 +392,14 @@ def LabelConeAssets():
     # camera.SetLag(.5)
 
     lidar = sens.ChLidarSensor(
-                vehicle.GetChassisBody(), # body camera is attached to
-                frame_rate,                 # update rate in Hz
-                camera_pose,                # offset pose
-                width,                      # image width
-                height,                     # image height
-                fov,                        # FOV
-                2,
-                sens.RADIAL
+        vehicle.GetChassisBody(),  # body camera is attached to
+        frame_rate,  # update rate in Hz
+        camera_pose,  # offset pose
+        width,  # image width
+        height,  # image height
+        fov,  # FOV
+        2,
+        sens.RADIAL,
     )
 
     if vis:
@@ -387,16 +411,18 @@ def LabelConeAssets():
     camera.PushFilter(sens.ChFilterRGBA8Access())
     manager.AddSensor(camera)
 
-
     camera2 = sens.ChCameraSensor(
-                patch.GetGroundBody(),  # body camera is attached to
-                30,                      # update rate in Hz
-                chrono.ChFrameD(chrono.ChVectorD(-3.09,-2.89,1.28), 
-                chrono.Q_from_Euler123(chrono.ChVectorD(.0,.45,chrono.CH_C_PI_4+.1))),  # offset pose   ------ not sure what CH_C_PI_2 is
-                1920,                                                           # image width
-                1080,                                                            # image height
-                1.2,                                                       # FOV
-                2)                                                              # super sample diameter
+        patch.GetGroundBody(),  # body camera is attached to
+        30,  # update rate in Hz
+        chrono.ChFrameD(
+            chrono.ChVectorD(-3.09, -2.89, 1.28),
+            chrono.Q_from_Euler123(chrono.ChVectorD(0.0, 0.45, chrono.CH_C_PI_4 + 0.1)),
+        ),  # offset pose   ------ not sure what CH_C_PI_2 is
+        1920,  # image width
+        1080,  # image height
+        1.2,  # FOV
+        2,
+    )  # super sample diameter
     camera2.SetName("Camera Sensor 2")
     if vis:
         camera2.PushFilter(sens.ChFilterVisualize(640, 360))
@@ -404,40 +430,53 @@ def LabelConeAssets():
         camera2.PushFilter(sens.ChFilterSave(sensor_data_dir + "cam2/"))
     manager.AddSensor(camera2)
 
-
     noise_model = sens.ChNoiseNone()
     gps_noise_model = sens.ChNoiseNone()
-    #gps_noise_model = sens.ChNoiseNormal(chrono.ChVectorD(0, 0, 0), chrono.ChVectorD(0.6, 0.6, 0.6))
-    imu_offset_pose = chrono.ChFrameD(chrono.ChVectorD(0, 0, 0), chrono.Q_from_AngAxis(0, chrono.ChVectorD(1, 0, 0)))
-    gps_offset_pose = chrono.ChFrameD(chrono.ChVectorD(0, 0, 0), chrono.Q_from_AngAxis(0, chrono.ChVectorD(1, 0, 0)))
+    # gps_noise_model = sens.ChNoiseNormal(chrono.ChVectorD(0, 0, 0), chrono.ChVectorD(0.6, 0.6, 0.6))
+    imu_offset_pose = chrono.ChFrameD(
+        chrono.ChVectorD(0, 0, 0), chrono.Q_from_AngAxis(0, chrono.ChVectorD(1, 0, 0))
+    )
+    gps_offset_pose = chrono.ChFrameD(
+        chrono.ChVectorD(0, 0, 0), chrono.Q_from_AngAxis(0, chrono.ChVectorD(1, 0, 0))
+    )
     gps_reference = chrono.ChVectorD(-89.400, 43.070, 260.0)
 
     # accelerometer
-    acc = sens.ChAccelerometerSensor(vehicle.GetChassisBody(), 100, imu_offset_pose, noise_model)
+    acc = sens.ChAccelerometerSensor(
+        vehicle.GetChassisBody(), 100, imu_offset_pose, noise_model
+    )
     acc.SetName("IMU - Accelerometer")
     acc.PushFilter(sens.ChFilterAccelAccess())
     manager.AddSensor(acc)
 
     # gyroscope
-    gyro = sens.ChGyroscopeSensor(vehicle.GetChassisBody(), 100, imu_offset_pose, noise_model)
+    gyro = sens.ChGyroscopeSensor(
+        vehicle.GetChassisBody(), 100, imu_offset_pose, noise_model
+    )
     gyro.SetName("IMU - Gyroscope")
     gyro.PushFilter(sens.ChFilterGyroAccess())
     manager.AddSensor(gyro)
 
     # magnetometer
-    mag = sens.ChMagnetometerSensor(vehicle.GetChassisBody(), 100, imu_offset_pose, noise_model, gps_reference)
+    mag = sens.ChMagnetometerSensor(
+        vehicle.GetChassisBody(), 100, imu_offset_pose, noise_model, gps_reference
+    )
     mag.SetName("IMU - Magnetometer")
     mag.PushFilter(sens.ChFilterMagnetAccess())
     manager.AddSensor(mag)
 
     # gps
-    gps = sens.ChGPSSensor(vehicle.GetChassisBody(), 10, gps_offset_pose, gps_reference, gps_noise_model)
+    gps = sens.ChGPSSensor(
+        vehicle.GetChassisBody(), 10, gps_offset_pose, gps_reference, gps_noise_model
+    )
     gps.SetName("GPS")
     gps.PushFilter(sens.ChFilterGPSAccess())
     manager.AddSensor(gps)
 
-    #Ground Truth
-    groundTruth = sens.ChGPSSensor(vehicle.GetChassisBody(), 10, gps_offset_pose, gps_reference, gps_noise_model)
+    # Ground Truth
+    groundTruth = sens.ChGPSSensor(
+        vehicle.GetChassisBody(), 10, gps_offset_pose, gps_reference, gps_noise_model
+    )
     groundTruth.SetName("groundTruth")
     groundTruth.PushFilter(sens.ChFilterGPSAccess())
     manager.AddSensor(groundTruth)
@@ -449,28 +488,42 @@ def LabelConeAssets():
 
     driver = veh.ChExternalDriver(vehicle.GetVehicle(), 50000)
 
-    system_generator = ChSystem_DataGeneratorFunctor("~/output/time", vehicle.GetSystem())
+    system_generator = ChSystem_DataGeneratorFunctor(
+        "~/output/time", vehicle.GetSystem()
+    )
     driver.AddDataGenerator(system_generator)
 
-    veh_generator = ChVehicle_DataGeneratorFunctor("~/output/vehicle", vehicle.GetVehicle())
+    veh_generator = ChVehicle_DataGeneratorFunctor(
+        "~/output/vehicle", vehicle.GetVehicle()
+    )
     driver.AddDataGenerator(veh_generator, 10)
 
-    cam_generator = ChCameraSensor_DataGeneratorFunctor("~/output/camera/front_facing_camera", camera)
+    cam_generator = ChCameraSensor_DataGeneratorFunctor(
+        "~/output/camera/front_facing_camera", camera
+    )
     driver.AddDataGenerator(cam_generator, frame_rate)
 
-    acc_generator = ChAccelerometerSensor_DataGeneratorFunctor("~/output/accelerometer/data", acc)
+    acc_generator = ChAccelerometerSensor_DataGeneratorFunctor(
+        "~/output/accelerometer/data", acc
+    )
     driver.AddDataGenerator(acc_generator, 100)
 
-    gyro_generator = ChGyroscopeSensor_DataGeneratorFunctor("~/output/gyroscope/data", gyro)
+    gyro_generator = ChGyroscopeSensor_DataGeneratorFunctor(
+        "~/output/gyroscope/data", gyro
+    )
     driver.AddDataGenerator(gyro_generator, 100)
 
-    mag_generator = ChMagnetometerSensor_DataGeneratorFunctor("~/output/magnetometer/data", mag)
+    mag_generator = ChMagnetometerSensor_DataGeneratorFunctor(
+        "~/output/magnetometer/data", mag
+    )
     driver.AddDataGenerator(mag_generator, 100)
 
     gps_generator = ChGPSSensor_DataGeneratorFunctor("~/output/gps/data", gps)
     driver.AddDataGenerator(gps_generator, 10)
 
-    groundTruth_generator = ChGPSSensor_DataGeneratorFunctor("~/output/groundTruth/data", groundTruth)
+    groundTruth_generator = ChGPSSensor_DataGeneratorFunctor(
+        "~/output/groundTruth/data", groundTruth
+    )
     driver.AddDataGenerator(groundTruth_generator, 10)
 
     inputs_parser = ChDriverInputs_DataParserFunctor(driver)
@@ -478,9 +531,9 @@ def LabelConeAssets():
 
     # Create the vehicle Irrlicht interface
     if USE_IRRLICHT:
-        app = veh.ChWheeledVehicleIrrApp(vehicle.GetVehicle(), 'RC Car')
+        app = veh.ChWheeledVehicleIrrApp(vehicle.GetVehicle(), "RC Car")
         app.AddTypicalLights()
-        app.AddLogo(chrono.GetChronoDataFile('logo_pychrono_alpha.png'))
+        app.AddLogo(chrono.GetChronoDataFile("logo_pychrono_alpha.png"))
         app.SetChaseCamera(trackPoint, 15.0, 0.5)
         app.SetTimestep(step_size)
         app.AssetBindAll()
@@ -495,14 +548,14 @@ def LabelConeAssets():
 
     throttle_time = 1.0
     steering_time = 1.0
-    braking_time = .5
+    braking_time = 0.5
     delta_throttle = 1.0 * step_size / throttle_time
     delta_braking = 1.0 * step_size / braking_time
     delta_steering = 2.0 * step_size / steering_time
 
     while True:
         time = vehicle.GetSystem().GetChTime()
-    
+
         # End simulation
         if (USE_IRRLICHT and not app.GetDevice().run()) or time >= t_end:
             break
@@ -516,12 +569,22 @@ def LabelConeAssets():
         # Get driver inputs
         driver_inputs = driver.GetInputs()
 
-        #clamp driver inputs
+        # clamp driver inputs
         driver_inputs.m_steering
 
-        pthrottle = np.clip(driver_inputs.m_throttle,pthrottle-delta_throttle,pthrottle+delta_throttle)
-        psteering = np.clip(driver_inputs.m_steering,psteering-delta_steering,psteering+delta_steering)
-        pbraking = np.clip(driver_inputs.m_braking,pbraking-delta_braking,pbraking+delta_braking)
+        pthrottle = np.clip(
+            driver_inputs.m_throttle,
+            pthrottle - delta_throttle,
+            pthrottle + delta_throttle,
+        )
+        psteering = np.clip(
+            driver_inputs.m_steering,
+            psteering - delta_steering,
+            psteering + delta_steering,
+        )
+        pbraking = np.clip(
+            driver_inputs.m_braking, pbraking - delta_braking, pbraking + delta_braking
+        )
 
         driver_inputs.m_throttle = pthrottle
         driver_inputs.m_steering = psteering
@@ -547,16 +610,16 @@ def LabelConeAssets():
 
         # Spin in place for real time to catch up
         realtime_timer.Spin(step_size)
-    
-    return 0       
-    
+
+    return 0
+
 
 # The path to the Chrono data directory containing various assets (meshes, textures, data files)
 # is automatically set, relative to the default location of this demo.
-# If running from a different directory, you must change the path to the data directory with: 
+# If running from a different directory, you must change the path to the data directory with:
 data_folder = os.path.join(os.path.dirname(os.path.abspath(__file__)), "..", "data", "")
 chrono.SetChronoDataPath(data_folder)
-veh.SetDataPath(chrono.GetChronoDataPath() + 'vehicle/')
+veh.SetDataPath(chrono.GetChronoDataPath() + "vehicle/")
 
 vis = True
 save_sensor_data = True
@@ -572,8 +635,12 @@ def LabelConeAssets():
 cone_spread_x = 1.0
 cone_spread_y = 1.0
 
-throttle_scaling = .5 * abs(1400.0 - 1500.0) / (1980.0 - 1500.0) # based on motor_driver.py limits for safety
-braking_scaling = abs(1600.0 - 1500.0) / (1980.0 - 1500.0)  # based on motor_driver.py limits for safety
+throttle_scaling = (
+    0.5 * abs(1400.0 - 1500.0) / (1980.0 - 1500.0)
+)  # based on motor_driver.py limits for safety
+braking_scaling = abs(1600.0 - 1500.0) / (
+    1980.0 - 1500.0
+)  # based on motor_driver.py limits for safety
 steering_scaling = 1.0  # abs(1725.0 - 1500.0) / (1980.0 - 1500.0);  // based on motor_driver.py limits for safety
 
 trackPoint = chrono.ChVectorD(0.0, 0.0, 1.75)
@@ -609,9 +676,9 @@ def LabelConeAssets():
 tire_model = veh.TireModelType_TMEASY
 
 # Rigid terrain -- not sure about this
-terrainHeight = -.2339      # terrain height (FLAT terrain only)
+terrainHeight = -0.2339  # terrain height (FLAT terrain only)
 terrainLength = 7.0  # size in X direction
-terrainWidth = 7.0   # size in Y direction
+terrainWidth = 7.0  # size in Y direction
 
 # Contact method
 contact_method = chrono.ChContactMethod_NSC
@@ -628,4 +695,3 @@ def LabelConeAssets():
 t_end = 1000
 
 main()
-
diff --git a/sim/python/demo_ARCLAB_IROS.py b/sim/python/demo_ARCLAB_IROS.py
index 17bb344f..3e6842ea 100644
--- a/sim/python/demo_ARCLAB_IROS.py
+++ b/sim/python/demo_ARCLAB_IROS.py
@@ -38,7 +38,8 @@
 import pychrono.irrlicht as irr
 import pychrono.sensor as sens
 
-#// =============================================================================
+# // =============================================================================
+
 
 class ChSystem_DataGeneratorFunctor(veh.ChExternalDriver_DataGeneratorFunctor):
     def __init__(self, id: str, system: chrono.ChSystem):
@@ -49,6 +50,7 @@ def __init__(self, id: str, system: chrono.ChSystem):
     def Serialize(self, writer):
         writer.Key("time") << self.system.GetChTime()
 
+
 class ChCameraSensor_DataGeneratorFunctor(veh.ChExternalDriver_DataGeneratorFunctor):
     def __init__(self, id: str, cam: sens.ChCameraSensor):
         super().__init__("ChCameraSensor", id)
@@ -59,18 +61,23 @@ def Serialize(self, writer):
         rgba8_buffer = self.cam.GetMostRecentRGBA8Buffer()
         if rgba8_buffer.HasData():
             rgba8_data = rgba8_buffer.GetRGBA8Data()
-            rgba8_data = np.ascontiguousarray(np.flipud(rgba8_data)) # Needs to be contigious
+            rgba8_data = np.ascontiguousarray(
+                np.flipud(rgba8_data)
+            )  # Needs to be contigious
             shape = rgba8_data.shape
             writer.Key("width") << shape[1]
-            writer.Key("height") << shape[0] 
+            writer.Key("height") << shape[0]
             writer.Key("size") << shape[2]
             writer.Key("encoding") << "rgba8"
-            writer.Key("image").PointerAsString(rgba8_data.ctypes.data, int(np.prod(shape)))
+            writer.Key("image").PointerAsString(
+                rgba8_data.ctypes.data, int(np.prod(shape))
+            )
 
     def HasData(self) -> bool:
         rgba8_buffer = self.cam.GetMostRecentRGBA8Buffer()
         return rgba8_buffer.HasData()
 
+
 class ChVehicle_DataGeneratorFunctor(veh.ChExternalDriver_DataGeneratorFunctor):
     def __init__(self, id: str, vehicle: veh.ChVehicle):
         super().__init__("ChVehicle", id)
@@ -87,7 +94,10 @@ def Serialize(self, writer):
         writer.Key("lin_acc") << body.GetPos_dtdt()
         writer.Key("ang_acc") << body.GetWacc_loc()
 
-class ChAccelerometerSensor_DataGeneratorFunctor(veh.ChExternalDriver_DataGeneratorFunctor):
+
+class ChAccelerometerSensor_DataGeneratorFunctor(
+    veh.ChExternalDriver_DataGeneratorFunctor
+):
     def __init__(self, id: str, acc: sens.ChAccelerometerSensor):
         super().__init__("ChAccelerometerSensor", id)
 
@@ -105,6 +115,7 @@ def HasData(self) -> bool:
         buffer = self.acc.GetMostRecentAccelBuffer()
         return buffer.HasData()
 
+
 class ChGyroscopeSensor_DataGeneratorFunctor(veh.ChExternalDriver_DataGeneratorFunctor):
     def __init__(self, id: str, gyro: sens.ChGyroscopeSensor):
         super().__init__("ChGyroscopeSensor", id)
@@ -123,7 +134,10 @@ def HasData(self) -> bool:
         buffer = self.gyro.GetMostRecentGyroBuffer()
         return buffer.HasData()
 
-class ChMagnetometerSensor_DataGeneratorFunctor(veh.ChExternalDriver_DataGeneratorFunctor):
+
+class ChMagnetometerSensor_DataGeneratorFunctor(
+    veh.ChExternalDriver_DataGeneratorFunctor
+):
     def __init__(self, id: str, mag: sens.ChMagnetometerSensor):
         super().__init__("ChMagnetometerSensor", id)
 
@@ -141,6 +155,7 @@ def HasData(self) -> bool:
         buffer = self.mag.GetMostRecentMagnetBuffer()
         return buffer.HasData()
 
+
 class ChGPSSensor_DataGeneratorFunctor(veh.ChExternalDriver_DataGeneratorFunctor):
     def __init__(self, id: str, gps: sens.ChGPSSensor):
         super().__init__("ChGPSSensor", id)
@@ -159,14 +174,13 @@ def HasData(self) -> bool:
         buffer = self.gps.GetMostRecentGPSBuffer()
         return buffer.HasData()
 
+
 class ChDriverInputs_DataParserFunctor(veh.ChExternalDriver_DataParserFunctor):
     def __init__(self, driver: veh.ChDriver):
         super().__init__("ChDriverInputs")
 
         self.driver = driver
 
-
-
     def Deserialize(self, reader):
         steering = reader.GetFloat()
         throttle = reader.GetFloat()
@@ -177,19 +191,19 @@ def Deserialize(self, reader):
         self.driver.SetBraking(braking)
 
 
-#// =============================================================================
+# // =============================================================================
+
 
 def main():
-    
     def AddRandomCones(count, filename, class_id=1):
         mmesh = chrono.ChTriangleMeshConnected()
-        mmesh.SetLoadWavefrontMesh(filename, False, True)  
-        mmesh.SetTransform(chrono.ChVectorD(0, 0, 0), chrono.ChMatrix33(1)) 
+        mmesh.SetLoadWavefrontMesh(filename, False, True)
+        mmesh.SetTransform(chrono.ChVectorD(0, 0, 0), chrono.ChMatrix33(1))
         for i in range(count):
             # Generate a random position
-            x = (chrono.ChRandom() - .5) * cone_spread_x + cone_offset_x
-            y = (chrono.ChRandom() - .5) * cone_spread_y + cone_offset_y
-            z = terrain.GetHeight(chrono.ChVectorD(x, y, 1000)) # get the terrain z
+            x = (chrono.ChRandom() - 0.5) * cone_spread_x + cone_offset_x
+            y = (chrono.ChRandom() - 0.5) * cone_spread_y + cone_offset_y
+            z = terrain.GetHeight(chrono.ChVectorD(x, y, 1000))  # get the terrain z
             pos = chrono.ChVectorD(x, y, z)
 
             trimesh_shape = chrono.ChTriangleMeshShape()
@@ -207,7 +221,7 @@ def AddRandomCones(count, filename, class_id=1):
             mesh_body.SetBodyFixed(True)
             vehicle.GetSystem().Add(mesh_body)
 
-            if (class_id == 1):
+            if class_id == 1:
                 red_cone_assets.append(trimesh_shape)
                 cones.append(mesh_body)
             else:
@@ -216,11 +230,15 @@ def AddRandomCones(count, filename, class_id=1):
 
     def AddConesFromFile():
         green_cone_mesh = chrono.ChTriangleMeshConnected()
-        green_cone_mesh.LoadWavefrontMesh(chrono.GetChronoDataFile("sensor/cones/green_cone.obj"), False, True) 
+        green_cone_mesh.LoadWavefrontMesh(
+            chrono.GetChronoDataFile("sensor/cones/green_cone.obj"), False, True
+        )
         green_cone_mesh.Transform(chrono.ChVectorD(0, 0, 0), chrono.ChMatrix33D(1))
 
         red_cone_mesh = chrono.ChTriangleMeshConnected()
-        red_cone_mesh.LoadWavefrontMesh(chrono.GetChronoDataFile("sensor/cones/red_cone.obj"), False, True)
+        red_cone_mesh.LoadWavefrontMesh(
+            chrono.GetChronoDataFile("sensor/cones/red_cone.obj"), False, True
+        )
         red_cone_mesh.Transform(chrono.ChVectorD(0, 0, 0), chrono.ChMatrix33D(1))
 
         with open(chrono.GetChronoDataFile(cone_path_file)) as cone_file:
@@ -228,7 +246,7 @@ def AddConesFromFile():
                 line = cone_file.readline()
                 if not line:
                     break
-                id, color, x, y = map(float, line.split('\t')) 
+                id, color, x, y = map(float, line.split("\t"))
 
                 pos_x = x + cone_offset_x
                 pos_y = y + cone_offset_y
@@ -238,7 +256,7 @@ def AddConesFromFile():
                 rot = chrono.ChQuaternionD(1, 0, 0, 0)
 
                 cone_shape = chrono.ChTriangleMeshShape()
-                if (color == 0):
+                if color == 0:
                     cone_shape.SetMesh(red_cone_mesh)
                     red_cone_assets.append(cone_shape)
                 else:
@@ -255,23 +273,20 @@ def AddConesFromFile():
                 cones.append(cone_body)
                 vehicle.GetSystem().Add(cone_body)
 
-            
     def LabelConeAssets():
-            cone_id = 0
-            for cone in red_cone_assets:
-                cone_id += 1
-                for mat in cone.GetMaterials():
-                    mat.SetClassID(1)
-                    mat.SetInstanceID(cone_id)
-
-
-            cone_id = 0
-            for cone in green_cone_assets:
-                cone_id += 1
-                for mat in cone.GetMaterials():
-                    mat.SetClassID(2)
-                    mat.SetInstanceID(cone_id)
-
+        cone_id = 0
+        for cone in red_cone_assets:
+            cone_id += 1
+            for mat in cone.GetMaterials():
+                mat.SetClassID(1)
+                mat.SetInstanceID(cone_id)
+
+        cone_id = 0
+        for cone in green_cone_assets:
+            cone_id += 1
+            for mat in cone.GetMaterials():
+                mat.SetClassID(2)
+                mat.SetInstanceID(cone_id)
 
     # Create the RCCar vehicle, set parameters, and initialize
     vehicle = veh.RCCar()
@@ -283,7 +298,6 @@ def LabelConeAssets():
     vehicle.SetTireStepSize(tire_step_size)
     vehicle.Initialize()
 
-
     vehicle.SetChassisVisualizationType(chassis_vis_type)
     vehicle.SetSuspensionVisualizationType(suspension_vis_type)
     vehicle.SetSteeringVisualizationType(steering_vis_type)
@@ -295,10 +309,16 @@ def LabelConeAssets():
 
     patch_mat = chrono.ChMaterialSurfaceNSC()
     patch_mat.SetFriction(0.9)
-    patch_mat.SetRestitution(0.01) #-----check if mu, cr, and y is set similar to minfo
+    patch_mat.SetRestitution(
+        0.01
+    )  # -----check if mu, cr, and y is set similar to minfo
 
     # not right need to introduce point cloud -- not sure how to access this using the package_share_directory
-    patch = terrain.AddPatch(patch_mat, chrono.CSYSNORM, chrono.GetChronoDataFile("autonomy-toolkit/me3038/me3038_stereo_10cm_2.obj"))  #chrono.ChCoordsysD(chrono.ChVectorD(0, 0, 0), chrono.ChQuaternionD(1, 0, 0, 0))
+    patch = terrain.AddPatch(
+        patch_mat,
+        chrono.CSYSNORM,
+        chrono.GetChronoDataFile("autonomy-toolkit/me3038/me3038_stereo_10cm_2.obj"),
+    )  # chrono.ChCoordsysD(chrono.ChVectorD(0, 0, 0), chrono.ChQuaternionD(1, 0, 0, 0))
     # patch = terrain.AddPatch(patch_mat, chrono.CSYSNORM, chrono.GetChronoDataFile("autonomy-toolkit/me3038/rm3038_pt_cloud_2.obj"))
     # patch.SetTexture(veh.GetDataFile("terrain/textures/tile4.jpg"), 200, 200)
     # patch.SetColor(chrono.ChColor(0.8, 0.8, 0.5))
@@ -306,7 +326,11 @@ def LabelConeAssets():
 
     # add in ME3038 room mesh
     room_mmesh = chrono.ChTriangleMeshConnected()
-    room_mmesh.LoadWavefrontMesh(chrono.GetChronoDataFile("autonomy-toolkit/me3038/rm3038_pt_cloud.obj"), False, True)
+    room_mmesh.LoadWavefrontMesh(
+        chrono.GetChronoDataFile("autonomy-toolkit/me3038/rm3038_pt_cloud.obj"),
+        False,
+        True,
+    )
     room_mmesh.Transform(chrono.ChVectorD(0, 0, 0), chrono.ChMatrix33D(1))
 
     room_trimesh_shape = chrono.ChTriangleMeshShape()
@@ -322,25 +346,32 @@ def LabelConeAssets():
 
     vehicle.GetSystem().Add(room_mesh_body)
 
-
     if cones_from_file:
         AddConesFromFile()
     else:
-        AddRandomCones(num_cones / 2, chrono.GetChronoDataFile("sensor/cones/green_cone.obj"), 2)
-        AddRandomCones(num_cones / 2, chrono.GetChronoDataFile("sensor/cones/red_cone.obj"), 1)
-
+        AddRandomCones(
+            num_cones / 2, chrono.GetChronoDataFile("sensor/cones/green_cone.obj"), 2
+        )
+        AddRandomCones(
+            num_cones / 2, chrono.GetChronoDataFile("sensor/cones/red_cone.obj"), 1
+        )
 
     # === create sensors ===
     manager = sens.ChSensorManager(vehicle.GetSystem())
-    manager.scene.AddPointLight(chrono.ChVectorF(0, 0, 100), chrono.ChColor(1, 1, 1), 5000)
-    
+    manager.scene.AddPointLight(
+        chrono.ChVectorF(0, 0, 100), chrono.ChColor(1, 1, 1), 5000
+    )
+
     b = sens.Background()
-    b.color_horizon = chrono.ChVectorF(.6, .7, .8)
-    b.color_zenith = chrono.ChVectorF(.4, .5, .6)
+    b.color_horizon = chrono.ChVectorF(0.6, 0.7, 0.8)
+    b.color_zenith = chrono.ChVectorF(0.4, 0.5, 0.6)
     b.mode = sens.BackgroundMode_GRADIENT
     manager.scene.SetBackground(b)
 
-    camera_pose = chrono.ChFrameD(chrono.ChVectorD(0.204, 0, 0.10018), chrono.Q_from_AngAxis(.1, chrono.ChVectorD(0, 1, 0)))
+    camera_pose = chrono.ChFrameD(
+        chrono.ChVectorD(0.204, 0, 0.10018),
+        chrono.Q_from_AngAxis(0.1, chrono.ChVectorD(0, 1, 0)),
+    )
     width = 1280
     height = 720
     frame_rate = 30.0
@@ -348,14 +379,15 @@ def LabelConeAssets():
     # fov = 1.59
 
     camera = sens.ChCameraSensor(
-                vehicle.GetChassisBody(), # body camera is attached to
-                frame_rate,                 # update rate in Hz
-                camera_pose,                # offset pose
-                width,                      # image width
-                height,                     # image height
-                fov,                        # FOV
-                2,
-                sens.RADIAL)  # super sample diameter
+        vehicle.GetChassisBody(),  # body camera is attached to
+        frame_rate,  # update rate in Hz
+        camera_pose,  # offset pose
+        width,  # image width
+        height,  # image height
+        fov,  # FOV
+        2,
+        sens.RADIAL,
+    )  # super sample diameter
     camera.SetRadialLensParameters(chrono.ChVectorF(-0.369, 0.1257, -0.0194))
     camera.SetName("Camera Sensor")
     c_window = float(0)
@@ -371,16 +403,18 @@ def LabelConeAssets():
     camera.PushFilter(sens.ChFilterRGBA8Access())
     manager.AddSensor(camera)
 
-
     camera2 = sens.ChCameraSensor(
-                patch.GetGroundBody(),  # body camera is attached to
-                30,                      # update rate in Hz
-                chrono.ChFrameD(chrono.ChVectorD(-3.09,-2.89,1.28), 
-                chrono.Q_from_Euler123(chrono.ChVectorD(.0,.45,chrono.CH_C_PI_4+.1))),  # offset pose   ------ not sure what CH_C_PI_2 is
-                1920,                                                           # image width
-                1080,                                                            # image height
-                1.2,                                                       # FOV
-                2)                                                              # super sample diameter
+        patch.GetGroundBody(),  # body camera is attached to
+        30,  # update rate in Hz
+        chrono.ChFrameD(
+            chrono.ChVectorD(-3.09, -2.89, 1.28),
+            chrono.Q_from_Euler123(chrono.ChVectorD(0.0, 0.45, chrono.CH_C_PI_4 + 0.1)),
+        ),  # offset pose   ------ not sure what CH_C_PI_2 is
+        1920,  # image width
+        1080,  # image height
+        1.2,  # FOV
+        2,
+    )  # super sample diameter
     camera2.SetName("Camera Sensor 2")
     if vis:
         camera2.PushFilter(sens.ChFilterVisualize(640, 360))
@@ -388,34 +422,47 @@ def LabelConeAssets():
         camera2.PushFilter(sens.ChFilterSave(sensor_data_dir + "cam2/"))
     manager.AddSensor(camera2)
 
-
     noise_model = sens.ChNoiseNone()
-    #gps_noise_model = sens.ChNoiseNone()
-    gps_noise_model = sens.ChNoiseNormal(chrono.ChVectorD(0, 0, 0), chrono.ChVectorD(0.6, 0.6, 0.6))
-    imu_offset_pose = chrono.ChFrameD(chrono.ChVectorD(0, 0, 0), chrono.Q_from_AngAxis(0, chrono.ChVectorD(1, 0, 0)))
-    gps_offset_pose = chrono.ChFrameD(chrono.ChVectorD(0, 0, 0), chrono.Q_from_AngAxis(0, chrono.ChVectorD(1, 0, 0)))
+    # gps_noise_model = sens.ChNoiseNone()
+    gps_noise_model = sens.ChNoiseNormal(
+        chrono.ChVectorD(0, 0, 0), chrono.ChVectorD(0.6, 0.6, 0.6)
+    )
+    imu_offset_pose = chrono.ChFrameD(
+        chrono.ChVectorD(0, 0, 0), chrono.Q_from_AngAxis(0, chrono.ChVectorD(1, 0, 0))
+    )
+    gps_offset_pose = chrono.ChFrameD(
+        chrono.ChVectorD(0, 0, 0), chrono.Q_from_AngAxis(0, chrono.ChVectorD(1, 0, 0))
+    )
     gps_reference = chrono.ChVectorD(-89.400, 43.070, 260.0)
 
     # accelerometer
-    acc = sens.ChAccelerometerSensor(vehicle.GetChassisBody(), 100, imu_offset_pose, noise_model)
+    acc = sens.ChAccelerometerSensor(
+        vehicle.GetChassisBody(), 100, imu_offset_pose, noise_model
+    )
     acc.SetName("IMU - Accelerometer")
     acc.PushFilter(sens.ChFilterAccelAccess())
     manager.AddSensor(acc)
 
     # gyroscope
-    gyro = sens.ChGyroscopeSensor(vehicle.GetChassisBody(), 100, imu_offset_pose, noise_model)
+    gyro = sens.ChGyroscopeSensor(
+        vehicle.GetChassisBody(), 100, imu_offset_pose, noise_model
+    )
     gyro.SetName("IMU - Gyroscope")
     gyro.PushFilter(sens.ChFilterGyroAccess())
     manager.AddSensor(gyro)
 
     # magnetometer
-    mag = sens.ChMagnetometerSensor(vehicle.GetChassisBody(), 100, imu_offset_pose, noise_model, gps_reference)
+    mag = sens.ChMagnetometerSensor(
+        vehicle.GetChassisBody(), 100, imu_offset_pose, noise_model, gps_reference
+    )
     mag.SetName("IMU - Magnetometer")
     mag.PushFilter(sens.ChFilterMagnetAccess())
     manager.AddSensor(mag)
 
     # gps
-    gps = sens.ChGPSSensor(vehicle.GetChassisBody(), 10, gps_offset_pose, gps_reference, gps_noise_model)
+    gps = sens.ChGPSSensor(
+        vehicle.GetChassisBody(), 10, gps_offset_pose, gps_reference, gps_noise_model
+    )
     gps.SetName("GPS")
     gps.PushFilter(sens.ChFilterGPSAccess())
     manager.AddSensor(gps)
@@ -427,22 +474,34 @@ def LabelConeAssets():
 
     driver = veh.ChExternalDriver(vehicle.GetVehicle(), 50000)
 
-    system_generator = ChSystem_DataGeneratorFunctor("~/output/time", vehicle.GetSystem())
+    system_generator = ChSystem_DataGeneratorFunctor(
+        "~/output/time", vehicle.GetSystem()
+    )
     driver.AddDataGenerator(system_generator)
 
-    veh_generator = ChVehicle_DataGeneratorFunctor("~/output/vehicle", vehicle.GetVehicle())
+    veh_generator = ChVehicle_DataGeneratorFunctor(
+        "~/output/vehicle", vehicle.GetVehicle()
+    )
     driver.AddDataGenerator(veh_generator, 10)
 
-    cam_generator = ChCameraSensor_DataGeneratorFunctor("~/output/camera/front_facing_camera", camera)
+    cam_generator = ChCameraSensor_DataGeneratorFunctor(
+        "~/output/camera/front_facing_camera", camera
+    )
     driver.AddDataGenerator(cam_generator, frame_rate)
 
-    acc_generator = ChAccelerometerSensor_DataGeneratorFunctor("~/output/accelerometer/data", acc)
+    acc_generator = ChAccelerometerSensor_DataGeneratorFunctor(
+        "~/output/accelerometer/data", acc
+    )
     driver.AddDataGenerator(acc_generator, 100)
 
-    gyro_generator = ChGyroscopeSensor_DataGeneratorFunctor("~/output/gyroscope/data", gyro)
+    gyro_generator = ChGyroscopeSensor_DataGeneratorFunctor(
+        "~/output/gyroscope/data", gyro
+    )
     driver.AddDataGenerator(gyro_generator, 100)
 
-    mag_generator = ChMagnetometerSensor_DataGeneratorFunctor("~/output/magnetometer/data", mag)
+    mag_generator = ChMagnetometerSensor_DataGeneratorFunctor(
+        "~/output/magnetometer/data", mag
+    )
     driver.AddDataGenerator(mag_generator, 100)
 
     gps_generator = ChGPSSensor_DataGeneratorFunctor("~/output/gps/data", gps)
@@ -453,9 +512,9 @@ def LabelConeAssets():
 
     # Create the vehicle Irrlicht interface
     if USE_IRRLICHT:
-        app = veh.ChWheeledVehicleIrrApp(vehicle.GetVehicle(), 'RC Car')
+        app = veh.ChWheeledVehicleIrrApp(vehicle.GetVehicle(), "RC Car")
         app.AddTypicalLights()
-        app.AddLogo(chrono.GetChronoDataFile('logo_pychrono_alpha.png'))
+        app.AddLogo(chrono.GetChronoDataFile("logo_pychrono_alpha.png"))
         app.SetChaseCamera(trackPoint, 15.0, 0.5)
         app.SetTimestep(step_size)
         app.AssetBindAll()
@@ -470,14 +529,14 @@ def LabelConeAssets():
 
     throttle_time = 1.0
     steering_time = 1.0
-    braking_time = .5
+    braking_time = 0.5
     delta_throttle = 1.0 * step_size / throttle_time
     delta_braking = 1.0 * step_size / braking_time
     delta_steering = 2.0 * step_size / steering_time
 
     while True:
         time = vehicle.GetSystem().GetChTime()
-    
+
         # End simulation
         if (USE_IRRLICHT and not app.GetDevice().run()) or time >= t_end:
             break
@@ -491,12 +550,22 @@ def LabelConeAssets():
         # Get driver inputs
         driver_inputs = driver.GetInputs()
 
-        #clamp driver inputs
+        # clamp driver inputs
         driver_inputs.m_steering
 
-        pthrottle = np.clip(driver_inputs.m_throttle,pthrottle-delta_throttle,pthrottle+delta_throttle)
-        psteering = np.clip(driver_inputs.m_steering,psteering-delta_steering,psteering+delta_steering)
-        pbraking = np.clip(driver_inputs.m_braking,pbraking-delta_braking,pbraking+delta_braking)
+        pthrottle = np.clip(
+            driver_inputs.m_throttle,
+            pthrottle - delta_throttle,
+            pthrottle + delta_throttle,
+        )
+        psteering = np.clip(
+            driver_inputs.m_steering,
+            psteering - delta_steering,
+            psteering + delta_steering,
+        )
+        pbraking = np.clip(
+            driver_inputs.m_braking, pbraking - delta_braking, pbraking + delta_braking
+        )
 
         driver_inputs.m_throttle = pthrottle
         driver_inputs.m_steering = psteering
@@ -522,16 +591,16 @@ def LabelConeAssets():
 
         # Spin in place for real time to catch up
         realtime_timer.Spin(step_size)
-    
-    return 0       
-    
+
+    return 0
+
 
 # The path to the Chrono data directory containing various assets (meshes, textures, data files)
 # is automatically set, relative to the default location of this demo.
-# If running from a different directory, you must change the path to the data directory with: 
+# If running from a different directory, you must change the path to the data directory with:
 data_folder = os.path.join(os.path.dirname(os.path.abspath(__file__)), "..", "data", "")
 chrono.SetChronoDataPath(data_folder)
-veh.SetDataPath(chrono.GetChronoDataPath() + 'vehicle/')
+veh.SetDataPath(chrono.GetChronoDataPath() + "vehicle/")
 
 vis = True
 save_sensor_data = False
@@ -547,8 +616,12 @@ def LabelConeAssets():
 cone_spread_x = 1.0
 cone_spread_y = 1.0
 
-throttle_scaling = .5 * abs(1400.0 - 1500.0) / (1980.0 - 1500.0) # based on motor_driver.py limits for safety
-braking_scaling = abs(1600.0 - 1500.0) / (1980.0 - 1500.0)  # based on motor_driver.py limits for safety
+throttle_scaling = (
+    0.5 * abs(1400.0 - 1500.0) / (1980.0 - 1500.0)
+)  # based on motor_driver.py limits for safety
+braking_scaling = abs(1600.0 - 1500.0) / (
+    1980.0 - 1500.0
+)  # based on motor_driver.py limits for safety
 steering_scaling = 1.0  # abs(1725.0 - 1500.0) / (1980.0 - 1500.0);  // based on motor_driver.py limits for safety
 
 trackPoint = chrono.ChVectorD(0.0, 0.0, 1.75)
@@ -584,9 +657,9 @@ def LabelConeAssets():
 tire_model = veh.TireModelType_TMEASY
 
 # Rigid terrain -- not sure about this
-terrainHeight = -.2339      # terrain height (FLAT terrain only)
+terrainHeight = -0.2339  # terrain height (FLAT terrain only)
 terrainLength = 7.0  # size in X direction
-terrainWidth = 7.0   # size in Y direction
+terrainWidth = 7.0  # size in Y direction
 
 # Contact method
 contact_method = chrono.ChContactMethod_NSC
@@ -603,4 +676,3 @@ def LabelConeAssets():
 t_end = 1000
 
 main()
-
diff --git a/sim/python/demo_ARCLAB_cone.py b/sim/python/demo_ARCLAB_cone.py
index 76254063..4e19e200 100644
--- a/sim/python/demo_ARCLAB_cone.py
+++ b/sim/python/demo_ARCLAB_cone.py
@@ -39,7 +39,8 @@
 import pychrono.irrlicht as irr
 import pychrono.sensor as sens
 
-#// =============================================================================
+# // =============================================================================
+
 
 class ChSystem_DataGeneratorFunctor(veh.ChExternalDriver_DataGeneratorFunctor):
     def __init__(self, id: str, system: chrono.ChSystem):
@@ -50,6 +51,7 @@ def __init__(self, id: str, system: chrono.ChSystem):
     def Serialize(self, writer):
         writer.Key("time") << self.system.GetChTime()
 
+
 class ChCameraSensor_DataGeneratorFunctor(veh.ChExternalDriver_DataGeneratorFunctor):
     def __init__(self, id: str, cam: sens.ChCameraSensor):
         super().__init__("ChCameraSensor", id)
@@ -60,18 +62,23 @@ def Serialize(self, writer):
         rgba8_buffer = self.cam.GetMostRecentRGBA8Buffer()
         if rgba8_buffer.HasData():
             rgba8_data = rgba8_buffer.GetRGBA8Data()
-            rgba8_data = np.ascontiguousarray(rgba8_data[::-1]) # Needs to be contigious
+            rgba8_data = np.ascontiguousarray(
+                rgba8_data[::-1]
+            )  # Needs to be contigious
             shape = rgba8_data.shape
             writer.Key("width") << shape[1]
-            writer.Key("height") << shape[0] 
+            writer.Key("height") << shape[0]
             writer.Key("size") << shape[2]
             writer.Key("encoding") << "rgba8"
-            writer.Key("image").PointerAsString(rgba8_data.ctypes.data, int(np.prod(shape)))
+            writer.Key("image").PointerAsString(
+                rgba8_data.ctypes.data, int(np.prod(shape))
+            )
 
     def HasData(self) -> bool:
         rgba8_buffer = self.cam.GetMostRecentRGBA8Buffer()
         return rgba8_buffer.HasData()
 
+
 class ChVehicle_DataGeneratorFunctor(veh.ChExternalDriver_DataGeneratorFunctor):
     def __init__(self, id: str, vehicle: veh.ChVehicle):
         super().__init__("ChVehicle", id)
@@ -88,7 +95,10 @@ def Serialize(self, writer):
         writer.Key("lin_acc") << body.GetPos_dtdt()
         writer.Key("ang_acc") << body.GetWacc_loc()
 
-class ChAccelerometerSensor_DataGeneratorFunctor(veh.ChExternalDriver_DataGeneratorFunctor):
+
+class ChAccelerometerSensor_DataGeneratorFunctor(
+    veh.ChExternalDriver_DataGeneratorFunctor
+):
     def __init__(self, id: str, acc: sens.ChAccelerometerSensor):
         super().__init__("ChAccelerometerSensor", id)
 
@@ -106,6 +116,7 @@ def HasData(self) -> bool:
         buffer = self.acc.GetMostRecentAccelBuffer()
         return buffer.HasData()
 
+
 class ChGyroscopeSensor_DataGeneratorFunctor(veh.ChExternalDriver_DataGeneratorFunctor):
     def __init__(self, id: str, gyro: sens.ChGyroscopeSensor):
         super().__init__("ChGyroscopeSensor", id)
@@ -124,7 +135,10 @@ def HasData(self) -> bool:
         buffer = self.gyro.GetMostRecentGyroBuffer()
         return buffer.HasData()
 
-class ChMagnetometerSensor_DataGeneratorFunctor(veh.ChExternalDriver_DataGeneratorFunctor):
+
+class ChMagnetometerSensor_DataGeneratorFunctor(
+    veh.ChExternalDriver_DataGeneratorFunctor
+):
     def __init__(self, id: str, mag: sens.ChMagnetometerSensor):
         super().__init__("ChMagnetometerSensor", id)
 
@@ -142,6 +156,7 @@ def HasData(self) -> bool:
         buffer = self.mag.GetMostRecentMagnetBuffer()
         return buffer.HasData()
 
+
 class ChGPSSensor_DataGeneratorFunctor(veh.ChExternalDriver_DataGeneratorFunctor):
     def __init__(self, id: str, gps: sens.ChGPSSensor):
         super().__init__("ChGPSSensor", id)
@@ -160,6 +175,7 @@ def HasData(self) -> bool:
         buffer = self.gps.GetMostRecentGPSBuffer()
         return buffer.HasData()
 
+
 class ChDriverInputs_DataParserFunctor(veh.ChExternalDriver_DataParserFunctor):
     def __init__(self, driver: veh.ChDriver):
         super().__init__("ChDriverInputs")
@@ -176,19 +192,19 @@ def Deserialize(self, reader):
         self.driver.SetBraking(braking)
 
 
-#// =============================================================================
+# // =============================================================================
+
 
 def main():
-    
     def AddRandomCones(count, filename, class_id=1):
         mmesh = chrono.ChTriangleMeshConnected()
-        mmesh.LoadWavefrontMesh(filename, False, True)  
-        mmesh.Transform(chrono.ChVectorD(0, 0, 0), chrono.ChMatrix33D(1)) 
+        mmesh.LoadWavefrontMesh(filename, False, True)
+        mmesh.Transform(chrono.ChVectorD(0, 0, 0), chrono.ChMatrix33D(1))
         for i in range(count):
             # Generate a random position
-            x = (chrono.ChRandom() - .5) * cone_spread_x + cone_offset_x
-            y = (chrono.ChRandom() - .5) * cone_spread_y + cone_offset_y
-            z = terrain.GetHeight(chrono.ChVectorD(x, y, 1000)) # get the terrain z
+            x = (chrono.ChRandom() - 0.5) * cone_spread_x + cone_offset_x
+            y = (chrono.ChRandom() - 0.5) * cone_spread_y + cone_offset_y
+            z = terrain.GetHeight(chrono.ChVectorD(x, y, 1000))  # get the terrain z
             pos = chrono.ChVectorD(x, y, z)
 
             trimesh_shape = chrono.ChTriangleMeshShape()
@@ -204,7 +220,7 @@ def AddRandomCones(count, filename, class_id=1):
             mesh_body.SetBodyFixed(True)
             vehicle.GetSystem().Add(mesh_body)
 
-            if (class_id == 1):
+            if class_id == 1:
                 red_cone_assets.append(trimesh_shape)
                 red_cones.append(mesh_body)
             else:
@@ -213,27 +229,37 @@ def AddRandomCones(count, filename, class_id=1):
 
     def AddConesFromFile():
         green_cone_mesh = chrono.ChTriangleMeshConnected()
-        green_cone_mesh.LoadWavefrontMesh(chrono.GetChronoDataFile("sensor/cones/green_cone.obj"), False, True) 
+        green_cone_mesh.LoadWavefrontMesh(
+            chrono.GetChronoDataFile("sensor/cones/green_cone.obj"), False, True
+        )
         green_cone_mesh.Transform(chrono.ChVectorD(0, 0, 0), chrono.ChMatrix33D(1))
 
         red_cone_mesh = chrono.ChTriangleMeshConnected()
-        red_cone_mesh.LoadWavefrontMesh(chrono.GetChronoDataFile("sensor/cones/red_cone.obj"), False, True)
+        red_cone_mesh.LoadWavefrontMesh(
+            chrono.GetChronoDataFile("sensor/cones/red_cone.obj"), False, True
+        )
         red_cone_mesh.Transform(chrono.ChVectorD(0, 0, 0), chrono.ChMatrix33D(1))
 
-        with open(chrono.GetChronoDataFile("autonomy-toolkit/paths/cone_paths_0.csv")) as cone_file:
+        with open(
+            chrono.GetChronoDataFile("autonomy-toolkit/paths/cone_paths_0.csv")
+        ) as cone_file:
             while True:
                 line = cone_file.readline()
                 if not line:
                     break
-                x_inner, y_inner, x_outer, y_outer = map(float, line.split(' ')) 
+                x_inner, y_inner, x_outer, y_outer = map(float, line.split(" "))
 
                 pos_green_x = x_inner + cone_offset_x
                 pos_green_y = y_inner + cone_offset_y
-                pos_green_z = terrain.GetHeight(chrono.ChVectorD(pos_green_x, pos_green_y, 1000))
+                pos_green_z = terrain.GetHeight(
+                    chrono.ChVectorD(pos_green_x, pos_green_y, 1000)
+                )
 
                 pos_red_x = x_outer + cone_offset_x
                 pos_red_y = y_outer + cone_offset_y
-                pos_red_z = terrain.GetHeight(chrono.ChVectorD(pos_red_x, pos_red_y, 1000))
+                pos_red_z = terrain.GetHeight(
+                    chrono.ChVectorD(pos_red_x, pos_red_y, 1000)
+                )
 
                 pos_green = chrono.ChVectorD(pos_green_x, pos_green_y, pos_green_z)
                 pos_red = chrono.ChVectorD(pos_red_x, pos_red_y, pos_red_z)
@@ -268,38 +294,35 @@ def AddConesFromFile():
                 red_body.SetBodyFixed(True)
                 red_cones.append(red_body)
                 vehicle.GetSystem().Add(red_body)
-            
-    def LabelConeAssets():
-            cone_id = 0
-            for cone in red_cone_assets:
-                cone_id += 1
-                for mat in cone.GetMaterials():
-                    mat.SetClassID(1)
-                    mat.SetInstanceID(cone_id)
-
 
-            cone_id = 0
-            for cone in green_cone_assets:
-                cone_id += 1
-                for mat in cone.GetMaterials():
-                    mat.SetClassID(2)
-                    mat.SetInstanceID(cone_id)
+    def LabelConeAssets():
+        cone_id = 0
+        for cone in red_cone_assets:
+            cone_id += 1
+            for mat in cone.GetMaterials():
+                mat.SetClassID(1)
+                mat.SetInstanceID(cone_id)
+
+        cone_id = 0
+        for cone in green_cone_assets:
+            cone_id += 1
+            for mat in cone.GetMaterials():
+                mat.SetClassID(2)
+                mat.SetInstanceID(cone_id)
 
     def RedistributeCones():
         for cone in green_cones:
-            x = (chrono.ChRandom() - .5) * cone_spread_x + cone_offset_x
-            y = (chrono.ChRandom() - .5) * cone_spread_y + cone_offset_y
+            x = (chrono.ChRandom() - 0.5) * cone_spread_x + cone_offset_x
+            y = (chrono.ChRandom() - 0.5) * cone_spread_y + cone_offset_y
             z = terrain.GetHeight((x, y, 1000))
             cone.SetPos((x, y, z))
 
         for cone in red_cones:
-            x = (chrono.ChRandom() - .5) * cone_spread_x + cone_offset_x
-            y = (chrono.ChRandom() - .5) * cone_spread_y + cone_offset_y
+            x = (chrono.ChRandom() - 0.5) * cone_spread_x + cone_offset_x
+            y = (chrono.ChRandom() - 0.5) * cone_spread_y + cone_offset_y
             z = terrain.GetHeight((x, y, 1000))
             cone.SetPos((x, y, z))
 
-
-
     # Create the RCCar vehicle, set parameters, and initialize
     vehicle = veh.RCCar()
     vehicle.SetContactMethod(contact_method)
@@ -310,7 +333,6 @@ def RedistributeCones():
     vehicle.SetTireStepSize(tire_step_size)
     vehicle.Initialize()
 
-
     vehicle.SetChassisVisualizationType(chassis_vis_type)
     vehicle.SetSuspensionVisualizationType(suspension_vis_type)
     vehicle.SetSteeringVisualizationType(steering_vis_type)
@@ -322,17 +344,27 @@ def RedistributeCones():
 
     patch_mat = chrono.ChMaterialSurfaceNSC()
     patch_mat.SetFriction(0.9)
-    patch_mat.SetRestitution(0.01) #-----check if mu, cr, and y is set similar to minfo
+    patch_mat.SetRestitution(
+        0.01
+    )  # -----check if mu, cr, and y is set similar to minfo
 
     # not right need to introduce point cloud -- not sure how to access this using the package_share_directory
-    patch = terrain.AddPatch(patch_mat, chrono.CSYSNORM, chrono.GetChronoDataFile("autonomy-toolkit/me3038/rm3038_pt_cloud.obj"))  #chrono.ChCoordsysD(chrono.ChVectorD(0, 0, 0), chrono.ChQuaternionD(1, 0, 0, 0))
+    patch = terrain.AddPatch(
+        patch_mat,
+        chrono.CSYSNORM,
+        chrono.GetChronoDataFile("autonomy-toolkit/me3038/rm3038_pt_cloud.obj"),
+    )  # chrono.ChCoordsysD(chrono.ChVectorD(0, 0, 0), chrono.ChQuaternionD(1, 0, 0, 0))
     # patch.SetTexture(veh.GetDataFile("terrain/textures/tile4.jpg"), 200, 200)
     # patch.SetColor(chrono.ChColor(0.8, 0.8, 0.5))
     terrain.Initialize()
 
     # add in ME3038 room mesh
     room_mmesh = chrono.ChTriangleMeshConnected()
-    room_mmesh.LoadWavefrontMesh(chrono.GetChronoDataFile("autonomy-toolkit/me3038/rm3038_pt_cloud.obj"), False, True)
+    room_mmesh.LoadWavefrontMesh(
+        chrono.GetChronoDataFile("autonomy-toolkit/me3038/rm3038_pt_cloud.obj"),
+        False,
+        True,
+    )
     room_mmesh.Transform(chrono.ChVectorD(0, 0, 0), chrono.ChMatrix33D(1))
 
     room_trimesh_shape = chrono.ChTriangleMeshShape()
@@ -347,39 +379,51 @@ def RedistributeCones():
 
     vehicle.GetSystem().Add(room_mesh_body)
 
-
     if cones_from_file:
         AddConesFromFile()
     else:
-        AddRandomCones(int(num_cones / 2), chrono.GetChronoDataFile("sensor/cones/green_cone.obj"), 2)
-        AddRandomCones(int(num_cones / 2), chrono.GetChronoDataFile("sensor/cones/red_cone.obj"), 1)
+        AddRandomCones(
+            int(num_cones / 2),
+            chrono.GetChronoDataFile("sensor/cones/green_cone.obj"),
+            2,
+        )
+        AddRandomCones(
+            int(num_cones / 2), chrono.GetChronoDataFile("sensor/cones/red_cone.obj"), 1
+        )
 
     # === create sensors ===
     manager = sens.ChSensorManager(vehicle.GetSystem())
-    manager.scene.AddPointLight(chrono.ChVectorF(100, 100, 100), chrono.ChColor(1, 1, 1), 5000)
-    
+    manager.scene.AddPointLight(
+        chrono.ChVectorF(100, 100, 100), chrono.ChColor(1, 1, 1), 5000
+    )
+
     b = sens.Background()
-    b.color_horizon = chrono.ChVectorF(.6, .7, .8)
-    b.color_zenith = chrono.ChVectorF(.4, .5, .6)
+    b.color_horizon = chrono.ChVectorF(0.6, 0.7, 0.8)
+    b.color_zenith = chrono.ChVectorF(0.4, 0.5, 0.6)
     b.mode = sens.BackgroundMode_GRADIENT
     manager.scene.SetBackground(b)
 
-    camera_pose = chrono.ChFrameD(chrono.ChVectorD(0.204, 0, 0.10018), chrono.Q_from_AngAxis(.2, chrono.ChVectorD(0, 1, 0)))
+    camera_pose = chrono.ChFrameD(
+        chrono.ChVectorD(0.204, 0, 0.10018),
+        chrono.Q_from_AngAxis(0.2, chrono.ChVectorD(0, 1, 0)),
+    )
     width = 1280
     height = 720
     frame_rate = 30.0
     if create_semantic_maps:
-        frame_rate = float(5)    # not sure about this
-    fov = 1.396;  # 80 degree FOV camera)
+        frame_rate = float(5)  # not sure about this
+    fov = 1.396
+    # 80 degree FOV camera)
 
     camera = sens.ChCameraSensor(
-                vehicle.GetChassisBody(), # body camera is attached to
-                frame_rate,                 # update rate in Hz
-                camera_pose,                # offset pose
-                width,                      # image width
-                height,                     # image height
-                fov,                        # FOV
-                2)
+        vehicle.GetChassisBody(),  # body camera is attached to
+        frame_rate,  # update rate in Hz
+        camera_pose,  # offset pose
+        width,  # image width
+        height,  # image height
+        fov,  # FOV
+        2,
+    )
 
     camera.SetName("Camera Sensor")
     c_window = float(0)
@@ -393,48 +437,61 @@ def RedistributeCones():
     camera.PushFilter(sens.ChFilterRGBA8Access())
     manager.AddSensor(camera)
 
-
     camera2 = sens.ChCameraSensor(
-                patch.GetGroundBody(),  # body camera is attached to
-                30,                      # update rate in Hz
-                chrono.ChFrameD(chrono.ChVectorD(init_loc_x + 2, init_loc_y, 10), 
-                chrono.Q_from_AngAxis(chrono.CH_C_PI_2, chrono.ChVectorD(0, 1, 0))),  # offset pose   ------ not sure what CH_C_PI_2 is
-                1280,                                                           # image width
-                720,                                                            # image height
-                3.14 / 4,                                                       # FOV
-                2)                                                              # super sample diameter
+        patch.GetGroundBody(),  # body camera is attached to
+        30,  # update rate in Hz
+        chrono.ChFrameD(
+            chrono.ChVectorD(init_loc_x + 2, init_loc_y, 10),
+            chrono.Q_from_AngAxis(chrono.CH_C_PI_2, chrono.ChVectorD(0, 1, 0)),
+        ),  # offset pose   ------ not sure what CH_C_PI_2 is
+        1280,  # image width
+        720,  # image height
+        3.14 / 4,  # FOV
+        2,
+    )  # super sample diameter
     camera2.SetName("Camera Sensor 2")
     # camera2.PushFilter(sens.ChFilterVisualize(1280, 720))
     if save_sensor_data:
         camera2.PushFilter(sens.ChFilterSave(sensor_data_dir + "cam2/"))
     manager.AddSensor(camera2)
 
-
     noise_model = sens.ChNoiseNone()
-    imu_offset_pose = chrono.ChFrameD(chrono.ChVectorD(0, 0, 0), chrono.Q_from_AngAxis(0, chrono.ChVectorD(1, 0, 0)))
-    gps_offset_pose = chrono.ChFrameD(chrono.ChVectorD(0, 0, 0), chrono.Q_from_AngAxis(0, chrono.ChVectorD(1, 0, 0)))
+    imu_offset_pose = chrono.ChFrameD(
+        chrono.ChVectorD(0, 0, 0), chrono.Q_from_AngAxis(0, chrono.ChVectorD(1, 0, 0))
+    )
+    gps_offset_pose = chrono.ChFrameD(
+        chrono.ChVectorD(0, 0, 0), chrono.Q_from_AngAxis(0, chrono.ChVectorD(1, 0, 0))
+    )
     gps_reference = chrono.ChVectorD(-89.400, 43.070, 260.0)
 
     # accelerometer
-    acc = sens.ChAccelerometerSensor(vehicle.GetChassisBody(), 100, imu_offset_pose, noise_model)
+    acc = sens.ChAccelerometerSensor(
+        vehicle.GetChassisBody(), 100, imu_offset_pose, noise_model
+    )
     acc.SetName("IMU - Accelerometer")
     acc.PushFilter(sens.ChFilterAccelAccess())
     manager.AddSensor(acc)
 
     # gyroscope
-    gyro = sens.ChGyroscopeSensor(vehicle.GetChassisBody(), 100, imu_offset_pose, noise_model)
+    gyro = sens.ChGyroscopeSensor(
+        vehicle.GetChassisBody(), 100, imu_offset_pose, noise_model
+    )
     gyro.SetName("IMU - Gyroscope")
     gyro.PushFilter(sens.ChFilterGyroAccess())
     manager.AddSensor(gyro)
 
     # magnetometer
-    mag = sens.ChMagnetometerSensor(vehicle.GetChassisBody(), 100, imu_offset_pose, noise_model, gps_reference)
+    mag = sens.ChMagnetometerSensor(
+        vehicle.GetChassisBody(), 100, imu_offset_pose, noise_model, gps_reference
+    )
     mag.SetName("IMU - Magnetometer")
     mag.PushFilter(sens.ChFilterMagnetAccess())
     manager.AddSensor(mag)
 
     # gps
-    gps = sens.ChGPSSensor(vehicle.GetChassisBody(), 10, gps_offset_pose, gps_reference, noise_model)
+    gps = sens.ChGPSSensor(
+        vehicle.GetChassisBody(), 10, gps_offset_pose, gps_reference, noise_model
+    )
     gps.SetName("GPS")
     gps.PushFilter(sens.ChFilterGPSAccess())
     manager.AddSensor(gps)
@@ -446,22 +503,34 @@ def RedistributeCones():
 
     driver = veh.ChExternalDriver(vehicle.GetVehicle(), 50000)
 
-    system_generator = ChSystem_DataGeneratorFunctor("~/output/time", vehicle.GetSystem())
+    system_generator = ChSystem_DataGeneratorFunctor(
+        "~/output/time", vehicle.GetSystem()
+    )
     driver.AddDataGenerator(system_generator)
 
-    veh_generator = ChVehicle_DataGeneratorFunctor("~/output/vehicle", vehicle.GetVehicle())
+    veh_generator = ChVehicle_DataGeneratorFunctor(
+        "~/output/vehicle", vehicle.GetVehicle()
+    )
     driver.AddDataGenerator(veh_generator, 10)
 
-    cam_generator = ChCameraSensor_DataGeneratorFunctor("~/output/camera/front_facing_camera", camera)
+    cam_generator = ChCameraSensor_DataGeneratorFunctor(
+        "~/output/camera/front_facing_camera", camera
+    )
     driver.AddDataGenerator(cam_generator, frame_rate)
 
-    acc_generator = ChAccelerometerSensor_DataGeneratorFunctor("~/output/accelerometer/data", acc)
+    acc_generator = ChAccelerometerSensor_DataGeneratorFunctor(
+        "~/output/accelerometer/data", acc
+    )
     driver.AddDataGenerator(acc_generator, 100)
 
-    gyro_generator = ChGyroscopeSensor_DataGeneratorFunctor("~/output/gyroscope/data", gyro)
+    gyro_generator = ChGyroscopeSensor_DataGeneratorFunctor(
+        "~/output/gyroscope/data", gyro
+    )
     driver.AddDataGenerator(gyro_generator, 100)
 
-    mag_generator = ChMagnetometerSensor_DataGeneratorFunctor("~/output/magnetometer/data", mag)
+    mag_generator = ChMagnetometerSensor_DataGeneratorFunctor(
+        "~/output/magnetometer/data", mag
+    )
     driver.AddDataGenerator(mag_generator, 100)
 
     gps_generator = ChGPSSensor_DataGeneratorFunctor("~/output/gps/data", gps)
@@ -472,9 +541,9 @@ def RedistributeCones():
 
     # Create the vehicle Irrlicht interface
     if USE_IRRLICHT:
-        app = veh.ChWheeledVehicleIrrApp(vehicle.GetVehicle(), 'RC Car')
+        app = veh.ChWheeledVehicleIrrApp(vehicle.GetVehicle(), "RC Car")
         app.AddTypicalLights()
-        app.AddLogo(chrono.GetChronoDataFile('logo_pychrono_alpha.png'))
+        app.AddLogo(chrono.GetChronoDataFile("logo_pychrono_alpha.png"))
         app.SetChaseCamera(trackPoint, 15.0, 0.5)
         app.SetTimestep(step_size)
         app.AssetBindAll()
@@ -484,7 +553,7 @@ def RedistributeCones():
     realtime_timer = chrono.ChRealtimeStepTimer()
     while True:
         time = vehicle.GetSystem().GetChTime()
-    
+
         # End simulation
         if (USE_IRRLICHT and not app.GetDevice().run()) or time >= t_end:
             break
@@ -518,16 +587,16 @@ def RedistributeCones():
 
         # Spin in place for real time to catch up
         realtime_timer.Spin(step_size)
-    
-    return 0       
-    
+
+    return 0
+
 
 # The path to the Chrono data directory containing various assets (meshes, textures, data files)
 # is automatically set, relative to the default location of this demo.
-# If running from a different directory, you must change the path to the data directory with: 
+# If running from a different directory, you must change the path to the data directory with:
 data_folder = os.path.join(os.path.dirname(os.path.abspath(__file__)), "..", "data", "")
 chrono.SetChronoDataPath(data_folder)
-veh.SetDataPath(chrono.GetChronoDataPath() + 'vehicle/')
+veh.SetDataPath(chrono.GetChronoDataPath() + "vehicle/")
 
 create_semantic_maps = False
 save_sensor_data = False
@@ -542,8 +611,12 @@ def RedistributeCones():
 cone_spread_x = 1.0
 cone_spread_y = 1.0
 
-throttle_scaling = .5 * abs(1400.0 - 1500.0) / (1980.0 - 1500.0) # based on motor_driver.py limits for safety
-braking_scaling = abs(1600.0 - 1500.0) / (1980.0 - 1500.0)  # based on motor_driver.py limits for safety
+throttle_scaling = (
+    0.5 * abs(1400.0 - 1500.0) / (1980.0 - 1500.0)
+)  # based on motor_driver.py limits for safety
+braking_scaling = abs(1600.0 - 1500.0) / (
+    1980.0 - 1500.0
+)  # based on motor_driver.py limits for safety
 steering_scaling = 1.0  # abs(1725.0 - 1500.0) / (1980.0 - 1500.0);  // based on motor_driver.py limits for safety
 
 trackPoint = chrono.ChVectorD(0.0, 0.0, 1.75)
@@ -581,9 +654,9 @@ def RedistributeCones():
 tire_model = veh.TireModelType_TMEASY
 
 # Rigid terrain -- not sure about this
-terrainHeight = -.2339      # terrain height (FLAT terrain only)
+terrainHeight = -0.2339  # terrain height (FLAT terrain only)
 terrainLength = 7.0  # size in X direction
-terrainWidth = 7.0   # size in Y direction
+terrainWidth = 7.0  # size in Y direction
 
 # Contact method
 contact_method = chrono.ChContactMethod_NSC
@@ -600,4 +673,3 @@ def RedistributeCones():
 t_end = 1000
 
 main()
-
diff --git a/sim/python/demo_straight_line.py b/sim/python/demo_straight_line.py
index be463761..4b86dc95 100644
--- a/sim/python/demo_straight_line.py
+++ b/sim/python/demo_straight_line.py
@@ -39,7 +39,8 @@
 import pychrono.irrlicht as irr
 import pychrono.sensor as sens
 
-#// =============================================================================
+# // =============================================================================
+
 
 class ChSystem_DataGeneratorFunctor(veh.ChExternalDriver_DataGeneratorFunctor):
     def __init__(self, id: str, system: chrono.ChSystem):
@@ -50,6 +51,7 @@ def __init__(self, id: str, system: chrono.ChSystem):
     def Serialize(self, writer):
         writer.Key("time") << self.system.GetChTime()
 
+
 class ChCameraSensor_DataGeneratorFunctor(veh.ChExternalDriver_DataGeneratorFunctor):
     def __init__(self, id: str, cam: sens.ChCameraSensor):
         super().__init__("ChCameraSensor", id)
@@ -60,18 +62,23 @@ def Serialize(self, writer):
         rgba8_buffer = self.cam.GetMostRecentRGBA8Buffer()
         if rgba8_buffer.HasData():
             rgba8_data = rgba8_buffer.GetRGBA8Data()
-            rgba8_data = np.ascontiguousarray(rgba8_data[::-1]) # Needs to be contigious
+            rgba8_data = np.ascontiguousarray(
+                rgba8_data[::-1]
+            )  # Needs to be contigious
             shape = rgba8_data.shape
             writer.Key("width") << shape[1]
-            writer.Key("height") << shape[0] 
+            writer.Key("height") << shape[0]
             writer.Key("size") << shape[2]
             writer.Key("encoding") << "rgba8"
-            writer.Key("image").PointerAsString(rgba8_data.ctypes.data, int(np.prod(shape)))
+            writer.Key("image").PointerAsString(
+                rgba8_data.ctypes.data, int(np.prod(shape))
+            )
 
     def HasData(self) -> bool:
         rgba8_buffer = self.cam.GetMostRecentRGBA8Buffer()
         return rgba8_buffer.HasData()
 
+
 class ChVehicle_DataGeneratorFunctor(veh.ChExternalDriver_DataGeneratorFunctor):
     def __init__(self, id: str, vehicle: veh.ChVehicle):
         super().__init__("ChVehicle", id)
@@ -88,7 +95,10 @@ def Serialize(self, writer):
         writer.Key("lin_acc") << body.GetPos_dtdt()
         writer.Key("ang_acc") << body.GetWacc_loc()
 
-class ChAccelerometerSensor_DataGeneratorFunctor(veh.ChExternalDriver_DataGeneratorFunctor):
+
+class ChAccelerometerSensor_DataGeneratorFunctor(
+    veh.ChExternalDriver_DataGeneratorFunctor
+):
     def __init__(self, id: str, acc: sens.ChAccelerometerSensor):
         super().__init__("ChAccelerometerSensor", id)
 
@@ -106,6 +116,7 @@ def HasData(self) -> bool:
         buffer = self.acc.GetMostRecentAccelBuffer()
         return buffer.HasData()
 
+
 class ChGyroscopeSensor_DataGeneratorFunctor(veh.ChExternalDriver_DataGeneratorFunctor):
     def __init__(self, id: str, gyro: sens.ChGyroscopeSensor):
         super().__init__("ChGyroscopeSensor", id)
@@ -124,7 +135,10 @@ def HasData(self) -> bool:
         buffer = self.gyro.GetMostRecentGyroBuffer()
         return buffer.HasData()
 
-class ChMagnetometerSensor_DataGeneratorFunctor(veh.ChExternalDriver_DataGeneratorFunctor):
+
+class ChMagnetometerSensor_DataGeneratorFunctor(
+    veh.ChExternalDriver_DataGeneratorFunctor
+):
     def __init__(self, id: str, mag: sens.ChMagnetometerSensor):
         super().__init__("ChMagnetometerSensor", id)
 
@@ -142,6 +156,7 @@ def HasData(self) -> bool:
         buffer = self.mag.GetMostRecentMagnetBuffer()
         return buffer.HasData()
 
+
 class ChGPSSensor_DataGeneratorFunctor(veh.ChExternalDriver_DataGeneratorFunctor):
     def __init__(self, id: str, gps: sens.ChGPSSensor):
         super().__init__("ChGPSSensor", id)
@@ -160,6 +175,7 @@ def HasData(self) -> bool:
         buffer = self.gps.GetMostRecentGPSBuffer()
         return buffer.HasData()
 
+
 class ChDriverInputs_DataParserFunctor(veh.ChExternalDriver_DataParserFunctor):
     def __init__(self, driver: veh.ChDriver):
         super().__init__("ChDriverInputs")
@@ -176,19 +192,19 @@ def Deserialize(self, reader):
         self.driver.SetBraking(braking)
 
 
-#// =============================================================================
+# // =============================================================================
+
 
 def main():
-    
     def AddRandomCones(count, filename, class_id=1):
         mmesh = chrono.ChTriangleMeshConnected()
-        mmesh.LoadWavefrontMesh(filename, False, True)  
-        mmesh.Transform(chrono.ChVectorD(0, 0, 0), chrono.ChMatrix33D(1)) 
+        mmesh.LoadWavefrontMesh(filename, False, True)
+        mmesh.Transform(chrono.ChVectorD(0, 0, 0), chrono.ChMatrix33D(1))
         for i in range(count):
             # Generate a random position
-            x = (chrono.ChRandom() - .5) * cone_spread_x + cone_offset_x
-            y = (chrono.ChRandom() - .5) * cone_spread_y + cone_offset_y
-            z = terrain.GetHeight(chrono.ChVectorD(x, y, 1000)) # get the terrain z
+            x = (chrono.ChRandom() - 0.5) * cone_spread_x + cone_offset_x
+            y = (chrono.ChRandom() - 0.5) * cone_spread_y + cone_offset_y
+            z = terrain.GetHeight(chrono.ChVectorD(x, y, 1000))  # get the terrain z
             pos = chrono.ChVectorD(x, y, z)
 
             trimesh_shape = chrono.ChTriangleMeshShape()
@@ -204,7 +220,7 @@ def AddRandomCones(count, filename, class_id=1):
             mesh_body.SetBodyFixed(True)
             vehicle.GetSystem().Add(mesh_body)
 
-            if (class_id == 1):
+            if class_id == 1:
                 red_cone_assets.append(trimesh_shape)
                 red_cones.append(mesh_body)
             else:
@@ -213,27 +229,37 @@ def AddRandomCones(count, filename, class_id=1):
 
     def AddConesFromFile():
         green_cone_mesh = chrono.ChTriangleMeshConnected()
-        green_cone_mesh.LoadWavefrontMesh(chrono.GetChronoDataFile("sensor/cones/green_cone.obj"), False, True) 
+        green_cone_mesh.LoadWavefrontMesh(
+            chrono.GetChronoDataFile("sensor/cones/green_cone.obj"), False, True
+        )
         green_cone_mesh.Transform(chrono.ChVectorD(0, 0, 0), chrono.ChMatrix33D(1))
 
         red_cone_mesh = chrono.ChTriangleMeshConnected()
-        red_cone_mesh.LoadWavefrontMesh(chrono.GetChronoDataFile("sensor/cones/red_cone.obj"), False, True)
+        red_cone_mesh.LoadWavefrontMesh(
+            chrono.GetChronoDataFile("sensor/cones/red_cone.obj"), False, True
+        )
         red_cone_mesh.Transform(chrono.ChVectorD(0, 0, 0), chrono.ChMatrix33D(1))
 
-        with open(chrono.GetChronoDataFile("autonomy-toolkit/paths/straight.csv")) as cone_file:
+        with open(
+            chrono.GetChronoDataFile("autonomy-toolkit/paths/straight.csv")
+        ) as cone_file:
             while True:
                 line = cone_file.readline()
                 if not line:
                     break
-                x_inner, y_inner, x_outer, y_outer = map(float, line.split(' ')) 
+                x_inner, y_inner, x_outer, y_outer = map(float, line.split(" "))
 
                 pos_green_x = x_inner + cone_offset_x
                 pos_green_y = y_inner + cone_offset_y
-                pos_green_z = terrain.GetHeight(chrono.ChVectorD(pos_green_x, pos_green_y, 1000))
+                pos_green_z = terrain.GetHeight(
+                    chrono.ChVectorD(pos_green_x, pos_green_y, 1000)
+                )
 
                 pos_red_x = x_outer + cone_offset_x
                 pos_red_y = y_outer + cone_offset_y
-                pos_red_z = terrain.GetHeight(chrono.ChVectorD(pos_red_x, pos_red_y, 1000))
+                pos_red_z = terrain.GetHeight(
+                    chrono.ChVectorD(pos_red_x, pos_red_y, 1000)
+                )
 
                 pos_green = chrono.ChVectorD(pos_green_x, pos_green_y, pos_green_z)
                 pos_red = chrono.ChVectorD(pos_red_x, pos_red_y, pos_red_z)
@@ -264,38 +290,35 @@ def AddConesFromFile():
                 red_body.SetBodyFixed(True)
                 red_cones.append(red_body)
                 vehicle.GetSystem().Add(red_body)
-            
-    def LabelConeAssets():
-            cone_id = 0
-            for cone in red_cone_assets:
-                cone_id += 1
-                for mat in cone.material_list:
-                    mat.SetClassID(1)
-                    mat.SetInstanceID(cone_id)
 
-
-            cone_id = 0
-            for cone in green_cone_assets:
-                cone_id += 1
-                for mat in cone.material_list:
-                    mat.SetClassID(2)
-                    mat.SetInstanceID(cone_id)
+    def LabelConeAssets():
+        cone_id = 0
+        for cone in red_cone_assets:
+            cone_id += 1
+            for mat in cone.material_list:
+                mat.SetClassID(1)
+                mat.SetInstanceID(cone_id)
+
+        cone_id = 0
+        for cone in green_cone_assets:
+            cone_id += 1
+            for mat in cone.material_list:
+                mat.SetClassID(2)
+                mat.SetInstanceID(cone_id)
 
     def RedistributeCones():
         for cone in green_cones:
-            x = (chrono.ChRandom() - .5) * cone_spread_x + cone_offset_x
-            y = (chrono.ChRandom() - .5) * cone_spread_y + cone_offset_y
+            x = (chrono.ChRandom() - 0.5) * cone_spread_x + cone_offset_x
+            y = (chrono.ChRandom() - 0.5) * cone_spread_y + cone_offset_y
             z = terrain.GetHeight((x, y, 1000))
             cone.SetPos((x, y, z))
 
         for cone in red_cones:
-            x = (chrono.ChRandom() - .5) * cone_spread_x + cone_offset_x
-            y = (chrono.ChRandom() - .5) * cone_spread_y + cone_offset_y
+            x = (chrono.ChRandom() - 0.5) * cone_spread_x + cone_offset_x
+            y = (chrono.ChRandom() - 0.5) * cone_spread_y + cone_offset_y
             z = terrain.GetHeight((x, y, 1000))
             cone.SetPos((x, y, z))
 
-
-
     # Create the RCCar vehicle, set parameters, and initialize
     vehicle = veh.RCCar()
     vehicle.SetContactMethod(contact_method)
@@ -306,7 +329,6 @@ def RedistributeCones():
     vehicle.SetTireStepSize(tire_step_size)
     vehicle.Initialize()
 
-
     vehicle.SetChassisVisualizationType(chassis_vis_type)
     vehicle.SetSuspensionVisualizationType(suspension_vis_type)
     vehicle.SetSteeringVisualizationType(steering_vis_type)
@@ -318,17 +340,27 @@ def RedistributeCones():
 
     patch_mat = chrono.ChMaterialSurfaceNSC()
     patch_mat.SetFriction(0.9)
-    patch_mat.SetRestitution(0.01) #-----check if mu, cr, and y is set similar to minfo
+    patch_mat.SetRestitution(
+        0.01
+    )  # -----check if mu, cr, and y is set similar to minfo
 
     # not right need to introduce point cloud -- not sure how to access this using the package_share_directory
-    patch = terrain.AddPatch(patch_mat, chrono.CSYSNORM, chrono.GetChronoDataFile("autonomy-toolkit/me3038/rm3038_pt_cloud.obj"))  #chrono.ChCoordsysD(chrono.ChVectorD(0, 0, 0), chrono.ChQuaternionD(1, 0, 0, 0))
+    patch = terrain.AddPatch(
+        patch_mat,
+        chrono.CSYSNORM,
+        chrono.GetChronoDataFile("autonomy-toolkit/me3038/rm3038_pt_cloud.obj"),
+    )  # chrono.ChCoordsysD(chrono.ChVectorD(0, 0, 0), chrono.ChQuaternionD(1, 0, 0, 0))
     # patch.SetTexture(veh.GetDataFile("terrain/textures/tile4.jpg"), 200, 200)
     # patch.SetColor(chrono.ChColor(0.8, 0.8, 0.5))
     terrain.Initialize()
 
     # add in ME3038 room mesh
     room_mmesh = chrono.ChTriangleMeshConnected()
-    room_mmesh.LoadWavefrontMesh(chrono.GetChronoDataFile("autonomy-toolkit/me3038/rm3038_pt_cloud.obj"), False, True)
+    room_mmesh.LoadWavefrontMesh(
+        chrono.GetChronoDataFile("autonomy-toolkit/me3038/rm3038_pt_cloud.obj"),
+        False,
+        True,
+    )
     room_mmesh.Transform(chrono.ChVectorD(0, 0, 0), chrono.ChMatrix33D(1))
 
     room_trimesh_shape = chrono.ChTriangleMeshShape()
@@ -342,39 +374,51 @@ def RedistributeCones():
 
     vehicle.GetSystem().Add(room_mesh_body)
 
-
     if cones_from_file:
         AddConesFromFile()
     else:
-        AddRandomCones(int(num_cones / 2), chrono.GetChronoDataFile("sensor/cones/green_cone.obj"), 2)
-        AddRandomCones(int(num_cones / 2), chrono.GetChronoDataFile("sensor/cones/red_cone.obj"), 1)
+        AddRandomCones(
+            int(num_cones / 2),
+            chrono.GetChronoDataFile("sensor/cones/green_cone.obj"),
+            2,
+        )
+        AddRandomCones(
+            int(num_cones / 2), chrono.GetChronoDataFile("sensor/cones/red_cone.obj"), 1
+        )
 
     # === create sensors ===
     manager = sens.ChSensorManager(vehicle.GetSystem())
-    manager.scene.AddPointLight(chrono.ChVectorF(100, 100, 100), chrono.ChVectorF(1, 1, 1), 5000)
-    
+    manager.scene.AddPointLight(
+        chrono.ChVectorF(100, 100, 100), chrono.ChVectorF(1, 1, 1), 5000
+    )
+
     b = sens.Background()
-    b.color_horizon = chrono.ChVectorF(.6, .7, .8)
-    b.color_zenith = chrono.ChVectorF(.4, .5, .6)
+    b.color_horizon = chrono.ChVectorF(0.6, 0.7, 0.8)
+    b.color_zenith = chrono.ChVectorF(0.4, 0.5, 0.6)
     b.mode = sens.BackgroundMode_GRADIENT
     manager.scene.SetBackground(b)
 
-    camera_pose = chrono.ChFrameD(chrono.ChVectorD(0.204, 0, 0.10018), chrono.Q_from_AngAxis(.2, chrono.ChVectorD(0, 1, 0)))
+    camera_pose = chrono.ChFrameD(
+        chrono.ChVectorD(0.204, 0, 0.10018),
+        chrono.Q_from_AngAxis(0.2, chrono.ChVectorD(0, 1, 0)),
+    )
     width = 1280
     height = 720
     frame_rate = 30.0
     if create_semantic_maps:
-        frame_rate = float(5)    # not sure about this
-    fov = 1.396;  # 80 degree FOV camera)
+        frame_rate = float(5)  # not sure about this
+    fov = 1.396
+    # 80 degree FOV camera)
 
     camera = sens.ChCameraSensor(
-                vehicle.GetChassisBody(), # body camera is attached to
-                frame_rate,                 # update rate in Hz
-                camera_pose,                # offset pose
-                width,                      # image width
-                height,                     # image height
-                fov,                        # FOV
-                2)
+        vehicle.GetChassisBody(),  # body camera is attached to
+        frame_rate,  # update rate in Hz
+        camera_pose,  # offset pose
+        width,  # image width
+        height,  # image height
+        fov,  # FOV
+        2,
+    )
 
     camera.SetName("Camera Sensor")
     c_window = float(0)
@@ -388,63 +432,75 @@ def RedistributeCones():
     camera.PushFilter(sens.ChFilterRGBA8Access())
     manager.AddSensor(camera)
 
-
     camera2 = sens.ChCameraSensor(
-                patch.GetGroundBody(),  # body camera is attached to
-                30,                      # update rate in Hz
-                chrono.ChFrameD(chrono.ChVectorD(init_loc_x + 2, init_loc_y, 10), 
-                chrono.Q_from_AngAxis(chrono.CH_C_PI_2, chrono.ChVectorD(0, 1, 0))),  # offset pose   ------ not sure what CH_C_PI_2 is
-                1280,                                                           # image width
-                720,                                                            # image height
-                3.14 / 4,                                                       # FOV
-                2)                                                              # super sample diameter
+        patch.GetGroundBody(),  # body camera is attached to
+        30,  # update rate in Hz
+        chrono.ChFrameD(
+            chrono.ChVectorD(init_loc_x + 2, init_loc_y, 10),
+            chrono.Q_from_AngAxis(chrono.CH_C_PI_2, chrono.ChVectorD(0, 1, 0)),
+        ),  # offset pose   ------ not sure what CH_C_PI_2 is
+        1280,  # image width
+        720,  # image height
+        3.14 / 4,  # FOV
+        2,
+    )  # super sample diameter
     camera2.SetName("Camera Sensor 2")
     # camera2.PushFilter(sens.ChFilterVisualize(1280, 720))
     if save_sensor_data:
         camera2.PushFilter(sens.ChFilterSave(sensor_data_dir + "cam2/"))
     manager.AddSensor(camera2)
 
-    
     lidar = sens.ChLidarSensor(
-                vehicle.GetChassisBody(), # body camera is attached to
-                frame_rate,                 # update rate in Hz
-                camera_pose,                # offset pose
-                900,                      # image width
-                30,                     # image height
-                3.14,                        # FOV
-                0.3,
-                - 0.3,
-                100
+        vehicle.GetChassisBody(),  # body camera is attached to
+        frame_rate,  # update rate in Hz
+        camera_pose,  # offset pose
+        900,  # image width
+        30,  # image height
+        3.14,  # FOV
+        0.3,
+        -0.3,
+        100,
     )
     lidar.SetName("Lidar")
     manager.AddSensor(lidar)
 
-
     noise_model = sens.ChNoiseNone()
-    imu_offset_pose = chrono.ChFrameD(chrono.ChVectorD(0, 0, 0), chrono.Q_from_AngAxis(0, chrono.ChVectorD(1, 0, 0)))
-    gps_offset_pose = chrono.ChFrameD(chrono.ChVectorD(0, 0, 0), chrono.Q_from_AngAxis(0, chrono.ChVectorD(1, 0, 0)))
+    imu_offset_pose = chrono.ChFrameD(
+        chrono.ChVectorD(0, 0, 0), chrono.Q_from_AngAxis(0, chrono.ChVectorD(1, 0, 0))
+    )
+    gps_offset_pose = chrono.ChFrameD(
+        chrono.ChVectorD(0, 0, 0), chrono.Q_from_AngAxis(0, chrono.ChVectorD(1, 0, 0))
+    )
     gps_reference = chrono.ChVectorD(-89.400, 43.070, 260.0)
 
     # accelerometer
-    acc = sens.ChAccelerometerSensor(vehicle.GetChassisBody(), 100, imu_offset_pose, noise_model)
+    acc = sens.ChAccelerometerSensor(
+        vehicle.GetChassisBody(), 100, imu_offset_pose, noise_model
+    )
     acc.SetName("IMU - Accelerometer")
     acc.PushFilter(sens.ChFilterAccelAccess())
     manager.AddSensor(acc)
 
     # gyroscope
-    gyro = sens.ChGyroscopeSensor(vehicle.GetChassisBody(), 100, imu_offset_pose, noise_model)
+    gyro = sens.ChGyroscopeSensor(
+        vehicle.GetChassisBody(), 100, imu_offset_pose, noise_model
+    )
     gyro.SetName("IMU - Gyroscope")
     gyro.PushFilter(sens.ChFilterGyroAccess())
     manager.AddSensor(gyro)
 
     # magnetometer
-    mag = sens.ChMagnetometerSensor(vehicle.GetChassisBody(), 100, imu_offset_pose, noise_model, gps_reference)
+    mag = sens.ChMagnetometerSensor(
+        vehicle.GetChassisBody(), 100, imu_offset_pose, noise_model, gps_reference
+    )
     mag.SetName("IMU - Magnetometer")
     mag.PushFilter(sens.ChFilterMagnetAccess())
     manager.AddSensor(mag)
 
     # gps
-    gps = sens.ChGPSSensor(vehicle.GetChassisBody(), 10, gps_offset_pose, gps_reference, noise_model)
+    gps = sens.ChGPSSensor(
+        vehicle.GetChassisBody(), 10, gps_offset_pose, gps_reference, noise_model
+    )
     gps.SetName("GPS")
     gps.PushFilter(sens.ChFilterGPSAccess())
     manager.AddSensor(gps)
@@ -456,25 +512,37 @@ def RedistributeCones():
 
     driver = veh.ChExternalDriver(vehicle.GetVehicle(), 50000)
 
-    system_generator = ChSystem_DataGeneratorFunctor("~/output/time", vehicle.GetSystem())
+    system_generator = ChSystem_DataGeneratorFunctor(
+        "~/output/time", vehicle.GetSystem()
+    )
     driver.AddDataGenerator(system_generator)
 
-    veh_generator = ChVehicle_DataGeneratorFunctor("~/output/vehicle", vehicle.GetVehicle())
+    veh_generator = ChVehicle_DataGeneratorFunctor(
+        "~/output/vehicle", vehicle.GetVehicle()
+    )
     driver.AddDataGenerator(veh_generator, 10)
 
-    cam_generator = ChCameraSensor_DataGeneratorFunctor("~/output/camera/front_facing_camera", camera)
+    cam_generator = ChCameraSensor_DataGeneratorFunctor(
+        "~/output/camera/front_facing_camera", camera
+    )
     driver.AddDataGenerator(cam_generator, frame_rate)
 
     # lidar_generator = ChLidarSensor_DataGeneratorFunctor("~/output/lidar", lidar)
     # driver.AddDataGenerator(lidar_generator, frame_rate)
 
-    acc_generator = ChAccelerometerSensor_DataGeneratorFunctor("~/output/accelerometer/data", acc)
+    acc_generator = ChAccelerometerSensor_DataGeneratorFunctor(
+        "~/output/accelerometer/data", acc
+    )
     driver.AddDataGenerator(acc_generator, 100)
 
-    gyro_generator = ChGyroscopeSensor_DataGeneratorFunctor("~/output/gyroscope/data", gyro)
+    gyro_generator = ChGyroscopeSensor_DataGeneratorFunctor(
+        "~/output/gyroscope/data", gyro
+    )
     driver.AddDataGenerator(gyro_generator, 100)
 
-    mag_generator = ChMagnetometerSensor_DataGeneratorFunctor("~/output/magnetometer/data", mag)
+    mag_generator = ChMagnetometerSensor_DataGeneratorFunctor(
+        "~/output/magnetometer/data", mag
+    )
     driver.AddDataGenerator(mag_generator, 100)
 
     gps_generator = ChGPSSensor_DataGeneratorFunctor("~/output/gps/data", gps)
@@ -485,9 +553,9 @@ def RedistributeCones():
 
     # Create the vehicle Irrlicht interface
     if USE_IRRLICHT:
-        app = veh.ChWheeledVehicleIrrApp(vehicle.GetVehicle(), 'RC Car')
+        app = veh.ChWheeledVehicleIrrApp(vehicle.GetVehicle(), "RC Car")
         app.AddTypicalLights()
-        app.AddLogo(chrono.GetChronoDataFile('logo_pychrono_alpha.png'))
+        app.AddLogo(chrono.GetChronoDataFile("logo_pychrono_alpha.png"))
         app.SetChaseCamera(trackPoint, 15.0, 0.5)
         app.SetTimestep(step_size)
         app.AssetBindAll()
@@ -497,7 +565,7 @@ def RedistributeCones():
     realtime_timer = chrono.ChRealtimeStepTimer()
     while True:
         time = vehicle.GetSystem().GetChTime()
-    
+
         # End simulation
         if (USE_IRRLICHT and not app.GetDevice().run()) or time >= t_end:
             break
@@ -531,16 +599,16 @@ def RedistributeCones():
 
         # Spin in place for real time to catch up
         realtime_timer.Spin(step_size)
-    
-    return 0       
-    
+
+    return 0
+
 
 # The path to the Chrono data directory containing various assets (meshes, textures, data files)
 # is automatically set, relative to the default location of this demo.
-# If running from a different directory, you must change the path to the data directory with: 
+# If running from a different directory, you must change the path to the data directory with:
 data_folder = os.path.join(os.path.dirname(os.path.abspath(__file__)), "..", "data", "")
 chrono.SetChronoDataPath(data_folder)
-veh.SetDataPath(chrono.GetChronoDataPath() + 'vehicle/')
+veh.SetDataPath(chrono.GetChronoDataPath() + "vehicle/")
 
 create_semantic_maps = False
 save_sensor_data = False
@@ -555,8 +623,12 @@ def RedistributeCones():
 cone_spread_x = 1.0
 cone_spread_y = 1.0
 
-throttle_scaling = .5 * abs(1400.0 - 1500.0) / (1980.0 - 1500.0) # based on motor_driver.py limits for safety
-braking_scaling = abs(1600.0 - 1500.0) / (1980.0 - 1500.0)  # based on motor_driver.py limits for safety
+throttle_scaling = (
+    0.5 * abs(1400.0 - 1500.0) / (1980.0 - 1500.0)
+)  # based on motor_driver.py limits for safety
+braking_scaling = abs(1600.0 - 1500.0) / (
+    1980.0 - 1500.0
+)  # based on motor_driver.py limits for safety
 steering_scaling = 1.0  # abs(1725.0 - 1500.0) / (1980.0 - 1500.0);  // based on motor_driver.py limits for safety
 
 trackPoint = chrono.ChVectorD(0.0, 0.0, 1.75)
@@ -575,7 +647,7 @@ def RedistributeCones():
 # Initial vehicle location
 init_loc_x = -2
 init_loc_y = 0
-init_angle_z = 0 #1.57
+init_angle_z = 0  # 1.57
 
 initLoc = chrono.ChVectorD(init_loc_x, init_loc_y, 0.5)
 initRot = chrono.Q_from_AngZ(init_angle_z)
@@ -594,9 +666,9 @@ def RedistributeCones():
 tire_model = veh.TireModelType_TMEASY
 
 # Rigid terrain -- not sure about this
-terrainHeight = -.2339      # terrain height (FLAT terrain only)
+terrainHeight = -0.2339  # terrain height (FLAT terrain only)
 terrainLength = 7.0  # size in X direction
-terrainWidth = 7.0   # size in Y direction
+terrainWidth = 7.0  # size in Y direction
 
 # Contact method
 contact_method = chrono.ChContactMethod_NSC
@@ -613,4 +685,3 @@ def RedistributeCones():
 t_end = 1000
 
 main()
-
diff --git a/workspace/src/common/interfaces/art_msgs/msg/VehicleInput.msg b/workspace/src/common/interfaces/art_msgs/msg/VehicleInput.msg
index dc1e8aa2..dcc01d62 100644
--- a/workspace/src/common/interfaces/art_msgs/msg/VehicleInput.msg
+++ b/workspace/src/common/interfaces/art_msgs/msg/VehicleInput.msg
@@ -3,7 +3,7 @@ std_msgs/Header header
 
 # Input message that is to be sent to the vehicle
 #
-# The input includes steering, throttle, and braking. 
+# The input includes steering, throttle, and braking.
 
 # Steering value [-1,1]
 # -1 indicates turn fully to the left, 1 indicates turn fully to the right
diff --git a/workspace/src/common/interfaces/art_perception_msgs/msg/Object.msg b/workspace/src/common/interfaces/art_perception_msgs/msg/Object.msg
index 42231ca4..9cf7e70f 100644
--- a/workspace/src/common/interfaces/art_perception_msgs/msg/Object.msg
+++ b/workspace/src/common/interfaces/art_perception_msgs/msg/Object.msg
@@ -18,4 +18,3 @@ geometry_msgs/Pose pose
 
 # The twist (derivative of pose) of the detected Object
 geometry_msgs/Twist twist
-
diff --git a/workspace/src/common/interfaces/art_perception_msgs/msg/ObjectClassification.msg b/workspace/src/common/interfaces/art_perception_msgs/msg/ObjectClassification.msg
index de4e9d73..28199943 100644
--- a/workspace/src/common/interfaces/art_perception_msgs/msg/ObjectClassification.msg
+++ b/workspace/src/common/interfaces/art_perception_msgs/msg/ObjectClassification.msg
@@ -8,12 +8,12 @@ float32 confidence
 
 # The classification types
 # See ART_OBJECT_CLASSIFICATION below
-uint8 classification 
+uint8 classification
 
 # Further custom classification types to be used to further define an object
 # 	Example use case is a traffic light, which can be defined as green, red, etc.
 # 	This variable should then be set to a different enum, as desired
-uint8 custom_classification 
+uint8 custom_classification
 
 # THe possible classification types
 uint8 ART_OBJECT_CLASSIFICATION_UNKNOWN=0
diff --git a/workspace/src/common/launch/art_control_launch/CMakeLists.txt b/workspace/src/common/launch/art_control_launch/CMakeLists.txt
index b1937f1a..4e20b7ca 100644
--- a/workspace/src/common/launch/art_control_launch/CMakeLists.txt
+++ b/workspace/src/common/launch/art_control_launch/CMakeLists.txt
@@ -13,4 +13,3 @@ ament_auto_package(
   INSTALL_TO_SHARE
   launch
 )
-
diff --git a/workspace/src/common/launch/art_control_launch/launch/art_control.launch.py b/workspace/src/common/launch/art_control_launch/launch/art_control.launch.py
index 668bbed1..1c372783 100644
--- a/workspace/src/common/launch/art_control_launch/launch/art_control.launch.py
+++ b/workspace/src/common/launch/art_control_launch/launch/art_control.launch.py
@@ -46,7 +46,9 @@ def generate_launch_description():
 
     AddLaunchArgument(ld, "art_control/input/path", "/path_planning/path")
     AddLaunchArgument(ld, "art_control/input/vehicle_state", "/vehicle/state")
-    AddLaunchArgument(ld, "art_control/output/vehicle_inputs", "/control/vehicle_inputs")
+    AddLaunchArgument(
+        ld, "art_control/output/vehicle_inputs", "/control/vehicle_inputs"
+    )
 
     AddLaunchArgument(ld, "control_mode", "PID")
     AddLaunchArgument(ld, "control_file", "data/smallest_radius_right.csv")
@@ -59,23 +61,29 @@ def generate_launch_description():
     # -----
 
     node = Node(
-            package='control',
-            executable='pid',
-            name='pid',
-            remappings=[
-                ("~/input/path", GetLaunchArgument("art_control/input/path")),
-                ("~/input/vehicle_state", GetLaunchArgument("art_control/input/vehicle_state")),
-                ("~/output/vehicle_inputs", GetLaunchArgument("art_control/output/vehicle_inputs"))
-            ],
-            parameters=[
-                {"input": GetLaunchArgument("art_control/input/path")},
-                {"control_mode": GetLaunchArgument("control_mode")},
-                {"control_file": GetLaunchArgument("control_file")},
-                {"steering_gain": GetLaunchArgument("steering_gain")},
-                {"throttle_gain": GetLaunchArgument("throttle_gain")},
-                {"use_sim_time": GetLaunchArgument("use_sim_time")}
-            ]
-        )
+        package="control",
+        executable="pid",
+        name="pid",
+        remappings=[
+            ("~/input/path", GetLaunchArgument("art_control/input/path")),
+            (
+                "~/input/vehicle_state",
+                GetLaunchArgument("art_control/input/vehicle_state"),
+            ),
+            (
+                "~/output/vehicle_inputs",
+                GetLaunchArgument("art_control/output/vehicle_inputs"),
+            ),
+        ],
+        parameters=[
+            {"input": GetLaunchArgument("art_control/input/path")},
+            {"control_mode": GetLaunchArgument("control_mode")},
+            {"control_file": GetLaunchArgument("control_file")},
+            {"steering_gain": GetLaunchArgument("steering_gain")},
+            {"throttle_gain": GetLaunchArgument("throttle_gain")},
+            {"use_sim_time": GetLaunchArgument("use_sim_time")},
+        ],
+    )
     ld.add_action(node)
 
-    return ld 
+    return ld
diff --git a/workspace/src/common/launch/art_launch/CMakeLists.txt b/workspace/src/common/launch/art_launch/CMakeLists.txt
index 2c1abff0..362188df 100644
--- a/workspace/src/common/launch/art_launch/CMakeLists.txt
+++ b/workspace/src/common/launch/art_launch/CMakeLists.txt
@@ -13,4 +13,3 @@ ament_auto_package(
   INSTALL_TO_SHARE
   launch
 )
-
diff --git a/workspace/src/common/launch/art_launch/launch/art.launch.py b/workspace/src/common/launch/art_launch/launch/art.launch.py
index 18a3b0c6..c74061fa 100644
--- a/workspace/src/common/launch/art_launch/launch/art.launch.py
+++ b/workspace/src/common/launch/art_launch/launch/art.launch.py
@@ -36,7 +36,13 @@
 from launch_ros.actions import ComposableNodeContainer
 
 # internal imports
-from launch_utils import IncludeLaunchDescriptionWithCondition, GetLaunchArgument, AddLaunchArgument, SetLaunchArgument
+from launch_utils import (
+    IncludeLaunchDescriptionWithCondition,
+    GetLaunchArgument,
+    AddLaunchArgument,
+    SetLaunchArgument,
+)
+
 
 def generate_launch_description():
     ld = LaunchDescription()
@@ -47,14 +53,31 @@ def generate_launch_description():
 
     AddLaunchArgument(ld, "use_sim", "False")
     AddLaunchArgument(ld, "use_sim_time", "False")
-    SetLaunchArgument(ld, "use_sim_time", "True", condition=IfCondition(GetLaunchArgument("use_sim")))
-   
-    AddLaunchArgument(ld, "container", "") 
-    container_name = AddLaunchArgument(ld, "container_name", "art_container") 
+    SetLaunchArgument(
+        ld, "use_sim_time", "True", condition=IfCondition(GetLaunchArgument("use_sim"))
+    )
 
-    SetLaunchArgument(ld, "disable_art_sensing", "True", condition=IfCondition(GetLaunchArgument("use_sim")))
-    SetLaunchArgument(ld, "disable_art_vehicle", "True", condition=IfCondition(GetLaunchArgument("use_sim")))
-    SetLaunchArgument(ld, "disable_art_simulation", "True", condition=UnlessCondition(GetLaunchArgument("use_sim")))
+    AddLaunchArgument(ld, "container", "")
+    container_name = AddLaunchArgument(ld, "container_name", "art_container")
+
+    SetLaunchArgument(
+        ld,
+        "disable_art_sensing",
+        "True",
+        condition=IfCondition(GetLaunchArgument("use_sim")),
+    )
+    SetLaunchArgument(
+        ld,
+        "disable_art_vehicle",
+        "True",
+        condition=IfCondition(GetLaunchArgument("use_sim")),
+    )
+    SetLaunchArgument(
+        ld,
+        "disable_art_simulation",
+        "True",
+        condition=UnlessCondition(GetLaunchArgument("use_sim")),
+    )
 
     # -------------
     # Composability
@@ -67,9 +90,7 @@ def generate_launch_description():
     # If a container name is not provided,
     # set the name of the container launched above for image_proc nodes
     set_container_name = SetLaunchConfiguration(
-        condition=use_composability,
-        name='container',
-        value=container_name
+        condition=use_composability, name="container", value=container_name
     )
     ld.add_action(set_container_name)
 
@@ -89,11 +110,13 @@ def generate_launch_description():
     # ---------------
 
     IncludeLaunchDescriptionWithCondition(ld, "art_perception_launch", "art_perception")
-    IncludeLaunchDescriptionWithCondition(ld, "art_localization_launch", "art_localization")
+    IncludeLaunchDescriptionWithCondition(
+        ld, "art_localization_launch", "art_localization"
+    )
     IncludeLaunchDescriptionWithCondition(ld, "art_planning_launch", "art_planning")
     IncludeLaunchDescriptionWithCondition(ld, "art_control_launch", "art_control")
     IncludeLaunchDescriptionWithCondition(ld, "art_sensing_launch", "art_sensing")
     IncludeLaunchDescriptionWithCondition(ld, "art_vehicle_launch", "art_vehicle")
     IncludeLaunchDescriptionWithCondition(ld, "art_simulation_launch", "art_simulation")
 
-    return ld 
+    return ld
diff --git a/workspace/src/common/launch/art_localization_launch/CMakeLists.txt b/workspace/src/common/launch/art_localization_launch/CMakeLists.txt
index cd319a70..72754a7b 100644
--- a/workspace/src/common/launch/art_localization_launch/CMakeLists.txt
+++ b/workspace/src/common/launch/art_localization_launch/CMakeLists.txt
@@ -14,4 +14,3 @@ ament_auto_package(
   launch
   config
 )
-
diff --git a/workspace/src/common/launch/art_localization_launch/config/4DOF_dynamics.yml b/workspace/src/common/launch/art_localization_launch/config/4DOF_dynamics.yml
index 70756711..10f29a14 100644
--- a/workspace/src/common/launch/art_localization_launch/config/4DOF_dynamics.yml
+++ b/workspace/src/common/launch/art_localization_launch/config/4DOF_dynamics.yml
@@ -7,4 +7,4 @@
       i_wheel: .001
       gamma: 0.33333333
       tau_0: 0.3
-      omega_0: 30.0
\ No newline at end of file
+      omega_0: 30.0
diff --git a/workspace/src/common/launch/art_localization_launch/config/EKF_param.yml b/workspace/src/common/launch/art_localization_launch/config/EKF_param.yml
index 8ee5c0fd..c0de48d8 100644
--- a/workspace/src/common/launch/art_localization_launch/config/EKF_param.yml
+++ b/workspace/src/common/launch/art_localization_launch/config/EKF_param.yml
@@ -4,4 +4,4 @@
       Q3: 3
       Q4: 0.1
       R1: 0.0
-      R3: 0.3
\ No newline at end of file
+      R3: 0.3
diff --git a/workspace/src/common/launch/art_localization_launch/launch/art_localization.launch.py b/workspace/src/common/launch/art_localization_launch/launch/art_localization.launch.py
index e6d3c76e..6017d2e2 100644
--- a/workspace/src/common/launch/art_localization_launch/launch/art_localization.launch.py
+++ b/workspace/src/common/launch/art_localization_launch/launch/art_localization.launch.py
@@ -47,38 +47,72 @@ def generate_launch_description():
     # ----------------
 
     AddLaunchArgument(ld, "art_localization/input/gps", "/sensing/gps/data")
-    AddLaunchArgument(ld, "art_localization/input/magnetometer", "/sensing/magnetometer/data")
+    AddLaunchArgument(
+        ld, "art_localization/input/magnetometer", "/sensing/magnetometer/data"
+    )
     AddLaunchArgument(ld, "art_localization/input/gyroscope", "/sensing/gyroscope/data")
-    AddLaunchArgument(ld, "art_localization/input/accelerometer", "/sensing/accelerometer/data")
-    AddLaunchArgument(ld, "art_localization/input/vehicle_inputs", "/control/vehicle_inputs")
+    AddLaunchArgument(
+        ld, "art_localization/input/accelerometer", "/sensing/accelerometer/data"
+    )
+    AddLaunchArgument(
+        ld, "art_localization/input/vehicle_inputs", "/control/vehicle_inputs"
+    )
     AddLaunchArgument(ld, "art_localization/input/ground_truth", "/vehicle/state")
-    AddLaunchArgument(ld, "art_localization/output/filtered_state", "/vehicle/filtered_state")
+    AddLaunchArgument(
+        ld, "art_localization/output/filtered_state", "/vehicle/filtered_state"
+    )
 
-    AddLaunchArgument(ld, "estimation_alg", EstimationAlgorithmOption.GROUND_TRUTH.value, choices=[o.value for o in EstimationAlgorithmOption.__members__.values()])
+    AddLaunchArgument(
+        ld,
+        "estimation_alg",
+        EstimationAlgorithmOption.GROUND_TRUTH.value,
+        choices=[o.value for o in EstimationAlgorithmOption.__members__.values()],
+    )
 
     # -----
     # Nodes
     # -----
 
     node = Node(
-        package='localization_py',
-        executable='state_estimation',
-        name='state_estimation',
+        package="localization_py",
+        executable="state_estimation",
+        name="state_estimation",
         remappings=[
-                ("~/input/gps", GetLaunchArgument("art_localization/input/gps")),
-                ("~/input/magnetometer", GetLaunchArgument("art_localization/input/magnetometer")),
-                ("~/input/gyroscope", GetLaunchArgument("art_localization/input/gyroscope")),
-                ("~/input/accelerometer", GetLaunchArgument("art_localization/input/accelerometer")),
-                ("~/input/vehicle_inputs", GetLaunchArgument("art_localization/input/vehicle_inputs")),
-                ("~/input/ground_truth", GetLaunchArgument("art_localization/input/ground_truth")),
-                ("~/output/filtered_state", GetLaunchArgument("art_localization/output/filtered_state")),
+            ("~/input/gps", GetLaunchArgument("art_localization/input/gps")),
+            (
+                "~/input/magnetometer",
+                GetLaunchArgument("art_localization/input/magnetometer"),
+            ),
+            (
+                "~/input/gyroscope",
+                GetLaunchArgument("art_localization/input/gyroscope"),
+            ),
+            (
+                "~/input/accelerometer",
+                GetLaunchArgument("art_localization/input/accelerometer"),
+            ),
+            (
+                "~/input/vehicle_inputs",
+                GetLaunchArgument("art_localization/input/vehicle_inputs"),
+            ),
+            (
+                "~/input/ground_truth",
+                GetLaunchArgument("art_localization/input/ground_truth"),
+            ),
+            (
+                "~/output/filtered_state",
+                GetLaunchArgument("art_localization/output/filtered_state"),
+            ),
         ],
-        parameters=[GetPackageSharePath("art_localization_launch", "config", "4DOF_dynamics.yml"),
+        parameters=[
+            GetPackageSharePath(
+                "art_localization_launch", "config", "4DOF_dynamics.yml"
+            ),
             GetPackageSharePath("art_localization_launch", "config", "EKF_param.yml"),
             {"use_sim_time": GetLaunchArgument("use_sim_time")},
-            {"estimation_alg": GetLaunchArgument("estimation_alg")}
-        ]
+            {"estimation_alg": GetLaunchArgument("estimation_alg")},
+        ],
     )
     ld.add_action(node)
 
-    return ld 
+    return ld
diff --git a/workspace/src/common/launch/art_perception_launch/CMakeLists.txt b/workspace/src/common/launch/art_perception_launch/CMakeLists.txt
index e24fe83c..eb797ff4 100644
--- a/workspace/src/common/launch/art_perception_launch/CMakeLists.txt
+++ b/workspace/src/common/launch/art_perception_launch/CMakeLists.txt
@@ -13,4 +13,3 @@ ament_auto_package(
   INSTALL_TO_SHARE
   launch
 )
-
diff --git a/workspace/src/common/launch/art_perception_launch/launch/art_perception.launch.py b/workspace/src/common/launch/art_perception_launch/launch/art_perception.launch.py
index 2d25422c..a5f32730 100644
--- a/workspace/src/common/launch/art_perception_launch/launch/art_perception.launch.py
+++ b/workspace/src/common/launch/art_perception_launch/launch/art_perception.launch.py
@@ -6,7 +6,6 @@
 
 
 def generate_launch_description():
-
     ld = LaunchDescription()
 
     # ---------------
@@ -15,4 +14,4 @@ def generate_launch_description():
 
     IncludeLaunchDescriptionWithCondition(ld, "art_perception_launch", "cone_detector")
 
-    return ld 
+    return ld
diff --git a/workspace/src/common/launch/art_perception_launch/launch/cone_detector.launch.py b/workspace/src/common/launch/art_perception_launch/launch/cone_detector.launch.py
index 26b4b4cf..4cf238c9 100644
--- a/workspace/src/common/launch/art_perception_launch/launch/cone_detector.launch.py
+++ b/workspace/src/common/launch/art_perception_launch/launch/cone_detector.launch.py
@@ -44,11 +44,15 @@ def generate_launch_description():
     # Launch Arguments
     # ----------------
 
-    AddLaunchArgument(ld, "cone_detector/input/image", "/sensing/front_facing_camera/raw")
+    AddLaunchArgument(
+        ld, "cone_detector/input/image", "/sensing/front_facing_camera/raw"
+    )
     AddLaunchArgument(ld, "cone_detector/input/vehicle_state", "/vehicle/state")
     AddLaunchArgument(ld, "cone_detector/output/objects", "/perception/objects")
 
-    node = AddLaunchArgument(ld, "node", "yolov5_detector", choices=("yolov5_detector", "object_recognition"))
+    node = AddLaunchArgument(
+        ld, "node", "yolov5_detector", choices=("yolov5_detector", "object_recognition")
+    )
     AddLaunchArgument(ld, "model", "data/real.onnx")
     AddLaunchArgument(ld, "camera_calibration_file", "data/calibration.json")
     AddLaunchArgument(ld, "vis", "False")
@@ -63,16 +67,19 @@ def generate_launch_description():
         name=node,
         remappings=[
             ("~/input/image", GetLaunchArgument("cone_detector/input/image")),
-            ("~/input/vehicle_state", GetLaunchArgument("cone_detector/input/vehicle_state")),
-            ("~/output/objects", GetLaunchArgument("cone_detector/output/objects"))
+            (
+                "~/input/vehicle_state",
+                GetLaunchArgument("cone_detector/input/vehicle_state"),
+            ),
+            ("~/output/objects", GetLaunchArgument("cone_detector/output/objects")),
         ],
         parameters=[
-             {"model": GetLaunchArgument("model")},
-             {"camera_calibration_file": GetLaunchArgument("camera_calibration_file")},
-             {"vis": GetLaunchArgument("vis")},
-             {"use_sim_time": GetLaunchArgument("use_sim_time")}
-        ]
+            {"model": GetLaunchArgument("model")},
+            {"camera_calibration_file": GetLaunchArgument("camera_calibration_file")},
+            {"vis": GetLaunchArgument("vis")},
+            {"use_sim_time": GetLaunchArgument("use_sim_time")},
+        ],
     )
     ld.add_action(node)
 
-    return ld 
+    return ld
diff --git a/workspace/src/common/launch/art_planning_launch/CMakeLists.txt b/workspace/src/common/launch/art_planning_launch/CMakeLists.txt
index 70d85ad7..952a6690 100644
--- a/workspace/src/common/launch/art_planning_launch/CMakeLists.txt
+++ b/workspace/src/common/launch/art_planning_launch/CMakeLists.txt
@@ -13,4 +13,3 @@ ament_auto_package(
   INSTALL_TO_SHARE
   launch
 )
-
diff --git a/workspace/src/common/launch/art_planning_launch/launch/art_planning.launch.py b/workspace/src/common/launch/art_planning_launch/launch/art_planning.launch.py
index 2dd050f7..e01b6a17 100644
--- a/workspace/src/common/launch/art_planning_launch/launch/art_planning.launch.py
+++ b/workspace/src/common/launch/art_planning_launch/launch/art_planning.launch.py
@@ -36,6 +36,7 @@
 # internal imports
 from launch_utils import AddLaunchArgument, GetLaunchArgument
 
+
 def generate_launch_description():
     ld = LaunchDescription()
 
@@ -55,20 +56,23 @@ def generate_launch_description():
     # -----
 
     node = Node(
-            package='path_planning',
-            executable='path_planning',
-            name='path_planning',
-            remappings=[
-                ("~/input/objects", GetLaunchArgument("art_planning/input/objects")),
-                ("~/input/vehicle_state", GetLaunchArgument("art_planning/input/vehicle_state")),
-                ("~/output/path", GetLaunchArgument("art_planning/output/path"))
-            ],
-            parameters=[
-                {"vis": GetLaunchArgument("vis")},
-                {"lookahead": GetLaunchArgument("lookahead")},
-                {"use_sim_time": GetLaunchArgument("use_sim_time")}
-            ]
-        )
+        package="path_planning",
+        executable="path_planning",
+        name="path_planning",
+        remappings=[
+            ("~/input/objects", GetLaunchArgument("art_planning/input/objects")),
+            (
+                "~/input/vehicle_state",
+                GetLaunchArgument("art_planning/input/vehicle_state"),
+            ),
+            ("~/output/path", GetLaunchArgument("art_planning/output/path")),
+        ],
+        parameters=[
+            {"vis": GetLaunchArgument("vis")},
+            {"lookahead": GetLaunchArgument("lookahead")},
+            {"use_sim_time": GetLaunchArgument("use_sim_time")},
+        ],
+    )
     ld.add_action(node)
 
-    return ld 
+    return ld
diff --git a/workspace/src/common/launch/art_sensing_launch/CMakeLists.txt b/workspace/src/common/launch/art_sensing_launch/CMakeLists.txt
index 11cc35bc..13c2a0fa 100644
--- a/workspace/src/common/launch/art_sensing_launch/CMakeLists.txt
+++ b/workspace/src/common/launch/art_sensing_launch/CMakeLists.txt
@@ -14,4 +14,3 @@ ament_auto_package(
   launch
   config
 )
-
diff --git a/workspace/src/common/launch/art_sensing_launch/launch/art_sensing.launch.py b/workspace/src/common/launch/art_sensing_launch/launch/art_sensing.launch.py
index b2f47c10..3c28f633 100644
--- a/workspace/src/common/launch/art_sensing_launch/launch/art_sensing.launch.py
+++ b/workspace/src/common/launch/art_sensing_launch/launch/art_sensing.launch.py
@@ -6,7 +6,6 @@
 
 
 def generate_launch_description():
-
     ld = LaunchDescription()
 
     # ---------------
@@ -16,4 +15,4 @@ def generate_launch_description():
     IncludeLaunchDescriptionWithCondition(ld, "art_sensing_launch", "usb_cam")
     IncludeLaunchDescriptionWithCondition(ld, "art_sensing_launch", "xsens")
 
-    return ld 
+    return ld
diff --git a/workspace/src/common/launch/art_sensing_launch/launch/usb_cam.launch.py b/workspace/src/common/launch/art_sensing_launch/launch/usb_cam.launch.py
index 655d593c..89355f41 100644
--- a/workspace/src/common/launch/art_sensing_launch/launch/usb_cam.launch.py
+++ b/workspace/src/common/launch/art_sensing_launch/launch/usb_cam.launch.py
@@ -36,7 +36,12 @@
 from launch import LaunchDescription
 
 # internal imports
-from launch_utils import AddLaunchArgument, GetLaunchArgument, AddComposableNode, GetPackageSharePath
+from launch_utils import (
+    AddLaunchArgument,
+    GetLaunchArgument,
+    AddComposableNode,
+    GetPackageSharePath,
+)
 
 
 def generate_launch_description():
@@ -53,18 +58,16 @@ def generate_launch_description():
     # -----
 
     AddComposableNode(
-       ld,
-       plugin="usb_cam::UsbCamNode",
-       package="usb_cam",
-       executable="usb_cam_node_exe",
-       name="usb_cam",
-       parameters=[
-         GetPackageSharePath("art_sensing_launch", "config", "usb_cam.params.yaml"),
-         {"use_sim_time": GetLaunchArgument("use_sim_time")}
-      ],
-       remappings=[
-        ("~/output/image_raw", GetLaunchArgument("usb_cam/output/image"))
-       ]
+        ld,
+        plugin="usb_cam::UsbCamNode",
+        package="usb_cam",
+        executable="usb_cam_node_exe",
+        name="usb_cam",
+        parameters=[
+            GetPackageSharePath("art_sensing_launch", "config", "usb_cam.params.yaml"),
+            {"use_sim_time": GetLaunchArgument("use_sim_time")},
+        ],
+        remappings=[("~/output/image_raw", GetLaunchArgument("usb_cam/output/image"))],
     )
 
     return ld
diff --git a/workspace/src/common/launch/art_sensing_launch/launch/xsens.launch.py b/workspace/src/common/launch/art_sensing_launch/launch/xsens.launch.py
index cd911a5b..12c6db3a 100644
--- a/workspace/src/common/launch/art_sensing_launch/launch/xsens.launch.py
+++ b/workspace/src/common/launch/art_sensing_launch/launch/xsens.launch.py
@@ -43,6 +43,8 @@ def generate_launch_description():
     # Launch Includes
     # ---------------
 
-    IncludeLaunchDescriptionWithCondition(ld, "bluespace_ai_xsens_mti_driver", "xsens_mti_node")
+    IncludeLaunchDescriptionWithCondition(
+        ld, "bluespace_ai_xsens_mti_driver", "xsens_mti_node"
+    )
 
     return ld
diff --git a/workspace/src/common/launch/art_simulation_launch/CMakeLists.txt b/workspace/src/common/launch/art_simulation_launch/CMakeLists.txt
index cb3ea4f5..646b21a5 100644
--- a/workspace/src/common/launch/art_simulation_launch/CMakeLists.txt
+++ b/workspace/src/common/launch/art_simulation_launch/CMakeLists.txt
@@ -13,4 +13,3 @@ ament_auto_package(
   INSTALL_TO_SHARE
   launch
 )
-
diff --git a/workspace/src/common/launch/art_simulation_launch/launch/art_simulation.launch.py b/workspace/src/common/launch/art_simulation_launch/launch/art_simulation.launch.py
index ccf11065..e197a65f 100644
--- a/workspace/src/common/launch/art_simulation_launch/launch/art_simulation.launch.py
+++ b/workspace/src/common/launch/art_simulation_launch/launch/art_simulation.launch.py
@@ -38,7 +38,7 @@
 from launch.conditions import IfCondition
 
 # internal imports
-from launch_utils import AddLaunchArgument, GetLaunchArgument 
+from launch_utils import AddLaunchArgument, GetLaunchArgument
 
 
 def generate_launch_description():
@@ -48,10 +48,14 @@ def generate_launch_description():
     # Launch Arguments
     # ----------------
 
-    AddLaunchArgument(ld, "art_simulation/input/vehicle_inputs", "/control/vehicle_inputs")
+    AddLaunchArgument(
+        ld, "art_simulation/input/vehicle_inputs", "/control/vehicle_inputs"
+    )
     AddLaunchArgument(ld, "art_simulation/output/time", "/clock")
     AddLaunchArgument(ld, "art_simulation/output/vehicle", "/vehicle/state")
-    AddLaunchArgument(ld, "art_simulation/output/camera", "/sensing/front_facing_camera/raw")
+    AddLaunchArgument(
+        ld, "art_simulation/output/camera", "/sensing/front_facing_camera/raw"
+    )
     AddLaunchArgument(ld, "ip", "")
     AddLaunchArgument(ld, "hostname", "")
 
@@ -62,23 +66,28 @@ def generate_launch_description():
     # -----
 
     node = Node(
-        package='chrono_ros_bridge',
-        executable='chrono_ros_bridge_node',
-        name='chrono_ros_bridge',
+        package="chrono_ros_bridge",
+        executable="chrono_ros_bridge_node",
+        name="chrono_ros_bridge",
         remappings=[
-            ("~/input/driver_inputs", GetLaunchArgument("art_simulation/input/vehicle_inputs")),
+            (
+                "~/input/driver_inputs",
+                GetLaunchArgument("art_simulation/input/vehicle_inputs"),
+            ),
             ("~/output/time", GetLaunchArgument("art_simulation/output/time")),
             ("~/output/vehicle", GetLaunchArgument("art_simulation/output/vehicle")),
-            ("~/output/camera/front_facing_camera", GetLaunchArgument("art_simulation/output/camera")),
+            (
+                "~/output/camera/front_facing_camera",
+                GetLaunchArgument("art_simulation/output/camera"),
+            ),
         ],
         parameters=[
-             {"ip": GetLaunchArgument("ip")},
-             {"hostname": GetLaunchArgument("hostname")},
-             {"use_sim_time": GetLaunchArgument("use_sim_time")}
+            {"ip": GetLaunchArgument("ip")},
+            {"hostname": GetLaunchArgument("hostname")},
+            {"use_sim_time": GetLaunchArgument("use_sim_time")},
         ],
-        condition=IfCondition(GetLaunchArgument("use_sim"))
+        condition=IfCondition(GetLaunchArgument("use_sim")),
     )
     ld.add_action(node)
 
     return ld
-
diff --git a/workspace/src/common/launch/art_vehicle_launch/CMakeLists.txt b/workspace/src/common/launch/art_vehicle_launch/CMakeLists.txt
index 838a3708..bb9399a3 100644
--- a/workspace/src/common/launch/art_vehicle_launch/CMakeLists.txt
+++ b/workspace/src/common/launch/art_vehicle_launch/CMakeLists.txt
@@ -13,4 +13,3 @@ ament_auto_package(
   INSTALL_TO_SHARE
   launch
 )
-
diff --git a/workspace/src/common/launch/art_vehicle_launch/launch/arduino_driver.launch.py b/workspace/src/common/launch/art_vehicle_launch/launch/arduino_driver.launch.py
index 78d21b6f..d0202952 100644
--- a/workspace/src/common/launch/art_vehicle_launch/launch/arduino_driver.launch.py
+++ b/workspace/src/common/launch/art_vehicle_launch/launch/arduino_driver.launch.py
@@ -44,20 +44,25 @@ def generate_launch_description():
     # Launch Arguments
     # ----------------
 
-    AddLaunchArgument(ld, "arduino_driver/input/vehicle_inputs", "/control/vehicle_inputs")
+    AddLaunchArgument(
+        ld, "arduino_driver/input/vehicle_inputs", "/control/vehicle_inputs"
+    )
 
     # -----
     # Nodes
     # -----
 
     node = Node(
-            package='arduino_driver',
-            executable='motor_driver',
-            name='motor_driver',
-            remappings=[
-                ("~/output/vehicle_inputs", GetLaunchArgument("arduino_driver/input/vehicle_inputs"))
-            ],
-        )
+        package="arduino_driver",
+        executable="motor_driver",
+        name="motor_driver",
+        remappings=[
+            (
+                "~/output/vehicle_inputs",
+                GetLaunchArgument("arduino_driver/input/vehicle_inputs"),
+            )
+        ],
+    )
     ld.add_action(node)
 
     return ld
diff --git a/workspace/src/common/launch/art_vehicle_launch/launch/art_vehicle.launch.py b/workspace/src/common/launch/art_vehicle_launch/launch/art_vehicle.launch.py
index 1afda4e0..122588ea 100644
--- a/workspace/src/common/launch/art_vehicle_launch/launch/art_vehicle.launch.py
+++ b/workspace/src/common/launch/art_vehicle_launch/launch/art_vehicle.launch.py
@@ -6,7 +6,6 @@
 
 
 def generate_launch_description():
-
     ld = LaunchDescription()
 
     # ---------------
@@ -15,4 +14,4 @@ def generate_launch_description():
 
     IncludeLaunchDescriptionWithCondition(ld, "art_vehicle_launch", "arduino_driver")
 
-    return ld 
+    return ld
diff --git a/workspace/src/common/launch/launch_utils/launch_utils/__init__.py b/workspace/src/common/launch/launch_utils/launch_utils/__init__.py
index ac8d1241..b025b39d 100644
--- a/workspace/src/common/launch/launch_utils/launch_utils/__init__.py
+++ b/workspace/src/common/launch/launch_utils/launch_utils/__init__.py
@@ -1,19 +1,27 @@
 """Main entry point for the `launch_utils` package"""
 
-from .utilities import AddLaunchArgument, SetLaunchArgument, GetLaunchArgument, AddComposableNode, GetPackageSourceDirectory, IncludeLaunchDescriptionWithCondition, GetPackageSharePath
+from .utilities import (
+    AddLaunchArgument,
+    SetLaunchArgument,
+    GetLaunchArgument,
+    AddComposableNode,
+    GetPackageSourceDirectory,
+    IncludeLaunchDescriptionWithCondition,
+    GetPackageSharePath,
+)
 
 from . import conditions
 from . import substitutions
 
 __all__ = [
-    'AddLaunchArgument',
-    'SetLaunchArgument',
-    'GetLaunchArgument',
-    'AddComposableNode',
-    'GetPackageSourceDirectory',
-    'IncludeLaunchDescriptionWithCondition',
-    'GetPackageSharePath',
+    "AddLaunchArgument",
+    "SetLaunchArgument",
+    "GetLaunchArgument",
+    "AddComposableNode",
+    "GetPackageSourceDirectory",
+    "IncludeLaunchDescriptionWithCondition",
+    "GetPackageSharePath",
     # Additional modules
-    'conditions',
-    'substitutions',
+    "conditions",
+    "substitutions",
 ]
diff --git a/workspace/src/common/launch/launch_utils/launch_utils/conditions/__init__.py b/workspace/src/common/launch/launch_utils/launch_utils/conditions/__init__.py
index e6f1fee8..acc36e2c 100644
--- a/workspace/src/common/launch/launch_utils/launch_utils/conditions/__init__.py
+++ b/workspace/src/common/launch/launch_utils/launch_utils/conditions/__init__.py
@@ -1,5 +1,5 @@
 from .multiple_if_conditions import MultipleIfConditions
 
 __all__ = [
-    'MultipleIfConditions',
+    "MultipleIfConditions",
 ]
diff --git a/workspace/src/common/launch/launch_utils/launch_utils/conditions/multiple_if_conditions.py b/workspace/src/common/launch/launch_utils/launch_utils/conditions/multiple_if_conditions.py
index 2a17db05..ef7ac213 100644
--- a/workspace/src/common/launch/launch_utils/launch_utils/conditions/multiple_if_conditions.py
+++ b/workspace/src/common/launch/launch_utils/launch_utils/conditions/multiple_if_conditions.py
@@ -22,15 +22,24 @@ class MultipleIfConditions(Condition):
     the sum of the evaluate predicates is >= ``num``. If -1, all must be true.
     """
 
-    def __init__(self, predicate_expressions: Iterable[SomeSubstitutionsType], *, num: int = -1) -> None:
-        self.__predicate_expressions = [normalize_to_list_of_substitutions(
-            expr) for expr in predicate_expressions]
+    def __init__(
+        self, predicate_expressions: Iterable[SomeSubstitutionsType], *, num: int = -1
+    ) -> None:
+        self.__predicate_expressions = [
+            normalize_to_list_of_substitutions(expr) for expr in predicate_expressions
+        ]
         super().__init__(predicate=self._predicate_func)
 
         self.__num = num if num != -1 else len(predicate_expressions)
 
     def _predicate_func(self, context: LaunchContext) -> bool:
-        return sum(evaluate_condition_expression(context, expr) for expr in self.__predicate_expressions) >= self.__num
+        return (
+            sum(
+                evaluate_condition_expression(context, expr)
+                for expr in self.__predicate_expressions
+            )
+            >= self.__num
+        )
 
     def describe(self) -> Text:
         """Return a description of this Condition."""
diff --git a/workspace/src/common/launch/launch_utils/launch_utils/substitutions/__init__.py b/workspace/src/common/launch/launch_utils/launch_utils/substitutions/__init__.py
index aeeb5120..867017f5 100644
--- a/workspace/src/common/launch/launch_utils/launch_utils/substitutions/__init__.py
+++ b/workspace/src/common/launch/launch_utils/launch_utils/substitutions/__init__.py
@@ -1,5 +1,5 @@
 from .quote_wrapped_python_expression import QuoteWrappedPythonExpression
 
 __all__ = [
-    'QuoteWrappedPythonExpression',
+    "QuoteWrappedPythonExpression",
 ]
diff --git a/workspace/src/common/launch/launch_utils/launch_utils/substitutions/quote_wrapped_python_expression.py b/workspace/src/common/launch/launch_utils/launch_utils/substitutions/quote_wrapped_python_expression.py
index 0dbbafb9..5b06b9e4 100644
--- a/workspace/src/common/launch/launch_utils/launch_utils/substitutions/quote_wrapped_python_expression.py
+++ b/workspace/src/common/launch/launch_utils/launch_utils/substitutions/quote_wrapped_python_expression.py
@@ -7,7 +7,7 @@ class QuoteWrappedPythonExpression(PythonExpression):
     """Helper class that fixes a weird flaw in PythonExpression perform method where
     substituted LaunchConfiguration variables need to be wrapped in quotes."""
 
-    def __init__(self, expression: 'SomeSubstitutionsType') -> None:
+    def __init__(self, expression: "SomeSubstitutionsType") -> None:
         super().__init__(expression)
 
         _expression = []
@@ -20,4 +20,4 @@ def __init__(self, expression: 'SomeSubstitutionsType') -> None:
                 _expression.append(expr)
 
         # HACK
-        vars(self)['_PythonExpression__expression'] = _expression
+        vars(self)["_PythonExpression__expression"] = _expression
diff --git a/workspace/src/common/launch/launch_utils/launch_utils/utilities.py b/workspace/src/common/launch/launch_utils/launch_utils/utilities.py
index 385c5a9f..08d4022a 100644
--- a/workspace/src/common/launch/launch_utils/launch_utils/utilities.py
+++ b/workspace/src/common/launch/launch_utils/launch_utils/utilities.py
@@ -2,7 +2,8 @@
 
 
 def AddLaunchArgument(
-        ld: LaunchDescription, arg: "Any", default: "Any", **kwargs) -> "LaunchConfiguration":
+    ld: LaunchDescription, arg: "Any", default: "Any", **kwargs
+) -> "LaunchConfiguration":
     """Helper method to add a launch argument to a LaunchDescription"""
     from launch.actions import DeclareLaunchArgument
 
@@ -10,7 +11,9 @@ def AddLaunchArgument(
     return GetLaunchArgument(arg)
 
 
-def SetLaunchArgument(ld: LaunchDescription, arg: "Any", value: "Any",  **kwargs) -> None:
+def SetLaunchArgument(
+    ld: LaunchDescription, arg: "Any", value: "Any", **kwargs
+) -> None:
     """Helper method to set a launch argument of a LaunchDescription"""
     from launch.actions import SetLaunchConfiguration
 
@@ -26,9 +29,9 @@ def GetLaunchArgument(name: str, **kwargs) -> "LaunchConfiguration":
 
 
 def GetPackageSharePath(package: str, *args, **kwargs) -> "pathlib.Path":
-    """Wrapper around ament_index_python.packages.get_package_share_directory that 
+    """Wrapper around ament_index_python.packages.get_package_share_directory that
     returns a pathlib.Path.
-    
+
     Args:
         package (str): the package passed to ``get_package_share_directory``.
     """
@@ -38,7 +41,7 @@ def GetPackageSharePath(package: str, *args, **kwargs) -> "pathlib.Path":
     return Path(get_package_share_directory(package), *args, **kwargs)
 
 
-def GetPackageSourceDirectory(package: str, base: str = None) -> 'pathlib.Path':
+def GetPackageSourceDirectory(package: str, base: str = None) -> "pathlib.Path":
     """Helper method to get the package **source** directory.
 
     Usually it's recommended to use ament_index_python.packages.get_package_share_directory,
@@ -64,7 +67,7 @@ def GetPackageSourceDirectory(package: str, base: str = None) -> 'pathlib.Path':
         base = os.environ.get("ROS_WORKSPACE_SRC", os.getcwd())
 
     for root, dirs, files in os.walk(base):
-        if package == os.path.basename(root) and 'package.xml' in files:
+        if package == os.path.basename(root) and "package.xml" in files:
             return Path(root)
 
     return None
@@ -82,7 +85,7 @@ def AddComposableNode(
     load_composable_nodes_kwargs: dict = {},
     node_kwargs: dict = {},
     add_as_regular_node: bool = True,
-    **kwargs
+    **kwargs,
 ) -> None:
     """
     Conditionally add a node dependent on whether the LaunchConfiguration ``container`` is
@@ -112,8 +115,7 @@ def AddComposableNode(
         )
     ]
     load_composable_nodes = LoadComposableNodes(
-        condition=MultipleIfConditions(
-            [composable_condition, *composable_conditions]),
+        condition=MultipleIfConditions([composable_condition, *composable_conditions]),
         composable_node_descriptions=composable_nodes,
         target_container=container,
         **load_composable_nodes_kwargs,
@@ -132,7 +134,9 @@ def AddComposableNode(
         ld.add_action(node)
 
 
-def IncludeLaunchDescriptionWithCondition(ld: LaunchDescription, share: str, package: str, *, default: str = "False", **kwargs):
+def IncludeLaunchDescriptionWithCondition(
+    ld: LaunchDescription, share: str, package: str, *, default: str = "False", **kwargs
+):
     """
     Conditionally include launch file.
 
@@ -157,16 +161,20 @@ def IncludeLaunchDescriptionWithCondition(ld: LaunchDescription, share: str, pac
 
     launch_argument = f"disable_{package}"
     disable_package = AddLaunchArgument(
-        ld, launch_argument, default, description=f"Disable the '{package}.launch.py' file from being included.")
+        ld,
+        launch_argument,
+        default,
+        description=f"Disable the '{package}.launch.py' file from being included.",
+    )
 
     package = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource([
-            PathJoinSubstitution([
-                FindPackageShare(share),
-                "launch",
-                f"{package}.launch.py"
-            ])
-        ]),
+        PythonLaunchDescriptionSource(
+            [
+                PathJoinSubstitution(
+                    [FindPackageShare(share), "launch", f"{package}.launch.py"]
+                )
+            ]
+        ),
         condition=UnlessCondition(disable_package),
     )
     ld.add_action(package)
diff --git a/workspace/src/common/launch/launch_utils/setup.py b/workspace/src/common/launch/launch_utils/setup.py
index 831e1c9e..1f5a8837 100644
--- a/workspace/src/common/launch/launch_utils/setup.py
+++ b/workspace/src/common/launch/launch_utils/setup.py
@@ -1,26 +1,24 @@
 from setuptools import setup
 from setuptools import find_packages
 
-package_name = 'launch_utils'
+package_name = "launch_utils"
 
 setup(
     name=package_name,
-    version='0.0.0',
-    packages=find_packages(exclude=['test']),
+    version="0.0.0",
+    packages=find_packages(exclude=["test"]),
     data_files=[
-        ('share/ament_index/resource_index/packages',
-            ['resource/' + package_name]),
-        ('share/' + package_name, ['package.xml']),
+        ("share/ament_index/resource_index/packages", ["resource/" + package_name]),
+        ("share/" + package_name, ["package.xml"]),
     ],
-    install_requires=['setuptools'],
+    install_requires=["setuptools"],
     zip_safe=True,
-    maintainer='todo',
-    maintainer_email='todo@todo.todo',
-    description='TODO: Package description',
-    license='TODO: License declaration',
-    tests_require=['pytest'],
+    maintainer="todo",
+    maintainer_email="todo@todo.todo",
+    description="TODO: Package description",
+    license="TODO: License declaration",
+    tests_require=["pytest"],
     entry_points={
-        'console_scripts': [
-        ],
+        "console_scripts": [],
     },
 )
diff --git a/workspace/src/control/control/control.py b/workspace/src/control/control/control.py
index 0fd8e0e6..5050bc30 100644
--- a/workspace/src/control/control/control.py
+++ b/workspace/src/control/control/control.py
@@ -40,9 +40,10 @@
 from rclpy.qos import QoSHistoryPolicy
 from rclpy.qos import QoSProfile
 
+
 class ControlNode(Node):
     def __init__(self):
-        super().__init__('control_node')
+        super().__init__("control_node")
 
         # DEFAULT SETTINGS
 
@@ -57,26 +58,36 @@ def __init__(self):
         self.t_start = self.get_clock().now().nanoseconds / 1e9
 
         # READ IN SHARE DIRECTORY LOCATION
-        package_share_directory = get_package_share_directory('control')
+        package_share_directory = get_package_share_directory("control")
 
         # ROS PARAMETERS
-        self.declare_parameter('control_mode', 'PID')
-        self.mode = self.get_parameter('control_mode').get_parameter_value().string_value
-        self.declare_parameter('control_file', "")
-        self.file = self.get_parameter('control_file').get_parameter_value().string_value
-
-        self.declare_parameter('steering_gain', 1.0)
-        self.steering_gain = self.get_parameter('steering_gain').get_parameter_value().double_value
-        self.declare_parameter('throttle_gain', 1.0)
-        self.throttle_gain = self.get_parameter('throttle_gain').get_parameter_value().double_value
-
-        self.use_sim_msg = self.get_parameter("use_sim_time").get_parameter_value().bool_value
-
-        if(self.file == ""):
+        self.declare_parameter("control_mode", "PID")
+        self.mode = (
+            self.get_parameter("control_mode").get_parameter_value().string_value
+        )
+        self.declare_parameter("control_file", "")
+        self.file = (
+            self.get_parameter("control_file").get_parameter_value().string_value
+        )
+
+        self.declare_parameter("steering_gain", 1.0)
+        self.steering_gain = (
+            self.get_parameter("steering_gain").get_parameter_value().double_value
+        )
+        self.declare_parameter("throttle_gain", 1.0)
+        self.throttle_gain = (
+            self.get_parameter("throttle_gain").get_parameter_value().double_value
+        )
+
+        self.use_sim_msg = (
+            self.get_parameter("use_sim_time").get_parameter_value().bool_value
+        )
+
+        if self.file == "":
             self.mode = "PID"
         else:
             file_path = os.path.join(package_share_directory, self.file)
-            self.recorded_inputs = np.loadtxt(file_path, delimiter=',')
+            self.recorded_inputs = np.loadtxt(file_path, delimiter=",")
 
         self.steering = 0.0
         self.throttle = 0.0
@@ -93,17 +104,20 @@ def __init__(self):
             from art_msgs.msg import VehicleInput
         self.vehicle_cmd = VehicleInput()
 
-        #waits for first path if using PID, otherwise runs right away
-        self.go = (self.mode == "File")
-        
+        # waits for first path if using PID, otherwise runs right away
+        self.go = self.mode == "File"
 
         # publishers and subscribers
         qos_profile = QoSProfile(depth=1)
         qos_profile.history = QoSHistoryPolicy.KEEP_LAST
-        self.sub_path = self.create_subscription(Path, '~/input/path', self.path_callback, qos_profile)
+        self.sub_path = self.create_subscription(
+            Path, "~/input/path", self.path_callback, qos_profile
+        )
         # self.sub_state = self.create_subscription(VehicleState, '~/input/vehicle_state', self.state_callback, qos_profile)
-        self.pub_vehicle_cmd = self.create_publisher(VehicleInput, '~/output/vehicle_inputs', 10)
-        self.timer = self.create_timer(1/self.freq, self.pub_callback)
+        self.pub_vehicle_cmd = self.create_publisher(
+            VehicleInput, "~/output/vehicle_inputs", 10
+        )
+        self.timer = self.create_timer(1 / self.freq, self.pub_callback)
 
     # function to process data this class subscribes to
     def state_callback(self, msg):
@@ -117,26 +131,29 @@ def path_callback(self, msg):
 
     # callback to run a loop and publish data this class generates
     def pub_callback(self):
-        if(not self.go):
+        if not self.go:
             return
 
         msg = VehicleInput()
 
-        if(self.mode == "File"):
+        if self.mode == "File":
             self.calc_inputs_from_file()
-        elif(self.mode == "PID" and len(self.path.poses)>0):
-            pt = [self.path.poses[0].pose.position.x,self.path.poses[0].pose.position.y]
-            
+        elif self.mode == "PID" and len(self.path.poses) > 0:
+            pt = [
+                self.path.poses[0].pose.position.x,
+                self.path.poses[0].pose.position.y,
+            ]
+
             ratio = pt[1] / pt[0]
             self.steering = self.steering_gain * ratio
             # self.get_logger().info('Target steering = %s' % self.steering)
 
-        #for circle
-        #self.steering = -0.5
+        # for circle
+        # self.steering = -0.5
 
-        #TODO: remove after debugging
+        # TODO: remove after debugging
         # self.steering = 0.0
-        self.throttle = self.throttle_gain*0.55 #only doing lateral conmtrol for now
+        self.throttle = self.throttle_gain * 0.55  # only doing lateral conmtrol for now
         # self.braking = 0.0
 
         msg.steering = np.clip(self.steering, -1, 1)
@@ -147,19 +164,25 @@ def pub_callback(self):
             msg.header.stamp = self.get_clock().now().to_msg()
 
         self.pub_vehicle_cmd.publish(msg)
-        
 
     def calc_inputs_from_file(self):
         t = self.get_clock().now().nanoseconds / 1e9 - self.t_start
 
-        self.throttle = np.interp(t,self.recorded_inputs[:,0],self.recorded_inputs[:,1])
-        self.braking = np.interp(t,self.recorded_inputs[:,0],self.recorded_inputs[:,2])
-        self.steering = np.interp(t,self.recorded_inputs[:,0],self.recorded_inputs[:,3])
+        self.throttle = np.interp(
+            t, self.recorded_inputs[:, 0], self.recorded_inputs[:, 1]
+        )
+        self.braking = np.interp(
+            t, self.recorded_inputs[:, 0], self.recorded_inputs[:, 2]
+        )
+        self.steering = np.interp(
+            t, self.recorded_inputs[:, 0], self.recorded_inputs[:, 3]
+        )
 
         # self.get_logger().info('Inputs %s' % self.recorded_inputs[0,:])
 
         # self.get_logger().info('Inputs from file: (t=%s, (%s,%s,%s)),' % (t,self.throttle,self.braking,self.steering))
 
+
 def main(args=None):
     rclpy.init(args=args)
     control = ControlNode()
@@ -169,5 +192,5 @@ def main(args=None):
     rclpy.shutdown()
 
 
-if __name__ == '__main__':
+if __name__ == "__main__":
     main()
diff --git a/workspace/src/control/data/throttle_threshold.csv b/workspace/src/control/data/throttle_threshold.csv
index 3c569ebe..e8fef9cf 100644
--- a/workspace/src/control/data/throttle_threshold.csv
+++ b/workspace/src/control/data/throttle_threshold.csv
@@ -1,2 +1,2 @@
 0,0,0,0
-100,1,0,0
\ No newline at end of file
+100,1,0,0
diff --git a/workspace/src/control/setup.py b/workspace/src/control/setup.py
index cc620f35..45f2be89 100644
--- a/workspace/src/control/setup.py
+++ b/workspace/src/control/setup.py
@@ -32,28 +32,25 @@
 import os
 from glob import glob
 
-package_name = 'control'
+package_name = "control"
 
 setup(
     name=package_name,
-    version='0.0.0',
+    version="0.0.0",
     packages=[package_name],
     data_files=[
-        ('share/ament_index/resource_index/packages',
-            ['resource/' + package_name]),
-        ('share/' + package_name, ['package.xml']),
-        (os.path.join('share', package_name, 'data/'), glob('data/*')),
+        ("share/ament_index/resource_index/packages", ["resource/" + package_name]),
+        ("share/" + package_name, ["package.xml"]),
+        (os.path.join("share", package_name, "data/"), glob("data/*")),
     ],
-    install_requires=['setuptools'],
+    install_requires=["setuptools"],
     zip_safe=True,
-    maintainer='asher',
-    maintainer_email='amelmquist@wisc.edu',
-    description='TODO: Package description',
-    license='TODO: License declaration',
-    tests_require=['pytest'],
+    maintainer="asher",
+    maintainer_email="amelmquist@wisc.edu",
+    description="TODO: Package description",
+    license="TODO: License declaration",
+    tests_require=["pytest"],
     entry_points={
-        'console_scripts': [
-            'pid = control.control:main'
-        ],
+        "console_scripts": ["pid = control.control:main"],
     },
 )
diff --git a/workspace/src/localization/localization_py/localization_py/EKF.py b/workspace/src/localization/localization_py/localization_py/EKF.py
index 6bd04ad3..fed53f57 100644
--- a/workspace/src/localization/localization_py/localization_py/EKF.py
+++ b/workspace/src/localization/localization_py/localization_py/EKF.py
@@ -6,7 +6,7 @@
 
 class EKF:
     """A basic Extended Kalman Filter implementation.
-    
+
     This currently uses the 4DOF motion model. The Motion Model inputs are throttle and steering, and the observation model uses GPS and Magnetometer as sensors. States are cartesian coordinates x, y, heading theta, and velocity v.
 
     Attributes:
@@ -22,7 +22,7 @@ def __init__(self, dt, dynamics, Q, R):
         Initialize each of the global variables for the EKF. Load the dynamics parameters for the vehicle from an external YAML file.
 
         Args:
-            dt: The timestep at which this filter will operate at. 
+            dt: The timestep at which this filter will operate at.
             dynamics_path: The path to the dynamics parameters yaml file.
             param_path: The path to the filter parameters yaml file
         """
@@ -30,18 +30,22 @@ def __init__(self, dt, dynamics, Q, R):
         self.dt = dt
         self.dyn = Dynamics(dt, dynamics)
 
-        self.Q = np.diag([
-            Q[0],  # variance of location on x-axis
-            Q[0],  # variance of location on y-axis
-            Q[2],  # variance of yaw angle
-            Q[3] # variance of velocity
-        ]) ** 2  # predict state covariance
+        self.Q = (
+            np.diag(
+                [
+                    Q[0],  # variance of location on x-axis
+                    Q[0],  # variance of location on y-axis
+                    Q[2],  # variance of yaw angle
+                    Q[3],  # variance of velocity
+                ]
+            )
+            ** 2
+        )  # predict state covariance
         # Observation x,y position covariance
         self.R = np.diag([R[0], R[0], R[1]]) ** 2
         self.P = np.eye(4)
 
-
-    def angle_diff(self,ref, act):
+    def angle_diff(self, ref, act):
         """Computes the difference between two angles.
 
         Computes the difference between two angles in radians using modular subtraction. This solves the issue when we have a positive and negative angle, both with very large magnitude.
@@ -49,23 +53,23 @@ def angle_diff(self,ref, act):
         Args:
             ref: The reference angle in radians.
             act: the actual angle in radians.
-        
+
         Returns:
             The difference between the two angles, using modular arithmatic. For example, an input of {pi, pi/2} will return pi/2. An input of {-pi+epsilon, pi-epsilon} will return 2 epsilon.
         """
-        
-        if( (ref>0 and act>0) or (ref<=0 and act <=0)):
-            err_theta = ref-act
-        elif( ref<=0 and act > 0):
-            if(abs(ref-act)<abs(2*np.pi+ref-act)):
-                err_theta = -abs(act-ref)
+
+        if (ref > 0 and act > 0) or (ref <= 0 and act <= 0):
+            err_theta = ref - act
+        elif ref <= 0 and act > 0:
+            if abs(ref - act) < abs(2 * np.pi + ref - act):
+                err_theta = -abs(act - ref)
             else:
-                err_theta = abs(2*np.pi + ref- act)
+                err_theta = abs(2 * np.pi + ref - act)
         else:
-            if(abs(ref-act)<abs(2*np.pi-ref+act)):
-                err_theta = abs(act-ref)
+            if abs(ref - act) < abs(2 * np.pi - ref + act):
+                err_theta = abs(act - ref)
             else:
-                err_theta = -abs(2*np.pi-ref+act)
+                err_theta = -abs(2 * np.pi - ref + act)
         return err_theta
 
     def predict(self, x, u):
@@ -82,15 +86,15 @@ def predict(self, x, u):
         """
 
         step_size = 1
-        self.dt = self.dt/step_size
-        for i in range(0,step_size):
+        self.dt = self.dt / step_size
+        for i in range(0, step_size):
             x = self.dyn.motion_model(x, u)
 
-        if(x[2,0]>np.pi):
-            x[2,0] = x[2,0]-2*np.pi
-        if(x[2,0]<-np.pi):
-            x[2,0] = x[2,0]+2*np.pi
-        self.dt = self.dt*step_size
+        if x[2, 0] > np.pi:
+            x[2, 0] = x[2, 0] - 2 * np.pi
+        if x[2, 0] < -np.pi:
+            x[2, 0] = x[2, 0] + 2 * np.pi
+        self.dt = self.dt * step_size
         F = self.dyn.calc_F(x[3][0], x[2][0], u[1][0])
 
         self.P = F @ self.P @ F.T + self.Q
@@ -108,27 +112,21 @@ def correct(self, x, z):
         Returns:
             The corrected state of the vehicle based off of the sensor measurements.
         """
-        H = np.array([
-            [1, 0, 0, 0],
-            [0, 1, 0, 0],
-            [0, 0, 1, 0]
-        ])
-        zPred = H@x
-        while(z[2,0]>np.pi):
-            z[2,0] = z[2,0]-2*np.pi
-        while(z[2,0]<-np.pi):
-            z[2,0] = z[2,0]+2*np.pi
-
-        y = z-zPred
-        y[2,0] = self.angle_diff(z[2,0], zPred[2,0])
-        if(y[2,0]>np.pi):
-            y[2,0] = y[2,0]-2*np.pi
-        if(y[2,0]<-np.pi):
-            y[2,0] = y[2,0]+2*np.pi
-        S = H@self.P@H.T + self.R
+        H = np.array([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0]])
+        zPred = H @ x
+        while z[2, 0] > np.pi:
+            z[2, 0] = z[2, 0] - 2 * np.pi
+        while z[2, 0] < -np.pi:
+            z[2, 0] = z[2, 0] + 2 * np.pi
+
+        y = z - zPred
+        y[2, 0] = self.angle_diff(z[2, 0], zPred[2, 0])
+        if y[2, 0] > np.pi:
+            y[2, 0] = y[2, 0] - 2 * np.pi
+        if y[2, 0] < -np.pi:
+            y[2, 0] = y[2, 0] + 2 * np.pi
+        S = H @ self.P @ H.T + self.R
         K = self.P @ H.T @ np.linalg.inv(S)
-        x = x+K@y
-        self.P = (np.eye(len(x))-K@H) @ self.P
+        x = x + K @ y
+        self.P = (np.eye(len(x)) - K @ H) @ self.P
         return x
-
-
diff --git a/workspace/src/localization/localization_py/localization_py/chrono_coordinate_transfer.py b/workspace/src/localization/localization_py/localization_py/chrono_coordinate_transfer.py
index 3213bc53..b76a01c7 100644
--- a/workspace/src/localization/localization_py/localization_py/chrono_coordinate_transfer.py
+++ b/workspace/src/localization/localization_py/localization_py/chrono_coordinate_transfer.py
@@ -1,5 +1,7 @@
 import numpy as np
 import math
+
+
 class Graph:
     """A copy of the CHRONO LTP gps2cartesian coordinate transfer.
 
@@ -14,9 +16,9 @@ class Graph:
 
     Chrono documentation: https://api.projectchrono.org/namespacechrono_1_1sensor.html#a69c276ee1766b50936ed940d39a2555d
     """
+
     def __init__(self):
-        """Initialize the graph instance.
-        """
+        """Initialize the graph instance."""
         self.r = 6378100
 
     def set_graph(self, lat, lon, alt):
@@ -33,7 +35,7 @@ def set_graph(self, lat, lon, alt):
         self.lon = lon
         self.alt = alt
 
-    def gps2cartesian(self,lat,lon,alt):
+    def gps2cartesian(self, lat, lon, alt):
         """Translates GPS coordinates to our LTP frame.
 
         Projects the GPS coordinates onto the 2D LTP, and then returns the coordinates relative to the unrotated LTP.
@@ -43,35 +45,31 @@ def gps2cartesian(self,lat,lon,alt):
             lon: The Longitude for the point we want to translate.
             alt: The Altitude for the point we want to translate.
         """
-        x = ((lon - self.lon) * math.pi / 180.0) * (self.r * math.cos(lat * math.pi / 180.0))
+        x = ((lon - self.lon) * math.pi / 180.0) * (
+            self.r * math.cos(lat * math.pi / 180.0)
+        )
         y = ((lat - self.lat) * math.pi / 180.0) * self.r
         z = alt - self.alt
-        return x,y,z
+        return x, y, z
 
     def set_rotation(self, D):
-        """Sets the 2D rotation matrix for the LTP. 
+        """Sets the 2D rotation matrix for the LTP.
 
         Args:
             D: The offset angle we want for the x-axis of our LTP.
         """
-        self.R = np.array([
-            [math.cos(D), -math.sin(D)],
-            [math.sin(D), math.cos(D)]
-        ])
+        self.R = np.array([[math.cos(D), -math.sin(D)], [math.sin(D), math.cos(D)]])
         self.R = np.linalg.inv(self.R)
-    
+
     def rotate(self, x, y, z):
         """Rotates a given point relative to the rotation matrix.
 
-        Args: 
+        Args:
             x: the x-coordinate we would like to rotate.
             y: the y-coordinate we would like to roatate.
             z: the z-coordinate.
         """
-        position = np.array([
-            [x],
-            [y]
-        ])
+        position = np.array([[x], [y]])
         position = self.R @ position
 
-        return position[0][0], position[1][0],z
+        return position[0][0], position[1][0], z
diff --git a/workspace/src/localization/localization_py/localization_py/dynamics.py b/workspace/src/localization/localization_py/localization_py/dynamics.py
index 10ad532c..56e3fbb9 100644
--- a/workspace/src/localization/localization_py/localization_py/dynamics.py
+++ b/workspace/src/localization/localization_py/localization_py/dynamics.py
@@ -1,9 +1,11 @@
 import numpy as np
 import yaml
 import math
+
+
 class Dynamics:
     """A 4DOF dynamics implementation.
-    
+
     This currently uses the 4DOF motion model. The Motion Model inputs are throttle and steering, and the observation model uses GPS and Magnetometer as sensors. States are cartesian coordinates x, y, heading theta, and velocity v.
 
     Attributes:
@@ -17,18 +19,19 @@ class Dynamics:
         omega_0: The maximum no-load speed for the motor torque model.
         df_1_dv: The derivative of the scaled motor torque relative to the velocity of the vehicle.
     """
+
     def __init__(self, dt, dynamics):
         """Initialize the dynamics object.
 
         Initialize each of the parameters for the 4DOF dynamics model. Load the dynamics parameters for the vehicle from an external YAML file.
 
         Args:
-            dt: The timestep at which this filter will operate at. 
+            dt: The timestep at which this filter will operate at.
             dynamics: the parameters for the dynamics
         """
 
         self.dt = dt
-        #vehicle parameters:
+        # vehicle parameters:
         self.c_1 = dynamics[0]
         self.c_0 = dynamics[1]
         self.l = dynamics[2]
@@ -37,29 +40,31 @@ def __init__(self, dt, dynamics):
         self.gamma = dynamics[5]
         self.tau_0 = dynamics[6]
         self.omega_0 = dynamics[7]
-        self.df_1_dv = -self.tau_0/(self.omega_0*self.r_wheel*self.gamma)
-
+        self.df_1_dv = -self.tau_0 / (self.omega_0 * self.r_wheel * self.gamma)
 
     def motion_model(self, x, u):
         """The 4-DOF motioin model.
-        
+
         This motion model is described in more detail here:https://sbel.wisc.edu/wp-content/uploads/sites/569/2023/06/TR-2023-06.pdf.
 
         Args:
             x: The state vector of the vehicle, consisting of cartesian coordinates x, y, heading theta, and velocity v.
             u: The control input vector for the vehicle, consisting of throttle alpha, and steering delta.
-        
+
         Returns:
             An updated state for the vehicle based off of the 4-DOF model propogation. This is always a 4 dimensional vector.
         """
 
-        x[0,0] = x[0,0]+math.cos(x[2,0])*self.dt*x[3,0]
-        x[1,0] = x[1,0]+math.sin(x[2,0])*self.dt*x[3,0]
-        x[2,0] = x[2,0]+self.dt*x[3,0]*math.tan(u[1,0])/self.l
-        f = self.tau_0*u[0,0]-self.tau_0*x[3,0]/(self.omega_0*self.r_wheel*self.gamma)
+        x[0, 0] = x[0, 0] + math.cos(x[2, 0]) * self.dt * x[3, 0]
+        x[1, 0] = x[1, 0] + math.sin(x[2, 0]) * self.dt * x[3, 0]
+        x[2, 0] = x[2, 0] + self.dt * x[3, 0] * math.tan(u[1, 0]) / self.l
+        f = self.tau_0 * u[0, 0] - self.tau_0 * x[3, 0] / (
+            self.omega_0 * self.r_wheel * self.gamma
+        )
 
-        x[3,0] = x[3,0]+ \
-            self.dt*((self.r_wheel*self.gamma)/self.i_wheel)*(f-(x[3,0]*self.c_1)/(self.r_wheel*self.gamma)-self.c_0)
+        x[3, 0] = x[3, 0] + self.dt * ((self.r_wheel * self.gamma) / self.i_wheel) * (
+            f - (x[3, 0] * self.c_1) / (self.r_wheel * self.gamma) - self.c_0
+        )
 
         return x
 
@@ -78,27 +83,31 @@ def calc_F(self, v, theta, delta):
             The linearized motion model - Jacobian for the equations described above.
         """
 
-        F = np.array([
-            [self.dt,
-                0.0,
-                -self.dt*v *math.sin(theta),
-                self.dt*math.cos(theta)
-            ],
-            [0.0, 
-                1.0, 
-                self.dt*v *math.cos(theta), 
-                self.dt*math.sin(theta)
-            ],
-            [0.0,
-                0.0,
-                self.dt, 
-                self.dt*np.tan(delta)/self.l
-            ],
-            [0.0, 
-                0.0,
-                0.0,
-                self.dt*(1+(self.r_wheel*self.gamma/(self.i_wheel))*(-(self.tau_0/(self.omega_0*self.r_wheel*self.gamma))-(self.c_1/(self.r_wheel*self.gamma))))
+        F = np.array(
+            [
+                [
+                    self.dt,
+                    0.0,
+                    -self.dt * v * math.sin(theta),
+                    self.dt * math.cos(theta),
+                ],
+                [0.0, 1.0, self.dt * v * math.cos(theta), self.dt * math.sin(theta)],
+                [0.0, 0.0, self.dt, self.dt * np.tan(delta) / self.l],
+                [
+                    0.0,
+                    0.0,
+                    0.0,
+                    self.dt
+                    * (
+                        1
+                        + (self.r_wheel * self.gamma / (self.i_wheel))
+                        * (
+                            -(self.tau_0 / (self.omega_0 * self.r_wheel * self.gamma))
+                            - (self.c_1 / (self.r_wheel * self.gamma))
+                        )
+                    ),
+                ],
             ]
-         ])
+        )
 
-        return F
\ No newline at end of file
+        return F
diff --git a/workspace/src/localization/localization_py/localization_py/particle_filter.py b/workspace/src/localization/localization_py/localization_py/particle_filter.py
index 1f05e421..611ba4f1 100644
--- a/workspace/src/localization/localization_py/localization_py/particle_filter.py
+++ b/workspace/src/localization/localization_py/localization_py/particle_filter.py
@@ -6,10 +6,11 @@
 import yaml
 from localization_py.dynamics import Dynamics
 
+
 class ParticleFilter:
     """
     A basic Particle Filter implementation.
-    
+
     This currently uses the 4DOF motion model. The Motion Model inputs are throttle and steering, and the observation model uses GPS and Magnetometer as sensors. States are cartesian coordinates x, y, heading theta, and velocity v. Further reference can be found at: https://sbel.wisc.edu/wp-content/uploads/sites/569/2023/07/TR-2023-05.pdf.
 
     Attributes:
@@ -26,7 +27,7 @@ class ParticleFilter:
         dist_distr: The expected noise distribution for the position of the particles.
         head_distr: The expected noise distribution for the heading of the particles.
         timestep: the current timestep. Used for the animation.
-    
+
     """
 
     def __init__(self, dt, dynamics):
@@ -35,7 +36,7 @@ def __init__(self, dt, dynamics):
         Initialize each of the global variables for the PF. Load the dynamics parameters for the vehicle from an external YAML file.
 
         Args:
-            dt: The timestep at which this filter will operate at. 
+            dt: The timestep at which this filter will operate at.
             dynamics: the parameters for the dynamics
         """
         self.dt = dt
@@ -44,27 +45,30 @@ def __init__(self, dt, dynamics):
         self.show_animation = False
         self.num_particles = 100
         self.particles = []
-        self.particle_weights =[]
+        self.particle_weights = []
         self.hpx = []
         self.hpy = []
         self.particle_distr_dist = []
         self.particle_distr_head = []
 
         self.dist_distr = [abs(np.random.normal(0, 0.8)) for _ in range(100)]
-        self.head_distr = [abs(np.random.normals(0,0.1)) for _ in range(100)]
-        #counts, bins = np.histogram(points, bins = 30)
+        self.head_distr = [abs(np.random.normals(0, 0.1)) for _ in range(100)]
+        # counts, bins = np.histogram(points, bins = 30)
 
-        for i in range(0,self.num_particles):
+        for i in range(0, self.num_particles):
             self.hpx.append([])
             self.hpy.append([])
             self.particle_distr_dist.append([])
             self.particle_distr_head.append([])
-            #self.particles.append(np.zeros((4,1)))
-            self.particles.append(np.array([[np.random.normals(0,0.8)], [np.random.normals(0,0.8)], [0],[0]]))
+            # self.particles.append(np.zeros((4,1)))
+            self.particles.append(
+                np.array(
+                    [[np.random.normals(0, 0.8)], [np.random.normals(0, 0.8)], [0], [0]]
+                )
+            )
             self.particle_weights.append(0.02)
         self.timestep = 0
 
-    
     def update(self, u, obs):
         """The filter update function.
 
@@ -79,49 +83,61 @@ def update(self, u, obs):
 
         """
 
-        self.timestep = self.timestep+1
-        #first, update the motion model
+        self.timestep = self.timestep + 1
+        # first, update the motion model
         for i in range(0, self.num_particles):
-            myu = np.array([[u[0,0]+np.random.normal(0,0.05)], [u[1,0]+np.random.normal(0,0.05)]])
+            myu = np.array(
+                [
+                    [u[0, 0] + np.random.normal(0, 0.05)],
+                    [u[1, 0] + np.random.normal(0, 0.05)],
+                ]
+            )
             self.particles[i] = self.dyn.motion_model(self.particles[i], myu)
         self.update_dist(obs)
-        #then, reassign weights in relation to the observation
+        # then, reassign weights in relation to the observation
         self.assign_weights()
 
-        state = np.zeros((4,1))
-        for i in range(0,self.num_particles):
-            for j in range(0,4):
-                state[j,0] = state[j,0]+self.particles[i][j,0]*self.particle_weights[i]
-        if(self.timestep%5 == 0):
+        state = np.zeros((4, 1))
+        for i in range(0, self.num_particles):
+            for j in range(0, 4):
+                state[j, 0] = (
+                    state[j, 0] + self.particles[i][j, 0] * self.particle_weights[i]
+                )
+        if self.timestep % 5 == 0:
             self.resample()
-        #then, return final value
+        # then, return final value
         return state
-    
-    def update_dist(self,obs):
+
+    def update_dist(self, obs):
         """Updates each particle error distribution from the observation.
 
         For each particle, compute the distance between the current state and the observation (position and heading). Add this to the associated distribution, and pop the oldest value.
-        
+
         Args:
             obs: The 3-D observation vector.
         """
 
         for i in range(0, self.num_particles):
-            distance = math.sqrt((self.particles[i][0,0]-obs[0,0])**2+(self.particles[i][1,0]-obs[1,0])**2)
+            distance = math.sqrt(
+                (self.particles[i][0, 0] - obs[0, 0]) ** 2
+                + (self.particles[i][1, 0] - obs[1, 0]) ** 2
+            )
             self.particle_distr_dist[i].append(distance)
-            self.particle_distr_head[i].append(self.particles[i][2,0]-obs[2,0])
-            if(len(self.particle_distr_dist[i])>10):
+            self.particle_distr_head[i].append(self.particles[i][2, 0] - obs[2, 0])
+            if len(self.particle_distr_dist[i]) > 10:
                 self.particle_distr_dist[i].pop(0)
                 self.particle_distr_head[i].pop(0)
 
     def resample(self):
         """Resample the particles for the next generation.
 
-        Generates a new generation of particles. Computes a cummulative weight, and then generates num_particles random numbers. Then, whichever cummulative range the random number lies in is the particle that gets propogated. If a particle gets propogated more than once, some randomness is added to the state. 
+        Generates a new generation of particles. Computes a cummulative weight, and then generates num_particles random numbers. Then, whichever cummulative range the random number lies in is the particle that gets propogated. If a particle gets propogated more than once, some randomness is added to the state.
 
         """
 
-        cumulative_weights = [sum(self.particle_weights[:i+1]) for i in range(self.num_particles)]
+        cumulative_weights = [
+            sum(self.particle_weights[: i + 1]) for i in range(self.num_particles)
+        ]
         sampled_points = []
         for _ in range(self.num_particles):
             random_num = random.random()  # Generate a random number between 0 and 1
@@ -135,9 +151,9 @@ def resample(self):
         new_particles = []
         new_pdd = []
         new_pdh = []
-        new_hpx= []
+        new_hpx = []
         new_hpy = []
-        for j in range(0,self.num_particles):
+        for j in range(0, self.num_particles):
             i = sampled_points[j]
             new_pdd.append(self.particle_distr_dist[i].copy())
             new_pdh.append(self.particle_distr_head[i].copy())
@@ -147,17 +163,23 @@ def resample(self):
                 already_seen.append(i)
                 new_particles.append(self.particles[i].copy())
             else:
-                new_particles.append(np.array([[self.particles[i][0,0]+np.random.normals(0,0.1)],[self.particles[i][1,0]+np.random.normals(0,0.1)],[self.particles[i][2,0]],[self.particles[i][3,0]]]))
-        if(len(already_seen)<50):
-            print("Particles Pruned: "+ str(50-len(already_seen)))
+                new_particles.append(
+                    np.array(
+                        [
+                            [self.particles[i][0, 0] + np.random.normals(0, 0.1)],
+                            [self.particles[i][1, 0] + np.random.normals(0, 0.1)],
+                            [self.particles[i][2, 0]],
+                            [self.particles[i][3, 0]],
+                        ]
+                    )
+                )
+        if len(already_seen) < 50:
+            print("Particles Pruned: " + str(50 - len(already_seen)))
         self.particles = new_particles
         self.particle_distr_dist = new_pdd
         self.particle_distr_head = new_pdh
         self.hpx = new_hpx
         self.hpy = new_hpy
-                
-
-
 
     def assign_weights(self):
         """Assigns weights to each particle.
@@ -165,8 +187,10 @@ def assign_weights(self):
         Assigns a weight to each particle based on the wasserstein distance between the current particle measurement probability distribution and the expected measurement probability distribution. 90% of weight is assigned to the position, and 10% weight is assigned to the heading. Normalizes the weights after each weight is assigned.
 
         """
-        for i in range(0,self.num_particles):
-            self.particle_weights[i] = 0.9/wasserstein_distance(self.particle_distr_dist[i], self.dist_distr)+0.1/wasserstein_distance(self.particle_distr_head[i], self.head_distr)
+        for i in range(0, self.num_particles):
+            self.particle_weights[i] = 0.9 / wasserstein_distance(
+                self.particle_distr_dist[i], self.dist_distr
+            ) + 0.1 / wasserstein_distance(self.particle_distr_head[i], self.head_distr)
         self.normalizeWeights()
 
     def normalizeWeights(self):
@@ -176,9 +200,7 @@ def normalizeWeights(self):
         """
 
         total = 0
-        for i in range(0,self.num_particles):
-            total = total+self.particle_weights[i]
-        for i in range(0,self.num_particles):
-            self.particle_weights[i] = self.particle_weights[i]/total
-    
-    
+        for i in range(0, self.num_particles):
+            total = total + self.particle_weights[i]
+        for i in range(0, self.num_particles):
+            self.particle_weights[i] = self.particle_weights[i] / total
diff --git a/workspace/src/localization/localization_py/localization_py/state_estimation.py b/workspace/src/localization/localization_py/localization_py/state_estimation.py
index 0d8d99ea..17df2b5d 100644
--- a/workspace/src/localization/localization_py/localization_py/state_estimation.py
+++ b/workspace/src/localization/localization_py/localization_py/state_estimation.py
@@ -16,6 +16,7 @@
 from localization_py.particle_filter import ParticleFilter as PF
 from localization_py.chrono_coordinate_transfer import Graph
 
+
 class EstimationAlgorithmOption(Enum):
     GROUND_TRUTH = "ground_truth"
     EXTENDED_KALMAN_FILTER = "extended_kalman_filter"
@@ -24,49 +25,53 @@ class EstimationAlgorithmOption(Enum):
 
 class StateEstimationNode(Node):
     def __init__(self):
-        super().__init__('state_estimation_node')
-        
+        super().__init__("state_estimation_node")
+
         # ROS PARAMETERS
-        self.use_sim_msg = self.get_parameter("use_sim_time").get_parameter_value().bool_value
-
-        self.declare_parameter('estimation_alg', EstimationAlgorithmOption.GROUND_TRUTH)
-        self.estimation_alg = self.get_parameter('estimation_alg').get_parameter_value().string_value
-
-        #EKF parameters
-        self.declare_parameter('Q1', 0.1)
-        Q1 = self.get_parameter('Q1').get_parameter_value().double_value
-        self.declare_parameter('Q3', 3)
-        Q3 = self.get_parameter('Q3').get_parameter_value().double_value
-        self.declare_parameter('Q4', 0.1)
-        Q4 = self.get_parameter('Q4').get_parameter_value().double_value
-        self.declare_parameter('R1', 0.0)
-        R1 = self.get_parameter('R1').get_parameter_value().double_value
-        self.declare_parameter('R3', 0.3)
-        R3 = self.get_parameter('R3').get_parameter_value().double_value
+        self.use_sim_msg = (
+            self.get_parameter("use_sim_time").get_parameter_value().bool_value
+        )
+
+        self.declare_parameter("estimation_alg", EstimationAlgorithmOption.GROUND_TRUTH)
+        self.estimation_alg = (
+            self.get_parameter("estimation_alg").get_parameter_value().string_value
+        )
+
+        # EKF parameters
+        self.declare_parameter("Q1", 0.1)
+        Q1 = self.get_parameter("Q1").get_parameter_value().double_value
+        self.declare_parameter("Q3", 3)
+        Q3 = self.get_parameter("Q3").get_parameter_value().double_value
+        self.declare_parameter("Q4", 0.1)
+        Q4 = self.get_parameter("Q4").get_parameter_value().double_value
+        self.declare_parameter("R1", 0.0)
+        R1 = self.get_parameter("R1").get_parameter_value().double_value
+        self.declare_parameter("R3", 0.3)
+        R3 = self.get_parameter("R3").get_parameter_value().double_value
         Q = [Q1, Q1, Q3, Q4]
         R = [R1, R1, R3]
 
-        #dynamics parameters
-        self.declare_parameter('c_1', 0.0001)
-        c_1 = self.get_parameter('c_1').get_parameter_value().double_value
-        self.declare_parameter('c_0', 0.02)
-        c_0 = self.get_parameter('c_0').get_parameter_value().double_value
-        self.declare_parameter('l', 0.5)
-        l = self.get_parameter('l').get_parameter_value().double_value
-        self.declare_parameter('r_wheel', 0.08451952624)
-        r_wheel = self.get_parameter('r_wheel').get_parameter_value().double_value
-        self.declare_parameter('i_wheel', 0.001)
-        i_wheel = self.get_parameter('i_wheel').get_parameter_value().double_value
-        self.declare_parameter('gamma', 0.33333333)
-        gamma = self.get_parameter('gamma').get_parameter_value().double_value
-        self.declare_parameter('tau_0', 0.3)
-        tau_0 = self.get_parameter('tau_0').get_parameter_value().double_value
-        self.declare_parameter('omega_0', 30.0)
-        omega_0 = self.get_parameter('omega_0').get_parameter_value().double_value
+        # dynamics parameters
+        self.declare_parameter("c_1", 0.0001)
+        c_1 = self.get_parameter("c_1").get_parameter_value().double_value
+        self.declare_parameter("c_0", 0.02)
+        c_0 = self.get_parameter("c_0").get_parameter_value().double_value
+        self.declare_parameter("l", 0.5)
+        l = self.get_parameter("l").get_parameter_value().double_value
+        self.declare_parameter("r_wheel", 0.08451952624)
+        r_wheel = self.get_parameter("r_wheel").get_parameter_value().double_value
+        self.declare_parameter("i_wheel", 0.001)
+        i_wheel = self.get_parameter("i_wheel").get_parameter_value().double_value
+        self.declare_parameter("gamma", 0.33333333)
+        gamma = self.get_parameter("gamma").get_parameter_value().double_value
+        self.declare_parameter("tau_0", 0.3)
+        tau_0 = self.get_parameter("tau_0").get_parameter_value().double_value
+        self.declare_parameter("omega_0", 30.0)
+        omega_0 = self.get_parameter("omega_0").get_parameter_value().double_value
         dyn = [c_1, c_0, l, r_wheel, i_wheel, gamma, tau_0, omega_0]
         self.get_logger().info(str(dyn))
 
-        #update frequency of this node
+        # update frequency of this node
         self.freq = 10.0
 
         self.gps = ""
@@ -80,27 +85,26 @@ def __init__(self):
             global VehicleInput
             from art_msgs.msg import VehicleInput
 
-        #x, y, from measurements
+        # x, y, from measurements
         self.x = 0
         self.y = 0
-        
-        #what we will be using for our state vector. (x, y, theta yaw, v vel)
-        self.state = np.zeros((4,1))
+
+        # what we will be using for our state vector. (x, y, theta yaw, v vel)
+        self.state = np.zeros((4, 1))
         np.vstack(self.state)
 
-        
         self.init_x = 0.0
         self.init_y = 0.0
         self.init_theta = 0.0
         self.init_v = 0.0
-        self.state[0,0] = self.init_x
-        self.state[1,0] = self.init_y
-        self.state[2,0] = self.init_theta
-        self.state[3,0] = self.init_v
+        self.state[0, 0] = self.init_x
+        self.state[1, 0] = self.init_y
+        self.state[2, 0] = self.init_theta
+        self.state[3, 0] = self.init_v
 
         self.gps_ready = False
 
-        #true X, True Y, velocity, z, heading(degrees)
+        # true X, True Y, velocity, z, heading(degrees)
         self.gtvy = 0
         self.gty = 0
         self.gtvx = 0
@@ -108,39 +112,48 @@ def __init__(self):
         self.gtz = 0
         self.D = 0
 
-        #origin, and whether or not the origin has been set yet.
+        # origin, and whether or not the origin has been set yet.
         self.origin_set = False
         self.orig_heading_set = False
 
-
-        #inputs to the vehicle
+        # inputs to the vehicle
         self.throttle = 0.0
         self.steering = 0
 
-        #time between imu updates, sec
-        self.dt_gps = 1/self.freq
-        
-        #filter
-        if(self.estimation_alg == EstimationAlgorithmOption.EXTENDED_KALMAN_FILTER):
+        # time between imu updates, sec
+        self.dt_gps = 1 / self.freq
+
+        # filter
+        if self.estimation_alg == EstimationAlgorithmOption.EXTENDED_KALMAN_FILTER:
             self.ekf = EKF(self.dt_gps, dyn, Q, R)
-        elif(self.estimation_alg == EstimationAlgorithmOption.PARTICLE_FILTER):
+        elif self.estimation_alg == EstimationAlgorithmOption.PARTICLE_FILTER:
             self.pf = PF(self.dt_gps, dyn)
 
-        #our graph object, for reference frame
+        # our graph object, for reference frame
         self.graph = Graph()
 
-        #subscribers
-        self.sub_gps = self.create_subscription(NavSatFix, '~/input/gps', self.gps_callback, 1)
-        if(self.use_sim_msg):
-            self.sub_groud_truth = self.create_subscription(ChVehicle, '~/input/ground_truth', self.ground_truth_callback, 1)
+        # subscribers
+        self.sub_gps = self.create_subscription(
+            NavSatFix, "~/input/gps", self.gps_callback, 1
+        )
+        if self.use_sim_msg:
+            self.sub_groud_truth = self.create_subscription(
+                ChVehicle, "~/input/ground_truth", self.ground_truth_callback, 1
+            )
 
-        self.sub_mag = self.create_subscription(MagneticField, "~/input/magnetometer", self.mag_callback, 1)
-        self.sub_control = self.create_subscription(VehicleInput, "~/input/vehicle_inputs", self.inputs_callback, 1)
-        #publishers
-        self.pub_objects = self.create_publisher(VehicleState, '~/output/vehicle/filtered_state', 1)
-        self.timer = self.create_timer(1/self.freq, self.pub_callback)
+        self.sub_mag = self.create_subscription(
+            MagneticField, "~/input/magnetometer", self.mag_callback, 1
+        )
+        self.sub_control = self.create_subscription(
+            VehicleInput, "~/input/vehicle_inputs", self.inputs_callback, 1
+        )
+        # publishers
+        self.pub_objects = self.create_publisher(
+            VehicleState, "~/output/vehicle/filtered_state", 1
+        )
+        self.timer = self.create_timer(1 / self.freq, self.pub_callback)
 
-    #CALLBACKS:
+    # CALLBACKS:
     def inputs_callback(self, msg):
         self.inputs = msg
         self.steering = self.inputs.steering
@@ -151,36 +164,36 @@ def ground_truth_callback(self, msg):
         self.gty = msg.pose.position.y
         self.gtvx = msg.twist.linear.x
         self.gtvy = msg.twist.linear.y
-        
-    def mag_callback(self,msg):
+
+    def mag_callback(self, msg):
         self.mag = msg
         mag_x = self.mag.magnetic_field.x
         mag_y = self.mag.magnetic_field.y
         mag_z = self.mag.magnetic_field.z
-        xGauss = mag_x*0.48828125
-        yGauss = mag_y*0.4882815
-        if(xGauss==0):
-            if(yGauss<0):
+        xGauss = mag_x * 0.48828125
+        yGauss = mag_y * 0.4882815
+        if xGauss == 0:
+            if yGauss < 0:
                 self.D = 0
             else:
                 self.D = 90
         else:
-            self.D = math.atan2(yGauss,xGauss)*180/math.pi
-        while(self.D>360):
-            self.D = self.D-360
-        while(self.D<0):
-            self.D = self.D+360
-        
-        if(not self.orig_heading_set):
+            self.D = math.atan2(yGauss, xGauss) * 180 / math.pi
+        while self.D > 360:
+            self.D = self.D - 360
+        while self.D < 0:
+            self.D = self.D + 360
+
+        if not self.orig_heading_set:
             self.orig_heading_set = True
-            self.graph.set_rotation(np.deg2rad(self.D) -self.init_theta)
-            self.state[2,0] = self.init_theta
+            self.graph.set_rotation(np.deg2rad(self.D) - self.init_theta)
+            self.state[2, 0] = self.init_theta
 
-    def gps_callback(self,msg):
+    def gps_callback(self, msg):
         self.gps = msg
         self.gps_ready = True
-        if(math.isnan(self.gps.latitude)):
-            #arbitrary values for when we don't get any data (in reality)
+        if math.isnan(self.gps.latitude):
+            # arbitrary values for when we don't get any data (in reality)
             self.lat = -10
             self.lon = -10
             self.alt = -10
@@ -189,55 +202,56 @@ def gps_callback(self,msg):
             self.lon = self.gps.longitude
             self.alt = self.gps.altitude
 
-        if(not self.origin_set):
-           self.origin_set = True
-           self.graph.set_graph(self.lat,self.lon, self.alt)
-        
-        x,y,z = self.graph.gps2cartesian(self.lat,self.lon,self.alt)
-        if(self.orig_heading_set):
-            self.x,self.y, self.z =self.graph.rotate(x,y,z)
-            self.x +=self.init_x
-            self.y +=self.init_y
-
-     #callback to run a loop and publish data this class generates
+        if not self.origin_set:
+            self.origin_set = True
+            self.graph.set_graph(self.lat, self.lon, self.alt)
+
+        x, y, z = self.graph.gps2cartesian(self.lat, self.lon, self.alt)
+        if self.orig_heading_set:
+            self.x, self.y, self.z = self.graph.rotate(x, y, z)
+            self.x += self.init_x
+            self.y += self.init_y
+
+    # callback to run a loop and publish data this class generates
     def pub_callback(self):
-        u = np.array([[self.throttle], [self.steering/2.2]])
+        u = np.array([[self.throttle], [self.steering / 2.2]])
 
-        z = np.array([[self.x],[self.y], [np.deg2rad(self.D)]])
-                
-        if(self.estimation_alg == EstimationAlgorithmOption.EXTENDED_KALMAN_FILTER):
+        z = np.array([[self.x], [self.y], [np.deg2rad(self.D)]])
+
+        if self.estimation_alg == EstimationAlgorithmOption.EXTENDED_KALMAN_FILTER:
             self.EKFstep(u, z)
-        elif(self.estimation_alg == EstimationAlgorithmOption.PARTICLE_FILTER):
-            self.PFstep(u,z)
-    
+        elif self.estimation_alg == EstimationAlgorithmOption.PARTICLE_FILTER:
+            self.PFstep(u, z)
+
         msg = VehicleState()
-        #pos and velocity are in meters, from the origin, [x, y, z]
-        if(self.estimation_alg == EstimationAlgorithmOption.GROUND_TRUTH):
+        # pos and velocity are in meters, from the origin, [x, y, z]
+        if self.estimation_alg == EstimationAlgorithmOption.GROUND_TRUTH:
             msg.pose.position.x = float(self.gtx)
             msg.pose.position.y = float(self.gty)
-            #TODO: this should be a quat in the future, not the heading.
+            # TODO: this should be a quat in the future, not the heading.
             msg.pose.orientation.z = np.deg2rad(self.D)
             msg.twist.linear.x = float(self.gtvx)
             msg.twist.linear.y = float(self.gtvy)
         else:
-            msg.pose.position.x = float(self.state[0,0])
-            msg.pose.position.y = float(self.state[1,0])
-            msg.pose.orientation.z = float(self.state[2,0])
-            msg.twist.linear.x = float(self.state[3,0]*math.cos(self.state[2,0]))
-            msg.twist.linear.y = float(self.state[3,0]*math.sin(self.state[2,0]))
-        
+            msg.pose.position.x = float(self.state[0, 0])
+            msg.pose.position.y = float(self.state[1, 0])
+            msg.pose.orientation.z = float(self.state[2, 0])
+            msg.twist.linear.x = float(self.state[3, 0] * math.cos(self.state[2, 0]))
+            msg.twist.linear.y = float(self.state[3, 0] * math.sin(self.state[2, 0]))
+
         msg.header.stamp = self.get_clock().now().to_msg()
         self.pub_objects.publish(msg)
-    
+
     def EKFstep(self, u, z):
         self.state = self.ekf.predict(self.state, u)
-        if(self.gps_ready):
+        if self.gps_ready:
             self.state = self.ekf.correct(self.state, z)
             self.gps_ready = False
 
     def PFstep(self, u, z):
         self.state = self.pf.update(u, z)
 
+
 def main(args=None):
     print("=== Starting State Estimation Node ===")
     rclpy.init(args=args)
@@ -246,5 +260,6 @@ def main(args=None):
     estimator.destroy_node()
     rclpy.shutdown()
 
-if __name__ == '__main__':
+
+if __name__ == "__main__":
     main()
diff --git a/workspace/src/localization/localization_py/setup.py b/workspace/src/localization/localization_py/setup.py
index c0ca66e3..39552465 100644
--- a/workspace/src/localization/localization_py/setup.py
+++ b/workspace/src/localization/localization_py/setup.py
@@ -1,26 +1,23 @@
 from setuptools import setup
 
-package_name = 'localization_py'
+package_name = "localization_py"
 
 setup(
     name=package_name,
-    version='0.0.0',
+    version="0.0.0",
     packages=[package_name],
     data_files=[
-        ('share/ament_index/resource_index/packages',
-            ['resource/' + package_name]),
-        ('share/' + package_name, ['package.xml']),
+        ("share/ament_index/resource_index/packages", ["resource/" + package_name]),
+        ("share/" + package_name, ["package.xml"]),
     ],
-    install_requires=['setuptools'],
+    install_requires=["setuptools"],
     zip_safe=True,
-    maintainer='art',
-    maintainer_email='art@todo.todo',
-    description='TODO: Package description',
-    license='TODO: License declaration',
-    tests_require=['pytest'],
+    maintainer="art",
+    maintainer_email="art@todo.todo",
+    description="TODO: Package description",
+    license="TODO: License declaration",
+    tests_require=["pytest"],
     entry_points={
-        'console_scripts': [
-            'state_estimation = localization_py.state_estimation:main'
-        ],
+        "console_scripts": ["state_estimation = localization_py.state_estimation:main"],
     },
 )
diff --git a/workspace/src/path_planning/path_planning/path_planning.py b/workspace/src/path_planning/path_planning/path_planning.py
index 1afa7c83..98f27a68 100644
--- a/workspace/src/path_planning/path_planning/path_planning.py
+++ b/workspace/src/path_planning/path_planning/path_planning.py
@@ -44,28 +44,31 @@
 import matplotlib
 import matplotlib.pyplot as plt
 import matplotlib.patches as patches
-from scipy.interpolate import interp1d,splev,splprep
+from scipy.interpolate import interp1d, splev, splprep
 import os
 import json
 
+
 class PathPlanningNode(Node):
     def __init__(self):
-        super().__init__('path_planning_node')
+        super().__init__("path_planning_node")
 
-        #update frequency of this node
+        # update frequency of this node
         self.freq = 10.0
 
         # READ IN SHARE DIRECTORY LOCATION
-        package_share_directory = get_package_share_directory('path_planning')
+        package_share_directory = get_package_share_directory("path_planning")
 
         # READ IN PARAMETERS
-        self.declare_parameter('vis', False)
-        self.vis = self.get_parameter('vis').get_parameter_value().bool_value
+        self.declare_parameter("vis", False)
+        self.vis = self.get_parameter("vis").get_parameter_value().bool_value
+
+        self.declare_parameter("lookahead", 2.0)
+        self.lookahead = (
+            self.get_parameter("lookahead").get_parameter_value().double_value
+        )
 
-        self.declare_parameter('lookahead', 2.0)
-        self.lookahead = self.get_parameter('lookahead').get_parameter_value().double_value
-        
-        #data that will be used by this class
+        # data that will be used by this class
         self.state = VehicleState()
         self.path = Path()
         # self.objects = ObjectArray()
@@ -75,27 +78,29 @@ def __init__(self):
 
         self.go = False
 
-        #subscribers
+        # subscribers
         qos_profile = QoSProfile(depth=1)
         qos_profile.history = QoSHistoryPolicy.KEEP_LAST
         # self.sub_state = self.create_subscription(VehicleState, '~/input/vehicle_state', self.state_callback, qos_profile)
-        self.sub_objects = self.create_subscription(ObjectArray, '~/input/objects', self.objects_callback, qos_profile)
+        self.sub_objects = self.create_subscription(
+            ObjectArray, "~/input/objects", self.objects_callback, qos_profile
+        )
 
         if self.vis:
             matplotlib.use("TKAgg")
             self.fig, self.ax = plt.subplots()
             plt.title("Path Planning")
             self.patches = []
-            self.ax.set_xlim((-1,11))
-            self.ax.set_ylim((-6,6))
+            self.ax.set_xlim((-1, 11))
+            self.ax.set_ylim((-6, 6))
             self.left_boundary = None
             self.right_boundary = None
-            
-        #publishers
-        self.pub_path = self.create_publisher(Path, '~/output/path', 10)
-        self.timer = self.create_timer(1/self.freq, self.pub_callback)
 
-    #function to process data this class subscribes to
+        # publishers
+        self.pub_path = self.create_publisher(Path, "~/output/path", 10)
+        self.timer = self.create_timer(1 / self.freq, self.pub_callback)
+
+    # function to process data this class subscribes to
     def state_callback(self, msg):
         # self.get_logger().info("Received '%s'" % msg)
         self.state = msg
@@ -105,118 +110,128 @@ def objects_callback(self, msg):
         # self.objects = msg
 
         self.go = True
-    
+
         self.green_cones = []
         self.red_cones = []
 
         # self.get_logger().info("Detected cones: %s" % (str(len(msg.objects))))
 
         for obj in msg.objects:
-            pos = [obj.pose.position.x,obj.pose.position.y,obj.pose.position.z]
+            pos = [obj.pose.position.x, obj.pose.position.y, obj.pose.position.z]
             id = obj.classification.classification
 
-            #calculate position from camera parameters, rect, and distance
-            if(np.linalg.norm(pos) < 2.0*self.lookahead):
-                if(id == 1):
+            # calculate position from camera parameters, rect, and distance
+            if np.linalg.norm(pos) < 2.0 * self.lookahead:
+                if id == 1:
                     self.red_cones.append(pos)
-                elif(id == 2):
+                elif id == 2:
                     self.green_cones.append(pos)
                 else:
-                    self.get_logger().info("Object with unknown label detected {}".format(id))
-        
+                    self.get_logger().info(
+                        "Object with unknown label detected {}".format(id)
+                    )
 
-    def order_cones(self,cones,start):
+    def order_cones(self, cones, start):
         ordered_cones = [start]
 
         ego = start
         for i in range(len(cones)):
-            dist_2 = np.sum((cones - ego)**2, axis=1)
+            dist_2 = np.sum((cones - ego) ** 2, axis=1)
             id = np.argmin(dist_2)
-            ordered_cones.append(cones[id,:])
-            cones = np.delete(cones,id,axis=0)
-        
+            ordered_cones.append(cones[id, :])
+            cones = np.delete(cones, id, axis=0)
+
         ordered_cones = np.asarray(ordered_cones)
         total_dist = 0
-        for i in range(len(ordered_cones)-1):
-            total_dist += np.linalg.norm(ordered_cones[i,:] -  ordered_cones[i+1,:])
+        for i in range(len(ordered_cones) - 1):
+            total_dist += np.linalg.norm(ordered_cones[i, :] - ordered_cones[i + 1, :])
 
         return ordered_cones, total_dist
-        
 
     def plan_path(self):
         self.red_cones = np.asarray(self.red_cones)
         self.green_cones = np.asarray(self.green_cones)
 
-        if(len(self.red_cones) == 0):
-            self.red_cones = np.asarray([1,-1.5,0]) #phantom cone to right if none are seen
-        if(len(self.green_cones) == 0):
-            self.green_cones = np.asarray([1,1.5,0]) #phantom cone to right if none are seen
+        if len(self.red_cones) == 0:
+            self.red_cones = np.asarray(
+                [1, -1.5, 0]
+            )  # phantom cone to right if none are seen
+        if len(self.green_cones) == 0:
+            self.green_cones = np.asarray(
+                [1, 1.5, 0]
+            )  # phantom cone to right if none are seen
+
+        self.red_cones = self.red_cones.reshape((-1, 3))
+        self.green_cones = self.green_cones.reshape((-1, 3))
 
-        self.red_cones = self.red_cones.reshape((-1,3))
-        self.green_cones = self.green_cones.reshape((-1,3))
+        left, l_dist = self.order_cones(self.green_cones, np.array([0.0, 0.5, 0]))
+        right, r_dist = self.order_cones(self.red_cones, np.array([0.0, -0.5, 0]))
 
-        left, l_dist = self.order_cones(self.green_cones,np.array([0.0,.5,0]))
-        right, r_dist = self.order_cones(self.red_cones,np.array([0.0,-.5,0]))
-        
         max_dist = 4
 
-        left_spline,u = splprep(left[:,0:2].transpose(),k=max(1,min(int(len(left)/2),5)))
+        left_spline, u = splprep(
+            left[:, 0:2].transpose(), k=max(1, min(int(len(left) / 2), 5))
+        )
         left_samples = np.linspace(0, max_dist / l_dist, 100)
-        b_left = splev(left_samples,left_spline)
+        b_left = splev(left_samples, left_spline)
 
-        right_spline,u = splprep(right[:,0:2].transpose(),k=max(1,min(int(len(right)/2),5)))
+        right_spline, u = splprep(
+            right[:, 0:2].transpose(), k=max(1, min(int(len(right) / 2), 5))
+        )
         right_samples = np.linspace(0, max_dist / r_dist, 100)
-        b_right = splev(right_samples,right_spline)
+        b_right = splev(right_samples, right_spline)
 
-        center_line = np.array([(b_left[0] + b_right[0]) / 2, (b_left[1] + b_right[1]) / 2])
+        center_line = np.array(
+            [(b_left[0] + b_right[0]) / 2, (b_left[1] + b_right[1]) / 2]
+        )
         # center_line = center_line[:,min(len(b_right[0]),len(b_left[0]))]
-        
-        distances = np.sum((center_line)**2, axis=0)
+
+        distances = np.sum((center_line) ** 2, axis=0)
         id = np.argmin(np.abs(distances - self.lookahead**2))
-        target_pt = center_line[:,id]
+        target_pt = center_line[:, id]
 
         # self.get_logger().info("B Left Spline: %s" % (str(len(b_left))))
-        
-        if(self.vis):
+
+        if self.vis:
             [p.remove() for p in self.patches]
             self.patches.clear()
 
-            if(self.left_boundary == None):
-                self.left_boundary, = self.ax.plot(b_left[0],b_left[1],c='g')
+            if self.left_boundary == None:
+                (self.left_boundary,) = self.ax.plot(b_left[0], b_left[1], c="g")
             else:
-                self.left_boundary.set_data(b_left[0],b_left[1])
+                self.left_boundary.set_data(b_left[0], b_left[1])
 
-            if(self.right_boundary == None):
-                self.right_boundary, = self.ax.plot(b_right[0],b_right[1],c='r')
+            if self.right_boundary == None:
+                (self.right_boundary,) = self.ax.plot(b_right[0], b_right[1], c="r")
             else:
-                self.right_boundary.set_data(b_right[0],b_right[1])
+                self.right_boundary.set_data(b_right[0], b_right[1])
 
             for pos in right:
-                circ = patches.Circle(pos[0:2],radius=.1,color='r')
+                circ = patches.Circle(pos[0:2], radius=0.1, color="r")
                 self.ax.add_patch(circ)
                 self.patches.append(circ)
 
             for pos in left:
-                circ = patches.Circle(pos[0:2],radius=.1,color='g')
+                circ = patches.Circle(pos[0:2], radius=0.1, color="g")
                 self.ax.add_patch(circ)
                 self.patches.append(circ)
 
-            circ = patches.Circle(target_pt,radius=.1,color='b')
+            circ = patches.Circle(target_pt, radius=0.1, color="b")
             self.ax.add_patch(circ)
             self.patches.append(circ)
 
         return target_pt
 
-    #callback to run a loop and publish data this class generates
+    # callback to run a loop and publish data this class generates
     def pub_callback(self):
-        if(not self.go):
+        if not self.go:
             return
         msg = Path()
-            
+
         target_pt = self.plan_path()
 
-        #calculate path from current cone locations
-        if(self.vis):
+        # calculate path from current cone locations
+        if self.vis:
             plt.draw()
             plt.pause(0.0001)
 
@@ -227,6 +242,7 @@ def pub_callback(self):
         msg.poses.append(pt)
         self.pub_path.publish(msg)
 
+
 def main(args=None):
     # print("=== Starting Path Planning Node ===")
     rclpy.init(args=args)
@@ -235,5 +251,6 @@ def main(args=None):
     planner.destroy_node()
     rclpy.shutdown()
 
-if __name__ == '__main__':
+
+if __name__ == "__main__":
     main()
diff --git a/workspace/src/path_planning/setup.py b/workspace/src/path_planning/setup.py
index e69419e8..8eeeed45 100644
--- a/workspace/src/path_planning/setup.py
+++ b/workspace/src/path_planning/setup.py
@@ -32,28 +32,25 @@
 import os
 from glob import glob
 
-package_name = 'path_planning'
+package_name = "path_planning"
 
 setup(
     name=package_name,
-    version='0.0.0',
+    version="0.0.0",
     packages=[package_name],
     data_files=[
-        ('share/ament_index/resource_index/packages',
-            ['resource/' + package_name]),
-        ('share/' + package_name, ['package.xml']),
-        (os.path.join('share', package_name, 'data/'), glob('data/*')),
+        ("share/ament_index/resource_index/packages", ["resource/" + package_name]),
+        ("share/" + package_name, ["package.xml"]),
+        (os.path.join("share", package_name, "data/"), glob("data/*")),
     ],
-    install_requires=['setuptools'],
+    install_requires=["setuptools"],
     zip_safe=True,
-    maintainer='asher',
-    maintainer_email='amelmquist@wisc.edu',
-    description='TODO: Package description',
-    license='TODO: License declaration',
-    tests_require=['pytest'],
+    maintainer="asher",
+    maintainer_email="amelmquist@wisc.edu",
+    description="TODO: Package description",
+    license="TODO: License declaration",
+    tests_require=["pytest"],
     entry_points={
-        'console_scripts': [
-            'path_planning = path_planning.path_planning:main'
-        ],
+        "console_scripts": ["path_planning = path_planning.path_planning:main"],
     },
 )
diff --git a/workspace/src/perception/cone_detector/cone_detector/loader.py b/workspace/src/perception/cone_detector/cone_detector/loader.py
index a56ee4e0..0ff89634 100644
--- a/workspace/src/perception/cone_detector/cone_detector/loader.py
+++ b/workspace/src/perception/cone_detector/cone_detector/loader.py
@@ -30,6 +30,7 @@
 # OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.#
 import torch
 import torchvision
+
 # import torch.nn as nn
 # import torch.optim as optim
 import torchvision.transforms as transforms
@@ -41,9 +42,11 @@
 from PIL import Image, ImageEnhance, ImageFilter
 import matplotlib.pyplot as plt
 import matplotlib.patches as patches
-# import cv2 
 
-class SegImgLoader():
+# import cv2
+
+
+class SegImgLoader:
     def __init__(self, data_root, max_samples=-1, img_format=".png", seg_format=".png"):
         self.imgs = []
         self.seg_imgs = []
@@ -52,31 +55,36 @@ def __init__(self, data_root, max_samples=-1, img_format=".png", seg_format=".pn
         self.seg_dir = os.path.join(data_root, "seg_imgs")
         self.max_samples = max_samples
 
-        if(not os.path.exists(self.img_dir)):
+        if not os.path.exists(self.img_dir):
             print("Error, img directory not found: {}".format(self.img_dir))
             exit(1)
 
-        if(not os.path.exists(self.seg_dir)):
+        if not os.path.exists(self.seg_dir):
             print("Error, segmented img directory not found: {}".format(self.seg_dir))
             exit(1)
 
-        self.imgs = glob.glob(self.img_dir + "/*"+img_format)
-        if(len(self.imgs) == 0):
-            print("Error: no images found in {}".format(
-                os.path.join(self.data_root, "imgs/*"+img_format)))
+        self.imgs = glob.glob(self.img_dir + "/*" + img_format)
+        if len(self.imgs) == 0:
+            print(
+                "Error: no images found in {}".format(
+                    os.path.join(self.data_root, "imgs/*" + img_format)
+                )
+            )
             exit(1)
 
         for f in self.imgs:
             basename = os.path.splitext(os.path.basename(f))[0]
-            self.seg_imgs.append(os.path.join(
-                self.seg_dir, basename+seg_format))
+            self.seg_imgs.append(os.path.join(self.seg_dir, basename + seg_format))
 
-        if(self.max_samples > 0 and self.max_samples < len(self.imgs)):
-            self.imgs = self.imgs[0:self.max_samples]
-            self.seg_imgs = self.seg_imgs[0:self.max_samples]
+        if self.max_samples > 0 and self.max_samples < len(self.imgs):
+            self.imgs = self.imgs[0 : self.max_samples]
+            self.seg_imgs = self.seg_imgs[0 : self.max_samples]
 
-        print("Data loaded. Imgs={}, Seg Imgs={}".format(
-            len(self.imgs), len(self.seg_imgs)))
+        print(
+            "Data loaded. Imgs={}, Seg Imgs={}".format(
+                len(self.imgs), len(self.seg_imgs)
+            )
+        )
 
     def ConvertSegToBoxes(self, semantic_maps):
         # boxes = np.asarray(
@@ -89,61 +97,69 @@ def ConvertSegToBoxes(self, semantic_maps):
 
         box_count = 0
 
-        for c in range(1, np.max(semantic_maps[:, :, 0])+1):
-            for i in range(1, np.max(semantic_maps[:, :, 1])+1):
+        for c in range(1, np.max(semantic_maps[:, :, 0]) + 1):
+            for i in range(1, np.max(semantic_maps[:, :, 1]) + 1):
                 indices = np.where(
-                    np.logical_and(semantic_maps[:, :, 1] == i, semantic_maps[:, :, 0] == c))
-                if(indices[0].shape[0] > 1):
+                    np.logical_and(
+                        semantic_maps[:, :, 1] == i, semantic_maps[:, :, 0] == c
+                    )
+                )
+                if indices[0].shape[0] > 1:
                     y0 = np.min(indices[0])
                     y1 = np.max(indices[0])
                     x0 = np.min(indices[1])
                     x1 = np.max(indices[1])
 
-                    if(x1 > x0 and y1 > y0 ):
-
-                        #change x0,y0,x1,y1 to normalized center (x,y) and width, height
-                        x_center = .5 * (x0 + x1) / float(semantic_maps.shape[1])
-                        y_center = .5 * (y0 + y1) / float(semantic_maps.shape[0])
-                        x_size = (x1-x0) /  float(semantic_maps.shape[1])
-                        y_size = (y1-y0) /  float(semantic_maps.shape[0])
+                    if x1 > x0 and y1 > y0:
+                        # change x0,y0,x1,y1 to normalized center (x,y) and width, height
+                        x_center = 0.5 * (x0 + x1) / float(semantic_maps.shape[1])
+                        y_center = 0.5 * (y0 + y1) / float(semantic_maps.shape[0])
+                        x_size = (x1 - x0) / float(semantic_maps.shape[1])
+                        y_size = (y1 - y0) / float(semantic_maps.shape[0])
 
                         # boxes.append(np.array([x0, y0, x1, y1]))
                         boxes.append(np.array([x_center, y_center, x_size, y_size]))
                         labels.append(c)
                         box_count += 1
 
-        boxes = np.asarray(boxes)#.astype(np.int32)
+        boxes = np.asarray(boxes)  # .astype(np.int32)
         labels = np.asarray(labels).astype(np.int32)
         return boxes, labels
 
-    def GenerateAAVBBFromSeg(self,label_format=".txt"):
+    def GenerateAAVBBFromSeg(self, label_format=".txt"):
         label_dir = os.path.join(self.data_root, "labels")
-        if(not os.path.exists(label_dir)):
+        if not os.path.exists(label_dir):
             os.mkdir(label_dir)
 
         for i in range(len(self.imgs)):
-            #load segmentation img
+            # load segmentation img
             seg_img = np.array(Image.open(self.seg_imgs[i])).view(np.uint16)[:, :, :]
 
-            #generate boxes and labels from segmentation img
-            boxes,classes = self.ConvertSegToBoxes(seg_img)
+            # generate boxes and labels from segmentation img
+            boxes, classes = self.ConvertSegToBoxes(seg_img)
 
-            if(len(classes)>0):
+            if len(classes) > 0:
                 classes -= np.ones(classes.shape).astype(np.int32)
-                classes = np.reshape(classes, (len(classes),1))
-                output = np.append(classes,boxes,axis=1)
+                classes = np.reshape(classes, (len(classes), 1))
+                output = np.append(classes, boxes, axis=1)
 
                 basename = os.path.splitext(os.path.basename(self.imgs[i]))[0]
-                file_name = os.path.join(label_dir, basename+label_format)
-                np.savetxt(file_name,output,fmt='%.6f')
-
-                print("Generated AABB file {}/{}".format(i+1,len(self.imgs)))
+                file_name = os.path.join(label_dir, basename + label_format)
+                np.savetxt(file_name, output, fmt="%.6f")
 
-                
+                print("Generated AABB file {}/{}".format(i + 1, len(self.imgs)))
 
 
 class ObjectDetectionImgLoader(torch.utils.data.Dataset):
-    def __init__(self, data_root, max_boxes, apply_transforms=False, max_samples=-1, img_format=".png", box_format=".txt"):
+    def __init__(
+        self,
+        data_root,
+        max_boxes,
+        apply_transforms=False,
+        max_samples=-1,
+        img_format=".png",
+        box_format=".txt",
+    ):
         self.name = "Object Detection Image Loader"
         self.data_root = data_root
         self.max_boxes = max_boxes
@@ -155,184 +171,216 @@ def __init__(self, data_root, max_boxes, apply_transforms=False, max_samples=-1,
         self.imgs = []
         self.labels = []
 
-        #transform parameters
+        # transform parameters
         self.use_transforms = apply_transforms
         np.random.seed(1)
         self.flip_prob = 0.5
-        self.max_translation = (0.2,0.2)
-        self.brightness = (0.75,1.33)
-        self.sharpness = (0.25,4.0)
-        self.saturation = (0.75,1.33)
-        self.contrast = (0.75,1.33)
+        self.max_translation = (0.2, 0.2)
+        self.brightness = (0.75, 1.33)
+        self.sharpness = (0.25, 4.0)
+        self.saturation = (0.75, 1.33)
+        self.contrast = (0.75, 1.33)
         self.max_zoom = 2.0
 
-        if(not os.path.exists(self.data_root)):
+        if not os.path.exists(self.data_root):
             print("Error: directory not found. Data root = {}".format(self.data_root))
             exit(1)
 
-        if(not os.path.exists(self.img_dir)):
-            print("Error: directory not found. Image directory = {}".format(self.img_dir))
+        if not os.path.exists(self.img_dir):
+            print(
+                "Error: directory not found. Image directory = {}".format(self.img_dir)
+            )
             exit(1)
 
-        if(not os.path.exists(self.label_dir)):
-            print("Error: directory not found. Label directory = {}".format(self.label_dir))
+        if not os.path.exists(self.label_dir):
+            print(
+                "Error: directory not found. Label directory = {}".format(
+                    self.label_dir
+                )
+            )
             exit(1)
 
         self.imgs = glob.glob(os.path.join(self.data_root, "imgs/*"))
 
-
-        if(len(self.imgs) == 0):
-            print("Error: no images found in {}".format(
-                os.path.join(self.data_root, "imgs/*")))
+        if len(self.imgs) == 0:
+            print(
+                "Error: no images found in {}".format(
+                    os.path.join(self.data_root, "imgs/*")
+                )
+            )
             exit(1)
 
         for f in self.imgs:
             basename = os.path.splitext(os.path.basename(f))[0]
-            self.labels.append(os.path.join(self.label_dir, basename+box_format))
+            self.labels.append(os.path.join(self.label_dir, basename + box_format))
 
-        if(self.max_samples > 0 and self.max_samples < len(self.imgs)):
-            self.imgs = self.imgs[0:self.max_samples]
-            self.labels = self.labels[0:self.max_samples]
+        if self.max_samples > 0 and self.max_samples < len(self.imgs):
+            self.imgs = self.imgs[0 : self.max_samples]
+            self.labels = self.labels[0 : self.max_samples]
 
-        print("Data loaded. Imgs={}, Labels={}".format(
-            len(self.imgs), len(self.labels)))
+        print(
+            "Data loaded. Imgs={}, Labels={}".format(len(self.imgs), len(self.labels))
+        )
 
     def __len__(self):
         return len(self.imgs)
 
-
-    def ApplyTransforms(self,img,boxes,classes):
-        #get height and width parameters
+    def ApplyTransforms(self, img, boxes, classes):
+        # get height and width parameters
         height = np.asarray(img).shape[0]
         width = np.asarray(img).shape[1]
 
-        #=== random horizontal flip ===
-        if(np.random.rand() > self.flip_prob):
-            #flip image horizontally
+        # === random horizontal flip ===
+        if np.random.rand() > self.flip_prob:
+            # flip image horizontally
             img = img.transpose(Image.FLIP_LEFT_RIGHT)
 
-            #flip boxes horizontally
-            boxes_x_0 = width - 1 - boxes[:,0]
-            boxes_x_1 = width - 1 - boxes[:,2]
-            boxes[:,0] = boxes_x_1
-            boxes[:,2] = boxes_x_0
-
-        #=== random zoom and crop ===
-        zoom = np.random.uniform(1.0,self.max_zoom)
-        img = img.resize((int(zoom*width),int(zoom*height)),resample=Image.BILINEAR)
-        crop_pt = np.random.uniform(0.0,0.9,(2))
+            # flip boxes horizontally
+            boxes_x_0 = width - 1 - boxes[:, 0]
+            boxes_x_1 = width - 1 - boxes[:, 2]
+            boxes[:, 0] = boxes_x_1
+            boxes[:, 2] = boxes_x_0
+
+        # === random zoom and crop ===
+        zoom = np.random.uniform(1.0, self.max_zoom)
+        img = img.resize(
+            (int(zoom * width), int(zoom * height)), resample=Image.BILINEAR
+        )
+        crop_pt = np.random.uniform(0.0, 0.9, (2))
         # crop_pt = [0,0]
-        crop_pt = (crop_pt * (zoom*width - width, zoom*height - height)).astype(np.int)
+        crop_pt = (crop_pt * (zoom * width - width, zoom * height - height)).astype(
+            np.int
+        )
 
-        crop_pt[0] = np.clip(crop_pt[0],0,img.size[0] - width)
-        crop_pt[1] = np.clip(crop_pt[1],0,img.size[1] - height)
+        crop_pt[0] = np.clip(crop_pt[0], 0, img.size[0] - width)
+        crop_pt[1] = np.clip(crop_pt[1], 0, img.size[1] - height)
 
         # img = img[crop_pt[1],crop_pt[1]+height,crop_pt[0],crop_pt[0]+width,:]
-        img = img.crop((crop_pt[0],crop_pt[1],crop_pt[0]+width,crop_pt[1]+height))
+        img = img.crop(
+            (crop_pt[0], crop_pt[1], crop_pt[0] + width, crop_pt[1] + height)
+        )
         boxes = (boxes * zoom).astype(np.int)
-        boxes = boxes - (crop_pt[0],crop_pt[1],crop_pt[0],crop_pt[1])
-        boxes[:,0] = np.clip(boxes[:,0],0,width-1) #clip x0 value
-        boxes[:,2] = np.clip(boxes[:,2],0,width-1) #clip x1 value 
-        boxes[:,1] = np.clip(boxes[:,1],0,height-1) #clip y0 value
-        boxes[:,3] = np.clip(boxes[:,3],0,height-1) #clip y1 value 
+        boxes = boxes - (crop_pt[0], crop_pt[1], crop_pt[0], crop_pt[1])
+        boxes[:, 0] = np.clip(boxes[:, 0], 0, width - 1)  # clip x0 value
+        boxes[:, 2] = np.clip(boxes[:, 2], 0, width - 1)  # clip x1 value
+        boxes[:, 1] = np.clip(boxes[:, 1], 0, height - 1)  # clip y0 value
+        boxes[:, 3] = np.clip(boxes[:, 3], 0, height - 1)  # clip y1 value
         for i in range(len(classes)):
-            #if box moved out of image, set label to 0
-            if(abs(boxes[i,0] - boxes[i,2]) < 0.5 or abs(boxes[i,1] - boxes[i,3]) < 0.5):
-                classes[i] = 0  #reset label
-                boxes[i,:] = np.array([0,1,0,1]) #reset to valid box coords
-
-
-
-
-        #=== random translation ===
-        #get translation parameters
-        t_x,t_y = np.random.uniform(-1,1,size=2) * self.max_translation * (height,width)
+            # if box moved out of image, set label to 0
+            if (
+                abs(boxes[i, 0] - boxes[i, 2]) < 0.5
+                or abs(boxes[i, 1] - boxes[i, 3]) < 0.5
+            ):
+                classes[i] = 0  # reset label
+                boxes[i, :] = np.array([0, 1, 0, 1])  # reset to valid box coords
+
+        # === random translation ===
+        # get translation parameters
+        t_x, t_y = (
+            np.random.uniform(-1, 1, size=2) * self.max_translation * (height, width)
+        )
         t_x = int(t_x)
         t_y = int(t_y)
-        #apply translation to img
-        img = img.transform(img.size,Image.AFFINE,(1,0,t_x,0,1,t_y))
+        # apply translation to img
+        img = img.transform(img.size, Image.AFFINE, (1, 0, t_x, 0, 1, t_y))
         # img = img.rotate(1,translate=(t_x,t_y))
-        #apply translation to boxes, cutting any that fully leave the image
-        boxes = boxes - np.array([t_x,t_y,t_x,t_y])
-        boxes[:,0] = np.clip(boxes[:,0],0,width-1) #clip x0 value
-        boxes[:,2] = np.clip(boxes[:,2],0,width-1) #clip x1 value 
-        boxes[:,1] = np.clip(boxes[:,1],0,height-1) #clip y0 value
-        boxes[:,3] = np.clip(boxes[:,3],0,height-1) #clip y1 value 
+        # apply translation to boxes, cutting any that fully leave the image
+        boxes = boxes - np.array([t_x, t_y, t_x, t_y])
+        boxes[:, 0] = np.clip(boxes[:, 0], 0, width - 1)  # clip x0 value
+        boxes[:, 2] = np.clip(boxes[:, 2], 0, width - 1)  # clip x1 value
+        boxes[:, 1] = np.clip(boxes[:, 1], 0, height - 1)  # clip y0 value
+        boxes[:, 3] = np.clip(boxes[:, 3], 0, height - 1)  # clip y1 value
         for i in range(len(classes)):
-            #if box moved out of image, set label to 0
-            if(abs(boxes[i,0] - boxes[i,2]) < 0.5 or abs(boxes[i,1] - boxes[i,3]) < 0.5):
-                classes[i] = 0  #reset label
-                boxes[i,:] = np.array([0,1,0,1]) #reset to valid box coords
-
-
-        #=== random brightness, hue, saturation changes ===
+            # if box moved out of image, set label to 0
+            if (
+                abs(boxes[i, 0] - boxes[i, 2]) < 0.5
+                or abs(boxes[i, 1] - boxes[i, 3]) < 0.5
+            ):
+                classes[i] = 0  # reset label
+                boxes[i, :] = np.array([0, 1, 0, 1])  # reset to valid box coords
+
+        # === random brightness, hue, saturation changes ===
         brighten = ImageEnhance.Brightness(img)
-        img = brighten.enhance(np.random.uniform(self.brightness[0],self.brightness[1],size=1))
+        img = brighten.enhance(
+            np.random.uniform(self.brightness[0], self.brightness[1], size=1)
+        )
 
         sharpen = ImageEnhance.Sharpness(img)
-        img = sharpen.enhance(np.random.uniform(self.sharpness[0],self.sharpness[1],size=1))
+        img = sharpen.enhance(
+            np.random.uniform(self.sharpness[0], self.sharpness[1], size=1)
+        )
 
         saturate = ImageEnhance.Color(img)
-        img = saturate.enhance(np.random.uniform(self.saturation[0],self.saturation[1],size=1))
+        img = saturate.enhance(
+            np.random.uniform(self.saturation[0], self.saturation[1], size=1)
+        )
 
         contrast = ImageEnhance.Contrast(img)
-        img = saturate.enhance(np.random.uniform(self.contrast[0],self.contrast[1],size=1))
-
-        return img,boxes,classes
+        img = saturate.enhance(
+            np.random.uniform(self.contrast[0], self.contrast[1], size=1)
+        )
 
+        return img, boxes, classes
 
     def __getitem__(self, idx):
         if torch.is_tensor(idx):
             idx = idx.tolist()
 
         # load files
-        img = Image.open(self.imgs[idx])        
-        boxes = np.asarray([.5, .5, .2, .2]*self.max_boxes).reshape((self.max_boxes, 4))
+        img = Image.open(self.imgs[idx])
+        boxes = np.asarray([0.5, 0.5, 0.2, 0.2] * self.max_boxes).reshape(
+            (self.max_boxes, 4)
+        )
         classes = np.zeros(self.max_boxes)
 
-        #if the boxes file doesn't exist, it means there were no boxes in that image
-        if(not os.path.exists(self.labels[idx])):
-            #ensure correct datatypes
-            img = np.asarray(img)/255.0
+        # if the boxes file doesn't exist, it means there were no boxes in that image
+        if not os.path.exists(self.labels[idx]):
+            # ensure correct datatypes
+            img = np.asarray(img) / 255.0
             img = np.transpose(img, (2, 0, 1)).astype(np.float32)[0:3, :, :]
-            boxes = np.asarray([0, 0, 1, 1]*self.max_boxes).reshape((self.max_boxes, 4))
+            boxes = np.asarray([0, 0, 1, 1] * self.max_boxes).reshape(
+                (self.max_boxes, 4)
+            )
             boxes = boxes.astype(np.int32)
             classes = classes.astype(np.int64)
-            return img,boxes,classes
+            return img, boxes, classes
 
-        classes_and_boxes = np.loadtxt(self.labels[idx]).reshape(-1,5)
+        classes_and_boxes = np.loadtxt(self.labels[idx]).reshape(-1, 5)
 
-        classes[0:classes_and_boxes.shape[0]] = classes_and_boxes[:,0] + 1
-        boxes[0:classes_and_boxes.shape[0],:] = classes_and_boxes[:,1:5]
+        classes[0 : classes_and_boxes.shape[0]] = classes_and_boxes[:, 0] + 1
+        boxes[0 : classes_and_boxes.shape[0], :] = classes_and_boxes[:, 1:5]
 
-        #convert normalized boxes to index-based boxes
+        # convert normalized boxes to index-based boxes
         height = np.asarray(img).shape[0]
         width = np.asarray(img).shape[1]
 
-        center_x = boxes[:,0].copy()
-        center_y = boxes[:,1].copy()
-        size_x = boxes[:,2].copy()
-        size_y = boxes[:,3].copy()
-
-        boxes[:,0] = np.clip(np.round((center_x-size_x/2) * width),0,width-2)
-        boxes[:,1] = np.clip(np.round((center_y-size_y/2) * height),0,height-2)
-        boxes[:,2] = np.clip(np.round((center_x+size_x/2) * width),boxes[:,0]+1,width-1)
-        boxes[:,3] = np.clip(np.round((center_y+size_y/2) * height),boxes[:,1]+1,height-1)
-        
-        #ensure correct datatypes and formats
+        center_x = boxes[:, 0].copy()
+        center_y = boxes[:, 1].copy()
+        size_x = boxes[:, 2].copy()
+        size_y = boxes[:, 3].copy()
+
+        boxes[:, 0] = np.clip(np.round((center_x - size_x / 2) * width), 0, width - 2)
+        boxes[:, 1] = np.clip(np.round((center_y - size_y / 2) * height), 0, height - 2)
+        boxes[:, 2] = np.clip(
+            np.round((center_x + size_x / 2) * width), boxes[:, 0] + 1, width - 1
+        )
+        boxes[:, 3] = np.clip(
+            np.round((center_y + size_y / 2) * height), boxes[:, 1] + 1, height - 1
+        )
+
+        # ensure correct datatypes and formats
         boxes = boxes.astype(np.int32)
         classes = classes.astype(np.int64)
 
-        #apply transforms
-        if(self.use_transforms):
-            img,boxes,classes = self.ApplyTransforms(img,boxes,classes)
+        # apply transforms
+        if self.use_transforms:
+            img, boxes, classes = self.ApplyTransforms(img, boxes, classes)
 
-
-        #TEMPORARILY BLUR IMAGES
+        # TEMPORARILY BLUR IMAGES
         # img = img.filter(ImageFilter.GaussianBlur(radius=1))
 
-        #ensure correct image format and channels first        
-        img = np.asarray(img)/255.0
+        # ensure correct image format and channels first
+        img = np.asarray(img) / 255.0
         img = np.transpose(img, (2, 0, 1)).astype(np.float32)[0:3, :, :]
 
         # fig, ax = plt.subplots()
@@ -348,5 +396,4 @@ def __getitem__(self, idx):
         # cv2.waitKey(0) # waits until a key is pressed
         # cv2.destroyAllWindows() # destroys the window showing image
 
-
         return img, boxes, classes
diff --git a/workspace/src/perception/cone_detector/cone_detector/object_recognition.py b/workspace/src/perception/cone_detector/cone_detector/object_recognition.py
index 913035f0..253dbf27 100644
--- a/workspace/src/perception/cone_detector/cone_detector/object_recognition.py
+++ b/workspace/src/perception/cone_detector/cone_detector/object_recognition.py
@@ -50,14 +50,15 @@
 import os
 import json
 
-ament_tools_root = os.path.join(os.path.dirname(__file__), '.')
+ament_tools_root = os.path.join(os.path.dirname(__file__), ".")
 sys.path.insert(0, os.path.abspath(ament_tools_root))
 
 from recognition_network import RecognitionNetwork
 
+
 class ObjectRecognitionNode(Node):
     def __init__(self):
-        super().__init__('object_recognition_node')
+        super().__init__("object_recognition_node")
 
         # update frequency of this node
         self.freq = 50.0
@@ -67,41 +68,53 @@ def __init__(self):
         # data that will be used by this class
         self.state = ""
         self.prediction = {}
-        self.prediction['boxes'] = torch.tensor([])
-        self.prediction['labels'] = torch.tensor([])
-        self.prediction['scores'] = torch.tensor([])
+        self.prediction["boxes"] = torch.tensor([])
+        self.prediction["labels"] = torch.tensor([])
+        self.prediction["scores"] = torch.tensor([])
 
         self.threshold = 0.001
         self.device = "cuda" if torch.cuda.is_available() else "cpu"
 
         # READ IN SHARE DIRECTORY LOCATION
-        package_share_directory = get_package_share_directory('cone_detector')
+        package_share_directory = get_package_share_directory("cone_detector")
 
         # READ IN PARAMETERS
-        self.declare_parameter('model', "")
-        self.model_file = self.get_parameter('model').get_parameter_value().string_value
-        self.declare_parameter('vis', False)
-        self.vis = self.get_parameter('vis').get_parameter_value().bool_value
-        self.declare_parameter('camera_calibration_file', "")
-        self.camera_calibration_file = self.get_parameter('camera_calibration_file').get_parameter_value().string_value
-
-        self.camera_params = json.load(open(os.path.join(package_share_directory,self.camera_calibration_file)))
+        self.declare_parameter("model", "")
+        self.model_file = self.get_parameter("model").get_parameter_value().string_value
+        self.declare_parameter("vis", False)
+        self.vis = self.get_parameter("vis").get_parameter_value().bool_value
+        self.declare_parameter("camera_calibration_file", "")
+        self.camera_calibration_file = (
+            self.get_parameter("camera_calibration_file")
+            .get_parameter_value()
+            .string_value
+        )
+
+        self.camera_params = json.load(
+            open(os.path.join(package_share_directory, self.camera_calibration_file))
+        )
         self.get_logger().info("Cam params '%s'" % str(self.camera_params))
 
         # publishers and subscribers
         qos_profile = QoSProfile(depth=1)
         qos_profile.history = QoSHistoryPolicy.KEEP_LAST
-        self.sub_image = self.create_subscription(Image, '~/input/image', self.image_callback, qos_profile)
-        self.sub_state = self.create_subscription(VehicleState, '~/input/vehicle_state', self.state_callback, qos_profile)
-        self.pub_objects = self.create_publisher(ObjectArray, '~/output/objects', 10)
-        self.timer = self.create_timer(1/self.freq, self.pub_callback)
+        self.sub_image = self.create_subscription(
+            Image, "~/input/image", self.image_callback, qos_profile
+        )
+        self.sub_state = self.create_subscription(
+            VehicleState, "~/input/vehicle_state", self.state_callback, qos_profile
+        )
+        self.pub_objects = self.create_publisher(ObjectArray, "~/output/objects", 10)
+        self.timer = self.create_timer(1 / self.freq, self.pub_callback)
 
         # object recognition
         self.model = RecognitionNetwork()
-        self.model.load(os.path.join(package_share_directory,self.model_file))
+        self.model.load(os.path.join(package_share_directory, self.model_file))
         self.model.eval()
-        self.get_logger().info('Model initialized | visualizing = %s | device = %s' % (str(self.vis),str(self.device)))
-
+        self.get_logger().info(
+            "Model initialized | visualizing = %s | device = %s"
+            % (str(self.vis), str(self.device))
+        )
 
         # nn optimizations
         # torch._C._jit_set_bailout_depth(1)
@@ -109,16 +122,20 @@ def __init__(self):
         torch.backends.cudnn.benchmark = True
         torch.backends.cudnn.enabled = True
 
-        if(torch.cuda.is_available()):
+        if torch.cuda.is_available():
             self.model.model.half()
 
-        #run a first test for optimization
-        dummy_input = torch.rand((3,720,1280),dtype=torch.float32,device=self.device)
-        if(torch.cuda.is_available()):
-            dummy_input = torch.rand((3,720,1280),dtype=torch.float16,device=self.device)
+        # run a first test for optimization
+        dummy_input = torch.rand(
+            (3, 720, 1280), dtype=torch.float32, device=self.device
+        )
+        if torch.cuda.is_available():
+            dummy_input = torch.rand(
+                (3, 720, 1280), dtype=torch.float16, device=self.device
+            )
         self.model.predict([dummy_input])
 
-        if(self.vis):
+        if self.vis:
             matplotlib.use("TKAgg")
             self.fig, self.ax = plt.subplots()
             plt.title("Object Recognition")
@@ -138,23 +155,26 @@ def image_callback(self, msg):
         self.image = msg
 
         t0 = time.time()
-        
+
         h = self.image.height
         w = self.image.width
         # self.get_logger().info("Image msg data: {} x {}".format(w,h))
-        x = np.asarray(self.image.data).reshape(
-            h, w, -1).astype(np.float32) / 255.0
+        x = np.asarray(self.image.data).reshape(h, w, -1).astype(np.float32) / 255.0
 
-        x = np.flip(x,axis=0).copy()
+        x = np.flip(x, axis=0).copy()
 
         if self.image.encoding == "bgr8":
-            x = np.flip(x[:,:,0:3],axis=2).copy()
+            x = np.flip(x[:, :, 0:3], axis=2).copy()
 
-        torch_img = None 
+        torch_img = None
         if torch.cuda.is_available():
-            torch_img = torch.from_numpy(x.transpose(2, 0, 1)[0:3, :, :]).half().to(self.device)
+            torch_img = (
+                torch.from_numpy(x.transpose(2, 0, 1)[0:3, :, :]).half().to(self.device)
+            )
         else:
-            torch_img = torch.from_numpy(x.transpose(2, 0, 1)[0:3, :, :]).to(self.device)
+            torch_img = torch.from_numpy(x.transpose(2, 0, 1)[0:3, :, :]).to(
+                self.device
+            )
 
         if self.vis:
             if self.im_show == None:
@@ -171,103 +191,126 @@ def image_callback(self, msg):
         # t_msg = self.get_clock().now()
         t_msg = rclpy.time.Time.from_msg(self.image.header.stamp)
         collection_to_perception = (t.nanoseconds - t_msg.nanoseconds) / 1e9
-        self.get_logger().info('Inference= %s, Col2Perc= %s, ID= %s' % ("{:.4f}".format(t2-t1),"{:.4f}".format(collection_to_perception),self.image.header.frame_id))
-
+        self.get_logger().info(
+            "Inference= %s, Col2Perc= %s, ID= %s"
+            % (
+                "{:.4f}".format(t2 - t1),
+                "{:.4f}".format(collection_to_perception),
+                self.image.header.frame_id,
+            )
+        )
 
     def estimate_cone_distance(self, rectangle):
         return 1.0
 
     def direction_to_pixel(self, px):
-
         img_w = self.camera_params["width"]
         img_h = self.camera_params["height"]
         img_fov = self.camera_params["FOV"]
 
-        pt_x = -((px[0]+.5) / img_w * 2 - 1) # -1 to 1
-        pt_y = -((px[1]+.5) / img_h * 2 - 1) # -1 to 1 (flipped so -1 is bottom of image)
+        pt_x = -((px[0] + 0.5) / img_w * 2 - 1)  # -1 to 1
+        pt_y = -(
+            (px[1] + 0.5) / img_h * 2 - 1
+        )  # -1 to 1 (flipped so -1 is bottom of image)
         pt_y *= img_h / img_w
 
         h_factor = img_fov / np.pi * 2.0
 
-        direction = np.array([1,pt_x*h_factor,pt_y*h_factor])
+        direction = np.array([1, pt_x * h_factor, pt_y * h_factor])
         direction = direction / np.linalg.norm(direction)
 
-
         return direction
 
     def calculate_position_from_box(self, rectangle):
-
-        top_px = np.array([.5*(rectangle[0] + rectangle[2]),rectangle[1]])
-        bottom_px = np.array([.5*(rectangle[0] + rectangle[2]),rectangle[3]])
+        top_px = np.array([0.5 * (rectangle[0] + rectangle[2]), rectangle[1]])
+        bottom_px = np.array([0.5 * (rectangle[0] + rectangle[2]), rectangle[3]])
 
         r1 = self.direction_to_pixel(top_px)
         r2 = self.direction_to_pixel(bottom_px)
 
-        h = .078
+        h = 0.078
 
-        l2 = h / ( r1[2]*r2[0] / r1[0] - r2[2] )        
-        p2 = r2*l2
+        l2 = h / (r1[2] * r2[0] / r1[0] - r2[2])
+        p2 = r2 * l2
 
         mount_pos = np.asarray(self.camera_params["position"])
-        mount_rot = np.asarray(self.camera_params["orientation"]).reshape((3,3))
+        mount_rot = np.asarray(self.camera_params["orientation"]).reshape((3, 3))
 
         # self.get_logger().info('Dir: Top=(%s), Bot=(%s)' % (str(r1), str(r2)))
 
-        p2 = np.matmul(mount_rot,p2) + mount_pos
+        p2 = np.matmul(mount_rot, p2) + mount_pos
 
         return p2
 
     # callback to run a loop and publish data this class generates
     def pub_callback(self):
-        if(not self.go):
+        if not self.go:
             return
         msg = ObjectArray()
 
-        boxes = self.prediction['boxes'].detach().cpu().numpy()
-        labels = self.prediction['labels'].detach().cpu().numpy()
-        scores = self.prediction['scores'].detach().cpu().numpy()
-        
+        boxes = self.prediction["boxes"].detach().cpu().numpy()
+        labels = self.prediction["labels"].detach().cpu().numpy()
+        scores = self.prediction["scores"].detach().cpu().numpy()
+
         t0 = time.time()
 
-        #clear old rectangles
-        if(self.vis):
+        # clear old rectangles
+        if self.vis:
             [p.remove() for p in self.patches]
             self.patches.clear()
             self.ax.texts.clear()
 
-        for b in range(boxes.shape[0]):    
-            if(scores[b] > self.threshold and labels[b]>0):
+        for b in range(boxes.shape[0]):
+            if scores[b] > self.threshold and labels[b] > 0:
                 obj = Object()
 
-                position = self.calculate_position_from_box(boxes[b, :].astype(np.float64))
+                position = self.calculate_position_from_box(
+                    boxes[b, :].astype(np.float64)
+                )
                 obj.pose.position.x = position[0]
                 obj.pose.position.y = position[1]
                 obj.pose.position.z = position[2]
                 obj.classification.classification = int(labels[b])
                 msg.objects.append(obj)
-                
-                if(self.vis):
-                    color = 'r' if int(labels[b])==1 else 'g'
-                    rect = patches.Rectangle((boxes[b, 0], boxes[b, 1]), boxes[b, 2]-boxes[b, 0], boxes[b, 3]-boxes[b,1], linewidth=1, edgecolor=color, facecolor='none')
+
+                if self.vis:
+                    color = "r" if int(labels[b]) == 1 else "g"
+                    rect = patches.Rectangle(
+                        (boxes[b, 0], boxes[b, 1]),
+                        boxes[b, 2] - boxes[b, 0],
+                        boxes[b, 3] - boxes[b, 1],
+                        linewidth=1,
+                        edgecolor=color,
+                        facecolor="none",
+                    )
                     self.ax.add_patch(rect)
                     self.patches.append(rect)
 
-                    self.ax.text(boxes[b, 0], boxes[b, 1], "{:.2f}".format(scores[b]), fontsize=8)
-                    self.ax.text(boxes[b, 0], boxes[b, 3], "{:.2f},{:.2f},{:.2f}".format(
-                        position[0],position[1],position[2]), fontsize=8)
+                    self.ax.text(
+                        boxes[b, 0], boxes[b, 1], "{:.2f}".format(scores[b]), fontsize=8
+                    )
+                    self.ax.text(
+                        boxes[b, 0],
+                        boxes[b, 3],
+                        "{:.2f},{:.2f},{:.2f}".format(
+                            position[0], position[1], position[2]
+                        ),
+                        fontsize=8,
+                    )
 
-        if(self.vis):
+        if self.vis:
             plt.draw()
             plt.pause(0.0001)
             self.counter += 1
 
-        t1 = time.time()    
+        t1 = time.time()
         # self.get_logger().info('Displaying Time = %s' % str(t1-t0))
 
         # Add header for timestamps
         msg.header.stamp = self.get_clock().now().to_msg()
         self.pub_objects.publish(msg)
 
+
 def main(args=None):
     rclpy.init(args=args)
     recognition = ObjectRecognitionNode()
@@ -275,5 +318,6 @@ def main(args=None):
     recognition.destroy_node()
     rclpy.shutdown()
 
-if __name__ == '__main__':
+
+if __name__ == "__main__":
     main()
diff --git a/workspace/src/perception/cone_detector/cone_detector/recognition_network.py b/workspace/src/perception/cone_detector/cone_detector/recognition_network.py
index 98e8fe84..7eab8773 100644
--- a/workspace/src/perception/cone_detector/cone_detector/recognition_network.py
+++ b/workspace/src/perception/cone_detector/cone_detector/recognition_network.py
@@ -50,7 +50,8 @@
 
 from loader import *
 
-class RecognitionNetwork():
+
+class RecognitionNetwork:
     def __init__(self):
         # network parameters
         num_classes = 3  # background, red cones, green cones
@@ -61,17 +62,30 @@ def __init__(self):
         trainable_layers = 0
         pretrained = False
         pretrained_backbone = True
-        self.device = torch.device("cuda") if torch.cuda.is_available() else torch.device("cpu")
+        self.device = (
+            torch.device("cuda") if torch.cuda.is_available() else torch.device("cpu")
+        )
 
-        #fasterrcnn_mobilenet_v3_large_fpn
-        #fasterrcnn_mobilenet_v3_large_320_fpn
+        # fasterrcnn_mobilenet_v3_large_fpn
+        # fasterrcnn_mobilenet_v3_large_320_fpn
         self.model = fasterrcnn_mobilenet_v3_large_320_fpn(
-            pretrained=pretrained, pretrained_backbone=pretrained_backbone, 
-            trainable_backbone_layers=trainable_layers, num_classes=num_classes, 
-            min_size=min_size, max_size=max_size, box_score_thresh=box_score_thresh,
-            box_detections_per_img=box_detections_per_img)
-
-        self.model.transform = GeneralizedRCNNTransform(min_size,max_size,(0.485, 0.456, 0.406), (0.229, 0.224, 0.225),fixed_size=(max_size,min_size))
+            pretrained=pretrained,
+            pretrained_backbone=pretrained_backbone,
+            trainable_backbone_layers=trainable_layers,
+            num_classes=num_classes,
+            min_size=min_size,
+            max_size=max_size,
+            box_score_thresh=box_score_thresh,
+            box_detections_per_img=box_detections_per_img,
+        )
+
+        self.model.transform = GeneralizedRCNNTransform(
+            min_size,
+            max_size,
+            (0.485, 0.456, 0.406),
+            (0.229, 0.224, 0.225),
+            fixed_size=(max_size, min_size),
+        )
 
         self.model.to(self.device)
 
@@ -108,10 +122,10 @@ def train_single_epoch(self, train_loader, opt, accumulation_step):
             epoch_loss_total += losses.item()
 
             losses.backward()
-            if((i + 1) % accumulation_step == 0):
+            if (i + 1) % accumulation_step == 0:
                 opt.step()
                 opt.zero_grad()
-        
+
         return epoch_loss_total
 
     def eval_dataset(self, val_loader):
@@ -132,7 +146,6 @@ def eval_dataset(self, val_loader):
                 img_list.append(img_tensor)
                 target_list.append(target_dict)
 
-
             # fig, ax = plt.subplots()
             # ax.imshow(img_list[0].cpu().numpy().transpose((1, 2, 0)))
             # for b in boxes[0,labels[0,:]>0,:]:
@@ -168,7 +181,7 @@ def evaluate_iou(self, data_loader, samples):
 
             prediction = self.predict(img_list)
 
-            #DEBUG INFO
+            # DEBUG INFO
             # fig, ax = plt.subplots()
             # gt_boxes = target_list[0]["boxes"].cpu().detach().numpy()
             # gt_labels = target_list[0]["labels"].cpu().detach().numpy()
@@ -176,87 +189,87 @@ def evaluate_iou(self, data_loader, samples):
             # pred_labels = prediction[0]["labels"].cpu().detach().numpy()
 
             # ax.imshow(img_list[0].cpu().detach().numpy().transpose((1,2,0)))
-            # for b in range(gt_boxes.shape[0]):    
+            # for b in range(gt_boxes.shape[0]):
             #     color = 'b'
             #     rect = patches.Rectangle((gt_boxes[b, 0], gt_boxes[b, 1]), gt_boxes[b, 2]-gt_boxes[b, 0], gt_boxes[b, 3]-gt_boxes[b,1], linewidth=2, edgecolor=color, facecolor='none')
             #     ax.add_patch(rect)
 
-            # for b in range(pred_boxes.shape[0]):    
+            # for b in range(pred_boxes.shape[0]):
             #     color = 'r' if int(pred_labels[b])==1 else 'g'
             #     rect = patches.Rectangle((pred_boxes[b, 0], pred_boxes[b, 1]), pred_boxes[b, 2]-pred_boxes[b, 0], pred_boxes[b, 3]-pred_boxes[b,1], linewidth=2, edgecolor=color, facecolor='none')
             #     ax.add_patch(rect)
             # plt.show()
 
-            #perform box matching for each class
+            # perform box matching for each class
             gt_boxes = target_list[0]["boxes"].cpu().detach().numpy()
             gt_labels = target_list[0]["labels"].cpu().detach().numpy()
             pred_boxes = prediction[0]["boxes"].cpu().detach().numpy()
             pred_labels = prediction[0]["labels"].cpu().detach().numpy()
 
-            # do matching on all classes together to include mislabeling in error metrics 
-            gt_boxes = gt_boxes[gt_labels>0]
-            gt_labels = gt_labels[gt_labels>0]
-            pred_boxes = pred_boxes[pred_labels>0]
-            pred_labels = pred_labels[pred_labels>0]
+            # do matching on all classes together to include mislabeling in error metrics
+            gt_boxes = gt_boxes[gt_labels > 0]
+            gt_labels = gt_labels[gt_labels > 0]
+            pred_boxes = pred_boxes[pred_labels > 0]
+            pred_labels = pred_labels[pred_labels > 0]
 
-            iou_matrix = np.zeros((len(pred_labels),len(gt_labels)))
+            iou_matrix = np.zeros((len(pred_labels), len(gt_labels)))
 
-            #calculate iou for predicted box vs each gt box
+            # calculate iou for predicted box vs each gt box
             for p in range(len(pred_labels)):
                 for g in range(len(gt_labels)):
-                    gx0,gy0,gx1,gy1 = gt_boxes[g,:].astype(np.int32)
-                    px0,py0,px1,py1 = pred_boxes[p,:].astype(np.int32)
+                    gx0, gy0, gx1, gy1 = gt_boxes[g, :].astype(np.int32)
+                    px0, py0, px1, py1 = pred_boxes[p, :].astype(np.int32)
 
-                    #do mask comparison
-                    mask = np.zeros((self.w,self.h)).astype(np.int32)
-                    mask[gx0:gx1,gy0:gy1] += 1
-                    mask[px0:px1,py0:py1] += 1
+                    # do mask comparison
+                    mask = np.zeros((self.w, self.h)).astype(np.int32)
+                    mask[gx0:gx1, gy0:gy1] += 1
+                    mask[px0:px1, py0:py1] += 1
 
-                    intersection = np.count_nonzero(mask[mask==2])
-                    union = np.count_nonzero(mask[mask>0])
+                    intersection = np.count_nonzero(mask[mask == 2])
+                    union = np.count_nonzero(mask[mask > 0])
 
-                    iou_matrix[p,g] = intersection / float(union)
+                    iou_matrix[p, g] = intersection / float(union)
 
-            #greedily find the max IOU to match boxes
+            # greedily find the max IOU to match boxes
             pairs = []
 
             gt_ids = list(range(len(gt_labels)))
             pred_ids = list(range(len(pred_labels)))
-            
-            if(len(pred_labels) > 0 and len(gt_labels) > 0):
+
+            if len(pred_labels) > 0 and len(gt_labels) > 0:
                 for p in range(len(pred_labels)):
-                    if(np.max(iou_matrix) < 1e-9):
+                    if np.max(iou_matrix) < 1e-9:
                         break
 
                     id_max = np.unravel_index(iou_matrix.argmax(), iou_matrix.shape)
                     pairs.append(id_max)
                     pred_ids.remove(id_max[0])
                     gt_ids.remove(id_max[1])
-                    #zero out the row and column selected
-                    iou_matrix[id_max[0],:] = -1
-                    iou_matrix[:,id_max[1]] = -1
+                    # zero out the row and column selected
+                    iou_matrix[id_max[0], :] = -1
+                    iou_matrix[:, id_max[1]] = -1
 
-            #handle any remaining predictions or gt boxes with no pairs
+            # handle any remaining predictions or gt boxes with no pairs
             for p in pred_ids:
-                pairs.append((p,-1))
+                pairs.append((p, -1))
             for g in gt_ids:
-                pairs.append((-1,g))      
+                pairs.append((-1, g))
 
-            for p,pair in enumerate(pairs):
+            for p, pair in enumerate(pairs):
                 iou = 0
                 p_id = pair[0]
                 g_id = pair[1]
-                if(p_id >= 0 and g_id >= 0):
-                    mask = np.zeros((self.w,self.h)).astype(np.int32)
-                    mask[gx0:gx1,gy0:gy1] += 1
-                    mask[px0:px1,py0:py1] += 1
+                if p_id >= 0 and g_id >= 0:
+                    mask = np.zeros((self.w, self.h)).astype(np.int32)
+                    mask[gx0:gx1, gy0:gy1] += 1
+                    mask[px0:px1, py0:py1] += 1
 
-                    intersection = np.count_nonzero(mask[mask==2])
-                    union = np.count_nonzero(mask[mask>0])
+                    intersection = np.count_nonzero(mask[mask == 2])
+                    union = np.count_nonzero(mask[mask > 0])
                     iou = intersection / union
                 iou_list.append(iou)
 
-            if(i >= samples):
+            if i >= samples:
                 break
                 # print("IOU:",iou_list)
                 # return np.mean(iou_list)
@@ -264,9 +277,20 @@ def evaluate_iou(self, data_loader, samples):
         # print("IOU:",iou_list)
         return np.mean(iou_list)
 
-    def train(self, train_loader, val_loader, epochs=1, lr=.001, use_scheduler=False, accumulation_step=4, scheduler_step=1, output_path="output", save_interval=10):
+    def train(
+        self,
+        train_loader,
+        val_loader,
+        epochs=1,
+        lr=0.001,
+        use_scheduler=False,
+        accumulation_step=4,
+        scheduler_step=1,
+        output_path="output",
+        save_interval=10,
+    ):
         self.model.train()
-        if(not os.path.exists(output_path)):
+        if not os.path.exists(output_path):
             os.mkdir(output_path)
 
         params = [p for p in self.model.parameters() if p.requires_grad]
@@ -276,13 +300,15 @@ def train(self, train_loader, val_loader, epochs=1, lr=.001, use_scheduler=False
         validation_samples = 2000
         output_samples = 100
 
-        if(val_loader==None):
+        if val_loader == None:
             val_loader = train_loader
 
         for e in range(epochs):
             self.model.train()
-            train_loss_total = self.train_single_epoch(train_loader, opt, accumulation_step)
-            if(use_scheduler and (e+1) % scheduler_step == 0):
+            train_loss_total = self.train_single_epoch(
+                train_loader, opt, accumulation_step
+            )
+            if use_scheduler and (e + 1) % scheduler_step == 0:
                 scheduler.step()
 
             val_loss_total = self.eval_dataset(val_loader)
@@ -290,12 +316,18 @@ def train(self, train_loader, val_loader, epochs=1, lr=.001, use_scheduler=False
             train_iou = self.evaluate_iou(train_loader, len(val_loader))
             val_iou = self.evaluate_iou(val_loader, len(val_loader))
 
-
-            print("Epoch [{}/{}], Lr [{:.6f}], Train Loss [{:.4f}], Val loss [{:.4f}, Train IOU [{:.4f}], Val IOU [{:.4f}]".format(e+1, epochs, scheduler.get_last_lr()[0],
-                                                                                     train_loss_total / len(train_loader),
-                                                                                     val_loss_total / len(val_loader),
-                                                                                     train_iou, val_iou))
-            if((e+1) % save_interval == 0):
+            print(
+                "Epoch [{}/{}], Lr [{:.6f}], Train Loss [{:.4f}], Val loss [{:.4f}, Train IOU [{:.4f}], Val IOU [{:.4f}]".format(
+                    e + 1,
+                    epochs,
+                    scheduler.get_last_lr()[0],
+                    train_loss_total / len(train_loader),
+                    val_loss_total / len(val_loader),
+                    train_iou,
+                    val_iou,
+                )
+            )
+            if (e + 1) % save_interval == 0:
                 self.save(output_path)
             # # get an example prediction and show it after each epoch
             # if((e+1) % display_interval == 0):
@@ -356,25 +388,23 @@ def train(self, train_loader, val_loader, epochs=1, lr=.001, use_scheduler=False
             #             break
 
     def load(self, path):
-        self.model.load_state_dict(torch.load(path,map_location=self.device))
+        self.model.load_state_dict(torch.load(path, map_location=self.device))
         print("Loaded pretrained model at {}".format(path))
 
-    def save(self,output_dir="output",model_name="model"):
-        if(not os.path.exists(output_dir)):
+    def save(self, output_dir="output", model_name="model"):
+        if not os.path.exists(output_dir):
             os.mkdir(output_dir)
 
-        torch.save(self.model.state_dict(), os.path.join(output_dir,model_name))
+        torch.save(self.model.state_dict(), os.path.join(output_dir, model_name))
 
-    def export(self,output_path="output",name="model.onnx",w=1280,h=720):
+    def export(self, output_path="output", name="model.onnx", w=1280, h=720):
         print("Starting export to onnx")
         # if(not os.path.exists(output_path)):
         #     os.mkdir(output_path)
 
-
         # # print(self.model)
         # # print(type(self.model))
 
-        
         # self.model.cpu()
         # self.model.train()
 
@@ -385,11 +415,8 @@ def export(self,output_path="output",name="model.onnx",w=1280,h=720):
         # self.eval()
         # self.model.cpu()
 
-
-
         # tmp_model = self.model
 
-
         # # print(tmp_model.transform)
 
         # tr = torch.nn.Sequential(
@@ -400,7 +427,6 @@ def export(self,output_path="output",name="model.onnx",w=1280,h=720):
         # p = tmp_model(x)
         # print(tmp_model)
 
-
         # # Export the model
         # torch.onnx.export(tmp_model,               # model being run
         #           x,                         # model input (or a tuple for multiple inputs)
@@ -410,4 +436,4 @@ def export(self,output_path="output",name="model.onnx",w=1280,h=720):
         #           do_constant_folding=False,  # whether to execute constant folding for optimization
         #           input_names = ['input'],   # the model's input names
         #           output_names = ['output'])  # the model's output names)
-        # print("Model exported to onnx")
\ No newline at end of file
+        # print("Model exported to onnx")
diff --git a/workspace/src/perception/cone_detector/cone_detector/train.py b/workspace/src/perception/cone_detector/cone_detector/train.py
index b1439f43..0e05920e 100644
--- a/workspace/src/perception/cone_detector/cone_detector/train.py
+++ b/workspace/src/perception/cone_detector/cone_detector/train.py
@@ -35,10 +35,9 @@
 import argparse
 import sys
 
-def generate_boxes(args):
 
-    train_dataset = SegImgLoader(data_root=args.training_path,
-                                 max_samples=args.n_train)
+def generate_boxes(args):
+    train_dataset = SegImgLoader(data_root=args.training_path, max_samples=args.n_train)
 
     train_dataset.GenerateAAVBBFromSeg()
 
@@ -47,36 +46,62 @@ def train(args):
     # run training as requested
     print("=== Loading Datasets ===")
 
-    train_dataset = ObjectDetectionImgLoader(data_root=args.training_path, max_boxes=args.max_boxes,
-                                             max_samples=args.n_train,apply_transforms=True)
-    val_dataset = ObjectDetectionImgLoader(data_root=args.validation_path, max_boxes=args.max_boxes,
-                                           max_samples=args.n_val,apply_transforms=False)
+    train_dataset = ObjectDetectionImgLoader(
+        data_root=args.training_path,
+        max_boxes=args.max_boxes,
+        max_samples=args.n_train,
+        apply_transforms=True,
+    )
+    val_dataset = ObjectDetectionImgLoader(
+        data_root=args.validation_path,
+        max_boxes=args.max_boxes,
+        max_samples=args.n_val,
+        apply_transforms=False,
+    )
 
     train_loader = torch.utils.data.DataLoader(
-        train_dataset, args.batch_size, shuffle=True, num_workers=args.threads, drop_last=True)
+        train_dataset,
+        args.batch_size,
+        shuffle=True,
+        num_workers=args.threads,
+        drop_last=True,
+    )
     val_loader = torch.utils.data.DataLoader(
-        val_dataset, args.batch_size, shuffle=True, num_workers=args.threads, drop_last=True)
+        val_dataset,
+        args.batch_size,
+        shuffle=True,
+        num_workers=args.threads,
+        drop_last=True,
+    )
 
     print("=== Training ===")
 
     model = RecognitionNetwork()
-    if(os.path.exists(args.input_model)):
+    if os.path.exists(args.input_model):
         model.load(args.input_model)
-    model.train(train_loader, val_loader, epochs=args.epochs,
-                lr=args.learning_rate, use_scheduler=args.use_scheduler,
-                accumulation_step=args.acc_step, scheduler_step=args.sched_step,
-                output_path=args.output_path, save_interval=args.save_interval)
+    model.train(
+        train_loader,
+        val_loader,
+        epochs=args.epochs,
+        lr=args.learning_rate,
+        use_scheduler=args.use_scheduler,
+        accumulation_step=args.acc_step,
+        scheduler_step=args.sched_step,
+        output_path=args.output_path,
+        save_interval=args.save_interval,
+    )
 
     # model.export(output_path=args.output_path,name=args.name+".onnx",w=1280,h=720)
 
     print("=== Training Complete ===")
 
+
 def main(args):
-    #save the configuration to a bash script
-    if(not os.path.exists(args.output_path)):
+    # save the configuration to a bash script
+    if not os.path.exists(args.output_path):
         os.mkdir(args.output_path)
 
-    run_file = open(os.path.join(args.output_path,"run.sh"),'w')
+    run_file = open(os.path.join(args.output_path, "run.sh"), "w")
     run_file.write("#!/usr/bin/env bash\n")
     command = "python3 "
     for a in sys.argv:
@@ -84,15 +109,13 @@ def main(args):
     run_file.write(command + "\n")
     run_file.close()
 
-
-    if(args.mode == "train"):
+    if args.mode == "train":
         train(args)
-    elif(args.mode == "generate_boxes"):
+    elif args.mode == "generate_boxes":
         generate_boxes(args)
 
 
 def parseargs():
-
     # === DEFAULTS === #
     training_path = "/home/asher/datasets/chrono_cones/train"
     validation_path = "/home/asher/datasets/chrono_cones/val"
@@ -114,53 +137,114 @@ def parseargs():
     acc_step = 4
     sched_step = 1
 
-    parser = argparse.ArgumentParser(description='Object Recognition Trainer.')
+    parser = argparse.ArgumentParser(description="Object Recognition Trainer.")
 
     # general mode
-    parser.add_argument('--mode', default="train",
-                        choices=["train", "generate_boxes"], help='mode of use')
+    parser.add_argument(
+        "--mode",
+        default="train",
+        choices=["train", "generate_boxes"],
+        help="mode of use",
+    )
 
     # information about the network
-    parser.add_argument('--name', type=str, default='model',
-                        help="network name for saving and loading")
-    parser.add_argument('--model_type', default=model_type,
-                        choices=avail_model_types, help='the object recognition model to use')
-    parser.add_argument('--output_path', type=str,
-                        default=output_path, help="output path for model and data")
-    parser.add_argument('--input_model', type=str, default=input_model,
-                        help="a pretrained model to use for continued training")
+    parser.add_argument(
+        "--name", type=str, default="model", help="network name for saving and loading"
+    )
+    parser.add_argument(
+        "--model_type",
+        default=model_type,
+        choices=avail_model_types,
+        help="the object recognition model to use",
+    )
+    parser.add_argument(
+        "--output_path",
+        type=str,
+        default=output_path,
+        help="output path for model and data",
+    )
+    parser.add_argument(
+        "--input_model",
+        type=str,
+        default=input_model,
+        help="a pretrained model to use for continued training",
+    )
 
     # training parameters
-    parser.add_argument('--epochs', '-e', type=int,
-                        default=epochs, help="number of epochs for training")
-    parser.add_argument('--batch_size', '-b', type=int,
-                        default=batch_size, help="samples per batch")
-    parser.add_argument('--learning_rate', '-lr', type=float,
-                        default=lr, help="learning rate")
-    parser.add_argument('--use_scheduler', action='store_true',
-                        help="use a learning rate scheduler to decrease learning rate during training")
-    parser.add_argument('--save_interval', type=int, default=display_interval,
-                        help="number of epochs between displaying and saving progress during training")
-    parser.add_argument('--threads', type=int, default=loading_threads,
-                        help="worker threads for loading data")
-    parser.add_argument('--max_boxes', type=int, default=max_boxes,
-                        help="maximum number of predicted boxes possible in an image")
-    parser.add_argument('--acc_step', type=int, default=acc_step,
-                        help="accumulation steps for gradient calculations")
-    parser.add_argument('--sched_step', type=int, default=sched_step,
-                        help="scheduler steps for learning rate changes")
+    parser.add_argument(
+        "--epochs", "-e", type=int, default=epochs, help="number of epochs for training"
+    )
+    parser.add_argument(
+        "--batch_size", "-b", type=int, default=batch_size, help="samples per batch"
+    )
+    parser.add_argument(
+        "--learning_rate", "-lr", type=float, default=lr, help="learning rate"
+    )
+    parser.add_argument(
+        "--use_scheduler",
+        action="store_true",
+        help="use a learning rate scheduler to decrease learning rate during training",
+    )
+    parser.add_argument(
+        "--save_interval",
+        type=int,
+        default=display_interval,
+        help="number of epochs between displaying and saving progress during training",
+    )
+    parser.add_argument(
+        "--threads",
+        type=int,
+        default=loading_threads,
+        help="worker threads for loading data",
+    )
+    parser.add_argument(
+        "--max_boxes",
+        type=int,
+        default=max_boxes,
+        help="maximum number of predicted boxes possible in an image",
+    )
+    parser.add_argument(
+        "--acc_step",
+        type=int,
+        default=acc_step,
+        help="accumulation steps for gradient calculations",
+    )
+    parser.add_argument(
+        "--sched_step",
+        type=int,
+        default=sched_step,
+        help="scheduler steps for learning rate changes",
+    )
 
     # dataset locations
-    parser.add_argument('--training_path', '-tr_data', type=str,
-                        default=training_path, help="path to training data")
-    parser.add_argument('--validation_path', '-val_data', type=str,
-                        default=validation_path, help="path to validation data")
+    parser.add_argument(
+        "--training_path",
+        "-tr_data",
+        type=str,
+        default=training_path,
+        help="path to training data",
+    )
+    parser.add_argument(
+        "--validation_path",
+        "-val_data",
+        type=str,
+        default=validation_path,
+        help="path to validation data",
+    )
 
     # number of samples to load
-    parser.add_argument('--n_train', type=int, default=num_training_samples,
-                        help="number of samples to load for training")
-    parser.add_argument('--n_val', type=int, default=num_validation_samples,
-                        help="number of samples to load for validation")
+    parser.add_argument(
+        "--n_train",
+        type=int,
+        default=num_training_samples,
+        help="number of samples to load for training",
+    )
+    parser.add_argument(
+        "--n_val",
+        type=int,
+        default=num_validation_samples,
+        help="number of samples to load for validation",
+    )
 
     return parser.parse_args()
 
diff --git a/workspace/src/perception/cone_detector/cone_detector/yolov5_detector.py b/workspace/src/perception/cone_detector/cone_detector/yolov5_detector.py
index 15ec5d80..12865e82 100644
--- a/workspace/src/perception/cone_detector/cone_detector/yolov5_detector.py
+++ b/workspace/src/perception/cone_detector/cone_detector/yolov5_detector.py
@@ -43,6 +43,7 @@
 import matplotlib.pyplot as plt
 import matplotlib.patches as patches
 import torchvision.ops as ops
+
 # import torchvision.transforms as transforms
 import torch
 
@@ -53,12 +54,13 @@
 import os
 import json
 
-ament_tools_root = os.path.join(os.path.dirname(__file__), '.')
+ament_tools_root = os.path.join(os.path.dirname(__file__), ".")
 sys.path.insert(0, os.path.abspath(ament_tools_root))
 
+
 class YOLODetectionNode(Node):
     def __init__(self):
-        super().__init__('object_recognition_node')
+        super().__init__("object_recognition_node")
 
         # update frequency of this node
         self.freq = 50.0
@@ -68,82 +70,96 @@ def __init__(self):
         # data that will be used by this class
         self.state = ""
         self.prediction = {}
-        self.prediction['boxes'] = torch.tensor([])
-        self.prediction['labels'] = torch.tensor([])
-        self.prediction['scores'] = torch.tensor([])
+        self.prediction["boxes"] = torch.tensor([])
+        self.prediction["labels"] = torch.tensor([])
+        self.prediction["scores"] = torch.tensor([])
 
         self.threshold = 0.001
         self.device = "cuda" if torch.cuda.is_available() else "cpu"
 
-        if(not torch.cuda.is_available()):
-            self.get_logger().info('cuda support in torch is not available. Either reconfigure with cuda, or fall back to a difference perception node.')
+        if not torch.cuda.is_available():
+            self.get_logger().info(
+                "cuda support in torch is not available. Either reconfigure with cuda, or fall back to a difference perception node."
+            )
             exit(1)
 
         # READ IN SHARE DIRECTORY LOCATION
-        package_share_directory = get_package_share_directory('cone_detector')
+        package_share_directory = get_package_share_directory("cone_detector")
 
         # READ IN PARAMETERS
-        self.declare_parameter('model', "")
-        self.model_file = self.get_parameter('model').get_parameter_value().string_value
-        self.declare_parameter('vis', False)
-        self.vis = self.get_parameter('vis').get_parameter_value().bool_value
-        self.declare_parameter('camera_calibration_file', "")
-        self.camera_calibration_file = self.get_parameter('camera_calibration_file').get_parameter_value().string_value
-
-        self.camera_params = json.load(open(os.path.join(package_share_directory,self.camera_calibration_file)))
+        self.declare_parameter("model", "")
+        self.model_file = self.get_parameter("model").get_parameter_value().string_value
+        self.declare_parameter("vis", False)
+        self.vis = self.get_parameter("vis").get_parameter_value().bool_value
+        self.declare_parameter("camera_calibration_file", "")
+        self.camera_calibration_file = (
+            self.get_parameter("camera_calibration_file")
+            .get_parameter_value()
+            .string_value
+        )
+
+        self.camera_params = json.load(
+            open(os.path.join(package_share_directory, self.camera_calibration_file))
+        )
         self.get_logger().info("Cam params '%s'" % str(self.camera_params))
 
         # publishers and subscribers
         qos_profile = QoSProfile(depth=1)
         qos_profile.history = QoSHistoryPolicy.KEEP_LAST
-        self.sub_image = self.create_subscription(Image, '~/input/image', self.image_callback, qos_profile)
+        self.sub_image = self.create_subscription(
+            Image, "~/input/image", self.image_callback, qos_profile
+        )
         # self.sub_state = self.create_subscription(VehicleState, '~/input/vehicle_state', self.state_callback, qos_profile)
-        self.pub_objects = self.create_publisher(ObjectArray, '~/output/objects', 10)
-        self.timer = self.create_timer(1/self.freq, self.pub_callback)
-
+        self.pub_objects = self.create_publisher(ObjectArray, "~/output/objects", 10)
+        self.timer = self.create_timer(1 / self.freq, self.pub_callback)
 
-        
-        #initialize the TensorRT Engine
+        # initialize the TensorRT Engine
         logger = trt.Logger(trt.Logger.INFO)
         self.engine = None
 
-        onnx_file = os.path.join(package_share_directory,self.model_file)
+        onnx_file = os.path.join(package_share_directory, self.model_file)
         engine_file = onnx_file + ".engine"
-        if(os.path.exists(engine_file)):
+        if os.path.exists(engine_file):
             print("Engine file found.")
             runtime = trt.Runtime(logger)
-            with open(engine_file, 'rb') as f:
+            with open(engine_file, "rb") as f:
                 self.engine = runtime.deserialize_cuda_engine(f.read())
-            
-            if(self.engine == None):
+
+            if self.engine == None:
                 raise RuntimeError("Engine is not initialized.")
 
         else:
-            self.get_logger().info('No existing engine file found, so building from Onnx. This may take a few minutes. Next time will be much faster.')
+            self.get_logger().info(
+                "No existing engine file found, so building from Onnx. This may take a few minutes. Next time will be much faster."
+            )
 
-            if(not os.path.exists(onnx_file)):
-                raise RuntimeError("Onnx file not found"+os.path.join(onnx_file))
+            if not os.path.exists(onnx_file):
+                raise RuntimeError("Onnx file not found" + os.path.join(onnx_file))
 
-            explicit_batch = 1 << (int)(trt.NetworkDefinitionCreationFlag.EXPLICIT_BATCH)
+            explicit_batch = 1 << (int)(
+                trt.NetworkDefinitionCreationFlag.EXPLICIT_BATCH
+            )
             builder = trt.Builder(logger)
             network = builder.create_network(explicit_batch)
             parser = trt.OnnxParser(network, logger)
             config = builder.create_builder_config()
             config.set_memory_pool_limit(trt.MemoryPoolType.WORKSPACE, 1 << 30)
 
-            with open(onnx_file, 'rb') as f:
+            with open(onnx_file, "rb") as f:
                 if not parser.parse(f.read()):
                     error_str = ""
                     for error in range(parser.num_errors):
-                        error_str+=str(parser.get_error(error))
+                        error_str += str(parser.get_error(error))
                     raise RuntimeError(error_str)
-            self.engine = builder.build_engine(network,config) 
-            
-            if(self.engine == None):
-                raise RuntimeError("Engine is not initialized from file: "+os.path.join(onnx_file))
-            
-            self.get_logger().info('Saving serialized engine for faster future loads.')
-            with open(engine_file, 'wb') as f:
+            self.engine = builder.build_engine(network, config)
+
+            if self.engine == None:
+                raise RuntimeError(
+                    "Engine is not initialized from file: " + os.path.join(onnx_file)
+                )
+
+            self.get_logger().info("Saving serialized engine for faster future loads.")
+            with open(engine_file, "wb") as f:
                 f.write(self.engine.serialize())
 
         self.trt_context = self.engine.create_execution_context()
@@ -152,19 +168,25 @@ def __init__(self):
         for binding in self.engine:
             if self.engine.binding_is_input(binding):
                 input_shape = self.engine.get_binding_shape(binding)
-                input_size = trt.volume(input_shape) * self.engine.max_batch_size * np.dtype(np.float16).itemsize  # in bytes
+                input_size = (
+                    trt.volume(input_shape)
+                    * self.engine.max_batch_size
+                    * np.dtype(np.float16).itemsize
+                )  # in bytes
             else:
                 self.output_shape = self.engine.get_binding_shape(binding)
 
-        self.confidence_thresh = .5
-        self.iou_thresh = .45
+        self.confidence_thresh = 0.5
+        self.iou_thresh = 0.45
         self.max_detections = 100
 
-        self.device_output = torch.zeros(tuple(self.output_shape), device="cuda:0",dtype=torch.float16)
+        self.device_output = torch.zeros(
+            tuple(self.output_shape), device="cuda:0", dtype=torch.float16
+        )
 
-        self.get_logger().info('Model initialized | visualizing = %s' % (str(self.vis)))
+        self.get_logger().info("Model initialized | visualizing = %s" % (str(self.vis)))
 
-        if(self.vis):
+        if self.vis:
             matplotlib.use("TKAgg")
             self.fig, self.ax = plt.subplots()
             plt.title("Object Recognition")
@@ -173,33 +195,33 @@ def __init__(self):
             self.counter = 0
 
     # function to process data this class subscribes to
-    def nms(self,pred):
-
-        #multiply class predictions by scores
+    def nms(self, pred):
+        # multiply class predictions by scores
         pred[:, 5:] *= pred[:, 4:5]
 
-        #chance anchor point to corner rather than center
-        boxes =  pred[:,0:4]
-        boxes[:, 0] -= boxes[:, 2]/2
-        boxes[:, 1] -= boxes[:, 3]/2
+        # chance anchor point to corner rather than center
+        boxes = pred[:, 0:4]
+        boxes[:, 0] -= boxes[:, 2] / 2
+        boxes[:, 1] -= boxes[:, 3] / 2
         boxes[:, 2] += boxes[:, 0]
         boxes[:, 3] += boxes[:, 1]
 
-        #get the confidence and class id
-        scores,class_id = pred[:, 5:].max(1, keepdims=False)
+        # get the confidence and class id
+        scores, class_id = pred[:, 5:].max(1, keepdims=False)
 
-        boxes = boxes[scores>self.confidence_thresh,:]
-        class_id = class_id[scores>self.confidence_thresh]
-        scores = scores[scores>self.confidence_thresh]
+        boxes = boxes[scores > self.confidence_thresh, :]
+        class_id = class_id[scores > self.confidence_thresh]
+        scores = scores[scores > self.confidence_thresh]
 
-        #have torchvision to nms
-        res = ops.batched_nms(boxes=boxes, scores=scores, idxs=class_id, iou_threshold=self.iou_thresh)
+        # have torchvision to nms
+        res = ops.batched_nms(
+            boxes=boxes, scores=scores, idxs=class_id, iou_threshold=self.iou_thresh
+        )
 
-        if(res.shape[0]>self.max_detections):
-            res = res[:self.max_detections]
-
-        return boxes[res,:],scores[res],class_id[res]
+        if res.shape[0] > self.max_detections:
+            res = res[: self.max_detections]
 
+        return boxes[res, :], scores[res], class_id[res]
 
     def state_callback(self, msg):
         # self.get_logger().info("Received '%s'" % msg)
@@ -211,31 +233,32 @@ def image_callback(self, msg):
         self.image = msg
 
         t0 = time.time()
-        
+
         h = self.image.height
         w = self.image.width
 
-        img = np.asarray(self.image.data).reshape(
-            h, w, -1).astype(np.float32) / 255.0
+        img = np.asarray(self.image.data).reshape(h, w, -1).astype(np.float32) / 255.0
         if self.image.encoding == "bgr8":
-            img = np.flip(img[:,:,0:3],axis=2)
-        img = img[:704,:,0:3]
-        device_input = torch.from_numpy(img).cuda().half().permute(2,0,1).unsqueeze(0)
+            img = np.flip(img[:, :, 0:3], axis=2)
+        img = img[:704, :, 0:3]
+        device_input = torch.from_numpy(img).cuda().half().permute(2, 0, 1).unsqueeze(0)
 
         # img = torch.HalfTensor(self.image.data).reshape(h, w, -1) / 255.0
         # if self.image.encoding == "bgr8":
         #     img = img.flip([2])
         # img = img.cuda()
         # img = img[720-704:,:,0:3]
-        # device_input = img.permute(2,0,1).unsqueeze(0) 
-                
+        # device_input = img.permute(2,0,1).unsqueeze(0)
+
         device_input = device_input.contiguous()
 
         t1 = time.time()
 
-        self.trt_context.execute_v2(bindings=[device_input.data_ptr(), self.device_output.data_ptr()])
+        self.trt_context.execute_v2(
+            bindings=[device_input.data_ptr(), self.device_output.data_ptr()]
+        )
 
-        boxes,scores,class_id = self.nms(self.device_output[0,:,:])
+        boxes, scores, class_id = self.nms(self.device_output[0, :, :])
 
         self.boxes = boxes.cpu().numpy()
         self.scores = scores.cpu().numpy()
@@ -253,95 +276,105 @@ def image_callback(self, msg):
         # t_msg = self.get_clock().now()
         t_msg = rclpy.time.Time.from_msg(self.image.header.stamp)
         collection_to_perception = (t.nanoseconds - t_msg.nanoseconds) / 1e9
-        self.get_logger().info('Prep= %s, Inference= %s, Col2Perc= %s, ID= %s' % ("{:.4f}".format(t1-t0),"{:.4f}".format(t2-t1),"{:.4f}".format(collection_to_perception),self.image.header.frame_id))
-
+        self.get_logger().info(
+            "Prep= %s, Inference= %s, Col2Perc= %s, ID= %s"
+            % (
+                "{:.4f}".format(t1 - t0),
+                "{:.4f}".format(t2 - t1),
+                "{:.4f}".format(collection_to_perception),
+                self.image.header.frame_id,
+            )
+        )
 
     def estimate_cone_distance(self, rectangle):
         return 1.0
 
     def direction_to_pixel(self, px):
-
         img_w = self.camera_params["width"]
         img_h = self.camera_params["height"]
         img_fov = self.camera_params["FOV"]
 
-        pt_x = -((px[0]+.5) / img_w * 2 - 1) # -1 to 1
-        pt_y = -((px[1]+.5) / img_h * 2 - 1) # -1 to 1 (flipped so -1 is bottom of image)
+        pt_x = -((px[0] + 0.5) / img_w * 2 - 1)  # -1 to 1
+        pt_y = -(
+            (px[1] + 0.5) / img_h * 2 - 1
+        )  # -1 to 1 (flipped so -1 is bottom of image)
         pt_y *= img_h / img_w
 
         h_factor = img_fov / np.pi * 2.0
 
-        direction = np.array([1,pt_x*h_factor,pt_y*h_factor])
+        direction = np.array([1, pt_x * h_factor, pt_y * h_factor])
         direction = direction / np.linalg.norm(direction)
 
         return direction
 
     def calculate_position_from_box(self, rectangle):
-
-        top_px = np.array([.5*(rectangle[0] + rectangle[2]),rectangle[1]])
-        bottom_px = np.array([.5*(rectangle[0] + rectangle[2]),rectangle[3]])
+        top_px = np.array([0.5 * (rectangle[0] + rectangle[2]), rectangle[1]])
+        bottom_px = np.array([0.5 * (rectangle[0] + rectangle[2]), rectangle[3]])
 
         r1 = self.direction_to_pixel(top_px)
         r2 = self.direction_to_pixel(bottom_px)
 
-        h = .078
+        h = 0.078
 
-        l2 = h / ( r1[2]*r2[0] / r1[0] - r2[2] )        
-        p2 = r2*l2
+        l2 = h / (r1[2] * r2[0] / r1[0] - r2[2])
+        p2 = r2 * l2
 
         mount_pos = np.asarray(self.camera_params["position"])
-        mount_rot = np.asarray(self.camera_params["orientation"]).reshape((3,3))
+        mount_rot = np.asarray(self.camera_params["orientation"]).reshape((3, 3))
 
         # self.get_logger().info('Dir: Top=(%s), Bot=(%s)' % (str(r1), str(r2)))
 
-        p2 = np.matmul(mount_rot,p2) + mount_pos
+        p2 = np.matmul(mount_rot, p2) + mount_pos
 
         return p2
 
     # callback to run a loop and publish data this class generates
     def pub_callback(self):
-        if(not self.go):
+        if not self.go:
             return
         msg = ObjectArray()
 
         t0 = time.time()
 
-        #clear old rectangles
-        if(self.vis):
+        # clear old rectangles
+        if self.vis:
             [p.remove() for p in self.patches]
             self.patches.clear()
             [t.remove() for t in self.ax.texts]
 
-        # self.get_logger().info('Detected %s cones' % len(self.boxes)) 
-        
-        for b,box in enumerate(self.boxes):
+        # self.get_logger().info('Detected %s cones' % len(self.boxes))
+
+        for b, box in enumerate(self.boxes):
             position = self.calculate_position_from_box(box.astype(np.float64))
             obj = Object()
             obj.pose.position.x = position[0]
             obj.pose.position.y = position[1]
             obj.pose.position.z = position[2]
-            obj.classification.classification = int(self.labels[b])+1
+            obj.classification.classification = int(self.labels[b]) + 1
             msg.objects.append(obj)
 
-            if(self.vis):
-                color = 'r' if self.labels[b] == 0 else 'g'
-                x = box[0] #- box[2]/2
-                y = box[1]# - box[3]/2
+            if self.vis:
+                color = "r" if self.labels[b] == 0 else "g"
+                x = box[0]  # - box[2]/2
+                y = box[1]  # - box[3]/2
                 w = box[2] - box[0]
                 h = box[3] - box[1]
-                rect = patches.Rectangle((x,y), w,h, linewidth=1, edgecolor=color, facecolor='none')
+                rect = patches.Rectangle(
+                    (x, y), w, h, linewidth=1, edgecolor=color, facecolor="none"
+                )
                 self.ax.add_patch(rect)
                 self.patches.append(rect)
 
-        if(self.vis):
+        if self.vis:
             plt.draw()
             plt.pause(0.0001)
             self.counter += 1
 
-        t1 = time.time()    
+        t1 = time.time()
         msg.header.stamp = self.get_clock().now().to_msg()
         self.pub_objects.publish(msg)
 
+
 def main(args=None):
     rclpy.init(args=args)
     recognition = YOLODetectionNode()
@@ -349,5 +382,6 @@ def main(args=None):
     recognition.destroy_node()
     rclpy.shutdown()
 
-if __name__ == '__main__':
+
+if __name__ == "__main__":
     main()
diff --git a/workspace/src/perception/cone_detector/data/calibration.json b/workspace/src/perception/cone_detector/data/calibration.json
index 330028d6..7205eddf 100644
--- a/workspace/src/perception/cone_detector/data/calibration.json
+++ b/workspace/src/perception/cone_detector/data/calibration.json
@@ -1,8 +1,22 @@
 {
-    "name":"primary camera",
-    "position":[0.0316,0,0.1933],
-    "orientation":[0.9962,0,0.0872, 0,1,0, -0.0872,0,0.9962],
-    "width":1280,
-    "height":720,
-    "FOV":1.396
+    "name": "primary camera",
+    "position": [
+        0.0316,
+        0,
+        0.1933
+    ],
+    "orientation": [
+        0.9962,
+        0,
+        0.0872,
+        0,
+        1,
+        0,
+        -0.0872,
+        0,
+        0.9962
+    ],
+    "width": 1280,
+    "height": 720,
+    "FOV": 1.396
 }
diff --git a/workspace/src/perception/cone_detector/setup.py b/workspace/src/perception/cone_detector/setup.py
index 342fd268..7ea4de3a 100644
--- a/workspace/src/perception/cone_detector/setup.py
+++ b/workspace/src/perception/cone_detector/setup.py
@@ -32,29 +32,28 @@
 import os
 from glob import glob
 
-package_name = 'cone_detector'
+package_name = "cone_detector"
 
 setup(
     name=package_name,
-    version='0.0.0',
+    version="0.0.0",
     packages=[package_name],
     data_files=[
-        ('share/ament_index/resource_index/packages',
-            ['resource/' + package_name]),
-        ('share/' + package_name, ['package.xml']),
-        (os.path.join('share', package_name, 'data/'), glob('data/*')),
+        ("share/ament_index/resource_index/packages", ["resource/" + package_name]),
+        ("share/" + package_name, ["package.xml"]),
+        (os.path.join("share", package_name, "data/"), glob("data/*")),
     ],
-    install_requires=['setuptools'],
+    install_requires=["setuptools"],
     zip_safe=True,
-    maintainer='asher',
-    maintainer_email='amelmquist@wisc.edu',
-    description='TODO: Package description',
-    license='TODO: License declaration',
-    tests_require=['pytest'],
+    maintainer="asher",
+    maintainer_email="amelmquist@wisc.edu",
+    description="TODO: Package description",
+    license="TODO: License declaration",
+    tests_require=["pytest"],
     entry_points={
-        'console_scripts': [
-            'object_recognition = cone_detector.object_recognition:main',
-            'yolov5_detector = cone_detector.yolov5_detector:main'
+        "console_scripts": [
+            "object_recognition = cone_detector.object_recognition:main",
+            "yolov5_detector = cone_detector.yolov5_detector:main",
         ],
     },
 )
diff --git a/workspace/src/vehicle/arduino_driver/arduino_driver/motor_driver.py b/workspace/src/vehicle/arduino_driver/arduino_driver/motor_driver.py
index f0c54797..0668652c 100644
--- a/workspace/src/vehicle/arduino_driver/arduino_driver/motor_driver.py
+++ b/workspace/src/vehicle/arduino_driver/arduino_driver/motor_driver.py
@@ -42,33 +42,41 @@
 import struct
 import time
 
-class SteeringServoDriver():
+
+class SteeringServoDriver:
     def __init__(self):
         # === CONSTANTS ===
         # Pulse width as measured from the RC car receiver in milliseconds
         self.TRIM = -50
-        self.PW_LEFT = 1700 + self.TRIM #absolute left: 1980
-        self.PW_RIGHT = 1300 + self.TRIM #absolute right: 1000
-        self.PW_CENTER = 1500 + self.TRIM 
-        
+        self.PW_LEFT = 1700 + self.TRIM  # absolute left: 1980
+        self.PW_RIGHT = 1300 + self.TRIM  # absolute right: 1000
+        self.PW_CENTER = 1500 + self.TRIM
+
         # clamp response to achieve target
         self.current_pw = self.PW_CENTER
         self.current_steering = 0.0
         self.target_steering = 0.0
-        self.MAX_STEERING_STEP = .1  # TODO find good value
+        self.MAX_STEERING_STEP = 0.1  # TODO find good value
 
     def step(self):
         # get steering value (-1,1) with a clamped step
-        self.current_steering = np.clip(self.target_steering, self.current_steering -
-                                        self.MAX_STEERING_STEP, self.current_steering + self.MAX_STEERING_STEP)
-        self.current_steering = np.clip(self.current_steering,-1.0,1.0)
+        self.current_steering = np.clip(
+            self.target_steering,
+            self.current_steering - self.MAX_STEERING_STEP,
+            self.current_steering + self.MAX_STEERING_STEP,
+        )
+        self.current_steering = np.clip(self.current_steering, -1.0, 1.0)
 
         # convert steering to duty cycle
-        self.current_pw = self.PW_RIGHT + \
-            (self.PW_LEFT - self.PW_RIGHT) * (.5*self.current_steering + .5)
+        self.current_pw = self.PW_RIGHT + (self.PW_LEFT - self.PW_RIGHT) * (
+            0.5 * self.current_steering + 0.5
+        )
 
-        self.current_pw = np.clip(self.current_pw, min(
-            self.PW_RIGHT, self.PW_LEFT), max(self.PW_RIGHT, self.PW_LEFT))
+        self.current_pw = np.clip(
+            self.current_pw,
+            min(self.PW_RIGHT, self.PW_LEFT),
+            max(self.PW_RIGHT, self.PW_LEFT),
+        )
 
         return self.current_pw
 
@@ -76,86 +84,99 @@ def setTargetSteering(self, steering):
         self.target_steering = np.clip(steering, -1.0, 1.0)
 
 
-class MotorDriver():
+class MotorDriver:
     def __init__(self):
         # === CONSTANTS ===
         # Pulse width as measured from the RC car receiver in milliseconds
         self.TRIM = 0
-        self.PW_BRAKE = 1600 + self.TRIM #absolute full brake = 1980
-        self.PW_FULL_THROTTLE = 1400 + self.TRIM #absolute full throttle = 1000
+        self.PW_BRAKE = 1600 + self.TRIM  # absolute full brake = 1980
+        self.PW_FULL_THROTTLE = 1400 + self.TRIM  # absolute full throttle = 1000
         self.PW_NEUTRAL = 1500 + self.TRIM
-        
+
         # clamp response to achieve target
         self.current_pw = self.PW_NEUTRAL
         self.current_throttle = 0.0
         self.target_throttle = 0.0
-        self.MAX_THROTTLE_STEP = .1  # TODO find good value
+        self.MAX_THROTTLE_STEP = 0.1  # TODO find good value
 
         self.forward = True  # whether vehicle is in forward or reverse mode
 
     def Reverse(self):
-        pass #TODO
+        pass  # TODO
 
     def Forward(self):
-        pass #TODO
+        pass  # TODO
 
     def step(self):
-        if(self.forward):
+        if self.forward:
             # get thottle and brake values (-1,1) with a clamped step
-            self.current_throttle = np.clip(self.target_throttle, self.current_throttle -
-                                            self.MAX_THROTTLE_STEP, self.current_throttle + self.MAX_THROTTLE_STEP)
-            self.current_throttle = np.clip(self.current_throttle, -1.0,1.0)
+            self.current_throttle = np.clip(
+                self.target_throttle,
+                self.current_throttle - self.MAX_THROTTLE_STEP,
+                self.current_throttle + self.MAX_THROTTLE_STEP,
+            )
+            self.current_throttle = np.clip(self.current_throttle, -1.0, 1.0)
             # convert steering to duty cycle
-            if(self.current_throttle >= 0):
-                self.current_pw = self.PW_FULL_THROTTLE + \
-                    (self.PW_NEUTRAL - self.PW_FULL_THROTTLE) * \
-                    (1-self.current_throttle)
+            if self.current_throttle >= 0:
+                self.current_pw = self.PW_FULL_THROTTLE + (
+                    self.PW_NEUTRAL - self.PW_FULL_THROTTLE
+                ) * (1 - self.current_throttle)
             else:
-                self.current_pw = self.PW_NEUTRAL + \
-                    (self.PW_BRAKE - self.PW_NEUTRAL) * \
-                    -self.current_throttle
+                self.current_pw = (
+                    self.PW_NEUTRAL
+                    + (self.PW_BRAKE - self.PW_NEUTRAL) * -self.current_throttle
+                )
 
             self.current_pw = np.clip(
-                self.current_pw, min(self.PW_BRAKE,self.PW_FULL_THROTTLE), max(self.PW_BRAKE,self.PW_FULL_THROTTLE))
+                self.current_pw,
+                min(self.PW_BRAKE, self.PW_FULL_THROTTLE),
+                max(self.PW_BRAKE, self.PW_FULL_THROTTLE),
+            )
             return self.current_pw
 
         else:
             pass
 
     def setTargetThrottle(self, throttle, braking):
-        self.target_throttle = np.clip(throttle-braking, -1.0, 1.0)
+        self.target_throttle = np.clip(throttle - braking, -1.0, 1.0)
         return self.target_throttle
 
+
 class MotorDriverNode(Node):
     def __init__(self):
-        super().__init__('motor_driver')
+        super().__init__("motor_driver")
 
         # update frequencies of this node
         self.freq = 20.0  # PWM is at 60Hz, so we should not overwrite previous signal too quickly
 
         # data that will be used by this class
         self.vehicle_cmd = VehicleInput()
-        self.stale_timer = 0  # will kill motors if no commands for specific amount of time
-        self.KILL_TIME = 0.3  # time after which motors will be killed if no new commands given
+        self.stale_timer = (
+            0  # will kill motors if no commands for specific amount of time
+        )
+        self.KILL_TIME = (
+            0.3  # time after which motors will be killed if no new commands given
+        )
 
         # subscriber
         qos_profile = QoSProfile(depth=1)
         qos_profile.history = QoSHistoryPolicy.KEEP_LAST
         self.sub_vehicle_cmd = self.create_subscription(
-            VehicleInput, '~/output/vehicle_inputs', self.control_callback, qos_profile)
+            VehicleInput, "~/output/vehicle_inputs", self.control_callback, qos_profile
+        )
 
         # call the driver callback even if we haven't heard from the subscribers
-        self.timer = self.create_timer(1/self.freq, self.update_motors)
+        self.timer = self.create_timer(1 / self.freq, self.update_motors)
 
         # motor and servo objects
         self.motor = MotorDriver()
         self.servo = SteeringServoDriver()
-        
+
         BAUD_RATE = 250000
         PORT = "/dev/ttyACM0"
         TIMEOUT = 0.1
-        self.arduino = serial.Serial(port=PORT,baudrate=BAUD_RATE,timeout=TIMEOUT)
-        
+        self.arduino = serial.Serial(port=PORT, baudrate=BAUD_RATE, timeout=TIMEOUT)
+
     # function to process data this class subscribes to
     def control_callback(self, msg):
         self.vehicle_cmd = msg  # save the message
@@ -164,8 +185,8 @@ def control_callback(self, msg):
 
     def update_motors(self):
         # print("Sending commands to motor and steering servo")
-        self.stale_timer += 1/self.freq
-        if(self.stale_timer >= self.KILL_TIME):
+        self.stale_timer += 1 / self.freq
+        if self.stale_timer >= self.KILL_TIME:
             self.vehicle_cmd.throttle = 0.0
             self.vehicle_cmd.braking = 0.0
 
@@ -173,14 +194,15 @@ def update_motors(self):
 
         self.servo.setTargetSteering(self.vehicle_cmd.steering)
         target = self.motor.setTargetThrottle(
-            self.vehicle_cmd.throttle, self.vehicle_cmd.braking)
+            self.vehicle_cmd.throttle, self.vehicle_cmd.braking
+        )
         # self.get_logger().info("target throttle='%s (%s - %s)'" % (target,self.vehicle_cmd.throttle,self.vehicle_cmd.braking))
         servo_pw = int(self.servo.step())
         esc_pw = int(self.motor.step())
-        
-        msg_size = struct.pack("<B",4)
-        msg_data = struct.pack("<2H",*[servo_pw,esc_pw])
-        msg = b"".join([msg_size,msg_data])
+
+        msg_size = struct.pack("<B", 4)
+        msg_data = struct.pack("<2H", *[servo_pw, esc_pw])
+        msg = b"".join([msg_size, msg_data])
         t0 = time.time()
         self.arduino.write(msg)
         t1 = time.time()
@@ -197,5 +219,5 @@ def main(args=None):
     rclpy.shutdown()
 
 
-if __name__ == '__main__':
+if __name__ == "__main__":
     main()
diff --git a/workspace/src/vehicle/arduino_driver/resource/servo_driver.ino b/workspace/src/vehicle/arduino_driver/resource/servo_driver.ino
index 6ed3d53e..08386395 100644
--- a/workspace/src/vehicle/arduino_driver/resource/servo_driver.ino
+++ b/workspace/src/vehicle/arduino_driver/resource/servo_driver.ino
@@ -1,29 +1,28 @@
 #include <Servo.h>
 #include <HardwareSerial.h>
 
-
-struct Msg{
-  unsigned short pw_servo;
-  unsigned short pw_esc;
+struct Msg {
+    unsigned short pw_servo;
+    unsigned short pw_esc;
 };
 
 union BytesToMsg {
-  byte bytes[4];
-  Msg msg;
+    byte bytes[4];
+    Msg msg;
 };
 
 BytesToMsg msg_buffer;
 
-//rate of serial communication
+// rate of serial communication
 const long BAUD_RATE = 250000;
 
 bool stale = true;
 long time_last_msg;
-long stale_time_limit = 200; //time in milliseconds before stopping vehicle when no signal
+long stale_time_limit = 200;  // time in milliseconds before stopping vehicle when no signal
 bool process = false;
 
-//servo variables
-//pulse widths in microseconds
+// servo variables
+// pulse widths in microseconds
 const unsigned short S_PW_MAX = 1980;
 const unsigned short S_PW_NEUTRAL = 1500;
 const unsigned short S_PW_MIN = 1000;
@@ -45,76 +44,78 @@ Servo esc;
 long last_t;
 
 void setup() {
-  Serial.begin(250000);
-  Serial.setTimeout(0.1);
-
-  //configure PWM drivers
-  steering.attach(STEERING_PIN);
-  esc.attach(ESC_PIN);
-
-  //wait for a serial connection
-  while(Serial.available() <=0){
-    delay(100);
-  }
-  
-  last_t = millis();
-  time_last_msg = millis();
-
-  pinMode(LED_BUILTIN, OUTPUT);
-  digitalWrite(LED_BUILTIN, LOW);
-}
+    Serial.begin(250000);
+    Serial.setTimeout(0.1);
+
+    // configure PWM drivers
+    steering.attach(STEERING_PIN);
+    esc.attach(ESC_PIN);
+
+    // wait for a serial connection
+    while (Serial.available() <= 0) {
+        delay(100);
+    }
+
+    last_t = millis();
+    time_last_msg = millis();
 
+    pinMode(LED_BUILTIN, OUTPUT);
+    digitalWrite(LED_BUILTIN, LOW);
+}
 
 void loop() {
-  //parse any waiting serial messages
-  while(Serial.available()){
-    uint8_t msg_size;
-    Serial.readBytes(&msg_size,sizeof(msg_size));
-    if(msg_size == sizeof(msg_buffer)){
-      Serial.readBytes(msg_buffer.bytes,sizeof(msg_buffer));
-      time_last_msg = millis();
-//      process = true;
-      digitalWrite(LED_BUILTIN, HIGH);
+    // parse any waiting serial messages
+    while (Serial.available()) {
+        uint8_t msg_size;
+        Serial.readBytes(&msg_size, sizeof(msg_size));
+        if (msg_size == sizeof(msg_buffer)) {
+            Serial.readBytes(msg_buffer.bytes, sizeof(msg_buffer));
+            time_last_msg = millis();
+            //      process = true;
+            digitalWrite(LED_BUILTIN, HIGH);
+        }
+    }
+
+    Serial.flush();
+
+    // check our timers for when no data is received
+    long t = millis();
+    // if(t > last_t + 1000){
+    //   Serial.print("Tick. Stale = ");
+    //  Serial.println(stale);
+    //  last_t = t;
+    //}
+    if (stale) {
+        // write a neutral signal to esc
+        motor_pw = M_PW_NEUTRAL;  // esc can go to neutral immediately, steering servo cannot
+        esc.writeMicroseconds(M_PW_NEUTRAL);
+
+        // reset the msg data
+        msg_buffer.msg.pw_servo = S_PW_NEUTRAL;
+        msg_buffer.msg.pw_esc = M_PW_NEUTRAL;
     }
-  }
-
-  Serial.flush();
-
-  //check our timers for when no data is received
-  long t = millis();
-  //if(t > last_t + 1000){
-  //  Serial.print("Tick. Stale = ");
-   // Serial.println(stale);
-   // last_t = t;
-  //}
-  if(stale){
-    //write a neutral signal to esc
-    motor_pw = M_PW_NEUTRAL; //esc can go to neutral immediately, steering servo cannot
-    esc.writeMicroseconds(M_PW_NEUTRAL);
-
-    //reset the msg data
-    msg_buffer.msg.pw_servo = S_PW_NEUTRAL;
-    msg_buffer.msg.pw_esc = M_PW_NEUTRAL;
-  }
-
-  //update our stale timer
-  stale = t - time_last_msg > stale_time_limit;
-
-  //if message was received, use it to determine pwm values (clamped to prevent instant changes in ESC and servo)
-//  if(process){
+
+    // update our stale timer
+    stale = t - time_last_msg > stale_time_limit;
+
+    // if message was received, use it to determine pwm values (clamped to prevent instant changes in ESC and servo)
+    //  if(process){
     process = false;
-    digitalWrite(LED_BUILTIN, LOW); 
-    //write values to steering and esc
-    unsigned short target_s_pw = (msg_buffer.msg.pw_servo <= S_PW_MAX && msg_buffer.msg.pw_servo >= S_PW_MIN) ? msg_buffer.msg.pw_servo : steering_pw;
+    digitalWrite(LED_BUILTIN, LOW);
+    // write values to steering and esc
+    unsigned short target_s_pw = (msg_buffer.msg.pw_servo <= S_PW_MAX && msg_buffer.msg.pw_servo >= S_PW_MIN)
+                                     ? msg_buffer.msg.pw_servo
+                                     : steering_pw;
     target_s_pw = constrain(target_s_pw, S_PW_MIN, S_PW_MAX);
-    steering_pw = constrain(target_s_pw, steering_pw-S_PW_MAX_STEP, steering_pw+S_PW_MAX_STEP);
+    steering_pw = constrain(target_s_pw, steering_pw - S_PW_MAX_STEP, steering_pw + S_PW_MAX_STEP);
     steering.writeMicroseconds(steering_pw);
-    
-    unsigned short target_m_pw = (msg_buffer.msg.pw_esc <= M_PW_MAX && msg_buffer.msg.pw_esc >= M_PW_MIN) ? msg_buffer.msg.pw_esc : motor_pw;
+
+    unsigned short target_m_pw =
+        (msg_buffer.msg.pw_esc <= M_PW_MAX && msg_buffer.msg.pw_esc >= M_PW_MIN) ? msg_buffer.msg.pw_esc : motor_pw;
     target_m_pw = constrain(target_m_pw, M_PW_MIN, M_PW_MAX);
-    motor_pw = constrain(target_m_pw, motor_pw-M_PW_MAX_STEP,motor_pw+M_PW_MAX_STEP);
+    motor_pw = constrain(target_m_pw, motor_pw - M_PW_MAX_STEP, motor_pw + M_PW_MAX_STEP);
     esc.writeMicroseconds(motor_pw);
-//  }
+    //  }
 
-  delay(10);
+    delay(10);
 }
diff --git a/workspace/src/vehicle/arduino_driver/setup.py b/workspace/src/vehicle/arduino_driver/setup.py
index e1f61abc..e718ad66 100644
--- a/workspace/src/vehicle/arduino_driver/setup.py
+++ b/workspace/src/vehicle/arduino_driver/setup.py
@@ -30,27 +30,24 @@
 # OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.#
 from setuptools import setup
 
-package_name = 'arduino_driver'
+package_name = "arduino_driver"
 
 setup(
     name=package_name,
-    version='0.0.0',
+    version="0.0.0",
     packages=[package_name],
     data_files=[
-        ('share/ament_index/resource_index/packages',
-            ['resource/' + package_name]),
-        ('share/' + package_name, ['package.xml']),
+        ("share/ament_index/resource_index/packages", ["resource/" + package_name]),
+        ("share/" + package_name, ["package.xml"]),
     ],
-    install_requires=['setuptools'],
+    install_requires=["setuptools"],
     zip_safe=True,
-    maintainer='asher',
-    maintainer_email='amelmquist@wisc.edu',
-    description='TODO: Package description',
-    license='TODO: License declaration',
-    tests_require=['pytest'],
+    maintainer="asher",
+    maintainer_email="amelmquist@wisc.edu",
+    description="TODO: Package description",
+    license="TODO: License declaration",
+    tests_require=["pytest"],
     entry_points={
-        'console_scripts': [
-            'motor_driver = arduino_driver.motor_driver:main'
-        ],
+        "console_scripts": ["motor_driver = arduino_driver.motor_driver:main"],
     },
 )


Test patch:
diff --git a/workspace/src/common/launch/launch_utils/test/test_copyright.py b/workspace/src/common/launch/launch_utils/test/test_copyright.py
index cc8ff03f..f46f861d 100644
--- a/workspace/src/common/launch/launch_utils/test/test_copyright.py
+++ b/workspace/src/common/launch/launch_utils/test/test_copyright.py
@@ -19,5 +19,5 @@
 @pytest.mark.copyright
 @pytest.mark.linter
 def test_copyright():
-    rc = main(argv=['.', 'test'])
-    assert rc == 0, 'Found errors'
+    rc = main(argv=[".", "test"])
+    assert rc == 0, "Found errors"
diff --git a/workspace/src/common/launch/launch_utils/test/test_flake8.py b/workspace/src/common/launch/launch_utils/test/test_flake8.py
index 27ee1078..ee79f31a 100644
--- a/workspace/src/common/launch/launch_utils/test/test_flake8.py
+++ b/workspace/src/common/launch/launch_utils/test/test_flake8.py
@@ -20,6 +20,6 @@
 @pytest.mark.linter
 def test_flake8():
     rc, errors = main_with_errors(argv=[])
-    assert rc == 0, \
-        'Found %d code style errors / warnings:\n' % len(errors) + \
-        '\n'.join(errors)
+    assert rc == 0, "Found %d code style errors / warnings:\n" % len(
+        errors
+    ) + "\n".join(errors)
diff --git a/workspace/src/common/launch/launch_utils/test/test_pep257.py b/workspace/src/common/launch/launch_utils/test/test_pep257.py
index b234a384..a2c3deb8 100644
--- a/workspace/src/common/launch/launch_utils/test/test_pep257.py
+++ b/workspace/src/common/launch/launch_utils/test/test_pep257.py
@@ -19,5 +19,5 @@
 @pytest.mark.linter
 @pytest.mark.pep257
 def test_pep257():
-    rc = main(argv=['.', 'test'])
-    assert rc == 0, 'Found code style errors / warnings'
+    rc = main(argv=[".", "test"])
+    assert rc == 0, "Found code style errors / warnings"
diff --git a/workspace/src/localization/localization_py/test/test_copyright.py b/workspace/src/localization/localization_py/test/test_copyright.py
index cc8ff03f..f46f861d 100644
--- a/workspace/src/localization/localization_py/test/test_copyright.py
+++ b/workspace/src/localization/localization_py/test/test_copyright.py
@@ -19,5 +19,5 @@
 @pytest.mark.copyright
 @pytest.mark.linter
 def test_copyright():
-    rc = main(argv=['.', 'test'])
-    assert rc == 0, 'Found errors'
+    rc = main(argv=[".", "test"])
+    assert rc == 0, "Found errors"
diff --git a/workspace/src/localization/localization_py/test/test_flake8.py b/workspace/src/localization/localization_py/test/test_flake8.py
index 27ee1078..ee79f31a 100644
--- a/workspace/src/localization/localization_py/test/test_flake8.py
+++ b/workspace/src/localization/localization_py/test/test_flake8.py
@@ -20,6 +20,6 @@
 @pytest.mark.linter
 def test_flake8():
     rc, errors = main_with_errors(argv=[])
-    assert rc == 0, \
-        'Found %d code style errors / warnings:\n' % len(errors) + \
-        '\n'.join(errors)
+    assert rc == 0, "Found %d code style errors / warnings:\n" % len(
+        errors
+    ) + "\n".join(errors)
diff --git a/workspace/src/localization/localization_py/test/test_pep257.py b/workspace/src/localization/localization_py/test/test_pep257.py
index b234a384..a2c3deb8 100644
--- a/workspace/src/localization/localization_py/test/test_pep257.py
+++ b/workspace/src/localization/localization_py/test/test_pep257.py
@@ -19,5 +19,5 @@
 @pytest.mark.linter
 @pytest.mark.pep257
 def test_pep257():
-    rc = main(argv=['.', 'test'])
-    assert rc == 0, 'Found code style errors / warnings'
+    rc = main(argv=[".", "test"])
+    assert rc == 0, "Found code style errors / warnings"