update

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

@@ -14,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
@@ -28,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);
     }
@@ -90,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;