wangfeiyu/behavior-tree
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Unify navigation on NavigateToPose and remove legacy proxies

Browse Source
This commit is contained in:
wangfeiyu
2026-09-22 17:40:25 +08:00
parent 964d1fde67
commit 24e0b922bc
40 changed files with 461 additions and 2592 deletions
Download Patch File Download Diff File Expand all files Collapse all files
tests/helpers/ros_shim.py
+37 -33
View File
@@ -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