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

ros2/bt_executor/CMakeLists.txt

@@ -0,0 +1,25 @@
+cmake_minimum_required(VERSION 3.16)
+project(bt_executor LANGUAGES CXX)
+set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+find_package(ament_cmake REQUIRED)
+find_package(ament_index_cpp REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_action REQUIRED)
+find_package(bt_skill_interfaces REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(behaviortree_cpp 4.10.0 EXACT REQUIRED)
+find_package(nlohmann_json REQUIRED)
+# The repository includes a ROS-free core. Both standalone tests and this node
+# compile these exact sources; no second implementation of the workflow exists.
+add_library(robot_bt_core STATIC ../../core/src/core.cpp ../../core/src/workflow.cpp)
+target_include_directories(robot_bt_core PUBLIC ../../core/include)
+add_executable(bt_executor_node src/executor_node.cpp src/ros_driver.cpp)
+target_include_directories(bt_executor_node PRIVATE include)
+target_link_libraries(bt_executor_node robot_bt_core BT::behaviortree_cpp nlohmann_json::nlohmann_json)
+ament_target_dependencies(bt_executor_node ament_index_cpp rclcpp rclcpp_action bt_skill_interfaces geometry_msgs std_msgs)
+target_compile_options(bt_executor_node PRIVATE -Wall -Wextra -Wpedantic)
+install(TARGETS bt_executor_node DESTINATION lib/${PROJECT_NAME})
+install(DIRECTORY trees launch config DESTINATION share/${PROJECT_NAME})
+ament_package()

ros2/bt_executor/config/executor.yaml

@@ -0,0 +1,25 @@
+bt_executor:
+  ros__parameters:
+    execution_enabled: false
+    robot_id: ""
+    allowed_robots: []
+    site_config_file: ""
+    journal_directory: ""
+    minimum_confidence: 0.9
+    observation_lifetime_ms: 2000
+    readiness_timeout_ms: 2000
+    acceptance_timeout_ms: 2000
+    feedback_timeout_ms: 5000
+    skill_timeout_ms: 120000
+    cancel_stop_timeout_ms: 5000
+    max_reobservations: 2
+    max_posture_adjustments: 1
+    navigate_action: skills/navigate
+    execute_manipulation_action: skills/execute_manipulation
+    locate_shelf_column_action: skills/locate_shelf_column
+    localize_target_3d_action: skills/localize_target_3d
+    assess_grasp_action: skills/assess_grasp
+    execute_posture_action: skills/execute_posture
+    verify_state_action: skills/verify_state
+    check_free_space_action: skills/check_free_space
+    execute_task_action: tasks/execute

ros2/bt_executor/include/bt_executor/admission.hpp

@@ -0,0 +1,120 @@
+#pragma once
+#include <bt_executor/ros_driver.hpp>
+#include <set>
+#include <fstream>
+#include <algorithm>
+
+namespace bt_executor {
+inline void require(bool ok,const std::string& message) {if(!ok)throw std::invalid_argument(message);}
+inline json strict_json(const std::string& raw) {
+  require(raw.size()<=262144,"JSON exceeds size bound");
+  std::map<int,std::set<std::string>> keys;
+  return json::parse(raw,[&keys](int depth,json::parse_event_t event,json& value) {
+    if(event==json::parse_event_t::object_start)keys[depth+1].clear();
+    if(event==json::parse_event_t::key && !keys[depth].insert(value.get<std::string>()).second)
+      throw std::invalid_argument("duplicate JSON key");
+    return true;
+  });
+}
+inline void fields(const json& j,const std::set<std::string>& exact) {
+  require(j.is_object(),"expected object");std::set<std::string> got;
+  for(auto it=j.begin();it!=j.end();++it)got.insert(it.key());
+  require(got==exact,"unknown or missing JSON fields");
+}
+inline std::string string_value(const json& j,const char* key,std::size_t max=200) {
+  require(j.contains(key)&&j.at(key).is_string(),std::string("missing string ")+key);
+  auto s=j.at(key).get<std::string>();require(!s.empty()&&s.size()<=max,std::string("invalid string ")+key);return s;
+}
+inline robot_bt::SiteConfig load_site(const json& data) {
+  robot_bt::SiteConfig site;
+  require(data.at("schema_version")==1,"site schema mismatch");
+  require(data.at("locations").is_object(),"site locations required");
+  for(auto it=data.at("locations").begin();it!=data.at("locations").end();++it) {
+    const auto& p=it.value();
+    robot_bt::Pose pose;pose.frame_id=string_value(p,"frame_id");
+    pose.x=p.at("x").get<double>();pose.y=p.at("y").get<double>();pose.z=p.value("z",0.0);
+    pose.qx=p.value("qx",0.0);pose.qy=p.value("qy",0.0);pose.qz=p.value("qz",0.0);pose.qw=p.value("qw",1.0);
+    require(robot_bt::valid_pose(pose),"invalid registered pose");site.locations.emplace(it.key(),pose);
+  }
+  site.parking_locations=data.at("parking_locations").get<std::map<std::string,std::string>>();
+  site.allowed_postures=data.at("allowed_postures").get<std::vector<std::string>>();
+  site.transport_posture=string_value(data,"transport_posture");
+  require(std::find(site.allowed_postures.begin(),site.allowed_postures.end(),site.transport_posture)!=site.allowed_postures.end(),"transport posture not allowed");
+  for(const auto& kv:site.parking_locations)require(site.locations.count(kv.second),"unknown registered parking location");
+  for(const auto& name:{"object_locations","object_postures","cell_locations","cell_postures"})if(data.contains(name))require(data.at(name).is_object(),"lookup table must be object");
+  site.object_locations=data.value("object_locations",std::map<std::string,std::string>{});
+  site.object_postures=data.value("object_postures",std::map<std::string,std::string>{});
+  site.cell_locations=data.value("cell_locations",std::map<std::string,std::string>{});
+  site.cell_postures=data.value("cell_postures",std::map<std::string,std::string>{});
+  for(const auto* table:{&site.object_locations,&site.cell_locations})for(const auto& kv:*table)require(site.locations.count(kv.second),"unknown lookup location");
+  for(const auto* table:{&site.object_postures,&site.cell_postures})for(const auto& kv:*table)require(std::find(site.allowed_postures.begin(),site.allowed_postures.end(),kv.second)!=site.allowed_postures.end(),"unknown lookup posture");
+  return site;
+}
+inline robot_bt::TaskConfig admit_legacy(const json& p,const json& c,const iface::msg::TaskTrace& trace,
+                                  const json& trusted,const std::string& robot_id) {
+  fields(p,{"schema_version","plan_version","task_type","goal","slots","missing_information","subtasks"});
+  require(p.at("schema_version").is_number_integer()&&p.at("schema_version")==1,"plan schema mismatch");
+  require(p.at("plan_version").is_number_integer()&&p.at("plan_version")==trace.plan_version&&trace.plan_version>0,"plan version mismatch");
+  require(p.at("task_type")=="pick_transport_place","unsupported task type");string_value(p,"goal",4000);
+  require(p.at("missing_information").is_array()&&p.at("missing_information").empty(),"unresolved information");
+  const auto& slots=p.at("slots");fields(slots,{"target_name","quantity","source_location","destination"});
+  require(slots.at("quantity").is_number_integer()&&slots.at("quantity")==1,"quantity must be one");
+  const auto target=string_value(slots,"target_name"),source=string_value(slots,"source_location"),dest=string_value(slots,"destination");
+  const auto& subtasks=p.at("subtasks");require(subtasks.is_array()&&subtasks.size()==6,"fixed six-step chain required");
+  const std::vector<std::string> skills={"NAVIGATE","GROUND_TARGET","PICK","NAVIGATE","CHECK_FREE_SPACE","PLACE"};
+  const std::vector<json> args={{{"destination",source}},{{"target",target}},{{"target",target}},
+    {{"destination",dest}},{{"destination",dest}},{{"target",target},{"destination",dest}}};
+  std::set<std::string> ids;
+  for(std::size_t i=0;i<subtasks.size();++i) {
+    const auto& s=subtasks[i];fields(s,{"id","skill","arguments","depends_on"});
+    auto id=string_value(s,"id",80);require(ids.insert(id).second,"duplicate subtask id");
+    require(s.at("skill")==skills.at(i)&&s.at("arguments")==args.at(i),"unsupported skill/arguments");
+    json deps=json::array();if(i)deps.push_back(subtasks[i-1].at("id"));
+    require(s.at("depends_on")==deps,"unsupported dependencies or cycle");
+  }
+  fields(c,{"schema_version","registry_version","robot_id","target_id","source_shelf","destination_id",
+             "observe_location","destination_location","task_revision"});
+  require(c.at("schema_version")==1&&c.at("registry_version")==trusted.at("registry_version"),"site registry mismatch");
+  require(c.at("robot_id")==robot_id&&c.at("task_revision")==trace.task_revision,"robot/revision mismatch");
+  require(c.at("target_id")==target,"plan/context target mismatch");
+  require(!trace.task_id.empty()&&!trace.run_id.empty()&&trace.task_revision>0&&trace.execution_generation>0&&trace.attempt>0,"invalid trace");
+  require(trusted.at("sources").contains(source)&&trusted.at("destinations").contains(dest),"unregistered site binding");
+  require(c.at("source_shelf")==trusted.at("sources").at(source).at("shelf_id")&&
+          c.at("destination_id")==trusted.at("destinations").at(dest).at("region_ref"),"plan/context binding mismatch");
+  require(c.at("observe_location")==trusted.at("sources").at(source).at("observe_location")&&
+          c.at("destination_location")==trusted.at("destinations").at(dest).at("location"),"registered station mismatch");
+  robot_bt::TaskConfig task;task.trace=trace_core(trace);task.robot_id=robot_id;task.target_id=target;
+  task.source_shelf=string_value(c,"source_shelf");task.destination_id=string_value(c,"destination_id");task.observe_location=string_value(c,"observe_location");
+  task.destination_location=string_value(c,"destination_location");return task;
+}
+inline robot_bt::TaskConfig admit(const json& p,const json& c,const iface::msg::TaskTrace& trace,const json& trusted,const std::string& robot_id) {
+  if(p.value("schema_version",0)==1)return admit_legacy(p,c,trace,trusted,robot_id);
+  fields(p,{"schema_version","plan_version","task_type","goal","route","slots","missing_information","subtasks"});
+  require(p.at("schema_version").is_number_integer()&&p.at("schema_version")==2,"unsupported schema");
+  const auto route=string_value(p,"route");require((route=="OBJECT_TABLE"||route=="SHELF_CELL")&&trusted.at("execution_route")==route,"route policy mismatch");
+  fields(p.at("slots"),{"items","destination"});const auto& items=p.at("slots").at("items");
+  require(items.is_array()&&items.size()==1,"executor accepts one physical item per run");
+  auto slots=items[0];fields(slots,{"target_name","quantity","source_location"});
+  slots["destination"]=p.at("slots").at("destination");
+  const auto target=string_value(slots,"target_name"),source=string_value(slots,"source_location"),dest=string_value(slots,"destination");
+  std::vector<std::string> skills={"NAVIGATE","PICK","NAVIGATE","PLACE"};
+  std::vector<json> args={{{"target",target}},{{"target",target}},{{"destination",dest}},{{"target",target},{"destination",dest}}};
+  if(route=="SHELF_CELL") {
+    skills={"NAVIGATE","ROBOBRAIN_SHELF_LOCALIZE","NAVIGATE","PICK","NAVIGATE","PLACE"};
+    args={{{"source_location",source},{"mode","observation"}},{{"target",target}},{{"source_location",source},{"mode","shelf_cell"}},{{"target",target}},{{"destination",dest}},{{"target",target},{"destination",dest}}};
+  }
+  const auto& steps=p.at("subtasks");require(steps.is_array()&&steps.size()==skills.size(),"invalid fixed v2 steps");std::set<std::string> ids;
+  for(std::size_t i=0;i<steps.size();++i){
+    const auto& step=steps[i];fields(step,{"id","skill","arguments","depends_on"});auto id=string_value(step,"id",80);require(ids.insert(id).second,"duplicate ID");
+    args[i]["item_index"]=0;require(step.at("skill")==skills[i]&&step.at("arguments")==args[i],"invalid fixed v2 arguments");
+    require(step.at("arguments").at("item_index").is_number_integer(),"item index type");
+    auto deps=json::array();if(i)deps.push_back(steps[i-1].at("id"));require(step.at("depends_on")==deps,"invalid v2 dependencies");
+  }
+  require(c.at("route")==route&&c.at("item_index").is_number_unsigned()&&c.at("item_index").get<unsigned>()<20,"invalid item context");
+  auto clean=c;clean.erase("route");clean.erase("item_index");auto legacy=p;legacy.erase("route");legacy["schema_version"]=1;legacy["slots"]=slots;
+  const std::vector<std::string> old_skills={"NAVIGATE","GROUND_TARGET","PICK","NAVIGATE","CHECK_FREE_SPACE","PLACE"};
+  const std::vector<json> old_args={{{"destination",source}},{{"target",target}},{{"target",target}},{{"destination",dest}},{{"destination",dest}},{{"target",target},{"destination",dest}}};
+  legacy["subtasks"]=json::array();for(unsigned i=0;i<6;++i){auto deps=json::array();if(i)deps.push_back("S"+std::to_string(i));legacy["subtasks"].push_back({{"id","S"+std::to_string(i+1)},{"skill",old_skills[i]},{"arguments",old_args[i]},{"depends_on",deps}});}
+  auto task=admit_legacy(legacy,clean,trace,trusted,robot_id);task.route=route;task.item_index=c.at("item_index").get<unsigned>();return task;
+}
+}  // namespace bt_executor

ros2/bt_executor/include/bt_executor/ros_driver.hpp

@@ -0,0 +1,172 @@
+#pragma once
+#include <robot_bt/core.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <rclcpp_action/rclcpp_action.hpp>
+#include <bt_skill_interfaces/action/navigate.hpp>
+#include <bt_skill_interfaces/action/navigate_semantic.hpp>
+#include <bt_skill_interfaces/action/execute_manipulation.hpp>
+#include <bt_skill_interfaces/action/locate_shelf_column.hpp>
+#include <bt_skill_interfaces/action/localize_target3_d.hpp>
+#include <bt_skill_interfaces/action/assess_grasp.hpp>
+#include <bt_skill_interfaces/action/execute_posture.hpp>
+#include <bt_skill_interfaces/action/verify_state.hpp>
+#include <bt_skill_interfaces/action/check_free_space.hpp>
+#include <bt_skill_interfaces/msg/safety_state.hpp>
+#include <bt_skill_interfaces/msg/robot_state.hpp>
+#include <bt_skill_interfaces/msg/target_binding.hpp>
+#include <bt_skill_interfaces/msg/placement_binding.hpp>
+#include <nlohmann/json.hpp>
+#include <functional>
+#include <memory>
+#include <set>
+#include <type_traits>
+#include <cmath>
+
+namespace bt_executor {
+namespace iface = bt_skill_interfaces;
+using json = nlohmann::json;
+robot_bt::RosTime ns(const builtin_interfaces::msg::Time&);
+builtin_interfaces::msg::Time stamp(robot_bt::RosTime);
+iface::msg::TaskTrace trace_msg(const robot_bt::Trace&);
+robot_bt::Trace trace_core(const iface::msg::TaskTrace&);
+robot_bt::NativeStatus native(rclcpp_action::ResultCode);
+
+// All methods and callbacks run on the node's single-threaded executor. Callbacks
+// enqueue immutable request-bound events; only ActiveGoalRegistry advances state.
+class RosDriver final : public robot_bt::GoalDriver {
+ public:
+  RosDriver(rclcpp::Node&, std::string robot_id, std::string uuid_journal,
+            double min_confidence, std::int64_t observation_lifetime_ns,
+            robot_bt::Milliseconds skill_timeout);
+  bool ready(robot_bt::Skill) const override;
+  void send(const robot_bt::GoalRequest&) override;
+  void cancel(const std::string&) override;
+  std::vector<robot_bt::GoalEvent> drain_events() override;
+  robot_bt::SafetySnapshot safety(const std::string& target_id) const;
+  json mappings() const;
+  bool faulted() const { return faulted_; }
+  void bind_task(const robot_bt::TaskConfig& task,const robot_bt::SiteConfig& site,std::uint32_t registry_version) {
+    current_task_=task;allowed_postures_=site.allowed_postures;registry_version_=registry_version;
+  }
+  std::optional<std::uint64_t> geometry_epoch() const;
+
+ private:
+  using Semantic = iface::action::NavigateSemantic;
+  using Navigate = iface::action::Navigate;
+  using Manipulate = iface::action::ExecuteManipulation;
+  using Locate = iface::action::LocateShelfColumn;
+  using Localize = iface::action::LocalizeTarget3D;
+  using Assess = iface::action::AssessGrasp;
+  using Posture = iface::action::ExecutePosture;
+  using Verify = iface::action::VerifyState;
+  using Space = iface::action::CheckFreeSpace;
+  rclcpp::Node& node_;
+  std::string robot_id_, uuid_journal_;
+  double min_confidence_;
+  std::int64_t observation_lifetime_ns_;
+  builtin_interfaces::msg::Duration skill_timeout_;
+  bool faulted_{false};
+  rclcpp_action::Client<Navigate>::SharedPtr navigate_;
+  rclcpp_action::Client<Semantic>::SharedPtr semantic_;
+  rclcpp_action::Client<Manipulate>::SharedPtr manipulate_;
+  rclcpp_action::Client<Locate>::SharedPtr locate_;
+  rclcpp_action::Client<Localize>::SharedPtr localize_;
+  rclcpp_action::Client<Assess>::SharedPtr assess_;
+  rclcpp_action::Client<Posture>::SharedPtr posture_;
+  rclcpp_action::Client<Verify>::SharedPtr verify_;
+  rclcpp_action::Client<Space>::SharedPtr space_;
+  rclcpp::Subscription<iface::msg::SafetyState>::SharedPtr safety_sub_;
+  rclcpp::Subscription<iface::msg::RobotState>::SharedPtr robot_sub_;
+  std::optional<iface::msg::SafetyState> safety_state_;
+  std::optional<iface::msg::RobotState> robot_state_;
+  std::vector<robot_bt::GoalEvent> events_;
+  std::map<std::string, std::function<void()>> cancelers_;
+  std::set<std::string> cancel_intents_;
+  std::map<std::string, json> mappings_;
+  std::map<std::string, json> shelf_bindings_;
+  robot_bt::TaskConfig current_task_;
+  std::vector<std::string> allowed_postures_;
+  std::uint32_t registry_version_{0};
+  rclcpp::Publisher<iface::msg::TargetBinding>::SharedPtr target_pub_;
+  rclcpp::Publisher<iface::msg::PlacementBinding>::SharedPtr placement_pub_;
+  void record_mapping(const robot_bt::GoalRequest&, const rclcpp_action::GoalUUID&);
+  builtin_interfaces::msg::Duration timeout() const { return skill_timeout_; }
+  bool fresh(robot_bt::RosTime observed, robot_bt::RosTime valid_until,
+             robot_bt::RosTime capture_after = 0) const;
+  robot_bt::ExecutionResult execution(const iface::msg::ExecutionResult&,const robot_bt::GoalRequest&) const;
+  robot_bt::ExecutionResult readonly_result(rclcpp_action::ResultCode, bool valid,
+                                           robot_bt::SkillResponse) const;
+  robot_bt::SnapshotMeta meta(const robot_bt::GoalRequest&, robot_bt::RosTime,
+                              robot_bt::RosTime, const std::string&) const;
+
+  template <class Action, class Decode>
+  void send_typed(const typename rclcpp_action::Client<Action>::SharedPtr& client,
+                  typename Action::Goal goal, const robot_bt::GoalRequest& request,
+                  unsigned max_phase, Decode decode) {
+    using Handle = rclcpp_action::ClientGoalHandle<Action>;
+    typename rclcpp_action::Client<Action>::SendGoalOptions options;
+    options.goal_response_callback = [this, client, request](typename Handle::SharedPtr handle) {
+      robot_bt::GoalEvent event;
+      event.goal_id = request.goal_id; event.trace = request.trace;
+      if (!handle) { event.kind = robot_bt::EventKind::REJECTED; events_.push_back(event); return; }
+      // Humble assigns the wire UUID inside async_send_goal; the persisted client
+      // goal_id already exists before send. Record the wire mapping on acceptance.
+      try { record_mapping(request, handle->get_goal_id()); }
+      catch (const std::exception& e) {
+        faulted_ = true; cancel_intents_.insert(request.goal_id);
+        RCLCPP_ERROR(node_.get_logger(), "UUID journal failure: %s", e.what());
+      }
+      cancelers_[request.goal_id] = [this, client, handle, request] {
+        client->async_cancel_goal(handle, [this, request](auto response) {
+          if (!response || response->return_code != 0) return;
+          robot_bt::GoalEvent ack; ack.kind = robot_bt::EventKind::CANCEL_ACK;
+          ack.goal_id = request.goal_id; ack.trace = request.trace; events_.push_back(ack);
+        });
+      };
+      event.kind = robot_bt::EventKind::ACCEPTED; events_.push_back(event);
+      if (cancel_intents_.count(request.goal_id)) cancelers_.at(request.goal_id)();
+    };
+    options.feedback_callback = [this, request, max_phase](typename Handle::SharedPtr handle,
+                                      const std::shared_ptr<const typename Action::Feedback> feedback) {
+      if (!handle || !feedback || feedback->sequence == 0 || feedback->phase > max_phase) return;
+      if constexpr (std::is_same_v<Action, Navigate>) {
+        if(feedback->errors_valid&&(!std::isfinite(feedback->position_error)||feedback->position_error<0||
+          !std::isfinite(feedback->orientation_error)||std::abs(feedback->orientation_error)>std::acos(-1.0)))return;
+        if(feedback->pose_valid) {
+          const auto& p=feedback->current_pose;
+          robot_bt::Pose pose{p.header.frame_id,p.pose.position.x,p.pose.position.y,p.pose.position.z,
+            p.pose.orientation.x,p.pose.orientation.y,p.pose.orientation.z,p.pose.orientation.w};
+          if(!robot_bt::valid_pose(pose))return;
+        }
+      }
+      if constexpr (std::is_same_v<Action, Manipulate>) {
+        if(feedback->progress_valid&&(!std::isfinite(feedback->progress)||feedback->progress<0||feedback->progress>1))return;
+      }
+      if constexpr (std::is_same_v<Action, Navigate>||std::is_same_v<Action, Manipulate>) {
+        if(feedback->elapsed_time.sec<0||feedback->elapsed_time.nanosec>=1000000000)return;
+      }
+      const auto at = ns(feedback->stamp), now = node_.now().nanoseconds();
+      // Feedback from the future, stale feedback, and malformed phases do not
+      // refresh liveness. Phase itself may legitimately move backwards.
+      if (at <= 0 || at > now || now - at > observation_lifetime_ns_) return;
+      robot_bt::GoalEvent event; event.kind = robot_bt::EventKind::FEEDBACK;
+      event.goal_id = request.goal_id; event.trace = request.trace;
+      event.sequence = feedback->sequence; events_.push_back(event);
+    };
+    options.result_callback = [this, request, decode](const typename Handle::WrappedResult& result) {
+      robot_bt::GoalEvent event; event.kind = robot_bt::EventKind::RESULT;
+      event.goal_id = request.goal_id; event.trace = request.trace;
+      event.native_status = native(result.code);
+      if (result.result) {
+        try { event.result = decode(*result.result, result.code); }
+        catch (const std::exception& e) { event.result.detail = std::string("invalid result: ") + e.what(); }
+      }
+      events_.push_back(event);
+      cancelers_.erase(request.goal_id);
+      cancel_intents_.erase(request.goal_id);
+    };
+    // No spin_until_future_complete, wait_for_action_server or blocking get here.
+    (void)client->async_send_goal(goal, options);
+  }
+};
+}  // namespace bt_executor

ros2/bt_executor/launch/executor.launch.py

@@ -0,0 +1,21 @@
+"""Explicit deployment identity/site/journal; motion disabled unless opted in."""
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+from launch_ros.parameter_descriptions import ParameterValue
+
+
+def generate_launch_description():
+    names = [DeclareLaunchArgument('namespace'), DeclareLaunchArgument('robot_id'),
+             DeclareLaunchArgument('site_config_file'), DeclareLaunchArgument('journal_directory'),
+             DeclareLaunchArgument('execution_enabled', default_value='false')]
+    return LaunchDescription(names + [Node(
+        package='bt_executor', executable='bt_executor_node', output='screen',
+        namespace=LaunchConfiguration('namespace'), parameters=[{
+            'robot_id': LaunchConfiguration('robot_id'),
+            'allowed_robots': [[LaunchConfiguration('robot_id')]],
+            'site_config_file': LaunchConfiguration('site_config_file'),
+            'journal_directory': LaunchConfiguration('journal_directory'),
+            'execution_enabled': ParameterValue(LaunchConfiguration('execution_enabled'), value_type=bool),
+        }])])

ros2/bt_executor/package.xml

@@ -0,0 +1,12 @@
+<?xml version="1.2.0"?>
+<package format="3">
+  <name>bt_executor</name><version>1.2.0</version>
+  <description>Fixed BehaviorTree.CPP execution with persistent asynchronous ROS2 goal tracking.</description>
+  <maintainer email="feiyuwang1998@gmail.com">wangfeiyu</maintainer><license>Proprietary</license>
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <depend>rclcpp</depend><depend>rclcpp_action</depend><depend>ament_index_cpp</depend>
+  <depend>bt_skill_interfaces</depend><depend>geometry_msgs</depend><depend>std_msgs</depend>
+  <depend version_eq="4.10.0">behaviortree_cpp</depend><depend>nlohmann_json</depend>
+  <exec_depend>launch_ros</exec_depend><exec_depend>launch</exec_depend>
+  <export><build_type>ament_cmake</build_type></export>
+</package>

ros2/bt_executor/src/executor_node.cpp

@@ -0,0 +1,306 @@
+#include <bt_executor/admission.hpp>
+#include <behaviortree_cpp/bt_factory.h>
+#include <behaviortree_cpp/action_node.h>
+#include <bt_skill_interfaces/action/execute_task.hpp>
+#include <std_msgs/msg/string.hpp>
+#include <ament_index_cpp/get_package_share_directory.hpp>
+#include <filesystem>
+#include <fstream>
+#include <fcntl.h>
+#include <unistd.h>
+#include <cerrno>
+#include <cstdio>
+
+namespace bt_executor {
+using namespace robot_bt;
+struct Runtime {
+  std::unique_ptr<StageRunner> runner;
+  rclcpp::Node* node{nullptr};
+  bool admitted{false}, clarification{false};
+  TickStatus last{TickStatus::RUNNING};
+  std::string stage;
+};
+class StageNode final:public BT::StatefulActionNode {
+ public:
+  StageNode(const std::string& name,const BT::NodeConfig& config):StatefulActionNode(name,config){}
+  static BT::PortsList providedPorts(){return {BT::InputPort<std::string>("stage")};}
+  BT::NodeStatus onStart()override{return step();}
+  BT::NodeStatus onRunning()override{return step();}
+  void onHalted()override {
+    auto runtime=config().blackboard->get<std::shared_ptr<Runtime>>("runtime");
+    if(runtime->runner)runtime->runner->halt(SteadyClock::now());
+  }
+ private:
+  BT::NodeStatus step() {
+    auto runtime=config().blackboard->get<std::shared_ptr<Runtime>>("runtime");
+    auto name=getInput<std::string>("stage");if(!name)throw BT::RuntimeError(name.error());
+    for(auto stage:fixed_stages())if(name.value()==stage_name(stage)) {
+      runtime->stage=name.value();runtime->last=runtime->runner->tick(stage,SteadyClock::now(),runtime->node->now().nanoseconds());
+      switch(runtime->last){case TickStatus::RUNNING:return BT::NodeStatus::RUNNING;
+        case TickStatus::SUCCESS:return BT::NodeStatus::SUCCESS;default:return BT::NodeStatus::FAILURE;}
+    }
+    throw BT::RuntimeError("unknown fixed stage");
+  }
+};
+static void append_receipt(const std::string& path,const json& data) {
+  const auto line=data.dump()+"\n";int fd=::open(path.c_str(),O_WRONLY|O_APPEND|O_CREAT,0600);
+  if(fd<0)throw std::runtime_error("receipt journal unavailable");
+  std::size_t offset=0;while(offset<line.size()) {
+    auto n=::write(fd,line.data()+offset,line.size()-offset);if(n<0&&errno==EINTR)continue;
+    if(n<=0){::close(fd);throw std::runtime_error("receipt journal write failed");}offset+=n;
+  }
+  auto ok=::fsync(fd);::close(fd);if(ok)throw std::runtime_error("receipt journal sync failed");
+}
+class ExecutorNode final:public rclcpp::Node {
+ public:
+  using Action=iface::action::ExecuteTask;
+  using Handle=rclcpp_action::ServerGoalHandle<Action>;
+  ExecutorNode():Node("bt_executor") {
+    robot_id_=declare_parameter<std::string>("robot_id","");
+    auto allowed=declare_parameter<std::vector<std::string>>("allowed_robots",std::vector<std::string>{});
+    enabled_=declare_parameter<bool>("execution_enabled",false);
+    require(!robot_id_.empty()&&std::find(allowed.begin(),allowed.end(),robot_id_)!=allowed.end(),"configured robot_id must be whitelisted");
+    const auto site_path=declare_parameter<std::string>("site_config_file","");
+    require(!site_path.empty(),"trusted site_config_file required");
+    std::ifstream stream(site_path);require(stream.good(),"cannot open trusted site file");
+    const std::string raw((std::istreambuf_iterator<char>(stream)),{});trusted_=strict_json(raw);site_=load_site(trusted_);
+    journal_dir_=declare_parameter<std::string>("journal_directory","");
+    require(!journal_dir_.empty(),"persistent journal_directory required");
+    std::filesystem::create_directories(journal_dir_);
+    const double confidence=declare_parameter<double>("minimum_confidence",0.9);
+    const auto lifetime=declare_parameter<std::int64_t>("observation_lifetime_ms",2000);
+    require(confidence>0&&confidence<=1&&lifetime>0&&lifetime<=10000,"invalid observation policy");
+    const auto timeout_policy=[this](const char* name,std::int64_t default_ms) {
+      // ROS integer parameters reject floating/non-finite values before this
+      // bounded conversion; milliseconds must also fit the outbound Duration.
+      const auto value=declare_parameter<std::int64_t>(name,default_ms);
+      require(value>0&&value<=3600000,std::string(name)+" must be 1..3600000 ms");
+      return Milliseconds(value);
+    };
+    budgets_.readiness=timeout_policy("readiness_timeout_ms",2000);
+    budgets_.acceptance=timeout_policy("acceptance_timeout_ms",2000);
+    budgets_.feedback=timeout_policy("feedback_timeout_ms",5000);
+    budgets_.execution=timeout_policy("skill_timeout_ms",120000);
+    budgets_.cancel_stop=timeout_policy("cancel_stop_timeout_ms",5000);
+    const auto reobservations=declare_parameter<std::int64_t>("max_reobservations",2);
+    const auto adjustments=declare_parameter<std::int64_t>("max_posture_adjustments",1);
+    require(reobservations>=0&&reobservations<=2,"max_reobservations must be 0..2");
+    require(adjustments>=0&&adjustments<=1,"max_posture_adjustments must be 0..1");
+    max_reobservations_=static_cast<unsigned>(reobservations);
+    max_posture_adjustments_=static_cast<unsigned>(adjustments);
+    driver_=std::make_unique<RosDriver>(*this,robot_id_,journal_dir_+"/ros_goal_uuids.jsonl",confidence,lifetime*1000000LL,budgets_.execution);
+    registry_=std::make_unique<ActiveGoalRegistry>(*driver_,journal_dir_+"/goal_registry.log",budgets_);
+    receipt_path_=journal_dir_+"/deliveries.jsonl";
+    task_journal_=journal_dir_+"/task_runs.jsonl";
+    std::ifstream task_history(task_journal_);std::string task_line;
+    while(std::getline(task_history,task_line))if(!task_line.empty()) {
+      const auto record=strict_json(task_line);seen_runs_.insert(record.at("run_id").get<std::string>());
+      faulted_=record.at("state")!="RELEASED";
+    }
+    std::ifstream receipts(receipt_path_);std::string line;
+    while(std::getline(receipts,line))if(!line.empty()) {
+      auto receipt=strict_json(line);receipts_[json::array({receipt.at("task_id"),receipt.at("item_index")}).dump()]=receipt;
+    }
+    factory_.registerNodeType<StageNode>("RunStage");
+    factory_.registerSimpleCondition("ApprovedPlanGate",[](BT::TreeNode& node) {
+      return node.config().blackboard->get<std::shared_ptr<Runtime>>("runtime")->admitted?BT::NodeStatus::SUCCESS:BT::NodeStatus::FAILURE;
+    });
+    factory_.registerSimpleAction("RequestClarification",[](BT::TreeNode& node) {
+      auto rt=node.config().blackboard->get<std::shared_ptr<Runtime>>("runtime");
+      rt->clarification=true;rt->stage="NeedsClarification";return BT::NodeStatus::FAILURE;
+    });
+    // The only XML comes from this installed package. No action field, plan, or
+    // parameter can select or inject another XML file.
+    factory_.registerBehaviorTreeFromFile(ament_index_cpp::get_package_share_directory("bt_executor")+"/trees/fixed_workflow.xml");
+    registry_pub_=create_publisher<std_msgs::msg::String>("goal_registry",rclcpp::QoS(1).reliable().transient_local());
+    server_=rclcpp_action::create_server<Action>(this,declare_parameter<std::string>("execute_task_action","tasks/execute"),
+      [this](const rclcpp_action::GoalUUID&,std::shared_ptr<const Action::Goal> goal) {
+        if(!enabled_||faulted_||active_||reserved_||registry_->robot_locked(robot_id_)||driver_->faulted())return rclcpp_action::GoalResponse::REJECT;
+        try {
+          auto plan=strict_json(goal->approved_plan_json),context=strict_json(goal->context_json);
+          auto task=admit(plan,context,goal->trace,trusted_,robot_id_);
+          require(!receipts_.count(json::array({task.trace.task_id,task.item_index}).dump()),"task already delivered; reconcile receipt without replay");
+          require(!seen_runs_.count(task.trace.run_id),"execution run already dispatched");
+          require(goal->timeout.sec>0&&goal->timeout.sec<=3600&&goal->timeout.nanosec<1000000000,"invalid task timeout");
+          reserved_=true;return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
+        }catch(const std::exception& e){RCLCPP_WARN(get_logger(),"Task rejected: %s",e.what());return rclcpp_action::GoalResponse::REJECT;}
+      },
+      [this](std::shared_ptr<Handle> handle) {
+        if(!active_||handle!=active_)return rclcpp_action::CancelResponse::REJECT;
+        cancel_requested_=true;return rclcpp_action::CancelResponse::ACCEPT;
+      },
+      [this](std::shared_ptr<Handle> handle){accept(std::move(handle));});
+    timer_=create_wall_timer(std::chrono::milliseconds(50),[this]{tick();});
+  }
+ private:
+  std::string robot_id_,journal_dir_,receipt_path_,task_journal_;
+  json trusted_;SiteConfig site_;Budgets budgets_;
+  unsigned max_reobservations_{2},max_posture_adjustments_{1};
+  bool enabled_{false},faulted_{false},reserved_{false},cancel_requested_{false},halting_{false},timed_out_{false};
+  std::unique_ptr<RosDriver> driver_;
+  std::unique_ptr<ActiveGoalRegistry> registry_;
+  std::unique_ptr<ContextStore> context_;
+  std::shared_ptr<Runtime> runtime_;
+  BT::BehaviorTreeFactory factory_;
+  std::optional<BT::Tree> tree_;
+  std::shared_ptr<Handle> active_;
+  rclcpp_action::Server<Action>::SharedPtr server_;
+  rclcpp::Publisher<std_msgs::msg::String>::SharedPtr registry_pub_;
+  rclcpp::TimerBase::SharedPtr timer_;
+  std::map<std::string,json> receipts_;
+  std::set<std::string> seen_runs_;
+  unsigned quantity_{0};std::uint32_t sequence_{0};std::uint64_t tick_count_{0};
+  SteadyTime deadline_{};
+  std::string requested_status_,detail_,active_receipt_key_;
+  void accept(std::shared_ptr<Handle> handle) {
+    active_=std::move(handle);reserved_=false;cancel_requested_=false;halting_=false;timed_out_=false;
+    quantity_=0;sequence_=0;active_receipt_key_.clear();detail_.clear();requested_status_.clear();
+    try {
+      const auto goal=active_->get_goal();auto task=admit(strict_json(goal->approved_plan_json),strict_json(goal->context_json),goal->trace,trusted_,robot_id_);
+      const auto epoch=driver_->geometry_epoch();require(epoch.has_value(),"fresh RobotState geometry epoch required");
+      active_receipt_key_=json::array({task.trace.task_id,task.item_index}).dump();
+      task.initial_geometry_epoch=*epoch;
+      task.max_reobservations=max_reobservations_;
+      task.max_posture_adjustments=max_posture_adjustments_;
+      append_receipt(task_journal_,{{"task_id",task.trace.task_id},{"run_id",task.trace.run_id},{"state","ACTIVE"}});
+      seen_runs_.insert(task.trace.run_id);
+      driver_->bind_task(task,site_,trusted_.at("registry_version").get<std::uint32_t>());
+      deadline_=SteadyClock::now()+std::chrono::seconds(goal->timeout.sec)+std::chrono::nanoseconds(goal->timeout.nanosec);
+      context_=std::make_unique<ContextStore>();runtime_=std::make_shared<Runtime>();runtime_->node=this;runtime_->admitted=true;
+      runtime_->runner=std::make_unique<StageRunner>(task,site_,*driver_,*registry_,*context_,
+        [this,task](const std::string& id,unsigned item,const std::string& verification) {
+          if(item!=task.item_index||id!=task.trace.task_id||verification.empty())return false;
+          if(!receipts_.count(active_receipt_key_)) {
+            const auto* record=registry_->find(verification);
+            if(!record||!record->result||!record->result->response.evidence||
+               !record->result->response.verified||!record->result->response.in_destination||
+               record->result->response.holding!=Holding::EMPTY)return false;
+            const auto& proof=record->result->response;
+            json receipt={{"task_id",id},{"item_index",item},{"run_id",task.trace.run_id},
+              {"verification_goal_id",verification},{"target_id",task.target_id},{"destination_id",task.destination_id},
+              {"verified_at_ns",now().nanoseconds()},{"evidence_id",verification},{"target_ref",proof.target_id},
+              {"destination_ref",proof.destination_id},{"passed",proof.verified},{"empty_hand",proof.holding==Holding::EMPTY},
+              {"in_destination",proof.in_destination},{"valid",proof.valid},
+              {"observed_at_ns",proof.evidence->observed_at},{"valid_until_ns",proof.evidence->valid_until}};
+            append_receipt(receipt_path_,receipt);receipts_[active_receipt_key_]=receipt;
+          }
+          quantity_=1;return true;
+        },budgets_);
+      auto blackboard=BT::Blackboard::create();blackboard->set("runtime",runtime_);
+      tree_.emplace(factory_.createTree("TaskRoot",blackboard));
+    }catch(const std::exception& e){detail_=e.what();finish("INTERVENTION_REQUIRED",false);}
+  }
+  void begin_halt(const std::string& status) {
+    if(halting_)return;halting_=true;requested_status_=status;
+    if(tree_)tree_->haltTree();if(runtime_&&runtime_->runner)runtime_->runner->halt(SteadyClock::now());
+  }
+  void finish(const std::string& requested_status,bool registry_stop_confirmed) {
+    if(!active_)return;
+    SafetySnapshot state;
+    try {
+      const auto config=strict_json(active_->get_goal()->context_json);
+      state=driver_->safety(config.at("target_id").get<std::string>());
+    }catch(const std::exception& e){detail_=std::string("invalid final context: ")+e.what();}
+    const bool stop_confirmed=registry_stop_confirmed&&state.stationary;
+    const bool physical_release=stop_confirmed&&state.safe&&state.holding==Holding::EMPTY&&
+      runtime_&&runtime_->runner&&runtime_->runner->empty_verified(now().nanoseconds());
+    std::string status=physical_release?requested_status:"INTERVENTION_REQUIRED";
+    if(!physical_release&&detail_.empty())detail_="fresh final empty-hand/stationary/safe evidence missing";
+    const auto id=active_->get_goal()->trace.task_id;
+    json evidence={{"status",status},{"stop_confirmed",stop_confirmed},{"completed_quantity",quantity_},
+      {"detail",detail_},{"goal_uuid_mappings",driver_->mappings()},
+      {"safe_to_release",physical_release&&status!="INTERVENTION_REQUIRED"},
+      {"current_empty_hand",state.holding==Holding::EMPTY},{"current_stationary",state.stationary}};
+    if(receipts_.count(active_receipt_key_)) {
+      const auto& receipt=receipts_.at(active_receipt_key_);evidence["delivery"]=receipt;
+      for(const auto* key:{"evidence_id","target_ref","destination_ref","passed","empty_hand","in_destination","valid"})
+        if(receipt.contains(key))evidence[key]=receipt.at(key);
+    } else {evidence["empty_hand"]=physical_release;evidence["valid"]=physical_release;}
+    if(runtime_&&runtime_->clarification)evidence["needs_clarification"]=true;
+    const bool release=physical_release&&status!="INTERVENTION_REQUIRED";
+    try {
+      append_receipt(task_journal_,{{"task_id",id},{"run_id",active_->get_goal()->trace.run_id},
+        {"state",release?"RELEASED":"QUARANTINED"},{"status",status}});
+    }catch(const std::exception& e) {
+      faulted_=true;status="INTERVENTION_REQUIRED";evidence["journal_error"]=e.what();
+      evidence["status"]=status;evidence["safe_to_release"]=false;
+    }
+    if(!release)faulted_=true;
+    auto result=std::make_shared<Action::Result>();
+    result->result.status=status=="SUCCEEDED"?0:status=="CANCELED"?2:timed_out_?3:1;
+    result->result.stop_state=stop_confirmed?1:0;result->result.message=detail_;
+    result->result.error_code=status;result->completed_quantity=quantity_;
+    if(stop_confirmed){result->result.stopped_at=now();result->result.stop_evidence_ref="registry/"+active_->get_goal()->trace.run_id;}
+    result->evidence_json=evidence.dump();
+    if(status=="SUCCEEDED")active_->succeed(result);
+    else if(status=="CANCELED"&&active_->is_canceling())active_->canceled(result);
+    else active_->abort(result);
+    tree_.reset();runtime_.reset();context_.reset();active_.reset();reserved_=false;
+    // registry_ and driver_ survive the tree, including STOP_UNKNOWN entries.
+  }
+  void publish_registry() {
+    json report={{"robot_id",robot_id_},{"locked",registry_->robot_locked(robot_id_)},{"faulted",faulted_||driver_->faulted()},
+                 {"uuid_mappings",driver_->mappings()},{"goals",json::array()}};
+    for(const auto& entry:registry_->records()) {
+      const auto& r=entry.second;report["goals"].push_back({{"goal_id",entry.first},{"task_id",r.request.trace.task_id},
+        {"run_id",r.request.trace.run_id},{"skill",r.request.skill==Skill::PICK?"pick":r.request.skill==Skill::PLACE?"place":"other"},
+        {"target_id",r.request.target_id},{"capture_after_ns",r.request.capture_after},
+        {"trace",{{"task_id",r.request.trace.task_id},{"subtask_id",r.request.trace.subtask_id},{"run_id",r.request.trace.run_id},{"task_revision",r.request.trace.task_revision},{"plan_version",r.request.trace.plan_version},{"execution_generation",r.request.trace.execution_generation},{"attempt",r.request.trace.attempt}}},
+        {"state",static_cast<int>(r.state)},{"cancel_intent",r.cancel_intent},
+        {"accepted",r.accepted},{"restarted",r.restarted}});
+    }
+    std_msgs::msg::String m;m.data=report.dump();registry_pub_->publish(m);
+  }
+  void tick() {
+    const auto steady=SteadyClock::now();
+    try {
+      registry_->pump(steady);if(++tick_count_%20==0)publish_registry();
+      if(!active_)return;
+      if(cancel_requested_)begin_halt("CANCELED");
+      if(steady>=deadline_&&!halting_){timed_out_=true;detail_="task execution deadline";begin_halt("FAILED");}
+      if(driver_->faulted()){detail_="driver durability fault";begin_halt("INTERVENTION_REQUIRED");}
+      if(!halting_) {
+        const auto config=strict_json(active_->get_goal()->context_json);
+        runtime_->runner->update_safety(driver_->safety(config.at("target_id").get<std::string>()));
+        const auto state=tree_->tickOnce();
+        if(state==BT::NodeStatus::SUCCESS){detail_=runtime_->runner->detail();begin_halt(quantity_==1?"SUCCEEDED":"FAILED");}
+        else if(state==BT::NodeStatus::FAILURE) {
+          detail_=runtime_->runner->detail();
+          if(runtime_->clarification)detail_="execution requires clarification; replan before motion";
+          begin_halt(runtime_->last==TickStatus::INTERVENTION_REQUIRED?"INTERVENTION_REQUIRED":"FAILED");
+        }
+      }
+      if(active_) {
+        auto feedback=std::make_shared<Action::Feedback>();feedback->stamp=now();feedback->sequence=++sequence_;
+        feedback->stage=halting_?"Stopping":runtime_->stage;
+        feedback->status_json=json({{"cancel_requested",cancel_requested_},{"completed_quantity",quantity_},
+          {"registry_locked",registry_->robot_locked(robot_id_)},{"detail",detail_}}).dump();active_->publish_feedback(feedback);
+      }
+      if(halting_) {
+        bool unknown=false;for(const auto& kv:registry_->records())if(kv.second.request.robot_id==robot_id_&&kv.second.state==GoalState::STOP_UNKNOWN)unknown=true;
+        if(unknown)finish("INTERVENTION_REQUIRED",false);
+        else if(runtime_&&runtime_->runner) {
+          const auto config=strict_json(active_->get_goal()->context_json);
+          runtime_->runner->update_safety(driver_->safety(config.at("target_id").get<std::string>()));
+          const auto settled=runtime_->runner->settle(steady,now().nanoseconds());
+          if(settled==TickStatus::SUCCESS)finish(requested_status_,true);
+          else if(settled!=TickStatus::RUNNING)finish("INTERVENTION_REQUIRED",!registry_->robot_locked(robot_id_));
+        } else if(!registry_->robot_locked(robot_id_))finish("INTERVENTION_REQUIRED",true);
+      }
+    }catch(const std::exception& e) {
+      faulted_=true;detail_=std::string("executor fault: ")+e.what();
+      try{begin_halt("INTERVENTION_REQUIRED");}catch(...){}
+      if(active_)finish("INTERVENTION_REQUIRED",false);
+      RCLCPP_ERROR(get_logger(),"%s",detail_.c_str());
+    }
+  }
+};
+}  // namespace bt_executor
+int main(int argc,char** argv) {
+  rclcpp::init(argc,argv);
+  try {
+    auto node=std::make_shared<bt_executor::ExecutorNode>();
+    rclcpp::executors::SingleThreadedExecutor executor;executor.add_node(node);executor.spin();
+  }catch(const std::exception& e){std::fprintf(stderr,"bt_executor startup failed: %s\n",e.what());rclcpp::shutdown();return 1;}
+  rclcpp::shutdown();return 0;
+}

ros2/bt_executor/src/ros_driver.cpp

@@ -0,0 +1,326 @@
+#include <bt_executor/ros_driver.hpp>
+#include <geometry_msgs/msg/pose_stamped.hpp>
+#include <fcntl.h>
+#include <unistd.h>
+#include <cerrno>
+#include <cmath>
+#include <iomanip>
+#include <fstream>
+#include <sstream>
+#include <stdexcept>
+
+namespace bt_executor {
+using namespace robot_bt;
+RosTime ns(const builtin_interfaces::msg::Time& t) { return std::int64_t(t.sec) * 1000000000LL + t.nanosec; }
+builtin_interfaces::msg::Time stamp(RosTime n) {
+  builtin_interfaces::msg::Time t; t.sec = static_cast<std::int32_t>(n / 1000000000LL);
+  t.nanosec = static_cast<std::uint32_t>(n % 1000000000LL); return t;
+}
+iface::msg::TaskTrace trace_msg(const Trace& t) {
+  iface::msg::TaskTrace m; m.task_id=t.task_id; m.subtask_id=t.subtask_id; m.run_id=t.run_id;
+  m.attempt=t.attempt; m.task_revision=t.task_revision; m.plan_version=t.plan_version;
+  m.execution_generation=t.execution_generation; return m;
+}
+Trace trace_core(const iface::msg::TaskTrace& m) {
+  Trace t; t.task_id=m.task_id; t.subtask_id=m.subtask_id; t.run_id=m.run_id;
+  t.attempt=m.attempt; t.task_revision=m.task_revision; t.plan_version=m.plan_version;
+  t.execution_generation=m.execution_generation; return t;
+}
+NativeStatus native(rclcpp_action::ResultCode c) {
+  switch(c) {case rclcpp_action::ResultCode::SUCCEEDED:return NativeStatus::SUCCEEDED;
+    case rclcpp_action::ResultCode::ABORTED:return NativeStatus::ABORTED;
+    case rclcpp_action::ResultCode::CANCELED:return NativeStatus::CANCELED;
+    default:return NativeStatus::UNKNOWN;}
+}
+static Holding holding(std::uint8_t s) {
+  switch(s) {case 0:return Holding::EMPTY; case 1:return Holding::HOLDING_TARGET;
+    case 2:return Holding::HOLDING_OTHER; default:return Holding::UNKNOWN;}
+}
+static Pose pose_core(const geometry_msgs::msg::PoseStamped& m) {
+  return {m.header.frame_id,m.pose.position.x,m.pose.position.y,m.pose.position.z,
+          m.pose.orientation.x,m.pose.orientation.y,m.pose.orientation.z,m.pose.orientation.w};
+}
+static geometry_msgs::msg::PoseStamped pose_msg(const Pose& p, RosTime now) {
+  geometry_msgs::msg::PoseStamped m; m.header.frame_id=p.frame_id; m.header.stamp=stamp(now);
+  m.pose.position.x=p.x; m.pose.position.y=p.y; m.pose.position.z=p.z;
+  m.pose.orientation.x=p.qx;m.pose.orientation.y=p.qy;m.pose.orientation.z=p.qz;m.pose.orientation.w=p.qw; return m;
+}
+static Pose point_core(const geometry_msgs::msg::PointStamped& p) {
+  // Carrier for observed position only. This quaternion is never sent as a
+  // manipulator command; legacy VLA goals carry semantic ObjectTarget/RegionTarget.
+  return {p.header.frame_id,p.point.x,p.point.y,p.point.z,0,0,0,1};
+}
+static iface::msg::ObservationContext context_msg(const SnapshotMeta& v) {
+  iface::msg::ObservationContext c;c.schema_version=v.schema_version;c.trace=trace_msg(v.trace);
+  c.source_goal_id=v.source_goal_id;c.geometry_epoch=v.geometry_epoch;c.observed_at=stamp(v.observed_at);
+  c.valid_until=stamp(v.valid_until);c.writer=v.writer;return c;
+}
+static SnapshotMeta context_core(const iface::msg::ObservationContext& c) {
+  SnapshotMeta m;m.schema_version=c.schema_version;m.trace=trace_core(c.trace);
+  m.source_goal_id=c.source_goal_id;m.geometry_epoch=c.geometry_epoch;m.observed_at=ns(c.observed_at);
+  m.valid_until=ns(c.valid_until);m.writer=c.writer;return m;
+}
+static void durable_append(const std::string& path, const std::string& line) {
+  const int fd=::open(path.c_str(),O_CREAT|O_WRONLY|O_APPEND,0600);
+  if(fd<0)throw std::runtime_error("cannot open UUID journal");
+  std::size_t offset=0;
+  while(offset<line.size()) {auto n=::write(fd,line.data()+offset,line.size()-offset);
+    if(n<0 && errno==EINTR)continue;
+    if(n<=0){::close(fd);throw std::runtime_error("cannot write UUID journal");}offset+=n;}
+  const int status=::fsync(fd);::close(fd);if(status)throw std::runtime_error("cannot sync UUID journal");
+}
+
+RosDriver::RosDriver(rclcpp::Node& n, std::string robot_id, std::string journal,
+                     double confidence, std::int64_t lifetime, Milliseconds skill_timeout)
+  :node_(n),robot_id_(std::move(robot_id)),uuid_journal_(std::move(journal)),
+   min_confidence_(confidence),observation_lifetime_ns_(lifetime) {
+  const auto timeout_ms=skill_timeout.count();
+  if(timeout_ms<=0||timeout_ms>3600000)throw std::invalid_argument("skill timeout must be 1..3600000 ms");
+  skill_timeout_.sec=static_cast<std::int32_t>(timeout_ms/1000);
+  skill_timeout_.nanosec=static_cast<std::uint32_t>((timeout_ms%1000)*1000000);
+  std::ifstream mappings(uuid_journal_);std::string line;
+  while(std::getline(mappings,line))if(!line.empty()) {
+    auto value=json::parse(line);const auto id=value.at("client_goal_id").get<std::string>();
+    if(id.empty()||value.at("ros_goal_uuid").get<std::string>().size()!=32)throw std::runtime_error("corrupt UUID journal");
+    mappings_[id]=std::move(value);
+  }
+  navigate_=rclcpp_action::create_client<Navigate>(&n,n.declare_parameter<std::string>("navigate_action","skills/navigate"));
+  manipulate_=rclcpp_action::create_client<Manipulate>(&n,n.declare_parameter<std::string>("execute_manipulation_action","skills/execute_manipulation"));
+  locate_=rclcpp_action::create_client<Locate>(&n,n.declare_parameter<std::string>("locate_shelf_column_action","skills/locate_shelf_column"));
+  semantic_=rclcpp_action::create_client<Semantic>(&n,n.declare_parameter<std::string>("navigate_semantic_action","skills/navigate_semantic"));
+  localize_=rclcpp_action::create_client<Localize>(&n,n.declare_parameter<std::string>("localize_target_3d_action","skills/localize_target_3d"));
+  assess_=rclcpp_action::create_client<Assess>(&n,n.declare_parameter<std::string>("assess_grasp_action","skills/assess_grasp"));
+  posture_=rclcpp_action::create_client<Posture>(&n,n.declare_parameter<std::string>("execute_posture_action","skills/execute_posture"));
+  verify_=rclcpp_action::create_client<Verify>(&n,n.declare_parameter<std::string>("verify_state_action","skills/verify_state"));
+  space_=rclcpp_action::create_client<Space>(&n,n.declare_parameter<std::string>("check_free_space_action","skills/check_free_space"));
+  safety_sub_=n.create_subscription<iface::msg::SafetyState>("safety_state",rclcpp::QoS(1).reliable(),
+    [this](iface::msg::SafetyState::ConstSharedPtr v){if(v->robot_id==robot_id_)safety_state_=*v;});
+  robot_sub_=n.create_subscription<iface::msg::RobotState>("robot_state",rclcpp::QoS(1).reliable(),
+    [this](iface::msg::RobotState::ConstSharedPtr v){if(v->robot_id==robot_id_)robot_state_=*v;});
+  target_pub_=n.create_publisher<iface::msg::TargetBinding>("context/target_binding",rclcpp::QoS(1).reliable().transient_local());
+  placement_pub_=n.create_publisher<iface::msg::PlacementBinding>("context/placement_binding",rclcpp::QoS(1).reliable().transient_local());
+}
+bool RosDriver::fresh(RosTime at,RosTime until,RosTime after)const {
+  const auto now=node_.now().nanoseconds();
+  return at>0 && at>=after && at<=now && until>now && until>=at &&
+         now-at<=observation_lifetime_ns_;
+}
+SafetySnapshot RosDriver::safety(const std::string& target_id)const {
+  SafetySnapshot s;
+  if(!safety_state_||!robot_state_)return s;
+  const auto& a=*safety_state_;const auto& b=*robot_state_;
+  if(!fresh(ns(a.stamp),ns(a.valid_until))||!fresh(ns(b.stamp),ns(b.valid_until))||
+     a.evidence_ref.empty()||b.evidence_ref.empty())return s;
+  s.safe=a.safety_valid&&a.motion_allowed&&!a.emergency_stop_active&&!a.protective_stop_active&&!faulted_;
+  s.stationary=b.base_stopped_valid&&b.base_stopped&&b.posture_settled_valid&&b.posture_settled;
+  s.holding=holding(b.holding_state);
+  if(s.holding==Holding::HOLDING_TARGET && b.held_target_ref!=target_id)s.holding=Holding::UNKNOWN;
+  s.observed_at=std::min(ns(a.stamp),ns(b.stamp));s.valid_until=std::min(ns(a.valid_until),ns(b.valid_until));return s;
+}
+std::optional<std::uint64_t> RosDriver::geometry_epoch() const {
+  if(!robot_state_||!fresh(ns(robot_state_->stamp),ns(robot_state_->valid_until))||robot_state_->evidence_ref.empty())return std::nullopt;
+  return robot_state_->geometry_epoch;
+}
+bool RosDriver::ready(Skill skill)const {
+  if(faulted_)return false;
+  switch(skill) {
+    case Skill::NAVIGATE:return current_task_.route=="LEGACY"?navigate_->action_server_is_ready():semantic_->action_server_is_ready();
+    case Skill::PICK:case Skill::PLACE:return manipulate_->action_server_is_ready();
+    case Skill::LOCATE_SHELF_COLUMN:return locate_->action_server_is_ready();
+    case Skill::LOCALIZE_TARGET:return localize_->action_server_is_ready();
+    case Skill::EVALUATE_GRASP:return assess_->action_server_is_ready();
+    case Skill::ADJUST_POSTURE:case Skill::TRANSPORT_POSTURE:return posture_->action_server_is_ready();
+    case Skill::VERIFY_EMPTY:case Skill::VERIFY_PICK:case Skill::VERIFY_TRANSPORT:case Skill::VERIFY_PLACE:return verify_->action_server_is_ready();
+    case Skill::CHECK_FREE_SPACE:return space_->action_server_is_ready();
+  }return false;
+}
+void RosDriver::record_mapping(const GoalRequest& r,const rclcpp_action::GoalUUID& uuid) {
+  std::ostringstream hex;for(auto b:uuid)hex<<std::hex<<std::setfill('0')<<std::setw(2)<<unsigned(b);
+  json m={{"client_goal_id",r.goal_id},{"ros_goal_uuid",hex.str()},{"task_id",r.trace.task_id},
+    {"run_id",r.trace.run_id},{"task_revision",r.trace.task_revision},{"plan_version",r.trace.plan_version},
+    {"execution_generation",r.trace.execution_generation},{"subtask_id",r.trace.subtask_id},{"attempt",r.trace.attempt}};
+  durable_append(uuid_journal_,m.dump()+"\n");mappings_[r.goal_id]=m;
+}
+json RosDriver::mappings()const {json j=json::array();for(const auto& kv:mappings_)j.push_back(kv.second);return j;}
+void RosDriver::cancel(const std::string& id) {
+  cancel_intents_.insert(id);auto it=cancelers_.find(id);if(it!=cancelers_.end())it->second();
+}
+std::vector<GoalEvent> RosDriver::drain_events(){std::vector<GoalEvent> v;v.swap(events_);return v;}
+ExecutionResult RosDriver::execution(const iface::msg::ExecutionResult& m,const GoalRequest& request)const {
+  ExecutionResult r;r.detail=m.error_code+": "+m.message;
+  switch(m.status){case 0:r.code=ResultCode::COMPLETED;break;case 1:r.code=ResultCode::FAILED;break;
+    case 2:r.code=ResultCode::CANCELED;break;case 3:r.code=ResultCode::TIMED_OUT;break;
+    case 4:r.code=ResultCode::REJECTED;break;default:return r;}
+  // A stop label without a timestamp and evidence cannot release motion resources.
+  if(m.stop_state==1&&!m.stop_evidence_ref.empty()&&fresh(ns(m.stopped_at),ns(m.stopped_at)+observation_lifetime_ns_,request.capture_after))
+    r.stop=StopState::CONFIRMED;
+  return r;
+}
+ExecutionResult RosDriver::readonly_result(rclcpp_action::ResultCode native_code,bool valid,SkillResponse response)const {
+  ExecutionResult r;r.response=std::move(response);r.response.valid=valid;
+  // These servers are contractually read-only. Their native terminal is enough
+  // for this call's stopping; global fresh RobotState still gates every motion.
+  r.stop=native_code==rclcpp_action::ResultCode::UNKNOWN?StopState::UNKNOWN:StopState::CONFIRMED;
+  if(native_code==rclcpp_action::ResultCode::SUCCEEDED)r.code=ResultCode::COMPLETED;
+  else if(native_code==rclcpp_action::ResultCode::CANCELED)r.code=ResultCode::CANCELED;
+  else r.code=ResultCode::FAILED;
+  return r;
+}
+SnapshotMeta RosDriver::meta(const GoalRequest& r,RosTime at,RosTime until,const std::string& writer)const {
+  SnapshotMeta m;m.trace=r.trace;m.source_goal_id=r.goal_id;m.geometry_epoch=r.geometry_epoch;
+  m.observed_at=at;m.valid_until=until;m.writer=writer;return m;
+}
+void RosDriver::send(const GoalRequest& r) {
+  if(r.robot_id!=robot_id_)throw std::runtime_error("wrong robot namespace");
+  const bool geometry_sensitive=r.skill==Skill::EVALUATE_GRASP||r.skill==Skill::PICK||r.skill==Skill::PLACE||
+    r.skill==Skill::ADJUST_POSTURE||r.skill==Skill::TRANSPORT_POSTURE;
+  if(geometry_sensitive) {
+    const auto epoch=geometry_epoch();
+    if(!epoch||*epoch!=r.geometry_epoch) {
+      // No wire send occurred. This exact attempt is explicitly rejected and
+      // still audited by the registry; no motion may use an old-epoch binding.
+      GoalEvent rejected;rejected.kind=EventKind::REJECTED;rejected.goal_id=r.goal_id;rejected.trace=r.trace;
+      events_.push_back(rejected);return;
+    }
+  }
+  const auto now=node_.now().nanoseconds();
+  switch(r.skill) {
+    case Skill::NAVIGATE: {
+      if(!r.registered_pose)throw std::runtime_error("registered navigation pose required");
+      if(!r.navigation_kind.empty()) {
+        Semantic::Goal g;g.trace=trace_msg(r.trace);g.kind=r.navigation_kind;g.reference=r.navigation_ref;g.shelf_id=r.shelf;g.side_id=r.side;g.column_id=r.column;g.tier_id=r.tier;g.registry_version=registry_version_;g.position_tolerance=r.position_tolerance_m;g.orientation_tolerance=r.orientation_tolerance_rad;g.timeout=timeout();
+        send_typed<Semantic>(semantic_,g,r,6,[this,r](const Semantic::Result& m,auto){
+          auto out=execution(m.result,r);out.response.valid=m.pose_valid&&m.errors_valid&&std::isfinite(m.final_position_error)&&std::isfinite(m.final_orientation_error)&&m.final_position_error>=0&&m.final_position_error<=r.position_tolerance_m&&std::abs(m.final_orientation_error)<=r.orientation_tolerance_rad;
+          if(m.pose_valid)out.response.final_pose=pose_core(m.final_pose);out.response.base_stopped=out.stop==StopState::CONFIRMED;return out;
+        });break;
+      }
+      Navigate::Goal g;g.trace=trace_msg(r.trace);g.target_pose=pose_msg(*r.registered_pose,now);
+      g.position_tolerance=r.position_tolerance_m;g.orientation_tolerance=r.orientation_tolerance_rad;g.timeout=timeout();
+      send_typed<Navigate>(navigate_,g,r,6,[this,r](const Navigate::Result& m,auto){
+        auto out=execution(m.result,r);out.response.valid=m.pose_valid&&m.errors_valid&&
+          std::isfinite(m.final_position_error)&&std::isfinite(m.final_orientation_error)&&
+          m.final_position_error>=0&&std::abs(m.final_orientation_error)<=std::acos(-1.0)&&
+          m.final_position_error<=r.position_tolerance_m&&std::abs(m.final_orientation_error)<=r.orientation_tolerance_rad;
+        if(m.pose_valid)out.response.final_pose=pose_core(m.final_pose);
+        out.response.base_stopped=out.stop==StopState::CONFIRMED;return out;});break;
+    }
+    case Skill::PICK:case Skill::PLACE: {
+      Manipulate::Goal g;g.trace=trace_msg(r.trace);g.skill=r.skill==Skill::PICK?"pick":"place";
+      g.instruction=g.skill+" registered target "+r.target_id;g.target.object_ref=r.target_id;g.target.description=r.target_id;
+      if(r.skill==Skill::PLACE){g.destination.region_ref=r.destination_id;g.destination.description=r.destination_id;}
+      g.timeout=timeout();
+      send_typed<Manipulate>(manipulate_,g,r,5,[this,r](const Manipulate::Result& m,auto){
+        auto out=execution(m.result,r);out.response.valid=!m.execution_record_ref.empty();
+        out.response.base_stopped=out.stop==StopState::CONFIRMED;return out;});break;
+    }
+    case Skill::LOCATE_SHELF_COLUMN: {
+      Locate::Goal g;g.task_id=r.trace.task_id;g.subtask_id=r.trace.subtask_id;g.target_ref=r.target_id;
+      g.target_description=r.target_id;g.observation_station_id=current_task_.observe_location;g.station_registry_version=registry_version_;
+      g.source_region_ref=r.shelf;g.capture_after=stamp(r.capture_after);g.timeout=timeout();
+      send_typed<Locate>(locate_,g,r,1,[this,r](const Locate::Result& m,auto code){
+        SkillResponse out;out.shelf=m.shelf_id;out.side=m.side_id;out.column=m.column_id;out.tier=m.tier_id;
+        bool ok=m.status==0&&std::isfinite(m.confidence)&&m.confidence>=min_confidence_&&m.confidence<=1&&
+          !m.observation_id.empty()&&!m.record_ref.empty()&&!m.shelf_id.empty()&&!m.side_id.empty()&&!m.column_id.empty()&&
+          fresh(ns(m.observed_at),ns(m.observed_at)+observation_lifetime_ns_,r.capture_after);
+        if(ok)shelf_bindings_[r.trace.run_id]={{"shelf",m.shelf_id},{"side",m.side_id},{"column",m.column_id},{"tier",m.tier_id},{"record",m.record_ref}};
+        return readonly_result(code,ok,out);});break;
+    }
+    case Skill::LOCALIZE_TARGET: {
+      Localize::Goal g;g.task_id=r.trace.task_id;g.subtask_id=r.trace.subtask_id;
+      g.target_ref=r.target_id;g.shelf_id=r.shelf;g.capture_after=stamp(r.capture_after);
+      g.target_description=r.target_id;
+      g.expected_geometry_epoch=r.geometry_epoch;g.timeout=timeout();
+      auto binding=shelf_bindings_.find(r.trace.run_id);
+      if(binding!=shelf_bindings_.end()){g.column_id=binding->second.at("column");g.tier_id=binding->second.at("tier");g.station_binding_ref=binding->second.at("record");}
+      send_typed<Localize>(localize_,g,r,1,[this,r](const Localize::Result& m,auto code){
+        SkillResponse out;const auto at=ns(m.target_point.header.stamp);
+        bool ok=m.status==0&&m.geometry_valid&&m.target_ref==r.target_id&&m.geometry_epoch==r.geometry_epoch&&
+          !m.observation_id.empty()&&!m.calibration_id.empty()&&!m.record_ref.empty()&&!m.quality_code.empty()&&m.measurement_source<=2&&
+          m.position_error_bound_valid&&std::isfinite(m.position_error_bound)&&m.position_error_bound>=0&&
+          fresh(at,at+observation_lifetime_ns_,r.capture_after)&&
+          fresh(ns(m.rgb_stamp),ns(m.rgb_stamp)+observation_lifetime_ns_,r.capture_after)&&
+          fresh(ns(m.depth_stamp),ns(m.depth_stamp)+observation_lifetime_ns_,r.capture_after);
+        TargetBinding b;b.meta=meta(r,at,at+observation_lifetime_ns_,"localize_target_3d");
+        b.target_id=m.target_ref;b.pose=point_core(m.target_point);ok=ok&&valid_pose(b.pose);
+        if(m.grasp_point_valid)ok=ok&&valid_pose(point_core(m.grasp_point));
+        if(ok){out.target=b;iface::msg::TargetBinding msg;msg.context=context_msg(b.meta);msg.context.observation_id=m.observation_id;
+          msg.target.object_ref=m.target_ref;msg.target.description=r.target_id;msg.target_point=m.target_point;
+          msg.grasp_point=m.grasp_point;msg.grasp_point_valid=m.grasp_point_valid;msg.grasp_region_ref=m.grasp_region_ref;
+          msg.geometry_valid=true;msg.calibration_id=m.calibration_id;msg.shelf_id=r.shelf;target_pub_->publish(msg);}
+        return readonly_result(code,ok,out);});break;
+    }
+    case Skill::EVALUATE_GRASP: {
+      if(!r.target||!robot_state_)throw std::runtime_error("assess missing binding/state");
+      Assess::Goal g;g.trace=trace_msg(r.trace);g.target_binding.context=context_msg(r.target->meta);
+      g.target_binding.target.object_ref=r.target_id;g.target_binding.target.description=r.target_id;
+      g.target_binding.geometry_valid=true;
+      g.target_binding.target_point.header.frame_id=r.target->pose.frame_id;
+      g.target_binding.target_point.header.stamp=stamp(r.target->meta.observed_at);
+      g.target_binding.target_point.point.x=r.target->pose.x;g.target_binding.target_point.point.y=r.target->pose.y;g.target_binding.target_point.point.z=r.target->pose.z;
+      g.robot_state=*robot_state_;g.allowed_posture_ids=allowed_postures_;g.timeout=timeout();
+      send_typed<Assess>(assess_,g,r,255,[this,r](const Assess::Result& m,auto code){
+        SkillResponse out;out.posture_id=m.posture_id;
+        switch(m.decision){case 0:out.admission=Admission::DIRECT;break;case 1:out.admission=Admission::ADJUST_POSTURE;break;
+          case 2:out.admission=Admission::NOT_REACHABLE;break;default:out.admission=Admission::UNKNOWN;}
+        bool ok=m.decision<=3&&m.geometry_epoch==r.geometry_epoch&&!m.evidence_ref.empty();
+        return readonly_result(code,ok,out);});break;
+    }
+    case Skill::ADJUST_POSTURE:case Skill::TRANSPORT_POSTURE: {
+      Posture::Goal g;g.trace=trace_msg(r.trace);g.posture_id=r.posture_id;
+      g.expected_geometry_epoch=r.geometry_epoch;g.timeout=timeout();
+      send_typed<Posture>(posture_,g,r,3,[this,r](const Posture::Result& m,auto){
+        auto out=execution(m.result,r);const auto& state=m.robot_state;
+        out.response.valid=state.robot_id==robot_id_&&state.posture_id==r.posture_id&&
+          state.posture_settled_valid&&state.posture_settled&&state.base_stopped_valid&&state.base_stopped&&
+          m.geometry_epoch==r.geometry_epoch+1&&state.geometry_epoch==m.geometry_epoch&&
+          !state.evidence_ref.empty()&&fresh(ns(state.stamp),ns(state.valid_until),r.capture_after);
+        out.response.posture_id=state.posture_id;out.response.base_stopped=state.base_stopped_valid&&state.base_stopped;
+        out.response.holding=holding(state.holding_state);return out;});break;
+    }
+    case Skill::VERIFY_EMPTY:case Skill::VERIFY_PICK:case Skill::VERIFY_TRANSPORT:case Skill::VERIFY_PLACE: {
+      Verify::Goal g;g.trace=trace_msg(r.trace);g.source_goal_id=r.goal_id;
+      g.check=r.skill==Skill::VERIFY_EMPTY?0:r.skill==Skill::VERIFY_PICK?1:r.skill==Skill::VERIFY_TRANSPORT?2:3;
+      g.target.object_ref=r.target_id;g.target.description=r.target_id;
+      g.destination.region_ref=r.destination_id;g.destination.description=r.destination_id;
+      g.expected_geometry_epoch=r.geometry_epoch;
+      g.capture_after=stamp(r.capture_after);g.timeout=timeout();
+      send_typed<Verify>(verify_,g,r,255,[this,r](const Verify::Result& m,auto code){
+        const auto& e=m.evidence;SkillResponse out;out.evidence=context_core(e.context);
+        out.target_id=e.target_ref;out.destination_id=e.destination_ref;out.holding=holding(e.holding_state);
+        out.base_stopped=e.stopped_valid&&e.stopped;out.in_destination=e.target_in_destination_valid&&e.target_in_destination;
+        bool ok=e.context.schema_version==1&&same_trace(trace_core(e.context.trace),r.trace)&&
+          e.context.source_goal_id==r.goal_id&&e.context.geometry_epoch==r.geometry_epoch&&
+          !e.context.writer.empty()&&!e.context.observation_id.empty()&&!e.evidence_ref.empty()&&!e.source.empty()&&
+          fresh(ns(e.context.observed_at),ns(e.context.valid_until),r.capture_after)&&e.target_ref==r.target_id;
+        out.verified=e.status==0&&out.base_stopped;
+        if(r.skill!=Skill::VERIFY_EMPTY)out.verified=out.verified&&e.target_match_valid&&e.target_match;
+        if(r.skill==Skill::VERIFY_PICK||r.skill==Skill::VERIFY_TRANSPORT)
+          out.verified=out.verified&&e.grasp_stable_valid&&e.grasp_stable&&out.holding==Holding::HOLDING_TARGET;
+        else if(r.skill==Skill::VERIFY_EMPTY)out.verified=out.verified&&e.hand_empty_valid&&e.hand_empty&&out.holding==Holding::EMPTY;
+        else out.verified=out.verified&&e.destination_ref==r.destination_id&&e.hand_empty_valid&&e.hand_empty&&
+                          out.holding==Holding::EMPTY&&out.in_destination;
+        return readonly_result(code,ok&&e.status<=2,out);});break;
+    }
+    case Skill::CHECK_FREE_SPACE: {
+      Space::Goal g;g.task_id=r.trace.task_id;g.subtask_id=r.trace.subtask_id;g.destination_ref=r.destination_id;
+      g.object_ref=r.target_id;g.object_description=r.target_id;g.destination_description=r.destination_id;
+      g.capture_after=stamp(r.capture_after);g.placement_constraints_json="{}";g.timeout=timeout();
+      send_typed<Space>(space_,g,r,1,[this,r](const Space::Result& m,auto code){
+        SkillResponse out;bool ok=m.status==0&&m.destination_ref==r.destination_id&&m.geometry_valid&&
+          std::isfinite(m.confidence)&&m.confidence>=min_confidence_&&m.confidence<=1&&
+          !m.observation_id.empty()&&!m.record_ref.empty()&&!m.placement_region_ref.empty()&&!m.quality_code.empty()&&
+          fresh(ns(m.observed_at),ns(m.valid_until),r.capture_after)&&(m.placement_pose_valid||m.placement_point_valid);
+        PlacementBinding b;b.meta=meta(r,ns(m.observed_at),ns(m.valid_until),"check_free_space");
+        b.target_id=r.target_id;b.destination_id=m.destination_ref;b.free_space_confirmed=ok;
+        if(m.placement_pose_valid)b.pose=pose_core(m.placement_pose);else if(m.placement_point_valid)b.pose=point_core(m.placement_point);
+        ok=ok&&valid_pose(b.pose);if(ok){out.placement=b;iface::msg::PlacementBinding msg;
+          msg.context=context_msg(b.meta);msg.context.observation_id=m.observation_id;
+          msg.destination.region_ref=m.destination_ref;msg.destination.description=r.destination_id;
+          msg.placement_region_ref=m.placement_region_ref;msg.placement_point=m.placement_point;
+          msg.placement_point_valid=m.placement_point_valid;msg.placement_pose=m.placement_pose;
+          msg.placement_pose_valid=m.placement_pose_valid;msg.geometry_valid=true;placement_pub_->publish(msg);}
+        return readonly_result(code,ok,out);});break;
+    }
+  }
+}
+}  // namespace bt_executor

ros2/bt_executor/tools/build_humble.sh

@@ -0,0 +1,15 @@
+#!/usr/bin/env bash
+set -euo pipefail
+if [[ ! -f /opt/ros/humble/setup.bash ]]; then
+  echo 'NOT RUN: ROS2 Humble is not installed; no ROS compilation claim.' >&2
+  exit 2
+fi
+source /opt/ros/humble/setup.bash
+command -v colcon >/dev/null || { echo 'colcon is required' >&2; exit 2; }
+bt_repo_root="$(cd "$(dirname "${BASH_SOURCE[0]}")/../../.." && pwd)"
+cd "$bt_repo_root"
+# Install/build upstream BehaviorTree.CPP 4.10.0 separately and source its prefix.
+# EXACT REQUIRED intentionally rejects an incompatible system BT.CPP package.
+colcon build --base-paths ros2 --packages-up-to bt_executor robobrain_services bt_mock_servers --event-handlers console_direct+
+colcon test --packages-select bt_skill_interfaces bt_executor --event-handlers console_direct+
+colcon test-result --verbose

ros2/bt_executor/tools/check_static.py

@@ -0,0 +1,46 @@
+#!/usr/bin/env python3
+"""Checks fixed-tree expansion and generated IDL headers without claiming ROS build."""
+from pathlib import Path
+import re
+import xml.etree.ElementTree as ET
+
+PACKAGE = Path(__file__).resolve().parents[1]
+REPOSITORY = PACKAGE.parents[1]
+root = ET.parse(PACKAGE / 'trees/fixed_workflow.xml').getroot()
+assert root.attrib['BTCPP_format'] == '4'
+trees = {v.attrib['ID']: v for v in root.findall('BehaviorTree')}
+assert {'NavigateSkill', 'GroundTargetSkill', 'PickSkill', 'CheckFreeSpaceSkill',
+        'PlaceSkill', 'AskUserSkill'} <= set(trees)
+assert [v.tag for v in trees['TaskRoot'][0]] == ['ApprovedPlanGate', 'SubTree']
+
+
+def expand(node, inputs=None):
+    inputs = inputs or {}
+    if node.tag == 'RunStage':
+        stage = node.attrib['stage']
+        return [inputs[stage[1:-1]] if stage.startswith('{') else stage]
+    if node.tag == 'SubTree':
+        return expand(trees[node.attrib['ID']], {**inputs, **node.attrib})
+    return [stage for child in node for stage in expand(child, inputs)]
+
+
+expected = re.findall(r'case Stage::\w+:return "(\w+)"',
+                      (REPOSITORY / 'core/src/workflow.cpp').read_text())
+actual = expand(trees['TaskRoot'])
+assert actual == expected, (actual, expected)
+assert not any(v.tag in {'Script', 'ScriptCondition', 'RetryUntilSuccessful'} for v in root.iter())
+assert trees['AskUserSkill'][0].tag == 'RequestClarification'
+
+
+def header_name(name):
+    return re.sub(r'([a-z0-9])([A-Z])', r'\1_\2',
+                  re.sub(r'(.)([A-Z][a-z]+)', r'\1_\2', name)).lower()
+
+
+for source in list((PACKAGE / 'src').glob('*.cpp')) + list((PACKAGE / 'include/bt_executor').glob('*.hpp')):
+    for kind, name in re.findall(r'bt_skill_interfaces/(action|msg)/(\w+)\.hpp', source.read_text()):
+        candidates = list((PACKAGE.parent / 'bt_skill_interfaces' / kind).glob('*'))
+        assert any(header_name(v.stem) == name for v in candidates), (source, kind, name)
+ET.parse(PACKAGE / 'package.xml')
+print(f'Static checks passed: {len(actual)} ordered core stages, six skill templates, generated IDL include names, package XML.')
+print('This does not compile C++ or validate ROS2 middleware behavior.')

ros2/bt_executor/tools/test_ros_backend.py

@@ -0,0 +1,117 @@
+"""Transport lifecycle tests using controlled callback objects; no robot simulation."""
+import sys
+from pathlib import Path
+import threading
+from collections import deque
+from types import SimpleNamespace as S
+import unittest
+import time
+
+sys.path.insert(0, str(Path(__file__).resolve().parents[3] / 'coordinator'))
+from robot_bt_coordinator.ros_backend import RosBackend
+
+
+class Future:
+    def __init__(self, value): self.value = value
+    def result(self): return self.value
+    def add_done_callback(self, cb): self.callback = cb
+
+
+class Handle:
+    accepted = True
+    def __init__(self): self.cancels = 0; self.future = Future(None)
+    def cancel_goal_async(self): self.cancels += 1
+    def get_result_async(self): return self.future
+
+
+class BackendLifecycle(unittest.TestCase):
+    def setUp(self):
+        self.backend = RosBackend.__new__(RosBackend)
+        self.backend._lock = threading.RLock()
+        self.backend._events = deque()
+        self.backend._feedback_timeout = 5
+        self.backend.node = S(get_clock=lambda: S(now=lambda: S(nanoseconds=10_000_000_000)))
+        self.key = ('task', 'run')
+        self.rec = dict(task_id='task', run_id='run', sent_at=time.monotonic(), last_feedback=0,
+                        sequence=0, handle=None, done=False, cancel_intent=False,
+                        cancel_at=None, unknown_emitted=False, wire_uuid='wire')
+        self.backend._runs = {self.key: self.rec}
+        self.backend._planning = {}
+
+    def result(self, native=4, business=0, stop=1, evidence='{"stop_confirmed":true}'):
+        return Future(S(status=native, result=S(result=S(status=business, stop_state=stop,
+            stopped_at=S(sec=10, nanosec=0), stop_evidence_ref='proof', error_code=''),
+            completed_quantity=1, evidence_json=evidence)))
+
+    def test_cancel_before_late_accept(self):
+        self.backend.cancel(*self.key)
+        handle = Handle()
+        self.backend._execution_accepted(self.key, Future(handle))
+        self.assertEqual(handle.cancels, 1)
+        self.assertFalse(self.rec['done'])
+        self.assertEqual(len(self.backend._events), 0)
+
+    def test_planning_cancel_cancels_handle(self):
+        handle = Handle()
+        planning = dict(task_id='task', task_revision=1, planning_generation=1, done=False, handle=handle)
+        self.backend._planning = {('task', 1, 1): planning}
+        self.backend.cancel('task', '')
+        self.assertTrue(planning['done'])
+        self.assertEqual(handle.cancels, 1)
+        self.assertFalse(self.backend._events)
+
+    def test_native_business_mismatch_quarantines(self):
+        self.backend._execution_result(self.key, self.result(native=5, business=0))
+        event = self.backend._events.pop()
+        self.assertEqual(event['status'], 'INTERVENTION_REQUIRED')
+        self.assertFalse(event['stop_confirmed'])
+        self.assertFalse(self.rec['done'])  # tracking survives unknown result
+
+    def test_unknown_stop_does_not_become_success(self):
+        self.backend._execution_result(self.key, self.result(stop=0))
+        self.assertEqual(self.backend._events.pop()['status'], 'INTERVENTION_REQUIRED')
+
+    def test_stale_stop_proof_does_not_release(self):
+        future = self.result()
+        future.value.result.result.stopped_at.sec = 1
+        self.backend._execution_result(self.key, future)
+        self.assertFalse(self.backend._events.pop()['stop_confirmed'])
+
+    def test_future_stop_proof_does_not_release(self):
+        future = self.result()
+        future.value.result.result.stopped_at.sec = 11
+        self.backend._execution_result(self.key, future)
+        self.assertFalse(self.backend._events.pop()['stop_confirmed'])
+
+    def test_cancel_ack_never_is_stop(self):
+        self.rec['handle'] = Handle()
+        self.backend.cancel(*self.key)
+        self.assertEqual(len(self.backend._events), 0)
+        self.assertFalse(self.rec['done'])
+
+    def test_stale_and_duplicate_feedback_do_not_refresh_liveness(self):
+        def feedback(sec, sequence, stage='Navigate'):
+            self.backend._feedback(self.key, S(feedback=S(stamp=S(sec=sec, nanosec=0),
+                sequence=sequence, stage=stage, status_json='{}')))
+        feedback(1, 1)
+        self.assertEqual(self.rec['last_feedback'], 0)
+        feedback(10, 2)
+        accepted = self.rec['last_feedback']
+        feedback(10, 2)
+        self.assertEqual(self.rec['last_feedback'], accepted)
+        self.assertEqual(len(self.backend._events), 1)
+
+    def test_late_terminal_after_unknown_still_emits_evidence(self):
+        self.backend._unknown(self.rec, 'ACCEPTANCE_TIMEOUT')
+        self.backend._execution_result(self.key, self.result(native=5, business=2))
+        events = list(self.backend._events)
+        self.assertEqual([e['status'] for e in events], ['INTERVENTION_REQUIRED', 'CANCELED'])
+        self.assertTrue(events[-1]['stop_confirmed'])
+
+    def test_duplicate_json_result_fails_closed(self):
+        self.backend._execution_result(self.key, self.result(evidence='{"stop_confirmed":false,"stop_confirmed":true}'))
+        self.assertEqual(self.backend._events.pop()['status'], 'INTERVENTION_REQUIRED')
+
+
+if __name__ == '__main__':
+    unittest.main()

ros2/bt_executor/trees/fixed_workflow.xml

@@ -0,0 +1,42 @@
+<?xml version="1.0"?>
+<root BTCPP_format="4" main_tree_to_execute="TaskRoot">
+  <!-- Stage one admits an already approved semantic plan. Planning is the
+       coordinator's separate PlanTask action; XML never comes from that model. -->
+  <BehaviorTree ID="TaskRoot">
+    <Sequence name="TwoStageTask">
+      <ApprovedPlanGate/>
+      <SubTree ID="ExecutionStage" _autoremap="true"/>
+    </Sequence>
+  </BehaviorTree>
+  <BehaviorTree ID="ExecutionStage">
+    <Sequence>
+      <RunStage stage="Preflight"/>
+      <SubTree ID="NavigateSkill" stage="NavigateObserve" _autoremap="true"/>
+      <RunStage stage="LocateShelfColumn"/>
+      <SubTree ID="NavigateSkill" stage="NavigateSource" _autoremap="true"/>
+      <SubTree ID="GroundTargetSkill" _autoremap="true"/>
+      <SubTree ID="PickSkill" _autoremap="true"/>
+      <SubTree ID="NavigateSkill" stage="NavigateDestination" _autoremap="true"/>
+      <SubTree ID="CheckFreeSpaceSkill" _autoremap="true"/>
+      <SubTree ID="PlaceSkill" _autoremap="true"/>
+      <RunStage stage="Cleanup"/>
+    </Sequence>
+  </BehaviorTree>
+  <BehaviorTree ID="NavigateSkill"><RunStage stage="{stage}"/></BehaviorTree>
+  <BehaviorTree ID="GroundTargetSkill"><RunStage stage="LocalizeTarget"/></BehaviorTree>
+  <BehaviorTree ID="PickSkill">
+    <Sequence>
+      <RunStage stage="Pick"/>
+      <RunStage stage="VerifyPick"/>
+      <RunStage stage="TransportPosture"/>
+      <RunStage stage="VerifyTransport"/>
+    </Sequence>
+  </BehaviorTree>
+  <BehaviorTree ID="CheckFreeSpaceSkill"><RunStage stage="CheckFreeSpace"/></BehaviorTree>
+  <BehaviorTree ID="PlaceSkill">
+    <Sequence><RunStage stage="Place"/><RunStage stage="VerifyPlace"/><RunStage stage="Deliver"/></Sequence>
+  </BehaviorTree>
+  <!-- Deliberate failure routes back to coordinator clarification; no script,
+       arbitrary XML, or autonomous motion while waiting for human information. -->
+  <BehaviorTree ID="AskUserSkill"><RequestClarification/></BehaviorTree>
+</root>