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

Merge remote-tracking branch 'origin/3.0' into awc-devel

Browse Source
This commit is contained in:
hoangson02
2026-03-24 08:06:47 +00:00
parent f69c3eac9f 5375a5ea84
commit 37c7707582
1 changed files with 4 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -477,6 +477,7 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
} }
double v_max = sign_x > 0 ? traj_->getTwistLinear(true).x : traj_->getTwistLinear(false).x; double v_max = sign_x > 0 ? traj_->getTwistLinear(true).x : traj_->getTwistLinear(false).x;
drive_cmd.x = std::min(sqrt(twist.x * twist.x), fabs(v_max)); drive_cmd.x = std::min(sqrt(twist.x * twist.x), fabs(v_max));
robot_nav_2d_msgs::Path2D transformed_plan = this->transform_plan_; robot_nav_2d_msgs::Path2D transformed_plan = this->transform_plan_;
if (transformed_plan.poses.empty()) if (transformed_plan.poses.empty())
{ {
@@ -512,7 +513,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;
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))
{ {
@@ -548,7 +549,6 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
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);
carrot_pose = *getLookAheadPoint(velocity, lookahead_dist, transformed_plan); carrot_pose = *getLookAheadPoint(velocity, lookahead_dist, transformed_plan);
// Normal Pure Pursuit // Normal Pure Pursuit
this->computePurePursuit( this->computePurePursuit(
carrot_pose, carrot_pose,
@@ -585,6 +585,8 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
break; break;
} }
if(fabs(v_max == 0.0))
drive_cmd.x = 0.0;
result.velocity = drive_cmd; result.velocity = drive_cmd;
prevous_drive_cmd_ = drive_cmd; prevous_drive_cmd_ = drive_cmd;
return result; return result;