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/native_workflow_regression.py
+23 -5
View File
@@ -94,6 +94,7 @@ def run_route(root, binary, output, suffix, recovery_enabled):
from rclpy.node import Node
from rclpy.serialization import deserialize_message
from bt_skill_interfaces.action import ExecuteTask, ExecuteManipulation
from navigation_interfaces.action import NavigateToPose
from bt_skill_interfaces.msg import RobotState
from bt_skill_interfaces.srv import ReconcileTask
from robot_bt_coordinator.plan_v2 import item_plan
@@ -105,7 +106,7 @@ def run_route(root, binary, output, suffix, recovery_enabled):
stops = ['shelf_A_stop', 'tote_A_stop'] if suffix == 'object_table' else ['observe_A', 'shelf_A_stop', 'tote_A_stop']
scenarios = {'verify_state': [{'kind': 'passed'}],
'locate_shelf_column': [{'kind': 'normal', 'shelf_id': 'shelf_A', 'side_id': 'FRONT', 'column_id': '1', 'tier_id': '2'}],
'navigate_semantic': [{'kind': 'normal', 'final_pose': site['locations'][name]} for name in stops]}
'navigate': [{'kind': 'normal', 'final_pose': site['locations'][name]} for name in stops]}
actions = [name for name in ACTION_TYPES if name != 'execute_task']
rclpy.init(args=['--ros-args', '-p', 'initial_holding_state:=EMPTY',
'-p', 'scenarios_json:=' + json.dumps(json.dumps(scenarios)),
@@ -178,7 +179,7 @@ def run_route(root, binary, output, suffix, recovery_enabled):
def motion_counts():
with server.lock:
return {name: server.counts[name] for name in ('navigate_semantic', 'execute_manipulation', 'execute_posture')}
return {name: server.counts[name] for name in ('navigate', 'execute_manipulation', 'execute_posture')}
try:
start()
@@ -195,6 +196,23 @@ def run_route(root, binary, output, suffix, recovery_enabled):
row['decoded_manipulation'] = assert_manipulation_evidence(
journal_records(Path(journal) / 'goal_registry.log'), goal.trace.run_id,
ExecuteManipulation, deserialize_message)
navigation_rows = [entry for entry in journal_records(Path(journal) / 'goal_registry.log')
if entry['run_id'] == goal.trace.run_id and entry['skill'] == 0]
assert len(navigation_rows) == len(stops)
expected_poses = {(site['locations'][name]['x'], site['locations'][name]['y']) for name in stops}
actual_poses = set()
for entry in navigation_rows:
assert entry['type'] == 'navigation_interfaces/action/NavigateToPose_Goal'
navigation_goal = deserialize_message(bytes.fromhex(entry['wire']), NavigateToPose.Goal)
assert navigation_goal.task_id == goal.trace.task_id and navigation_goal.subtask_id
assert navigation_goal.target_pose.header.frame_id == 'map'
actual_poses.add((navigation_goal.target_pose.pose.position.x, navigation_goal.target_pose.pose.position.y))
assert entry['result_type'] == 'navigation_interfaces/action/NavigateToPose_Result'
navigation_result = deserialize_message(bytes.fromhex(entry['result_wire']), NavigateToPose.Result)
assert navigation_result.status == NavigateToPose.Result.SUCCEEDED
assert navigation_result.stop_state == NavigateToPose.Result.STOP_CONFIRMED
assert actual_poses == expected_poses
row['direct_navigation_goals'] = len(navigation_rows)
assert motion_counts()['execute_manipulation'] == 2
if recovery_enabled:
stop()
@@ -228,11 +246,11 @@ def run_route(root, binary, output, suffix, recovery_enabled):
unknown_holding=unknown.error_code, recovered=state)
if suffix == 'object_table':
with server.lock:
server.scenarios['navigate_semantic'] = [{'kind': 'normal', 'duration_seconds': 3., 'final_pose': site['locations']['shelf_A_stop']}]
server.scenarios['navigate'] = [{'kind': 'normal', 'duration_seconds': 3., 'final_pose': site['locations']['shelf_A_stop']}]
canceled_goal = task_goal('cancel')
canceled_handle = wait(client.send_goal_async(canceled_goal), 8)
assert canceled_handle.accepted
eventually(lambda: motion_counts()['navigate_semantic'] > before['navigate_semantic'])
eventually(lambda: motion_counts()['navigate'] > before['navigate'])
wait(canceled_handle.cancel_goal_async(), 5)
canceled_result = wait(canceled_handle.get_result_async(), 15)
assert canceled_result.status == 5 and canceled_result.result.result.stop_state == 1
@@ -267,7 +285,7 @@ def run_route(root, binary, output, suffix, recovery_enabled):
if suffix == 'shelf_cell':
with server.lock:
server.scenarios['locate_shelf_column'] = [{'kind': 'ambiguous'}]
server.scenarios['navigate_semantic'] = [{'kind': 'normal', 'final_pose': site['locations']['observe_A']}]
server.scenarios['navigate'] = [{'kind': 'normal', 'final_pose': site['locations']['observe_A']}]
before_ask = motion_counts()
ask_handle = wait(client.send_goal_async(task_goal('ambiguous')), 8)
assert ask_handle.accepted