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

Fixed camera based approach

Browse Source
This commit was merged in pull request #3.
This commit is contained in:
Pimpest
2026-04-18 04:32:02 +02:00
parent a1c39cb943
commit be9744ac47
2 changed files with 11 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files

14
toid_behaviors/src/approach_acorns.cpp
View File

@@ -93,7 +93,7 @@ void ApproachAcorns::acorn_position_cb(PoseStamped::ConstSharedPtr msg)
double x = pose_local.pose.position.x;
double y = pose_local.pose.position.y;
if (x * x + y * y > distance_ + 0.005) {
if (std::sqrt(x * x + y * y) > distance_ + 0.005) {
return;
}
@@ -105,15 +105,13 @@ void ApproachAcorns::acorn_position_cb(PoseStamped::ConstSharedPtr msg)
double yaw = angles::normalize_angle(tf2::getYaw(pose_global.pose.orientation));
if (tf2::getYaw(pose_local.pose.orientation) > 0) {
yaw += M_PI;
pose_global.pose.position.x += std::cos(yaw) * 0.01 - std::sin(yaw) * 0.35;
pose_global.pose.position.y += std::sin(yaw) * 0.01 + std::cos(yaw) * 0.35;
} else {
pose_global.pose.position.x += std::cos(yaw) * -0.01 - std::sin(yaw) * -0.35;
pose_global.pose.position.y += std::sin(yaw) * -0.01 + std::cos(yaw) * -0.35;
}
yaw += M_PI / 2;
pose_global.pose.position.x += std::cos(yaw) * -0.35 - std::sin(yaw) * 0.010;
pose_global.pose.position.y += std::sin(yaw) * -0.35 + std::cos(yaw) * 0.010;
std::lock_guard _lock(mutex_);
const double dx = pose_global.pose.position.x - initial_pose_.position.x;