Fix performance metrics deadlock (#2917)

* Add test for performance metrics deadlock

Load the pr2 world with --lockstep and then subscribe to
/gazebo/performance_metrics to reproduce the deadlock.

* Fix deadlock when publishing performance metrics

Currently the PublishPerformanceMetrics function is called within
SensorManager.cc by each SensorContainer with protection by the
mutex of each SensorContainer. This leads to a deadlock when a
world has sensors in more than one container (i.e. more than one
category of sensors) as described in #2902.

Since the PublishPerformanceMetrics function is not a member
function of SensorContainer and uses only global variables,
the function can be called by the SensorManager thread instead
of by each SensorContainer thread. This change will avoid the
deadlock.

Signed-off-by: Steve Peters <scpeters@openrobotics.org>

Author: scpeters

gazebo/sensors/SensorManager.cc

@@ -368,6 +368,8 @@ void SensorManager::Update(bool _force)
   // Only update if there are sensors
   if (this->sensorContainers[sensors::IMAGE]->sensors.size() > 0)
     this->sensorContainers[sensors::IMAGE]->Update(_force);
+
+  PublishPerformanceMetrics();
 }
 
 //////////////////////////////////////////////////
@@ -850,8 +852,6 @@ void SensorManager::SensorContainer::Update(bool _force)
 {
   boost::recursive_mutex::scoped_lock lock(this->mutex);
 
-  PublishPerformanceMetrics();
-
   if (this->sensors.empty())
     gzlog << "Updating a sensor container without any sensors.\n";
 

test/regression/2902_performance_metrics_deadlock.cc

@@ -0,0 +1,93 @@
+/*
+ * Copyright (C) 2021 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+
+#include "gazebo/test/ServerFixture.hh"
+
+using namespace gazebo;
+
+class Issue2902Test : public ServerFixture
+{
+  /// \brief Test with PR2 world run with --lockstep.
+  public: void PR2Test();
+
+  /// \brief Callback for performance_metrics subscriber.
+  /// \param[in] _msg Performance Metrics message.
+  public: void Callback(const ConstPerformanceMetricsPtr &_msg);
+};
+
+unsigned int g_messageCount = 0;
+
+////////////////////////////////////////////////////////////////////////
+void Issue2902Test::Callback(const ConstPerformanceMetricsPtr &/*_msg*/)
+{
+  g_messageCount++;
+}
+
+/////////////////////////////////////////////////
+TEST_F(Issue2902Test, PR2)
+{
+  PR2Test();
+}
+
+////////////////////////////////////////////////////////////////////////
+void Issue2902Test::PR2Test()
+{
+  this->LoadArgs(" --lockstep -u worlds/pr2.world");
+  physics::WorldPtr world = physics::get_world();
+  ASSERT_TRUE(world != NULL);
+
+  // Check that transport is running and there are advertised topics
+  EXPECT_FALSE(transport::is_stopped());
+  EXPECT_TRUE(transport::ConnectionManager::Instance()->IsRunning());
+  EXPECT_FALSE(transport::getAdvertisedTopics().empty());
+
+  // Check that image, laser scan and contacts topics are advertised
+  auto topics = transport::getAdvertisedTopics("gazebo.msgs.Contacts");
+  EXPECT_EQ(13u, topics.size());
+
+  topics = transport::getAdvertisedTopics("gazebo.msgs.ImageStamped");
+  EXPECT_EQ(10u, topics.size());
+
+  topics = transport::getAdvertisedTopics("gazebo.msgs.LaserScanStamped");
+  EXPECT_EQ(2u, topics.size());
+
+  gzdbg << "Step simulation before subscribing" << std::endl;
+  world->Step(1000);
+  gzdbg << " -- Completed simulation steps" << std::endl;
+
+  // Initialize transport node
+  transport::NodePtr node = transport::NodePtr(new transport::Node());
+  node->Init();
+  ASSERT_TRUE(node != NULL);
+
+  // Subscribe to performance metrics
+  std::string topicName = "/gazebo/performance_metrics";
+  auto subscriber = node->Subscribe(topicName, &Issue2902Test::Callback, this);
+
+  gzdbg << "Step simulation after subscribing (#2902 deadlock here)"
+        << std::endl;
+  world->Step(1000);
+  gzdbg << " -- Completed simulation steps" << std::endl;
+}
+
+/////////////////////////////////////////////////
+// Main
+int main(int argc, char **argv)
+{
+  ::testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}

test/regression/CMakeLists.txt

@@ -45,6 +45,7 @@ set(tests
   2505_revolute_joint_SetAxis.cc
   2728_nested_urdf.cc
   2896_gazebo_subnamespace.cc
+  2902_performance_metrics_deadlock.cc
 )
 gz_build_tests(${tests} EXTRA_LIBS gazebo_test_fixture)