17 #include <geometry_msgs/Pose.h>
18 #include <geometry_msgs/PoseWithCovariance.h>
19 #include <geometry_msgs/Quaternion.h>
20 #include <gtest/gtest.h>
26 #include <Eigen/Dense>
30 TEST(PoseConversions, copyMatrix3x3ToCMatrixDouble33)
32 tf2::Matrix3x3 src(0, 1, 2, 3, 4, 5, 6, 7, 8);
35 for (
int r = 0; r < 3; r++)
36 for (
int c = 0; c < 3; c++) EXPECT_FLOAT_EQ(des(r, c), src[r][c]);
38 TEST(PoseConversions, copyCMatrixDouble33ToMatrix3x3)
41 for (
int r = 0, i = 0; r < 3; r++)
42 for (
int c = 0; c < 3; c++, i++) src(r, c) = i;
46 for (
int r = 0; r < 3; r++)
47 for (
int c = 0; c < 3; c++) EXPECT_FLOAT_EQ(des[r][c], src(r, c));
51 TEST(PoseConversions, reference_frame_change_with_rotations)
53 geometry_msgs::PoseWithCovariance ros_msg_original_pose;
55 ros_msg_original_pose.pose.position.x = 1;
56 ros_msg_original_pose.pose.position.y = 0;
57 ros_msg_original_pose.pose.position.z = 0;
58 ros_msg_original_pose.pose.orientation.x = 0;
59 ros_msg_original_pose.pose.orientation.y = 0;
60 ros_msg_original_pose.pose.orientation.z = 0;
61 ros_msg_original_pose.pose.orientation.w = 1;
68 ros_msg_original_pose.pose.position.x, mrpt_original_pose.
mean[0]);
72 tf::Pose tf_original_pose;
73 tf::poseMsgToTF(ros_msg_original_pose.pose, tf_original_pose);
77 double yaw =
M_PI / 2.0;
83 tf::Quaternion rotation_tf;
84 rotation_tf.setRPY(0, 0, yaw);
85 tf::Pose rotation_pose_tf;
86 rotation_pose_tf.setIdentity();
87 rotation_pose_tf.setRotation(rotation_tf);
88 tf::Pose tf_result = rotation_pose_tf * tf_original_pose;
91 geometry_msgs::Pose mrpt_ros_result;
94 EXPECT_NEAR(mrpt_ros_result.position.x, tf_result.getOrigin()[0], 0.01);
95 EXPECT_NEAR(mrpt_ros_result.position.y, tf_result.getOrigin()[1], 0.01);
96 EXPECT_NEAR(mrpt_ros_result.position.z, tf_result.getOrigin()[2], 0.01);
101 double x,
double y,
double z,
double yaw,
double pitch,
double roll)
112 EXPECT_NEAR(ros_p3D.position.x, p3D.
x(), 1e-4) <<
"p: " << p3D << endl;
113 EXPECT_NEAR(ros_p3D.position.y, p3D.
y(), 1e-4) <<
"p: " << p3D << endl;
114 EXPECT_NEAR(ros_p3D.position.z, p3D.z(), 1e-4) <<
"p: " << p3D << endl;
116 EXPECT_NEAR(ros_p3D.orientation.x, q.
x(), 1e-4) <<
"p: " << p3D << endl;
117 EXPECT_NEAR(ros_p3D.orientation.y, q.
y(), 1e-4) <<
"p: " << p3D << endl;
118 EXPECT_NEAR(ros_p3D.orientation.z, q.
z(), 1e-4) <<
"p: " << p3D << endl;
119 EXPECT_NEAR(ros_p3D.orientation.w, q.
r(), 1e-4) <<
"p: " << p3D << endl;
128 <<
"p_bis: " << p_bis << endl
129 <<
"p3D: " << p3D << endl;
137 using namespace mrpt;
150 TEST(PoseConversions, check_CPose2D_to_ROS)
165 <<
"p3D_ros: " << p3D_ros << endl
166 <<
"p3D: " << p3D << endl;