From a9360f53343d6b42955b62726e20ad3257ff8f3e Mon Sep 17 00:00:00 2001 From: Vaibhav Hariani Date: Sun, 2 Jun 2024 09:50:17 -0400 Subject: [PATCH] Refactored the entire State Machine Directory: We now call main("function_test_x",Function_Test) in each test, rather than modifying the executor. --- .../{ => modules}/State_Machine_Executor.py | 15 +++++---------- .../{ => modules}/State_Machine_Interface.py | 4 ++-- ros2/cmd/src/modules/__init__.py | 0 .../src/{state_machines => modules}/car_sm.py | 0 .../controller/clothoid_path_planner.py | 0 .../lane_behaviors/controller/stanley.py | 0 .../lane_behaviors/individual_follower.py | 0 .../lane_behaviors/lane_change.py | 0 .../lane_behaviors/lane_follower.py | 0 .../{ => modules}/lane_behaviors/odom_sub.py | 0 ros2/cmd/src/{ => modules}/lane_change_test.py | 0 ros2/cmd/src/{ => modules}/lane_follow_test.py | 0 .../qualifying_test_q3.py | 18 +++++++++++++----- .../qualifying_test_q5.py | 0 .../qualifying_test_q6.py | 0 15 files changed, 20 insertions(+), 17 deletions(-) rename ros2/cmd/src/{ => modules}/State_Machine_Executor.py (83%) rename ros2/cmd/src/{ => modules}/State_Machine_Interface.py (99%) create mode 100644 ros2/cmd/src/modules/__init__.py rename ros2/cmd/src/{state_machines => modules}/car_sm.py (100%) rename ros2/cmd/src/{ => modules}/lane_behaviors/controller/clothoid_path_planner.py (100%) rename ros2/cmd/src/{ => modules}/lane_behaviors/controller/stanley.py (100%) rename ros2/cmd/src/{ => modules}/lane_behaviors/individual_follower.py (100%) rename ros2/cmd/src/{ => modules}/lane_behaviors/lane_change.py (100%) rename ros2/cmd/src/{ => modules}/lane_behaviors/lane_follower.py (100%) rename ros2/cmd/src/{ => modules}/lane_behaviors/odom_sub.py (100%) rename ros2/cmd/src/{ => modules}/lane_change_test.py (100%) rename ros2/cmd/src/{ => modules}/lane_follow_test.py (100%) rename ros2/cmd/src/state_machines/{qualificaton_test => qualifying_tests}/qualifying_test_q3.py (64%) rename ros2/cmd/src/state_machines/{qualificaton_test => qualifying_tests}/qualifying_test_q5.py (100%) rename ros2/cmd/src/state_machines/{qualificaton_test => qualifying_tests}/qualifying_test_q6.py (100%) diff --git a/ros2/cmd/src/State_Machine_Executor.py b/ros2/cmd/src/modules/State_Machine_Executor.py similarity index 83% rename from ros2/cmd/src/State_Machine_Executor.py rename to ros2/cmd/src/modules/State_Machine_Executor.py index da2e0573d..0067dc71b 100644 --- a/ros2/cmd/src/State_Machine_Executor.py +++ b/ros2/cmd/src/modules/State_Machine_Executor.py @@ -1,22 +1,17 @@ +import string import rclpy from threading import Thread import traceback -from state_machines.qualificaton_test.qualifying_test_q5 import ( - Function_Test_Q5 as Function_Test, -) - -# This is one of two lines that needs to be changed every time from State_Machine_Interface import Interface as SM_Interface from lane_behaviors.odom_sub import OdomSubscriber from lane_behaviors.lane_change import LaneChange from lane_behaviors.lane_follower import LaneFollower -def main(args=None): +def main(node_label:string, Function_Test:object,args=None): try: - rclpy.init(args=args) - + rclpy.init(args=args) max_dist_to_goal = 0.5 max_dist_to_path = 1.5 @@ -25,7 +20,7 @@ def main(args=None): lane_follow = LaneFollower(odom_sub) # Change this to specify which function test to run - Interface = SM_Interface("Quali_Test_Q4", lane_change, lane_follow) + Interface = SM_Interface(node_label, lane_change, lane_follow) function_test = Function_Test(Interface) executor = rclpy.executors.MultiThreadedExecutor() @@ -47,7 +42,7 @@ def main(args=None): except (KeyboardInterrupt, rclpy.executors.ExternalShutdownException): print("Exception Thrown, Handling Gracefully") - function_test.interface.Estop_Action() + Interface.Estop_Action() except Exception as e: traceback.print_exception() diff --git a/ros2/cmd/src/State_Machine_Interface.py b/ros2/cmd/src/modules/State_Machine_Interface.py similarity index 99% rename from ros2/cmd/src/State_Machine_Interface.py rename to ros2/cmd/src/modules/State_Machine_Interface.py index 0487153e8..81497a0ec 100644 --- a/ros2/cmd/src/State_Machine_Interface.py +++ b/ros2/cmd/src/modules/State_Machine_Interface.py @@ -4,7 +4,7 @@ from std_msgs.msg import Float32MultiArray, String from geometry_msgs.msg import PoseWithCovariance -from state_machines import car_sm +from car_sm import CarSM from lane_behaviors.lane_follower import LaneFollower from lane_behaviors.lane_change import LaneChange @@ -26,7 +26,7 @@ def __init__( self.lane_change = lane_change self.lane_follow = lane_follow - self.car_sm = car_sm.CarSM() + self.car_sm = CarSM() self.object_global_position_x = None self.object_global_position_y = None diff --git a/ros2/cmd/src/modules/__init__.py b/ros2/cmd/src/modules/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/ros2/cmd/src/state_machines/car_sm.py b/ros2/cmd/src/modules/car_sm.py similarity index 100% rename from ros2/cmd/src/state_machines/car_sm.py rename to ros2/cmd/src/modules/car_sm.py diff --git a/ros2/cmd/src/lane_behaviors/controller/clothoid_path_planner.py b/ros2/cmd/src/modules/lane_behaviors/controller/clothoid_path_planner.py similarity index 100% rename from ros2/cmd/src/lane_behaviors/controller/clothoid_path_planner.py rename to ros2/cmd/src/modules/lane_behaviors/controller/clothoid_path_planner.py diff --git a/ros2/cmd/src/lane_behaviors/controller/stanley.py b/ros2/cmd/src/modules/lane_behaviors/controller/stanley.py similarity index 100% rename from ros2/cmd/src/lane_behaviors/controller/stanley.py rename to ros2/cmd/src/modules/lane_behaviors/controller/stanley.py diff --git a/ros2/cmd/src/lane_behaviors/individual_follower.py b/ros2/cmd/src/modules/lane_behaviors/individual_follower.py similarity index 100% rename from ros2/cmd/src/lane_behaviors/individual_follower.py rename to ros2/cmd/src/modules/lane_behaviors/individual_follower.py diff --git a/ros2/cmd/src/lane_behaviors/lane_change.py b/ros2/cmd/src/modules/lane_behaviors/lane_change.py similarity index 100% rename from ros2/cmd/src/lane_behaviors/lane_change.py rename to ros2/cmd/src/modules/lane_behaviors/lane_change.py diff --git a/ros2/cmd/src/lane_behaviors/lane_follower.py b/ros2/cmd/src/modules/lane_behaviors/lane_follower.py similarity index 100% rename from ros2/cmd/src/lane_behaviors/lane_follower.py rename to ros2/cmd/src/modules/lane_behaviors/lane_follower.py diff --git a/ros2/cmd/src/lane_behaviors/odom_sub.py b/ros2/cmd/src/modules/lane_behaviors/odom_sub.py similarity index 100% rename from ros2/cmd/src/lane_behaviors/odom_sub.py rename to ros2/cmd/src/modules/lane_behaviors/odom_sub.py diff --git a/ros2/cmd/src/lane_change_test.py b/ros2/cmd/src/modules/lane_change_test.py similarity index 100% rename from ros2/cmd/src/lane_change_test.py rename to ros2/cmd/src/modules/lane_change_test.py diff --git a/ros2/cmd/src/lane_follow_test.py b/ros2/cmd/src/modules/lane_follow_test.py similarity index 100% rename from ros2/cmd/src/lane_follow_test.py rename to ros2/cmd/src/modules/lane_follow_test.py diff --git a/ros2/cmd/src/state_machines/qualificaton_test/qualifying_test_q3.py b/ros2/cmd/src/state_machines/qualifying_tests/qualifying_test_q3.py similarity index 64% rename from ros2/cmd/src/state_machines/qualificaton_test/qualifying_test_q3.py rename to ros2/cmd/src/state_machines/qualifying_tests/qualifying_test_q3.py index 1a6d2de11..ad13411dd 100644 --- a/ros2/cmd/src/state_machines/qualificaton_test/qualifying_test_q3.py +++ b/ros2/cmd/src/state_machines/qualifying_tests/qualifying_test_q3.py @@ -1,11 +1,15 @@ -# Test Q.3 Lane Keeping (Go straight and stop at barrel) +#Executor +from State_Machine_Executor import main +from State_Machine_Interface import Interface -# Get data from 2 metres away -threshold_distance = 5 # meters +# Test Q.3 Lane Keeping (Go straight and stop at barrel) class Function_Test_Q3: - def __init__(self, interface): + # Get data from 5 meters away + threshold_distance = 5 # meters + + def __init__(self, interface: Interface): self.interface = interface # State transistion logic @@ -15,7 +19,7 @@ def function_test(self): self.interface.car_sm.Resume() while barrel_counter < 1: obj_data = self.interface.Object_Detection( - threshold_distance, object_list=["Barrel"] + self.threshold_distance, object_list=["Barrel"] ) if obj_data[0]: distance = obj_data[1] @@ -24,3 +28,7 @@ def function_test(self): # 0.9144 = 3 feet to meters self.interface.car_sm.Stop_Trigger() self.interface.Run(distance - 0.9144) + + +if __name__ == "__main__": + main("Function_Test_Q3",Function_Test_Q3) \ No newline at end of file diff --git a/ros2/cmd/src/state_machines/qualificaton_test/qualifying_test_q5.py b/ros2/cmd/src/state_machines/qualifying_tests/qualifying_test_q5.py similarity index 100% rename from ros2/cmd/src/state_machines/qualificaton_test/qualifying_test_q5.py rename to ros2/cmd/src/state_machines/qualifying_tests/qualifying_test_q5.py diff --git a/ros2/cmd/src/state_machines/qualificaton_test/qualifying_test_q6.py b/ros2/cmd/src/state_machines/qualifying_tests/qualifying_test_q6.py similarity index 100% rename from ros2/cmd/src/state_machines/qualificaton_test/qualifying_test_q6.py rename to ros2/cmd/src/state_machines/qualifying_tests/qualifying_test_q6.py