From 1cf62c085687dc167e28cded970371b329a83479 Mon Sep 17 00:00:00 2001 From: duongtd Date: Mon, 4 May 2026 10:51:11 +0700 Subject: [PATCH] update function getYaw template --- include/data_convert/data_convert.h | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/include/data_convert/data_convert.h b/include/data_convert/data_convert.h index 3d3afb4..6494c1b 100644 --- a/include/data_convert/data_convert.h +++ b/include/data_convert/data_convert.h @@ -12,10 +12,11 @@ namespace data_convert { - inline double getYaw(const robot_geometry_msgs::Quaternion& q) + template + 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); }