test: add TF listener/broadcaster tests

aica-technology · Dec 5, 2024 · a0e2a43 · a0e2a43
1 parent decebff
commit a0e2a43
Showing 1 changed file with 59 additions and 1 deletion.
diff --git a/source/modulo_controllers/test/test_controller_interface.cpp b/source/modulo_controllers/test/test_controller_interface.cpp
@@ -18,6 +18,7 @@ class FriendControllerInterface : public ControllerInterface {
 public:
   using ControllerInterface::add_input;
   using ControllerInterface::add_output;
+  using ControllerInterface::add_tf_listener;
   using ControllerInterface::read_input;
   using ControllerInterface::write_output;
 
@@ -211,7 +212,64 @@ TYPED_TEST_P(ControllerInterfaceTest, OutputTest) {
   }
 }
 
-REGISTER_TYPED_TEST_CASE_P(ControllerInterfaceTest, ConfigureErrorTest, InputTest, OutputTest);
+TYPED_TEST_P(ControllerInterfaceTest, TF) {
+  this->interface_->add_tf_broadcaster();
+  this->interface_->add_static_tf_broadcaster();
+  this->interface_->add_tf_listener();
+  auto send_tf = state_representation::CartesianPose::Random("test", "world");
+  EXPECT_NO_THROW(this->interface_->send_transform(send_tf));
+  sleep(1);
+  state_representation::CartesianPose lookup_tf;
+  EXPECT_NO_THROW(lookup_tf = this->interface_->lookup_transform("test", "world"));
+  auto identity = send_tf * lookup_tf.inverse();
+  EXPECT_FLOAT_EQ(identity.data().norm(), 1.);
+  EXPECT_FLOAT_EQ(abs(identity.get_orientation().w()), 1.);
+
+  sleep(1);
+  EXPECT_THROW(
+      lookup_tf = this->interface_->lookup_transform("test", "world", 0.9),
+      modulo_core::exceptions::LookupTransformException);
+
+  auto send_static_tf = state_representation::CartesianPose::Random("static_test", "world");
+  EXPECT_NO_THROW(this->interface_->send_static_transform(send_static_tf));
+  EXPECT_THROW(
+      auto throw_tf = this->interface_->lookup_transform("dummy", "world"),
+      modulo_core::exceptions::LookupTransformException);
+
+  EXPECT_NO_THROW(lookup_tf = this->interface_->lookup_transform("static_test", "world"));
+  identity = send_static_tf * lookup_tf.inverse();
+  EXPECT_FLOAT_EQ(identity.data().norm(), 1.);
+  EXPECT_FLOAT_EQ(abs(identity.get_orientation().w()), 1.);
+
+  std::vector<state_representation::CartesianPose> send_tfs;
+  send_tfs.reserve(3);
+  for (std::size_t idx = 0; idx < 3; ++idx) {
+    send_tfs.emplace_back(state_representation::CartesianPose::Random("test_" + std::to_string(idx), "world"));
+  }
+  EXPECT_NO_THROW(this->interface_->send_transforms(send_tfs));
+  for (const auto& tf : send_tfs) {
+    lookup_tf = this->interface_->lookup_transform(tf.get_name(), tf.get_reference_frame());
+    identity = tf * lookup_tf.inverse();
+    EXPECT_FLOAT_EQ(identity.data().norm(), 1.);
+    EXPECT_FLOAT_EQ(abs(identity.get_orientation().w()), 1.);
+  }
+
+  std::vector<state_representation::CartesianPose> send_static_tfs;
+  send_static_tfs.reserve(3);
+  for (std::size_t idx = 0; idx < 3; ++idx) {
+    send_static_tfs.emplace_back(
+        state_representation::CartesianPose::Random("test_static_" + std::to_string(idx), "world"));
+  }
+  EXPECT_NO_THROW(this->interface_->send_static_transforms(send_static_tfs));
+  for (const auto& tf : send_static_tfs) {
+    lookup_tf = this->interface_->lookup_transform(tf.get_name(), tf.get_reference_frame());
+    identity = tf * lookup_tf.inverse();
+    EXPECT_FLOAT_EQ(identity.data().norm(), 1.);
+    EXPECT_FLOAT_EQ(abs(identity.get_orientation().w()), 1.);
+  }
+}
+
+REGISTER_TYPED_TEST_CASE_P(ControllerInterfaceTest, ConfigureErrorTest, InputTest, OutputTest, TF);
 
 typedef ::testing::Types<BoolT, DoubleT, DoubleVecT, IntT, StringT, CartesianStateT, JointStateT, ImageT> SignalTypes;
 INSTANTIATE_TYPED_TEST_CASE_P(TestPrefix, ControllerInterfaceTest, SignalTypes);