From 6b78c98b9f34da55a8c6e6af1c5ba34d674a11ad Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Thu, 9 Jul 2020 21:27:25 -0700 Subject: [PATCH 1/4] fix sensor manager max update rate after spawning new sensors Signed-off-by: Ian Chen --- Changelog.md | 3 + gazebo/rendering/Visual.cc | 8 ++- gazebo/sensors/SensorManager.cc | 48 +++++++++++---- test/integration/sensor.cc | 105 ++++++++++++++++++++++++++++++++ 4 files changed, 148 insertions(+), 16 deletions(-) diff --git a/Changelog.md b/Changelog.md index c3f0b316c8..391fa49b85 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 #](https://github.com/osrf/gazebo/pull/) + 1. LensFlare: initialize OGRE compositors during plugin initialization * [Pull request #2762](https://github.com/osrf/gazebo/pull/2762) diff --git a/gazebo/rendering/Visual.cc b/gazebo/rendering/Visual.cc index e511ff2b64..52063c6c4f 100644 --- a/gazebo/rendering/Visual.cc +++ b/gazebo/rendering/Visual.cc @@ -817,9 +817,11 @@ void Visual::SetScale(const ignition::math::Vector3d &_scale) { this->dataPtr->sceneNode->setScale( Conversions::Convert(this->dataPtr->scale)); - } else { - gzerr << Name() << ": Size of the collision contains one or several zeros." << - " Collisions may not visualize properly." << std::endl; + } + else + { + gzerr << Name() << ": Size of the collision contains one or several zeros." + << " Collisions may not visualize properly." << std::endl; } // Scale selection object in case we have one attached. Other children were // scaled from UpdateGeomSize 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); From 8abcc2917c67951e7ef56baee9c84c28294671e0 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Thu, 9 Jul 2020 21:48:35 -0700 Subject: [PATCH 2/4] changelog Signed-off-by: Ian Chen --- Changelog.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Changelog.md b/Changelog.md index 391fa49b85..a36043c64d 100644 --- a/Changelog.md +++ b/Changelog.md @@ -3,7 +3,7 @@ ## Gazebo 9.xx.x (202x-xx-xx) 1. Fix sensor update rate throttling when new sensors are spawned - * [Pull request #](https://github.com/osrf/gazebo/pull/) + * [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) From 5227bbfba3b150635ffc9af2bcfed8e3221f8e8a Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Fri, 10 Jul 2020 12:21:20 -0700 Subject: [PATCH 3/4] revert changes in Visual.cc Signed-off-by: Ian Chen --- gazebo/rendering/Visual.cc | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/gazebo/rendering/Visual.cc b/gazebo/rendering/Visual.cc index 52063c6c4f..f1212a29a0 100644 --- a/gazebo/rendering/Visual.cc +++ b/gazebo/rendering/Visual.cc @@ -818,10 +818,9 @@ void Visual::SetScale(const ignition::math::Vector3d &_scale) this->dataPtr->sceneNode->setScale( Conversions::Convert(this->dataPtr->scale)); } - else - { - gzerr << Name() << ": Size of the collision contains one or several zeros." - << " Collisions may not visualize properly." << std::endl; + else { + gzerr << Name() << ": Size of the collision contains one or several zeros." << + " Collisions may not visualize properly." << std::endl; } // Scale selection object in case we have one attached. Other children were // scaled from UpdateGeomSize From 3122b12ee1f1aa9855cb8edf606aed59a855e965 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Fri, 10 Jul 2020 12:22:33 -0700 Subject: [PATCH 4/4] revert changes Signed-off-by: Ian Chen --- gazebo/rendering/Visual.cc | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/gazebo/rendering/Visual.cc b/gazebo/rendering/Visual.cc index f1212a29a0..e511ff2b64 100644 --- a/gazebo/rendering/Visual.cc +++ b/gazebo/rendering/Visual.cc @@ -817,8 +817,7 @@ void Visual::SetScale(const ignition::math::Vector3d &_scale) { this->dataPtr->sceneNode->setScale( Conversions::Convert(this->dataPtr->scale)); - } - else { + } else { gzerr << Name() << ": Size of the collision contains one or several zeros." << " Collisions may not visualize properly." << std::endl; }