velocity_updater done debugging

rb_ws/src/buggy/buggy/simulator/velocity_ui.py

@@ -5,6 +5,7 @@
 # from controller_2d import Controller
 import rclpy
 from rclpy.node import Node
+from std_msgs.msg import Float64
 
 class VelocityUI(Node):
     def __init__(self):
@@ -27,7 +28,12 @@ def __init__(self):
 
         self.buggy_vel = self.init_vel
 
-        # TODO: This line currently generate error
+        # TODO: remove after controller is implemented
+        # publish velocity 
+        self.velocity_publisher = self.create_publisher(Float64, "velocity", 1)
+
+
+        # TODO: tk is not displaying rn
         self.root = tk.Tk()
 
         self.root.title(self.buggy_name + ' Manual Velocity: scale = 0.1m/s')
@@ -53,9 +59,15 @@ def step(self):
         # Update velocity of the buggy
         # '/10' set velocity with 0.1 precision
         self.buggy_vel = self.scale.get() / 10
+
         # TODO: uncomment after controller is implemented
         # self.controller.set_velocity(self.buggy_vel)
 
+        # TODO: remove after controller is implemented
+        float_64_velocity = Float64()
+        float_64_velocity.data = float(self.buggy_vel)
+        self.velocity_publisher .publish(float_64_velocity)
+
 def main(args=None):
     rclpy.init(args=args)
     vel_ui = VelocityUI()

rb_ws/src/buggy/buggy/simulator/velocity_updater.py

@@ -7,6 +7,7 @@
 # from controller_2d import Controller
 from geometry_msgs.msg import Pose
 from geometry_msgs.msg import Point
+from std_msgs.msg import Float64
 
 class VelocityUpdater(Node):
     RATE = 100
@@ -15,7 +16,8 @@ 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, 0.5)
+        (589701, 4477160, 10000000000, 100) # for testing
     ]
 
     def __init__(self):
@@ -37,14 +39,18 @@ def __init__(self):
         # self.controller = Controller(self.buggy_name)
         # self.lock = threading.Lock()
 
-        # ROS2 subscription
+        # subscribe pose
         self.pose_subscriber = self.create_subscription(
             Pose,
             f"{self.buggy_name}/sim_2d/utm",
             self.update_position,
             10  # QoS profile
         )
 
+        # TODO: remove after controller is implemented
+        # publish velocity 
+        self.velocity_publisher = self.create_publisher(Float64, "velocity", 1)
+
         # ROS2 timer for stepping
         self.timer = self.create_timer(1.0 / self.RATE, self.step)
 
@@ -73,9 +79,15 @@ def step(self):
         self.calculate_accel()
         new_velocity = self.buggy_vel + self.accel / self.RATE
         self.buggy_vel = new_velocity
+
         # TODO: uncomment after controller is implemented
         # self.controller.set_velocity(new_velocity)
 
+        # TODO: remove after controller is implemented
+        float_64_velocity = Float64()
+        float_64_velocity.data = float(new_velocity)
+        self.velocity_publisher.publish(float_64_velocity)
+
 def main(args=None):
     rclpy.init(args=args)
     vel_updater = VelocityUpdater()

rb_ws/src/buggy/launch/sim_2d_single_velocity.xml

@@ -11,11 +11,11 @@
         <param name="pose" value="Hill1_NAND"/>
     </node>
 
-    <!-- velocity ui and updater nodes -->
-    <node pkg="buggy" exec="velocity_ui" name="SC_velocity_ui" namespace="SC">
+    <!-- velocity ui or updater nodes -->
+    <!-- <node pkg="buggy" exec="velocity_ui" name="SC_velocity_ui" namespace="SC">
         <param name="init_vel" value="12"/>
         <param name="buggy_name" value="SC"/>
-    </node>
+    </node> -->
     <node pkg="buggy" exec="velocity_updater" name="SC_velocity_updater" namespace="SC">
         <param name="init_vel" value="12"/>
         <param name="buggy_name" value="SC"/>

rb_ws/src/buggy/setup.py

@@ -26,7 +26,7 @@
         'console_scripts': [
             'hello_world = buggy.hello_world:main',
             'sim_single = buggy.simulator.engine:main',
-            'velocity_ui = buggy.simulator.velocity_ui:main',
+            # 'velocity_ui = buggy.simulator.velocity_ui:main',
             'velocity_updater = buggy.simulator.velocity_updater:main',
             'watchdog = buggy.watchdog.watchdog_node:main'
         ],