Refactor sensor data handling and improve point cloud generation logic

workspaces/COLCON_WS/src/ser_test/ser_test/ser_test_node.py

@@ -5,19 +5,35 @@ import numpy as np
 from rclpy.node import Node
 import sensor_msgs.msg as sensor_msgs
 from std_msgs.msg import Header
-angles = [19.923,14.515,8.829,2.964,182.964,188.829,194.515,199.923]
+angles = [29.923, 22.515, 15.829, 8.964, 1.964, 188.964, 195.829, 202.515, 209.923] # angles of the sensorzone centers in degrees
+# Dictionary structure for sensors
+sensor_data = {
+    "sensor0": [[5], [-1]],     # the second value determines if the sensor looks down (-1) or up (1) in 60° steps
+    "sensor1": [[0], [-1]],     # the first value determines how far the points rotate around the z-axis in 60° steps
+    "sensor2": [[1], [-1]], 
+    "sensor3": [[5], [0]],
+    "sensor4": [[0], [0]],
+    "sensor5": [[1], [0]],
+    "sensor6": [[5], [1]],
+    "sensor7": [[0], [1]],
+    "sensor8": [[1], [1]],
+    "sensor9": [[2], [-1]],
+    "sensor10": [[3], [-1]],
+    "sensor11": [[4], [-1]],
+    "sensor12": [[2], [0]],
+    "sensor13": [[3], [0]],
+    "sensor14": [[4], [0]],
+    "sensor15": [[2], [1]],
+    "sensor16": [[3], [1]],
+    "sensor17": [[4], [1]],
+}
 points = np.empty((0, 0))
-# Define the rotation angle in radians
-theta = np.pi / 4  # 45 degrees
-# Define the rotation matrix around the z-axis
-rotation_matrix = np.array([
-    [np.cos(theta), -np.sin(theta), 0],
-    [np.sin(theta), np.cos(theta),  0],
-    [0,             0,              1]
-])
+
+# Define a translation vector to move points 85 mm in the x direction
+translation_vector = np.array([85, 0, 0])  # 85 mm = 0.085 meters
 
 class SerialListPublisher(Node):
-    def __init__(self, serial_port='/dev/ttyACM0', baudrate=115200):                                                # the Serialport or baudrate may have to be edited when changing host pc
+    def __init__(self, serial_port='/dev/ttyACM0', baudrate=1000000):                                                # the Serialport or baudrate may have to be edited when changing host pc
         super().__init__('serial_list_publisher')
         self.pcd_publisher = self.create_publisher(sensor_msgs.PointCloud2, 'pcd', 10)
         self.serial_port = serial.Serial(serial_port, baudrate, timeout=None)
@@ -28,12 +44,18 @@ class SerialListPublisher(Node):
         try:
             while rclpy.ok():
                 line = self.serial_port.readline().decode('utf-8').strip()                                          # Read a line from the serial port
+                try:
                     data = json.loads(line)                                                                          # Interpret arrays from JSON String
+                except json.JSONDecodeError as e:                                                                    # If the JSON String is not valid, we log the error and skip this line
+                    self.get_logger().error(f"JSON decode error: {e} | Line: {line}")
+                    continue
+                if not isinstance(data, dict):                                                                       # Check if the parsed JSON is a dictionary
+                    self.get_logger().error(f"Unexpected JSON format: {data}")
+                    continue
+
                 points_all=[]                                                                                       # Create an empty List for the Arrays of points from the Sensors
-                count = 0                                                                                           # Set the Counter to Zero. The Counter is needed to rotate the sensor Points in the self.create_plc_from_distance function
                 for key, array in data.items():                                                                     # a for-Loop that goes through the arrays of data from each sensor
-                    points_all.append(self.create_pcd_from_distance(np.array(array).reshape(8,8), count*45))        # in this line we create an 8x8 np.array from the sensor data list, then we feed it into the the create_plc_from_distance() with the angle we want the points to be rotated around the z-axis  
-                    count = count + 1                                                                               # the counter counts one up
+                    points_all.append(self.create_pcd_from_distance(np.array(array).reshape(8,8), key))        # in this line we create an 8x8 np.array from the sensor data list, then we feed it into the the create_plc_from_distance() with the angle we want the points to be rotated around the z-axis  
                 self.points = np.vstack(points_all)                                                                 # stacking the sensor 64x3 arrays to one (count*64)x3 vertically
                 self.pcd = self.point_cloud(self.points, 'vl53l7cx_link')                                                     # converting the np.array to pointcloud data with the parent_frame. The parent_frame has to be edited in the future to suit the use-case
                 self.pcd_publisher.publish(self.pcd)                                                                # publishing the pointcloud2 topic
@@ -45,24 +67,14 @@ class SerialListPublisher(Node):
         finally:
             self.serial_port.close()
             
-    def create_pcd_from_distance(self, distance, rotation_angle):
-        # Define the rotation angle in radians
-        theta = np.radians(rotation_angle) # 45 degrees
-        # Define the rotation matrix around the z-axis
-        rotation_matrix = np.array([
-            [np.cos(theta), -np.sin(theta), 0],
-            [np.sin(theta), np.cos(theta),  0],
-            [0,             0,              1]
-        ])
-        rotation_matrix_90_y = np.array([
-            [0, 0, 1],
-            [0, 1, 0],
-            [-1, 0, 0]
-        ])
+    def create_pcd_from_distance(self, distance, sensor_key):
+
         #declaring the lists for the koordinates components
         x_collum = []
         z_collum = []
         y_collum = []
+
+        # Create a 3D point cloud from the distance data
         for j in range(len(distance[:][0])):
             for i in range(len(distance[0])):
                 x = (distance[i][j])
@@ -71,14 +83,48 @@ class SerialListPublisher(Node):
                 # Calculate x and y using trigonometry
                 z = x * np.sin(np.radians(angles[j]))
                 y = x * np.sin(np.radians(angles[i]))
-
+                # Append the calculated coordinates to the lists
                 z_collum.append(z)
                 y_collum.append(y)
+                     
+        # Convert the lists to numpy arrays and reshape them
         z_collum = np.array(z_collum).reshape(64,1).astype(int)                                                                 # Convert list to numpy array
         y_collum = np.array(y_collum).reshape(64,1).astype(int)                                                                 # Convert list to numpy array
         x_collum = np.array(x_collum).reshape(64,1).astype(int)                                                                 # Convert list to numpy array
-        self.points = np.dot(np.hstack((x_collum, y_collum, z_collum ))/1000, rotation_matrix.T)                                # Creating a 64x3 array out of the z_collum, y_collum and x_collum
-        self.points = np.dot(self.points, rotation_matrix_90_y.T)
+        
+        # Apply the translation vector to the points
+        x_collum = x_collum + translation_vector[0]                                                                          # Adding the translation vector to the x_collum
+        y_collum = y_collum + translation_vector[1]                                                                          # Adding the translation vector to the y_collum
+        z_collum = z_collum + translation_vector[2]                                                                          # Adding the translation vector to the z_collum
+        
+        # Apply the rotation matrix to the points
+        # Retrieve the rotation step from the sensor_data dictionary
+        rotation_angle_y = sensor_data[sensor_key][0][0] * np.radians(60) 
+        
+        # Define the rotation matrix for the calculated angle around the y-axis
+        rotation_matrix_dynamic_y = np.array([
+            [np.cos(rotation_angle_y), 0, np.sin(rotation_angle_y)],
+            [0, 1, 0],
+            [-np.sin(rotation_angle_y), 0, np.cos(rotation_angle_y)]
+        ])
+        
+        # Apply the dynamic rotation matrix around the y-axis
+        self.points = np.dot(np.hstack((x_collum, y_collum, z_collum)) / 1000, rotation_matrix_dynamic_y.T)
+
+                # Define the rotation angle in radians based on the second column from the sensor_data dictionary
+        theta = sensor_data[sensor_key][1][0] * np.radians(60)  # Multiply by 60 degrees step
+
+        self.get_logger().info(f"Sensor key: {sensor_key}, Rotation angle: {np.degrees(theta)} degrees")
+
+        # Define the rotation matrix around the z-axis
+        rotation_matrix = np.array([
+            [np.cos(theta), -np.sin(theta), 0],
+            [np.sin(theta), np.cos(theta),  0],
+            [0,             0,              1]
+        ])
+
+        # Apply the rotation matrix around the z-axis
+        self.points = np.dot(self.points, rotation_matrix.T)                                # Creating a 64x3 array out of the z_collum, y_collum and x_collum
         return self.points
 
     def point_cloud(self, points, parent_frame):