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 : diff --git a/gazebo/sensors/CameraSensor.cc b/gazebo/sensors/CameraSensor.cc index 1357ddd05f..bb3dcc9e81 100644 --- a/gazebo/sensors/CameraSensor.cc +++ b/gazebo/sensors/CameraSensor.cc @@ -275,18 +275,7 @@ 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 82f50320b2..dcafe1a45c 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/PhysicsEngine.hh" #include "gazebo/common/Exception.hh" #include "gazebo/common/Events.hh" @@ -79,12 +80,26 @@ std::string GpuRaySensor::Topic() const void GpuRaySensor::Load(const std::string &_worldName, sdf::ElementPtr _sdf) { Sensor::Load(_worldName, _sdf); + // useStrictRate is set in Sensor::Load() + if (this->useStrictRate) + { + this->connections.push_back( + event::Events::ConnectPreRenderEnded( + boost::bind(&GpuRaySensor::PrerenderEnded, this))); + } } ////////////////////////////////////////////////// void GpuRaySensor::Load(const std::string &_worldName) { Sensor::Load(_worldName); + // useStrictRate is set 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); @@ -346,13 +361,66 @@ 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; + 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(); + } } ////////////////////////////////////////////////// @@ -582,16 +650,47 @@ int GpuRaySensor::Fiducial(const unsigned int /*_index*/) const return -1; } +////////////////////////////////////////////////// +void GpuRaySensor::PrerenderEnded() +{ + 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; + 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 GpuRaySensor::Render() { - if (!this->dataPtr->laserCam || !this->IsActive() || !this->NeedsUpdate()) - return; + if (this->useStrictRate) + { + if (!this->dataPtr->renderNeeded) + return; - this->lastMeasurementTime = this->scene->SimTime(); + 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; + this->dataPtr->laserCam->Render(); + this->dataPtr->rendered = true; + } } ////////////////////////////////////////////////// @@ -692,11 +791,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/MultiCameraSensor.cc b/gazebo/sensors/MultiCameraSensor.cc index d358330769..9f626ad7d6 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); + // useStrictRate is set 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 4b12f127b8..f919ccf19c 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" @@ -44,6 +45,8 @@ using namespace sensors; sdf::ElementPtr SensorPrivate::sdfSensor; +bool Sensor::useStrictRate = false; + ////////////////////////////////////////////////// Sensor::Sensor(SensorCategory _cat) : dataPtr(new SensorPrivate) @@ -65,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(); } @@ -142,7 +143,9 @@ void Sensor::Load(const std::string &_worldName) this->useStrictRate = rendering::lockstep_enabled(); if (this->dataPtr->category == IMAGE) + { this->scene = rendering::get_scene(_worldName); + } // loaded, but not updated this->lastUpdateTime = common::Time(0.0); @@ -231,44 +234,52 @@ 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(); - + 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(); + } } } } 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; diff --git a/test/integration/contact_sensor.cc b/test/integration/contact_sensor.cc index 5c96ff4b45..62ddb6f56d 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,63 @@ TEST_P(ContactSensor, TorqueTest) TorqueTest(GetParam()); } +//////////////////////////////////////////////////////////////////////// +// Test strict update rate of contact sensor. +//////////////////////////////////////////////////////////////////////// +void ContactSensor::StrictUpdateRateTest(const std::string &_physicsEngine) +{ + LoadArgs(" --lockstep -u -e " + _physicsEngine + + " worlds/contact_strict_rate.world"); + + // 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/world_1/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("world_1"); + 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/gpu_laser.cc b/test/integration/gpu_laser.cc index 0c4a1bf139..1706426420 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,67 @@ TEST_F(GPURaySensorTest, LaserScanResolution) delete [] scan; } +TEST_F(GPURaySensorTest, StrictUpdateRate) +{ + LoadArgs(" --lockstep worlds/gpu_laser_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 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) { ::testing::InitGoogleTest(&argc, argv); diff --git a/test/integration/imu.cc b/test/integration/imu.cc index 9eb098f923..af3122aa6a 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,69 @@ TEST_P(ImuTest, EmptyWorldBias) Stationary_EmptyWorld_Bias(GetParam()); } +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"); + + // 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/integration/laser.cc b/test/integration/laser.cc index 694fda4104..a79bd178d4 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) +{ + LoadArgs(" --lockstep 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) +{ + LaserStrictUpdateRate(GetParam()); +} + INSTANTIATE_TEST_CASE_P(PhysicsEngines, LaserTest, PHYSICS_ENGINE_VALUES,); // NOLINT int main(int argc, char **argv) diff --git a/test/integration/logical_camera_sensor.cc b/test/integration/logical_camera_sensor.cc index 01cb9c9238..bd4a528e71 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) +{ + LoadArgs(" --lockstep 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/integration/multicamera_sensor.cc b/test/integration/multicamera_sensor.cc index 258bb9efe8..b6336756be 100644 --- a/test/integration/multicamera_sensor.cc +++ b/test/integration/multicamera_sensor.cc @@ -568,6 +568,90 @@ TEST_F(MultiCameraSensor, CameraRotationWorldPoseTest) << "]\n"; } +///////////////////////////////////////////////// +TEST_F(MultiCameraSensor, StrictUpdateRate) +{ + LoadArgs(" --lockstep worlds/multicamera_strict_rate_test.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 = "multicamera_sensor"; + sensors::SensorPtr sensor = sensors::get_sensor(sensorName); + sensors::MultiCameraSensorPtr multiCamSensor = + std::dynamic_pointer_cast(sensor); + EXPECT_TRUE(multiCamSensor != nullptr); + multiCamSensor->SetActive(true); + + // 3 cameras + EXPECT_EQ(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)); + + common::Timer timer; + timer.Start(); + + // 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 (imageCount0 < totalImagesPerCam || imageCount1 < totalImagesPerCam || + imageCount2 < totalImagesPerCam) + { + // 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(); + } + common::Time::MSleep(1); + } + + // check that the obtained rate is the one expected + double dt = world->SimTime().Double() - simT0; + 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)); + + c0.reset(); + c1.reset(); + c2.reset(); + delete [] img0; + delete [] img1; + delete [] img2; +} + ///////////////////////////////////////////////// int main(int argc, char **argv) { 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/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 new file mode 100644 index 0000000000..3f74262a96 --- /dev/null +++ b/test/worlds/contact_strict_rate.world @@ -0,0 +1,173 @@ + + + + + 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 + + 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/gpu_laser_strict_rate.world b/test/worlds/gpu_laser_strict_rate.world new file mode 100644 index 0000000000..c1714cce96 --- /dev/null +++ b/test/worlds/gpu_laser_strict_rate.world @@ -0,0 +1,86 @@ + + + + + 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 + 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 + + + + diff --git a/test/worlds/imu_strict_rate_test.world b/test/worlds/imu_strict_rate_test.world new file mode 100644 index 0000000000..353e846d1d --- /dev/null +++ b/test/worlds/imu_strict_rate_test.world @@ -0,0 +1,133 @@ + + + + + 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 + + + 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..7e9e5baf31 --- /dev/null +++ b/test/worlds/laser_hit_strict_rate_test.world @@ -0,0 +1,57 @@ + + + + + 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 + + + + + diff --git a/test/worlds/logical_camera_strict_rate.world b/test/worlds/logical_camera_strict_rate.world new file mode 100644 index 0000000000..8092eb767a --- /dev/null +++ b/test/worlds/logical_camera_strict_rate.world @@ -0,0 +1,31 @@ + + + + 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 + + + + + diff --git a/test/worlds/multicamera_strict_rate_test.world b/test/worlds/multicamera_strict_rate_test.world new file mode 100644 index 0000000000..ae764a227a --- /dev/null +++ b/test/worlds/multicamera_strict_rate_test.world @@ -0,0 +1,110 @@ + + + + + model://ground_plane + + + model://sun + + + + true + 0 0 0.5 0 0 0 + + + + + 1 1 1 + + + + + + + + 1 1 1 + + + + + + + 500 + + + 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 + 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 + + + + 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 + + + + + +