From b0f8c26efbc6ab2f234a47689ed0f018a3c4fb04 Mon Sep 17 00:00:00 2001
From: JoyceZhu2486 <junyuzhu@andrew.cmu.edu>
Date: Wed, 20 Nov 2024 18:02:38 -0500
Subject: [PATCH] added velocity launch file and edited setup.py, launch file
 need to be debug

---
 .../src/buggy/buggy/simulator/velocity_ui.py  | 12 +++------
 .../buggy/buggy/simulator/velocity_updater.py | 14 ++++------
 .../buggy/launch/sim_2d_single_velocity.xml   | 26 +++++++++++++++++++
 rb_ws/src/buggy/setup.py                      |  2 ++
 4 files changed, 37 insertions(+), 17 deletions(-)
 create mode 100644 rb_ws/src/buggy/launch/sim_2d_single_velocity.xml

diff --git a/rb_ws/src/buggy/buggy/simulator/velocity_ui.py b/rb_ws/src/buggy/buggy/simulator/velocity_ui.py
index 4bc845e..6fc3112 100644
--- a/rb_ws/src/buggy/buggy/simulator/velocity_ui.py
+++ b/rb_ws/src/buggy/buggy/simulator/velocity_ui.py
@@ -52,15 +52,11 @@ def main(args=None):
     init_vel = float(sys.argv[1])
     buggy_name = sys.argv[2]
 
-    vel_ui = VelocityUI(init_vel, buggy_name)
+    velocity_ui = VelocityUI(init_vel, buggy_name)
 
-    try:
-        rclpy.spin(vel_ui)
-    except KeyboardInterrupt:
-        print("Shutting down velocity UI")
-    finally:
-        vel_ui.destroy_node()
-        rclpy.shutdown()
+    rclpy.spin(velocity_ui)
+    velocity_ui.destroy_node()
+    rclpy.shutdown()
 
 if __name__ == "__main__":
     main()
\ No newline at end of file
diff --git a/rb_ws/src/buggy/buggy/simulator/velocity_updater.py b/rb_ws/src/buggy/buggy/simulator/velocity_updater.py
index 6cc90f3..ed63a02 100644
--- a/rb_ws/src/buggy/buggy/simulator/velocity_updater.py
+++ b/rb_ws/src/buggy/buggy/simulator/velocity_updater.py
@@ -15,7 +15,7 @@ class VelocityUpdater(Node):
     # 'list[tuple[float,float,float,float]]'
     # need further update such as more data or import data from certain files
     CHECKPOINTS = [
-        (589701, 4477160, 20, 0.5)
+        (589701, 4477160, 20, 5)
     ]
 
     def __init__(self, init_vel: float, buggy_name: str):
@@ -76,15 +76,11 @@ def main(args=None):
     init_vel = float(sys.argv[1])
     buggy_name = sys.argv[2]
 
-    vel_updater = VelocityUpdater(init_vel, buggy_name)
+    velocity_updater = VelocityUpdater(init_vel, buggy_name)
 
-    try:
-        rclpy.spin(vel_updater)
-    except KeyboardInterrupt:
-        print("Shutting down velocity updater")
-    finally:
-        vel_updater.destroy_node()
-        rclpy.shutdown()
+    rclpy.spin(velocity_updater)
+    velocity_updater.destroy_node()
+    rclpy.shutdown()
 
 if __name__ == "__main__":
     main()
\ No newline at end of file
diff --git a/rb_ws/src/buggy/launch/sim_2d_single_velocity.xml b/rb_ws/src/buggy/launch/sim_2d_single_velocity.xml
new file mode 100644
index 0000000..c64491b
--- /dev/null
+++ b/rb_ws/src/buggy/launch/sim_2d_single_velocity.xml
@@ -0,0 +1,26 @@
+<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">
+        <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>
+
+    <!-- velocity ui and updater nodes -->
+    <node pkg="buggy" exec="velocity_ui" name="velocity_ui" namespace="SC">
+        <param name="initial_vel" value="12"/>
+        <param name="buggy_name" value="SC"/>
+    </node>
+    <node pkg="buggy" exec="VelocityUpdater" name="velocity_updater" namespace="SC">
+        <param name="initial_vel" value="12"/>
+        <param name="buggy_name" value="SC"/>
+    </node>
+
+    <!-- <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/setup.py b/rb_ws/src/buggy/setup.py
index 42318d0..444cf4c 100644
--- a/rb_ws/src/buggy/setup.py
+++ b/rb_ws/src/buggy/setup.py
@@ -26,6 +26,8 @@
         'console_scripts': [
             'hello_world = buggy.hello_world:main',
             'sim_single = buggy.simulator.engine:main',
+            'sim_velocity_ui = buggy.similator.velocity_ui:main',
+            'sim_velocity_updater = buggy.similator.velocity_updater:main',
             'watchdog = buggy.watchdog.watchdog_node:main'
         ],
     },