fix(pose2twist): compute angular velocity through quaternion

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/README.md

@@ -5,7 +5,7 @@
 This `pose2twist` calculates the velocity from the input pose history. In addition to the computed twist, this node outputs the linear-x and angular-z components as a float message to simplify debugging.
 
 The `twist.linear.x` is calculated as `sqrt(dx * dx + dy * dy + dz * dz) / dt`, and the values in the `y` and `z` fields are zero.
-The `twist.angular` is calculated as `d_roll / dt`, `d_pitch / dt` and `d_yaw / dt` for each field.
+The `twist.angular` is calculated as `relative_rotation_vector / dt` for each field.
 
 ## Inputs / Outputs
 

localization/pose2twist/include/pose2twist/pose2twist_core.hpp

@@ -21,6 +21,16 @@
 #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
+
+// Compute the relative rotation of q2 from q1 as a rotation vector
+geometry_msgs::msg::Vector3 compute_relative_rotation_vector(
+  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>
@@ -41,29 +35,21 @@ Pose2Twist::Pose2Twist(const rclcpp::NodeOptions & options) : rclcpp::Node("pose
     "pose", queue_size, std::bind(&Pose2Twist::callback_pose, this, _1));
 }
 
-double calc_diff_for_radian(const double lhs_rad, const double rhs_rad)
-{
-  double diff_rad = lhs_rad - rhs_rad;
-  if (diff_rad > M_PI) {
-    diff_rad = diff_rad - 2 * M_PI;
-  } else if (diff_rad < -M_PI) {
-    diff_rad = diff_rad + 2 * M_PI;
-  }
-  return diff_rad;
-}
-
-// x: roll, y: pitch, z: yaw
-geometry_msgs::msg::Vector3 get_rpy(const geometry_msgs::msg::Pose & pose)
+tf2::Quaternion get_quaternion(const geometry_msgs::msg::PoseStamped::SharedPtr & pose_stamped_ptr)
 {
-  geometry_msgs::msg::Vector3 rpy;
-  tf2::Quaternion q(pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w);
-  tf2::Matrix3x3(q).getRPY(rpy.x, rpy.y, rpy.z);
-  return rpy;
+  const auto & orientation = pose_stamped_ptr->pose.orientation;
+  return tf2::Quaternion{orientation.x, orientation.y, orientation.z, orientation.w};
 }
 
-geometry_msgs::msg::Vector3 get_rpy(geometry_msgs::msg::PoseStamped::SharedPtr pose)
+geometry_msgs::msg::Vector3 compute_relative_rotation_vector(
+  const tf2::Quaternion & q1, const tf2::Quaternion & q2)
 {
-  return get_rpy(pose->pose);
+  // If we define q2 as the rotation obtained by applying dq after applying q1,
+  // then q2 = q1 * dq .
+  // Therefore, dq = q1.inverse() * q2 .
+  const tf2::Quaternion diff_quaternion = q1.inverse() * q2;
+  const tf2::Vector3 axis = diff_quaternion.getAxis() * diff_quaternion.getAngle();
+  return geometry_msgs::msg::Vector3{}.set__x(axis.x()).set__y(axis.y()).set__z(axis.z());
 }
 
 geometry_msgs::msg::TwistStamped calc_twist(
@@ -79,18 +65,16 @@ geometry_msgs::msg::TwistStamped calc_twist(
     return twist;
   }
 
-  const auto pose_a_rpy = get_rpy(pose_a);
-  const auto pose_b_rpy = get_rpy(pose_b);
+  const auto pose_a_quaternion = get_quaternion(pose_a);
+  const auto pose_b_quaternion = get_quaternion(pose_b);
 
   geometry_msgs::msg::Vector3 diff_xyz;
-  geometry_msgs::msg::Vector3 diff_rpy;
+  const geometry_msgs::msg::Vector3 relative_rotation_vector =
+    compute_relative_rotation_vector(pose_a_quaternion, pose_b_quaternion);
 
   diff_xyz.x = pose_b->pose.position.x - pose_a->pose.position.x;
   diff_xyz.y = pose_b->pose.position.y - pose_a->pose.position.y;
   diff_xyz.z = pose_b->pose.position.z - pose_a->pose.position.z;
-  diff_rpy.x = calc_diff_for_radian(pose_b_rpy.x, pose_a_rpy.x);
-  diff_rpy.y = calc_diff_for_radian(pose_b_rpy.y, pose_a_rpy.y);
-  diff_rpy.z = calc_diff_for_radian(pose_b_rpy.z, pose_a_rpy.z);
 
   geometry_msgs::msg::TwistStamped twist;
   twist.header = pose_b->header;
@@ -99,9 +83,9 @@ geometry_msgs::msg::TwistStamped calc_twist(
     dt;
   twist.twist.linear.y = 0;
   twist.twist.linear.z = 0;
-  twist.twist.angular.x = diff_rpy.x / dt;
-  twist.twist.angular.y = diff_rpy.y / dt;
-  twist.twist.angular.z = diff_rpy.z / dt;
+  twist.twist.angular.x = relative_rotation_vector.x / dt;
+  twist.twist.angular.y = relative_rotation_vector.y / dt;
+  twist.twist.angular.z = relative_rotation_vector.z / dt;
 
   return twist;
 }

localization/pose2twist/test/test_angular_velocity.cpp

@@ -0,0 +1,115 @@
+// 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>
+
+// 1e-3 radian = 0.057 degrees
+constexpr double acceptable_error = 1e-3;
+
+TEST(AngularVelocityFromQuaternion, CheckMultiplicationOrder)
+{
+  // If we define q2 as the rotation obtained by applying dq after applying q1, then q2 = q1 * dq .
+  //
+  // IT IS NOT q2 = dq * q1 .
+  //
+  // This test checks that the multiplication order is correct.
+
+  const tf2::Vector3 target_vector(1, 2, 3);
+  // initial state
+  // Now, car is facing to the +x direction
+  //         z
+  //         ^     y
+  //         |    ^
+  //         |   /
+  //         |  /
+  //        car -> x
+  //
+  //
+  //
+
+  tf2::Quaternion q1;
+  q1.setRPY(0., 0., M_PI / 2.);  // yaw = 90 degrees
+  const tf2::Vector3 initially_rotated_vector = tf2::quatRotate(q1, target_vector);
+  // after applying q1
+  // Now, car is facing to the +y direction
+  //         z
+  //         ^
+  //         |    y
+  //         |  ^
+  //         | /
+  //     <--car    x
+  //
+  //
+  //
+  EXPECT_NEAR(initially_rotated_vector.x(), -2., acceptable_error);
+  EXPECT_NEAR(initially_rotated_vector.y(), 1., acceptable_error);
+  EXPECT_NEAR(initially_rotated_vector.z(), 3., acceptable_error);
+
+  tf2::Quaternion dq;
+  dq.setRPY(0., M_PI / 2., 0.);  // pitch = 90 degrees
+  const tf2::Vector3 finally_rotated_vector = tf2::quatRotate(q1 * dq, target_vector);
+  // after applying dq
+  // Now, car is facing to the -z direction
+  //         z     y
+  //              ^
+  //             /
+  //            /
+  //           /
+  //     <--car    x
+  //         |
+  //         v
+  //
+  EXPECT_NEAR(finally_rotated_vector.x(), -2., acceptable_error);
+  EXPECT_NEAR(finally_rotated_vector.y(), 3., acceptable_error);
+  EXPECT_NEAR(finally_rotated_vector.z(), -1., acceptable_error);
+
+  // Failure case
+  {
+    const tf2::Vector3 false_rotated_vector = tf2::quatRotate(dq * q1, target_vector);
+
+    EXPECT_FALSE(std::abs(false_rotated_vector.x() - (-2)) < acceptable_error);
+    EXPECT_FALSE(std::abs(false_rotated_vector.y() - (3)) < acceptable_error);
+    EXPECT_FALSE(std::abs(false_rotated_vector.z() - (-1)) < acceptable_error);
+  }
+}
+
+TEST(AngularVelocityFromQuaternion, CheckNumericalValidity)
+{
+  auto test = [](const tf2::Vector3 & expected_axis, const double expected_angle) -> void {
+    tf2::Quaternion expected_q;
+    expected_q.setRotation(expected_axis, expected_angle);
+
+    // Create a random initial quaternion
+    tf2::Quaternion initial_q;
+    initial_q.setRPY(0.2, 0.3, 0.4);
+
+    // Calculate the final quaternion by rotating the initial quaternion by the expected
+    // quaternion
+    const tf2::Quaternion final_q = initial_q * expected_q;
+
+    // Calculate the relative rotation between the initial and final quaternion
+    const geometry_msgs::msg::Vector3 rotation_vector =
+      compute_relative_rotation_vector(initial_q, final_q);
+
+    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);
+  };
+
+  test(tf2::Vector3(1.0, 0.0, 0.0).normalized(), 0.1);   // 0.1 radian =  5.7 degrees
+  test(tf2::Vector3(1.0, 1.0, 0.0).normalized(), -0.2);  // 0.2 radian = 11.4 degrees
+  test(tf2::Vector3(1.0, 2.0, 3.0).normalized(), 0.3);   // 0.3 radian = 17.2 degrees
+}