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

fix

Browse Source
This commit is contained in:
HiepLM
2026-03-26 14:42:26 +07:00
parent ba503eca85
commit 58d925f2be
3 changed files with 43 additions and 39 deletions
Download Patch File Download Diff File Expand all files Collapse all files

12
config/pnkx_local_planner_params.yaml
View File

@@ -106,8 +106,8 @@ MKTAlgorithmDiffPredictiveTrajectory:
angular_decel_zone: 0.1 angular_decel_zone: 0.1
# stoped # stoped
rot_stopped_velocity: 0.05 rot_stopped_velocity: 0.03
trans_stopped_velocity: 0.06 trans_stopped_velocity: 0.03
use_final_heading_alignment: true use_final_heading_alignment: true
final_heading_xy_tolerance: 0.1 final_heading_xy_tolerance: 0.1
@@ -143,8 +143,8 @@ MKTAlgorithmDiffGoStraight:
angular_decel_zone: 0.1 angular_decel_zone: 0.1
# stoped # stoped
rot_stopped_velocity: 0.05 rot_stopped_velocity: 0.03
trans_stopped_velocity: 0.06 trans_stopped_velocity: 0.03
use_final_heading_alignment: true use_final_heading_alignment: true
final_heading_xy_tolerance: 0.1 final_heading_xy_tolerance: 0.1
@@ -180,8 +180,8 @@ MKTAlgorithmDiffRotateToGoal:
angular_decel_zone: 0.1 angular_decel_zone: 0.1
# stoped # stoped
rot_stopped_velocity: 0.05 rot_stopped_velocity: 0.03
trans_stopped_velocity: 0.06 trans_stopped_velocity: 0.03
use_final_heading_alignment: true use_final_heading_alignment: true
final_heading_xy_tolerance: 0.1 final_heading_xy_tolerance: 0.1

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

@@ -543,13 +543,10 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
// } // }
// else // else
// { // {
// if(fabs(carrot_pose.pose.y) > 0.2) robot_nav_2d_msgs::Twist2D drive_target = drive_cmd;
// {
// lookahead_dist = sqrt(carrot_pose.pose.y *carrot_pose.pose.y + lookahead_dist * lookahead_dist);
// }
robot_nav_2d_msgs::Twist2D drive_target;
transformed_plan = this->generateTrajectory(transformed_plan, drive_cmd, velocity, sign_x, drive_target); transformed_plan = this->generateTrajectory(transformed_plan, drive_cmd, velocity, sign_x, drive_target);
carrot_pose = *getLookAheadPoint(velocity, lookahead_dist, transformed_plan); carrot_pose = *getLookAheadPoint(velocity, lookahead_dist, transformed_plan);
// Normal Pure Pursuit // Normal Pure Pursuit
this->computePurePursuit( this->computePurePursuit(
carrot_pose, carrot_pose,
@@ -633,28 +630,32 @@ void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit(
if (std::fabs(v_target) < min_approach_linear_velocity) if (std::fabs(v_target) < min_approach_linear_velocity)
v_target = std::copysign(min_approach_linear_velocity, sign_x); v_target = std::copysign(min_approach_linear_velocity, sign_x);
std::stringstream ss;
// 5) Angular speed from curvature // 5) Angular speed from curvature
double w_target = v_target * kappa + std::copysign(carrot_pose.pose.theta * dt, kappa); double w_target = v_target * kappa;
if(journey(trajectory.poses, 0, trajectory.poses.size() - 1) <= min_journey_squared_) if(journey(trajectory.poses, 0, trajectory.poses.size() - 1) <= min_journey_squared_)
{ {
ss << w_target << " ";
if (trajectory.poses.size() >= 2) { if (trajectory.poses.size() >= 2) {
const auto& p1 = trajectory.poses[trajectory.poses.size() - 1].pose; const auto& p1 = trajectory.poses[trajectory.poses.size() - 1].pose;
double heading_ref = 0.0; double heading_ref = 0.0;
for(int i = trajectory.poses.size() - 2; i >= 0; i--) for(int i = trajectory.poses.size() - 2; i >= 0; i--)
{ {
const auto& p = trajectory.poses[i].pose; const auto& p = trajectory.poses[i].pose;
if(std::hypot(p1.x - p.x, p1.y - p.y) >= costmap_robot_->getCostmap()->getResolution()) const auto& dx = sign_x < 0.0 ? p1.x - p.x : p.x - p1.x;
const auto& dy = sign_x < 0.0 ? p1.y - p.y : p.y - p1.y;
if(std::hypot(dx, dy) >= costmap_robot_->getCostmap()->getResolution())
{ {
heading_ref = angles::normalize_angle(std::atan2(p1.y - p.y, p1.x - p.x)); if(fabs(dx) < 1e-6 && fabs(dy) < 1e-6)
if(sign_x < 0.0) continue;
{ heading_ref = angles::normalize_angle(std::atan2(dy, dx));
heading_ref = angles::normalize_angle(M_PI + heading_ref);
}
break; break;
} }
} }
const double error = angles::normalize_angle(heading_ref); const double error = angles::normalize_angle(heading_ref);
ss << "error: " << error << " ";
double w_heading = 0.0; double w_heading = 0.0;
pid(error, pid(error,
near_goal_heading_integral_, near_goal_heading_integral_,
@@ -667,6 +668,7 @@ void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit(
// Apply acceleration limits // Apply acceleration limits
double dw_heading = std::clamp(w_heading - velocity.theta, -acc_lim_theta_ * dt, acc_lim_theta_ * dt); double dw_heading = std::clamp(w_heading - velocity.theta, -acc_lim_theta_ * dt, acc_lim_theta_ * dt);
w_target = velocity.theta + dw_heading; w_target = velocity.theta + dw_heading;
ss << w_target << " ";
} }
else else
{ {
@@ -678,6 +680,7 @@ void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit(
{ {
near_goal_heading_was_active_ = false; near_goal_heading_was_active_ = false;
} }
w_target = std::clamp(w_target, -fabs(drive_target.theta), fabs(drive_target.theta)); w_target = std::clamp(w_target, -fabs(drive_target.theta), fabs(drive_target.theta));
// 6) Apply acceleration limits (linear + angular) // 6) Apply acceleration limits (linear + angular)
@@ -687,7 +690,7 @@ void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit(
drive_cmd.x = velocity.x + dv; drive_cmd.x = velocity.x + dv;
drive_cmd.theta = velocity.theta + dw; drive_cmd.theta = velocity.theta + dw;
ss << drive_cmd.theta << " ";
Eigen::VectorXd y(2); Eigen::VectorXd y(2);
y << drive_cmd.x, drive_cmd.theta; y << drive_cmd.x, drive_cmd.theta;
@@ -707,6 +710,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); drive_cmd.x = fabs(drive_cmd.x) >= v_min ? drive_cmd.x : std::copysign(v_min, sign_x);
if (kf_filter_angular_) 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], -max_vel_theta_, max_vel_theta_);
ROS_WARN_THROTTLE(0.1, "%s", ss.str().c_str());
} }
void mkt_algorithm::diff::PredictiveTrajectory::applyDistanceSpeedScaling( void mkt_algorithm::diff::PredictiveTrajectory::applyDistanceSpeedScaling(
@@ -792,16 +796,16 @@ bool mkt_algorithm::diff::PredictiveTrajectory::shouldRotateToPath(
// (is_stopped || sign(angle_to_path) * sign_x < 0 ) && fabs(angle_to_path) > heading_rotate; // (is_stopped || sign(angle_to_path) * sign_x < 0 ) && fabs(angle_to_path) > heading_rotate;
bool result = use_rotate_to_heading_ && fabs(angle_to_path) > heading_rotate; bool result = use_rotate_to_heading_ && fabs(angle_to_path) > heading_rotate;
// #ifdef BUILD_WITH_ROS #ifdef BUILD_WITH_ROS
// if (result) if (result)
// ROS_WARN_THROTTLE(0.1, "angle_to_path: %f, heading_rotate: %f, is_stopped: %x %x, sign_x: %f", angle_to_path, heading_rotate, is_stopped, sign(angle_to_path) * sign_x < 0, sign_x); ROS_WARN_THROTTLE(0.1, "angle_to_path: %f, heading_rotate: %f, is_stopped: %x %x, sign_x: %f", angle_to_path, heading_rotate, is_stopped, sign(angle_to_path) * sign_x < 0, sign_x);
// else if(fabs(velocity.x) < min_speed_xy_) // else if(fabs(velocity.x) < min_speed_xy_)
// { // {
// ROS_INFO_THROTTLE(0.1, "velocity.x: %f, velocity.theta: %f, ", velocity.x, velocity.theta); // ROS_INFO_THROTTLE(0.1, "velocity.x: %f, velocity.theta: %f, ", velocity.x, velocity.theta);
// ROS_INFO_THROTTLE(0.1, "angle_to_path: %f, heading_rotate: %f, is_stopped: %x %x, sign_x: %f", angle_to_path, heading_rotate, is_stopped, sign(angle_to_path) * sign_x < 0, sign_x); // ROS_INFO_THROTTLE(0.1, "angle_to_path: %f, heading_rotate: %f, is_stopped: %x %x, sign_x: %f", angle_to_path, heading_rotate, is_stopped, sign(angle_to_path) * sign_x < 0, sign_x);
// } // }
// #endif #endif
return result; return result;
} }
@@ -883,13 +887,11 @@ bool mkt_algorithm::diff::PredictiveTrajectory::shouldAlignToFinalHeading(
for(int i = trajectory.poses.size() - 2; i >= 0; i--) for(int i = trajectory.poses.size() - 2; i >= 0; i--)
{ {
const auto& p = trajectory.poses[i].pose; const auto& p = trajectory.poses[i].pose;
if(std::hypot(p1.x - p.x, p1.y - p.y) >= costmap_robot_->getCostmap()->getResolution()) const auto& dx = sign_x < 0.0 ? p1.x - p.x : p.x - p1.x;
const auto& dy = sign_x < 0.0 ? p1.y - p.y : p.y - p1.y;
if(std::hypot(dx, dy) >= costmap_robot_->getCostmap()->getResolution())
{ {
heading_error = angles::normalize_angle(std::atan2(p1.y - p.y, p1.x - p.x)); heading_error = angles::normalize_angle(std::atan2(dy, dx));
if(sign_x < 0.0)
{
heading_error = angles::normalize_angle(M_PI + heading_error);
}
break; break;
} }
} }
@@ -1239,7 +1241,7 @@ robot_nav_2d_msgs::Path2D mkt_algorithm::diff::PredictiveTrajectory::generateTra
if (max_kappa <= straight_threshold && fabs(path.poses.back().pose.x) < min_lookahead_dist_) // nếu đường thẳng if (max_kappa <= straight_threshold && fabs(path.poses.back().pose.x) < min_lookahead_dist_) // nếu đường thẳng
{ {
if(fabs(path.poses.front().pose.y) <= 0.03 && fabs(path.poses.front().pose.x) < (min_lookahead_dist_ + max_path_distance_)) if(fabs(path.poses.front().pose.y) <= 0.03 && fabs(path.poses.back().pose.x) < (min_lookahead_dist_ + max_path_distance_))
{ {
return generateParallelPath(path, sign_x); return generateParallelPath(path, sign_x);
} }
@@ -1279,15 +1281,16 @@ robot_nav_2d_msgs::Path2D mkt_algorithm::diff::PredictiveTrajectory::generatePar
dx = path.poses[i+1].pose.x - path.poses[i-1].pose.x; dx = path.poses[i+1].pose.x - path.poses[i-1].pose.x;
dy = path.poses[i+1].pose.y - path.poses[i-1].pose.y; dy = path.poses[i+1].pose.y - path.poses[i-1].pose.y;
} }
if(fabs(dx) < 1e-6 && fabs(dy) < 1e-6)
continue;
double theta = atan2(dy, dx); double theta = atan2(dy, dx);
double x_off = p.x - offset_y * sin(theta)*sign_x; double x_off = p.x - offset_y * sin(theta)*sign_x;
double y_off = p.y - offset_y * cos(theta)*sign_x; double y_off = p.y - offset_y * cos(theta)*sign_x;
parallel_path.poses[i].header = path.poses[i].header;
parallel_path.poses[i].pose.x = x_off; parallel_path.poses[i].pose.x = x_off;
parallel_path.poses[i].pose.y = y_off; parallel_path.poses[i].pose.y = y_off;
parallel_path.poses[i].pose.theta = p.theta; parallel_path.poses[i].pose.theta = sign_x < 0 ? angles::normalize_angle(theta + M_PI) : theta;
parallel_path.poses[i].header = path.poses[i].header;
} }
return parallel_path; return parallel_path;
} }
@@ -1312,10 +1315,8 @@ robot_nav_2d_msgs::Path2D mkt_algorithm::diff::PredictiveTrajectory::generateHer
const double x = goal.pose.x; const double x = goal.pose.x;
const double y = goal.pose.y; const double y = goal.pose.y;
const double theta = goal.pose.theta; double theta = goal.pose.theta;
robot::log_debug("x: %f, y: %f, theta: %f", x, y, theta);
const double L = std::hypot(x, y); const double L = std::hypot(x, y);
if (L < 1e-6) { if (L < 1e-6) {
robot_nav_2d_msgs::Pose2DStamped pose_stamped; robot_nav_2d_msgs::Pose2DStamped pose_stamped;
pose_stamped.pose.x = x; pose_stamped.pose.x = x;
@@ -1356,6 +1357,8 @@ robot_nav_2d_msgs::Path2D mkt_algorithm::diff::PredictiveTrajectory::generateHer
double dx = dh10 * Lnegative + dh01 * x + dh11 * Lnegative * std::cos(theta); double dx = dh10 * Lnegative + dh01 * x + dh11 * Lnegative * std::cos(theta);
double dy = dh01 * y + dh11 * Lnegative * std::sin(theta); double dy = dh01 * y + dh11 * Lnegative * std::sin(theta);
if(fabs(dx) < 1e-6 && fabs(dy) < 1e-6)
continue;
double heading = std::atan2(dy, dx); double heading = std::atan2(dy, dx);
robot_nav_2d_msgs::Pose2DStamped pose_out; robot_nav_2d_msgs::Pose2DStamped pose_out;
@@ -1426,6 +1429,8 @@ robot_nav_2d_msgs::Path2D mkt_algorithm::diff::PredictiveTrajectory::generateHer
double dx = 2.0 * ax * t + bx; double dx = 2.0 * ax * t + bx;
double dy = 2.0 * ay * t + by; double dy = 2.0 * ay * t + by;
if(fabs(dx) < 1e-6 && fabs(dy) < 1e-6)
continue;
double heading = std::atan2(dy, dx); double heading = std::atan2(dy, dx);
robot_nav_2d_msgs::Pose2DStamped pose_out; robot_nav_2d_msgs::Pose2DStamped pose_out;

5
src/Algorithms/Libraries/mkt_plugins/src/goal_checker.cpp
View File

@@ -70,9 +70,8 @@ bool mkt_plugins::GoalChecker::isGoalReached(const robot_nav_2d_msgs::Pose2DStam
double tolerance = fabs(cos(theta)) >= fabs(sin(theta)) ? x : y; double tolerance = fabs(cos(theta)) >= fabs(sin(theta)) ? x : y;
if(fabs(tolerance) <= xy_goal_tolerance_) if(fabs(tolerance) <= xy_goal_tolerance_)
{ {
robot::log_info_at(__FILE__, __LINE__, "%x %x", fabs(tolerance) <= xy_goal_tolerance_, tolerance * old_xy_goal_tolerance_ < 0); robot::log_info_at(__FILE__, __LINE__, "%.3f %.3f %.3f %.3f %.3f", fabs(cos(theta)), fabs(sin(theta)),xy_tolerance, xy_goal_tolerance_, yaw_goal_tolerance_);
robot::log_info_at(__FILE__, __LINE__, "%f %f %f %f", fabs(cos(theta)), fabs(sin(theta)), xy_goal_tolerance_, yaw_goal_tolerance_); robot::log_info_at(__FILE__, __LINE__, "Goal checker 1 ok %.3f %.3f %.3f %.3f %.3f ", tolerance, old_xy_goal_tolerance_, x, y, theta);
robot::log_info_at(__FILE__, __LINE__, "Goal checker 1 ok %f %f %f %f %f ", tolerance, old_xy_goal_tolerance_, x, y, theta);
return true; return true;
} }
} }