2727 * POSSIBILITY OF SUCH DAMAGE.
2828 */
2929
30- #include < memory>
30+ #include < gtest/gtest.h>
31+
32+ #include < tf2_ros/buffer.h>
33+ #include < tf2_ros/transform_listener.h>
3134
3235#include < builtin_interfaces/msg/time.hpp>
3336#include < geometry_msgs/msg/pose_stamped.hpp>
34- #include < gtest/gtest.h>
3537#include < rclcpp/rclcpp.hpp>
3638#include < sensor_msgs/point_cloud2_iterator.hpp>
37- #include < tf2_ros/buffer.h>
38- #include < tf2_ros/transform_listener.h>
3939#include < tf2_sensor_msgs/tf2_sensor_msgs.hpp>
4040
41+ #include < memory>
42+
4143std::unique_ptr<tf2_ros::Buffer> tf_buffer = nullptr ;
4244static const double EPS = 1e-3 ;
4345
@@ -57,14 +59,12 @@ TEST(Tf2Sensor, PointCloud2)
5759 *iter_y = 2 ;
5860 *iter_z = 3 ;
5961
60- cloud.header .stamp .sec = 2 ;
61- cloud.header .stamp .nanosec = 0 ;
62+ cloud.header .stamp = rclcpp::Time (2 , 0 );
6263 cloud.header .frame_id = " A" ;
6364
6465 // simple api
6566 sensor_msgs::msg::PointCloud2 cloud_simple = tf_buffer->transform (
66- cloud, " B" , tf2::durationFromSec (
67- 2.0 ));
67+ cloud, " B" , tf2::durationFromSec (2.0 ));
6868 sensor_msgs::PointCloud2Iterator<float > iter_x_after (cloud_simple, " x" );
6969 sensor_msgs::PointCloud2Iterator<float > iter_y_after (cloud_simple, " y" );
7070 sensor_msgs::PointCloud2Iterator<float > iter_z_after (cloud_simple, " z" );
@@ -74,8 +74,7 @@ TEST(Tf2Sensor, PointCloud2)
7474
7575 // advanced api
7676 sensor_msgs::msg::PointCloud2 cloud_advanced = tf_buffer->transform (
77- cloud, " B" , tf2::timeFromSec (2.0 ),
78- " A" , tf2::durationFromSec (3.0 ));
77+ cloud, " B" , tf2::timeFromSec (2.0 ), " A" , tf2::durationFromSec (3.0 ));
7978 sensor_msgs::PointCloud2Iterator<float > iter_x_advanced (cloud_advanced, " x" );
8079 sensor_msgs::PointCloud2Iterator<float > iter_y_advanced (cloud_advanced, " y" );
8180 sensor_msgs::PointCloud2Iterator<float > iter_z_advanced (cloud_advanced, " z" );
@@ -100,8 +99,7 @@ int main(int argc, char ** argv)
10099 t.transform .rotation .y = 0 ;
101100 t.transform .rotation .z = 0 ;
102101 t.transform .rotation .w = 0 ;
103- t.header .stamp .sec = 2 ;
104- t.header .stamp .nanosec = 0 ;
102+ t.header .stamp = rclcpp::Time (2 , 0 );
105103 t.header .frame_id = " A" ;
106104 t.child_frame_id = " B" ;
107105 tf_buffer->setTransform (t, " test" );
0 commit comments