307 lines
20 KiB
C++
307 lines
20 KiB
C++
#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;
|
|
}
|