diff --git a/tf2_sensor_msgs/AMENT_IGNORE b/tf2_sensor_msgs/AMENT_IGNORE deleted file mode 100644 index e69de29bb..000000000 diff --git a/tf2_sensor_msgs/CMakeLists.txt b/tf2_sensor_msgs/CMakeLists.txt index 46dc8fb68..08ac2f23e 100644 --- a/tf2_sensor_msgs/CMakeLists.txt +++ b/tf2_sensor_msgs/CMakeLists.txt @@ -1,42 +1,49 @@ cmake_minimum_required(VERSION 3.5) project(tf2_sensor_msgs) -find_package(catkin REQUIRED COMPONENTS cmake_modules sensor_msgs tf2_ros tf2) -find_package(Boost COMPONENTS thread REQUIRED) -find_package(Eigen) +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra) +endif() -catkin_package( - INCLUDE_DIRS include - CATKIN_DEPENDS sensor_msgs tf2_ros tf2 - DEPENDS Eigen +find_package(ament_cmake_auto REQUIRED) +set(required_dependencies + "sensor_msgs" + #"python_orocos_kdl" + "tf2" + "tf2_ros" + "Eigen3" ) +ament_auto_find_build_dependencies(REQUIRED ${required_dependencies}) include_directories(include - ${catkin_INCLUDE_DIRS} -) - -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + ${sensor_msgs_INCLUDE_DIRS} + ${tf2_INCLUDE_DIRS} + ${tf2_ros_INCLUDE_DIRS} + ${Eigen3_INCLUDE_DIRS} ) -catkin_python_setup() - -if(CATKIN_ENABLE_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() +# 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_auto_package() diff --git a/tf2_sensor_msgs/include/tf2_sensor_msgs/tf2_sensor_msgs.h b/tf2_sensor_msgs/include/tf2_sensor_msgs/tf2_sensor_msgs.h index 7fdb6a4f1..bd401019b 100644 --- a/tf2_sensor_msgs/include/tf2_sensor_msgs/tf2_sensor_msgs.h +++ b/tf2_sensor_msgs/include/tf2_sensor_msgs/tf2_sensor_msgs.h @@ -31,8 +31,9 @@ #define TF2_SENSOR_MSGS_H #include -#include -#include +#include +#include +#include #include #include @@ -46,17 +47,17 @@ namespace tf2 // method to extract timestamp from object template <> inline -tf2::TimePoint getTimestamp(const sensor_msgs::PointCloud2& p) {return p.header.stamp;} +tf2::TimePoint getTimestamp(const sensor_msgs::msg::PointCloud2& p) {return tf2_ros::fromMsg(p.header.stamp);} // method to extract frame id from object template <> inline -std::string getFrameId(const sensor_msgs::PointCloud2 &p) {return p.header.frame_id;} +std::string getFrameId(const sensor_msgs::msg::PointCloud2 &p) {return p.header.frame_id;} // 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::msg::PointCloud2 &p_in, sensor_msgs::msg::PointCloud2 &p_out, const geometry_msgs::msg::TransformStamped& t_in) { p_out = p_in; p_out.header = t_in.header; @@ -65,13 +66,13 @@ void doTransform(const sensor_msgs::PointCloud2 &p_in, sensor_msgs::PointCloud2 t_in.transform.rotation.w, t_in.transform.rotation.x, t_in.transform.rotation.y, t_in.transform.rotation.z); - sensor_msgs::PointCloud2ConstIterator x_in(p_in, "x"); - sensor_msgs::PointCloud2ConstIterator y_in(p_in, "y"); - sensor_msgs::PointCloud2ConstIterator z_in(p_in, "z"); + sensor_msgs::PointCloud2ConstIterator x_in(p_in, std::string("x")); + sensor_msgs::PointCloud2ConstIterator y_in(p_in, std::string("y")); + sensor_msgs::PointCloud2ConstIterator z_in(p_in, std::string("z")); - sensor_msgs::PointCloud2Iterator x_out(p_out, "x"); - sensor_msgs::PointCloud2Iterator y_out(p_out, "y"); - sensor_msgs::PointCloud2Iterator z_out(p_out, "z"); + sensor_msgs::PointCloud2Iterator x_out(p_out, std::string("x")); + sensor_msgs::PointCloud2Iterator y_out(p_out, std::string("y")); + sensor_msgs::PointCloud2Iterator z_out(p_out, std::string("z")); Eigen::Vector3f point; for(; x_in != x_in.end(); ++x_in, ++y_in, ++z_in, ++x_out, ++y_out, ++z_out) { @@ -82,12 +83,12 @@ void doTransform(const sensor_msgs::PointCloud2 &p_in, sensor_msgs::PointCloud2 } } inline -sensor_msgs::PointCloud2 toMsg(const sensor_msgs::PointCloud2 &in) +sensor_msgs::msg::PointCloud2 toMsg(const sensor_msgs::msg::PointCloud2 &in) { return in; } inline -void fromMsg(const sensor_msgs::PointCloud2 &msg, sensor_msgs::PointCloud2 &out) +void fromMsg(const sensor_msgs::msg::PointCloud2 &msg, sensor_msgs::msg::PointCloud2 &out) { out = msg; } diff --git a/tf2_sensor_msgs/package.xml b/tf2_sensor_msgs/package.xml index dda4d1575..f02e95241 100644 --- a/tf2_sensor_msgs/package.xml +++ b/tf2_sensor_msgs/package.xml @@ -1,4 +1,4 @@ - + tf2_sensor_msgs 0.9.1 @@ -9,23 +9,20 @@ BSD http://www.ros.org/wiki/tf2_ros - - catkin - cmake_modules - eigen - sensor_msgs - tf2 - tf2_ros + ament_cmake_auto - cmake_modules - eigen - sensor_msgs - tf2_ros - tf2 - python_orocos_kdl + cmake_modules + Eigen3 + sensor_msgs + tf2 + tf2_ros + + - rostest + ament_cmake_gtest + + ament_cmake + - diff --git a/tf2_sensor_msgs/test/test_tf2_sensor_msgs.cpp b/tf2_sensor_msgs/test/test_tf2_sensor_msgs.cpp index 5ec3cfc24..f9361d182 100644 --- a/tf2_sensor_msgs/test/test_tf2_sensor_msgs.cpp +++ b/tf2_sensor_msgs/test/test_tf2_sensor_msgs.cpp @@ -41,14 +41,14 @@ static const double EPS = 1e-3; TEST(Tf2Sensor, PointCloud2) { - sensor_msgs::PointCloud2 cloud; - sensor_msgs::PointCloud2Modifier modifier(cloud); + sensor_msgs::msg::PointCloud2 cloud; + sensor_msgs::msg::PointCloud2Modifier modifier(cloud); modifier.setPointCloud2FieldsByString(2, "xyz", "rgb"); modifier.resize(1); - sensor_msgs::PointCloud2Iterator iter_x(cloud, "x"); - sensor_msgs::PointCloud2Iterator iter_y(cloud, "y"); - sensor_msgs::PointCloud2Iterator iter_z(cloud, "z"); + sensor_msgs::msg::PointCloud2Iterator iter_x(cloud, "x"); + sensor_msgs::msg::PointCloud2Iterator iter_y(cloud, "y"); + sensor_msgs::msg::PointCloud2Iterator iter_z(cloud, "z"); *iter_x = 1; *iter_y = 2; @@ -58,20 +58,20 @@ TEST(Tf2Sensor, PointCloud2) cloud.header.frame_id = "A"; // simple api - sensor_msgs::PointCloud2 cloud_simple = tf_buffer->transform(cloud, "B", tf2::durationFromSec(2.0)); - sensor_msgs::PointCloud2Iterator iter_x_after(cloud_simple, "x"); - sensor_msgs::PointCloud2Iterator iter_y_after(cloud_simple, "y"); - sensor_msgs::PointCloud2Iterator iter_z_after(cloud_simple, "z"); + sensor_msgs::msg::PointCloud2 cloud_simple = tf_buffer->transform(cloud, "B", tf2::durationFromSec(2.0)); + sensor_msgs::msg::PointCloud2Iterator iter_x_after(cloud_simple, "x"); + sensor_msgs::msg::PointCloud2Iterator iter_y_after(cloud_simple, "y"); + sensor_msgs::msg::PointCloud2Iterator 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::PointCloud2 cloud_advanced = tf_buffer->transform(cloud, "B", builtin_interfaces::msg::Time(2.0), + sensor_msgs::msg::PointCloud2 cloud_advanced = tf_buffer->transform(cloud, "B", builtin_interfaces::msg::Time(2.0), "A", tf2::durationFromSec(3.0)); - sensor_msgs::PointCloud2Iterator iter_x_advanced(cloud_advanced, "x"); - sensor_msgs::PointCloud2Iterator iter_y_advanced(cloud_advanced, "y"); - sensor_msgs::PointCloud2Iterator iter_z_advanced(cloud_advanced, "z"); + sensor_msgs::msg::PointCloud2Iterator iter_x_advanced(cloud_advanced, "x"); + sensor_msgs::msg::PointCloud2Iterator iter_y_advanced(cloud_advanced, "y"); + sensor_msgs::msg::PointCloud2Iterator 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); @@ -82,10 +82,11 @@ int main(int argc, char **argv){ ros::init(argc, argv, "test"); ros::NodeHandle n; - tf_buffer = new tf2_ros::Buffer(); + rclcpp::Clock::SharedPtr clock = std::make_shared(RCL_SYSTEM_TIME); + tf_buffer = new tf2_ros::Buffer(clock); // populate buffer - geometry_msgs::TransformStamped t; + geometry_msgs::msg::TransformStamped t; t.transform.translation.x = 10; t.transform.translation.y = 20; t.transform.translation.z = 30;