From 8b22de38ac16eccce348e505e12cfafdf33e2943 Mon Sep 17 00:00:00 2001
From: duongtd <tdd149cm@gmail.com>
Date: Fri, 19 Dec 2025 15:31:18 +0700
Subject: [PATCH] add function createQuaternionMsgFromYaw

---
 include/data_convert/data_convert.h | 42 ++++++++++-------------------
 1 file changed, 14 insertions(+), 28 deletions(-)

diff --git a/include/data_convert/data_convert.h b/include/data_convert/data_convert.h
index 1597a42..35dc295 100644
--- a/include/data_convert/data_convert.h
+++ b/include/data_convert/data_convert.h
@@ -10,36 +10,8 @@
 #include <robot/time.h>
 #include <cmath>
 
-// template <typename T>
-// bool isEqual(const T& a, const T& b, double eps = 1e-5)
-// {
-//     return std::fabs(a - b) < eps;
-// }
-
-// inline bool operator==(const geometry_msgs::Quaternion& a, const geometry_msgs::Quaternion& b)
-// {
-//     return isEqual(a.x, b.x) && isEqual(a.y, b.y) && isEqual(a.z, b.z) && isEqual(a.w, b.w);
-// }
-
-// inline bool operator==(const geometry_msgs::Point& a, const geometry_msgs::Point& b)
-// {
-//     return isEqual(a.x, b.x) && isEqual(a.y, b.y) && isEqual(a.z, b.z);
-// }
-
-// inline bool operator==(const geometry_msgs::Pose& a, const geometry_msgs::Pose& b)
-// {
-//     return (a.position == b.position) && (a.orientation == b.orientation);
-// }
-
-// inline bool operator==(const geometry_msgs::PoseStamped& a, const geometry_msgs::PoseStamped& b)
-// {
-//     return (a.header.frame_id == b.header.frame_id) &&
-//         (a.pose == b.pose);
-// }
 namespace data_convert
 {
-
-
     inline double getYaw(const geometry_msgs::Quaternion& q)
     {
         double siny_cosp = 2.0 * (q.w * q.z + q.x * q.y);
@@ -88,6 +60,20 @@ namespace data_convert
         out.w = q.w();
         return out;
     }
+
+    inline geometry_msgs::Quaternion createQuaternionMsgFromYaw(const double& yaw)
+    {
+        tf3::Quaternion q;
+        q.setRPY(0.0, 0.0, yaw);
+
+        geometry_msgs::Quaternion q_msg;
+        q_msg.x = q.x();
+        q_msg.y = q.y();
+        q_msg.z = q.z();
+        q_msg.w = q.w();
+
+        return q_msg;
+    }
     
     inline tf3::Transform convertTotf3Transform(const geometry_msgs::Transform& msg)
     {