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

HiepLM update

Browse Source
This commit is contained in:
HiepLM 2025-12-30 09:07:01 +07:00
parent 8b22de38ac
commit b1aaa1a946
2 changed files with 21 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
CMakeLists.txt
View File

@ -5,6 +5,10 @@ set(CMAKE_CXX_STANDARD 17)
add_library(data_convert INTERFACE) add_library(data_convert INTERFACE)
target_link_libraries(data_convert INTERFACE
robot_geometry_msgs
)
target_include_directories(data_convert target_include_directories(data_convert
INTERFACE INTERFACE
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include> $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>

34
include/data_convert/data_convert.h
View File

@ -1,10 +1,10 @@
#ifndef COSTMAP_2D_DATA_CONVERT_H #ifndef COSTMAP_2D_DATA_CONVERT_H
#define COSTMAP_2D_DATA_CONVERT_H #define COSTMAP_2D_DATA_CONVERT_H
#include <geometry_msgs/TransformStamped.h> #include <robot_geometry_msgs/TransformStamped.h>
#include <geometry_msgs/Quaternion.h> #include <robot_geometry_msgs/Quaternion.h>
#include <geometry_msgs/Pose.h> #include <robot_geometry_msgs/Pose.h>
#include <geometry_msgs/PoseStamped.h> #include <robot_geometry_msgs/PoseStamped.h>
#include <tf3/utils.h> #include <tf3/utils.h>
#include <tf3/compat.h> #include <tf3/compat.h>
#include <robot/time.h> #include <robot/time.h>
@ -12,16 +12,16 @@
namespace data_convert namespace data_convert
{ {
inline double getYaw(const geometry_msgs::Quaternion& q) inline double getYaw(const robot_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);
double cosy_cosp = 1.0 - 2.0 * (q.y * q.y + q.z * q.z); double cosy_cosp = 1.0 - 2.0 * (q.y * q.y + q.z * q.z);
return std::atan2(siny_cosp, cosy_cosp); return std::atan2(siny_cosp, cosy_cosp);
} }
inline geometry_msgs::Quaternion getQuaternion(const double& yaw) inline robot_geometry_msgs::Quaternion getQuaternion(const double& yaw)
{ {
geometry_msgs::Quaternion q; robot_geometry_msgs::Quaternion q;
q.x = 0.0; q.x = 0.0;
q.y = 0.0; q.y = 0.0;
q.z = std::sin(yaw / 2.0); q.z = std::sin(yaw / 2.0);
@ -45,15 +45,15 @@ namespace data_convert
return time_tmp; return time_tmp;
} }
inline tf3::Quaternion convertQuaternion(const geometry_msgs::Quaternion& q) inline tf3::Quaternion convertQuaternion(const robot_geometry_msgs::Quaternion& q)
{ {
tf3::Quaternion out(q.x,q.y,q.z,q.w); tf3::Quaternion out(q.x,q.y,q.z,q.w);
return out; return out;
} }
inline geometry_msgs::Quaternion convertQuaternion(const tf3::Quaternion& q) inline robot_geometry_msgs::Quaternion convertQuaternion(const tf3::Quaternion& q)
{ {
geometry_msgs::Quaternion out; robot_geometry_msgs::Quaternion out;
out.x = q.x(); out.x = q.x();
out.y = q.y(); out.y = q.y();
out.z = q.z(); out.z = q.z();
@ -61,12 +61,12 @@ namespace data_convert
return out; return out;
} }
inline geometry_msgs::Quaternion createQuaternionMsgFromYaw(const double& yaw) inline robot_geometry_msgs::Quaternion createQuaternionMsgFromYaw(const double& yaw)
{ {
tf3::Quaternion q; tf3::Quaternion q;
q.setRPY(0.0, 0.0, yaw); q.setRPY(0.0, 0.0, yaw);
geometry_msgs::Quaternion q_msg; robot_geometry_msgs::Quaternion q_msg;
q_msg.x = q.x(); q_msg.x = q.x();
q_msg.y = q.y(); q_msg.y = q.y();
q_msg.z = q.z(); q_msg.z = q.z();
@ -75,7 +75,7 @@ namespace data_convert
return q_msg; return q_msg;
} }
inline tf3::Transform convertTotf3Transform(const geometry_msgs::Transform& msg) inline tf3::Transform convertTotf3Transform(const robot_geometry_msgs::Transform& msg)
{ {
tf3::Quaternion q( tf3::Quaternion q(
msg.rotation.x, msg.rotation.x,
@ -134,7 +134,7 @@ namespace data_convert
return out; return out;
} }
inline tf3::Transform convertTotf3Transform(const geometry_msgs::TransformStamped& msg) inline tf3::Transform convertTotf3Transform(const robot_geometry_msgs::TransformStamped& msg)
{ {
tf3::Transform out; tf3::Transform out;
@ -155,7 +155,7 @@ namespace data_convert
return out; return out;
} }
inline tf3::TransformStampedMsg convertToTransformStampedMsg(const geometry_msgs::TransformStamped& msg) inline tf3::TransformStampedMsg convertToTransformStampedMsg(const robot_geometry_msgs::TransformStamped& msg)
{ {
tf3::TransformStampedMsg out; tf3::TransformStampedMsg out;
out.header.seq = msg.header.seq; out.header.seq = msg.header.seq;
@ -172,9 +172,9 @@ namespace data_convert
return out; return out;
} }
inline geometry_msgs::TransformStamped convertToTransformStamped(const tf3::TransformStampedMsg& msg) inline robot_geometry_msgs::TransformStamped convertToTransformStamped(const tf3::TransformStampedMsg& msg)
{ {
geometry_msgs::TransformStamped out; robot_geometry_msgs::TransformStamped out;
out.header.seq = msg.header.seq; out.header.seq = msg.header.seq;
out.header.stamp = convertTime(msg.header.stamp); out.header.stamp = convertTime(msg.header.stamp);
out.header.frame_id = msg.header.frame_id; out.header.frame_id = msg.header.frame_id;