update
This commit is contained in:
parent
33afdfcc5c
commit
bdd78c5f1e
3
config1/two_points_global_params.yaml
Normal file
3
config1/two_points_global_params.yaml
Normal file
|
|
@ -0,0 +1,3 @@
|
|||
base_global_planner: TwoPointsPlanner
|
||||
TwoPointsPlanner:
|
||||
lethal_obstacle: 20
|
||||
Binary file not shown.
14
src/Libraries/robot_cpp/.gitignore
vendored
14
src/Libraries/robot_cpp/.gitignore
vendored
|
|
@ -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
|
||||
|
||||
|
|
@ -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
|
||||
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
|
||||
$<INSTALL_INTERFACE:include>
|
||||
)
|
||||
|
||||
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
|
||||
$<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)
|
||||
|
||||
# ========================================================
|
||||
# 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()
|
||||
|
|
@ -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.
|
||||
|
|
@ -1,2 +0,0 @@
|
|||
# robot_cpp
|
||||
|
||||
|
|
@ -1,89 +0,0 @@
|
|||
#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
|
||||
|
||||
|
|
@ -1,156 +0,0 @@
|
|||
#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>
|
||||
|
||||
// Forward declaration for XmlRpcValue
|
||||
namespace XmlRpc {
|
||||
class XmlRpcValue;
|
||||
}
|
||||
|
||||
namespace robot
|
||||
{
|
||||
// Type alias for remappings (map of string to string)
|
||||
using M_string = std::map<std::string, std::string>;
|
||||
|
||||
class NodeHandle
|
||||
{
|
||||
public:
|
||||
using Ptr = std::shared_ptr<NodeHandle>;
|
||||
|
||||
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
|
||||
|
|
@ -1,207 +0,0 @@
|
|||
#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
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
|
|
@ -1,839 +0,0 @@
|
|||
/*********************************************************************
|
||||
*
|
||||
* Test program for robot::NodeHandle
|
||||
* Tests all functionality including YAML loading, getParam, setParam, etc.
|
||||
*
|
||||
*********************************************************************/
|
||||
|
||||
#include <robot/node_handle.h>
|
||||
#include <yaml-cpp/yaml.h>
|
||||
#include <iostream>
|
||||
#include <cassert>
|
||||
#include <cmath>
|
||||
#include <map>
|
||||
#include <vector>
|
||||
#include <string>
|
||||
|
||||
// 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>();
|
||||
std::string type = behaviors[i]["type"].as<std::string>();
|
||||
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<double> vec_double;
|
||||
if (nh.getParam("local_costmap/plugins", vec_double, std::vector<double>()))
|
||||
{
|
||||
// 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<std::string, std::string> 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<std::string, double> 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<double>();
|
||||
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<bool>
|
||||
std::vector<bool> vec_bool = {true, false, true};
|
||||
nh.setParam("test_vec_bool", vec_bool);
|
||||
std::vector<bool> vec_bool_read;
|
||||
TEST_ASSERT(nh.getParam("test_vec_bool", vec_bool_read, std::vector<bool>()),
|
||||
"getParam vector<bool> should succeed");
|
||||
TEST_ASSERT(vec_bool_read.size() == 3, "vector<bool> should have 3 elements");
|
||||
TEST_ASSERT(vec_bool_read[0] == true && vec_bool_read[1] == false && vec_bool_read[2] == true,
|
||||
"vector<bool> values should match");
|
||||
TEST_PASS("setParam/getParam vector<bool> works");
|
||||
|
||||
// Test vector<int>
|
||||
std::vector<int> vec_int = {1, 2, 3, 4, 5};
|
||||
nh.setParam("test_vec_int", vec_int);
|
||||
std::vector<int> vec_int_read;
|
||||
TEST_ASSERT(nh.getParam("test_vec_int", vec_int_read, std::vector<int>()),
|
||||
"getParam vector<int> should succeed");
|
||||
TEST_ASSERT(vec_int_read.size() == 5, "vector<int> should have 5 elements");
|
||||
TEST_ASSERT(vec_int_read[0] == 1 && vec_int_read[4] == 5, "vector<int> values should match");
|
||||
TEST_PASS("setParam/getParam vector<int> works");
|
||||
|
||||
// Test vector<float>
|
||||
std::vector<float> vec_float = {1.1f, 2.2f, 3.3f};
|
||||
nh.setParam("test_vec_float", vec_float);
|
||||
std::vector<float> vec_float_read;
|
||||
TEST_ASSERT(nh.getParam("test_vec_float", vec_float_read, std::vector<float>()),
|
||||
"getParam vector<float> should succeed");
|
||||
TEST_ASSERT(vec_float_read.size() == 3, "vector<float> should have 3 elements");
|
||||
TEST_PASS("setParam/getParam vector<float> works");
|
||||
|
||||
// Test vector<double>
|
||||
std::vector<double> vec_double = {1.5, 2.5, 3.5, 4.5};
|
||||
nh.setParam("test_vec_double", vec_double);
|
||||
std::vector<double> vec_double_read;
|
||||
TEST_ASSERT(nh.getParam("test_vec_double", vec_double_read, std::vector<double>()),
|
||||
"getParam vector<double> should succeed");
|
||||
TEST_ASSERT(vec_double_read.size() == 4, "vector<double> should have 4 elements");
|
||||
TEST_PASS("setParam/getParam vector<double> works");
|
||||
|
||||
// Test vector<string>
|
||||
std::vector<std::string> vec_string = {"one", "two", "three"};
|
||||
nh.setParam("test_vec_string", vec_string);
|
||||
std::vector<std::string> vec_string_read;
|
||||
TEST_ASSERT(nh.getParam("test_vec_string", vec_string_read, std::vector<std::string>()),
|
||||
"getParam vector<string> should succeed");
|
||||
TEST_ASSERT(vec_string_read.size() == 3, "vector<string> should have 3 elements");
|
||||
TEST_ASSERT(vec_string_read[0] == "one" && vec_string_read[2] == "three",
|
||||
"vector<string> values should match");
|
||||
TEST_PASS("setParam/getParam vector<string> works");
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool testSetParamMaps()
|
||||
{
|
||||
std::cout << "\n=== Test 12: setParam Maps ===" << std::endl;
|
||||
|
||||
robot::NodeHandle nh("test_maps");
|
||||
|
||||
// Test map<string, bool>
|
||||
std::map<std::string, bool> map_bool = {{"key1", true}, {"key2", false}, {"key3", true}};
|
||||
nh.setParam("test_map_bool", map_bool);
|
||||
std::map<std::string, bool> map_bool_read;
|
||||
TEST_ASSERT(nh.getParam("test_map_bool", map_bool_read, std::map<std::string, bool>()),
|
||||
"getParam map<string, bool> should succeed");
|
||||
TEST_ASSERT(map_bool_read.size() == 3, "map<string, bool> should have 3 elements");
|
||||
TEST_ASSERT(map_bool_read["key1"] == true && map_bool_read["key2"] == false,
|
||||
"map<string, bool> values should match");
|
||||
TEST_PASS("setParam/getParam map<string, bool> works");
|
||||
|
||||
// Test map<string, int>
|
||||
std::map<std::string, int> map_int = {{"a", 10}, {"b", 20}, {"c", 30}};
|
||||
nh.setParam("test_map_int", map_int);
|
||||
std::map<std::string, int> map_int_read;
|
||||
TEST_ASSERT(nh.getParam("test_map_int", map_int_read, std::map<std::string, int>()),
|
||||
"getParam map<string, int> should succeed");
|
||||
TEST_ASSERT(map_int_read.size() == 3, "map<string, int> should have 3 elements");
|
||||
TEST_ASSERT(map_int_read["a"] == 10 && map_int_read["c"] == 30,
|
||||
"map<string, int> values should match");
|
||||
TEST_PASS("setParam/getParam map<string, int> works");
|
||||
|
||||
// Test map<string, double>
|
||||
std::map<std::string, double> map_double = {{"x", 1.1}, {"y", 2.2}, {"z", 3.3}};
|
||||
nh.setParam("test_map_double", map_double);
|
||||
std::map<std::string, double> map_double_read;
|
||||
TEST_ASSERT(nh.getParam("test_map_double", map_double_read, std::map<std::string, double>()),
|
||||
"getParam map<string, double> should succeed");
|
||||
TEST_ASSERT(map_double_read.size() == 3, "map<string, double> should have 3 elements");
|
||||
TEST_PASS("setParam/getParam map<string, double> works");
|
||||
|
||||
// Test map<string, float>
|
||||
std::map<std::string, float> map_float = {{"f1", 1.5f}, {"f2", 2.5f}};
|
||||
nh.setParam("test_map_float", map_float);
|
||||
std::map<std::string, float> map_float_read;
|
||||
TEST_ASSERT(nh.getParam("test_map_float", map_float_read, std::map<std::string, float>()),
|
||||
"getParam map<string, float> should succeed");
|
||||
TEST_ASSERT(map_float_read.size() == 2, "map<string, float> should have 2 elements");
|
||||
TEST_PASS("setParam/getParam map<string, float> works");
|
||||
|
||||
// Test map<string, string>
|
||||
std::map<std::string, std::string> map_string = {{"name", "test"}, {"value", "123"}};
|
||||
nh.setParam("test_map_string", map_string);
|
||||
std::map<std::string, std::string> map_string_read;
|
||||
TEST_ASSERT(nh.getParam("test_map_string", map_string_read, std::map<std::string, std::string>()),
|
||||
"getParam map<string, string> should succeed");
|
||||
TEST_ASSERT(map_string_read.size() == 2, "map<string, string> should have 2 elements");
|
||||
TEST_ASSERT(map_string_read["name"] == "test" && map_string_read["value"] == "123",
|
||||
"map<string, string> values should match");
|
||||
TEST_PASS("setParam/getParam map<string, string> 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>
|
||||
bool b = false;
|
||||
TEST_ASSERT(nh.getParam("test_bool", b, false), "getParam<bool> should succeed");
|
||||
TEST_ASSERT(b == true, "bool value should be true");
|
||||
TEST_PASS("getParam<bool> works");
|
||||
|
||||
// Test getParam<int>
|
||||
int i = 0;
|
||||
TEST_ASSERT(nh.getParam("test_int", i, 0), "getParam<int> should succeed");
|
||||
TEST_ASSERT(i == 42, "int value should be 42");
|
||||
TEST_PASS("getParam<int> works");
|
||||
|
||||
// Test getParam<double>
|
||||
double d = 0.0;
|
||||
TEST_ASSERT(nh.getParam("test_double", d, 0.0), "getParam<double> should succeed");
|
||||
TEST_ASSERT(std::abs(d - 3.14) < 0.001, "double value should be 3.14");
|
||||
TEST_PASS("getParam<double> works");
|
||||
|
||||
// Test getParam<float>
|
||||
float f = 0.0f;
|
||||
TEST_ASSERT(nh.getParam("test_float", f, 0.0f), "getParam<float> should succeed");
|
||||
TEST_ASSERT(std::abs(f - 2.5f) < 0.001f, "float value should be 2.5");
|
||||
TEST_PASS("getParam<float> works");
|
||||
|
||||
// Test getParam<string>
|
||||
std::string s;
|
||||
TEST_ASSERT(nh.getParam("test_string", s, std::string("")), "getParam<string> should succeed");
|
||||
TEST_ASSERT(s == "test", "string value should be 'test'");
|
||||
TEST_PASS("getParam<string> 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>
|
||||
YAML::Node node;
|
||||
TEST_ASSERT(nh.getParam(unique_key, node, YAML::Node()),
|
||||
"getParam<YAML::Node> 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<int>();
|
||||
TEST_ASSERT(value == 100, "Node value should be 100");
|
||||
TEST_PASS("getParam<YAML::Node> works (scalar)");
|
||||
}
|
||||
else
|
||||
{
|
||||
// If not scalar, it might be stored differently due to static node_handle_ sharing
|
||||
// Try to get value using getParam<int> 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<YAML::Node> works (using getParam<int> 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<int> - 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;
|
||||
}
|
||||
}
|
||||
|
|
@ -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<std::string>();
|
||||
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<YAML::Node>();
|
||||
|
|
|
|||
Loading…
Reference in New Issue
Block a user