Skip to content
Snippets Groups Projects
Commit 250e9d5d authored by René Ebeling's avatar René Ebeling
Browse files

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

parent ecb0ffb3
Branches
No related tags found
No related merge requests found
<?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
...@@ -20,7 +20,7 @@ SENSOR_DATA = { ...@@ -20,7 +20,7 @@ SENSOR_DATA = {
} }
# Translation vector for the sensor in meters # 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): class SerialListPublisher(Node):
...@@ -60,15 +60,17 @@ class SerialListPublisher(Node): ...@@ -60,15 +60,17 @@ class SerialListPublisher(Node):
filtered_array = [] filtered_array = []
for pair in array: for pair in array:
if pair[1] != 5: if pair[1] != 5:
filtered_array.append([99999, pair[1]]) filtered_array.append([99999])
else: else:
filtered_array.append(pair) filtered_array.append([pair[0]])
filtered_data[key] = filtered_array filtered_data[key] = filtered_array
# Process the data for each sensor and create a point cloud # Process the data for each sensor and create a point cloud
points_all = [self.create_pcd_from_distance( points_all = []
np.array([pair[0] for pair in array]).reshape(8, 8), key) for key, array in filtered_data.items():
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 = None
points = np.vstack(points_all) # Combine all points into a single array 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 pcd = self.create_point_cloud(points, self.parent_frame) # Create a PointCloud2 message
...@@ -105,7 +107,7 @@ class SerialListPublisher(Node): ...@@ -105,7 +107,7 @@ class SerialListPublisher(Node):
# Get the yaw and pitch angles for the sensor # Get the yaw and pitch angles for the sensor
yaw_deg, pitch_deg = self.get_sensor_angles(sensor_key) 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 return points_local
def calculate_coordinates(self, distance): def calculate_coordinates(self, distance):
...@@ -129,7 +131,7 @@ class SerialListPublisher(Node): ...@@ -129,7 +131,7 @@ class SerialListPublisher(Node):
pitch_deg = SENSOR_DATA[sensor_key][0][0] * 60 # Calculate pitch angle pitch_deg = SENSOR_DATA[sensor_key][0][0] * 60 # Calculate pitch angle
return yaw_deg, pitch_deg 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. Apply rotation and translation transformations to the points.
""" """
...@@ -146,7 +148,7 @@ class SerialListPublisher(Node): ...@@ -146,7 +148,7 @@ class SerialListPublisher(Node):
points = (Rz @ points.T).T points = (Rz @ points.T).T
# Calculate the translation vector # 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 return points + translation # Apply the translation
@staticmethod @staticmethod
...@@ -182,14 +184,28 @@ class SerialListPublisher(Node): ...@@ -182,14 +184,28 @@ class SerialListPublisher(Node):
[0, 0, 1] [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 if SENSOR_DATA[sensor_key][1][0] == 0: # Check if the second list contains a 0
ty = TRANSLATION_VECTOR[0] * np.sin(yaw_rad) * np.cos(pitch_rad) +0.085# y-component of translation tx = TRANSLATION_VECTOR[0] * np.cos(pitch_rad) # x-component of translation
tz = TRANSLATION_VECTOR[0] * np.sin(pitch_rad) +0.085 # z-component of translation ty = TRANSLATION_VECTOR[0] * np.sin(pitch_rad) # y-component of translation
return np.array([tx, ty, tz]) + TRANSLATION_VECTOR 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): def distance_filter(self, points, threshold):
""" """
......
...@@ -23,7 +23,6 @@ setup( ...@@ -23,7 +23,6 @@ setup(
'ser_test_node = ser_test.ser_test_node:main', 'ser_test_node = ser_test.ser_test_node:main',
'pcl_rob_node = ser_test.pcl_rob_node:main', 'pcl_rob_node = ser_test.pcl_rob_node:main',
'pcl_rob_v2_node = ser_test.pcl_rob_v2_node:main', 'pcl_rob_v2_node = ser_test.pcl_rob_v2_node:main',
'random_pointcloud_publisher_node = ser_test.random_pointcloud_publisher:main'
], ],
}, },
) )
...@@ -23,7 +23,7 @@ kinematics: ...@@ -23,7 +23,7 @@ kinematics:
sensor: sensor:
x: -0.30635 x: -0.30635
y: 0.0 y: 0.0
z: 0.1 z: 0.05
roll: 1.570796327 roll: 1.570796327
pitch: 3.141592653589793 pitch: 3.141592653589793
yaw: -1.570796327 yaw: -1.570796327
......
Universal_Robots_ROS2_Description @ 8a880832
Subproject commit 91f51a89be2f488afc4d25f03b2bd75f47399608 Subproject commit 8a8808325a038f3cd4f452fa4eabfd4974e73067
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment