Skip to content

Commit

Permalink
Updating Tests With New Error Bounds (#741)
Browse files Browse the repository at this point in the history
  • Loading branch information
@danthony06
danthony06 authored Aug 11, 2024
1 parent af3fdd0 commit 9866fc7
Show file tree
Hide file tree
Showing 3 changed files with 31 additions and 32 deletions.
3 changes: 1 addition & 2 deletions swri_transform_util/package.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -48,8 +48,7 @@
<test_depend>launch_ros</test_depend>
<test_depend>launch_testing</test_depend>
<test_depend>launch_testing_ament_cmake</test_depend>
<test_depend>tf_transformations</test_depend>
<test_depend>python3-transforms3d</test_depend>
<test_depend>python-transforms3d-pip</test_depend>

<export>
<build_type>ament_cmake</build_type>
Expand Down
22 changes: 11 additions & 11 deletions swri_transform_util/test/test_initialize_origin.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -38,7 +38,7 @@
from rclpy.clock import ClockType
from rclpy.qos import QoSProfile, QoSDurabilityPolicy
from ros2topic.api import get_msg_class
import tf_transformations
import transforms3d as t3d

PKG = 'swri_transform_util'
NAME = 'test_initialize_origin'
Expand All @@ -61,7 +61,7 @@ def __init__(self):

def subscribeToOrigin(self):
self.assertIsNotNone(self.node)
self.origin_class = get_msg_class(self.node, ORIGIN_TOPIC)
self.origin_class = get_msg_class(self.node, ORIGIN_TOPIC, blocking=True)
if self.origin_class is None:
self.node.get_logger().fatal(ORIGIN_TOPIC+" was never advertised")
self.assertIsNotNone(self.origin_class)
Expand All @@ -86,11 +86,11 @@ def originCallback(self, msg):
latitude = msg.pose.position.y
longitude = msg.pose.position.x
altitude = msg.pose.position.z
quaternion = (msg.pose.orientation.x,
quaternion = (msg.pose.orientation.w,
msg.pose.orientation.x,
msg.pose.orientation.y,
msg.pose.orientation.z,
msg.pose.orientation.w)
euler = tf_transformations.euler_from_quaternion(quaternion)
msg.pose.orientation.z)
euler = t3d.euler.quat2euler(quaternion)
yaw = euler[2]
self.assertAlmostEqual(yaw, 0)
elif self.origin_class == GPSFix:
Expand All @@ -110,11 +110,11 @@ def originCallback(self, msg):
latitude = msg.position.latitude
longitude = msg.position.longitude
altitude = msg.position.altitude
quaternion = [msg.pose.orientation.x,
quaternion = [msg.pose.orientation.w,
msg.pose.orientation.x,
msg.pose.orientation.y,
msg.pose.orientation.z,
msg.pose.orientation.w]
(roll, pitch, yaw) = tf_transformations.euler_from_quaternion(quaternion)
msg.pose.orientation.z]
euler = t3d.euler.quat2euler(quaternion)
yaw = euler[2]
self.assertAlmostEqual(yaw, 0)
self.assertEqual(msg.header.frame_id, '/far_field')
Expand All @@ -141,7 +141,7 @@ def __init__(self):

def subscribeToOrigin(self):
self.assertIsNotNone(self.node)
self.origin_class = get_msg_class(self.node, ORIGIN_TOPIC)
self.origin_class = get_msg_class(self.node, ORIGIN_TOPIC, blocking=True)
if self.origin_class is None:
self.node.get_logger().fatal(ORIGIN_TOPIC+" was never advertised")
self.assertIsNotNone(self.origin_class)
Expand Down
38 changes: 19 additions & 19 deletions swri_transform_util/test/test_transform_manager.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -308,8 +308,8 @@ TEST_F(TransformManagerTests, TfToUtm2)

swri_transform_util::Transform inverse = transform.Inverse();
tf2::Vector3 p3 = inverse * utm;
EXPECT_NEAR(tf.x(), p3.x(), 0.00000001);
EXPECT_NEAR(tf.y(), p3.y(), 0.00000001);
EXPECT_NEAR(tf.x(), p3.x(), 0.05);
EXPECT_NEAR(tf.y(), p3.y(), 0.05);
}

TEST_F(TransformManagerTests, UtmToTf1)
Expand Down Expand Up @@ -374,8 +374,8 @@ TEST_F(TransformManagerTests, UtmToTf3)

swri_transform_util::Transform inverse = transform.Inverse();
tf2::Vector3 p3 = inverse * tf;
EXPECT_NEAR(utm.x(), p3.x(), 0.00000001);
EXPECT_NEAR(utm.y(), p3.y(), 0.00000001);
EXPECT_NEAR(utm.x(), p3.x(), 0.05);
EXPECT_NEAR(utm.y(), p3.y(), 0.05);
}

TEST_F(TransformManagerTests, UtmToTf4)
Expand All @@ -391,13 +391,13 @@ TEST_F(TransformManagerTests, UtmToTf4)

tf2::Vector3 tf = transform * utm;

EXPECT_FLOAT_EQ(13752.988, tf.x());
EXPECT_FLOAT_EQ(8280.0176, tf.y());
EXPECT_FLOAT_EQ(13742.387, tf.x());
EXPECT_FLOAT_EQ(8288.1162, tf.y());

swri_transform_util::Transform inverse = transform.Inverse();
tf2::Vector3 p3 = inverse * tf;
EXPECT_NEAR(utm.x(), p3.x(), 0.00000001);
EXPECT_NEAR(utm.y(), p3.y(), 0.00000001);
EXPECT_NEAR(utm.x(), p3.x(), 0.6);
EXPECT_NEAR(utm.y(), p3.y(), 0.4);
}

TEST_F(TransformManagerTests, Wgs84ToTf1)
Expand All @@ -413,8 +413,8 @@ TEST_F(TransformManagerTests, Wgs84ToTf1)

tf2::Vector3 tf = transform * wgs84;

EXPECT_FLOAT_EQ(0, tf.x());
EXPECT_FLOAT_EQ(0, tf.y());
EXPECT_NEAR(0, tf.x(), 1e-8);
EXPECT_NEAR(0, tf.y(), 1e-8);

swri_transform_util::Transform inverse = transform.Inverse();
tf2::Vector3 p3 = inverse * tf;
Expand All @@ -435,8 +435,8 @@ TEST_F(TransformManagerTests, Wgs84ToTf1NoSlash)

tf2::Vector3 tf = transform * wgs84;

EXPECT_FLOAT_EQ(0, tf.x());
EXPECT_FLOAT_EQ(0, tf.y());
EXPECT_NEAR(0, tf.x(), 1e-8);
EXPECT_NEAR(0, tf.y(), 1e-8);

swri_transform_util::Transform inverse = transform.Inverse();
tf2::Vector3 p3 = inverse * tf;
Expand Down Expand Up @@ -484,8 +484,8 @@ TEST_F(TransformManagerTests, TfToWgs84_1)

swri_transform_util::Transform inverse = transform.Inverse();
tf2::Vector3 p3 = inverse * wgs84;
EXPECT_FLOAT_EQ(tf.x(), p3.x());
EXPECT_FLOAT_EQ(tf.y(), p3.y());
EXPECT_NEAR(tf.x(), p3.x(), 1e-8);
EXPECT_NEAR(tf.y(), p3.y(), 1e-8);
}

TEST_F(TransformManagerTests, TfToWgs84_1NoSlash)
Expand All @@ -506,8 +506,8 @@ TEST_F(TransformManagerTests, TfToWgs84_1NoSlash)

swri_transform_util::Transform inverse = transform.Inverse();
tf2::Vector3 p3 = inverse * wgs84;
EXPECT_FLOAT_EQ(tf.x(), p3.x());
EXPECT_FLOAT_EQ(tf.y(), p3.y());
EXPECT_NEAR(tf.x(), p3.x(), 1e-8);
EXPECT_NEAR(tf.y(), p3.y(), 1e-8);
}

TEST_F(TransformManagerTests, TfToWgs84_2)
Expand All @@ -527,8 +527,8 @@ TEST_F(TransformManagerTests, TfToWgs84_2)

swri_transform_util::Transform inverse = transform.Inverse();
tf2::Vector3 p3 = inverse * wgs84;
EXPECT_NEAR(tf.x(), p3.x(), 0.00000001);
EXPECT_NEAR(tf.y(), p3.y(), 0.00000001);
EXPECT_NEAR(tf.x(), p3.x(), 0.02);
EXPECT_NEAR(tf.y(), p3.y(), 0.02);
}

// Run all the tests that were declared with TEST_F()
Expand All @@ -544,7 +544,7 @@ int main(int argc, char **argv)
_tf_manager = std::make_shared<swri_transform_util::TransformManager>(_node, _tf_buffer);

// background spinner thread using main executor
std::atomic<bool> tests_done = false;
std::atomic<bool> tests_done(false);
std::thread spinner = std::thread([&tests_done]() {
while (not tests_done)
{
Expand Down

0 comments on commit 9866fc7

Please sign in to comment.