remove file data_convert
This commit is contained in:
parent
d3588f0bb5
commit
12000dabcc
|
|
@ -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
|
||||
)
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
@ -35,7 +35,7 @@
|
|||
#include <tf3/convert.h>
|
||||
#include <tf3/LinearMath/Quaternion.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/QuaternionStamped.h>
|
||||
#include <geometry_msgs/TransformStamped.h>
|
||||
|
|
@ -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<tf3::Vector3>& 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<tf3::Vector3>& in)
|
|||
inline
|
||||
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.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<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.point.x = in.getX();
|
||||
out.point.y = in.getY();
|
||||
|
|
@ -277,7 +277,7 @@ geometry_msgs::PointStamped toMsg(const tf3::Stamped<tf3::Vector3>& in, geometry
|
|||
inline
|
||||
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.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<tf3::Quaternion>& 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<tf3::Quaternion>& in)
|
|||
inline
|
||||
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;
|
||||
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<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_;
|
||||
toMsg(in.getOrigin(), out.pose.position);
|
||||
out.pose.orientation = toMsg(in.getRotation());
|
||||
|
|
@ -517,7 +517,7 @@ geometry_msgs::PoseStamped toMsg(const tf3::Stamped<tf3::Transform>& in, geometr
|
|||
inline
|
||||
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;
|
||||
tf3::Transform tmp;
|
||||
fromMsg(msg.pose, tmp);
|
||||
|
|
@ -535,7 +535,7 @@ void fromMsg(const geometry_msgs::PoseStamped& msg, tf3::Stamped<tf3::Transform>
|
|||
*/
|
||||
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<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_;
|
||||
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<tf3::Transform
|
|||
inline
|
||||
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;
|
||||
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<tf3::Transform>& 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<tf3::Transform>& in)
|
|||
inline
|
||||
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;
|
||||
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<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.wrench.force = toMsg(in[0]);
|
||||
out.wrench.torque = toMsg(in[1]);
|
||||
|
|
@ -1049,7 +1049,7 @@ geometry_msgs::WrenchStamped toMsg(const tf3::Stamped<std::array<tf3::Vector3, 2
|
|||
inline
|
||||
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;
|
||||
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;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -34,7 +34,7 @@
|
|||
#include <gtest/gtest.h>
|
||||
#include <tf3/buffer_core.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::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");
|
||||
|
|
|
|||
|
|
@ -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
|
||||
)
|
||||
|
|
|
|||
|
|
@ -35,7 +35,7 @@
|
|||
#include <sensor_msgs/point_cloud2_iterator.h>
|
||||
#include <Eigen/Eigen>
|
||||
#include <Eigen/Geometry>
|
||||
#include <tf3_sensor_msgs/data_convert.h>
|
||||
#include <data_convert/data_convert.h>
|
||||
|
||||
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<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>(
|
||||
|
|
|
|||
|
|
@ -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();
|
||||
|
|
|
|||
Loading…
Reference in New Issue
Block a user