From dbbda958a215726abefd4bcdb22537cf9b2fa065 Mon Sep 17 00:00:00 2001 From: HiepLM Date: Fri, 24 Apr 2026 15:01:37 +0700 Subject: [PATCH] update logic xoay --- .../mkt_algorithm/src/diff/diff_predictive_trajectory.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory.cpp b/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory.cpp index 4536f5b..621f729 100644 --- a/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory.cpp +++ b/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory.cpp @@ -521,7 +521,8 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator( const double path_distance_to_rotate = journey(transformed_plan.poses, 0, transformed_plan.poses.size() - 1); allow_rotate |= path_distance_to_rotate >= distance_allow_rotate; allow_rotate |= fabs(transformed_plan.poses.back().pose.theta) >= M_PI / 2; - allow_rotate &= fabs(transformed_plan.poses.front().pose.y * cos(transformed_plan.poses.front().pose.theta)) <= 0.1; + allow_rotate &= (fabs(transformed_plan.poses.front().pose.y) <= 0.5); + double angle_to_heading; if (allow_rotate && shouldRotateToPath(transformed_plan, carrot_pose, velocity, angle_to_heading, sign_x))