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

first commit

Browse Source
This commit is contained in:
duongtd 2025-11-26 16:48:29 +07:00
commit 411dc68e84
2 changed files with 209 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
CMakeLists.txt Normal file
View File

@ -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)

202
include/data_convert/data_convert.h Normal file
View File

@ -0,0 +1,202 @@
#ifndef COSTMAP_2D_DATA_CONVERT_H
#define COSTMAP_2D_DATA_CONVERT_H
#include <geometry_msgs/TransformStamped.h>
#include <geometry_msgs/Quaternion.h>
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/PoseStamped.h>
#include <tf3/utils.h>
#include <tf3/compat.h>
#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);
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