DuongTD/data_convert
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

update function getYaw template

Browse Source
This commit is contained in:
duongtd
2026-05-04 10:51:11 +07:00
parent 004dd7f0ff
commit 1cf62c0856
1 changed files with 4 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files

7
include/data_convert/data_convert.h
View File

@@ -12,10 +12,11 @@
namespace data_convert
{
inline double getYaw(const robot_geometry_msgs::Quaternion& q)
template<typename T>
inline double getYaw(const T& value)
{
double siny_cosp = 2.0 * (q.w * q.z + q.x * q.y);
double cosy_cosp = 1.0 - 2.0 * (q.y * q.y + q.z * q.z);
double siny_cosp = 2.0 * (value.w * value.z + value.x * value.y);
double cosy_cosp = 1.0 - 2.0 * (value.y * value.y + value.z * value.z);
return std::atan2(siny_cosp, cosy_cosp);
}