more function tests with updates

ros2/cmd/src/state_machines/function_tests/function_test_f5_4.py

@@ -0,0 +1,46 @@
+#Test FV4. Obstacle detection. Lane Changing (goes straight, detects obstacle, and changes lanes)
+
+
+from State_Machine_Executor import main
+from State_Machine_Interface import Interface
+
+class Function_Test_F3_1():
+    def __init__(self, interface):
+        self.interface = interface
+
+
+    # State transistion logic
+    def function_test(self):
+        barrel_count = 0
+        barrel_data = []
+        self.interface.car_SM.Resume()
+
+        ZED_TO_BUMPER = 0.4
+        barrel1_distance_threshold = 4.572
+        barrel2_distance_threshold = 3
+        stop_dist = 0.9144
+
+        while barrel_count < 1:
+            barrel_data = self.interface.Object_Detection(barrel1_distance_threshold, object_lists=["Barrel"],check_in_lane=True)
+            self.interface.Run()
+            if(barrel_data[0]):
+                self.interface.car_SM.Obj_Avoidance()
+                if(self.interface.current_lane):
+                    self.interface.Run([0.254,-3.048,0])
+                else: 
+                    self.interface.Run([0.254,3.048,0])
+                barrel_detected += 1
+
+        while barrel_count == 1:
+            barrel_data = self.interface.Object_Detection(barrel2_distance_threshold, object_lists=["Barrel"],check_in_lane=True)
+            self.interface.Run()  
+            if(barrel_data[0]):
+                self.interface.car_SM.Stop_Trigger()
+                self.interface.Run((barrel_data[1]+ZED_TO_BUMPER) - stop_dist)
+
+
+
+
+
+
+

ros2/cmd/src/state_machines/function_tests/function_test_f6.py

@@ -0,0 +1,34 @@
+#Executor
+from State_Machine_Executor import main
+from State_Machine_Interface import Interface
+
+
+# Test FV.1 (Go straight and stop five feet away from detected person)
+
+class Function_Test_FV_1:
+    # Get data from 5 meters away
+    threshold_distance = 5  # meters
+
+    def __init__(self, interface: Interface):
+        self.interface = interface
+
+    # State transistion logic
+    def function_test(self):
+        person_detected = False
+        distance = 0
+        self.interface.car_sm.Resume()
+        while not person_detected:
+            obj_data = self.interface.Object_Detection(
+                self.threshold_distance, object_list=["Person"]
+            )
+            if obj_data[0]:
+                distance = obj_data[1]
+                person_detected = True
+            self.interface.Run()
+        # 1.524 = 5 feet to meters
+        self.interface.car_sm.Stop_Trigger()
+        self.interface.Run(distance - 1.524)
+
+
+if __name__ == "__main__":
+    main("Function_Test_Q3",Function_Test_FV_1)

ros2/cmd/src/state_machines/function_tests/function_test_f7_1.py

@@ -0,0 +1,48 @@
+#Test F7.1 Pothole Detection
+
+
+from State_Machine_Executor import main
+from State_Machine_Interface import Interface
+
+class Function_Test_F3_1():
+    def __init__(self, interface):
+        self.interface = interface
+
+    # State transistion logic
+    def function_test(self):
+        pothole_detected = False
+        barrel_detected = False
+        pothole_data = []
+        barrel_data = []
+        self.interface.car_SM.Resume()
+
+        ZED_TO_BUMPER = 0.4
+        pothole_distance_threshold = 4.572
+        barrel_distance_threshold = 3
+        stop_dist = 0.9144
+
+        while not pothole_detected:
+            pothole_data = self.interface.Object_Detection(pothole_distance_threshold, object_lists=["PotHole"],check_in_lane=True)
+            self.interface.Run()
+            if(pothole_data[0]):
+                self.interface.car_SM.Obj_Avoidance()
+                if(self.interface.current_lane):
+                    self.interface.Run([0.254,-3.048,0])
+                else: 
+                    self.interface.Run([0.254,3.048,0])
+                pothole_detected = True
+
+        while not barrel_detected:
+            barrel_data = self.interface.Object_Detection(barrel_distance_threshold, object_lists=["Barrel"],check_in_lane=True)
+            self.interface.Run()  
+            if(barrel_data[0]):
+                self.interface.car_SM.Stop_Trigger()
+                self.interface.Run((barrel_data[1]+ZED_TO_BUMPER) - stop_dist)
+                barrel_detected = True
+
+
+
+
+
+
+

ros2/cmd/src/state_machines/function_tests/function_test_q3.py

@@ -1,7 +1,6 @@
 #Test Q.3 Lane Keeping (Go straight and stop at barrel)
 
-#Get data from 2 metres away
-threshold_distance = 5
+threshold_distance = 0.9144
 
 class Function_Test_Q3():
     def __init__(self, interface):
@@ -10,14 +9,12 @@ def __init__(self, interface):
     # State transistion logic
     def function_test(self):
         barrel_counter = 0
-        distance = 0
         self.interface.car_SM.Resume()
-        while barrel_counter < 1:
-            obj_data = self.interface.Object_Detection(threshold_distance, object_list=["Barrel"])           
-            if(obj_data[0]):
-                distance = obj_data[1] 
+        while barrel_counter < 1:            
+            if(self.interface.Object_Detection(threshold_distance, cared_objects=["Barrel"],)):
+                self.interface.car_SM.Obj_Avoidance()
                 barrel_counter += 1
             self.interface.Run()
-        #0.9144 = 3 feet to meters
-        self.interface.car_SM.Stop_Trigger(distance - 0.9144)
+
+        self.interface.car_SM.Stop_Trigger()
         self.interface.Run()