实现行为树执行器、任务协调和技能接口
This commit is contained in:
@@ -0,0 +1 @@
|
||||
"""Test support for executing ROS adapters without a ROS installation."""
|
||||
@@ -0,0 +1,155 @@
|
||||
#!/usr/bin/env python3
|
||||
"""ROS Humble smoke test for bt_mock_servers; run only in a built ROS overlay."""
|
||||
import json
|
||||
import threading
|
||||
import time
|
||||
|
||||
import rclpy
|
||||
from rclpy.action import ActionClient
|
||||
from rclpy.executors import MultiThreadedExecutor
|
||||
from rclpy.node import Node
|
||||
|
||||
from bt_mock_servers.mock_skills import ACTION_ENDPOINTS, ACTION_TYPES, MockSkills
|
||||
from bt_skill_interfaces.msg import VerificationEvidence
|
||||
from bt_skill_interfaces.srv import GetRobotState, ReconcileGoal
|
||||
|
||||
|
||||
def trace(value):
|
||||
value.task_id, value.subtask_id = "smoke-task", "smoke-step"
|
||||
value.attempt = value.task_revision = value.plan_version = 1
|
||||
value.run_id, value.execution_generation = "smoke-run", 1
|
||||
|
||||
|
||||
def goal_for(name):
|
||||
goal = ACTION_TYPES[name].Goal()
|
||||
goal.timeout.sec = 2
|
||||
if hasattr(goal, "trace"):
|
||||
trace(goal.trace)
|
||||
if hasattr(goal, "task_id"):
|
||||
goal.task_id = "smoke-task"
|
||||
if hasattr(goal, "subtask_id"):
|
||||
goal.subtask_id = "smoke-step"
|
||||
if name == "navigate":
|
||||
goal.target_pose.header.frame_id = "map"
|
||||
goal.target_pose.pose.orientation.w = 1.0
|
||||
goal.position_tolerance = goal.orientation_tolerance = 0.1
|
||||
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"
|
||||
elif name == "execute_posture":
|
||||
goal.posture_id, goal.expected_geometry_epoch = "home", 1
|
||||
elif name == "plan_task":
|
||||
goal.task_id, goal.task_revision, goal.planning_generation = "smoke-task", 1, 1
|
||||
goal.instruction = "fetch smoke target"
|
||||
goal.known_info_json = goal.context_snapshot_json = goal.constraints_json = "{}"
|
||||
elif name == "verify_state":
|
||||
goal.check, goal.expected_geometry_epoch = goal.PRECHECK, 1
|
||||
goal.target.object_ref, goal.target.description = "smoke-target", "smoke target"
|
||||
elif name == "locate_shelf_column":
|
||||
goal.target_ref, goal.target_description = "smoke-target", "smoke target"
|
||||
goal.source_region_ref, goal.observation_station_id = "shelf_zone", "station_A"
|
||||
goal.station_registry_version = 1
|
||||
elif name == "localize_target_3d":
|
||||
goal.target_ref, goal.target_description, goal.expected_geometry_epoch = "smoke-target", "smoke target", 1
|
||||
goal.shelf_id, goal.column_id, goal.station_binding_ref = "shelf_A", "1", "station_A"
|
||||
elif name == "check_free_space":
|
||||
goal.destination_ref, goal.destination_description = "bin_A", "bin A"
|
||||
goal.object_ref, goal.object_description, goal.placement_constraints_json = "smoke-target", "smoke target", "{}"
|
||||
elif name == "assess_grasp":
|
||||
goal.allowed_posture_ids = ["pregrasp"]
|
||||
goal.target_binding.target.object_ref, goal.target_binding.target.description = "smoke-target", "smoke target"
|
||||
goal.target_binding.context.schema_version = goal.target_binding.context.geometry_epoch = 1
|
||||
goal.robot_state.robot_id = "robot_01"
|
||||
goal.robot_state.valid_until.sec = 2_000_000_000
|
||||
elif name == "evaluate_progress":
|
||||
goal.task_description, goal.sequence = "fetch smoke target", 1
|
||||
goal.window_json = '[{"stamp":1,"views":{"front":"/tmp/smoke.png"}}]'
|
||||
elif name == "execute_task":
|
||||
goal.approved_plan_json, goal.context_json = '{"subtasks":[]}', "{}"
|
||||
return goal
|
||||
|
||||
|
||||
def await_future(future, seconds=5.0):
|
||||
deadline = time.monotonic() + seconds
|
||||
while not future.done() and time.monotonic() < deadline:
|
||||
time.sleep(0.01)
|
||||
if not future.done():
|
||||
raise TimeoutError("ROS smoke future timed out")
|
||||
return future.result()
|
||||
|
||||
|
||||
def main():
|
||||
rclpy.init()
|
||||
server, client_node = MockSkills(), Node("bt_mock_live_smoke", namespace="/sim/robot_01")
|
||||
executor = MultiThreadedExecutor(num_threads=8)
|
||||
executor.add_node(server); executor.add_node(client_node)
|
||||
thread = threading.Thread(target=executor.spin, daemon=True); thread.start()
|
||||
try:
|
||||
for name, action_type in ACTION_TYPES.items():
|
||||
feedback = []
|
||||
client = ActionClient(client_node, action_type, ACTION_ENDPOINTS[name])
|
||||
if not client.wait_for_server(timeout_sec=3.0):
|
||||
raise RuntimeError("unreachable action: " + name)
|
||||
handle = await_future(client.send_goal_async(
|
||||
goal_for(name), feedback_callback=lambda item, out=feedback: out.append(item.feedback)))
|
||||
if not handle.accepted:
|
||||
raise RuntimeError("valid smoke goal rejected: " + name)
|
||||
wrapped = await_future(handle.get_result_async())
|
||||
if wrapped.result is None:
|
||||
raise RuntimeError("missing result: " + name)
|
||||
if not feedback:
|
||||
raise RuntimeError("missing feedback: " + name)
|
||||
client.destroy()
|
||||
|
||||
server.scenarios["navigate"] = [{"kind": "cancel_stop_unknown", "duration_seconds": 1.0}]
|
||||
cancel_client = ActionClient(client_node, ACTION_TYPES["navigate"], ACTION_ENDPOINTS["navigate"])
|
||||
handle = await_future(cancel_client.send_goal_async(goal_for("navigate")))
|
||||
cancel = await_future(handle.cancel_goal_async())
|
||||
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):
|
||||
raise RuntimeError("cancel terminal did not preserve unknown stop")
|
||||
|
||||
state_client = client_node.create_client(GetRobotState, "get_robot_state")
|
||||
if not state_client.wait_for_service(timeout_sec=3.0):
|
||||
raise RuntimeError("GetRobotState unavailable")
|
||||
state = await_future(state_client.call_async(GetRobotState.Request(robot_id="robot_01")))
|
||||
if not state.available:
|
||||
raise RuntimeError("GetRobotState did not return simulator state")
|
||||
|
||||
reconcile_client = client_node.create_client(ReconcileGoal, "reconcile_goal")
|
||||
if not reconcile_client.wait_for_service(timeout_sec=3.0):
|
||||
raise RuntimeError("ReconcileGoal unavailable")
|
||||
request = ReconcileGoal.Request()
|
||||
trace(request.trace)
|
||||
request.goal_id = bytes(handle.goal_id.uuid).hex()
|
||||
request.operator_id, request.reason = "smoke", "live smoke"
|
||||
evidence = request.evidence
|
||||
evidence.status = VerificationEvidence.PASSED
|
||||
evidence.context.trace = request.trace
|
||||
evidence.context.source_goal_id = request.goal_id
|
||||
evidence.context.writer = "independent_smoke_observer"
|
||||
evidence.context.observed_at = client_node.get_clock().now().to_msg()
|
||||
evidence.context.valid_until.sec = evidence.context.observed_at.sec + 5
|
||||
evidence.context.valid_until.nanosec = evidence.context.observed_at.nanosec
|
||||
evidence.stopped_valid = evidence.stopped = True
|
||||
evidence.source = "SIMULATOR_INDEPENDENT_FIXTURE"
|
||||
evidence.evidence_ref = "sim://smoke/stopped"
|
||||
reconciled = await_future(reconcile_client.call_async(request))
|
||||
if not reconciled.accepted:
|
||||
raise RuntimeError("bound reconciliation rejected: " + reconciled.error_code)
|
||||
print(json.dumps({"actions": len(ACTION_TYPES), "services": 2, "status": "passed"}))
|
||||
finally:
|
||||
executor.shutdown(timeout_sec=2.0)
|
||||
server.destroy_node(); client_node.destroy_node()
|
||||
if rclpy.ok(): rclpy.shutdown()
|
||||
thread.join(timeout=2.0)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,234 @@
|
||||
"""Minimal ROS runtime and message classes generated from repository IDL."""
|
||||
from __future__ import annotations
|
||||
|
||||
import importlib.util
|
||||
import pathlib
|
||||
import sys
|
||||
import types
|
||||
|
||||
ROOT = pathlib.Path(__file__).resolve().parents[2]
|
||||
IDL = ROOT / "ros2" / "bt_skill_interfaces"
|
||||
|
||||
|
||||
def _module(name):
|
||||
module = types.ModuleType(name)
|
||||
sys.modules[name] = module
|
||||
return module
|
||||
|
||||
|
||||
def _fields(path, section=0):
|
||||
parts = [[]]
|
||||
for raw in path.read_text().splitlines():
|
||||
line = raw.split("#", 1)[0].strip()
|
||||
if not line:
|
||||
continue
|
||||
if line == "---":
|
||||
parts.append([])
|
||||
else:
|
||||
parts[-1].append(line)
|
||||
return parts[section]
|
||||
|
||||
|
||||
class RosClock:
|
||||
_nanoseconds = 10_000_000_000
|
||||
|
||||
def now(self):
|
||||
self._nanoseconds += 1_000_000
|
||||
return types.SimpleNamespace(
|
||||
nanoseconds=self._nanoseconds,
|
||||
to_msg=lambda: _type("builtin_interfaces/Time")(
|
||||
sec=self._nanoseconds // 1_000_000_000,
|
||||
nanosec=self._nanoseconds % 1_000_000_000,
|
||||
),
|
||||
)
|
||||
|
||||
|
||||
_registry = {}
|
||||
|
||||
|
||||
def _type(type_name):
|
||||
if type_name.endswith("[]"):
|
||||
return list
|
||||
return _registry[type_name]
|
||||
|
||||
|
||||
def _default(type_name):
|
||||
if type_name.endswith("[]"):
|
||||
return []
|
||||
if type_name in ("string",):
|
||||
return ""
|
||||
if type_name in ("bool",):
|
||||
return False
|
||||
if type_name.startswith(("uint", "int", "float")):
|
||||
return 0
|
||||
return _type(type_name)()
|
||||
|
||||
|
||||
def _make_class(name, lines):
|
||||
constants, fields = {}, []
|
||||
for line in lines:
|
||||
type_name, declaration = line.split(None, 1)
|
||||
if "=" in declaration:
|
||||
key, value = declaration.split("=", 1)
|
||||
constants[key] = int(value)
|
||||
else:
|
||||
fields.append((type_name, declaration))
|
||||
slots = tuple(field for _, field in fields)
|
||||
|
||||
def init(self, **kwargs):
|
||||
for type_name, field in fields:
|
||||
setattr(self, field, kwargs.pop(field, _default(type_name)))
|
||||
if kwargs:
|
||||
raise TypeError("unexpected fields: " + ", ".join(kwargs))
|
||||
|
||||
attrs = {"__slots__": slots, "__init__": init, **constants}
|
||||
return type(name, (), attrs)
|
||||
|
||||
|
||||
def install():
|
||||
"""Install deterministic rclpy and generated interface modules."""
|
||||
for name in list(sys.modules):
|
||||
if name == "rclpy" or name.startswith("rclpy.") or name.startswith("bt_skill_interfaces"):
|
||||
del sys.modules[name]
|
||||
|
||||
builtin = _module("builtin_interfaces")
|
||||
builtin_msg = _module("builtin_interfaces.msg")
|
||||
builtin.msg = builtin_msg
|
||||
for name, fields in {
|
||||
"Time": ["int32 sec", "uint32 nanosec"],
|
||||
"Duration": ["int32 sec", "uint32 nanosec"],
|
||||
}.items():
|
||||
cls = _make_class(name, fields)
|
||||
setattr(builtin_msg, name, cls)
|
||||
_registry[f"builtin_interfaces/{name}"] = cls
|
||||
|
||||
std = _module("std_msgs")
|
||||
std_msg = _module("std_msgs.msg")
|
||||
std.msg = std_msg
|
||||
header = _make_class("Header", ["builtin_interfaces/Time stamp", "string frame_id"])
|
||||
std_msg.Header = header
|
||||
_registry["std_msgs/Header"] = header
|
||||
string = _make_class("String", ["string data"])
|
||||
std_msg.String = string
|
||||
_registry["std_msgs/String"] = string
|
||||
|
||||
geometry = _module("geometry_msgs")
|
||||
geometry_msg = _module("geometry_msgs.msg")
|
||||
geometry.msg = geometry_msg
|
||||
definitions = {
|
||||
"Point": ["float64 x", "float64 y", "float64 z"],
|
||||
"Quaternion": ["float64 x", "float64 y", "float64 z", "float64 w"],
|
||||
"Pose": ["geometry_msgs/Point position", "geometry_msgs/Quaternion orientation"],
|
||||
"PoseStamped": ["std_msgs/Header header", "geometry_msgs/Pose pose"],
|
||||
"PointStamped": ["std_msgs/Header header", "geometry_msgs/Point point"],
|
||||
}
|
||||
for name, fields in definitions.items():
|
||||
cls = _make_class(name, fields)
|
||||
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))
|
||||
|
||||
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)
|
||||
|
||||
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)
|
||||
|
||||
rclpy = _module("rclpy")
|
||||
rclpy.ok = lambda: True
|
||||
rclpy.init = lambda **_: None
|
||||
rclpy.shutdown = lambda: None
|
||||
action = _module("rclpy.action")
|
||||
action.GoalResponse = types.SimpleNamespace(ACCEPT=1, REJECT=2)
|
||||
action.CancelResponse = types.SimpleNamespace(ACCEPT=1)
|
||||
action.ActionServer = lambda *args, **kwargs: types.SimpleNamespace(destroy=lambda: None)
|
||||
callbacks = _module("rclpy.callback_groups")
|
||||
callbacks.ReentrantCallbackGroup = object
|
||||
executors = _module("rclpy.executors")
|
||||
executors.MultiThreadedExecutor = lambda **_: types.SimpleNamespace(
|
||||
add_node=lambda node: None, spin=lambda: None, shutdown=lambda **_: None)
|
||||
node_module = _module("rclpy.node")
|
||||
|
||||
class Node:
|
||||
parameters = {}
|
||||
|
||||
def __init__(self, *_args, namespace="/sim/robot_01", **_kwargs):
|
||||
self._namespace = namespace
|
||||
self._clock = RosClock()
|
||||
|
||||
def get_namespace(self): return self._namespace
|
||||
def declare_parameter(self, name, default): self.parameters.setdefault(name, default)
|
||||
def get_parameter(self, name): return types.SimpleNamespace(value=self.parameters[name])
|
||||
def resolve_topic_name(self, name): return self._namespace + "/" + name
|
||||
def create_publisher(self, _type, name, *_a, **_k):
|
||||
publisher = types.SimpleNamespace(name=name, values=[])
|
||||
publisher.publish = publisher.values.append
|
||||
return publisher
|
||||
def create_service(self, _type, name, callback, **_k): return types.SimpleNamespace(name=name, callback=callback)
|
||||
def create_timer(self, *_a, **_k): return object()
|
||||
def get_logger(self): return types.SimpleNamespace(warning=lambda *_: None, error=lambda *_: None)
|
||||
def get_clock(self): return self._clock
|
||||
def destroy_node(self): pass
|
||||
|
||||
node_module.Node = Node
|
||||
rclpy.action, rclpy.callback_groups, rclpy.executors, rclpy.node = action, callbacks, executors, node_module
|
||||
|
||||
|
||||
def load_mock_module(parameters=None):
|
||||
install()
|
||||
sys.modules["rclpy.node"].Node.parameters = dict(parameters or {})
|
||||
package = _module("bt_mock_servers")
|
||||
package.__path__ = [str(IDL.parent / "bt_mock_servers" / "bt_mock_servers")]
|
||||
source = IDL.parent / "bt_mock_servers" / "bt_mock_servers" / "mock_skills.py"
|
||||
spec = importlib.util.spec_from_file_location("bt_mock_servers.mock_skills", source)
|
||||
module = importlib.util.module_from_spec(spec)
|
||||
sys.modules[spec.name] = module
|
||||
spec.loader.exec_module(module)
|
||||
return module
|
||||
|
||||
|
||||
class GoalHandle:
|
||||
def __init__(self, request, cancel=False, uuid=bytes(range(16))):
|
||||
self.request, self.is_cancel_requested = request, cancel
|
||||
self.goal_id = types.SimpleNamespace(uuid=uuid)
|
||||
self.feedback, self.native = [], None
|
||||
self.is_active = True
|
||||
self.cancel_acknowledged = False
|
||||
|
||||
def execute(self): pass
|
||||
def publish_feedback(self, value): self.feedback.append(value)
|
||||
def succeed(self): self.native, self.is_active = "succeeded", False
|
||||
def abort(self): self.native, self.is_active = "aborted", False
|
||||
def canceled(self): self.native, self.is_active = "canceled", False
|
||||
|
||||
def request_cancel(self):
|
||||
self.is_cancel_requested = True
|
||||
self.cancel_acknowledged = True
|
||||
return True
|
||||
Reference in New Issue
Block a user