From 251c741dd9b02062e537f74ef2fe142b81a5c82e Mon Sep 17 00:00:00 2001 From: HiepLM Date: Thu, 23 Apr 2026 10:28:28 +0700 Subject: [PATCH] update phan di thang --- CMAKE_BUILD_README.md | 3 +- .../diff/diff_predictive_trajectory.h | 2 +- .../src/diff/diff_go_straight.cpp | 2 +- .../src/diff/diff_predictive_trajectory.cpp | 51 +++++++++++-------- 4 files changed, 34 insertions(+), 24 deletions(-) diff --git a/CMAKE_BUILD_README.md b/CMAKE_BUILD_README.md index 4361fd5..a6f434c 100644 --- a/CMAKE_BUILD_README.md +++ b/CMAKE_BUILD_README.md @@ -356,8 +356,7 @@ export PNKX_NAV_CORE_CONFIG_DIR=/path/to/config # Chỉ định workspace directory export PNKX_NAV_CORE_DIR=/path/to/pnkx_nav_core - -# LD_LIBRARY_PATH (nếu không install) + ] không install) export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/path/to/pnkx_nav_core/build/lib ``` diff --git a/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_predictive_trajectory.h b/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_predictive_trajectory.h index 936d3be..76233d8 100644 --- a/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_predictive_trajectory.h +++ b/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_predictive_trajectory.h @@ -181,7 +181,7 @@ namespace mkt_algorithm */ robot_nav_2d_msgs::Path2D generateTrajectory( const robot_nav_2d_msgs::Path2D &path, const robot_nav_2d_msgs::Twist2D &drive_target, - const robot_nav_2d_msgs::Twist2D &velocity, const double &sign_x, robot_nav_2d_msgs::Twist2D &drive_cmd); + const robot_nav_2d_msgs::Twist2D &velocity, const double &sign_x, robot_nav_2d_msgs::Twist2D &drive_cmd, const double &dt); /** * @brief Generate trajectory 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 d3351eb..7af465a 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 @@ -151,7 +151,7 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::GoStraight::calculator( lookahead_dist = sqrt(carrot_pose.pose.y *carrot_pose.pose.y + lookahead_dist * lookahead_dist); } robot_nav_2d_msgs::Twist2D drive_target; - transformed_plan = this->generateTrajectory(transformed_plan, drive_cmd, velocity, sign_x, drive_target); + transformed_plan = this->generateTrajectory(transformed_plan, drive_cmd, velocity, sign_x, drive_target, dt); carrot_pose = *getLookAheadPoint(velocity, lookahead_dist, transformed_plan); // Normal Pure Pursuit 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 1b66616..f56ff51 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 @@ -519,7 +519,9 @@ 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 &= 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; if (allow_rotate && shouldRotateToPath(transformed_plan, carrot_pose, velocity, angle_to_heading, sign_x)) { @@ -549,7 +551,7 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator( // else // { robot_nav_2d_msgs::Twist2D drive_target = drive_cmd; - transformed_plan = this->generateTrajectory(transformed_plan, drive_cmd, velocity, sign_x, drive_target); + transformed_plan = this->generateTrajectory(transformed_plan, drive_cmd, velocity, sign_x, drive_target, dt); carrot_pose = *getLookAheadPoint(velocity, lookahead_dist, transformed_plan); // Normal Pure Pursuit @@ -721,7 +723,6 @@ 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], -max_vel_theta_, max_vel_theta_); - // robot::log_info("%s", ss.str().c_str()); } void mkt_algorithm::diff::PredictiveTrajectory::applyDistanceSpeedScaling( @@ -745,8 +746,9 @@ void mkt_algorithm::diff::PredictiveTrajectory::applyDistanceSpeedScaling( double cosine_factor = 0.5 * (1.0 + std::cos(M_PI * (1.0 - r))); target_speed = max_speed * cosine_factor; } - - double reduce_speed = std::min(max_speed, min_speed_xy_); + const double v_limited = sign_x > 0 ? traj_->getTwistLinear(true).x : traj_->getTwistLinear(false).x; + const double v_min = std::min(fabs(v_limited), min_speed_xy_); + double reduce_speed = std::min(max_speed, v_min); if (s < S_final) { double r = std::clamp(s / S_final, 0.0, 1.0); @@ -818,12 +820,6 @@ bool mkt_algorithm::diff::PredictiveTrajectory::shouldRotateToPath( #ifdef BUILD_WITH_ROS if (result) ROS_WARN_THROTTLE(0.1, "angle_to_path: %f, heading_rotate: %f, is_stopped: %x %x, sign_x: %f", angle_to_path, heading_rotate, is_stopped, sign(angle_to_path) * sign_x < 0, sign_x); - - // else if(fabs(velocity.x) < min_speed_xy_) - // { - // ROS_INFO_THROTTLE(0.1, "velocity.x: %f, velocity.theta: %f, ", velocity.x, velocity.theta); - // ROS_INFO_THROTTLE(0.1, "angle_to_path: %f, heading_rotate: %f, is_stopped: %x %x, sign_x: %f", angle_to_path, heading_rotate, is_stopped, sign(angle_to_path) * sign_x < 0, sign_x); - // } #else if (result) robot::log_info_throttle(0.1, "angle_to_path: %f, heading_rotate: %f, is_stopped: %x %x, sign_x: %f", angle_to_path, heading_rotate, is_stopped, sign(angle_to_path) * sign_x < 0, sign_x); @@ -963,8 +959,10 @@ void mkt_algorithm::diff::PredictiveTrajectory::alignToFinalHeading( // --- Linear velocity calculation --- // Base velocity proportional to distance, with minimum for smooth motion double v_base = std::sqrt(2.0 * std::fabs(decel_lim_x_) * xy_error); + const double v_limited = sign_x > 0 ? traj_->getTwistLinear(true).x : traj_->getTwistLinear(false).x; + const double v_min = std::min(fabs(v_limited), min_speed_xy_); v_base = std::max(v_base, final_heading_min_velocity_); - v_base = std::min(v_base, min_speed_xy_); + v_base = std::min(v_base, v_min); // Scale down when heading error is large (prioritize rotation) double heading_scale = 1.0; @@ -1036,7 +1034,7 @@ void mkt_algorithm::diff::PredictiveTrajectory::alignToFinalHeading( cmd_vel.theta = omega_current + domega; // --- Apply velocity limits --- - cmd_vel.x = std::clamp(cmd_vel.x, -min_speed_xy_, min_speed_xy_); + cmd_vel.x = std::clamp(cmd_vel.x, -v_min, v_min); cmd_vel.theta = std::clamp(cmd_vel.theta, -max_vel_theta_, max_vel_theta_); // --- Safety: ensure we can stop --- @@ -1218,9 +1216,11 @@ double mkt_algorithm::diff::PredictiveTrajectory::adjustSpeedWithHermiteTrajecto double v_limit = std::fabs(v_target); double journey_distance = journey(trajectory.poses, 0, trajectory.poses.size() - 1); + const double v_limited = sign_x > 0 ? traj_->getTwistLinear(true).x : traj_->getTwistLinear(false).x; + const double v_min = std::min(fabs(v_limited), min_speed_xy_); if (journey_distance < min_journey_squared_) { - v_limit = std::clamp(sqrt(2.0 * fabs(decel_lim_x_) * journey_distance), min_approach_linear_velocity_, min_speed_xy_) * sign_x; + v_limit = std::clamp(sqrt(2.0 * fabs(decel_lim_x_) * journey_distance), min_approach_linear_velocity_, v_min) * sign_x; } if (max_kappa > 1e-6 && max_lateral_accel_ > 1e-6) @@ -1230,7 +1230,7 @@ double mkt_algorithm::diff::PredictiveTrajectory::adjustSpeedWithHermiteTrajecto } if(trajectory.poses.size() > 2 && fabs(trajectory.poses.front().pose.theta) >= angle_threshold_) - v_limit = min_speed_xy_ * sign_x; + v_limit = v_min * sign_x; if (fabs(decel_lim_x_) > 1e-6) { @@ -1248,7 +1248,8 @@ robot_nav_2d_msgs::Path2D mkt_algorithm::diff::PredictiveTrajectory::generateTra const robot_nav_2d_msgs::Twist2D &drive_target, const robot_nav_2d_msgs::Twist2D &velocity, const double &sign_x, - robot_nav_2d_msgs::Twist2D &drive_cmd) + robot_nav_2d_msgs::Twist2D &drive_cmd, + const double &dt) { if (path.poses.empty()) { @@ -1261,9 +1262,10 @@ robot_nav_2d_msgs::Path2D mkt_algorithm::diff::PredictiveTrajectory::generateTra drive_cmd.x = this->adjustSpeedWithHermiteTrajectory(velocity, path, drive_target.x, sign_x); drive_cmd.theta = max_vel_theta_; - if (max_kappa <= straight_threshold && fabs(path.poses.back().pose.x) < min_lookahead_dist_) // nếu đường thẳng + // nếu đường thẳng + if (max_kappa <= straight_threshold) { - if(fabs(path.poses.front().pose.y) <= 0.03 && fabs(path.poses.back().pose.x) < min_lookahead_dist_ ) + if(fabs(path.poses.back().pose.x) * 0.9 < min_lookahead_dist_) { return generateParallelPath(path, sign_x); } @@ -1271,8 +1273,17 @@ robot_nav_2d_msgs::Path2D mkt_algorithm::diff::PredictiveTrajectory::generateTra } else // nếu đường cong { - if(fabs(drive_cmd.x) < min_speed_xy_) - drive_cmd.x = std::copysign(min_speed_xy_, sign_x); + const double v_limited = sign_x > 0 ? traj_->getTwistLinear(true).x : traj_->getTwistLinear(false).x; + 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); + } return generateHermiteQuadraticTrajectory(path, sign_x); } }