From 9e8e8cd42c2866dba31006aa7434e78989d3cefd Mon Sep 17 00:00:00 2001 From: Thomas Reidemeister Date: Thu, 29 Jan 2026 14:44:56 -0500 Subject: [PATCH 1/8] Debugging --- vmd3_radar_driver/vmd3.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/vmd3_radar_driver/vmd3.py b/vmd3_radar_driver/vmd3.py index 7b53fa2..6830d98 100644 --- a/vmd3_radar_driver/vmd3.py +++ b/vmd3_radar_driver/vmd3.py @@ -139,6 +139,7 @@ def __init__(self, tcp_ip: str = '192.168.100.201', tcp_port: int = 6172, udp_po self._sock_udp = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) # UDP local_ip = self._sock_tcp.getsockname()[0] self._sock_udp.bind((local_ip, udp_port)) + self._sock_udp.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) self._sock_udp.setsockopt(socket.SOL_SOCKET, SO_TIMESTAMP, 1) # Initialize connection with sensor @@ -157,6 +158,7 @@ def _udp_receiver(self): packet, ancdata, flags, addr = self._sock_udp.recvmsg(packet_length, 1024) packet_timestamp = time.time() # Fallback timestamp for cmsg_level, cmsg_type, cmsg_data in ancdata: + print("cmsg_level:", cmsg_level, "cmsg_type:", cmsg_type) if cmsg_level == socket.SOL_SOCKET and cmsg_type == SO_TIMESTAMP: tv_sec, tv_usec = struct.unpack('ll', cmsg_data) packet_timestamp = tv_sec + tv_usec / 1e6 # Packet receive timestamp From e21d717be1010b342cb1e82aa34cd498cebf8ec7 Mon Sep 17 00:00:00 2001 From: Thomas Reidemeister Date: Thu, 29 Jan 2026 14:47:46 -0500 Subject: [PATCH 2/8] Debugging --- vmd3_radar_driver/vmd3.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/vmd3_radar_driver/vmd3.py b/vmd3_radar_driver/vmd3.py index 6830d98..b24a6cc 100644 --- a/vmd3_radar_driver/vmd3.py +++ b/vmd3_radar_driver/vmd3.py @@ -158,7 +158,7 @@ def _udp_receiver(self): packet, ancdata, flags, addr = self._sock_udp.recvmsg(packet_length, 1024) packet_timestamp = time.time() # Fallback timestamp for cmsg_level, cmsg_type, cmsg_data in ancdata: - print("cmsg_level:", cmsg_level, "cmsg_type:", cmsg_type) + print("cmsg_level:", cmsg_level, cmsg_level==socket.SOL_SOCKET, "cmsg_type:", cmsg_type, cmsg_type==SO_TIMESTAMP) if cmsg_level == socket.SOL_SOCKET and cmsg_type == SO_TIMESTAMP: tv_sec, tv_usec = struct.unpack('ll', cmsg_data) packet_timestamp = tv_sec + tv_usec / 1e6 # Packet receive timestamp From 121f21b2c46d9be90873b87033c8311bdc88148c Mon Sep 17 00:00:00 2001 From: Thomas Reidemeister Date: Thu, 29 Jan 2026 14:49:04 -0500 Subject: [PATCH 3/8] Debugging --- vmd3_radar_driver/vmd3.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/vmd3_radar_driver/vmd3.py b/vmd3_radar_driver/vmd3.py index b24a6cc..c6be5bb 100644 --- a/vmd3_radar_driver/vmd3.py +++ b/vmd3_radar_driver/vmd3.py @@ -167,6 +167,8 @@ def _udp_receiver(self): self._udp_queue.put((packet, packet_timestamp)) except Exception: + import traceback + traceback.print_exc() continue def check_send(self, command: bytes): From 652561acffdf49eb78d0d36f2ff35c286edc3347 Mon Sep 17 00:00:00 2001 From: Thomas Reidemeister Date: Thu, 29 Jan 2026 14:57:18 -0500 Subject: [PATCH 4/8] Debugging --- vmd3_radar_driver/vmd3.py | 1 + 1 file changed, 1 insertion(+) diff --git a/vmd3_radar_driver/vmd3.py b/vmd3_radar_driver/vmd3.py index c6be5bb..785aeb0 100644 --- a/vmd3_radar_driver/vmd3.py +++ b/vmd3_radar_driver/vmd3.py @@ -26,6 +26,7 @@ import math import enum import time +import struct from collections import namedtuple import threading import queue From 18cce0a0fc7ef0ed840d5475a0f4b57363bf2673 Mon Sep 17 00:00:00 2001 From: Thomas Reidemeister Date: Thu, 29 Jan 2026 14:59:29 -0500 Subject: [PATCH 5/8] Debugging --- vmd3_radar_driver/vmd3.py | 3 +++ 1 file changed, 3 insertions(+) diff --git a/vmd3_radar_driver/vmd3.py b/vmd3_radar_driver/vmd3.py index 785aeb0..9e99a2f 100644 --- a/vmd3_radar_driver/vmd3.py +++ b/vmd3_radar_driver/vmd3.py @@ -162,8 +162,11 @@ def _udp_receiver(self): print("cmsg_level:", cmsg_level, cmsg_level==socket.SOL_SOCKET, "cmsg_type:", cmsg_type, cmsg_type==SO_TIMESTAMP) if cmsg_level == socket.SOL_SOCKET and cmsg_type == SO_TIMESTAMP: tv_sec, tv_usec = struct.unpack('ll', cmsg_data) + old_time = packet_timestamp + print("System time", old_time) packet_timestamp = tv_sec + tv_usec / 1e6 # Packet receive timestamp print("PKT Timestamp:", packet_timestamp) + print("Delta:", old_time - packet_timestamp) #packet, _ = self._sock_udp.recvfrom(packet_length) self._udp_queue.put((packet, packet_timestamp)) From 461d89ae8c984c35b2f0b62e98152afc8db2df42 Mon Sep 17 00:00:00 2001 From: Thomas Reidemeister Date: Thu, 29 Jan 2026 15:01:50 -0500 Subject: [PATCH 6/8] Debugging --- vmd3_radar_driver/vmd3.py | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) diff --git a/vmd3_radar_driver/vmd3.py b/vmd3_radar_driver/vmd3.py index 9e99a2f..53f6202 100644 --- a/vmd3_radar_driver/vmd3.py +++ b/vmd3_radar_driver/vmd3.py @@ -156,18 +156,14 @@ def _udp_receiver(self): packet_length = 1500 while self._receiver_running: try: + #packet, _ = self._sock_udp.recvfrom(packet_length) packet, ancdata, flags, addr = self._sock_udp.recvmsg(packet_length, 1024) packet_timestamp = time.time() # Fallback timestamp for cmsg_level, cmsg_type, cmsg_data in ancdata: print("cmsg_level:", cmsg_level, cmsg_level==socket.SOL_SOCKET, "cmsg_type:", cmsg_type, cmsg_type==SO_TIMESTAMP) if cmsg_level == socket.SOL_SOCKET and cmsg_type == SO_TIMESTAMP: tv_sec, tv_usec = struct.unpack('ll', cmsg_data) - old_time = packet_timestamp - print("System time", old_time) packet_timestamp = tv_sec + tv_usec / 1e6 # Packet receive timestamp - print("PKT Timestamp:", packet_timestamp) - print("Delta:", old_time - packet_timestamp) - #packet, _ = self._sock_udp.recvfrom(packet_length) self._udp_queue.put((packet, packet_timestamp)) except Exception: From 1ded040f43533e5b1c76bf8b18dfb889ae3e8bdc Mon Sep 17 00:00:00 2001 From: Thomas Reidemeister Date: Thu, 29 Jan 2026 15:15:37 -0500 Subject: [PATCH 7/8] Debugging --- vmd3_radar_driver/driver.py | 1 + 1 file changed, 1 insertion(+) diff --git a/vmd3_radar_driver/driver.py b/vmd3_radar_driver/driver.py index 7db36bd..8126ee0 100644 --- a/vmd3_radar_driver/driver.py +++ b/vmd3_radar_driver/driver.py @@ -203,6 +203,7 @@ def destroy_node(self): """Clean up the node and stop polling.""" self.get_logger().info("Destroying VMD3RadarNode") self._running = False + self._thread.join() super().destroy_node() From 58c67d1d6ee596e34d7d4aa0a9248af44a15a453 Mon Sep 17 00:00:00 2001 From: Thomas Reidemeister Date: Thu, 29 Jan 2026 15:16:24 -0500 Subject: [PATCH 8/8] Debugging --- vmd3_radar_driver/vmd3.py | 1 - 1 file changed, 1 deletion(-) diff --git a/vmd3_radar_driver/vmd3.py b/vmd3_radar_driver/vmd3.py index 53f6202..a13a603 100644 --- a/vmd3_radar_driver/vmd3.py +++ b/vmd3_radar_driver/vmd3.py @@ -160,7 +160,6 @@ def _udp_receiver(self): packet, ancdata, flags, addr = self._sock_udp.recvmsg(packet_length, 1024) packet_timestamp = time.time() # Fallback timestamp for cmsg_level, cmsg_type, cmsg_data in ancdata: - print("cmsg_level:", cmsg_level, cmsg_level==socket.SOL_SOCKET, "cmsg_type:", cmsg_type, cmsg_type==SO_TIMESTAMP) if cmsg_level == socket.SOL_SOCKET and cmsg_type == SO_TIMESTAMP: tv_sec, tv_usec = struct.unpack('ll', cmsg_data) packet_timestamp = tv_sec + tv_usec / 1e6 # Packet receive timestamp