From 964d1fde67c00c66bfdc3b7676b9e51d1b367968 Mon Sep 17 00:00:00 2001 From: wangfeiyu Date: Tue, 22 Sep 2026 14:35:14 +0800 Subject: [PATCH] fix: align navigation contract and readiness handling --- core/src/workflow.cpp | 5 + core/tests/navigation_readiness_test.cpp | 25 ++ navigation_gateway/backends.py | 26 +- navigation_gateway/gateway.py | 26 +- navigation_gateway/ros2_proxy.py | 142 +++++++--- navigation_gateway/semantic_proxy.py | 46 +++- .../include/bt_executor/ros_driver.hpp | 15 +- ros2/bt_executor/src/ros_driver.cpp | 32 ++- .../tools/native_navigation_probe.cpp | 69 +++++ .../bt_mock_servers/mock_skills.py | 78 ++++-- .../bt_mock_servers/scenarios.py | 12 +- ros2/bt_skill_interfaces/CMakeLists.txt | 1 + .../action/Navigate.action | 29 +- .../msg/NavigationResult.msg | 15 ++ ros2/bt_skill_interfaces/package.xml | 4 +- tests/helpers/live_mock_smoke.py | 2 +- tests/helpers/native_navigation_contract.py | 250 ++++++++++++++++++ tests/test_mock_runtime.py | 74 +++++- tests/test_navigation_catalog.py | 32 +++ tests/test_navigation_gateway.py | 154 +++++++++++ tests/test_ros_contracts.py | 24 +- 21 files changed, 922 insertions(+), 139 deletions(-) create mode 100644 core/tests/navigation_readiness_test.cpp create mode 100644 ros2/bt_executor/tools/native_navigation_probe.cpp create mode 100644 ros2/bt_skill_interfaces/msg/NavigationResult.msg create mode 100644 tests/helpers/native_navigation_contract.py diff --git a/core/src/workflow.cpp b/core/src/workflow.cpp index 7f781d6..02658aa 100644 --- a/core/src/workflow.cpp +++ b/core/src/workflow.cpp @@ -132,6 +132,11 @@ TickStatus StageRunner::run_goal(Stage stage,Skill skill,SteadyTime now,RosTime if(error_code_.empty()&&record->result&&!record->result->error_code.empty()&&(record->state==GoalState::STOP_UNKNOWN||record->cancel_intent||record->result->code!=ResultCode::COMPLETED||record->result->stop!=StopState::CONFIRMED||!record->result->response.valid))error_code_=record->result->error_code; if(record->state==GoalState::STOP_UNKNOWN)return fail("physical stop unknown; robot quarantined"); if(record->state!=GoalState::TERMINAL)return TickStatus::RUNNING; + if(skill==Skill::NAVIGATE&&record->result&&record->result->code==ResultCode::REJECTED) { + // NOT_READY is a deployment/input gate, not permission to retry motion or + // admit another queued task. Recovery uses the existing trusted recheck. + return fail("navigation not ready: "+record->result->error_code+"; awaiting input/backend recovery and explicit recheck"); + } if(record->cancel_intent||!record->result||record->result->code!=ResultCode::COMPLETED||record->result->stop!=StopState::CONFIRMED) {return fail("goal failed/canceled/timed out; automatic motion retry disabled",false);} response=record->result->response;completed_goal=active_goal_; if(!response.valid) { diff --git a/core/tests/navigation_readiness_test.cpp b/core/tests/navigation_readiness_test.cpp new file mode 100644 index 0000000..fa00cea --- /dev/null +++ b/core/tests/navigation_readiness_test.cpp @@ -0,0 +1,25 @@ +#include "workflow_fixture.hpp" +int main() { + for(const auto* code:{"INPUTS_UNHEALTHY","ROBOT_STATE_UNAVAILABLE","ROBOT_ESTOP","BACKEND_NOT_CONFIGURED","NAV_NOT_READY"}) { + Fixture f(std::string("navigation_")+code);auto runner=f.runner();Workflow flow(runner); + auto status=TickStatus::RUNNING; + for(unsigned i=0;i<100&&status==TickStatus::RUNNING;++i) { + f.driver.now=1000000+static_cast(i)*1000000; + runner.update_safety({true,true,Holding::EMPTY,f.driver.now,f.driver.now+1000000000}); + status=flow.tick(SteadyTime{}+Milliseconds(i),f.driver.now); + for(auto& event:f.driver.events) { + if(event.kind==EventKind::RESULT&&!f.driver.sent.empty()&&f.driver.sent.back().skill==Skill::NAVIGATE) { + event.native_status=NativeStatus::ABORTED;event.result.code=ResultCode::REJECTED; + event.result.error_code=code;event.result.stop=StopState::CONFIRMED; + } + } + } + assert(status==TickStatus::INTERVENTION_REQUIRED); + assert(runner.error_code()==code);assert(f.count(Skill::NAVIGATE)==1);assert(f.count(Skill::PICK)==0); + for(unsigned i=100;i<120;++i) { + assert(flow.tick(SteadyTime{}+Milliseconds(i),f.driver.now)==TickStatus::INTERVENTION_REQUIRED); + } + assert(f.count(Skill::NAVIGATE)==1); + } + std::cout<<"navigation NOT_READY retains reason and requires explicit recovery without motion resend\n"; +} diff --git a/navigation_gateway/backends.py b/navigation_gateway/backends.py index ffe5c64..c710b2b 100644 --- a/navigation_gateway/backends.py +++ b/navigation_gateway/backends.py @@ -16,7 +16,7 @@ class NavigationBackend(ABC): @abstractmethod def health(self): - """Return ready, reason, map_id, received_at (local monotonic), source_fresh.""" + """Return readiness plus a timestamped blocked boolean sample; absence is unknown.""" @abstractmethod def send(self, goal): @@ -48,9 +48,11 @@ class MockBackend(NavigationBackend): return (sample.get("source_fresh") is True and isinstance(stamp, (int, float)) and math.isfinite(stamp) and 0 < stamp <= self.source_clock()) - def health_sample(self, ready): + def health_sample(self, ready, blocked=False): self.health_value = {"ready": ready, "reason": "SIMULATION", "map_id": self.map_id, "received_at": self.clock(), "source_fresh": True, "source_stamp": self.source_clock()} + self.health_value["blocked"] = ({"value": blocked, "received_at": self.clock(), + "source_fresh": True, "source_stamp": self.source_clock()} if type(blocked) is bool else None) def health(self): if self.auto_complete_sec is not None: self.health_sample(True) @@ -59,7 +61,8 @@ class MockBackend(NavigationBackend): def send(self, goal): self.send_count += 1 self.goals[goal["goal_id"]] = (copy.deepcopy(goal), self.clock()) - self.samples[goal["goal_id"]] = {"controller_state": "REJECTED" if self.send_mode == "REJECTED" else "ACTIVE"} + self.samples[goal["goal_id"]] = {"controller_state": "REJECTED" if self.send_mode == "REJECTED" else "ACTIVE", + "blocked": copy.deepcopy(self.health_value.get("blocked"))} return self.send_mode def cancel(self, goal_id): @@ -67,9 +70,11 @@ class MockBackend(NavigationBackend): self.cancelled.add(goal_id) return True # ACK only; tests supply independent telemetry. - def set_snapshot(self, goal_id, state, linear=0.0, angular=0.0, pose=None, source_fresh=True): + def set_snapshot(self, goal_id, state, linear=0.0, angular=0.0, pose=None, source_fresh=True, blocked=False): self.sequence += 1 self.samples[goal_id] = {"controller_state": state, + "blocked": ({"value": blocked, "received_at": self.clock(), "source_fresh": source_fresh, + "source_stamp": self.source_clock()} if type(blocked) is bool else None), "odom": {"sequence": self.sequence, "received_at": self.clock(), "source_fresh": source_fresh, "source_stamp": self.source_clock(), "linear": linear, "angular": angular}, "pose": {"value": copy.deepcopy(pose), "received_at": self.clock(), "source_fresh": source_fresh, @@ -92,7 +97,7 @@ class Ros1MoveBaseBackend(NavigationBackend): All ROS endpoint names and freshness limits must be configured. A trusted ROS safety/health monitor publishes JSON String: ready, map_id, stamp (ROS seconds), - reason; HTTP clients cannot set these values. ROS1 graph must be access controlled. + reason, blocked (explicit bool); HTTP clients cannot set these values. ROS1 graph must be access controlled. """ def __init__(self, *, action_name, odom_topic, pose_topic, readiness_topic, map_id, source_max_age_sec, server_wait_sec): @@ -176,10 +181,14 @@ class Ros1MoveBaseBackend(NavigationBackend): import json try: data = json.loads(msg.data) + if not isinstance(data, dict): raise ValueError("health payload must be an object") metadata = self._source_metadata(float(data["stamp"])) valid = data.get("ready") is True and data.get("map_id") == self.map_id and metadata["source_fresh"] value = {"ready": valid, "map_id": data.get("map_id"), **metadata, - "reason": str(data.get("reason", "")), "received_at": time.monotonic()} + "reason": str(data.get("reason", "")), "received_at": time.monotonic(), + "error_code": data.get("error_code") if isinstance(data.get("error_code"), str) else ""} + value["blocked"] = ({"value": data["blocked"], **metadata, "received_at": value["received_at"]} + if type(data.get("blocked")) is bool else None) except (ValueError, TypeError, KeyError): value = {"ready": False, "map_id": self.map_id, "source_fresh": False, "reason": "invalid health monitor payload", "received_at": time.monotonic()} @@ -223,8 +232,9 @@ class Ros1MoveBaseBackend(NavigationBackend): # get_state also surfaces LOST without waiting for a done callback. state = self.status_names.get(self.client.get_state(), self.state) out = {"controller_state": state, "odom": copy.deepcopy(self.odom), "pose": copy.deepcopy(self.pose), - "odom_samples": copy.deepcopy(self.odom_queue)} - for sample in [out["odom"], out["pose"]] + out["odom_samples"]: + "odom_samples": copy.deepcopy(self.odom_queue), + "blocked": copy.deepcopy(self.health_value.get("blocked"))} + for sample in [out["odom"], out["pose"], out["blocked"]] + out["odom_samples"]: if isinstance(sample, dict): sample["source_fresh"] = self.source_is_fresh(sample) self.odom_queue.clear() return out diff --git a/navigation_gateway/gateway.py b/navigation_gateway/gateway.py index f1360b0..01b25af 100644 --- a/navigation_gateway/gateway.py +++ b/navigation_gateway/gateway.py @@ -55,7 +55,6 @@ def validate_goal(body): elif not isinstance(value, str) or len(value) > 256: raise GatewayError("invalid trace text") validate_pose(body["target_pose"]) for key in ("position_tolerance", "yaw_tolerance", "timeout_sec"): number(body[key], key, positive=True) - if body["yaw_tolerance"] > math.pi: raise GatewayError("yaw_tolerance is radians and must be <= pi") return copy.deepcopy(body) @@ -130,10 +129,14 @@ class Gateway: try: health = self.backend.health() except Exception: health = {} fresh = self._fresh(health, self.config.readiness_max_age_sec) - ready = health.get("ready") is True and fresh and not self._occupied() + blocked_known = self._blocked_known(health.get("blocked")) + ready = health.get("ready") is True and fresh and blocked_known and not self._occupied() return {"ready": ready, "map_id": health.get("map_id"), "stamp_monotonic": self.clock(), + "error_code": ("" if self._occupied() else "INPUTS_UNHEALTHY" if not blocked_known + else health.get("error_code", "") if fresh else ""), "reason": "motion resource occupied or quarantined" if self._occupied() else - (health.get("reason", "") if fresh else "readiness missing or stale")} + ("blocked telemetry missing or stale" if not blocked_known else + health.get("reason", "") if fresh else "readiness missing or stale")} def submit(self, body): goal = validate_goal(body) @@ -189,6 +192,10 @@ class Gateway: stamp = sample.get("received_at") return isinstance(stamp, (int, float)) and math.isfinite(stamp) and 0 <= self.clock()-stamp <= maximum + def _blocked_known(self, sample): + return (self._fresh(sample, self.config.readiness_max_age_sec) and + type(sample.get("value")) is bool) + def poll(self, goal_id): with self.lock: r = self._record(goal_id) @@ -198,14 +205,25 @@ class Gateway: if not r["_cancel_sent"]: try: health = self.backend.health() except Exception: health = {} - if not self._fresh(health, self.config.readiness_max_age_sec) or health.get("ready") is not True or health.get("map_id") != r["request"]["map_id"]: + if not self._fresh(health, self.config.readiness_max_age_sec) or health.get("ready") is not True or health.get("map_id") != r["request"]["map_id"] or not self._blocked_known(health.get("blocked")): self.cancel(goal_id, "FAILED") + if (self._fresh(health, self.config.readiness_max_age_sec) and + health.get("error_code") in {"INPUTS_UNHEALTHY", "ROBOT_STATE_UNAVAILABLE", "BACKEND_NOT_CONFIGURED"}): + r["error_code"] = health["error_code"] + if not self._blocked_known(health.get("blocked")): r["error_code"] = "INPUTS_UNHEALTHY" r["message"] = "readiness lost during execution; stop requested" try: snapshot = self.backend.snapshot(goal_id) except Exception: snapshot = {"controller_state": "UNKNOWN"} state = snapshot.get("controller_state", "UNKNOWN") r.update(controller_state=state, sequence=r["sequence"]+1, elapsed=max(0, self.clock()-r["_started"])) r.update(pose_valid=False, current_pose=None) + blocked_sample = snapshot.get("blocked") + r["blocked"] = (blocked_sample["value"] if + self._fresh(blocked_sample, self.config.readiness_max_age_sec) and + type(blocked_sample.get("value")) is bool else None) + if r["blocked"] is None: + if not r["_cancel_sent"]: self.cancel(goal_id, "FAILED") + r["error_code"] = "INPUTS_UNHEALTHY" pose_sample = snapshot.get("pose") if self._fresh(pose_sample, self.config.pose_max_age_sec): try: diff --git a/navigation_gateway/ros2_proxy.py b/navigation_gateway/ros2_proxy.py index a1fe39c..de60223 100644 --- a/navigation_gateway/ros2_proxy.py +++ b/navigation_gateway/ros2_proxy.py @@ -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): diff --git a/navigation_gateway/semantic_proxy.py b/navigation_gateway/semantic_proxy.py index 4f67971..405b26f 100644 --- a/navigation_gateway/semantic_proxy.py +++ b/navigation_gateway/semantic_proxy.py @@ -9,6 +9,31 @@ def duration_seconds(value): if not 0= 0 and abs(source.final_yaw_error) <= math.pi) + target.final_position_error = source.final_position_error + target.final_orientation_error = source.final_yaw_error + + def main(): import rclpy from rclpy.node import Node @@ -16,6 +41,8 @@ def main(): from rclpy.callback_groups import ReentrantCallbackGroup from rclpy.executors import MultiThreadedExecutor from bt_skill_interfaces.action import Navigate,NavigateSemantic + from bt_skill_interfaces.msg import NavigationResult,ExecutionResult + from action_msgs.msg import GoalStatus class Proxy(Node): def __init__(self): super().__init__('navigation_semantic_proxy');self.group=ReentrantCallbackGroup();self.lock=threading.Lock();self.reserved=False;self.faulted=False @@ -38,7 +65,7 @@ def main(): try: g=h.request;lookup=self.catalog.resolve(g.kind,g.reference,g.registry_version,shelf=g.shelf_id,side=g.side_id,column=g.column_id,tier=g.tier_id) deadline=self.accepted_at+duration_seconds(g.timeout) - q=Navigate.Goal();q.trace=g.trace;q.position_tolerance=g.position_tolerance;q.orientation_tolerance=g.orientation_tolerance + q=Navigate.Goal();q.trace=g.trace;q.position_tolerance=g.position_tolerance;q.yaw_tolerance=g.orientation_tolerance p=lookup['pose'];q.target_pose.header.frame_id=p['frame_id'];q.target_pose.header.stamp=self.get_clock().now().to_msg() for k in ('x','y','z'):setattr(q.target_pose.pose.position,k,float(p[k])) for k in ('x','y','z','w'):setattr(q.target_pose.pose.orientation,k,float(p['q'+k])) @@ -46,7 +73,12 @@ def main(): remaining_ns=int((deadline-time.monotonic())*1e9) if remaining_ns<=0 or h.is_cancel_requested:raise RuntimeError('EXPIRED_BEFORE_DISPATCH') q.timeout.sec=remaining_ns//1_000_000_000;q.timeout.nanosec=remaining_ns%1_000_000_000 - future=self.client.send_goal_async(q);acceptance_deadline=min(deadline,time.monotonic()+3.);sequence=0;last_feedback=0. + def forward_feedback(event): + source=event.feedback + f=NavigateSemantic.Feedback() + for field in ('stamp','sequence','phase','message'):setattr(f,field,getattr(source,field)) + h.publish_feedback(f) + future=self.client.send_goal_async(q,feedback_callback=forward_feedback);acceptance_deadline=min(deadline,time.monotonic()+3.) while not future.done(): if h.is_cancel_requested or time.monotonic()>acceptance_deadline: def late(f): @@ -63,20 +95,18 @@ def main(): now=time.monotonic() if (h.is_cancel_requested or now>=deadline) and cancel_at is None:downstream.cancel_goal_async();cancel_at=now if cancel_at is not None and now-cancel_at>5:raise RuntimeError('STOP_UNKNOWN') - if now-last_feedback>=.2: - sequence+=1;f=NavigateSemantic.Feedback();f.stamp=self.get_clock().now().to_msg();f.sequence=sequence;f.phase=2 if cancel_at else 1;f.message='awaiting downstream result';h.publish_feedback(f);last_feedback=now time.sleep(.01) wrapped=done.result();m=wrapped.result - for field in ('result','final_pose','pose_valid','final_position_error','final_orientation_error','errors_valid'):setattr(result,field,getattr(m,field)) - if wrapped.status==4 and m.result.status==0:h.succeed() - elif wrapped.status==5 and h.is_cancel_requested:h.canceled() + translate_navigation_result(m,result,NavigationResult,ExecutionResult) + if wrapped.status==GoalStatus.STATUS_SUCCEEDED and m.result.status==NavigationResult.SUCCEEDED:h.succeed() + elif wrapped.status==GoalStatus.STATUS_CANCELED and m.result.status==NavigationResult.CANCELED and h.is_cancel_requested:h.canceled() else:h.abort() except Exception as ex: self.faulted=True if downstream is not None and downstream.accepted: try:downstream.cancel_goal_async() except Exception:pass - result.result.status=1;result.result.stop_state=0;result.result.error_code=str(ex);h.abort() + result.result.status=ExecutionResult.FAILED;result.result.stop_state=ExecutionResult.UNKNOWN;result.result.error_code=str(ex);h.abort() finally: with self.lock:self.reserved=False return result diff --git a/ros2/bt_executor/include/bt_executor/ros_driver.hpp b/ros2/bt_executor/include/bt_executor/ros_driver.hpp index 125bc60..b42029c 100644 --- a/ros2/bt_executor/include/bt_executor/ros_driver.hpp +++ b/ros2/bt_executor/include/bt_executor/ros_driver.hpp @@ -98,6 +98,7 @@ class RosDriver final : public robot_bt::GoalDriver { bool fresh(robot_bt::RosTime observed, robot_bt::RosTime valid_until, robot_bt::RosTime capture_after = 0) const; robot_bt::ExecutionResult execution(const iface::msg::ExecutionResult&,const robot_bt::GoalRequest&) const; + robot_bt::ExecutionResult execution(const iface::msg::NavigationResult&,const robot_bt::GoalRequest&) const; robot_bt::ExecutionResult readonly_result(rclcpp_action::ResultCode, bool valid, robot_bt::SkillResponse) const; robot_bt::SnapshotMeta meta(const robot_bt::GoalRequest&, robot_bt::RosTime, @@ -137,13 +138,13 @@ class RosDriver final : public robot_bt::GoalDriver { const std::shared_ptr feedback) { if (!handle || !feedback || feedback->sequence == 0 || feedback->phase > max_phase) return; if constexpr (std::is_same_v) { - if(feedback->errors_valid&&(!std::isfinite(feedback->position_error)||feedback->position_error<0|| - !std::isfinite(feedback->orientation_error)||std::abs(feedback->orientation_error)>std::acos(-1.0)))return; - if(feedback->pose_valid) { + if(feedback->error_valid&&(!std::isfinite(feedback->position_error)||feedback->position_error<0|| + !std::isfinite(feedback->yaw_error)||std::abs(feedback->yaw_error)>std::acos(-1.0)))return; + if(feedback->current_pose_valid) { const auto& p=feedback->current_pose; robot_bt::Pose pose{p.header.frame_id,p.pose.position.x,p.pose.position.y,p.pose.position.z, p.pose.orientation.x,p.pose.orientation.y,p.pose.orientation.z,p.pose.orientation.w}; - if(!robot_bt::valid_pose(pose))return; + if(pose.frame_id!="map"||!robot_bt::valid_pose(pose))return; } } if constexpr (std::is_same_v) { @@ -168,9 +169,9 @@ class RosDriver final : public robot_bt::GoalDriver { payload["progress_valid"]=feedback->progress_valid;payload["progress"]=feedback->progress; } if constexpr (std::is_same_v) { - payload["pose_valid"]=feedback->pose_valid;payload["errors_valid"]=feedback->errors_valid; - payload["position_error"]=feedback->position_error;payload["orientation_error"]=feedback->orientation_error; - payload["blocked_valid"]=feedback->blocked_valid;payload["blocked"]=feedback->blocked; + payload["current_pose_valid"]=feedback->current_pose_valid;payload["error_valid"]=feedback->error_valid; + payload["position_error"]=feedback->position_error;payload["yaw_error"]=feedback->yaw_error; + payload["blocked"]=feedback->blocked; } event.feedback_snapshot=payload.dump();events_.push_back(event); }; diff --git a/ros2/bt_executor/src/ros_driver.cpp b/ros2/bt_executor/src/ros_driver.cpp index d856459..0a01f4c 100644 --- a/ros2/bt_executor/src/ros_driver.cpp +++ b/ros2/bt_executor/src/ros_driver.cpp @@ -170,6 +170,24 @@ ExecutionResult RosDriver::execution(const iface::msg::ExecutionResult& m,const r.stop=StopState::CONFIRMED; return r; } +ExecutionResult RosDriver::execution(const iface::msg::NavigationResult& m,const GoalRequest& request)const { + // Navigation outcomes have different numeric values from other skill results. + iface::msg::ExecutionResult common; + common.error_code=m.error_code;common.message=m.message; + common.stop_state=m.stop_state;common.stopped_at=m.stopped_at;common.stop_evidence_ref=m.stop_evidence_ref; + using Nav=iface::msg::NavigationResult; + using Common=iface::msg::ExecutionResult; + switch(m.status) { + case Nav::SUCCEEDED:common.status=Common::COMPLETED;break; + case Nav::CANCELED:common.status=Common::CANCELED;break; + case Nav::TIMEOUT:common.status=Common::TIMED_OUT;break; + case Nav::BLOCKED:common.status=Common::FAILED;if(common.error_code.empty())common.error_code="NAV_BLOCKED";break; + case Nav::NOT_READY:common.status=Common::REJECTED;if(common.error_code.empty())common.error_code="NAV_NOT_READY";break; + case Nav::FAILED:common.status=Common::FAILED;break; + default:{ExecutionResult invalid;invalid.error_code="NAV_RESULT_PROTOCOL_ERROR";return invalid;} + } + return execution(common,request); +} ExecutionResult RosDriver::readonly_result(rclcpp_action::ResultCode native_code,bool valid,SkillResponse response)const { ExecutionResult r;r.response=std::move(response);r.response.valid=valid; // These servers are contractually read-only. Their native terminal is enough @@ -210,13 +228,13 @@ void RosDriver::send(const GoalRequest& r) { });break; } Navigate::Goal g;g.trace=trace_msg(r.trace);g.target_pose=pose_msg(*r.registered_pose,now); - g.position_tolerance=r.position_tolerance_m;g.orientation_tolerance=r.orientation_tolerance_rad;g.timeout=timeout(); - send_typed(navigate_,g,r,6,[this,r](const Navigate::Result& m,auto){ - auto out=execution(m.result,r);out.response.valid=m.pose_valid&&m.errors_valid&& - std::isfinite(m.final_position_error)&&std::isfinite(m.final_orientation_error)&& - m.final_position_error>=0&&std::abs(m.final_orientation_error)<=std::acos(-1.0)&& - m.final_position_error<=r.position_tolerance_m&&std::abs(m.final_orientation_error)<=r.orientation_tolerance_rad; - if(m.pose_valid)out.response.final_pose=pose_core(m.final_pose); + g.position_tolerance=r.position_tolerance_m;g.yaw_tolerance=r.orientation_tolerance_rad;g.timeout=timeout(); + send_typed(navigate_,g,r,Navigate::Feedback::STOPPING,[this,r](const Navigate::Result& m,auto){ + auto out=execution(m.result,r);out.response.valid=m.final_pose_valid&& + std::isfinite(m.final_position_error)&&std::isfinite(m.final_yaw_error)&& + m.final_position_error>=0&&std::abs(m.final_yaw_error)<=std::acos(-1.0)&& + m.final_position_error<=r.position_tolerance_m&&std::abs(m.final_yaw_error)<=r.orientation_tolerance_rad; + if(m.final_pose_valid)out.response.final_pose=pose_core(m.final_pose); out.response.base_stopped=out.stop==StopState::CONFIRMED;return out;});break; } case Skill::PICK:case Skill::PLACE: { diff --git a/ros2/bt_executor/tools/native_navigation_probe.cpp b/ros2/bt_executor/tools/native_navigation_probe.cpp new file mode 100644 index 0000000..05de919 --- /dev/null +++ b/ros2/bt_executor/tools/native_navigation_probe.cpp @@ -0,0 +1,69 @@ +// Test-only live DDS probe. Build via tests/helpers/native_navigation_contract.py. +#include +#include +#include +#include + +int main(int argc,char** argv) { + if(argc!=4) return 2; + const int scenario=std::stoi(argv[1]); + const std::string journal=argv[2], ns=argv[3]; + if(ns.rfind("/sim/",0)!=0) return 3; + rclcpp::init(0,nullptr); + try { + auto node=std::make_shared("native_navigation_probe",ns); + bt_executor::RosDriver driver(*node,"robot_01",journal+"/uuids.jsonl",0.5,10000000000LL,robot_bt::Milliseconds(5000)); + robot_bt::TaskConfig task; task.route="LEGACY"; + driver.bind_task(task,robot_bt::SiteConfig{},1); + robot_bt::ActiveGoalRegistry registry(driver,journal+"/goals.jsonl"); + auto until=robot_bt::SteadyClock::now()+std::chrono::seconds(12); + while(!driver.ready(robot_bt::Skill::NAVIGATE)&&robot_bt::SteadyClock::now()now().nanoseconds(); + const auto started=registry.start(request,robot_bt::SteadyClock::now()); + if(!started) throw std::runtime_error("start refused"); + const std::string active_id=*started; + bool canceled=false; + until=robot_bt::SteadyClock::now()+std::chrono::seconds(12); + while(robot_bt::SteadyClock::now()accepted&&!canceled) { + registry.request_cancel(active_id,robot_bt::SteadyClock::now());canceled=true; + } + if(record->result) break; + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + // Pump beyond terminal delivery to expose accidental sends/retries in live transport. + until=robot_bt::SteadyClock::now()+std::chrono::milliseconds(350); + while(robot_bt::SteadyClock::now()result) throw std::runtime_error("result timeout"); + const auto& result=*record->result; + bool blocked_redispatch=false; + if(scenario==6) { + auto next=request;next.goal_id+="-forbidden-retry"; + next.trace.subtask_id="navigate-forbidden-retry";++next.trace.attempt; + blocked_redispatch=!registry.start(next,robot_bt::SteadyClock::now()).has_value(); + } + nlohmann::json report={{"scenario",scenario},{"code",int(result.code)},{"stop",int(result.stop)}, + {"state",int(record->state)},{"robot_locked",registry.robot_locked("robot_01")}, + {"error_code",result.error_code},{"detail",result.detail},{"feedback_sequence",record->last_sequence}, + {"feedback",record->feedback_snapshot},{"wire_request_type",record->request.wire_request_type}, + {"wire_result_type",result.wire_result_type},{"wire_result_bytes",result.wire_result_snapshot.size()/2}, + {"response_valid",result.response.valid},{"mapping_count",driver.mappings().size()}, + {"unknown_stop_blocks_redispatch",blocked_redispatch}}; + std::cout< 0.001: - raise ValueError("navigation quaternion must have unit norm") + if not any((q.x, q.y, q.z, q.w)): + raise ValueError("navigation quaternion must be nonzero") elif name == "execute_manipulation": if request.skill not in ("pick", "place") or not request.instruction.strip(): raise ValueError("manipulation skill/instruction is invalid") @@ -310,7 +324,7 @@ class MockSkills(Node): self._feedback(name, handle, sequence, now - started, phases[phase_index]) emitted_phase = phase_index time.sleep(min(0.02, max(0, deadline - now))) - if kind == "failed" and outcome == ExecutionResult.COMPLETED: + if (kind == "failed" or name == "navigate" and kind in ("blocked", "not_ready")) and outcome == ExecutionResult.COMPLETED: outcome = ExecutionResult.FAILED if kind == "stop_unknown" and outcome == ExecutionResult.COMPLETED: outcome, stop_state = ExecutionResult.FAILED, ExecutionResult.UNKNOWN @@ -320,7 +334,7 @@ class MockSkills(Node): # Unknown execution state stays reserved. A client cannot infer stop from this exception. stop_state = ExecutionResult.UNKNOWN if name in MOTION else ExecutionResult.CONFIRMED if hasattr(result, "result"): - result.result = self._execution_result(ExecutionResult.FAILED, stop_state, goal_id, "MOCK_EXCEPTION", str(exc)) + result.result = self._execution_result(ExecutionResult.FAILED, stop_state, goal_id, "MOCK_EXCEPTION", str(exc), name=name) elif hasattr(result, "evidence"): result.evidence.status = VerificationEvidence.UNKNOWN result.evidence.error_code = "MOCK_EXCEPTION" @@ -371,13 +385,12 @@ class MockSkills(Node): if hasattr(feedback, "elapsed_time"): feedback.elapsed_time = elapsed_message(elapsed) if name == "navigate": - feedback.pose_valid = True - feedback.current_pose = copy.deepcopy(handle.request.target_pose) - feedback.errors_valid = True + feedback.current_pose_valid = True + feedback.current_pose = normalized_navigation_pose(handle.request.target_pose) + feedback.error_valid = True feedback.position_error = 0.0 - feedback.orientation_error = 0.0 - feedback.blocked_valid = True - feedback.blocked = False + feedback.yaw_error = 0.0 + feedback.blocked = feedback.phase == Navigate.Feedback.BLOCKED if name == "execute_manipulation": feedback.progress_valid = False handle.publish_feedback(feedback) @@ -389,9 +402,18 @@ class MockSkills(Node): if stopping is not None: self._feedback(name, handle, sequence, elapsed, stopping) - def _execution_result(self, outcome, stop_state, goal_id, error="", message="SIMULATED execution only"): - result = ExecutionResult() - result.status, result.stop_state = outcome, stop_state + def _execution_result(self, outcome, stop_state, goal_id, error="", message="SIMULATED execution only", name="", kind="normal"): + result = NavigationResult() if name == "navigate" else ExecutionResult() + if name == "navigate": + status = {ExecutionResult.COMPLETED: NavigationResult.SUCCEEDED, + ExecutionResult.CANCELED: NavigationResult.CANCELED, + ExecutionResult.TIMED_OUT: NavigationResult.TIMEOUT, + ExecutionResult.FAILED: NavigationResult.FAILED}[outcome] + if outcome == ExecutionResult.FAILED and kind in ("blocked", "not_ready"): + status = NavigationResult.BLOCKED if kind == "blocked" else NavigationResult.NOT_READY + else: + status = outcome + result.status, result.stop_state = status, stop_state result.error_code = error or ("" if outcome == ExecutionResult.COMPLETED else "MOCK_TERMINATED") result.message = message if stop_state == ExecutionResult.CONFIRMED: @@ -408,14 +430,20 @@ class MockSkills(Node): record = "sim://" + name + "/" + goal_id ok = outcome == ExecutionResult.COMPLETED if hasattr(result, "result"): - result.result = self._execution_result(outcome, stop_state, goal_id) + error = "" + if name == "navigate" and outcome == ExecutionResult.FAILED: + if kind == "blocked": error = "BLOCKED" + if kind == "not_ready": error = fixture.get("error_code", "INPUTS_UNHEALTHY") + result.result = self._execution_result(outcome, stop_state, goal_id, error, name=name, kind=kind) if name in ("navigate", "navigate_semantic"): if ok and stop_state == ExecutionResult.CONFIRMED: with self.lock: self.geometry_epoch += 1 if name == "navigate": - result.pose_valid = result.errors_valid = ok - result.final_pose = copy.deepcopy(request.target_pose) + result.final_pose_valid = ok + if ok: + result.final_pose = normalized_navigation_pose(request.target_pose) + result.final_position_error = result.final_yaw_error = 0.0 else: pose = fixture.get("final_pose") result.pose_valid = result.errors_valid = ok and pose is not None diff --git a/ros2/bt_mock_servers/bt_mock_servers/scenarios.py b/ros2/bt_mock_servers/bt_mock_servers/scenarios.py index 1eacc8e..6939c3b 100644 --- a/ros2/bt_mock_servers/bt_mock_servers/scenarios.py +++ b/ros2/bt_mock_servers/bt_mock_servers/scenarios.py @@ -17,7 +17,7 @@ KINDS = { "unavailable", "invalid_pose", "emergency_stop", "protective_stop", "model_estimate", "fused", "cancel_stop_unknown", "timeout_stop_unknown", - "obstacle_recovery", + "obstacle_recovery", "blocked", "not_ready", } FIXTURE_FIELDS = { "kind", "duration_seconds", "shelf_id", "side_id", "column_id", "tier_id", @@ -26,12 +26,12 @@ FIXTURE_FIELDS = { "observation_id", "image_path", "station_id", "registry_version", "calibration_id", "geometry_epoch", "status_json", "stop_delay_seconds", - "final_pose", + "final_pose", "error_code", } BASE_KINDS = {"normal", "failed", "timeout", "silence", "reject", "native_mismatch"} ACTION_KINDS = { - "navigate": BASE_KINDS | {"stop_unknown", "cancel_stop_unknown", "timeout_stop_unknown", "obstacle_recovery"}, + "navigate": BASE_KINDS | {"stop_unknown", "cancel_stop_unknown", "timeout_stop_unknown", "obstacle_recovery", "blocked", "not_ready"}, "navigate_semantic": BASE_KINDS | {"stop_unknown", "cancel_stop_unknown", "timeout_stop_unknown"}, "execute_manipulation": BASE_KINDS | {"stop_unknown", "cancel_stop_unknown", "timeout_stop_unknown"}, "execute_posture": BASE_KINDS | {"stop_unknown", "cancel_stop_unknown", "timeout_stop_unknown"}, @@ -86,6 +86,12 @@ def parse_scenarios(raw): raise ValueError("unsupported fixture kind") if fixture.get("kind", "normal") not in ACTION_KINDS[name]: raise ValueError("fixture kind has no effect for " + name) + if "error_code" in fixture: + code = fixture["error_code"] + if (name != "navigate" or fixture.get("kind") != "not_ready" or + code not in {"INPUTS_UNHEALTHY", "ROBOT_STATE_UNAVAILABLE", "BACKEND_NOT_CONFIGURED", + "ROBOT_EMERGENCY_STOP", "ROBOT_PROTECTIVE_STOP", "ROBOT_MOTION_NOT_ALLOWED"}): + raise ValueError("error_code requires a supported navigation readiness reason") delay = fixture.get("duration_seconds", 0.2) if type(delay) not in (int, float) or not math.isfinite(delay) or not 0 <= delay <= 120: raise ValueError("fixture duration must be finite in [0,120]") diff --git a/ros2/bt_skill_interfaces/CMakeLists.txt b/ros2/bt_skill_interfaces/CMakeLists.txt index 27af1b0..f6fe3f6 100644 --- a/ros2/bt_skill_interfaces/CMakeLists.txt +++ b/ros2/bt_skill_interfaces/CMakeLists.txt @@ -10,6 +10,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/ObjectTarget.msg" "msg/RegionTarget.msg" "msg/ExecutionResult.msg" + "msg/NavigationResult.msg" "msg/ObservationContext.msg" "msg/RobotState.msg" "msg/SafetyState.msg" diff --git a/ros2/bt_skill_interfaces/action/Navigate.action b/ros2/bt_skill_interfaces/action/Navigate.action index 0d05421..a7f9b8b 100644 --- a/ros2/bt_skill_interfaces/action/Navigate.action +++ b/ros2/bt_skill_interfaces/action/Navigate.action @@ -1,33 +1,30 @@ -# Preserved outer interface from BT DR pp13-14. Canonical ROS2 type: Navigate. +# Navigation contract: trace and stop evidence retained across all attempts. +# Enum values are explicit; both peers must build this same interface package. bt_skill_interfaces/TaskTrace trace geometry_msgs/PoseStamped target_pose float64 position_tolerance -float64 orientation_tolerance +float64 yaw_tolerance builtin_interfaces/Duration timeout --- -bt_skill_interfaces/ExecutionResult result -bool pose_valid +bt_skill_interfaces/NavigationResult result +bool final_pose_valid geometry_msgs/PoseStamped final_pose -bool errors_valid float64 final_position_error -float64 final_orientation_error +float64 final_yaw_error --- -uint8 CHECKING=0 -uint8 PLANNING=1 +uint8 ACCEPTED=0 +uint8 CHECKING=1 uint8 NAVIGATING=2 -uint8 WAITING_OBSTACLE=3 -uint8 RECOVERING=4 -uint8 ARRIVING=5 -uint8 STOPPING=6 +uint8 BLOCKED=3 +uint8 STOPPING=4 builtin_interfaces/Time stamp uint32 sequence uint8 phase -bool pose_valid +bool current_pose_valid geometry_msgs/PoseStamped current_pose -bool errors_valid +bool error_valid float64 position_error -float64 orientation_error -bool blocked_valid +float64 yaw_error bool blocked builtin_interfaces/Duration elapsed_time string message diff --git a/ros2/bt_skill_interfaces/msg/NavigationResult.msg b/ros2/bt_skill_interfaces/msg/NavigationResult.msg new file mode 100644 index 0000000..d29968d --- /dev/null +++ b/ros2/bt_skill_interfaces/msg/NavigationResult.msg @@ -0,0 +1,15 @@ +# Navigation-specific outcomes; do not decode with ExecutionResult enum values. +uint8 SUCCEEDED=0 +uint8 CANCELED=1 +uint8 TIMEOUT=2 +uint8 BLOCKED=3 +uint8 NOT_READY=4 +uint8 FAILED=5 +uint8 UNKNOWN=0 +uint8 CONFIRMED=1 +uint8 status +string error_code +string message +uint8 stop_state +builtin_interfaces/Time stopped_at +string stop_evidence_ref diff --git a/ros2/bt_skill_interfaces/package.xml b/ros2/bt_skill_interfaces/package.xml index bcc3349..b4447e8 100644 --- a/ros2/bt_skill_interfaces/package.xml +++ b/ros2/bt_skill_interfaces/package.xml @@ -1,8 +1,8 @@ bt_skill_interfaces - 1.2.0 - Candidate v1 robot behavior-tree skill and evidence contracts. + 2.0.0 + Robot skill contracts with navigation-specific outcomes and stop evidence. wangfeiyu Proprietary ament_cmake diff --git a/tests/helpers/live_mock_smoke.py b/tests/helpers/live_mock_smoke.py index dcea99d..0672d50 100644 --- a/tests/helpers/live_mock_smoke.py +++ b/tests/helpers/live_mock_smoke.py @@ -32,7 +32,7 @@ def goal_for(name): if name == "navigate": goal.target_pose.header.frame_id = "map" goal.target_pose.pose.orientation.w = 1.0 - goal.position_tolerance = goal.orientation_tolerance = 0.1 + goal.position_tolerance = goal.yaw_tolerance = 0.1 elif name == "navigate_semantic": goal.kind, goal.reference, goal.registry_version = "LOCATION", "bin_A", 1 goal.position_tolerance = goal.orientation_tolerance = 0.1 diff --git a/tests/helpers/native_navigation_contract.py b/tests/helpers/native_navigation_contract.py new file mode 100644 index 0000000..6e378f8 --- /dev/null +++ b/tests/helpers/native_navigation_contract.py @@ -0,0 +1,250 @@ +#!/usr/bin/env python3 +"""Generated Navigate messages + real DDS + production C++ RosDriver regression. + +In a sourced ROS Humble overlay: + python3 tests/helpers/native_navigation_contract.py --build-native --output /tmp/nav-contract +An external temporary CMake project compiles the existing production driver/core; +no ROS package CMake or installed executable is modified. All endpoints use a +unique /sim namespace. This is contract evidence, not physical navigation evidence. +""" +import argparse +from collections import Counter +from copy import deepcopy +import json +import os +from pathlib import Path +import subprocess +import tempfile +import threading +import time + + +def build_native(root, directory): + directory.mkdir(parents=True, exist_ok=True) + # Paths are CMake quoted so workspace names containing spaces remain valid. + q = lambda p: '"' + str(p).replace('\\', '/').replace('"', '\\"') + '"' + (directory / 'CMakeLists.txt').write_text('''cmake_minimum_required(VERSION 3.16) +project(native_navigation_probe LANGUAGES CXX) +set(CMAKE_CXX_STANDARD 17) +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_action REQUIRED) +find_package(bt_skill_interfaces REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(std_msgs REQUIRED) +find_package(nlohmann_json REQUIRED) +add_executable(native_navigation_probe %s %s %s) +target_include_directories(native_navigation_probe PRIVATE %s %s) +target_link_libraries(native_navigation_probe nlohmann_json::nlohmann_json) +ament_target_dependencies(native_navigation_probe rclcpp rclcpp_action bt_skill_interfaces geometry_msgs std_msgs) +''' % tuple(q(root / p) for p in ( + 'ros2/bt_executor/tools/native_navigation_probe.cpp', + 'ros2/bt_executor/src/ros_driver.cpp', 'core/src/core.cpp', + 'ros2/bt_executor/include', 'core/include')), encoding='utf-8') + subprocess.run(['cmake', '-S', str(directory), '-B', str(directory / 'build')], check=True) + subprocess.run(['cmake', '--build', str(directory / 'build'), '-j2'], check=True) + return directory / 'build/native_navigation_probe' + + +def wait(future, timeout=12): + until = time.monotonic() + timeout + while not future.done() and time.monotonic() < until: + time.sleep(.01) + assert future.done(), 'ROS future timed out' + return future.result() + + +def run(binary, output): + import rclpy + from rclpy.node import Node + from rclpy.action import ActionClient, ActionServer, CancelResponse + from rclpy.callback_groups import ReentrantCallbackGroup + from rclpy.executors import MultiThreadedExecutor + from bt_skill_interfaces.action import Navigate + from bt_skill_interfaces.msg import NavigationResult + from action_msgs.msg import GoalStatus + + assert [getattr(NavigationResult, name) for name in + ('SUCCEEDED', 'CANCELED', 'TIMEOUT', 'BLOCKED', 'NOT_READY', 'FAILED')] == list(range(6)) + assert [getattr(Navigate.Feedback, name) for name in + ('ACCEPTED', 'CHECKING', 'NAVIGATING', 'BLOCKED', 'STOPPING')] == list(range(5)) + assert set(Navigate.Goal.get_fields_and_field_types()) == { + 'trace', 'target_pose', 'position_tolerance', 'yaw_tolerance', 'timeout'} + assert set(Navigate.Result.get_fields_and_field_types()) == { + 'result', 'final_pose_valid', 'final_pose', 'final_position_error', 'final_yaw_error'} + assert set(Navigate.Feedback.get_fields_and_field_types()) == { + 'stamp', 'sequence', 'phase', 'current_pose_valid', 'current_pose', 'error_valid', + 'position_error', 'yaw_error', 'blocked', 'elapsed_time', 'message'} + namespace = '/sim/navigation_contract_' + str(os.getpid()) + rclpy.init() + node = Node('navigation_contract_fixture', namespace=namespace) + counts = Counter() + failures = [] + lock = threading.Lock() + + def execute(handle): + goal = handle.request + scenario = round(goal.target_pose.pose.position.x) + status = scenario if scenario < 6 else {6: 1, 7: 3, 8: 4}[scenario] + with lock: + counts[(goal.trace.task_id, scenario)] += 1 + try: + assert goal.target_pose.header.frame_id == 'map' + assert goal.target_pose.pose.orientation.w == 1. + assert goal.position_tolerance == .05 and goal.yaw_tolerance == .1 + assert goal.timeout.sec == 5 + assert goal.trace.attempt == 1 and goal.trace.task_revision == 1 + assert goal.trace.plan_version == 1 and goal.trace.execution_generation == 1 + except AssertionError: + failures.append('Goal contract mismatch: ' + str(goal)) + for phase in range(5): + feedback = Navigate.Feedback() + feedback.stamp = node.get_clock().now().to_msg() + feedback.sequence = phase + 1 + feedback.phase = phase + feedback.current_pose_valid = True + feedback.current_pose = goal.target_pose + feedback.error_valid = True + feedback.position_error = .02 + feedback.yaw_error = -.03 + feedback.blocked = phase == Navigate.Feedback.BLOCKED + feedback.elapsed_time.nanosec = (phase + 1) * 10000000 + feedback.message = 'phase-' + str(phase) + handle.publish_feedback(feedback) + time.sleep(.03) + if phase == 2 and goal.trace.task_id.startswith('probe-'): + # A later malformed sample must not poison the accepted sequence. + wrong_frame = deepcopy(feedback) + wrong_frame.sequence = 100 + wrong_frame.current_pose.header.frame_id = 'odom' + handle.publish_feedback(wrong_frame) + wrong_phase = deepcopy(feedback) + wrong_phase.sequence = 101 + wrong_phase.phase = 255 + handle.publish_feedback(wrong_phase) + result = Navigate.Result() + result.result.status = status + result.result.error_code = '' if scenario in (7, 8) else 'FIXTURE_' + str(scenario) + result.result.message = 'outcome-' + str(scenario) + result.result.stop_state = 0 if scenario == 6 else 1 + result.result.stopped_at = node.get_clock().now().to_msg() + result.result.stop_evidence_ref = '' if scenario == 6 else 'sim://navigation/stop' + result.final_pose_valid = True + result.final_pose = goal.target_pose + result.final_position_error = .02 + result.final_yaw_error = -.03 + if status == 1: + deadline = time.monotonic() + 6 + while not handle.is_cancel_requested and time.monotonic() < deadline: + time.sleep(.01) + if not handle.is_cancel_requested: + failures.append('Canceled fixture never received cancellation') + handle.abort() + else: + handle.canceled() + elif status == 0: + handle.succeed() + else: + handle.abort() + return result + + server = ActionServer(node, Navigate, 'skills/navigate', execute_callback=execute, + cancel_callback=lambda _: CancelResponse.ACCEPT, + callback_group=ReentrantCallbackGroup()) + client = ActionClient(node, Navigate, 'skills/navigate', callback_group=ReentrantCallbackGroup()) + executor = MultiThreadedExecutor(num_threads=6) + executor.add_node(node) + thread = threading.Thread(target=executor.spin, daemon=True) + thread.start() + report = {'scope': 'simulation-only generated Navigate and production RosDriver over DDS', + 'namespace': namespace, 'direct': [], 'native': []} + try: + assert client.wait_for_server(timeout_sec=10) + for scenario in range(9): + goal = Navigate.Goal() + goal.trace.task_id = 'direct-' + str(scenario) + goal.trace.run_id = goal.trace.task_id + goal.trace.subtask_id = 'navigate' + goal.trace.attempt = goal.trace.task_revision = goal.trace.plan_version = goal.trace.execution_generation = 1 + goal.target_pose.header.frame_id = 'map' + goal.target_pose.pose.position.x = float(scenario) + goal.target_pose.pose.orientation.w = 1. + goal.position_tolerance = .05 + goal.yaw_tolerance = .1 + goal.timeout.sec = 5 + feedback = [] + handle = wait(client.send_goal_async(goal, feedback_callback=lambda value: feedback.append(value.feedback))) + assert handle.accepted + if scenario in (1, 6): + assert wait(handle.cancel_goal_async()).return_code == 0 + response = wait(handle.get_result_async()) + expected = scenario if scenario < 6 else {6: 1, 7: 3, 8: 4}[scenario] + assert response.result.result.status == expected + assert response.status == (GoalStatus.STATUS_SUCCEEDED if expected == 0 else + GoalStatus.STATUS_CANCELED if expected == 1 else GoalStatus.STATUS_ABORTED) + assert response.result.final_pose_valid + assert response.result.final_pose.pose.position.x == float(scenario) + assert response.result.final_position_error == .02 and response.result.final_yaw_error == -.03 + assert response.result.result.stop_state == (0 if scenario == 6 else 1) + assert response.result.result.error_code == ('' if scenario in (7, 8) else 'FIXTURE_' + str(scenario)) + assert [f.phase for f in feedback] == list(range(5)), feedback + assert [f.sequence for f in feedback] == list(range(1, 6)) + assert all(f.current_pose_valid and f.error_valid and f.position_error == .02 and + f.yaw_error == -.03 and f.stamp.sec > 0 for f in feedback) + assert feedback[3].blocked and not feedback[4].blocked + report['direct'].append({'scenario': scenario, 'status': expected, 'feedback_phases': [f.phase for f in feedback]}) + for scenario, expected_code in enumerate((0, 2, 3, 1, 4, 1, 2, 1, 4)): + state = output / ('state-' + str(scenario)) + state.mkdir(exist_ok=True) + completed = subprocess.run([str(binary), str(scenario), str(state), namespace], + text=True, capture_output=True, timeout=25) + (output / ('native-' + str(scenario) + '.log')).write_text(completed.stdout + completed.stderr) + if completed.returncode != 0: + report['passed'] = False + report['failure'] = {'scenario': scenario, 'returncode': completed.returncode, + 'stdout': completed.stdout, 'stderr': completed.stderr, + 'log': str(output / ('native-' + str(scenario) + '.log'))} + (output / 'navigation-contract-report.json').write_text(json.dumps(report, indent=2) + '\n') + raise AssertionError('Native probe failed: ' + json.dumps(report['failure'])) + result = json.loads(next(line for line in reversed(completed.stdout.splitlines()) if line.startswith('{'))) + assert result['code'] == expected_code, result + assert result['stop'] == (0 if scenario == 6 else 1), result + assert result['state'] == (3 if scenario == 6 else 4), result + assert result['robot_locked'] == (scenario == 6), result + assert result['unknown_stop_blocks_redispatch'] == (scenario == 6), result + assert result['error_code'] == ({7: 'NAV_BLOCKED', 8: 'NAV_NOT_READY'}.get(scenario, 'FIXTURE_' + str(scenario))), result + assert result['feedback_sequence'] == 5, result + assert json.loads(result['feedback'])['phase'] == 4, result + assert result['mapping_count'] == 1 and result['wire_result_bytes'] > 0, result + assert 'Navigate' in result['wire_request_type'] and 'Navigate' in result['wire_result_type'], result + assert result['response_valid'], result + report['native'].append(result) + assert not failures, failures + assert len(counts) == 18 and all(count == 1 for count in counts.values()), dict(counts) + report['exactly_once_goal_count'] = sum(counts.values()) + report['passed'] = True + (output / 'navigation-contract-report.json').write_text(json.dumps(report, indent=2) + '\n') + print(json.dumps(report, indent=2)) + finally: + executor.shutdown() + thread.join(timeout=3) + client.destroy() + server.destroy() + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument('--root', type=Path, default=Path(__file__).resolve().parents[2]) + parser.add_argument('--output', type=Path, required=True) + parser.add_argument('--native-probe', type=Path) + parser.add_argument('--build-native', action='store_true') + args = parser.parse_args() + args.output = args.output.resolve() + args.output.mkdir(parents=True, exist_ok=True) + with tempfile.TemporaryDirectory(prefix='native-navigation-build-') as temporary: + binary = build_native(args.root.resolve(), Path(temporary)) if args.build_native else args.native_probe + if binary is None: + parser.error('Supply --build-native or --native-probe') + run(binary.resolve(), args.output) diff --git a/tests/test_mock_runtime.py b/tests/test_mock_runtime.py index 6dfa4ce..296c486 100644 --- a/tests/test_mock_runtime.py +++ b/tests/test_mock_runtime.py @@ -33,7 +33,7 @@ class MockRuntimeTests(unittest.TestCase): if name == "navigate": goal.target_pose.header.frame_id = "map" goal.target_pose.pose.orientation.w = 1.0 - goal.position_tolerance = goal.orientation_tolerance = 0.1 + goal.position_tolerance = goal.yaw_tolerance = 0.1 elif name == "navigate_semantic": goal.kind, goal.reference, goal.registry_version = "LOCATION", "bin_A", 1 goal.position_tolerance = goal.orientation_tolerance = 0.1 @@ -89,7 +89,7 @@ class MockRuntimeTests(unittest.TestCase): handle, result = self.execute(name, {"kind": positive_kind, "duration_seconds": 0}) self.assertEqual(handle.native, "succeeded") if hasattr(result, "result"): - self.assertEqual(result.result.status, result.result.COMPLETED) + self.assertEqual(result.result.status, result.result.SUCCEEDED if name == "navigate" else result.result.COMPLETED) self.assertEqual(result.result.stop_state, result.result.CONFIRMED) elif hasattr(result, "status"): self.assertNotEqual(result.status, result.FAILED) @@ -118,7 +118,7 @@ class MockRuntimeTests(unittest.TestCase): def test_success_feedback_follows_normal_lifecycle_and_populates_fields(self): cases = { - "navigate": [0, 1, 2, 5], + "navigate": [0, 1, 2], "execute_manipulation": [0, 1, 2, 3, 4], "execute_posture": [0, 1, 2], } @@ -131,7 +131,7 @@ class MockRuntimeTests(unittest.TestCase): self.assertTrue(all(item.stamp.sec > 0 and item.message for item in handle.feedback)) self.assertNotIn(getattr(self.module.ACTION_TYPES[name].Feedback, "STOPPING"), expected) nav = self.execute("navigate", {"kind": "normal", "duration_seconds": 0.05})[0].feedback[-1] - self.assertTrue(nav.pose_valid and nav.errors_valid and nav.blocked_valid) + self.assertTrue(nav.current_pose_valid and nav.error_valid) self.assertFalse(nav.blocked) self.assertGreaterEqual(nav.elapsed_time.nanosec, 0) manipulation = self.execute("execute_manipulation", {"kind": "normal", "duration_seconds": 0.05})[0].feedback[-1] @@ -139,7 +139,7 @@ class MockRuntimeTests(unittest.TestCase): def test_each_action_feedback_lifecycle_and_navigation_recovery_execute(self): expected = { - "navigate": [0, 1, 2, 5], "execute_manipulation": [0, 1, 2, 3, 4], + "navigate": [0, 1, 2], "execute_manipulation": [0, 1, 2, 3, 4], "plan_task": [0, 1, 2], "locate_shelf_column": [0, 1], "localize_target_3d": [0, 1], "check_free_space": [0, 1], "assess_grasp": [0], "execute_posture": [0, 1, 2], "verify_state": [0], @@ -153,7 +153,8 @@ class MockRuntimeTests(unittest.TestCase): self.assertEqual([item.phase for item in handle.feedback], phases) self.node.counts["navigate"] = 0 recovery, _ = self.execute("navigate", {"kind": "obstacle_recovery", "duration_seconds": 0.7}) - self.assertEqual([item.phase for item in recovery.feedback], [0, 1, 2, 3, 4, 5]) + self.assertEqual([item.phase for item in recovery.feedback], [0, 1, 2, 3, 2]) + self.assertEqual([item.blocked for item in recovery.feedback], [False, False, False, True, False]) self.node.counts["execute_task"] = 0 task, _ = self.execute("execute_task", {"kind": "normal", "duration_seconds": 0.05}) self.assertTrue(task.feedback[0].stage) @@ -181,7 +182,7 @@ class MockRuntimeTests(unittest.TestCase): self.assertEqual(self.node._goal("navigate", goal), self.module.GoalResponse.ACCEPT) timeout_handle = GoalHandle(goal); self.node._accepted("navigate", timeout_handle) timeout_result = self.node._execute("navigate", timeout_handle) - self.assertEqual((timeout_handle.native, timeout_result.result.status), ("aborted", timeout_result.result.TIMED_OUT)) + self.assertEqual((timeout_handle.native, timeout_result.result.status), ("aborted", timeout_result.result.TIMEOUT)) self.assertEqual(timeout_handle.feedback[-1].phase, self.module.Navigate.Feedback.STOPPING) self.node.motion_reserved = False; self.node.counts["navigate"] = 0 unknown_handle, unknown = self.execute("navigate", {"kind": "stop_unknown", "duration_seconds": 0}) @@ -205,6 +206,63 @@ class MockRuntimeTests(unittest.TestCase): self.assertEqual(timed_out.result.stop_state, timed_out.result.UNKNOWN) self.assertTrue(self.node.motion_reserved) + def test_navigation_status_mapping_and_readiness_reasons(self): + cases = [("normal", 0, "succeeded", ""), ("canceled", 1, "canceled", "MOCK_TERMINATED"), + ("timeout", 2, "aborted", "MOCK_TERMINATED"), ("blocked", 3, "aborted", "BLOCKED"), + ("failed", 5, "aborted", "MOCK_TERMINATED")] + cases += [("not_ready", 4, "aborted", code) for code in + ("INPUTS_UNHEALTHY", "ROBOT_STATE_UNAVAILABLE", "BACKEND_NOT_CONFIGURED", + "ROBOT_EMERGENCY_STOP", "ROBOT_PROTECTIVE_STOP", "ROBOT_MOTION_NOT_ALLOWED")] + for kind, status, native, code in cases: + self.node.counts["navigate"] = 0 + fixture = {"kind": "normal" if kind == "canceled" else kind, "duration_seconds": 0} + if kind == "not_ready": fixture["error_code"] = code + self.module.parse_scenarios(json.dumps({"navigate": fixture})) + handle, result = self.execute("navigate", fixture, cancel=kind == "canceled") + with self.subTest(kind=kind, code=code): + self.assertEqual((result.result.status, handle.native), (status, native)) + self.assertEqual(result.result.error_code, code) + self.assertEqual(result.final_pose_valid, status == 0) + if result.final_pose_valid: + self.assertEqual((result.final_position_error, result.final_yaw_error), (0.0, 0.0)) + self.assertFalse(hasattr(result, "errors_valid")) + self.node.counts["navigate"] = 0 + _, result = self.execute("navigate", {"kind": "stop_unknown", "duration_seconds": 0}) + self.assertEqual((result.result.status, result.result.stop_state), (5, 0)) + self.assertEqual((result.result.stopped_at.sec, result.result.stopped_at.nanosec, + result.result.stop_evidence_ref), (0, 0, "")) + + def test_navigation_goal_requires_map_and_finite_nonzero_quaternion(self): + mutations = ( + lambda g: setattr(g.target_pose.header, "frame_id", "odom"), + lambda g: setattr(g.target_pose.pose.orientation, "w", 0.0), + lambda g: setattr(g.target_pose.pose.orientation, "w", float("nan")), + lambda g: setattr(g.target_pose.pose.position, "x", float("nan")), + lambda g: setattr(g, "yaw_tolerance", float("nan")), + ) + for mutate in mutations: + self.node.inflight, self.node.motion_reserved = 0, False + goal = self.goal("navigate"); mutate(goal) + with self.subTest(mutate=mutate): + self.assertEqual(self.node._goal("navigate", goal), self.module.GoalResponse.REJECT) + + def test_navigation_accepts_large_yaw_tolerance_and_normalizes_quaternion(self): + for magnitude in (2.0, 1e308, 1e-308): + goal = self.goal("navigate") + goal.yaw_tolerance = 4.0 + goal.target_pose.pose.orientation.w = magnitude + self.node.scenarios["navigate"] = [{"kind": "normal", "duration_seconds": 0.1}] + with self.subTest(magnitude=magnitude): + self.assertEqual(self.node._goal("navigate", goal), self.module.GoalResponse.ACCEPT) + handle = GoalHandle(goal); self.node._accepted("navigate", handle) + result = self.node._execute("navigate", handle) + self.assertEqual(result.result.status, 0) + self.assertTrue(result.final_pose_valid) + self.assertEqual(result.final_pose.pose.orientation.w, 1.0) + self.assertTrue(handle.feedback) + self.assertTrue(all(item.current_pose.pose.orientation.w == 1.0 for item in handle.feedback)) + self.assertEqual(goal.target_pose.pose.orientation.w, magnitude) + def test_action_specific_malformed_goals_are_rejected(self): mutations = { "navigate": lambda g: setattr(g.target_pose.header, "frame_id", ""), @@ -387,7 +445,7 @@ class MockRuntimeTests(unittest.TestCase): self.node.counts["navigate"] = 0 handle, result = self.execute("navigate", {"kind": "native_mismatch", "duration_seconds": 0}) self.assertEqual(handle.native, "aborted") - self.assertEqual(result.result.status, result.result.COMPLETED) + self.assertEqual(result.result.status, result.result.SUCCEEDED) def test_topic_and_result_fixture_values_are_finite_and_bounded(self): for raw in ( diff --git a/tests/test_navigation_catalog.py b/tests/test_navigation_catalog.py index c7c826b..a850401 100644 --- a/tests/test_navigation_catalog.py +++ b/tests/test_navigation_catalog.py @@ -8,3 +8,35 @@ class CatalogTests(unittest.TestCase): self.assertEqual(c.resolve('CELL','',3,shelf='s',side='FRONT',column='1',tier='2')['location_id'],'stop') for args in [('OBJECT','unknown',3),('OBJECT','water',2),('LOCATION','water',3)]: with self.assertRaises(ValueError):c.resolve(*args) + +class SemanticTranslationTests(unittest.TestCase): + def test_navigation_status_is_translated_not_copied(self): + from types import SimpleNamespace as NS + from navigation_gateway import semantic_proxy as m + nav = NS(SUCCEEDED=0, CANCELED=1, TIMEOUT=2, BLOCKED=3, NOT_READY=4, FAILED=5, UNKNOWN=0, CONFIRMED=1) + old = NS(COMPLETED=0, FAILED=1, CANCELED=2, TIMED_OUT=3, REJECTED=4, UNKNOWN=0, CONFIRMED=1) + self.assertTrue(hasattr(m, 'translate_navigation_result')) + for status, expected in ((0,0),(1,2),(2,3),(3,1),(4,4),(5,1)): + source = NS(result=NS(status=status, stop_state=1, error_code='INPUTS_UNHEALTHY', message='source', stopped_at=NS(sec=123,nanosec=9), stop_evidence_ref='proof'), final_pose_valid=False, final_pose=object(), final_position_error=float('nan'), final_yaw_error=float('nan')) + target = NS(result=NS()) + m.translate_navigation_result(source, target, nav, old) + self.assertEqual(target.result.status, expected) + self.assertEqual((target.result.error_code, target.result.stop_state, target.result.stopped_at.sec, target.result.stop_evidence_ref), ('INPUTS_UNHEALTHY',1,123,'proof')) + self.assertFalse(target.errors_valid) + + def test_invalid_pose_does_not_make_default_zero_errors_valid(self): + from types import SimpleNamespace as NS + from navigation_gateway.semantic_proxy import translate_navigation_result + nav=NS(SUCCEEDED=0,CANCELED=1,TIMEOUT=2,BLOCKED=3,NOT_READY=4,FAILED=5,UNKNOWN=0,CONFIRMED=1) + old=NS(COMPLETED=0,FAILED=1,CANCELED=2,TIMED_OUT=3,REJECTED=4,UNKNOWN=0,CONFIRMED=1) + for status,code in [(3,'NAV_BLOCKED'),(4,'NAV_NOT_READY')]: + source=NS(result=NS(status=status,stop_state=0,error_code='',message='',stopped_at=None,stop_evidence_ref=''),final_pose_valid=False,final_pose=None,final_position_error=0.,final_yaw_error=0.) + target=NS(result=NS()) + translate_navigation_result(source,target,nav,old) + self.assertFalse(target.errors_valid) + self.assertEqual(target.result.error_code,code) + source.final_pose_valid=True + for p,y in [(-1.,0.),(0.,4.)]: + source.final_position_error=p;source.final_yaw_error=y + translate_navigation_result(source,target,nav,old) + self.assertFalse(target.errors_valid) diff --git a/tests/test_navigation_gateway.py b/tests/test_navigation_gateway.py index 0fc04bd..074856e 100644 --- a/tests/test_navigation_gateway.py +++ b/tests/test_navigation_gateway.py @@ -489,4 +489,158 @@ class ProxyTests(unittest.TestCase): self.assertTrue(any(e.error for e in events)) + + +class NavigationWireTests(unittest.TestCase): + def test_blocked_unknown_is_not_reported_as_clear(self): + s = module.GatewaySnapshot.parse(snapshot(), ID) + self.assertIsNone(getattr(s, 'blocked', 'missing')) + with self.assertRaises(module.GatewayError): + module.assign_navigation_feedback(object(), s) + + def test_explicit_blocked_and_error_code_survive_parse(self): + s = module.GatewaySnapshot.parse(snapshot(blocked=True, error_code='INPUTS_UNHEALTHY'), ID) + self.assertEqual((getattr(s, 'blocked', None), getattr(s, 'error_code', None)), (True, 'INPUTS_UNHEALTHY')) + for invalid in (0, 'false'): + with self.assertRaises(module.GatewayError): + module.GatewaySnapshot.parse(snapshot(blocked=invalid), ID) + + def test_new_feedback_maps_yaw_and_blocked_phase(self): + from types import SimpleNamespace as NS + f = NS(STOPPING=4, CHECKING=1, NAVIGATING=2, BLOCKED=3, elapsed_time=NS(sec=0, nanosec=0)) + s = module.GatewaySnapshot.parse(snapshot(blocked=True, position_error=.5, yaw_error=-.2), ID) + self.assertTrue(hasattr(module, 'assign_navigation_feedback')) + module.assign_navigation_feedback(f, s) + self.assertEqual((f.phase, f.blocked, f.error_valid, f.yaw_error), (3, True, True, -.2)) + self.assertFalse(f.current_pose_valid) + + def test_goal_uses_yaw_tolerance(self): + from types import SimpleNamespace as NS + goal = NS(trace=NS(**{k: 't' if k in ('task_id', 'subtask_id', 'run_id') else 1 for k in module._TRACE_FIELDS}), timeout=NS(sec=2, nanosec=0), position_tolerance=.1, yaw_tolerance=.23, + target_pose=NS(header=NS(frame_id='map'), pose=NS(position=NS(x=1., y=2., z=0.), orientation=NS(x=0., y=0., z=0., w=1.)))) + self.assertEqual(module.build_goal_body(goal, ID, 'map-a')['yaw_tolerance'], .23) + +class GatewayTelemetryTests(unittest.TestCase): + setUp = GatewayTests.setUp + def test_only_fresh_explicit_blocked_sample_is_published(self): + self.gateway.submit(request()) + self.backend.set_snapshot(request()['goal_id'], 'ACTIVE', pose=request()['target_pose']) + sample = self.backend.samples[request()['goal_id']] + sample['blocked'] = dict(self.backend.health_value, value=True) + self.assertIs(self.gateway.poll(request()['goal_id']).get('blocked'), True) + self.clock.advance(1) + self.assertIsNone(self.gateway.poll(request()['goal_id']).get('blocked')) + + def test_readiness_code_requires_fresh_explicit_backend_diagnostic(self): + self.gateway.submit(request()) + self.backend.health_sample(False) + self.backend.health_value['error_code'] = 'INPUTS_UNHEALTHY' + out = self.gateway.poll(request()['goal_id']) + self.assertEqual(out.get('error_code'), 'INPUTS_UNHEALTHY') + self.assertEqual(out['stop_state'], 'UNKNOWN') + +class NavigationOutcomeTests(unittest.TestCase): + def test_terminal_status_mapping_distinguishes_timeout_blocked_and_not_ready(self): + from types import SimpleNamespace as NS + enum = NS(SUCCEEDED=0, CANCELED=1, TIMEOUT=2, BLOCKED=3, NOT_READY=4, FAILED=5) + for outcome, expected in [('COMPLETED',0),('CANCELED',1),('TIMED_OUT',2),('BLOCKED',3),('NOT_READY',4),('REJECTED',5),('FAILED',5)]: + value = module.GatewaySnapshot.parse(snapshot(outcome=outcome), ID) + self.assertEqual(module.navigation_status(value, enum), expected) + value = module.GatewaySnapshot.parse(snapshot(outcome='FAILED', error_code='ROBOT_STATE_UNAVAILABLE'), ID) + self.assertEqual(module.navigation_status(value, enum), 4) + +class GoalValidationTests(unittest.TestCase): + def test_trace_ranges_quaternion_and_yaw_are_checked_before_network(self): + from types import SimpleNamespace as NS + goal = NS(trace=NS(task_id='t', subtask_id='s', run_id='r', attempt=1, task_revision=1, plan_version=1, execution_generation=1), timeout=NS(sec=2,nanosec=0), position_tolerance=.1, yaw_tolerance=.2, target_pose=NS(header=NS(frame_id='map'), pose=NS(position=NS(x=0.,y=0.,z=0.),orientation=NS(x=0.,y=0.,z=0.,w=1.)))) + for field,value in [('run_id',''),('attempt',True),('attempt',2**32),('task_revision',0),('plan_version',True),('execution_generation',2**64)]: + bad=copy.deepcopy(goal);setattr(bad.trace,field,value) + with self.subTest(field=field), self.assertRaises(ValueError):module.build_goal_body(bad,ID,'map-a') + goal.yaw_tolerance=4. + goal.target_pose.pose.orientation.w=2. + body=module.build_goal_body(goal,ID,'map-a') + self.assertEqual(body['yaw_tolerance'],4.) + self.assertEqual(body['target_pose']['orientation']['w'],1.) + self.assertEqual(validate_goal(body)['yaw_tolerance'],4.) + goal.target_pose.pose.orientation.w=0. + with self.assertRaises(ValueError):module.build_goal_body(goal,ID,'map-a') + +class BackendDiagnosticTests(unittest.TestCase): + def test_health_callback_keeps_explicit_machine_readable_cause(self): + from types import SimpleNamespace as NS + from navigation_gateway.backends import Ros1MoveBaseBackend + backend = Ros1MoveBaseBackend.__new__(Ros1MoveBaseBackend) + backend.map_id='map-a'; backend.lock=threading.RLock() + backend._source_metadata=lambda stamp: {'source_fresh':True,'source_stamp':stamp} + backend._health(NS(data=json.dumps({'ready':False,'map_id':'map-a','stamp':12.,'error_code':'INPUTS_UNHEALTHY'}))) + self.assertEqual(backend.health_value.get('error_code'), 'INPUTS_UNHEALTHY') + backend._health(NS(data=json.dumps({'ready':False,'map_id':'map-a','stamp':12.,'reason':'some prose'}))) + self.assertEqual(backend.health_value.get('error_code', ''), '') + +class LocalNotReadyTests(unittest.IsolatedAsyncioTestCase): + async def test_valid_unready_goal_returns_not_ready_without_http_submit(self): + from types import SimpleNamespace as NS + from unittest.mock import patch, Mock + class Node: + def __init__(self,*args):pass + def create_timer(self,*args):pass + nav=NS(SUCCEEDED=0,CANCELED=1,TIMEOUT=2,BLOCKED=3,NOT_READY=4,FAILED=5,UNKNOWN=0,CONFIRMED=1) + imports={'rclpy':NS(), 'rclpy.action':NS(ActionServer=lambda *a,**k:NS(),CancelResponse=NS(ACCEPT=1),GoalResponse=NS(ACCEPT=1,REJECT=2)), + 'rclpy.node':NS(Node=Node),'rclpy.task':NS(Future=lambda:object()),'rclpy.callback_groups':NS(ReentrantCallbackGroup=lambda:object()), + 'bt_skill_interfaces':NS(), 'bt_skill_interfaces.action':NS(Navigate=NS(Result=lambda:NS(result=NS()))),'bt_skill_interfaces.msg':NS(NavigationResult=nav)} + goal=NS(trace=NS(task_id='t',subtask_id='s',run_id='r',attempt=1,task_revision=1,plan_version=1,execution_generation=1),timeout=NS(sec=2,nanosec=0),position_tolerance=.1,yaw_tolerance=.2,target_pose=NS(header=NS(frame_id='map'),pose=NS(position=NS(x=0.,y=0.,z=0.),orientation=NS(x=0.,y=0.,z=0.,w=1.)))) + config=module.ProxyConfig(token='a-long-test-token',connect_timeout_sec=.1,read_timeout_sec=.1,request_timeout_sec=.2,poll_interval_sec=.01,readiness_max_age_sec=.5,feedback_silence_timeout_sec=.5) + client=Mock() + with patch.dict(sys.modules,imports), patch.object(module,'GatewayClient',return_value=client), patch.object(module.threading,'Thread'): + node=module.create_ros_node(config,map_id='map-a',action_name='skills/navigate') + node._health_at=time.monotonic();node._health_error_code='INPUTS_UNHEALTHY' + self.assertEqual(node._accept(goal),1) + handle=NS(request=goal,goal_id=NS(uuid=list(__import__('uuid').UUID(ID).bytes)),abort=Mock()) + result=await node._execute(handle) + self.assertEqual((result.result.status,result.result.error_code,result.result.stop_state),(4,'INPUTS_UNHEALTHY',0)) + self.assertFalse(node._reserved) + client.submit.assert_not_called() + node._health_at=0 + self.assertEqual(node._accept(goal),1) + result=await node._execute(handle) + self.assertEqual(result.result.error_code,'NAV_NOT_READY') + client.submit.assert_not_called() + +class BlockedAdmissionTests(unittest.TestCase): + setUp = GatewayTests.setUp + def test_missing_or_stale_blocked_rejects_before_send(self): + for blocked in (None, {'value':False,'received_at':0.,'source_fresh':False}): + self.backend.health_sample(True) + self.backend.health_value['blocked']=blocked + self.assertEqual(self.gateway.health()['error_code'],'INPUTS_UNHEALTHY') + with self.assertRaises(GatewayError):self.gateway.submit(request()) + self.assertEqual(self.backend.send_count,0) + + def test_explicit_simulation_blocked_can_progress_then_loss_cancels(self): + self.assertTrue(self.gateway.health()['ready']) + self.gateway.submit(request()) + for blocked in (False,True): + self.backend.set_snapshot(request()['goal_id'],'ACTIVE',pose=request()['target_pose'],blocked=blocked) + out=self.gateway.poll(request()['goal_id']) + self.assertIs(out['blocked'],blocked) + self.assertEqual(self.backend.cancel_count,0) + self.backend.set_snapshot(request()['goal_id'],'ACTIVE',pose=request()['target_pose'],blocked=None) + out=self.gateway.poll(request()['goal_id']) + self.assertEqual((self.backend.cancel_count,out['error_code'],out['stop_state']),(1,'INPUTS_UNHEALTHY','UNKNOWN')) + + def test_ros_health_carries_only_explicit_blocked_with_original_source_time(self): + from types import SimpleNamespace as NS + from navigation_gateway.backends import Ros1MoveBaseBackend + backend=Ros1MoveBaseBackend.__new__(Ros1MoveBaseBackend) + backend.map_id='sim-map';backend.lock=threading.RLock();backend.connected=True + backend._source_metadata=lambda stamp:{'source_stamp':stamp,'source_fresh':True} + backend.source_is_fresh=lambda sample:sample.get('source_fresh') is True + for blocked in (False,True,None,0): + payload={'ready':True,'map_id':'sim-map','stamp':100.,'blocked':blocked} + backend._health(NS(data=json.dumps(payload))) + sample=backend.health().get('blocked') + if type(blocked) is bool: + self.assertEqual((sample['value'],sample['source_stamp']),(blocked,100.)) + else:self.assertIsNone(sample) + if __name__ == "__main__": unittest.main() diff --git a/tests/test_ros_contracts.py b/tests/test_ros_contracts.py index b5b16db..8337ab7 100644 --- a/tests/test_ros_contracts.py +++ b/tests/test_ros_contracts.py @@ -79,21 +79,29 @@ class RosContractTests(unittest.TestCase): goal, result, feedback = sections("Navigate") self.assertEqual(goal, "\n".join([ "bt_skill_interfaces/TaskTrace trace", "geometry_msgs/PoseStamped target_pose", - "float64 position_tolerance", "float64 orientation_tolerance", "builtin_interfaces/Duration timeout", + "float64 position_tolerance", "float64 yaw_tolerance", "builtin_interfaces/Duration timeout", ])) self.assertEqual(result, "\n".join([ - "bt_skill_interfaces/ExecutionResult result", "bool pose_valid", "geometry_msgs/PoseStamped final_pose", - "bool errors_valid", "float64 final_position_error", "float64 final_orientation_error", + "bt_skill_interfaces/NavigationResult result", "bool final_pose_valid", "geometry_msgs/PoseStamped final_pose", + "float64 final_position_error", "float64 final_yaw_error", ])) self.assertEqual(feedback, "\n".join([ - "uint8 CHECKING=0", "uint8 PLANNING=1", "uint8 NAVIGATING=2", "uint8 WAITING_OBSTACLE=3", - "uint8 RECOVERING=4", "uint8 ARRIVING=5", "uint8 STOPPING=6", "builtin_interfaces/Time stamp", - "uint32 sequence", "uint8 phase", "bool pose_valid", "geometry_msgs/PoseStamped current_pose", - "bool errors_valid", "float64 position_error", "float64 orientation_error", "bool blocked_valid", + "uint8 ACCEPTED=0", "uint8 CHECKING=1", "uint8 NAVIGATING=2", "uint8 BLOCKED=3", + "uint8 STOPPING=4", "builtin_interfaces/Time stamp", + "uint32 sequence", "uint8 phase", "bool current_pose_valid", "geometry_msgs/PoseStamped current_pose", + "bool error_valid", "float64 position_error", "float64 yaw_error", "bool blocked", "builtin_interfaces/Duration elapsed_time", "string message", ])) self.assertFalse((INTERFACES / "action" / "ExecuteNavigation.action").exists()) + def test_navigation_result_is_separate_from_other_skill_results(self): + self.assertEqual(fields(INTERFACES / "msg" / "NavigationResult.msg"), [ + "uint8 SUCCEEDED=0", "uint8 CANCELED=1", "uint8 TIMEOUT=2", "uint8 BLOCKED=3", + "uint8 NOT_READY=4", "uint8 FAILED=5", "uint8 UNKNOWN=0", "uint8 CONFIRMED=1", + "uint8 status", "string error_code", "string message", "uint8 stop_state", + "builtin_interfaces/Time stopped_at", "string stop_evidence_ref", + ]) + def test_manipulation_exact_source_outer_contract(self): goal, result, feedback = sections("ExecuteManipulation") self.assertEqual(goal, "\n".join([ @@ -156,7 +164,7 @@ class RosContractTests(unittest.TestCase): def test_all_idl_files_are_registered_in_build(self): cmake = (INTERFACES / "CMakeLists.txt").read_text() declarations = re.findall(r'"((?:msg|srv|action)/[^"\n]+)"', cmake) - actual = sorted(str(p.relative_to(INTERFACES)) for p in INTERFACES.rglob("*") if p.suffix in (".msg", ".srv", ".action")) + actual = sorted(p.relative_to(INTERFACES).as_posix() for p in INTERFACES.rglob("*") if p.suffix in (".msg", ".srv", ".action")) self.assertEqual(sorted(declarations), actual) self.assertEqual(len(declarations), len(set(declarations))) manifest = ET.parse(INTERFACES / "package.xml").getroot()