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

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
// {
// if(fabs(carrot_pose.pose.y) > 0.2)
// {
// lookahead_dist = sqrt(carrot_pose.pose.y *carrot_pose.pose.y + lookahead_dist * lookahead_dist);
// }
robot_nav_2d_msgs::Twist2D drive_target;
robot_nav_2d_msgs::Twist2D drive_target = drive_cmd;
transformed_plan = this->generateTrajectory(transformed_plan, drive_cmd, velocity, sign_x, drive_target);
carrot_pose = *getLookAheadPoint(velocity, lookahead_dist, transformed_plan);
// Normal Pure Pursuit
this->computePurePursuit(
carrot_pose,
@@ -633,28 +630,32 @@ void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit(
if (std::fabs(v_target) < min_approach_linear_velocity)
v_target = std::copysign(min_approach_linear_velocity, sign_x);
std::stringstream ss;
// 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_)
{
ss << w_target << " ";
if (trajectory.poses.size() >= 2) {
const auto& p1 = trajectory.poses[trajectory.poses.size() - 1].pose;
double heading_ref = 0.0;
for(int i = trajectory.poses.size() - 2; i >= 0; i--)
{
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(sign_x < 0.0)
{
heading_ref = angles::normalize_angle(M_PI + heading_ref);
}
if(fabs(dx) < 1e-6 && fabs(dy) < 1e-6)
continue;
heading_ref = angles::normalize_angle(std::atan2(dy, dx));
break;
}
}
const double error = angles::normalize_angle(heading_ref);
ss << "error: " << error << " ";
double w_heading = 0.0;
pid(error,
near_goal_heading_integral_,
@@ -667,6 +668,7 @@ void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit(
// Apply acceleration limits
double dw_heading = std::clamp(w_heading - velocity.theta, -acc_lim_theta_ * dt, acc_lim_theta_ * dt);
w_target = velocity.theta + dw_heading;
ss << w_target << " ";
}
else
{
@@ -678,6 +680,7 @@ void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit(
{
near_goal_heading_was_active_ = false;
}
w_target = std::clamp(w_target, -fabs(drive_target.theta), fabs(drive_target.theta));
// 6) Apply acceleration limits (linear + angular)
@@ -687,7 +690,7 @@ void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit(
drive_cmd.x = velocity.x + dv;
drive_cmd.theta = velocity.theta + dw;
ss << drive_cmd.theta << " ";
Eigen::VectorXd y(2);
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);
if (kf_filter_angular_)
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(
@@ -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;
bool result = use_rotate_to_heading_ && fabs(angle_to_path) > heading_rotate;
// #ifdef BUILD_WITH_ROS
// 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);
#ifdef BUILD_WITH_ROS
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);
// 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, "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;
}
@@ -883,13 +887,11 @@ bool mkt_algorithm::diff::PredictiveTrajectory::shouldAlignToFinalHeading(
for(int i = trajectory.poses.size() - 2; i >= 0; i--)
{
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));
if(sign_x < 0.0)
{
heading_error = angles::normalize_angle(M_PI + heading_error);
}
heading_error = angles::normalize_angle(std::atan2(dy, dx));
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(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);
}
@@ -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;
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 x_off = p.x - offset_y * sin(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.y = y_off;
parallel_path.poses[i].pose.theta = p.theta;
parallel_path.poses[i].header = path.poses[i].header;
parallel_path.poses[i].pose.theta = sign_x < 0 ? angles::normalize_angle(theta + M_PI) : theta;
}
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 y = goal.pose.y;
const double theta = goal.pose.theta;
robot::log_debug("x: %f, y: %f, theta: %f", x, y, theta);
double theta = goal.pose.theta;
const double L = std::hypot(x, y);
if (L < 1e-6) {
robot_nav_2d_msgs::Pose2DStamped pose_stamped;
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 dy = dh01 * y + dh11 * Lnegative * std::sin(theta);
if(fabs(dx) < 1e-6 && fabs(dy) < 1e-6)
continue;
double heading = std::atan2(dy, dx);
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 dy = 2.0 * ay * t + by;
if(fabs(dx) < 1e-6 && fabs(dy) < 1e-6)
continue;
double heading = std::atan2(dy, dx);
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;
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__, "%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 %f %f %f %f %f ", tolerance, old_xy_goal_tolerance_, x, y, theta);
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__, "Goal checker 1 ok %.3f %.3f %.3f %.3f %.3f ", tolerance, old_xy_goal_tolerance_, x, y, theta);
return true;
}
}