Unify navigation on NavigateToPose and remove legacy proxies
This commit is contained in:
@@ -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: '{}'
|
||||
|
||||
@@ -60,7 +60,6 @@ struct GoalRequest {
|
||||
std::optional<TargetBinding> target;
|
||||
std::optional<PlacementBinding> 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<SteadyTime> waiting_since_;
|
||||
std::optional<SteadyTime> motion_waiting_since_;
|
||||
std::optional<SteadyTime> stopped_waiting_since_;
|
||||
|
||||
@@ -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:
|
||||
|
||||
@@ -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) {
|
||||
|
||||
@@ -1 +0,0 @@
|
||||
"""Explicit ROS1 navigation / HTTP / ROS2 adapter; defaults are simulation only."""
|
||||
@@ -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
|
||||
@@ -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)
|
||||
@@ -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
|
||||
@@ -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()
|
||||
@@ -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<seconds<=3600:raise ValueError('Duration must be in (0,3600] seconds')
|
||||
return seconds
|
||||
|
||||
def translate_navigation_result(source, target, navigation_enum, execution_enum):
|
||||
"""Preserve proof and diagnostics while explicitly translating different enums."""
|
||||
mapping = {navigation_enum.SUCCEEDED: execution_enum.COMPLETED,
|
||||
navigation_enum.CANCELED: execution_enum.CANCELED,
|
||||
navigation_enum.TIMEOUT: execution_enum.TIMED_OUT,
|
||||
navigation_enum.BLOCKED: execution_enum.FAILED,
|
||||
navigation_enum.NOT_READY: execution_enum.REJECTED,
|
||||
navigation_enum.FAILED: execution_enum.FAILED}
|
||||
target.result.status = mapping[source.result.status]
|
||||
target.result.stop_state = {navigation_enum.UNKNOWN: execution_enum.UNKNOWN,
|
||||
navigation_enum.CONFIRMED: execution_enum.CONFIRMED}[source.result.stop_state]
|
||||
for field in ('error_code', 'message', 'stopped_at', 'stop_evidence_ref'):
|
||||
setattr(target.result, field, getattr(source.result, field))
|
||||
if not target.result.error_code:
|
||||
target.result.error_code = {navigation_enum.BLOCKED: 'NAV_BLOCKED',
|
||||
navigation_enum.NOT_READY: 'NAV_NOT_READY'}.get(source.result.status, '')
|
||||
target.final_pose = source.final_pose
|
||||
target.pose_valid = source.final_pose_valid
|
||||
target.errors_valid = (source.final_pose_valid and
|
||||
all(math.isfinite(v) for v in (source.final_position_error, source.final_yaw_error)) and
|
||||
source.final_position_error >= 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()
|
||||
@@ -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()
|
||||
@@ -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
|
||||
}
|
||||
}
|
||||
@@ -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})
|
||||
|
||||
@@ -3,8 +3,7 @@
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <rclcpp/serialization.hpp>
|
||||
#include <rclcpp_action/rclcpp_action.hpp>
|
||||
#include <bt_skill_interfaces/action/navigate.hpp>
|
||||
#include <bt_skill_interfaces/action/navigate_semantic.hpp>
|
||||
#include <navigation_interfaces/action/navigate_to_pose.hpp>
|
||||
#include <bt_skill_interfaces/action/execute_manipulation.hpp>
|
||||
#include <bt_skill_interfaces/action/locate_shelf_column.hpp>
|
||||
#include <bt_skill_interfaces/action/localize_target3_d.hpp>
|
||||
@@ -54,8 +53,7 @@ class RosDriver final : public robot_bt::GoalDriver {
|
||||
std::optional<std::uint64_t> 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<Navigate>::SharedPtr navigate_;
|
||||
rclcpp_action::Client<Semantic>::SharedPtr semantic_;
|
||||
rclcpp_action::Client<Manipulate>::SharedPtr manipulate_;
|
||||
rclcpp_action::Client<Locate>::SharedPtr locate_;
|
||||
rclcpp_action::Client<Localize>::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<Action, Navigate>) {
|
||||
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);
|
||||
};
|
||||
|
||||
@@ -5,7 +5,7 @@
|
||||
<maintainer email="feiyuwang1998@gmail.com">wangfeiyu</maintainer><license>Proprietary</license>
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
<depend>rclcpp</depend><depend>rclcpp_action</depend><depend>ament_index_cpp</depend>
|
||||
<depend>bt_skill_interfaces</depend><depend>geometry_msgs</depend><depend>std_msgs</depend>
|
||||
<depend>bt_skill_interfaces</depend><depend>navigation_interfaces</depend><depend>geometry_msgs</depend><depend>std_msgs</depend>
|
||||
<depend version_eq="4.10.0">behaviortree_cpp</depend><depend>nlohmann_json</depend>
|
||||
<exec_depend>launch_ros</exec_depend><exec_depend>launch</exec_depend>
|
||||
<export><build_type>ament_cmake</build_type></export>
|
||||
|
||||
@@ -89,7 +89,6 @@ RosDriver::RosDriver(rclcpp::Node& n, std::string robot_id, std::string journal,
|
||||
navigate_=rclcpp_action::create_client<Navigate>(&n,n.declare_parameter<std::string>("navigate_action","skills/navigate"));
|
||||
manipulate_=rclcpp_action::create_client<Manipulate>(&n,n.declare_parameter<std::string>("execute_manipulation_action","skills/execute_manipulation"));
|
||||
locate_=rclcpp_action::create_client<Locate>(&n,n.declare_parameter<std::string>("locate_shelf_column_action","skills/locate_shelf_column"));
|
||||
semantic_=rclcpp_action::create_client<Semantic>(&n,n.declare_parameter<std::string>("navigate_semantic_action","skills/navigate_semantic"));
|
||||
localize_=rclcpp_action::create_client<Localize>(&n,n.declare_parameter<std::string>("localize_target_3d_action","skills/localize_target_3d"));
|
||||
assess_=rclcpp_action::create_client<Assess>(&n,n.declare_parameter<std::string>("assess_grasp_action","skills/assess_grasp"));
|
||||
posture_=rclcpp_action::create_client<Posture>(&n,n.declare_parameter<std::string>("execute_posture_action","skills/execute_posture"));
|
||||
@@ -126,7 +125,7 @@ std::optional<std::uint64_t> 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>(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>(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;
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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.')
|
||||
|
||||
@@ -20,7 +20,7 @@ int main(int argc,char** argv) {
|
||||
while(!driver.ready(robot_bt::Skill::NAVIGATE)&&robot_bt::SteadyClock::now()<until) {
|
||||
rclcpp::spin_some(node); std::this_thread::sleep_for(std::chrono::milliseconds(10));
|
||||
}
|
||||
if(!driver.ready(robot_bt::Skill::NAVIGATE)) throw std::runtime_error("Navigate discovery timed out");
|
||||
if(!driver.ready(robot_bt::Skill::NAVIGATE)) throw std::runtime_error("NavigateToPose discovery timed out");
|
||||
robot_bt::GoalRequest request;
|
||||
request.goal_id="probe-"+std::to_string(scenario);request.robot_id="robot_01";
|
||||
request.trace.task_id=request.goal_id;request.trace.run_id=request.goal_id;request.trace.subtask_id="navigate";
|
||||
@@ -35,7 +35,7 @@ int main(int argc,char** argv) {
|
||||
rclcpp::spin_some(node);registry.pump(robot_bt::SteadyClock::now());
|
||||
auto record=registry.find(active_id);
|
||||
if(!record) throw std::runtime_error("registry lost active goal: "+active_id);
|
||||
if((scenario==1||scenario==6)&&record->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();
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -8,6 +8,7 @@
|
||||
<buildtool_depend>ament_python</buildtool_depend>
|
||||
<exec_depend>rclpy</exec_depend>
|
||||
<exec_depend>bt_skill_interfaces</exec_depend>
|
||||
<exec_depend>navigation_interfaces</exec_depend>
|
||||
<exec_depend>builtin_interfaces</exec_depend>
|
||||
<exec_depend>std_msgs</exec_depend>
|
||||
<export><build_type>ament_python</build_type></export>
|
||||
|
||||
@@ -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"
|
||||
|
||||
@@ -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
|
||||
@@ -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
|
||||
@@ -2,7 +2,7 @@
|
||||
<package format="3">
|
||||
<name>bt_skill_interfaces</name>
|
||||
<version>2.0.0</version>
|
||||
<description>Robot skill contracts with navigation-specific outcomes and stop evidence.</description>
|
||||
<description>Robot task and manipulation contracts with stop evidence.</description>
|
||||
<maintainer email="feiyuwang1998@gmail.com">wangfeiyu</maintainer>
|
||||
<license>Proprietary</license>
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
|
||||
@@ -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()
|
||||
+22
-8
@@ -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
|
||||
@@ -0,0 +1,12 @@
|
||||
<?xml version="1.0"?>
|
||||
<package format="3">
|
||||
<name>navigation_interfaces</name><version>1.0.0</version>
|
||||
<description>Navigation action contract.</description>
|
||||
<maintainer email="feiyuwang1998@gmail.com">wangfeiyu</maintainer><license>Proprietary</license>
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
<buildtool_depend>rosidl_default_generators</buildtool_depend>
|
||||
<depend>builtin_interfaces</depend><depend>geometry_msgs</depend>
|
||||
<exec_depend>rosidl_default_runtime</exec_depend>
|
||||
<member_of_group>rosidl_interface_packages</member_of_group>
|
||||
<export><build_type>ament_cmake</build_type></export>
|
||||
</package>
|
||||
@@ -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")
|
||||
|
||||
@@ -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)
|
||||
|
||||
@@ -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')
|
||||
|
||||
@@ -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
|
||||
|
||||
+37
-33
@@ -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
|
||||
|
||||
@@ -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__':
|
||||
|
||||
+80
-69
@@ -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 (
|
||||
|
||||
@@ -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)
|
||||
@@ -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()
|
||||
+31
-26
@@ -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")
|
||||
|
||||
@@ -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',)
|
||||
|
||||
|
||||
|
||||
Reference in New Issue
Block a user