实现行为树执行器、任务协调和技能接口
This commit is contained in:
@@ -0,0 +1,22 @@
|
||||
cmake_minimum_required(VERSION 3.16)
|
||||
project(robot_bt_core LANGUAGES CXX)
|
||||
set(CMAKE_CXX_STANDARD 17)
|
||||
set(CMAKE_CXX_STANDARD_REQUIRED ON)
|
||||
add_library(robot_bt_core src/core.cpp src/workflow.cpp)
|
||||
target_include_directories(robot_bt_core PUBLIC include)
|
||||
target_compile_options(robot_bt_core PRIVATE -Wall -Wextra -Werror)
|
||||
add_library(robot_bt_sim src/sim_driver.cpp)
|
||||
target_link_libraries(robot_bt_sim PUBLIC robot_bt_core)
|
||||
target_compile_options(robot_bt_sim PRIVATE -Wall -Wextra -Werror)
|
||||
add_executable(robot_bt_demo examples/demo.cpp)
|
||||
target_link_libraries(robot_bt_demo PRIVATE robot_bt_sim)
|
||||
target_compile_options(robot_bt_demo PRIVATE -Wall -Wextra -Werror)
|
||||
include(CTest)
|
||||
if(BUILD_TESTING)
|
||||
foreach(test_name core_test workflow_test journal_failure_test preflight_test settlement_test proof_regression_test readiness_regression_test scenario_test lifecycle_test)
|
||||
add_executable(${test_name} tests/${test_name}.cpp)
|
||||
target_link_libraries(${test_name} PRIVATE robot_bt_core)
|
||||
target_compile_options(${test_name} PRIVATE -Wall -Wextra -Werror -UNDEBUG)
|
||||
add_test(NAME ${test_name} COMMAND ${test_name})
|
||||
endforeach()
|
||||
endif()
|
||||
@@ -0,0 +1,32 @@
|
||||
#include "robot_bt/core.hpp"
|
||||
#include "robot_bt/sim_driver.hpp"
|
||||
#include <iostream>
|
||||
#include <map>
|
||||
#include <stdexcept>
|
||||
#include <string>
|
||||
using namespace robot_bt;
|
||||
namespace {
|
||||
std::string json(const std::string& text) {std::string result="\"";static const char hex[]="0123456789abcdef";for(unsigned char c:text){if(c=='"'||c=='\\'){result+='\\';result+=static_cast<char>(c);}else if(c<32){result+="\\u00";result+=hex[c>>4];result+=hex[c&15];}else result+=static_cast<char>(c);}return result+'"';}
|
||||
const char* status_name(TickStatus s){switch(s){case TickStatus::SUCCESS:return "SUCCEEDED";case TickStatus::FAILURE:return "FAILED";case TickStatus::INTERVENTION_REQUIRED:return "INTERVENTION_REQUIRED";default:return "EXECUTING";}}
|
||||
}
|
||||
int main(int argc,char** argv) {
|
||||
bool jsonl=false;std::map<std::string,std::string> args={{"--target-ref","sim-item"},{"--destination-ref","sim-bin"},{"--task-id","sim-task"},{"--run-id","sim-run"},{"--robot-id","sim-robot"},{"--scenario","happy"},{"--route","LEGACY"},{"--item-index","0"}};
|
||||
try {
|
||||
for(int i=1;i<argc;++i){std::string key=argv[i];if(key=="--events-jsonl"){jsonl=true;continue;}if((key!="--journal"&&args.count(key)==0)||i+1>=argc)throw std::invalid_argument("usage: robot_bt_demo --journal PATH [--events-jsonl --target-ref ID --destination-ref ID --task-id ID --run-id ID --robot-id ID --scenario NAME]");args[key]=argv[++i];}
|
||||
if(args["--journal"].empty())throw std::invalid_argument("explicit --journal PATH required");
|
||||
SimDriver driver(args["--scenario"]);ActiveGoalRegistry registry(driver,args["--journal"]);ContextStore context;
|
||||
TaskConfig task;task.trace={args["--task-id"],"fixed-template",args["--run-id"],1,1,1,1};task.robot_id=args["--robot-id"];task.target_id=args["--target-ref"];task.source_shelf="sim-shelf";task.destination_id=args["--destination-ref"];task.observe_location="sim-observe";task.destination_location="sim-destination";
|
||||
task.route=args["--route"];task.item_index=static_cast<unsigned>(std::stoul(args["--item-index"]));
|
||||
SiteConfig site;site.locations={{"sim-observe",Pose{"map",0,0,0,0,0,0,1}},{"sim-source",Pose{"map",1,0,0,0,0,0,1}},{"sim-destination",Pose{"map",2,0,0,0,0,0,1}}};site.parking_locations={{"sim-shelf/front/1","sim-source"}};site.allowed_postures={"registered-small-lift","registered-carry"};site.transport_posture="registered-carry";
|
||||
site.object_locations[task.target_id]="sim-source";site.object_postures[task.target_id]="registered-small-lift";site.cell_locations["sim-shelf/front/1/2"]="sim-source";site.cell_postures["sim-shelf/front/1/2"]="registered-small-lift";
|
||||
std::string evidence_id;bool delivered=false;
|
||||
StageRunner runner(task,site,driver,registry,context,[&](const std::string& id,unsigned item,const std::string& evidence){if(id!=task.trace.task_id||item!=task.item_index||evidence.empty())return false;delivered=true;evidence_id=evidence;return true;});Workflow workflow(runner);
|
||||
auto status=TickStatus::RUNNING;
|
||||
for(unsigned tick=0;tick<500&&status==TickStatus::RUNNING;++tick){const auto ros=1000000+static_cast<RosTime>(tick)*50000000;driver.set_ros_time(ros);runner.update_safety({true,true,driver.sensor_holding(),ros,ros+1000000000});auto stage=workflow.current_stage();status=workflow.tick(SteadyTime{}+Milliseconds(tick*50),ros);if(workflow.current_stage()!=stage||status!=TickStatus::RUNNING){if(jsonl)std::cout<<"{\"type\":\"stage\",\"stage\":"<<json(stage_name(stage))<<",\"status\":"<<json(status_name(status))<<"}\n";else std::cout<<stage_name(stage)<<": "<<status_name(status)<<'\n';}}
|
||||
if(status==TickStatus::RUNNING){workflow.halt(SteadyTime{}+Milliseconds(25000));status=TickStatus::INTERVENTION_REQUIRED;}
|
||||
const bool success=status==TickStatus::SUCCESS&&delivered&&runner.holding()==Holding::EMPTY;
|
||||
if(jsonl)std::cout<<"{\"type\":\"result\",\"status\":"<<json(status_name(status))<<",\"completed_quantity\":"<<(success?1:0)<<",\"stop_confirmed\":"<<(!registry.has_unresolved()?"true":"false")<<",\"simulated\":true,\"detail\":"<<json(runner.detail())<<",\"evidence\":{\"target_ref\":"<<json(task.target_id)<<",\"destination_ref\":"<<json(task.destination_id)<<",\"empty_hand\":"<<(success?"true":"false")<<",\"in_destination\":"<<(success?"true":"false")<<",\"safe_to_release\":"<<(success?"true":"false")<<",\"valid\":"<<(success?"true":"false")<<",\"passed\":"<<(success?"true":"false")<<",\"evidence_id\":"<<json(evidence_id)<<",\"source\":\"simulated-independent-verifier\"}}\n";
|
||||
else std::cout<<"Simulation result: "<<status_name(status)<<"; delivered="<<(success?1:0)<<"; "<<runner.detail()<<'\n';
|
||||
return success?0:1;
|
||||
}catch(const std::exception& error){if(jsonl)std::cout<<"{\"type\":\"result\",\"status\":\"INTERVENTION_REQUIRED\",\"completed_quantity\":0,\"stop_confirmed\":false,\"simulated\":true,\"detail\":"<<json(error.what())<<",\"evidence\":{\"safe_to_release\":false,\"valid\":false,\"passed\":false}}\n";else std::cerr<<error.what()<<'\n';return 2;}
|
||||
}
|
||||
@@ -0,0 +1,211 @@
|
||||
#pragma once
|
||||
#include <chrono>
|
||||
#include <cstdint>
|
||||
#include <functional>
|
||||
#include <map>
|
||||
#include <mutex>
|
||||
#include <optional>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
namespace robot_bt {
|
||||
using SteadyClock = std::chrono::steady_clock;
|
||||
using SteadyTime = SteadyClock::time_point;
|
||||
using Milliseconds = std::chrono::milliseconds;
|
||||
using RosTime = std::int64_t;
|
||||
struct Trace {
|
||||
std::string task_id, subtask_id, run_id;
|
||||
std::uint64_t task_revision{1}, plan_version{1}, execution_generation{1};
|
||||
std::uint32_t attempt{1};
|
||||
};
|
||||
bool same_trace(const Trace&, const Trace&);
|
||||
struct Pose { std::string frame_id; double x{0}, y{0}, z{0}, qx{0}, qy{0}, qz{0}, qw{1}; };
|
||||
bool valid_pose(const Pose&);
|
||||
bool within_tolerance(const Pose& actual, const Pose& expected, double position_m, double orientation_rad);
|
||||
enum class Holding { EMPTY, HOLDING_TARGET, HOLDING_OTHER, UNKNOWN };
|
||||
enum class Admission { DIRECT, ADJUST_POSTURE, UNKNOWN, NOT_REACHABLE };
|
||||
enum class StopState { UNKNOWN, CONFIRMED };
|
||||
enum class ResultCode { COMPLETED, FAILED, CANCELED, TIMED_OUT, REJECTED };
|
||||
enum class NativeStatus { SUCCEEDED, ABORTED, CANCELED, REJECTED, UNKNOWN };
|
||||
enum class Skill { NAVIGATE, LOCATE_SHELF_COLUMN, LOCALIZE_TARGET, EVALUATE_GRASP, ADJUST_POSTURE, PICK, VERIFY_PICK, TRANSPORT_POSTURE, VERIFY_TRANSPORT, CHECK_FREE_SPACE, PLACE, VERIFY_PLACE, VERIFY_EMPTY };
|
||||
struct SnapshotMeta {
|
||||
std::uint32_t schema_version{1};
|
||||
Trace trace;
|
||||
std::string source_goal_id, writer;
|
||||
std::uint64_t geometry_epoch{0};
|
||||
RosTime observed_at{0}, valid_until{0};
|
||||
};
|
||||
struct TargetBinding { SnapshotMeta meta; std::string target_id; Pose pose; };
|
||||
struct PlacementBinding { SnapshotMeta meta; std::string target_id, destination_id; Pose pose; bool free_space_confirmed{false}; };
|
||||
struct SafetySnapshot { bool safe{false}, stationary{false}; Holding holding{Holding::UNKNOWN}; RosTime observed_at{0}, valid_until{0}; };
|
||||
// Default-constructed responses cannot advance execution.
|
||||
struct SkillResponse {
|
||||
bool valid{false};
|
||||
std::optional<TargetBinding> target;
|
||||
std::optional<PlacementBinding> placement;
|
||||
std::optional<SnapshotMeta> evidence;
|
||||
std::optional<Pose> final_pose;
|
||||
std::string target_id, destination_id, shelf, side, column, tier, posture_id;
|
||||
Admission admission{Admission::UNKNOWN};
|
||||
Holding holding{Holding::UNKNOWN};
|
||||
bool verified{false}, in_destination{false}, base_stopped{false};
|
||||
};
|
||||
struct ExecutionResult { ResultCode code{ResultCode::FAILED}; StopState stop{StopState::UNKNOWN}; SkillResponse response; std::string detail; };
|
||||
struct GoalRequest {
|
||||
std::string goal_id, robot_id;
|
||||
Trace trace;
|
||||
Skill skill{Skill::LOCALIZE_TARGET};
|
||||
std::optional<Pose> registered_pose;
|
||||
std::optional<TargetBinding> target;
|
||||
std::optional<PlacementBinding> placement;
|
||||
std::string target_id, destination_id, shelf, posture_id;
|
||||
std::string navigation_kind, navigation_ref, side, column, tier;
|
||||
std::uint64_t geometry_epoch{0};
|
||||
RosTime capture_after{0};
|
||||
double position_tolerance_m{0.05}, orientation_tolerance_rad{0.1};
|
||||
};
|
||||
enum class EventKind { ACCEPTED, REJECTED, FEEDBACK, CANCEL_ACK, RESULT };
|
||||
struct GoalEvent {
|
||||
EventKind kind{EventKind::FEEDBACK};
|
||||
std::string goal_id;
|
||||
Trace trace;
|
||||
std::uint64_t sequence{0};
|
||||
NativeStatus native_status{NativeStatus::UNKNOWN};
|
||||
ExecutionResult result;
|
||||
};
|
||||
class GoalDriver {
|
||||
public:
|
||||
virtual ~GoalDriver() = default;
|
||||
virtual bool ready(Skill) const = 0;
|
||||
// Must be asynchronous; enqueue callback events and return promptly.
|
||||
virtual void send(const GoalRequest&) = 0;
|
||||
virtual void cancel(const std::string& goal_id) = 0;
|
||||
virtual std::vector<GoalEvent> drain_events() = 0;
|
||||
};
|
||||
struct Budgets { Milliseconds readiness{2000}, acceptance{2000}, feedback{5000}, execution{120000}, cancel_stop{5000}; };
|
||||
enum class GoalState { SENDING, ACTIVE, CANCEL_REQUESTED, STOP_UNKNOWN, TERMINAL };
|
||||
struct GoalRecord {
|
||||
GoalRequest request;
|
||||
GoalState state{GoalState::SENDING};
|
||||
bool cancel_intent{false}, accepted{false}, restarted{false};
|
||||
std::uint64_t last_sequence{0};
|
||||
SteadyTime sent_at{}, accepted_at{}, last_feedback{}, cancel_at{};
|
||||
std::optional<ExecutionResult> result;
|
||||
};
|
||||
class ActiveGoalRegistry {
|
||||
public:
|
||||
ActiveGoalRegistry(GoalDriver&, std::string journal_path, Budgets = {});
|
||||
~ActiveGoalRegistry();
|
||||
ActiveGoalRegistry(const ActiveGoalRegistry&) = delete;
|
||||
ActiveGoalRegistry& operator=(const ActiveGoalRegistry&) = delete;
|
||||
// Registration is flushed before send. Failed journal writes throw and prevent send.
|
||||
std::optional<std::string> start(GoalRequest, SteadyTime);
|
||||
void pump(SteadyTime);
|
||||
void request_cancel(const std::string&, SteadyTime);
|
||||
bool robot_locked(const std::string&) const;
|
||||
bool has_unresolved() const;
|
||||
const GoalRecord* find(const std::string&) const;
|
||||
const std::map<std::string, GoalRecord>& records() const { return records_; }
|
||||
// Caller must verify the authenticated dedicated physical-reconciliation interface.
|
||||
bool reconcile(const std::string& goal_id, const Trace&, StopState, bool authorized, const std::string& evidence_id);
|
||||
private:
|
||||
GoalDriver& driver_;
|
||||
std::string journal_path_;
|
||||
Budgets budgets_;
|
||||
std::map<std::string, GoalRecord> records_;
|
||||
bool journal_failed_{false};
|
||||
int lock_fd_{-1};
|
||||
void append(const GoalRecord&);
|
||||
void ingest(const GoalEvent&, SteadyTime);
|
||||
void load();
|
||||
};
|
||||
class ContextStore {
|
||||
public:
|
||||
void replace_target(TargetBinding);
|
||||
void replace_placement(PlacementBinding);
|
||||
std::optional<TargetBinding> target() const;
|
||||
std::optional<PlacementBinding> placement() const;
|
||||
void invalidate_geometry();
|
||||
private:
|
||||
mutable std::mutex mutex_;
|
||||
std::optional<TargetBinding> target_;
|
||||
std::optional<PlacementBinding> placement_;
|
||||
};
|
||||
bool valid_target(const TargetBinding&, const Trace&, const std::string& target_id, std::uint64_t epoch, RosTime capture_after, RosTime now);
|
||||
bool valid_placement(const PlacementBinding&, const Trace&, const std::string& target_id, const std::string& destination_id, std::uint64_t epoch, RosTime capture_after, RosTime now);
|
||||
struct SiteConfig {
|
||||
std::map<std::string, Pose> locations;
|
||||
// key = shelf + "/" + side + "/" + column; value = registered location ID.
|
||||
std::map<std::string, std::string> parking_locations;
|
||||
std::map<std::string,std::string> object_locations, object_postures, cell_locations, cell_postures;
|
||||
std::vector<std::string> allowed_postures;
|
||||
std::string transport_posture;
|
||||
};
|
||||
struct TaskConfig {
|
||||
Trace trace;
|
||||
std::string route{"LEGACY"};
|
||||
unsigned item_index{0};
|
||||
std::uint64_t initial_geometry_epoch{0};
|
||||
std::string robot_id, target_id, source_shelf, destination_id, observe_location, destination_location;
|
||||
double position_tolerance_m{0.05}, orientation_tolerance_rad{0.1};
|
||||
unsigned max_reobservations{2}, max_posture_adjustments{1};
|
||||
};
|
||||
enum class Stage { PREFLIGHT, NAVIGATE_OBSERVE, LOCATE_SHELF_COLUMN, NAVIGATE_SOURCE, LOCALIZE_TARGET, PICK, VERIFY_PICK, TRANSPORT_POSTURE, VERIFY_TRANSPORT, NAVIGATE_DESTINATION, CHECK_FREE_SPACE, PLACE, VERIFY_PLACE, DELIVER, CLEANUP };
|
||||
enum class TickStatus { RUNNING, SUCCESS, FAILURE, INTERVENTION_REQUIRED };
|
||||
const std::vector<Stage>& fixed_stages();
|
||||
const char* stage_name(Stage);
|
||||
class StageRunner {
|
||||
public:
|
||||
using Delivery = std::function<bool(const std::string& task_id, unsigned item_index, const std::string& verification_goal_id)>;
|
||||
StageRunner(TaskConfig, SiteConfig, GoalDriver&, ActiveGoalRegistry&, ContextStore&, Delivery, Budgets = {});
|
||||
TickStatus tick(Stage, SteadyTime, RosTime ros_now_ns);
|
||||
void halt(SteadyTime);
|
||||
// After halt, reconcile physical facts with read-only skills. SUCCESS here
|
||||
// means settled, never changes the original failed/canceled task outcome.
|
||||
TickStatus settle(SteadyTime, RosTime);
|
||||
void update_safety(SafetySnapshot value) { safety_ = value; }
|
||||
const std::string& detail() const { return detail_; }
|
||||
const std::string& active_goal_id() const { return active_goal_; }
|
||||
std::uint64_t geometry_epoch() const { return geometry_epoch_; }
|
||||
Holding holding() const { return holding_; }
|
||||
bool empty_verified(RosTime now) const { return holding_==Holding::EMPTY && empty_observed_at_>0 && empty_observed_at_<=now && empty_valid_until_>now; }
|
||||
private:
|
||||
TaskConfig task_;
|
||||
SiteConfig site_;
|
||||
GoalDriver& driver_;
|
||||
ActiveGoalRegistry& registry_;
|
||||
ContextStore& context_;
|
||||
Delivery delivery_;
|
||||
Budgets budgets_;
|
||||
SafetySnapshot safety_;
|
||||
Holding holding_{Holding::UNKNOWN};
|
||||
std::uint64_t geometry_epoch_{0};
|
||||
RosTime capture_after_{0}, last_ros_time_{0}, holding_valid_until_{0}, empty_valid_until_{0}, empty_observed_at_{0};
|
||||
std::size_t stage_index_{0};
|
||||
std::string active_goal_, source_location_, source_side_, source_column_, source_tier_, verification_goal_, pending_posture_, detail_;
|
||||
std::optional<SteadyTime> waiting_since_;
|
||||
std::optional<SteadyTime> motion_waiting_since_;
|
||||
unsigned reobservations_{0}, adjustments_{0}, serial_{0};
|
||||
enum class PickPhase { ASSESS, REOBSERVE, ADJUST, EXECUTE };
|
||||
PickPhase pick_phase_{PickPhase::ASSESS};
|
||||
bool halted_{false}, delivered_{false}, verify_place_ok_{false}, place_refresh_started_{false}, place_refresh_done_{false};
|
||||
std::optional<TickStatus> failure_;
|
||||
bool empty_refresh_started_{false};
|
||||
bool settlement_started_{false}, settlement_done_{false}, settlement_place_{false};
|
||||
std::optional<TickStatus> settlement_failure_;
|
||||
TickStatus fail(std::string, bool intervention = true);
|
||||
bool safe(RosTime) const;
|
||||
TickStatus run_goal(Stage, Skill, SteadyTime, RosTime, SkillResponse&, std::string& completed_goal);
|
||||
GoalRequest make_request(Stage, Skill, RosTime);
|
||||
};
|
||||
class Workflow {
|
||||
public:
|
||||
explicit Workflow(StageRunner& runner) : runner_(runner) {}
|
||||
TickStatus tick(SteadyTime, RosTime);
|
||||
void halt(SteadyTime now) { runner_.halt(now); }
|
||||
Stage current_stage() const;
|
||||
private:
|
||||
StageRunner& runner_;
|
||||
std::size_t index_{0};
|
||||
};
|
||||
} // namespace robot_bt
|
||||
@@ -0,0 +1,22 @@
|
||||
#pragma once
|
||||
#include "robot_bt/core.hpp"
|
||||
namespace robot_bt {
|
||||
// Deterministic, explicitly simulated capability responses. Never connects to ROS.
|
||||
class SimDriver final : public GoalDriver {
|
||||
public:
|
||||
explicit SimDriver(std::string scenario = "happy");
|
||||
bool ready(Skill) const override { return true; }
|
||||
void send(const GoalRequest&) override;
|
||||
void cancel(const std::string&) override;
|
||||
std::vector<GoalEvent> drain_events() override;
|
||||
Holding sensor_holding() const { return sensor_holding_; }
|
||||
void set_ros_time(RosTime value) { now_ = value; }
|
||||
private:
|
||||
std::string scenario_;
|
||||
RosTime now_{1};
|
||||
bool adjusted_{false};
|
||||
Holding sensor_holding_{Holding::EMPTY};
|
||||
std::map<std::string, GoalRequest> requests_;
|
||||
std::vector<GoalEvent> events_;
|
||||
};
|
||||
} // namespace robot_bt
|
||||
@@ -0,0 +1,193 @@
|
||||
#include "robot_bt/core.hpp"
|
||||
#include <algorithm>
|
||||
#include <array>
|
||||
#include <cmath>
|
||||
#include <filesystem>
|
||||
#include <fstream>
|
||||
#include <exception>
|
||||
#include <iomanip>
|
||||
#include <random>
|
||||
#include <sstream>
|
||||
#include <stdexcept>
|
||||
#include <utility>
|
||||
#include <cerrno>
|
||||
#include <fcntl.h>
|
||||
#include <sys/file.h>
|
||||
#include <unistd.h>
|
||||
|
||||
namespace robot_bt {
|
||||
namespace {
|
||||
bool same_context(const Trace& a,const Trace& b) {
|
||||
return a.task_id==b.task_id && a.run_id==b.run_id && a.task_revision==b.task_revision && a.plan_version==b.plan_version && a.execution_generation==b.execution_generation;
|
||||
}
|
||||
bool valid_trace(const Trace& t) { return !t.task_id.empty() && !t.subtask_id.empty() && !t.run_id.empty() && t.task_revision>0 && t.plan_version>0 && t.execution_generation>0 && t.attempt>0; }
|
||||
std::string uuid() {
|
||||
std::random_device random; std::array<unsigned char,16> bytes{};
|
||||
for(auto& b:bytes) b=static_cast<unsigned char>(random());
|
||||
bytes[6]=static_cast<unsigned char>((bytes[6]&0x0fU)|0x40U); bytes[8]=static_cast<unsigned char>((bytes[8]&0x3fU)|0x80U);
|
||||
std::ostringstream out; out<<std::hex<<std::setfill('0');
|
||||
for(std::size_t i=0;i<bytes.size();++i) { if(i==4||i==6||i==8||i==10) out<<'-'; out<<std::setw(2)<<static_cast<unsigned>(bytes[i]); }
|
||||
return out.str();
|
||||
}
|
||||
bool protocol_matches(NativeStatus native,ResultCode result) {
|
||||
switch(native) {
|
||||
case NativeStatus::SUCCEEDED:return result==ResultCode::COMPLETED;
|
||||
case NativeStatus::ABORTED:return result==ResultCode::FAILED||result==ResultCode::TIMED_OUT||result==ResultCode::REJECTED;
|
||||
case NativeStatus::CANCELED:return result==ResultCode::CANCELED;
|
||||
case NativeStatus::REJECTED:return result==ResultCode::REJECTED;
|
||||
default:return false;
|
||||
}
|
||||
}
|
||||
bool valid_meta(const SnapshotMeta& m,const Trace& t,std::uint64_t epoch,RosTime after,RosTime now) {
|
||||
return m.schema_version==1 && same_context(m.trace,t) && valid_trace(m.trace) && !m.source_goal_id.empty() && !m.writer.empty() && m.geometry_epoch==epoch && m.observed_at>0 && m.observed_at>=after && m.observed_at<=now && m.valid_until>now && m.valid_until>=m.observed_at;
|
||||
}
|
||||
}
|
||||
bool same_trace(const Trace& a,const Trace& b) { return same_context(a,b)&&a.subtask_id==b.subtask_id&&a.attempt==b.attempt; }
|
||||
bool valid_pose(const Pose& p) {
|
||||
if(p.frame_id.empty()) return false;
|
||||
for(double value:{p.x,p.y,p.z,p.qx,p.qy,p.qz,p.qw}) if(!std::isfinite(value)) return false;
|
||||
const double norm=p.qx*p.qx+p.qy*p.qy+p.qz*p.qz+p.qw*p.qw;
|
||||
return std::abs(norm-1.0)<=0.001;
|
||||
}
|
||||
bool within_tolerance(const Pose& a,const Pose& b,double position,double orientation) {
|
||||
if(!valid_pose(a)||!valid_pose(b)||a.frame_id!=b.frame_id||!std::isfinite(position)||!std::isfinite(orientation)||position<0||orientation<0) return false;
|
||||
const double distance=std::hypot(a.x-b.x,a.y-b.y);
|
||||
const auto yaw=[](const Pose& p){return std::atan2(2*(p.qw*p.qz+p.qx*p.qy),1-2*(p.qy*p.qy+p.qz*p.qz));};
|
||||
const double error=std::remainder(yaw(b)-yaw(a),2*std::acos(-1.0));
|
||||
return distance<=position && std::abs(error)<=orientation;
|
||||
}
|
||||
bool valid_target(const TargetBinding& b,const Trace& t,const std::string& target,std::uint64_t epoch,RosTime after,RosTime now) { return !target.empty()&&b.target_id==target&&valid_meta(b.meta,t,epoch,after,now)&&valid_pose(b.pose); }
|
||||
bool valid_placement(const PlacementBinding& b,const Trace& t,const std::string& target,const std::string& destination,std::uint64_t epoch,RosTime after,RosTime now) { return !target.empty()&&!destination.empty()&&b.target_id==target&&b.destination_id==destination&&b.free_space_confirmed&&valid_meta(b.meta,t,epoch,after,now)&&valid_pose(b.pose); }
|
||||
|
||||
ActiveGoalRegistry::ActiveGoalRegistry(GoalDriver& driver,std::string path,Budgets budgets):driver_(driver),journal_path_(std::move(path)),budgets_(budgets) {
|
||||
if(journal_path_.empty()) throw std::invalid_argument("explicit goal journal path required");
|
||||
for(auto budget:{budgets_.readiness,budgets_.acceptance,budgets_.feedback,budgets_.execution,budgets_.cancel_stop}) if(budget.count()<=0) throw std::invalid_argument("budgets must be positive");
|
||||
lock_fd_=::open((journal_path_+".lock").c_str(),O_RDWR|O_CREAT|O_CLOEXEC|O_NOFOLLOW,0600);
|
||||
if(lock_fd_<0) throw std::runtime_error("goal journal process lock cannot be opened");
|
||||
if(::flock(lock_fd_,LOCK_EX|LOCK_NB)!=0) {::close(lock_fd_);lock_fd_=-1;throw std::runtime_error("another executor owns this goal journal");}
|
||||
try {load();} catch(...) {::flock(lock_fd_,LOCK_UN);::close(lock_fd_);lock_fd_=-1;throw;}
|
||||
}
|
||||
ActiveGoalRegistry::~ActiveGoalRegistry() {if(lock_fd_>=0){::flock(lock_fd_,LOCK_UN);::close(lock_fd_);}}
|
||||
void ActiveGoalRegistry::append(const GoalRecord& r) {
|
||||
std::ostringstream out;
|
||||
const auto& t=r.request.trace;
|
||||
std::string detail_hex;
|
||||
if(r.result) {static const char hex[]="0123456789abcdef";for(unsigned char c:r.result->detail){detail_hex.push_back(hex[c>>4]);detail_hex.push_back(hex[c&15]);}}
|
||||
out<<2<<' '<<std::quoted(r.request.goal_id)<<' '<<std::quoted(r.request.robot_id)<<' '<<std::quoted(t.task_id)<<' '<<std::quoted(t.subtask_id)<<' '<<std::quoted(t.run_id)<<' '<<t.task_revision<<' '<<t.plan_version<<' '<<t.execution_generation<<' '<<t.attempt<<' '<<static_cast<int>(r.request.skill)<<' '<<static_cast<int>(r.state)<<' '<<r.cancel_intent<<' '<<r.accepted<<' '<<r.last_sequence<<' '<<(r.result?static_cast<int>(r.result->code):-1)<<' '<<(r.result?static_cast<int>(r.result->stop):0)<<' '<<std::quoted(detail_hex)<<'\n';
|
||||
const std::string bytes=out.str();
|
||||
const int fd=::open(journal_path_.c_str(),O_WRONLY|O_CREAT|O_APPEND|O_CLOEXEC|O_NOFOLLOW,0600);
|
||||
bool ok=fd>=0;
|
||||
if(ok) {std::size_t written=0;while(written<bytes.size()){const auto count=::write(fd,bytes.data()+written,bytes.size()-written);if(count<0&&errno==EINTR)continue;if(count<=0){ok=false;break;}written+=static_cast<std::size_t>(count);}if(::fsync(fd)!=0)ok=false;if(::close(fd)!=0)ok=false;}
|
||||
// Sync the containing directory as well, including first journal creation.
|
||||
auto parent=std::filesystem::path(journal_path_).parent_path();if(parent.empty())parent=".";
|
||||
const int directory=::open(parent.c_str(),O_RDONLY|O_DIRECTORY|O_CLOEXEC);
|
||||
if(directory<0)ok=false;else {if(::fsync(directory)!=0)ok=false;::close(directory);}
|
||||
if(!ok) { journal_failed_=true; throw std::runtime_error("goal journal sync failed; executor quarantined"); }
|
||||
}
|
||||
void ActiveGoalRegistry::load() {
|
||||
if(!std::filesystem::exists(journal_path_)) return;
|
||||
std::ifstream in(journal_path_); if(!in) throw std::runtime_error("goal journal unreadable; startup refused");
|
||||
std::string line;
|
||||
while(std::getline(in,line)) {
|
||||
if(in.eof())throw std::runtime_error("truncated goal journal; startup refused");
|
||||
std::istringstream row(line); std::string detail_hex; GoalRecord r; auto& t=r.request.trace; int version=0,skill=-1,state=-1,code=-1,stop=-1;
|
||||
if(!(row>>version>>std::quoted(r.request.goal_id)>>std::quoted(r.request.robot_id)>>std::quoted(t.task_id)>>std::quoted(t.subtask_id)>>std::quoted(t.run_id)>>t.task_revision>>t.plan_version>>t.execution_generation>>t.attempt>>skill>>state>>r.cancel_intent>>r.accepted>>r.last_sequence>>code>>stop>>std::quoted(detail_hex)) || version!=2 || skill<0 || skill>static_cast<int>(Skill::VERIFY_EMPTY) || state<0 || state>static_cast<int>(GoalState::TERMINAL) || code < -1 || code>static_cast<int>(ResultCode::REJECTED) || stop<0 || stop>1 || r.request.goal_id.empty() || r.request.robot_id.empty() || !valid_trace(t)) throw std::runtime_error("corrupt goal journal; startup refused for physical reconciliation");
|
||||
if(detail_hex.size()%2!=0||detail_hex.find_first_not_of("0123456789abcdef")!=std::string::npos)throw std::runtime_error("corrupt journal evidence");
|
||||
row>>std::ws; if(!row.eof()) throw std::runtime_error("unexpected goal journal fields");
|
||||
r.request.skill=static_cast<Skill>(skill); r.state=static_cast<GoalState>(state);
|
||||
if(code>=0) { ExecutionResult result; result.code=static_cast<ResultCode>(code); result.stop=static_cast<StopState>(stop); for(std::size_t i=0;i<detail_hex.size();i+=2)result.detail.push_back(static_cast<char>(std::stoi(detail_hex.substr(i,2),nullptr,16))); r.result=result; }
|
||||
records_[r.request.goal_id]=r;
|
||||
}
|
||||
if(in.bad()) throw std::runtime_error("goal journal read failed");
|
||||
for(auto& entry:records_) if(entry.second.state!=GoalState::TERMINAL) { entry.second.state=GoalState::STOP_UNKNOWN; entry.second.restarted=true; entry.second.cancel_intent=true; }
|
||||
}
|
||||
std::optional<std::string> ActiveGoalRegistry::start(GoalRequest request,SteadyTime now) {
|
||||
if(journal_failed_||request.robot_id.empty()||!valid_trace(request.trace)||robot_locked(request.robot_id)||!driver_.ready(request.skill)) return {};
|
||||
for(const auto& item:records_) if(same_trace(item.second.request.trace,request.trace)) return {};
|
||||
request.goal_id=uuid(); GoalRecord record; record.request=std::move(request); record.sent_at=now; record.last_feedback=now;
|
||||
auto inserted=records_.emplace(record.request.goal_id,std::move(record)); auto& r=inserted.first->second;
|
||||
append(r); // This happens before transport can observe the request.
|
||||
try { driver_.send(r.request); } catch(...) { r.state=GoalState::STOP_UNKNOWN; r.cancel_intent=true; r.cancel_at=now;
|
||||
try {driver_.cancel(r.request.goal_id);} catch(...) {}
|
||||
append(r); }
|
||||
return r.request.goal_id;
|
||||
}
|
||||
bool ActiveGoalRegistry::robot_locked(const std::string& robot) const {
|
||||
if(journal_failed_) return true;
|
||||
for(const auto& item:records_) if(item.second.request.robot_id==robot&&item.second.state!=GoalState::TERMINAL) return true;
|
||||
return false;
|
||||
}
|
||||
bool ActiveGoalRegistry::has_unresolved() const { if(journal_failed_) return true; for(const auto& item:records_) if(item.second.state!=GoalState::TERMINAL) return true; return false; }
|
||||
const GoalRecord* ActiveGoalRegistry::find(const std::string& id) const { auto it=records_.find(id); return it==records_.end()?nullptr:&it->second; }
|
||||
void ActiveGoalRegistry::request_cancel(const std::string& id,SteadyTime now) {
|
||||
auto it=records_.find(id); if(it==records_.end()) return; auto& r=it->second;
|
||||
if(r.state==GoalState::TERMINAL||r.cancel_intent) return;
|
||||
r.cancel_intent=true; r.cancel_at=now; r.state=GoalState::CANCEL_REQUESTED;
|
||||
// Persist the intent first when possible, but storage failure must never
|
||||
// suppress the already-authorized stop request for an outstanding motion.
|
||||
std::exception_ptr storage_error;
|
||||
try { append(r); } catch(...) { storage_error=std::current_exception();r.state=GoalState::STOP_UNKNOWN; }
|
||||
try { driver_.cancel(id); } catch(...) {
|
||||
r.state=GoalState::STOP_UNKNOWN;
|
||||
if(!storage_error)append(r);
|
||||
}
|
||||
if(storage_error)std::rethrow_exception(storage_error);
|
||||
}
|
||||
void ActiveGoalRegistry::ingest(const GoalEvent& event,SteadyTime now) {
|
||||
auto it=records_.find(event.goal_id); if(it==records_.end()) return; auto& r=it->second;
|
||||
if(!same_trace(r.request.trace,event.trace)||r.state==GoalState::TERMINAL) return;
|
||||
switch(event.kind) {
|
||||
case EventKind::ACCEPTED:
|
||||
if(r.accepted) return;
|
||||
r.accepted=true; r.accepted_at=now; r.last_feedback=now;
|
||||
if(!r.cancel_intent&&!r.restarted) r.state=GoalState::ACTIVE;
|
||||
append(r);
|
||||
if(r.cancel_intent) { try { driver_.cancel(event.goal_id); } catch(...) { r.state=GoalState::STOP_UNKNOWN; append(r); } }
|
||||
break;
|
||||
case EventKind::REJECTED:
|
||||
if(r.accepted||r.result) { r.state=GoalState::STOP_UNKNOWN; append(r); break; }
|
||||
r.state=GoalState::TERMINAL; r.result=ExecutionResult{ResultCode::REJECTED,StopState::CONFIRMED,{},"server rejected before execution"}; append(r); break;
|
||||
case EventKind::FEEDBACK:
|
||||
if(!r.accepted||r.cancel_intent||r.restarted||event.sequence<=r.last_sequence) return;
|
||||
r.last_sequence=event.sequence; r.last_feedback=now; break;
|
||||
case EventKind::CANCEL_ACK: break; // An ACK says nothing about physical stop.
|
||||
case EventKind::RESULT:
|
||||
if(!protocol_matches(event.native_status,event.result.code)||event.result.stop!=StopState::CONFIRMED) {
|
||||
r.state=GoalState::STOP_UNKNOWN; r.result=event.result; append(r);
|
||||
if(!r.cancel_intent) { r.cancel_intent=true; r.cancel_at=now; append(r); try { driver_.cancel(event.goal_id); } catch(...) {} }
|
||||
break;
|
||||
}
|
||||
r.result=event.result; r.state=GoalState::TERMINAL; append(r); break;
|
||||
}
|
||||
}
|
||||
void ActiveGoalRegistry::pump(SteadyTime now) {
|
||||
try {
|
||||
for(auto& item:records_) {
|
||||
auto& r=item.second; if(r.state==GoalState::TERMINAL||r.restarted) continue;
|
||||
if(r.cancel_intent) { if(r.state==GoalState::CANCEL_REQUESTED&&now-r.cancel_at>=budgets_.cancel_stop) { r.state=GoalState::STOP_UNKNOWN; append(r); } continue; }
|
||||
if((!r.accepted&&now-r.sent_at>=budgets_.acceptance)||(r.accepted&&(now-r.last_feedback>=budgets_.feedback||now-r.accepted_at>=budgets_.execution))) request_cancel(item.first,now);
|
||||
}
|
||||
for(const auto& event:driver_.drain_events()) ingest(event,now);
|
||||
} catch(...) {
|
||||
// A callback journal failure or transport exception cannot strand live goals.
|
||||
// Keep all resources quarantined and issue only cancellation, never a resend.
|
||||
for(auto& item:records_) {
|
||||
auto& r=item.second;
|
||||
if(r.state==GoalState::TERMINAL)continue;
|
||||
r.state=GoalState::STOP_UNKNOWN;r.cancel_intent=true;r.cancel_at=now;
|
||||
try { driver_.cancel(item.first); } catch(...) {}
|
||||
}
|
||||
throw;
|
||||
}
|
||||
}
|
||||
bool ActiveGoalRegistry::reconcile(const std::string& id,const Trace& trace,StopState stop,bool authorized,const std::string& evidence) {
|
||||
auto it=records_.find(id);
|
||||
if(!authorized||evidence.empty()||stop!=StopState::CONFIRMED||it==records_.end()||!same_trace(it->second.request.trace,trace)||it->second.state==GoalState::TERMINAL) return false;
|
||||
auto& r=it->second; r.result=ExecutionResult{ResultCode::CANCELED,StopState::CONFIRMED,{},"authorized reconciliation: "+evidence}; r.state=GoalState::TERMINAL; append(r); return true;
|
||||
}
|
||||
void ContextStore::replace_target(TargetBinding b) { std::lock_guard<std::mutex> lock(mutex_); target_=std::move(b); }
|
||||
void ContextStore::replace_placement(PlacementBinding b) { std::lock_guard<std::mutex> lock(mutex_); placement_=std::move(b); }
|
||||
std::optional<TargetBinding> ContextStore::target() const { std::lock_guard<std::mutex> lock(mutex_); return target_; }
|
||||
std::optional<PlacementBinding> ContextStore::placement() const { std::lock_guard<std::mutex> lock(mutex_); return placement_; }
|
||||
void ContextStore::invalidate_geometry() { std::lock_guard<std::mutex> lock(mutex_); target_.reset(); placement_.reset(); }
|
||||
} // namespace robot_bt
|
||||
@@ -0,0 +1,38 @@
|
||||
#include "robot_bt/sim_driver.hpp"
|
||||
#include <limits>
|
||||
#include <stdexcept>
|
||||
#include <utility>
|
||||
namespace robot_bt {
|
||||
SimDriver::SimDriver(std::string scenario):scenario_(std::move(scenario)) {
|
||||
if(scenario_!="happy"&&scenario_!="unknown-grasp"&&scenario_!="adjust"&&scenario_!="wrong-container"&&scenario_!="unknown-verification"&&scenario_!="stop-unknown"&&scenario_!="nan-geometry")throw std::invalid_argument("unknown simulation scenario");
|
||||
}
|
||||
void SimDriver::send(const GoalRequest& q) {
|
||||
if(q.skill==Skill::PICK)sensor_holding_=Holding::HOLDING_TARGET;
|
||||
if(q.skill==Skill::PLACE)sensor_holding_=Holding::EMPTY;
|
||||
requests_[q.goal_id]=q;
|
||||
GoalEvent accepted;accepted.kind=EventKind::ACCEPTED;accepted.goal_id=q.goal_id;accepted.trace=q.trace;events_.push_back(accepted);
|
||||
GoalEvent done=accepted;done.kind=EventKind::RESULT;done.native_status=NativeStatus::SUCCEEDED;done.result.code=ResultCode::COMPLETED;done.result.stop=scenario_=="stop-unknown"?StopState::UNKNOWN:StopState::CONFIRMED;
|
||||
auto& r=done.result.response;r.valid=true;r.target_id=q.target_id;r.destination_id=scenario_=="wrong-container"?"unrequested-container":q.destination_id;r.base_stopped=true;r.verified=scenario_!="unknown-verification";r.in_destination=true;
|
||||
const SnapshotMeta meta{1,q.trace,q.goal_id,"simulated-independent-verifier",q.geometry_epoch,now_,now_+1000000000};r.evidence=meta;
|
||||
Pose pose{"map",0.5,0.2,0.8,0,0,0,1};if(scenario_=="nan-geometry")pose.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,pose};break;
|
||||
case Skill::EVALUATE_GRASP:r.admission=scenario_=="unknown-grasp"?Admission::UNKNOWN:(scenario_=="adjust"&&!adjusted_?Admission::ADJUST_POSTURE:Admission::DIRECT);r.posture_id="registered-small-lift";break;
|
||||
case Skill::ADJUST_POSTURE:adjusted_=true;break;
|
||||
case Skill::VERIFY_PICK:case Skill::VERIFY_TRANSPORT:r.holding=r.verified?Holding::HOLDING_TARGET:Holding::UNKNOWN;break;
|
||||
case Skill::CHECK_FREE_SPACE:r.placement=PlacementBinding{meta,q.target_id,r.destination_id,pose,true};break;
|
||||
case Skill::VERIFY_EMPTY:case Skill::VERIFY_PLACE:r.holding=r.verified?Holding::EMPTY:Holding::UNKNOWN;break;
|
||||
default:break;
|
||||
}
|
||||
events_.push_back(done);
|
||||
}
|
||||
void SimDriver::cancel(const std::string& id) {
|
||||
auto it=requests_.find(id);if(it==requests_.end())return;
|
||||
GoalEvent ack;ack.goal_id=id;ack.trace=it->second.trace;ack.kind=EventKind::CANCEL_ACK;events_.push_back(ack);
|
||||
if(scenario_=="stop-unknown")return;
|
||||
GoalEvent done=ack;done.kind=EventKind::RESULT;done.native_status=NativeStatus::CANCELED;done.result.code=ResultCode::CANCELED;done.result.stop=StopState::CONFIRMED;events_.push_back(done);
|
||||
}
|
||||
std::vector<GoalEvent> SimDriver::drain_events() {std::vector<GoalEvent> result;result.swap(events_);return result;}
|
||||
} // namespace robot_bt
|
||||
@@ -0,0 +1,221 @@
|
||||
#include "robot_bt/core.hpp"
|
||||
#include <algorithm>
|
||||
#include <cmath>
|
||||
#include <stdexcept>
|
||||
#include <utility>
|
||||
namespace robot_bt {
|
||||
namespace {
|
||||
bool evidence_valid(const SkillResponse& response,const GoalRequest& q,RosTime now) {
|
||||
if(!response.evidence) return false;
|
||||
const auto& m=*response.evidence;
|
||||
return m.schema_version==1 && same_trace(m.trace,q.trace) && m.source_goal_id==q.goal_id && !m.writer.empty() && m.geometry_epoch==q.geometry_epoch && m.observed_at>=q.capture_after && m.observed_at>0 && m.observed_at<=now && m.valid_until>now;
|
||||
}
|
||||
bool registered_posture(const SiteConfig& site,const std::string& posture) { return !posture.empty()&&std::find(site.allowed_postures.begin(),site.allowed_postures.end(),posture)!=site.allowed_postures.end(); }
|
||||
}
|
||||
const std::vector<Stage>& fixed_stages() {
|
||||
static const std::vector<Stage> stages={Stage::PREFLIGHT,Stage::NAVIGATE_OBSERVE,Stage::LOCATE_SHELF_COLUMN,Stage::NAVIGATE_SOURCE,Stage::LOCALIZE_TARGET,Stage::PICK,Stage::VERIFY_PICK,Stage::TRANSPORT_POSTURE,Stage::VERIFY_TRANSPORT,Stage::NAVIGATE_DESTINATION,Stage::CHECK_FREE_SPACE,Stage::PLACE,Stage::VERIFY_PLACE,Stage::DELIVER,Stage::CLEANUP};return stages;
|
||||
}
|
||||
const char* stage_name(Stage s) {
|
||||
switch(s) {
|
||||
case Stage::PREFLIGHT:return "Preflight";case Stage::NAVIGATE_OBSERVE:return "NavigateObserve";case Stage::LOCATE_SHELF_COLUMN:return "LocateShelfColumn";case Stage::NAVIGATE_SOURCE:return "NavigateSource";case Stage::LOCALIZE_TARGET:return "LocalizeTarget";case Stage::PICK:return "Pick";case Stage::VERIFY_PICK:return "VerifyPick";case Stage::TRANSPORT_POSTURE:return "TransportPosture";case Stage::VERIFY_TRANSPORT:return "VerifyTransport";case Stage::NAVIGATE_DESTINATION:return "NavigateDestination";case Stage::CHECK_FREE_SPACE:return "CheckFreeSpace";case Stage::PLACE:return "Place";case Stage::VERIFY_PLACE:return "VerifyPlace";case Stage::DELIVER:return "Deliver";case Stage::CLEANUP:return "Cleanup";
|
||||
} return "Invalid";
|
||||
}
|
||||
StageRunner::StageRunner(TaskConfig task,SiteConfig site,GoalDriver& driver,ActiveGoalRegistry& registry,ContextStore& context,Delivery delivery,Budgets budgets):task_(std::move(task)),site_(std::move(site)),driver_(driver),registry_(registry),context_(context),delivery_(std::move(delivery)),budgets_(budgets),geometry_epoch_(task_.initial_geometry_epoch) {
|
||||
if(task_.robot_id.empty()||task_.target_id.empty()||task_.source_shelf.empty()||task_.destination_id.empty()||task_.trace.task_id.empty()||task_.trace.run_id.empty()||task_.trace.task_revision==0||task_.trace.plan_version==0||task_.trace.execution_generation==0||!delivery_) throw std::invalid_argument("complete task and delivery callback required");
|
||||
if((task_.route!="LEGACY"&&task_.route!="OBJECT_TABLE"&&task_.route!="SHELF_CELL")||task_.item_index>=20)throw std::invalid_argument("invalid route/item index");
|
||||
if(task_.max_reobservations>2||task_.max_posture_adjustments>1||!std::isfinite(task_.position_tolerance_m)||!std::isfinite(task_.orientation_tolerance_rad)||task_.position_tolerance_m<=0||task_.orientation_tolerance_rad<=0||task_.orientation_tolerance_rad>std::acos(-1.0)||budgets_.readiness.count()<=0) throw std::invalid_argument("unsafe task budgets/tolerances");
|
||||
}
|
||||
bool StageRunner::safe(RosTime now) const { return now>0&&safety_.safe&&safety_.observed_at>0&&safety_.observed_at<=now&&safety_.valid_until>now; }
|
||||
TickStatus StageRunner::fail(std::string detail,bool intervention) { detail_=std::move(detail);failure_=intervention?TickStatus::INTERVENTION_REQUIRED:TickStatus::FAILURE;return *failure_; }
|
||||
void StageRunner::halt(SteadyTime now) { halted_=true;if(!active_goal_.empty())registry_.request_cancel(active_goal_,now); }
|
||||
TickStatus StageRunner::settle(SteadyTime now,RosTime ros) {
|
||||
if(!halted_)return TickStatus::INTERVENTION_REQUIRED;
|
||||
if(last_ros_time_>0&&ros<last_ros_time_){empty_valid_until_=0;holding_valid_until_=0;context_.invalidate_geometry();settlement_failure_=TickStatus::INTERVENTION_REQUIRED;return *settlement_failure_;}
|
||||
last_ros_time_=ros;
|
||||
if(settlement_failure_)return *settlement_failure_;
|
||||
if(settlement_done_)return empty_verified(ros)?TickStatus::SUCCESS:TickStatus::INTERVENTION_REQUIRED;
|
||||
try { registry_.pump(now); } catch(const std::exception& e) {detail_=e.what();return TickStatus::INTERVENTION_REQUIRED;}
|
||||
for(const auto& entry:registry_.records())if(entry.second.request.robot_id==task_.robot_id&&entry.second.state==GoalState::STOP_UNKNOWN)return TickStatus::INTERVENTION_REQUIRED;
|
||||
if(!settlement_started_) {
|
||||
if(registry_.robot_locked(task_.robot_id))return TickStatus::RUNNING;
|
||||
if(!safe(ros)||!safety_.stationary)return TickStatus::INTERVENTION_REQUIRED;
|
||||
if(delivered_&&empty_verified(ros)){settlement_done_=true;return TickStatus::SUCCESS;}
|
||||
for(const auto& entry:registry_.records()) {
|
||||
const auto& q=entry.second.request;
|
||||
if(q.trace.task_id==task_.trace.task_id&&q.trace.run_id==task_.trace.run_id&&q.skill==Skill::PLACE)settlement_place_=true;
|
||||
}
|
||||
settlement_started_=true;active_goal_.clear();waiting_since_.reset();capture_after_=ros;
|
||||
}
|
||||
if(!safe(ros)||!safety_.stationary){settlement_failure_=TickStatus::INTERVENTION_REQUIRED;return *settlement_failure_;}
|
||||
SkillResponse response;std::string proof;
|
||||
const auto status=run_goal(Stage::CLEANUP,settlement_place_?Skill::VERIFY_PLACE:Skill::VERIFY_EMPTY,now,ros,response,proof);
|
||||
if(status==TickStatus::RUNNING)return status;
|
||||
if(status!=TickStatus::SUCCESS||!response.verified||!response.base_stopped||response.holding!=Holding::EMPTY||
|
||||
(settlement_place_&&(response.target_id!=task_.target_id||response.destination_id!=task_.destination_id||!response.in_destination))) {
|
||||
settlement_failure_=TickStatus::INTERVENTION_REQUIRED;return *settlement_failure_;
|
||||
}
|
||||
if(settlement_place_&&!delivered_) {
|
||||
try {if(!delivery_(task_.trace.task_id,task_.item_index,proof)){settlement_failure_=TickStatus::INTERVENTION_REQUIRED;return *settlement_failure_;}}
|
||||
catch(const std::exception& e){detail_=e.what();settlement_failure_=TickStatus::INTERVENTION_REQUIRED;return *settlement_failure_;}
|
||||
delivered_=true;
|
||||
}
|
||||
holding_=Holding::EMPTY;empty_valid_until_=response.evidence->valid_until;empty_observed_at_=response.evidence->observed_at;settlement_done_=true;return TickStatus::SUCCESS;
|
||||
}
|
||||
GoalRequest StageRunner::make_request(Stage stage,Skill skill,RosTime ros) {
|
||||
GoalRequest q;q.robot_id=task_.robot_id;q.trace=task_.trace;q.trace.subtask_id=std::string(stage_name(stage))+"/"+std::to_string(++serial_);q.trace.attempt=1;q.skill=skill;q.target_id=task_.target_id;q.destination_id=task_.destination_id;q.shelf=task_.source_shelf;q.geometry_epoch=geometry_epoch_;q.capture_after=std::max(capture_after_,ros);q.position_tolerance_m=task_.position_tolerance_m;q.orientation_tolerance_rad=task_.orientation_tolerance_rad;
|
||||
if(skill==Skill::NAVIGATE) { const auto& location=stage==Stage::NAVIGATE_OBSERVE?task_.observe_location:stage==Stage::NAVIGATE_SOURCE?source_location_:task_.destination_location;auto it=site_.locations.find(location);if(it==site_.locations.end()||!valid_pose(it->second))throw std::invalid_argument("navigation location is not registered with a valid pose");q.registered_pose=it->second;
|
||||
if(task_.route!="LEGACY") {
|
||||
q.navigation_kind="LOCATION";q.navigation_ref=location;
|
||||
if(stage==Stage::NAVIGATE_SOURCE){q.navigation_kind=task_.route=="OBJECT_TABLE"?"OBJECT":"CELL";q.navigation_ref=task_.target_id;q.side=source_side_;q.column=source_column_;q.tier=source_tier_;}
|
||||
}
|
||||
}
|
||||
if(task_.route=="LEGACY"&&(skill==Skill::EVALUATE_GRASP||skill==Skill::PICK)) { q.target=context_.target();if(!q.target||!valid_target(*q.target,task_.trace,task_.target_id,geometry_epoch_,capture_after_,ros))throw std::invalid_argument("target binding stale, incomplete or mismatched"); }
|
||||
if(task_.route=="LEGACY"&&skill==Skill::PLACE) { q.placement=context_.placement();if(!q.placement||!valid_placement(*q.placement,task_.trace,task_.target_id,task_.destination_id,geometry_epoch_,capture_after_,ros))throw std::invalid_argument("placement binding stale, incomplete or mismatched"); }
|
||||
if(skill==Skill::ADJUST_POSTURE||skill==Skill::TRANSPORT_POSTURE) { q.posture_id=skill==Skill::ADJUST_POSTURE?pending_posture_:site_.transport_posture;if(!registered_posture(site_,q.posture_id))throw std::invalid_argument("posture is not registered"); }
|
||||
return q;
|
||||
}
|
||||
TickStatus StageRunner::run_goal(Stage stage,Skill skill,SteadyTime now,RosTime ros,SkillResponse& response,std::string& completed_goal) {
|
||||
const bool needs_empty=skill==Skill::PICK||skill==Skill::ADJUST_POSTURE||(skill==Skill::NAVIGATE&&stage!=Stage::NAVIGATE_DESTINATION);
|
||||
if(needs_empty&&(active_goal_.empty()||empty_refresh_started_)) {
|
||||
if(!driver_.ready(skill)) {
|
||||
if(!motion_waiting_since_)motion_waiting_since_=now;
|
||||
if(now-*motion_waiting_since_>=budgets_.readiness)return fail("motion readiness deadline exceeded during empty-hand refresh",false);
|
||||
} else motion_waiting_since_.reset();
|
||||
}
|
||||
if(needs_empty&&((active_goal_.empty()&&!empty_verified(ros))||empty_refresh_started_)) {
|
||||
empty_refresh_started_=true;SkillResponse proof;std::string proof_id;
|
||||
const auto refreshed=run_goal(stage,Skill::VERIFY_EMPTY,now,ros,proof,proof_id);
|
||||
if(refreshed!=TickStatus::SUCCESS)return refreshed;
|
||||
if(!proof.verified||proof.holding!=Holding::EMPTY||!proof.base_stopped)return fail("fresh independent empty-hand refresh failed");
|
||||
holding_=Holding::EMPTY;empty_valid_until_=proof.evidence->valid_until;empty_observed_at_=proof.evidence->observed_at;
|
||||
empty_refresh_started_=false;return TickStatus::RUNNING;
|
||||
}
|
||||
if(active_goal_.empty()) {
|
||||
if(registry_.robot_locked(task_.robot_id))return fail("unresolved goal retains robot resource");
|
||||
if(!safety_.stationary)return fail("fresh stationary evidence required before dispatch");
|
||||
const bool carrying_motion=skill==Skill::TRANSPORT_POSTURE||skill==Skill::PLACE||(skill==Skill::NAVIGATE&&stage==Stage::NAVIGATE_DESTINATION);
|
||||
const bool empty_motion=skill==Skill::PICK||skill==Skill::ADJUST_POSTURE||(skill==Skill::NAVIGATE&&stage!=Stage::NAVIGATE_DESTINATION);
|
||||
if(carrying_motion&&holding_valid_until_<=ros){holding_=Holding::UNKNOWN;return fail("independent held-target verification expired before motion dispatch");}
|
||||
if(carrying_motion&&safety_.holding!=Holding::HOLDING_TARGET){holding_=Holding::UNKNOWN;return fail("fresh holding state contradicts verified target; motion blocked");}
|
||||
if(empty_motion&&safety_.holding!=Holding::EMPTY){holding_=Holding::UNKNOWN;return fail("fresh empty-hand evidence required for motion");}
|
||||
if(!driver_.ready(skill)) { if(!waiting_since_)waiting_since_=now;if(now-*waiting_since_>=budgets_.readiness)return fail("skill readiness deadline exceeded",false);return TickStatus::RUNNING; }
|
||||
waiting_since_.reset();
|
||||
try { auto request=make_request(stage,skill,ros);if(skill==Skill::PICK||skill==Skill::PLACE){holding_=Holding::UNKNOWN;holding_valid_until_=0;empty_valid_until_=0;}auto id=registry_.start(std::move(request),now);if(!id)return fail("goal admission refused");active_goal_=*id; }
|
||||
catch(const std::exception& error) { return fail(error.what()); }
|
||||
return TickStatus::RUNNING;
|
||||
}
|
||||
const auto* record=registry_.find(active_goal_);if(!record)return fail("active goal missing from registry");
|
||||
if(record->state==GoalState::STOP_UNKNOWN)return fail("physical stop unknown; robot quarantined");
|
||||
if(record->state!=GoalState::TERMINAL)return TickStatus::RUNNING;
|
||||
if(record->cancel_intent||!record->result||record->result->code!=ResultCode::COMPLETED||record->result->stop!=StopState::CONFIRMED)return fail("goal failed/canceled/timed out; automatic motion retry disabled",false);
|
||||
response=record->result->response;completed_goal=active_goal_;
|
||||
if(!response.valid)return fail("typed skill response invalid or absent");
|
||||
if(skill==Skill::NAVIGATE&&(!response.base_stopped||!response.final_pose||!record->request.registered_pose||!within_tolerance(*response.final_pose,*record->request.registered_pose,task_.position_tolerance_m,task_.orientation_tolerance_rad)))return fail("navigation stopped pose outside exact tolerance or unknown");
|
||||
if((skill==Skill::VERIFY_PICK||skill==Skill::VERIFY_TRANSPORT||skill==Skill::VERIFY_PLACE||skill==Skill::VERIFY_EMPTY)&&!evidence_valid(response,record->request,ros))return fail("verification evidence stale or mismatched");
|
||||
if(skill==Skill::LOCALIZE_TARGET&&(!response.target||response.target->meta.source_goal_id!=active_goal_||!valid_target(*response.target,task_.trace,task_.target_id,geometry_epoch_,record->request.capture_after,ros)))return fail("invalid localization snapshot");
|
||||
if(skill==Skill::CHECK_FREE_SPACE&&(!response.placement||response.placement->meta.source_goal_id!=active_goal_||!valid_placement(*response.placement,task_.trace,task_.target_id,task_.destination_id,geometry_epoch_,record->request.capture_after,ros)))return fail("invalid free-space snapshot or wrong container");
|
||||
active_goal_.clear();return TickStatus::SUCCESS;
|
||||
}
|
||||
TickStatus StageRunner::tick(Stage stage,SteadyTime now,RosTime ros) {
|
||||
try { registry_.pump(now); } catch(const std::exception& error) { return fail(error.what()); }
|
||||
if(failure_)return *failure_;
|
||||
if(halted_)return fail("execution halted; resume requires fresh physical reconciliation");
|
||||
if(last_ros_time_>0&&ros<last_ros_time_) {context_.invalidate_geometry();if(!active_goal_.empty())registry_.request_cancel(active_goal_,now);return fail("ROS clock moved backwards; observations invalidated");}
|
||||
last_ros_time_=ros;
|
||||
if((stage==Stage::NAVIGATE_DESTINATION||stage==Stage::TRANSPORT_POSTURE)&&!active_goal_.empty()&&safety_.holding!=Holding::HOLDING_TARGET){holding_=Holding::UNKNOWN;registry_.request_cancel(active_goal_,now);return fail("holding became uncertain during carrying motion");}
|
||||
if(!safe(ros)) {if(!active_goal_.empty())registry_.request_cancel(active_goal_,now);return fail("robot safety state unsafe or stale");}
|
||||
const auto& stages=fixed_stages();auto found=std::find(stages.begin(),stages.end(),stage);if(found==stages.end())return fail("unknown fixed stage");const auto index=static_cast<std::size_t>(found-stages.begin());
|
||||
if(index<stage_index_)return TickStatus::SUCCESS;
|
||||
if(index!=stage_index_)return fail("fixed workflow stage order violated");
|
||||
SkillResponse response;std::string completed;TickStatus result=TickStatus::RUNNING;
|
||||
auto invoke=[&](Skill skill){return run_goal(stage,skill,now,ros,response,completed);};
|
||||
auto geometry_changed=[&](){++geometry_epoch_;capture_after_=ros;context_.invalidate_geometry();};
|
||||
switch(stage) {
|
||||
case Stage::PREFLIGHT:
|
||||
if(active_goal_.empty()&&(registry_.robot_locked(task_.robot_id)||!safety_.stationary))return fail("preflight needs idle robot and fresh stationary evidence");
|
||||
result=invoke(Skill::VERIFY_EMPTY);if(result!=TickStatus::SUCCESS)return result;
|
||||
if(!response.verified||response.holding!=Holding::EMPTY||!response.base_stopped)return fail("independent preflight empty-hand verification missing");
|
||||
empty_valid_until_=response.evidence->valid_until;empty_observed_at_=response.evidence->observed_at;
|
||||
if(task_.route=="OBJECT_TABLE") {
|
||||
auto loc=site_.object_locations.find(task_.target_id),posture=site_.object_postures.find(task_.target_id);
|
||||
if(loc==site_.object_locations.end()||posture==site_.object_postures.end()||!registered_posture(site_,posture->second))return fail("object table missing location/posture");
|
||||
source_location_=loc->second;pending_posture_=posture->second;
|
||||
}
|
||||
holding_=Holding::EMPTY;capture_after_=ros;result=TickStatus::SUCCESS;break;
|
||||
case Stage::NAVIGATE_OBSERVE:case Stage::NAVIGATE_SOURCE:case Stage::NAVIGATE_DESTINATION:
|
||||
if(stage==Stage::NAVIGATE_OBSERVE&&task_.route=="OBJECT_TABLE"){result=TickStatus::SUCCESS;break;}
|
||||
if(stage==Stage::NAVIGATE_DESTINATION&&holding_!=Holding::HOLDING_TARGET)return fail("carrying target has not been verified");
|
||||
result=invoke(Skill::NAVIGATE);if(result==TickStatus::SUCCESS)geometry_changed();break;
|
||||
case Stage::LOCATE_SHELF_COLUMN:
|
||||
if(task_.route=="OBJECT_TABLE"){result=TickStatus::SUCCESS;break;}
|
||||
result=invoke(Skill::LOCATE_SHELF_COLUMN);if(result==TickStatus::SUCCESS) {
|
||||
if(response.shelf!=task_.source_shelf||response.side.empty()||response.column.empty())return fail("shelf observation invalid");
|
||||
auto key=response.shelf+"/"+response.side+"/"+response.column;
|
||||
if(task_.route=="SHELF_CELL") {
|
||||
if(response.tier.empty())return fail("tier required for calibrated shelf route");
|
||||
key+="/"+response.tier;auto loc=site_.cell_locations.find(key),posture=site_.cell_postures.find(key);
|
||||
if(loc==site_.cell_locations.end()||posture==site_.cell_postures.end()||!registered_posture(site_,posture->second))return fail("cell lacks calibrated location/posture");
|
||||
source_location_=loc->second;pending_posture_=posture->second;source_side_=response.side;source_column_=response.column;source_tier_=response.tier;
|
||||
}else {auto it=site_.parking_locations.find(key);if(it==site_.parking_locations.end())return fail("observed shelf column has no registered parking pose");source_location_=it->second;}
|
||||
}break;
|
||||
case Stage::LOCALIZE_TARGET:
|
||||
if(task_.route!="LEGACY") {
|
||||
result=invoke(Skill::ADJUST_POSTURE);if(result==TickStatus::SUCCESS){if(!response.base_stopped)return fail("registered source posture stop unknown");geometry_changed();}break;
|
||||
}
|
||||
result=invoke(Skill::LOCALIZE_TARGET);if(result==TickStatus::SUCCESS)context_.replace_target(*response.target);break;
|
||||
case Stage::PICK:
|
||||
if(task_.route!="LEGACY")pick_phase_=PickPhase::EXECUTE;
|
||||
if(holding_!=Holding::EMPTY&&!(pick_phase_==PickPhase::EXECUTE&&!active_goal_.empty()))return fail("pick requires verified empty hand");
|
||||
if(pick_phase_==PickPhase::ASSESS) {
|
||||
result=invoke(Skill::EVALUATE_GRASP);if(result!=TickStatus::SUCCESS)return result;
|
||||
switch(response.admission) {
|
||||
case Admission::DIRECT:pick_phase_=PickPhase::EXECUTE;break;
|
||||
case Admission::UNKNOWN:if(reobservations_>=task_.max_reobservations)return fail("grasp admission UNKNOWN after bounded reobservation");++reobservations_;capture_after_=ros;pick_phase_=PickPhase::REOBSERVE;break;
|
||||
case Admission::ADJUST_POSTURE:if(adjustments_>=task_.max_posture_adjustments||!registered_posture(site_,response.posture_id))return fail("grasp posture adjustment unregistered or exhausted");++adjustments_;pending_posture_=response.posture_id;pick_phase_=PickPhase::ADJUST;break;
|
||||
case Admission::NOT_REACHABLE:return fail("target not reachable; operator intervention required");
|
||||
default:return fail("invalid grasp admission enum");
|
||||
}
|
||||
return TickStatus::RUNNING;
|
||||
}
|
||||
if(pick_phase_==PickPhase::REOBSERVE) {result=invoke(Skill::LOCALIZE_TARGET);if(result!=TickStatus::SUCCESS)return result;context_.replace_target(*response.target);pick_phase_=PickPhase::ASSESS;return TickStatus::RUNNING;}
|
||||
if(pick_phase_==PickPhase::ADJUST) {result=invoke(Skill::ADJUST_POSTURE);if(result!=TickStatus::SUCCESS)return result;if(!response.base_stopped)return fail("posture stop evidence missing");geometry_changed();pick_phase_=PickPhase::REOBSERVE;return TickStatus::RUNNING;}
|
||||
result=invoke(Skill::PICK);if(result==TickStatus::SUCCESS)holding_=Holding::UNKNOWN;break;
|
||||
case Stage::VERIFY_PICK:
|
||||
result=invoke(Skill::VERIFY_PICK);if(result==TickStatus::SUCCESS) {if(!response.verified||response.target_id!=task_.target_id||response.holding!=Holding::HOLDING_TARGET||!response.base_stopped)return fail("independent pick verification not confirmed");holding_=Holding::HOLDING_TARGET;holding_valid_until_=response.evidence->valid_until;}break;
|
||||
case Stage::TRANSPORT_POSTURE:
|
||||
if(holding_!=Holding::HOLDING_TARGET)return fail("transport posture requires verified held target");
|
||||
result=invoke(Skill::TRANSPORT_POSTURE);if(result==TickStatus::SUCCESS) {if(!response.base_stopped)return fail("transport posture stop unconfirmed");geometry_changed();}break;
|
||||
case Stage::VERIFY_TRANSPORT:
|
||||
result=invoke(Skill::VERIFY_TRANSPORT);if(result==TickStatus::SUCCESS){if(!response.verified||response.holding!=Holding::HOLDING_TARGET||response.target_id!=task_.target_id||!response.base_stopped)return fail("independent transport verification not confirmed");holding_valid_until_=response.evidence->valid_until;}break;
|
||||
case Stage::CHECK_FREE_SPACE:
|
||||
if(task_.route!="LEGACY"){result=TickStatus::SUCCESS;break;}
|
||||
result=invoke(Skill::CHECK_FREE_SPACE);if(result==TickStatus::SUCCESS)context_.replace_placement(*response.placement);break;
|
||||
case Stage::PLACE:
|
||||
if(holding_!=Holding::HOLDING_TARGET&&active_goal_.empty())return fail("place requires verified held target");
|
||||
if(active_goal_.empty()&&holding_valid_until_<=ros&&!place_refresh_done_)place_refresh_started_=true;
|
||||
if(place_refresh_started_){
|
||||
result=invoke(Skill::VERIFY_TRANSPORT);if(result!=TickStatus::SUCCESS)return result;
|
||||
if(!response.verified||response.holding!=Holding::HOLDING_TARGET||response.target_id!=task_.target_id||!response.base_stopped)return fail("held-target refresh before place not confirmed");
|
||||
holding_valid_until_=response.evidence->valid_until;place_refresh_started_=false;place_refresh_done_=true;return TickStatus::RUNNING;
|
||||
}
|
||||
result=invoke(Skill::PLACE);if(result==TickStatus::SUCCESS)holding_=Holding::UNKNOWN;break;
|
||||
case Stage::VERIFY_PLACE:
|
||||
result=invoke(Skill::VERIFY_PLACE);if(result==TickStatus::SUCCESS) {if(!response.verified||response.target_id!=task_.target_id||response.destination_id!=task_.destination_id||response.holding!=Holding::EMPTY||!response.in_destination||!response.base_stopped)return fail("independent place verification did not prove target/container/empty-hand/in-box");holding_=Holding::EMPTY;empty_valid_until_=response.evidence->valid_until;empty_observed_at_=response.evidence->observed_at;verify_place_ok_=true;verification_goal_=completed;}break;
|
||||
case Stage::DELIVER:
|
||||
if(!verify_place_ok_||holding_!=Holding::EMPTY||verification_goal_.empty())return fail("delivery requires independent placement evidence");
|
||||
if(!delivered_) {try {if(!delivery_(task_.trace.task_id,task_.item_index,verification_goal_))return fail("durable delivery transaction not confirmed");}catch(const std::exception& error){return fail(error.what());}delivered_=true;}result=TickStatus::SUCCESS;break;
|
||||
case Stage::CLEANUP:
|
||||
if((active_goal_.empty()&®istry_.robot_locked(task_.robot_id))||holding_!=Holding::EMPTY||!delivered_)return fail("cleanup invariant failed");
|
||||
result=invoke(Skill::VERIFY_EMPTY);if(result!=TickStatus::SUCCESS)return result;
|
||||
if(!response.verified||response.holding!=Holding::EMPTY||!response.base_stopped)return fail("independent final empty-hand verification missing");
|
||||
empty_valid_until_=response.evidence->valid_until;empty_observed_at_=response.evidence->observed_at;break;
|
||||
}
|
||||
if(result==TickStatus::SUCCESS)++stage_index_;
|
||||
return result;
|
||||
}
|
||||
TickStatus Workflow::tick(SteadyTime now,RosTime ros) {
|
||||
const auto& stages=fixed_stages();if(index_>=stages.size())return TickStatus::SUCCESS;
|
||||
auto result=runner_.tick(stages[index_],now,ros);if(result==TickStatus::SUCCESS){++index_;return index_==stages.size()?TickStatus::SUCCESS:TickStatus::RUNNING;}return result;
|
||||
}
|
||||
Stage Workflow::current_stage() const {const auto& stages=fixed_stages();return stages[std::min(index_,stages.size()-1)];}
|
||||
} // namespace robot_bt
|
||||
@@ -0,0 +1,105 @@
|
||||
#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";
|
||||
}
|
||||
@@ -0,0 +1,30 @@
|
||||
#include "robot_bt/core.hpp"
|
||||
#include <cassert>
|
||||
#include <cstdlib>
|
||||
#include <filesystem>
|
||||
#include <iostream>
|
||||
using namespace robot_bt;
|
||||
struct FaultDriver final : GoalDriver {
|
||||
unsigned sent{0}, canceled{0};
|
||||
bool ready(Skill) const override { return true; }
|
||||
void send(const GoalRequest&) override { ++sent; }
|
||||
void cancel(const std::string&) override { ++canceled; }
|
||||
std::vector<GoalEvent> drain_events() override { return {}; }
|
||||
};
|
||||
int main() {
|
||||
char name[]="/tmp/bt_journal_failure_XXXXXX";
|
||||
const char* directory=::mkdtemp(name); assert(directory);
|
||||
std::string path=std::string(directory)+"/goals";
|
||||
FaultDriver driver;
|
||||
ActiveGoalRegistry registry(driver,path);
|
||||
GoalRequest q;q.robot_id="robot";q.trace={"task","pick","run",1,1,1,1};
|
||||
const auto id=registry.start(q,SteadyTime{});assert(id&&driver.sent==1);
|
||||
// Simulate an unavailable journal after the motion request has been sent.
|
||||
std::filesystem::remove(path);std::filesystem::create_directory(path);
|
||||
try { registry.request_cancel(*id,SteadyTime{}); } catch(const std::exception&) {}
|
||||
assert(driver.canceled==1 && "journal failure must never suppress a stop request");
|
||||
assert(registry.robot_locked("robot"));
|
||||
q.trace.attempt=2;assert(!registry.start(q,SteadyTime{}));
|
||||
std::filesystem::remove_all(directory);
|
||||
std::cout<<"journal fault preserves cancel and quarantine\n";
|
||||
}
|
||||
@@ -0,0 +1,62 @@
|
||||
#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";
|
||||
}
|
||||
@@ -0,0 +1,14 @@
|
||||
#include "workflow_fixture.hpp"
|
||||
#include <cmath>
|
||||
int main() {
|
||||
const Pose flat{"map",0,0,0,0,0,0,1};
|
||||
const Pose tilted{"map",0,0,0.3,0.1,0,0,std::sqrt(.99)};
|
||||
assert(within_tolerance(tilted,flat,.01,.01) && "navigation tolerance is XY/yaw per DR");
|
||||
Fixture f("preflight_independent");
|
||||
f.driver.unknown_verification=true;
|
||||
auto runner=f.runner();
|
||||
const auto result=f.execute(runner);
|
||||
assert(result==TickStatus::INTERVENTION_REQUIRED);
|
||||
assert(f.count(Skill::NAVIGATE)==0 && "default sensor EMPTY must not authorize first motion");
|
||||
std::cout<<"independent preflight gates first motion\n";
|
||||
}
|
||||
@@ -0,0 +1,8 @@
|
||||
#include "workflow_fixture.hpp"
|
||||
#include <stdexcept>
|
||||
struct ThrowWire: GoalDriver {unsigned sends=0,cancels=0;bool ready(Skill)const override{return true;}void send(const GoalRequest&)override{++sends;throw std::runtime_error("after-send exception");}void cancel(const std::string&)override{++cancels;}std::vector<GoalEvent> drain_events()override{return {};}};
|
||||
int main(){
|
||||
ThrowWire wire;ActiveGoalRegistry registry(wire,Fixture::test_root()+"/throw.journal");GoalRequest q;q.robot_id="r";q.trace={"t","s","run",1,1,1,1};auto id=registry.start(q,SteadyTime{});registry.request_cancel(*id,SteadyTime{});registry.pump(SteadyTime{}+Milliseconds(9000));assert(wire.cancels>=1);std::cout<<"send-throw sends="<<wire.sends<<" cancels="<<wire.cancels<<" locked="<<registry.robot_locked("r")<<"\n";
|
||||
Fixture f("clock");auto runner=f.runner();assert(f.execute(runner)==TickStatus::SUCCESS);auto before=f.driver.now;runner.halt(SteadyTime{});runner.update_safety({true,true,Holding::EMPTY,1,1000000000});auto status=runner.settle(SteadyTime{},1);assert(status==TickStatus::INTERVENTION_REQUIRED);assert(!runner.empty_verified(1));std::cout<<"clock rollback from="<<before<<" to=1 settle_success="<<(status==TickStatus::SUCCESS)<<" empty_verified="<<runner.empty_verified(1)<<"\n";
|
||||
Fixture e("expiry");auto er=e.runner();er.update_safety({true,true,Holding::EMPTY,1000000,9000000000});assert(er.tick(Stage::PREFLIGHT,SteadyTime{},1000000)==TickStatus::RUNNING);assert(er.tick(Stage::PREFLIGHT,SteadyTime{}+Milliseconds(1),2000000)==TickStatus::SUCCESS);e.driver.now=2000000000;er.update_safety({true,true,Holding::EMPTY,e.driver.now,e.driver.now+1000000000});bool proof=er.empty_verified(e.driver.now);auto nav=er.tick(Stage::NAVIGATE_OBSERVE,SteadyTime{}+Milliseconds(2),e.driver.now);assert(e.count(Skill::NAVIGATE)==0);assert(nav==TickStatus::RUNNING);std::cout<<"expired preflight empty_verified="<<proof<<" nav_count="<<e.count(Skill::NAVIGATE)<<" status_running="<<(nav==TickStatus::RUNNING)<<"\n";
|
||||
}
|
||||
@@ -0,0 +1,2 @@
|
||||
#include "workflow_fixture.hpp"
|
||||
int main(){Fixture f("refresh_readiness");auto r=f.runner();r.update_safety({true,true,Holding::EMPTY,1000000,9000000000});assert(r.tick(Stage::PREFLIGHT,SteadyTime{},1000000)==TickStatus::RUNNING);assert(r.tick(Stage::PREFLIGHT,SteadyTime{}+Milliseconds(1),2000000)==TickStatus::SUCCESS);f.driver.unavailable_skill=Skill::NAVIGATE;TickStatus status=TickStatus::RUNNING;for(int i=1;i<=60&&status==TickStatus::RUNNING;++i){f.driver.now=2000000+i*100000000LL;r.update_safety({true,true,Holding::EMPTY,f.driver.now,f.driver.now+1000000000});status=r.tick(Stage::NAVIGATE_OBSERVE,SteadyTime{}+Milliseconds(i*100),f.driver.now);}assert(status==TickStatus::FAILURE);assert(f.count(Skill::NAVIGATE)==0);std::cout<<"after 6000ms unavailable navigation: still_running="<<(status==TickStatus::RUNNING)<<" verify_empty_calls="<<f.count(Skill::VERIFY_EMPTY)<<" nav_calls="<<f.count(Skill::NAVIGATE)<<"\n";}
|
||||
@@ -0,0 +1,102 @@
|
||||
#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";
|
||||
}
|
||||
@@ -0,0 +1,23 @@
|
||||
#include "workflow_fixture.hpp"
|
||||
struct FailedPlaceDriver : Simulator {
|
||||
void send(const GoalRequest& q) override {
|
||||
Simulator::send(q);
|
||||
if(q.skill==Skill::PLACE){auto& event=events.back();event.native_status=NativeStatus::ABORTED;event.result.code=ResultCode::FAILED;}
|
||||
}
|
||||
};
|
||||
int main(){
|
||||
Fixture config("settlement_config");FailedPlaceDriver driver;ContextStore context;
|
||||
ActiveGoalRegistry registry(driver,Fixture::test_root()+"/settlement.journal");unsigned deliveries=0;
|
||||
StageRunner runner(config.task,config.site,driver,registry,context,[&](const std::string&,unsigned,const std::string&){++deliveries;return true;});
|
||||
Workflow flow(runner);TickStatus status=TickStatus::RUNNING;unsigned i=0;
|
||||
for(;i<200&&status==TickStatus::RUNNING;++i){driver.now=1000000+i*1000000;runner.update_safety({true,true,driver.sensor_holding,driver.now,driver.now+1000000000});status=flow.tick(SteadyTime{}+Milliseconds(i),driver.now);}
|
||||
assert(status==TickStatus::FAILURE);assert(deliveries==0);
|
||||
runner.halt(SteadyTime{}+Milliseconds(i));
|
||||
// Settlement is read-only except for the idempotent business receipt.
|
||||
for(status=TickStatus::RUNNING;i<300&&status==TickStatus::RUNNING;++i){driver.now=1000000+i*1000000;runner.update_safety({true,true,driver.sensor_holding,driver.now,driver.now+1000000000});status=runner.settle(SteadyTime{}+Milliseconds(i),driver.now);}
|
||||
assert(status==TickStatus::SUCCESS);assert(deliveries==1);
|
||||
unsigned place=0,verify=0;for(const auto&q:driver.sent){place+=q.skill==Skill::PLACE;verify+=q.skill==Skill::VERIFY_PLACE;}
|
||||
assert(place==1&&verify==1);assert(runner.empty_verified(driver.now));
|
||||
assert(runner.settle(SteadyTime{}+Milliseconds(i),driver.now)==TickStatus::SUCCESS);assert(deliveries==1);
|
||||
std::cout<<"failed place settlement records verified delivery without motion retry\n";
|
||||
}
|
||||
@@ -0,0 +1,51 @@
|
||||
#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;}
|
||||
};
|
||||
@@ -0,0 +1,51 @@
|
||||
#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";}
|
||||
Reference in New Issue
Block a user