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

fixed target angle

Browse Source
This commit is contained in:
Pimpest
2026-04-15 16:05:13 +02:00
parent 9abbd93b1a
commit ac402a584c
1 changed files with 2 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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_);
const double dx = initial_pose_.position.x - pose_global.pose.position.x;
const double dy = initial_pose_.position.y - pose_global.pose.position.y;
const double dx = pose_global.pose.position.x - initial_pose_.position.x;
const double dy = pose_global.pose.position.y - initial_pose_.position.y;
double yaw_to_goal = std::atan2(dy,dx);
tf2::convert(tf2::Quaternion::createFromRPY(0, 0, yaw_to_goal), pose_global.pose.orientation);