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

uodate lần 3

Browse Source
This commit is contained in:
HiepLM
2026-04-23 18:26:13 +07:00
parent 0c65a5b6ba
commit 5812542eaf
2 changed files with 4 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -219,7 +219,7 @@ 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 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; 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.1) if (std::abs(std::sqrt(dx * dx + dy * dy)) <= xy_local_goal_tolerance_ + 0.3)
{ {
double tolerance = fabs(cos(theta)) >= fabs(sin(theta)) ? x : y; double tolerance = fabs(cos(theta)) >= fabs(sin(theta)) ? x : y;
if (fabs(tolerance) <= xy_local_goal_tolerance_) if (fabs(tolerance) <= xy_local_goal_tolerance_)

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

@@ -518,8 +518,8 @@ 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; // allow_rotate &= fabs(transformed_plan.poses.front().pose.y) <= 0.1;
// robot::log_info("pose.y %f", fabs(transformed_plan.poses.front().pose.y)); // robot::log_info("pose.y %f", fabs(transformed_plan.poses.front().pose.y));
double angle_to_heading; double angle_to_heading;
@@ -1268,7 +1268,7 @@ robot_nav_2d_msgs::Path2D mkt_algorithm::diff::PredictiveTrajectory::generateTra
{ {
if(fabs(path.poses.back().pose.x) * 0.9 < min_lookahead_dist_) if(fabs(path.poses.back().pose.x) * 0.9 < min_lookahead_dist_)
{ {
if(fabs(path.poses.back().pose.x) < min_journey_squared_ && fabs(path.poses.back().pose.theta) < 0.05) if(fabs(path.poses.back().pose.x) < min_journey_squared_)
drive_cmd.theta = 0.01; drive_cmd.theta = 0.01;
return generateParallelPath(path, sign_x); return generateParallelPath(path, sign_x);
} }