diff --git a/README.md b/README.md index 3a8e551..9ce7717 100644 --- a/README.md +++ b/README.md @@ -6,6 +6,7 @@ WebSocket backend for the Robocoop medical assistance robot. Bridges the robot h - Connects to the robot via **rosbridge** (WebSocket → ROS topics) - Reads battery voltage from `/battery`, converts it to a percentage +- Runs an **RGB LED chenillard** on startup (`/rgb`, real env) — rainbow chase across 8 LEDs - Broadcasts real-time robot state to all connected dashboard clients - Records audit events (robot connected/disconnected, battery low, emergency stop) - Supports **mock mode** for frontend development without a physical robot @@ -31,7 +32,7 @@ Dashboard (frontend) │ rosbridge server (ws://robot:9090) │ - ROS topics (/battery, ...) + ROS topics (/battery, /rgb, /cmd_vel, ...) ``` ## Project structure @@ -43,8 +44,9 @@ src/ app/ — server, websocket handler, contracts, DI context adapters/ — robot adapter implementations + rosbridge client modules/ - robot/ — RobotState, RobotStateStore, TelemetryService + robot/ — RobotState, RobotStateStore, TelemetryService, RgbChenillard audit/ — AuditEvent, AuditService, sinks (console, file) + scripts/ — standalone utilities (rgb chenillard test) utils/ — Config loader (.env + YAML) robocoop_bringup/ config/ — YAML config files per environment @@ -95,6 +97,17 @@ bash run_backend.sh real Server starts on `ws://0.0.0.0:8765`. +In **real** mode, once rosbridge is connected, the backend starts the RGB chenillard automatically (`rgb.enabled: true` in `real.params.yaml`). Logs should show `RGB chenillard started on /rgb`. + +**Standalone LED test** (rosbridge only, no full backend): + +```bash +./scripts/rgb_chenillard.sh +# or: ROBOCOOP_ENV=real ./scripts/rgb_chenillard.sh +``` + +> Do not use `ros2 topic pub` from the dev laptop unless ROS2 is on the same DDS network as the robot. This project publishes `/rgb` through **rosbridge**, like `/battery` and `/cmd_vel`. + ## Quick connection test Connect to `ws://localhost:8765` with any WebSocket client (Postman, WebSocket King, wscat) and send: @@ -132,6 +145,7 @@ Expected response: |---|---|---| | `ROBOCOOP_ENV` | `mock` | Which config to load: `mock` or `real` | | `ROBOCOOP_CONFIG_DIR` | auto-detected | Path to the YAML config directory | +| `ROBOCOOP_RGB_CHENILLARD` | _(from YAML)_ | Override LED chenillard: `1` on, `0` off (ignores `rgb.enabled`) | ## Configuration diff --git a/docs/topics.jetson.md b/docs/topics.jetson.md new file mode 100644 index 0000000..15376bd --- /dev/null +++ b/docs/topics.jetson.md @@ -0,0 +1,46 @@ +```bash + /arm6_joints + /arm_joint + /battery + /beep + /camera/color/camera_info + /camera/color/image_raw + /camera/color/image_raw/compressed + /camera/color/image_raw/compressedDepth + /camera/color/image_raw/theora + /camera/depth/camera_info + /camera/depth/image_raw + /camera/depth/image_raw/compressed + /camera/depth/image_raw/compressedDepth + /camera/depth/image_raw/theora + /camera/depth/points + /camera/depth_filter_status + /camera/depth_registered/points + /camera/depth_to_color + /camera/depth_to_ir + /camera/ir/camera_info + /camera/ir/image_raw + /camera/ir/image_raw/compressed + /camera/ir/image_raw/compressedDepth + /camera/ir/image_raw/theora + /camera/rgb/image_raw/compressed + /client_count + /cmd_vel + /connected_clients + /diagnostics + /imu/data + /imu/data_raw + /joint_states + /odom + /odom_raw + /parameter_events + /rgb + /robot_description + /rosout + /scan0 + /scan1 + /set_pose + /smoother_server/transition_event + /tf + /tf_static +``` \ No newline at end of file diff --git a/run_backend.sh b/run_backend.sh index c73a4e3..6f2d306 100755 --- a/run_backend.sh +++ b/run_backend.sh @@ -46,6 +46,8 @@ export ROBOCOOP_ENV="$ENVIRONMENT" export ROBOCOOP_CONFIG_DIR="$CONFIG_DIR" export PYTHONPATH="$BACKEND_DIR:$PYTHONPATH" +# Chenillard RGB : géré par le backend via rosbridge (rgb.enabled dans real.params.yaml) + # Run server cd "$BACKEND_DIR" python3 -m robocoop_backend.app.server diff --git a/scripts/README.md b/scripts/README.md new file mode 100644 index 0000000..f679943 --- /dev/null +++ b/scripts/README.md @@ -0,0 +1,16 @@ +# scripts/ + +Utility scripts at the repo root. Not part of the Python package install. + +## `rgb_chenillard.sh` + +Runs the RGB LED rainbow chase **via rosbridge only** (no `ros2` CLI on the laptop). + +```bash +./scripts/rgb_chenillard.sh +ROBOCOOP_ENV=real ./scripts/rgb_chenillard.sh +``` + +Uses `src/robocoop_bringup/config/` (`common.params.yaml` + `{ROBOCOOP_ENV}.params.yaml`) for the rosbridge URL and `rgb.*` settings. + +For normal operation, prefer `bash run_backend.sh real` — the chenillard is started automatically inside `RosbridgeRobotAdapter` when `rgb.enabled: true`. diff --git a/scripts/rgb_chenillard.sh b/scripts/rgb_chenillard.sh new file mode 100755 index 0000000..b042ebd --- /dev/null +++ b/scripts/rgb_chenillard.sh @@ -0,0 +1,16 @@ +#!/bin/bash +# Chenillard RGB autonome via rosbridge (sans ros2 CLI). +# Utile pour tester ; en production, utiliser run_backend.sh real. + +set -euo pipefail + +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +REPO_ROOT="$(cd "$SCRIPT_DIR/.." && pwd)" +CONFIG_DIR="${ROBOCOOP_CONFIG_DIR:-$REPO_ROOT/src/robocoop_bringup/config}" +ENVIRONMENT="${ROBOCOOP_ENV:-real}" + +export ROBOCOOP_CONFIG_DIR="$CONFIG_DIR" +export ROBOCOOP_ENV="$ENVIRONMENT" +export PYTHONPATH="$REPO_ROOT/src/robocoop_backend:${PYTHONPATH:-}" + +exec python3 -m robocoop_backend.scripts.rgb_chenillard_standalone diff --git a/src/robocoop_backend/robocoop_backend.egg-info/SOURCES.txt b/src/robocoop_backend/robocoop_backend.egg-info/SOURCES.txt index f08ede4..cb4d976 100644 --- a/src/robocoop_backend/robocoop_backend.egg-info/SOURCES.txt +++ b/src/robocoop_backend/robocoop_backend.egg-info/SOURCES.txt @@ -24,5 +24,8 @@ robocoop_backend/modules/audit/audit_service.py robocoop_backend/modules/audit/event_formatter.py robocoop_backend/modules/audit/sinks.py robocoop_backend/modules/robot/__init__.py +robocoop_backend/modules/robot/rgb_chenillard.py robocoop_backend/modules/robot/state_store.py -robocoop_backend/modules/robot/telemetry_service.py \ No newline at end of file +robocoop_backend/modules/robot/telemetry_service.py +robocoop_backend/scripts/__init__.py +robocoop_backend/scripts/rgb_chenillard_standalone.py \ No newline at end of file diff --git a/src/robocoop_backend/robocoop_backend/adapters/README.md b/src/robocoop_backend/robocoop_backend/adapters/README.md index 76ee671..8f7ea41 100644 --- a/src/robocoop_backend/robocoop_backend/adapters/README.md +++ b/src/robocoop_backend/robocoop_backend/adapters/README.md @@ -9,7 +9,7 @@ This layer is the **only part of the codebase that talks to the robot**. Everyth | `base_adapter.py` | Abstract interface — any adapter must implement `is_connected()` | | `factory.py` | `create_adapter()` — reads `adapter_type` from config, returns the right instance | | `mock_adapter.py` | No-op adapter, always connected. Used when `ROBOCOOP_ENV=mock` | -| `rosbridge_adapter.py` | Real adapter. Subscribes to ROS topics, converts messages, notifies services | +| `rosbridge_adapter.py` | Real adapter. Subscribes to ROS topics, converts messages, notifies services; starts RGB chenillard on connect | | `rosbridge_client.py` | Pure WebSocket transport for the rosbridge protocol. No business logic | ## Two-layer design @@ -118,24 +118,48 @@ That's it. `TelemetryService.on_telemetry_received()` already handles any dict g --- +## RGB LED chenillard (publish only) + +The adapter publishes `/rgb` at connect when `rgb.enabled` is true (see `real.params.yaml`). Logic lives in `modules/robot/rgb_chenillard.py`; `factory.py` passes topic, message type, and timing from config. + +``` +rosbridge_adapter.connect() + → RgbChenillard.start() # asyncio loop: hue sweep + LED index 1→7 + → _client.publish("/rgb", "std_msgs/msg/ColorRGBA", {r, g, b, a}) + +rosbridge_adapter.disconnect() + → RgbChenillard.stop() # publishes {r:0, g:0, b:0, a:0} (all off) +``` + +Wired in `factory.py`: + +```python +rgb_cfg = config.get("rgb", {}) +return RosbridgeRobotAdapter( + ... + rgb_chenillard_enabled=_rgb_chenillard_enabled(rgb_cfg), # env ROBOCOOP_RGB_CHENILLARD overrides + rgb_topic=topics.get("rgb", "/rgb"), + rgb_msg_type=msg_types.get("rgb", "std_msgs/msg/ColorRGBA"), + ... +) +``` + +**Important:** publish from the backend goes through **rosbridge WebSocket**, not `ros2 topic pub` on the dev machine (unless that machine shares the robot's ROS domain). + +--- + ## How to publish to a ROS topic Example: you want to send a navigation goal to `/move_base_simple/goal`. -### Step 1 — Add `publish()` to rosbridge_client +### Step 1 — Use `rosbridge_client.publish()` + +`RosbridgeClient.publish()` is already implemented. From the adapter, schedule or await it: -`adapters/rosbridge_client.py`: ```python -async def publish(self, topic: str, msg_type: str, msg: dict) -> None: - if not self._websocket: - logger.error("Cannot publish: not connected") - return - try: - await self._websocket.send(json.dumps({ - "op": "publish", "topic": topic, "type": msg_type, "msg": msg, - })) - except Exception as e: - logger.error(f"Publish failed on {topic}: {e}") +await self._client.publish(topic, msg_type, msg) +# or from a sync handler: +self._schedule_publish(topic, msg_type, msg) # creates asyncio task ``` ### Step 2 — Implement the method in the adapter diff --git a/src/robocoop_backend/robocoop_backend/adapters/base_adapter.py b/src/robocoop_backend/robocoop_backend/adapters/base_adapter.py index 13d0cf3..dfe07ed 100644 --- a/src/robocoop_backend/robocoop_backend/adapters/base_adapter.py +++ b/src/robocoop_backend/robocoop_backend/adapters/base_adapter.py @@ -1,4 +1,8 @@ +import logging from abc import ABC, abstractmethod +from typing import Any, Dict + +logger = logging.getLogger(__name__) class RobotAdapter(ABC): @@ -16,3 +20,11 @@ async def disconnect(self) -> None: def is_connected(self) -> bool: """Check if currently connected.""" ... + + def send_velocity(self, data: Dict[str, Any]) -> None: + """Publish cmd_vel from teleop.move data. Override in subclasses.""" + logger.debug("send_velocity not implemented for %s", type(self).__name__) + + def emergency_stop(self) -> None: + """Trigger emergency stop. Override in subclasses.""" + logger.debug("emergency_stop not implemented for %s", type(self).__name__) diff --git a/src/robocoop_backend/robocoop_backend/adapters/factory.py b/src/robocoop_backend/robocoop_backend/adapters/factory.py index 9b7a16f..e7c877e 100644 --- a/src/robocoop_backend/robocoop_backend/adapters/factory.py +++ b/src/robocoop_backend/robocoop_backend/adapters/factory.py @@ -1,4 +1,5 @@ import logging +import os from robocoop_backend.adapters.base_adapter import RobotAdapter from robocoop_backend.adapters.mock_adapter import MockRobotAdapter @@ -7,6 +8,15 @@ logger = logging.getLogger(__name__) +def _rgb_chenillard_enabled(rgb_cfg: dict) -> bool: + override = os.environ.get("ROBOCOOP_RGB_CHENILLARD", "").lower() + if override in ("0", "false", "no", "off"): + return False + if override in ("1", "true", "yes", "on"): + return True + return bool(rgb_cfg.get("enabled", False)) + + def create_adapter( adapter_type: str, @@ -17,11 +27,18 @@ def create_adapter( logger.info(f"Creating adapter: {adapter_type}") if adapter_type == "mock": - return MockRobotAdapter() + return MockRobotAdapter(telemetry_service=telemetry_service) if adapter_type == "rosbridge": rb = config.get("rosbridge", {}) topics = rb.get("topics", {}) + msg_types = config.get("rosbridge_msg_types", {}) + teleop = config.get("teleop", {}) + watchdog_timeout = config.get( + "battery_watchdog_timeout_seconds", + rb.get("battery_watchdog_timeout_seconds", 15.0), + ) + rgb_cfg = config.get("rgb", {}) return RosbridgeRobotAdapter( url_primary=rb.get("url_primary", "ws://localhost:9090"), url_secondary=rb.get("url_secondary"), @@ -29,8 +46,25 @@ def create_adapter( reconnect_interval=rb.get("reconnect_interval_seconds", 2.0), max_reconnect_attempts=rb.get("max_reconnect_attempts", 5), battery_topic=topics.get("battery", "/battery"), - battery_watchdog_timeout=rb.get("battery_watchdog_timeout_seconds", 15.0), + battery_msg_type=msg_types.get("battery", "std_msgs/msg/Float32"), + cmd_vel_topic=topics.get("cmd_vel", "/cmd_vel"), + cmd_vel_msg_type=msg_types.get("cmd_vel", "geometry_msgs/msg/Twist"), + emergency_stop_topic=topics.get("emergency_stop", "/emergency_stop"), + emergency_stop_msg_type=msg_types.get("emergency_stop", "std_msgs/msg/Bool"), + odom_topic=topics.get("odom", "/odom"), + odom_msg_type=msg_types.get("odom", "nav_msgs/msg/Odometry"), + max_linear_x=float(teleop.get("max_linear_x", 0.5)), + max_angular_z=float(teleop.get("max_angular_z", 1.0)), + battery_watchdog_timeout=float(watchdog_timeout), telemetry_service=telemetry_service, + rgb_chenillard_enabled=_rgb_chenillard_enabled(rgb_cfg), + rgb_topic=topics.get("rgb", rgb_cfg.get("topic", "/rgb")), + rgb_msg_type=msg_types.get("rgb", "std_msgs/msg/ColorRGBA"), + rgb_frame_delay=float(rgb_cfg.get("frame_delay_seconds", 0.07)), + rgb_hue_step=float(rgb_cfg.get("hue_step", 15)), + rgb_brightness=float(rgb_cfg.get("brightness", 0.85)), + rgb_led_min=int(rgb_cfg.get("led_min", 1)), + rgb_led_max=int(rgb_cfg.get("led_max", 7)), ) raise ValueError(f"Unknown adapter type: '{adapter_type}'. Supported: mock, rosbridge") diff --git a/src/robocoop_backend/robocoop_backend/adapters/mock_adapter.py b/src/robocoop_backend/robocoop_backend/adapters/mock_adapter.py index c299832..0db5c95 100644 --- a/src/robocoop_backend/robocoop_backend/adapters/mock_adapter.py +++ b/src/robocoop_backend/robocoop_backend/adapters/mock_adapter.py @@ -1,9 +1,15 @@ +import logging +from typing import Any, Dict + from robocoop_backend.adapters.base_adapter import RobotAdapter +logger = logging.getLogger(__name__) + class MockRobotAdapter(RobotAdapter): - def __init__(self): + def __init__(self, telemetry_service=None): self._is_connected = True + self.telemetry_service = telemetry_service async def connect(self) -> bool: """Mock connect always succeeds.""" @@ -15,3 +21,11 @@ async def disconnect(self) -> None: def is_connected(self) -> bool: return self._is_connected + + def send_velocity(self, data: Dict[str, Any]) -> None: + linear_x = data.get("linear_x", data.get("linear", 0.0)) + angular_z = data.get("angular_z", data.get("angular", 0.0)) + logger.info("[MOCK] teleop.move linear_x=%s angular_z=%s", linear_x, angular_z) + + def emergency_stop(self) -> None: + logger.warning("[MOCK] emergency_stop") diff --git a/src/robocoop_backend/robocoop_backend/adapters/rosbridge_adapter.py b/src/robocoop_backend/robocoop_backend/adapters/rosbridge_adapter.py index d92922c..caf6e27 100644 --- a/src/robocoop_backend/robocoop_backend/adapters/rosbridge_adapter.py +++ b/src/robocoop_backend/robocoop_backend/adapters/rosbridge_adapter.py @@ -5,6 +5,7 @@ from robocoop_backend.adapters.base_adapter import RobotAdapter from robocoop_backend.adapters.rosbridge_client import RosbridgeClient +from robocoop_backend.modules.robot.rgb_chenillard import RgbChenillard logger = logging.getLogger(__name__) @@ -18,14 +19,53 @@ def __init__( reconnect_interval: float = 2.0, max_reconnect_attempts: int = 5, battery_topic: str = "/battery", + battery_msg_type: str = "std_msgs/msg/Float32", + cmd_vel_topic: str = "/cmd_vel", + cmd_vel_msg_type: str = "geometry_msgs/msg/Twist", # TODO: change to /cmd_vel_out if this is the correct topic + emergency_stop_topic: str = "/emergency_stop", + emergency_stop_msg_type: str = "std_msgs/msg/Bool", + odom_topic: str = "/odom", + odom_msg_type: str = "nav_msgs/msg/Odometry", + max_linear_x: float = 0.5, + max_angular_z: float = 1.0, battery_watchdog_timeout: float = 15.0, ping_interval: float = 5.0, telemetry_service=None, + rgb_chenillard_enabled: bool = False, + rgb_topic: str = "/rgb", + rgb_msg_type: str = "std_msgs/msg/ColorRGBA", + rgb_frame_delay: float = 0.07, + rgb_hue_step: float = 15.0, + rgb_brightness: float = 0.85, + rgb_led_min: int = 1, + rgb_led_max: int = 7, ): self.battery_topic = battery_topic + self.battery_msg_type = battery_msg_type + self.cmd_vel_topic = cmd_vel_topic + self.cmd_vel_msg_type = cmd_vel_msg_type + self.emergency_stop_topic = emergency_stop_topic + self.emergency_stop_msg_type = emergency_stop_msg_type + self.odom_topic = odom_topic + self.odom_msg_type = odom_msg_type + self.max_linear_x = max_linear_x + self.max_angular_z = max_angular_z self.battery_watchdog_timeout = battery_watchdog_timeout self.ping_interval = ping_interval self.telemetry_service = telemetry_service + self._rgb_chenillard_enabled = rgb_chenillard_enabled + self._rgb_chenillard: Optional[RgbChenillard] = None + if rgb_chenillard_enabled: + self._rgb_chenillard = RgbChenillard( + publish=self._client_publish, + topic=rgb_topic, + msg_type=rgb_msg_type, + frame_delay=rgb_frame_delay, + hue_step=rgb_hue_step, + brightness=rgb_brightness, + led_min=rgb_led_min, + led_max=rgb_led_max, + ) self._last_battery_time: Optional[datetime] = None self._watchdog_task: Optional[asyncio.Task] = None self._ping_task: Optional[asyncio.Task] = None @@ -43,11 +83,16 @@ async def connect(self) -> bool: if not await self._client.connect(): return False await self._subscribe_battery() + await self._subscribe_odom() self._watchdog_task = asyncio.create_task(self._battery_watchdog()) self._ping_task = asyncio.create_task(self._ping_loop()) + if self._rgb_chenillard: + await self._rgb_chenillard.start() return True async def disconnect(self) -> None: + if self._rgb_chenillard: + await self._rgb_chenillard.stop() if self._ping_task: self._ping_task.cancel() try: @@ -63,13 +108,77 @@ async def disconnect(self) -> None: await self._client.disconnect() async def _subscribe_battery(self) -> None: - await self._client.subscribe(self.battery_topic, "std_msgs/msg/Float32", self._on_battery_received) + await self._client.subscribe( + self.battery_topic, self.battery_msg_type, self._on_battery_received + ) + + async def _subscribe_odom(self) -> None: + await self._client.subscribe( + self.odom_topic, self.odom_msg_type, self._on_odom_received + ) + + def _on_odom_received(self, msg_data: Dict[str, Any]) -> None: + try: + twist_outer = msg_data.get("twist", {}) + if not isinstance(twist_outer, dict): + return + twist = twist_outer.get("twist", {}) + if not isinstance(twist, dict): + return + linear = twist.get("linear", {}) + if not isinstance(linear, dict): + return + linear_x = float(linear.get("x", 0.0)) + if self.telemetry_service: + self.telemetry_service.on_telemetry_received({"linear_velocity": linear_x}) + except (TypeError, ValueError) as e: + logger.error(f"[ODOM] Error: {e}") + + def send_velocity(self, data: Dict[str, Any]) -> None: + try: + linear_x = self._clamp_axis( + float(data.get("linear_x", data.get("linear", 0.0))), self.max_linear_x + ) + angular_z = self._clamp_axis( + float(data.get("angular_z", data.get("angular", 0.0))), self.max_angular_z + ) + msg = { + "linear": {"x": linear_x, "y": 0.0, "z": 0.0}, + "angular": {"x": 0.0, "y": 0.0, "z": angular_z}, + } + self._schedule_publish(self.cmd_vel_topic, self.cmd_vel_msg_type, msg) + logger.debug("[TELEOP] cmd_vel linear_x=%.3f angular_z=%.3f", linear_x, angular_z) + except (TypeError, ValueError) as e: + logger.error(f"[TELEOP] Invalid velocity data: {e}") + + def emergency_stop(self) -> None: + logger.warning("[EMERGENCY] Publishing emergency stop") + self._schedule_publish( + self.emergency_stop_topic, + self.emergency_stop_msg_type, + {"data": True}, + ) + + @staticmethod + def _clamp_axis(value: float, limit: float) -> float: + return max(-limit, min(limit, value)) + + async def _client_publish(self, topic: str, msg_type: str, msg: Dict[str, Any]) -> None: + await self._client.publish(topic, msg_type, msg) + + def _schedule_publish(self, topic: str, msg_type: str, msg: Dict[str, Any]) -> None: + try: + loop = asyncio.get_running_loop() + loop.create_task(self._client.publish(topic, msg_type, msg)) + except RuntimeError: + logger.error("Cannot publish on %s: no running event loop", topic) def _on_battery_received(self, msg_data: Dict[str, Any]) -> None: try: battery_level = None if "data" in msg_data: voltage = float(msg_data["data"]) + # battery_level = max(0, min(100, (voltage - 9.0) / (12.6 - 9.0) * 100)) battery_level = max(0, min(100, voltage / 12.0 * 100)) logger.info(f"[BATTERY] {voltage:.2f}V -> {battery_level:.1f}%") elif "percentage" in msg_data: @@ -108,6 +217,7 @@ def _notify_disconnected(self) -> None: def _on_bridge_reconnected(self) -> None: self._last_battery_time = None asyncio.create_task(self._subscribe_battery()) + asyncio.create_task(self._subscribe_odom()) if self._ping_task and self._ping_task.done(): self._ping_task = asyncio.create_task(self._ping_loop()) @@ -131,4 +241,4 @@ async def _ping_loop(self) -> None: logger.error(f"Ping loop error: {e}") def is_connected(self) -> bool: - return self._client.is_connected() + return self._client.is_connected() \ No newline at end of file diff --git a/src/robocoop_backend/robocoop_backend/adapters/rosbridge_client.py b/src/robocoop_backend/robocoop_backend/adapters/rosbridge_client.py index 763d531..d19b7b6 100644 --- a/src/robocoop_backend/robocoop_backend/adapters/rosbridge_client.py +++ b/src/robocoop_backend/robocoop_backend/adapters/rosbridge_client.py @@ -53,6 +53,20 @@ async def disconnect(self) -> None: self._is_connected = False logger.info("Disconnected from rosbridge") + async def publish(self, topic: str, msg_type: str, msg: Dict[str, Any]) -> None: + if not self._websocket: + logger.error("Cannot publish: not connected") + return + try: + await self._websocket.send(json.dumps({ + "op": "publish", + "topic": topic, + "type": msg_type, + "msg": msg, + })) + except Exception as e: + logger.error(f"Publish failed on {topic}: {e}") + async def subscribe(self, topic: str, msg_type: str, callback: Callable) -> None: if not self._websocket: logger.error("Cannot subscribe: not connected") diff --git a/src/robocoop_backend/robocoop_backend/app/README.md b/src/robocoop_backend/robocoop_backend/app/README.md index 554693e..876ab3e 100644 --- a/src/robocoop_backend/robocoop_backend/app/README.md +++ b/src/robocoop_backend/robocoop_backend/app/README.md @@ -85,7 +85,7 @@ When you add a new message type (inbound or outbound), always: 1. Load .env 2. Load common.params.yaml + {ROBOCOOP_ENV}.params.yaml 3. BackendContext.initialize(config) — builds all services -4. context.connect() — connects adapter to rosbridge (or mock) +4. context.connect() — connects adapter to rosbridge (or mock); real: RGB chenillard starts 5. websockets.serve() — starts WebSocket server on port 8765 6. context.set_websocket_handler() — wires broadcasts 7. await shutdown_event — blocks until SIGTERM/SIGINT diff --git a/src/robocoop_backend/robocoop_backend/app/backend_context.py b/src/robocoop_backend/robocoop_backend/app/backend_context.py index e655dd7..a7305cf 100644 --- a/src/robocoop_backend/robocoop_backend/app/backend_context.py +++ b/src/robocoop_backend/robocoop_backend/app/backend_context.py @@ -4,23 +4,34 @@ from robocoop_backend.adapters.factory import create_adapter from robocoop_backend.modules.audit.audit_logger import AuditLogger from robocoop_backend.modules.audit.audit_service import AuditService -from robocoop_backend.modules.audit.sinks import ConsoleSink +from robocoop_backend.modules.audit.sinks import AuditSink, ConsoleSink, FileSink from robocoop_backend.modules.robot.state_store import RobotStateStore from robocoop_backend.modules.robot.telemetry_service import TelemetryService logger = logging.getLogger(__name__) +def _build_audit_sinks(config: Dict[str, Any]) -> list[AuditSink]: + sinks: list[AuditSink] = [ConsoleSink()] + audit_path = config.get("audit_log_path") + if isinstance(audit_path, str) and audit_path.strip(): + sinks.append(FileSink(audit_path.strip())) + return sinks + + class BackendContext: """Dependency injection container for backend services.""" def __init__(self, config: Dict[str, Any]): self.config = config self.robot_state_store = RobotStateStore() - self.audit_service = AuditService(AuditLogger(sinks=[ConsoleSink()])) + self.audit_service = AuditService(AuditLogger(sinks=_build_audit_sinks(config))) self.telemetry_service = TelemetryService( robot_state_store=self.robot_state_store, audit_service=self.audit_service, + battery_warning_threshold=float(config.get("battery_warning_threshold", 20.0)), + battery_critical_threshold=float(config.get("battery_critical_threshold", 10.0)), + velocity_warning_threshold=float(config.get("velocity_warning_threshold", 1.0)), ) adapter_type = config.get("adapter_type", "mock") self.adapter = create_adapter( diff --git a/src/robocoop_backend/robocoop_backend/app/contracts.py b/src/robocoop_backend/robocoop_backend/app/contracts.py index 87b7537..7ceb0f4 100644 --- a/src/robocoop_backend/robocoop_backend/app/contracts.py +++ b/src/robocoop_backend/robocoop_backend/app/contracts.py @@ -22,6 +22,9 @@ emergency_stop {"type": "emergency_stop"} +get_health + {"type": "get_health"} + ━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━ BACKEND → FRONTEND ━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━ @@ -35,6 +38,7 @@ "data": { "is_connected": false, "battery_level": 0.0, # 0–100 (%) + "linear_velocity": 0.0, # m/s from /odom "ping_ms": 0, # latency en millisecondes "last_updated": "2025-05-18T12:00:00" } @@ -59,6 +63,7 @@ "data": { "is_connected": false, "battery_level": 0.0, + "linear_velocity": 0.0, "ping_ms": 0, "last_updated": "2025-05-18T12:00:00" } @@ -70,6 +75,7 @@ "data": { "is_connected": false, "battery_level": 0.0, + "linear_velocity": 0.0, "ping_ms": 0, "last_updated": "2025-05-18T12:00:00" } @@ -88,12 +94,30 @@ } } +health_response (réponse à get_health) + { + "type": "health_response", + "data": { + "status": "ok", + "adapter": "MockRobotAdapter", + "environment": "mock", + "robot_state": { + "is_connected": true, + "battery_level": 0.0, + "linear_velocity": 0.0, + "ping_ms": 0, + "last_updated": "2025-05-18T12:00:00" + } + } + } + ━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━ ACTIONS AUDIT (activity_event.data.action) ━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━ robot.connected robot.disconnected - battery.low emergency_stop + battery.low battery.critical + velocity.high emergency_stop """ # Message type constants — utiliser dans le code au lieu de strings brutes @@ -104,6 +128,7 @@ MSG_GET_ACTIVITY = "get_activity" MSG_TELEOP_MOVE = "teleop.move" MSG_EMERGENCY_STOP = "emergency_stop" +MSG_GET_HEALTH = "get_health" # Backend → Frontend MSG_PONG = "pong" @@ -112,3 +137,4 @@ MSG_STATE_RESPONSE = "state_response" MSG_STATE_UPDATED = "robot_state_updated" MSG_ACTIVITY_EVENT = "activity_event" +MSG_HEALTH_RESPONSE = "health_response" diff --git a/src/robocoop_backend/robocoop_backend/app/websocket_handler.py b/src/robocoop_backend/robocoop_backend/app/websocket_handler.py index 89e6b43..d208541 100644 --- a/src/robocoop_backend/robocoop_backend/app/websocket_handler.py +++ b/src/robocoop_backend/robocoop_backend/app/websocket_handler.py @@ -1,13 +1,14 @@ import json import logging +import os from typing import Callable, Set import websockets from robocoop_backend.app.contracts import ( MSG_PING, MSG_GET_STATE, MSG_GET_ACTIVITY, MSG_TELEOP_MOVE, MSG_EMERGENCY_STOP, - MSG_PONG, MSG_INITIAL_STATE, MSG_ACTIVITY_HISTORY, MSG_STATE_RESPONSE, - MSG_STATE_UPDATED, MSG_ACTIVITY_EVENT, + MSG_GET_HEALTH, MSG_PONG, MSG_INITIAL_STATE, MSG_ACTIVITY_HISTORY, MSG_STATE_RESPONSE, + MSG_STATE_UPDATED, MSG_ACTIVITY_EVENT, MSG_HEALTH_RESPONSE, ) from robocoop_backend.modules.audit.audit_event import AuditEvent @@ -63,6 +64,19 @@ async def handle_message(self, websocket, message: dict) -> None: elif msg_type == MSG_GET_ACTIVITY: history = self.context.audit_service.get_history(limit=int(message.get("limit", 50))) await websocket.send(json.dumps({"type": MSG_ACTIVITY_HISTORY, "data": history})) + elif msg_type == MSG_GET_HEALTH: + state = self.context.robot_state_store.to_dict() + adapter_type = type(self.context.adapter).__name__ + environment = os.environ.get("ROBOCOOP_ENV", "unknown") + await websocket.send(json.dumps({ + "type": MSG_HEALTH_RESPONSE, + "data": { + "status": "ok", + "adapter": adapter_type, + "environment": environment, + "robot_state": state, + } + })) elif msg_type == MSG_TELEOP_MOVE: self.context.adapter.send_velocity(message.get("data")) elif msg_type == MSG_EMERGENCY_STOP: diff --git a/src/robocoop_backend/robocoop_backend/modules/audit/README.md b/src/robocoop_backend/robocoop_backend/modules/audit/README.md index f7fa9f0..6148b42 100644 --- a/src/robocoop_backend/robocoop_backend/modules/audit/README.md +++ b/src/robocoop_backend/robocoop_backend/modules/audit/README.md @@ -48,7 +48,9 @@ Serializes to: |---|---|---|---| | `robot.connected` | `system` | Battery message received after a disconnection | — | | `robot.disconnected` | `system` | No battery message for 15s (watchdog) | — | -| `battery.low` | `system` | Battery drops below threshold (default 20%) | `battery_level`, `threshold` | +| `battery.low` | `system` | Battery drops below warning threshold (default 20%) | `battery_level`, `threshold` | +| `battery.critical` | `system` | Battery drops below critical threshold (default 10%) | `battery_level`, `threshold` | +| `velocity.high` | `system` | Linear speed exceeds threshold (default 1.0 m/s) | `velocity`, `threshold` | | `emergency_stop` | `dashboard` | Frontend sends `emergency_stop` message | — | ## How to record a new event diff --git a/src/robocoop_backend/robocoop_backend/modules/robot/README.md b/src/robocoop_backend/robocoop_backend/modules/robot/README.md index 83b8f96..0bfba6f 100644 --- a/src/robocoop_backend/robocoop_backend/modules/robot/README.md +++ b/src/robocoop_backend/robocoop_backend/modules/robot/README.md @@ -1,9 +1,15 @@ # modules/robot/ -Handles robot state and the telemetry pipeline. Two files, two responsibilities. +Handles robot state, the telemetry pipeline, and optional RGB LED effects. ## Files +| File | Role | +|---|---| +| `state_store.py` | Current robot state (battery, connection, …) | +| `telemetry_service.py` | Adapter → store → audit → WebSocket broadcast | +| `rgb_chenillard.py` | Rainbow chase on `/rgb` via rosbridge (real env) | + ### `state_store.py` Single source of truth for the robot's current state. Every part of the app reads from and writes to this store — never to a local variable. @@ -105,3 +111,24 @@ def _check_velocity(self, velocity: float) -> None: ``` Then call it from `on_telemetry_received()` alongside `_check_battery()`. + +--- + +### `rgb_chenillard.py` + +Async loop that publishes `std_msgs/msg/ColorRGBA` on `/rgb` through a `publish(topic, msg_type, msg)` callback (provided by `RosbridgeRobotAdapter`). + +- **Not** part of `RobotState` or the dashboard — side effect only +- Started in `rosbridge_adapter.connect()` when `rgb.enabled` is true +- Stopped in `disconnect()` with an all-off message (`a: 0.0`) + +Config (`common.params.yaml` + `real.params.yaml`): `rgb.enabled`, `rgb.brightness`, `rgb.frame_delay_seconds`, `rgb.hue_step`, `rgb.led_min` / `rgb.led_max`. + +Environment override: `ROBOCOOP_RGB_CHENILLARD=0|1`. + +Standalone test without the full server: + +```bash +./scripts/rgb_chenillard.sh +# runs: python -m robocoop_backend.scripts.rgb_chenillard_standalone +``` diff --git a/src/robocoop_backend/robocoop_backend/modules/robot/rgb_chenillard.py b/src/robocoop_backend/robocoop_backend/modules/robot/rgb_chenillard.py new file mode 100644 index 0000000..70f5d0e --- /dev/null +++ b/src/robocoop_backend/robocoop_backend/modules/robot/rgb_chenillard.py @@ -0,0 +1,77 @@ +import asyncio +import colorsys +import logging +from typing import Awaitable, Callable, Optional + +logger = logging.getLogger(__name__) + +PublishFn = Callable[[str, str, dict], Awaitable[None]] + + +class RgbChenillard: + """Publie un chenillard RGB sur /rgb via rosbridge.""" + + def __init__( + self, + publish: PublishFn, + topic: str = "/rgb", + msg_type: str = "std_msgs/msg/ColorRGBA", + frame_delay: float = 0.07, + hue_step: float = 15.0, + brightness: float = 0.85, + led_min: int = 1, + led_max: int = 7, + ): + self._publish = publish + self.topic = topic + self.msg_type = msg_type + self.frame_delay = frame_delay + self.hue_step = hue_step + self.brightness = brightness + self.led_min = led_min + self.led_max = led_max + self._task: Optional[asyncio.Task] = None + + async def start(self) -> None: + if self._task and not self._task.done(): + return + self._task = asyncio.create_task(self._run()) + logger.info("RGB chenillard started on %s", self.topic) + + async def stop(self) -> None: + if self._task: + self._task.cancel() + try: + await self._task + except asyncio.CancelledError: + pass + self._task = None + await self._publish_color(0.0, 0.0, 0.0, 0.0) + logger.info("RGB chenillard stopped") + + async def _run(self) -> None: + hue = 0.0 + try: + while True: + for led in range(self.led_min, self.led_max + 1): + r, g, b = self._hsv_to_rgb255(hue) + await self._publish_color(r, g, b, float(led)) + hue = (hue + self.hue_step) % 360.0 + await asyncio.sleep(self.frame_delay) + except asyncio.CancelledError: + raise + except Exception as e: + logger.error("RGB chenillard error: %s", e) + + def _hsv_to_rgb255(self, hue_deg: float) -> tuple[float, float, float]: + r, g, b = colorsys.hsv_to_rgb( + (hue_deg % 360.0) / 360.0, 1.0, self.brightness + ) + return r * 255.0, g * 255.0, b * 255.0 + + async def _publish_color(self, r: float, g: float, b: float, a: float) -> None: + await self._publish( + self.topic, + self.msg_type, + {"r": r, "g": g, "b": b, "a": a}, + ) diff --git a/src/robocoop_backend/robocoop_backend/modules/robot/state_store.py b/src/robocoop_backend/robocoop_backend/modules/robot/state_store.py index cc6201a..bb9114a 100644 --- a/src/robocoop_backend/robocoop_backend/modules/robot/state_store.py +++ b/src/robocoop_backend/robocoop_backend/modules/robot/state_store.py @@ -10,6 +10,7 @@ class RobotState: is_connected: bool = False battery_level: float = 0.0 + linear_velocity: float = 0.0 ping_ms: int = 0 last_updated: datetime = field(default_factory=datetime.now) @@ -17,6 +18,7 @@ def to_dict(self) -> Dict[str, Any]: return { "is_connected": self.is_connected, "battery_level": self.battery_level, + "linear_velocity": self.linear_velocity, "ping_ms": self.ping_ms, "last_updated": self.last_updated.isoformat(), } diff --git a/src/robocoop_backend/robocoop_backend/modules/robot/telemetry_service.py b/src/robocoop_backend/robocoop_backend/modules/robot/telemetry_service.py index 2e2d374..e34e6e7 100644 --- a/src/robocoop_backend/robocoop_backend/modules/robot/telemetry_service.py +++ b/src/robocoop_backend/robocoop_backend/modules/robot/telemetry_service.py @@ -14,12 +14,18 @@ def __init__( websocket_handler=None, audit_service=None, battery_warning_threshold: float = 20.0, + battery_critical_threshold: float = 10.0, + velocity_warning_threshold: float = 1.0, ): self.robot_state_store = robot_state_store self.websocket_handler = websocket_handler self.audit_service = audit_service self.battery_warning_threshold = battery_warning_threshold + self.battery_critical_threshold = battery_critical_threshold + self.velocity_warning_threshold = velocity_warning_threshold self._battery_alert_sent = False + self._battery_critical_alert_sent = False + self._velocity_alert_sent = False def on_telemetry_received(self, data: Dict[str, Any]) -> None: try: @@ -39,7 +45,12 @@ def on_telemetry_received(self, data: Dict[str, Any]) -> None: battery = data.get("battery_level") if battery is not None: - self._check_battery(battery) + self._check_battery_critical(battery) + self._check_battery_low(battery) + + velocity = data.get("linear_velocity") + if velocity is not None: + self._check_velocity(float(velocity)) self._broadcast({"type": "robot_state_updated", "data": state}) except Exception as e: @@ -51,7 +62,38 @@ def _emit_connection_event(self, is_connected: bool) -> None: action = "robot.connected" if is_connected else "robot.disconnected" self.audit_service.record(AuditEvent(action=action, actor="system", payload={})) - def _check_battery(self, battery_level: float) -> None: + def _check_battery_critical(self, battery_level: float) -> None: + if battery_level < self.battery_critical_threshold: + logger.warning(f"Battery critical: {battery_level:.1f}%") + if self.audit_service and not self._battery_critical_alert_sent: + self._battery_critical_alert_sent = True + self.audit_service.record(AuditEvent( + action="battery.critical", + actor="system", + payload={ + "battery_level": round(battery_level, 1), + "threshold": self.battery_critical_threshold, + }, + )) + else: + self._battery_critical_alert_sent = False + + def _check_velocity(self, velocity: float) -> None: + if velocity > self.velocity_warning_threshold: + if self.audit_service and not self._velocity_alert_sent: + self._velocity_alert_sent = True + self.audit_service.record(AuditEvent( + action="velocity.high", + actor="system", + payload={ + "velocity": round(velocity, 2), + "threshold": self.velocity_warning_threshold, + }, + )) + else: + self._velocity_alert_sent = False + + def _check_battery_low(self, battery_level: float) -> None: if battery_level < self.battery_warning_threshold: logger.warning(f"Battery low: {battery_level:.1f}%") if self.audit_service and not self._battery_alert_sent: @@ -59,7 +101,10 @@ def _check_battery(self, battery_level: float) -> None: self.audit_service.record(AuditEvent( action="battery.low", actor="system", - payload={"battery_level": round(battery_level, 1), "threshold": self.battery_warning_threshold}, + payload={ + "battery_level": round(battery_level, 1), + "threshold": self.battery_warning_threshold, + }, )) else: self._battery_alert_sent = False diff --git a/src/robocoop_backend/robocoop_backend/scripts/__init__.py b/src/robocoop_backend/robocoop_backend/scripts/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/src/robocoop_backend/robocoop_backend/scripts/rgb_chenillard_standalone.py b/src/robocoop_backend/robocoop_backend/scripts/rgb_chenillard_standalone.py new file mode 100644 index 0000000..eee4755 --- /dev/null +++ b/src/robocoop_backend/robocoop_backend/scripts/rgb_chenillard_standalone.py @@ -0,0 +1,56 @@ +"""Lance le chenillard RGB via rosbridge (sans backend complet).""" + +import asyncio +import logging +import os +import sys + +from robocoop_backend.adapters.rosbridge_client import RosbridgeClient +from robocoop_backend.modules.robot.rgb_chenillard import RgbChenillard +from robocoop_backend.utils.config import Config + +logging.basicConfig(level=logging.INFO, format="%(levelname)s: %(message)s") +logger = logging.getLogger(__name__) + + +async def main() -> int: + config_dir = os.environ.get("ROBOCOOP_CONFIG_DIR", "./config") + config = Config.load(config_dir=config_dir) + rb = config.get("rosbridge", {}) + rgb_cfg = config.get("rgb", {}) + msg_types = config.get("rosbridge_msg_types", {}) + topics = rb.get("topics", {}) + + client = RosbridgeClient( + url_primary=rb.get("url_primary", "ws://localhost:9090"), + url_secondary=rb.get("url_secondary"), + connection_timeout=float(rb.get("connection_timeout_seconds", 8.0)), + ) + if not await client.connect(): + logger.error("Connexion rosbridge impossible") + return 1 + + chenillard = RgbChenillard( + publish=client.publish, + topic=topics.get("rgb", rgb_cfg.get("topic", "/rgb")), + msg_type=msg_types.get("rgb", "std_msgs/msg/ColorRGBA"), + frame_delay=float(rgb_cfg.get("frame_delay_seconds", 0.07)), + hue_step=float(rgb_cfg.get("hue_step", 15)), + brightness=float(rgb_cfg.get("brightness", 0.85)), + led_min=int(rgb_cfg.get("led_min", 1)), + led_max=int(rgb_cfg.get("led_max", 7)), + ) + logger.info("Chenillard sur %s — Ctrl+C pour arrêter", chenillard.topic) + await chenillard.start() + try: + await asyncio.Event().wait() + except KeyboardInterrupt: + pass + finally: + await chenillard.stop() + await client.disconnect() + return 0 + + +if __name__ == "__main__": + sys.exit(asyncio.run(main())) diff --git a/src/robocoop_backend/robocoop_backend/tests/integration/test_context_lifecycle.py b/src/robocoop_backend/robocoop_backend/tests/integration/test_context_lifecycle.py index 2f80e0a..8f44d21 100644 --- a/src/robocoop_backend/robocoop_backend/tests/integration/test_context_lifecycle.py +++ b/src/robocoop_backend/robocoop_backend/tests/integration/test_context_lifecycle.py @@ -1,12 +1,16 @@ """ Integration tests: BackendContext wires all services correctly. """ +import json +from pathlib import Path from unittest.mock import AsyncMock, MagicMock import pytest from robocoop_backend.app.backend_context import BackendContext +from robocoop_backend.app.contracts import MSG_TELEOP_MOVE, MSG_EMERGENCY_STOP from robocoop_backend.app.websocket_handler import WebSocketHandler +from robocoop_backend.utils.config import Config @pytest.mark.integration @@ -42,3 +46,52 @@ def test_audit_record_visible_in_history(self): ctx.audit_service.record(AuditEvent(action="test.event", actor="test")) history = ctx.audit_service.get_history() assert any(e["action"] == "test.event" for e in history) + + async def test_teleop_move_via_handler_does_not_raise(self): + ctx = BackendContext({"adapter_type": "mock"}) + handler = WebSocketHandler(ctx) + ws = AsyncMock() + ws.send = AsyncMock() + await handler.handle_message( + ws, {"type": MSG_TELEOP_MOVE, "data": {"linear_x": 0.2, "angular_z": 0.0}} + ) + + async def test_emergency_stop_via_handler_does_not_raise(self): + ctx = BackendContext({"adapter_type": "mock"}) + handler = WebSocketHandler(ctx) + ws = AsyncMock() + ws.send = AsyncMock() + await handler.handle_message(ws, {"type": MSG_EMERGENCY_STOP}) + + def test_audit_file_sink_writes_json_line(self, tmp_path): + log_file = tmp_path / "audit.jsonl" + ctx = BackendContext({ + "adapter_type": "mock", + "audit_log_path": str(log_file), + }) + ctx.telemetry_service.on_telemetry_received({"battery_level": 8.0, "is_connected": True}) + assert log_file.exists() + lines = log_file.read_text(encoding="utf-8").strip().splitlines() + assert len(lines) >= 1 + events = [json.loads(line) for line in lines] + actions = {e["action"] for e in events} + assert "battery.critical" in actions or "battery.low" in actions + + +@pytest.mark.integration +class TestBundledConfig: + def test_mock_config_loads_and_builds_context(self): + config_dir = Path(__file__).resolve().parents[4] / "robocoop_bringup" / "config" + cfg = Config.load(str(config_dir), env="mock") + ctx = BackendContext(cfg.to_dict()) + assert cfg.get("adapter_type") == "mock" + + def test_real_config_merges_publish_topics(self): + config_dir = Path(__file__).resolve().parents[4] / "robocoop_bringup" / "config" + cfg = Config.load(str(config_dir), env="real") + assert cfg.get("rosbridge.topics.cmd_vel") == "/cmd_vel" + assert cfg.get("rosbridge.topics.odom") == "/odom" + assert cfg.get("rosbridge.topics.rgb") == "/rgb" + assert cfg.get("rgb.enabled") is True + assert cfg.get("teleop.max_linear_x") == pytest.approx(0.5) + assert cfg.get("velocity_warning_threshold") == pytest.approx(1.0) diff --git a/src/robocoop_backend/robocoop_backend/tests/integration/test_telemetry_pipeline.py b/src/robocoop_backend/robocoop_backend/tests/integration/test_telemetry_pipeline.py index 51818dc..143455f 100644 --- a/src/robocoop_backend/robocoop_backend/tests/integration/test_telemetry_pipeline.py +++ b/src/robocoop_backend/robocoop_backend/tests/integration/test_telemetry_pipeline.py @@ -44,6 +44,13 @@ def test_low_battery_creates_audit_event(self, pipeline): actions = [e.action for e in audit._history] assert "battery.low" in actions + def test_critical_battery_creates_audit_event(self, pipeline): + adapter, telemetry, audit, _ = pipeline + telemetry.battery_critical_threshold = 10.0 + adapter._on_battery_received({"data": 1.0}) + actions = [e.action for e in audit._history] + assert "battery.critical" in actions + def test_connection_recovery_creates_robot_connected_event(self, pipeline): adapter, telemetry, audit, _ = pipeline telemetry.on_telemetry_received({"is_connected": False}) @@ -72,3 +79,22 @@ def test_full_cycle_state_transitions(self, pipeline): connection_events = [e.action for e in audit._history if "robot." in e.action] assert "robot.connected" in connection_events assert "robot.disconnected" in connection_events + + +@pytest.mark.integration +class TestOdomFlow: + def test_odom_flows_to_state_store(self, pipeline): + adapter, _, _, store = pipeline + adapter._on_odom_received({ + "twist": {"twist": {"linear": {"x": 0.6, "y": 0.0, "z": 0.0}}}, + }) + assert store.get().linear_velocity == pytest.approx(0.6) + + def test_high_velocity_creates_audit_event(self, pipeline): + adapter, telemetry, audit, _ = pipeline + telemetry.velocity_warning_threshold = 0.5 + adapter._on_odom_received({ + "twist": {"twist": {"linear": {"x": 0.9}}}, + }) + actions = [e.action for e in audit._history] + assert "velocity.high" in actions diff --git a/src/robocoop_backend/robocoop_backend/tests/unit/adapters/test_factory.py b/src/robocoop_backend/robocoop_backend/tests/unit/adapters/test_factory.py index 455fc78..bbbebb6 100644 --- a/src/robocoop_backend/robocoop_backend/tests/unit/adapters/test_factory.py +++ b/src/robocoop_backend/robocoop_backend/tests/unit/adapters/test_factory.py @@ -8,8 +8,19 @@ "rosbridge": { "url_primary": "ws://localhost:9090", "url_secondary": "ws://localhost:9091", - "topics": {"battery": "/robot/battery"}, - } + "topics": { + "battery": "/robot/battery", + "cmd_vel": "/robot/cmd_vel", + "emergency_stop": "/robot/e_stop", + "odom": "/robot/odom", + }, + }, + "rosbridge_msg_types": { + "cmd_vel": "geometry_msgs/msg/Twist", + "emergency_stop": "std_msgs/msg/Bool", + }, + "teleop": {"max_linear_x": 0.3, "max_angular_z": 0.8}, + "battery_watchdog_timeout_seconds": 20.0, } @@ -56,6 +67,32 @@ def test_rosbridge_passes_telemetry_service(self): adapter = create_adapter("rosbridge", {"rosbridge": {}}, svc) assert adapter.telemetry_service is svc + def test_rosbridge_uses_config_odom_topic(self): + adapter = create_adapter("rosbridge", ROSBRIDGE_CONFIG, None) + assert adapter.odom_topic == "/robot/odom" + + def test_rosbridge_uses_config_cmd_vel_topic(self): + adapter = create_adapter("rosbridge", ROSBRIDGE_CONFIG, None) + assert adapter.cmd_vel_topic == "/robot/cmd_vel" + + def test_rosbridge_uses_config_emergency_topic(self): + adapter = create_adapter("rosbridge", ROSBRIDGE_CONFIG, None) + assert adapter.emergency_stop_topic == "/robot/e_stop" + + def test_rosbridge_applies_teleop_limits(self): + adapter = create_adapter("rosbridge", ROSBRIDGE_CONFIG, None) + assert adapter.max_linear_x == pytest.approx(0.3) + assert adapter.max_angular_z == pytest.approx(0.8) + + def test_rosbridge_applies_watchdog_from_common_config(self): + adapter = create_adapter("rosbridge", ROSBRIDGE_CONFIG, None) + assert adapter.battery_watchdog_timeout == pytest.approx(20.0) + + def test_mock_passes_telemetry_service(self): + svc = object() + adapter = create_adapter("mock", {}, svc) + assert adapter.telemetry_service is svc + def test_unknown_type_raises_value_error(self): with pytest.raises(ValueError, match="unknown_type"): create_adapter("unknown_type", {}, None) @@ -63,3 +100,23 @@ def test_unknown_type_raises_value_error(self): def test_empty_string_raises_value_error(self): with pytest.raises(ValueError): create_adapter("", {}, None) + + def test_rosbridge_rgb_chenillard_from_config(self, monkeypatch): + monkeypatch.delenv("ROBOCOOP_RGB_CHENILLARD", raising=False) + cfg = { + **ROSBRIDGE_CONFIG, + "rgb": {"enabled": True, "topic": "/led_rgb"}, + "rosbridge": { + **ROSBRIDGE_CONFIG["rosbridge"], + "topics": {**ROSBRIDGE_CONFIG["rosbridge"]["topics"], "rgb": "/led_rgb"}, + }, + } + adapter = create_adapter("rosbridge", cfg, None) + assert adapter._rgb_chenillard is not None + assert adapter._rgb_chenillard.topic == "/led_rgb" + + def test_rosbridge_rgb_chenillard_env_override_off(self, monkeypatch): + monkeypatch.setenv("ROBOCOOP_RGB_CHENILLARD", "0") + cfg = {**ROSBRIDGE_CONFIG, "rgb": {"enabled": True}} + adapter = create_adapter("rosbridge", cfg, None) + assert adapter._rgb_chenillard is None diff --git a/src/robocoop_backend/robocoop_backend/tests/unit/adapters/test_mock_adapter.py b/src/robocoop_backend/robocoop_backend/tests/unit/adapters/test_mock_adapter.py index d3913b2..395d647 100644 --- a/src/robocoop_backend/robocoop_backend/tests/unit/adapters/test_mock_adapter.py +++ b/src/robocoop_backend/robocoop_backend/tests/unit/adapters/test_mock_adapter.py @@ -21,3 +21,9 @@ def test_multiple_instances_are_independent(self): b = MockRobotAdapter() a._is_connected = False assert b.is_connected() is True + + def test_send_velocity_does_not_raise(self): + MockRobotAdapter().send_velocity({"linear_x": 0.5, "angular_z": 0.1}) + + def test_emergency_stop_does_not_raise(self): + MockRobotAdapter().emergency_stop() diff --git a/src/robocoop_backend/robocoop_backend/tests/unit/adapters/test_rosbridge_adapter.py b/src/robocoop_backend/robocoop_backend/tests/unit/adapters/test_rosbridge_adapter.py index 6bea9e9..04c2faf 100644 --- a/src/robocoop_backend/robocoop_backend/tests/unit/adapters/test_rosbridge_adapter.py +++ b/src/robocoop_backend/robocoop_backend/tests/unit/adapters/test_rosbridge_adapter.py @@ -147,6 +147,17 @@ async def test_disconnect_cancels_watchdog_task(self, patch_ws_connect): await adapter.disconnect() assert task.cancelled() or task.done() + async def test_connect_starts_rgb_chenillard_when_enabled(self, patch_ws_connect): + adapter = RosbridgeRobotAdapter( + url_primary="ws://localhost:9090", + rgb_chenillard_enabled=True, + ) + await adapter.connect() + assert adapter._rgb_chenillard is not None + assert adapter._rgb_chenillard._task is not None + await adapter.disconnect() + assert adapter._rgb_chenillard._task is None + @pytest.mark.unit class TestCallbacks: @@ -154,8 +165,9 @@ def test_on_bridge_reconnected_resets_last_battery_time(self): adapter = make_adapter() adapter._last_battery_time = datetime.now() with patch.object(adapter, "_subscribe_battery", new_callable=AsyncMock): - with patch("asyncio.create_task"): - adapter._on_bridge_reconnected() + with patch.object(adapter, "_subscribe_odom", new_callable=AsyncMock): + with patch("asyncio.create_task"): + adapter._on_bridge_reconnected() assert adapter._last_battery_time is None def test_on_bridge_disconnected_notifies_telemetry(self): @@ -167,3 +179,72 @@ def test_on_bridge_disconnected_notifies_telemetry(self): def test_is_connected_delegates_to_client(self): adapter = make_adapter() assert adapter.is_connected() == adapter._client.is_connected() + +@pytest.mark.unit +class TestOdom: + def test_odom_linear_velocity_forwarded_to_telemetry(self): + svc = MagicMock() + adapter = make_adapter(telemetry_service=svc) + adapter._on_odom_received({ + "twist": {"twist": {"linear": {"x": 0.75, "y": 0.0, "z": 0.0}}}, + }) + svc.on_telemetry_received.assert_called_once_with({"linear_velocity": pytest.approx(0.75)}) + + def test_odom_malformed_message_does_not_raise(self): + adapter = make_adapter() + adapter._on_odom_received({"twist": "invalid"}) + + def test_odom_without_telemetry_service_does_not_raise(self): + adapter = make_adapter() + adapter._on_odom_received({ + "twist": {"twist": {"linear": {"x": 1.0}}}, + }) + + +@pytest.mark.unit +class TestCommands: + def test_clamp_axis_within_limit(self): + adapter = make_adapter() + assert adapter._clamp_axis(0.3, 0.5) == pytest.approx(0.3) + + def test_clamp_axis_exceeds_positive_limit(self): + adapter = make_adapter() + assert adapter._clamp_axis(2.0, 0.5) == pytest.approx(0.5) + + def test_clamp_axis_exceeds_negative_limit(self): + adapter = make_adapter() + assert adapter._clamp_axis(-2.0, 0.5) == pytest.approx(-0.5) + + async def test_send_velocity_schedules_publish(self): + adapter = make_adapter() + adapter.cmd_vel_topic = "/cmd_vel" + adapter.cmd_vel_msg_type = "geometry_msgs/msg/Twist" + adapter.max_linear_x = 0.5 + adapter.max_angular_z = 1.0 + adapter._client.publish = AsyncMock() + + async def run(): + adapter.send_velocity({"linear_x": 1.0, "angular_z": 0.1}) + await asyncio.sleep(0) + + await run() + adapter._client.publish.assert_called_once() + topic, msg_type, msg = adapter._client.publish.call_args[0] + assert topic == "/cmd_vel" + assert msg["linear"]["x"] == pytest.approx(0.5) + assert msg["angular"]["z"] == pytest.approx(0.1) + + async def test_emergency_stop_publishes_bool(self): + adapter = make_adapter() + adapter.emergency_stop_topic = "/emergency_stop" + adapter.emergency_stop_msg_type = "std_msgs/msg/Bool" + adapter._client.publish = AsyncMock() + + async def run(): + adapter.emergency_stop() + await asyncio.sleep(0) + + await run() + adapter._client.publish.assert_called_once_with( + "/emergency_stop", "std_msgs/msg/Bool", {"data": True} + ) diff --git a/src/robocoop_backend/robocoop_backend/tests/unit/adapters/test_rosbridge_client.py b/src/robocoop_backend/robocoop_backend/tests/unit/adapters/test_rosbridge_client.py index 9073140..780fb81 100644 --- a/src/robocoop_backend/robocoop_backend/tests/unit/adapters/test_rosbridge_client.py +++ b/src/robocoop_backend/robocoop_backend/tests/unit/adapters/test_rosbridge_client.py @@ -92,6 +92,24 @@ async def test_subscribe_stores_callback(self, patch_ws_connect): assert client._subscribers["/battery"] is cb +@pytest.mark.unit +class TestPublish: + async def test_publish_sends_correct_json(self, patch_ws_connect): + client = make_client() + await client.connect() + msg = {"linear": {"x": 0.5, "y": 0.0, "z": 0.0}, "angular": {"x": 0.0, "y": 0.0, "z": 0.1}} + await client.publish("/cmd_vel", "geometry_msgs/msg/Twist", msg) + sent = json.loads(patch_ws_connect.send.call_args[0][0]) + assert sent["op"] == "publish" + assert sent["topic"] == "/cmd_vel" + assert sent["type"] == "geometry_msgs/msg/Twist" + assert sent["msg"] == msg + + async def test_publish_without_connect_logs_no_exception(self): + client = make_client() + await client.publish("/cmd_vel", "geometry_msgs/msg/Twist", {}) + + @pytest.mark.unit class TestMessageDispatch: async def test_message_dispatched_to_subscriber(self): diff --git a/src/robocoop_backend/robocoop_backend/tests/unit/app/test_backend_context.py b/src/robocoop_backend/robocoop_backend/tests/unit/app/test_backend_context.py index 847f9ee..79af8db 100644 --- a/src/robocoop_backend/robocoop_backend/tests/unit/app/test_backend_context.py +++ b/src/robocoop_backend/robocoop_backend/tests/unit/app/test_backend_context.py @@ -4,6 +4,7 @@ from robocoop_backend.adapters.mock_adapter import MockRobotAdapter from robocoop_backend.app.backend_context import BackendContext +from robocoop_backend.modules.audit.sinks import ConsoleSink, FileSink @pytest.mark.unit @@ -29,6 +30,30 @@ def test_telemetry_service_references_correct_dependencies(self): assert ctx.telemetry_service.robot_state_store is ctx.robot_state_store assert ctx.telemetry_service.audit_service is ctx.audit_service + def test_telemetry_service_reads_battery_thresholds_from_config(self): + ctx = BackendContext({ + "adapter_type": "mock", + "battery_warning_threshold": 25.0, + "battery_critical_threshold": 12.0, + }) + assert ctx.telemetry_service.battery_warning_threshold == 25.0 + assert ctx.telemetry_service.battery_critical_threshold == 12.0 + + def test_audit_logger_uses_file_sink_when_path_configured(self, tmp_path): + log_file = tmp_path / "audit.jsonl" + ctx = BackendContext({ + "adapter_type": "mock", + "audit_log_path": str(log_file), + }) + sink_types = [type(s) for s in ctx.audit_service._logger._sinks] + assert ConsoleSink in sink_types + assert FileSink in sink_types + + def test_audit_logger_console_only_without_audit_path(self): + ctx = BackendContext({"adapter_type": "mock"}) + sink_types = [type(s) for s in ctx.audit_service._logger._sinks] + assert sink_types == [ConsoleSink] + @pytest.mark.unit class TestBackendContextServices: diff --git a/src/robocoop_backend/robocoop_backend/tests/unit/app/test_websocket_handler.py b/src/robocoop_backend/robocoop_backend/tests/unit/app/test_websocket_handler.py index 275d91d..d604a05 100644 --- a/src/robocoop_backend/robocoop_backend/tests/unit/app/test_websocket_handler.py +++ b/src/robocoop_backend/robocoop_backend/tests/unit/app/test_websocket_handler.py @@ -7,7 +7,9 @@ MSG_ACTIVITY_HISTORY, MSG_EMERGENCY_STOP, MSG_GET_ACTIVITY, + MSG_GET_HEALTH, MSG_GET_STATE, + MSG_HEALTH_RESPONSE, MSG_INITIAL_STATE, MSG_PING, MSG_PONG, @@ -105,6 +107,20 @@ async def test_teleop_move_calls_send_velocity(self, mock_websocket): await handler.handle_message(mock_websocket, {"type": MSG_TELEOP_MOVE, "data": data}) ctx.adapter.send_velocity.assert_called_once_with(data) + async def test_get_health_returns_health_response(self, mock_websocket): + ctx = make_context() + ctx.adapter.__class__.__name__ = "MockRobotAdapter" + handler = WebSocketHandler(ctx) + import os + with __import__("unittest").mock.patch.dict(os.environ, {"ROBOCOOP_ENV": "mock"}): + await handler.handle_message(mock_websocket, {"type": MSG_GET_HEALTH}) + sent = json.loads(mock_websocket.send.call_args[0][0]) + assert sent["type"] == MSG_HEALTH_RESPONSE + assert sent["data"]["status"] == "ok" + assert sent["data"]["adapter"] == "MockRobotAdapter" + assert sent["data"]["environment"] == "mock" + assert "robot_state" in sent["data"] + async def test_unknown_type_silently_ignored(self, mock_websocket): handler = WebSocketHandler(make_context()) await handler.handle_message(mock_websocket, {"type": "does_not_exist"}) diff --git a/src/robocoop_backend/robocoop_backend/tests/unit/modules/test_rgb_chenillard.py b/src/robocoop_backend/robocoop_backend/tests/unit/modules/test_rgb_chenillard.py new file mode 100644 index 0000000..290fb38 --- /dev/null +++ b/src/robocoop_backend/robocoop_backend/tests/unit/modules/test_rgb_chenillard.py @@ -0,0 +1,29 @@ +import asyncio + +import pytest + +from robocoop_backend.modules.robot.rgb_chenillard import RgbChenillard + + +@pytest.mark.asyncio +async def test_chenillard_publishes_and_stops_with_blackout(): + published = [] + + async def capture(topic, msg_type, msg): + published.append((topic, msg_type, dict(msg))) + + c = RgbChenillard( + publish=capture, + topic="/rgb", + frame_delay=0.001, + led_min=1, + led_max=2, + ) + await c.start() + await asyncio.sleep(0.05) + await c.stop() + + assert published[0][0] == "/rgb" + assert published[0][1] == "std_msgs/msg/ColorRGBA" + assert "r" in published[0][2] and "a" in published[0][2] + assert published[-1][2] == {"r": 0.0, "g": 0.0, "b": 0.0, "a": 0.0} diff --git a/src/robocoop_backend/robocoop_backend/tests/unit/modules/test_state_store.py b/src/robocoop_backend/robocoop_backend/tests/unit/modules/test_state_store.py index 0f315c8..a87df31 100644 --- a/src/robocoop_backend/robocoop_backend/tests/unit/modules/test_state_store.py +++ b/src/robocoop_backend/robocoop_backend/tests/unit/modules/test_state_store.py @@ -18,6 +18,7 @@ def test_to_dict_contains_required_keys(self): d = RobotState().to_dict() assert "is_connected" in d assert "battery_level" in d + assert "linear_velocity" in d assert "last_updated" in d def test_to_dict_last_updated_is_iso_string(self): @@ -40,6 +41,10 @@ def test_update_sets_is_connected(self, state_store): state_store.update({"is_connected": True}) assert state_store.get().is_connected is True + def test_update_sets_linear_velocity(self, state_store): + state_store.update({"linear_velocity": 0.42}) + assert state_store.get().linear_velocity == pytest.approx(0.42) + def test_update_multiple_fields(self, state_store): state_store.update({"battery_level": 80.0, "is_connected": True}) assert state_store.get().battery_level == 80.0 diff --git a/src/robocoop_backend/robocoop_backend/tests/unit/modules/test_telemetry_service.py b/src/robocoop_backend/robocoop_backend/tests/unit/modules/test_telemetry_service.py index fe799ad..d2ec791 100644 --- a/src/robocoop_backend/robocoop_backend/tests/unit/modules/test_telemetry_service.py +++ b/src/robocoop_backend/robocoop_backend/tests/unit/modules/test_telemetry_service.py @@ -90,3 +90,79 @@ def test_battery_above_threshold_does_not_trigger(self, telemetry_service, audit telemetry_service.on_telemetry_received({"battery_level": 80.0}) actions = [e.action for e in audit_service._history] assert "battery.low" not in actions + + +@pytest.mark.unit +class TestBatteryCriticalThreshold: + @pytest.fixture + def critical_telemetry(self, state_store, audit_service): + return TelemetryService( + robot_state_store=state_store, + audit_service=audit_service, + battery_warning_threshold=20.0, + battery_critical_threshold=10.0, + ) + + def test_battery_critical_fires_below_threshold(self, critical_telemetry, audit_service): + critical_telemetry.on_telemetry_received({"battery_level": 8.0}) + actions = [e.action for e in audit_service._history] + assert "battery.critical" in actions + + def test_battery_critical_fires_only_once(self, critical_telemetry, audit_service): + critical_telemetry.on_telemetry_received({"battery_level": 8.0}) + critical_telemetry.on_telemetry_received({"battery_level": 5.0}) + critical_events = [e for e in audit_service._history if e.action == "battery.critical"] + assert len(critical_events) == 1 + + def test_battery_critical_and_low_on_same_reading(self, critical_telemetry, audit_service): + critical_telemetry.on_telemetry_received({"battery_level": 8.0}) + actions = [e.action for e in audit_service._history] + assert "battery.critical" in actions + assert "battery.low" in actions + + def test_battery_critical_resets_after_recovery(self, critical_telemetry, audit_service): + critical_telemetry.on_telemetry_received({"battery_level": 8.0}) + critical_telemetry.on_telemetry_received({"battery_level": 15.0}) + critical_telemetry.on_telemetry_received({"battery_level": 7.0}) + critical_events = [e for e in audit_service._history if e.action == "battery.critical"] + assert len(critical_events) == 2 + + def test_battery_at_critical_threshold_does_not_trigger(self, critical_telemetry, audit_service): + critical_telemetry.on_telemetry_received({"battery_level": 10.0}) + actions = [e.action for e in audit_service._history] + assert "battery.critical" not in actions + + +@pytest.mark.unit +class TestVelocityThreshold: + @pytest.fixture + def velocity_telemetry(self, state_store, audit_service): + return TelemetryService( + robot_state_store=state_store, + audit_service=audit_service, + velocity_warning_threshold=1.0, + ) + + def test_velocity_high_fires_above_threshold(self, velocity_telemetry, audit_service, state_store): + velocity_telemetry.on_telemetry_received({"linear_velocity": 1.5}) + actions = [e.action for e in audit_service._history] + assert "velocity.high" in actions + assert state_store.get().linear_velocity == pytest.approx(1.5) + + def test_velocity_high_fires_only_once(self, velocity_telemetry, audit_service): + velocity_telemetry.on_telemetry_received({"linear_velocity": 1.5}) + velocity_telemetry.on_telemetry_received({"linear_velocity": 2.0}) + high_events = [e for e in audit_service._history if e.action == "velocity.high"] + assert len(high_events) == 1 + + def test_velocity_high_resets_after_slowdown(self, velocity_telemetry, audit_service): + velocity_telemetry.on_telemetry_received({"linear_velocity": 1.5}) + velocity_telemetry.on_telemetry_received({"linear_velocity": 0.5}) + velocity_telemetry.on_telemetry_received({"linear_velocity": 1.8}) + high_events = [e for e in audit_service._history if e.action == "velocity.high"] + assert len(high_events) == 2 + + def test_velocity_at_threshold_does_not_trigger(self, velocity_telemetry, audit_service): + velocity_telemetry.on_telemetry_received({"linear_velocity": 1.0}) + actions = [e.action for e in audit_service._history] + assert "velocity.high" not in actions diff --git a/src/robocoop_bringup/config/README.md b/src/robocoop_bringup/config/README.md index d0ba9ce..8c8efa4 100644 --- a/src/robocoop_bringup/config/README.md +++ b/src/robocoop_bringup/config/README.md @@ -33,10 +33,30 @@ websocket: port: 8765 # dashboard connects here battery_warning_threshold: 20.0 # % — triggers battery.low audit event -battery_critical_threshold: 10.0 # % — reserved for future critical alert +battery_critical_threshold: 10.0 # % — triggers battery.critical audit event battery_watchdog_timeout_seconds: 15 # seconds without /battery → robot disconnected audit_log_path: "/var/log/robocoop/audit.jsonl" + +teleop: + max_linear_x: 0.5 # m/s — clamp for teleop.move + max_angular_z: 1.0 # rad/s — clamp for teleop.move + +rosbridge_msg_types: + cmd_vel: "geometry_msgs/msg/Twist" + battery: "std_msgs/msg/Float32" + emergency_stop: "std_msgs/msg/Bool" + odom: "nav_msgs/msg/Odometry" + rgb: "std_msgs/msg/ColorRGBA" + +rgb: + enabled: false # overridden to true in real.params.yaml + topic: "/rgb" + frame_delay_seconds: 0.07 # delay between LED steps + hue_step: 15 # rainbow shift per step (degrees) + brightness: 0.85 # 0.0–1.0 + led_min: 1 # rightmost LED index (field `a`) + led_max: 7 # leftmost LED index (field `a`) ``` ### `real.params.yaml` @@ -52,8 +72,55 @@ rosbridge: max_reconnect_attempts: 10 topics: battery: "/battery" + cmd_vel: "/cmd_vel" + emergency_stop: "/emergency_stop" + odom: "/odom" + rgb: "/rgb" + +rgb: + enabled: true # chenillard at backend connect +``` + +## RGB LED chenillard (`/rgb`) + +At startup in **real** mode, the backend publishes `std_msgs/msg/ColorRGBA` on `/rgb` via rosbridge (same transport as teleop). No local `ros2` CLI is required on the laptop. + +| Field | Meaning | +|---|---| +| `r`, `g`, `b` | Color 0–255 | +| `a` | LED selector: `0.0` = all LEDs; `1.0` = rightmost; `7.0` = leftmost | + +Example equivalent on the robot (if ROS2 is sourced there): + +```bash +ros2 topic pub --once /rgb std_msgs/msg/ColorRGBA "{r: 200.0, g: 10.0, b: 200.0, a: 1.0}" ``` +Disable without editing YAML: `ROBOCOOP_RGB_CHENILLARD=0 ./run_backend.sh real`. + +Implementation: `modules/robot/rgb_chenillard.py`, started from `rosbridge_adapter.connect()`. + +## Publish topics (commands to the robot) + +Commands from the dashboard (`teleop.move`, `emergency_stop`) are published via rosbridge. Topic names live under `rosbridge.topics`; message types under `rosbridge_msg_types` in `common.params.yaml`. + +```yaml +# real.params.yaml +rosbridge: + topics: + cmd_vel: "/cmd_vel" + emergency_stop: "/emergency_stop" + rgb: "/rgb" + +# common.params.yaml +rosbridge_msg_types: + cmd_vel: "geometry_msgs/msg/Twist" + emergency_stop: "std_msgs/msg/Bool" + rgb: "std_msgs/msg/ColorRGBA" +``` + +Confirm topic names on the robot with `ros2 topic list` before deploying. + ## How to add a new environment 1. Create `your_env.params.yaml` with only the values that differ from `common.params.yaml` diff --git a/src/robocoop_bringup/config/common.params.yaml b/src/robocoop_bringup/config/common.params.yaml index 238b8fb..b512529 100644 --- a/src/robocoop_bringup/config/common.params.yaml +++ b/src/robocoop_bringup/config/common.params.yaml @@ -7,6 +7,30 @@ websocket: battery_warning_threshold: 20.0 battery_critical_threshold: 10.0 battery_watchdog_timeout_seconds: 15.0 +velocity_warning_threshold: 1.0 # === Audit Logging === audit_log_path: "/var/log/robocoop/audit.jsonl" + +# === Teleop (dashboard → robot commands) === +teleop: + max_linear_x: 0.5 + max_angular_z: 1.0 + +# === ROS message types (defaults; override per env if needed) === +rosbridge_msg_types: + cmd_vel: "geometry_msgs/msg/Twist" + battery: "std_msgs/msg/Float32" + emergency_stop: "std_msgs/msg/Bool" + odom: "nav_msgs/msg/Odometry" + rgb: "std_msgs/msg/ColorRGBA" + +# Chenillard LED au démarrage (rosbridge uniquement ; activé en real.params.yaml) +rgb: + enabled: false + topic: "/rgb" + frame_delay_seconds: 0.07 + hue_step: 15 + brightness: 0.85 + led_min: 1 + led_max: 7 diff --git a/src/robocoop_bringup/config/real.params.yaml b/src/robocoop_bringup/config/real.params.yaml index 4539d41..bab55f2 100644 --- a/src/robocoop_bringup/config/real.params.yaml +++ b/src/robocoop_bringup/config/real.params.yaml @@ -8,3 +8,10 @@ rosbridge: max_reconnect_attempts: 10 topics: battery: "/battery" + cmd_vel: "/cmd_vel" + emergency_stop: "/emergency_stop" + odom: "/odom" + rgb: "/rgb" + +rgb: + enabled: true diff --git a/supervisor/src/robocoop_supervisor/ui/main_window.py b/supervisor/src/robocoop_supervisor/ui/main_window.py new file mode 100644 index 0000000..e69de29