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

14
toid_behaviors/src/approach_acorns.cpp
View File

@@ -96,14 +96,16 @@ 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;
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); tf2::convert(tf2::Quaternion::createFromRPY(0,0,yaw ), pose_global.pose.orientation);
if(debug_marker_) { if(debug_marker_) {
@@ -111,7 +113,7 @@ void ApproachAcorns::acorn_position_cb(PoseStamped::ConstSharedPtr msg)
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;