move_base2/test/fake_ports.h

/*********************************************************************
 *
 * 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_