update

CMakeLists.txt

@@ -1,39 +1,95 @@
-# --- CMake version và project name ---
-cmake_minimum_required(VERSION 3.10)
-project(dock_planner)
+cmake_minimum_required(VERSION 3.0.2)
+project(dock_planner VERSION 1.0.0 LANGUAGES CXX)
 
-# --- C++ standard và position independent code ---
-set(CMAKE_CXX_STANDARD 17)            # Sử dụng C++17
-set(CMAKE_POSITION_INDEPENDENT_CODE ON)  # Thư viện có thể build thành shared lib
+if(DEFINED CATKIN_DEVEL_PREFIX OR DEFINED CATKIN_TOPLEVEL)
+  set(BUILDING_WITH_CATKIN TRUE)
+  message(STATUS "Building dock_planner with Catkin")
 
-# --- RPATH settings: ưu tiên thư viện build tại chỗ ---
-# Dùng để runtime linker tìm thư viện đã build trước khi install
-set(CMAKE_SKIP_BUILD_RPATH FALSE)
-set(CMAKE_BUILD_WITH_INSTALL_RPATH FALSE)
-set(CMAKE_BUILD_RPATH "${CMAKE_BINARY_DIR}/dock_planner")
-set(CMAKE_INSTALL_RPATH "${CMAKE_BINARY_DIR}/dock_planner")
-set(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE)
+else()
+  set(BUILDING_WITH_CATKIN FALSE)
+  message(STATUS "Building dock_planner with Standalone CMake")
+endif()
 
-# --- Dependencies ---
-# Tìm các thư viện cần thiết
-# find_package(tf3 REQUIRED)            # Nếu dùng tf3
-find_package(Eigen3 REQUIRED)          # Thư viện Eigen cho toán học
-find_package(Boost REQUIRED COMPONENTS system thread filesystem)  # Boost: system, thread, filesystem
+# C++ Standard - must be set before find_package
+set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+set(CMAKE_CXX_EXTENSIONS OFF)
 
-# --- Include directories ---
-# Thêm các folder chứa header files
-include_directories(
-    include  
+# Find dependencies
+find_package(Eigen3 REQUIRED)
+find_package(Boost REQUIRED COMPONENTS system thread filesystem)
+
+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_visualization_msgs
+    robot_nav_msgs
+    robot_nav_2d_msgs
+    robot_nav_2d_utils
+    robot_std_msgs
+    robot_geometry_msgs
+    tf3
+    robot_time
+    data_convert
+    robot_costmap_2d
+    robot_nav_core
+    robot_cpp
+  )
+
+  find_library(TF3_LIBRARY NAMES tf3 PATHS /usr/lib /usr/local/lib /usr/lib/x86_64-linux-gnu)
+
+else()
+
+# ========================================================
+# Catkin specific configuration
+# ========================================================
+  find_package(catkin REQUIRED COMPONENTS
+    robot_visualization_msgs
+    robot_nav_msgs
+    robot_nav_2d_msgs
+    robot_nav_2d_utils
+    robot_std_msgs
+    robot_geometry_msgs
+    robot_time
+    data_convert
+    robot_costmap_2d
+    robot_nav_core
+    robot_cpp
+  )
+
+  find_package(Boost REQUIRED COMPONENTS system thread filesystem)
+  find_package(Eigen3 REQUIRED)
+  # Find tf3 library
+  find_library(TF3_LIBRARY NAMES tf3 PATHS /usr/lib /usr/local/lib /usr/lib/x86_64-linux-gnu)
+  
+  catkin_package(
+    INCLUDE_DIRS include
+    LIBRARIES ${PROJECT_NAME}
+    CATKIN_DEPENDS robot_visualization_msgs robot_nav_msgs robot_nav_2d_msgs robot_nav_2d_utils robot_std_msgs robot_geometry_msgs robot_time data_convert robot_costmap_2d robot_nav_core robot_cpp
+    DEPENDS Eigen3 Boost
+  )
+
+  include_directories(
+    include
+    ${catkin_INCLUDE_DIRS}
     ${EIGEN3_INCLUDE_DIRS}
     ${Boost_INCLUDE_DIRS}
-)
+    ${TF3_INCLUDE_DIR}
+  )
+endif()
 
-# --- Eigen và PCL definitions ---
-add_definitions(${EIGEN3_DEFINITIONS})
-
-# --- Core library: dock_planner ---
-# Tạo thư viện chính
-add_library(dock_planner
+# ========================================================
+# Libraries
+# ========================================================
+add_library(${PROJECT_NAME} SHARED
   src/dock_planner.cpp
   src/dock_calc.cpp
   src/utils/curve_common.cpp
@@ -41,50 +97,94 @@ add_library(dock_planner
   src/utils/line_common.cpp
 )
 
-# --- Link các thư viện phụ thuộc ---
-target_link_libraries(dock_planner
-    ${Boost_LIBRARIES}        # Boost
-    robot_visualization_msgs
-    robot_nav_msgs
-    tf3
-    tf3_geometry_msgs
-    robot_time
-    data_convert
-    costmap_2d
-    nav_core
-    robot_cpp
-    robot_nav_2d_utils
-)
+if(BUILDING_WITH_CATKIN)
+  add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 
-# --- Include directories cho target ---
-target_include_directories(dock_planner 
-        PUBLIC 
-            ${Boost_INCLUDE_DIRS}  # Boost headers
-            $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>   # Khi build từ source
-            $<INSTALL_INTERFACE:include/${PROJECT_NAME}>             # Khi install
-        )
+  target_include_directories(${PROJECT_NAME}
+    PUBLIC
+      $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+      $<INSTALL_INTERFACE:include>
+    ${TF3_INCLUDE_DIR}
+  )
 
-# --- Cài đặt thư viện vào hệ thống khi chạy make install ---
-install(TARGETS dock_planner
-  EXPORT dock_planner-targets
-  ARCHIVE DESTINATION lib      # Thư viện tĩnh .a
-  LIBRARY DESTINATION lib      # Thư viện động .so
-  RUNTIME DESTINATION bin      # File thực thi (nếu có)
-  INCLUDES DESTINATION include # Cài đặt include
-)
+  target_link_libraries(${PROJECT_NAME}
+    PUBLIC ${catkin_LIBRARIES}
+    PRIVATE Eigen3::Eigen Boost::system Boost::thread Boost::filesystem
+    ${TF3_LIBRARY}
+  )
 
-# --- Xuất export set costmap_2dTargets thành file CMake module ---
-# --- Tạo file lib/cmake/dock_planner/costmap_2dTargets.cmake ---
-# --- File này chứa cấu hình giúp project khác có thể dùng ---
-# --- Find_package(dock_planner REQUIRED) ---
-# --- Target_link_libraries(my_app PRIVATE dock_planner::dock_planner) ---
-install(EXPORT dock_planner-targets 
-  FILE dock_planner-targets.cmake
-  NAMESPACE dock_planner::
-  DESTINATION lib/cmake/dock_planner
-)
+else()
 
-# --- Cài đặt headers ---
-install(DIRECTORY include/${PROJECT_NAME}/
-  DESTINATION include/${PROJECT_NAME}
-)
+  target_include_directories(${PROJECT_NAME}
+    PUBLIC
+      $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+      $<INSTALL_INTERFACE:include>
+    ${TF3_INCLUDE_DIR}
+  )
+
+  target_link_libraries(${PROJECT_NAME}
+    PUBLIC
+      ${PACKAGES_DIR}
+    PRIVATE
+      Eigen3::Eigen Boost::system Boost::thread Boost::filesystem
+      ${TF3_LIBRARY}
+  )
+
+  set_target_properties(${PROJECT_NAME} PROPERTIES
+    LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}
+    BUILD_RPATH "${CMAKE_BINARY_DIR}"
+    INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/lib"
+  )
+
+endif()
+
+# ========================================================
+# Install
+# ========================================================
+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}
+  )
+
+  ## Mark cpp header files for installation
+  install(DIRECTORY include/${PROJECT_NAME}/
+    DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+    FILES_MATCHING PATTERN "*.h"
+    PATTERN ".svn" EXCLUDE
+  )
+
+else()
+
+  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/${PROJECT_NAME}/
+    DESTINATION include
+    FILES_MATCHING PATTERN "*.h"
+    PATTERN ".svn" EXCLUDE
+  )
+
+  # 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_visualization_msgs, robot_nav_msgs, robot_nav_2d_msgs, robot_nav_2d_utils, robot_std_msgs, robot_geometry_msgs, tf3, robot_time, data_convert, robot_costmap_2d, robot_nav_core, robot_cpp, Eigen3, Boost")
+  message(STATUS "=================================")
+endif()

include/dock_planner/dock_calc.h

@@ -8,16 +8,16 @@
 #include <robot_nav_msgs/OccupancyGrid.h>
 #include <robot_nav_msgs/GetPlan.h>
 
-#include <costmap_2d/costmap_2d_robot.h>
-#include <costmap_2d/costmap_2d.h>
-#include <costmap_2d/inflation_layer.h>
+#include <robot_costmap_2d/costmap_2d_robot.h>
+#include <robot_costmap_2d/costmap_2d.h>
+#include <robot_costmap_2d/inflation_layer.h>
 
 #include <tf3/LinearMath/Quaternion.h>
 
 #include "dock_planner/utils/curve_common.h"
 #include "dock_planner/utils/pose.h"
 #include "dock_planner/utils/common.h"
-#include <robot/console.h>
+#include <robot/robot.h>
 
 #include <vector>
 
@@ -120,7 +120,7 @@ namespace dock_planner
 
     public:
         //Params
-        costmap_2d::Costmap2D* costmap_;
+        robot_costmap_2d::Costmap2D* costmap_;
 
         int cost_threshold_;     // Threshold for obstacle detection
         double safety_distance_;    // Safety distance from obstacles

include/dock_planner/dock_planner.h

@@ -4,25 +4,27 @@
 
 
 #include "dock_planner/dock_calc.h"
-#include <nav_core/base_global_planner.h>
+#include <robot_nav_core/base_global_planner.h>
 
 namespace dock_planner {
-    class DockPlanner : public nav_core::BaseGlobalPlanner 
+    class DockPlanner : public robot_nav_core::BaseGlobalPlanner 
     {
     public:
         DockPlanner();
-        DockPlanner(std::string name, costmap_2d::Costmap2DROBOT* costmap_robot);
+        DockPlanner(std::string name, robot_costmap_2d::Costmap2DROBOT* costmap_robot);
         
-        bool initialize(std::string name, costmap_2d::Costmap2DROBOT* costmap_robot);
+        bool initialize(std::string name, robot_costmap_2d::Costmap2DROBOT* costmap_robot);
         
         bool makePlan(const robot_geometry_msgs::PoseStamped& start,
                     const robot_geometry_msgs::PoseStamped& goal,
                     std::vector<robot_geometry_msgs::PoseStamped>& plan);
 
+        static robot_nav_core::BaseGlobalPlanner::Ptr create();
+
     private:
         // Core components
-        costmap_2d::Costmap2DROBOT* costmap_robot_;
-        costmap_2d::Costmap2D* costmap_;
+        robot_costmap_2d::Costmap2DROBOT* costmap_robot_;
+        robot_costmap_2d::Costmap2D* costmap_;
         bool initialized_;
         std::string frame_id_;
         // ros::Publisher plan_pub_;
@@ -33,9 +35,6 @@ namespace dock_planner {
         bool check_free_space_;
 
         DockCalc calc_plan_to_dock_;
-        
-
-        void publishPlan(const std::vector<robot_geometry_msgs::PoseStamped>& plan);
     };
 } // namespace dock_planner
 

include/dock_planner/utils/curve_common.h

@@ -6,7 +6,7 @@
 #include <robot_geometry_msgs/Point.h>
 #include <robot_visualization_msgs/Marker.h>
 #include "color.h"
-#include <robot/console.h>
+#include <robot/robot.h>
 
 struct Spline_Inf
 {

package.xml

@@ -0,0 +1,58 @@
+<package>
+  <name>dock_planner</name>
+  <version>0.7.10</version>
+  <description>
+    dock_planner is the second generation of the transform library, which lets
+    the user keep track of multiple coordinate frames over time. dock_planner
+    maintains the relationship between coordinate frames in a tree
+    structure buffered in time, and lets the user transform points,
+    vectors, etc between any two coordinate frames at any desired
+    point in time.
+  </description>
+  <author>Tully Foote</author>
+  <author>Eitan Marder-Eppstein</author>
+  <author>Wim Meeussen</author>
+  <maintainer email="tfoote@osrfoundation.org">Tully Foote</maintainer>
+  <license>BSD</license>
+
+  <url type="website">http://www.ros.org/wiki/dock_planner</url>
+    
+  <buildtool_depend version_gte="0.5.68">catkin</buildtool_depend>
+
+  <build_depend>robot_costmap_2d</build_depend>
+  <run_depend>robot_costmap_2d</run_depend>
+
+  <build_depend>robot_nav_core</build_depend>
+  <run_depend>robot_nav_core</run_depend>
+
+  <build_depend>robot_geometry_msgs</build_depend>
+  <run_depend>robot_geometry_msgs</run_depend>
+
+  <build_depend>robot_nav_msgs</build_depend>
+  <run_depend>robot_nav_msgs</run_depend>
+
+  <build_depend>robot_std_msgs</build_depend>
+  <run_depend>robot_std_msgs</run_depend>
+
+  
+  
+
+  <build_depend>robot_cpp</build_depend>
+  <run_depend>robot_cpp</run_depend>
+
+  <build_depend>robot_time</build_depend>
+  <run_depend>robot_time</run_depend>
+
+  <build_depend>robot_visualization_msgs</build_depend>
+  <run_depend>robot_visualization_msgs</run_depend>
+
+  <build_depend>data_convert</build_depend>
+  <run_depend>data_convert</run_depend>
+
+  <build_depend>robot_nav_2d_msgs</build_depend>
+  <run_depend>robot_nav_2d_msgs</run_depend>
+
+  <build_depend>robot_nav_2d_utils</build_depend>
+  <run_depend>robot_nav_2d_utils</run_depend>
+
+</package>

src/dock_calc.cpp

@@ -728,7 +728,7 @@ namespace dock_planner
         //         check_y >= 0 && check_y < costmap_->getSizeInCellsY()) 
         //     {
         //         unsigned char cost = costmap_->getCost(check_x, check_y);
-        //         if (cost >= costmap_2d::LETHAL_OBSTACLE) 
+        //         if (cost >= robot_costmap_2d::LETHAL_OBSTACLE) 
         //         {
         //             double dx = pose.position.x - space_footprint[i].x;
         //             double dy = pose.position.y - space_footprint[i].y;

src/dock_planner.cpp

@@ -1,6 +1,7 @@
 #include "dock_planner/dock_planner.h"
 #include <cmath>
 #include <algorithm>
+#include <boost/dll/alias.hpp>
 
 namespace dock_planner 
 {
@@ -21,7 +22,7 @@ namespace dock_planner
      * @param costmap_robot Pointer tới costmap ROS wrapper
      * Tự động gọi initialize() nếu được cung cấp costmap
      */
-    DockPlanner::DockPlanner(std::string name, costmap_2d::Costmap2DROBOT* costmap_robot) 
+    DockPlanner::DockPlanner(std::string name, robot_costmap_2d::Costmap2DROBOT* costmap_robot) 
         : costmap_robot_(nullptr), 
         costmap_(nullptr), 
         initialized_(false), 
@@ -43,7 +44,7 @@ namespace dock_planner
      * - Load parameters từ ROS parameter server
      * - Đánh dấu trạng thái initialized
      */
-    bool DockPlanner::initialize(std::string name, costmap_2d::Costmap2DROBOT* costmap_robot) 
+    bool DockPlanner::initialize(std::string name, robot_costmap_2d::Costmap2DROBOT* costmap_robot) 
     {
         if (!initialized_) 
         {
@@ -79,6 +80,7 @@ namespace dock_planner
             initialized_ = true;
             // ROS_WARN("[dock_planner] check_free_space_(%d)", check_free_space_);
         }
+        return true;
     }
     
     bool DockPlanner::makePlan(const robot_geometry_msgs::PoseStamped& start,
@@ -108,7 +110,6 @@ namespace dock_planner
           pose.pose.orientation = planMoveToDock[i].orientation;
           plan.push_back(pose);
         }
-        publishPlan(plan);
         return true;
     }
 
@@ -121,4 +122,13 @@ namespace dock_planner
     //     plan_pub_.publish(path_msg);
     // }
 
-} // namespace dock_planner
+    // Export factory function
+    robot_nav_core::BaseGlobalPlanner::Ptr DockPlanner::create() {
+        return std::make_shared<dock_planner::DockPlanner>();
+    }
+
+
+} // namespace dock_planner
+
+// Alias cho Boost.DLL (nếu muốn dùng boost::dll::import_alias)
+BOOST_DLL_ALIAS(dock_planner::DockPlanner::create, DockPlanner)