diff --git a/examples/save_image/main.cc b/examples/save_image/main.cc index b38b9a73..ddaa707f 100644 --- a/examples/save_image/main.cc +++ b/examples/save_image/main.cc @@ -113,7 +113,7 @@ int main() cameraSensor->ConnectImageCallback(&OnImageFrame); // Force the camera to generate an image - mgr.RunOnce(ignition::common::Time::Zero, true); + mgr.RunOnce(std::chrono::steady_clock::duration::zero(), true); return 0; } diff --git a/include/ignition/sensors/AirPressureSensor.hh b/include/ignition/sensors/AirPressureSensor.hh index 69b52c80..d3db6b8b 100644 --- a/include/ignition/sensors/AirPressureSensor.hh +++ b/include/ignition/sensors/AirPressureSensor.hh @@ -67,7 +67,14 @@ namespace ignition /// \brief Update the sensor and generate data /// \param[in] _now The current time /// \return true if the update was successfull - public: virtual bool Update(const common::Time &_now) override; + public: virtual bool IGN_DEPRECATED(4) Update( + const ignition::common::Time &_now) override; + + /// \brief Update the sensor and generate data + /// \param[in] _now The current time + /// \return true if the update was successfull + public: virtual bool Update( + const std::chrono::steady_clock::duration &_now) override; /// \brief Set the reference altitude. /// \param[in] _ref Verical reference position in meters diff --git a/include/ignition/sensors/AltimeterSensor.hh b/include/ignition/sensors/AltimeterSensor.hh index 87f4a3c9..af582786 100644 --- a/include/ignition/sensors/AltimeterSensor.hh +++ b/include/ignition/sensors/AltimeterSensor.hh @@ -67,7 +67,14 @@ namespace ignition /// \brief Update the sensor and generate data /// \param[in] _now The current time /// \return true if the update was successfull - public: virtual bool Update(const common::Time &_now) override; + public: virtual bool IGN_DEPRECATED(4) Update( + const ignition::common::Time &_now) override; + + /// \brief Update the sensor and generate data + /// \param[in] _now The current time + /// \return true if the update was successfull + public: virtual bool Update( + const std::chrono::steady_clock::duration &_now) override; /// \brief Set the vertical reference position of the altimeter /// \param[in] _ref Verical reference position in meters diff --git a/include/ignition/sensors/CameraSensor.hh b/include/ignition/sensors/CameraSensor.hh index 147a3a3d..6da658fc 100644 --- a/include/ignition/sensors/CameraSensor.hh +++ b/include/ignition/sensors/CameraSensor.hh @@ -73,10 +73,17 @@ namespace ignition /// \return True on success public: virtual bool Init() override; + /// \brief Update the sensor and generate data + /// \param[in] _now The current time + /// \return true if the update was successfull + public: virtual bool IGN_DEPRECATED(4) Update( + const ignition::common::Time &_now) override; + /// \brief Force the sensor to generate data /// \param[in] _now The current time /// \return true if the update was successfull - public: virtual bool Update(const common::Time &_now) override; + public: virtual bool Update( + const std::chrono::steady_clock::duration &_now) override; /// \brief Set a callback to be called when image frame data is /// generated. @@ -138,7 +145,13 @@ namespace ignition /// \brief Publish camera info message. /// \param[in] _now The current time - protected: void PublishInfo(const ignition::common::Time &_now); + protected: void IGN_DEPRECATED(4) PublishInfo( + const ignition::common::Time &_now); + + /// \brief Publish camera info message. + /// \param[in] _now The current time + protected: void PublishInfo( + const std::chrono::steady_clock::duration &_now); /// \brief Create a camera in a scene /// \return True on success. diff --git a/include/ignition/sensors/DepthCameraSensor.hh b/include/ignition/sensors/DepthCameraSensor.hh index a05b40e8..e5fd8f45 100644 --- a/include/ignition/sensors/DepthCameraSensor.hh +++ b/include/ignition/sensors/DepthCameraSensor.hh @@ -79,7 +79,14 @@ namespace ignition /// \brief Force the sensor to generate data /// \param[in] _now The current time /// \return true if the update was successfull - public: virtual bool Update(const common::Time &_now) override; + public: virtual bool IGN_DEPRECATED(4) Update( + const common::Time &_now) override; + + /// \brief Force the sensor to generate data + /// \param[in] _now The current time + /// \return true if the update was successfull + public: virtual bool Update( + const std::chrono::steady_clock::duration &_now) override; /// \brief Force the sensor to generate data /// \param[in] _now The current time diff --git a/include/ignition/sensors/GpuLidarSensor.hh b/include/ignition/sensors/GpuLidarSensor.hh index 46eb46a1..a7376192 100644 --- a/include/ignition/sensors/GpuLidarSensor.hh +++ b/include/ignition/sensors/GpuLidarSensor.hh @@ -58,7 +58,14 @@ namespace ignition /// \brief Force the sensor to generate data /// \param[in] _now The current time /// \return true if the update was successfull - public: virtual bool Update(const common::Time &_now) override; + public: virtual bool IGN_DEPRECATED(4) Update( + const ignition::common::Time &_now) override; + + /// \brief Force the sensor to generate data + /// \param[in] _now The current time + /// \return true if the update was successfull + public: virtual bool Update( + const std::chrono::steady_clock::duration &_now) override; /// \brief Initialize values in the sensor /// \return True on success diff --git a/include/ignition/sensors/ImuSensor.hh b/include/ignition/sensors/ImuSensor.hh index c174224e..c490d0dc 100644 --- a/include/ignition/sensors/ImuSensor.hh +++ b/include/ignition/sensors/ImuSensor.hh @@ -68,7 +68,14 @@ namespace ignition /// \brief Update the sensor and generate data /// \param[in] _now The current time /// \return true if the update was successfull - public: virtual bool Update(const common::Time &_now) override; + public: virtual bool IGN_DEPRECATED(4) Update( + const ignition::common::Time &_now) override; + + /// \brief Update the sensor and generate data + /// \param[in] _now The current time + /// \return true if the update was successfull + public: virtual bool Update( + const std::chrono::steady_clock::duration &_now) override; /// \brief Set the angular velocity of the imu /// \param[in] _angularVel Angular velocity of the imu in body frame diff --git a/include/ignition/sensors/Lidar.hh b/include/ignition/sensors/Lidar.hh index 1144f059..44f5c780 100644 --- a/include/ignition/sensors/Lidar.hh +++ b/include/ignition/sensors/Lidar.hh @@ -53,15 +53,29 @@ namespace ignition /// \brief destructor public: virtual ~Lidar(); + /// \brief Update the sensor and generate data + /// \param[in] _now The current time + /// \return true if the update was successfull + public: virtual bool IGN_DEPRECATED(4) Update( + const ignition::common::Time &_now) override; + /// \brief Force the sensor to generate data /// \param[in] _now The current time /// \return true if the update was successfull - public: virtual bool Update(const common::Time &_now) override; + public: virtual bool Update( + const std::chrono::steady_clock::duration &_now) override; + + /// \brief Publish LaserScan message + /// \param[in] _now The current time + /// \return true if the update was successfull + public: virtual bool IGN_DEPRECATED(4) PublishLidarScan( + const ignition::common::Time &_now); /// \brief Publish LaserScan message /// \param[in] _now The current time /// \return true if the update was successfull - public: virtual bool PublishLidarScan(const common::Time &_now); + public: virtual bool PublishLidarScan( + const std::chrono::steady_clock::duration &_now); /// \brief Load the sensor based on data from an sdf::Sensor object. /// \param[in] _sdf SDF Sensor parameters. diff --git a/include/ignition/sensors/LogicalCameraSensor.hh b/include/ignition/sensors/LogicalCameraSensor.hh index da882b99..54a1b179 100644 --- a/include/ignition/sensors/LogicalCameraSensor.hh +++ b/include/ignition/sensors/LogicalCameraSensor.hh @@ -68,10 +68,17 @@ namespace ignition /// \return True on success public: virtual bool Init() override; + /// \brief Update the sensor and generate data + /// \param[in] _now The current time + /// \return true if the update was successfull + public: virtual bool IGN_DEPRECATED(4) Update( + const ignition::common::Time &_now) override; + /// \brief Force the sensor to generate data /// \param[in] _now The current time /// \return true if the update was successfull - public: virtual bool Update(const common::Time &_now) override; + public: virtual bool Update( + const std::chrono::steady_clock::duration &_now) override; /// \brief Get the near distance. This is the distance from the /// frustum's vertex to the closest plane. diff --git a/include/ignition/sensors/MagnetometerSensor.hh b/include/ignition/sensors/MagnetometerSensor.hh index a50abb02..fe077167 100644 --- a/include/ignition/sensors/MagnetometerSensor.hh +++ b/include/ignition/sensors/MagnetometerSensor.hh @@ -68,7 +68,14 @@ namespace ignition /// \brief Update the sensor and generate data /// \param[in] _now The current time /// \return true if the update was successfull - public: virtual bool Update(const common::Time &_now) override; + public: virtual bool IGN_DEPRECATED(4) Update( + const ignition::common::Time &_now) override; + + /// \brief Update the sensor and generate data + /// \param[in] _now The current time + /// \return true if the update was successfull + public: virtual bool Update( + const std::chrono::steady_clock::duration &_now) override; /// \brief Set the world pose of the sensor /// \param[in] _pose Pose in world frame diff --git a/include/ignition/sensors/Manager.hh b/include/ignition/sensors/Manager.hh index 01b8fa50..75dd14b7 100644 --- a/include/ignition/sensors/Manager.hh +++ b/include/ignition/sensors/Manager.hh @@ -188,7 +188,14 @@ namespace ignition /// \param _time: The current simulated time /// \param _force: If true, all sensors are forced to update. Otherwise /// a sensor will update based on it's Hz rate. - public: void RunOnce(const ignition::common::Time &_time, + public: void IGN_DEPRECATED(4) RunOnce( + const ignition::common::Time &_time, bool _force = false); + + /// \brief Run the sensor generation one step. + /// \param _time: The current simulated time + /// \param _force: If true, all sensors are forced to update. Otherwise + /// a sensor will update based on it's Hz rate. + public: void RunOnce(const std::chrono::steady_clock::duration &_time, bool _force = false); /// \brief Adds colon delimited paths sensor plugins may be diff --git a/include/ignition/sensors/RgbdCameraSensor.hh b/include/ignition/sensors/RgbdCameraSensor.hh index 20a16828..17c563a2 100644 --- a/include/ignition/sensors/RgbdCameraSensor.hh +++ b/include/ignition/sensors/RgbdCameraSensor.hh @@ -66,10 +66,17 @@ namespace ignition /// \return True on success public: virtual bool Init() override; + /// \brief Force the sensor to generate data + /// \param[in] _now The current time + /// \return true if the update was successfull + public: virtual bool IGN_DEPRECATED(4) Update( + const ignition::common::Time &_now) override; + /// \brief Force the sensor to generate data /// \param[in] _now The current time /// \return true if the update was successful - public: virtual bool Update(const common::Time &_now) override; + public: virtual bool Update( + const std::chrono::steady_clock::duration &_now) override; /// \brief Set the rendering scene. /// \param[in] _scene Pointer to the scene diff --git a/include/ignition/sensors/Sensor.hh b/include/ignition/sensors/Sensor.hh index 99e8271c..adcc1db8 100644 --- a/include/ignition/sensors/Sensor.hh +++ b/include/ignition/sensors/Sensor.hh @@ -81,10 +81,47 @@ namespace ignition /// \param[in] _now The current time /// \return true if the update was successfull /// \sa SetUpdateRate() - public: virtual bool Update(const common::Time &_now) = 0; + public: + virtual bool IGN_DEPRECATED(4) Update(const common::Time &_now) = 0; + + /// \brief Force the sensor to generate data + /// + /// This method must be overridden by sensors. Subclasses should not + /// not make a decision about whether or not they need to update. The + /// Sensor class will make sure Update() is called at the correct time. + /// + /// If a subclass wants to have a variable update rate it should call + /// SetUpdateRate(). + /// + /// A subclass should return false if there was an error while updating + /// \param[in] _now The current time + /// \return true if the update was successfull + /// \sa SetUpdateRate() + public: virtual bool Update( + const std::chrono::steady_clock::duration &_now) = 0; /// \brief Return the next time the sensor will generate data - public: common::Time NextUpdateTime() const; + public: ignition::common::Time IGN_DEPRECATED(4) NextUpdateTime() const; + + /// \brief Return the next time the sensor will generate data + public: std::chrono::steady_clock::duration NextDataUpdateTime() const; + + /// \brief Update the sensor. + /// + /// This is called by the manager, and is responsible for determining + /// if this sensor needs to generate data at this time. If so, the + /// subclasses' Update() method will be called. + /// \param[in] _now The current time + /// \param[in] _force Force the update to happen even if it's not time + /// \return True if the update was triggered (_force was true or _now + /// >= next_update_time) and the sensor's + /// bool Sensor::Update(std::chrono::steady_clock::time_point) + /// function returned true. + /// False otherwise. + /// \remarks If forced the NextUpdateTime() will be unchanged. + /// \sa virtual bool Update(const common::Time &_name) = 0 + public: bool IGN_DEPRECATED(4) + Update(const ignition::common::Time &_now, const bool _force); /// \brief Update the sensor. /// @@ -95,11 +132,13 @@ namespace ignition /// \param[in] _force Force the update to happen even if it's not time /// \return True if the update was triggered (_force was true or _now /// >= next_update_time) and the sensor's - /// bool Sensor::Update(const common::Time &_now) function returned true. + /// bool Sensor::Update(std::chrono::steady_clock::time_point) + /// function returned true. /// False otherwise. /// \remarks If forced the NextUpdateTime() will be unchanged. /// \sa virtual bool Update(const common::Time &_name) = 0 - public: bool Update(const common::Time &_now, const bool _force); + public: bool Update( + const std::chrono::steady_clock::duration &_now, const bool _force); /// \brief Get the update rate of the sensor. /// diff --git a/include/ignition/sensors/ThermalCameraSensor.hh b/include/ignition/sensors/ThermalCameraSensor.hh index 1b598d8d..3eab5123 100644 --- a/include/ignition/sensors/ThermalCameraSensor.hh +++ b/include/ignition/sensors/ThermalCameraSensor.hh @@ -79,7 +79,14 @@ namespace ignition /// \brief Force the sensor to generate data /// \param[in] _now The current time /// \return true if the update was successfull - public: virtual bool Update(const common::Time &_now) override; + public: virtual bool IGN_DEPRECATED(4) Update( + const ignition::common::Time &_now) override; + + /// \brief Force the sensor to generate data + /// \param[in] _now The current time + /// \return true if the update was successfull + public: virtual bool Update( + const std::chrono::steady_clock::duration &_now) override; /// \brief Force the sensor to generate data /// \param[in] _now The current time diff --git a/src/AirPressureSensor.cc b/src/AirPressureSensor.cc index 53c64824..cbdf5124 100644 --- a/src/AirPressureSensor.cc +++ b/src/AirPressureSensor.cc @@ -133,7 +133,15 @@ bool AirPressureSensor::Load(sdf::ElementPtr _sdf) } ////////////////////////////////////////////////// -bool AirPressureSensor::Update(const ignition::common::Time &_now) +bool AirPressureSensor::Update( + const ignition::common::Time &_now) +{ + return this->Update(math::secNsecToDuration(_now.sec, _now.nsec)); +} + +////////////////////////////////////////////////// +bool AirPressureSensor::Update( + const std::chrono::steady_clock::duration &_now) { IGN_PROFILE("AirPressureSensor::Update"); if (!this->dataPtr->initialized) @@ -143,8 +151,7 @@ bool AirPressureSensor::Update(const ignition::common::Time &_now) } msgs::FluidPressure msg; - msg.mutable_header()->mutable_stamp()->set_sec(_now.sec); - msg.mutable_header()->mutable_stamp()->set_nsec(_now.nsec); + *msg.mutable_header()->mutable_stamp() = msgs::Convert(_now); auto frame = msg.mutable_header()->add_data(); frame->set_key("frame_id"); frame->add_value(this->Name()); diff --git a/src/AltimeterSensor.cc b/src/AltimeterSensor.cc index f860b289..7d29a9b7 100644 --- a/src/AltimeterSensor.cc +++ b/src/AltimeterSensor.cc @@ -130,7 +130,14 @@ bool AltimeterSensor::Load(sdf::ElementPtr _sdf) } ////////////////////////////////////////////////// -bool AltimeterSensor::Update(const ignition::common::Time &_now) +bool AltimeterSensor::Update( + const ignition::common::Time &_now) +{ + return this->Update(math::secNsecToDuration(_now.sec, _now.nsec)); +} + +////////////////////////////////////////////////// +bool AltimeterSensor::Update(const std::chrono::steady_clock::duration &_now) { IGN_PROFILE("AltimeterSensor::Update"); if (!this->dataPtr->initialized) @@ -140,8 +147,7 @@ bool AltimeterSensor::Update(const ignition::common::Time &_now) } msgs::Altimeter msg; - msg.mutable_header()->mutable_stamp()->set_sec(_now.sec); - msg.mutable_header()->mutable_stamp()->set_nsec(_now.nsec); + *msg.mutable_header()->mutable_stamp() = msgs::Convert(_now); auto frame = msg.mutable_header()->add_data(); frame->set_key("frame_id"); frame->add_value(this->Name()); diff --git a/src/CameraSensor.cc b/src/CameraSensor.cc index 18d6482a..3e3046d1 100644 --- a/src/CameraSensor.cc +++ b/src/CameraSensor.cc @@ -306,7 +306,14 @@ void CameraSensor::SetScene(ignition::rendering::ScenePtr _scene) } ////////////////////////////////////////////////// -bool CameraSensor::Update(const ignition::common::Time &_now) +bool CameraSensor::Update( + const ignition::common::Time &_now) +{ + return this->Update(math::secNsecToDuration(_now.sec, _now.nsec)); +} + +////////////////////////////////////////////////// +bool CameraSensor::Update(const std::chrono::steady_clock::duration &_now) { IGN_PROFILE("CameraSensor::Update"); if (!this->dataPtr->initialized) @@ -387,8 +394,7 @@ bool CameraSensor::Update(const ignition::common::Time &_now) msg.set_step(width * rendering::PixelUtil::BytesPerPixel( this->dataPtr->camera->ImageFormat())); msg.set_pixel_format_type(msgsPixelFormat); - msg.mutable_header()->mutable_stamp()->set_sec(_now.sec); - msg.mutable_header()->mutable_stamp()->set_nsec(_now.nsec); + *msg.mutable_header()->mutable_stamp() = msgs::Convert(_now); auto frame = msg.mutable_header()->add_data(); frame->set_key("frame_id"); frame->add_value(this->Name()); @@ -524,14 +530,21 @@ bool CameraSensor::AdvertiseInfo(const std::string &_topic) } ////////////////////////////////////////////////// -void CameraSensor::PublishInfo(const ignition::common::Time &_now) +void CameraSensor::PublishInfo( + const std::chrono::steady_clock::duration &_now) { - this->dataPtr->infoMsg.mutable_header()->mutable_stamp()->set_sec(_now.sec); - this->dataPtr->infoMsg.mutable_header()->mutable_stamp()->set_nsec( - _now.nsec); + *this->dataPtr->infoMsg.mutable_header()->mutable_stamp() = + msgs::Convert(_now); this->dataPtr->infoPub.Publish(this->dataPtr->infoMsg); } +////////////////////////////////////////////////// +void CameraSensor::PublishInfo( + const ignition::common::Time &_now) +{ + this->PublishInfo(math::secNsecToDuration(_now.sec, _now.nsec)); +} + ////////////////////////////////////////////////// void CameraSensor::PopulateInfo(const sdf::Camera *_cameraSdf) { diff --git a/src/Camera_TEST.cc b/src/Camera_TEST.cc index d59fbb4b..521070b2 100644 --- a/src/Camera_TEST.cc +++ b/src/Camera_TEST.cc @@ -144,7 +144,7 @@ TEST(Camera_TEST, CreateCamera) // however camera is not loaded because a rendering scene is missing so // updates will not be successful and image size will be 0 - EXPECT_FALSE(cam->Update(ignition::common::Time())); + EXPECT_FALSE(cam->Update(std::chrono::steady_clock::duration::zero())); EXPECT_EQ(0u, cam->ImageWidth()); EXPECT_EQ(0u, cam->ImageHeight()); diff --git a/src/DepthCameraSensor.cc b/src/DepthCameraSensor.cc index a0277646..d78822e4 100644 --- a/src/DepthCameraSensor.cc +++ b/src/DepthCameraSensor.cc @@ -499,6 +499,13 @@ void DepthCameraSensor::SetScene(ignition::rendering::ScenePtr _scene) ////////////////////////////////////////////////// bool DepthCameraSensor::Update(const ignition::common::Time &_now) +{ + return this->Update(math::secNsecToDuration(_now.sec, _now.nsec)); +} + +////////////////////////////////////////////////// +bool DepthCameraSensor::Update( + const std::chrono::steady_clock::duration &_now) { IGN_PROFILE("DepthCameraSensor::Update"); if (!this->dataPtr->initialized) @@ -528,8 +535,7 @@ bool DepthCameraSensor::Update(const ignition::common::Time &_now) msg.set_step(width * rendering::PixelUtil::BytesPerPixel( rendering::PF_FLOAT32_R)); msg.set_pixel_format_type(msgsFormat); - msg.mutable_header()->mutable_stamp()->set_sec(_now.sec); - msg.mutable_header()->mutable_stamp()->set_nsec(_now.nsec); + *msg.mutable_header()->mutable_stamp() = msgs::Convert(_now); auto frame = msg.mutable_header()->add_data(); frame->set_key("frame_id"); frame->add_value(this->Name()); @@ -560,10 +566,8 @@ bool DepthCameraSensor::Update(const ignition::common::Time &_now) this->dataPtr->pointCloudBuffer) { // Set the time stamp - this->dataPtr->pointMsg.mutable_header()->mutable_stamp()->set_sec( - _now.sec); - this->dataPtr->pointMsg.mutable_header()->mutable_stamp()->set_nsec( - _now.nsec); + *this->dataPtr->pointMsg.mutable_header()->mutable_stamp() = + msgs::Convert(_now); this->dataPtr->pointMsg.set_is_dense(true); if (!this->dataPtr->xyzBuffer) diff --git a/src/GpuLidarSensor.cc b/src/GpuLidarSensor.cc index 2cbc059a..548e8d2a 100644 --- a/src/GpuLidarSensor.cc +++ b/src/GpuLidarSensor.cc @@ -205,6 +205,12 @@ bool GpuLidarSensor::CreateLidar() ////////////////////////////////////////////////// bool GpuLidarSensor::Update(const ignition::common::Time &_now) +{ + return this->Update(math::secNsecToDuration(_now.sec, _now.nsec)); +} + +////////////////////////////////////////////////// +bool GpuLidarSensor::Update(const std::chrono::steady_clock::duration &_now) { IGN_PROFILE("GpuLidarSensor::Update"); if (!this->initialized) @@ -237,11 +243,8 @@ bool GpuLidarSensor::Update(const ignition::common::Time &_now) if (this->dataPtr->pointPub.HasConnections()) { // Set the time stamp - this->dataPtr->pointMsg.mutable_header()->mutable_stamp()->set_sec( - _now.sec); - this->dataPtr->pointMsg.mutable_header()->mutable_stamp()->set_nsec( - _now.nsec); - + *this->dataPtr->pointMsg.mutable_header()->mutable_stamp() = + msgs::Convert(_now); this->dataPtr->pointMsg.set_is_dense(true); this->dataPtr->FillPointCloudMsg(); diff --git a/src/ImuSensor.cc b/src/ImuSensor.cc index ad617027..2c9103f4 100644 --- a/src/ImuSensor.cc +++ b/src/ImuSensor.cc @@ -61,7 +61,8 @@ class ignition::sensors::ImuSensorPrivate public: bool timeInitialized = false; /// \brief Previous update time step. - public: ignition::common::Time prevStep { ignition::common::Time::Zero }; + public: std::chrono::steady_clock::duration prevStep + {std::chrono::steady_clock::duration::zero()}; /// \brief Noise added to sensor data public: std::map noises; @@ -146,7 +147,14 @@ bool ImuSensor::Load(sdf::ElementPtr _sdf) } ////////////////////////////////////////////////// -bool ImuSensor::Update(const ignition::common::Time &_now) +bool ImuSensor::Update( + const ignition::common::Time &_now) +{ + return this->Update(math::secNsecToDuration(_now.sec, _now.nsec)); +} + +////////////////////////////////////////////////// +bool ImuSensor::Update(const std::chrono::steady_clock::duration &_now) { IGN_PROFILE("ImuSensor::Update"); if (!this->dataPtr->initialized) @@ -165,7 +173,9 @@ bool ImuSensor::Update(const ignition::common::Time &_now) double dt; if (this->dataPtr->timeInitialized) { - dt = (_now - this->dataPtr->prevStep).Double(); + auto delay = std::chrono::duration_cast>( + _now - this->dataPtr->prevStep); + dt = delay.count(); } else { @@ -200,8 +210,7 @@ bool ImuSensor::Update(const ignition::common::Time &_now) this->dataPtr->worldPose.Rot(); msgs::IMU msg; - msg.mutable_header()->mutable_stamp()->set_sec(_now.sec); - msg.mutable_header()->mutable_stamp()->set_nsec(_now.nsec); + *msg.mutable_header()->mutable_stamp() = msgs::Convert(_now); msg.set_entity_name(this->Name()); auto frame = msg.mutable_header()->add_data(); frame->set_key("frame_id"); diff --git a/src/ImuSensor_TEST.cc b/src/ImuSensor_TEST.cc index 7838652e..8ee8fd02 100644 --- a/src/ImuSensor_TEST.cc +++ b/src/ImuSensor_TEST.cc @@ -256,8 +256,10 @@ TEST(ImuSensor_TEST, ComputeNoise) // Note no gravity. sensor_truth->SetGravity(math::Vector3d::Zero); - sensor->Update(common::Time(0, 10000000)); - sensor_truth->Update(common::Time(0, 10000000)); + sensor->Update(std::chrono::steady_clock::duration( + std::chrono::nanoseconds(10000000))); + sensor_truth->Update(std::chrono::steady_clock::duration( + std::chrono::nanoseconds(10000000))); // Since this IMU has no noise, measurements should equal the inputs. EXPECT_DOUBLE_EQ(sensor_truth->AngularVelocity().X(), 0.0); @@ -275,8 +277,10 @@ TEST(ImuSensor_TEST, ComputeNoise) EXPECT_GT(sensor->AngularVelocity().SquaredLength(), 0.0); EXPECT_GT(sensor->LinearAcceleration().SquaredLength(), 0.0); - sensor->Update(common::Time(0, 20000000)); - sensor_truth->Update(common::Time(0, 20000000)); + sensor->Update(std::chrono::steady_clock::duration( + std::chrono::nanoseconds(20000000))); + sensor_truth->Update(std::chrono::steady_clock::duration( + std::chrono::nanoseconds(20000000))); // Since this IMU has no noise, measurements should equal the inputs. EXPECT_DOUBLE_EQ(sensor_truth->AngularVelocity().X(), 0.0); diff --git a/src/Lidar.cc b/src/Lidar.cc index d74040c0..b5c6147a 100644 --- a/src/Lidar.cc +++ b/src/Lidar.cc @@ -186,7 +186,14 @@ ignition::common::ConnectionPtr Lidar::ConnectNewLidarFrame( } ////////////////////////////////////////////////// -bool Lidar::Update(const ignition::common::Time &/*_now*/) +bool Lidar::Update( + const ignition::common::Time &_now) +{ + return this->Update(math::secNsecToDuration(_now.sec, _now.nsec)); +} + +////////////////////////////////////////////////// +bool Lidar::Update(const std::chrono::steady_clock::duration &/*_now*/) { ignerr << "No lidar data being updated.\n"; return false; @@ -194,6 +201,12 @@ bool Lidar::Update(const ignition::common::Time &/*_now*/) ////////////////////////////////////////////////// bool Lidar::PublishLidarScan(const ignition::common::Time &_now) +{ + return this->PublishLidarScan(math::secNsecToDuration(_now.sec, _now.nsec)); +} + +////////////////////////////////////////////////// +bool Lidar::PublishLidarScan(const std::chrono::steady_clock::duration &_now) { IGN_PROFILE("Lidar::PublishLidarScan"); if (!this->laserBuffer) @@ -201,10 +214,8 @@ bool Lidar::PublishLidarScan(const ignition::common::Time &_now) std::lock_guard lock(this->lidarMutex); - this->dataPtr->laserMsg.mutable_header()->mutable_stamp()->set_sec( - _now.sec); - this->dataPtr->laserMsg.mutable_header()->mutable_stamp()->set_nsec( - _now.nsec); + *this->dataPtr->laserMsg.mutable_header()->mutable_stamp() = + msgs::Convert(_now); // Remove 'data' entries before adding new ones this->dataPtr->laserMsg.mutable_header()->clear_data(); auto frame = this->dataPtr->laserMsg.mutable_header()->add_data(); diff --git a/src/LogicalCameraSensor.cc b/src/LogicalCameraSensor.cc index 82579baf..75294581 100644 --- a/src/LogicalCameraSensor.cc +++ b/src/LogicalCameraSensor.cc @@ -129,7 +129,15 @@ void LogicalCameraSensor::SetModelPoses( } ////////////////////////////////////////////////// -bool LogicalCameraSensor::Update(const ignition::common::Time &_now) +bool LogicalCameraSensor::Update( + const ignition::common::Time &_now) +{ + return this->Update(math::secNsecToDuration(_now.sec, _now.nsec)); +} + +////////////////////////////////////////////////// +bool LogicalCameraSensor::Update( + const std::chrono::steady_clock::duration &_now) { IGN_PROFILE("LogicalCameraSensor::Update"); if (!this->dataPtr->initialized) @@ -156,9 +164,7 @@ bool LogicalCameraSensor::Update(const ignition::common::Time &_now) msgs::Set(modelMsg->mutable_pose(), it.second - this->Pose()); } } - - this->dataPtr->msg.mutable_header()->mutable_stamp()->set_sec(_now.sec); - this->dataPtr->msg.mutable_header()->mutable_stamp()->set_nsec(_now.nsec); + *this->dataPtr->msg.mutable_header()->mutable_stamp() = msgs::Convert(_now); // Remove 'data' entries before adding new ones this->dataPtr->msg.mutable_header()->clear_data(); auto frame = this->dataPtr->msg.mutable_header()->add_data(); diff --git a/src/MagnetometerSensor.cc b/src/MagnetometerSensor.cc index 87698227..325b92df 100644 --- a/src/MagnetometerSensor.cc +++ b/src/MagnetometerSensor.cc @@ -137,7 +137,15 @@ bool MagnetometerSensor::Load(sdf::ElementPtr _sdf) } ////////////////////////////////////////////////// -bool MagnetometerSensor::Update(const ignition::common::Time &_now) +bool MagnetometerSensor::Update( + const ignition::common::Time &_now) +{ + return this->Update(math::secNsecToDuration(_now.sec, _now.nsec)); +} + +////////////////////////////////////////////////// +bool MagnetometerSensor::Update( + const std::chrono::steady_clock::duration &_now) { IGN_PROFILE("MagnetometerSensor::Update"); if (!this->dataPtr->initialized) @@ -152,8 +160,7 @@ bool MagnetometerSensor::Update(const ignition::common::Time &_now) this->dataPtr->worldField); msgs::Magnetometer msg; - msg.mutable_header()->mutable_stamp()->set_sec(_now.sec); - msg.mutable_header()->mutable_stamp()->set_nsec(_now.nsec); + *msg.mutable_header()->mutable_stamp() = msgs::Convert(_now); auto frame = msg.mutable_header()->add_data(); frame->set_key("frame_id"); frame->add_value(this->Name()); diff --git a/src/Manager.cc b/src/Manager.cc index 9e49eb24..9f337279 100644 --- a/src/Manager.cc +++ b/src/Manager.cc @@ -93,6 +93,13 @@ bool Manager::Remove(const ignition::sensors::SensorId _id) ////////////////////////////////////////////////// void Manager::RunOnce(const ignition::common::Time &_time, bool _force) +{ + this->RunOnce(math::secNsecToDuration(_time.sec, _time.nsec), _force); +} + +////////////////////////////////////////////////// +void Manager::RunOnce( + const std::chrono::steady_clock::duration &_time, bool _force) { IGN_PROFILE("SensorManager::RunOnce"); for (auto &s : this->dataPtr->sensors) diff --git a/src/RgbdCameraSensor.cc b/src/RgbdCameraSensor.cc index 2e672563..48ce5af3 100644 --- a/src/RgbdCameraSensor.cc +++ b/src/RgbdCameraSensor.cc @@ -409,6 +409,12 @@ void RgbdCameraSensorPrivate::OnNewRgbPointCloud(const float *_scan, ////////////////////////////////////////////////// bool RgbdCameraSensor::Update(const ignition::common::Time &_now) +{ + return this->Update(math::secNsecToDuration(_now.sec, _now.nsec)); +} + +////////////////////////////////////////////////// +bool RgbdCameraSensor::Update(const std::chrono::steady_clock::duration &_now) { IGN_PROFILE("RgbdCameraSensor::Update"); if (!this->dataPtr->initialized) @@ -439,8 +445,7 @@ bool RgbdCameraSensor::Update(const ignition::common::Time &_now) msg.set_step(width * rendering::PixelUtil::BytesPerPixel( rendering::PF_FLOAT32_R)); msg.set_pixel_format_type(msgs::PixelFormatType::R_FLOAT32); - msg.mutable_header()->mutable_stamp()->set_sec(_now.sec); - msg.mutable_header()->mutable_stamp()->set_nsec(_now.nsec); + *msg.mutable_header()->mutable_stamp() = msgs::Convert(_now); auto frame = msg.mutable_header()->add_data(); frame->set_key("frame_id"); frame->add_value(this->Name()); @@ -492,11 +497,9 @@ bool RgbdCameraSensor::Update(const ignition::common::Time &_now) // publish point cloud msg if (this->dataPtr->pointPub.HasConnections()) { + *this->dataPtr->pointMsg.mutable_header()->mutable_stamp() = + msgs::Convert(_now); // Set the time stamp - this->dataPtr->pointMsg.mutable_header()->mutable_stamp()->set_sec( - _now.sec); - this->dataPtr->pointMsg.mutable_header()->mutable_stamp()->set_nsec( - _now.nsec); this->dataPtr->pointMsg.set_is_dense(true); if ((this->dataPtr->hasDepthNearClip || this->dataPtr->hasDepthFarClip) @@ -554,8 +557,7 @@ bool RgbdCameraSensor::Update(const ignition::common::Time &_now) msg.set_step(width * rendering::PixelUtil::BytesPerPixel( rendering::PF_R8G8B8)); msg.set_pixel_format_type(msgs::PixelFormatType::RGB_INT8); - msg.mutable_header()->mutable_stamp()->set_sec(_now.sec); - msg.mutable_header()->mutable_stamp()->set_nsec(_now.nsec); + *msg.mutable_header()->mutable_stamp() = msgs::Convert(_now); auto frame = msg.mutable_header()->add_data(); frame->set_key("frame_id"); frame->add_value(this->Name()); diff --git a/src/Sensor.cc b/src/Sensor.cc index 6a09277b..11400628 100644 --- a/src/Sensor.cc +++ b/src/Sensor.cc @@ -58,7 +58,8 @@ class ignition::sensors::SensorPrivate public: double updateRate = 0.0; /// \brief What sim time should this sensor update at - public: ignition::common::Time nextUpdateTime; + public: std::chrono::steady_clock::duration nextUpdateTime + {std::chrono::steady_clock::duration::zero()}; /// \brief SDF element with sensor information. public: sdf::ElementPtr sdf = nullptr; @@ -239,7 +240,7 @@ void Sensor::SetUpdateRate(const double _hz) } ////////////////////////////////////////////////// -bool Sensor::Update(const ignition::common::Time &_now, +bool Sensor::Update(const std::chrono::steady_clock::duration &_now, const bool _force) { IGN_PROFILE("Sensor::Update"); @@ -258,7 +259,8 @@ bool Sensor::Update(const ignition::common::Time &_now, if (!_force && this->dataPtr->updateRate > 0.0) { // Update the time the plugin should be loaded - ignition::common::Time delta(1.0 / this->dataPtr->updateRate); + auto delta = std::chrono::duration_cast< std::chrono::milliseconds> + (std::chrono::duration< double >(1.0 / this->dataPtr->updateRate)); this->dataPtr->nextUpdateTime += delta; } @@ -266,11 +268,25 @@ bool Sensor::Update(const ignition::common::Time &_now, } ////////////////////////////////////////////////// -ignition::common::Time Sensor::NextUpdateTime() const +bool Sensor::Update(const ignition::common::Time &_now, const bool _force) +{ + return this->Update(math::secNsecToDuration(_now.sec, _now.nsec), _force); +} + +////////////////////////////////////////////////// +std::chrono::steady_clock::duration Sensor::NextDataUpdateTime() const { return this->dataPtr->nextUpdateTime; } +////////////////////////////////////////////////// +ignition::common::Time Sensor::NextUpdateTime() const +{ + std::pair secNsec = + math::durationToSecNsec(this->dataPtr->nextUpdateTime); + return common::Time(secNsec.first, secNsec.second); +} + ///////////////////////////////////////////////// void Sensor::AddSequence(ignition::msgs::Header *_msg, const std::string &_seqKey) diff --git a/src/Sensor_TEST.cc b/src/Sensor_TEST.cc index 8acd0db0..e1493f30 100644 --- a/src/Sensor_TEST.cc +++ b/src/Sensor_TEST.cc @@ -24,12 +24,17 @@ using namespace sensors; class TestSensor : public Sensor { - public: bool Update(const common::Time &) override + public: bool Update(const std::chrono::steady_clock::duration &) override { updateCount++; return true; } + public: bool Update(const common::Time &) override + { + return false; + } + public: unsigned int updateCount{0}; }; diff --git a/src/ThermalCameraSensor.cc b/src/ThermalCameraSensor.cc index b19734f1..1b40c67b 100644 --- a/src/ThermalCameraSensor.cc +++ b/src/ThermalCameraSensor.cc @@ -363,6 +363,13 @@ void ThermalCameraSensor::SetScene(ignition::rendering::ScenePtr _scene) ////////////////////////////////////////////////// bool ThermalCameraSensor::Update(const ignition::common::Time &_now) +{ + return this->Update(math::secNsecToDuration(_now.sec, _now.nsec)); +} + +////////////////////////////////////////////////// +bool ThermalCameraSensor::Update( + const std::chrono::steady_clock::duration &_now) { IGN_PROFILE("ThermalCameraSensor::Update"); if (!this->dataPtr->initialized) @@ -400,8 +407,7 @@ bool ThermalCameraSensor::Update(const ignition::common::Time &_now) width * rendering::PixelUtil::BytesPerPixel(rendering::PF_L16)); this->dataPtr->thermalMsg.set_pixel_format_type(msgsFormat); auto stamp = this->dataPtr->thermalMsg.mutable_header()->mutable_stamp(); - stamp->set_sec(_now.sec); - stamp->set_nsec(_now.nsec); + *stamp = msgs::Convert(_now); auto frame = this->dataPtr->thermalMsg.mutable_header()->add_data(); frame->set_key("frame_id"); frame->add_value(this->Name()); diff --git a/test/integration/air_pressure_plugin.cc b/test/integration/air_pressure_plugin.cc index d42c1a43..c48fefe2 100644 --- a/test/integration/air_pressure_plugin.cc +++ b/test/integration/air_pressure_plugin.cc @@ -202,7 +202,7 @@ TEST_F(AirPressureSensorTest, SensorReadings) // verify msg received on the topic WaitForMessageTestHelper msgHelper(topic); - sensor->Update(ignition::common::Time(1, 0)); + sensor->Update(std::chrono::steady_clock::duration(std::chrono::seconds(1))); EXPECT_TRUE(msgHelper.WaitForMessage()) << msgHelper; auto msg = msgHelper.Message(); EXPECT_EQ(1, msg.header().stamp().sec()); @@ -213,7 +213,8 @@ TEST_F(AirPressureSensorTest, SensorReadings) // verify msg with noise received on the topic WaitForMessageTestHelper msgHelperNoise(topicNoise); - sensorNoise->Update(ignition::common::Time(1, 0)); + sensorNoise->Update(std::chrono::steady_clock::duration( + std::chrono::seconds(1))); EXPECT_TRUE(msgHelperNoise.WaitForMessage()) << msgHelperNoise; auto msgNoise = msgHelperNoise.Message(); EXPECT_EQ(1, msg.header().stamp().sec()); diff --git a/test/integration/altimeter_plugin.cc b/test/integration/altimeter_plugin.cc index e0788604..ab202a81 100644 --- a/test/integration/altimeter_plugin.cc +++ b/test/integration/altimeter_plugin.cc @@ -227,7 +227,7 @@ TEST_F(AltimeterSensorTest, SensorReadings) // verify msg received on the topic WaitForMessageTestHelper msgHelper(topic); - sensor->Update(ignition::common::Time(1, 0)); + sensor->Update(std::chrono::steady_clock::duration(std::chrono::seconds(1))); EXPECT_TRUE(msgHelper.WaitForMessage()) << msgHelper; auto msg = msgHelper.Message(); EXPECT_EQ(1, msg.header().stamp().sec()); @@ -239,7 +239,8 @@ TEST_F(AltimeterSensorTest, SensorReadings) // verify msg with noise received on the topic WaitForMessageTestHelper msgHelperNoise(topicNoise); - sensorNoise->Update(ignition::common::Time(1, 0)); + sensorNoise->Update(std::chrono::steady_clock::duration( + std::chrono::seconds(1))); EXPECT_TRUE(msgHelperNoise.WaitForMessage()) << msgHelperNoise; auto msgNoise = msgHelperNoise.Message(); EXPECT_EQ(1, msg.header().stamp().sec()); diff --git a/test/integration/camera_plugin.cc b/test/integration/camera_plugin.cc index d9724a0d..18bfd38e 100644 --- a/test/integration/camera_plugin.cc +++ b/test/integration/camera_plugin.cc @@ -79,7 +79,7 @@ void CameraSensorTest::ImagesWithBuiltinSDF(const std::string &_renderEngine) WaitForMessageTestHelper helper(topic); // Update once to create image - mgr.RunOnce(ignition::common::Time::Zero); + mgr.RunOnce(std::chrono::steady_clock::duration::zero()); EXPECT_TRUE(helper.WaitForMessage()) << helper; diff --git a/test/integration/depth_camera_plugin.cc b/test/integration/depth_camera_plugin.cc index fded2bcb..fe7aeb31 100644 --- a/test/integration/depth_camera_plugin.cc +++ b/test/integration/depth_camera_plugin.cc @@ -229,7 +229,7 @@ void DepthCameraSensorTest::ImagesWithBuiltinSDF( WaitForMessageTestHelper infoHelper(infoTopic); // Update once to create image - mgr.RunOnce(ignition::common::Time::Zero); + mgr.RunOnce(std::chrono::steady_clock::duration::zero()); EXPECT_TRUE(helper.WaitForMessage()) << helper; EXPECT_TRUE(pointsHelper.WaitForMessage()) << pointsHelper; @@ -246,14 +246,15 @@ void DepthCameraSensorTest::ImagesWithBuiltinSDF( node.Subscribe(infoTopic, &OnCameraInfo); // wait for a few depth camera frames - mgr.RunOnce(ignition::common::Time::Zero, true); + mgr.RunOnce(std::chrono::steady_clock::duration::zero(), true); int midWidth = depthSensor->ImageWidth() * 0.5; int midHeight = depthSensor->ImageHeight() * 0.5; int mid = midHeight * depthSensor->ImageWidth() + midWidth -1; double expectedRangeAtMidPoint = boxPosition.X() - unitBoxSize * 0.5; - ignition::common::Time waitTime = ignition::common::Time(0.001); + auto waitTime = std::chrono::duration_cast< std::chrono::milliseconds >( + std::chrono::duration< double >(0.001)); int counter = 0; int infoCounter = 0; int pcCounter = 0; @@ -269,7 +270,7 @@ void DepthCameraSensorTest::ImagesWithBuiltinSDF( infoCounter = g_infoCounter; infoMsg = g_infoMsg; g_infoMutex.unlock(); - ignition::common::Time::Sleep(waitTime); + std::this_thread::sleep_for(waitTime); } g_mutex.lock(); g_infoMutex.lock(); @@ -317,7 +318,7 @@ void DepthCameraSensorTest::ImagesWithBuiltinSDF( box->SetLocalPosition(boxPositionNear); root->AddChild(box); - mgr.RunOnce(ignition::common::Time::Zero, true); + mgr.RunOnce(std::chrono::steady_clock::duration::zero(), true); for (int sleep = 0; sleep < 300 && (counter == 0 || infoCounter == 0); ++sleep) { @@ -328,7 +329,7 @@ void DepthCameraSensorTest::ImagesWithBuiltinSDF( g_infoMutex.lock(); infoCounter = g_infoCounter; g_infoMutex.unlock(); - ignition::common::Time::Sleep(waitTime); + std::this_thread::sleep_for(waitTime); } g_mutex.lock(); @@ -353,7 +354,7 @@ void DepthCameraSensorTest::ImagesWithBuiltinSDF( box->SetLocalPosition(boxPositionFar); root->AddChild(box); - mgr.RunOnce(ignition::common::Time::Zero, true); + mgr.RunOnce(std::chrono::steady_clock::duration::zero(), true); for (int sleep = 0; sleep < 300 && (counter == 0 || infoCounter == 0); ++sleep) { @@ -364,7 +365,7 @@ void DepthCameraSensorTest::ImagesWithBuiltinSDF( g_infoMutex.lock(); infoCounter = g_infoCounter; g_infoMutex.unlock(); - ignition::common::Time::Sleep(waitTime); + std::this_thread::sleep_for(waitTime); } g_mutex.lock(); g_infoMutex.lock(); @@ -389,7 +390,7 @@ void DepthCameraSensorTest::ImagesWithBuiltinSDF( box->SetLocalPosition(boxPositionFillFrame); root->AddChild(box); - mgr.RunOnce(ignition::common::Time::Zero, true); + mgr.RunOnce(std::chrono::steady_clock::duration::zero(), true); for (int sleep = 0; sleep < 300 && (counter == 0 || infoCounter == 0 || pcCounter == 0); ++sleep) @@ -406,7 +407,7 @@ void DepthCameraSensorTest::ImagesWithBuiltinSDF( pcCounter = g_pcCounter; g_pcMutex.unlock(); - ignition::common::Time::Sleep(waitTime); + std::this_thread::sleep_for(waitTime); } g_mutex.lock(); g_infoMutex.lock(); diff --git a/test/integration/gpu_lidar_sensor_plugin.cc b/test/integration/gpu_lidar_sensor_plugin.cc index c4c0d8e6..98793a38 100644 --- a/test/integration/gpu_lidar_sensor_plugin.cc +++ b/test/integration/gpu_lidar_sensor_plugin.cc @@ -233,7 +233,7 @@ void GpuLidarSensorTest::CreateGpuLidar(const std::string &_renderEngine) WaitForMessageTestHelper helper(topic); // Update once to verify that a message is sent - mgr.RunOnce(ignition::common::Time::Zero); + mgr.RunOnce(std::chrono::steady_clock::duration::zero()); EXPECT_TRUE(helper.WaitForMessage()) << helper; @@ -335,7 +335,7 @@ void GpuLidarSensorTest::DetectBox(const std::string &_renderEngine) WaitForMessageTestHelper helper(topic); // Update sensor - mgr.RunOnce(ignition::common::Time::Zero, true); + mgr.RunOnce(std::chrono::steady_clock::duration::zero(), true); EXPECT_TRUE(helper.WaitForMessage()) << helper; int mid = horzSamples / 2; @@ -350,11 +350,12 @@ void GpuLidarSensorTest::DetectBox(const std::string &_renderEngine) EXPECT_DOUBLE_EQ(sensor->Range(last), ignition::math::INF_D); // Make sure to wait to receive the message - ignition::common::Time waitTime = ignition::common::Time(0.01); + auto waitTime = std::chrono::duration_cast< std::chrono::milliseconds >( + std::chrono::duration< double >(0.01)); int i = 0; while ((laserMsgs.empty() || pointMsgs.empty()) && i < 300) { - ignition::common::Time::Sleep(waitTime); + std::this_thread::sleep_for(waitTime); i++; } EXPECT_LT(i, 300); @@ -508,7 +509,7 @@ void GpuLidarSensorTest::TestThreeBoxes(const std::string &_renderEngine) root->AddChild(visualBox3); // Update sensors - mgr.RunOnce(ignition::common::Time::Zero); + mgr.RunOnce(std::chrono::steady_clock::duration::zero()); int mid = horzSamples / 2; int last = (horzSamples - 1); @@ -536,7 +537,7 @@ void GpuLidarSensorTest::TestThreeBoxes(const std::string &_renderEngine) visualBox2->SetLocalPosition(box2PositionFar); // Update sensors - mgr.RunOnce(ignition::common::Time::Zero, true); + mgr.RunOnce(std::chrono::steady_clock::duration::zero(), true); // Verify values out of range for (unsigned int i = 0; i < sensor1->RayCount(); ++i) @@ -619,7 +620,7 @@ void GpuLidarSensorTest::VerticalLidar(const std::string &_renderEngine) sensor->SetScene(scene); // Update sensor - mgr.RunOnce(ignition::common::Time::Zero); + mgr.RunOnce(std::chrono::steady_clock::duration::zero()); unsigned int mid = horzSamples / 2; double unitBoxSize = 1.0; @@ -651,7 +652,7 @@ void GpuLidarSensorTest::VerticalLidar(const std::string &_renderEngine) ignition::math::Vector3d(rangeMax + 1, 0, 0)); // Wait for a few more laser scans - mgr.RunOnce(ignition::common::Time::Zero, true); + mgr.RunOnce(std::chrono::steady_clock::duration::zero(), true); // Verify all values are out of range for (unsigned int j = 0; j < sensor->VerticalRayCount(); ++j) @@ -762,7 +763,7 @@ void GpuLidarSensorTest::ManualUpdate(const std::string &_renderEngine) scene->PreRender(); // Render and update - mgr.RunOnce(ignition::common::Time::Zero); + mgr.RunOnce(std::chrono::steady_clock::duration::zero()); int mid = horzSamples / 2; int last = (horzSamples - 1); diff --git a/test/integration/imu_plugin.cc b/test/integration/imu_plugin.cc index d7c52794..908ca723 100644 --- a/test/integration/imu_plugin.cc +++ b/test/integration/imu_plugin.cc @@ -156,7 +156,8 @@ TEST_F(ImuSensorTest, SensorReadings) EXPECT_EQ(ignition::math::Quaterniond::Identity, sensor->Orientation()); // update sensor and verify new readings - EXPECT_TRUE(sensor->Update(ignition::common::Time(1, 0))); + EXPECT_TRUE(sensor->Update(std::chrono::steady_clock::duration( + std::chrono::seconds(1)))); EXPECT_EQ(orientRef, sensor->OrientationReference()); EXPECT_EQ(gravity, sensor->Gravity()); EXPECT_EQ(pose, sensor->WorldPose()); @@ -196,7 +197,8 @@ TEST_F(ImuSensorTest, SensorReadings) EXPECT_EQ(newPose, sensor->WorldPose()); // update sensor and verify new readings - EXPECT_TRUE(sensor->Update(ignition::common::Time(2, 0))); + EXPECT_TRUE(sensor->Update(std::chrono::steady_clock::duration( + std::chrono::seconds(2)))); EXPECT_EQ(orientRef, sensor->OrientationReference()); EXPECT_EQ(gravity, sensor->Gravity()); EXPECT_EQ(angularVel, sensor->AngularVelocity()); diff --git a/test/integration/logical_camera_plugin.cc b/test/integration/logical_camera_plugin.cc index 403367fb..b5c14b81 100644 --- a/test/integration/logical_camera_plugin.cc +++ b/test/integration/logical_camera_plugin.cc @@ -168,7 +168,7 @@ TEST_F(LogicalCameraSensorTest, DetectBox) sensor->SetModelPoses(std::move(modelPoses)); // update - sensor->Update(ignition::common::Time::Zero); + sensor->Update(std::chrono::steady_clock::duration::zero()); // verify box is in image img = sensor->Image(); @@ -186,7 +186,7 @@ TEST_F(LogicalCameraSensorTest, DetectBox) sensor->SetModelPoses(std::move(modelPoses2)); // update - sensor->Update(ignition::common::Time::Zero); + sensor->Update(std::chrono::steady_clock::duration::zero()); // verify box is not in the image img = sensor->Image(); @@ -206,7 +206,7 @@ TEST_F(LogicalCameraSensorTest, DetectBox) sensor->SetModelPoses(std::move(modelPoses3)); // update - sensor->Update(ignition::common::Time::Zero); + sensor->Update(std::chrono::steady_clock::duration::zero()); // verify box is in image img = sensor->Image(); @@ -222,7 +222,7 @@ TEST_F(LogicalCameraSensorTest, DetectBox) sensor->SetPose(sensorPose4); // update - sensor->Update(ignition::common::Time::Zero); + sensor->Update(std::chrono::steady_clock::duration::zero()); // verify box is no longer in the image img = sensor->Image(); diff --git a/test/integration/magnetometer_plugin.cc b/test/integration/magnetometer_plugin.cc index 694448b3..ee44ccae 100644 --- a/test/integration/magnetometer_plugin.cc +++ b/test/integration/magnetometer_plugin.cc @@ -238,12 +238,14 @@ TEST_F(MagnetometerSensorTest, SensorReadings) // update sensor and verify new readings // there are not sensor rotations so the magnetic fields in body frame and // world frame should be the same - EXPECT_TRUE(sensor->Update(ignition::common::Time(1, 0))); + EXPECT_TRUE(sensor->Update(std::chrono::steady_clock::duration( + std::chrono::seconds(1)))); EXPECT_EQ(pose, sensor->WorldPose()); EXPECT_EQ(worldField, sensor->WorldMagneticField()); EXPECT_EQ(worldField, sensor->MagneticField()); - EXPECT_TRUE(sensorNoise->Update(ignition::common::Time(1, 0))); + EXPECT_TRUE(sensorNoise->Update(std::chrono::steady_clock::duration( + std::chrono::seconds(1)))); EXPECT_EQ(poseNoise, sensorNoise->WorldPose()); EXPECT_EQ(worldFieldNoise, sensorNoise->WorldMagneticField()); // There should be noise in the MagneticField @@ -271,7 +273,8 @@ TEST_F(MagnetometerSensorTest, SensorReadings) EXPECT_EQ(newPose, sensor->WorldPose()); // update sensor and verify new readings - EXPECT_TRUE(sensor->Update(ignition::common::Time(2, 0))); + EXPECT_TRUE(sensor->Update(std::chrono::steady_clock::duration( + std::chrono::seconds(2)))); EXPECT_EQ(worldField, sensor->WorldMagneticField()); ignition::math::Vector3d localField = newOrientation.RotateVectorReverse(worldField); diff --git a/test/integration/rgbd_camera_plugin.cc b/test/integration/rgbd_camera_plugin.cc index 98de0fca..df6b2909 100644 --- a/test/integration/rgbd_camera_plugin.cc +++ b/test/integration/rgbd_camera_plugin.cc @@ -251,7 +251,7 @@ void RgbdCameraSensorTest::ImagesWithBuiltinSDF( WaitForMessageTestHelper infoHelper(infoTopic); // Update once to create image - mgr.RunOnce(ignition::common::Time::Zero); + mgr.RunOnce(std::chrono::steady_clock::duration::zero()); EXPECT_TRUE(depthHelper.WaitForMessage()) << depthHelper; EXPECT_TRUE(imageHelper.WaitForMessage()) << imageHelper; @@ -271,10 +271,11 @@ void RgbdCameraSensorTest::ImagesWithBuiltinSDF( node.Subscribe(infoTopic, &OnCameraInfo); // Update once more - mgr.RunOnce(ignition::common::Time::Zero, true); + mgr.RunOnce(std::chrono::steady_clock::duration::zero(), true); // wait for an image - ignition::common::Time waitTime = ignition::common::Time(0.001); + auto waitTime = std::chrono::duration_cast< std::chrono::milliseconds >( + std::chrono::duration< double >(0.001)); int counter = 0; int infoCounter = 0; int imgCounter = 0; @@ -297,7 +298,7 @@ void RgbdCameraSensorTest::ImagesWithBuiltinSDF( g_infoMutex.unlock(); g_imgMutex.unlock(); g_pcMutex.unlock(); - ignition::common::Time::Sleep(waitTime); + std::this_thread::sleep_for(waitTime); } g_mutex.lock(); @@ -504,7 +505,7 @@ void RgbdCameraSensorTest::ImagesWithBuiltinSDF( box->SetLocalPosition(boxPositionNear); root->AddChild(box); - mgr.RunOnce(ignition::common::Time::Zero, true); + mgr.RunOnce(std::chrono::steady_clock::duration::zero(), true); for (int sleep = 0; sleep < 300 && (counter == 0 || infoCounter == 0 || imgCounter == 0 || pcCounter == 0); ++sleep) @@ -522,7 +523,7 @@ void RgbdCameraSensorTest::ImagesWithBuiltinSDF( g_infoMutex.unlock(); g_imgMutex.unlock(); g_pcMutex.unlock(); - ignition::common::Time::Sleep(waitTime); + std::this_thread::sleep_for(waitTime); } g_mutex.lock(); @@ -610,7 +611,7 @@ void RgbdCameraSensorTest::ImagesWithBuiltinSDF( box->SetLocalPosition(boxPositionFar); root->AddChild(box); - mgr.RunOnce(ignition::common::Time::Zero, true); + mgr.RunOnce(std::chrono::steady_clock::duration::zero(), true); for (int sleep = 0; sleep < 300 && (counter == 0 || infoCounter == 0 || imgCounter == 0 || pcCounter == 0); ++sleep) @@ -628,7 +629,7 @@ void RgbdCameraSensorTest::ImagesWithBuiltinSDF( g_infoMutex.unlock(); g_imgMutex.unlock(); g_pcMutex.unlock(); - ignition::common::Time::Sleep(waitTime); + std::this_thread::sleep_for(waitTime); } g_mutex.lock(); diff --git a/test/integration/thermal_camera_plugin.cc b/test/integration/thermal_camera_plugin.cc index c4455b76..42e1d043 100644 --- a/test/integration/thermal_camera_plugin.cc +++ b/test/integration/thermal_camera_plugin.cc @@ -162,7 +162,7 @@ void ThermalCameraSensorTest::ImagesWithBuiltinSDF( WaitForMessageTestHelper infoHelper(infoTopic); // Update once to create image - mgr.RunOnce(ignition::common::Time::Zero); + mgr.RunOnce(std::chrono::steady_clock::duration::zero()); EXPECT_TRUE(helper.WaitForMessage()) << helper; EXPECT_TRUE(infoHelper.WaitForMessage()) << infoHelper; @@ -175,7 +175,7 @@ void ThermalCameraSensorTest::ImagesWithBuiltinSDF( node.Subscribe(infoTopic, &OnCameraInfo); // wait for a few thermal camera frames - mgr.RunOnce(ignition::common::Time::Zero, true); + mgr.RunOnce(std::chrono::steady_clock::duration::zero(), true); int midWidth = thermalSensor->ImageWidth() * 0.5; int midHeight = thermalSensor->ImageHeight() * 0.5; @@ -183,7 +183,8 @@ void ThermalCameraSensorTest::ImagesWithBuiltinSDF( int left = midHeight * thermalSensor->ImageWidth(); int right = (midHeight+1) * thermalSensor->ImageWidth() - 1; - ignition::common::Time waitTime = ignition::common::Time(0.001); + auto waitTime = std::chrono::duration_cast< std::chrono::milliseconds >( + std::chrono::duration< double >(0.001)); int counter = 0; int infoCounter = 0; ignition::msgs::CameraInfo infoMsg; @@ -198,7 +199,7 @@ void ThermalCameraSensorTest::ImagesWithBuiltinSDF( infoCounter = g_infoCounter; infoMsg = g_infoMsg; g_infoMutex.unlock(); - ignition::common::Time::Sleep(waitTime); + std::this_thread::sleep_for(waitTime); } g_mutex.lock(); g_infoMutex.lock(); @@ -248,7 +249,7 @@ void ThermalCameraSensorTest::ImagesWithBuiltinSDF( box->SetLocalPosition(boxPositionNear); root->AddChild(box); - mgr.RunOnce(ignition::common::Time::Zero, true); + mgr.RunOnce(std::chrono::steady_clock::duration::zero(), true); for (int sleep = 0; sleep < 300 && (counter == 0 || infoCounter == 0); ++sleep) { @@ -259,7 +260,7 @@ void ThermalCameraSensorTest::ImagesWithBuiltinSDF( g_infoMutex.lock(); infoCounter = g_infoCounter; g_infoMutex.unlock(); - ignition::common::Time::Sleep(waitTime); + std::this_thread::sleep_for(waitTime); } g_mutex.lock(); @@ -282,7 +283,7 @@ void ThermalCameraSensorTest::ImagesWithBuiltinSDF( box->SetLocalPosition(boxPositionFar); root->AddChild(box); - mgr.RunOnce(ignition::common::Time::Zero, true); + mgr.RunOnce(std::chrono::steady_clock::duration::zero(), true); for (int sleep = 0; sleep < 300 && (counter == 0 || infoCounter == 0); ++sleep) { @@ -293,7 +294,7 @@ void ThermalCameraSensorTest::ImagesWithBuiltinSDF( g_infoMutex.lock(); infoCounter = g_infoCounter; g_infoMutex.unlock(); - ignition::common::Time::Sleep(waitTime); + std::this_thread::sleep_for(waitTime); } g_mutex.lock(); g_infoMutex.lock();