Merge pull request #249 from una-auxme/232-feature-clean-up-of-unnece…

…ssary-experiment-plot-data

fix: last fix

code/acting/src/acting/velocity_controller.py

@@ -5,7 +5,6 @@
 from rospy import Publisher, Subscriber
 from simple_pid import PID
 from std_msgs.msg import Float32, Bool
-from nav_msgs.msg import Path
 import rospy
 
 
@@ -49,14 +48,6 @@ def __init__(self):
             f"/paf/{self.role_name}/reverse",
             qos_profile=1)
 
-        # needed to prevent the car from driving before a path to follow is
-        # available. Might be needed later to slow down in curves
-        self.trajectory_sub: Subscriber = self.new_subscription(
-            Path,
-            f"/paf/{self.role_name}/trajectory",
-            self.__set_trajectory,
-            qos_profile=1)
-
         self.__current_velocity: float = None
         self.__target_velocity: float = None
 

code/perception/src/00_Experiments/Position_Heading_Datasets/viz.py

@@ -13,7 +13,7 @@
 FILE_START = 0
 FILE_END = 20
 
-FILE_NUM = 26  # Change this to plot your wanted file #
+FILE_NUM = "00"  # Change this to plot your wanted file #
 
 
 # current file_name to plot the data of one file!
@@ -176,7 +176,7 @@ def plot_x_or_y_or_h_notched_box(file_name, type='x', error_type='MSE'):
     tuple = zip(boxplot['boxes'], colors, boxplot['medians'],
                 boxplot['whiskers'][::2])
 
-    for i, (box, color, median) in enumerate(tuple):
+    for i, (box, color, median, whiskers) in enumerate(tuple):
         box.set_facecolor(color)
         median_val = median.get_ydata()[1]
         ax.text(i+1, median_val, f'Median: {median_val:.2f}', va='center',
@@ -327,14 +327,14 @@ def plot_CEP(file_name):
     df_y.set_index(df_y.columns[0], inplace=True)
 
     # create pos tuples of the x and y data and store them as numpy arrays
-    ideal_pos = np.array(list(zip(df_x['Ideal (Carla) X'],
-                                  df_y['Ideal (Carla) Y'])))
-    test_filter_pos = np.array(list(zip(df_x['Test Filter X'],
-                                        df_y['Test Filter Y'])))
-    current_pos = np.array(list(zip(df_x['Current X'],
-                                    df_y['Current Y'])))
-    unfiltered_pos = np.array(list(zip(df_x['Unfiltered X'],
-                                       df_y['Unfiltered Y'])))
+    ideal_pos = np.array(list(zip(df_x['Ideal (Carla)'],
+                                  df_y['Ideal (Carla)'])))
+    test_filter_pos = np.array(list(zip(df_x['Test Filter'],
+                                        df_y['Test Filter'])))
+    current_pos = np.array(list(zip(df_x['Current'],
+                                    df_y['Current'])))
+    unfiltered_pos = np.array(list(zip(df_x['Unfiltered'],
+                                       df_y['Unfiltered'])))
 
     # create CEP for each method
     cep_test_filter, cep_current, cep_unfiltered = calculate_cep(
@@ -642,14 +642,14 @@ def get_positions_from_csv_file(file_name, file_name_y=file_name):
     df_y.set_index(df_y.columns[0], inplace=True)
 
     # create pos tuples of the x and y data and store them as numpy arrays
-    ideal_pos = np.array(list(zip(df_x['Ideal (Carla) X'],
-                                  df_y['Ideal (Carla) Y'])))
-    test_filter_pos = np.array(list(zip(df_x['Test Filter X'],
-                                        df_y['Test Filter Y'])))
-    current_pos = np.array(list(zip(df_x['Current X'],
-                                    df_y['Current Y'])))
-    unfiltered_pos = np.array(list(zip(df_x['Unfiltered X'],
-                                       df_y['Unfiltered Y'])))
+    ideal_pos = np.array(list(zip(df_x['Ideal (Carla)'],
+                                  df_y['Ideal (Carla)'])))
+    test_filter_pos = np.array(list(zip(df_x['Test Filter'],
+                                        df_y['Test Filter'])))
+    current_pos = np.array(list(zip(df_x['Current'],
+                                    df_y['Current'])))
+    unfiltered_pos = np.array(list(zip(df_x['Unfiltered'],
+                                       df_y['Unfiltered'])))
 
     return ideal_pos, test_filter_pos, current_pos, unfiltered_pos
 
@@ -680,23 +680,23 @@ def get_x_or_y_or_h_from_csv_file(file_name, type='x'):
 
     # Read the CSV file into a DataFrame
     # (skip the first couple rows because of wrong measurements)
-    df = pd.read_csv(file_path, skiprows=8)
+    df = pd.read_csv(file_path)
 
     # Set the first column (time) as the index of the DataFrames
     df.set_index(df.columns[0], inplace=True)
 
     if type == 'x':
         # store x as numpy arrays
-        ideal = np.array(df['Ideal (Carla) X'])
-        test_filter = np.array(df['Test Filter X'])
-        current = np.array(df['Current X'])
-        unfiltered = np.array(df['Unfiltered X'])
+        ideal = np.array(df['Ideal (Carla)'])
+        test_filter = np.array(df['Test Filter'])
+        current = np.array(df['Current'])
+        unfiltered = np.array(df['Unfiltered'])
     elif type == 'y':
         # store y as numpy arrays
-        ideal = np.array(df['Ideal (Carla) Y'])
-        test_filter = np.array(df['Test Filter Y'])
-        current = np.array(df['Current Y'])
-        unfiltered = np.array(df['Unfiltered Y'])
+        ideal = np.array(df['Ideal (Carla)'])
+        test_filter = np.array(df['Test Filter'])
+        current = np.array(df['Current'])
+        unfiltered = np.array(df['Unfiltered'])
     elif type == 'h':
         # store heading as numpy arrays
         ideal = np.array(df['Ideal (Carla)'])
@@ -717,8 +717,8 @@ def get_x_or_y_or_h_from_csv_file(file_name, type='x'):
     plot_x_or_y_or_h_notched_box(data, type='x', error_type='MSE')
     plot_x_or_y_or_h_notched_box(data, type='x', error_type='MAE')
 
-    # plot_x_or_y_or_h_notched_box(data, type='y', error_type='MSE')
-    # plot_x_or_y_or_h_notched_box(data, type='y', error_type='MAE')
+    plot_x_or_y_or_h_notched_box(data, type='y', error_type='MSE')
+    plot_x_or_y_or_h_notched_box(data, type='y', error_type='MAE')
 
     # plot_x_or_y_or_h_notched_box(data, type='h', error_type='MSE')
     # plot_x_or_y_or_h_notched_box(data, type='h', error_type='MAE')

code/perception/src/position_heading_filter_debug_node.py

@@ -9,7 +9,7 @@
 import ros_compatibility as roscomp
 from ros_compatibility.node import CompatibleNode
 from geometry_msgs.msg import PoseStamped
-from std_msgs.msg import Float32
+from std_msgs.msg import Float32, Header
 # from tf.transformations import euler_from_quaternion
 from std_msgs.msg import Float32MultiArray
 import rospy
@@ -18,7 +18,7 @@
 
 GPS_RUNNING_AVG_ARGS: int = 10
 DATA_SAVING_MAX_TIME: int = 45
-FOLDER_PATH: str = "Position_Heading_Datasets"
+FOLDER_PATH: str = "/Position_Heading_Datasets"
 
 
 class position_heading_filter_debug_node(CompatibleNode):
@@ -43,16 +43,16 @@ def __init__(self):
         CARLA_HOST = os.environ.get('CARLA_HOST', 'paf23-carla-simulator-1')
         CARLA_PORT = int(os.environ.get('CARLA_PORT', '2000'))
         self.client = carla.Client(CARLA_HOST, CARLA_PORT)
-        self.world = self.client.get_world()
-        self.world.wait_for_tick()
+        self.world = None
         self.carla_car = None
 
-        self.set_carla_attributes()
+        # self.set_carla_attributes()
 
         # Tracked Attributes for Debugging
         self.current_pos = PoseStamped()
         self.current_heading = Float32()
-        self.carla_current_pos = PoseStamped()
+        self.carla_current_pos = carla.Location()
+        self.carla_current_heading = 0.0
         self.unfiltered_pos = PoseStamped()
         self.unfiltered_heading = Float32()
 
@@ -144,6 +144,13 @@ def __init__(self):
         # endregion Publisher END
 
     # region Subscriber Callbacks
+
+    def set_unfiltered_heading(self, data: Float32):
+        """
+        This method is called when new unfiltered_heading data is received.
+        """
+        self.unfiltered_heading = data
+
     def set_unfiltered_pos(self, data: PoseStamped):
         """
         This method is called when new unfiltered_pos data is received.
@@ -193,8 +200,8 @@ def save_position_data(self):
         # Specify the path to the folder where you want to save the data
         base_path = ('/workspace/code/perception/'
                      'src/00_Experiments/' + FOLDER_PATH)
-        folder_path_x = base_path + 'x_error'
-        folder_path_y = base_path + 'y_error'
+        folder_path_x = base_path + '/x_error'
+        folder_path_y = base_path + '/y_error'
         # Ensure the directories exist
         os.makedirs(folder_path_x, exist_ok=True)
         os.makedirs(folder_path_y, exist_ok=True)
@@ -226,7 +233,7 @@ def save_heading_data(self):
         # Specify the path to the folder where you want to save the data
         base_path = ('/workspace/code/perception/'
                      'src/00_Experiments' + FOLDER_PATH)
-        folder_path_heading = base_path + 'heading_error'
+        folder_path_heading = base_path + '/heading_error'
 
         # Ensure the directories exist
         os.makedirs(folder_path_heading, exist_ok=True)
@@ -248,7 +255,7 @@ def write_csv_heading(self):
                 writer.writerow([
                     "Time",
                     "Unfiltered",
-                    "Ideal(Carla)",
+                    "Ideal (Carla)",
                     "Current",
                     "Test Filter",
                     "Unfiltered Error",
@@ -272,13 +279,13 @@ def write_csv_x(self):
             if os.stat(self.csv_file_path_x).st_size == 0:
                 writer.writerow([
                     "Time",
-                    "Unfiltered X",
-                    "Ideal (Carla) X",
-                    "Current X",
-                    "Test Filter X",
-                    "Unfiltered X Error",
-                    "Current X Error",
-                    "Test Filter X Error"
+                    "Unfiltered",
+                    "Ideal (Carla)",
+                    "Current",
+                    "Test Filter",
+                    "Unfiltered Error",
+                    "Current Error",
+                    "Test Filter Error"
                 ])
             writer.writerow([
                 rospy.get_time(),
@@ -298,13 +305,13 @@ def write_csv_y(self):
             if os.stat(self.csv_file_path_y).st_size == 0:
                 writer.writerow([
                     "Time",
-                    "Unfiltered Y",
-                    "Ideal (Carla) Y",
-                    "Current Y",
-                    "Test Filter Y",
-                    "Unfiltered Y Error",
-                    "Current Y Error",
-                    "Test Filter Y Error"
+                    "Unfiltered",
+                    "Ideal (Carla)",
+                    "Current",
+                    "Test Filter",
+                    "Unfiltered Error",
+                    "Current Error",
+                    "Test Filter Error"
                 ])
             writer.writerow([
                 rospy.get_time(),
@@ -328,13 +335,22 @@ def set_carla_attributes(self):
                 self.carla_car = actor
                 break
         if self.carla_car is None:
-            self.logwarn("Carla Hero car still none!")
+            # self.logwarn("Carla Hero car still none!")
             return
         else:
-            self.carla_current_pos = self.carla_car.get_location()
-            self.carla_current_heading = (
-                                        self.carla_car.get_transform()
-                                        .rotation.yaw
+            # save carla Position
+            # for some reason the carla y is flipped
+            carla_pos = self.carla_car.get_location()
+            carla_pos.y = -carla_pos.y
+            self.carla_current_pos = carla_pos
+
+            # save carla Heading
+            # for some reason the carla yaw is in flipped degrees
+            # -> convert to radians
+            # -> also flip the sign to minus
+            self.carla_current_heading = -math.radians(
+                                         self.carla_car.get_transform()
+                                         .rotation.yaw
                                         )
 
     def position_debug(self):
@@ -383,13 +399,13 @@ def position_debug(self):
         """
 
         if self.carla_car is None:
-            self.logwarn("""Carla Hero car still none!
-                         Can not record data for position_debug yet.""")
+            # self.logwarn("""Carla Hero car still none!
+            #              Can not record data for position_debug yet.""")
             return
 
         debug = Float32MultiArray()
 
-        debug.data = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
+        debug.data = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
 
         # all x and y coordinates
         debug.data[0] = self.unfiltered_pos.pose.position.x
@@ -433,8 +449,11 @@ def position_debug(self):
 
         self.position_debug_data = debug
         self.position_debug_publisher.publish(debug)
+
         # for easier debugging with rqt_plot
-        self.carla_pos_publisher.publish(self.carla_current_pos)
+        # Publish carla Location as PoseStamped:
+        self.carla_pos_publisher.publish(carla_location_to_pose_stamped(
+                                         self.carla_current_pos))
 
     def heading_debug(self):
         """
@@ -461,13 +480,13 @@ def heading_debug(self):
         """
 
         if self.carla_car is None:
-            self.logwarn("""Carla Hero car still none!
-                         Can not record data for heading_debug yet.""")
+            # self.logwarn("""Carla Hero car still none!
+            #              Can not record data for heading_debug yet.""")
             return
 
         debug = Float32MultiArray()
 
-        debug.data = [0, 0, 0, 0, 0, 0, 0, 0]
+        debug.data = [0, 0, 0, 0, 0, 0, 0]
 
         # all heading values
         debug.data[0] = self.unfiltered_heading.data
@@ -502,6 +521,21 @@ def run(self):
         - saves position and heading errors in csv files (if uncommented)
         :return:
         """
+        # Retry connecting to the CARLA simulator up to 5 times
+        for _ in range(5):
+            try:
+                self.world = self.client.get_world()
+                break
+            except RuntimeError:
+                print("Failed to connect to the CARLA simulator, retrying...")
+                rospy.sleep(1)  # Wait for 1 second before retrying
+        self.world.wait_for_tick()
+
+        # Wait for the car to be spawned
+        # Otherwise we save invalid data (before car teleports
+        # to start position)
+        rospy.sleep(5)
+
         def loop():
             """
             Loop for the data gathering
@@ -512,8 +546,8 @@ def loop():
 
                 # if carla_car still not found -> skip
                 if self.carla_car is None:
-                    self.logwarn("""Carla Hero car still none!
-                                 Can not record data for debug yet.""")
+                    # self.logwarn("""Carla Hero car still none!
+                    #              Can not record data for debug yet.""")
                     continue
 
                 # update & publish pos debug and heading debug
@@ -547,6 +581,29 @@ def create_file(folder_path):
         i += 1
 
 
+def carla_location_to_pose_stamped(location: carla.Location) -> PoseStamped:
+    """
+    Convert a carla.Location to a geometry_msgs/PoseStamped message.
+    """
+    pose_stamped = PoseStamped()
+    # Fill in the header
+    pose_stamped.header = Header()
+    pose_stamped.header.stamp = rospy.Time.now()
+
+    # Fill in the pose
+    pose_stamped.pose.position.x = location.x
+    pose_stamped.pose.position.y = location.y
+    pose_stamped.pose.position.z = location.z
+
+    # Assuming you have no orientation information
+    pose_stamped.pose.orientation.x = 0
+    pose_stamped.pose.orientation.y = 0
+    pose_stamped.pose.orientation.z = 0
+    pose_stamped.pose.orientation.w = 1
+
+    return pose_stamped
+
+
 def main(args=None):
     """
     main function