实现行为树执行器、任务协调和技能接口

2026-09-20 12:18:52 +08:00
commit 492676344a
143 changed files with 13010 additions and 0 deletions
@@ -0,0 +1,39 @@
+cmake_minimum_required(VERSION 3.8)
+project(bt_skill_interfaces)
+find_package(ament_cmake REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
+find_package(builtin_interfaces REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(std_msgs REQUIRED)
+rosidl_generate_interfaces(${PROJECT_NAME}
+  "msg/TaskTrace.msg"
+  "msg/ObjectTarget.msg"
+  "msg/RegionTarget.msg"
+  "msg/ExecutionResult.msg"
+  "msg/ObservationContext.msg"
+  "msg/RobotState.msg"
+  "msg/SafetyState.msg"
+  "msg/VerificationEvidence.msg"
+  "msg/Station.msg"
+  "msg/TargetBinding.msg"
+  "msg/PlacementBinding.msg"
+  "action/Navigate.action"
+  "action/NavigateSemantic.action"
+  "action/EvaluateProgress.action"
+  "msg/VisualObservation.msg"
+  "msg/DenseProgress.msg"
+  "action/ExecuteManipulation.action"
+  "action/PlanTask.action"
+  "action/LocateShelfColumn.action"
+  "action/LocalizeTarget3D.action"
+  "action/CheckFreeSpace.action"
+  "action/AssessGrasp.action"
+  "action/ExecutePosture.action"
+  "action/VerifyState.action"
+  "action/ExecuteTask.action"
+  "srv/GetRobotState.srv"
+  "srv/ReconcileGoal.srv"
+  DEPENDENCIES builtin_interfaces geometry_msgs std_msgs
+)
+ament_export_dependencies(rosidl_default_runtime)
+ament_package()
@@ -0,0 +1,22 @@
+# New v1: read-only admission assessment; it must never actuate the robot.
+bt_skill_interfaces/TaskTrace trace
+bt_skill_interfaces/TargetBinding target_binding
+bt_skill_interfaces/RobotState robot_state
+string[] allowed_posture_ids
+builtin_interfaces/Duration timeout
+---
+uint8 DIRECT=0
+uint8 ADJUST_POSTURE=1
+uint8 NOT_REACHABLE=2
+uint8 UNKNOWN=3
+uint8 decision
+string posture_id
+uint64 geometry_epoch
+string evidence_ref
+string error_code
+string message
+---
+builtin_interfaces/Time stamp
+uint32 sequence
+uint8 phase
+string message
@@ -0,0 +1,38 @@
+# BT DR pp17-18; explicit optional-point/pose validity bits added in v1.
+string task_id
+string subtask_id
+string destination_ref
+string destination_description
+string object_ref
+string object_description
+builtin_interfaces/Time capture_after
+string placement_constraints_json
+builtin_interfaces/Duration timeout
+---
+uint8 SUCCEEDED=0
+uint8 FAILED=1
+uint8 NO_FREE_SPACE=2
+uint8 AMBIGUOUS=3
+uint8 status
+string destination_ref
+string placement_region_ref
+geometry_msgs/PointStamped placement_point
+bool placement_point_valid
+geometry_msgs/PoseStamped placement_pose
+bool placement_pose_valid
+float32 confidence
+bool geometry_valid
+string quality_code
+string observation_id
+builtin_interfaces/Time observed_at
+builtin_interfaces/Time valid_until
+string error_code
+string message
+string record_ref
+---
+uint8 CAPTURING=0
+uint8 PROCESSING=1
+builtin_interfaces/Time stamp
+uint32 sequence
+uint8 phase
+string message
@@ -0,0 +1,15 @@
+bt_skill_interfaces/TaskTrace trace
+string task_description
+builtin_interfaces/Time capture_after
+uint32 sequence
+# Ordered [{stamp: UNIX ROS seconds, views: {camera_name: absolute_local_path}}]
+string window_json
+builtin_interfaces/Duration timeout
+---
+bt_skill_interfaces/DenseProgress feedback_state
+string error_code
+---
+builtin_interfaces/Time stamp
+uint32 sequence
+uint8 phase
+string message
@@ -0,0 +1,24 @@
+# Preserved outer interface from BT DR pp18-19 / VLA DR pp11-12.
+bt_skill_interfaces/TaskTrace trace
+string skill
+string instruction
+bt_skill_interfaces/ObjectTarget target
+bt_skill_interfaces/RegionTarget destination
+builtin_interfaces/Duration timeout
+---
+bt_skill_interfaces/ExecutionResult result
+string execution_record_ref
+---
+uint8 PREPARING=0
+uint8 WAITING_OBSERVATION=1
+uint8 INFERRING=2
+uint8 EXECUTING=3
+uint8 COMPLETING=4
+uint8 STOPPING=5
+builtin_interfaces/Time stamp
+uint32 sequence
+uint8 phase
+bool progress_valid
+float32 progress
+builtin_interfaces/Duration elapsed_time
+string message
@@ -0,0 +1,18 @@
+# New v1: posture_id is a deployment whitelist key, never arbitrary joints/poses.
+bt_skill_interfaces/TaskTrace trace
+string posture_id
+uint64 expected_geometry_epoch
+builtin_interfaces/Duration timeout
+---
+bt_skill_interfaces/ExecutionResult result
+uint64 geometry_epoch
+bt_skill_interfaces/RobotState robot_state
+---
+uint8 CHECKING=0
+uint8 MOVING=1
+uint8 SETTLING=2
+uint8 STOPPING=3
+builtin_interfaces/Time stamp
+uint32 sequence
+uint8 phase
+string message
@@ -0,0 +1,14 @@
+# New v1 coordinator -> fixed-tree executor contract. Strict-validate JSON before acceptance.
+bt_skill_interfaces/TaskTrace trace
+string approved_plan_json
+string context_json
+builtin_interfaces/Duration timeout
+---
+bt_skill_interfaces/ExecutionResult result
+uint32 completed_quantity
+string evidence_json
+---
+builtin_interfaces/Time stamp
+uint32 sequence
+string stage
+string status_json
@@ -0,0 +1,46 @@
+# BT DR pp15-17; explicit optional-point/error-bound validity bits added in v1.
+string task_id
+string subtask_id
+string target_ref
+string target_description
+string shelf_id
+string column_id
+string tier_id
+string station_binding_ref
+builtin_interfaces/Time capture_after
+uint64 expected_geometry_epoch
+builtin_interfaces/Duration timeout
+---
+uint8 SUCCEEDED=0
+uint8 FAILED=1
+uint8 NOT_FOUND=2
+uint8 AMBIGUOUS=3
+uint8 RGBD=0
+uint8 MODEL_ESTIMATE=1
+uint8 FUSED=2
+uint8 status
+string target_ref
+string grasp_region_ref
+geometry_msgs/PointStamped target_point
+geometry_msgs/PointStamped grasp_point
+bool grasp_point_valid
+uint8 measurement_source
+bool geometry_valid
+string quality_code
+float32 position_error_bound
+bool position_error_bound_valid
+string observation_id
+builtin_interfaces/Time rgb_stamp
+builtin_interfaces/Time depth_stamp
+string calibration_id
+uint64 geometry_epoch
+string error_code
+string message
+string record_ref
+---
+uint8 CAPTURING=0
+uint8 PROCESSING=1
+builtin_interfaces/Time stamp
+uint32 sequence
+uint8 phase
+string message
@@ -0,0 +1,33 @@
+# BT DR p15; numeric values and phase are new v1.
+string task_id
+string subtask_id
+string target_ref
+string target_description
+string source_region_ref
+string observation_station_id
+uint32 station_registry_version
+builtin_interfaces/Time capture_after
+builtin_interfaces/Duration timeout
+---
+uint8 SUCCEEDED=0
+uint8 FAILED=1
+uint8 NOT_FOUND=2
+uint8 AMBIGUOUS=3
+uint8 status
+string shelf_id
+string side_id
+string column_id
+string tier_id
+float32 confidence
+string observation_id
+builtin_interfaces/Time observed_at
+string error_code
+string message
+string record_ref
+---
+uint8 CAPTURING=0
+uint8 PROCESSING=1
+builtin_interfaces/Time stamp
+uint32 sequence
+uint8 phase
+string message
@@ -0,0 +1,33 @@
+# Preserved outer interface from BT DR pp13-14. Canonical ROS2 type: Navigate.
+bt_skill_interfaces/TaskTrace trace
+geometry_msgs/PoseStamped target_pose
+float64 position_tolerance
+float64 orientation_tolerance
+builtin_interfaces/Duration timeout
+---
+bt_skill_interfaces/ExecutionResult result
+bool pose_valid
+geometry_msgs/PoseStamped final_pose
+bool errors_valid
+float64 final_position_error
+float64 final_orientation_error
+---
+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
+bool blocked
+builtin_interfaces/Duration elapsed_time
+string message
@@ -0,0 +1,24 @@
+# v1.1 navigation-owned lookup; no model-supplied pose.
+bt_skill_interfaces/TaskTrace trace
+string kind
+string reference
+string shelf_id
+string side_id
+string column_id
+string tier_id
+uint32 registry_version
+float32 position_tolerance
+float32 orientation_tolerance
+builtin_interfaces/Duration timeout
+---
+bt_skill_interfaces/ExecutionResult result
+geometry_msgs/PoseStamped final_pose
+bool pose_valid
+float32 final_position_error
+float32 final_orientation_error
+bool errors_valid
+---
+builtin_interfaces/Time stamp
+uint32 sequence
+uint8 phase
+string message
@@ -0,0 +1,26 @@
+# Field names/status labels from BT DR p14; numeric values and phase are new v1.
+string task_id
+uint32 task_revision
+uint64 planning_generation
+string instruction
+string known_info_json
+string context_snapshot_json
+string constraints_json
+builtin_interfaces/Duration timeout
+---
+uint8 PLAN_READY=0
+uint8 NEEDS_CLARIFICATION=1
+uint8 FAILED=2
+uint8 status
+string task_plan_json
+string error_code
+string message
+string planning_record_ref
+---
+uint8 PREPARING=0
+uint8 PLANNING=1
+uint8 VALIDATING=2
+builtin_interfaces/Time stamp
+uint32 sequence
+uint8 phase
+string message
@@ -0,0 +1,21 @@
+# New v1: independent observations, never inferred solely from action COMPLETED.
+uint8 PRECHECK=0
+uint8 PICK=1
+uint8 TRANSPORT=2
+uint8 PLACE=3
+uint8 STOPPED=4
+bt_skill_interfaces/TaskTrace trace
+uint8 check
+string source_goal_id
+bt_skill_interfaces/ObjectTarget target
+bt_skill_interfaces/RegionTarget destination
+uint64 expected_geometry_epoch
+builtin_interfaces/Time capture_after
+builtin_interfaces/Duration timeout
+---
+bt_skill_interfaces/VerificationEvidence evidence
+---
+builtin_interfaces/Time stamp
+uint32 sequence
+uint8 phase
+string message
@@ -0,0 +1,9 @@
+# Advisory only. Cannot constitute VerifyState or delivery proof.
+bt_skill_interfaces/TaskTrace trace
+builtin_interfaces/Time observed_at
+uint32 sequence
+string state
+float32 progress
+bool progress_valid
+string hop_json
+string record_ref
@@ -0,0 +1,14 @@
+# New v1 numbers and layout. COMPLETED is execution completion, not business success.
+uint8 COMPLETED=0
+uint8 FAILED=1
+uint8 CANCELED=2
+uint8 TIMED_OUT=3
+uint8 REJECTED=4
+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
@@ -0,0 +1,2 @@
+string object_ref
+string description
@@ -0,0 +1,8 @@
+uint32 schema_version
+bt_skill_interfaces/TaskTrace trace
+string source_goal_id
+uint64 geometry_epoch
+builtin_interfaces/Time observed_at
+builtin_interfaces/Time valid_until
+string writer
+string observation_id
@@ -0,0 +1,8 @@
+bt_skill_interfaces/ObservationContext context
+bt_skill_interfaces/RegionTarget destination
+string placement_region_ref
+geometry_msgs/PointStamped placement_point
+bool placement_point_valid
+geometry_msgs/PoseStamped placement_pose
+bool placement_pose_valid
+bool geometry_valid
@@ -0,0 +1,2 @@
+string region_ref
+string description
@@ -0,0 +1,18 @@
+uint8 EMPTY=0
+uint8 HOLDING_TARGET=1
+uint8 HOLDING_OTHER=2
+uint8 HOLDING_UNKNOWN=3
+string robot_id
+builtin_interfaces/Time stamp
+builtin_interfaces/Time valid_until
+uint64 geometry_epoch
+bool base_stopped_valid
+bool base_stopped
+bool posture_settled_valid
+bool posture_settled
+uint8 holding_state
+string held_target_ref
+string posture_id
+geometry_msgs/PoseStamped pose
+bool pose_valid
+string evidence_ref
@@ -0,0 +1,9 @@
+string robot_id
+builtin_interfaces/Time stamp
+builtin_interfaces/Time valid_until
+bool safety_valid
+bool motion_allowed
+bool emergency_stop_active
+bool protective_stop_active
+string error_code
+string evidence_ref
@@ -0,0 +1,10 @@
+uint32 registry_version
+string station_id
+string region_ref
+string shelf_id
+string side_id
+string column_id
+string station_type
+geometry_msgs/PoseStamped pose
+float64 position_tolerance
+float64 orientation_tolerance
@@ -0,0 +1,13 @@
+bt_skill_interfaces/ObservationContext context
+bt_skill_interfaces/ObjectTarget target
+string grasp_region_ref
+geometry_msgs/PointStamped target_point
+geometry_msgs/PointStamped grasp_point
+bool grasp_point_valid
+bool geometry_valid
+string station_binding_ref
+string shelf_id
+string side_id
+string column_id
+string tier_id
+string calibration_id
@@ -0,0 +1,8 @@
+# New v1 shared layout; business identity does not replace the ROS Goal UUID.
+string task_id
+string subtask_id
+uint32 attempt
+uint32 task_revision
+uint32 plan_version
+string run_id
+uint64 execution_generation
@@ -0,0 +1,26 @@
+uint8 PASSED=0
+uint8 FAILED=1
+uint8 UNKNOWN=2
+uint8 EMPTY=0
+uint8 HOLDING_TARGET=1
+uint8 HOLDING_OTHER=2
+uint8 HOLDING_UNKNOWN=3
+bt_skill_interfaces/ObservationContext context
+uint8 status
+string target_ref
+string destination_ref
+uint8 holding_state
+bool target_match_valid
+bool target_match
+bool grasp_stable_valid
+bool grasp_stable
+bool hand_empty_valid
+bool hand_empty
+bool target_in_destination_valid
+bool target_in_destination
+bool stopped_valid
+bool stopped
+string source
+string evidence_ref
+string error_code
+string message
@@ -0,0 +1,9 @@
+# Local immutable media file; absolute path restricted to configured media_root.
+std_msgs/Header header
+string observation_id
+string image_path
+string station_id
+uint32 registry_version
+string shelf_id
+string calibration_id
+uint64 geometry_epoch
@@ -0,0 +1,16 @@
+<?xml version="1.2.0"?>
+<package format="3">
+  <name>bt_skill_interfaces</name>
+  <version>1.2.0</version>
+  <description>Candidate v1 robot behavior-tree skill and evidence contracts.</description>
+  <maintainer email="feiyuwang1998@gmail.com">wangfeiyu</maintainer>
+  <license>Proprietary</license>
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <buildtool_depend>rosidl_default_generators</buildtool_depend>
+  <depend>builtin_interfaces</depend>
+  <depend>geometry_msgs</depend>
+  <depend>std_msgs</depend>
+  <exec_depend>rosidl_default_runtime</exec_depend>
+  <member_of_group>rosidl_interface_packages</member_of_group>
+  <export><build_type>ament_cmake</build_type></export>
+</package>
@@ -0,0 +1,7 @@
+string robot_id
+---
+bool available
+bt_skill_interfaces/RobotState robot_state
+bt_skill_interfaces/SafetyState safety_state
+string error_code
+string message
@@ -0,0 +1,11 @@
+# New v1; exact persisted task/run/goal plus fresh independently sourced evidence.
+# Caller authentication is required outside this IDL. No confirm_stopped bypass.
+bt_skill_interfaces/TaskTrace trace
+string goal_id
+bt_skill_interfaces/VerificationEvidence evidence
+string operator_id
+string reason
+---
+bool accepted
+string error_code
+string message