osiris-agent 0.1.2__py3-none-any.whl → 0.1.3__py3-none-any.whl

This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.
@@ -14,6 +14,8 @@ from rosidl_runtime_py.utilities import get_message
14
14
  from rosidl_runtime_py import message_to_ordereddict
15
15
  import psutil
16
16
 
17
+ from .bt_collector import BTCollector
18
+
17
19
  # Security and configuration constants
18
20
  MAX_SUBSCRIPTIONS = 100
19
21
  ALLOWED_TOPIC_PREFIXES = ['/', ]
@@ -30,7 +32,7 @@ class WebBridge(Node):
30
32
  auth_token = os.environ.get('OSIRIS_AUTH_TOKEN')
31
33
  if not auth_token:
32
34
  raise ValueError("OSIRIS_AUTH_TOKEN environment variable must be set")
33
-
35
+
34
36
  self.ws_url = f'wss://osiris-gateway.fly.dev?robot=true&token={auth_token}'
35
37
  self.ws = None
36
38
  self._topic_subs = {}
@@ -57,6 +59,7 @@ class WebBridge(Node):
57
59
  self._last_sent_topics = None
58
60
  self._last_sent_actions = None
59
61
  self._last_sent_services = None
62
+ self._cached_bt_tree_event = None # Cache tree event until WS connects
60
63
 
61
64
  self._check_graph_changes()
62
65
  self.create_timer(0.1, self._check_graph_changes)
@@ -64,6 +67,19 @@ class WebBridge(Node):
64
67
  self.create_timer(1.0, self._collect_telemetry)
65
68
 
66
69
  threading.Thread(target=self._run_ws_client, daemon=True).start()
70
+
71
+ # Initialize BT Collector for Groot2 events (optional)
72
+ bt_enabled = os.environ.get('OSIRIS_BT_COLLECTOR_ENABLED', '').lower() in ('true', '1', 'yes')
73
+ if bt_enabled:
74
+ self._bt_collector = BTCollector(
75
+ event_callback=self._on_bt_event,
76
+ logger=self.get_logger()
77
+ )
78
+ self._bt_collector.start()
79
+ self.get_logger().info("BT Collector started")
80
+ else:
81
+ self._bt_collector = None
82
+ self.get_logger().info("BT Collector disabled (OSIRIS_BT_COLLECTOR_ENABLED not set)")
67
83
 
68
84
  # Create event loop and queue, run websocket client
69
85
  def _run_ws_client(self):
@@ -166,6 +182,12 @@ class WebBridge(Node):
166
182
  await self._send_queue.put(json.dumps(message))
167
183
  self.get_logger().info(f"Sent initial state: {len(nodes)} nodes, {len(topics)} topics, {len(actions)} actions, {len(services)} services")
168
184
 
185
+ # Send cached BT tree event if we have one
186
+ if self._cached_bt_tree_event:
187
+ self.get_logger().info("Sending cached BT tree event")
188
+ await self._send_queue.put(json.dumps(self._cached_bt_tree_event))
189
+ self._cached_bt_tree_event = None
190
+
169
191
  await self._send_bridge_subscriptions()
170
192
 
171
193
  # Send list of currently subscribed topics to gateway
@@ -954,6 +976,39 @@ class WebBridge(Node):
954
976
  }
955
977
  }
956
978
 
979
+ # Handle BT events from BTCollector
980
+ def _on_bt_event(self, event):
981
+ """Handle behavior tree events from BTCollector and forward to websocket."""
982
+ event_type = event.get('type')
983
+ self.get_logger().info(f"BT event received: {event_type}")
984
+
985
+ # Cache tree events if WS not connected yet
986
+ if not self.ws or not self.loop:
987
+ if event_type == 'bt_tree':
988
+ self._cached_bt_tree_event = event
989
+ self.get_logger().info("BT tree event cached (WS not connected yet)")
990
+ else:
991
+ self.get_logger().warn(f"Dropping BT event {event_type} (WS not connected)")
992
+ return
993
+
994
+ try:
995
+ asyncio.run_coroutine_threadsafe(
996
+ self._send_queue.put(json.dumps(event)),
997
+ self.loop
998
+ )
999
+ self.get_logger().info(f"BT event {event_type} queued for sending")
1000
+ except Exception as e:
1001
+ self.get_logger().error(f"Failed to queue BT event: {e}")
1002
+
1003
+ def destroy_node(self):
1004
+ """Clean up resources before destroying the node."""
1005
+ # Stop BT collector
1006
+ if self._bt_collector:
1007
+ self._bt_collector.stop()
1008
+ self.get_logger().info("BT Collector stopped")
1009
+
1010
+ super().destroy_node()
1011
+
957
1012
 
958
1013
  # Initialize ROS, create node, and run until shutdown
959
1014
  def main(args=None):
@@ -0,0 +1,571 @@
1
+ """
2
+ BT.CPP Groot2 ZMQ Event Collector
3
+
4
+ Connects to the Groot2 ZMQ publisher from BT.CPP and collects:
5
+ - Tree structure (on connect and on change)
6
+ - Node status updates
7
+ - Breakpoint events
8
+
9
+ Events are forwarded to the WebBridge for transmission over WebSocket.
10
+ """
11
+
12
+ import json
13
+ import os
14
+ import struct
15
+ import threading
16
+ import time
17
+ from typing import Callable, Optional, Dict, Any, List
18
+ from dataclasses import dataclass, field
19
+ from enum import IntEnum
20
+
21
+ import zmq
22
+ import random # added to support unique_id generation in requests
23
+
24
+ # Default Groot2 configuration (industry standard)
25
+ DEFAULT_GROOT_SERVER_PORT = 1667 # REQ/REP for tree structure and status polling
26
+ DEFAULT_GROOT_PUBLISHER_PORT = 1668 # PUB/SUB for breakpoint notifications only
27
+ DEFAULT_GROOT_HOST = "127.0.0.1"
28
+
29
+ # ZMQ timeouts
30
+ ZMQ_RECV_TIMEOUT_MS = 2000
31
+ ZMQ_RECONNECT_INTERVAL = 2.0
32
+ STATUS_POLL_INTERVAL = 0.1 # Poll for status every 100ms
33
+
34
+
35
+ class NodeStatus(IntEnum):
36
+ """BT.CPP NodeStatus enum values (includes extended states)"""
37
+ IDLE = 0
38
+ RUNNING = 1
39
+ SUCCESS = 2
40
+ FAILURE = 3
41
+ SKIPPED = 4
42
+ # Extended states (BT.CPP internal)
43
+ IDLE_WAS_RUNNING = 11
44
+ IDLE_WAS_SUCCESS = 12
45
+ IDLE_WAS_FAILURE = 13
46
+
47
+ @classmethod
48
+ def to_string(cls, value: int) -> str:
49
+ try:
50
+ return cls(value).name
51
+ except ValueError:
52
+ return f"UNKNOWN({value})"
53
+
54
+
55
+ @dataclass
56
+ class BTNode:
57
+ """Represents a node in the behavior tree"""
58
+ uid: int
59
+ name: str
60
+ tag: str # Node type (e.g., "Sequence", "Action", "Condition")
61
+ status: str = "IDLE"
62
+
63
+
64
+ @dataclass
65
+ class BTTree:
66
+ """Represents the full behavior tree structure"""
67
+ tree_id: Optional[str] = None
68
+ nodes: Dict[int, BTNode] = field(default_factory=dict)
69
+ xml: str = ""
70
+ structure: Dict[str, Any] = field(default_factory=dict)
71
+
72
+
73
+ class BTCollector:
74
+ """
75
+ Collects BT events from Groot2 ZMQ publisher and forwards them.
76
+
77
+ Usage:
78
+ collector = BTCollector(event_callback)
79
+ collector.start()
80
+ # ... later ...
81
+ collector.stop()
82
+ """
83
+
84
+ def __init__(
85
+ self,
86
+ event_callback: Callable[[Dict[str, Any]], None],
87
+ host: Optional[str] = None,
88
+ server_port: Optional[int] = None,
89
+ publisher_port: Optional[int] = None,
90
+ logger=None
91
+ ):
92
+ """
93
+ Initialize the BT collector.
94
+
95
+ Args:
96
+ event_callback: Function to call with parsed events (dict)
97
+ host: Groot2 host address (default: from BT_GROOT_HOST env or 127.0.0.1)
98
+ server_port: Groot2 server port (default: from BT_GROOT_SERVER_PORT env or 1667)
99
+ publisher_port: Groot2 publisher port (default: from BT_GROOT_PUBLISHER_PORT env or 1668)
100
+ logger: Optional logger (uses print if None)
101
+ """
102
+ self._event_callback = event_callback
103
+ # Read from environment variables with fallback to defaults
104
+ self._host = host or os.environ.get('OSIRIS_BT_GROOT_HOST', DEFAULT_GROOT_HOST)
105
+ self._server_port = server_port or int(os.environ.get('OSIRIS_BT_GROOT_SERVER_PORT', str(DEFAULT_GROOT_SERVER_PORT)))
106
+ self._publisher_port = publisher_port or int(os.environ.get('OSIRIS_BT_GROOT_PUBLISHER_PORT', str(DEFAULT_GROOT_PUBLISHER_PORT)))
107
+ self._logger = logger
108
+
109
+ self._running = False
110
+ self._thread: Optional[threading.Thread] = None
111
+ self._context: Optional[zmq.Context] = None
112
+
113
+ self._current_tree: Optional[BTTree] = None
114
+ self._last_statuses: Dict[int, str] = {}
115
+
116
+ def _log_info(self, msg: str):
117
+ if self._logger:
118
+ self._logger.info(msg)
119
+ else:
120
+ print(f"[BTCollector INFO] {msg}")
121
+
122
+ def _log_debug(self, msg: str):
123
+ if self._logger:
124
+ self._logger.debug(msg)
125
+
126
+ def _log_error(self, msg: str):
127
+ if self._logger:
128
+ self._logger.error(msg)
129
+ else:
130
+ print(f"[BTCollector ERROR] {msg}")
131
+
132
+ def _log_warn(self, msg: str):
133
+ if self._logger:
134
+ self._logger.warn(msg)
135
+ else:
136
+ print(f"[BTCollector WARN] {msg}")
137
+
138
+ def start(self):
139
+ """Start the collector in a background thread."""
140
+ if self._running:
141
+ self._log_warn("BTCollector already running")
142
+ return
143
+
144
+ self._running = True
145
+ self._thread = threading.Thread(target=self._run_loop, daemon=True)
146
+ self._thread.start()
147
+ self._log_info(f"BTCollector started (server={self._host}:{self._server_port}, pub={self._publisher_port})")
148
+
149
+ def stop(self):
150
+ """Stop the collector."""
151
+ self._running = False
152
+ if self._thread:
153
+ self._thread.join(timeout=2.0)
154
+ self._thread = None
155
+ self._log_info("BTCollector stopped")
156
+
157
+ def _run_loop(self):
158
+ """Main collector loop - runs in background thread."""
159
+ self._context = zmq.Context()
160
+
161
+ while self._running:
162
+ try:
163
+ self._collect_loop()
164
+ except Exception as e:
165
+ self._log_error(f"Collection error: {e}")
166
+
167
+ if self._running:
168
+ self._log_info(f"Reconnecting in {ZMQ_RECONNECT_INTERVAL}s...")
169
+ time.sleep(ZMQ_RECONNECT_INTERVAL)
170
+
171
+ if self._context:
172
+ self._context.term()
173
+ self._context = None
174
+
175
+ def _collect_loop(self):
176
+ """Single collection session - connects, gets tree, polls for status updates."""
177
+ # Create REQ socket for tree structure AND status polling
178
+ req_socket = self._context.socket(zmq.REQ)
179
+ req_socket.setsockopt(zmq.RCVTIMEO, ZMQ_RECV_TIMEOUT_MS)
180
+ req_socket.setsockopt(zmq.LINGER, 0)
181
+
182
+ try:
183
+ # Connect to server for tree structure and status polling
184
+ server_addr = f"tcp://{self._host}:{self._server_port}"
185
+ self._log_info(f"Connecting to Groot2 server: {server_addr}")
186
+ req_socket.connect(server_addr)
187
+
188
+ # Main loop - request tree first, then poll for status
189
+ self._log_info("Starting BT collection loop...")
190
+ tree_received = False
191
+
192
+ while self._running:
193
+ try:
194
+ # Request tree structure if we don't have it yet
195
+ if not tree_received:
196
+ tree_received = self._request_tree_structure(req_socket)
197
+ if not tree_received:
198
+ # No tree yet, wait and retry
199
+ time.sleep(1.0)
200
+ continue
201
+ # Immediately try to fetch current statuses so the initial tree event can include them
202
+ try:
203
+ self._poll_status(req_socket)
204
+ except Exception:
205
+ # ignore timeout/errors here; we'll keep trying in the main loop
206
+ pass
207
+ # emit bt_tree now with status merged into nodes (if available)
208
+ if self._current_tree:
209
+ self._log_info("Sending initial tree event with statuses")
210
+ self._emit_current_tree_event()
211
+
212
+ # Poll for status updates
213
+ self._poll_status(req_socket)
214
+ time.sleep(STATUS_POLL_INTERVAL)
215
+
216
+ except zmq.Again:
217
+ # Timeout - need to recreate socket because REQ is in bad state
218
+ self._log_warn("Timeout - recreating socket...")
219
+ req_socket.close()
220
+ req_socket = self._context.socket(zmq.REQ)
221
+ req_socket.setsockopt(zmq.RCVTIMEO, ZMQ_RECV_TIMEOUT_MS)
222
+ req_socket.setsockopt(zmq.LINGER, 0)
223
+ req_socket.connect(server_addr)
224
+ tree_received = False # Need to re-request tree after reconnect
225
+ time.sleep(1.0)
226
+
227
+ except zmq.ZMQError as e:
228
+ if "current state" in str(e):
229
+ # Socket in bad state, recreate it
230
+ self._log_warn(f"Socket in bad state, recreating: {e}")
231
+ req_socket.close()
232
+ req_socket = self._context.socket(zmq.REQ)
233
+ req_socket.setsockopt(zmq.RCVTIMEO, ZMQ_RECV_TIMEOUT_MS)
234
+ req_socket.setsockopt(zmq.LINGER, 0)
235
+ req_socket.connect(server_addr)
236
+ tree_received = False
237
+ time.sleep(1.0)
238
+ else:
239
+ self._log_error(f"ZMQ error: {e}")
240
+ time.sleep(0.5)
241
+
242
+ except Exception as e:
243
+ self._log_error(f"Error in collection loop: {e}")
244
+ time.sleep(0.5)
245
+
246
+ finally:
247
+ req_socket.close()
248
+
249
+ def _poll_status(self, socket: zmq.Socket):
250
+ """Poll for status update via REQ/REP."""
251
+ # Send STATUS request: protocol=2, type='S'
252
+ protocol = 2
253
+ req_type = ord('S') # 'S' = STATUS request
254
+ unique_id = random.getrandbits(32)
255
+ header = struct.pack('<BBI', protocol, req_type, unique_id)
256
+
257
+ socket.send(header, zmq.SNDMORE)
258
+ socket.send(b"")
259
+
260
+ parts = socket.recv_multipart()
261
+ if parts and len(parts) >= 2:
262
+ resp_tree_id = self._extract_tree_id_from_header(parts[0])
263
+ if resp_tree_id and (not self._current_tree or self._current_tree.tree_id != resp_tree_id):
264
+ self._log_warn(
265
+ f"Tree changed behind endpoint: {self._current_tree.tree_id if self._current_tree else None} -> {resp_tree_id}. Refreshing FULLTREE..."
266
+ )
267
+ if self._request_tree_structure(socket):
268
+ self._emit_current_tree_event()
269
+ self._handle_status_message(parts, resp_tree_id)
270
+
271
+ def _extract_tree_id_from_header(self, hdr: bytes) -> Optional[str]:
272
+ """
273
+ Groot2 multipart header (22 bytes):
274
+ protocol(1), type(1), unique_id(4), tree_id(16)
275
+ Returns tree_id as hex string or None.
276
+ """
277
+ if not hdr or len(hdr) < 22:
278
+ return None
279
+ return hdr[6:22].hex()
280
+
281
+ def _emit_current_tree_event(self):
282
+ if not self._current_tree:
283
+ return
284
+ nodes_with_status = []
285
+ for nd in getattr(self._current_tree, "nodes_list", []):
286
+ uid = nd.get("uid")
287
+ nd_copy = dict(nd)
288
+ nd_copy["status"] = self._last_statuses.get(uid, NodeStatus.IDLE.name)
289
+ nodes_with_status.append(nd_copy)
290
+ event = {
291
+ "type": "bt_tree",
292
+ "timestamp": time.time(),
293
+ "tree_id": self._current_tree.tree_id,
294
+ "tree": self._current_tree.structure,
295
+ "nodes": nodes_with_status,
296
+ }
297
+ self._log_info(f"Sending tree event ({len(nodes_with_status)} nodes)")
298
+ self._event_callback(event)
299
+
300
+ def _request_tree_structure(self, socket: zmq.Socket) -> bool:
301
+ """Request and parse tree structure from Groot2 server. Returns True if successful."""
302
+ # Groot2 protocol: multipart message with binary header
303
+ # Header: protocol(uint8)=2, type(uint8)='T', unique_id(uint32)
304
+ protocol = 2
305
+ req_type = ord('T') # 'T' = FULLTREE request
306
+ unique_id = random.getrandbits(32)
307
+ header = struct.pack('<BBI', protocol, req_type, unique_id)
308
+
309
+ self._log_info(f"Requesting tree structure...")
310
+ socket.send(header, zmq.SNDMORE)
311
+ socket.send(b"") # Empty body
312
+
313
+ # Receive multipart response
314
+ parts = socket.recv_multipart()
315
+ self._log_info(f"Tree response: {len(parts)} parts")
316
+
317
+ if parts and len(parts) >= 2:
318
+ return self._parse_tree_response(parts)
319
+ else:
320
+ self._log_warn("Invalid tree response (expected 2+ parts)")
321
+ return False
322
+
323
+ def _parse_tree_response(self, parts: list) -> bool:
324
+ """
325
+ Parse tree structure response from Groot2.
326
+ Returns True if successful.
327
+
328
+ Response format (multipart):
329
+ - Part 0: Header (22 bytes): protocol(1), type(1), unique_id(4), tree_id(16)
330
+ - Part 1: XML string of the tree
331
+ """
332
+ if not parts:
333
+ self._log_warn("Empty tree response")
334
+ return False
335
+
336
+ self._log_info(f"Parsing tree response: {len(parts)} parts")
337
+
338
+ try:
339
+ # Part 0: Header
340
+ hdr = parts[0]
341
+ tree_id = None
342
+ if len(hdr) >= 22:
343
+ resp_protocol, resp_type, resp_id = struct.unpack('<BBI', hdr[:6])
344
+ tree_id = hdr[6:22].hex()
345
+ self._log_info(f"Tree header: protocol={resp_protocol}, type={chr(resp_type)}, tree_id={tree_id[:16]}...")
346
+ elif len(hdr) >= 6:
347
+ # Check if error response
348
+ try:
349
+ err = hdr.decode('utf-8')
350
+ self._log_error(f"Error response: {err}")
351
+ if len(parts) > 1:
352
+ self._log_error(f"Detail: {parts[1].decode('utf-8')}")
353
+ return False
354
+ except:
355
+ self._log_warn(f"Short header: {len(hdr)} bytes")
356
+
357
+ # Part 1: XML
358
+ if len(parts) < 2:
359
+ self._log_warn("No XML part in response")
360
+ return False
361
+
362
+ xml = parts[1].decode('utf-8', errors='replace')
363
+ self._log_info(f"Received tree XML: {len(xml)} chars")
364
+
365
+ # Parse XML to extract nodes
366
+ tree = BTTree()
367
+ tree.tree_id = tree_id # Set the tree_id from header
368
+ tree.xml = xml
369
+ nodes_list = self._parse_xml_tree(xml, tree)
370
+
371
+ if not nodes_list:
372
+ self._log_warn("No nodes extracted from tree XML")
373
+ return False
374
+
375
+ # store parsed tree and node list; do not emit tree event here
376
+ tree.nodes_list = nodes_list
377
+ self._current_tree = tree
378
+ self._last_statuses.clear()
379
+ self._log_info(f"Tree structure received: {len(nodes_list)} nodes")
380
+ return True
381
+
382
+ except Exception as e:
383
+ import traceback
384
+ self._log_error(f"Error parsing tree response: {e}\n{traceback.format_exc()}")
385
+ return False
386
+
387
+ def _parse_xml_tree(self, xml: str, tree: BTTree) -> list:
388
+ """Parse XML tree and extract nodes with UIDs."""
389
+ import xml.etree.ElementTree as ET
390
+
391
+ nodes_list = []
392
+ try:
393
+ root = ET.fromstring(xml)
394
+ self._extract_nodes_from_xml(root, tree, nodes_list)
395
+
396
+ # Also build hierarchical tree structure
397
+ tree.structure = self._build_tree_structure(root)
398
+
399
+ self._log_info(f"Extracted {len(nodes_list)} nodes from XML")
400
+ except ET.ParseError as e:
401
+ self._log_error(f"XML parse error: {e}")
402
+
403
+ return nodes_list
404
+
405
+ def _build_tree_structure(self, elem) -> dict:
406
+ """Recursively build hierarchical tree structure from XML."""
407
+ # Find the BehaviorTree element
408
+ behavior_tree = elem.find('.//BehaviorTree')
409
+ if behavior_tree is None:
410
+ return {}
411
+
412
+ # Build from the first child of BehaviorTree (the root node)
413
+ children = list(behavior_tree)
414
+ if not children:
415
+ return {}
416
+
417
+ return self._element_to_tree_node(children[0])
418
+
419
+ def _element_to_tree_node(self, elem) -> dict:
420
+ """Convert an XML element to a tree node dict with children."""
421
+ node = {
422
+ 'tag': elem.tag,
423
+ 'name': elem.attrib.get('name', elem.attrib.get('ID', elem.tag)),
424
+ 'attributes': dict(elem.attrib),
425
+ }
426
+
427
+ # Add uid if present
428
+ uid_str = elem.attrib.get('_uid')
429
+ if uid_str:
430
+ try:
431
+ node['uid'] = int(uid_str)
432
+ except ValueError:
433
+ pass
434
+
435
+ # Recursively add children
436
+ children = []
437
+ for child in elem:
438
+ children.append(self._element_to_tree_node(child))
439
+
440
+ if children:
441
+ node['children'] = children
442
+
443
+ return node
444
+
445
+ def _extract_nodes_from_xml(self, elem, tree: BTTree, nodes_list: list):
446
+ """Recursively extract nodes from XML element."""
447
+ # Check if this element has a _uid attribute
448
+ uid_str = elem.attrib.get('_uid')
449
+ if uid_str:
450
+ try:
451
+ uid = int(uid_str)
452
+ name = elem.attrib.get('name', elem.attrib.get('ID', elem.tag))
453
+ tag = elem.tag
454
+
455
+ node = BTNode(uid=uid, name=name, tag=tag)
456
+ tree.nodes[uid] = node
457
+ nodes_list.append({
458
+ 'uid': uid,
459
+ 'name': name,
460
+ 'tag': tag
461
+ })
462
+ self._log_info(f" Parsed node {uid}: {name} ({tag})")
463
+ except ValueError:
464
+ pass
465
+
466
+ # Recurse into children
467
+ for child in elem:
468
+ self._extract_nodes_from_xml(child, tree, nodes_list)
469
+
470
+ def _handle_status_message(self, parts: list, tree_id: Optional[str] = None):
471
+ """
472
+ Parse status update message from Groot2 REQ/REP response.
473
+
474
+ Response format (multipart):
475
+ - Part 0: Header (22 bytes): protocol(1), type(1), unique_id(4), tree_id(16)
476
+ - Part 1: Status data: repeated (uid: uint16, status: uint8) tuples
477
+ """
478
+ if not parts or len(parts) < 2:
479
+ return
480
+
481
+ # Part 1 contains the status data
482
+ data = parts[1]
483
+ if len(data) < 3:
484
+ return
485
+
486
+ try:
487
+ offset = 0
488
+ changes = []
489
+
490
+ # Parse status updates: (uid: uint16, status: uint8) pairs
491
+ while offset + 3 <= len(data):
492
+ uid = struct.unpack('<H', data[offset:offset+2])[0]
493
+ status_val = data[offset+2]
494
+ offset += 3
495
+
496
+ status_str = NodeStatus.to_string(status_val)
497
+
498
+ # Only report changes
499
+ if self._last_statuses.get(uid) != status_str:
500
+ self._last_statuses[uid] = status_str
501
+
502
+ # Get node info if available
503
+ node_name = ""
504
+ node_tag = ""
505
+ if self._current_tree and uid in self._current_tree.nodes:
506
+ node = self._current_tree.nodes[uid]
507
+ node_name = node.name
508
+ node_tag = node.tag
509
+
510
+ self._log_info(f" Node {uid} ({node_name}): -> {status_str}")
511
+
512
+ changes.append({
513
+ 'uid': uid,
514
+ 'name': node_name,
515
+ 'tag': node_tag,
516
+ 'status': status_str
517
+ })
518
+
519
+ if changes:
520
+ event = {
521
+ 'type': 'bt_status',
522
+ 'timestamp': time.time(),
523
+ 'tree_id': tree_id or (self._current_tree.tree_id if self._current_tree else None),
524
+ 'changes': changes
525
+ }
526
+
527
+ self._log_info(f"Status update: {len(changes)} changes")
528
+ self._log_info(f"Sending status event: {json.dumps(event, indent=2)}")
529
+ self._event_callback(event)
530
+
531
+ except Exception as e:
532
+ self._log_error(f"Error parsing status message: {e}")
533
+
534
+ def _handle_breakpoint_message(self, data: bytes):
535
+ """Handle breakpoint messages from Groot2."""
536
+ try:
537
+ # Breakpoint data is typically raw bytes that can be hex-encoded
538
+ parts = []
539
+
540
+ # Split into chunks if needed (some protocols send multi-part)
541
+ chunk_size = 64
542
+ for i in range(0, len(data), chunk_size):
543
+ chunk = data[i:i+chunk_size]
544
+ parts.append(chunk.hex())
545
+
546
+ event = {
547
+ 'type': 'bt_breakpoint',
548
+ 'timestamp': time.time(),
549
+ 'parts': parts
550
+ }
551
+
552
+ self._log_debug("Breakpoint event received")
553
+ self._event_callback(event)
554
+
555
+ except Exception as e:
556
+ self._log_error(f"Error parsing breakpoint message: {e}")
557
+
558
+
559
+ # For testing standalone
560
+ if __name__ == "__main__":
561
+ def print_event(event):
562
+ print(f"EVENT: {json.dumps(event, indent=2)}")
563
+
564
+ collector = BTCollector(print_event)
565
+ collector.start()
566
+
567
+ try:
568
+ while True:
569
+ time.sleep(1)
570
+ except KeyboardInterrupt:
571
+ collector.stop()
@@ -1,6 +1,6 @@
1
1
  Metadata-Version: 2.4
2
2
  Name: osiris_agent
3
- Version: 0.1.2
3
+ Version: 0.1.3
4
4
  Summary: OSIRIS agent for ROS2/Humble
5
5
  Home-page: https://github.com/nicolaselielll/osiris_agent
6
6
  Author: Nicolas Tuomaala
@@ -17,6 +17,7 @@ Description-Content-Type: text/markdown
17
17
  License-File: LICENSE
18
18
  Requires-Dist: websockets
19
19
  Requires-Dist: psutil
20
+ Requires-Dist: pyzmq
20
21
  Provides-Extra: ros
21
22
  Requires-Dist: rclpy; extra == "ros"
22
23
  Dynamic: author
@@ -0,0 +1,9 @@
1
+ osiris_agent/__init__.py,sha256=E0YxGzSMUelxIPB6kRPKL-2Da_QaYCZeOd8fEaLtQ5c,73
2
+ osiris_agent/agent_node.py,sha256=yEnYln1iDahrRILVeeQ3bRHTlWEYj9mLgPbhaKE9ZrY,41051
3
+ osiris_agent/bt_collector.py,sha256=-VkEIkHoL1WNWNU8waMbehjakJywHr6sibbpa015PZQ,21252
4
+ osiris_agent-0.1.3.dist-info/licenses/LICENSE,sha256=tv_rYfXPsDuLDPIrpAFFZfgaO15H-gz89GW1TfyCQ48,10758
5
+ osiris_agent-0.1.3.dist-info/METADATA,sha256=ijbKyI_Vlyl7y6tcFk4Vujz7QWRvdrjZAl6o-dW-TPE,2702
6
+ osiris_agent-0.1.3.dist-info/WHEEL,sha256=wUyA8OaulRlbfwMtmQsvNngGrxQHAvkKcvRmdizlJi0,92
7
+ osiris_agent-0.1.3.dist-info/entry_points.txt,sha256=ff4USKDLj8unIsvb7iQrVvfhP52Fhp3QAR5AchypnuE,60
8
+ osiris_agent-0.1.3.dist-info/top_level.txt,sha256=qT-C0LRSrwlNjTuA7bVsTmwYJzhONP18wuyAXwUBZnU,13
9
+ osiris_agent-0.1.3.dist-info/RECORD,,
@@ -1,5 +1,5 @@
1
1
  Wheel-Version: 1.0
2
- Generator: setuptools (80.9.0)
2
+ Generator: setuptools (80.10.2)
3
3
  Root-Is-Purelib: true
4
4
  Tag: py3-none-any
5
5
 
@@ -1,8 +0,0 @@
1
- osiris_agent/__init__.py,sha256=E0YxGzSMUelxIPB6kRPKL-2Da_QaYCZeOd8fEaLtQ5c,73
2
- osiris_agent/agent_node.py,sha256=4412uLiIWPkv0XikSwOoVxJJmNvJIWRerfTQmdQfcuo,38740
3
- osiris_agent-0.1.2.dist-info/licenses/LICENSE,sha256=tv_rYfXPsDuLDPIrpAFFZfgaO15H-gz89GW1TfyCQ48,10758
4
- osiris_agent-0.1.2.dist-info/METADATA,sha256=c9rX-hOlaZChpJySHvOg6VNCI0gy4mqbXIjbh9tMhgQ,2681
5
- osiris_agent-0.1.2.dist-info/WHEEL,sha256=_zCd3N1l69ArxyTb8rzEoP9TpbYXkqRFSNOD5OuxnTs,91
6
- osiris_agent-0.1.2.dist-info/entry_points.txt,sha256=ff4USKDLj8unIsvb7iQrVvfhP52Fhp3QAR5AchypnuE,60
7
- osiris_agent-0.1.2.dist-info/top_level.txt,sha256=qT-C0LRSrwlNjTuA7bVsTmwYJzhONP18wuyAXwUBZnU,13
8
- osiris_agent-0.1.2.dist-info/RECORD,,