Setup profiler

README.md

@@ -52,6 +52,9 @@ Clone this repository then run
 colcon build
 ```
 
+> You can pass `--cmake-args ' -DENABLE_PROFILER=1'` to use the profiler.
+> See more on [this tutorial](https://ignitionrobotics.org/api/common/4.4/profiler.html)
+
 ## To test simulation in Ignition standalone (without MBARI integration)
 
 This package comes with an empty example world. To run this example world simply

lrauv_ignition_plugins/CMakeLists.txt

@@ -7,6 +7,15 @@ cmake_minimum_required(VERSION 3.10.2 FATAL_ERROR)
 
 project(lrauv_ignition_plugins)
 
+# Option to enable profiler
+option(ENABLE_PROFILER "Enable Ignition Profiler" FALSE)
+
+if(ENABLE_PROFILER)
+  add_definitions("-DIGN_PROFILER_ENABLE=1")
+else()
+  add_definitions("-DIGN_PROFILER_ENABLE=0")
+endif()
+
 #============================================================================
 # Find dependencies
 find_package(ignition-cmake2 REQUIRED)
@@ -31,6 +40,9 @@ set(IGN_PLUGIN_VER ${ignition-plugin1_VERSION_MAJOR})
 find_package(ignition-utils1 REQUIRED)
 set(IGN_UTILS_VER ${ignition-utils1_VERSION_MAJOR})
 
+find_package(ignition-common4 REQUIRED COMPONENTS profiler)
+set(IGN_COMMON_VER ${ignition-common4_VERSION_MAJOR})
+
 find_package (Eigen3 3.3 REQUIRED)
 
 find_package(PCL 1.2 REQUIRED)
@@ -87,6 +99,7 @@ function(add_lrauv_plugin PLUGIN)
   set_property(TARGET ${PLUGIN} PROPERTY CXX_STANDARD 17)
   target_link_libraries(${PLUGIN}
     PUBLIC
+      ignition-common${IGN_COMMON_VER}::profiler
       ignition-plugin${IGN_PLUGIN_VER}::ignition-plugin${IGN_PLUGIN_VER}
       ignition-gazebo${IGN_GAZEBO_VER}::ignition-gazebo${IGN_GAZEBO_VER}
     PRIVATE

lrauv_ignition_plugins/src/ScienceSensorsSystem.cc

@@ -22,6 +22,7 @@
 
 #include <ignition/msgs/pointcloud_packed.pb.h>
 
+#include <ignition/common/Profiler.hh>
 #include <ignition/common/SystemPaths.hh>
 #include <ignition/gazebo/World.hh>
 #include <ignition/msgs/Utility.hh>
@@ -227,6 +228,8 @@ ScienceSensorsSystem::ScienceSensorsSystem()
 /////////////////////////////////////////////////
 void ScienceSensorsSystemPrivate::ReadData()
 {
+  IGN_PROFILE("ScienceSensorsSystemPrivate::ReadData");
+
   std::fstream fs;
   fs.open(this->dataPath, std::ios::in);
 
@@ -435,6 +438,7 @@ void ScienceSensorsSystemPrivate::GenerateOctrees()
 /////////////////////////////////////////////////
 void ScienceSensorsSystemPrivate::PublishData()
 {
+  IGN_PROFILE("ScienceSensorsSystemPrivate::PublishData");
   this->cloudPub.Publish(this->PointCloudMsg());
 }
 
@@ -444,6 +448,8 @@ float ScienceSensorsSystemPrivate::InterpolateData(
   std::vector<int> &_inds,
   std::vector<float> &_dists)
 {
+  IGN_PROFILE("ScienceSensorsSystemPrivate::InterpolateData");
+
   // Sanity checks
   if (_inds.size() == 0 || _dists.size() == 0)
   {
@@ -557,6 +563,9 @@ void ScienceSensorsSystem::PreUpdate(
 void ScienceSensorsSystem::PostUpdate(const ignition::gazebo::UpdateInfo &_info,
   const ignition::gazebo::EntityComponentManager &_ecm)
 {
+  IGN_PROFILE_THREAD_NAME("ScienceSensorsSystem PostUpdate");
+  IGN_PROFILE("ScienceSensorsSystem::PostUpdate");
+
   // Only update and publish if data has been loaded and simulation is not
   // paused.
   if (this->dataPtr->initialized && !_info.paused)
@@ -609,12 +618,15 @@ void ScienceSensorsSystem::PostUpdate(const ignition::gazebo::UpdateInfo &_info,
       std::vector<float> spatialSqrDist;
 
       // Search in octree to find spatial index of science data
-      if (this->dataPtr->spatialOctrees[this->dataPtr->timeIdx].nearestKSearch(
-        searchPoint, k, spatialIdx, spatialSqrDist) <= 0)
       {
-        ignwarn << "No data found near sensor location " << sensorLatLon.value()
-                << std::endl;
-        continue;
+        IGN_PROFILE("ScienceSensorsSystem::PostUpdate nearestKSearch");
+        if (this->dataPtr->spatialOctrees[this->dataPtr->timeIdx].nearestKSearch(
+          searchPoint, k, spatialIdx, spatialSqrDist) <= 0)
+        {
+          ignwarn << "No data found near sensor location " << sensorLatLon.value()
+                  << std::endl;
+          continue;
+        }
       }
       // Debug output
       /*
@@ -691,6 +703,8 @@ void ScienceSensorsSystem::PostUpdate(const ignition::gazebo::UpdateInfo &_info,
 void ScienceSensorsSystem::RemoveSensorEntities(
     const ignition::gazebo::EntityComponentManager &_ecm)
 {
+  IGN_PROFILE("ScienceSensorsSystem::RemoveSensorEntities");
+
   _ecm.EachRemoved<ignition::gazebo::components::CustomSensor>(
     [&](const ignition::gazebo::Entity &_entity,
         const ignition::gazebo::components::CustomSensor *)->bool
@@ -722,6 +736,8 @@ bool ScienceSensorsSystemPrivate::ScienceDataService(
 //////////////////////////////////////////////////
 ignition::msgs::PointCloudPacked ScienceSensorsSystemPrivate::PointCloudMsg()
 {
+  IGN_PROFILE("ScienceSensorsSystemPrivate::PointCloudMsg");
+
   ignition::msgs::PointCloudPacked msg;
 
   if (this->timeIdx < 0 || this->timeIdx >= this->timestamps.size())

lrauv_ignition_plugins/src/VisualizePointCloud.cc

@@ -30,6 +30,7 @@
 #include "VisualizePointCloud.hh"
 
 #include <ignition/common/Console.hh>
+#include <ignition/common/Profiler.hh>
 
 #include <ignition/plugin/Register.hh>
 
@@ -206,6 +207,8 @@ void VisualizePointCloud::OnService(const ignition::msgs::PointCloudPacked &_res
 //////////////////////////////////////////////////
 void VisualizePointCloud::PublishMarkers()
 {
+  IGN_PROFILE("VisualizePointCloud::PublishMarkers");
+
   // If point cloud empty, do nothing. (PointCloudPackedIteratorBase errors on
   // empty cloud.)
   if (this->dataPtr->pcMsg.height() == 0 && this->dataPtr->pcMsg.width() == 0)