From 1a19e38b2dba22d02f1b4c26aabdccf37aaa1e48 Mon Sep 17 00:00:00 2001 From: HiepLM Date: Sat, 25 Apr 2026 16:26:12 +0700 Subject: [PATCH] update --- config/pnkx_local_planner_params.yaml | 10 +++++----- .../Cores/score_algorithm/src/score_algorithm.cpp | 5 +++-- .../src/diff/diff_predictive_trajectory.cpp | 13 +++++++------ 3 files changed, 15 insertions(+), 13 deletions(-) diff --git a/config/pnkx_local_planner_params.yaml b/config/pnkx_local_planner_params.yaml index e6aaea8..f372cc2 100644 --- a/config/pnkx_local_planner_params.yaml +++ b/config/pnkx_local_planner_params.yaml @@ -56,9 +56,9 @@ LimitedAccelGenerator: acc_lim_x: 3.0 acc_lim_y: 0.0 # diff drive robot acc_lim_theta: 1.5 - decel_lim_x: -1.5 + decel_lim_x: -3.0 decel_lim_y: -0.0 - decel_lim_theta: -3.0 + decel_lim_theta: -2.0 # Whether to split the path into segments or not split_path: true @@ -74,7 +74,7 @@ LimitedAccelGenerator: MKTAlgorithmDiffPredictiveTrajectory: library_path: libmkt_algorithm_diff - xy_local_goal_tolerance: 0.02 + xy_local_goal_tolerance: 0.05 angle_threshold: 0.6 index_samples: 60 follow_step_path: true @@ -119,7 +119,7 @@ MKTAlgorithmDiffPredictiveTrajectory: MKTAlgorithmDiffGoStraight: library_path: libmkt_algorithm_diff - xy_local_goal_tolerance: 0.02 + xy_local_goal_tolerance: 0.05 angle_threshold: 0.8 index_samples: 60 follow_step_path: true @@ -156,7 +156,7 @@ MKTAlgorithmDiffGoStraight: MKTAlgorithmDiffRotateToGoal: library_path: libmkt_algorithm_diff - xy_local_goal_tolerance: 0.02 + xy_local_goal_tolerance: 0.05 angle_threshold: 0.47 index_samples: 60 follow_step_path: true diff --git a/src/Algorithms/Cores/score_algorithm/src/score_algorithm.cpp b/src/Algorithms/Cores/score_algorithm/src/score_algorithm.cpp index beda55c..a70675d 100644 --- a/src/Algorithms/Cores/score_algorithm/src/score_algorithm.cpp +++ b/src/Algorithms/Cores/score_algorithm/src/score_algorithm.cpp @@ -219,11 +219,12 @@ 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.3) + if (std::abs(std::sqrt(dx * dx + dy * dy)) <= xy_local_goal_tolerance_ + 0.2) { double tolerance = fabs(cos(theta)) >= fabs(sin(theta)) ? x : y; if (fabs(tolerance) <= xy_local_goal_tolerance_) { + if (index_s[i] > sub_goal_index_saved_) { sub_goal_index = (i < index_s.size() - 1) ? index_s[i] : (unsigned int)global_plan.poses.size() - 1; @@ -257,7 +258,7 @@ bool score_algorithm::ScoreAlgorithm::computePlanCommand(const robot_nav_2d_msgs double theta = angles::normalize_angle(robot_pose.pose.theta - global_plan.poses[sub_goal_index].pose.theta); double x = cos(global_plan.poses[sub_goal_index].pose.theta) * dx + sin(global_plan.poses[sub_goal_index].pose.theta) * dy; double y = -sin(global_plan.poses[sub_goal_index].pose.theta) * dx + cos(global_plan.poses[sub_goal_index].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.2) { 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 621f729..a91ae3a 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 @@ -520,9 +520,10 @@ 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 |= fabs(transformed_plan.poses.back().pose.theta) >= M_PI / 2; - allow_rotate &= (fabs(transformed_plan.poses.front().pose.y) <= 0.5); + robot_geometry_msgs::Pose2D back_pose = transformed_plan.poses.back().pose; + allow_rotate |= fabs(atan2(back_pose.y, back_pose.x) - back_pose.theta) > M_PI / 6.0; + 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)) @@ -624,15 +625,15 @@ void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit( // 3) Adjust speed using Hermite trajectory curvature + remaining distance double v_target = adjustSpeedWithHermiteTrajectory(velocity, trajectory, drive_target.x, sign_x); - const double L_min = 0.1; // m, chỉnh theo nhu cầu - double scale_close = std::clamp(L / L_min, 0.0, 1.0); - v_target *= scale_close; + // const double L_min = 0.1; // m, chỉnh theo nhu cầu + // double scale_close = std::clamp(L / L_min, 0.0, 1.0); + // v_target *= scale_close; const double y_abs = std::fabs(carrot_pose.pose.y); const double y_soft = 0.1; if (y_abs > y_soft) { double scale = y_soft / y_abs; // y càng lớn => scale càng nhỏ - scale = std::clamp(scale, 0.2, 1.0); // không giảm quá sâu + scale = std::clamp(scale, 0.6, 1.0); // không giảm quá sâu v_target *= scale; robot_nav_2d_msgs::Twist2D cmd, result; cmd.x = v_target;