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

update

Browse Source
This commit is contained in:
hoangson02
2026-03-30 07:55:41 +00:00
parent ff8ecf6126
commit 08d597304e
2 changed files with 59 additions and 46 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -630,49 +630,53 @@ 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; 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; const auto& dx = p1.x - p.x ;
// const auto& dx = sign_x < 0.0 ? p1.x - p.x : p.x - p1.x; const auto& dy = p1.y - p.y ;
// const auto& dy = sign_x < 0.0 ? p1.y - p.y : p.y - p1.y; if(std::hypot(dx, dy) >= costmap_robot_->getCostmap()->getResolution())
// if(std::hypot(dx, dy) >= costmap_robot_->getCostmap()->getResolution()) {
// { if(fabs(dx) < 1e-6 && fabs(dy) < 1e-6)
// if(fabs(dx) < 1e-6 && fabs(dy) < 1e-6) continue;
// continue; heading_ref = std::atan2(dy, dx);
// heading_ref = angles::normalize_angle(std::atan2(dy, dx)); ss << "error " << heading_ref << " ";
// break; if(sign_x < 0.0)
// } heading_ref += std::copysign(M_PI, heading_ref) * (-1.0);
// } break;
}
}
// const double error = angles::normalize_angle(heading_ref); const double error = heading_ref;
// ss << "error: " << error << " "; ss << error << " ";
// double w_heading = 0.0; double w_heading = 0.0;
// pid(error, pid(error,
// near_goal_heading_integral_, near_goal_heading_integral_,
// near_goal_heading_last_error_, near_goal_heading_last_error_,
// dt, dt,
// final_heading_kp_angular_, final_heading_kp_angular_,
// final_heading_ki_angular_, final_heading_ki_angular_,
// final_heading_kd_angular_, final_heading_kd_angular_,
// w_heading); w_heading);
// // 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; ss << "dw_heading " << dw_heading << " ";
// ss << w_target << " "; w_target = velocity.theta + dw_heading;
// } }
// else else
// { {
w_target = 0.0; w_target = std::clamp(w_target, -0.001, 0.001);
near_goal_heading_was_active_ = false; near_goal_heading_was_active_ = false;
// } }
} }
else else
{ {
@@ -687,6 +691,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;
Eigen::VectorXd y(2); Eigen::VectorXd y(2);
y << drive_cmd.x, drive_cmd.theta; y << drive_cmd.x, drive_cmd.theta;
@@ -706,6 +711,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_);
// robot::log_info("%s", ss.str().c_str());
} }
void mkt_algorithm::diff::PredictiveTrajectory::applyDistanceSpeedScaling( void mkt_algorithm::diff::PredictiveTrajectory::applyDistanceSpeedScaling(
@@ -769,16 +775,22 @@ bool mkt_algorithm::diff::PredictiveTrajectory::shouldRotateToPath(
for(int i = 2; i < global_plan.poses.size(); i++) for(int i = 2; i < global_plan.poses.size(); i++)
{ {
const auto& p = global_plan.poses[i]; const auto& p = global_plan.poses[i];
const auto& dx = p.pose.x - p1.pose.x;
const auto& dy = p.pose.y - p1.pose.y;
if(std::hypot(p.pose.x, p.pose.y) > costmap_robot_->getCostmap()->getResolution()) if(std::hypot(p.pose.x, p.pose.y) > costmap_robot_->getCostmap()->getResolution())
{ {
path_angle = std::atan2(p.pose.y - p1.pose.y, p.pose.x - p1.pose.x); if(fabs(dx) < 1e-9 && fabs(dy) < 1e-9)
continue;
path_angle = std::atan2(dy, dx);
break; break;
} }
} }
} }
// Whether we should rotate robot to rough path heading // Whether we should rotate robot to rough path heading
angle_to_path = sign_x < 0.0 ? angles::normalize_angle(M_PI + path_angle) : path_angle; if(sign_x < 0.0)
path_angle += std::copysign(M_PI, path_angle) * (-1.0);
angle_to_path = path_angle;
double heading_linear = sqrt(velocity.x * velocity.x + velocity.y * velocity.y); double heading_linear = sqrt(velocity.x * velocity.x + velocity.y * velocity.y);
// The difference in the path orientation and the starting robot orientation (radians) to trigger a rotate in place. (default: 0.785) // The difference in the path orientation and the starting robot orientation (radians) to trigger a rotate in place. (default: 0.785)
double heading_rotate = rotate_to_heading_min_angle_; double heading_rotate = rotate_to_heading_min_angle_;

13
src/Algorithms/Packages/global_planners/two_points_planner/src/two_points_planner.cpp
View File

@@ -190,13 +190,14 @@ namespace two_points_planner
} }
else else
{ {
robot_geometry_msgs::PoseStamped pose = start; auto goal_2d = robot_nav_2d_utils::poseStampedToPose2D(goal); // hoặc start.theta
pose.pose.position.x += resolution * cos(theta); robot_geometry_msgs::PoseStamped pose = goal;
pose.pose.position.y += resolution * sin(theta); pose.pose.position.x += resolution * std::cos(goal_2d.pose.theta);
pose.pose.position.y += resolution * std::sin(goal_2d.pose.theta);
plan.push_back(pose); plan.push_back(pose);
pose = start; pose = goal;
pose.pose.position.x -= resolution * cos(theta); pose.pose.position.x -= resolution * std::cos(goal_2d.pose.theta);
pose.pose.position.y -= resolution * sin(theta); pose.pose.position.y -= resolution * std::sin(goal_2d.pose.theta);
plan.push_back(pose); plan.push_back(pose);
plan.push_back(goal); plan.push_back(goal);
return true; return true;