Update package.xml and ser_test_node.py for improved sensor handling and dependencies

workspaces/COLCON_WS/src/ser_test/package.xml

+<?xml version="1.0"?>
+<package format="2">
+    <name>ser_test</name>
+    <version>0.0.1</version>
+    <description>ROS 2 package for serial communication testing</description>
+
+    <maintainer email="rene.ebeling@alumni.fh-aachen.de">Rene Ebeling</maintainer>
+    <license>Apache-2.0</license>
+
+    <buildtool_depend>ament_python</buildtool_depend>
+
+    <exec_depend>rclpy</exec_depend>
+    <exec_depend>std_msgs</exec_depend>
+    <exec_depend>sensor_msgs</exec_depend>
+    <exec_depend>numpy</exec_depend>
+    <exec_depend>pyserial</exec_depend>
+    <exec_depend>json</exec_depend>
+
+    <test_depend>ament_lint_auto</test_depend>
+    <test_depend>ament_lint_common</test_depend>
+
+    <export>
+        <build_type>ament_python</build_type>
+    </export>
+</package>
\ No newline at end of file

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

@@ -20,7 +20,7 @@ SENSOR_DATA = {
 }
 
 # Translation vector for the sensor in meters
-TRANSLATION_VECTOR = np.array([0.085, 0, 0])  # 85 mm in meters
+TRANSLATION_VECTOR = np.array([0.085, 0.06, 0])  # 85 mm in meters
 
 
 class SerialListPublisher(Node):
@@ -60,15 +60,17 @@ class SerialListPublisher(Node):
                     filtered_array = []
                     for pair in array:
                         if pair[1] != 5:
-                            filtered_array.append([99999, pair[1]])
+                            filtered_array.append([99999])
                         else:
-                            filtered_array.append(pair)
+                            filtered_array.append([pair[0]])
                     filtered_data[key] = filtered_array
-
                 # Process the data for each sensor and create a point cloud
-                points_all = [self.create_pcd_from_distance(
-                              np.array([pair[0] for pair in array]).reshape(8, 8), key)
-                              for key, array in filtered_data.items()]
+                points_all = []
+                for key, array in filtered_data.items():
+                    distances = [pair[0] for pair in array if len(pair) > 0]
+                    if len(distances) == 64:  # Ensure the array has exactly 64 elements
+                        reshaped_array = np.array(distances).reshape(8, 8)
+                        points_all.append(self.create_pcd_from_distance(reshaped_array, key))
                 points = None
                 points = np.vstack(points_all)  # Combine all points into a single array
                 pcd = self.create_point_cloud(points, self.parent_frame)  # Create a PointCloud2 message
@@ -105,7 +107,7 @@ class SerialListPublisher(Node):
 
         # Get the yaw and pitch angles for the sensor
         yaw_deg, pitch_deg = self.get_sensor_angles(sensor_key)
-        points_local = self.apply_transformations(points_local, yaw_deg, pitch_deg)  # Apply transformations
+        points_local = self.apply_transformations(points_local, yaw_deg, pitch_deg, sensor_key)  # Apply transformations
         return points_local
 
     def calculate_coordinates(self, distance):
@@ -129,7 +131,7 @@ class SerialListPublisher(Node):
         pitch_deg = SENSOR_DATA[sensor_key][0][0] * 60  # Calculate pitch angle
         return yaw_deg, pitch_deg
 
-    def apply_transformations(self, points, yaw_deg, pitch_deg):
+    def apply_transformations(self, points, yaw_deg, pitch_deg, sensor_key):
         """
         Apply rotation and translation transformations to the points.
         """
@@ -146,7 +148,7 @@ class SerialListPublisher(Node):
         points = (Rz @ points.T).T
 
         # Calculate the translation vector
-        translation = self.calculate_translation(yaw_rad, pitch_rad)
+        translation = self.calculate_translation(yaw_rad, pitch_rad, sensor_key)
         return points + translation  # Apply the translation
 
     @staticmethod
@@ -182,14 +184,28 @@ class SerialListPublisher(Node):
             [0, 0, 1]
         ])
 
-    def calculate_translation(self, yaw_rad, pitch_rad):
+    def calculate_translation(self, yaw_rad, pitch_rad, sensor_key):
         """
-        Calculate the translation vector based on yaw and pitch angles.
+        Calculate the translation vector based on yaw and pitch angles for specific sensors.
+        Only sensors with a 0 in the second list from SENSOR_DATA are moved.
         """
-        tx = TRANSLATION_VECTOR[0] * np.cos(yaw_rad) * np.cos(pitch_rad) -0.085  # x-component of translation
-        ty = TRANSLATION_VECTOR[0] * np.sin(yaw_rad) * np.cos(pitch_rad) +0.085# y-component of translation
-        tz = TRANSLATION_VECTOR[0] * np.sin(pitch_rad) +0.085 # z-component of translation
-        return np.array([tx, ty, tz]) + TRANSLATION_VECTOR
+        if SENSOR_DATA[sensor_key][1][0] == 0:  # Check if the second list contains a 0
+            tx = TRANSLATION_VECTOR[0] * np.cos(pitch_rad) # x-component of translation
+            ty = TRANSLATION_VECTOR[0] * np.sin(pitch_rad)  # y-component of translation
+            tz = 0
+            return np.array([tx, ty, tz])  # Return the translation vector
+        if SENSOR_DATA[sensor_key][1][0] == 1:
+            tx = TRANSLATION_VECTOR[1] * np.cos(pitch_rad)
+            ty = TRANSLATION_VECTOR[1] * np.sin(pitch_rad)
+            tz = -(TRANSLATION_VECTOR[1] * np.cos(yaw_rad))  # z-component of translation
+            return np.array([tx, ty, tz])
+        if SENSOR_DATA[sensor_key][1][0] == -1:
+            tx = TRANSLATION_VECTOR[1] * np.cos(pitch_rad)
+            ty = TRANSLATION_VECTOR[1] * np.sin(pitch_rad)
+            tz = (TRANSLATION_VECTOR[1] * np.cos(yaw_rad))
+            return np.array([tx, ty, tz])
+        else:
+            return np.array([0, 0, 0])  # No translation for other sensors
 
     def distance_filter(self, points, threshold):
         """

workspaces/COLCON_WS/src/ser_test/setup.py

@@ -23,7 +23,6 @@ setup(
             'ser_test_node = ser_test.ser_test_node:main',
             'pcl_rob_node = ser_test.pcl_rob_node:main',
             'pcl_rob_v2_node = ser_test.pcl_rob_v2_node:main',
-            'random_pointcloud_publisher_node = ser_test.random_pointcloud_publisher:main'
         ],
     },
 )

workspaces/COLCON_WS/src/ur_description/config/ur10/default_kinematics.yaml

@@ -23,7 +23,7 @@ kinematics:
   sensor:
     x: -0.30635
     y: 0.0
-    z: 0.1
+    z: 0.05
     roll: 1.570796327
     pitch: 3.141592653589793
     yaw: -1.570796327

Universal_Robots_ROS2_Description @ 8a880832

-Subproject commit 91f51a89be2f488afc4d25f03b2bd75f47399608
+Subproject commit 8a8808325a038f3cd4f452fa4eabfd4974e73067