Skip to content

Commit

Permalink
add some comments
Browse files Browse the repository at this point in the history 
Signed-off-by: Kento Yabuuchi <[email protected]>
  • Loading branch information
@KYabuuchi
KYabuuchi committed Jun 7, 2024
1 parent 43ffb89 commit 1d1bdb7
Show file tree
Hide file tree
Showing 3 changed files with 97 additions and 18 deletions.
4 changes: 3 additions & 1 deletion localization/pose2twist/include/pose2twist/pose2twist_core.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -27,8 +27,10 @@
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#endif

geometry_msgs::msg::Vector3 rotation_vector_from_quaternion(
// 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:
Expand Down
7 changes: 5 additions & 2 deletions localization/pose2twist/src/pose2twist_core.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -41,9 +41,12 @@ tf2::Quaternion get_quaternion(const geometry_msgs::msg::PoseStamped::SharedPtr
return tf2::Quaternion{orientation.x, orientation.y, orientation.z, orientation.w};
}

geometry_msgs::msg::Vector3 rotation_vector_from_quaternion(
geometry_msgs::msg::Vector3 compute_relative_rotation_vector(
const tf2::Quaternion & q1, const tf2::Quaternion & q2)
{
// If we define q2 as the rotation obntained 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());
Expand All @@ -67,7 +70,7 @@ geometry_msgs::msg::TwistStamped calc_twist(

geometry_msgs::msg::Vector3 diff_xyz;
const geometry_msgs::msg::Vector3 diff_rpy =
rotation_vector_from_quaternion(pose_a_quaternion, pose_b_quaternion);
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;
Expand Down
104 changes: 89 additions & 15 deletions localization/pose2twist/test/test_angular_velocity.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,26 +16,100 @@

#include <gtest/gtest.h>

TEST(AngularVelocityFromQuaternion, CheckNumericalValidity)
// 1e-3 radian = 0.057 degrees
constexpr double ACCEPTABLE_ERROR = 1e-3;

TEST(AngularVelocityFromQuaternion, CheckMultiplicationOrder)
{
tf2::Quaternion initial_q;
initial_q.setRPY(0.2, 0.3, 0.4);
// If we define q2 as the rotation obntained 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);

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
// 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);

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);

const tf2::Quaternion final_q = initial_q * expected_q;
// Calculate the final quaternion by rotating the initial quaternion by the expected
// quaternion
const tf2::Quaternion final_q = initial_q * expected_q;

const geometry_msgs::msg::Vector3 rotation_vector =
rotation_vector_from_quaternion(initial_q, final_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);

// 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);
};

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
}

0 comments on commit 1d1bdb7

Please sign in to comment.