From 5812542eafb6d22cf61bfacf8fede3213416ed8a Mon Sep 17 00:00:00 2001 From: HiepLM Date: Thu, 23 Apr 2026 18:26:13 +0700 Subject: [PATCH] =?UTF-8?q?uodate=20l=E1=BA=A7n=203?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../Cores/score_algorithm/src/score_algorithm.cpp | 2 +- .../mkt_algorithm/src/diff/diff_predictive_trajectory.cpp | 6 +++--- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/Algorithms/Cores/score_algorithm/src/score_algorithm.cpp b/src/Algorithms/Cores/score_algorithm/src/score_algorithm.cpp index 2f48520..46a85a4 100644 --- a/src/Algorithms/Cores/score_algorithm/src/score_algorithm.cpp +++ b/src/Algorithms/Cores/score_algorithm/src/score_algorithm.cpp @@ -219,7 +219,7 @@ bool score_algorithm::ScoreAlgorithm::computePlanCommand(const robot_nav_2d_msgs double x = cos(global_plan.poses[index_s[i - 1]].pose.theta) * dx + sin(global_plan.poses[index_s[i - 1]].pose.theta) * dy; double y = -sin(global_plan.poses[index_s[i - 1]].pose.theta) * dx + cos(global_plan.poses[index_s[i - 1]].pose.theta) * dy; - if (std::abs(std::sqrt(dx * dx + dy * dy)) <= xy_local_goal_tolerance_ + 0.1) + if (std::abs(std::sqrt(dx * dx + dy * dy)) <= xy_local_goal_tolerance_ + 0.3) { double tolerance = fabs(cos(theta)) >= fabs(sin(theta)) ? x : y; if (fabs(tolerance) <= xy_local_goal_tolerance_) 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 90424dd..f0389f3 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,8 +518,8 @@ 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 |= 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)); double angle_to_heading; @@ -1268,7 +1268,7 @@ robot_nav_2d_msgs::Path2D mkt_algorithm::diff::PredictiveTrajectory::generateTra { if(fabs(path.poses.back().pose.x) * 0.9 < min_lookahead_dist_) { - if(fabs(path.poses.back().pose.x) < min_journey_squared_ && fabs(path.poses.back().pose.theta) < 0.05) + if(fabs(path.poses.back().pose.x) < min_journey_squared_) drive_cmd.theta = 0.01; return generateParallelPath(path, sign_x); }