Reenable sensor_msgs cpp test

Author: gleichdick

tf2_sensor_msgs/CMakeLists.txt

@@ -24,24 +24,26 @@ find_package(Eigen3 REQUIRED)
 
 ament_export_dependencies(eigen3_cmake_module)
 ament_export_dependencies(Eigen3)
-
-# TODO enable tests
-#if(BUILD_TESTING)
-#  catkin_add_nosetests(test/test_tf2_sensor_msgs.py)
-
-#find_package(catkin REQUIRED COMPONENTS
-#  sensor_msgs
-#  rostest
-#  tf2_ros
-#  tf2
-#)
-#include_directories(${EIGEN_INCLUDE_DIRS})
-#add_executable(test_tf2_sensor_msgs_cpp EXCLUDE_FROM_ALL test/test_tf2_sensor_msgs.cpp)
-#target_link_libraries(test_tf2_sensor_msgs_cpp ${catkin_LIBRARIES} ${GTEST_LIBRARIES})
-#if(TARGET tests)
-#  add_dependencies(tests test_tf2_sensor_msgs_cpp)
-#endif()
-#add_rostest(${CMAKE_CURRENT_SOURCE_DIR}/test/test.launch)
-#endif()
+ament_export_dependencies(tf2)
+
+if(BUILD_TESTING)
+  find_package(ament_cmake_gtest REQUIRED)
+  find_package(ament_cmake_pytest REQUIRED)
+
+  # TODO(ros2/orocos_kinematics_dynamics): reenable when PyKDL is ready to use
+  #ament_add_pytest_test(test_tf2_sensor_msgs_py test/test_tf2_sensor_msgs.py)
+
+  ament_add_gtest(test_tf2_sensor_msgs_cpp test/test_tf2_sensor_msgs.cpp)
+  if(TARGET test_tf2_sensor_msgs_cpp)
+    target_include_directories(test_tf2_sensor_msgs_cpp PUBLIC include)
+    ament_target_dependencies(test_tf2_sensor_msgs_cpp
+      "Eigen3"
+      "rclcpp"
+      "sensor_msgs"
+      "tf2"
+      "tf2_ros"
+    )
+  endif()
+endif()
 
 ament_auto_package()

tf2_sensor_msgs/test/test_tf2_sensor_msgs.cpp

@@ -28,12 +28,13 @@
  */
 
 
-#include <tf2_sensor_msgs/tf2_sensor_msgs.hpp>
-#include <geometry_msgs/PoseStamped.h>
-#include <tf2_ros/transform_listener.h>
-#include <ros/ros.h>
+#include <builtin_interfaces/msg/time.hpp>
+#include <geometry_msgs/msg/pose_stamped.hpp>
 #include <gtest/gtest.h>
+#include <sensor_msgs/point_cloud2_iterator.hpp>
 #include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_listener.h>
+#include <tf2_sensor_msgs/tf2_sensor_msgs.hpp>
 
 std::unique_ptr<tf2_ros::Buffer> tf_buffer = nullptr;
 static const double EPS = 1e-3;
@@ -42,45 +43,48 @@ static const double EPS = 1e-3;
 TEST(Tf2Sensor, PointCloud2)
 {
   sensor_msgs::msg::PointCloud2 cloud;
-  sensor_msgs::msg::PointCloud2Modifier modifier(cloud);
+  sensor_msgs::PointCloud2Modifier modifier(cloud);
   modifier.setPointCloud2FieldsByString(2, "xyz", "rgb");
   modifier.resize(1);
 
-  sensor_msgs::msg::PointCloud2Iterator<float> iter_x(cloud, "x");
-  sensor_msgs::msg::PointCloud2Iterator<float> iter_y(cloud, "y");
-  sensor_msgs::msg::PointCloud2Iterator<float> iter_z(cloud, "z");
+  sensor_msgs::PointCloud2Iterator<float> iter_x(cloud, "x");
+  sensor_msgs::PointCloud2Iterator<float> iter_y(cloud, "y");
+  sensor_msgs::PointCloud2Iterator<float> iter_z(cloud, "z");
 
   *iter_x = 1;
   *iter_y = 2;
   *iter_z = 3;
 
-  cloud.header.stamp = builtin_interfaces::msg::Time(2);
+  cloud.header.stamp.sec = 2;
+  cloud.header.stamp.nanosec = 0;
   cloud.header.frame_id = "A";
 
   // simple api
-  sensor_msgs::msg::PointCloud2 cloud_simple = tf_buffer->transform(cloud, "B", tf2::durationFromSec(2.0));
-  sensor_msgs::msg::PointCloud2Iterator<float> iter_x_after(cloud_simple, "x");
-  sensor_msgs::msg::PointCloud2Iterator<float> iter_y_after(cloud_simple, "y");
-  sensor_msgs::msg::PointCloud2Iterator<float> iter_z_after(cloud_simple, "z");
+  sensor_msgs::msg::PointCloud2 cloud_simple = tf_buffer->transform(
+    cloud, "B", tf2::durationFromSec(
+      2.0));
+  sensor_msgs::PointCloud2Iterator<float> iter_x_after(cloud_simple, "x");
+  sensor_msgs::PointCloud2Iterator<float> iter_y_after(cloud_simple, "y");
+  sensor_msgs::PointCloud2Iterator<float> iter_z_after(cloud_simple, "z");
   EXPECT_NEAR(*iter_x_after, -9, EPS);
   EXPECT_NEAR(*iter_y_after, 18, EPS);
   EXPECT_NEAR(*iter_z_after, 27, EPS);
 
   // advanced api
-  sensor_msgs::msg::PointCloud2 cloud_advanced = tf_buffer->transform(cloud, "B", builtin_interfaces::msg::Time(2.0),
-                                                                 "A", tf2::durationFromSec(3.0));
-  sensor_msgs::msg::PointCloud2Iterator<float> iter_x_advanced(cloud_advanced, "x");
-  sensor_msgs::msg::PointCloud2Iterator<float> iter_y_advanced(cloud_advanced, "y");
-  sensor_msgs::msg::PointCloud2Iterator<float> iter_z_advanced(cloud_advanced, "z");
+  sensor_msgs::msg::PointCloud2 cloud_advanced = tf_buffer->transform(
+    cloud, "B", tf2::timeFromSec(2.0),
+    "A", tf2::durationFromSec(3.0));
+  sensor_msgs::PointCloud2Iterator<float> iter_x_advanced(cloud_advanced, "x");
+  sensor_msgs::PointCloud2Iterator<float> iter_y_advanced(cloud_advanced, "y");
+  sensor_msgs::PointCloud2Iterator<float> iter_z_advanced(cloud_advanced, "z");
   EXPECT_NEAR(*iter_x_advanced, -9, EPS);
   EXPECT_NEAR(*iter_y_advanced, 18, EPS);
   EXPECT_NEAR(*iter_z_advanced, 27, EPS);
 }
 
-int main(int argc, char **argv){
+int main(int argc, char ** argv)
+{
   testing::InitGoogleTest(&argc, argv);
-  ros::init(argc, argv, "test");
-  ros::NodeHandle n;
 
   rclcpp::Clock::SharedPtr clock = std::make_shared<rclcpp::Clock>(RCL_SYSTEM_TIME);
   tf_buffer = std::make_unique<tf2_ros::Buffer>(clock);
@@ -94,7 +98,8 @@ int main(int argc, char **argv){
   t.transform.rotation.y = 0;
   t.transform.rotation.z = 0;
   t.transform.rotation.w = 0;
-  t.header.stamp = builtin_interfaces::msg::Time(2.0);
+  t.header.stamp.sec = 2;
+  t.header.stamp.nanosec = 0;
   t.header.frame_id = "A";
   t.child_frame_id = "B";
   tf_buffer->setTransform(t, "test");