Add interface configuration tests for broadcasters

force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp

@@ -32,6 +32,8 @@
 #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
 
 using hardware_interface::LoanedStateInterface;
+using testing::IsEmpty;
+using testing::SizeIs;
 
 namespace
 {
@@ -157,6 +159,12 @@ TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_Configure_Success)
 
   // configure passed
   ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS);
+
+  // check interface configuration
+  auto cmd_if_conf = fts_broadcaster_->command_interface_configuration();
+  ASSERT_THAT(cmd_if_conf.names, IsEmpty());
+  auto state_if_conf = fts_broadcaster_->state_interface_configuration();
+  ASSERT_THAT(state_if_conf.names, SizeIs(6lu));
 }
 
 TEST_F(ForceTorqueSensorBroadcasterTest, InterfaceNames_Configure_Success)
@@ -175,7 +183,7 @@ TEST_F(ForceTorqueSensorBroadcasterTest, InterfaceNames_Configure_Success)
   ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS);
 }
 
-TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_Activate_Success)
+TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_ActivateDeactivate_Success)
 {
   SetUpFTSBroadcaster();
 
@@ -186,6 +194,21 @@ TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_Activate_Success)
   // configure and activate success
   ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS);
   ASSERT_EQ(fts_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS);
+
+  // check interface configuration
+  auto cmd_if_conf = fts_broadcaster_->command_interface_configuration();
+  ASSERT_THAT(cmd_if_conf.names, IsEmpty());
+  auto state_if_conf = fts_broadcaster_->state_interface_configuration();
+  ASSERT_THAT(state_if_conf.names, SizeIs(6lu));
+
+  // deactivate passed
+  ASSERT_EQ(fts_broadcaster_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS);
+
+  // check interface configuration
+  cmd_if_conf = fts_broadcaster_->command_interface_configuration();
+  ASSERT_THAT(cmd_if_conf.names, IsEmpty());
+  state_if_conf = fts_broadcaster_->state_interface_configuration();
+  ASSERT_THAT(state_if_conf.names, SizeIs(6lu));  // did not change
 }
 
 TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_Update_Success)

force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.hpp

@@ -36,7 +36,7 @@ class FriendForceTorqueSensorBroadcaster
   FRIEND_TEST(ForceTorqueSensorBroadcasterTest, SensorNameParameterIsEmpty);
   FRIEND_TEST(ForceTorqueSensorBroadcasterTest, InterfaceNameParameterIsEmpty);
 
-  FRIEND_TEST(ForceTorqueSensorBroadcasterTest, ActivateSuccess);
+  FRIEND_TEST(ForceTorqueSensorBroadcasterTest, SensorName_ActivateDeactivate_Success);
   FRIEND_TEST(ForceTorqueSensorBroadcasterTest, UpdateTest);
   FRIEND_TEST(ForceTorqueSensorBroadcasterTest, SensorStatePublishTest);
 };

imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp

@@ -32,6 +32,8 @@
 #include "sensor_msgs/msg/imu.hpp"
 
 using hardware_interface::LoanedStateInterface;
+using testing::IsEmpty;
+using testing::SizeIs;
 
 namespace
 {
@@ -113,6 +115,12 @@ TEST_F(IMUSensorBroadcasterTest, SensorName_Configure_Success)
 
   // configure passed
   ASSERT_EQ(imu_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS);
+
+  // check interface configuration
+  auto cmd_if_conf = imu_broadcaster_->command_interface_configuration();
+  ASSERT_THAT(cmd_if_conf.names, IsEmpty());
+  auto state_if_conf = imu_broadcaster_->state_interface_configuration();
+  ASSERT_THAT(state_if_conf.names, SizeIs(10lu));
 }
 
 TEST_F(IMUSensorBroadcasterTest, SensorName_Activate_Success)
@@ -126,6 +134,21 @@ TEST_F(IMUSensorBroadcasterTest, SensorName_Activate_Success)
   // configure and activate success
   ASSERT_EQ(imu_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS);
   ASSERT_EQ(imu_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS);
+
+  // check interface configuration
+  auto cmd_if_conf = imu_broadcaster_->command_interface_configuration();
+  ASSERT_THAT(cmd_if_conf.names, IsEmpty());
+  auto state_if_conf = imu_broadcaster_->state_interface_configuration();
+  ASSERT_THAT(state_if_conf.names, SizeIs(10lu));
+
+  // deactivate passed
+  ASSERT_EQ(imu_broadcaster_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS);
+
+  // check interface configuration
+  cmd_if_conf = imu_broadcaster_->command_interface_configuration();
+  ASSERT_THAT(cmd_if_conf.names, IsEmpty());
+  state_if_conf = imu_broadcaster_->state_interface_configuration();
+  ASSERT_THAT(state_if_conf.names, SizeIs(10lu));  // did not change
 }
 
 TEST_F(IMUSensorBroadcasterTest, SensorName_Update_Success)

joint_state_broadcaster/test/test_joint_state_broadcaster.cpp

@@ -163,7 +163,7 @@ TEST_F(JointStateBroadcasterTest, ConfigureErrorTest)
   ASSERT_FALSE(state_broadcaster_->realtime_dynamic_joint_state_publisher_);
 }
 
-TEST_F(JointStateBroadcasterTest, ActivateTest)
+TEST_F(JointStateBroadcasterTest, ActivateEmptyTest)
 {
   // publishers not initialized yet
   ASSERT_FALSE(state_broadcaster_->joint_state_publisher_);
@@ -177,6 +177,12 @@ TEST_F(JointStateBroadcasterTest, ActivateTest)
 
   const size_t NUM_JOINTS = joint_names_.size();
 
+  // check interface configuration
+  auto cmd_if_conf = state_broadcaster_->command_interface_configuration();
+  ASSERT_THAT(cmd_if_conf.names, IsEmpty());
+  auto state_if_conf = state_broadcaster_->state_interface_configuration();
+  ASSERT_THAT(state_if_conf.names, IsEmpty());
+
   // publishers initialized
   ASSERT_TRUE(state_broadcaster_->realtime_joint_state_publisher_);
   ASSERT_TRUE(state_broadcaster_->realtime_dynamic_joint_state_publisher_);
@@ -218,6 +224,12 @@ TEST_F(JointStateBroadcasterTest, ActivateTestWithoutJointsParameter)
 
   const size_t NUM_JOINTS = joint_names_.size();
 
+  // check interface configuration
+  auto cmd_if_conf = state_broadcaster_->command_interface_configuration();
+  ASSERT_THAT(cmd_if_conf.names, IsEmpty());
+  auto state_if_conf = state_broadcaster_->state_interface_configuration();
+  ASSERT_THAT(state_if_conf.names, IsEmpty());
+
   // publishers initialized
   ASSERT_TRUE(state_broadcaster_->realtime_joint_state_publisher_);
   ASSERT_TRUE(state_broadcaster_->realtime_dynamic_joint_state_publisher_);
@@ -259,6 +271,12 @@ TEST_F(JointStateBroadcasterTest, ActivateTestWithoutInterfacesParameter)
 
   const size_t NUM_JOINTS = joint_names_.size();
 
+  // check interface configuration
+  auto cmd_if_conf = state_broadcaster_->command_interface_configuration();
+  ASSERT_THAT(cmd_if_conf.names, IsEmpty());
+  auto state_if_conf = state_broadcaster_->state_interface_configuration();
+  ASSERT_THAT(state_if_conf.names, IsEmpty());
+
   // publishers initialized
   ASSERT_TRUE(state_broadcaster_->realtime_joint_state_publisher_);
   ASSERT_TRUE(state_broadcaster_->realtime_dynamic_joint_state_publisher_);
@@ -287,7 +305,7 @@ TEST_F(JointStateBroadcasterTest, ActivateTestWithoutInterfacesParameter)
     ElementsAreArray(interface_names_));
 }
 
-TEST_F(JointStateBroadcasterTest, ActivateTestTwoJointsOneInterface)
+TEST_F(JointStateBroadcasterTest, ActivateDeactivateTestTwoJointsOneInterface)
 {
   const std::vector<std::string> JOINT_NAMES = {joint_names_[0], joint_names_[1]};
   const std::vector<std::string> IF_NAMES = {interface_names_[0]};
@@ -300,6 +318,12 @@ TEST_F(JointStateBroadcasterTest, ActivateTestTwoJointsOneInterface)
 
   const size_t NUM_JOINTS = JOINT_NAMES.size();
 
+  // check interface configuration
+  auto cmd_if_conf = state_broadcaster_->command_interface_configuration();
+  ASSERT_THAT(cmd_if_conf.names, IsEmpty());
+  auto state_if_conf = state_broadcaster_->state_interface_configuration();
+  ASSERT_THAT(state_if_conf.names, SizeIs(JOINT_NAMES.size() * IF_NAMES.size()));
+
   // publishers initialized
   ASSERT_TRUE(state_broadcaster_->realtime_joint_state_publisher_);
   ASSERT_TRUE(state_broadcaster_->realtime_dynamic_joint_state_publisher_);
@@ -329,6 +353,16 @@ TEST_F(JointStateBroadcasterTest, ActivateTestTwoJointsOneInterface)
     dynamic_joint_state_msg.interface_values[0].interface_names, ElementsAreArray(IF_NAMES));
   ASSERT_THAT(
     dynamic_joint_state_msg.interface_values[1].interface_names, ElementsAreArray(IF_NAMES));
+
+  // deactivate
+  ASSERT_EQ(state_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS);
+
+  // check interface configuration
+  cmd_if_conf = state_broadcaster_->command_interface_configuration();
+  ASSERT_THAT(cmd_if_conf.names, IsEmpty());
+  state_if_conf = state_broadcaster_->state_interface_configuration();
+  ASSERT_THAT(
+    state_if_conf.names, SizeIs(JOINT_NAMES.size() * IF_NAMES.size()));  // does not change
 }
 
 TEST_F(JointStateBroadcasterTest, ActivateTestOneJointTwoInterfaces)
@@ -344,6 +378,12 @@ TEST_F(JointStateBroadcasterTest, ActivateTestOneJointTwoInterfaces)
 
   const size_t NUM_JOINTS = JOINT_NAMES.size();
 
+  // check interface configuration
+  auto cmd_if_conf = state_broadcaster_->command_interface_configuration();
+  ASSERT_THAT(cmd_if_conf.names, IsEmpty());
+  auto state_if_conf = state_broadcaster_->state_interface_configuration();
+  ASSERT_THAT(state_if_conf.names, SizeIs(JOINT_NAMES.size() * IF_NAMES.size()));
+
   // publishers initialized
   ASSERT_TRUE(state_broadcaster_->realtime_joint_state_publisher_);
   ASSERT_TRUE(state_broadcaster_->realtime_dynamic_joint_state_publisher_);
@@ -412,6 +452,12 @@ TEST_F(JointStateBroadcasterTest, ActivateTestTwoJointTwoInterfacesOneMissing)
 
   const size_t NUM_JOINTS = JOINT_NAMES.size();
 
+  // check interface configuration
+  auto cmd_if_conf = state_broadcaster_->command_interface_configuration();
+  ASSERT_THAT(cmd_if_conf.names, IsEmpty());
+  auto state_if_conf = state_broadcaster_->state_interface_configuration();
+  ASSERT_THAT(state_if_conf.names, SizeIs(JOINT_NAMES.size() * IF_NAMES.size()));
+
   // publishers initialized
   ASSERT_TRUE(state_broadcaster_->realtime_joint_state_publisher_);
   ASSERT_TRUE(state_broadcaster_->realtime_dynamic_joint_state_publisher_);
@@ -455,6 +501,12 @@ TEST_F(JointStateBroadcasterTest, TestCustomInterfaceWithoutMapping)
 
   const size_t NUM_JOINTS = JOINT_NAMES.size();
 
+  // check interface configuration
+  auto cmd_if_conf = state_broadcaster_->command_interface_configuration();
+  ASSERT_THAT(cmd_if_conf.names, IsEmpty());
+  auto state_if_conf = state_broadcaster_->state_interface_configuration();
+  ASSERT_THAT(state_if_conf.names, SizeIs(JOINT_NAMES.size() * IF_NAMES.size()));
+
   // joint state initialized
   const auto & joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_;
   ASSERT_THAT(joint_state_msg.name, SizeIs(0));
@@ -492,6 +544,12 @@ TEST_F(JointStateBroadcasterTest, TestCustomInterfaceMapping)
 
   const size_t NUM_JOINTS = JOINT_NAMES.size();
 
+  // check interface configuration
+  auto cmd_if_conf = state_broadcaster_->command_interface_configuration();
+  ASSERT_THAT(cmd_if_conf.names, IsEmpty());
+  auto state_if_conf = state_broadcaster_->state_interface_configuration();
+  ASSERT_THAT(state_if_conf.names, SizeIs(JOINT_NAMES.size() * IF_NAMES.size()));
+
   // joint state initialized
   const auto & joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_;
   ASSERT_THAT(joint_state_msg.name, ElementsAreArray(JOINT_NAMES));

joint_state_broadcaster/test/test_joint_state_broadcaster.hpp

@@ -33,10 +33,10 @@ using hardware_interface::HW_IF_VELOCITY;
 class FriendJointStateBroadcaster : public joint_state_broadcaster::JointStateBroadcaster
 {
   FRIEND_TEST(JointStateBroadcasterTest, ConfigureErrorTest);
-  FRIEND_TEST(JointStateBroadcasterTest, ActivateTest);
+  FRIEND_TEST(JointStateBroadcasterTest, ActivateEmptyTest);
   FRIEND_TEST(JointStateBroadcasterTest, ActivateTestWithoutJointsParameter);
   FRIEND_TEST(JointStateBroadcasterTest, ActivateTestWithoutInterfacesParameter);
-  FRIEND_TEST(JointStateBroadcasterTest, ActivateTestTwoJointsOneInterface);
+  FRIEND_TEST(JointStateBroadcasterTest, ActivateDeactivateTestTwoJointsOneInterface);
   FRIEND_TEST(JointStateBroadcasterTest, ActivateTestOneJointTwoInterfaces);
   FRIEND_TEST(JointStateBroadcasterTest, ActivateTestTwoJointTwoInterfacesAllMissing);
   FRIEND_TEST(JointStateBroadcasterTest, ActivateTestTwoJointTwoInterfacesOneMissing);

range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp

@@ -22,6 +22,9 @@
 
 #include "hardware_interface/loaned_state_interface.hpp"
 
+using testing::IsEmpty;
+using testing::SizeIs;
+
 void RangeSensorBroadcasterTest::SetUp()
 {
   // initialize controller
@@ -130,9 +133,15 @@ TEST_F(RangeSensorBroadcasterTest, Configure_RangeBroadcaster_Success)
   ASSERT_EQ(
     range_broadcaster_->on_configure(rclcpp_lifecycle::State()),
     controller_interface::CallbackReturn::SUCCESS);
+
+  // check interface configuration
+  auto cmd_if_conf = range_broadcaster_->command_interface_configuration();
+  ASSERT_THAT(cmd_if_conf.names, IsEmpty());
+  auto state_if_conf = range_broadcaster_->state_interface_configuration();
+  ASSERT_THAT(state_if_conf.names, SizeIs(1lu));
 }
 
-TEST_F(RangeSensorBroadcasterTest, Activate_RangeBroadcaster_Success)
+TEST_F(RangeSensorBroadcasterTest, ActivateDeactivate_RangeBroadcaster_Success)
 {
   init_broadcaster("test_range_sensor_broadcaster");
 
@@ -141,6 +150,22 @@ TEST_F(RangeSensorBroadcasterTest, Activate_RangeBroadcaster_Success)
   ASSERT_EQ(
     range_broadcaster_->on_activate(rclcpp_lifecycle::State()),
     controller_interface::CallbackReturn::SUCCESS);
+
+  // check interface configuration
+  auto cmd_if_conf = range_broadcaster_->command_interface_configuration();
+  ASSERT_THAT(cmd_if_conf.names, IsEmpty());
+  auto state_if_conf = range_broadcaster_->state_interface_configuration();
+  ASSERT_THAT(state_if_conf.names, SizeIs(1lu));
+
+  ASSERT_EQ(
+    range_broadcaster_->on_deactivate(rclcpp_lifecycle::State()),
+    controller_interface::CallbackReturn::SUCCESS);
+
+  // check interface configuration
+  cmd_if_conf = range_broadcaster_->command_interface_configuration();
+  ASSERT_THAT(cmd_if_conf.names, IsEmpty());
+  state_if_conf = range_broadcaster_->state_interface_configuration();
+  ASSERT_THAT(state_if_conf.names, SizeIs(1lu));  // did not change
 }
 
 TEST_F(RangeSensorBroadcasterTest, Update_RangeBroadcaster_Success)