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

test rotate

Browse Source
This commit is contained in:
HiepLM
2026-04-06 04:46:09 +07:00
parent d88e547676
commit ef76c43029
1 changed files with 5 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -784,7 +784,7 @@ bool mkt_algorithm::diff::PredictiveTrajectory::shouldRotateToPath(
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& dx = p.pose.x - p1.pose.x;
const auto& dy = p.pose.y - p1.pose.y; 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(dx, dy) > costmap_robot_->getCostmap()->getResolution())
{ {
if(fabs(dx) < 1e-9 && fabs(dy) < 1e-9) if(fabs(dx) < 1e-9 && fabs(dy) < 1e-9)
continue; continue;
@@ -795,9 +795,10 @@ bool mkt_algorithm::diff::PredictiveTrajectory::shouldRotateToPath(
} }
// Whether we should rotate robot to rough path heading // Whether we should rotate robot to rough path heading
if(sign_x < 0.0) // if(sign_x < 0.0)
path_angle += std::copysign(M_PI, path_angle) * (-1.0); // path_angle += std::copysign(M_PI, path_angle) * (-1.0);
angle_to_path = path_angle; // angle_to_path = path_angle;
angle_to_path = sign_x < 0.0 ? angles::normalize_angle(M_PI + path_angle) : 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_;