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

wip-behaviors #3

Merged
Pimpest merged 86 commits from wip-behaviors into main 2026-05-28 06:21:26 +00:00
Conversation 0 Commits 86 Files Changed 129 +2 -2
1 changed files with 2 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Showing only changes of commit 2619a9b0b5 - Show all commits

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;