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: # only when false:
lookahead_dist: 0.5 # The lookahead distance (m) to use to find the lookahead point. (default: 0.6) lookahead_dist: 0.5 # The lookahead distance (m) to use to find the lookahead point. (default: 0.6)
# only when true: # 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) 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) 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_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) 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); double lookahead_dist = this->getLookAheadDistance(velocity);
transform_plan_.poses.clear(); 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__); robot::log_warning("[%s:%d]\n Could not transform the global plan to the frame of the controller", __FILE__, __LINE__);
return false; return false;
} }
x_direction = x_direction_; x_direction = x_direction_;
y_direction = y_direction_ = 0; y_direction = y_direction_ = 0;
theta_direction = theta_direction_; theta_direction = theta_direction_;
@ -394,6 +395,12 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
double lookahead_dist = getLookAheadDistance(velocity); double lookahead_dist = getLookAheadDistance(velocity);
auto carrot_pose = *getLookAheadPoint(velocity, lookahead_dist, transformed_plan); 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; bool allow_rotate = false;
nh_priv_.param("allow_rotate", 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()); result.poses.reserve(transformed_plan.poses.size());
for (const auto &pose_stamped : transformed_plan.poses) 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.poses.push_back(pose_stamped.pose);
} }
result.velocity = drive_cmd; result.velocity = drive_cmd;
@ -587,7 +598,6 @@ double mkt_algorithm::diff::PredictiveTrajectory::adjustSpeedWithHermiteTrajecto
if (traveled >= preview_dist) if (traveled >= preview_dist)
break; break;
} }
robot::log_info("max_kappa: %f", max_kappa);
double v_limit = std::fabs(v_target); double v_limit = std::fabs(v_target);
if (max_kappa > 1e-6 && max_lateral_accel_ > 1e-6) 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 // 3) Adjust speed using Hermite trajectory curvature + remaining distance
double v_target = adjustSpeedWithHermiteTrajectory(velocity, transform_plan_, v_max, sign_x); 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); robot::log_info("v_max: %f, v_target: %f", v_max , v_target);
// 4) Maintain minimum approach speed // 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()); auto goal_pose_it = std::prev(global_plan.poses.end());
return goal_pose_it; return goal_pose_it;
} }
unsigned int goal_index = (unsigned)global_plan.poses.size() - 1; 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; // // make sure that atan2 is defined
double start_direction_y = global_plan.poses[1].pose.y - global_plan.poses[0].pose.y; // double start_angle = atan2(start_direction_y, start_direction_x);
double end_direction_x = global_plan.poses[goal_index].pose.x - global_plan.poses[goal_index - 1].pose.x; // double end_angle = atan2(end_direction_y, end_direction_x);
double end_direction_y = global_plan.poses[goal_index].pose.y - global_plan.poses[goal_index - 1].pose.y; // double permition_threshold = std::clamp(fabs(end_angle - start_angle) / 2.0, 0.32, M_PI_2);
// make sure that atan2 is defined // for (unsigned int i = 1; i < (int)global_plan.poses.size(); i++)
double start_angle = atan2(start_direction_y, start_direction_x); // {
double end_angle = atan2(end_direction_y, end_direction_x); // if (fabs(start_direction_x) > 1e-9 || fabs(start_direction_y) > 1e-9)
double permition_threshold = std::clamp(fabs(end_angle - start_angle) / 2.0, 0.32, M_PI_2); // {
// 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(current_direction_x) > 1e-9 || fabs(current_direction_y) > 1e-9)
{ // {
if (fabs(start_direction_x) > 1e-9 || fabs(start_direction_y) > 1e-9) // double current_angle = atan2(current_direction_y, current_direction_x);
{ // goal_index = i;
const double current_direction_x = global_plan.poses[i].pose.x - global_plan.poses[i - 1].pose.x; // if (fabs(velocity.x) <= trans_stopped_velocity_ && fabs(velocity.theta) >= (0.8 * min_vel_theta_))
const double current_direction_y = global_plan.poses[i].pose.y - global_plan.poses[i - 1].pose.y; // continue;
if (fabs(current_direction_x) > 1e-9 || fabs(current_direction_y) > 1e-9) // if (fabs(remainder(start_angle - current_angle, 2 * M_PI)) >= permition_threshold)
{ // {
double current_angle = atan2(current_direction_y, current_direction_x); // goal_index = i;
goal_index = i; // break;
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;
}
}
}
}
// Find the first pose which is at a distance greater than the lookahead distance // Find the first pose which is at a distance greater than the lookahead distance
auto goal_pose_it = std::find_if( auto goal_pose_it = std::find_if(
global_plan.poses.begin(), global_plan.poses.begin() + goal_index, global_plan.poses.begin(), global_plan.poses.begin() + goal_index,
[&](const auto &ps) [&](const auto &ps)
{ {
return std::hypot(ps.pose.x, ps.pose.y) >= lookahead_dist; 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 the number of poses is not far enough, take the last pose
if (goal_pose_it == global_plan.poses.end()) if (goal_pose_it == global_plan.poses.end())