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 +9 -7
1 changed files with 9 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Showing only changes of commit 5624c44574 - Show all commits

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) {
angles::normalize_angle(yaw + M_PI / 2);
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.20;
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);
yaw += M_PI/2;
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_) {
visualization_msgs::msg::Marker marker;
marker.lifetime.sec = 1.0;
marker.header = pose_global.header;
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.y = 0.02;
marker.scale.z = 0.02;