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

update xoay

Browse Source
This commit is contained in:
HiepLM
2026-04-23 17:57:31 +07:00
parent 1616ac8d7b
commit 0c65a5b6ba
1 changed files with 2 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -518,7 +518,7 @@ 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));
@@ -723,7 +723,7 @@ 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], -fabs(drive_target.theta), fabs(drive_target.theta)); drive_cmd.theta = std::clamp(kf_->state()[3], -fabs(drive_target.theta), fabs(drive_target.theta));
robot::log_info("drive_cmd.theta: %f, drive_target.theta: %f", drive_cmd.theta, drive_target.theta); // robot::log_info("drive_cmd.theta: %f, drive_target.theta: %f", drive_cmd.theta, drive_target.theta);
} }
void mkt_algorithm::diff::PredictiveTrajectory::applyDistanceSpeedScaling( void mkt_algorithm::diff::PredictiveTrajectory::applyDistanceSpeedScaling(