From fc57679db817be6e94c4a13c7601c36d923c9195 Mon Sep 17 00:00:00 2001 From: JoyceZhu2486 Date: Tue, 14 Jan 2025 17:50:43 -0500 Subject: [PATCH 01/11] move files from simulation-velocity-port --- .../buggy/scripts/simulator/velocity_ui.py | 78 +++++++++++++++ .../scripts/simulator/velocity_updater.py | 98 +++++++++++++++++++ 2 files changed, 176 insertions(+) create mode 100644 rb_ws/src/buggy/scripts/simulator/velocity_ui.py create mode 100644 rb_ws/src/buggy/scripts/simulator/velocity_updater.py diff --git a/rb_ws/src/buggy/scripts/simulator/velocity_ui.py b/rb_ws/src/buggy/scripts/simulator/velocity_ui.py new file mode 100644 index 00000000..280144e5 --- /dev/null +++ b/rb_ws/src/buggy/scripts/simulator/velocity_ui.py @@ -0,0 +1,78 @@ +#! /usr/bin/env python3 +import sys +import threading +import tkinter as tk +# from controller_2d import Controller +import rclpy +from rclpy.node import Node +from std_msgs.msg import Float64 + +class VelocityUI(Node): + def __init__(self): + super().__init__('velocity_ui') + self.get_logger().info('INITIALIZED.') + + # 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: 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') + self.root.geometry('600x100') + self.root.configure(background='#342d66') + + self.scale = tk.Scale(self.root, from_=0, to=300, orient=tk.HORIZONTAL, + length=500, width=30) + self.scale.pack() + + self.root.bind("", lambda i: self.scale.set(self.scale.get() + 2)) + self.root.bind("", lambda d: self.scale.set(self.scale.get() - 2)) + + # ROS2 timer for stepping + # 0.01 is equivalent to 100Hz (100 times per second) + # self.create_timer(0.01, self.step) + + def step(self): + # Sets the velocity of the buggy to the current scale value + # called every tick + self.root.update_idletasks() + self.root.update() + # 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() + rclpy.spin(vel_ui) + rclpy.shutdown() + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/rb_ws/src/buggy/scripts/simulator/velocity_updater.py b/rb_ws/src/buggy/scripts/simulator/velocity_updater.py new file mode 100644 index 00000000..983158e8 --- /dev/null +++ b/rb_ws/src/buggy/scripts/simulator/velocity_updater.py @@ -0,0 +1,98 @@ +#! /usr/bin/env python3 +import sys +import math +import threading +import rclpy +from rclpy.node import Node +# 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 + # Bubbles for updating acceleration based on position + # represented as 4-tuples: (x-pos, y-pos, radius, acceleration) + # '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, 10000000000, 100) # for testing + ] + + def __init__(self): + super().__init__('velocity_updater') + self.get_logger().info('INITIALIZED.') + + # 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() + # TODO: uncomment after controller is implemented + # self.controller = Controller(self.buggy_name) + # self.lock = threading.Lock() + + # 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) + + def update_position(self, new_pose: Pose): + '''Callback function to update internal position variable when new + buggy position is published + + Args: + new_pose (Pose): Pose object from topic + ''' + with self.lock: + self.position = new_pose.position + + def calculate_accel(self): + '''Check if the position of the buggy is in any of the checkpoints set + in self.CHECKPOINTS, and update acceleration of buggy accordingly + ''' + for (x, y, r, a) in self.CHECKPOINTS: + dist = math.sqrt((x-self.position.x)**2 + (y-self.position.y)**2) + if dist < r: + self.accel = a + break + + def step(self): + '''Update acceleration and velocity of buggy for one timestep''' + 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() + rclpy.spin(vel_updater) + rclpy.shutdown() + +if __name__ == "__main__": + main() \ No newline at end of file From 441020c7fd6268bb4c0edcd468d20637d404ff47 Mon Sep 17 00:00:00 2001 From: JoyceZhu2486 Date: Tue, 14 Jan 2025 17:50:43 -0500 Subject: [PATCH 02/11] move files from simulation-velocity-port --- .../buggy/scripts/simulator/velocity_ui.py | 78 +++++++++++++++ .../scripts/simulator/velocity_updater.py | 98 +++++++++++++++++++ 2 files changed, 176 insertions(+) create mode 100644 rb_ws/src/buggy/scripts/simulator/velocity_ui.py create mode 100644 rb_ws/src/buggy/scripts/simulator/velocity_updater.py diff --git a/rb_ws/src/buggy/scripts/simulator/velocity_ui.py b/rb_ws/src/buggy/scripts/simulator/velocity_ui.py new file mode 100644 index 00000000..280144e5 --- /dev/null +++ b/rb_ws/src/buggy/scripts/simulator/velocity_ui.py @@ -0,0 +1,78 @@ +#! /usr/bin/env python3 +import sys +import threading +import tkinter as tk +# from controller_2d import Controller +import rclpy +from rclpy.node import Node +from std_msgs.msg import Float64 + +class VelocityUI(Node): + def __init__(self): + super().__init__('velocity_ui') + self.get_logger().info('INITIALIZED.') + + # 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: 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') + self.root.geometry('600x100') + self.root.configure(background='#342d66') + + self.scale = tk.Scale(self.root, from_=0, to=300, orient=tk.HORIZONTAL, + length=500, width=30) + self.scale.pack() + + self.root.bind("", lambda i: self.scale.set(self.scale.get() + 2)) + self.root.bind("", lambda d: self.scale.set(self.scale.get() - 2)) + + # ROS2 timer for stepping + # 0.01 is equivalent to 100Hz (100 times per second) + # self.create_timer(0.01, self.step) + + def step(self): + # Sets the velocity of the buggy to the current scale value + # called every tick + self.root.update_idletasks() + self.root.update() + # 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() + rclpy.spin(vel_ui) + rclpy.shutdown() + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/rb_ws/src/buggy/scripts/simulator/velocity_updater.py b/rb_ws/src/buggy/scripts/simulator/velocity_updater.py new file mode 100644 index 00000000..983158e8 --- /dev/null +++ b/rb_ws/src/buggy/scripts/simulator/velocity_updater.py @@ -0,0 +1,98 @@ +#! /usr/bin/env python3 +import sys +import math +import threading +import rclpy +from rclpy.node import Node +# 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 + # Bubbles for updating acceleration based on position + # represented as 4-tuples: (x-pos, y-pos, radius, acceleration) + # '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, 10000000000, 100) # for testing + ] + + def __init__(self): + super().__init__('velocity_updater') + self.get_logger().info('INITIALIZED.') + + # 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() + # TODO: uncomment after controller is implemented + # self.controller = Controller(self.buggy_name) + # self.lock = threading.Lock() + + # 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) + + def update_position(self, new_pose: Pose): + '''Callback function to update internal position variable when new + buggy position is published + + Args: + new_pose (Pose): Pose object from topic + ''' + with self.lock: + self.position = new_pose.position + + def calculate_accel(self): + '''Check if the position of the buggy is in any of the checkpoints set + in self.CHECKPOINTS, and update acceleration of buggy accordingly + ''' + for (x, y, r, a) in self.CHECKPOINTS: + dist = math.sqrt((x-self.position.x)**2 + (y-self.position.y)**2) + if dist < r: + self.accel = a + break + + def step(self): + '''Update acceleration and velocity of buggy for one timestep''' + 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() + rclpy.spin(vel_updater) + rclpy.shutdown() + +if __name__ == "__main__": + main() \ No newline at end of file From 911509dc8b6a788d6ae090c45d11f54e9173408e Mon Sep 17 00:00:00 2001 From: JoyceZhu2486 Date: Tue, 21 Jan 2025 17:40:04 -0500 Subject: [PATCH 03/11] copy paste files from simulation-velocity-port --- rb_ws/src/buggy/CMakeLists.txt | 1 + .../buggy/launch/sim_2d_single_velocity.xml | 18 +++++ .../buggy/scripts/simulator/checkpoints.json | 17 ++++ .../buggy/scripts/simulator/velocity_ui.py | 78 ------------------- .../scripts/simulator/velocity_updater.py | 59 ++++++++++---- 5 files changed, 80 insertions(+), 93 deletions(-) create mode 100644 rb_ws/src/buggy/launch/sim_2d_single_velocity.xml create mode 100644 rb_ws/src/buggy/scripts/simulator/checkpoints.json delete mode 100644 rb_ws/src/buggy/scripts/simulator/velocity_ui.py diff --git a/rb_ws/src/buggy/CMakeLists.txt b/rb_ws/src/buggy/CMakeLists.txt index a8a51421..97cccb36 100644 --- a/rb_ws/src/buggy/CMakeLists.txt +++ b/rb_ws/src/buggy/CMakeLists.txt @@ -31,6 +31,7 @@ install(PROGRAMS scripts/controller/controller_node.py scripts/path_planner/path_planner.py scripts/simulator/engine.py + scripts/simulator/velocity_updater.py scripts/watchdog/watchdog_node.py scripts/buggy_state_converter.py scripts/serial/ros_to_bnyahaj.py 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 00000000..edbdf92f --- /dev/null +++ b/rb_ws/src/buggy/launch/sim_2d_single_velocity.xml @@ -0,0 +1,18 @@ + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/rb_ws/src/buggy/scripts/simulator/checkpoints.json b/rb_ws/src/buggy/scripts/simulator/checkpoints.json new file mode 100644 index 00000000..83dd0500 --- /dev/null +++ b/rb_ws/src/buggy/scripts/simulator/checkpoints.json @@ -0,0 +1,17 @@ +{ + "checkpoints": + [ + { + "x-pos": 589701, + "y-pos": 4477160, + "radius": 10, + "velocity": 10 + }, + { + "x-pos": 589701, + "y-pos": 4476860, + "radius": 10, + "velocity": 50 + } + ] +} \ No newline at end of file diff --git a/rb_ws/src/buggy/scripts/simulator/velocity_ui.py b/rb_ws/src/buggy/scripts/simulator/velocity_ui.py deleted file mode 100644 index 280144e5..00000000 --- a/rb_ws/src/buggy/scripts/simulator/velocity_ui.py +++ /dev/null @@ -1,78 +0,0 @@ -#! /usr/bin/env python3 -import sys -import threading -import tkinter as tk -# from controller_2d import Controller -import rclpy -from rclpy.node import Node -from std_msgs.msg import Float64 - -class VelocityUI(Node): - def __init__(self): - super().__init__('velocity_ui') - self.get_logger().info('INITIALIZED.') - - # 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: 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') - self.root.geometry('600x100') - self.root.configure(background='#342d66') - - self.scale = tk.Scale(self.root, from_=0, to=300, orient=tk.HORIZONTAL, - length=500, width=30) - self.scale.pack() - - self.root.bind("", lambda i: self.scale.set(self.scale.get() + 2)) - self.root.bind("", lambda d: self.scale.set(self.scale.get() - 2)) - - # ROS2 timer for stepping - # 0.01 is equivalent to 100Hz (100 times per second) - # self.create_timer(0.01, self.step) - - def step(self): - # Sets the velocity of the buggy to the current scale value - # called every tick - self.root.update_idletasks() - self.root.update() - # 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() - rclpy.spin(vel_ui) - rclpy.shutdown() - -if __name__ == "__main__": - main() \ No newline at end of file diff --git a/rb_ws/src/buggy/scripts/simulator/velocity_updater.py b/rb_ws/src/buggy/scripts/simulator/velocity_updater.py index 983158e8..7d914de3 100644 --- a/rb_ws/src/buggy/scripts/simulator/velocity_updater.py +++ b/rb_ws/src/buggy/scripts/simulator/velocity_updater.py @@ -1,7 +1,7 @@ #! /usr/bin/env python3 -import sys import math import threading +import json import rclpy from rclpy.node import Node # from controller_2d import Controller @@ -15,10 +15,15 @@ class VelocityUpdater(Node): # represented as 4-tuples: (x-pos, y-pos, radius, acceleration) # '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, 10000000000, 100) # for testing - ] + + # TODO: remove after new version with json is tested + # CHECKPOINTS = [ + # # (589701, 4477160, 20, 0.5) + # (589701, 4477160, 10000000000, 100) # for testing + # ] + + with open("checkpoints.json", 'r') as checkpoints_file: + CHECKPOINTS = (json.load(checkpoints_file))["checkpoints"] def __init__(self): super().__init__('velocity_updater') @@ -33,8 +38,9 @@ def __init__(self): # initialize variables self.buggy_vel = self.init_vel - self.accel = 0.0 + self.update_vel = 0.0 self.position = Point() + # TODO: uncomment after controller is implemented # self.controller = Controller(self.buggy_name) # self.lock = threading.Lock() @@ -48,7 +54,7 @@ def __init__(self): ) # TODO: remove after controller is implemented - # publish velocity + # publish velocity self.velocity_publisher = self.create_publisher(Float64, "velocity", 1) # ROS2 timer for stepping @@ -57,35 +63,58 @@ def __init__(self): def update_position(self, new_pose: Pose): '''Callback function to update internal position variable when new buggy position is published - + Args: new_pose (Pose): Pose object from topic ''' with self.lock: self.position = new_pose.position + # TODO: maybe we should update velocity directly + # instead of calculating form acceleration def calculate_accel(self): '''Check if the position of the buggy is in any of the checkpoints set in self.CHECKPOINTS, and update acceleration of buggy accordingly ''' - for (x, y, r, a) in self.CHECKPOINTS: + + # TODO: remove after new version with json is tested + # for (x, y, r, a) in self.CHECKPOINTS: + # dist = math.sqrt((x-self.position.x)**2 + (y-self.position.y)**2) + # if dist < r: + # self.accel = a + # break + + for checkpoint in self.CHECKPOINTS: + x = checkpoint["x-pos"] + y = checkpoint["y-pos"] + r = checkpoint["radius"] + v = checkpoint["velocity"] dist = math.sqrt((x-self.position.x)**2 + (y-self.position.y)**2) if dist < r: - self.accel = a + self.update_vel = v break def step(self): '''Update acceleration and velocity of buggy for one timestep''' - self.calculate_accel() - new_velocity = self.buggy_vel + self.accel / self.RATE - self.buggy_vel = new_velocity + # self.calculate_accel() + for checkpoint in self.CHECKPOINTS: + x = checkpoint["x-pos"] + y = checkpoint["y-pos"] + r = checkpoint["radius"] + v = checkpoint["velocity"] + dist = math.sqrt((x-self.position.x)**2 + (y-self.position.y)**2) + if dist < r: + self.update_vel = v + break + + self.buggy_vel = self.update_vel # TODO: uncomment after controller is implemented - # self.controller.set_velocity(new_velocity) + # self.controller.set_velocity(self.update_vel) # TODO: remove after controller is implemented float_64_velocity = Float64() - float_64_velocity.data = float(new_velocity) + float_64_velocity.data = float(self.buggy_vel) self.velocity_publisher.publish(float_64_velocity) def main(args=None): From 9701efedeca7110c835b1f7ee1629e5caf4dcbc5 Mon Sep 17 00:00:00 2001 From: JoyceZhu2486 Date: Thu, 23 Jan 2025 13:17:41 -0500 Subject: [PATCH 04/11] fix file reading --- rb_ws/src/buggy/CMakeLists.txt | 2 +- rb_ws/src/buggy/launch/sim_2d_single_velocity.xml | 2 +- rb_ws/src/buggy/scripts/simulator/velocity_updater.py | 7 ++++--- 3 files changed, 6 insertions(+), 5 deletions(-) diff --git a/rb_ws/src/buggy/CMakeLists.txt b/rb_ws/src/buggy/CMakeLists.txt index 97cccb36..425a09ee 100644 --- a/rb_ws/src/buggy/CMakeLists.txt +++ b/rb_ws/src/buggy/CMakeLists.txt @@ -46,4 +46,4 @@ rosidl_generate_interfaces(${PROJECT_NAME} ) ament_export_dependencies(rosidl_default_runtime) -ament_package() \ No newline at end of file +ament_package() 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 edbdf92f..a0db17a1 100644 --- a/rb_ws/src/buggy/launch/sim_2d_single_velocity.xml +++ b/rb_ws/src/buggy/launch/sim_2d_single_velocity.xml @@ -11,7 +11,7 @@ - + diff --git a/rb_ws/src/buggy/scripts/simulator/velocity_updater.py b/rb_ws/src/buggy/scripts/simulator/velocity_updater.py index 7d914de3..bf974f1d 100644 --- a/rb_ws/src/buggy/scripts/simulator/velocity_updater.py +++ b/rb_ws/src/buggy/scripts/simulator/velocity_updater.py @@ -22,7 +22,9 @@ class VelocityUpdater(Node): # (589701, 4477160, 10000000000, 100) # for testing # ] - with open("checkpoints.json", 'r') as checkpoints_file: + checkpoints_path = "/rb_ws/src/buggy/scripts/simulator/checkpoints.json" + + with open(checkpoints_path, 'r') as checkpoints_file: CHECKPOINTS = (json.load(checkpoints_file))["checkpoints"] def __init__(self): @@ -70,8 +72,7 @@ def update_position(self, new_pose: Pose): with self.lock: self.position = new_pose.position - # TODO: maybe we should update velocity directly - # instead of calculating form acceleration + def calculate_accel(self): '''Check if the position of the buggy is in any of the checkpoints set in self.CHECKPOINTS, and update acceleration of buggy accordingly From 2b6ab2456726024c727f4e56ac9a4c413ca5e637 Mon Sep 17 00:00:00 2001 From: JoyceZhu2486 Date: Thu, 23 Jan 2025 13:21:14 -0500 Subject: [PATCH 05/11] fix merge error --- .../scripts/simulator/velocity_updater.py | 127 ++++++++++++++++++ 1 file changed, 127 insertions(+) diff --git a/rb_ws/src/buggy/scripts/simulator/velocity_updater.py b/rb_ws/src/buggy/scripts/simulator/velocity_updater.py index e69de29b..7d914de3 100644 --- a/rb_ws/src/buggy/scripts/simulator/velocity_updater.py +++ b/rb_ws/src/buggy/scripts/simulator/velocity_updater.py @@ -0,0 +1,127 @@ +#! /usr/bin/env python3 +import math +import threading +import json +import rclpy +from rclpy.node import Node +# 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 + # Bubbles for updating acceleration based on position + # represented as 4-tuples: (x-pos, y-pos, radius, acceleration) + # 'list[tuple[float,float,float,float]]' + # need further update such as more data or import data from certain files + + # TODO: remove after new version with json is tested + # CHECKPOINTS = [ + # # (589701, 4477160, 20, 0.5) + # (589701, 4477160, 10000000000, 100) # for testing + # ] + + with open("checkpoints.json", 'r') as checkpoints_file: + CHECKPOINTS = (json.load(checkpoints_file))["checkpoints"] + + def __init__(self): + super().__init__('velocity_updater') + self.get_logger().info('INITIALIZED.') + + # 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.update_vel = 0.0 + self.position = Point() + + # TODO: uncomment after controller is implemented + # self.controller = Controller(self.buggy_name) + # self.lock = threading.Lock() + + # 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) + + def update_position(self, new_pose: Pose): + '''Callback function to update internal position variable when new + buggy position is published + + Args: + new_pose (Pose): Pose object from topic + ''' + with self.lock: + self.position = new_pose.position + + # TODO: maybe we should update velocity directly + # instead of calculating form acceleration + def calculate_accel(self): + '''Check if the position of the buggy is in any of the checkpoints set + in self.CHECKPOINTS, and update acceleration of buggy accordingly + ''' + + # TODO: remove after new version with json is tested + # for (x, y, r, a) in self.CHECKPOINTS: + # dist = math.sqrt((x-self.position.x)**2 + (y-self.position.y)**2) + # if dist < r: + # self.accel = a + # break + + for checkpoint in self.CHECKPOINTS: + x = checkpoint["x-pos"] + y = checkpoint["y-pos"] + r = checkpoint["radius"] + v = checkpoint["velocity"] + dist = math.sqrt((x-self.position.x)**2 + (y-self.position.y)**2) + if dist < r: + self.update_vel = v + break + + def step(self): + '''Update acceleration and velocity of buggy for one timestep''' + # self.calculate_accel() + for checkpoint in self.CHECKPOINTS: + x = checkpoint["x-pos"] + y = checkpoint["y-pos"] + r = checkpoint["radius"] + v = checkpoint["velocity"] + dist = math.sqrt((x-self.position.x)**2 + (y-self.position.y)**2) + if dist < r: + self.update_vel = v + break + + self.buggy_vel = self.update_vel + + # TODO: uncomment after controller is implemented + # self.controller.set_velocity(self.update_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_updater = VelocityUpdater() + rclpy.spin(vel_updater) + rclpy.shutdown() + +if __name__ == "__main__": + main() \ No newline at end of file From 89230094070d5f00e4e4b4862abb4506b1a0b29e Mon Sep 17 00:00:00 2001 From: JoyceZhu2486 Date: Thu, 23 Jan 2025 13:28:41 -0500 Subject: [PATCH 06/11] fix merge error --- rb_ws/src/buggy/scripts/simulator/velocity_updater.py | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/rb_ws/src/buggy/scripts/simulator/velocity_updater.py b/rb_ws/src/buggy/scripts/simulator/velocity_updater.py index 7d914de3..bf974f1d 100644 --- a/rb_ws/src/buggy/scripts/simulator/velocity_updater.py +++ b/rb_ws/src/buggy/scripts/simulator/velocity_updater.py @@ -22,7 +22,9 @@ class VelocityUpdater(Node): # (589701, 4477160, 10000000000, 100) # for testing # ] - with open("checkpoints.json", 'r') as checkpoints_file: + checkpoints_path = "/rb_ws/src/buggy/scripts/simulator/checkpoints.json" + + with open(checkpoints_path, 'r') as checkpoints_file: CHECKPOINTS = (json.load(checkpoints_file))["checkpoints"] def __init__(self): @@ -70,8 +72,7 @@ def update_position(self, new_pose: Pose): with self.lock: self.position = new_pose.position - # TODO: maybe we should update velocity directly - # instead of calculating form acceleration + def calculate_accel(self): '''Check if the position of the buggy is in any of the checkpoints set in self.CHECKPOINTS, and update acceleration of buggy accordingly From d32f012392765320468a4e965b4d01d27534ae36 Mon Sep 17 00:00:00 2001 From: JoyceZhu2486 Date: Thu, 23 Jan 2025 15:07:09 -0500 Subject: [PATCH 07/11] fix velocity publisher and wrong topic name for subscriber error --- .../buggy/launch/sim_2d_single_velocity.xml | 2 +- .../buggy/scripts/simulator/checkpoints.json | 16 ++-- .../buggy/scripts/simulator/velocity_ui.py | 78 ------------------- .../scripts/simulator/velocity_updater.py | 73 ++++++----------- 4 files changed, 31 insertions(+), 138 deletions(-) delete mode 100644 rb_ws/src/buggy/scripts/simulator/velocity_ui.py 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 a0db17a1..0e5edc4b 100644 --- a/rb_ws/src/buggy/launch/sim_2d_single_velocity.xml +++ b/rb_ws/src/buggy/launch/sim_2d_single_velocity.xml @@ -12,7 +12,7 @@ - + \ No newline at end of file diff --git a/rb_ws/src/buggy/scripts/simulator/checkpoints.json b/rb_ws/src/buggy/scripts/simulator/checkpoints.json index 83dd0500..177c60c8 100644 --- a/rb_ws/src/buggy/scripts/simulator/checkpoints.json +++ b/rb_ws/src/buggy/scripts/simulator/checkpoints.json @@ -2,16 +2,16 @@ "checkpoints": [ { - "x-pos": 589701, - "y-pos": 4477160, - "radius": 10, - "velocity": 10 + "x-pos": 589700, + "y-pos": 4477159, + "radius": 20, + "velocity": 12.345 }, { - "x-pos": 589701, - "y-pos": 4476860, - "radius": 10, - "velocity": 50 + "x-pos": 589685, + "y-pos": 4477153, + "radius": 20, + "velocity": 17.234 } ] } \ No newline at end of file diff --git a/rb_ws/src/buggy/scripts/simulator/velocity_ui.py b/rb_ws/src/buggy/scripts/simulator/velocity_ui.py deleted file mode 100644 index 280144e5..00000000 --- a/rb_ws/src/buggy/scripts/simulator/velocity_ui.py +++ /dev/null @@ -1,78 +0,0 @@ -#! /usr/bin/env python3 -import sys -import threading -import tkinter as tk -# from controller_2d import Controller -import rclpy -from rclpy.node import Node -from std_msgs.msg import Float64 - -class VelocityUI(Node): - def __init__(self): - super().__init__('velocity_ui') - self.get_logger().info('INITIALIZED.') - - # 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: 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') - self.root.geometry('600x100') - self.root.configure(background='#342d66') - - self.scale = tk.Scale(self.root, from_=0, to=300, orient=tk.HORIZONTAL, - length=500, width=30) - self.scale.pack() - - self.root.bind("", lambda i: self.scale.set(self.scale.get() + 2)) - self.root.bind("", lambda d: self.scale.set(self.scale.get() - 2)) - - # ROS2 timer for stepping - # 0.01 is equivalent to 100Hz (100 times per second) - # self.create_timer(0.01, self.step) - - def step(self): - # Sets the velocity of the buggy to the current scale value - # called every tick - self.root.update_idletasks() - self.root.update() - # 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() - rclpy.spin(vel_ui) - rclpy.shutdown() - -if __name__ == "__main__": - main() \ No newline at end of file diff --git a/rb_ws/src/buggy/scripts/simulator/velocity_updater.py b/rb_ws/src/buggy/scripts/simulator/velocity_updater.py index bf974f1d..65ec1e79 100644 --- a/rb_ws/src/buggy/scripts/simulator/velocity_updater.py +++ b/rb_ws/src/buggy/scripts/simulator/velocity_updater.py @@ -4,26 +4,18 @@ import json import rclpy from rclpy.node import Node -# from controller_2d import Controller +from engine import Simulator from geometry_msgs.msg import Pose from geometry_msgs.msg import Point from std_msgs.msg import Float64 class VelocityUpdater(Node): RATE = 100 - # Bubbles for updating acceleration based on position - # represented as 4-tuples: (x-pos, y-pos, radius, acceleration) - # 'list[tuple[float,float,float,float]]' - # need further update such as more data or import data from certain files - - # TODO: remove after new version with json is tested - # CHECKPOINTS = [ - # # (589701, 4477160, 20, 0.5) - # (589701, 4477160, 10000000000, 100) # for testing - # ] + # Bubbles for updating acceleration based on position + # represented as {x-pos, y-pos, radius, velocity} + # imported from checkpoints.json checkpoints_path = "/rb_ws/src/buggy/scripts/simulator/checkpoints.json" - with open(checkpoints_path, 'r') as checkpoints_file: CHECKPOINTS = (json.load(checkpoints_file))["checkpoints"] @@ -40,24 +32,21 @@ def __init__(self): # initialize variables self.buggy_vel = self.init_vel - self.update_vel = 0.0 - self.position = Point() + self.debug_dist = 0 - # TODO: uncomment after controller is implemented - # self.controller = Controller(self.buggy_name) - # self.lock = threading.Lock() + self.position = Point() + self.lock = threading.Lock() - # subscribe pose + # subscribe sim_2d/utm for position values self.pose_subscriber = self.create_subscription( Pose, - f"{self.buggy_name}/sim_2d/utm", + "sim_2d/utm", self.update_position, - 10 # QoS profile + 1 ) - # TODO: remove after controller is implemented - # publish velocity - self.velocity_publisher = self.create_publisher(Float64, "velocity", 1) + # publish velocity to "sim/velocity" + self.velocity_publisher = self.create_publisher(Float64, "sim/velocity", 1) # ROS2 timer for stepping self.timer = self.create_timer(1.0 / self.RATE, self.step) @@ -72,52 +61,34 @@ def update_position(self, new_pose: Pose): with self.lock: self.position = new_pose.position - - def calculate_accel(self): + def check_velocity(self): '''Check if the position of the buggy is in any of the checkpoints set - in self.CHECKPOINTS, and update acceleration of buggy accordingly + in self.CHECKPOINTS, and update velocity of buggy accordingly ''' - - # TODO: remove after new version with json is tested - # for (x, y, r, a) in self.CHECKPOINTS: - # dist = math.sqrt((x-self.position.x)**2 + (y-self.position.y)**2) - # if dist < r: - # self.accel = a - # break - for checkpoint in self.CHECKPOINTS: x = checkpoint["x-pos"] y = checkpoint["y-pos"] r = checkpoint["radius"] v = checkpoint["velocity"] dist = math.sqrt((x-self.position.x)**2 + (y-self.position.y)**2) + self.debug_dist = dist if dist < r: - self.update_vel = v + self.buggy_vel = v break def step(self): - '''Update acceleration and velocity of buggy for one timestep''' - # self.calculate_accel() - for checkpoint in self.CHECKPOINTS: - x = checkpoint["x-pos"] - y = checkpoint["y-pos"] - r = checkpoint["radius"] - v = checkpoint["velocity"] - dist = math.sqrt((x-self.position.x)**2 + (y-self.position.y)**2) - if dist < r: - self.update_vel = v - break - - self.buggy_vel = self.update_vel + '''Update velocity of buggy for one timestep + and publish it so that the simulator engine can subscribe and use it''' - # TODO: uncomment after controller is implemented - # self.controller.set_velocity(self.update_vel) + # update velocity + self.check_velocity() - # TODO: remove after controller is implemented + # publish velocity 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_updater = VelocityUpdater() From 765f2b9e107cb9495b78365a9c07f863fa8a6eda Mon Sep 17 00:00:00 2001 From: JoyceZhu2486 Date: Thu, 23 Jan 2025 15:09:48 -0500 Subject: [PATCH 08/11] fix unused import --- rb_ws/src/buggy/scripts/simulator/velocity_updater.py | 1 - 1 file changed, 1 deletion(-) diff --git a/rb_ws/src/buggy/scripts/simulator/velocity_updater.py b/rb_ws/src/buggy/scripts/simulator/velocity_updater.py index 65ec1e79..b1a820c9 100644 --- a/rb_ws/src/buggy/scripts/simulator/velocity_updater.py +++ b/rb_ws/src/buggy/scripts/simulator/velocity_updater.py @@ -4,7 +4,6 @@ import json import rclpy from rclpy.node import Node -from engine import Simulator from geometry_msgs.msg import Pose from geometry_msgs.msg import Point from std_msgs.msg import Float64 From 3060cce0eb9baed931031a1820053b265435fb26 Mon Sep 17 00:00:00 2001 From: JoyceZhu2486 Date: Sun, 26 Jan 2025 16:17:19 -0500 Subject: [PATCH 09/11] add environment variable for the file path of checkpoints --- rb_ws/environments/docker_env.bash | 3 ++- .../buggy/scripts/simulator/checkpoints.json | 17 ----------------- .../buggy/scripts/simulator/velocity_updater.py | 17 ++++++++++------- 3 files changed, 12 insertions(+), 25 deletions(-) delete mode 100644 rb_ws/src/buggy/scripts/simulator/checkpoints.json diff --git a/rb_ws/environments/docker_env.bash b/rb_ws/environments/docker_env.bash index f580c4d0..9c1ef1c1 100755 --- a/rb_ws/environments/docker_env.bash +++ b/rb_ws/environments/docker_env.bash @@ -1,4 +1,5 @@ #!/bin/sh export RBROOT=/rb_ws export PYTHONPATH=$PYTHONPATH:$RBROOT/src/buggy/scripts -export TRAJPATH=$RBROOT/src/buggy/paths/ \ No newline at end of file +export TRAJPATH=$RBROOT/src/buggy/paths/ +export VELPATH=$RBROOT/src/buggy/scripts/simulator/velocity_checkpoints/ \ No newline at end of file diff --git a/rb_ws/src/buggy/scripts/simulator/checkpoints.json b/rb_ws/src/buggy/scripts/simulator/checkpoints.json deleted file mode 100644 index 177c60c8..00000000 --- a/rb_ws/src/buggy/scripts/simulator/checkpoints.json +++ /dev/null @@ -1,17 +0,0 @@ -{ - "checkpoints": - [ - { - "x-pos": 589700, - "y-pos": 4477159, - "radius": 20, - "velocity": 12.345 - }, - { - "x-pos": 589685, - "y-pos": 4477153, - "radius": 20, - "velocity": 17.234 - } - ] -} \ No newline at end of file diff --git a/rb_ws/src/buggy/scripts/simulator/velocity_updater.py b/rb_ws/src/buggy/scripts/simulator/velocity_updater.py index b1a820c9..084eb9e9 100644 --- a/rb_ws/src/buggy/scripts/simulator/velocity_updater.py +++ b/rb_ws/src/buggy/scripts/simulator/velocity_updater.py @@ -1,4 +1,5 @@ #! /usr/bin/env python3 +import os import math import threading import json @@ -11,17 +12,19 @@ class VelocityUpdater(Node): RATE = 100 - # Bubbles for updating acceleration based on position - # represented as {x-pos, y-pos, radius, velocity} - # imported from checkpoints.json - checkpoints_path = "/rb_ws/src/buggy/scripts/simulator/checkpoints.json" - with open(checkpoints_path, 'r') as checkpoints_file: - CHECKPOINTS = (json.load(checkpoints_file))["checkpoints"] - def __init__(self): super().__init__('velocity_updater') self.get_logger().info('INITIALIZED.') + # Bubbles for updating acceleration based on position + # represented as {x-pos, y-pos, radius, velocity} + # imported from a json file corresponding to the path + self.declare_parameter("checkpoints_name", "buggycourse_safe_checkpoints_1.json") + checkpoints_name = self.get_parameter("checkpoints_name").value + checkpoints_path = os.environ["VELPATH"] + checkpoints_name + with open(checkpoints_path, 'r') as checkpoints_file: + self.CHECKPOINTS = (json.load(checkpoints_file))["checkpoints"] + # Declare parameters with default values self.declare_parameter('init_vel', 12) self.declare_parameter('buggy_name', 'SC') From 08b2e29cb200779213f395ed7ee74c61a10014fb Mon Sep 17 00:00:00 2001 From: JoyceZhu2486 Date: Sun, 26 Jan 2025 16:18:10 -0500 Subject: [PATCH 10/11] add environment variable for the file path of checkpoints --- .../buggycourse_safe_checkpoints_1.json | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) create mode 100644 rb_ws/src/buggy/scripts/simulator/velocity_checkpoints/buggycourse_safe_checkpoints_1.json diff --git a/rb_ws/src/buggy/scripts/simulator/velocity_checkpoints/buggycourse_safe_checkpoints_1.json b/rb_ws/src/buggy/scripts/simulator/velocity_checkpoints/buggycourse_safe_checkpoints_1.json new file mode 100644 index 00000000..177c60c8 --- /dev/null +++ b/rb_ws/src/buggy/scripts/simulator/velocity_checkpoints/buggycourse_safe_checkpoints_1.json @@ -0,0 +1,17 @@ +{ + "checkpoints": + [ + { + "x-pos": 589700, + "y-pos": 4477159, + "radius": 20, + "velocity": 12.345 + }, + { + "x-pos": 589685, + "y-pos": 4477153, + "radius": 20, + "velocity": 17.234 + } + ] +} \ No newline at end of file From 283f95b3fd9358d1b390869c69da4e72321e24dc Mon Sep 17 00:00:00 2001 From: JoyceZhu2486 Date: Sun, 26 Jan 2025 16:37:22 -0500 Subject: [PATCH 11/11] fix minor error in xml --- rb_ws/src/buggy/launch/sim_2d_single_velocity.xml | 1 + 1 file changed, 1 insertion(+) 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 0e5edc4b..a2053367 100644 --- a/rb_ws/src/buggy/launch/sim_2d_single_velocity.xml +++ b/rb_ws/src/buggy/launch/sim_2d_single_velocity.xml @@ -13,6 +13,7 @@ + \ No newline at end of file