HiepLM/pnkx_nav_core
1
1
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

update thuat toan pp

Browse Source
This commit is contained in:
HiepLM
2026-03-12 10:29:55 +07:00
parent ae2f647fc9
commit 75cbf5a7ef
2 changed files with 11 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files

5
src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_predictive_trajectory.h
View File

@@ -394,12 +394,7 @@ namespace mkt_algorithm
double near_goal_heading_last_error_; double near_goal_heading_last_error_;
bool near_goal_heading_was_active_; bool near_goal_heading_was_active_;
// Regulated linear velocity scaling
bool use_regulated_linear_velocity_scaling_;
double min_approach_linear_velocity_; double min_approach_linear_velocity_;
double regulated_linear_scaling_min_radius_;
double regulated_linear_scaling_min_speed_;
bool use_cost_regulated_linear_velocity_scaling_; bool use_cost_regulated_linear_velocity_scaling_;
double inflation_cost_scaling_factor_; double inflation_cost_scaling_factor_;

21
src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory.cpp
View File

@@ -28,10 +28,7 @@ mkt_algorithm::diff::PredictiveTrajectory::PredictiveTrajectory()
near_goal_heading_integral_(0.0), near_goal_heading_integral_(0.0),
near_goal_heading_last_error_(0.0), near_goal_heading_last_error_(0.0),
near_goal_heading_was_active_(false), near_goal_heading_was_active_(false),
use_regulated_linear_velocity_scaling_(false),
min_approach_linear_velocity_(0.0), 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), use_cost_regulated_linear_velocity_scaling_(false),
inflation_cost_scaling_factor_(0.0), inflation_cost_scaling_factor_(0.0),
cost_scaling_dist_(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_) if (trans_stopped_velocity_ > min_approach_linear_velocity_)
trans_stopped_velocity_ = min_approach_linear_velocity_ + 0.01; 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) // 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<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); 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>("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>("decel_lim_theta", decel_lim_theta_, 0.0);
traj_.get()->getNodeHandle().param<double>("min_speed_xy", min_speed_xy_, 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); 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); drive_cmd);
} }
applyDistanceSpeedScaling(compute_plan_, velocity, drive_cmd, sign_x, dt); applyDistanceSpeedScaling(compute_plan_, velocity, drive_cmd, sign_x, dt);
if (this->nav_stop_) if (this->nav_stop_)
{ {
if (!stopped(velocity, rot_stopped_velocity_, trans_stopped_velocity_)) 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.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_); // drive_cmd.theta = std::clamp(kf_->state()[3], -max_vel_theta_, max_vel_theta_);
} }
result.poses.clear(); result.poses.clear();
result.poses.reserve(transformed_plan.poses.size()); result.poses.reserve(transformed_plan.poses.size());
for (const auto &pose_stamped : transformed_plan.poses) 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 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); double scale_close = std::clamp(L / L_min, 0.0, 1.0);
v_target *= scale_close; v_target *= scale_close;
const double y_abs = std::fabs(carrot_pose.pose.y); const double y_abs = std::fabs(carrot_pose.pose.y);
const double y_soft = 0.1; const double y_soft = 0.1;
if (y_abs > y_soft) 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)); 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, v_stop);
} }
v_limit = std::min(v_limit, fabs(v_target));
return std::copysign(v_limit, sign_x); return std::copysign(v_limit, sign_x);
} }