update for ROS

2026-01-07 17:01:51 +07:00
parent ca9e100bd9
commit 768573b2bb
34 changed files with 2630 additions and 1963 deletions
--- a/src/Libraries/robot_cpp/CMakeLists.txt
+++ b/src/Libraries/robot_cpp/CMakeLists.txt
@@ -1,40 +1,64 @@
 cmake_minimum_required(VERSION 3.0.2)
+project(robot_cpp VERSION 1.0.0 LANGUAGES CXX)

-# ========================================================
-# Dual-mode CMakeLists.txt: Supports both Catkin and Standalone CMake
-# ========================================================
-
-# Detect if building with Catkin
 if(DEFINED CATKIN_DEVEL_PREFIX OR DEFINED CATKIN_TOPLEVEL)
  set(BUILDING_WITH_CATKIN TRUE)
  message(STATUS "Building robot_cpp with Catkin")
+
 else()
  set(BUILDING_WITH_CATKIN FALSE)
  message(STATUS "Building robot_cpp with Standalone CMake")
 endif()

-project(robot_cpp)
-
+# C++ Standard - must be set before find_package
 set(CMAKE_CXX_STANDARD 17)
 set(CMAKE_CXX_STANDARD_REQUIRED ON)
-set(CMAKE_POSITION_INDEPENDENT_CODE ON)
+set(CMAKE_CXX_EXTENSIONS OFF)

-# ========================================================
-# Find Packages
-# ========================================================
+# Find dependencies
+find_package(yaml-cpp REQUIRED)
+find_package(console_bridge REQUIRED)

-if(BUILDING_WITH_CATKIN)
-  ## Find catkin macros and libraries
-  find_package(catkin REQUIRED COMPONENTS
+if (NOT BUILDING_WITH_CATKIN)
+
+  # Enable Position Independent Code
+  set(CMAKE_POSITION_INDEPENDENT_CODE ON)
+
+  # Cấu hình RPATH để tránh cycle trong runtime search path
+  set(CMAKE_BUILD_RPATH_USE_ORIGIN TRUE)
+  set(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE)
+  set(CMAKE_BUILD_RPATH "${CMAKE_BINARY_DIR}")
+
+  set(PACKAGES_DIR  
    robot_xmlrpcpp
+    robot_time
+  )
+
+else()
+
+# ========================================================
+# Catkin specific configuration
+# ========================================================
+  find_package(catkin REQUIRED COMPONENTS         
+    robot_xmlrpcpp
+    robot_time
+  )
+
+  catkin_package(
+    INCLUDE_DIRS include
+    LIBRARIES ${PROJECT_NAME}
+    CATKIN_DEPENDS robot_xmlrpcpp robot_time
+    # Note: yaml-cpp and console_bridge are system dependencies, 
+    # linked via target_link_libraries, not via catkin_package DEPENDS
+  )
+
+  include_directories(
+    include
+    ${catkin_INCLUDE_DIRS}
  )
 endif()

-## System dependencies
-# Find yaml-cpp library
-find_package(yaml-cpp REQUIRED)
-
-# Use target if available (modern CMake)
+# Use yaml-cpp target if available (modern CMake)
 if(TARGET yaml-cpp::yaml-cpp)
  set(YAML_CPP_TARGET yaml-cpp::yaml-cpp)
  message(STATUS "Using yaml-cpp target: yaml-cpp::yaml-cpp")
@@ -51,80 +75,83 @@ else()
  message(STATUS "Using yaml-cpp library name: yaml-cpp")
 endif()

-find_package(console_bridge REQUIRED)
-
-# ========================================================
-# Catkin specific configuration
-# ========================================================
-
-if(BUILDING_WITH_CATKIN)
-  ## The catkin_package macro generates cmake config files for your package
-  ## Note: yaml-cpp and console_bridge are system dependencies, not declared in DEPENDS
-  ## as they don't provide the required INCLUDE_DIRS/LIBRARIES variables for catkin
-  catkin_package(
-    INCLUDE_DIRS include
-    LIBRARIES robot_cpp
-    CATKIN_DEPENDS robot_xmlrpcpp
-  )
-endif()
-
-###########
-## Build ##
-###########
-
-## Compiler flags
+# Compiler flags
 if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  # Warning flags - disabled to suppress warnings during build
  # add_compile_options(-Wall -Wextra -Wpedantic -fPIC)
  add_compile_options(-w -fPIC)  # -w suppresses all warnings
 endif()

-## Include directories
-include_directories(
-  include
-)
-
-# yaml-cpp and console_bridge are linked via targets, so we don't need to add their include dirs
-# if they provide targets (modern CMake), or we handle them via target_link_libraries
-
-if(BUILDING_WITH_CATKIN)
-  include_directories(${catkin_INCLUDE_DIRS})
-endif()
-
-# Create unified robot_cpp library combining console and node_handle
+# Libraries
 add_library(${PROJECT_NAME} SHARED
  src/console.cpp
  src/node_handle.cpp
  src/plugin_loader_helper.cpp
 )

-target_include_directories(${PROJECT_NAME}
-  PUBLIC
-    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
-    $<INSTALL_INTERFACE:include>
-)
+# Enable C++17 filesystem feature
+target_compile_features(${PROJECT_NAME} PUBLIC cxx_std_17)

-## Link libraries
-# Use the determined yaml-cpp target/library
-target_link_libraries(${PROJECT_NAME}
-  PUBLIC
-    ${YAML_CPP_TARGET}
-    robot_xmlrpcpp
-    robot_time
-  PRIVATE
-    dl  # Required for dladdr() function used in plugin_loader_helper.cpp
-)
-
-
-
-## Add cmake target dependencies
 if(BUILDING_WITH_CATKIN)
  add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-endif()

-set_target_properties(${PROJECT_NAME} PROPERTIES
-  LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}
-)
+  target_include_directories(${PROJECT_NAME}
+    PUBLIC
+      $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+      $<INSTALL_INTERFACE:include>
+  )
+
+  # Link filesystem library if needed (for GCC < 9 or Clang)
+  set(FILESYSTEM_LIB "")
+  if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU" AND CMAKE_CXX_COMPILER_VERSION VERSION_LESS "9.0")
+    set(FILESYSTEM_LIB "stdc++fs")
+  elseif(CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+    find_library(FILESYSTEM_LIB c++fs)
+  endif()
+
+  target_link_libraries(${PROJECT_NAME}
+    PUBLIC ${catkin_LIBRARIES}
+    PRIVATE ${YAML_CPP_TARGET}
+    PRIVATE dl  # Required for dladdr() function used in plugin_loader_helper.cpp
+  )
+  
+  if(FILESYSTEM_LIB)
+    target_link_libraries(${PROJECT_NAME} PRIVATE ${FILESYSTEM_LIB})
+  endif()
+
+else()
+
+  target_include_directories(${PROJECT_NAME}
+    PUBLIC
+      $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+      $<INSTALL_INTERFACE:include>
+  )
+
+  # Link filesystem library if needed (for GCC < 9 or Clang)
+  set(FILESYSTEM_LIB "")
+  if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU" AND CMAKE_CXX_COMPILER_VERSION VERSION_LESS "9.0")
+    set(FILESYSTEM_LIB "stdc++fs")
+  elseif(CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+    find_library(FILESYSTEM_LIB c++fs)
+  endif()
+
+  target_link_libraries(${PROJECT_NAME}
+    PUBLIC
+      ${PACKAGES_DIR}
+      ${YAML_CPP_TARGET}
+    PRIVATE
+      dl  # Required for dladdr() function used in plugin_loader_helper.cpp
+  )
+  
+  if(FILESYSTEM_LIB)
+    target_link_libraries(${PROJECT_NAME} PRIVATE ${FILESYSTEM_LIB})
+  endif()
+
+  set_target_properties(${PROJECT_NAME} PROPERTIES
+    LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}
+  )
+
+endif()

 # Define CMAKE_BINARY_DIR as compile definition so it's available at runtime
 target_compile_definitions(${PROJECT_NAME} 
@@ -132,25 +159,44 @@ target_compile_definitions(${PROJECT_NAME}
    PNKX_NAV_CORE_BUILD_DIR="${CMAKE_BINARY_DIR}"
 )

-# ========================================================
-# Installation (Standalone CMake only)
-# ========================================================
+if(BUILDING_WITH_CATKIN)
+  ## Mark libraries for installation
+  ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
+  install(TARGETS ${PROJECT_NAME}
+    ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+    LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+    RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
+  )

-# Export target trong mọi trường hợp để các target khác có thể export và phụ thuộc vào nó
-install(TARGETS ${PROJECT_NAME}
-  EXPORT ${PROJECT_NAME}-targets
-  ARCHIVE DESTINATION lib
-  LIBRARY DESTINATION lib
-  RUNTIME DESTINATION bin)
+  ## Mark cpp header files for installation
+  install(DIRECTORY include/
+    DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+    FILES_MATCHING PATTERN "*.h"
+    PATTERN ".svn" EXCLUDE
+  )
+  
+else()

-if(NOT BUILDING_WITH_CATKIN)
+  install(TARGETS ${PROJECT_NAME}
+    EXPORT ${PROJECT_NAME}-targets
+    ARCHIVE DESTINATION lib
+    LIBRARY DESTINATION lib
+    RUNTIME DESTINATION bin
+  )
+
+  # Export targets
+  install(EXPORT ${PROJECT_NAME}-targets
+    FILE ${PROJECT_NAME}-targets.cmake
+    NAMESPACE ${PROJECT_NAME}::
+    DESTINATION lib/cmake/${PROJECT_NAME}
+  )
+
+  ## Mark cpp header files for installation
  install(DIRECTORY include/
    DESTINATION include
-    FILES_MATCHING PATTERN "*.h")
-
-  install(EXPORT ${PROJECT_NAME}-targets
-    # NAMESPACE robot::
-    DESTINATION lib/cmake/${PROJECT_NAME})
+    FILES_MATCHING PATTERN "*.h"
+    PATTERN ".svn" EXCLUDE
+  )

  # ========================================================
  # Install CMake Config File for find_package()
@@ -167,29 +213,16 @@ if(NOT BUILDING_WITH_CATKIN)
    ${CMAKE_CURRENT_BINARY_DIR}/robot_cppConfig.cmake
    DESTINATION lib/cmake/${PROJECT_NAME}
  )
-else()
-  # Khi build với Catkin, vẫn cần export để các target khác có thể export
-  install(EXPORT ${PROJECT_NAME}-targets
-    # NAMESPACE robot::
-    DESTINATION lib/cmake/${PROJECT_NAME})
+
+  # Print configuration info
+  message(STATUS "=================================")
+  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 "=================================")
 endif()

-# ========================================================
-# Install CMake Config File for find_package()
-# ========================================================
-# Generate config file from template
-configure_file(
-  ${CMAKE_CURRENT_SOURCE_DIR}/robot_cppConfig.cmake.in
-  ${CMAKE_CURRENT_BINARY_DIR}/robot_cppConfig.cmake
-  @ONLY
-)
-
-# Install config file
-install(FILES
-  ${CMAKE_CURRENT_BINARY_DIR}/robot_cppConfig.cmake
-  DESTINATION lib/cmake/${PROJECT_NAME}
-)
-
 # ========================================================
 # Test executable
 # ========================================================
@@ -207,8 +240,8 @@ if(BUILD_TESTS)
  
  target_link_libraries(test_node_handle
    PRIVATE
-      robot_cpp
-      yaml-cpp
+      ${PROJECT_NAME}
+      ${YAML_CPP_TARGET}
  )
  
  set_target_properties(test_node_handle PROPERTIES
--- a/src/Libraries/robot_cpp/include/robot/plugin_loader_helper.h
+++ b/src/Libraries/robot_cpp/include/robot/plugin_loader_helper.h
@@ -38,10 +38,15 @@ class PluginLoaderHelper
 public:
  /**
   * @brief Constructor
-   * @param nh NodeHandle to read parameters from (default: root NodeHandle)
+   */
+  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 = robot::NodeHandle(), const std::string& config_namespace = "");
+  PluginLoaderHelper(robot::NodeHandle nh, const std::string& config_namespace = "");

  /**
   * @brief Find library path from symbol name (export name)
@@ -76,6 +81,12 @@ public:
   */
  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)
--- a/src/Libraries/robot_cpp/src/plugin_loader_helper.cpp
+++ b/src/Libraries/robot_cpp/src/plugin_loader_helper.cpp
@@ -27,17 +27,25 @@
 namespace robot
 {

+PluginLoaderHelper::PluginLoaderHelper()
+  : PluginLoaderHelper(robot::NodeHandle(), "")
+{
+  // Delegate to the other constructor with default NodeHandle
+}
+
 PluginLoaderHelper::PluginLoaderHelper(robot::NodeHandle nh, const std::string& config_namespace)
  : nh_(nh), config_namespace_(config_namespace)
 {
  std::string build_dir = getBuildDirectory();
  if (!build_dir.empty()) {
    search_paths_.push_back(build_dir);
-    // Thêm các subdirectories thường dùng
-    search_paths_.push_back(build_dir + "/src/Algorithms/Packages/global_planners");
-    search_paths_.push_back(build_dir + "/src/Algorithms/Packages/local_planners");
-    search_paths_.push_back(build_dir + "/src/Navigations/Cores/robot_nav_core2_adapter");
-    search_paths_.push_back(build_dir + "/src/Libraries/robot_costmap_2d");
+  }
+  
+  // Thêm các subdirectories thường dùng từ workspace path
+  std::string workspace_path = getWorkspacePath();
+  if (!workspace_path.empty()) {
+    search_paths_.push_back(workspace_path + "/install/lib");
+    search_paths_.push_back(workspace_path + "/devel/lib");
  }
 }

@@ -56,7 +64,6 @@ std::string PluginLoaderHelper::findLibraryPath(const std::string& symbol_name)
  } else {
    param_path = symbol_name + "/library_path";
  }
-  
  // Try to read from NodeHandle
  std::string library_path;
  if (nh_.hasParam(param_path)) {
@@ -97,12 +104,13 @@ std::string PluginLoaderHelper::findLibraryPath(const std::string& symbol_name)
    std::string ld_path_str(ld_path);
    std::stringstream ss(ld_path_str);
    std::string path;
+
    while (std::getline(ss, path, ':')) {
      if (!path.empty()) {
        // Try different naming conventions
        std::vector<std::string> possible_names = {
-          "lib" + symbol_name + ".so",
-          symbol_name + ".so"
+          "lib" + library_path + ".so",
+          library_path + ".so"
        };
        
        // Also try without namespace
@@ -111,7 +119,7 @@ std::string PluginLoaderHelper::findLibraryPath(const std::string& symbol_name)
          possible_names.push_back("lib" + short_name + ".so");
          possible_names.push_back(short_name + ".so");
        }
-        
+      
        for (const auto& name : possible_names) {
          std::filesystem::path full_path = std::filesystem::path(path) / name;
          if (std::filesystem::exists(full_path)) {
@@ -327,5 +335,103 @@ std::string PluginLoaderHelper::getBuildDirectory()
  return "";
 }

+std::string PluginLoaderHelper::getWorkspacePath()
+{
+  // Method 1: Từ environment variable PNKX_NAV_CORE_DIR
+  const char* workspace_path = std::getenv("PNKX_NAV_CORE_DIR");
+  if (workspace_path && std::filesystem::exists(workspace_path)) {
+    return std::string(workspace_path);
+  }
+
+  // Method 2: Từ environment variable ROS_WORKSPACE hoặc CATKIN_WS
+  const char* ros_workspace = std::getenv("ROS_WORKSPACE");
+  if (ros_workspace && std::filesystem::exists(ros_workspace)) {
+    return std::string(ros_workspace);
+  }
+
+  const char* catkin_ws = std::getenv("CATKIN_WS");
+  if (catkin_ws && std::filesystem::exists(catkin_ws)) {
+    return std::string(catkin_ws);
+  }
+
+  // Method 3: Từ build directory (lấy parent directory)
+  std::string build_dir = getBuildDirectory();
+  if (!build_dir.empty()) {
+    std::filesystem::path build_path(build_dir);
+    std::filesystem::path parent = build_path.parent_path();
+    // Kiểm tra xem có phải workspace không (có thư mục src/)
+    if (std::filesystem::exists(parent / "src") && std::filesystem::is_directory(parent / "src")) {
+      return parent.string();
+    }
+  }
+
+  // Method 4: Từ executable path và tìm workspace
+  try {
+    char exe_path[PATH_MAX];
+    ssize_t count = readlink("/proc/self/exe", exe_path, PATH_MAX);
+    if (count != -1) {
+      exe_path[count] = '\0';
+      std::filesystem::path exe_dir = std::filesystem::path(exe_path).parent_path();
+      
+      // Thử các relative paths: ../.., ../../.., etc. để tìm workspace
+      std::vector<std::filesystem::path> relative_paths = {
+        exe_dir.parent_path().parent_path(),  // ../..
+        exe_dir.parent_path().parent_path().parent_path(),  // ../../..
+        exe_dir.parent_path().parent_path().parent_path().parent_path()  // ../../../..
+      };
+      
+      for (const auto& rel_path : relative_paths) {
+        // Kiểm tra xem có phải workspace không (có thư mục src/ hoặc build/)
+        if (std::filesystem::exists(rel_path) && 
+            std::filesystem::is_directory(rel_path) &&
+            (std::filesystem::exists(rel_path / "src") || 
+             std::filesystem::exists(rel_path / "build"))) {
+          try {
+            return std::filesystem::canonical(rel_path).string();
+          } catch (...) {
+            return rel_path.string();
+          }
+        }
+      }
+    }
+  } catch (const std::exception&) {
+    // Ignore errors
+  }
+
+  // Method 5: Từ library path (librobot_cpp.so) và tìm workspace
+  Dl_info dl_info;
+  if (dladdr(reinterpret_cast<void*>(&PluginLoaderHelper::getWorkspacePath), &dl_info) != 0) {
+    if (dl_info.dli_fname) {
+      std::filesystem::path lib_path(dl_info.dli_fname);
+      std::filesystem::path lib_dir = lib_path.parent_path();
+      
+      // Tìm workspace từ library path
+      std::filesystem::path current = lib_dir;
+      for (int i = 0; i < 5; ++i) {  // Tối đa 5 levels up
+        if (std::filesystem::exists(current / "src") || 
+            std::filesystem::exists(current / "build")) {
+          try {
+            return std::filesystem::canonical(current).string();
+          } catch (...) {
+            return current.string();
+          }
+        }
+        current = current.parent_path();
+        if (current == current.parent_path()) {  // Reached root
+          break;
+        }
+      }
+    }
+  }
+
+  // Method 6: Hardcoded fallback (nếu các phương pháp trên không hoạt động)
+  std::string fallback = "/home/robotics/AGV/Diff_Wheel_Prj/t800_v2_ws";
+  if (std::filesystem::exists(fallback)) {
+    return fallback;
+  }
+
+  return "";
+}
+
 } // namespace robot