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

update thuat toan moi

Browse Source
This commit is contained in:
HiepLM 2026-01-28 10:54:04 +07:00
parent 620db96de0
commit 85789855a8
2 changed files with 61 additions and 39 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
config/pnkx_local_planner_params.yaml
View File

@ -85,9 +85,9 @@ MKTAlgorithmDiffPredictiveTrajectory:
# only when false:
lookahead_dist: 0.5 # The lookahead distance (m) to use to find the lookahead point. (default: 0.6)
# only when true:
min_lookahead_dist: 0.6 # The minimum lookahead distance (m) threshold. (default: 0.3)
min_lookahead_dist: 0.4 # The minimum lookahead distance (m) threshold. (default: 0.3)
max_lookahead_dist: 2.0 # The maximum lookahead distance (m) threshold. (default: 0.9)
lookahead_time: 1.9 # The time (s) to project the velocity by, a.k.a. lookahead gain. (default: 1.5)
lookahead_time: 2.0 # The time (s) to project the velocity by, a.k.a. lookahead gain. (default: 1.5)
min_journey_squared: 0.3 # Minimum squared journey to consider for goal (default: 0.2)
max_journey_squared: 0.8 # Maximum squared journey to consider for goal (default: 0.2)
max_lateral_accel: 2.0 # Max lateral accel for speed reduction on curves (m/s^2)

94
src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory_.cpp
View File

@ -280,11 +280,12 @@ bool mkt_algorithm::diff::PredictiveTrajectory::prepare(const robot_nav_2d_msgs:
double lookahead_dist = this->getLookAheadDistance(velocity);
transform_plan_.poses.clear();
if (!this->transformGlobalPlan(tf_, compute_plan_, pose, costmap_robot_, robot_base_frame_, lookahead_dist, transform_plan_))
if (!this->transformGlobalPlan(tf_, compute_plan_, pose, costmap_robot_, robot_base_frame_, sqrt(lookahead_dist * lookahead_dist + 0.25), transform_plan_))
{
robot::log_warning("[%s:%d]\n Could not transform the global plan to the frame of the controller", __FILE__, __LINE__);
return false;
}
x_direction = x_direction_;
y_direction = y_direction_ = 0;
theta_direction = theta_direction_;
@ -394,6 +395,12 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
double lookahead_dist = getLookAheadDistance(velocity);
auto carrot_pose = *getLookAheadPoint(velocity, lookahead_dist, transformed_plan);
if(fabs(carrot_pose.pose.y) > 0.2)
{
lookahead_dist = sqrt(carrot_pose.pose.y *carrot_pose.pose.y + lookahead_dist * lookahead_dist);
carrot_pose = *getLookAheadPoint(velocity, lookahead_dist, transform_plan_);
}
bool allow_rotate = false;
nh_priv_.param("allow_rotate", allow_rotate, false);
@ -487,6 +494,10 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
result.poses.reserve(transformed_plan.poses.size());
for (const auto &pose_stamped : transformed_plan.poses)
{
if(fabs(pose_stamped.pose.x - carrot_pose.pose.x) < 1e-6 &&
fabs(pose_stamped.pose.y - carrot_pose.pose.y) < 1e-6 &&
fabs(pose_stamped.pose.theta - carrot_pose.pose.theta) < 1e-6)
break;
result.poses.push_back(pose_stamped.pose);
}
result.velocity = drive_cmd;
@ -587,7 +598,6 @@ double mkt_algorithm::diff::PredictiveTrajectory::adjustSpeedWithHermiteTrajecto
if (traveled >= preview_dist)
break;
}
robot::log_info("max_kappa: %f", max_kappa);
double v_limit = std::fabs(v_target);
if (max_kappa > 1e-6 && max_lateral_accel_ > 1e-6)
{
@ -634,6 +644,18 @@ void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit(
// 3) Adjust speed using Hermite trajectory curvature + remaining distance
double v_target = adjustSpeedWithHermiteTrajectory(velocity, transform_plan_, v_max, sign_x);
const double y_abs = std::fabs(carrot_pose.pose.y);
const double y_soft = 0.1;
if (y_abs > y_soft)
{
double scale = y_soft / y_abs; // y càng lớn => scale càng nhỏ
scale = std::clamp(scale, 0.2, 1.0); // không giảm quá sâu
v_target *= scale;
robot_nav_2d_msgs::Twist2D cmd, result;
cmd.x = v_target;
this->moveWithAccLimits(velocity, cmd, result);
v_target = result.x;
}
robot::log_info("v_max: %f, v_target: %f", v_max , v_target);
// 4) Maintain minimum approach speed
@ -881,48 +903,48 @@ mkt_algorithm::diff::PredictiveTrajectory::getLookAheadPoint(const robot_nav_2d_
auto goal_pose_it = std::prev(global_plan.poses.end());
return goal_pose_it;
}
unsigned int goal_index = (unsigned)global_plan.poses.size() - 1;
// double start_direction_x = global_plan.poses[1].pose.x - global_plan.poses[0].pose.x;
// double start_direction_y = global_plan.poses[1].pose.y - global_plan.poses[0].pose.y;
// double end_direction_x = global_plan.poses[goal_index].pose.x - global_plan.poses[goal_index - 1].pose.x;
// double end_direction_y = global_plan.poses[goal_index].pose.y - global_plan.poses[goal_index - 1].pose.y;
double start_direction_x = global_plan.poses[1].pose.x - global_plan.poses[0].pose.x;
double start_direction_y = global_plan.poses[1].pose.y - global_plan.poses[0].pose.y;
double end_direction_x = global_plan.poses[goal_index].pose.x - global_plan.poses[goal_index - 1].pose.x;
double end_direction_y = global_plan.poses[goal_index].pose.y - global_plan.poses[goal_index - 1].pose.y;
// // make sure that atan2 is defined
// double start_angle = atan2(start_direction_y, start_direction_x);
// double end_angle = atan2(end_direction_y, end_direction_x);
// double permition_threshold = std::clamp(fabs(end_angle - start_angle) / 2.0, 0.32, M_PI_2);
// make sure that atan2 is defined
double start_angle = atan2(start_direction_y, start_direction_x);
double end_angle = atan2(end_direction_y, end_direction_x);
double permition_threshold = std::clamp(fabs(end_angle - start_angle) / 2.0, 0.32, M_PI_2);
// for (unsigned int i = 1; i < (int)global_plan.poses.size(); i++)
// {
// if (fabs(start_direction_x) > 1e-9 || fabs(start_direction_y) > 1e-9)
// {
// const double current_direction_x = global_plan.poses[i].pose.x - global_plan.poses[i - 1].pose.x;
// const double current_direction_y = global_plan.poses[i].pose.y - global_plan.poses[i - 1].pose.y;
for (unsigned int i = 1; i < (int)global_plan.poses.size(); i++)
{
if (fabs(start_direction_x) > 1e-9 || fabs(start_direction_y) > 1e-9)
{
const double current_direction_x = global_plan.poses[i].pose.x - global_plan.poses[i - 1].pose.x;
const double current_direction_y = global_plan.poses[i].pose.y - global_plan.poses[i - 1].pose.y;
// if (fabs(current_direction_x) > 1e-9 || fabs(current_direction_y) > 1e-9)
// {
// double current_angle = atan2(current_direction_y, current_direction_x);
// goal_index = i;
// if (fabs(velocity.x) <= trans_stopped_velocity_ && fabs(velocity.theta) >= (0.8 * min_vel_theta_))
// continue;
if (fabs(current_direction_x) > 1e-9 || fabs(current_direction_y) > 1e-9)
{
double current_angle = atan2(current_direction_y, current_direction_x);
goal_index = i;
if (fabs(velocity.x) <= trans_stopped_velocity_ && fabs(velocity.theta) >= (0.8 * min_vel_theta_))
continue;
if (fabs(remainder(start_angle - current_angle, 2 * M_PI)) >= permition_threshold)
{
goal_index = i;
break;
}
}
}
}
// if (fabs(remainder(start_angle - current_angle, 2 * M_PI)) >= permition_threshold)
// {
// goal_index = i;
// break;
// }
// }
// }
// }
// Find the first pose which is at a distance greater than the lookahead distance
auto goal_pose_it = std::find_if(
global_plan.poses.begin(), global_plan.poses.begin() + goal_index,
[&](const auto &ps)
{
return std::hypot(ps.pose.x, ps.pose.y) >= lookahead_dist;
});
global_plan.poses.begin(), global_plan.poses.begin() + goal_index,
[&](const auto &ps)
{
return std::hypot(ps.pose.x, ps.pose.y) >= lookahead_dist;
});
// If the number of poses is not far enough, take the last pose
if (goal_pose_it == global_plan.poses.end())