63 lines
3.7 KiB
C++
63 lines
3.7 KiB
C++
|
#include "robot_bt/core.hpp"
|
||
|
|
#include <cassert>
|
||
|
|
#include <cstdlib>
|
||
|
|
#include <iostream>
|
||
|
|
#include <set>
|
||
|
|
using namespace robot_bt;
|
||
|
|
struct Wire : GoalDriver {
|
||
|
|
unsigned sends{0},cancels{0};std::vector<GoalEvent> events;
|
||
|
|
bool ready(Skill)const override{return true;}
|
||
|
|
void send(const GoalRequest&)override{++sends;}
|
||
|
|
void cancel(const std::string&)override{++cancels;}
|
||
|
|
std::vector<GoalEvent> drain_events()override{auto out=events;events.clear();return out;}
|
||
|
|
};
|
||
|
|
int main(){
|
||
|
|
char directory[]="/tmp/bt_lifecycle_XXXXXX";assert(::mkdtemp(directory));
|
||
|
|
unsigned cases=0;std::set<std::pair<int,int>> transitions;
|
||
|
|
// All sequences of length three over the ten documented event classes.
|
||
|
|
// This is a finite bound, not an assertion about arbitrary-length schedules.
|
||
|
|
for(unsigned word=0;word<1000;++word){
|
||
|
|
Wire wire;Budgets budgets{Milliseconds(2),Milliseconds(2),Milliseconds(2),Milliseconds(3),Milliseconds(2)};
|
||
|
|
ActiveGoalRegistry registry(wire,std::string(directory)+"/"+std::to_string(cases),budgets);
|
||
|
|
GoalRequest q;q.robot_id="r";q.trace={"t","s","run",1,1,1,1};
|
||
|
|
const auto id=*registry.start(q,SteadyTime{});unsigned rest=word;bool terminal=false;SteadyTime now{};
|
||
|
|
for(unsigned step=0;step<3;++step){
|
||
|
|
const auto before=registry.find(id)->state;const unsigned kind=rest%10;rest/=10;
|
||
|
|
GoalEvent e;e.goal_id=id;e.trace=q.trace;
|
||
|
|
switch(kind){
|
||
|
|
case 0:e.kind=EventKind::ACCEPTED;break;
|
||
|
|
case 1:e.kind=EventKind::REJECTED;break;
|
||
|
|
case 2:e.kind=EventKind::FEEDBACK;e.sequence=step+1;break;
|
||
|
|
case 3:e.kind=EventKind::FEEDBACK;e.sequence=0;break;
|
||
|
|
case 4:e.kind=EventKind::CANCEL_ACK;break;
|
||
|
|
case 5:e.kind=EventKind::RESULT;e.native_status=NativeStatus::SUCCEEDED;e.result.code=ResultCode::COMPLETED;e.result.stop=StopState::CONFIRMED;break;
|
||
|
|
case 6:e.kind=EventKind::RESULT;e.native_status=NativeStatus::CANCELED;e.result.code=ResultCode::CANCELED;e.result.stop=StopState::UNKNOWN;break;
|
||
|
|
case 7:e.kind=EventKind::RESULT;e.trace.run_id="stale";e.native_status=NativeStatus::SUCCEEDED;e.result.code=ResultCode::COMPLETED;e.result.stop=StopState::CONFIRMED;break;
|
||
|
|
case 8:registry.request_cancel(id,now);break;
|
||
|
|
case 9:now+=Milliseconds(5);break;
|
||
|
|
}
|
||
|
|
if(kind<8)wire.events.push_back(e);
|
||
|
|
registry.pump(now);const auto* record=registry.find(id);
|
||
|
|
transitions.emplace(static_cast<int>(before),static_cast<int>(record->state));
|
||
|
|
if(terminal)assert(record->state==GoalState::TERMINAL);
|
||
|
|
terminal=record->state==GoalState::TERMINAL;
|
||
|
|
if(terminal){assert(record->result);assert(record->result->stop==StopState::CONFIRMED);}
|
||
|
|
if(!terminal)assert(registry.robot_locked("r"));
|
||
|
|
if(kind==4&&!terminal)assert(registry.robot_locked("r"));
|
||
|
|
if(kind==7)assert((before==GoalState::TERMINAL)==terminal);
|
||
|
|
assert(!registry.start(q,now));assert(wire.sends==1);
|
||
|
|
}
|
||
|
|
++cases;
|
||
|
|
}
|
||
|
|
// Full native-status x business-result x stop matrix, including mismatches.
|
||
|
|
unsigned combinations=0;
|
||
|
|
for(int n=0;n<5;++n)for(int b=0;b<5;++b)for(int stop=0;stop<2;++stop){
|
||
|
|
Wire wire;ActiveGoalRegistry registry(wire,std::string(directory)+"/"+std::to_string(cases++));
|
||
|
|
GoalRequest q;q.robot_id="r";q.trace={"t","s","run",1,1,1,1};const auto id=*registry.start(q,SteadyTime{});
|
||
|
|
GoalEvent e;e.goal_id=id;e.trace=q.trace;e.kind=EventKind::RESULT;e.native_status=static_cast<NativeStatus>(n);e.result.code=static_cast<ResultCode>(b);e.result.stop=static_cast<StopState>(stop);wire.events.push_back(e);registry.pump(SteadyTime{});
|
||
|
|
const bool matches=(n==0&&b==0)||(n==1&&(b==1||b==3||b==4))||(n==2&&b==2)||(n==3&&b==4);
|
||
|
|
assert(registry.robot_locked("r")==!(matches&&stop==1));++combinations;
|
||
|
|
}
|
||
|
|
std::cout<<"{\"event_sequences\":1000,\"sequence_length\":3,\"event_alphabet\":10,\"result_combinations\":"<<combinations<<",\"observed_state_edges\":"<<transitions.size()<<"}\n";
|
||
|
|
}
|