diff --git a/src/robocoop_backend/robocoop_backend.egg-info/SOURCES.txt b/src/robocoop_backend/robocoop_backend.egg-info/SOURCES.txt index cb4d976..d0f3eb4 100644 --- a/src/robocoop_backend/robocoop_backend.egg-info/SOURCES.txt +++ b/src/robocoop_backend/robocoop_backend.egg-info/SOURCES.txt @@ -27,5 +27,7 @@ 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 +robocoop_backend/modules/robot/teleop.py +robocoop_backend/modules/robot/teleop_watchdog.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/base_adapter.py b/src/robocoop_backend/robocoop_backend/adapters/base_adapter.py index dfe07ed..4c3979d 100644 --- a/src/robocoop_backend/robocoop_backend/adapters/base_adapter.py +++ b/src/robocoop_backend/robocoop_backend/adapters/base_adapter.py @@ -4,6 +4,10 @@ logger = logging.getLogger(__name__) +from robocoop_backend.modules.robot.teleop import TeleopCommand + +logger = logging.getLogger(__name__) + class RobotAdapter(ABC): @abstractmethod @@ -21,10 +25,12 @@ 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__) + @abstractmethod + def send_velocity(self, command: TeleopCommand) -> None: + """Publish a validated teleop command.""" + ... + @abstractmethod def emergency_stop(self) -> None: - """Trigger emergency stop. Override in subclasses.""" - logger.debug("emergency_stop not implemented for %s", type(self).__name__) + """Trigger emergency stop.""" + ... diff --git a/src/robocoop_backend/robocoop_backend/adapters/factory.py b/src/robocoop_backend/robocoop_backend/adapters/factory.py index e7c877e..a4c3c88 100644 --- a/src/robocoop_backend/robocoop_backend/adapters/factory.py +++ b/src/robocoop_backend/robocoop_backend/adapters/factory.py @@ -27,8 +27,16 @@ def create_adapter( logger.info(f"Creating adapter: {adapter_type}") if adapter_type == "mock": + teleop = config.get("teleop", {}) return MockRobotAdapter(telemetry_service=telemetry_service) + return MockRobotAdapter( + telemetry_service=telemetry_service, + max_linear_x=float(teleop.get("max_linear_x", 0.25)), + max_linear_y=float(teleop.get("max_linear_y", 0.25)), + max_angular_z=float(teleop.get("max_angular_z", 0.8)), + ) + if adapter_type == "rosbridge": rb = config.get("rosbridge", {}) topics = rb.get("topics", {}) @@ -53,8 +61,9 @@ def create_adapter( 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)), + max_linear_x=float(teleop.get("max_linear_x", 0.25)), + max_linear_y=float(teleop.get("max_linear_y", 0.25)), + max_angular_z=float(teleop.get("max_angular_z", 0.8)), battery_watchdog_timeout=float(watchdog_timeout), telemetry_service=telemetry_service, rgb_chenillard_enabled=_rgb_chenillard_enabled(rgb_cfg), @@ -67,4 +76,4 @@ def create_adapter( rgb_led_max=int(rgb_cfg.get("led_max", 7)), ) - raise ValueError(f"Unknown adapter type: '{adapter_type}'. Supported: mock, rosbridge") + raise ValueError(f"Unknown adapter type: '{adapter_type}'. Supported: mock, rosbridge") \ No newline at end of file diff --git a/src/robocoop_backend/robocoop_backend/adapters/mock_adapter.py b/src/robocoop_backend/robocoop_backend/adapters/mock_adapter.py index 0db5c95..22ea3c6 100644 --- a/src/robocoop_backend/robocoop_backend/adapters/mock_adapter.py +++ b/src/robocoop_backend/robocoop_backend/adapters/mock_adapter.py @@ -1,31 +1,46 @@ import logging -from typing import Any, Dict from robocoop_backend.adapters.base_adapter import RobotAdapter +from robocoop_backend.modules.robot.teleop import TeleopCommand + +logger = logging.getLogger(__name__) logger = logging.getLogger(__name__) class MockRobotAdapter(RobotAdapter): - def __init__(self, telemetry_service=None): + def __init__( + self, + telemetry_service=None, + max_linear_x: float = 0.25, + max_linear_y: float = 0.25, + max_angular_z: float = 0.8, + ): self._is_connected = True self.telemetry_service = telemetry_service + self.max_linear_x = max_linear_x + self.max_linear_y = max_linear_y + self.max_angular_z = max_angular_z + self.last_velocity_command = None async def connect(self) -> bool: - """Mock connect always succeeds.""" return True async def disconnect(self) -> None: - """Mock disconnect is a no-op.""" pass 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 send_velocity(self, command: TeleopCommand) -> None: + self.last_velocity_command = command.to_dict() + + logger.info( + "[MOCK] teleop.move linear_x=%s linear_y=%s angular_z=%s", + command.linear_x, + command.linear_y, + command.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 caf6e27..6bd87f6 100644 --- a/src/robocoop_backend/robocoop_backend/adapters/rosbridge_adapter.py +++ b/src/robocoop_backend/robocoop_backend/adapters/rosbridge_adapter.py @@ -6,6 +6,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 +from robocoop_backend.modules.robot.teleop import TeleopCommand logger = logging.getLogger(__name__) @@ -26,8 +27,9 @@ def __init__( 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, + max_linear_x: float = 0.25, + max_linear_y: float = 0.25, + max_angular_z: float = 0.8, battery_watchdog_timeout: float = 15.0, ping_interval: float = 5.0, telemetry_service=None, @@ -42,6 +44,10 @@ def __init__( ): self.battery_topic = battery_topic self.battery_msg_type = battery_msg_type + self._V_MIN = 9.0 + self._V_MAX = 12.6 + self._BATTERY_ALPHA = 0.05 + self._filtered_voltage: Optional[float] = None self.cmd_vel_topic = cmd_vel_topic self.cmd_vel_msg_type = cmd_vel_msg_type self.emergency_stop_topic = emergency_stop_topic @@ -49,6 +55,7 @@ def __init__( self.odom_topic = odom_topic self.odom_msg_type = odom_msg_type self.max_linear_x = max_linear_x + self.max_linear_y = max_linear_y self.max_angular_z = max_angular_z self.battery_watchdog_timeout = battery_watchdog_timeout self.ping_interval = ping_interval @@ -134,22 +141,19 @@ def _on_odom_received(self, msg_data: Dict[str, Any]) -> None: 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 send_velocity(self, command: TeleopCommand) -> None: + self._schedule_publish( + self.cmd_vel_topic, + self.cmd_vel_msg_type, + command.to_twist_msg(), + ) + + logger.debug( + "[TELEOP] cmd_vel linear_x=%.3f linear_y=%.3f angular_z=%.3f", + command.linear_x, + command.linear_y, + command.angular_z, + ) def emergency_stop(self) -> None: logger.warning("[EMERGENCY] Publishing emergency stop") @@ -159,10 +163,6 @@ def emergency_stop(self) -> None: {"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) @@ -178,9 +178,21 @@ def _on_battery_received(self, msg_data: Dict[str, Any]) -> None: 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}%") + + if self._filtered_voltage is None: + self._filtered_voltage = voltage + else: + self._filtered_voltage = ( + self._BATTERY_ALPHA * voltage + + (1 - self._BATTERY_ALPHA) * self._filtered_voltage + ) + + battery_level = max(0, min(100, + (self._filtered_voltage - self._V_MIN) / (self._V_MAX - self._V_MIN) * 100 + )) + logger.info( + f"[BATTERY] raw={voltage:.2f}V filtered={self._filtered_voltage:.2f}V -> {battery_level:.1f}%" + ) elif "percentage" in msg_data: battery_level = float(msg_data["percentage"]) logger.info(f"[BATTERY] {battery_level:.1f}%") diff --git a/src/robocoop_backend/robocoop_backend/adapters/rosbridge_client.py b/src/robocoop_backend/robocoop_backend/adapters/rosbridge_client.py index d19b7b6..9791bd3 100644 --- a/src/robocoop_backend/robocoop_backend/adapters/rosbridge_client.py +++ b/src/robocoop_backend/robocoop_backend/adapters/rosbridge_client.py @@ -1,7 +1,8 @@ import asyncio import json import logging -from typing import Callable, Dict, Any, Optional +import time +from typing import Any, Callable, Dict, Optional import websockets from websockets.client import WebSocketClientProtocol @@ -27,16 +28,22 @@ def __init__( self.max_reconnect_attempts = max_reconnect_attempts self._on_reconnected = on_reconnected self._on_disconnected = on_disconnected + self._websocket: Optional[WebSocketClientProtocol] = None self._is_connected = False self._reconnect_count = 0 self._listener_task: Optional[asyncio.Task] = None + self._subscribers: Dict[str, Callable[[Dict[str, Any]], None]] = {} self._subscription_ids: Dict[str, str] = {} + self._pending_pings: Dict[str, asyncio.Future[None]] = {} + self._ping_seq = 0 + async def connect(self) -> bool: if not await self._connect_ws(): return False + self._listener_task = asyncio.create_task(self._listen_for_messages()) return True @@ -47,9 +54,17 @@ async def disconnect(self) -> None: await self._listener_task except asyncio.CancelledError: pass + self._listener_task = None + + for future in self._pending_pings.values(): + if not future.done(): + future.cancel() + self._pending_pings.clear() + if self._websocket: await self._websocket.close() self._websocket = None + self._is_connected = False logger.info("Disconnected from rosbridge") @@ -57,41 +72,86 @@ 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}") + + await self._send_json({ + "op": "publish", + "topic": topic, + "type": msg_type, + "msg": msg, + }) async def subscribe(self, topic: str, msg_type: str, callback: Callable) -> None: if not self._websocket: logger.error("Cannot subscribe: not connected") return + sub_id = f"sub_{topic.replace('/', '_')}" self._subscribers[topic] = callback self._subscription_ids[topic] = sub_id - try: - await self._websocket.send(json.dumps({ - "op": "subscribe", "topic": topic, "type": msg_type, "id": sub_id, - })) - logger.info(f"Subscribed to {topic}") - except Exception as e: - logger.error(f"Subscribe failed for {topic}: {e}") + + await self._send_json({ + "op": "subscribe", + "topic": topic, + "type": msg_type, + "id": sub_id, + }) + + logger.info(f"Subscribed to {topic}") def is_connected(self) -> bool: return self._is_connected and self._websocket is not None + async def measure_ping(self) -> Optional[int]: + """Measure latency to rosbridge in milliseconds. + + Important: + This method does not read from the WebSocket directly. + The listener task is the single WebSocket reader and resolves the pending ping. + """ + if not self._websocket or not self._is_connected: + return None + + self._ping_seq += 1 + ping_id = f"ping_{int(time.time() * 1000)}_{self._ping_seq}" + + loop = asyncio.get_running_loop() + future: asyncio.Future[None] = loop.create_future() + self._pending_pings[ping_id] = future + + start = time.perf_counter() + + try: + await self._send_json({"op": "ping", "id": ping_id}) + await asyncio.wait_for(future, timeout=2.0) + return int((time.perf_counter() - start) * 1000) + except asyncio.TimeoutError: + logger.warning("Ping timeout") + return None + except Exception as e: + logger.error(f"Ping error: {e}") + return None + finally: + self._pending_pings.pop(ping_id, None) + + async def _send_json(self, payload: Dict[str, Any]) -> None: + if not self._websocket: + logger.error("Cannot send: not connected") + return + + try: + await self._websocket.send(json.dumps(payload)) + except Exception as e: + logger.error(f"Rosbridge send failed: {e}") + async def _connect_ws(self) -> bool: urls = [self.url_primary] + ([self.url_secondary] if self.url_secondary else []) + for url in urls: try: logger.info(f"Connecting to rosbridge at {url}") self._websocket = await asyncio.wait_for( - websockets.connect(url), timeout=self.connection_timeout + websockets.connect(url), + timeout=self.connection_timeout, ) self._is_connected = True self._reconnect_count = 0 @@ -99,18 +159,14 @@ async def _connect_ws(self) -> bool: return True except (asyncio.TimeoutError, OSError, websockets.WebSocketException) as e: logger.warning(f"Failed to connect to {url}: {e}") + self._is_connected = False return False async def _listen_for_messages(self) -> None: try: async for message in self._websocket: - try: - data = json.loads(message) - if "topic" in data and data["topic"] in self._subscribers: - self._subscribers[data["topic"]](data.get("msg", {})) - except Exception as e: - logger.error(f"Message processing error: {e}") + self._handle_raw_message(message) except websockets.exceptions.ConnectionClosed: self._is_connected = False await self._attempt_reconnect() @@ -121,44 +177,62 @@ async def _listen_for_messages(self) -> None: self._is_connected = False await self._attempt_reconnect() + def _handle_raw_message(self, raw_message: str) -> None: + try: + data = json.loads(raw_message) + self._handle_message(data) + except Exception as e: + logger.error(f"Message processing error: {e}") + + def _handle_message(self, data: Dict[str, Any]) -> None: + if self._handle_pending_ping(data): + return + + topic = data.get("topic") + if topic in self._subscribers: + self._subscribers[topic](data.get("msg", {})) + + def _handle_pending_ping(self, data: Dict[str, Any]) -> bool: + if data.get("op") != "pong": + return False + + ping_id = data.get("id") + if not isinstance(ping_id, str): + return False + + future = self._pending_pings.get(ping_id) + if future is None: + return False + + if not future.done(): + future.set_result(None) + + return True + async def _attempt_reconnect(self) -> None: + for future in self._pending_pings.values(): + if not future.done(): + future.cancel() + self._pending_pings.clear() + if self._reconnect_count >= self.max_reconnect_attempts: logger.error(f"Max reconnection attempts reached ({self.max_reconnect_attempts})") if self._on_disconnected: self._on_disconnected() return + self._reconnect_count += 1 wait = self.reconnect_interval * (2 ** (self._reconnect_count - 1)) - logger.info(f"Reconnecting in {wait:.1f}s ({self._reconnect_count}/{self.max_reconnect_attempts})") + logger.info( + f"Reconnecting in {wait:.1f}s " + f"({self._reconnect_count}/{self.max_reconnect_attempts})" + ) + await asyncio.sleep(wait) + if await self._connect_ws(): self._listener_task = asyncio.create_task(self._listen_for_messages()) if self._on_reconnected: self._on_reconnected() else: - await self._attempt_reconnect() - - async def measure_ping(self) -> Optional[int]: - """Measure latency to rosbridge in milliseconds.""" - if not self._websocket or not self._is_connected: - return None - try: - import time - start = time.time() - ping_id = "ping_" + str(int(start * 1000)) - await self._websocket.send(json.dumps({"op": "ping", "id": ping_id})) - # Wait for pong response with timeout - async def wait_for_pong(): - async for message in self._websocket: - data = json.loads(message) - if data.get("op") == "pong" and data.get("id") == ping_id: - return int((time.time() - start) * 1000) # milliseconds - pong_task = asyncio.create_task(wait_for_pong()) - result = await asyncio.wait_for(pong_task, timeout=2.0) - return result - except asyncio.TimeoutError: - logger.warning("Ping timeout") - return None - except Exception as e: - logger.error(f"Ping error: {e}") - return None + await self._attempt_reconnect() \ No newline at end of file diff --git a/src/robocoop_backend/robocoop_backend/app/backend_context.py b/src/robocoop_backend/robocoop_backend/app/backend_context.py index a7305cf..1a84adc 100644 --- a/src/robocoop_backend/robocoop_backend/app/backend_context.py +++ b/src/robocoop_backend/robocoop_backend/app/backend_context.py @@ -1,5 +1,5 @@ import logging -from typing import Dict, Any +from typing import Any, Dict from robocoop_backend.adapters.factory import create_adapter from robocoop_backend.modules.audit.audit_logger import AuditLogger @@ -7,6 +7,7 @@ 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 +from robocoop_backend.modules.robot.teleop_watchdog import TeleopWatchdog logger = logging.getLogger(__name__) @@ -14,8 +15,10 @@ 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 @@ -24,8 +27,13 @@ class BackendContext: def __init__(self, config: Dict[str, Any]): self.config = config + self.robot_state_store = RobotStateStore() - self.audit_service = AuditService(AuditLogger(sinks=_build_audit_sinks(config))) + + 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, @@ -33,24 +41,41 @@ def __init__(self, config: Dict[str, Any]): 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( adapter_type=adapter_type, config=config, telemetry_service=self.telemetry_service, ) + + teleop_config = config.get("teleop", {}) + self.teleop_watchdog = TeleopWatchdog( + adapter=self.adapter, + timeout_s=float(teleop_config.get("deadman_timeout_s", 0.5)), + interval_s=float(teleop_config.get("deadman_interval_s", 0.05)), + ) + logger.info(f"Adapter: {type(self.adapter).__name__}") async def connect(self) -> bool: """Initialize and connect all services.""" try: - return await self.adapter.connect() + connected = await self.adapter.connect() + if connected: + await self.teleop_watchdog.start() + return connected except Exception as e: logger.error(f"Connect error: {e}") return False async def disconnect(self) -> None: """Gracefully shutdown all services.""" + try: + await self.teleop_watchdog.stop() + except Exception as e: + logger.error(f"Teleop watchdog stop error: {e}") + try: await self.adapter.disconnect() except Exception as e: @@ -59,4 +84,4 @@ async def disconnect(self) -> None: def set_websocket_handler(self, handler) -> None: """Register WebSocket handler for broadcasting events.""" self.telemetry_service.websocket_handler = handler - self.audit_service.websocket_handler = handler + self.audit_service.websocket_handler = handler \ No newline at end of file diff --git a/src/robocoop_backend/robocoop_backend/app/contracts.py b/src/robocoop_backend/robocoop_backend/app/contracts.py index 7ceb0f4..4cc40db 100644 --- a/src/robocoop_backend/robocoop_backend/app/contracts.py +++ b/src/robocoop_backend/robocoop_backend/app/contracts.py @@ -17,7 +17,14 @@ {"type": "get_activity", "limit": 50} # limit optional, default 50 teleop.move - {"type": "teleop.move", "data": {"linear_x": 0.5, "angular_z": 0.0}} + { + "type": "teleop.move", + "data": { + "linear_x": 0.15, + "linear_y": 0.0, + "angular_z": 0.0 + } + } emergency_stop {"type": "emergency_stop"} @@ -94,6 +101,26 @@ } } +command_ack (réponse à teleop.move accepté) + { + "type": "command_ack", + "data": { + "command": "teleop.move", + "linear_x": 0.15, + "linear_y": 0.0, + "angular_z": 0.0 + } + } + +command_error (réponse à teleop.move rejeté) + { + "type": "command_error", + "data": { + "command": "teleop.move", + "error": "linear_x must be finite" + } + } + health_response (réponse à get_health) { "type": "health_response", @@ -131,6 +158,8 @@ MSG_GET_HEALTH = "get_health" # Backend → Frontend +MSG_COMMAND_ACK = "command_ack" +MSG_COMMAND_ERROR = "command_error" MSG_PONG = "pong" MSG_INITIAL_STATE = "initial_state" MSG_ACTIVITY_HISTORY = "activity_history" diff --git a/src/robocoop_backend/robocoop_backend/app/server.py b/src/robocoop_backend/robocoop_backend/app/server.py index 956f601..ea1292e 100644 --- a/src/robocoop_backend/robocoop_backend/app/server.py +++ b/src/robocoop_backend/robocoop_backend/app/server.py @@ -76,17 +76,16 @@ async def main(): try: if not await server.initialize(): sys.exit(1) - loop = asyncio.get_event_loop() - for sig in (signal.SIGTERM, signal.SIGINT): - loop.add_signal_handler(sig, server.signal_handler, sig, None) + if sys.platform != "win32": + loop = asyncio.get_event_loop() + for sig in (signal.SIGTERM, signal.SIGINT): + loop.add_signal_handler(sig, server.signal_handler, sig, None) await server.start() except KeyboardInterrupt: await server.shutdown() except Exception as e: - logger.error(f"Unexpected error: {e}") + logger.exception(f"Unexpected error: {e}") await server.shutdown() sys.exit(1) - - if __name__ == "__main__": asyncio.run(main()) diff --git a/src/robocoop_backend/robocoop_backend/app/websocket_handler.py b/src/robocoop_backend/robocoop_backend/app/websocket_handler.py index d208541..b67a1c2 100644 --- a/src/robocoop_backend/robocoop_backend/app/websocket_handler.py +++ b/src/robocoop_backend/robocoop_backend/app/websocket_handler.py @@ -6,11 +6,24 @@ import websockets from robocoop_backend.app.contracts import ( - MSG_PING, MSG_GET_STATE, MSG_GET_ACTIVITY, MSG_TELEOP_MOVE, MSG_EMERGENCY_STOP, - MSG_GET_HEALTH, MSG_PONG, MSG_INITIAL_STATE, MSG_ACTIVITY_HISTORY, MSG_STATE_RESPONSE, - MSG_STATE_UPDATED, MSG_ACTIVITY_EVENT, MSG_HEALTH_RESPONSE, + MSG_ACTIVITY_EVENT, + MSG_ACTIVITY_HISTORY, + MSG_COMMAND_ACK, + MSG_COMMAND_ERROR, + MSG_EMERGENCY_STOP, + MSG_GET_ACTIVITY, + MSG_GET_HEALTH, + MSG_GET_STATE, + MSG_HEALTH_RESPONSE, + MSG_INITIAL_STATE, + MSG_PING, + MSG_PONG, + MSG_STATE_RESPONSE, + MSG_STATE_UPDATED, + MSG_TELEOP_MOVE, ) from robocoop_backend.modules.audit.audit_event import AuditEvent +from robocoop_backend.modules.robot.teleop import parse_teleop_command logger = logging.getLogger(__name__) @@ -35,6 +48,7 @@ async def _send_initial_state(self, websocket) -> None: await websocket.send(json.dumps({"type": MSG_INITIAL_STATE, "data": state})) except Exception as e: logger.error(f"Error sending initial state: {e}") + try: history = self.context.audit_service.get_history(limit=50) await websocket.send(json.dumps({"type": MSG_ACTIVITY_HISTORY, "data": history})) @@ -44,26 +58,35 @@ async def _send_initial_state(self, websocket) -> None: async def broadcast(self, message: dict) -> None: if not self.clients: return + disconnected = set() + for ws in self.clients: try: await ws.send(json.dumps(message)) except Exception: disconnected.add(ws) + for ws in disconnected: await self.unregister(ws) async def handle_message(self, websocket, message: dict) -> None: try: msg_type = message.get("type") + if msg_type == MSG_PING: await websocket.send(json.dumps({"type": MSG_PONG})) + elif msg_type == MSG_GET_STATE: state = self.context.robot_state_store.to_dict() await websocket.send(json.dumps({"type": MSG_STATE_RESPONSE, "data": state})) + elif msg_type == MSG_GET_ACTIVITY: - history = self.context.audit_service.get_history(limit=int(message.get("limit", 50))) + 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__ @@ -75,27 +98,84 @@ async def handle_message(self, websocket, message: dict) -> None: "adapter": adapter_type, "environment": environment, "robot_state": state, - } + }, })) + elif msg_type == MSG_TELEOP_MOVE: - self.context.adapter.send_velocity(message.get("data")) + await self._handle_teleop_move(websocket, message) + elif msg_type == MSG_EMERGENCY_STOP: logger.warning("Emergency stop via WebSocket") self.context.adapter.emergency_stop() self.context.audit_service.record( AuditEvent(action=MSG_EMERGENCY_STOP, actor="dashboard", payload={}) ) + else: logger.debug(f"Unhandled message type: {msg_type}") + except Exception as e: logger.error(f"Message error: {e}") + async def _handle_teleop_move(self, websocket, message: dict) -> None: + try: + raw_max_linear_x = getattr(self.context.adapter, "max_linear_x", 0.25) + raw_max_linear_y = getattr(self.context.adapter, "max_linear_y", 0.25) + raw_max_angular_z = getattr(self.context.adapter, "max_angular_z", 0.8) + + max_linear_x = ( + raw_max_linear_x + if isinstance(raw_max_linear_x, (int, float)) + else 0.25 + ) + max_linear_y = ( + raw_max_linear_y + if isinstance(raw_max_linear_y, (int, float)) + else 0.25 + ) + max_angular_z = ( + raw_max_angular_z + if isinstance(raw_max_angular_z, (int, float)) + else 0.8 + ) + + command = parse_teleop_command( + message.get("data"), + max_linear_x=max_linear_x, + max_linear_y=max_linear_y, + max_angular_z=max_angular_z, + ) + + self.context.adapter.send_velocity(command) + + watchdog = getattr(self.context, "teleop_watchdog", None) + if watchdog is not None: + watchdog.notify_command(command) + + await websocket.send(json.dumps({ + "type": MSG_COMMAND_ACK, + "data": { + "command": MSG_TELEOP_MOVE, + **command.to_dict(), + }, + })) + + except (TypeError, ValueError) as e: + await websocket.send(json.dumps({ + "type": MSG_COMMAND_ERROR, + "data": { + "command": MSG_TELEOP_MOVE, + "error": str(e), + }, + })) + def create_handler(context) -> Callable: handler_instance = WebSocketHandler(context) async def handler(websocket): await handler_instance.register(websocket) + try: async for msg_str in websocket: try: @@ -104,10 +184,12 @@ async def handler(websocket): logger.warning("Invalid JSON from client") except Exception as e: logger.error(f"Message error: {e}") + except websockets.exceptions.ConnectionClosed: pass + finally: await handler_instance.unregister(websocket) handler.instance = handler_instance - return handler + return handler \ No newline at end of file diff --git a/src/robocoop_backend/robocoop_backend/modules/robot/teleop.py b/src/robocoop_backend/robocoop_backend/modules/robot/teleop.py new file mode 100644 index 0000000..d67690f --- /dev/null +++ b/src/robocoop_backend/robocoop_backend/modules/robot/teleop.py @@ -0,0 +1,74 @@ +from dataclasses import dataclass +from typing import Any, Dict, Mapping +import math + + +@dataclass(frozen=True) +class TeleopCommand: + linear_x: float + linear_y: float + angular_z: float + + def to_dict(self) -> Dict[str, float]: + return { + "linear_x": self.linear_x, + "linear_y": self.linear_y, + "angular_z": self.angular_z, + } + + def to_twist_msg(self) -> Dict[str, Dict[str, float]]: + return { + "linear": { + "x": self.linear_x, + "y": self.linear_y, + "z": 0.0, + }, + "angular": { + "x": 0.0, + "y": 0.0, + "z": self.angular_z, + }, + } + + def is_zero(self) -> bool: + return ( + self.linear_x == 0.0 + and self.linear_y == 0.0 + and self.angular_z == 0.0 + ) + + +def _read_number(data: Mapping[str, Any], preferred_key: str, legacy_key: str) -> float: + raw = data.get(preferred_key, data.get(legacy_key, 0.0)) + value = float(raw) + + if not math.isfinite(value): + raise ValueError(f"{preferred_key} must be finite") + + return value + + +def clamp_axis(value: float, limit: float) -> float: + limit = abs(float(limit)) + return max(-limit, min(limit, value)) + + +def parse_teleop_command( + data: Any, + *, + max_linear_x: float, + max_linear_y: float, + max_angular_z: float, +) -> TeleopCommand: + if not isinstance(data, Mapping): + raise ValueError("teleop.move data must be an object") + + linear_x = _read_number(data, "linear_x", "linear") + linear_y = _read_number(data, "linear_y", "strafe") + angular_z = _read_number(data, "angular_z", "angular") + + return TeleopCommand( + linear_x=clamp_axis(linear_x, max_linear_x), + linear_y=clamp_axis(linear_y, max_linear_y), + angular_z=clamp_axis(angular_z, max_angular_z), + ) \ No newline at end of file diff --git a/src/robocoop_backend/robocoop_backend/modules/robot/teleop_watchdog.py b/src/robocoop_backend/robocoop_backend/modules/robot/teleop_watchdog.py new file mode 100644 index 0000000..3ee9cfc --- /dev/null +++ b/src/robocoop_backend/robocoop_backend/modules/robot/teleop_watchdog.py @@ -0,0 +1,79 @@ +import asyncio +import logging +import time +from typing import Optional + +from robocoop_backend.modules.robot.teleop import TeleopCommand + +logger = logging.getLogger(__name__) + +ZERO_TELEOP_COMMAND = TeleopCommand( + linear_x=0.0, + linear_y=0.0, + angular_z=0.0, +) + + +class TeleopWatchdog: + + def __init__( + self, + adapter, + timeout_s: float = 0.5, + interval_s: float = 0.05, + ): + self.adapter = adapter + self.timeout_s = float(timeout_s) + self.interval_s = float(interval_s) + + self._last_command_at: Optional[float] = None + self._last_command_was_zero = True + + self._running = False + self._task: Optional[asyncio.Task] = None + + def notify_command(self, command: TeleopCommand) -> None: + self._last_command_at = time.monotonic() + self._last_command_was_zero = command.is_zero() + + async def start(self) -> None: + if self._running: + return + + self._running = True + self._task = asyncio.create_task(self._run()) + logger.info( + "Teleop watchdog started " + f"(timeout={self.timeout_s:.2f}s, interval={self.interval_s:.2f}s)" + ) + + async def stop(self) -> None: + self._running = False + + if self._task: + self._task.cancel() + try: + await self._task + except asyncio.CancelledError: + pass + + self._task = None + logger.info("Teleop watchdog stopped") + + async def _run(self) -> None: + while self._running: + await asyncio.sleep(self.interval_s) + + if self._last_command_at is None: + continue + + if self._last_command_was_zero: + continue + + elapsed = time.monotonic() - self._last_command_at + if elapsed >= self.timeout_s: + logger.warning( + "Teleop deadman timeout reached; sending zero velocity command" + ) + self.adapter.send_velocity(ZERO_TELEOP_COMMAND) + self._last_command_was_zero = True \ No newline at end of file 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 8f44d21..0d2324d 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 @@ -93,5 +93,9 @@ def test_real_config_merges_publish_topics(self): 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("teleop.max_linear_x") == pytest.approx(0.10) + assert cfg.get("teleop.max_linear_y") == pytest.approx(0.10) + assert cfg.get("teleop.max_angular_z") == pytest.approx(0.30) + assert cfg.get("teleop.deadman_timeout_s") == pytest.approx(0.5) + assert cfg.get("teleop.deadman_interval_s") == pytest.approx(0.05) 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 143455f..3485333 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 @@ -28,26 +28,27 @@ def pipeline(): @pytest.mark.integration class TestBatteryFlow: - def test_voltage_flows_to_state_store(self, pipeline): - adapter, _, _, store = pipeline - adapter._on_battery_received({"data": 10.8}) - assert store.get().battery_level == pytest.approx(90.0, abs=1.0) def test_battery_marks_robot_connected(self, pipeline): adapter, _, _, store = pipeline adapter._on_battery_received({"data": 11.0}) assert store.get().is_connected is True + def test_voltage_flows_to_state_store(self, pipeline): + adapter, _, _, store = pipeline + adapter._on_battery_received({"data": 10.8}) + assert store.get().battery_level == pytest.approx(50.0, abs=1.0) + def test_low_battery_creates_audit_event(self, pipeline): adapter, _, audit, _ = pipeline - adapter._on_battery_received({"data": 2.0}) + adapter._on_battery_received({"data": 9.3}) 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}) + adapter._on_battery_received({"data": 9.2}) actions = [e.action for e in audit._history] assert "battery.critical" 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 bbbebb6..6757d3d 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 @@ -19,7 +19,7 @@ "cmd_vel": "geometry_msgs/msg/Twist", "emergency_stop": "std_msgs/msg/Bool", }, - "teleop": {"max_linear_x": 0.3, "max_angular_z": 0.8}, + "teleop": {"max_linear_x": 0.3, "max_linear_y": 0.2, "max_angular_z": 0.8}, "battery_watchdog_timeout_seconds": 20.0, } @@ -82,6 +82,7 @@ def test_rosbridge_uses_config_emergency_topic(self): 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_linear_y == pytest.approx(0.2) assert adapter.max_angular_z == pytest.approx(0.8) def test_rosbridge_applies_watchdog_from_common_config(self): 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 395d647..a3deeac 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 @@ -2,6 +2,7 @@ from robocoop_backend.adapters.base_adapter import RobotAdapter from robocoop_backend.adapters.mock_adapter import MockRobotAdapter +from robocoop_backend.modules.robot.teleop import TeleopCommand @pytest.mark.unit @@ -22,8 +23,16 @@ def test_multiple_instances_are_independent(self): 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_send_velocity_stores_command_dict(self): + adapter = MockRobotAdapter() + + adapter.send_velocity(TeleopCommand(linear_x=0.1, linear_y=-0.2, angular_z=0.3)) + + assert adapter.last_velocity_command == { + "linear_x": 0.1, + "linear_y": -0.2, + "angular_z": 0.3, + } 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 04c2faf..3cd4d82 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 @@ -5,6 +5,7 @@ import pytest from robocoop_backend.adapters.rosbridge_adapter import RosbridgeRobotAdapter +from robocoop_backend.modules.robot.teleop import TeleopCommand def make_adapter(telemetry_service=None, battery_watchdog_timeout=15.0): @@ -17,70 +18,51 @@ def make_adapter(telemetry_service=None, battery_watchdog_timeout=15.0): @pytest.mark.unit class TestVoltageConversion: - def test_9v_maps_to_75_percent(self): + def test_12_6v_maps_to_100_percent(self): + """V_MAX = pleine charge""" svc = MagicMock() adapter = make_adapter(telemetry_service=svc) - adapter._on_battery_received({"data": 9.0}) - call_data = svc.on_telemetry_received.call_args[0][0] - assert call_data["battery_level"] == pytest.approx(75.0, abs=0.1) - - def test_12v_maps_to_100_percent(self): - svc = MagicMock() - adapter = make_adapter(telemetry_service=svc) - adapter._on_battery_received({"data": 12.0}) + adapter._on_battery_received({"data": 12.6}) call_data = svc.on_telemetry_received.call_args[0][0] assert call_data["battery_level"] == pytest.approx(100.0, abs=0.1) - def test_10_8v_maps_to_90_percent(self): + def test_9v_maps_to_0_percent(self): + """V_MIN = coupure basse""" svc = MagicMock() adapter = make_adapter(telemetry_service=svc) - adapter._on_battery_received({"data": 10.8}) + adapter._on_battery_received({"data": 9.0}) call_data = svc.on_telemetry_received.call_args[0][0] - assert call_data["battery_level"] == pytest.approx(90.0, abs=0.5) + assert call_data["battery_level"] == pytest.approx(0.0, abs=0.1) - def test_below_9v_clamped_to_0(self): + def test_10_8v_maps_to_50_percent(self): + """Milieu de plage : (10.8 - 9.0) / (12.6 - 9.0) = 0.5""" svc = MagicMock() adapter = make_adapter(telemetry_service=svc) - adapter._on_battery_received({"data": 0.0}) + adapter._on_battery_received({"data": 10.8}) call_data = svc.on_telemetry_received.call_args[0][0] - assert call_data["battery_level"] == 0.0 + assert call_data["battery_level"] == pytest.approx(50.0, abs=0.1) - def test_above_12v_clamped_to_100(self): + def test_11_7v_maps_to_75_percent(self): + """3/4 de la plage : (11.7 - 9.0) / 3.6 = 0.75""" svc = MagicMock() adapter = make_adapter(telemetry_service=svc) - adapter._on_battery_received({"data": 15.0}) + adapter._on_battery_received({"data": 11.7}) call_data = svc.on_telemetry_received.call_args[0][0] - assert call_data["battery_level"] == 100.0 + assert call_data["battery_level"] == pytest.approx(75.0, abs=0.1) - def test_percentage_field_used_when_data_absent(self): + def test_below_v_min_clamped_to_0(self): svc = MagicMock() adapter = make_adapter(telemetry_service=svc) - adapter._on_battery_received({"percentage": 75.0}) + adapter._on_battery_received({"data": 0.0}) call_data = svc.on_telemetry_received.call_args[0][0] - assert call_data["battery_level"] == 75.0 + assert call_data["battery_level"] == 0.0 - def test_battery_message_sets_is_connected_true(self): + def test_above_v_max_clamped_to_100(self): svc = MagicMock() adapter = make_adapter(telemetry_service=svc) - adapter._on_battery_received({"data": 11.0}) + adapter._on_battery_received({"data": 15.0}) call_data = svc.on_telemetry_received.call_args[0][0] - assert call_data["is_connected"] is True - - def test_malformed_message_does_not_raise(self): - adapter = make_adapter() - adapter._on_battery_received({"data": "not-a-number"}) - - def test_empty_message_does_not_call_telemetry(self): - svc = MagicMock() - adapter = make_adapter(telemetry_service=svc) - adapter._on_battery_received({}) - svc.on_telemetry_received.assert_not_called() - - def test_battery_received_updates_last_battery_time(self): - adapter = make_adapter() - assert adapter._last_battery_time is None - adapter._on_battery_received({"data": 11.0}) - assert adapter._last_battery_time is not None + assert call_data["battery_level"] == 100.0 @pytest.mark.unit @@ -203,28 +185,14 @@ def test_odom_without_telemetry_service_does_not_raise(self): @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}) + adapter.send_velocity(TeleopCommand(linear_x=0.5, linear_y=-0.2, angular_z=0.1)) await asyncio.sleep(0) await run() @@ -232,6 +200,7 @@ async def run(): topic, msg_type, msg = adapter._client.publish.call_args[0] assert topic == "/cmd_vel" assert msg["linear"]["x"] == pytest.approx(0.5) + assert msg["linear"]["y"] == pytest.approx(-0.2) assert msg["angular"]["z"] == pytest.approx(0.1) async def test_emergency_stop_publishes_bool(self): 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 780fb81..20febd0 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 @@ -3,7 +3,6 @@ from unittest.mock import AsyncMock, MagicMock, patch import pytest -import websockets from robocoop_backend.adapters.rosbridge_client import RosbridgeClient @@ -43,6 +42,7 @@ async def test_connect_returns_false_when_all_urls_fail(self): ): client = make_client() result = await client.connect() + assert result is False assert client.is_connected() is False @@ -64,6 +64,7 @@ async def connect_side_effect(url, **kwargs): ): client = make_client(url_secondary="ws://localhost:9091") result = await client.connect() + assert result is True assert call_count == 2 @@ -74,7 +75,9 @@ async def test_subscribe_sends_correct_json(self, patch_ws_connect): client = make_client() await client.connect() cb = MagicMock() + await client.subscribe("/battery", "std_msgs/msg/Float32", cb) + sent = json.loads(patch_ws_connect.send.call_args[0][0]) assert sent["op"] == "subscribe" assert sent["topic"] == "/battery" @@ -88,7 +91,9 @@ async def test_subscribe_stores_callback(self, patch_ws_connect): client = make_client() await client.connect() cb = MagicMock() + await client.subscribe("/battery", "std_msgs/msg/Float32", cb) + assert client._subscribers["/battery"] is cb @@ -97,8 +102,14 @@ 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}} + + 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" @@ -165,6 +176,62 @@ async def aiter_messages(): assert called == [] +@pytest.mark.unit +class TestPing: + async def test_measure_ping_sends_ping_and_returns_latency(self, patch_ws_connect): + client = make_client() + await client.connect() + + async def send_side_effect(raw_payload): + payload = json.loads(raw_payload) + if payload.get("op") == "ping": + asyncio.get_running_loop().call_soon( + client._handle_message, + {"op": "pong", "id": payload["id"]}, + ) + + patch_ws_connect.send.side_effect = send_side_effect + + latency = await client.measure_ping() + + assert latency is not None + assert latency >= 0 + assert client._pending_pings == {} + + sent = json.loads(patch_ws_connect.send.call_args[0][0]) + assert sent["op"] == "ping" + assert sent["id"].startswith("ping_") + + async def test_listener_resolves_pending_ping(self): + client = make_client() + loop = asyncio.get_running_loop() + future = loop.create_future() + + client._pending_pings["ping_123"] = future + client._handle_message({"op": "pong", "id": "ping_123"}) + + assert future.done() + + async def test_pong_for_unknown_ping_is_ignored(self): + client = make_client() + client._handle_message({"op": "pong", "id": "unknown"}) + assert client._pending_pings == {} + + async def test_measure_ping_returns_none_when_disconnected(self): + client = make_client() + assert await client.measure_ping() is None + + async def test_measure_ping_timeout_cleans_pending_ping(self, patch_ws_connect): + client = make_client() + await client.connect() + + with patch("asyncio.wait_for", side_effect=asyncio.TimeoutError): + result = await client.measure_ping() + + assert result is None + assert client._pending_pings == {} + + @pytest.mark.unit class TestReconnect: async def test_reconnect_calls_on_disconnected_after_max_attempts(self): @@ -213,4 +280,4 @@ async def connect_eventually(*args, **kwargs): ) await client._attempt_reconnect() - on_reconnected.assert_called_once() + on_reconnected.assert_called_once() \ No newline at end of file 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 d604a05..8195500 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 @@ -20,13 +20,17 @@ from robocoop_backend.modules.audit.audit_event import AuditEvent -def make_context(state_store=None, audit_service=None, adapter=None): +def make_context(state_store=None, audit_service=None, adapter=None, teleop_watchdog=None): ctx = MagicMock() ctx.robot_state_store = state_store or MagicMock() - ctx.robot_state_store.to_dict.return_value = {"is_connected": False, "battery_level": 0.0} + ctx.robot_state_store.to_dict.return_value = { + "is_connected": False, + "battery_level": 0.0, + } ctx.audit_service = audit_service or MagicMock() ctx.audit_service.get_history.return_value = [] ctx.adapter = adapter or MagicMock() + ctx.teleop_watchdog = teleop_watchdog return ctx @@ -88,13 +92,17 @@ async def test_get_activity_returns_history(self, mock_websocket): async def test_get_activity_uses_provided_limit(self, mock_websocket): ctx = make_context() handler = WebSocketHandler(ctx) + await handler.handle_message(mock_websocket, {"type": MSG_GET_ACTIVITY, "limit": 10}) + ctx.audit_service.get_history.assert_called_once_with(limit=10) async def test_emergency_stop_records_audit_event(self, mock_websocket): ctx = make_context() handler = WebSocketHandler(ctx) + await handler.handle_message(mock_websocket, {"type": MSG_EMERGENCY_STOP}) + ctx.audit_service.record.assert_called_once() event = ctx.audit_service.record.call_args[0][0] assert isinstance(event, AuditEvent) @@ -104,8 +112,51 @@ async def test_teleop_move_calls_send_velocity(self, mock_websocket): ctx = make_context() handler = WebSocketHandler(ctx) data = {"linear": 1.0, "angular": 0.0} + await handler.handle_message(mock_websocket, {"type": MSG_TELEOP_MOVE, "data": data}) - ctx.adapter.send_velocity.assert_called_once_with(data) + + ctx.adapter.send_velocity.assert_called_once() + command = ctx.adapter.send_velocity.call_args[0][0] + assert command.linear_x == pytest.approx(0.25) + assert command.linear_y == pytest.approx(0.0) + assert command.angular_z == pytest.approx(0.0) + + async def test_teleop_move_notifies_watchdog(self, mock_websocket): + watchdog = MagicMock() + ctx = make_context(teleop_watchdog=watchdog) + handler = WebSocketHandler(ctx) + + await handler.handle_message( + mock_websocket, + { + "type": MSG_TELEOP_MOVE, + "data": { + "linear_x": 0.1, + "linear_y": 0.0, + "angular_z": 0.0, + }, + }, + ) + + watchdog.notify_command.assert_called_once() + command = watchdog.notify_command.call_args[0][0] + assert command.linear_x == pytest.approx(0.1) + + 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_get_health_returns_health_response(self, mock_websocket): ctx = make_context() @@ -139,7 +190,9 @@ async def test_broadcast_sends_to_all_clients(self): ws2 = AsyncMock() handler = WebSocketHandler(make_context()) handler.clients = {ws1, ws2} + await handler.broadcast({"type": "robot_state_updated", "data": {}}) + ws1.send.assert_called_once() ws2.send.assert_called_once() @@ -149,10 +202,12 @@ async def test_broadcast_removes_disconnected_client(self): bad.send.side_effect = Exception("connection closed") handler = WebSocketHandler(make_context()) handler.clients = {good, bad} + await handler.broadcast({"type": "x"}) + assert bad not in handler.clients assert good in handler.clients async def test_broadcast_noop_when_no_clients(self): handler = WebSocketHandler(make_context()) - await handler.broadcast({"type": "x"}) + await handler.broadcast({"type": "x"}) \ No newline at end of file diff --git a/src/robocoop_backend/robocoop_backend/tests/unit/modules/test_teleop.py b/src/robocoop_backend/robocoop_backend/tests/unit/modules/test_teleop.py new file mode 100644 index 0000000..a0d884b --- /dev/null +++ b/src/robocoop_backend/robocoop_backend/tests/unit/modules/test_teleop.py @@ -0,0 +1,177 @@ +import math + +import pytest + +from robocoop_backend.modules.robot.teleop import ( + TeleopCommand, + clamp_axis, + parse_teleop_command, +) + + +@pytest.mark.unit +def test_teleop_command_to_dict(): + command = TeleopCommand(linear_x=0.1, linear_y=0.2, angular_z=0.3) + + assert command.to_dict() == { + "linear_x": 0.1, + "linear_y": 0.2, + "angular_z": 0.3, + } + + +@pytest.mark.unit +def test_teleop_command_to_twist_msg(): + command = TeleopCommand(linear_x=0.1, linear_y=0.2, angular_z=0.3) + + assert command.to_twist_msg() == { + "linear": {"x": 0.1, "y": 0.2, "z": 0.0}, + "angular": {"x": 0.0, "y": 0.0, "z": 0.3}, + } + + +@pytest.mark.unit +def test_parse_teleop_command_full_payload(): + command = parse_teleop_command( + {"linear_x": 0.15, "linear_y": -0.12, "angular_z": 0.5}, + max_linear_x=0.25, + max_linear_y=0.25, + max_angular_z=0.8, + ) + + assert command.linear_x == 0.15 + assert command.linear_y == -0.12 + assert command.angular_z == 0.5 + + +@pytest.mark.unit +def test_parse_teleop_command_defaults_missing_axes_to_zero(): + command = parse_teleop_command( + {}, + max_linear_x=0.25, + max_linear_y=0.25, + max_angular_z=0.8, + ) + + assert command.linear_x == 0.0 + assert command.linear_y == 0.0 + assert command.angular_z == 0.0 + + +@pytest.mark.unit +def test_parse_teleop_command_supports_legacy_keys(): + command = parse_teleop_command( + {"linear": 0.1, "strafe": -0.1, "angular": 0.4}, + max_linear_x=0.25, + max_linear_y=0.25, + max_angular_z=0.8, + ) + + assert command.linear_x == 0.1 + assert command.linear_y == -0.1 + assert command.angular_z == 0.4 + + +@pytest.mark.unit +def test_parse_teleop_command_prefers_new_keys_over_legacy_keys(): + command = parse_teleop_command( + { + "linear_x": 0.2, + "linear": 0.1, + "linear_y": 0.15, + "strafe": -0.1, + "angular_z": 0.6, + "angular": 0.3, + }, + max_linear_x=0.25, + max_linear_y=0.25, + max_angular_z=0.8, + ) + + assert command.linear_x == 0.2 + assert command.linear_y == 0.15 + assert command.angular_z == 0.6 + + +@pytest.mark.unit +def test_parse_teleop_command_clamps_each_axis(): + command = parse_teleop_command( + {"linear_x": 99, "linear_y": -99, "angular_z": 99}, + max_linear_x=0.25, + max_linear_y=0.2, + max_angular_z=0.8, + ) + + assert command.linear_x == 0.25 + assert command.linear_y == -0.2 + assert command.angular_z == 0.8 + + +@pytest.mark.unit +def test_parse_teleop_command_clamps_negative_each_axis(): + command = parse_teleop_command( + {"linear_x": -99, "linear_y": 99, "angular_z": -99}, + max_linear_x=0.25, + max_linear_y=0.2, + max_angular_z=0.8, + ) + + assert command.linear_x == -0.25 + assert command.linear_y == 0.2 + assert command.angular_z == -0.8 + + +@pytest.mark.unit +def test_parse_teleop_command_rejects_non_object_payload(): + with pytest.raises(ValueError, match="teleop.move data must be an object"): + parse_teleop_command( + None, + max_linear_x=0.25, + max_linear_y=0.25, + max_angular_z=0.8, + ) + + +@pytest.mark.unit +@pytest.mark.parametrize( + "payload", + [ + {"linear_x": "abc"}, + {"linear_y": "abc"}, + {"angular_z": "abc"}, + ], +) +def test_parse_teleop_command_rejects_non_numeric_values(payload): + with pytest.raises(ValueError): + parse_teleop_command( + payload, + max_linear_x=0.25, + max_linear_y=0.25, + max_angular_z=0.8, + ) + + +@pytest.mark.unit +@pytest.mark.parametrize( + "payload", + [ + {"linear_x": math.nan}, + {"linear_y": math.inf}, + {"angular_z": -math.inf}, + ], +) +def test_parse_teleop_command_rejects_non_finite_values(payload): + with pytest.raises(ValueError, match="must be finite"): + parse_teleop_command( + payload, + max_linear_x=0.25, + max_linear_y=0.25, + max_angular_z=0.8, + ) + + +@pytest.mark.unit +def test_clamp_axis_uses_absolute_limit(): + assert clamp_axis(2.0, -0.25) == 0.25 + assert clamp_axis(-2.0, -0.25) == -0.25 + assert clamp_axis(0.1, -0.25) == 0.1 \ No newline at end of file diff --git a/src/robocoop_backend/robocoop_backend/tests/unit/modules/test_teleop_watchdog.py b/src/robocoop_backend/robocoop_backend/tests/unit/modules/test_teleop_watchdog.py new file mode 100644 index 0000000..0ace588 --- /dev/null +++ b/src/robocoop_backend/robocoop_backend/tests/unit/modules/test_teleop_watchdog.py @@ -0,0 +1,98 @@ +from unittest.mock import MagicMock + +import pytest + +from robocoop_backend.modules.robot.teleop import TeleopCommand +from robocoop_backend.modules.robot.teleop_watchdog import ( + TeleopWatchdog, + ZERO_TELEOP_COMMAND, +) + + +@pytest.mark.unit +class TestTeleopWatchdog: + def test_notify_command_marks_non_zero_command(self): + adapter = MagicMock() + watchdog = TeleopWatchdog(adapter=adapter) + + watchdog.notify_command(TeleopCommand( + linear_x=0.1, + linear_y=0.0, + angular_z=0.0, + )) + + assert watchdog._last_command_at is not None + assert watchdog._last_command_was_zero is False + + def test_notify_command_marks_zero_command(self): + adapter = MagicMock() + watchdog = TeleopWatchdog(adapter=adapter) + + watchdog.notify_command(ZERO_TELEOP_COMMAND) + + assert watchdog._last_command_at is not None + assert watchdog._last_command_was_zero is True + + async def test_start_is_idempotent(self): + adapter = MagicMock() + watchdog = TeleopWatchdog(adapter=adapter, timeout_s=10.0) + + await watchdog.start() + first_task = watchdog._task + + await watchdog.start() + + assert watchdog._task is first_task + + await watchdog.stop() + + async def test_stop_is_idempotent(self): + adapter = MagicMock() + watchdog = TeleopWatchdog(adapter=adapter) + + await watchdog.stop() + await watchdog.stop() + + async def test_deadman_sends_zero_after_timeout(self): + adapter = MagicMock() + watchdog = TeleopWatchdog( + adapter=adapter, + timeout_s=0.01, + interval_s=0.005, + ) + + await watchdog.start() + + watchdog.notify_command(TeleopCommand( + linear_x=0.1, + linear_y=0.0, + angular_z=0.0, + )) + + try: + import asyncio + await asyncio.sleep(0.05) + + adapter.send_velocity.assert_called_with(ZERO_TELEOP_COMMAND) + assert watchdog._last_command_was_zero is True + finally: + await watchdog.stop() + + async def test_deadman_does_not_send_stop_after_zero_command(self): + adapter = MagicMock() + watchdog = TeleopWatchdog( + adapter=adapter, + timeout_s=0.01, + interval_s=0.005, + ) + + await watchdog.start() + watchdog.notify_command(ZERO_TELEOP_COMMAND) + + try: + import asyncio + await asyncio.sleep(0.05) + + adapter.send_velocity.assert_not_called() + finally: + await watchdog.stop() \ No newline at end of file diff --git a/src/robocoop_bringup/config/common.params.yaml b/src/robocoop_bringup/config/common.params.yaml index b512529..df3fb11 100644 --- a/src/robocoop_bringup/config/common.params.yaml +++ b/src/robocoop_bringup/config/common.params.yaml @@ -14,8 +14,11 @@ audit_log_path: "/var/log/robocoop/audit.jsonl" # === Teleop (dashboard → robot commands) === teleop: - max_linear_x: 0.5 - max_angular_z: 1.0 + max_linear_x: 0.25 + max_linear_y: 0.25 + max_angular_z: 0.8 + deadman_timeout_s: 0.5 + deadman_interval_s: 0.05 # === ROS message types (defaults; override per env if needed) === rosbridge_msg_types: diff --git a/src/robocoop_bringup/config/real.params.yaml b/src/robocoop_bringup/config/real.params.yaml index bab55f2..875e8b5 100644 --- a/src/robocoop_bringup/config/real.params.yaml +++ b/src/robocoop_bringup/config/real.params.yaml @@ -13,5 +13,12 @@ rosbridge: odom: "/odom" rgb: "/rgb" +teleop: + max_linear_x: 0.10 + max_linear_y: 0.10 + max_angular_z: 0.30 + deadman_timeout_s: 0.5 ## Ajouter un délais pour les tests POSTMAN + deadman_interval_s: 0.05 + rgb: enabled: true