@@ -24,19 +24,6 @@ bool quat_are_near(urdf::Rotation left, urdf::Rotation right)
24
24
std::abs (l[3 ] + r[3 ]) < epsilon);
25
25
}
26
26
27
- std::ostream &operator <<(std::ostream &os, const urdf::Rotation& rot)
28
- {
29
- double roll, pitch, yaw;
30
- double x, y, z, w;
31
- rot.getRPY (roll, pitch, yaw);
32
- rot.getQuaternion (x, y, z, w);
33
- os << std::setprecision (9 )
34
- << " x: " << x << " y: " << y << " z: " << z << " w: " << w
35
- << " roll: " << roll << " pitch: " << pitch << " yaw: " << yaw;
36
- return os;
37
- }
38
-
39
-
40
27
void check_get_set_rpy_is_idempotent (double x, double y, double z, double w)
41
28
{
42
29
urdf::Rotation rot0;
@@ -45,12 +32,6 @@ void check_get_set_rpy_is_idempotent(double x, double y, double z, double w)
45
32
rot0.getRPY (roll, pitch, yaw);
46
33
urdf::Rotation rot1;
47
34
rot1.setFromRPY (roll, pitch, yaw);
48
- if (true ) {
49
- std::cout << " \n "
50
- << " before " << rot0 << " \n "
51
- << " after " << rot1 << " \n "
52
- << " ok " << quat_are_near (rot0, rot1) << " \n " ;
53
- }
54
35
EXPECT_TRUE (quat_are_near (rot0, rot1));
55
36
}
56
37
@@ -63,12 +44,6 @@ void check_get_set_rpy_is_idempotent_from_rpy(double r, double p, double y)
63
44
urdf::Rotation rot1;
64
45
rot1.setFromRPY (roll, pitch, yaw);
65
46
bool ok = quat_are_near (rot0, rot1);
66
- if (!ok) {
67
- std::cout << " initial rpy: " << r << " " << p << " " << y << " \n "
68
- << " before " << rot0 << " \n "
69
- << " after " << rot1 << " \n "
70
- << " ok " << ok << " \n " ;
71
- }
72
47
EXPECT_TRUE (ok);
73
48
}
74
49
0 commit comments