Better GZ_PROFILE instrumentation for rendering sensors (#532)

Signed-off-by: Maksim Derbasov <ntfs.hard@gmail.com>
(cherry picked from commit 76532a6)

# Conflicts:
#	src/LogicalCameraSensor.cc

Author: ntfshard

examples/loop_sensor/main.cc

@@ -72,7 +72,7 @@ int main(int argc,  char **argv)
       {
         auto sensor = link->SensorByIndex(s);
 
-        gz::sensors::Sensor *sensorPtr;
+        gz::sensors::Sensor *sensorPtr{};
         if (sensor->Type() == sdf::SensorType::ALTIMETER)
         {
           sensorPtr = mgr.CreateSensor<gz::sensors::AltimeterSensor>(

include/gz/sensors/Manager.hh

@@ -17,11 +17,10 @@
 #ifndef GZ_SENSORS_MANAGER_HH_
 #define GZ_SENSORS_MANAGER_HH_
 
+#include <chrono>
 #include <memory>
-#include <string>
 #include <utility>
 #include <type_traits>
-#include <vector>
 #include <sdf/sdf.hh>
 #include <gz/utils/SuppressWarning.hh>
 #include <gz/common/Console.hh>
@@ -42,7 +41,7 @@ namespace gz
     /// \brief Loads and runs sensors
     ///
     ///   This class is responsible for loading and running sensors, and
-    ///   providing sensors with common environments to generat data from.
+    ///   providing sensors with common environments to generate data from.
     ///
     ///   The primary interface through which to load a sensor is LoadSensor().
     ///   This takes an sdf element pointer that should be configured with
@@ -62,7 +61,7 @@ namespace gz
       /// \return True if successfully initialized, false if not
       public: bool Init();
 
-      /// \brief Create a sensor from an SDF ovject with a known sensor type.
+      /// \brief Create a sensor from an SDF object with a known sensor type.
       /// \sa Sensor()
       /// \param[in] _sdf An SDF element or DOM object.
       /// \tparam SensorType Sensor type

src/BoundingBoxCameraSensor.cc

@@ -393,10 +393,9 @@ rendering::BoundingBoxCameraPtr
 void BoundingBoxCameraSensor::OnNewBoundingBoxes(
   const std::vector<rendering::BoundingBox> &_boxes)
 {
+  GZ_PROFILE("BoundingBoxCameraSensor::OnNewBoundingBoxes");
   std::lock_guard<std::mutex> lock(this->dataPtr->mutex);
-  this->dataPtr->boundingBoxes.clear();
-  for (const auto &box : _boxes)
-    this->dataPtr->boundingBoxes.push_back(box);
+  this->dataPtr->boundingBoxes = _boxes;
 }
 
 //////////////////////////////////////////////////

src/CameraSensor.cc

@@ -15,7 +15,6 @@
  *
 */
 
-#include <gz/msgs/boolean.pb.h>
 #include <gz/msgs/camera_info.pb.h>
 #include <gz/msgs/image.pb.h>
 
@@ -490,8 +489,8 @@ bool CameraSensor::Update(const std::chrono::steady_clock::duration &_now)
   {
     if (this->dataPtr->generatingData)
     {
-      gzdbg << "Disabling camera sensor: '" << this->Name() << "' data "
-             << "generation. " << std::endl;;
+      gzdbg << "Disabling camera sensor: '" << this->Name()
+            << "' data generation. " << std::endl;
       this->dataPtr->generatingData = false;
     }
 
@@ -501,8 +500,8 @@ bool CameraSensor::Update(const std::chrono::steady_clock::duration &_now)
   {
     if (!this->dataPtr->generatingData)
     {
-      gzdbg << "Enabling camera sensor: '" << this->Name() << "' data "
-             << "generation." << std::endl;;
+      gzdbg << "Enabling camera sensor: '" << this->Name()
+            << "' data generation." << std::endl;
       this->dataPtr->generatingData = true;
     }
   }

src/DepthCameraSensor.cc

@@ -465,6 +465,7 @@ void DepthCameraSensor::OnNewDepthFrame(const float *_scan,
                     unsigned int /*_channels*/,
                     const std::string &_format)
 {
+  GZ_PROFILE("DepthCameraSensor::OnNewDepthFrame");
   std::lock_guard<std::mutex> lock(this->dataPtr->mutex);
 
   unsigned int depthSamples = _width * _height;
@@ -492,6 +493,7 @@ void DepthCameraSensor::OnNewRgbPointCloud(const float *_scan,
                     unsigned int _channels,
                     const std::string &/*_format*/)
 {
+  GZ_PROFILE("DepthCameraSensor::OnNewRgbPointCloud");
   std::lock_guard<std::mutex> lock(this->dataPtr->mutex);
 
   unsigned int pointCloudSamples = _width * _height;
@@ -599,10 +601,12 @@ bool DepthCameraSensor::Update(
   msg.set_data(this->dataPtr->depthBuffer,
       rendering::PixelUtil::MemorySize(rendering::PF_FLOAT32_R,
       width, height));
-
   this->AddSequence(msg.mutable_header(), "default");
-  this->dataPtr->pub.Publish(msg);
 
+  {
+    GZ_PROFILE("DepthCameraSensor::Update Publish");
+    this->dataPtr->pub.Publish(msg);
+  }
 
   if (this->dataPtr->imageEvent.ConnectionCount() > 0u)
   {

src/GpuLidarSensor.cc

@@ -242,6 +242,7 @@ void GpuLidarSensor::OnNewLidarFrame(const float *_scan,
     unsigned int _width, unsigned int _height, unsigned int _channels,
     const std::string &_format)
 {
+  GZ_PROFILE("GpuLidarSensor::OnNewLidarFrame");
   std::lock_guard<std::mutex> lock(this->lidarMutex);
 
   unsigned int samples = _width * _height * _channels;

src/ImuSensor_TEST.cc

@@ -508,7 +508,7 @@ TEST(ImuSensor_TEST, CustomRpyParentFrame)
 }
 
 sdf::ElementPtr sensorWithLocalization(
-  std::string _orientationLocalization)
+  const std::string &_orientationLocalization)
 {
   std::ostringstream stream;
   stream

src/LogicalCameraSensor.cc

@@ -166,9 +166,34 @@ bool LogicalCameraSensor::Update(
   frame->set_key("frame_id");
   frame->add_value(this->FrameId());
 
+<<<<<<< HEAD
   // publish
   this->AddSequence(this->dataPtr->msg.mutable_header());
   this->dataPtr->pub.Publish(this->dataPtr->msg);
+=======
+  *this->dataPtr->msgLogic.mutable_header()->mutable_stamp() =
+    msgs::Convert(_now);
+  this->dataPtr->msgLogic.mutable_header()->clear_data();
+  auto frame_log = this->dataPtr->msgLogic.mutable_header()->add_data();
+
+  frame_log->set_key("frame_id");
+  frame_log->add_value(this->FrameId());
+
+  this->dataPtr->msgLogic.set_near_clip(this->dataPtr->frustum.Near());
+  this->dataPtr->msgLogic.set_far_clip(this->dataPtr->frustum.Far());
+  this->dataPtr->msgLogic.set_horizontal_fov(
+    this->dataPtr->frustum.FOV().Radian());
+  this->dataPtr->msgLogic.set_aspect_ratio(
+    this->dataPtr->frustum.AspectRatio());
+  this->AddSequence(this->dataPtr->msg.mutable_header());
+
+  // publish
+  {
+    GZ_PROFILE("LogicalCameraSensor::Update Publish");
+    this->dataPtr->pub.Publish(this->dataPtr->msg);
+    this->dataPtr->pubLogic.Publish(this->dataPtr->msgLogic);
+  }
+>>>>>>> 76532a6 (Better GZ_PROFILE instrumentation for rendering sensors (#532))
 
   return true;
 }

src/RgbdCameraSensor.cc

@@ -430,6 +430,7 @@ void RgbdCameraSensorPrivate::OnNewDepthFrame(const float *_scan,
                     unsigned int /*_channels*/,
                     const std::string &/*_format*/)
 {
+  GZ_PROFILE("RgbdCameraSensorPrivate::OnNewDepthFrame");
   std::lock_guard<std::mutex> lock(this->mutex);
 
   unsigned int depthSamples = _width * _height;
@@ -447,6 +448,7 @@ void RgbdCameraSensorPrivate::OnNewRgbPointCloud(const float *_scan,
                     unsigned int _channels,
                     const std::string &/*_format*/)
 {
+  GZ_PROFILE("RgbdCameraSensorPrivate::OnNewRgbPointCloud");
   std::lock_guard<std::mutex> lock(this->mutex);
 
   unsigned int pointCloudSamples = _width * _height;
@@ -512,7 +514,7 @@ bool RgbdCameraSensor::Update(const std::chrono::steady_clock::duration &_now)
   // generate sensor data
   this->Render();
 
-  // create and publish the depthmessage
+  // create and publish the depth message
   if (this->HasDepthConnections())
   {
     msgs::Image msg;

src/SegmentationCameraSensor.cc

@@ -434,6 +434,7 @@ void SegmentationCameraSensor::OnNewSegmentationFrame(const uint8_t * _data,
   unsigned int _width, unsigned int _height, unsigned int _channels,
   const std::string &/*_format*/)
 {
+  GZ_PROFILE("SegmentationCameraSensor::OnNewSegmentationFrame");
   std::lock_guard<std::mutex> lock(this->dataPtr->mutex);
 
   unsigned int bufferSize = _width * _height * _channels;
@@ -539,8 +540,11 @@ bool SegmentationCameraSensor::Update(
       width, height));
 
   // Publish
-  this->dataPtr->coloredMapPublisher.Publish(this->dataPtr->coloredMapMsg);
-  this->dataPtr->labelsMapPublisher.Publish(this->dataPtr->labelsMapMsg);
+  {
+    GZ_PROFILE("SegmentationCameraSensor::Update Publish");
+    this->dataPtr->coloredMapPublisher.Publish(this->dataPtr->coloredMapMsg);
+    this->dataPtr->labelsMapPublisher.Publish(this->dataPtr->labelsMapMsg);
+  }
 
   // Trigger callbacks.
   if (this->dataPtr->imageEvent.ConnectionCount() > 0u)