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' ],