fix: align navigation contract and readiness handling

2026-09-22 14:35:14 +08:00
parent 91bcd92d6b
commit 964d1fde67
21 changed files with 922 additions and 139 deletions
@@ -0,0 +1,25 @@
+#include "workflow_fixture.hpp"
+int main() {
+ for(const auto* code:{"INPUTS_UNHEALTHY","ROBOT_STATE_UNAVAILABLE","ROBOT_ESTOP","BACKEND_NOT_CONFIGURED","NAV_NOT_READY"}) {
+  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";
+}