From 9c14054d3fe4ae665a66501a1df2c16f207aee4e Mon Sep 17 00:00:00 2001 From: HiepLM Date: Fri, 24 Apr 2026 12:32:47 +0700 Subject: [PATCH] update reduce speed --- config/pnkx_local_planner_params.yaml | 4 ++-- .../src/diff/diff_predictive_trajectory.cpp | 22 ++++++------------- 2 files changed, 9 insertions(+), 17 deletions(-) diff --git a/config/pnkx_local_planner_params.yaml b/config/pnkx_local_planner_params.yaml index 247843f..d5380d9 100644 --- a/config/pnkx_local_planner_params.yaml +++ b/config/pnkx_local_planner_params.yaml @@ -53,12 +53,12 @@ LimitedAccelGenerator: max_vel_theta: 0.4 # max_rot_vel: 1.0 # choose slightly less than the base's capability min_vel_theta: 0.05 # min_rot_vel: 0.1 default: 0.4 # this is the min angular velocity when there is negligible translational velocity - acc_lim_x: 1.5 + 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_y: -0.0 - decel_lim_theta: -1.5 + decel_lim_theta: -3.0 # Whether to split the path into segments or not split_path: true 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 b67c3ac..dc31648 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 @@ -522,9 +522,7 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator( 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 &= 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("%f front.y %f", path_distance_to_rotate, fabs(transformed_plan.poses.front().pose.y * cos(transformed_plan.poses.front().pose.theta))); + double angle_to_heading; if (allow_rotate && shouldRotateToPath(transformed_plan, carrot_pose, velocity, angle_to_heading, sign_x)) { @@ -581,6 +579,7 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator( result.velocity = drive_cmd; return result; } + } result.poses.clear(); result.poses.reserve(transformed_plan.poses.size()); @@ -643,9 +642,6 @@ void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit( // 4) Maintain minimum approach speed if (std::fabs(v_target) < min_approach_linear_velocity) v_target = std::copysign(min_approach_linear_velocity, sign_x); - - std::stringstream ss; - // 5) Angular speed from curvature double w_target = v_target * kappa; if(journey(trajectory.poses, 0, trajectory.poses.size() - 1) <= min_journey_squared_) @@ -663,7 +659,6 @@ void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit( if(fabs(dx) < 1e-6 && fabs(dy) < 1e-6) continue; heading_ref = std::atan2(dy, dx); - ss << "error " << heading_ref << " "; if(sign_x < 0.0) heading_ref += std::copysign(M_PI, heading_ref) * (-1.0); break; @@ -671,7 +666,6 @@ void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit( } const double error = heading_ref; - ss << error << " "; double w_heading = 0.0; pid(error, near_goal_heading_integral_, @@ -683,7 +677,6 @@ void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit( w_heading); // Apply acceleration limits double dw_heading = std::clamp(w_heading - velocity.theta, -acc_lim_theta_ * dt, acc_lim_theta_ * dt); - ss << "dw_heading " << dw_heading << " "; w_target = velocity.theta + dw_heading; w_target = std::clamp(w_target, -fabs(drive_target.theta), fabs(drive_target.theta)); } @@ -697,7 +690,6 @@ void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit( { near_goal_heading_was_active_ = false; } - w_target = std::clamp(w_target, -fabs(drive_target.theta), fabs(drive_target.theta)); // 6) Apply acceleration limits (linear + angular) @@ -706,7 +698,7 @@ void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit( drive_cmd.x = velocity.x + dv; drive_cmd.theta = velocity.theta + dw; - + Eigen::VectorXd y(2); y << drive_cmd.x, drive_cmd.theta; @@ -726,7 +718,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( @@ -1261,11 +1253,11 @@ robot_nav_2d_msgs::Path2D mkt_algorithm::diff::PredictiveTrajectory::generateTra drive_cmd.theta = 0.0; return robot_nav_2d_msgs::Path2D(); } + + drive_cmd.x = drive_target.x; + drive_cmd.theta = max_vel_theta_; double max_kappa = calculateMaxKappa(path); const double straight_threshold = std::max(0.05, 2.0 * (costmap_robot_ ? costmap_robot_->getCostmap()->getResolution() : 0.05)); - drive_cmd.x = this->adjustSpeedWithHermiteTrajectory(velocity, path, drive_target.x, sign_x); - drive_cmd.theta = max_vel_theta_; - // nếu đường thẳng if (max_kappa <= straight_threshold) {