simple logged added

Author: Davide Faconti

CMakeLists.txt

@@ -0,0 +1,64 @@
+cmake_minimum_required(VERSION 2.8.12) # version on Ubuntu Trusty
+project(behaviortree_ros)
+
+if(NOT CMAKE_VERSION VERSION_LESS 3.1)
+    set(CMAKE_CXX_STANDARD 11)
+    set(CMAKE_CXX_STANDARD_REQUIRED ON)
+else()
+    set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
+endif()
+
+set(CMAKE_POSITION_INDEPENDENT_CODE ON)
+
+######################################################
+
+set(ROS_DEPENDENCIES roscpp std_msgs message_generation behaviortree_cpp)
+
+find_package(catkin REQUIRED COMPONENTS ${ROS_DEPENDENCIES} )
+find_package(GTest)
+
+add_message_files(
+  FILES
+  BehaviorTree.msg
+  NodeParameter.msg
+  NodeStatus.msg
+  StatusChange.msg
+  StatusChangeLog.msg
+  TreeNode.msg
+)
+
+generate_messages(
+  DEPENDENCIES
+  std_msgs
+)
+
+catkin_package(
+    INCLUDE_DIRS include
+    LIBRARIES
+    CATKIN_DEPENDS ${ROS_DEPENDENCIES}
+)
+
+include_directories( include ${catkin_INCLUDE_DIRS})
+
+
+######################################################
+# LIBRARIES
+
+add_library(behaviortree_ros
+    src/loggers/rosout_logger.cpp )
+
+target_link_libraries(behaviortree_ros ${catkin_LIBRARIES})
+
+######################################################
+# TESTS
+
+
+######################################################
+# INSTALL
+
+
+######################################################
+# EXAMPLES and TOOLS
+
+
+

include/behaviortree_ros/loggers/rosout_logger.h

@@ -0,0 +1,38 @@
+#ifndef BT_ROSOUT_LOGGER_H
+#define BT_ROSOUT_LOGGER_H
+
+#include <behaviortree_cpp/loggers/abstract_logger.h>
+#include <ros/console.h>
+
+namespace BT
+{
+
+class RosoutLogger : public StatusChangeLogger
+{
+    static std::atomic<bool> ref_count;
+
+  public:
+    RosoutLogger(TreeNode* root_node,
+                 ros::console::Level verbosity_level = ros::console::Level::Info);
+
+    ros::console::Level getLevel() const;
+
+    // Accepts only Info and Debug
+    void setLevel(ros::console::Level level);
+
+    ~RosoutLogger() override;
+
+    virtual void callback(Duration timestamp,
+                          const TreeNode& node,
+                          NodeStatus prev_status,
+                          NodeStatus status) override;
+
+    virtual void flush() override;
+
+private:
+    ros::console::Level _level;
+};
+
+}   // end namespace
+
+#endif   //BT_ROSOUT_LOGGER_H

msg/BehaviorTree.msg

@@ -0,0 +1,2 @@
+uint16     root_uid
+TreeNode[] nodes

msg/NodeParameter.msg

@@ -0,0 +1,2 @@
+string key
+string value

msg/NodeStatus.msg

@@ -0,0 +1,7 @@
+int8 IDLE = 0
+int8 RUNNING = 1
+int8 SUCCESS = 2
+int8 FAILURE = 3
+
+
+int8 value

msg/StatusChange.msg

@@ -0,0 +1,4 @@
+uint16      uid       
+NodeStatus  prev_status 
+NodeStatus  status
+time        timestamp

msg/StatusChangeLog.msg

@@ -0,0 +1,3 @@
+
+BehaviorTree    behavior_tree
+StatusChange[]  state_changes

msg/TreeNode.msg

@@ -0,0 +1,15 @@
+uint16         uid
+uint16[]       children_uid
+int8           type
+NodeStatus     status
+string         instance_name
+string         registration_name
+NodeParameter  params
+
+
+int8 UNDEFINED = 0
+int8 ACTION = 1
+int8 CONDITION = 2
+int8 CONTROL = 3
+int8 DECORATOR = 4
+int8 SUBTREE = 5

package.xml

@@ -0,0 +1,27 @@
+<package>
+  <name>behaviortree_ros</name>
+  <version>0.0.1</version>
+  <description>
+  This package provides usefull utilities on top of BehaviorTree.CPP.
+  </description>
+
+  <maintainer email="[email protected]">Davide Faconti</maintainer>
+
+  <license>MIT</license>
+
+  <author>Davide Faconti</author>
+
+  <build_depend>roscpp</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_depend>behaviortree_cpp</build_depend>
+  
+  <run_depend>roscpp</run_depend>
+  <run_depend>std_msgs</run_depend>
+  <run_depend>behaviortree_cpp</run_depend>
+
+  <build_depend>message_generation</build_depend>
+  <run_depend>message_generation</run_depend>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+</package>

src/loggers/rosout_logger.cpp

@@ -0,0 +1,69 @@
+#include "behaviortree_ros/loggers/rosout_logger.h"
+
+namespace BT
+{
+std::atomic<bool> RosoutLogger::ref_count(false);
+
+RosoutLogger::RosoutLogger(TreeNode* root_node, ros::console::Level verbosity_level) :
+    StatusChangeLogger(root_node),
+    _level(verbosity_level)
+{
+    bool expected = false;
+    if (!ref_count.compare_exchange_strong(expected, true))
+    {
+        throw std::logic_error("Only a single instance of RosoutLogger shall be created");
+    }
+}
+
+ros::console::Level RosoutLogger::getLevel() const
+{
+    return _level;
+}
+
+void RosoutLogger::setLevel(ros::console::Level level)
+{
+    if( level != ros::console::Level::Debug &&
+        level != ros::console::Level::Info )
+    {
+        throw std::invalid_argument("RosoutLogger::setLevel acepts only Debug or Info");
+    }
+    _level = level;
+}
+
+RosoutLogger::~RosoutLogger()
+{
+    ref_count.store(false);
+}
+
+void RosoutLogger::callback(Duration timestamp, const TreeNode& node, NodeStatus prev_status,
+                             NodeStatus status)
+{
+    constexpr const char* whitespaces = "                         ";
+    const size_t ws_count = strlen(whitespaces)-1;
+
+    const auto& node_name = node.name();
+
+    switch( _level )
+    {
+    case ros::console::Level::Debug :
+        ROS_DEBUG("[%s%s]: %s -> %s",  node_name.c_str(),
+                  &whitespaces[std::min(ws_count, node_name.size())],
+                  toStr(prev_status, true),
+                  toStr(status, true));
+        break;
+
+    case ros::console::Level::Info :
+        ROS_INFO("[%s%s]: %s -> %s",  node_name.c_str(),
+                  &whitespaces[std::min(ws_count, node_name.size())],
+                  toStr(prev_status, true),
+                  toStr(status, true));
+        break;
+    }
+}
+
+void RosoutLogger::flush()
+{
+
+}
+
+}   // end namespace