add TickMonitorCallback

Author: facontidavide

include/behaviortree_cpp/tree_node.h

@@ -164,6 +164,8 @@ class TreeNode
 
   using PreTickCallback = std::function<NodeStatus(TreeNode&)>;
   using PostTickCallback = std::function<NodeStatus(TreeNode&, NodeStatus)>;
+  using TickMonitorCallback =
+      std::function<void(TreeNode&, NodeStatus, std::chrono::microseconds)>;
 
   /**
      * @brief subscribeToStatusChange is used to attach a callback to a status change.
@@ -179,9 +181,9 @@ class TreeNode
 
   /** This method attaches to the TreeNode a callback with signature:
      *
-     *     Optional<NodeStatus> myCallback(TreeNode& node)
+     *     NodeStatus callback(TreeNode& node)
      *
-     * This callback is executed BEFORE the tick() and, if it returns a valid Optional<NodeStatus>,
+     * This callback is executed BEFORE the tick() and, if it returns SUCCESS or FAILURE,
      * the actual tick() will NOT be executed and this result will be returned instead.
      *
      * This is useful to inject a "dummy" implementation of the TreeNode at run-time
@@ -191,13 +193,23 @@ class TreeNode
   /**
    * This method attaches to the TreeNode a callback with signature:
    *
-   *     Optional<NodeStatus> myCallback(TreeNode& node, NodeStatus new_status)
+   *     NodeStatus myCallback(TreeNode& node, NodeStatus status)
    *
-   * This callback is executed AFTER the tick() and, if it returns a valid Optional<NodeStatus>,
+   * This callback is executed AFTER the tick() and, if it returns SUCCESS or FAILURE,
    * the value returned by the actual tick() is overriden with this one.
    */
   void setPostTickFunction(PostTickCallback callback);
 
+  /**
+   * This method attaches to the TreeNode a callback with signature:
+   *
+   *     void myCallback(TreeNode& node, NodeStatus status, std::chrono::microseconds duration)
+   *
+   * This callback is executed AFTER the tick() and will inform the user about its status and
+   * the execution time. Works only if the tick was not substituted by a pre-condition.
+   */
+  void setTickMonitorCallback(TickMonitorCallback callback);
+
   /// The unique identifier of this instance of treeNode.
   /// It is assigneld by the factory
   [[nodiscard]] uint16_t UID() const;

src/tree_node.cpp

@@ -38,9 +38,9 @@ struct TreeNode::PImpl
 
   std::string registration_ID;
 
-  PreTickCallback substitution_callback;
-
-  PostTickCallback post_condition_callback;
+  PreTickCallback pre_tick_callback;
+  PostTickCallback post_tick_callback;
+  TickMonitorCallback tick_monitor_callback;
 
   std::mutex callback_injection_mutex;
 
@@ -70,6 +70,7 @@ TreeNode::~TreeNode()
 
 NodeStatus TreeNode::executeTick()
 {
+  std::unique_lock lk(_p->callback_injection_mutex);
   auto new_status = _p->status;
 
   // a pre-condition may return the new status.
@@ -82,48 +83,48 @@ NodeStatus TreeNode::executeTick()
   {
     // injected pre-callback
     bool substituted = false;
-    if(!isStatusCompleted(_p->status))
+    if(_p->pre_tick_callback && !isStatusCompleted(_p->status))
     {
-      PreTickCallback callback;
-      {
-        std::unique_lock lk(_p->callback_injection_mutex);
-        callback = _p->substitution_callback;
-      }
-      if(callback)
+      auto override_status = _p->pre_tick_callback(*this);
+      if(isStatusCompleted(override_status))
       {
-        auto override_status = callback(*this);
-        if(isStatusCompleted(override_status))
-        {
-          // don't execute the actual tick()
-          substituted = true;
-          new_status = override_status;
-        }
+        // don't execute the actual tick()
+        substituted = true;
+        new_status = override_status;
       }
     }
 
     // Call the ACTUAL tick
     if(!substituted)
     {
-      new_status = tick();
+      if(_p->tick_monitor_callback)
+      {
+        using namespace std::chrono;
+        auto t1 = steady_clock::now();
+        new_status = tick();
+        auto t2 = steady_clock::now();
+        _p->tick_monitor_callback(*this, new_status,
+                                  duration_cast<microseconds>(t2 - t1));
+      }
+      else
+      {
+        new_status = tick();
+      }
     }
   }
 
   // injected post callback
   if(isStatusCompleted(new_status))
   {
     checkPostConditions(new_status);
-    PostTickCallback callback;
-    {
-      std::unique_lock lk(_p->callback_injection_mutex);
-      callback = _p->post_condition_callback;
-    }
-    if(callback)
+  }
+
+  if(_p->post_tick_callback)
+  {
+    auto override_status = _p->post_tick_callback(*this, new_status);
+    if(isStatusCompleted(override_status))
     {
-      auto override_status = callback(*this, new_status);
-      if(isStatusCompleted(override_status))
-      {
-        new_status = override_status;
-      }
+      new_status = override_status;
     }
   }
 
@@ -308,13 +309,19 @@ TreeNode::subscribeToStatusChange(TreeNode::StatusChangeCallback callback)
 void TreeNode::setPreTickFunction(PreTickCallback callback)
 {
   std::unique_lock lk(_p->callback_injection_mutex);
-  _p->substitution_callback = callback;
+  _p->pre_tick_callback = callback;
 }
 
 void TreeNode::setPostTickFunction(PostTickCallback callback)
 {
   std::unique_lock lk(_p->callback_injection_mutex);
-  _p->post_condition_callback = callback;
+  _p->post_tick_callback = callback;
+}
+
+void TreeNode::setTickMonitorCallback(TickMonitorCallback callback)
+{
+  std::unique_lock lk(_p->callback_injection_mutex);
+  _p->tick_monitor_callback = callback;
 }
 
 uint16_t TreeNode::UID() const