52 lines
4.0 KiB
C++
52 lines
4.0 KiB
C++
|
#pragma once
|
||
|
|
#include "robot_bt/core.hpp"
|
||
|
|
#include <cassert>
|
||
|
|
#include <cstdlib>
|
||
|
|
#include <filesystem>
|
||
|
|
#include <iostream>
|
||
|
|
#include <limits>
|
||
|
|
using namespace robot_bt;
|
||
|
|
struct Simulator : GoalDriver {
|
||
|
|
std::vector<GoalRequest> sent; std::vector<GoalEvent> events; RosTime now{1000000};
|
||
|
|
std::optional<Skill> unavailable_skill;
|
||
|
|
Holding sensor_holding{Holding::EMPTY};
|
||
|
|
Admission admission{Admission::DIRECT}; bool bad_container{false}, unknown_verification{false}, nan_geometry{false}, no_stop{false};
|
||
|
|
bool ready(Skill skill)const override{return !unavailable_skill||*unavailable_skill!=skill;}
|
||
|
|
void cancel(const std::string&)override{}
|
||
|
|
void send(const GoalRequest& q)override {
|
||
|
|
if(q.skill==Skill::PICK)sensor_holding=Holding::HOLDING_TARGET;
|
||
|
|
if(q.skill==Skill::PLACE)sensor_holding=Holding::EMPTY;
|
||
|
|
sent.push_back(q); GoalEvent accept; accept.goal_id=q.goal_id; accept.trace=q.trace; accept.kind=EventKind::ACCEPTED; events.push_back(accept);
|
||
|
|
GoalEvent e=accept; e.kind=EventKind::RESULT; e.native_status=NativeStatus::SUCCEEDED; e.result.code=ResultCode::COMPLETED; e.result.stop=no_stop?StopState::UNKNOWN:StopState::CONFIRMED;
|
||
|
|
auto& r=e.result.response; r.valid=true; r.target_id=q.target_id; r.destination_id=bad_container?"wrong-bin":q.destination_id; r.base_stopped=true; r.verified=!unknown_verification; r.in_destination=true;
|
||
|
|
SnapshotMeta meta{1,q.trace,q.goal_id,"independent-simulator",q.geometry_epoch,now,now+1000000000}; r.evidence=meta;
|
||
|
|
Pose p{"map",0,0,0,0,0,0,1}; if(nan_geometry)p.x=std::numeric_limits<double>::quiet_NaN();
|
||
|
|
switch(q.skill) {
|
||
|
|
case Skill::NAVIGATE:r.final_pose=q.registered_pose;break;
|
||
|
|
case Skill::LOCATE_SHELF_COLUMN:r.shelf=q.shelf;r.side="front";r.column="1";r.tier="2";break;
|
||
|
|
case Skill::LOCALIZE_TARGET:r.target=TargetBinding{meta,q.target_id,p};break;
|
||
|
|
case Skill::EVALUATE_GRASP:r.admission=admission;r.posture_id="small-lift";break;
|
||
|
|
case Skill::ADJUST_POSTURE:admission=Admission::DIRECT;break;
|
||
|
|
case Skill::VERIFY_PICK:case Skill::VERIFY_TRANSPORT:r.holding=unknown_verification?Holding::UNKNOWN:Holding::HOLDING_TARGET;break;
|
||
|
|
case Skill::CHECK_FREE_SPACE:r.placement=PlacementBinding{meta,q.target_id,r.destination_id,p,true};break;
|
||
|
|
case Skill::VERIFY_EMPTY:case Skill::VERIFY_PLACE:r.holding=unknown_verification?Holding::UNKNOWN:Holding::EMPTY;break;
|
||
|
|
default:break;
|
||
|
|
}
|
||
|
|
events.push_back(e);
|
||
|
|
}
|
||
|
|
std::vector<GoalEvent> drain_events()override{auto r=events;events.clear();return r;}
|
||
|
|
};
|
||
|
|
struct Fixture {
|
||
|
|
Simulator driver; ContextStore context; std::string journal; ActiveGoalRegistry registry; TaskConfig task; SiteConfig site; unsigned deliveries{0};
|
||
|
|
Fixture(const std::string& name):journal(test_root()+"/"+name+".journal"),registry(driver,journal) {
|
||
|
|
task.trace={"task","root","run",1,1,1,1}; task.robot_id="sim";task.target_id="item";task.source_shelf="shelf";task.destination_id="bin";task.observe_location="observe";task.destination_location="bin-nav";
|
||
|
|
site.locations={{"observe",Pose{"map",0,0,0,0,0,0,1}},{"source",Pose{"map",1,0,0,0,0,0,1}},{"bin-nav",Pose{"map",2,0,0,0,0,0,1}}};site.parking_locations={{"shelf/front/1","source"}};site.allowed_postures={"small-lift","carry"};site.transport_posture="carry";
|
||
|
|
}
|
||
|
|
static const std::string& test_root(){static const std::string root=[](){char path[]="/tmp/robot_bt_workflow_tests_XXXXXX";const char* made=::mkdtemp(path);assert(made);return std::string(made);}();return root;}
|
||
|
|
StageRunner runner(){return StageRunner(task,site,driver,registry,context,[this](const std::string& id,unsigned index,const std::string& evidence){assert(id=="task"&&index==0&&!evidence.empty());++deliveries;return true;});}
|
||
|
|
TickStatus execute(StageRunner& r,unsigned ticks=300) {
|
||
|
|
Workflow flow(r); auto status=TickStatus::RUNNING; for(unsigned i=0;i<ticks&&status==TickStatus::RUNNING;++i) { driver.now=1000000+static_cast<RosTime>(i)*1000000; r.update_safety({true,true,driver.sensor_holding,driver.now,driver.now+1000000000}); status=flow.tick(SteadyTime{}+Milliseconds(i),driver.now); } return status;
|
||
|
|
}
|
||
|
|
unsigned count(Skill skill)const{unsigned n=0;for(const auto&q:driver.sent)if(q.skill==skill)++n;return n;}
|
||
|
|
};
|