move_base2/test/plugins/test_local_planner.cpp

/*********************************************************************
 *
 * Software License Agreement (BSD License)
 *
 * move_base2 — plugin local planner CHỈ dùng cho test.
 *
 * Instance được nạp qua Boost.DLL nên test không giữ được con trỏ tới nó. Thay vì mở một cửa hậu để
 * đọc trạng thái, planner này **phản ánh** thứ nó nhận được vào chính lệnh vận tốc nó trả về:
 *
 *   cmd.linear.x  = clamp(kBaseSpeed + vận_tốc_đo_được.x, trần_tiến)
 *   cmd.angular.z = clamp(kBaseYawRate, trần_góc)
 *
 * Nhờ vậy "trần đã tới plugin chưa" và "vận tốc đo được đã tới plugin chưa" kiểm được qua đúng API
 * mà runtime dùng, không cần cơ chế quan sát riêng nào.
 *
 * Không alias nào chạm vào con trỏ TF hay costmap — đó là điều kiện để test truyền con trỏ giả thay
 * vì phải dựng `tf3::BufferCore` và `Costmap2DROBOT` thật.
 *
 * Author: DuongTD
 *********************************************************************/
#include <algorithm>
#include <atomic>
#include <cmath>
#include <cstddef>
#include <limits>
#include <memory>
#include <stdexcept>
#include <string>
#include <utility>
#include <vector>

#include <boost/config.hpp>
#include <boost/dll/alias.hpp>

#include <robot_nav_2d_utils/conversions.h>
#include <robot_nav_core2/local_planner.h>

namespace move_base2
{
namespace testing
{

constexpr double kBaseSpeed = 0.25;    ///< [m/s] lệnh nền khi chưa có trần nào
constexpr double kBaseYawRate = 0.40;  ///< [rad/s]

/**
 * @class TestLocalPlanner
 * @brief Local planner giả, hành vi cố định theo tham số dựng.
 */
class TestLocalPlanner : public robot_nav_core2::LocalPlanner
{
public:
  enum class Behavior
  {
    kOk,             ///< Sinh lệnh hợp lệ, phản ánh trần và vận tốc đo được.
    kNoCommand,      ///< computeVelocityCommands trả false.
    kNaN,            ///< Sinh lệnh chứa NaN — phải bị chặn tại biên.
    kThrow,          ///< Ném exception khi tính lệnh.
    kRefusesLimits,  ///< setTwistLinear/Angular trả false (planner không hỗ trợ đặt trần).
    kMarkerProbe,    ///< Đọc `maker_name` MỘT lần lúc initialize, mã hoá vào lệnh — mô phỏng
                     ///< getMaker() của docking planner để test đường re-init khi đổi marker.
    kFootprintProbe  ///< Mỗi initialize có generation mới; cần reapply goal/plan mới sinh lệnh.
  };

  explicit TestLocalPlanner(Behavior behavior) : behavior_(behavior)
  {
  }

  void initialize(robot::NodeHandle& parent, const std::string& name,
                  std::shared_ptr<tf3::BufferCore> /*tf*/,
                  robot_costmap_2d::Costmap2DROBOT* /*costmap*/) override
  {
    // Cố ý KHÔNG chạm tf hay costmap — xem chú thích đầu file.
    name_ = name;
    // Như PNKXDockingLocalPlanner::getMaker(): đọc đúng MỘT lần, không bao giờ đọc lại.
    parent.param("maker_name", marker_at_init_, std::string(""));
    if (behavior_ == Behavior::kFootprintProbe)
    {
      footprint_generation_ = ++footprint_probe_generation_;
    }
  }

  void setGoalPose(const robot_nav_2d_msgs::Pose2DStamped& /*goal_pose*/) override
  {
    saw_goal_ = true;
  }

  void setPlan(const robot_nav_2d_msgs::Path2D& path) override
  {
    plan_size_ = path.poses.size();
  }

  void getPlan(robot_nav_2d_msgs::Path2D& path) override
  {
    path = robot_nav_2d_msgs::Path2D();
  }

  void getGlobalPlan(robot_nav_2d_msgs::Path2D& path) override
  {
    path = robot_nav_2d_msgs::Path2D();
  }

  robot_nav_2d_msgs::Twist2DStamped computeVelocityCommands(
      const robot_nav_2d_msgs::Pose2DStamped& /*pose*/,
      const robot_nav_2d_msgs::Twist2D& velocity) override
  {
    robot_nav_2d_msgs::Twist2DStamped cmd;

    switch (behavior_)
    {
      case Behavior::kThrow:
        throw std::runtime_error("TestLocalPlanner was asked to throw an exception");
      case Behavior::kNoCommand:
        // Gen-2 không có cờ thành công/thất bại: "không sinh được lệnh" biểu đạt bằng exception.
        throw std::runtime_error("TestLocalPlanner: could not produce a command");
      case Behavior::kNaN:
        cmd.velocity.x = std::numeric_limits<double>::quiet_NaN();
        return cmd;
      case Behavior::kMarkerProbe:
        // Mã hoá marker đọc được lúc initialize vào lệnh — bảng cố định, test đối chiếu.
        cmd.velocity.x = marker_at_init_ == "dock_a" ? 0.11 : marker_at_init_ == "dock_b" ? 0.22 : 0.0;
        return cmd;
      case Behavior::kFootprintProbe:
        // Nếu refresh chỉ dựng instance mà quên setGoalPose/setPlan lại, probe trả 0 thay vì lệnh
        // mang generation mới. Như vậy test kiểm đồng thời cache footprint và khôi phục chặng.
        cmd.velocity.x = (saw_goal_ && plan_size_ != 0) ? 0.01 * footprint_generation_ : 0.0;
        return cmd;
      case Behavior::kOk:
      case Behavior::kRefusesLimits:
        break;
    }

    double linear = kBaseSpeed + velocity.x;
    if (has_limit_forward_)
    {
      linear = std::min(linear, limit_forward_);
    }

    double yaw = kBaseYawRate;
    if (has_limit_angular_)
    {
      yaw = std::min(yaw, limit_angular_);
    }

    cmd.velocity.x = linear;
    cmd.velocity.theta = yaw;
    return cmd;
  }

  bool isGoalReached(const robot_nav_2d_msgs::Pose2DStamped& /*pose*/,
                     const robot_nav_2d_msgs::Twist2D& /*velocity*/) override
  {
    return false;
  }

  bool setTwistLinear(robot_geometry_msgs::Vector3 linear) override
  {
    if (behavior_ == Behavior::kRefusesLimits)
    {
      return false;
    }
    // Dấu chọn chiều, đúng quy ước của interface gen-1.
    if (linear.x < 0.0)
    {
      limit_backward_ = linear.x;
    }
    else
    {
      limit_forward_ = linear.x;
      has_limit_forward_ = true;
    }
    return true;
  }

  robot_geometry_msgs::Vector3 getTwistLinear(bool direct) override
  {
    robot_geometry_msgs::Vector3 out;
    out.x = direct ? limit_forward_ : limit_backward_;
    return out;
  }

  bool setTwistAngular(robot_geometry_msgs::Vector3 angular) override
  {
    if (behavior_ == Behavior::kRefusesLimits)
    {
      return false;
    }
    limit_angular_ = angular.z;
    has_limit_angular_ = true;
    return true;
  }

  robot_geometry_msgs::Vector3 getTwistAngular(bool /*direct*/) override
  {
    robot_geometry_msgs::Vector3 out;
    out.z = limit_angular_;
    return out;
  }

private:
  Behavior behavior_;
  std::string name_;
  std::string marker_at_init_;  ///< `maker_name` tại thời điểm initialize — không bao giờ đọc lại.
  inline static std::atomic<unsigned int> footprint_probe_generation_{ 0 };
  unsigned int footprint_generation_ = 0;
  std::size_t plan_size_ = 0;
  bool saw_goal_ = false;
  double limit_forward_ = 0.0;   ///< [m/s]
  double limit_backward_ = 0.0;  ///< [m/s], âm
  double limit_angular_ = 0.0;   ///< [rad/s]
  bool has_limit_forward_ = false;
  bool has_limit_angular_ = false;
};

robot_nav_core2::LocalPlanner::Ptr createOk()
{
  return std::make_shared<TestLocalPlanner>(TestLocalPlanner::Behavior::kOk);
}

robot_nav_core2::LocalPlanner::Ptr createSecondary()
{
  return std::make_shared<TestLocalPlanner>(TestLocalPlanner::Behavior::kOk);
}

robot_nav_core2::LocalPlanner::Ptr createNoCommand()
{
  return std::make_shared<TestLocalPlanner>(TestLocalPlanner::Behavior::kNoCommand);
}

robot_nav_core2::LocalPlanner::Ptr createNaN()
{
  return std::make_shared<TestLocalPlanner>(TestLocalPlanner::Behavior::kNaN);
}

robot_nav_core2::LocalPlanner::Ptr createThrowing()
{
  return std::make_shared<TestLocalPlanner>(TestLocalPlanner::Behavior::kThrow);
}

robot_nav_core2::LocalPlanner::Ptr createRefusingLimits()
{
  return std::make_shared<TestLocalPlanner>(TestLocalPlanner::Behavior::kRefusesLimits);
}

robot_nav_core2::LocalPlanner::Ptr createMarkerProbe()
{
  return std::make_shared<TestLocalPlanner>(TestLocalPlanner::Behavior::kMarkerProbe);
}

robot_nav_core2::LocalPlanner::Ptr createFootprintProbe()
{
  return std::make_shared<TestLocalPlanner>(TestLocalPlanner::Behavior::kFootprintProbe);
}

}  // namespace testing
}  // namespace move_base2

BOOST_DLL_ALIAS(move_base2::testing::createOk, TestControllerOk)
BOOST_DLL_ALIAS(move_base2::testing::createSecondary, TestControllerSecondary)
BOOST_DLL_ALIAS(move_base2::testing::createNoCommand, TestControllerNoCommand)
BOOST_DLL_ALIAS(move_base2::testing::createNaN, TestControllerNaN)
BOOST_DLL_ALIAS(move_base2::testing::createThrowing, TestControllerThrowing)
BOOST_DLL_ALIAS(move_base2::testing::createRefusingLimits, TestControllerRefusesLimits)
BOOST_DLL_ALIAS(move_base2::testing::createMarkerProbe, TestControllerMarkerProbe)
BOOST_DLL_ALIAS(move_base2::testing::createFootprintProbe, TestControllerFootprintProbe)