1414 * limitations under the License.
1515 *
1616*/
17- #include " gazebo/common/Profiler.hh"
18-
1917#include < functional>
2018#include < boost/bind.hpp>
21- #include " gazebo/common/Assert.hh"
22- #include " gazebo/common/Time.hh"
2319
2420#include " gazebo/msgs/poses_stamped.pb.h"
2521
26- #include " gazebo/physics/PhysicsIface.hh"
22+ #include " gazebo/common/Assert.hh"
23+ #include " gazebo/common/Profiler.hh"
24+ #include " gazebo/common/Time.hh"
25+ #include " gazebo/physics/Link.hh"
26+ #include " gazebo/physics/Model.hh"
2727#include " gazebo/physics/PhysicsEngine.hh"
28+ #include " gazebo/physics/PhysicsIface.hh"
2829#include " gazebo/physics/World.hh"
30+ #include " gazebo/rendering/Camera.hh"
31+ #include " gazebo/sensors/CameraSensor.hh"
2932#include " gazebo/sensors/Sensor.hh"
30- #include " gazebo/sensors/SensorsIface.hh"
3133#include " gazebo/sensors/SensorFactory.hh"
3234#include " gazebo/sensors/SensorManager.hh"
35+ #include " gazebo/sensors/SensorsIface.hh"
36+ #include " gazebo/transport/transport.hh"
3337#include " gazebo/util/LogPlay.hh"
3438
3539using namespace gazebo ;
@@ -46,6 +50,46 @@ bool g_sensorsDirty = true;
4650std::condition_variable
4751 SensorManager::ImageSensorContainer::conditionPrerendered;
4852
53+ // / Performance metrics variables
54+ // / \brief last sensor measurement sim time
55+ std::map<std::string, gazebo::common::Time> sensorsLastMeasurementTime;
56+
57+ // / \brief last sensor measurement real time
58+ std::map<std::string, gazebo::common::Time> worldLastMeasurementTime;
59+
60+ // / \brief Data structure for storing metric data to be published
61+ struct sensorPerformanceMetricsType
62+ {
63+ // / \brief sensor real time update rate
64+ double sensorRealUpdateRate;
65+
66+ // / \brief sensor sim time update rate
67+ double sensorSimUpdateRate;
68+
69+ // / \brief Rendering sensor average real time update rate. The difference
70+ // / between sensorAvgFps and sensorRealUpdateRte is that sensorAvgFps is
71+ // / for rendering sensors only and the rate is averaged over a fixed
72+ // / window size, whereas the sensorSimUpdateRate stores the instantaneous
73+ // / update rate and it is filled by all sensors.
74+ double sensorAvgFPS;
75+ };
76+
77+ // / \brief A map of sensor name to its performance metrics data
78+ std::map<std::string, struct sensorPerformanceMetricsType >
79+ sensorPerformanceMetrics;
80+
81+ // / \brief Publisher for run-time simulation performance metrics.
82+ transport::PublisherPtr performanceMetricsPub;
83+
84+ // / \brief Node for publishing performance metrics
85+ transport::NodePtr node = nullptr ;
86+
87+ // / \brief Last sim time measured for performance metrics
88+ common::Time lastSimTime;
89+
90+ // / \brief Last real time measured for performance metrics
91+ common::Time lastRealTime;
92+
4993// ////////////////////////////////////////////////
5094SensorManager::SensorManager ()
5195 : initialized(false ), removeAllSensors(false )
@@ -132,6 +176,126 @@ void SensorManager::WaitForSensors(double _clk, double _dt)
132176 }
133177}
134178
179+ void PublishPerformanceMetrics ()
180+ {
181+ if (node == nullptr )
182+ {
183+ auto world = physics::get_world ();
184+ // Transport
185+ node = transport::NodePtr (new transport::Node ());
186+ node->Init (world->Name ());
187+ performanceMetricsPub =
188+ node->Advertise <msgs::PerformanceMetrics>(
189+ " /gazebo/performance_metrics" 10 , 5 );
190+ }
191+
192+ if (!performanceMetricsPub || !performanceMetricsPub->HasConnections ())
193+ {
194+ return ;
195+ }
196+
197+ physics::WorldPtr world = physics::get_world (
198+ gazebo::physics::get_world ()->Name ());
199+
200+ // / Outgoing run-time simulation performance metrics.
201+ msgs::PerformanceMetrics performanceMetricsMsg;
202+
203+ // Real time factor
204+ common::Time realTime = world->RealTime ();
205+ common::Time diffRealtime = realTime - lastRealTime;
206+ common::Time simTime = world->SimTime ();
207+ common::Time diffSimTime = simTime - lastSimTime;
208+ common::Time realTimeFactor;
209+
210+ if (ignition::math::equal (diffSimTime.Double (), 0.0 ))
211+ return ;
212+
213+ if (realTime == 0 )
214+ realTimeFactor = 0 ;
215+ else
216+ realTimeFactor = diffSimTime / diffRealtime;
217+
218+ performanceMetricsMsg.set_real_time_factor (realTimeFactor.Double ());
219+
220+ lastRealTime = realTime;
221+ lastSimTime = simTime;
222+
223+ // / update sim time for sensors
224+ for (auto model: world->Models ())
225+ {
226+ for (auto link : model->GetLinks ())
227+ {
228+ for (unsigned int i = 0 ; i < link ->GetSensorCount (); i++)
229+ {
230+ std::string name = link ->GetSensorName (i);
231+ sensors::SensorPtr sensor = sensors::get_sensor (name);
232+
233+ auto ret = sensorsLastMeasurementTime.insert (
234+ std::pair<std::string, gazebo::common::Time>(name, 0 ));
235+ worldLastMeasurementTime.insert (
236+ std::pair<std::string, gazebo::common::Time>(name, 0 ));
237+ if (ret.second == false )
238+ {
239+ double updateSimRate =
240+ (sensor->LastMeasurementTime () - sensorsLastMeasurementTime[name])
241+ .Double ();
242+ if (updateSimRate > 0.0 )
243+ {
244+ sensorsLastMeasurementTime[name] = sensor->LastMeasurementTime ();
245+ double updateRealRate =
246+ (world->RealTime () - worldLastMeasurementTime[name]).Double ();
247+
248+ struct sensorPerformanceMetricsType emptySensorPerfomanceMetrics;
249+ auto ret2 = sensorPerformanceMetrics.insert (
250+ std::pair<std::string, struct sensorPerformanceMetricsType >
251+ (name, emptySensorPerfomanceMetrics));
252+ if (ret2.second == false )
253+ {
254+ ret2.first ->second .sensorSimUpdateRate =
255+ 1.0 /updateSimRate;
256+ ret2.first ->second .sensorRealUpdateRate =
257+ 1.0 /updateRealRate;
258+ worldLastMeasurementTime[name] = world->RealTime ();
259+
260+ // Special case for stereo cameras
261+ sensors::CameraSensorPtr cameraSensor =
262+ std::dynamic_pointer_cast<sensors::CameraSensor>(sensor);
263+ if (nullptr != cameraSensor)
264+ {
265+ ret2.first ->second .sensorAvgFPS =
266+ cameraSensor->Camera ()->AvgFPS ();
267+ }
268+ else
269+ {
270+ ret2.first ->second .sensorAvgFPS = -1 ;
271+ }
272+ }
273+ }
274+ }
275+ }
276+ }
277+ }
278+
279+ for (auto sensorPerformanceMetric: sensorPerformanceMetrics)
280+ {
281+ msgs::PerformanceMetrics::PerformanceSensorMetrics
282+ *performanceSensorMetricsMsg = performanceMetricsMsg.add_sensor ();
283+ performanceSensorMetricsMsg->set_name (sensorPerformanceMetric.first );
284+ performanceSensorMetricsMsg->set_real_update_rate (
285+ sensorPerformanceMetric.second .sensorRealUpdateRate );
286+ performanceSensorMetricsMsg->set_sim_update_rate (
287+ sensorPerformanceMetric.second .sensorSimUpdateRate );
288+ if (sensorPerformanceMetric.second .sensorAvgFPS >= 0.0 )
289+ {
290+ performanceSensorMetricsMsg->set_fps (
291+ sensorPerformanceMetric.second .sensorAvgFPS );
292+ }
293+ }
294+
295+ // Publish data
296+ performanceMetricsPub->Publish (performanceMetricsMsg);
297+ }
298+
135299// ////////////////////////////////////////////////
136300void SensorManager::Update (bool _force)
137301{
@@ -689,6 +853,8 @@ void SensorManager::SensorContainer::Update(bool _force)
689853{
690854 boost::recursive_mutex::scoped_lock lock (this ->mutex );
691855
856+ PublishPerformanceMetrics ();
857+
692858 if (this ->sensors .empty ())
693859 gzlog << " Updating a sensor container without any sensors.\n "
694860
0 commit comments