实现行为树执行器、任务协调和技能接口

core/tests/core_test.cpp

@@ -0,0 +1,105 @@
+#include "robot_bt/core.hpp"
+#include <cassert>
+#include <cstdlib>
+#include <filesystem>
+#include <iostream>
+#include <limits>
+#include <stdexcept>
+using namespace robot_bt;
+struct Driver final : GoalDriver {
+  bool available{true};
+  std::vector<GoalRequest> sent;
+  std::vector<std::string> cancellations;
+  std::vector<GoalEvent> events;
+  bool ready(Skill) const override { return available; }
+  void send(const GoalRequest& request) override { sent.push_back(request); }
+  void cancel(const std::string& id) override { cancellations.push_back(id); }
+  std::vector<GoalEvent> drain_events() override { auto result=events; events.clear(); return result; }
+};
+std::string journal(const std::string& name) {
+ static const std::string root=[](){char path[]="/tmp/robot_bt_registry_tests_XXXXXX";const char* made=::mkdtemp(path);assert(made);return std::string(made);}();
+ return root+"/"+name+".journal";
+}
+GoalRequest request(unsigned attempt=1) {
+ GoalRequest q; q.robot_id="sim_robot"; q.trace={"task","nav","run",1,1,1,attempt}; q.skill=Skill::NAVIGATE; return q;
+}
+GoalEvent event(const GoalRecord& r, EventKind kind) { GoalEvent e; e.goal_id=r.request.goal_id; e.trace=r.request.trace; e.kind=kind; return e; }
+void test_journal_excludes_second_executor() {
+ Driver d; auto path=journal("process_lock"); ActiveGoalRegistry first(d,path); bool refused=false;
+ try { ActiveGoalRegistry second(d,path); } catch(const std::runtime_error&) { refused=true; }
+ assert(refused);
+}
+void test_one_send_and_restart_lock() {
+ Driver d; auto path=journal("one"); auto now=SteadyTime{}; std::string id;
+ {
+ ActiveGoalRegistry r(d,path);
+ auto started=r.start(request(),now); assert(started && d.sent.size()==1); id=*started;
+ for(int i=0;i<50;++i) { r.pump(now); auto second=r.start(request(),now); assert(!second); }
+ assert(d.sent.size()==1 && r.robot_locked("sim_robot"));
+ }
+ Driver other; ActiveGoalRegistry recovered(other,path);
+ assert(recovered.robot_locked("sim_robot"));
+ assert(recovered.find(id)->state==GoalState::STOP_UNKNOWN);
+ assert(!recovered.start(request(2),now)); assert(other.sent.empty());
+}
+void test_cancellation_and_stale_messages() {
+ Driver d; Budgets b; b.acceptance=Milliseconds(10); b.cancel_stop=Milliseconds(20);
+ ActiveGoalRegistry r(d,journal("cancel"),b); auto now=SteadyTime{}; auto id=*r.start(request(),now);
+ r.pump(now+Milliseconds(11)); assert(r.find(id)->cancel_intent && d.cancellations.size()==1);
+ auto ack=event(*r.find(id),EventKind::CANCEL_ACK); d.events.push_back(ack); r.pump(now+Milliseconds(12)); assert(r.robot_locked("sim_robot"));
+ auto accepted=event(*r.find(id),EventKind::ACCEPTED); d.events.push_back(accepted); r.pump(now+Milliseconds(13)); assert(d.cancellations.size()==2);
+ auto stale=event(*r.find(id),EventKind::RESULT); stale.trace.execution_generation=2; stale.native_status=NativeStatus::SUCCEEDED; stale.result={ResultCode::COMPLETED,StopState::CONFIRMED,{},""};
+ d.events.push_back(stale); r.pump(now+Milliseconds(14)); assert(r.robot_locked("sim_robot"));
+ r.pump(now+Milliseconds(32)); assert(r.find(id)->state==GoalState::STOP_UNKNOWN);
+ auto result=event(*r.find(id),EventKind::RESULT); result.native_status=NativeStatus::CANCELED; result.result={ResultCode::CANCELED,StopState::CONFIRMED,{},"stopped"};
+ d.events.push_back(result); r.pump(now+Milliseconds(33)); assert(!r.robot_locked("sim_robot"));
+ assert(!r.start(request(),now)); assert(r.start(request(2),now));
+}
+void test_feedback_and_protocol_failure() {
+ Driver d; Budgets b; b.feedback=Milliseconds(10); ActiveGoalRegistry r(d,journal("feedback"),b); auto now=SteadyTime{}; auto id=*r.start(request(),now);
+ d.events.push_back(event(*r.find(id),EventKind::ACCEPTED)); r.pump(now);
+ auto e=event(*r.find(id),EventKind::FEEDBACK); e.sequence=2; d.events.push_back(e); r.pump(now+Milliseconds(5));
+ e.sequence=1; d.events.push_back(e); r.pump(now+Milliseconds(12)); assert(!r.find(id)->cancel_intent);
+ d.events.push_back(e); r.pump(now+Milliseconds(16)); assert(r.find(id)->cancel_intent);
+ auto result=event(*r.find(id),EventKind::RESULT); result.native_status=NativeStatus::SUCCEEDED; result.result={ResultCode::FAILED,StopState::CONFIRMED,{},"contradiction"};
+ d.events.push_back(result); r.pump(now+Milliseconds(17)); assert(r.find(id)->state==GoalState::STOP_UNKNOWN);
+ assert(!r.reconcile(id,request().trace,StopState::CONFIRMED,false,"evidence"));
+ assert(!r.reconcile(id,request().trace,StopState::CONFIRMED,true,""));
+ assert(r.reconcile(id,request().trace,StopState::CONFIRMED,true,"operator-17"));
+}
+void test_rejection_releases_and_unknown_stops_lock() {
+ Driver d; ActiveGoalRegistry r(d,journal("reject")); auto now=SteadyTime{}; auto id=*r.start(request(),now);
+ d.events.push_back(event(*r.find(id),EventKind::REJECTED)); r.pump(now); assert(!r.robot_locked("sim_robot"));
+ id=*r.start(request(2),now); auto e=event(*r.find(id),EventKind::RESULT); e.native_status=NativeStatus::SUCCEEDED; e.result={ResultCode::COMPLETED,StopState::UNKNOWN,{},""};
+ d.events.push_back(e); r.pump(now); assert(r.robot_locked("sim_robot"));
+}
+void test_completion_after_deadline_cannot_be_success() {
+ Driver d;Budgets b;b.acceptance=Milliseconds(10);ActiveGoalRegistry r(d,journal("late_success"),b);auto now=SteadyTime{};auto id=*r.start(request(),now);
+ auto result=event(*r.find(id),EventKind::RESULT);result.native_status=NativeStatus::SUCCEEDED;result.result={ResultCode::COMPLETED,StopState::CONFIRMED,{},"late"};d.events.push_back(result);
+ r.pump(now+Milliseconds(11));assert(r.find(id)->cancel_intent);assert(!r.robot_locked("sim_robot"));
+}
+void test_contradictory_rejection_cannot_release_unknown_motion() {
+ Driver d;ActiveGoalRegistry r(d,journal("contradiction"));auto now=SteadyTime{};auto id=*r.start(request(),now);
+ auto result=event(*r.find(id),EventKind::RESULT);result.native_status=NativeStatus::SUCCEEDED;result.result={ResultCode::COMPLETED,StopState::UNKNOWN,{},"uncertain execution"};d.events.push_back(result);r.pump(now);
+ d.events.push_back(event(*r.find(id),EventKind::REJECTED));r.pump(now);assert(r.robot_locked("sim_robot"));
+}
+void test_accepted_goal_remote_rejection_releases_stopped_resource() {
+ Driver d;ActiveGoalRegistry r(d,journal("remote_rejected"));auto now=SteadyTime{};auto id=*r.start(request(),now);d.events.push_back(event(*r.find(id),EventKind::ACCEPTED));r.pump(now);
+ auto result=event(*r.find(id),EventKind::RESULT);result.native_status=NativeStatus::ABORTED;result.result={ResultCode::REJECTED,StopState::CONFIRMED,{},"remote controller refused"};d.events.push_back(result);r.pump(now);assert(!r.robot_locked("sim_robot"));assert(r.find(id)->result->code==ResultCode::REJECTED);
+}
+void test_geometry_and_snapshot_binding() {
+ Pose p{"map",1,2,0,0,0,0,1}; assert(valid_pose(p)); auto q=p; q.x+=0.06; assert(!within_tolerance(q,p,0.05,0.1));
+ q=p; q.x=std::numeric_limits<double>::quiet_NaN(); assert(!valid_pose(q)); assert(!within_tolerance(q,p,0.05,0.1));
+ TargetBinding t; t.meta={1,request().trace,"goal","perception",3,100,300}; t.target_id="item"; t.pose=p;
+ assert(valid_target(t,request().trace,"item",3,100,200));
+ assert(!valid_target(t,request().trace,"item",4,100,200));
+ assert(!valid_target(t,request().trace,"item",3,101,200));
+ assert(!valid_target(t,request().trace,"item",3,100,301));
+ PlacementBinding place{t.meta,"item","bin",p,true};
+ assert(!valid_placement(place,request().trace,"item","wrong",3,100,200));
+ assert(!SkillResponse{}.valid);
+}
+int main() {
+ test_accepted_goal_remote_rejection_releases_stopped_resource(); test_contradictory_rejection_cannot_release_unknown_motion(); test_completion_after_deadline_cannot_be_success(); test_journal_excludes_second_executor(); test_one_send_and_restart_lock(); test_cancellation_and_stale_messages(); test_feedback_and_protocol_failure(); test_rejection_releases_and_unknown_stops_lock(); test_geometry_and_snapshot_binding();
+ std::cout << "core registry and geometry tests passed\n";
+}

core/tests/journal_failure_test.cpp

@@ -0,0 +1,30 @@
+#include "robot_bt/core.hpp"
+#include <cassert>
+#include <cstdlib>
+#include <filesystem>
+#include <iostream>
+using namespace robot_bt;
+struct FaultDriver final : GoalDriver {
+  unsigned sent{0}, canceled{0};
+  bool ready(Skill) const override { return true; }
+  void send(const GoalRequest&) override { ++sent; }
+  void cancel(const std::string&) override { ++canceled; }
+  std::vector<GoalEvent> drain_events() override { return {}; }
+};
+int main() {
+  char name[]="/tmp/bt_journal_failure_XXXXXX";
+  const char* directory=::mkdtemp(name); assert(directory);
+  std::string path=std::string(directory)+"/goals";
+  FaultDriver driver;
+  ActiveGoalRegistry registry(driver,path);
+  GoalRequest q;q.robot_id="robot";q.trace={"task","pick","run",1,1,1,1};
+  const auto id=registry.start(q,SteadyTime{});assert(id&&driver.sent==1);
+  // Simulate an unavailable journal after the motion request has been sent.
+  std::filesystem::remove(path);std::filesystem::create_directory(path);
+  try { registry.request_cancel(*id,SteadyTime{}); } catch(const std::exception&) {}
+  assert(driver.canceled==1 && "journal failure must never suppress a stop request");
+  assert(registry.robot_locked("robot"));
+  q.trace.attempt=2;assert(!registry.start(q,SteadyTime{}));
+  std::filesystem::remove_all(directory);
+  std::cout<<"journal fault preserves cancel and quarantine\n";
+}

core/tests/lifecycle_test.cpp

@@ -0,0 +1,62 @@
+#include "robot_bt/core.hpp"
+#include <cassert>
+#include <cstdlib>
+#include <iostream>
+#include <set>
+using namespace robot_bt;
+struct Wire : GoalDriver {
+ unsigned sends{0},cancels{0};std::vector<GoalEvent> events;
+ bool ready(Skill)const override{return true;}
+ void send(const GoalRequest&)override{++sends;}
+ void cancel(const std::string&)override{++cancels;}
+ std::vector<GoalEvent> drain_events()override{auto out=events;events.clear();return out;}
+};
+int main(){
+ char directory[]="/tmp/bt_lifecycle_XXXXXX";assert(::mkdtemp(directory));
+ unsigned cases=0;std::set<std::pair<int,int>> transitions;
+ // All sequences of length three over the ten documented event classes.
+ // This is a finite bound, not an assertion about arbitrary-length schedules.
+ for(unsigned word=0;word<1000;++word){
+  Wire wire;Budgets budgets{Milliseconds(2),Milliseconds(2),Milliseconds(2),Milliseconds(3),Milliseconds(2)};
+  ActiveGoalRegistry registry(wire,std::string(directory)+"/"+std::to_string(cases),budgets);
+  GoalRequest q;q.robot_id="r";q.trace={"t","s","run",1,1,1,1};
+  const auto id=*registry.start(q,SteadyTime{});unsigned rest=word;bool terminal=false;SteadyTime now{};
+  for(unsigned step=0;step<3;++step){
+   const auto before=registry.find(id)->state;const unsigned kind=rest%10;rest/=10;
+   GoalEvent e;e.goal_id=id;e.trace=q.trace;
+   switch(kind){
+    case 0:e.kind=EventKind::ACCEPTED;break;
+    case 1:e.kind=EventKind::REJECTED;break;
+    case 2:e.kind=EventKind::FEEDBACK;e.sequence=step+1;break;
+    case 3:e.kind=EventKind::FEEDBACK;e.sequence=0;break;
+    case 4:e.kind=EventKind::CANCEL_ACK;break;
+    case 5:e.kind=EventKind::RESULT;e.native_status=NativeStatus::SUCCEEDED;e.result.code=ResultCode::COMPLETED;e.result.stop=StopState::CONFIRMED;break;
+    case 6:e.kind=EventKind::RESULT;e.native_status=NativeStatus::CANCELED;e.result.code=ResultCode::CANCELED;e.result.stop=StopState::UNKNOWN;break;
+    case 7:e.kind=EventKind::RESULT;e.trace.run_id="stale";e.native_status=NativeStatus::SUCCEEDED;e.result.code=ResultCode::COMPLETED;e.result.stop=StopState::CONFIRMED;break;
+    case 8:registry.request_cancel(id,now);break;
+    case 9:now+=Milliseconds(5);break;
+   }
+   if(kind<8)wire.events.push_back(e);
+   registry.pump(now);const auto* record=registry.find(id);
+   transitions.emplace(static_cast<int>(before),static_cast<int>(record->state));
+   if(terminal)assert(record->state==GoalState::TERMINAL);
+   terminal=record->state==GoalState::TERMINAL;
+   if(terminal){assert(record->result);assert(record->result->stop==StopState::CONFIRMED);}
+   if(!terminal)assert(registry.robot_locked("r"));
+   if(kind==4&&!terminal)assert(registry.robot_locked("r"));
+   if(kind==7)assert((before==GoalState::TERMINAL)==terminal);
+   assert(!registry.start(q,now));assert(wire.sends==1);
+  }
+  ++cases;
+ }
+ // Full native-status x business-result x stop matrix, including mismatches.
+ unsigned combinations=0;
+ for(int n=0;n<5;++n)for(int b=0;b<5;++b)for(int stop=0;stop<2;++stop){
+  Wire wire;ActiveGoalRegistry registry(wire,std::string(directory)+"/"+std::to_string(cases++));
+  GoalRequest q;q.robot_id="r";q.trace={"t","s","run",1,1,1,1};const auto id=*registry.start(q,SteadyTime{});
+  GoalEvent e;e.goal_id=id;e.trace=q.trace;e.kind=EventKind::RESULT;e.native_status=static_cast<NativeStatus>(n);e.result.code=static_cast<ResultCode>(b);e.result.stop=static_cast<StopState>(stop);wire.events.push_back(e);registry.pump(SteadyTime{});
+  const bool matches=(n==0&&b==0)||(n==1&&(b==1||b==3||b==4))||(n==2&&b==2)||(n==3&&b==4);
+  assert(registry.robot_locked("r")==!(matches&&stop==1));++combinations;
+ }
+ std::cout<<"{\"event_sequences\":1000,\"sequence_length\":3,\"event_alphabet\":10,\"result_combinations\":"<<combinations<<",\"observed_state_edges\":"<<transitions.size()<<"}\n";
+}

core/tests/preflight_test.cpp

@@ -0,0 +1,14 @@
+#include "workflow_fixture.hpp"
+#include <cmath>
+int main() {
+  const Pose flat{"map",0,0,0,0,0,0,1};
+  const Pose tilted{"map",0,0,0.3,0.1,0,0,std::sqrt(.99)};
+  assert(within_tolerance(tilted,flat,.01,.01) && "navigation tolerance is XY/yaw per DR");
+  Fixture f("preflight_independent");
+  f.driver.unknown_verification=true;
+  auto runner=f.runner();
+  const auto result=f.execute(runner);
+  assert(result==TickStatus::INTERVENTION_REQUIRED);
+  assert(f.count(Skill::NAVIGATE)==0 && "default sensor EMPTY must not authorize first motion");
+  std::cout<<"independent preflight gates first motion\n";
+}

core/tests/proof_regression_test.cpp

@@ -0,0 +1,8 @@
+#include "workflow_fixture.hpp"
+#include <stdexcept>
+struct ThrowWire: GoalDriver {unsigned sends=0,cancels=0;bool ready(Skill)const override{return true;}void send(const GoalRequest&)override{++sends;throw std::runtime_error("after-send exception");}void cancel(const std::string&)override{++cancels;}std::vector<GoalEvent> drain_events()override{return {};}};
+int main(){
+ ThrowWire wire;ActiveGoalRegistry registry(wire,Fixture::test_root()+"/throw.journal");GoalRequest q;q.robot_id="r";q.trace={"t","s","run",1,1,1,1};auto id=registry.start(q,SteadyTime{});registry.request_cancel(*id,SteadyTime{});registry.pump(SteadyTime{}+Milliseconds(9000));assert(wire.cancels>=1);std::cout<<"send-throw sends="<<wire.sends<<" cancels="<<wire.cancels<<" locked="<<registry.robot_locked("r")<<"\n";
+ Fixture f("clock");auto runner=f.runner();assert(f.execute(runner)==TickStatus::SUCCESS);auto before=f.driver.now;runner.halt(SteadyTime{});runner.update_safety({true,true,Holding::EMPTY,1,1000000000});auto status=runner.settle(SteadyTime{},1);assert(status==TickStatus::INTERVENTION_REQUIRED);assert(!runner.empty_verified(1));std::cout<<"clock rollback from="<<before<<" to=1 settle_success="<<(status==TickStatus::SUCCESS)<<" empty_verified="<<runner.empty_verified(1)<<"\n";
+ Fixture e("expiry");auto er=e.runner();er.update_safety({true,true,Holding::EMPTY,1000000,9000000000});assert(er.tick(Stage::PREFLIGHT,SteadyTime{},1000000)==TickStatus::RUNNING);assert(er.tick(Stage::PREFLIGHT,SteadyTime{}+Milliseconds(1),2000000)==TickStatus::SUCCESS);e.driver.now=2000000000;er.update_safety({true,true,Holding::EMPTY,e.driver.now,e.driver.now+1000000000});bool proof=er.empty_verified(e.driver.now);auto nav=er.tick(Stage::NAVIGATE_OBSERVE,SteadyTime{}+Milliseconds(2),e.driver.now);assert(e.count(Skill::NAVIGATE)==0);assert(nav==TickStatus::RUNNING);std::cout<<"expired preflight empty_verified="<<proof<<" nav_count="<<e.count(Skill::NAVIGATE)<<" status_running="<<(nav==TickStatus::RUNNING)<<"\n";
+}

core/tests/readiness_regression_test.cpp

@@ -0,0 +1,2 @@
+#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";}

core/tests/scenario_test.cpp

@@ -0,0 +1,102 @@
+#include "workflow_fixture.hpp"
+#include <set>
+
+struct ScriptedDriver : Simulator {
+  std::optional<Skill> fault_skill;
+  std::string fault;
+  unsigned injections{0}, cancels{0};
+  std::vector<GoalEvent> pending;
+  void send(const GoalRequest& q) override {
+    Simulator::send(q);
+    if(!fault_skill || q.skill!=*fault_skill || injections++)return;
+    auto& e=events.back();
+    if(fault=="failed") {e.native_status=NativeStatus::ABORTED;e.result.code=ResultCode::FAILED;}
+    else if(fault=="canceled") {e.native_status=NativeStatus::CANCELED;e.result.code=ResultCode::CANCELED;}
+    else if(fault=="timed_out") {e.native_status=NativeStatus::ABORTED;e.result.code=ResultCode::TIMED_OUT;}
+    else if(fault=="stop_unknown")e.result.stop=StopState::UNKNOWN;
+    else if(fault=="invalid")e.result.response.valid=false;
+    else if(fault=="mismatch")e.native_status=NativeStatus::ABORTED;
+    else if(fault=="stale_trace")e.trace.execution_generation++;
+    else if(fault=="wrong_goal")e.goal_id="old-goal";
+    else if(fault=="reject") {events.clear(); // rebuild without accessing the invalidated reference
+      GoalEvent rejected;rejected.kind=EventKind::REJECTED;rejected.goal_id=q.goal_id;rejected.trace=q.trace;events.push_back(rejected);}
+    else if(fault=="silence")events.clear();
+    else if(fault=="stale_evidence") {
+      e.result.response.evidence->valid_until=now;
+      if(e.result.response.target)e.result.response.target->meta.valid_until=now;
+      if(e.result.response.placement)e.result.response.placement->meta.valid_until=now;
+    }
+  }
+  void cancel(const std::string& id) override {
+    ++cancels;
+    for(const auto& q:sent)if(q.goal_id==id){
+      GoalEvent ack;ack.goal_id=id;ack.trace=q.trace;ack.kind=EventKind::CANCEL_ACK;pending.push_back(ack);
+      // A scripted missing result is deliberately not replaced by fabricated stop.
+    }
+  }
+  std::vector<GoalEvent> drain_events() override {
+    auto out=Simulator::drain_events();out.insert(out.end(),pending.begin(),pending.end());pending.clear();return out;
+  }
+};
+
+struct Campaign {
+  unsigned runs{0}, injected_runs{0}, not_applicable_runs{0};
+  std::set<std::string> visited;
+  TickStatus run(const std::string& route,const std::string& fault,std::optional<Skill> skill,
+                 std::optional<Stage> interrupt={},bool throw_delivery=false) {
+    Fixture config("campaign_config_"+std::to_string(runs));
+    config.task.route=route;
+    config.site.object_locations["item"]="source";config.site.object_postures["item"]="small-lift";
+    config.site.cell_locations["shelf/front/1/2"]="source";config.site.cell_postures["shelf/front/1/2"]="small-lift";
+    ScriptedDriver driver;driver.fault_skill=skill;driver.fault=fault;
+    Budgets budgets{Milliseconds(3),Milliseconds(3),Milliseconds(4),Milliseconds(5),Milliseconds(3)};
+    ActiveGoalRegistry registry(driver,Fixture::test_root()+"/campaign_"+std::to_string(runs)+".journal",budgets);
+    ContextStore context;unsigned deliveries=0, attempted=0;
+    StageRunner runner(config.task,config.site,driver,registry,context,[&](const std::string&,unsigned,const std::string&){
+      ++attempted;if(throw_delivery)throw std::runtime_error("injected delivery commit failure");
+      if(fault=="delivery_reject")return false;
+      ++deliveries;return true;
+    },budgets);
+    Workflow flow(runner);TickStatus status=TickStatus::RUNNING;bool interrupted=false;
+    unsigned dispatched_at_interrupt=0;
+    for(unsigned i=0;i<300&&status==TickStatus::RUNNING;++i) {
+      driver.now=1000000+static_cast<RosTime>(i)*1000000;
+      bool safe=true;
+      if(interrupt&&flow.current_stage()==*interrupt&&!interrupted){
+        interrupted=true;dispatched_at_interrupt=driver.sent.size();
+        if(fault=="halt")runner.halt(SteadyTime{}+Milliseconds(i));
+        else if(fault=="unsafe")safe=false;
+      }
+      runner.update_safety({safe,true,driver.sensor_holding,driver.now,driver.now+1000000000});
+      visited.insert(route+":"+stage_name(flow.current_stage()));
+      status=flow.tick(SteadyTime{}+Milliseconds(i),driver.now);
+    }
+    assert(status!=TickStatus::RUNNING);
+    if(interrupted)assert(driver.sent.size()==dispatched_at_interrupt);
+    if(skill&&driver.injections) {assert(status!=TickStatus::SUCCESS);assert(deliveries==0);}
+    if(throw_delivery||fault=="delivery_reject"){assert(status==TickStatus::INTERVENTION_REQUIRED);assert(attempted==1);assert(deliveries==0);}
+    if(fault.empty()&&!throw_delivery){assert(status==TickStatus::SUCCESS);assert(deliveries==1);}
+    if(skill){if(driver.injections)++injected_runs;else ++not_applicable_runs;}
+    ++runs;return status;
+  }
+};
+
+int main() {
+  Campaign c;
+  const std::vector<std::string> routes={"LEGACY","OBJECT_TABLE","SHELF_CELL"};
+  for(const auto& route:routes){
+    c.run(route,"",{});
+    for(const auto stage:fixed_stages()){
+      assert(c.run(route,"halt",{},stage)==TickStatus::INTERVENTION_REQUIRED);
+      assert(c.run(route,"unsafe",{},stage)==TickStatus::INTERVENTION_REQUIRED);
+    }
+    for(int s=0;s<=static_cast<int>(Skill::VERIFY_EMPTY);++s){
+      const auto skill=static_cast<Skill>(s);
+      for(const auto& fault:{"failed","canceled","timed_out","stop_unknown","invalid","mismatch","stale_trace","wrong_goal","reject","silence"})c.run(route,fault,skill);
+    }
+    c.run(route,"delivery_reject",{});c.run(route,"",{}, {},true);
+  }
+  for(const auto s:{Skill::VERIFY_PICK,Skill::VERIFY_TRANSPORT,Skill::VERIFY_PLACE,Skill::LOCALIZE_TARGET,Skill::CHECK_FREE_SPACE})c.run("LEGACY","stale_evidence",s);
+  assert(c.visited.size()==routes.size()*fixed_stages().size());
+  std::cout<<"{\"scenario_runs\":"<<c.runs<<",\"injected_runs\":"<<c.injected_runs<<",\"not_applicable_runs\":"<<c.not_applicable_runs<<",\"route_stage_pairs\":"<<c.visited.size()<<",\"routes\":3,\"fixed_stages\":15,\"scope\":\"bounded deterministic core campaign\"}\n";
+}

core/tests/settlement_test.cpp

@@ -0,0 +1,23 @@
+#include "workflow_fixture.hpp"
+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;}
+ }
+};
+int main(){
+ Fixture config("settlement_config");FailedPlaceDriver driver;ContextStore context;
+ ActiveGoalRegistry registry(driver,Fixture::test_root()+"/settlement.journal");unsigned deliveries=0;
+ StageRunner runner(config.task,config.site,driver,registry,context,[&](const std::string&,unsigned,const std::string&){++deliveries;return true;});
+ Workflow flow(runner);TickStatus status=TickStatus::RUNNING;unsigned i=0;
+ 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));
+ // 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);
+ 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);
+ std::cout<<"failed place settlement records verified delivery without motion retry\n";
+}

core/tests/workflow_fixture.hpp

@@ -0,0 +1,51 @@
+#pragma once
+#include "robot_bt/core.hpp"
+#include <cassert>
+#include <cstdlib>
+#include <filesystem>
+#include <iostream>
+#include <limits>
+using namespace robot_bt;
+struct Simulator : GoalDriver {
+ std::vector<GoalRequest> sent; std::vector<GoalEvent> events; RosTime now{1000000};
+ std::optional<Skill> unavailable_skill;
+ Holding sensor_holding{Holding::EMPTY};
+ Admission admission{Admission::DIRECT}; bool bad_container{false}, unknown_verification{false}, nan_geometry{false}, no_stop{false};
+ bool ready(Skill skill)const override{return !unavailable_skill||*unavailable_skill!=skill;}
+ void cancel(const std::string&)override{}
+ void send(const GoalRequest& q)override {
+  if(q.skill==Skill::PICK)sensor_holding=Holding::HOLDING_TARGET;
+  if(q.skill==Skill::PLACE)sensor_holding=Holding::EMPTY;
+  sent.push_back(q); GoalEvent accept; accept.goal_id=q.goal_id; accept.trace=q.trace; accept.kind=EventKind::ACCEPTED; events.push_back(accept);
+  GoalEvent e=accept; e.kind=EventKind::RESULT; e.native_status=NativeStatus::SUCCEEDED; e.result.code=ResultCode::COMPLETED; e.result.stop=no_stop?StopState::UNKNOWN:StopState::CONFIRMED;
+  auto& r=e.result.response; r.valid=true; r.target_id=q.target_id; r.destination_id=bad_container?"wrong-bin":q.destination_id; r.base_stopped=true; r.verified=!unknown_verification; r.in_destination=true;
+  SnapshotMeta meta{1,q.trace,q.goal_id,"independent-simulator",q.geometry_epoch,now,now+1000000000}; r.evidence=meta;
+  Pose p{"map",0,0,0,0,0,0,1}; if(nan_geometry)p.x=std::numeric_limits<double>::quiet_NaN();
+  switch(q.skill) {
+  case Skill::NAVIGATE:r.final_pose=q.registered_pose;break;
+  case Skill::LOCATE_SHELF_COLUMN:r.shelf=q.shelf;r.side="front";r.column="1";r.tier="2";break;
+  case Skill::LOCALIZE_TARGET:r.target=TargetBinding{meta,q.target_id,p};break;
+  case Skill::EVALUATE_GRASP:r.admission=admission;r.posture_id="small-lift";break;
+  case Skill::ADJUST_POSTURE:admission=Admission::DIRECT;break;
+  case Skill::VERIFY_PICK:case Skill::VERIFY_TRANSPORT:r.holding=unknown_verification?Holding::UNKNOWN:Holding::HOLDING_TARGET;break;
+  case Skill::CHECK_FREE_SPACE:r.placement=PlacementBinding{meta,q.target_id,r.destination_id,p,true};break;
+  case Skill::VERIFY_EMPTY:case Skill::VERIFY_PLACE:r.holding=unknown_verification?Holding::UNKNOWN:Holding::EMPTY;break;
+  default:break;
+  }
+  events.push_back(e);
+ }
+ std::vector<GoalEvent> drain_events()override{auto r=events;events.clear();return r;}
+};
+struct Fixture {
+ Simulator driver; ContextStore context; std::string journal; ActiveGoalRegistry registry; TaskConfig task; SiteConfig site; unsigned deliveries{0};
+ Fixture(const std::string& name):journal(test_root()+"/"+name+".journal"),registry(driver,journal) {
+  task.trace={"task","root","run",1,1,1,1}; task.robot_id="sim";task.target_id="item";task.source_shelf="shelf";task.destination_id="bin";task.observe_location="observe";task.destination_location="bin-nav";
+  site.locations={{"observe",Pose{"map",0,0,0,0,0,0,1}},{"source",Pose{"map",1,0,0,0,0,0,1}},{"bin-nav",Pose{"map",2,0,0,0,0,0,1}}};site.parking_locations={{"shelf/front/1","source"}};site.allowed_postures={"small-lift","carry"};site.transport_posture="carry";
+ }
+ static const std::string& test_root(){static const std::string root=[](){char path[]="/tmp/robot_bt_workflow_tests_XXXXXX";const char* made=::mkdtemp(path);assert(made);return std::string(made);}();return root;}
+ StageRunner runner(){return StageRunner(task,site,driver,registry,context,[this](const std::string& id,unsigned index,const std::string& evidence){assert(id=="task"&&index==0&&!evidence.empty());++deliveries;return true;});}
+ TickStatus execute(StageRunner& r,unsigned ticks=300) {
+  Workflow flow(r); auto status=TickStatus::RUNNING; for(unsigned i=0;i<ticks&&status==TickStatus::RUNNING;++i) { driver.now=1000000+static_cast<RosTime>(i)*1000000; r.update_safety({true,true,driver.sensor_holding,driver.now,driver.now+1000000000}); status=flow.tick(SteadyTime{}+Milliseconds(i),driver.now); } return status;
+ }
+ unsigned count(Skill skill)const{unsigned n=0;for(const auto&q:driver.sent)if(q.skill==skill)++n;return n;}
+};

core/tests/workflow_test.cpp

@@ -0,0 +1,51 @@
+#include "workflow_fixture.hpp"
+void happy_flow_is_verified_once() {
+ Fixture f("happy");auto r=f.runner();assert(f.execute(r)==TickStatus::SUCCESS);assert(f.deliveries==1);assert(f.count(Skill::NAVIGATE)==3);assert(f.count(Skill::PICK)==1);assert(f.count(Skill::PLACE)==1);assert(f.count(Skill::VERIFY_PICK)==1);assert(f.count(Skill::VERIFY_TRANSPORT)==1);assert(f.count(Skill::VERIFY_PLACE)==1);
+ assert(r.tick(Stage::DELIVER,SteadyTime{},f.driver.now)==TickStatus::SUCCESS);assert(f.deliveries==1);
+}
+void uncertain_evidence_blocks_delivery() {
+ { Fixture f("container");f.driver.bad_container=true;auto r=f.runner();assert(f.execute(r)==TickStatus::INTERVENTION_REQUIRED);assert(f.deliveries==0);assert(f.count(Skill::PLACE)==0); }
+ { Fixture f("unknown");f.driver.unknown_verification=true;auto r=f.runner();assert(f.execute(r)==TickStatus::INTERVENTION_REQUIRED);assert(f.deliveries==0);assert(f.count(Skill::TRANSPORT_POSTURE)==0); }
+ { Fixture f("nan");f.driver.nan_geometry=true;auto r=f.runner();assert(f.execute(r)==TickStatus::INTERVENTION_REQUIRED);assert(f.count(Skill::PICK)==0); }
+ { Fixture f("stop");f.driver.no_stop=true;auto r=f.runner();assert(f.execute(r)==TickStatus::INTERVENTION_REQUIRED);assert(f.registry.robot_locked("sim"));assert(f.count(Skill::NAVIGATE)==0); }
+}
+void admission_retry_is_bounded_and_posture_reobserves() {
+ {Fixture f("unknown_grasp");f.driver.admission=Admission::UNKNOWN;auto r=f.runner();assert(f.execute(r)==TickStatus::INTERVENTION_REQUIRED);assert(f.count(Skill::LOCALIZE_TARGET)==3);assert(f.count(Skill::PICK)==0);}
+ {Fixture f("adjust");f.driver.admission=Admission::ADJUST_POSTURE;auto r=f.runner();assert(f.execute(r)==TickStatus::SUCCESS);assert(f.count(Skill::ADJUST_POSTURE)==1);assert(f.count(Skill::LOCALIZE_TARGET)==2);assert(r.geometry_epoch()>=4);}
+ {Fixture f("invalid_admission");f.driver.admission=static_cast<Admission>(99);auto r=f.runner();assert(f.execute(r)==TickStatus::INTERVENTION_REQUIRED);assert(f.count(Skill::PICK)==0);}
+ {Fixture f("unreachable");f.driver.admission=Admission::NOT_REACHABLE;auto r=f.runner();assert(f.execute(r)==TickStatus::INTERVENTION_REQUIRED);assert(f.count(Skill::PICK)==0);}
+}
+void stage_order_and_stale_safety_block_motion() {
+ {Fixture f("initial_epoch");f.task.initial_geometry_epoch=10;auto r=f.runner();assert(r.geometry_epoch()==10);assert(f.execute(r)==TickStatus::SUCCESS);assert(r.geometry_epoch()>=14);}
+ {Fixture f("order");auto r=f.runner();r.update_safety({true,true,Holding::EMPTY,100,1000});assert(r.tick(Stage::PLACE,SteadyTime{},200)==TickStatus::INTERVENTION_REQUIRED);assert(f.driver.sent.empty());}
+ {Fixture f("safety");auto r=f.runner();r.update_safety({true,true,Holding::EMPTY,100,200});assert(r.tick(Stage::PREFLIGHT,SteadyTime{},300)==TickStatus::INTERVENTION_REQUIRED);assert(f.driver.sent.empty());}
+}
+void holding_uncertainty_blocks_dispatch_and_cancellation_release() {
+ {Fixture f("refresh_before_place");auto r=f.runner();Workflow flow(r);bool jumped=false;RosTime offset=0;auto status=TickStatus::RUNNING;
+  for(unsigned i=0;i<150&&status==TickStatus::RUNNING;++i){if(flow.current_stage()==Stage::CHECK_FREE_SPACE&&!jumped){offset=2000000000;jumped=true;}f.driver.now=1000000+static_cast<RosTime>(i)*1000000+offset;r.update_safety({true,true,f.driver.sensor_holding,f.driver.now,f.driver.now+1000000000});status=flow.tick(SteadyTime{}+Milliseconds(i),f.driver.now);}
+  assert(status==TickStatus::SUCCESS);assert(f.count(Skill::VERIFY_TRANSPORT)==2);assert(f.count(Skill::PLACE)==1);assert(f.deliveries==1);
+ }
+ {Fixture f("expired_hold");auto r=f.runner();Workflow flow(r);unsigned i=0;
+  for(;i<100&&flow.current_stage()!=Stage::NAVIGATE_DESTINATION;++i){f.driver.now=1000000+static_cast<RosTime>(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::NAVIGATE_DESTINATION);f.driver.unavailable_skill=Skill::NAVIGATE;f.driver.now+=2000000000;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::INTERVENTION_REQUIRED);assert(f.count(Skill::NAVIGATE)==2);
+ }
+ {Fixture f("pick_uncertain");auto r=f.runner();Workflow flow(r);
+  for(unsigned i=0;i<100&&f.count(Skill::PICK)==0;++i){f.driver.now=1000000+static_cast<RosTime>(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(f.count(Skill::PICK)==1);assert(r.holding()==Holding::UNKNOWN);r.halt(SteadyTime{}+Milliseconds(100));assert(r.holding()!=Holding::EMPTY);
+ }
+ {Fixture f("lost_hold");auto r=f.runner();Workflow flow(r);unsigned i=0;
+  for(;i<100&&flow.current_stage()!=Stage::NAVIGATE_DESTINATION;++i){f.driver.now=1000000+static_cast<RosTime>(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::NAVIGATE_DESTINATION);assert(f.count(Skill::NAVIGATE)==2);f.driver.now+=1000000;r.update_safety({true,true,Holding::UNKNOWN,f.driver.now,f.driver.now+1000000000});assert(flow.tick(SteadyTime{}+Milliseconds(i),f.driver.now)==TickStatus::INTERVENTION_REQUIRED);assert(f.count(Skill::NAVIGATE)==2);assert(r.holding()==Holding::UNKNOWN);
+ }
+}
+void new_routes_do_not_depend_on_3d_and_preserve_item_index() {
+ for(const auto& route: {std::string("OBJECT_TABLE"),std::string("SHELF_CELL")}) {
+  Fixture f("route_"+route);f.task.route=route;f.task.item_index=2;
+  f.site.object_locations["item"]="source";f.site.object_postures["item"]="small-lift";
+  f.site.cell_locations["shelf/front/1/2"]="source";f.site.cell_postures["shelf/front/1/2"]="small-lift";
+  StageRunner r(f.task,f.site,f.driver,f.registry,f.context,[&](const std::string&,unsigned index,const std::string&){assert(index==2);++f.deliveries;return true;});
+  assert(f.execute(r)==TickStatus::SUCCESS);assert(f.count(Skill::LOCALIZE_TARGET)==0);assert(f.count(Skill::EVALUATE_GRASP)==0);assert(f.count(Skill::CHECK_FREE_SPACE)==0);assert(f.count(Skill::ADJUST_POSTURE)==1);assert(f.count(Skill::LOCATE_SHELF_COLUMN)==(route=="SHELF_CELL"?1:0));assert(f.deliveries==1);
+ }
+ {Fixture f("missing_cell");f.task.route="SHELF_CELL";auto r=f.runner();assert(f.execute(r)==TickStatus::INTERVENTION_REQUIRED);assert(f.count(Skill::PICK)==0);}
+}
+int main(){new_routes_do_not_depend_on_3d_and_preserve_item_index();holding_uncertainty_blocks_dispatch_and_cancellation_release();happy_flow_is_verified_once();uncertain_evidence_blocks_delivery();admission_retry_is_bounded_and_posture_reobserves();stage_order_and_stale_safety_block_motion();std::cout<<"workflow tests passed\n";}