From c85b83d211b92973bdf23b6ddb185f9e621217e2 Mon Sep 17 00:00:00 2001 From: Kento Yabuuchi Date: Fri, 7 Jun 2024 14:44:31 +0900 Subject: [PATCH] fix(pose2twist): compute angular velocity through quaternion (#7322) * compute angular velocity from two quaternions Signed-off-by: Kento Yabuuchi * add test Signed-off-by: Kento Yabuuchi * add some comments Signed-off-by: Kento Yabuuchi * remove rpy description Signed-off-by: Kento Yabuuchi * fix typo Signed-off-by: Kento Yabuuchi * fix clang-tidy's warning Signed-off-by: Kento Yabuuchi --------- Signed-off-by: Kento Yabuuchi --- localization/pose2twist/CMakeLists.txt | 7 ++ localization/pose2twist/README.md | 2 +- .../include/pose2twist/pose2twist_core.hpp | 10 ++ .../pose2twist/src/pose2twist_core.cpp | 52 +++----- .../pose2twist/test/test_angular_velocity.cpp | 115 ++++++++++++++++++ 5 files changed, 151 insertions(+), 35 deletions(-) create mode 100644 localization/pose2twist/test/test_angular_velocity.cpp diff --git a/localization/pose2twist/CMakeLists.txt b/localization/pose2twist/CMakeLists.txt index 2a586aa9cd049..ee63d9f43559a 100644 --- a/localization/pose2twist/CMakeLists.txt +++ b/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 diff --git a/localization/pose2twist/README.md b/localization/pose2twist/README.md index 07d9c37b710fc..f1f7d6408fafb 100644 --- a/localization/pose2twist/README.md +++ b/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 diff --git a/localization/pose2twist/include/pose2twist/pose2twist_core.hpp b/localization/pose2twist/include/pose2twist/pose2twist_core.hpp index 274fec47b3c32..d1ff6ee5ff8b6 100644 --- a/localization/pose2twist/include/pose2twist/pose2twist_core.hpp +++ b/localization/pose2twist/include/pose2twist/pose2twist_core.hpp @@ -21,6 +21,16 @@ #include #include +#ifdef ROS_DISTRO_GALACTIC +#include +#else +#include +#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: diff --git a/localization/pose2twist/src/pose2twist_core.cpp b/localization/pose2twist/src/pose2twist_core.cpp index f1106c15ebc0d..cdde78ed7e357 100644 --- a/localization/pose2twist/src/pose2twist_core.cpp +++ b/localization/pose2twist/src/pose2twist_core.cpp @@ -14,12 +14,6 @@ #include "pose2twist/pose2twist_core.hpp" -#ifdef ROS_DISTRO_GALACTIC -#include -#else -#include -#endif - #include #include #include @@ -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; } diff --git a/localization/pose2twist/test/test_angular_velocity.cpp b/localization/pose2twist/test/test_angular_velocity.cpp new file mode 100644 index 0000000000000..bf2ca0a3ba5c2 --- /dev/null +++ b/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 + +// 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 +}