HiepLM/pnkx_nav_core
1
1
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

update phan di thang

Browse Source
This commit is contained in:
HiepLM
2026-04-23 10:28:28 +07:00
parent a9c56261ea
commit 251c741dd9
4 changed files with 34 additions and 24 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
CMAKE_BUILD_README.md
View File

@@ -356,8 +356,7 @@ export PNKX_NAV_CORE_CONFIG_DIR=/path/to/config
# Chỉ định workspace directory # Chỉ định workspace directory
export PNKX_NAV_CORE_DIR=/path/to/pnkx_nav_core export PNKX_NAV_CORE_DIR=/path/to/pnkx_nav_core
] không install)
# LD_LIBRARY_PATH (nếu không install)
export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/path/to/pnkx_nav_core/build/lib export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/path/to/pnkx_nav_core/build/lib
``` ```

2
src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_predictive_trajectory.h
View File

@@ -181,7 +181,7 @@ namespace mkt_algorithm
*/ */
robot_nav_2d_msgs::Path2D generateTrajectory( 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::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 * @brief Generate trajectory

2
src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_go_straight.cpp
View File

@@ -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); lookahead_dist = sqrt(carrot_pose.pose.y *carrot_pose.pose.y + lookahead_dist * lookahead_dist);
} }
robot_nav_2d_msgs::Twist2D drive_target; 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); carrot_pose = *getLookAheadPoint(velocity, lookahead_dist, transformed_plan);
// Normal Pure Pursuit // Normal Pure Pursuit

51
src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory.cpp
View File

@@ -519,7 +519,9 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
const double distance_allow_rotate = min_journey_squared_; const double distance_allow_rotate = min_journey_squared_;
const double path_distance_to_rotate = journey(transformed_plan.poses, 0, transformed_plan.poses.size() - 1); 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 |= 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; double angle_to_heading;
if (allow_rotate && shouldRotateToPath(transformed_plan, carrot_pose, velocity, angle_to_heading, sign_x)) 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 // else
// { // {
robot_nav_2d_msgs::Twist2D drive_target = drive_cmd; 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); carrot_pose = *getLookAheadPoint(velocity, lookahead_dist, transformed_plan);
// Normal Pure Pursuit // 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); drive_cmd.x = fabs(drive_cmd.x) >= v_min ? drive_cmd.x : std::copysign(v_min, sign_x);
if (kf_filter_angular_) 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], -max_vel_theta_, max_vel_theta_);
// robot::log_info("%s", ss.str().c_str());
} }
void mkt_algorithm::diff::PredictiveTrajectory::applyDistanceSpeedScaling( 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))); double cosine_factor = 0.5 * (1.0 + std::cos(M_PI * (1.0 - r)));
target_speed = max_speed * cosine_factor; target_speed = max_speed * cosine_factor;
} }
const double v_limited = sign_x > 0 ? traj_->getTwistLinear(true).x : traj_->getTwistLinear(false).x;
double reduce_speed = std::min(max_speed, min_speed_xy_); 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) if (s < S_final)
{ {
double r = std::clamp(s / S_final, 0.0, 1.0); 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 #ifdef BUILD_WITH_ROS
if (result) 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); 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 #else
if (result) 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); 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 --- // --- Linear velocity calculation ---
// Base velocity proportional to distance, with minimum for smooth motion // Base velocity proportional to distance, with minimum for smooth motion
double v_base = std::sqrt(2.0 * std::fabs(decel_lim_x_) * xy_error); 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::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) // Scale down when heading error is large (prioritize rotation)
double heading_scale = 1.0; double heading_scale = 1.0;
@@ -1036,7 +1034,7 @@ void mkt_algorithm::diff::PredictiveTrajectory::alignToFinalHeading(
cmd_vel.theta = omega_current + domega; cmd_vel.theta = omega_current + domega;
// --- Apply velocity limits --- // --- 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_); cmd_vel.theta = std::clamp(cmd_vel.theta, -max_vel_theta_, max_vel_theta_);
// --- Safety: ensure we can stop --- // --- Safety: ensure we can stop ---
@@ -1218,9 +1216,11 @@ double mkt_algorithm::diff::PredictiveTrajectory::adjustSpeedWithHermiteTrajecto
double v_limit = std::fabs(v_target); double v_limit = std::fabs(v_target);
double journey_distance = journey(trajectory.poses, 0, trajectory.poses.size() - 1); 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_) 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) 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_) 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) 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 &drive_target,
const robot_nav_2d_msgs::Twist2D &velocity, const robot_nav_2d_msgs::Twist2D &velocity,
const double &sign_x, 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()) 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.x = this->adjustSpeedWithHermiteTrajectory(velocity, path, drive_target.x, sign_x);
drive_cmd.theta = max_vel_theta_; 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); return generateParallelPath(path, sign_x);
} }
@@ -1271,8 +1273,17 @@ robot_nav_2d_msgs::Path2D mkt_algorithm::diff::PredictiveTrajectory::generateTra
} }
else // nếu đường cong else // nếu đường cong
{ {
if(fabs(drive_cmd.x) < min_speed_xy_) const double v_limited = sign_x > 0 ? traj_->getTwistLinear(true).x : traj_->getTwistLinear(false).x;
drive_cmd.x = std::copysign(min_speed_xy_, sign_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); return generateHermiteQuadraticTrajectory(path, sign_x);
} }
} }