From 875db4ba1e726d7a8b9fe7872d9b5d1193627bd3 Mon Sep 17 00:00:00 2001 From: HiepLM Date: Fri, 24 Apr 2026 14:52:11 +0700 Subject: [PATCH] =?UTF-8?q?Hiep=20s=E1=BB=AD=20custom=20v=C3=A0=20th=C3=B4?= =?UTF-8?q?ng=20s=E1=BB=91=20=20rotate?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../mkt_algorithm/src/diff/diff_predictive_trajectory.cpp | 8 ++++---- src/Algorithms/Packages/global_planners/custom_planner | 2 +- 2 files changed, 5 insertions(+), 5 deletions(-) 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 dc31648..4536f5b 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 @@ -519,11 +519,11 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator( const double distance_allow_rotate = min_journey_squared_; 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 && - fabs(transformed_plan.poses.front().pose.y * cos(transformed_plan.poses.front().pose.theta)) <= 0.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; - double angle_to_heading; + double angle_to_heading; if (allow_rotate && shouldRotateToPath(transformed_plan, carrot_pose, velocity, angle_to_heading, sign_x)) { if (!stopped(velocity, max_vel_theta_ + rot_stopped_velocity_, trans_stopped_velocity_)) diff --git a/src/Algorithms/Packages/global_planners/custom_planner b/src/Algorithms/Packages/global_planners/custom_planner index 62a493a..d5ef0b1 160000 --- a/src/Algorithms/Packages/global_planners/custom_planner +++ b/src/Algorithms/Packages/global_planners/custom_planner @@ -1 +1 @@ -Subproject commit 62a493a892e6a0936cc584a434ac694f9bbf0f5a +Subproject commit d5ef0b1075f09b27606e1d2ec04f001ac3de69bf