wangfeiyu/behavior-tree
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

fix: align navigation contract and readiness handling

Browse Source
This commit is contained in:
wangfeiyu
2026-09-22 14:35:14 +08:00
parent 91bcd92d6b
commit 964d1fde67
21 changed files with 922 additions and 139 deletions
Download Patch File Download Diff File Expand all files Collapse all files
navigation_gateway/ros2_proxy.py
+100 -42
View File
@@ -21,7 +21,7 @@ from urllib.parse import urlsplit
_STATES = {'SENDING', 'ACTIVE', 'CANCEL_REQUESTED', 'STOP_UNKNOWN', 'TERMINAL'}
_OUTCOMES = {'COMPLETED', 'FAILED', 'CANCELED', 'TIMED_OUT', 'REJECTED'}
_OUTCOMES = {'COMPLETED', 'FAILED', 'CANCELED', 'TIMED_OUT', 'REJECTED', 'BLOCKED', 'NOT_READY'}
_CONTROLLER_TERMINALS = {'ARRIVED', 'SUCCEEDED', 'ABORTED', 'PREEMPTED', 'RECALLED', 'REJECTED'}
_TRACE_FIELDS = ('task_id', 'subtask_id', 'attempt', 'task_revision', 'plan_version',
'run_id', 'execution_generation')
@@ -92,18 +92,24 @@ class GatewaySnapshot:
current_pose: dict[str, Any] | None = None
final_pose: dict[str, Any] | None = None
stopped_at: float | None = None
blocked: bool | None = None
error_code: str = ''
@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'}}
if name not in {'pose_valid', 'current_pose', 'final_pose', 'stopped_at', 'blocked', 'error_code'}}
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'))
stopped_at=data.get('stopped_at'), blocked=data.get('blocked'), error_code=data.get('error_code', ''))
except (TypeError, KeyError) as exc:
raise GatewayError('Gateway snapshot is missing required fields') from exc
if snapshot.blocked is not None and type(snapshot.blocked) is not bool:
raise GatewayError('Gateway blocked must be boolean or unknown')
if not isinstance(snapshot.error_code, str):
raise GatewayError('Gateway error_code must be text')
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')
@@ -191,6 +197,37 @@ def assign_ros_pose(destination: Any, source: Mapping[str, Any]) -> None:
setattr(getattr(destination.pose, name), axis, float(source[name][axis]))
def navigation_status(snapshot, enum):
"""Translate wire outcomes by name; execution and navigation numbers differ."""
names = {'COMPLETED': 'SUCCEEDED', 'CANCELED': 'CANCELED', 'TIMED_OUT': 'TIMEOUT',
'BLOCKED': 'BLOCKED', 'NOT_READY': 'NOT_READY', 'REJECTED': 'FAILED', 'FAILED': 'FAILED'}
if snapshot.outcome in {'FAILED', 'REJECTED', 'NOT_READY'} and snapshot.error_code in {'INPUTS_UNHEALTHY', 'ROBOT_STATE_UNAVAILABLE', 'BACKEND_NOT_CONFIGURED'}:
return enum.NOT_READY
return getattr(enum, names[snapshot.outcome])
def assign_navigation_feedback(feedback, snapshot):
"""A required bool cannot truthfully encode missing blocked telemetry."""
if snapshot.blocked is None:
raise GatewayError('blocked telemetry unavailable; stop requested')
feedback.sequence = snapshot.sequence
feedback.phase = (feedback.STOPPING if snapshot.status in {'CANCEL_REQUESTED', 'STOP_UNKNOWN', 'TERMINAL'}
else feedback.CHECKING if snapshot.status == 'SENDING'
else feedback.BLOCKED if snapshot.blocked else feedback.NAVIGATING)
feedback.blocked = snapshot.blocked
feedback.current_pose_valid = snapshot.pose_valid and snapshot.current_pose is not None
if feedback.current_pose_valid:
assign_ros_pose(feedback.current_pose, snapshot.current_pose)
feedback.error_valid = snapshot.position_error is not None and snapshot.yaw_error is not None
if feedback.error_valid:
feedback.position_error = float(snapshot.position_error)
feedback.yaw_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
class GatewayClient:
"""One fresh bounded connection per request; no redirect or proxy handling."""
@@ -396,14 +433,27 @@ def build_goal_body(goal: Any, goal_id: str, map_id: str) -> dict[str, Any]:
'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'),
'yaw_tolerance': _positive(goal.yaw_tolerance, 'yaw_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')
for name in ('task_id', 'subtask_id', 'run_id'):
if not isinstance(body['trace'][name], str) or not body['trace'][name]:
raise ValueError('TaskTrace identifiers must be nonempty strings')
for name in ('attempt', 'task_revision', 'plan_version', 'execution_generation'):
value = body['trace'][name]
maximum = 2**64 if name == 'execution_generation' else 2**32
if type(value) is not int or not 0 < value < maximum:
raise ValueError('TaskTrace counters must be positive and fit their wire types')
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')
orientation = body['target_pose']['orientation']
scale = max(abs(v) for v in orientation.values())
if scale == 0:
raise ValueError('target quaternion must be nonzero')
scaled = {axis: value / scale for axis, value in orientation.items()}
norm = math.sqrt(sum(v*v for v in scaled.values()))
body['target_pose']['orientation'] = {axis: value / norm for axis, value in scaled.items()}
return json.loads(json.dumps(body, allow_nan=False))
@@ -418,7 +468,7 @@ def create_ros_node(config: ProxyConfig, *, map_id: str, action_name: str):
from rclpy.node import Node
from rclpy.task import Future
from bt_skill_interfaces.action import Navigate
from bt_skill_interfaces.msg import ExecutionResult
from bt_skill_interfaces.msg import NavigationResult
if not map_id or not action_name:
raise ValueError('map_id and action_name must be explicitly configured')
@@ -430,6 +480,7 @@ def create_ros_node(config: ProxyConfig, *, map_id: str, action_name: str):
self._guard = threading.Lock()
self._ready = False
self._health_at = 0.0
self._health_error_code = ''
self._reserved = False
self._stop_unknown = False
self._record = None
@@ -448,10 +499,14 @@ def create_ros_node(config: ProxyConfig, *, map_id: str, action_name: str):
try:
response = self._client.health()
ready = response['ready'] and response.get('map_id') == map_id
error_code = response.get('error_code', '')
if not isinstance(error_code, str): error_code = ''
except Exception:
ready = False
error_code = ''
with self._guard:
self._ready, self._health_at = ready, time.monotonic()
self._health_error_code = error_code
self._closing.wait(config.poll_interval_sec)
def _accept(self, request):
@@ -462,13 +517,23 @@ def create_ros_node(config: ProxyConfig, *, map_id: str, action_name: str):
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):
if self._reserved or self._stop_unknown:
return GoalResponse.REJECT
self._reserved = True
return GoalResponse.ACCEPT
async def _execute(self, handle):
with self._guard:
fresh = time.monotonic() - self._health_at <= config.readiness_max_age_sec
ready = self._ready and fresh
if not ready:
code = self._health_error_code if fresh else ''
self._reserved = False
if not ready:
result = self._unknown_result('navigation backend is not ready; no goal dispatched', code or 'NAV_NOT_READY')
result.result.status = NavigationResult.NOT_READY
handle.abort()
return result
goal_id = str(uuid.UUID(bytes=bytes(handle.goal_id.uuid)))
future = Future()
try:
@@ -494,14 +559,16 @@ def create_ros_node(config: ProxyConfig, *, map_id: str, action_name: str):
# covering cancellation before a worker has been registered.
return CancelResponse.ACCEPT
def _unknown_result(self, message):
def _unknown_result(self, message, error_code='NAV_GATEWAY_STOP_UNKNOWN'):
result = Navigate.Result()
result.result.status = ExecutionResult.FAILED
result.result.stop_state = ExecutionResult.UNKNOWN
result.result.error_code = 'NAV_GATEWAY_STOP_UNKNOWN'
result.result.status = (NavigationResult.NOT_READY if error_code in
{'INPUTS_UNHEALTHY', 'ROBOT_STATE_UNAVAILABLE', 'BACKEND_NOT_CONFIGURED'} else NavigationResult.FAILED)
result.result.stop_state = NavigationResult.UNKNOWN
result.result.error_code = error_code
result.result.message = message
result.pose_valid = False
result.errors_valid = False
result.final_pose_valid = False
result.final_position_error = math.nan
result.final_yaw_error = math.nan
return result
def _pump(self):
@@ -521,19 +588,17 @@ def create_ros_node(config: ProxyConfig, *, map_id: str, action_name: str):
snapshot = event.snapshot
if event.terminal:
result = Navigate.Result()
result.result.status = getattr(ExecutionResult, snapshot.outcome)
result.result.stop_state = ExecutionResult.CONFIRMED
result.result.status = navigation_status(snapshot, NavigationResult)
result.result.stop_state = NavigationResult.CONFIRMED
result.result.message = snapshot.message
result.result.error_code = '' if snapshot.outcome == 'COMPLETED' else 'NAV_' + snapshot.outcome
result.result.error_code = snapshot.error_code or ('' 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:
result.final_pose_valid = snapshot.pose_valid and snapshot.final_pose is not None
if result.final_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)
result.final_position_error = math.nan if snapshot.position_error is None else float(snapshot.position_error)
result.final_yaw_error = math.nan if snapshot.yaw_error is None else float(snapshot.yaw_error)
if snapshot.outcome == 'COMPLETED':
handle.succeed()
elif snapshot.outcome == 'CANCELED' and handle.is_cancel_requested:
@@ -542,7 +607,7 @@ def create_ros_node(config: ProxyConfig, *, map_id: str, action_name: str):
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.status = NavigationResult.FAILED
result.result.error_code = 'NAV_REMOTE_CANCELED'
handle.abort()
with self._guard:
@@ -552,23 +617,16 @@ def create_ros_node(config: ProxyConfig, *, map_id: str, action_name: str):
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
try:
assign_navigation_feedback(feedback, snapshot)
except GatewayError as exc:
session.request_cancel()
with self._guard:
self._stop_unknown = True
self._record = None
handle.abort()
future.set_result(self._unknown_result(str(exc), snapshot.error_code or 'NAV_GATEWAY_STOP_UNKNOWN'))
continue
handle.publish_feedback(feedback)
def destroy_node(self):