621 lines
30 KiB
Python
621 lines
30 KiB
Python
"""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()
|