From 5d4d77155bdb59c8c3daf3e0871f9e137eb211f4 Mon Sep 17 00:00:00 2001 From: HiepLM Date: Thu, 23 Apr 2026 17:11:47 +0700 Subject: [PATCH] update khi lui --- CATKIN_BUILD_README.md | 6 +++- config/pnkx_local_planner_params.yaml | 2 +- .../src/diff/diff_go_straight.cpp | 28 +++++++++---------- .../src/diff/diff_predictive_trajectory.cpp | 15 ++++------ 4 files changed, 26 insertions(+), 25 deletions(-) diff --git a/CATKIN_BUILD_README.md b/CATKIN_BUILD_README.md index c0db0e7..6add951 100644 --- a/CATKIN_BUILD_README.md +++ b/CATKIN_BUILD_README.md @@ -20,7 +20,11 @@ The specified base path contains a CMakeLists.txt but "catkin_make" must be invo # Build trong workspace mới cd ../pnkx_nav_catkin_ws -catkin_make +rm -rf build devel +catkin_make -DCMAKE_BUILD_TYPE=RelWithDebInfo \ + -DCMAKE_CXX_FLAGS="-fsanitize=address -fno-omit-frame-pointer" \ + -DCMAKE_C_FLAGS="-fsanitize=address -fno-omit-frame-pointer" \ + -DCMAKE_EXE_LINKER_FLAGS="-fsanitize=address" source devel/setup.bash ``` diff --git a/config/pnkx_local_planner_params.yaml b/config/pnkx_local_planner_params.yaml index f430f63..014d06b 100644 --- a/config/pnkx_local_planner_params.yaml +++ b/config/pnkx_local_planner_params.yaml @@ -75,7 +75,7 @@ LimitedAccelGenerator: MKTAlgorithmDiffPredictiveTrajectory: library_path: libmkt_algorithm_diff xy_local_goal_tolerance: 0.02 - angle_threshold: 0.47 + angle_threshold: 0.6 index_samples: 60 follow_step_path: true diff --git a/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_go_straight.cpp b/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_go_straight.cpp index 7af465a..eaf041d 100644 --- a/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_go_straight.cpp +++ b/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_go_straight.cpp @@ -131,19 +131,19 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::GoStraight::calculator( auto carrot_pose = *getLookAheadPoint(velocity, lookahead_dist, transformed_plan); robot_geometry_msgs::PoseStamped carrot_pose_stamped = robot_nav_2d_utils::pose2DToPoseStamped(carrot_pose); - // === Final Heading Alignment Check === - double xy_error = 0.0, heading_error = 0.0; - if (shouldAlignToFinalHeading(transformed_plan, carrot_pose, velocity, xy_error, heading_error, sign_x)) - { - // Use Arc Motion controller for final heading alignment - alignToFinalHeading(xy_error, heading_error, velocity, sign_x, dt, drive_cmd); -#ifdef BUILD_WITH_ROS - ROS_INFO("xy_err=%.3f, heading_err=%.3f deg, v=%.3f, w_current=%.3f, w_target=%.3f", - xy_error, heading_error * 180.0 / M_PI, drive_cmd.x, velocity.theta, drive_cmd.theta); -#endif - } - else - { +// // === Final Heading Alignment Check === +// double xy_error = 0.0, heading_error = 0.0; +// if (shouldAlignToFinalHeading(transformed_plan, carrot_pose, velocity, xy_error, heading_error, sign_x)) +// { +// // Use Arc Motion controller for final heading alignment +// alignToFinalHeading(xy_error, heading_error, velocity, sign_x, dt, drive_cmd); +// #ifdef BUILD_WITH_ROS +// ROS_INFO("xy_err=%.3f, heading_err=%.3f deg, v=%.3f, w_current=%.3f, w_target=%.3f", +// xy_error, heading_error * 180.0 / M_PI, drive_cmd.x, velocity.theta, drive_cmd.theta); +// #endif +// } +// else +// { // robot::log_info_at(__FILE__, __LINE__, "journey : %f lookahead_dist : %f", // journey(transformed_plan.poses, 0, transformed_plan.poses.size() - 1), lookahead_dist); if(fabs(carrot_pose.pose.y) > 0.2) @@ -164,7 +164,7 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::GoStraight::calculator( sign_x, dt, drive_cmd); - } + // } applyDistanceSpeedScaling(compute_plan_, velocity, drive_cmd, sign_x, dt); if (this->nav_stop_) 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 b1ed3bb..cb5aa1a 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 @@ -682,7 +682,7 @@ void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit( 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, -0.03, 0.03); + w_target = std::clamp(w_target, -fabs(drive_target.theta), fabs(drive_target.theta)); } else { @@ -722,7 +722,8 @@ void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit( drive_cmd.x = std::clamp(kf_->state()[0], -fabs(v_target), fabs(v_target)); 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], -max_vel_theta_, max_vel_theta_); + 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); } void mkt_algorithm::diff::PredictiveTrajectory::applyDistanceSpeedScaling( @@ -1265,8 +1266,9 @@ robot_nav_2d_msgs::Path2D mkt_algorithm::diff::PredictiveTrajectory::generateTra // nếu đường thẳng if (max_kappa <= straight_threshold) { - if(fabs(path.poses.back().pose.x) * 0.9 < min_lookahead_dist_) + if(fabs(path.poses.back().pose.x) * 0.9 < min_lookahead_dist_ && fabs(path.poses.back().pose.theta) < 0.1) { + drive_cmd.theta = 0.01; return generateParallelPath(path, sign_x); } return generateHermiteTrajectory(path, sign_x); @@ -1277,12 +1279,7 @@ robot_nav_2d_msgs::Path2D mkt_algorithm::diff::PredictiveTrajectory::generateTra const double v_min = std::min(fabs(v_limited), min_speed_xy_); if(fabs(drive_cmd.x) < v_min) { - robot_nav_2d_msgs::Twist2D vel_in, v_out; - vel_in.x = v_min; - vel_in.y = 0.0; - vel_in.theta = drive_cmd.theta; - moveWithAccLimits(velocity, vel_in, v_out, dt); - drive_cmd.x = std::copysign(v_out.x, sign_x); + drive_cmd.x = std::copysign(v_min, sign_x); } return generateHermiteQuadraticTrajectory(path, sign_x); }