update

include/dock_planner/dock_planner.h

@@ -19,6 +19,8 @@ namespace dock_planner {
                     const robot_geometry_msgs::PoseStamped& goal,
                     std::vector<robot_geometry_msgs::PoseStamped>& plan);
 
+        static robot_nav_core::BaseGlobalPlanner::Ptr create();
+
     private:
         // Core components
         robot_costmap_2d::Costmap2DROBOT* costmap_robot_;
@@ -33,9 +35,6 @@ namespace dock_planner {
         bool check_free_space_;
 
         DockCalc calc_plan_to_dock_;
-        
-
-        void publishPlan(const std::vector<robot_geometry_msgs::PoseStamped>& plan);
     };
 } // namespace dock_planner
 

src/dock_calc.cpp

@@ -108,11 +108,11 @@ namespace dock_planner
                                     bool reverse)
     {
         constexpr double epsilon = 1e-6;
-        robot::log_error("[dock_calc] makePlanMoveToDock start");
+        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());
         planMoveToDock.clear();
 
         if(save_goal_.pose.position.x != goal.pose.position.x ||
-            save_goal_.pose.position.y != 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");

src/dock_planner.cpp

@@ -1,6 +1,7 @@
 #include "dock_planner/dock_planner.h"
 #include <cmath>
 #include <algorithm>
+#include <boost/dll/alias.hpp>
 
 namespace dock_planner 
 {
@@ -13,7 +14,7 @@ namespace dock_planner
                                 initialized_(false), 
                                 check_free_space_(false),
                                 cost_threshold_(200),
-                                calib_safety_distance_(0.05) {}
+                                calib_safety_distance_(0.0) {}
 
     /**
      * @brief Constructor với parameters
@@ -27,7 +28,7 @@ namespace dock_planner
         initialized_(false), 
         check_free_space_(false),
         cost_threshold_(200),
-        calib_safety_distance_(0.05)
+        calib_safety_distance_(0.0)
     {
         initialize(name, costmap_robot);
     }
@@ -79,6 +80,7 @@ namespace dock_planner
             initialized_ = true;
             // ROS_WARN("[dock_planner] check_free_space_(%d)", check_free_space_);
         }
+        return true;
     }
     
     bool DockPlanner::makePlan(const robot_geometry_msgs::PoseStamped& start,
@@ -88,7 +90,16 @@ namespace dock_planner
         if(!initialized_) return false;
         if(!plan.empty()) 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 = 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();
         int degree = 2;
@@ -108,7 +119,6 @@ namespace dock_planner
           pose.pose.orientation = planMoveToDock[i].orientation;
           plan.push_back(pose);
         }
-        publishPlan(plan);
         return true;
     }
 
@@ -121,4 +131,13 @@ namespace dock_planner
     //     plan_pub_.publish(path_msg);
     // }
 
+    // Export factory function
+    robot_nav_core::BaseGlobalPlanner::Ptr DockPlanner::create() {
+        return std::make_shared<dock_planner::DockPlanner>();
+    }
+
+
 } // namespace dock_planner
+
+// Alias cho Boost.DLL (nếu muốn dùng boost::dll::import_alias)
+BOOST_DLL_ALIAS(dock_planner::DockPlanner::create, DockPlanner)