fix: harden task recovery and DR contract handling
This commit is contained in:
+4
-2
@@ -13,8 +13,10 @@ target_link_libraries(robot_bt_demo PRIVATE robot_bt_sim)
|
||||
target_compile_options(robot_bt_demo PRIVATE -Wall -Wextra -Werror)
|
||||
include(CTest)
|
||||
if(BUILD_TESTING)
|
||||
foreach(test_name core_test workflow_test journal_failure_test preflight_test settlement_test proof_regression_test readiness_regression_test scenario_test lifecycle_test)
|
||||
add_executable(${test_name} tests/${test_name}.cpp)
|
||||
file(GLOB core_test_sources CONFIGURE_DEPENDS "tests/*_test.cpp")
|
||||
foreach(test_source IN LISTS core_test_sources)
|
||||
get_filename_component(test_name "${test_source}" NAME_WE)
|
||||
add_executable(${test_name} "${test_source}")
|
||||
target_link_libraries(${test_name} PRIVATE robot_bt_core)
|
||||
target_compile_options(${test_name} PRIVATE -Wall -Wextra -Werror -UNDEBUG)
|
||||
add_test(NAME ${test_name} COMMAND ${test_name})
|
||||
|
||||
@@ -1,5 +1,6 @@
|
||||
#pragma once
|
||||
#include <chrono>
|
||||
#include <deque>
|
||||
#include <cstdint>
|
||||
#include <functional>
|
||||
#include <map>
|
||||
@@ -50,7 +51,7 @@ struct SkillResponse {
|
||||
Holding holding{Holding::UNKNOWN};
|
||||
bool verified{false}, in_destination{false}, base_stopped{false};
|
||||
};
|
||||
struct ExecutionResult { ResultCode code{ResultCode::FAILED}; StopState stop{StopState::UNKNOWN}; SkillResponse response; std::string detail; };
|
||||
struct ExecutionResult { ResultCode code{ResultCode::FAILED}; StopState stop{StopState::UNKNOWN}; SkillResponse response; std::string detail; std::string error_code{}, execution_record_ref{}, wire_result_type{}, wire_result_snapshot{}; };
|
||||
struct GoalRequest {
|
||||
std::string goal_id, robot_id;
|
||||
Trace trace;
|
||||
@@ -63,6 +64,7 @@ struct GoalRequest {
|
||||
std::uint64_t geometry_epoch{0};
|
||||
RosTime capture_after{0};
|
||||
double position_tolerance_m{0.05}, orientation_tolerance_rad{0.1};
|
||||
std::string wire_request_type{}, wire_request_snapshot{};
|
||||
};
|
||||
enum class EventKind { ACCEPTED, REJECTED, FEEDBACK, CANCEL_ACK, RESULT };
|
||||
struct GoalEvent {
|
||||
@@ -72,6 +74,7 @@ struct GoalEvent {
|
||||
std::uint64_t sequence{0};
|
||||
NativeStatus native_status{NativeStatus::UNKNOWN};
|
||||
ExecutionResult result;
|
||||
std::string feedback_snapshot{};
|
||||
};
|
||||
class GoalDriver {
|
||||
public:
|
||||
@@ -81,6 +84,14 @@ class GoalDriver {
|
||||
virtual void send(const GoalRequest&) = 0;
|
||||
virtual void cancel(const std::string& goal_id) = 0;
|
||||
virtual std::vector<GoalEvent> drain_events() = 0;
|
||||
using RequestRecorder = std::function<void(const std::string&, const std::string&, const std::string&)>;
|
||||
void set_request_recorder(RequestRecorder recorder) { request_recorder_=std::move(recorder); }
|
||||
protected:
|
||||
void record_wire_request(const std::string& id,const std::string& type,const std::string& snapshot) {
|
||||
if(request_recorder_)request_recorder_(id,type,snapshot);
|
||||
}
|
||||
private:
|
||||
RequestRecorder request_recorder_;
|
||||
};
|
||||
struct Budgets { Milliseconds readiness{2000}, acceptance{2000}, feedback{5000}, execution{120000}, cancel_stop{5000}; };
|
||||
enum class GoalState { SENDING, ACTIVE, CANCEL_REQUESTED, STOP_UNKNOWN, TERMINAL };
|
||||
@@ -91,6 +102,7 @@ struct GoalRecord {
|
||||
std::uint64_t last_sequence{0};
|
||||
SteadyTime sent_at{}, accepted_at{}, last_feedback{}, cancel_at{};
|
||||
std::optional<ExecutionResult> result;
|
||||
std::string feedback_snapshot{};
|
||||
};
|
||||
class ActiveGoalRegistry {
|
||||
public:
|
||||
@@ -101,15 +113,19 @@ class ActiveGoalRegistry {
|
||||
// Registration is flushed before send. Failed journal writes throw and prevent send.
|
||||
std::optional<std::string> start(GoalRequest, SteadyTime);
|
||||
void pump(SteadyTime);
|
||||
void set_dispatch_recorder(std::function<void(const GoalRequest&)> recorder) { dispatch_recorder_=std::move(recorder); }
|
||||
void request_cancel(const std::string&, SteadyTime);
|
||||
bool robot_locked(const std::string&) const;
|
||||
bool has_unresolved() const;
|
||||
const GoalRecord* find(const std::string&) const;
|
||||
std::optional<GoalRecord> history(const std::string&) const;
|
||||
std::vector<GoalRecord> task_records(const std::string& run_id) const;
|
||||
const std::map<std::string, GoalRecord>& records() const { return records_; }
|
||||
// Caller must verify the authenticated dedicated physical-reconciliation interface.
|
||||
bool reconcile(const std::string& goal_id, const Trace&, StopState, bool authorized, const std::string& evidence_id);
|
||||
private:
|
||||
GoalDriver& driver_;
|
||||
std::function<void(const GoalRequest&)> dispatch_recorder_;
|
||||
std::string journal_path_;
|
||||
Budgets budgets_;
|
||||
std::map<std::string, GoalRecord> records_;
|
||||
@@ -118,6 +134,8 @@ class ActiveGoalRegistry {
|
||||
void append(const GoalRecord&);
|
||||
void ingest(const GoalEvent&, SteadyTime);
|
||||
void load();
|
||||
void prune_terminal();
|
||||
std::deque<std::string> terminal_order_;
|
||||
};
|
||||
class ContextStore {
|
||||
public:
|
||||
@@ -165,6 +183,7 @@ class StageRunner {
|
||||
TickStatus settle(SteadyTime, RosTime);
|
||||
void update_safety(SafetySnapshot value) { safety_ = value; }
|
||||
const std::string& detail() const { return detail_; }
|
||||
const std::string& error_code() const { return error_code_; }
|
||||
const std::string& active_goal_id() const { return active_goal_; }
|
||||
std::uint64_t geometry_epoch() const { return geometry_epoch_; }
|
||||
Holding holding() const { return holding_; }
|
||||
@@ -180,11 +199,13 @@ class StageRunner {
|
||||
SafetySnapshot safety_;
|
||||
Holding holding_{Holding::UNKNOWN};
|
||||
std::uint64_t geometry_epoch_{0};
|
||||
RosTime capture_after_{0}, last_ros_time_{0}, holding_valid_until_{0}, empty_valid_until_{0}, empty_observed_at_{0};
|
||||
RosTime capture_after_{0}, last_ros_time_{0}, holding_valid_until_{0}, holding_observed_at_{0}, empty_valid_until_{0}, empty_observed_at_{0};
|
||||
std::size_t stage_index_{0};
|
||||
std::string active_goal_, source_location_, source_side_, source_column_, source_tier_, verification_goal_, pending_posture_, detail_;
|
||||
std::string active_goal_, source_location_, source_side_, source_column_, source_tier_, verification_goal_, pending_posture_, detail_, error_code_;
|
||||
std::optional<SteadyTime> waiting_since_;
|
||||
std::optional<SteadyTime> motion_waiting_since_;
|
||||
std::optional<SteadyTime> stopped_waiting_since_;
|
||||
std::optional<SteadyTime> corroboration_waiting_since_, settlement_waiting_since_;
|
||||
unsigned reobservations_{0}, adjustments_{0}, serial_{0};
|
||||
enum class PickPhase { ASSESS, REOBSERVE, ADJUST, EXECUTE };
|
||||
PickPhase pick_phase_{PickPhase::ASSESS};
|
||||
|
||||
+48
-25
@@ -38,6 +38,30 @@ bool protocol_matches(NativeStatus native,ResultCode result) {
|
||||
default:return false;
|
||||
}
|
||||
}
|
||||
std::string hex_encode(const std::string& text) {
|
||||
static const char hex[]="0123456789abcdef";std::string out;
|
||||
for(unsigned char c:text){out.push_back(hex[c>>4]);out.push_back(hex[c&15]);}return out;
|
||||
}
|
||||
std::string hex_decode(const std::string& text) {
|
||||
if(text.size()%2||text.find_first_not_of("0123456789abcdef")!=std::string::npos)throw std::runtime_error("corrupt journal evidence");
|
||||
std::string out;for(std::size_t i=0;i<text.size();i+=2)out.push_back(static_cast<char>(std::stoi(text.substr(i,2),nullptr,16)));return out;
|
||||
}
|
||||
GoalRecord parse_record(const std::string& line) {
|
||||
std::istringstream row(line);std::string detail;GoalRecord r;auto& t=r.request.trace;int version=0,skill=-1,state=-1,code=-1,stop=-1;
|
||||
if(!(row>>version>>std::quoted(r.request.goal_id)>>std::quoted(r.request.robot_id)>>std::quoted(t.task_id)>>std::quoted(t.subtask_id)>>std::quoted(t.run_id)>>t.task_revision>>t.plan_version>>t.execution_generation>>t.attempt>>skill>>state>>r.cancel_intent>>r.accepted>>r.last_sequence>>code>>stop>>std::quoted(detail)) || (version!=2&&version!=3&&version!=4) || skill<0 || skill>static_cast<int>(Skill::VERIFY_EMPTY) || state<0 || state>static_cast<int>(GoalState::TERMINAL) || code < -1 || code>static_cast<int>(ResultCode::REJECTED) || stop<0 || stop>1 || r.request.goal_id.empty() || r.request.robot_id.empty() || !valid_trace(t))throw std::runtime_error("corrupt goal journal; startup refused for physical reconciliation");
|
||||
r.request.skill=static_cast<Skill>(skill);r.state=static_cast<GoalState>(state);
|
||||
const auto decoded=hex_decode(detail);
|
||||
if(code>=0){ExecutionResult result;result.code=static_cast<ResultCode>(code);result.stop=static_cast<StopState>(stop);result.detail=decoded;r.result=result;}
|
||||
if(version>=3){std::string type,snapshot,feedback,error,reference;if(!(row>>std::quoted(type)>>std::quoted(snapshot)>>std::quoted(feedback)>>std::quoted(error)>>std::quoted(reference)))throw std::runtime_error("truncated journal evidence");r.request.wire_request_type=hex_decode(type);r.request.wire_request_snapshot=hex_decode(snapshot);r.feedback_snapshot=hex_decode(feedback);const auto e=hex_decode(error),ref=hex_decode(reference);if(r.result){r.result->error_code=e;r.result->execution_record_ref=ref;}else if(!e.empty()||!ref.empty())throw std::runtime_error("result evidence without result");}
|
||||
if(version>=4){std::string type,snapshot;if(!(row>>std::quoted(type)>>std::quoted(snapshot)))throw std::runtime_error("truncated result snapshot");const auto t=hex_decode(type),b=hex_decode(snapshot);if(r.result){r.result->wire_result_type=t;r.result->wire_result_snapshot=b;}else if(!t.empty()||!b.empty())throw std::runtime_error("wire result without result");}
|
||||
row>>std::ws;if(!row.eof())throw std::runtime_error("unexpected goal journal fields");return r;
|
||||
}
|
||||
void scan_journal(const std::string& path,const std::function<void(GoalRecord)>& visitor) {
|
||||
if(!std::filesystem::exists(path))return;
|
||||
std::ifstream in(path);if(!in)throw std::runtime_error("goal journal unreadable; startup refused");std::string line;
|
||||
while(std::getline(in,line)){if(in.eof())throw std::runtime_error("truncated goal journal; startup refused");visitor(parse_record(line));}
|
||||
if(in.bad())throw std::runtime_error("goal journal read failed");
|
||||
}
|
||||
bool valid_meta(const SnapshotMeta& m,const Trace& t,std::uint64_t epoch,RosTime after,RosTime now) {
|
||||
return m.schema_version==1 && same_context(m.trace,t) && valid_trace(m.trace) && !m.source_goal_id.empty() && !m.writer.empty() && m.geometry_epoch==epoch && m.observed_at>0 && m.observed_at>=after && m.observed_at<=now && m.valid_until>now && m.valid_until>=m.observed_at;
|
||||
}
|
||||
@@ -65,15 +89,15 @@ ActiveGoalRegistry::ActiveGoalRegistry(GoalDriver& driver,std::string path,Budge
|
||||
lock_fd_=::open((journal_path_+".lock").c_str(),O_RDWR|O_CREAT|O_CLOEXEC|O_NOFOLLOW,0600);
|
||||
if(lock_fd_<0) throw std::runtime_error("goal journal process lock cannot be opened");
|
||||
if(::flock(lock_fd_,LOCK_EX|LOCK_NB)!=0) {::close(lock_fd_);lock_fd_=-1;throw std::runtime_error("another executor owns this goal journal");}
|
||||
try {load();} catch(...) {::flock(lock_fd_,LOCK_UN);::close(lock_fd_);lock_fd_=-1;throw;}
|
||||
try {load();driver_.set_request_recorder([this](const std::string& id,const std::string& type,const std::string& snapshot){auto it=records_.find(id);if(it==records_.end()||it->second.state!=GoalState::SENDING||type.empty()||snapshot.empty())throw std::runtime_error("invalid wire request snapshot");auto& r=it->second;if(!r.request.wire_request_snapshot.empty())throw std::runtime_error("wire request already frozen");r.request.wire_request_type=type;r.request.wire_request_snapshot=snapshot;append(r);});} catch(...) {::flock(lock_fd_,LOCK_UN);::close(lock_fd_);lock_fd_=-1;throw;}
|
||||
}
|
||||
ActiveGoalRegistry::~ActiveGoalRegistry() {if(lock_fd_>=0){::flock(lock_fd_,LOCK_UN);::close(lock_fd_);}}
|
||||
ActiveGoalRegistry::~ActiveGoalRegistry() {driver_.set_request_recorder({});if(lock_fd_>=0){::flock(lock_fd_,LOCK_UN);::close(lock_fd_);}}
|
||||
void ActiveGoalRegistry::append(const GoalRecord& r) {
|
||||
std::ostringstream out;
|
||||
const auto& t=r.request.trace;
|
||||
std::string detail_hex;
|
||||
if(r.result) {static const char hex[]="0123456789abcdef";for(unsigned char c:r.result->detail){detail_hex.push_back(hex[c>>4]);detail_hex.push_back(hex[c&15]);}}
|
||||
out<<2<<' '<<std::quoted(r.request.goal_id)<<' '<<std::quoted(r.request.robot_id)<<' '<<std::quoted(t.task_id)<<' '<<std::quoted(t.subtask_id)<<' '<<std::quoted(t.run_id)<<' '<<t.task_revision<<' '<<t.plan_version<<' '<<t.execution_generation<<' '<<t.attempt<<' '<<static_cast<int>(r.request.skill)<<' '<<static_cast<int>(r.state)<<' '<<r.cancel_intent<<' '<<r.accepted<<' '<<r.last_sequence<<' '<<(r.result?static_cast<int>(r.result->code):-1)<<' '<<(r.result?static_cast<int>(r.result->stop):0)<<' '<<std::quoted(detail_hex)<<'\n';
|
||||
out<<4<<' '<<std::quoted(r.request.goal_id)<<' '<<std::quoted(r.request.robot_id)<<' '<<std::quoted(t.task_id)<<' '<<std::quoted(t.subtask_id)<<' '<<std::quoted(t.run_id)<<' '<<t.task_revision<<' '<<t.plan_version<<' '<<t.execution_generation<<' '<<t.attempt<<' '<<static_cast<int>(r.request.skill)<<' '<<static_cast<int>(r.state)<<' '<<r.cancel_intent<<' '<<r.accepted<<' '<<r.last_sequence<<' '<<(r.result?static_cast<int>(r.result->code):-1)<<' '<<(r.result?static_cast<int>(r.result->stop):0)<<' '<<std::quoted(detail_hex)<<' '<<std::quoted(hex_encode(r.request.wire_request_type))<<' '<<std::quoted(hex_encode(r.request.wire_request_snapshot))<<' '<<std::quoted(hex_encode(r.feedback_snapshot))<<' '<<std::quoted(hex_encode(r.result?r.result->error_code:std::string{}))<<' '<<std::quoted(hex_encode(r.result?r.result->execution_record_ref:std::string{}))<<' '<<std::quoted(hex_encode(r.result?r.result->wire_result_type:std::string{}))<<' '<<std::quoted(hex_encode(r.result?r.result->wire_result_snapshot:std::string{}))<<'\n';
|
||||
const std::string bytes=out.str();
|
||||
const int fd=::open(journal_path_.c_str(),O_WRONLY|O_CREAT|O_APPEND|O_CLOEXEC|O_NOFOLLOW,0600);
|
||||
bool ok=fd>=0;
|
||||
@@ -85,29 +109,16 @@ void ActiveGoalRegistry::append(const GoalRecord& r) {
|
||||
if(!ok) { journal_failed_=true; throw std::runtime_error("goal journal sync failed; executor quarantined"); }
|
||||
}
|
||||
void ActiveGoalRegistry::load() {
|
||||
if(!std::filesystem::exists(journal_path_)) return;
|
||||
std::ifstream in(journal_path_); if(!in) throw std::runtime_error("goal journal unreadable; startup refused");
|
||||
std::string line;
|
||||
while(std::getline(in,line)) {
|
||||
if(in.eof())throw std::runtime_error("truncated goal journal; startup refused");
|
||||
std::istringstream row(line); std::string detail_hex; GoalRecord r; auto& t=r.request.trace; int version=0,skill=-1,state=-1,code=-1,stop=-1;
|
||||
if(!(row>>version>>std::quoted(r.request.goal_id)>>std::quoted(r.request.robot_id)>>std::quoted(t.task_id)>>std::quoted(t.subtask_id)>>std::quoted(t.run_id)>>t.task_revision>>t.plan_version>>t.execution_generation>>t.attempt>>skill>>state>>r.cancel_intent>>r.accepted>>r.last_sequence>>code>>stop>>std::quoted(detail_hex)) || version!=2 || skill<0 || skill>static_cast<int>(Skill::VERIFY_EMPTY) || state<0 || state>static_cast<int>(GoalState::TERMINAL) || code < -1 || code>static_cast<int>(ResultCode::REJECTED) || stop<0 || stop>1 || r.request.goal_id.empty() || r.request.robot_id.empty() || !valid_trace(t)) throw std::runtime_error("corrupt goal journal; startup refused for physical reconciliation");
|
||||
if(detail_hex.size()%2!=0||detail_hex.find_first_not_of("0123456789abcdef")!=std::string::npos)throw std::runtime_error("corrupt journal evidence");
|
||||
row>>std::ws; if(!row.eof()) throw std::runtime_error("unexpected goal journal fields");
|
||||
r.request.skill=static_cast<Skill>(skill); r.state=static_cast<GoalState>(state);
|
||||
if(code>=0) { ExecutionResult result; result.code=static_cast<ResultCode>(code); result.stop=static_cast<StopState>(stop); for(std::size_t i=0;i<detail_hex.size();i+=2)result.detail.push_back(static_cast<char>(std::stoi(detail_hex.substr(i,2),nullptr,16))); r.result=result; }
|
||||
records_[r.request.goal_id]=r;
|
||||
}
|
||||
if(in.bad()) throw std::runtime_error("goal journal read failed");
|
||||
for(auto& entry:records_) if(entry.second.state!=GoalState::TERMINAL) { entry.second.state=GoalState::STOP_UNKNOWN; entry.second.restarted=true; entry.second.cancel_intent=true; }
|
||||
scan_journal(journal_path_,[this](GoalRecord r){const auto id=r.request.goal_id;const auto terminal=r.state==GoalState::TERMINAL;records_[id]=std::move(r);if(terminal){terminal_order_.erase(std::remove(terminal_order_.begin(),terminal_order_.end(),id),terminal_order_.end());terminal_order_.push_back(id);prune_terminal();}});
|
||||
for(auto& entry:records_)if(entry.second.state!=GoalState::TERMINAL){entry.second.state=GoalState::STOP_UNKNOWN;entry.second.restarted=true;entry.second.cancel_intent=true;}
|
||||
}
|
||||
std::optional<std::string> ActiveGoalRegistry::start(GoalRequest request,SteadyTime now) {
|
||||
if(journal_failed_||request.robot_id.empty()||!valid_trace(request.trace)||robot_locked(request.robot_id)||!driver_.ready(request.skill)) return {};
|
||||
for(const auto& item:records_) if(same_trace(item.second.request.trace,request.trace)) return {};
|
||||
bool duplicate=false;scan_journal(journal_path_,[&](GoalRecord r){if(same_trace(r.request.trace,request.trace))duplicate=true;});if(duplicate)return {};
|
||||
request.goal_id=uuid(); GoalRecord record; record.request=std::move(request); record.sent_at=now; record.last_feedback=now;
|
||||
auto inserted=records_.emplace(record.request.goal_id,std::move(record)); auto& r=inserted.first->second;
|
||||
append(r); // This happens before transport can observe the request.
|
||||
try { driver_.send(r.request); } catch(...) { r.state=GoalState::STOP_UNKNOWN; r.cancel_intent=true; r.cancel_at=now;
|
||||
try { if(dispatch_recorder_)dispatch_recorder_(r.request);driver_.send(r.request); } catch(...) { r.state=GoalState::STOP_UNKNOWN; r.cancel_intent=true; r.cancel_at=now;
|
||||
try {driver_.cancel(r.request.goal_id);} catch(...) {}
|
||||
append(r); }
|
||||
return r.request.goal_id;
|
||||
@@ -119,6 +130,18 @@ bool ActiveGoalRegistry::robot_locked(const std::string& robot) const {
|
||||
}
|
||||
bool ActiveGoalRegistry::has_unresolved() const { if(journal_failed_) return true; for(const auto& item:records_) if(item.second.state!=GoalState::TERMINAL) return true; return false; }
|
||||
const GoalRecord* ActiveGoalRegistry::find(const std::string& id) const { auto it=records_.find(id); return it==records_.end()?nullptr:&it->second; }
|
||||
void ActiveGoalRegistry::prune_terminal() {
|
||||
while(terminal_order_.size()>256){const auto id=terminal_order_.front();terminal_order_.pop_front();auto it=records_.find(id);if(it!=records_.end()&&it->second.state==GoalState::TERMINAL)records_.erase(it);}
|
||||
}
|
||||
std::optional<GoalRecord> ActiveGoalRegistry::history(const std::string& id) const {
|
||||
if(auto r=find(id))return *r;
|
||||
std::optional<GoalRecord> out;scan_journal(journal_path_,[&](GoalRecord r){if(r.request.goal_id==id)out=std::move(r);});return out;
|
||||
}
|
||||
std::vector<GoalRecord> ActiveGoalRegistry::task_records(const std::string& run_id) const {
|
||||
std::map<std::string,GoalRecord> selected;scan_journal(journal_path_,[&](GoalRecord r){if(r.request.trace.run_id==run_id){const auto id=r.request.goal_id;selected[id]=std::move(r);}});
|
||||
for(const auto& item:records_)if(item.second.request.trace.run_id==run_id)selected[item.first]=item.second;
|
||||
std::vector<GoalRecord> out;for(auto& item:selected)out.push_back(std::move(item.second));return out;
|
||||
}
|
||||
void ActiveGoalRegistry::request_cancel(const std::string& id,SteadyTime now) {
|
||||
auto it=records_.find(id); if(it==records_.end()) return; auto& r=it->second;
|
||||
if(r.state==GoalState::TERMINAL||r.cancel_intent) return;
|
||||
@@ -146,10 +169,10 @@ void ActiveGoalRegistry::ingest(const GoalEvent& event,SteadyTime now) {
|
||||
break;
|
||||
case EventKind::REJECTED:
|
||||
if(r.accepted||r.result) { r.state=GoalState::STOP_UNKNOWN; append(r); break; }
|
||||
r.state=GoalState::TERMINAL; r.result=ExecutionResult{ResultCode::REJECTED,StopState::CONFIRMED,{},"server rejected before execution"}; append(r); break;
|
||||
r.state=GoalState::TERMINAL; r.result=ExecutionResult{ResultCode::REJECTED,StopState::CONFIRMED,{},"server rejected before execution",{},{} }; append(r);terminal_order_.push_back(event.goal_id);prune_terminal(); break;
|
||||
case EventKind::FEEDBACK:
|
||||
if(!r.accepted||r.cancel_intent||r.restarted||event.sequence<=r.last_sequence) return;
|
||||
r.last_sequence=event.sequence; r.last_feedback=now; break;
|
||||
if(!r.accepted||r.restarted||event.sequence<=r.last_sequence) return;
|
||||
r.last_sequence=event.sequence; if(!r.cancel_intent)r.last_feedback=now; r.feedback_snapshot=event.feedback_snapshot; append(r); break;
|
||||
case EventKind::CANCEL_ACK: break; // An ACK says nothing about physical stop.
|
||||
case EventKind::RESULT:
|
||||
if(!protocol_matches(event.native_status,event.result.code)||event.result.stop!=StopState::CONFIRMED) {
|
||||
@@ -157,7 +180,7 @@ void ActiveGoalRegistry::ingest(const GoalEvent& event,SteadyTime now) {
|
||||
if(!r.cancel_intent) { r.cancel_intent=true; r.cancel_at=now; append(r); try { driver_.cancel(event.goal_id); } catch(...) {} }
|
||||
break;
|
||||
}
|
||||
r.result=event.result; r.state=GoalState::TERMINAL; append(r); break;
|
||||
r.result=event.result; r.state=GoalState::TERMINAL; append(r);terminal_order_.push_back(event.goal_id);prune_terminal(); break;
|
||||
}
|
||||
}
|
||||
void ActiveGoalRegistry::pump(SteadyTime now) {
|
||||
@@ -183,7 +206,7 @@ void ActiveGoalRegistry::pump(SteadyTime now) {
|
||||
bool ActiveGoalRegistry::reconcile(const std::string& id,const Trace& trace,StopState stop,bool authorized,const std::string& evidence) {
|
||||
auto it=records_.find(id);
|
||||
if(!authorized||evidence.empty()||stop!=StopState::CONFIRMED||it==records_.end()||!same_trace(it->second.request.trace,trace)||it->second.state==GoalState::TERMINAL) return false;
|
||||
auto& r=it->second; r.result=ExecutionResult{ResultCode::CANCELED,StopState::CONFIRMED,{},"authorized reconciliation: "+evidence}; r.state=GoalState::TERMINAL; append(r); return true;
|
||||
auto& r=it->second; r.result=ExecutionResult{ResultCode::CANCELED,StopState::CONFIRMED,{},"authorized reconciliation: "+evidence,{},{} }; r.state=GoalState::TERMINAL; append(r);terminal_order_.push_back(id);prune_terminal(); return true;
|
||||
}
|
||||
void ContextStore::replace_target(TargetBinding b) { std::lock_guard<std::mutex> lock(mutex_); target_=std::move(b); }
|
||||
void ContextStore::replace_placement(PlacementBinding b) { std::lock_guard<std::mutex> lock(mutex_); placement_=std::move(b); }
|
||||
|
||||
+41
-13
@@ -33,15 +33,31 @@ TickStatus StageRunner::settle(SteadyTime now,RosTime ros) {
|
||||
if(last_ros_time_>0&&ros<last_ros_time_){empty_valid_until_=0;holding_valid_until_=0;context_.invalidate_geometry();settlement_failure_=TickStatus::INTERVENTION_REQUIRED;return *settlement_failure_;}
|
||||
last_ros_time_=ros;
|
||||
if(settlement_failure_)return *settlement_failure_;
|
||||
if(settlement_done_)return empty_verified(ros)?TickStatus::SUCCESS:TickStatus::INTERVENTION_REQUIRED;
|
||||
if(settlement_done_) {
|
||||
if(!safe(ros)||!empty_verified(ros))return TickStatus::INTERVENTION_REQUIRED;
|
||||
if(!safety_.stationary||safety_.observed_at<empty_observed_at_) {
|
||||
if(!settlement_waiting_since_)settlement_waiting_since_=now;
|
||||
if(now-*settlement_waiting_since_>=budgets_.readiness)return TickStatus::INTERVENTION_REQUIRED;
|
||||
return TickStatus::RUNNING;
|
||||
}
|
||||
return safety_.holding==Holding::EMPTY?TickStatus::SUCCESS:TickStatus::INTERVENTION_REQUIRED;
|
||||
}
|
||||
try { registry_.pump(now); } catch(const std::exception& e) {detail_=e.what();return TickStatus::INTERVENTION_REQUIRED;}
|
||||
for(const auto& entry:registry_.records())if(entry.second.request.robot_id==task_.robot_id&&entry.second.state==GoalState::STOP_UNKNOWN)return TickStatus::INTERVENTION_REQUIRED;
|
||||
if(!settlement_started_&®istry_.robot_locked(task_.robot_id))return TickStatus::RUNNING;
|
||||
if(!safe(ros))return TickStatus::INTERVENTION_REQUIRED;
|
||||
if(!safety_.stationary) {
|
||||
if(!settlement_waiting_since_)settlement_waiting_since_=now;
|
||||
if(now-*settlement_waiting_since_>=budgets_.readiness)return TickStatus::INTERVENTION_REQUIRED;
|
||||
return TickStatus::RUNNING;
|
||||
}
|
||||
settlement_waiting_since_.reset();
|
||||
if(!settlement_started_) {
|
||||
if(registry_.robot_locked(task_.robot_id))return TickStatus::RUNNING;
|
||||
if(!safe(ros)||!safety_.stationary)return TickStatus::INTERVENTION_REQUIRED;
|
||||
if(delivered_&&empty_verified(ros)){settlement_done_=true;return TickStatus::SUCCESS;}
|
||||
for(const auto& entry:registry_.records()) {
|
||||
const auto& q=entry.second.request;
|
||||
if(delivered_&&empty_verified(ros)){settlement_done_=true;return settle(now,ros);}
|
||||
for(const auto& record:registry_.task_records(task_.trace.run_id)) {
|
||||
const auto& q=record.request;
|
||||
if(q.trace.task_id==task_.trace.task_id&&q.trace.run_id==task_.trace.run_id&&q.skill==Skill::PLACE)settlement_place_=true;
|
||||
}
|
||||
settlement_started_=true;active_goal_.clear();waiting_since_.reset();capture_after_=ros;
|
||||
@@ -59,7 +75,7 @@ TickStatus StageRunner::settle(SteadyTime now,RosTime ros) {
|
||||
catch(const std::exception& e){detail_=e.what();settlement_failure_=TickStatus::INTERVENTION_REQUIRED;return *settlement_failure_;}
|
||||
delivered_=true;
|
||||
}
|
||||
holding_=Holding::EMPTY;empty_valid_until_=response.evidence->valid_until;empty_observed_at_=response.evidence->observed_at;settlement_done_=true;return TickStatus::SUCCESS;
|
||||
holding_=Holding::EMPTY;empty_valid_until_=response.evidence->valid_until;empty_observed_at_=response.evidence->observed_at;settlement_done_=true;return settle(now,ros);
|
||||
}
|
||||
GoalRequest StageRunner::make_request(Stage stage,Skill skill,RosTime ros) {
|
||||
GoalRequest q;q.robot_id=task_.robot_id;q.trace=task_.trace;q.trace.subtask_id=std::string(stage_name(stage))+"/"+std::to_string(++serial_);q.trace.attempt=1;q.skill=skill;q.target_id=task_.target_id;q.destination_id=task_.destination_id;q.shelf=task_.source_shelf;q.geometry_epoch=geometry_epoch_;q.capture_after=std::max(capture_after_,ros);q.position_tolerance_m=task_.position_tolerance_m;q.orientation_tolerance_rad=task_.orientation_tolerance_rad;
|
||||
@@ -92,9 +108,17 @@ TickStatus StageRunner::run_goal(Stage stage,Skill skill,SteadyTime now,RosTime
|
||||
}
|
||||
if(active_goal_.empty()) {
|
||||
if(registry_.robot_locked(task_.robot_id))return fail("unresolved goal retains robot resource");
|
||||
if(!safety_.stationary)return fail("fresh stationary evidence required before dispatch");
|
||||
if(!safety_.stationary) { if(!stopped_waiting_since_)stopped_waiting_since_=now; if(now-*stopped_waiting_since_>=budgets_.readiness)return fail("fresh stationary evidence deadline exceeded before dispatch",false); return TickStatus::RUNNING; }
|
||||
stopped_waiting_since_.reset();
|
||||
const bool carrying_motion=skill==Skill::TRANSPORT_POSTURE||skill==Skill::PLACE||(skill==Skill::NAVIGATE&&stage==Stage::NAVIGATE_DESTINATION);
|
||||
const bool empty_motion=skill==Skill::PICK||skill==Skill::ADJUST_POSTURE||(skill==Skill::NAVIGATE&&stage!=Stage::NAVIGATE_DESTINATION);
|
||||
const auto proof_at=carrying_motion?holding_observed_at_:empty_motion?empty_observed_at_:0;
|
||||
if(proof_at>0&&safety_.observed_at<proof_at) {
|
||||
if(!corroboration_waiting_since_)corroboration_waiting_since_=now;
|
||||
if(now-*corroboration_waiting_since_>=budgets_.readiness)return fail("robot state did not corroborate independent verification before deadline",false);
|
||||
return TickStatus::RUNNING;
|
||||
}
|
||||
corroboration_waiting_since_.reset();
|
||||
if(carrying_motion&&holding_valid_until_<=ros){holding_=Holding::UNKNOWN;return fail("independent held-target verification expired before motion dispatch");}
|
||||
if(carrying_motion&&safety_.holding!=Holding::HOLDING_TARGET){holding_=Holding::UNKNOWN;return fail("fresh holding state contradicts verified target; motion blocked");}
|
||||
if(empty_motion&&safety_.holding!=Holding::EMPTY){holding_=Holding::UNKNOWN;return fail("fresh empty-hand evidence required for motion");}
|
||||
@@ -104,12 +128,16 @@ TickStatus StageRunner::run_goal(Stage stage,Skill skill,SteadyTime now,RosTime
|
||||
catch(const std::exception& error) { return fail(error.what()); }
|
||||
return TickStatus::RUNNING;
|
||||
}
|
||||
const auto* record=registry_.find(active_goal_);if(!record)return fail("active goal missing from registry");
|
||||
if(record->state==GoalState::STOP_UNKNOWN)return fail("physical stop unknown; robot quarantined");
|
||||
const auto saved_record=registry_.history(active_goal_);const auto* record=saved_record?&*saved_record:nullptr;if(!record)return fail("active goal missing from registry");
|
||||
if(error_code_.empty()&&record->result&&!record->result->error_code.empty()&&(record->state==GoalState::STOP_UNKNOWN||record->cancel_intent||record->result->code!=ResultCode::COMPLETED||record->result->stop!=StopState::CONFIRMED||!record->result->response.valid))error_code_=record->result->error_code;
|
||||
if(record->state==GoalState::STOP_UNKNOWN)return fail("physical stop unknown; robot quarantined");
|
||||
if(record->state!=GoalState::TERMINAL)return TickStatus::RUNNING;
|
||||
if(record->cancel_intent||!record->result||record->result->code!=ResultCode::COMPLETED||record->result->stop!=StopState::CONFIRMED)return fail("goal failed/canceled/timed out; automatic motion retry disabled",false);
|
||||
if(record->cancel_intent||!record->result||record->result->code!=ResultCode::COMPLETED||record->result->stop!=StopState::CONFIRMED) {return fail("goal failed/canceled/timed out; automatic motion retry disabled",false);}
|
||||
response=record->result->response;completed_goal=active_goal_;
|
||||
if(!response.valid)return fail("typed skill response invalid or absent");
|
||||
if(!response.valid) {
|
||||
const bool semantic_ambiguity=(skill==Skill::LOCATE_SHELF_COLUMN||skill==Skill::LOCALIZE_TARGET)&&(record->result->error_code=="NOT_FOUND"||record->result->error_code=="AMBIGUOUS");
|
||||
return fail("typed skill response invalid or absent",!semantic_ambiguity);
|
||||
}
|
||||
if(skill==Skill::NAVIGATE&&(!response.base_stopped||!response.final_pose||!record->request.registered_pose||!within_tolerance(*response.final_pose,*record->request.registered_pose,task_.position_tolerance_m,task_.orientation_tolerance_rad)))return fail("navigation stopped pose outside exact tolerance or unknown");
|
||||
if((skill==Skill::VERIFY_PICK||skill==Skill::VERIFY_TRANSPORT||skill==Skill::VERIFY_PLACE||skill==Skill::VERIFY_EMPTY)&&!evidence_valid(response,record->request,ros))return fail("verification evidence stale or mismatched");
|
||||
if(skill==Skill::LOCALIZE_TARGET&&(!response.target||response.target->meta.source_goal_id!=active_goal_||!valid_target(*response.target,task_.trace,task_.target_id,geometry_epoch_,record->request.capture_after,ros)))return fail("invalid localization snapshot");
|
||||
@@ -181,12 +209,12 @@ TickStatus StageRunner::tick(Stage stage,SteadyTime now,RosTime ros) {
|
||||
if(pick_phase_==PickPhase::ADJUST) {result=invoke(Skill::ADJUST_POSTURE);if(result!=TickStatus::SUCCESS)return result;if(!response.base_stopped)return fail("posture stop evidence missing");geometry_changed();pick_phase_=PickPhase::REOBSERVE;return TickStatus::RUNNING;}
|
||||
result=invoke(Skill::PICK);if(result==TickStatus::SUCCESS)holding_=Holding::UNKNOWN;break;
|
||||
case Stage::VERIFY_PICK:
|
||||
result=invoke(Skill::VERIFY_PICK);if(result==TickStatus::SUCCESS) {if(!response.verified||response.target_id!=task_.target_id||response.holding!=Holding::HOLDING_TARGET||!response.base_stopped)return fail("independent pick verification not confirmed");holding_=Holding::HOLDING_TARGET;holding_valid_until_=response.evidence->valid_until;}break;
|
||||
result=invoke(Skill::VERIFY_PICK);if(result==TickStatus::SUCCESS) {if(!response.verified||response.target_id!=task_.target_id||response.holding!=Holding::HOLDING_TARGET||!response.base_stopped)return fail("independent pick verification not confirmed");holding_=Holding::HOLDING_TARGET;holding_valid_until_=response.evidence->valid_until;holding_observed_at_=response.evidence->observed_at;}break;
|
||||
case Stage::TRANSPORT_POSTURE:
|
||||
if(holding_!=Holding::HOLDING_TARGET)return fail("transport posture requires verified held target");
|
||||
result=invoke(Skill::TRANSPORT_POSTURE);if(result==TickStatus::SUCCESS) {if(!response.base_stopped)return fail("transport posture stop unconfirmed");geometry_changed();}break;
|
||||
case Stage::VERIFY_TRANSPORT:
|
||||
result=invoke(Skill::VERIFY_TRANSPORT);if(result==TickStatus::SUCCESS){if(!response.verified||response.holding!=Holding::HOLDING_TARGET||response.target_id!=task_.target_id||!response.base_stopped)return fail("independent transport verification not confirmed");holding_valid_until_=response.evidence->valid_until;}break;
|
||||
result=invoke(Skill::VERIFY_TRANSPORT);if(result==TickStatus::SUCCESS){if(!response.verified||response.holding!=Holding::HOLDING_TARGET||response.target_id!=task_.target_id||!response.base_stopped)return fail("independent transport verification not confirmed");holding_valid_until_=response.evidence->valid_until;holding_observed_at_=response.evidence->observed_at;}break;
|
||||
case Stage::CHECK_FREE_SPACE:
|
||||
if(task_.route!="LEGACY"){result=TickStatus::SUCCESS;break;}
|
||||
result=invoke(Skill::CHECK_FREE_SPACE);if(result==TickStatus::SUCCESS)context_.replace_placement(*response.placement);break;
|
||||
@@ -196,7 +224,7 @@ TickStatus StageRunner::tick(Stage stage,SteadyTime now,RosTime ros) {
|
||||
if(place_refresh_started_){
|
||||
result=invoke(Skill::VERIFY_TRANSPORT);if(result!=TickStatus::SUCCESS)return result;
|
||||
if(!response.verified||response.holding!=Holding::HOLDING_TARGET||response.target_id!=task_.target_id||!response.base_stopped)return fail("held-target refresh before place not confirmed");
|
||||
holding_valid_until_=response.evidence->valid_until;place_refresh_started_=false;place_refresh_done_=true;return TickStatus::RUNNING;
|
||||
holding_valid_until_=response.evidence->valid_until;holding_observed_at_=response.evidence->observed_at;place_refresh_started_=false;place_refresh_done_=true;return TickStatus::RUNNING;
|
||||
}
|
||||
result=invoke(Skill::PLACE);if(result==TickStatus::SUCCESS)holding_=Holding::UNKNOWN;break;
|
||||
case Stage::VERIFY_PLACE:
|
||||
|
||||
@@ -0,0 +1,11 @@
|
||||
#include "workflow_fixture.hpp"
|
||||
#include <fstream>
|
||||
int main(){
|
||||
Simulator driver;const auto path=Fixture::test_root()+"/dispatch-audit.journal";
|
||||
ActiveGoalRegistry registry(driver,path);GoalRequest q;q.robot_id="robot";q.trace={"task","pick","run",1,1,1,1};q.skill=Skill::PICK;
|
||||
bool observed=false;
|
||||
registry.set_dispatch_recorder([&](const GoalRequest& request){std::ifstream durable(path);std::string line;std::getline(durable,line);assert(line.find(request.goal_id)!=std::string::npos);assert(driver.sent.empty());observed=true;throw std::runtime_error("task audit journal unavailable");});
|
||||
auto id=registry.start(q,SteadyTime{});assert(id);assert(observed);assert(driver.sent.empty());assert(registry.robot_locked("robot"));
|
||||
q.trace.attempt=2;assert(!registry.start(q,SteadyTime{}));
|
||||
std::cout<<"dispatch manifest audit failure prevents transport and retains lock\n";
|
||||
}
|
||||
@@ -0,0 +1,31 @@
|
||||
#include "workflow_fixture.hpp"
|
||||
struct EvidenceDriver:Simulator {
|
||||
std::string sabotage;
|
||||
void send(const GoalRequest& q)override {
|
||||
if(!sabotage.empty()){std::filesystem::remove(sabotage);std::filesystem::create_directory(sabotage);}
|
||||
record_wire_request(q.goal_id,"bt_skill_interfaces/action/ExecuteManipulation_Goal","000102ff");
|
||||
sent.push_back(q);
|
||||
}
|
||||
};
|
||||
int main(){
|
||||
const auto path=Fixture::test_root()+"/evidence.journal"; EvidenceDriver driver; std::string id;
|
||||
GoalRequest q;q.robot_id="robot";q.trace={"task","pick","run",1,1,1,1};q.skill=Skill::PICK;
|
||||
{ActiveGoalRegistry registry(driver,path);id=*registry.start(q,SteadyTime{});
|
||||
assert(registry.find(id)->request.wire_request_snapshot=="000102ff");
|
||||
GoalEvent e;e.goal_id=id;e.trace=q.trace;e.kind=EventKind::ACCEPTED;driver.events.push_back(e);registry.pump(SteadyTime{});
|
||||
e.kind=EventKind::FEEDBACK;e.sequence=2;e.feedback_snapshot="{\"phase\":3,\"message\":\"executing\\nchunk\",\"progress\":0.25}";driver.events.push_back(e);registry.pump(SteadyTime{});
|
||||
e.sequence=1;e.feedback_snapshot="old feedback";driver.events.push_back(e);registry.pump(SteadyTime{});
|
||||
assert(registry.find(id)->feedback_snapshot.find("executing")!=std::string::npos);
|
||||
registry.request_cancel(id,SteadyTime{});e.sequence=3;e.feedback_snapshot="STOPPING";driver.events.push_back(e);registry.pump(SteadyTime{});assert(registry.find(id)->feedback_snapshot=="STOPPING");assert(registry.robot_locked("robot"));
|
||||
e.kind=EventKind::RESULT;e.native_status=NativeStatus::ABORTED;e.result.code=ResultCode::FAILED;e.result.stop=StopState::CONFIRMED;e.result.error_code="VLA_INFERENCE_TIMEOUT";e.result.execution_record_ref="records/run/pick.json";e.result.wire_result_type="bt_skill_interfaces/action/ExecuteManipulation_Result";e.result.wire_result_snapshot="000abbff";driver.events.push_back(e);registry.pump(SteadyTime{});
|
||||
}
|
||||
{ActiveGoalRegistry registry(driver,path);auto r=registry.find(id);assert(r);
|
||||
assert(r->request.wire_request_type=="bt_skill_interfaces/action/ExecuteManipulation_Goal");
|
||||
assert(r->request.wire_request_snapshot=="000102ff");assert(r->feedback_snapshot=="STOPPING");
|
||||
assert(r->result->wire_result_type=="bt_skill_interfaces/action/ExecuteManipulation_Result");assert(r->result->wire_result_snapshot=="000abbff");assert(r->result->error_code=="VLA_INFERENCE_TIMEOUT");assert(r->result->execution_record_ref=="records/run/pick.json");
|
||||
}
|
||||
const auto bad=Fixture::test_root()+"/wire-write-failure.journal";EvidenceDriver broken;broken.sabotage=bad;
|
||||
{ActiveGoalRegistry registry(broken,bad);try{registry.start(q,SteadyTime{});}catch(const std::exception&){}assert(broken.sent.empty());assert(registry.robot_locked("robot"));}
|
||||
std::filesystem::remove(bad);
|
||||
std::cout<<"wire request, ordered feedback and structured result survive restart; snapshot storage failure prevents transport\n";
|
||||
}
|
||||
@@ -1,2 +1,23 @@
|
||||
#include "workflow_fixture.hpp"
|
||||
int main(){Fixture f("refresh_readiness");auto r=f.runner();r.update_safety({true,true,Holding::EMPTY,1000000,9000000000});assert(r.tick(Stage::PREFLIGHT,SteadyTime{},1000000)==TickStatus::RUNNING);assert(r.tick(Stage::PREFLIGHT,SteadyTime{}+Milliseconds(1),2000000)==TickStatus::SUCCESS);f.driver.unavailable_skill=Skill::NAVIGATE;TickStatus status=TickStatus::RUNNING;for(int i=1;i<=60&&status==TickStatus::RUNNING;++i){f.driver.now=2000000+i*100000000LL;r.update_safety({true,true,Holding::EMPTY,f.driver.now,f.driver.now+1000000000});status=r.tick(Stage::NAVIGATE_OBSERVE,SteadyTime{}+Milliseconds(i*100),f.driver.now);}assert(status==TickStatus::FAILURE);assert(f.count(Skill::NAVIGATE)==0);std::cout<<"after 6000ms unavailable navigation: still_running="<<(status==TickStatus::RUNNING)<<" verify_empty_calls="<<f.count(Skill::VERIFY_EMPTY)<<" nav_calls="<<f.count(Skill::NAVIGATE)<<"\n";}
|
||||
void stopped_state_race(bool recover) {
|
||||
Fixture f(recover?"stopped_recovers":"stopped_expires"); auto r=f.runner();
|
||||
r.update_safety({true,true,Holding::EMPTY,1000000,9000000000});
|
||||
assert(r.tick(Stage::PREFLIGHT,SteadyTime{},1000000)==TickStatus::RUNNING);
|
||||
assert(r.tick(Stage::PREFLIGHT,SteadyTime{}+Milliseconds(1),2000000)==TickStatus::SUCCESS);
|
||||
assert(r.tick(Stage::NAVIGATE_OBSERVE,SteadyTime{}+Milliseconds(2),3000000)==TickStatus::RUNNING);
|
||||
r.update_safety({true,false,Holding::EMPTY,4000000,9000000000});
|
||||
assert(r.tick(Stage::NAVIGATE_OBSERVE,SteadyTime{}+Milliseconds(3),4000000)==TickStatus::SUCCESS);
|
||||
// Terminal result can arrive before the independent stopped-state subscription.
|
||||
assert(r.tick(Stage::LOCATE_SHELF_COLUMN,SteadyTime{}+Milliseconds(4),5000000)==TickStatus::RUNNING);
|
||||
assert(f.count(Skill::LOCATE_SHELF_COLUMN)==0);
|
||||
if(recover) {
|
||||
r.update_safety({true,true,Holding::EMPTY,6000000,9000000000});
|
||||
assert(r.tick(Stage::LOCATE_SHELF_COLUMN,SteadyTime{}+Milliseconds(5),6000000)==TickStatus::RUNNING);
|
||||
assert(f.count(Skill::LOCATE_SHELF_COLUMN)==1);
|
||||
assert(r.tick(Stage::LOCATE_SHELF_COLUMN,SteadyTime{}+Milliseconds(6),7000000)==TickStatus::SUCCESS);
|
||||
} else {
|
||||
assert(r.tick(Stage::LOCATE_SHELF_COLUMN,SteadyTime{}+Milliseconds(2004),2005000000)==TickStatus::FAILURE);
|
||||
assert(f.count(Skill::LOCATE_SHELF_COLUMN)==0);
|
||||
}
|
||||
}
|
||||
int main(){stopped_state_race(true);stopped_state_race(false);Fixture f("refresh_readiness");auto r=f.runner();r.update_safety({true,true,Holding::EMPTY,1000000,9000000000});assert(r.tick(Stage::PREFLIGHT,SteadyTime{},1000000)==TickStatus::RUNNING);assert(r.tick(Stage::PREFLIGHT,SteadyTime{}+Milliseconds(1),2000000)==TickStatus::SUCCESS);f.driver.unavailable_skill=Skill::NAVIGATE;TickStatus status=TickStatus::RUNNING;for(int i=1;i<=60&&status==TickStatus::RUNNING;++i){f.driver.now=2000000+i*100000000LL;r.update_safety({true,true,Holding::EMPTY,f.driver.now,f.driver.now+1000000000});status=r.tick(Stage::NAVIGATE_OBSERVE,SteadyTime{}+Milliseconds(i*100),f.driver.now);}assert(status==TickStatus::FAILURE);assert(f.count(Skill::NAVIGATE)==0);std::cout<<"stopped-state race bounded; unavailable readiness bounded\n";}
|
||||
|
||||
@@ -0,0 +1,8 @@
|
||||
#include "workflow_fixture.hpp"
|
||||
#include <set>
|
||||
int main(){Simulator d;const auto path=Fixture::test_root()+"/retention.journal";std::string first;Trace original;
|
||||
{ActiveGoalRegistry r(d,path);for(int i=0;i<280;++i){GoalRequest q;q.robot_id="r";q.trace={"task","step"+std::to_string(i),"run",1,1,1,1};auto id=r.start(q,SteadyTime{});assert(id);if(i==0){first=*id;original=q.trace;}r.pump(SteadyTime{});}assert(r.records().size()<=256);assert(!r.robot_locked("r"));auto old=r.history(first);assert(old&&old->state==GoalState::TERMINAL&&same_trace(old->request.trace,original));const auto history=r.task_records("run");assert(history.size()==280);std::set<std::string> identities;for(const auto& record:history){assert(!record.request.goal_id.empty());identities.insert(record.request.goal_id);}assert(identities.size()==280);}
|
||||
{ActiveGoalRegistry r(d,path);assert(r.records().size()<=256);GoalRequest q;q.robot_id="r";q.trace=original;assert(!r.start(q,SteadyTime{}));q.trace.subtask_id="pending";auto id=r.start(q,SteadyTime{});assert(id);}
|
||||
{ActiveGoalRegistry r(d,path);assert(r.robot_locked("r"));assert(r.records().size()<=257);}
|
||||
std::cout<<"terminal history bounded and archived attempts never replayed\n";
|
||||
}
|
||||
@@ -0,0 +1,12 @@
|
||||
#include "workflow_fixture.hpp"
|
||||
void semantic_failure(const std::string& code,bool ordinary) {
|
||||
Fixture f("semantic_"+code);auto r=f.runner();Workflow flow(r);unsigned i=0;
|
||||
for(;f.count(Skill::LOCATE_SHELF_COLUMN)==0&&i<40;++i){f.driver.now=1000000+i*1000000;r.update_safety({true,true,Holding::EMPTY,f.driver.now,f.driver.now+1000000000});assert(flow.tick(SteadyTime{}+Milliseconds(i),f.driver.now)==TickStatus::RUNNING);}
|
||||
assert(f.count(Skill::LOCATE_SHELF_COLUMN)==1);
|
||||
for(auto& e:f.driver.events)if(e.kind==EventKind::RESULT){e.result.response.valid=false;e.result.error_code=code;}
|
||||
f.driver.now+=1000000;r.update_safety({true,true,Holding::EMPTY,f.driver.now,f.driver.now+1000000000});
|
||||
assert(flow.tick(SteadyTime{}+Milliseconds(++i),f.driver.now)==(ordinary?TickStatus::FAILURE:TickStatus::INTERVENTION_REQUIRED));
|
||||
assert(r.error_code()==code);assert(f.count(Skill::PICK)==0);
|
||||
if(ordinary){r.halt(SteadyTime{}+Milliseconds(i));TickStatus status=TickStatus::RUNNING;for(unsigned n=0;n<5&&status==TickStatus::RUNNING;++n){f.driver.now+=1000000;r.update_safety({true,true,Holding::EMPTY,f.driver.now,f.driver.now+1000000000});status=r.settle(SteadyTime{}+Milliseconds(++i),f.driver.now);}assert(status==TickStatus::SUCCESS);assert(r.error_code()==code);assert(f.count(Skill::PICK)==0);}
|
||||
}
|
||||
int main(){semantic_failure("AMBIGUOUS",true);semantic_failure("NOT_FOUND",true);semantic_failure("INVALID_RESULT",false);std::cout<<"typed readonly ambiguity allows independently verified safe settlement\n";}
|
||||
@@ -2,7 +2,7 @@
|
||||
struct FailedPlaceDriver : Simulator {
|
||||
void send(const GoalRequest& q) override {
|
||||
Simulator::send(q);
|
||||
if(q.skill==Skill::PLACE){auto& event=events.back();event.native_status=NativeStatus::ABORTED;event.result.code=ResultCode::FAILED;}
|
||||
if(q.skill==Skill::PLACE){auto& event=events.back();event.native_status=NativeStatus::ABORTED;event.result.code=ResultCode::FAILED;event.result.error_code="VLA_EXECUTION_FAILED";}
|
||||
}
|
||||
};
|
||||
int main(){
|
||||
@@ -13,9 +13,11 @@ int main(){
|
||||
for(;i<200&&status==TickStatus::RUNNING;++i){driver.now=1000000+i*1000000;runner.update_safety({true,true,driver.sensor_holding,driver.now,driver.now+1000000000});status=flow.tick(SteadyTime{}+Milliseconds(i),driver.now);}
|
||||
assert(status==TickStatus::FAILURE);assert(deliveries==0);
|
||||
runner.halt(SteadyTime{}+Milliseconds(i));
|
||||
runner.update_safety({true,false,driver.sensor_holding,driver.now,driver.now+1000000000});
|
||||
assert(runner.settle(SteadyTime{}+Milliseconds(i),driver.now)==TickStatus::RUNNING);
|
||||
// Settlement is read-only except for the idempotent business receipt.
|
||||
for(status=TickStatus::RUNNING;i<300&&status==TickStatus::RUNNING;++i){driver.now=1000000+i*1000000;runner.update_safety({true,true,driver.sensor_holding,driver.now,driver.now+1000000000});status=runner.settle(SteadyTime{}+Milliseconds(i),driver.now);}
|
||||
assert(status==TickStatus::SUCCESS);assert(deliveries==1);
|
||||
assert(status==TickStatus::SUCCESS);assert(deliveries==1);assert(runner.error_code()=="VLA_EXECUTION_FAILED");
|
||||
unsigned place=0,verify=0;for(const auto&q:driver.sent){place+=q.skill==Skill::PLACE;verify+=q.skill==Skill::VERIFY_PLACE;}
|
||||
assert(place==1&&verify==1);assert(runner.empty_verified(driver.now));
|
||||
assert(runner.settle(SteadyTime{}+Milliseconds(i),driver.now)==TickStatus::SUCCESS);assert(deliveries==1);
|
||||
|
||||
@@ -0,0 +1,16 @@
|
||||
#include "workflow_fixture.hpp"
|
||||
void holding_alignment(bool contradictory){Fixture f(contradictory?"fresh_contradiction":"holding_lag");auto r=f.runner();Workflow flow(r);unsigned i=0;
|
||||
for(;flow.current_stage()!=Stage::TRANSPORT_POSTURE&&i<100;++i){f.driver.now=1000000+i*1000000;r.update_safety({true,true,f.driver.sensor_holding,f.driver.now,f.driver.now+1000000000});assert(flow.tick(SteadyTime{}+Milliseconds(i),f.driver.now)==TickStatus::RUNNING);}
|
||||
assert(flow.current_stage()==Stage::TRANSPORT_POSTURE);auto proof=f.registry.history(f.driver.sent.back().goal_id)->result->response.evidence->observed_at;
|
||||
f.driver.now+=1000000;r.update_safety({true,true,Holding::EMPTY,contradictory?f.driver.now:proof-1,f.driver.now+1000000000});
|
||||
auto status=flow.tick(SteadyTime{}+Milliseconds(++i),f.driver.now);
|
||||
if(contradictory){assert(status==TickStatus::INTERVENTION_REQUIRED);assert(f.count(Skill::TRANSPORT_POSTURE)==0);return;}
|
||||
assert(status==TickStatus::RUNNING);assert(f.count(Skill::TRANSPORT_POSTURE)==0);
|
||||
f.driver.now+=1000000;r.update_safety({true,true,Holding::HOLDING_TARGET,f.driver.now,f.driver.now+1000000000});
|
||||
assert(flow.tick(SteadyTime{}+Milliseconds(++i),f.driver.now)==TickStatus::RUNNING);assert(f.count(Skill::TRANSPORT_POSTURE)==1);
|
||||
}
|
||||
void empty_alignment(){Fixture f("empty_lag");auto r=f.runner();r.update_safety({true,true,Holding::EMPTY,1000000,9000000000});assert(r.tick(Stage::PREFLIGHT,SteadyTime{},1000000)==TickStatus::RUNNING);assert(r.tick(Stage::PREFLIGHT,SteadyTime{}+Milliseconds(1),2000000)==TickStatus::SUCCESS);
|
||||
r.update_safety({true,true,Holding::UNKNOWN,999999,9000000000});assert(r.tick(Stage::NAVIGATE_OBSERVE,SteadyTime{}+Milliseconds(2),3000000)==TickStatus::RUNNING);assert(f.count(Skill::NAVIGATE)==0);
|
||||
r.update_safety({true,true,Holding::EMPTY,4000000,9000000000});assert(r.tick(Stage::NAVIGATE_OBSERVE,SteadyTime{}+Milliseconds(3),4000000)==TickStatus::RUNNING);assert(f.count(Skill::NAVIGATE)==1);
|
||||
}
|
||||
int main(){holding_alignment(false);holding_alignment(true);empty_alignment();std::cout<<"older robot state waits for proof corroboration; fresh contradictions block motion\n";}
|
||||
Reference in New Issue
Block a user