30 #ifndef TF2_SENSOR_MSGS_H 31 #define TF2_SENSOR_MSGS_H 34 #include <sensor_msgs/PointCloud2.h> 36 #include <Eigen/Eigen> 37 #include <Eigen/Geometry> 64 const std::string&
getFrameId(
const sensor_msgs::PointCloud2 &p) {
return p.header.frame_id;}
69 void doTransform(
const sensor_msgs::PointCloud2 &p_in, sensor_msgs::PointCloud2 &p_out,
const geometry_msgs::TransformStamped& t_in)
72 p_out.header = t_in.header;
73 Eigen::Transform<float,3,Eigen::Affine> t = Eigen::Translation3f(t_in.transform.translation.x, t_in.transform.translation.y,
74 t_in.transform.translation.z) * Eigen::Quaternion<float>(
75 t_in.transform.rotation.w, t_in.transform.rotation.x,
76 t_in.transform.rotation.y, t_in.transform.rotation.z);
86 Eigen::Vector3f point;
87 for(; x_in != x_in.end(); ++x_in, ++y_in, ++z_in, ++x_out, ++y_out, ++z_out) {
88 point = t * Eigen::Vector3f(*x_in, *y_in, *z_in);
95 sensor_msgs::PointCloud2
toMsg(
const sensor_msgs::PointCloud2 &in)
100 void fromMsg(
const sensor_msgs::PointCloud2 &msg, sensor_msgs::PointCloud2 &out)
107 #endif // TF2_SENSOR_MSGS_H const std::string & getFrameId(const T &t)
void doTransform(const T &data_in, T &data_out, const geometry_msgs::TransformStamped &transform)
void fromMsg(const A &, B &b)
const ros::Time & getTimestamp(const T &t)