update

CMakeLists.txt

@@ -1,39 +1,95 @@
-# --- CMake version và project name ---
+cmake_minimum_required(VERSION 3.0.2)
-cmake_minimum_required(VERSION 3.10)
+project(dock_planner VERSION 1.0.0 LANGUAGES CXX)
-project(dock_planner)
 
-# --- C++ standard và position independent code ---
+if(DEFINED CATKIN_DEVEL_PREFIX OR DEFINED CATKIN_TOPLEVEL)
-set(CMAKE_CXX_STANDARD 17)            # Sử dụng C++17
+  set(BUILDING_WITH_CATKIN TRUE)
-set(CMAKE_POSITION_INDEPENDENT_CODE ON)  # Thư viện có thể build thành shared lib
+  message(STATUS "Building dock_planner with Catkin")
 
-# --- RPATH settings: ưu tiên thư viện build tại chỗ ---
+else()
-# Dùng để runtime linker tìm thư viện đã build trước khi install
+  set(BUILDING_WITH_CATKIN FALSE)
-set(CMAKE_SKIP_BUILD_RPATH FALSE)
+  message(STATUS "Building dock_planner with Standalone CMake")
-set(CMAKE_BUILD_WITH_INSTALL_RPATH FALSE)
+endif()
-set(CMAKE_BUILD_RPATH "${CMAKE_BINARY_DIR}/dock_planner")
+
-set(CMAKE_INSTALL_RPATH "${CMAKE_BINARY_DIR}/dock_planner")
+# C++ Standard - must be set before find_package
+set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+set(CMAKE_CXX_EXTENSIONS OFF)
+
+# 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}")
 
-# --- Dependencies ---
+  set(PACKAGES_DIR
-# Tìm các thư viện cần thiết
+    robot_visualization_msgs
-# find_package(tf3 REQUIRED)            # Nếu dùng tf3
+    robot_nav_msgs
-find_package(Eigen3 REQUIRED)          # Thư viện Eigen cho toán học
+    robot_nav_2d_msgs
-find_package(Boost REQUIRED COMPONENTS system thread filesystem)  # Boost: system, thread, filesystem
+    robot_nav_2d_utils
-
+    robot_std_msgs
-# --- Include directories ---
+    robot_geometry_msgs
-# Thêm các folder chứa header files
+    tf3
-include_directories(
+    robot_time
-    include  
+    data_convert
-    ${EIGEN3_INCLUDE_DIRS}
+    robot_costmap_2d
-    ${Boost_INCLUDE_DIRS}
+    robot_nav_core
+    robot_cpp
   )
 
-# --- Eigen và PCL definitions ---
+  find_library(TF3_LIBRARY NAMES tf3 PATHS /usr/lib /usr/local/lib /usr/lib/x86_64-linux-gnu)
-add_definitions(${EIGEN3_DEFINITIONS})
 
-# --- Core library: dock_planner ---
+else()
-# Tạo thư viện chính
+
-add_library(dock_planner
+# ========================================================
+# 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()
+
+# ========================================================
+# 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 ---
+if(BUILDING_WITH_CATKIN)
-target_link_libraries(dock_planner
+  add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-    ${Boost_LIBRARIES}        # Boost
-    visualization_msgs
-    nav_msgs
-    tf3
-    tf3_geometry_msgs
-    robot_time
-    data_convert
-    costmap_2d
-    nav_core
-    robot_cpp
-    nav_2d_utils
-)
 
-# --- Include directories cho target ---
+  target_include_directories(${PROJECT_NAME}
-target_include_directories(dock_planner 
     PUBLIC
-            ${Boost_INCLUDE_DIRS}  # Boost headers
+      $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
-            $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>   # Khi build từ source
+      $<INSTALL_INTERFACE:include>
-            $<INSTALL_INTERFACE:include/${PROJECT_NAME}>             # Khi install
+    ${TF3_INCLUDE_DIR}
   )
 
-# --- Cài đặt thư viện vào hệ thống khi chạy make install ---
+  target_link_libraries(${PROJECT_NAME}
-install(TARGETS dock_planner
+    PUBLIC ${catkin_LIBRARIES}
-  EXPORT dock_planner-targets
+    PRIVATE Eigen3::Eigen Boost::system Boost::thread Boost::filesystem
-  ARCHIVE DESTINATION lib      # Thư viện tĩnh .a
+    ${TF3_LIBRARY}
-  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
   )
 
-# --- Xuất export set costmap_2dTargets thành file CMake module ---
+else()
-# --- 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 ---
+  target_include_directories(${PROJECT_NAME}
-# --- Find_package(dock_planner REQUIRED) ---
+    PUBLIC
-# --- Target_link_libraries(my_app PRIVATE dock_planner::dock_planner) ---
+      $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
-install(EXPORT dock_planner-targets 
+      $<INSTALL_INTERFACE:include>
-  FILE dock_planner-targets.cmake
+    ${TF3_INCLUDE_DIR}
-  NAMESPACE dock_planner::
-  DESTINATION lib/cmake/dock_planner
   )
 
-# --- Cài đặt headers ---
+  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 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

@@ -1,23 +1,23 @@
 #ifndef DOCK_CALC_H
 #define DOCK_CALC_H
 
-#include <nav_2d_utils/footprint.h>
+#include <robot_nav_2d_utils/footprint.h>
-#include <geometry_msgs/PoseStamped.h>
+#include <robot_geometry_msgs/PoseStamped.h>
 
-#include <nav_msgs/Path.h>
+#include <robot_nav_msgs/Path.h>
-#include <nav_msgs/OccupancyGrid.h>
+#include <robot_nav_msgs/OccupancyGrid.h>
-#include <nav_msgs/GetPlan.h>
+#include <robot_nav_msgs/GetPlan.h>
 
-#include <costmap_2d/costmap_2d_robot.h>
+#include <robot_costmap_2d/costmap_2d_robot.h>
-#include <costmap_2d/costmap_2d.h>
+#include <robot_costmap_2d/costmap_2d.h>
-#include <costmap_2d/inflation_layer.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>
 
@@ -39,10 +39,10 @@ namespace dock_planner
         Spline_Inf* input_spline_inf;
         CurveCommon* CurveDesign;
 
-        vector<geometry_msgs::Pose> posesOnPathWay;
+        vector<robot_geometry_msgs::Pose> posesOnPathWay;
-        vector<geometry_msgs::Point> footprint_robot_;
+        vector<robot_geometry_msgs::Point> footprint_robot_;
-        geometry_msgs::PoseStamped save_goal_;
+        robot_geometry_msgs::PoseStamped save_goal_;
-        // std::vector<geometry_msgs::Pose> free_zone_;
+        // std::vector<robot_geometry_msgs::Pose> free_zone_;
 
         bool check_goal_;
         int store_start_nearest_idx_;
@@ -50,7 +50,7 @@ namespace dock_planner
         double min_distance_to_goal_;
 
         // Tính khoảng cách giữa 2 điểm
-        inline double calc_distance(const geometry_msgs::Pose& p1, const geometry_msgs::Pose& p2) 
+        inline double calc_distance(const robot_geometry_msgs::Pose& p1, const robot_geometry_msgs::Pose& p2) 
         {
             return std::hypot(p1.position.x - p2.position.x, p1.position.y - p2.position.y);
         }
@@ -80,47 +80,47 @@ namespace dock_planner
                                     int degree, 
                                     double initial_step = 0.02);
 
-        geometry_msgs::Pose offsetPoint(const geometry_msgs::Pose& A,const double& theta, const double& d);
+        robot_geometry_msgs::Pose offsetPoint(const robot_geometry_msgs::Pose& A,const double& theta, const double& d);
 
-        double pointToSegmentDistance(const geometry_msgs::Pose& A, const geometry_msgs::Pose& B, const geometry_msgs::Pose& C);
+        double pointToSegmentDistance(const robot_geometry_msgs::Pose& A, const robot_geometry_msgs::Pose& B, const robot_geometry_msgs::Pose& C);
 
-        double signedAngle(const geometry_msgs::Pose& A, const geometry_msgs::Pose& B, const geometry_msgs::Pose& C);
+        double signedAngle(const robot_geometry_msgs::Pose& A, const robot_geometry_msgs::Pose& B, const robot_geometry_msgs::Pose& C);
 
-        double compute_t(const geometry_msgs::Pose& A, const geometry_msgs::Pose& B, const geometry_msgs::Pose& C);
+        double compute_t(const robot_geometry_msgs::Pose& A, const robot_geometry_msgs::Pose& B, const robot_geometry_msgs::Pose& C);
 
-        geometry_msgs::Pose compute_D(const geometry_msgs::Pose& B, const geometry_msgs::Pose& C,const double& t);
+        robot_geometry_msgs::Pose compute_D(const robot_geometry_msgs::Pose& B, const robot_geometry_msgs::Pose& C,const double& t);
 
-        void updatePoseOrientation(std::vector<geometry_msgs::Pose>& saved_poses,
+        void updatePoseOrientation(std::vector<robot_geometry_msgs::Pose>& saved_poses,
-                                    std::vector<geometry_msgs::Pose>& nurbs_path,
+                                    std::vector<robot_geometry_msgs::Pose>& nurbs_path,
                                     bool reverse = true);
 
-        bool findPathTMP(const vector<geometry_msgs::Pose>& control_points,
+        bool findPathTMP(const vector<robot_geometry_msgs::Pose>& control_points,
-                        vector<geometry_msgs::Pose>& saved_poses,                
+                        vector<robot_geometry_msgs::Pose>& saved_poses,                
                         const int& degree, double step = 0.02);
 
-        bool findNURBSControlPoints(int& dir, vector<geometry_msgs::Pose>& control_points,
+        bool findNURBSControlPoints(int& dir, vector<robot_geometry_msgs::Pose>& control_points,
-                                    const geometry_msgs::Pose& start_pose,
+                                    const robot_geometry_msgs::Pose& start_pose,
-                                    const std::vector<geometry_msgs::Pose>& posesOnPathWay,
+                                    const std::vector<robot_geometry_msgs::Pose>& posesOnPathWay,
                                     const double& angle_threshol, const int& degree);
 
-        int findNearestPointOnPath(int& dir, const geometry_msgs::Pose& pose, 
+        int findNearestPointOnPath(int& dir, const robot_geometry_msgs::Pose& pose, 
-                                const std::vector<geometry_msgs::Pose>& posesOnPathWay, 
+                                const std::vector<robot_geometry_msgs::Pose>& posesOnPathWay, 
                                 const double& angle_threshol);
 
-        bool isFreeSpace(const geometry_msgs::Pose& pose, const std::vector<geometry_msgs::Point>& footprint);
+        bool isFreeSpace(const robot_geometry_msgs::Pose& pose, const std::vector<robot_geometry_msgs::Point>& footprint);
 
-        std::vector<geometry_msgs::Point> calcFreeZone(const geometry_msgs::Pose& pose, 
+        std::vector<robot_geometry_msgs::Point> calcFreeZone(const robot_geometry_msgs::Pose& pose, 
-                                    const std::vector<geometry_msgs::Point>& footprint);
+                                    const std::vector<robot_geometry_msgs::Point>& footprint);
 
-        std::vector<geometry_msgs::Point> interpolateFootprint(const geometry_msgs::Pose& pose, 
+        std::vector<robot_geometry_msgs::Point> interpolateFootprint(const robot_geometry_msgs::Pose& pose, 
-                const std::vector<geometry_msgs::Point>& footprint, const double& resolution);
+                const std::vector<robot_geometry_msgs::Point>& footprint, const double& resolution);
 
-        RobotGeometry computeGeometry(const std::vector<geometry_msgs::Point>& fp);
+        RobotGeometry computeGeometry(const std::vector<robot_geometry_msgs::Point>& fp);
 
 
     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
@@ -130,12 +130,12 @@ namespace dock_planner
 
         ~DockCalc();
 
-        bool makePlanMoveToDock(const geometry_msgs::PoseStamped& start,
+        bool makePlanMoveToDock(const robot_geometry_msgs::PoseStamped& start,
-                                const geometry_msgs::PoseStamped& goal,
+                                const robot_geometry_msgs::PoseStamped& goal,
-                                const vector<geometry_msgs::Point>& footprint_robot,
+                                const vector<robot_geometry_msgs::Point>& footprint_robot,
-                                const vector<geometry_msgs::Point>& footprint_dock,
+                                const vector<robot_geometry_msgs::Point>& footprint_dock,
                                 const int& degree, const double& step,
-                                vector<geometry_msgs::Pose>& planMoveToDock,
+                                vector<robot_geometry_msgs::Pose>& planMoveToDock,
                                 bool reverse = false);
         
         

include/dock_planner/dock_planner.h

@@ -4,38 +4,37 @@
 
 
 #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 geometry_msgs::PoseStamped& start,
+        bool makePlan(const robot_geometry_msgs::PoseStamped& start,
-                    const geometry_msgs::PoseStamped& goal,
+                    const robot_geometry_msgs::PoseStamped& goal,
-                    std::vector<geometry_msgs::PoseStamped>& plan);
+                    std::vector<robot_geometry_msgs::PoseStamped>& plan);
+
+        static robot_nav_core::BaseGlobalPlanner::Ptr create();
 
     private:
         // Core components
-        costmap_2d::Costmap2DROBOT* costmap_robot_;
+        robot_costmap_2d::Costmap2DROBOT* costmap_robot_;
-        costmap_2d::Costmap2D* costmap_;
+        robot_costmap_2d::Costmap2D* costmap_;
         bool initialized_;
         std::string frame_id_;
         // ros::Publisher plan_pub_;
-        std::vector<geometry_msgs::Point> footprint_dock_;
+        std::vector<robot_geometry_msgs::Point> footprint_dock_;
 
         int cost_threshold_;
         double calib_safety_distance_;
         bool check_free_space_;
 
         DockCalc calc_plan_to_dock_;
-        
-
-        void publishPlan(const std::vector<geometry_msgs::PoseStamped>& plan);
     };
 } // namespace dock_planner
 

include/dock_planner/utils/color.h

@@ -1,14 +1,14 @@
 #ifndef COLOR_H_
 #define COLOR_H_
 
-#include <std_msgs/ColorRGBA.h>
+#include <robot_std_msgs/ColorRGBA.h>
 
 namespace agv_visualization
 {
-  class Color : public std_msgs::ColorRGBA
+  class Color : public robot_std_msgs::ColorRGBA
   {
   public:
-    Color() : std_msgs::ColorRGBA() {}
+    Color() : robot_std_msgs::ColorRGBA() {}
     Color(double red, double green, double blue) : Color(red, green, blue, 1.0) {}
     Color(double red, double green, double blue, double alpha) : Color() {
       r = red;

include/dock_planner/utils/conversion.h

@@ -3,14 +3,14 @@
 #include "curve_common.h"
 
 #include <Eigen/Geometry>
-#include <geometry_msgs/Point.h>
+#include <robot_geometry_msgs/Point.h>
-#include <geometry_msgs/Quaternion.h>
+#include <robot_geometry_msgs/Quaternion.h>
 
-inline Eigen::Vector3d EigenVecter3dFromPointMsg(const geometry_msgs::Point& msg) {
+inline Eigen::Vector3d EigenVecter3dFromPointMsg(const robot_geometry_msgs::Point& msg) {
   return Eigen::Vector3d(msg.x, msg.y, msg.z);
 }
 
-inline EigenTrajectoryPoint::Vector EigenTrajectoryVectorFromVector(const std::vector<geometry_msgs::PoseStamped> &plan)
+inline EigenTrajectoryPoint::Vector EigenTrajectoryVectorFromVector(const std::vector<robot_geometry_msgs::PoseStamped> &plan)
 {
   EigenTrajectoryPoint::Vector eigen_trajectory_point_vec;
   EigenTrajectoryPoint eigen_trajectory_point;
@@ -25,7 +25,7 @@ inline EigenTrajectoryPoint::Vector EigenTrajectoryVectorFromVector(const std::v
   return eigen_trajectory_point_vec;
 }
 
-inline EigenTrajectoryPoint::Vector EigenTrajectoryVectorFromVector(const std::vector<geometry_msgs::Point> &discreate_point_vec)
+inline EigenTrajectoryPoint::Vector EigenTrajectoryVectorFromVector(const std::vector<robot_geometry_msgs::Point> &discreate_point_vec)
 {
   EigenTrajectoryPoint::Vector eigen_trajectory_point_vec;
   EigenTrajectoryPoint eigen_trajectory_point;

include/dock_planner/utils/curve_common.h

@@ -2,11 +2,11 @@
 #define CURVE_COMMON_H_
 
 #include <Eigen/Eigen>
-#include <nav_msgs/Path.h>
+#include <robot_nav_msgs/Path.h>
-#include <geometry_msgs/Point.h>
+#include <robot_geometry_msgs/Point.h>
-#include <visualization_msgs/Marker.h>
+#include <robot_visualization_msgs/Marker.h>
 #include "color.h"
-#include <robot/console.h>
+#include <robot/robot.h>
 
 struct Spline_Inf
 {
@@ -43,16 +43,16 @@ class CurveCommon
 {
 public:
   CurveCommon();
-  nav_msgs::Path Generate_Line(geometry_msgs::Point start_point, geometry_msgs::Point end_point, double t_intervel, std::string frame_id);
+  robot_nav_msgs::Path Generate_Line(robot_geometry_msgs::Point start_point, robot_geometry_msgs::Point end_point, double t_intervel, std::string frame_id);
-  nav_msgs::Path Generate_BezierCurve(EigenTrajectoryPoint::Vector control_point, double t_intervel, std::string frame_id);
+  robot_nav_msgs::Path Generate_BezierCurve(EigenTrajectoryPoint::Vector control_point, double t_intervel, std::string frame_id);
-  nav_msgs::Path Generate_BsplineCurve(Spline_Inf bspline_inf, double t_intervel, std::string frame_id);
+  robot_nav_msgs::Path Generate_BsplineCurve(Spline_Inf bspline_inf, double t_intervel, std::string frame_id);
-  nav_msgs::Path Generate_NURBSCurve(Spline_Inf spline_inf, double t_intervel, std::string frame_id);
+  robot_nav_msgs::Path Generate_NURBSCurve(Spline_Inf spline_inf, double t_intervel, std::string frame_id);
-  nav_msgs::Path Generate_DerivativeBsplineCurve(Spline_Inf bspline_inf, int differential_times, double t_intervel, std::string frame_id);
+  robot_nav_msgs::Path Generate_DerivativeBsplineCurve(Spline_Inf bspline_inf, int differential_times, double t_intervel, std::string frame_id);
-  nav_msgs::Path Generate_DerivativeBasisFuncCurve(Spline_Inf bspline_inf, int differential_times, int index, double t_intervel, std::string frame_id);
+  robot_nav_msgs::Path Generate_DerivativeBasisFuncCurve(Spline_Inf bspline_inf, int differential_times, int index, double t_intervel, std::string frame_id);
 
   void CalculateDerivativeBasisFunc(Spline_Inf* spline_inf, double u_data, int differential_times);
-  geometry_msgs::Point CalculateDerivativeCurvePoint(Spline_Inf* spline_inf, double u_data, int differential_times, bool UsingNURBS);
+  robot_geometry_msgs::Point CalculateDerivativeCurvePoint(Spline_Inf* spline_inf, double u_data, int differential_times, bool UsingNURBS);
-  geometry_msgs::Point CalculateCurvePoint(Spline_Inf* spline_inf, double u_data, bool UsingNURBS);
+  robot_geometry_msgs::Point CalculateCurvePoint(Spline_Inf* spline_inf, double u_data, bool UsingNURBS);
   double CalculateCurvature(Spline_Inf spline_inf, double u_data, bool UsingNURBS);
   double CalculateSignedCurvature(Spline_Inf spline_inf, double u_data, bool UsingNURBS);
   Eigen::Vector3d CalculateCurvatureDirectionVector(Spline_Inf spline_inf, double u_data, bool UsingNURBS);
@@ -65,15 +65,15 @@ public:
   void ReadSplineInf(Spline_Inf* bspline_inf, std::vector<double> weight_vector, bool use_limit_derivative_fitting);
   void ReadDiscreate2DPointFromLaunch(EigenTrajectoryPoint::Vector* input_point, std::vector<double> file_discreate_point);
   void ReadDiscreate2DPointFromLaunch(std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> >* input_point, std::vector<double> file_discreate_point);
-  void ShowDiscreatePoint(visualization_msgs::Marker* points, EigenTrajectoryPoint::Vector discreate_point);
+  void ShowDiscreatePoint(robot_visualization_msgs::Marker* points, EigenTrajectoryPoint::Vector discreate_point);
-  void ShowDiscreatePoint(visualization_msgs::Marker* points, std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > discreate_point);
+  void ShowDiscreatePoint(robot_visualization_msgs::Marker* points, std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > discreate_point);
 
   //TODO: move relate visualize function to new vislization.h 
   int print();
-  visualization_msgs::Marker ShowDiscreatePoint(EigenTrajectoryPoint::Vector& discreate_point, const std::string& frame_id, const std::string& name, double scale);
+  robot_visualization_msgs::Marker ShowDiscreatePoint(EigenTrajectoryPoint::Vector& discreate_point, const std::string& frame_id, const std::string& name, double scale);
-  visualization_msgs::Marker ShowDiscreatePoint2(EigenTrajectoryPoint::Vector& discreate_point, const std::string& frame_id, std_msgs::ColorRGBA point_color, const std::string& name, double scale);
+  robot_visualization_msgs::Marker ShowDiscreatePoint2(EigenTrajectoryPoint::Vector& discreate_point, const std::string& frame_id, robot_std_msgs::ColorRGBA point_color, const std::string& name, double scale);
-  visualization_msgs::Marker ShowDiscreatePoint(std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > discreate_point, const std::string& frame_id, const std::string& name, double scale);
+  robot_visualization_msgs::Marker ShowDiscreatePoint(std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > discreate_point, const std::string& frame_id, const std::string& name, double scale);
-  visualization_msgs::Marker ShowDiscreatePoint2(std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > discreate_point, const std::string& frame_id, std_msgs::ColorRGBA point_color, const std::string& name, double scale);
+  robot_visualization_msgs::Marker ShowDiscreatePoint2(std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > discreate_point, const std::string& frame_id, robot_std_msgs::ColorRGBA point_color, const std::string& name, double scale);
 private:
 
 

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

@@ -18,15 +18,15 @@ namespace dock_planner
         delete input_spline_inf;
     }
 
-    geometry_msgs::Pose DockCalc::offsetPoint(const geometry_msgs::Pose& A, const double& theta, const double& d) 
+    robot_geometry_msgs::Pose DockCalc::offsetPoint(const robot_geometry_msgs::Pose& A, const double& theta, const double& d) 
     {
-        geometry_msgs::Pose B;
+        robot_geometry_msgs::Pose B;
         B.position.x = A.position.x + d * std::cos(theta);
         B.position.y = A.position.y + d * std::sin(theta);
         return B;
     }
 
-    double DockCalc::pointToSegmentDistance(const geometry_msgs::Pose& A, const geometry_msgs::Pose& B, const geometry_msgs::Pose& C) 
+    double DockCalc::pointToSegmentDistance(const robot_geometry_msgs::Pose& A, const robot_geometry_msgs::Pose& B, const robot_geometry_msgs::Pose& C) 
     {
        double theta = signedAngle(A,B,C);
        double sin_theta = sin(fabs(theta));
@@ -35,7 +35,7 @@ namespace dock_planner
        return distance;
     }
 
-    double DockCalc::signedAngle(const geometry_msgs::Pose& A, const geometry_msgs::Pose& B, const geometry_msgs::Pose& C)
+    double DockCalc::signedAngle(const robot_geometry_msgs::Pose& A, const robot_geometry_msgs::Pose& B, const robot_geometry_msgs::Pose& C)
     {
         double ABx = B.position.x - A.position.x;
         double ABy = B.position.y - A.position.y;
@@ -50,7 +50,7 @@ namespace dock_planner
         return angle;
     }
 
-    double DockCalc::compute_t(const geometry_msgs::Pose& A, const geometry_msgs::Pose& B, const geometry_msgs::Pose& C) 
+    double DockCalc::compute_t(const robot_geometry_msgs::Pose& A, const robot_geometry_msgs::Pose& B, const robot_geometry_msgs::Pose& C) 
     {
         constexpr double epsilon = 1e-6;
         double dx = C.position.x - B.position.x;
@@ -69,15 +69,15 @@ namespace dock_planner
         return -numerator / denominator;
     }
 
-    geometry_msgs::Pose DockCalc::compute_D(const geometry_msgs::Pose& B, const geometry_msgs::Pose& C, const double& t) 
+    robot_geometry_msgs::Pose DockCalc::compute_D(const robot_geometry_msgs::Pose& B, const robot_geometry_msgs::Pose& C, const double& t) 
     {
-        geometry_msgs::Pose D;
+        robot_geometry_msgs::Pose D;
         D.position.x = B.position.x + t * (C.position.x - B.position.x);
         D.position.y = B.position.y + t * (C.position.y - B.position.y); 
         return D;
     }
 
-    RobotGeometry DockCalc::computeGeometry(const std::vector<geometry_msgs::Point>& fp)
+    RobotGeometry DockCalc::computeGeometry(const std::vector<robot_geometry_msgs::Point>& fp)
     {
         double min_x =  1e9, max_x = -1e9;
         double min_y =  1e9, max_y = -1e9;
@@ -99,12 +99,12 @@ namespace dock_planner
         return {length, width, radius};           
     }
 
-    bool DockCalc::makePlanMoveToDock(const geometry_msgs::PoseStamped& start,
+    bool DockCalc::makePlanMoveToDock(const robot_geometry_msgs::PoseStamped& start,
-                                    const geometry_msgs::PoseStamped& goal,
+                                    const robot_geometry_msgs::PoseStamped& goal,
-                                    const vector<geometry_msgs::Point>& footprint_robot,
+                                    const vector<robot_geometry_msgs::Point>& footprint_robot,
-                                    const vector<geometry_msgs::Point>& footprint_dock,
+                                    const vector<robot_geometry_msgs::Point>& footprint_dock,
                                     const int& degree, const double& step,
-                                    vector<geometry_msgs::Pose>& planMoveToDock,
+                                    vector<robot_geometry_msgs::Pose>& planMoveToDock,
                                     bool reverse)
     {
         constexpr double epsilon = 1e-6;
@@ -141,10 +141,10 @@ namespace dock_planner
         }
 
         // Lambda: Generate NURBS path
-        auto generateNURBSPath = [&](const vector<geometry_msgs::Pose>& control_points, bool check_isFreeSpace, 
+        auto generateNURBSPath = [&](const vector<robot_geometry_msgs::Pose>& control_points, bool check_isFreeSpace, 
-                                    bool edge_end_plag, bool path_reverse) -> vector<geometry_msgs::Pose> 
+                                    bool edge_end_plag, bool path_reverse) -> vector<robot_geometry_msgs::Pose> 
         {
-            vector<geometry_msgs::Pose> result;
+            vector<robot_geometry_msgs::Pose> result;
             bool obstacle_flag = false;
 
             // Convert to Eigen format
@@ -180,14 +180,14 @@ namespace dock_planner
             // robot::log_error("[dock_calc] Adaptive step size: %f", step_new);
 
             // Generate path points
-            vector<geometry_msgs::Pose> saved_poses;
+            vector<robot_geometry_msgs::Pose> saved_poses;
             for (double t = 0.0; t <= 1.0; t += step_new) 
             {
-                geometry_msgs::Point point = CurveDesign->CalculateCurvePoint(input_spline_inf, t, true);
+                robot_geometry_msgs::Point point = CurveDesign->CalculateCurvePoint(input_spline_inf, t, true);
 
                 if (!std::isnan(point.x) && !std::isnan(point.y))
                 {
-                    geometry_msgs::Pose pose;
+                    robot_geometry_msgs::Pose pose;
                     pose.position = point;
                     pose.orientation = data_convert::createQuaternionMsgFromYaw(0.0);
 
@@ -261,9 +261,9 @@ namespace dock_planner
         min_distance_to_goal_ = (safety_distance_ + distance_dock / 2.0);
 
         // Calculate pose C (offset from goal)
-        geometry_msgs::Pose Goal_Pose = goal.pose;
+        robot_geometry_msgs::Pose Goal_Pose = goal.pose;
         double yaw_goal = normalizeAngle(data_convert::getYaw(goal.pose.orientation));
-        geometry_msgs::Pose Pose_C = offsetPoint(Goal_Pose, yaw_goal, distance_min_for_pose_C);
+        robot_geometry_msgs::Pose Pose_C = offsetPoint(Goal_Pose, yaw_goal, distance_min_for_pose_C);
 
         // Compute t parameter and determine direction
         double t = compute_t(start.pose, goal.pose, Pose_C);
@@ -288,7 +288,7 @@ namespace dock_planner
         }
 
         // Calculate pose D and distance AD
-        geometry_msgs::Pose Pose_D = compute_D(goal.pose, Pose_C, t);
+        robot_geometry_msgs::Pose Pose_D = compute_D(goal.pose, Pose_C, t);
         double distance_AD = pointToSegmentDistance(start.pose, goal.pose, Pose_C);
 
         // Calculate angle threshold with scaling
@@ -308,13 +308,13 @@ namespace dock_planner
         double distance_DF = distance_AD / tan(angle_alpha);
         double distance_min_for_pose_F = distance_min_for_pose_C + distance_CD + distance_DF;
 
-        geometry_msgs::Pose Pose_F = offsetPoint(Goal_Pose, yaw_goal, distance_min_for_pose_F);
+        robot_geometry_msgs::Pose Pose_F = offsetPoint(Goal_Pose, yaw_goal, distance_min_for_pose_F);
-        geometry_msgs::Pose mid_pose;
+        robot_geometry_msgs::Pose mid_pose;
         mid_pose.position.x = (Pose_C.position.x + Pose_F.position.x) / 2.0;
         mid_pose.position.y = (Pose_C.position.y + Pose_F.position.y) / 2.0;
 
-        vector<geometry_msgs::Pose> control_points_tmp = {Pose_C, mid_pose, Pose_F};
+        vector<robot_geometry_msgs::Pose> control_points_tmp = {Pose_C, mid_pose, Pose_F};
-        vector<geometry_msgs::Pose> saved_poses_tmp;
+        vector<robot_geometry_msgs::Pose> saved_poses_tmp;
 
 
         if (!findPathTMP(control_points_tmp, saved_poses_tmp, degree, step)) 
@@ -325,7 +325,7 @@ namespace dock_planner
 
         // Find first segment control points
         int dir;
-        vector<geometry_msgs::Pose> control_points_1;
+        vector<robot_geometry_msgs::Pose> control_points_1;
         if (!findNURBSControlPoints(dir, control_points_1, start.pose, saved_poses_tmp, angle_threshold, degree)) 
         {
             robot::log_error("[dock_calc] Failed to find NURBS control points for first segment.");
@@ -337,21 +337,21 @@ namespace dock_planner
                                     ((dir == 1 || dir == 0) && dir_robot_move_to_goal);
 
         // Generate first segment
-        vector<geometry_msgs::Pose> first_segment = generateNURBSPath(control_points_1, true, false, first_segment_reverse);
+        vector<robot_geometry_msgs::Pose> first_segment = generateNURBSPath(control_points_1, true, false, first_segment_reverse);
         planMoveToDock.insert(planMoveToDock.end(), first_segment.begin(), first_segment.end());
 
         //Generate second segment (to goal)
         if (!planMoveToDock.empty()) 
         {
-            geometry_msgs::Pose mid_control_point;
+            robot_geometry_msgs::Pose mid_control_point;
             mid_control_point.position.x = (planMoveToDock.back().position.x + goal.pose.position.x) / 2.0;
             mid_control_point.position.y = (planMoveToDock.back().position.y + goal.pose.position.y) / 2.0;
 
-            vector<geometry_msgs::Pose> control_points_2 = {planMoveToDock.back(), 
+            vector<robot_geometry_msgs::Pose> control_points_2 = {planMoveToDock.back(), 
                                                             mid_control_point, 
                                                             goal.pose};
 
-            vector<geometry_msgs::Pose> second_segment = generateNURBSPath(control_points_2, true, true, dir_robot_move_to_goal);
+            vector<robot_geometry_msgs::Pose> second_segment = generateNURBSPath(control_points_2, true, true, dir_robot_move_to_goal);
             planMoveToDock.insert(planMoveToDock.end(), second_segment.begin(), second_segment.end());
             planMoveToDock.insert(planMoveToDock.begin(),start.pose);
             planMoveToDock.insert(planMoveToDock.end(),goal.pose);
@@ -360,8 +360,8 @@ namespace dock_planner
         return !planMoveToDock.empty();
     }
 
-    bool DockCalc::findPathTMP(const vector<geometry_msgs::Pose>& control_points,
+    bool DockCalc::findPathTMP(const vector<robot_geometry_msgs::Pose>& control_points,
-                                vector<geometry_msgs::Pose>& saved_poses,                
+                                vector<robot_geometry_msgs::Pose>& saved_poses,                
                                 const int& degree, double step)
     {
         // Chuyển đổi control points sang định dạng Eigen::Vector3d
@@ -393,15 +393,15 @@ namespace dock_planner
         CurveDesign->ReadSplineInf(input_spline_inf, degree + 1, eigen_control_points, knot_vector);
         CurveDesign->ReadSplineInf(input_spline_inf, weights, false);
 
-        // vector<geometry_msgs::Pose> poses_tmp;
+        // vector<robot_geometry_msgs::Pose> poses_tmp;
         for(double t = 0.0; t <= 1.0; t += step) 
         {
             // robot::log_error("[dock_calc][findPathTMP] t(%f) DEBUG 10", t);
-            geometry_msgs::Point point = CurveDesign->CalculateCurvePoint(input_spline_inf, t, true);
+            robot_geometry_msgs::Point point = CurveDesign->CalculateCurvePoint(input_spline_inf, t, true);
             if(!std::isnan(point.x) && !std::isnan(point.y)) 
             {
                 // robot::log_error("[dock_calc][findPathTMP] point(%f, %f) at t(%f)", point.x, point.y, t);
-                geometry_msgs::Pose pose;
+                robot_geometry_msgs::Pose pose;
                 pose.position = point;
                 pose.orientation.x = 0.0;
                 pose.orientation.y = 0.0; 
@@ -432,8 +432,8 @@ namespace dock_planner
         return true;
     }
 
-    void DockCalc::updatePoseOrientation(std::vector<geometry_msgs::Pose>& saved_poses,
+    void DockCalc::updatePoseOrientation(std::vector<robot_geometry_msgs::Pose>& saved_poses,
-                                        std::vector<geometry_msgs::Pose>& nurbs_path,
+                                        std::vector<robot_geometry_msgs::Pose>& nurbs_path,
                                         bool reverse) 
     {
         double yaw;
@@ -465,7 +465,7 @@ namespace dock_planner
                                         double initial_step) 
     {
         double length = 0.0;
-        geometry_msgs::Point prev_point, curr_point;
+        robot_geometry_msgs::Point prev_point, curr_point;
         std::vector<double> segment_lengths;
 
         static double step;
@@ -492,9 +492,9 @@ namespace dock_planner
         return length;
     }
 
-    bool DockCalc::findNURBSControlPoints(int& dir, vector<geometry_msgs::Pose>& control_points,
+    bool DockCalc::findNURBSControlPoints(int& dir, vector<robot_geometry_msgs::Pose>& control_points,
-                                        const geometry_msgs::Pose& start_pose,
+                                        const robot_geometry_msgs::Pose& start_pose,
-                                        const std::vector<geometry_msgs::Pose>& saved_poses,
+                                        const std::vector<robot_geometry_msgs::Pose>& saved_poses,
                                         const double& angle_threshold, const int& degree)
     {
         if (saved_poses.empty()) 
@@ -507,9 +507,9 @@ namespace dock_planner
         int idx = findNearestPointOnPath(dir, start_pose, saved_poses, angle_threshold);
 
         // Helper lambda to create pose at position
-        auto createPose = [](double x, double y, double yaw = 0.0) -> geometry_msgs::Pose 
+        auto createPose = [](double x, double y, double yaw = 0.0) -> robot_geometry_msgs::Pose 
         {
-            geometry_msgs::Pose pose;
+            robot_geometry_msgs::Pose pose;
             pose.position.x = x;
             pose.position.y = y;
             pose.orientation = data_convert::createQuaternionMsgFromYaw(0);
@@ -584,8 +584,8 @@ namespace dock_planner
         return true;
     }
 
-    int DockCalc::findNearestPointOnPath(int& dir, const geometry_msgs::Pose& pose, 
+    int DockCalc::findNearestPointOnPath(int& dir, const robot_geometry_msgs::Pose& pose, 
-                                        const std::vector<geometry_msgs::Pose>& saved_poses, 
+                                        const std::vector<robot_geometry_msgs::Pose>& saved_poses, 
                                         const double& angle_threshol)
     {
         // Tìm điểm gần nhất
@@ -657,14 +657,14 @@ namespace dock_planner
         return nearest_idx;
     }
 
-    bool DockCalc::isFreeSpace(const geometry_msgs::Pose& pose, 
+    bool DockCalc::isFreeSpace(const robot_geometry_msgs::Pose& pose, 
-                            const std::vector<geometry_msgs::Point>& footprint) 
+                            const std::vector<robot_geometry_msgs::Point>& footprint) 
     {
-        // static std::vector<geometry_msgs::Pose> free_zone;
+        // static std::vector<robot_geometry_msgs::Pose> free_zone;
 
         // auto makePose = [](double x, double y, double yaw) 
         // {
-        //     geometry_msgs::Pose p;
+        //     robot_geometry_msgs::Pose p;
         //     p.position.x = x;
         //     p.position.y = y;
         //     p.position.z = 0.0;
@@ -696,17 +696,17 @@ namespace dock_planner
 
         // if(check_goal_)
         // {
-        //     std::vector<geometry_msgs::Point> result_free_zone = calcFreeZone(pose, footprint);
+        //     std::vector<robot_geometry_msgs::Point> result_free_zone = calcFreeZone(pose, footprint);
         //     if((int)result_free_zone.size() < 4) return false;
         //     free_zone.push_back(makePose(result_free_zone[0].x, result_free_zone[0].y, 0.0));
         //     free_zone.push_back(makePose(result_free_zone[1].x, result_free_zone[1].y, 0.0));
         //     free_zone.push_back(makePose(result_free_zone[2].x, result_free_zone[2].y, 0.0));
         //     check_goal_ = false;
         // }
-        // std::vector<geometry_msgs::Point> space_footprint = interpolateFootprint(pose, footprint, resolution);
+        // std::vector<robot_geometry_msgs::Point> space_footprint = interpolateFootprint(pose, footprint, resolution);
         // for (int i = 0; i < (int)space_footprint.size(); i++) 
         // {
-        //     geometry_msgs::Pose Pose_on_footprint;
+        //     robot_geometry_msgs::Pose Pose_on_footprint;
         //     Pose_on_footprint.position.x = space_footprint[i].x;
         //     Pose_on_footprint.position.y = space_footprint[i].y;
         //     if((int)free_zone.size() < 3) return false;
@@ -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;
@@ -745,11 +745,11 @@ namespace dock_planner
         return true;
     }
 
-    std::vector<geometry_msgs::Point> DockCalc::calcFreeZone(const geometry_msgs::Pose& pose, 
+    std::vector<robot_geometry_msgs::Point> DockCalc::calcFreeZone(const robot_geometry_msgs::Pose& pose, 
-                                        const std::vector<geometry_msgs::Point>& footprint)
+                                        const std::vector<robot_geometry_msgs::Point>& footprint)
     {
         // Dịch sang tọa độ tuyệt đối
-        std::vector<geometry_msgs::Point> free_zone;
+        std::vector<robot_geometry_msgs::Point> free_zone;
         double cos_th = std::cos(data_convert::getYaw(pose.orientation));
         double sin_th = std::sin(data_convert::getYaw(pose.orientation));
         for (auto pt : footprint)
@@ -757,7 +757,7 @@ namespace dock_planner
             pt.x *= 1.2;
             pt.y *= 1.2;
 
-            geometry_msgs::Point abs_pt;
+            robot_geometry_msgs::Point abs_pt;
             abs_pt.x = pose.position.x + pt.x * cos_th - pt.y * sin_th;
             abs_pt.y = pose.position.y + pt.x * sin_th + pt.y * cos_th;
             free_zone.push_back(abs_pt);
@@ -765,29 +765,29 @@ namespace dock_planner
         return free_zone;
     }
 
-    std::vector<geometry_msgs::Point> DockCalc::interpolateFootprint(const geometry_msgs::Pose& pose, 
+    std::vector<robot_geometry_msgs::Point> DockCalc::interpolateFootprint(const robot_geometry_msgs::Pose& pose, 
-                        const std::vector<geometry_msgs::Point>& footprint, const double& resolution)
+                        const std::vector<robot_geometry_msgs::Point>& footprint, const double& resolution)
     {
 
     // Dịch sang tọa độ tuyệt đối
-    std::vector<geometry_msgs::Point> abs_footprint;
+    std::vector<robot_geometry_msgs::Point> abs_footprint;
     double cos_th = std::cos(data_convert::getYaw(pose.orientation));
     double sin_th = std::sin(data_convert::getYaw(pose.orientation));
     for (auto pt : footprint)
     {
         pt.x *= 1.1;
         pt.y *= 1.1;
-        geometry_msgs::Point abs_pt;
+        robot_geometry_msgs::Point abs_pt;
         abs_pt.x = pose.position.x + pt.x * cos_th - pt.y * sin_th;
         abs_pt.y = pose.position.y + pt.x * sin_th + pt.y * cos_th;
         abs_footprint.push_back(abs_pt);
     }
 
-    std::vector<geometry_msgs::Point> points;
+    std::vector<robot_geometry_msgs::Point> points;
     for (size_t i = 0; i < (int)abs_footprint.size(); ++i)
     {
-        const geometry_msgs::Point &start = abs_footprint[i];
+        const robot_geometry_msgs::Point &start = abs_footprint[i];
-        const geometry_msgs::Point &end = abs_footprint[(i + 1) % abs_footprint.size()];
+        const robot_geometry_msgs::Point &end = abs_footprint[(i + 1) % abs_footprint.size()];
 
         double dx = end.x - start.x;
         double dy = end.y - start.y;
@@ -799,7 +799,7 @@ namespace dock_planner
         if (j == steps && i != (int)abs_footprint.size() - 1)
             continue; // tránh lặp
         double t = static_cast<double>(j) / steps;
-        geometry_msgs::Point pt;
+        robot_geometry_msgs::Point pt;
         pt.x = start.x + t * dx;
         pt.y = start.y + t * dy;
         points.push_back(pt);

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_) 
         {
@@ -63,13 +64,13 @@ namespace dock_planner
             calc_plan_to_dock_.calib_safety_distance_ = calib_safety_distance_;
 
 
-            // plan_pub_ = private_nh.advertise<nav_msgs::Path>("plan", 1);
+            // plan_pub_ = private_nh.advertise<robot_nav_msgs::Path>("plan", 1);
 
-            nav_2d_msgs::Polygon2D footprint_dock ;//= nav_2d_utils::footprintFromParams(private_nh,false);
+            robot_nav_2d_msgs::Polygon2D footprint_dock ;//= robot_nav_2d_utils::footprintFromParams(private_nh,false);
             
             for(auto pt : footprint_dock.points)
             {
-                geometry_msgs::Point footprint_point;
+                robot_geometry_msgs::Point footprint_point;
                 footprint_point.x = pt.x;
                 footprint_point.y = pt.y;
                 footprint_point.z = 0.0;
@@ -79,16 +80,17 @@ namespace dock_planner
             initialized_ = true;
             // ROS_WARN("[dock_planner] check_free_space_(%d)", check_free_space_);
         }
+        return true;
     }
     
-    bool DockPlanner::makePlan(const geometry_msgs::PoseStamped& start,
+    bool DockPlanner::makePlan(const robot_geometry_msgs::PoseStamped& start,
-                                const geometry_msgs::PoseStamped& goal,
+                                const robot_geometry_msgs::PoseStamped& goal,
-                                std::vector<geometry_msgs::PoseStamped>& plan)
+                                std::vector<robot_geometry_msgs::PoseStamped>& plan)
     {
         if(!initialized_) return false;
         if(!plan.empty()) plan.clear();
-        vector<geometry_msgs::Pose> planMoveToDock;
+        vector<robot_geometry_msgs::Pose> planMoveToDock;
-        std::vector<geometry_msgs::Point> footprint_robot = costmap_robot_->getRobotFootprint();
+        std::vector<robot_geometry_msgs::Point> footprint_robot = costmap_robot_->getRobotFootprint();
 
         // footprint_dock_ = costmap_robot_->getRobotFootprint();
         int degree = 2;
@@ -99,7 +101,7 @@ namespace dock_planner
         if(!status_makePlanMoveToDock) return false;
         for(int i = 0; i < (int)planMoveToDock.size(); i++)
         {
-          geometry_msgs::PoseStamped pose;
+          robot_geometry_msgs::PoseStamped pose;
           pose.header.stamp = plan_time;
           pose.header.frame_id = costmap_robot_->getGlobalFrameID();
           pose.pose.position.x = planMoveToDock[i].position.x;
@@ -108,17 +110,25 @@ namespace dock_planner
           pose.pose.orientation = planMoveToDock[i].orientation;
           plan.push_back(pose);
         }
-        publishPlan(plan);
         return true;
     }
 
-    // void DockPlanner::publishPlan(const std::vector<geometry_msgs::PoseStamped>& plan) 
+    // void DockPlanner::publishPlan(const std::vector<robot_geometry_msgs::PoseStamped>& plan) 
     // {
-    //     nav_msgs::Path path_msg;
+    //     robot_nav_msgs::Path path_msg;
     //     path_msg.header.stamp = robot::Time::now();
     //     path_msg.header.frame_id = frame_id_;
     //     path_msg.poses = plan;
     //     plan_pub_.publish(path_msg);
     // }
 
+    // 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) 

src/utils/curve_common.cpp

@@ -1,16 +1,16 @@
 #include "dock_planner/utils/curve_common.h"
 #include "dock_planner/utils/conversion.h"
-#include <geometry_msgs/PoseStamped.h>
+#include <robot_geometry_msgs/PoseStamped.h>
 #include <iostream>
 
 CurveCommon::CurveCommon()
 {
 }
 
-nav_msgs::Path CurveCommon::Generate_Line(geometry_msgs::Point start_point, geometry_msgs::Point end_point, double t_intervel, std::string frame_id)
+robot_nav_msgs::Path CurveCommon::Generate_Line(robot_geometry_msgs::Point start_point, robot_geometry_msgs::Point end_point, double t_intervel, std::string frame_id)
 {
-  nav_msgs::Path line_result;
+  robot_nav_msgs::Path line_result;
-  geometry_msgs::PoseStamped current_pose;
+  robot_geometry_msgs::PoseStamped current_pose;
 
   line_result.header.frame_id = frame_id;
   line_result.header.stamp = robot::Time::now();
@@ -35,10 +35,10 @@ nav_msgs::Path CurveCommon::Generate_Line(geometry_msgs::Point start_point, geom
   return line_result;
 }
 
-nav_msgs::Path CurveCommon::Generate_BezierCurve(EigenTrajectoryPoint::Vector control_point, double t_intervel, std::string frame_id)
+robot_nav_msgs::Path CurveCommon::Generate_BezierCurve(EigenTrajectoryPoint::Vector control_point, double t_intervel, std::string frame_id)
 {
-  nav_msgs::Path bezier_curve_result;
+  robot_nav_msgs::Path bezier_curve_result;
-  geometry_msgs::PoseStamped current_pose;
+  robot_geometry_msgs::PoseStamped current_pose;
   EigenTrajectoryPoint::Vector temp_control_point_vec;
 
   bezier_curve_result.header.frame_id = frame_id;
@@ -133,13 +133,13 @@ void CurveCommon::ReadDiscreate2DPointFromLaunch(std::vector<Eigen::Vector3d, Ei
   //  }
 }
 
-visualization_msgs::Marker CurveCommon::ShowDiscreatePoint(std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>> discreate_point, const std::string &frame_id, const std::string &name, double scale)
+robot_visualization_msgs::Marker CurveCommon::ShowDiscreatePoint(std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>> discreate_point, const std::string &frame_id, const std::string &name, double scale)
 {
-  visualization_msgs::Marker waypoints_marker;
+  robot_visualization_msgs::Marker waypoints_marker;
 
   waypoints_marker.header.frame_id = frame_id;
   waypoints_marker.header.stamp = robot::Time::now();
-  waypoints_marker.type = visualization_msgs::Marker::SPHERE_LIST;
+  waypoints_marker.type = robot_visualization_msgs::Marker::SPHERE_LIST;
   // waypoints_marker.color = color;
   waypoints_marker.color.r = 1;
   waypoints_marker.color.g = 1;
@@ -151,7 +151,7 @@ visualization_msgs::Marker CurveCommon::ShowDiscreatePoint(std::vector<Eigen::Ve
 
   waypoints_marker.points.reserve(discreate_point.size());
 
-  geometry_msgs::Point view_point;
+  robot_geometry_msgs::Point view_point;
   for (int i = 0; i < discreate_point.size(); i++)
   {
     view_point.x = discreate_point.at(i)(0);
@@ -162,13 +162,13 @@ visualization_msgs::Marker CurveCommon::ShowDiscreatePoint(std::vector<Eigen::Ve
   return waypoints_marker;
 }
 
-visualization_msgs::Marker CurveCommon::ShowDiscreatePoint2(std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>> discreate_point, const std::string &frame_id, std_msgs::ColorRGBA point_color, const std::string &name, double scale)
+robot_visualization_msgs::Marker CurveCommon::ShowDiscreatePoint2(std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>> discreate_point, const std::string &frame_id, robot_std_msgs::ColorRGBA point_color, const std::string &name, double scale)
 {
-  visualization_msgs::Marker waypoints_marker;
+  robot_visualization_msgs::Marker waypoints_marker;
 
   waypoints_marker.header.frame_id = frame_id;
   waypoints_marker.header.stamp = robot::Time::now();
-  waypoints_marker.type = visualization_msgs::Marker::SPHERE_LIST;
+  waypoints_marker.type = robot_visualization_msgs::Marker::SPHERE_LIST;
   waypoints_marker.color = point_color;
   waypoints_marker.color.a = 0.75;
   waypoints_marker.ns = name;
@@ -178,7 +178,7 @@ visualization_msgs::Marker CurveCommon::ShowDiscreatePoint2(std::vector<Eigen::V
 
   waypoints_marker.points.reserve(discreate_point.size());
 
-  geometry_msgs::Point view_point;
+  robot_geometry_msgs::Point view_point;
   for (int i = 0; i < discreate_point.size(); i++)
   {
     view_point.x = discreate_point.at(i)(0);
@@ -189,13 +189,13 @@ visualization_msgs::Marker CurveCommon::ShowDiscreatePoint2(std::vector<Eigen::V
   return waypoints_marker;
 }
 
-visualization_msgs::Marker CurveCommon::ShowDiscreatePoint(EigenTrajectoryPoint::Vector &discreate_point, const std::string &frame_id, const std::string &name, double scale)
+robot_visualization_msgs::Marker CurveCommon::ShowDiscreatePoint(EigenTrajectoryPoint::Vector &discreate_point, const std::string &frame_id, const std::string &name, double scale)
 {
-  visualization_msgs::Marker waypoints_marker;
+  robot_visualization_msgs::Marker waypoints_marker;
 
   waypoints_marker.header.frame_id = frame_id;
   waypoints_marker.header.stamp = robot::Time::now();
-  waypoints_marker.type = visualization_msgs::Marker::SPHERE_LIST;
+  waypoints_marker.type = robot_visualization_msgs::Marker::SPHERE_LIST;
   // waypoints_marker.color = color;
   waypoints_marker.color.r = 1;
   waypoints_marker.color.g = 1;
@@ -207,7 +207,7 @@ visualization_msgs::Marker CurveCommon::ShowDiscreatePoint(EigenTrajectoryPoint:
 
   waypoints_marker.points.reserve(discreate_point.size());
 
-  geometry_msgs::Point view_point;
+  robot_geometry_msgs::Point view_point;
   for (int i = 0; i < discreate_point.size(); i++)
   {
     view_point.x = discreate_point.at(i).position(0);
@@ -218,13 +218,13 @@ visualization_msgs::Marker CurveCommon::ShowDiscreatePoint(EigenTrajectoryPoint:
   return waypoints_marker;
 }
 
-visualization_msgs::Marker CurveCommon::ShowDiscreatePoint2(EigenTrajectoryPoint::Vector &discreate_point, const std::string &frame_id, std_msgs::ColorRGBA point_color, const std::string &name, double scale)
+robot_visualization_msgs::Marker CurveCommon::ShowDiscreatePoint2(EigenTrajectoryPoint::Vector &discreate_point, const std::string &frame_id, robot_std_msgs::ColorRGBA point_color, const std::string &name, double scale)
 {
-  visualization_msgs::Marker waypoints_marker;
+  robot_visualization_msgs::Marker waypoints_marker;
 
   waypoints_marker.header.frame_id = frame_id;
   waypoints_marker.header.stamp = robot::Time::now();
-  waypoints_marker.type = visualization_msgs::Marker::SPHERE_LIST;
+  waypoints_marker.type = robot_visualization_msgs::Marker::SPHERE_LIST;
   waypoints_marker.color = point_color;
   // waypoints_marker.color.r = r;
   // waypoints_marker.color.g = g;
@@ -237,7 +237,7 @@ visualization_msgs::Marker CurveCommon::ShowDiscreatePoint2(EigenTrajectoryPoint
 
   waypoints_marker.points.reserve(discreate_point.size());
 
-  geometry_msgs::Point view_point;
+  robot_geometry_msgs::Point view_point;
   for (int i = 0; i < discreate_point.size(); i++)
   {
     view_point.x = discreate_point.at(i).position(0);
@@ -248,9 +248,9 @@ visualization_msgs::Marker CurveCommon::ShowDiscreatePoint2(EigenTrajectoryPoint
   return waypoints_marker;
 }
 
-void CurveCommon::ShowDiscreatePoint(visualization_msgs::Marker *points, EigenTrajectoryPoint::Vector discreate_point)
+void CurveCommon::ShowDiscreatePoint(robot_visualization_msgs::Marker *points, EigenTrajectoryPoint::Vector discreate_point)
 {
-  geometry_msgs::Point view_point;
+  robot_geometry_msgs::Point view_point;
   for (int i = 0; i < discreate_point.size(); i++)
   {
     view_point.x = discreate_point.at(i).position(0);
@@ -259,9 +259,9 @@ void CurveCommon::ShowDiscreatePoint(visualization_msgs::Marker *points, EigenTr
   }
 }
 
-void CurveCommon::ShowDiscreatePoint(visualization_msgs::Marker *points, std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>> discreate_point)
+void CurveCommon::ShowDiscreatePoint(robot_visualization_msgs::Marker *points, std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>> discreate_point)
 {
-  geometry_msgs::Point view_point;
+  robot_geometry_msgs::Point view_point;
   for (int i = 0; i < discreate_point.size(); i++)
   {
     view_point.x = discreate_point.at(i)(0);
@@ -322,10 +322,10 @@ void CurveCommon::ReadSplineInf(Spline_Inf *spline_inf, std::vector<double> weig
   // }
 }
 
-nav_msgs::Path CurveCommon::Generate_BsplineCurve(Spline_Inf bspline_inf, double t_intervel, std::string frame_id)
+robot_nav_msgs::Path CurveCommon::Generate_BsplineCurve(Spline_Inf bspline_inf, double t_intervel, std::string frame_id)
 {
-  nav_msgs::Path bspline_curve_result;
+  robot_nav_msgs::Path bspline_curve_result;
-  geometry_msgs::PoseStamped current_pose;
+  robot_geometry_msgs::PoseStamped current_pose;
 
   bspline_curve_result.header.frame_id = frame_id;
   bspline_curve_result.header.stamp = robot::Time::now();
@@ -495,10 +495,10 @@ nav_msgs::Path CurveCommon::Generate_BsplineCurve(Spline_Inf bspline_inf, double
   return bspline_curve_result;
 }
 
-nav_msgs::Path CurveCommon::Generate_NURBSCurve(Spline_Inf spline_inf, double t_intervel, std::string frame_id)
+robot_nav_msgs::Path CurveCommon::Generate_NURBSCurve(Spline_Inf spline_inf, double t_intervel, std::string frame_id)
 {
-  nav_msgs::Path nurbs_curve_result;
+  robot_nav_msgs::Path nurbs_curve_result;
-  geometry_msgs::PoseStamped current_pose;
+  robot_geometry_msgs::PoseStamped current_pose;
 
   nurbs_curve_result.header.frame_id = frame_id;
   nurbs_curve_result.header.stamp = robot::Time::now();
@@ -749,9 +749,9 @@ void CurveCommon::CalculateDerivativeBasisFunc(Spline_Inf *spline_inf, double u_
   }
 }
 
-geometry_msgs::Point CurveCommon::CalculateDerivativeCurvePoint(Spline_Inf *spline_inf, double u_data, int differential_times, bool UsingNURBS)
+robot_geometry_msgs::Point CurveCommon::CalculateDerivativeCurvePoint(Spline_Inf *spline_inf, double u_data, int differential_times, bool UsingNURBS)
 {
-  geometry_msgs::Point derivative_curve_point;
+  robot_geometry_msgs::Point derivative_curve_point;
   int p_degree = spline_inf->order - 1;
   double sum_x = 0, sum_y = 0;
   double sum_denom = 0;
@@ -873,11 +873,11 @@ geometry_msgs::Point CurveCommon::CalculateDerivativeCurvePoint(Spline_Inf *spli
   return derivative_curve_point;
 }
 
-nav_msgs::Path CurveCommon::Generate_DerivativeBsplineCurve(Spline_Inf bspline_inf, int differential_times, double t_intervel, std::string frame_id)
+robot_nav_msgs::Path CurveCommon::Generate_DerivativeBsplineCurve(Spline_Inf bspline_inf, int differential_times, double t_intervel, std::string frame_id)
 {
-  geometry_msgs::Point derivative_point_result;
+  robot_geometry_msgs::Point derivative_point_result;
-  nav_msgs::Path bspline_derivative_result;
+  robot_nav_msgs::Path bspline_derivative_result;
-  geometry_msgs::PoseStamped current_pose;
+  robot_geometry_msgs::PoseStamped current_pose;
 
   bspline_derivative_result.header.frame_id = frame_id;
   bspline_derivative_result.header.stamp = robot::Time::now();
@@ -916,11 +916,11 @@ nav_msgs::Path CurveCommon::Generate_DerivativeBsplineCurve(Spline_Inf bspline_i
   return bspline_derivative_result;
 }
 
-nav_msgs::Path CurveCommon::Generate_DerivativeBasisFuncCurve(Spline_Inf bspline_inf, int differential_times, int index, double t_intervel, std::string frame_id)
+robot_nav_msgs::Path CurveCommon::Generate_DerivativeBasisFuncCurve(Spline_Inf bspline_inf, int differential_times, int index, double t_intervel, std::string frame_id)
 {
-  geometry_msgs::Point derivative_point_result;
+  robot_geometry_msgs::Point derivative_point_result;
-  nav_msgs::Path derivative_basis_result;
+  robot_nav_msgs::Path derivative_basis_result;
-  geometry_msgs::PoseStamped current_pose;
+  robot_geometry_msgs::PoseStamped current_pose;
 
   derivative_basis_result.header.frame_id = frame_id;
   derivative_basis_result.header.stamp = robot::Time::now();
@@ -1166,11 +1166,11 @@ bool CurveCommon::curveIsValid(int degree, const std::vector<double> &knot_vecto
   return true;
 }
 
-geometry_msgs::Point CurveCommon::CalculateCurvePoint(Spline_Inf *spline_inf, double u_data, bool UsingNURBS)
+robot_geometry_msgs::Point CurveCommon::CalculateCurvePoint(Spline_Inf *spline_inf, double u_data, bool UsingNURBS)
 {
   // TODO: Check u = 1 bug, why x=nan and y=nan?
 
-  geometry_msgs::Point curve_point;
+  robot_geometry_msgs::Point curve_point;
   int p_degree = spline_inf->order - 1;
   int n = spline_inf->control_point.size() - 1;
   // TODO: Check knot vector size and sequence is correect