optimal and add file readme

README.md

@@ -0,0 +1,312 @@
+# Dock Planner
+
+Global planner tao duong di cho robot tiep can tram dock (tram sac, tram hang) bang duong cong NURBS.
+
+## Cau truc thu muc
+
+```
+dock_planner/
+├── include/dock_planner/
+│   ├── dock_planner.h          # Lop DockPlanner - plugin interface
+│   ├── dock_calc.h             # Lop DockCalc - logic tinh toan duong di
+│   └── utils/
+│       ├── curve_common.h      # Thu vien NURBS curve
+│       ├── pose.h              # Chuyen doi pose/quaternion
+│       └── common.h            # Tien ich chung
+├── src/
+│   ├── dock_planner.cpp        # Khoi tao, load param, goi DockCalc
+│   ├── dock_calc.cpp           # Thuat toan chinh: NURBS path generation
+│   └── utils/
+│       ├── curve_common.cpp
+│       ├── line_common.cpp
+│       └── pose.cpp
+└── config/
+    └── dock_global_params.yaml # Tham so cau hinh
+```
+
+## Pipeline tong quan
+
+```mermaid
+flowchart TB
+    subgraph INIT["1. KHOI TAO - DockPlanner::initialize"]
+        I1["Lien ket costmap_robot"] --> I2["Load ROS params:<br/>cost_threshold, calib_safety_distance"]
+        I2 --> I3["Load footprint_dock<br/>tu param server"]
+    end
+
+    subgraph PLAN["2. LAP KE HOACH - DockPlanner::makePlan"]
+        P1["Lay footprint_robot<br/>tu costmap"] --> P2["Goi DockCalc::makePlanMoveToDock"]
+        P2 --> P3["Chuyen vector Pose<br/>thanh vector PoseStamped"]
+    end
+
+    subgraph CALC["3. TINH TOAN - DockCalc::makePlanMoveToDock"]
+        C1["Tinh RobotGeometry + safety_distance"]
+        C2["Xac dinh huong tiep can<br/>forward / backward"]
+        C3["Tinh angle_threshold<br/>heuristic"]
+        C4["Tao duong tam NURBS<br/>findPathTMP"]
+        C5["Tim control points<br/>segment 1"]
+        C6["Sinh NURBS segment 1"]
+        C7["Sinh NURBS segment 2"]
+        C8["Ghep path: start + seg1 + seg2 + goal"]
+        C1 --> C2 --> C3 --> C4 --> C5 --> C6 --> C7 --> C8
+    end
+
+    INIT --> PLAN --> CALC
+```
+
+## So do giai thuat chi tiet
+
+### 1. Khoi tao (initialize)
+
+```mermaid
+flowchart LR
+    A["DockPlanner::initialize(name, costmap_robot)"] --> B{Da initialized?}
+    B -- Co --> Z["return true"]
+    B -- Chua --> C["Lien ket costmap_robot_<br/>costmap_<br/>frame_id_"]
+    C --> D["getParam:<br/>check_free_space<br/>cost_threshold<br/>calib_safety_distance"]
+    D --> E["Copy params<br/>sang DockCalc"]
+    E --> F["footprintFromParams<br/>→ footprint_dock_"]
+    F --> G["initialized_ = true"]
+    G --> Z
+```
+
+### 2. Lap ke hoach (makePlan)
+
+```mermaid
+flowchart TD
+    A["makePlan(start, goal, plan)"] --> B{initialized_?}
+    B -- false --> FAIL["return false"]
+    B -- true --> C["footprint_robot =<br/>costmap_robot_->getRobotFootprint()"]
+    C --> D["makePlanMoveToDock<br/>(start, goal, footprint_robot,<br/>footprint_dock_, degree=2, step=0.02)"]
+    D --> E{Thanh cong?}
+    E -- false --> FAIL
+    E -- true --> F["Chuyen moi Pose thanh PoseStamped<br/>(gan timestamp, frame_id)"]
+    F --> G["return true"]
+```
+
+### 3. Thuat toan chinh (makePlanMoveToDock)
+
+```mermaid
+flowchart TD
+    START["makePlanMoveToDock(start, goal)"] --> GEOM
+
+    subgraph GEOM["BUOC 1: Tinh hinh hoc"]
+        G1["computeGeometry(footprint_robot)<br/>→ robot.length, robot.radius"]
+        G2["computeGeometry(footprint_dock)<br/>→ dock.length"]
+        G3["safety_distance =<br/>(robot.radius + calib) * 2"]
+        G1 --> G3
+        G2 --> G3
+    end
+
+    GEOM --> VALID{robot.length > 0<br/>AND safety > 0?}
+    VALID -- false --> ERR["return false"]
+    VALID -- true --> POSE_C
+
+    subgraph POSE_C["BUOC 2: Tinh diem trung gian C"]
+        PC1["distance_min_C =<br/>(robot.length + dock.length) / 2"]
+        PC2["yaw_goal = normalizeAngle(goal.yaw)"]
+        PC3["Pose_C = offsetPoint(goal, yaw_goal, distance_min_C)"]
+        PC1 --> PC3
+        PC2 --> PC3
+    end
+
+    POSE_C --> DIR
+
+    subgraph DIR["BUOC 3: Xac dinh huong di"]
+        D1["t = compute_t(start, goal, Pose_C)"]
+        D2{t < 0 ?}
+        D3["Lat yaw_goal 180 do<br/>Tinh lai Pose_C va t<br/>dir = backward"]
+        D4["dir = forward"]
+        D1 --> D2
+        D2 -- Co --> D3
+        D2 -- Khong --> D4
+    end
+
+    DIR --> T_CHECK{t > 1 ?}
+    T_CHECK -- false --> ERR
+    T_CHECK -- true --> ANGLE
+
+    subgraph ANGLE["BUOC 4: Tinh angle threshold"]
+        A1["Pose_D = compute_D(goal, Pose_C, t)"]
+        A2["distance_AD =<br/>pointToSegmentDistance(start, goal, Pose_C)"]
+        A3["scale = f(distance_AD)<br/>angle_threshold =<br/>min((alpha+|scale|)*beta*pi, gamma*pi)"]
+        A1 --> A3
+        A2 --> A3
+    end
+
+    ANGLE --> TMP_PATH
+
+    subgraph TMP_PATH["BUOC 5: Tao duong tam"]
+        T1["Tinh Pose_F = offsetPoint(goal, yaw, dist_F)"]
+        T2["mid = trung diem(C, F)"]
+        T3["findPathTMP({C, mid, F})<br/>→ saved_poses_tmp"]
+        T1 --> T2 --> T3
+    end
+
+    TMP_PATH --> TMP_OK{Path tmp ok?}
+    TMP_OK -- false --> ERR
+    TMP_OK -- true --> SEG1
+
+    subgraph SEG1["BUOC 6: Sinh segment 1"]
+        S1["findNURBSControlPoints<br/>(start, saved_poses_tmp, angle_threshold)<br/>→ control_points_1, dir"]
+        S2["Xac dinh chieu reverse<br/>cua segment 1"]
+        S3["generateNURBSPath(control_points_1)<br/>→ first_segment"]
+        S1 --> S2 --> S3
+    end
+
+    SEG1 --> SEG1_OK{first_segment<br/>khong rong?}
+    SEG1_OK -- false --> ERR
+    SEG1_OK -- true --> SEG2
+
+    subgraph SEG2["BUOC 7: Sinh segment 2"]
+        S4["mid_ctrl = trung diem<br/>(first_segment.back, goal)"]
+        S5["generateNURBSPath<br/>({last_of_seg1, mid_ctrl, goal})<br/>→ second_segment"]
+        S4 --> S5
+    end
+
+    SEG2 --> ASSEMBLE["BUOC 8: Ghep duong di<br/>start + seg1 + seg2 + goal"]
+    ASSEMBLE --> OK["return true"]
+```
+
+### 4. Sinh duong NURBS (generateNURBSPath)
+
+```mermaid
+flowchart TD
+    A["generateNURBSPath<br/>(control_points, check_free,<br/>edge_end, reverse)"] --> B["setupSpline(control_points, degree)<br/>Tao knot vector + weights<br/>Nap vao Spline_Inf"]
+    B --> C["Tinh chieu dai NURBS<br/>calculateNURBSLength"]
+    C --> D["step_new = resolution / length<br/>(adaptive step)"]
+    D --> E["Duyet t = 0 → 1<br/>(buoc step_new)"]
+
+    E --> F["point = CalculateCurvePoint(t)"]
+    F --> G{point hop le?<br/>khong NaN?}
+    G -- false --> E
+    G -- true --> H{Kiem tra<br/>isFreeSpace?}
+    H -- Bi chan --> I["obstacle_flag = true<br/>BREAK"]
+    H -- Thong --> J["Luu pose<br/>Cap nhat orientation"]
+    J --> E
+
+    I --> K["result.clear()"]
+    E -- t > 1.0 --> L["Gan orientation<br/>cho diem cuoi"]
+    L --> M["return result"]
+    K --> M
+```
+
+### 5. Tim control points (findNURBSControlPoints)
+
+```mermaid
+flowchart TD
+    A["findNURBSControlPoints<br/>(start, saved_poses, angle_threshold)"] --> B["findNearestPointOnPath<br/>→ idx, dir"]
+    B --> C{idx != -1 ?}
+
+    C -- idx hop le --> D["control_points = {start, saved[idx]}"]
+    D --> E{dir == 1?}
+    E -- Co --> F["Tim diem lui ve<br/>sao cho dist >= target<br/>→ end_idx"]
+    F --> G["Them saved[end_idx]"]
+    E -- Khong, dir == -1 --> H["Them saved.back()"]
+
+    C -- idx == -1 --> I["mid = trung diem<br/>(start, nearest)"]
+    I --> J["control_points =<br/>{start, mid, nearest}"]
+
+    G --> K["return true"]
+    H --> K
+    J --> K
+```
+
+### 6. Tim diem gan nhat tren duong (findNearestPointOnPath)
+
+```mermaid
+flowchart TD
+    A["findNearestPointOnPath<br/>(pose, saved_poses, angle_threshold)"] --> B["Tim nearest_idx<br/>(khoang cach nho nhat)"]
+    B --> C["Duyet nguoc tu nearest_idx → 1"]
+    C --> D{signedAngle >= threshold?}
+    D -- Co --> E["nearest_idx = i<br/>found_flag_1 = true"]
+    D -- Khong --> C
+
+    E --> F{Kiem tra khoang cach<br/>distance_2 >= distance_1?}
+    F -- Co --> G["dir = 1<br/>return nearest_idx"]
+    F -- Khong --> H["Duyet xuoi tu nearest_idx → cuoi"]
+    C -- Het vong --> H
+
+    H --> I{signedAngle >= threshold - 0.05?}
+    I -- Co --> J["dir = -1<br/>return nearest_idx"]
+    I -- Khong, i == cuoi --> K["dir = 0<br/>return -1"]
+
+    J --> L{dist hop ly?}
+    L -- false --> K
+    L -- true --> J
+```
+
+## Mo ta cac diem hinh hoc
+
+```
+    F ──────── mid ──────── C ──────── Goal
+    |          (duong tam)          |     |
+    |                               |     |
+    |     Pose_D                   |     |
+    |       ╲                      |     |
+    |        ╲  distance_AD       |     |
+    |         ╲                    |     |
+    Start      ╲                   |     |
+                                   |     |
+          ◄── distance_min_F ────►|     |
+                         ◄── dist_C ──►|
+```
+
+- **Goal**: Vi tri dock dich
+- **Pose_C**: Diem offset tu Goal theo huong yaw_goal, cach `(robot.length + dock.length) / 2`
+- **Pose_F**: Diem xa hon tren cung huong, cach Goal = `dist_C + dist_CD + dist_DF`
+- **Pose_D**: Hinh chieu vuong goc tu Start len duong Goal→C keo dai
+- **distance_AD**: Khoang cach vuong goc tu Start den duong Goal→C
+- **Duong tam (C → mid → F)**: Duong NURBS tam de tim diem noi segment 1
+
+## Luong du lieu giua cac lop
+
+```mermaid
+flowchart LR
+    subgraph Plugin["DockPlanner (Plugin Interface)"]
+        init["initialize()"]
+        make["makePlan()"]
+    end
+
+    subgraph Core["DockCalc (Core Algorithm)"]
+        calc["makePlanMoveToDock()"]
+        nurbs["generateNURBSPath()"]
+        tmp["findPathTMP()"]
+        ctrl["findNURBSControlPoints()"]
+        near["findNearestPointOnPath()"]
+        setup["setupSpline()"]
+    end
+
+    subgraph Lib["Utils"]
+        curve["CurveCommon<br/>(NURBS engine)"]
+        spline["Spline_Inf<br/>(knot/ctrl/weight)"]
+    end
+
+    init -->|"config params"| calc
+    make -->|"start, goal,<br/>footprints"| calc
+    calc --> tmp
+    calc --> ctrl
+    calc --> nurbs
+    ctrl --> near
+    tmp --> setup
+    nurbs --> setup
+    setup --> curve
+    setup --> spline
+    curve -->|"CalculateCurvePoint"| nurbs
+    curve -->|"CalculateCurvePoint"| tmp
+```
+
+## Tham so cau hinh
+
+| Tham so | Kieu | Mac dinh | Mo ta |
+|---------|------|----------|-------|
+| `cost_threshold` | int | 200 | Nguong chi phi vat can (0-255) |
+| `calib_safety_distance` | double | 0.0 | Khoang cach an toan bo sung (met) |
+| `check_free_space` | bool | false | Bat/tat kiem tra vung tu do |
+| `degree` (hardcode) | int | 2 | Bac cua duong cong NURBS |
+| `step` (hardcode) | double | 0.02 | Buoc lay mau tham so t |
+
+## Ghi chu
+
+- **NURBS (Non-Uniform Rational B-Spline)**: Duong cong bac 2 voi 3 control points, knot vector dang clamped `[0,0,0,1,1,1]`.
+- **Adaptive step**: Buoc lay mau duoc tinh tu `resolution / curve_length` de dam bao mat do diem dong deu theo do phan giai costmap.
+- **2-segment path**: Duong di gom 2 doan NURBS noi tiep — segment 1 dua robot tu vi tri hien tai vao duong tiep can, segment 2 dua robot tu diem noi den dock.

include/dock_planner/dock_calc.h

@@ -5,12 +5,9 @@
 #include <robot_geometry_msgs/PoseStamped.h>
 
 #include <robot_nav_msgs/Path.h>
-#include <robot_nav_msgs/OccupancyGrid.h>
-#include <robot_nav_msgs/GetPlan.h>
 
 #include <robot_costmap_2d/costmap_2d_robot.h>
 #include <robot_costmap_2d/costmap_2d.h>
-#include <robot_costmap_2d/inflation_layer.h>
 
 #include <tf3/LinearMath/Quaternion.h>
 
@@ -19,10 +16,9 @@
 #include "dock_planner/utils/common.h"
 #include <robot/robot.h>
 
+#include <memory>
 #include <vector>
 
-using namespace std;
-
 namespace dock_planner
 {
     struct RobotGeometry
@@ -35,14 +31,11 @@ namespace dock_planner
     class DockCalc
     {
     private:
-        /* data */
+        std::unique_ptr<Spline_Inf> input_spline_inf;
-        Spline_Inf* input_spline_inf;
+        std::unique_ptr<CurveCommon> CurveDesign;
-        CurveCommon* CurveDesign;
 
-        vector<robot_geometry_msgs::Pose> posesOnPathWay;
+        std::vector<robot_geometry_msgs::Point> footprint_robot_;
-        vector<robot_geometry_msgs::Point> footprint_robot_;
         robot_geometry_msgs::PoseStamped save_goal_;
-        // std::vector<robot_geometry_msgs::Pose> free_zone_;
 
         bool check_goal_;
         int store_start_nearest_idx_;
@@ -57,7 +50,8 @@ namespace dock_planner
 
         inline double calculateAdaptiveStep(double segment_length, double desired_point_spacing = 0.02)
         {
-        return desired_point_spacing / segment_length;
+            if (segment_length <= 1e-9) return 1.0;
+            return desired_point_spacing / segment_length;
         }
 
         inline double calculateAngle(double x1, double y1, double x2, double y2)
@@ -68,11 +62,7 @@ namespace dock_planner
 
         inline double normalizeAngle(double angle)
         {
-            while (angle > M_PI)
+            return std::remainder(angle, 2.0 * M_PI);
-                angle -= 2.0 * M_PI;
-            while (angle <= -M_PI)
-                angle += 2.0 * M_PI;
-            return angle;
         }
 
         double calculateNURBSLength(CurveCommon* curve_design, 
@@ -90,15 +80,17 @@ namespace dock_planner
 
         robot_geometry_msgs::Pose compute_D(const robot_geometry_msgs::Pose& B, const robot_geometry_msgs::Pose& C,const double& t);
 
+        void setupSpline(const std::vector<robot_geometry_msgs::Pose>& control_points, int degree);
+
         void updatePoseOrientation(std::vector<robot_geometry_msgs::Pose>& saved_poses,
                                     std::vector<robot_geometry_msgs::Pose>& nurbs_path,
                                     bool reverse = true);
 
-        bool findPathTMP(const vector<robot_geometry_msgs::Pose>& control_points,
+        bool findPathTMP(const std::vector<robot_geometry_msgs::Pose>& control_points,
-                        vector<robot_geometry_msgs::Pose>& saved_poses,                
+                        std::vector<robot_geometry_msgs::Pose>& saved_poses,
                         const int& degree, double step = 0.02);
 
-        bool findNURBSControlPoints(int& dir, vector<robot_geometry_msgs::Pose>& control_points,
+        bool findNURBSControlPoints(int& dir, std::vector<robot_geometry_msgs::Pose>& control_points,
                                     const robot_geometry_msgs::Pose& start_pose,
                                     const std::vector<robot_geometry_msgs::Pose>& posesOnPathWay,
                                     const double& angle_threshol, const int& degree);
@@ -122,20 +114,20 @@ namespace dock_planner
         //Params
         robot_costmap_2d::Costmap2D* costmap_;
 
-        int cost_threshold_;     // Threshold for obstacle detection
+        int cost_threshold_;              // Threshold for obstacle detection
-        double safety_distance_;    // Safety distance from obstacles
+        double safety_distance_;          // Derived = (robot.radius + calib_safety_distance_) * 2.0, recomputed each plan
-        double calib_safety_distance_;
+        double calib_safety_distance_;    // Configured via ROS param "calib_safety_distance"
 
-        DockCalc(/* args */);
+        DockCalc();
 
         ~DockCalc();
 
         bool makePlanMoveToDock(const robot_geometry_msgs::PoseStamped& start,
                                 const robot_geometry_msgs::PoseStamped& goal,
-                                const vector<robot_geometry_msgs::Point>& footprint_robot,
+                                const std::vector<robot_geometry_msgs::Point>& footprint_robot,
-                                const vector<robot_geometry_msgs::Point>& footprint_dock,
+                                const std::vector<robot_geometry_msgs::Point>& footprint_dock,
                                 const int& degree, const double& step,
-                                vector<robot_geometry_msgs::Pose>& planMoveToDock,
+                                std::vector<robot_geometry_msgs::Pose>& planMoveToDock,
                                 bool reverse = false);
         
         

include/dock_planner/dock_planner.h

@@ -27,7 +27,6 @@ namespace dock_planner {
         robot_costmap_2d::Costmap2D* costmap_;
         bool initialized_;
         std::string frame_id_;
-        // ros::Publisher plan_pub_;
         std::vector<robot_geometry_msgs::Point> footprint_dock_;
 
         int cost_threshold_;

src/dock_calc.cpp

@@ -2,21 +2,21 @@
 
 namespace dock_planner
 {
-    DockCalc::DockCalc(/* args */) :costmap_(nullptr)
+    DockCalc::DockCalc()
+        : input_spline_inf(std::make_unique<Spline_Inf>()),
+          CurveDesign(std::make_unique<CurveCommon>()),
+          check_goal_(false),
+          store_start_nearest_idx_(0),
+          start_target_idx_(0),
+          min_distance_to_goal_(0.0),
+          costmap_(nullptr),
+          cost_threshold_(200),
+          safety_distance_(0.0),
+          calib_safety_distance_(0.0)
     {
-        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()
+    DockCalc::~DockCalc() = default;
-    {
-        delete CurveDesign;
-        delete input_spline_inf;
-    }
 
     robot_geometry_msgs::Pose DockCalc::offsetPoint(const robot_geometry_msgs::Pose& A, const double& theta, const double& d) 
     {
@@ -77,47 +77,73 @@ namespace dock_planner
         return D;
     }
 
+    void DockCalc::setupSpline(const std::vector<robot_geometry_msgs::Pose>& control_points, int degree)
+    {
+        std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>> eigen_points;
+        eigen_points.reserve(control_points.size());
+        for (const auto& cp : control_points)
+        {
+            eigen_points.emplace_back(cp.position.x, cp.position.y, 0);
+        }
+
+        const int n = static_cast<int>(control_points.size()) - 1;
+        const int m = n + degree + 1;
+        std::vector<double> knot_vector;
+        knot_vector.reserve(m + 1);
+        for (int i = 0; i <= m; i++)
+        {
+            knot_vector.push_back(i <= degree ? 0.0 : 1.0);
+        }
+
+        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.get(), degree + 1, eigen_points, knot_vector);
+        CurveDesign->ReadSplineInf(input_spline_inf.get(), weights, false);
+    }
+
     RobotGeometry DockCalc::computeGeometry(const std::vector<robot_geometry_msgs::Point>& fp)
     {
-        double min_x =  1e9, max_x = -1e9;
+        if (fp.size() < 4) return {0, 0, 0};
-        double min_y =  1e9, max_y = -1e9;
+
+        double min_x = std::numeric_limits<double>::max();
+        double max_x = std::numeric_limits<double>::lowest();
+        double min_y = std::numeric_limits<double>::max();
+        double max_y = std::numeric_limits<double>::lowest();
         double radius = 0.0;
-        if((int)fp.size() >= 4)
+
+        for (const auto& p : fp)
         {
-            for (const auto& p : fp)
+            min_x  = std::min(min_x, p.x);
-            {
+            max_x  = std::max(max_x, p.x);
-                min_x  = std::min(min_x, p.x);
+            min_y  = std::min(min_y, p.y);
-                max_x  = std::max(max_x, p.x);
+            max_y  = std::max(max_y, p.y);
-                min_y  = std::min(min_y, p.y);
+            radius = std::max(radius, std::hypot(p.x, 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};              
+        return {max_x - min_x, max_y - min_y, radius};
-        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 robot_geometry_msgs::PoseStamped& start,
                                     const robot_geometry_msgs::PoseStamped& goal,
-                                    const vector<robot_geometry_msgs::Point>& footprint_robot,
+                                    const std::vector<robot_geometry_msgs::Point>& footprint_robot,
-                                    const vector<robot_geometry_msgs::Point>& footprint_dock,
+                                    const std::vector<robot_geometry_msgs::Point>& footprint_dock,
                                     const int& degree, const double& step,
-                                    vector<robot_geometry_msgs::Pose>& planMoveToDock,
+                                    std::vector<robot_geometry_msgs::Pose>& planMoveToDock,
                                     bool reverse)
     {
         constexpr double epsilon = 1e-6;
-        robot::log_error("[dock_calc] makePlanMoveToDock start: (%f, %f), goal: (%f, %f), goal frame id: %s", start.pose.position.x, start.pose.position.y, goal.pose.position.x, goal.pose.position.y, goal.header.frame_id.c_str());
+        robot::log_info("[dock_calc] makePlanMoveToDock start: (%f, %f), goal: (%f, %f), goal frame id: %s", start.pose.position.x, start.pose.position.y, goal.pose.position.x, goal.pose.position.y, goal.header.frame_id.c_str());
         planMoveToDock.clear();
 
         if(save_goal_.pose.position.x != goal.pose.position.x ||
             save_goal_.pose.position.y != goal.pose.position.y ||
             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;
+            save_goal_ = goal;
         }
 
         footprint_robot_ = footprint_robot;
@@ -132,7 +158,6 @@ namespace dock_planner
             return false;
         }
 
-        // Calculate safety distance
         safety_distance_ = (robot.radius + calib_safety_distance_) * 2.0;
         if (safety_distance_ <= epsilon)
         {
@@ -141,49 +166,23 @@ namespace dock_planner
         }
 
         // Lambda: Generate NURBS path
-        auto generateNURBSPath = [&](const vector<robot_geometry_msgs::Pose>& control_points, bool check_isFreeSpace, 
+        auto generateNURBSPath = [&](const std::vector<robot_geometry_msgs::Pose>& control_points, bool check_isFreeSpace,
-                                    bool edge_end_plag, bool path_reverse) -> vector<robot_geometry_msgs::Pose> 
+                                    bool edge_end_plag, bool path_reverse) -> std::vector<robot_geometry_msgs::Pose>
         {
-            vector<robot_geometry_msgs::Pose> result;
+            std::vector<robot_geometry_msgs::Pose> result;
             bool obstacle_flag = false;
 
-            // Convert to Eigen format
+            setupSpline(control_points, degree);
-            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 edge_length = calculateNURBSLength(CurveDesign.get(), input_spline_inf.get(), degree, step);
             double resolution = costmap_->getResolution();
             double step_new = calculateAdaptiveStep(edge_length, resolution);
 
-            // robot::log_error("[dock_calc] Adaptive step size: %f", step_new);
+            std::vector<robot_geometry_msgs::Pose> saved_poses;
-
-            // Generate path points
-            vector<robot_geometry_msgs::Pose> saved_poses;
             for (double t = 0.0; t <= 1.0; t += step_new)
             {
-                robot_geometry_msgs::Point point = CurveDesign->CalculateCurvePoint(input_spline_inf, t, true);
+                robot_geometry_msgs::Point point = CurveDesign->CalculateCurvePoint(input_spline_inf.get(), t, true);
 
                 if (!std::isnan(point.x) && !std::isnan(point.y))
                 {
@@ -256,7 +255,6 @@ namespace dock_planner
         };
 
         // 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);
 
@@ -291,16 +289,22 @@ namespace dock_planner
         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
+        // Angle threshold heuristic: linearly scale [MIN_DISTANCE..MAX_DISTANCE] of
-        const double MAX_DISTANCE = 1.0, MIN_DISTANCE = 0.1;
+        // perpendicular distance AD into an angle in [alpha*beta*pi .. gamma*pi].
-        const double MIN_SCALE = 0.2, MAX_SCALE = 1.0;
+        // Closer to the line → larger angle threshold (more permissive turn).
-        const double calib_factor_alpha = 0.5;
+        constexpr double MAX_DISTANCE       = 1.0;   // distance at which scale = MIN_SCALE
-        const double calib_factor_beta = 0.5;
+        constexpr double MIN_DISTANCE       = 0.1;   // distance at which scale = MAX_SCALE
-        const double calib_factor_gamma = 0.77;
+        constexpr double MIN_SCALE          = 0.2;
+        constexpr double MAX_SCALE          = 1.0;
+        constexpr double CALIB_FACTOR_ALPHA = 0.5;   // base offset added to |scale|
+        constexpr double CALIB_FACTOR_BETA  = 0.5;   // multiplier applied to (alpha + |scale|)
+        constexpr double CALIB_FACTOR_GAMMA = 0.77;  // upper clamp expressed as fraction of pi
 
-        double scale = MIN_SCALE + (MAX_SCALE - MIN_SCALE) * 
+        const 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);
+        const double angle_threshold = std::min(
+            (CALIB_FACTOR_ALPHA + std::fabs(scale)) * CALIB_FACTOR_BETA * M_PI,
+            CALIB_FACTOR_GAMMA * M_PI);
 
         // Generate temporary path
         double distance_CD = calc_distance(Pose_C, Pose_D);
@@ -313,8 +317,8 @@ namespace dock_planner
         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<robot_geometry_msgs::Pose> control_points_tmp = {Pose_C, mid_pose, Pose_F};
+        std::vector<robot_geometry_msgs::Pose> control_points_tmp = {Pose_C, mid_pose, Pose_F};
-        vector<robot_geometry_msgs::Pose> saved_poses_tmp;
+        std::vector<robot_geometry_msgs::Pose> saved_poses_tmp;
 
 
         if (!findPathTMP(control_points_tmp, saved_poses_tmp, degree, step)) 
@@ -325,7 +329,7 @@ namespace dock_planner
 
         // Find first segment control points
         int dir;
-        vector<robot_geometry_msgs::Pose> control_points_1;
+        std::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,70 +341,41 @@ namespace dock_planner
                                     ((dir == 1 || dir == 0) && dir_robot_move_to_goal);
 
         // Generate first segment
-        vector<robot_geometry_msgs::Pose> first_segment = generateNURBSPath(control_points_1, true, false, first_segment_reverse);
+        std::vector<robot_geometry_msgs::Pose> first_segment = generateNURBSPath(control_points_1, true, false, first_segment_reverse);
+        if (first_segment.empty()) return false;
+
+        // Generate second segment (to goal)
+        robot_geometry_msgs::Pose mid_control_point;
+        mid_control_point.position.x = (first_segment.back().position.x + goal.pose.position.x) / 2.0;
+        mid_control_point.position.y = (first_segment.back().position.y + goal.pose.position.y) / 2.0;
+
+        std::vector<robot_geometry_msgs::Pose> control_points_2 = {first_segment.back(),
+                                                        mid_control_point,
+                                                        goal.pose};
+
+        std::vector<robot_geometry_msgs::Pose> second_segment = generateNURBSPath(control_points_2, true, true, dir_robot_move_to_goal);
+
+        // Assemble final path: start + first_segment + second_segment + goal
+        planMoveToDock.reserve(1 + first_segment.size() + second_segment.size() + 1);
+        planMoveToDock.push_back(start.pose);
         planMoveToDock.insert(planMoveToDock.end(), first_segment.begin(), first_segment.end());
+        planMoveToDock.insert(planMoveToDock.end(), second_segment.begin(), second_segment.end());
+        planMoveToDock.push_back(goal.pose);
 
-        //Generate second segment (to goal)
+        return true;
-        if (!planMoveToDock.empty()) 
-        {
-            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<robot_geometry_msgs::Pose> control_points_2 = {planMoveToDock.back(), 
-                                                            mid_control_point, 
-                                                            goal.pose};
-
-            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);
-        }        
-
-        return !planMoveToDock.empty();
     }
 
-    bool DockCalc::findPathTMP(const vector<robot_geometry_msgs::Pose>& control_points,
+    bool DockCalc::findPathTMP(const std::vector<robot_geometry_msgs::Pose>& control_points,
-                                vector<robot_geometry_msgs::Pose>& saved_poses,                
+                                std::vector<robot_geometry_msgs::Pose>& saved_poses,
                                 const int& degree, double step)
     {
-        // Chuyển đổi control points sang định dạng Eigen::Vector3d
+        setupSpline(control_points, degree);
-        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<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);
+            robot_geometry_msgs::Point point = CurveDesign->CalculateCurvePoint(input_spline_inf.get(), 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);
                 robot_geometry_msgs::Pose pose;
                 pose.position = point;
                 pose.orientation.x = 0.0;
@@ -436,27 +411,21 @@ namespace dock_planner
                                         std::vector<robot_geometry_msgs::Pose>& nurbs_path,
                                         bool reverse)
     {
-        double yaw;
+        auto& prev = saved_poses[saved_poses.size() - 2];
-        // robot::log_error("[merge_path_calc] DEBUG: reverse(%x)", reverse);
+        const auto& curr = saved_poses.back();
-        if(!reverse)
+
-        yaw = calculateAngle(saved_poses[saved_poses.size() - 2].position.x,
+        const double yaw = reverse
-                                saved_poses[saved_poses.size() - 2].position.y,
+            ? calculateAngle(curr.position.x, curr.position.y, prev.position.x, prev.position.y)
-                                saved_poses.back().position.x,
+            : calculateAngle(prev.position.x, prev.position.y, curr.position.x, curr.position.y);
-                                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();
+        prev.orientation.x = q.x();
-        saved_poses[saved_poses.size() - 2].orientation.y = q.y();
+        prev.orientation.y = q.y();
-        saved_poses[saved_poses.size() - 2].orientation.z = q.z();
+        prev.orientation.z = q.z();
-        saved_poses[saved_poses.size() - 2].orientation.w = q.w();
+        prev.orientation.w = q.w();
 
-        nurbs_path.push_back(saved_poses[saved_poses.size() - 2]);
+        nurbs_path.push_back(prev);
     }
 
     double DockCalc::calculateNURBSLength(CurveCommon* curve_design, 
@@ -466,33 +435,24 @@ namespace dock_planner
     {
         double length = 0.0;
         robot_geometry_msgs::Point prev_point, curr_point;
-        std::vector<double> segment_lengths;
 
-        static double step;
+        const double step = (degree == 1) ? 0.1 : initial_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) + 
+                length += std::hypot(curr_point.x - prev_point.x,
-                                                std::pow(curr_point.y - prev_point.y, 2));
+                                     curr_point.y - prev_point.y);
-                length += segment_length;
-                segment_lengths.push_back(segment_length);
             }
             prev_point = curr_point;
         }
         return length;
     }
 
-    bool DockCalc::findNURBSControlPoints(int& dir, vector<robot_geometry_msgs::Pose>& control_points,
+    bool DockCalc::findNURBSControlPoints(int& dir, std::vector<robot_geometry_msgs::Pose>& control_points,
                                         const robot_geometry_msgs::Pose& start_pose,
                                         const std::vector<robot_geometry_msgs::Pose>& saved_poses,
                                         const double& angle_threshold, const int& degree)
@@ -506,8 +466,7 @@ namespace dock_planner
         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) -> robot_geometry_msgs::Pose
-        auto createPose = [](double x, double y, double yaw = 0.0) -> robot_geometry_msgs::Pose 
         {
             robot_geometry_msgs::Pose pose;
             pose.position.x = x;
@@ -553,8 +512,6 @@ namespace dock_planner
             } 
             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;
@@ -563,21 +520,15 @@ namespace dock_planner
         } 
         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.x + nearest_pose.position.x) / 2.0,
-            (start_pose.position.y + nearest_pose.position.y) / 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_;
         }
 
@@ -607,7 +558,7 @@ namespace dock_planner
         bool found_pose_flag_1 = false;
         bool found_pose_flag_2 = false;
 
-        for(int i = store_start_nearest_idx_; i >= 0; i--)
+        for(int i = store_start_nearest_idx_; i >= 1; i--)
         {
             double angle = signedAngle(pose, saved_poses[i], saved_poses[i - 1]);
 
@@ -629,7 +580,8 @@ namespace dock_planner
         
         if(!found_pose_flag_2)
         {
-            for(int i = store_start_nearest_idx_; i < (int)saved_poses.size(); i++)
+            const int last_idx = (int)saved_poses.size() - 1;
+            for(int i = store_start_nearest_idx_; i < last_idx; i++)
             {
                 double angle = signedAngle(pose, saved_poses[i], saved_poses[i + 1]);
 
@@ -639,7 +591,7 @@ namespace dock_planner
                     dir = -1;
                     break;
                 }
-                if(i == (int)saved_poses.size() - 1)
+                if(i == last_idx - 1)
                 {
                     dir = 0;
                     return -1;
@@ -660,88 +612,7 @@ namespace dock_planner
     bool DockCalc::isFreeSpace(const robot_geometry_msgs::Pose& pose,
                             const std::vector<robot_geometry_msgs::Point>& footprint)
     {
-        // static std::vector<robot_geometry_msgs::Pose> free_zone;
+        // TODO: implement obstacle checking using costmap_
-
-        // auto makePose = [](double x, double y, double yaw) 
-        // {
-        //     robot_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<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<robot_geometry_msgs::Point> space_footprint = interpolateFootprint(pose, footprint, resolution);
-        // for (int i = 0; i < (int)space_footprint.size(); i++) 
-        // {
-        //     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;
-        //     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 >= robot_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;
     }
 
@@ -784,10 +655,11 @@ namespace dock_planner
     }
 
     std::vector<robot_geometry_msgs::Point> points;
-    for (size_t i = 0; i < (int)abs_footprint.size(); ++i)
+    const size_t fp_size = abs_footprint.size();
+    for (size_t i = 0; i < fp_size; ++i)
     {
         const robot_geometry_msgs::Point &start = abs_footprint[i];
-        const robot_geometry_msgs::Point &end = abs_footprint[(i + 1) % abs_footprint.size()];
+        const robot_geometry_msgs::Point &end = abs_footprint[(i + 1) % fp_size];
 
         double dx = end.x - start.x;
         double dy = end.y - start.y;
@@ -796,13 +668,13 @@ namespace dock_planner
 
         for (int j = 0; j <= steps; ++j)
         {
-        if (j == steps && i != (int)abs_footprint.size() - 1)
+            if (j == steps && i != fp_size - 1)
-            continue; // tránh lặp
+                continue; // tránh lặp điểm chung giữa 2 cạnh liên tiếp
-        double t = static_cast<double>(j) / steps;
+            double t = static_cast<double>(j) / steps;
-        robot_geometry_msgs::Point pt;
+            robot_geometry_msgs::Point pt;
-        pt.x = start.x + t * dx;
+            pt.x = start.x + t * dx;
-        pt.y = start.y + t * dy;
+            pt.y = start.y + t * dy;
-        points.push_back(pt);
+            points.push_back(pt);
         }
     }
     return points;

src/dock_planner.cpp

@@ -12,9 +12,9 @@ namespace dock_planner
     DockPlanner::DockPlanner() : costmap_robot_(nullptr),
                                 costmap_(nullptr),
                                 initialized_(false),
-                                check_free_space_(false),
                                 cost_threshold_(200),
-                                calib_safety_distance_(0.0) {}
+                                calib_safety_distance_(0.0),
+                                check_free_space_(false) {}
 
     /**
      * @brief Constructor với parameters
@@ -26,9 +26,9 @@ namespace dock_planner
         : costmap_robot_(nullptr),
         costmap_(nullptr),
         initialized_(false),
-        check_free_space_(false),
         cost_threshold_(200),
-        calib_safety_distance_(0.0)
+        calib_safety_distance_(0.0),
+        check_free_space_(false)
     {
         initialize(name, costmap_robot);
     }
@@ -64,9 +64,7 @@ namespace dock_planner
             calc_plan_to_dock_.calib_safety_distance_ = calib_safety_distance_;
 
 
-            // plan_pub_ = private_nh.advertise<robot_nav_msgs::Path>("plan", 1);
+            robot_nav_2d_msgs::Polygon2D footprint_dock = robot_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)
             {
@@ -78,7 +76,6 @@ namespace dock_planner
             }
             
             initialized_ = true;
-            // ROS_WARN("[dock_planner] check_free_space_(%d)", check_free_space_);
         }
         return true;
     }
@@ -88,49 +85,33 @@ namespace dock_planner
                                 std::vector<robot_geometry_msgs::PoseStamped>& plan)
     {
         if(!initialized_) return false;
-        if(!plan.empty()) plan.clear();
+        plan.clear();
-        vector<robot_geometry_msgs::Pose> planMoveToDock;
-        // std::vector<robot_geometry_msgs::Point> footprint_robot = costmap_robot_->getRobotFootprint();
-        std::vector<robot_geometry_msgs::Point> footprint_robot;
-        robot_geometry_msgs::Point p1; p1.x = 0.1; p1.y = -0.1;
-        robot_geometry_msgs::Point p2; p2.x = 0.1; p2.y = 0.1;
-        robot_geometry_msgs::Point p3; p3.x = -0.1; p3.y = 0.1;
-        robot_geometry_msgs::Point p4; p4.x = -0.1; p4.y = -0.1;
-        footprint_robot.push_back(p1);
-        footprint_robot.push_back(p2);
-        footprint_robot.push_back(p3);
-        footprint_robot.push_back(p4);
 
-        // footprint_dock_ = costmap_robot_->getRobotFootprint();
+        std::vector<robot_geometry_msgs::Pose> planMoveToDock;
-        int degree = 2;
+        std::vector<robot_geometry_msgs::Point> footprint_robot = costmap_robot_->getRobotFootprint();
-        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;
+        constexpr int degree = 2;
-        for(int i = 0; i < (int)planMoveToDock.size(); i++)
+        constexpr double step = 0.02;
+        if (!calc_plan_to_dock_.makePlanMoveToDock(start, goal, footprint_robot, footprint_dock_, degree, step, planMoveToDock, true))
+            return false;
+
+        const robot::Time plan_time = robot::Time::now();
+        const std::string& global_frame = costmap_robot_->getGlobalFrameID();
+        plan.reserve(planMoveToDock.size());
+        for (const auto& p : planMoveToDock)
         {
-          robot_geometry_msgs::PoseStamped pose;
+            robot_geometry_msgs::PoseStamped pose;
-          pose.header.stamp = plan_time;
+            pose.header.stamp = plan_time;
-          pose.header.frame_id = costmap_robot_->getGlobalFrameID();
+            pose.header.frame_id = global_frame;
-          pose.pose.position.x = planMoveToDock[i].position.x;
+            pose.pose.position.x = p.position.x;
-          pose.pose.position.y = planMoveToDock[i].position.y;
+            pose.pose.position.y = p.position.y;
-          pose.pose.position.z = 0;
+            pose.pose.position.z = 0;
-          pose.pose.orientation = planMoveToDock[i].orientation;
+            pose.pose.orientation = p.orientation;
-          plan.push_back(pose);
+            plan.push_back(pose);
         }
         return true;
     }
 
-    // void DockPlanner::publishPlan(const std::vector<robot_geometry_msgs::PoseStamped>& plan) 
-    // {
-    //     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>();