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

fix lỗi xoay khi về đích

Browse Source
This commit is contained in:
HiepLM
2026-04-27 14:24:40 +07:00
parent 1a19e38b2d
commit 62a2fed488
3 changed files with 4 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
config/pnkx_local_planner_params.yaml
View File

@@ -1,5 +1,5 @@
yaw_goal_tolerance: 0.02 yaw_goal_tolerance: 0.02
xy_goal_tolerance: 0.02 xy_goal_tolerance: 0.03
min_approach_linear_velocity: 0.05 min_approach_linear_velocity: 0.05
LocalPlannerAdapter: LocalPlannerAdapter:
@@ -95,7 +95,7 @@ MKTAlgorithmDiffPredictiveTrajectory:
min_lookahead_dist: 0.6 # The minimum lookahead distance (m) threshold. (default: 0.3) min_lookahead_dist: 0.6 # The minimum lookahead distance (m) threshold. (default: 0.3)
max_lookahead_dist: 2.0 # The maximum lookahead distance (m) threshold. (default: 0.9) max_lookahead_dist: 2.0 # The maximum lookahead distance (m) threshold. (default: 0.9)
lookahead_time: 1.9 # The time (s) to project the velocity by, a.k.a. lookahead gain. (default: 1.5) lookahead_time: 1.9 # The time (s) to project the velocity by, a.k.a. lookahead gain. (default: 1.5)
min_journey_squared: 0.35 # Minimum squared journey to consider for goal (default: 0.2) min_journey_squared: 0.2 # Minimum squared journey to consider for goal (default: 0.2)
max_journey_squared: 0.5 # Maximum squared journey to consider for goal (default: 0.2) max_journey_squared: 0.5 # Maximum squared journey to consider for goal (default: 0.2)
max_lateral_accel: 0.9 # Max lateral accel for speed reduction on curves (m/s^2) max_lateral_accel: 0.9 # Max lateral accel for speed reduction on curves (m/s^2)

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

@@ -206,7 +206,7 @@ bool score_algorithm::ScoreAlgorithm::computePlanCommand(const robot_nav_2d_msgs
// Process index_s with multiple elements // Process index_s with multiple elements
if (index_s.size() > 1) if (index_s.size() > 1)
{ {
for (size_t i = 0; i < index_s.size(); ++i) for (size_t i = 1; i < index_s.size(); ++i)
{ {
if (index_s[i - 1] >= (unsigned int)global_plan.poses.size() || index_s[i] >= (unsigned int)global_plan.poses.size()) if (index_s[i - 1] >= (unsigned int)global_plan.poses.size() || index_s[i] >= (unsigned int)global_plan.poses.size())
{ {

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

@@ -521,7 +521,7 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
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;
robot_geometry_msgs::Pose2D back_pose = transformed_plan.poses.back().pose; 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(atan2(back_pose.y, back_pose.x) - back_pose.theta) > M_PI / 3.0;
allow_rotate &= (fabs(transformed_plan.poses.front().pose.y) <= 0.5); allow_rotate &= (fabs(transformed_plan.poses.front().pose.y) <= 0.5);