Added tests for the exported state interfaces

ros-controls · Oct 7, 2024 · b2924e2 · b2924e2
1 parent b287c54
commit b2924e2
Showing 1 changed file with 47 additions and 0 deletions.
diff --git a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp
@@ -307,6 +307,29 @@ TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_Publish_Success_with_Offsets
   ASSERT_EQ(wrench_msg.wrench.torque.x, sensor_values_[3] - torque_offsets[0]);
   ASSERT_EQ(wrench_msg.wrench.torque.y, sensor_values_[4] - torque_offsets[1]);
   ASSERT_EQ(wrench_msg.wrench.torque.z, sensor_values_[5] - torque_offsets[2]);
+
+  // Check the exported state interfaces
+  const auto exported_state_interfaces = fts_broadcaster_->export_state_interfaces();
+  ASSERT_EQ(exported_state_interfaces.size(), 6u);
+  const std::string controller_name = fts_broadcaster_->get_node()->get_name();
+  ASSERT_EQ(
+    exported_state_interfaces[0].get_name(), controller_name + "/" + sensor_name_ + "/force.x");
+  ASSERT_EQ(
+    exported_state_interfaces[1].get_name(), controller_name + "/" + sensor_name_ + "/force.y");
+  ASSERT_EQ(
+    exported_state_interfaces[2].get_name(), controller_name + "/" + sensor_name_ + "/force.z");
+  ASSERT_EQ(
+    exported_state_interfaces[3].get_name(), controller_name + "/" + sensor_name_ + "/torque.x");
+  ASSERT_EQ(
+    exported_state_interfaces[4].get_name(), controller_name + "/" + sensor_name_ + "/torque.y");
+  ASSERT_EQ(
+    exported_state_interfaces[5].get_name(), controller_name + "/" + sensor_name_ + "/torque.z");
+  for (size_t i = 0; i < 6; ++i)
+  {
+    ASSERT_EQ(
+      exported_state_interfaces[i].get_value(),
+      sensor_values_[i] - (i < 3 ? force_offsets[i] : torque_offsets[i - 3]));
+  }
 }
 
 TEST_F(ForceTorqueSensorBroadcasterTest, InterfaceNames_Publish_Success)
@@ -331,6 +354,15 @@ TEST_F(ForceTorqueSensorBroadcasterTest, InterfaceNames_Publish_Success)
   ASSERT_TRUE(std::isnan(wrench_msg.wrench.torque.x));
   ASSERT_TRUE(std::isnan(wrench_msg.wrench.torque.y));
   ASSERT_EQ(wrench_msg.wrench.torque.z, sensor_values_[5]);
+
+  // Check the exported state interfaces
+  const auto exported_state_interfaces = fts_broadcaster_->export_state_interfaces();
+  ASSERT_EQ(exported_state_interfaces.size(), 2u);
+  const std::string controller_name = fts_broadcaster_->get_node()->get_name();
+  ASSERT_EQ(exported_state_interfaces[0].get_name(), controller_name + "/fts_sensor/force.x");
+  ASSERT_EQ(exported_state_interfaces[1].get_name(), controller_name + "/fts_sensor/torque.z");
+  ASSERT_EQ(exported_state_interfaces[0].get_value(), sensor_values_[0]);
+  ASSERT_EQ(exported_state_interfaces[1].get_value(), sensor_values_[5]);
 }
 
 TEST_F(ForceTorqueSensorBroadcasterTest, All_InterfaceNames_Publish_Success)
@@ -359,6 +391,21 @@ TEST_F(ForceTorqueSensorBroadcasterTest, All_InterfaceNames_Publish_Success)
   ASSERT_EQ(wrench_msg.wrench.torque.x, sensor_values_[3]);
   ASSERT_EQ(wrench_msg.wrench.torque.y, sensor_values_[4]);
   ASSERT_EQ(wrench_msg.wrench.torque.z, sensor_values_[5]);
+
+  // Check the exported state interfaces
+  const auto exported_state_interfaces = fts_broadcaster_->export_state_interfaces();
+  ASSERT_EQ(exported_state_interfaces.size(), 6u);
+  const std::string controller_name = fts_broadcaster_->get_node()->get_name();
+  ASSERT_EQ(exported_state_interfaces[0].get_name(), controller_name + "/fts_sensor/force.x");
+  ASSERT_EQ(exported_state_interfaces[1].get_name(), controller_name + "/fts_sensor/force.y");
+  ASSERT_EQ(exported_state_interfaces[2].get_name(), controller_name + "/fts_sensor/force.z");
+  ASSERT_EQ(exported_state_interfaces[3].get_name(), controller_name + "/fts_sensor/torque.x");
+  ASSERT_EQ(exported_state_interfaces[4].get_name(), controller_name + "/fts_sensor/torque.y");
+  ASSERT_EQ(exported_state_interfaces[5].get_name(), controller_name + "/fts_sensor/torque.z");
+  for (size_t i = 0; i < 6; ++i)
+  {
+    ASSERT_EQ(exported_state_interfaces[i].get_value(), sensor_values_[i]);
+  }
 }
 
 int main(int argc, char ** argv)