Refactored the entire State Machine Directory: We now call main("func…

…tion_test_x",Function_Test) in each test, rather than modifying the executor.

ros2/cmd/src/State_Machine_Executor.py → 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()
 

ros2/cmd/src/State_Machine_Interface.py → 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

ros2/cmd/src/state_machines/qualificaton_test/qualifying_test_q3.py → 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)