实现行为树执行器、任务协调和技能接口

2026-09-20 12:18:52 +08:00
commit 492676344a
143 changed files with 13010 additions and 0 deletions
@@ -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