From d3588f0bb53936a41f4edbc4b8124d57fb399cb8 Mon Sep 17 00:00:00 2001 From: duongtd Date: Wed, 26 Nov 2025 15:01:26 +0700 Subject: [PATCH] update file data_convert.h --- .../include/tf3_geometry_msgs/data_convert.h | 16 ++++++++-------- .../test/test_tf2_geometry_msgs.cpp | 6 ++++++ .../include/tf3_sensor_msgs/data_convert.h | 16 ++++++++-------- 3 files changed, 22 insertions(+), 16 deletions(-) diff --git a/tf3_geometry_msgs/include/tf3_geometry_msgs/data_convert.h b/tf3_geometry_msgs/include/tf3_geometry_msgs/data_convert.h index bd86b16..e30cf55 100644 --- a/tf3_geometry_msgs/include/tf3_geometry_msgs/data_convert.h +++ b/tf3_geometry_msgs/include/tf3_geometry_msgs/data_convert.h @@ -11,7 +11,7 @@ namespace tf3 { - robot::Time convertTime(const tf3::Time& time) + inline robot::Time convertTime(const tf3::Time& time) { robot::Time time_tmp; time_tmp.sec = time.sec; @@ -19,7 +19,7 @@ namespace tf3 return time_tmp; } - tf3::Time convertTime(const robot::Time& time) + inline tf3::Time convertTime(const robot::Time& time) { tf3::Time time_tmp; time_tmp.sec = time.sec; @@ -27,18 +27,18 @@ namespace tf3 return time_tmp; } - tf3::Quaternion convertQuaternion(const geometry_msgs::Quaternion& q) + inline tf3::Quaternion convertQuaternion(const geometry_msgs::Quaternion& q) { tf3::Quaternion out(q.x,q.y,q.z,q.w); return out; } - geometry_msgs::Quaternion convertQuaternion(const tf3::Quaternion& q) + inline geometry_msgs::Quaternion convertQuaternion(const tf3::Quaternion& q) { return geometry_msgs::Quaternion(q.x(),q.y(),q.z(),q.w()); } - tf3::Transform convertToTransform(const tf3::TransformStampedMsg& msg) + inline tf3::Transform convertToTransform(const tf3::TransformStampedMsg& msg) { tf3::Transform out; @@ -59,7 +59,7 @@ namespace tf3 return out; } - tf3::Transform convertToTransform(const geometry_msgs::TransformStamped& msg) + inline tf3::Transform convertToTransform(const geometry_msgs::TransformStamped& msg) { tf3::Transform out; @@ -81,7 +81,7 @@ namespace tf3 } - tf3::TransformStampedMsg convertToTransformStampedMsg(const geometry_msgs::TransformStamped& msg) + inline tf3::TransformStampedMsg convertToTransformStampedMsg(const geometry_msgs::TransformStamped& msg) { tf3::TransformStampedMsg out; out.header.seq = msg.header.seq; @@ -98,7 +98,7 @@ namespace tf3 return out; } - geometry_msgs::TransformStamped convertToTransformStamped(const tf3::TransformStampedMsg& msg) + inline geometry_msgs::TransformStamped convertToTransformStamped(const tf3::TransformStampedMsg& msg) { geometry_msgs::TransformStamped out; out.header.seq = msg.header.seq; diff --git a/tf3_geometry_msgs/test/test_tf2_geometry_msgs.cpp b/tf3_geometry_msgs/test/test_tf2_geometry_msgs.cpp index b5dac40..7bf0911 100755 --- a/tf3_geometry_msgs/test/test_tf2_geometry_msgs.cpp +++ b/tf3_geometry_msgs/test/test_tf2_geometry_msgs.cpp @@ -252,6 +252,12 @@ TEST(TfGeometry, PoseWithCovarianceStamped) int main(int argc, char **argv){ testing::InitGoogleTest(&argc, argv); + // std::vector data = {2,2,7,4,5}; + // for(int i = 0; i < data.size(); i++) { + // std::cout << data.size() << std::endl; + // std::cout<< i << " " << data[i] << std::endl; + // } + tf_buffer = new tf3::BufferCore(); tf_buffer->setUsingDedicatedThread(true); // std::cout << "STEP 1" << std::endl; diff --git a/tf3_sensor_msgs/include/tf3_sensor_msgs/data_convert.h b/tf3_sensor_msgs/include/tf3_sensor_msgs/data_convert.h index 1d377b9..01cf92a 100644 --- a/tf3_sensor_msgs/include/tf3_sensor_msgs/data_convert.h +++ b/tf3_sensor_msgs/include/tf3_sensor_msgs/data_convert.h @@ -11,7 +11,7 @@ namespace tf3 { - robot::Time convertTime(const tf3::Time& time) + inline robot::Time convertTime(const tf3::Time& time) { robot::Time time_tmp; time_tmp.sec = time.sec; @@ -19,7 +19,7 @@ namespace tf3 return time_tmp; } - tf3::Time convertTime(const robot::Time& time) + inline tf3::Time convertTime(const robot::Time& time) { tf3::Time time_tmp; time_tmp.sec = time.sec; @@ -27,18 +27,18 @@ namespace tf3 return time_tmp; } - tf3::Quaternion convertQuaternion(const geometry_msgs::Quaternion& q) + inline tf3::Quaternion convertQuaternion(const geometry_msgs::Quaternion& q) { tf3::Quaternion out(q.x,q.y,q.z,q.w); return out; } - geometry_msgs::Quaternion convertQuaternion(const tf3::Quaternion& q) + inline geometry_msgs::Quaternion convertQuaternion(const tf3::Quaternion& q) { return geometry_msgs::Quaternion(q.x(),q.y(),q.z(),q.w()); } - tf3::Transform convertToTransform(const tf3::TransformStampedMsg& msg) + inline tf3::Transform convertToTransform(const tf3::TransformStampedMsg& msg) { tf3::Transform out; @@ -59,7 +59,7 @@ namespace tf3 return out; } - tf3::Transform convertToTransform(const geometry_msgs::TransformStamped& msg) + inline tf3::Transform convertToTransform(const geometry_msgs::TransformStamped& msg) { tf3::Transform out; @@ -81,7 +81,7 @@ namespace tf3 } - tf3::TransformStampedMsg convertToTransformStampedMsg(const geometry_msgs::TransformStamped& msg) + inline tf3::TransformStampedMsg convertToTransformStampedMsg(const geometry_msgs::TransformStamped& msg) { tf3::TransformStampedMsg out; out.header.seq = msg.header.seq; @@ -98,7 +98,7 @@ namespace tf3 return out; } - geometry_msgs::TransformStamped convertToTransformStamped(const tf3::TransformStampedMsg& msg) + inline geometry_msgs::TransformStamped convertToTransformStamped(const tf3::TransformStampedMsg& msg) { geometry_msgs::TransformStamped out; out.header.seq = msg.header.seq;