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
core/tests/readiness_regression_test.cpp
+22 -1
View File
@@ -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";}