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

update

Browse Source
This commit is contained in:
HiepLM
2026-04-25 16:26:12 +07:00
parent 0d10ec2208
commit 1a19e38b2d
3 changed files with 15 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
config/pnkx_local_planner_params.yaml
View File

@@ -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

5
src/Algorithms/Cores/score_algorithm/src/score_algorithm.cpp
View File

@@ -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_)

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

@@ -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;