move_base2/test/recovery_scenario_driver.h

/*********************************************************************
 *
 * Software License Agreement (BSD License)
 *
 * move_base2 — ScenarioDriver thứ hai: recovery_core THẬT trên một thế giới giả có vật cản.
 *
 * Vì sao cần driver thứ hai. @ref MoveBase2ScenarioDriver mô tả **hành vi của các cổng**: recovery
 * của nó là một script, nên nó kiểm được "lõi phản ứng đúng khi recovery báo hỏng" nhưng không bao
 * giờ kiểm được "recovery có tự phát hiện ra vật cản sau lưng hay không" — câu hỏi đó chỉ có nghĩa
 * khi plugin thật chạy trên một lưới thật có vật cản thật.
 *
 * Ba thứ driver này có mà driver kia không có:
 *  1. **Plugin thật**, nạp qua đúng đường Boost.DLL mà runtime đi.
 *  2. **Lưới và va chạm thật** — `FakeCostmap` + `FakeCollisionChecker` của harness, vật cản lấy
 *     thẳng từ `Scenario::obstacles`.
 *  3. **Robot thật sự di chuyển**: lệnh vận tốc phát ra được tích phân vào pose mỗi cycle. Không có
 *     phần này thì `BackUpRecovery` không bao giờ tiến tới vật cản, và ca test "lùi vào vật cản" chỉ
 *     là một cái tên.
 *
 * Author: DuongTD
 *********************************************************************/
#ifndef MOVE_BASE2_TEST_RECOVERY_SCENARIO_DRIVER_H_
#define MOVE_BASE2_TEST_RECOVERY_SCENARIO_DRIVER_H_

#include <cmath>
#include <cstddef>
#include <memory>
#include <string>
#include <vector>

#include <nav_test_harness/fake_collision_checker.h>
#include <nav_test_harness/fake_costmap.h>
#include <nav_test_harness/fake_pose_provider.h>
#include <nav_test_harness/scenario.h>
#include <nav_test_harness/scenario_runner.h>

#include <recovery_core/recovery_behavior.h>
#include <recovery_core/recovery_context.h>
#include <recovery_core/recovery_registry.h>

#include <move_base2/control_loop.h>

#include "fake_ports.h"

namespace move_base2
{
namespace testing
{

/// @brief Nối `nav_test_harness::FakePoseProvider` vào cổng pose của recovery_core.
class HarnessPoseProvider final : public recovery_core::PoseProvider
{
public:
  explicit HarnessPoseProvider(nav_test_harness::FakePoseProvider* fake) : fake_(fake)
  {
  }

  bool getRobotPose(robot_geometry_msgs::PoseStamped& pose) const override
  {
    return fake_ != nullptr && fake_->getRobotPose(pose);
  }

private:
  nav_test_harness::FakePoseProvider* fake_ = nullptr;  ///< non-owning
};

/// @brief Nối `nav_test_harness::FakeCollisionChecker` vào cổng va chạm của recovery_core.
class HarnessCollisionChecker final : public recovery_core::CollisionChecker
{
public:
  explicit HarnessCollisionChecker(nav_test_harness::FakeCollisionChecker* fake) : fake_(fake)
  {
  }

  double footprintCost(double x, double y, double theta) const override
  {
    // Không có checker thì coi như không đặt được — giả định an toàn, không phải 0.
    return fake_ == nullptr ? -1.0 : fake_->footprintCost(x, y, theta);
  }

private:
  nav_test_harness::FakeCollisionChecker* fake_ = nullptr;  ///< non-owning
};

/**
 * @class RegistryRecoveryPort
 * @brief Hiện thực @ref RecoveryPort trên một `recovery_core::RecoveryRegistry` đã nạp sẵn.
 *
 * Là bản rút gọn của @ref RecoveryRunner: cùng ánh xạ enum, cùng ngữ nghĩa tick, nhưng lấy context
 * từ ngoài thay vì dựng từ `Costmap2DROBOT`. Nhờ vậy plugin thật chạy được trên lưới giả.
 *
 * @note Cố ý **không** dùng lại `RecoveryRunner`: cổng pose/va chạm của nó được dựng từ costmap thật
 *       và không bơm được từ ngoài. Thêm một seam chỉ-dành-cho-test vào lớp runtime để test dễ hơn
 *       là đổi runtime vì test — hướng phụ thuộc sai.
 */
class RegistryRecoveryPort final : public RecoveryPort
{
public:
  explicit RegistryRecoveryPort(recovery_core::RecoveryRegistry* registry, ClockPort* clock)
    : registry_(registry), clock_(clock)
  {
  }

  bool configure(robot::NodeHandle& /*nh*/) override
  {
    // Registry đã được nạp bởi driver trước khi control loop chạy.
    return registry_ != nullptr && registry_->size() > 0;
  }

  std::size_t behaviorCount() const override
  {
    return registry_ == nullptr ? 0 : registry_->size();
  }

  RecoveryOutputKind outputKind(std::size_t index) const override
  {
    recovery_core::RecoveryBehavior* behavior = behaviorAt(index);
    if (behavior == nullptr)
    {
      return RecoveryOutputKind::kNone;
    }
    switch (behavior->outputKind())
    {
      case recovery_core::RecoveryOutputType::kVelocity:
        return RecoveryOutputKind::kVelocity;
      case recovery_core::RecoveryOutputType::kPath:
        return RecoveryOutputKind::kPath;
      case recovery_core::RecoveryOutputType::kNone:
        break;
    }
    return RecoveryOutputKind::kNone;
  }

  bool start(std::size_t index, RecoveryTrigger trigger) override
  {
    active_ = behaviorAt(index);
    if (active_ == nullptr || clock_ == nullptr)
    {
      return false;
    }

    recovery_core::RecoveryGoal goal;
    switch (trigger)
    {
      case RecoveryTrigger::kPlanningFailed:
        goal.trigger = recovery_core::RecoveryTrigger::kPlanningFailed;
        break;
      case RecoveryTrigger::kControllingFailed:
        goal.trigger = recovery_core::RecoveryTrigger::kControllingFailed;
        break;
      case RecoveryTrigger::kOscillation:
        goal.trigger = recovery_core::RecoveryTrigger::kOscillation;
        break;
    }

    if (!active_->start(goal, clock_->now()))
    {
      // Đây chính là đường mà ca "vật cản sau lưng" đi qua: BackUpRecovery quét trước quãng lùi và
      // TỪ CHỐI khởi động nếu đã có va chạm — robot không được nhúc nhích lấy một cycle.
      ++start_rejections_;
      active_ = nullptr;
      return false;
    }
    return true;
  }

  RecoveryTick update() override
  {
    RecoveryTick tick;
    if (active_ == nullptr || clock_ == nullptr)
    {
      tick.status = RecoveryTick::Status::kFailed;
      return tick;
    }

    const recovery_core::RecoveryResult result = active_->update(clock_->now());
    switch (result.status)
    {
      case recovery_core::RecoveryStatus::kRunning:
        tick.status = RecoveryTick::Status::kRunning;
        break;
      case recovery_core::RecoveryStatus::kSucceeded:
        tick.status = RecoveryTick::Status::kSucceeded;
        break;
      case recovery_core::RecoveryStatus::kIdle:
      case recovery_core::RecoveryStatus::kCancelled:
      case recovery_core::RecoveryStatus::kFailed:
        tick.status = RecoveryTick::Status::kFailed;
        break;
    }

    if (const robot_geometry_msgs::Twist* velocity = result.velocity())
    {
      tick.has_velocity = true;
      tick.cmd = *velocity;
    }
    tick.message = result.message;
    return tick;
  }

  void cancel() override
  {
    if (active_ != nullptr)
    {
      active_->cancel();
    }
  }

  std::string behaviorName(std::size_t index) const override
  {
    return registry_ != nullptr && index < registry_->size() ? registry_->nameAt(index)
                                                             : std::string();
  }

  /// @brief Số lần behavior từ chối khởi động — bằng chứng ca test đi đúng nhánh nó nói là đang kiểm.
  std::size_t startRejections() const
  {
    return start_rejections_;
  }

private:
  recovery_core::RecoveryBehavior* behaviorAt(std::size_t index) const
  {
    return registry_ == nullptr ? nullptr : registry_->at(index);
  }

  recovery_core::RecoveryRegistry* registry_ = nullptr;  ///< non-owning
  ClockPort* clock_ = nullptr;                           ///< non-owning
  recovery_core::RecoveryBehavior* active_ = nullptr;    ///< non-owning
  std::size_t start_rejections_ = 0;
};

/**
 * @class RecoveryScenarioDriver
 * @brief Chạy kịch bản qua @ref ControlLoop với recovery_core THẬT trên lưới giả có vật cản.
 *
 * Planner và controller vẫn là cổng giả theo script: ca test ở đây nói về **recovery**, và một
 * planner thật sẽ làm kết quả phụ thuộc vào chất lượng đường đi thay vì vào thứ đang được kiểm.
 */
class RecoveryScenarioDriver final : public nav_test_harness::ScenarioDriver
{
public:
  bool setup(const nav_test_harness::Scenario& scenario, std::string& error) override
  {
    scenario_ = scenario;

    // Gọi setup() lần thứ hai phải dựng lại từ đầu, và THỨ TỰ tháo là bắt buộc: behavior trong
    // registry giữ con trỏ tới pose/collision bridge qua RecoveryContext, nên registry phải chết
    // TRƯỚC chúng. Tháo ngược lại là use-after-free, và triệu chứng của nó ("không lấy được pose")
    // trông y hệt một lỗi TF bình thường.
    recovery_port_.reset();
    registry_.reset();
    collision_bridge_.reset();
    pose_bridge_.reset();
    checker_.reset();
    costmap_.reset();

    if (scenario.obstacles.empty())
    {
      // Không phải lỗi chết người, nhưng nói ra: driver này tồn tại vì vật cản. Kịch bản không có
      // vật cản nào chạy ở đây là đang trả giá dựng plugin thật mà không kiểm thêm được gì.
      error = "RecoveryScenarioDriver is for scenarios WITH obstacles; use MoveBase2ScenarioDriver";
      return false;
    }

    // --- Thế giới: lưới, vật cản, pose ban đầu --------------------------------------------------
    costmap_.reset(new nav_test_harness::FakeCostmap(nav_test_harness::FakeCostmap::centered(
        kWorldSpan, kResolution)));
    for (const nav_test_harness::ScenarioObstacle& obstacle : scenario.obstacles)
    {
      if (costmap_->setLethalCircle(obstacle.x, obstacle.y, obstacle.radius) == 0)  // [m]
      {
        // Vật cản nằm ngoài lưới = kịch bản dựng sai. Im lặng ở đây nghĩa là ca test chạy trên một
        // thế giới không có vật cản nào và vẫn xanh.
        error = "the obstacle lies outside the fake grid — re-check the coordinates in the "
                "scenario";
        return false;
      }
    }

    checker_.reset(new nav_test_harness::FakeCollisionChecker(
        costmap_.get(),
        nav_test_harness::FakeCollisionChecker::rectangleFootprint(kFootprintLength,
                                                                   kFootprintWidth)));
    pose_source_.setPose(scenario.initial_pose.x, scenario.initial_pose.y,
                         scenario.initial_pose.theta);

    pose_bridge_.reset(new HarnessPoseProvider(&pose_source_));
    collision_bridge_.reset(new HarnessCollisionChecker(checker_.get()));

    recovery_core::RecoveryContext ctx;
    ctx.pose = pose_bridge_.get();
    ctx.collision = collision_bridge_.get();

    // --- Recovery THẬT --------------------------------------------------------------------------
    registry_.reset(new recovery_core::RecoveryRegistry());
    robot::NodeHandle nh;
    if (!registry_->loadFromConfig(nh, kRecoveryNamespace, ctx))
    {
      error = "could not load the real recovery behaviors from namespace '" +
              std::string(kRecoveryNamespace) + "' — check library_path in the test config";
      return false;
    }
    if (registry_->size() == 0)
    {
      error = "the recovery namespace is empty — the scenario would run without checking anything";
      return false;
    }

    recovery_port_.reset(new RegistryRecoveryPort(registry_.get(), &clock_));

    // --- Cổng giả cho phần còn lại --------------------------------------------------------------
    std::vector<PlannerScript> planner_script;
    for (const std::string& item : scenario.planner_script)
    {
      if (item == "ok")
      {
        planner_script.push_back(PlannerScript::kOk);
      }
      else if (item == "fail")
      {
        planner_script.push_back(PlannerScript::kFail);
      }
      else if (item == "empty")
      {
        planner_script.push_back(PlannerScript::kEmpty);
      }
      else
      {
        error = "unknown planner_script: '" + item + "'";
        return false;
      }
    }
    planner_.setScript(planner_script);

    std::vector<ControllerScript> controller_script;
    for (const std::string& item : scenario.controller_script)
    {
      if (item == "ok")
      {
        controller_script.push_back(ControllerScript::kOk);
      }
      else if (item == "fail")
      {
        controller_script.push_back(ControllerScript::kFail);
      }
      else if (item == "goal_reached")
      {
        controller_script.push_back(ControllerScript::kGoalReached);
      }
      else
      {
        error = "controller_script is not supported by this driver: '" + item + "'";
        return false;
      }
    }
    controller_.setScript(controller_script);

    if (!scenario.recovery_script.empty())
    {
      error = "recovery_script cannot be used here: recovery is a REAL plugin, its result is "
              "decided by collisions and geometry, not assigned by the scenario";
      return false;
    }

    pose_port_.setPosition(scenario.initial_pose.x, scenario.initial_pose.y);

    ControlLoopConfig config;
    config.nominal_control_period = scenario.control_period;  // [s]
    config.state_machine.planner_patience = 0.5;              // [s]
    config.state_machine.controller_patience = 0.5;           // [s]
    config.state_machine.oscillation_timeout = 0.0;           // tắt
    config.state_machine.oscillation_distance = 0.5;          // [m]
    config.state_machine.max_planning_retries = -1;
    config.state_machine.recovery_behavior_count = registry_->size();
    config.state_machine.recovery_enabled = true;

    config.velocity.max_vel_x = scenario.expect_max_speed > 0.0 ? scenario.expect_max_speed : 0.5;
    config.velocity.min_vel_x = -config.velocity.max_vel_x;
    config.velocity.max_vel_theta =
        scenario.expect_max_yaw_rate > 0.0 ? scenario.expect_max_yaw_rate : 1.0;
    config.velocity.max_accel_x = 100.0;      // [m/s^2] lớn: ca test kiểm va chạm, không kiểm ramp
    config.velocity.max_accel_theta = 100.0;  // [rad/s^2]

    config.position.global_planner_name = "ScenarioGlobalPlanner";
    config.position.local_planner_name = "ScenarioLocalPlanner";
    config.docking = config.position;
    config.go_straight = config.position;
    config.rotate = config.position;

    deps_.clock = &clock_;
    deps_.pose = &pose_port_;
    deps_.planner = &planner_;
    deps_.controller = &controller_;
    deps_.recovery = recovery_port_.get();
    deps_.mission = &mission_;
    deps_.action = &action_;

    if (!loop_.configure(config, deps_, error))
    {
      return false;
    }

    NavigationRequest request;
    request.profile = MotionProfile::kPosition;
    request.goal.header.frame_id = "map";
    request.goal.pose.position.x = scenario.goal.x;  // [m]
    request.goal.pose.position.y = scenario.goal.y;  // [m]
    request.goal.pose.orientation.z = std::sin(scenario.goal.theta * 0.5);
    request.goal.pose.orientation.w = std::cos(scenario.goal.theta * 0.5);

    if (!loop_.submit(request, error))
    {
      return false;
    }

    cycle_ = 0;
    started_ = false;
    return true;
  }

  bool step(nav_test_harness::ScenarioStep& step) override
  {
    if (!started_)
    {
      started_ = true;
      step.cycle = 0;
      step.state = toString(loop_.state());
      step.linear_x = 0.0;
      step.angular_z = 0.0;
      return true;
    }

    applyEventsFor(cycle_);

    const bool running = loop_.step();

    const robot_geometry_msgs::Twist& command = loop_.lastCommand();
    step.cycle = cycle_;
    step.state = toString(loop_.state());
    step.linear_x = command.linear.x;    // [m/s]
    step.angular_z = command.angular.z;  // [rad/s]

    // Robot thật sự đi theo lệnh vừa phát. Đây là điểm khác biệt của driver này: không có tích phân
    // chuyển động thì pose đứng yên, `BackUpRecovery` không bao giờ đo được quãng đã lùi, và ca test
    // "lùi vào vật cản" sẽ kết thúc vì hết giờ chứ không vì va chạm — xanh vì lý do sai.
    integrateMotion(command, scenario_.control_period);

    clock_.advance(scenario_.control_period);
    ++cycle_;
    return running;
  }

  std::string outcome() const override
  {
    const char* text = loop_.lastOutcome();
    return text != nullptr ? std::string(text) : std::string();
  }

  /// @brief Số lần behavior từ chối khởi động — dùng để khẳng định ca test đi đúng nhánh.
  std::size_t startRejections() const
  {
    return recovery_port_ == nullptr ? 0 : recovery_port_->startRejections();
  }

private:
  /// [m] Cạnh của lưới giả, đủ rộng để quãng lùi và footprint không chạm biên.
  static constexpr double kWorldSpan = 8.0;
  static constexpr double kResolution = 0.05;      ///< [m/ô] khớp costmap thật của workspace
  static constexpr double kFootprintLength = 0.6;  ///< [m]
  static constexpr double kFootprintWidth = 0.4;   ///< [m]
  static constexpr const char* kRecoveryNamespace = "recovery_scenario";

  void integrateMotion(const robot_geometry_msgs::Twist& command, double dt)
  {
    const double yaw = pose_source_.rawPose().theta;  // [rad]
    pose_source_.moveBy(command.linear.x * std::cos(yaw) * dt,
                        command.linear.x * std::sin(yaw) * dt, command.angular.z * dt);

    // Hai nguồn pose phải đi cùng nhau: `pose_source_` là thứ recovery_core nhìn thấy, `pose_port_`
    // là thứ lõi nhìn thấy. Lệch nhau thì chống quẩn và recovery nói về hai robot khác nhau.
    const auto& pose = pose_source_.rawPose();
    pose_port_.setPosition(pose.x, pose.y);
  }

  void applyEventsFor(std::size_t cycle)
  {
    for (const nav_test_harness::ScenarioEvent& event : scenario_.events)
    {
      if (event.cycle != cycle)
      {
        continue;
      }
      if (event.action == "cancel")
      {
        loop_.requestCancel();
      }
      else if (event.action == "pause")
      {
        loop_.requestPause();
      }
      else if (event.action == "resume")
      {
        loop_.requestResume();
      }
      else if (event.action == "lose_pose")
      {
        pose_port_.setAvailable(false);
      }
      else if (event.action == "restore_pose")
      {
        pose_port_.setAvailable(true);
      }
      else if (event.action == "sensors_stale")
      {
        costmap_status_.setCurrent(false);
      }
      else if (event.action == "sensors_ok")
      {
        costmap_status_.setCurrent(true);
      }
    }
  }

  nav_test_harness::Scenario scenario_;

  std::unique_ptr<nav_test_harness::FakeCostmap> costmap_;
  std::unique_ptr<nav_test_harness::FakeCollisionChecker> checker_;
  nav_test_harness::FakePoseProvider pose_source_;
  std::unique_ptr<HarnessPoseProvider> pose_bridge_;
  std::unique_ptr<HarnessCollisionChecker> collision_bridge_;

  // Khai SAU các cầu nối: thành viên bị huỷ theo thứ tự ngược, nên registry (giữ con trỏ tới chúng)
  // chết trước — cùng lý do với thứ tự tháo trong setup().
  std::unique_ptr<recovery_core::RecoveryRegistry> registry_;
  std::unique_ptr<RegistryRecoveryPort> recovery_port_;

  ControlLoop loop_;
  ControlLoopDeps deps_;
  FakeClockPort clock_;
  FakePosePort pose_port_;
  FakePlannerPort planner_;
  FakeControllerPort controller_;
  FakeMissionPort mission_;
  FakeActionPort action_;
  FakeCostmapStatusPort costmap_status_;

  std::size_t cycle_ = 0;
  bool started_ = false;
};

}  // namespace testing
}  // namespace move_base2

#endif  // MOVE_BASE2_TEST_RECOVERY_SCENARIO_DRIVER_H_