diff --git a/Changelog.md b/Changelog.md index 6399b1b3..fc4e7906 100644 --- a/Changelog.md +++ b/Changelog.md @@ -29,6 +29,41 @@ ### Ignition Sensors 4.X.X +### Ignition Sensors 4.2.0 (2021-07-12) + +1. Add API for enabling / disabling IMU orientation + * [Pull request #142](https://github.com/ignitionrobotics/ign-sensors/pull/142) + +1. Init will now set the nextUpdateTime to zero + * [Pull request #137](https://github.com/ignitionrobotics/ign-sensors/pull/137) + +1. Remove clamping from lidar noise + * [Pull request #132](https://github.com/ignitionrobotics/ign-sensors/pull/132) + +1. Remove tools/code_check and update codecov + * [Pull request #130](https://github.com/ignitionrobotics/ign-sensors/pull/130) + +1. Disable macOS workflow + * [Pull request #124](https://github.com/ignitionrobotics/ign-sensors/pull/124) + +1. 👩‍🌾 Disable tests that consistently fail on macOS + * [Pull request #121](https://github.com/ignitionrobotics/ign-sensors/pull/121) + +1. Master branch updates + * [Pull request #106](https://github.com/ignitionrobotics/ign-sensors/pull/106) + +1. 👩‍🌾 Clear Windows warnings (backport #58) + * [Pull request #102](https://github.com/ignitionrobotics/ign-sensors/pull/102) + +1. Update thermal camera tutorial - include varying temp. objects + * [Pull request #79](https://github.com/ignitionrobotics/ign-sensors/pull/79) + +1. Fix macOS/windows tests that failed to load library + * [Pull request #60](https://github.com/ignitionrobotics/ign-sensors/pull/60) + +1. Removed issue & PR templates + * [Pull request #99](https://github.com/ignitionrobotics/ign-sensors/pull/99) + ### Ignition Sensors 4.1.0 (2021-02-10) 1. Added issue and PR templates. @@ -72,6 +107,30 @@ ## Ignition Sensors 3 +### Ignition Sensors 3.X.X (202X-XX-XX) + +### Ignition Sensors 3.3.0 (2021-08-26) + +1. 👩‍🌾 Print debug messages when sensors advertise topics + * [Pull request #151](https://github.com/ignitionrobotics/ign-sensors/pull/151) + +1. Publish performance sensor metrics. + * [Pull request #146](https://github.com/ignitionrobotics/ign-sensors/pull/146) + +1. CI and infrastructure + * [Pull request #130](https://github.com/ignitionrobotics/ign-sensors/pull/130) + * [Pull request #150](https://github.com/ignitionrobotics/ign-sensors/pull/150) + * [Pull request #106](https://github.com/ignitionrobotics/ign-sensors/pull/106) + +1. 👩‍🌾 Disable tests that consistently fail on macOS + * [Pull request #121](https://github.com/ignitionrobotics/ign-sensors/pull/121) + +1. 👩‍🌾 Clear Windows warnings (backport #58) + * [Pull request #58](https://github.com/ignitionrobotics/ign-sensors/pull/58) + +1. Fix macOS/windows tests that failed to load library (backport #60) + * [Pull request #60](https://github.com/ignitionrobotics/ign-sensors/pull/60) + ### Ignition Sensors 3.2.0 (2021-02-08) 1. Apply noise to lidar point cloud. diff --git a/bitbucket-pipelines.yml b/bitbucket-pipelines.yml deleted file mode 100644 index 02c81840..00000000 --- a/bitbucket-pipelines.yml +++ /dev/null @@ -1,88 +0,0 @@ -image: ubuntu:bionic - -pipelines: - default: - - step: - script: # Modify the commands below to build your repository. - - apt update - - apt -y install wget lsb-release gnupg - - sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-stable.list' - - wget http://packages.osrfoundation.org/gazebo.key -O - | apt-key add - - - apt-get update - - apt-get -y install - cmake pkg-config cppcheck doxygen ruby-ronn curl git g++-8 - - update-alternatives --install /usr/bin/gcc gcc /usr/bin/gcc-8 800 --slave /usr/bin/g++ g++ /usr/bin/g++-8 --slave /usr/bin/gcov gcov /usr/bin/gcov-8 - # lcov - - git clone https://github.com/linux-test-project/lcov.git -b v1.14 - - cd lcov - - make install - - cd .. - # Dependency: Ignition packages and sdformat - - apt-get -y install - libignition-cmake2-dev - libignition-common4-dev - libignition-math6-dev - libignition-msgs5-dev - libignition-tools-dev - libignition-transport8-dev - libsdformat9-dev - # libignition-rendering3-dev - # # SDFormat (uncomment if a specific branch is needed) - # - apt install -y - # libxml2-utils - # libtinyxml-dev - # - git clone http://github.com/osrf/sdformat - # - cd sdformat - # - mkdir build - # - cd build - # - cmake .. -DBUILD_TESTING=false - # - make -j4 install - # - cd ../.. - # # Ignition msgs (uncomment if a specific branch is needed) - # - apt install -y - # libprotobuf-dev protobuf-compiler libprotoc-dev - # - git clone http://github.com/ignitionrobotics/ign-msgs - # - cd ign-msgs - # - mkdir build - # - cd build - # - cmake .. - # - make -j4 install - # - cd ../.. - # # Ignition transport (uncomment if a specific branch is needed) - # - apt install -y - # libzmq3-dev uuid-dev libsqlite3-dev - # - git clone http://github.com/ignitionrobotics/ign-transport - # - cd ign-transport - # - mkdir build - # - cd build - # - cmake .. - # - make -j4 install - # - cd ../.. - # Install ign-rendering dependencies - - apt-get -y install - libogre-1.9-dev libogre-2.1-dev libglew-dev libfreeimage-dev freeglut3-dev libxmu-dev libxi-dev uuid-dev - - git clone http://github.com/ignitionrobotics/ign-rendering -b master - - cd ign-rendering - - mkdir build - - cd build - - cmake .. - - make -j4 install - - cd ../.. - # Ignition sensors - - mkdir build - - cd build - - cmake .. -DCMAKE_BUILD_TYPE=coverage -DDRI_TESTS=FALSE - - make -j4 install - - make test - - make coverage - # Use a special version of codecov for handling gcc8 output. - - bash <(curl -s https://raw.githubusercontent.com/codecov/codecov-bash/4678d212cce2078bbaaf5027af0c0dafaad6a095/codecov) -X gcovout - - make codecheck - # Examples - - apt-get -y install libfreeimage-dev - - cd .. - - cd examples/save_image - - mkdir build - - cd build - - cmake .. - - make diff --git a/include/ignition/sensors/ImuSensor.hh b/include/ignition/sensors/ImuSensor.hh index 46fba50a..a0212b72 100644 --- a/include/ignition/sensors/ImuSensor.hh +++ b/include/ignition/sensors/ImuSensor.hh @@ -111,13 +111,27 @@ namespace ignition public: math::Quaterniond OrientationReference() const; /// \brief Get the orienation of the imu with respect to reference frame - /// \return Orientation in reference frame + /// \return Orientation in reference frame. If orientation is not + /// enabled, this will return the last computed orientation before + /// orientation is disabled or identity Quaternion if orientation has + /// never been enabled. public: math::Quaterniond Orientation() const; /// \brief Set the gravity vector /// \param[in] _gravity gravity vector in meters per second squared. public: void SetGravity(const math::Vector3d &_gravity); + /// \brief Set whether to output orientation. Not all imu's generate + /// orientation values as they use filters to produce orientation + /// estimates. + /// \param[in] _enabled True to publish orientation data, false to leave + /// the message field empty. + public: void SetOrientationEnabled(bool _enabled); + + /// \brief Get whether or not orientation is enabled. + /// \return True if orientation is enabled, false otherwise. + public: bool OrientationEnabled() const; + /// \brief Get the gravity vector /// \return Gravity vectory in meters per second squared. public: math::Vector3d Gravity() const; diff --git a/include/ignition/sensors/Sensor.hh b/include/ignition/sensors/Sensor.hh index 4d169599..12cb076d 100644 --- a/include/ignition/sensors/Sensor.hh +++ b/include/ignition/sensors/Sensor.hh @@ -27,6 +27,7 @@ #pragma warning(pop) #endif +#include #include #include @@ -150,6 +151,14 @@ namespace ignition /// \return True if a valid topic was set. public: bool SetTopic(const std::string &_topic); + /// \brief Get flag state for enabling performance metrics publication. + /// \return True if performance metrics are enabled, false otherwise. + public: bool EnableMetrics() const; + + /// \brief Set flag to enable publishing performance metrics + /// \param[in] _enableMetrics True to enable. + public: void SetEnableMetrics(bool _enableMetrics); + /// \brief Get parent link of the sensor. /// \return Parent link of sensor. public: std::string Parent() const; @@ -184,6 +193,12 @@ namespace ignition public: void AddSequence(ignition::msgs::Header *_msg, const std::string &_seqKey = "default"); + /// \brief Publishes information about the performance of the sensor. + /// This method is called by Update(). + /// \param[in] _now Current time. + public: void PublishMetrics( + const std::chrono::duration &_now); + IGN_COMMON_WARN_IGNORE__DLL_INTERFACE_MISSING /// \internal /// \brief Data pointer for private data diff --git a/src/AirPressureSensor.cc b/src/AirPressureSensor.cc index b10ed384..98fde943 100644 --- a/src/AirPressureSensor.cc +++ b/src/AirPressureSensor.cc @@ -122,6 +122,9 @@ bool AirPressureSensor::Load(const sdf::Sensor &_sdf) return false; } + igndbg << "Air pressure for [" << this->Name() << "] advertised on [" + << this->Topic() << "]" << std::endl; + // Load the noise parameters if (_sdf.AirPressureSensor()->PressureNoise().Type() != sdf::NoiseType::NONE) { diff --git a/src/AltimeterSensor.cc b/src/AltimeterSensor.cc index f77ab47a..a3b5cd8b 100644 --- a/src/AltimeterSensor.cc +++ b/src/AltimeterSensor.cc @@ -100,6 +100,9 @@ bool AltimeterSensor::Load(const sdf::Sensor &_sdf) return false; } + igndbg << "Altimeter data for [" << this->Name() << "] advertised on [" + << this->Topic() << "]" << std::endl; + // Load the noise parameters if (_sdf.AltimeterSensor()->VerticalPositionNoise().Type() != sdf::NoiseType::NONE) diff --git a/src/CameraSensor.cc b/src/CameraSensor.cc index 75648486..9bca4f80 100644 --- a/src/CameraSensor.cc +++ b/src/CameraSensor.cc @@ -268,6 +268,9 @@ bool CameraSensor::Load(const sdf::Sensor &_sdf) return false; } + igndbg << "Camera images for [" << this->Name() << "] advertised on [" + << this->Topic() << "]" << std::endl; + if (!this->AdvertiseInfo()) return false; @@ -500,16 +503,7 @@ bool CameraSensor::AdvertiseInfo() } this->dataPtr->infoTopic += "/camera_info"; - this->dataPtr->infoPub = - this->dataPtr->node.Advertise( - this->dataPtr->infoTopic); - if (!this->dataPtr->infoPub) - { - ignerr << "Unable to create publisher on topic[" - << this->dataPtr->infoTopic << "].\n"; - } - - return this->dataPtr->infoPub; + return this->AdvertiseInfo(this->dataPtr->infoTopic); } ////////////////////////////////////////////////// @@ -522,9 +516,14 @@ bool CameraSensor::AdvertiseInfo(const std::string &_topic) this->dataPtr->infoTopic); if (!this->dataPtr->infoPub) { - ignerr << "Unable to create publisher on topic[" + ignerr << "Unable to create publisher on topic [" << this->dataPtr->infoTopic << "].\n"; } + else + { + igndbg << "Camera info for [" << this->Name() << "] advertised on [" + << this->dataPtr->infoTopic << "]" << std::endl; + } return this->dataPtr->infoPub; } diff --git a/src/DepthCameraSensor.cc b/src/DepthCameraSensor.cc index 8e231e1f..d912aafd 100644 --- a/src/DepthCameraSensor.cc +++ b/src/DepthCameraSensor.cc @@ -284,6 +284,9 @@ bool DepthCameraSensor::Load(const sdf::Sensor &_sdf) return false; } + igndbg << "Depth images for [" << this->Name() << "] advertised on [" + << this->Topic() << "]" << std::endl; + if (!this->AdvertiseInfo()) return false; @@ -298,6 +301,9 @@ bool DepthCameraSensor::Load(const sdf::Sensor &_sdf) return false; } + igndbg << "Points for [" << this->Name() << "] advertised on [" + << this->Topic() << "/points]" << std::endl; + // Initialize the point message. // \todo(anyone) The true value in the following function call forces // the xyz and rgb fields to be aligned to memory boundaries. This is need diff --git a/src/GpuLidarSensor.cc b/src/GpuLidarSensor.cc index 8db59f21..12fe37ff 100644 --- a/src/GpuLidarSensor.cc +++ b/src/GpuLidarSensor.cc @@ -146,6 +146,9 @@ bool GpuLidarSensor::Load(const sdf::Sensor &_sdf) return false; } + igndbg << "Lidar points for [" << this->Name() << "] advertised on [" + << this->Topic() << "]" << std::endl; + this->initialized = true; return true; diff --git a/src/ImuSensor.cc b/src/ImuSensor.cc index f8e2db5e..25498cce 100644 --- a/src/ImuSensor.cc +++ b/src/ImuSensor.cc @@ -59,6 +59,9 @@ class ignition::sensors::ImuSensorPrivate /// \brief transform to Imu frame from Imu reference frame. public: ignition::math::Quaterniond orientation; + /// \brief True to publish orientation data. + public: bool orientationEnabled = true; + /// \brief store gravity vector to be added to the IMU output. public: ignition::math::Vector3d gravity; @@ -125,6 +128,9 @@ bool ImuSensor::Load(const sdf::Sensor &_sdf) return false; } + igndbg << "IMU data for [" << this->Name() << "] advertised on [" + << this->Topic() << "]" << std::endl; + const std::map noises = { {ACCELEROMETER_X_NOISE_M_S_S, _sdf.ImuSensor()->LinearAccelerationXNoise()}, {ACCELEROMETER_Y_NOISE_M_S_S, _sdf.ImuSensor()->LinearAccelerationYNoise()}, @@ -204,12 +210,6 @@ bool ImuSensor::Update(const std::chrono::steady_clock::duration &_now) applyNoise(GYROSCOPE_Y_NOISE_RAD_S, this->dataPtr->angularVel.Y()); applyNoise(GYROSCOPE_Z_NOISE_RAD_S, this->dataPtr->angularVel.Z()); - // Set the IMU orientation - // imu orientation with respect to reference frame - this->dataPtr->orientation = - this->dataPtr->orientationReference.Inverse() * - this->dataPtr->worldPose.Rot(); - msgs::IMU msg; *msg.mutable_header()->mutable_stamp() = msgs::Convert(_now); msg.set_entity_name(this->Name()); @@ -217,7 +217,16 @@ bool ImuSensor::Update(const std::chrono::steady_clock::duration &_now) frame->set_key("frame_id"); frame->add_value(this->Name()); - msgs::Set(msg.mutable_orientation(), this->dataPtr->orientation); + if (this->dataPtr->orientationEnabled) + { + // Set the IMU orientation + // imu orientation with respect to reference frame + this->dataPtr->orientation = + this->dataPtr->orientationReference.Inverse() * + this->dataPtr->worldPose.Rot(); + + msgs::Set(msg.mutable_orientation(), this->dataPtr->orientation); + } msgs::Set(msg.mutable_angular_velocity(), this->dataPtr->angularVel); msgs::Set(msg.mutable_linear_acceleration(), this->dataPtr->linearAcc); @@ -277,6 +286,18 @@ math::Quaterniond ImuSensor::OrientationReference() const return this->dataPtr->orientationReference; } +////////////////////////////////////////////////// +void ImuSensor::SetOrientationEnabled(bool _enabled) +{ + this->dataPtr->orientationEnabled = _enabled; +} + +////////////////////////////////////////////////// +bool ImuSensor::OrientationEnabled() const +{ + return this->dataPtr->orientationEnabled; +} + ////////////////////////////////////////////////// void ImuSensor::SetGravity(const math::Vector3d &_gravity) { diff --git a/src/ImuSensor_TEST.cc b/src/ImuSensor_TEST.cc index d690f460..d6b4f7c8 100644 --- a/src/ImuSensor_TEST.cc +++ b/src/ImuSensor_TEST.cc @@ -311,6 +311,84 @@ TEST(ImuSensor_TEST, ComputeNoise) EXPECT_GT(sensor->LinearAcceleration().SquaredLength(), 0.0); } +////////////////////////////////////////////////// +TEST(ImuSensor_TEST, Orientation) +{ + // Create a sensor manager + ignition::sensors::Manager mgr; + + sdf::ElementPtr imuSDF; + + { + const std::string name = "TestImu_Truth"; + const std::string topic = "/ignition/sensors/test/imu_truth"; + const double updateRate = 100; + const auto accelNoise = noNoiseParameters(updateRate, 0.0); + const auto gyroNoise = noNoiseParameters(updateRate, 0.0); + const bool always_on = 1; + const bool visualize = 1; + + imuSDF = ImuSensorToSDF(name, updateRate, topic, + accelNoise, gyroNoise, always_on, visualize); + } + + // Create an ImuSensor + auto sensor = mgr.CreateSensor( + imuSDF); + + // Make sure the above dynamic cast worked. + ASSERT_NE(nullptr, sensor); + + math::Quaterniond orientRef; + math::Quaterniond orientValue(math::Vector3d(IGN_PI/2.0, 0, IGN_PI)); + math::Pose3d pose(math::Vector3d(0, 1, 2), orientValue); + + sensor->SetOrientationReference(orientRef); + sensor->SetWorldPose(pose); + + sensor->Update(std::chrono::steady_clock::duration( + std::chrono::nanoseconds(10000000))); + + // Check orientation + EXPECT_TRUE(sensor->OrientationEnabled()); + EXPECT_EQ(orientRef, sensor->OrientationReference()); + EXPECT_EQ(orientValue, sensor->Orientation()); + + // update pose and check orientation + math::Quaterniond newOrientValue(math::Vector3d(IGN_PI, IGN_PI/2, IGN_PI)); + math::Pose3d newPose(math::Vector3d(0, 1, 1), newOrientValue); + sensor->SetWorldPose(newPose); + + sensor->Update(std::chrono::steady_clock::duration( + std::chrono::nanoseconds(20000000))); + + EXPECT_TRUE(sensor->OrientationEnabled()); + EXPECT_EQ(orientRef, sensor->OrientationReference()); + EXPECT_EQ(newOrientValue, sensor->Orientation()); + + // disable orientation and check + sensor->SetOrientationEnabled(false); + EXPECT_FALSE(sensor->OrientationEnabled()); + EXPECT_EQ(orientRef, sensor->OrientationReference()); + // orientation remains the same after disabling orientation + EXPECT_EQ(newOrientValue, sensor->Orientation()); + + // update world pose with orientation disabled and verify that orientation + // does not change + math::Quaterniond newOrientValue2(math::Vector3d(IGN_PI/2, IGN_PI/2, IGN_PI)); + math::Pose3d newPose2(math::Vector3d(1, 1, 0), newOrientValue2); + sensor->SetWorldPose(newPose2); + sensor->Update(std::chrono::steady_clock::duration( + std::chrono::nanoseconds(20000000))); + + sensor->SetOrientationEnabled(false); + EXPECT_FALSE(sensor->OrientationEnabled()); + EXPECT_EQ(orientRef, sensor->OrientationReference()); + // orientation should still be the previous value because it is not being + // updated. + EXPECT_EQ(newOrientValue, sensor->Orientation()); + +} ////////////////////////////////////////////////// int main(int argc, char **argv) diff --git a/src/Lidar.cc b/src/Lidar.cc index a73b10ee..1b6e61e4 100644 --- a/src/Lidar.cc +++ b/src/Lidar.cc @@ -118,7 +118,9 @@ bool Lidar::Load(const sdf::Sensor &_sdf) << this->Topic() << "].\n"; return false; } - ignmsg << "Publishing laser scans on [" << this->Topic() << "]" << std::endl; + + igndbg << "Laser scans for [" << this->Name() << "] advertised on [" + << this->Topic() << "]" << std::endl; // Load ray atributes this->dataPtr->sdfLidar = *_sdf.LidarSensor(); diff --git a/src/LogicalCameraSensor.cc b/src/LogicalCameraSensor.cc index b190c099..4aa13451 100644 --- a/src/LogicalCameraSensor.cc +++ b/src/LogicalCameraSensor.cc @@ -115,6 +115,9 @@ bool LogicalCameraSensor::Load(sdf::ElementPtr _sdf) return false; } + igndbg << "Logical images for [" << this->Name() << "] advertised on [" + << this->Topic() << "]" << std::endl; + this->dataPtr->initialized = true; return true; } diff --git a/src/MagnetometerSensor.cc b/src/MagnetometerSensor.cc index 81a0da5f..6836255e 100644 --- a/src/MagnetometerSensor.cc +++ b/src/MagnetometerSensor.cc @@ -114,6 +114,9 @@ bool MagnetometerSensor::Load(const sdf::Sensor &_sdf) return false; } + igndbg << "Magnetometer data for [" << this->Name() << "] advertised on [" + << this->Topic() << "]" << std::endl; + // Load the noise parameters if (_sdf.MagnetometerSensor()->XNoise().Type() != sdf::NoiseType::NONE) { diff --git a/src/RgbdCameraSensor.cc b/src/RgbdCameraSensor.cc index d368d930..8b24b3ee 100644 --- a/src/RgbdCameraSensor.cc +++ b/src/RgbdCameraSensor.cc @@ -219,6 +219,9 @@ bool RgbdCameraSensor::Load(const sdf::Sensor &_sdf) return false; } + igndbg << "RGB images for [" << this->Name() << "] advertised on [" + << this->Topic() << "/image]" << std::endl; + // Create the depth image publisher this->dataPtr->depthPub = this->dataPtr->node.Advertise( @@ -230,6 +233,9 @@ bool RgbdCameraSensor::Load(const sdf::Sensor &_sdf) return false; } + igndbg << "Depth images for [" << this->Name() << "] advertised on [" + << this->Topic() << "/depth_image]" << std::endl; + // Create the point cloud publisher this->dataPtr->pointPub = this->dataPtr->node.Advertise( @@ -241,6 +247,9 @@ bool RgbdCameraSensor::Load(const sdf::Sensor &_sdf) return false; } + igndbg << "Points for [" << this->Name() << "] advertised on [" + << this->Topic() << "/points]" << std::endl; + if (!this->AdvertiseInfo(this->Topic() + "/camera_info")) return false; diff --git a/src/Sensor.cc b/src/Sensor.cc index 32b6e928..d7230a28 100644 --- a/src/Sensor.cc +++ b/src/Sensor.cc @@ -16,19 +16,19 @@ */ #include "ignition/sensors/Sensor.hh" + +#include #include #include + #include #include #include #include #include -#include - using namespace ignition::sensors; - class ignition::sensors::SensorPrivate { /// \brief Populates fields from a DOM @@ -39,6 +39,10 @@ class ignition::sensors::SensorPrivate /// \return True if a valid topic was set. public: bool SetTopic(const std::string &_topic); + /// \brief Publishes information about the performance of the sensor. + /// \param[in] _now Current simulation time. + public: void PublishMetrics(const std::chrono::duration &_now); + /// \brief Set the rate on which the sensor should publish its data. This /// method doesn't allow to set a higher rate than what is in the SDF. /// \param[in] _rate Maximum rate of the sensor. It is capped by the @@ -65,6 +69,9 @@ class ignition::sensors::SensorPrivate /// \brief Pose of the sensor public: ignition::math::Pose3d pose; + /// \brief Flag to enable publishing performance metrics. + public: bool enableMetrics{false}; + /// \brief How many times the sensor will generate data per second (value from /// SDF. public: double sdfUpdateRate = 0.0; @@ -73,13 +80,22 @@ class ignition::sensors::SensorPrivate /// used value). public: double updateRate = 0.0; - /// \brief node to create rate update service server - public: transport::Node node; - /// \brief What sim time should this sensor update at public: std::chrono::steady_clock::duration nextUpdateTime {std::chrono::steady_clock::duration::zero()}; + /// \brief Last steady clock time reading from last Update call. + public: std::chrono::time_point lastRealTime; + + /// \brief Last sim time at Update call. + public: std::chrono::duration lastUpdateTime{0}; + + /// \brief Transport node. + public: ignition::transport::Node node; + + /// \brief Publishes the PerformanceSensorMetrics message. + public: ignition::transport::Node::Publisher performanceSensorMetricsPub; + /// \brief SDF element with sensor information. public: sdf::ElementPtr sdf = nullptr; @@ -129,6 +145,8 @@ bool SensorPrivate::PopulateFromSDF(const sdf::Sensor &_sdf) } this->sdfUpdateRate = this->updateRate = _sdf.UpdateRate(); + + this->enableMetrics = _sdf.EnableMetrics(); return true; } @@ -234,6 +252,79 @@ bool SensorPrivate::SetTopic(const std::string &_topic) return true; } +////////////////////////////////////////////////// +bool Sensor::EnableMetrics() const +{ + return this->dataPtr->enableMetrics; +} + + +////////////////////////////////////////////////// +void Sensor::SetEnableMetrics(bool _enableMetrics) +{ + this->dataPtr->enableMetrics = _enableMetrics; +} + +////////////////////////////////////////////////// +void Sensor::PublishMetrics(const std::chrono::duration &_now) +{ + return this->dataPtr->PublishMetrics(_now); +} + +////////////////////////////////////////////////// +void SensorPrivate::PublishMetrics(const std::chrono::duration &_now) +{ + if(!this->performanceSensorMetricsPub) + { + const auto validTopic = transport::TopicUtils::AsValidTopic( + this->topic + "/performance_metrics"); + if (validTopic.empty()) + { + ignerr << "Failed to set metrics sensor topic [" << topic << "]" << + std::endl; + return; + } + this->performanceSensorMetricsPub = + node.Advertise(validTopic); + } + if (!performanceSensorMetricsPub || + !performanceSensorMetricsPub.HasConnections()) + { + return; + } + + // Computes simulation update rate and real update rate. + double simUpdateRate; + double realUpdateRate; + const auto clockNow = std::chrono::steady_clock::now(); + // If lastUpdateTime == 0 means it wasn't initialized yet. + if(this->lastUpdateTime.count() > 0) + { + const double diffSimUpdate = _now.count() - + this->lastUpdateTime.count(); + simUpdateRate = 1.0 / diffSimUpdate; + const double diffRealUpdate = + std::chrono::duration_cast>( + clockNow - this->lastRealTime).count(); + realUpdateRate = diffRealUpdate < std::numeric_limits::epsilon() ? + std::numeric_limits::infinity() : 1.0 / diffRealUpdate; + } + + // Update last time values. + this->lastUpdateTime = _now; + this->lastRealTime = clockNow; + + // Fill performance sensor metrics message. + msgs::PerformanceSensorMetrics performanceSensorMetricsMsg; + performanceSensorMetricsMsg.set_name(this->name); + performanceSensorMetricsMsg.set_real_update_rate(realUpdateRate); + performanceSensorMetricsMsg.set_sim_update_rate(simUpdateRate); + performanceSensorMetricsMsg.set_nominal_update_rate(this->updateRate); + + // Publish data + performanceSensorMetricsPub.Publish(performanceSensorMetricsMsg); +} + ////////////////////////////////////////////////// void SensorPrivate::SetRate(const ignition::msgs::Double &_rate) { @@ -330,6 +421,13 @@ bool Sensor::Update(const std::chrono::steady_clock::duration &_now, // Make the update happen result = this->Update(_now); + // Publish metrics + if (this->EnableMetrics()) + { + auto secs = std::chrono::duration_cast>(_now); + this->PublishMetrics(secs); + } + if (!_force && this->dataPtr->updateRate > 0.0) { // Update the time the plugin should be loaded diff --git a/src/Sensor_TEST.cc b/src/Sensor_TEST.cc index 835a2d53..98ff7d7e 100644 --- a/src/Sensor_TEST.cc +++ b/src/Sensor_TEST.cc @@ -14,9 +14,24 @@ * limitations under the License. * */ + +#include +#include + #include +#ifdef _WIN32 +#pragma warning(push) +#pragma warning(disable: 4005) +#pragma warning(disable: 4251) +#endif +#include +#ifdef _WIN32 +#pragma warning(pop) +#endif + #include +#include #include #include #include @@ -125,6 +140,76 @@ TEST(Sensor_TEST, Topic) EXPECT_FALSE(sensor.SetTopic("")); } +class SensorUpdate : public ::testing::Test +{ + // Documentation inherited + protected: void SetUp() override + { + ignition::common::Console::SetVerbosity(4); + node.Subscribe(kPerformanceMetricTopic, + &SensorUpdate::OnPerformanceMetric, this); + } + + // Callback function for the performance metric topic. + protected: void OnPerformanceMetric( + const ignition::msgs::PerformanceSensorMetrics &_msg) + { + EXPECT_EQ(kSensorName, _msg.name()); + performanceMetricsMsgsCount++; + } + + // Sensor name. + protected: const std::string kSensorName{"sensor_test"}; + // Sensor topic. + protected: const std::string kSensorTopic{"/sensor_test"}; + // Enable metrics flag. + protected: const bool kEnableMetrics{true}; + // Topic where performance metrics are published. + protected: const std::string kPerformanceMetricTopic{ + kSensorTopic + "/performance_metrics"}; + // Number of messages to be published. + protected: const unsigned int kNumberOfMessages{10}; + // Counter for performance metrics messages published. + protected: std::atomic performanceMetricsMsgsCount{0}; + // Transport node. + protected: transport::Node node; +}; + +////////////////////////////////////////////////// +TEST_F(SensorUpdate, Update) +{ + // Create sensor. + sdf::Sensor sdfSensor; + sdfSensor.SetName(kSensorName); + sdfSensor.SetTopic(kSensorTopic); + sdfSensor.SetEnableMetrics(kEnableMetrics); + std::unique_ptr sensor = std::make_unique(); + sensor->Load(sdfSensor); + ASSERT_EQ(kSensorTopic, sensor->Topic()); + ASSERT_EQ(kEnableMetrics, sensor->EnableMetrics()); + + // Call Update() method kNumberOfMessages times. + // For each call there is also a call to Sensor::PublishMetrics() that + // publishes metrics in the correspondant topic. + for (int sec = 0 ; sec < static_cast(kNumberOfMessages) ; ++sec) + { + std::chrono::steady_clock::duration now = std::chrono::seconds(sec); + sensor->Update(now, true); + } + + int sleep = 0; + const int maxSleep = 30; + while (performanceMetricsMsgsCount < kNumberOfMessages && sleep < maxSleep) + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + sleep++; + } + ASSERT_EQ(kNumberOfMessages, performanceMetricsMsgsCount); + + auto testSensor = dynamic_cast(sensor.get()); + EXPECT_EQ(testSensor->updateCount, performanceMetricsMsgsCount); +} + ////////////////////////////////////////////////// TEST(Sensor_TEST, SetRateService) { diff --git a/src/ThermalCameraSensor.cc b/src/ThermalCameraSensor.cc index d47a64b4..038577b8 100644 --- a/src/ThermalCameraSensor.cc +++ b/src/ThermalCameraSensor.cc @@ -220,6 +220,9 @@ bool ThermalCameraSensor::Load(const sdf::Sensor &_sdf) return false; } + igndbg << "Thermal images for [" << this->Name() << "] advertised on [" + << this->Topic() << "]" << std::endl; + if (!this->AdvertiseInfo()) return false;