osiris-agent 0.1.1__tar.gz → 0.1.3__tar.gz
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.
- {osiris_agent-0.1.1 → osiris_agent-0.1.3}/PKG-INFO +2 -1
- {osiris_agent-0.1.1 → osiris_agent-0.1.3}/osiris_agent/agent_node.py +60 -2
- osiris_agent-0.1.3/osiris_agent/bt_collector.py +571 -0
- {osiris_agent-0.1.1 → osiris_agent-0.1.3}/osiris_agent.egg-info/PKG-INFO +2 -1
- {osiris_agent-0.1.1 → osiris_agent-0.1.3}/osiris_agent.egg-info/SOURCES.txt +1 -0
- {osiris_agent-0.1.1 → osiris_agent-0.1.3}/osiris_agent.egg-info/requires.txt +1 -0
- {osiris_agent-0.1.1 → osiris_agent-0.1.3}/setup.py +2 -1
- {osiris_agent-0.1.1 → osiris_agent-0.1.3}/LICENSE +0 -0
- {osiris_agent-0.1.1 → osiris_agent-0.1.3}/README.md +0 -0
- {osiris_agent-0.1.1 → osiris_agent-0.1.3}/osiris_agent/__init__.py +0 -0
- {osiris_agent-0.1.1 → osiris_agent-0.1.3}/osiris_agent.egg-info/dependency_links.txt +0 -0
- {osiris_agent-0.1.1 → osiris_agent-0.1.3}/osiris_agent.egg-info/entry_points.txt +0 -0
- {osiris_agent-0.1.1 → osiris_agent-0.1.3}/osiris_agent.egg-info/top_level.txt +0 -0
- {osiris_agent-0.1.1 → osiris_agent-0.1.3}/setup.cfg +0 -0
- {osiris_agent-0.1.1 → osiris_agent-0.1.3}/tests/test_agent_node.py +0 -0
|
@@ -1,6 +1,6 @@
|
|
|
1
1
|
Metadata-Version: 2.4
|
|
2
2
|
Name: osiris_agent
|
|
3
|
-
Version: 0.1.
|
|
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
|
|
@@ -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
|
|
@@ -938,7 +960,10 @@ class WebBridge(Node):
|
|
|
938
960
|
def _get_telemetry_snapshot(self):
|
|
939
961
|
"""Return a snapshot of system telemetry (CPU, RAM, disk)."""
|
|
940
962
|
return {
|
|
941
|
-
'cpu':
|
|
963
|
+
'cpu': {
|
|
964
|
+
'percent': psutil.cpu_percent(interval=None),
|
|
965
|
+
'cores': psutil.cpu_count(logical=False),
|
|
966
|
+
},
|
|
942
967
|
'ram': {
|
|
943
968
|
'percent': psutil.virtual_memory().percent,
|
|
944
969
|
'used_mb': psutil.virtual_memory().used / (1024 * 1024),
|
|
@@ -951,6 +976,39 @@ class WebBridge(Node):
|
|
|
951
976
|
}
|
|
952
977
|
}
|
|
953
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
|
+
|
|
954
1012
|
|
|
955
1013
|
# Initialize ROS, create node, and run until shutdown
|
|
956
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.
|
|
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
|
|
@@ -6,7 +6,7 @@ long_description = (HERE / "README.md").read_text(encoding="utf-8")
|
|
|
6
6
|
|
|
7
7
|
setup(
|
|
8
8
|
name='osiris_agent',
|
|
9
|
-
version='0.1.
|
|
9
|
+
version='0.1.3',
|
|
10
10
|
description='OSIRIS agent for ROS2/Humble',
|
|
11
11
|
long_description=long_description,
|
|
12
12
|
long_description_content_type="text/markdown",
|
|
@@ -27,6 +27,7 @@ setup(
|
|
|
27
27
|
install_requires=[
|
|
28
28
|
'websockets',
|
|
29
29
|
'psutil',
|
|
30
|
+
'pyzmq',
|
|
30
31
|
],
|
|
31
32
|
extras_require={
|
|
32
33
|
'ros': ['rclpy'],
|
|
File without changes
|
|
File without changes
|
|
File without changes
|
|
File without changes
|
|
File without changes
|
|
File without changes
|
|
File without changes
|
|
File without changes
|