created

src/Libraries/robot_cpp/CMakeLists.txt

@@ -0,0 +1,69 @@
+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
+    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+    $<INSTALL_INTERFACE:include>
+)
+
+target_link_libraries(${PROJECT_NAME}_node_handle
+  PUBLIC
+    yaml-cpp
+    xmlrpcpp
+)
+
+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
+    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+    $<INSTALL_INTERFACE:include>
+)
+
+# 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)

src/Libraries/robot_cpp/include/robot/console.h

@@ -0,0 +1,89 @@
+#ifndef ROBOT_CONSOLE_H
+#define ROBOT_CONSOLE_H
+
+#include <cstdio>
+#include <cstdarg>
+
+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
+

src/Libraries/robot_cpp/include/robot/node_handle.h

@@ -0,0 +1,83 @@
+#ifndef ROBOT_NODE_H_INCLUDED_H
+#define ROBOT_NODE_H_INCLUDED_H
+
+#include <yaml-cpp/yaml.h>
+#include <memory>
+#include <string>
+#include <map>
+#include <vector>
+
+namespace robot
+{
+  class NodeHandle
+  {
+  public:
+    using Ptr = std::shared_ptr<NodeHandle>;
+
+    NodeHandle();
+
+    NodeHandle(const std::string &name); 
+
+    ~NodeHandle();
+
+    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);
+    
+    // Overloads for basic types
+    void setParam(const std::string &key, bool value);
+    void setParam(const std::string &key, int value);
+    void setParam(const std::string &key, double value);
+    void setParam(const std::string &key, float value);
+    void setParam(const std::string &key, const std::string &value);
+
+    // 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);
+
+    // Get namespace
+    std::string getNamespace() const;
+ 	
+  private:
+    std::string namespace_;
+    YAML::Node node_handle_;
+
+    // Helper method to split key path and get nested value
+    YAML::Node getNestedValue(const std::string &key) const;
+  };
+} // namespace robot
+
+#endif // ROBOT_NODE_H_INCLUDED_H

src/Libraries/robot_cpp/src/console.cpp

@@ -0,0 +1,207 @@
+#include "robot/console.h"
+#include <cstdlib>
+#include <cstring>
+
+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
+

src/Libraries/robot_cpp/src/node_handle.cpp

@@ -0,0 +1,631 @@
+#include <robot/node_handle.h>
+#include <sstream>
+#include <algorithm>
+#include <cctype>
+#include <fstream>
+
+namespace robot
+{
+
+NodeHandle::NodeHandle() : namespace_("")
+{
+  node_handle_ = YAML::Node();
+}
+
+NodeHandle::NodeHandle(const std::string &name) : namespace_(name)
+{
+  node_handle_ = YAML::Node();
+}
+
+NodeHandle::~NodeHandle()
+{
+}
+
+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<std::string> segments;
+  
+  while (std::getline(ss, segment, '/'))
+  {
+    if (!segment.empty())
+    {
+      segments.push_back(segment);
+    }
+  }
+
+  if (segments.empty())
+  {
+    return node_handle_;
+  }
+
+  YAML::Node current = node_handle_;
+  
+  for (size_t i = 0; i < segments.size(); ++i)
+  {
+    if (!current.IsMap())
+    {
+      return YAML::Node();
+    }
+    
+    if (!current[segments[i]].IsDefined())
+    {
+      return YAML::Node();
+    }
+    
+    current = current[segments[i]];
+  }
+  
+  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<std::string> segments;
+  
+  while (std::getline(ss, segment, '/'))
+  {
+    if (!segment.empty())
+    {
+      segments.push_back(segment);
+    }
+  }
+
+  if (segments.empty())
+  {
+    node_handle_ = value;
+    return;
+  }
+
+  YAML::Node current = node_handle_;
+  
+  for (size_t i = 0; i < segments.size() - 1; ++i)
+  {
+    if (!current[segments[i]].IsDefined() || !current[segments[i]].IsMap())
+    {
+      current[segments[i]] = YAML::Node(YAML::NodeType::Map);
+    }
+    current = current[segments[i]];
+  }
+  
+  // Set the final value
+  current[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
+{
+  return namespace_;
+}
+
+void NodeHandle::setParam(const std::string &key, bool value)
+{
+  setParam(key, YAML::Node(value));
+}
+
+void NodeHandle::setParam(const std::string &key, int value)
+{
+  setParam(key, YAML::Node(value));
+}
+
+void NodeHandle::setParam(const std::string &key, double value)
+{
+  setParam(key, YAML::Node(value));
+}
+
+void NodeHandle::setParam(const std::string &key, float value)
+{
+  setParam(key, YAML::Node(static_cast<double>(value)));
+}
+
+void NodeHandle::setParam(const std::string &key, const std::string &value)
+{
+  setParam(key, YAML::Node(value));
+}
+
+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::string>();
+        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<bool>();
+      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<double>();
+    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<double>(default_value)))
+  {
+    f = static_cast<float>(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<std::string>();
+    // Handle hex format
+    if (str.length() > 2 && str.substr(0, 2) == "0x")
+    {
+      i = std::stoi(str, nullptr, 16);
+    }
+    else
+    {
+      i = value.as<int>();
+    }
+    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 (!value.IsDefined() || !value.IsScalar())
+  {
+    s = default_value;
+    return false;
+  }
+  
+  try
+  {
+    s = value.as<std::string>();
+    return true;
+  }
+  catch (...)
+  {
+    s = default_value;
+    return false;
+  }
+}
+
+bool NodeHandle::getParam(const std::string &key, std::vector<bool> &vec, std::vector<bool> 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<bool>();
+          vec.push_back(b);
+        }
+        catch (...)
+        {
+          // Try as string and convert
+          std::string str = value[i].as<std::string>();
+          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<double> &vec, std::vector<double> 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<double>());
+    }
+    return true;
+  }
+  catch (...)
+  {
+    return false;
+  }
+}
+
+bool NodeHandle::getParam(const std::string &key, std::vector<float> &vec, std::vector<float> default_value) const
+{
+  std::vector<double> dvec;
+  std::vector<double> ddefault;
+  ddefault.reserve(default_value.size());
+  for (float f : default_value)
+  {
+    ddefault.push_back(static_cast<double>(f));
+  }
+  
+  if (getParam(key, dvec, ddefault))
+  {
+    vec.clear();
+    vec.reserve(dvec.size());
+    for (double d : dvec)
+    {
+      vec.push_back(static_cast<float>(d));
+    }
+    return true;
+  }
+  vec = default_value;
+  return false;
+}
+
+bool NodeHandle::getParam(const std::string &key, std::vector<int> &vec, std::vector<int> 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<int>());
+    }
+    return true;
+  }
+  catch (...)
+  {
+    return false;
+  }
+}
+
+bool NodeHandle::getParam(const std::string &key, std::vector<std::string> &vec, std::vector<std::string> 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<std::string>());
+    }
+    return true;
+  }
+  catch (...)
+  {
+    return false;
+  }
+}
+
+bool NodeHandle::getParam(const std::string &key, std::map<std::string, bool> &map, std::map<std::string, bool> 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<std::string>();
+      if (it->second.IsScalar())
+      {
+        try
+        {
+          bool b = it->second.as<bool>();
+          map[key_str] = b;
+        }
+        catch (...)
+        {
+          // Try as string and convert
+          std::string str = it->second.as<std::string>();
+          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<std::string, double> &map, std::map<std::string, double> 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<std::string>();
+      if (it->second.IsScalar())
+      {
+        map[key_str] = it->second.as<double>();
+      }
+      else
+      {
+        return false;
+      }
+    }
+    return true;
+  }
+  catch (...)
+  {
+    return false;
+  }
+}
+
+bool NodeHandle::getParam(const std::string &key, std::map<std::string, float> &map, std::map<std::string, float> default_value) const
+{
+  std::map<std::string, double> dmap;
+  std::map<std::string, double> ddefault;
+  for (const auto &pair : default_value)
+  {
+    ddefault[pair.first] = static_cast<double>(pair.second);
+  }
+  
+  if (getParam(key, dmap, ddefault))
+  {
+    map.clear();
+    for (const auto &pair : dmap)
+    {
+      map[pair.first] = static_cast<float>(pair.second);
+    }
+    return true;
+  }
+  map = default_value;
+  return false;
+}
+
+bool NodeHandle::getParam(const std::string &key, std::map<std::string, int> &map, std::map<std::string, int> 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<std::string>();
+      if (it->second.IsScalar())
+      {
+        map[key_str] = it->second.as<int>();
+      }
+      else
+      {
+        return false;
+      }
+    }
+    return true;
+  }
+  catch (...)
+  {
+    return false;
+  }
+}
+
+bool NodeHandle::getParam(const std::string &key, std::map<std::string, std::string> &map, std::map<std::string, std::string> 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<std::string>();
+      if (it->second.IsScalar())
+      {
+        map[key_str] = it->second.as<std::string>();
+      }
+      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