diff --git a/CMakeLists.txt b/CMakeLists.txt index a4d52c7..3b941ee 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -17,7 +17,6 @@ set(CMAKE_CXX_EXTENSIONS OFF) # Find dependencies find_package(yaml-cpp REQUIRED) -find_package(console_bridge REQUIRED) if (NOT BUILDING_WITH_CATKIN) @@ -48,7 +47,7 @@ else() INCLUDE_DIRS include LIBRARIES ${PROJECT_NAME} CATKIN_DEPENDS robot_xmlrpcpp robot_time - # Note: yaml-cpp and console_bridge are system dependencies, + # Note: yaml-cpp are system dependencies, # linked via target_link_libraries, not via catkin_package DEPENDS ) @@ -150,6 +149,8 @@ else() set_target_properties(${PROJECT_NAME} PROPERTIES LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR} + BUILD_RPATH "${CMAKE_BINARY_DIR}" + INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/lib" ) endif() @@ -220,7 +221,7 @@ else() message(STATUS "Project: ${PROJECT_NAME}") message(STATUS "Version: ${PROJECT_VERSION}") message(STATUS "C++ Standard: ${CMAKE_CXX_STANDARD}") - message(STATUS "Dependencies: robot_xmlrpcpp, robot_time, yaml-cpp, console_bridge") + message(STATUS "Dependencies: robot_xmlrpcpp, robot_time, yaml-cpp") message(STATUS "=================================") endif() @@ -247,6 +248,8 @@ if(BUILD_TESTS) set_target_properties(test_node_handle PROPERTIES LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR} + BUILD_RPATH "${CMAKE_BINARY_DIR}" + INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/lib" ) # Link filesystem library if needed diff --git a/include/robot/node_handle.h b/include/robot/node_handle.h index af3155b..9931e3f 100644 --- a/include/robot/node_handle.h +++ b/include/robot/node_handle.h @@ -283,6 +283,14 @@ namespace robot */ bool getParam (const std::string &key, YAML::Node &v, YAML::Node default_value = YAML::Node()) const; + /** + * @brief Get a parameter as robot_xmlrpcpp::XmlRpcValue (converted from YAML). + * @param key The parameter key (supports nested keys with '/' separator). + * @param v Storage for the retrieved value. Left unchanged if key not found. + * @return true if the parameter was retrieved and converted successfully, false otherwise. + */ + bool getParam (const std::string &key, robot_xmlrpcpp::XmlRpcValue &v) const; + /** * @brief Template method to get a parameter value (without default). * diff --git a/include/robot/robot.h b/include/robot/robot.h index ae57aa0..bf829ff 100644 --- a/include/robot/robot.h +++ b/include/robot/robot.h @@ -2,11 +2,17 @@ #define ROBOT_ROBOT_H #include -#include -#include -#include #include #include #include + +#include +#include +#include +#include +#include +#include +#include +#include #endif \ No newline at end of file diff --git a/src/node_handle.cpp b/src/node_handle.cpp index 31657aa..2b70a79 100644 --- a/src/node_handle.cpp +++ b/src/node_handle.cpp @@ -651,7 +651,8 @@ namespace robot std::string key = it->first.as(); const YAML::Node &value = it->second; std::string full_key = prefix.empty() ? key : prefix + "/" + key; - + if(full_key.find("MKTAlgorithmDiffPredictiveTrajectory") == std::string::npos) + continue; if (value.IsMap()) { std::cout << "[NodeHandle] " << indent << full_key << ":" << std::endl; @@ -1727,6 +1728,87 @@ namespace robot return true; } + namespace + { + bool yamlToXmlRpc(const YAML::Node &y, robot_xmlrpcpp::XmlRpcValue &out) + { + if (!y.IsDefined()) + return false; + if (y.IsScalar()) + { + try + { + std::string s = y.as(); + std::string lower = s; + std::transform(lower.begin(), lower.end(), lower.begin(), ::tolower); + if (lower == "true" || lower == "1" || lower == "yes" || lower == "on") + { + out = robot_xmlrpcpp::XmlRpcValue(true); + return true; + } + if (lower == "false" || lower == "0" || lower == "no" || lower == "off") + { + out = robot_xmlrpcpp::XmlRpcValue(false); + return true; + } + try + { + int i = y.as(); + out = robot_xmlrpcpp::XmlRpcValue(i); + return true; + } + catch (...) {} + try + { + double d = y.as(); + out = robot_xmlrpcpp::XmlRpcValue(d); + return true; + } + catch (...) {} + out = robot_xmlrpcpp::XmlRpcValue(s); + return true; + } + catch (...) + { + return false; + } + } + if (y.IsSequence()) + { + out = robot_xmlrpcpp::XmlRpcValue(); + out.setSize(static_cast(y.size())); + for (size_t i = 0; i < y.size(); ++i) + { + robot_xmlrpcpp::XmlRpcValue item; + if (yamlToXmlRpc(y[i], item)) + out[static_cast(i)] = item; + } + return true; + } + if (y.IsMap()) + { + out = robot_xmlrpcpp::XmlRpcValue(); + for (YAML::const_iterator it = y.begin(); it != y.end(); ++it) + { + std::string k = it->first.as(); + robot_xmlrpcpp::XmlRpcValue item; + if (yamlToXmlRpc(it->second, item)) + out[k] = item; + } + return true; + } + return false; + } + } + + bool NodeHandle::getParam(const std::string &key, robot_xmlrpcpp::XmlRpcValue &v) const + { + YAML::Node value = getNestedValue(key); + if (!value.IsDefined()) + return false; + return yamlToXmlRpc(value, v); + } + template T NodeHandle::param(const std::string ¶m_name) const { diff --git a/src/plugin_loader_helper.cpp b/src/plugin_loader_helper.cpp index bcdbdc9..ea1dbe8 100644 --- a/src/plugin_loader_helper.cpp +++ b/src/plugin_loader_helper.cpp @@ -66,6 +66,7 @@ std::string PluginLoaderHelper::findLibraryPath(const std::string& symbol_name) } // Try to read from NodeHandle std::string library_path; + robot::log_info_at(__FILE__, __LINE__, "%s", symbol_name.c_str()); if (nh_.hasParam(param_path)) { nh_.getParam(param_path, library_path, std::string("")); if (!library_path.empty()) { @@ -338,7 +339,7 @@ std::string PluginLoaderHelper::getBuildDirectory() std::string PluginLoaderHelper::getWorkspacePath() { // Method 1: Từ environment variable PNKX_NAV_CORE_DIR - const char* workspace_path = std::getenv("PNKX_NAV_CORE_DIR"); + const char* workspace_path = std::getenv("PNKX_NAV_CORE_LIBRARY_PATH"); if (workspace_path && std::filesystem::exists(workspace_path)) { return std::string(workspace_path); }