import socket
import threading
import time
import msgpack
# -----------------------------------------------------------------
# Core Infrastructure Setup & Reliable Class
# -----------------------------------------------------------------
udp_connections = {}
connection_counter = 0
connection_lock = threading.Lock()
incoming_payloads = {}
PACKET_MSG = 1
PACKET_ACK = 2
class ReliableMsgPackConnection:
"""Thread-safe Reliable MessagePack UDP wrapper for Linux Side"""
def __init__(self, connection_id, host, port):
self.connection_id = connection_id
self.host = host
self.port = port
self.socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
self.socket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
self.socket.bind((host, port))
self.sequence_counter = 0
self.running = True
self.ack_event = threading.Event()
self.expected_ack_id = None
self.receive_thread = threading.Thread(target=self._receive_loop, daemon=True)
self.receive_thread.start()
print(f"Reliable Node initialized. Listening locally on {host}:{port}")
def _receive_loop(self):
while self.running:
try:
data, addr = self.socket.recvfrom(4096)
if not data:
continue
try:
envelope = msgpack.unpackb(data, strict_map_key=False)
except Exception as e:
print(f"[RECV-ERR] Malformed packet from {addr}: {e}")
continue
packet_type = int(envelope.get("_type", 0))
msg_id = int(envelope.get("_id", 0))
if packet_type == PACKET_MSG:
self._send_raw_ack(addr, msg_id)
payload = envelope.get("payload")
with connection_lock:
if self.connection_id not in incoming_payloads:
incoming_payloads[self.connection_id] = []
incoming_payloads[self.connection_id].append((payload, addr))
elif packet_type == PACKET_ACK:
if self.expected_ack_id is not None and msg_id == self.expected_ack_id:
self.ack_event.set()
except Exception as e:
if self.running:
print(f"Socket exception: {e}")
break
def _send_raw_ack(self, target_addr, target_id):
ack_envelope = {"_type": PACKET_ACK, "_id": target_id}
try:
self.socket.sendto(msgpack.packb(ack_envelope, use_bin_type=True), target_addr)
except Exception as e:
print(f"Failed to transmit ACK: {e}")
def send_reliable(self, target_host, target_port, payload, timeout=0.15, max_retries=4):
with connection_lock:
self.sequence_counter += 1
current_id = self.sequence_counter
envelope = {"_type": PACKET_MSG, "_id": current_id, "payload": payload}
binary_packet = msgpack.packb(envelope, use_bin_type=True)
for attempt in range(max_retries + 1):
if attempt > 0:
print(f"[ACK-LOG] Retry {attempt}/{max_retries} for Msg ID {current_id}...")
self.ack_event.clear()
self.expected_ack_id = current_id
try:
self.socket.sendto(binary_packet, (target_host, target_port))
if self.ack_event.wait(timeout):
self.expected_ack_id = None
return True
except Exception as e:
print(f"Transmission write failure: {e}")
self.expected_ack_id = None
print(f"[ACK-WARN] Message ID {current_id} dropped permanently. Node failed to respond.")
return False
# -----------------------------------------------------------------
# RPC Bridge Application Helpers
# -----------------------------------------------------------------
def reliable_connect(hostname: str, port: int) -> int:
global connection_counter
with connection_lock:
connection_counter += 1
conn_id = connection_counter
conn = ReliableMsgPackConnection(conn_id, hostname, port)
udp_connections[conn_id] = conn
incoming_payloads[conn_id] = []
return conn_id
def reliable_write(connection_id: int, target_host: str, target_port: int, payload) -> bool:
conn = udp_connections.get(connection_id)
if not conn:
return False
return conn.send_reliable(target_host, target_port, payload)
def reliable_read(connection_id: int):
with connection_lock:
queue = incoming_payloads.get(connection_id)
if not queue:
return None
return queue.pop(0) if queue else None
# -----------------------------------------------------------------
# Main Operational Implementation (Teensy Equivalent)
# -----------------------------------------------------------------
import socket
import threading
import time
import msgpack
# -----------------------------------------------------------------
# Core Infrastructure Setup & Reliable Class
# -----------------------------------------------------------------
udp_connections = {}
connection_counter = 0
connection_lock = threading.Lock()
incoming_payloads = {}
PACKET_MSG = 1
PACKET_ACK = 2
class ReliableMsgPackConnection:
"""Thread-safe Reliable MessagePack UDP wrapper for Linux Side"""
def __init__(self, connection_id, host, port):
self.connection_id = connection_id
self.host = host
self.port = port
self.socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
self.socket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
self.socket.bind((host, port))
self.sequence_counter = 0
self.running = True
self.ack_event = threading.Event()
self.expected_ack_id = None
self.receive_thread = threading.Thread(target=self._receive_loop, daemon=True)
self.receive_thread.start()
print(f"Reliable Node initialized. Listening locally on {host}:{port}")
def _receive_loop(self):
while self.running:
try:
data, addr = self.socket.recvfrom(4096)
if not data:
continue
try:
envelope = msgpack.unpackb(data, strict_map_key=False)
except Exception as e:
print(f"[RECV-ERR] Malformed packet from {addr}: {e}")
continue
packet_type = int(envelope.get("_type", 0))
msg_id = int(envelope.get("_id", 0))
if packet_type == PACKET_MSG:
self._send_raw_ack(addr, msg_id)
payload = envelope.get("payload")
with connection_lock:
if self.connection_id not in incoming_payloads:
incoming_payloads[self.connection_id] = []
incoming_payloads[self.connection_id].append((payload, addr))
elif packet_type == PACKET_ACK:
if self.expected_ack_id is not None and msg_id == self.expected_ack_id:
self.ack_event.set()
except Exception as e:
if self.running:
print(f"Socket exception: {e}")
break
def _send_raw_ack(self, target_addr, target_id):
ack_envelope = {"_type": PACKET_ACK, "_id": target_id}
try:
self.socket.sendto(msgpack.packb(ack_envelope, use_bin_type=True), target_addr)
except Exception as e:
print(f"Failed to transmit ACK: {e}")
def send_reliable(self, target_host, target_port, payload, timeout=0.15, max_retries=4):
with connection_lock:
self.sequence_counter += 1
current_id = self.sequence_counter
envelope = {"_type": PACKET_MSG, "_id": current_id, "payload": payload}
binary_packet = msgpack.packb(envelope, use_bin_type=True)
for attempt in range(max_retries + 1):
if attempt > 0:
print(f"[ACK-LOG] Retry {attempt}/{max_retries} for Msg ID {current_id}...")
self.ack_event.clear()
self.expected_ack_id = current_id
try:
self.socket.sendto(binary_packet, (target_host, target_port))
if self.ack_event.wait(timeout):
self.expected_ack_id = None
return True
except Exception as e:
print(f"Transmission write failure: {e}")
self.expected_ack_id = None
print(f"[ACK-WARN] Message ID {current_id} dropped permanently. Node failed to respond.")
return False
# -----------------------------------------------------------------
# RPC Bridge Application Helpers
# -----------------------------------------------------------------
def reliable_connect(hostname: str, port: int) -> int:
global connection_counter
with connection_lock:
connection_counter += 1
conn_id = connection_counter
conn = ReliableMsgPackConnection(conn_id, hostname, port)
udp_connections[conn_id] = conn
incoming_payloads[conn_id] = []
return conn_id
def reliable_write(connection_id: int, target_host: str, target_port: int, payload) -> bool:
conn = udp_connections.get(connection_id)
if not conn:
return False
return conn.send_reliable(target_host, target_port, payload)
def reliable_read(connection_id: int):
with connection_lock:
queue = incoming_payloads.get(connection_id)
if not queue:
return None
return queue.pop(0) if queue else None
# -----------------------------------------------------------------
# Main Operational Implementation (Teensy Equivalent)
# -----------------------------------------------------------------
# Network Rules matching the exact logic from your script
LOCAL_PORT = 3333
ESP32_PORT = 8888
ESP32_IP = "192.168.1.221"
if __name__ == "__main__":
# Setup / Initialization phase
print("Starting Linux Node...")
reliable_node = reliable_connect("0.0.0.0", LOCAL_PORT)
print(f"Linux Online. Listening on Port: {LOCAL_PORT}")
last_tx_time = time.time()
current_step = 0
# The Loop Execution phase
while True:
# ==========================================
# PHASE 1: RECEIVE DATA FROM ESP32
# ==========================================
incoming_data = reliable_read(reliable_node)
if incoming_data:
incoming_doc, sender = incoming_data
print(f"\n[LINUX RECV] Incoming packet from ESP32 ({sender}) recognized!")
# Read variant content safely matching your C++ keys
msg_type = incoming_doc.get("msg_type", "UNKNOWN")
payload = incoming_doc.get("payload")
if msg_type == "STRUCT_DATA":
print(f" > Type: Struct\n > Node ID: {payload.get('id')}, Battery: {payload.get('bat')}V, Active: {'YES' if payload.get('act') else 'NO'}")
elif msg_type == "ARRAY_DATA":
print(f" > Type: Array ({len(payload)} elements)\n > Values: {' '.join(str(v) for v in payload)}")
elif msg_type == "GENERIC_DATA":
print(f" > Type: Generic Primitive\n > Value: {payload}")
# ==========================================
# PHASE 2: TRANSMIT CORRESPONDING DESIGNS BACK
# ==========================================
if time.time() - last_tx_time > 4.0:
last_tx_time = time.time()
doc = {}
if current_step == 0:
print("\n[LINUX TX] Distributing state struct to ESP32...")
doc["msg_type"] = "STRUCT_DATA"
doc["payload"] = {"id": 999, "bat": 3.82, "act": False}
ok = reliable_write(reliable_node, ESP32_IP, ESP32_PORT, doc)
print(" >> ESP32 confirmed structural receipt." if ok else " >> Timeout error.")
elif current_step == 1:
print("\n[LINUX TX] Distributing a float Array to ESP32...")
doc["msg_type"] = "ARRAY_DATA"
doc["payload"] = [12.3, 45.6, 78.9]
ok = reliable_write(reliable_node, ESP32_IP, ESP32_PORT, doc)
print(" >> ESP32 confirmed array receipt." if ok else " >> Timeout error.")
elif current_step == 2:
print("\n[LINUX TX] Distributing basic string diagnostic to ESP32...")
doc["msg_type"] = "GENERIC_DATA"
doc["payload"] = "TEENSY_CORE_CRITICAL_OK"
ok = reliable_write(reliable_node, ESP32_IP, ESP32_PORT, doc)
print(" >> ESP32 confirmed generic receipt." if ok else " >> Timeout error.")
current_step = (current_step + 1) % 3
# Tiny sleep interval to ensure the CPU isn't spinning at 100% load
time.sleep(0.01)