Fix sensor update rate throttling when new sensors are spawned (#2784)

* fix sensor manager max update rate after spawning new sensors

Signed-off-by: Ian Chen <ichen@osrfoundation.org>

* changelog

Signed-off-by: Ian Chen <ichen@osrfoundation.org>

* revert changes in Visual.cc

Signed-off-by: Ian Chen <ichen@osrfoundation.org>

* revert changes

Signed-off-by: Ian Chen <ichen@osrfoundation.org>

Author: iche033

Changelog.md

@@ -2,6 +2,9 @@
 
 ## Gazebo 9.xx.x (202x-xx-xx)
 
+1. Fix sensor update rate throttling when new sensors are spawned
+    * [Pull request #2784](https://github.com/osrf/gazebo/pull/2784)
+
 1. LensFlare: initialize OGRE compositors during plugin initialization
     * [Pull request #2762](https://github.com/osrf/gazebo/pull/2762)
 

gazebo/sensors/SensorManager.cc

@@ -35,6 +35,10 @@ using namespace sensors;
 /// for timing coordination.
 boost::mutex g_sensorTimingMutex;
 
+/// \brief Flag to indicate if number of sensors has changed and that the
+/// max update rate needs to be recalculated
+bool g_sensorsDirty = true;
+
 //////////////////////////////////////////////////
 SensorManager::SensorManager()
   : initialized(false), removeAllSensors(false)
@@ -527,23 +531,34 @@ void SensorManager::SensorContainer::RunLoop()
       return;
   }
 
-  {
-    boost::recursive_mutex::scoped_lock lock(this->mutex);
 
-    // Get the minimum update rate from the sensors.
-    for (Sensor_V::iterator iter = this->sensors.begin();
-        iter != this->sensors.end() && !this->stop; ++iter)
+  auto computeMaxUpdateRate = [&]()
+  {
     {
-      GZ_ASSERT((*iter) != nullptr, "Sensor is null");
-      maxUpdateRate = std::max((*iter)->UpdateRate(), maxUpdateRate);
+      boost::recursive_mutex::scoped_lock lock(this->mutex);
+
+      if (!g_sensorsDirty)
+        return;
+
+      // Get the minimum update rate from the sensors.
+      for (Sensor_V::iterator iter = this->sensors.begin();
+          iter != this->sensors.end() && !this->stop; ++iter)
+      {
+        GZ_ASSERT((*iter) != nullptr, "Sensor is null");
+        maxUpdateRate = std::max((*iter)->UpdateRate(), maxUpdateRate);
+      }
+
+      g_sensorsDirty = false;
     }
-  }
 
-  // Calculate an appropriate sleep time.
-  if (maxUpdateRate > 0)
-    sleepTime.Set(1.0 / (maxUpdateRate));
-  else
-    sleepTime.Set(0, 1e6);
+    // Calculate an appropriate sleep time.
+    if (maxUpdateRate > 0)
+      sleepTime.Set(1.0 / (maxUpdateRate));
+    else
+      sleepTime.Set(0, 1e6);
+  };
+
+  computeMaxUpdateRate();
 
   while (!this->stop)
   {
@@ -556,6 +571,8 @@ void SensorManager::SensorContainer::RunLoop()
         return;
     }
 
+    computeMaxUpdateRate();
+
     // Get the start time of the update.
     startTime = world->SimTime();
 
@@ -653,6 +670,7 @@ void SensorManager::SensorContainer::AddSensor(SensorPtr _sensor)
   {
     boost::recursive_mutex::scoped_lock lock(this->mutex);
     this->sensors.push_back(_sensor);
+    g_sensorsDirty = true;
   }
 
   // Tell the run loop that we have received a sensor
@@ -682,6 +700,8 @@ bool SensorManager::SensorContainer::RemoveSensor(const std::string &_name)
     }
   }
 
+  g_sensorsDirty = true;
+
   return removed;
 }
 
@@ -717,6 +737,8 @@ void SensorManager::SensorContainer::RemoveSensors()
     (*iter)->Fini();
   }
 
+  g_sensorsDirty = true;
+
   this->sensors.clear();
 }
 

test/integration/sensor.cc

@@ -22,6 +22,16 @@ class SensorTest : public ServerFixture
 {
 };
 
+std::mutex g_mutex;
+unsigned int g_messageCount = 0;
+
+////////////////////////////////////////////////////////////////////////
+void SensorCallback(const ConstIMUSensorPtr &/*_msg*/)
+{
+  std::lock_guard<std::mutex> lock(g_mutex);
+  g_messageCount++;
+}
+
 /////////////////////////////////////////////////
 // This tests getting links from a model.
 TEST_F(SensorTest, GetScopedName)
@@ -48,6 +58,101 @@ TEST_F(SensorTest, FastSensor)
   // SensorManager::SensorContainer::RunLoop() is set improperly
 }
 
+/////////////////////////////////////////////////
+// Make sure sensors update rates are respected
+// Spawn two sensors, one after another, with different update rates and
+// verify the rates are correctly throttled
+TEST_F(SensorTest, MaxUpdateRate)
+{
+  Load("worlds/empty.world");
+
+  physics::WorldPtr world = physics::get_world("default");
+  ASSERT_NE(nullptr, world);
+
+  auto spawnSensorWithUpdateRate = [&](const std::string &_name,
+      const ignition::math::Pose3d &_pose, double _rate)
+  {
+    std::ostringstream newModelStr;
+    newModelStr << "<sdf version='" << SDF_VERSION << "'>"
+      << "<model name ='" << _name << "'>\n"
+      << "<static>true</static>\n"
+      << "<pose>" << _pose << "</pose>\n"
+      << "<link name ='body'>\n"
+      << "<inertial>\n"
+      << "<mass>0.1</mass>\n"
+      << "</inertial>\n"
+      << "<collision name='parent_collision'>\n"
+      << "  <pose>0 0 0.0205 0 0 0</pose>\n"
+      << "  <geometry>\n"
+      << "    <cylinder>\n"
+      << "      <radius>0.021</radius>\n"
+      << "      <length>0.029</length>\n"
+      << "    </cylinder>\n"
+      << "  </geometry>\n"
+      << "</collision>\n"
+      << "  <sensor name ='" << _name << "' type ='imu'>\n"
+      << "    <update_rate>" << _rate << "</update_rate>\n"
+      << "    <topic>" << _name << "</topic>\n"
+      << "    <imu>\n"
+      << "    </imu>\n"
+      << "  </sensor>\n"
+      << "</link>\n"
+      << "</model>\n"
+      << "</sdf>\n";
+
+    SpawnSDF(newModelStr.str());
+  };
+
+  transport::NodePtr node = transport::NodePtr(new transport::Node());
+  node->Init();
+
+  g_messageCount = 0;
+
+  // spawn first sensor with low update rate
+  spawnSensorWithUpdateRate("sensor1", ignition::math::Pose3d::Zero, 5);
+
+  transport::SubscriberPtr sub = node->Subscribe("~/sensor1/body/sensor1/imu",
+      SensorCallback);
+
+  // wait for messages
+  int sleep = 0;
+  int maxSleep = 1000;
+  double t0 = 0.0;
+  while (g_messageCount < 30 && sleep++ < maxSleep)
+  {
+    if (g_messageCount == 0)
+      t0 = world->SimTime().Double();
+    common::Time::MSleep(10);
+  }
+
+  // verify update rate by checking the time it takes to receive n msgs
+  double elapsed = world->SimTime().Double() - t0;
+  EXPECT_NEAR(6.0, elapsed, 0.5);
+
+  // disconnect first sensor
+  sub.reset();
+
+  g_messageCount = 0;
+
+  // spawn another sensor with higher update rate
+  spawnSensorWithUpdateRate("sensor2", ignition::math::Pose3d::Zero, 10);
+  sub = node->Subscribe("~/sensor2/body/sensor2/imu", SensorCallback);
+
+  // wait for more msgs
+  sleep = 0;
+  t0 = 0.0;
+  while (g_messageCount < 30 && sleep++ < maxSleep)
+  {
+    if (g_messageCount == 0)
+      t0 = world->SimTime().Double();
+    common::Time::MSleep(10);
+  }
+
+  // verify update rate by checking the time it takes to receive n msgs
+  elapsed = world->SimTime().Double() - t0;
+  EXPECT_NEAR(3.0, elapsed, 0.5);
+}
+
 int main(int argc, char **argv)
 {
   ::testing::InitGoogleTest(&argc, argv);