add test

Signed-off-by: Kento Yabuuchi <[email protected]>

localization/pose2twist/CMakeLists.txt

@@ -14,6 +14,13 @@ rclcpp_components_register_node(${PROJECT_NAME}
   EXECUTOR SingleThreadedExecutor
 )
 
+if(BUILD_TESTING)
+  find_package(ament_cmake_gtest REQUIRED)
+  ament_auto_add_gtest(test_angular_velocity
+    test/test_angular_velocity.cpp
+  )
+endif()
+
 ament_auto_package(
   INSTALL_TO_SHARE
   launch

localization/pose2twist/include/pose2twist/pose2twist_core.hpp

@@ -21,6 +21,14 @@
 #include <geometry_msgs/msg/twist_stamped.hpp>
 #include <tier4_debug_msgs/msg/float32_stamped.hpp>
 
+#ifdef ROS_DISTRO_GALACTIC
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#else
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+#endif
+
+geometry_msgs::msg::Vector3 rotation_vector_from_quaternion(
+  const tf2::Quaternion & q1, const tf2::Quaternion & q2);
 class Pose2Twist : public rclcpp::Node
 {
 public:

localization/pose2twist/src/pose2twist_core.cpp

@@ -14,12 +14,6 @@
 
 #include "pose2twist/pose2twist_core.hpp"
 
-#ifdef ROS_DISTRO_GALACTIC
-#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
-#else
-#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
-#endif
-
 #include <cmath>
 #include <cstddef>
 #include <functional>

localization/pose2twist/test/test_angular_velocity.cpp

@@ -0,0 +1,41 @@
+// Copyright 2022 The Autoware Contributors
+//
+// 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 "pose2twist/pose2twist_core.hpp"
+
+#include <gtest/gtest.h>
+
+TEST(AngularVelocityFromQuaternion, CheckNumericalValidity)
+{
+  tf2::Quaternion initial_q;
+  initial_q.setRPY(0.2, 0.3, 0.4);
+
+  const tf2::Vector3 expected_axis = tf2::Vector3(0.1, 0.2, 0.3).normalized();
+  const double expected_angle = 0.1;  // 0.1 radian = 5.7 degrees
+
+  tf2::Quaternion expected_q;
+  expected_q.setRotation(expected_axis, expected_angle);
+
+  const tf2::Quaternion final_q = initial_q * expected_q;
+
+  const geometry_msgs::msg::Vector3 rotation_vector =
+    rotation_vector_from_quaternion(initial_q, final_q);
+
+  // 1e-3 radian = 0.057 degrees
+  constexpr double ACCEPTABLE_ERROR = 1e-3;
+
+  EXPECT_NEAR(rotation_vector.x, expected_axis.x() * expected_angle, ACCEPTABLE_ERROR);
+  EXPECT_NEAR(rotation_vector.y, expected_axis.y() * expected_angle, ACCEPTABLE_ERROR);
+  EXPECT_NEAR(rotation_vector.z, expected_axis.z() * expected_angle, ACCEPTABLE_ERROR);
+}