Switched to yaml-based param in filter test

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})
 

test/control_filters/test_gravity_compensation.cpp

@@ -20,8 +20,6 @@ TEST_F(GravityCompensationTest, TestGravityCompensationMissingParameters)
   std::shared_ptr<filters::FilterBase<geometry_msgs::msg::WrenchStamped>> filter_ =
     std::make_shared<control_filters::GravityCompensation<geometry_msgs::msg::WrenchStamped>>();
 
-  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<filters::FilterBase<geometry_msgs::msg::WrenchStamped>> filter_ =
     std::make_shared<control_filters::GravityCompensation<geometry_msgs::msg::WrenchStamped>>();
 
-  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<double>({0.0, 0.0, -gravity_acc * mass}));
-
-  node_->declare_parameter("CoG.pos", std::vector<double>({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<double>({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<filters::FilterBase<geometry_msgs::msg::WrenchStamped>> filter_ =
+    std::make_shared<control_filters::GravityCompensation<geometry_msgs::msg::WrenchStamped>>();
+
+  // 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<filters::FilterBase<geometry_msgs::msg::WrenchStamped>> filter_ =
     std::make_shared<control_filters::GravityCompensation<geometry_msgs::msg::WrenchStamped>>();
 
   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<double>({0.0, 0.0, 0.0}));
-  node_->declare_parameter("CoG.force", std::vector<double>({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;
+}

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<rclcpp::Node>(testname);
     executor_->add_node(node_);
     executor_thread_ = std::thread([this]() { executor_->spin(); });
   }
 
   GravityCompensationTest()
   {
-    rclcpp::init(0, nullptr);
-    node_ = std::make_shared<rclcpp::Node>("test_gravity_compensation");
     executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
   }
 
@@ -52,7 +52,6 @@ class GravityCompensationTest : public ::testing::Test
       executor_thread_.join();
     }
     node_.reset();
-    rclcpp::shutdown();
   }
 
 protected:

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]