103 lines
5.7 KiB
C++
103 lines
5.7 KiB
C++
#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";
|
|
}
|