diff --git a/app/backend/routers/poi.py b/app/backend/routers/poi.py index 664ee866..16137543 100644 --- a/app/backend/routers/poi.py +++ b/app/backend/routers/poi.py @@ -2,7 +2,21 @@ POI management — reads/writes pois.json in the map directory. pois.json schema: - { "": {"id": int, "name": str, "position": [x, y, z]} } + { + "": { + "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 diff --git a/tinynav/core/action_executor.py b/tinynav/core/action_executor.py new file mode 100644 index 00000000..45192011 --- /dev/null +++ b/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 diff --git a/tinynav/core/map_node.py b/tinynav/core/map_node.py index 6fb6c805..8f93fc39 100644 --- a/tinynav/core/map_node.py +++ b/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