temporary storage

src/recovery_config.cpp

@@ -0,0 +1,102 @@
+/*********************************************************************
+ * 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