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

add function createQuaternionMsgFromYaw

Browse Source
This commit is contained in:
duongtd 2025-12-19 15:31:18 +07:00
parent 03a8fa3a80
commit 8b22de38ac
1 changed files with 14 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

42
include/data_convert/data_convert.h
View File

@ -10,36 +10,8 @@
#include <robot/time.h> #include <robot/time.h>
#include <cmath> #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 namespace data_convert
{ {
inline double getYaw(const geometry_msgs::Quaternion& q) inline double getYaw(const geometry_msgs::Quaternion& q)
{ {
double siny_cosp = 2.0 * (q.w * q.z + q.x * q.y); double siny_cosp = 2.0 * (q.w * q.z + q.x * q.y);
@ -89,6 +61,20 @@ namespace data_convert
return out; 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) inline tf3::Transform convertTotf3Transform(const geometry_msgs::Transform& msg)
{ {
tf3::Quaternion q( tf3::Quaternion q(