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/test_ros_contracts.py
+31 -26
View File
@@ -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")