CopterExpress/clover
1
0
Fork 0
mirror of https://github.com/CopterExpress/clover.git synced 2026-06-02 16:09:32 +00:00
Code Issues Packages Projects Releases Wiki Activity

aruco_detect: fix single markers frame (with snapping)

Browse Source
This commit is contained in:
Oleg Kalachev
2019-02-12 00:15:59 +03:00
parent e61ea4adc8
commit 9cf6524ad6
1 changed files with 1 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files

12
aruco_pose/src/aruco_detect.cpp
View File

@@ -153,7 +153,7 @@ private:
// snap orientation (if enabled and snap frame avaiable)
if (!snap_orientation_.empty() && !snap_to.header.frame_id.empty()) {
snapOrientation(marker.pose.pose, snap_to.transform.rotation);
snapOrientation(marker.pose.pose.orientation, snap_to.transform.rotation);
}
// TODO: check IDs are unique
@@ -275,16 +275,6 @@ private:
vis_array_.markers.push_back(marker);
}
void snapOrientation(geometry_msgs::Pose& pose, const geometry_msgs::Quaternion orientation)
{
tf::Quaternion q;
q.setRPY(0, 0, -tf::getYaw(pose.orientation) + tf::getYaw(orientation) + M_PI / 2);
tf::Quaternion pq;
tf::quaternionMsgToTF(orientation, pq);
pq = pq * q;
tf::quaternionTFToMsg(pq, pose.orientation);
}
inline std::string getChildFrameId(int id) const
{
return frame_id_prefix_ + std::to_string(id);