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); }