Skip to content

Commit

Permalink
fix clang-tidy's warning
Browse files Browse the repository at this point in the history
Signed-off-by: Kento Yabuuchi <[email protected]>
  • Loading branch information
KYabuuchi committed Jun 7, 2024
1 parent 3e08985 commit 159d072
Showing 1 changed file with 13 additions and 13 deletions.
26 changes: 13 additions & 13 deletions localization/pose2twist/test/test_angular_velocity.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -17,7 +17,7 @@
#include <gtest/gtest.h>

// 1e-3 radian = 0.057 degrees
constexpr double ACCEPTABLE_ERROR = 1e-3;
constexpr double acceptable_error = 1e-3;

TEST(AngularVelocityFromQuaternion, CheckMultiplicationOrder)
{
Expand Down Expand Up @@ -54,9 +54,9 @@ TEST(AngularVelocityFromQuaternion, CheckMultiplicationOrder)
//
//
//
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);
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
Expand All @@ -72,17 +72,17 @@ TEST(AngularVelocityFromQuaternion, CheckMultiplicationOrder)
// |
// 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);
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);
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);
}
}

Expand All @@ -104,9 +104,9 @@ TEST(AngularVelocityFromQuaternion, CheckNumericalValidity)
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);
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
Expand Down

0 comments on commit 159d072

Please sign in to comment.