first commit

test/plugins/test_local_planner.cpp

@@ -0,0 +1,216 @@
+/*********************************************************************
+ *
+ * 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 <cmath>
+#include <limits>
+#include <memory>
+#include <stdexcept>
+#include <string>
+#include <utility>
+#include <vector>
+
+#include <boost/config.hpp>
+#include <boost/dll/alias.hpp>
+
+#include <robot_nav_core/base_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_core::BaseLocalPlanner
+{
+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).
+  };
+
+  explicit TestLocalPlanner(Behavior behavior) : behavior_(behavior)
+  {
+  }
+
+  void initialize(std::string name, tf3::BufferCore* /*tf*/,
+                  robot_costmap_2d::Costmap2DROBOT* /*costmap_robot*/) override
+  {
+    // Cố ý KHÔNG chạm tf hay costmap — xem chú thích đầu file.
+    name_ = std::move(name);
+  }
+
+  bool setPlan(const std::vector<robot_geometry_msgs::PoseStamped>& plan) override
+  {
+    return !plan.empty();
+  }
+
+  void getPlan(std::vector<robot_geometry_msgs::PoseStamped>& path) override
+  {
+    path.clear();
+  }
+
+  void getGlobalPlan(std::vector<robot_geometry_msgs::PoseStamped>& path) override
+  {
+    path.clear();
+  }
+
+  bool computeVelocityCommands(const robot_geometry_msgs::Twist& velocity,
+                               robot_geometry_msgs::Twist& cmd_vel) override
+  {
+    switch (behavior_)
+    {
+      case Behavior::kThrow:
+        throw std::runtime_error("TestLocalPlanner được yêu cầu ném exception");
+      case Behavior::kNoCommand:
+        return false;
+      case Behavior::kNaN:
+        cmd_vel.linear.x = std::numeric_limits<double>::quiet_NaN();
+        return true;
+      case Behavior::kOk:
+      case Behavior::kRefusesLimits:
+        break;
+    }
+
+    double linear = kBaseSpeed + velocity.linear.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_vel.linear.x = linear;
+    cmd_vel.angular.z = yaw;
+    return true;
+  }
+
+  bool isGoalReached() 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_;
+  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_core::BaseLocalPlanner::Ptr createOk()
+{
+  return std::make_shared<TestLocalPlanner>(TestLocalPlanner::Behavior::kOk);
+}
+
+robot_nav_core::BaseLocalPlanner::Ptr createSecondary()
+{
+  return std::make_shared<TestLocalPlanner>(TestLocalPlanner::Behavior::kOk);
+}
+
+robot_nav_core::BaseLocalPlanner::Ptr createNoCommand()
+{
+  return std::make_shared<TestLocalPlanner>(TestLocalPlanner::Behavior::kNoCommand);
+}
+
+robot_nav_core::BaseLocalPlanner::Ptr createNaN()
+{
+  return std::make_shared<TestLocalPlanner>(TestLocalPlanner::Behavior::kNaN);
+}
+
+robot_nav_core::BaseLocalPlanner::Ptr createThrowing()
+{
+  return std::make_shared<TestLocalPlanner>(TestLocalPlanner::Behavior::kThrow);
+}
+
+robot_nav_core::BaseLocalPlanner::Ptr createRefusingLimits()
+{
+  return std::make_shared<TestLocalPlanner>(TestLocalPlanner::Behavior::kRefusesLimits);
+}
+
+}  // 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)