feature(navigation): add waypoint navigation flow

src/robocoop_backend/robocoop_backend.egg-info/SOURCES.txt

@@ -23,6 +23,9 @@ robocoop_backend/modules/audit/audit_logger.py
 robocoop_backend/modules/audit/audit_service.py
 robocoop_backend/modules/audit/event_formatter.py
 robocoop_backend/modules/audit/sinks.py
+robocoop_backend/modules/navigation/__init__.py
+robocoop_backend/modules/navigation/waypoint.py
+robocoop_backend/modules/navigation/waypoint_store.py
 robocoop_backend/modules/robot/__init__.py
 robocoop_backend/modules/robot/rgb_chenillard.py
 robocoop_backend/modules/robot/state_store.py

src/robocoop_backend/robocoop_backend/adapters/base_adapter.py

@@ -1,6 +1,6 @@
 import logging
 from abc import ABC, abstractmethod
-from typing import Any, Dict
+from robocoop_backend.modules.navigation.waypoint import Waypoint
 
 logger = logging.getLogger(__name__)
 
@@ -34,3 +34,13 @@ def send_velocity(self, command: TeleopCommand) -> None:
     def emergency_stop(self) -> None:
         """Trigger emergency stop."""
         ...
+
+    @abstractmethod
+    def send_goal(self, waypoint: Waypoint) -> None:
+        """Send navigation goal (action Nav2 NavigateToPose)."""
+        ...
+
+    @abstractmethod
+    def cancel_goal(self) -> None:
+        """Cancel in-progress navigation."""
+        ...

src/robocoop_backend/robocoop_backend/adapters/mock_adapter.py

@@ -1,12 +1,12 @@
+import asyncio
 import logging
+import math
 
 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__(
@@ -22,6 +22,8 @@ def __init__(
         self.max_linear_y = max_linear_y
         self.max_angular_z = max_angular_z
         self.last_velocity_command = None
+        self.navigation_service = None
+        self._nav_task = None
 
     async def connect(self) -> bool:
         return True
@@ -34,7 +36,6 @@ def is_connected(self) -> bool:
 
     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,
@@ -44,3 +45,43 @@ def send_velocity(self, command: TeleopCommand) -> None:
 
     def emergency_stop(self) -> None:
         logger.warning("[MOCK] emergency_stop")
+
+    def send_goal(self, waypoint) -> None:
+        logger.info("[MOCK] nav.goto -> %s (x=%.2f y=%.2f)", waypoint.name, waypoint.x, waypoint.y)
+        self._cancel_sim()
+        try:
+            self._nav_task = asyncio.get_running_loop().create_task(
+                self._simulate_navigation(waypoint)
+            )
+        except RuntimeError:
+            logger.warning("[MOCK] pas de boucle asyncio, nav non simulée")
+
+    def cancel_goal(self) -> None:
+        logger.warning("[MOCK] nav.cancel")
+        self._cancel_sim()
+        if self.navigation_service:
+            self.navigation_service.on_nav_result(False, {"reason": "cancelled"})
+
+    def _cancel_sim(self) -> None:
+        if self._nav_task and not self._nav_task.done():
+            self._nav_task.cancel()
+
+    async def _simulate_navigation(self, waypoint, steps: int = 10, step_delay: float = 0.4):
+        total = math.hypot(waypoint.x, waypoint.y)
+        try:
+            for i in range(1, steps + 1):
+                await asyncio.sleep(step_delay)
+                t = i / steps
+                if self.navigation_service:
+                    self.navigation_service.on_nav_feedback({
+                        "distance_remaining": round(total * (1 - t), 2),
+                        "eta_seconds": round((steps - i) * step_delay, 1),
+                        "navigation_time": round(i * step_delay, 1),
+                        "recoveries": 0,
+                        "current_x": round(waypoint.x * t, 2),
+                        "current_y": round(waypoint.y * t, 2),
+                    })
+            if self.navigation_service:
+                self.navigation_service.on_nav_result(True)
+        except asyncio.CancelledError:
+            raise

src/robocoop_backend/robocoop_backend/adapters/rosbridge_adapter.py

@@ -85,6 +85,9 @@ def __init__(
             on_reconnected=self._on_bridge_reconnected,
             on_disconnected=self._on_bridge_disconnected,
         )
+        nav_action: str = "/navigate_to_pose",
+        nav_action_type: str = "nav2_msgs/action/NavigateToPose",
+        nav_frame_id: str = "map",
 
     async def connect(self) -> bool:
         if not await self._client.connect():
@@ -253,4 +256,58 @@ 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()
+
+    def send_goal(self, waypoint) -> None:
+        goal_id = f"nav_goal_{int(time.time() * 1000)}"
+        self._active_goal_id = goal_id
+        args = {"pose": waypoint.to_pose_stamped(self.nav_frame_id), "behavior_tree": ""}
+        self._schedule(self._client.send_action_goal(
+            action=self.nav_action,
+            action_type=self.nav_action_type,
+            args=args,
+            goal_id=goal_id,
+            on_feedback=self._on_nav_feedback_raw,
+            on_result=self._on_nav_result_raw,
+        ))
+
+    def cancel_goal(self) -> None:
+        if self._active_goal_id:
+            self._schedule(self._client.cancel_action_goal(self.nav_action, self._active_goal_id))
+
+    def _schedule(self, coro) -> None:
+        try:
+            asyncio.get_running_loop().create_task(coro)
+        except RuntimeError:
+            logger.error("No asyncio loop for navigation action")
+
+    @staticmethod
+    def _dur_to_s(d) -> float:
+        d = d or {}
+        return d.get("sec", 0) + d.get("nanosec", 0) / 1e9
+
+    def _on_nav_feedback_raw(self, values: Dict[str, Any]) -> None:
+        if not self.navigation_service:
+            return
+        pos = (values.get("current_pose") or {}).get("pose", {}).get("position", {})
+        self.navigation_service.on_nav_feedback({
+            "distance_remaining": float(values.get("distance_remaining", 0.0)),
+            "eta_seconds": self._dur_to_s(values.get("estimated_time_remaining")),
+            "navigation_time": self._dur_to_s(values.get("navigation_time")),
+            "recoveries": int(values.get("number_of_recoveries", 0)),
+            "current_x": float(pos.get("x", 0.0)),
+            "current_y": float(pos.get("y", 0.0)),
+        })
+
+    def _on_nav_result_raw(self, msg: Dict[str, Any]) -> None:
+        if not self.navigation_service:
+            return
+        status = msg.get("status", (msg.get("values") or {}).get("status"))
+        values = msg.get("values") or {}
+        self._active_goal_id = None
+        self.navigation_service.on_nav_result(
+            success=(status == 4),  # 4 = SUCCEEDED
+            detail={"status": status,
+                    "error_code": values.get("error_code"),
+                    "error_msg": values.get("error_msg", "")},
+        )

src/robocoop_backend/robocoop_backend/adapters/rosbridge_client.py

@@ -40,6 +40,8 @@ def __init__(
         self._pending_pings: Dict[str, asyncio.Future[None]] = {}
         self._ping_seq = 0
 
+        self._action_callbacks: Dict[str, Dict[str, Callable]] = {}
+
     async def connect(self) -> bool:
         if not await self._connect_ws():
             return False
@@ -60,6 +62,7 @@ async def disconnect(self) -> None:
             if not future.done():
                 future.cancel()
         self._pending_pings.clear()
+        self._action_callbacks.clear()
 
         if self._websocket:
             await self._websocket.close()
@@ -98,6 +101,36 @@ async def subscribe(self, topic: str, msg_type: str, callback: Callable) -> None
 
         logger.info(f"Subscribed to {topic}")
 
+    async def send_action_goal(
+        self, action: str, action_type: str, args: Dict[str, Any], goal_id: str,
+        on_feedback: Optional[Callable] = None, on_result: Optional[Callable] = None,
+    ) -> None:
+        if not self._websocket:
+            logger.error("Cannot send action goal: not connected")
+            return
+        self._action_callbacks[goal_id] = {"feedback": on_feedback, "result": on_result}
+        await self._send_json({
+            "op": "send_action_goal",
+            "id": goal_id,
+            "action": action,
+            "action_type": action_type,
+            "args": args,
+            "feedback": True,
+        })
+
+    async def cancel_action_goal(self, action: str, goal_id: str) -> None:
+        await self._send_json({"op": "cancel_action_goal", "id": goal_id, "action": action})
+
+    def _dispatch_action(self, data: Dict[str, Any], kind: str, payload: Any) -> None:
+        callbacks = self._action_callbacks.get(data.get("id"))
+        if not callbacks:
+            return
+        cb = callbacks.get(kind)
+        if cb:
+            cb(payload)
+        if kind == "result":                     
+            self._action_callbacks.pop(data.get("id"), None)
+
     def is_connected(self) -> bool:
         return self._is_connected and self._websocket is not None
 
@@ -188,6 +221,14 @@ def _handle_message(self, data: Dict[str, Any]) -> None:
         if self._handle_pending_ping(data):
             return
 
+        op = data.get("op")
+        if op == "action_feedback":
+            self._dispatch_action(data, "feedback", data.get("values", {}))
+            return
+        if op == "action_result":
+            self._dispatch_action(data, "result", data)   
+            return
+
         topic = data.get("topic")
         if topic in self._subscribers:
             self._subscribers[topic](data.get("msg", {}))

src/robocoop_backend/robocoop_backend/app/backend_context.py

@@ -8,6 +8,9 @@
 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
+from robocoop_backend.modules.navigation.waypoint_store import WaypointStore
+from robocoop_backend.modules.navigation.map_store import MapStore
+from robocoop_backend.modules.navigation.navigation_service import NavigationService
 
 logger = logging.getLogger(__name__)
 
@@ -58,6 +61,18 @@ def __init__(self, config: Dict[str, Any]):
 
         logger.info(f"Adapter: {type(self.adapter).__name__}")
 
+        self.waypoint_store = WaypointStore(
+            persist_path=config.get("waypoints_path", "waypoints.json")
+        )
+        self.map_store = MapStore()
+        self.navigation_service = NavigationService(
+            waypoint_store=self.waypoint_store,
+            adapter=self.adapter,
+            audit_service=self.audit_service,
+        )
+
+        self.adapter.navigation_service = self.navigation_service
+
     async def connect(self) -> bool:
         """Initialize and connect all services."""
         try:
@@ -84,4 +99,5 @@ 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
+        self.navigation_service.websocket_handler = handler

src/robocoop_backend/robocoop_backend/app/contracts.py

@@ -32,6 +32,13 @@
 get_health
   {"type": "get_health"}
 
+waypoint.save     {"type": "waypoint.save", "data": {"name": "Chambre B", "x": 2.0, "y": 1.5, "yaw": 0.0, "map_id": "default"}}
+waypoint.list     {"type": "waypoint.list", "map_id": "default"}   # map_id optionnel
+waypoint.delete   {"type": "waypoint.delete", "data": {"id": "uuid"}}
+nav.goto          {"type": "nav.goto", "data": {"id": "uuid"}}      # ou {"name": "Chambre B"}
+nav.cancel        {"type": "nav.cancel"}
+get_map           {"type": "get_map"}
+
 ━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━
 BACKEND → FRONTEND
 ━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━
@@ -156,6 +163,12 @@
 MSG_TELEOP_MOVE = "teleop.move"
 MSG_EMERGENCY_STOP = "emergency_stop"
 MSG_GET_HEALTH = "get_health"
+MSG_WAYPOINT_SAVE = "waypoint.save"
+MSG_WAYPOINT_LIST = "waypoint.list"
+MSG_WAYPOINT_DELETE = "waypoint.delete"
+MSG_NAV_GOTO = "nav.goto"
+MSG_NAV_CANCEL = "nav.cancel"
+MSG_GET_MAP = "get_map"
 
 # Backend → Frontend
 MSG_COMMAND_ACK = "command_ack"
@@ -167,3 +180,10 @@
 MSG_STATE_UPDATED = "robot_state_updated"
 MSG_ACTIVITY_EVENT = "activity_event"
 MSG_HEALTH_RESPONSE = "health_response"
+MSG_WAYPOINT_SAVED = "waypoint.saved"
+MSG_WAYPOINT_LIST_RESPONSE = "waypoint_list"
+MSG_MAP_RESPONSE = "map_response"
+MSG_NAV_STARTED = "nav.started"
+MSG_NAV_PROGRESS = "nav.progress"
+MSG_NAV_ARRIVED = "nav.arrived"
+MSG_NAV_FAILED = "nav.failed"

src/robocoop_backend/robocoop_backend/app/handlers/map.py

@@ -0,0 +1,15 @@
+from robocoop_backend.app.contracts import MSG_GET_MAP, MSG_MAP_RESPONSE
+from robocoop_backend.app.handlers.utils import send_message
+
+
+class MapHandlers:
+    """Handlers for map metadata."""
+
+    def __init__(self, context):
+        self.context = context
+
+    def routes(self):
+        return {MSG_GET_MAP: self.get_map}
+
+    async def get_map(self, websocket, message: dict) -> None:
+        await send_message(websocket, MSG_MAP_RESPONSE, self.context.map_store.to_dict())

src/robocoop_backend/robocoop_backend/app/handlers/navigation.py

@@ -0,0 +1,28 @@
+from robocoop_backend.app.contracts import MSG_NAV_CANCEL, MSG_NAV_GOTO
+from robocoop_backend.app.handlers.utils import send_command_error
+
+
+class NavigationHandlers:
+    """Handlers for autonomous navigation commands."""
+
+    def __init__(self, context):
+        self.context = context
+
+    def routes(self):
+        return {
+            MSG_NAV_GOTO: self.goto,
+            MSG_NAV_CANCEL: self.cancel,
+        }
+
+    async def goto(self, websocket, message: dict) -> None:
+        data = message.get("data") or {}
+        try:
+            self.context.navigation_service.go_to(
+                waypoint_id=data.get("id"),
+                name=data.get("name"),
+            )
+        except ValueError as e:
+            await send_command_error(websocket, MSG_NAV_GOTO, str(e))
+
+    async def cancel(self, websocket, message: dict) -> None:
+        self.context.navigation_service.cancel()