103 lines
2.0 KiB
C++
103 lines
2.0 KiB
C++
/*********************************************************************
|
|
* recovery_core — validate + đọc RecoveryConfig.
|
|
*
|
|
* Author: DuongTD
|
|
*********************************************************************/
|
|
#include <recovery_core/recovery_config.h>
|
|
|
|
#include <cmath>
|
|
#include <string>
|
|
|
|
#include <robot/robot.h>
|
|
|
|
namespace recovery_core
|
|
{
|
|
namespace
|
|
{
|
|
|
|
constexpr double kDefaultControlFrequency = 20.0;
|
|
constexpr double kDefaultTimeout = 0.0;
|
|
|
|
void appendError(std::string* error, const std::string& message)
|
|
{
|
|
if (error == nullptr)
|
|
{
|
|
return;
|
|
}
|
|
|
|
if (!error->empty())
|
|
{
|
|
*error += "; ";
|
|
}
|
|
*error += message;
|
|
}
|
|
|
|
bool invalidControlFrequency(double value)
|
|
{
|
|
return !std::isfinite(value) || value <= 0.0;
|
|
}
|
|
|
|
bool invalidTimeout(double value)
|
|
{
|
|
return !std::isfinite(value) || value < 0.0;
|
|
}
|
|
|
|
} // namespace
|
|
|
|
bool RecoveryConfig::validate(std::string* error) const
|
|
{
|
|
if (error != nullptr)
|
|
{
|
|
error->clear();
|
|
}
|
|
|
|
bool valid = true;
|
|
if (invalidControlFrequency(control_frequency))
|
|
{
|
|
appendError(error, "control_frequency must be finite and > 0 Hz");
|
|
valid = false;
|
|
}
|
|
|
|
if (invalidTimeout(timeout))
|
|
{
|
|
appendError(error, "timeout must be finite and >= 0 s");
|
|
valid = false;
|
|
}
|
|
|
|
return valid;
|
|
}
|
|
|
|
RecoveryConfig RecoveryConfig::fromNodeHandle(robot::NodeHandle& nh)
|
|
{
|
|
RecoveryConfig config;
|
|
nh.param("control_frequency", config.control_frequency, kDefaultControlFrequency);
|
|
nh.param("timeout", config.timeout, kDefaultTimeout);
|
|
|
|
std::string error;
|
|
if (config.validate(&error))
|
|
{
|
|
return config;
|
|
}
|
|
|
|
robot::log_warning("[recovery_core] Invalid common recovery config: %s. "
|
|
"Replacing invalid values with defaults.",
|
|
error.c_str());
|
|
|
|
if (invalidControlFrequency(config.control_frequency))
|
|
{
|
|
config.control_frequency = kDefaultControlFrequency;
|
|
}
|
|
if (invalidTimeout(config.timeout))
|
|
{
|
|
config.timeout = kDefaultTimeout;
|
|
}
|
|
|
|
if (!config.validate(nullptr))
|
|
{
|
|
return RecoveryConfig{};
|
|
}
|
|
return config;
|
|
}
|
|
|
|
} // namespace recovery_core
|