[CI] test_force_torque_sensor_broadcaster regularily times out (#1639)

Author: Juliaj

force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp

@@ -76,6 +76,12 @@ controller_interface::CallbackReturn ForceTorqueSensorBroadcaster::on_configure(
   {
     force_torque_sensor_ = std::make_unique<semantic_components::ForceTorqueSensor>(
       semantic_components::ForceTorqueSensor(params_.sensor_name));
+
+    // TODO(juliaj): remove the logging after resolving
+    // https://github.com/ros-controls/ros2_controllers/issues/1574
+    RCLCPP_INFO(
+      get_node()->get_logger(), "Initialized force_torque_sensor with sensor name %s",
+      params_.sensor_name.c_str());
   }
   else
   {
@@ -85,6 +91,14 @@ controller_interface::CallbackReturn ForceTorqueSensorBroadcaster::on_configure(
       semantic_components::ForceTorqueSensor(
         force_names.x, force_names.y, force_names.z, torque_names.x, torque_names.y,
         torque_names.z));
+
+    // TODO(juliaj): remove the logging after resolving
+    // https://github.com/ros-controls/ros2_controllers/issues/1574
+    RCLCPP_INFO(
+      get_node()->get_logger(),
+      "Initialized force_torque_sensor with interface names %s, %s, %s, %s, %s, %s",
+      force_names.x.c_str(), force_names.y.c_str(), force_names.z.c_str(), torque_names.x.c_str(),
+      torque_names.y.c_str(), torque_names.z.c_str());
   }
 
   try
@@ -102,11 +116,19 @@ controller_interface::CallbackReturn ForceTorqueSensorBroadcaster::on_configure(
     return controller_interface::CallbackReturn::ERROR;
   }
 
+  // TODO(juliaj): remove the logging after resolving
+  // https://github.com/ros-controls/ros2_controllers/issues/1574
+  RCLCPP_INFO(get_node()->get_logger(), "Locking realtime publisher");
   realtime_publisher_->lock();
+  RCLCPP_INFO(get_node()->get_logger(), "Locked realtime publisher");
+
   realtime_publisher_->msg_.header.frame_id = params_.frame_id;
+
+  RCLCPP_INFO(get_node()->get_logger(), "Unlocking realtime publisher");
   realtime_publisher_->unlock();
+  RCLCPP_INFO(get_node()->get_logger(), "Unlocked realtime publisher");
 
-  RCLCPP_DEBUG(get_node()->get_logger(), "configure successful");
+  RCLCPP_INFO(get_node()->get_logger(), "Configure successful");
   return controller_interface::CallbackReturn::SUCCESS;
 }
 

force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp

@@ -48,7 +48,13 @@ void ForceTorqueSensorBroadcasterTest::SetUp()
   fts_broadcaster_ = std::make_unique<FriendForceTorqueSensorBroadcaster>();
 }
 
-void ForceTorqueSensorBroadcasterTest::TearDown() { fts_broadcaster_.reset(nullptr); }
+void ForceTorqueSensorBroadcasterTest::TearDown()
+{
+  // TODO(juliaj): remove the logging after resolving
+  // https://github.com/ros-controls/ros2_controllers/issues/1574
+  RCLCPP_INFO(fts_broadcaster_->get_node()->get_logger(), "In TearDown");
+  fts_broadcaster_.reset(nullptr);
+}
 
 void ForceTorqueSensorBroadcasterTest::SetUpFTSBroadcaster()
 {
@@ -66,6 +72,9 @@ void ForceTorqueSensorBroadcasterTest::SetUpFTSBroadcaster()
   state_ifs.emplace_back(fts_torque_z_);
 
   fts_broadcaster_->assign_interfaces({}, std::move(state_ifs));
+  // TODO(juliaj): remove the logging after resolving
+  // https://github.com/ros-controls/ros2_controllers/issues/1574
+  RCLCPP_INFO(fts_broadcaster_->get_node()->get_logger(), "FTSBroadcaster setup done");
 }
 
 void ForceTorqueSensorBroadcasterTest::subscribe_and_get_message(
@@ -180,13 +189,19 @@ TEST_F(ForceTorqueSensorBroadcasterTest, InterfaceNames_Configure_Success)
 {
   SetUpFTSBroadcaster();
 
+  // TODO(juliaj): remove the logging after resolving
+  // https://github.com/ros-controls/ros2_controllers/issues/1574
+  RCLCPP_INFO(fts_broadcaster_->get_node()->get_logger(), "Setting up interface names");
+
   // set the 'interface_names'
   fts_broadcaster_->get_node()->set_parameter({"interface_names.force.x", "fts_sensor/force.x"});
   fts_broadcaster_->get_node()->set_parameter({"interface_names.torque.z", "fts_sensor/torque.z"});
 
+  RCLCPP_INFO(fts_broadcaster_->get_node()->get_logger(), "Setting up frame_id");
   // set the 'frame_id'
   fts_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_});
 
+  RCLCPP_INFO(fts_broadcaster_->get_node()->get_logger(), "Calling on_configure");
   // configure passed
   ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS);
 }