From 79879a8d696c7143b78cda12e106659c8829418e Mon Sep 17 00:00:00 2001
From: TiaSinghania <samanvita.singhania@gmail.com>
Date: Mon, 21 Oct 2024 15:37:39 -0400
Subject: [PATCH 1/9] basic node setup

---
 rb_ws/src/buggy/launch/sim_2d_single.xml    |  3 +++
 rb_ws/src/buggy/scripts/simulator/engine.py | 27 +++++++++++++++++++++
 rb_ws/src/buggy/setup.py                    |  2 ++
 3 files changed, 32 insertions(+)
 create mode 100644 rb_ws/src/buggy/scripts/simulator/engine.py

diff --git a/rb_ws/src/buggy/launch/sim_2d_single.xml b/rb_ws/src/buggy/launch/sim_2d_single.xml
index e0b5846..0aed958 100755
--- a/rb_ws/src/buggy/launch/sim_2d_single.xml
+++ b/rb_ws/src/buggy/launch/sim_2d_single.xml
@@ -1,6 +1,9 @@
 <launch>
     <node pkg="foxglove_bridge" exec="foxglove_bridge" name="foxglove" namespace="SC"/>
     <node pkg="buggy" exec="hello_world" name="hello_world" namespace="SC"/>
+    <node pkg="buggy" exec="sim-single" name="SC-sim-single" namespace="SC"/>
+    <node pkg="buggy" exec="sim-single" name="NAND-sim-single" namespace="NAND"/>
+
     <!-- <node name="foxglove" pkg="foxglove_bridge" type="foxglove_bridge" /> -->
     <!-- <node name="hello_world" pkg="buggy" type="hello_world" /> -->
 </launch>
\ No newline at end of file
diff --git a/rb_ws/src/buggy/scripts/simulator/engine.py b/rb_ws/src/buggy/scripts/simulator/engine.py
new file mode 100644
index 0000000..f91efda
--- /dev/null
+++ b/rb_ws/src/buggy/scripts/simulator/engine.py
@@ -0,0 +1,27 @@
+#! /usr/bin/env python3
+import sys
+import time
+import rclpy
+from rclpy.node import Node
+
+class Simulator(Node):
+    # simulator constants:
+
+    def __init__(self):
+        if (self.get_namespace() == "SC"):
+            self.buggy_name = "SC"
+        if (self.get_namespace() == "NAND"):
+            self.buggy_name = "NAND"
+
+    if __name__ == "__main__":
+        rclpy.init()
+        sim = Simulator()
+        rclpy.spin(sim)
+
+        # publish initial position, then sleep
+        # so that auton stack has time to initialize
+        # before buggy moves
+        time.sleep(15.0)
+        sim.loop()
+
+
diff --git a/rb_ws/src/buggy/setup.py b/rb_ws/src/buggy/setup.py
index 4373639..b8edf92 100644
--- a/rb_ws/src/buggy/setup.py
+++ b/rb_ws/src/buggy/setup.py
@@ -25,6 +25,8 @@
     entry_points={
         'console_scripts': [
             'hello_world = buggy.hello_world:main'
+            'sim-single = buggy.engine:main'
+
         ],
     },
 )

From 1043ab8ee413589111fb1db65854c6de58f0318d Mon Sep 17 00:00:00 2001
From: TiaSinghania <samanvita.singhania@gmail.com>
Date: Tue, 22 Oct 2024 15:08:39 -0400
Subject: [PATCH 2/9] added publisher

---
 rb_ws/src/buggy/scripts/simulator/engine.py | 27 ++++++++++++---------
 1 file changed, 16 insertions(+), 11 deletions(-)

diff --git a/rb_ws/src/buggy/scripts/simulator/engine.py b/rb_ws/src/buggy/scripts/simulator/engine.py
index f91efda..d0bab70 100644
--- a/rb_ws/src/buggy/scripts/simulator/engine.py
+++ b/rb_ws/src/buggy/scripts/simulator/engine.py
@@ -2,26 +2,31 @@
 import sys
 import time
 import rclpy
-from rclpy.node import Node
+from std_msgs.msg import String
 
-class Simulator(Node):
+class Simulator():
     # simulator constants:
 
     def __init__(self):
+        self.test_publisher = self.create_publisher(String, 'test', 10)
         if (self.get_namespace() == "SC"):
             self.buggy_name = "SC"
+
         if (self.get_namespace() == "NAND"):
             self.buggy_name = "NAND"
 
-    if __name__ == "__main__":
-        rclpy.init()
-        sim = Simulator()
-        rclpy.spin(sim)
+    def loop(self):
+        print("hello")
+        self.test_publisher.publish(self.buggy_name)
+
+if __name__ == "__main__":
+    rclpy.init()
+    sim = Simulator()
 
-        # publish initial position, then sleep
-        # so that auton stack has time to initialize
-        # before buggy moves
-        time.sleep(15.0)
-        sim.loop()
+    # publish initial position, then sleep
+    # so that auton stack has time to initialize
+    # before buggy moves
+    time.sleep(15.0)
+    sim.loop()
 
 

From 858196f001f1864c4bbce49f8381f835122490e2 Mon Sep 17 00:00:00 2001
From: TiaSinghania <samanvita.singhania@gmail.com>
Date: Tue, 22 Oct 2024 21:11:31 -0400
Subject: [PATCH 3/9] implemented callback in loop with timer

---
 rb_ws/src/buggy/buggy/simulator/engine.py   | 49 +++++++++++++++++++++
 rb_ws/src/buggy/launch/sim_2d_single.xml    | 10 ++---
 rb_ws/src/buggy/scripts/simulator/engine.py | 32 --------------
 rb_ws/src/buggy/setup.py                    |  7 ++-
 4 files changed, 57 insertions(+), 41 deletions(-)
 create mode 100644 rb_ws/src/buggy/buggy/simulator/engine.py
 delete mode 100644 rb_ws/src/buggy/scripts/simulator/engine.py

diff --git a/rb_ws/src/buggy/buggy/simulator/engine.py b/rb_ws/src/buggy/buggy/simulator/engine.py
new file mode 100644
index 0000000..346ae3a
--- /dev/null
+++ b/rb_ws/src/buggy/buggy/simulator/engine.py
@@ -0,0 +1,49 @@
+#! /usr/bin/env python3
+import sys
+import time
+import rclpy
+from rclpy.node import Node
+
+from std_msgs.msg import String, Float64
+
+class Simulator(Node):
+    # simulator constants:
+
+    def __init__(self):
+        super().__init__('SC_sim_single')
+        self.get_logger().info("INITIALIZING")
+        self.number_publisher = self.create_publisher(Float64, 'numbers', 1)
+        self.test_publisher = self.create_publisher(String, 'test', 1)
+        self.i = 0
+
+        self.buggy_name = "NONE"
+
+        if (self.get_namespace() == "/SC"):
+            self.buggy_name = "SC"
+
+        if (self.get_namespace() == "/NAND"):
+            self.buggy_name = "NAND"
+
+        freq = 10
+        timer_period = 1/freq  # seconds
+        self.timer = self.create_timer(timer_period, self.loop)
+
+    def loop(self):
+
+        self.get_logger().info("LOOPING")
+
+        msg = String()
+        msg.data = self.buggy_name
+        self.test_publisher.publish(msg)
+        msg2 = Float64()
+        msg2.data = float(self.i)
+        self.number_publisher.publish(msg2)
+        self.i += 1
+
+def main(args=None):
+    rclpy.init(args=args)
+    sim = Simulator()
+    sim.get_logger().info("CREATED NODE")
+    rclpy.spin(sim)
+
+
diff --git a/rb_ws/src/buggy/launch/sim_2d_single.xml b/rb_ws/src/buggy/launch/sim_2d_single.xml
index 0aed958..550c259 100755
--- a/rb_ws/src/buggy/launch/sim_2d_single.xml
+++ b/rb_ws/src/buggy/launch/sim_2d_single.xml
@@ -1,9 +1,9 @@
 <launch>
     <node pkg="foxglove_bridge" exec="foxglove_bridge" name="foxglove" namespace="SC"/>
-    <node pkg="buggy" exec="hello_world" name="hello_world" namespace="SC"/>
-    <node pkg="buggy" exec="sim-single" name="SC-sim-single" namespace="SC"/>
-    <node pkg="buggy" exec="sim-single" name="NAND-sim-single" namespace="NAND"/>
+    <node pkg="buggy" exec="hello_world" name="hello_world" namespace="SC" />
+    <node pkg="buggy" exec="sim_single" name="SC_sim_single" namespace="SC"/>
+    <node pkg="buggy" exec="sim_single" name="NAND_sim_single" namespace="NAND"/>
 
-    <!-- <node name="foxglove" pkg="foxglove_bridge" type="foxglove_bridge" /> -->
-    <!-- <node name="hello_world" pkg="buggy" type="hello_world" /> -->
+    <!-- <node name="foxglove" pkg="foxglove_bridge" type="foxglove_bridge" />
+    <node name="hello_world" pkg="buggy" type="hello_world" /> -->
 </launch>
\ No newline at end of file
diff --git a/rb_ws/src/buggy/scripts/simulator/engine.py b/rb_ws/src/buggy/scripts/simulator/engine.py
deleted file mode 100644
index d0bab70..0000000
--- a/rb_ws/src/buggy/scripts/simulator/engine.py
+++ /dev/null
@@ -1,32 +0,0 @@
-#! /usr/bin/env python3
-import sys
-import time
-import rclpy
-from std_msgs.msg import String
-
-class Simulator():
-    # simulator constants:
-
-    def __init__(self):
-        self.test_publisher = self.create_publisher(String, 'test', 10)
-        if (self.get_namespace() == "SC"):
-            self.buggy_name = "SC"
-
-        if (self.get_namespace() == "NAND"):
-            self.buggy_name = "NAND"
-
-    def loop(self):
-        print("hello")
-        self.test_publisher.publish(self.buggy_name)
-
-if __name__ == "__main__":
-    rclpy.init()
-    sim = Simulator()
-
-    # publish initial position, then sleep
-    # so that auton stack has time to initialize
-    # before buggy moves
-    time.sleep(15.0)
-    sim.loop()
-
-
diff --git a/rb_ws/src/buggy/setup.py b/rb_ws/src/buggy/setup.py
index b8edf92..42b86b1 100644
--- a/rb_ws/src/buggy/setup.py
+++ b/rb_ws/src/buggy/setup.py
@@ -13,7 +13,7 @@
         ('share/ament_index/resource_index/packages',
             ['resource/' + package_name]),
         ('share/' + package_name, ['package.xml']),
-        (os.path.join("share", package_name), glob("launch/*.xml")),
+        (os.path.join("share", package_name), glob("launch/*.xml"))
     ],
     install_requires=['setuptools'],
     zip_safe=True,
@@ -24,9 +24,8 @@
     tests_require=['pytest'],
     entry_points={
         'console_scripts': [
-            'hello_world = buggy.hello_world:main'
-            'sim-single = buggy.engine:main'
-
+            'hello_world = buggy.hello_world:main',
+            'sim_single = buggy.simulator.engine:main'
         ],
     },
 )

From b9c8231614f9f4153e7d00908260bf192c69b6c4 Mon Sep 17 00:00:00 2001
From: TiaSinghania <samanvita.singhania@gmail.com>
Date: Tue, 22 Oct 2024 21:17:26 -0400
Subject: [PATCH 4/9] publish/subscriber base code

---
 rb_ws/src/buggy/buggy/simulator/engine.py | 18 +++++-------------
 1 file changed, 5 insertions(+), 13 deletions(-)

diff --git a/rb_ws/src/buggy/buggy/simulator/engine.py b/rb_ws/src/buggy/buggy/simulator/engine.py
index 346ae3a..4873035 100644
--- a/rb_ws/src/buggy/buggy/simulator/engine.py
+++ b/rb_ws/src/buggy/buggy/simulator/engine.py
@@ -4,20 +4,17 @@
 import rclpy
 from rclpy.node import Node
 
-from std_msgs.msg import String, Float64
+from std_msgs.msg import Float64
 
 class Simulator(Node):
     # simulator constants:
 
     def __init__(self):
-        super().__init__('SC_sim_single')
-        self.get_logger().info("INITIALIZING")
+        super().__init__('sim_single')
         self.number_publisher = self.create_publisher(Float64, 'numbers', 1)
-        self.test_publisher = self.create_publisher(String, 'test', 1)
         self.i = 0
 
         self.buggy_name = "NONE"
-
         if (self.get_namespace() == "/SC"):
             self.buggy_name = "SC"
 
@@ -29,12 +26,6 @@ def __init__(self):
         self.timer = self.create_timer(timer_period, self.loop)
 
     def loop(self):
-
-        self.get_logger().info("LOOPING")
-
-        msg = String()
-        msg.data = self.buggy_name
-        self.test_publisher.publish(msg)
         msg2 = Float64()
         msg2.data = float(self.i)
         self.number_publisher.publish(msg2)
@@ -43,7 +34,8 @@ def loop(self):
 def main(args=None):
     rclpy.init(args=args)
     sim = Simulator()
-    sim.get_logger().info("CREATED NODE")
     rclpy.spin(sim)
+    rclpy.shutdown()
 
-
+if __name__ == "__main__":
+    main()
\ No newline at end of file

From a8e41519585a678c2a79d5c14b2bf0f2313e7e7f Mon Sep 17 00:00:00 2001
From: TiaSinghania <samanvita.singhania@gmail.com>
Date: Wed, 23 Oct 2024 16:47:30 -0400
Subject: [PATCH 5/9] naive translation (ros1 -> ros2)

---
 rb_ws/src/buggy/buggy/simulator/engine.py | 180 +++++++++++++++++++++-
 1 file changed, 173 insertions(+), 7 deletions(-)

diff --git a/rb_ws/src/buggy/buggy/simulator/engine.py b/rb_ws/src/buggy/buggy/simulator/engine.py
index 4873035..c62e39d 100644
--- a/rb_ws/src/buggy/buggy/simulator/engine.py
+++ b/rb_ws/src/buggy/buggy/simulator/engine.py
@@ -1,35 +1,201 @@
 #! /usr/bin/env python3
+import re
 import sys
-import time
+import threading
 import rclpy
 from rclpy.node import Node
-
+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 numpy as np
+import utm
 
 class Simulator(Node):
     # simulator constants:
 
     def __init__(self):
         super().__init__('sim_single')
+        UTM_EAST_ZERO = 589702.87
+        UTM_NORTH_ZERO = 4477172.947
+        UTM_ZONE_NUM = 17
+        UTM_ZONE_LETTER = "T"
+        #TODO: make these values accurate
+        WHEELBASE_SC = 1.17
+        WHEELBASE_NAND= 1.17
+
+        self.starting_poses = {
+            "Hill1_NAND": (Simulator.UTM_EAST_ZERO + 0, Simulator.UTM_NORTH_ZERO + 0, -110),
+            "Hill1_SC": (Simulator.UTM_EAST_ZERO + 20, Simulator.UTM_NORTH_ZERO + 30, -110),
+            "WESTINGHOUSE": (589647, 4477143, -150),
+            "UC_TO_PURNELL": (589635, 4477468, 160),
+            "UC": (589681, 4477457, 160),
+            "TRACK_EAST_END": (589953, 4477465, 90),
+            "TRACK_RESNICK": (589906, 4477437, -20),
+            "GARAGE": (589846, 4477580, 180),
+            "PASS_PT": (589491, 4477003, -160),
+            "FREW_ST": (589646, 4477359, -20),
+            "FREW_ST_PASS": (589644, 4477368, -20),
+            "RACELINE_PASS": (589468.02, 4476993.07, -160),
+        }
+
         self.number_publisher = self.create_publisher(Float64, 'numbers', 1)
         self.i = 0
 
-        self.buggy_name = "NONE"
+        # for X11 matplotlib (direction included)
+        self.plot_publisher = self.create_publisher(Pose, "sim_2d/utm", 1)
+
+        # simulate the INS's outputs (noise included)
+        self.pose_publisher = self.create_publisher(Odometry, "nav/odom", 1)
+
+        self.steering_subscriber = self.create_subscription(
+            Float64, "buggy/input/steering", self.update_steering_angle, 1
+        )
+        # To read from velocity
+        self.velocity_subscriber = self.create_subscription(
+            Float64, "velocity", self.update_velocity, 1
+        )
+        self.navsatfix_noisy_publisher = self.create_publisher(
+                NavSatFix, "state/pose_navsat_noisy", 1
+        )
         if (self.get_namespace() == "/SC"):
             self.buggy_name = "SC"
+            init_pose = self.starting_poses["Hill_1SC"]
+
 
         if (self.get_namespace() == "/NAND"):
             self.buggy_name = "NAND"
+            init_pose = self.starting_poses["Hill_1NAND"]
+
+        self.e_utm, self.n_utm, self.heading = init_pose
+        # TODO: do we want to not hard code this
+        self.velocity = 12 # m/s
+        self.steering_angle = 0  # degrees
+        self.rate = 200  # Hz
+        self.pub_skip = 2  # publish every pub_skip ticks
+
+        self.lock = threading.Lock()
 
         freq = 10
         timer_period = 1/freq  # seconds
         self.timer = self.create_timer(timer_period, self.loop)
 
+    def update_steering_angle(self, data: Float64):
+        with self.lock:
+            self.steering_angle = data.data
+
+    def update_velocity(self, data: Float64):
+        with self.lock:
+            self.velocity = data.data
+
+    def get_steering_arc(self):
+        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):
+        l = Simulator.WHEELBASE
+        _, _, theta, delta = state
+
+        return np.array([v * np.cos(theta),
+                         v * np.sin(theta),
+                         v / l * np.tan(delta),
+                         0])
+
+    def step(self):
+        with self.lock:
+            heading = self.heading
+            e_utm = self.e_utm
+            n_utm = self.n_utm
+            velocity = self.velocity
+            steering_angle = self.steering_angle
+
+        h = 1/self.rate
+        state = np.array([e_utm, n_utm, np.deg2rad(heading), np.deg2rad(steering_angle)])
+        k1 = self.dynamics(state, velocity)
+        k2 = self.dynamics(state + h/2 * k1, velocity)
+        k3 = self.dynamics(state + h/2 * k2, velocity)
+        k4 = self.dynamics(state + h/2 * k3, velocity)
+
+        final_state = state + h/6 * (k1 + 2 * k2 + 2 * k3 + k4)
+
+        e_utm_new, n_utm_new, heading_new, _ = final_state
+        heading_new = np.rad2deg(heading_new)
+
+        with self.lock:
+            self.e_utm = e_utm_new
+            self.n_utm = n_utm_new
+            self.heading = heading_new
+
+    def publish(self):
+        p = Pose()
+        time_stamp = self.get_clock().now().to_msg()
+        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)
+
+        (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
+        nsf.header.stamp = time_stamp
+        self.navsatfix_publisher.publish(nsf)
+
+        if Simulator.NOISE:
+            lat_noisy = lat + np.random.normal(0, 1e-6)
+            long_noisy = long + np.random.normal(0, 1e-6)
+            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)
+
+        odom = Odometry()
+        odom.header.stamp = time_stamp
+
+        odom_pose = Pose()
+        odom_pose.position.x = long
+        odom_pose.position.y = lat
+        odom_pose.position.z = 260
+
+        odom_pose.orientation.z = np.sin(np.deg2rad(self.heading) / 2)
+        odom_pose.orientation.w = np.cos(np.deg2rad(self.heading) / 2)
+
+        odom_twist = Twist()
+        odom_twist.linear.x = velocity
+
+        odom.pose = PoseWithCovariance(pose=odom_pose)
+        odom.twist = TwistWithCovariance(twist=odom_twist)
+
+        self.pose_publisher.publish(odom)
+
     def loop(self):
-        msg2 = Float64()
-        msg2.data = float(self.i)
-        self.number_publisher.publish(msg2)
-        self.i += 1
+        rate = self.create_rate(self.rate)
+        pub_tick_count = 0
+
+        self.step()
+
+        if pub_tick_count == self.pub_skip:
+            self.publish()
+            pub_tick_count = 0
+        else:
+            pub_tick_count += 1
+
+
 
 def main(args=None):
     rclpy.init(args=args)

From 97191d00cbfe4700055e43b36d592388b0b11964 Mon Sep 17 00:00:00 2001
From: TiaSinghania <samanvita.singhania@gmail.com>
Date: Tue, 29 Oct 2024 19:21:15 -0400
Subject: [PATCH 6/9] the dots move now

---
 docker_tester                               | 13 ++++
 rb_ws/src/buggy/buggy/simulator/engine.py   | 67 +++++++++++----------
 rb_ws/src/buggy/scripts/simulator/engine.py | 32 ++++++++++
 3 files changed, 81 insertions(+), 31 deletions(-)
 create mode 100644 docker_tester
 create mode 100644 rb_ws/src/buggy/scripts/simulator/engine.py

diff --git a/docker_tester b/docker_tester
new file mode 100644
index 0000000..a7b9fe2
--- /dev/null
+++ b/docker_tester
@@ -0,0 +1,13 @@
+matplotlib==3.1.2
+NavPy==1.0
+numba==0.58.0
+numpy<1.21.0
+osqp==0.6.3
+pandas==2.0.3
+pyembree==0.2.11
+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/buggy/simulator/engine.py b/rb_ws/src/buggy/buggy/simulator/engine.py
index c62e39d..55599f0 100644
--- a/rb_ws/src/buggy/buggy/simulator/engine.py
+++ b/rb_ws/src/buggy/buggy/simulator/engine.py
@@ -13,16 +13,17 @@
 
 class Simulator(Node):
     # simulator constants:
+    UTM_EAST_ZERO = 589702.87
+    UTM_NORTH_ZERO = 4477172.947
+    UTM_ZONE_NUM = 17
+    UTM_ZONE_LETTER = "T"
+    #TODO: make these values accurate
+    WHEELBASE_SC = 1.17
+    WHEELBASE_NAND= 1.17
 
     def __init__(self):
         super().__init__('sim_single')
-        UTM_EAST_ZERO = 589702.87
-        UTM_NORTH_ZERO = 4477172.947
-        UTM_ZONE_NUM = 17
-        UTM_ZONE_LETTER = "T"
-        #TODO: make these values accurate
-        WHEELBASE_SC = 1.17
-        WHEELBASE_NAND= 1.17
+        self.get_logger().info('INITIALIZED.')
 
         self.starting_poses = {
             "Hill1_NAND": (Simulator.UTM_EAST_ZERO + 0, Simulator.UTM_NORTH_ZERO + 0, -110),
@@ -60,12 +61,14 @@ def __init__(self):
         )
         if (self.get_namespace() == "/SC"):
             self.buggy_name = "SC"
-            init_pose = self.starting_poses["Hill_1SC"]
-
+            init_pose = self.starting_poses["Hill1_SC"]
+            self.wheelbase = Simulator.WHEELBASE_SC
 
         if (self.get_namespace() == "/NAND"):
             self.buggy_name = "NAND"
-            init_pose = self.starting_poses["Hill_1NAND"]
+            init_pose = self.starting_poses["Hill1_NAND"]
+            self.wheelbase = Simulator.WHEELBASE_NAND
+
 
         self.e_utm, self.n_utm, self.heading = init_pose
         # TODO: do we want to not hard code this
@@ -73,6 +76,7 @@ def __init__(self):
         self.steering_angle = 0  # degrees
         self.rate = 200  # Hz
         self.pub_skip = 2  # publish every pub_skip ticks
+        self.pub_tick_count = 0
 
         self.lock = threading.Lock()
 
@@ -97,7 +101,7 @@ def get_steering_arc(self):
         return Simulator.WHEELBASE / np.tan(np.deg2rad(steering_angle))
 
     def dynamics(self, state, v):
-        l = Simulator.WHEELBASE
+        l = self.wheelbase
         _, _, theta, delta = state
 
         return np.array([v * np.cos(theta),
@@ -151,32 +155,30 @@ def publish(self):
         nsf = NavSatFix()
         nsf.latitude = lat
         nsf.longitude = long
-        nsf.altitude = 260
+        nsf.altitude = float(260)
         nsf.header.stamp = time_stamp
-        self.navsatfix_publisher.publish(nsf)
 
-        if Simulator.NOISE:
-            lat_noisy = lat + np.random.normal(0, 1e-6)
-            long_noisy = long + np.random.normal(0, 1e-6)
-            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)
+        lat_noisy = lat + np.random.normal(0, 1e-6)
+        long_noisy = long + np.random.normal(0, 1e-6)
+        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)
 
         odom = Odometry()
         odom.header.stamp = time_stamp
 
         odom_pose = Pose()
-        odom_pose.position.x = long
-        odom_pose.position.y = lat
-        odom_pose.position.z = 260
+        odom_pose.position.x = float(long)
+        odom_pose.position.y = float(lat)
+        odom_pose.position.z = float(260)
 
         odom_pose.orientation.z = np.sin(np.deg2rad(self.heading) / 2)
         odom_pose.orientation.w = np.cos(np.deg2rad(self.heading) / 2)
 
         odom_twist = Twist()
-        odom_twist.linear.x = velocity
+        odom_twist.linear.x = float(velocity)
 
         odom.pose = PoseWithCovariance(pose=odom_pose)
         odom.twist = TwistWithCovariance(twist=odom_twist)
@@ -184,16 +186,19 @@ def publish(self):
         self.pose_publisher.publish(odom)
 
     def loop(self):
-        rate = self.create_rate(self.rate)
-        pub_tick_count = 0
+        msg = Float64()
+        msg.data = float(self.i)
 
-        self.step()
+        self.number_publisher.publish(msg)
+        self.i += 1
 
-        if pub_tick_count == self.pub_skip:
+        self.step()
+        if self.pub_tick_count == self.pub_skip:
             self.publish()
-            pub_tick_count = 0
+            self.pub_tick_count = 0
         else:
-            pub_tick_count += 1
+            self.pub_tick_count += 1
+
 
 
 
diff --git a/rb_ws/src/buggy/scripts/simulator/engine.py b/rb_ws/src/buggy/scripts/simulator/engine.py
new file mode 100644
index 0000000..d0bab70
--- /dev/null
+++ b/rb_ws/src/buggy/scripts/simulator/engine.py
@@ -0,0 +1,32 @@
+#! /usr/bin/env python3
+import sys
+import time
+import rclpy
+from std_msgs.msg import String
+
+class Simulator():
+    # simulator constants:
+
+    def __init__(self):
+        self.test_publisher = self.create_publisher(String, 'test', 10)
+        if (self.get_namespace() == "SC"):
+            self.buggy_name = "SC"
+
+        if (self.get_namespace() == "NAND"):
+            self.buggy_name = "NAND"
+
+    def loop(self):
+        print("hello")
+        self.test_publisher.publish(self.buggy_name)
+
+if __name__ == "__main__":
+    rclpy.init()
+    sim = Simulator()
+
+    # publish initial position, then sleep
+    # so that auton stack has time to initialize
+    # before buggy moves
+    time.sleep(15.0)
+    sim.loop()
+
+

From a62da31d1097e1286f29f60c252290161f54bdc3 Mon Sep 17 00:00:00 2001
From: TiaSinghania <samanvita.singhania@gmail.com>
Date: Tue, 29 Oct 2024 19:59:01 -0400
Subject: [PATCH 7/9] added command line args

---
 rb_ws/src/buggy/buggy/simulator/engine.py | 18 +++++++++++++-----
 rb_ws/src/buggy/launch/sim_2d_single.xml  | 11 +++++++++--
 2 files changed, 22 insertions(+), 7 deletions(-)

diff --git a/rb_ws/src/buggy/buggy/simulator/engine.py b/rb_ws/src/buggy/buggy/simulator/engine.py
index 55599f0..67c2473 100644
--- a/rb_ws/src/buggy/buggy/simulator/engine.py
+++ b/rb_ws/src/buggy/buggy/simulator/engine.py
@@ -25,6 +25,7 @@ def __init__(self):
         super().__init__('sim_single')
         self.get_logger().info('INITIALIZED.')
 
+
         self.starting_poses = {
             "Hill1_NAND": (Simulator.UTM_EAST_ZERO + 0, Simulator.UTM_NORTH_ZERO + 0, -110),
             "Hill1_SC": (Simulator.UTM_EAST_ZERO + 20, Simulator.UTM_NORTH_ZERO + 30, -110),
@@ -59,20 +60,27 @@ def __init__(self):
         self.navsatfix_noisy_publisher = self.create_publisher(
                 NavSatFix, "state/pose_navsat_noisy", 1
         )
+
+
+
+
+
+        self.declare_parameter('velocity', 12)
         if (self.get_namespace() == "/SC"):
             self.buggy_name = "SC"
-            init_pose = self.starting_poses["Hill1_SC"]
+            self.declare_parameter('pose', "Hill1_SC")
             self.wheelbase = Simulator.WHEELBASE_SC
 
         if (self.get_namespace() == "/NAND"):
             self.buggy_name = "NAND"
-            init_pose = self.starting_poses["Hill1_NAND"]
+            self.declare_parameter('pose', "Hill1_NAND")
             self.wheelbase = Simulator.WHEELBASE_NAND
 
+        self.velocity = self.get_parameter("velocity").value
+        init_pose_name = self.get_parameter("pose").value
+        self.init_pose = self.starting_poses[init_pose_name]
 
-        self.e_utm, self.n_utm, self.heading = init_pose
-        # TODO: do we want to not hard code this
-        self.velocity = 12 # m/s
+        self.e_utm, self.n_utm, self.heading = self.init_pose
         self.steering_angle = 0  # degrees
         self.rate = 200  # Hz
         self.pub_skip = 2  # publish every pub_skip ticks
diff --git a/rb_ws/src/buggy/launch/sim_2d_single.xml b/rb_ws/src/buggy/launch/sim_2d_single.xml
index 550c259..ca458b8 100755
--- a/rb_ws/src/buggy/launch/sim_2d_single.xml
+++ b/rb_ws/src/buggy/launch/sim_2d_single.xml
@@ -1,8 +1,15 @@
 <launch>
+
     <node pkg="foxglove_bridge" exec="foxglove_bridge" name="foxglove" namespace="SC"/>
     <node pkg="buggy" exec="hello_world" name="hello_world" namespace="SC" />
-    <node pkg="buggy" exec="sim_single" name="SC_sim_single" namespace="SC"/>
-    <node pkg="buggy" exec="sim_single" name="NAND_sim_single" namespace="NAND"/>
+    <node pkg="buggy" exec="sim_single" name="SC_sim_single" namespace="SC">
+        <param name="velocity" value="12"/>
+        <param name="pose" value="Hill1_SC"/>
+    </node>
+    <node pkg="buggy" exec="sim_single" name="NAND_sim_single" namespace="NAND">
+        <param name="velocity" value="12"/>
+        <param name="pose" value="Hill1_NAND"/>
+    </node>
 
     <!-- <node name="foxglove" pkg="foxglove_bridge" type="foxglove_bridge" />
     <node name="hello_world" pkg="buggy" type="hello_world" /> -->

From c256d1218e3976dd24d0f6979ba0379a56b4490b Mon Sep 17 00:00:00 2001
From: TiaSinghania <samanvita.singhania@gmail.com>
Date: Wed, 30 Oct 2024 17:17:14 -0400
Subject: [PATCH 8/9] pylint

---
 rb_ws/src/buggy/buggy/simulator/engine.py | 2 --
 1 file changed, 2 deletions(-)

diff --git a/rb_ws/src/buggy/buggy/simulator/engine.py b/rb_ws/src/buggy/buggy/simulator/engine.py
index 67c2473..361ae4b 100644
--- a/rb_ws/src/buggy/buggy/simulator/engine.py
+++ b/rb_ws/src/buggy/buggy/simulator/engine.py
@@ -1,6 +1,4 @@
 #! /usr/bin/env python3
-import re
-import sys
 import threading
 import rclpy
 from rclpy.node import Node

From fd52cbd98bc8d5f3c73539bb8d56c7536da66700 Mon Sep 17 00:00:00 2001
From: TiaSinghania <samanvita.singhania@gmail.com>
Date: Wed, 30 Oct 2024 17:21:34 -0400
Subject: [PATCH 9/9] got rid of duplicate

---
 rb_ws/src/buggy/scripts/simulator/engine.py | 32 ---------------------
 1 file changed, 32 deletions(-)
 delete mode 100644 rb_ws/src/buggy/scripts/simulator/engine.py

diff --git a/rb_ws/src/buggy/scripts/simulator/engine.py b/rb_ws/src/buggy/scripts/simulator/engine.py
deleted file mode 100644
index d0bab70..0000000
--- a/rb_ws/src/buggy/scripts/simulator/engine.py
+++ /dev/null
@@ -1,32 +0,0 @@
-#! /usr/bin/env python3
-import sys
-import time
-import rclpy
-from std_msgs.msg import String
-
-class Simulator():
-    # simulator constants:
-
-    def __init__(self):
-        self.test_publisher = self.create_publisher(String, 'test', 10)
-        if (self.get_namespace() == "SC"):
-            self.buggy_name = "SC"
-
-        if (self.get_namespace() == "NAND"):
-            self.buggy_name = "NAND"
-
-    def loop(self):
-        print("hello")
-        self.test_publisher.publish(self.buggy_name)
-
-if __name__ == "__main__":
-    rclpy.init()
-    sim = Simulator()
-
-    # publish initial position, then sleep
-    # so that auton stack has time to initialize
-    # before buggy moves
-    time.sleep(15.0)
-    sim.loop()
-
-