change the behavior of tickOnce to actually loop is wake up signal is… (#522)

* change the behavior of tickOnce to actually loop is wake up signal is received

* fix warning

Author: facontidavide

include/behaviortree_cpp/bt_factory.h

@@ -157,7 +157,15 @@ class Tree
 
   ~Tree();
 
-  /// Tick the root of the tree once.
+  /// Tick the root of the tree once, even if a node invoked
+  /// emitWakeUpSignal()
+  NodeStatus tickExactlyOnce();
+
+  /**
+   * @brief by default, tickOnce() sends a single tick, BUT
+   * as long as there is at least one node of the tree
+   * invoking TreeNode::emitWakeUpSignal(), it will be ticked again.
+   */
   NodeStatus tickOnce();
 
   /// Call tickOnce until the status is different from RUNNING.
@@ -179,7 +187,8 @@ class Tree
 
   enum TickOption
   {
-    ONCE,
+    EXACTLY_ONCE,
+    ONCE_UNLESS_WOKEN_UP,
     WHILE_RUNNING
   };
 

include/behaviortree_cpp/utils/wakeup_signal.hpp

@@ -11,7 +11,8 @@ namespace BT
 class WakeUpSignal
 {
 public:
-    /// Return true if the
+    /// Return true if the timeout was NOT reached and the
+    /// signal was received.
     bool waitFor(std::chrono::system_clock::duration tm)
     {
         std::unique_lock<std::mutex> lk(mutex_);

src/bt_factory.cpp

@@ -375,9 +375,14 @@ Tree::~Tree()
   haltTree();
 }
 
+NodeStatus Tree::tickExactlyOnce()
+{
+  return tickRoot(EXACTLY_ONCE, {});
+}
+
 NodeStatus Tree::tickOnce()
 {
-  return tickRoot(ONCE, {});
+  return tickRoot(ONCE_UNLESS_WOKEN_UP, {});
 }
 
 NodeStatus Tree::tickWhileRunning(std::chrono::milliseconds sleep_time)
@@ -416,19 +421,30 @@ NodeStatus Tree::tickRoot(TickOption opt, std::chrono::milliseconds sleep_time)
 {
   NodeStatus status = NodeStatus::IDLE;
 
+  if (!wake_up_)
+  {
+    initialize();
+  }
+
+  if (!rootNode())
+  {
+    throw RuntimeError("Empty Tree");
+  }
+
   while (status == NodeStatus::IDLE ||
          (opt == TickOption::WHILE_RUNNING && status == NodeStatus::RUNNING))
   {
-    if (!wake_up_)
-    {
-      initialize();
-    }
+    status = rootNode()->executeTick();
 
-    if (!rootNode())
+    // Inner loop. The previous tick might have triggered the wake-up
+    // in this case, unless TickOption::EXACTLY_ONCE, we tick again
+    while( opt != TickOption::EXACTLY_ONCE &&
+           status == NodeStatus::RUNNING &&
+           wake_up_->waitFor(std::chrono::milliseconds(0)) )
     {
-      throw RuntimeError("Empty Tree");
+      status = rootNode()->executeTick();
     }
-    status = rootNode()->executeTick();
+
     if (status == NodeStatus::SUCCESS || status == NodeStatus::FAILURE)
     {
       rootNode()->resetStatus();

tools/bt_log_cat.cpp

@@ -24,7 +24,8 @@ int main(int argc, char* argv[])
     const size_t length = static_cast<size_t>(ftell(file));
     fseek(file, 0L, SEEK_SET);
     std::vector<char> buffer(length);
-    fread(buffer.data(), sizeof(char), length, file);
+    auto ret = fread(buffer.data(), sizeof(char), length, file);
+    (void)ret;
     fclose(file);
 
     const auto bt_header_size = flatbuffers::ReadScalar<uint32_t>(&buffer[0]);