HiepLM/robot_cpp
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

first commit

Browse Source
This commit is contained in:
HiepLM
2026-01-31 17:20:07 +07:00
commit 091aad07d4
17 changed files with 6500 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

525
include/robot/console.h Normal file
View File

@@ -0,0 +1,525 @@
#ifndef ROBOT_CONSOLE_H
#define ROBOT_CONSOLE_H
#include <cstdio>
#include <cstdarg>
namespace robot
{
/**
* @namespace color
* @brief ANSI color code constants for terminal output formatting.
*
* This namespace provides ANSI escape sequences for colored terminal output.
* All color codes are compatible with terminals that support ANSI escape codes.
* Colors are automatically disabled if the terminal doesn't support them or if
* the NO_COLOR environment variable is set.
*/
namespace color
{
/**
* @brief ANSI escape code to reset all formatting.
*
* Resets text color, background color, and all text styles to default.
*/
static const char *RESET = "\033[0m";
// Text colors
/**
* @brief ANSI escape code for black text color.
*/
static const char *BLACK = "\033[30m";
/**
* @brief ANSI escape code for red text color.
*/
static const char *RED = "\033[31m";
/**
* @brief ANSI escape code for green text color.
*/
static const char *GREEN = "\033[32m";
/**
* @brief ANSI escape code for yellow text color.
*/
static const char *YELLOW = "\033[33m";
/**
* @brief ANSI escape code for blue text color.
*/
static const char *BLUE = "\033[34m";
/**
* @brief ANSI escape code for magenta text color.
*/
static const char *MAGENTA = "\033[35m";
/**
* @brief ANSI escape code for cyan text color.
*/
static const char *CYAN = "\033[36m";
/**
* @brief ANSI escape code for white text color.
*/
static const char *WHITE = "\033[37m";
// Bright text colors
/**
* @brief ANSI escape code for bright black text color.
*/
static const char *BRIGHT_BLACK = "\033[90m";
/**
* @brief ANSI escape code for bright red text color.
*/
static const char *BRIGHT_RED = "\033[91m";
/**
* @brief ANSI escape code for bright green text color.
*/
static const char *BRIGHT_GREEN = "\033[92m";
/**
* @brief ANSI escape code for bright yellow text color.
*/
static const char *BRIGHT_YELLOW = "\033[93m";
/**
* @brief ANSI escape code for bright blue text color.
*/
static const char *BRIGHT_BLUE = "\033[94m";
/**
* @brief ANSI escape code for bright magenta text color.
*/
static const char *BRIGHT_MAGENTA = "\033[95m";
/**
* @brief ANSI escape code for bright cyan text color.
*/
static const char *BRIGHT_CYAN = "\033[96m";
/**
* @brief ANSI escape code for bright white text color.
*/
static const char *BRIGHT_WHITE = "\033[97m";
// Background colors
/**
* @brief ANSI escape code for black background color.
*/
static const char *BG_BLACK = "\033[40m";
/**
* @brief ANSI escape code for red background color.
*/
static const char *BG_RED = "\033[41m";
/**
* @brief ANSI escape code for green background color.
*/
static const char *BG_GREEN = "\033[42m";
/**
* @brief ANSI escape code for yellow background color.
*/
static const char *BG_YELLOW = "\033[43m";
/**
* @brief ANSI escape code for blue background color.
*/
static const char *BG_BLUE = "\033[44m";
/**
* @brief ANSI escape code for magenta background color.
*/
static const char *BG_MAGENTA = "\033[45m";
/**
* @brief ANSI escape code for cyan background color.
*/
static const char *BG_CYAN = "\033[46m";
/**
* @brief ANSI escape code for white background color.
*/
static const char *BG_WHITE = "\033[47m";
// Text styles
/**
* @brief ANSI escape code for bold text style.
*/
static const char *BOLD = "\033[1m";
/**
* @brief ANSI escape code for dim text style.
*/
static const char *DIM = "\033[2m";
/**
* @brief ANSI escape code for italic text style.
*/
static const char *ITALIC = "\033[3m";
/**
* @brief ANSI escape code for underlined text style.
*/
static const char *UNDERLINE = "\033[4m";
/**
* @brief ANSI escape code for blinking text style.
*/
static const char *BLINK = "\033[5m";
/**
* @brief ANSI escape code for reverse video (swap foreground/background).
*/
static const char *REVERSE = "\033[7m";
}
/**
* @brief Check if the terminal supports color output.
*
* Checks various conditions to determine if the terminal supports ANSI color codes:
* - Checks if NO_COLOR environment variable is set (disables colors)
* - Checks TERM environment variable for common color-capable terminals
* - Defaults to true for most modern terminals
*
* @return true if colors are supported, false otherwise.
*/
bool is_color_supported();
/**
* @brief Enable or disable color output.
*
* Sets whether color output should be used. This is useful for redirecting output
* to files or non-terminal outputs where color codes would be unwanted.
*
* @param enabled true to enable colors (if supported), false to disable.
*
* @note Colors are only enabled if both this flag is true AND the terminal supports colors.
*/
void set_color_enabled(bool enabled);
/**
* @brief Check if color output is currently enabled.
*
* Returns true only if both color_enabled flag is true AND the terminal supports colors.
*
* @return true if colors are enabled and supported, false otherwise.
*/
bool is_color_enabled();
/**
* @brief Print formatted text in red color.
*
* Prints formatted text using printf-style formatting with red text color.
* Automatically resets color after printing.
*
* @param format printf-style format string.
* @param ... Variable arguments for format string.
*
* @code
* printf_red("Error: %s\n", error_message);
* @endcode
*/
void printf_red(const char *format, ...);
/**
* @brief Print formatted text in green color.
*
* Prints formatted text using printf-style formatting with green text color.
* Automatically resets color after printing.
*
* @param format printf-style format string.
* @param ... Variable arguments for format string.
*/
void printf_green(const char *format, ...);
/**
* @brief Print formatted text in yellow color.
*
* Prints formatted text using printf-style formatting with yellow text color.
* Automatically resets color after printing.
*
* @param format printf-style format string.
* @param ... Variable arguments for format string.
*/
void printf_yellow(const char *format, ...);
/**
* @brief Print formatted text in blue color.
*
* Prints formatted text using printf-style formatting with blue text color.
* Automatically resets color after printing.
*
* @param format printf-style format string.
* @param ... Variable arguments for format string.
*/
void printf_blue(const char *format, ...);
/**
* @brief Print formatted text in cyan color.
*
* Prints formatted text using printf-style formatting with cyan text color.
* Automatically resets color after printing.
*
* @param format printf-style format string.
* @param ... Variable arguments for format string.
*/
void printf_cyan(const char *format, ...);
/**
* @brief Print formatted text in magenta color.
*
* Prints formatted text using printf-style formatting with magenta text color.
* Automatically resets color after printing.
*
* @param format printf-style format string.
* @param ... Variable arguments for format string.
*/
void printf_magenta(const char *format, ...);
/**
* @brief Print formatted text in white color.
*
* Prints formatted text using printf-style formatting with white text color.
* Automatically resets color after printing.
*
* @param format printf-style format string.
* @param ... Variable arguments for format string.
*/
void printf_white(const char *format, ...);
/**
* @brief Print formatted text with a custom color code.
*
* Prints formatted text using printf-style formatting with a custom ANSI color code.
* Useful when you want to use a color from the color namespace or combine styles.
* Automatically resets color after printing.
*
* @param color_code ANSI escape code for the desired color/style (e.g., color::RED, color::BOLD).
* @param format printf-style format string.
* @param ... Variable arguments for format string.
*
* @code
* printf_color(color::BRIGHT_RED, "Critical: %s\n", message);
* printf_color(color::BOLD, "Important message\n");
* @endcode
*/
void printf_color(const char *color_code, const char *format, ...);
/**
* @brief Log an informational message (blue color).
*
* Logs an informational message with blue text color. Use for general information
* that doesn't indicate success, warning, or error.
*
* @param format printf-style format string.
* @param ... Variable arguments for format string.
*
* @code
* log_info("Processing %d items\n", count);
* @endcode
*/
void log_info(const char *format, ...);
/**
* @brief Log a success message (green color).
*
* Logs a success message with green text color. Use for successful operations
* or positive status updates.
*
* @param format printf-style format string.
* @param ... Variable arguments for format string.
*
* @code
* log_success("Operation completed successfully\n");
* @endcode
*/
void log_success(const char *format, ...);
/**
* @brief Log a warning message (yellow color).
*
* Logs a warning message with yellow text color. Use for warnings that don't
* prevent operation but should be noted.
*
* @param format printf-style format string.
* @param ... Variable arguments for format string.
*
* @code
* log_warning("Low battery: %d%%\n", battery_level);
* @endcode
*/
void log_warning(const char *format, ...);
/**
* @brief Log an error message (red color).
*
* Logs an error message with red text color. Use for errors that indicate
* problems or failures.
*
* @param format printf-style format string.
* @param ... Variable arguments for format string.
*
* @code
* log_error("Failed to open file: %s\n", filename);
* @endcode
*/
void log_error(const char *format, ...);
/**
* @brief Log a debug message (cyan color).
*
* Logs a debug message with cyan text color. Use for debugging information
* that is typically only needed during development.
*
* @param format printf-style format string.
* @param ... Variable arguments for format string.
*
* @code
* log_debug("Variable value: %d\n", value);
* @endcode
*/
void log_debug(const char *format, ...);
/**
* @brief Log an informational message with file and line information.
*
* Logs an informational message with blue color, including the source file
* and line number where the log was called. Useful for debugging and tracing.
*
* @param file Source file name (typically use __FILE__ macro).
* @param line Line number (typically use __LINE__ macro).
* @param format printf-style format string.
* @param ... Variable arguments for format string.
*
* @code
* log_info_at(__FILE__, __LINE__, "Processing started\n");
* @endcode
*/
void log_info_at(const char *file, int line, const char *format, ...);
/**
* @brief Log a success message with file and line information.
*
* Logs a success message with green color, including the source file and line number.
*
* @param file Source file name (typically use __FILE__ macro).
* @param line Line number (typically use __LINE__ macro).
* @param format printf-style format string.
* @param ... Variable arguments for format string.
*/
void log_success_at(const char *file, int line, const char *format, ...);
/**
* @brief Log a warning message with file and line information.
*
* Logs a warning message with yellow color, including the source file and line number.
*
* @param file Source file name (typically use __FILE__ macro).
* @param line Line number (typically use __LINE__ macro).
* @param format printf-style format string.
* @param ... Variable arguments for format string.
*/
void log_warning_at(const char *file, int line, const char *format, ...);
/**
* @brief Log an error message with file and line information.
*
* Logs an error message with red color, including the source file and line number.
*
* @param file Source file name (typically use __FILE__ macro).
* @param line Line number (typically use __LINE__ macro).
* @param format printf-style format string.
* @param ... Variable arguments for format string.
*/
void log_error_at(const char *file, int line, const char *format, ...);
/**
* @brief Log a debug message with file and line information.
*
* Logs a debug message with cyan color, including the source file and line number.
*
* @param file Source file name (typically use __FILE__ macro).
* @param line Line number (typically use __LINE__ macro).
* @param format printf-style format string.
* @param ... Variable arguments for format string.
*/
void log_debug_at(const char *file, int line, const char *format, ...);
/**
* @brief Log an error message with throttle.
*
* Logs an error message with red text color. Use for errors that indicate
* problems or failures.
*
* @param format printf-style format string.
* @param ... Variable arguments for format string.
*/
void log_error_throttle(double throttle, const char *format, ...);
/**
* @brief Log a debug message with throttle.
*
* Logs a debug message with cyan text color. Use for debugging information
* that is typically only needed during development.
*
* @param format printf-style format string.
* @param ... Variable arguments for format string.
*/
void log_info_throttle(double throttle, const char *format, ...);
/**
* @brief Log a success message with throttle.
*
* Logs a success message with green text color. Use for successful operations
* or positive status updates.
*
* @param format printf-style format string.
* @param ... Variable arguments for format string.
*/
void log_success_throttle(double throttle, const char *format, ...);
/**
* @brief Log a warning message with throttle.
*
* Logs a warning message with yellow text color. Use for warnings that don't
* prevent operation but should be noted.
*
* @param format printf-style format string.
* @param ... Variable arguments for format string.
*/
void log_warning_throttle(double throttle, const char *format, ...);
/**
* @brief Assert a condition and log error if it fails.
*
* Checks if a condition is true. If the condition is false, logs an error
* message with red text color and terminates the program by calling abort().
* This is useful for debugging and catching logic errors during development.
*
* @param condition The condition to check (must evaluate to true).
* @param format Optional printf-style format string for error message.
* @param ... Variable arguments for format string.
*
* @note In release builds, assertions may be disabled. Use this for conditions
* that should never be false (logic errors), not for runtime error handling.
*
* @code
* log_assert(ptr != nullptr, "Pointer is null!");
* log_assert(index < size, "Index %d out of bounds (size: %d)", index, size);
* log_assert(velocity > 0, "Velocity must be positive, got: %f", velocity);
* @endcode
*/
void log_assert(bool condition, const char *format = nullptr, ...);
} // namespace robot
#endif // ROBOT_CONSOLE_H

134
include/robot/init.h Normal file
View File

@@ -0,0 +1,134 @@
#ifndef ROBOT_INIT_H_INCLUDED
#define ROBOT_INIT_H_INCLUDED
#include <atomic>
#include <csignal>
#include <string>
namespace robot
{
/**
* @brief Check if the robot system is still running
*
* Similar to ros::ok(), this function returns false when the system
* has finished shutting down. Use this in main loops and thread loops
* to check if the system should continue running.
*
* @return true if system is running, false if shutdown is complete
*
* @note This returns false only after shutdown is complete, not when
* shutdown is requested. Use isShuttingDown() to check if shutdown
* has been requested.
*
* Example:
* @code
* while (robot::ok())
* {
* // Do work
* }
* @endcode
*/
bool ok();
/**
* @brief Check if shutdown has been requested
*
* Similar to ros::isShuttingDown(), this function returns true as soon
* as shutdown() is called, not when shutdown is complete. Use this in
* long-running callbacks to check if they should exit early.
*
* @return true if shutdown has been requested, false otherwise
*
* @note This returns true immediately when shutdown() is called, while
* ok() returns false only after shutdown is complete.
*
* Example:
* @code
* void longCallback()
* {
* for (int i = 0; i < 1000000; i++)
* {
* if (robot::isShuttingDown())
* return; // Exit early
* // Do work...
* }
* }
* @endcode
*/
bool isShuttingDown();
/**
* @brief Shutdown the robot system
*
* Similar to ros::shutdown(), this function initiates the shutdown process.
* After calling this:
* - isShuttingDown() will return true immediately
* - ok() will return false after shutdown is complete
*
* @note This function is thread-safe and can be called multiple times.
* Subsequent calls have no effect.
*
* Example:
* @code
* // Custom signal handler
* void mySigintHandler(int sig)
* {
* // Do custom cleanup
* robot::shutdown();
* }
* @endcode
*/
void shutdown();
/**
* @brief Initialize the robot system
*
* Similar to ros::init(), this function initializes the robot system.
* It sets up signal handlers and initializes internal state.
*
* @param argc Number of command line arguments (can be modified)
* @param argv Command line arguments (can be modified)
* @param node_name Name of the node (optional, for logging purposes)
* @param install_sigint_handler If true, install SIGINT handler (default: true)
*
* @note If install_sigint_handler is true, Ctrl-C will automatically
* call shutdown(). Set to false if you want custom signal handling.
*
* Example:
* @code
* int main(int argc, char** argv)
* {
* robot::init(argc, argv, "my_node");
* // ... use robot system ...
* return 0;
* }
* @endcode
*/
void init(int& argc, char** argv, const std::string& node_name = "", bool install_sigint_handler = true);
/**
* @brief Initialize the robot system without command line arguments
*
* Simplified version of init() that doesn't require argc/argv.
*
* @param node_name Name of the node (optional, for logging purposes)
* @param install_sigint_handler If true, install SIGINT handler (default: true)
*/
void init(const std::string& node_name = "", bool install_sigint_handler = true);
namespace detail
{
/**
* @brief Internal signal handler for SIGINT
*
* This is called automatically when SIGINT is received (Ctrl-C).
* It calls shutdown() to gracefully shut down the system.
*/
void sigintHandler(int sig);
}
} // namespace robot
#endif // ROBOT_INIT_H_INCLUDED

649
include/robot/node_handle.h Normal file
View File

@@ -0,0 +1,649 @@
#ifndef ROBOT_NODE_H_INCLUDED_H
#define ROBOT_NODE_H_INCLUDED_H
#include <yaml-cpp/yaml.h>
#include <robot_xmlrpcpp/XmlRpcValue.h>
#include <memory>
#include <string>
#include <map>
#include <vector>
namespace robot
{
/**
* @class NodeHandle
* @brief Main interface for accessing and setting parameters using YAML-based parameter server.
*
* NodeHandle provides a ROS-like interface for parameter management, using YAML files as the backend.
* It supports namespaces, nested parameters, and automatic loading of YAML configuration files.
*
* @note This class uses YAML::Node internally to store and manage parameters, providing a hierarchical
* parameter structure similar to ROS parameter server.
*/
class NodeHandle
{
public:
/**
* @brief Type alias for remappings (map of string to string).
*
* Used for topic and parameter name remapping, similar to ROS remapping mechanism.
*/
using M_string = std::map<std::string, std::string>;
/**
* @brief Type alias for shared pointer to NodeHandle.
*
* Convenience type for managing NodeHandle instances with shared ownership.
*/
using Ptr = std::shared_ptr<NodeHandle>;
/**
* @brief Default constructor with optional namespace and remappings.
*
* Creates a root-level NodeHandle or one scoped to the specified namespace.
* If namespace is "~" or empty, operates at root level. Automatically loads
* YAML configuration files from the config directory if available.
*
* @param ns The namespace for this NodeHandle (default: root).
* Use "~" for private namespace (maps to config directory).
* @param remappings Map of parameter name remappings (default: empty).
*/
NodeHandle(const std::string &ns = std::string(), const M_string &remappings = M_string());
/**
* @brief Constructor from parent NodeHandle with new namespace.
*
* Creates a new NodeHandle that is scoped to a child namespace of the parent.
* The new NodeHandle will have access to parameters under the specified namespace.
*
* @param parent The parent NodeHandle to inherit from.
* @param ns The namespace to scope this NodeHandle to (relative to parent).
* Leading '/' is automatically stripped.
*/
NodeHandle(const NodeHandle &parent, const std::string &ns);
/**
* @brief Constructor from parent NodeHandle with new namespace and remappings.
*
* Creates a new NodeHandle with a child namespace and optional remappings.
*
* @param parent The parent NodeHandle to inherit from.
* @param ns The namespace to scope this NodeHandle to (relative to parent).
* Leading '/' is automatically stripped.
* @param remappings Map of parameter name remappings (currently not fully implemented).
*/
NodeHandle(const NodeHandle &parent, const std::string &ns, const M_string &remappings);
/**
* @brief Copy constructor.
*
* Creates a copy of the NodeHandle with the same namespace scope.
* Both instances will reference the same underlying parameter tree.
*
* @param parent The NodeHandle to copy from.
*/
NodeHandle(const NodeHandle &parent);
/**
* @brief Assignment operator.
*
* Assigns the namespace and parameter scope from another NodeHandle.
*
* @param parent The NodeHandle to assign from.
* @return Reference to this NodeHandle.
*/
NodeHandle &operator=(const NodeHandle &parent);
/**
* @brief Destructor.
*
* Cleans up the NodeHandle instance. Note that static members (root_ and config_directory_)
* persist across NodeHandle instances.
*/
~NodeHandle();
/**
* @brief Check if a parameter exists.
*
* Checks whether a parameter with the given key exists in the parameter server.
* Supports nested keys using '/' separator (e.g., "parent/child").
*
* @param key The parameter key to check. Can be nested using '/' separator.
* @return true if the parameter exists, false otherwise.
*/
bool hasParam(const std::string &key) const;
/**
* @brief Get a boolean value from the parameter server.
*
* Retrieves a boolean parameter value. Supports string-to-boolean conversion
* for values like "true", "false", "1", "0", "yes", "no", "on", "off".
*
* @param key The parameter key (supports nested keys with '/' separator).
* @param b Storage for the retrieved value. Set to default_value if key not found.
* @param default_value Default value to use if parameter doesn't exist (default: false).
* @return true if the parameter value was retrieved successfully, false otherwise.
*
* @note If you want to provide a default value in case the key does not exist, use param() instead.
*/
bool getParam(const std::string &key, bool &b, bool default_value = false) const;
/**
* @brief Get a double value from the parameter server.
*
* @param key The parameter key (supports nested keys with '/' separator).
* @param d Storage for the retrieved value. Set to default_value if key not found.
* @param default_value Default value to use if parameter doesn't exist (default: 0.0).
* @return true if the parameter value was retrieved successfully, false otherwise.
*/
bool getParam(const std::string &key, double &d, double default_value = 0.0) const;
/**
* @brief Get a float value from the parameter server.
*
* @param key The parameter key (supports nested keys with '/' separator).
* @param f Storage for the retrieved value. Set to default_value if key not found.
* @param default_value Default value to use if parameter doesn't exist (default: 0.0).
* @return true if the parameter value was retrieved successfully, false otherwise.
*/
bool getParam (const std::string &key, float &f, float default_value = 0.0) const;
/**
* @brief Get an integer value from the parameter server.
*
* Supports hexadecimal format (e.g., "0x10").
*
* @param key The parameter key (supports nested keys with '/' separator).
* @param i Storage for the retrieved value. Set to default_value if key not found.
* @param default_value Default value to use if parameter doesn't exist (default: 0).
* @return true if the parameter value was retrieved successfully, false otherwise.
*/
bool getParam (const std::string &key, int &i, int default_value = 0) const;
/**
* @brief Get a map of string-to-bool from the parameter server.
*
* @param key The parameter key (supports nested keys with '/' separator).
* @param map Storage for the retrieved map. Set to default_value if key not found.
* @param default_value Default map to use if parameter doesn't exist (default: empty map).
* @return true if the parameter value was retrieved successfully, false otherwise.
*/
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;
/**
* @brief Get a map of string-to-double from the parameter server.
*
* @param key The parameter key (supports nested keys with '/' separator).
* @param map Storage for the retrieved map. Set to default_value if key not found.
* @param default_value Default map to use if parameter doesn't exist (default: empty map).
* @return true if the parameter value was retrieved successfully, false otherwise.
*/
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;
/**
* @brief Get a map of string-to-float from the parameter server.
*
* @param key The parameter key (supports nested keys with '/' separator).
* @param map Storage for the retrieved map. Set to default_value if key not found.
* @param default_value Default map to use if parameter doesn't exist (default: empty map).
* @return true if the parameter value was retrieved successfully, false otherwise.
*/
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;
/**
* @brief Get a map of string-to-int from the parameter server.
*
* @param key The parameter key (supports nested keys with '/' separator).
* @param map Storage for the retrieved map. Set to default_value if key not found.
* @param default_value Default map to use if parameter doesn't exist (default: empty map).
* @return true if the parameter value was retrieved successfully, false otherwise.
*/
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;
/**
* @brief Get a map of string-to-string from the parameter server.
*
* @param key The parameter key (supports nested keys with '/' separator).
* @param map Storage for the retrieved map. Set to default_value if key not found.
* @param default_value Default map to use if parameter doesn't exist (default: empty map).
* @return true if the parameter value was retrieved successfully, false otherwise.
*/
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;
/**
* @brief Get a string value from the parameter server.
*
* @param key The parameter key (supports nested keys with '/' separator).
* @param s Storage for the retrieved value. Set to default_value if key not found.
* @param default_value Default value to use if parameter doesn't exist (default: empty string).
* @return true if the parameter value was retrieved successfully, false otherwise.
*/
bool getParam (const std::string &key, std::string &s, std::string default_value = "") const;
/**
* @brief Get a vector of boolean values from the parameter server.
*
* @param key The parameter key (supports nested keys with '/' separator).
* @param vec Storage for the retrieved vector. Set to default_value if key not found.
* @param default_value Default vector to use if parameter doesn't exist (default: empty vector).
* @return true if the parameter value was retrieved successfully, false otherwise.
*/
bool getParam (const std::string &key, std::vector< bool > &vec, std::vector< bool > default_value = std::vector< bool >()) const;
/**
* @brief Get a vector of double values from the parameter server.
*
* @param key The parameter key (supports nested keys with '/' separator).
* @param vec Storage for the retrieved vector. Set to default_value if key not found.
* @param default_value Default vector to use if parameter doesn't exist (default: empty vector).
* @return true if the parameter value was retrieved successfully, false otherwise.
*/
bool getParam (const std::string &key, std::vector< double > &vec, std::vector< double > default_value = std::vector< double >()) const;
/**
* @brief Get a vector of float values from the parameter server.
*
* @param key The parameter key (supports nested keys with '/' separator).
* @param vec Storage for the retrieved vector. Set to default_value if key not found.
* @param default_value Default vector to use if parameter doesn't exist (default: empty vector).
* @return true if the parameter value was retrieved successfully, false otherwise.
*/
bool getParam (const std::string &key, std::vector< float > &vec, std::vector< float > default_value = std::vector< float >()) const;
/**
* @brief Get a vector of integer values from the parameter server.
*
* @param key The parameter key (supports nested keys with '/' separator).
* @param vec Storage for the retrieved vector. Set to default_value if key not found.
* @param default_value Default vector to use if parameter doesn't exist (default: empty vector).
* @return true if the parameter value was retrieved successfully, false otherwise.
*/
bool getParam (const std::string &key, std::vector< int > &vec, std::vector< int > default_value = std::vector< int >()) const;
/**
* @brief Get a vector of string values from the parameter server.
*
* @param key The parameter key (supports nested keys with '/' separator).
* @param vec Storage for the retrieved vector. Set to default_value if key not found.
* @param default_value Default vector to use if parameter doesn't exist (default: empty vector).
* @return true if the parameter value was retrieved successfully, false otherwise.
*/
bool getParam (const std::string &key, std::vector< std::string > &vec, std::vector< std::string > default_value = std::vector< std::string >()) const;
/**
* @brief Get a YAML::Node value from the parameter server.
*
* Returns the raw YAML node, which can represent any YAML structure (scalar, sequence, map).
*
* @param key The parameter key (supports nested keys with '/' separator).
* @param v Storage for the retrieved YAML node. Set to default_value if key not found.
* @param default_value Default YAML node to use if parameter doesn't exist (default: empty node).
* @return true if the parameter value was retrieved successfully, false otherwise.
*/
bool getParam (const std::string &key, YAML::Node &v, YAML::Node default_value = YAML::Node()) const;
/**
* @brief Template method to get a parameter value (without default).
*
* Gets a parameter value. If the parameter doesn't exist, returns a default-constructed value.
* Use hasParam() to check existence before calling this method.
*
* @tparam T The type of the parameter (must be supported by getParam).
* @param param_name The parameter key (supports nested keys with '/' separator).
* @return The parameter value if found, otherwise a default-constructed value of type T.
*
* @code
* if (nh.hasParam("my_param")) {
* double value = nh.param<double>("my_param");
* }
* @endcode
*/
template<typename T >
T param (const std::string &param_name) const;
/**
* @brief Template method to get a parameter value with default.
*
* Convenience method that returns the parameter value directly, or the default if not found.
* This is the recommended way to get parameters when you have a default value.
*
* @tparam T The type of the parameter (must be supported by getParam).
* @param param_name The parameter key (supports nested keys with '/' separator).
* @param default_val The default value to return if parameter doesn't exist.
* @return The parameter value if found, otherwise default_val.
*
* @code
* int max_iterations = nh.param("max_iterations", 100);
* std::string frame_id = nh.param("frame_id", std::string("base_link"));
* @endcode
*/
template<typename T >
T param (const std::string &param_name, const T &default_val) const;
/**
* @brief Template method to get a parameter value with default (output parameter version).
*
* Similar to param() but uses an output parameter instead of return value.
*
* @tparam T The type of the parameter (must be supported by getParam).
* @param param_name The parameter key (supports nested keys with '/' separator).
* @param param_val Output parameter to store the retrieved value.
* @param default_val The default value to use if parameter doesn't exist.
* @return true if the parameter was found, false if default was used.
*/
template<typename T >
bool param (const std::string &param_name, T &param_val, const T &default_val) const;
/**
* @brief Search for a parameter and return the result.
*
* @param key The parameter key (supports nested keys with '/' separator).
* @param result The result of the search.
* @return true if the parameter was found, false otherwise.
*/
bool searchParam (const std::string &key, std::string &result) const;
/**
* @brief Set a parameter from a YAML::Node.
*
* Sets a parameter value using a YAML node. This is the most flexible setParam method
* as it can handle any YAML structure (scalar, sequence, map).
*
* @param key The parameter key (supports nested keys with '/' separator).
* @param value The YAML node containing the value to set.
*/
void setParam(const std::string &key, const YAML::Node &value);
/**
* @brief Set a boolean parameter.
*
* @param key The parameter key (supports nested keys with '/' separator).
* @param b The boolean value to set.
*/
void setParam (const std::string &key, bool b) const;
/**
* @brief Set a string parameter from C-style string.
*
* @param key The parameter key (supports nested keys with '/' separator).
* @param s The C-style string value to set.
*/
void setParam (const std::string &key, const char *s) const;
/**
* @brief Set a map of string-to-bool parameter.
*
* @param key The parameter key (supports nested keys with '/' separator).
* @param map The map of string-to-bool values to set.
*/
void setParam (const std::string &key, const std::map< std::string, bool > &map) const;
/**
* @brief Set a map of string-to-double parameter.
*
* @param key The parameter key (supports nested keys with '/' separator).
* @param map The map of string-to-double values to set.
*/
void setParam (const std::string &key, const std::map< std::string, double > &map) const;
/**
* @brief Set a map of string-to-float parameter.
*
* @param key The parameter key (supports nested keys with '/' separator).
* @param map The map of string-to-float values to set.
*/
void setParam (const std::string &key, const std::map< std::string, float > &map) const;
/**
* @brief Set a map of string-to-int parameter.
*
* @param key The parameter key (supports nested keys with '/' separator).
* @param map The map of string-to-int values to set.
*/
void setParam (const std::string &key, const std::map< std::string, int > &map) const;
/**
* @brief Set a map of string-to-string parameter.
*
* @param key The parameter key (supports nested keys with '/' separator).
* @param map The map of string-to-string values to set.
*/
void setParam (const std::string &key, const std::map< std::string, std::string > &map) const;
/**
* @brief Set a string parameter.
*
* @param key The parameter key (supports nested keys with '/' separator).
* @param s The string value to set.
*/
void setParam (const std::string &key, const std::string &s) const;
/**
* @brief Set a vector of boolean values parameter.
*
* @param key The parameter key (supports nested keys with '/' separator).
* @param vec The vector of boolean values to set.
*/
void setParam (const std::string &key, const std::vector< bool > &vec) const;
/**
* @brief Set a vector of double values parameter.
*
* @param key The parameter key (supports nested keys with '/' separator).
* @param vec The vector of double values to set.
*/
void setParam (const std::string &key, const std::vector< double > &vec) const;
/**
* @brief Set a vector of float values parameter.
*
* @param key The parameter key (supports nested keys with '/' separator).
* @param vec The vector of float values to set.
*/
void setParam (const std::string &key, const std::vector< float > &vec) const;
/**
* @brief Set a vector of integer values parameter.
*
* @param key The parameter key (supports nested keys with '/' separator).
* @param vec The vector of integer values to set.
*/
void setParam (const std::string &key, const std::vector< int > &vec) const;
/**
* @brief Set a vector of string values parameter.
*
* @param key The parameter key (supports nested keys with '/' separator).
* @param vec The vector of string values to set.
*/
void setParam (const std::string &key, const std::vector< std::string > &vec) const;
/**
* @brief Set a parameter from robot_xmlrpcpp::XmlRpcValue.
*
* Converts an XmlRpcValue to YAML format and sets it as a parameter.
* Supports boolean, int, double, string, array, and struct types.
*
* @param key The parameter key (supports nested keys with '/' separator).
* @param v The XmlRpcValue to convert and set.
*/
void setParam (const std::string &key, const robot_xmlrpcpp::XmlRpcValue &v) const;
/**
* @brief Set a double parameter.
*
* @param key The parameter key (supports nested keys with '/' separator).
* @param d The double value to set.
*/
void setParam (const std::string &key, double d) const;
/**
* @brief Set an integer parameter.
*
* @param key The parameter key (supports nested keys with '/' separator).
* @param i The integer value to set.
*/
void setParam (const std::string &key, int i) const;
/**
* @brief Get a nested parameter value by key path.
*
* Retrieves a YAML node from the parameter tree using a key path.
* Supports nested keys using '/' separator (e.g., "parent/child/grandchild").
*
* @param key The parameter key path (supports nested keys with '/' separator).
* @return YAML::Node containing the parameter value, or empty node if not found.
*/
YAML::Node getParamValue(const std::string &key) const;
/**
* @brief Get the underlying YAML node handle.
*
* Returns a const reference to the internal YAML node that stores all parameters
* for this NodeHandle's namespace scope.
*
* @return Const reference to the YAML node handle.
*/
const YAML::Node &getNodeHandle() const;
/**
* @brief Merge a YAML node into another node (recursively merges maps).
*
* Recursively merges the source YAML node into the output node.
* If both nodes contain maps with the same key, the maps are merged recursively.
* Scalar values and sequences are overwritten.
*
* @param source The source YAML node to merge from.
* @param output The output YAML node to merge into (modified in place).
*/
void mergeYamlNode(const YAML::Node &source, YAML::Node &output);
/**
* @brief Load all YAML files from a directory and merge them.
*
* Scans the specified directory for .yaml and .yml files, loads them,
* and merges them into the root parameter tree. Files are processed in
* filesystem order.
*
* @param directory_path Path to the directory containing YAML files.
* @return Number of files successfully loaded.
*/
int loadYamlFilesFromDirectory(const std::string &directory_path);
/**
* @brief Get the namespace of this NodeHandle.
*
* Returns the namespace string that this NodeHandle is scoped to.
* If the namespace was "~" or empty, returns "/" (root namespace).
*
* @return The namespace string (e.g., "/robot/base" or "/" for root).
*/
std::string getNamespace() const;
/**
* @brief Print all parameters in the current namespace scope (debug method).
*
* Debug utility that prints all parameters in the node_handle_ to stdout.
* Shows the hierarchical structure with indentation. Useful for debugging
* parameter loading and namespace scoping.
*/
void printParams() const;
private:
/**
* @brief Helper method to split key path and get nested value.
*
* Internal method that traverses the parameter tree using a key path
* (e.g., "parent/child") and returns the nested YAML node.
*
* @param key The parameter key path (supports nested keys with '/' separator).
* @return YAML::Node containing the nested value, or empty node if not found.
*/
YAML::Node getNestedValue(const std::string &key) const;
/**
* @brief Debug version of getNestedValue with verbose output.
*
* Same as getNestedValue() but with additional debug output for troubleshooting.
*
* @param key The parameter key path (supports nested keys with '/' separator).
* @return YAML::Node containing the nested value, or empty node if not found.
*/
YAML::Node getNestedValueDebug(const std::string &key) const;
/**
* @brief Helper method to set parameter with type checking.
*
* Internal method that sets a parameter value with optional type checking.
* If the key already exists with a different type, it will be overwritten.
*
* @param key The parameter key (supports nested keys with '/' separator).
* @param value The YAML node value to set.
* @param expected_type The expected YAML node type (for type checking).
*/
void setParamInternal(const std::string &key, const YAML::Node &value, YAML::NodeType::value expected_type);
/**
* @brief Helper method to find config directory automatically.
*
* Static method that attempts to locate the config directory using various
* strategies: environment variables, relative paths from executable, hardcoded paths.
*
* @return The config directory path if found, empty string otherwise.
*/
static std::string findConfigDirectory();
/**
* @brief Auto-load YAML files from config directory.
*
* Internal method that automatically loads all YAML files from the config
* directory and merges them into the root_ parameter tree. Called during
* NodeHandle construction if config_directory_ is set.
*/
void autoLoadConfigFiles();
/**
* @brief Debug method to print all parameters in a YAML node.
*
* Recursive helper method that prints the structure and values of a YAML node
* with indentation to show hierarchy. Used by printAllParams().
*
* @param node The YAML node to print.
*/
void printParams(YAML::Node node) const;
/**
* @brief The namespace this NodeHandle is scoped to.
*
* Stores the namespace string. If namespace is "~" or empty, it's set to "/" (root).
*/
std::string namespace_;
/**
* @brief Mutable storage for parameters in this namespace scope.
*
* YAML node that contains all parameters accessible through this NodeHandle.
* This is a reference into the static root_ tree, scoped to the namespace_.
*/
// YAML::Node node_handle_;
/**
* @brief Static root parameter tree shared by all NodeHandle instances.
*
* The root YAML node containing the entire parameter tree. All NodeHandle
* instances share this static tree, with each instance scoped to a namespace.
*/
static YAML::Node root_;
/**
* @brief Static config directory path for automatic YAML loading.
*
* Directory path from which YAML configuration files are automatically loaded.
* Set via setConfigDirectory() or found automatically via findConfigDirectory().
*/
static std::string config_directory_;
};
} // namespace robot
#endif // ROBOT_NODE_H_INCLUDED_H

106
include/robot/plugin_loader_helper.h Normal file
View File

@@ -0,0 +1,106 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Plugin Loader Helper for Boost.DLL
* Maps symbol names (export names) to library file paths
* Similar to plugins.xml but for boost::dll
*********************************************************************/
#ifndef ROBOT_PLUGIN_LOADER_HELPER_H_
#define ROBOT_PLUGIN_LOADER_HELPER_H_
#include <string>
#include <vector>
#include <map>
#include <yaml-cpp/yaml.h>
#include <filesystem>
#include <robot/robot.h>
namespace robot
{
/**
* @class PluginLoaderHelper
* @brief Helper class to find library paths from symbol names (export names)
*
* This class reads a YAML configuration file (similar to plugins.xml) that maps
* symbol names (export names used in BOOST_DLL_ALIAS) to library file paths.
*
* Usage:
* PluginLoaderHelper loader;
* std::string lib_path = loader.findLibraryPath("CustomPlanner");
* if (!lib_path.empty()) {
* // Use lib_path with boost::dll::import_alias
* }
*/
class PluginLoaderHelper
{
public:
/**
* @brief Constructor
*/
PluginLoaderHelper();
/**
* @brief Constructor
* @param nh NodeHandle to read parameters from
* @param config_namespace Namespace in NodeHandle where plugins are stored (default: root)
*/
PluginLoaderHelper(robot::NodeHandle nh, const std::string& config_namespace = "");
/**
* @brief Find library path from symbol name (export name)
* @param symbol_name The export name (e.g., "CustomPlanner", "PNKXLocalPlanner")
* @return Full path to the library file, or empty string if not found
*/
std::string findLibraryPath(const std::string& symbol_name);
/**
* @brief Check if a symbol is registered
* @param symbol_name The export name
* @return True if symbol is found in config
*/
bool hasSymbol(const std::string& symbol_name) const;
/**
* @brief Set NodeHandle and namespace for reading plugins
* @param nh NodeHandle to read parameters from
* @param config_namespace Namespace in NodeHandle where plugins are stored
*/
void setNodeHandle(robot::NodeHandle nh, const std::string& config_namespace = "");
/**
* @brief Get all registered symbol names
* @return Vector of all symbol names
*/
std::vector<std::string> getRegisteredSymbols() const;
/**
* @brief Get build directory (CMAKE_BINARY_DIR) at runtime
* @return Build directory path, or empty if not found
*/
static std::string getBuildDirectory();
/**
* @brief Get workspace path at runtime
* @return Workspace path (e.g., /home/robotics/AGV/Diff_Wheel_Prj/t800_v2_ws), or empty if not found
*/
static std::string getWorkspacePath();
private:
/**
* @brief Resolve library path (handle relative paths, search in search_paths)
* @param library_path Path from config (may be relative or absolute)
* @return Resolved absolute path, or empty if not found
*/
std::string resolveLibraryPath(const std::string& library_path) const;
robot::NodeHandle nh_; // NodeHandle to read parameters
std::string config_namespace_; // Namespace for plugins in NodeHandle
std::vector<std::string> search_paths_; // Search paths for libraries
};
} // namespace robot
#endif // ROBOT_PLUGIN_LOADER_HELPER_H_

12
include/robot/robot.h Normal file
View File

@@ -0,0 +1,12 @@
#ifndef ROBOT_ROBOT_H
#define ROBOT_ROBOT_H
#include <robot/init.h>
#include <robot/time.h>
#include <robot/timer.h>
#include <robot/rate.h>
#include <robot/console.h>
#include <robot/node_handle.h>
#include <robot/plugin_loader_helper.h>
#endif