From 88d6b8a7a25b4814a245b2a335a6b9757afcfbef Mon Sep 17 00:00:00 2001 From: Guillaume Walck Date: Sat, 25 Mar 2023 19:25:44 +0100 Subject: [PATCH] Switched to yaml-based param in filter test --- CMakeLists.txt | 4 +- .../test_gravity_compensation.cpp | 53 ++++++++++++------- .../test_gravity_compensation.hpp | 5 +- .../test_gravity_compensation_parameters.yaml | 35 ++++++++++++ 4 files changed, 73 insertions(+), 24 deletions(-) create mode 100644 test/control_filters/test_gravity_compensation_parameters.yaml diff --git a/CMakeLists.txt b/CMakeLists.txt index ee02b5cf..39531a87 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -107,7 +107,9 @@ if(BUILD_TESTING) ament_target_dependencies(pid_publisher_tests rclcpp_lifecycle) ## Control Filters - ament_add_gmock(test_gravity_compensation test/control_filters/test_gravity_compensation.cpp) + add_rostest_with_parameters_gmock(test_gravity_compensation test/control_filters/test_gravity_compensation.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/control_filters/test_gravity_compensation_parameters.yaml + ) target_link_libraries(test_gravity_compensation gravity_compensation gravity_compensation_filter_parameters) ament_target_dependencies(test_gravity_compensation ${CONTROL_FILTERS_INCLUDE_DEPENDS}) diff --git a/test/control_filters/test_gravity_compensation.cpp b/test/control_filters/test_gravity_compensation.cpp index 987c560a..39ac1055 100644 --- a/test/control_filters/test_gravity_compensation.cpp +++ b/test/control_filters/test_gravity_compensation.cpp @@ -20,8 +20,6 @@ TEST_F(GravityCompensationTest, TestGravityCompensationMissingParameters) std::shared_ptr> filter_ = std::make_shared>(); - node_->declare_parameter("world_frame", "world"); - // one mandatory param missing, should fail ASSERT_FALSE(filter_->configure("", "TestGravityCompensation", node_->get_node_logging_interface(), node_->get_node_parameters_interface())); @@ -30,22 +28,16 @@ TEST_F(GravityCompensationTest, TestGravityCompensationMissingParameters) */ } -TEST_F(GravityCompensationTest, TestGravityCompensationParameters) +TEST_F(GravityCompensationTest, TestGravityCompensationInvalidThenFixedParameter) { std::shared_ptr> filter_ = std::make_shared>(); - double gravity_acc = 9.81; - double mass = 5.0; - node_->declare_parameter("world_frame", "world"); - node_->declare_parameter("sensor_frame", "sensor"); - node_->declare_parameter("CoG.force", std::vector({0.0, 0.0, -gravity_acc * mass})); - - node_->declare_parameter("CoG.pos", std::vector({0.0, 0.0})); // wrong vector size, should fail ASSERT_FALSE(filter_->configure("", "TestGravityCompensation", node_->get_node_logging_interface(), node_->get_node_parameters_interface())); + // fixed wrong vector size node_->set_parameter(rclcpp::Parameter("CoG.pos", std::vector({0.0, 0.0, 0.0}))); // all parameters correctly set AND second call to yet unconfigured filter ASSERT_TRUE(filter_->configure("", "TestGravityCompensation", @@ -58,17 +50,32 @@ TEST_F(GravityCompensationTest, TestGravityCompensationParameters) node_->get_node_logging_interface(), node_->get_node_parameters_interface())); } -TEST_F(GravityCompensationTest, TestGravityCompensation) + +TEST_F(GravityCompensationTest, TestGravityCompensationMissingTransform) +{ + std::shared_ptr> filter_ = + std::make_shared>(); + + // all parameters correctly set + ASSERT_TRUE(filter_->configure("", "TestGravityCompensation", + node_->get_node_logging_interface(), node_->get_node_parameters_interface())); + + geometry_msgs::msg::WrenchStamped in, out; + in.header.frame_id = "world"; + in.wrench.force.x = 1.0; + in.wrench.torque.x = 10.0; + + // should fail due to missing datain frame to sensor frame transform + ASSERT_FALSE(filter_->update(in, out)); +} + +TEST_F(GravityCompensationTest, TestGravityCompensationComputation) { std::shared_ptr> filter_ = std::make_shared>(); double gravity_acc = 9.81; double mass = 5.0; - node_->declare_parameter("world_frame", "world"); - node_->declare_parameter("sensor_frame", "sensor"); - node_->declare_parameter("CoG.pos", std::vector({0.0, 0.0, 0.0})); - node_->declare_parameter("CoG.force", std::vector({0.0, 0.0, -gravity_acc * mass})); ASSERT_TRUE(filter_->configure("", "TestGravityCompensation", node_->get_node_logging_interface(), node_->get_node_parameters_interface())); @@ -78,15 +85,12 @@ TEST_F(GravityCompensationTest, TestGravityCompensation) in.wrench.force.x = 1.0; in.wrench.torque.x = 10.0; - // should fail due to missing datain frame to sensor frame transform - ASSERT_FALSE(filter_->update(in, out)); - node_->set_parameter(rclcpp::Parameter("sensor_frame", "world")); - // should pass (now transform is identity) + // should pass (transform is identity) ASSERT_TRUE(filter_->update(in, out)); ASSERT_EQ(out.wrench.force.x, 1.0); ASSERT_EQ(out.wrench.force.y, 0.0); - ASSERT_EQ(out.wrench.force.z, gravity_acc * mass); + ASSERT_NEAR(out.wrench.force.z, gravity_acc * mass, 0.0001); ASSERT_EQ(out.wrench.torque.x, 10.0); ASSERT_EQ(out.wrench.torque.y, 0.0); @@ -98,3 +102,12 @@ TEST_F(GravityCompensationTest, TestGravityCompensation) // TODO(guihomework) Add a test with real lookups } + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/test/control_filters/test_gravity_compensation.hpp b/test/control_filters/test_gravity_compensation.hpp index 14e24129..367811d6 100644 --- a/test/control_filters/test_gravity_compensation.hpp +++ b/test/control_filters/test_gravity_compensation.hpp @@ -33,14 +33,14 @@ class GravityCompensationTest : public ::testing::Test public: void SetUp() override { + auto testname = ::testing::UnitTest::GetInstance()->current_test_info()->name(); + node_ = std::make_shared(testname); executor_->add_node(node_); executor_thread_ = std::thread([this]() { executor_->spin(); }); } GravityCompensationTest() { - rclcpp::init(0, nullptr); - node_ = std::make_shared("test_gravity_compensation"); executor_ = std::make_shared(); } @@ -52,7 +52,6 @@ class GravityCompensationTest : public ::testing::Test executor_thread_.join(); } node_.reset(); - rclcpp::shutdown(); } protected: diff --git a/test/control_filters/test_gravity_compensation_parameters.yaml b/test/control_filters/test_gravity_compensation_parameters.yaml new file mode 100644 index 00000000..1224e284 --- /dev/null +++ b/test/control_filters/test_gravity_compensation_parameters.yaml @@ -0,0 +1,35 @@ +TestGravityCompensationAllParameters: + ros__parameters: + world_frame: world + sensor_frame: sensor + CoG: + force: [0.0, 0.0, -49.05] + pos: [0.0, 0.0, 0.0] + +TestGravityCompensationMissingParameters: + ros__parameters: + world_frame: world + +TestGravityCompensationInvalidThenFixedParameter: + ros__parameters: + world_frame: world + sensor_frame: sensor + CoG: + force: [0.0, 0.0, -49.05] + pos: [0.0, 0.0] + +TestGravityCompensationMissingTransform: + ros__parameters: + world_frame: world + sensor_frame: sensor + CoG: + force: [0.0, 0.0, -49.05] + pos: [0.0, 0.0, 0.0] + +TestGravityCompensationComputation: + ros__parameters: + world_frame: world + sensor_frame: world + CoG: + force: [0.0, 0.0, -49.05] + pos: [0.0, 0.0, 0.0]