From fd827a67fe1a6662b9d44d6630ad9ef043fd014b Mon Sep 17 00:00:00 2001
From: Rene Ebeling <rene.ebeling@alumni.fh-aachen.de>
Date: Fri, 23 May 2025 20:25:36 +0200
Subject: [PATCH] Enhance point cloud processing by adding timestamp handling
in serial_to_pcl_node and pcl_filter_node; update sensor initialization array
size in V1_6_Sensoren_mit_LPn_Control_Vincent.ino; include new bibliography
entry for DIN EN ISO 13855.
---
.../V1_6_Sensoren_mit_LPn_Control_Vincent.ino | 2 +-
Bachelorarbeit/Exported Items.bib | 10 ++++++++++
.../serial_to_pcl/moveit_stop_node.py | 5 +++++
.../serial_to_pcl/pcl_filter_node.py | 2 ++
.../serial_to_pcl/serial_to_pcl_node.py | 18 ++----------------
5 files changed, 20 insertions(+), 17 deletions(-)
create mode 100644 Bachelorarbeit/Exported Items.bib
diff --git a/Arduino/V1_6_Sensoren_mit_LPn_Control_Vincent/V1_6_Sensoren_mit_LPn_Control_Vincent.ino b/Arduino/V1_6_Sensoren_mit_LPn_Control_Vincent/V1_6_Sensoren_mit_LPn_Control_Vincent.ino
index 51f87f4..d429797 100644
--- a/Arduino/V1_6_Sensoren_mit_LPn_Control_Vincent/V1_6_Sensoren_mit_LPn_Control_Vincent.ino
+++ b/Arduino/V1_6_Sensoren_mit_LPn_Control_Vincent/V1_6_Sensoren_mit_LPn_Control_Vincent.ino
@@ -78,7 +78,7 @@ pin_size_t SDA_PIN0 = 20;
pin_size_t SCL_PIN0 = 21;
int sensor_count = 0;
-bool sensorInitialized[8] = { false }; // global oder static
+bool sensorInitialized[28] = { false }; // global oder static
JsonDocument doc;
diff --git a/Bachelorarbeit/Exported Items.bib b/Bachelorarbeit/Exported Items.bib
new file mode 100644
index 0000000..14f3ddb
--- /dev/null
+++ b/Bachelorarbeit/Exported Items.bib
@@ -0,0 +1,10 @@
+
+@misc{noauthor_din_nodate,
+ address = {Berlin},
+ title = {{DIN} {EN} {ISO} 13855:2022-04, {Sicherheit} von {Maschinen}\_- {Anordnung} von {Schutzeinrichtungen} im {Hinblick} auf {Annäherung} des menschlichen {Körpers} ({ISO}/{DIS}\_13855:2022); {Deutsche} und {Englische} {Fassung} {prEN}\_ISO\_13855:2022},
+ shorttitle = {{DIN} {EN} {ISO} 13855},
+ url = {https://www.dinmedia.de/de/-/-/349862066},
+ doi = {10.31030/3329034},
+ urldate = {2025-05-23},
+ publisher = {DIN Media GmbH},
+}
diff --git a/workspaces/COLCON_WS/src/serial_to_pcl/serial_to_pcl/moveit_stop_node.py b/workspaces/COLCON_WS/src/serial_to_pcl/serial_to_pcl/moveit_stop_node.py
index f8c2f85..e89a7d5 100644
--- a/workspaces/COLCON_WS/src/serial_to_pcl/serial_to_pcl/moveit_stop_node.py
+++ b/workspaces/COLCON_WS/src/serial_to_pcl/serial_to_pcl/moveit_stop_node.py
@@ -70,6 +70,7 @@ class PointCloudProcessor(Node):
def pointcloud_callback(self, msg):
self.perspective_frame = 'vl53l7cx_link'
+ self.stamp = msg.header.stamp
try:
now = rclpy.time.Time()
transforms = {
@@ -111,6 +112,9 @@ class PointCloudProcessor(Node):
def stop_trajectory(self):
self.get_logger().info("Stopping trajectory execution due to point cloud threshold.")
+ ros_time = self.get_clock().now().to_msg()
+ time_diff = (ros_time.sec + ros_time.nanosec * 1e-9) - (self.stamp.sec + self.stamp.nanosec * 1e-9)
+ self.get_logger().info(f"Time difference between ROS time and pointcloud stamp: {time_diff:.6f} seconds")
cancel_string = String()
cancel_string.data = "stop"
self.cancellation_pub.publish(cancel_string)
@@ -129,6 +133,7 @@ class PointCloudProcessor(Node):
for i, n in enumerate('xyz')] # Define the fields for x, y, and z
header = Header(frame_id=parent_frame) # Create a header with the parent frame
+ header.stamp = self.stamp # Set the timestamp
return PointCloud2(
header=header,
height=1, # Single row of points
diff --git a/workspaces/COLCON_WS/src/serial_to_pcl/serial_to_pcl/pcl_filter_node.py b/workspaces/COLCON_WS/src/serial_to_pcl/serial_to_pcl/pcl_filter_node.py
index 0a568b4..9740d91 100644
--- a/workspaces/COLCON_WS/src/serial_to_pcl/serial_to_pcl/pcl_filter_node.py
+++ b/workspaces/COLCON_WS/src/serial_to_pcl/serial_to_pcl/pcl_filter_node.py
@@ -65,6 +65,7 @@ class PointCloudProcessor(Node):
def pointcloud_callback(self, msg):
self.perspective_frame = 'vl53l7cx_link'
+ self.stamp = msg.header.stamp
try:
now = rclpy.time.Time()
transforms = {
@@ -118,6 +119,7 @@ class PointCloudProcessor(Node):
data = points.astype(dtype).tobytes()
fields = [sensor_msgs.PointField(name=n, offset=i * itemsize, datatype=ros_dtype, count=1) for i, n in enumerate('xyz')]
header = Header(frame_id=parent_frame)
+ header.stamp = self.stamp
return sensor_msgs.PointCloud2(
header=header,
diff --git a/workspaces/COLCON_WS/src/serial_to_pcl/serial_to_pcl/serial_to_pcl_node.py b/workspaces/COLCON_WS/src/serial_to_pcl/serial_to_pcl/serial_to_pcl_node.py
index c90f25e..40aa73f 100644
--- a/workspaces/COLCON_WS/src/serial_to_pcl/serial_to_pcl/serial_to_pcl_node.py
+++ b/workspaces/COLCON_WS/src/serial_to_pcl/serial_to_pcl/serial_to_pcl_node.py
@@ -49,6 +49,7 @@ class SerialListPublisher(Node):
"""
try:
while rclpy.ok(): # Keep running while ROS 2 is active
+ self.stamp = self.get_clock().now().to_msg() # Get the current timestamp
line = self.serial_port.readline().decode('utf-8').strip() # Read a line from the serial port
data = self.parse_json(line) # Parse the JSON data
if not data: # Skip if the data is invalid
@@ -229,6 +230,7 @@ class SerialListPublisher(Node):
for i, n in enumerate('xyz')] # Define the fields for x, y, and z
header = Header(frame_id=parent_frame) # Create a header with the parent frame
+ header.stamp = self.stamp
return PointCloud2(
header=header,
height=1, # Single row of points
@@ -240,22 +242,6 @@ class SerialListPublisher(Node):
row_step=(itemsize * 3 * points.shape[0]), # Size of each row in bytes
data=data # Binary data for the points
)
- def create_empty_point_cloud(self):
- """
- Create an empty PointCloud2 message.
- """
- header = Header(frame_id=self.parent_frame)
- return PointCloud2(
- header=header,
- height=1,
- width=0,
- is_dense=True,
- is_bigendian=False,
- fields=[],
- point_step=0,
- row_step=0,
- data=b''
- )
def main(args=None):
"""
--
GitLab