diff --git a/CMakeLists.txt b/CMakeLists.txt
index 32f34fb674db5c49156338ed7c0ca5cc5eb7871f..9ed29c9a4a39a0fdef02c22e5bdee3dc3e0fe7da 100644
--- a/CMakeLists.txt
+++ b/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)
diff --git a/include/iri_bt_logger.h b/include/iri_bt_logger.h
new file mode 100644
index 0000000000000000000000000000000000000000..870ed6ce6b8d0d79193547129de87de509bf6506
--- /dev/null
+++ b/include/iri_bt_logger.h
@@ -0,0 +1,68 @@
+#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
diff --git a/src/iri_bt_logger.cpp b/src/iri_bt_logger.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..8fbc941214b53d66fddfe13ea98f07447f03cff1
--- /dev/null
+++ b/src/iri_bt_logger.cpp
@@ -0,0 +1,101 @@
+#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