Skip to content

Commit

Permalink
fix: rename TF frames to not interfere with components
Browse files Browse the repository at this point in the history
  • Loading branch information
bpapaspyros committed Dec 5, 2024
1 parent 35283ae commit 33c77f7
Showing 1 changed file with 9 additions and 8 deletions.
17 changes: 9 additions & 8 deletions source/modulo_controllers/test/test_controller_interface.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -223,41 +223,42 @@ TYPED_TEST_P(ControllerInterfaceTest, TF) {
EXPECT_THROW(this->interface_->add_tf_broadcaster(), modulo_core::exceptions::CoreException);
EXPECT_THROW(this->interface_->add_static_tf_broadcaster(), modulo_core::exceptions::CoreException);
EXPECT_THROW(this->interface_->add_tf_listener(), modulo_core::exceptions::CoreException);
auto send_early_tf = state_representation::CartesianPose::Random("test", "world");
auto send_early_tf = state_representation::CartesianPose::Random("controller_test", "world");
EXPECT_THROW(this->interface_->send_transform(send_early_tf), modulo_core::exceptions::CoreException);
this->init();
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");
auto send_tf = state_representation::CartesianPose::Random("controller_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"));
EXPECT_NO_THROW(lookup_tf = this->interface_->lookup_transform("controller_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),
lookup_tf = this->interface_->lookup_transform("controller_test", "world", 0.1),
modulo_core::exceptions::LookupTransformException);

auto send_static_tf = state_representation::CartesianPose::Random("static_test", "world");
auto send_static_tf = state_representation::CartesianPose::Random("controller_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"));
EXPECT_NO_THROW(lookup_tf = this->interface_->lookup_transform("controller_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"));
send_tfs.emplace_back(
state_representation::CartesianPose::Random("controller_test_" + std::to_string(idx), "world"));
}
EXPECT_NO_THROW(this->interface_->send_transforms(send_tfs));
for (const auto& tf : send_tfs) {
Expand All @@ -271,7 +272,7 @@ TYPED_TEST_P(ControllerInterfaceTest, TF) {
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"));
state_representation::CartesianPose::Random("controller_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) {
Expand Down

0 comments on commit 33c77f7

Please sign in to comment.