From 0348e454a9713f6d67121e81f7eda20184efbed1 Mon Sep 17 00:00:00 2001 From: AlexTemirov Date: Mon, 10 Aug 2026 15:17:55 -0700 Subject: [PATCH] Add persistent ROSOrin mapping workflow --- README.md | 2 +- blacknode-package.toml | 2 +- .../slam/adapters/ros2/nodes/mapping.py | 46 +++++++++++++++++++ .../rosorin-persist-environment-map.json | 10 ++-- tests/test_slam_toolbox_mapping.py | 18 +++++++- 5 files changed, 70 insertions(+), 8 deletions(-) diff --git a/README.md b/README.md index 28415c3..8597459 100644 --- a/README.md +++ b/README.md @@ -36,7 +36,7 @@ does not use Warp. - `DepthCamera` and `DepthObstacleWarning` expose capability and safety state after depth processing. - `CameraViewer`, `DepthViewer`, and `IMUViewer` present processed sensor contracts with controls specific to that sensor. - `LiDAR` exposes normalized scans; the optional CUDA `LiDARViewer` is the explicit Warp visualization path. -- `MapEnvironment` attaches to or session-scopes SLAM Toolbox and persists an occupancy map plus pose graph. +- `MapEnvironment` owns a persistent SLAM Toolbox mapping session when deployed with `action=start` and `lifecycle=managed`. In **Deployments**, the selected robot shows the live occupancy grid and provides **Save map** and **Stop mapping** controls. Saving writes the occupancy map and pose graph to the configured device directory. Stopping the deployment ends only the Blacknode-owned SLAM process and leaves the robot vendor bringup unchanged. - `TrackingObject` serves annotated MJPEG, masks, snapshots, and latest detections. - `VLM`, `ReasoningStream`, and `ReasoningDashboard` support OpenAI-compatible, NVIDIA NIM, Anthropic, and local Ollama endpoints. diff --git a/blacknode-package.toml b/blacknode-package.toml index 3545145..6ef71e1 100644 --- a/blacknode-package.toml +++ b/blacknode-package.toml @@ -1,6 +1,6 @@ [package] name = "blacknode-perception" -version = "0.5.5" +version = "0.5.6" description = "Camera, tracking, VLM, and spatial-perception capabilities organized as selectable components." requires-blacknode = ">=0.3.0" layer = "perception" diff --git a/components/slam/adapters/ros2/nodes/mapping.py b/components/slam/adapters/ros2/nodes/mapping.py index 3d0fc5f..14c6bc5 100644 --- a/components/slam/adapters/ros2/nodes/mapping.py +++ b/components/slam/adapters/ros2/nodes/mapping.py @@ -4,6 +4,7 @@ import json import re import shlex +import threading import time from pathlib import Path from typing import Any @@ -26,6 +27,8 @@ def __getattr__(self, name: str): _MAP_TYPE = "nav_msgs/msg/OccupancyGrid" _SAVE_MAP_TYPE = "slam_toolbox/srv/SaveMap" _SERIALIZE_TYPE = "slam_toolbox/srv/SerializePoseGraph" +_owned_run_ids: set[str] = set() +_owned_lock = threading.RLock() def _managed_id(value: Any) -> str: @@ -143,6 +146,7 @@ def _safe_map_name(value: Any) -> str: }, primary_inputs=["trigger", "action", "lifecycle", "map_name"], primary_outputs=["provider", "map_artifact", "report"], + live=True, ) def map_environment(ctx: dict) -> dict: action = str(ctx.get("action") or "status").strip().lower() @@ -199,6 +203,8 @@ def map_environment(ctx: dict) -> dict: "map_artifact": empty_artifact, "report": f"mapping start FAILED: {launch.get('error') or 'could not launch SLAM Toolbox'}", } + with _owned_lock: + _owned_run_ids.add(run_id) wait_seconds = max(0.0, min(60.0, _float(ctx, "wait_seconds", 10.0))) waited = rt.wait_for_topic_interfaces( [{"topic": map_topic, "message_type": _MAP_TYPE, "required": True}], @@ -306,6 +312,8 @@ def map_environment(ctx: dict) -> dict: "report": f"mapping stop FAILED: {result.get('error') or 'owned process could not be stopped'}", } stopped = int(result.get("stopped") or 0) + with _owned_lock: + _owned_run_ids.discard(run_id) current, _ = _status_result(map_topic, lifecycle, run_id) return { "ready": bool(current["ready"]), @@ -348,3 +356,41 @@ def map_environment(ctx: dict) -> dict: "map_artifact": empty_artifact, "report": report, } + + +def runtime_status() -> dict[str, Any]: + """Expose mapping sessions to exported live-runtime status output.""" + with _owned_lock: + run_ids = sorted(_owned_run_ids) + managed_runs = [] + for run_id in run_ids: + status = _owned_status(run_id) + managed_runs.append({ + "run_id": run_id, + "ok": bool(status.get("running")), + "streaming": bool(status.get("running")), + "report": ( + "SLAM Toolbox mapping process is running" + if status.get("running") + else str(status.get("error") or "mapping process is not running") + ), + }) + return {"managed_runs": managed_runs} + + +def stop_runtime_services() -> dict[str, Any]: + """Stop only SLAM sessions launched by this Blacknode process.""" + with _owned_lock: + run_ids = sorted(_owned_run_ids) + _owned_run_ids.clear() + stopped = 0 + errors: list[str] = [] + for run_id in run_ids: + try: + result = rt.stop_ros2_managed(run_id) + stopped += int(result.get("stopped") or 0) + if not result.get("ok", True): + errors.append(str(result.get("error") or run_id)) + except Exception as exc: + errors.append(f"{run_id}: {exc}") + return {"ok": not errors, "stopped": stopped, "errors": errors} diff --git a/components/slam/adapters/ros2/templates/rosorin-persist-environment-map.json b/components/slam/adapters/ros2/templates/rosorin-persist-environment-map.json index c5b4f94..f629ff4 100644 --- a/components/slam/adapters/ros2/templates/rosorin-persist-environment-map.json +++ b/components/slam/adapters/ros2/templates/rosorin-persist-environment-map.json @@ -1,15 +1,15 @@ { "kind": "blacknode.workflow", "schema_version": 1, - "name": "ROSOrin Persist Environment Map", + "name": "ROSOrin Map Environment", "saved_at": "2026-08-10T00:00:00", "entrypoint": { "node_id": "map_environment", - "port": "map_artifact" + "port": "provider" }, "metadata": { "template": true, - "description": "Persist the live ROSOrin SLAM Toolbox occupancy map and serialized pose graph as a Blacknode map artifact. The workflow attaches to the existing ROS graph and leaves vendor bringup unchanged.", + "description": "Run a persistent SLAM Toolbox mapping session on the selected ROSOrin deployment. Watch the occupancy map live, save named map artifacts, and stop the session from Deployments while the vendor robot bringup remains unchanged.", "color": "#18a058", "required_packages": ["blacknode-perception", "blacknode-ros2"], "required_components": ["blacknode-perception/slam", "blacknode-ros2/core"], @@ -20,8 +20,8 @@ "id": "map_environment", "type": "MapEnvironment", "params": { - "action": "save", - "lifecycle": "existing", + "action": "start", + "lifecycle": "managed", "run_id": "rosorin-map", "map_topic": "/map", "params_file": "/home/ubuntu/ros2_ws/src/slam/config/slam.yaml", diff --git a/tests/test_slam_toolbox_mapping.py b/tests/test_slam_toolbox_mapping.py index 490d29e..e17fe93 100644 --- a/tests/test_slam_toolbox_mapping.py +++ b/tests/test_slam_toolbox_mapping.py @@ -130,5 +130,21 @@ def test_rosorin_map_template_is_a_real_persistence_workflow(): workflow = json.loads(path.read_text(encoding="utf-8")) report = validate_workflow(workflow) assert report.ok, report.to_dict() - assert workflow["node_meta"]["map_environment"]["params"]["action"] == "save" + params = workflow["node_meta"]["map_environment"]["params"] + assert params["action"] == "start" + assert params["lifecycle"] == "managed" + assert _NODE_REGISTRY["MapEnvironment"]._bn_live_capable is True assert not any("check" in node["type"].lower() or "test" in node["type"].lower() for node in workflow["node_meta"].values()) + + +def test_mapping_runtime_stop_only_stops_sessions_started_here(monkeypatch): + fake = _runtime(ready=False, owned=False) + monkeypatch.setattr(mapping, "rt", fake) + mapping._owned_run_ids.clear() + mapping._owned_run_ids.add("blacknode-mapping:rosorin-map") + + result = mapping.stop_runtime_services() + + assert result == {"ok": True, "stopped": 1, "errors": []} + assert fake.calls == [("stop", "blacknode-mapping:rosorin-map")] + assert mapping._owned_run_ids == set()