实现行为树执行器、任务协调和技能接口
This commit is contained in:
@@ -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;
|
||||
}
|
||||
@@ -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
|
||||
Reference in New Issue
Block a user