From c426a72c80210d50bf07ca58ad5b036702679ed9 Mon Sep 17 00:00:00 2001
From: Louise Poubel <louise@openrobotics.org>
Date: Tue, 31 May 2022 16:09:46 -0700
Subject: [PATCH] Setup profiler

Signed-off-by: Louise Poubel <louise@openrobotics.org>
---
 README.md                                           | 13 +++++++++++++
 buoy_gazebo/CMakeLists.txt                          | 12 ++++++++++++
 .../src/ElectroHydraulicPTO/ElectroHydraulicPTO.cpp |  4 +---
 3 files changed, 26 insertions(+), 3 deletions(-)

diff --git a/README.md b/README.md
index f172ac6d..0099441c 100644
--- a/README.md
+++ b/README.md
@@ -2,3 +2,16 @@ This repository contains packages used for Gazebo simulation of marine buoys.
 
 Start from [Buoy entrypoint](https://github.com/osrf/buoy_entrypoint).
 
+## Development
+
+### Profiler
+
+By default, the profiler isn't enabled. To enable it at compilation time,
+pass the `-DENABLE_PROFILER=1` CMake argument. For example, to compile tests
+and enable the profiler with `colcon`:
+
+```
+colcon build --cmake-args ' -DBUILD_TESTING=false' ' -DENABLE_PROFILER=1'
+```
+
+See more on [this tutorial](https://ignitionrobotics.org/api/common/4.4/profiler.html).
diff --git a/buoy_gazebo/CMakeLists.txt b/buoy_gazebo/CMakeLists.txt
index 3af2b972..8ebde651 100644
--- a/buoy_gazebo/CMakeLists.txt
+++ b/buoy_gazebo/CMakeLists.txt
@@ -1,6 +1,15 @@
 cmake_minimum_required(VERSION 3.5)
 project(buoy_gazebo)
 
+# 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_package(ament_cmake REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(buoy_msgs REQUIRED)
@@ -8,6 +17,8 @@ find_package(buoy_msgs REQUIRED)
 find_package(ignition-cmake2 REQUIRED)
 find_package(ignition-plugin1 REQUIRED COMPONENTS register)
 set(IGN_PLUGIN_VER ${ignition-plugin1_VERSION_MAJOR})
+find_package(ignition-common4 REQUIRED COMPONENTS profiler)
+set(IGN_COMMON_VER ${ignition-common4_VERSION_MAJOR})
 find_package(ignition-gazebo6 REQUIRED)
 set(IGN_GAZEBO_VER ${ignition-gazebo6_VERSION_MAJOR})
 
@@ -52,6 +63,7 @@ function(gz_add_plugin plugin_name)
     PUBLIC
       ignition-gazebo${IGN_GAZEBO_VER}::ignition-gazebo${IGN_GAZEBO_VER}
       ignition-plugin${IGN_PLUGIN_VER}::ignition-plugin${IGN_PLUGIN_VER}
+      ignition-common${IGN_COMMON_VER}::profiler
       ${gz_add_plugin_PUBLIC_LINK_LIBS}
     PRIVATE
       ${gz_add_plugin_PRIVATE_LINK_LIBS}
diff --git a/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicPTO.cpp b/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicPTO.cpp
index e18ed87f..97fcc762 100644
--- a/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicPTO.cpp
+++ b/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicPTO.cpp
@@ -306,7 +306,7 @@ void ElectroHydraulicPTO::PreUpdate(
   const int n = 2;
   int info;
 
-  IGN_PROFILE("ElectroHydraulicPTO::Update");
+  IGN_PROFILE("#ElectroHydraulicPTO::PreUpdate");
   // Nothing left to do if paused.
   if (_info.paused) {
     return;
@@ -314,8 +314,6 @@ void ElectroHydraulicPTO::PreUpdate(
 
   auto SimTime = std::chrono::duration<double>(_info.simTime).count();
 
-  IGN_PROFILE("#ElectroHydraulicPTO::PreUpdate");
-
   // If the joints haven't been identified yet, the plugin is disabled
   if (this->dataPtr->PrismaticJointEntity == ignition::gazebo::kNullEntity) {
     return;