From 64eb44f6fc2920d78fb2c6c89e5ae75c2bc75151 Mon Sep 17 00:00:00 2001 From: samuel lekieffre Date: Tue, 15 Nov 2016 11:06:44 +0100 Subject: [PATCH 01/46] Make sure camera's FPS is strictly applied, should the real time factor be impacted --- gazebo/Server.cc | 5 +- gazebo/common/Events.cc | 1 + gazebo/common/Events.hh | 16 ++++ gazebo/physics/CMakeLists.txt | 1 + gazebo/physics/World.cc | 103 +++++++++++++------------- gazebo/physics/World.hh | 7 ++ gazebo/physics/WorldPrivate.hh | 3 - gazebo/rendering/RenderingIface.cc | 16 ++++ gazebo/rendering/RenderingIface.hh | 10 ++- gazebo/rendering/Scene.cc | 43 +++++++++-- gazebo/rendering/Scene.hh | 9 +++ gazebo/rendering/ScenePrivate.hh | 10 +++ gazebo/sensors/CameraSensor.cc | 102 ++++++++++++++++++++++++- gazebo/sensors/CameraSensor.hh | 18 +++++ gazebo/sensors/CameraSensorPrivate.hh | 9 +++ gazebo/sensors/Sensor.cc | 13 +++- gazebo/sensors/Sensor.hh | 13 +++- gazebo/sensors/SensorManager.cc | 65 +++++++++++++++- gazebo/sensors/SensorManager.hh | 27 ++++++- gazebo/sensors/SensorPrivate.hh | 3 - gazebo/test/ServerFixture.cc | 3 +- test/integration/camera_sensor.cc | 33 +++++---- 22 files changed, 418 insertions(+), 92 deletions(-) diff --git a/gazebo/Server.cc b/gazebo/Server.cc index 4854f23e53..e75e485ee9 100644 --- a/gazebo/Server.cc +++ b/gazebo/Server.cc @@ -55,6 +55,7 @@ #include "gazebo/physics/PresetManager.hh" #include "gazebo/physics/World.hh" #include "gazebo/physics/Base.hh" +#include "gazebo/rendering/RenderingIface.hh" #include "gazebo/Master.hh" #include "gazebo/Server.hh" @@ -569,9 +570,11 @@ void Server::Run() // Update the sensors. while (!this->dataPtr->stop && physics::worlds_running()) { + bool ret = rendering::wait_for_render_request("", 0.100); + if (ret == false) + gzerr << "time out reached!" << std::endl; this->ProcessControlMsgs(); sensors::run_once(); - common::Time::MSleep(1); } // Shutdown gazebo diff --git a/gazebo/common/Events.cc b/gazebo/common/Events.cc index 458015d3d9..40a35c6425 100644 --- a/gazebo/common/Events.cc +++ b/gazebo/common/Events.cc @@ -38,6 +38,7 @@ EventT Events::worldReset; EventT Events::timeReset; EventT Events::preRender; +EventT Events::preRenderEnded; EventT Events::render; EventT Events::postRender; diff --git a/gazebo/common/Events.hh b/gazebo/common/Events.hh index 8babd54079..4957dd7a75 100644 --- a/gazebo/common/Events.hh +++ b/gazebo/common/Events.hh @@ -254,6 +254,19 @@ namespace gazebo GAZEBO_DEPRECATED(8.0) { createSensor.Disconnect(_subscriber->Id()); } + ////////////////////////////////////////////////////////////////////////// + /// \brief End of prerender phase signal + /// \param[in] _subscriber the subscriber to this event + /// \return a connection + public: template + static ConnectionPtr ConnectPreRenderEnded(T _subscriber) + { return preRenderEnded.Connect(_subscriber); } + /// \brief Disconnect an end of prerender signal + /// \param[in] _subscriber the subscriber to this event + public: static void DisconnectPreRenderEnded(ConnectionPtr _subscriber) + GAZEBO_DEPRECATED(8.0) + { preRenderEnded.Disconnect(_subscriber->Id()); } + ////////////////////////////////////////////////////////////////////////// /// \brief Render start signal /// \param[in] _subscriber the subscriber to this event @@ -381,6 +394,9 @@ namespace gazebo /// \brief Pre-render public: static EventT preRender; + /// \brief End of Pre-render + public: static EventT preRenderEnded; + /// \brief Render public: static EventT render; diff --git a/gazebo/physics/CMakeLists.txt b/gazebo/physics/CMakeLists.txt index 1712593b33..4a31c63c03 100644 --- a/gazebo/physics/CMakeLists.txt +++ b/gazebo/physics/CMakeLists.txt @@ -161,6 +161,7 @@ target_link_libraries(gazebo_physics gazebo_util gazebo_ode gazebo_opcode + gazebo_rendering ${Boost_LIBRARIES} ) diff --git a/gazebo/physics/World.cc b/gazebo/physics/World.cc index a528c4e5fe..81ee88fc7c 100644 --- a/gazebo/physics/World.cc +++ b/gazebo/physics/World.cc @@ -83,6 +83,8 @@ #include "gazebo/physics/WorldPrivate.hh" #include "gazebo/physics/World.hh" #include "gazebo/common/SphericalCoordinates.hh" +#include "gazebo/rendering/RenderingIface.hh" +#include "gazebo/rendering/Scene.hh" #include "gazebo/physics/Collision.hh" #include "gazebo/physics/ContactManager.hh" @@ -200,12 +202,6 @@ void World::Load(sdf::ElementPtr _sdf) this->dataPtr->node = transport::NodePtr(new transport::Node()); this->dataPtr->node->Init(this->GetName()); - // pose pub for server side, mainly used for updating and timestamping - // Scene, which in turn will be used by rendering sensors. - // TODO: replace local communication with shared memory for efficiency. - this->dataPtr->poseLocalPub = - this->dataPtr->node->Advertise("~/pose/local/info", 10); - // pose pub for client with a cap on publishing rate to reduce traffic // overhead this->dataPtr->posePub = this->dataPtr->node->Advertise( @@ -628,6 +624,12 @@ bool World::SensorsInitialized() const return this->dataPtr->sensorsInitialized; } +///////////////////////////////////////////////// +void World::SetSensorWaitFunc(std::function _func) +{ + this->waitForSensors = _func; +} + ////////////////////////////////////////////////// void World::Step() { @@ -650,6 +652,10 @@ void World::Step() DIAG_TIMER_LAP("World::Step", "publishWorldStats"); + if (this->waitForSensors) + this->waitForSensors(this->dataPtr->simTime.Double(), + this->dataPtr->physicsEngine->GetMaxStepSize()); + double updatePeriod = this->dataPtr->physicsEngine->GetUpdatePeriod(); // sleep here to get the correct update rate common::Time tmpTime = common::Time::GetWallTime(); @@ -859,7 +865,6 @@ void World::Fini() this->dataPtr->lightFactoryMsgs.clear(); this->dataPtr->lightModifyMsgs.clear(); - this->dataPtr->poseLocalPub.reset(); this->dataPtr->posePub.reset(); this->dataPtr->guiPub.reset(); this->dataPtr->responsePub.reset(); @@ -2413,61 +2418,55 @@ void World::ProcessMessages() { boost::recursive_mutex::scoped_lock lock(*this->dataPtr->receiveMutex); - if ((this->dataPtr->posePub && this->dataPtr->posePub->HasConnections()) || - (this->dataPtr->poseLocalPub && - this->dataPtr->poseLocalPub->HasConnections())) - { - msgs::PosesStamped msg; + msgs::PosesStamped msg; - // Time stamp this PosesStamped message - msgs::Set(msg.mutable_time(), this->GetSimTime()); + // Time stamp this PosesStamped message + msgs::Set(msg.mutable_time(), this->GetSimTime()); - if (!this->dataPtr->publishModelPoses.empty()) + if (!this->dataPtr->publishModelPoses.empty()) + { + for (auto const &model : this->dataPtr->publishModelPoses) { - for (auto const &model : this->dataPtr->publishModelPoses) + std::list modelList; + modelList.push_back(model); + while (!modelList.empty()) { - std::list modelList; - modelList.push_back(model); - while (!modelList.empty()) + ModelPtr m = modelList.front(); + modelList.pop_front(); + msgs::Pose *poseMsg = msg.add_pose(); + + // Publish the model's relative pose + poseMsg->set_name(m->GetScopedName()); + poseMsg->set_id(m->GetId()); + msgs::Set(poseMsg, m->GetRelativePose().Ign()); + + // Publish each of the model's child links relative poses + Link_V links = m->GetLinks(); + for (auto const &link : links) { - ModelPtr m = modelList.front(); - modelList.pop_front(); - msgs::Pose *poseMsg = msg.add_pose(); - - // Publish the model's relative pose - poseMsg->set_name(m->GetScopedName()); - poseMsg->set_id(m->GetId()); - msgs::Set(poseMsg, m->GetRelativePose().Ign()); - - // Publish each of the model's child links relative poses - Link_V links = m->GetLinks(); - for (auto const &link : links) - { - poseMsg = msg.add_pose(); - poseMsg->set_name(link->GetScopedName()); - poseMsg->set_id(link->GetId()); - msgs::Set(poseMsg, link->GetRelativePose().Ign()); - } - - // add all nested models to the queue - Model_V models = m->NestedModels(); - for (auto const &n : models) - modelList.push_back(n); + poseMsg = msg.add_pose(); + poseMsg->set_name(link->GetScopedName()); + poseMsg->set_id(link->GetId()); + msgs::Set(poseMsg, link->GetRelativePose().Ign()); } - } - if (this->dataPtr->posePub && this->dataPtr->posePub->HasConnections()) - this->dataPtr->posePub->Publish(msg); + // add all nested models to the queue + Model_V models = m->NestedModels(); + for (auto const &n : models) + modelList.push_back(n); + } } - if (this->dataPtr->poseLocalPub && - this->dataPtr->poseLocalPub->HasConnections()) - { - // rendering::Scene depends on this timestamp, which is used by - // rendering sensors to time stamp their data - this->dataPtr->poseLocalPub->Publish(msg); - } + if (this->dataPtr->posePub && this->dataPtr->posePub->HasConnections()) + this->dataPtr->posePub->Publish(msg); } + + // rendering::Scene depends on this timestamp, which is used by + // rendering sensors to time stamp their data + rendering::ScenePtr scn = rendering::get_scene(this->GetName()); + if (scn) + scn->SetPoseMsg(msg); + this->dataPtr->publishModelPoses.clear(); } diff --git a/gazebo/physics/World.hh b/gazebo/physics/World.hh index 7c84661cb5..7d34436af8 100644 --- a/gazebo/physics/World.hh +++ b/gazebo/physics/World.hh @@ -437,6 +437,10 @@ namespace gazebo /// \return Unique model name. public: std::string UniqueModelName(const std::string &_name); + /// \brief Set callback 'waitForSensors' + /// \param[in] function to be called + public: void SetSensorWaitFunc(std::function _func); + /// \cond /// This is an internal function. /// \brief Get a model by id. @@ -501,6 +505,9 @@ namespace gazebo /// \brief Step callback. private: void OnStep(); + /// \brief Wait until all sensors do not need the current step any more + private: std::function waitForSensors; + /// \brief Called when a world control message is received. /// \param[in] _data The world control message. private: void OnControl(ConstWorldControlPtr &_data); diff --git a/gazebo/physics/WorldPrivate.hh b/gazebo/physics/WorldPrivate.hh index 2d74398247..2e6590a51b 100644 --- a/gazebo/physics/WorldPrivate.hh +++ b/gazebo/physics/WorldPrivate.hh @@ -112,9 +112,6 @@ namespace gazebo /// \brief Publisher for pose messages. public: transport::PublisherPtr posePub; - /// \brief Publisher for local pose messages. - public: transport::PublisherPtr poseLocalPub; - /// \brief Subscriber to world control messages. public: transport::SubscriberPtr controlSub; diff --git a/gazebo/rendering/RenderingIface.cc b/gazebo/rendering/RenderingIface.cc index f2ad67ed37..bd9b9ef58b 100644 --- a/gazebo/rendering/RenderingIface.cc +++ b/gazebo/rendering/RenderingIface.cc @@ -20,6 +20,7 @@ #include "gazebo/rendering/RenderEngine.hh" #include "gazebo/rendering/RenderingIface.hh" +#include "gazebo/rendering/Scene.hh" using namespace gazebo; @@ -107,3 +108,18 @@ void rendering::remove_scene(const std::string &_name) { rendering::RenderEngine::Instance()->RemoveScene(_name); } + +////////////////////////////////////////////////// +bool rendering::wait_for_render_request(const std::string &_name, + double _timeoutsec) +{ + ScenePtr scene = get_scene(_name); + + if (!scene) + { + common::Time::NSleep(_timeoutsec * 1e9); + return false; + } + + return scene->WaitForRenderRequest(_timeoutsec); +} diff --git a/gazebo/rendering/RenderingIface.hh b/gazebo/rendering/RenderingIface.hh index 5e86e04115..f2460a020d 100644 --- a/gazebo/rendering/RenderingIface.hh +++ b/gazebo/rendering/RenderingIface.hh @@ -41,10 +41,18 @@ namespace gazebo bool fini(); /// \brief get pointer to rendering::Scene by name. - /// \param[in] _name Name of the scene to retreive. + /// \param[in] _name Name of the scene to retrieve. GZ_RENDERING_VISIBLE rendering::ScenePtr get_scene(const std::string &_name = ""); + /// \brief wait until a render request occurs + /// \param[in] _name Name of the scene to retrieve + /// \param[in] _timeoutsec timeout expressed in seconds + /// \return true if a render request occured, false in case + /// we waited until the timeout + GZ_RENDERING_VISIBLE + bool wait_for_render_request(const std::string &_name, double _timeoutsec); + /// \brief create rendering::Scene by name. /// \param[in] _name Name of the scene to create. /// \param[in] _enableVisualizations True enables visualization diff --git a/gazebo/rendering/Scene.cc b/gazebo/rendering/Scene.cc index d78619d98c..2a67573182 100644 --- a/gazebo/rendering/Scene.cc +++ b/gazebo/rendering/Scene.cc @@ -150,12 +150,7 @@ Scene::Scene(const std::string &_name, const bool _enableVisualizations, this->dataPtr->node->Subscribe("~/light/modify", &Scene::OnLightModifyMsg, this); - if (_isServer) - { - this->dataPtr->poseSub = this->dataPtr->node->Subscribe("~/pose/local/info", - &Scene::OnPoseMsg, this); - } - else + if (!_isServer) { this->dataPtr->poseSub = this->dataPtr->node->Subscribe("~/pose/info", &Scene::OnPoseMsg, this); @@ -2760,6 +2755,42 @@ void Scene::OnPoseMsg(ConstPosesStampedPtr &_msg) } } +///////////////////////////////////////////////// +void Scene::SetPoseMsg(msgs::PosesStamped &_msg) +{ + std::lock_guard lock(this->dataPtr->poseMsgMutex); + this->dataPtr->sceneSimTimePosesReceived = + common::Time(_msg.time().sec(), _msg.time().nsec()); + + for (int i = 0; i < _msg.pose_size(); ++i) + { + PoseMsgs_M::iterator iter = + this->dataPtr->poseMsgs.find(_msg.pose(i).id()); + if (iter != this->dataPtr->poseMsgs.end()) + iter->second.CopyFrom(_msg.pose(i)); + else + this->dataPtr->poseMsgs.insert( + std::make_pair(_msg.pose(i).id(), _msg.pose(i))); + } + + std::unique_lock lck(this->dataPtr->newPoseMutex); + this->dataPtr->newPoseAvailable = true; + this->dataPtr->newPoseCondition.notify_all(); +} + +///////////////////////////////////////////////// +bool Scene::WaitForRenderRequest(double _timeoutsec) +{ + std::unique_lock lck(this->dataPtr->newPoseMutex); + bool ret = this->dataPtr->newPoseCondition.wait_for(lck, + std::chrono::duration(_timeoutsec), + [&](){return this->dataPtr->newPoseAvailable;}); + + this->dataPtr->newPoseAvailable = false; + + return ret; +} + ///////////////////////////////////////////////// void Scene::OnSkeletonPoseMsg(ConstPoseAnimationPtr &_msg) { diff --git a/gazebo/rendering/Scene.hh b/gazebo/rendering/Scene.hh index 2cce19090b..f9bee5f820 100644 --- a/gazebo/rendering/Scene.hh +++ b/gazebo/rendering/Scene.hh @@ -108,6 +108,11 @@ namespace gazebo /// \brief Process all received messages. public: void PreRender(); + /// \brief Wait until a render request occurs + /// \param[in] _timeoutsec timeout expressed in seconds + /// \return True if timeout was NOT met + public: bool WaitForRenderRequest(double _timeoutsec); + /// \brief Get the OGRE scene manager. /// \return Pointer to the Ogre SceneManager. public: Ogre::SceneManager *OgreSceneManager() const; @@ -629,6 +634,10 @@ namespace gazebo /// \param[in] _msg The message data. private: void OnPoseMsg(ConstPosesStampedPtr &_msg); + /// \brief Set new Pose message + /// \param[in] _msg The message data. + public: void SetPoseMsg(msgs::PosesStamped& _msg); + /// \brief Skeleton animation callback. /// \param[in] _msg The message data. private: void OnSkeletonPoseMsg(ConstPoseAnimationPtr &_msg); diff --git a/gazebo/rendering/ScenePrivate.hh b/gazebo/rendering/ScenePrivate.hh index ab5cfdfeb5..701f13fac4 100644 --- a/gazebo/rendering/ScenePrivate.hh +++ b/gazebo/rendering/ScenePrivate.hh @@ -22,6 +22,7 @@ #include #include #include +#include #include @@ -208,6 +209,15 @@ namespace gazebo /// \brief List of road messages to process. public: RoadMsgs_L roadMsgs; + /// \brief used to wake up upon new Pose available + public: std::condition_variable newPoseCondition; + + /// \brief Flag indicating that a new pose msg is available + public: bool newPoseAvailable; + + /// \brief Protects flag newPoseAvailable + public: std::mutex newPoseMutex; + /// \brief Mutex to lock the various message buffers. public: std::mutex *receiveMutex; diff --git a/gazebo/sensors/CameraSensor.cc b/gazebo/sensors/CameraSensor.cc index 87792c98e8..d009d66e78 100644 --- a/gazebo/sensors/CameraSensor.cc +++ b/gazebo/sensors/CameraSensor.cc @@ -29,6 +29,7 @@ #include "gazebo/msgs/msgs.hh" #include "gazebo/physics/World.hh" +#include "gazebo/physics/physics.hh" #include "gazebo/rendering/Camera.hh" #include "gazebo/rendering/RenderEngine.hh" @@ -56,6 +57,9 @@ CameraSensor::CameraSensor() this->connections.push_back( event::Events::ConnectRender( std::bind(&CameraSensor::Render, this))); + this->connections.push_back( + event::Events::ConnectPreRenderEnded( + boost::bind(&CameraSensor::PrerenderEnded, this))); } ////////////////////////////////////////////////// @@ -151,6 +155,8 @@ void CameraSensor::Init() this->Type()); this->noises[CAMERA_NOISE]->SetCamera(this->camera); } + + this->dataPtr->hasStrictFps = cameraSdf->Get("strict_rate"); } else gzerr << "No world name\n"; @@ -179,17 +185,90 @@ void CameraSensor::Fini() Sensor::Fini(); } +////////////////////////////////////////////////// +bool CameraSensor::NeedsUpdate() +{ + double simTime = this->scene->SimTime().Double(); + + if (simTime < this->lastMeasurementTime.Double()) + { + // Rendering sensors also set the lastMeasurementTime variable in Render() + // and lastUpdateTime in Sensor::Update based on Scene::SimTime() which + // could be outdated when the world is reset. In this case reset + // the variables back to 0. + gzwarn << "reset detected !" << std::endl; + this->ResetLastUpdateTime(); + return false; + } + + double dt = this->world->GetPhysicsEngine()->GetMaxStepSize(); + + // If next rendering time is not set yet + if (std::isnan(this->dataPtr->nextRenderingTime)) + { + if (this->updatePeriod == 0 + || (simTime > 0.0 && + std::abs(std::fmod(simTime, this->updatePeriod.Double())) < dt)) + { + this->dataPtr->nextRenderingTime = simTime; + return true; + } + else + { + return false; + } + } + + if (simTime > this->dataPtr->nextRenderingTime + dt) + return true; + + //gzmsg << __func__ << ": " << simTime << ", " << + // this->dataPtr->nextRenderingTime << ", " << dt << std::endl; + + // Trigger on the tick the closest from the targeted rendering time + return (ignition::math::lessOrEqual( + std::abs(simTime - this->dataPtr->nextRenderingTime), dt / 2.0)); +} + +////////////////////////////////////////////////// +void CameraSensor::Update(bool _force) +{ + if (this->IsActive() || _force) + { + if (this->UpdateImpl(_force)) + this->updated(); + } +} + +////////////////////////////////////////////////// +void CameraSensor::PrerenderEnded() +{ + if (this->camera && this->IsActive() && this->NeedsUpdate()) + { + // compute next rendering time, take care of the case where period is zero. + double dt; + if (this->updatePeriod <= 0.0) + dt = this->world->GetPhysicsEngine()->GetMaxStepSize(); + else + dt = this->updatePeriod.Double(); + this->dataPtr->nextRenderingTime += dt; + + this->dataPtr->renderNeeded = true; + this->lastMeasurementTime = this->scene->SimTime(); + } +} + ////////////////////////////////////////////////// void CameraSensor::Render() { - if (!this->camera || !this->IsActive() || !this->NeedsUpdate()) - return; + if (!this->dataPtr->renderNeeded) + return; // Update all the cameras this->camera->Render(); this->dataPtr->rendered = true; - this->lastMeasurementTime = this->scene->SimTime(); + this->dataPtr->renderNeeded = false; } ////////////////////////////////////////////////// @@ -301,3 +380,20 @@ void CameraSensor::SetRendered(const bool _value) this->dataPtr->rendered = _value; } +////////////////////////////////////////////////// +double CameraSensor::GetNextRequiredTimestamp() const +{ + if (this->dataPtr->hasStrictFps + && !ignition::math::equal(this->updatePeriod.Double(), 0.0)) + return this->dataPtr->nextRenderingTime; + else + return std::numeric_limits::quiet_NaN(); +} + +////////////////////////////////////////////////// +void CameraSensor::ResetLastUpdateTime() +{ + this->lastMeasurementTime = 0.0; + this->dataPtr->nextRenderingTime = std::numeric_limits::quiet_NaN(); +} + diff --git a/gazebo/sensors/CameraSensor.hh b/gazebo/sensors/CameraSensor.hh index 774fbd51d1..c4b70465f1 100644 --- a/gazebo/sensors/CameraSensor.hh +++ b/gazebo/sensors/CameraSensor.hh @@ -60,6 +60,21 @@ namespace gazebo /// \brief Initialize the camera public: virtual void Init(); + /// \brief reset timing related members + protected: void ResetLastUpdateTime() override; + + /// \brief Return true if the sensor needs to be updated. + /// \return True when sensor should be updated. + protected: bool NeedsUpdate() override; + + /// \brief Update the sensor. + /// \param[in] _force True to force update, false otherwise. + public: void Update(bool _force) override; + + /// \brief Return the next timestamp going to be used by the sensor + /// \return the timestamp + public: double GetNextRequiredTimestamp() const override; + /// \brief Gets the topic name of the sensor /// \return Topic name public: virtual std::string Topic() const; @@ -105,6 +120,9 @@ namespace gazebo /// \param[in] _value New rendered value. protected: void SetRendered(const bool _value); + /// \brief Handle the prerenderEnded event. + protected: void PrerenderEnded(); + /// \brief Pointer to the camera. protected: rendering::CameraPtr camera; diff --git a/gazebo/sensors/CameraSensorPrivate.hh b/gazebo/sensors/CameraSensorPrivate.hh index b427e68b5f..c707712d63 100644 --- a/gazebo/sensors/CameraSensorPrivate.hh +++ b/gazebo/sensors/CameraSensorPrivate.hh @@ -27,6 +27,15 @@ namespace gazebo { /// \brief True if the sensor was rendered. public: bool rendered = false; + + /// \brief True if the sensor needs a rendering + public: bool renderNeeded = false; + + /// \brief Timestamp of the forthcoming rendering + public: double nextRenderingTime + = std::numeric_limits::quiet_NaN(); + + public: bool hasStrictFps = true; }; } } diff --git a/gazebo/sensors/Sensor.cc b/gazebo/sensors/Sensor.cc index 78ee3b1ee2..3b79ed93ee 100644 --- a/gazebo/sensors/Sensor.cc +++ b/gazebo/sensors/Sensor.cc @@ -225,7 +225,7 @@ void Sensor::Update(const bool _force) { std::lock_guard lock(this->dataPtr->mutexLastUpdateTime); this->lastUpdateTime = simTime; - this->dataPtr->updated(); + this->updated(); } } } @@ -470,11 +470,18 @@ void Sensor::ResetLastUpdateTime() ////////////////////////////////////////////////// event::ConnectionPtr Sensor::ConnectUpdated(std::function _subscriber) { - return this->dataPtr->updated.Connect(_subscriber); + return this->updated.Connect(_subscriber); } ////////////////////////////////////////////////// void Sensor::DisconnectUpdated(event::ConnectionPtr &_c) { - this->dataPtr->updated.Disconnect(_c->Id()); + this->updated.Disconnect(_c->Id()); +} + +////////////////////////////////////////////////// +double Sensor::GetNextRequiredTimestamp() const +{ + // implementation by default: next required timestamp is ignored + return std::numeric_limits::quiet_NaN(); } diff --git a/gazebo/sensors/Sensor.hh b/gazebo/sensors/Sensor.hh index 63bb31042d..50e543c697 100644 --- a/gazebo/sensors/Sensor.hh +++ b/gazebo/sensors/Sensor.hh @@ -81,7 +81,7 @@ namespace gazebo /// \brief Update the sensor. /// \param[in] _force True to force update, false otherwise. - public: void Update(const bool _force); + public: virtual void Update(const bool _force); /// \brief Get the update rate of the sensor. /// \return _hz update rate of sensor. Returns 0 if unthrottled. @@ -171,7 +171,7 @@ namespace gazebo public: SensorCategory Category() const; /// \brief Reset the lastUpdateTime to zero. - public: void ResetLastUpdateTime(); + public: virtual void ResetLastUpdateTime(); /// \brief Get the sensor's ID. /// \return The sensor's ID. @@ -187,6 +187,10 @@ namespace gazebo /// \return The sensor's noise model for the given noise type public: NoisePtr Noise(const SensorNoiseType _type) const; + /// \brief Get the next timestamp that will be used by the sensor + /// \return the timestamp + public: virtual double GetNextRequiredTimestamp() const; + /// \brief This gets overwritten by derived sensor types. /// This function is called during Sensor::Update. /// And in turn, Sensor::Update is called by @@ -197,7 +201,7 @@ namespace gazebo /// \brief Return true if the sensor needs to be updated. /// \return True when sensor should be updated. - protected: bool NeedsUpdate(); + protected: virtual bool NeedsUpdate(); /// \brief Load a plugin for this sensor. /// \param[in] _sdf SDF parameters. @@ -247,6 +251,9 @@ namespace gazebo /// \brief Noise added to sensor data protected: std::map noises; + /// \brief Event triggered when a sensor is updated. + protected: event::EventT updated; + /// \internal /// \brief Data pointer for private data private: std::unique_ptr dataPtr; diff --git a/gazebo/sensors/SensorManager.cc b/gazebo/sensors/SensorManager.cc index 1b120993ae..a3e838d1c6 100644 --- a/gazebo/sensors/SensorManager.cc +++ b/gazebo/sensors/SensorManager.cc @@ -98,6 +98,20 @@ void SensorManager::Stop() } } +////////////////////////////////////////////////// +void SensorManager::WaitForSensors(double _clk, double _dt) +{ + double tnext = this->GetNextRequiredTimestamp(); + + while (!std::isnan(tnext) + && ignition::math::lessOrEqual(tnext - _dt / 2.0, _clk) + && physics::worlds_running()) + { + this->WaitForPrerendered(0.001); + tnext = this->GetNextRequiredTimestamp(); + } +} + ////////////////////////////////////////////////// void SensorManager::Update(bool _force) { @@ -191,6 +205,24 @@ void SensorManager::ResetLastUpdateTimes() } } +////////////////////////////////////////////////// +double SensorManager::GetNextRequiredTimestamp() +{ + double rv = std::numeric_limits::quiet_NaN(); + + // scan all sensors whose category is IMAGE + for (auto& s : this->sensorContainers[sensors::IMAGE]->sensors) + { + double candidate = s->GetNextRequiredTimestamp(); + // take the smallest valid value + if (!std::isnan(candidate) + && (std::isnan(rv) || rv > candidate)) + rv = candidate; + } + + return rv; +} + ////////////////////////////////////////////////// void SensorManager::Init() { @@ -284,6 +316,11 @@ std::string SensorManager::CreateSensor(sdf::ElementPtr _elem, sensor->Load(_worldName, _elem); this->worlds[_worldName] = physics::get_world(_worldName); + // Provide the wait function to the given world + this->worlds[_worldName]->SetSensorWaitFunc( + std::bind(&SensorManager::WaitForSensors, this, + std::placeholders::_1, std::placeholders::_2)); + // If the SensorManager has not been initialized, then it's okay to push // the sensor into one of the sensor vectors because the sensor will get // initialized in SensorManager::Init @@ -392,6 +429,16 @@ void SensorManager::RemoveSensor(const std::string &_name) } } +////////////////////////////////////////////////// +bool SensorManager::WaitForPrerendered(double _timeoutsec) +{ + if (this->sensorContainers[sensors::IMAGE]->sensors.size() > 0) + return ((ImageSensorContainer*)this->sensorContainers[sensors::IMAGE]) + ->WaitForPrerendered(_timeoutsec); + + return true; +} + ////////////////////////////////////////////////// void SensorManager::RemoveSensors() { @@ -692,8 +739,15 @@ void SensorManager::SensorContainer::RemoveSensors() ////////////////////////////////////////////////// void SensorManager::ImageSensorContainer::Update(bool _force) { + // Prerender phase event::Events::preRender(); + // Signals end of prerender phase + event::Events::preRenderEnded(); + + // Notify that prerender is over + this->conditionPrerendered.notify_all(); + // Tell all the cameras to render event::Events::render(); @@ -703,8 +757,15 @@ void SensorManager::ImageSensorContainer::Update(bool _force) SensorContainer::Update(_force); } - - +////////////////////////////////////////////////// +bool SensorManager::ImageSensorContainer::WaitForPrerendered(double _timeoutsec) +{ + std::cv_status ret; + std::mutex mtx; + std::unique_lock lck(mtx); + ret = this->conditionPrerendered.wait_for(lck, std::chrono::duration(_timeoutsec)); + return (ret == std::cv_status::no_timeout); +} ///////////////////////////////////////////////// SimTimeEventHandler::SimTimeEventHandler() diff --git a/gazebo/sensors/SensorManager.hh b/gazebo/sensors/SensorManager.hh index 78dbe9d1f6..186e621c25 100644 --- a/gazebo/sensors/SensorManager.hh +++ b/gazebo/sensors/SensorManager.hh @@ -22,6 +22,7 @@ #include #include #include +#include #include @@ -29,6 +30,7 @@ #include "gazebo/common/SingletonT.hh" #include "gazebo/common/UpdateInfo.hh" #include "gazebo/sensors/SensorTypes.hh" +#include "gazebo/sensors/Sensor.hh" #include "gazebo/util/system.hh" namespace gazebo @@ -101,6 +103,11 @@ namespace gazebo /// \param[in] _force True force update, false if not public: void Update(bool _force = false); + /// \brief Amongst all IMAGE sensors, returns the forthcoming timestamp + /// used by one (or several) sensor + /// \return the timestamp + public: double GetNextRequiredTimestamp(); + /// \brief Init all the sensors public: void Init(); @@ -165,6 +172,16 @@ namespace gazebo /// \brief Reset last update times in all sensors. public: void ResetLastUpdateTimes(); + /// \brief Block until all sensors do not need current world tick + /// \param[in] _clk simulated clock of the world + /// \param[in] _dt world time step + private: void WaitForSensors(double _clk, double _dt); + + /// \brief Wait until pre-rendering phase is over. + /// \param[in] _timeoutsec timeout expressed in seconds + /// \return True if timeout has NOT been met + private: bool WaitForPrerendered(double _timeoutsec); + /// \brief Add a new sensor to a sensor container. /// \param[in] _sensor Pointer to a sensor to add. private: void AddSensor(SensorPtr _sensor); @@ -254,11 +271,19 @@ namespace gazebo /// the SensorContainer. private: class ImageSensorContainer : public SensorContainer { + /// \brief Wait until pre-rendering phase is over. + /// \param[in] _timeoutsec timeout expressed in seconds + /// \return True if timeout has NOT been met + public: bool WaitForPrerendered(double _timeoutsec); + /// \brief The special update for image based sensors. /// \param[in] _force True to force the sensors to update, /// even if they are not active. public: virtual void Update(bool _force = false); - }; + + /// \brief used to wait for the end of prerendering + private: std::condition_variable conditionPrerendered; + }; /// \endcond /// \brief True if SensorManager::Init has been called diff --git a/gazebo/sensors/SensorPrivate.hh b/gazebo/sensors/SensorPrivate.hh index e51dc37f7f..b3cef0c7f0 100644 --- a/gazebo/sensors/SensorPrivate.hh +++ b/gazebo/sensors/SensorPrivate.hh @@ -39,9 +39,6 @@ namespace gazebo /// \brief Mutex to protect resetting lastUpdateTime. public: std::mutex mutexLastUpdateTime; - /// \brief Event triggered when a sensor is updated. - public: event::EventT updated; - /// \brief Publish sensor data. public: transport::PublisherPtr sensorPub; diff --git a/gazebo/test/ServerFixture.cc b/gazebo/test/ServerFixture.cc index d84712a03c..7a947d6276 100644 --- a/gazebo/test/ServerFixture.cc +++ b/gazebo/test/ServerFixture.cc @@ -209,7 +209,7 @@ void ServerFixture::LoadArgs(const std::string &_args) this->node = transport::NodePtr(new transport::Node()); ASSERT_NO_THROW(this->node->Init()); - this->poseSub = this->node->Subscribe("~/pose/local/info", + this->poseSub = this->node->Subscribe("~/pose/info", &ServerFixture::OnPose, this, true); this->statsSub = this->node->Subscribe("~/world_stats", &ServerFixture::OnStats, this); @@ -582,6 +582,7 @@ void ServerFixture::SpawnCamera(const std::string &_modelName, << " " << _rate << "" << " true" << " " + << " true" << " 0.78539816339744828" << " " << " " << _width << "" diff --git a/test/integration/camera_sensor.cc b/test/integration/camera_sensor.cc index 6c3986335e..4b7719041d 100644 --- a/test/integration/camera_sensor.cc +++ b/test/integration/camera_sensor.cc @@ -235,9 +235,11 @@ TEST_F(CameraSensor, CheckThrottle) // spawn sensors of various sizes to test speed std::string modelName = "camera_model"; std::string cameraName = "camera_sensor"; - unsigned int width = 320; - unsigned int height = 240; // 106 fps - double updateRate = 10; + unsigned int width = 1280; + unsigned int height = 720; + // we choose a high fps on purpose. The goal is to check the effect + // of the flag "strict_rate". + double updateRate = 500; math::Pose setPose, testPose( math::Vector3(-5, 0, 5), math::Quaternion(0, GZ_DTOR(15), 0)); SpawnCamera(modelName, cameraName, setPose.pos, @@ -251,19 +253,24 @@ TEST_F(CameraSensor, CheckThrottle) std::bind(&::OnNewCameraFrame, &imageCount, img, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4, std::placeholders::_5)); - common::Timer timer; - timer.Start(); - // time how long it takes to get 50 images @ 10Hz - int total_images = 50; + // how many images produced for 5 seconds (in simulated clock domain) + int total_images = 5 * updateRate; + physics::WorldPtr world = physics::get_world("default"); + ASSERT_TRUE(world != NULL); + double simT0 = world->GetSimTime().Double(); + // wait until we get the expected amount of images while (imageCount < total_images) - common::Time::MSleep(10); - common::Time dt = timer.GetElapsed(); - double rate = static_cast(total_images)/dt.Double(); - gzdbg << "timer [" << dt.Double() << "] seconds rate [" << rate << "] fps\n"; - EXPECT_GT(rate, 7.0); - EXPECT_LT(rate, 11.0); + common::Time::MSleep(1); + + // check that the obtained rate is the one expected + double dt = world->GetSimTime().Double() - simT0; + double rate = static_cast(total_images) / dt; + gzdbg << "timer [" << dt << "] seconds rate [" << rate << "] fps\n"; + const double tolerance = 0.02; + EXPECT_GT(rate, updateRate * (1 - tolerance)); + EXPECT_LT(rate, updateRate * (1 + tolerance)); c.reset(); delete [] img; } From c2a1f2fe56e036edec1482bfbbf84fca5c781076 Mon Sep 17 00:00:00 2001 From: samuel lekieffre Date: Fri, 18 Nov 2016 11:02:19 +0100 Subject: [PATCH 02/46] Update lessOrEqual() calls with new method name --- gazebo/sensors/CameraSensor.cc | 2 +- gazebo/sensors/SensorManager.cc | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/gazebo/sensors/CameraSensor.cc b/gazebo/sensors/CameraSensor.cc index d009d66e78..5dfb106d76 100644 --- a/gazebo/sensors/CameraSensor.cc +++ b/gazebo/sensors/CameraSensor.cc @@ -226,7 +226,7 @@ bool CameraSensor::NeedsUpdate() // this->dataPtr->nextRenderingTime << ", " << dt << std::endl; // Trigger on the tick the closest from the targeted rendering time - return (ignition::math::lessOrEqual( + return (ignition::math::lessOrNearEqual( std::abs(simTime - this->dataPtr->nextRenderingTime), dt / 2.0)); } diff --git a/gazebo/sensors/SensorManager.cc b/gazebo/sensors/SensorManager.cc index a3e838d1c6..75bb57842e 100644 --- a/gazebo/sensors/SensorManager.cc +++ b/gazebo/sensors/SensorManager.cc @@ -104,7 +104,7 @@ void SensorManager::WaitForSensors(double _clk, double _dt) double tnext = this->GetNextRequiredTimestamp(); while (!std::isnan(tnext) - && ignition::math::lessOrEqual(tnext - _dt / 2.0, _clk) + && ignition::math::lessOrNearEqual(tnext - _dt / 2.0, _clk) && physics::worlds_running()) { this->WaitForPrerendered(0.001); From f63709d264b46760a546b0d6c6379a3d4c550f20 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Tue, 21 Jan 2020 17:18:56 -0800 Subject: [PATCH 03/46] whitespace --- gazebo/rendering/RenderingIface.hh | 2 +- gazebo/sensors/SensorManager.hh | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/gazebo/rendering/RenderingIface.hh b/gazebo/rendering/RenderingIface.hh index b44a801dae..9e57309fd8 100644 --- a/gazebo/rendering/RenderingIface.hh +++ b/gazebo/rendering/RenderingIface.hh @@ -45,7 +45,7 @@ namespace gazebo /// \param[in] _name Name of the scene to retrieve. GZ_RENDERING_VISIBLE rendering::ScenePtr get_scene(const std::string &_name = ""); - + /// \brief directly provide Pose message to the corresponding scene. /// \param[in] _name Name of the scene concerned. /// \param[in] _msg message to be passed. diff --git a/gazebo/sensors/SensorManager.hh b/gazebo/sensors/SensorManager.hh index 932c6bc32a..dea9caee49 100644 --- a/gazebo/sensors/SensorManager.hh +++ b/gazebo/sensors/SensorManager.hh @@ -294,7 +294,7 @@ namespace gazebo /// \brief used to wait for the end of prerendering private: std::condition_variable conditionPrerendered; - }; + }; /// \endcond /// \brief True if SensorManager::Init has been called From c2f69da4839c42e274cce6fd090e348dcb137332 Mon Sep 17 00:00:00 2001 From: Mabel Zhang Date: Thu, 14 May 2020 02:39:25 -0400 Subject: [PATCH 04/46] Generalize from camera to sensors. Enable lines to directly set pose. Add test world file. New test is passing. Signed-off-by: Mabel Zhang --- gazebo/Server.cc | 4 +- gazebo/physics/World.cc | 12 ++--- gazebo/rendering/Scene.cc | 4 +- gazebo/rendering/Visual.hh | 2 +- gazebo/sensors/CameraSensor.cc | 4 +- gazebo/sensors/CameraSensorPrivate.hh | 2 - gazebo/sensors/Sensor.cc | 4 ++ gazebo/sensors/Sensor.hh | 5 +++ gazebo/test/ServerFixture.cc | 6 +-- gazebo/test/ServerFixture.hh | 3 +- test/integration/camera_sensor.cc | 64 ++++++++++++++++++++++++++- test/worlds/sensor_strict_rate.world | 63 ++++++++++++++++++++++++++ 12 files changed, 153 insertions(+), 20 deletions(-) create mode 100644 test/worlds/sensor_strict_rate.world diff --git a/gazebo/Server.cc b/gazebo/Server.cc index 8075da0b62..320d3339f8 100644 --- a/gazebo/Server.cc +++ b/gazebo/Server.cc @@ -587,8 +587,8 @@ void Server::Run() while (!this->dataPtr->stop) { bool ret = rendering::wait_for_render_request("", 0.100); - if (ret == false) - gzerr << "time out reached!" << std::endl; + // if (ret == false) + // gzerr << "time out reached!" << std::endl; this->ProcessControlMsgs(); diff --git a/gazebo/physics/World.cc b/gazebo/physics/World.cc index bdd356ab1a..ee9f78eb4b 100644 --- a/gazebo/physics/World.cc +++ b/gazebo/physics/World.cc @@ -2662,7 +2662,7 @@ void World::ProcessMessages() if ((this->dataPtr->posePub && this->dataPtr->posePub->HasConnections()) || // When ready to use the direct API for updating scene poses from server, // uncomment the following line: - // this->dataPtr->updateScenePoses || + this->dataPtr->updateScenePoses || (this->dataPtr->poseLocalPub && this->dataPtr->poseLocalPub->HasConnections())) { @@ -2730,11 +2730,11 @@ void World::ProcessMessages() // When ready to use the direct API for updating scene poses from server, // uncomment the following lines: - // // Execute callback to export Pose msg - // if (this->dataPtr->updateScenePoses) - // { - // this->dataPtr->updateScenePoses(this->Name(), msg); - // } + // Execute callback to export Pose msg + if (this->dataPtr->updateScenePoses) + { + this->dataPtr->updateScenePoses(this->Name(), msg); + } } this->dataPtr->publishModelPoses.clear(); diff --git a/gazebo/rendering/Scene.cc b/gazebo/rendering/Scene.cc index 5205e82b8d..35d664c2bb 100644 --- a/gazebo/rendering/Scene.cc +++ b/gazebo/rendering/Scene.cc @@ -157,15 +157,17 @@ Scene::Scene(const std::string &_name, const bool _enableVisualizations, &Scene::OnLightModifyMsg, this); this->dataPtr->isServer = _isServer; + /* if (_isServer) { this->dataPtr->poseSub = this->dataPtr->node->Subscribe("~/pose/local/info", &Scene::OnPoseMsg, this); } else + */ // When ready to use the direct API for updating scene poses from server, // uncomment the following line and delete the if and else directly above - // if (!_isServer) + if (!_isServer) { this->dataPtr->poseSub = this->dataPtr->node->Subscribe("~/pose/info", &Scene::OnPoseMsg, this); diff --git a/gazebo/rendering/Visual.hh b/gazebo/rendering/Visual.hh index 61dd52ded4..7921bf0e87 100644 --- a/gazebo/rendering/Visual.hh +++ b/gazebo/rendering/Visual.hh @@ -549,7 +549,7 @@ namespace gazebo /// \param[in] _scene Pointer to the scene. public: void SetScene(ScenePtr _scene); - /// \brief Get current. + /// \brief Get current scene. /// \return Pointer to the scene. public: ScenePtr GetScene() const; diff --git a/gazebo/sensors/CameraSensor.cc b/gazebo/sensors/CameraSensor.cc index 42804584b9..0da1cb0899 100644 --- a/gazebo/sensors/CameraSensor.cc +++ b/gazebo/sensors/CameraSensor.cc @@ -167,8 +167,6 @@ void CameraSensor::Init() this->Type()); this->noises[CAMERA_NOISE]->SetCamera(this->camera); } - - this->dataPtr->hasStrictFps = cameraSdf->Get("strict_rate"); } else gzerr << "No world name\n"; @@ -432,7 +430,7 @@ void CameraSensor::SetRendered(const bool _value) ////////////////////////////////////////////////// double CameraSensor::NextRequiredTimestamp() const { - if (this->dataPtr->hasStrictFps + if (this->useStrictRate && !ignition::math::equal(this->updatePeriod.Double(), 0.0)) return this->dataPtr->nextRenderingTime; else diff --git a/gazebo/sensors/CameraSensorPrivate.hh b/gazebo/sensors/CameraSensorPrivate.hh index dab8e205e4..9db880d57b 100644 --- a/gazebo/sensors/CameraSensorPrivate.hh +++ b/gazebo/sensors/CameraSensorPrivate.hh @@ -36,8 +36,6 @@ namespace gazebo /// \brief Timestamp of the forthcoming rendering public: double nextRenderingTime = std::numeric_limits::quiet_NaN(); - - public: bool hasStrictFps = true; }; } } diff --git a/gazebo/sensors/Sensor.cc b/gazebo/sensors/Sensor.cc index 72b4485e7e..69b1ab1de2 100644 --- a/gazebo/sensors/Sensor.cc +++ b/gazebo/sensors/Sensor.cc @@ -65,6 +65,8 @@ Sensor::Sensor(SensorCategory _cat) this->dataPtr->updateDelay = common::Time(0.0); this->updatePeriod = common::Time(0.0); + this->useStrictRate = false; + this->dataPtr->id = physics::getUniqueId(); } @@ -137,6 +139,8 @@ void Sensor::Load(const std::string &_worldName) if (this->sdf->Get("always_on")) this->SetActive(true); + this->useStrictRate = this->sdf->Get("strict_rate:value", false).first; + if (this->dataPtr->category == IMAGE) this->scene = rendering::get_scene(_worldName); diff --git a/gazebo/sensors/Sensor.hh b/gazebo/sensors/Sensor.hh index 732bce49df..30a97a07a9 100644 --- a/gazebo/sensors/Sensor.hh +++ b/gazebo/sensors/Sensor.hh @@ -235,6 +235,11 @@ namespace gazebo /// Sensor::SetUpdateRate. protected: common::Time updatePeriod; + /// \brief Whether to enforce strict sensor update rate, even if physics + /// time has to slow down to wait for sensor updates to satisfy + /// the desired rate. + protected: bool useStrictRate; + /// \brief Time of the last update. protected: common::Time lastUpdateTime; diff --git a/gazebo/test/ServerFixture.cc b/gazebo/test/ServerFixture.cc index 81593fb05a..a3e46e58b0 100644 --- a/gazebo/test/ServerFixture.cc +++ b/gazebo/test/ServerFixture.cc @@ -570,7 +570,7 @@ void ServerFixture::SpawnCamera(const std::string &_modelName, const std::string &_noiseType, double _noiseMean, double _noiseStdDev, bool _distortion, double _distortionK1, double _distortionK2, double _distortionK3, double _distortionP1, double _distortionP2, - double _cx, double _cy) + double _cx, double _cy, bool _strictRate) { msgs::Factory msg; std::ostringstream newModelStr; @@ -581,12 +581,12 @@ void ServerFixture::SpawnCamera(const std::string &_modelName, << "" << _pos << " " << _rpy << "" << "" << " " + << "' type ='camera' xmlns:strict_rate='placeholder'>" << " 1" << " " << _rate << "" + << " " << _strictRate << "" << " true" << " " - << " true" << " 0.78539816339744828" << " " << " " << _width << "" diff --git a/gazebo/test/ServerFixture.hh b/gazebo/test/ServerFixture.hh index 00d9542424..eed72da88f 100644 --- a/gazebo/test/ServerFixture.hh +++ b/gazebo/test/ServerFixture.hh @@ -267,7 +267,8 @@ namespace gazebo bool _distortion = false, double _distortionK1 = 0.0, double _distortionK2 = 0.0, double _distortionK3 = 0.0, double _distortionP1 = 0.0, double _distortionP2 = 0.0, - double _cx = 0.5, double _cy = 0.5); + double _cx = 0.5, double _cy = 0.5, + bool _strictRate = false); /// \brief Spawn a wide angle camera. /// \param[in] _modelName Name of the model. diff --git a/test/integration/camera_sensor.cc b/test/integration/camera_sensor.cc index e79de2a2fa..287b2297d2 100644 --- a/test/integration/camera_sensor.cc +++ b/test/integration/camera_sensor.cc @@ -267,6 +267,61 @@ TEST_F(CameraSensor, CheckThrottle) // spawn sensors of various sizes to test speed std::string modelName = "camera_model"; std::string cameraName = "camera_sensor"; + unsigned int width = 320; + unsigned int height = 240; // 106 fps + double updateRate = 10; + ignition::math::Pose3d setPose, testPose(ignition::math::Vector3d(-5, 0, 5), + ignition::math::Quaterniond(0, IGN_DTOR(15), 0)); + SpawnCamera(modelName, cameraName, setPose.Pos(), + setPose.Rot().Euler(), width, height, updateRate); + sensors::SensorPtr sensor = sensors::get_sensor(cameraName); + sensors::CameraSensorPtr camSensor = + std::dynamic_pointer_cast(sensor); + imageCount = 0; + img = new unsigned char[width * height*3]; + event::ConnectionPtr c = camSensor->Camera()->ConnectNewImageFrame( + std::bind(&::OnNewCameraFrame, &imageCount, img, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, + std::placeholders::_4, std::placeholders::_5)); + common::Timer timer; + timer.Start(); + + // time how long it takes to get 50 images @ 10Hz + int total_images = 50; + + while (imageCount < total_images) + common::Time::MSleep(10); + common::Time dt = timer.GetElapsed(); + double rate = static_cast(total_images)/dt.Double(); + gzdbg << "timer [" << dt.Double() << "] seconds rate [" << rate << "] fps\n"; + EXPECT_GT(rate, 7.0); + EXPECT_LT(rate, 11.0); + + c.reset(); + delete [] img; +} + +///////////////////////////////////////////////// +TEST_F(CameraSensor, CheckThrottleStrictRate) +{ + // Load sensor_strict_rate.world instead, and don't call SpawnCamera. + // That allows us to secify the custom namespace in the world file instead + // of modifying SpawnCamera() in ServerFixture.cc. + //Load("worlds/empty_test.world"); + Load("worlds/sensor_strict_rate.world"); + + // Make sure the render engine is available. + if (rendering::RenderEngine::Instance()->GetRenderPathType() == + rendering::RenderEngine::NONE) + { + gzerr << "No rendering engine, unable to run camera test\n"; + return; + } + + // TODO(mabelzhang) Read these values from the SDF. Is it possible? + // std::string modelName = "camera_model"; + std::string cameraName = "camera_sensor"; + /* unsigned int width = 1280; unsigned int height = 720; // we choose a high fps on purpose. The goal is to check the effect @@ -275,10 +330,16 @@ TEST_F(CameraSensor, CheckThrottle) ignition::math::Pose3d setPose, testPose(ignition::math::Vector3d(-5, 0, 5), ignition::math::Quaterniond(0, IGN_DTOR(15), 0)); SpawnCamera(modelName, cameraName, setPose.Pos(), - setPose.Rot().Euler(), width, height, updateRate); + setPose.Rot().Euler(), width, height, updateRate, + // use default values for distortion + "", 0.0, 0.0, false, 0.0, 0.0, 0.0, 0.0, 0.0, 0.5, 0.5, + false); + */ sensors::SensorPtr sensor = sensors::get_sensor(cameraName); sensors::CameraSensorPtr camSensor = std::dynamic_pointer_cast(sensor); + unsigned int width = camSensor->ImageWidth(); + unsigned int height = camSensor->ImageHeight(); imageCount = 0; img = new unsigned char[width * height*3]; event::ConnectionPtr c = camSensor->Camera()->ConnectNewImageFrame( @@ -289,6 +350,7 @@ TEST_F(CameraSensor, CheckThrottle) timer.Start(); // how many images produced for 5 seconds (in simulated clock domain) + double updateRate = camSensor->UpdateRate(); int total_images = 5 * updateRate; physics::WorldPtr world = physics::get_world("default"); ASSERT_TRUE(world != NULL); diff --git a/test/worlds/sensor_strict_rate.world b/test/worlds/sensor_strict_rate.world new file mode 100644 index 0000000000..4217844447 --- /dev/null +++ b/test/worlds/sensor_strict_rate.world @@ -0,0 +1,63 @@ + + + + + model://ground_plane + + + model://sun + + + + true + 0.0 0.0 0.5 0 0 0 + + + + + 0.1 0.1 0.1 + + + + + + 1.0472 + + + 1280 + 720 + + + 1 + + 500 + true + true + + + + + + 3 0 0.5 0 0 0 + + + + + 1 1 1 + + + + + + + 1 1 1 + + + + + + + + From f2e88c90ef3b3f8be512bad5249e7daf1ca7f641 Mon Sep 17 00:00:00 2001 From: Mabel Zhang Date: Thu, 14 May 2020 22:05:09 -0400 Subject: [PATCH 05/46] add regular camera to test world Signed-off-by: Mabel Zhang --- test/worlds/sensor_strict_rate.world | 16 ++++++++++++++-- 1 file changed, 14 insertions(+), 2 deletions(-) diff --git a/test/worlds/sensor_strict_rate.world b/test/worlds/sensor_strict_rate.world index 4217844447..ac7f59960f 100644 --- a/test/worlds/sensor_strict_rate.world +++ b/test/worlds/sensor_strict_rate.world @@ -19,12 +19,11 @@ + 1.0472 - 1280 720 @@ -36,6 +35,19 @@ true true + + + + 1.0472 + + 320 + 240 + + + 1 + 30 + true + From c3302f422d52649d9fb1ad3a0ed233faed170538 Mon Sep 17 00:00:00 2001 From: Mabel Zhang Date: Sun, 24 May 2020 03:27:36 -0400 Subject: [PATCH 06/46] add pendulum to have changing pose in scene Signed-off-by: Mabel Zhang --- test/worlds/sensor_strict_rate.world | 16 ++++++++++++---- 1 file changed, 12 insertions(+), 4 deletions(-) diff --git a/test/worlds/sensor_strict_rate.world b/test/worlds/sensor_strict_rate.world index ac7f59960f..47639f2759 100644 --- a/test/worlds/sensor_strict_rate.world +++ b/test/worlds/sensor_strict_rate.world @@ -10,12 +10,12 @@ true - 0.0 0.0 0.5 0 0 0 + -10.0 0.0 0.5 0 0 0 - 0.1 0.1 0.1 + 1 1 1 @@ -52,12 +52,12 @@ - 3 0 0.5 0 0 0 + 13 0 0.5 0 0 0 - 1 1 1 + 5 5 5 @@ -71,5 +71,13 @@ + + + active_pendulum + model://double_pendulum_with_base + 2 0 0 0 0 0 + 0.5 0.5 0.5 + + From b691c2d64da2e24777801279db51d77aae0d9881 Mon Sep 17 00:00:00 2001 From: Mabel Zhang Date: Tue, 2 Jun 2020 04:50:56 -0400 Subject: [PATCH 07/46] copy CameraSensor functions to GpuRaySensor. GpuRaySensor tests are failing Signed-off-by: Mabel Zhang --- gazebo/sensors/CameraSensor.cc | 130 +++++++++++------- gazebo/sensors/GpuRaySensor.cc | 124 +++++++++++++++-- gazebo/sensors/GpuRaySensor.hh | 3 + gazebo/sensors/GpuRaySensorPrivate.hh | 8 ++ gazebo/sensors/Sensor.cc | 23 ++++ gazebo/sensors/Sensor.hh | 4 + gazebo/sensors/SensorManager.cc | 7 +- test/integration/camera_sensor.cc | 5 +- test/integration/gpu_laser.cc | 56 ++++++++ ...ct_rate.world => camera_strict_rate.world} | 0 10 files changed, 297 insertions(+), 63 deletions(-) rename test/worlds/{sensor_strict_rate.world => camera_strict_rate.world} (100%) diff --git a/gazebo/sensors/CameraSensor.cc b/gazebo/sensors/CameraSensor.cc index 0da1cb0899..75f48e2e0f 100644 --- a/gazebo/sensors/CameraSensor.cc +++ b/gazebo/sensors/CameraSensor.cc @@ -26,8 +26,8 @@ #include "gazebo/msgs/msgs.hh" -#include "gazebo/physics/World.hh" #include "gazebo/physics/physics.hh" +#include "gazebo/physics/World.hh" #include "gazebo/rendering/Camera.hh" #include "gazebo/rendering/RenderEngine.hh" @@ -199,7 +199,7 @@ void CameraSensor::Fini() void CameraSensor::SetActive(bool _value) { // If this sensor is reactivated - if (_value && !this->IsActive()) + if (this->useStrictRate && _value && !this->IsActive()) { // the next rendering time must be reset to ensure it is properly // computed by CameraSensor::NeedsUpdate. @@ -211,52 +211,65 @@ void CameraSensor::SetActive(bool _value) ////////////////////////////////////////////////// bool CameraSensor::NeedsUpdate() { - double simTime = this->scene->SimTime().Double(); - - if (simTime < this->lastMeasurementTime.Double()) + if (this->useStrictRate) { - // Rendering sensors also set the lastMeasurementTime variable in Render() - // and lastUpdateTime in Sensor::Update based on Scene::SimTime() which - // could be outdated when the world is reset. In this case reset - // the variables back to 0. - gzwarn << "reset detected !" << std::endl; - this->ResetLastUpdateTime(); - return false; - } - - double dt = this->world->Physics()->GetMaxStepSize(); - - // If next rendering time is not set yet - if (std::isnan(this->dataPtr->nextRenderingTime)) - { - if (this->updatePeriod == 0 - || (simTime > 0.0 && - std::abs(std::fmod(simTime, this->updatePeriod.Double())) < dt)) + double simTime = this->scene->SimTime().Double(); + + if (simTime < this->lastMeasurementTime.Double()) { - this->dataPtr->nextRenderingTime = simTime; - return true; + // Rendering sensors also set the lastMeasurementTime variable in Render() + // and lastUpdateTime in Sensor::Update based on Scene::SimTime() which + // could be outdated when the world is reset. In this case reset + // the variables back to 0. + this->ResetLastUpdateTime(); + return false; } - else + + double dt = this->world->Physics()->GetMaxStepSize(); + + // If next rendering time is not set yet + if (std::isnan(this->dataPtr->nextRenderingTime)) { - return false; + if (this->updatePeriod == 0 + || (simTime > 0.0 && + std::abs(std::fmod(simTime, this->updatePeriod.Double())) < dt)) + { + this->dataPtr->nextRenderingTime = simTime; + return true; + } + else + { + return false; + } } + + if (simTime > this->dataPtr->nextRenderingTime + dt) + return true; + + // Trigger on the tick the closest from the targeted rendering time + return (ignition::math::lessOrNearEqual( + std::abs(simTime - this->dataPtr->nextRenderingTime), dt / 2.0)); + } + else + { + return Sensor::NeedsUpdate(); } - - if (simTime > this->dataPtr->nextRenderingTime + dt) - return true; - - // Trigger on the tick the closest from the targeted rendering time - return (ignition::math::lessOrNearEqual( - std::abs(simTime - this->dataPtr->nextRenderingTime), dt / 2.0)); } ////////////////////////////////////////////////// void CameraSensor::Update(bool _force) { - if (this->IsActive() || _force) + if (this->useStrictRate) + { + if (this->IsActive() || _force) + { + if (this->UpdateImpl(_force)) + this->updated(); + } + } + else { - if (this->UpdateImpl(_force)) - this->updated(); + Sensor::Update(_force); } } @@ -281,14 +294,28 @@ void CameraSensor::PrerenderEnded() ////////////////////////////////////////////////// void CameraSensor::Render() { - if (!this->dataPtr->renderNeeded) + if (this->useStrictRate) + { + if (!this->dataPtr->renderNeeded) + return; + + // Update all the cameras + this->camera->Render(); + + this->dataPtr->rendered = true; + this->dataPtr->renderNeeded = false; + } + else + { + if (!this->camera || !this->IsActive() || !this->NeedsUpdate()) return; - // Update all the cameras - this->camera->Render(); - - this->dataPtr->rendered = true; - this->dataPtr->renderNeeded = false; + // Update all the cameras + this->camera->Render(); + + this->dataPtr->rendered = true; + this->lastMeasurementTime = this->scene->SimTime(); + } } ////////////////////////////////////////////////// @@ -430,17 +457,24 @@ void CameraSensor::SetRendered(const bool _value) ////////////////////////////////////////////////// double CameraSensor::NextRequiredTimestamp() const { - if (this->useStrictRate - && !ignition::math::equal(this->updatePeriod.Double(), 0.0)) - return this->dataPtr->nextRenderingTime; + if (this->useStrictRate) + { + if (!ignition::math::equal(this->updatePeriod.Double(), 0.0)) + return this->dataPtr->nextRenderingTime; + else + return std::numeric_limits::quiet_NaN(); + } else - return std::numeric_limits::quiet_NaN(); + { + return Sensor::NextRequiredTimestamp(); + } } ////////////////////////////////////////////////// void CameraSensor::ResetLastUpdateTime() { - this->lastMeasurementTime = 0.0; - this->dataPtr->nextRenderingTime = std::numeric_limits::quiet_NaN(); + Sensor::ResetLastUpdateTime(); + if (this->useStrictRate) + this->dataPtr->nextRenderingTime = std::numeric_limits::quiet_NaN(); } diff --git a/gazebo/sensors/GpuRaySensor.cc b/gazebo/sensors/GpuRaySensor.cc index 82f50320b2..3c302328c2 100644 --- a/gazebo/sensors/GpuRaySensor.cc +++ b/gazebo/sensors/GpuRaySensor.cc @@ -21,6 +21,7 @@ #include "gazebo/physics/World.hh" #include "gazebo/physics/Entity.hh" #include "gazebo/physics/Model.hh" +#include "gazebo/physics/physics.hh" #include "gazebo/common/Exception.hh" #include "gazebo/common/Events.hh" @@ -57,6 +58,9 @@ GpuRaySensor::GpuRaySensor() this->connections.push_back( event::Events::ConnectRender( std::bind(&GpuRaySensor::Render, this))); + this->connections.push_back( + event::Events::ConnectPreRenderEnded( + boost::bind(&GpuRaySensor::PrerenderEnded, this))); } ////////////////////////////////////////////////// @@ -346,19 +350,79 @@ void GpuRaySensor::Fini() ////////////////////////////////////////////////// void GpuRaySensor::SetActive(bool _value) { + // If this sensor is reactivated + if (this->useStrictRate && _value && !this->IsActive()) + { + // the next rendering time must be reset to ensure it is properly + // computed by GpuRaySensor::NeedsUpdate. + this->dataPtr->nextRenderingTime = std::numeric_limits::quiet_NaN(); + } Sensor::SetActive(_value); } ////////////////////////////////////////////////// bool GpuRaySensor::NeedsUpdate() { - return Sensor::NeedsUpdate(); + if (this->useStrictRate) + { + double simTime = this->scene->SimTime().Double(); + + if (simTime < this->lastMeasurementTime.Double()) + { + // Rendering sensors also set the lastMeasurementTime variable in Render() + // and lastUpdateTime in Sensor::Update based on Scene::SimTime() which + // could be outdated when the world is reset. In this case reset + // the variables back to 0. + this->ResetLastUpdateTime(); + return false; + } + + double dt = this->world->Physics()->GetMaxStepSize(); + + // If next rendering time is not set yet + if (std::isnan(this->dataPtr->nextRenderingTime)) + { + if (this->updatePeriod == 0 + || (simTime > 0.0 && + std::abs(std::fmod(simTime, this->updatePeriod.Double())) < dt)) + { + this->dataPtr->nextRenderingTime = simTime; + return true; + } + else + { + return false; + } + } + + if (simTime > this->dataPtr->nextRenderingTime + dt) + return true; + + // Trigger on the tick the closest from the targeted rendering time + return (ignition::math::lessOrNearEqual( + std::abs(simTime - this->dataPtr->nextRenderingTime), dt / 2.0)); + } + else + { + return Sensor::NeedsUpdate(); + } } ////////////////////////////////////////////////// void GpuRaySensor::Update(bool _force) { - Sensor::Update(_force); + if (this->useStrictRate) + { + if (this->IsActive() || _force) + { + if (this->UpdateImpl(_force)) + this->updated(); + } + } + else + { + Sensor::Update(_force); + } } ////////////////////////////////////////////////// @@ -583,15 +647,45 @@ int GpuRaySensor::Fiducial(const unsigned int /*_index*/) const } ////////////////////////////////////////////////// -void GpuRaySensor::Render() +void GpuRaySensor::PrerenderEnded() { - if (!this->dataPtr->laserCam || !this->IsActive() || !this->NeedsUpdate()) - return; + if (this->dataPtr->laserCam && this->IsActive() && this->NeedsUpdate()) + { + // compute next rendering time, take care of the case where period is zero. + double dt; + if (this->updatePeriod <= 0.0) + dt = this->world->Physics()->GetMaxStepSize(); + else + dt = this->updatePeriod.Double(); + this->dataPtr->nextRenderingTime += dt; - this->lastMeasurementTime = this->scene->SimTime(); + this->dataPtr->renderNeeded = true; + this->lastMeasurementTime = this->scene->SimTime(); + } +} - this->dataPtr->laserCam->Render(); - this->dataPtr->rendered = true; +////////////////////////////////////////////////// +void GpuRaySensor::Render() +{ + if (this->useStrictRate) + { + if (!this->dataPtr->renderNeeded) + return; + + this->dataPtr->laserCam->Render(); + this->dataPtr->rendered = true; + this->dataPtr->renderNeeded = false; + } + else + { + if (!this->dataPtr->laserCam || !this->IsActive() || !this->NeedsUpdate()) + return; + + this->lastMeasurementTime = this->scene->SimTime(); + + this->dataPtr->laserCam->Render(); + this->dataPtr->rendered = true; + } } ////////////////////////////////////////////////// @@ -692,11 +786,23 @@ rendering::GpuLaserPtr GpuRaySensor::LaserCamera() const ////////////////////////////////////////////////// double GpuRaySensor::NextRequiredTimestamp() const { - return Sensor::NextRequiredTimestamp(); + if (this->useStrictRate) + { + if (!ignition::math::equal(this->updatePeriod.Double(), 0.0)) + return this->dataPtr->nextRenderingTime; + else + return std::numeric_limits::quiet_NaN(); + } + else + { + return Sensor::NextRequiredTimestamp(); + } } ////////////////////////////////////////////////// void GpuRaySensor::ResetLastUpdateTime() { Sensor::ResetLastUpdateTime(); + if (this->useStrictRate) + this->dataPtr->nextRenderingTime = std::numeric_limits::quiet_NaN(); } diff --git a/gazebo/sensors/GpuRaySensor.hh b/gazebo/sensors/GpuRaySensor.hh index 36167127fb..715028df86 100644 --- a/gazebo/sensors/GpuRaySensor.hh +++ b/gazebo/sensors/GpuRaySensor.hh @@ -270,6 +270,9 @@ namespace gazebo /// brief Render the camera. private: void Render(); + /// \brief Handle the prerenderEnded event. + private: void PrerenderEnded(); + /// \internal /// \brief Private data pointer. private: std::unique_ptr dataPtr; diff --git a/gazebo/sensors/GpuRaySensorPrivate.hh b/gazebo/sensors/GpuRaySensorPrivate.hh index dcb0b4cb71..9dc5f28faf 100644 --- a/gazebo/sensors/GpuRaySensorPrivate.hh +++ b/gazebo/sensors/GpuRaySensorPrivate.hh @@ -17,6 +17,7 @@ #ifndef _GAZEBO_SENSORS_GPURAYENSOR_PRIVATE_HH_ #define _GAZEBO_SENSORS_GPURAYENSOR_PRIVATE_HH_ +#include #include #include @@ -86,6 +87,13 @@ namespace gazebo /// \brief True if the sensor was rendered. public: bool rendered; + + /// \brief True if the sensor needs a rendering + public: bool renderNeeded = false; + + /// \brief Timestamp of the forthcoming rendering + public: double nextRenderingTime + = std::numeric_limits::quiet_NaN(); }; } } diff --git a/gazebo/sensors/Sensor.cc b/gazebo/sensors/Sensor.cc index 69b1ab1de2..a77979b6d0 100644 --- a/gazebo/sensors/Sensor.cc +++ b/gazebo/sensors/Sensor.cc @@ -142,7 +142,14 @@ void Sensor::Load(const std::string &_worldName) this->useStrictRate = this->sdf->Get("strict_rate:value", false).first; if (this->dataPtr->category == IMAGE) + { this->scene = rendering::get_scene(_worldName); + // TODO(mabelzhang) Decide whether to have this in Sensor or individually + // in each of the rendering sensors + // this->connections.push_back( + // event::Events::ConnectPreRenderEnded( + // boost::bind(&Sensor::PrerenderEnded, this))); + } // loaded, but not updated this->lastUpdateTime = common::Time(0.0); @@ -273,6 +280,16 @@ void Sensor::Update(const bool _force) } } +////////////////////////////////////////////////// +// TODO(mabelzhang) To have this here for rendering sensors, this needs to +// be virtual and be implemented by CameraSensor, MultiCameraSensor, and +// GpuRaySensor. +/* +void CameraSensor::PrerenderEnded() +{ +} +*/ + ////////////////////////////////////////////////// void Sensor::Fini() { @@ -521,3 +538,9 @@ double Sensor::NextRequiredTimestamp() const // implementation by default: next required timestamp is ignored return std::numeric_limits::quiet_NaN(); } + +////////////////////////////////////////////////// +bool Sensor::StrictRate() const +{ + return this->useStrictRate; +} diff --git a/gazebo/sensors/Sensor.hh b/gazebo/sensors/Sensor.hh index 30a97a07a9..da72331950 100644 --- a/gazebo/sensors/Sensor.hh +++ b/gazebo/sensors/Sensor.hh @@ -185,6 +185,10 @@ namespace gazebo /// \return the timestamp public: virtual double NextRequiredTimestamp() const; + /// \brief Returns true if the sensor is to follow strict update rate + /// \return True when sensor should follow strict update rate + public: bool StrictRate() const; + /// \brief This gets overwritten by derived sensor types. /// This function is called during Sensor::Update. /// And in turn, Sensor::Update is called by diff --git a/gazebo/sensors/SensorManager.cc b/gazebo/sensors/SensorManager.cc index e02ba44f12..dc18cdc40a 100644 --- a/gazebo/sensors/SensorManager.cc +++ b/gazebo/sensors/SensorManager.cc @@ -334,9 +334,10 @@ std::string SensorManager::CreateSensor(sdf::ElementPtr _elem, this->worlds[_worldName] = physics::get_world(_worldName); // Provide the wait function to the given world - this->worlds[_worldName]->SetSensorWaitFunc( - std::bind(&SensorManager::WaitForSensors, this, - std::placeholders::_1, std::placeholders::_2)); + if (sensor->StrictRate()) + this->worlds[_worldName]->SetSensorWaitFunc( + std::bind(&SensorManager::WaitForSensors, this, + std::placeholders::_1, std::placeholders::_2)); // If the SensorManager has not been initialized, then it's okay to push // the sensor into one of the sensor vectors because the sensor will get diff --git a/test/integration/camera_sensor.cc b/test/integration/camera_sensor.cc index 287b2297d2..0e2039428a 100644 --- a/test/integration/camera_sensor.cc +++ b/test/integration/camera_sensor.cc @@ -304,11 +304,11 @@ TEST_F(CameraSensor, CheckThrottle) ///////////////////////////////////////////////// TEST_F(CameraSensor, CheckThrottleStrictRate) { - // Load sensor_strict_rate.world instead, and don't call SpawnCamera. + // Load camera_strict_rate.world instead, and don't call SpawnCamera. // That allows us to secify the custom namespace in the world file instead // of modifying SpawnCamera() in ServerFixture.cc. //Load("worlds/empty_test.world"); - Load("worlds/sensor_strict_rate.world"); + Load("worlds/camera_strict_rate.world"); // Make sure the render engine is available. if (rendering::RenderEngine::Instance()->GetRenderPathType() == @@ -318,7 +318,6 @@ TEST_F(CameraSensor, CheckThrottleStrictRate) return; } - // TODO(mabelzhang) Read these values from the SDF. Is it possible? // std::string modelName = "camera_model"; std::string cameraName = "camera_sensor"; /* diff --git a/test/integration/gpu_laser.cc b/test/integration/gpu_laser.cc index 0c4a1bf139..e353348fb9 100644 --- a/test/integration/gpu_laser.cc +++ b/test/integration/gpu_laser.cc @@ -16,6 +16,7 @@ */ #include +#include "gazebo/common/Timer.hh" #include "gazebo/test/ServerFixture.hh" #include "gazebo/sensors/sensors.hh" @@ -610,6 +611,61 @@ TEST_F(GPURaySensorTest, LaserScanResolution) delete [] scan; } +TEST_F(GPURaySensorTest, StrictUpdateRate) +{ + Load("worlds/gpu_ray_strict_rate.world"); + + // Make sure the render engine is available. + if (rendering::RenderEngine::Instance()->GetRenderPathType() == + rendering::RenderEngine::NONE) + { + gzerr << "No rendering engine, unable to run camera test\n"; + return; + } + + std::string sensorName = "gpu_ray_sensor"; + sensors::SensorPtr sensor = sensors::get_sensor(sensorName); + sensors::GpuRaySensorPtr raySensor = + std::dynamic_pointer_cast(sensor); + EXPECT_TRUE(raySensor != nullptr); + raySensor->SetActive(true); + + float *scan = new float[raySensor->RangeCount() + * raySensor->VerticalRangeCount() * 3]; + int scanCount = 0; + event::ConnectionPtr c = + raySensor->ConnectNewLaserFrame( + std::bind(&::OnNewLaserFrame, &scanCount, scan, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, + std::placeholders::_4, std::placeholders::_5)); + + // wait for a few laser scans + common::Timer timer; + timer.Start(); + + // how many scans produced for 5 seconds (in simulated clock domain) + double updateRate = raySensor->UpdateRate(); + int total_scans = 5 * updateRate; + physics::WorldPtr world = physics::get_world("default"); + ASSERT_TRUE(world != NULL); + double simT0 = world->SimTime().Double(); + + while (scanCount < total_scans) + common::Time::MSleep(1); + + // check that the obtained rate is the one expected + double dt = world->SimTime().Double() - simT0; + double rate = static_cast(total_scans) / dt; + gzdbg << "timer [" << dt << "] seconds rate [" << rate << "] fps\n"; + const double tolerance = 0.02; + EXPECT_GT(rate, updateRate * (1 - tolerance)); + EXPECT_LT(rate, updateRate * (1 + tolerance)); + + c.reset(); + + delete [] scan; +} + int main(int argc, char **argv) { ::testing::InitGoogleTest(&argc, argv); diff --git a/test/worlds/sensor_strict_rate.world b/test/worlds/camera_strict_rate.world similarity index 100% rename from test/worlds/sensor_strict_rate.world rename to test/worlds/camera_strict_rate.world From d80846d9f68d20003f89b3ff251877e5cc0f4aa0 Mon Sep 17 00:00:00 2001 From: Mabel Zhang Date: Sun, 24 May 2020 03:27:36 -0400 Subject: [PATCH 08/46] add pendulum to have changing pose in scene Signed-off-by: Mabel Zhang --- test/worlds/sensor_strict_rate.world | 16 ++++++++++++---- 1 file changed, 12 insertions(+), 4 deletions(-) diff --git a/test/worlds/sensor_strict_rate.world b/test/worlds/sensor_strict_rate.world index ac7f59960f..47639f2759 100644 --- a/test/worlds/sensor_strict_rate.world +++ b/test/worlds/sensor_strict_rate.world @@ -10,12 +10,12 @@ true - 0.0 0.0 0.5 0 0 0 + -10.0 0.0 0.5 0 0 0 - 0.1 0.1 0.1 + 1 1 1 @@ -52,12 +52,12 @@ - 3 0 0.5 0 0 0 + 13 0 0.5 0 0 0 - 1 1 1 + 5 5 5 @@ -71,5 +71,13 @@ + + + active_pendulum + model://double_pendulum_with_base + 2 0 0 0 0 0 + 0.5 0.5 0.5 + + From 760636ec5c3dd14351565c21c40faa74afe7d30d Mon Sep 17 00:00:00 2001 From: Mabel Zhang Date: Wed, 3 Jun 2020 04:57:52 -0400 Subject: [PATCH 09/46] fix test failures caused by backward compatibility if-stmts Signed-off-by: Mabel Zhang --- gazebo/sensors/CameraSensor.cc | 11 ++++++++--- gazebo/sensors/GpuRaySensor.cc | 10 +++++++--- test/integration/camera_sensor.cc | 8 +++++--- 3 files changed, 20 insertions(+), 9 deletions(-) diff --git a/gazebo/sensors/CameraSensor.cc b/gazebo/sensors/CameraSensor.cc index 75f48e2e0f..72cf980847 100644 --- a/gazebo/sensors/CameraSensor.cc +++ b/gazebo/sensors/CameraSensor.cc @@ -55,9 +55,6 @@ CameraSensor::CameraSensor() this->connections.push_back( event::Events::ConnectRender( std::bind(&CameraSensor::Render, this))); - this->connections.push_back( - event::Events::ConnectPreRenderEnded( - boost::bind(&CameraSensor::PrerenderEnded, this))); } ////////////////////////////////////////////////// @@ -96,6 +93,14 @@ std::string CameraSensor::TopicIgn() const void CameraSensor::Load(const std::string &_worldName) { Sensor::Load(_worldName); + // strict_rate parameter is parsed in Sensor::Load() + if (this->useStrictRate) + { + this->connections.push_back( + event::Events::ConnectPreRenderEnded( + boost::bind(&CameraSensor::PrerenderEnded, this))); + } + this->imagePub = this->node->Advertise(this->Topic(), 50); ignition::transport::AdvertiseMessageOptions opts; diff --git a/gazebo/sensors/GpuRaySensor.cc b/gazebo/sensors/GpuRaySensor.cc index 3c302328c2..cace7b8352 100644 --- a/gazebo/sensors/GpuRaySensor.cc +++ b/gazebo/sensors/GpuRaySensor.cc @@ -58,9 +58,6 @@ GpuRaySensor::GpuRaySensor() this->connections.push_back( event::Events::ConnectRender( std::bind(&GpuRaySensor::Render, this))); - this->connections.push_back( - event::Events::ConnectPreRenderEnded( - boost::bind(&GpuRaySensor::PrerenderEnded, this))); } ////////////////////////////////////////////////// @@ -83,6 +80,13 @@ std::string GpuRaySensor::Topic() const void GpuRaySensor::Load(const std::string &_worldName, sdf::ElementPtr _sdf) { Sensor::Load(_worldName, _sdf); + // strict_rate parameter is parsed in Sensor::Load() + if (this->useStrictRate) + { + this->connections.push_back( + event::Events::ConnectPreRenderEnded( + boost::bind(&GpuRaySensor::PrerenderEnded, this))); + } } ////////////////////////////////////////////////// diff --git a/test/integration/camera_sensor.cc b/test/integration/camera_sensor.cc index 0e2039428a..f6c5b5382a 100644 --- a/test/integration/camera_sensor.cc +++ b/test/integration/camera_sensor.cc @@ -353,16 +353,18 @@ TEST_F(CameraSensor, CheckThrottleStrictRate) int total_images = 5 * updateRate; physics::WorldPtr world = physics::get_world("default"); ASSERT_TRUE(world != NULL); - double simT0 = world->SimTime().Double(); + // Sleep until all expected images arrive + double simT0 = world->SimTime().Double(); while (imageCount < total_images) common::Time::MSleep(1); + double dt = world->SimTime().Double() - simT0; // check that the obtained rate is the one expected - double dt = world->SimTime().Double() - simT0; double rate = static_cast(total_images) / dt; gzdbg << "timer [" << dt << "] seconds rate [" << rate << "] fps\n"; - const double tolerance = 0.02; + // Set a little higher than we actually want (2%), so that test isn't flakey + const double tolerance = 0.06; EXPECT_GT(rate, updateRate * (1 - tolerance)); EXPECT_LT(rate, updateRate * (1 + tolerance)); c.reset(); From ead5bd074834188317a80663ec38a4e3993161ef Mon Sep 17 00:00:00 2001 From: Mabel Zhang Date: Mon, 8 Jun 2020 14:11:00 -0400 Subject: [PATCH 10/46] SDF file for GpuRaySensor lockstep test Signed-off-by: Mabel Zhang --- test/worlds/gpu_ray_strict_rate.world | 85 +++++++++++++++++++++++++++ 1 file changed, 85 insertions(+) create mode 100644 test/worlds/gpu_ray_strict_rate.world diff --git a/test/worlds/gpu_ray_strict_rate.world b/test/worlds/gpu_ray_strict_rate.world new file mode 100644 index 0000000000..6189aba0ee --- /dev/null +++ b/test/worlds/gpu_ray_strict_rate.world @@ -0,0 +1,85 @@ + + + + + model://ground_plane + + + model://sun + + + + true + 0 0 0.035 0. 0 0 + + + 0.1 + + + + + model://hokuyo/meshes/hokuyo.dae + + + + + 0.1 0 0.0175 0 -0 0 + + + + 640 + 1 + -1.396263 + 1.396263 + + + 480 + 1 + -1.396263 + 1.396263 + + + + 0.08 + 10.0 + 0.01 + + + 1 + 500 + true + false + + + + + + 13 0 0.5 0 0 0 + + + + + 5 5 5 + + + + + + + 1 1 1 + + + + + + + + + active_pendulum + model://double_pendulum_with_base + 2 0 0 0 0 0 + 0.5 0.5 0.5 + + + + From d5bd097c18816e1d09ed9dbc8a33e012d08d7240 Mon Sep 17 00:00:00 2001 From: Mabel Zhang Date: Mon, 8 Jun 2020 20:24:19 -0400 Subject: [PATCH 11/46] style Signed-off-by: Mabel Zhang --- gazebo/Server.cc | 3 ++- test/integration/camera_sensor.cc | 2 +- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/gazebo/Server.cc b/gazebo/Server.cc index 320d3339f8..8e14768e96 100644 --- a/gazebo/Server.cc +++ b/gazebo/Server.cc @@ -586,7 +586,8 @@ void Server::Run() // The server and sensor manager outlive worlds while (!this->dataPtr->stop) { - bool ret = rendering::wait_for_render_request("", 0.100); + rendering::wait_for_render_request("", 0.100); + // bool ret = rendering::wait_for_render_request("", 0.100); // if (ret == false) // gzerr << "time out reached!" << std::endl; diff --git a/test/integration/camera_sensor.cc b/test/integration/camera_sensor.cc index 287b2297d2..543d868734 100644 --- a/test/integration/camera_sensor.cc +++ b/test/integration/camera_sensor.cc @@ -307,7 +307,7 @@ TEST_F(CameraSensor, CheckThrottleStrictRate) // Load sensor_strict_rate.world instead, and don't call SpawnCamera. // That allows us to secify the custom namespace in the world file instead // of modifying SpawnCamera() in ServerFixture.cc. - //Load("worlds/empty_test.world"); + // Load("worlds/empty_test.world"); Load("worlds/sensor_strict_rate.world"); // Make sure the render engine is available. From 6afd412907b83fb57e83b7f892b26427ed049705 Mon Sep 17 00:00:00 2001 From: Mabel Zhang Date: Tue, 9 Jun 2020 16:12:53 -0400 Subject: [PATCH 12/46] backward compatibility conditionals Signed-off-by: Mabel Zhang --- gazebo/sensors/CameraSensor.cc | 133 +++++++++++------- gazebo/sensors/Sensor.cc | 6 + gazebo/sensors/Sensor.hh | 4 + gazebo/sensors/SensorManager.cc | 7 +- test/integration/camera_sensor.cc | 4 +- ...ct_rate.world => camera_strict_rate.world} | 0 6 files changed, 100 insertions(+), 54 deletions(-) rename test/worlds/{sensor_strict_rate.world => camera_strict_rate.world} (100%) diff --git a/gazebo/sensors/CameraSensor.cc b/gazebo/sensors/CameraSensor.cc index 0da1cb0899..378d7bd20b 100644 --- a/gazebo/sensors/CameraSensor.cc +++ b/gazebo/sensors/CameraSensor.cc @@ -26,8 +26,8 @@ #include "gazebo/msgs/msgs.hh" -#include "gazebo/physics/World.hh" #include "gazebo/physics/physics.hh" +#include "gazebo/physics/World.hh" #include "gazebo/rendering/Camera.hh" #include "gazebo/rendering/RenderEngine.hh" @@ -199,7 +199,7 @@ void CameraSensor::Fini() void CameraSensor::SetActive(bool _value) { // If this sensor is reactivated - if (_value && !this->IsActive()) + if (this->useStrictRate && _value && !this->IsActive()) { // the next rendering time must be reset to ensure it is properly // computed by CameraSensor::NeedsUpdate. @@ -211,59 +211,73 @@ void CameraSensor::SetActive(bool _value) ////////////////////////////////////////////////// bool CameraSensor::NeedsUpdate() { - double simTime = this->scene->SimTime().Double(); - - if (simTime < this->lastMeasurementTime.Double()) + if (this->useStrictRate) { - // Rendering sensors also set the lastMeasurementTime variable in Render() - // and lastUpdateTime in Sensor::Update based on Scene::SimTime() which - // could be outdated when the world is reset. In this case reset - // the variables back to 0. - gzwarn << "reset detected !" << std::endl; - this->ResetLastUpdateTime(); - return false; - } - - double dt = this->world->Physics()->GetMaxStepSize(); - - // If next rendering time is not set yet - if (std::isnan(this->dataPtr->nextRenderingTime)) - { - if (this->updatePeriod == 0 - || (simTime > 0.0 && - std::abs(std::fmod(simTime, this->updatePeriod.Double())) < dt)) + double simTime = this->scene->SimTime().Double(); + + if (simTime < this->lastMeasurementTime.Double()) { - this->dataPtr->nextRenderingTime = simTime; - return true; + // Rendering sensors also set the lastMeasurementTime variable in Render() + // and lastUpdateTime in Sensor::Update based on Scene::SimTime() which + // could be outdated when the world is reset. In this case reset + // the variables back to 0. + this->ResetLastUpdateTime(); + return false; } - else + + double dt = this->world->Physics()->GetMaxStepSize(); + + // If next rendering time is not set yet + if (std::isnan(this->dataPtr->nextRenderingTime)) { - return false; + if (this->updatePeriod == 0 + || (simTime > 0.0 && + std::abs(std::fmod(simTime, this->updatePeriod.Double())) < dt)) + { + this->dataPtr->nextRenderingTime = simTime; + return true; + } + else + { + return false; + } } + + if (simTime > this->dataPtr->nextRenderingTime + dt) + return true; + + // Trigger on the tick the closest from the targeted rendering time + return (ignition::math::lessOrNearEqual( + std::abs(simTime - this->dataPtr->nextRenderingTime), dt / 2.0)); + } + else + { + return Sensor::NeedsUpdate(); } - - if (simTime > this->dataPtr->nextRenderingTime + dt) - return true; - - // Trigger on the tick the closest from the targeted rendering time - return (ignition::math::lessOrNearEqual( - std::abs(simTime - this->dataPtr->nextRenderingTime), dt / 2.0)); } ////////////////////////////////////////////////// void CameraSensor::Update(bool _force) { - if (this->IsActive() || _force) + if (this->useStrictRate) + { + if (this->IsActive() || _force) + { + if (this->UpdateImpl(_force)) + this->updated(); + } + } + else { - if (this->UpdateImpl(_force)) - this->updated(); + Sensor::Update(_force); } } ////////////////////////////////////////////////// void CameraSensor::PrerenderEnded() { - if (this->camera && this->IsActive() && this->NeedsUpdate()) + if (this->useStrictRate && this->camera && this->IsActive() && + this->NeedsUpdate()) { // compute next rendering time, take care of the case where period is zero. double dt; @@ -281,14 +295,28 @@ void CameraSensor::PrerenderEnded() ////////////////////////////////////////////////// void CameraSensor::Render() { - if (!this->dataPtr->renderNeeded) + if (this->useStrictRate) + { + if (!this->dataPtr->renderNeeded) + return; + + // Update all the cameras + this->camera->Render(); + + this->dataPtr->rendered = true; + this->dataPtr->renderNeeded = false; + } + else + { + if (!this->camera || !this->IsActive() || !this->NeedsUpdate()) return; - // Update all the cameras - this->camera->Render(); - - this->dataPtr->rendered = true; - this->dataPtr->renderNeeded = false; + // Update all the cameras + this->camera->Render(); + + this->dataPtr->rendered = true; + this->lastMeasurementTime = this->scene->SimTime(); + } } ////////////////////////////////////////////////// @@ -430,17 +458,24 @@ void CameraSensor::SetRendered(const bool _value) ////////////////////////////////////////////////// double CameraSensor::NextRequiredTimestamp() const { - if (this->useStrictRate - && !ignition::math::equal(this->updatePeriod.Double(), 0.0)) - return this->dataPtr->nextRenderingTime; + if (this->useStrictRate) + { + if (!ignition::math::equal(this->updatePeriod.Double(), 0.0)) + return this->dataPtr->nextRenderingTime; + else + return std::numeric_limits::quiet_NaN(); + } else - return std::numeric_limits::quiet_NaN(); + { + return Sensor::NextRequiredTimestamp(); + } } ////////////////////////////////////////////////// void CameraSensor::ResetLastUpdateTime() { - this->lastMeasurementTime = 0.0; - this->dataPtr->nextRenderingTime = std::numeric_limits::quiet_NaN(); + Sensor::ResetLastUpdateTime(); + if (this->useStrictRate) + this->dataPtr->nextRenderingTime = std::numeric_limits::quiet_NaN(); } diff --git a/gazebo/sensors/Sensor.cc b/gazebo/sensors/Sensor.cc index 69b1ab1de2..5c2641a108 100644 --- a/gazebo/sensors/Sensor.cc +++ b/gazebo/sensors/Sensor.cc @@ -521,3 +521,9 @@ double Sensor::NextRequiredTimestamp() const // implementation by default: next required timestamp is ignored return std::numeric_limits::quiet_NaN(); } + +////////////////////////////////////////////////// +bool Sensor::StrictRate() const +{ + return this->useStrictRate; +} diff --git a/gazebo/sensors/Sensor.hh b/gazebo/sensors/Sensor.hh index 30a97a07a9..da72331950 100644 --- a/gazebo/sensors/Sensor.hh +++ b/gazebo/sensors/Sensor.hh @@ -185,6 +185,10 @@ namespace gazebo /// \return the timestamp public: virtual double NextRequiredTimestamp() const; + /// \brief Returns true if the sensor is to follow strict update rate + /// \return True when sensor should follow strict update rate + public: bool StrictRate() const; + /// \brief This gets overwritten by derived sensor types. /// This function is called during Sensor::Update. /// And in turn, Sensor::Update is called by diff --git a/gazebo/sensors/SensorManager.cc b/gazebo/sensors/SensorManager.cc index e02ba44f12..dc18cdc40a 100644 --- a/gazebo/sensors/SensorManager.cc +++ b/gazebo/sensors/SensorManager.cc @@ -334,9 +334,10 @@ std::string SensorManager::CreateSensor(sdf::ElementPtr _elem, this->worlds[_worldName] = physics::get_world(_worldName); // Provide the wait function to the given world - this->worlds[_worldName]->SetSensorWaitFunc( - std::bind(&SensorManager::WaitForSensors, this, - std::placeholders::_1, std::placeholders::_2)); + if (sensor->StrictRate()) + this->worlds[_worldName]->SetSensorWaitFunc( + std::bind(&SensorManager::WaitForSensors, this, + std::placeholders::_1, std::placeholders::_2)); // If the SensorManager has not been initialized, then it's okay to push // the sensor into one of the sensor vectors because the sensor will get diff --git a/test/integration/camera_sensor.cc b/test/integration/camera_sensor.cc index 543d868734..8cd4a3224a 100644 --- a/test/integration/camera_sensor.cc +++ b/test/integration/camera_sensor.cc @@ -304,11 +304,11 @@ TEST_F(CameraSensor, CheckThrottle) ///////////////////////////////////////////////// TEST_F(CameraSensor, CheckThrottleStrictRate) { - // Load sensor_strict_rate.world instead, and don't call SpawnCamera. + // Load camera_strict_rate.world instead, and don't call SpawnCamera. // That allows us to secify the custom namespace in the world file instead // of modifying SpawnCamera() in ServerFixture.cc. // Load("worlds/empty_test.world"); - Load("worlds/sensor_strict_rate.world"); + Load("worlds/camera_strict_rate.world"); // Make sure the render engine is available. if (rendering::RenderEngine::Instance()->GetRenderPathType() == diff --git a/test/worlds/sensor_strict_rate.world b/test/worlds/camera_strict_rate.world similarity index 100% rename from test/worlds/sensor_strict_rate.world rename to test/worlds/camera_strict_rate.world From f43b1004a7ef51490953b0f29519e56924d54513 Mon Sep 17 00:00:00 2001 From: Mabel Zhang Date: Wed, 10 Jun 2020 02:11:15 -0400 Subject: [PATCH 13/46] style and PR comments Signed-off-by: Mabel Zhang --- gazebo/sensors/CameraSensor.cc | 16 ++++++++-------- gazebo/test/ServerFixture.hh | 1 + test/integration/camera_sensor.cc | 17 ----------------- 3 files changed, 9 insertions(+), 25 deletions(-) diff --git a/gazebo/sensors/CameraSensor.cc b/gazebo/sensors/CameraSensor.cc index 378d7bd20b..f6bb9c3763 100644 --- a/gazebo/sensors/CameraSensor.cc +++ b/gazebo/sensors/CameraSensor.cc @@ -214,7 +214,7 @@ bool CameraSensor::NeedsUpdate() if (this->useStrictRate) { double simTime = this->scene->SimTime().Double(); - + if (simTime < this->lastMeasurementTime.Double()) { // Rendering sensors also set the lastMeasurementTime variable in Render() @@ -224,9 +224,9 @@ bool CameraSensor::NeedsUpdate() this->ResetLastUpdateTime(); return false; } - + double dt = this->world->Physics()->GetMaxStepSize(); - + // If next rendering time is not set yet if (std::isnan(this->dataPtr->nextRenderingTime)) { @@ -242,10 +242,10 @@ bool CameraSensor::NeedsUpdate() return false; } } - + if (simTime > this->dataPtr->nextRenderingTime + dt) return true; - + // Trigger on the tick the closest from the targeted rendering time return (ignition::math::lessOrNearEqual( std::abs(simTime - this->dataPtr->nextRenderingTime), dt / 2.0)); @@ -299,10 +299,10 @@ void CameraSensor::Render() { if (!this->dataPtr->renderNeeded) return; - + // Update all the cameras this->camera->Render(); - + this->dataPtr->rendered = true; this->dataPtr->renderNeeded = false; } @@ -313,7 +313,7 @@ void CameraSensor::Render() // Update all the cameras this->camera->Render(); - + this->dataPtr->rendered = true; this->lastMeasurementTime = this->scene->SimTime(); } diff --git a/gazebo/test/ServerFixture.hh b/gazebo/test/ServerFixture.hh index eed72da88f..cf9d788943 100644 --- a/gazebo/test/ServerFixture.hh +++ b/gazebo/test/ServerFixture.hh @@ -254,6 +254,7 @@ namespace gazebo /// \param[in] _distortionP2 Distortion coefficient p2. /// \param[in] _cx Normalized optical center x, used for distortion. /// \param[in] _cy Normalized optical center y, used for distortion. + /// \param[in] _strictRate Flag to enable strict update rate adherence. protected: void SpawnCamera(const std::string &_modelName, const std::string &_cameraName, const ignition::math::Vector3d &_pos = diff --git a/test/integration/camera_sensor.cc b/test/integration/camera_sensor.cc index 8cd4a3224a..909e57d884 100644 --- a/test/integration/camera_sensor.cc +++ b/test/integration/camera_sensor.cc @@ -307,7 +307,6 @@ TEST_F(CameraSensor, CheckThrottleStrictRate) // Load camera_strict_rate.world instead, and don't call SpawnCamera. // That allows us to secify the custom namespace in the world file instead // of modifying SpawnCamera() in ServerFixture.cc. - // Load("worlds/empty_test.world"); Load("worlds/camera_strict_rate.world"); // Make sure the render engine is available. @@ -318,23 +317,7 @@ TEST_F(CameraSensor, CheckThrottleStrictRate) return; } - // TODO(mabelzhang) Read these values from the SDF. Is it possible? - // std::string modelName = "camera_model"; std::string cameraName = "camera_sensor"; - /* - unsigned int width = 1280; - unsigned int height = 720; - // we choose a high fps on purpose. The goal is to check the effect - // of the flag "strict_rate". - double updateRate = 500; - ignition::math::Pose3d setPose, testPose(ignition::math::Vector3d(-5, 0, 5), - ignition::math::Quaterniond(0, IGN_DTOR(15), 0)); - SpawnCamera(modelName, cameraName, setPose.Pos(), - setPose.Rot().Euler(), width, height, updateRate, - // use default values for distortion - "", 0.0, 0.0, false, 0.0, 0.0, 0.0, 0.0, 0.0, 0.5, 0.5, - false); - */ sensors::SensorPtr sensor = sensors::get_sensor(cameraName); sensors::CameraSensorPtr camSensor = std::dynamic_pointer_cast(sensor); From c134381626fd499eca63d633925c8bd1bedf89a5 Mon Sep 17 00:00:00 2001 From: Mabel Zhang Date: Wed, 10 Jun 2020 02:12:22 -0400 Subject: [PATCH 14/46] raise strict rate tolerance to 0.06 Signed-off-by: Mabel Zhang --- test/integration/camera_sensor.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/test/integration/camera_sensor.cc b/test/integration/camera_sensor.cc index 909e57d884..d6c99e12da 100644 --- a/test/integration/camera_sensor.cc +++ b/test/integration/camera_sensor.cc @@ -346,7 +346,7 @@ TEST_F(CameraSensor, CheckThrottleStrictRate) double dt = world->SimTime().Double() - simT0; double rate = static_cast(total_images) / dt; gzdbg << "timer [" << dt << "] seconds rate [" << rate << "] fps\n"; - const double tolerance = 0.02; + const double tolerance = 0.06; EXPECT_GT(rate, updateRate * (1 - tolerance)); EXPECT_LT(rate, updateRate * (1 + tolerance)); c.reset(); From ed6fc04ef0d38e0cdf955739f2bdc607a1532c06 Mon Sep 17 00:00:00 2001 From: Mabel Zhang Date: Thu, 18 Jun 2020 02:33:59 -0400 Subject: [PATCH 15/46] fix time calculation in test Signed-off-by: Mabel Zhang --- test/integration/camera_sensor.cc | 18 ++++++++++++------ 1 file changed, 12 insertions(+), 6 deletions(-) diff --git a/test/integration/camera_sensor.cc b/test/integration/camera_sensor.cc index d6c99e12da..180eac688b 100644 --- a/test/integration/camera_sensor.cc +++ b/test/integration/camera_sensor.cc @@ -268,7 +268,7 @@ TEST_F(CameraSensor, CheckThrottle) std::string modelName = "camera_model"; std::string cameraName = "camera_sensor"; unsigned int width = 320; - unsigned int height = 240; // 106 fps + unsigned int height = 240; double updateRate = 10; ignition::math::Pose3d setPose, testPose(ignition::math::Vector3d(-5, 0, 5), ignition::math::Quaterniond(0, IGN_DTOR(15), 0)); @@ -334,19 +334,25 @@ TEST_F(CameraSensor, CheckThrottleStrictRate) // how many images produced for 5 seconds (in simulated clock domain) double updateRate = camSensor->UpdateRate(); - int total_images = 5 * updateRate; + int totalImages = 5 * updateRate; physics::WorldPtr world = physics::get_world("default"); ASSERT_TRUE(world != NULL); - double simT0 = world->SimTime().Double(); + double simT0 = 0.0; - while (imageCount < total_images) + while (imageCount < totalImages) + { + if (imageCount == 1) + { + simT0 = world->SimTime().Double(); + } common::Time::MSleep(1); + } // check that the obtained rate is the one expected double dt = world->SimTime().Double() - simT0; - double rate = static_cast(total_images) / dt; + double rate = static_cast(totalImages) / dt; gzdbg << "timer [" << dt << "] seconds rate [" << rate << "] fps\n"; - const double tolerance = 0.06; + const double tolerance = 0.02; EXPECT_GT(rate, updateRate * (1 - tolerance)); EXPECT_LT(rate, updateRate * (1 + tolerance)); c.reset(); From bd0085b992e8fc90eed87ff488a9c0d1f1018e68 Mon Sep 17 00:00:00 2001 From: Mabel Zhang Date: Thu, 18 Jun 2020 03:22:36 -0400 Subject: [PATCH 16/46] correct test timing Signed-off-by: Mabel Zhang --- test/integration/camera_sensor.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/test/integration/camera_sensor.cc b/test/integration/camera_sensor.cc index 180eac688b..b0c3a35598 100644 --- a/test/integration/camera_sensor.cc +++ b/test/integration/camera_sensor.cc @@ -341,7 +341,7 @@ TEST_F(CameraSensor, CheckThrottleStrictRate) while (imageCount < totalImages) { - if (imageCount == 1) + if (imageCount == 0) { simT0 = world->SimTime().Double(); } From 90485b1a4a7fac26932905d400c2613c674c4942 Mon Sep 17 00:00:00 2001 From: Mabel Zhang Date: Thu, 18 Jun 2020 04:49:23 -0400 Subject: [PATCH 17/46] simplify header include Signed-off-by: Mabel Zhang --- gazebo/sensors/CameraSensor.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gazebo/sensors/CameraSensor.cc b/gazebo/sensors/CameraSensor.cc index f6bb9c3763..52749d9bca 100644 --- a/gazebo/sensors/CameraSensor.cc +++ b/gazebo/sensors/CameraSensor.cc @@ -26,7 +26,7 @@ #include "gazebo/msgs/msgs.hh" -#include "gazebo/physics/physics.hh" +#include "gazebo/physics/PhysicsEngine.hh" #include "gazebo/physics/World.hh" #include "gazebo/rendering/Camera.hh" From 9a44c66088563e78edcd6e4e6206c2d921656355 Mon Sep 17 00:00:00 2001 From: Mabel Zhang Date: Thu, 18 Jun 2020 05:03:35 -0400 Subject: [PATCH 18/46] one more backward compatibility conditional Signed-off-by: Mabel Zhang --- gazebo/sensors/CameraSensor.cc | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/gazebo/sensors/CameraSensor.cc b/gazebo/sensors/CameraSensor.cc index 52749d9bca..1674793a6f 100644 --- a/gazebo/sensors/CameraSensor.cc +++ b/gazebo/sensors/CameraSensor.cc @@ -55,9 +55,6 @@ CameraSensor::CameraSensor() this->connections.push_back( event::Events::ConnectRender( std::bind(&CameraSensor::Render, this))); - this->connections.push_back( - event::Events::ConnectPreRenderEnded( - boost::bind(&CameraSensor::PrerenderEnded, this))); } ////////////////////////////////////////////////// @@ -96,6 +93,14 @@ std::string CameraSensor::TopicIgn() const void CameraSensor::Load(const std::string &_worldName) { Sensor::Load(_worldName); + // strict_rate parameter is parsed in Sensor::Load() + if (this->useStrictRate) + { + this->connections.push_back( + event::Events::ConnectPreRenderEnded( + boost::bind(&CameraSensor::PrerenderEnded, this))); + } + this->imagePub = this->node->Advertise(this->Topic(), 50); ignition::transport::AdvertiseMessageOptions opts; From 034953d8f292d6cb9654a86302e05510634a222e Mon Sep 17 00:00:00 2001 From: Mabel Zhang Date: Thu, 18 Jun 2020 05:12:34 -0400 Subject: [PATCH 19/46] sync with CameraSensor branch: simplify header include, change test timestamp Signed-off-by: Mabel Zhang --- gazebo/sensors/GpuRaySensor.cc | 2 +- test/integration/gpu_laser.cc | 14 ++++++++++---- 2 files changed, 11 insertions(+), 5 deletions(-) diff --git a/gazebo/sensors/GpuRaySensor.cc b/gazebo/sensors/GpuRaySensor.cc index cace7b8352..bd573908f2 100644 --- a/gazebo/sensors/GpuRaySensor.cc +++ b/gazebo/sensors/GpuRaySensor.cc @@ -21,7 +21,7 @@ #include "gazebo/physics/World.hh" #include "gazebo/physics/Entity.hh" #include "gazebo/physics/Model.hh" -#include "gazebo/physics/physics.hh" +#include "gazebo/physics/PhysicsEngine.hh" #include "gazebo/common/Exception.hh" #include "gazebo/common/Events.hh" diff --git a/test/integration/gpu_laser.cc b/test/integration/gpu_laser.cc index e353348fb9..3dde8baff3 100644 --- a/test/integration/gpu_laser.cc +++ b/test/integration/gpu_laser.cc @@ -645,17 +645,23 @@ TEST_F(GPURaySensorTest, StrictUpdateRate) // how many scans produced for 5 seconds (in simulated clock domain) double updateRate = raySensor->UpdateRate(); - int total_scans = 5 * updateRate; + int totalScans = 5 * updateRate; physics::WorldPtr world = physics::get_world("default"); ASSERT_TRUE(world != NULL); - double simT0 = world->SimTime().Double(); + double simT0 = 0.0; - while (scanCount < total_scans) + while (scanCount < totalScans) + { + if (scanCount == 0) + { + simT0 = world->SimTime().Double(); + } common::Time::MSleep(1); + } // check that the obtained rate is the one expected double dt = world->SimTime().Double() - simT0; - double rate = static_cast(total_scans) / dt; + double rate = static_cast(totalScans) / dt; gzdbg << "timer [" << dt << "] seconds rate [" << rate << "] fps\n"; const double tolerance = 0.02; EXPECT_GT(rate, updateRate * (1 - tolerance)); From c7c67950ef09a897b3f072a3b3f1cd65c76c68c5 Mon Sep 17 00:00:00 2001 From: Mabel Zhang Date: Fri, 19 Jun 2020 22:04:42 -0400 Subject: [PATCH 20/46] extend lockstep to MultiCameraSensor and Sensor Signed-off-by: Mabel Zhang --- gazebo/sensors/CameraSensor.cc | 17 ++- gazebo/sensors/GpuRaySensor.cc | 29 +++- gazebo/sensors/MultiCameraSensor.cc | 166 +++++++++++++++++++-- gazebo/sensors/MultiCameraSensor.hh | 18 +++ gazebo/sensors/MultiCameraSensorPrivate.hh | 8 + gazebo/sensors/Sensor.cc | 96 ++++++------ test/integration/multicamera_sensor.cc | 65 ++++++++ test/worlds/multicamera_strict_rate.world | 79 ++++++++++ 8 files changed, 407 insertions(+), 71 deletions(-) create mode 100644 test/worlds/multicamera_strict_rate.world diff --git a/gazebo/sensors/CameraSensor.cc b/gazebo/sensors/CameraSensor.cc index 1674793a6f..a42dcbda38 100644 --- a/gazebo/sensors/CameraSensor.cc +++ b/gazebo/sensors/CameraSensor.cc @@ -67,6 +67,13 @@ CameraSensor::~CameraSensor() void CameraSensor::Load(const std::string &_worldName, sdf::ElementPtr _sdf) { Sensor::Load(_worldName, _sdf); + // strict_rate parameter is parsed in Sensor::Load() + if (this->useStrictRate) + { + this->connections.push_back( + event::Events::ConnectPreRenderEnded( + boost::bind(&CameraSensor::PrerenderEnded, this))); + } } ////////////////////////////////////////////////// @@ -218,7 +225,11 @@ bool CameraSensor::NeedsUpdate() { if (this->useStrictRate) { - double simTime = this->scene->SimTime().Double(); + double simTime; + if (this->scene) + simTime = this->scene->SimTime().Double(); + else + simTime = this->world->SimTime().Double(); if (simTime < this->lastMeasurementTime.Double()) { @@ -264,6 +275,7 @@ bool CameraSensor::NeedsUpdate() ////////////////////////////////////////////////// void CameraSensor::Update(bool _force) { + /* if (this->useStrictRate) { if (this->IsActive() || _force) @@ -276,6 +288,8 @@ void CameraSensor::Update(bool _force) { Sensor::Update(_force); } + */ + Sensor::Update(_force); } ////////////////////////////////////////////////// @@ -483,4 +497,3 @@ void CameraSensor::ResetLastUpdateTime() if (this->useStrictRate) this->dataPtr->nextRenderingTime = std::numeric_limits::quiet_NaN(); } - diff --git a/gazebo/sensors/GpuRaySensor.cc b/gazebo/sensors/GpuRaySensor.cc index bd573908f2..ab7d37ddcc 100644 --- a/gazebo/sensors/GpuRaySensor.cc +++ b/gazebo/sensors/GpuRaySensor.cc @@ -93,6 +93,13 @@ void GpuRaySensor::Load(const std::string &_worldName, sdf::ElementPtr _sdf) void GpuRaySensor::Load(const std::string &_worldName) { Sensor::Load(_worldName); + // strict_rate parameter is parsed in Sensor::Load() + if (this->useStrictRate) + { + this->connections.push_back( + event::Events::ConnectPreRenderEnded( + boost::bind(&GpuRaySensor::PrerenderEnded, this))); + } this->dataPtr->scanPub = this->node->Advertise(this->Topic(), 50); @@ -369,8 +376,12 @@ bool GpuRaySensor::NeedsUpdate() { if (this->useStrictRate) { - double simTime = this->scene->SimTime().Double(); - + double simTime; + if (this->scene) + simTime = this->scene->SimTime().Double(); + else + simTime = this->world->SimTime().Double(); + if (simTime < this->lastMeasurementTime.Double()) { // Rendering sensors also set the lastMeasurementTime variable in Render() @@ -380,9 +391,9 @@ bool GpuRaySensor::NeedsUpdate() this->ResetLastUpdateTime(); return false; } - + double dt = this->world->Physics()->GetMaxStepSize(); - + // If next rendering time is not set yet if (std::isnan(this->dataPtr->nextRenderingTime)) { @@ -398,10 +409,10 @@ bool GpuRaySensor::NeedsUpdate() return false; } } - + if (simTime > this->dataPtr->nextRenderingTime + dt) return true; - + // Trigger on the tick the closest from the targeted rendering time return (ignition::math::lessOrNearEqual( std::abs(simTime - this->dataPtr->nextRenderingTime), dt / 2.0)); @@ -415,6 +426,7 @@ bool GpuRaySensor::NeedsUpdate() ////////////////////////////////////////////////// void GpuRaySensor::Update(bool _force) { + /* if (this->useStrictRate) { if (this->IsActive() || _force) @@ -427,6 +439,8 @@ void GpuRaySensor::Update(bool _force) { Sensor::Update(_force); } + */ + Sensor::Update(_force); } ////////////////////////////////////////////////// @@ -653,7 +667,8 @@ int GpuRaySensor::Fiducial(const unsigned int /*_index*/) const ////////////////////////////////////////////////// void GpuRaySensor::PrerenderEnded() { - if (this->dataPtr->laserCam && this->IsActive() && this->NeedsUpdate()) + if (this->useStrictRate && this->dataPtr->laserCam && this->IsActive() && + this->NeedsUpdate()) { // compute next rendering time, take care of the case where period is zero. double dt; diff --git a/gazebo/sensors/MultiCameraSensor.cc b/gazebo/sensors/MultiCameraSensor.cc index d358330769..bcec44b462 100644 --- a/gazebo/sensors/MultiCameraSensor.cc +++ b/gazebo/sensors/MultiCameraSensor.cc @@ -22,6 +22,7 @@ #include "gazebo/common/EnumIface.hh" #include "gazebo/common/Image.hh" +#include "gazebo/physics/PhysicsEngine.hh" #include "gazebo/physics/World.hh" #include "gazebo/transport/transport.hh" @@ -79,6 +80,13 @@ std::string MultiCameraSensor::Topic() const void MultiCameraSensor::Load(const std::string &_worldName) { Sensor::Load(_worldName); + // strict_rate parameter is parsed in Sensor::Load() + if (this->useStrictRate) + { + this->connections.push_back( + event::Events::ConnectPreRenderEnded( + boost::bind(&MultiCameraSensor::PrerenderEnded, this))); + } // Create the publisher of image data. this->dataPtr->imagePub = @@ -224,28 +232,138 @@ rendering::CameraPtr MultiCameraSensor::Camera(const unsigned int _index) const } ////////////////////////////////////////////////// -void MultiCameraSensor::Render() +void MultiCameraSensor::SetActive(bool _value) { - if (!this->IsActive() || !this->NeedsUpdate()) + // If this sensor is reactivated + if (this->useStrictRate && _value && !this->IsActive()) { - return; + // the next rendering time must be reset to ensure it is properly + // computed by Sensor::NeedsUpdate. + this->dataPtr->nextRenderingTime = std::numeric_limits::quiet_NaN(); } + Sensor::SetActive(_value); +} - // Update all the cameras - std::lock_guard lock(this->dataPtr->cameraMutex); - if (this->dataPtr->cameras.empty()) +////////////////////////////////////////////////// +bool MultiCameraSensor::NeedsUpdate() +{ + if (this->useStrictRate) { - return; + double simTime; + if (this->scene) + simTime = this->scene->SimTime().Double(); + else + simTime = this->world->SimTime().Double(); + + if (simTime < this->lastMeasurementTime.Double()) + { + // Rendering sensors also set the lastMeasurementTime variable in Render() + // and lastUpdateTime in Sensor::Update based on Scene::SimTime() which + // could be outdated when the world is reset. In this case reset + // the variables back to 0. + this->ResetLastUpdateTime(); + return false; + } + + double dt = this->world->Physics()->GetMaxStepSize(); + + // If next rendering time is not set yet + if (std::isnan(this->dataPtr->nextRenderingTime)) + { + if (this->updatePeriod == 0 + || (simTime > 0.0 && + std::abs(std::fmod(simTime, this->updatePeriod.Double())) < dt)) + { + this->dataPtr->nextRenderingTime = simTime; + return true; + } + else + { + return false; + } + } + + if (simTime > this->dataPtr->nextRenderingTime + dt) + return true; + + // Trigger on the tick the closest from the targeted rendering time + return (ignition::math::lessOrNearEqual( + std::abs(simTime - this->dataPtr->nextRenderingTime), dt / 2.0)); } + else + { + return Sensor::NeedsUpdate(); + } +} - for (auto iter = this->dataPtr->cameras.begin(); - iter != this->dataPtr->cameras.end(); ++iter) +////////////////////////////////////////////////// +void MultiCameraSensor::PrerenderEnded() +{ + if (this->useStrictRate && this->dataPtr->cameras.size() > 0 && + this->IsActive() && this->NeedsUpdate()) + { + // compute next rendering time, take care of the case where period is zero. + double dt; + if (this->updatePeriod <= 0.0) + dt = this->world->Physics()->GetMaxStepSize(); + else + dt = this->updatePeriod.Double(); + this->dataPtr->nextRenderingTime += dt; + + this->dataPtr->renderNeeded = true; + this->lastMeasurementTime = this->scene->SimTime(); + } +} + +////////////////////////////////////////////////// +void MultiCameraSensor::Render() +{ + if (this->useStrictRate) { - (*iter)->Render(); + if (!this->dataPtr->renderNeeded) + { + return; + } + + // Update all the cameras + std::lock_guard lock(this->dataPtr->cameraMutex); + if (this->dataPtr->cameras.empty()) + { + return; + } + + for (auto iter = this->dataPtr->cameras.begin(); + iter != this->dataPtr->cameras.end(); ++iter) + { + (*iter)->Render(); + } + + this->dataPtr->rendered = true; + this->dataPtr->renderNeeded = false; } + else + { + if (!this->IsActive() || !this->NeedsUpdate()) + { + return; + } + + // Update all the cameras + std::lock_guard lock(this->dataPtr->cameraMutex); + if (this->dataPtr->cameras.empty()) + { + return; + } - this->dataPtr->rendered = true; - this->lastMeasurementTime = this->scene->SimTime(); + for (auto iter = this->dataPtr->cameras.begin(); + iter != this->dataPtr->cameras.end(); ++iter) + { + (*iter)->Render(); + } + + this->dataPtr->rendered = true; + this->lastMeasurementTime = this->scene->SimTime(); + } } ////////////////////////////////////////////////// @@ -338,3 +456,27 @@ bool MultiCameraSensor::IsActive() const return Sensor::IsActive() || (this->dataPtr->imagePub && this->dataPtr->imagePub->HasConnections()); } + +////////////////////////////////////////////////// +double MultiCameraSensor::NextRequiredTimestamp() const +{ + if (this->useStrictRate) + { + if (!ignition::math::equal(this->updatePeriod.Double(), 0.0)) + return this->dataPtr->nextRenderingTime; + else + return std::numeric_limits::quiet_NaN(); + } + else + { + return Sensor::NextRequiredTimestamp(); + } +} + +////////////////////////////////////////////////// +void MultiCameraSensor::ResetLastUpdateTime() +{ + Sensor::ResetLastUpdateTime(); + if (this->useStrictRate) + this->dataPtr->nextRenderingTime = std::numeric_limits::quiet_NaN(); +} diff --git a/gazebo/sensors/MultiCameraSensor.hh b/gazebo/sensors/MultiCameraSensor.hh index 5a4027c3d5..2edc4fc1d1 100644 --- a/gazebo/sensors/MultiCameraSensor.hh +++ b/gazebo/sensors/MultiCameraSensor.hh @@ -55,9 +55,24 @@ namespace gazebo // Documentation inherited public: virtual void Init(); + /// \brief reset timing related members + protected: void ResetLastUpdateTime() override; + + /// \brief Return true if the sensor needs to be updated. + /// \return True when sensor should be updated. + protected: bool NeedsUpdate() override; + + /// \brief Return the next timestamp going to be used by the sensor + /// \return the timestamp + public: double NextRequiredTimestamp() const override; + // Documentation inherited public: virtual std::string Topic() const; + /// \brief Set whether the sensor is active or not. + /// \param[in] _value True if active, false if not. + public: void SetActive(bool _value) override; + /// \brief Get the number of cameras. /// \return The number of cameras. public: unsigned int CameraCount() const; @@ -105,6 +120,9 @@ namespace gazebo /// \brief Handle the render event. private: void Render(); + /// \brief Handle the prerenderEnded event. + private: void PrerenderEnded(); + /// \internal /// \brief Private data pointer private: std::unique_ptr dataPtr; diff --git a/gazebo/sensors/MultiCameraSensorPrivate.hh b/gazebo/sensors/MultiCameraSensorPrivate.hh index 4f6d5df4d0..f01dc5f993 100644 --- a/gazebo/sensors/MultiCameraSensorPrivate.hh +++ b/gazebo/sensors/MultiCameraSensorPrivate.hh @@ -19,6 +19,7 @@ #include #include +#include #include "gazebo/msgs/msgs.hh" #include "gazebo/transport/TransportTypes.hh" @@ -45,6 +46,13 @@ namespace gazebo /// \brief True if the sensor was rendered. public: bool rendered; + + /// \brief True if the sensor needs a rendering + public: bool renderNeeded = false; + + /// \brief Timestamp of the forthcoming rendering + public: double nextRenderingTime + = std::numeric_limits::quiet_NaN(); }; } } diff --git a/gazebo/sensors/Sensor.cc b/gazebo/sensors/Sensor.cc index a77979b6d0..13c27e54d1 100644 --- a/gazebo/sensors/Sensor.cc +++ b/gazebo/sensors/Sensor.cc @@ -20,6 +20,7 @@ #include "gazebo/physics/Link.hh" #include "gazebo/physics/Joint.hh" #include "gazebo/physics/World.hh" +#include "gazebo/physics/PhysicsEngine.hh" #include "gazebo/common/Timer.hh" #include "gazebo/common/Console.hh" @@ -144,11 +145,6 @@ void Sensor::Load(const std::string &_worldName) if (this->dataPtr->category == IMAGE) { this->scene = rendering::get_scene(_worldName); - // TODO(mabelzhang) Decide whether to have this in Sensor or individually - // in each of the rendering sensors - // this->connections.push_back( - // event::Events::ConnectPreRenderEnded( - // boost::bind(&Sensor::PrerenderEnded, this))); } // loaded, but not updated @@ -238,58 +234,58 @@ void Sensor::Update(const bool _force) { if (this->IsActive() || _force) { - common::Time simTime; - if (this->dataPtr->category == IMAGE && this->scene) - simTime = this->scene->SimTime(); - else - simTime = this->world->SimTime(); - + // TODO(mabelzhang): Does strict rate not need anything in the else-clause + // below at all? Strict rate PR did not use them + if (this->useStrictRate) { - std::lock_guard lock(this->dataPtr->mutexLastUpdateTime); - - if (simTime <= this->lastUpdateTime && !_force) - return; - - // Adjust time-to-update period to compensate for delays caused by another - // sensor's update in the same thread. - // NOTE: If you change this equation, also change the matching equation in - // Sensor::NeedsUpdate - common::Time adjustedElapsed = simTime - - this->lastUpdateTime + this->dataPtr->updateDelay; - - if (adjustedElapsed < this->updatePeriod && !_force) - return; - - this->dataPtr->updateDelay = std::max(common::Time::Zero, - adjustedElapsed - this->updatePeriod); - - // if delay is more than a full update period, then give up trying - // to catch up. This happens normally when the sensor just changed from - // an inactive to an active state, or the sensor just cannot hit its - // target update rate (worst case). - if (this->dataPtr->updateDelay >= this->updatePeriod) - this->dataPtr->updateDelay = common::Time::Zero; + if (this->UpdateImpl(_force)) + this->updated(); } - - if (this->UpdateImpl(_force)) + else { - std::lock_guard lock(this->dataPtr->mutexLastUpdateTime); - this->lastUpdateTime = simTime; - this->updated(); + common::Time simTime; + if (this->dataPtr->category == IMAGE && this->scene) + simTime = this->scene->SimTime(); + else + simTime = this->world->SimTime(); + + { + std::lock_guard lock(this->dataPtr->mutexLastUpdateTime); + + if (simTime <= this->lastUpdateTime && !_force) + return; + + // Adjust time-to-update period to compensate for delays caused by another + // sensor's update in the same thread. + // NOTE: If you change this equation, also change the matching equation in + // Sensor::NeedsUpdate + common::Time adjustedElapsed = simTime - + this->lastUpdateTime + this->dataPtr->updateDelay; + + if (adjustedElapsed < this->updatePeriod && !_force) + return; + + this->dataPtr->updateDelay = std::max(common::Time::Zero, + adjustedElapsed - this->updatePeriod); + + // if delay is more than a full update period, then give up trying + // to catch up. This happens normally when the sensor just changed from + // an inactive to an active state, or the sensor just cannot hit its + // target update rate (worst case). + if (this->dataPtr->updateDelay >= this->updatePeriod) + this->dataPtr->updateDelay = common::Time::Zero; + } + + if (this->UpdateImpl(_force)) + { + std::lock_guard lock(this->dataPtr->mutexLastUpdateTime); + this->lastUpdateTime = simTime; + this->updated(); + } } } } -////////////////////////////////////////////////// -// TODO(mabelzhang) To have this here for rendering sensors, this needs to -// be virtual and be implemented by CameraSensor, MultiCameraSensor, and -// GpuRaySensor. -/* -void CameraSensor::PrerenderEnded() -{ -} -*/ - ////////////////////////////////////////////////// void Sensor::Fini() { diff --git a/test/integration/multicamera_sensor.cc b/test/integration/multicamera_sensor.cc index 258bb9efe8..681932eacf 100644 --- a/test/integration/multicamera_sensor.cc +++ b/test/integration/multicamera_sensor.cc @@ -568,6 +568,71 @@ TEST_F(MultiCameraSensor, CameraRotationWorldPoseTest) << "]\n"; } +///////////////////////////////////////////////// +// TODO(mabelzhang): adapt to MultiCameraSensor. Create SDF file. +/* +TEST_F(MultiCameraSensorTest, StrictUpdateRate) +{ + Load("worlds/multi_camera_strict_rate.world"); + + // Make sure the render engine is available. + if (rendering::RenderEngine::Instance()->GetRenderPathType() == + rendering::RenderEngine::NONE) + { + gzerr << "No rendering engine, unable to run camera test\n"; + return; + } + + std::string sensorName = "multi_camera_sensor"; + sensors::SensorPtr sensor = sensors::get_sensor(sensorName); + sensors::MultiCameraSensorPtr raySensor = + std::dynamic_pointer_cast(sensor); + EXPECT_TRUE(raySensor != nullptr); + raySensor->SetActive(true); + + float *scan = new float[raySensor->RangeCount() + * raySensor->VerticalRangeCount() * 3]; + int scanCount = 0; + event::ConnectionPtr c = + raySensor->ConnectNewLaserFrame( + std::bind(&::OnNewLaserFrame, &scanCount, scan, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, + std::placeholders::_4, std::placeholders::_5)); + + // wait for a few laser scans + common::Timer timer; + timer.Start(); + + // how many scans produced for 5 seconds (in simulated clock domain) + double updateRate = raySensor->UpdateRate(); + int totalScans = 5 * updateRate; + physics::WorldPtr world = physics::get_world("default"); + ASSERT_TRUE(world != NULL); + double simT0 = 0.0; + + while (scanCount < totalScans) + { + if (scanCount == 0) + { + simT0 = world->SimTime().Double(); + } + common::Time::MSleep(1); + } + + // check that the obtained rate is the one expected + double dt = world->SimTime().Double() - simT0; + double rate = static_cast(totalScans) / dt; + gzdbg << "timer [" << dt << "] seconds rate [" << rate << "] fps\n"; + const double tolerance = 0.02; + EXPECT_GT(rate, updateRate * (1 - tolerance)); + EXPECT_LT(rate, updateRate * (1 + tolerance)); + + c.reset(); + + delete [] scan; +} +*/ + ///////////////////////////////////////////////// int main(int argc, char **argv) { diff --git a/test/worlds/multicamera_strict_rate.world b/test/worlds/multicamera_strict_rate.world new file mode 100644 index 0000000000..5389783e01 --- /dev/null +++ b/test/worlds/multicamera_strict_rate.world @@ -0,0 +1,79 @@ + + + + + model://ground_plane + + + model://sun + + + + true + 0 0 0.5 0 0 0 + + + + + 1 1 1 + + + + + + + + 1 1 1 + + + + + + 500 + true + + 1.3962634 + + 1024 + 544 + R8G8B8 + + + 0.02 + 100 + + + + + + + + 13 0 0.5 0 0 0 + + + + + 5 5 5 + + + + + + + 1 1 1 + + + + + + + + + active_pendulum + model://double_pendulum_with_base + 2 0 0 0 0 0 + 0.5 0.5 0.5 + + + + From 536c26bee6378eb50c2ca2c77ada6b0b9dca9859 Mon Sep 17 00:00:00 2001 From: Mabel Zhang Date: Fri, 19 Jun 2020 22:06:13 -0400 Subject: [PATCH 21/46] minor fixes Signed-off-by: Mabel Zhang --- gazebo/sensors/CameraSensor.cc | 13 ++++++++++++- 1 file changed, 12 insertions(+), 1 deletion(-) diff --git a/gazebo/sensors/CameraSensor.cc b/gazebo/sensors/CameraSensor.cc index 1674793a6f..31cfd42a50 100644 --- a/gazebo/sensors/CameraSensor.cc +++ b/gazebo/sensors/CameraSensor.cc @@ -67,6 +67,13 @@ CameraSensor::~CameraSensor() void CameraSensor::Load(const std::string &_worldName, sdf::ElementPtr _sdf) { Sensor::Load(_worldName, _sdf); + // strict_rate parameter is parsed in Sensor::Load() + if (this->useStrictRate) + { + this->connections.push_back( + event::Events::ConnectPreRenderEnded( + boost::bind(&CameraSensor::PrerenderEnded, this))); + } } ////////////////////////////////////////////////// @@ -218,7 +225,11 @@ bool CameraSensor::NeedsUpdate() { if (this->useStrictRate) { - double simTime = this->scene->SimTime().Double(); + double simTime; + if (this->scene) + simTime = this->scene->SimTime().Double(); + else + simTime = this->world->SimTime().Double(); if (simTime < this->lastMeasurementTime.Double()) { From a01101b3b51408eb20dacf998bc67e9a61138a31 Mon Sep 17 00:00:00 2001 From: Mabel Zhang Date: Fri, 19 Jun 2020 22:07:50 -0400 Subject: [PATCH 22/46] minor fixes Signed-off-by: Mabel Zhang --- gazebo/sensors/GpuRaySensor.cc | 15 +++++++++++++-- 1 file changed, 13 insertions(+), 2 deletions(-) diff --git a/gazebo/sensors/GpuRaySensor.cc b/gazebo/sensors/GpuRaySensor.cc index bd573908f2..fe5976670e 100644 --- a/gazebo/sensors/GpuRaySensor.cc +++ b/gazebo/sensors/GpuRaySensor.cc @@ -93,6 +93,12 @@ void GpuRaySensor::Load(const std::string &_worldName, sdf::ElementPtr _sdf) void GpuRaySensor::Load(const std::string &_worldName) { Sensor::Load(_worldName); + // strict_rate parameter is parsed in Sensor::Load() + if (this->useStrictRate) + { + this->connections.push_back( + event::Events::ConnectPreRenderEnded( + boost::bind(&GpuRaySensor::PrerenderEnded, this))); this->dataPtr->scanPub = this->node->Advertise(this->Topic(), 50); @@ -369,7 +375,11 @@ bool GpuRaySensor::NeedsUpdate() { if (this->useStrictRate) { - double simTime = this->scene->SimTime().Double(); + double simTime; + if (this->scene) + simTime = this->scene->SimTime().Double(); + else + simTime = this->world->SimTime().Double(); if (simTime < this->lastMeasurementTime.Double()) { @@ -653,7 +663,8 @@ int GpuRaySensor::Fiducial(const unsigned int /*_index*/) const ////////////////////////////////////////////////// void GpuRaySensor::PrerenderEnded() { - if (this->dataPtr->laserCam && this->IsActive() && this->NeedsUpdate()) + if (this->useStrictRate && this->dataPtr->laserCam && this->IsActive() && + this->NeedsUpdate()) { // compute next rendering time, take care of the case where period is zero. double dt; From 86c90d2597c6f4dd1987487058dc7d4a6d1dcda5 Mon Sep 17 00:00:00 2001 From: Mabel Zhang Date: Fri, 19 Jun 2020 22:10:17 -0400 Subject: [PATCH 23/46] minor fixes and style Signed-off-by: Mabel Zhang --- gazebo/sensors/GpuRaySensor.cc | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/gazebo/sensors/GpuRaySensor.cc b/gazebo/sensors/GpuRaySensor.cc index fe5976670e..b0a36b6f57 100644 --- a/gazebo/sensors/GpuRaySensor.cc +++ b/gazebo/sensors/GpuRaySensor.cc @@ -99,6 +99,7 @@ void GpuRaySensor::Load(const std::string &_worldName) this->connections.push_back( event::Events::ConnectPreRenderEnded( boost::bind(&GpuRaySensor::PrerenderEnded, this))); + } this->dataPtr->scanPub = this->node->Advertise(this->Topic(), 50); @@ -380,7 +381,7 @@ bool GpuRaySensor::NeedsUpdate() simTime = this->scene->SimTime().Double(); else simTime = this->world->SimTime().Double(); - + if (simTime < this->lastMeasurementTime.Double()) { // Rendering sensors also set the lastMeasurementTime variable in Render() @@ -390,9 +391,9 @@ bool GpuRaySensor::NeedsUpdate() this->ResetLastUpdateTime(); return false; } - + double dt = this->world->Physics()->GetMaxStepSize(); - + // If next rendering time is not set yet if (std::isnan(this->dataPtr->nextRenderingTime)) { @@ -408,10 +409,10 @@ bool GpuRaySensor::NeedsUpdate() return false; } } - + if (simTime > this->dataPtr->nextRenderingTime + dt) return true; - + // Trigger on the tick the closest from the targeted rendering time return (ignition::math::lessOrNearEqual( std::abs(simTime - this->dataPtr->nextRenderingTime), dt / 2.0)); From 8e61753a40fd765eeddc785e301aa99b45ea15c4 Mon Sep 17 00:00:00 2001 From: Mabel Zhang Date: Sat, 20 Jun 2020 01:17:34 -0400 Subject: [PATCH 24/46] MultiCameraSensor test Signed-off-by: Mabel Zhang --- test/integration/camera_sensor.cc | 4 +- test/integration/multicamera_sensor.cc | 71 ++++++++++++------- ...rld => multicamera_strict_rate_test.world} | 36 +++++++++- 3 files changed, 81 insertions(+), 30 deletions(-) rename test/worlds/{multicamera_strict_rate.world => multicamera_strict_rate_test.world} (63%) diff --git a/test/integration/camera_sensor.cc b/test/integration/camera_sensor.cc index b0c3a35598..dba8a21fec 100644 --- a/test/integration/camera_sensor.cc +++ b/test/integration/camera_sensor.cc @@ -302,7 +302,7 @@ TEST_F(CameraSensor, CheckThrottle) } ///////////////////////////////////////////////// -TEST_F(CameraSensor, CheckThrottleStrictRate) +TEST_F(CameraSensor, StrictUpdateRate) { // Load camera_strict_rate.world instead, and don't call SpawnCamera. // That allows us to secify the custom namespace in the world file instead @@ -341,6 +341,8 @@ TEST_F(CameraSensor, CheckThrottleStrictRate) while (imageCount < totalImages) { + // An approximation of when we receive the first image. In reality one + // iteration before we receive the second image. if (imageCount == 0) { simT0 = world->SimTime().Double(); diff --git a/test/integration/multicamera_sensor.cc b/test/integration/multicamera_sensor.cc index 681932eacf..f6199f5c41 100644 --- a/test/integration/multicamera_sensor.cc +++ b/test/integration/multicamera_sensor.cc @@ -569,11 +569,9 @@ TEST_F(MultiCameraSensor, CameraRotationWorldPoseTest) } ///////////////////////////////////////////////// -// TODO(mabelzhang): adapt to MultiCameraSensor. Create SDF file. -/* -TEST_F(MultiCameraSensorTest, StrictUpdateRate) +TEST_F(MultiCameraSensor, StrictUpdateRate) { - Load("worlds/multi_camera_strict_rate.world"); + Load("worlds/multicamera_strict_rate_test.world"); // Make sure the render engine is available. if (rendering::RenderEngine::Instance()->GetRenderPathType() == @@ -583,36 +581,55 @@ TEST_F(MultiCameraSensorTest, StrictUpdateRate) return; } - std::string sensorName = "multi_camera_sensor"; + std::string sensorName = "multicamera_sensor"; sensors::SensorPtr sensor = sensors::get_sensor(sensorName); - sensors::MultiCameraSensorPtr raySensor = + sensors::MultiCameraSensorPtr multiCamSensor = std::dynamic_pointer_cast(sensor); - EXPECT_TRUE(raySensor != nullptr); - raySensor->SetActive(true); - - float *scan = new float[raySensor->RangeCount() - * raySensor->VerticalRangeCount() * 3]; - int scanCount = 0; - event::ConnectionPtr c = - raySensor->ConnectNewLaserFrame( - std::bind(&::OnNewLaserFrame, &scanCount, scan, + EXPECT_TRUE(multiCamSensor != nullptr); + multiCamSensor->SetActive(true); + + // 3 cameras + EXPECT_TRUE(multiCamSensor->CameraCount() == 3u); + int imageCount0 = 0; + unsigned char* img0 = new unsigned char[ + multiCamSensor->ImageWidth(0) * multiCamSensor->ImageHeight(0) * 3]; + event::ConnectionPtr c0 = multiCamSensor->Camera(0)->ConnectNewImageFrame( + std::bind(&::OnNewFrameTest, &imageCount0, img0, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, + std::placeholders::_4, std::placeholders::_5)); + + int imageCount1 = 0; + unsigned char* img1 = new unsigned char[ + multiCamSensor->ImageWidth(1) * multiCamSensor->ImageHeight(1) * 3]; + event::ConnectionPtr c1 = multiCamSensor->Camera(1)->ConnectNewImageFrame( + std::bind(&::OnNewFrameTest, &imageCount1, img1, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, + std::placeholders::_4, std::placeholders::_5)); + + int imageCount2 = 0; + unsigned char* img2 = new unsigned char[ + multiCamSensor->ImageWidth(2) * multiCamSensor->ImageHeight(2) * 3]; + event::ConnectionPtr c2 = multiCamSensor->Camera(2)->ConnectNewImageFrame( + std::bind(&::OnNewFrameTest, &imageCount2, img2, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4, std::placeholders::_5)); - // wait for a few laser scans common::Timer timer; timer.Start(); - // how many scans produced for 5 seconds (in simulated clock domain) - double updateRate = raySensor->UpdateRate(); - int totalScans = 5 * updateRate; + // how many images produced for 5 seconds (in simulated clock domain) + double updateRate = multiCamSensor->UpdateRate(); + int totalImagesPerCam = 5 * updateRate; physics::WorldPtr world = physics::get_world("default"); ASSERT_TRUE(world != NULL); double simT0 = 0.0; - while (scanCount < totalScans) + while (imageCount0 < totalImagesPerCam || imageCount1 < totalImagesPerCam || + imageCount2 < totalImagesPerCam) { - if (scanCount == 0) + // An approximation of when we receive the first image. In reality one + // iteration before we receive the second image. + if (imageCount0 == 0) { simT0 = world->SimTime().Double(); } @@ -621,17 +638,19 @@ TEST_F(MultiCameraSensorTest, StrictUpdateRate) // check that the obtained rate is the one expected double dt = world->SimTime().Double() - simT0; - double rate = static_cast(totalScans) / dt; + double rate = static_cast(totalImagesPerCam) / dt; gzdbg << "timer [" << dt << "] seconds rate [" << rate << "] fps\n"; const double tolerance = 0.02; EXPECT_GT(rate, updateRate * (1 - tolerance)); EXPECT_LT(rate, updateRate * (1 + tolerance)); - c.reset(); - - delete [] scan; + c0.reset(); + c1.reset(); + c2.reset(); + delete [] img0; + delete [] img1; + delete [] img2; } -*/ ///////////////////////////////////////////////// int main(int argc, char **argv) diff --git a/test/worlds/multicamera_strict_rate.world b/test/worlds/multicamera_strict_rate_test.world similarity index 63% rename from test/worlds/multicamera_strict_rate.world rename to test/worlds/multicamera_strict_rate_test.world index 5389783e01..976abc7b1c 100644 --- a/test/worlds/multicamera_strict_rate.world +++ b/test/worlds/multicamera_strict_rate_test.world @@ -8,7 +8,7 @@ model://sun - + true 0 0 0.5 0 0 0 @@ -28,10 +28,40 @@ - + 500 true - + + + 0 -0.1 0.1 0 0 0 + 1.3962634 + + 1024 + 544 + R8G8B8 + + + 0.02 + 100 + + + + + 0 0.1 0.1 0 0 0 + 1.3962634 + + 1024 + 544 + R8G8B8 + + + 0.02 + 100 + + + + + 0 -0.1 -0.1 0 0 0 1.3962634 1024 From ec1916a8eed2cdbaf408ad6c6b76c8ca28b377ae Mon Sep 17 00:00:00 2001 From: Mabel Zhang Date: Sat, 20 Jun 2020 01:20:05 -0400 Subject: [PATCH 25/46] small fixes Signed-off-by: Mabel Zhang --- gazebo/sensors/CameraSensor.cc | 1 - test/integration/camera_sensor.cc | 4 +++- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/gazebo/sensors/CameraSensor.cc b/gazebo/sensors/CameraSensor.cc index 31cfd42a50..283fb909a6 100644 --- a/gazebo/sensors/CameraSensor.cc +++ b/gazebo/sensors/CameraSensor.cc @@ -494,4 +494,3 @@ void CameraSensor::ResetLastUpdateTime() if (this->useStrictRate) this->dataPtr->nextRenderingTime = std::numeric_limits::quiet_NaN(); } - diff --git a/test/integration/camera_sensor.cc b/test/integration/camera_sensor.cc index b0c3a35598..dba8a21fec 100644 --- a/test/integration/camera_sensor.cc +++ b/test/integration/camera_sensor.cc @@ -302,7 +302,7 @@ TEST_F(CameraSensor, CheckThrottle) } ///////////////////////////////////////////////// -TEST_F(CameraSensor, CheckThrottleStrictRate) +TEST_F(CameraSensor, StrictUpdateRate) { // Load camera_strict_rate.world instead, and don't call SpawnCamera. // That allows us to secify the custom namespace in the world file instead @@ -341,6 +341,8 @@ TEST_F(CameraSensor, CheckThrottleStrictRate) while (imageCount < totalImages) { + // An approximation of when we receive the first image. In reality one + // iteration before we receive the second image. if (imageCount == 0) { simT0 = world->SimTime().Double(); From 805c3e9625a3f69d35a6fc0627620e71b4db5fa5 Mon Sep 17 00:00:00 2001 From: Mabel Zhang Date: Sat, 20 Jun 2020 01:52:34 -0400 Subject: [PATCH 26/46] LogicalCameraSensor test Signed-off-by: Mabel Zhang --- test/integration/logical_camera_sensor.cc | 57 +++++++++++++++++++ test/worlds/gpu_ray_strict_rate.world | 2 + test/worlds/logical_camera_strict_rate.world | 32 +++++++++++ .../worlds/multicamera_strict_rate_test.world | 2 + 4 files changed, 93 insertions(+) create mode 100644 test/worlds/logical_camera_strict_rate.world diff --git a/test/integration/logical_camera_sensor.cc b/test/integration/logical_camera_sensor.cc index 01cb9c9238..b5d7575305 100644 --- a/test/integration/logical_camera_sensor.cc +++ b/test/integration/logical_camera_sensor.cc @@ -15,6 +15,8 @@ * */ +#include "gazebo/common/Timer.hh" + #include "gazebo/sensors/sensors.hh" #include "gazebo/sensors/LogicalCameraSensor.hh" @@ -25,6 +27,12 @@ class LogicalCameraSensor : public ServerFixture { }; +///////////////////////////////////////////////// +void OnNewUpdate(int* _msgCounter) +{ + *_msgCounter += 1; +} + ///////////////////////////////////////////////// TEST_F(LogicalCameraSensor, GroundPlane) { @@ -155,6 +163,55 @@ TEST_F(LogicalCameraSensor, NestedModels) EXPECT_EQ("base_target::nested_target", cam->Image().model(0).name()); } +///////////////////////////////////////////////// +TEST_F(LogicalCameraSensor, StrictUpdateRate) +{ + Load("worlds/logical_camera_strict_rate.world"); + + // Wait until the sensors have been initialized + while (!sensors::SensorManager::Instance()->SensorsInitialized()) + common::Time::MSleep(1000); + + std::string cameraName = "logical_camera_sensor"; + sensors::SensorPtr sensor = sensors::get_sensor(cameraName); + sensors::LogicalCameraSensorPtr logCamSensor = + std::dynamic_pointer_cast(sensor); + ASSERT_TRUE(logCamSensor != NULL); + + int msgCount = 0; + event::ConnectionPtr c = logCamSensor->ConnectUpdated( + std::bind(&::OnNewUpdate, &msgCount)); + common::Timer timer; + timer.Start(); + + // how many msgs produced for 5 seconds (in simulated clock domain) + double updateRate = logCamSensor->UpdateRate(); + int totalMsgs = 5 * updateRate; + physics::WorldPtr world = physics::get_world("default"); + ASSERT_TRUE(world != NULL); + double simT0 = 0.0; + + while (msgCount < totalMsgs) + { + // An approximation of when we receive the first image. In reality one + // iteration before we receive the second image. + if (msgCount == 0) + { + simT0 = world->SimTime().Double(); + } + common::Time::MSleep(1); + } + + // check that the obtained rate is the one expected + double dt = world->SimTime().Double() - simT0; + double rate = static_cast(totalMsgs) / dt; + gzdbg << "timer [" << dt << "] seconds rate [" << rate << "] fps\n"; + const double tolerance = 0.02; + EXPECT_GT(rate, updateRate * (1 - tolerance)); + EXPECT_LT(rate, updateRate * (1 + tolerance)); + c.reset(); +} + ///////////////////////////////////////////////// int main(int argc, char **argv) { diff --git a/test/worlds/gpu_ray_strict_rate.world b/test/worlds/gpu_ray_strict_rate.world index 6189aba0ee..4e74620923 100644 --- a/test/worlds/gpu_ray_strict_rate.world +++ b/test/worlds/gpu_ray_strict_rate.world @@ -46,6 +46,8 @@ 1 + 500 true false diff --git a/test/worlds/logical_camera_strict_rate.world b/test/worlds/logical_camera_strict_rate.world new file mode 100644 index 0000000000..07fac6f280 --- /dev/null +++ b/test/worlds/logical_camera_strict_rate.world @@ -0,0 +1,32 @@ + + + + 0 0 0 + + model://sun + + + + 0 0 1 0 0 0 + + + 0 0 1 0 0 0 + + 0.55 + 5 + 1.05 + 1.8 + + + true + true + + + 500 + true + + + + + diff --git a/test/worlds/multicamera_strict_rate_test.world b/test/worlds/multicamera_strict_rate_test.world index 976abc7b1c..73c8177017 100644 --- a/test/worlds/multicamera_strict_rate_test.world +++ b/test/worlds/multicamera_strict_rate_test.world @@ -29,6 +29,8 @@ + 500 true From 744cc843a384360f6e61bb2049b428782a0f8663 Mon Sep 17 00:00:00 2001 From: Mabel Zhang Date: Fri, 26 Jun 2020 00:41:35 -0400 Subject: [PATCH 27/46] add reference to custom XML namespace Signed-off-by: Mabel Zhang --- gazebo/test/ServerFixture.cc | 2 +- test/worlds/camera_strict_rate.world | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/gazebo/test/ServerFixture.cc b/gazebo/test/ServerFixture.cc index a3e46e58b0..a9ef5010cd 100644 --- a/gazebo/test/ServerFixture.cc +++ b/gazebo/test/ServerFixture.cc @@ -581,7 +581,7 @@ void ServerFixture::SpawnCamera(const std::string &_modelName, << "" << _pos << " " << _rpy << "" << "" << " " + << "' type ='camera' xmlns:strict_rate='https://github.com/osrf/gazebo/issues/2736'>" << " 1" << " " << _rate << "" << " " << _strictRate << "" diff --git a/test/worlds/camera_strict_rate.world b/test/worlds/camera_strict_rate.world index 47639f2759..879ffad666 100644 --- a/test/worlds/camera_strict_rate.world +++ b/test/worlds/camera_strict_rate.world @@ -20,7 +20,7 @@ - + 1.0472 From 159624f03b331546ac0543a62917bad5530abb7a Mon Sep 17 00:00:00 2001 From: Mabel Zhang Date: Fri, 26 Jun 2020 00:45:03 -0400 Subject: [PATCH 28/46] add reference to custom XML namespaces Signed-off-by: Mabel Zhang --- test/worlds/gpu_ray_strict_rate.world | 2 +- test/worlds/logical_camera_strict_rate.world | 2 +- test/worlds/multicamera_strict_rate_test.world | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/test/worlds/gpu_ray_strict_rate.world b/test/worlds/gpu_ray_strict_rate.world index 4e74620923..24c73ce69c 100644 --- a/test/worlds/gpu_ray_strict_rate.world +++ b/test/worlds/gpu_ray_strict_rate.world @@ -22,7 +22,7 @@ - + 0.1 0 0.0175 0 -0 0 diff --git a/test/worlds/logical_camera_strict_rate.world b/test/worlds/logical_camera_strict_rate.world index 07fac6f280..aa267a112b 100644 --- a/test/worlds/logical_camera_strict_rate.world +++ b/test/worlds/logical_camera_strict_rate.world @@ -9,7 +9,7 @@ 0 0 1 0 0 0 - + 0 0 1 0 0 0 0.55 diff --git a/test/worlds/multicamera_strict_rate_test.world b/test/worlds/multicamera_strict_rate_test.world index 73c8177017..af408388fa 100644 --- a/test/worlds/multicamera_strict_rate_test.world +++ b/test/worlds/multicamera_strict_rate_test.world @@ -28,7 +28,7 @@ - + 500 From 83491359e67b4d057d26c129b80b7b04ac5fb53b Mon Sep 17 00:00:00 2001 From: Mabel Zhang Date: Mon, 29 Jun 2020 04:01:30 -0400 Subject: [PATCH 29/46] --lockstep command line per Ian Chen Signed-off-by: Mabel Zhang --- gazebo/Server.cc | 22 ++++++++++++++++++++-- gazebo/gazebo_main.cc | 2 ++ gazebo/rendering/RenderingIface.cc | 14 ++++++++++++++ gazebo/rendering/RenderingIface.hh | 16 ++++++++++++++++ gazebo/rendering/Scene.cc | 6 +++--- gazebo/sensors/CameraSensor.cc | 1 + test/integration/camera_sensor.cc | 2 +- 7 files changed, 57 insertions(+), 6 deletions(-) diff --git a/gazebo/Server.cc b/gazebo/Server.cc index 8e14768e96..21e2b5705c 100644 --- a/gazebo/Server.cc +++ b/gazebo/Server.cc @@ -100,6 +100,9 @@ namespace gazebo /// \brief Save argv for access by system plugins. char **systemPluginsArgv; + + /// \brief Set whether to lockstep physics and rendering + bool lockstep = false; }; } @@ -149,6 +152,7 @@ bool Server::ParseArgs(int _argc, char **_argv) ("verbose", "Increase the messages written to the terminal.") ("help,h", "Produce this help message.") ("pause,u", "Start the server in a paused state.") + ("lockstep", "Lockstep simulation so sensor update rates are respected.") ("physics,e", po::value(), "Specify a physics engine (ode|bullet|dart|simbody).") ("play,p", po::value(), "Play a log file.") @@ -282,6 +286,12 @@ bool Server::ParseArgs(int _argc, char **_argv) } } + if (this->dataPtr->vm.count("lockstep")) + { + this->dataPtr->lockstep = true; + rendering::set_lockstep_enabled(this->dataPtr->lockstep); + } + if (!this->PreLoad()) { gzerr << "Unable to load gazebo\n"; @@ -497,7 +507,11 @@ bool Server::LoadImpl(sdf::ElementPtr _elem, << " seconds for namespaces. Giving up.\n"; } - physics::init_worlds(rendering::update_scene_poses); + if (this->dataPtr->lockstep) + physics::init_worlds(rendering::update_scene_poses); + else + physics::init_worlds(nullptr); + this->dataPtr->stop = false; return true; @@ -586,7 +600,8 @@ void Server::Run() // The server and sensor manager outlive worlds while (!this->dataPtr->stop) { - rendering::wait_for_render_request("", 0.100); + if (this->dataPtr->lockstep) + rendering::wait_for_render_request("", 0.100); // bool ret = rendering::wait_for_render_request("", 0.100); // if (ret == false) // gzerr << "time out reached!" << std::endl; @@ -597,6 +612,9 @@ void Server::Run() sensors::run_once(); else if (sensors::running()) sensors::stop(); + + if (!this->dataPtr->lockstep) + common::Time::MSleep(1); } // Shutdown gazebo diff --git a/gazebo/gazebo_main.cc b/gazebo/gazebo_main.cc index bf3b289ea8..78b3f5c104 100644 --- a/gazebo/gazebo_main.cc +++ b/gazebo/gazebo_main.cc @@ -72,6 +72,8 @@ void help() << " -o [ --profile ] arg Physics preset profile name from the " << "options in\n" << " the world file.\n" + << " --lockstep Lockstep simulation so sensor update " + << "rates are respected.\n" << "\n"; } diff --git a/gazebo/rendering/RenderingIface.cc b/gazebo/rendering/RenderingIface.cc index ce8add2592..49be770cdd 100644 --- a/gazebo/rendering/RenderingIface.cc +++ b/gazebo/rendering/RenderingIface.cc @@ -24,6 +24,8 @@ using namespace gazebo; +bool g_lockstep = false; + ////////////////////////////////////////////////// bool rendering::load() { @@ -134,3 +136,15 @@ void rendering::update_scene_poses(const std::string &_name, scene->UpdatePoses(_msg); } } + +////////////////////////////////////////////////// +void rendering::set_lockstep_enabled(bool _enable) +{ + g_lockstep = _enable; +} + +////////////////////////////////////////////////// +bool rendering::lockstep_enabled() +{ + return g_lockstep; +} diff --git a/gazebo/rendering/RenderingIface.hh b/gazebo/rendering/RenderingIface.hh index 5d27a1e181..54fe1431b7 100644 --- a/gazebo/rendering/RenderingIface.hh +++ b/gazebo/rendering/RenderingIface.hh @@ -55,6 +55,22 @@ namespace gazebo void update_scene_poses(const std::string &_name, const msgs::PosesStamped &_msg); + /// \brief Set whether to enable lockstepping for rendering and physics. + /// If enabled, the poses of objects in rendering will be updated via + /// direct API call instead of transport. + /// \param[in] _enable True to enable lockstepping, false to disable + /// \sa update_scene_poses + GZ_RENDERING_VISIBLE + void set_lockstep_enabled(bool _enable); + + /// \brief Get whether or not lockstepping is enabled for rendering and + /// physics. If enabled, the poses of objects in rendering is updated via + /// direct API call instead of transport. + /// \return True if lockstepping is enabled, false if disabled + /// \sa lockstep_enabled + GZ_RENDERING_VISIBLE + bool lockstep_enabled(); + /// \brief wait until a render request occurs /// \param[in] _name Name of the scene to retrieve /// \param[in] _timeoutsec timeout expressed in seconds diff --git a/gazebo/rendering/Scene.cc b/gazebo/rendering/Scene.cc index 35d664c2bb..dda91e1eb7 100644 --- a/gazebo/rendering/Scene.cc +++ b/gazebo/rendering/Scene.cc @@ -62,6 +62,7 @@ #include "gazebo/rendering/TransmitterVisual.hh" #include "gazebo/rendering/SelectionObj.hh" #include "gazebo/rendering/RayQuery.hh" +#include "gazebo/rendering/RenderingIface.hh" #if OGRE_VERSION_MAJOR >= 1 && OGRE_VERSION_MINOR >= 8 #include "gazebo/rendering/deferred_shading/SSAOLogic.hh" @@ -157,14 +158,13 @@ Scene::Scene(const std::string &_name, const bool _enableVisualizations, &Scene::OnLightModifyMsg, this); this->dataPtr->isServer = _isServer; - /* - if (_isServer) + + if (_isServer && !rendering::lockstep_enabled()) { this->dataPtr->poseSub = this->dataPtr->node->Subscribe("~/pose/local/info", &Scene::OnPoseMsg, this); } else - */ // When ready to use the direct API for updating scene poses from server, // uncomment the following line and delete the if and else directly above if (!_isServer) diff --git a/gazebo/sensors/CameraSensor.cc b/gazebo/sensors/CameraSensor.cc index 283fb909a6..fcfc505502 100644 --- a/gazebo/sensors/CameraSensor.cc +++ b/gazebo/sensors/CameraSensor.cc @@ -67,6 +67,7 @@ CameraSensor::~CameraSensor() void CameraSensor::Load(const std::string &_worldName, sdf::ElementPtr _sdf) { Sensor::Load(_worldName, _sdf); + this->useStrictRate = this->useStrictRate && rendering::lockstep_enabled(); // strict_rate parameter is parsed in Sensor::Load() if (this->useStrictRate) { diff --git a/test/integration/camera_sensor.cc b/test/integration/camera_sensor.cc index dba8a21fec..b3965f572f 100644 --- a/test/integration/camera_sensor.cc +++ b/test/integration/camera_sensor.cc @@ -307,7 +307,7 @@ TEST_F(CameraSensor, StrictUpdateRate) // Load camera_strict_rate.world instead, and don't call SpawnCamera. // That allows us to secify the custom namespace in the world file instead // of modifying SpawnCamera() in ServerFixture.cc. - Load("worlds/camera_strict_rate.world"); + LoadArgs(" --lockstep worlds/camera_strict_rate.world"); // Make sure the render engine is available. if (rendering::RenderEngine::Instance()->GetRenderPathType() == From d88307096a6266a59c28538713a8318328d0d8d8 Mon Sep 17 00:00:00 2001 From: Mabel Zhang Date: Mon, 29 Jun 2020 06:09:25 -0400 Subject: [PATCH 30/46] lockstep test for contact sensor and laser Signed-off-by: Mabel Zhang --- test/integration/contact_sensor.cc | 57 ++++++ test/integration/laser.cc | 61 +++++++ test/worlds/contact_strict_rate.world | 174 +++++++++++++++++++ test/worlds/laser_hit_strict_rate_test.world | 58 +++++++ 4 files changed, 350 insertions(+) create mode 100644 test/worlds/contact_strict_rate.world create mode 100644 test/worlds/laser_hit_strict_rate_test.world diff --git a/test/integration/contact_sensor.cc b/test/integration/contact_sensor.cc index 5c96ff4b45..d4d5a0e92b 100644 --- a/test/integration/contact_sensor.cc +++ b/test/integration/contact_sensor.cc @@ -41,6 +41,7 @@ class ContactSensor : public ServerFixture, public: void MultipleSensors(const std::string &_physicsEngine); public: void StackTest(const std::string &_physicsEngine); public: void TorqueTest(const std::string &_physicsEngine); + public: void StrictUpdateRateTest(const std::string &_physicsEngine); /// \brief Callback for sensor subscribers in MultipleSensors test. private: void Callback(const ConstContactsPtr &_msg); @@ -685,6 +686,62 @@ TEST_P(ContactSensor, TorqueTest) TorqueTest(GetParam()); } +//////////////////////////////////////////////////////////////////////// +// Test strict update rate of contact sensor. +//////////////////////////////////////////////////////////////////////// +void ContactSensor::StrictUpdateRateTest(const std::string &_physicsEngine) +{ + Load("worlds/contact_strict_rate.world", true, _physicsEngine); + + // Wait until the sensors have been initialized + while (!sensors::SensorManager::Instance()->SensorsInitialized()) + common::Time::MSleep(1000); + + std::string contactSensorName = "sensor_contact"; + sensors::SensorPtr sensor = sensors::get_sensor(contactSensorName); + sensors::ContactSensorPtr contactSensor = + std::dynamic_pointer_cast(sensor); + ASSERT_TRUE(contactSensor != NULL); + + g_messageCount = 0; + transport::SubscriberPtr sub = this->node->Subscribe("/gazebo/default/physics/contacts", + &ContactSensor::Callback, this); + common::Timer timer; + SetPause(false); + timer.Start(); + + // how many msgs produced for 5 seconds (in simulated clock domain) + double updateRate = contactSensor->UpdateRate(); + unsigned int totalMsgs = 5 * updateRate; + physics::WorldPtr world = physics::get_world("default"); + ASSERT_TRUE(world != NULL); + double simT0 = 0.0; + + while (g_messageCount < totalMsgs) + { + // An approximation of when we receive the first image. In reality one + // iteration before we receive the second image. + if (g_messageCount == 0) + { + simT0 = world->SimTime().Double(); + } + common::Time::MSleep(1); + } + + // check that the obtained rate is the one expected + double dt = world->SimTime().Double() - simT0; + double rate = static_cast(totalMsgs) / dt; + gzdbg << "timer [" << dt << "] seconds rate [" << rate << "] fps\n"; + const double tolerance = 0.02; + EXPECT_GT(rate, updateRate * (1 - tolerance)); + EXPECT_LT(rate, updateRate * (1 + tolerance)); +} + +TEST_P(ContactSensor, StrictUpdateRateTest) +{ + StrictUpdateRateTest(GetParam()); +} + INSTANTIATE_TEST_CASE_P(PhysicsEngines, ContactSensor, PHYSICS_ENGINE_VALUES,); // NOLINT int main(int argc, char **argv) diff --git a/test/integration/laser.cc b/test/integration/laser.cc index 694fda4104..bb98324822 100644 --- a/test/integration/laser.cc +++ b/test/integration/laser.cc @@ -37,8 +37,16 @@ class LaserTest : public ServerFixture, public: void LaserUnitNoise(const std::string &_physicsEngine); public: void LaserVertical(const std::string &_physicsEngine); public: void LaserScanResolution(const std::string &_physicsEngine); + public: void LaserStrictUpdateRate(const std::string &_physicsEngine); + + private: void OnNewUpdate(int* _msgCounter); }; +void LaserTest::OnNewUpdate(int* _msgCounter) +{ + *_msgCounter += 1; +} + void LaserTest::Stationary_EmptyWorld(const std::string &_physicsEngine) { if (_physicsEngine == "dart") @@ -608,6 +616,59 @@ TEST_P(LaserTest, LaserNoise) LaserUnitNoise(GetParam()); } +void LaserTest::LaserStrictUpdateRate(const std::string &_physicsEngine) +{ + Load("worlds/laser_hit_strict_rate_test.world"); + + // Wait until the sensors have been initialized + while (!sensors::SensorManager::Instance()->SensorsInitialized()) + common::Time::MSleep(1000); + + std::string rayName = "laser"; + sensors::SensorPtr sensor = sensors::get_sensor(rayName); + sensors::RaySensorPtr raySensor = + std::dynamic_pointer_cast(sensor); + ASSERT_TRUE(raySensor != NULL); + + int msgCount = 0; + event::ConnectionPtr c = raySensor->ConnectUpdated( + std::bind(&LaserTest::OnNewUpdate, this, &msgCount)); + common::Timer timer; + timer.Start(); + + // how many msgs produced for 5 seconds (in simulated clock domain) + double updateRate = raySensor->UpdateRate(); + int totalMsgs = 5 * updateRate; + physics::WorldPtr world = physics::get_world("default"); + ASSERT_TRUE(world != NULL); + double simT0 = 0.0; + + while (msgCount < totalMsgs) + { + // An approximation of when we receive the first image. In reality one + // iteration before we receive the second image. + if (msgCount == 0) + { + simT0 = world->SimTime().Double(); + } + common::Time::MSleep(1); + } + + // check that the obtained rate is the one expected + double dt = world->SimTime().Double() - simT0; + double rate = static_cast(totalMsgs) / dt; + gzdbg << "timer [" << dt << "] seconds rate [" << rate << "] fps\n"; + const double tolerance = 0.02; + EXPECT_GT(rate, updateRate * (1 - tolerance)); + EXPECT_LT(rate, updateRate * (1 + tolerance)); + c.reset(); +} + +TEST_P(LaserTest, LaserStrictUpdateRate) +{ + LaserUnitNoise(GetParam()); +} + INSTANTIATE_TEST_CASE_P(PhysicsEngines, LaserTest, PHYSICS_ENGINE_VALUES,); // NOLINT int main(int argc, char **argv) diff --git a/test/worlds/contact_strict_rate.world b/test/worlds/contact_strict_rate.world new file mode 100644 index 0000000000..1e6f7e0911 --- /dev/null +++ b/test/worlds/contact_strict_rate.world @@ -0,0 +1,174 @@ + + + + + 1 + 0.4 0.4 0.4 1 + 0.7 0.7 0.7 1 + + + 250 + + + quick + 20 + 1.3 + 0 + + + 0 + 0.2 + 100 + 0 + + + 0 0 -10 + 0.001 + 1 + 1000 + 5.5645e-06 2.28758e-05 -4.23884e-05 + + + 0 0 2 0 -0 0 + + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + + 1 + 0 + 0 + 1 + 0 + 1 + + 10 + + + 0 0 0 0 -0 0 + + + 1 1 2 + + + 1 + 100 + + + 0 0 0 0 -0 0 + 250 + + + 1 1 2 + + + + + + 0.5 + 0.2 + 1 0 0 + 0 + 0 + + + + 0 + 1e+06 + + + + 0 + 0.2 + 1e+15 + 1e+13 + 100 + 0.0001 + + + + 100 + + + 0 0 0 0 -0 0 + + __default__ + __default_topic__ + + 1 + 1000 + true + + 1 + 1 + 0 + + 0 + 0 + + + 0 + + + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + + + 0 0 1 + 1 1 + + + + + + 50 + 50 + 0 0 0 + 0 + 0 + + + + 0 + 0 + + + + 0 + 0 + 1e+09 + 1 + -1 + 0 + + + + 0 + + + 0 0 0 0 -0 0 + + + 0 0 1 + 1 1 + + + 1 + 0 + 0 + + 1 + 0 + 0 + + 0 + 0 + + + 1 + + + diff --git a/test/worlds/laser_hit_strict_rate_test.world b/test/worlds/laser_hit_strict_rate_test.world new file mode 100644 index 0000000000..3bdc88416d --- /dev/null +++ b/test/worlds/laser_hit_strict_rate_test.world @@ -0,0 +1,58 @@ + + + + + 1 1 1 1 + 1 1 1 1 + false + false + + + + + 2 0 1.0 0 1.5707 0 + + + + + true + 8 0 0.5 0 0 0 + + + + + 1 20 1 + + + + + + + + true + + + + + + 4 + 1 + -2.26889 + 2.268899 + + + + 0.0 + 20 + 0.01 + + + 1 + 500 + true + true + + + + + From 1f39fd5f7b018325d9c461c1d85cd6314183f849 Mon Sep 17 00:00:00 2001 From: Mabel Zhang Date: Mon, 29 Jun 2020 06:17:02 -0400 Subject: [PATCH 31/46] style Signed-off-by: Mabel Zhang --- gazebo/sensors/GpuRaySensor.cc | 6 +++--- gazebo/sensors/Sensor.cc | 8 ++++---- gazebo/test/ServerFixture.cc | 3 ++- test/integration/contact_sensor.cc | 4 ++-- test/integration/multicamera_sensor.cc | 2 +- 5 files changed, 12 insertions(+), 11 deletions(-) diff --git a/gazebo/sensors/GpuRaySensor.cc b/gazebo/sensors/GpuRaySensor.cc index ab7d37ddcc..faba9d3ab3 100644 --- a/gazebo/sensors/GpuRaySensor.cc +++ b/gazebo/sensors/GpuRaySensor.cc @@ -690,7 +690,7 @@ void GpuRaySensor::Render() { if (!this->dataPtr->renderNeeded) return; - + this->dataPtr->laserCam->Render(); this->dataPtr->rendered = true; this->dataPtr->renderNeeded = false; @@ -699,9 +699,9 @@ void GpuRaySensor::Render() { if (!this->dataPtr->laserCam || !this->IsActive() || !this->NeedsUpdate()) return; - + this->lastMeasurementTime = this->scene->SimTime(); - + this->dataPtr->laserCam->Render(); this->dataPtr->rendered = true; } diff --git a/gazebo/sensors/Sensor.cc b/gazebo/sensors/Sensor.cc index 13c27e54d1..0183f30d51 100644 --- a/gazebo/sensors/Sensor.cc +++ b/gazebo/sensors/Sensor.cc @@ -255,10 +255,10 @@ void Sensor::Update(const bool _force) if (simTime <= this->lastUpdateTime && !_force) return; - // Adjust time-to-update period to compensate for delays caused by another - // sensor's update in the same thread. - // NOTE: If you change this equation, also change the matching equation in - // Sensor::NeedsUpdate + // Adjust time-to-update period to compensate for delays caused by + // another sensor's update in the same thread. + // NOTE: If you change this equation, also change the matching equation + // in Sensor::NeedsUpdate common::Time adjustedElapsed = simTime - this->lastUpdateTime + this->dataPtr->updateDelay; diff --git a/gazebo/test/ServerFixture.cc b/gazebo/test/ServerFixture.cc index a9ef5010cd..eecfeb1eff 100644 --- a/gazebo/test/ServerFixture.cc +++ b/gazebo/test/ServerFixture.cc @@ -581,7 +581,8 @@ void ServerFixture::SpawnCamera(const std::string &_modelName, << "" << _pos << " " << _rpy << "" << "" << " " + << "' type ='camera' " + << "xmlns:strict_rate='https://github.com/osrf/gazebo/issues/2736'>" << " 1" << " " << _rate << "" << " " << _strictRate << "" diff --git a/test/integration/contact_sensor.cc b/test/integration/contact_sensor.cc index d4d5a0e92b..48e4c686f2 100644 --- a/test/integration/contact_sensor.cc +++ b/test/integration/contact_sensor.cc @@ -704,8 +704,8 @@ void ContactSensor::StrictUpdateRateTest(const std::string &_physicsEngine) ASSERT_TRUE(contactSensor != NULL); g_messageCount = 0; - transport::SubscriberPtr sub = this->node->Subscribe("/gazebo/default/physics/contacts", - &ContactSensor::Callback, this); + transport::SubscriberPtr sub = this->node->Subscribe( + "/gazebo/default/physics/contacts", &ContactSensor::Callback, this); common::Timer timer; SetPause(false); timer.Start(); diff --git a/test/integration/multicamera_sensor.cc b/test/integration/multicamera_sensor.cc index f6199f5c41..43854893df 100644 --- a/test/integration/multicamera_sensor.cc +++ b/test/integration/multicamera_sensor.cc @@ -589,7 +589,7 @@ TEST_F(MultiCameraSensor, StrictUpdateRate) multiCamSensor->SetActive(true); // 3 cameras - EXPECT_TRUE(multiCamSensor->CameraCount() == 3u); + EXPECT_EQ(multiCamSensor->CameraCount(), 3u); int imageCount0 = 0; unsigned char* img0 = new unsigned char[ multiCamSensor->ImageWidth(0) * multiCamSensor->ImageHeight(0) * 3]; From d9affaef4943531921c4f57acc8c7970e7d9f061 Mon Sep 17 00:00:00 2001 From: Mabel Zhang Date: Thu, 9 Jul 2020 03:56:21 -0400 Subject: [PATCH 32/46] fix hanging tests because rendering:: flag not reset after lockstep test Signed-off-by: Mabel Zhang --- gazebo/Server.cc | 2 +- gazebo/rendering/Scene.cc | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/gazebo/Server.cc b/gazebo/Server.cc index 21e2b5705c..70b16ea850 100644 --- a/gazebo/Server.cc +++ b/gazebo/Server.cc @@ -289,8 +289,8 @@ bool Server::ParseArgs(int _argc, char **_argv) if (this->dataPtr->vm.count("lockstep")) { this->dataPtr->lockstep = true; - rendering::set_lockstep_enabled(this->dataPtr->lockstep); } + rendering::set_lockstep_enabled(this->dataPtr->lockstep); if (!this->PreLoad()) { diff --git a/gazebo/rendering/Scene.cc b/gazebo/rendering/Scene.cc index dda91e1eb7..e58174d3ca 100644 --- a/gazebo/rendering/Scene.cc +++ b/gazebo/rendering/Scene.cc @@ -164,7 +164,7 @@ Scene::Scene(const std::string &_name, const bool _enableVisualizations, this->dataPtr->poseSub = this->dataPtr->node->Subscribe("~/pose/local/info", &Scene::OnPoseMsg, this); } - else + // When ready to use the direct API for updating scene poses from server, // uncomment the following line and delete the if and else directly above if (!_isServer) From e1ca9f3cf9cd3e1df34125637b0eb4fa4b2253ab Mon Sep 17 00:00:00 2001 From: Mabel Zhang Date: Thu, 9 Jul 2020 04:53:57 -0400 Subject: [PATCH 33/46] remove custom sdf tag Signed-off-by: Mabel Zhang --- gazebo/physics/World.cc | 2 ++ gazebo/sensors/CameraSensor.cc | 5 ++--- gazebo/sensors/Sensor.cc | 2 +- gazebo/test/ServerFixture.cc | 5 ++--- gazebo/test/ServerFixture.hh | 4 +--- test/worlds/camera_strict_rate.world | 5 ++--- 6 files changed, 10 insertions(+), 13 deletions(-) diff --git a/gazebo/physics/World.cc b/gazebo/physics/World.cc index ee9f78eb4b..7b4e3ca971 100644 --- a/gazebo/physics/World.cc +++ b/gazebo/physics/World.cc @@ -159,6 +159,8 @@ World::World(const std::string &_name) event::Events::ConnectPause( std::bind(&World::SetPaused, this, std::placeholders::_1))); + this->dataPtr->waitForSensors = nullptr; + // Make sure dbs are initialized common::ModelDatabase::Instance(); common::FuelModelDatabase::Instance(); diff --git a/gazebo/sensors/CameraSensor.cc b/gazebo/sensors/CameraSensor.cc index fcfc505502..1357ddd05f 100644 --- a/gazebo/sensors/CameraSensor.cc +++ b/gazebo/sensors/CameraSensor.cc @@ -67,8 +67,7 @@ CameraSensor::~CameraSensor() void CameraSensor::Load(const std::string &_worldName, sdf::ElementPtr _sdf) { Sensor::Load(_worldName, _sdf); - this->useStrictRate = this->useStrictRate && rendering::lockstep_enabled(); - // strict_rate parameter is parsed in Sensor::Load() + // useStrictRate is set in Sensor::Load() if (this->useStrictRate) { this->connections.push_back( @@ -101,7 +100,7 @@ std::string CameraSensor::TopicIgn() const void CameraSensor::Load(const std::string &_worldName) { Sensor::Load(_worldName); - // strict_rate parameter is parsed in Sensor::Load() + // useStrictRate is set in Sensor::Load() if (this->useStrictRate) { this->connections.push_back( diff --git a/gazebo/sensors/Sensor.cc b/gazebo/sensors/Sensor.cc index 5c2641a108..4b12f127b8 100644 --- a/gazebo/sensors/Sensor.cc +++ b/gazebo/sensors/Sensor.cc @@ -139,7 +139,7 @@ void Sensor::Load(const std::string &_worldName) if (this->sdf->Get("always_on")) this->SetActive(true); - this->useStrictRate = this->sdf->Get("strict_rate:value", false).first; + this->useStrictRate = rendering::lockstep_enabled(); if (this->dataPtr->category == IMAGE) this->scene = rendering::get_scene(_worldName); diff --git a/gazebo/test/ServerFixture.cc b/gazebo/test/ServerFixture.cc index a9ef5010cd..699948d39a 100644 --- a/gazebo/test/ServerFixture.cc +++ b/gazebo/test/ServerFixture.cc @@ -570,7 +570,7 @@ void ServerFixture::SpawnCamera(const std::string &_modelName, const std::string &_noiseType, double _noiseMean, double _noiseStdDev, bool _distortion, double _distortionK1, double _distortionK2, double _distortionK3, double _distortionP1, double _distortionP2, - double _cx, double _cy, bool _strictRate) + double _cx, double _cy) { msgs::Factory msg; std::ostringstream newModelStr; @@ -581,10 +581,9 @@ void ServerFixture::SpawnCamera(const std::string &_modelName, << "" << _pos << " " << _rpy << "" << "" << " " + << "' type ='camera'>" << " 1" << " " << _rate << "" - << " " << _strictRate << "" << " true" << " " << " 0.78539816339744828" diff --git a/gazebo/test/ServerFixture.hh b/gazebo/test/ServerFixture.hh index cf9d788943..00d9542424 100644 --- a/gazebo/test/ServerFixture.hh +++ b/gazebo/test/ServerFixture.hh @@ -254,7 +254,6 @@ namespace gazebo /// \param[in] _distortionP2 Distortion coefficient p2. /// \param[in] _cx Normalized optical center x, used for distortion. /// \param[in] _cy Normalized optical center y, used for distortion. - /// \param[in] _strictRate Flag to enable strict update rate adherence. protected: void SpawnCamera(const std::string &_modelName, const std::string &_cameraName, const ignition::math::Vector3d &_pos = @@ -268,8 +267,7 @@ namespace gazebo bool _distortion = false, double _distortionK1 = 0.0, double _distortionK2 = 0.0, double _distortionK3 = 0.0, double _distortionP1 = 0.0, double _distortionP2 = 0.0, - double _cx = 0.5, double _cy = 0.5, - bool _strictRate = false); + double _cx = 0.5, double _cy = 0.5); /// \brief Spawn a wide angle camera. /// \param[in] _modelName Name of the model. diff --git a/test/worlds/camera_strict_rate.world b/test/worlds/camera_strict_rate.world index 879ffad666..dec5c2e3a1 100644 --- a/test/worlds/camera_strict_rate.world +++ b/test/worlds/camera_strict_rate.world @@ -20,7 +20,7 @@ - + 1.0472 @@ -30,9 +30,8 @@ 1 + of lockstep. --> 500 - true true From 66d2811d15d59bc0e8649ada8665dc418cc2a90b Mon Sep 17 00:00:00 2001 From: Mabel Zhang Date: Thu, 9 Jul 2020 20:25:21 -0400 Subject: [PATCH 34/46] remove from other sensor tests Signed-off-by: Mabel Zhang --- test/integration/contact_sensor.cc | 3 ++- test/integration/gpu_laser.cc | 2 +- test/integration/laser.cc | 2 +- test/integration/logical_camera_sensor.cc | 2 +- test/integration/multicamera_sensor.cc | 2 +- test/worlds/contact_strict_rate.world | 3 +-- test/worlds/gpu_ray_strict_rate.world | 5 ++--- test/worlds/laser_hit_strict_rate_test.world | 3 +-- test/worlds/logical_camera_strict_rate.world | 5 ++--- test/worlds/multicamera_strict_rate_test.world | 5 ++--- 10 files changed, 14 insertions(+), 18 deletions(-) diff --git a/test/integration/contact_sensor.cc b/test/integration/contact_sensor.cc index 48e4c686f2..4cb7774f19 100644 --- a/test/integration/contact_sensor.cc +++ b/test/integration/contact_sensor.cc @@ -691,7 +691,8 @@ TEST_P(ContactSensor, TorqueTest) //////////////////////////////////////////////////////////////////////// void ContactSensor::StrictUpdateRateTest(const std::string &_physicsEngine) { - Load("worlds/contact_strict_rate.world", true, _physicsEngine); + LoadArgs(" --lockstep -u -e " + _physicsEngine + + " worlds/contact_strict_rate.world"); // Wait until the sensors have been initialized while (!sensors::SensorManager::Instance()->SensorsInitialized()) diff --git a/test/integration/gpu_laser.cc b/test/integration/gpu_laser.cc index 3dde8baff3..a4aa062d17 100644 --- a/test/integration/gpu_laser.cc +++ b/test/integration/gpu_laser.cc @@ -613,7 +613,7 @@ TEST_F(GPURaySensorTest, LaserScanResolution) TEST_F(GPURaySensorTest, StrictUpdateRate) { - Load("worlds/gpu_ray_strict_rate.world"); + LoadArgs(" --lockstep worlds/gpu_ray_strict_rate.world"); // Make sure the render engine is available. if (rendering::RenderEngine::Instance()->GetRenderPathType() == diff --git a/test/integration/laser.cc b/test/integration/laser.cc index bb98324822..8af5a48887 100644 --- a/test/integration/laser.cc +++ b/test/integration/laser.cc @@ -618,7 +618,7 @@ TEST_P(LaserTest, LaserNoise) void LaserTest::LaserStrictUpdateRate(const std::string &_physicsEngine) { - Load("worlds/laser_hit_strict_rate_test.world"); + LoadArgs(" --lockstep worlds/laser_hit_strict_rate_test.world"); // Wait until the sensors have been initialized while (!sensors::SensorManager::Instance()->SensorsInitialized()) diff --git a/test/integration/logical_camera_sensor.cc b/test/integration/logical_camera_sensor.cc index b5d7575305..bd4a528e71 100644 --- a/test/integration/logical_camera_sensor.cc +++ b/test/integration/logical_camera_sensor.cc @@ -166,7 +166,7 @@ TEST_F(LogicalCameraSensor, NestedModels) ///////////////////////////////////////////////// TEST_F(LogicalCameraSensor, StrictUpdateRate) { - Load("worlds/logical_camera_strict_rate.world"); + LoadArgs(" --lockstep worlds/logical_camera_strict_rate.world"); // Wait until the sensors have been initialized while (!sensors::SensorManager::Instance()->SensorsInitialized()) diff --git a/test/integration/multicamera_sensor.cc b/test/integration/multicamera_sensor.cc index 43854893df..b6336756be 100644 --- a/test/integration/multicamera_sensor.cc +++ b/test/integration/multicamera_sensor.cc @@ -571,7 +571,7 @@ TEST_F(MultiCameraSensor, CameraRotationWorldPoseTest) ///////////////////////////////////////////////// TEST_F(MultiCameraSensor, StrictUpdateRate) { - Load("worlds/multicamera_strict_rate_test.world"); + LoadArgs(" --lockstep worlds/multicamera_strict_rate_test.world"); // Make sure the render engine is available. if (rendering::RenderEngine::Instance()->GetRenderPathType() == diff --git a/test/worlds/contact_strict_rate.world b/test/worlds/contact_strict_rate.world index 1e6f7e0911..f6747b9778 100644 --- a/test/worlds/contact_strict_rate.world +++ b/test/worlds/contact_strict_rate.world @@ -89,7 +89,7 @@ 100 - + 0 0 0 0 -0 0 __default__ @@ -97,7 +97,6 @@ 1 1000 - true 1 1 diff --git a/test/worlds/gpu_ray_strict_rate.world b/test/worlds/gpu_ray_strict_rate.world index 24c73ce69c..c1714cce96 100644 --- a/test/worlds/gpu_ray_strict_rate.world +++ b/test/worlds/gpu_ray_strict_rate.world @@ -22,7 +22,7 @@ - + 0.1 0 0.0175 0 -0 0 @@ -47,9 +47,8 @@ 1 + of lockstep. --> 500 - true false diff --git a/test/worlds/laser_hit_strict_rate_test.world b/test/worlds/laser_hit_strict_rate_test.world index 3bdc88416d..7e9e5baf31 100644 --- a/test/worlds/laser_hit_strict_rate_test.world +++ b/test/worlds/laser_hit_strict_rate_test.world @@ -31,7 +31,7 @@ true - + @@ -49,7 +49,6 @@ 1 500 - true true diff --git a/test/worlds/logical_camera_strict_rate.world b/test/worlds/logical_camera_strict_rate.world index aa267a112b..8092eb767a 100644 --- a/test/worlds/logical_camera_strict_rate.world +++ b/test/worlds/logical_camera_strict_rate.world @@ -9,7 +9,7 @@ 0 0 1 0 0 0 - + 0 0 1 0 0 0 0.55 @@ -22,9 +22,8 @@ true + of lockstep. --> 500 - true diff --git a/test/worlds/multicamera_strict_rate_test.world b/test/worlds/multicamera_strict_rate_test.world index af408388fa..ae764a227a 100644 --- a/test/worlds/multicamera_strict_rate_test.world +++ b/test/worlds/multicamera_strict_rate_test.world @@ -28,11 +28,10 @@ - + + of lockstep. --> 500 - true 0 -0.1 0.1 0 0 0 From e116de584af04d839b67b03a7f1700b051143404 Mon Sep 17 00:00:00 2001 From: Mabel Zhang Date: Fri, 10 Jul 2020 03:49:13 -0400 Subject: [PATCH 35/46] imu strict rate test Signed-off-by: Mabel Zhang --- test/integration/contact_sensor.cc | 2 +- test/integration/imu.cc | 70 +++++++++++ test/worlds/imu_strict_rate_test.world | 167 +++++++++++++++++++++++++ 3 files changed, 238 insertions(+), 1 deletion(-) create mode 100644 test/worlds/imu_strict_rate_test.world diff --git a/test/integration/contact_sensor.cc b/test/integration/contact_sensor.cc index 4cb7774f19..2aec9c533a 100644 --- a/test/integration/contact_sensor.cc +++ b/test/integration/contact_sensor.cc @@ -706,7 +706,7 @@ void ContactSensor::StrictUpdateRateTest(const std::string &_physicsEngine) g_messageCount = 0; transport::SubscriberPtr sub = this->node->Subscribe( - "/gazebo/default/physics/contacts", &ContactSensor::Callback, this); + "/gazebo/default/physics/contacts", &ContactSensor::Callback, this); common::Timer timer; SetPause(false); timer.Start(); diff --git a/test/integration/imu.cc b/test/integration/imu.cc index 9eb098f923..3de2120fa6 100644 --- a/test/integration/imu.cc +++ b/test/integration/imu.cc @@ -47,6 +47,11 @@ class ImuTest : public ServerFixture, /// in the empty world. Test basic IMU outputs with bias enabled. public: void Stationary_EmptyWorld_Bias(const std::string &_physicsEngine); + /// \brief Spawn a static model with an ImuSensor attached + /// in the empty world. Test basic IMU outputs with strict update rate. + public: void Stationary_EmptyWorld_StrictRate( + const std::string &_physicsEngine); + /// \brief Return gravity rotated by some orientation /// \param[in] _rot User specified rotation /// \param[out] _g gravity in user specified orientation @@ -65,6 +70,10 @@ class ImuTest : public ServerFixture, ignition::math::Vector3d &_rateMean, ignition::math::Vector3d &_accelMean, ignition::math::Quaterniond &_orientation); + + /// \brief Increment message counter + /// \param[out] _msgCounter number of messages received so far + private: void OnNewUpdate(int* _msgCounter); }; void ImuTest::GetGravity(const ignition::math::Quaterniond &_rot, @@ -109,6 +118,11 @@ void ImuTest::GetImuData(sensors::ImuSensorPtr _imu, _orientation = _imu->Orientation(); } +void ImuTest::OnNewUpdate(int* _msgCounter) +{ + *_msgCounter += 1; +} + void ImuTest::ImuSensorTestWorld(const std::string &_physicsEngine) { if (_physicsEngine != "ode") @@ -834,6 +848,62 @@ TEST_P(ImuTest, EmptyWorldBias) Stationary_EmptyWorld_Bias(GetParam()); } +void ImuTest::Stationary_EmptyWorld_StrictRate( + const std::string &_physicsEngine) +{ + LoadArgs(" --lockstep -u -e " + _physicsEngine + + " worlds/imu_strict_rate_test.world"); + + // Wait until the sensors have been initialized + while (!sensors::SensorManager::Instance()->SensorsInitialized()) + common::Time::MSleep(1000); + + std::string imuName = "imu_sensor"; + sensors::SensorPtr sensor = sensors::get_sensor(imuName); + sensors::ImuSensorPtr imuSensor = + std::dynamic_pointer_cast(sensor); + ASSERT_TRUE(imuSensor != NULL); + + int msgCount = 0; + event::ConnectionPtr c = imuSensor->ConnectUpdated( + std::bind(&ImuTest::OnNewUpdate, this, &msgCount)); + common::Timer timer; + SetPause(false); + timer.Start(); + + // how many msgs produced for 5 seconds (in simulated clock domain) + double updateRate = imuSensor->UpdateRate(); + int totalMsgs = 5 * updateRate; + physics::WorldPtr world = physics::get_world("default"); + ASSERT_TRUE(world != NULL); + double simT0 = 0.0; + + while (msgCount < totalMsgs) + { + // An approximation of when we receive the first image. In reality one + // iteration before we receive the second image. + if (msgCount == 0) + { + simT0 = world->SimTime().Double(); + } + common::Time::MSleep(1); + } + + // check that the obtained rate is the one expected + double dt = world->SimTime().Double() - simT0; + double rate = static_cast(totalMsgs) / dt; + gzdbg << "timer [" << dt << "] seconds rate [" << rate << "] fps\n"; + const double tolerance = 0.02; + EXPECT_GT(rate, updateRate * (1 - tolerance)); + EXPECT_LT(rate, updateRate * (1 + tolerance)); + c.reset(); +} + +TEST_P(ImuTest, StrictUpdateRate) +{ + Stationary_EmptyWorld_StrictRate(GetParam()); +} + INSTANTIATE_TEST_CASE_P(PhysicsEngines, ImuTest, PHYSICS_ENGINE_VALUES,); // NOLINT int main(int argc, char **argv) diff --git a/test/worlds/imu_strict_rate_test.world b/test/worlds/imu_strict_rate_test.world new file mode 100644 index 0000000000..45b50961fe --- /dev/null +++ b/test/worlds/imu_strict_rate_test.world @@ -0,0 +1,167 @@ + + + + + + + model://sun + + + model://ground_plane + + + + 0.0 0.0 10.2 1.57079633 0.0 0.0 + + 0.0 0.0 0.0 0.0 0.0 0.0 + + 0.0 0.0 -10.0 0.0 0.0 0.0 + + 1e-1 + 0.0 + 0.0 + 1e-1 + 0.0 + 1e-1 + + 5.0 + + + 0.0 + 0.0 + + + 0.0 0.0 -5.0 0.0 0.0 0.0 + + + 0.05 + 10.0 + + + + + + + + 0.0 0.0 -5.0 0.0 0.0 0.0 + 250 + + + 0.05 + 10.0 + + + + true + true + false + + + world + link_pendulum_rod + 0.0 0.0 0.0 0.0 0.0 0.0 + + 1.0 0.0 0.0 + + + + 0.0 0.0 -10.0 0.0 0.0 0.0 + + 0.0 0.0 0.0 0.0 0.0 0.0 + + 1e-1 + 0.0 + 0.0 + 1e-1 + 0.0 + 1e-1 + + 5.0 + + + 0.0 + 0.0 + + + 0.0 0.0 0.0 0.0 0.0 0.0 + + + 0.1 + + + + + + + + 0.0 0.0 0.0 0.0 0.0 0.0 + 250 + + + 0.1 + + + + true + true + false + + 1 + + 1 + + + 1000 + + + + link_pendulum_rod + link_pendulum_ball + 0.0 0.0 0.0 0.0 0.0 0.0 + + + 0 + 0 + 0 + 0 + + 0 0 1 + + + false + + + From 484f0ae540592920954230191088f71dbef1f444 Mon Sep 17 00:00:00 2001 From: Mabel Zhang Date: Fri, 10 Jul 2020 03:58:53 -0400 Subject: [PATCH 36/46] cleanup Signed-off-by: Mabel Zhang --- gazebo/sensors/CameraSensor.cc | 14 -------------- gazebo/sensors/GpuRaySensor.cc | 18 ++---------------- gazebo/sensors/MultiCameraSensor.cc | 2 +- 3 files changed, 3 insertions(+), 31 deletions(-) diff --git a/gazebo/sensors/CameraSensor.cc b/gazebo/sensors/CameraSensor.cc index c50aa2ef77..bb3dcc9e81 100644 --- a/gazebo/sensors/CameraSensor.cc +++ b/gazebo/sensors/CameraSensor.cc @@ -275,20 +275,6 @@ bool CameraSensor::NeedsUpdate() ////////////////////////////////////////////////// void CameraSensor::Update(bool _force) { - /* - if (this->useStrictRate) - { - if (this->IsActive() || _force) - { - if (this->UpdateImpl(_force)) - this->updated(); - } - } - else - { - Sensor::Update(_force); - } - */ Sensor::Update(_force); } diff --git a/gazebo/sensors/GpuRaySensor.cc b/gazebo/sensors/GpuRaySensor.cc index faba9d3ab3..dcafe1a45c 100644 --- a/gazebo/sensors/GpuRaySensor.cc +++ b/gazebo/sensors/GpuRaySensor.cc @@ -80,7 +80,7 @@ std::string GpuRaySensor::Topic() const void GpuRaySensor::Load(const std::string &_worldName, sdf::ElementPtr _sdf) { Sensor::Load(_worldName, _sdf); - // strict_rate parameter is parsed in Sensor::Load() + // useStrictRate is set in Sensor::Load() if (this->useStrictRate) { this->connections.push_back( @@ -93,7 +93,7 @@ void GpuRaySensor::Load(const std::string &_worldName, sdf::ElementPtr _sdf) void GpuRaySensor::Load(const std::string &_worldName) { Sensor::Load(_worldName); - // strict_rate parameter is parsed in Sensor::Load() + // useStrictRate is set in Sensor::Load() if (this->useStrictRate) { this->connections.push_back( @@ -426,20 +426,6 @@ bool GpuRaySensor::NeedsUpdate() ////////////////////////////////////////////////// void GpuRaySensor::Update(bool _force) { - /* - if (this->useStrictRate) - { - if (this->IsActive() || _force) - { - if (this->UpdateImpl(_force)) - this->updated(); - } - } - else - { - Sensor::Update(_force); - } - */ Sensor::Update(_force); } diff --git a/gazebo/sensors/MultiCameraSensor.cc b/gazebo/sensors/MultiCameraSensor.cc index bcec44b462..9f626ad7d6 100644 --- a/gazebo/sensors/MultiCameraSensor.cc +++ b/gazebo/sensors/MultiCameraSensor.cc @@ -80,7 +80,7 @@ std::string MultiCameraSensor::Topic() const void MultiCameraSensor::Load(const std::string &_worldName) { Sensor::Load(_worldName); - // strict_rate parameter is parsed in Sensor::Load() + // useStrictRate is set in Sensor::Load() if (this->useStrictRate) { this->connections.push_back( From 59a2c9afd6a441ea3b24fa845809f6167479e507 Mon Sep 17 00:00:00 2001 From: Mabel Zhang Date: Fri, 10 Jul 2020 04:14:47 -0400 Subject: [PATCH 37/46] minor cleanup Signed-off-by: Mabel Zhang --- gazebo/sensors/Sensor.cc | 2 -- 1 file changed, 2 deletions(-) diff --git a/gazebo/sensors/Sensor.cc b/gazebo/sensors/Sensor.cc index 6eca0bfd8b..918373dbb8 100644 --- a/gazebo/sensors/Sensor.cc +++ b/gazebo/sensors/Sensor.cc @@ -234,8 +234,6 @@ void Sensor::Update(const bool _force) { if (this->IsActive() || _force) { - // TODO(mabelzhang): Does strict rate not need anything in the else-clause - // below at all? Strict rate PR did not use them if (this->useStrictRate) { if (this->UpdateImpl(_force)) From 867b25cfabb925ff6f808165ca31c3aae4df8895 Mon Sep 17 00:00:00 2001 From: Mabel Zhang Date: Fri, 10 Jul 2020 20:01:23 -0400 Subject: [PATCH 38/46] fix laser test bug Signed-off-by: Mabel Zhang --- test/integration/laser.cc | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/test/integration/laser.cc b/test/integration/laser.cc index 8af5a48887..a79bd178d4 100644 --- a/test/integration/laser.cc +++ b/test/integration/laser.cc @@ -627,12 +627,12 @@ void LaserTest::LaserStrictUpdateRate(const std::string &_physicsEngine) std::string rayName = "laser"; sensors::SensorPtr sensor = sensors::get_sensor(rayName); sensors::RaySensorPtr raySensor = - std::dynamic_pointer_cast(sensor); + std::dynamic_pointer_cast(sensor); ASSERT_TRUE(raySensor != NULL); int msgCount = 0; event::ConnectionPtr c = raySensor->ConnectUpdated( - std::bind(&LaserTest::OnNewUpdate, this, &msgCount)); + std::bind(&LaserTest::OnNewUpdate, this, &msgCount)); common::Timer timer; timer.Start(); @@ -666,7 +666,7 @@ void LaserTest::LaserStrictUpdateRate(const std::string &_physicsEngine) TEST_P(LaserTest, LaserStrictUpdateRate) { - LaserUnitNoise(GetParam()); + LaserStrictUpdateRate(GetParam()); } INSTANTIATE_TEST_CASE_P(PhysicsEngines, LaserTest, PHYSICS_ENGINE_VALUES,); // NOLINT From d761e4d7cbdae295dfc1200c128098c09ad5f297 Mon Sep 17 00:00:00 2001 From: Mabel Zhang Date: Fri, 10 Jul 2020 22:42:19 -0400 Subject: [PATCH 39/46] transceiver strict rate test Signed-off-by: Mabel Zhang --- test/integration/transceiver.cc | 72 +++++++++++++++++++++++ test/worlds/transceiver_strict_rate.world | 61 +++++++++++++++++++ 2 files changed, 133 insertions(+) create mode 100644 test/worlds/transceiver_strict_rate.world diff --git a/test/integration/transceiver.cc b/test/integration/transceiver.cc index b2c0e88d11..4f8e2aed61 100644 --- a/test/integration/transceiver.cc +++ b/test/integration/transceiver.cc @@ -32,7 +32,9 @@ class TransceiverTest : public ServerFixture, public: void TxRxEmptySpace(const std::string &_physicsEngine); public: void TxRxObstacle(const std::string &_physicsEngine); public: void TxRxFreqOutOfBounds(const std::string &_physicsEngine); + public: void TxRxStrictUpdateRate(const std::string &_physicsEngine); private: void RxMsg(const ConstWirelessNodesPtr &_msg); + private: void OnNewUpdate(int* _msgCounter); private: static const double MinFreq; private: static const double MaxFreq; @@ -70,6 +72,12 @@ void TransceiverTest::RxMsg(const ConstWirelessNodesPtr &_msg) this->receivedMsg = true; } +///////////////////////////////////////////////// +void TransceiverTest::OnNewUpdate(int* _msgCounter) +{ + *_msgCounter += 1; +} + ///////////////////////////////////////////////// void TransceiverTest::TxRxEmptySpace(const std::string &_physicsEngine) { @@ -388,6 +396,64 @@ void TransceiverTest::TxRxObstacle(const std::string &_physicsEngine) EXPECT_GT(avgSignalLevelEmpty / samples, avgSignalLevelObstacle / samples); } +///////////////////////////////////////////////// +void TransceiverTest::TxRxStrictUpdateRate(const std::string &_physicsEngine) +{ + LoadArgs(" --lockstep -u -e " + _physicsEngine + + " worlds/transceiver_strict_rate.world"); + + // Wait until the sensors have been initialized + while (!sensors::SensorManager::Instance()->SensorsInitialized()) + common::Time::MSleep(1000); + + std::string txSensorName = "tx_sensor"; + sensors::SensorPtr txSensor = sensors::get_sensor(txSensorName); + sensors::WirelessTransmitterPtr tx = + std::dynamic_pointer_cast(txSensor); + ASSERT_TRUE(tx != NULL); + + std::string rxSensorName = "rx_sensor"; + sensors::SensorPtr rxSensor = sensors::get_sensor(rxSensorName); + sensors::WirelessReceiverPtr rx = + std::dynamic_pointer_cast(rxSensor); + ASSERT_TRUE(rx != NULL); + + // Connect update callback and start timer + int msgCount = 0; + event::ConnectionPtr c = rx->ConnectUpdated( + std::bind(&TransceiverTest::OnNewUpdate, this, &msgCount)); + common::Timer timer; + SetPause(false); + timer.Start(); + + // how many msgs produced for 5 seconds (in simulated clock domain) + double updateRate = rx->UpdateRate(); + int totalMsgs = 5 * updateRate; + physics::WorldPtr world = physics::get_world("default"); + ASSERT_TRUE(world != NULL); + double simT0 = 0.0; + + while (msgCount < totalMsgs) + { + // An approximation of when we receive the first image. In reality one + // iteration before we receive the second image. + if (msgCount == 0) + { + simT0 = world->SimTime().Double(); + } + common::Time::MSleep(1); + } + + // check that the obtained rate is the one expected + double dt = world->SimTime().Double() - simT0; + double rate = static_cast(totalMsgs) / dt; + gzdbg << "timer [" << dt << "] seconds rate [" << rate << "] fps\n"; + const double tolerance = 0.02; + EXPECT_GT(rate, updateRate * (1 - tolerance)); + EXPECT_LT(rate, updateRate * (1 + tolerance)); + c.reset(); +} + ///////////////////////////////////////////////// TEST_P(TransceiverTest, EmptyWorld) { @@ -419,6 +485,12 @@ TEST_P(TransceiverTest, FreqOutOfBounds) TxRxFreqOutOfBounds(GetParam()); } +///////////////////////////////////////////////// +TEST_P(TransceiverTest, StrictUpdateRate) +{ + TxRxStrictUpdateRate(GetParam()); +} + ///////////////////////////////////////////////// INSTANTIATE_TEST_CASE_P(PhysicsEngines, TransceiverTest, PHYSICS_ENGINE_VALUES,); // NOLINT diff --git a/test/worlds/transceiver_strict_rate.world b/test/worlds/transceiver_strict_rate.world new file mode 100644 index 0000000000..91ae0f7afd --- /dev/null +++ b/test/worlds/transceiver_strict_rate.world @@ -0,0 +1,61 @@ + + + + + model://ground_plane + + + model://sun + + + + true + -10.0 0.0 0.5 0 0 0 + + + + + 1 1 1 + + + + + + MyESSID + 2442.0 + + 1 + + 500 + true + + + + + + 13 0 0.5 0 0 0 + + + + + 1 1 1 + + + + + + 2.5 + 14.5 + + 1 + + 500 + true + + + + + + From d09afc5bfdfdd333fd9d18d63a745e09fb42e835 Mon Sep 17 00:00:00 2001 From: Mabel Zhang Date: Fri, 10 Jul 2020 23:46:37 -0400 Subject: [PATCH 40/46] tidy up test worlds Signed-off-by: Mabel Zhang --- test/integration/contact_sensor.cc | 4 +-- test/worlds/camera_strict_rate.world | 33 ++++++++++--------------- test/worlds/contact_strict_rate.world | 2 +- test/worlds/imu_strict_rate_test.world | 34 -------------------------- 4 files changed, 16 insertions(+), 57 deletions(-) diff --git a/test/integration/contact_sensor.cc b/test/integration/contact_sensor.cc index 2aec9c533a..62ddb6f56d 100644 --- a/test/integration/contact_sensor.cc +++ b/test/integration/contact_sensor.cc @@ -706,7 +706,7 @@ void ContactSensor::StrictUpdateRateTest(const std::string &_physicsEngine) g_messageCount = 0; transport::SubscriberPtr sub = this->node->Subscribe( - "/gazebo/default/physics/contacts", &ContactSensor::Callback, this); + "/gazebo/world_1/physics/contacts", &ContactSensor::Callback, this); common::Timer timer; SetPause(false); timer.Start(); @@ -714,7 +714,7 @@ void ContactSensor::StrictUpdateRateTest(const std::string &_physicsEngine) // how many msgs produced for 5 seconds (in simulated clock domain) double updateRate = contactSensor->UpdateRate(); unsigned int totalMsgs = 5 * updateRate; - physics::WorldPtr world = physics::get_world("default"); + physics::WorldPtr world = physics::get_world("world_1"); ASSERT_TRUE(world != NULL); double simT0 = 0.0; diff --git a/test/worlds/camera_strict_rate.world b/test/worlds/camera_strict_rate.world index dec5c2e3a1..6d22357c53 100644 --- a/test/worlds/camera_strict_rate.world +++ b/test/worlds/camera_strict_rate.world @@ -34,32 +34,12 @@ 500 true - - - - 1.0472 - - 320 - 240 - - - 1 - 30 - true - 13 0 0.5 0 0 0 - - - - 5 5 5 - - - @@ -67,6 +47,19 @@ + + + + 1.0472 + + 320 + 240 + + + 1 + 30 + true + diff --git a/test/worlds/contact_strict_rate.world b/test/worlds/contact_strict_rate.world index f6747b9778..3f74262a96 100644 --- a/test/worlds/contact_strict_rate.world +++ b/test/worlds/contact_strict_rate.world @@ -1,6 +1,6 @@ - + 1 0.4 0.4 0.4 1 diff --git a/test/worlds/imu_strict_rate_test.world b/test/worlds/imu_strict_rate_test.world index 45b50961fe..353e846d1d 100644 --- a/test/worlds/imu_strict_rate_test.world +++ b/test/worlds/imu_strict_rate_test.world @@ -1,40 +1,6 @@ - - model://sun From 7491fa4c9162f8650d10adf17b2d22ce814ea330 Mon Sep 17 00:00:00 2001 From: Mabel Zhang Date: Fri, 10 Jul 2020 23:56:05 -0400 Subject: [PATCH 41/46] rename gpu ray test world Signed-off-by: Mabel Zhang --- .../{gpu_ray_strict_rate.world => gpu_laser_strict_rate.world} | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename test/worlds/{gpu_ray_strict_rate.world => gpu_laser_strict_rate.world} (100%) diff --git a/test/worlds/gpu_ray_strict_rate.world b/test/worlds/gpu_laser_strict_rate.world similarity index 100% rename from test/worlds/gpu_ray_strict_rate.world rename to test/worlds/gpu_laser_strict_rate.world From 37eeec94639541384b2542bbb0900aa68b0800b4 Mon Sep 17 00:00:00 2001 From: Mabel Zhang Date: Fri, 10 Jul 2020 23:57:49 -0400 Subject: [PATCH 42/46] change file name in gpu ray test Signed-off-by: Mabel Zhang --- test/integration/gpu_laser.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/test/integration/gpu_laser.cc b/test/integration/gpu_laser.cc index a4aa062d17..1706426420 100644 --- a/test/integration/gpu_laser.cc +++ b/test/integration/gpu_laser.cc @@ -613,7 +613,7 @@ TEST_F(GPURaySensorTest, LaserScanResolution) TEST_F(GPURaySensorTest, StrictUpdateRate) { - LoadArgs(" --lockstep worlds/gpu_ray_strict_rate.world"); + LoadArgs(" --lockstep worlds/gpu_laser_strict_rate.world"); // Make sure the render engine is available. if (rendering::RenderEngine::Instance()->GetRenderPathType() == From 02269c0b8653c2d3734b4a683430388e39aa64ec Mon Sep 17 00:00:00 2001 From: Mabel Zhang Date: Mon, 13 Jul 2020 23:26:29 -0400 Subject: [PATCH 43/46] disable simbody imu test Signed-off-by: Mabel Zhang --- test/integration/imu.cc | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/test/integration/imu.cc b/test/integration/imu.cc index 3de2120fa6..af3122aa6a 100644 --- a/test/integration/imu.cc +++ b/test/integration/imu.cc @@ -851,6 +851,13 @@ TEST_P(ImuTest, EmptyWorldBias) void ImuTest::Stationary_EmptyWorld_StrictRate( const std::string &_physicsEngine) { + // static models not fully working in simbody yet + if (_physicsEngine == "simbody") + { + gzerr << "Aborting test for Simbody, see issue #860.\n"; + return; + } + LoadArgs(" --lockstep -u -e " + _physicsEngine + " worlds/imu_strict_rate_test.world"); From e7da851b6b864453e6b6eec90f616c394b3db30b Mon Sep 17 00:00:00 2001 From: Mabel Zhang Date: Tue, 14 Jul 2020 01:01:44 -0400 Subject: [PATCH 44/46] make Sensor::useStrictRate static Signed-off-by: Mabel Zhang --- gazebo/sensors/Sensor.cc | 4 ++-- gazebo/sensors/Sensor.hh | 12 +++++++----- 2 files changed, 9 insertions(+), 7 deletions(-) diff --git a/gazebo/sensors/Sensor.cc b/gazebo/sensors/Sensor.cc index 918373dbb8..f919ccf19c 100644 --- a/gazebo/sensors/Sensor.cc +++ b/gazebo/sensors/Sensor.cc @@ -45,6 +45,8 @@ using namespace sensors; sdf::ElementPtr SensorPrivate::sdfSensor; +bool Sensor::useStrictRate = false; + ////////////////////////////////////////////////// Sensor::Sensor(SensorCategory _cat) : dataPtr(new SensorPrivate) @@ -66,8 +68,6 @@ Sensor::Sensor(SensorCategory _cat) this->dataPtr->updateDelay = common::Time(0.0); this->updatePeriod = common::Time(0.0); - this->useStrictRate = false; - this->dataPtr->id = physics::getUniqueId(); } diff --git a/gazebo/sensors/Sensor.hh b/gazebo/sensors/Sensor.hh index da72331950..011d77b7bd 100644 --- a/gazebo/sensors/Sensor.hh +++ b/gazebo/sensors/Sensor.hh @@ -205,6 +205,13 @@ namespace gazebo /// \param[in] _sdf SDF parameters. private: void LoadPlugin(sdf::ElementPtr _sdf); + /// \brief Whether to enforce strict sensor update rate, even if physics + /// time has to slow down to wait for sensor updates to satisfy + /// the desired rate. + /// \details static type to avoid breaking ABI and because lockstep + /// setting is global. + protected: static bool useStrictRate; + /// \brief True if sensor generation is active. protected: bool active; @@ -239,11 +246,6 @@ namespace gazebo /// Sensor::SetUpdateRate. protected: common::Time updatePeriod; - /// \brief Whether to enforce strict sensor update rate, even if physics - /// time has to slow down to wait for sensor updates to satisfy - /// the desired rate. - protected: bool useStrictRate; - /// \brief Time of the last update. protected: common::Time lastUpdateTime; From 9c61a7c61a30069d23bbad20f5eb0156e1d8e01c Mon Sep 17 00:00:00 2001 From: Mabel Zhang Date: Tue, 14 Jul 2020 22:38:58 -0400 Subject: [PATCH 45/46] fix ManTest Signed-off-by: Mabel Zhang --- gazebo/gazebo.1.ronn | 2 ++ gazebo/gzserver.1.ronn | 2 ++ 2 files changed, 4 insertions(+) diff --git a/gazebo/gazebo.1.ronn b/gazebo/gazebo.1.ronn index b6c19a3174..7202cd54f9 100644 --- a/gazebo/gazebo.1.ronn +++ b/gazebo/gazebo.1.ronn @@ -49,6 +49,8 @@ Gazebo server runs simulation and handles commandline options, starts a Master, Load a server plugin. * -o, --profile arg : Physics preset profile name from the options in the world file. +* --lockstep : + Lockstep simulation so sensor update rates are respected. ## AUTHOR diff --git a/gazebo/gzserver.1.ronn b/gazebo/gzserver.1.ronn index efcf52d6eb..9b9791fdbf 100644 --- a/gazebo/gzserver.1.ronn +++ b/gazebo/gzserver.1.ronn @@ -19,6 +19,8 @@ Gazebo server runs simulation and handles commandline options, starts a Master, Produce this help message. * -u, --pause : Start the server in a paused state. +* --lockstep : + Lockstep simulation so sensor update rates are respected. * -e, --physics arg : Specify a physics engine (ode|bullet|dart|simbody). * -p, --play arg : From b35191322d97e7476fc8658dc2bda62da3645c27 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Fri, 17 Jul 2020 15:59:10 -0700 Subject: [PATCH 46/46] fix for wide angle camera Signed-off-by: Ian Chen --- gazebo/rendering/Visual.cc | 4 ++-- gazebo/sensors/WideAngleCameraSensor.cc | 2 +- test/worlds/imu_strict_rate_test.world | 2 +- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/gazebo/rendering/Visual.cc b/gazebo/rendering/Visual.cc index c20c6ff85a..8e1d4e0bcc 100644 --- a/gazebo/rendering/Visual.cc +++ b/gazebo/rendering/Visual.cc @@ -820,8 +820,8 @@ void Visual::SetScale(const ignition::math::Vector3d &_scale) } else { - gzerr << Name() << ": Size of the collision contains one or several zeros." << - " Collisions may not visualize properly." << std::endl; + 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/WideAngleCameraSensor.cc b/gazebo/sensors/WideAngleCameraSensor.cc index 650e109754..b079ac09c0 100644 --- a/gazebo/sensors/WideAngleCameraSensor.cc +++ b/gazebo/sensors/WideAngleCameraSensor.cc @@ -140,7 +140,7 @@ void WideAngleCameraSensor::Init() ////////////////////////////////////////////////// void WideAngleCameraSensor::Load(const std::string &_worldName) { - Sensor::Load(_worldName); + CameraSensor::Load(_worldName); this->imagePub = this->node->Advertise( this->Topic(), 50); diff --git a/test/worlds/imu_strict_rate_test.world b/test/worlds/imu_strict_rate_test.world index 353e846d1d..486a37843e 100644 --- a/test/worlds/imu_strict_rate_test.world +++ b/test/worlds/imu_strict_rate_test.world @@ -1,5 +1,5 @@ - + model://sun