Add regression tests for geometry2 targets

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(

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,
+)

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)

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 <string>
+#include <vector>
+
+#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<std::string> 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();
+}

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)

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();
+}