diff --git a/tf2/include/tf2/convert.h b/tf2/include/tf2/convert.h index 73cc52f28..0b1f51e22 100644 --- a/tf2/include/tf2/convert.h +++ b/tf2/include/tf2/convert.h @@ -34,9 +34,6 @@ #include -// TODO(tfoote) -#error This has not been converted to ROS2 and will not compile - #include #include #include @@ -112,6 +109,9 @@ template * \param a an object to convert from * \param b the object to convert to */ + + //TODO(dhood): re-instate if/when IsMessage message traits available in ROS 2 + /* template void convert(const A& a, B& b) { @@ -126,6 +126,7 @@ template if(&a1 != &a2) a2 = a1; } + */ } diff --git a/tf2/include/tf2/impl/utils.h b/tf2/include/tf2/impl/utils.h index 70ac45f89..84c63bf0c 100644 --- a/tf2/include/tf2/impl/utils.h +++ b/tf2/include/tf2/impl/utils.h @@ -32,22 +32,22 @@ tf2::Quaternion toQuaternion(const tf2::Quaternion& q) { } /** Function needed for the generalization of toQuaternion - * \param q a geometry_msgs::Quaternion + * \param q a geometry_msgs::msg::Quaternion * \return a copy of the same quaternion as a tf2::Quaternion */ inline -tf2::Quaternion toQuaternion(const geometry_msgs::Quaternion& q) { +tf2::Quaternion toQuaternion(const geometry_msgs::msg::Quaternion& q) { tf2::Quaternion res; fromMsg(q, res); return res; } /** Function needed for the generalization of toQuaternion - * \param q a geometry_msgs::QuaternionStamped + * \param q a geometry_msgs::msg::QuaternionStamped * \return a copy of the same quaternion as a tf2::Quaternion */ inline -tf2::Quaternion toQuaternion(const geometry_msgs::QuaternionStamped& q) { +tf2::Quaternion toQuaternion(const geometry_msgs::msg::QuaternionStamped& q) { tf2::Quaternion res; fromMsg(q.quaternion, res); return res; @@ -59,18 +59,18 @@ tf2::Quaternion toQuaternion(const geometry_msgs::QuaternionStamped& q) { */ template tf2::Quaternion toQuaternion(const tf2::Stamped& t) { - geometry_msgs::QuaternionStamped q = toMsg, geometry_msgs::QuaternionStamped>(t); + geometry_msgs::msg::QuaternionStamped q = toMsg, geometry_msgs::msg::QuaternionStamped>(t); return toQuaternion(q); } /** Generic version of toQuaternion. It tries to convert the argument - * to a geometry_msgs::Quaternion + * to a geometry_msgs::msg::Quaternion * \param t some object * \return a copy of the same quaternion as a tf2::Quaternion */ template tf2::Quaternion toQuaternion(const T& t) { - geometry_msgs::Quaternion q = toMsg(t); + geometry_msgs::msg::Quaternion q = toMsg(t); return toQuaternion(q); } diff --git a/tf2_geometry_msgs/AMENT_IGNORE b/tf2_geometry_msgs/AMENT_IGNORE deleted file mode 100644 index e69de29bb..000000000 diff --git a/tf2_geometry_msgs/CMakeLists.txt b/tf2_geometry_msgs/CMakeLists.txt index 0483278ad..2ae229475 100644 --- a/tf2_geometry_msgs/CMakeLists.txt +++ b/tf2_geometry_msgs/CMakeLists.txt @@ -1,22 +1,21 @@ cmake_minimum_required(VERSION 3.5) project(tf2_geometry_msgs) -find_package(orocos_kdl) -find_package(catkin REQUIRED COMPONENTS geometry_msgs tf2_ros tf2) -find_package(Boost COMPONENTS thread REQUIRED) - -# Issue #53 -find_library(KDL_LIBRARY REQUIRED NAMES orocos-kdl HINTS ${orocos_kdl_LIBRARY_DIRS}) - -catkin_package( - LIBRARIES ${KDL_LIBRARY} - INCLUDE_DIRS include - DEPENDS orocos_kdl - CATKIN_DEPENDS geometry_msgs tf2_ros tf2) +if(NOT WIN32) + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14 -Wall -Wextra") +endif() +find_package(orocos_kdl) +find_package(ament_cmake REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_ros REQUIRED) include_directories(include - ${catkin_INCLUDE_DIRS} + ${orocos_kdl_INCLUDE_DIRS} + ${tf2_INCLUDE_DIRS} + ${tf2_ros_INCLUDE_DIRS} + ${geometry_msgs_INCLUDE_DIRS} ) link_directories(${orocos_kdl_LIBRARY_DIRS}) @@ -24,24 +23,28 @@ link_directories(${orocos_kdl_LIBRARY_DIRS}) install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + DESTINATION include/${PROJECT_NAME} ) -catkin_python_setup() - -if(CATKIN_ENABLE_TESTING) - -find_package(catkin REQUIRED COMPONENTS geometry_msgs rostest tf2_ros tf2) - -add_executable(test_geometry_msgs EXCLUDE_FROM_ALL test/test_tf2_geometry_msgs.cpp) -target_link_libraries(test_geometry_msgs ${catkin_LIBRARIES} ${GTEST_LIBRARIES} ${orocos_kdl_LIBRARIES}) -add_rostest(${CMAKE_CURRENT_SOURCE_DIR}/test/test.launch) -add_rostest(${CMAKE_CURRENT_SOURCE_DIR}/test/test_python.launch) - - -if(TARGET tests) - add_dependencies(tests test_geometry_msgs) -endif() - - -endif() +# TODO(dhood): enable python support once ported to ROS 2 +# catkin_python_setup() + +# TODO(dhood): enable tests once ported to ROS 2 +# if(CATKIN_ENABLE_TESTING) +# +# find_package(catkin REQUIRED COMPONENTS geometry_msgs rostest tf2_ros tf2) +# +# add_executable(test_geometry_msgs EXCLUDE_FROM_ALL test/test_tf2_geometry_msgs.cpp) +# target_link_libraries(test_geometry_msgs ${catkin_LIBRARIES} ${GTEST_LIBRARIES} ${orocos_kdl_LIBRARIES}) +# add_rostest(${CMAKE_CURRENT_SOURCE_DIR}/test/test.launch) +# add_rostest(${CMAKE_CURRENT_SOURCE_DIR}/test/test_python.launch) +# +# +# if(TARGET tests) +# add_dependencies(tests test_geometry_msgs) +# endif() +# +# +# endif() +ament_export_include_directories(include) +ament_package() diff --git a/tf2_geometry_msgs/include/tf2_geometry_msgs/tf2_geometry_msgs.h b/tf2_geometry_msgs/include/tf2_geometry_msgs/tf2_geometry_msgs.h index f3ec1a737..27f1e29e4 100644 --- a/tf2_geometry_msgs/include/tf2_geometry_msgs/tf2_geometry_msgs.h +++ b/tf2_geometry_msgs/include/tf2_geometry_msgs/tf2_geometry_msgs.h @@ -35,21 +35,21 @@ #include #include #include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include #include namespace tf2 { inline -KDL::Frame gmTransformToKDL(const geometry_msgs::TransformStamped& t) +KDL::Frame gmTransformToKDL(const geometry_msgs::msg::TransformStamped& t) { - return KDL::Frame(KDL::Rotation::Quaternion(t.transform.rotation.x, t.transform.rotation.y, + return KDL::Frame(KDL::Rotation::Quaternion(t.transform.rotation.x, t.transform.rotation.y, t.transform.rotation.z, t.transform.rotation.w), KDL::Vector(t.transform.translation.x, t.transform.translation.y, t.transform.translation.z)); } @@ -62,17 +62,17 @@ KDL::Frame gmTransformToKDL(const geometry_msgs::TransformStamped& t) // method to extract timestamp from object template <> inline - const builtin_interfaces::msg::Time& getTimestamp(const geometry_msgs::Vector3Stamped& t) {return t.header.stamp;} + const tf2::TimePoint& getTimestamp(const geometry_msgs::msg::Vector3Stamped& t) {return tf2_ros::fromMsg(t.header.stamp);} // method to extract frame id from object template <> inline - const std::string& getFrameId(const geometry_msgs::Vector3Stamped& t) {return t.header.frame_id;} + const std::string& getFrameId(const geometry_msgs::msg::Vector3Stamped& t) {return t.header.frame_id;} // this method needs to be implemented by client library developers template <> inline - void doTransform(const geometry_msgs::Vector3Stamped& t_in, geometry_msgs::Vector3Stamped& t_out, const geometry_msgs::TransformStamped& transform) + void doTransform(const geometry_msgs::msg::Vector3Stamped& t_in, geometry_msgs::msg::Vector3Stamped& t_out, const geometry_msgs::msg::TransformStamped& transform) { KDL::Vector v_out = gmTransformToKDL(transform).M * KDL::Vector(t_in.vector.x, t_in.vector.y, t_in.vector.z); t_out.vector.x = v_out[0]; @@ -82,12 +82,12 @@ inline t_out.header.frame_id = transform.header.frame_id; } inline -geometry_msgs::Vector3Stamped toMsg(const geometry_msgs::Vector3Stamped& in) +geometry_msgs::msg::Vector3Stamped toMsg(const geometry_msgs::msg::Vector3Stamped& in) { return in; } inline -void fromMsg(const geometry_msgs::Vector3Stamped& msg, geometry_msgs::Vector3Stamped& out) +void fromMsg(const geometry_msgs::msg::Vector3Stamped& msg, geometry_msgs::msg::Vector3Stamped& out) { out = msg; } @@ -101,17 +101,17 @@ void fromMsg(const geometry_msgs::Vector3Stamped& msg, geometry_msgs::Vector3Sta // method to extract timestamp from object template <> inline - const builtin_interfaces::msg::Time& getTimestamp(const geometry_msgs::PointStamped& t) {return t.header.stamp;} + const tf2::TimePoint& getTimestamp(const geometry_msgs::msg::PointStamped& t) {return tf2_ros::fromMsg(t.header.stamp);} // method to extract frame id from object template <> inline - const std::string& getFrameId(const geometry_msgs::PointStamped& t) {return t.header.frame_id;} + const std::string& getFrameId(const geometry_msgs::msg::PointStamped& t) {return t.header.frame_id;} // this method needs to be implemented by client library developers template <> inline - void doTransform(const geometry_msgs::PointStamped& t_in, geometry_msgs::PointStamped& t_out, const geometry_msgs::TransformStamped& transform) + void doTransform(const geometry_msgs::msg::PointStamped& t_in, geometry_msgs::msg::PointStamped& t_out, const geometry_msgs::msg::TransformStamped& transform) { KDL::Vector v_out = gmTransformToKDL(transform) * KDL::Vector(t_in.point.x, t_in.point.y, t_in.point.z); t_out.point.x = v_out[0]; @@ -121,12 +121,12 @@ inline t_out.header.frame_id = transform.header.frame_id; } inline -geometry_msgs::PointStamped toMsg(const geometry_msgs::PointStamped& in) +geometry_msgs::msg::PointStamped toMsg(const geometry_msgs::msg::PointStamped& in) { return in; } inline -void fromMsg(const geometry_msgs::PointStamped& msg, geometry_msgs::PointStamped& out) +void fromMsg(const geometry_msgs::msg::PointStamped& msg, geometry_msgs::msg::PointStamped& out) { out = msg; } @@ -139,17 +139,17 @@ void fromMsg(const geometry_msgs::PointStamped& msg, geometry_msgs::PointStamped // method to extract timestamp from object template <> inline - const builtin_interfaces::msg::Time& getTimestamp(const geometry_msgs::PoseStamped& t) {return t.header.stamp;} + const tf2::TimePoint& getTimestamp(const geometry_msgs::msg::PoseStamped& t) {return tf2_ros::fromMsg(t.header.stamp);} // method to extract frame id from object template <> inline - const std::string& getFrameId(const geometry_msgs::PoseStamped& t) {return t.header.frame_id;} + const std::string& getFrameId(const geometry_msgs::msg::PoseStamped& t) {return t.header.frame_id;} // this method needs to be implemented by client library developers template <> inline - void doTransform(const geometry_msgs::PoseStamped& t_in, geometry_msgs::PoseStamped& t_out, const geometry_msgs::TransformStamped& transform) + void doTransform(const geometry_msgs::msg::PoseStamped& t_in, geometry_msgs::msg::PoseStamped& t_out, const geometry_msgs::msg::TransformStamped& transform) { KDL::Vector v(t_in.pose.position.x, t_in.pose.position.y, t_in.pose.position.z); KDL::Rotation r = KDL::Rotation::Quaternion(t_in.pose.orientation.x, t_in.pose.orientation.y, t_in.pose.orientation.z, t_in.pose.orientation.w); @@ -163,12 +163,12 @@ inline t_out.header.frame_id = transform.header.frame_id; } inline -geometry_msgs::PoseStamped toMsg(const geometry_msgs::PoseStamped& in) +geometry_msgs::msg::PoseStamped toMsg(const geometry_msgs::msg::PoseStamped& in) { return in; } inline -void fromMsg(const geometry_msgs::PoseStamped& msg, geometry_msgs::PoseStamped& out) +void fromMsg(const geometry_msgs::msg::PoseStamped& msg, geometry_msgs::msg::PoseStamped& out) { out = msg; } @@ -179,9 +179,9 @@ void fromMsg(const geometry_msgs::PoseStamped& msg, geometry_msgs::PoseStamped& /****************/ inline -geometry_msgs::Quaternion toMsg(const tf2::Quaternion& in) +geometry_msgs::msg::Quaternion toMsg(const tf2::Quaternion& in) { - geometry_msgs::Quaternion out; + geometry_msgs::msg::Quaternion out; out.w = in.getW(); out.x = in.getX(); out.y = in.getY(); @@ -190,7 +190,7 @@ geometry_msgs::Quaternion toMsg(const tf2::Quaternion& in) } inline -void fromMsg(const geometry_msgs::Quaternion& in, tf2::Quaternion& out) +void fromMsg(const geometry_msgs::msg::Quaternion& in, tf2::Quaternion& out) { // w at the end in the constructor out = tf2::Quaternion(in.x, in.y, in.z, in.w); @@ -204,17 +204,17 @@ void fromMsg(const geometry_msgs::Quaternion& in, tf2::Quaternion& out) // method to extract timestamp from object template <> inline -const builtin_interfaces::msg::Time& getTimestamp(const geometry_msgs::QuaternionStamped& t) {return t.header.stamp;} +const tf2::TimePoint& getTimestamp(const geometry_msgs::msg::QuaternionStamped& t) {return tf2_ros::fromMsg(t.header.stamp);} // method to extract frame id from object template <> inline -const std::string& getFrameId(const geometry_msgs::QuaternionStamped& t) {return t.header.frame_id;} +const std::string& getFrameId(const geometry_msgs::msg::QuaternionStamped& t) {return t.header.frame_id;} // this method needs to be implemented by client library developers template <> inline -void doTransform(const geometry_msgs::QuaternionStamped& t_in, geometry_msgs::QuaternionStamped& t_out, const geometry_msgs::TransformStamped& transform) +void doTransform(const geometry_msgs::msg::QuaternionStamped& t_in, geometry_msgs::msg::QuaternionStamped& t_out, const geometry_msgs::msg::TransformStamped& transform) { tf2::Quaternion q_out = tf2::Quaternion(transform.transform.rotation.x, transform.transform.rotation.y, transform.transform.rotation.z, transform.transform.rotation.w)* @@ -224,22 +224,22 @@ void doTransform(const geometry_msgs::QuaternionStamped& t_in, geometry_msgs::Qu t_out.header.frame_id = transform.header.frame_id; } inline -geometry_msgs::QuaternionStamped toMsg(const geometry_msgs::QuaternionStamped& in) +geometry_msgs::msg::QuaternionStamped toMsg(const geometry_msgs::msg::QuaternionStamped& in) { return in; } inline -void fromMsg(const geometry_msgs::QuaternionStamped& msg, geometry_msgs::QuaternionStamped& out) +void fromMsg(const geometry_msgs::msg::QuaternionStamped& msg, geometry_msgs::msg::QuaternionStamped& out) { out = msg; } template <> inline -geometry_msgs::QuaternionStamped toMsg(const tf2::Stamped& in) +geometry_msgs::msg::QuaternionStamped toMsg(const tf2::Stamped& in) { - geometry_msgs::QuaternionStamped out; - out.header.stamp = in.stamp_; + geometry_msgs::msg::QuaternionStamped out; + out.header.stamp = tf2_ros::toMsg(in.stamp_); out.header.frame_id = in.frame_id_; out.quaternion.w = in.getW(); out.quaternion.x = in.getX(); @@ -250,9 +250,9 @@ geometry_msgs::QuaternionStamped toMsg(const tf2::Stamped& in) template <> inline -void fromMsg(const geometry_msgs::QuaternionStamped& in, tf2::Stamped& out) +void fromMsg(const geometry_msgs::msg::QuaternionStamped& in, tf2::Stamped& out) { - out.stamp_ = in.header.stamp; + out.stamp_ = tf2_ros::fromMsg(in.header.stamp); out.frame_id_ = in.header.frame_id; tf2::Quaternion tmp; fromMsg(in.quaternion, tmp); @@ -267,17 +267,17 @@ void fromMsg(const geometry_msgs::QuaternionStamped& in, tf2::Stamped inline -const builtin_interfaces::msg::Time& getTimestamp(const geometry_msgs::TransformStamped& t) {return t.header.stamp;} +const tf2::TimePoint& getTimestamp(const geometry_msgs::msg::TransformStamped& t) {return tf2_ros::fromMsg(t.header.stamp);} // method to extract frame id from object template <> inline -const std::string& getFrameId(const geometry_msgs::TransformStamped& t) {return t.header.frame_id;} +const std::string& getFrameId(const geometry_msgs::msg::TransformStamped& t) {return t.header.frame_id;} // this method needs to be implemented by client library developers template <> inline -void doTransform(const geometry_msgs::TransformStamped& t_in, geometry_msgs::TransformStamped& t_out, const geometry_msgs::TransformStamped& transform) +void doTransform(const geometry_msgs::msg::TransformStamped& t_in, geometry_msgs::msg::TransformStamped& t_out, const geometry_msgs::msg::TransformStamped& transform) { KDL::Vector v(t_in.transform.translation.x, t_in.transform.translation.y, t_in.transform.translation.z); @@ -294,12 +294,12 @@ void doTransform(const geometry_msgs::TransformStamped& t_in, geometry_msgs::Tra t_out.header.frame_id = transform.header.frame_id; } inline -geometry_msgs::TransformStamped toMsg(const geometry_msgs::TransformStamped& in) +geometry_msgs::msg::TransformStamped toMsg(const geometry_msgs::msg::TransformStamped& in) { return in; } inline -void fromMsg(const geometry_msgs::TransformStamped& msg, geometry_msgs::TransformStamped& out) +void fromMsg(const geometry_msgs::msg::TransformStamped& msg, geometry_msgs::msg::TransformStamped& out) { out = msg; } @@ -310,9 +310,9 @@ void fromMsg(const geometry_msgs::TransformStamped& msg, geometry_msgs::Transfor /***************/ inline -geometry_msgs::Transform toMsg(const tf2::Transform& in) +geometry_msgs::msg::Transform toMsg(const tf2::Transform& in) { - geometry_msgs::Transform out; + geometry_msgs::msg::Transform out; out.translation.x = in.getOrigin().getX(); out.translation.y = in.getOrigin().getY(); out.translation.z = in.getOrigin().getZ(); @@ -321,7 +321,7 @@ geometry_msgs::Transform toMsg(const tf2::Transform& in) } inline -void fromMsg(const geometry_msgs::Transform& in, tf2::Transform& out) +void fromMsg(const geometry_msgs::msg::Transform& in, tf2::Transform& out) { out.setOrigin(tf2::Vector3(in.translation.x, in.translation.y, in.translation.z)); // w at the end in the constructor @@ -335,7 +335,7 @@ void fromMsg(const geometry_msgs::Transform& in, tf2::Transform& out) inline /** This section is about converting */ -void toMsg(const tf2::Transform& in, geometry_msgs::Pose& out ) +void toMsg(const tf2::Transform& in, geometry_msgs::msg::Pose& out ) { out.position.x = in.getOrigin().getX(); out.position.y = in.getOrigin().getY(); @@ -344,7 +344,7 @@ void toMsg(const tf2::Transform& in, geometry_msgs::Pose& out ) } inline -void fromMsg(const geometry_msgs::Pose& in, tf2::Transform& out) +void fromMsg(const geometry_msgs::msg::Pose& in, tf2::Transform& out) { out.setOrigin(tf2::Vector3(in.position.x, in.position.y, in.position.z)); // w at the end in the constructor diff --git a/tf2_geometry_msgs/package.xml b/tf2_geometry_msgs/package.xml index 4186c2271..2e1d4ec99 100644 --- a/tf2_geometry_msgs/package.xml +++ b/tf2_geometry_msgs/package.xml @@ -1,4 +1,4 @@ - + tf2_geometry_msgs 0.5.12 @@ -9,19 +9,21 @@ BSD http://www.ros.org/wiki/tf2_ros - - catkin - geometry_msgs - tf2_ros - tf2 - orocos_kdl + ament_cmake + + geometry_msgs + orocos_kdl + tf2 + tf2_ros + + python_orocos_kdl - geometry_msgs - tf2_ros - tf2 - orocos_kdl + python_orocos_kdl - rostest - + + + ament_cmake + + diff --git a/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp b/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp index caf02cac4..cfd1d67d8 100644 --- a/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp +++ b/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp @@ -43,7 +43,7 @@ static const double EPS = 1e-3; TEST(TfGeometry, Frame) { - geometry_msgs::PoseStamped v1; + geometry_msgs::msg::PoseStamped v1; v1.pose.position.x = 1; v1.pose.position.y = 2; v1.pose.position.z = 3; @@ -52,7 +52,7 @@ TEST(TfGeometry, Frame) v1.header.frame_id = "A"; // simple api - geometry_msgs::PoseStamped v_simple = tf_buffer->transform(v1, "B", tf2::Duration(2.0)); + geometry_msgs::msg::PoseStamped v_simple = tf_buffer->transform(v1, "B", tf2::Duration(2.0)); EXPECT_NEAR(v_simple.pose.position.x, -9, EPS); EXPECT_NEAR(v_simple.pose.position.y, 18, EPS); EXPECT_NEAR(v_simple.pose.position.z, 27, EPS); @@ -63,7 +63,7 @@ TEST(TfGeometry, Frame) // advanced api - geometry_msgs::PoseStamped v_advanced = tf_buffer->transform(v1, "B", builtin_interfaces::msg::Time(2.0), + geometry_msgs::msg::PoseStamped v_advanced = tf_buffer->transform(v1, "B", builtin_interfaces::msg::Time(2.0), "A", tf2::Duration(3.0)); EXPECT_NEAR(v_advanced.pose.position.x, -9, EPS); EXPECT_NEAR(v_advanced.pose.position.y, 18, EPS); @@ -78,7 +78,7 @@ TEST(TfGeometry, Frame) TEST(TfGeometry, Vector) { - geometry_msgs::Vector3Stamped v1, res; + geometry_msgs::msg::Vector3Stamped v1, res; v1.vector.x = 1; v1.vector.y = 2; v1.vector.z = 3; @@ -86,13 +86,13 @@ TEST(TfGeometry, Vector) v1.header.frame_id = "A"; // simple api - geometry_msgs::Vector3Stamped v_simple = tf_buffer->transform(v1, "B", tf2::Duration(2.0)); + geometry_msgs::msg::Vector3Stamped v_simple = tf_buffer->transform(v1, "B", tf2::Duration(2.0)); EXPECT_NEAR(v_simple.vector.x, 1, EPS); EXPECT_NEAR(v_simple.vector.y, -2, EPS); EXPECT_NEAR(v_simple.vector.z, -3, EPS); // advanced api - geometry_msgs::Vector3Stamped v_advanced = tf_buffer->transform(v1, "B", builtin_interfaces::msg::Time(2.0), + geometry_msgs::msg::Vector3Stamped v_advanced = tf_buffer->transform(v1, "B", builtin_interfaces::msg::Time(2.0), "A", tf2::Duration(3.0)); EXPECT_NEAR(v_advanced.vector.x, 1, EPS); EXPECT_NEAR(v_advanced.vector.y, -2, EPS); @@ -102,7 +102,7 @@ TEST(TfGeometry, Vector) TEST(TfGeometry, Point) { - geometry_msgs::PointStamped v1, res; + geometry_msgs::msg::PointStamped v1, res; v1.point.x = 1; v1.point.y = 2; v1.point.z = 3; @@ -110,13 +110,13 @@ TEST(TfGeometry, Point) v1.header.frame_id = "A"; // simple api - geometry_msgs::PointStamped v_simple = tf_buffer->transform(v1, "B", tf2::Duration(2.0)); + geometry_msgs::msg::PointStamped v_simple = tf_buffer->transform(v1, "B", tf2::Duration(2.0)); EXPECT_NEAR(v_simple.point.x, -9, EPS); EXPECT_NEAR(v_simple.point.y, 18, EPS); EXPECT_NEAR(v_simple.point.z, 27, EPS); // advanced api - geometry_msgs::PointStamped v_advanced = tf_buffer->transform(v1, "B", builtin_interfaces::msg::Time(2.0), + geometry_msgs::msg::PointStamped v_advanced = tf_buffer->transform(v1, "B", builtin_interfaces::msg::Time(2.0), "A", tf2::Duration(3.0)); EXPECT_NEAR(v_advanced.point.x, -9, EPS); EXPECT_NEAR(v_advanced.point.y, 18, EPS); @@ -132,7 +132,7 @@ int main(int argc, char **argv){ tf_buffer = new tf2_ros::Buffer(); // 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;