diff --git a/tf3_geometry_msgs/CMakeLists.txt b/tf3_geometry_msgs/CMakeLists.txt index 124fe8c..8f14893 100755 --- a/tf3_geometry_msgs/CMakeLists.txt +++ b/tf3_geometry_msgs/CMakeLists.txt @@ -43,6 +43,7 @@ target_include_directories(tf3_geometry_msgs INTERFACE # Liên kết với std_msgs nếu bạn có file Header.h trong include/std_msgs/ target_link_libraries(tf3_geometry_msgs INTERFACE geometry_msgs + data_convert ) # # Test: tomsg_frommsg @@ -52,6 +53,7 @@ target_link_libraries(tf3_geometry_msgs INTERFACE tf3_geometry_msgs Threads::Threads tf3 + data_convert ) # # Test: tf2_geometry_msgs add_executable(test_geometry_msgs test/test_tf2_geometry_msgs.cpp) @@ -60,4 +62,5 @@ target_link_libraries(tf3_geometry_msgs INTERFACE Threads::Threads tf3 tf3_geometry_msgs + data_convert ) diff --git a/tf3_geometry_msgs/include/tf3_geometry_msgs/data_convert.h b/tf3_geometry_msgs/include/tf3_geometry_msgs/data_convert.h deleted file mode 100644 index e30cf55..0000000 --- a/tf3_geometry_msgs/include/tf3_geometry_msgs/data_convert.h +++ /dev/null @@ -1,119 +0,0 @@ -#ifndef DATA_CONVERT_TF3_GEOMETRY_MSGS_H -#define DATA_CONVERT_TF3_GEOMETRY_MSGS_H - -#include -#include -#include -#include -#include -#include - -namespace tf3 -{ - - inline robot::Time convertTime(const tf3::Time& time) - { - robot::Time time_tmp; - time_tmp.sec = time.sec; - time_tmp.nsec = time.nsec; - return time_tmp; - } - - inline tf3::Time convertTime(const 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 convertToTransform(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 convertToTransform(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_TF3_GEOMETRY_MSGS_H \ No newline at end of file diff --git a/tf3_geometry_msgs/include/tf3_geometry_msgs/tf3_geometry_msgs.h b/tf3_geometry_msgs/include/tf3_geometry_msgs/tf3_geometry_msgs.h index e896b51..54eca5d 100755 --- a/tf3_geometry_msgs/include/tf3_geometry_msgs/tf3_geometry_msgs.h +++ b/tf3_geometry_msgs/include/tf3_geometry_msgs/tf3_geometry_msgs.h @@ -35,7 +35,7 @@ #include #include #include -#include +#include #include #include #include @@ -111,7 +111,7 @@ void fromMsg(const geometry_msgs::Vector3& in, tf3::Vector3& out) */ template <> inline - const tf3::Time& getTimestamp(const geometry_msgs::Vector3Stamped& t) {return tf3::convertTime(t.header.stamp);} + const tf3::Time& getTimestamp(const geometry_msgs::Vector3Stamped& t) {return data_convert::convertTime(t.header.stamp);} /** \brief Extract a frame ID from the header of a Vector message. * This function is a specialization of the getFrameId template defined in tf3/convert.h. @@ -155,7 +155,7 @@ inline geometry_msgs::Vector3Stamped toMsg(const tf3::Stamped& in) { geometry_msgs::Vector3Stamped out; - out.header.stamp = tf3::convertTime(in.stamp_); + out.header.stamp = data_convert::convertTime(in.stamp_); out.header.frame_id = in.frame_id_; out.vector.x = in.getX(); out.vector.y = in.getY(); @@ -171,7 +171,7 @@ geometry_msgs::Vector3Stamped toMsg(const tf3::Stamped& in) inline void fromMsg(const geometry_msgs::Vector3Stamped& msg, tf3::Stamped& out) { - out.stamp_ = tf3::convertTime(msg.header.stamp); + out.stamp_ = data_convert::convertTime(msg.header.stamp); out.frame_id_ = msg.header.frame_id; out.setData(tf3::Vector3(msg.vector.x, msg.vector.y, msg.vector.z)); } @@ -219,7 +219,7 @@ void fromMsg(const geometry_msgs::Point& in, tf3::Vector3& out) */ template <> inline - const tf3::Time& getTimestamp(const geometry_msgs::PointStamped& t) {return tf3::convertTime(t.header.stamp);} + const tf3::Time& getTimestamp(const geometry_msgs::PointStamped& t) {return data_convert::convertTime(t.header.stamp);} /** \brief Extract a frame ID from the header of a Point message. * This function is a specialization of the getFrameId template defined in tf3/convert.h. @@ -261,7 +261,7 @@ void fromMsg(const geometry_msgs::PointStamped& msg, geometry_msgs::PointStamped inline geometry_msgs::PointStamped toMsg(const tf3::Stamped& in, geometry_msgs::PointStamped & out) { - out.header.stamp = tf3::convertTime(in.stamp_); + out.header.stamp = data_convert::convertTime(in.stamp_); out.header.frame_id = in.frame_id_; out.point.x = in.getX(); out.point.y = in.getY(); @@ -277,7 +277,7 @@ geometry_msgs::PointStamped toMsg(const tf3::Stamped& in, geometry inline void fromMsg(const geometry_msgs::PointStamped& msg, tf3::Stamped& out) { - out.stamp_ = tf3::convertTime(msg.header.stamp); + out.stamp_ = data_convert::convertTime(msg.header.stamp); out.frame_id_ = msg.header.frame_id; out.setData(tf3::Vector3(msg.point.x, msg.point.y, msg.point.z)); } @@ -328,7 +328,7 @@ void fromMsg(const geometry_msgs::Quaternion& in, tf3::Quaternion& out) */ template <> inline -const tf3::Time& getTimestamp(const geometry_msgs::QuaternionStamped& t) {return tf3::convertTime(t.header.stamp);} +const tf3::Time& getTimestamp(const geometry_msgs::QuaternionStamped& t) {return data_convert::convertTime(t.header.stamp);} /** \brief Extract a frame ID from the header of a Quaternion message. * This function is a specialization of the getFrameId template defined in tf3/convert.h. @@ -371,7 +371,7 @@ inline geometry_msgs::QuaternionStamped toMsg(const tf3::Stamped& in) { geometry_msgs::QuaternionStamped out; - out.header.stamp = tf3::convertTime(in.stamp_); + out.header.stamp = data_convert::convertTime(in.stamp_); out.header.frame_id = in.frame_id_; out.quaternion.w = in.getW(); out.quaternion.x = in.getX(); @@ -401,7 +401,7 @@ geometry_msgs::QuaternionStamped toMsg(const tf3::Stamped& in) inline void fromMsg(const geometry_msgs::QuaternionStamped& in, tf3::Stamped& out) { - out.stamp_ = tf3::convertTime(in.header.stamp); + out.stamp_ = data_convert::convertTime(in.header.stamp); out.frame_id_ = in.header.frame_id; tf3::Quaternion tmp; fromMsg(in.quaternion, tmp); @@ -461,7 +461,7 @@ void fromMsg(const geometry_msgs::Pose& in, tf3::Transform& out) */ template <> inline - const tf3::Time& getTimestamp(const geometry_msgs::PoseStamped& t) {return tf3::convertTime(t.header.stamp);} + const tf3::Time& getTimestamp(const geometry_msgs::PoseStamped& t) {return data_convert::convertTime(t.header.stamp);} /** \brief Extract a frame ID from the header of a Pose message. * This function is a specialization of the getFrameId template defined in tf3/convert.h. @@ -502,7 +502,7 @@ void fromMsg(const geometry_msgs::PoseStamped& msg, geometry_msgs::PoseStamped& inline geometry_msgs::PoseStamped toMsg(const tf3::Stamped& in, geometry_msgs::PoseStamped & out) { - out.header.stamp = tf3::convertTime(in.stamp_); + out.header.stamp = data_convert::convertTime(in.stamp_); out.header.frame_id = in.frame_id_; toMsg(in.getOrigin(), out.pose.position); out.pose.orientation = toMsg(in.getRotation()); @@ -517,7 +517,7 @@ geometry_msgs::PoseStamped toMsg(const tf3::Stamped& in, geometr inline void fromMsg(const geometry_msgs::PoseStamped& msg, tf3::Stamped& out) { - out.stamp_ = tf3::convertTime(msg.header.stamp); + out.stamp_ = data_convert::convertTime(msg.header.stamp); out.frame_id_ = msg.header.frame_id; tf3::Transform tmp; fromMsg(msg.pose, tmp); @@ -535,7 +535,7 @@ void fromMsg(const geometry_msgs::PoseStamped& msg, tf3::Stamped */ template <> inline - const tf3::Time& getTimestamp(const geometry_msgs::PoseWithCovarianceStamped& t) {return tf3::convertTime(t.header.stamp);} + const tf3::Time& getTimestamp(const geometry_msgs::PoseWithCovarianceStamped& t) {return data_convert::convertTime(t.header.stamp);} /** \brief Extract a frame ID from the header of a PoseWithCovarianceStamped message. * This function is a specialization of the getFrameId template defined in tf3/convert.h. @@ -576,7 +576,7 @@ void fromMsg(const geometry_msgs::PoseWithCovarianceStamped& msg, geometry_msgs: inline geometry_msgs::PoseWithCovarianceStamped toMsg(const tf3::Stamped& in, geometry_msgs::PoseWithCovarianceStamped & out) { - out.header.stamp = tf3::convertTime(in.stamp_); + out.header.stamp = data_convert::convertTime(in.stamp_); out.header.frame_id = in.frame_id_; toMsg(in.getOrigin(), out.pose.pose.position); out.pose.pose.orientation = toMsg(in.getRotation()); @@ -591,7 +591,7 @@ geometry_msgs::PoseWithCovarianceStamped toMsg(const tf3::Stamped& out) { - out.stamp_ = tf3::convertTime(msg.header.stamp); + out.stamp_ = data_convert::convertTime(msg.header.stamp); out.frame_id_ = msg.header.frame_id; tf3::Transform tmp; fromMsg(msg.pose, tmp); @@ -659,7 +659,7 @@ void fromMsg(const tf3::TransformMsg& in, tf3::Transform& out) */ template <> inline -const tf3::Time& getTimestamp(const geometry_msgs::TransformStamped& t) {return tf3::convertTime(t.header.stamp);} +const tf3::Time& getTimestamp(const geometry_msgs::TransformStamped& t) {return data_convert::convertTime(t.header.stamp);} /** \brief Extract a frame ID from the header of a Transform message. * This function is a specialization of the getFrameId template defined in tf3/convert.h. @@ -700,7 +700,7 @@ inline geometry_msgs::TransformStamped toMsg(const tf3::Stamped& in) { geometry_msgs::TransformStamped out; - out.header.stamp = tf3::convertTime(in.stamp_); + out.header.stamp = data_convert::convertTime(in.stamp_); out.header.frame_id = in.frame_id_; out.transform.translation = toMsg(in.getOrigin()); out.transform.rotation = toMsg(in.getRotation()); @@ -716,7 +716,7 @@ geometry_msgs::TransformStamped toMsg(const tf3::Stamped& in) inline void fromMsg(const geometry_msgs::TransformStamped& msg, tf3::Stamped& out) { - out.stamp_ = tf3::convertTime(msg.header.stamp); + out.stamp_ = data_convert::convertTime(msg.header.stamp); out.frame_id_ = msg.header.frame_id; tf3::Transform tmp; fromMsg(msg.transform, tmp); @@ -734,7 +734,7 @@ inline void doTransform(const geometry_msgs::Point& t_in, geometry_msgs::Point& t_out, const tf3::TransformStampedMsg& transform) { - tf3::Transform t = convertToTransform(transform); + tf3::Transform t = data_convert::convertTotf3Transform(transform); tf3::Vector3 v_in; fromMsg(t_in, v_in); tf3::Vector3 v_out = t * v_in; @@ -752,7 +752,7 @@ inline void doTransform(const geometry_msgs::PointStamped& t_in, geometry_msgs::PointStamped& t_out, const tf3::TransformStampedMsg& transform) { doTransform(t_in.point, t_out.point, transform); - t_out.header.stamp = tf3::convertTime(transform.header.stamp); + t_out.header.stamp = data_convert::convertTime(transform.header.stamp); t_out.header.frame_id = transform.header.frame_id; } @@ -785,7 +785,7 @@ inline void doTransform(const geometry_msgs::QuaternionStamped& t_in, geometry_msgs::QuaternionStamped& t_out, const tf3::TransformStampedMsg& transform) { doTransform(t_in.quaternion, t_out.quaternion, transform); - t_out.header.stamp = tf3::convertTime(transform.header.stamp); + t_out.header.stamp = data_convert::convertTime(transform.header.stamp); t_out.header.frame_id = transform.header.frame_id; } @@ -837,7 +837,7 @@ inline void doTransform(const geometry_msgs::PoseStamped& t_in, geometry_msgs::PoseStamped& t_out, const tf3::TransformStampedMsg& transform) { doTransform(t_in.pose, t_out.pose, transform); - t_out.header.stamp = tf3::convertTime(transform.header.stamp); + t_out.header.stamp = data_convert::convertTime(transform.header.stamp); t_out.header.frame_id = transform.header.frame_id; } @@ -949,7 +949,7 @@ void doTransform(const geometry_msgs::PoseWithCovarianceStamped& t_in, geometry_ fromMsg(transform.transform, t); tf3::Transform v_out = t * tf3::Transform(r, v); toMsg(v_out, t_out.pose.pose); - t_out.header.stamp = tf3::convertTime(transform.header.stamp); + t_out.header.stamp = data_convert::convertTime(transform.header.stamp); t_out.header.frame_id = transform.header.frame_id; t_out.pose.covariance = transformCovariance(t_in.pose.covariance, t); @@ -965,14 +965,14 @@ template <> inline void doTransform(const geometry_msgs::TransformStamped& t_in, geometry_msgs::TransformStamped& t_out, const tf3::TransformStampedMsg& transform) { - tf3::Transform input = convertToTransform(t_in); + tf3::Transform input = data_convert::convertTotf3Transform(t_in); - tf3::Transform t = convertToTransform(transform); + tf3::Transform t = data_convert::convertTotf3Transform(transform); tf3::Transform v_out = t * input; t_out.transform = toMsg(v_out); - t_out.header.stamp = tf3::convertTime(transform.header.stamp); + t_out.header.stamp = data_convert::convertTime(transform.header.stamp); t_out.header.frame_id = transform.header.frame_id; } @@ -986,7 +986,7 @@ template <> inline void doTransform(const geometry_msgs::Vector3& t_in, geometry_msgs::Vector3& t_out, const tf3::TransformStampedMsg& transform) { - tf3::Transform t = convertToTransform(transform); + tf3::Transform t = data_convert::convertTotf3Transform(transform); tf3::Vector3 v_out = t.getBasis() * tf3::Vector3(t_in.x, t_in.y, t_in.z); t_out.x = v_out[0]; t_out.y = v_out[1]; @@ -1004,7 +1004,7 @@ inline void doTransform(const geometry_msgs::Vector3Stamped& t_in, geometry_msgs::Vector3Stamped& t_out, const tf3::TransformStampedMsg& transform) { doTransform(t_in.vector, t_out.vector, transform); - t_out.header.stamp = tf3::convertTime(transform.header.stamp); + t_out.header.stamp = data_convert::convertTime(transform.header.stamp); t_out.header.frame_id = transform.header.frame_id; } @@ -1014,7 +1014,7 @@ inline /**********************/ template <> inline -const tf3::Time& getTimestamp(const geometry_msgs::WrenchStamped& t) {return tf3::convertTime(t.header.stamp);} +const tf3::Time& getTimestamp(const geometry_msgs::WrenchStamped& t) {return data_convert::convertTime(t.header.stamp);} template <> @@ -1038,7 +1038,7 @@ void fromMsg(const geometry_msgs::WrenchStamped& msg, geometry_msgs::WrenchStamp inline geometry_msgs::WrenchStamped toMsg(const tf3::Stamped>& in, geometry_msgs::WrenchStamped & out) { - out.header.stamp = tf3::convertTime(in.stamp_); + out.header.stamp = data_convert::convertTime(in.stamp_); out.header.frame_id = in.frame_id_; out.wrench.force = toMsg(in[0]); out.wrench.torque = toMsg(in[1]); @@ -1049,7 +1049,7 @@ geometry_msgs::WrenchStamped toMsg(const tf3::Stamped>& out) { - out.stamp_ = tf3::convertTime(msg.header.stamp); + out.stamp_ = data_convert::convertTime(msg.header.stamp); out.frame_id_ = msg.header.frame_id; tf3::Vector3 tmp; fromMsg(msg.wrench.force, tmp); @@ -1082,7 +1082,7 @@ inline void doTransform(const geometry_msgs::WrenchStamped& t_in, geometry_msgs::WrenchStamped& t_out, const tf3::TransformStampedMsg& transform) { doTransform(t_in.wrench, t_out.wrench, transform); - t_out.header.stamp = tf3::convertTime(transform.header.stamp); + t_out.header.stamp = data_convert::convertTime(transform.header.stamp); t_out.header.frame_id = transform.header.frame_id; } diff --git a/tf3_geometry_msgs/test/test_tf2_geometry_msgs.cpp b/tf3_geometry_msgs/test/test_tf2_geometry_msgs.cpp index 7bf0911..78be67b 100755 --- a/tf3_geometry_msgs/test/test_tf2_geometry_msgs.cpp +++ b/tf3_geometry_msgs/test/test_tf2_geometry_msgs.cpp @@ -34,7 +34,7 @@ #include #include #include -#include +#include tf3::BufferCore* tf_buffer; tf3::TransformStampedMsg t; @@ -63,7 +63,7 @@ TEST(TfGeometry, Frame) tf3::TransformStampedMsg tfm = tf_buffer->lookupTransform( "B", // frame đích v1.header.frame_id, // frame nguồn - // tf3::convertTime(v1.header.stamp) + // data_convert::convertTime(v1.header.stamp) tf3::Time() ); // std::cout << "STEP 3" << std::endl; @@ -92,7 +92,7 @@ TEST(TfGeometry, Frame) tf3::TransformStampedMsg tfm1 = tf_buffer->lookupTransform( "B", // frame đích "A", // frame nguồn - tf3::convertTime(robot::Time(2.0)) + data_convert::convertTime(robot::Time(2.0)) ); geometry_msgs::PoseStamped v_advanced = v1; // hoặc v1 @@ -142,7 +142,7 @@ TEST(TfGeometry, PoseWithCovarianceStamped) tf3::TransformStampedMsg tfm1 = tf_buffer->lookupTransform( "B", // frame đích "A", // frame nguồn - tf3::convertTime(robot::Time(2.0)) + data_convert::convertTime(robot::Time(2.0)) ); tf3::doTransform(v1, v_simple, tfm1); std::cout << v_simple.pose.pose.position.x << " , " @@ -174,7 +174,7 @@ TEST(TfGeometry, PoseWithCovarianceStamped) tf3::TransformStampedMsg tfm2 = tf_buffer->lookupTransform( "B", // frame đích "A", // frame nguồn - tf3::convertTime(robot::Time(2.0)) + data_convert::convertTime(robot::Time(2.0)) ); tf3::doTransform(v1, v_advanced, tfm2); @@ -215,7 +215,7 @@ TEST(TfGeometry, PoseWithCovarianceStamped) t_rot.header.stamp = robot::Time(2.0); t_rot.header.frame_id = "A"; t_rot.child_frame_id = "rotated"; - tf3::TransformStampedMsg t_rot_msg = tf3::convertToTransformStampedMsg(t_rot); + tf3::TransformStampedMsg t_rot_msg = data_convert::convertToTransformStampedMsg(t_rot); tf_buffer->setTransform(t_rot_msg, "rotation_test"); // need to put some covariance in the matrix @@ -228,7 +228,7 @@ TEST(TfGeometry, PoseWithCovarianceStamped) tf3::TransformStampedMsg tfm3 = tf_buffer->lookupTransform( "rotated", // frame đích "A", // frame nguồn - tf3::convertTime(robot::Time(2.0)) + data_convert::convertTime(robot::Time(2.0)) ); tf3::doTransform(v1, v_rotated, tfm3); @@ -270,7 +270,7 @@ int main(int argc, char **argv){ t.transform.rotation.z = 0; t.transform.rotation.w = 0; - t.header.stamp = tf3::convertTime(robot::Time(2.0)); + t.header.stamp = data_convert::convertTime(robot::Time(2.0)); t.header.frame_id = "A"; t.child_frame_id = "B"; tf_buffer->setTransform(t, "test"); diff --git a/tf3_sensor_msgs/CMakeLists.txt b/tf3_sensor_msgs/CMakeLists.txt index 9f0587c..789afc8 100755 --- a/tf3_sensor_msgs/CMakeLists.txt +++ b/tf3_sensor_msgs/CMakeLists.txt @@ -41,6 +41,7 @@ target_include_directories(tf3_sensor_msgs INTERFACE target_link_libraries(tf3_sensor_msgs INTERFACE sensor_msgs + data_convert ) @@ -58,4 +59,5 @@ target_link_libraries(test_tf2_sensor_msgs tf3_sensor_msgs geometry_msgs tf3 + data_convert ) diff --git a/tf3_sensor_msgs/include/tf3_sensor_msgs/tf3_sensor_msgs.h b/tf3_sensor_msgs/include/tf3_sensor_msgs/tf3_sensor_msgs.h index 43f2b8f..051c8e7 100755 --- a/tf3_sensor_msgs/include/tf3_sensor_msgs/tf3_sensor_msgs.h +++ b/tf3_sensor_msgs/include/tf3_sensor_msgs/tf3_sensor_msgs.h @@ -35,7 +35,7 @@ #include #include #include -#include +#include namespace tf3 { @@ -52,7 +52,7 @@ namespace tf3 */ template <> inline -const tf3::Time& getTimestamp(const sensor_msgs::PointCloud2& p) {return convertTime(p.header.stamp);} +const tf3::Time& getTimestamp(const sensor_msgs::PointCloud2& p) {return data_convert::convertTime(p.header.stamp);} /** \brief Extract a frame ID from the header of a PointCloud2 message. * This function is a specialization of the getFrameId template defined in tf3/convert.h. @@ -71,7 +71,7 @@ void doTransform(const sensor_msgs::PointCloud2 &p_in, sensor_msgs::PointCloud2 { p_out = p_in; p_out.header.seq = p_in.header.seq; - p_out.header.stamp = tf3::convertTime(t_in.header.stamp); + p_out.header.stamp = data_convert::convertTime(t_in.header.stamp); p_out.header.frame_id = t_in.header.frame_id; Eigen::Transform t = Eigen::Translation3f(t_in.transform.translation.x, t_in.transform.translation.y, t_in.transform.translation.z) * Eigen::Quaternion( diff --git a/tf3_sensor_msgs/test/test_tf2_sensor_msgs.cpp b/tf3_sensor_msgs/test/test_tf2_sensor_msgs.cpp index a539c3b..a948ebe 100755 --- a/tf3_sensor_msgs/test/test_tf2_sensor_msgs.cpp +++ b/tf3_sensor_msgs/test/test_tf2_sensor_msgs.cpp @@ -59,7 +59,7 @@ TEST(Tf2Sensor, PointCloud2) tf3::TransformStampedMsg tfm1 = tf_buffer->lookupTransform( "B", // frame đích cloud.header.frame_id, // frame nguồn - tf3::convertTime(robot::Time(2.0)) + data_convert::convertTime(robot::Time(2.0)) ); sensor_msgs::PointCloud2 cloud_simple; // hoặc v1 @@ -76,7 +76,7 @@ TEST(Tf2Sensor, PointCloud2) tf3::TransformStampedMsg tfm2 = tf_buffer->lookupTransform( "B", // frame đích "A", // frame nguồn - tf3::convertTime(robot::Time(2.0)) + data_convert::convertTime(robot::Time(2.0)) ); sensor_msgs::PointCloud2 cloud_advanced; // hoặc v1 @@ -108,7 +108,7 @@ int main(int argc, char **argv){ t.header.stamp = robot::Time(2.0); t.header.frame_id = "A"; t.child_frame_id = "B"; - tf3::TransformStampedMsg t_msg = tf3::convertToTransformStampedMsg(t); + tf3::TransformStampedMsg t_msg = data_convert::convertToTransformStampedMsg(t); tf_buffer->setTransform(t_msg, "test"); int ret = RUN_ALL_TESTS();