diff --git a/rb_ws/src/buggy/buggy/simulator/velocity_ui.py b/rb_ws/src/buggy/buggy/simulator/velocity_ui.py
index 3615ec6..280144e 100644
--- a/rb_ws/src/buggy/buggy/simulator/velocity_ui.py
+++ b/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()
diff --git a/rb_ws/src/buggy/buggy/simulator/velocity_updater.py b/rb_ws/src/buggy/buggy/simulator/velocity_updater.py
index 5858115..983158e 100644
--- a/rb_ws/src/buggy/buggy/simulator/velocity_updater.py
+++ b/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,7 +39,7 @@ 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",
@@ -45,6 +47,10 @@ def __init__(self):
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()
diff --git a/rb_ws/src/buggy/launch/sim_2d_single_velocity.xml b/rb_ws/src/buggy/launch/sim_2d_single_velocity.xml
index d329832..b24a3b3 100644
--- a/rb_ws/src/buggy/launch/sim_2d_single_velocity.xml
+++ b/rb_ws/src/buggy/launch/sim_2d_single_velocity.xml
@@ -11,11 +11,11 @@
-
-
+
+
diff --git a/rb_ws/src/buggy/setup.py b/rb_ws/src/buggy/setup.py
index 05d2dcc..435bf8b 100644
--- a/rb_ws/src/buggy/setup.py
+++ b/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'
],