Files for each type of algo and added Manual algo type

robomechanics · May 2, 2024 · f26d1f6 · f26d1f6
1 parent 9ca6f3d
commit f26d1f6
Show file tree

Hide file tree

Showing 2 changed files with 27 additions and 7 deletions.
diff --git a/gps_gui/src/gui.py b/gps_gui/src/gui.py
@@ -40,6 +40,7 @@
 ALGO_GRID = 'grid'
 ALGO_WAYPOINT = 'waypoint'
 ALGO_NONE = 'algo_none'
+ALGO_MANUAL = 'manual'
 
 class GpsNavigationGui:
     def __init__(self, lat, lon, zoom, width, height):
@@ -419,6 +420,9 @@ def pxrfResponseCallback(self, result:String):
 
             self.addMarkerAt(self.prev_lat, self.prev_lon, f'Sample#{self.numMeasurements}')
 
+            rospy.set_param(self._algorithm_type_param_name, self.algorithm_type_before_manual_sample)
+            rospy.sleep(0.5)
+
             try:
                 lower_arm_service = rospy.ServiceProxy(self._lower_arm_service_name, SetBool)
                 lower_arm_service(False)
@@ -711,6 +715,8 @@ def togglePxrfCollection(self):
             lower_arm_service = rospy.ServiceProxy(self._lower_arm_service_name, SetBool)
             lower_arm_service(True)
 
+            self.algorithm_type_before_manual_sample = rospy.get_param(self._algorithm_type_param_name)
+            rospy.set_param(self._algorithm_type_param_name, ALGO_MANUAL)
             rospy.sleep(1.5)
 
             start_scan_service = rospy.ServiceProxy(self._start_scan_service_name, Complete)

diff --git a/pxrf/scripts/pxrf_handler.py b/pxrf/scripts/pxrf_handler.py
@@ -45,13 +45,18 @@ def chemistry_parser(chemistry):
             error.append(arr[i])
     return element, concentration, error
 
+ALGO_ADAPTIVE = 'adaptive'
+ALGO_GRID = 'grid'
+ALGO_WAYPOINT = 'waypoint'
+ALGO_MANUAL = 'manual'
+ALGO_NONE = 'algo_none'
+VALID_ALGOS = [ALGO_ADAPTIVE, ALGO_GRID, ALGO_WAYPOINT, ALGO_MANUAL]
+
 class PXRFHandler(object):
     def __init__(self):
         rospy.init_node("pxrf_handler", anonymous=True)
         self.load_ros_params()
 
-        self.create_log_file()
-
         self.pxrf_command_pub = rospy.Publisher(self._pxrf_cmd_topic, String, queue_size=1)
         self.scan_completed_pub = rospy.Publisher(self._scan_completed_topic, CompletedScanData, queue_size=1)
 
@@ -86,7 +91,9 @@ def load_ros_params(self):
         self._gps_odom_topic = rospy.get_param("gps_odom_topic")
         self._rotate_scan_log_file_service_name = rospy.get_param("rotate_scan_log_file_service_name")
         self._autonomy_params_service_name = rospy.get_param("autonomy_params_service_name")
+        self._algorithm_type_param_name = rospy.get_param("algorithm_type_param_name")
 
+        self.algorithm_type = rospy.get_param(self._algorithm_type_param_name)
         self.root_data_dir = os.path.expanduser(rospy.get_param("data_dir"))
         self.element_of_interest = rospy.get_param("element_of_interest")
 
@@ -114,18 +121,21 @@ def scan_start(self, data):
         return True
 
     def create_log_file(self):
+        self.algorithm_type = rospy.get_param(self._algorithm_type_param_name)
         self.data_dir = os.path.join(self.root_data_dir, datetime.now().strftime("%d-%m-%Y_%H:%M:%S"))
-        self.data_file = os.path.join(self.data_dir, "scan_results.csv")
+        self.data_file = os.path.join(self.data_dir, "scan_results_" + self.algorithm_type + ".csv")
         self.yaml_file = os.path.join(self.data_dir, "params.yaml")
 
         if not os.path.exists(self.data_dir):
             rospy.loginfo(f" |Creating directory {self.data_dir}")
             os.makedirs(self.data_dir)
 
-        if not os.path.exists(self.data_file):
-            rospy.loginfo(f" |Creating file {self.data_file}")
-            with open(self.data_file, 'w') as fp:
-                pass
+        for algo_type in VALID_ALGOS:
+            data_file = os.path.join(self.data_dir, "scan_results_" + algo_type + ".csv")
+            if not os.path.exists(data_file):
+                rospy.loginfo(f" |Creating file {data_file}")
+                with open(data_file, 'w') as fp:
+                    pass
 
     def autonomy_params_callback(self, data: AutonomyParams):
         data_info = {
@@ -175,6 +185,10 @@ def data_listener(self, data: PxrfMsg):
                 self.gps_odom_heading,
             ]
 
+
+            self.algorithm_type = rospy.get_param(self._algorithm_type_param_name)
+            self.data_file = os.path.join(self.data_dir, "scan_results_" + self.algorithm_type + ".csv")
+
             with open(self.data_file, "a+") as f:
                 writer = csv.writer(f)
                 writer.writerow(header)