From 9f00e35f0f83f8e7dbc5c8cb7e952f42a61c9266 Mon Sep 17 00:00:00 2001
From: PatXue <95881915+PatXue@users.noreply.github.com>
Date: Thu, 26 Oct 2023 22:39:13 -0400
Subject: [PATCH 01/21] Create velocity_updater.py

Basic framework of VelocityUpdater class, just applies constant acceleration to the buggy
---
 .../buggy/scripts/2d_sim/velocity_updater.py  | 26 +++++++++++++++++++
 1 file changed, 26 insertions(+)
 create mode 100644 rb_ws/src/buggy/scripts/2d_sim/velocity_updater.py

diff --git a/rb_ws/src/buggy/scripts/2d_sim/velocity_updater.py b/rb_ws/src/buggy/scripts/2d_sim/velocity_updater.py
new file mode 100644
index 00000000..83e6f68f
--- /dev/null
+++ b/rb_ws/src/buggy/scripts/2d_sim/velocity_updater.py
@@ -0,0 +1,26 @@
+#! /usr/bin/env python3
+import rospy
+from std_msgs.msg import Float64
+
+class VelocityUpdater:
+    def __init__(self):
+        self.vel_publisher = rospy.Publisher(
+            "buggy/velocity", Float64, queue_size=1
+        )
+        self.velocity: float = 15.0
+        self.accel: float = 0.1 # Dummy value for acceleration
+        
+        self.rate = 100
+
+    def tick(self):
+        new_velocity = Float64()
+        new_velocity.data = self.velocity + self.accel / self.rate
+
+        self.velocity = new_velocity.data
+        self.vel_publisher.publish(new_velocity)
+
+if __name__ == "__main__":
+    vel = VelocityUpdater()
+
+    while not rospy.is_shutdown():
+        vel.tick()
\ No newline at end of file

From 0703bd69250194275fe2f8f0670f49bb30c5e92e Mon Sep 17 00:00:00 2001
From: TiaSinghania <samanvita.singhania@gmail.com>
Date: Mon, 30 Oct 2023 18:24:04 -0400
Subject: [PATCH 02/21] theoretically added keybinds for changing speed

---
 .../scripts/2d_sim/user_control_velocity.py   | 35 +++++++++++++++++++
 1 file changed, 35 insertions(+)
 create mode 100644 rb_ws/src/buggy/scripts/2d_sim/user_control_velocity.py

diff --git a/rb_ws/src/buggy/scripts/2d_sim/user_control_velocity.py b/rb_ws/src/buggy/scripts/2d_sim/user_control_velocity.py
new file mode 100644
index 00000000..4cfe3a0b
--- /dev/null
+++ b/rb_ws/src/buggy/scripts/2d_sim/user_control_velocity.py
@@ -0,0 +1,35 @@
+#! /usr/bin/env python3
+import rospy
+from std_msgs.msg import Float64
+import keyboard
+
+class VelocityUpdater:
+    def __init__(self):
+        self.vel_publisher = rospy.Publisher(
+            "buggy/velocity", Float64, queue_size=1
+        )
+        self.velocity: float = 0
+        self.accel: float = 0.1 # Dummy value for acceleration
+        
+        self.rate = 100
+
+    def tick(self):
+        new_velocity = Float64()
+
+        event = keyboard.read_event()
+        if event.event_type == 'down':
+            if event.name == 'w':
+                new_velocity.data = self.velocity + 1
+            if event.name == 's':
+                new_velocity.data = self.velocity - 1
+
+        # new_velocity.data = self.velocity + self.accel / self.rate
+
+        self.velocity = new_velocity.data
+        self.vel_publisher.publish(new_velocity)
+
+if __name__ == "__main__":
+    vel = VelocityUpdater()
+
+    while not rospy.is_shutdown():
+        vel.tick()
\ No newline at end of file

From 813909990de108a071d4a940ee0b25829322eaf3 Mon Sep 17 00:00:00 2001
From: TiaSinghania <samanvita.singhania@gmail.com>
Date: Mon, 30 Oct 2023 18:42:19 -0400
Subject: [PATCH 03/21] modified engine but can't test bc sim errors

---
 rb_ws/src/buggy/launch/sim_2d.launch          |   2 +
 .../scripts/2d_sim/engine_usercontrol.py      | 303 ++++++++++++++++++
 2 files changed, 305 insertions(+)
 create mode 100644 rb_ws/src/buggy/scripts/2d_sim/engine_usercontrol.py

diff --git a/rb_ws/src/buggy/launch/sim_2d.launch b/rb_ws/src/buggy/launch/sim_2d.launch
index abaff00c..4dccc75f 100644
--- a/rb_ws/src/buggy/launch/sim_2d.launch
+++ b/rb_ws/src/buggy/launch/sim_2d.launch
@@ -13,5 +13,7 @@
     <!-- <node name="sim_2d_visualizer" pkg="buggy" type="grapher.py" output="screen"/> -->
     <node name="sim_2d_controller" pkg="buggy" type="controller_2d.py" output="screen"/>
 
+    <node name="sim_2d_user_controller" pkg="buggy" type="user_control_velocity.py" output="screen"/> <!-- not quite sure what this does tbh just pattern matching the others-->
+
     <node name="auton_system" pkg="buggy" type="autonsystem.py" output="screen" args="$(arg controller) $(arg start_dist)"/>
 </launch>
diff --git a/rb_ws/src/buggy/scripts/2d_sim/engine_usercontrol.py b/rb_ws/src/buggy/scripts/2d_sim/engine_usercontrol.py
new file mode 100644
index 00000000..6e6eeb92
--- /dev/null
+++ b/rb_ws/src/buggy/scripts/2d_sim/engine_usercontrol.py
@@ -0,0 +1,303 @@
+#! /usr/bin/env python3
+import rospy
+from geometry_msgs.msg import Pose, Twist, PoseWithCovariance, TwistWithCovariance
+from std_msgs.msg import Float64
+from sensor_msgs.msg import NavSatFix
+from nav_msgs.msg import Odometry
+import threading
+import numpy as np
+import utm
+import time
+import keyboard
+
+class Simulator:
+    UTM_EAST_ZERO = 589702.87
+    UTM_NORTH_ZERO = 4477172.947
+    UTM_ZONE_NUM = 17
+    UTM_ZONE_LETTER = "T"
+    WHEELBASE = 1.17
+    # Start positions for Outdoor track
+    START_LAT = 40.443024364623916
+    START_LONG = -79.9409643423245
+    NOISE = True  # Noisy outputs for nav/odom?
+
+    def __init__(self, heading: float):
+        """
+        Args:
+            heading (float): degrees start heading of buggy
+        """
+        # for X11 matplotlib (direction included)
+        self.plot_publisher = rospy.Publisher("sim_2d/utm", Pose, queue_size=1)
+
+        # simulate the INS's outputs (noise included)
+        self.pose_publisher = rospy.Publisher("nav/odom", Odometry, queue_size=1)
+
+        self.steering_subscriber = rospy.Subscriber(
+            "buggy/input/steering", Float64, self.update_steering_angle
+        )
+        self.velocity_subscriber = rospy.Subscriber(
+            "buggy/velocity", Float64, self.update_velocity
+        )
+
+        # to plot on Foxglove (no noise)
+        self.navsatfix_publisher = rospy.Publisher(
+            "state/pose_navsat", NavSatFix, queue_size=1
+        )
+
+        # to plot on Foxglove (with noise)
+        self.navsatfix_noisy_publisher = rospy.Publisher(
+            "state/pose_navsat_noisy", NavSatFix, queue_size=1
+        )
+
+        # Start position for Start of Course
+        self.e_utm = Simulator.UTM_EAST_ZERO + 60
+        self.n_utm = Simulator.UTM_NORTH_ZERO + 150
+
+        # Start position for End of Hill 2
+        # self.e_utm = Simulator.UTM_EAST_ZERO - 3
+        # self.n_utm = Simulator.UTM_NORTH_ZERO - 10
+
+        # Start position for Outdoor track
+        # self.e_utm = Simulator.UTM_EAST_ZERO + 110
+        # self.n_utm = Simulator.UTM_NORTH_ZERO + 296
+
+        # Start positions for Outdoor track
+        # utm_coords = utm.from_latlon(Simulator.START_LAT, Simulator.START_LONG)
+        # self.e_utm = utm_coords[0]
+        # self.n_utm = utm_coords[1]
+
+        self.heading = heading  # degrees
+        self.velocity = 15  # m/s
+
+        self.steering_angle = 0  # degrees
+
+        self.rate = 100  # Hz
+        self.pub_skip = 10  # publish every pub_skip ticks
+
+        self.lock = threading.Lock()
+
+    def update_steering_angle(self, data: Float64):
+        """Updates the steering angle as callback function for subscriber
+
+        Args:
+            data (Float64): angle in degrees
+        """
+        with self.lock:
+            self.steering_angle = data.data
+
+    def update_velocity(self, data: Float64):
+        """Updates the velocity as callback function for subscriber
+
+        Args:
+            data (Float64): velocity in m/s
+        """
+        with self.lock:
+            self.velocity = data.data
+
+    def get_steering_arc(self):
+        # Adapted from simulator.py (Joseph Li)
+        # calculate the radius of the steering arc
+        #   1) If steering angle is 0, return infinity
+        #   2) Otherwise, return radius of arc
+        # If turning right, steering radius is positive
+        # If turning left, steering radius is negative
+        with self.lock:
+            steering_angle = self.steering_angle
+        if steering_angle == 0.0:
+            return np.inf
+
+        return Simulator.WHEELBASE / np.tan(np.deg2rad(steering_angle))
+
+    def dynamics(self, state, v):
+        """ Calculates continuous time bicycle dynamics as a function of state and velocity
+
+        Args:
+            state (np.Array): State vector [x, y, heading, steering] in m, m, rad, rad respectively
+            v (Float 64): velocity in m/s
+
+        Returns:
+            dstate (np.Array): time derivative of state from dynamics
+        """
+        l = Simulator.WHEELBASE
+        x, y, theta, delta = state
+
+        return np.array([v * np.cos(theta),
+                         v * np.sin(theta),
+                         v / l * np.tan(delta),
+                         0])
+
+    def step(self):
+        """Complete the step at a certain Hz here. Do physics"""
+        with self.lock:
+            heading = self.heading
+            e_utm = self.e_utm
+            n_utm = self.n_utm
+            velocity = self.velocity
+            steering_angle = self.steering_angle
+
+        # Calculate new position
+        if steering_angle == 0.0:
+            # Straight
+            e_utm_new = e_utm + (velocity / self.rate) * np.cos(np.deg2rad(heading))
+            n_utm_new = n_utm + (velocity / self.rate) * np.sin(np.deg2rad(heading))
+            heading_new = heading
+        else:
+            # steering radius
+            radius = self.get_steering_arc()
+
+            distance = velocity / self.rate
+
+            delta_heading = distance / radius
+            heading_new = heading + np.rad2deg(delta_heading) / 2
+            e_utm_new = e_utm + (velocity / self.rate) * np.cos(np.deg2rad(heading_new))
+            n_utm_new = n_utm + (velocity / self.rate) * np.sin(np.deg2rad(heading_new))
+            heading_new = heading_new + np.rad2deg(delta_heading) / 2
+
+        with self.lock:
+            self.e_utm = e_utm_new
+            self.n_utm = n_utm_new
+            self.heading = heading_new
+        
+        event = keyboard.read_event()
+        if event.event_type == 'down':
+            if event.name == 'w':
+                self.velocity += 1
+            if event.name == 's':
+                self.velocity -= 1
+
+    def publish(self):
+        """Publishes the pose the arrow in visualizer should be at"""
+        p = Pose()
+
+        time_stamp = rospy.Time.now()
+
+        with self.lock:
+            p.position.x = self.e_utm
+            p.position.y = self.n_utm
+            p.position.z = self.heading
+            velocity = self.velocity
+
+        self.plot_publisher.publish(p)  # publish to X11 matplotlib window
+
+        # NavSatFix for usage with X11 matplotlib AND Foxglove plotting
+        (lat, long) = utm.to_latlon(
+            p.position.x,
+            p.position.y,
+            Simulator.UTM_ZONE_NUM,
+            Simulator.UTM_ZONE_LETTER,
+        )
+
+        nsf = NavSatFix()
+        nsf.latitude = lat
+        nsf.longitude = long
+        nsf.altitude = 260  # constant
+        nsf.header.stamp = time_stamp
+        self.navsatfix_publisher.publish(nsf)
+
+        # Possibly Noisy Data
+        lat_noisy = lat
+        long_noisy = long
+        velocity_noisy = velocity
+        heading = np.deg2rad(p.position.z)
+        heading_noisy = heading
+
+        if (Simulator.NOISE):
+            lat_noisy = lat + np.random.normal(0, 1e-8)  # ~.1cm error
+            long_noisy = long + np.random.normal(0, 1e-8)  # ~.1cm error
+            velocity_noisy = velocity + np.random.normal(0, 0.01)
+            heading_noisy = heading + np.random.normal(0, 0.01)
+
+            # Publish a new point on Foxglove to indicate the noisy location
+            nsf_noisy = NavSatFix()
+            nsf_noisy.latitude = lat_noisy
+            nsf_noisy.longitude = long_noisy
+            nsf_noisy.header.stamp = time_stamp
+            self.navsatfix_noisy_publisher.publish(nsf_noisy)
+
+        # Odometry for using with autonomous code
+        odom = Odometry()
+        odom_noisy = Odometry()
+        odom.header.stamp = time_stamp
+        odom_noisy.header.stamp = time_stamp
+
+        odom_pose = Pose()
+        odom_pose.position.x = long_noisy  # may not be noisy depending on Simulator.NOISE flag
+        odom_pose.position.y = lat_noisy
+        odom_pose.position.z = 260
+
+        odom_pose.orientation.x = 0
+        odom_pose.orientation.y = 0
+
+        odom_pose.orientation.z = np.sin(heading_noisy / 2)  # radians
+        odom_pose.orientation.w = np.cos(heading_noisy / 2)
+
+        odom_twist = Twist()
+        odom_twist.linear.x = velocity_noisy  # may not be noisy depending on Simulator.NOISE flag
+
+        odom.pose = PoseWithCovariance(pose=odom_pose)
+        odom.twist = TwistWithCovariance(twist=odom_twist)
+
+        # This is just dummy data
+        odom.pose.covariance = [
+            0.01,
+            0,
+            0,
+            0,
+            0,
+            0,
+            0,
+            0.01,
+            0,
+            0,
+            0,
+            0,
+            0,
+            0,
+            0.01,
+            0,
+            0,
+            0,
+            0,
+            0,
+            0,
+            0.01,
+            0,
+            0,
+            0,
+            0,
+            0,
+            0,
+            0.01,
+            0,
+            0,
+            0,
+            0,
+            0,
+            0,
+            0.01,
+        ]
+
+        self.pose_publisher.publish(odom)
+
+    def loop(self):
+        """Loop for the main simulator engine"""
+        rate = rospy.Rate(self.rate)
+        pub_tick_count = 0
+
+        while not rospy.is_shutdown():
+            self.step()
+
+            # Publish every self.pub_skip ticks
+            if pub_tick_count == self.pub_skip:
+                self.publish()
+                pub_tick_count = 0
+            else:
+                pub_tick_count += 1
+
+            rate.sleep()
+
+
+if __name__ == "__main__":
+    rospy.init_node("sim_2d_engine")
+    sim = Simulator(-110)
+    sim.loop()

From b6551d41fdd5960d68bc72827d6b20de42923c50 Mon Sep 17 00:00:00 2001
From: TiaSinghania <samanvita.singhania@gmail.com>
Date: Tue, 31 Oct 2023 18:31:38 -0400
Subject: [PATCH 04/21] test

---
 rb_ws/src/buggy/scripts/2d_sim/controller_2d.py | 1 +
 1 file changed, 1 insertion(+)

diff --git a/rb_ws/src/buggy/scripts/2d_sim/controller_2d.py b/rb_ws/src/buggy/scripts/2d_sim/controller_2d.py
index a51dacd0..e6894135 100755
--- a/rb_ws/src/buggy/scripts/2d_sim/controller_2d.py
+++ b/rb_ws/src/buggy/scripts/2d_sim/controller_2d.py
@@ -35,6 +35,7 @@ def set_velocity(self, vel: float):
         Args:
             vel (float): velocity (m/s)
         """
+        vel  = 0;
         msg = Float64()
         msg.data = vel
 

From bcceddbc423db501053cd3aef1fd8490265c887e Mon Sep 17 00:00:00 2001
From: TiaSinghania <samanvita.singhania@gmail.com>
Date: Thu, 2 Nov 2023 09:17:13 -0400
Subject: [PATCH 05/21] updated engine, keyboard import erroring

---
 rb_ws/src/buggy/scripts/2d_sim/engine.py             | 8 ++++++++
 rb_ws/src/buggy/scripts/2d_sim/engine_usercontrol.py | 7 +------
 2 files changed, 9 insertions(+), 6 deletions(-)

diff --git a/rb_ws/src/buggy/scripts/2d_sim/engine.py b/rb_ws/src/buggy/scripts/2d_sim/engine.py
index 5d67ce14..59581113 100755
--- a/rb_ws/src/buggy/scripts/2d_sim/engine.py
+++ b/rb_ws/src/buggy/scripts/2d_sim/engine.py
@@ -8,6 +8,7 @@
 import numpy as np
 import utm
 import time
+import keyboard
 
 
 class Simulator:
@@ -135,6 +136,13 @@ def step(self):
             velocity = self.velocity
             steering_angle = self.steering_angle
 
+        if (keyboard.is_pressed('w')):
+            print("increasing v")
+            self.velocity += 1
+        elif (keyboard.is_pressed('s')):
+            print("decreasing v")
+            self.velocity -= 1
+            
         # Calculate new position
         if steering_angle == 0.0:
             # Straight
diff --git a/rb_ws/src/buggy/scripts/2d_sim/engine_usercontrol.py b/rb_ws/src/buggy/scripts/2d_sim/engine_usercontrol.py
index 6e6eeb92..cd9654f8 100644
--- a/rb_ws/src/buggy/scripts/2d_sim/engine_usercontrol.py
+++ b/rb_ws/src/buggy/scripts/2d_sim/engine_usercontrol.py
@@ -158,12 +158,7 @@ def step(self):
             self.n_utm = n_utm_new
             self.heading = heading_new
         
-        event = keyboard.read_event()
-        if event.event_type == 'down':
-            if event.name == 'w':
-                self.velocity += 1
-            if event.name == 's':
-                self.velocity -= 1
+        
 
     def publish(self):
         """Publishes the pose the arrow in visualizer should be at"""

From eac616a3322e31d636b607cc324abb6e82010ffe Mon Sep 17 00:00:00 2001
From: TiaSinghania <samanvita.singhania@gmail.com>
Date: Sat, 18 Nov 2023 10:09:13 -0500
Subject: [PATCH 06/21] added publisher/subscriber for manual velocity (can
 publish via foxglove node)

---
 rb_ws/src/buggy/launch/sim_2d.launch          |   9 +-
 .../src/buggy/scripts/2d_sim/controller_2d.py |   3 +-
 rb_ws/src/buggy/scripts/2d_sim/engine.py      |  43 ++-
 .../scripts/2d_sim/engine_usercontrol.py      | 298 ------------------
 .../scripts/2d_sim/manual-controller_2d.py    |  35 ++
 .../scripts/2d_sim/user_control_velocity.py   |  35 --
 .../buggy/scripts/2d_sim/velocity_updater.py  |  26 --
 rb_ws/src/buggy/scripts/auton/autonsystem.py  |  11 +-
 8 files changed, 80 insertions(+), 380 deletions(-)
 delete mode 100644 rb_ws/src/buggy/scripts/2d_sim/engine_usercontrol.py
 create mode 100644 rb_ws/src/buggy/scripts/2d_sim/manual-controller_2d.py
 delete mode 100644 rb_ws/src/buggy/scripts/2d_sim/user_control_velocity.py
 delete mode 100644 rb_ws/src/buggy/scripts/2d_sim/velocity_updater.py

diff --git a/rb_ws/src/buggy/launch/sim_2d.launch b/rb_ws/src/buggy/launch/sim_2d.launch
index 4dccc75f..aaa81fe6 100644
--- a/rb_ws/src/buggy/launch/sim_2d.launch
+++ b/rb_ws/src/buggy/launch/sim_2d.launch
@@ -7,13 +7,16 @@
     <arg name="start_dist"      default="0.0" />
     <node name="foxglove" pkg="foxglove_bridge" type="foxglove_bridge" />
     <node name="mpc_calculator" pkg="buggy" type="model_predictive_controller.py" output="screen"/>
-
     
-    <node name="sim_2d_engine" pkg="buggy" type="engine.py" output="screen"/>
+    
     <!-- <node name="sim_2d_visualizer" pkg="buggy" type="grapher.py" output="screen"/> -->
+    
+    <node name="sim_2d_engine" pkg="buggy" type="engine.py" output="screen"/>
+
+
     <node name="sim_2d_controller" pkg="buggy" type="controller_2d.py" output="screen"/>
 
-    <node name="sim_2d_user_controller" pkg="buggy" type="user_control_velocity.py" output="screen"/> <!-- not quite sure what this does tbh just pattern matching the others-->
+    <node name="manual_sim_2d_controller" pkg="buggy" type="manual-controller_2d.py" output="screen"/>
 
     <node name="auton_system" pkg="buggy" type="autonsystem.py" output="screen" args="$(arg controller) $(arg start_dist)"/>
 </launch>
diff --git a/rb_ws/src/buggy/scripts/2d_sim/controller_2d.py b/rb_ws/src/buggy/scripts/2d_sim/controller_2d.py
index e6894135..0c5e4376 100755
--- a/rb_ws/src/buggy/scripts/2d_sim/controller_2d.py
+++ b/rb_ws/src/buggy/scripts/2d_sim/controller_2d.py
@@ -8,6 +8,7 @@ class Controller:
     def __init__(self):
         self.steering_publisher = rospy.Publisher("sim_2d/steering", Float64, queue_size=10)
         self.velocity_publisher = rospy.Publisher("buggy/velocity", Float64, queue_size=10)
+        
         self.steering_angle = 0
         self.velocity = 0
 
@@ -35,7 +36,6 @@ def set_velocity(self, vel: float):
         Args:
             vel (float): velocity (m/s)
         """
-        vel  = 0;
         msg = Float64()
         msg.data = vel
 
@@ -50,5 +50,6 @@ def set_velocity(self, vel: float):
     rate = rospy.Rate(5)
     i = 0
     while not rospy.is_shutdown():
+        controller.set_velocity(15)
         rate.sleep()
 
diff --git a/rb_ws/src/buggy/scripts/2d_sim/engine.py b/rb_ws/src/buggy/scripts/2d_sim/engine.py
index 59581113..4e9cc369 100755
--- a/rb_ws/src/buggy/scripts/2d_sim/engine.py
+++ b/rb_ws/src/buggy/scripts/2d_sim/engine.py
@@ -22,6 +22,8 @@ class Simulator:
     START_LONG = -79.9409643423245
     NOISE = True  # Noisy outputs for nav/odom?
 
+    MANUAL_V_FLAG = True
+
     def __init__(self, heading: float):
         """
         Args:
@@ -36,9 +38,14 @@ def __init__(self, heading: float):
         self.steering_subscriber = rospy.Subscriber(
             "buggy/input/steering", Float64, self.update_steering_angle
         )
-        self.velocity_subscriber = rospy.Subscriber(
-            "buggy/velocity", Float64, self.update_velocity
-        )
+        # To read from autogenerated velocity (currently set to a constant)
+        self.simvelocity_subscriber = rospy.Subscriber(
+            "buggy/velocity", Float64, self.update_velocity, "simulated"
+        ) 
+        # To read from manually entered velocity
+        self.manvelocity_subscriber = rospy.Subscriber(
+            "buggy/man_velocity", Float64, self.update_velocity, "manual"
+        ) 
 
         # to plot on Foxglove (no noise)
         self.navsatfix_publisher = rospy.Publisher(
@@ -68,7 +75,7 @@ def __init__(self, heading: float):
         # self.n_utm = utm_coords[1]
 
         self.heading = heading  # degrees
-        self.velocity = 15  # m/s
+        self.velocity = 0.1  # m/s
 
         self.steering_angle = 0  # degrees
 
@@ -77,6 +84,7 @@ def __init__(self, heading: float):
 
         self.lock = threading.Lock()
 
+    #TODO: can i get this callback function to take in data from multiple subscribers and choose which one based on the state of a toggle?
     def update_steering_angle(self, data: Float64):
         """Updates the steering angle as callback function for subscriber
 
@@ -86,15 +94,20 @@ def update_steering_angle(self, data: Float64):
         with self.lock:
             self.steering_angle = data.data
 
-    def update_velocity(self, data: Float64):
+    def update_velocity(self, data: Float64, source):
         """Updates the velocity as callback function for subscriber
 
         Args:
             data (Float64): velocity in m/s
+            source (string): whether incoming data is manual or simulated
         """
-        with self.lock:
-            self.velocity = data.data
-
+        if (source == "simulated" and not Simulator.MANUAL_V_FLAG):
+            with self.lock:
+                self.velocity = data.data
+        if (source == "manual" and Simulator.MANUAL_V_FLAG):
+            with self.lock:
+                self.velocity = data.data
+            
     def get_steering_arc(self):
         # Adapted from simulator.py (Joseph Li)
         # calculate the radius of the steering arc
@@ -129,20 +142,17 @@ def dynamics(self, state, v):
 
     def step(self):
         """Complete the step at a certain Hz here. Do physics"""
-        with self.lock:
+        with self.lock: 
             heading = self.heading
             e_utm = self.e_utm
             n_utm = self.n_utm
             velocity = self.velocity
             steering_angle = self.steering_angle
 
-        if (keyboard.is_pressed('w')):
-            print("increasing v")
-            self.velocity += 1
-        elif (keyboard.is_pressed('s')):
-            print("decreasing v")
-            self.velocity -= 1
-            
+        #if (Simulator.TEST_MANUALV_FLAG): #if manual toggle is flipped
+            #print("should change velocity (but how)")
+            # self.update_velocity(data=3); # 3 should be the data from the subscriber? get data from wherever it was published to 
+        
         # Calculate new position
         if steering_angle == 0.0:
             # Straight
@@ -302,3 +312,4 @@ def loop(self):
     rospy.init_node("sim_2d_engine")
     sim = Simulator(-110)
     sim.loop()
+
diff --git a/rb_ws/src/buggy/scripts/2d_sim/engine_usercontrol.py b/rb_ws/src/buggy/scripts/2d_sim/engine_usercontrol.py
deleted file mode 100644
index cd9654f8..00000000
--- a/rb_ws/src/buggy/scripts/2d_sim/engine_usercontrol.py
+++ /dev/null
@@ -1,298 +0,0 @@
-#! /usr/bin/env python3
-import rospy
-from geometry_msgs.msg import Pose, Twist, PoseWithCovariance, TwistWithCovariance
-from std_msgs.msg import Float64
-from sensor_msgs.msg import NavSatFix
-from nav_msgs.msg import Odometry
-import threading
-import numpy as np
-import utm
-import time
-import keyboard
-
-class Simulator:
-    UTM_EAST_ZERO = 589702.87
-    UTM_NORTH_ZERO = 4477172.947
-    UTM_ZONE_NUM = 17
-    UTM_ZONE_LETTER = "T"
-    WHEELBASE = 1.17
-    # Start positions for Outdoor track
-    START_LAT = 40.443024364623916
-    START_LONG = -79.9409643423245
-    NOISE = True  # Noisy outputs for nav/odom?
-
-    def __init__(self, heading: float):
-        """
-        Args:
-            heading (float): degrees start heading of buggy
-        """
-        # for X11 matplotlib (direction included)
-        self.plot_publisher = rospy.Publisher("sim_2d/utm", Pose, queue_size=1)
-
-        # simulate the INS's outputs (noise included)
-        self.pose_publisher = rospy.Publisher("nav/odom", Odometry, queue_size=1)
-
-        self.steering_subscriber = rospy.Subscriber(
-            "buggy/input/steering", Float64, self.update_steering_angle
-        )
-        self.velocity_subscriber = rospy.Subscriber(
-            "buggy/velocity", Float64, self.update_velocity
-        )
-
-        # to plot on Foxglove (no noise)
-        self.navsatfix_publisher = rospy.Publisher(
-            "state/pose_navsat", NavSatFix, queue_size=1
-        )
-
-        # to plot on Foxglove (with noise)
-        self.navsatfix_noisy_publisher = rospy.Publisher(
-            "state/pose_navsat_noisy", NavSatFix, queue_size=1
-        )
-
-        # Start position for Start of Course
-        self.e_utm = Simulator.UTM_EAST_ZERO + 60
-        self.n_utm = Simulator.UTM_NORTH_ZERO + 150
-
-        # Start position for End of Hill 2
-        # self.e_utm = Simulator.UTM_EAST_ZERO - 3
-        # self.n_utm = Simulator.UTM_NORTH_ZERO - 10
-
-        # Start position for Outdoor track
-        # self.e_utm = Simulator.UTM_EAST_ZERO + 110
-        # self.n_utm = Simulator.UTM_NORTH_ZERO + 296
-
-        # Start positions for Outdoor track
-        # utm_coords = utm.from_latlon(Simulator.START_LAT, Simulator.START_LONG)
-        # self.e_utm = utm_coords[0]
-        # self.n_utm = utm_coords[1]
-
-        self.heading = heading  # degrees
-        self.velocity = 15  # m/s
-
-        self.steering_angle = 0  # degrees
-
-        self.rate = 100  # Hz
-        self.pub_skip = 10  # publish every pub_skip ticks
-
-        self.lock = threading.Lock()
-
-    def update_steering_angle(self, data: Float64):
-        """Updates the steering angle as callback function for subscriber
-
-        Args:
-            data (Float64): angle in degrees
-        """
-        with self.lock:
-            self.steering_angle = data.data
-
-    def update_velocity(self, data: Float64):
-        """Updates the velocity as callback function for subscriber
-
-        Args:
-            data (Float64): velocity in m/s
-        """
-        with self.lock:
-            self.velocity = data.data
-
-    def get_steering_arc(self):
-        # Adapted from simulator.py (Joseph Li)
-        # calculate the radius of the steering arc
-        #   1) If steering angle is 0, return infinity
-        #   2) Otherwise, return radius of arc
-        # If turning right, steering radius is positive
-        # If turning left, steering radius is negative
-        with self.lock:
-            steering_angle = self.steering_angle
-        if steering_angle == 0.0:
-            return np.inf
-
-        return Simulator.WHEELBASE / np.tan(np.deg2rad(steering_angle))
-
-    def dynamics(self, state, v):
-        """ Calculates continuous time bicycle dynamics as a function of state and velocity
-
-        Args:
-            state (np.Array): State vector [x, y, heading, steering] in m, m, rad, rad respectively
-            v (Float 64): velocity in m/s
-
-        Returns:
-            dstate (np.Array): time derivative of state from dynamics
-        """
-        l = Simulator.WHEELBASE
-        x, y, theta, delta = state
-
-        return np.array([v * np.cos(theta),
-                         v * np.sin(theta),
-                         v / l * np.tan(delta),
-                         0])
-
-    def step(self):
-        """Complete the step at a certain Hz here. Do physics"""
-        with self.lock:
-            heading = self.heading
-            e_utm = self.e_utm
-            n_utm = self.n_utm
-            velocity = self.velocity
-            steering_angle = self.steering_angle
-
-        # Calculate new position
-        if steering_angle == 0.0:
-            # Straight
-            e_utm_new = e_utm + (velocity / self.rate) * np.cos(np.deg2rad(heading))
-            n_utm_new = n_utm + (velocity / self.rate) * np.sin(np.deg2rad(heading))
-            heading_new = heading
-        else:
-            # steering radius
-            radius = self.get_steering_arc()
-
-            distance = velocity / self.rate
-
-            delta_heading = distance / radius
-            heading_new = heading + np.rad2deg(delta_heading) / 2
-            e_utm_new = e_utm + (velocity / self.rate) * np.cos(np.deg2rad(heading_new))
-            n_utm_new = n_utm + (velocity / self.rate) * np.sin(np.deg2rad(heading_new))
-            heading_new = heading_new + np.rad2deg(delta_heading) / 2
-
-        with self.lock:
-            self.e_utm = e_utm_new
-            self.n_utm = n_utm_new
-            self.heading = heading_new
-        
-        
-
-    def publish(self):
-        """Publishes the pose the arrow in visualizer should be at"""
-        p = Pose()
-
-        time_stamp = rospy.Time.now()
-
-        with self.lock:
-            p.position.x = self.e_utm
-            p.position.y = self.n_utm
-            p.position.z = self.heading
-            velocity = self.velocity
-
-        self.plot_publisher.publish(p)  # publish to X11 matplotlib window
-
-        # NavSatFix for usage with X11 matplotlib AND Foxglove plotting
-        (lat, long) = utm.to_latlon(
-            p.position.x,
-            p.position.y,
-            Simulator.UTM_ZONE_NUM,
-            Simulator.UTM_ZONE_LETTER,
-        )
-
-        nsf = NavSatFix()
-        nsf.latitude = lat
-        nsf.longitude = long
-        nsf.altitude = 260  # constant
-        nsf.header.stamp = time_stamp
-        self.navsatfix_publisher.publish(nsf)
-
-        # Possibly Noisy Data
-        lat_noisy = lat
-        long_noisy = long
-        velocity_noisy = velocity
-        heading = np.deg2rad(p.position.z)
-        heading_noisy = heading
-
-        if (Simulator.NOISE):
-            lat_noisy = lat + np.random.normal(0, 1e-8)  # ~.1cm error
-            long_noisy = long + np.random.normal(0, 1e-8)  # ~.1cm error
-            velocity_noisy = velocity + np.random.normal(0, 0.01)
-            heading_noisy = heading + np.random.normal(0, 0.01)
-
-            # Publish a new point on Foxglove to indicate the noisy location
-            nsf_noisy = NavSatFix()
-            nsf_noisy.latitude = lat_noisy
-            nsf_noisy.longitude = long_noisy
-            nsf_noisy.header.stamp = time_stamp
-            self.navsatfix_noisy_publisher.publish(nsf_noisy)
-
-        # Odometry for using with autonomous code
-        odom = Odometry()
-        odom_noisy = Odometry()
-        odom.header.stamp = time_stamp
-        odom_noisy.header.stamp = time_stamp
-
-        odom_pose = Pose()
-        odom_pose.position.x = long_noisy  # may not be noisy depending on Simulator.NOISE flag
-        odom_pose.position.y = lat_noisy
-        odom_pose.position.z = 260
-
-        odom_pose.orientation.x = 0
-        odom_pose.orientation.y = 0
-
-        odom_pose.orientation.z = np.sin(heading_noisy / 2)  # radians
-        odom_pose.orientation.w = np.cos(heading_noisy / 2)
-
-        odom_twist = Twist()
-        odom_twist.linear.x = velocity_noisy  # may not be noisy depending on Simulator.NOISE flag
-
-        odom.pose = PoseWithCovariance(pose=odom_pose)
-        odom.twist = TwistWithCovariance(twist=odom_twist)
-
-        # This is just dummy data
-        odom.pose.covariance = [
-            0.01,
-            0,
-            0,
-            0,
-            0,
-            0,
-            0,
-            0.01,
-            0,
-            0,
-            0,
-            0,
-            0,
-            0,
-            0.01,
-            0,
-            0,
-            0,
-            0,
-            0,
-            0,
-            0.01,
-            0,
-            0,
-            0,
-            0,
-            0,
-            0,
-            0.01,
-            0,
-            0,
-            0,
-            0,
-            0,
-            0,
-            0.01,
-        ]
-
-        self.pose_publisher.publish(odom)
-
-    def loop(self):
-        """Loop for the main simulator engine"""
-        rate = rospy.Rate(self.rate)
-        pub_tick_count = 0
-
-        while not rospy.is_shutdown():
-            self.step()
-
-            # Publish every self.pub_skip ticks
-            if pub_tick_count == self.pub_skip:
-                self.publish()
-                pub_tick_count = 0
-            else:
-                pub_tick_count += 1
-
-            rate.sleep()
-
-
-if __name__ == "__main__":
-    rospy.init_node("sim_2d_engine")
-    sim = Simulator(-110)
-    sim.loop()
diff --git a/rb_ws/src/buggy/scripts/2d_sim/manual-controller_2d.py b/rb_ws/src/buggy/scripts/2d_sim/manual-controller_2d.py
new file mode 100644
index 00000000..ba2e4be8
--- /dev/null
+++ b/rb_ws/src/buggy/scripts/2d_sim/manual-controller_2d.py
@@ -0,0 +1,35 @@
+#! /usr/bin/env python3
+import rospy
+from std_msgs.msg import Float64
+import threading
+import numpy as np
+from controller_2d import Controller
+
+class ManualController (Controller):
+    def __init__(self):
+        self.manual_velocity_publisher = rospy.Publisher("buggy/man_velocity", Float64, queue_size=10)
+        Controller.__init__(self)
+        
+    
+    def set_steering(self, angle: float):
+        Controller.set_steering(self, angle)
+    
+    def set_velocity(self, vel: float): # ACTUALLY MODIFY THIS ONE 
+        """Set the velocity and publish to simulator engine
+
+        Args:
+            vel (float): velocity (m/s)
+        """
+        msg = Float64()
+        msg.data = vel
+        with self.lock:
+            self.velocity = vel
+        self.manual_velocity_publisher.publish(msg) 
+
+if __name__ == "__main__": 
+    rospy.init_node("manual_sim_2d_controller")
+    m_controller = ManualController()
+    rate = rospy.Rate(5)
+    i = 0
+    while not rospy.is_shutdown():
+        rate.sleep()
diff --git a/rb_ws/src/buggy/scripts/2d_sim/user_control_velocity.py b/rb_ws/src/buggy/scripts/2d_sim/user_control_velocity.py
deleted file mode 100644
index 4cfe3a0b..00000000
--- a/rb_ws/src/buggy/scripts/2d_sim/user_control_velocity.py
+++ /dev/null
@@ -1,35 +0,0 @@
-#! /usr/bin/env python3
-import rospy
-from std_msgs.msg import Float64
-import keyboard
-
-class VelocityUpdater:
-    def __init__(self):
-        self.vel_publisher = rospy.Publisher(
-            "buggy/velocity", Float64, queue_size=1
-        )
-        self.velocity: float = 0
-        self.accel: float = 0.1 # Dummy value for acceleration
-        
-        self.rate = 100
-
-    def tick(self):
-        new_velocity = Float64()
-
-        event = keyboard.read_event()
-        if event.event_type == 'down':
-            if event.name == 'w':
-                new_velocity.data = self.velocity + 1
-            if event.name == 's':
-                new_velocity.data = self.velocity - 1
-
-        # new_velocity.data = self.velocity + self.accel / self.rate
-
-        self.velocity = new_velocity.data
-        self.vel_publisher.publish(new_velocity)
-
-if __name__ == "__main__":
-    vel = VelocityUpdater()
-
-    while not rospy.is_shutdown():
-        vel.tick()
\ No newline at end of file
diff --git a/rb_ws/src/buggy/scripts/2d_sim/velocity_updater.py b/rb_ws/src/buggy/scripts/2d_sim/velocity_updater.py
deleted file mode 100644
index 83e6f68f..00000000
--- a/rb_ws/src/buggy/scripts/2d_sim/velocity_updater.py
+++ /dev/null
@@ -1,26 +0,0 @@
-#! /usr/bin/env python3
-import rospy
-from std_msgs.msg import Float64
-
-class VelocityUpdater:
-    def __init__(self):
-        self.vel_publisher = rospy.Publisher(
-            "buggy/velocity", Float64, queue_size=1
-        )
-        self.velocity: float = 15.0
-        self.accel: float = 0.1 # Dummy value for acceleration
-        
-        self.rate = 100
-
-    def tick(self):
-        new_velocity = Float64()
-        new_velocity.data = self.velocity + self.accel / self.rate
-
-        self.velocity = new_velocity.data
-        self.vel_publisher.publish(new_velocity)
-
-if __name__ == "__main__":
-    vel = VelocityUpdater()
-
-    while not rospy.is_shutdown():
-        vel.tick()
\ No newline at end of file
diff --git a/rb_ws/src/buggy/scripts/auton/autonsystem.py b/rb_ws/src/buggy/scripts/auton/autonsystem.py
index 0338ed09..28b3245f 100755
--- a/rb_ws/src/buggy/scripts/auton/autonsystem.py
+++ b/rb_ws/src/buggy/scripts/auton/autonsystem.py
@@ -41,6 +41,7 @@ class AutonSystem:
     ticks = 0
 
     def __init__(self, trajectory, controller, brake_controller) -> None:
+        print("autonsystem initializer")
         self.trajectory = trajectory
         self.controller = controller
         self.brake_controller = brake_controller
@@ -51,6 +52,7 @@ def __init__(self, trajectory, controller, brake_controller) -> None:
         
         self.msg = None
 
+        # where ARE these 
         rospy.Subscriber("nav/odom", Odometry, self.update_msg)
         self.covariance_warning_publisher = rospy.Publisher(
             "buggy/debug/is_high_covariance", Bool, queue_size=1
@@ -148,21 +150,28 @@ def tick(self):
             self.distance_publisher.publish(distance_msg)
 
 if __name__ == "__main__":
+    print("7: autonsys main started")
     rospy.init_node("auton_system")
 
     arg_ctrl = sys.argv[1]
+    print("8: argument controller")
+    print(arg_ctrl);
     arg_start_dist = sys.argv[2]
+    print("9: starting distance")
+    print(arg_start_dist)
+
     start_dist = float(arg_start_dist)
 
     print("\n\nStarting Controller: " + str(arg_ctrl) + "\n\n")
     print("\n\nStarting at distance: " + str(arg_start_dist) + "\n\n")
 
-    trajectory = Trajectory("/rb_ws/src/buggy/paths/frew_parkinglot_1.json")
+    trajectory = Trajectory("/rb_ws/src/buggy/paths/buggycourse_safe_1.json")
     # calculate starting index
     start_index = trajectory.get_index_from_distance(start_dist)
 
 
     # Add Controllers Here
+    # TODO: is manual controller another type of controller? do i modify this else if loop??
     ctrller = None
     if (arg_ctrl == "stanley"):
         ctrller = StanleyController(start_index)

From f18863c9a5d0dacca99cba552dfb60bfb98d0b27 Mon Sep 17 00:00:00 2001
From: TiaSinghania <samanvita.singhania@gmail.com>
Date: Sat, 18 Nov 2023 10:09:36 -0500
Subject: [PATCH 07/21] fixed python reqs

---
 python-requirements.txt | 20 ++++++++++----------
 1 file changed, 10 insertions(+), 10 deletions(-)

diff --git a/python-requirements.txt b/python-requirements.txt
index ab966e4c..78cd90e5 100755
--- a/python-requirements.txt
+++ b/python-requirements.txt
@@ -1,10 +1,10 @@
-trimesh
-pyembree
-numpy>=1.20.0
-pandas
-pymap3d
-matplotlib
-utm
-scipy
-osqp
-numba
+trimesh
+pyembree
+numpy>=1.20.0
+pandas
+pymap3d
+matplotlib
+utm
+scipy
+osqp
+numba
\ No newline at end of file

From 32c3f69f297d4762f9d47c771c830c2fdadbf050 Mon Sep 17 00:00:00 2001
From: TiaSinghania <samanvita.singhania@gmail.com>
Date: Sat, 18 Nov 2023 10:30:42 -0500
Subject: [PATCH 08/21] cleaned up comments

---
 rb_ws/src/buggy/scripts/2d_sim/engine.py               | 4 ----
 rb_ws/src/buggy/scripts/2d_sim/manual-controller_2d.py | 2 +-
 2 files changed, 1 insertion(+), 5 deletions(-)

diff --git a/rb_ws/src/buggy/scripts/2d_sim/engine.py b/rb_ws/src/buggy/scripts/2d_sim/engine.py
index 8bea48cc..d21ceea3 100755
--- a/rb_ws/src/buggy/scripts/2d_sim/engine.py
+++ b/rb_ws/src/buggy/scripts/2d_sim/engine.py
@@ -147,10 +147,6 @@ def step(self):
             n_utm = self.n_utm
             velocity = self.velocity
             steering_angle = self.steering_angle
-
-        #if (Simulator.TEST_MANUALV_FLAG): #if manual toggle is flipped
-            #print("should change velocity (but how)")
-            # self.update_velocity(data=3); # 3 should be the data from the subscriber? get data from wherever it was published to 
         
         # Calculate new position
         if steering_angle == 0.0:
diff --git a/rb_ws/src/buggy/scripts/2d_sim/manual-controller_2d.py b/rb_ws/src/buggy/scripts/2d_sim/manual-controller_2d.py
index ba2e4be8..f95b5c3c 100644
--- a/rb_ws/src/buggy/scripts/2d_sim/manual-controller_2d.py
+++ b/rb_ws/src/buggy/scripts/2d_sim/manual-controller_2d.py
@@ -14,7 +14,7 @@ def __init__(self):
     def set_steering(self, angle: float):
         Controller.set_steering(self, angle)
     
-    def set_velocity(self, vel: float): # ACTUALLY MODIFY THIS ONE 
+    def set_velocity(self, vel: float):  
         """Set the velocity and publish to simulator engine
 
         Args:

From 2f70b762ed1f69514bf6ab50466379ffe15c564e Mon Sep 17 00:00:00 2001
From: TiaSinghania <samanvita.singhania@gmail.com>
Date: Sat, 18 Nov 2023 23:23:43 -0500
Subject: [PATCH 09/21] integrated manual controller with simulated control

---
 rb_ws/src/buggy/launch/sim_2d_single.launch   |  2 ++
 .../src/buggy/scripts/2d_sim/controller_2d.py |  2 ++
 rb_ws/src/buggy/scripts/2d_sim/engine.py      |  5 +--
 .../scripts/2d_sim/manual-controller_2d.py    | 35 -------------------
 4 files changed, 7 insertions(+), 37 deletions(-)
 delete mode 100644 rb_ws/src/buggy/scripts/2d_sim/manual-controller_2d.py

diff --git a/rb_ws/src/buggy/launch/sim_2d_single.launch b/rb_ws/src/buggy/launch/sim_2d_single.launch
index 3554bfcc..d08bb193 100644
--- a/rb_ws/src/buggy/launch/sim_2d_single.launch
+++ b/rb_ws/src/buggy/launch/sim_2d_single.launch
@@ -14,8 +14,10 @@
 
     <node name="sim_2d_engine" pkg="buggy" type="engine.py" output="screen"
         args="$(arg starting_pose) $(arg velocity) $(arg buggy_name)"/>
+
     <node name="vel_updater" pkg="buggy" type="velocity_updater.py"
         output="screen" args="$(arg velocity) $(arg buggy_name)"/>
+
     <!-- autonsystem args: controller start_dist path buggy_name is_sim -->
     <node name="auton_system" pkg="buggy" type="autonsystem.py" output="screen" 
         args="$(arg controller) $(arg start_dist) $(arg path) $(arg buggy_name) True"/>
diff --git a/rb_ws/src/buggy/scripts/2d_sim/controller_2d.py b/rb_ws/src/buggy/scripts/2d_sim/controller_2d.py
index dff5756b..4bceecb0 100755
--- a/rb_ws/src/buggy/scripts/2d_sim/controller_2d.py
+++ b/rb_ws/src/buggy/scripts/2d_sim/controller_2d.py
@@ -1,4 +1,5 @@
 #! /usr/bin/env python3
+import sys
 import rospy
 from std_msgs.msg import Float64
 import threading
@@ -8,6 +9,7 @@ class Controller:
     def __init__(self, buggy_name):
         self.steering_publisher = rospy.Publisher(buggy_name + "/input/steering", Float64, queue_size=10)
         self.velocity_publisher = rospy.Publisher(buggy_name + "/velocity", Float64, queue_size=10)
+        self.manual_velocity_publisher = rospy.Publisher(buggy_name + "/man_velocity", Float64, queue_size=10)
         self.steering_angle = 0
         self.velocity = 0
 
diff --git a/rb_ws/src/buggy/scripts/2d_sim/engine.py b/rb_ws/src/buggy/scripts/2d_sim/engine.py
index d21ceea3..f07c1aa6 100755
--- a/rb_ws/src/buggy/scripts/2d_sim/engine.py
+++ b/rb_ws/src/buggy/scripts/2d_sim/engine.py
@@ -20,6 +20,7 @@ class Simulator:
     # Start positions for Outdoor track
     START_LAT = 40.443024364623916
     START_LONG = -79.9409643423245
+    NOISE = True
 
     MANUAL_V_FLAG = True
 
@@ -37,13 +38,13 @@ def __init__(self, starting_pose, velocity, buggy_name):
         self.steering_subscriber = rospy.Subscriber(
             buggy_name + "/input/steering", Float64, self.update_steering_angle
         )
-        # To read from autogenerated velocity (currently set to a constant)
+        # To read from autogenerated velocity 
         self.simvelocity_subscriber = rospy.Subscriber(
             buggy_name + "/velocity", Float64, self.update_velocity, "simulated"
         ) 
         # To read from manually entered velocity
         self.manvelocity_subscriber = rospy.Subscriber(
-            buggy_name + "/velocity", Float64, self.update_velocity, "manual"
+            buggy_name + "/man_velocity", Float64, self.update_velocity, "manual"
         ) 
 
         # to plot on Foxglove (no noise)
diff --git a/rb_ws/src/buggy/scripts/2d_sim/manual-controller_2d.py b/rb_ws/src/buggy/scripts/2d_sim/manual-controller_2d.py
deleted file mode 100644
index f95b5c3c..00000000
--- a/rb_ws/src/buggy/scripts/2d_sim/manual-controller_2d.py
+++ /dev/null
@@ -1,35 +0,0 @@
-#! /usr/bin/env python3
-import rospy
-from std_msgs.msg import Float64
-import threading
-import numpy as np
-from controller_2d import Controller
-
-class ManualController (Controller):
-    def __init__(self):
-        self.manual_velocity_publisher = rospy.Publisher("buggy/man_velocity", Float64, queue_size=10)
-        Controller.__init__(self)
-        
-    
-    def set_steering(self, angle: float):
-        Controller.set_steering(self, angle)
-    
-    def set_velocity(self, vel: float):  
-        """Set the velocity and publish to simulator engine
-
-        Args:
-            vel (float): velocity (m/s)
-        """
-        msg = Float64()
-        msg.data = vel
-        with self.lock:
-            self.velocity = vel
-        self.manual_velocity_publisher.publish(msg) 
-
-if __name__ == "__main__": 
-    rospy.init_node("manual_sim_2d_controller")
-    m_controller = ManualController()
-    rate = rospy.Rate(5)
-    i = 0
-    while not rospy.is_shutdown():
-        rate.sleep()

From a9018c668270a16469659a5e5c4fa48ecb709b05 Mon Sep 17 00:00:00 2001
From: TiaSinghania <samanvita.singhania@gmail.com>
Date: Sun, 19 Nov 2023 09:08:39 -0500
Subject: [PATCH 10/21] made controller and engine generic

---
 rb_ws/src/buggy/launch/sim_2d.launch          | 22 ---------
 rb_ws/src/buggy/launch/sim_2d_2buggies.launch |  4 ++
 rb_ws/src/buggy/launch/sim_2d_single.launch   |  3 +-
 .../src/buggy/scripts/2d_sim/controller_2d.py |  1 -
 rb_ws/src/buggy/scripts/2d_sim/engine.py      | 25 ++++------
 rb_ws/src/buggy/scripts/2d_sim/velocity_ui.py | 47 +++++++++++++++++++
 6 files changed, 61 insertions(+), 41 deletions(-)
 delete mode 100644 rb_ws/src/buggy/launch/sim_2d.launch
 create mode 100644 rb_ws/src/buggy/scripts/2d_sim/velocity_ui.py

diff --git a/rb_ws/src/buggy/launch/sim_2d.launch b/rb_ws/src/buggy/launch/sim_2d.launch
deleted file mode 100644
index aaa81fe6..00000000
--- a/rb_ws/src/buggy/launch/sim_2d.launch
+++ /dev/null
@@ -1,22 +0,0 @@
-<!-- roslaunch buggy main.launch simulation:=true -->
-<!-- roslaunch buggy main.launch simulation:=false -->
-<!-- <arg name="name" value="INS" /> -->
-
-<launch>
-    <arg name="controller"      default="stanley" />
-    <arg name="start_dist"      default="0.0" />
-    <node name="foxglove" pkg="foxglove_bridge" type="foxglove_bridge" />
-    <node name="mpc_calculator" pkg="buggy" type="model_predictive_controller.py" output="screen"/>
-    
-    
-    <!-- <node name="sim_2d_visualizer" pkg="buggy" type="grapher.py" output="screen"/> -->
-    
-    <node name="sim_2d_engine" pkg="buggy" type="engine.py" output="screen"/>
-
-
-    <node name="sim_2d_controller" pkg="buggy" type="controller_2d.py" output="screen"/>
-
-    <node name="manual_sim_2d_controller" pkg="buggy" type="manual-controller_2d.py" output="screen"/>
-
-    <node name="auton_system" pkg="buggy" type="autonsystem.py" output="screen" args="$(arg controller) $(arg start_dist)"/>
-</launch>
diff --git a/rb_ws/src/buggy/launch/sim_2d_2buggies.launch b/rb_ws/src/buggy/launch/sim_2d_2buggies.launch
index 61ad3111..4f78a3d9 100644
--- a/rb_ws/src/buggy/launch/sim_2d_2buggies.launch
+++ b/rb_ws/src/buggy/launch/sim_2d_2buggies.launch
@@ -21,6 +21,8 @@
         args="$(arg sc_starting_pose) $(arg sc_velocity) SC"/>
     <node name="sc_vel_updater" pkg="buggy" type="velocity_updater.py"
         output="screen" args="$(arg sc_velocity) SC"/>
+    <node name="sc_velocity_ui" pkg="buggy" type="velocity_ui.py"
+        output="screen" args="$(arg sc_velocity) SC"/>
 
     <!-- autonsystem args: controller start_dist path buggy_name is_sim -->
     <node name="sc_auton_system" pkg="buggy" type="autonsystem.py" output="screen" 
@@ -30,6 +32,8 @@
         args="$(arg nand_starting_pose) $(arg nand_velocity) Nand"/>
     <node name="nand_vel_updater" pkg="buggy" type="velocity_updater.py"
         output="screen" args="$(arg nand_velocity) Nand"/>
+    <node name="nand_velocity_ui" pkg="buggy" type="velocity_ui.py"
+        output="screen" args="$(arg nand_velocity) Nand"/>
 
     <node name="nand_auton_system" pkg="buggy" type="autonsystem.py" output="screen" 
         args="$(arg nand_controller) $(arg nand_start_dist) $(arg nand_path) Nand True"/>
diff --git a/rb_ws/src/buggy/launch/sim_2d_single.launch b/rb_ws/src/buggy/launch/sim_2d_single.launch
index d08bb193..cffe7a2f 100644
--- a/rb_ws/src/buggy/launch/sim_2d_single.launch
+++ b/rb_ws/src/buggy/launch/sim_2d_single.launch
@@ -17,7 +17,8 @@
 
     <node name="vel_updater" pkg="buggy" type="velocity_updater.py"
         output="screen" args="$(arg velocity) $(arg buggy_name)"/>
-
+    <node name="velocity_ui" pkg="buggy" type="velocity_ui.py"
+        output="screen" args="$(arg velocity) $(arg buggy_name)"/>
     <!-- autonsystem args: controller start_dist path buggy_name is_sim -->
     <node name="auton_system" pkg="buggy" type="autonsystem.py" output="screen" 
         args="$(arg controller) $(arg start_dist) $(arg path) $(arg buggy_name) True"/>
diff --git a/rb_ws/src/buggy/scripts/2d_sim/controller_2d.py b/rb_ws/src/buggy/scripts/2d_sim/controller_2d.py
index 4bceecb0..f5443114 100755
--- a/rb_ws/src/buggy/scripts/2d_sim/controller_2d.py
+++ b/rb_ws/src/buggy/scripts/2d_sim/controller_2d.py
@@ -9,7 +9,6 @@ class Controller:
     def __init__(self, buggy_name):
         self.steering_publisher = rospy.Publisher(buggy_name + "/input/steering", Float64, queue_size=10)
         self.velocity_publisher = rospy.Publisher(buggy_name + "/velocity", Float64, queue_size=10)
-        self.manual_velocity_publisher = rospy.Publisher(buggy_name + "/man_velocity", Float64, queue_size=10)
         self.steering_angle = 0
         self.velocity = 0
 
diff --git a/rb_ws/src/buggy/scripts/2d_sim/engine.py b/rb_ws/src/buggy/scripts/2d_sim/engine.py
index f07c1aa6..f5cffb65 100755
--- a/rb_ws/src/buggy/scripts/2d_sim/engine.py
+++ b/rb_ws/src/buggy/scripts/2d_sim/engine.py
@@ -20,9 +20,7 @@ class Simulator:
     # Start positions for Outdoor track
     START_LAT = 40.443024364623916
     START_LONG = -79.9409643423245
-    NOISE = True
-
-    MANUAL_V_FLAG = True
+    NOISE = True # Noisy outputs for nav/odom?
 
     def __init__(self, starting_pose, velocity, buggy_name):
         """
@@ -38,13 +36,9 @@ def __init__(self, starting_pose, velocity, buggy_name):
         self.steering_subscriber = rospy.Subscriber(
             buggy_name + "/input/steering", Float64, self.update_steering_angle
         )
-        # To read from autogenerated velocity 
-        self.simvelocity_subscriber = rospy.Subscriber(
-            buggy_name + "/velocity", Float64, self.update_velocity, "simulated"
-        ) 
-        # To read from manually entered velocity
-        self.manvelocity_subscriber = rospy.Subscriber(
-            buggy_name + "/man_velocity", Float64, self.update_velocity, "manual"
+        # To read from velocity 
+        self.velocity_subscriber = rospy.Subscriber(
+            buggy_name + "/velocity", Float64, self.update_velocity
         ) 
 
         # to plot on Foxglove (no noise)
@@ -94,19 +88,16 @@ def update_steering_angle(self, data: Float64):
         with self.lock:
             self.steering_angle = data.data
 
-    def update_velocity(self, data: Float64, source):
+    def update_velocity(self, data: Float64):
         """Updates the velocity as callback function for subscriber
 
         Args:
             data (Float64): velocity in m/s
             source (string): whether incoming data is manual or simulated
         """
-        if (source == "simulated" and not Simulator.MANUAL_V_FLAG):
-            with self.lock:
-                self.velocity = data.data
-        if (source == "manual" and Simulator.MANUAL_V_FLAG):
-            with self.lock:
-                self.velocity = data.data
+        with self.lock:
+            self.velocity = data.data
+        
             
     def get_steering_arc(self):
         # Adapted from simulator.py (Joseph Li)
diff --git a/rb_ws/src/buggy/scripts/2d_sim/velocity_ui.py b/rb_ws/src/buggy/scripts/2d_sim/velocity_ui.py
new file mode 100644
index 00000000..a5138bfc
--- /dev/null
+++ b/rb_ws/src/buggy/scripts/2d_sim/velocity_ui.py
@@ -0,0 +1,47 @@
+#! /usr/bin/env python3
+import numpy as np
+import rospy
+import sys
+from controller_2d import Controller
+from std_msgs.msg import Float64
+import threading
+import math
+
+class VelocityUI:
+    
+    def __init__(self, init_vel: float, buggy_name: str):
+        # To provide the built in Foxglove UI a location to publish to
+        self.manual_velocity_publisher = rospy.Publisher(buggy_name + "/input_velocity", Float64, queue_size=10)
+
+        self.buggy_vel = 0 # So the buggy doesn't start moving without user input
+
+        self.controller = Controller(buggy_name)
+        self.lock = threading.Lock()
+
+        
+        self.manual_velocity_subscriber = rospy.Subscriber(
+            buggy_name + "/input_velocity", Float64, self.update_velocity
+        ) 
+
+
+
+    def update_velocity(self, data: Float64):
+        with self.lock:
+            self.buggy_vel = data.data
+
+    def step(self):
+        '''Update velocity of buggy'''
+        self.controller.set_velocity(self.buggy_vel)
+
+
+if __name__ == "__main__":
+    rospy.init_node("velocity_ui")
+
+    init_vel = float(sys.argv[1])
+    buggy_name = sys.argv[2]
+    vel = VelocityUI(init_vel, buggy_name)
+    rate = rospy.Rate(100)
+
+    while not rospy.is_shutdown():
+        vel.step()
+        rate.sleep()

From 7041a8d5191a771d0aa7cd87e1d9a588d38a0368 Mon Sep 17 00:00:00 2001
From: TiaSinghania <samanvita.singhania@gmail.com>
Date: Sun, 26 Nov 2023 20:02:02 -0500
Subject: [PATCH 11/21] slider shows up

---
 python-requirements.txt                       |  2 ++
 rb_ws/src/buggy/launch/sim_2d_single.launch   |  2 --
 .../src/buggy/scripts/2d_sim/controller_2d.py |  1 +
 rb_ws/src/buggy/scripts/2d_sim/velocity_ui.py | 34 +++++++++++++------
 4 files changed, 26 insertions(+), 13 deletions(-)

diff --git a/python-requirements.txt b/python-requirements.txt
index cbc5d7de..254c5dac 100755
--- a/python-requirements.txt
+++ b/python-requirements.txt
@@ -8,3 +8,5 @@ pymap3d==3.0.1
 scipy==1.10.1
 trimesh==3.23.5
 utm==0.7.0
+keyboard==0.13.5
+tk==0.1.0
\ No newline at end of file
diff --git a/rb_ws/src/buggy/launch/sim_2d_single.launch b/rb_ws/src/buggy/launch/sim_2d_single.launch
index cffe7a2f..d2602305 100644
--- a/rb_ws/src/buggy/launch/sim_2d_single.launch
+++ b/rb_ws/src/buggy/launch/sim_2d_single.launch
@@ -15,8 +15,6 @@
     <node name="sim_2d_engine" pkg="buggy" type="engine.py" output="screen"
         args="$(arg starting_pose) $(arg velocity) $(arg buggy_name)"/>
 
-    <node name="vel_updater" pkg="buggy" type="velocity_updater.py"
-        output="screen" args="$(arg velocity) $(arg buggy_name)"/>
     <node name="velocity_ui" pkg="buggy" type="velocity_ui.py"
         output="screen" args="$(arg velocity) $(arg buggy_name)"/>
     <!-- autonsystem args: controller start_dist path buggy_name is_sim -->
diff --git a/rb_ws/src/buggy/scripts/2d_sim/controller_2d.py b/rb_ws/src/buggy/scripts/2d_sim/controller_2d.py
index f5443114..57bb0701 100755
--- a/rb_ws/src/buggy/scripts/2d_sim/controller_2d.py
+++ b/rb_ws/src/buggy/scripts/2d_sim/controller_2d.py
@@ -31,6 +31,7 @@ def set_steering(self, angle: float):
         self.steering_publisher.publish(msg)
     
     def set_velocity(self, vel: float):
+        print("setting vel in controller!")
         """Set the velocity and publish to simulator engine
 
         Args:
diff --git a/rb_ws/src/buggy/scripts/2d_sim/velocity_ui.py b/rb_ws/src/buggy/scripts/2d_sim/velocity_ui.py
index a5138bfc..b302bdf8 100644
--- a/rb_ws/src/buggy/scripts/2d_sim/velocity_ui.py
+++ b/rb_ws/src/buggy/scripts/2d_sim/velocity_ui.py
@@ -6,32 +6,43 @@
 from std_msgs.msg import Float64
 import threading
 import math
+import tkinter as tk
+import keyboard
 
 class VelocityUI:
-    
+       
+
     def __init__(self, init_vel: float, buggy_name: str):
         # To provide the built in Foxglove UI a location to publish to
-        self.manual_velocity_publisher = rospy.Publisher(buggy_name + "/input_velocity", Float64, queue_size=10)
+        self.manual_velocity_publisher = rospy.Publisher(buggy_name + "/velocity", Float64, queue_size=10)
 
         self.buggy_vel = 0 # So the buggy doesn't start moving without user input
 
         self.controller = Controller(buggy_name)
         self.lock = threading.Lock()
 
-        
-        self.manual_velocity_subscriber = rospy.Subscriber(
-            buggy_name + "/input_velocity", Float64, self.update_velocity
-        ) 
+        root = tk.Tk()
 
+        root.title('Manual Velocity')
+        root.geometry('600x100')
+        root.configure(background = '#342d66')
 
+        self.scale = tk.Scale(root, from_=0, to=300, orient=tk.HORIZONTAL, length=500, width=30)
+        self.scale.pack()
 
-    def update_velocity(self, data: Float64):
-        with self.lock:
-            self.buggy_vel = data.data
+        root.mainloop()
 
     def step(self):
         '''Update velocity of buggy'''
-        self.controller.set_velocity(self.buggy_vel)
+        print("calling step!")
+        print("v: " + self.scale.get())
+        self.buggy_vel = self.scale.get()/10
+        self.controller.set_velocity(self.buggy_vel())
+        # if (keyboard.is_pressed('w')):
+        #     self.controller.set_velocity(self.buggy_vel + 1)
+        # elif (keyboard.is_pressed('s')):
+        #     self.controller.set_velocity(self.buggy_vel - 1)
+            
 
 
 if __name__ == "__main__":
@@ -41,7 +52,8 @@ def step(self):
     buggy_name = sys.argv[2]
     vel = VelocityUI(init_vel, buggy_name)
     rate = rospy.Rate(100)
-
+    print("im working")
     while not rospy.is_shutdown():
+        print("stepping")
         vel.step()
         rate.sleep()

From 5ddecf43e8e26af71eb074840c1cc79c01f95782 Mon Sep 17 00:00:00 2001
From: TiaSinghania <samanvita.singhania@gmail.com>
Date: Mon, 27 Nov 2023 12:44:14 -0500
Subject: [PATCH 12/21] reverted changes to launch file, publisher reads from
 slider

---
 rb_ws/src/buggy/launch/sim_2d_single.launch   |  5 ++--
 rb_ws/src/buggy/scripts/2d_sim/velocity_ui.py | 30 +++++++------------
 .../buggy/scripts/2d_sim/velocity_updater.py  |  1 +
 3 files changed, 14 insertions(+), 22 deletions(-)

diff --git a/rb_ws/src/buggy/launch/sim_2d_single.launch b/rb_ws/src/buggy/launch/sim_2d_single.launch
index d2602305..75cc97c3 100644
--- a/rb_ws/src/buggy/launch/sim_2d_single.launch
+++ b/rb_ws/src/buggy/launch/sim_2d_single.launch
@@ -14,11 +14,10 @@
 
     <node name="sim_2d_engine" pkg="buggy" type="engine.py" output="screen"
         args="$(arg starting_pose) $(arg velocity) $(arg buggy_name)"/>
-
-    <node name="velocity_ui" pkg="buggy" type="velocity_ui.py"
+    <node name="vel_updater" pkg="buggy" type="velocity_updater.py"
         output="screen" args="$(arg velocity) $(arg buggy_name)"/>
     <!-- autonsystem args: controller start_dist path buggy_name is_sim -->
     <node name="auton_system" pkg="buggy" type="autonsystem.py" output="screen" 
         args="$(arg controller) $(arg start_dist) $(arg path) $(arg buggy_name) True"/>
 
-</launch>
+</launch>
\ No newline at end of file
diff --git a/rb_ws/src/buggy/scripts/2d_sim/velocity_ui.py b/rb_ws/src/buggy/scripts/2d_sim/velocity_ui.py
index b302bdf8..a7d2cd19 100644
--- a/rb_ws/src/buggy/scripts/2d_sim/velocity_ui.py
+++ b/rb_ws/src/buggy/scripts/2d_sim/velocity_ui.py
@@ -13,36 +13,30 @@ class VelocityUI:
        
 
     def __init__(self, init_vel: float, buggy_name: str):
-        # To provide the built in Foxglove UI a location to publish to
-        self.manual_velocity_publisher = rospy.Publisher(buggy_name + "/velocity", Float64, queue_size=10)
-
         self.buggy_vel = 0 # So the buggy doesn't start moving without user input
 
         self.controller = Controller(buggy_name)
         self.lock = threading.Lock()
 
-        root = tk.Tk()
+        self.root = tk.Tk()
 
-        root.title('Manual Velocity')
-        root.geometry('600x100')
-        root.configure(background = '#342d66')
+        self.root.title('Manual Velocity')
+        self.root.geometry('600x100')
+        self.root.configure(background = '#342d66')
 
-        self.scale = tk.Scale(root, from_=0, to=300, orient=tk.HORIZONTAL, length=500, width=30)
+        self.scale = tk.Scale(self.root, from_=0, to=300, orient=tk.HORIZONTAL, length=500, width=30)
         self.scale.pack()
 
-        root.mainloop()
+       
 
     def step(self):
+        self.root.update_idletasks()
+        self.root.update()
         '''Update velocity of buggy'''
-        print("calling step!")
-        print("v: " + self.scale.get())
+        print("v: " + str(self.scale.get()))
         self.buggy_vel = self.scale.get()/10
-        self.controller.set_velocity(self.buggy_vel())
-        # if (keyboard.is_pressed('w')):
-        #     self.controller.set_velocity(self.buggy_vel + 1)
-        # elif (keyboard.is_pressed('s')):
-        #     self.controller.set_velocity(self.buggy_vel - 1)
-            
+        self.controller.set_velocity(self.buggy_vel)
+          
 
 
 if __name__ == "__main__":
@@ -52,8 +46,6 @@ def step(self):
     buggy_name = sys.argv[2]
     vel = VelocityUI(init_vel, buggy_name)
     rate = rospy.Rate(100)
-    print("im working")
     while not rospy.is_shutdown():
-        print("stepping")
         vel.step()
         rate.sleep()
diff --git a/rb_ws/src/buggy/scripts/2d_sim/velocity_updater.py b/rb_ws/src/buggy/scripts/2d_sim/velocity_updater.py
index a6bebe18..a40cbcbb 100755
--- a/rb_ws/src/buggy/scripts/2d_sim/velocity_updater.py
+++ b/rb_ws/src/buggy/scripts/2d_sim/velocity_updater.py
@@ -62,6 +62,7 @@ def step(self):
 
 
 if __name__ == "__main__":
+    print("velocity updater main ??? \n\n\n")
     rospy.init_node("vel_updater")
 
     init_vel = float(sys.argv[1])

From 12a762ff51df3dc7ceeea506e44c9aba6666b4a8 Mon Sep 17 00:00:00 2001
From: TiaSinghania <samanvita.singhania@gmail.com>
Date: Mon, 27 Nov 2023 14:18:57 -0500
Subject: [PATCH 13/21] removed print debugs + bound scale to arrow keys

---
 rb_ws/src/buggy/launch/sim_2d_single.launch        | 2 +-
 rb_ws/src/buggy/scripts/2d_sim/controller_2d.py    | 1 -
 rb_ws/src/buggy/scripts/2d_sim/velocity_ui.py      | 5 ++++-
 rb_ws/src/buggy/scripts/2d_sim/velocity_updater.py | 1 -
 4 files changed, 5 insertions(+), 4 deletions(-)

diff --git a/rb_ws/src/buggy/launch/sim_2d_single.launch b/rb_ws/src/buggy/launch/sim_2d_single.launch
index 75cc97c3..6591e2af 100644
--- a/rb_ws/src/buggy/launch/sim_2d_single.launch
+++ b/rb_ws/src/buggy/launch/sim_2d_single.launch
@@ -14,7 +14,7 @@
 
     <node name="sim_2d_engine" pkg="buggy" type="engine.py" output="screen"
         args="$(arg starting_pose) $(arg velocity) $(arg buggy_name)"/>
-    <node name="vel_updater" pkg="buggy" type="velocity_updater.py"
+    <node name="velocity_ui" pkg="buggy" type="velocity_ui.py"
         output="screen" args="$(arg velocity) $(arg buggy_name)"/>
     <!-- autonsystem args: controller start_dist path buggy_name is_sim -->
     <node name="auton_system" pkg="buggy" type="autonsystem.py" output="screen" 
diff --git a/rb_ws/src/buggy/scripts/2d_sim/controller_2d.py b/rb_ws/src/buggy/scripts/2d_sim/controller_2d.py
index 57bb0701..f5443114 100755
--- a/rb_ws/src/buggy/scripts/2d_sim/controller_2d.py
+++ b/rb_ws/src/buggy/scripts/2d_sim/controller_2d.py
@@ -31,7 +31,6 @@ def set_steering(self, angle: float):
         self.steering_publisher.publish(msg)
     
     def set_velocity(self, vel: float):
-        print("setting vel in controller!")
         """Set the velocity and publish to simulator engine
 
         Args:
diff --git a/rb_ws/src/buggy/scripts/2d_sim/velocity_ui.py b/rb_ws/src/buggy/scripts/2d_sim/velocity_ui.py
index a7d2cd19..5d4e4c57 100644
--- a/rb_ws/src/buggy/scripts/2d_sim/velocity_ui.py
+++ b/rb_ws/src/buggy/scripts/2d_sim/velocity_ui.py
@@ -27,13 +27,16 @@ def __init__(self, init_vel: float, buggy_name: str):
         self.scale = tk.Scale(self.root, from_=0, to=300, orient=tk.HORIZONTAL, length=500, width=30)
         self.scale.pack()
 
+        self.root.bind("<Up>", lambda i: self.scale.set(self.scale.get() - 2))
+        self.root.bind("<Down>", lambda d: self.scale.set(self.scale.get() + 2))
+
+
        
 
     def step(self):
         self.root.update_idletasks()
         self.root.update()
         '''Update velocity of buggy'''
-        print("v: " + str(self.scale.get()))
         self.buggy_vel = self.scale.get()/10
         self.controller.set_velocity(self.buggy_vel)
           
diff --git a/rb_ws/src/buggy/scripts/2d_sim/velocity_updater.py b/rb_ws/src/buggy/scripts/2d_sim/velocity_updater.py
index a40cbcbb..a6bebe18 100755
--- a/rb_ws/src/buggy/scripts/2d_sim/velocity_updater.py
+++ b/rb_ws/src/buggy/scripts/2d_sim/velocity_updater.py
@@ -62,7 +62,6 @@ def step(self):
 
 
 if __name__ == "__main__":
-    print("velocity updater main ??? \n\n\n")
     rospy.init_node("vel_updater")
 
     init_vel = float(sys.argv[1])

From e1f98182ceffc30cb8d305db65cf5c2aa844506d Mon Sep 17 00:00:00 2001
From: TiaSinghania <samanvita.singhania@gmail.com>
Date: Mon, 27 Nov 2023 14:19:40 -0500
Subject: [PATCH 14/21] reversed binding

---
 rb_ws/src/buggy/scripts/2d_sim/velocity_ui.py | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/rb_ws/src/buggy/scripts/2d_sim/velocity_ui.py b/rb_ws/src/buggy/scripts/2d_sim/velocity_ui.py
index 5d4e4c57..e4544b43 100644
--- a/rb_ws/src/buggy/scripts/2d_sim/velocity_ui.py
+++ b/rb_ws/src/buggy/scripts/2d_sim/velocity_ui.py
@@ -27,8 +27,8 @@ def __init__(self, init_vel: float, buggy_name: str):
         self.scale = tk.Scale(self.root, from_=0, to=300, orient=tk.HORIZONTAL, length=500, width=30)
         self.scale.pack()
 
-        self.root.bind("<Up>", lambda i: self.scale.set(self.scale.get() - 2))
-        self.root.bind("<Down>", lambda d: self.scale.set(self.scale.get() + 2))
+        self.root.bind("<Up>", lambda i: self.scale.set(self.scale.get() + 2))
+        self.root.bind("<Down>", lambda d: self.scale.set(self.scale.get() - 2))
 
 
        

From b8ee09f30bc4befde829b88076f17122b21035f3 Mon Sep 17 00:00:00 2001
From: TiaSinghania <samanvita.singhania@gmail.com>
Date: Wed, 29 Nov 2023 10:40:30 -0500
Subject: [PATCH 15/21] merge main

---
 rb_ws/src/buggy/launch/sim_2d_single.launch | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/rb_ws/src/buggy/launch/sim_2d_single.launch b/rb_ws/src/buggy/launch/sim_2d_single.launch
index 6591e2af..75cc97c3 100644
--- a/rb_ws/src/buggy/launch/sim_2d_single.launch
+++ b/rb_ws/src/buggy/launch/sim_2d_single.launch
@@ -14,7 +14,7 @@
 
     <node name="sim_2d_engine" pkg="buggy" type="engine.py" output="screen"
         args="$(arg starting_pose) $(arg velocity) $(arg buggy_name)"/>
-    <node name="velocity_ui" pkg="buggy" type="velocity_ui.py"
+    <node name="vel_updater" pkg="buggy" type="velocity_updater.py"
         output="screen" args="$(arg velocity) $(arg buggy_name)"/>
     <!-- autonsystem args: controller start_dist path buggy_name is_sim -->
     <node name="auton_system" pkg="buggy" type="autonsystem.py" output="screen" 

From 931c7a8c39f04de3b8c7dd8847b998ae3c64927a Mon Sep 17 00:00:00 2001
From: TiaSinghania <samanvita.singhania@gmail.com>
Date: Mon, 5 Feb 2024 00:35:49 -0500
Subject: [PATCH 16/21] minor cleanup

---
 rb_ws/src/buggy/scripts/2d_sim/controller_2d.py | 1 -
 rb_ws/src/buggy/scripts/2d_sim/velocity_ui.py   | 9 ++++-----
 rb_ws/src/buggy/scripts/auton/autonsystem.py    | 3 ++-
 3 files changed, 6 insertions(+), 7 deletions(-)

diff --git a/rb_ws/src/buggy/scripts/2d_sim/controller_2d.py b/rb_ws/src/buggy/scripts/2d_sim/controller_2d.py
index f5443114..4ca18a61 100755
--- a/rb_ws/src/buggy/scripts/2d_sim/controller_2d.py
+++ b/rb_ws/src/buggy/scripts/2d_sim/controller_2d.py
@@ -51,6 +51,5 @@ def set_velocity(self, vel: float):
     rate = rospy.Rate(5)
     i = 0
     while not rospy.is_shutdown():
-        controller.set_velocity(15)
         rate.sleep()
 
diff --git a/rb_ws/src/buggy/scripts/2d_sim/velocity_ui.py b/rb_ws/src/buggy/scripts/2d_sim/velocity_ui.py
index e4544b43..bed311f9 100644
--- a/rb_ws/src/buggy/scripts/2d_sim/velocity_ui.py
+++ b/rb_ws/src/buggy/scripts/2d_sim/velocity_ui.py
@@ -1,13 +1,9 @@
 #! /usr/bin/env python3
-import numpy as np
 import rospy
 import sys
 from controller_2d import Controller
-from std_msgs.msg import Float64
 import threading
-import math
 import tkinter as tk
-import keyboard
 
 class VelocityUI:
        
@@ -34,10 +30,13 @@ def __init__(self, init_vel: float, buggy_name: str):
        
 
     def step(self):
+        """sets velocity of buggy to the current scale value 
+            called once every tick
+        """
         self.root.update_idletasks()
         self.root.update()
         '''Update velocity of buggy'''
-        self.buggy_vel = self.scale.get()/10
+        self.buggy_vel = self.scale.get()/10 # so we can set velocity with 0.1 precision
         self.controller.set_velocity(self.buggy_vel)
           
 
diff --git a/rb_ws/src/buggy/scripts/auton/autonsystem.py b/rb_ws/src/buggy/scripts/auton/autonsystem.py
index 22039b0b..e8d783c0 100755
--- a/rb_ws/src/buggy/scripts/auton/autonsystem.py
+++ b/rb_ws/src/buggy/scripts/auton/autonsystem.py
@@ -53,7 +53,8 @@ def __init__(self, trajectory, controller, brake_controller, buggy_name, is_sim)
             rospy.Subscriber(buggy_name + "/nav/odom", Odometry, self.update_msg)
 
 
-        rospy.Subscriber("nav/odom", Odometry, self.update_msg)
+        rospy.Subscriber(buggy_name + "nav/odom", Odometry, self.update_msg)
+        
         self.covariance_warning_publisher = rospy.Publisher(
             buggy_name + "/debug/is_high_covariance", Bool, queue_size=1
         )

From 7624e65a5a923482a64f2784d627454ef2f4edea Mon Sep 17 00:00:00 2001
From: TiaSinghania <samanvita.singhania@gmail.com>
Date: Mon, 5 Feb 2024 08:49:18 -0500
Subject: [PATCH 17/21] reverted imports

---
 rb_ws/src/buggy/scripts/2d_sim/velocity_ui.py | 4 ++++
 1 file changed, 4 insertions(+)

diff --git a/rb_ws/src/buggy/scripts/2d_sim/velocity_ui.py b/rb_ws/src/buggy/scripts/2d_sim/velocity_ui.py
index bed311f9..83357f7b 100644
--- a/rb_ws/src/buggy/scripts/2d_sim/velocity_ui.py
+++ b/rb_ws/src/buggy/scripts/2d_sim/velocity_ui.py
@@ -1,9 +1,13 @@
 #! /usr/bin/env python3
+import numpy as np
 import rospy
 import sys
 from controller_2d import Controller
+from std_msgs.msg import Float64
 import threading
+import math
 import tkinter as tk
+import keyboard
 
 class VelocityUI:
        

From f79d1e916bf9c469c2d2e25cdadf2db862d76981 Mon Sep 17 00:00:00 2001
From: TiaSinghania <samanvita.singhania@gmail.com>
Date: Mon, 5 Feb 2024 09:43:05 -0500
Subject: [PATCH 18/21] cleanup

---
 rb_ws/src/buggy/scripts/2d_sim/controller_2d.py |  7 +------
 rb_ws/src/buggy/scripts/2d_sim/engine.py        | 16 ++--------------
 rb_ws/src/buggy/scripts/2d_sim/velocity_ui.py   | 11 -----------
 rb_ws/src/buggy/scripts/auton/autonsystem.py    | 10 ++--------
 4 files changed, 5 insertions(+), 39 deletions(-)

diff --git a/rb_ws/src/buggy/scripts/2d_sim/controller_2d.py b/rb_ws/src/buggy/scripts/2d_sim/controller_2d.py
index 4ca18a61..2caa89dd 100755
--- a/rb_ws/src/buggy/scripts/2d_sim/controller_2d.py
+++ b/rb_ws/src/buggy/scripts/2d_sim/controller_2d.py
@@ -1,9 +1,8 @@
 #! /usr/bin/env python3
 import sys
+import threading
 import rospy
 from std_msgs.msg import Float64
-import threading
-import numpy as np
 
 class Controller:
     def __init__(self, buggy_name):
@@ -15,7 +14,6 @@ def __init__(self, buggy_name):
         self.lock = threading.Lock()
         self.set_steering(0)
         self.set_velocity(0)
-    
     def set_steering(self, angle: float):
         """Set the steering angle and publish to simulator engine
 
@@ -27,9 +25,7 @@ def set_steering(self, angle: float):
 
         with self.lock:
             self.steering_angle = angle
-        
         self.steering_publisher.publish(msg)
-    
     def set_velocity(self, vel: float):
         """Set the velocity and publish to simulator engine
 
@@ -41,7 +37,6 @@ def set_velocity(self, vel: float):
 
         with self.lock:
             self.velocity = vel
-        
         self.velocity_publisher.publish(msg)
 
 if __name__ == "__main__":
diff --git a/rb_ws/src/buggy/scripts/2d_sim/engine.py b/rb_ws/src/buggy/scripts/2d_sim/engine.py
index f5cffb65..bc596876 100755
--- a/rb_ws/src/buggy/scripts/2d_sim/engine.py
+++ b/rb_ws/src/buggy/scripts/2d_sim/engine.py
@@ -1,5 +1,4 @@
 #! /usr/bin/env python3
-import rospy
 from geometry_msgs.msg import Pose, Twist, PoseWithCovariance, TwistWithCovariance
 from std_msgs.msg import Float64
 from sensor_msgs.msg import NavSatFix
@@ -7,8 +6,8 @@
 import threading
 import numpy as np
 import utm
-import time
 import sys
+import rospy
 
 
 class Simulator:
@@ -40,7 +39,6 @@ def __init__(self, starting_pose, velocity, buggy_name):
         self.velocity_subscriber = rospy.Subscriber(
             buggy_name + "/velocity", Float64, self.update_velocity
         ) 
-
         # to plot on Foxglove (no noise)
         self.navsatfix_publisher = rospy.Publisher(
             buggy_name + "/state/pose_navsat", NavSatFix, queue_size=1
@@ -50,7 +48,6 @@ def __init__(self, starting_pose, velocity, buggy_name):
         self.navsatfix_noisy_publisher = rospy.Publisher(
             buggy_name + "/state/pose_navsat_noisy", NavSatFix, queue_size=1
         )
-
         # (UTM east, UTM north, HEADING(degs))
         self.starting_poses = {
             "Hill1_SC": (Simulator.UTM_EAST_ZERO + 60, Simulator.UTM_NORTH_ZERO + 150, -110),
@@ -69,7 +66,6 @@ def __init__(self, starting_pose, velocity, buggy_name):
         # utm_coords = utm.from_latlon(Simulator.START_LAT, Simulator.START_LONG)
         # self.e_utm = utm_coords[0]
         # self.n_utm = utm_coords[1]
-
         self.e_utm, self.n_utm, self.heading = self.starting_poses[starting_pose]        
         self.velocity = velocity # m/s
 
@@ -96,9 +92,7 @@ def update_velocity(self, data: Float64):
             source (string): whether incoming data is manual or simulated
         """
         with self.lock:
-            self.velocity = data.data
-        
-            
+            self.velocity = data.data  
     def get_steering_arc(self):
         # Adapted from simulator.py (Joseph Li)
         # calculate the radius of the steering arc
@@ -112,7 +106,6 @@ def get_steering_arc(self):
             return np.inf
 
         return Simulator.WHEELBASE / np.tan(np.deg2rad(steering_angle))
-
     def dynamics(self, state, v):
         """ Calculates continuous time bicycle dynamics as a function of state and velocity
 
@@ -139,7 +132,6 @@ def step(self):
             n_utm = self.n_utm
             velocity = self.velocity
             steering_angle = self.steering_angle
-        
         # Calculate new position
         if steering_angle == 0.0:
             # Straight
@@ -149,9 +141,7 @@ def step(self):
         else:
             # steering radius
             radius = self.get_steering_arc()
-
             distance = velocity / self.rate
-
             delta_heading = distance / radius
             heading_new = heading + np.rad2deg(delta_heading) / 2
             e_utm_new = e_utm + (velocity / self.rate) * np.cos(np.deg2rad(heading_new))
@@ -162,12 +152,10 @@ def step(self):
             self.e_utm = e_utm_new
             self.n_utm = n_utm_new
             self.heading = heading_new
-
     def publish(self):
         """Publishes the pose the arrow in visualizer should be at"""
         p = Pose()
         time_stamp = rospy.Time.now()
-
         with self.lock:
             p.position.x = self.e_utm
             p.position.y = self.n_utm
diff --git a/rb_ws/src/buggy/scripts/2d_sim/velocity_ui.py b/rb_ws/src/buggy/scripts/2d_sim/velocity_ui.py
index 83357f7b..4444c71b 100644
--- a/rb_ws/src/buggy/scripts/2d_sim/velocity_ui.py
+++ b/rb_ws/src/buggy/scripts/2d_sim/velocity_ui.py
@@ -1,20 +1,15 @@
 #! /usr/bin/env python3
-import numpy as np
 import rospy
 import sys
 from controller_2d import Controller
-from std_msgs.msg import Float64
 import threading
-import math
 import tkinter as tk
-import keyboard
 
 class VelocityUI:
        
 
     def __init__(self, init_vel: float, buggy_name: str):
         self.buggy_vel = 0 # So the buggy doesn't start moving without user input
-
         self.controller = Controller(buggy_name)
         self.lock = threading.Lock()
 
@@ -30,9 +25,6 @@ def __init__(self, init_vel: float, buggy_name: str):
         self.root.bind("<Up>", lambda i: self.scale.set(self.scale.get() + 2))
         self.root.bind("<Down>", lambda d: self.scale.set(self.scale.get() - 2))
 
-
-       
-
     def step(self):
         """sets velocity of buggy to the current scale value 
             called once every tick
@@ -43,11 +35,8 @@ def step(self):
         self.buggy_vel = self.scale.get()/10 # so we can set velocity with 0.1 precision
         self.controller.set_velocity(self.buggy_vel)
           
-
-
 if __name__ == "__main__":
     rospy.init_node("velocity_ui")
-
     init_vel = float(sys.argv[1])
     buggy_name = sys.argv[2]
     vel = VelocityUI(init_vel, buggy_name)
diff --git a/rb_ws/src/buggy/scripts/auton/autonsystem.py b/rb_ws/src/buggy/scripts/auton/autonsystem.py
index e8d783c0..3931850d 100755
--- a/rb_ws/src/buggy/scripts/auton/autonsystem.py
+++ b/rb_ws/src/buggy/scripts/auton/autonsystem.py
@@ -1,6 +1,5 @@
 #!/usr/bin/env python3
 
-import rospy
 import sys
 
 # ROS Message Imports
@@ -9,6 +8,8 @@
 
 import numpy as np
 from threading import Lock
+import rospy
+
 
 from trajectory import Trajectory
 from world import World
@@ -35,24 +36,19 @@ class AutonSystem:
     controller: Controller = None
     brake_controller: BrakeController = None
     lock = None
-
     steer_publisher = None
-    
     ticks = 0
 
     def __init__(self, trajectory, controller, brake_controller, buggy_name, is_sim) -> None:
         self.trajectory = trajectory
         self.controller = controller
         self.brake_controller = brake_controller
-
         self.lock = Lock()
         self.ticks = 0
         self.msg = None
-        
         if (is_sim):
             rospy.Subscriber(buggy_name + "/nav/odom", Odometry, self.update_msg)
 
-
         rospy.Subscriber(buggy_name + "nav/odom", Odometry, self.update_msg)
         
         self.covariance_warning_publisher = rospy.Publisher(
@@ -90,7 +86,6 @@ def update_msg(self, msg):
     def tick_caller(self):
         while ((not rospy.is_shutdown()) and (self.msg == None)): # with no message, we wait
             rospy.sleep(0.001)
-        
         # wait for covariance matrix to be better
         while ((not rospy.is_shutdown()) and
                (self.msg.pose.covariance[0] ** 2 + self.msg.pose.covariance[7] ** 2 > 1**2)):
@@ -177,7 +172,6 @@ def tick(self):
         ctrller = ModelPredictiveController(buggy_name, start_index)
     if (ctrller == None):
         raise Exception("Invalid Controller Argument")
-    
     auton_system = AutonSystem(
         trajectory,
         ctrller,

From 507462b8885ca2d1ee3589c590238a5dfd3134ba Mon Sep 17 00:00:00 2001
From: TiaSinghania <samanvita.singhania@gmail.com>
Date: Mon, 5 Feb 2024 10:34:55 -0500
Subject: [PATCH 19/21] more cleanup

---
 rb_ws/src/buggy/scripts/2d_sim/engine.py      | 14 +++++++-------
 rb_ws/src/buggy/scripts/2d_sim/velocity_ui.py | 10 ++++------
 rb_ws/src/buggy/scripts/auton/autonsystem.py  | 12 +++++-------
 3 files changed, 16 insertions(+), 20 deletions(-)

diff --git a/rb_ws/src/buggy/scripts/2d_sim/engine.py b/rb_ws/src/buggy/scripts/2d_sim/engine.py
index bc596876..c2585b25 100755
--- a/rb_ws/src/buggy/scripts/2d_sim/engine.py
+++ b/rb_ws/src/buggy/scripts/2d_sim/engine.py
@@ -1,12 +1,12 @@
 #! /usr/bin/env python3
+import sys
+import threading
 from geometry_msgs.msg import Pose, Twist, PoseWithCovariance, TwistWithCovariance
 from std_msgs.msg import Float64
 from sensor_msgs.msg import NavSatFix
 from nav_msgs.msg import Odometry
-import threading
 import numpy as np
 import utm
-import sys
 import rospy
 
 
@@ -35,10 +35,10 @@ def __init__(self, starting_pose, velocity, buggy_name):
         self.steering_subscriber = rospy.Subscriber(
             buggy_name + "/input/steering", Float64, self.update_steering_angle
         )
-        # To read from velocity 
+        # To read from velocity
         self.velocity_subscriber = rospy.Subscriber(
             buggy_name + "/velocity", Float64, self.update_velocity
-        ) 
+        )
         # to plot on Foxglove (no noise)
         self.navsatfix_publisher = rospy.Publisher(
             buggy_name + "/state/pose_navsat", NavSatFix, queue_size=1
@@ -66,7 +66,7 @@ def __init__(self, starting_pose, velocity, buggy_name):
         # utm_coords = utm.from_latlon(Simulator.START_LAT, Simulator.START_LONG)
         # self.e_utm = utm_coords[0]
         # self.n_utm = utm_coords[1]
-        self.e_utm, self.n_utm, self.heading = self.starting_poses[starting_pose]        
+        self.e_utm, self.n_utm, self.heading = self.starting_poses[starting_pose]
         self.velocity = velocity # m/s
 
         self.steering_angle = 0  # degrees
@@ -92,7 +92,7 @@ def update_velocity(self, data: Float64):
             source (string): whether incoming data is manual or simulated
         """
         with self.lock:
-            self.velocity = data.data  
+            self.velocity = data.data
     def get_steering_arc(self):
         # Adapted from simulator.py (Joseph Li)
         # calculate the radius of the steering arc
@@ -126,7 +126,7 @@ def dynamics(self, state, v):
 
     def step(self):
         """Complete the step at a certain Hz here. Do physics"""
-        with self.lock: 
+        with self.lock:
             heading = self.heading
             e_utm = self.e_utm
             n_utm = self.n_utm
diff --git a/rb_ws/src/buggy/scripts/2d_sim/velocity_ui.py b/rb_ws/src/buggy/scripts/2d_sim/velocity_ui.py
index 4444c71b..84d29a03 100644
--- a/rb_ws/src/buggy/scripts/2d_sim/velocity_ui.py
+++ b/rb_ws/src/buggy/scripts/2d_sim/velocity_ui.py
@@ -1,13 +1,11 @@
 #! /usr/bin/env python3
-import rospy
 import sys
-from controller_2d import Controller
 import threading
+from controller_2d import Controller
 import tkinter as tk
+import rospy
 
 class VelocityUI:
-       
-
     def __init__(self, init_vel: float, buggy_name: str):
         self.buggy_vel = 0 # So the buggy doesn't start moving without user input
         self.controller = Controller(buggy_name)
@@ -34,7 +32,7 @@ def step(self):
         '''Update velocity of buggy'''
         self.buggy_vel = self.scale.get()/10 # so we can set velocity with 0.1 precision
         self.controller.set_velocity(self.buggy_vel)
-          
+
 if __name__ == "__main__":
     rospy.init_node("velocity_ui")
     init_vel = float(sys.argv[1])
@@ -43,4 +41,4 @@ def step(self):
     rate = rospy.Rate(100)
     while not rospy.is_shutdown():
         vel.step()
-        rate.sleep()
+        rate.sleep()
\ No newline at end of file
diff --git a/rb_ws/src/buggy/scripts/auton/autonsystem.py b/rb_ws/src/buggy/scripts/auton/autonsystem.py
index 3931850d..8b50232d 100755
--- a/rb_ws/src/buggy/scripts/auton/autonsystem.py
+++ b/rb_ws/src/buggy/scripts/auton/autonsystem.py
@@ -1,15 +1,14 @@
 #!/usr/bin/env python3
 
 import sys
-
-# ROS Message Imports
-from std_msgs.msg import Float32, Float64, Bool
-from nav_msgs.msg import Odometry
+from threading import Lock
 
 import numpy as np
-from threading import Lock
 import rospy
 
+# ROS Message Imports
+from std_msgs.msg import Float32, Float64, Bool
+from nav_msgs.msg import Odometry
 
 from trajectory import Trajectory
 from world import World
@@ -50,7 +49,6 @@ def __init__(self, trajectory, controller, brake_controller, buggy_name, is_sim)
             rospy.Subscriber(buggy_name + "/nav/odom", Odometry, self.update_msg)
 
         rospy.Subscriber(buggy_name + "nav/odom", Odometry, self.update_msg)
-        
         self.covariance_warning_publisher = rospy.Publisher(
             buggy_name + "/debug/is_high_covariance", Bool, queue_size=1
         )
@@ -82,7 +80,7 @@ def update_speed(self, msg):
     def update_msg(self, msg):
         with self.lock:
             self.msg = msg
-        
+
     def tick_caller(self):
         while ((not rospy.is_shutdown()) and (self.msg == None)): # with no message, we wait
             rospy.sleep(0.001)

From e46bca6f88c30e1d99e20059c0d28ac3bc93885d Mon Sep 17 00:00:00 2001
From: TiaSinghania <samanvita.singhania@gmail.com>
Date: Mon, 5 Feb 2024 10:37:45 -0500
Subject: [PATCH 20/21] final cleanup

---
 rb_ws/src/buggy/scripts/2d_sim/engine.py      | 2 +-
 rb_ws/src/buggy/scripts/2d_sim/velocity_ui.py | 2 +-
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/rb_ws/src/buggy/scripts/2d_sim/engine.py b/rb_ws/src/buggy/scripts/2d_sim/engine.py
index c2585b25..43c03a85 100755
--- a/rb_ws/src/buggy/scripts/2d_sim/engine.py
+++ b/rb_ws/src/buggy/scripts/2d_sim/engine.py
@@ -117,7 +117,7 @@ def dynamics(self, state, v):
             dstate (np.Array): time derivative of state from dynamics
         """
         l = Simulator.WHEELBASE
-        x, y, theta, delta = state
+        theta, delta = state
 
         return np.array([v * np.cos(theta),
                          v * np.sin(theta),
diff --git a/rb_ws/src/buggy/scripts/2d_sim/velocity_ui.py b/rb_ws/src/buggy/scripts/2d_sim/velocity_ui.py
index 84d29a03..8a2061c4 100644
--- a/rb_ws/src/buggy/scripts/2d_sim/velocity_ui.py
+++ b/rb_ws/src/buggy/scripts/2d_sim/velocity_ui.py
@@ -1,8 +1,8 @@
 #! /usr/bin/env python3
 import sys
 import threading
-from controller_2d import Controller
 import tkinter as tk
+from controller_2d import Controller
 import rospy
 
 class VelocityUI:

From c157ed00cbf65378a8f33e56c5b9818842286420 Mon Sep 17 00:00:00 2001
From: TiaSinghania <samanvita.singhania@gmail.com>
Date: Wed, 7 Feb 2024 17:37:14 -0500
Subject: [PATCH 21/21] added wildcards to pattern match (please please approve
 i dont want to do merging)

---
 rb_ws/src/buggy/scripts/2d_sim/engine.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/rb_ws/src/buggy/scripts/2d_sim/engine.py b/rb_ws/src/buggy/scripts/2d_sim/engine.py
index 43c03a85..3d48bd34 100755
--- a/rb_ws/src/buggy/scripts/2d_sim/engine.py
+++ b/rb_ws/src/buggy/scripts/2d_sim/engine.py
@@ -117,7 +117,7 @@ def dynamics(self, state, v):
             dstate (np.Array): time derivative of state from dynamics
         """
         l = Simulator.WHEELBASE
-        theta, delta = state
+        _, _, theta, delta = state
 
         return np.array([v * np.cos(theta),
                          v * np.sin(theta),