first commit

CMakeLists.txt

@@ -0,0 +1,90 @@
+# --- CMake version và project name ---
+cmake_minimum_required(VERSION 3.10)
+project(dock_planner)
+
+# --- 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
+
+# --- 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)
+
+# --- 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
+
+# --- Include directories ---
+# Thêm các folder chứa header files
+include_directories(
+    include  
+    ${EIGEN3_INCLUDE_DIRS}
+    ${Boost_INCLUDE_DIRS}
+)
+
+# --- Eigen và PCL definitions ---
+add_definitions(${EIGEN3_DEFINITIONS})
+
+# --- Core library: dock_planner ---
+# Tạo thư viện chính
+add_library(dock_planner
+  src/dock_planner.cpp
+  src/dock_calc.cpp
+  src/utils/curve_common.cpp
+  src/utils/pose.cpp
+  src/utils/line_common.cpp
+)
+
+# --- Link các thư viện phụ thuộc ---
+target_link_libraries(dock_planner
+    ${Boost_LIBRARIES}        # Boost
+    visualization_msgs
+    nav_msgs
+    tf3
+    tf3_geometry_msgs
+    robot_time
+    data_convert
+    costmap_2d
+    nav_core
+    robot::console
+    nav_2d_utils
+)
+
+# --- 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
+        )
+
+# --- 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
+)
+
+# --- 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
+)
+
+# --- Cài đặt headers ---
+install(DIRECTORY include/${PROJECT_NAME}/
+  DESTINATION include/${PROJECT_NAME}
+)

config/dock_global_params.yaml

@@ -0,0 +1,12 @@
+base_global_planner: "DockPlanner"
+DockPlanner:
+  # File: config/planner_params.yaml
+  MyGlobalPlanner:
+    cost_threshold: 200        # Ngưỡng chi phí vật cản (0-255)
+    safety_distance: 2         # Khoảng cách an toàn (cells)
+    use_dijkstra: false        # Sử dụng Dijkstra thay vì A*
+    
+  # File: config/costmap_params.yaml
+  global_costmap:
+    inflation_radius: 0.3      # Bán kính phình vật cản
+    cost_scaling_factor: 10.0  # Hệ số tỷ lệ chi phí

include/dock_planner/dock_calc.h

@@ -0,0 +1,147 @@
+#ifndef DOCK_CALC_H
+#define DOCK_CALC_H
+
+#include <nav_2d_utils/footprint.h>
+#include <geometry_msgs/PoseStamped.h>
+
+#include <nav_msgs/Path.h>
+#include <nav_msgs/OccupancyGrid.h>
+#include <nav_msgs/GetPlan.h>
+
+#include <costmap_2d/costmap_2d_robot.h>
+#include <costmap_2d/costmap_2d.h>
+#include <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 <vector>
+
+using namespace std;
+
+namespace dock_planner
+{
+    struct RobotGeometry
+    {
+        double length;   // dọc trục X
+        double width;    // dọc trục Y
+        double radius;   // circle around base_link
+    };
+
+    class DockCalc
+    {
+    private:
+        /* data */
+        Spline_Inf* input_spline_inf;
+        CurveCommon* CurveDesign;
+
+        vector<geometry_msgs::Pose> posesOnPathWay;
+        vector<geometry_msgs::Point> footprint_robot_;
+        geometry_msgs::PoseStamped save_goal_;
+        // std::vector<geometry_msgs::Pose> free_zone_;
+
+        bool check_goal_;
+        int store_start_nearest_idx_;
+        int start_target_idx_;
+        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) 
+        {
+            return std::hypot(p1.position.x - p2.position.x, p1.position.y - p2.position.y);
+        }
+
+        inline double calculateAdaptiveStep(double segment_length, double desired_point_spacing = 0.02) 
+        {
+        return desired_point_spacing / segment_length;
+        }
+
+        inline double calculateAngle(double x1, double y1, double x2, double y2)
+        {
+            double angle = atan2(y2 - y1, x2 - x1);
+            return angle;
+        }
+
+        inline double normalizeAngle(double angle) 
+        {
+            while (angle > M_PI)
+                angle -= 2.0 * M_PI;
+            while (angle <= -M_PI)
+                angle += 2.0 * M_PI;
+            return angle;
+        }
+
+        double calculateNURBSLength(CurveCommon* curve_design, 
+                                    Spline_Inf* spline_inf, 
+                                    int degree, 
+                                    double initial_step = 0.02);
+
+        geometry_msgs::Pose offsetPoint(const 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 signedAngle(const geometry_msgs::Pose& A, const geometry_msgs::Pose& B, const geometry_msgs::Pose& C);
+
+        double compute_t(const geometry_msgs::Pose& A, const geometry_msgs::Pose& B, const geometry_msgs::Pose& C);
+
+        geometry_msgs::Pose compute_D(const geometry_msgs::Pose& B, const geometry_msgs::Pose& C,const double& t);
+
+        void updatePoseOrientation(std::vector<geometry_msgs::Pose>& saved_poses,
+                                    std::vector<geometry_msgs::Pose>& nurbs_path,
+                                    bool reverse = true);
+
+        bool findPathTMP(const vector<geometry_msgs::Pose>& control_points,
+                        vector<geometry_msgs::Pose>& saved_poses,                
+                        const int& degree, double step = 0.02);
+
+        bool findNURBSControlPoints(int& dir, vector<geometry_msgs::Pose>& control_points,
+                                    const geometry_msgs::Pose& start_pose,
+                                    const std::vector<geometry_msgs::Pose>& posesOnPathWay,
+                                    const double& angle_threshol, const int& degree);
+
+        int findNearestPointOnPath(int& dir, const geometry_msgs::Pose& pose, 
+                                const std::vector<geometry_msgs::Pose>& posesOnPathWay, 
+                                const double& angle_threshol);
+
+        bool isFreeSpace(const geometry_msgs::Pose& pose, const std::vector<geometry_msgs::Point>& footprint);
+
+        std::vector<geometry_msgs::Point> calcFreeZone(const geometry_msgs::Pose& pose, 
+                                    const std::vector<geometry_msgs::Point>& footprint);
+
+        std::vector<geometry_msgs::Point> interpolateFootprint(const geometry_msgs::Pose& pose, 
+                const std::vector<geometry_msgs::Point>& footprint, const double& resolution);
+
+        RobotGeometry computeGeometry(const std::vector<geometry_msgs::Point>& fp);
+
+
+    public:
+        //Params
+        costmap_2d::Costmap2D* costmap_;
+
+        int cost_threshold_;     // Threshold for obstacle detection
+        double safety_distance_;    // Safety distance from obstacles
+        double calib_safety_distance_;
+
+        DockCalc(/* args */);
+
+        ~DockCalc();
+
+        bool makePlanMoveToDock(const geometry_msgs::PoseStamped& start,
+                                const geometry_msgs::PoseStamped& goal,
+                                const vector<geometry_msgs::Point>& footprint_robot,
+                                const vector<geometry_msgs::Point>& footprint_dock,
+                                const int& degree, const double& step,
+                                vector<geometry_msgs::Pose>& planMoveToDock,
+                                bool reverse = false);
+        
+        
+    };
+
+}//namespace dock_planner
+
+
+#endif

include/dock_planner/dock_planner.h

@@ -0,0 +1,42 @@
+#ifndef DOCK_PLANNER_H
+#define DOCK_PLANNER_H
+
+
+
+#include "dock_planner/dock_calc.h"
+#include <nav_core/base_global_planner.h>
+
+namespace dock_planner {
+    class DockPlanner : public nav_core::BaseGlobalPlanner 
+    {
+    public:
+        DockPlanner();
+        DockPlanner(std::string name, costmap_2d::Costmap2DROBOT* costmap_robot);
+        
+        bool initialize(std::string name, costmap_2d::Costmap2DROBOT* costmap_robot);
+        
+        bool makePlan(const geometry_msgs::PoseStamped& start,
+                    const geometry_msgs::PoseStamped& goal,
+                    std::vector<geometry_msgs::PoseStamped>& plan);
+
+    private:
+        // Core components
+        costmap_2d::Costmap2DROBOT* costmap_robot_;
+        costmap_2d::Costmap2D* costmap_;
+        bool initialized_;
+        std::string frame_id_;
+        // ros::Publisher plan_pub_;
+        std::vector<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
+
+#endif

include/dock_planner/utils/color.h

@@ -0,0 +1,34 @@
+#ifndef COLOR_H_
+#define COLOR_H_
+
+#include <std_msgs/ColorRGBA.h>
+
+namespace agv_visualization
+{
+  class Color : public std_msgs::ColorRGBA
+  {
+  public:
+    Color() : 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;
+      g = green;
+      b = blue;
+      a = alpha;
+    }
+
+    static const Color White() { return Color(1.0, 1.0, 1.0); }
+    static const Color Black() { return Color(0.0, 0.0, 0.0); }
+    static const Color Gray() { return Color(0.5, 0.5, 0.5); }
+    static const Color Red() { return Color(1.0, 0.0, 0.0); }
+    static const Color Green() { return Color(0.0, 1.0, 0.0); }
+    static const Color Blue() { return Color(0.0, 0.0, 1.0); }
+    static const Color Yellow() { return Color(1.0, 1.0, 0.0); }
+    static const Color Orange() { return Color(1.0, 0.5, 0.0); }
+    static const Color Purple() { return Color(0.5, 0.0, 1.0); }
+    static const Color Chartreuse() { return Color(0.5, 1.0, 0.0); }
+    static const Color Teal() { return Color(0.0, 1.0, 1.0); }
+    static const Color Pink() { return Color(1.0, 0.0, 0.5); }
+  };
+}
+#endif //COLOR_H_

include/dock_planner/utils/common.h

@@ -0,0 +1,472 @@
+#ifndef _VDA_5050_COMMON_H_INCLUDE_
+#define _VDA_5050_COMMON_H_INCLUDE_
+#include <iostream>
+#include <vector>
+
+namespace vda_5050
+{
+  enum class OrderType
+  {
+    Base,
+    Horizon
+  };
+
+  struct NodePosition
+  {
+    double x, y, theta;
+    double allowedDeviationXY, allowedDeviationTheta;
+    std::string mapId, mapDescription;
+  };
+
+  struct ActionParameter
+  {
+    std::string key;
+    std::string value;
+  };
+
+  struct Action
+  {
+    std::string actionType;
+    std::string actionId;
+    std::string actionDescription;
+    std::string blockingType;
+    std::vector<ActionParameter> actionParameters;
+  };
+
+  struct Node
+  {
+    std::string nodeId;
+    int sequenceId;
+    std::string nodeDescription;
+    bool released;
+    NodePosition nodePosition;
+    std::vector<std::shared_ptr<Action>> actions;
+  };
+
+  struct Corridor
+  {
+    double leftWidth;
+    double rightWidth;
+    std::string corridorRefPoint;
+  };
+
+  struct ControlPoint
+  {
+    double x, y, weight;
+  };
+
+  struct Trajectory
+  {
+    int degree;
+    std::vector<double> knotVector;
+    std::vector<ControlPoint> controlPoints;
+  };
+
+  struct Edge
+  {
+    std::string edgeId;
+    int sequenceId;
+    std::string edgeDescription;
+    bool released;
+    std::string startNodeId;
+    std::string endNodeId;
+    double maxSpeed;
+    double maxHeight;
+    double minHeight;
+    double orientation;
+    std::string orientationType;
+    std::string direction;
+    bool rotationAllowed;
+    double maxRotationSpeed;
+    Trajectory trajectory;
+    double length;
+    Corridor corridor;
+    std::vector<Action> actions;
+  };
+
+  struct Order
+  {
+    int headerId;
+    std::string timestamp;
+    std::string version;
+    std::string manufacturer;
+    std::string serialNumber;
+    std::string orderId;
+    int orderUpdateId;
+    std::string zoneSetId;
+    std::vector<Node> nodes;
+    std::vector<Edge> edges;
+
+    OrderType getTypeOrder()
+    {
+      for (auto &edge : edges)
+      {
+        if (!edge.released)
+          return OrderType::Horizon;
+      }
+      for (auto &node : nodes)
+      {
+        if (!node.released)
+          return OrderType::Horizon;
+      }
+      return OrderType::Base;
+    }
+  };
+
+  struct InstantAction
+  {
+    int headerId;
+    std::string timestamp;
+    std::string version;
+    std::string manufacturer;
+    std::string serialNumber;
+    std::vector<std::shared_ptr<Action>> actions;
+  };
+
+  struct AGVPosition
+  {
+    double x;
+    double y;
+    double theta;
+    std::string mapId;
+    bool positionInitialized;
+    double localizationScore;
+    double deviationRange;
+  };
+
+  struct Velocity
+  {
+    double vx;
+    double vy;
+    double omega;
+  };
+
+  struct BatteryState
+  {
+    double batteryCharge;
+    double batteryVoltage;
+    int8_t batteryHealth;
+    bool charging;
+    uint32_t reach;
+  };
+
+  struct SafetyState
+  {
+    std::string eStop;
+    bool fieldViolation;
+    std::string AUTO_ACK = "autoAck";
+    std::string MANUAL = "manual";
+    std::string REMOTE = "remote";
+    std::string NONE = "none";
+  };
+
+  struct ErrorReference
+  {
+    std::string referenceKey;
+    std::string referenceValue;
+  };
+
+  struct Error
+  {
+    struct ErrorLevel
+    {
+      enum ErrorLevel_EN
+      {
+        WARNING,
+        FATAL
+      };
+
+      operator ErrorLevel_EN() const { return level; }
+
+      ErrorLevel &operator=(const ErrorLevel_EN &newType)
+      {
+        level = newType;
+        return *this;
+      }
+
+    private:
+      ErrorLevel_EN level;
+
+    public:
+      // Function to convert ActionStatus to string
+      std::string _toString() const
+      {
+        switch (level)
+        {
+        case WARNING:
+          return "WARNING";
+        case FATAL:
+          return "FATAL";
+        default:
+          return "";
+        }
+      }
+    };
+
+    std::string errorType;
+    std::vector<ErrorReference> errorReferences;
+    std::string errorDescription;
+    ErrorLevel errorLevel;
+  };
+
+  struct NodeState
+  {
+    std::string nodeId;
+    int32_t sequenceId;
+    std::string nodeDescription;
+    NodePosition nodePosition;
+    bool released;
+  };
+
+  struct EdgeState
+  {
+    std::string edgeId;
+    uint32_t sequenceId;
+    std::string edgeDescription;
+    bool released;
+    Trajectory trajectory;
+  };
+
+  struct ActionState
+  {
+    struct ActionStatus
+    {
+    public:
+      enum ActionStatus_EN
+      {
+        WAITING,
+        INITIALIZING,
+        RUNNING,
+        PAUSED,
+        FINISHED,
+        FAILED
+      };
+
+      operator ActionStatus_EN() const { return type; }
+
+      ActionStatus &operator=(const ActionStatus_EN &newType)
+      {
+        type = newType;
+        return *this;
+      }
+
+      bool operator==(const ActionStatus_EN &otherType) const
+      {
+        return type == otherType;
+      }
+
+    private:
+      ActionStatus_EN type;
+
+    public:
+      // Function to convert ActionStatus to string
+      std::string _toString() const
+      {
+        switch (type)
+        {
+        case WAITING:
+          return "WAITING";
+        case INITIALIZING:
+          return "INITIALIZING";
+        case RUNNING:
+          return "RUNNING";
+        case PAUSED:
+          return "PAUSED";
+        case FINISHED:
+          return "FINISHED";
+        case FAILED:
+          return "FAILED";
+        default:
+          return "";
+        }
+      }
+    };
+
+    std::string actionId;
+    std::string actionType;
+    std::string actionDescription;
+    ActionStatus actionStatus;
+    std::string resultDescription;
+  };
+
+  struct InfoReference
+  {
+    std::string referenceKey;
+    std::string referenceValue;
+  };
+
+  struct Info
+  {
+    std::string infoType;
+    std::vector<InfoReference> infoReferences;
+    std::string infoDescription;
+    std::string infoLevel;
+
+    std::string DEBUG = "DEBUG";
+    std::string INFO = "INFO";
+  };
+
+  struct BoundingBoxReference
+  {
+    double x;
+    double y;
+    double z;
+    double theta;
+  };
+
+  struct LoadDimensions
+  {
+    double length;
+    double width;
+    double height;
+  };
+
+  struct Load
+  {
+    std::string loadId;
+    std::string loadType;
+    std::string loadPosition;
+    BoundingBoxReference boundingBoxReference;
+    LoadDimensions loadDimensions;
+    uint32_t weight;
+  };
+
+  struct State
+  {
+    struct OperatingMode
+    {
+      enum OperatingMode_EN
+      {
+        AUTOMATIC,
+        SEMIAUTOMATIC,
+        MANUAL,
+        SERVICE,
+        TEACHING
+      };
+
+    private:
+      OperatingMode_EN type;
+
+    public:
+      // Constructor mặc định
+      OperatingMode() : type(SERVICE) {}
+
+      // Toán tử chuyển đổi sang enum
+      operator OperatingMode_EN() const { return type; }
+
+      OperatingMode &operator=(const OperatingMode_EN &newType)
+      {
+        type = newType;
+        return *this;
+      }
+
+      bool operator==(const OperatingMode_EN &otherType) const
+      {
+        return type == otherType;
+      }
+
+      // Chuyển đổi sang chuỗi
+      std::string _toString() const
+      {
+        switch (type)
+        {
+        case AUTOMATIC:
+          return "AUTOMATIC";
+        case SEMIAUTOMATIC:
+          return "SEMIAUTOMATIC";
+        case MANUAL:
+          return "MANUAL";
+        case SERVICE:
+          return "SERVICE";
+        case TEACHING:
+          return "TEACHING";
+        default:
+          return "SERVICE";
+        }
+      }
+    };
+
+    State()
+    {
+      headerId = 1;
+      version = "1.0.0";
+      manufacturer = "Phenikaa-X";
+      serialNumber = "";
+      velocity.vx = 0;
+      velocity.vy = 0;
+      velocity.omega = 0;
+      batteryState.batteryCharge = 100;
+      batteryState.batteryVoltage = 48;
+      batteryState.batteryHealth = 100;
+      batteryState.charging = false;
+      batteryState.reach = 0;
+      std::vector<Error> errors_;
+      errors = errors_;
+    }
+
+    uint32_t headerId;
+    std::string timestamp;
+    std::string version;
+    std::string manufacturer;
+    std::string serialNumber;
+    std::string orderId;
+    uint32_t orderUpdateId;
+    std::string zoneSetId;
+    std::string lastNodeId;
+    uint32_t lastNodeSequenceId;
+    bool driving;
+    bool paused;
+    bool newBaseRequest;
+    double distanceSinceLastNode;
+    OperatingMode operatingMode;
+    std::vector<NodeState> nodeStates;
+    std::vector<EdgeState> edgeStates;
+    std::vector<std::shared_ptr<vda_5050::ActionState>> actionStates;
+    AGVPosition agvPosition;
+    Velocity velocity;
+    std::vector<Load> loads;
+    BatteryState batteryState;
+    std::vector<Error> errors;
+    std::vector<Info> information;
+    SafetyState safetyState;
+
+    OrderType getTypeOrder()
+    {
+      for (auto &edge : edgeStates)
+      {
+        if (!edge.released)
+          return OrderType::Horizon;
+      }
+      for (auto &node : nodeStates)
+      {
+        if (!node.released)
+          return OrderType::Horizon;
+      }
+      return OrderType::Base;
+    }
+  };
+
+  struct Visualization
+  {
+    Visualization()
+    {
+      headerId = 1;
+      version = "1.0.0";
+      manufacturer = "Phenikaa-X";
+      serialNumber = "AMR2WS";
+      velocity.vx = 0;
+      velocity.vy = 0;
+      velocity.omega = 0;
+    }
+
+    uint32_t headerId;
+    std::string timestamp;
+    std::string version;
+    std::string manufacturer;
+    std::string serialNumber;
+    AGVPosition agvPosition;
+    Velocity velocity;
+  };
+}
+
+#endif // _VDA_5050_COMMON_H_INCLUDE_

include/dock_planner/utils/conversion.h

@@ -0,0 +1,43 @@
+#ifndef CONVERSION_H_
+#define CONVERSION_H_
+#include "curve_common.h"
+
+#include <Eigen/Geometry>
+#include <geometry_msgs/Point.h>
+#include <geometry_msgs/Quaternion.h>
+
+inline Eigen::Vector3d EigenVecter3dFromPointMsg(const geometry_msgs::Point& msg) {
+  return Eigen::Vector3d(msg.x, msg.y, msg.z);
+}
+
+inline EigenTrajectoryPoint::Vector EigenTrajectoryVectorFromVector(const std::vector<geometry_msgs::PoseStamped> &plan)
+{
+  EigenTrajectoryPoint::Vector eigen_trajectory_point_vec;
+  EigenTrajectoryPoint eigen_trajectory_point;
+
+  for(int i = 0; i < plan.size(); i++)
+  {
+    eigen_trajectory_point.position(0) = plan.at(i).pose.position.x;
+    eigen_trajectory_point.position(1) = plan.at(i).pose.position.y;
+    eigen_trajectory_point_vec.push_back(eigen_trajectory_point);
+  }
+
+  return eigen_trajectory_point_vec;
+}
+
+inline EigenTrajectoryPoint::Vector EigenTrajectoryVectorFromVector(const std::vector<geometry_msgs::Point> &discreate_point_vec)
+{
+  EigenTrajectoryPoint::Vector eigen_trajectory_point_vec;
+  EigenTrajectoryPoint eigen_trajectory_point;
+
+  for(int i = 0; i < discreate_point_vec.size(); i++)
+  {
+    eigen_trajectory_point.position(0) = discreate_point_vec.at(i).x;
+    eigen_trajectory_point.position(1) = discreate_point_vec.at(i).y;
+    eigen_trajectory_point_vec.push_back(eigen_trajectory_point);
+  }
+  
+  return eigen_trajectory_point_vec;
+}
+
+#endif //CONVERSION_H_

include/dock_planner/utils/curve_common.h

@@ -0,0 +1,82 @@
+#ifndef CURVE_COMMON_H_
+#define CURVE_COMMON_H_
+
+#include <Eigen/Eigen>
+#include <nav_msgs/Path.h>
+#include <geometry_msgs/Point.h>
+#include <visualization_msgs/Marker.h>
+#include "color.h"
+#include <robot/console.h>
+
+struct Spline_Inf
+{
+  int order;
+  std::vector<double> knot_vector;
+  std::vector<double> weight;
+  //std::vector<double> N;
+
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+    std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > control_point;
+  std::vector<Eigen::VectorXd, Eigen::aligned_allocator<Eigen::VectorXd> > N;
+  std::vector<Eigen::VectorXd, Eigen::aligned_allocator<Eigen::VectorXd> > dN;
+  std::vector<Eigen::VectorXd, Eigen::aligned_allocator<Eigen::VectorXd> > ddN;
+  std::vector<double> N_double_vec;
+  std::vector<double> R_double_vec;
+  std::vector<double> dR_double_vec;
+  std::vector<double> ddR_double_vec;
+  // std::vector<double> ddN_double_vec;
+  //Eigen::Matrix<Eigen::VectorXd, Eigen::Dynamic, Eigen::Dynamic> N; //bsaic function
+  Eigen::MatrixXd curvefit_N;
+
+};
+
+struct EigenTrajectoryPoint
+{
+  typedef std::vector<EigenTrajectoryPoint, Eigen::aligned_allocator<EigenTrajectoryPoint> > Vector;
+  double u_data;
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+    Eigen::Vector3d position;
+  //Eigen::VectorXd u_data;
+};
+
+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);
+  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);
+  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);
+  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);
+  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);
+  double CalculateCurvatureRadius(Spline_Inf spline_inf, double u_data, bool UsingNURBS);
+  double CalculateCurveLength(Spline_Inf spline_inf, double start_u, double end_u, int sub_intervals, bool UsingNURBS);
+  bool curveIsValid(int degree, const std::vector<double>& knot_vector,
+    std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>>& control_points);
+
+  void ReadSplineInf(Spline_Inf* bspline_inf, int order, std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > control_point, std::vector<double> knot_vector);
+  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(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);
+  visualization_msgs::Marker ShowDiscreatePoint2(EigenTrajectoryPoint::Vector& discreate_point, const std::string& frame_id, 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);
+  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);
+private:
+
+
+};
+
+#endif //CURVE_COMMON_H_

include/dock_planner/utils/line_common.h

@@ -0,0 +1,13 @@
+#ifndef __LINE_COMMON_H_
+#define __LINE_COMMON_H_
+
+#include <cmath>
+
+namespace vda_5050_utils
+{
+  double distanceFromPointToLine(double x0, double y0, double x1, double y1, double x2, double y2);
+
+  bool isPointOnSegment(double x0, double y0, double x1, double y1, double x2, double y2, const double tolerance);
+}
+
+#endif

include/dock_planner/utils/pose.h

@@ -0,0 +1,66 @@
+#ifndef AMR_CONTROL_POSE_H
+#define AMR_CONTROL_POSE_H
+#include <stdio.h>
+#include <iostream>
+#include <stdlib.h>
+#include <unistd.h>
+#include <vector>
+#include <fstream>
+#include <math.h>
+#include <cmath>
+
+namespace vda_5050
+{
+  class Pose {
+  private:
+    double x_, y_, yaw_;
+    double v_max_, accuracy_;
+
+    void modifyYaw(void) {
+      while (yaw_ < -M_PI)
+        yaw_ += 2.0 * M_PI;
+      while (yaw_ > M_PI)
+        yaw_ -= 2.0 * M_PI;
+    }
+
+  public:
+    Pose() :
+      x_(0.0), y_(0.0), yaw_(0.0), v_max_(0.0), accuracy_(0.0) {
+    };
+
+    Pose(double x, double y, double yaw) :
+      x_(x), y_(y), yaw_(yaw), v_max_(0.0), accuracy_(0.0) {
+    };
+
+    Pose(double x, double y, double yaw, double v_max, double accuracy) :
+      x_(x), y_(y), yaw_(yaw), v_max_(v_max), accuracy_(accuracy) {
+    };
+
+    ~Pose() {};
+
+    inline void setX(double x) { x_ = x; }
+    inline void setY(double y) { y_ = y; }
+    inline void setYaw(double yaw) { yaw_ = yaw, modifyYaw(); }
+    inline void setVmax(double v_max) { v_max_ = v_max; }
+    inline void setAccuracy(double accuracy) { accuracy_ = accuracy; }
+    inline void setPose(double x, double y, double yaw) { x_ = x, y_ = y, yaw_ = yaw, modifyYaw(); }
+    inline void setPose(double x, double y, double yaw, double v_max, double accuracy)
+    {
+      x_ = x, y_ = y, yaw_ = yaw, v_max_ = v_max, accuracy_ = accuracy, modifyYaw();
+    }
+    inline void setPose(Pose p) { x_ = p.x_, y_ = p.y_, yaw_ = p.yaw_, modifyYaw(); }
+
+    inline double getX(void) { return x_; }
+    inline double getY(void) { return y_; }
+    inline double getYaw(void) { return yaw_; }
+    inline double getVmax(void) { return v_max_; }
+    inline double getAccuracy(void) { return accuracy_; }
+    inline Pose getPose(void) { return Pose(x_, y_, yaw_, v_max_, accuracy_); }
+    bool SavePoseAsFile(const std::string& file_name);
+    bool LoadPoseFromFile(const std::string& file_name);
+
+  }; // class Pose
+
+}
+
+#endif

src/dock_calc.cpp

@@ -0,0 +1,811 @@
+#include "dock_planner/dock_calc.h"
+
+namespace dock_planner
+{
+    DockCalc::DockCalc(/* args */) :costmap_(nullptr)
+    {
+        check_goal_ = false;
+        // cost_threshold_ = 200;     // Threshold for obstacle detection
+        safety_distance_ = 0;    // Safety distance from obstacles
+        calib_safety_distance_ = 0;
+        input_spline_inf = new Spline_Inf();
+        CurveDesign = new CurveCommon();
+    }
+    
+    DockCalc::~DockCalc()
+    {
+        delete CurveDesign;
+        delete input_spline_inf;
+    }
+
+    geometry_msgs::Pose DockCalc::offsetPoint(const geometry_msgs::Pose& A, const double& theta, const double& d) 
+    {
+        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 theta = signedAngle(A,B,C);
+       double sin_theta = sin(fabs(theta));
+       double h = calc_distance(A,B);
+       double distance = sin_theta * h;
+       return distance;
+    }
+
+    double DockCalc::signedAngle(const geometry_msgs::Pose& A, const geometry_msgs::Pose& B, const geometry_msgs::Pose& C)
+    {
+        double ABx = B.position.x - A.position.x;
+        double ABy = B.position.y - A.position.y;
+
+        double CBx = B.position.x - C.position.x;
+        double CBy = B.position.y - C.position.y;
+
+        double cross = ABx*CBy - ABy*CBx;
+        double dot   = ABx*CBx + ABy*CBy;
+
+        double angle = atan2(cross,dot);
+        return angle;
+    }
+
+    double DockCalc::compute_t(const geometry_msgs::Pose& A, const geometry_msgs::Pose& B, const geometry_msgs::Pose& C) 
+    {
+        constexpr double epsilon = 1e-6;
+        double dx = C.position.x - B.position.x;
+        double dy = C.position.y - B.position.y;
+        double bx = B.position.x - A.position.x;
+        double by = B.position.y - A.position.y;
+
+        double numerator = dx * bx + dy * by;
+        double denominator = dx * dx + dy * dy;
+
+        if (denominator <= epsilon)
+        {
+            throw std::runtime_error("B and C are the same!");
+        }
+
+        return -numerator / denominator;
+    }
+
+    geometry_msgs::Pose DockCalc::compute_D(const geometry_msgs::Pose& B, const geometry_msgs::Pose& C, const double& t) 
+    {
+        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)
+    {
+        double min_x =  1e9, max_x = -1e9;
+        double min_y =  1e9, max_y = -1e9;
+        double radius = 0.0;
+        if((int)fp.size() >= 4)
+        {
+            for (const auto& p : fp)
+            {
+                min_x  = std::min(min_x, p.x);
+                max_x  = std::max(max_x, p.x);
+                min_y  = std::min(min_y, p.y);
+                max_y  = std::max(max_y, p.y);
+                radius = std::max(radius, std::hypot(p.x, p.y));
+            }
+        }
+        else return {0, 0, 0};              
+        double length = std::max(max_x - min_x, max_y - min_y);
+        double width = std::min(max_x - min_x, max_y - min_y);
+        return {length, width, radius};           
+    }
+
+    bool DockCalc::makePlanMoveToDock(const geometry_msgs::PoseStamped& start,
+                                    const geometry_msgs::PoseStamped& goal,
+                                    const vector<geometry_msgs::Point>& footprint_robot,
+                                    const vector<geometry_msgs::Point>& footprint_dock,
+                                    const int& degree, const double& step,
+                                    vector<geometry_msgs::Pose>& planMoveToDock,
+                                    bool reverse)
+    {
+        constexpr double epsilon = 1e-6;
+        robot::log_error("[dock_calc] makePlanMoveToDock start");
+        planMoveToDock.clear();
+
+        if(save_goal_.pose.position.x != goal.pose.position.x ||
+            save_goal_.pose.position.y != goal.pose.position.x ||
+            data_convert::getYaw(save_goal_.pose.orientation) !=  data_convert::getYaw(goal.pose.orientation))
+        {
+            // robot::log_error("[dock_calc] DEBUG 1000");
+            check_goal_ = true;
+            save_goal_ == goal;
+        }
+
+        footprint_robot_ = footprint_robot;
+        // Calculate footprint distances
+        RobotGeometry robot = computeGeometry(footprint_robot);
+        RobotGeometry dock = computeGeometry(footprint_dock);
+        double distance_robot = robot.length;
+        double distance_dock = dock.length;
+        if (distance_robot <= epsilon)
+        {
+            robot::log_error("[dock_calc] Robot geometry is invalid, cannot calculate plan to dock.");
+            return false;
+        }
+
+        // Calculate safety distance
+        safety_distance_ = (robot.radius + calib_safety_distance_) * 2.0;
+        if (safety_distance_ <= epsilon) 
+        {
+            robot::log_error("[dock_calc] Safety distance is zero, cannot calculate plan to dock.");
+            return false;
+        }
+
+        // Lambda: Generate NURBS path
+        auto generateNURBSPath = [&](const vector<geometry_msgs::Pose>& control_points, bool check_isFreeSpace, 
+                                    bool edge_end_plag, bool path_reverse) -> vector<geometry_msgs::Pose> 
+        {
+            vector<geometry_msgs::Pose> result;
+            bool obstacle_flag = false;
+
+            // Convert to Eigen format
+            std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>> eigen_points;
+            for (const auto& cp : control_points) 
+            {
+                eigen_points.emplace_back(cp.position.x, cp.position.y, 0);
+            }
+
+            // Create knot vector
+            std::vector<double> knot_vector;
+            int n = control_points.size() - 1;
+            int m = n + degree + 1;
+            for (int i = 0; i <= m; i++) 
+            {
+                knot_vector.push_back(i <= degree ? 0.0 : 1.0);
+            }
+
+            // Setup spline
+            std::vector<double> weights(control_points.size(), 1.0);
+            input_spline_inf->knot_vector.clear();
+            input_spline_inf->control_point.clear();
+            input_spline_inf->weight.clear();
+
+            CurveDesign->ReadSplineInf(input_spline_inf, degree + 1, eigen_points, knot_vector);
+            CurveDesign->ReadSplineInf(input_spline_inf, weights, false);
+
+            // Calculate adaptive step
+            double edge_length = calculateNURBSLength(CurveDesign, input_spline_inf, degree, step);
+            double resolution = costmap_->getResolution();
+            double step_new = calculateAdaptiveStep(edge_length, resolution);
+
+            // robot::log_error("[dock_calc] Adaptive step size: %f", step_new);
+
+            // Generate path points
+            vector<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);
+
+                if (!std::isnan(point.x) && !std::isnan(point.y))
+                {
+                    geometry_msgs::Pose pose;
+                    pose.position = point;
+                    pose.orientation = data_convert::createQuaternionMsgFromYaw(0.0);
+
+                    bool status_isFreeSpace = true;
+                    if(edge_end_plag)
+                    {
+                        double distance_to_goal = calc_distance(pose, goal.pose);
+                        if(distance_to_goal >= min_distance_to_goal_)
+                        {
+                            if(check_isFreeSpace) status_isFreeSpace = isFreeSpace(pose, footprint_robot_);
+                        }
+                    }
+                    else 
+                    {
+                        if(check_isFreeSpace) status_isFreeSpace = isFreeSpace(pose, footprint_robot_);
+                    }
+
+                    if (status_isFreeSpace && t < 1.0)
+                    {
+                        saved_poses.push_back(pose);
+                        if (saved_poses.size() > 1) 
+                        {
+                            updatePoseOrientation(saved_poses, result, path_reverse);
+                        }
+                    }
+                    if(!status_isFreeSpace)
+                    {
+                        obstacle_flag = true;
+                        break;
+                    }
+                }
+            }
+
+            // Set final orientation
+            if (!saved_poses.empty()) 
+            {
+                double end_yaw = path_reverse ? 
+                calculateAngle(control_points.back().position.x, control_points.back().position.y,
+                    saved_poses.back().position.x, saved_poses.back().position.y) :
+                calculateAngle(saved_poses.back().position.x, saved_poses.back().position.y,
+                    control_points.back().position.x, control_points.back().position.y);
+
+                tf3::Quaternion q;
+                q.setRPY(0, 0, end_yaw);
+                saved_poses.back().orientation.x = q.x();
+                saved_poses.back().orientation.y = q.y();
+                saved_poses.back().orientation.z = q.z();
+                saved_poses.back().orientation.w = q.w();
+
+                result.push_back(saved_poses.back());
+            }
+            if(obstacle_flag)
+            {
+                robot::log_error("[dock_calc] Obstacle detected, path generation interrupted.");
+                result.clear();
+            }
+            if (result.empty()) 
+            {
+                robot::log_error("[dock_calc] No valid path generated.");
+            }
+            else 
+            {
+                robot::log_info("[dock_calc] Path generated with %zu points.", result.size());
+            }
+            return result;
+        };
+
+        // Calculate minimum distance for pose C
+        // Can safety_distance_ replace distance_robot
+        double distance_min_for_pose_C = (distance_robot + distance_dock) / 2.0;
+        min_distance_to_goal_ = (safety_distance_ + distance_dock / 2.0);
+
+        // Calculate pose C (offset from goal)
+        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);
+
+        // Compute t parameter and determine direction
+        double t = compute_t(start.pose, goal.pose, Pose_C);
+        /**
+         * dir_robot_move_to_goal = false robot move backward
+         * dir_robot_move_to_goal = true robot move forward
+         */
+        bool dir_robot_move_to_goal = true;
+
+        if (t < 0) 
+        {
+            yaw_goal = (yaw_goal > 0) ? yaw_goal - M_PI : yaw_goal + M_PI;
+            Pose_C = offsetPoint(Goal_Pose, yaw_goal, distance_min_for_pose_C);
+            t = compute_t(start.pose, goal.pose, Pose_C);
+            dir_robot_move_to_goal = false;
+        }
+
+        if (t <= 1) 
+        {
+            robot::log_error("[dock_calc] t(%f) <= 1, cannot calculate plan to dock.", t);
+            return false;
+        }
+
+        // Calculate pose D and distance AD
+        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
+        const double MAX_DISTANCE = 1.0, MIN_DISTANCE = 0.1;
+        const double MIN_SCALE = 0.2, MAX_SCALE = 1.0;
+        const double calib_factor_alpha = 0.5;
+        const double calib_factor_beta = 0.5;
+        const double calib_factor_gamma = 0.77;
+
+        double scale = MIN_SCALE + (MAX_SCALE - MIN_SCALE) * 
+                    (MAX_DISTANCE - distance_AD) / (MAX_DISTANCE - MIN_DISTANCE);
+        double angle_threshold = std::min((calib_factor_alpha + fabs(scale)) * calib_factor_beta * M_PI, calib_factor_gamma * M_PI);
+
+        // Generate temporary path
+        double distance_CD = calc_distance(Pose_C, Pose_D);
+        double angle_alpha = (M_PI - angle_threshold) / 2.0;
+        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);
+        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<geometry_msgs::Pose> saved_poses_tmp;
+
+
+        if (!findPathTMP(control_points_tmp, saved_poses_tmp, degree, step)) 
+        {
+            robot::log_error("[dock_calc] Failed to find path with temporary control points.");
+            return false;
+        }
+
+        // Find first segment control points
+        int dir;
+        vector<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.");
+            return false;
+        }
+
+        // Determine reverse direction for first segment
+        bool first_segment_reverse = (dir == -1 && !dir_robot_move_to_goal) || 
+                                    ((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);
+        planMoveToDock.insert(planMoveToDock.end(), first_segment.begin(), first_segment.end());
+
+        //Generate second segment (to goal)
+        if (!planMoveToDock.empty()) 
+        {
+            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(), 
+                                                            mid_control_point, 
+                                                            goal.pose};
+
+            vector<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);
+        }        
+
+        return !planMoveToDock.empty();
+    }
+
+    bool DockCalc::findPathTMP(const vector<geometry_msgs::Pose>& control_points,
+                                vector<geometry_msgs::Pose>& saved_poses,                
+                                const int& degree, double step)
+    {
+        // Chuyển đổi control points sang định dạng Eigen::Vector3d
+        std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>> eigen_control_points;
+        for(const auto& cp : control_points)
+        {
+            Eigen::Vector3d point(cp.position.x, cp.position.y, 0);
+            eigen_control_points.push_back(point);
+        }
+        // Tạo knot vector tự động
+        std::vector<double> knot_vector;
+        int n = control_points.size() - 1; // n + 1 control points
+        int m = n + degree + 1; // m + 1 knots
+        // Tạo uniform knot vector
+        for(int i = 0; i <= m; i++) 
+        {
+            if(i <= degree) knot_vector.push_back(0.0);
+            else knot_vector.push_back(1.0);
+        }
+
+        // Điều chỉnh weights để tăng ảnh hưởng của các control points
+        std::vector<double> weights(control_points.size(), 1.0);
+
+        input_spline_inf->knot_vector.clear();
+        input_spline_inf->control_point.clear();
+        input_spline_inf->weight.clear();
+
+        // Cài đặt thông số cho spline
+        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;
+        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);
+            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;
+                pose.position = point;
+                pose.orientation.x = 0.0;
+                pose.orientation.y = 0.0; 
+                pose.orientation.z = 0.0;
+                pose.orientation.w = 1.0;
+                
+                bool status_isFreeSpace = isFreeSpace(pose, footprint_robot_);
+                if(status_isFreeSpace)
+                {
+                    saved_poses.push_back(pose);
+                }
+                else
+                {
+                    robot::log_error("[dock_calc][findPathTMP] Obstacle detected at t(%f)", t);
+                    break;
+                }
+            }
+            else 
+            {
+                robot::log_error("[dock_calc][findPathTMP] NaN point at t(%f)", t);
+            }
+        }
+        if(saved_poses.empty()) 
+        {
+            robot::log_error("[dock_calc][findPathTMP] No valid poses generated.");
+            return false;
+        }
+        return true;
+    }
+
+    void DockCalc::updatePoseOrientation(std::vector<geometry_msgs::Pose>& saved_poses,
+                                        std::vector<geometry_msgs::Pose>& nurbs_path,
+                                        bool reverse) 
+    {
+        double yaw;
+        // robot::log_error("[merge_path_calc] DEBUG: reverse(%x)", reverse);
+        if(!reverse)
+        yaw = calculateAngle(saved_poses[saved_poses.size() - 2].position.x,
+                                saved_poses[saved_poses.size() - 2].position.y,
+                                saved_poses.back().position.x,
+                                saved_poses.back().position.y);
+        else
+         yaw = calculateAngle(saved_poses.back().position.x,
+                                    saved_poses.back().position.y,
+                                    saved_poses[saved_poses.size() - 2].position.x,
+                                    saved_poses[saved_poses.size() - 2].position.y);
+
+        tf3::Quaternion q;
+        q.setRPY(0, 0, yaw);
+        saved_poses[saved_poses.size() - 2].orientation.x = q.x();
+        saved_poses[saved_poses.size() - 2].orientation.y = q.y();
+        saved_poses[saved_poses.size() - 2].orientation.z = q.z();
+        saved_poses[saved_poses.size() - 2].orientation.w = q.w();
+
+        nurbs_path.push_back(saved_poses[saved_poses.size() - 2]);
+    }
+
+    double DockCalc::calculateNURBSLength(CurveCommon* curve_design, 
+                                        Spline_Inf* spline_inf, 
+                                        int degree, 
+                                        double initial_step) 
+    {
+        double length = 0.0;
+        geometry_msgs::Point prev_point, curr_point;
+        std::vector<double> segment_lengths;
+
+        static double step;
+        // Đường cong bậc 1 (đường thẳng) thì bước nhảy bằng 0.1
+        if(degree == 1) step = 0.1;
+        // Đường cong bậc 2 trở lên thì bước nhảy bằng initial_step
+        else step = initial_step;
+
+        // Tính độ dài với step
+        for(double u = 0; u <= 1; u += step) 
+        {
+            // Lấy điểm trên đường cong tại tham số u
+            curr_point = curve_design->CalculateCurvePoint(spline_inf, u, true);
+
+            if(u > 0 && !std::isnan(curr_point.x) && !std::isnan(curr_point.y)) 
+            {
+                double segment_length = std::sqrt(std::pow(curr_point.x - prev_point.x, 2) + 
+                                                std::pow(curr_point.y - prev_point.y, 2));
+                length += segment_length;
+                segment_lengths.push_back(segment_length);
+            }
+            prev_point = curr_point;
+        }
+        return length;
+    }
+
+    bool DockCalc::findNURBSControlPoints(int& dir, vector<geometry_msgs::Pose>& control_points,
+                                        const geometry_msgs::Pose& start_pose,
+                                        const std::vector<geometry_msgs::Pose>& saved_poses,
+                                        const double& angle_threshold, const int& degree)
+    {
+        if (saved_poses.empty()) 
+        {
+            robot::log_error("[dock_calc] No saved poses available to find NURBS control points.");
+            return false;
+        }
+
+        control_points.clear();
+        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 
+        {
+            geometry_msgs::Pose pose;
+            pose.position.x = x;
+            pose.position.y = y;
+            pose.orientation = data_convert::createQuaternionMsgFromYaw(0);
+            return pose;
+        };
+
+        if (idx != -1) 
+        {
+            start_target_idx_ = idx;
+
+            // Add start and middle poses
+            control_points.push_back(start_pose);
+            control_points.push_back(createPose(saved_poses[idx].position.x, saved_poses[idx].position.y));
+
+            // Calculate distance from start to middle point
+            double dx = start_pose.position.x - saved_poses[idx].position.x;
+            double dy = start_pose.position.y - saved_poses[idx].position.y;
+            double target_distance = std::hypot(dx, dy);
+
+            if (dir == 1) 
+            {
+                // Find appropriate end point going backwards
+                int end_idx = idx;
+                const auto& mid_pose = control_points[1];
+
+                for (int i = idx; i >= 0; i--) 
+                {
+                    double dx_me = mid_pose.position.x - saved_poses[i].position.x;
+                    double dy_me = mid_pose.position.y - saved_poses[i].position.y;
+                    double dist_me = std::hypot(dx_me, dy_me);
+
+                    if (dist_me >= target_distance || i == 0) 
+                    {
+                        end_idx = i;
+                        break;
+                    }
+                }
+
+                control_points.push_back(createPose(saved_poses[end_idx].position.x, saved_poses[end_idx].position.y));
+                start_target_idx_ = end_idx;
+            } 
+            else if (dir == -1) 
+            {
+                // robot::log_error("[dock_calc] DEBUG 200:");
+                // Use last pose as end point
+                const auto& last_pose = saved_poses.back();
+                control_points.push_back(createPose(last_pose.position.x, last_pose.position.y));
+                start_target_idx_ = saved_poses.size() - 1;
+            }
+
+        } 
+        else 
+        {
+            // robot::log_error("[dock_calc] DEBUG 300:");
+            // Create midpoint between start and nearest saved pose
+            const auto& nearest_pose = saved_poses[store_start_nearest_idx_];
+            auto mid_pose = createPose(
+            (start_pose.position.x + nearest_pose.position.x) / 2.0,
+            (start_pose.position.y + nearest_pose.position.y) / 2.0
+            );
+
+            control_points.push_back(start_pose);
+            control_points.push_back(mid_pose);
+            control_points.push_back(nearest_pose);
+            // robot::log_error("[dock_calc] DEBUG 1: start_pose(%f,%f)",start_pose.position.x, start_pose.position.y);
+            // robot::log_error("[dock_calc] DEBUG 1: mid_pose(%f,%f)",mid_pose.position.x, mid_pose.position.y);
+            // robot::log_error("[dock_calc] DEBUG 1: nearest_pose(%f,%f)",nearest_pose.position.x, nearest_pose.position.y);
+
+            start_target_idx_ = store_start_nearest_idx_;
+        }
+
+        return true;
+    }
+
+    int DockCalc::findNearestPointOnPath(int& dir, const geometry_msgs::Pose& pose, 
+                                        const std::vector<geometry_msgs::Pose>& saved_poses, 
+                                        const double& angle_threshol)
+    {
+        // Tìm điểm gần nhất
+        int nearest_idx = 0;
+        double min_dist = std::numeric_limits<double>::max();
+        for (int i = 0; i < (int)saved_poses.size(); ++i) 
+        {
+            double distance = std::hypot(saved_poses[i].position.x - pose.position.x, saved_poses[i].position.y - pose.position.y);
+
+            if (distance < min_dist) 
+            {
+                min_dist = distance;
+                nearest_idx = i;
+            }
+        }
+
+        // Lưu chỉ mục của điểm gần nhất
+        store_start_nearest_idx_ = nearest_idx;
+        bool found_pose_flag_1 = false;
+        bool found_pose_flag_2 = false;
+
+        for(int i = store_start_nearest_idx_; i >= 0; i--)
+        {
+            double angle = signedAngle(pose, saved_poses[i], saved_poses[i - 1]);
+
+            if(fabs(angle) >= fabs(angle_threshol))
+            {
+                nearest_idx = i;
+                found_pose_flag_1 = true;
+                break;
+            }
+        }
+
+        double distance_1 = calc_distance(pose,saved_poses[nearest_idx]);
+        double distance_2 = calc_distance(saved_poses[0],saved_poses[nearest_idx]);
+        if(found_pose_flag_1 && (distance_2 >= distance_1))
+        {
+            found_pose_flag_2 = true;
+            dir = 1;
+        }
+        
+        if(!found_pose_flag_2)
+        {
+            for(int i = store_start_nearest_idx_; i < (int)saved_poses.size(); i++)
+            {
+                double angle = signedAngle(pose, saved_poses[i], saved_poses[i + 1]);
+
+                if(fabs(angle) >= (fabs(angle_threshol) - 0.05))
+                {
+                    nearest_idx = i;
+                    dir = -1;
+                    break;
+                }
+                if(i == (int)saved_poses.size() - 1)
+                {
+                    dir = 0;
+                    return -1;
+                }
+            }
+            double distance_1 = calc_distance(pose,saved_poses[nearest_idx]);
+            double distance_2 = calc_distance(saved_poses[nearest_idx],saved_poses.back());
+            if(distance_2 + 0.1 < distance_1)
+            {
+                dir = 0;
+                return -1;
+            }
+        }
+        
+        return nearest_idx;
+    }
+
+    bool DockCalc::isFreeSpace(const geometry_msgs::Pose& pose, 
+                            const std::vector<geometry_msgs::Point>& footprint) 
+    {
+        // static std::vector<geometry_msgs::Pose> free_zone;
+
+        // auto makePose = [](double x, double y, double yaw) 
+        // {
+        //     geometry_msgs::Pose p;
+        //     p.position.x = x;
+        //     p.position.y = y;
+        //     p.position.z = 0.0;
+
+        //     tf3::Quaternion q;
+        //     q.setRPY(0.0, 0.0, yaw);
+        //     p.orientation.x = q.x();
+        //     p.orientation.y = q.y();
+        //     p.orientation.z = q.z();
+        //     p.orientation.w = q.w();
+        //     return p;
+        // };
+        // unsigned int mx, my;
+
+        // if (!costmap_->worldToMap(pose.position.x, pose.position.y, mx, my)) 
+        // {
+        //     // robot::log_error("[dock_calc][isFreeSpace]Debug 1");
+        //     return false; // pose nằm ngoài bản đồ
+        // }
+
+        // unsigned char center_cost = costmap_->getCost(mx, my);
+        // if (center_cost >= cost_threshold_)
+        // {
+        //     // robot::log_error("[dock_calc][isFreeSpace]Debug 2");
+        //     return false; // chính giữa đã bị chiếm
+        // }
+
+        // double resolution = costmap_->getResolution();
+
+        // if(check_goal_)
+        // {
+        //     std::vector<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);
+        // for (int i = 0; i < (int)space_footprint.size(); i++) 
+        // {
+        //     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;
+        //     double t_L = compute_t(Pose_on_footprint, free_zone[0], free_zone[1]);
+        //     double t_W = compute_t(Pose_on_footprint, free_zone[1], free_zone[2]);
+        //     if(t_L >= 0 && t_L <= 1 && t_W >= 0 && t_W <= 1)
+        //     {
+        //         return true;
+        //     }
+        //     if (!costmap_->worldToMap(space_footprint[i].x, space_footprint[i].y, mx, my))
+        //     {
+        //         // robot::log_error("[dock_calc][isFreeSpace]Debug 3");
+        //         return false; // pose nằm ngoài bản đồ
+        //     }
+        //     int check_x = mx;
+        //     int check_y = my;
+
+        //     if (check_x >= 0 && check_x < costmap_->getSizeInCellsX() && 
+        //         check_y >= 0 && check_y < costmap_->getSizeInCellsY()) 
+        //     {
+        //         unsigned char cost = costmap_->getCost(check_x, check_y);
+        //         if (cost >= costmap_2d::LETHAL_OBSTACLE) 
+        //         {
+        //             double dx = pose.position.x - space_footprint[i].x;
+        //             double dy = pose.position.y - space_footprint[i].y;
+        //             double dist = std::hypot(dx, dy);
+        //             if (dist <= (safety_distance_/2.0)) 
+        //             {
+        //                 // robot::log_error("[dock_calc][isFreeSpace]Debug dist: %f", dist);
+        //                 return false;
+        //             }
+        //         }
+        //     }
+        // }
+
+        return true;
+    }
+
+    std::vector<geometry_msgs::Point> DockCalc::calcFreeZone(const geometry_msgs::Pose& pose, 
+                                        const std::vector<geometry_msgs::Point>& footprint)
+    {
+        // Dịch sang tọa độ tuyệt đối
+        std::vector<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)
+        {
+            pt.x *= 1.2;
+            pt.y *= 1.2;
+
+            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);
+        }
+        return free_zone;
+    }
+
+    std::vector<geometry_msgs::Point> DockCalc::interpolateFootprint(const geometry_msgs::Pose& pose, 
+                        const std::vector<geometry_msgs::Point>& footprint, const double& resolution)
+    {
+
+    // Dịch sang tọa độ tuyệt đối
+    std::vector<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;
+        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;
+    for (size_t i = 0; i < (int)abs_footprint.size(); ++i)
+    {
+        const geometry_msgs::Point &start = abs_footprint[i];
+        const geometry_msgs::Point &end = abs_footprint[(i + 1) % abs_footprint.size()];
+
+        double dx = end.x - start.x;
+        double dy = end.y - start.y;
+        double distance = std::hypot(dx, dy);
+        int steps = std::max(1, static_cast<int>(std::floor(distance / resolution)));
+
+        for (int j = 0; j <= steps; ++j)
+        {
+        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;
+        pt.x = start.x + t * dx;
+        pt.y = start.y + t * dy;
+        points.push_back(pt);
+        }
+    }
+    return points;
+    }
+
+}

src/dock_planner.cpp

@@ -0,0 +1,124 @@
+#include "dock_planner/dock_planner.h"
+#include <cmath>
+#include <algorithm>
+
+namespace dock_planner 
+{
+    /**
+     * @brief Constructor mặc định
+     * Khởi tạo tất cả các pointer về nullptr và trạng thái chưa initialized
+     */
+    DockPlanner::DockPlanner() : costmap_robot_(nullptr), 
+                                costmap_(nullptr), 
+                                initialized_(false), 
+                                check_free_space_(false),
+                                cost_threshold_(200),
+                                calib_safety_distance_(0.05) {}
+
+    /**
+     * @brief Constructor với parameters
+     * @param name Tên của 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) 
+        : costmap_robot_(nullptr), 
+        costmap_(nullptr), 
+        initialized_(false), 
+        check_free_space_(false),
+        cost_threshold_(200),
+        calib_safety_distance_(0.05)
+    {
+        initialize(name, costmap_robot);
+    }
+
+    /**
+     * @brief Khởi tạo global planner
+     * @param name Tên của planner instance
+     * @param costmap_robot Pointer tới costmap ROS wrapper
+     * 
+     * Thiết lập:
+     * - Liên kết với costmap
+     * - Tạo publisher cho visualization
+     * - 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) 
+    {
+        if (!initialized_) 
+        {
+            costmap_robot_ = costmap_robot;
+            costmap_ = costmap_robot_->getCostmap();
+            frame_id_ = costmap_robot_->getGlobalFrameID();
+            calc_plan_to_dock_.costmap_ = costmap_robot_->getCostmap();
+            
+            robot::NodeHandle private_nh("~/" + name);
+
+            // Load parameters from the private namespace
+            private_nh.getParam("check_free_space", check_free_space_);
+            private_nh.getParam("cost_threshold", cost_threshold_);
+            private_nh.getParam("calib_safety_distance", calib_safety_distance_);
+
+            calc_plan_to_dock_.cost_threshold_ = cost_threshold_;
+            calc_plan_to_dock_.calib_safety_distance_ = calib_safety_distance_;
+
+
+            // plan_pub_ = private_nh.advertise<nav_msgs::Path>("plan", 1);
+
+            nav_2d_msgs::Polygon2D footprint_dock ;//= nav_2d_utils::footprintFromParams(private_nh,false);
+            
+            for(auto pt : footprint_dock.points)
+            {
+                geometry_msgs::Point footprint_point;
+                footprint_point.x = pt.x;
+                footprint_point.y = pt.y;
+                footprint_point.z = 0.0;
+                footprint_dock_.push_back(footprint_point);
+            }
+            
+            initialized_ = true;
+            // ROS_WARN("[dock_planner] check_free_space_(%d)", check_free_space_);
+        }
+    }
+    
+    bool DockPlanner::makePlan(const geometry_msgs::PoseStamped& start,
+                                const geometry_msgs::PoseStamped& goal,
+                                std::vector<geometry_msgs::PoseStamped>& plan)
+    {
+        if(!initialized_) return false;
+        if(!plan.empty()) plan.clear();
+        vector<geometry_msgs::Pose> planMoveToDock;
+        std::vector<geometry_msgs::Point> footprint_robot = costmap_robot_->getRobotFootprint();
+
+        // footprint_dock_ = costmap_robot_->getRobotFootprint();
+        int degree = 2;
+        double step = 0.02;
+        bool status_makePlanMoveToDock = calc_plan_to_dock_.makePlanMoveToDock(start, goal, footprint_robot, footprint_dock_, degree, step, planMoveToDock, true);
+        robot::Time plan_time = robot::Time::now();
+
+        if(!status_makePlanMoveToDock) return false;
+        for(int i = 0; i < (int)planMoveToDock.size(); i++)
+        {
+          geometry_msgs::PoseStamped pose;
+          pose.header.stamp = plan_time;
+          pose.header.frame_id = costmap_robot_->getGlobalFrameID();
+          pose.pose.position.x = planMoveToDock[i].position.x;
+          pose.pose.position.y = planMoveToDock[i].position.y;
+          pose.pose.position.z = 0;
+          pose.pose.orientation = planMoveToDock[i].orientation;
+          plan.push_back(pose);
+        }
+        publishPlan(plan);
+        return true;
+    }
+
+    // void DockPlanner::publishPlan(const std::vector<geometry_msgs::PoseStamped>& plan) 
+    // {
+    //     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);
+    // }
+
+} // namespace dock_planner

src/utils/line_common.cpp

@@ -0,0 +1,26 @@
+#include "dock_planner/utils/line_common.h"
+
+double vda_5050_utils::distanceFromPointToLine(double x0, double y0, double x1, double y1, double x2, double y2)
+{
+  double A = y2 - y1;
+  double B = -(x2 - x1);
+  double C = -A * x1 - B * y1;
+
+  return fabs(A * x0 + B * y0 + C) / sqrt(A * A + B * B);
+}
+
+bool vda_5050_utils::isPointOnSegment(double x0, double y0, double x1, double y1, double x2, double y2, const double tolerance)
+{
+  double crossProduct = distanceFromPointToLine(x0,  y0, x1, y1, x2, y2);
+  if (fabs(crossProduct) > fabs(tolerance)) 
+    return false;
+
+  // Kiểm tra tọa độ của P có nằm giữa A và B không
+  if (x0 >= std::min(x1, x2) && x0 <= std::max(x1, x2) &&
+      y0 >= std::min(y1, y2) && y0 <= std::max(y1, y2))
+  {
+    return true;
+  }
+
+  return false;
+}

src/utils/pose.cpp

@@ -0,0 +1,39 @@
+#include "dock_planner/utils/pose.h"
+
+
+bool vda_5050::Pose::SavePoseAsFile(const std::string& file_name)
+{
+  bool result = false;
+  std::ofstream file(file_name);
+  if (file.is_open())
+  {
+    file << x_ << " " << y_ << " " << yaw_ << "\n";
+    file.close();
+    result = true;
+  }
+  else
+  {
+    result = false;
+  }
+  return result;
+}
+
+bool vda_5050::Pose::LoadPoseFromFile(const std::string& file_name)
+{
+  bool result = false;
+  std::ifstream file(file_name);
+  if (file.is_open())
+  {
+    double x, y, yaw;
+    file >> x >> y >> yaw;
+    setPose(x, y, yaw);
+    file.close();
+    result = true;
+  }
+  else
+  {
+    result = false;
+  }
+  return result;
+}
+