From 24e0b922bc0a3177e3806932c11dfd6ad1210022 Mon Sep 17 00:00:00 2001 From: wangfeiyu Date: Tue, 22 Sep 2026 17:40:25 +0800 Subject: [PATCH] Unify navigation on NavigateToPose and remove legacy proxies --- config/mock_executor_inputs.example.yaml | 2 +- core/include/robot_bt/core.hpp | 3 +- core/src/workflow.cpp | 7 +- core/tests/navigation_readiness_test.cpp | 2 +- navigation_gateway/__init__.py | 1 - navigation_gateway/backends.py | 240 ------- navigation_gateway/catalog.py | 20 - navigation_gateway/gateway.py | 306 -------- navigation_gateway/ros2_proxy.py | 678 ------------------ navigation_gateway/semantic_proxy.py | 116 --- navigation_gateway/server.py | 139 ---- navigation_gateway/simulation.json | 15 - ros2/bt_executor/CMakeLists.txt | 3 +- .../include/bt_executor/ros_driver.hpp | 12 +- ros2/bt_executor/package.xml | 2 +- ros2/bt_executor/src/ros_driver.cpp | 45 +- ros2/bt_executor/tools/build_humble.sh | 2 +- ros2/bt_executor/tools/check_static.py | 4 +- .../tools/native_navigation_probe.cpp | 6 +- .../bt_mock_servers/mock_skills.py | 208 ++++-- .../bt_mock_servers/scenarios.py | 11 +- ros2/bt_mock_servers/package.xml | 1 + ros2/bt_skill_interfaces/CMakeLists.txt | 3 - .../action/NavigateSemantic.action | 24 - .../msg/NavigationResult.msg | 15 - ros2/bt_skill_interfaces/package.xml | 2 +- ros2/navigation_interfaces/CMakeLists.txt | 12 + .../action/NavigateToPose.action} | 30 +- ros2/navigation_interfaces/package.xml | 12 + tests/helpers/live_mock_smoke.py | 7 +- .../helpers/native_coordinator_regression.py | 4 +- tests/helpers/native_navigation_contract.py | 107 +-- tests/helpers/native_workflow_regression.py | 28 +- tests/helpers/ros_shim.py | 70 +- tests/test_dr_regressions.py | 20 - tests/test_mock_runtime.py | 149 ++-- tests/test_navigation_catalog.py | 42 -- tests/test_navigation_gateway.py | 646 ----------------- tests/test_ros_contracts.py | 57 +- tools/coverage_report.py | 2 +- 40 files changed, 461 insertions(+), 2592 deletions(-) delete mode 100644 navigation_gateway/__init__.py delete mode 100644 navigation_gateway/backends.py delete mode 100644 navigation_gateway/catalog.py delete mode 100644 navigation_gateway/gateway.py delete mode 100644 navigation_gateway/ros2_proxy.py delete mode 100644 navigation_gateway/semantic_proxy.py delete mode 100644 navigation_gateway/server.py delete mode 100644 navigation_gateway/simulation.json delete mode 100644 ros2/bt_skill_interfaces/action/NavigateSemantic.action delete mode 100644 ros2/bt_skill_interfaces/msg/NavigationResult.msg create mode 100644 ros2/navigation_interfaces/CMakeLists.txt rename ros2/{bt_skill_interfaces/action/Navigate.action => navigation_interfaces/action/NavigateToPose.action} (57%) create mode 100644 ros2/navigation_interfaces/package.xml delete mode 100644 tests/test_navigation_catalog.py delete mode 100644 tests/test_navigation_gateway.py diff --git a/config/mock_executor_inputs.example.yaml b/config/mock_executor_inputs.example.yaml index da20343..e11b2a7 100644 --- a/config/mock_executor_inputs.example.yaml +++ b/config/mock_executor_inputs.example.yaml @@ -3,7 +3,7 @@ # Fill scenarios_json explicitly for the selected route/site and evidence policy. /sim/robot_01/bt_mock_skills: ros__parameters: - enabled_actions: [navigate, navigate_semantic, execute_manipulation, plan_task, locate_shelf_column, localize_target_3d, check_free_space, assess_grasp, execute_posture, verify_state, evaluate_progress] + enabled_actions: [navigate, execute_manipulation, plan_task, locate_shelf_column, localize_target_3d, check_free_space, assess_grasp, execute_posture, verify_state, evaluate_progress] enabled_topics: [robot_state, safety_state, visual_observation, dense_progress] initial_holding_state: UNKNOWN scenarios_json: '{}' diff --git a/core/include/robot_bt/core.hpp b/core/include/robot_bt/core.hpp index 8853b3e..a5e3e25 100644 --- a/core/include/robot_bt/core.hpp +++ b/core/include/robot_bt/core.hpp @@ -60,7 +60,6 @@ struct GoalRequest { std::optional target; std::optional placement; std::string target_id, destination_id, shelf, posture_id; - std::string navigation_kind, navigation_ref, side, column, tier; std::uint64_t geometry_epoch{0}; RosTime capture_after{0}; double position_tolerance_m{0.05}, orientation_tolerance_rad{0.1}; @@ -201,7 +200,7 @@ class StageRunner { std::uint64_t geometry_epoch_{0}; RosTime capture_after_{0}, last_ros_time_{0}, holding_valid_until_{0}, holding_observed_at_{0}, empty_valid_until_{0}, empty_observed_at_{0}; std::size_t stage_index_{0}; - std::string active_goal_, source_location_, source_side_, source_column_, source_tier_, verification_goal_, pending_posture_, detail_, error_code_; + std::string active_goal_, source_location_, verification_goal_, pending_posture_, detail_, error_code_; std::optional waiting_since_; std::optional motion_waiting_since_; std::optional stopped_waiting_since_; diff --git a/core/src/workflow.cpp b/core/src/workflow.cpp index 02658aa..33ab1ff 100644 --- a/core/src/workflow.cpp +++ b/core/src/workflow.cpp @@ -80,10 +80,7 @@ TickStatus StageRunner::settle(SteadyTime now,RosTime ros) { GoalRequest StageRunner::make_request(Stage stage,Skill skill,RosTime ros) { GoalRequest q;q.robot_id=task_.robot_id;q.trace=task_.trace;q.trace.subtask_id=std::string(stage_name(stage))+"/"+std::to_string(++serial_);q.trace.attempt=1;q.skill=skill;q.target_id=task_.target_id;q.destination_id=task_.destination_id;q.shelf=task_.source_shelf;q.geometry_epoch=geometry_epoch_;q.capture_after=std::max(capture_after_,ros);q.position_tolerance_m=task_.position_tolerance_m;q.orientation_tolerance_rad=task_.orientation_tolerance_rad; if(skill==Skill::NAVIGATE) { const auto& location=stage==Stage::NAVIGATE_OBSERVE?task_.observe_location:stage==Stage::NAVIGATE_SOURCE?source_location_:task_.destination_location;auto it=site_.locations.find(location);if(it==site_.locations.end()||!valid_pose(it->second))throw std::invalid_argument("navigation location is not registered with a valid pose");q.registered_pose=it->second; - if(task_.route!="LEGACY") { - q.navigation_kind="LOCATION";q.navigation_ref=location; - if(stage==Stage::NAVIGATE_SOURCE){q.navigation_kind=task_.route=="OBJECT_TABLE"?"OBJECT":"CELL";q.navigation_ref=task_.target_id;q.side=source_side_;q.column=source_column_;q.tier=source_tier_;} - } + } if(task_.route=="LEGACY"&&(skill==Skill::EVALUATE_GRASP||skill==Skill::PICK)) { q.target=context_.target();if(!q.target||!valid_target(*q.target,task_.trace,task_.target_id,geometry_epoch_,capture_after_,ros))throw std::invalid_argument("target binding stale, incomplete or mismatched"); } if(task_.route=="LEGACY"&&skill==Skill::PLACE) { q.placement=context_.placement();if(!q.placement||!valid_placement(*q.placement,task_.trace,task_.target_id,task_.destination_id,geometry_epoch_,capture_after_,ros))throw std::invalid_argument("placement binding stale, incomplete or mismatched"); } @@ -188,7 +185,7 @@ TickStatus StageRunner::tick(Stage stage,SteadyTime now,RosTime ros) { if(response.tier.empty())return fail("tier required for calibrated shelf route"); key+="/"+response.tier;auto loc=site_.cell_locations.find(key),posture=site_.cell_postures.find(key); if(loc==site_.cell_locations.end()||posture==site_.cell_postures.end()||!registered_posture(site_,posture->second))return fail("cell lacks calibrated location/posture"); - source_location_=loc->second;pending_posture_=posture->second;source_side_=response.side;source_column_=response.column;source_tier_=response.tier; + source_location_=loc->second;pending_posture_=posture->second; }else {auto it=site_.parking_locations.find(key);if(it==site_.parking_locations.end())return fail("observed shelf column has no registered parking pose");source_location_=it->second;} }break; case Stage::LOCALIZE_TARGET: diff --git a/core/tests/navigation_readiness_test.cpp b/core/tests/navigation_readiness_test.cpp index fa00cea..e7f3d53 100644 --- a/core/tests/navigation_readiness_test.cpp +++ b/core/tests/navigation_readiness_test.cpp @@ -1,6 +1,6 @@ #include "workflow_fixture.hpp" int main() { - for(const auto* code:{"INPUTS_UNHEALTHY","ROBOT_STATE_UNAVAILABLE","ROBOT_ESTOP","BACKEND_NOT_CONFIGURED","NAV_NOT_READY"}) { + for(const auto* code:{"INPUTS_UNHEALTHY","ROBOT_STATE_UNAVAILABLE","ROBOT_ESTOP","EXECUTION_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) { diff --git a/navigation_gateway/__init__.py b/navigation_gateway/__init__.py deleted file mode 100644 index 3481855..0000000 --- a/navigation_gateway/__init__.py +++ /dev/null @@ -1 +0,0 @@ -"""Explicit ROS1 navigation / HTTP / ROS2 adapter; defaults are simulation only.""" diff --git a/navigation_gateway/backends.py b/navigation_gateway/backends.py deleted file mode 100644 index c710b2b..0000000 --- a/navigation_gateway/backends.py +++ /dev/null @@ -1,240 +0,0 @@ -"""Backend boundary. Only backend telemetry can establish readiness or stop evidence.""" -from abc import ABC, abstractmethod -import copy -import math -import threading -import time - - -TERMINAL_CONTROLLERS = frozenset({"SUCCEEDED", "ARRIVED", "ABORTED", "PREEMPTED", "RECALLED", "REJECTED"}) - - -class NavigationBackend(ABC): - @abstractmethod - def source_is_fresh(self, sample): - """Re-evaluate original source time now; never trust receipt age alone.""" - - @abstractmethod - def health(self): - """Return readiness plus a timestamped blocked boolean sample; absence is unknown.""" - - @abstractmethod - def send(self, goal): - """Exactly one attempt; return ACCEPTED, REJECTED, or UNKNOWN. Never retry.""" - - @abstractmethod - def cancel(self, goal_id): - """Send cancel intent; return value is never stop evidence.""" - - @abstractmethod - def snapshot(self, goal_id): - """Return correlated controller_state and odom/pose telemetry, or UNKNOWN.""" - - -class MockBackend(NavigationBackend): - """Simulation only: explicit synthetic telemetry, no ROS imports or motion output.""" - def __init__(self, clock=time.monotonic, map_id="sim-map", auto_complete_sec=None, source_clock=None): - self.clock, self.map_id = clock, map_id - origin_source, origin_monotonic = time.time(), clock() - self.source_clock = source_clock or (lambda: origin_source + clock() - origin_monotonic) - self.auto_complete_sec = auto_complete_sec - self.send_count = self.cancel_count = self.sequence = 0 - self.send_mode = "ACCEPTED" - self.goals, self.samples, self.cancelled, self.odom_queues = {}, {}, set(), {} - self.health_sample(True) - - def source_is_fresh(self, sample): - stamp = sample.get("source_stamp") - 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, 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) - return copy.deepcopy(self.health_value) - - 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", - "blocked": copy.deepcopy(self.health_value.get("blocked"))} - return self.send_mode - - def cancel(self, goal_id): - self.cancel_count += 1 - 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, 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, - "source_stamp": self.source_clock()}} - self.odom_queues.setdefault(goal_id, []).append(copy.deepcopy(self.samples[goal_id]["odom"])) - - def snapshot(self, goal_id): - if self.auto_complete_sec is not None and goal_id in self.goals: - goal, started = self.goals[goal_id] - done = self.clock() - started >= self.auto_complete_sec - state = "PREEMPTED" if goal_id in self.cancelled else ("SUCCEEDED" if done else "ACTIVE") - self.set_snapshot(goal_id, state, linear=0.0 if state != "ACTIVE" else 0.1, pose=goal["target_pose"]) - out = copy.deepcopy(self.samples.get(goal_id, {"controller_state": "UNKNOWN"})) - out["odom_samples"] = self.odom_queues.pop(goal_id, []) - return out - - -class Ros1MoveBaseBackend(NavigationBackend): - """Optional Noetic adapter. No automatic assumption that move_base is deployed. - - 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, 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): - import rospy - import actionlib - from actionlib_msgs.msg import GoalStatus - from geometry_msgs.msg import PoseStamped - from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal - from nav_msgs.msg import Odometry - from std_msgs.msg import String - for name in (action_name, odom_topic, pose_topic, readiness_topic, map_id): - if not isinstance(name, str) or not name: raise ValueError("ROS endpoints/map_id must be explicit") - for value in (source_max_age_sec, server_wait_sec): - if not math.isfinite(value) or value <= 0: raise ValueError("ROS timing limits must be explicit and positive") - self.rospy, self.MoveBaseGoal = rospy, MoveBaseGoal - self.map_id, self.max_age = map_id, source_max_age_sec - self.lock, self.sequence = threading.RLock(), 0 - self.current_id = None - self.state = "UNKNOWN" - self.odom = self.pose = None - self.odom_queue, self.previous_odom_stamp = [], None - self._ros_epoch, self._last_ros_time = 0, None - self.health_value = {"ready": False, "map_id": map_id, "reason": "health monitor missing", "received_at": 0, "source_fresh": False} - self.status_names = {getattr(GoalStatus, name): name for name in - ("PENDING", "ACTIVE", "PREEMPTED", "SUCCEEDED", "ABORTED", "REJECTED", "PREEMPTING", "RECALLING", "RECALLED", "LOST")} - self.client = actionlib.SimpleActionClient(action_name, MoveBaseAction) - self.connected = self.client.wait_for_server(rospy.Duration(server_wait_sec)) - self.subscribers = [rospy.Subscriber(odom_topic, Odometry, self._odom, queue_size=20), - rospy.Subscriber(pose_topic, PoseStamped, self._pose, queue_size=10), - rospy.Subscriber(readiness_topic, String, self._health, queue_size=1)] - - def _observe_ros_clock(self): - """A backward jump permanently invalidates all previously cached samples.""" - with self.lock: - now = self.rospy.Time.now().to_sec() - previous = getattr(self, "_last_ros_time", None) - epoch = getattr(self, "_ros_epoch", 0) - if not math.isfinite(now) or (previous is not None and now < previous): - epoch += 1 - self.previous_odom_stamp = None - self._ros_epoch, self._last_ros_time = epoch, now - return now, epoch - - def _source_metadata(self, stamp): - now, epoch = self._observe_ros_clock() - valid = (isinstance(stamp, (int, float)) and math.isfinite(stamp) and stamp > 0 - and math.isfinite(now) and 0 <= now-stamp <= self.max_age) - return {"source_stamp": stamp, "source_epoch": epoch, "source_fresh": valid} - - def source_is_fresh(self, sample): - now, epoch = self._observe_ros_clock() - stamp = sample.get("source_stamp") - return (sample.get("source_fresh") is True and sample.get("source_epoch") == epoch - and isinstance(stamp, (int, float)) and math.isfinite(stamp) and stamp > 0 - and math.isfinite(now) and 0 <= now-stamp <= self.max_age) - - def _odom(self, msg): - v, w = msg.twist.twist.linear, msg.twist.twist.angular - stamp = msg.header.stamp.to_sec() - with self.lock: - self.sequence += 1 - metadata = self._source_metadata(stamp) - metadata["source_fresh"] = metadata["source_fresh"] and (self.previous_odom_stamp is None or stamp > self.previous_odom_stamp) - self.odom = {"sequence": self.sequence, "received_at": time.monotonic(), - **metadata, - "linear": math.sqrt(v.x*v.x + v.y*v.y + v.z*v.z), - "angular": math.sqrt(w.x*w.x + w.y*w.y + w.z*w.z)} - self.previous_odom_stamp = stamp - self.odom_queue.append(copy.deepcopy(self.odom)) - # Overflow loses continuity; the sequence gap resets the core's window. - if len(self.odom_queue) > 4096: self.odom_queue = self.odom_queue[-1:] - - def _pose(self, msg): - p, q = msg.pose.position, msg.pose.orientation - with self.lock: - self.pose = {"received_at": time.monotonic(), **self._source_metadata(msg.header.stamp.to_sec()), - "value": {"frame_id": msg.header.frame_id, "position": dict(x=p.x, y=p.y, z=p.z), - "orientation": dict(x=q.x, y=q.y, z=q.z, w=q.w)}} - - def _health(self, msg): - 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(), - "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()} - with self.lock: self.health_value = value - - def health(self): - with self.lock: - out = copy.deepcopy(self.health_value) - out["source_fresh"] = self.source_is_fresh(out) - if not out["source_fresh"]: out.update(ready=False, reason="health source timestamp stale or invalidated") - if not self.connected: out.update(ready=False, reason="move_base server unavailable") - return out - - def send(self, goal): - with self.lock: - self.current_id, self.state = goal["goal_id"], "PENDING" - msg = self.MoveBaseGoal() - msg.target_pose.header.frame_id = "map" - msg.target_pose.header.stamp = self.rospy.Time.now() - for key, value in goal["target_pose"]["position"].items(): setattr(msg.target_pose.pose.position, key, value) - for key, value in goal["target_pose"]["orientation"].items(): setattr(msg.target_pose.pose.orientation, key, value) - goal_id = goal["goal_id"] - def active(): - with self.lock: - if self.current_id == goal_id: self.state = "ACTIVE" - def done(status, result): - with self.lock: - if self.current_id == goal_id: self.state = self.status_names.get(status, "UNKNOWN") - # send_goal is asynchronous; return indicates local dispatch, not server acceptance. - self.client.send_goal(msg, done_cb=done, active_cb=active) - return "ACCEPTED" - - def cancel(self, goal_id): - with self.lock: - if goal_id != self.current_id: raise RuntimeError("cannot correlate ROS1 goal after restart") - self.client.cancel_goal() - - def snapshot(self, goal_id): - with self.lock: - if goal_id != self.current_id: return {"controller_state": "UNKNOWN"} - # 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), - "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/catalog.py b/navigation_gateway/catalog.py deleted file mode 100644 index fc28889..0000000 --- a/navigation_gateway/catalog.py +++ /dev/null @@ -1,20 +0,0 @@ -"""Navigation-owned immutable, versioned exact-match lookup. No fuzzy guesses.""" -import copy,math -class Catalog: - def __init__(self,site): - self.site=copy.deepcopy(site) - if type(site.get('registry_version')) is not int or site['registry_version']<=0:raise ValueError('registry version required') - def resolve(self,kind,reference,version,*,shelf='',side='',column='',tier=''): - if type(version) is not int or version!=self.site['registry_version']:raise ValueError('REGISTRY_VERSION_MISMATCH') - if kind=='LOCATION':location=reference - elif kind=='OBJECT':location=self.site.get('object_locations',{}).get(reference) - elif kind=='CELL': - if any(not isinstance(x,str) or not x or '/' in x for x in (shelf,side,column,tier)):raise ValueError('INVALID_CELL') - location=self.site.get('cell_locations',{}).get('/'.join((shelf,side,column,tier))) - else:raise ValueError('UNKNOWN_LOOKUP_KIND') - pose=self.site.get('locations',{}).get(location) - if not isinstance(pose,dict) or pose.get('frame_id')!='map':raise ValueError('NOT_FOUND') - for k in ('x','y','z','qx','qy','qz','qw'): - if type(pose.get(k)) not in (int,float) or not math.isfinite(pose[k]):raise ValueError('INVALID_POSE') - if abs(sum(pose[k]**2 for k in ('qx','qy','qz','qw'))-1)>.001:raise ValueError('INVALID_QUATERNION') - return dict(location_id=location,pose=copy.deepcopy(pose),registry_version=version) diff --git a/navigation_gateway/gateway.py b/navigation_gateway/gateway.py deleted file mode 100644 index 01b25af..0000000 --- a/navigation_gateway/gateway.py +++ /dev/null @@ -1,306 +0,0 @@ -"""Persistent, single robot navigation resource ownership and stop verification.""" -from dataclasses import dataclass, asdict -import copy -import fcntl -import hashlib -import json -import math -import os -import sqlite3 -import threading -import time -import uuid -from .backends import TERMINAL_CONTROLLERS - - -class GatewayError(Exception): - def __init__(self, message, http_status=400): - super().__init__(message) - self.http_status = http_status - - -def number(value, name, positive=False): - if isinstance(value, bool) or not isinstance(value, (float, int)) or not math.isfinite(value): - raise GatewayError(name + " must be finite numeric") - if positive and value <= 0: raise GatewayError(name + " must be positive") - return float(value) - - -def validate_pose(pose): - if not isinstance(pose, dict) or set(pose) != {"frame_id", "position", "orientation"} or pose["frame_id"] != "map": - raise GatewayError("target_pose must use map frame and exact pose fields") - for group, keys in (("position", {"x", "y", "z"}), ("orientation", {"x", "y", "z", "w"})): - if not isinstance(pose[group], dict) or set(pose[group]) != keys: raise GatewayError("invalid " + group) - for key in keys: number(pose[group][key], group + "." + key) - norm = sum(v*v for v in pose["orientation"].values()) - if abs(norm - 1.0) > 1e-3: raise GatewayError("quaternion must be normalized") - - -def validate_goal(body): - required = {"goal_id", "trace", "map_id", "target_pose", "position_tolerance", "yaw_tolerance", "timeout_sec"} - if not isinstance(body, dict) or set(body) != required: raise GatewayError("invalid or unknown goal fields") - try: - if str(uuid.UUID(body["goal_id"])) != body["goal_id"]: raise ValueError() - except (ValueError, TypeError, AttributeError): raise GatewayError("goal_id must be canonical UUID") - if not isinstance(body["map_id"], str) or not body["map_id"] or len(body["map_id"]) > 256: raise GatewayError("invalid map_id") - trace = body["trace"] - if not isinstance(trace, dict) or not {"task_id", "subtask_id", "attempt"}.issubset(trace): raise GatewayError("trace identifiers required") - allowed_trace = {"task_id", "subtask_id", "attempt", "task_revision", "plan_version", "run_id", "execution_generation"} - if set(trace) - allowed_trace: raise GatewayError("unknown trace fields") - for key in ("task_id", "subtask_id"): - if not isinstance(trace[key], str) or not trace[key] or len(trace[key]) > 256: raise GatewayError("invalid trace " + key) - for key, value in trace.items(): - if key in {"attempt", "task_revision", "plan_version", "execution_generation"}: - if type(value) is not int or value < 0: raise GatewayError("invalid trace counter") - 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) - return copy.deepcopy(body) - - -@dataclass(frozen=True) -class SafetyConfig: - odom_max_age_sec: float - stationary_window_sec: float - linear_stopped_mps: float - angular_stopped_radps: float - pose_max_age_sec: float - readiness_max_age_sec: float - stop_wait_timeout_sec: float - - def __post_init__(self): - for key, value in asdict(self).items(): number(value, key, positive=True) - - -def pose_errors(goal_pose, actual_pose): - validate_pose(actual_pose) - p, a = goal_pose["position"], actual_pose["position"] - position = math.hypot(p["x"] - a["x"], p["y"] - a["y"]) - def yaw(pose): - q = pose["orientation"] - return math.atan2(2*(q["w"]*q["z"]+q["x"]*q["y"]), 1-2*(q["y"]**2+q["z"]**2)) - return position, math.remainder(yaw(goal_pose)-yaw(actual_pose), 2*math.pi) - - -class Gateway: - def __init__(self, journal_path, backend, config, clock=time.monotonic): - self.backend, self.config, self.clock = backend, config, clock - self.lock, self.records, self.closed = threading.RLock(), {}, False - os.makedirs(os.path.dirname(os.path.abspath(journal_path)), exist_ok=True) - self.lockfile = open(journal_path + ".lock", "a+") - try: fcntl.flock(self.lockfile.fileno(), fcntl.LOCK_EX | fcntl.LOCK_NB) - except OSError: - self.lockfile.close() - raise RuntimeError("another gateway owns this journal") - self.db = sqlite3.connect(journal_path, check_same_thread=False) - os.chmod(journal_path, 0o600) - self.db.execute("PRAGMA journal_mode=WAL") - self.db.execute("PRAGMA synchronous=FULL") - self.db.execute("CREATE TABLE IF NOT EXISTS goals (goal_id TEXT PRIMARY KEY, request_hash TEXT NOT NULL, record TEXT NOT NULL)") - for _, _, raw in self.db.execute("SELECT goal_id, request_hash, record FROM goals"): - record = json.loads(raw) - if record["status"] != "TERMINAL" or record["stop_state"] != "CONFIRMED": - record.update(status="STOP_UNKNOWN", stop_state="UNKNOWN", quarantined=True, - message="process restart: manual reconciliation required") - self.records[record["goal_id"]] = record - self._save(record) - - def _save(self, record): - raw = json.dumps(record, sort_keys=True, allow_nan=False) - with self.db: - self.db.execute("INSERT INTO goals VALUES (?, ?, ?) ON CONFLICT(goal_id) DO UPDATE SET record=excluded.record", - (record["goal_id"], record["request_hash"], raw)) - - def _public(self, record): - out = {key: copy.deepcopy(value) for key, value in record.items() if not key.startswith("_")} - out.pop("request", None) - if not record["quarantined"] and record["status"] != "TERMINAL": out["elapsed"] = max(0, self.clock()-record["_started"]) - return out - - def _record(self, goal_id): - if goal_id not in self.records: raise GatewayError("unknown goal UUID", 404) - return self.records[goal_id] - - def _occupied(self): - return any(r["status"] != "TERMINAL" or r["stop_state"] != "CONFIRMED" for r in self.records.values()) - - def health(self): - with self.lock: - try: health = self.backend.health() - except Exception: health = {} - fresh = self._fresh(health, self.config.readiness_max_age_sec) - 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 - ("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) - canonical = json.dumps(goal, sort_keys=True, separators=(",", ":"), allow_nan=False) - digest = hashlib.sha256(canonical.encode()).hexdigest() - with self.lock: - if goal["goal_id"] in self.records: - record = self._record(goal["goal_id"]) - if record["request_hash"] != digest: raise GatewayError("UUID already bound to different immutable request", 409) - return self._public(record) - if self._occupied(): raise GatewayError("motion resource occupied; manual reconciliation may be required", 409) - health = self.health() - if not health["ready"] or health["map_id"] != goal["map_id"]: raise GatewayError("backend not ready for requested map", 503) - record = {"goal_id": goal["goal_id"], "request_hash": digest, "request": goal, "status": "SENDING", - "outcome": None, "stop_state": "UNKNOWN", "controller_state": "UNKNOWN", "message": "dispatch pending", - "sequence": 0, "elapsed": 0, "position_error": None, "yaw_error": None, "quarantined": False, - "pose_valid": False, "current_pose": None, "final_pose": None, - "stopped_at": None, - "_started": self.clock(), "_cancel_sent": False, "_cancel_outcome": None, "_cancel_at": None, - "_terminal_at": None, "_stationary_since": None, "_last_odom_at": None, "_last_odom_sequence": None} - self.records[goal["goal_id"]] = record - self._save(record) # Commit ownership BEFORE any potentially side-effecting backend send. - try: acceptance = self.backend.send(goal) - except Exception: acceptance = "UNKNOWN" - if acceptance == "ACCEPTED": record.update(status="ACTIVE", message="dispatched; server acceptance observed asynchronously") - elif acceptance == "REJECTED": record.update(status="ACTIVE", controller_state="REJECTED", outcome="REJECTED", message="rejected; stop evidence pending") - else: record.update(status="STOP_UNKNOWN", quarantined=True, message="dispatch outcome unknown: reconcile original goal; never retry") - self._save(record) - if record["quarantined"]: self.cancel(goal["goal_id"], "FAILED") - return self._public(record) - - def get(self, goal_id): - with self.lock: return self._public(self._record(goal_id)) - - def cancel(self, goal_id, outcome="CANCELED"): - with self.lock: - r = self._record(goal_id) - if r["status"] == "TERMINAL" and r["stop_state"] == "CONFIRMED": return self._public(r) - if not r["_cancel_sent"]: - r.update(_cancel_sent=True, _cancel_outcome=outcome, _cancel_at=self.clock()) - if not r["quarantined"]: r["status"] = "CANCEL_REQUESTED" - self._save(r) # A crash here must NOT repeat a possibly side-effecting cancel. - try: self.backend.cancel(goal_id) - except Exception: r.update(status="STOP_UNKNOWN", message="cancel delivery unknown; stop not confirmed") - self._save(r) - return self._public(r) - - def _fresh(self, sample, maximum): - if not isinstance(sample, dict) or sample.get("source_fresh") is not True: return False - try: - if not self.backend.source_is_fresh(sample): return False - except (AttributeError, KeyError, TypeError, ValueError): return False - 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) - if r["quarantined"] or (r["status"] == "TERMINAL" and r["stop_state"] == "CONFIRMED"): return self._public(r) - if self.clock()-r["_started"] >= r["request"]["timeout_sec"] and not r["_cancel_sent"]: - self.cancel(goal_id, "TIMED_OUT") - 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"] 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: - p_error, y_error = pose_errors(r["request"]["target_pose"], pose_sample["value"]) - r.update(pose_valid=True, current_pose=copy.deepcopy(pose_sample["value"]), - position_error=p_error, yaw_error=y_error) - except (GatewayError, KeyError, TypeError): pass - if not r["pose_valid"]: r.update(position_error=None, yaw_error=None) - if state in {"UNKNOWN", "LOST"}: - if not r["_cancel_sent"]: self.cancel(goal_id, "FAILED") - r.update(status="STOP_UNKNOWN", message="controller state unknown; resource remains locked", _stationary_since=None) - elif state in TERMINAL_CONTROLLERS: - if r["_terminal_at"] is None: r["_terminal_at"] = self.clock() - stationary = False - # Consume every received sample, so motion between HTTP polls cannot disappear. - samples = snapshot.get("odom_samples") or [snapshot.get("odom")] - for odom in samples: - stationary = False - if self._fresh(odom, self.config.odom_max_age_sec): - try: - linear = number(odom["linear"], "measured linear speed") - angular = number(odom["angular"], "measured angular speed") - stationary = abs(linear) <= self.config.linear_stopped_mps and abs(angular) <= self.config.angular_stopped_radps - except (GatewayError, KeyError): pass - if not stationary: - r["_stationary_since"] = None - if not isinstance(odom, dict): continue - seq, stamp = odom.get("sequence"), odom.get("received_at") - if type(seq) is not int or seq < 1: - r["_stationary_since"] = None - continue - if seq == r["_last_odom_sequence"]: continue # Re-reading one sample never advances the window. - if not isinstance(stamp, (int, float)) or not math.isfinite(stamp): - r["_stationary_since"] = None - continue - gap = (r["_last_odom_at"] is None or stamp-r["_last_odom_at"] > self.config.odom_max_age_sec - or stamp <= r["_last_odom_at"] or (r["_last_odom_sequence"] is not None and seq != r["_last_odom_sequence"]+1)) - if stamp < r["_terminal_at"]: r["_stationary_since"] = None - elif stationary and (r["_stationary_since"] is None or gap): r["_stationary_since"] = stamp - r["_last_odom_at"], r["_last_odom_sequence"] = stamp, seq - if stationary and r["_stationary_since"] is not None and r["_last_odom_at"]-r["_stationary_since"] >= self.config.stationary_window_sec: - outcome = r["_cancel_outcome"] or ({"SUCCEEDED": "COMPLETED", "ARRIVED": "COMPLETED", - "REJECTED": "REJECTED", "PREEMPTED": "CANCELED", "RECALLED": "CANCELED"}.get(state, "FAILED")) - message = "controller terminal and measured stationary window confirmed" - if outcome == "COMPLETED": - pose = snapshot.get("pose") - try: - if not self._fresh(pose, self.config.pose_max_age_sec): raise GatewayError("pose stale") - p_err, y_err = pose_errors(r["request"]["target_pose"], pose["value"]) - r.update(position_error=p_err, yaw_error=y_err) - if p_err > r["request"]["position_tolerance"] or abs(y_err) > r["request"]["yaw_tolerance"]: raise GatewayError("pose outside tolerance") - except (GatewayError, KeyError, TypeError): outcome, message = "FAILED", "terminal success failed fresh pose/tolerance verification" - # Preserve the original last physical odom observation that closed - # the stationary window. Never timestamp cached results at query time. - proof_stamp = odom.get("source_stamp") - if not isinstance(proof_stamp, (int, float)) or not math.isfinite(proof_stamp) or proof_stamp <= 0: - r.update(status="STOP_UNKNOWN", message="stationary proof lacks original source timestamp") - self._save(r) - return self._public(r) - r.update(status="TERMINAL", outcome=outcome, stop_state="CONFIRMED", message=message, - stopped_at=proof_stamp) - if r["pose_valid"]: r["final_pose"] = copy.deepcopy(r["current_pose"]) - else: - r.update(_terminal_at=None, _stationary_since=None) - if r["_cancel_at"] is not None and self.clock()-r["_cancel_at"] >= self.config.stop_wait_timeout_sec and r["stop_state"] != "CONFIRMED": - r.update(status="STOP_UNKNOWN", message="stop confirmation deadline exceeded; resource remains locked") - self._save(r) - return self._public(r) - - def poll_all(self): - with self.lock: - for goal_id in list(self.records): self.poll(goal_id) - - def close(self): - with self.lock: - if not self.closed: - self.db.close() - fcntl.flock(self.lockfile.fileno(), fcntl.LOCK_UN) - self.lockfile.close() - self.closed = True diff --git a/navigation_gateway/ros2_proxy.py b/navigation_gateway/ros2_proxy.py deleted file mode 100644 index de60223..0000000 --- a/navigation_gateway/ros2_proxy.py +++ /dev/null @@ -1,678 +0,0 @@ -"""ROS 2 Navigate Action -> authenticated navigation gateway. - -The HTTP/session layer uses only the Python standard library. ROS imports are -lazy so transport behavior can be exercised without a ROS installation. -""" -from __future__ import annotations - -import argparse -import http.client -import ipaddress -import json -import math -import os -import socket -import threading -import time -import uuid -from dataclasses import dataclass, field -from typing import Any, Mapping -from urllib.parse import urlsplit - - -_STATES = {'SENDING', 'ACTIVE', 'CANCEL_REQUESTED', 'STOP_UNKNOWN', 'TERMINAL'} -_OUTCOMES = {'COMPLETED', 'FAILED', 'CANCELED', 'TIMED_OUT', 'REJECTED', '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') -_MAX_RESPONSE_BYTES = 65536 - - -class GatewayError(RuntimeError): - """A response cannot establish the state of the physical goal.""" - - -def _positive(value: Any, name: str) -> float: - if isinstance(value, bool) or not isinstance(value, (int, float)): - raise ValueError(f'{name} must be a finite positive number') - result = float(value) - if not math.isfinite(result) or result <= 0: - raise ValueError(f'{name} must be a finite positive number') - return result - - -@dataclass(frozen=True) -class ProxyConfig: - token: str = field(repr=False) - connect_timeout_sec: float - read_timeout_sec: float - request_timeout_sec: float - poll_interval_sec: float - readiness_max_age_sec: float - feedback_silence_timeout_sec: float - endpoint: str = 'http://127.0.0.1:8766' - - def __post_init__(self) -> None: - if not isinstance(self.token, str) or len(self.token) < 16 or any(c.isspace() for c in self.token): - raise ValueError('A bearer token of at least 16 characters without whitespace is required') - for name in ('connect_timeout_sec', 'read_timeout_sec', 'request_timeout_sec', - 'poll_interval_sec', 'readiness_max_age_sec', 'feedback_silence_timeout_sec'): - _positive(getattr(self, name), name) - parsed = urlsplit(self.endpoint) - if (parsed.scheme not in {'http', 'https'} or not parsed.hostname or - parsed.username is not None or parsed.password is not None or - parsed.path not in {'', '/'} or parsed.query or parsed.fragment): - raise ValueError('endpoint must be an http(s) origin without credentials or path') - # DNS lookup has no socket deadline in stdlib. Use an explicit deployment - # IP; localhost is normalized without invoking the resolver. - if parsed.hostname != 'localhost': - try: - ipaddress.ip_address(parsed.hostname) - except ValueError as exc: - raise ValueError('endpoint must use an IP address or localhost') from exc - try: - parsed.port - except ValueError as exc: - raise ValueError('invalid endpoint port') from exc - - -@dataclass(frozen=True) -class GatewaySnapshot: - goal_id: str - status: str - outcome: str | None - stop_state: str - controller_state: str - message: str - position_error: float | None - yaw_error: float | None - sequence: int - elapsed: float - pose_valid: bool = False - current_pose: dict[str, Any] | None = None - final_pose: dict[str, Any] | None = None - stopped_at: float | None = None - 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', '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'), 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') - if snapshot.pose_valid and snapshot.current_pose is None and snapshot.final_pose is None: - raise GatewayError('Gateway marks pose valid without an observed pose') - if snapshot.goal_id != goal_id: - raise GatewayError('Gateway returned a different goal UUID') - if snapshot.status not in _STATES or snapshot.outcome not in _OUTCOMES | {None}: - raise GatewayError('Gateway returned an unknown status or outcome') - if snapshot.stop_state not in {'UNKNOWN', 'CONFIRMED'}: - raise GatewayError('Gateway returned an unknown stop state') - if not isinstance(snapshot.controller_state, str) or not isinstance(snapshot.message, str): - raise GatewayError('Gateway returned malformed text fields') - if type(snapshot.sequence) is not int or not 0 <= snapshot.sequence <= 0xffffffff: - raise GatewayError('Gateway sequence must fit Navigate uint32') - for name in ('elapsed', 'position_error', 'yaw_error'): - value = getattr(snapshot, name) - if value is None and name != 'elapsed': - continue - if (isinstance(value, bool) or not isinstance(value, (float, int)) or - not math.isfinite(value) or (name != 'yaw_error' and value < 0) or - (name == 'yaw_error' and abs(value) > math.pi)): - raise GatewayError(f'Gateway {name} is invalid') - if snapshot.status == 'TERMINAL' and snapshot.outcome is None: - raise GatewayError('Terminal gateway snapshot has no outcome') - if snapshot.status == 'TERMINAL' and snapshot.stop_state == 'CONFIRMED': - if stop_time_parts is None or stop_time_parts == (0, 0): - raise GatewayError('Confirmed result requires a positive original ROS stop timestamp') - if snapshot.controller_state not in _CONTROLLER_TERMINALS: - raise GatewayError('Confirmed result lacks a native controller terminal') - if snapshot.outcome == 'COMPLETED' and snapshot.controller_state not in {'ARRIVED', 'SUCCEEDED'}: - raise GatewayError('COMPLETED conflicts with native controller state') - return snapshot - - @property - def is_terminal(self) -> bool: - return self.status == 'TERMINAL' and self.stop_state == 'CONFIRMED' - - -def _ros_time_parts(source_seconds: Any) -> tuple[int, int]: - if (isinstance(source_seconds, bool) or not isinstance(source_seconds, (int, float)) or - not math.isfinite(source_seconds) or source_seconds < 0 or - source_seconds >= 2147483648): - raise GatewayError('Stop timestamp must fit a nonnegative ROS int32 seconds value') - seconds = math.floor(source_seconds) - nanoseconds = round((source_seconds - seconds) * 1_000_000_000) - if nanoseconds >= 1_000_000_000: - seconds += 1 - nanoseconds -= 1_000_000_000 - if seconds > 2147483647: - raise GatewayError('Rounded stop timestamp exceeds ROS int32 seconds') - return seconds, nanoseconds - - -def assign_ros_time(destination: Any, source_seconds: float) -> None: - """Copy the original gateway evidence time, never callback receipt time.""" - destination.sec, destination.nanosec = _ros_time_parts(source_seconds) - - -def validate_observed_pose(pose: Any) -> dict[str, Any] | None: - """Validate an actual map-frame observation without normalizing bad data.""" - if pose is None: - return None - if not isinstance(pose, dict) or set(pose) != {'frame_id', 'position', 'orientation'}: - raise GatewayError('Observed pose must use the gateway map-pose schema') - if pose['frame_id'] != 'map': - raise GatewayError('Observed pose must be in the map frame') - for name, axes in (('position', {'x', 'y', 'z'}), ('orientation', {'x', 'y', 'z', 'w'})): - coordinates = pose[name] - if not isinstance(coordinates, dict) or set(coordinates) != axes: - raise GatewayError('Observed pose coordinates are incomplete') - if any(isinstance(v, bool) or not isinstance(v, (float, int)) or - not math.isfinite(v) for v in coordinates.values()): - raise GatewayError('Observed pose coordinates must be finite') - if abs(sum(value * value for value in pose['orientation'].values()) - 1.0) > 1e-3: - raise GatewayError('Observed pose quaternion must be normalized') - return json.loads(json.dumps(pose, allow_nan=False)) - - -def assign_ros_pose(destination: Any, source: Mapping[str, Any]) -> None: - """Copy validated geometry; unavailable source timestamp remains unset.""" - destination.header.frame_id = source['frame_id'] - for name, axes in (('position', ('x', 'y', 'z')), ('orientation', ('x', 'y', 'z', 'w'))): - for axis in axes: - setattr(getattr(destination.pose, name), axis, float(source[name][axis])) - - -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.""" - - def __init__(self, config: ProxyConfig): - self.config = config - parsed = urlsplit(config.endpoint) - self._host = '127.0.0.1' if parsed.hostname == 'localhost' else parsed.hostname - self._port = parsed.port - self._connection_type = (http.client.HTTPSConnection if parsed.scheme == 'https' - else http.client.HTTPConnection) - - def _request(self, method: str, path: str, - body: Mapping[str, Any] | None = None) -> dict[str, Any]: - raw = None if body is None else json.dumps(body, allow_nan=False, sort_keys=True, - separators=(',', ':')).encode('utf-8') - config = self.config - connection = self._connection_type(self._host, self._port, - timeout=min(config.connect_timeout_sec, config.request_timeout_sec)) - expired = threading.Event() - connected_socket = None - - def expire() -> None: - expired.set() - active_socket = connected_socket or connection.sock - if active_socket is not None: - try: - active_socket.shutdown(socket.SHUT_RDWR) - except OSError: - pass - active_socket.close() - - deadline = time.monotonic() + config.request_timeout_sec - watchdog = threading.Timer(config.request_timeout_sec, expire) - watchdog.daemon = True - watchdog.start() - try: - connection.connect() - connected_socket = connection.sock - if expired.is_set() or connection.sock is None: - raise GatewayError('Gateway request deadline exceeded') - connection.sock.settimeout(min(config.read_timeout_sec, - max(0.001, deadline - time.monotonic()))) - connection.request(method, path, raw, { - 'Authorization': f'Bearer {config.token}', - 'Accept': 'application/json', - 'Content-Type': 'application/json', - 'Connection': 'close', - }) - response = connection.getresponse() - # The response may own the socket once Connection: close is seen. - # The watchdog retains its original reference below through this read. - payload = response.read(_MAX_RESPONSE_BYTES + 1) - if expired.is_set() or time.monotonic() > deadline: - raise GatewayError('Gateway request deadline exceeded') - if len(payload) > _MAX_RESPONSE_BYTES: - raise GatewayError('Gateway response exceeds size limit') - if not 200 <= response.status < 300: - raise GatewayError(f'Gateway HTTP status {response.status}; stop is unknown') - data = json.loads(payload) - if not isinstance(data, dict): - raise GatewayError('Gateway response must be a JSON object') - return data - except (OSError, http.client.HTTPException, ValueError) as exc: - # Do not log the token, endpoint credentials, or arbitrary response body. - raise GatewayError(f'Gateway transport/protocol failure: {type(exc).__name__}') from exc - finally: - watchdog.cancel() - connection.close() - - def health(self) -> dict[str, Any]: - health = self._request('GET', '/healthz') - if type(health.get('ready')) is not bool or not isinstance(health.get('reason'), str): - raise GatewayError('Gateway health response is malformed') - return health - - def submit(self, body: Mapping[str, Any]) -> GatewaySnapshot: - return GatewaySnapshot.parse(self._request('POST', '/v1/goals', body), body['goal_id']) - - def query(self, goal_id: str) -> GatewaySnapshot: - _canonical_uuid(goal_id) - return GatewaySnapshot.parse(self._request('GET', f'/v1/goals/{goal_id}'), goal_id) - - def cancel(self, goal_id: str) -> GatewaySnapshot: - _canonical_uuid(goal_id) - return GatewaySnapshot.parse(self._request('POST', f'/v1/goals/{goal_id}/cancel', {}), goal_id) - - -def _canonical_uuid(goal_id: str) -> None: - if not isinstance(goal_id, str) or str(uuid.UUID(goal_id)) != goal_id: - raise ValueError('goal_id must be a canonical UUID') - - -@dataclass(frozen=True) -class SessionEvent: - snapshot: GatewaySnapshot | None = None - error: str | None = None - terminal: bool = False - - -class GoalSession: - """Background transport for one immutable attempt; cancel means intent only. - - The caller owns physical resource locking. An error is always stop UNKNOWN. - Coalescing to the latest event bounds memory while the ROS executor is busy. - """ - - def __init__(self, client: GatewayClient, body: Mapping[str, Any]): - self.client = client - self._body = json.dumps(body, allow_nan=False, sort_keys=True, - separators=(',', ':')).encode('utf-8') - frozen_body = json.loads(self._body) - self._goal_id = frozen_body['goal_id'] - self._position_tolerance = _positive(frozen_body['position_tolerance'], 'position_tolerance') - self._yaw_tolerance = _positive(frozen_body['yaw_tolerance'], 'yaw_tolerance') - _canonical_uuid(self._goal_id) - self._cancel = threading.Event() - self._lock = threading.Lock() - self._event: SessionEvent | None = None - self._started = False - - @property - def goal_id(self) -> str: - return self._goal_id - - def start(self) -> None: - with self._lock: - if self._started: - raise RuntimeError('One GoalSession can be started only once') - self._started = True - threading.Thread(target=self._run, name=f'nav-{self.goal_id}', daemon=True).start() - - def request_cancel(self) -> None: - self._cancel.set() - - def drain_events(self) -> list[SessionEvent]: - with self._lock: - event, self._event = self._event, None - return [] if event is None else [event] - - def _publish(self, event: SessionEvent) -> None: - with self._lock: - self._event = event - - def _run(self) -> None: - cancel_sent = False - last_sequence = -1 - last_progress = time.monotonic() - try: - # A submission acknowledgement, even one containing a terminal - # snapshot, is not consumed as a physical completion result. - self.client.submit(json.loads(self._body)) - while True: - if self._cancel.is_set() and not cancel_sent: - cancel_sent = True - self.client.cancel(self.goal_id) # ACK only; never terminal. - snapshot = self.client.query(self.goal_id) - if snapshot.sequence > last_sequence: - if snapshot.is_terminal and snapshot.outcome == 'COMPLETED': - if (snapshot.position_error is None or snapshot.yaw_error is None or - snapshot.position_error > self._position_tolerance or - abs(snapshot.yaw_error) > self._yaw_tolerance): - raise GatewayError('COMPLETED lacks matching pose tolerance evidence') - last_sequence = snapshot.sequence - last_progress = time.monotonic() - self._publish(SessionEvent(snapshot=snapshot, terminal=snapshot.is_terminal)) - if snapshot.is_terminal: - return - if time.monotonic() - last_progress >= self.client.config.feedback_silence_timeout_sec: - self._cancel.set() - # Event.wait would spin forever once cancel is set. A private - # event keeps polling bounded and leaves ROS callbacks unblocked. - threading.Event().wait(self.client.config.poll_interval_sec) - except Exception as exc: - # A lost submit response may conceal an accepted moving goal. Send - # one best-effort cancel under the same UUID before reporting UNKNOWN. - if not cancel_sent: - try: - self.client.cancel(self.goal_id) - except Exception: - pass - error = str(exc) if isinstance(exc, GatewayError) else type(exc).__name__ - self._publish(SessionEvent(error=error)) - - -def build_goal_body(goal: Any, goal_id: str, map_id: str) -> dict[str, Any]: - """Freeze the p13 Navigate goal into the Noetic gateway's explicit schema.""" - _canonical_uuid(goal_id) - if not isinstance(map_id, str) or not map_id.strip(): - raise ValueError('map_id must be explicitly configured') - if goal.target_pose.header.frame_id != 'map': - raise ValueError('Navigate only accepts registered poses in the map frame') - timeout = goal.timeout.sec + goal.timeout.nanosec / 1e9 - if not 0 <= goal.timeout.nanosec < 1_000_000_000: - raise ValueError('timeout nanosec is not normalized') - pose = goal.target_pose.pose - body = { - 'goal_id': goal_id, - 'trace': {name: getattr(goal.trace, name) for name in _TRACE_FIELDS}, - 'map_id': map_id, - 'target_pose': { - 'frame_id': 'map', - 'position': {axis: getattr(pose.position, axis) for axis in ('x', 'y', 'z')}, - 'orientation': {axis: getattr(pose.orientation, axis) for axis in ('x', 'y', 'z', 'w')}, - }, - 'position_tolerance': _positive(goal.position_tolerance, 'position_tolerance'), - 'yaw_tolerance': _positive(goal.yaw_tolerance, 'yaw_tolerance'), - 'timeout_sec': _positive(timeout, 'timeout'), - } - 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)) - - -def create_ros_node(config: ProxyConfig, *, map_id: str, action_name: str): - """Construct the ROS node; call only after rclpy.init(). - - ROS 2 Humble imports and action runtime require validation on the target. - """ - import rclpy - from rclpy.action import ActionServer, CancelResponse, GoalResponse - from rclpy.callback_groups import ReentrantCallbackGroup - from rclpy.node import Node - from rclpy.task import Future - from bt_skill_interfaces.action import Navigate - from bt_skill_interfaces.msg import NavigationResult - - if not map_id or not action_name: - raise ValueError('map_id and action_name must be explicitly configured') - - class NavigateProxy(Node): - def __init__(self): - super().__init__('navigation_gateway_proxy') - self._client = GatewayClient(config) - self._guard = threading.Lock() - self._ready = False - self._health_at = 0.0 - self._health_error_code = '' - self._reserved = False - self._stop_unknown = False - self._record = None - self._closing = threading.Event() - self._group = ReentrantCallbackGroup() - self._server = ActionServer(self, Navigate, action_name, - execute_callback=self._execute, goal_callback=self._accept, - cancel_callback=self._cancel_goal, callback_group=self._group) - self._pump_timer = self.create_timer(config.poll_interval_sec, self._pump) - self._health_thread = threading.Thread(target=self._health_worker, - name='nav-readiness', daemon=True) - self._health_thread.start() - - def _health_worker(self): - while not self._closing.is_set(): - try: - response = self._client.health() - ready = response['ready'] and response.get('map_id') == map_id - 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): - try: - # Validation uses a temporary local UUID because ROS supplies the - # actual UUID with the accepted goal handle. Nothing is sent here. - build_goal_body(request, '00000000-0000-4000-8000-000000000000', map_id) - except (AttributeError, TypeError, ValueError): - return GoalResponse.REJECT - with self._guard: - if self._reserved or self._stop_unknown: - 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: - session = GoalSession(self._client, build_goal_body(handle.request, goal_id, map_id)) - with self._guard: - self._record = (handle, session, future) - if handle.is_cancel_requested: - session.request_cancel() - session.start() - except Exception as exc: - with self._guard: - self._stop_unknown = True - handle.abort() - return self._unknown_result(type(exc).__name__) - return await future - - def _cancel_goal(self, handle): - with self._guard: - record = self._record - if record is not None and bytes(record[0].goal_id.uuid) == bytes(handle.goal_id.uuid): - record[1].request_cancel() - # The accepted execute callback also checks is_cancel_requested, - # covering cancellation before a worker has been registered. - return CancelResponse.ACCEPT - - def _unknown_result(self, message, error_code='NAV_GATEWAY_STOP_UNKNOWN'): - result = Navigate.Result() - 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.final_pose_valid = False - result.final_position_error = math.nan - result.final_yaw_error = math.nan - return result - - def _pump(self): - with self._guard: - record = self._record - if record is None: - return - handle, session, future = record - for event in session.drain_events(): - if event.error is not None: - with self._guard: - self._stop_unknown = True - self._record = None - handle.abort() - future.set_result(self._unknown_result(event.error)) - continue - snapshot = event.snapshot - if event.terminal: - result = Navigate.Result() - result.result.status = navigation_status(snapshot, NavigationResult) - result.result.stop_state = NavigationResult.CONFIRMED - result.result.message = snapshot.message - 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.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.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: - handle.canceled() - else: - if snapshot.outcome == 'CANCELED': - # A remote cancellation has no matching ROS cancel - # transition. Preserve physical stop, report FAILED. - result.result.status = NavigationResult.FAILED - result.result.error_code = 'NAV_REMOTE_CANCELED' - handle.abort() - with self._guard: - self._record = None - self._reserved = False - future.set_result(result) - else: - feedback = Navigate.Feedback() - feedback.stamp = self.get_clock().now().to_msg() - 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): - self._closing.set() - with self._guard: - record = self._record - if record is not None: - record[1].request_cancel() - self._server.destroy() - return super().destroy_node() - - return NavigateProxy() - - -def main(argv: list[str] | None = None) -> None: - parser = argparse.ArgumentParser(description=__doc__) - parser.add_argument('--endpoint', default=os.environ.get('NAV_GATEWAY_URL', 'http://127.0.0.1:8766')) - parser.add_argument('--map-id', default=os.environ.get('NAV_GATEWAY_MAP_ID')) - parser.add_argument('--action-name', default=os.environ.get('NAV_PROXY_ACTION_NAME')) - for name in ('connect_timeout_sec', 'read_timeout_sec', 'request_timeout_sec', - 'poll_interval_sec', 'readiness_max_age_sec', 'feedback_silence_timeout_sec'): - parser.add_argument('--' + name.replace('_', '-'), type=float, - default=os.environ.get('NAV_PROXY_' + name.upper())) - args, ros_args = parser.parse_known_args(argv) - if not args.map_id or not args.action_name: - parser.error('--map-id and --action-name (or matching environment variables) are required') - try: - config = ProxyConfig(token=os.environ.get('NAV_GATEWAY_TOKEN', ''), endpoint=args.endpoint, - **{name: getattr(args, name) for name in ('connect_timeout_sec', 'read_timeout_sec', - 'request_timeout_sec', 'poll_interval_sec', 'readiness_max_age_sec', - 'feedback_silence_timeout_sec')}) - except ValueError as exc: - parser.error(str(exc)) - import rclpy - from rclpy.executors import MultiThreadedExecutor - rclpy.init(args=ros_args) - node = create_ros_node(config, map_id=args.map_id, action_name=args.action_name) - executor = MultiThreadedExecutor(num_threads=2) - executor.add_node(node) - try: - executor.spin() - finally: - node.destroy_node() - executor.shutdown() - rclpy.shutdown() - - -if __name__ == '__main__': - main() diff --git a/navigation_gateway/semantic_proxy.py b/navigation_gateway/semantic_proxy.py deleted file mode 100644 index 405b26f..0000000 --- a/navigation_gateway/semantic_proxy.py +++ /dev/null @@ -1,116 +0,0 @@ -"""ROS2 lookup facade. Existing Navigate proxy still owns physical stop proof.""" -import json,threading,time,math -from .catalog import Catalog - -def duration_seconds(value): - if type(value.sec) is not int or type(value.nanosec) is not int or value.sec<0 or not 0<=value.nanosec<1_000_000_000: - raise ValueError('invalid normalized Duration') - seconds=value.sec+value.nanosec/1e9 - 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 - from rclpy.action import ActionServer,ActionClient,GoalResponse,CancelResponse - 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 - path=self.declare_parameter('site_config_file','').value - with open(path) as f:self.catalog=Catalog(json.load(f)) - self.client=ActionClient(self,Navigate,self.declare_parameter('navigate_action','skills/navigate').value,callback_group=self.group) - self.server=ActionServer(self,NavigateSemantic,'skills/navigate_semantic',execute_callback=self.execute,goal_callback=self.admit,cancel_callback=lambda _:CancelResponse.ACCEPT,callback_group=self.group) - def admit(self,g): - try: - 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) - duration_seconds(g.timeout) - if any(not math.isfinite(x) or x<=0 for x in (g.position_tolerance,g.orientation_tolerance)) or g.orientation_tolerance>math.pi:return GoalResponse.REJECT - with self.lock: - if self.reserved or self.faulted:return GoalResponse.REJECT - self.reserved=True;self.accepted_at=time.monotonic() - return GoalResponse.ACCEPT - except (ValueError,KeyError):return GoalResponse.REJECT - def execute(self,h): - result=NavigateSemantic.Result();downstream=None;future=None - 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.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])) - if not self.client.server_is_ready():raise RuntimeError('NAVIGATE_UNAVAILABLE') - 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 - 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): - try: - accepted=f.result() - if accepted.accepted:accepted.cancel_goal_async() - except Exception:pass - future.add_done_callback(late);raise RuntimeError('ACCEPTANCE_UNKNOWN') - time.sleep(.01) - downstream=future.result() - if not downstream.accepted:raise RuntimeError('NAVIGATE_REJECTED') - done=downstream.get_result_async();cancel_at=None - while not done.done(): - 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') - time.sleep(.01) - wrapped=done.result();m=wrapped.result - 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=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 - rclpy.init();node=Proxy();executor=MultiThreadedExecutor(num_threads=4);executor.add_node(node) - try:executor.spin() - finally:executor.shutdown();node.destroy_node();rclpy.shutdown() -if __name__=='__main__':main() diff --git a/navigation_gateway/server.py b/navigation_gateway/server.py deleted file mode 100644 index 8c1bcde..0000000 --- a/navigation_gateway/server.py +++ /dev/null @@ -1,139 +0,0 @@ -"""Authenticated stdlib HTTP server; ROS imports are optional and isolated.""" -import argparse -import hmac -from http.server import BaseHTTPRequestHandler, ThreadingHTTPServer -import ipaddress -import json -import os -from pathlib import Path -import signal -import threading -import time -from urllib.parse import urlsplit -from .backends import MockBackend, Ros1MoveBaseBackend -from .gateway import Gateway, GatewayError, SafetyConfig - - -def make_server(gateway, token, host="127.0.0.1", port=8766): - if not isinstance(token, str) or len(token) < 16: raise ValueError("bearer token must contain at least 16 characters") - class Handler(BaseHTTPRequestHandler): - server_version = "NavigationGateway/1" - - def setup(self): - super().setup() - self.connection.settimeout(5.0) # HTTP resource bound, not a robot safety threshold. - - def log_message(self, format, *args): - return # Never log bearer headers or raw task input. - - def respond(self, status, body): - raw = json.dumps(body, allow_nan=False, separators=(",", ":")).encode() - self.send_response(status) - self.send_header("Content-Type", "application/json") - self.send_header("Content-Length", str(len(raw))) - self.send_header("Cache-Control", "no-store") - self.end_headers() - self.wfile.write(raw) - - def body(self): - if self.headers.get("Transfer-Encoding"): raise GatewayError("chunked bodies are unsupported") - try: length = int(self.headers.get("Content-Length", "0")) - except ValueError: raise GatewayError("invalid Content-Length") - if not 0 < length <= 65536: raise GatewayError("body must contain 1..65536 bytes", 413) - if self.headers.get("Content-Type", "").split(";")[0].strip() != "application/json": raise GatewayError("application/json required", 415) - def duplicate_safe(pairs): - obj = {} - for key, value in pairs: - if key in obj: raise GatewayError("duplicate JSON field") - obj[key] = value - return obj - try: - return json.loads(self.rfile.read(length), object_pairs_hook=duplicate_safe, - parse_constant=lambda value: (_ for _ in ()).throw(GatewayError("non-finite JSON value"))) - except (ValueError, UnicodeError): raise GatewayError("invalid JSON") - - def handle_request(self): - try: - expected = "Bearer " + token - if not hmac.compare_digest(self.headers.get("Authorization", ""), expected): - raise GatewayError("valid bearer authorization required", 401) - parts = urlsplit(self.path) - if parts.query or parts.fragment: raise GatewayError("query parameters unsupported") - path = parts.path.rstrip("/") - if self.command == "GET" and path == "/healthz": return self.respond(200, gateway.health()) - if self.command == "POST" and path == "/v1/goals": return self.respond(202, gateway.submit(self.body())) - split = path.split("/") - if len(split) in (4, 5) and split[1:3] == ["v1", "goals"]: - goal_id = split[3] - if self.command == "GET" and len(split) == 4: return self.respond(200, gateway.poll(goal_id)) - if self.command == "POST" and len(split) == 5 and split[4] == "cancel": - if self.body() != {}: raise GatewayError("cancel body must be {}") - return self.respond(202, gateway.cancel(goal_id)) - raise GatewayError("route not found", 404) - except GatewayError as exc: - self.respond(exc.http_status, {"error": str(exc)}) - except (TimeoutError, ConnectionError, BrokenPipeError): - self.close_connection = True - except Exception: - self.respond(500, {"error": "internal error; query original UUID; never resubmit with a new UUID"}) - - do_GET = handle_request - do_POST = handle_request - - class Server(ThreadingHTTPServer): - daemon_threads = True - request_queue_size = 16 - return Server((host, port), Handler) - - -def main(): - parser = argparse.ArgumentParser(description=__doc__) - parser.add_argument("--config", required=True, help="JSON config with explicit safety thresholds") - parser.add_argument("--journal", required=True, help="durable SQLite journal; one per physical robot") - parser.add_argument("--host", default="127.0.0.1") - parser.add_argument("--port", type=int, default=8766) - args = parser.parse_args() - if not ipaddress.ip_address(args.host).is_loopback: - parser.error("HTTP bearer transport listens only on loopback; use an authenticated SSH/TLS tunnel") - token = os.environ.get("NAV_GATEWAY_TOKEN", "") - if len(token) < 16: parser.error("set NAV_GATEWAY_TOKEN to at least 16 characters") - config = json.loads(Path(args.config).read_text()) - safety = SafetyConfig(**config["safety"]) - if config["mode"] == "simulation": - backend = MockBackend(map_id=config["map_id"], auto_complete_sec=config["simulation_complete_sec"]) - elif config["mode"] == "ros1_move_base": - if config.get("production_enable") is not True: parser.error("production_enable must be explicitly true after site acceptance") - import rospy - rospy.init_node("navigation_http_gateway", disable_signals=True) - backend = Ros1MoveBaseBackend(map_id=config["map_id"], **config["ros1"]) - else: parser.error("mode must be simulation or ros1_move_base") - period = config["poll_interval_sec"] - if isinstance(period, bool) or not isinstance(period, (int, float)) or not 0 < period <= safety.odom_max_age_sec: - parser.error("poll_interval_sec must be positive and <= odom_max_age_sec") - gateway = Gateway(args.journal, backend, safety) - server = make_server(gateway, token, args.host, args.port) - stop = threading.Event() - def monitor(): - while not stop.wait(period): - try: gateway.poll_all() - except Exception: - # Persist/telemetry errors must not release resource ownership. - stop.set() - threading.Thread(target=server.shutdown, daemon=True).start() - thread = threading.Thread(target=monitor, daemon=True); thread.start() - def shutdown(signum, frame): - stop.set() - threading.Thread(target=server.shutdown, daemon=True).start() - signal.signal(signal.SIGTERM, shutdown) - signal.signal(signal.SIGINT, shutdown) - try: server.serve_forever(poll_interval=0.1) - finally: - stop.set(); thread.join(timeout=2) - # Best effort cancel cannot turn shutdown into stop confirmation. - for goal_id in list(gateway.records): - try: gateway.cancel(goal_id) - except Exception: pass - server.server_close(); gateway.close() - - -if __name__ == "__main__": main() diff --git a/navigation_gateway/simulation.json b/navigation_gateway/simulation.json deleted file mode 100644 index 1f0bbf6..0000000 --- a/navigation_gateway/simulation.json +++ /dev/null @@ -1,15 +0,0 @@ -{ - "mode": "simulation", - "map_id": "sim-map", - "simulation_complete_sec": 0.5, - "poll_interval_sec": 0.05, - "safety": { - "odom_max_age_sec": 0.2, - "stationary_window_sec": 0.3, - "linear_stopped_mps": 0.005, - "angular_stopped_radps": 0.005, - "pose_max_age_sec": 0.2, - "readiness_max_age_sec": 0.5, - "stop_wait_timeout_sec": 1.0 - } -} diff --git a/ros2/bt_executor/CMakeLists.txt b/ros2/bt_executor/CMakeLists.txt index 9052a56..d579dde 100644 --- a/ros2/bt_executor/CMakeLists.txt +++ b/ros2/bt_executor/CMakeLists.txt @@ -7,6 +7,7 @@ find_package(ament_index_cpp REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_action REQUIRED) find_package(bt_skill_interfaces REQUIRED) +find_package(navigation_interfaces REQUIRED) find_package(geometry_msgs REQUIRED) find_package(std_msgs REQUIRED) find_package(behaviortree_cpp 4.10.0 EXACT REQUIRED) @@ -18,7 +19,7 @@ target_include_directories(robot_bt_core PUBLIC ../../core/include) add_executable(bt_executor_node src/executor_node.cpp src/ros_driver.cpp) target_include_directories(bt_executor_node PRIVATE include) target_link_libraries(bt_executor_node robot_bt_core behaviortree_cpp::behaviortree_cpp nlohmann_json::nlohmann_json) -ament_target_dependencies(bt_executor_node ament_index_cpp rclcpp rclcpp_action bt_skill_interfaces geometry_msgs std_msgs) +ament_target_dependencies(bt_executor_node ament_index_cpp rclcpp rclcpp_action bt_skill_interfaces navigation_interfaces geometry_msgs std_msgs) target_compile_options(bt_executor_node PRIVATE -Wall -Wextra -Wpedantic) install(TARGETS bt_executor_node DESTINATION lib/${PROJECT_NAME}) install(DIRECTORY trees launch config DESTINATION share/${PROJECT_NAME}) diff --git a/ros2/bt_executor/include/bt_executor/ros_driver.hpp b/ros2/bt_executor/include/bt_executor/ros_driver.hpp index b42029c..bd4acf9 100644 --- a/ros2/bt_executor/include/bt_executor/ros_driver.hpp +++ b/ros2/bt_executor/include/bt_executor/ros_driver.hpp @@ -3,8 +3,7 @@ #include #include #include -#include -#include +#include #include #include #include @@ -54,8 +53,7 @@ class RosDriver final : public robot_bt::GoalDriver { std::optional geometry_epoch() const; private: - using Semantic = iface::action::NavigateSemantic; - using Navigate = iface::action::Navigate; + using Navigate = navigation_interfaces::action::NavigateToPose; using Manipulate = iface::action::ExecuteManipulation; using Locate = iface::action::LocateShelfColumn; using Localize = iface::action::LocalizeTarget3D; @@ -70,7 +68,6 @@ class RosDriver final : public robot_bt::GoalDriver { builtin_interfaces::msg::Duration skill_timeout_; bool faulted_{false}; rclcpp_action::Client::SharedPtr navigate_; - rclcpp_action::Client::SharedPtr semantic_; rclcpp_action::Client::SharedPtr manipulate_; rclcpp_action::Client::SharedPtr locate_; rclcpp_action::Client::SharedPtr localize_; @@ -98,7 +95,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 execution(const Navigate::Result&,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, @@ -171,7 +168,8 @@ class RosDriver final : public robot_bt::GoalDriver { if constexpr (std::is_same_v) { 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; + payload["blocked_valid"]=feedback->blocked_valid; + payload["blocked"]=feedback->blocked_valid?json(feedback->blocked):json(nullptr); } event.feedback_snapshot=payload.dump();events_.push_back(event); }; diff --git a/ros2/bt_executor/package.xml b/ros2/bt_executor/package.xml index c17e7e1..39a5198 100644 --- a/ros2/bt_executor/package.xml +++ b/ros2/bt_executor/package.xml @@ -5,7 +5,7 @@ wangfeiyuProprietary ament_cmake rclcpprclcpp_actionament_index_cpp - bt_skill_interfacesgeometry_msgsstd_msgs + bt_skill_interfacesnavigation_interfacesgeometry_msgsstd_msgs behaviortree_cppnlohmann_json launch_roslaunch ament_cmake diff --git a/ros2/bt_executor/src/ros_driver.cpp b/ros2/bt_executor/src/ros_driver.cpp index 0a01f4c..cb33596 100644 --- a/ros2/bt_executor/src/ros_driver.cpp +++ b/ros2/bt_executor/src/ros_driver.cpp @@ -89,7 +89,6 @@ RosDriver::RosDriver(rclcpp::Node& n, std::string robot_id, std::string journal, navigate_=rclcpp_action::create_client(&n,n.declare_parameter("navigate_action","skills/navigate")); manipulate_=rclcpp_action::create_client(&n,n.declare_parameter("execute_manipulation_action","skills/execute_manipulation")); locate_=rclcpp_action::create_client(&n,n.declare_parameter("locate_shelf_column_action","skills/locate_shelf_column")); - semantic_=rclcpp_action::create_client(&n,n.declare_parameter("navigate_semantic_action","skills/navigate_semantic")); localize_=rclcpp_action::create_client(&n,n.declare_parameter("localize_target_3d_action","skills/localize_target_3d")); assess_=rclcpp_action::create_client(&n,n.declare_parameter("assess_grasp_action","skills/assess_grasp")); posture_=rclcpp_action::create_client(&n,n.declare_parameter("execute_posture_action","skills/execute_posture")); @@ -126,7 +125,7 @@ std::optional RosDriver::geometry_epoch() const { bool RosDriver::ready(Skill skill)const { if(faulted_)return false; switch(skill) { - case Skill::NAVIGATE:return current_task_.route=="LEGACY"?navigate_->action_server_is_ready():semantic_->action_server_is_ready(); + case Skill::NAVIGATE:return navigate_->action_server_is_ready(); case Skill::PICK:case Skill::PLACE:return manipulate_->action_server_is_ready(); case Skill::LOCATE_SHELF_COLUMN:return locate_->action_server_is_ready(); case Skill::LOCALIZE_TARGET:return localize_->action_server_is_ready(); @@ -170,23 +169,23 @@ 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; +ExecutionResult RosDriver::execution(const Navigate::Result& m,const GoalRequest& request)const { + ExecutionResult out;out.error_code=m.error_code; + using Nav=Navigate::Result; 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;} + case Nav::SUCCEEDED:out.code=ResultCode::COMPLETED;break; + case Nav::CANCELED:out.code=ResultCode::CANCELED;break; + case Nav::TIMEOUT:out.code=ResultCode::TIMED_OUT;break; + case Nav::BLOCKED:out.code=ResultCode::FAILED;if(out.error_code.empty())out.error_code="NAV_BLOCKED";break; + case Nav::NOT_READY:out.code=ResultCode::REJECTED;if(out.error_code.empty())out.error_code="NAV_NOT_READY";break; + case Nav::FAILED:out.code=ResultCode::FAILED;break; + default:out.error_code="NAV_RESULT_PROTOCOL_ERROR";return out; } - return execution(common,request); + out.detail=out.error_code+": "+m.message; + if(m.stop_state==Nav::STOP_CONFIRMED&&!m.stop_evidence_ref.empty()&& + fresh(ns(m.stopped_at),ns(m.stopped_at)+observation_lifetime_ns_,request.capture_after)) + out.stop=StopState::CONFIRMED; + return out; } 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; @@ -219,18 +218,10 @@ void RosDriver::send(const GoalRequest& r) { switch(r.skill) { case Skill::NAVIGATE: { if(!r.registered_pose)throw std::runtime_error("registered navigation pose required"); - if(!r.navigation_kind.empty()) { - Semantic::Goal g;g.trace=trace_msg(r.trace);g.kind=r.navigation_kind;g.reference=r.navigation_ref;g.shelf_id=r.shelf;g.side_id=r.side;g.column_id=r.column;g.tier_id=r.tier;g.registry_version=registry_version_;g.position_tolerance=r.position_tolerance_m;g.orientation_tolerance=r.orientation_tolerance_rad;g.timeout=timeout(); - send_typed(semantic_,g,r,6,[this,r](const Semantic::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&&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); - out.response.base_stopped=out.stop==StopState::CONFIRMED;return out; - });break; - } - Navigate::Goal g;g.trace=trace_msg(r.trace);g.target_pose=pose_msg(*r.registered_pose,now); + Navigate::Goal g;g.task_id=r.trace.task_id;g.subtask_id=r.trace.subtask_id;g.target_pose=pose_msg(*r.registered_pose,now); 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&& + auto out=execution(m,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; diff --git a/ros2/bt_executor/tools/build_humble.sh b/ros2/bt_executor/tools/build_humble.sh index 9459b1a..404341e 100755 --- a/ros2/bt_executor/tools/build_humble.sh +++ b/ros2/bt_executor/tools/build_humble.sh @@ -14,5 +14,5 @@ cd "$bt_repo_root" # Install/build upstream BehaviorTree.CPP 4.10.0 separately and source its prefix. # EXACT REQUIRED intentionally rejects an incompatible system BT.CPP package. colcon build --base-paths ros2 --packages-up-to bt_executor robobrain_services bt_mock_servers --event-handlers console_direct+ -colcon test --packages-select bt_skill_interfaces bt_executor --event-handlers console_direct+ +colcon test --packages-select navigation_interfaces bt_skill_interfaces bt_executor --event-handlers console_direct+ colcon test-result --verbose diff --git a/ros2/bt_executor/tools/check_static.py b/ros2/bt_executor/tools/check_static.py index 55e783d..16f37c9 100644 --- a/ros2/bt_executor/tools/check_static.py +++ b/ros2/bt_executor/tools/check_static.py @@ -38,8 +38,8 @@ def header_name(name): for source in list((PACKAGE / 'src').glob('*.cpp')) + list((PACKAGE / 'include/bt_executor').glob('*.hpp')): - for kind, name in re.findall(r'bt_skill_interfaces/(action|msg)/(\w+)\.hpp', source.read_text()): - candidates = list((PACKAGE.parent / 'bt_skill_interfaces' / kind).glob('*')) + for package, kind, name in re.findall(r'(bt_skill_interfaces|navigation_interfaces)/(action|msg)/(\w+)\.hpp', source.read_text()): + candidates = list((PACKAGE.parent / package / kind).glob('*')) assert any(header_name(v.stem) == name for v in candidates), (source, kind, name) ET.parse(PACKAGE / 'package.xml') print(f'Static checks passed: {len(actual)} ordered core stages, six skill templates, generated IDL include names, package XML.') diff --git a/ros2/bt_executor/tools/native_navigation_probe.cpp b/ros2/bt_executor/tools/native_navigation_probe.cpp index 05de919..6af624b 100644 --- a/ros2/bt_executor/tools/native_navigation_probe.cpp +++ b/ros2/bt_executor/tools/native_navigation_probe.cpp @@ -20,7 +20,7 @@ int main(int argc,char** argv) { while(!driver.ready(robot_bt::Skill::NAVIGATE)&&robot_bt::SteadyClock::now()accepted&&!canceled) { + if((scenario==1||scenario==6||scenario==9)&&record->accepted&&!canceled) { registry.request_cancel(active_id,robot_bt::SteadyClock::now());canceled=true; } if(record->result) break; @@ -51,7 +51,7 @@ int main(int argc,char** argv) { if(!record||!record->result) throw std::runtime_error("result timeout"); const auto& result=*record->result; bool blocked_redispatch=false; - if(scenario==6) { + if(scenario==6||scenario==9||scenario==10) { 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(); diff --git a/ros2/bt_mock_servers/bt_mock_servers/mock_skills.py b/ros2/bt_mock_servers/bt_mock_servers/mock_skills.py index d02fc73..15ece4b 100644 --- a/ros2/bt_mock_servers/bt_mock_servers/mock_skills.py +++ b/ros2/bt_mock_servers/bt_mock_servers/mock_skills.py @@ -19,20 +19,21 @@ from rclpy.node import Node from bt_skill_interfaces.action import ( AssessGrasp, CheckFreeSpace, ExecuteManipulation, ExecutePosture, EvaluateProgress, ExecuteTask, LocalizeTarget3D, LocateShelfColumn, - Navigate, NavigateSemantic, PlanTask, VerifyState, + PlanTask, VerifyState, ) -from bt_skill_interfaces.msg import (DenseProgress, ExecutionResult, NavigationResult, RobotState, SafetyState, +from navigation_interfaces.action import NavigateToPose +from bt_skill_interfaces.msg import (DenseProgress, ExecutionResult, RobotState, SafetyState, VerificationEvidence, VisualObservation) from bt_skill_interfaces.srv import GetRobotState, ReconcileGoal from std_msgs.msg import String from .scenarios import duration_seconds, fixture_at, fixed_plan, parse_scenarios, strict_json, validate_trace ACTION_TYPES = { - "navigate": Navigate, "execute_manipulation": ExecuteManipulation, + "navigate": NavigateToPose, "execute_manipulation": ExecuteManipulation, "plan_task": PlanTask, "locate_shelf_column": LocateShelfColumn, "localize_target_3d": LocalizeTarget3D, "check_free_space": CheckFreeSpace, "assess_grasp": AssessGrasp, "execute_posture": ExecutePosture, "verify_state": VerifyState, - "navigate_semantic": NavigateSemantic, "evaluate_progress": EvaluateProgress, + "evaluate_progress": EvaluateProgress, "execute_task": ExecuteTask, } ACTION_ENDPOINTS = { @@ -41,10 +42,10 @@ ACTION_ENDPOINTS = { "execute_task": "tasks/execute", "evaluate_progress": "monitor/evaluate_progress", } -MOTION = frozenset(("navigate", "navigate_semantic", "execute_manipulation", "execute_posture", "execute_task")) +MOTION = frozenset(("navigate", "execute_manipulation", "execute_posture", "execute_task")) SUCCESS_PHASES = { - "navigate": (Navigate.Feedback.ACCEPTED, Navigate.Feedback.CHECKING, - Navigate.Feedback.NAVIGATING), + "navigate": (NavigateToPose.Feedback.ACCEPTED, NavigateToPose.Feedback.CHECKING, + NavigateToPose.Feedback.PLANNING, NavigateToPose.Feedback.NAVIGATING), "execute_manipulation": (ExecuteManipulation.Feedback.PREPARING, ExecuteManipulation.Feedback.WAITING_OBSERVATION, ExecuteManipulation.Feedback.INFERRING, @@ -55,7 +56,7 @@ SUCCESS_PHASES = { "assess_grasp": (0,), "execute_posture": (ExecutePosture.Feedback.CHECKING, ExecutePosture.Feedback.MOVING, ExecutePosture.Feedback.SETTLING), - "verify_state": (0,), "navigate_semantic": (0,), "evaluate_progress": (0,), + "verify_state": (0,), "evaluate_progress": (0,), } @@ -80,13 +81,13 @@ def normalized_navigation_pose(target_pose): def lifecycle_phases(name, kind): if name == "navigate" and kind == "obstacle_recovery": - return (Navigate.Feedback.ACCEPTED, Navigate.Feedback.CHECKING, - Navigate.Feedback.NAVIGATING, Navigate.Feedback.BLOCKED, - Navigate.Feedback.NAVIGATING) + return (NavigateToPose.Feedback.ACCEPTED, NavigateToPose.Feedback.CHECKING, + NavigateToPose.Feedback.PLANNING, NavigateToPose.Feedback.NAVIGATING, NavigateToPose.Feedback.BLOCKED, + NavigateToPose.Feedback.NAVIGATING) if name == "navigate" and kind == "blocked": - return (*SUCCESS_PHASES[name], Navigate.Feedback.BLOCKED) + return (*SUCCESS_PHASES[name], NavigateToPose.Feedback.BLOCKED) if name == "navigate" and kind == "not_ready": - return (Navigate.Feedback.ACCEPTED, Navigate.Feedback.CHECKING) + return (NavigateToPose.Feedback.ACCEPTED, NavigateToPose.Feedback.CHECKING) return SUCCESS_PHASES.get(name, (0,)) @@ -199,15 +200,6 @@ class MockSkills(Node): destination = (bool(request.destination.region_ref), bool(request.destination.description)) if destination != ((False, False) if request.skill == "pick" else (True, True)): raise ValueError("destination must be empty for pick and complete for place") - elif name == "navigate_semantic": - if request.kind not in ("LOCATION", "OBJECT", "CELL") or not request.reference or not request.registry_version: - raise ValueError("semantic navigation binding is invalid") - if request.kind == "CELL" and any( - not isinstance(value, str) or not value or "/" in value - for value in (request.shelf_id, request.side_id, request.column_id, request.tier_id)): - raise ValueError("semantic cell binding is incomplete") - if request.position_tolerance <= 0 or not 0 < request.orientation_tolerance <= math.pi: - raise ValueError("semantic navigation tolerances are invalid") elif name == "execute_posture": if request.posture_id not in self.allowed_postures or not request.expected_geometry_epoch: raise ValueError("posture or geometry epoch is invalid") @@ -281,10 +273,18 @@ class MockSkills(Node): self.counts[name] += 1 self.accepted[goal_id] = (time.monotonic(), fixture) if name in MOTION: - self.motion_owner = (goal_id, name, copy.deepcopy(handle.request.trace)) + self.motion_owner = (goal_id, name, self._goal_identity(name, handle.request)) handle.execute() + @staticmethod + def _goal_identity(name, request): + if name == "navigate": + return (request.task_id, request.subtask_id) + return copy.deepcopy(request.trace) + def _execute(self, name, handle): + if name == "navigate": + return self._execute_navigation(handle) goal_id = bytes(handle.goal_id.uuid).hex() with self.lock: started, fixture = self.accepted.pop(goal_id) @@ -324,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" or name == "navigate" and kind in ("blocked", "not_ready")) and outcome == ExecutionResult.COMPLETED: + if kind == "failed" and outcome == ExecutionResult.COMPLETED: outcome = ExecutionResult.FAILED if kind == "stop_unknown" and outcome == ExecutionResult.COMPLETED: outcome, stop_state = ExecutionResult.FAILED, ExecutionResult.UNKNOWN @@ -334,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), name=name) + result.result = self._execution_result(ExecutionResult.FAILED, stop_state, goal_id, "MOCK_EXCEPTION", str(exc)) elif hasattr(result, "evidence"): result.evidence.status = VerificationEvidence.UNKNOWN result.evidence.error_code = "MOCK_EXCEPTION" @@ -357,7 +357,109 @@ class MockSkills(Node): if self.motion_owner and self.motion_owner[0] == goal_id: self.motion_owner = None elif name in MOTION: - self.unresolved_motion[goal_id] = (name, copy.deepcopy(handle.request.trace)) + self.unresolved_motion[goal_id] = (name, self._goal_identity(name, handle.request)) + return result + + def _execute_navigation(self, handle): + """Explicit simulator backend producing only native navigation messages.""" + goal_id = bytes(handle.goal_id.uuid).hex() + with self.lock: + started, fixture = self.accepted.pop(goal_id) + kind = fixture.get("kind", "normal") + deadline = started + duration_seconds(handle.request.timeout) + finish = started + fixture.get("duration_seconds", 0.2) + result = NavigateToPose.Result() + result.status = result.SUCCEEDED + result.stop_state = result.STOP_UNKNOWN + sequence, emitted_phase = 0, -1 + try: + while True: + now = time.monotonic() + if handle.is_cancel_requested: + result.status = result.CANCELED + self._stopping_feedback("navigate", handle, sequence + 1, now - started) + time.sleep(fixture.get("stop_delay_seconds", 0.0)) + break + if now >= deadline or not rclpy.ok(): + result.status = result.TIMEOUT + self._stopping_feedback("navigate", handle, sequence + 1, now - started) + break + if now >= finish and kind not in ("timeout", "timeout_stop_unknown", "silence"): + break + if kind != "silence": + phases = lifecycle_phases("navigate", kind) + phase_index = min(len(phases) - 1, int((now - started) / max(0.001, finish - started) * len(phases))) + if phase_index > emitted_phase: + sequence += 1 + self._feedback("navigate", handle, sequence, now - started, phases[phase_index]) + emitted_phase = phase_index + time.sleep(min(0.02, max(0, deadline - now))) + if result.status == result.SUCCEEDED: + if kind in ("failed", "stop_unknown"): + result.status = result.FAILED + elif kind == "blocked": + result.status = result.BLOCKED + elif kind == "not_ready": + result.status = result.NOT_READY + if kind not in ("stop_unknown", "cancel_stop_unknown", "timeout_stop_unknown"): + # This dedicated simulator explicitly observes its own stopped state. + result.stop_state = result.STOP_CONFIRMED + result.stopped_at = self.get_clock().now().to_msg() + result.stop_evidence_ref = "sim://stop/" + goal_id + result.error_code = "" if result.status == result.SUCCEEDED else "MOCK_TERMINATED" + if result.status == result.BLOCKED: + result.error_code = "BLOCKED" + elif result.status == result.NOT_READY: + result.error_code = fixture.get("error_code", "INPUTS_UNHEALTHY") + result.message = "SIMULATED navigation only" + result.final_pose_valid = result.status == result.SUCCEEDED + if result.final_pose_valid: + result.final_pose = normalized_navigation_pose(handle.request.target_pose) + result.final_pose.header.stamp = self.get_clock().now().to_msg() + pose_fixture = fixture.get("final_pose") + if pose_fixture is not None: + result.final_pose.header.frame_id = pose_fixture["frame_id"] + for field in ("x", "y", "z"): + setattr(result.final_pose.pose.position, field, float(pose_fixture[field])) + for field in ("x", "y", "z", "w"): + setattr(result.final_pose.pose.orientation, field, float(pose_fixture["q" + field])) + actual, target = result.final_pose.pose, normalized_navigation_pose(handle.request.target_pose).pose + result.final_position_error = math.hypot(actual.position.x - target.position.x, + actual.position.y - target.position.y, + actual.position.z - target.position.z) + def yaw(q): + return math.atan2(2 * (q.w * q.z + q.x * q.y), 1 - 2 * (q.y * q.y + q.z * q.z)) + delta = yaw(target.orientation) - yaw(actual.orientation) + result.final_yaw_error = math.atan2(math.sin(delta), math.cos(delta)) + if result.stop_state == result.STOP_CONFIRMED: + with self.lock: + self.geometry_epoch += 1 + if kind == "native_mismatch": + handle.abort() + elif result.status == result.CANCELED: + handle.canceled() + elif result.status == result.SUCCEEDED: + handle.succeed() + else: + handle.abort() + except Exception as exc: + result.status, result.stop_state = result.FAILED, result.STOP_UNKNOWN + result.error_code, result.message = "MOCK_EXCEPTION", str(exc) + result.stopped_at, result.stop_evidence_ref = Time(), "" + result.final_pose_valid = False + if handle.is_active: + handle.abort() + self.get_logger().error("Mock navigation exception: " + str(exc)) + finally: + with self.lock: + self.inflight -= 1 + if result.stop_state == result.STOP_CONFIRMED: + self.motion_reserved = False + self.unresolved_motion.pop(goal_id, None) + if self.motion_owner and self.motion_owner[0] == goal_id: + self.motion_owner = None + else: + self.unresolved_motion[goal_id] = ("navigate", self._goal_identity("navigate", handle.request)) return result @staticmethod @@ -390,30 +492,22 @@ class MockSkills(Node): feedback.error_valid = True feedback.position_error = 0.0 feedback.yaw_error = 0.0 - feedback.blocked = feedback.phase == Navigate.Feedback.BLOCKED + feedback.blocked_valid = True + feedback.blocked = feedback.phase == NavigateToPose.Feedback.BLOCKED if name == "execute_manipulation": feedback.progress_valid = False handle.publish_feedback(feedback) def _stopping_feedback(self, name, handle, sequence, elapsed): - stopping = {"navigate": Navigate.Feedback.STOPPING, + stopping = {"navigate": NavigateToPose.Feedback.STOPPING, "execute_manipulation": ExecuteManipulation.Feedback.STOPPING, "execute_posture": ExecutePosture.Feedback.STOPPING}.get(name) 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", 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 + def _execution_result(self, outcome, stop_state, goal_id, error="", message="SIMULATED execution only"): + result = ExecutionResult() + result.status, result.stop_state = outcome, stop_state result.error_code = error or ("" if outcome == ExecutionResult.COMPLETED else "MOCK_TERMINATED") result.message = message if stop_state == ExecutionResult.CONFIRMED: @@ -430,34 +524,8 @@ class MockSkills(Node): record = "sim://" + name + "/" + goal_id ok = outcome == ExecutionResult.COMPLETED if hasattr(result, "result"): - 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.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 - if pose is not None: - result.final_pose.header.frame_id = pose["frame_id"] - result.final_pose.pose.position.x = pose["x"] - result.final_pose.pose.position.y = pose["y"] - result.final_pose.pose.position.z = pose["z"] - result.final_pose.pose.orientation.x = pose["qx"] - result.final_pose.pose.orientation.y = pose["qy"] - result.final_pose.pose.orientation.z = pose["qz"] - result.final_pose.pose.orientation.w = pose["qw"] - result.final_pose.header.stamp = observed - elif name == "execute_manipulation": + result.result = self._execution_result(outcome, stop_state, goal_id) + if name == "execute_manipulation": result.execution_record_ref = record # Deliberately no holding/verification state mutation here. elif name == "plan_task": @@ -725,10 +793,12 @@ class MockSkills(Node): fresh = 0 < observed_ns <= now < valid_until_ns with self.lock: owned = self.unresolved_motion.get(request.goal_id) - owned_trace_matches = owned is not None and all( + owned_trace_matches = owned is not None and owned[0] != "navigate" and all( getattr(owned[1], field) == getattr(request.trace, field) for field in ("task_id", "subtask_id", "attempt", "task_revision", "plan_version", "run_id", "execution_generation")) + if owned is not None and owned[0] == "navigate": + owned_trace_matches = owned[1] == (request.trace.task_id, request.trace.subtask_id) owner_matches = self.motion_owner is not None and self.motion_owner[0] == request.goal_id bound = (owner_matches and owned_trace_matches and evidence.context.source_goal_id == request.goal_id and same_trace and evidence.status == evidence.PASSED and diff --git a/ros2/bt_mock_servers/bt_mock_servers/scenarios.py b/ros2/bt_mock_servers/bt_mock_servers/scenarios.py index 6939c3b..330263d 100644 --- a/ros2/bt_mock_servers/bt_mock_servers/scenarios.py +++ b/ros2/bt_mock_servers/bt_mock_servers/scenarios.py @@ -6,7 +6,7 @@ ACTION_NAMES = ( "navigate", "execute_manipulation", "plan_task", "locate_shelf_column", "localize_target_3d", "check_free_space", "assess_grasp", "execute_posture", "verify_state", - "navigate_semantic", "evaluate_progress", "execute_task", + "evaluate_progress", "execute_task", "robot_state", "safety_state", "visual_observation", "dense_progress", "goal_registry", ) KINDS = { @@ -32,7 +32,6 @@ FIXTURE_FIELDS = { BASE_KINDS = {"normal", "failed", "timeout", "silence", "reject", "native_mismatch"} ACTION_KINDS = { "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"}, "execute_task": BASE_KINDS | {"stop_unknown", "cancel_stop_unknown", "timeout_stop_unknown"}, @@ -89,7 +88,7 @@ def parse_scenarios(raw): 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", + code not in {"INPUTS_UNHEALTHY", "ROBOT_STATE_UNAVAILABLE", "EXECUTION_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) @@ -115,12 +114,12 @@ def parse_scenarios(raw): if "final_pose" in fixture: pose = fixture["final_pose"] required = {"frame_id", "x", "y", "z", "qx", "qy", "qz", "qw"} - if not isinstance(pose, dict) or set(pose) != required or not isinstance(pose["frame_id"], str) or not pose["frame_id"]: - raise ValueError("final_pose requires an exact frame and pose") + if name != "navigate" or not isinstance(pose, dict) or set(pose) != required or pose["frame_id"] != "map": + raise ValueError("final_pose requires a navigation pose in map") values = [pose[key] for key in ("x", "y", "z", "qx", "qy", "qz", "qw")] if any(type(value) not in (int, float) or not math.isfinite(value) for value in values): raise ValueError("final_pose values must be finite") - if abs(sum(pose[key] ** 2 for key in ("qx", "qy", "qz", "qw")) - 1.0) > 0.001: + if abs(math.hypot(*(pose[key] for key in ("qx", "qy", "qz", "qw"))) - 1.0) > 0.001: raise ValueError("final_pose quaternion must have unit norm") result[name] = entries return result diff --git a/ros2/bt_mock_servers/package.xml b/ros2/bt_mock_servers/package.xml index 4371701..d8a5d96 100644 --- a/ros2/bt_mock_servers/package.xml +++ b/ros2/bt_mock_servers/package.xml @@ -8,6 +8,7 @@ ament_python rclpy bt_skill_interfaces + navigation_interfaces builtin_interfaces std_msgs ament_python diff --git a/ros2/bt_skill_interfaces/CMakeLists.txt b/ros2/bt_skill_interfaces/CMakeLists.txt index f6fe3f6..3a9bfd8 100644 --- a/ros2/bt_skill_interfaces/CMakeLists.txt +++ b/ros2/bt_skill_interfaces/CMakeLists.txt @@ -10,7 +10,6 @@ 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" @@ -18,8 +17,6 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/Station.msg" "msg/TargetBinding.msg" "msg/PlacementBinding.msg" - "action/Navigate.action" - "action/NavigateSemantic.action" "action/EvaluateProgress.action" "msg/VisualObservation.msg" "msg/DenseProgress.msg" diff --git a/ros2/bt_skill_interfaces/action/NavigateSemantic.action b/ros2/bt_skill_interfaces/action/NavigateSemantic.action deleted file mode 100644 index 677a0b9..0000000 --- a/ros2/bt_skill_interfaces/action/NavigateSemantic.action +++ /dev/null @@ -1,24 +0,0 @@ -# v1.1 navigation-owned lookup; no model-supplied pose. -bt_skill_interfaces/TaskTrace trace -string kind -string reference -string shelf_id -string side_id -string column_id -string tier_id -uint32 registry_version -float32 position_tolerance -float32 orientation_tolerance -builtin_interfaces/Duration timeout ---- -bt_skill_interfaces/ExecutionResult result -geometry_msgs/PoseStamped final_pose -bool pose_valid -float32 final_position_error -float32 final_orientation_error -bool errors_valid ---- -builtin_interfaces/Time stamp -uint32 sequence -uint8 phase -string message diff --git a/ros2/bt_skill_interfaces/msg/NavigationResult.msg b/ros2/bt_skill_interfaces/msg/NavigationResult.msg deleted file mode 100644 index d29968d..0000000 --- a/ros2/bt_skill_interfaces/msg/NavigationResult.msg +++ /dev/null @@ -1,15 +0,0 @@ -# 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 b4447e8..271d22d 100644 --- a/ros2/bt_skill_interfaces/package.xml +++ b/ros2/bt_skill_interfaces/package.xml @@ -2,7 +2,7 @@ bt_skill_interfaces 2.0.0 - Robot skill contracts with navigation-specific outcomes and stop evidence. + Robot task and manipulation contracts with stop evidence. wangfeiyu Proprietary ament_cmake diff --git a/ros2/navigation_interfaces/CMakeLists.txt b/ros2/navigation_interfaces/CMakeLists.txt new file mode 100644 index 0000000..aa90b23 --- /dev/null +++ b/ros2/navigation_interfaces/CMakeLists.txt @@ -0,0 +1,12 @@ +cmake_minimum_required(VERSION 3.8) +project(navigation_interfaces) +find_package(ament_cmake REQUIRED) +find_package(rosidl_default_generators REQUIRED) +find_package(builtin_interfaces REQUIRED) +find_package(geometry_msgs REQUIRED) +rosidl_generate_interfaces(${PROJECT_NAME} + "action/NavigateToPose.action" + DEPENDENCIES builtin_interfaces geometry_msgs +) +ament_export_dependencies(rosidl_default_runtime) +ament_package() diff --git a/ros2/bt_skill_interfaces/action/Navigate.action b/ros2/navigation_interfaces/action/NavigateToPose.action similarity index 57% rename from ros2/bt_skill_interfaces/action/Navigate.action rename to ros2/navigation_interfaces/action/NavigateToPose.action index a7f9b8b..c7c4c13 100644 --- a/ros2/bt_skill_interfaces/action/Navigate.action +++ b/ros2/navigation_interfaces/action/NavigateToPose.action @@ -1,30 +1,44 @@ -# 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 +string task_id +string subtask_id geometry_msgs/PoseStamped target_pose float64 position_tolerance float64 yaw_tolerance builtin_interfaces/Duration timeout --- -bt_skill_interfaces/NavigationResult result +uint8 SUCCEEDED=0 +uint8 CANCELED=1 +uint8 TIMEOUT=2 +uint8 BLOCKED=3 +uint8 NOT_READY=4 +uint8 FAILED=5 +uint8 STOP_UNKNOWN=0 +uint8 STOP_CONFIRMED=1 +uint8 status +string error_code +string message bool final_pose_valid geometry_msgs/PoseStamped final_pose float64 final_position_error float64 final_yaw_error +uint8 stop_state +builtin_interfaces/Time stopped_at +string stop_evidence_ref --- uint8 ACCEPTED=0 uint8 CHECKING=1 -uint8 NAVIGATING=2 -uint8 BLOCKED=3 -uint8 STOPPING=4 +uint8 PLANNING=2 +uint8 NAVIGATING=3 +uint8 BLOCKED=4 +uint8 STOPPING=5 builtin_interfaces/Time stamp -uint32 sequence +uint64 sequence uint8 phase bool current_pose_valid geometry_msgs/PoseStamped current_pose bool error_valid float64 position_error float64 yaw_error +bool blocked_valid bool blocked builtin_interfaces/Duration elapsed_time string message diff --git a/ros2/navigation_interfaces/package.xml b/ros2/navigation_interfaces/package.xml new file mode 100644 index 0000000..5b8d241 --- /dev/null +++ b/ros2/navigation_interfaces/package.xml @@ -0,0 +1,12 @@ + + + navigation_interfaces1.0.0 + Navigation action contract. + wangfeiyuProprietary + ament_cmake + rosidl_default_generators + builtin_interfacesgeometry_msgs + rosidl_default_runtime + rosidl_interface_packages + ament_cmake + diff --git a/tests/helpers/live_mock_smoke.py b/tests/helpers/live_mock_smoke.py index 0672d50..b87a064 100644 --- a/tests/helpers/live_mock_smoke.py +++ b/tests/helpers/live_mock_smoke.py @@ -33,9 +33,6 @@ def goal_for(name): goal.target_pose.header.frame_id = "map" goal.target_pose.pose.orientation.w = 1.0 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 elif name == "execute_manipulation": goal.skill, goal.instruction = "pick", "pick the smoke target" goal.target.object_ref, goal.target.description = "smoke-target", "smoke target" @@ -111,8 +108,8 @@ def main(): if not cancel.goals_canceling: raise RuntimeError("cancel was not acknowledged") canceled = await_future(handle.get_result_async()) - if (canceled.result.result.status != canceled.result.result.CANCELED or - canceled.result.result.stop_state != canceled.result.result.UNKNOWN): + if (canceled.result.status != canceled.result.CANCELED or + canceled.result.stop_state != canceled.result.STOP_UNKNOWN): raise RuntimeError("cancel terminal did not preserve unknown stop") state_client = client_node.create_client(GetRobotState, "get_robot_state") diff --git a/tests/helpers/native_coordinator_regression.py b/tests/helpers/native_coordinator_regression.py index a26a9f0..140e1ef 100644 --- a/tests/helpers/native_coordinator_regression.py +++ b/tests/helpers/native_coordinator_regression.py @@ -38,7 +38,7 @@ def run(root, binary, output): scenarios = { 'plan_task':[{'kind':'normal', 'plan':plan}], 'verify_state':[{'kind':'passed'}], - 'navigate_semantic':[{'kind':'normal','final_pose':site['locations'][name]} + 'navigate':[{'kind':'normal','final_pose':site['locations'][name]} for name in ['shelf_A_stop','tote_A_stop']*2], } actions = [name for name in ACTION_TYPES if name != 'execute_task'] @@ -78,7 +78,7 @@ def run(root, binary, output): def counts(): with server.lock: - return {name:server.counts[name] for name in ('plan_task','navigate_semantic','execute_manipulation','execute_posture')} + return {name:server.counts[name] for name in ('plan_task','navigate','execute_manipulation','execute_posture')} with tempfile.TemporaryDirectory(prefix='native-coordinator-regression-') as state: state = Path(state) diff --git a/tests/helpers/native_navigation_contract.py b/tests/helpers/native_navigation_contract.py index 6e378f8..6994c39 100644 --- a/tests/helpers/native_navigation_contract.py +++ b/tests/helpers/native_navigation_contract.py @@ -1,5 +1,5 @@ #!/usr/bin/env python3 -"""Generated Navigate messages + real DDS + production C++ RosDriver regression. +"""Generated NavigateToPose 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 @@ -30,13 +30,14 @@ find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_action REQUIRED) find_package(bt_skill_interfaces REQUIRED) +find_package(navigation_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) +ament_target_dependencies(native_navigation_probe rclcpp rclcpp_action bt_skill_interfaces navigation_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', @@ -60,21 +61,23 @@ def run(binary, output): 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 navigation_interfaces.action import NavigateToPose as Navigate from action_msgs.msg import GoalStatus - assert [getattr(NavigationResult, name) for name in + assert [getattr(Navigate.Result, 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)) + ('ACCEPTED', 'CHECKING', 'PLANNING', 'NAVIGATING', 'BLOCKED', 'STOPPING')] == list(range(6)) assert set(Navigate.Goal.get_fields_and_field_types()) == { - 'trace', 'target_pose', 'position_tolerance', 'yaw_tolerance', 'timeout'} + 'task_id', 'subtask_id', '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'} + 'status', 'error_code', 'message', 'final_pose_valid', 'final_pose', + 'final_position_error', 'final_yaw_error', 'stop_state', 'stopped_at', 'stop_evidence_ref'} 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'} + 'position_error', 'yaw_error', 'blocked_valid', 'blocked', 'elapsed_time', 'message'} + assert Navigate.Feedback.get_fields_and_field_types()['sequence'] == 'uint64' + assert (Navigate.Result.STOP_UNKNOWN, Navigate.Result.STOP_CONFIRMED) == (0, 1) namespace = '/sim/navigation_contract_' + str(os.getpid()) rclpy.init() node = Node('navigation_contract_fixture', namespace=namespace) @@ -85,51 +88,51 @@ def run(binary, output): 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] + status = scenario if scenario < 6 else {6: 1, 7: 3, 8: 4, 9: 1, 10: 4}[scenario] with lock: - counts[(goal.trace.task_id, scenario)] += 1 + counts[(goal.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 + assert goal.task_id and goal.subtask_id == 'navigate' except AssertionError: failures.append('Goal contract mismatch: ' + str(goal)) - for phase in range(5): + for phase in range(6): feedback = Navigate.Feedback() feedback.stamp = node.get_clock().now().to_msg() - feedback.sequence = phase + 1 + feedback.sequence = (1 << 32) + 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_valid = phase != Navigate.Feedback.STOPPING 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-'): + if phase == 2 and goal.task_id.startswith('probe-'): # A later malformed sample must not poison the accepted sequence. wrong_frame = deepcopy(feedback) - wrong_frame.sequence = 100 + wrong_frame.sequence = (1 << 32) + 100 wrong_frame.current_pose.header.frame_id = 'odom' handle.publish_feedback(wrong_frame) wrong_phase = deepcopy(feedback) - wrong_phase.sequence = 101 + wrong_phase.sequence = (1 << 32) + 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.status = status + result.error_code = '' if scenario in (7, 8) else 'EXECUTION_BACKEND_NOT_CONFIGURED' if scenario == 10 else 'FIXTURE_' + str(scenario) + result.message = 'outcome-' + str(scenario) + result.stop_state = 0 if scenario in (6, 10) else 1 + result.stopped_at = node.get_clock().now().to_msg() + result.stop_evidence_ref = '' if scenario in (6, 9, 10) else 'sim://navigation/stop' + result.final_pose_valid = scenario != 10 result.final_pose = goal.target_pose result.final_position_error = .02 result.final_yaw_error = -.03 @@ -156,16 +159,14 @@ def run(binary, output): 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', + report = {'scope': 'simulation-only generated NavigateToPose and production RosDriver over DDS', 'namespace': namespace, 'direct': [], 'native': []} try: assert client.wait_for_server(timeout_sec=10) - for scenario in range(9): + for scenario in range(11): 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.task_id = 'direct-' + str(scenario) + goal.subtask_id = 'navigate' goal.target_pose.header.frame_id = 'map' goal.target_pose.pose.position.x = float(scenario) goal.target_pose.pose.orientation.w = 1. @@ -175,25 +176,26 @@ def run(binary, output): feedback = [] handle = wait(client.send_goal_async(goal, feedback_callback=lambda value: feedback.append(value.feedback))) assert handle.accepted - if scenario in (1, 6): + if scenario in (1, 6, 9): 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 + expected = scenario if scenario < 6 else {6: 1, 7: 3, 8: 4, 9: 1, 10: 4}[scenario] + assert response.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_valid == (scenario != 10) 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 response.result.stop_state == (0 if scenario in (6, 10) else 1) + assert response.result.error_code == ('' if scenario in (7, 8) else 'EXECUTION_BACKEND_NOT_CONFIGURED' if scenario == 10 else 'FIXTURE_' + str(scenario)) + assert [f.phase for f in feedback] == list(range(6)), feedback + assert [f.sequence for f in feedback] == [(1 << 32) + n for n in range(1, 7)] 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 + assert feedback[4].blocked_valid and feedback[4].blocked + assert not feedback[5].blocked_valid and not feedback[5].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)): + for scenario, expected_code in enumerate((0, 2, 3, 1, 4, 1, 2, 1, 4, 2, 4)): state = output / ('state-' + str(scenario)) state.mkdir(exist_ok=True) completed = subprocess.run([str(binary), str(scenario), str(state), namespace], @@ -208,19 +210,22 @@ def run(binary, output): 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['stop'] == (0 if scenario in (6, 9, 10) else 1), result + assert result['state'] == (3 if scenario in (6, 9, 10) else 4), result + assert result['robot_locked'] == (scenario in (6, 9, 10)), result + assert result['unknown_stop_blocks_redispatch'] == (scenario in (6, 9, 10)), result + assert result['error_code'] == ({7: 'NAV_BLOCKED', 8: 'NAV_NOT_READY', 10: 'EXECUTION_BACKEND_NOT_CONFIGURED'}.get(scenario, 'FIXTURE_' + str(scenario))), result + assert result['feedback_sequence'] == (1 << 32) + 6, result + assert json.loads(result['feedback'])['phase'] == 5 + assert json.loads(result['feedback'])['blocked_valid'] is False, result + assert json.loads(result['feedback'])['blocked'] is None, 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 + assert result['wire_request_type'] == 'navigation_interfaces/action/NavigateToPose_Goal', result + assert result['wire_result_type'] == 'navigation_interfaces/action/NavigateToPose_Result', result + assert result['response_valid'] == (scenario != 10), result report['native'].append(result) assert not failures, failures - assert len(counts) == 18 and all(count == 1 for count in counts.values()), dict(counts) + assert len(counts) == 22 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') diff --git a/tests/helpers/native_workflow_regression.py b/tests/helpers/native_workflow_regression.py index 69111b5..8ac9679 100644 --- a/tests/helpers/native_workflow_regression.py +++ b/tests/helpers/native_workflow_regression.py @@ -94,6 +94,7 @@ def run_route(root, binary, output, suffix, recovery_enabled): from rclpy.node import Node from rclpy.serialization import deserialize_message from bt_skill_interfaces.action import ExecuteTask, ExecuteManipulation + from navigation_interfaces.action import NavigateToPose from bt_skill_interfaces.msg import RobotState from bt_skill_interfaces.srv import ReconcileTask from robot_bt_coordinator.plan_v2 import item_plan @@ -105,7 +106,7 @@ def run_route(root, binary, output, suffix, recovery_enabled): stops = ['shelf_A_stop', 'tote_A_stop'] if suffix == 'object_table' else ['observe_A', 'shelf_A_stop', 'tote_A_stop'] scenarios = {'verify_state': [{'kind': 'passed'}], 'locate_shelf_column': [{'kind': 'normal', 'shelf_id': 'shelf_A', 'side_id': 'FRONT', 'column_id': '1', 'tier_id': '2'}], - 'navigate_semantic': [{'kind': 'normal', 'final_pose': site['locations'][name]} for name in stops]} + 'navigate': [{'kind': 'normal', 'final_pose': site['locations'][name]} for name in stops]} actions = [name for name in ACTION_TYPES if name != 'execute_task'] rclpy.init(args=['--ros-args', '-p', 'initial_holding_state:=EMPTY', '-p', 'scenarios_json:=' + json.dumps(json.dumps(scenarios)), @@ -178,7 +179,7 @@ def run_route(root, binary, output, suffix, recovery_enabled): def motion_counts(): with server.lock: - return {name: server.counts[name] for name in ('navigate_semantic', 'execute_manipulation', 'execute_posture')} + return {name: server.counts[name] for name in ('navigate', 'execute_manipulation', 'execute_posture')} try: start() @@ -195,6 +196,23 @@ def run_route(root, binary, output, suffix, recovery_enabled): row['decoded_manipulation'] = assert_manipulation_evidence( journal_records(Path(journal) / 'goal_registry.log'), goal.trace.run_id, ExecuteManipulation, deserialize_message) + navigation_rows = [entry for entry in journal_records(Path(journal) / 'goal_registry.log') + if entry['run_id'] == goal.trace.run_id and entry['skill'] == 0] + assert len(navigation_rows) == len(stops) + expected_poses = {(site['locations'][name]['x'], site['locations'][name]['y']) for name in stops} + actual_poses = set() + for entry in navigation_rows: + assert entry['type'] == 'navigation_interfaces/action/NavigateToPose_Goal' + navigation_goal = deserialize_message(bytes.fromhex(entry['wire']), NavigateToPose.Goal) + assert navigation_goal.task_id == goal.trace.task_id and navigation_goal.subtask_id + assert navigation_goal.target_pose.header.frame_id == 'map' + actual_poses.add((navigation_goal.target_pose.pose.position.x, navigation_goal.target_pose.pose.position.y)) + assert entry['result_type'] == 'navigation_interfaces/action/NavigateToPose_Result' + navigation_result = deserialize_message(bytes.fromhex(entry['result_wire']), NavigateToPose.Result) + assert navigation_result.status == NavigateToPose.Result.SUCCEEDED + assert navigation_result.stop_state == NavigateToPose.Result.STOP_CONFIRMED + assert actual_poses == expected_poses + row['direct_navigation_goals'] = len(navigation_rows) assert motion_counts()['execute_manipulation'] == 2 if recovery_enabled: stop() @@ -228,11 +246,11 @@ def run_route(root, binary, output, suffix, recovery_enabled): unknown_holding=unknown.error_code, recovered=state) if suffix == 'object_table': with server.lock: - server.scenarios['navigate_semantic'] = [{'kind': 'normal', 'duration_seconds': 3., 'final_pose': site['locations']['shelf_A_stop']}] + server.scenarios['navigate'] = [{'kind': 'normal', 'duration_seconds': 3., 'final_pose': site['locations']['shelf_A_stop']}] canceled_goal = task_goal('cancel') canceled_handle = wait(client.send_goal_async(canceled_goal), 8) assert canceled_handle.accepted - eventually(lambda: motion_counts()['navigate_semantic'] > before['navigate_semantic']) + eventually(lambda: motion_counts()['navigate'] > before['navigate']) wait(canceled_handle.cancel_goal_async(), 5) canceled_result = wait(canceled_handle.get_result_async(), 15) assert canceled_result.status == 5 and canceled_result.result.result.stop_state == 1 @@ -267,7 +285,7 @@ def run_route(root, binary, output, suffix, recovery_enabled): if suffix == 'shelf_cell': with server.lock: server.scenarios['locate_shelf_column'] = [{'kind': 'ambiguous'}] - server.scenarios['navigate_semantic'] = [{'kind': 'normal', 'final_pose': site['locations']['observe_A']}] + server.scenarios['navigate'] = [{'kind': 'normal', 'final_pose': site['locations']['observe_A']}] before_ask = motion_counts() ask_handle = wait(client.send_goal_async(task_goal('ambiguous')), 8) assert ask_handle.accepted diff --git a/tests/helpers/ros_shim.py b/tests/helpers/ros_shim.py index cf565cf..2d0e4b7 100644 --- a/tests/helpers/ros_shim.py +++ b/tests/helpers/ros_shim.py @@ -81,14 +81,16 @@ def _make_class(name, lines): if kwargs: raise TypeError("unexpected fields: " + ", ".join(kwargs)) - attrs = {"__slots__": slots, "__init__": init, **constants} + attrs = {"__slots__": slots, "__init__": init, **constants, + "get_fields_and_field_types": classmethod(lambda cls: {field: kind for kind, field in fields})} return type(name, (), attrs) def install(): """Install deterministic rclpy and generated interface modules.""" + _registry.clear() for name in list(sys.modules): - if name == "rclpy" or name.startswith("rclpy.") or name.startswith("bt_skill_interfaces"): + if name == "rclpy" or name.startswith("rclpy.") or name.startswith(("bt_skill_interfaces", "navigation_interfaces")): del sys.modules[name] builtin = _module("builtin_interfaces") @@ -127,39 +129,41 @@ def install(): setattr(geometry_msg, name, cls) _registry[f"geometry_msgs/{name}"] = cls - package = _module("bt_skill_interfaces") - msg_module = _module("bt_skill_interfaces.msg") - package.msg = msg_module - pending = {p.stem: _fields(p) for p in (IDL / "msg").glob("*.msg")} - while pending: - progress = False - for name, lines in list(pending.items()): - deps = [line.split()[0].removesuffix("[]") for line in lines if "=" not in line] - if all(dep in _registry or dep in ("string", "bool") or dep.startswith(("uint", "int", "float")) for dep in deps): - cls = _make_class(name, lines) - setattr(msg_module, name, cls) - _registry[f"bt_skill_interfaces/{name}"] = cls - del pending[name] - progress = True - if not progress: - raise RuntimeError("unresolved IDL: " + repr(pending)) + for package_name in ("bt_skill_interfaces", "navigation_interfaces"): + package_idl = IDL.parent / package_name + package = _module(package_name) + msg_module = _module(f"{package_name}.msg") + package.msg = msg_module + pending = {p.stem: _fields(p) for p in (package_idl / "msg").glob("*.msg")} + while pending: + progress = False + for name, lines in list(pending.items()): + deps = [line.split()[0].removesuffix("[]") for line in lines if "=" not in line] + if all(dep in _registry or dep in ("string", "bool") or dep.startswith(("uint", "int", "float")) for dep in deps): + cls = _make_class(name, lines) + setattr(msg_module, name, cls) + _registry[f"{package_name}/{name}"] = cls + del pending[name] + progress = True + if not progress: + raise RuntimeError("unresolved IDL: " + repr(pending)) - action_module = _module("bt_skill_interfaces.action") - package.action = action_module - for path in (IDL / "action").glob("*.action"): - action = type(path.stem, (), {}) - action.Goal = _make_class("Goal", _fields(path, 0)) - action.Result = _make_class("Result", _fields(path, 1)) - action.Feedback = _make_class("Feedback", _fields(path, 2)) - setattr(action_module, path.stem, action) + action_module = _module(f"{package_name}.action") + package.action = action_module + for path in (package_idl / "action").glob("*.action"): + action = type(path.stem, (), {}) + action.Goal = _make_class("Goal", _fields(path, 0)) + action.Result = _make_class("Result", _fields(path, 1)) + action.Feedback = _make_class("Feedback", _fields(path, 2)) + setattr(action_module, path.stem, action) - srv_module = _module("bt_skill_interfaces.srv") - package.srv = srv_module - for path in (IDL / "srv").glob("*.srv"): - srv = type(path.stem, (), {}) - srv.Request = _make_class("Request", _fields(path, 0)) - srv.Response = _make_class("Response", _fields(path, 1)) - setattr(srv_module, path.stem, srv) + srv_module = _module(f"{package_name}.srv") + package.srv = srv_module + for path in (package_idl / "srv").glob("*.srv"): + srv = type(path.stem, (), {}) + srv.Request = _make_class("Request", _fields(path, 0)) + srv.Response = _make_class("Response", _fields(path, 1)) + setattr(srv_module, path.stem, srv) rclpy = _module("rclpy") rclpy.ok = lambda: True diff --git a/tests/test_dr_regressions.py b/tests/test_dr_regressions.py index cb7a593..6564982 100644 --- a/tests/test_dr_regressions.py +++ b/tests/test_dr_regressions.py @@ -1,7 +1,5 @@ """DR semantic regression tests independent of ROS transport.""" -import copy import json -import math import sys import tempfile import unittest @@ -23,13 +21,6 @@ class DrSemanticsTests(unittest.TestCase): for key in ('shelf_id','column_id','tier_id','station_binding_ref','expected_geometry_epoch'): self.assertEqual(result[key],getattr(goal,key)) - def test_semantic_navigation_accepts_positive_subsecond_budget(self): - from navigation_gateway.semantic_proxy import duration_seconds - from types import SimpleNamespace - self.assertAlmostEqual(duration_seconds(SimpleNamespace(sec=0,nanosec=500000000)),.5) - for sec,nanosec in [(0,0),(-1,1),(3600,1),(1,1000000000),(1,-1)]: - with self.assertRaises(ValueError):duration_seconds(SimpleNamespace(sec=sec,nanosec=nanosec)) - def test_shelf_tier_can_be_empty_without_guessing(self): # BT p15 and RB02 allow an unknown tier; execution must decide whether # its chosen route can use the observation without that calibration. @@ -48,17 +39,6 @@ class DrSemanticsTests(unittest.TestCase): self.assertEqual(result['status'], 'SUCCEEDED') self.assertEqual(result['tier_id'], '') - def test_navigation_reports_signed_shortest_yaw_error(self): - from navigation_gateway.gateway import pose_errors - from test_navigation_gateway import request - target = request()['target_pose'] - current = copy.deepcopy(target) - current['orientation'].update(z=math.sin(.2/2), w=math.cos(.2/2)) - distance, yaw = pose_errors(target, current) - self.assertEqual(distance, 0) - self.assertAlmostEqual(yaw, -.2) - current['orientation'].update(z=math.sin(-.2/2), w=math.cos(-.2/2)) - self.assertAlmostEqual(pose_errors(target, current)[1], .2) if __name__ == '__main__': diff --git a/tests/test_mock_runtime.py b/tests/test_mock_runtime.py index 296c486..f41ab6e 100644 --- a/tests/test_mock_runtime.py +++ b/tests/test_mock_runtime.py @@ -1,4 +1,5 @@ import json +import math import pathlib import sys import threading @@ -34,9 +35,6 @@ class MockRuntimeTests(unittest.TestCase): goal.target_pose.header.frame_id = "map" goal.target_pose.pose.orientation.w = 1.0 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 elif name == "execute_manipulation": goal.skill, goal.instruction = "pick", "pick bottle" goal.target.object_ref, goal.target.description = "bottle", "bottle" @@ -78,8 +76,8 @@ class MockRuntimeTests(unittest.TestCase): self.node._accepted(name, handle) return handle, self.node._execute(name, handle) - def test_all_twelve_actions_execute_success_and_failure(self): - self.assertEqual(len(self.module.ACTION_TYPES), 12) + def test_all_eleven_actions_execute_success_and_failure(self): + self.assertEqual(len(self.module.ACTION_TYPES), 11) self.assertEqual(self.module.ACTION_ENDPOINTS["plan_task"], "tasks/plan") self.assertEqual(self.module.ACTION_ENDPOINTS["execute_task"], "tasks/execute") self.assertEqual(self.module.ACTION_ENDPOINTS["evaluate_progress"], "monitor/evaluate_progress") @@ -89,7 +87,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.SUCCEEDED if name == "navigate" else result.result.COMPLETED) + self.assertEqual(result.result.status, result.result.COMPLETED) self.assertEqual(result.result.stop_state, result.result.CONFIRMED) elif hasattr(result, "status"): self.assertNotEqual(result.status, result.FAILED) @@ -118,7 +116,7 @@ class MockRuntimeTests(unittest.TestCase): def test_success_feedback_follows_normal_lifecycle_and_populates_fields(self): cases = { - "navigate": [0, 1, 2], + "navigate": [0, 1, 2, 3], "execute_manipulation": [0, 1, 2, 3, 4], "execute_posture": [0, 1, 2], } @@ -131,7 +129,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.current_pose_valid and nav.error_valid) + self.assertTrue(nav.current_pose_valid and nav.error_valid and nav.blocked_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,11 +137,11 @@ class MockRuntimeTests(unittest.TestCase): def test_each_action_feedback_lifecycle_and_navigation_recovery_execute(self): expected = { - "navigate": [0, 1, 2], "execute_manipulation": [0, 1, 2, 3, 4], + "navigate": [0, 1, 2, 3], "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], - "navigate_semantic": [0], "evaluate_progress": [0], + "evaluate_progress": [0], } for name, phases in expected.items(): self.node.counts[name] = 0 @@ -153,8 +151,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, 2]) - self.assertEqual([item.blocked for item in recovery.feedback], [False, False, False, True, False]) + self.assertEqual([item.phase for item in recovery.feedback], [0, 1, 2, 3, 4, 3]) + self.assertEqual([item.blocked for item in recovery.feedback], [False, 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) @@ -173,22 +171,22 @@ class MockRuntimeTests(unittest.TestCase): self.assertTrue(thread.is_alive(), "cancel ACK must precede delayed stop termination") thread.join(1) result = box["result"] - self.assertEqual((handle.native, result.result.status, result.result.stop_state), - ("canceled", result.result.CANCELED, result.result.CONFIRMED)) - self.assertEqual(handle.feedback[-1].phase, self.module.Navigate.Feedback.STOPPING) + self.assertEqual((handle.native, result.status, result.stop_state), + ("canceled", result.CANCELED, result.STOP_CONFIRMED)) + self.assertEqual(handle.feedback[-1].phase, self.module.NavigateToPose.Feedback.STOPPING) self.node.motion_reserved = False; self.node.counts["navigate"] = 0 goal = self.goal("navigate"); goal.timeout.sec = 0; goal.timeout.nanosec = 1_000_000 self.node.scenarios["navigate"] = [{"kind": "timeout"}] 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.TIMEOUT)) - self.assertEqual(timeout_handle.feedback[-1].phase, self.module.Navigate.Feedback.STOPPING) + self.assertEqual((timeout_handle.native, timeout_result.status), ("aborted", timeout_result.TIMEOUT)) + self.assertEqual(timeout_handle.feedback[-1].phase, self.module.NavigateToPose.Feedback.STOPPING) self.node.motion_reserved = False; self.node.counts["navigate"] = 0 unknown_handle, unknown = self.execute("navigate", {"kind": "stop_unknown", "duration_seconds": 0}) - self.assertEqual(unknown.result.stop_state, unknown.result.UNKNOWN) + self.assertEqual(unknown.stop_state, unknown.STOP_UNKNOWN) self.assertTrue(self.node.motion_reserved) - self.assertEqual(unknown.result.stop_evidence_ref, "") + self.assertEqual(unknown.stop_evidence_ref, "") def test_cancel_and_timeout_can_acknowledge_without_confirming_stop(self): handle, canceled = self.execute( @@ -203,7 +201,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) timed_out = self.node._execute("navigate", timeout_handle) - self.assertEqual(timed_out.result.stop_state, timed_out.result.UNKNOWN) + self.assertEqual(timed_out.stop_state, timed_out.STOP_UNKNOWN) self.assertTrue(self.node.motion_reserved) def test_navigation_status_mapping_and_readiness_reasons(self): @@ -211,7 +209,7 @@ class MockRuntimeTests(unittest.TestCase): ("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", + ("INPUTS_UNHEALTHY", "ROBOT_STATE_UNAVAILABLE", "EXECUTION_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 @@ -220,17 +218,17 @@ class MockRuntimeTests(unittest.TestCase): 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.status, handle.native), (status, native)) + self.assertEqual(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, "")) + self.assertEqual((result.status, result.stop_state), (5, 0)) + self.assertEqual((result.stopped_at.sec, result.stopped_at.nanosec, + result.stop_evidence_ref), (0, 0, "")) def test_navigation_goal_requires_map_and_finite_nonzero_quaternion(self): mutations = ( @@ -256,7 +254,7 @@ class MockRuntimeTests(unittest.TestCase): 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.assertEqual(result.status, 0) self.assertTrue(result.final_pose_valid) self.assertEqual(result.final_pose.pose.orientation.w, 1.0) self.assertTrue(handle.feedback) @@ -266,7 +264,6 @@ class MockRuntimeTests(unittest.TestCase): def test_action_specific_malformed_goals_are_rejected(self): mutations = { "navigate": lambda g: setattr(g.target_pose.header, "frame_id", ""), - "navigate_semantic": lambda g: setattr(g, "registry_version", 0), "execute_manipulation": lambda g: setattr(g.target, "object_ref", ""), "execute_posture": lambda g: setattr(g, "expected_geometry_epoch", 0), "plan_task": lambda g: setattr(g, "known_info_json", "[]"), @@ -283,56 +280,65 @@ class MockRuntimeTests(unittest.TestCase): with self.subTest(action=name): self.assertEqual(self.node._goal(name, goal), self.module.GoalResponse.REJECT) - def test_semantic_navigation_accepts_production_kind_matrix_only(self): - accepted = ( - ("LOCATION", "destination_A", {}), - ("OBJECT", "bottle", {}), - ("CELL", "bottle", {"shelf_id": "shelf_A", "side_id": "FRONT", - "column_id": "1", "tier_id": "2"}), - ) - for kind, reference, fields in accepted: - goal = self.goal("navigate_semantic") - goal.kind, goal.reference = kind, reference - for field, value in fields.items(): setattr(goal, field, value) - with self.subTest(kind=kind): - self.assertEqual(self.node._goal("navigate_semantic", goal), self.module.GoalResponse.ACCEPT) - self.node.inflight, self.node.motion_reserved = 0, False - cell = self.goal("navigate_semantic"); cell.kind = "CELL" - self.assertEqual(self.node._goal("navigate_semantic", cell), self.module.GoalResponse.REJECT) - for invented in ("region", "shelf", "station"): - goal = self.goal("navigate_semantic"); goal.kind = invented - with self.subTest(invented=invented): - self.assertEqual(self.node._goal("navigate_semantic", goal), self.module.GoalResponse.REJECT) - - def test_semantic_navigation_pose_is_explicit_and_validated(self): + def test_navigation_explicit_pose_preserves_measurements_and_epoch(self): pose = {"frame_id": "map", "x": 1.25, "y": -2.5, "z": 0.0, "qx": 0.0, "qy": 0.0, "qz": 0.0, "qw": 1.0} - _, explicit = self.execute("navigate_semantic", {"kind": "normal", "duration_seconds": 0, - "final_pose": pose}) - self.assertTrue(explicit.pose_valid and explicit.errors_valid) - self.assertEqual(explicit.final_pose.header.frame_id, "map") - self.assertEqual((explicit.final_pose.pose.position.x, explicit.final_pose.pose.position.y), (1.25, -2.5)) - self.node.counts["navigate_semantic"] = 0 - _, unspecified = self.execute("navigate_semantic", {"kind": "normal", "duration_seconds": 0}) - self.assertFalse(unspecified.pose_valid) - self.assertFalse(unspecified.errors_valid) - for raw in ( - '{"navigate_semantic":{"final_pose":{"frame_id":"map","x":NaN,"y":0,"z":0,"qx":0,"qy":0,"qz":0,"qw":1}}}', - '{"navigate_semantic":{"final_pose":{"frame_id":"map","x":1,"y":2,"z":0,"qx":0,"qy":0,"qz":0,"qw":0}}}', - ): - with self.subTest(raw=raw), self.assertRaises(ValueError): - self.module.parse_scenarios(raw) + fixture = {"kind": "normal", "duration_seconds": 0, "final_pose": pose} + self.module.parse_scenarios(json.dumps({"navigate": fixture})) + before = self.node.geometry_epoch + _, result = self.execute("navigate", fixture) + self.assertEqual(result.status, result.SUCCEEDED) + self.assertTrue(result.final_pose_valid) + self.assertEqual((result.final_pose.pose.position.x, result.final_pose.pose.position.y), (1.25, -2.5)) + self.assertGreater(result.final_position_error, 2.5) + self.assertEqual(result.final_yaw_error, 0.0) + self.assertEqual(self.node.geometry_epoch, before + 1) + self.assertEqual(result.stop_state, result.STOP_CONFIRMED) + self.assertTrue(result.stop_evidence_ref) + self.assertFalse(hasattr(result, "result")) + for invalid in (dict(pose, x=float("nan")), dict(pose, qw=0.0), dict(pose, frame_id="odom")): + with self.subTest(pose=invalid), self.assertRaises(ValueError): + self.module.parse_scenarios(json.dumps({"navigate": {"final_pose": invalid}})) + + def test_navigation_final_yaw_error_is_signed_shortest_target_minus_current(self): + for target_deg, current_deg, expected_deg in ((30, 10, 20), (10, 30, -20), + (-170, 170, 20), (170, -170, -20)): + with self.subTest(target=target_deg, current=current_deg): + target, current = math.radians(target_deg), math.radians(current_deg) + goal = self.goal("navigate") + goal.target_pose.pose.orientation.z = math.sin(target / 2) + goal.target_pose.pose.orientation.w = math.cos(target / 2) + fixture = {"kind": "normal", "duration_seconds": 0, "final_pose": { + "frame_id": "map", "x": 0.0, "y": 0.0, "z": 0.0, + "qx": 0.0, "qy": 0.0, "qz": math.sin(current / 2), "qw": math.cos(current / 2)}} + self.module.parse_scenarios(json.dumps({"navigate": fixture})) + self.node.scenarios["navigate"] = [fixture] + 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.status, result.SUCCEEDED) + self.assertAlmostEqual(result.final_yaw_error, math.radians(expected_deg), places=12) + + def test_navigation_goal_has_native_identity_without_trace(self): + goal = self.goal("navigate") + self.assertFalse(hasattr(goal, "trace")) + for field in ("task_id", "subtask_id"): + invalid = self.goal("navigate") + setattr(invalid, field, "") + with self.subTest(field=field): + self.assertEqual(self.node._goal("navigate", invalid), self.module.GoalResponse.REJECT) def test_enabled_interfaces_can_exclude_executor_owned_endpoints(self): module = load_mock_module({ "scenarios_json": "{}", "max_goal_seconds": 1.0, "allowed_postures": ["pregrasp", "transport", "home"], "initial_holding_state": "UNKNOWN", - "enabled_actions": ["plan_task", "navigate_semantic"], + "enabled_actions": ["plan_task", "navigate"], "enabled_topics": ["robot_state"], }) node = module.MockSkills() - self.assertEqual(node.enabled_actions, ("plan_task", "navigate_semantic")) + self.assertEqual(node.enabled_actions, ("plan_task", "navigate")) self.assertEqual(len(node.servers), 2) self.assertTrue(hasattr(node, "state_pub")) self.assertFalse(hasattr(node, "registry_pub")) @@ -352,7 +358,7 @@ class MockRuntimeTests(unittest.TestCase): self.assertEqual(missing.error_code, "UNKNOWN_ROBOT") handle, unresolved = self.execute("navigate", {"kind": "stop_unknown", "duration_seconds": 0}) goal_id = bytes(handle.goal_id.uuid).hex() - self.assertEqual(unresolved.result.stop_state, unresolved.result.UNKNOWN) + self.assertEqual(unresolved.stop_state, unresolved.STOP_UNKNOWN) req = srv.ReconcileGoal.Request(trace=self.trace(), goal_id="never-accepted", operator_id="op", reason="review") req.evidence.status = req.evidence.PASSED req.evidence.context.source_goal_id = "never-accepted" @@ -367,6 +373,11 @@ class MockRuntimeTests(unittest.TestCase): self.assertFalse(rejected.accepted) self.assertTrue(self.node.motion_reserved) req.goal_id = req.evidence.context.source_goal_id = goal_id + req.trace.subtask_id = req.evidence.context.trace.subtask_id = "other" + wrong_identity = self.node._reconcile(req, srv.ReconcileGoal.Response()) + self.assertFalse(wrong_identity.accepted) + self.assertTrue(self.node.motion_reserved) + req.trace.subtask_id = req.evidence.context.trace.subtask_id = "s" accepted = self.node._reconcile(req, srv.ReconcileGoal.Response()) self.assertTrue(accepted.accepted) self.assertFalse(self.node.motion_reserved) @@ -445,7 +456,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.SUCCEEDED) + self.assertEqual(result.status, 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 deleted file mode 100644 index a850401..0000000 --- a/tests/test_navigation_catalog.py +++ /dev/null @@ -1,42 +0,0 @@ -import unittest -class CatalogTests(unittest.TestCase): - def test_names_cells_and_version_are_exact(self): - from navigation_gateway.catalog import Catalog - site={'registry_version':3,'locations':{'stop':{'frame_id':'map','x':1,'y':2,'z':0,'qx':0,'qy':0,'qz':0,'qw':1}},'object_locations':{'water':'stop'},'cell_locations':{'s/FRONT/1/2':'stop'}} - c=Catalog(site) - self.assertEqual(c.resolve('OBJECT','water',3)['pose']['x'],1) - 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 deleted file mode 100644 index 074856e..0000000 --- a/tests/test_navigation_gateway.py +++ /dev/null @@ -1,646 +0,0 @@ -"""Safety contract tests; no ROS installation or robot required.""" -import copy -import json -import sys -import tempfile -import threading -import unittest -import urllib.error -import urllib.request -from pathlib import Path - -PROJECT_ROOT = Path(__file__).resolve().parents[1] -if str(PROJECT_ROOT) not in sys.path: - sys.path.insert(0, str(PROJECT_ROOT)) - -try: - from navigation_gateway.gateway import Gateway, GatewayError, SafetyConfig, validate_goal - from navigation_gateway.backends import MockBackend - from navigation_gateway.server import make_server - IMPORT_ERROR = None -except ImportError as exc: - IMPORT_ERROR = str(exc) - - -class Clock: - def __init__(self): self.now = 100.0 - def __call__(self): return self.now - def advance(self, seconds): self.now += seconds - - -def request(goal_id="11111111-1111-4111-8111-111111111111"): - return {"goal_id": goal_id, "trace": {"task_id": "task-1", "subtask_id": "navigate", "attempt": 1}, - "map_id": "sim-map", "target_pose": {"frame_id": "map", "position": {"x": 1.0, "y": 2.0, "z": 0.0}, - "orientation": {"x": 0.0, "y": 0.0, "z": 0.0, "w": 1.0}}, - "position_tolerance": 0.05, "yaw_tolerance": 0.05, "timeout_sec": 10.0} - - -class GatewayTests(unittest.TestCase): - def setUp(self): - self.assertIsNone(IMPORT_ERROR, "navigation gateway implementation missing: " + str(IMPORT_ERROR)) - self.tmp = tempfile.TemporaryDirectory() - self.clock = Clock() - self.config = SafetyConfig(odom_max_age_sec=0.2, stationary_window_sec=0.3, - linear_stopped_mps=0.005, angular_stopped_radps=0.005, pose_max_age_sec=0.2, - readiness_max_age_sec=0.5, stop_wait_timeout_sec=1.0) - self.backend = MockBackend(self.clock, map_id="sim-map", source_clock=lambda: 1700000000.0 + self.clock()) - self.path = str(Path(self.tmp.name) / "journal.sqlite3") - self.gateway = Gateway(self.path, self.backend, self.config, self.clock) - self.addCleanup(self.tmp.cleanup) - self.addCleanup(lambda: self.gateway.close()) - - def sample(self, state="ACTIVE", linear=0.0, pose=None, fresh=True): - self.backend.health_sample(True) - self.backend.set_snapshot(request()["goal_id"], state, linear=linear, angular=0.0, - pose=pose or request()["target_pose"], source_fresh=fresh) - return self.gateway.poll(request()["goal_id"]) - - def stop_window(self, state="SUCCEEDED", pose=None): - out = self.sample(state, pose=pose) - for _ in range(4): - self.clock.advance(0.1) - out = self.sample(state, pose=pose) - return out - - def test_goal_is_sent_once_and_replay_is_immutable(self): - first = self.gateway.submit(request()) - self.assertEqual(first["goal_id"], self.gateway.submit(copy.deepcopy(request()))["goal_id"]) - self.assertEqual(self.backend.send_count, 1) - changed = request(); changed["timeout_sec"] = 8 - with self.assertRaises(GatewayError) as cm: self.gateway.submit(changed) - self.assertEqual(cm.exception.http_status, 409) - self.assertEqual(self.backend.send_count, 1) - - def test_goal_validation_rejects_nonfinite_bad_quaternion_and_duration(self): - for path, value in [(('position_tolerance',), float('nan')), (('yaw_tolerance',), -1), - (('timeout_sec',), float('inf')), (('timeout_sec',), 0), - (('target_pose', 'position', 'x'), float('inf')), - (('target_pose', 'orientation', 'w'), 0), (('target_pose', 'frame_id'), 'odom')]: - body = request(); node = body - for key in path[:-1]: node = node[key] - node[path[-1]] = value - with self.subTest(path=path, value=value), self.assertRaises(GatewayError): validate_goal(body) - - def test_readiness_cannot_be_claimed_by_http_caller(self): - self.backend.health_sample(False) - with self.assertRaises(GatewayError): self.gateway.submit(request()) - self.assertEqual(self.backend.send_count, 0) - body = request(); body["ready"] = True - with self.assertRaises(GatewayError): self.gateway.submit(body) - - def test_stale_health_and_wrong_map_reject_before_sending(self): - self.clock.advance(1) - with self.assertRaises(GatewayError): self.gateway.submit(request()) - self.backend.health_sample(True) - body = request(); body["map_id"] = "another-map" - with self.assertRaises(GatewayError): self.gateway.submit(body) - self.assertEqual(self.backend.send_count, 0) - - def test_cancel_ack_is_not_stop_confirmation_and_cancel_is_idempotent(self): - self.gateway.submit(request()) - out = self.gateway.cancel(request()["goal_id"]) - self.assertEqual(out["stop_state"], "UNKNOWN") - self.gateway.cancel(request()["goal_id"]) - self.assertEqual(self.backend.cancel_count, 1) - self.assertEqual(self.stop_window("ACTIVE")["stop_state"], "UNKNOWN") - stopped = self.stop_window("PREEMPTED") - self.assertEqual((stopped["outcome"], stopped["stop_state"]), ("CANCELED", "CONFIRMED")) - - def test_success_requires_terminal_pose_and_continuous_fresh_odom(self): - self.gateway.submit(request()) - out = self.sample("SUCCEEDED") - self.assertEqual(out["stop_state"], "UNKNOWN") - self.clock.advance(0.5) - self.assertEqual(self.gateway.poll(request()["goal_id"])["stop_state"], "UNKNOWN") - out = self.stop_window() - self.assertEqual((out["outcome"], out["stop_state"]), ("COMPLETED", "CONFIRMED")) - - def test_stale_source_odom_never_confirms_stop(self): - self.gateway.submit(request()) - for _ in range(10): - self.clock.advance(0.1) - out = self.sample("SUCCEEDED", fresh=False) - self.assertEqual(out["stop_state"], "UNKNOWN") - - def test_moving_sample_resets_stationary_window(self): - self.gateway.submit(request()); self.sample("SUCCEEDED") - self.clock.advance(0.15); self.sample("SUCCEEDED", linear=0.1) - self.clock.advance(0.15); self.sample("SUCCEEDED") - self.clock.advance(0.15) - self.assertEqual(self.sample("SUCCEEDED")["stop_state"], "UNKNOWN") - - def test_odom_samples_between_polls_cannot_hide_motion(self): - self.gateway.submit(request()); self.sample("SUCCEEDED") - for _ in range(2): - self.clock.advance(0.1) - self.sample("SUCCEEDED") - self.clock.advance(0.05) - self.backend.set_snapshot(request()["goal_id"], "SUCCEEDED", linear=0.1, pose=request()["target_pose"]) - self.clock.advance(0.1) - self.backend.set_snapshot(request()["goal_id"], "SUCCEEDED", linear=0.0, pose=request()["target_pose"]) - out = self.gateway.poll(request()["goal_id"]) - self.assertEqual(out["stop_state"], "UNKNOWN") - - def test_readiness_loss_during_motion_requests_stop(self): - self.gateway.submit(request()) - self.backend.health_sample(False) - out = self.gateway.poll(request()["goal_id"]) - self.assertEqual(self.backend.cancel_count, 1) - self.assertEqual(out["stop_state"], "UNKNOWN") - out = self.stop_window("PREEMPTED") - self.assertEqual(out["outcome"], "FAILED") - - def test_success_with_health_lost_does_not_complete(self): - self.gateway.submit(request()); self.sample("SUCCEEDED") - self.backend.health_sample(False) - self.gateway.poll(request()["goal_id"]) - out = self.stop_window("SUCCEEDED") - self.assertEqual(out["outcome"], "FAILED") - - def test_success_outside_tolerance_becomes_failed_not_completed(self): - self.gateway.submit(request()) - pose = copy.deepcopy(request()["target_pose"]); pose["position"]["x"] = 2.0 - out = self.stop_window(pose=pose) - self.assertEqual((out["outcome"], out["stop_state"]), ("FAILED", "CONFIRMED")) - self.assertIn("tolerance", out["message"]) - - def test_execution_timeout_cancels_once_and_waits_for_actual_stop(self): - body = request(); body["timeout_sec"] = 0.2 - self.gateway.submit(body); self.clock.advance(0.3) - out = self.gateway.poll(body["goal_id"]) - self.assertEqual(out["stop_state"], "UNKNOWN") - self.assertEqual(self.backend.cancel_count, 1) - out = self.stop_window("PREEMPTED") - self.assertEqual(out["outcome"], "TIMED_OUT") - - def test_ambiguous_send_is_quarantined_and_never_retried(self): - self.backend.send_mode = "UNKNOWN" - out = self.gateway.submit(request()) - self.assertEqual(out["status"], "STOP_UNKNOWN") - self.gateway.submit(request()) - with self.assertRaises(GatewayError): self.gateway.submit(request("22222222-2222-4222-8222-222222222222")) - self.assertEqual(self.backend.send_count, 1) - self.assertEqual(self.stop_window()["stop_state"], "UNKNOWN") - self.assertEqual(self.backend.cancel_count, 1) - - def test_controller_state_loss_requests_cancel_and_keeps_lock(self): - self.gateway.submit(request()) - self.backend.set_snapshot(request()["goal_id"], "LOST", pose=request()["target_pose"]) - out = self.gateway.poll(request()["goal_id"]) - self.assertEqual(self.backend.cancel_count, 1) - self.assertEqual(out["stop_state"], "UNKNOWN") - with self.assertRaises(GatewayError): self.gateway.submit(request("22222222-2222-4222-8222-222222222222")) - - def test_second_process_cannot_open_the_same_robot_journal(self): - with self.assertRaises(RuntimeError): Gateway(self.path, self.backend, self.config, self.clock) - - def test_completed_record_survives_restart_without_resending(self): - self.gateway.submit(request()); expected = self.stop_window() - self.gateway.close() - self.clock.advance(100) - self.gateway = Gateway(self.path, self.backend, self.config, self.clock) - actual = self.gateway.submit(request()) - self.assertEqual(actual, expected) - self.assertEqual(self.backend.send_count, 1) - - def test_stop_proof_keeps_original_source_timestamp_across_queries_and_restart(self): - self.gateway.submit(request()) - out = self.stop_window() - self.assertEqual(out["stopped_at"], 1700000000.0 + self.clock()) - original_stamp = out["stopped_at"] - self.clock.advance(50) - self.assertEqual(self.gateway.poll(request()["goal_id"])["stopped_at"], original_stamp) - self.gateway.close() - self.gateway = Gateway(self.path, self.backend, self.config, self.clock) - self.assertEqual(self.gateway.get(request()["goal_id"])["stopped_at"], original_stamp) - - def test_rejected_goal_requires_stop_evidence(self): - self.backend.send_mode = "REJECTED" - out = self.gateway.submit(request()) - self.assertEqual(out["stop_state"], "UNKNOWN") - out = self.stop_window("REJECTED") - self.assertEqual((out["outcome"], out["stop_state"]), ("REJECTED", "CONFIRMED")) - - def test_restart_locks_unfinished_goal_and_does_not_resend(self): - self.gateway.submit(request()); self.gateway.close() - self.gateway = Gateway(self.path, self.backend, self.config, self.clock) - old = self.gateway.get(request()["goal_id"]) - self.assertEqual(old["status"], "STOP_UNKNOWN") - self.assertTrue(old["quarantined"]) - self.gateway.submit(request()) - with self.assertRaises(GatewayError): self.gateway.submit(request("22222222-2222-4222-8222-222222222222")) - self.assertEqual(self.backend.send_count, 1) - - def test_http_all_routes_require_bearer_and_goal_query_cancel_work(self): - server = make_server(self.gateway, "test-secret-token", host="127.0.0.1", port=0) - thread = threading.Thread(target=server.serve_forever, daemon=True); thread.start() - self.addCleanup(server.server_close); self.addCleanup(server.shutdown) - base = f"http://127.0.0.1:{server.server_address[1]}" - with self.assertRaises(urllib.error.HTTPError) as cm: urllib.request.urlopen(base + "/healthz") - self.assertEqual(cm.exception.code, 401) - def call(path, body=None): - data = None if body is None else json.dumps(body).encode() - req = urllib.request.Request(base + path, data=data, - headers={"Authorization": "Bearer test-secret-token", "Content-Type": "application/json"}) - with urllib.request.urlopen(req, timeout=2) as response: return json.load(response) - self.assertTrue(call("/healthz")["ready"]) - self.assertEqual(call("/v1/goals", request())["goal_id"], request()["goal_id"]) - self.assertEqual(call("/v1/goals/" + request()["goal_id"])["status"], "ACTIVE") - self.assertEqual(call("/v1/goals/" + request()["goal_id"] + "/cancel", {})["stop_state"], "UNKNOWN") - - - - - -class RosSourceTimeTests(unittest.TestCase): - """Exercise actual Noetic callbacks and gateway checks with injected clocks.""" - def setUp(self): - from types import SimpleNamespace as NS - from unittest.mock import patch - from navigation_gateway.backends import Ros1MoveBaseBackend - self.NS = NS - self.ros_time, self.monotonic_time = [100.0], [10.0] - self.backend = Ros1MoveBaseBackend.__new__(Ros1MoveBaseBackend) - self.backend.rospy = NS(Time=NS(now=lambda: NS(to_sec=lambda: self.ros_time[0]))) - self.backend.max_age = 1.0 - self.backend.map_id = "sim-map" - self.backend.lock = threading.RLock() - self.backend.sequence = 0 - self.backend.previous_odom_stamp = None - self.backend.odom_queue = [] - self.gateway = Gateway.__new__(Gateway) - self.gateway.backend = self.backend - self.gateway.clock = lambda: self.monotonic_time[0] - self.patch = patch("navigation_gateway.backends.time.monotonic", lambda: self.monotonic_time[0]) - self.patch.start() - self.addCleanup(self.patch.stop) - - def receive_all(self, stamp): - NS = self.NS - header = NS(stamp=NS(to_sec=lambda: stamp), frame_id="map") - self.backend._odom(NS(header=header, twist=NS(twist=NS(linear=NS(x=0., y=0., z=0.), angular=NS(x=0., y=0., z=0.))))) - self.backend._pose(NS(header=header, pose=NS(position=NS(x=1., y=2., z=0.), orientation=NS(x=0., y=0., z=0., w=1.)))) - self.backend._health(NS(data=json.dumps({"ready": True, "map_id": "sim-map", "stamp": stamp}))) - return (self.backend.odom, self.backend.pose, self.backend.health_value) - - def test_source_age_is_rechecked_even_when_receipt_is_fresh(self): - samples = self.receive_all(99.1) - self.assertTrue(all(self.gateway._fresh(sample, 1.0) for sample in samples)) - self.ros_time[0], self.monotonic_time[0] = 100.5, 10.5 - for sample in samples: - with self.subTest(sample=sample): self.assertFalse(self.gateway._fresh(sample, 1.0)) - - def test_backward_ros_jump_invalidates_old_epoch_even_when_age_is_in_range(self): - samples = self.receive_all(99.8) - self.ros_time[0], self.monotonic_time[0] = 99.9, 10.05 - for sample in samples: - with self.subTest(sample=sample): self.assertFalse(self.gateway._fresh(sample, 1.0)) - self.ros_time[0] = 100.1 - self.assertTrue(all(not self.gateway._fresh(sample, 1.0) for sample in samples)) - - def test_future_source_time_is_never_admitted_later_as_a_cached_sample(self): - samples = self.receive_all(100.1) - self.assertTrue(all(not self.gateway._fresh(sample, 1.0) for sample in samples)) - self.ros_time[0], self.monotonic_time[0] = 100.2, 10.1 - self.assertTrue(all(not self.gateway._fresh(sample, 1.0) for sample in samples)) - - - -# HTTP proxy contract tests use real loopback sockets; ROS imports remain lazy. -from navigation_gateway import ros2_proxy as module -import time -from http.server import BaseHTTPRequestHandler, ThreadingHTTPServer - -ID = '11111111-1111-4111-8111-111111111111' -BODY = {'goal_id': ID, 'trace': {'task_id': 't', 'subtask_id': 's', 'attempt': 1}, 'map_id': 'map-a', 'target_pose': {'frame_id': 'map', 'position': {'x': 1., 'y': 2., 'z': 0.}, 'orientation': {'x': 0., 'y': 0., 'z': 0., 'w': 1.}}, 'position_tolerance': .1, 'yaw_tolerance': .2, 'timeout_sec': 2.} -def snapshot(**updates): - value = {'goal_id': ID, 'status': 'ACTIVE', 'outcome': None, 'stop_state': 'UNKNOWN', 'controller_state': 'ACTIVE', 'message': '', 'position_error': None, 'yaw_error': None, 'sequence': 1, 'elapsed': .1, 'stopped_at': 1234.5} - value.update(updates) - return value - -class ProxyTests(unittest.TestCase): - def setUp(self): - self.assertIsNotNone(module, 'ROS2 proxy behavior is not implemented') - self.requests = [] - self.reply = snapshot() - self.delay = 0 - self.dribble = False - self.cancel_reply = None - owner = self - class Handler(BaseHTTPRequestHandler): - def do_GET(self): self.respond() - def do_POST(self): self.respond() - def log_message(self, *args): pass - def respond(self): - raw = self.rfile.read(int(self.headers.get('Content-Length', '0'))) - owner.requests.append((self.command, self.path, self.headers.get('Authorization'), json.loads(raw) if raw else None)) - if owner.delay: time.sleep(owner.delay) - value = {'ready': True, 'reason': '', 'map_id': 'map-a'} if self.path == '/healthz' else copy.deepcopy(owner.reply) - if self.path.endswith('/cancel') and owner.cancel_reply is not None: - value = owner.cancel_reply - data = json.dumps(value).encode() - try: - self.send_response(200) - self.send_header('Content-Length', str(len(data))) - self.end_headers() - if owner.dribble: - for byte in data: - self.wfile.write(bytes([byte])) - self.wfile.flush() - time.sleep(.005) - else: - self.wfile.write(data) - except (BrokenPipeError, ConnectionResetError): pass - self.server = ThreadingHTTPServer(('127.0.0.1', 0), Handler) - self.server.daemon_threads = True - self.thread = threading.Thread(target=self.server.serve_forever, daemon=True) - self.thread.start() - self.config = module.ProxyConfig(endpoint='http://127.0.0.1:%d' % self.server.server_port, token='test-secret-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) - self.client = module.GatewayClient(self.config) - def tearDown(self): - if hasattr(self, 'server'): - self.server.shutdown() - self.server.server_close() - def wait_for(self, predicate): - deadline = time.monotonic() + 2 - while time.monotonic() < deadline: - if predicate(): return - time.sleep(.005) - self.fail('condition not reached') - def test_body_is_frozen_and_all_http_routes_are_authenticated(self): - body = copy.deepcopy(BODY) - session = module.GoalSession(self.client, body) - body['trace']['task_id'] = 'changed' - self.client.health() - session.start() - self.wait_for(lambda: len(self.requests) >= 3) - self.assertEqual(self.requests[1][3]['trace']['task_id'], 't') - session.request_cancel() - self.wait_for(lambda: any(r[1].endswith('/cancel') for r in self.requests)) - self.assertTrue(all(r[2] == 'Bearer test-secret-token' for r in self.requests)) - self.assertEqual([r[3]['goal_id'] for r in self.requests if r[1] == '/v1/goals'], [ID]) - self.reply = snapshot(status='TERMINAL', outcome='CANCELED', stop_state='CONFIRMED', controller_state='PREEMPTED', sequence=9) - events = [] - self.wait_for(lambda: bool(events.extend(session.drain_events())) or any(e.terminal for e in events)) - self.assertTrue(any(e.terminal and e.snapshot.outcome == 'CANCELED' for e in events)) - def test_cancel_ack_and_unconfirmed_terminal_do_not_finish(self): - session = module.GoalSession(self.client, BODY) - session.start() - self.wait_for(lambda: len(self.requests) >= 2) - self.cancel_reply = snapshot(status='TERMINAL', outcome='CANCELED', controller_state='PREEMPTED', stop_state='CONFIRMED', sequence=8) - session.request_cancel() - self.wait_for(lambda: any(r[1].endswith('/cancel') for r in self.requests)) - self.reply = snapshot(status='TERMINAL', outcome='CANCELED', controller_state='PREEMPTED', sequence=2) - time.sleep(.06) - self.assertFalse(any(e.terminal for e in session.drain_events())) - self.reply = snapshot(status='TERMINAL', outcome='CANCELED', controller_state='PREEMPTED', stop_state='CONFIRMED', sequence=3) - events = [] - self.wait_for(lambda: bool(events.extend(session.drain_events())) or any(e.terminal for e in events)) - self.assertEqual([e.snapshot.stop_state for e in events if e.terminal], ['CONFIRMED']) - def test_transport_timeout_produces_unknown_error_and_bounded_wait(self): - self.delay = .7 - session = module.GoalSession(self.client, BODY) - started = time.monotonic() - session.start() - self.assertLess(time.monotonic() - started, .05) - events = [] - self.wait_for(lambda: bool(events.extend(session.drain_events())) or any(e.error for e in events)) - self.assertLess(time.monotonic() - started, .6) - self.assertFalse(any(e.terminal for e in events)) - self.assertTrue(any(e.error for e in events)) - def test_completion_outside_tolerance_never_releases(self): - self.reply = snapshot(status='TERMINAL', outcome='COMPLETED', controller_state='SUCCEEDED', stop_state='CONFIRMED', position_error=.9, yaw_error=.01) - session = module.GoalSession(self.client, BODY) - session.start() - events = [] - self.wait_for(lambda: bool(events.extend(session.drain_events())) or any(e.error or e.terminal for e in events)) - self.assertFalse(any(e.terminal for e in events)) - self.assertTrue(any(e.error for e in events)) - - def test_native_arrived_with_stop_and_tolerances_completes(self): - self.reply = snapshot(status='TERMINAL', outcome='COMPLETED', controller_state='ARRIVED', stop_state='CONFIRMED', position_error=.02, yaw_error=.01) - session = module.GoalSession(self.client, BODY) - session.start() - events = [] - self.wait_for(lambda: bool(events.extend(session.drain_events())) or any(e.error or e.terminal for e in events)) - self.assertTrue(any(e.terminal for e in events)) - self.assertFalse(any(e.error for e in events)) - - def test_short_token_is_rejected_before_network_io(self): - from dataclasses import replace - with self.assertRaises(ValueError): - replace(self.config, token='short') - - def test_actual_pose_reaches_ros_pose_stamped_without_invented_stamp(self): - from types import SimpleNamespace as NS - pose = {'frame_id': 'map', 'position': {'x': 1.0, 'y': 2.0, 'z': 0.0}, 'orientation': {'x': 0.0, 'y': 0.0, 'z': 0.0, 'w': 1.0}} - value = module.GatewaySnapshot.parse(snapshot(pose_valid=True, current_pose=pose, final_pose=pose), ID) - self.assertTrue(hasattr(value, 'pose_valid'), 'actual pose evidence is missing from gateway snapshot') - self.assertTrue(value.pose_valid) - target = NS(header=NS(frame_id='', stamp=NS(sec=0, nanosec=0)), pose=NS(position=NS(x=0., y=0., z=0.), orientation=NS(x=0., y=0., z=0., w=0.))) - module.assign_ros_pose(target, value.final_pose) - self.assertEqual((target.header.frame_id, target.pose.position.x, target.pose.position.y, target.pose.orientation.w), ('map', 1.0, 2.0, 1.0)) - self.assertEqual((target.header.stamp.sec, target.header.stamp.nanosec), (0, 0)) - - def test_invalid_observed_pose_is_rejected(self): - for changes in ({'frame_id': 'odom'}, {'orientation': {'x': 0., 'y': 0., 'z': 0., 'w': 0.}}, {'position': {'x': float('nan'), 'y': 0., 'z': 0.}}): - pose = {'frame_id': 'map', 'position': {'x': 1., 'y': 2., 'z': 0.}, 'orientation': {'x': 0., 'y': 0., 'z': 0., 'w': 1.}} - pose.update(changes) - with self.assertRaises(module.GatewayError): - module.GatewaySnapshot.parse(snapshot(pose_valid=True, final_pose=pose), ID) - - def test_confirmed_terminal_rejects_missing_or_invalid_stop_stamp(self): - for stamp in (None, 0.0, -1.0, True, float('nan'), float('inf'), 2147483648.0): - with self.subTest(stamp=stamp), self.assertRaises(module.GatewayError): - module.GatewaySnapshot.parse(snapshot(status='TERMINAL', outcome='CANCELED', controller_state='PREEMPTED', stop_state='CONFIRMED', stopped_at=stamp), ID) - - def test_ros_stop_time_normalizes_nanosecond_rounding_and_range(self): - from types import SimpleNamespace as NS - self.assertTrue(hasattr(module, 'assign_ros_time'), 'source stop time mapping is missing') - for source, expected in ((1234.5, (1234, 500000000)), (1.9999999996, (2, 0)), (2147483647.0, (2147483647, 0))): - result = NS(sec=0, nanosec=0) - module.assign_ros_time(result, source) - self.assertEqual((result.sec, result.nanosec), expected) - for invalid in (-1.0, 2147483648.0, float('inf'), True): - with self.assertRaises(module.GatewayError): - module.assign_ros_time(NS(sec=0, nanosec=0), invalid) - - def test_repeated_query_preserves_original_stop_evidence_stamp(self): - self.reply = snapshot(status='TERMINAL', outcome='COMPLETED', controller_state='SUCCEEDED', stop_state='CONFIRMED', position_error=.02, yaw_error=.01, stopped_at=1234.5) - first = self.client.query(ID) - second = self.client.query(ID) - self.assertTrue(hasattr(first, 'stopped_at'), 'original stop evidence timestamp was dropped') - self.assertEqual((first.stopped_at, second.stopped_at), (1234.5, 1234.5)) - - def test_request_deadline_bounds_slow_trickle_response(self): - self.dribble = True - started = time.monotonic() - with self.assertRaises(module.GatewayError): - self.client.query(ID) - self.assertLess(time.monotonic() - started, .4) - - def test_mismatched_uuid_never_completes_current_goal(self): - self.reply = snapshot(goal_id='22222222-2222-4222-8222-222222222222', status='TERMINAL', outcome='COMPLETED', controller_state='SUCCEEDED', stop_state='CONFIRMED') - session = module.GoalSession(self.client, BODY) - session.start() - events=[] - self.wait_for(lambda: bool(events.extend(session.drain_events())) or any(e.error for e in events)) - self.assertFalse(any(e.terminal for e in events)) - 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 8337ab7..9faa2d9 100644 --- a/tests/test_ros_contracts.py +++ b/tests/test_ros_contracts.py @@ -9,6 +9,7 @@ from types import SimpleNamespace ROOT = Path(__file__).resolve().parents[1] INTERFACES = ROOT / "ros2" / "bt_skill_interfaces" +NAVIGATION = ROOT / "ros2" / "navigation_interfaces" MOCKS = ROOT / "ros2" / "bt_mock_servers" PRIMITIVES = {"bool", "byte", "char", "float32", "float64", "int8", "uint8", "int16", "uint16", "int32", "uint32", "int64", "uint64", "string", "wstring"} EXTERNAL = {"builtin_interfaces/Time", "builtin_interfaces/Duration", "geometry_msgs/PoseStamped", "geometry_msgs/PointStamped", "std_msgs/Header"} @@ -43,7 +44,7 @@ class RosContractTests(unittest.TestCase): self.assertIn("g.destination.description=r.destination_id;", verify) def test_action_and_service_sections(self): - self.assertEqual(len(list((INTERFACES / "action").glob("*.action"))), 12) + self.assertEqual(len(list((INTERFACES / "action").glob("*.action"))), 10) for suffix, expected in (("action", 2), ("srv", 1), ("msg", 0)): for path in (INTERFACES / suffix).glob("*." + suffix): with self.subTest(path=path.name): @@ -75,32 +76,36 @@ class RosContractTests(unittest.TestCase): else: self.assertRegex(name, r"^[a-z][a-z0-9_]*$") - def test_navigate_exact_source_outer_contract(self): - goal, result, feedback = sections("Navigate") - self.assertEqual(goal, "\n".join([ - "bt_skill_interfaces/TaskTrace trace", "geometry_msgs/PoseStamped target_pose", - "float64 position_tolerance", "float64 yaw_tolerance", "builtin_interfaces/Duration timeout", - ])) - self.assertEqual(result, "\n".join([ - "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 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_navigate_flat_canonical_contract(self): + goal, result, feedback = "\n".join(fields(NAVIGATION / "action/NavigateToPose.action")).split("\n---\n") + self.assertEqual(goal.splitlines(), ["string task_id", "string subtask_id", + "geometry_msgs/PoseStamped target_pose", "float64 position_tolerance", + "float64 yaw_tolerance", "builtin_interfaces/Duration timeout"]) + self.assertEqual(result.splitlines(), ["uint8 SUCCEEDED=0", "uint8 CANCELED=1", + "uint8 TIMEOUT=2", "uint8 BLOCKED=3", "uint8 NOT_READY=4", "uint8 FAILED=5", + "uint8 STOP_UNKNOWN=0", "uint8 STOP_CONFIRMED=1", "uint8 status", "string error_code", + "string message", "bool final_pose_valid", "geometry_msgs/PoseStamped final_pose", + "float64 final_position_error", "float64 final_yaw_error", "uint8 stop_state", + "builtin_interfaces/Time stopped_at", "string stop_evidence_ref"]) + self.assertEqual(feedback.splitlines(), ["uint8 ACCEPTED=0", "uint8 CHECKING=1", + "uint8 PLANNING=2", "uint8 NAVIGATING=3", "uint8 BLOCKED=4", "uint8 STOPPING=5", + "builtin_interfaces/Time stamp", "uint64 sequence", "uint8 phase", "bool current_pose_valid", + "geometry_msgs/PoseStamped current_pose", "bool error_valid", "float64 position_error", + "float64 yaw_error", "bool blocked_valid", "bool blocked", + "builtin_interfaces/Duration elapsed_time", "string message"]) + self.assertIn('"action/NavigateToPose.action"', (NAVIGATION / "CMakeLists.txt").read_text()) + self.assertEqual(ET.parse(NAVIGATION / "package.xml").getroot().findtext("name"), "navigation_interfaces") - 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_navigation_has_no_duplicate_wire_contract_or_proxy(self): + for path in ("action/Navigate.action", "action/NavigateSemantic.action", "msg/NavigationResult.msg"): + self.assertFalse((INTERFACES / path).exists()) + self.assertEqual(list((ROOT / "navigation_gateway").rglob("*.py")), []) + header = (ROOT / "ros2/bt_executor/include/bt_executor/ros_driver.hpp").read_text() + self.assertIn("navigation_interfaces::action::NavigateToPose", header) + self.assertNotIn("semantic_", header) + for package in ("bt_executor", "bt_mock_servers"): + manifest = ET.parse(ROOT / "ros2" / package / "package.xml").getroot() + self.assertIn("navigation_interfaces", [v.text for v in manifest if v.tag in ("depend", "exec_depend")]) def test_manipulation_exact_source_outer_contract(self): goal, result, feedback = sections("ExecuteManipulation") diff --git a/tools/coverage_report.py b/tools/coverage_report.py index fedc463..d44af75 100644 --- a/tools/coverage_report.py +++ b/tools/coverage_report.py @@ -28,7 +28,7 @@ def executable_lines(path): def production(path): relative = Path(path).resolve().relative_to(ROOT) - return relative.parts[0] in ('coordinator', 'robobrain', 'navigation_gateway', 'ros2') and not any( + return relative.parts[0] in ('coordinator', 'robobrain', 'ros2') and not any( p in ('tests', 'test', 'tools', '__pycache__') for p in relative.parts) and relative.name not in ('setup.py',)