Increase test coverage of interface configuration getters (#856)

ros-controls · Nov 27, 2023 · b6841ea · b6841ea
1 parent fe0c91d
commit b6841ea
Show file tree

Hide file tree

Showing 10 changed files with 228 additions and 7 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
@@ -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)

diff --git a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.hpp b/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);
 };

diff --git a/forward_command_controller/test/test_forward_command_controller.cpp b/forward_command_controller/test/test_forward_command_controller.cpp
@@ -34,6 +34,8 @@
 #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
 
 using hardware_interface::LoanedCommandInterface;
+using testing::IsEmpty;
+using testing::SizeIs;
 
 namespace
 {
@@ -128,6 +130,13 @@ TEST_F(ForwardCommandControllerTest, ConfigureParamsSuccess)
   ASSERT_EQ(
     controller_->on_configure(rclcpp_lifecycle::State()),
     controller_interface::CallbackReturn::SUCCESS);
+
+  // check interface configuration
+  auto cmd_if_conf = controller_->command_interface_configuration();
+  EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL);
+  ASSERT_THAT(cmd_if_conf.names, SizeIs(2lu));
+  auto state_if_conf = controller_->state_interface_configuration();
+  ASSERT_THAT(state_if_conf.names, IsEmpty());
 }
 
 TEST_F(ForwardCommandControllerTest, ActivateWithWrongJointsNamesFails)
@@ -173,9 +182,23 @@ TEST_F(ForwardCommandControllerTest, ActivateSuccess)
   ASSERT_EQ(
     controller_->on_configure(rclcpp_lifecycle::State()),
     controller_interface::CallbackReturn::SUCCESS);
+
+  // check interface configuration
+  auto cmd_if_conf = controller_->command_interface_configuration();
+  EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL);
+  ASSERT_THAT(cmd_if_conf.names, SizeIs(joint_names_.size()));
+  auto state_if_conf = controller_->state_interface_configuration();
+  ASSERT_THAT(state_if_conf.names, IsEmpty());
+
   ASSERT_EQ(
     controller_->on_activate(rclcpp_lifecycle::State()),
     controller_interface::CallbackReturn::SUCCESS);
+
+  // check interface configuration
+  cmd_if_conf = controller_->command_interface_configuration();
+  ASSERT_THAT(cmd_if_conf.names, SizeIs(joint_names_.size()));
+  state_if_conf = controller_->state_interface_configuration();
+  ASSERT_THAT(state_if_conf.names, IsEmpty());
 }
 
 TEST_F(ForwardCommandControllerTest, CommandSuccessTest)
@@ -323,9 +346,22 @@ TEST_F(ForwardCommandControllerTest, ActivateDeactivateCommandsResetSuccess)
   auto node_state = controller_->configure();
   ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE);
 
+  // check interface configuration
+  auto cmd_if_conf = controller_->command_interface_configuration();
+  ASSERT_THAT(cmd_if_conf.names, SizeIs(joint_names_.size()));
+  EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL);
+  auto state_if_conf = controller_->state_interface_configuration();
+  ASSERT_THAT(state_if_conf.names, IsEmpty());
+
   node_state = controller_->get_node()->activate();
   ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE);
 
+  // check interface configuration
+  cmd_if_conf = controller_->command_interface_configuration();
+  ASSERT_THAT(cmd_if_conf.names, SizeIs(joint_names_.size()));
+  state_if_conf = controller_->state_interface_configuration();
+  ASSERT_THAT(state_if_conf.names, IsEmpty());
+
   auto command_msg = std::make_shared<std_msgs::msg::Float64MultiArray>();
   command_msg->data = {10.0, 20.0, 30.0};
 
@@ -344,6 +380,12 @@ TEST_F(ForwardCommandControllerTest, ActivateDeactivateCommandsResetSuccess)
   node_state = controller_->get_node()->deactivate();
   ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE);
 
+  // check interface configuration
+  cmd_if_conf = controller_->command_interface_configuration();
+  ASSERT_THAT(cmd_if_conf.names, SizeIs(joint_names_.size()));  // did not change
+  state_if_conf = controller_->state_interface_configuration();
+  ASSERT_THAT(state_if_conf.names, IsEmpty());
+
   // command ptr should be reset (nullptr) after deactivation - same check as in `update`
   ASSERT_FALSE(
     controller_->rt_command_ptr_.readFromNonRT() &&

diff --git a/forward_command_controller/test/test_multi_interface_forward_command_controller.cpp b/forward_command_controller/test/test_multi_interface_forward_command_controller.cpp
@@ -36,6 +36,8 @@
 #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
 
 using hardware_interface::LoanedCommandInterface;
+using testing::IsEmpty;
+using testing::SizeIs;
 
 namespace
 {
@@ -148,6 +150,13 @@ TEST_F(MultiInterfaceForwardCommandControllerTest, ConfigureParamsSuccess)
   ASSERT_EQ(
     controller_->on_configure(rclcpp_lifecycle::State()),
     controller_interface::CallbackReturn::SUCCESS);
+
+  // check interface configuration
+  auto cmd_if_conf = controller_->command_interface_configuration();
+  ASSERT_THAT(cmd_if_conf.names, SizeIs(3lu));
+  EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL);
+  auto state_if_conf = controller_->state_interface_configuration();
+  ASSERT_THAT(state_if_conf.names, IsEmpty());
 }
 
 TEST_F(MultiInterfaceForwardCommandControllerTest, ActivateWithWrongJointsNamesFails)
@@ -282,6 +291,13 @@ TEST_F(MultiInterfaceForwardCommandControllerTest, ActivateDeactivateCommandsRes
 {
   SetUpController(true);
 
+  // check interface configuration
+  auto cmd_if_conf = controller_->command_interface_configuration();
+  ASSERT_THAT(cmd_if_conf.names, SizeIs(3lu));
+  EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL);
+  auto state_if_conf = controller_->state_interface_configuration();
+  ASSERT_THAT(state_if_conf.names, IsEmpty());
+
   // send command
   auto command_ptr = std::make_shared<forward_command_controller::CmdType>();
   command_ptr->data = {10.0, 20.0, 30.0};
@@ -300,6 +316,13 @@ TEST_F(MultiInterfaceForwardCommandControllerTest, ActivateDeactivateCommandsRes
   auto node_state = controller_->get_node()->deactivate();
   ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE);
 
+  // check interface configuration
+  cmd_if_conf = controller_->command_interface_configuration();
+  ASSERT_THAT(cmd_if_conf.names, SizeIs(3lu));
+  EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL);
+  state_if_conf = controller_->state_interface_configuration();
+  ASSERT_THAT(state_if_conf.names, IsEmpty());
+
   // command ptr should be reset (nullptr) after deactivation - same check as in `update`
   ASSERT_FALSE(
     controller_->rt_command_ptr_.readFromNonRT() &&

diff --git a/gripper_controllers/test/test_gripper_controllers.cpp b/gripper_controllers/test/test_gripper_controllers.cpp
@@ -31,6 +31,8 @@ using hardware_interface::LoanedCommandInterface;
 using hardware_interface::LoanedStateInterface;
 using GripperCommandAction = control_msgs::action::GripperCommand;
 using GoalHandle = rclcpp_action::ServerGoalHandle<GripperCommandAction>;
+using testing::SizeIs;
+using testing::UnorderedElementsAre;
 
 template <typename T>
 void GripperControllerTest<T>::SetUpTestCase()
@@ -108,6 +110,16 @@ TYPED_TEST(GripperControllerTest, ConfigureParamsSuccess)
   ASSERT_EQ(
     this->controller_->on_configure(rclcpp_lifecycle::State()),
     controller_interface::CallbackReturn::SUCCESS);
+
+  // check interface configuration
+  auto cmd_if_conf = this->controller_->command_interface_configuration();
+  ASSERT_THAT(cmd_if_conf.names, SizeIs(1lu));
+  ASSERT_THAT(cmd_if_conf.names, UnorderedElementsAre(std::string("joint_1/") + TypeParam::value));
+  EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL);
+  auto state_if_conf = this->controller_->state_interface_configuration();
+  ASSERT_THAT(state_if_conf.names, SizeIs(2lu));
+  ASSERT_THAT(state_if_conf.names, UnorderedElementsAre("joint_1/position", "joint_1/velocity"));
+  EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL);
 }
 
 TYPED_TEST(GripperControllerTest, ActivateWithWrongJointsNamesFails)

diff --git a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp b/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)