feat(nav): execute arrival actions when robot reaches a POI

app/backend/routers/poi.py

@@ -2,7 +2,21 @@
 POI management — reads/writes pois.json in the map directory.
 
 pois.json schema:
-  { "<id_str>": {"id": int, "name": str, "position": [x, y, z]} }
+  {
+    "<id_str>": {
+      "id": int,
+      "name": str,
+      "position": [x, y, z],
+      "actions": [                          # optional; omit or [] for no-op
+        {"type": "lookat", "params": {"target": [x, y, z]}},
+        {"type": "wait",   "params": {"seconds": 3}},
+        {"type": "photo",  "params": {"save_path": "/data/photos"}},
+        {"type": "custom", "params": {"command": "bark"}}
+      ]
+    }
+  }
+
+Supported action types are defined in tinynav/core/action_executor.py.
 """
 from __future__ import annotations
 

tinynav/core/action_executor.py

@@ -0,0 +1,157 @@
+"""
+POI Arrival Action Executor
+============================
+Runs a sequential list of actions when the robot reaches a POI.
+All actions are best-effort: failures are logged and execution continues.
+
+Supported action types:
+
+  lookat   – rotate to face a map-frame coordinate
+             params: {"target": [x, y, z]}
+
+  wait     – pause for N seconds
+             params: {"seconds": 5}
+
+  photo    – save the latest keyframe image to disk
+             params: {"save_path": "/data/photos"}   (optional)
+
+  custom   – publish an arbitrary command string to /service/command
+             params: {"command": "bark"}
+
+To add a new action type, implement an async handler with signature
+    async def my_handler(params: dict, node) -> None
+and register it in ACTION_REGISTRY at the bottom of this file.
+
+Example POI JSON:
+  {
+    "id": 1,
+    "name": "reception",
+    "position": [3.2, 1.5, 0.0],
+    "actions": [
+      {"type": "lookat", "params": {"target": [5.0, 2.0, 0.0]}},
+      {"type": "photo",  "params": {}},
+      {"type": "wait",   "params": {"seconds": 3}}
+    ]
+  }
+"""
+from __future__ import annotations
+
+import asyncio
+import math
+import os
+import time
+
+import cv2
+import numpy as np
+from std_msgs.msg import String
+
+from tinynav.core.math_utils import np2msg
+
+
+async def execute_poi_actions(actions: list, node) -> None:
+    """Run each action in order. Skips unknown types."""
+    for action in actions:
+        action_type = action.get("type", "")
+        handler = ACTION_REGISTRY.get(action_type)
+        if handler is None:
+            node.get_logger().warn(f"[action] unknown type '{action_type}', skipping")
+            continue
+        try:
+            await handler(action.get("params", {}), node)
+        except Exception as e:  # noqa: BLE001
+            node.get_logger().error(f"[action] {action_type} failed: {e}")
+
+
+# ---------------------------------------------------------------------------
+# Handlers
+# ---------------------------------------------------------------------------
+
+async def _handle_lookat(params: dict, node) -> None:
+    """Rotate the robot to face a map-frame target coordinate."""
+    target = params.get("target")
+    if target is None:
+        node.get_logger().warn("[action] lookat: missing 'target' param")
+        return
+    if node.latest_pose_in_map is None:
+        node.get_logger().warn("[action] lookat: no pose available, skipping")
+        return
+
+    pose = node.latest_pose_in_map
+    dx = target[0] - pose[0, 3]
+    dy = target[1] - pose[1, 3]
+    desired_yaw = math.atan2(dy, dx)
+
+    # Build a target pose: same position, desired heading only (flat rotation).
+    target_mat = np.eye(4)
+    target_mat[:3, 3] = pose[:3, 3]
+    c, s = math.cos(desired_yaw), math.sin(desired_yaw)
+    target_mat[0, 0], target_mat[0, 1] = c, -s
+    target_mat[1, 0], target_mat[1, 1] = s,  c
+
+    stamp = node.get_clock().now().to_msg()
+    node.target_pose_pub.publish(np2msg(target_mat, stamp, "world", "map"))
+    node.get_logger().info(f"[action] lookat target yaw={math.degrees(desired_yaw):.1f}°")
+
+    deadline = time.time() + 10.0
+    while time.time() < deadline:
+        if node.latest_pose_in_map is not None:
+            R = node.latest_pose_in_map[:3, :3]
+            current_yaw = math.atan2(R[1, 0], R[0, 0])
+            if abs(_angle_diff(current_yaw, desired_yaw)) < math.radians(5):
+                node.get_logger().info("[action] lookat: aligned")
+                return
+        await asyncio.sleep(0.1)
+
+    node.get_logger().warn("[action] lookat: timeout, continuing")
+
+
+async def _handle_wait(params: dict, node) -> None:
+    seconds = float(params.get("seconds", 1))
+    node.get_logger().info(f"[action] wait {seconds}s")
+    await asyncio.sleep(seconds)
+
+
+async def _handle_photo(params: dict, node) -> None:
+    if node.last_keyframe_image is None:
+        node.get_logger().warn("[action] photo: no keyframe image available, skipping")
+        return
+    save_path = params.get("save_path", "/tmp/tinynav_photos")
+    os.makedirs(save_path, exist_ok=True)
+    ts = int(time.time())
+    fname = os.path.join(save_path, f"poi{node.poi_index}_{ts}.jpg")
+    cv2.imwrite(fname, node.last_keyframe_image)
+    node.get_logger().info(f"[action] photo saved: {fname}")
+
+
+async def _handle_custom(params: dict, node) -> None:
+    command = params.get("command", "").strip()
+    if not command:
+        node.get_logger().warn("[action] custom: empty command, skipping")
+        return
+    node._action_pub.publish(String(data=f"play {command}"))
+    node.get_logger().info(f"[action] custom: sent 'play {command}'")
+
+
+# ---------------------------------------------------------------------------
+# Registry — add new action types here
+# ---------------------------------------------------------------------------
+
+ACTION_REGISTRY: dict[str, callable] = {
+    "lookat": _handle_lookat,
+    "wait":   _handle_wait,
+    "photo":  _handle_photo,
+    "custom": _handle_custom,
+}
+
+
+# ---------------------------------------------------------------------------
+# Helpers
+# ---------------------------------------------------------------------------
+
+def _angle_diff(a: float, b: float) -> float:
+    d = a - b
+    while d > math.pi:
+        d -= 2 * math.pi
+    while d < -math.pi:
+        d += 2 * math.pi
+    return d

tinynav/core/map_node.py

@@ -15,12 +15,14 @@
 from cv_bridge import CvBridge
 import cv2
 from codetiming import Timer
+import asyncio
 import argparse
+import threading
 
 from tinynav.tinynav_cpp_bind import pose_graph_solve
+from tinynav.core.action_executor import execute_poi_actions
 from tinynav.core.models_trt import LightGlueTRT, Dinov2TRT, SuperPointTRT
 import logging
-import asyncio
 from tf2_ros import TransformBroadcaster
 from tinynav.core.build_map_node import TinyNavDB
 from tinynav.core.build_map_node import find_loop, solve_pose_graph
@@ -217,7 +219,11 @@ def __init__(self, tinynav_db_path: str, tinynav_map_path: str, verbose_timer: b
         self.T_from_map_to_odom = None
 
         self.pois = {}
+        self.pois_data = {}          # index → full POI dict (position + actions)
         self.poi_index = -1
+        self.latest_pose_in_map = None
+        self._action_executing = False
+        self._action_pub = self.create_publisher(String, '/service/command', 10)
 
         self.poi_pub = self.create_publisher(Odometry, "/mapping/poi", 10)
         self.poi_change_pub = self.create_publisher(Odometry, "/mapping/poi_change", 10)
@@ -233,19 +239,42 @@ def __init__(self, tinynav_db_path: str, tinynav_map_path: str, verbose_timer: b
     def pois_callback(self, msg: String):
         self.get_logger().info("Received POIs from planner: " + msg.data)
         try:
-            self.pois = json.loads(msg.data)
-
+            raw = json.loads(msg.data)
             pois_dict = {}
-            keys = sorted([int (key) for key in self.pois.keys()])
+            pois_data = {}
+            keys = sorted([int(key) for key in raw.keys()])
             for index, key in enumerate(keys):
-                pois_dict[index] = np.array(self.pois[str(key)]["position"])
+                poi = raw[str(key)]
+                pois_dict[index] = np.array(poi["position"])
+                pois_data[index] = poi
             self.pois = pois_dict
-
+            self.pois_data = pois_data
             self.poi_index = min(0, len(self.pois) - 1)
+            self._action_executing = False
             self.get_logger().info(f"Parsed POIs: {self.pois}")
         except json.JSONDecodeError as e:
             self.get_logger().error(f"Failed to parse POIs JSON: {e}")
             self.pois = {}
+            self.pois_data = {}
+
+    def _advance_poi(self, timestamp: int) -> None:
+        self.poi_index += 1
+        dummy_pose = np.eye(4)
+        stamp_msg = self.get_clock().now().to_msg()
+        stamp_msg.sec = int(timestamp / 1e9)
+        stamp_msg.nanosec = int(timestamp % 1e9)
+        self.poi_change_pub.publish(np2msg(dummy_pose, stamp_msg, "world", "map"))
+
+    def _run_poi_actions(self, poi_idx: int, actions: list) -> None:
+        """Run in a daemon thread; advances poi_index when done."""
+        self.get_logger().info(f"[action] starting {len(actions)} action(s) for POI {poi_idx}")
+        loop = asyncio.new_event_loop()
+        loop.run_until_complete(execute_poi_actions(actions, self))
+        loop.close()
+        self.get_logger().info(f"[action] done for POI {poi_idx}, advancing")
+        self._action_executing = False
+        # Use a dummy timestamp; poi_change only signals planning_node to clear target.
+        self._advance_poi(int(self.get_clock().now().nanoseconds))
 
     def info_callback(self, msg:CameraInfo):
         if self.K is None:
@@ -556,19 +585,28 @@ def try_publish_nav_path(self, timestamp: int):
         self.current_pose_in_map_pub.publish(np2msg(pose_in_map, self.get_clock().now().to_msg(), "world", "map"))
 
         pose_in_map_position = pose_in_map[:3, 3]
+        self.latest_pose_in_map = pose_in_map
+
+        # If arrival actions are running, hold position and wait.
+        if self._action_executing:
+            return
 
         while self.poi_index < len(self.pois):
             poi = self.pois[self.poi_index]
             diff_position_norm_xy = np.linalg.norm(poi[:2] - pose_in_map_position[:2])
             diff_position_norm_z = np.linalg.norm(poi[2] - pose_in_map_position[2])
             if diff_position_norm_xy < 0.5 and diff_position_norm_z < 2.0:
-                self.poi_index += 1
-                dummy_pose = np.eye(4)
-
-                stamp_msg = self.get_clock().now().to_msg()
-                stamp_msg.sec = int(timestamp / 1e9)
-                stamp_msg.nanosec = int(timestamp % 1e9)
-                self.poi_change_pub.publish(np2msg(dummy_pose, stamp_msg, "world", "map"))
+                actions = self.pois_data.get(self.poi_index, {}).get("actions", [])
+                if actions:
+                    self._action_executing = True
+                    thread = threading.Thread(
+                        target=self._run_poi_actions,
+                        args=(self.poi_index, actions),
+                        daemon=True,
+                    )
+                    thread.start()
+                    return
+                self._advance_poi(timestamp)
                 continue
             else:
                 break