first commit

test/plugins/test_global_planner.cpp

@@ -0,0 +1,128 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ * move_base2 — plugin global planner CHỈ dùng cho test.
+ *
+ * Bốn alias được export từ cùng một thư viện, mỗi alias là một hành vi mà `PlannerRunner` phải xử lý
+ * đúng. Chọn cách này thay vì một plugin đọc config: nó làm test không phụ thuộc vào cây config, và
+ * mỗi kịch bản gọi tên đúng thứ nó kiểm.
+ *
+ * Không alias nào chạm vào con trỏ costmap — đó là điều kiện để test truyền vào một con trỏ giả
+ * thay vì phải dựng `Costmap2DROBOT` thật (cần tf3::BufferCore và cây config đầy đủ).
+ *
+ * Author: DuongTD
+ *********************************************************************/
+#include <stdexcept>
+#include <string>
+#include <vector>
+
+#include <boost/config.hpp>
+#include <boost/dll/alias.hpp>
+#include <boost/make_shared.hpp>
+
+#include <robot_nav_core/base_global_planner.h>
+
+namespace move_base2
+{
+namespace testing
+{
+
+/// @brief Số pose trong plan mà `TestPlannerOk` sinh ra.
+constexpr std::size_t kPlanLength = 3;
+
+/**
+ * @class TestGlobalPlanner
+ * @brief Planner giả, hành vi cố định theo tham số dựng.
+ */
+class TestGlobalPlanner : public robot_nav_core::BaseGlobalPlanner
+{
+public:
+  enum class Behavior
+  {
+    kOk,          ///< Trả plan hợp lệ.
+    kEmptyPlan,   ///< Trả true kèm plan RỖNG — bẫy mà PlannerRunner phải quy về false.
+    kThrow,       ///< Ném exception giữa lúc lập plan.
+    kInitFails    ///< initialize() trả false.
+  };
+
+  explicit TestGlobalPlanner(Behavior behavior) : behavior_(behavior)
+  {
+  }
+
+  bool initialize(std::string name, robot_costmap_2d::Costmap2DROBOT* /*costmap_robot*/) override
+  {
+    // Cố ý KHÔNG chạm costmap_robot — xem chú thích đầu file.
+    name_ = std::move(name);
+    return behavior_ != Behavior::kInitFails;
+  }
+
+  bool makePlan(const robot_geometry_msgs::PoseStamped& start,
+                const robot_geometry_msgs::PoseStamped& goal,
+                std::vector<robot_geometry_msgs::PoseStamped>& plan) override
+  {
+    ++call_count_;
+
+    if (behavior_ == Behavior::kThrow)
+    {
+      throw std::runtime_error("TestGlobalPlanner được yêu cầu ném exception");
+    }
+
+    plan.clear();
+    if (behavior_ == Behavior::kEmptyPlan)
+    {
+      return true;  // true + rỗng: đúng thứ contract PlannerPort cấm lọt qua.
+    }
+
+    plan.push_back(start);
+    for (std::size_t i = plan.size(); i + 1 < kPlanLength; ++i)
+    {
+      plan.push_back(start);
+    }
+    plan.push_back(goal);
+    return true;
+  }
+
+  bool makePlan(const robot_protocol_msgs::Order& /*msg*/,
+                const robot_geometry_msgs::PoseStamped& start,
+                const robot_geometry_msgs::PoseStamped& goal,
+                std::vector<robot_geometry_msgs::PoseStamped>& plan) override
+  {
+    saw_order_ = true;
+    return makePlan(start, goal, plan);
+  }
+
+private:
+  Behavior behavior_;
+  std::string name_;
+  std::size_t call_count_ = 0;
+  bool saw_order_ = false;
+};
+
+robot_nav_core::BaseGlobalPlanner::Ptr createOk()
+{
+  return std::make_shared<TestGlobalPlanner>(TestGlobalPlanner::Behavior::kOk);
+}
+
+robot_nav_core::BaseGlobalPlanner::Ptr createEmptyPlan()
+{
+  return std::make_shared<TestGlobalPlanner>(TestGlobalPlanner::Behavior::kEmptyPlan);
+}
+
+robot_nav_core::BaseGlobalPlanner::Ptr createThrowing()
+{
+  return std::make_shared<TestGlobalPlanner>(TestGlobalPlanner::Behavior::kThrow);
+}
+
+robot_nav_core::BaseGlobalPlanner::Ptr createInitFailing()
+{
+  return std::make_shared<TestGlobalPlanner>(TestGlobalPlanner::Behavior::kInitFails);
+}
+
+}  // namespace testing
+}  // namespace move_base2
+
+BOOST_DLL_ALIAS(move_base2::testing::createOk, TestPlannerOk)
+BOOST_DLL_ALIAS(move_base2::testing::createEmptyPlan, TestPlannerEmptyPlan)
+BOOST_DLL_ALIAS(move_base2::testing::createThrowing, TestPlannerThrowing)
+BOOST_DLL_ALIAS(move_base2::testing::createInitFailing, TestPlannerInitFails)