wangfeiyu/behavior-tree
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

fix: harden task recovery and DR contract handling

Browse Source
This commit is contained in:
wangfeiyu
2026-09-20 13:36:48 +08:00
parent 492676344a
commit f9d8feb6f0
49 changed files with 2083 additions and 165 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ros2/bt_executor/src/executor_node.cpp
+275 -29
View File
@@ -1,7 +1,9 @@
#include <bt_executor/admission.hpp>
#include <bt_executor/recovery_policy.hpp>
#include <behaviortree_cpp/bt_factory.h>
#include <behaviortree_cpp/action_node.h>
#include <bt_skill_interfaces/action/execute_task.hpp>
#include <bt_skill_interfaces/srv/reconcile_task.hpp>
#include <std_msgs/msg/string.hpp>
#include <ament_index_cpp/get_package_share_directory.hpp>
#include <filesystem>
@@ -10,6 +12,8 @@
#include <unistd.h>
#include <cerrno>
#include <cstdio>
#include <random>
#include <sstream>
namespace bt_executor {
using namespace robot_bt;
@@ -50,12 +54,41 @@ static void append_receipt(const std::string& path,const json& data) {
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");
int directory=::open(std::filesystem::path(path).parent_path().c_str(),O_RDONLY|O_DIRECTORY);
if(directory<0)throw std::runtime_error("journal directory unavailable");
ok=::fsync(directory);::close(directory);if(ok)throw std::runtime_error("journal directory sync failed");
}
static json trace_json(const iface::msg::TaskTrace& trace) {
return {{"task_id",trace.task_id},{"subtask_id",trace.subtask_id},{"run_id",trace.run_id},
{"attempt",trace.attempt},{"task_revision",trace.task_revision},{"plan_version",trace.plan_version},
{"execution_generation",trace.execution_generation}};
}
static std::string recovery_id() {
std::random_device random;std::ostringstream out;out<<"reconcile/"<<std::hex;
for(unsigned i=0;i<4;++i)out<<random()<<"-";
return out.str();
}
template<class Visit> static void scan_records(const std::string& path,Visit visit) {
std::ifstream stream(path);
if(!stream) {
if(std::filesystem::exists(path))throw std::runtime_error("history unreadable");
return;
}
std::string line;
while(std::getline(stream,line)) {
require(!stream.eof(),"journal row is not durably newline-terminated");
require(line.size()<=1048576,"journal row exceeds bound");
if(!line.empty())visit(json::parse(line));
}
require(stream.eof(),"history read failed");
}
class ExecutorNode final:public rclcpp::Node {
public:
using Action=iface::action::ExecuteTask;
using Handle=rclcpp_action::ServerGoalHandle<Action>;
ExecutorNode():Node("bt_executor") {
using Reconcile=iface::srv::ReconcileTask;
using Verify=iface::action::VerifyState;
ExecutorNode():Node("bt_executor",rclcpp::NodeOptions().start_parameter_services(false).start_parameter_event_publisher(false)) {
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);
@@ -92,21 +125,29 @@ class ExecutorNode final:public rclcpp::Node {
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;
}
registry_->set_dispatch_recorder([this](const GoalRequest& request){
append_receipt(task_journal_,{{"task_id",request.trace.task_id},{"run_id",request.trace.run_id},{"state","ACTIVE"},
{"dispatch",{{"goal_id",request.goal_id},{"skill",static_cast<int>(request.skill)},{"trace",trace_json(trace_msg(request.trace))}}}});
});
scan_records(task_journal_,[this](const json& record){
const auto run=record.at("run_id").get<std::string>();
if(record.at("state")=="RELEASED")unresolved_runs_.erase(run);else unresolved_runs_.insert(run);
});
faulted_=!unresolved_runs_.empty();
scan_records(receipt_path_,[this](const json& receipt){cache_receipt(receipt);});
recovery_token_=declare_parameter<std::string>("recovery_token","");
require(recovery_token_.empty()||(recovery_token_.size()>=16&&recovery_token_.size()<=1024),"recovery_token must be empty (disabled) or 16..1024 bytes");
recovery_operator_=declare_parameter<std::string>("recovery_operator_id","");
recovery_timeout_=timeout_policy("recovery_timeout_ms",5000);
verifier_=rclcpp_action::create_client<Verify>(this,"skills/verify_state");
reconcile_=create_service<Reconcile>("tasks/reconcile",
[this](std::shared_ptr<rmw_request_id_t> header,std::shared_ptr<Reconcile::Request> request){begin_recovery(header,request);});
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;
return static_cast<const BT::TreeNode&>(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");
auto rt=static_cast<const BT::TreeNode&>(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
@@ -119,8 +160,8 @@ class ExecutorNode final:public rclcpp::Node {
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(receipt_for(task.trace.task_id,task.item_index).is_null(),"task already delivered; reconcile receipt without replay");
require(task_record(task.trace.run_id).empty(),"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;}
@@ -148,29 +189,189 @@ class ExecutorNode final:public rclcpp::Node {
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_;
std::set<std::string> unresolved_runs_;
std::string recovery_token_,recovery_operator_;
Milliseconds recovery_timeout_{5000};
rclcpp::Service<Reconcile>::SharedPtr reconcile_;
rclcpp_action::Client<Verify>::SharedPtr verifier_;
struct Recovery {
std::shared_ptr<rmw_request_id_t> header;
iface::msg::TaskTrace trace;
json task;
std::string operator_id,evidence_ref,resolution,verification_id;
RosTime issued_at{0};std::uint64_t geometry_epoch{0};SteadyTime deadline;
rclcpp_action::ClientGoalHandle<Verify>::SharedPtr handle;
};
std::shared_ptr<Recovery> recovery_;
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 cache_receipt(const json& receipt) {
receipts_[json::array({receipt.at("task_id"),receipt.at("item_index")}).dump()]=receipt;
while(receipts_.size()>256)receipts_.erase(receipts_.begin());
}
json receipt_for(const std::string& task,unsigned item) {
const auto key=json::array({task,item}).dump();
if(receipts_.count(key))return receipts_.at(key);
json found;
scan_records(receipt_path_,[&](const json& row){if(row.at("task_id")==task&&row.at("item_index")==item)found=row;});
if(!found.is_null())cache_receipt(found);
return found;
}
json task_record(const std::string& run) const {
json found=json::object(),dispatches=json::object();
scan_records(task_journal_,[&](const json& row){if(row.at("run_id")==run){
found.update(row);
if(row.contains("dispatch")){
const auto& dispatch=row.at("dispatch");const auto id=dispatch.at("goal_id").get<std::string>();
require(!dispatches.contains(id)||dispatches.at(id)==dispatch,"conflicting dispatch manifest");dispatches[id]=dispatch;
}
}});
if(!found.empty())found["dispatches"]=dispatches;
return found;
}
bool complete_dispatch_history(const json& task,const std::vector<GoalRecord>& records) const {
if(!task.value("history_complete",false)||task.value("dispatch_manifest_version",0)!=1||!task.contains("dispatches"))return false;
const auto& manifest=task.at("dispatches");
if(!manifest.is_object()||manifest.size()!=records.size())return false;
for(const auto& record:records) {
if(!manifest.contains(record.request.goal_id))return false;
const auto& saved=manifest.at(record.request.goal_id);
if(saved.at("skill")!=static_cast<int>(record.request.skill)||saved.at("trace")!=trace_json(trace_msg(record.request.trace)))return false;
}
return true;
}
json task_receipts(const std::string& task) const {
std::map<unsigned,json> found;
scan_records(receipt_path_,[&](const json& row){if(row.at("task_id")==task){
const auto item=row.at("item_index").get<unsigned>();require(item<20,"invalid receipt item index");found[item]=row;
}});
json result=json::array();
for(const auto& entry:found){const auto& row=entry.second;json evidence=json::object();
for(const auto* key:{"evidence_id","target_ref","destination_ref","passed","empty_hand","in_destination","valid"})evidence[key]=row.at(key);
result.push_back({{"task_id",task},{"item_index",entry.first},{"run_id",row.at("run_id")},{"completed_quantity",1},{"evidence",evidence}});
}
return result;
}
void recovery_reply(const std::shared_ptr<rmw_request_id_t>& header,bool accepted,const std::string& error,const std::string& message,const json& state=json::object()) {
Reconcile::Response response;response.accepted=accepted;response.error_code=error;response.message=message;response.state_json=state.dump();
try{reconcile_->send_response(*header,response);}catch(const std::exception& e){RCLCPP_ERROR(get_logger(),"Recovery response failed: %s",e.what());}
}
void reject_recovery(std::shared_ptr<Recovery> pending,const std::string& reason) {
if(recovery_!=pending)return;
if(pending->handle) {
try{verifier_->async_cancel_goal(pending->handle);}catch(...){}
}
recovery_.reset();
faulted_=true;
recovery_reply(pending->header,false,"RECONCILIATION_FAILED",reason);
}
void begin_recovery(const std::shared_ptr<rmw_request_id_t>& header,const std::shared_ptr<Reconcile::Request>& request) {
try {
require(!recovery_operator_.empty()&&request->operator_id==recovery_operator_&&recovery_authorized(recovery_token_,request->authorization),"recovery authorization rejected");
require(!active_&&!reserved_&&!recovery_&&!driver_->faulted(),"executor busy or driver durability fault");
require(request->resolution=="resume_task"||request->resolution=="cancel_task"||request->resolution=="replan_task","unsupported recovery resolution");
require(!request->evidence_ref.empty()&&request->evidence_ref.size()<=500,"operator evidence reference required");
const auto saved=task_record(request->trace.run_id);
require(!saved.empty()&&saved.contains("trace")&&saved.at("trace")==trace_json(request->trace),"exact durable task trace required");
require(saved.contains("approved_plan_json")&&saved.contains("context_json"),"legacy task lacks complete recovery snapshot");
for(const auto& run:unresolved_runs_)require(run==request->trace.run_id,"another task remains unresolved");
for(const auto& entry:registry_->records())if(entry.second.state!=GoalState::TERMINAL)
require(entry.second.request.trace.run_id==request->trace.run_id,"another goal remains unresolved");
require(verifier_->action_server_is_ready(),"independent verifier unavailable");
auto task=admit(strict_json(saved.at("approved_plan_json").get<std::string>()),strict_json(saved.at("context_json").get<std::string>()),request->trace,trusted_,robot_id_);
const auto state=driver_->safety(task.target_id);const auto epoch=driver_->geometry_epoch();
require(state.safe&&state.stationary&&state.holding==Holding::EMPTY&&epoch.has_value(),"fresh safe stationary empty RobotState/Safety required");
auto pending=std::make_shared<Recovery>();pending->header=header;pending->trace=request->trace;pending->task=saved;
pending->operator_id=request->operator_id;pending->evidence_ref=request->evidence_ref;pending->resolution=request->resolution;
pending->verification_id=recovery_id();pending->issued_at=now().nanoseconds();pending->geometry_epoch=*epoch;pending->deadline=SteadyClock::now()+recovery_timeout_;
// Record the read-only verification intent, never the authorization secret.
unresolved_runs_.insert(request->trace.run_id);faulted_=true;
append_receipt(task_journal_,{{"task_id",request->trace.task_id},{"run_id",request->trace.run_id},{"state","RECONCILING"},
{"recovery",{{"operator_id",pending->operator_id},{"evidence_ref",pending->evidence_ref},{"resolution",pending->resolution},{"verification_id",pending->verification_id},{"issued_at_ns",pending->issued_at}}}});
recovery_=pending;
Verify::Goal goal;goal.trace=request->trace;goal.check=Verify::Goal::PRECHECK;goal.source_goal_id=pending->verification_id;
goal.target.object_ref=task.target_id;goal.target.description=task.target_id;goal.destination.region_ref=task.destination_id;goal.destination.description=task.destination_id;
goal.expected_geometry_epoch=*epoch;goal.capture_after=stamp(pending->issued_at);
goal.timeout.sec=static_cast<int>(recovery_timeout_.count()/1000);goal.timeout.nanosec=static_cast<unsigned>((recovery_timeout_.count()%1000)*1000000);
rclcpp_action::Client<Verify>::SendGoalOptions options;
options.goal_response_callback=[this,pending](rclcpp_action::ClientGoalHandle<Verify>::SharedPtr handle){
if(recovery_!=pending) {
if(handle)verifier_->async_cancel_goal(handle);
return;
}
if(!handle) {
reject_recovery(pending,"independent verification rejected");
return;
}
pending->handle=handle;
};
options.result_callback=[this,pending](const rclcpp_action::ClientGoalHandle<Verify>::WrappedResult& result){complete_recovery(pending,result);};
try{verifier_->async_send_goal(goal,options);}catch(const std::exception& e){reject_recovery(pending,e.what());}
}catch(const std::exception& e){recovery_reply(header,false,"RECONCILIATION_REJECTED",e.what());}
}
void complete_recovery(const std::shared_ptr<Recovery>& pending,const rclcpp_action::ClientGoalHandle<Verify>::WrappedResult& result) {
if(recovery_!=pending)return;
try {
require(SteadyClock::now()<pending->deadline&&result.code==rclcpp_action::ResultCode::SUCCEEDED&&result.result,"independent verification failed or late");
const auto& e=result.result->evidence;const auto current=now().nanoseconds();
const auto context=strict_json(pending->task.at("context_json").get<std::string>());
const auto target=context.at("target_id").get<std::string>();const auto state=driver_->safety(target);const auto epoch=driver_->geometry_epoch();
require(e.context.schema_version==1&&same_trace(trace_core(e.context.trace),trace_core(pending->trace))&&e.context.source_goal_id==pending->verification_id,
"verification identity mismatch");
require(epoch&&*epoch==pending->geometry_epoch&&e.context.geometry_epoch==pending->geometry_epoch,"geometry changed during recovery");
require(ns(e.context.observed_at)>pending->issued_at&&ns(e.context.observed_at)<=current&&ns(e.context.valid_until)>current&&current-ns(e.context.observed_at)<=2000000000LL,
"verification stale, future, or predates request");
require(!e.context.observation_id.empty()&&!e.context.writer.empty()&&!e.evidence_ref.empty()&&!e.source.empty()&&e.target_ref==target,
"verification provenance incomplete");
require(e.status==0&&e.stopped_valid&&e.stopped&&e.hand_empty_valid&&e.hand_empty&&e.holding_state==0,
"independent stationary empty-hand proof missing");
require(state.safe&&state.stationary&&state.holding==Holding::EMPTY&&state.observed_at>=pending->issued_at&&!driver_->faulted(),"fresh safety or robot state missing");
bool manipulation=false;const auto records=registry_->task_records(pending->trace.run_id);
for(const auto& record:records){const auto& t=record.request.trace;
require(t.task_id==pending->trace.task_id&&t.task_revision==pending->trace.task_revision&&t.plan_version==pending->trace.plan_version&&t.execution_generation==pending->trace.execution_generation,
"goal history belongs to another intent");
manipulation=manipulation||record.request.skill==Skill::PICK||record.request.skill==Skill::PLACE;
}
const auto item=context.value("item_index",0u);const auto receipt=receipt_for(pending->trace.task_id,item);
const bool complete_history=complete_dispatch_history(pending->task,records);
const bool safe_retry=complete_history&&!manipulation;
require(pending->resolution!="resume_task"||recovery_resume_permitted(complete_history,manipulation,!receipt.is_null()),
"resume requires a durable receipt or complete history proving no manipulation dispatch");
const auto receipts=task_receipts(pending->trace.task_id);
require(pending->resolution!="replan_task"||(safe_retry&&receipts.empty()),"replan requires no manipulation or deliveries");
json report={{"verified",true},{"stop_confirmed",true},{"holding_state","EMPTY"},{"run_id",pending->trace.run_id},
{"evidence_ref",pending->evidence_ref},{"verification_ref",e.evidence_ref},{"safe_to_retry",safe_retry},{"receipts",receipts},{"observed_at_ns",ns(e.context.observed_at)}};
append_receipt(task_journal_,{{"task_id",pending->trace.task_id},{"run_id",pending->trace.run_id},{"state","RECONCILING"},{"recovery_verification",report}});
for(const auto& record:records)if(record.state!=GoalState::TERMINAL)
require(registry_->reconcile(record.request.goal_id,record.request.trace,StopState::CONFIRMED,true,e.evidence_ref),"goal reconciliation failed");
require(!registry_->robot_locked(robot_id_),"unresolved registry state remains");
append_receipt(task_journal_,{{"task_id",pending->trace.task_id},{"run_id",pending->trace.run_id},{"state","RELEASED"},{"status","RECONCILED"},{"recovery_resolution",pending->resolution}});
unresolved_runs_.erase(pending->trace.run_id);faulted_=!unresolved_runs_.empty();recovery_.reset();
recovery_reply(pending->header,true,"","independent physical reconciliation completed",report);
}catch(const std::exception& e){reject_recovery(pending,e.what());}
}
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();
append_receipt(task_journal_,{{"schema_version",2},{"robot_id",robot_id_},{"task_id",task.trace.task_id},{"run_id",task.trace.run_id},{"state","ACTIVE"},
{"trace",trace_json(goal->trace)},{"approved_plan_json",goal->approved_plan_json},{"context_json",goal->context_json},
{"timeout",{{"sec",goal->timeout.sec},{"nanosec",goal->timeout.nanosec}}},{"history_complete",true},{"dispatch_manifest_version",1},{"accepted_at_ns",now().nanoseconds()}});
unresolved_runs_.insert(task.trace.run_id);
const auto epoch=driver_->geometry_epoch();require(epoch.has_value(),"fresh RobotState geometry epoch required");
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_)) {
if(receipt_for(task.trace.task_id,task.item_index).is_null()) {
const auto* record=registry_->find(verification);
if(!record||!record->result||!record->result->response.evidence||
!record->result->response.verified||!record->result->response.in_destination||
@@ -182,17 +383,26 @@ class ExecutorNode final:public rclcpp::Node {
{"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;
append_receipt(receipt_path_,receipt);cache_receipt(receipt);
}
quantity_=1;return true;
},budgets_);
auto blackboard=BT::Blackboard::create();blackboard->set("runtime",runtime_);
blackboard->set("task",trace_json(goal->trace));
blackboard->set("approved_plan",strict_json(goal->approved_plan_json));
blackboard->set("context",strict_json(goal->context_json));
blackboard->set("versions",json{{"registry",trusted_.at("registry_version")},{"plan",goal->trace.plan_version},{"task_revision",goal->trace.task_revision},{"geometry_epoch",*epoch}});
blackboard->set("execution",json{{"stage","Accepted"},{"completed_quantity",0}});
blackboard->set("evidence",json::object());
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());
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;
@@ -202,25 +412,54 @@ class ExecutorNode final:public rclcpp::Node {
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&&
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()},
{"detail",detail_},
{"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;
try {
evidence["goal_uuid_mappings"]=driver_->mappings(active_->get_goal()->trace.run_id);
const auto receipt=receipt_for(id,strict_json(active_->get_goal()->context_json).value("item_index",0u));
if(!receipt.is_null()) {
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;
evidence["goal_records"]=json::array();bool manipulation_dispatched=false,localization_ambiguous=false;
const auto task_records=registry_->task_records(active_->get_goal()->trace.run_id);
for(const auto& record:task_records) {
manipulation_dispatched=manipulation_dispatched||record.request.skill==Skill::PICK||record.request.skill==Skill::PLACE;
json row={{"goal_id",record.request.goal_id},{"state",static_cast<int>(record.state)},
{"request_type",record.request.wire_request_type},{"snapshot_ref",journal_dir_+"/goal_registry.log#"+record.request.goal_id},
{"request_snapshot_present",!record.request.wire_request_snapshot.empty()},{"feedback_snapshot_present",!record.feedback_snapshot.empty()}};
if(record.result){
row.update({{"error_code",record.result->error_code},{"execution_record_ref",record.result->execution_record_ref},{"detail",record.result->detail},
{"result_type",record.result->wire_result_type},{"result_snapshot_present",!record.result->wire_result_snapshot.empty()}});
localization_ambiguous=localization_ambiguous||((record.request.skill==Skill::LOCATE_SHELF_COLUMN||record.request.skill==Skill::LOCALIZE_TARGET)&&
(record.result->error_code=="NOT_FOUND"||record.result->error_code=="AMBIGUOUS"));
}
evidence["goal_records"].push_back(row);
}
const bool safe_retry=complete_dispatch_history(task_record(active_->get_goal()->trace.run_id),task_records)&&!manipulation_dispatched;
evidence["safe_to_retry"]=safe_retry;
if(localization_ambiguous&&safe_retry&&physical_release&&quantity_==0) {
evidence["needs_clarification"]=true;
evidence["questions"]=json::array({"Please confirm the target item and its source shelf after localization could not identify it uniquely."});
}
}catch(const std::exception& e) {
faulted_=true;physical_release=false;status="INTERVENTION_REQUIRED";
evidence["status"]=status;evidence["safe_to_release"]=false;evidence["safe_to_retry"]=false;
evidence["needs_clarification"]=false;evidence["history_error"]=e.what();
}
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}});
{"state",release?"RELEASED":"QUARANTINED"},{"status",status},{"final_evidence",evidence}});
if(release)unresolved_runs_.erase(active_->get_goal()->trace.run_id);
}catch(const std::exception& e) {
faulted_=true;status="INTERVENTION_REQUIRED";evidence["journal_error"]=e.what();
evidence["status"]=status;evidence["safe_to_release"]=false;
@@ -229,7 +468,7 @@ class ExecutorNode final:public rclcpp::Node {
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_;
result->result.error_code=runtime_&&runtime_->runner&&!runtime_->runner->error_code().empty()?runtime_->runner->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);
@@ -255,6 +494,7 @@ class ExecutorNode final:public rclcpp::Node {
const auto steady=SteadyClock::now();
try {
registry_->pump(steady);if(++tick_count_%20==0)publish_registry();
if(recovery_&&steady>=recovery_->deadline)reject_recovery(recovery_,"independent verification timeout");
if(!active_)return;
if(cancel_requested_)begin_halt("CANCELED");
if(steady>=deadline_&&!halting_){timed_out_=true;detail_="task execution deadline";begin_halt("FAILED");}
@@ -262,6 +502,11 @@ class ExecutorNode final:public rclcpp::Node {
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>()));
auto blackboard=tree_->rootBlackboard();
const auto safety=driver_->safety(config.at("target_id").get<std::string>());
blackboard->set("execution",json{{"stage",runtime_->stage},{"completed_quantity",quantity_},{"cancel_requested",cancel_requested_},{"registry_locked",registry_->robot_locked(robot_id_)}});
blackboard->set("evidence",json{{"safe",safety.safe},{"stationary",safety.stationary},{"holding",static_cast<int>(safety.holding)},
{"observed_at_ns",safety.observed_at},{"valid_until_ns",safety.valid_until}});
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) {
@@ -289,6 +534,7 @@ class ExecutorNode final:public rclcpp::Node {
}
}catch(const std::exception& e) {
faulted_=true;detail_=std::string("executor fault: ")+e.what();
if(recovery_)reject_recovery(recovery_,detail_);
try{begin_halt("INTERVENTION_REQUIRED");}catch(...){}
if(active_)finish("INTERVENTION_REQUIRED",false);
RCLCPP_ERROR(get_logger(),"%s",detail_.c_str());
ros2/bt_executor/src/ros_driver.cpp
+24 -9
View File
@@ -6,6 +6,7 @@
#include <cmath>
#include <iomanip>
#include <fstream>
#include <filesystem>
#include <sstream>
#include <stdexcept>
@@ -83,6 +84,7 @@ RosDriver::RosDriver(rclcpp::Node& n, std::string robot_id, std::string journal,
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"));
@@ -139,15 +141,27 @@ void RosDriver::record_mapping(const GoalRequest& r,const rclcpp_action::GoalUUI
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;
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;
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;}
@@ -191,7 +205,8 @@ void RosDriver::send(const GoalRequest& r) {
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;
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);
@@ -210,7 +225,7 @@ void RosDriver::send(const GoalRequest& r) {
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();
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: {
@@ -223,7 +238,7 @@ void RosDriver::send(const GoalRequest& r) {
!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;
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;
@@ -247,7 +262,7 @@ void RosDriver::send(const GoalRequest& r) {
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;
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");
@@ -263,7 +278,7 @@ void RosDriver::send(const GoalRequest& r) {
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;
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;
@@ -299,7 +314,7 @@ void RosDriver::send(const GoalRequest& r) {
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;
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;
@@ -319,7 +334,7 @@ void RosDriver::send(const GoalRequest& r) {
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;
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;
}
}
}