refactored

toppers · Dec 19, 2024 · 9620abb · 9620abb
1 parent 297cfbd
commit 9620abb
Show file tree

Hide file tree

Showing 3 changed files with 82 additions and 36 deletions.
diff --git a/drone_control/common/include/drone_controller.hpp b/drone_control/common/include/drone_controller.hpp
@@ -21,7 +21,8 @@ class DroneController {
         loader() 
     {
         if (HakoControllerParamLoader::is_exist_envpath()) {
-            loader.loadParameters();
+            auto param_data = HakoControllerParamLoader::get_controller_param_filedata();
+            loader.reload(param_data);
         } else {
             throw std::runtime_error("Parameter file is not found on HAKO_CONTROLLER_PARAM_FILE");
         }
@@ -31,7 +32,10 @@ class DroneController {
         angle = std::make_unique<DroneAngleController>(loader);
     }
     void reset() {
-        this->loader.reload();
+        if (HakoControllerParamLoader::is_exist_envpath()) {
+            auto param_data = HakoControllerParamLoader::get_controller_param_filedata();
+            this->loader.reload(param_data);
+        }
         alt->reset();
         pos->reset();
         head->reset();

diff --git a/drone_control/common/include/drone_radio_controller.hpp b/drone_control/common/include/drone_radio_controller.hpp
@@ -62,7 +62,8 @@ class DroneRadioController {
         loader() 
     {
         if (HakoControllerParamLoader::is_exist_envpath()) {
-            loader.loadParameters();
+            auto param_data = HakoControllerParamLoader::get_controller_param_filedata();
+            loader.reload(param_data);
         } else {
             throw std::runtime_error("Parameter file is not found on HAKO_CONTROLLER_PARAM_FILE");
         }
@@ -74,7 +75,10 @@ class DroneRadioController {
         this->loadParameters();
     }
     void reset() {
-        this->loader.reload();
+        if (HakoControllerParamLoader::is_exist_envpath()) {
+            auto param_data = HakoControllerParamLoader::get_controller_param_filedata();
+            this->loader.reload(param_data);
+        }
         r_altitude_initialized = false;
         r_altitude = 0;
         alt_time = 0;

diff --git a/drone_control/common/include/hako_controller_param_loader.hpp b/drone_control/common/include/hako_controller_param_loader.hpp
@@ -7,7 +7,7 @@
 #include <sstream>
 #include <iostream>
 #include <stdexcept>
-#include <cstdlib> // for std::getenv
+#include <cstdlib>
 #include "flight_controller_types.hpp"
 
 class HakoControllerParamLoader {
@@ -16,33 +16,46 @@ class HakoControllerParamLoader {
         const char* env_p = std::getenv("HAKO_CONTROLLER_PARAM_FILE");
         return env_p != nullptr && env_p[0] != '\0';
     }
-
-    HakoControllerParamLoader() {
+    static std::string get_controller_param_filedata()
+    {
         const char* env_p = std::getenv("HAKO_CONTROLLER_PARAM_FILE");
         if (env_p == nullptr || env_p[0] == '\0') {
             throw std::runtime_error("Environment variable HAKO_CONTROLLER_PARAM_FILE is not set or is empty");
         }
-        std::cout << "controller param path: " << env_p << std::endl;
-        filename = std::string(env_p);
-        loadParameters();
+        std::ifstream file(env_p);
+        if (!file.is_open()) {
+            throw std::runtime_error("Failed to open file: " + std::string(env_p));
+        }
+        std::string filedata((std::istreambuf_iterator<char>(file)), std::istreambuf_iterator<char>());
+        return filedata;
     }
-
-    HakoControllerParamLoader(const std::string& filename) : filename(filename) {
-        loadParameters();
+    HakoControllerParamLoader() {
+    }
+    HakoControllerParamLoader(const std::string& input) {
+        loadParametersFromString(input);
     }
 
-    void loadParameters() {
-        std::ifstream file(filename);
-        if (!file.is_open()) {
-            throw std::runtime_error("Failed to open file: " + filename);
+    void reload(const std::string &input) {
+        std::cout << "Reloading parameters from string" << std::endl;
+        loadParametersFromString(input);
+    }
+    double getParameter(const std::string& paramName) const {
+        auto it = parameters.find(paramName);
+        if (it != parameters.end()) {
+            std::cout << paramName << ": " << it->second << std::endl;
+            return it->second;
+        } else {
+            throw std::runtime_error("Parameter not found: " + paramName);
         }
+    }
 
-        // パラメータをクリアして再読み込み
-        parameters.clear();
+private:
+    std::unordered_map<std::string, double> parameters;
 
+    void parseStream(std::istream& stream) {
+        validateAndParseStream(stream);
         std::string line;
-        while (std::getline(file, line)) {
-            // コメント行をスキップ
+        while (std::getline(stream, line)) {
             if (line.empty() || line[0] == '#') {
                 continue;
             }
@@ -54,28 +67,53 @@ class HakoControllerParamLoader {
                 parameters[param] = value;
             }
         }
-
-        file.close();
     }
 
-    void reload() {
-        std::cout << "Reloading parameters from: " << filename << std::endl;
-        loadParameters();
+    void loadParametersFromString(const std::string& input) {
+        std::istringstream iss(input);
+        parameters.clear();
+        parseStream(iss);
+    }    
+    void validateAndParseStream(std::istream& stream) {
+        std::string line;
+        int lineNumber = 0;
+
+        while (std::getline(stream, line)) {
+            lineNumber++;
+
+            if (line.empty() || line[0] == '#') {
+                continue;
+            }
+
+            if (!validateLine(line)) {
+                throw std::runtime_error("Invalid parameter format at line " + std::to_string(lineNumber) + ": " + line);
+            }
+
+            std::istringstream iss(line);
+            std::string param;
+            double value;
+            iss >> param >> value;
+            parameters[param] = value;
+        }
     }
 
-    double getParameter(const std::string& paramName) const {
-        auto it = parameters.find(paramName);
-        if (it != parameters.end()) {
-            std::cout << paramName << ": " << it->second << std::endl;
-            return it->second;
-        } else {
-            throw std::runtime_error("Parameter not found: " + paramName);
+    bool validateLine(const std::string& line) const {
+        std::istringstream iss(line);
+        std::string param;
+        double value;
+
+        if (!(iss >> param >> value)) {
+            return false;
+        }
+
+        std::string extra;
+        if (iss >> extra) {
+            return false;
         }
+
+        return true;
     }
 
-private:
-    std::string filename;
-    std::unordered_map<std::string, double> parameters;
 };
 
 #endif // HAKO_CONTROLLER_PARAM_LOADER_HPP