实现行为树执行器、任务协调和技能接口
This commit is contained in:
@@ -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