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

remove file data_convert

Browse Source
This commit is contained in:
duongtd 2025-11-26 16:25:06 +07:00
parent d3588f0bb5
commit 12000dabcc
7 changed files with 52 additions and 166 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
tf3_geometry_msgs/CMakeLists.txt
View File

@ -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/ # 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 target_link_libraries(tf3_geometry_msgs INTERFACE
geometry_msgs geometry_msgs
data_convert
) )
# # Test: tomsg_frommsg # # Test: tomsg_frommsg
@ -52,6 +53,7 @@ target_link_libraries(tf3_geometry_msgs INTERFACE
tf3_geometry_msgs tf3_geometry_msgs
Threads::Threads Threads::Threads
tf3 tf3
data_convert
) )
# # Test: tf2_geometry_msgs # # Test: tf2_geometry_msgs
add_executable(test_geometry_msgs test/test_tf2_geometry_msgs.cpp) add_executable(test_geometry_msgs test/test_tf2_geometry_msgs.cpp)
@ -60,4 +62,5 @@ target_link_libraries(tf3_geometry_msgs INTERFACE
Threads::Threads Threads::Threads
tf3 tf3
tf3_geometry_msgs tf3_geometry_msgs
data_convert
) )

119
tf3_geometry_msgs/include/tf3_geometry_msgs/data_convert.h
View File

@ -1,119 +0,0 @@
#ifndef DATA_CONVERT_TF3_GEOMETRY_MSGS_H
#define DATA_CONVERT_TF3_GEOMETRY_MSGS_H
#include <tf3/time.h>
#include <tf3/compat.h>
#include <tf3/LinearMath/Transform.h>
#include <geometry_msgs/TransformStamped.h>
#include <robot/time.h>
#include <cmath>
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

66
tf3_geometry_msgs/include/tf3_geometry_msgs/tf3_geometry_msgs.h
View File

@ -35,7 +35,7 @@
#include <tf3/convert.h> #include <tf3/convert.h>
#include <tf3/LinearMath/Quaternion.h> #include <tf3/LinearMath/Quaternion.h>
#include <tf3/LinearMath/Transform.h> #include <tf3/LinearMath/Transform.h>
#include <tf3_geometry_msgs/data_convert.h> #include <data_convert/data_convert.h>
#include <geometry_msgs/PointStamped.h> #include <geometry_msgs/PointStamped.h>
#include <geometry_msgs/QuaternionStamped.h> #include <geometry_msgs/QuaternionStamped.h>
#include <geometry_msgs/TransformStamped.h> #include <geometry_msgs/TransformStamped.h>
@ -111,7 +111,7 @@ void fromMsg(const geometry_msgs::Vector3& in, tf3::Vector3& out)
*/ */
template <> template <>
inline 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. /** \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. * 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<tf3::Vector3>& in) geometry_msgs::Vector3Stamped toMsg(const tf3::Stamped<tf3::Vector3>& in)
{ {
geometry_msgs::Vector3Stamped out; 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.header.frame_id = in.frame_id_;
out.vector.x = in.getX(); out.vector.x = in.getX();
out.vector.y = in.getY(); out.vector.y = in.getY();
@ -171,7 +171,7 @@ geometry_msgs::Vector3Stamped toMsg(const tf3::Stamped<tf3::Vector3>& in)
inline inline
void fromMsg(const geometry_msgs::Vector3Stamped& msg, tf3::Stamped<tf3::Vector3>& out) void fromMsg(const geometry_msgs::Vector3Stamped& msg, tf3::Stamped<tf3::Vector3>& out)
{ {
out.stamp_ = tf3::convertTime(msg.header.stamp); out.stamp_ = data_convert::convertTime(msg.header.stamp);
out.frame_id_ = msg.header.frame_id; out.frame_id_ = msg.header.frame_id;
out.setData(tf3::Vector3(msg.vector.x, msg.vector.y, msg.vector.z)); 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 <> template <>
inline 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. /** \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. * 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 inline
geometry_msgs::PointStamped toMsg(const tf3::Stamped<tf3::Vector3>& in, geometry_msgs::PointStamped & out) geometry_msgs::PointStamped toMsg(const tf3::Stamped<tf3::Vector3>& 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.header.frame_id = in.frame_id_;
out.point.x = in.getX(); out.point.x = in.getX();
out.point.y = in.getY(); out.point.y = in.getY();
@ -277,7 +277,7 @@ geometry_msgs::PointStamped toMsg(const tf3::Stamped<tf3::Vector3>& in, geometry
inline inline
void fromMsg(const geometry_msgs::PointStamped& msg, tf3::Stamped<tf3::Vector3>& out) void fromMsg(const geometry_msgs::PointStamped& msg, tf3::Stamped<tf3::Vector3>& out)
{ {
out.stamp_ = tf3::convertTime(msg.header.stamp); out.stamp_ = data_convert::convertTime(msg.header.stamp);
out.frame_id_ = msg.header.frame_id; out.frame_id_ = msg.header.frame_id;
out.setData(tf3::Vector3(msg.point.x, msg.point.y, msg.point.z)); 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 <> template <>
inline 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. /** \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. * 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<tf3::Quaternion>& in) geometry_msgs::QuaternionStamped toMsg(const tf3::Stamped<tf3::Quaternion>& in)
{ {
geometry_msgs::QuaternionStamped out; 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.header.frame_id = in.frame_id_;
out.quaternion.w = in.getW(); out.quaternion.w = in.getW();
out.quaternion.x = in.getX(); out.quaternion.x = in.getX();
@ -401,7 +401,7 @@ geometry_msgs::QuaternionStamped toMsg(const tf3::Stamped<tf3::Quaternion>& in)
inline inline
void fromMsg(const geometry_msgs::QuaternionStamped& in, tf3::Stamped<tf3::Quaternion>& out) void fromMsg(const geometry_msgs::QuaternionStamped& in, tf3::Stamped<tf3::Quaternion>& out)
{ {
out.stamp_ = tf3::convertTime(in.header.stamp); out.stamp_ = data_convert::convertTime(in.header.stamp);
out.frame_id_ = in.header.frame_id; out.frame_id_ = in.header.frame_id;
tf3::Quaternion tmp; tf3::Quaternion tmp;
fromMsg(in.quaternion, tmp); fromMsg(in.quaternion, tmp);
@ -461,7 +461,7 @@ void fromMsg(const geometry_msgs::Pose& in, tf3::Transform& out)
*/ */
template <> template <>
inline 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. /** \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. * 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 inline
geometry_msgs::PoseStamped toMsg(const tf3::Stamped<tf3::Transform>& in, geometry_msgs::PoseStamped & out) geometry_msgs::PoseStamped toMsg(const tf3::Stamped<tf3::Transform>& 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_; out.header.frame_id = in.frame_id_;
toMsg(in.getOrigin(), out.pose.position); toMsg(in.getOrigin(), out.pose.position);
out.pose.orientation = toMsg(in.getRotation()); out.pose.orientation = toMsg(in.getRotation());
@ -517,7 +517,7 @@ geometry_msgs::PoseStamped toMsg(const tf3::Stamped<tf3::Transform>& in, geometr
inline inline
void fromMsg(const geometry_msgs::PoseStamped& msg, tf3::Stamped<tf3::Transform>& out) void fromMsg(const geometry_msgs::PoseStamped& msg, tf3::Stamped<tf3::Transform>& out)
{ {
out.stamp_ = tf3::convertTime(msg.header.stamp); out.stamp_ = data_convert::convertTime(msg.header.stamp);
out.frame_id_ = msg.header.frame_id; out.frame_id_ = msg.header.frame_id;
tf3::Transform tmp; tf3::Transform tmp;
fromMsg(msg.pose, tmp); fromMsg(msg.pose, tmp);
@ -535,7 +535,7 @@ void fromMsg(const geometry_msgs::PoseStamped& msg, tf3::Stamped<tf3::Transform>
*/ */
template <> template <>
inline 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. /** \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. * 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 inline
geometry_msgs::PoseWithCovarianceStamped toMsg(const tf3::Stamped<tf3::Transform>& in, geometry_msgs::PoseWithCovarianceStamped & out) geometry_msgs::PoseWithCovarianceStamped toMsg(const tf3::Stamped<tf3::Transform>& 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_; out.header.frame_id = in.frame_id_;
toMsg(in.getOrigin(), out.pose.pose.position); toMsg(in.getOrigin(), out.pose.pose.position);
out.pose.pose.orientation = toMsg(in.getRotation()); out.pose.pose.orientation = toMsg(in.getRotation());
@ -591,7 +591,7 @@ geometry_msgs::PoseWithCovarianceStamped toMsg(const tf3::Stamped<tf3::Transform
inline inline
void fromMsg(const geometry_msgs::PoseWithCovarianceStamped& msg, tf3::Stamped<tf3::Transform>& out) void fromMsg(const geometry_msgs::PoseWithCovarianceStamped& msg, tf3::Stamped<tf3::Transform>& out)
{ {
out.stamp_ = tf3::convertTime(msg.header.stamp); out.stamp_ = data_convert::convertTime(msg.header.stamp);
out.frame_id_ = msg.header.frame_id; out.frame_id_ = msg.header.frame_id;
tf3::Transform tmp; tf3::Transform tmp;
fromMsg(msg.pose, tmp); fromMsg(msg.pose, tmp);
@ -659,7 +659,7 @@ void fromMsg(const tf3::TransformMsg& in, tf3::Transform& out)
*/ */
template <> template <>
inline 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. /** \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. * 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<tf3::Transform>& in) geometry_msgs::TransformStamped toMsg(const tf3::Stamped<tf3::Transform>& in)
{ {
geometry_msgs::TransformStamped out; 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.header.frame_id = in.frame_id_;
out.transform.translation = toMsg(in.getOrigin()); out.transform.translation = toMsg(in.getOrigin());
out.transform.rotation = toMsg(in.getRotation()); out.transform.rotation = toMsg(in.getRotation());
@ -716,7 +716,7 @@ geometry_msgs::TransformStamped toMsg(const tf3::Stamped<tf3::Transform>& in)
inline inline
void fromMsg(const geometry_msgs::TransformStamped& msg, tf3::Stamped<tf3::Transform>& out) void fromMsg(const geometry_msgs::TransformStamped& msg, tf3::Stamped<tf3::Transform>& out)
{ {
out.stamp_ = tf3::convertTime(msg.header.stamp); out.stamp_ = data_convert::convertTime(msg.header.stamp);
out.frame_id_ = msg.header.frame_id; out.frame_id_ = msg.header.frame_id;
tf3::Transform tmp; tf3::Transform tmp;
fromMsg(msg.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) 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; tf3::Vector3 v_in;
fromMsg(t_in, v_in); fromMsg(t_in, v_in);
tf3::Vector3 v_out = t * 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) 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); 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; 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) 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); 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; 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) 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); 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; 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); fromMsg(transform.transform, t);
tf3::Transform v_out = t * tf3::Transform(r, v); tf3::Transform v_out = t * tf3::Transform(r, v);
toMsg(v_out, t_out.pose.pose); 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.header.frame_id = transform.header.frame_id;
t_out.pose.covariance = transformCovariance(t_in.pose.covariance, t); t_out.pose.covariance = transformCovariance(t_in.pose.covariance, t);
@ -965,14 +965,14 @@ template <>
inline inline
void doTransform(const geometry_msgs::TransformStamped& t_in, geometry_msgs::TransformStamped& t_out, const tf3::TransformStampedMsg& transform) 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; tf3::Transform v_out = t * input;
t_out.transform = toMsg(v_out); 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; t_out.header.frame_id = transform.header.frame_id;
} }
@ -986,7 +986,7 @@ template <>
inline inline
void doTransform(const geometry_msgs::Vector3& t_in, geometry_msgs::Vector3& t_out, const tf3::TransformStampedMsg& transform) 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); 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.x = v_out[0];
t_out.y = v_out[1]; 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) 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); 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; t_out.header.frame_id = transform.header.frame_id;
} }
@ -1014,7 +1014,7 @@ inline
/**********************/ /**********************/
template <> template <>
inline 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 <> template <>
@ -1038,7 +1038,7 @@ void fromMsg(const geometry_msgs::WrenchStamped& msg, geometry_msgs::WrenchStamp
inline inline
geometry_msgs::WrenchStamped toMsg(const tf3::Stamped<std::array<tf3::Vector3, 2>>& in, geometry_msgs::WrenchStamped & out) geometry_msgs::WrenchStamped toMsg(const tf3::Stamped<std::array<tf3::Vector3, 2>>& 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.header.frame_id = in.frame_id_;
out.wrench.force = toMsg(in[0]); out.wrench.force = toMsg(in[0]);
out.wrench.torque = toMsg(in[1]); out.wrench.torque = toMsg(in[1]);
@ -1049,7 +1049,7 @@ geometry_msgs::WrenchStamped toMsg(const tf3::Stamped<std::array<tf3::Vector3, 2
inline inline
void fromMsg(const geometry_msgs::WrenchStamped& msg, tf3::Stamped<std::array<tf3::Vector3, 2>>& out) void fromMsg(const geometry_msgs::WrenchStamped& msg, tf3::Stamped<std::array<tf3::Vector3, 2>>& out)
{ {
out.stamp_ = tf3::convertTime(msg.header.stamp); out.stamp_ = data_convert::convertTime(msg.header.stamp);
out.frame_id_ = msg.header.frame_id; out.frame_id_ = msg.header.frame_id;
tf3::Vector3 tmp; tf3::Vector3 tmp;
fromMsg(msg.wrench.force, 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) 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); 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; t_out.header.frame_id = transform.header.frame_id;
} }

16
tf3_geometry_msgs/test/test_tf2_geometry_msgs.cpp
View File

@ -34,7 +34,7 @@
#include <gtest/gtest.h> #include <gtest/gtest.h>
#include <tf3/buffer_core.h> #include <tf3/buffer_core.h>
#include <tf3_geometry_msgs/tf3_geometry_msgs.h> #include <tf3_geometry_msgs/tf3_geometry_msgs.h>
#include <tf3_geometry_msgs/data_convert.h> #include <data_convert/data_convert.h>
tf3::BufferCore* tf_buffer; tf3::BufferCore* tf_buffer;
tf3::TransformStampedMsg t; tf3::TransformStampedMsg t;
@ -63,7 +63,7 @@ TEST(TfGeometry, Frame)
tf3::TransformStampedMsg tfm = tf_buffer->lookupTransform( tf3::TransformStampedMsg tfm = tf_buffer->lookupTransform(
"B", // frame đích "B", // frame đích
v1.header.frame_id, // frame nguồn v1.header.frame_id, // frame nguồn
// tf3::convertTime(v1.header.stamp) // data_convert::convertTime(v1.header.stamp)
tf3::Time() tf3::Time()
); );
// std::cout << "STEP 3" << std::endl; // std::cout << "STEP 3" << std::endl;
@ -92,7 +92,7 @@ TEST(TfGeometry, Frame)
tf3::TransformStampedMsg tfm1 = tf_buffer->lookupTransform( tf3::TransformStampedMsg tfm1 = tf_buffer->lookupTransform(
"B", // frame đích "B", // frame đích
"A", // frame nguồn "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 geometry_msgs::PoseStamped v_advanced = v1; // hoặc v1
@ -142,7 +142,7 @@ TEST(TfGeometry, PoseWithCovarianceStamped)
tf3::TransformStampedMsg tfm1 = tf_buffer->lookupTransform( tf3::TransformStampedMsg tfm1 = tf_buffer->lookupTransform(
"B", // frame đích "B", // frame đích
"A", // frame nguồn "A", // frame nguồn
tf3::convertTime(robot::Time(2.0)) data_convert::convertTime(robot::Time(2.0))
); );
tf3::doTransform(v1, v_simple, tfm1); tf3::doTransform(v1, v_simple, tfm1);
std::cout << v_simple.pose.pose.position.x << " , " std::cout << v_simple.pose.pose.position.x << " , "
@ -174,7 +174,7 @@ TEST(TfGeometry, PoseWithCovarianceStamped)
tf3::TransformStampedMsg tfm2 = tf_buffer->lookupTransform( tf3::TransformStampedMsg tfm2 = tf_buffer->lookupTransform(
"B", // frame đích "B", // frame đích
"A", // frame nguồn "A", // frame nguồn
tf3::convertTime(robot::Time(2.0)) data_convert::convertTime(robot::Time(2.0))
); );
tf3::doTransform(v1, v_advanced, tfm2); tf3::doTransform(v1, v_advanced, tfm2);
@ -215,7 +215,7 @@ TEST(TfGeometry, PoseWithCovarianceStamped)
t_rot.header.stamp = robot::Time(2.0); t_rot.header.stamp = robot::Time(2.0);
t_rot.header.frame_id = "A"; t_rot.header.frame_id = "A";
t_rot.child_frame_id = "rotated"; 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"); tf_buffer->setTransform(t_rot_msg, "rotation_test");
// need to put some covariance in the matrix // need to put some covariance in the matrix
@ -228,7 +228,7 @@ TEST(TfGeometry, PoseWithCovarianceStamped)
tf3::TransformStampedMsg tfm3 = tf_buffer->lookupTransform( tf3::TransformStampedMsg tfm3 = tf_buffer->lookupTransform(
"rotated", // frame đích "rotated", // frame đích
"A", // frame nguồn "A", // frame nguồn
tf3::convertTime(robot::Time(2.0)) data_convert::convertTime(robot::Time(2.0))
); );
tf3::doTransform(v1, v_rotated, tfm3); tf3::doTransform(v1, v_rotated, tfm3);
@ -270,7 +270,7 @@ int main(int argc, char **argv){
t.transform.rotation.z = 0; t.transform.rotation.z = 0;
t.transform.rotation.w = 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.header.frame_id = "A";
t.child_frame_id = "B"; t.child_frame_id = "B";
tf_buffer->setTransform(t, "test"); tf_buffer->setTransform(t, "test");

2
tf3_sensor_msgs/CMakeLists.txt
View File

@ -41,6 +41,7 @@ target_include_directories(tf3_sensor_msgs INTERFACE
target_link_libraries(tf3_sensor_msgs INTERFACE target_link_libraries(tf3_sensor_msgs INTERFACE
sensor_msgs sensor_msgs
data_convert
) )
@ -58,4 +59,5 @@ target_link_libraries(test_tf2_sensor_msgs
tf3_sensor_msgs tf3_sensor_msgs
geometry_msgs geometry_msgs
tf3 tf3
data_convert
) )

6
tf3_sensor_msgs/include/tf3_sensor_msgs/tf3_sensor_msgs.h
View File

@ -35,7 +35,7 @@
#include <sensor_msgs/point_cloud2_iterator.h> #include <sensor_msgs/point_cloud2_iterator.h>
#include <Eigen/Eigen> #include <Eigen/Eigen>
#include <Eigen/Geometry> #include <Eigen/Geometry>
#include <tf3_sensor_msgs/data_convert.h> #include <data_convert/data_convert.h>
namespace tf3 namespace tf3
{ {
@ -52,7 +52,7 @@ namespace tf3
*/ */
template <> template <>
inline 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. /** \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. * 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 = p_in;
p_out.header.seq = p_in.header.seq; 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; p_out.header.frame_id = t_in.header.frame_id;
Eigen::Transform<float,3,Eigen::Isometry> t = Eigen::Translation3f(t_in.transform.translation.x, t_in.transform.translation.y, Eigen::Transform<float,3,Eigen::Isometry> t = Eigen::Translation3f(t_in.transform.translation.x, t_in.transform.translation.y,
t_in.transform.translation.z) * Eigen::Quaternion<float>( t_in.transform.translation.z) * Eigen::Quaternion<float>(

6
tf3_sensor_msgs/test/test_tf2_sensor_msgs.cpp
View File

@ -59,7 +59,7 @@ TEST(Tf2Sensor, PointCloud2)
tf3::TransformStampedMsg tfm1 = tf_buffer->lookupTransform( tf3::TransformStampedMsg tfm1 = tf_buffer->lookupTransform(
"B", // frame đích "B", // frame đích
cloud.header.frame_id, // frame nguồn 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 sensor_msgs::PointCloud2 cloud_simple; // hoặc v1
@ -76,7 +76,7 @@ TEST(Tf2Sensor, PointCloud2)
tf3::TransformStampedMsg tfm2 = tf_buffer->lookupTransform( tf3::TransformStampedMsg tfm2 = tf_buffer->lookupTransform(
"B", // frame đích "B", // frame đích
"A", // frame nguồn "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 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.stamp = robot::Time(2.0);
t.header.frame_id = "A"; t.header.frame_id = "A";
t.child_frame_id = "B"; 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"); tf_buffer->setTransform(t_msg, "test");
int ret = RUN_ALL_TESTS(); int ret = RUN_ALL_TESTS();