2626// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
2727// POSSIBILITY OF SUCH DAMAGE.
2828
29- #ifndef TF2_SENSOR_MSGS_HPP
30- #define TF2_SENSOR_MSGS_HPP
29+ #ifndef TF2_SENSOR_MSGS__TF2_SENSOR_MSGS_HPP_
30+ #define TF2_SENSOR_MSGS__TF2_SENSOR_MSGS_HPP_
3131
3232#include < tf2/convert.h>
3333#include < tf2/time.h>
3737#include < Eigen/Geometry>
3838#include < tf2_ros/buffer_interface.h>
3939
40+ #include < string>
41+
4042namespace tf2
4143{
4244
@@ -45,19 +47,24 @@ namespace tf2
4547/* *******************/
4648
4749// method to extract timestamp from object
48- template <>
50+ template <>
4951inline
50- tf2::TimePoint getTimestamp (const sensor_msgs::msg::PointCloud2& p) {return tf2_ros::fromMsg (p.header .stamp );}
52+ tf2::TimePoint getTimestamp (const sensor_msgs::msg::PointCloud2 & p)
53+ {
54+ return tf2_ros::fromMsg (p.header .stamp );
55+ }
5156
5257// method to extract frame id from object
53- template <>
58+ template <>
5459inline
55- std::string getFrameId (const sensor_msgs::msg::PointCloud2 &p) {return p.header .frame_id ;}
60+ std::string getFrameId (const sensor_msgs::msg::PointCloud2 & p) {return p.header .frame_id ;}
5661
5762// this method needs to be implemented by client library developers
58- template <>
63+ template <>
5964inline
60- void doTransform (const sensor_msgs::msg::PointCloud2 &p_in, sensor_msgs::msg::PointCloud2 &p_out, const geometry_msgs::msg::TransformStamped& t_in)
65+ void doTransform (
66+ const sensor_msgs::msg::PointCloud2 & p_in, sensor_msgs::msg::PointCloud2 & p_out,
67+ const geometry_msgs::msg::TransformStamped & t_in)
6168{
6269 p_out = p_in;
6370 p_out.header = t_in.header ;
@@ -75,7 +82,7 @@ void doTransform(const sensor_msgs::msg::PointCloud2 &p_in, sensor_msgs::msg::Po
7582 static_cast <float >(t_in.transform .rotation .y ),
7683 static_cast <float >(t_in.transform .rotation .z ));
7784
78- Eigen::Transform<float ,3 , Eigen::Affine> t = translation * quaternion;
85+ Eigen::Transform<float , 3 , Eigen::Affine> t = translation * quaternion;
7986
8087 sensor_msgs::PointCloud2ConstIterator<float > x_in (p_in, std::string (" x" ));
8188 sensor_msgs::PointCloud2ConstIterator<float > y_in (p_in, std::string (" y" ));
@@ -86,24 +93,24 @@ void doTransform(const sensor_msgs::msg::PointCloud2 &p_in, sensor_msgs::msg::Po
8693 sensor_msgs::PointCloud2Iterator<float > z_out (p_out, std::string (" z" ));
8794
8895 Eigen::Vector3f point;
89- for (; x_in != x_in.end (); ++x_in, ++y_in, ++z_in, ++x_out, ++y_out, ++z_out) {
96+ for (; x_in != x_in.end (); ++x_in, ++y_in, ++z_in, ++x_out, ++y_out, ++z_out) {
9097 point = t * Eigen::Vector3f (*x_in, *y_in, *z_in);
9198 *x_out = point.x ();
9299 *y_out = point.y ();
93100 *z_out = point.z ();
94101 }
95102}
96103inline
97- sensor_msgs::msg::PointCloud2 toMsg (const sensor_msgs::msg::PointCloud2 &in)
104+ sensor_msgs::msg::PointCloud2 toMsg (const sensor_msgs::msg::PointCloud2 & in)
98105{
99106 return in;
100107}
101108inline
102- void fromMsg (const sensor_msgs::msg::PointCloud2 &msg, sensor_msgs::msg::PointCloud2 &out)
109+ void fromMsg (const sensor_msgs::msg::PointCloud2 & msg, sensor_msgs::msg::PointCloud2 & out)
103110{
104111 out = msg;
105112}
106113
107- } // namespace
114+ } // namespace tf2
108115
109- #endif // TF2_SENSOR_MSGS_HPP
116+ #endif // TF2_SENSOR_MSGS__TF2_SENSOR_MSGS_HPP_
0 commit comments