288 lines
9.6 KiB
C++
288 lines
9.6 KiB
C++
/*********************************************************************
|
|
*
|
|
* 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));
|
|
recovery_core::RecoveryContext ctx;
|
|
ctx.global_path = &global_path_;
|
|
behavior->configure(plugin.name, ctx);
|
|
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")
|
|
{
|
|
// Caller đặt góc quay RUNTIME = pi/2 (90 độ) cho lượt này.
|
|
recovery_core::RecoveryGoal goal;
|
|
goal.angle = 1.57079632679;
|
|
|
|
const recovery_core::RecoveryResult started = behavior->start(goal);
|
|
expect(started.status == recovery_core::RecoveryStatus::kRunning,
|
|
"rotate must be running right after start");
|
|
|
|
const recovery_core::RecoveryResult first = behavior->update();
|
|
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 for +angle");
|
|
expect(first.progress >= 0.0 && first.progress < 1.0,
|
|
"rotate progress must advance within [0,1)");
|
|
expect(first.remaining > 0.0, "rotate must report remaining angle while running");
|
|
|
|
recovery_core::RecoveryResult last = first;
|
|
for (int i = 0; i < 100 && last.status == recovery_core::RecoveryStatus::kRunning; ++i)
|
|
{
|
|
last = behavior->update();
|
|
}
|
|
|
|
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");
|
|
expect(std::abs(last.progress - 1.0) < 1e-9, "rotate must report full progress on success");
|
|
expect(last.remaining < 1e-9, "rotate must report zero remaining on success");
|
|
}
|
|
}
|
|
}
|
|
|
|
void testBackupPlugin()
|
|
{
|
|
for(const auto& behavior : creators_)
|
|
{
|
|
if(behavior->getNameRecoveryBehavior() == "backward_rc")
|
|
{
|
|
// Caller đặt khoảng lùi RUNTIME = 0.3 m cho lượt này.
|
|
recovery_core::RecoveryGoal goal;
|
|
goal.distance = 0.3;
|
|
|
|
behavior->start(goal);
|
|
const recovery_core::RecoveryResult first = behavior->update();
|
|
expect(first.status == recovery_core::RecoveryStatus::kRunning,
|
|
"backup first cycle must be running");
|
|
expect(first.command.linear.x < 0.0, "backup must command negative linear.x");
|
|
|
|
recovery_core::RecoveryResult last = first;
|
|
for (int i = 0; i < 1000 && last.status == recovery_core::RecoveryStatus::kRunning; ++i)
|
|
{
|
|
last = behavior->update();
|
|
}
|
|
|
|
expect(last.status == recovery_core::RecoveryStatus::kSucceeded,
|
|
"backup must finish within bounded cycles");
|
|
expect(std::abs(last.command.linear.x) < 1e-9,
|
|
"backup must return a zero command when complete");
|
|
expect(std::abs(last.progress - 1.0) < 1e-9, "backup must report full progress on success");
|
|
}
|
|
}
|
|
}
|
|
|
|
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")
|
|
{
|
|
behavior->start(recovery_core::RecoveryGoal());
|
|
const recovery_core::RecoveryResult result = behavior->update();
|
|
|
|
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();
|
|
testBackupPlugin();
|
|
|
|
std::cout << "[PASS] recovery_core plugin loader contract" << std::endl;
|
|
return 0;
|
|
}
|