52 lines
6.7 KiB
C++
52 lines
6.7 KiB
C++
|
#include "workflow_fixture.hpp"
|
||
|
|
void happy_flow_is_verified_once() {
|
||
|
|
Fixture f("happy");auto r=f.runner();assert(f.execute(r)==TickStatus::SUCCESS);assert(f.deliveries==1);assert(f.count(Skill::NAVIGATE)==3);assert(f.count(Skill::PICK)==1);assert(f.count(Skill::PLACE)==1);assert(f.count(Skill::VERIFY_PICK)==1);assert(f.count(Skill::VERIFY_TRANSPORT)==1);assert(f.count(Skill::VERIFY_PLACE)==1);
|
||
|
|
assert(r.tick(Stage::DELIVER,SteadyTime{},f.driver.now)==TickStatus::SUCCESS);assert(f.deliveries==1);
|
||
|
|
}
|
||
|
|
void uncertain_evidence_blocks_delivery() {
|
||
|
|
{ Fixture f("container");f.driver.bad_container=true;auto r=f.runner();assert(f.execute(r)==TickStatus::INTERVENTION_REQUIRED);assert(f.deliveries==0);assert(f.count(Skill::PLACE)==0); }
|
||
|
|
{ Fixture f("unknown");f.driver.unknown_verification=true;auto r=f.runner();assert(f.execute(r)==TickStatus::INTERVENTION_REQUIRED);assert(f.deliveries==0);assert(f.count(Skill::TRANSPORT_POSTURE)==0); }
|
||
|
|
{ Fixture f("nan");f.driver.nan_geometry=true;auto r=f.runner();assert(f.execute(r)==TickStatus::INTERVENTION_REQUIRED);assert(f.count(Skill::PICK)==0); }
|
||
|
|
{ Fixture f("stop");f.driver.no_stop=true;auto r=f.runner();assert(f.execute(r)==TickStatus::INTERVENTION_REQUIRED);assert(f.registry.robot_locked("sim"));assert(f.count(Skill::NAVIGATE)==0); }
|
||
|
|
}
|
||
|
|
void admission_retry_is_bounded_and_posture_reobserves() {
|
||
|
|
{Fixture f("unknown_grasp");f.driver.admission=Admission::UNKNOWN;auto r=f.runner();assert(f.execute(r)==TickStatus::INTERVENTION_REQUIRED);assert(f.count(Skill::LOCALIZE_TARGET)==3);assert(f.count(Skill::PICK)==0);}
|
||
|
|
{Fixture f("adjust");f.driver.admission=Admission::ADJUST_POSTURE;auto r=f.runner();assert(f.execute(r)==TickStatus::SUCCESS);assert(f.count(Skill::ADJUST_POSTURE)==1);assert(f.count(Skill::LOCALIZE_TARGET)==2);assert(r.geometry_epoch()>=4);}
|
||
|
|
{Fixture f("invalid_admission");f.driver.admission=static_cast<Admission>(99);auto r=f.runner();assert(f.execute(r)==TickStatus::INTERVENTION_REQUIRED);assert(f.count(Skill::PICK)==0);}
|
||
|
|
{Fixture f("unreachable");f.driver.admission=Admission::NOT_REACHABLE;auto r=f.runner();assert(f.execute(r)==TickStatus::INTERVENTION_REQUIRED);assert(f.count(Skill::PICK)==0);}
|
||
|
|
}
|
||
|
|
void stage_order_and_stale_safety_block_motion() {
|
||
|
|
{Fixture f("initial_epoch");f.task.initial_geometry_epoch=10;auto r=f.runner();assert(r.geometry_epoch()==10);assert(f.execute(r)==TickStatus::SUCCESS);assert(r.geometry_epoch()>=14);}
|
||
|
|
{Fixture f("order");auto r=f.runner();r.update_safety({true,true,Holding::EMPTY,100,1000});assert(r.tick(Stage::PLACE,SteadyTime{},200)==TickStatus::INTERVENTION_REQUIRED);assert(f.driver.sent.empty());}
|
||
|
|
{Fixture f("safety");auto r=f.runner();r.update_safety({true,true,Holding::EMPTY,100,200});assert(r.tick(Stage::PREFLIGHT,SteadyTime{},300)==TickStatus::INTERVENTION_REQUIRED);assert(f.driver.sent.empty());}
|
||
|
|
}
|
||
|
|
void holding_uncertainty_blocks_dispatch_and_cancellation_release() {
|
||
|
|
{Fixture f("refresh_before_place");auto r=f.runner();Workflow flow(r);bool jumped=false;RosTime offset=0;auto status=TickStatus::RUNNING;
|
||
|
|
for(unsigned i=0;i<150&&status==TickStatus::RUNNING;++i){if(flow.current_stage()==Stage::CHECK_FREE_SPACE&&!jumped){offset=2000000000;jumped=true;}f.driver.now=1000000+static_cast<RosTime>(i)*1000000+offset;r.update_safety({true,true,f.driver.sensor_holding,f.driver.now,f.driver.now+1000000000});status=flow.tick(SteadyTime{}+Milliseconds(i),f.driver.now);}
|
||
|
|
assert(status==TickStatus::SUCCESS);assert(f.count(Skill::VERIFY_TRANSPORT)==2);assert(f.count(Skill::PLACE)==1);assert(f.deliveries==1);
|
||
|
|
}
|
||
|
|
{Fixture f("expired_hold");auto r=f.runner();Workflow flow(r);unsigned i=0;
|
||
|
|
for(;i<100&&flow.current_stage()!=Stage::NAVIGATE_DESTINATION;++i){f.driver.now=1000000+static_cast<RosTime>(i)*1000000;r.update_safety({true,true,f.driver.sensor_holding,f.driver.now,f.driver.now+1000000000});assert(flow.tick(SteadyTime{}+Milliseconds(i),f.driver.now)==TickStatus::RUNNING);}
|
||
|
|
assert(flow.current_stage()==Stage::NAVIGATE_DESTINATION);f.driver.unavailable_skill=Skill::NAVIGATE;f.driver.now+=2000000000;r.update_safety({true,true,Holding::HOLDING_TARGET,f.driver.now,f.driver.now+1000000000});assert(flow.tick(SteadyTime{}+Milliseconds(i),f.driver.now)==TickStatus::INTERVENTION_REQUIRED);assert(f.count(Skill::NAVIGATE)==2);
|
||
|
|
}
|
||
|
|
{Fixture f("pick_uncertain");auto r=f.runner();Workflow flow(r);
|
||
|
|
for(unsigned i=0;i<100&&f.count(Skill::PICK)==0;++i){f.driver.now=1000000+static_cast<RosTime>(i)*1000000;r.update_safety({true,true,f.driver.sensor_holding,f.driver.now,f.driver.now+1000000000});assert(flow.tick(SteadyTime{}+Milliseconds(i),f.driver.now)==TickStatus::RUNNING);}
|
||
|
|
assert(f.count(Skill::PICK)==1);assert(r.holding()==Holding::UNKNOWN);r.halt(SteadyTime{}+Milliseconds(100));assert(r.holding()!=Holding::EMPTY);
|
||
|
|
}
|
||
|
|
{Fixture f("lost_hold");auto r=f.runner();Workflow flow(r);unsigned i=0;
|
||
|
|
for(;i<100&&flow.current_stage()!=Stage::NAVIGATE_DESTINATION;++i){f.driver.now=1000000+static_cast<RosTime>(i)*1000000;r.update_safety({true,true,f.driver.sensor_holding,f.driver.now,f.driver.now+1000000000});assert(flow.tick(SteadyTime{}+Milliseconds(i),f.driver.now)==TickStatus::RUNNING);}
|
||
|
|
assert(flow.current_stage()==Stage::NAVIGATE_DESTINATION);assert(f.count(Skill::NAVIGATE)==2);f.driver.now+=1000000;r.update_safety({true,true,Holding::UNKNOWN,f.driver.now,f.driver.now+1000000000});assert(flow.tick(SteadyTime{}+Milliseconds(i),f.driver.now)==TickStatus::INTERVENTION_REQUIRED);assert(f.count(Skill::NAVIGATE)==2);assert(r.holding()==Holding::UNKNOWN);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
void new_routes_do_not_depend_on_3d_and_preserve_item_index() {
|
||
|
|
for(const auto& route: {std::string("OBJECT_TABLE"),std::string("SHELF_CELL")}) {
|
||
|
|
Fixture f("route_"+route);f.task.route=route;f.task.item_index=2;
|
||
|
|
f.site.object_locations["item"]="source";f.site.object_postures["item"]="small-lift";
|
||
|
|
f.site.cell_locations["shelf/front/1/2"]="source";f.site.cell_postures["shelf/front/1/2"]="small-lift";
|
||
|
|
StageRunner r(f.task,f.site,f.driver,f.registry,f.context,[&](const std::string&,unsigned index,const std::string&){assert(index==2);++f.deliveries;return true;});
|
||
|
|
assert(f.execute(r)==TickStatus::SUCCESS);assert(f.count(Skill::LOCALIZE_TARGET)==0);assert(f.count(Skill::EVALUATE_GRASP)==0);assert(f.count(Skill::CHECK_FREE_SPACE)==0);assert(f.count(Skill::ADJUST_POSTURE)==1);assert(f.count(Skill::LOCATE_SHELF_COLUMN)==(route=="SHELF_CELL"?1:0));assert(f.deliveries==1);
|
||
|
|
}
|
||
|
|
{Fixture f("missing_cell");f.task.route="SHELF_CELL";auto r=f.runner();assert(f.execute(r)==TickStatus::INTERVENTION_REQUIRED);assert(f.count(Skill::PICK)==0);}
|
||
|
|
}
|
||
|
|
int main(){new_routes_do_not_depend_on_3d_and_preserve_item_index();holding_uncertainty_blocks_dispatch_and_cancellation_release();happy_flow_is_verified_once();uncertain_evidence_blocks_delivery();admission_retry_is_bounded_and_posture_reobserves();stage_order_and_stale_safety_block_motion();std::cout<<"workflow tests passed\n";}
|