commit 411dc68e84866dc0bb455fa61ff4e92e82285c9e Author: duongtd Date: Wed Nov 26 16:48:29 2025 +0700 first commit diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..6762953 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,7 @@ +cmake_minimum_required(VERSION 3.10) +project(data_convert) + +set(CMAKE_CXX_STANDARD 17) + +add_library(data_convert INTERFACE) +target_include_directories(data_convert INTERFACE ${CMAKE_CURRENT_SOURCE_DIR}/include) diff --git a/include/data_convert/data_convert.h b/include/data_convert/data_convert.h new file mode 100644 index 0000000..87354f5 --- /dev/null +++ b/include/data_convert/data_convert.h @@ -0,0 +1,202 @@ +#ifndef COSTMAP_2D_DATA_CONVERT_H +#define COSTMAP_2D_DATA_CONVERT_H + +#include +#include +#include +#include +#include +#include +#include +#include + +template +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); + double cosy_cosp = 1.0 - 2.0 * (q.y * q.y + q.z * q.z); + return std::atan2(siny_cosp, cosy_cosp); + } + + inline geometry_msgs::Quaternion getQuaternion(const double& yaw) + { + geometry_msgs::Quaternion q; + q.x = 0.0; + q.y = 0.0; + q.z = std::sin(yaw / 2.0); + q.w = std::cos(yaw / 2.0); + return q; + } + + inline robot::Time convertTime(tf3::Time time) + { + robot::Time time_tmp; + time_tmp.sec = time.sec; + time_tmp.nsec = time.nsec; + return time_tmp; + } + + inline tf3::Time convertTime(robot::Time time) + { + tf3::Time time_tmp; + time_tmp.sec = time.sec; + time_tmp.nsec = time.nsec; + return time_tmp; + } + + inline tf3::Quaternion convertQuaternion(const geometry_msgs::Quaternion& q) + { + tf3::Quaternion out(q.x,q.y,q.z,q.w); + return out; + } + + inline geometry_msgs::Quaternion convertQuaternion(const tf3::Quaternion& q) + { + return geometry_msgs::Quaternion(q.x(),q.y(),q.z(),q.w()); + } + + inline tf3::Transform convertTotf3Transform(const geometry_msgs::Transform& msg) + { + tf3::Quaternion q( + msg.rotation.x, + msg.rotation.y, + msg.rotation.z, + msg.rotation.w + ); + + tf3::Vector3 t( + msg.translation.x, + msg.translation.y, + msg.translation.z + ); + + tf3::Transform tf(q, t); + return tf; + } + + inline tf3::Transform convertTotf3Transform(const tf3::TransformMsg& msg) + { + tf3::Quaternion q( + msg.rotation.x, + msg.rotation.y, + msg.rotation.z, + msg.rotation.w + ); + + tf3::Vector3 t( + msg.translation.x, + msg.translation.y, + msg.translation.z + ); + + tf3::Transform tf(q, t); + return tf; + } + + inline tf3::Transform convertTotf3Transform(const tf3::TransformStampedMsg& msg) + { + tf3::Transform out; + + out.setOrigin(tf3::Vector3( + msg.transform.translation.x, + msg.transform.translation.y, + msg.transform.translation.z + )); + + tf3::Quaternion q( + msg.transform.rotation.x, + msg.transform.rotation.y, + msg.transform.rotation.z, + msg.transform.rotation.w + ); + out.setBasis(tf3::Matrix3x3(q)); + + return out; + } + + inline tf3::Transform convertTotf3Transform(const geometry_msgs::TransformStamped& msg) + { + tf3::Transform out; + + out.setOrigin(tf3::Vector3( + msg.transform.translation.x, + msg.transform.translation.y, + msg.transform.translation.z + )); + + tf3::Quaternion q( + msg.transform.rotation.x, + msg.transform.rotation.y, + msg.transform.rotation.z, + msg.transform.rotation.w + ); + out.setBasis(tf3::Matrix3x3(q)); + + return out; + } + + inline tf3::TransformStampedMsg convertToTransformStampedMsg(const geometry_msgs::TransformStamped& msg) + { + tf3::TransformStampedMsg out; + out.header.seq = msg.header.seq; + out.header.stamp = convertTime(msg.header.stamp); + out.header.frame_id = msg.header.frame_id; + out.child_frame_id = msg.child_frame_id; + out.transform.translation.x = msg.transform.translation.x; + out.transform.translation.y = msg.transform.translation.y; + out.transform.translation.z = msg.transform.translation.z; + out.transform.rotation.x = msg.transform.rotation.x; + out.transform.rotation.y = msg.transform.rotation.y; + out.transform.rotation.z = msg.transform.rotation.z; + out.transform.rotation.w = msg.transform.rotation.w; + return out; + } + + inline geometry_msgs::TransformStamped convertToTransformStamped(const tf3::TransformStampedMsg& msg) + { + geometry_msgs::TransformStamped out; + out.header.seq = msg.header.seq; + out.header.stamp = convertTime(msg.header.stamp); + out.header.frame_id = msg.header.frame_id; + out.child_frame_id = msg.child_frame_id; + out.transform.translation.x = msg.transform.translation.x; + out.transform.translation.y = msg.transform.translation.y; + out.transform.translation.z = msg.transform.translation.z; + out.transform.rotation.x = msg.transform.rotation.x; + out.transform.rotation.y = msg.transform.rotation.y; + out.transform.rotation.z = msg.transform.rotation.z; + out.transform.rotation.w = msg.transform.rotation.w; + return out; + } +} + +#endif // DATA_CONVERT_H \ No newline at end of file