106 lines
7.8 KiB
C++
106 lines
7.8 KiB
C++
#include "robot_bt/core.hpp"
|
|
#include <cassert>
|
|
#include <cstdlib>
|
|
#include <filesystem>
|
|
#include <iostream>
|
|
#include <limits>
|
|
#include <stdexcept>
|
|
using namespace robot_bt;
|
|
struct Driver final : GoalDriver {
|
|
bool available{true};
|
|
std::vector<GoalRequest> sent;
|
|
std::vector<std::string> cancellations;
|
|
std::vector<GoalEvent> events;
|
|
bool ready(Skill) const override { return available; }
|
|
void send(const GoalRequest& request) override { sent.push_back(request); }
|
|
void cancel(const std::string& id) override { cancellations.push_back(id); }
|
|
std::vector<GoalEvent> drain_events() override { auto result=events; events.clear(); return result; }
|
|
};
|
|
std::string journal(const std::string& name) {
|
|
static const std::string root=[](){char path[]="/tmp/robot_bt_registry_tests_XXXXXX";const char* made=::mkdtemp(path);assert(made);return std::string(made);}();
|
|
return root+"/"+name+".journal";
|
|
}
|
|
GoalRequest request(unsigned attempt=1) {
|
|
GoalRequest q; q.robot_id="sim_robot"; q.trace={"task","nav","run",1,1,1,attempt}; q.skill=Skill::NAVIGATE; return q;
|
|
}
|
|
GoalEvent event(const GoalRecord& r, EventKind kind) { GoalEvent e; e.goal_id=r.request.goal_id; e.trace=r.request.trace; e.kind=kind; return e; }
|
|
void test_journal_excludes_second_executor() {
|
|
Driver d; auto path=journal("process_lock"); ActiveGoalRegistry first(d,path); bool refused=false;
|
|
try { ActiveGoalRegistry second(d,path); } catch(const std::runtime_error&) { refused=true; }
|
|
assert(refused);
|
|
}
|
|
void test_one_send_and_restart_lock() {
|
|
Driver d; auto path=journal("one"); auto now=SteadyTime{}; std::string id;
|
|
{
|
|
ActiveGoalRegistry r(d,path);
|
|
auto started=r.start(request(),now); assert(started && d.sent.size()==1); id=*started;
|
|
for(int i=0;i<50;++i) { r.pump(now); auto second=r.start(request(),now); assert(!second); }
|
|
assert(d.sent.size()==1 && r.robot_locked("sim_robot"));
|
|
}
|
|
Driver other; ActiveGoalRegistry recovered(other,path);
|
|
assert(recovered.robot_locked("sim_robot"));
|
|
assert(recovered.find(id)->state==GoalState::STOP_UNKNOWN);
|
|
assert(!recovered.start(request(2),now)); assert(other.sent.empty());
|
|
}
|
|
void test_cancellation_and_stale_messages() {
|
|
Driver d; Budgets b; b.acceptance=Milliseconds(10); b.cancel_stop=Milliseconds(20);
|
|
ActiveGoalRegistry r(d,journal("cancel"),b); auto now=SteadyTime{}; auto id=*r.start(request(),now);
|
|
r.pump(now+Milliseconds(11)); assert(r.find(id)->cancel_intent && d.cancellations.size()==1);
|
|
auto ack=event(*r.find(id),EventKind::CANCEL_ACK); d.events.push_back(ack); r.pump(now+Milliseconds(12)); assert(r.robot_locked("sim_robot"));
|
|
auto accepted=event(*r.find(id),EventKind::ACCEPTED); d.events.push_back(accepted); r.pump(now+Milliseconds(13)); assert(d.cancellations.size()==2);
|
|
auto stale=event(*r.find(id),EventKind::RESULT); stale.trace.execution_generation=2; stale.native_status=NativeStatus::SUCCEEDED; stale.result={ResultCode::COMPLETED,StopState::CONFIRMED,{},""};
|
|
d.events.push_back(stale); r.pump(now+Milliseconds(14)); assert(r.robot_locked("sim_robot"));
|
|
r.pump(now+Milliseconds(32)); assert(r.find(id)->state==GoalState::STOP_UNKNOWN);
|
|
auto result=event(*r.find(id),EventKind::RESULT); result.native_status=NativeStatus::CANCELED; result.result={ResultCode::CANCELED,StopState::CONFIRMED,{},"stopped"};
|
|
d.events.push_back(result); r.pump(now+Milliseconds(33)); assert(!r.robot_locked("sim_robot"));
|
|
assert(!r.start(request(),now)); assert(r.start(request(2),now));
|
|
}
|
|
void test_feedback_and_protocol_failure() {
|
|
Driver d; Budgets b; b.feedback=Milliseconds(10); ActiveGoalRegistry r(d,journal("feedback"),b); auto now=SteadyTime{}; auto id=*r.start(request(),now);
|
|
d.events.push_back(event(*r.find(id),EventKind::ACCEPTED)); r.pump(now);
|
|
auto e=event(*r.find(id),EventKind::FEEDBACK); e.sequence=2; d.events.push_back(e); r.pump(now+Milliseconds(5));
|
|
e.sequence=1; d.events.push_back(e); r.pump(now+Milliseconds(12)); assert(!r.find(id)->cancel_intent);
|
|
d.events.push_back(e); r.pump(now+Milliseconds(16)); assert(r.find(id)->cancel_intent);
|
|
auto result=event(*r.find(id),EventKind::RESULT); result.native_status=NativeStatus::SUCCEEDED; result.result={ResultCode::FAILED,StopState::CONFIRMED,{},"contradiction"};
|
|
d.events.push_back(result); r.pump(now+Milliseconds(17)); assert(r.find(id)->state==GoalState::STOP_UNKNOWN);
|
|
assert(!r.reconcile(id,request().trace,StopState::CONFIRMED,false,"evidence"));
|
|
assert(!r.reconcile(id,request().trace,StopState::CONFIRMED,true,""));
|
|
assert(r.reconcile(id,request().trace,StopState::CONFIRMED,true,"operator-17"));
|
|
}
|
|
void test_rejection_releases_and_unknown_stops_lock() {
|
|
Driver d; ActiveGoalRegistry r(d,journal("reject")); auto now=SteadyTime{}; auto id=*r.start(request(),now);
|
|
d.events.push_back(event(*r.find(id),EventKind::REJECTED)); r.pump(now); assert(!r.robot_locked("sim_robot"));
|
|
id=*r.start(request(2),now); auto e=event(*r.find(id),EventKind::RESULT); e.native_status=NativeStatus::SUCCEEDED; e.result={ResultCode::COMPLETED,StopState::UNKNOWN,{},""};
|
|
d.events.push_back(e); r.pump(now); assert(r.robot_locked("sim_robot"));
|
|
}
|
|
void test_completion_after_deadline_cannot_be_success() {
|
|
Driver d;Budgets b;b.acceptance=Milliseconds(10);ActiveGoalRegistry r(d,journal("late_success"),b);auto now=SteadyTime{};auto id=*r.start(request(),now);
|
|
auto result=event(*r.find(id),EventKind::RESULT);result.native_status=NativeStatus::SUCCEEDED;result.result={ResultCode::COMPLETED,StopState::CONFIRMED,{},"late"};d.events.push_back(result);
|
|
r.pump(now+Milliseconds(11));assert(r.find(id)->cancel_intent);assert(!r.robot_locked("sim_robot"));
|
|
}
|
|
void test_contradictory_rejection_cannot_release_unknown_motion() {
|
|
Driver d;ActiveGoalRegistry r(d,journal("contradiction"));auto now=SteadyTime{};auto id=*r.start(request(),now);
|
|
auto result=event(*r.find(id),EventKind::RESULT);result.native_status=NativeStatus::SUCCEEDED;result.result={ResultCode::COMPLETED,StopState::UNKNOWN,{},"uncertain execution"};d.events.push_back(result);r.pump(now);
|
|
d.events.push_back(event(*r.find(id),EventKind::REJECTED));r.pump(now);assert(r.robot_locked("sim_robot"));
|
|
}
|
|
void test_accepted_goal_remote_rejection_releases_stopped_resource() {
|
|
Driver d;ActiveGoalRegistry r(d,journal("remote_rejected"));auto now=SteadyTime{};auto id=*r.start(request(),now);d.events.push_back(event(*r.find(id),EventKind::ACCEPTED));r.pump(now);
|
|
auto result=event(*r.find(id),EventKind::RESULT);result.native_status=NativeStatus::ABORTED;result.result={ResultCode::REJECTED,StopState::CONFIRMED,{},"remote controller refused"};d.events.push_back(result);r.pump(now);assert(!r.robot_locked("sim_robot"));assert(r.find(id)->result->code==ResultCode::REJECTED);
|
|
}
|
|
void test_geometry_and_snapshot_binding() {
|
|
Pose p{"map",1,2,0,0,0,0,1}; assert(valid_pose(p)); auto q=p; q.x+=0.06; assert(!within_tolerance(q,p,0.05,0.1));
|
|
q=p; q.x=std::numeric_limits<double>::quiet_NaN(); assert(!valid_pose(q)); assert(!within_tolerance(q,p,0.05,0.1));
|
|
TargetBinding t; t.meta={1,request().trace,"goal","perception",3,100,300}; t.target_id="item"; t.pose=p;
|
|
assert(valid_target(t,request().trace,"item",3,100,200));
|
|
assert(!valid_target(t,request().trace,"item",4,100,200));
|
|
assert(!valid_target(t,request().trace,"item",3,101,200));
|
|
assert(!valid_target(t,request().trace,"item",3,100,301));
|
|
PlacementBinding place{t.meta,"item","bin",p,true};
|
|
assert(!valid_placement(place,request().trace,"item","wrong",3,100,200));
|
|
assert(!SkillResponse{}.valid);
|
|
}
|
|
int main() {
|
|
test_accepted_goal_remote_rejection_releases_stopped_resource(); test_contradictory_rejection_cannot_release_unknown_motion(); test_completion_after_deadline_cannot_be_success(); test_journal_excludes_second_executor(); test_one_send_and_restart_lock(); test_cancellation_and_stale_messages(); test_feedback_and_protocol_failure(); test_rejection_releases_and_unknown_stops_lock(); test_geometry_and_snapshot_binding();
|
|
std::cout << "core registry and geometry tests passed\n";
|
|
}
|