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 ac402a584c - Show all commits

4
toid_behaviors/src/approach_acorns.cpp
View File

@@ -116,8 +116,8 @@ void ApproachAcorns::acorn_position_cb(PoseStamped::ConstSharedPtr msg)
std::lock_guard _lock(mutex_); std::lock_guard _lock(mutex_);
const double dx = initial_pose_.position.x - pose_global.pose.position.x; const double dx = pose_global.pose.position.x - initial_pose_.position.x;
const double dy = initial_pose_.position.y - pose_global.pose.position.y; const double dy = pose_global.pose.position.y - initial_pose_.position.y;
double yaw_to_goal = std::atan2(dy,dx); double yaw_to_goal = std::atan2(dy,dx);
tf2::convert(tf2::Quaternion::createFromRPY(0, 0, yaw_to_goal), pose_global.pose.orientation); tf2::convert(tf2::Quaternion::createFromRPY(0, 0, yaw_to_goal), pose_global.pose.orientation);