"""ROS 2 Navigate Action -> authenticated navigation gateway. The HTTP/session layer uses only the Python standard library. ROS imports are lazy so transport behavior can be exercised without a ROS installation. """ from __future__ import annotations import argparse import http.client import ipaddress import json import math import os import socket import threading import time import uuid from dataclasses import dataclass, field from typing import Any, Mapping from urllib.parse import urlsplit _STATES = {'SENDING', 'ACTIVE', 'CANCEL_REQUESTED', 'STOP_UNKNOWN', 'TERMINAL'} _OUTCOMES = {'COMPLETED', 'FAILED', 'CANCELED', 'TIMED_OUT', 'REJECTED'} _CONTROLLER_TERMINALS = {'ARRIVED', 'SUCCEEDED', 'ABORTED', 'PREEMPTED', 'RECALLED', 'REJECTED'} _TRACE_FIELDS = ('task_id', 'subtask_id', 'attempt', 'task_revision', 'plan_version', 'run_id', 'execution_generation') _MAX_RESPONSE_BYTES = 65536 class GatewayError(RuntimeError): """A response cannot establish the state of the physical goal.""" def _positive(value: Any, name: str) -> float: if isinstance(value, bool) or not isinstance(value, (int, float)): raise ValueError(f'{name} must be a finite positive number') result = float(value) if not math.isfinite(result) or result <= 0: raise ValueError(f'{name} must be a finite positive number') return result @dataclass(frozen=True) class ProxyConfig: token: str = field(repr=False) connect_timeout_sec: float read_timeout_sec: float request_timeout_sec: float poll_interval_sec: float readiness_max_age_sec: float feedback_silence_timeout_sec: float endpoint: str = 'http://127.0.0.1:8766' def __post_init__(self) -> None: if not isinstance(self.token, str) or len(self.token) < 16 or any(c.isspace() for c in self.token): raise ValueError('A bearer token of at least 16 characters without whitespace is required') for name in ('connect_timeout_sec', 'read_timeout_sec', 'request_timeout_sec', 'poll_interval_sec', 'readiness_max_age_sec', 'feedback_silence_timeout_sec'): _positive(getattr(self, name), name) parsed = urlsplit(self.endpoint) if (parsed.scheme not in {'http', 'https'} or not parsed.hostname or parsed.username is not None or parsed.password is not None or parsed.path not in {'', '/'} or parsed.query or parsed.fragment): raise ValueError('endpoint must be an http(s) origin without credentials or path') # DNS lookup has no socket deadline in stdlib. Use an explicit deployment # IP; localhost is normalized without invoking the resolver. if parsed.hostname != 'localhost': try: ipaddress.ip_address(parsed.hostname) except ValueError as exc: raise ValueError('endpoint must use an IP address or localhost') from exc try: parsed.port except ValueError as exc: raise ValueError('invalid endpoint port') from exc @dataclass(frozen=True) class GatewaySnapshot: goal_id: str status: str outcome: str | None stop_state: str controller_state: str message: str position_error: float | None yaw_error: float | None sequence: int elapsed: float pose_valid: bool = False current_pose: dict[str, Any] | None = None final_pose: dict[str, Any] | None = None stopped_at: float | None = None @classmethod def parse(cls, data: Mapping[str, Any], goal_id: str) -> 'GatewaySnapshot': try: required = {name: data[name] for name in cls.__dataclass_fields__ if name not in {'pose_valid', 'current_pose', 'final_pose', 'stopped_at'}} snapshot = cls(**required, pose_valid=data.get('pose_valid', False), current_pose=validate_observed_pose(data.get('current_pose')), final_pose=validate_observed_pose(data.get('final_pose')), stopped_at=data.get('stopped_at')) except (TypeError, KeyError) as exc: raise GatewayError('Gateway snapshot is missing required fields') from exc stop_time_parts = None if snapshot.stopped_at is None else _ros_time_parts(snapshot.stopped_at) if type(snapshot.pose_valid) is not bool: raise GatewayError('Gateway pose_valid must be boolean') if snapshot.pose_valid and snapshot.current_pose is None and snapshot.final_pose is None: raise GatewayError('Gateway marks pose valid without an observed pose') if snapshot.goal_id != goal_id: raise GatewayError('Gateway returned a different goal UUID') if snapshot.status not in _STATES or snapshot.outcome not in _OUTCOMES | {None}: raise GatewayError('Gateway returned an unknown status or outcome') if snapshot.stop_state not in {'UNKNOWN', 'CONFIRMED'}: raise GatewayError('Gateway returned an unknown stop state') if not isinstance(snapshot.controller_state, str) or not isinstance(snapshot.message, str): raise GatewayError('Gateway returned malformed text fields') if type(snapshot.sequence) is not int or not 0 <= snapshot.sequence <= 0xffffffff: raise GatewayError('Gateway sequence must fit Navigate uint32') for name in ('elapsed', 'position_error', 'yaw_error'): value = getattr(snapshot, name) if value is None and name != 'elapsed': continue if (isinstance(value, bool) or not isinstance(value, (float, int)) or not math.isfinite(value) or (name != 'yaw_error' and value < 0) or (name == 'yaw_error' and abs(value) > math.pi)): raise GatewayError(f'Gateway {name} is invalid') if snapshot.status == 'TERMINAL' and snapshot.outcome is None: raise GatewayError('Terminal gateway snapshot has no outcome') if snapshot.status == 'TERMINAL' and snapshot.stop_state == 'CONFIRMED': if stop_time_parts is None or stop_time_parts == (0, 0): raise GatewayError('Confirmed result requires a positive original ROS stop timestamp') if snapshot.controller_state not in _CONTROLLER_TERMINALS: raise GatewayError('Confirmed result lacks a native controller terminal') if snapshot.outcome == 'COMPLETED' and snapshot.controller_state not in {'ARRIVED', 'SUCCEEDED'}: raise GatewayError('COMPLETED conflicts with native controller state') return snapshot @property def is_terminal(self) -> bool: return self.status == 'TERMINAL' and self.stop_state == 'CONFIRMED' def _ros_time_parts(source_seconds: Any) -> tuple[int, int]: if (isinstance(source_seconds, bool) or not isinstance(source_seconds, (int, float)) or not math.isfinite(source_seconds) or source_seconds < 0 or source_seconds >= 2147483648): raise GatewayError('Stop timestamp must fit a nonnegative ROS int32 seconds value') seconds = math.floor(source_seconds) nanoseconds = round((source_seconds - seconds) * 1_000_000_000) if nanoseconds >= 1_000_000_000: seconds += 1 nanoseconds -= 1_000_000_000 if seconds > 2147483647: raise GatewayError('Rounded stop timestamp exceeds ROS int32 seconds') return seconds, nanoseconds def assign_ros_time(destination: Any, source_seconds: float) -> None: """Copy the original gateway evidence time, never callback receipt time.""" destination.sec, destination.nanosec = _ros_time_parts(source_seconds) def validate_observed_pose(pose: Any) -> dict[str, Any] | None: """Validate an actual map-frame observation without normalizing bad data.""" if pose is None: return None if not isinstance(pose, dict) or set(pose) != {'frame_id', 'position', 'orientation'}: raise GatewayError('Observed pose must use the gateway map-pose schema') if pose['frame_id'] != 'map': raise GatewayError('Observed pose must be in the map frame') for name, axes in (('position', {'x', 'y', 'z'}), ('orientation', {'x', 'y', 'z', 'w'})): coordinates = pose[name] if not isinstance(coordinates, dict) or set(coordinates) != axes: raise GatewayError('Observed pose coordinates are incomplete') if any(isinstance(v, bool) or not isinstance(v, (float, int)) or not math.isfinite(v) for v in coordinates.values()): raise GatewayError('Observed pose coordinates must be finite') if abs(sum(value * value for value in pose['orientation'].values()) - 1.0) > 1e-3: raise GatewayError('Observed pose quaternion must be normalized') return json.loads(json.dumps(pose, allow_nan=False)) def assign_ros_pose(destination: Any, source: Mapping[str, Any]) -> None: """Copy validated geometry; unavailable source timestamp remains unset.""" destination.header.frame_id = source['frame_id'] for name, axes in (('position', ('x', 'y', 'z')), ('orientation', ('x', 'y', 'z', 'w'))): for axis in axes: setattr(getattr(destination.pose, name), axis, float(source[name][axis])) class GatewayClient: """One fresh bounded connection per request; no redirect or proxy handling.""" def __init__(self, config: ProxyConfig): self.config = config parsed = urlsplit(config.endpoint) self._host = '127.0.0.1' if parsed.hostname == 'localhost' else parsed.hostname self._port = parsed.port self._connection_type = (http.client.HTTPSConnection if parsed.scheme == 'https' else http.client.HTTPConnection) def _request(self, method: str, path: str, body: Mapping[str, Any] | None = None) -> dict[str, Any]: raw = None if body is None else json.dumps(body, allow_nan=False, sort_keys=True, separators=(',', ':')).encode('utf-8') config = self.config connection = self._connection_type(self._host, self._port, timeout=min(config.connect_timeout_sec, config.request_timeout_sec)) expired = threading.Event() connected_socket = None def expire() -> None: expired.set() active_socket = connected_socket or connection.sock if active_socket is not None: try: active_socket.shutdown(socket.SHUT_RDWR) except OSError: pass active_socket.close() deadline = time.monotonic() + config.request_timeout_sec watchdog = threading.Timer(config.request_timeout_sec, expire) watchdog.daemon = True watchdog.start() try: connection.connect() connected_socket = connection.sock if expired.is_set() or connection.sock is None: raise GatewayError('Gateway request deadline exceeded') connection.sock.settimeout(min(config.read_timeout_sec, max(0.001, deadline - time.monotonic()))) connection.request(method, path, raw, { 'Authorization': f'Bearer {config.token}', 'Accept': 'application/json', 'Content-Type': 'application/json', 'Connection': 'close', }) response = connection.getresponse() # The response may own the socket once Connection: close is seen. # The watchdog retains its original reference below through this read. payload = response.read(_MAX_RESPONSE_BYTES + 1) if expired.is_set() or time.monotonic() > deadline: raise GatewayError('Gateway request deadline exceeded') if len(payload) > _MAX_RESPONSE_BYTES: raise GatewayError('Gateway response exceeds size limit') if not 200 <= response.status < 300: raise GatewayError(f'Gateway HTTP status {response.status}; stop is unknown') data = json.loads(payload) if not isinstance(data, dict): raise GatewayError('Gateway response must be a JSON object') return data except (OSError, http.client.HTTPException, ValueError) as exc: # Do not log the token, endpoint credentials, or arbitrary response body. raise GatewayError(f'Gateway transport/protocol failure: {type(exc).__name__}') from exc finally: watchdog.cancel() connection.close() def health(self) -> dict[str, Any]: health = self._request('GET', '/healthz') if type(health.get('ready')) is not bool or not isinstance(health.get('reason'), str): raise GatewayError('Gateway health response is malformed') return health def submit(self, body: Mapping[str, Any]) -> GatewaySnapshot: return GatewaySnapshot.parse(self._request('POST', '/v1/goals', body), body['goal_id']) def query(self, goal_id: str) -> GatewaySnapshot: _canonical_uuid(goal_id) return GatewaySnapshot.parse(self._request('GET', f'/v1/goals/{goal_id}'), goal_id) def cancel(self, goal_id: str) -> GatewaySnapshot: _canonical_uuid(goal_id) return GatewaySnapshot.parse(self._request('POST', f'/v1/goals/{goal_id}/cancel', {}), goal_id) def _canonical_uuid(goal_id: str) -> None: if not isinstance(goal_id, str) or str(uuid.UUID(goal_id)) != goal_id: raise ValueError('goal_id must be a canonical UUID') @dataclass(frozen=True) class SessionEvent: snapshot: GatewaySnapshot | None = None error: str | None = None terminal: bool = False class GoalSession: """Background transport for one immutable attempt; cancel means intent only. The caller owns physical resource locking. An error is always stop UNKNOWN. Coalescing to the latest event bounds memory while the ROS executor is busy. """ def __init__(self, client: GatewayClient, body: Mapping[str, Any]): self.client = client self._body = json.dumps(body, allow_nan=False, sort_keys=True, separators=(',', ':')).encode('utf-8') frozen_body = json.loads(self._body) self._goal_id = frozen_body['goal_id'] self._position_tolerance = _positive(frozen_body['position_tolerance'], 'position_tolerance') self._yaw_tolerance = _positive(frozen_body['yaw_tolerance'], 'yaw_tolerance') _canonical_uuid(self._goal_id) self._cancel = threading.Event() self._lock = threading.Lock() self._event: SessionEvent | None = None self._started = False @property def goal_id(self) -> str: return self._goal_id def start(self) -> None: with self._lock: if self._started: raise RuntimeError('One GoalSession can be started only once') self._started = True threading.Thread(target=self._run, name=f'nav-{self.goal_id}', daemon=True).start() def request_cancel(self) -> None: self._cancel.set() def drain_events(self) -> list[SessionEvent]: with self._lock: event, self._event = self._event, None return [] if event is None else [event] def _publish(self, event: SessionEvent) -> None: with self._lock: self._event = event def _run(self) -> None: cancel_sent = False last_sequence = -1 last_progress = time.monotonic() try: # A submission acknowledgement, even one containing a terminal # snapshot, is not consumed as a physical completion result. self.client.submit(json.loads(self._body)) while True: if self._cancel.is_set() and not cancel_sent: cancel_sent = True self.client.cancel(self.goal_id) # ACK only; never terminal. snapshot = self.client.query(self.goal_id) if snapshot.sequence > last_sequence: if snapshot.is_terminal and snapshot.outcome == 'COMPLETED': if (snapshot.position_error is None or snapshot.yaw_error is None or snapshot.position_error > self._position_tolerance or abs(snapshot.yaw_error) > self._yaw_tolerance): raise GatewayError('COMPLETED lacks matching pose tolerance evidence') last_sequence = snapshot.sequence last_progress = time.monotonic() self._publish(SessionEvent(snapshot=snapshot, terminal=snapshot.is_terminal)) if snapshot.is_terminal: return if time.monotonic() - last_progress >= self.client.config.feedback_silence_timeout_sec: self._cancel.set() # Event.wait would spin forever once cancel is set. A private # event keeps polling bounded and leaves ROS callbacks unblocked. threading.Event().wait(self.client.config.poll_interval_sec) except Exception as exc: # A lost submit response may conceal an accepted moving goal. Send # one best-effort cancel under the same UUID before reporting UNKNOWN. if not cancel_sent: try: self.client.cancel(self.goal_id) except Exception: pass error = str(exc) if isinstance(exc, GatewayError) else type(exc).__name__ self._publish(SessionEvent(error=error)) def build_goal_body(goal: Any, goal_id: str, map_id: str) -> dict[str, Any]: """Freeze the p13 Navigate goal into the Noetic gateway's explicit schema.""" _canonical_uuid(goal_id) if not isinstance(map_id, str) or not map_id.strip(): raise ValueError('map_id must be explicitly configured') if goal.target_pose.header.frame_id != 'map': raise ValueError('Navigate only accepts registered poses in the map frame') timeout = goal.timeout.sec + goal.timeout.nanosec / 1e9 if not 0 <= goal.timeout.nanosec < 1_000_000_000: raise ValueError('timeout nanosec is not normalized') pose = goal.target_pose.pose body = { 'goal_id': goal_id, 'trace': {name: getattr(goal.trace, name) for name in _TRACE_FIELDS}, 'map_id': map_id, 'target_pose': { 'frame_id': 'map', 'position': {axis: getattr(pose.position, axis) for axis in ('x', 'y', 'z')}, 'orientation': {axis: getattr(pose.orientation, axis) for axis in ('x', 'y', 'z', 'w')}, }, 'position_tolerance': _positive(goal.position_tolerance, 'position_tolerance'), 'yaw_tolerance': _positive(goal.orientation_tolerance, 'orientation_tolerance'), 'timeout_sec': _positive(timeout, 'timeout'), } if not body['trace']['task_id'] or not body['trace']['subtask_id'] or body['trace']['attempt'] < 1: raise ValueError('TaskTrace task_id, subtask_id and attempt are required') values = list(body['target_pose']['position'].values()) + list(body['target_pose']['orientation'].values()) if any(isinstance(v, bool) or not isinstance(v, (float, int)) or not math.isfinite(v) for v in values): raise ValueError('target pose must contain finite coordinates') return json.loads(json.dumps(body, allow_nan=False)) def create_ros_node(config: ProxyConfig, *, map_id: str, action_name: str): """Construct the ROS node; call only after rclpy.init(). ROS 2 Humble imports and action runtime require validation on the target. """ import rclpy from rclpy.action import ActionServer, CancelResponse, GoalResponse from rclpy.callback_groups import ReentrantCallbackGroup from rclpy.node import Node from rclpy.task import Future from bt_skill_interfaces.action import Navigate from bt_skill_interfaces.msg import ExecutionResult if not map_id or not action_name: raise ValueError('map_id and action_name must be explicitly configured') class NavigateProxy(Node): def __init__(self): super().__init__('navigation_gateway_proxy') self._client = GatewayClient(config) self._guard = threading.Lock() self._ready = False self._health_at = 0.0 self._reserved = False self._stop_unknown = False self._record = None self._closing = threading.Event() self._group = ReentrantCallbackGroup() self._server = ActionServer(self, Navigate, action_name, execute_callback=self._execute, goal_callback=self._accept, cancel_callback=self._cancel_goal, callback_group=self._group) self._pump_timer = self.create_timer(config.poll_interval_sec, self._pump) self._health_thread = threading.Thread(target=self._health_worker, name='nav-readiness', daemon=True) self._health_thread.start() def _health_worker(self): while not self._closing.is_set(): try: response = self._client.health() ready = response['ready'] and response.get('map_id') == map_id except Exception: ready = False with self._guard: self._ready, self._health_at = ready, time.monotonic() self._closing.wait(config.poll_interval_sec) def _accept(self, request): try: # Validation uses a temporary local UUID because ROS supplies the # actual UUID with the accepted goal handle. Nothing is sent here. build_goal_body(request, '00000000-0000-4000-8000-000000000000', map_id) except (AttributeError, TypeError, ValueError): return GoalResponse.REJECT with self._guard: if (self._reserved or self._stop_unknown or not self._ready or time.monotonic() - self._health_at > config.readiness_max_age_sec): return GoalResponse.REJECT self._reserved = True return GoalResponse.ACCEPT async def _execute(self, handle): goal_id = str(uuid.UUID(bytes=bytes(handle.goal_id.uuid))) future = Future() try: session = GoalSession(self._client, build_goal_body(handle.request, goal_id, map_id)) with self._guard: self._record = (handle, session, future) if handle.is_cancel_requested: session.request_cancel() session.start() except Exception as exc: with self._guard: self._stop_unknown = True handle.abort() return self._unknown_result(type(exc).__name__) return await future def _cancel_goal(self, handle): with self._guard: record = self._record if record is not None and bytes(record[0].goal_id.uuid) == bytes(handle.goal_id.uuid): record[1].request_cancel() # The accepted execute callback also checks is_cancel_requested, # covering cancellation before a worker has been registered. return CancelResponse.ACCEPT def _unknown_result(self, message): result = Navigate.Result() result.result.status = ExecutionResult.FAILED result.result.stop_state = ExecutionResult.UNKNOWN result.result.error_code = 'NAV_GATEWAY_STOP_UNKNOWN' result.result.message = message result.pose_valid = False result.errors_valid = False return result def _pump(self): with self._guard: record = self._record if record is None: return handle, session, future = record for event in session.drain_events(): if event.error is not None: with self._guard: self._stop_unknown = True self._record = None handle.abort() future.set_result(self._unknown_result(event.error)) continue snapshot = event.snapshot if event.terminal: result = Navigate.Result() result.result.status = getattr(ExecutionResult, snapshot.outcome) result.result.stop_state = ExecutionResult.CONFIRMED result.result.message = snapshot.message result.result.error_code = '' if snapshot.outcome == 'COMPLETED' else 'NAV_' + snapshot.outcome result.result.stop_evidence_ref = f'nav-gateway:{snapshot.goal_id}:sequence:{snapshot.sequence}' assign_ros_time(result.result.stopped_at, snapshot.stopped_at) result.pose_valid = snapshot.pose_valid and snapshot.final_pose is not None if result.pose_valid: assign_ros_pose(result.final_pose, snapshot.final_pose) result.errors_valid = snapshot.position_error is not None and snapshot.yaw_error is not None if result.errors_valid: result.final_position_error = float(snapshot.position_error) result.final_orientation_error = float(snapshot.yaw_error) if snapshot.outcome == 'COMPLETED': handle.succeed() elif snapshot.outcome == 'CANCELED' and handle.is_cancel_requested: handle.canceled() else: if snapshot.outcome == 'CANCELED': # A remote cancellation has no matching ROS cancel # transition. Preserve physical stop, report FAILED. result.result.status = ExecutionResult.FAILED result.result.error_code = 'NAV_REMOTE_CANCELED' handle.abort() with self._guard: self._record = None self._reserved = False future.set_result(result) else: feedback = Navigate.Feedback() feedback.stamp = self.get_clock().now().to_msg() feedback.sequence = snapshot.sequence feedback.phase = (Navigate.Feedback.STOPPING if snapshot.status in {'CANCEL_REQUESTED', 'STOP_UNKNOWN', 'TERMINAL'} else Navigate.Feedback.CHECKING if snapshot.status == 'SENDING' else Navigate.Feedback.NAVIGATING) feedback.pose_valid = snapshot.pose_valid and snapshot.current_pose is not None if feedback.pose_valid: assign_ros_pose(feedback.current_pose, snapshot.current_pose) feedback.blocked_valid = False feedback.errors_valid = snapshot.position_error is not None and snapshot.yaw_error is not None if feedback.errors_valid: feedback.position_error = float(snapshot.position_error) feedback.orientation_error = float(snapshot.yaw_error) seconds = min(snapshot.elapsed, 2147483647.0) feedback.elapsed_time.sec = int(seconds) feedback.elapsed_time.nanosec = int((seconds - int(seconds)) * 1e9) feedback.message = snapshot.message handle.publish_feedback(feedback) def destroy_node(self): self._closing.set() with self._guard: record = self._record if record is not None: record[1].request_cancel() self._server.destroy() return super().destroy_node() return NavigateProxy() def main(argv: list[str] | None = None) -> None: parser = argparse.ArgumentParser(description=__doc__) parser.add_argument('--endpoint', default=os.environ.get('NAV_GATEWAY_URL', 'http://127.0.0.1:8766')) parser.add_argument('--map-id', default=os.environ.get('NAV_GATEWAY_MAP_ID')) parser.add_argument('--action-name', default=os.environ.get('NAV_PROXY_ACTION_NAME')) for name in ('connect_timeout_sec', 'read_timeout_sec', 'request_timeout_sec', 'poll_interval_sec', 'readiness_max_age_sec', 'feedback_silence_timeout_sec'): parser.add_argument('--' + name.replace('_', '-'), type=float, default=os.environ.get('NAV_PROXY_' + name.upper())) args, ros_args = parser.parse_known_args(argv) if not args.map_id or not args.action_name: parser.error('--map-id and --action-name (or matching environment variables) are required') try: config = ProxyConfig(token=os.environ.get('NAV_GATEWAY_TOKEN', ''), endpoint=args.endpoint, **{name: getattr(args, name) for name in ('connect_timeout_sec', 'read_timeout_sec', 'request_timeout_sec', 'poll_interval_sec', 'readiness_max_age_sec', 'feedback_silence_timeout_sec')}) except ValueError as exc: parser.error(str(exc)) import rclpy from rclpy.executors import MultiThreadedExecutor rclpy.init(args=ros_args) node = create_ros_node(config, map_id=args.map_id, action_name=args.action_name) executor = MultiThreadedExecutor(num_threads=2) executor.add_node(node) try: executor.spin() finally: node.destroy_node() executor.shutdown() rclpy.shutdown() if __name__ == '__main__': main()