From f4e78e443d6c44c34ba4261bb58bf01426023607 Mon Sep 17 00:00:00 2001 From: Milan Vukov Date: Thu, 28 May 2026 20:24:20 +0000 Subject: [PATCH] Add regression tests for geometry2 targets Adds ros2/test/geometry2/ with one test target per library in @ros2_geometry2: tf2 core, tf2_ros, tf2_eigen, tf2_geometry_msgs, tf2_sensor_msgs (C++), tf2_py, tf2_ros_py (Python), and launch-based startup tests for the static_transform_publisher and tf2_echo binaries. Also fixes geometry2.BUILD.bazel: adds public visibility and missing Python runtime deps (rpyutils, rclpy, py_geometry_msgs, py_tf2_msgs) to the tf2_py library, which were needed for the C++ extension to load. Co-Authored-By: Claude Sonnet 4.6 --- repositories/geometry2.BUILD.bazel | 7 ++ ros2/test/geometry2/BUILD.bazel | 100 ++++++++++++++++++ .../static_transform_publisher_tests.py | 51 +++++++++ ros2/test/geometry2/tf2_core_tests.cc | 74 +++++++++++++ ros2/test/geometry2/tf2_echo_tests.py | 58 ++++++++++ ros2/test/geometry2/tf2_eigen_tests.cc | 69 ++++++++++++ .../test/geometry2/tf2_geometry_msgs_tests.cc | 78 ++++++++++++++ ros2/test/geometry2/tf2_py_tests.py | 32 ++++++ ros2/test/geometry2/tf2_ros_py_tests.py | 51 +++++++++ ros2/test/geometry2/tf2_ros_tests.cc | 93 ++++++++++++++++ ros2/test/geometry2/tf2_sensor_msgs_tests.cc | 69 ++++++++++++ 11 files changed, 682 insertions(+) create mode 100644 ros2/test/geometry2/BUILD.bazel create mode 100644 ros2/test/geometry2/static_transform_publisher_tests.py create mode 100644 ros2/test/geometry2/tf2_core_tests.cc create mode 100644 ros2/test/geometry2/tf2_echo_tests.py create mode 100644 ros2/test/geometry2/tf2_eigen_tests.cc create mode 100644 ros2/test/geometry2/tf2_geometry_msgs_tests.cc create mode 100644 ros2/test/geometry2/tf2_py_tests.py create mode 100644 ros2/test/geometry2/tf2_ros_py_tests.py create mode 100644 ros2/test/geometry2/tf2_ros_tests.cc create mode 100644 ros2/test/geometry2/tf2_sensor_msgs_tests.cc diff --git a/repositories/geometry2.BUILD.bazel b/repositories/geometry2.BUILD.bazel index f6a43808..08de0485 100644 --- a/repositories/geometry2.BUILD.bazel +++ b/repositories/geometry2.BUILD.bazel @@ -193,6 +193,13 @@ py_library( ":tf2_py/tf2_py/_tf2_py.so", ], imports = ["tf2_py"], + visibility = ["//visibility:public"], + deps = [ + ":py_tf2_msgs", + "@ros2_common_interfaces//:py_geometry_msgs", + "@ros2_rclpy//:rclpy", + "@ros2_rpyutils//:rpyutils", + ], ) py_library( diff --git a/ros2/test/geometry2/BUILD.bazel b/ros2/test/geometry2/BUILD.bazel new file mode 100644 index 00000000..800111e3 --- /dev/null +++ b/ros2/test/geometry2/BUILD.bazel @@ -0,0 +1,100 @@ +load("@com_github_mvukov_rules_ros2//ros2:cc_defs.bzl", "ros2_cpp_test") +load("@com_github_mvukov_rules_ros2//ros2:py_defs.bzl", "ros2_py_test") +load("@com_github_mvukov_rules_ros2//ros2:test.bzl", "ros2_test") +load("@rules_ros2_pip_deps//:requirements.bzl", "requirement") + +ros2_cpp_test( + name = "tf2_core_tests", + size = "small", + srcs = ["tf2_core_tests.cc"], + deps = [ + "@googletest//:gtest", + "@ros2_geometry2//:tf2", + ], +) + +ros2_cpp_test( + name = "tf2_ros_tests", + size = "small", + srcs = ["tf2_ros_tests.cc"], + deps = [ + "@googletest//:gtest", + "@ros2_geometry2//:tf2", + "@ros2_geometry2//:tf2_ros", + "@ros2_rclcpp//:rclcpp", + ], +) + +ros2_cpp_test( + name = "tf2_eigen_tests", + size = "small", + srcs = ["tf2_eigen_tests.cc"], + deps = [ + "@googletest//:gtest", + "@ros2_geometry2//:tf2_eigen", + ], +) + +ros2_cpp_test( + name = "tf2_geometry_msgs_tests", + size = "small", + srcs = ["tf2_geometry_msgs_tests.cc"], + deps = [ + "@googletest//:gtest", + "@ros2_geometry2//:cpp_tf2_geometry_msgs", + ], +) + +ros2_cpp_test( + name = "tf2_sensor_msgs_tests", + size = "small", + srcs = ["tf2_sensor_msgs_tests.cc"], + deps = [ + "@googletest//:gtest", + "@ros2_geometry2//:cpp_tf2_sensor_msgs", + ], +) + +ros2_py_test( + name = "tf2_py_tests", + size = "small", + srcs = ["tf2_py_tests.py"], + main = "tf2_py_tests.py", + deps = [ + requirement("pytest"), + "@ros2_geometry2//:tf2_py", + ], +) + +ros2_py_test( + name = "tf2_ros_py_tests", + size = "small", + srcs = ["tf2_ros_py_tests.py"], + main = "tf2_ros_py_tests.py", + deps = [ + requirement("pytest"), + "@ros2_geometry2//:tf2_ros_py", + "@ros2_rclpy//:rclpy", + ], +) + +ros2_test( + name = "static_transform_publisher_tests", + size = "small", + launch_file = "static_transform_publisher_tests.py", + nodes = [ + "@ros2_geometry2//:static_transform_publisher", + ], + use_pytest = True, +) + +ros2_test( + name = "tf2_echo_tests", + size = "small", + launch_file = "tf2_echo_tests.py", + nodes = [ + "@ros2_geometry2//:static_transform_publisher", + "@ros2_geometry2//:tf2_echo", + ], + use_pytest = True, +) diff --git a/ros2/test/geometry2/static_transform_publisher_tests.py b/ros2/test/geometry2/static_transform_publisher_tests.py new file mode 100644 index 00000000..64a6a7fd --- /dev/null +++ b/ros2/test/geometry2/static_transform_publisher_tests.py @@ -0,0 +1,51 @@ +# Copyright 2026 Milan Vukov +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +import launch +import launch_pytest.tools +import pytest + + +@pytest.fixture +def static_transform_publisher_proc(): + return launch.actions.ExecuteProcess( + cmd=[ + 'ros2/test/geometry2/static_transform_publisher', + '--frame-id', + 'world', + '--child-frame-id', + 'base', + '--x', + '1.0', + '--y', + '0.0', + '--z', + '0.0', + ], + cached_output=True, + ) + + +@launch_pytest.fixture +def launch_description(static_transform_publisher_proc): + return launch.LaunchDescription([ + static_transform_publisher_proc, + launch_pytest.actions.ReadyToTest(), + ]) + + +@pytest.mark.launch(fixture=launch_description) +def test_node_starts(static_transform_publisher_proc, launch_context): + launch_pytest.tools.wait_for_start_sync(launch_context, + static_transform_publisher_proc, + timeout=5) diff --git a/ros2/test/geometry2/tf2_core_tests.cc b/ros2/test/geometry2/tf2_core_tests.cc new file mode 100644 index 00000000..40424221 --- /dev/null +++ b/ros2/test/geometry2/tf2_core_tests.cc @@ -0,0 +1,74 @@ +// Copyright 2026 Milan Vukov +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#include +#include + +#include "geometry_msgs/msg/transform_stamped.hpp" +#include "gtest/gtest.h" +#include "tf2/buffer_core.hpp" +#include "tf2/exceptions.hpp" +#include "tf2/time.hpp" + +namespace { + +geometry_msgs::msg::TransformStamped MakeIdentityTransform( + const std::string& parent_frame, const std::string& child_frame) { + geometry_msgs::msg::TransformStamped t; + t.header.frame_id = parent_frame; + t.child_frame_id = child_frame; + t.transform.rotation.w = 1.0; + return t; +} + +TEST(TestTf2BufferCore, WhenNoTransformAvailable_EnsureLookupThrows) { + tf2::BufferCore buffer; + EXPECT_THROW(buffer.lookupTransform("target", "source", tf2::TimePointZero), + tf2::LookupException); +} + +TEST(TestTf2BufferCore, WhenTransformSet_EnsureLookupSucceeds) { + tf2::BufferCore buffer; + buffer.setTransform(MakeIdentityTransform("base", "child"), "test", + /*is_static=*/false); + + const auto result = + buffer.lookupTransform("base", "child", tf2::TimePointZero); + EXPECT_EQ(result.header.frame_id, "base"); + EXPECT_EQ(result.child_frame_id, "child"); + EXPECT_DOUBLE_EQ(result.transform.rotation.w, 1.0); +} + +TEST(TestTf2BufferCore, WhenTransformSet_EnsureCanTransformReturnsTrue) { + tf2::BufferCore buffer; + buffer.setTransform(MakeIdentityTransform("base", "child"), "test", + /*is_static=*/false); + + EXPECT_TRUE(buffer.canTransform("base", "child", tf2::TimePointZero)); +} + +TEST(TestTf2BufferCore, WhenFrameAdded_EnsureItAppearsInFrameList) { + tf2::BufferCore buffer; + buffer.setTransform(MakeIdentityTransform("base", "child"), "test", + /*is_static=*/false); + + const std::vector frames = buffer.getAllFrameNames(); + EXPECT_NE(std::find(frames.begin(), frames.end(), "child"), frames.end()); +} + +} // namespace + +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/ros2/test/geometry2/tf2_echo_tests.py b/ros2/test/geometry2/tf2_echo_tests.py new file mode 100644 index 00000000..bf55e6ce --- /dev/null +++ b/ros2/test/geometry2/tf2_echo_tests.py @@ -0,0 +1,58 @@ +# Copyright 2026 Milan Vukov +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +import launch +import launch_pytest.tools +import pytest + + +@pytest.fixture +def static_transform_publisher_proc(): + return launch.actions.ExecuteProcess( + cmd=[ + 'ros2/test/geometry2/static_transform_publisher', + '--frame-id', + 'world', + '--child-frame-id', + 'base', + ], + cached_output=True, + ) + + +@pytest.fixture +def tf2_echo_proc(): + return launch.actions.ExecuteProcess( + cmd=['ros2/test/geometry2/tf2_echo', 'world', 'base'], + cached_output=True, + ) + + +@launch_pytest.fixture +def launch_description(static_transform_publisher_proc, tf2_echo_proc): + return launch.LaunchDescription([ + static_transform_publisher_proc, + tf2_echo_proc, + launch_pytest.actions.ReadyToTest(), + ]) + + +@pytest.mark.launch(fixture=launch_description) +def test_nodes_start(static_transform_publisher_proc, tf2_echo_proc, + launch_context): + launch_pytest.tools.wait_for_start_sync(launch_context, + static_transform_publisher_proc, + timeout=5) + launch_pytest.tools.wait_for_start_sync(launch_context, + tf2_echo_proc, + timeout=5) diff --git a/ros2/test/geometry2/tf2_eigen_tests.cc b/ros2/test/geometry2/tf2_eigen_tests.cc new file mode 100644 index 00000000..34288c00 --- /dev/null +++ b/ros2/test/geometry2/tf2_eigen_tests.cc @@ -0,0 +1,69 @@ +// Copyright 2026 Milan Vukov +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#include "Eigen/Geometry" +#include "geometry_msgs/msg/transform_stamped.hpp" +#include "gtest/gtest.h" +#include "tf2_eigen/tf2_eigen.hpp" + +namespace { + +geometry_msgs::msg::TransformStamped MakeTransformStamped(double tx, double ty, + double tz, + double qw) { + geometry_msgs::msg::TransformStamped t; + t.header.frame_id = "parent"; + t.child_frame_id = "child"; + t.transform.translation.x = tx; + t.transform.translation.y = ty; + t.transform.translation.z = tz; + t.transform.rotation.w = qw; + return t; +} + +TEST(TestTf2Eigen, + WhenIdentityTransformConverted_EnsureEigenIsometry3dIsIdentity) { + const auto transform = MakeTransformStamped(0.0, 0.0, 0.0, 1.0); + const Eigen::Isometry3d isometry = tf2::transformToEigen(transform); + EXPECT_TRUE(isometry.isApprox(Eigen::Isometry3d::Identity())); +} + +TEST(TestTf2Eigen, WhenEigenIsometry3dConverted_EnsureTranslationMatches) { + Eigen::Isometry3d isometry = Eigen::Isometry3d::Identity(); + isometry.translation() = Eigen::Vector3d(1.0, 2.0, 3.0); + + const auto transform = tf2::eigenToTransform(isometry); + EXPECT_DOUBLE_EQ(transform.transform.translation.x, 1.0); + EXPECT_DOUBLE_EQ(transform.transform.translation.y, 2.0); + EXPECT_DOUBLE_EQ(transform.transform.translation.z, 3.0); +} + +TEST(TestTf2Eigen, WhenPointTransformed_EnsureValueCorrect) { + geometry_msgs::msg::TransformStamped transform = + MakeTransformStamped(1.0, 2.0, 3.0, 1.0); + + const Eigen::Vector3d point_in(0.0, 0.0, 0.0); + Eigen::Vector3d point_out; + tf2::doTransform(point_in, point_out, transform); + + EXPECT_DOUBLE_EQ(point_out.x(), 1.0); + EXPECT_DOUBLE_EQ(point_out.y(), 2.0); + EXPECT_DOUBLE_EQ(point_out.z(), 3.0); +} + +} // namespace + +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/ros2/test/geometry2/tf2_geometry_msgs_tests.cc b/ros2/test/geometry2/tf2_geometry_msgs_tests.cc new file mode 100644 index 00000000..fc62e6e8 --- /dev/null +++ b/ros2/test/geometry2/tf2_geometry_msgs_tests.cc @@ -0,0 +1,78 @@ +// Copyright 2026 Milan Vukov +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#include + +#include "geometry_msgs/msg/point_stamped.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "geometry_msgs/msg/transform_stamped.hpp" +#include "gtest/gtest.h" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" + +namespace { + +geometry_msgs::msg::TransformStamped MakeTranslationTransform( + const std::string& parent_frame, const std::string& child_frame, double tx, + double ty, double tz) { + geometry_msgs::msg::TransformStamped t; + t.header.frame_id = parent_frame; + t.child_frame_id = child_frame; + t.transform.translation.x = tx; + t.transform.translation.y = ty; + t.transform.translation.z = tz; + t.transform.rotation.w = 1.0; + return t; +} + +TEST(TestTf2GeometryMsgs, WhenPointTransformed_EnsureFrameIdUpdated) { + geometry_msgs::msg::PointStamped point_in, point_out; + point_in.header.frame_id = "child"; + + const auto transform = MakeTranslationTransform("parent", "child", 0, 0, 0); + tf2::doTransform(point_in, point_out, transform); + + EXPECT_EQ(point_out.header.frame_id, "parent"); +} + +TEST(TestTf2GeometryMsgs, WhenPoseTransformed_EnsureFrameIdUpdated) { + geometry_msgs::msg::PoseStamped pose_in, pose_out; + pose_in.header.frame_id = "child"; + pose_in.pose.orientation.w = 1.0; + + const auto transform = MakeTranslationTransform("parent", "child", 0, 0, 0); + tf2::doTransform(pose_in, pose_out, transform); + + EXPECT_EQ(pose_out.header.frame_id, "parent"); +} + +TEST(TestTf2GeometryMsgs, WhenIdentityTransformApplied_EnsureValuesUnchanged) { + geometry_msgs::msg::PointStamped point_in, point_out; + point_in.header.frame_id = "child"; + point_in.point.x = 1.0; + point_in.point.y = 2.0; + point_in.point.z = 3.0; + + const auto transform = MakeTranslationTransform("parent", "child", 0, 0, 0); + tf2::doTransform(point_in, point_out, transform); + + EXPECT_DOUBLE_EQ(point_out.point.x, 1.0); + EXPECT_DOUBLE_EQ(point_out.point.y, 2.0); + EXPECT_DOUBLE_EQ(point_out.point.z, 3.0); +} + +} // namespace + +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/ros2/test/geometry2/tf2_py_tests.py b/ros2/test/geometry2/tf2_py_tests.py new file mode 100644 index 00000000..be3b9f41 --- /dev/null +++ b/ros2/test/geometry2/tf2_py_tests.py @@ -0,0 +1,32 @@ +# Copyright 2026 Milan Vukov +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +import pytest +import tf2_py + + +def test_buffer_core_creation(): + buffer = tf2_py.BufferCore() + assert buffer is not None + + +def test_exception_hierarchy(): + assert issubclass(tf2_py.LookupException, tf2_py.TransformException) + assert issubclass(tf2_py.ConnectivityException, tf2_py.TransformException) + assert issubclass(tf2_py.ExtrapolationException, tf2_py.TransformException) + assert issubclass(tf2_py.InvalidArgumentException, + tf2_py.TransformException) + + +if __name__ == '__main__': + raise SystemExit(pytest.main([__file__])) diff --git a/ros2/test/geometry2/tf2_ros_py_tests.py b/ros2/test/geometry2/tf2_ros_py_tests.py new file mode 100644 index 00000000..fbc99fe8 --- /dev/null +++ b/ros2/test/geometry2/tf2_ros_py_tests.py @@ -0,0 +1,51 @@ +# Copyright 2026 Milan Vukov +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +import pytest +import rclpy.time +import tf2_ros +from geometry_msgs.msg import TransformStamped + + +def _make_identity_transform(parent_frame: str, + child_frame: str) -> TransformStamped: + t = TransformStamped() + t.header.frame_id = parent_frame + t.child_frame_id = child_frame + t.transform.rotation.w = 1.0 + return t + + +def test_buffer_creation(): + buffer = tf2_ros.Buffer() + assert buffer is not None + + +def test_can_transform_returns_false_for_unknown_frames(): + buffer = tf2_ros.Buffer() + assert not buffer.can_transform('unknown_parent', 'unknown_child', + rclpy.time.Time()) + + +def test_set_and_lookup_transform(): + buffer = tf2_ros.Buffer() + transform = _make_identity_transform('base', 'child') + buffer.set_transform(transform, 'test') + + result = buffer.lookup_transform('base', 'child', rclpy.time.Time()) + assert result.header.frame_id == 'base' + assert result.child_frame_id == 'child' + + +if __name__ == '__main__': + raise SystemExit(pytest.main([__file__])) diff --git a/ros2/test/geometry2/tf2_ros_tests.cc b/ros2/test/geometry2/tf2_ros_tests.cc new file mode 100644 index 00000000..29e8817e --- /dev/null +++ b/ros2/test/geometry2/tf2_ros_tests.cc @@ -0,0 +1,93 @@ +// Copyright 2026 Milan Vukov +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#include +#include +#include + +#include "geometry_msgs/msg/transform_stamped.hpp" +#include "gtest/gtest.h" +#include "rclcpp/rclcpp.hpp" +#include "tf2/time.hpp" +#include "tf2_ros/buffer.h" +#include "tf2_ros/static_transform_broadcaster.h" +#include "tf2_ros/transform_broadcaster.h" +#include "tf2_ros/transform_listener.h" + +namespace { + +geometry_msgs::msg::TransformStamped MakeIdentityTransform( + const std::string& parent_frame, const std::string& child_frame) { + geometry_msgs::msg::TransformStamped t; + t.header.frame_id = parent_frame; + t.child_frame_id = child_frame; + t.transform.rotation.w = 1.0; + return t; +} + +TEST(TestTf2RosBuffer, WhenBufferCreated_EnsureInitiallyEmpty) { + auto clock = std::make_shared(); + tf2_ros::Buffer buffer(clock); + EXPECT_TRUE(buffer.getAllFrameNames().empty()); +} + +TEST(TestTf2RosBuffer, WhenTransformSetDirectly_EnsureLookupSucceeds) { + auto clock = std::make_shared(); + tf2_ros::Buffer buffer(clock); + buffer.setTransform(MakeIdentityTransform("base", "child"), "test", + /*is_static=*/false); + + const auto result = + buffer.lookupTransform("base", "child", tf2::TimePointZero); + EXPECT_EQ(result.header.frame_id, "base"); + EXPECT_EQ(result.child_frame_id, "child"); +} + +TEST(TestTf2RosTransformBroadcaster, WhenCreated_EnsureNoException) { + auto node = rclcpp::Node::make_shared("tf2_ros_broadcaster_test_node"); + EXPECT_NO_THROW(tf2_ros::TransformBroadcaster broadcaster(node)); +} + +TEST(TestTf2RosStaticTransformBroadcaster, + WhenStaticTransformSent_EnsureBufferContainsTransform) { + auto node = rclcpp::Node::make_shared("tf2_ros_static_broadcaster_test_node"); + auto clock = std::make_shared(RCL_ROS_TIME); + tf2_ros::Buffer buffer(clock); + tf2_ros::TransformListener listener(buffer, node, /*spin_thread=*/false); + tf2_ros::StaticTransformBroadcaster broadcaster(node); + + broadcaster.sendTransform(MakeIdentityTransform("world", "sensor")); + + const auto deadline = + std::chrono::steady_clock::now() + std::chrono::seconds(5); + while (std::chrono::steady_clock::now() < deadline) { + rclcpp::spin_some(node); + if (buffer.canTransform("world", "sensor", tf2::TimePointZero)) { + break; + } + } + + EXPECT_TRUE(buffer.canTransform("world", "sensor", tf2::TimePointZero)); +} + +} // namespace + +int main(int argc, char** argv) { + rclcpp::init(argc, argv); + testing::InitGoogleTest(&argc, argv); + const int result = RUN_ALL_TESTS(); + if (!rclcpp::shutdown() || result) { + return EXIT_FAILURE; + } + return EXIT_SUCCESS; +} diff --git a/ros2/test/geometry2/tf2_sensor_msgs_tests.cc b/ros2/test/geometry2/tf2_sensor_msgs_tests.cc new file mode 100644 index 00000000..c601e468 --- /dev/null +++ b/ros2/test/geometry2/tf2_sensor_msgs_tests.cc @@ -0,0 +1,69 @@ +// Copyright 2026 Milan Vukov +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#include + +#include "geometry_msgs/msg/transform_stamped.hpp" +#include "gtest/gtest.h" +#include "sensor_msgs/msg/point_cloud2.hpp" +#include "sensor_msgs/point_cloud2_iterator.hpp" +#include "tf2_sensor_msgs/tf2_sensor_msgs.hpp" + +namespace { + +sensor_msgs::msg::PointCloud2 MakeXyzCloud(const std::string& frame_id, + int num_points) { + sensor_msgs::msg::PointCloud2 cloud; + cloud.header.frame_id = frame_id; + sensor_msgs::PointCloud2Modifier modifier(cloud); + modifier.setPointCloud2FieldsByString(1, "xyz"); + modifier.resize(num_points); + return cloud; +} + +TEST(TestTf2SensorMsgs, WhenPointCloud2Transformed_EnsureFrameIdUpdated) { + sensor_msgs::msg::PointCloud2 cloud_in = MakeXyzCloud("sensor", 3); + sensor_msgs::msg::PointCloud2 cloud_out; + + geometry_msgs::msg::TransformStamped transform; + transform.header.frame_id = "world"; + transform.child_frame_id = "sensor"; + transform.transform.rotation.w = 1.0; + + tf2::doTransform(cloud_in, cloud_out, transform); + + EXPECT_EQ(cloud_out.header.frame_id, "world"); +} + +TEST(TestTf2SensorMsgs, + WhenIdentityTransformApplied_EnsurePointCountPreserved) { + sensor_msgs::msg::PointCloud2 cloud_in = MakeXyzCloud("sensor", 5); + sensor_msgs::msg::PointCloud2 cloud_out; + + geometry_msgs::msg::TransformStamped transform; + transform.header.frame_id = "world"; + transform.child_frame_id = "sensor"; + transform.transform.rotation.w = 1.0; + + tf2::doTransform(cloud_in, cloud_out, transform); + + EXPECT_EQ(cloud_out.width, cloud_in.width); + EXPECT_EQ(cloud_out.height, cloud_in.height); +} + +} // namespace + +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +}