diff --git a/Changelog.md b/Changelog.md index c3f0b316c8..a36043c64d 100644 --- a/Changelog.md +++ b/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) diff --git a/gazebo/sensors/SensorManager.cc b/gazebo/sensors/SensorManager.cc index 8579f612fb..345998b11d 100644 --- a/gazebo/sensors/SensorManager.cc +++ b/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(); } diff --git a/test/integration/sensor.cc b/test/integration/sensor.cc index 14785e506c..44110f0fe1 100644 --- a/test/integration/sensor.cc +++ b/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 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 << "" + << "\n" + << "true\n" + << "" << _pose << "\n" + << "\n" + << "\n" + << "0.1\n" + << "\n" + << "\n" + << " 0 0 0.0205 0 0 0\n" + << " \n" + << " \n" + << " 0.021\n" + << " 0.029\n" + << " \n" + << " \n" + << "\n" + << " \n" + << " " << _rate << "\n" + << " " << _name << "\n" + << " \n" + << " \n" + << " \n" + << "\n" + << "\n" + << "\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);