diff --git a/workspaces/COLCON_WS/src/ser_test/package.xml b/workspaces/COLCON_WS/src/ser_test/package.xml index e69de29bb2d1d6434b8b29ae775ad8c2e48c5391..b7ca9374fc0ac32930a3cfa10b1bdf6b5e70a32f 100644 --- a/workspaces/COLCON_WS/src/ser_test/package.xml +++ b/workspaces/COLCON_WS/src/ser_test/package.xml @@ -0,0 +1,25 @@ +<?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 diff --git a/workspaces/COLCON_WS/src/ser_test/ser_test/ser_test_node.py b/workspaces/COLCON_WS/src/ser_test/ser_test/ser_test_node.py index b79f6fe835b3157f901452c74523d9b41ef09815..fd3b7d7d21d5cb3e00716102818c361745fe33f3 100644 --- a/workspaces/COLCON_WS/src/ser_test/ser_test/ser_test_node.py +++ b/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): """ diff --git a/workspaces/COLCON_WS/src/ser_test/setup.py b/workspaces/COLCON_WS/src/ser_test/setup.py index d6bc12c1559a8beb472c380cee21aa2f36c37fc8..f60ad645fde3d50ff777fa64686b3646da55bcd7 100644 --- a/workspaces/COLCON_WS/src/ser_test/setup.py +++ b/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' ], }, ) diff --git a/workspaces/COLCON_WS/src/ur_description/config/ur10/default_kinematics.yaml b/workspaces/COLCON_WS/src/ur_description/config/ur10/default_kinematics.yaml index 04a95027395db47650f06b6072d8ea97f1d233b7..3177f10391cae11d5ffc4bdf7d986abd5083de46 100644 --- a/workspaces/COLCON_WS/src/ur_description/config/ur10/default_kinematics.yaml +++ b/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 diff --git a/workspaces/rob-sensor_ws/src/Universal_Robots_ROS2_Description b/workspaces/rob-sensor_ws/src/Universal_Robots_ROS2_Description index 91f51a89be2f488afc4d25f03b2bd75f47399608..8a8808325a038f3cd4f452fa4eabfd4974e73067 160000 --- a/workspaces/rob-sensor_ws/src/Universal_Robots_ROS2_Description +++ b/workspaces/rob-sensor_ws/src/Universal_Robots_ROS2_Description @@ -1 +1 @@ -Subproject commit 91f51a89be2f488afc4d25f03b2bd75f47399608 +Subproject commit 8a8808325a038f3cd4f452fa4eabfd4974e73067