temporary storage

test/CMakeLists.txt

@@ -0,0 +1,26 @@
+# Test target cho recovery_core contract.
+# Build khi cấu hình standalone và khi catkin bật test target của package.
+
+add_executable(recovery_core_plugin_loader_test plugin_loader_contract_test.cpp)
+
+target_include_directories(recovery_core_plugin_loader_test
+  PRIVATE
+    ${CMAKE_CURRENT_SOURCE_DIR}
+    ${STANDALONE_INCLUDE_DIRS}
+)
+
+target_link_libraries(recovery_core_plugin_loader_test
+  PRIVATE
+    recovery_core
+    yaml-cpp
+    ${Boost_LIBRARIES}
+    ${CMAKE_THREAD_LIBS_INIT}
+    ${CMAKE_DL_LIBS}
+)
+
+add_dependencies(recovery_core_plugin_loader_test ${RECOVERY_CORE_PLUGIN_TARGETS})
+
+target_compile_definitions(recovery_core_plugin_loader_test
+  PRIVATE
+    RECOVERY_CORE_PLUGIN_DIR=\"$<TARGET_FILE_DIR:recovery_core_rotate_recovery>\"
+)

test/plugin_loader_contract_test.cpp

@@ -0,0 +1,240 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ * recovery_core — Boost.DLL plugin contract test.
+ *
+ * Author: DuongTD
+ *********************************************************************/
+
+#include <recovery_core/recovery_behavior.h>
+
+#include <cmath>
+#include <cstdlib>
+#include <iostream>
+#include <string>
+#include <vector>
+#include <robot/robot.h>
+#include <robot_xmlrpcpp/XmlRpcValue.h>
+
+#include <boost/dll/import.hpp>
+#include <boost/dll/shared_library.hpp>
+#include <boost/filesystem/path.hpp>
+
+namespace
+{
+using Factory = recovery_core::RecoveryBehavior::RecoveryBehaviorPtr();
+
+struct PluginConfig
+{
+    std::string name;
+    std::string type;
+};
+std::vector<boost::dll::shared_library> libraries_;
+std::vector<recovery_core::RecoveryBehavior::RecoveryBehaviorPtr> creators_;
+std::vector<std::string> name_plugins_;
+
+std::vector<robot_geometry_msgs::PoseStamped> global_path_;
+
+void expect(bool condition, const std::string& message)
+{
+  if (!condition)
+  {
+    std::cerr << "[FAIL] " << message << std::endl;
+    std::exit(1);
+  }
+}
+
+std::vector<PluginConfig> getListRecoveryPlugins()
+{
+
+std::vector<PluginConfig> my_list;
+
+robot::NodeHandle priv_nh;
+
+if (priv_nh.hasParam("recovery_behaviors"))
+{
+    YAML::Node my_plugins = priv_nh.getParamValue("recovery_behaviors");
+
+    if (my_plugins.IsDefined() && my_plugins.IsSequence())
+    {
+       std::set<std::string> name_plugins;
+        for (std::size_t i = 0; i < my_plugins.size(); ++i)
+        {
+            YAML::Node plugin_i = my_plugins[i];
+
+            // 1. Phải là map
+            if (!plugin_i.IsMap())
+            {
+                std::cerr<< "Recovery plugin at index " << i << " must be a map." << std::endl;
+                continue;
+            }
+
+            // 2. Phải có name và type
+            if (!plugin_i["name"].IsDefined() || !plugin_i["type"].IsDefined())
+            {
+                std::cerr << "Recovery plugin at index " << i << " must have 'name' and 'type'." << std::endl;
+                continue;
+            }
+
+            PluginConfig p;
+
+            try
+            {
+                p.name = plugin_i["name"].as<std::string>();
+                p.type = plugin_i["type"].as<std::string>();
+            }
+            catch (const YAML::Exception& e)
+            {
+                std::cerr << "Invalid recovery plugin at index " << i << ": " << e.what() << std::endl;
+                continue;
+            }
+
+            // 3. Kiểm tra duplicate name
+            const auto result = name_plugins.insert(p.name);
+
+            if (!result.second)
+            {
+                std::cerr << "A recovery plugin with name '" << p.name << "' already exists." << std::endl;
+                continue;
+            }
+
+            // 4. Chỉ thêm sau khi validate hoàn toàn
+            my_list.push_back(p);
+            name_plugins_.push_back(p.name);
+
+            robot::log_warning("Load plugin: name: %s, type: %s", p.name.c_str(), p.type.c_str());
+        }
+    }
+    else
+    {
+        std::cerr << "'recovery_behaviors' must be a sequence." << std::endl;
+    }
+}
+
+return my_list;
+}
+
+void testLoadPlugins()
+{
+       std::vector<PluginConfig> my_list_plugin = getListRecoveryPlugins();
+       if(my_list_plugin.empty())
+       {
+              robot::log_error("No recovery plugins found in configuration.");
+              return;
+       }
+       for(const auto& plugin : my_list_plugin)
+       {
+              robot::PluginLoaderHelper loader;
+              std::string path_file_so = loader.findLibraryPath(plugin.type);
+              if(path_file_so == "")
+              {
+                     robot::log_error("Cannot find library for recovery behavior type '%s'", plugin.type.c_str());
+                     return;
+              }
+              robot::log_info("Loading recovery behavior type '%s' from '%s'", plugin.type.c_str(), path_file_so.c_str());
+              try
+              {
+                     // 1. Load library vào local handle.
+                     boost::dll::shared_library library(path_file_so);
+
+                     // 2. Lấy factory alias.
+                     auto& factory = library.get_alias<Factory>(plugin.type);
+
+                     // 3. Factory tạo behavior object.
+                     recovery_core::RecoveryBehavior::RecoveryBehaviorPtr behavior = factory();
+
+                     if (!behavior)
+                     {
+                            robot::log_error("Factory returned nullptr for '%s'", plugin.type.c_str());
+                            return;
+                     }
+
+                     // 4. Chuyển quyền giữ library vào storage sống lâu dài.
+                     libraries_.push_back(std::move(library));
+                     behavior->initialize(plugin.name, nullptr, &global_path_, nullptr, nullptr);
+                     creators_.push_back(behavior);
+              }
+              catch (const std::exception& e)
+              {
+                     robot::log_error("Failed to load recovery behavior '%s': %s", plugin.type.c_str(), e.what());
+                     return;
+              }
+       //     expect(static_cast<bool>(behavior), "Failed to load plugin: " + plugin.name + " of type: " + plugin.type);
+       }
+}
+
+void testRotatePlugin()
+{
+for(const auto& behavior : creators_)
+ {
+     if(behavior->getNameRecoveryBehavior() == "rotation_rc")
+     {
+         const recovery_core::RecoveryResult first = behavior->computeCommand(0.1);
+
+         expect(first.status == recovery_core::RecoveryStatus::kRunning,
+                "rotate first cycle must be running");
+         expect(first.output_type == recovery_core::RecoveryOutputType::kVelocity,
+                "rotate must return velocity output");
+         expect(first.command.angular.z > 0.0, "rotate must command positive angular.z by default");
+
+         recovery_core::RecoveryResult last = first;
+         for (int i = 0; i < 100 && last.status == recovery_core::RecoveryStatus::kRunning; ++i)
+         {
+           last = behavior->computeCommand(0.1);
+         }
+
+         expect(last.status == recovery_core::RecoveryStatus::kSucceeded,
+                "rotate must finish within bounded cycles");
+         expect(last.output_type == recovery_core::RecoveryOutputType::kVelocity,
+                "rotate final result must still be a velocity output");
+         expect(std::abs(last.command.angular.z) < 1e-9,
+                "rotate must return a zero angular command when complete");
+     }
+ }
+}
+
+void testRegenPathPlugin()
+{
+robot_geometry_msgs::PoseStamped pose1;
+pose1.pose.position.x = 1.0;
+robot_geometry_msgs::PoseStamped pose2;
+pose2.pose.position.x = 2.0;
+global_path_.push_back(pose1);
+global_path_.push_back(pose2);
+
+
+ for(const auto& behavior : creators_)
+ {
+     if(behavior->getNameRecoveryBehavior() == "regen_path_rc")
+     {
+         const recovery_core::RecoveryResult result = behavior->runBehavior();
+
+         expect(result.status == recovery_core::RecoveryStatus::kSucceeded,
+                "regen path must succeed with a non-empty global path");
+         expect(result.output_type == recovery_core::RecoveryOutputType::kPath,
+                "regen path must return path output");
+         expect(result.path.poses.size() == global_path_.size(),
+                "regen path must preserve the global path size");
+       global_path_ = result.path.poses;
+     }
+ }
+}
+
+}  // namespace
+
+int main()
+{
+  testLoadPlugins();
+  if(creators_.empty()) return 0;
+//   for(auto& behavior : creators_)
+//   {
+//     std::cout<<behavior->getNameRecoveryBehavior()<<std::endl;
+//   }
+
+  testRegenPathPlugin();
+  testRotatePlugin();
+
+  std::cout << "[PASS] recovery_core plugin loader contract" << std::endl;
+  return 0;
+}