update

src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory.cpp

@@ -1244,8 +1244,8 @@ robot_nav_2d_msgs::Path2D mkt_algorithm::diff::PredictiveTrajectory::generateTra
   }
   else // nếu đường cong
   {
-    if(fabs(drive_cmd.x) < min_speed_xy_)
+    double v_min = std::clamp(fabs(drive_cmd.x), min_approach_linear_velocity_, min_speed_xy_);
-      drive_cmd.x = std::copysign(min_speed_xy_, sign_x);
+    drive_cmd.x = std::copysign(v_min, sign_x);
     return generateHermiteQuadraticTrajectory(path.poses.back(), sign_x);
   }
 }

src/Algorithms/Packages/global_planners/two_points_planner/src/two_points_planner.cpp

@@ -176,7 +176,9 @@ namespace two_points_planner
     const double dy = goal.pose.position.y - start.pose.position.y;
     const double distance = std::sqrt(dx * dx + dy * dy);
     double theta;
-
+    // Lấy độ phân giải của costmap
+    double resolution = costmap_robot_->getCostmap()->getResolution();
+    
     if(fabs(dx) > 1e-9 || fabs(dy) > 1e-9)
     {
       theta = std::atan2(dy, dx);
@@ -189,16 +191,12 @@ namespace two_points_planner
     else 
     { 
       robot_geometry_msgs::PoseStamped pose = start;
-      pose.pose.position.x += 0.01 * cos(theta);
+      pose.pose.position.x += resolution * cos(theta);
-      pose.pose.position.y += 0.01 * sin(theta);
+      pose.pose.position.y += resolution * sin(theta);
       plan.push_back(pose);
       plan.push_back(goal);
       return true;
     }
-
-    // Lấy độ phân giải của costmap
-    double resolution = costmap_robot_->getCostmap()->getResolution();
-
     // Tính số điểm cần chia
     int num_points = std::ceil(distance / resolution);
 

src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_docking_local_planner.cpp

@@ -250,17 +250,12 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::reset()
   if(rotate_algorithm_) rotate_algorithm_->reset();
   ret_nav_ = ret_angle_ = false;
 
-  robot::log_info_at(__FILE__, __LINE__, "Debug");
-  parent_.printParams();
   std::string algorithm_nav_name;
   planner_nh_.param("algorithm_nav_name", algorithm_nav_name, std::string("pnkx_local_planner::PTA"));
 
   parent_.setParam(algorithm_nav_name, original_papams_);
   robot::NodeHandle nh_algorithm = robot::NodeHandle(parent_, algorithm_nav_name);
   nh_algorithm.setParam("allow_rotate", false);
-
-  robot::log_info_at(__FILE__, __LINE__, "Debug ở đây");
-  parent_.printParams();
 }
 
 void pnkx_local_planner::PNKXDockingLocalPlanner::prepare(const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity)
@@ -455,7 +450,6 @@ bool pnkx_local_planner::PNKXDockingLocalPlanner::isGoalReached(const robot_nav_
     {
       robot::log_info_at(__FILE__, __LINE__, "local_pose %f %f %f", local_pose.pose.x, local_pose.pose.y, local_pose.pose.theta);
       robot::log_info_at(__FILE__, __LINE__, "local_goal %f %f %f", local_goal.pose.x, local_goal.pose.y, local_goal.pose.theta);
-      robot::log_info_at(__FILE__, __LINE__, "goal_pose_ %f %f %f", goal_pose_.pose.x, goal_pose_.pose.y, goal_pose_.pose.theta);
     }
   }
 
@@ -801,7 +795,8 @@ bool pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::getLocalPath(
   robot_geometry_msgs::PoseStamped start = robot_nav_2d_utils::pose2DToPoseStamped(local_pose);
   robot_geometry_msgs::PoseStamped goal = robot_nav_2d_utils::pose2DToPoseStamped(local_goal);
   std::vector<robot_geometry_msgs::PoseStamped> docking_plan;
-
+  robot::log_info_at(__FILE__, __LINE__, "start %s %f %f", start.header.frame_id.c_str(), start.pose.position.x, start.pose.position.y);
+  robot::log_info_at(__FILE__, __LINE__, "goal %s %f %f", goal.header.frame_id.c_str(), goal.pose.position.x, goal.pose.position.y);
   if (!docking_planner_->makePlan(start, goal, docking_plan))
   {
     throw robot_nav_core2::LocalPlannerException("Making plan from goal maker is failed");