move_base2/plugins/noop_action_handler.cpp

/*********************************************************************
 *
 * Software License Agreement (BSD License)
 *
 * move_base2 — action handler mặc định: log rồi báo xong sau một khoảng thời gian.
 *
 * Author: DuongTD
 *********************************************************************/

#include <move_base2/runners/action_handler.h>

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

#include <boost/dll/alias.hpp>
#include <robot/robot.h>

namespace move_base2
{
namespace
{
constexpr double kDefaultDuration = 0.0;  // [s] 0 = xong ngay ở tick đầu.
constexpr double kMaxDuration = 600.0;    // [s] trần vệ sinh cho param cấu hình sai.
constexpr double kDefaultTimeout = 30.0;  // [s]
}  // namespace

/**
 * @class NoopActionHandler
 * @brief Handler mặc định — không điều khiển thiết bị nào, chỉ log và đếm giờ.
 *
 * Có ba công dụng thật, không phải chỗ giữ chỗ:
 *  1. cho `actionType` chưa có thiết bị tương ứng (`wait`, hoặc action chỉ mang ý nghĩa ghi nhật ký)
 *     chạy được mà không phải viết handler riêng;
 *  2. dựng một deployment chạy end-to-end trước khi phần cứng sẵn sàng;
 *  3. làm ví dụ tham chiếu cho contract — đặc biệt là **timeout tầng 1**.
 *
 * `duration = 0` nghĩa là xong ngay ở tick đầu. Đặt > 0 để mô phỏng một thiết bị chậm; đặt
 * `hang: true` để mô phỏng thiết bị không bao giờ trả lời — khi đó chỉ `timeout` cắt được, đúng
 * tình huống mà timeout tầng 1 sinh ra để xử lý.
 */
class NoopActionHandler final : public ActionHandler
{
public:
  NoopActionHandler() = default;

  static ActionHandler::Ptr create()
  {
    return std::make_shared<NoopActionHandler>();
  }

  bool configure(const std::string& name, robot::NodeHandle& nh) override
  {
    name_ = name;

    nh.param("duration", duration_, kDefaultDuration);
    nh.param("timeout", timeout_, kDefaultTimeout);
    nh.param("hang", hang_, false);
    nh.param("action_types", action_types_, std::vector<std::string>{"wait"});

    if (!std::isfinite(duration_) || duration_ < 0.0 || duration_ > kMaxDuration)
    {
      robot::log_warning("[move_base2] '%s': duration=%.3f s ngoài [0, %.0f]; dùng %.3f s.",
                         name_.c_str(), duration_, kMaxDuration, kDefaultDuration);
      duration_ = kDefaultDuration;
    }

    if (!std::isfinite(timeout_) || timeout_ < 0.0)
    {
      robot::log_warning("[move_base2] '%s': timeout=%.3f s không hợp lệ; dùng %.3f s.",
                         name_.c_str(), timeout_, kDefaultTimeout);
      timeout_ = kDefaultTimeout;
    }

    if (hang_ && timeout_ <= 0.0)
    {
      // Chế độ mô phỏng thiết bị treo mà lại tắt timeout thì action sẽ chạy vĩnh viễn — đúng thứ
      // contract cấm.
      robot::log_error("[move_base2] '%s': hang=true nhưng timeout=%.3f s; handler sẽ không bao "
                       "giờ kết thúc.", name_.c_str(), timeout_);
      return false;
    }

    if (!hang_ && timeout_ > 0.0 && duration_ > 0.0 && timeout_ <= duration_)
    {
      // Cấu hình này khiến action LUÔN hỏng vì timeout — gần như chắc chắn là gõ nhầm.
      robot::log_error("[move_base2] '%s': timeout=%.3f s <= duration=%.3f s; action sẽ luôn thất "
                       "bại.", name_.c_str(), timeout_, duration_);
      return false;
    }

    if (action_types_.empty())
    {
      robot::log_error("[move_base2] '%s': action_types rỗng — handler sẽ không bao giờ được gọi.",
                       name_.c_str());
      return false;
    }

    return true;
  }

  std::vector<std::string> supportedActionTypes() const override
  {
    return action_types_;
  }

  bool start(const robot_protocol_msgs::Action& action, const robot::Time& now) override
  {
    started_at_ = now;
    action_id_ = action.actionId;

    robot::log_info("[move_base2] '%s': bắt đầu action '%s' (id '%s'), duration %.3f s.",
                    name_.c_str(), action.actionType.c_str(), action.actionId.c_str(), duration_);
    return true;
  }

  ActionTick update(const robot::Time& now) override
  {
    ActionTick tick;
    const double elapsed = (now - started_at_).toSec();  // [s]

    // Timeout TẦNG 1 — trách nhiệm của chính handler, không dựa vào action_patience của state
    // machine (mặc định tắt). Ở đây ngưỡng là cấu hình vì handler này không nói chuyện với thiết bị
    // nào; handler thật thì suy ngưỡng từ hiểu biết về thiết bị của nó.
    if (timeout_ > 0.0 && elapsed >= timeout_)
    {
      tick.status = ActionTick::Status::kFailed;
      tick.message = "action '" + action_id_ + "' quá timeout";
      return tick;
    }

    if (hang_)
    {
      // Mô phỏng thiết bị không bao giờ trả lời. Có để fault-injection trong test tích hợp: đây là
      // đúng tình huống mà timeout tầng 1 sinh ra để xử lý.
      tick.status = ActionTick::Status::kRunning;
      return tick;
    }

    if (elapsed >= duration_)
    {
      tick.status = ActionTick::Status::kSucceeded;
      tick.message = "action '" + action_id_ + "' hoàn tất";
      return tick;
    }

    tick.status = ActionTick::Status::kRunning;
    return tick;
  }

  void cancel() override
  {
    // Không có thiết bị nào để đưa về trạng thái an toàn. Handler thật phải làm việc đó ở đây.
    robot::log_info("[move_base2] '%s': action '%s' bị huỷ.", name_.c_str(), action_id_.c_str());
  }

private:
  std::string name_;
  std::vector<std::string> action_types_{"wait"};
  double duration_ = kDefaultDuration;  ///< [s]
  double timeout_ = kDefaultTimeout;    ///< [s] 0 = không giới hạn
  bool hang_ = false;                   ///< true = không bao giờ hoàn tất (fault injection)

  robot::Time started_at_;
  std::string action_id_;
};

}  // namespace move_base2

BOOST_DLL_ALIAS(move_base2::NoopActionHandler::create, NoopActionHandler)