wangfeiyu/behavior-tree
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

fix: harden task recovery and DR contract handling

Browse Source
This commit is contained in:
wangfeiyu
2026-09-20 13:36:48 +08:00
parent 492676344a
commit f9d8feb6f0
49 changed files with 2083 additions and 165 deletions
Download Patch File Download Diff File Expand all files Collapse all files
core/include/robot_bt/core.hpp
+24 -3
View File
@@ -1,5 +1,6 @@
#pragma once
#include <chrono>
#include <deque>
#include <cstdint>
#include <functional>
#include <map>
@@ -50,7 +51,7 @@ struct SkillResponse {
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 ExecutionResult { ResultCode code{ResultCode::FAILED}; StopState stop{StopState::UNKNOWN}; SkillResponse response; std::string detail; std::string error_code{}, execution_record_ref{}, wire_result_type{}, wire_result_snapshot{}; };
struct GoalRequest {
std::string goal_id, robot_id;
Trace trace;
@@ -63,6 +64,7 @@ struct GoalRequest {
std::uint64_t geometry_epoch{0};
RosTime capture_after{0};
double position_tolerance_m{0.05}, orientation_tolerance_rad{0.1};
std::string wire_request_type{}, wire_request_snapshot{};
};
enum class EventKind { ACCEPTED, REJECTED, FEEDBACK, CANCEL_ACK, RESULT };
struct GoalEvent {
@@ -72,6 +74,7 @@ struct GoalEvent {
std::uint64_t sequence{0};
NativeStatus native_status{NativeStatus::UNKNOWN};
ExecutionResult result;
std::string feedback_snapshot{};
};
class GoalDriver {
public:
@@ -81,6 +84,14 @@ class GoalDriver {
virtual void send(const GoalRequest&) = 0;
virtual void cancel(const std::string& goal_id) = 0;
virtual std::vector<GoalEvent> drain_events() = 0;
using RequestRecorder = std::function<void(const std::string&, const std::string&, const std::string&)>;
void set_request_recorder(RequestRecorder recorder) { request_recorder_=std::move(recorder); }
protected:
void record_wire_request(const std::string& id,const std::string& type,const std::string& snapshot) {
if(request_recorder_)request_recorder_(id,type,snapshot);
}
private:
RequestRecorder request_recorder_;
};
struct Budgets { Milliseconds readiness{2000}, acceptance{2000}, feedback{5000}, execution{120000}, cancel_stop{5000}; };
enum class GoalState { SENDING, ACTIVE, CANCEL_REQUESTED, STOP_UNKNOWN, TERMINAL };
@@ -91,6 +102,7 @@ struct GoalRecord {
std::uint64_t last_sequence{0};
SteadyTime sent_at{}, accepted_at{}, last_feedback{}, cancel_at{};
std::optional<ExecutionResult> result;
std::string feedback_snapshot{};
};
class ActiveGoalRegistry {
public:
@@ -101,15 +113,19 @@ class ActiveGoalRegistry {
// Registration is flushed before send. Failed journal writes throw and prevent send.
std::optional<std::string> start(GoalRequest, SteadyTime);
void pump(SteadyTime);
void set_dispatch_recorder(std::function<void(const GoalRequest&)> recorder) { dispatch_recorder_=std::move(recorder); }
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;
std::optional<GoalRecord> history(const std::string&) const;
std::vector<GoalRecord> task_records(const std::string& run_id) 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::function<void(const GoalRequest&)> dispatch_recorder_;
std::string journal_path_;
Budgets budgets_;
std::map<std::string, GoalRecord> records_;
@@ -118,6 +134,8 @@ class ActiveGoalRegistry {
void append(const GoalRecord&);
void ingest(const GoalEvent&, SteadyTime);
void load();
void prune_terminal();
std::deque<std::string> terminal_order_;
};
class ContextStore {
public:
@@ -165,6 +183,7 @@ class StageRunner {
TickStatus settle(SteadyTime, RosTime);
void update_safety(SafetySnapshot value) { safety_ = value; }
const std::string& detail() const { return detail_; }
const std::string& error_code() const { return error_code_; }
const std::string& active_goal_id() const { return active_goal_; }
std::uint64_t geometry_epoch() const { return geometry_epoch_; }
Holding holding() const { return holding_; }
@@ -180,11 +199,13 @@ class StageRunner {
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};
RosTime capture_after_{0}, last_ros_time_{0}, holding_valid_until_{0}, holding_observed_at_{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::string active_goal_, source_location_, source_side_, source_column_, source_tier_, verification_goal_, pending_posture_, detail_, error_code_;
std::optional<SteadyTime> waiting_since_;
std::optional<SteadyTime> motion_waiting_since_;
std::optional<SteadyTime> stopped_waiting_since_;
std::optional<SteadyTime> corroboration_waiting_since_, settlement_waiting_since_;
unsigned reobservations_{0}, adjustments_{0}, serial_{0};
enum class PickPhase { ASSESS, REOBSERVE, ADJUST, EXECUTE };
PickPhase pick_phase_{PickPhase::ASSESS};