From bdd78c5f1ee7d3906799de77e473e11455a479e1 Mon Sep 17 00:00:00 2001 From: HiepLM Date: Thu, 11 Dec 2025 11:11:32 +0700 Subject: [PATCH] update --- config/{ => mprim}/costmap_common_params.yaml | 0 config/{ => mprim}/costmap_global_params.yaml | 0 ...lobal_params_plugins_no_virtual_walls.yaml | 0 config/{ => mprim}/costmap_local_params.yaml | 0 .../{ => mprim}/move_base_common_params.yaml | 0 .../pnkx_local_planner_params.yaml | 0 .../{ => mprim}/two_points_global_params.yaml | 0 config1/two_points_global_params.yaml | 3 + examples/NavigationExample/libnav_c_api.so | Bin 161112 -> 161112 bytes src/Libraries/robot_cpp/.gitignore | 14 - src/Libraries/robot_cpp/CMakeLists.txt | 117 -- src/Libraries/robot_cpp/LICENSE | 5 - src/Libraries/robot_cpp/README.md | 2 - .../robot_cpp/include/robot/console.h | 89 - .../robot_cpp/include/robot/node_handle.h | 156 -- src/Libraries/robot_cpp/src/console.cpp | 207 --- src/Libraries/robot_cpp/src/node_handle.cpp | 1445 ----------------- .../robot_cpp/test/test_node_handle.cpp | 839 ---------- .../Packages/move_base/src/move_base.cpp | 2 +- 19 files changed, 4 insertions(+), 2875 deletions(-) rename config/{ => mprim}/costmap_common_params.yaml (100%) rename config/{ => mprim}/costmap_global_params.yaml (100%) rename config/{ => mprim}/costmap_global_params_plugins_no_virtual_walls.yaml (100%) rename config/{ => mprim}/costmap_local_params.yaml (100%) rename config/{ => mprim}/move_base_common_params.yaml (100%) rename config/{ => mprim}/pnkx_local_planner_params.yaml (100%) rename config/{ => mprim}/two_points_global_params.yaml (100%) create mode 100644 config1/two_points_global_params.yaml delete mode 100644 src/Libraries/robot_cpp/.gitignore delete mode 100644 src/Libraries/robot_cpp/CMakeLists.txt delete mode 100644 src/Libraries/robot_cpp/LICENSE delete mode 100644 src/Libraries/robot_cpp/README.md delete mode 100644 src/Libraries/robot_cpp/include/robot/console.h delete mode 100644 src/Libraries/robot_cpp/include/robot/node_handle.h delete mode 100644 src/Libraries/robot_cpp/src/console.cpp delete mode 100644 src/Libraries/robot_cpp/src/node_handle.cpp delete mode 100644 src/Libraries/robot_cpp/test/test_node_handle.cpp diff --git a/config/costmap_common_params.yaml b/config/mprim/costmap_common_params.yaml similarity index 100% rename from config/costmap_common_params.yaml rename to config/mprim/costmap_common_params.yaml diff --git a/config/costmap_global_params.yaml b/config/mprim/costmap_global_params.yaml similarity index 100% rename from config/costmap_global_params.yaml rename to config/mprim/costmap_global_params.yaml diff --git a/config/costmap_global_params_plugins_no_virtual_walls.yaml b/config/mprim/costmap_global_params_plugins_no_virtual_walls.yaml similarity index 100% rename from config/costmap_global_params_plugins_no_virtual_walls.yaml rename to config/mprim/costmap_global_params_plugins_no_virtual_walls.yaml diff --git a/config/costmap_local_params.yaml b/config/mprim/costmap_local_params.yaml similarity index 100% rename from config/costmap_local_params.yaml rename to config/mprim/costmap_local_params.yaml diff --git a/config/move_base_common_params.yaml b/config/mprim/move_base_common_params.yaml similarity index 100% rename from config/move_base_common_params.yaml rename to config/mprim/move_base_common_params.yaml diff --git a/config/pnkx_local_planner_params.yaml b/config/mprim/pnkx_local_planner_params.yaml similarity index 100% rename from config/pnkx_local_planner_params.yaml rename to config/mprim/pnkx_local_planner_params.yaml diff --git a/config/two_points_global_params.yaml b/config/mprim/two_points_global_params.yaml similarity index 100% rename from config/two_points_global_params.yaml rename to config/mprim/two_points_global_params.yaml diff --git a/config1/two_points_global_params.yaml b/config1/two_points_global_params.yaml new file mode 100644 index 0000000..ca0b60f --- /dev/null +++ b/config1/two_points_global_params.yaml @@ -0,0 +1,3 @@ +base_global_planner: TwoPointsPlanner +TwoPointsPlanner: + lethal_obstacle: 20 \ No newline at end of file diff --git a/examples/NavigationExample/libnav_c_api.so b/examples/NavigationExample/libnav_c_api.so index 2eb66c6a490ac47a365bdd23bf966c86caac6c2a..4c1e0d17128b5f31995297a3f870f924f65c649a 100755 GIT binary patch delta 54 zcmccdiu1-R&J8!2gsU#hm&koN$FTSFGFQW1raDHgBb(nash(w$VBIWy;Rz3rZvJ^^ K`_DU!@iPF&`WYku delta 54 zcmccdiu1-R&J8!2griowv9{V>(VrPw*qA6W`@dob$L2Ros%M#guxu8-@Pr3QH~+k| K{pTIV_!$7T^%>v* diff --git a/src/Libraries/robot_cpp/.gitignore b/src/Libraries/robot_cpp/.gitignore deleted file mode 100644 index 8c2b884..0000000 --- a/src/Libraries/robot_cpp/.gitignore +++ /dev/null @@ -1,14 +0,0 @@ -# ---> VisualStudioCode -.vscode/* -!.vscode/settings.json -!.vscode/tasks.json -!.vscode/launch.json -!.vscode/extensions.json -!.vscode/*.code-snippets - -# Local History for Visual Studio Code -.history/ - -# Built Visual Studio Code Extensions -*.vsix - diff --git a/src/Libraries/robot_cpp/CMakeLists.txt b/src/Libraries/robot_cpp/CMakeLists.txt deleted file mode 100644 index 56b80e1..0000000 --- a/src/Libraries/robot_cpp/CMakeLists.txt +++ /dev/null @@ -1,117 +0,0 @@ -cmake_minimum_required(VERSION 3.10) -project(robot_cpp) - -set(CMAKE_CXX_STANDARD 17) -set(CMAKE_CXX_STANDARD_REQUIRED ON) -set(CMAKE_POSITION_INDEPENDENT_CODE ON) - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic -fPIC) -endif() - -add_library(${PROJECT_NAME}_node_handle SHARED - src/node_handle.cpp -) - -target_include_directories(${PROJECT_NAME}_node_handle - PUBLIC - $ - $ -) - -target_link_libraries(${PROJECT_NAME}_node_handle - PUBLIC - yaml-cpp - xmlrpcpp -) - -# Link filesystem library if needed (for GCC < 9 or certain configurations) -if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU" AND CMAKE_CXX_COMPILER_VERSION VERSION_LESS "9.0") - target_link_libraries(${PROJECT_NAME}_node_handle PRIVATE stdc++fs) -elseif(CMAKE_CXX_COMPILER_ID MATCHES "Clang") - # Clang may need c++fs on some systems - find_library(FILESYSTEM_LIB c++fs) - if(FILESYSTEM_LIB) - target_link_libraries(${PROJECT_NAME}_node_handle PRIVATE ${FILESYSTEM_LIB}) - endif() -endif() - -install(TARGETS ${PROJECT_NAME}_node_handle - EXPORT ${PROJECT_NAME}_node_handle-targets - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION bin) - -install(DIRECTORY include/ - DESTINATION include - FILES_MATCHING PATTERN "*.h") - -install(EXPORT ${PROJECT_NAME}_node_handle-targets - NAMESPACE robot:: - DESTINATION lib/cmake/${PROJECT_NAME}_node_handle) - -# Create alias for easier usage -add_library(robot::node_handle ALIAS ${PROJECT_NAME}_node_handle) - - -add_library(${PROJECT_NAME}_console SHARED - src/console.cpp -) - -target_include_directories(${PROJECT_NAME}_console - PUBLIC - $ - $ -) - -# Console library only needs standard C++ library, no external dependencies needed - -install(TARGETS ${PROJECT_NAME}_console - EXPORT ${PROJECT_NAME}_console-targets - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION bin) - -install(EXPORT ${PROJECT_NAME}_console-targets - NAMESPACE robot:: - DESTINATION lib/cmake/${PROJECT_NAME}_console) - -# Create alias for easier usage -add_library(robot::console ALIAS ${PROJECT_NAME}_console) - -# ======================================================== -# Test executable -# ======================================================== -option(BUILD_TESTS "Build test programs" ON) - -if(BUILD_TESTS) - add_executable(test_node_handle - test/test_node_handle.cpp - ) - - target_include_directories(test_node_handle - PRIVATE - ${CMAKE_CURRENT_SOURCE_DIR}/include - ) - - target_link_libraries(test_node_handle - PRIVATE - robot::node_handle - yaml-cpp - ) - - # Link filesystem library if needed - if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU" AND CMAKE_CXX_COMPILER_VERSION VERSION_LESS "9.0") - target_link_libraries(test_node_handle PRIVATE stdc++fs) - elseif(CMAKE_CXX_COMPILER_ID MATCHES "Clang") - find_library(FILESYSTEM_LIB c++fs) - if(FILESYSTEM_LIB) - target_link_libraries(test_node_handle PRIVATE ${FILESYSTEM_LIB}) - endif() - endif() - - # Add test to CTest - add_test(NAME NodeHandleTest COMMAND test_node_handle) - - message(STATUS "Test executable 'test_node_handle' will be built") -endif() diff --git a/src/Libraries/robot_cpp/LICENSE b/src/Libraries/robot_cpp/LICENSE deleted file mode 100644 index 0b8ae76..0000000 --- a/src/Libraries/robot_cpp/LICENSE +++ /dev/null @@ -1,5 +0,0 @@ -Copyright (C) YEAR by AUTHOR EMAIL - -Permission to use, copy, modify, and/or distribute this software for any purpose with or without fee is hereby granted. - -THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. diff --git a/src/Libraries/robot_cpp/README.md b/src/Libraries/robot_cpp/README.md deleted file mode 100644 index 09af609..0000000 --- a/src/Libraries/robot_cpp/README.md +++ /dev/null @@ -1,2 +0,0 @@ -# robot_cpp - diff --git a/src/Libraries/robot_cpp/include/robot/console.h b/src/Libraries/robot_cpp/include/robot/console.h deleted file mode 100644 index 41102f3..0000000 --- a/src/Libraries/robot_cpp/include/robot/console.h +++ /dev/null @@ -1,89 +0,0 @@ -#ifndef ROBOT_CONSOLE_H -#define ROBOT_CONSOLE_H - -#include -#include - -namespace robot { - -// ANSI Color Codes -namespace color { - // Reset - static const char* RESET = "\033[0m"; - - // Text colors - static const char* BLACK = "\033[30m"; - static const char* RED = "\033[31m"; - static const char* GREEN = "\033[32m"; - static const char* YELLOW = "\033[33m"; - static const char* BLUE = "\033[34m"; - static const char* MAGENTA = "\033[35m"; - static const char* CYAN = "\033[36m"; - static const char* WHITE = "\033[37m"; - - // Bright text colors - static const char* BRIGHT_BLACK = "\033[90m"; - static const char* BRIGHT_RED = "\033[91m"; - static const char* BRIGHT_GREEN = "\033[92m"; - static const char* BRIGHT_YELLOW = "\033[93m"; - static const char* BRIGHT_BLUE = "\033[94m"; - static const char* BRIGHT_MAGENTA = "\033[95m"; - static const char* BRIGHT_CYAN = "\033[96m"; - static const char* BRIGHT_WHITE = "\033[97m"; - - // Background colors - static const char* BG_BLACK = "\033[40m"; - static const char* BG_RED = "\033[41m"; - static const char* BG_GREEN = "\033[42m"; - static const char* BG_YELLOW = "\033[43m"; - static const char* BG_BLUE = "\033[44m"; - static const char* BG_MAGENTA = "\033[45m"; - static const char* BG_CYAN = "\033[46m"; - static const char* BG_WHITE = "\033[47m"; - - // Text styles - static const char* BOLD = "\033[1m"; - static const char* DIM = "\033[2m"; - static const char* ITALIC = "\033[3m"; - static const char* UNDERLINE = "\033[4m"; - static const char* BLINK = "\033[5m"; - static const char* REVERSE = "\033[7m"; -} - -// Check if terminal supports colors -bool is_color_supported(); - -// Enable/disable color output (useful for non-terminal outputs) -void set_color_enabled(bool enabled); -bool is_color_enabled(); - -// Colored printf functions -void printf_red(const char* format, ...); -void printf_green(const char* format, ...); -void printf_yellow(const char* format, ...); -void printf_blue(const char* format, ...); -void printf_cyan(const char* format, ...); -void printf_magenta(const char* format, ...); -void printf_white(const char* format, ...); - -// Colored printf with custom color -void printf_color(const char* color_code, const char* format, ...); - -// Log level functions -void log_info(const char* format, ...); -void log_success(const char* format, ...); -void log_warning(const char* format, ...); -void log_error(const char* format, ...); -void log_debug(const char* format, ...); - -// Print with file and line info (colored) -void log_info_at(const char* file, int line, const char* format, ...); -void log_success_at(const char* file, int line, const char* format, ...); -void log_warning_at(const char* file, int line, const char* format, ...); -void log_error_at(const char* file, int line, const char* format, ...); -void log_debug_at(const char* file, int line, const char* format, ...); - -} // namespace robot - -#endif // ROBOT_CONSOLE_H - diff --git a/src/Libraries/robot_cpp/include/robot/node_handle.h b/src/Libraries/robot_cpp/include/robot/node_handle.h deleted file mode 100644 index 769f86d..0000000 --- a/src/Libraries/robot_cpp/include/robot/node_handle.h +++ /dev/null @@ -1,156 +0,0 @@ -#ifndef ROBOT_NODE_H_INCLUDED_H -#define ROBOT_NODE_H_INCLUDED_H - -#include -#include -#include -#include -#include - -// Forward declaration for XmlRpcValue -namespace XmlRpc { - class XmlRpcValue; -} - -namespace robot -{ - // Type alias for remappings (map of string to string) - using M_string = std::map; - - class NodeHandle - { - public: - using Ptr = std::shared_ptr; - - NodeHandle (const NodeHandle &parent, const std::string &ns); - - NodeHandle (const NodeHandle &parent, const std::string &ns, const M_string &remappings); - - NodeHandle (const NodeHandle &rhs); - - NodeHandle (const std::string &ns=std::string(), const M_string &remappings=M_string()); - - NodeHandle & operator= (const NodeHandle &rhs); - - ~NodeHandle(); - - bool hasParam (const std::string &key) const; - - bool getParam (const std::string &key, bool &b, bool default_value = false) const; - - bool getParam (const std::string &key, double &d, double default_value = 0.0) const; - - bool getParam (const std::string &key, float &f, float default_value = 0.0) const; - - bool getParam (const std::string &key, int &i, int default_value = 0) const; - - bool getParam (const std::string &key, std::map< std::string, bool > &map, std::map< std::string, bool > default_value = std::map< std::string, bool >()) const; - - bool getParam (const std::string &key, std::map< std::string, double > &map, std::map< std::string, double > default_value = std::map< std::string, double >()) const; - - bool getParam (const std::string &key, std::map< std::string, float > &map, std::map< std::string, float > default_value = std::map< std::string, float >()) const; - - bool getParam (const std::string &key, std::map< std::string, int > &map, std::map< std::string, int > default_value = std::map< std::string, int >()) const; - - bool getParam (const std::string &key, std::map< std::string, std::string > &map, std::map< std::string, std::string > default_value = std::map< std::string, std::string >()) const; - - bool getParam (const std::string &key, std::string &s, std::string default_value = "") const; - - bool getParam (const std::string &key, std::vector< bool > &vec, std::vector< bool > default_value = std::vector< bool >()) const; - - bool getParam (const std::string &key, std::vector< double > &vec, std::vector< double > default_value = std::vector< double >()) const; - - bool getParam (const std::string &key, std::vector< float > &vec, std::vector< float > default_value = std::vector< float >()) const; - - bool getParam (const std::string &key, std::vector< int > &vec, std::vector< int > default_value = std::vector< int >()) const; - - bool getParam (const std::string &key, std::vector< std::string > &vec, std::vector< std::string > default_value = std::vector< std::string >()) const; - - bool getParam (const std::string &key, YAML::Node &v, YAML::Node default_value = YAML::Node()) const; - - // Helper method to set parameters from YAML::Node - void setParam(const std::string &key, const YAML::Node &value); - - void setParam (const std::string &key, bool b) const; - - void setParam (const std::string &key, const char *s) const; - - void setParam (const std::string &key, const std::map< std::string, bool > &map) const; - - void setParam (const std::string &key, const std::map< std::string, double > &map) const; - - void setParam (const std::string &key, const std::map< std::string, float > &map) const; - - void setParam (const std::string &key, const std::map< std::string, int > &map) const; - - void setParam (const std::string &key, const std::map< std::string, std::string > &map) const; - - void setParam (const std::string &key, const std::string &s) const; - - void setParam (const std::string &key, const std::vector< bool > &vec) const; - - void setParam (const std::string &key, const std::vector< double > &vec) const; - - void setParam (const std::string &key, const std::vector< float > &vec) const; - - void setParam (const std::string &key, const std::vector< int > &vec) const; - - void setParam (const std::string &key, const std::vector< std::string > &vec) const; - - void setParam (const std::string &key, const XmlRpc::XmlRpcValue &v) const; - - void setParam (const std::string &key, double d) const; - - void setParam (const std::string &key, int i) const; - - - // Helper method to get nested parameter by key path (e.g., "parent/child") - YAML::Node getParamValue(const std::string &key) const; - - // Load YAML file - bool loadYamlFile(const std::string &filepath); - - // Merge YAML node into current node (recursively merges maps) - void mergeYamlNode(const YAML::Node &source); - - // Load all YAML files from a directory and merge them - // Returns number of files successfully loaded - int loadYamlFilesFromDirectory(const std::string &directory_path); - - // Get namespace - // If namespace is "~", returns the config directory path (first found path) - std::string getNamespace() const; - - // Get config directory path (static method) - static std::string getConfigDirectory(); - - // Debug: Print all parameters in node_handle_ (for debugging) - void printAllParams() const; - - // Set config directory path (for automatic YAML loading) - // If set, NodeHandle will automatically load YAML files from this directory - static void setConfigDirectory(const std::string &config_dir); - - private: - std::string namespace_; - static YAML::Node node_handle_; - static std::string config_directory_; // Static config directory path - - // Helper method to split key path and get nested value - YAML::Node getNestedValue(const std::string &key) const; - - // Debug version with verbose output - YAML::Node getNestedValueDebug(const std::string &key) const; - - // Helper method to set parameter with type checking - void setParamInternal(const std::string &key, const YAML::Node &value, YAML::NodeType::value expected_type); - - // Helper method to find config directory automatically - static std::string findConfigDirectory(); - - // Auto-load YAML files from config directory if namespace is "~" - void autoLoadConfigFiles(); - }; -} // namespace robot - -#endif // ROBOT_NODE_H_INCLUDED_H \ No newline at end of file diff --git a/src/Libraries/robot_cpp/src/console.cpp b/src/Libraries/robot_cpp/src/console.cpp deleted file mode 100644 index 299f88d..0000000 --- a/src/Libraries/robot_cpp/src/console.cpp +++ /dev/null @@ -1,207 +0,0 @@ -#include "robot/console.h" -#include -#include - -namespace robot { - -// Global flag to enable/disable colors -static bool color_enabled = true; - -bool is_color_supported() { - // Check if NO_COLOR environment variable is set - const char* no_color = std::getenv("NO_COLOR"); - if (no_color != nullptr && strlen(no_color) > 0) { - return false; - } - - // Check if TERM environment variable suggests color support - const char* term = std::getenv("TERM"); - if (term != nullptr) { - // Common terminals that support colors - if (strstr(term, "xterm") != nullptr || - strstr(term, "color") != nullptr || - strstr(term, "256") != nullptr || - strstr(term, "screen") != nullptr || - strstr(term, "tmux") != nullptr) { - return true; - } - } - - // Default to true for most modern terminals - return true; -} - -void set_color_enabled(bool enabled) { - color_enabled = enabled && is_color_supported(); -} - -bool is_color_enabled() { - return color_enabled && is_color_supported(); -} - -// Helper function that accepts va_list -static void vprintf_color(const char* color_code, const char* format, va_list args) { - if (is_color_enabled()) { - std::printf("%s", color_code); - } - - std::vprintf(format, args); - - if (is_color_enabled()) { - std::printf("%s", color::RESET); - } -} - -void printf_color(const char* color_code, const char* format, ...) { - va_list args; - va_start(args, format); - vprintf_color(color_code, format, args); - va_end(args); -} - -void printf_red(const char* format, ...) { - va_list args; - va_start(args, format); - vprintf_color(color::RED, format, args); - va_end(args); -} - -void printf_green(const char* format, ...) { - va_list args; - va_start(args, format); - vprintf_color(color::GREEN, format, args); - va_end(args); -} - -void printf_yellow(const char* format, ...) { - va_list args; - va_start(args, format); - vprintf_color(color::YELLOW, format, args); - va_end(args); -} - -void printf_blue(const char* format, ...) { - va_list args; - va_start(args, format); - vprintf_color(color::BLUE, format, args); - va_end(args); -} - -void printf_cyan(const char* format, ...) { - va_list args; - va_start(args, format); - vprintf_color(color::CYAN, format, args); - va_end(args); -} - -void printf_magenta(const char* format, ...) { - va_list args; - va_start(args, format); - vprintf_color(color::MAGENTA, format, args); - va_end(args); -} - -void printf_white(const char* format, ...) { - va_list args; - va_start(args, format); - vprintf_color(color::WHITE, format, args); - va_end(args); -} - -void log_info(const char* format, ...) { - va_list args; - va_start(args, format); - vprintf_color(color::BLUE, format, args); - va_end(args); -} - -void log_success(const char* format, ...) { - va_list args; - va_start(args, format); - vprintf_color(color::GREEN, format, args); - va_end(args); -} - -void log_warning(const char* format, ...) { - va_list args; - va_start(args, format); - vprintf_color(color::YELLOW, format, args); - va_end(args); -} - -void log_error(const char* format, ...) { - va_list args; - va_start(args, format); - vprintf_color(color::RED, format, args); - va_end(args); -} - -void log_debug(const char* format, ...) { - va_list args; - va_start(args, format); - vprintf_color(color::CYAN, format, args); - va_end(args); -} - -void log_info_at(const char* file, int line, const char* format, ...) { - va_list args; - va_start(args, format); - if (is_color_enabled()) { - std::printf("%s[INFO]%s [%s:%d] ", color::BLUE, color::RESET, file, line); - } else { - std::printf("[INFO] [%s:%d] ", file, line); - } - std::vprintf(format, args); - va_end(args); -} - -void log_success_at(const char* file, int line, const char* format, ...) { - va_list args; - va_start(args, format); - if (is_color_enabled()) { - std::printf("%s[SUCCESS]%s [%s:%d] ", color::GREEN, color::RESET, file, line); - } else { - std::printf("[SUCCESS] [%s:%d] ", file, line); - } - std::vprintf(format, args); - va_end(args); -} - -void log_warning_at(const char* file, int line, const char* format, ...) { - va_list args; - va_start(args, format); - if (is_color_enabled()) { - std::printf("%s[WARNING]%s [%s:%d] ", color::YELLOW, color::RESET, file, line); - } else { - std::printf("[WARNING] [%s:%d] ", file, line); - } - std::vprintf(format, args); - va_end(args); -} - -void log_error_at(const char* file, int line, const char* format, ...) { - va_list args; - va_start(args, format); - if (is_color_enabled()) { - std::printf("%s[ERROR]%s [%s:%d] ", color::RED, color::RESET, file, line); - } else { - std::printf("[ERROR] [%s:%d] ", file, line); - } - std::vprintf(format, args); - va_end(args); -} - -void log_debug_at(const char* file, int line, const char* format, ...) { - va_list args; - va_start(args, format); - if (is_color_enabled()) { - std::printf("%s[DEBUG]%s [%s:%d] ", color::CYAN, color::RESET, file, line); - } else { - std::printf("[DEBUG] [%s:%d] ", file, line); - } - std::vprintf(format, args); - va_end(args); -} - -} // namespace robot - diff --git a/src/Libraries/robot_cpp/src/node_handle.cpp b/src/Libraries/robot_cpp/src/node_handle.cpp deleted file mode 100644 index a4d78f7..0000000 --- a/src/Libraries/robot_cpp/src/node_handle.cpp +++ /dev/null @@ -1,1445 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#ifdef __linux__ -#include -#endif -#include - -namespace robot -{ - -// Static member initialization -std::string NodeHandle::config_directory_ = ""; -YAML::Node NodeHandle::node_handle_; - -// Constructor with namespace and optional remappings -NodeHandle::NodeHandle(const std::string &ns, const M_string &remappings) : namespace_(ns) -{ - // If namespace is "~" or empty, set namespace to config directory path (root path) - if (ns == "~" || ns.empty()) - { - std::string config_dir = findConfigDirectory(); - if (!config_dir.empty()) - { - namespace_ = config_dir; - } - } - - // Apply remappings if provided - // Remappings are used to remap parameter names (e.g., "old_key" -> "new_key") - // For now, we'll store them but not actively use them in getParam - // In a full implementation, remappings would be applied when getting parameters - (void)remappings; // Suppress unused parameter warning for now - - // Only auto-load config files if namespace is "~" or empty (root namespace) - if (ns == "~" || ns.empty()) - { - autoLoadConfigFiles(); - } -} - -// Constructor from parent NodeHandle with new namespace -NodeHandle::NodeHandle(const NodeHandle &parent, const std::string &ns) : namespace_(ns) -{ - // Inherit the parent's node_handle_ (static, so shared) - // Set namespace based on parent's namespace and new namespace - if (ns.empty()) - { - namespace_ = parent.namespace_; - } - else if (ns == "~") - { - std::string config_dir = findConfigDirectory(); - if (!config_dir.empty()) - { - namespace_ = config_dir; - } - else - { - namespace_ = parent.namespace_; - } - } - else - { - // Combine parent namespace with new namespace - if (!parent.namespace_.empty() && parent.namespace_ != "~") - { - namespace_ = parent.namespace_ + "/" + ns; - } - else - { - namespace_ = ns; - } - } - - // Don't auto-load config files for child namespaces - // They share the same node_handle_ as parent -} - -// Constructor from parent NodeHandle with new namespace and remappings -NodeHandle::NodeHandle(const NodeHandle &parent, const std::string &ns, const M_string &remappings) - : namespace_(ns) -{ - // Inherit the parent's node_handle_ (static, so shared) - // Set namespace based on parent's namespace and new namespace - if (ns.empty()) - { - namespace_ = parent.namespace_; - } - else if (ns == "~") - { - std::string config_dir = findConfigDirectory(); - if (!config_dir.empty()) - { - namespace_ = config_dir; - } - else - { - namespace_ = parent.namespace_; - } - } - else - { - // Combine parent namespace with new namespace - if (!parent.namespace_.empty() && parent.namespace_ != "~") - { - namespace_ = parent.namespace_ + "/" + ns; - } - else - { - namespace_ = ns; - } - } - - // Apply remappings if provided - (void)remappings; // Suppress unused parameter warning for now - - // Don't auto-load config files for child namespaces - // They share the same node_handle_ as parent -} - -// Copy constructor -NodeHandle::NodeHandle(const NodeHandle &rhs) : namespace_(rhs.namespace_) -{ - // Copy namespace, node_handle_ is static so it's already shared - // No need to copy node_handle_ as it's static and shared across all instances -} - -// Assignment operator -NodeHandle &NodeHandle::operator=(const NodeHandle &rhs) -{ - if (this != &rhs) - { - namespace_ = rhs.namespace_; - // node_handle_ is static, so no need to copy it - } - return *this; -} - -NodeHandle::~NodeHandle() -{ -} - -bool NodeHandle::hasParam(const std::string &key) const -{ - YAML::Node value = getNestedValue(key); - return value.IsDefined(); -} - -YAML::Node NodeHandle::getNestedValue(const std::string &key) const -{ - if (!node_handle_.IsDefined() || !node_handle_.IsMap()) - { - return YAML::Node(); - } - - // Split key by '/' to handle nested keys - std::stringstream ss(key); - std::string segment; - std::vector segments; - - while (std::getline(ss, segment, '/')) - { - if (!segment.empty()) - { - segments.push_back(segment); - } - } - - if (segments.empty()) - { - // Return a copy to avoid modifying the original - return YAML::Clone(node_handle_); - } - - // Traverse the tree without modifying node_handle_ - // Use const reference to the original node_handle_ to avoid any modifications - const YAML::Node& root = node_handle_; - const YAML::Node* current = &root; - - for (size_t i = 0; i < segments.size(); ++i) - { - if (!current->IsMap()) - { - return YAML::Node(); - } - - // Access child node without creating a copy that might share reference - const YAML::Node& child = (*current)[segments[i]]; - if (!child.IsDefined()) - { - return YAML::Node(); - } - - // Move to next level - current = &child; - } - - // Return a clone of the final node to ensure it's independent - return YAML::Clone(*current); -} - -YAML::Node NodeHandle::getNestedValueDebug(const std::string &key) const -{ - std::cout << "[getNestedValue] Looking for key: '" << key << "'" << std::endl; - std::cout << "[getNestedValue] node_handle_ state: IsDefined=" << node_handle_.IsDefined() - << ", IsMap=" << node_handle_.IsMap() << ", Type=" << node_handle_.Type() << std::endl; - - if (!node_handle_.IsDefined() || !node_handle_.IsMap()) - { - std::cout << "[getNestedValue] node_handle_ is not defined or not a map!" << std::endl; - return YAML::Node(); - } - - // Split key by '/' to handle nested keys - std::stringstream ss(key); - std::string segment; - std::vector segments; - - while (std::getline(ss, segment, '/')) - { - if (!segment.empty()) - { - segments.push_back(segment); - } - } - - if (segments.empty()) - { - std::cout << "[getNestedValue] No segments, returning entire node_handle_" << std::endl; - return node_handle_; - } - - std::cout << "[getNestedValue] Segments: "; - for (size_t i = 0; i < segments.size(); ++i) - { - std::cout << segments[i]; - if (i < segments.size() - 1) std::cout << " -> "; - } - std::cout << std::endl; - - YAML::Node current = node_handle_; - - for (size_t i = 0; i < segments.size(); ++i) - { - if (!current.IsMap()) - { - std::cout << "[getNestedValue] Segment '" << segments[i] << "' failed: current is not a map" << std::endl; - return YAML::Node(); - } - - YAML::Node segment_node = current[segments[i]]; - if (!segment_node.IsDefined()) - { - std::cout << "[getNestedValue] Segment '" << segments[i] << "' not found in current map" << std::endl; - std::cout << "[getNestedValue] Available keys in current map: "; - // Use a safer iteration method to avoid potential iterator issues - try { - for (const auto& pair : current) - { - std::cout << pair.first.as(); - std::cout << ", "; - } - } catch (...) { - std::cout << "(error iterating)"; - } - std::cout << std::endl; - return YAML::Node(); - } - - current = segment_node; - std::cout << "[getNestedValue] Found segment '" << segments[i] << "'" << std::endl; - } - - std::cout << "[getNestedValue] Successfully found value for key: '" << key << "'" << std::endl; - std::cout << "[getNestedValue] After traversal, node_handle_ state: IsDefined=" << node_handle_.IsDefined() - << ", IsMap=" << node_handle_.IsMap() << ", Type=" << node_handle_.Type() << std::endl; - return current; -} - -YAML::Node NodeHandle::getParamValue(const std::string &key) const -{ - return getNestedValue(key); -} - -void NodeHandle::setParam(const std::string &key, const YAML::Node &value) -{ - if (!node_handle_.IsMap()) - { - node_handle_ = YAML::Node(YAML::NodeType::Map); - } - - // Split key by '/' to handle nested keys - std::stringstream ss(key); - std::string segment; - std::vector segments; - - while (std::getline(ss, segment, '/')) - { - if (!segment.empty()) - { - segments.push_back(segment); - } - } - - if (segments.empty()) - { - node_handle_ = value; - return; - } - - // Reference-safe traversal using yaml-cpp handles (Node is a shared handle) - if (!node_handle_.IsDefined() || !node_handle_.IsMap()) - { - node_handle_ = YAML::Node(YAML::NodeType::Map); - } - - YAML::Node cur = node_handle_; // handle to the root - for (size_t i = 0; i + 1 < segments.size(); ++i) - { - YAML::Node child = cur[segments[i]]; // handle to child - if (!child.IsDefined() || !child.IsMap()) - { - cur[segments[i]] = YAML::Node(YAML::NodeType::Map); - child = cur[segments[i]]; - } - cur = child; // descend - } - - cur[segments.back()] = value; -} - -bool NodeHandle::loadYamlFile(const std::string &filepath) -{ - try - { - node_handle_ = YAML::LoadFile(filepath); - return true; - } - catch (const YAML::Exception &e) - { - return false; - } -} - -std::string NodeHandle::getNamespace() const -{ - // namespace_ is already set to config directory path if name was "~" or "" - return namespace_; -} - -std::string NodeHandle::getConfigDirectory() -{ - return findConfigDirectory(); -} - -void NodeHandle::printAllParams() const -{ - std::cout << "\n[NodeHandle] ========== All Parameters After Merge ==========" << std::endl; - if (!node_handle_.IsDefined()) - { - std::cout << "[NodeHandle] node_handle_ is not defined!" << std::endl; - return; - } - - if (!node_handle_.IsMap()) - { - std::cout << "[NodeHandle] node_handle_ is not a map!" << std::endl; - return; - } - - std::cout << "[NodeHandle] Total top-level keys: " << node_handle_.size() << std::endl; - std::cout << std::endl; - - // Helper lambda to print nested structure with indentation - std::function printNode = - [&printNode](const YAML::Node& node, const std::string& prefix, int depth) { - if (depth > 10) return; // Limit depth to avoid infinite recursion - - std::string indent(depth * 2, ' '); // 2 spaces per depth level - - if (node.IsMap()) - { - for (auto it = node.begin(); it != node.end(); ++it) - { - std::string key = it->first.as(); - const YAML::Node& value = it->second; - std::string full_key = prefix.empty() ? key : prefix + "/" + key; - - if (value.IsMap()) - { - std::cout << "[NodeHandle] " << indent << full_key << ":" << std::endl; - printNode(value, full_key, depth + 1); - } - else if (value.IsSequence()) - { - std::cout << "[NodeHandle] " << indent << full_key << " (sequence, " << value.size() << " items):" << std::endl; - // Print sequence items - for (size_t i = 0; i < value.size(); ++i) - { - const YAML::Node& item = value[i]; - if (item.IsMap()) - { - std::cout << "[NodeHandle] " << indent << " [" << i << "]:" << std::endl; - printNode(item, full_key + "[" + std::to_string(i) + "]", depth + 2); - } - else if (item.IsScalar()) - { - std::cout << "[NodeHandle] " << indent << " [" << i << "] = " << item.as() << std::endl; - } - } - } - else if (value.IsScalar()) - { - try - { - std::cout << "[NodeHandle] " << indent << full_key << " = " << value.as() << std::endl; - } - catch (...) - { - std::cout << "[NodeHandle] " << indent << full_key << " = (unable to convert to string)" << std::endl; - } - } - else if (value.IsNull()) - { - std::cout << "[NodeHandle] " << indent << full_key << " = null" << std::endl; - } - } - } - }; - - printNode(node_handle_, "", 0); - std::cout << "[NodeHandle] ================================================\n" << std::endl; -} - -void NodeHandle::mergeYamlNode(const YAML::Node &source) -{ - if (!source.IsDefined()) - { - return; - } - - // If source is not a map, just replace node_handle_ - if (!source.IsMap()) - { - node_handle_ = source; - return; - } - - // Initialize node_handle_ as a map if it's not already - if (!node_handle_.IsDefined() || !node_handle_.IsMap()) - { - node_handle_ = YAML::Node(YAML::NodeType::Map); - } - - // Helper lambda to recursively merge two YAML nodes - // Note: yaml-cpp's operator[] returns YAML::Node (not reference), so we need to work with copies - std::function mergeRecursive = - [&mergeRecursive](YAML::Node& dest, const YAML::Node& src) { - if (!src.IsMap()) - { - dest = src; - return; - } - - // Ensure dest is a map - if (!dest.IsMap()) - { - dest = YAML::Node(YAML::NodeType::Map); - } - - for (auto it = src.begin(); it != src.end(); ++it) - { - std::string key = it->first.as(); - const YAML::Node& src_value = it->second; - - // Get current value in dest (if exists) - YAML::Node dest_current = dest[key]; - - // Check if key exists in dest and if both are maps - if (dest_current.IsDefined() && dest_current.IsMap() && src_value.IsMap()) - { - // Both are maps, recursively merge - // Create a copy of dest[key] to work with, then assign back - YAML::Node dest_value = dest_current; - mergeRecursive(dest_value, src_value); - dest[key] = dest_value; - } - else - { - // Overwrite with source value (or add if doesn't exist) - dest[key] = src_value; - } - } - }; - - // Merge source into node_handle_ - mergeRecursive(node_handle_, source); -} - -int NodeHandle::loadYamlFilesFromDirectory(const std::string &directory_path) -{ - int loaded_count = 0; - - try - { - // Check if directory exists - if (!std::filesystem::exists(directory_path) || !std::filesystem::is_directory(directory_path)) - { - std::cerr << "Directory does not exist or is not a directory: " << directory_path << std::endl; - return 0; - } - - // Iterate through all files in the directory - for (const auto &entry : std::filesystem::directory_iterator(directory_path)) - { - if (entry.is_regular_file()) - { - std::string file_path = entry.path().string(); - std::string extension = entry.path().extension().string(); - - // Only process .yaml and .yml files - if (extension == ".yaml" || extension == ".yml") - { - try - { - YAML::Node file_node = YAML::LoadFile(file_path); - if (file_node.IsDefined()) - { - // // Debug: show what's being loaded - // std::cout << "[NodeHandle] Loading YAML file: " << file_path << std::endl; - // if (file_node.IsMap()) - // { - // std::cout << "[NodeHandle] File contains " << file_node.size() << " top-level keys" << std::endl; - // for (auto it = file_node.begin(); it != file_node.end(); ++it) - // { - // std::cout << "[NodeHandle] - " << it->first.as() << std::endl; - // } - // } - - mergeYamlNode(file_node); - loaded_count++; - - // // Debug: verify data was merged - // if (node_handle_.IsMap()) - // { - // std::cout << "[NodeHandle] After merge, node_handle_ has " << node_handle_.size() << " top-level keys" << std::endl; - // } - } - } - catch (const YAML::Exception &e) - { - std::cerr << "Error loading YAML file " << file_path << ": " << e.what() << std::endl; - } - } - } - } - } - catch (const std::exception &e) - { - std::cerr << "Error reading directory " << directory_path << ": " << e.what() << std::endl; - } - - return loaded_count; -} - -void NodeHandle::setConfigDirectory(const std::string &config_dir) -{ - config_directory_ = config_dir; -} - -std::string NodeHandle::findConfigDirectory() -{ - // If config directory is already set, use it - if (!config_directory_.empty() && std::filesystem::exists(config_directory_)) - { - return config_directory_; - } - - // Try environment variable first - const char* env_config = std::getenv("PNKX_NAV_CORE_CONFIG_DIR"); - if (env_config && std::filesystem::exists(env_config)) - { - config_directory_ = std::string(env_config); - return config_directory_; - } - - // Try workspace path + /config - const char* workspace_path = std::getenv("PNKX_NAV_CORE_DIR"); - if (workspace_path) - { - std::string config_path = std::string(workspace_path) + "/config"; - if (std::filesystem::exists(config_path)) - { - config_directory_ = config_path; - return config_directory_; - } - } - - // Try to find config relative to executable path - try - { - char exe_path[PATH_MAX]; - ssize_t count = readlink("/proc/self/exe", exe_path, PATH_MAX); - if (count != -1) - { - exe_path[count] = '\0'; - std::filesystem::path exe_dir = std::filesystem::path(exe_path).parent_path(); - - // Try paths relative to executable: ../../config, ../config, etc. - std::vector relative_paths = { - exe_dir / "../../config", - exe_dir / "../config", - exe_dir / "config", - exe_dir.parent_path() / "config", - exe_dir.parent_path().parent_path() / "config" - }; - - for (const auto& rel_path : relative_paths) - { - if (std::filesystem::exists(rel_path) && std::filesystem::is_directory(rel_path)) - { - config_directory_ = std::filesystem::canonical(rel_path).string(); - return config_directory_; - } - } - } - } - catch (const std::exception&) - { - // Ignore errors when trying to find executable path - } - - // Try hardcoded fallback paths - std::vector possible_paths = { - "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/config", - "../config", - "../../config", - "./config" - }; - - for (const auto& path : possible_paths) - { - if (std::filesystem::exists(path) && std::filesystem::is_directory(path)) - { - try - { - config_directory_ = std::filesystem::canonical(path).string(); - return config_directory_; - } - catch (const std::exception&) - { - // If canonical fails, try the path as-is - config_directory_ = path; - return config_directory_; - } - } - } - - // Return empty string if not found - return ""; -} - -void NodeHandle::autoLoadConfigFiles() -{ - // Auto-load only if namespace is config directory path (which happens when name was "~" or "") - // Don't auto-load for custom namespaces - std::string config_dir_to_load = ""; - - // Check if namespace_ is a valid directory path (config directory) - // This means the original name was "~" or "" - if (std::filesystem::exists(namespace_) && std::filesystem::is_directory(namespace_)) - { - // namespace_ is already a config directory path (set in constructor when name was "~" or "") - config_dir_to_load = namespace_; - } - else - { - // Custom namespace, don't auto-load - return; - } - - if (config_dir_to_load.empty()) - { - // Silently fail - config directory not found, which is OK - // User can still manually load files or set parameters - return; - } - - // Load all YAML files from config directory - int loaded_count = loadYamlFilesFromDirectory(config_dir_to_load); - if (loaded_count > 0) - { - std::cout << "[NodeHandle] Auto-loaded " << loaded_count - << " YAML file(s) from: " << config_dir_to_load << std::endl; - - // Debug: show final state of node_handle_ - if (node_handle_.IsDefined() && node_handle_.IsMap()) - { - std::cout << "[NodeHandle] Final node_handle_ contains " << node_handle_.size() - << " top-level keys after loading all files" << std::endl; - - // Print all parameters after merge - printAllParams(); - } - } - else - { - std::cout << "[NodeHandle] No YAML files found or loaded from: " << config_dir_to_load << std::endl; - } -} - -// Helper function to set parameter with type checking -// If key doesn't exist, create it with the new value -// If key exists, check if type matches; if not, overwrite with new type -void NodeHandle::setParamInternal(const std::string &key, const YAML::Node &value, YAML::NodeType::value expected_type) -{ - YAML::Node existing = getNestedValue(key); - - // If key doesn't exist, create it - if (!existing.IsDefined()) - { - setParam(key, value); - return; - } - - // If key exists, check type compatibility - // For scalar types, we allow conversion between compatible types - if (expected_type == YAML::NodeType::Scalar && existing.IsScalar()) - { - // Scalar types are compatible, just update the value - setParam(key, value); - } - else if (existing.Type() == expected_type) - { - // Same type, update value - setParam(key, value); - } - else - { - // Type mismatch - overwrite with new type (as per requirement) - setParam(key, value); - } -} - -void NodeHandle::setParam(const std::string &key, bool b) const -{ - const_cast(this)->setParamInternal(key, YAML::Node(b), YAML::NodeType::Scalar); -} - -void NodeHandle::setParam(const std::string &key, const char *s) const -{ - const_cast(this)->setParamInternal(key, YAML::Node(std::string(s)), YAML::NodeType::Scalar); -} - -void NodeHandle::setParam(const std::string &key, const std::string &s) const -{ - const_cast(this)->setParamInternal(key, YAML::Node(s), YAML::NodeType::Scalar); -} - -void NodeHandle::setParam(const std::string &key, int i) const -{ - const_cast(this)->setParamInternal(key, YAML::Node(i), YAML::NodeType::Scalar); -} - -void NodeHandle::setParam(const std::string &key, double d) const -{ - const_cast(this)->setParamInternal(key, YAML::Node(d), YAML::NodeType::Scalar); -} - -void NodeHandle::setParam(const std::string &key, const std::vector &vec) const -{ - YAML::Node node(YAML::NodeType::Sequence); - for (bool b : vec) - { - node.push_back(YAML::Node(b)); - } - const_cast(this)->setParamInternal(key, node, YAML::NodeType::Sequence); -} - -void NodeHandle::setParam(const std::string &key, const std::vector &vec) const -{ - YAML::Node node(YAML::NodeType::Sequence); - for (int i : vec) - { - node.push_back(YAML::Node(i)); - } - const_cast(this)->setParamInternal(key, node, YAML::NodeType::Sequence); -} - -void NodeHandle::setParam(const std::string &key, const std::vector &vec) const -{ - YAML::Node node(YAML::NodeType::Sequence); - for (float f : vec) - { - node.push_back(YAML::Node(static_cast(f))); - } - const_cast(this)->setParamInternal(key, node, YAML::NodeType::Sequence); -} - -void NodeHandle::setParam(const std::string &key, const std::vector &vec) const -{ - YAML::Node node(YAML::NodeType::Sequence); - for (double d : vec) - { - node.push_back(YAML::Node(d)); - } - const_cast(this)->setParamInternal(key, node, YAML::NodeType::Sequence); -} - -void NodeHandle::setParam(const std::string &key, const std::vector &vec) const -{ - YAML::Node node(YAML::NodeType::Sequence); - for (const std::string &s : vec) - { - node.push_back(YAML::Node(s)); - } - const_cast(this)->setParamInternal(key, node, YAML::NodeType::Sequence); -} - -void NodeHandle::setParam(const std::string &key, const std::map &map) const -{ - YAML::Node node(YAML::NodeType::Map); - for (const auto &pair : map) - { - node[pair.first] = YAML::Node(pair.second); - } - const_cast(this)->setParamInternal(key, node, YAML::NodeType::Map); -} - -void NodeHandle::setParam(const std::string &key, const std::map &map) const -{ - YAML::Node node(YAML::NodeType::Map); - for (const auto &pair : map) - { - node[pair.first] = YAML::Node(pair.second); - } - const_cast(this)->setParamInternal(key, node, YAML::NodeType::Map); -} - -void NodeHandle::setParam(const std::string &key, const std::map &map) const -{ - YAML::Node node(YAML::NodeType::Map); - for (const auto &pair : map) - { - node[pair.first] = YAML::Node(static_cast(pair.second)); - } - const_cast(this)->setParamInternal(key, node, YAML::NodeType::Map); -} - -void NodeHandle::setParam(const std::string &key, const std::map &map) const -{ - YAML::Node node(YAML::NodeType::Map); - for (const auto &pair : map) - { - node[pair.first] = YAML::Node(pair.second); - } - const_cast(this)->setParamInternal(key, node, YAML::NodeType::Map); -} - -void NodeHandle::setParam(const std::string &key, const std::map &map) const -{ - YAML::Node node(YAML::NodeType::Map); - for (const auto &pair : map) - { - node[pair.first] = YAML::Node(pair.second); - } - const_cast(this)->setParamInternal(key, node, YAML::NodeType::Map); -} - -void NodeHandle::setParam(const std::string &key, const XmlRpc::XmlRpcValue &v) const -{ - // Convert XmlRpcValue to YAML::Node - // Create non-const copy to use conversion operators - XmlRpc::XmlRpcValue v_copy = v; - YAML::Node node; - try - { - switch (v.getType()) - { - case XmlRpc::XmlRpcValue::TypeBoolean: - { - bool b = v_copy; - node = YAML::Node(b); - const_cast(this)->setParamInternal(key, node, YAML::NodeType::Scalar); - } - break; - case XmlRpc::XmlRpcValue::TypeInt: - { - int i = v_copy; - node = YAML::Node(i); - const_cast(this)->setParamInternal(key, node, YAML::NodeType::Scalar); - } - break; - case XmlRpc::XmlRpcValue::TypeDouble: - { - double d = v_copy; - node = YAML::Node(d); - const_cast(this)->setParamInternal(key, node, YAML::NodeType::Scalar); - } - break; - case XmlRpc::XmlRpcValue::TypeString: - { - std::string s = v_copy; - node = YAML::Node(s); - const_cast(this)->setParamInternal(key, node, YAML::NodeType::Scalar); - } - break; - case XmlRpc::XmlRpcValue::TypeArray: - { - YAML::Node seq(YAML::NodeType::Sequence); - for (int i = 0; i < v_copy.size(); ++i) - { - YAML::Node item; - XmlRpc::XmlRpcValue item_v = v_copy[i]; - if (item_v.getType() == XmlRpc::XmlRpcValue::TypeBoolean) - { - bool b = item_v; - item = YAML::Node(b); - } - else if (item_v.getType() == XmlRpc::XmlRpcValue::TypeInt) - { - int i_val = item_v; - item = YAML::Node(i_val); - } - else if (item_v.getType() == XmlRpc::XmlRpcValue::TypeDouble) - { - double d = item_v; - item = YAML::Node(d); - } - else if (item_v.getType() == XmlRpc::XmlRpcValue::TypeString) - { - std::string s = item_v; - item = YAML::Node(s); - } - seq.push_back(item); - } - const_cast(this)->setParamInternal(key, seq, YAML::NodeType::Sequence); - } - break; - case XmlRpc::XmlRpcValue::TypeStruct: - { - YAML::Node map_node(YAML::NodeType::Map); - // XmlRpcValue::TypeStruct doesn't have begin/end, need to use different approach - // We'll need to iterate through the struct differently - // For now, create empty map - const_cast(this)->setParamInternal(key, map_node, YAML::NodeType::Map); - } - break; - default: - // Unknown type, create empty node - const_cast(this)->setParamInternal(key, YAML::Node(), YAML::NodeType::Null); - break; - } - } - catch (...) - { - // On error, create empty node - const_cast(this)->setParamInternal(key, YAML::Node(), YAML::NodeType::Null); - } -} - -bool NodeHandle::getParam(const std::string &key, bool &b, bool default_value) const -{ - YAML::Node value = getNestedValue(key); - - if (!value.IsDefined()) - { - b = default_value; - return false; - } - - if (value.IsScalar()) - { - try - { - if (value.Type() == YAML::NodeType::Scalar) - { - std::string str = value.as(); - std::transform(str.begin(), str.end(), str.begin(), ::tolower); - if (str == "true" || str == "1" || str == "yes" || str == "on") - { - b = true; - return true; - } - else if (str == "false" || str == "0" || str == "no" || str == "off") - { - b = false; - return true; - } - } - b = value.as(); - return true; - } - catch (...) - { - b = default_value; - return false; - } - } - - b = default_value; - return false; -} - -bool NodeHandle::getParam(const std::string &key, double &d, double default_value) const -{ - YAML::Node value = getNestedValue(key); - - if (!value.IsDefined() || !value.IsScalar()) - { - d = default_value; - return false; - } - - try - { - d = value.as(); - return true; - } - catch (...) - { - d = default_value; - return false; - } -} - -bool NodeHandle::getParam(const std::string &key, float &f, float default_value) const -{ - double d; - if (getParam(key, d, static_cast(default_value))) - { - f = static_cast(d); - return true; - } - f = default_value; - return false; -} - -bool NodeHandle::getParam(const std::string &key, int &i, int default_value) const -{ - YAML::Node value = getNestedValue(key); - - if (!value.IsDefined() || !value.IsScalar()) - { - i = default_value; - return false; - } - - try - { - std::string str = value.as(); - // Handle hex format - if (str.length() > 2 && str.substr(0, 2) == "0x") - { - i = std::stoi(str, nullptr, 16); - } - else - { - i = value.as(); - } - return true; - } - catch (...) - { - i = default_value; - return false; - } -} - -bool NodeHandle::getParam(const std::string &key, std::string &s, std::string default_value) const -{ - YAML::Node value = getNestedValue(key); - - // If key doesn't exist or is not a scalar, return default value - if (!value.IsDefined() || !value.IsScalar()) - { - s = default_value; - return false; - } - - try - { - s = value.as(); - return true; - } - catch (...) - { - s = default_value; - return false; - } -} - -bool NodeHandle::getParam(const std::string &key, std::vector &vec, std::vector default_value) const -{ - YAML::Node value = getNestedValue(key); - - if (!value.IsDefined() || !value.IsSequence()) - { - vec = default_value; - return false; - } - - vec.clear(); - try - { - for (size_t i = 0; i < value.size(); ++i) - { - if (value[i].IsScalar()) - { - try - { - bool b = value[i].as(); - vec.push_back(b); - } - catch (...) - { - // Try as string and convert - std::string str = value[i].as(); - std::transform(str.begin(), str.end(), str.begin(), ::tolower); - if (str == "true" || str == "1" || str == "yes" || str == "on") - { - vec.push_back(true); - } - else if (str == "false" || str == "0" || str == "no" || str == "off") - { - vec.push_back(false); - } - else - { - return false; - } - } - } - else - { - return false; - } - } - return true; - } - catch (...) - { - return false; - } -} - -bool NodeHandle::getParam(const std::string &key, std::vector &vec, std::vector default_value) const -{ - YAML::Node value = getNestedValue(key); - - if (!value.IsDefined() || !value.IsSequence()) - { - vec = default_value; - return false; - } - - vec.clear(); - try - { - for (size_t i = 0; i < value.size(); ++i) - { - vec.push_back(value[i].as()); - } - return true; - } - catch (...) - { - return false; - } -} - -bool NodeHandle::getParam(const std::string &key, std::vector &vec, std::vector default_value) const -{ - std::vector dvec; - std::vector ddefault; - ddefault.reserve(default_value.size()); - for (float f : default_value) - { - ddefault.push_back(static_cast(f)); - } - - if (getParam(key, dvec, ddefault)) - { - vec.clear(); - vec.reserve(dvec.size()); - for (double d : dvec) - { - vec.push_back(static_cast(d)); - } - return true; - } - vec = default_value; - return false; -} - -bool NodeHandle::getParam(const std::string &key, std::vector &vec, std::vector default_value) const -{ - YAML::Node value = getNestedValue(key); - - if (!value.IsDefined() || !value.IsSequence()) - { - vec = default_value; - return false; - } - - vec.clear(); - try - { - for (size_t i = 0; i < value.size(); ++i) - { - vec.push_back(value[i].as()); - } - return true; - } - catch (...) - { - return false; - } -} - -bool NodeHandle::getParam(const std::string &key, std::vector &vec, std::vector default_value) const -{ - YAML::Node value = getNestedValue(key); - - if (!value.IsDefined() || !value.IsSequence()) - { - vec = default_value; - return false; - } - - vec.clear(); - try - { - for (size_t i = 0; i < value.size(); ++i) - { - vec.push_back(value[i].as()); - } - return true; - } - catch (...) - { - return false; - } -} - -bool NodeHandle::getParam(const std::string &key, std::map &map, std::map default_value) const -{ - YAML::Node value = getNestedValue(key); - - if (!value.IsDefined() || !value.IsMap()) - { - map = default_value; - return false; - } - - map.clear(); - try - { - for (auto it = value.begin(); it != value.end(); ++it) - { - std::string key_str = it->first.as(); - if (it->second.IsScalar()) - { - try - { - bool b = it->second.as(); - map[key_str] = b; - } - catch (...) - { - // Try as string and convert - std::string str = it->second.as(); - std::transform(str.begin(), str.end(), str.begin(), ::tolower); - if (str == "true" || str == "1" || str == "yes" || str == "on") - { - map[key_str] = true; - } - else if (str == "false" || str == "0" || str == "no" || str == "off") - { - map[key_str] = false; - } - else - { - return false; - } - } - } - else - { - return false; - } - } - return true; - } - catch (...) - { - return false; - } -} - -bool NodeHandle::getParam(const std::string &key, std::map &map, std::map default_value) const -{ - YAML::Node value = getNestedValue(key); - - if (!value.IsDefined() || !value.IsMap()) - { - map = default_value; - return false; - } - - map.clear(); - try - { - for (auto it = value.begin(); it != value.end(); ++it) - { - std::string key_str = it->first.as(); - if (it->second.IsScalar()) - { - map[key_str] = it->second.as(); - } - else - { - return false; - } - } - return true; - } - catch (...) - { - return false; - } -} - -bool NodeHandle::getParam(const std::string &key, std::map &map, std::map default_value) const -{ - std::map dmap; - std::map ddefault; - for (const auto &pair : default_value) - { - ddefault[pair.first] = static_cast(pair.second); - } - - if (getParam(key, dmap, ddefault)) - { - map.clear(); - for (const auto &pair : dmap) - { - map[pair.first] = static_cast(pair.second); - } - return true; - } - map = default_value; - return false; -} - -bool NodeHandle::getParam(const std::string &key, std::map &map, std::map default_value) const -{ - YAML::Node value = getNestedValue(key); - - if (!value.IsDefined() || !value.IsMap()) - { - map = default_value; - return false; - } - - map.clear(); - try - { - for (auto it = value.begin(); it != value.end(); ++it) - { - std::string key_str = it->first.as(); - if (it->second.IsScalar()) - { - map[key_str] = it->second.as(); - } - else - { - return false; - } - } - return true; - } - catch (...) - { - return false; - } -} - -bool NodeHandle::getParam(const std::string &key, std::map &map, std::map default_value) const -{ - YAML::Node value = getNestedValue(key); - - if (!value.IsDefined() || !value.IsMap()) - { - map = default_value; - return false; - } - - map.clear(); - try - { - for (auto it = value.begin(); it != value.end(); ++it) - { - std::string key_str = it->first.as(); - if (it->second.IsScalar()) - { - map[key_str] = it->second.as(); - } - else - { - return false; - } - } - return true; - } - catch (...) - { - return false; - } -} - -bool NodeHandle::getParam(const std::string &key, YAML::Node &v, YAML::Node default_value) const -{ - YAML::Node value = getNestedValue(key); - - if (!value.IsDefined()) - { - v = default_value; - return false; - } - - v = value; - return true; -} - -} // namespace robot diff --git a/src/Libraries/robot_cpp/test/test_node_handle.cpp b/src/Libraries/robot_cpp/test/test_node_handle.cpp deleted file mode 100644 index 649a56f..0000000 --- a/src/Libraries/robot_cpp/test/test_node_handle.cpp +++ /dev/null @@ -1,839 +0,0 @@ -/********************************************************************* - * - * Test program for robot::NodeHandle - * Tests all functionality including YAML loading, getParam, setParam, etc. - * - *********************************************************************/ - -#include -#include -#include -#include -#include -#include -#include -#include - -// Test helper macros -#define TEST_ASSERT(condition, message) \ - do { \ - if (!(condition)) { \ - std::cerr << "FAIL: " << message << std::endl; \ - return false; \ - } \ - } while(0) - -#define TEST_PASS(message) \ - std::cout << "PASS: " << message << std::endl - -bool testBasicGetParam() -{ - std::cout << "\n=== Test 1: Basic getParam ===" << std::endl; - - robot::NodeHandle nh("~"); - - // Test scalar values - double controller_freq = 0.0; - if (nh.getParam("controller_frequency", controller_freq, 0.0)) - { - TEST_PASS("controller_frequency = " << controller_freq); - TEST_ASSERT(std::abs(controller_freq - 20.0) < 0.001, "controller_frequency should be 20.0"); - } - else - { - std::cout << "WARN: controller_frequency not found" << std::endl; - } - - double planner_freq = 0.0; - if (nh.getParam("planner_frequency", planner_freq, 0.0)) - { - TEST_PASS("planner_frequency = " << planner_freq); - TEST_ASSERT(std::abs(planner_freq - 0.0) < 0.001, "planner_frequency should be 0.0"); - } - else - { - std::cout << "WARN: planner_frequency not found" << std::endl; - } - - bool recovery_enabled = false; - if (nh.getParam("recovery_behavior_enabled", recovery_enabled, false)) - { - TEST_PASS("recovery_behavior_enabled = " << (recovery_enabled ? "true" : "false")); - TEST_ASSERT(recovery_enabled == true, "recovery_behavior_enabled should be true"); - } - else - { - std::cout << "WARN: recovery_behavior_enabled not found" << std::endl; - } - - // Test string - std::string base_planner; - if (nh.getParam("base_global_planner", base_planner, std::string(""))) - { - TEST_PASS("base_global_planner = " << base_planner); - } - else - { - std::cout << "WARN: base_global_planner not found" << std::endl; - } - - return true; -} - -bool testNestedKeys() -{ - std::cout << "\n=== Test 2: Nested Keys ===" << std::endl; - - robot::NodeHandle nh("~"); - - // Test nested keys - std::string global_frame; - if (nh.getParam("global_costmap/global_frame", global_frame, std::string(""))) - { - TEST_PASS("global_costmap/global_frame = " << global_frame); - TEST_ASSERT(global_frame == "map", "global_frame should be 'map'"); - } - else - { - std::cout << "WARN: global_costmap/global_frame not found" << std::endl; - } - - double resolution = 0.0; - if (nh.getParam("global_costmap/resolution", resolution, 0.0)) - { - TEST_PASS("global_costmap/resolution = " << resolution); - TEST_ASSERT(std::abs(resolution - 0.05) < 0.001, "resolution should be 0.05"); - } - else - { - std::cout << "WARN: global_costmap/resolution not found" << std::endl; - } - - // Test deeply nested - double cost_scaling = 0.0; - if (nh.getParam("global_costmap/inflation/cost_scaling_factor", cost_scaling, 0.0)) - { - TEST_PASS("global_costmap/inflation/cost_scaling_factor = " << cost_scaling); - TEST_ASSERT(std::abs(cost_scaling - 10.0) < 0.001, "cost_scaling_factor should be 10.0"); - } - else - { - std::cout << "WARN: global_costmap/inflation/cost_scaling_factor not found" << std::endl; - } - - return true; -} - -bool testSequences() -{ - std::cout << "\n=== Test 3: Sequences ===" << std::endl; - - robot::NodeHandle nh("~"); - - // Test recovery_behaviors sequence - YAML::Node behaviors = nh.getParamValue("recovery_behaviors"); - if (behaviors.IsDefined() && behaviors.IsSequence()) - { - TEST_PASS("recovery_behaviors found (sequence with " << behaviors.size() << " items)"); - TEST_ASSERT(behaviors.size() >= 2, "recovery_behaviors should have at least 2 items"); - - for (size_t i = 0; i < behaviors.size(); ++i) - { - if (behaviors[i].IsMap()) - { - std::string name = behaviors[i]["name"].as(); - std::string type = behaviors[i]["type"].as(); - std::cout << " Item [" << i << "]: name=" << name << ", type=" << type << std::endl; - } - } - } - else - { - std::cout << "WARN: recovery_behaviors not found or not a sequence" << std::endl; - } - - // Test vector of doubles - std::vector vec_double; - if (nh.getParam("local_costmap/plugins", vec_double, std::vector())) - { - // This might not be a vector of doubles, so just check if it exists - std::cout << "INFO: local_costmap/plugins found" << std::endl; - } - - return true; -} - -bool testSetParam() -{ - std::cout << "\n=== Test 4: setParam ===" << std::endl; - - // Create NodeHandle with a custom namespace to avoid auto-loading - robot::NodeHandle nh("test_namespace"); - - // Use unique prefix to avoid conflicts (node_handle_ is static) - std::string prefix = "test4_"; - - // Test setting various types - YAML::Node bool_node(true); - std::cout << "DEBUG: Before setParam - bool_node IsScalar: " << bool_node.IsScalar() - << ", Type: " << (int)bool_node.Type() << std::endl; - - nh.setParam(prefix + "test_bool", true); - nh.setParam(prefix + "test_int", 42); - nh.setParam(prefix + "test_double", 3.14); - nh.setParam(prefix + "test_string", std::string("hello")); - nh.setParam(prefix + "test_float", 2.5f); - - // Verify they can be retrieved - bool b = false; - TEST_ASSERT(nh.getParam(prefix + "test_bool", b, false), "getParam test_bool should succeed"); - TEST_ASSERT(b == true, "test_bool should be true"); - TEST_PASS("setParam/getParam bool works"); - - int i = 0; - TEST_ASSERT(nh.getParam(prefix + "test_int", i, 0), "getParam test_int should succeed"); - TEST_ASSERT(i == 42, "test_int should be 42"); - TEST_PASS("setParam/getParam int works"); - - double d = 0.0; - TEST_ASSERT(nh.getParam(prefix + "test_double", d, 0.0), "getParam test_double should succeed"); - TEST_ASSERT(std::abs(d - 3.14) < 0.001, "test_double should be 3.14"); - TEST_PASS("setParam/getParam double works"); - - std::string s; - TEST_ASSERT(nh.getParam(prefix + "test_string", s, std::string("")), "getParam test_string should succeed"); - TEST_ASSERT(s == "hello", "test_string should be 'hello'"); - TEST_PASS("setParam/getParam string works"); - - float f = 0.0f; - TEST_ASSERT(nh.getParam(prefix + "test_float", f, 0.0f), "getParam test_float should succeed"); - TEST_ASSERT(std::abs(f - 2.5f) < 0.001f, "test_float should be 2.5"); - TEST_PASS("setParam/getParam float works"); - - // Test nested setParam with unique prefix - nh.setParam(prefix + "nested/test/value", 100); - int nested_val = 0; - TEST_ASSERT(nh.getParam(prefix + "nested/test/value", nested_val, 0), "getParam nested/test/value should succeed"); - TEST_ASSERT(nested_val == 100, "nested/test/value should be 100"); - TEST_PASS("setParam/getParam nested keys works"); - - return true; -} - -bool testLoadYamlFile() -{ - std::cout << "\n=== Test 5: loadYamlFile ===" << std::endl; - - robot::NodeHandle nh; - - // Try to load a specific YAML file - std::string config_dir = robot::NodeHandle::getConfigDirectory(); - if (!config_dir.empty()) - { - std::string test_file = config_dir + "/move_base_common_params.yaml"; - if (nh.loadYamlFile(test_file)) - { - TEST_PASS("loadYamlFile succeeded for " << test_file); - - // Verify we can read from it - double controller_freq = 0.0; - if (nh.getParam("controller_frequency", controller_freq, 0.0)) - { - TEST_PASS("Can read controller_frequency from loaded file: " << controller_freq); - } - } - else - { - std::cout << "WARN: loadYamlFile failed for " << test_file << std::endl; - } - } - else - { - std::cout << "WARN: Config directory not found, skipping loadYamlFile test" << std::endl; - } - - return true; -} - -bool testMaps() -{ - std::cout << "\n=== Test 6: Maps ===" << std::endl; - - robot::NodeHandle nh("~"); - - // Test map of strings - std::map map_str; - // Note: Most YAML files don't have map structures at top level - // This is just to test the function exists and works - - // Test map of doubles (if exists) - std::map map_double; - // This might not exist, so we just test the function call doesn't crash - - return true; -} - -bool testGetParamValue() -{ - std::cout << "\n=== Test 7: getParamValue ===" << std::endl; - - robot::NodeHandle nh("~"); - - // Test getting a nested node - YAML::Node global_costmap = nh.getParamValue("global_costmap"); - if (global_costmap.IsDefined() && global_costmap.IsMap()) - { - TEST_PASS("getParamValue('global_costmap') returned a map with " << global_costmap.size() << " keys"); - - // Access nested values directly - if (global_costmap["resolution"].IsDefined()) - { - double res = global_costmap["resolution"].as(); - std::cout << " Direct access: global_costmap['resolution'] = " << res << std::endl; - } - } - else - { - std::cout << "WARN: global_costmap not found or not a map" << std::endl; - } - - return true; -} - -bool testNamespace() -{ - std::cout << "\n=== Test 8: Namespace ===" << std::endl; - - // Test default constructor - robot::NodeHandle nh1; - std::string ns1 = nh1.getNamespace(); - std::cout << "Default constructor namespace: '" << ns1 << "'" << std::endl; - - // Test with "~" - robot::NodeHandle nh2("~"); - std::string ns2 = nh2.getNamespace(); - std::cout << "NodeHandle('~') namespace: '" << ns2 << "'" << std::endl; - // Should be config directory path - if (!ns2.empty() && ns2 != "~") - { - TEST_PASS("NodeHandle('~') namespace is config directory: " << ns2); - } - - // Test with custom namespace - robot::NodeHandle nh3("custom_namespace"); - std::string ns3 = nh3.getNamespace(); - TEST_ASSERT(ns3 == "custom_namespace", "Custom namespace should be preserved"); - TEST_PASS("Custom namespace works: " << ns3); - - return true; -} - -bool testConfigDirectory() -{ - std::cout << "\n=== Test 9: Config Directory ===" << std::endl; - - std::string config_dir = robot::NodeHandle::getConfigDirectory(); - if (!config_dir.empty()) - { - TEST_PASS("Config directory found: " << config_dir); - - // Test setting custom config directory - robot::NodeHandle::setConfigDirectory("/tmp/test_config"); - std::string custom_dir = robot::NodeHandle::getConfigDirectory(); - std::cout << "After setConfigDirectory('/tmp/test_config'): " << custom_dir << std::endl; - - // Reset to original - robot::NodeHandle::setConfigDirectory(config_dir); - } - else - { - std::cout << "WARN: Config directory not found" << std::endl; - } - - return true; -} - -bool testSetParamAllTypes() -{ - std::cout << "\n=== Test 10: setParam All Types ===" << std::endl; - - robot::NodeHandle nh("test_setparam"); - - // Test bool - nh.setParam("test_bool", true); - bool b = false; - TEST_ASSERT(nh.getParam("test_bool", b, false), "getParam bool should succeed"); - TEST_ASSERT(b == true, "test_bool should be true"); - TEST_PASS("setParam/getParam bool works"); - - // Test const char* - nh.setParam("test_cstring", "test_string"); - std::string s1; - TEST_ASSERT(nh.getParam("test_cstring", s1, std::string("")), "getParam const char* should succeed"); - TEST_ASSERT(s1 == "test_string", "test_cstring should be 'test_string'"); - TEST_PASS("setParam/getParam const char* works"); - - // Test string - nh.setParam("test_string", std::string("hello_world")); - std::string s2; - TEST_ASSERT(nh.getParam("test_string", s2, std::string("")), "getParam string should succeed"); - TEST_ASSERT(s2 == "hello_world", "test_string should be 'hello_world'"); - TEST_PASS("setParam/getParam string works"); - - // Test int - nh.setParam("test_int", 123); - int i = 0; - TEST_ASSERT(nh.getParam("test_int", i, 0), "getParam int should succeed"); - TEST_ASSERT(i == 123, "test_int should be 123"); - TEST_PASS("setParam/getParam int works"); - - // Test double - nh.setParam("test_double", 3.14159); - double d = 0.0; - TEST_ASSERT(nh.getParam("test_double", d, 0.0), "getParam double should succeed"); - TEST_ASSERT(std::abs(d - 3.14159) < 0.0001, "test_double should be 3.14159"); - TEST_PASS("setParam/getParam double works"); - - return true; -} - -bool testSetParamVectors() -{ - std::cout << "\n=== Test 11: setParam Vectors ===" << std::endl; - - robot::NodeHandle nh("test_vectors"); - - // Test vector - std::vector vec_bool = {true, false, true}; - nh.setParam("test_vec_bool", vec_bool); - std::vector vec_bool_read; - TEST_ASSERT(nh.getParam("test_vec_bool", vec_bool_read, std::vector()), - "getParam vector should succeed"); - TEST_ASSERT(vec_bool_read.size() == 3, "vector should have 3 elements"); - TEST_ASSERT(vec_bool_read[0] == true && vec_bool_read[1] == false && vec_bool_read[2] == true, - "vector values should match"); - TEST_PASS("setParam/getParam vector works"); - - // Test vector - std::vector vec_int = {1, 2, 3, 4, 5}; - nh.setParam("test_vec_int", vec_int); - std::vector vec_int_read; - TEST_ASSERT(nh.getParam("test_vec_int", vec_int_read, std::vector()), - "getParam vector should succeed"); - TEST_ASSERT(vec_int_read.size() == 5, "vector should have 5 elements"); - TEST_ASSERT(vec_int_read[0] == 1 && vec_int_read[4] == 5, "vector values should match"); - TEST_PASS("setParam/getParam vector works"); - - // Test vector - std::vector vec_float = {1.1f, 2.2f, 3.3f}; - nh.setParam("test_vec_float", vec_float); - std::vector vec_float_read; - TEST_ASSERT(nh.getParam("test_vec_float", vec_float_read, std::vector()), - "getParam vector should succeed"); - TEST_ASSERT(vec_float_read.size() == 3, "vector should have 3 elements"); - TEST_PASS("setParam/getParam vector works"); - - // Test vector - std::vector vec_double = {1.5, 2.5, 3.5, 4.5}; - nh.setParam("test_vec_double", vec_double); - std::vector vec_double_read; - TEST_ASSERT(nh.getParam("test_vec_double", vec_double_read, std::vector()), - "getParam vector should succeed"); - TEST_ASSERT(vec_double_read.size() == 4, "vector should have 4 elements"); - TEST_PASS("setParam/getParam vector works"); - - // Test vector - std::vector vec_string = {"one", "two", "three"}; - nh.setParam("test_vec_string", vec_string); - std::vector vec_string_read; - TEST_ASSERT(nh.getParam("test_vec_string", vec_string_read, std::vector()), - "getParam vector should succeed"); - TEST_ASSERT(vec_string_read.size() == 3, "vector should have 3 elements"); - TEST_ASSERT(vec_string_read[0] == "one" && vec_string_read[2] == "three", - "vector values should match"); - TEST_PASS("setParam/getParam vector works"); - - return true; -} - -bool testSetParamMaps() -{ - std::cout << "\n=== Test 12: setParam Maps ===" << std::endl; - - robot::NodeHandle nh("test_maps"); - - // Test map - std::map map_bool = {{"key1", true}, {"key2", false}, {"key3", true}}; - nh.setParam("test_map_bool", map_bool); - std::map map_bool_read; - TEST_ASSERT(nh.getParam("test_map_bool", map_bool_read, std::map()), - "getParam map should succeed"); - TEST_ASSERT(map_bool_read.size() == 3, "map should have 3 elements"); - TEST_ASSERT(map_bool_read["key1"] == true && map_bool_read["key2"] == false, - "map values should match"); - TEST_PASS("setParam/getParam map works"); - - // Test map - std::map map_int = {{"a", 10}, {"b", 20}, {"c", 30}}; - nh.setParam("test_map_int", map_int); - std::map map_int_read; - TEST_ASSERT(nh.getParam("test_map_int", map_int_read, std::map()), - "getParam map should succeed"); - TEST_ASSERT(map_int_read.size() == 3, "map should have 3 elements"); - TEST_ASSERT(map_int_read["a"] == 10 && map_int_read["c"] == 30, - "map values should match"); - TEST_PASS("setParam/getParam map works"); - - // Test map - std::map map_double = {{"x", 1.1}, {"y", 2.2}, {"z", 3.3}}; - nh.setParam("test_map_double", map_double); - std::map map_double_read; - TEST_ASSERT(nh.getParam("test_map_double", map_double_read, std::map()), - "getParam map should succeed"); - TEST_ASSERT(map_double_read.size() == 3, "map should have 3 elements"); - TEST_PASS("setParam/getParam map works"); - - // Test map - std::map map_float = {{"f1", 1.5f}, {"f2", 2.5f}}; - nh.setParam("test_map_float", map_float); - std::map map_float_read; - TEST_ASSERT(nh.getParam("test_map_float", map_float_read, std::map()), - "getParam map should succeed"); - TEST_ASSERT(map_float_read.size() == 2, "map should have 2 elements"); - TEST_PASS("setParam/getParam map works"); - - // Test map - std::map map_string = {{"name", "test"}, {"value", "123"}}; - nh.setParam("test_map_string", map_string); - std::map map_string_read; - TEST_ASSERT(nh.getParam("test_map_string", map_string_read, std::map()), - "getParam map should succeed"); - TEST_ASSERT(map_string_read.size() == 2, "map should have 2 elements"); - TEST_ASSERT(map_string_read["name"] == "test" && map_string_read["value"] == "123", - "map values should match"); - TEST_PASS("setParam/getParam map works"); - - return true; -} - -bool testSetParamTypeChecking() -{ - std::cout << "\n=== Test 13: setParam Type Checking ===" << std::endl; - - robot::NodeHandle nh("test_type_check"); - - // Test 1: Set key with one type, then change to another type (should overwrite) - nh.setParam("test_type_change", 42); - int i = 0; - TEST_ASSERT(nh.getParam("test_type_change", i, 0), "Should get int value"); - TEST_ASSERT(i == 42, "Value should be 42"); - - // Change to string type - nh.setParam("test_type_change", std::string("changed")); - std::string s; - TEST_ASSERT(nh.getParam("test_type_change", s, std::string("")), "Should get string value"); - TEST_ASSERT(s == "changed", "Value should be 'changed'"); - TEST_PASS("Type change (int -> string) works"); - - // Test 2: Set key that doesn't exist (should create it) - nh.setParam("test_new_key", 100); - int new_val = 0; - TEST_ASSERT(nh.getParam("test_new_key", new_val, 0), "New key should be created"); - TEST_ASSERT(new_val == 100, "New key value should be 100"); - TEST_PASS("Creating new key works"); - - // Test 3: Set same type again (should update value) - nh.setParam("test_new_key", 200); - int updated_val = 0; - TEST_ASSERT(nh.getParam("test_new_key", updated_val, 0), "Should get updated value"); - TEST_ASSERT(updated_val == 200, "Updated value should be 200"); - TEST_PASS("Updating existing key with same type works"); - - return true; -} - -bool testGetParamAllTypes() -{ - std::cout << "\n=== Test 14: getParam All Types ===" << std::endl; - - robot::NodeHandle nh("test_getparam"); - - // Set up test data - nh.setParam("test_bool", true); - nh.setParam("test_int", 42); - nh.setParam("test_double", 3.14); - nh.setParam("test_float", 2.5f); - nh.setParam("test_string", std::string("test")); - - // Test getParam - bool b = false; - TEST_ASSERT(nh.getParam("test_bool", b, false), "getParam should succeed"); - TEST_ASSERT(b == true, "bool value should be true"); - TEST_PASS("getParam works"); - - // Test getParam - int i = 0; - TEST_ASSERT(nh.getParam("test_int", i, 0), "getParam should succeed"); - TEST_ASSERT(i == 42, "int value should be 42"); - TEST_PASS("getParam works"); - - // Test getParam - double d = 0.0; - TEST_ASSERT(nh.getParam("test_double", d, 0.0), "getParam should succeed"); - TEST_ASSERT(std::abs(d - 3.14) < 0.001, "double value should be 3.14"); - TEST_PASS("getParam works"); - - // Test getParam - float f = 0.0f; - TEST_ASSERT(nh.getParam("test_float", f, 0.0f), "getParam should succeed"); - TEST_ASSERT(std::abs(f - 2.5f) < 0.001f, "float value should be 2.5"); - TEST_PASS("getParam works"); - - // Test getParam - std::string s; - TEST_ASSERT(nh.getParam("test_string", s, std::string("")), "getParam should succeed"); - TEST_ASSERT(s == "test", "string value should be 'test'"); - TEST_PASS("getParam works"); - - // Test getParam with non-existent key (should return default) - int default_val = 999; - TEST_ASSERT(!nh.getParam("non_existent_key", default_val, 999), - "getParam for non-existent key should return false"); - TEST_ASSERT(default_val == 999, "Default value should be used"); - TEST_PASS("getParam with default value works"); - - return true; -} - -bool testGetParamYAMLNode() -{ - std::cout << "\n=== Test 15: getParam YAML::Node ===" << std::endl; - - robot::NodeHandle nh("test_yaml_node"); - - // Use a unique key to avoid conflicts with other tests (node_handle_ is static) - std::string unique_key = "test15_unique/level1/level2/value"; - - // Set up nested structure - nh.setParam(unique_key, 100); - - // Test getParam - YAML::Node node; - TEST_ASSERT(nh.getParam(unique_key, node, YAML::Node()), - "getParam should succeed"); - TEST_ASSERT(node.IsDefined(), "Node should be defined"); - - // Try to get value - if it's scalar, great; if not, try to convert anyway - int value = 0; - if (node.IsScalar()) - { - value = node.as(); - TEST_ASSERT(value == 100, "Node value should be 100"); - TEST_PASS("getParam works (scalar)"); - } - else - { - // If not scalar, it might be stored differently due to static node_handle_ sharing - // Try to get value using getParam instead - int direct_value = 0; - if (nh.getParam(unique_key, direct_value, 0)) - { - TEST_ASSERT(direct_value == 100, "Node value should be 100"); - TEST_PASS("getParam works (using getParam as fallback)"); - } - else - { - // If both fail, that's a problem - TEST_ASSERT(false, "Node should be retrievable either as YAML::Node or int"); - } - } - - // Test getParamValue with a simpler nested structure - nh.setParam("test15_simple/nested", 42); - YAML::Node nested = nh.getParamValue("test15_simple"); - TEST_ASSERT(nested.IsDefined(), "getParamValue should return defined node"); - TEST_ASSERT(nested.IsMap(), "Nested node should be a map"); - TEST_PASS("getParamValue works"); - - return true; -} - -bool testMergeYamlNode() -{ - std::cout << "\n=== Test 16: mergeYamlNode ===" << std::endl; - - robot::NodeHandle nh("test_merge"); - - // Use unique prefix to avoid conflicts with other tests (node_handle_ is static) - std::string prefix = "test16_"; - - // Create first YAML node - YAML::Node node1; - node1[prefix + "key1"] = "value1"; - node1[prefix + "key2"] = 42; - node1[prefix + "nested"]["a"] = 1; - node1[prefix + "nested"]["b"] = 2; - - // Merge first node - nh.mergeYamlNode(node1); - - // Debug: Print all params after first merge - std::cout << "DEBUG: After first merge:" << std::endl; - nh.printAllParams(); - - // Verify merged values - std::string s; - bool key1_exists = nh.getParam(prefix + "key1", s, std::string("")); - std::cout << "DEBUG: key1 exists: " << key1_exists << ", value: " << s << std::endl; - TEST_ASSERT(key1_exists, "key1 should exist"); - TEST_ASSERT(s == "value1", "key1 should be 'value1'"); - - // Try getParamValue first to see what we get - YAML::Node key2_node = nh.getParamValue(prefix + "key2"); - std::cout << "DEBUG: key2_node from getParamValue:" << std::endl; - std::cout << " IsDefined: " << key2_node.IsDefined() << std::endl; - std::cout << " IsScalar: " << key2_node.IsScalar() << std::endl; - std::cout << " Type: " << (int)key2_node.Type() << std::endl; - - // Try getParam - if it fails, the value might be stored but getParam can't read it - // This could be due to static node_handle_ sharing issues - int i = 0; - bool key2_exists = nh.getParam(prefix + "key2", i, 0); - if (!key2_exists) - { - // If getParam failed, check if we can at least verify the value exists via printAllParams - // For now, we'll mark this as a known issue with static node_handle_ - std::cout << "WARN: getParam failed for key2, but value exists in node_handle_ (static sharing issue)" << std::endl; - // Try to verify via getParamValue and manual conversion - if (key2_node.IsDefined()) - { - try { - // Try to dump and parse - std::string dumped = YAML::Dump(key2_node); - std::cout << "DEBUG: key2_node dumped: " << dumped << std::endl; - // For now, just verify the node exists - TEST_PASS("key2 exists in node_handle_ (getParam has issue with static node_handle_)"); - return true; // Skip the strict check - } catch (...) { - } - } - } - std::cout << "DEBUG: key2 exists: " << key2_exists << ", value: " << i << std::endl; - if (key2_exists) - { - TEST_ASSERT(i == 42, "key2 should be 42"); - } - else - { - // If getParam fails, we'll accept it as a known limitation - TEST_PASS("key2 verified via printAllParams (getParam limitation with static node_handle_)"); - } - - // Create second YAML node with overlapping keys - YAML::Node node2; - node2[prefix + "key2"] = 100; // Overwrite key2 - node2[prefix + "key3"] = "new_value"; // New key - node2[prefix + "nested"]["c"] = 3; // Add to nested - - // Merge second node - nh.mergeYamlNode(node2); - - // Verify merged values - TEST_ASSERT(nh.getParam(prefix + "key2", i, 0), "key2 should still exist"); - TEST_ASSERT(i == 100, "key2 should be overwritten to 100"); - - TEST_ASSERT(nh.getParam(prefix + "key3", s, std::string("")), "key3 should exist"); - TEST_ASSERT(s == "new_value", "key3 should be 'new_value'"); - - TEST_PASS("mergeYamlNode works correctly"); - - return true; -} - -bool testLoadYamlFilesFromDirectory() -{ - std::cout << "\n=== Test 17: loadYamlFilesFromDirectory ===" << std::endl; - - robot::NodeHandle nh("test_load_dir"); - - std::string config_dir = robot::NodeHandle::getConfigDirectory(); - if (!config_dir.empty()) - { - int count = nh.loadYamlFilesFromDirectory(config_dir); - std::cout << "Loaded " << count << " YAML files from " << config_dir << std::endl; - TEST_ASSERT(count > 0, "Should load at least one YAML file"); - TEST_PASS("loadYamlFilesFromDirectory works"); - } - else - { - std::cout << "WARN: Config directory not found, skipping test" << std::endl; - } - - return true; -} - -bool testPrintAllParams() -{ - std::cout << "\n=== Test 18: printAllParams ===" << std::endl; - - robot::NodeHandle nh("test_print"); - - // Set up some test data - nh.setParam("test1", 1); - nh.setParam("test2", std::string("value2")); - nh.setParam("nested/test3", 3.14); - - // Call printAllParams (should not crash) - nh.printAllParams(); - TEST_PASS("printAllParams executed without errors"); - - return true; -} - -int main(int /*argc*/, char** /*argv*/) -{ - std::cout << "========================================" << std::endl; - std::cout << "NodeHandle Test Suite" << std::endl; - std::cout << "========================================" << std::endl; - - bool all_passed = true; - - try - { - all_passed &= testBasicGetParam(); - all_passed &= testNestedKeys(); - all_passed &= testSequences(); - all_passed &= testSetParam(); - all_passed &= testLoadYamlFile(); - all_passed &= testMaps(); - all_passed &= testGetParamValue(); - all_passed &= testNamespace(); - all_passed &= testConfigDirectory(); - all_passed &= testSetParamAllTypes(); - all_passed &= testSetParamVectors(); - all_passed &= testSetParamMaps(); - all_passed &= testSetParamTypeChecking(); - all_passed &= testGetParamAllTypes(); - all_passed &= testGetParamYAMLNode(); - all_passed &= testMergeYamlNode(); - all_passed &= testLoadYamlFilesFromDirectory(); - all_passed &= testPrintAllParams(); - } - catch (const std::exception& e) - { - std::cerr << "EXCEPTION: " << e.what() << std::endl; - return 1; - } - - std::cout << "\n========================================" << std::endl; - if (all_passed) - { - std::cout << "All tests PASSED!" << std::endl; - return 0; - } - else - { - std::cout << "Some tests FAILED!" << std::endl; - return 1; - } -} diff --git a/src/Navigations/Packages/move_base/src/move_base.cpp b/src/Navigations/Packages/move_base/src/move_base.cpp index 1e00a32..2f8b84f 100644 --- a/src/Navigations/Packages/move_base/src/move_base.cpp +++ b/src/Navigations/Packages/move_base/src/move_base.cpp @@ -73,7 +73,6 @@ void move_base::MoveBase::initialize(TFListenerPtr tf) // NodeHandle("~") will automatically load YAML files from config directory robot::NodeHandle nh("~"); private_nh_ = nh; - printf("[%s:%d] private_nh_.getNamespace(): %s\n", __FILE__, __LINE__, private_nh_.getNamespace().c_str()); recovery_trigger_ = PLANNING_R; // get some parameters that will be global to the move base node @@ -776,6 +775,7 @@ bool move_base::MoveBase::loadRecoveryBehaviors(const robot::NodeHandle &node) // check for recovery behaviors with the same name std::string name_i = behavior["name"].as(); + printf("[%s:%d] name_i: %s\n", __FILE__, __LINE__, name_i.c_str()); for (size_t j = i + 1; j < behavior_list.size(); ++j) { YAML::Node behavior_j = behavior_list[j].as();