2026-09-22 14:35:14 +08:00
|
|
|
#include "workflow_fixture.hpp"
|
|
|
|
|
int main() {
|
2026-09-22 17:40:25 +08:00
|
|
|
for(const auto* code:{"INPUTS_UNHEALTHY","ROBOT_STATE_UNAVAILABLE","ROBOT_ESTOP","EXECUTION_BACKEND_NOT_CONFIGURED","NAV_NOT_READY"}) {
|
2026-09-22 14:35:14 +08:00
|
|
|
Fixture f(std::string("navigation_")+code);auto runner=f.runner();Workflow flow(runner);
|
|
|
|
|
auto status=TickStatus::RUNNING;
|
|
|
|
|
for(unsigned i=0;i<100&&status==TickStatus::RUNNING;++i) {
|
|
|
|
|
f.driver.now=1000000+static_cast<RosTime>(i)*1000000;
|
|
|
|
|
runner.update_safety({true,true,Holding::EMPTY,f.driver.now,f.driver.now+1000000000});
|
|
|
|
|
status=flow.tick(SteadyTime{}+Milliseconds(i),f.driver.now);
|
|
|
|
|
for(auto& event:f.driver.events) {
|
|
|
|
|
if(event.kind==EventKind::RESULT&&!f.driver.sent.empty()&&f.driver.sent.back().skill==Skill::NAVIGATE) {
|
|
|
|
|
event.native_status=NativeStatus::ABORTED;event.result.code=ResultCode::REJECTED;
|
|
|
|
|
event.result.error_code=code;event.result.stop=StopState::CONFIRMED;
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
assert(status==TickStatus::INTERVENTION_REQUIRED);
|
|
|
|
|
assert(runner.error_code()==code);assert(f.count(Skill::NAVIGATE)==1);assert(f.count(Skill::PICK)==0);
|
|
|
|
|
for(unsigned i=100;i<120;++i) {
|
|
|
|
|
assert(flow.tick(SteadyTime{}+Milliseconds(i),f.driver.now)==TickStatus::INTERVENTION_REQUIRED);
|
|
|
|
|
}
|
|
|
|
|
assert(f.count(Skill::NAVIGATE)==1);
|
|
|
|
|
}
|
|
|
|
|
std::cout<<"navigation NOT_READY retains reason and requires explicit recovery without motion resend\n";
|
|
|
|
|
}
|
