From 44b81b7cac5f969fcfe04ef73c5f960b067795f4 Mon Sep 17 00:00:00 2001 From: JoyceZhu2486 Date: Thu, 21 Nov 2024 15:29:34 -0500 Subject: [PATCH] fixed bug in launch file and getting parameters in node, need futher test with X11 installed. --- .../src/buggy/buggy/simulator/velocity_ui.py | 46 ++++++++++--------- .../buggy/buggy/simulator/velocity_updater.py | 44 +++++++++--------- .../buggy/launch/sim_2d_single_velocity.xml | 8 ++-- rb_ws/src/buggy/setup.py | 4 +- 4 files changed, 53 insertions(+), 49 deletions(-) diff --git a/rb_ws/src/buggy/buggy/simulator/velocity_ui.py b/rb_ws/src/buggy/buggy/simulator/velocity_ui.py index 6fc3112..3615ec6 100644 --- a/rb_ws/src/buggy/buggy/simulator/velocity_ui.py +++ b/rb_ws/src/buggy/buggy/simulator/velocity_ui.py @@ -2,22 +2,35 @@ import sys import threading import tkinter as tk -from controller_2d import Controller +# from controller_2d import Controller import rclpy from rclpy.node import Node class VelocityUI(Node): - def __init__(self, init_vel: float, buggy_name: str): + def __init__(self): super().__init__('velocity_ui') + self.get_logger().info('INITIALIZED.') - # So the buggy doesn't start moving without user input - self.buggy_vel = 0 - self.controller = Controller(buggy_name) - self.lock = threading.Lock() + # Declare parameters with default values + self.declare_parameter('init_vel', 12) + self.declare_parameter('buggy_name', 'SC') + # Get the parameter values + self.init_vel = self.get_parameter("init_vel").value + self.buggy_name = self.get_parameter("buggy_name").value + # initialize variables + # TODO: uncomment after controller is implemented + # # So the buggy doesn't start moving without user input + # self.buggy_vel = 0 + # # self.controller = Controller(buggy_name) + # self.lock = threading.Lock() + + self.buggy_vel = self.init_vel + + # TODO: This line currently generate error self.root = tk.Tk() - self.root.title(buggy_name + ' Manual Velocity: scale = 0.1m/s') + self.root.title(self.buggy_name + ' Manual Velocity: scale = 0.1m/s') self.root.geometry('600x100') self.root.configure(background='#342d66') @@ -30,7 +43,7 @@ def __init__(self, init_vel: float, buggy_name: str): # ROS2 timer for stepping # 0.01 is equivalent to 100Hz (100 times per second) - self.create_timer(0.01, self.step) + # self.create_timer(0.01, self.step) def step(self): # Sets the velocity of the buggy to the current scale value @@ -40,22 +53,13 @@ def step(self): # Update velocity of the buggy # '/10' set velocity with 0.1 precision self.buggy_vel = self.scale.get() / 10 - self.controller.set_velocity(self.buggy_vel) + # TODO: uncomment after controller is implemented + # self.controller.set_velocity(self.buggy_vel) def main(args=None): rclpy.init(args=args) - - if len(sys.argv) < 3: - print("Usage: velocity_ui ") - sys.exit(1) - - init_vel = float(sys.argv[1]) - buggy_name = sys.argv[2] - - velocity_ui = VelocityUI(init_vel, buggy_name) - - rclpy.spin(velocity_ui) - velocity_ui.destroy_node() + vel_ui = VelocityUI() + rclpy.spin(vel_ui) rclpy.shutdown() if __name__ == "__main__": diff --git a/rb_ws/src/buggy/buggy/simulator/velocity_updater.py b/rb_ws/src/buggy/buggy/simulator/velocity_updater.py index ed63a02..5858115 100644 --- a/rb_ws/src/buggy/buggy/simulator/velocity_updater.py +++ b/rb_ws/src/buggy/buggy/simulator/velocity_updater.py @@ -4,7 +4,7 @@ import threading import rclpy from rclpy.node import Node -from controller_2d import Controller +# from controller_2d import Controller from geometry_msgs.msg import Pose from geometry_msgs.msg import Point @@ -15,23 +15,32 @@ 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, 5) + (589701, 4477160, 20, 0.5) ] - def __init__(self, init_vel: float, buggy_name: str): - super().__init__('vel_updater') + def __init__(self): + super().__init__('velocity_updater') + self.get_logger().info('INITIALIZED.') - self.buggy_vel = init_vel + # Declare parameters with default values + self.declare_parameter('init_vel', 12) + self.declare_parameter('buggy_name', 'SC') + # Get the parameter values + self.init_vel = self.get_parameter("init_vel").value + self.buggy_name = self.get_parameter("buggy_name").value + + # initialize variables + self.buggy_vel = self.init_vel self.accel = 0.0 self.position = Point() - self.controller = Controller(buggy_name) - - self.lock = threading.Lock() + # TODO: uncomment after controller is implemented + # self.controller = Controller(self.buggy_name) + # self.lock = threading.Lock() # ROS2 subscription self.pose_subscriber = self.create_subscription( Pose, - f"{buggy_name}/sim_2d/utm", + f"{self.buggy_name}/sim_2d/utm", self.update_position, 10 # QoS profile ) @@ -64,22 +73,13 @@ def step(self): self.calculate_accel() new_velocity = self.buggy_vel + self.accel / self.RATE self.buggy_vel = new_velocity - self.controller.set_velocity(new_velocity) + # TODO: uncomment after controller is implemented + # self.controller.set_velocity(new_velocity) def main(args=None): rclpy.init(args=args) - - if len(sys.argv) < 3: - print("Usage: vel_updater ") - sys.exit(1) - - init_vel = float(sys.argv[1]) - buggy_name = sys.argv[2] - - velocity_updater = VelocityUpdater(init_vel, buggy_name) - - rclpy.spin(velocity_updater) - velocity_updater.destroy_node() + vel_updater = VelocityUpdater() + rclpy.spin(vel_updater) rclpy.shutdown() if __name__ == "__main__": 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 c64491b..d329832 100644 --- a/rb_ws/src/buggy/launch/sim_2d_single_velocity.xml +++ b/rb_ws/src/buggy/launch/sim_2d_single_velocity.xml @@ -12,12 +12,12 @@ - - + + - - + + diff --git a/rb_ws/src/buggy/setup.py b/rb_ws/src/buggy/setup.py index 444cf4c..05d2dcc 100644 --- a/rb_ws/src/buggy/setup.py +++ b/rb_ws/src/buggy/setup.py @@ -26,8 +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', + 'velocity_ui = buggy.simulator.velocity_ui:main', + 'velocity_updater = buggy.simulator.velocity_updater:main', 'watchdog = buggy.watchdog.watchdog_node:main' ], },