behavior-tree/ros2/bt_executor/src/ros_driver.cpp

#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 <filesystem>
#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);
    prune_mappings();
  }
  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"));
  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 navigate_->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;prune_mappings(r.goal_id);
}
json RosDriver::mappings()const {json j=json::array();for(const auto& kv:mappings_)j.push_back(kv.second);return j;}
void RosDriver::prune_mappings(const std::string& keep) {
  for(auto it=mappings_.begin();mappings_.size()>256&&it!=mappings_.end();) {
    if(it->first!=keep&&!cancelers_.count(it->first)&&!cancel_intents_.count(it->first))it=mappings_.erase(it);else ++it;
  }
}
json RosDriver::mappings(const std::string& run_id)const {
  std::ifstream input(uuid_journal_);if(!input&&std::filesystem::exists(uuid_journal_))throw std::runtime_error("UUID journal unreadable");
  std::map<std::string,json> selected;std::string line;
  while(std::getline(input,line)){if(line.empty())continue;auto entry=json::parse(line);if(entry.at("run_id")==run_id){const auto id=entry.at("client_goal_id").get<std::string>();selected[id]=std::move(entry);}}
  if(input.bad())throw std::runtime_error("UUID journal read failed");
  json out=json::array();for(const auto& item:selected)out.push_back(item.second);return out;
}
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;r.error_code=m.error_code;
  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::execution(const Navigate::Result& m,const GoalRequest& request)const {
  ExecutionResult out;out.error_code=m.error_code;
  using Nav=Navigate::Result;
  switch(m.status) {
    case Nav::SUCCEEDED:out.code=ResultCode::COMPLETED;break;
    case Nav::CANCELED:out.code=ResultCode::CANCELED;break;
    case Nav::TIMEOUT:out.code=ResultCode::TIMED_OUT;break;
    case Nav::BLOCKED:out.code=ResultCode::FAILED;if(out.error_code.empty())out.error_code="NAV_BLOCKED";break;
    case Nav::NOT_READY:out.code=ResultCode::REJECTED;if(out.error_code.empty())out.error_code="NAV_NOT_READY";break;
    case Nav::FAILED:out.code=ResultCode::FAILED;break;
    default:out.error_code="NAV_RESULT_PROTOCOL_ERROR";return out;
  }
  out.detail=out.error_code+": "+m.message;
  if(m.stop_state==Nav::STOP_CONFIRMED&&!m.stop_evidence_ref.empty()&&
     fresh(ns(m.stopped_at),ns(m.stopped_at)+observation_lifetime_ns_,request.capture_after))
    out.stop=StopState::CONFIRMED;
  return out;
}
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");
      Navigate::Goal g;g.task_id=r.trace.task_id;g.subtask_id=r.trace.subtask_id;g.target_pose=pose_msg(*r.registered_pose,now);
      g.position_tolerance=r.position_tolerance_m;g.yaw_tolerance=r.orientation_tolerance_rad;g.timeout=timeout();
      send_typed<Navigate>(navigate_,g,r,Navigate::Feedback::STOPPING,[this,r](const Navigate::Result& m,auto){
        auto out=execution(m,r);out.response.valid=m.final_pose_valid&&
          std::isfinite(m.final_position_error)&&std::isfinite(m.final_yaw_error)&&
          m.final_position_error>=0&&std::abs(m.final_yaw_error)<=std::acos(-1.0)&&
          m.final_position_error<=r.position_tolerance_m&&std::abs(m.final_yaw_error)<=r.orientation_tolerance_rad;
        if(m.final_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.execution_record_ref=m.execution_record_ref;
        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}};
        auto result=readonly_result(code,ok,out);result.error_code=m.status==Locate::Result::NOT_FOUND?"NOT_FOUND":m.status==Locate::Result::AMBIGUOUS?"AMBIGUOUS":m.error_code;result.detail=m.error_code+": "+m.message;result.execution_record_ref=m.record_ref;return result;});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);}
        auto result=readonly_result(code,ok,out);result.error_code=m.status==Localize::Result::NOT_FOUND?"NOT_FOUND":m.status==Localize::Result::AMBIGUOUS?"AMBIGUOUS":m.error_code;result.detail=m.error_code+": "+m.message;result.execution_record_ref=m.record_ref;return result;});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();
        auto result=readonly_result(code,ok,out);result.error_code=m.error_code;result.detail=m.message;result.execution_record_ref=m.evidence_ref;return result;});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;
        auto result=readonly_result(code,ok&&e.status<=2,out);result.error_code=e.error_code;result.detail=e.message;result.execution_record_ref=e.evidence_ref;return result;});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);}
        auto result=readonly_result(code,ok,out);result.error_code=m.error_code;result.detail=m.message;result.execution_record_ref=m.record_ref;return result;});break;
    }
  }
}
}  // namespace bt_executor