Pimpest/toid
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Fix target update distance

Browse Source
This commit is contained in:
Pimpest
2026-04-15 13:23:31 +02:00
parent 1558ca1217
commit 2619a9b0b5
1 changed files with 2 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
toid_behaviors/src/approach_acorns.cpp
View File

@@ -98,7 +98,7 @@ void ApproachAcorns::acorn_position_cb(PoseStamped::ConstSharedPtr msg)
} }
// Smooth out approach // Smooth out approach
if (x * x + y * y < 0.42) { if (x * x + y * y < 0.42 * 0.42) {
return; return;
} }
@@ -275,7 +275,7 @@ ResultStatus ApproachAcorns::updateVel(
return ResultStatus{Status::RUNNING}; return ResultStatus{Status::RUNNING};
} }
if (dist_left <= 0.02) { if (dist_left >= 0.001 && dist_left <= 0.02) {
out_vel.linear.x = velocityTarget(dist_left); out_vel.linear.x = velocityTarget(dist_left);
out_vel.angular.z = std::clamp(kp_ * angle_dist, -max_angular_speed_, max_angular_speed_); out_vel.angular.z = std::clamp(kp_ * angle_dist, -max_angular_speed_, max_angular_speed_);
last_speed_ = out_vel.linear.x; last_speed_ = out_vel.linear.x;