Skip to content

Commit

Permalink
Got a few more transform manager tests to work. Loss of precision som…
Browse files Browse the repository at this point in the history 
…ewhere causing existing test to fail.
  • Loading branch information
@rjb0026
rjb0026 committed Jan 26, 2024
1 parent 55791c8 commit 019ad11
Show file tree
Hide file tree
Showing 2 changed files with 59 additions and 59 deletions.
12 changes: 6 additions & 6 deletions swri_transform_util/launch/transform_manager.test.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -73,12 +73,12 @@ def generate_test_description():
arguments=[
"--x", "500.0",
"--y", "500.0",
"--z", "0.0",
"--roll", "0.0",
"--pitch", "0.0",
"--yaw", "0.0",
"--frame_id", "/far_field",
"--child_frame_id", "/near_field"],
"--z", "0",
"--roll", "0",
"--pitch", "0",
"--yaw", "0",
"--frame-id", "far_field",
"--child-frame-id", "near_field"],
)
# TODO static transform publisher tf2
# TODO static transform publisher tf3
Expand Down
106 changes: 53 additions & 53 deletions swri_transform_util/test/test_transform_manager.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -89,11 +89,11 @@ TEST_F(TransformManagerTests, Identity1)
/* TEST_F(TransformManagerTests, IdentityNoSlash) */
/* { */
/* swri_transform_util::Transform transform; */
/* ASSERT_TRUE(_tf_manager.GetTransform( */
/* ASSERT_TRUE(_tf_manager->GetTransform( */
/* "/near_field", */
/* "near_field", */
/* transform)); */
/* ASSERT_TRUE(_tf_manager.GetTransform( */
/* ASSERT_TRUE(_tf_manager->GetTransform( */
/* "near_field", */
/* "/near_field", */
/* transform)); */
Expand All @@ -104,7 +104,7 @@ TEST_F(TransformManagerTests, Identity1)
/* tf2::Vector3 p1(435, -900, 0); */

/* swri_transform_util::Transform transform; */
/* ASSERT_TRUE(_tf_manager.GetTransform( */
/* ASSERT_TRUE(_tf_manager->GetTransform( */
/* "/some_frame", */
/* "/some_frame", */
/* transform)); */
Expand All @@ -126,7 +126,7 @@ TEST_F(TransformManagerTests, Identity1)
/* tf2::Vector3 far_field(0, 0, 0); */

/* swri_transform_util::Transform transform; */
/* ASSERT_TRUE(_tf_manager.GetTransform( */
/* ASSERT_TRUE(_tf_manager->GetTransform( */
/* "/near_field", */
/* "/far_field", */
/* transform)); */
Expand All @@ -148,7 +148,7 @@ TEST_F(TransformManagerTests, Identity1)
/* tf2::Vector3 far_field(0, 0, 0); */

/* swri_transform_util::Transform transform; */
/* ASSERT_TRUE(_tf_manager.GetTransform( */
/* ASSERT_TRUE(_tf_manager->GetTransform( */
/* "/near_field", */
/* "far_field", */
/* transform)); */
Expand All @@ -170,7 +170,7 @@ TEST_F(TransformManagerTests, Identity1)
/* tf2::Vector3 far_field(0, 0, 0); */

/* tf::StampedTransform transform; */
/* ASSERT_TRUE(_tf_manager.GetTransform( */
/* ASSERT_TRUE(_tf_manager->GetTransform( */
/* "/near_field", */
/* "/far_field", */
/* transform)); */
Expand All @@ -187,7 +187,7 @@ TEST_F(TransformManagerTests, Identity1)
/* tf2::Vector3 wgs84(-98.471944, 29.526667, 0); */

/* swri_transform_util::Transform transform; */
/* ASSERT_TRUE(_tf_manager.GetTransform( */
/* ASSERT_TRUE(_tf_manager->GetTransform( */
/* swri_transform_util::_utm_frame, */
/* swri_transform_util::_wgs84_frame, */
/* transform)); */
Expand All @@ -209,7 +209,7 @@ TEST_F(TransformManagerTests, Identity1)
/* tf2::Vector3 wgs84(-98.471944, 29.526667, 0); */

/* swri_transform_util::Transform transform; */
/* ASSERT_TRUE(_tf_manager.GetTransform( */
/* ASSERT_TRUE(_tf_manager->GetTransform( */
/* "utm", */
/* "wgs84", */
/* transform)); */
Expand All @@ -231,7 +231,7 @@ TEST_F(TransformManagerTests, Identity1)
/* tf2::Vector3 utm(551170, 3266454, 0); */

/* swri_transform_util::Transform transform; */
/* ASSERT_TRUE(_tf_manager.GetTransform( */
/* ASSERT_TRUE(_tf_manager->GetTransform( */
/* swri_transform_util::_wgs84_frame, */
/* swri_transform_util::_utm_frame, */
/* transform)); */
Expand All @@ -253,7 +253,7 @@ TEST_F(TransformManagerTests, Identity1)
/* tf2::Vector3 tf(0, 0, 0); */

/* swri_transform_util::Transform transform; */
/* ASSERT_TRUE(_tf_manager.GetTransform( */
/* ASSERT_TRUE(_tf_manager->GetTransform( */
/* swri_transform_util::_utm_frame, */
/* "/far_field", */
/* transform)); */
Expand All @@ -275,7 +275,7 @@ TEST_F(TransformManagerTests, Identity1)
/* tf2::Vector3 tf(0, 0, 0); */

/* swri_transform_util::Transform transform; */
/* ASSERT_TRUE(_tf_manager.GetTransform( */
/* ASSERT_TRUE(_tf_manager->GetTransform( */
/* "utm", */
/* "far_field", */
/* transform)); */
Expand All @@ -296,7 +296,7 @@ TEST_F(TransformManagerTests, Identity1)
/* tf2::Vector3 tf(500, 500, 0); */

/* swri_transform_util::Transform transform; */
/* ASSERT_TRUE(_tf_manager.GetTransform( */
/* ASSERT_TRUE(_tf_manager->GetTransform( */
/* swri_transform_util::_utm_frame, */
/* "/far_field", */
/* transform)); */
Expand All @@ -318,7 +318,7 @@ TEST_F(TransformManagerTests, Identity1)
/* tf2::Vector3 utm(537460.3372816057, 3258123.434110421, 0); */

/* swri_transform_util::Transform transform; */
/* ASSERT_TRUE(_tf_manager.GetTransform( */
/* ASSERT_TRUE(_tf_manager->GetTransform( */
/* "/far_field", */
/* swri_transform_util::_utm_frame, */
/* transform)); */
Expand All @@ -340,7 +340,7 @@ TEST_F(TransformManagerTests, Identity1)
/* tf2::Vector3 utm(537460.3372816057, 3258123.434110421, 0); */

/* swri_transform_util::Transform transform; */
/* ASSERT_TRUE(_tf_manager.GetTransform( */
/* ASSERT_TRUE(_tf_manager->GetTransform( */
/* "/near_field", */
/* swri_transform_util::_utm_frame, */
/* transform)); */
Expand All @@ -362,7 +362,7 @@ TEST_F(TransformManagerTests, Identity1)
/* tf2::Vector3 utm(537460.3372816057 - 500, 3258123.434110421 - 500, 0); */

/* swri_transform_util::Transform transform; */
/* ASSERT_TRUE(_tf_manager.GetTransform( */
/* ASSERT_TRUE(_tf_manager->GetTransform( */
/* "/far_field", */
/* swri_transform_util::_utm_frame, */
/* transform)); */
Expand All @@ -384,7 +384,7 @@ TEST_F(TransformManagerTests, Identity1)
/* tf2::Vector3 utm(551170, 3266454, 0); */

/* swri_transform_util::Transform transform; */
/* ASSERT_TRUE(_tf_manager.GetTransform( */
/* ASSERT_TRUE(_tf_manager->GetTransform( */
/* "/far_field", */
/* swri_transform_util::_utm_frame, */
/* transform)); */
Expand Down Expand Up @@ -422,57 +422,57 @@ TEST_F(TransformManagerTests, Wgs84ToTf1)
EXPECT_FLOAT_EQ(wgs84.y(), p3.y());
}

/* TEST_F(TransformManagerTests, Wgs84ToTf1NoSlash) */
/* { */
/* // Local Origin */
/* tf2::Vector3 wgs84(-98.61370577, 29.45196669, 0); */
TEST_F(TransformManagerTests, Wgs84ToTf1NoSlash)
{
// Local Origin
tf2::Vector3 wgs84(-98.61370577, 29.45196669, 0);

/* swri_transform_util::Transform transform; */
/* ASSERT_TRUE(_tf_manager.GetTransform( */
/* "far_field", */
/* "wgs84", */
/* transform)); */
swri_transform_util::Transform transform;
ASSERT_TRUE(_tf_manager->GetTransform(
"far_field",
"wgs84",
transform));

/* tf2::Vector3 tf = transform * wgs84; */
tf2::Vector3 tf = transform * wgs84;

/* EXPECT_FLOAT_EQ(0, tf.x()); */
/* EXPECT_FLOAT_EQ(0, tf.y()); */
EXPECT_FLOAT_EQ(0, tf.x());
EXPECT_FLOAT_EQ(0, tf.y());

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

/* TEST_F(TransformManagerTests, Wgs84ToTf2) */
/* { */
/* // Local Origin */
/* tf2::Vector3 wgs84(-98.61370577, 29.45196669, 0); */
TEST_F(TransformManagerTests, Wgs84ToTf2)
{
// Local Origin
tf2::Vector3 wgs84(-98.61370577, 29.45196669, 0);

/* swri_transform_util::Transform transform; */
/* ASSERT_TRUE(_tf_manager.GetTransform( */
/* "/near_field", */
/* swri_transform_util::_wgs84_frame, */
/* transform)); */
swri_transform_util::Transform transform;
ASSERT_TRUE(_tf_manager->GetTransform(
"/near_field",
swri_transform_util::_wgs84_frame,
transform));

/* tf2::Vector3 tf = transform * wgs84; */
tf2::Vector3 tf = transform * wgs84;

/* EXPECT_FLOAT_EQ(-500, tf.x()); */
/* EXPECT_FLOAT_EQ(-500, tf.y()); */
EXPECT_FLOAT_EQ(-500, tf.x());
EXPECT_FLOAT_EQ(-500, tf.y());

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

/* TEST_F(TransformManagerTests, TfToWgs84_1) */
/* { */
/* // Local Origin */
/* tf2::Vector3 tf(0, 0, 0); */

/* swri_transform_util::Transform transform; */
/* ASSERT_TRUE(_tf_manager.GetTransform( */
/* ASSERT_TRUE(_tf_manager->GetTransform( */
/* swri_transform_util::_wgs84_frame, */
/* "/far_field", */
/* transform)); */
Expand All @@ -494,7 +494,7 @@ TEST_F(TransformManagerTests, Wgs84ToTf1)
/* tf2::Vector3 tf(0, 0, 0); */

/* swri_transform_util::Transform transform; */
/* ASSERT_TRUE(_tf_manager.GetTransform( */
/* ASSERT_TRUE(_tf_manager->GetTransform( */
/* "wgs84", */
/* "far_field", */
/* transform)); */
Expand All @@ -515,7 +515,7 @@ TEST_F(TransformManagerTests, Wgs84ToTf1)
/* tf2::Vector3 tf(0, 0, 0); */

/* swri_transform_util::Transform transform; */
/* ASSERT_TRUE(_tf_manager.GetTransform( */
/* ASSERT_TRUE(_tf_manager->GetTransform( */
/* swri_transform_util::_wgs84_frame, */
/* "/near_field", */
/* transform)); */
Expand Down

0 comments on commit 019ad11

Please sign in to comment.