behavior-tree/core/tests/scenario_test.cpp

#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";
}