temporary storage
This commit is contained in:
240
test/plugin_loader_contract_test.cpp
Normal file
240
test/plugin_loader_contract_test.cpp
Normal file
@@ -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;
|
||||
}
|
||||
Reference in New Issue
Block a user