diff --git a/gazebo/msgs/CMakeLists.txt b/gazebo/msgs/CMakeLists.txt index cc19f5fa1e..c12584da96 100644 --- a/gazebo/msgs/CMakeLists.txt +++ b/gazebo/msgs/CMakeLists.txt @@ -8,9 +8,9 @@ set (msgs axis.proto battery.proto boxgeom.proto - camerasensor.proto camera_cmd.proto camera_lens.proto + camerasensor.proto cessna.proto collision.proto color.proto @@ -29,16 +29,16 @@ set (msgs geometry.proto gps.proto gps_sensor.proto - gui_camera.proto gui.proto + gui_camera.proto gz_string.proto gz_string_v.proto header.proto heightmapgeom.proto hydra.proto - imagegeom.proto image.proto image_stamped.proto + imagegeom.proto images_stamped.proto imu.proto imu_sensor.proto @@ -68,11 +68,12 @@ set (msgs model_configuration.proto model_v.proto packet.proto - physics.proto param.proto param_v.proto - planegeom.proto + performance_metrics.proto + physics.proto pid.proto + planegeom.proto plugin.proto pointcloud.proto polylinegeom.proto @@ -83,20 +84,18 @@ set (msgs pose_v.proto poses_stamped.proto projector.proto - propagation_particle.proto propagation_grid.proto - publishers.proto + propagation_particle.proto publish.proto + publishers.proto quaternion.proto - sonar.proto - sonar_stamped.proto raysensor.proto request.proto response.proto - rest_response.proto rest_login.proto rest_logout.proto rest_post.proto + rest_response.proto road.proto scene.proto selection.proto @@ -106,6 +105,8 @@ set (msgs shadows.proto sim_event.proto sky.proto + sonar.proto + sonar_stamped.proto spheregeom.proto spherical_coordinates.proto subscribe.proto @@ -126,9 +127,9 @@ set (msgs wireless_node.proto wireless_nodes.proto world_control.proto + world_modify.proto world_reset.proto world_stats.proto - world_modify.proto wrench.proto wrench_stamped.proto ) diff --git a/gazebo/msgs/performance_metrics.proto b/gazebo/msgs/performance_metrics.proto new file mode 100644 index 0000000000..a61a609d89 --- /dev/null +++ b/gazebo/msgs/performance_metrics.proto @@ -0,0 +1,37 @@ +syntax = "proto2"; +package gazebo.msgs; + +/// \ingroup gazebo_msgs +/// \interface PerformanceMetrics +/// \brief A message for run-time simulation performance metrics + +message PerformanceMetrics +{ + /// \brief This message contains information about the performance of + /// each sensor in the world. + /// If the sensor is a camera then it will publish the frame per second (fps). + message PerformanceSensorMetrics + { + /// \brief Sensor name + required string name = 1; + + /// \brief The update rate of the sensor in real time. + required double real_update_rate = 2; + + /// \brief The update rate of the sensor in simulation time. + required double sim_update_rate = 3; + + /// \brief If the sensor is a camera then this field should be filled + /// with average fps in real time. + optional double fps = 4; + } + + /// max_step_size x real_time_update_rate sets an upper bound of + /// real_time_factor. If real_time_factor < 1 the simulation is + /// slower than real time. + required double real_time_factor = 1[default=0.0]; + + /// Each sensor in the world will create a PerformanceSensorMetrics + /// message publishing information about the performance. + repeated PerformanceSensorMetrics sensor = 2; +} diff --git a/gazebo/sensors/SensorManager.cc b/gazebo/sensors/SensorManager.cc index 357b610c93..7f189f04b8 100644 --- a/gazebo/sensors/SensorManager.cc +++ b/gazebo/sensors/SensorManager.cc @@ -14,22 +14,26 @@ * limitations under the License. * */ -#include "gazebo/common/Profiler.hh" - #include #include -#include "gazebo/common/Assert.hh" -#include "gazebo/common/Time.hh" #include "gazebo/msgs/poses_stamped.pb.h" -#include "gazebo/physics/PhysicsIface.hh" +#include "gazebo/common/Assert.hh" +#include "gazebo/common/Profiler.hh" +#include "gazebo/common/Time.hh" +#include "gazebo/physics/Link.hh" +#include "gazebo/physics/Model.hh" #include "gazebo/physics/PhysicsEngine.hh" +#include "gazebo/physics/PhysicsIface.hh" #include "gazebo/physics/World.hh" +#include "gazebo/rendering/Camera.hh" +#include "gazebo/sensors/CameraSensor.hh" #include "gazebo/sensors/Sensor.hh" -#include "gazebo/sensors/SensorsIface.hh" #include "gazebo/sensors/SensorFactory.hh" #include "gazebo/sensors/SensorManager.hh" +#include "gazebo/sensors/SensorsIface.hh" +#include "gazebo/transport/transport.hh" #include "gazebo/util/LogPlay.hh" using namespace gazebo; @@ -46,6 +50,46 @@ bool g_sensorsDirty = true; std::condition_variable SensorManager::ImageSensorContainer::conditionPrerendered; +/// Performance metrics variables +/// \brief last sensor measurement sim time +std::map sensorsLastMeasurementTime; + +/// \brief last sensor measurement real time +std::map worldLastMeasurementTime; + +/// \brief Data structure for storing metric data to be published +struct sensorPerformanceMetricsType +{ + /// \brief sensor real time update rate + double sensorRealUpdateRate; + + /// \brief sensor sim time update rate + double sensorSimUpdateRate; + + /// \brief Rendering sensor average real time update rate. The difference + /// between sensorAvgFps and sensorRealUpdateRte is that sensorAvgFps is + /// for rendering sensors only and the rate is averaged over a fixed + /// window size, whereas the sensorSimUpdateRate stores the instantaneous + /// update rate and it is filled by all sensors. + double sensorAvgFPS; +}; + +/// \brief A map of sensor name to its performance metrics data +std::map + sensorPerformanceMetrics; + +/// \brief Publisher for run-time simulation performance metrics. +transport::PublisherPtr performanceMetricsPub; + +/// \brief Node for publishing performance metrics +transport::NodePtr node = nullptr; + +/// \brief Last sim time measured for performance metrics +common::Time lastSimTime; + +/// \brief Last real time measured for performance metrics +common::Time lastRealTime; + ////////////////////////////////////////////////// SensorManager::SensorManager() : initialized(false), removeAllSensors(false) @@ -132,6 +176,126 @@ void SensorManager::WaitForSensors(double _clk, double _dt) } } +void PublishPerformanceMetrics() +{ + if (node == nullptr) + { + auto world = physics::get_world(); + // Transport + node = transport::NodePtr(new transport::Node()); + node->Init(world->Name()); + performanceMetricsPub = + node->Advertise( + "/gazebo/performance_metrics", 10, 5); + } + + if (!performanceMetricsPub || !performanceMetricsPub->HasConnections()) + { + return; + } + + physics::WorldPtr world = physics::get_world( + gazebo::physics::get_world()->Name()); + + /// Outgoing run-time simulation performance metrics. + msgs::PerformanceMetrics performanceMetricsMsg; + + // Real time factor + common::Time realTime = world->RealTime(); + common::Time diffRealtime = realTime - lastRealTime; + common::Time simTime = world->SimTime(); + common::Time diffSimTime = simTime - lastSimTime; + common::Time realTimeFactor; + + if (ignition::math::equal(diffSimTime.Double(), 0.0)) + return; + + if (realTime == 0) + realTimeFactor = 0; + else + realTimeFactor = diffSimTime / diffRealtime; + + performanceMetricsMsg.set_real_time_factor(realTimeFactor.Double()); + + lastRealTime = realTime; + lastSimTime = simTime; + + /// update sim time for sensors + for (auto model: world->Models()) + { + for (auto link: model->GetLinks()) + { + for (unsigned int i = 0; i < link->GetSensorCount(); i++) + { + std::string name = link->GetSensorName(i); + sensors::SensorPtr sensor = sensors::get_sensor(name); + + auto ret = sensorsLastMeasurementTime.insert( + std::pair(name, 0)); + worldLastMeasurementTime.insert( + std::pair(name, 0)); + if (ret.second == false) + { + double updateSimRate = + (sensor->LastMeasurementTime() - sensorsLastMeasurementTime[name]) + .Double(); + if (updateSimRate > 0.0) + { + sensorsLastMeasurementTime[name] = sensor->LastMeasurementTime(); + double updateRealRate = + (world->RealTime() - worldLastMeasurementTime[name]).Double(); + + struct sensorPerformanceMetricsType emptySensorPerfomanceMetrics; + auto ret2 = sensorPerformanceMetrics.insert( + std::pair + (name, emptySensorPerfomanceMetrics)); + if (ret2.second == false) + { + ret2.first->second.sensorSimUpdateRate = + 1.0/updateSimRate; + ret2.first->second.sensorRealUpdateRate = + 1.0/updateRealRate; + worldLastMeasurementTime[name] = world->RealTime(); + + // Special case for stereo cameras + sensors::CameraSensorPtr cameraSensor = + std::dynamic_pointer_cast(sensor); + if (nullptr != cameraSensor) + { + ret2.first->second.sensorAvgFPS = + cameraSensor->Camera()->AvgFPS(); + } + else + { + ret2.first->second.sensorAvgFPS = -1; + } + } + } + } + } + } + } + + for (auto sensorPerformanceMetric: sensorPerformanceMetrics) + { + msgs::PerformanceMetrics::PerformanceSensorMetrics + *performanceSensorMetricsMsg = performanceMetricsMsg.add_sensor(); + performanceSensorMetricsMsg->set_name(sensorPerformanceMetric.first); + performanceSensorMetricsMsg->set_real_update_rate( + sensorPerformanceMetric.second.sensorRealUpdateRate); + performanceSensorMetricsMsg->set_sim_update_rate( + sensorPerformanceMetric.second.sensorSimUpdateRate); + if (sensorPerformanceMetric.second.sensorAvgFPS >= 0.0) + { + performanceSensorMetricsMsg->set_fps( + sensorPerformanceMetric.second.sensorAvgFPS); + } + } + + // Publish data + performanceMetricsPub->Publish(performanceMetricsMsg); +} + ////////////////////////////////////////////////// void SensorManager::Update(bool _force) { @@ -689,6 +853,8 @@ void SensorManager::SensorContainer::Update(bool _force) { boost::recursive_mutex::scoped_lock lock(this->mutex); + PublishPerformanceMetrics(); + if (this->sensors.empty()) gzlog << "Updating a sensor container without any sensors.\n";