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

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>

ros2/bt_mock_servers/bt_mock_servers/__init__.py

@@ -0,0 +1 @@
+"""ROS2 simulation fixtures; never connect these servers to a robot."""

ros2/bt_mock_servers/bt_mock_servers/mock_skills.py

@@ -0,0 +1,747 @@
+"""Bounded rclpy action fixtures scoped to /sim/<robot>; never issue robot commands.
+
+VLA completion and verification are separate fixtures. Default verification is
+UNKNOWN, so a default successful VLA result cannot count a delivered item.
+"""
+import copy
+import json
+import math
+import threading
+import time
+
+import rclpy
+from builtin_interfaces.msg import Duration, Time
+from rclpy.action import ActionServer, CancelResponse, GoalResponse
+from rclpy.callback_groups import ReentrantCallbackGroup
+from rclpy.executors import MultiThreadedExecutor
+from rclpy.node import Node
+
+from bt_skill_interfaces.action import (
+    AssessGrasp, CheckFreeSpace, ExecuteManipulation, ExecutePosture,
+    EvaluateProgress, ExecuteTask, LocalizeTarget3D, LocateShelfColumn,
+    Navigate, NavigateSemantic, PlanTask, VerifyState,
+)
+from bt_skill_interfaces.msg import (DenseProgress, ExecutionResult, RobotState, SafetyState,
+                                     VerificationEvidence, VisualObservation)
+from bt_skill_interfaces.srv import GetRobotState, ReconcileGoal
+from std_msgs.msg import String
+from .scenarios import duration_seconds, fixture_at, fixed_plan, parse_scenarios, strict_json, validate_trace
+
+ACTION_TYPES = {
+    "navigate": Navigate, "execute_manipulation": ExecuteManipulation,
+    "plan_task": PlanTask, "locate_shelf_column": LocateShelfColumn,
+    "localize_target_3d": LocalizeTarget3D, "check_free_space": CheckFreeSpace,
+    "assess_grasp": AssessGrasp, "execute_posture": ExecutePosture, "verify_state": VerifyState,
+    "navigate_semantic": NavigateSemantic, "evaluate_progress": EvaluateProgress,
+    "execute_task": ExecuteTask,
+}
+ACTION_ENDPOINTS = {
+    **{name: "skills/" + name for name in ACTION_TYPES},
+    "plan_task": "tasks/plan",
+    "execute_task": "tasks/execute",
+    "evaluate_progress": "monitor/evaluate_progress",
+}
+MOTION = frozenset(("navigate", "navigate_semantic", "execute_manipulation", "execute_posture", "execute_task"))
+SUCCESS_PHASES = {
+    "navigate": (Navigate.Feedback.CHECKING, Navigate.Feedback.PLANNING,
+                 Navigate.Feedback.NAVIGATING, Navigate.Feedback.ARRIVING),
+    "execute_manipulation": (ExecuteManipulation.Feedback.PREPARING,
+                             ExecuteManipulation.Feedback.WAITING_OBSERVATION,
+                             ExecuteManipulation.Feedback.INFERRING,
+                             ExecuteManipulation.Feedback.EXECUTING,
+                             ExecuteManipulation.Feedback.COMPLETING),
+    "plan_task": tuple(range(3)), "locate_shelf_column": (0, 1),
+    "localize_target_3d": (0, 1), "check_free_space": (0, 1),
+    "assess_grasp": (0,), "execute_posture": (ExecutePosture.Feedback.CHECKING,
+                                                ExecutePosture.Feedback.MOVING,
+                                                ExecutePosture.Feedback.SETTLING),
+    "verify_state": (0,), "navigate_semantic": (0,), "evaluate_progress": (0,),
+}
+
+
+def ros_time(nanoseconds):
+    return Time(sec=max(0, nanoseconds) // 1000000000, nanosec=max(0, nanoseconds) % 1000000000)
+
+
+def elapsed_message(seconds):
+    nanoseconds = max(0, int(seconds * 1e9))
+    return Duration(sec=nanoseconds // 1000000000, nanosec=nanoseconds % 1000000000)
+
+
+def lifecycle_phases(name, kind):
+    if name == "navigate" and kind == "obstacle_recovery":
+        return (Navigate.Feedback.CHECKING, Navigate.Feedback.PLANNING,
+                Navigate.Feedback.NAVIGATING, Navigate.Feedback.WAITING_OBSTACLE,
+                Navigate.Feedback.RECOVERING, Navigate.Feedback.ARRIVING)
+    return SUCCESS_PHASES.get(name, (0,))
+
+
+class MockSkills(Node):
+    def __init__(self):
+        super().__init__("bt_mock_skills", namespace="/sim/robot_01")
+        namespace = self.get_namespace()
+        if not namespace.startswith("/sim/") or len(namespace.split("/")) != 3:
+            raise RuntimeError("Mock servers require the dedicated /sim/<robot> namespace")
+        self.robot_id = namespace.rsplit("/", 1)[1]
+        self.declare_parameter("scenarios_json", "{}")
+        self.declare_parameter("max_goal_seconds", 30.0)
+        self.declare_parameter("allowed_postures", ["pregrasp", "transport", "home"])
+        self.declare_parameter("initial_holding_state", "UNKNOWN")
+        self.declare_parameter("enabled_actions", list(ACTION_TYPES))
+        self.declare_parameter("enabled_topics", ["robot_state", "safety_state", "visual_observation",
+                                                   "dense_progress", "goal_registry"])
+        self.scenarios = parse_scenarios(self.get_parameter("scenarios_json").value)
+        self.max_seconds = self.get_parameter("max_goal_seconds").value
+        if not math.isfinite(self.max_seconds) or not 0 < self.max_seconds <= 120:
+            raise ValueError("max_goal_seconds must be finite in (0,120]")
+        self.allowed_postures = frozenset(self.get_parameter("allowed_postures").value)
+        if not self.allowed_postures or any(not isinstance(p, str) or not p for p in self.allowed_postures):
+            raise ValueError("allowed_postures must contain nonempty whitelist keys")
+        self.enabled_actions = tuple(self.get_parameter("enabled_actions").value)
+        self.enabled_topics = frozenset(self.get_parameter("enabled_topics").value)
+        if len(set(self.enabled_actions)) != len(self.enabled_actions) or set(self.enabled_actions) - set(ACTION_TYPES):
+            raise ValueError("enabled_actions contains an unknown or duplicate action")
+        topic_names = {"robot_state", "safety_state", "visual_observation", "dense_progress", "goal_registry"}
+        if set(self.enabled_topics) - topic_names:
+            raise ValueError("enabled_topics contains an unknown topic")
+        self.lock = threading.Lock()
+        self.motion_reserved = False
+        self.inflight = 0
+        self.counts = {name: 0 for name in ACTION_TYPES}
+        self.topic_counts = {name: 0 for name in
+                             ("robot_state", "safety_state", "visual_observation", "dense_progress", "goal_registry")}
+        self.accepted = {}
+        self.unresolved_motion = {}
+        self.motion_owner = None
+        self.geometry_epoch = 1
+        initial_holding = self.get_parameter("initial_holding_state").value
+        if initial_holding not in ("UNKNOWN", "EMPTY"):
+            raise ValueError("initial_holding_state must be an explicit UNKNOWN or EMPTY fixture")
+        self.holding_state = RobotState.EMPTY if initial_holding == "EMPTY" else RobotState.HOLDING_UNKNOWN
+        self.held_target = ""
+        self.posture_id = "home"
+        self.group = ReentrantCallbackGroup()
+        self.servers = []
+        for name in self.enabled_actions:
+            action = ACTION_TYPES[name]
+            endpoint = ACTION_ENDPOINTS[name]
+            self._check_endpoint(endpoint)
+            self.servers.append(ActionServer(
+                self, action, endpoint, callback_group=self.group,
+                goal_callback=lambda request, n=name: self._goal(n, request),
+                cancel_callback=lambda _: CancelResponse.ACCEPT,
+                handle_accepted_callback=lambda handle, n=name: self._accepted(n, handle),
+                execute_callback=lambda handle, n=name: self._execute(n, handle),
+            ))
+        endpoints = {"robot_state": "robot_state", "safety_state": "safety_state",
+                     "visual_observation": "observations/scene", "dense_progress": "monitor/dense_progress",
+                     "goal_registry": "goal_registry"}
+        for name in [endpoints[item] for item in self.enabled_topics] + ["get_robot_state", "reconcile_goal"]:
+            self._check_endpoint(name)
+        if "robot_state" in self.enabled_topics: self.state_pub = self.create_publisher(RobotState, "robot_state", 10)
+        if "safety_state" in self.enabled_topics: self.safety_pub = self.create_publisher(SafetyState, "safety_state", 10)
+        if "visual_observation" in self.enabled_topics: self.observation_pub = self.create_publisher(VisualObservation, "observations/scene", 10)
+        if "dense_progress" in self.enabled_topics: self.progress_pub = self.create_publisher(DenseProgress, "monitor/dense_progress", 10)
+        if "goal_registry" in self.enabled_topics: self.registry_pub = self.create_publisher(String, "goal_registry", 10)
+        self.state_service = self.create_service(GetRobotState, "get_robot_state", self._get_state, callback_group=self.group)
+        self.reconcile_service = self.create_service(ReconcileGoal, "reconcile_goal", self._reconcile, callback_group=self.group)
+        self.timer = self.create_timer(0.2, self._publish_states, callback_group=self.group)
+        self.get_logger().warning("SIMULATION ONLY: no hardware commands; verification defaults to UNKNOWN")
+
+    def _check_endpoint(self, name):
+        resolved = self.resolve_topic_name(name)
+        if not resolved.startswith(self.get_namespace() + "/"):
+            raise RuntimeError("Mock endpoint remappings must remain in this simulation namespace")
+
+    def _goal(self, name, request):
+        try:
+            if duration_seconds(request.timeout) > self.max_seconds:
+                raise ValueError("requested timeout exceeds mock bound")
+            if hasattr(request, "trace"):
+                validate_trace(request.trace)
+            elif not request.task_id or (hasattr(request, "subtask_id") and not request.subtask_id):
+                raise ValueError("task/subtask is required")
+            if hasattr(request, "capture_after"):
+                capture = request.capture_after
+                if capture.sec < 0 or not 0 <= capture.nanosec < 1000000000:
+                    raise ValueError("capture boundary is invalid")
+                capture_ns = capture.sec * 1000000000 + capture.nanosec
+                if capture_ns > self.get_clock().now().nanoseconds:
+                    raise ValueError("capture boundary is in the future")
+            if name == "navigate":
+                p, q = request.target_pose.pose.position, request.target_pose.pose.orientation
+                values = [p.x, p.y, p.z, q.x, q.y, q.z, q.w, request.position_tolerance, request.orientation_tolerance]
+                if not all(math.isfinite(v) for v in values) or not request.target_pose.header.frame_id:
+                    raise ValueError("navigation pose is invalid")
+                if request.position_tolerance <= 0 or not 0 < request.orientation_tolerance <= math.pi:
+                    raise ValueError("navigation tolerances are invalid")
+                if abs(q.x*q.x + q.y*q.y + q.z*q.z + q.w*q.w - 1.0) > 0.001:
+                    raise ValueError("navigation quaternion must have unit norm")
+            elif name == "execute_manipulation":
+                if request.skill not in ("pick", "place") or not request.instruction.strip():
+                    raise ValueError("manipulation skill/instruction is invalid")
+                if not request.target.object_ref or not request.target.description:
+                    raise ValueError("manipulation target is required")
+                destination = (bool(request.destination.region_ref), bool(request.destination.description))
+                if destination != ((False, False) if request.skill == "pick" else (True, True)):
+                    raise ValueError("destination must be empty for pick and complete for place")
+            elif name == "navigate_semantic":
+                if request.kind not in ("LOCATION", "OBJECT", "CELL") or not request.reference or not request.registry_version:
+                    raise ValueError("semantic navigation binding is invalid")
+                if request.kind == "CELL" and any(
+                        not isinstance(value, str) or not value or "/" in value
+                        for value in (request.shelf_id, request.side_id, request.column_id, request.tier_id)):
+                    raise ValueError("semantic cell binding is incomplete")
+                if request.position_tolerance <= 0 or not 0 < request.orientation_tolerance <= math.pi:
+                    raise ValueError("semantic navigation tolerances are invalid")
+            elif name == "execute_posture":
+                if request.posture_id not in self.allowed_postures or not request.expected_geometry_epoch:
+                    raise ValueError("posture or geometry epoch is invalid")
+            elif name == "plan_task":
+                if not request.instruction or not request.task_revision or not request.planning_generation:
+                    raise ValueError("planning identity/instruction is invalid")
+                for raw in (request.known_info_json, request.context_snapshot_json, request.constraints_json):
+                    if not isinstance(strict_json(raw), dict):
+                        raise ValueError("planning JSON must be an object")
+            elif name == "verify_state" and request.check not in range(5):
+                raise ValueError("unsupported verification check")
+            elif name == "verify_state":
+                if not request.expected_geometry_epoch or not request.target.object_ref or not request.target.description:
+                    raise ValueError("verification target and geometry epoch are required")
+                if request.check in (request.PICK, request.TRANSPORT, request.PLACE) and not request.source_goal_id:
+                    raise ValueError("verification source goal is required")
+                if request.check == request.PLACE and (not request.destination.region_ref or not request.destination.description):
+                    raise ValueError("place verification destination is required")
+            elif name == "locate_shelf_column":
+                if not all((request.target_ref, request.target_description, request.source_region_ref,
+                            request.observation_station_id)) or not request.station_registry_version:
+                    raise ValueError("shelf localization binding is incomplete")
+            elif name == "localize_target_3d":
+                if not all((request.target_ref, request.target_description, request.shelf_id,
+                            request.column_id, request.station_binding_ref)) or not request.expected_geometry_epoch:
+                    raise ValueError("3D localization binding is incomplete")
+            elif name == "check_free_space":
+                if not all((request.destination_ref, request.destination_description,
+                            request.object_ref, request.object_description)):
+                    raise ValueError("free-space target is incomplete")
+                if not isinstance(strict_json(request.placement_constraints_json), dict):
+                    raise ValueError("placement constraints must be an object")
+            elif name == "assess_grasp":
+                context = request.target_binding.context
+                valid_until_ns = (request.robot_state.valid_until.sec * 1000000000 +
+                                  request.robot_state.valid_until.nanosec)
+                if (not request.target_binding.target.object_ref or not request.target_binding.target.description or
+                        not context.schema_version or not context.geometry_epoch or
+                        not request.robot_state.robot_id or not request.allowed_posture_ids or
+                        valid_until_ns <= self.get_clock().now().nanoseconds or
+                        any(p not in self.allowed_postures for p in request.allowed_posture_ids)):
+                    raise ValueError("grasp assessment inputs are incomplete")
+            elif name == "evaluate_progress":
+                window = strict_json(request.window_json)
+                if not request.task_description.strip() or not request.sequence or not isinstance(window, list) or not window:
+                    raise ValueError("progress window is invalid")
+            elif name == "execute_task":
+                plan, context = strict_json(request.approved_plan_json), strict_json(request.context_json)
+                if not isinstance(plan, dict) or not isinstance(plan.get("subtasks"), list) or not isinstance(context, dict):
+                    raise ValueError("task boundary JSON is invalid")
+            with self.lock:
+                scenario = fixture_at(self.scenarios, name, self.counts[name])
+                if scenario.get("kind") == "reject":
+                    self.counts[name] += 1
+                    return GoalResponse.REJECT
+                # Four active work callbacks leave threads for cancellation and state.
+                if self.inflight >= 4 or (name in MOTION and self.motion_reserved):
+                    return GoalResponse.REJECT
+                self.inflight += 1
+                if name in MOTION:
+                    self.motion_reserved = True
+            return GoalResponse.ACCEPT
+        except (ValueError, TypeError, OverflowError) as exc:
+            self.get_logger().warning("Rejecting " + name + ": " + str(exc))
+            return GoalResponse.REJECT
+
+    def _accepted(self, name, handle):
+        goal_id = bytes(handle.goal_id.uuid).hex()
+        with self.lock:
+            fixture = fixture_at(self.scenarios, name, self.counts[name])
+            self.counts[name] += 1
+            self.accepted[goal_id] = (time.monotonic(), fixture)
+            if name in MOTION:
+                self.motion_owner = (goal_id, name, copy.deepcopy(handle.request.trace))
+        handle.execute()
+
+    def _execute(self, name, handle):
+        goal_id = bytes(handle.goal_id.uuid).hex()
+        with self.lock:
+            started, fixture = self.accepted.pop(goal_id)
+        kind = fixture.get("kind", "normal")
+        budget = duration_seconds(handle.request.timeout)
+        deadline = started + budget
+        finish = started + fixture.get("duration_seconds", 0.2)
+        outcome, stop_state = ExecutionResult.COMPLETED, ExecutionResult.CONFIRMED
+        sequence = 0
+        emitted_phase = -1
+        result = ACTION_TYPES[name].Result()
+        try:
+            while True:
+                now = time.monotonic()
+                if handle.is_cancel_requested:
+                    outcome = ExecutionResult.CANCELED
+                    self._stopping_feedback(name, handle, sequence + 1, now - started)
+                    if kind == "cancel_stop_unknown":
+                        stop_state = ExecutionResult.UNKNOWN
+                    else:
+                        time.sleep(fixture.get("stop_delay_seconds", 0.0))
+                    break
+                if now >= deadline or not rclpy.ok():
+                    outcome = ExecutionResult.TIMED_OUT
+                    self._stopping_feedback(name, handle, sequence + 1, now - started)
+                    if kind == "timeout_stop_unknown":
+                        stop_state = ExecutionResult.UNKNOWN
+                    break
+                if now >= finish and kind not in ("timeout", "timeout_stop_unknown", "silence"):
+                    break
+                if kind != "silence":
+                    phases = lifecycle_phases(name, kind)
+                    duration = max(0.001, finish - started)
+                    phase_index = min(len(phases) - 1, int((now - started) / duration * len(phases)))
+                    if phase_index > emitted_phase:
+                        sequence += 1
+                        self._feedback(name, handle, sequence, now - started, phases[phase_index])
+                        emitted_phase = phase_index
+                time.sleep(min(0.02, max(0, deadline - now)))
+            if kind == "failed" and outcome == ExecutionResult.COMPLETED:
+                outcome = ExecutionResult.FAILED
+            if kind == "stop_unknown" and outcome == ExecutionResult.COMPLETED:
+                outcome, stop_state = ExecutionResult.FAILED, ExecutionResult.UNKNOWN
+            self._fill_result(name, handle.request, result, fixture, goal_id, outcome, stop_state)
+            self._finish_native(handle, outcome, kind)
+        except Exception as exc:
+            # Unknown execution state stays reserved. A client cannot infer stop from this exception.
+            stop_state = ExecutionResult.UNKNOWN if name in MOTION else ExecutionResult.CONFIRMED
+            if hasattr(result, "result"):
+                result.result = self._execution_result(ExecutionResult.FAILED, stop_state, goal_id, "MOCK_EXCEPTION", str(exc))
+            elif hasattr(result, "evidence"):
+                result.evidence.status = VerificationEvidence.UNKNOWN
+                result.evidence.error_code = "MOCK_EXCEPTION"
+            elif hasattr(result, "status"):
+                result.status = result.FAILED
+                result.error_code = "MOCK_EXCEPTION"
+                result.message = str(exc)
+            elif hasattr(result, "decision"):
+                result.decision = result.UNKNOWN
+                result.error_code = "MOCK_EXCEPTION"
+            if handle.is_active:
+                handle.abort()
+            self.get_logger().error("Mock execution exception: " + str(exc))
+        finally:
+            with self.lock:
+                self.inflight -= 1
+                if name in MOTION and stop_state == ExecutionResult.CONFIRMED:
+                    self.motion_reserved = False
+                    self.unresolved_motion.pop(goal_id, None)
+                    if self.motion_owner and self.motion_owner[0] == goal_id:
+                        self.motion_owner = None
+                elif name in MOTION:
+                    self.unresolved_motion[goal_id] = (name, copy.deepcopy(handle.request.trace))
+        return result
+
+    @staticmethod
+    def _finish_native(handle, outcome, kind):
+        if kind == "native_mismatch":
+            handle.abort()  # Intentional protocol-negative fixture: payload remains COMPLETED.
+        elif outcome == ExecutionResult.CANCELED:
+            handle.canceled()
+        elif outcome == ExecutionResult.COMPLETED:
+            handle.succeed()
+        else:
+            handle.abort()
+
+    def _feedback(self, name, handle, sequence, elapsed, phase=None):
+        feedback = ACTION_TYPES[name].Feedback()
+        feedback.stamp = self.get_clock().now().to_msg()
+        feedback.sequence = sequence
+        if hasattr(feedback, "phase"):
+            feedback.phase = SUCCESS_PHASES[name][0] if phase is None else phase
+        if hasattr(feedback, "message"):
+            feedback.message = "SIMULATED " + name
+        if name == "execute_task":
+            feedback.stage = "SIMULATED_STAGE_" + str(sequence)
+            feedback.status_json = json.dumps({"sequence": sequence}, allow_nan=False)
+        if hasattr(feedback, "elapsed_time"):
+            feedback.elapsed_time = elapsed_message(elapsed)
+        if name == "navigate":
+            feedback.pose_valid = True
+            feedback.current_pose = copy.deepcopy(handle.request.target_pose)
+            feedback.errors_valid = True
+            feedback.position_error = 0.0
+            feedback.orientation_error = 0.0
+            feedback.blocked_valid = True
+            feedback.blocked = False
+        if name == "execute_manipulation":
+            feedback.progress_valid = False
+        handle.publish_feedback(feedback)
+
+    def _stopping_feedback(self, name, handle, sequence, elapsed):
+        stopping = {"navigate": Navigate.Feedback.STOPPING,
+                    "execute_manipulation": ExecuteManipulation.Feedback.STOPPING,
+                    "execute_posture": ExecutePosture.Feedback.STOPPING}.get(name)
+        if stopping is not None:
+            self._feedback(name, handle, sequence, elapsed, stopping)
+
+    def _execution_result(self, outcome, stop_state, goal_id, error="", message="SIMULATED execution only"):
+        result = ExecutionResult()
+        result.status, result.stop_state = outcome, stop_state
+        result.error_code = error or ("" if outcome == ExecutionResult.COMPLETED else "MOCK_TERMINATED")
+        result.message = message
+        if stop_state == ExecutionResult.CONFIRMED:
+            result.stopped_at = self.get_clock().now().to_msg()
+            result.stop_evidence_ref = "sim://stop/" + goal_id
+        return result
+
+    def _fill_result(self, name, request, result, fixture, goal_id, outcome, stop_state):
+        kind = fixture.get("kind", "normal")
+        stamp = self.get_clock().now().nanoseconds
+        if kind == "stale_observation":
+            stamp = max(0, stamp - 60000000000)
+        observed = ros_time(stamp)
+        record = "sim://" + name + "/" + goal_id
+        ok = outcome == ExecutionResult.COMPLETED
+        if hasattr(result, "result"):
+            result.result = self._execution_result(outcome, stop_state, goal_id)
+        if name in ("navigate", "navigate_semantic"):
+            if ok and stop_state == ExecutionResult.CONFIRMED:
+                with self.lock:
+                    self.geometry_epoch += 1
+            if name == "navigate":
+                result.pose_valid = result.errors_valid = ok
+                result.final_pose = copy.deepcopy(request.target_pose)
+            else:
+                pose = fixture.get("final_pose")
+                result.pose_valid = result.errors_valid = ok and pose is not None
+                if pose is not None:
+                    result.final_pose.header.frame_id = pose["frame_id"]
+                    result.final_pose.pose.position.x = pose["x"]
+                    result.final_pose.pose.position.y = pose["y"]
+                    result.final_pose.pose.position.z = pose["z"]
+                    result.final_pose.pose.orientation.x = pose["qx"]
+                    result.final_pose.pose.orientation.y = pose["qy"]
+                    result.final_pose.pose.orientation.z = pose["qz"]
+                    result.final_pose.pose.orientation.w = pose["qw"]
+            result.final_pose.header.stamp = observed
+        elif name == "execute_manipulation":
+            result.execution_record_ref = record
+            # Deliberately no holding/verification state mutation here.
+        elif name == "plan_task":
+            plan = fixture.get("plan") or fixed_plan(request.instruction, strict_json(request.known_info_json), fixture.get("plan_version", 1))
+            result.status = result.FAILED if not ok else (result.NEEDS_CLARIFICATION if plan.get("missing_information") else result.PLAN_READY)
+            result.task_plan_json = json.dumps(plan, ensure_ascii=False, allow_nan=False)
+            result.planning_record_ref = record
+            if result.status == result.NEEDS_CLARIFICATION:
+                result.error_code, result.message = "MOCK_MISSING_INFORMATION", "SIMULATED plan needs clarification"
+        elif name == "locate_shelf_column":
+            result.status = result.SUCCEEDED if ok else result.FAILED
+            if kind == "not_found": result.status = result.NOT_FOUND
+            if kind == "ambiguous": result.status = result.AMBIGUOUS
+            result.shelf_id = fixture.get("shelf_id", "shelf_A")
+            result.side_id = fixture.get("side_id", "FRONT")
+            result.column_id = fixture.get("column_id", "1")
+            result.tier_id = fixture.get("tier_id", "1")
+            result.confidence = 0.99 if result.status == result.SUCCEEDED else 0.0
+            result.observation_id, result.observed_at, result.record_ref = record, observed, record
+            if result.status != result.SUCCEEDED:
+                label = ("NOT_FOUND" if result.status == result.NOT_FOUND else
+                         "AMBIGUOUS" if result.status == result.AMBIGUOUS else "FAILED")
+                result.error_code = "MOCK_" + label
+                result.message = "SIMULATED shelf localization " + label.lower()
+        elif name == "localize_target_3d":
+            result.status = result.SUCCEEDED if ok else result.FAILED
+            if kind == "not_found": result.status = result.NOT_FOUND
+            if kind == "ambiguous": result.status = result.AMBIGUOUS
+            result.target_ref = "wrong_object" if kind == "wrong_object" else request.target_ref
+            result.grasp_region_ref = "sim_grasp:" + result.target_ref
+            result.target_point.header.frame_id = "base_link"
+            result.target_point.header.stamp = observed
+            result.target_point.point.x, result.target_point.point.z = 0.5, 0.8
+            result.grasp_point = copy.deepcopy(result.target_point)
+            result.grasp_point_valid = True
+            result.measurement_source = (result.MODEL_ESTIMATE if kind == "model_estimate" else
+                                         result.FUSED if kind == "fused" else result.RGBD)
+            result.geometry_valid = result.status == result.SUCCEEDED
+            result.quality_code = "OK" if result.geometry_valid else "MOCK_INVALID"
+            result.position_error_bound, result.position_error_bound_valid = 0.005, True
+            result.observation_id = record
+            result.rgb_stamp = result.depth_stamp = observed
+            result.calibration_id = "sim_calibration_v1"
+            result.geometry_epoch = request.expected_geometry_epoch
+            result.record_ref = record
+            if result.status != result.SUCCEEDED:
+                result.error_code = "MOCK_" + ("NOT_FOUND" if result.status == result.NOT_FOUND else
+                                                "AMBIGUOUS" if result.status == result.AMBIGUOUS else "FAILED")
+                result.message = "SIMULATED 3D localization did not produce a usable target"
+        elif name == "check_free_space":
+            result.status = result.SUCCEEDED if ok else result.FAILED
+            if kind == "no_free_space": result.status = result.NO_FREE_SPACE
+            if kind == "ambiguous": result.status = result.AMBIGUOUS
+            result.destination_ref = "wrong_container" if kind == "wrong_destination" else request.destination_ref
+            result.placement_region_ref = "sim_place:" + result.destination_ref
+            result.placement_point.header.frame_id = "base_link"
+            result.placement_point.header.stamp = observed
+            result.placement_point.point.x, result.placement_point.point.z = 0.5, 0.6
+            result.placement_point_valid = True
+            result.placement_pose_valid = False
+            result.geometry_valid = result.status == result.SUCCEEDED
+            result.confidence = 0.99 if result.geometry_valid else 0.0
+            result.quality_code = "OK" if result.geometry_valid else "MOCK_INVALID"
+            result.observation_id, result.observed_at, result.valid_until = record, observed, ros_time(stamp + 5000000000)
+            result.record_ref = record
+            if result.status != result.SUCCEEDED:
+                result.error_code = "MOCK_" + ("NO_FREE_SPACE" if result.status == result.NO_FREE_SPACE else
+                                                "AMBIGUOUS" if result.status == result.AMBIGUOUS else "FAILED")
+                result.message = "SIMULATED placement search did not produce usable free space"
+        elif name == "assess_grasp":
+            result.decision = result.DIRECT if ok else result.UNKNOWN
+            if kind == "unknown": result.decision = result.UNKNOWN
+            if kind == "not_reachable": result.decision = result.NOT_REACHABLE
+            if kind == "adjust_posture":
+                result.decision = result.ADJUST_POSTURE
+                result.posture_id = fixture.get("posture_id", "pregrasp")
+                if result.posture_id not in request.allowed_posture_ids:
+                    result.decision, result.posture_id = result.UNKNOWN, ""
+            result.geometry_epoch = request.target_binding.context.geometry_epoch
+            result.evidence_ref = record
+            if result.decision != result.DIRECT:
+                labels = {result.ADJUST_POSTURE: "ADJUST_POSTURE", result.NOT_REACHABLE: "NOT_REACHABLE",
+                          result.UNKNOWN: "UNKNOWN"}
+                label = labels[result.decision]
+                result.error_code = "MOCK_" + label
+                result.message = "SIMULATED grasp assessment " + label.lower()
+        elif name == "execute_posture":
+            if ok and stop_state == ExecutionResult.CONFIRMED:
+                with self.lock:
+                    self.geometry_epoch = max(self.geometry_epoch, request.expected_geometry_epoch) + 1
+                    self.posture_id = request.posture_id
+            result.geometry_epoch = self.geometry_epoch
+            result.robot_state = self._robot_state()
+            if ok and stop_state == ExecutionResult.CONFIRMED:
+                # Completion snapshot is stopped; registry ownership is released only after native terminal.
+                result.robot_state.base_stopped_valid = result.robot_state.base_stopped = True
+                result.robot_state.posture_settled_valid = result.robot_state.posture_settled = True
+                result.robot_state.evidence_ref = "sim://posture_settled/" + goal_id
+        elif name == "verify_state":
+            self._verify(request, result.evidence, fixture, observed, stamp, record, ok)
+        elif name == "evaluate_progress":
+            result.feedback_state.trace = copy.deepcopy(request.trace)
+            result.feedback_state.observed_at = observed
+            result.feedback_state.sequence = request.sequence
+            result.feedback_state.state = fixture.get("state", "IN_PROGRESS") if ok else "UNKNOWN"
+            result.feedback_state.progress = float(fixture.get("progress", 0.5)) if ok else 0.0
+            result.feedback_state.progress_valid = ok
+            result.feedback_state.hop_json = fixture.get("hop_json", "{}")
+            result.feedback_state.record_ref = record
+            result.error_code = "" if ok else "MOCK_TERMINATED"
+        elif name == "execute_task":
+            result.completed_quantity = int(fixture.get("completed_quantity", 1 if ok else 0))
+            result.evidence_json = fixture.get("evidence_json", "{}")
+        if hasattr(result, "error_code") and not ok:
+            result.error_code = "MOCK_TERMINATED"
+            result.message = "SIMULATED non-completion"
+
+    def _verify(self, request, evidence, fixture, observed, stamp, record, ok):
+        kind = fixture.get("kind", "unknown")
+        evidence.context.schema_version = 1
+        evidence.context.trace = copy.deepcopy(request.trace)
+        evidence.context.source_goal_id = request.source_goal_id
+        evidence.context.geometry_epoch = request.expected_geometry_epoch
+        evidence.context.observed_at = observed
+        evidence.context.valid_until = ros_time(stamp + 5000000000)
+        evidence.context.writer = "bt_mock_verifier"
+        evidence.context.observation_id = record
+        evidence.source, evidence.evidence_ref = "SIMULATOR_INDEPENDENT_FIXTURE", record
+        evidence.status, evidence.holding_state = evidence.UNKNOWN, evidence.HOLDING_UNKNOWN
+        evidence.target_ref, evidence.destination_ref = request.target.object_ref, request.destination.region_ref
+        if not ok or kind not in ("passed", "wrong_object", "wrong_destination", "stale_observation"):
+            evidence.error_code = "MOCK_VERIFICATION_UNKNOWN"
+            evidence.message = "SIMULATED verification is unknown"
+            return
+        evidence.status = evidence.PASSED
+        evidence.stopped_valid = evidence.stopped = True
+        evidence.target_match_valid = True
+        evidence.target_match = kind != "wrong_object"
+        if kind == "wrong_object": evidence.target_ref = "wrong_object"
+        if request.check in (request.PRECHECK, request.PLACE):
+            evidence.holding_state = evidence.EMPTY
+            evidence.hand_empty_valid = evidence.hand_empty = True
+        elif request.check in (request.PICK, request.TRANSPORT):
+            evidence.holding_state = evidence.HOLDING_OTHER if kind == "wrong_object" else evidence.HOLDING_TARGET
+            evidence.grasp_stable_valid = evidence.grasp_stable = True
+        if request.check == request.PLACE:
+            evidence.target_in_destination_valid = True
+            evidence.target_in_destination = kind != "wrong_destination"
+            if kind == "wrong_destination": evidence.destination_ref = "wrong_container"
+        if kind in ("wrong_object", "wrong_destination"):
+            evidence.status = evidence.FAILED
+            evidence.error_code = "MOCK_VERIFICATION_FAILED"
+            evidence.message = "SIMULATED verification contradicted the requested state"
+        with self.lock:
+            self.holding_state = evidence.holding_state
+            self.held_target = evidence.target_ref if evidence.holding_state == evidence.HOLDING_TARGET else ""
+
+    def _robot_state(self):
+        state = RobotState()
+        now = self.get_clock().now().nanoseconds
+        state.robot_id = self.robot_id
+        state.stamp, state.valid_until = ros_time(now), ros_time(now + 1000000000)
+        with self.lock:
+            state.geometry_epoch, state.holding_state = self.geometry_epoch, self.holding_state
+            state.held_target_ref, state.posture_id = self.held_target, self.posture_id
+            state.base_stopped_valid = state.posture_settled_valid = not self.motion_reserved
+            state.base_stopped = state.posture_settled = not self.motion_reserved
+        state.pose.header.frame_id = "map"
+        state.pose.header.stamp = state.stamp
+        state.pose.pose.orientation.w = 1.0
+        state.pose_valid = True
+        state.evidence_ref = "sim://state/" + self.robot_id
+        return state
+
+    def _safety_state(self):
+        state = SafetyState()
+        now = self.get_clock().now().nanoseconds
+        state.robot_id = self.robot_id
+        state.stamp, state.valid_until = ros_time(now), ros_time(now + 1000000000)
+        state.safety_valid = state.motion_allowed = True
+        state.evidence_ref = "sim://safety/" + self.robot_id
+        return state
+
+    def _publish_states(self):
+        state_fixture = self._next_topic_fixture("robot_state")
+        safety_fixture = self._next_topic_fixture("safety_state")
+        state, safety = self._robot_state(), self._safety_state()
+        if state_fixture.get("kind") == "stale_observation":
+            state.valid_until = ros_time(1)
+        elif state_fixture.get("kind") == "invalid_pose":
+            state.pose_valid = False
+        elif state_fixture.get("kind") == "stop_unknown":
+            state.base_stopped_valid = state.posture_settled_valid = False
+        if safety_fixture.get("kind") == "unavailable":
+            safety.safety_valid = safety.motion_allowed = False
+            safety.error_code = "MOCK_SAFETY_UNAVAILABLE"
+        elif safety_fixture.get("kind") == "emergency_stop":
+            safety.emergency_stop_active, safety.motion_allowed = True, False
+        elif safety_fixture.get("kind") == "protective_stop":
+            safety.protective_stop_active, safety.motion_allowed = True, False
+        if "robot_state" in self.enabled_topics: self.state_pub.publish(state)
+        if "safety_state" in self.enabled_topics: self.safety_pub.publish(safety)
+        if "visual_observation" in self.enabled_topics:
+            self.observation_pub.publish(self._visual_observation(self._next_topic_fixture("visual_observation")))
+        if "dense_progress" in self.enabled_topics:
+            self.progress_pub.publish(self._dense_progress(self._next_topic_fixture("dense_progress")))
+        if "goal_registry" in self.enabled_topics:
+            registry = self._next_topic_fixture("goal_registry")
+            default_registry = json.dumps(
+                {"robot_id": self.robot_id, "motion_reserved": self.motion_reserved}, allow_nan=False)
+            self.registry_pub.publish(String(data=registry.get("status_json", default_registry)))
+
+    def _next_topic_fixture(self, name):
+        with self.lock:
+            fixture = fixture_at(self.scenarios, name, self.topic_counts[name])
+            self.topic_counts[name] += 1
+        return fixture
+
+    def _visual_observation(self, fixture):
+        message = VisualObservation()
+        now = self.get_clock().now().nanoseconds
+        message.header.stamp = ros_time(now)
+        message.header.frame_id = "camera_link"
+        message.observation_id = fixture.get("observation_id", "sim_observation")
+        message.image_path = fixture.get("image_path", "/tmp/bt_mock_scene.png")
+        message.station_id = fixture.get("station_id", "station_A")
+        message.registry_version = int(fixture.get("registry_version", 1))
+        message.shelf_id = fixture.get("shelf_id", "shelf_A")
+        message.calibration_id = fixture.get("calibration_id", "sim_calibration_v1")
+        message.geometry_epoch = int(fixture.get("geometry_epoch", self.geometry_epoch))
+        if fixture.get("kind") == "stale_observation":
+            message.header.stamp = ros_time(max(0, now - 60000000000))
+        return message
+
+    def _dense_progress(self, fixture):
+        message = DenseProgress()
+        message.observed_at = self.get_clock().now().to_msg()
+        message.sequence = self.topic_counts["dense_progress"]
+        message.state = fixture.get("state", "UNKNOWN")
+        message.progress = float(fixture.get("progress", 0.0))
+        message.progress_valid = "progress" in fixture and fixture.get("kind") != "unknown"
+        message.hop_json = fixture.get("hop_json", "{}")
+        message.record_ref = "sim://dense_progress/" + str(message.sequence)
+        return message
+
+    def _get_state(self, request, response):
+        response.available = request.robot_id == self.robot_id
+        if response.available:
+            response.robot_state, response.safety_state = self._robot_state(), self._safety_state()
+        else:
+            response.error_code = "UNKNOWN_ROBOT"
+        return response
+
+    def _reconcile(self, request, response):
+        """Simulation-only release requires fresh evidence bound to this exact goal and trace."""
+        try:
+            validate_trace(request.trace)
+            evidence = request.evidence
+            same_trace = all(getattr(evidence.context.trace, field) == getattr(request.trace, field)
+                             for field in ("task_id", "subtask_id", "attempt", "task_revision",
+                                           "plan_version", "run_id", "execution_generation"))
+            now = self.get_clock().now().nanoseconds
+            observed_ns = evidence.context.observed_at.sec * 1000000000 + evidence.context.observed_at.nanosec
+            valid_until_ns = evidence.context.valid_until.sec * 1000000000 + evidence.context.valid_until.nanosec
+            fresh = 0 < observed_ns <= now < valid_until_ns
+            with self.lock:
+                owned = self.unresolved_motion.get(request.goal_id)
+            owned_trace_matches = owned is not None and all(
+                getattr(owned[1], field) == getattr(request.trace, field)
+                for field in ("task_id", "subtask_id", "attempt", "task_revision",
+                              "plan_version", "run_id", "execution_generation"))
+            owner_matches = self.motion_owner is not None and self.motion_owner[0] == request.goal_id
+            bound = (owner_matches and owned_trace_matches and evidence.context.source_goal_id == request.goal_id and
+                     same_trace and evidence.status == evidence.PASSED and
+                     evidence.stopped_valid and evidence.stopped and evidence.evidence_ref and
+                     evidence.context.writer and evidence.source == "SIMULATOR_INDEPENDENT_FIXTURE" and fresh and
+                     request.operator_id and request.reason)
+            if not bound:
+                raise ValueError("reconciliation evidence is not bound and stopped")
+            with self.lock:
+                if request.goal_id not in self.unresolved_motion:
+                    raise ValueError("goal reservation is no longer unresolved")
+                self.unresolved_motion.pop(request.goal_id)
+                self.motion_reserved = False
+                self.motion_owner = None
+            response.accepted = True
+            response.message = "SIMULATED evidence-bound reconciliation"
+        except (ValueError, TypeError):
+            response.accepted = False
+            response.error_code = "UNBOUND_EVIDENCE"
+            response.message = "Reconciliation requires exact goal/trace and stopped evidence"
+        return response
+
+    def destroy_node(self):
+        for server in self.servers:
+            server.destroy()
+        super().destroy_node()
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = None
+    executor = MultiThreadedExecutor(num_threads=8)
+    try:
+        node = MockSkills()
+        executor.add_node(node)
+        executor.spin()
+    except KeyboardInterrupt:
+        pass
+    finally:
+        if rclpy.ok():
+            rclpy.shutdown()
+        executor.shutdown(timeout_sec=2.0)
+        if node is not None:
+            node.destroy_node()

ros2/bt_mock_servers/bt_mock_servers/scenarios.py

@@ -0,0 +1,160 @@
+"""Pure-Python, bounded fixture parsing; no ROS or hardware dependency."""
+import json
+import math
+
+ACTION_NAMES = (
+    "navigate", "execute_manipulation", "plan_task", "locate_shelf_column",
+    "localize_target_3d", "check_free_space", "assess_grasp", "execute_posture",
+    "verify_state",
+    "navigate_semantic", "evaluate_progress", "execute_task",
+    "robot_state", "safety_state", "visual_observation", "dense_progress", "goal_registry",
+)
+KINDS = {
+    "normal", "failed", "timeout", "silence", "stop_unknown", "reject",
+    "stale_observation", "wrong_object", "wrong_destination", "unknown",
+    "passed", "not_found", "ambiguous", "no_free_space", "adjust_posture",
+    "not_reachable", "native_mismatch",
+    "unavailable", "invalid_pose", "emergency_stop", "protective_stop",
+    "model_estimate", "fused",
+    "cancel_stop_unknown", "timeout_stop_unknown",
+    "obstacle_recovery",
+}
+FIXTURE_FIELDS = {
+    "kind", "duration_seconds", "shelf_id", "side_id", "column_id", "tier_id",
+    "posture_id", "plan", "plan_version", "target_ref", "destination_ref",
+    "completed_quantity", "progress", "state", "hop_json", "evidence_json",
+    "observation_id", "image_path", "station_id", "registry_version",
+    "calibration_id", "geometry_epoch", "status_json",
+    "stop_delay_seconds",
+    "final_pose",
+}
+
+BASE_KINDS = {"normal", "failed", "timeout", "silence", "reject", "native_mismatch"}
+ACTION_KINDS = {
+    "navigate": BASE_KINDS | {"stop_unknown", "cancel_stop_unknown", "timeout_stop_unknown", "obstacle_recovery"},
+    "navigate_semantic": BASE_KINDS | {"stop_unknown", "cancel_stop_unknown", "timeout_stop_unknown"},
+    "execute_manipulation": BASE_KINDS | {"stop_unknown", "cancel_stop_unknown", "timeout_stop_unknown"},
+    "execute_posture": BASE_KINDS | {"stop_unknown", "cancel_stop_unknown", "timeout_stop_unknown"},
+    "execute_task": BASE_KINDS | {"stop_unknown", "cancel_stop_unknown", "timeout_stop_unknown"},
+    "plan_task": BASE_KINDS,
+    "locate_shelf_column": BASE_KINDS | {"not_found", "ambiguous", "stale_observation"},
+    "localize_target_3d": BASE_KINDS | {"not_found", "ambiguous", "wrong_object", "stale_observation", "model_estimate", "fused"},
+    "check_free_space": BASE_KINDS | {"no_free_space", "ambiguous", "wrong_destination", "stale_observation"},
+    "assess_grasp": BASE_KINDS | {"adjust_posture", "not_reachable", "unknown"},
+    "verify_state": BASE_KINDS | {"passed", "unknown", "wrong_object", "wrong_destination", "stale_observation"},
+    "evaluate_progress": BASE_KINDS | {"unknown", "stale_observation"},
+    "robot_state": {"normal", "stale_observation", "invalid_pose", "stop_unknown"},
+    "safety_state": {"normal", "unavailable", "emergency_stop", "protective_stop"},
+    "visual_observation": {"normal", "stale_observation"},
+    "dense_progress": {"normal", "unknown", "stale_observation"},
+    "goal_registry": {"normal"},
+}
+
+
+def strict_json(raw):
+    if not isinstance(raw, str) or len(raw.encode("utf-8")) > 262144:
+        raise ValueError("JSON fixture must be at most 256 KiB")
+
+    def pairs(items):
+        result = {}
+        for key, value in items:
+            if key in result:
+                raise ValueError("duplicate JSON key")
+            result[key] = value
+        return result
+
+    return json.loads(raw, object_pairs_hook=pairs, parse_constant=lambda _: invalid())
+
+
+def invalid():
+    raise ValueError("nonfinite JSON number")
+
+
+def parse_scenarios(raw):
+    data = strict_json(raw)
+    if not isinstance(data, dict) or set(data) - set(ACTION_NAMES):
+        raise ValueError("scenario keys must name a supported mock action")
+    result = {}
+    for name, value in data.items():
+        entries = value if isinstance(value, list) else [value]
+        if not entries or len(entries) > 100:
+            raise ValueError("each action requires 1..100 fixtures")
+        for fixture in entries:
+            if not isinstance(fixture, dict) or set(fixture) - FIXTURE_FIELDS:
+                raise ValueError("invalid fixture fields")
+            if fixture.get("kind", "normal") not in KINDS:
+                raise ValueError("unsupported fixture kind")
+            if fixture.get("kind", "normal") not in ACTION_KINDS[name]:
+                raise ValueError("fixture kind has no effect for " + name)
+            delay = fixture.get("duration_seconds", 0.2)
+            if type(delay) not in (int, float) or not math.isfinite(delay) or not 0 <= delay <= 120:
+                raise ValueError("fixture duration must be finite in [0,120]")
+            stop_delay = fixture.get("stop_delay_seconds", 0.0)
+            if type(stop_delay) not in (int, float) or not math.isfinite(stop_delay) or not 0 <= stop_delay <= 5:
+                raise ValueError("stop delay must be finite in [0,5]")
+            if "progress" in fixture:
+                progress = fixture["progress"]
+                if type(progress) not in (int, float) or not math.isfinite(progress) or not 0 <= progress <= 1:
+                    raise ValueError("fixture progress must be finite in [0,1]")
+            if "completed_quantity" in fixture:
+                quantity = fixture["completed_quantity"]
+                if type(quantity) is not int or not 0 <= quantity <= 1000000:
+                    raise ValueError("completed quantity must be an integer in [0,1000000]")
+            for field in ("hop_json", "evidence_json", "status_json"):
+                if field in fixture and not isinstance(strict_json(fixture[field]), dict):
+                    raise ValueError(field + " must encode a JSON object")
+            for field in ("image_path", "observation_id", "station_id", "calibration_id", "state"):
+                if field in fixture and (not isinstance(fixture[field], str) or not fixture[field]):
+                    raise ValueError(field + " must be a nonempty string")
+            if "final_pose" in fixture:
+                pose = fixture["final_pose"]
+                required = {"frame_id", "x", "y", "z", "qx", "qy", "qz", "qw"}
+                if not isinstance(pose, dict) or set(pose) != required or not isinstance(pose["frame_id"], str) or not pose["frame_id"]:
+                    raise ValueError("final_pose requires an exact frame and pose")
+                values = [pose[key] for key in ("x", "y", "z", "qx", "qy", "qz", "qw")]
+                if any(type(value) not in (int, float) or not math.isfinite(value) for value in values):
+                    raise ValueError("final_pose values must be finite")
+                if abs(sum(pose[key] ** 2 for key in ("qx", "qy", "qz", "qw")) - 1.0) > 0.001:
+                    raise ValueError("final_pose quaternion must have unit norm")
+        result[name] = entries
+    return result
+
+
+def fixture_at(scenarios, name, index):
+    # Exhausted arrays hold their last scenario, making repeated calls deterministic.
+    fixtures = scenarios.get(name, [{"kind": "unknown" if name == "verify_state" else "normal"}])
+    return dict(fixtures[min(index, len(fixtures) - 1)])
+
+
+def duration_seconds(value):
+    if value.sec < 0 or not 0 <= value.nanosec < 1000000000:
+        raise ValueError("invalid ROS Duration")
+    seconds = value.sec + value.nanosec / 1e9
+    if seconds <= 0:
+        raise ValueError("timeout must be positive")
+    return seconds
+
+
+def validate_trace(trace):
+    if not all((trace.task_id, trace.subtask_id, trace.run_id)):
+        raise ValueError("trace identifiers are required")
+    if not all((trace.attempt, trace.task_revision, trace.plan_version, trace.execution_generation)):
+        raise ValueError("trace counters begin at one")
+
+
+def fixed_plan(instruction, slots, version=1):
+    required = ("target_name", "source_location", "destination")
+    missing = [name for name in required if not slots.get(name)]
+    plan = {"schema_version": 1, "plan_version": version, "task_type": "pick_transport_place",
+            "goal": instruction, "slots": dict(slots), "missing_information": missing, "subtasks": []}
+    if missing:
+        return plan
+    plan["slots"].setdefault("quantity", 1)
+    target, source, dest = (slots[name] for name in required)
+    steps = [("NAVIGATE", {"destination": source}), ("GROUND_TARGET", {"target": target}),
+             ("PICK", {"target": target}), ("NAVIGATE", {"destination": dest}),
+             ("CHECK_FREE_SPACE", {"destination": dest}), ("PLACE", {"target": target, "destination": dest})]
+    for index, (skill, arguments) in enumerate(steps):
+        plan["subtasks"].append({"id": "s" + str(index + 1), "skill": skill,
+                                 "arguments": arguments, "depends_on": [] if index == 0 else ["s" + str(index)]})
+    return plan

ros2/bt_mock_servers/package.xml

@@ -0,0 +1,14 @@
+<?xml version="1.2.0"?>
+<package format="3">
+  <name>bt_mock_servers</name>
+  <version>1.2.0</version>
+  <description>Explicit simulator-only bounded robot skill action servers.</description>
+  <maintainer email="feiyuwang1998@gmail.com">wangfeiyu</maintainer>
+  <license>Proprietary</license>
+  <buildtool_depend>ament_python</buildtool_depend>
+  <exec_depend>rclpy</exec_depend>
+  <exec_depend>bt_skill_interfaces</exec_depend>
+  <exec_depend>builtin_interfaces</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+  <export><build_type>ament_python</build_type></export>
+</package>

ros2/bt_mock_servers/setup.cfg

@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/bt_mock_servers
+[install]
+install_scripts=$base/lib/bt_mock_servers

ros2/bt_mock_servers/setup.py

@@ -0,0 +1,18 @@
+from setuptools import find_packages, setup
+
+setup(
+    name="bt_mock_servers",
+    version="1.2.0",
+    packages=find_packages(exclude=["test"]),
+    data_files=[
+        ("share/ament_index/resource_index/packages", ["resource/bt_mock_servers"]),
+        ("share/bt_mock_servers", ["package.xml"]),
+    ],
+    install_requires=["setuptools"],
+    zip_safe=True,
+    maintainer="wangfeiyu",
+    maintainer_email="feiyuwang1998@gmail.com",
+    description="Simulator-only bounded ROS2 robot skill servers",
+    license="Apache-2.0",
+    entry_points={"console_scripts": ["mock_skills = bt_mock_servers.mock_skills:main"]},
+)

ros2/bt_skill_interfaces/CMakeLists.txt

@@ -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()

ros2/bt_skill_interfaces/action/AssessGrasp.action

@@ -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

ros2/bt_skill_interfaces/action/CheckFreeSpace.action

@@ -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

ros2/bt_skill_interfaces/action/EvaluateProgress.action

@@ -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

ros2/bt_skill_interfaces/action/ExecuteManipulation.action

@@ -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

ros2/bt_skill_interfaces/action/ExecutePosture.action

@@ -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

ros2/bt_skill_interfaces/action/ExecuteTask.action

@@ -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

ros2/bt_skill_interfaces/action/LocalizeTarget3D.action

@@ -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

ros2/bt_skill_interfaces/action/LocateShelfColumn.action

@@ -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

ros2/bt_skill_interfaces/action/Navigate.action

@@ -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

ros2/bt_skill_interfaces/action/NavigateSemantic.action

@@ -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

ros2/bt_skill_interfaces/action/PlanTask.action

@@ -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

ros2/bt_skill_interfaces/action/VerifyState.action

@@ -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

ros2/bt_skill_interfaces/msg/DenseProgress.msg

@@ -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

ros2/bt_skill_interfaces/msg/ExecutionResult.msg

@@ -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

ros2/bt_skill_interfaces/msg/ObjectTarget.msg

@@ -0,0 +1,2 @@
+string object_ref
+string description

ros2/bt_skill_interfaces/msg/ObservationContext.msg

@@ -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

ros2/bt_skill_interfaces/msg/PlacementBinding.msg

@@ -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

ros2/bt_skill_interfaces/msg/RegionTarget.msg

@@ -0,0 +1,2 @@
+string region_ref
+string description

ros2/bt_skill_interfaces/msg/RobotState.msg

@@ -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

ros2/bt_skill_interfaces/msg/SafetyState.msg

@@ -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

ros2/bt_skill_interfaces/msg/Station.msg

@@ -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

ros2/bt_skill_interfaces/msg/TargetBinding.msg

@@ -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

ros2/bt_skill_interfaces/msg/TaskTrace.msg

@@ -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

ros2/bt_skill_interfaces/msg/VerificationEvidence.msg

@@ -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

ros2/bt_skill_interfaces/msg/VisualObservation.msg

@@ -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

ros2/bt_skill_interfaces/package.xml

@@ -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>

ros2/bt_skill_interfaces/srv/GetRobotState.srv

@@ -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

ros2/bt_skill_interfaces/srv/ReconcileGoal.srv

@@ -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

ros2/robobrain_services/package.xml

@@ -0,0 +1,2 @@
+<?xml version="1.2.0"?>
+<package format="3"><name>robobrain_services</name><version>1.2.0</version><description>RoboBrain planning/spatial and RoboDopamine advisory services</description><maintainer email="feiyuwang1998@gmail.com">wangfeiyu</maintainer><license>Proprietary</license><buildtool_depend>ament_python</buildtool_depend><exec_depend>rclpy</exec_depend><exec_depend>std_msgs</exec_depend><exec_depend>bt_skill_interfaces</exec_depend><export><build_type>ament_python</build_type></export></package>

ros2/robobrain_services/robobrain_services/monitor_client.py

@@ -0,0 +1,60 @@
+"""Optional advisory loop: active pick/place -> bounded frames -> RoboDopamine.
+No robot commands, completion calls, retry requests or delivery writes exist here.
+"""
+import json
+from robot_robobrain.windows import FrameWindow
+
+def main():
+    import rclpy
+    from rclpy.node import Node
+    from rclpy.action import ActionClient
+    from bt_skill_interfaces.action import EvaluateProgress
+    from bt_skill_interfaces.msg import VisualObservation
+    from std_msgs.msg import String
+    class Monitor(Node):
+        def __init__(self):
+            super().__init__('dense_feedback_client');self.frames=FrameWindow();self.active=None;self.handle=None;self.pending=False;self.sequence=0
+            self.client=ActionClient(self,EvaluateProgress,'monitor/evaluate_progress')
+            self.scene=self.create_subscription(VisualObservation,'observations/scene',self.frame,10)
+            self.registry=self.create_subscription(String,'goal_registry',self.goals,10)
+            self.timer=self.create_timer(1.,self.tick)
+            self.last_registry=0
+        def frame(self,m):self.frames.add(m.header.stamp.sec+m.header.stamp.nanosec/1e9,m.header.frame_id,m.image_path)
+        def goals(self,m):
+            try:
+                data=json.loads(m.data);candidates=[g for g in data['goals'] if g.get('state')==1 and g.get('skill') in ('pick','place') and not g.get('cancel_intent')]
+                candidate=candidates[0] if len(candidates)==1 else None
+                if (candidate or {}).get('goal_id')!=(self.active or {}).get('goal_id'):
+                    if self.handle:self.handle.cancel_goal_async()
+                    self.active=candidate;self.sequence=0
+                self.last_registry=self.get_clock().now().nanoseconds
+            except (ValueError,KeyError,TypeError):self.active=None
+        def tick(self):
+            now=self.get_clock().now().nanoseconds
+            if now<self.last_registry or now-self.last_registry>2_000_000_000:
+                if self.handle:self.handle.cancel_goal_async()
+                self.active=None
+            if self.pending or not self.active or not self.client.server_is_ready():return
+            active=dict(self.active);frames=self.frames.since(active['capture_after_ns']/1e9,now/1e9)
+            if not frames or now/1e9-frames[-1]['stamp']>2:return
+            q=EvaluateProgress.Goal()
+            for key,value in active['trace'].items():setattr(q.trace,key,value)
+            q.task_description=active['skill']+' '+active['target_id'];q.capture_after.sec=active['capture_after_ns']//1_000_000_000;q.capture_after.nanosec=active['capture_after_ns']%1_000_000_000
+            self.sequence+=1;q.sequence=self.sequence;q.window_json=json.dumps(frames);q.timeout.sec=5;self.pending=True
+            def accepted(f):
+                try:
+                    h=f.result()
+                    if not h.accepted:self.pending=False;return
+                    self.handle=h
+                    if (self.active or {}).get('goal_id')!=active['goal_id']:h.cancel_goal_async()
+                    h.get_result_async().add_done_callback(self.finished)
+                except Exception:self.pending=False;self.handle=None
+            try:self.client.send_goal_async(q).add_done_callback(accepted)
+            except Exception:self.pending=False
+        def finished(self,f):self.pending=False;self.handle=None
+    rclpy.init();node=Monitor()
+    try:rclpy.spin(node)
+    finally:
+        if node.handle:node.handle.cancel_goal_async()
+        node.destroy_node();rclpy.shutdown()
+if __name__=='__main__':main()

ros2/robobrain_services/robobrain_services/nodes.py

@@ -0,0 +1,121 @@
+"""ROS adapters; numerical/model logic lives in the separately installable package."""
+import threading,time,json,queue
+from pathlib import Path
+from robot_bt_coordinator.plan import strict_json,canonical
+from robot_robobrain.service import BrainService
+from robot_robobrain.dopamine import DenseFeedbackService
+from robot_robobrain.backends import ProcessBackend,FixtureBackend
+from robot_robobrain.observations import Observation,ObservationCache
+
+def ns(t):return t.sec*1_000_000_000+t.nanosec
+def assign_time(t,n):t.sec=int(n)//1_000_000_000;t.nanosec=int(n)%1_000_000_000
+
+def perception_goal(g,kind):
+    q=dict(task_id=g.task_id,subtask_id=g.subtask_id,target_ref=g.target_ref,
+           target_description=g.target_description,capture_after=ns(g.capture_after),
+           timeout=g.timeout.sec+g.timeout.nanosec/1e9)
+    if kind=='shelf':
+        q.update(source_region_ref=g.source_region_ref,observation_station_id=g.observation_station_id,
+                 station_registry_version=g.station_registry_version)
+    else:
+        q.update(expected_geometry_epoch=g.expected_geometry_epoch,shelf_id=g.shelf_id,
+                 column_id=g.column_id,tier_id=g.tier_id,station_binding_ref=g.station_binding_ref)
+    return q
+
+def run_node(dense=False):
+    import rclpy
+    from rclpy.node import Node
+    from rclpy.action import ActionServer,GoalResponse,CancelResponse
+    from rclpy.callback_groups import ReentrantCallbackGroup
+    from rclpy.executors import MultiThreadedExecutor
+    from bt_skill_interfaces.action import PlanTask,LocateShelfColumn,LocalizeTarget3D,EvaluateProgress
+    from bt_skill_interfaces.msg import VisualObservation,DenseProgress
+    class Server(Node):
+        def __init__(self):
+            super().__init__('robodopamine_server' if dense else 'robobrain_server')
+            self.group=ReentrantCallbackGroup();self.lock=threading.Lock();self.busy=False
+            simulation=self.declare_parameter('simulation',False).value
+            record_dir=self.declare_parameter('record_directory','').value
+            if not record_dir:raise ValueError('persistent record_directory required')
+            media=self.declare_parameter('media_root','').value
+            if not media:raise ValueError('trusted media_root required')
+            self.cache=ObservationCache(media)
+            if simulation:
+                if not self.get_namespace().startswith('/sim'):raise ValueError('fixture server restricted to /sim namespace')
+                fixture=self.declare_parameter('fixture_file','').value
+                with open(fixture) as f:raw=strict_json(f.read())
+                backend=FixtureBackend(lambda request:canonical(raw[request['capability']]))
+            else:
+                argv=strict_json(self.declare_parameter('worker_argv_json','[]').value)
+                backend=ProcessBackend(argv,self.declare_parameter('model_version','').value)
+                backend.infer({'capability':'__health__'},float(self.declare_parameter('model_load_timeout_seconds',300.).value))
+            self.service=DenseFeedbackService(backend,record_dir) if dense else BrainService(backend,record_dir)
+            self.publisher=self.create_publisher(DenseProgress,'monitor/dense_progress',10) if dense else None
+            self.subscription=self.create_subscription(VisualObservation,'observations/scene',self.observation,10,callback_group=self.group)
+            self.servers=[]
+            for name,action,kind in ([('monitor/evaluate_progress',EvaluateProgress,'progress')] if dense else [('tasks/plan',PlanTask,'plan'),('skills/locate_shelf_column',LocateShelfColumn,'shelf'),('skills/localize_target_3d',LocalizeTarget3D,'localize3d')]):
+                self.servers.append(ActionServer(self,action,name,execute_callback=lambda h,a=action,k=kind:self.execute(h,a,k),goal_callback=self.admit,cancel_callback=lambda _:CancelResponse.ACCEPT,callback_group=self.group))
+        def observation(self,m):
+            try:self.cache.put(Observation(m.observation_id,ns(m.header.stamp),m.header.frame_id,m.image_path,m.station_id,m.registry_version,m.shelf_id,m.calibration_id,m.geometry_epoch))
+            except (ValueError,OSError) as ex:self.get_logger().warning(str(ex))
+        def admit(self,g):
+            if not 0<g.timeout.sec+g.timeout.nanosec/1e9<=3600 or not 0<=g.timeout.nanosec<1e9:return GoalResponse.REJECT
+            with self.lock:
+                if self.busy:return GoalResponse.REJECT
+                self.busy=True
+            return GoalResponse.ACCEPT
+        def work(self,g,kind,cancel):
+            timeout=g.timeout.sec+g.timeout.nanosec/1e9
+            if kind=='plan':
+                q=dict(task_id=g.task_id,task_revision=g.task_revision,planning_generation=g.planning_generation,instruction=g.instruction,known_info=strict_json(g.known_info_json),context=strict_json(g.context_snapshot_json),constraints=strict_json(g.constraints_json),timeout=timeout)
+                return self.service.plan(q,cancel)
+            if kind=='progress':
+                frames=strict_json(g.window_json)
+                if not isinstance(frames,list):raise ValueError('window must be an array')
+                for frame in frames:
+                    for path in frame['views'].values():
+                        p=Path(path).resolve(strict=True)
+                        if not p.is_relative_to(self.cache.root) or not p.is_file() or p.stat().st_size>32*1024*1024:raise ValueError('invalid window media path')
+                q=dict(task_id=g.trace.task_id,run_id=g.trace.run_id,subtask_id=g.trace.subtask_id,task_description=g.task_description,capture_after=ns(g.capture_after)/1e9,sequence=g.sequence,timeout=timeout)
+                return self.service.evaluate(q,frames,self.get_clock().now().nanoseconds/1e9,cancel)
+            q=perception_goal(g,kind)
+            obs=self.cache.get();method=self.service.shelf if kind=='shelf' else self.service.localize
+            return method(q,obs,self.get_clock().now().nanoseconds,cancel=cancel)
+        def execute(self,h,action,kind):
+            cancel=threading.Event();completed=queue.Queue(maxsize=1);deadline=time.monotonic()+h.request.timeout.sec+h.request.timeout.nanosec/1e9
+            def worker():
+                try:completed.put(self.work(h.request,kind,cancel))
+                except Exception as ex:completed.put(dict(status='FAILED',state='UNKNOWN',error_code='SERVICE_ERROR',message=str(ex)))
+            thread=threading.Thread(target=worker,daemon=True);thread.start();sequence=0;expired=False
+            while thread.is_alive():
+                if h.is_cancel_requested or time.monotonic()>=deadline:cancel.set();expired=time.monotonic()>=deadline
+                sequence+=1;f=action.Feedback();f.stamp=self.get_clock().now().to_msg();f.sequence=sequence;f.phase=1;f.message='canceling inference' if cancel.is_set() else 'processing';h.publish_feedback(f)
+                thread.join(.2)
+            data=completed.get();r=action.Result()
+            try:
+                if cancel.is_set():data=dict(status='FAILED',state='UNKNOWN',error_code='TIMEOUT' if expired else 'CANCELED',record_ref=data.get('record_ref',''))
+                if kind=='plan':
+                    r.status={'PLAN_READY':0,'NEEDS_CLARIFICATION':1}.get(data.get('status'),2);r.task_plan_json=canonical(data.get('plan',{}));r.planning_record_ref=data.get('record_ref','');r.error_code=data.get('error_code','');r.message=data.get('message','')
+                elif kind=='progress':
+                    f=r.feedback_state;f.trace=h.request.trace;f.sequence=h.request.sequence;f.state=data.get('state','UNKNOWN');f.progress=float(data.get('progress',0));f.progress_valid=f.state!='UNKNOWN';f.hop_json=canonical(data.get('hop'));f.record_ref=data.get('record_ref','');assign_time(f.observed_at,int(data.get('stamp',0)*1e9));r.error_code=data.get('error_code','');self.publisher.publish(f)
+                else:
+                    r.status={'SUCCEEDED':0,'NOT_FOUND':2,'AMBIGUOUS':3}.get(data.get('status'),1);r.error_code=data.get('error_code','');r.message=data.get('message','');r.record_ref=data.get('record_ref','')
+                    if kind=='shelf':
+                        for field in ('shelf_id','side_id','column_id','tier_id','observation_id'):setattr(r,field,data.get(field,''))
+                        r.confidence=float(data.get('confidence',0));assign_time(r.observed_at,data.get('observed_at',0))
+                    else:
+                        r.target_ref=h.request.target_ref;r.geometry_valid=False;r.grasp_point_valid=False;r.position_error_bound_valid=False;r.measurement_source=1;r.quality_code=data.get('quality_code','INVALID');r.observation_id=data.get('observation_id','');r.calibration_id=data.get('calibration_id','');r.geometry_epoch=data.get('geometry_epoch',0)
+                        if 'target_point' in data:
+                            p=data['target_point'];r.target_point.header.frame_id=p['frame_id'];assign_time(r.target_point.header.stamp,p['stamp_ns']);assign_time(r.rgb_stamp,p['stamp_ns']);r.target_point.point.x,r.target_point.point.y,r.target_point.point.z=map(float,p['point'])
+                if h.is_cancel_requested:h.canceled()
+                elif expired:h.abort()
+                elif kind=='plan' or kind=='progress' or data.get('status')=='SUCCEEDED':h.succeed()
+                else:h.abort()
+            finally:
+                with self.lock:self.busy=False
+            return r
+    rclpy.init();node=Server();executor=MultiThreadedExecutor(num_threads=4);executor.add_node(node)
+    try:executor.spin()
+    finally:node.service.backend.close();executor.shutdown();node.destroy_node();rclpy.shutdown()
+def brain_main():run_node(False)
+def dopamine_main():run_node(True)

ros2/robobrain_services/setup.cfg

@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/robobrain_services
+[install]
+install_scripts=$base/lib/robobrain_services

ros2/robobrain_services/setup.py

@@ -0,0 +1,2 @@
+from setuptools import setup
+setup(name='robobrain_services',version='1.2.0',packages=['robobrain_services'],data_files=[('share/ament_index/resource_index/packages',['resource/robobrain_services']),('share/robobrain_services',['package.xml'])],entry_points={'console_scripts':['dense_feedback_client=robobrain_services.monitor_client:main','brain_server=robobrain_services.nodes:brain_main','dopamine_server=robobrain_services.nodes:dopamine_main']})