first commit

test/fake_ports.h

@@ -0,0 +1,870 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ * move_base2 — hiện thực giả của các port, kịch bản hoá bằng chuỗi kết quả định sẵn.
+ *
+ * Đặt trong test/ của chính move_base2 chứ không đặt trong nav_test_harness: các fake này hiện thực
+ * port CỦA move_base2, nếu để trong harness thì harness phải phụ thuộc ngược vào move_base2 và
+ * chiều phụ thuộc một chiều bị phá vỡ. Phần fake thực sự dùng chung (đồng hồ, costmap, pose,
+ * kiểm va chạm, kịch bản) nằm ở nav_test_harness.
+ *
+ * Author: DuongTD
+ *********************************************************************/
+#ifndef MOVE_BASE2_TEST_FAKE_PORTS_H_
+#define MOVE_BASE2_TEST_FAKE_PORTS_H_
+
+#include <cstddef>
+#include <cstdint>
+#include <map>
+#include <limits>
+#include <string>
+#include <utility>
+#include <vector>
+
+#include <robot/time.h>
+
+#include <move_base2/ports/action_port.h>
+#include <move_base2/ports/clock_port.h>
+#include <move_base2/ports/controller_port.h>
+#include <move_base2/ports/mission_port.h>
+#include <move_base2/ports/planner_port.h>
+#include <move_base2/ports/pose_port.h>
+#include <move_base2/ports/recovery_port.h>
+
+namespace move_base2
+{
+namespace testing
+{
+
+/// @brief Kết quả một lần lập plan trong kịch bản.
+enum class PlannerScript
+{
+  kOk,     ///< Trả plan hợp lệ.
+  kFail,   ///< makePlan trả false.
+  kEmpty   ///< makePlan trả true nhưng plan rỗng — bẫy front()/back() trên vector rỗng.
+};
+
+/// @brief Kết quả một lần gọi controller trong kịch bản.
+enum class ControllerScript
+{
+  kOk,           ///< Sinh lệnh hợp lệ.
+  kFail,         ///< Không sinh được lệnh.
+  kGoalReached,  ///< Báo đã tới đích.
+  kNaN,          ///< Sinh lệnh chứa NaN — phải bị bộ trọng tài chặn.
+  kTooFast       ///< Sinh lệnh vượt trần vận tốc — phải bị clamp.
+};
+
+/// @brief Kết quả một tick recovery trong kịch bản.
+enum class RecoveryScript
+{
+  kRunning,
+  kSucceeded,
+  kFailed
+};
+
+/// @brief Kết quả một tick action trong kịch bản (D8).
+enum class ActionScript
+{
+  kRunning,
+  kSucceeded,
+  kFailed
+};
+
+// ------------------------------------------------------------------------------------------------
+
+/// @brief Đồng hồ do test điều khiển, chuyển tiếp một FakeClock của harness qua ClockPort.
+class FakeClockPort final : public ClockPort
+{
+public:
+  explicit FakeClockPort(double start_sec = 1000.0) : now_(start_sec)
+  {
+  }
+
+  robot::Time now() const override
+  {
+    return now_;
+  }
+
+  /// @param seconds [s] Lượng thời gian trôi. Giá trị âm bị bỏ qua.
+  void advance(double seconds)
+  {
+    if (seconds > 0.0)
+    {
+      now_ = robot::Time(now_.toSec() + seconds);
+    }
+  }
+
+  void setTime(double seconds)
+  {
+    now_ = robot::Time(seconds);
+  }
+
+private:
+  robot::Time now_;
+};
+
+// ------------------------------------------------------------------------------------------------
+
+class FakePosePort final : public PosePort
+{
+public:
+  bool getRobotPose(robot_geometry_msgs::PoseStamped& pose) const override
+  {
+    ++call_count_;
+    if (!available_)
+    {
+      return false;
+    }
+    pose = pose_;
+    return true;
+  }
+
+  /// @param x,y [m]
+  void setPosition(double x, double y)
+  {
+    pose_.header.frame_id = "map";
+    pose_.pose.position.x = x;
+    pose_.pose.position.y = y;
+    pose_.pose.orientation.w = 1.0;
+  }
+
+  /// @brief false = mô phỏng TF thiếu/stale.
+  void setAvailable(bool available)
+  {
+    available_ = available;
+  }
+
+  std::size_t callCount() const
+  {
+    return call_count_;
+  }
+
+private:
+  robot_geometry_msgs::PoseStamped pose_;
+  bool available_ = true;
+  mutable std::size_t call_count_ = 0;
+};
+
+// ------------------------------------------------------------------------------------------------
+
+class FakePlannerPort final : public PlannerPort
+{
+public:
+  bool swapPlanner(const std::string& planner_name) override
+  {
+    if (!swap_succeeds_)
+    {
+      return false;
+    }
+    active_ = planner_name;
+    ++swap_count_;
+    return true;
+  }
+
+  bool startPlan(const robot_geometry_msgs::PoseStamped& /*start*/,
+                 const robot_geometry_msgs::PoseStamped& goal,
+                 const robot_protocol_msgs::Order* order, std::uint64_t tag) override
+  {
+    if (in_flight_)
+    {
+      return false;  // Đúng như PlannerRunner: một lượt tại một thời điểm.
+    }
+
+    ++make_plan_count_;
+    saw_order_ = saw_order_ || order != nullptr;
+
+    in_flight_ = true;
+    pending_tag_ = tag;
+    pending_goal_ = goal;
+    cycles_left_ = latency_cycles_;
+    pending_action_ = nextAction();
+    return true;
+  }
+
+  bool isPlanning() const override
+  {
+    return in_flight_;
+  }
+
+  bool pollPlan(PlanResult& result) override
+  {
+    if (!in_flight_)
+    {
+      return false;
+    }
+    if (cycles_left_ > 0)
+    {
+      --cycles_left_;
+      return false;  // Còn "đang tính" — bên gọi phải thấy kBusy.
+    }
+
+    in_flight_ = false;
+    result.tag = pending_tag_;
+    result.plan.clear();
+
+    switch (pending_action_)
+    {
+      case PlannerScript::kFail:
+        result.succeeded = false;
+        return true;
+      case PlannerScript::kEmpty:
+        // Contract nói thành công phải kèm plan không rỗng; fake cố ý vi phạm để kiểm guard của
+        // bên gọi.
+        result.succeeded = true;
+        return true;
+      case PlannerScript::kOk:
+        break;
+    }
+
+    result.succeeded = true;
+    result.plan.push_back(pending_goal_);
+    return true;
+  }
+
+  void cancelPlan() override
+  {
+    in_flight_ = false;
+    ++cancel_count_;
+  }
+
+  std::string activePlanner() const override
+  {
+    return active_;
+  }
+
+  /**
+   * @brief Số cycle mà một lượt lập plan "mất" trước khi có kết quả.
+   *
+   * 0 (mặc định) = kết quả có ngay ở lần poll kế tiếp, tức đúng nhịp của bản lập plan đồng bộ cũ:
+   * kick ở cuối cycle N, state machine thấy plan ở cycle N+1. Nhờ vậy mọi test viết cho bản đồng bộ
+   * giữ nguyên ý nghĩa.
+   */
+  void setLatencyCycles(std::size_t cycles)
+  {
+    latency_cycles_ = cycles;
+  }
+
+  std::size_t cancelCount() const
+  {
+    return cancel_count_;
+  }
+
+  void setScript(std::vector<PlannerScript> script)
+  {
+    script_ = std::move(script);
+    index_ = 0;
+  }
+
+  void setSwapSucceeds(bool succeeds)
+  {
+    swap_succeeds_ = succeeds;
+  }
+
+  std::size_t makePlanCount() const
+  {
+    return make_plan_count_;
+  }
+
+  std::size_t swapCount() const
+  {
+    return swap_count_;
+  }
+
+  bool sawOrder() const
+  {
+    return saw_order_;
+  }
+
+private:
+  /// Hết kịch bản thì giữ kết quả cuối; kịch bản rỗng thì luôn thành công.
+  PlannerScript nextAction()
+  {
+    if (script_.empty())
+    {
+      return PlannerScript::kOk;
+    }
+    if (index_ >= script_.size())
+    {
+      return script_.back();
+    }
+    return script_[index_++];
+  }
+
+  std::vector<PlannerScript> script_;
+  std::size_t index_ = 0;
+  std::string active_;
+  bool swap_succeeds_ = true;
+
+  bool in_flight_ = false;
+  std::uint64_t pending_tag_ = 0;
+  robot_geometry_msgs::PoseStamped pending_goal_;
+  PlannerScript pending_action_ = PlannerScript::kOk;
+  std::size_t latency_cycles_ = 0;
+  std::size_t cycles_left_ = 0;
+  std::size_t cancel_count_ = 0;
+
+  std::size_t make_plan_count_ = 0;
+  std::size_t swap_count_ = 0;
+  bool saw_order_ = false;
+};
+
+// ------------------------------------------------------------------------------------------------
+
+class FakeControllerPort final : public ControllerPort
+{
+public:
+  bool swapPlanner(const std::string& planner_name) override
+  {
+    if (!swap_succeeds_)
+    {
+      return false;
+    }
+    active_ = planner_name;
+    return true;
+  }
+
+  void setTolerance(double xy_m, double yaw_rad) override
+  {
+    xy_tolerance_ = xy_m;
+    yaw_tolerance_ = yaw_rad;
+  }
+
+  bool setPlan(const std::vector<robot_geometry_msgs::PoseStamped>& plan) override
+  {
+    ++set_plan_count_;
+    last_plan_size_ = plan.size();
+    return set_plan_succeeds_ && !plan.empty();
+  }
+
+  bool computeVelocityCommands(robot_geometry_msgs::Twist& cmd) override
+  {
+    ++compute_count_;
+    switch (current_action_)
+    {
+      case ControllerScript::kFail:
+        return false;
+      case ControllerScript::kNaN:
+        cmd.linear.x = std::numeric_limits<double>::quiet_NaN();
+        cmd.angular.z = 0.0;
+        return true;
+      case ControllerScript::kTooFast:
+        cmd.linear.x = 99.0;
+        cmd.angular.z = 99.0;
+        return true;
+      case ControllerScript::kGoalReached:
+      case ControllerScript::kOk:
+        break;
+    }
+    cmd.linear.x = nominal_speed_;
+    cmd.angular.z = 0.0;
+    return true;
+  }
+
+  bool isGoalReached() override
+  {
+    // Lấy hành động cho cycle này ở đây vì đây là lời gọi ĐẦU TIÊN của một cycle controller, đúng
+    // thứ tự mà control loop dùng.
+    current_action_ = nextAction();
+    ++goal_check_count_;
+    return current_action_ == ControllerScript::kGoalReached;
+  }
+
+  void setMeasuredVelocity(const robot_geometry_msgs::Twist& velocity) override
+  {
+    measured_velocity_ = velocity;
+  }
+
+  bool setTwistLinear(const robot_geometry_msgs::Vector3& linear) override
+  {
+    if (linear.x < 0.0)
+    {
+      limit_backward_ = linear.x;  // [m/s], âm
+    }
+    else
+    {
+      limit_forward_ = linear.x;  // [m/s]
+    }
+    return true;
+  }
+
+  bool setTwistAngular(const robot_geometry_msgs::Vector3& angular) override
+  {
+    limit_angular_ = angular.z;  // [rad/s]
+    return true;
+  }
+
+  const robot_geometry_msgs::Twist& measuredVelocity() const
+  {
+    return measured_velocity_;
+  }
+
+  double limitForward() const
+  {
+    return limit_forward_;
+  }
+
+  double limitBackward() const
+  {
+    return limit_backward_;
+  }
+
+  double limitAngular() const
+  {
+    return limit_angular_;
+  }
+
+  std::string activeController() const override
+  {
+    return active_;
+  }
+
+  void setScript(std::vector<ControllerScript> script)
+  {
+    script_ = std::move(script);
+    index_ = 0;
+  }
+
+  void setSwapSucceeds(bool succeeds)
+  {
+    swap_succeeds_ = succeeds;
+  }
+
+  void setSetPlanSucceeds(bool succeeds)
+  {
+    set_plan_succeeds_ = succeeds;
+  }
+
+  /// @param speed [m/s] Tốc độ dài của lệnh khi kịch bản là kOk.
+  void setNominalSpeed(double speed)
+  {
+    nominal_speed_ = speed;
+  }
+
+  std::size_t setPlanCount() const
+  {
+    return set_plan_count_;
+  }
+
+  std::size_t computeCount() const
+  {
+    return compute_count_;
+  }
+
+  std::size_t goalCheckCount() const
+  {
+    return goal_check_count_;
+  }
+
+  std::size_t lastPlanSize() const
+  {
+    return last_plan_size_;
+  }
+
+  double xyTolerance() const
+  {
+    return xy_tolerance_;
+  }
+
+  double yawTolerance() const
+  {
+    return yaw_tolerance_;
+  }
+
+private:
+  robot_geometry_msgs::Twist measured_velocity_;
+  double limit_forward_ = 0.0;   ///< [m/s]
+  double limit_backward_ = 0.0;  ///< [m/s], âm
+  double limit_angular_ = 0.0;   ///< [rad/s]
+
+  ControllerScript nextAction()
+  {
+    if (script_.empty())
+    {
+      return ControllerScript::kOk;
+    }
+    if (index_ >= script_.size())
+    {
+      return script_.back();
+    }
+    return script_[index_++];
+  }
+
+  std::vector<ControllerScript> script_;
+  std::size_t index_ = 0;
+  ControllerScript current_action_ = ControllerScript::kOk;
+
+  std::string active_;
+  bool swap_succeeds_ = true;
+  bool set_plan_succeeds_ = true;
+  double nominal_speed_ = 0.3;  ///< [m/s]
+  double xy_tolerance_ = 0.0;   ///< [m]
+  double yaw_tolerance_ = 0.0;  ///< [rad]
+
+  std::size_t set_plan_count_ = 0;
+  std::size_t compute_count_ = 0;
+  std::size_t goal_check_count_ = 0;
+  std::size_t last_plan_size_ = 0;
+};
+
+// ------------------------------------------------------------------------------------------------
+
+class FakeRecoveryPort final : public RecoveryPort
+{
+public:
+  explicit FakeRecoveryPort(std::size_t behavior_count = 2) : behavior_count_(behavior_count)
+  {
+  }
+
+  bool configure(robot::NodeHandle& /*nh*/) override
+  {
+    return true;
+  }
+
+  std::size_t behaviorCount() const override
+  {
+    return behavior_count_;
+  }
+
+  RecoveryOutputKind outputKind(std::size_t index) const override
+  {
+    if (index >= behavior_count_)
+    {
+      return RecoveryOutputKind::kNone;
+    }
+    const auto it = output_kinds_.find(index);
+    return it == output_kinds_.end() ? default_output_kind_ : it->second;
+  }
+
+  /// @brief Đặt họ output cho behavior thứ @p index (mặc định mọi behavior đều lái robot).
+  void setOutputKind(std::size_t index, RecoveryOutputKind kind)
+  {
+    output_kinds_[index] = kind;
+  }
+
+  void setDefaultOutputKind(RecoveryOutputKind kind)
+  {
+    default_output_kind_ = kind;
+  }
+
+  bool start(std::size_t index, RecoveryTrigger trigger) override
+  {
+    ++start_count_;
+    last_start_index_ = index;
+    last_trigger_ = trigger;
+    started_indices_.push_back(index);
+
+    if (index >= behavior_count_ || !start_succeeds_)
+    {
+      return false;
+    }
+    active_ = true;
+    return true;
+  }
+
+  RecoveryTick update() override
+  {
+    ++update_count_;
+
+    RecoveryTick tick;
+    switch (nextAction())
+    {
+      case RecoveryScript::kRunning:
+        tick.status = RecoveryTick::Status::kRunning;
+        tick.has_velocity = emits_velocity_;
+        tick.cmd.linear.x = recovery_speed_;
+        break;
+      case RecoveryScript::kSucceeded:
+        tick.status = RecoveryTick::Status::kSucceeded;
+        active_ = false;
+        break;
+      case RecoveryScript::kFailed:
+        tick.status = RecoveryTick::Status::kFailed;
+        active_ = false;
+        break;
+    }
+    return tick;
+  }
+
+  void cancel() override
+  {
+    ++cancel_count_;
+    active_ = false;
+  }
+
+  std::string behaviorName(std::size_t index) const override
+  {
+    return index < behavior_count_ ? "fake_behavior_" + std::to_string(index) : std::string();
+  }
+
+  void setScript(std::vector<RecoveryScript> script)
+  {
+    script_ = std::move(script);
+    index_ = 0;
+  }
+
+  void setStartSucceeds(bool succeeds)
+  {
+    start_succeeds_ = succeeds;
+  }
+
+  /// @param speed [m/s] Vận tốc behavior phát khi đang chạy. Dấu âm nghĩa là lùi.
+  void setRecoveryVelocity(bool emits, double speed)
+  {
+    emits_velocity_ = emits;
+    recovery_speed_ = speed;
+  }
+
+  std::size_t startCount() const
+  {
+    return start_count_;
+  }
+
+  std::size_t updateCount() const
+  {
+    return update_count_;
+  }
+
+  std::size_t cancelCount() const
+  {
+    return cancel_count_;
+  }
+
+  std::size_t lastStartIndex() const
+  {
+    return last_start_index_;
+  }
+
+  RecoveryTrigger lastTrigger() const
+  {
+    return last_trigger_;
+  }
+
+  const std::vector<std::size_t>& startedIndices() const
+  {
+    return started_indices_;
+  }
+
+  bool active() const
+  {
+    return active_;
+  }
+
+private:
+  RecoveryScript nextAction()
+  {
+    if (script_.empty())
+    {
+      return RecoveryScript::kSucceeded;
+    }
+    if (index_ >= script_.size())
+    {
+      return script_.back();
+    }
+    return script_[index_++];
+  }
+
+  std::size_t behavior_count_;
+  std::vector<RecoveryScript> script_;
+  std::size_t index_ = 0;
+
+  bool start_succeeds_ = true;
+  bool active_ = false;
+  bool emits_velocity_ = false;
+  double recovery_speed_ = -0.1;  ///< [m/s], âm = lùi
+
+  /// Mặc định coi mọi behavior đều lái robot — giữ nguyên hành vi của các test viết trước khi
+  /// RecoveryPort có outputKind().
+  RecoveryOutputKind default_output_kind_ = RecoveryOutputKind::kVelocity;
+  std::map<std::size_t, RecoveryOutputKind> output_kinds_;
+
+  std::size_t start_count_ = 0;
+  std::size_t update_count_ = 0;
+  std::size_t cancel_count_ = 0;
+  std::size_t last_start_index_ = 0;
+  RecoveryTrigger last_trigger_ = RecoveryTrigger::kPlanningFailed;
+  std::vector<std::size_t> started_indices_;
+};
+
+// ------------------------------------------------------------------------------------------------
+
+class FakeActionPort final : public ActionPort
+{
+public:
+  bool configure(robot::NodeHandle& /*nh*/) override
+  {
+    return true;
+  }
+
+  bool start(const robot_protocol_msgs::Action& action) override
+  {
+    ++start_count_;
+    started_action_types_.push_back(action.actionType);
+    if (!start_succeeds_)
+    {
+      return false;
+    }
+    active_ = true;
+    return true;
+  }
+
+  ActionTick update() override
+  {
+    ++update_count_;
+
+    ActionTick tick;
+    switch (nextAction())
+    {
+      case ActionScript::kRunning:
+        tick.status = ActionTick::Status::kRunning;
+        break;
+      case ActionScript::kSucceeded:
+        tick.status = ActionTick::Status::kSucceeded;
+        active_ = false;
+        break;
+      case ActionScript::kFailed:
+        tick.status = ActionTick::Status::kFailed;
+        tick.message = "fake action failed";
+        active_ = false;
+        break;
+    }
+    return tick;
+  }
+
+  void cancel() override
+  {
+    ++cancel_count_;
+    active_ = false;
+  }
+
+  void setScript(std::vector<ActionScript> script)
+  {
+    script_ = std::move(script);
+    index_ = 0;
+  }
+
+  void setStartSucceeds(bool succeeds)
+  {
+    start_succeeds_ = succeeds;
+  }
+
+  std::size_t startCount() const
+  {
+    return start_count_;
+  }
+
+  std::size_t updateCount() const
+  {
+    return update_count_;
+  }
+
+  std::size_t cancelCount() const
+  {
+    return cancel_count_;
+  }
+
+  /// @brief actionType của từng lần start, theo thứ tự — kiểm "actions đi nguyên vẹn, đúng thứ tự".
+  const std::vector<std::string>& startedActionTypes() const
+  {
+    return started_action_types_;
+  }
+
+  bool active() const
+  {
+    return active_;
+  }
+
+private:
+  ActionScript nextAction()
+  {
+    if (script_.empty())
+    {
+      return ActionScript::kSucceeded;
+    }
+    if (index_ >= script_.size())
+    {
+      return script_.back();
+    }
+    return script_[index_++];
+  }
+
+  std::vector<ActionScript> script_;
+  std::size_t index_ = 0;
+
+  bool start_succeeds_ = true;
+  bool active_ = false;
+
+  std::size_t start_count_ = 0;
+  std::size_t update_count_ = 0;
+  std::size_t cancel_count_ = 0;
+  std::vector<std::string> started_action_types_;
+};
+
+// ------------------------------------------------------------------------------------------------
+
+class FakeMissionPort final : public MissionPort
+{
+public:
+  void setRequestCallback(RequestCallback callback) override
+  {
+    callback_ = std::move(callback);
+  }
+
+  void reportOutcome(std::uint64_t mission_sequence_id, NavigationOutcome outcome) override
+  {
+    reports_.emplace_back(mission_sequence_id, outcome);
+  }
+
+  bool hasActiveMission() const override
+  {
+    return active_;
+  }
+
+  void start() override
+  {
+    active_ = true;
+  }
+
+  void stop() override
+  {
+    active_ = false;
+  }
+
+  /// @brief Giả lập mission layer đẩy một chặng xuống.
+  void emit(const NavigationRequest& request)
+  {
+    if (callback_)
+    {
+      callback_(request);
+    }
+  }
+
+  const std::vector<std::pair<std::uint64_t, NavigationOutcome>>& reports() const
+  {
+    return reports_;
+  }
+
+  /// @brief Số lần đã báo kết quả cho một sequence id — bất biến là phải bằng 1.
+  std::size_t reportCountFor(std::uint64_t mission_sequence_id) const
+  {
+    std::size_t count = 0;
+    for (const auto& report : reports_)
+    {
+      if (report.first == mission_sequence_id)
+      {
+        ++count;
+      }
+    }
+    return count;
+  }
+
+private:
+  RequestCallback callback_;
+  std::vector<std::pair<std::uint64_t, NavigationOutcome>> reports_;
+  bool active_ = false;
+};
+
+}  // namespace testing
+}  // namespace move_base2
+
+#endif  // MOVE_BASE2_TEST_FAKE_PORTS_H_