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

update reduce speed

Browse Source
This commit is contained in:
HiepLM
2026-04-24 12:32:47 +07:00
parent 498a85a199
commit 9c14054d3f
2 changed files with 9 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
config/pnkx_local_planner_params.yaml
View File

@@ -53,12 +53,12 @@ LimitedAccelGenerator:
max_vel_theta: 0.4 # max_rot_vel: 1.0 # choose slightly less than the base's capability
min_vel_theta: 0.05 # min_rot_vel: 0.1 default: 0.4 # this is the min angular velocity when there is negligible translational velocity
acc_lim_x: 1.5
acc_lim_x: 3.0
acc_lim_y: 0.0 # diff drive robot
acc_lim_theta: 1.5
decel_lim_x: -1.5
decel_lim_y: -0.0
decel_lim_theta: -1.5
decel_lim_theta: -3.0
# Whether to split the path into segments or not
split_path: true

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

@@ -522,9 +522,7 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
allow_rotate |=
(path_distance_to_rotate >= distance_allow_rotate &&
fabs(transformed_plan.poses.front().pose.y * cos(transformed_plan.poses.front().pose.theta)) <= 0.1);
// allow_rotate &= std::hypot(compute_plan_.poses.front().pose.x - pose.pose.x, compute_plan_.poses.front().pose.y - pose.pose.y) <= 0.1;
// allow_rotate &= fabs(transformed_plan.poses.front().pose.y) <= 0.1;
// robot::log_info("%f front.y %f", path_distance_to_rotate, fabs(transformed_plan.poses.front().pose.y * cos(transformed_plan.poses.front().pose.theta)));
double angle_to_heading;
if (allow_rotate && shouldRotateToPath(transformed_plan, carrot_pose, velocity, angle_to_heading, sign_x))
{
@@ -581,6 +579,7 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
result.velocity = drive_cmd;
return result;
}
}
result.poses.clear();
result.poses.reserve(transformed_plan.poses.size());
@@ -643,9 +642,6 @@ void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit(
// 4) Maintain minimum approach speed
if (std::fabs(v_target) < min_approach_linear_velocity)
v_target = std::copysign(min_approach_linear_velocity, sign_x);
std::stringstream ss;
// 5) Angular speed from curvature
double w_target = v_target * kappa;
if(journey(trajectory.poses, 0, trajectory.poses.size() - 1) <= min_journey_squared_)
@@ -663,7 +659,6 @@ void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit(
if(fabs(dx) < 1e-6 && fabs(dy) < 1e-6)
continue;
heading_ref = std::atan2(dy, dx);
ss << "error " << heading_ref << " ";
if(sign_x < 0.0)
heading_ref += std::copysign(M_PI, heading_ref) * (-1.0);
break;
@@ -671,7 +666,6 @@ void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit(
}
const double error = heading_ref;
ss << error << " ";
double w_heading = 0.0;
pid(error,
near_goal_heading_integral_,
@@ -683,7 +677,6 @@ void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit(
w_heading);
// Apply acceleration limits
double dw_heading = std::clamp(w_heading - velocity.theta, -acc_lim_theta_ * dt, acc_lim_theta_ * dt);
ss << "dw_heading " << dw_heading << " ";
w_target = velocity.theta + dw_heading;
w_target = std::clamp(w_target, -fabs(drive_target.theta), fabs(drive_target.theta));
}
@@ -697,7 +690,6 @@ void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit(
{
near_goal_heading_was_active_ = false;
}
w_target = std::clamp(w_target, -fabs(drive_target.theta), fabs(drive_target.theta));
// 6) Apply acceleration limits (linear + angular)
@@ -726,7 +718,7 @@ void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit(
drive_cmd.x = fabs(drive_cmd.x) >= v_min ? drive_cmd.x : std::copysign(v_min, sign_x);
if (kf_filter_angular_)
drive_cmd.theta = std::clamp(kf_->state()[3], -fabs(drive_target.theta), fabs(drive_target.theta));
robot::log_info("drive_cmd.theta: %f, drive_target.theta: %f", drive_cmd.theta, drive_target.theta);
// robot::log_info("drive_cmd.theta: %f, drive_target.theta: %f", drive_cmd.theta, drive_target.theta);
}
void mkt_algorithm::diff::PredictiveTrajectory::applyDistanceSpeedScaling(
@@ -1261,11 +1253,11 @@ robot_nav_2d_msgs::Path2D mkt_algorithm::diff::PredictiveTrajectory::generateTra
drive_cmd.theta = 0.0;
return robot_nav_2d_msgs::Path2D();
}
drive_cmd.x = drive_target.x;
drive_cmd.theta = max_vel_theta_;
double max_kappa = calculateMaxKappa(path);
const double straight_threshold = std::max(0.05, 2.0 * (costmap_robot_ ? costmap_robot_->getCostmap()->getResolution() : 0.05));
drive_cmd.x = this->adjustSpeedWithHermiteTrajectory(velocity, path, drive_target.x, sign_x);
drive_cmd.theta = max_vel_theta_;
// nếu đường thẳng
if (max_kappa <= straight_threshold)
{