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

fixed approac acorn target offset

Browse Source
This commit is contained in:
Pimpest
2026-04-11 16:47:57 +02:00
parent 7f6cfbeb87
commit 5624c44574
1 changed files with 9 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files

16
toid_behaviors/src/approach_acorns.cpp
View File

@@ -96,22 +96,24 @@ void ApproachAcorns::acorn_position_cb(PoseStamped::ConstSharedPtr msg)
} }
double yaw = tf2::getYaw(pose_global.pose.orientation); double yaw = angles::normalize_angle(tf2::getYaw(pose_global.pose.orientation));
if (yaw < 0) { if(tf2::getYaw(pose_local.pose.orientation) > 0) {
angles::normalize_angle(yaw + M_PI / 2); yaw += M_PI;
} }
pose_global.pose.position.x += std::cos(yaw) * 0.01 + std::sin(yaw) * -0.20; yaw += M_PI/2;
pose_global.pose.position.y += -std::cos(yaw) * 0.01 + std::cos(yaw) * -0.20;
tf2::convert(tf2::Quaternion::createFromRPY(0,0,yaw), pose_global.pose.orientation); pose_global.pose.position.x += std::cos(yaw) * -0.35 - std::sin(yaw) * + 0.005;
pose_global.pose.position.y += std::sin(yaw) * -0.35 + std::cos(yaw) * + 0.005;
tf2::convert(tf2::Quaternion::createFromRPY(0,0,yaw ), pose_global.pose.orientation);
if(debug_marker_) { if(debug_marker_) {
visualization_msgs::msg::Marker marker; visualization_msgs::msg::Marker marker;
marker.lifetime.sec = 1.0; marker.lifetime.sec = 1.0;
marker.header = pose_global.header; marker.header = pose_global.header;
marker.pose = pose_global.pose; marker.pose = pose_global.pose;
marker.type = visualization_msgs::msg::Marker::SPHERE; marker.type = visualization_msgs::msg::Marker::ARROW;
marker.scale.x = 0.02; marker.scale.x = 0.02;
marker.scale.y = 0.02; marker.scale.y = 0.02;
marker.scale.z = 0.02; marker.scale.z = 0.02;