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

update package tf3_sensor_msgs

Browse Source
This commit is contained in:
duongtd
2025-11-24 11:17:47 +07:00
parent 0dca929962
commit 88613a94f0
311 changed files with 22225 additions and 214 deletions
Download Patch File Download Diff File Expand all files Collapse all files

119
tf3_sensor_msgs/include/tf3_sensor_msgs/data_convert.h Normal file
View File

@@ -0,0 +1,119 @@
#ifndef DATA_CONVERT_H
#define DATA_CONVERT_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
{
robot::Time convertTime(const tf3::Time& time)
{
robot::Time time_tmp;
time_tmp.sec = time.sec;
time_tmp.nsec = time.nsec;
return time_tmp;
}
tf3::Time convertTime(const robot::Time& time)
{
tf3::Time time_tmp;
time_tmp.sec = time.sec;
time_tmp.nsec = time.nsec;
return time_tmp;
}
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)
{
return geometry_msgs::Quaternion(q.x(),q.y(),q.z(),q.w());
}
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;
}
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;
}
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;
}
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_H

24
tf3_sensor_msgs/include/tf2_sensor_msgs/tf2_sensor_msgs.h → tf3_sensor_msgs/include/tf3_sensor_msgs/tf3_sensor_msgs.h
View File

@@ -27,16 +27,17 @@
* POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef TF2_SENSOR_MSGS_H
#define TF2_SENSOR_MSGS_H
#ifndef TF3_SENSOR_MSGS_H
#define TF3_SENSOR_MSGS_H
#include <tf2/convert.h>
#include <tf3/convert.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/point_cloud2_iterator.h>
#include <Eigen/Eigen>
#include <Eigen/Geometry>
#include <tf3_sensor_msgs/data_convert.h>
namespace tf2
namespace tf3
{
/********************/
@@ -44,17 +45,17 @@ namespace tf2
/********************/
/** \brief Extract a timestamp from the header of a PointCloud2 message.
* This function is a specialization of the getTimestamp template defined in tf2/convert.h.
* This function is a specialization of the getTimestamp template defined in tf3/convert.h.
* \param t PointCloud2 message to extract the timestamp from.
* \return The timestamp of the message. The lifetime of the returned reference
* is bound to the lifetime of the argument.
*/
template <>
inline
const ros::Time& getTimestamp(const sensor_msgs::PointCloud2& p) {return p.header.stamp;}
const tf3::Time& getTimestamp(const sensor_msgs::PointCloud2& p) {return 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 tf2/convert.h.
* This function is a specialization of the getFrameId template defined in tf3/convert.h.
* \param t PointCloud2 message to extract the frame ID from.
* \return A string containing the frame ID of the message. The lifetime of the
* returned reference is bound to the lifetime of the argument.
@@ -66,10 +67,12 @@ const std::string& getFrameId(const sensor_msgs::PointCloud2 &p) {return p.heade
// this method needs to be implemented by client library developers
template <>
inline
void doTransform(const sensor_msgs::PointCloud2 &p_in, sensor_msgs::PointCloud2 &p_out, const geometry_msgs::TransformStamped& t_in)
void doTransform(const sensor_msgs::PointCloud2 &p_in, sensor_msgs::PointCloud2 &p_out, const tf3::TransformStampedMsg& t_in)
{
p_out = p_in;
p_out.header = t_in.header;
p_out.header.seq = p_in.header.seq;
p_out.header.stamp = tf3::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>(
t_in.transform.rotation.w, t_in.transform.rotation.x,
@@ -91,6 +94,7 @@ void doTransform(const sensor_msgs::PointCloud2 &p_in, sensor_msgs::PointCloud2
*z_out = point.z();
}
}
inline
sensor_msgs::PointCloud2 toMsg(const sensor_msgs::PointCloud2 &in)
{
@@ -104,4 +108,4 @@ void fromMsg(const sensor_msgs::PointCloud2 &msg, sensor_msgs::PointCloud2 &out)
} // namespace
#endif // TF2_SENSOR_MSGS_H
#endif // TF3_SENSOR_MSGS_H