实现行为树执行器、任务协调和技能接口
This commit is contained in:
@@ -0,0 +1,155 @@
|
||||
#!/usr/bin/env python3
|
||||
"""ROS Humble smoke test for bt_mock_servers; run only in a built ROS overlay."""
|
||||
import json
|
||||
import threading
|
||||
import time
|
||||
|
||||
import rclpy
|
||||
from rclpy.action import ActionClient
|
||||
from rclpy.executors import MultiThreadedExecutor
|
||||
from rclpy.node import Node
|
||||
|
||||
from bt_mock_servers.mock_skills import ACTION_ENDPOINTS, ACTION_TYPES, MockSkills
|
||||
from bt_skill_interfaces.msg import VerificationEvidence
|
||||
from bt_skill_interfaces.srv import GetRobotState, ReconcileGoal
|
||||
|
||||
|
||||
def trace(value):
|
||||
value.task_id, value.subtask_id = "smoke-task", "smoke-step"
|
||||
value.attempt = value.task_revision = value.plan_version = 1
|
||||
value.run_id, value.execution_generation = "smoke-run", 1
|
||||
|
||||
|
||||
def goal_for(name):
|
||||
goal = ACTION_TYPES[name].Goal()
|
||||
goal.timeout.sec = 2
|
||||
if hasattr(goal, "trace"):
|
||||
trace(goal.trace)
|
||||
if hasattr(goal, "task_id"):
|
||||
goal.task_id = "smoke-task"
|
||||
if hasattr(goal, "subtask_id"):
|
||||
goal.subtask_id = "smoke-step"
|
||||
if name == "navigate":
|
||||
goal.target_pose.header.frame_id = "map"
|
||||
goal.target_pose.pose.orientation.w = 1.0
|
||||
goal.position_tolerance = goal.orientation_tolerance = 0.1
|
||||
elif name == "navigate_semantic":
|
||||
goal.kind, goal.reference, goal.registry_version = "LOCATION", "bin_A", 1
|
||||
goal.position_tolerance = goal.orientation_tolerance = 0.1
|
||||
elif name == "execute_manipulation":
|
||||
goal.skill, goal.instruction = "pick", "pick the smoke target"
|
||||
goal.target.object_ref, goal.target.description = "smoke-target", "smoke target"
|
||||
elif name == "execute_posture":
|
||||
goal.posture_id, goal.expected_geometry_epoch = "home", 1
|
||||
elif name == "plan_task":
|
||||
goal.task_id, goal.task_revision, goal.planning_generation = "smoke-task", 1, 1
|
||||
goal.instruction = "fetch smoke target"
|
||||
goal.known_info_json = goal.context_snapshot_json = goal.constraints_json = "{}"
|
||||
elif name == "verify_state":
|
||||
goal.check, goal.expected_geometry_epoch = goal.PRECHECK, 1
|
||||
goal.target.object_ref, goal.target.description = "smoke-target", "smoke target"
|
||||
elif name == "locate_shelf_column":
|
||||
goal.target_ref, goal.target_description = "smoke-target", "smoke target"
|
||||
goal.source_region_ref, goal.observation_station_id = "shelf_zone", "station_A"
|
||||
goal.station_registry_version = 1
|
||||
elif name == "localize_target_3d":
|
||||
goal.target_ref, goal.target_description, goal.expected_geometry_epoch = "smoke-target", "smoke target", 1
|
||||
goal.shelf_id, goal.column_id, goal.station_binding_ref = "shelf_A", "1", "station_A"
|
||||
elif name == "check_free_space":
|
||||
goal.destination_ref, goal.destination_description = "bin_A", "bin A"
|
||||
goal.object_ref, goal.object_description, goal.placement_constraints_json = "smoke-target", "smoke target", "{}"
|
||||
elif name == "assess_grasp":
|
||||
goal.allowed_posture_ids = ["pregrasp"]
|
||||
goal.target_binding.target.object_ref, goal.target_binding.target.description = "smoke-target", "smoke target"
|
||||
goal.target_binding.context.schema_version = goal.target_binding.context.geometry_epoch = 1
|
||||
goal.robot_state.robot_id = "robot_01"
|
||||
goal.robot_state.valid_until.sec = 2_000_000_000
|
||||
elif name == "evaluate_progress":
|
||||
goal.task_description, goal.sequence = "fetch smoke target", 1
|
||||
goal.window_json = '[{"stamp":1,"views":{"front":"/tmp/smoke.png"}}]'
|
||||
elif name == "execute_task":
|
||||
goal.approved_plan_json, goal.context_json = '{"subtasks":[]}', "{}"
|
||||
return goal
|
||||
|
||||
|
||||
def await_future(future, seconds=5.0):
|
||||
deadline = time.monotonic() + seconds
|
||||
while not future.done() and time.monotonic() < deadline:
|
||||
time.sleep(0.01)
|
||||
if not future.done():
|
||||
raise TimeoutError("ROS smoke future timed out")
|
||||
return future.result()
|
||||
|
||||
|
||||
def main():
|
||||
rclpy.init()
|
||||
server, client_node = MockSkills(), Node("bt_mock_live_smoke", namespace="/sim/robot_01")
|
||||
executor = MultiThreadedExecutor(num_threads=8)
|
||||
executor.add_node(server); executor.add_node(client_node)
|
||||
thread = threading.Thread(target=executor.spin, daemon=True); thread.start()
|
||||
try:
|
||||
for name, action_type in ACTION_TYPES.items():
|
||||
feedback = []
|
||||
client = ActionClient(client_node, action_type, ACTION_ENDPOINTS[name])
|
||||
if not client.wait_for_server(timeout_sec=3.0):
|
||||
raise RuntimeError("unreachable action: " + name)
|
||||
handle = await_future(client.send_goal_async(
|
||||
goal_for(name), feedback_callback=lambda item, out=feedback: out.append(item.feedback)))
|
||||
if not handle.accepted:
|
||||
raise RuntimeError("valid smoke goal rejected: " + name)
|
||||
wrapped = await_future(handle.get_result_async())
|
||||
if wrapped.result is None:
|
||||
raise RuntimeError("missing result: " + name)
|
||||
if not feedback:
|
||||
raise RuntimeError("missing feedback: " + name)
|
||||
client.destroy()
|
||||
|
||||
server.scenarios["navigate"] = [{"kind": "cancel_stop_unknown", "duration_seconds": 1.0}]
|
||||
cancel_client = ActionClient(client_node, ACTION_TYPES["navigate"], ACTION_ENDPOINTS["navigate"])
|
||||
handle = await_future(cancel_client.send_goal_async(goal_for("navigate")))
|
||||
cancel = await_future(handle.cancel_goal_async())
|
||||
if not cancel.goals_canceling:
|
||||
raise RuntimeError("cancel was not acknowledged")
|
||||
canceled = await_future(handle.get_result_async())
|
||||
if (canceled.result.result.status != canceled.result.result.CANCELED or
|
||||
canceled.result.result.stop_state != canceled.result.result.UNKNOWN):
|
||||
raise RuntimeError("cancel terminal did not preserve unknown stop")
|
||||
|
||||
state_client = client_node.create_client(GetRobotState, "get_robot_state")
|
||||
if not state_client.wait_for_service(timeout_sec=3.0):
|
||||
raise RuntimeError("GetRobotState unavailable")
|
||||
state = await_future(state_client.call_async(GetRobotState.Request(robot_id="robot_01")))
|
||||
if not state.available:
|
||||
raise RuntimeError("GetRobotState did not return simulator state")
|
||||
|
||||
reconcile_client = client_node.create_client(ReconcileGoal, "reconcile_goal")
|
||||
if not reconcile_client.wait_for_service(timeout_sec=3.0):
|
||||
raise RuntimeError("ReconcileGoal unavailable")
|
||||
request = ReconcileGoal.Request()
|
||||
trace(request.trace)
|
||||
request.goal_id = bytes(handle.goal_id.uuid).hex()
|
||||
request.operator_id, request.reason = "smoke", "live smoke"
|
||||
evidence = request.evidence
|
||||
evidence.status = VerificationEvidence.PASSED
|
||||
evidence.context.trace = request.trace
|
||||
evidence.context.source_goal_id = request.goal_id
|
||||
evidence.context.writer = "independent_smoke_observer"
|
||||
evidence.context.observed_at = client_node.get_clock().now().to_msg()
|
||||
evidence.context.valid_until.sec = evidence.context.observed_at.sec + 5
|
||||
evidence.context.valid_until.nanosec = evidence.context.observed_at.nanosec
|
||||
evidence.stopped_valid = evidence.stopped = True
|
||||
evidence.source = "SIMULATOR_INDEPENDENT_FIXTURE"
|
||||
evidence.evidence_ref = "sim://smoke/stopped"
|
||||
reconciled = await_future(reconcile_client.call_async(request))
|
||||
if not reconciled.accepted:
|
||||
raise RuntimeError("bound reconciliation rejected: " + reconciled.error_code)
|
||||
print(json.dumps({"actions": len(ACTION_TYPES), "services": 2, "status": "passed"}))
|
||||
finally:
|
||||
executor.shutdown(timeout_sec=2.0)
|
||||
server.destroy_node(); client_node.destroy_node()
|
||||
if rclpy.ok(): rclpy.shutdown()
|
||||
thread.join(timeout=2.0)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
Reference in New Issue
Block a user