Added a BT logger to get the names and the previous and current states of the...

Added a BT logger to get the names and the previous and current states of the nodes that have changed their state

CMakeLists.txt

@@ -7,6 +7,7 @@ add_compile_options(-std=c++11)
 ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
 ## is used, also find other catkin packages
 find_package(catkin REQUIRED COMPONENTS behaviortree_cpp_v3)
+find_package(iriutils REQUIRED)
 
 ## System dependencies are found with CMake's conventions
 # find_package(Boost REQUIRED COMPONENTS system)
@@ -80,6 +81,7 @@ catkin_package(
   INCLUDE_DIRS include
   LIBRARIES ${PROJECT_NAME}
   CATKIN_DEPENDS behaviortree_cpp_v3 
+  DEPENDS iriutils
 )
 
 ###########
@@ -90,13 +92,15 @@ catkin_package(
 ## Your package locations should be listed before other locations
 include_directories(include)
 include_directories(${catkin_INCLUDE_DIRS})
+include_directories(${iriutils_INCLUDE_DIR})
 
 ## Declare a cpp library
 # add_library(iri_behaviortree
 #   src/${PROJECT_NAME}/iri_behaviortree.cpp
 # )
-add_library(${PROJECT_NAME} src/iri_async_action.cpp src/iri_bt_factory.cpp)
+add_library(${PROJECT_NAME} src/iri_async_action.cpp src/iri_bt_factory.cpp src/iri_bt_logger.cpp)
 target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
+target_link_libraries(${PROJECT_NAME} ${iriutils_LIBRARY})
 
 ## Declare a cpp executable
 # add_executable(iri_behaviortree_node src/iri_behaviortree_node.cpp)

include/iri_bt_logger.h

+#ifndef IRI_BT_ROS_LOGGER_H
+#define IRI_BT_ROS_LOGGER_H
+
+#include <cstring>
+#include <queue>
+#include "behaviortree_cpp_v3/loggers/abstract_logger.h"
+
+/**
+ * @brief AddIriBtRosLogger. Give  the root node of a tree,
+ * a simple callback is subscribed to any status change of each node.
+ *
+ *
+ * @param root_node
+ * @return Important: the returned shared_ptr must not go out of scope,
+ *         otherwise the logger is removed.
+ */
+
+class IriBtLogger : public BT::StatusChangeLogger
+{
+    static std::atomic<bool> ref_count;
+
+  public:
+    IriBtLogger(const BT::Tree& tree);
+    ~IriBtLogger() override;
+
+    virtual void callback(BT::Duration timestamp, const BT::TreeNode& node, BT::NodeStatus prev_status,
+                          BT::NodeStatus status) override;
+
+    virtual void flush() override;
+
+    /**
+     * \brief Function to get the identifier of the ID new_data_available.
+     * 
+     * \return The new_data_available's ID.
+     */
+    std::string get_new_data_available_event_id(void);
+
+    /**
+     * \brief Function to get the amount of data available.
+     * 
+     * \return The data available.
+     */
+    uint16_t get_data_available(void);
+
+    /**
+     * \brief Function to get the data.
+     * 
+     * \param names A reference to the buffer where the available names must be copied.
+     * 
+     * \param prev_status A reference to the buffer where the available prev_status must be copied.
+     * 
+     * \param status A reference to the buffer where the available status must be copied.
+     *
+     * \param len the number of data to be read.
+     * 
+     * \return the number of data copied.
+     */
+    uint16_t get_data(std::string *names, BT::NodeStatus *prev_status, BT::NodeStatus *status, uint16_t len);
+
+  private:
+    std::queue<std::string> names; //< Variable to store the names of the nodes that have change their status.
+    std::queue<BT::NodeStatus> prev_status; //< Variable to store the nodes previous status.
+    std::queue<BT::NodeStatus> status; //< Variable to store the nodes status.
+    uint16_t data_available; //< Variable to store the data available.
+    std::string new_data_available; //< Variable to store the identifier of the ID new_data_available.
+};
+
+#endif   // IRI_BT_ROS_LOGGER_H

src/iri_bt_logger.cpp

+#include "iri_bt_logger.h"
+#include "eventserver.h"
+#include "eventexceptions.h"
+
+std::atomic<bool> IriBtLogger::ref_count(false);
+
+IriBtLogger::IriBtLogger(const BT::Tree& tree) :  BT::StatusChangeLogger(tree.root_node)
+{
+  bool expected = false;
+  if (!ref_count.compare_exchange_strong(expected, true))
+  {
+    throw BT::LogicError("Only one instance of IriBtLogger shall be created");
+  }
+  this->data_available = 0;
+  try
+  {
+    CEventServer *event_server;
+    event_server = CEventServer::instance();
+    this->new_data_available = "iri_bt_logger_new_data_available";
+    event_server->create_event(this->new_data_available);
+  }
+  catch(CEventException &e)
+  {
+    std::cout << "Exception caught on IriBtLogger: " << e.what() << std::endl;
+    throw;
+  }
+}
+IriBtLogger::~IriBtLogger()
+{
+  ref_count.store(false);
+  CEventServer *event_server;
+  event_server = CEventServer::instance();
+  if(this->new_data_available != "")
+  {
+    event_server->delete_event(this->new_data_available);
+    this->new_data_available = "";
+  }
+}
+
+void IriBtLogger::callback(BT::Duration timestamp, const BT::TreeNode& node, BT::NodeStatus prev_status,
+                             BT::NodeStatus status)
+{
+  using namespace std::chrono;
+
+  constexpr const char* whitespaces = "                         ";
+  constexpr const size_t ws_count = 25;
+
+  double since_epoch = duration<double>(timestamp).count();
+  // printf("[%.3f]IRI: %s%s %s -> %s",
+  //        since_epoch, node.name().c_str(),
+  //        &whitespaces[std::min(ws_count, node.name().size())],
+  //        toStr(prev_status, true).c_str(),
+  //        toStr(status, true).c_str() );
+  // std::cout << std::endl;
+
+  this->names.push(node.name().c_str());
+  this->prev_status.push(prev_status);
+  this->status.push(status);
+  this->data_available++;
+  CEventServer *event_server;
+  event_server = CEventServer::instance();
+  if (!event_server->event_is_set(this->new_data_available))
+    event_server->set_event(this->new_data_available);
+}
+
+void IriBtLogger::flush()
+{
+  for (uint16_t i = 0; i < this->data_available; i++)
+  {
+    this->names.pop();
+    this->prev_status.pop();
+    this->status.pop();
+  }
+  this->data_available = 0;
+}
+
+std::string IriBtLogger::get_new_data_available_event_id(void)
+{
+  return this->new_data_available;
+}
+
+uint16_t IriBtLogger::get_data_available(void)
+{
+  return this->data_available;
+}
+
+uint16_t IriBtLogger::get_data(std::string *names, BT::NodeStatus *prev_status, BT::NodeStatus *status, uint16_t len)
+{
+  uint16_t data_to_read = (len <= this->data_available? len: this->data_available);
+  for (uint16_t i = 0; i < data_to_read; i++)
+  {
+    names[i] = this->names.front();
+    this->names.pop();
+    prev_status[i] = this->prev_status.front();
+    this->prev_status.pop();
+    status[i] = this->status.front();
+    this->status.pop();
+  }
+  this->data_available-= data_to_read;
+  return data_to_read;
+}
\ No newline at end of file