update thuat toan pp

src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_predictive_trajectory.h

@@ -393,13 +393,8 @@ namespace mkt_algorithm
       double near_goal_heading_integral_;
       double near_goal_heading_last_error_;
       bool near_goal_heading_was_active_;
-
+      
-      // Regulated linear velocity scaling
-      bool use_regulated_linear_velocity_scaling_;
-
       double min_approach_linear_velocity_;
-      double regulated_linear_scaling_min_radius_;
-      double regulated_linear_scaling_min_speed_;
 
       bool use_cost_regulated_linear_velocity_scaling_;
       double inflation_cost_scaling_factor_;

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

@@ -28,10 +28,7 @@ mkt_algorithm::diff::PredictiveTrajectory::PredictiveTrajectory()
     near_goal_heading_integral_(0.0),
     near_goal_heading_last_error_(0.0),
     near_goal_heading_was_active_(false),
-    use_regulated_linear_velocity_scaling_(false),
     min_approach_linear_velocity_(0.0),
-    regulated_linear_scaling_min_radius_(0.0),
-    regulated_linear_scaling_min_speed_(0.0),
     use_cost_regulated_linear_velocity_scaling_(false),
     inflation_cost_scaling_factor_(0.0),
     cost_scaling_dist_(0.0),
@@ -169,11 +166,6 @@ void mkt_algorithm::diff::PredictiveTrajectory::getParams()
   if (trans_stopped_velocity_ > min_approach_linear_velocity_)
     trans_stopped_velocity_ = min_approach_linear_velocity_ + 0.01;
 
-  // Regulated linear velocity scaling
-  nh_priv_.param<bool>("use_regulated_linear_velocity_scaling", use_regulated_linear_velocity_scaling_, false);
-  nh_priv_.param<double>("regulated_linear_scaling_min_radius", regulated_linear_scaling_min_radius_, 0.9);
-  nh_priv_.param<double>("regulated_linear_scaling_min_speed", regulated_linear_scaling_min_speed_, 0.25);
-
   // Inflation cost scaling (Limit velocity by proximity to obstacles)
   nh_priv_.param<bool>("use_cost_regulated_linear_velocity_scaling", use_cost_regulated_linear_velocity_scaling_, true);
   nh_priv_.param<double>("inflation_cost_scaling_factor", inflation_cost_scaling_factor_, 3.0);
@@ -205,6 +197,15 @@ void mkt_algorithm::diff::PredictiveTrajectory::getParams()
     traj_.get()->getNodeHandle().param<double>("acc_lim_theta", acc_lim_theta_, 0.0);
     traj_.get()->getNodeHandle().param<double>("decel_lim_theta", decel_lim_theta_, 0.0);
     traj_.get()->getNodeHandle().param<double>("min_speed_xy", min_speed_xy_, 0.0);
+    if(fabs(min_speed_xy_) > sqrt(max_vel_x_ * max_vel_x_ + max_vel_y_ * max_vel_y_))
+    {
+      min_speed_xy_ = sqrt(max_vel_x_ * max_vel_x_ + max_vel_y_ * max_vel_y_);
+    }
+    if(fabs(min_speed_xy_) > sqrt(min_vel_x_ * min_vel_x_ + min_vel_y_ * min_vel_y_))
+    {
+      min_speed_xy_ = sqrt(max_vel_x_ * max_vel_x_ + max_vel_y_ * max_vel_y_);
+    }
+
     traj_.get()->getNodeHandle().param<double>("max_speed_xy", max_speed_xy_, 0.0);
   }
 }
@@ -545,7 +546,6 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
       drive_cmd);
     }
     applyDistanceSpeedScaling(compute_plan_, velocity, drive_cmd, sign_x, dt);
-
     if (this->nav_stop_)
     {
       if (!stopped(velocity, rot_stopped_velocity_, trans_stopped_velocity_))
@@ -578,7 +578,6 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
     // drive_cmd.x = fabs(drive_cmd.x) >= v_min ? drive_cmd.x : std::copysign(v_min, sign_x);
     // drive_cmd.theta = std::clamp(kf_->state()[3], -max_vel_theta_, max_vel_theta_);
   }
-
   result.poses.clear();
   result.poses.reserve(transformed_plan.poses.size());
   for (const auto &pose_stamped : transformed_plan.poses)
@@ -618,7 +617,6 @@ void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit(
   const double L_min = 0.1;  // m, chỉnh theo nhu cầu
   double scale_close = std::clamp(L / L_min, 0.0, 1.0);
   v_target *= scale_close;
-
   const double y_abs = std::fabs(carrot_pose.pose.y);
   const double y_soft = 0.1;
   if (y_abs > y_soft) 
@@ -1196,6 +1194,7 @@ double mkt_algorithm::diff::PredictiveTrajectory::adjustSpeedWithHermiteTrajecto
     const double v_stop = std::sqrt(2.0 * fabs(decel_lim_x_) * std::max(0.0, remaining));
     v_limit = std::min(v_limit, v_stop);
   }
+  v_limit = std::min(v_limit, fabs(v_target));
 
   return std::copysign(v_limit, sign_x);
 }