From 274d3dd858201084070ce59a2786ec9a230424d9 Mon Sep 17 00:00:00 2001 From: HiepLM Date: Thu, 23 Apr 2026 10:29:19 +0700 Subject: [PATCH] update rotation --- .../mkt_algorithm/src/diff/diff_predictive_trajectory.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 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 f56ff51..b1ed3bb 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,9 +519,9 @@ 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; - // allow_rotate &= std::hypot(compute_plan_.poses.front().pose.x - pose.pose.x, compute_plan_.poses.front().pose.y - pose.pose.y) <= 0.1; - allow_rotate &= fabs(transformed_plan.poses.front().pose.y) <= 0.1; - robot::log_info("pose.y %f", fabs(transformed_plan.poses.front().pose.y)); + allow_rotate &= std::hypot(compute_plan_.poses.front().pose.x - pose.pose.x, compute_plan_.poses.front().pose.y - pose.pose.y) <= 0.1; + // allow_rotate &= fabs(transformed_plan.poses.front().pose.y) <= 0.1; + // robot::log_info("pose.y %f", fabs(transformed_plan.poses.front().pose.y)); double angle_to_heading; if (allow_rotate && shouldRotateToPath(transformed_plan, carrot_pose, velocity, angle_to_heading, sign_x)) {