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

update khi lui

Browse Source
This commit is contained in:
HiepLM
2026-04-23 17:11:47 +07:00
parent 274d3dd858
commit 5d4d77155b
4 changed files with 26 additions and 25 deletions
Download Patch File Download Diff File Expand all files Collapse all files

28
src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_go_straight.cpp
View File

@@ -131,19 +131,19 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::GoStraight::calculator(
auto carrot_pose = *getLookAheadPoint(velocity, lookahead_dist, transformed_plan);
robot_geometry_msgs::PoseStamped carrot_pose_stamped = robot_nav_2d_utils::pose2DToPoseStamped(carrot_pose);
// === Final Heading Alignment Check ===
double xy_error = 0.0, heading_error = 0.0;
if (shouldAlignToFinalHeading(transformed_plan, carrot_pose, velocity, xy_error, heading_error, sign_x))
{
// Use Arc Motion controller for final heading alignment
alignToFinalHeading(xy_error, heading_error, velocity, sign_x, dt, drive_cmd);
#ifdef BUILD_WITH_ROS
ROS_INFO("xy_err=%.3f, heading_err=%.3f deg, v=%.3f, w_current=%.3f, w_target=%.3f",
xy_error, heading_error * 180.0 / M_PI, drive_cmd.x, velocity.theta, drive_cmd.theta);
#endif
}
else
{
// // === Final Heading Alignment Check ===
// double xy_error = 0.0, heading_error = 0.0;
// if (shouldAlignToFinalHeading(transformed_plan, carrot_pose, velocity, xy_error, heading_error, sign_x))
// {
// // Use Arc Motion controller for final heading alignment
// alignToFinalHeading(xy_error, heading_error, velocity, sign_x, dt, drive_cmd);
// #ifdef BUILD_WITH_ROS
// ROS_INFO("xy_err=%.3f, heading_err=%.3f deg, v=%.3f, w_current=%.3f, w_target=%.3f",
// xy_error, heading_error * 180.0 / M_PI, drive_cmd.x, velocity.theta, drive_cmd.theta);
// #endif
// }
// else
// {
// robot::log_info_at(__FILE__, __LINE__, "journey : %f lookahead_dist : %f",
// journey(transformed_plan.poses, 0, transformed_plan.poses.size() - 1), lookahead_dist);
if(fabs(carrot_pose.pose.y) > 0.2)
@@ -164,7 +164,7 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::GoStraight::calculator(
sign_x,
dt,
drive_cmd);
}
// }
applyDistanceSpeedScaling(compute_plan_, velocity, drive_cmd, sign_x, dt);
if (this->nav_stop_)

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

@@ -682,7 +682,7 @@ void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit(
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, -0.03, 0.03);
w_target = std::clamp(w_target, -fabs(drive_target.theta), fabs(drive_target.theta));
}
else
{
@@ -722,7 +722,8 @@ void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit(
drive_cmd.x = std::clamp(kf_->state()[0], -fabs(v_target), fabs(v_target));
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], -max_vel_theta_, max_vel_theta_);
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);
}
void mkt_algorithm::diff::PredictiveTrajectory::applyDistanceSpeedScaling(
@@ -1265,8 +1266,9 @@ robot_nav_2d_msgs::Path2D mkt_algorithm::diff::PredictiveTrajectory::generateTra
// nếu đường thẳng
if (max_kappa <= straight_threshold)
{
if(fabs(path.poses.back().pose.x) * 0.9 < min_lookahead_dist_)
if(fabs(path.poses.back().pose.x) * 0.9 < min_lookahead_dist_ && fabs(path.poses.back().pose.theta) < 0.1)
{
drive_cmd.theta = 0.01;
return generateParallelPath(path, sign_x);
}
return generateHermiteTrajectory(path, sign_x);
@@ -1277,12 +1279,7 @@ robot_nav_2d_msgs::Path2D mkt_algorithm::diff::PredictiveTrajectory::generateTra
const double v_min = std::min(fabs(v_limited), min_speed_xy_);
if(fabs(drive_cmd.x) < v_min)
{
robot_nav_2d_msgs::Twist2D vel_in, v_out;
vel_in.x = v_min;
vel_in.y = 0.0;
vel_in.theta = drive_cmd.theta;
moveWithAccLimits(velocity, vel_in, v_out, dt);
drive_cmd.x = std::copysign(v_out.x, sign_x);
drive_cmd.x = std::copysign(v_min, sign_x);
}
return generateHermiteQuadraticTrajectory(path, sign_x);
}