From 0c65a5b6ba75944a58b4d626524f962f1804e9ea Mon Sep 17 00:00:00 2001 From: HiepLM Date: Thu, 23 Apr 2026 17:57:31 +0700 Subject: [PATCH] update xoay --- .../mkt_algorithm/src/diff/diff_predictive_trajectory.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 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 fd76e9e..90424dd 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 @@ -518,7 +518,7 @@ 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 &= 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)); @@ -723,7 +723,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], -fabs(drive_target.theta), fabs(drive_target.theta)); - robot::log_info("drive_cmd.theta: %f, drive_target.theta: %f", drive_cmd.theta, drive_target.theta); + // robot::log_info("drive_cmd.theta: %f, drive_target.theta: %f", drive_cmd.theta, drive_target.theta); } void mkt_algorithm::diff::PredictiveTrajectory::applyDistanceSpeedScaling(