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

fix: align navigation contract and readiness handling

Browse Source
This commit is contained in:
wangfeiyu
2026-09-22 14:35:14 +08:00
parent 91bcd92d6b
commit 964d1fde67
21 changed files with 922 additions and 139 deletions
Download Patch File Download Diff File Expand all files Collapse all files
tests/test_ros_contracts.py
+16 -8
View File
@@ -79,21 +79,29 @@ class RosContractTests(unittest.TestCase):
goal, result, feedback = sections("Navigate")
self.assertEqual(goal, "\n".join([
"bt_skill_interfaces/TaskTrace trace", "geometry_msgs/PoseStamped target_pose",
"float64 position_tolerance", "float64 orientation_tolerance", "builtin_interfaces/Duration timeout",
"float64 position_tolerance", "float64 yaw_tolerance", "builtin_interfaces/Duration timeout",
]))
self.assertEqual(result, "\n".join([
"bt_skill_interfaces/ExecutionResult result", "bool pose_valid", "geometry_msgs/PoseStamped final_pose",
"bool errors_valid", "float64 final_position_error", "float64 final_orientation_error",
"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 CHECKING=0", "uint8 PLANNING=1", "uint8 NAVIGATING=2", "uint8 WAITING_OBSTACLE=3",
"uint8 RECOVERING=4", "uint8 ARRIVING=5", "uint8 STOPPING=6", "builtin_interfaces/Time stamp",
"uint32 sequence", "uint8 phase", "bool pose_valid", "geometry_msgs/PoseStamped current_pose",
"bool errors_valid", "float64 position_error", "float64 orientation_error", "bool blocked_valid",
"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_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_manipulation_exact_source_outer_contract(self):
goal, result, feedback = sections("ExecuteManipulation")
self.assertEqual(goal, "\n".join([
@@ -156,7 +164,7 @@ class RosContractTests(unittest.TestCase):
def test_all_idl_files_are_registered_in_build(self):
cmake = (INTERFACES / "CMakeLists.txt").read_text()
declarations = re.findall(r'"((?:msg|srv|action)/[^"\n]+)"', cmake)
actual = sorted(str(p.relative_to(INTERFACES)) for p in INTERFACES.rglob("*") if p.suffix in (".msg", ".srv", ".action"))
actual = sorted(p.relative_to(INTERFACES).as_posix() for p in INTERFACES.rglob("*") if p.suffix in (".msg", ".srv", ".action"))
self.assertEqual(sorted(declarations), actual)
self.assertEqual(len(declarations), len(set(declarations)))
manifest = ET.parse(INTERFACES / "package.xml").getroot()