diff --git a/behaviortree_ros2/CMakeLists.txt b/behaviortree_ros2/CMakeLists.txt index edf1440..d28f358 100644 --- a/behaviortree_ros2/CMakeLists.txt +++ b/behaviortree_ros2/CMakeLists.txt @@ -27,7 +27,8 @@ generate_parameter_library( add_library(${PROJECT_NAME} src/bt_ros2.cpp src/bt_utils.cpp - src/tree_execution_server.cpp ) + src/tree_execution_server.cpp + src/bt_ros_logger.cpp ) ament_target_dependencies(${PROJECT_NAME} ${THIS_PACKAGE_DEPS}) diff --git a/behaviortree_ros2/include/behaviortree_ros2/bt_ros_logger.hpp b/behaviortree_ros2/include/behaviortree_ros2/bt_ros_logger.hpp new file mode 100644 index 0000000..66ba4ce --- /dev/null +++ b/behaviortree_ros2/include/behaviortree_ros2/bt_ros_logger.hpp @@ -0,0 +1,32 @@ +#ifndef BT_ROS_LOGGER_H +#define BT_ROS_LOGGER_H + +#include +#include +#include +#include +#include "behaviortree_cpp/loggers/abstract_logger.h" + +namespace BT +{ +/** + * @brief RosLogger is a very simple logger that + * displays all the transitions on the console. + */ + +class RosLogger : public StatusChangeLogger +{ +public: + RosLogger(const BT::Tree& tree, std::shared_ptr node); + ~RosLogger() override; + + virtual void flush() override; + +private: + virtual void callback(Duration timestamp, const TreeNode& node, NodeStatus prev_status, + NodeStatus status) override; + std::weak_ptr node_; +}; +} // namespace BT + +#endif // BT_ROS_LOGGER_H diff --git a/behaviortree_ros2/src/bt_ros_logger.cpp b/behaviortree_ros2/src/bt_ros_logger.cpp new file mode 100644 index 0000000..61b6ee5 --- /dev/null +++ b/behaviortree_ros2/src/bt_ros_logger.cpp @@ -0,0 +1,38 @@ +#include "behaviortree_ros2/bt_ros_logger.hpp" + +namespace BT +{ + +RosLogger::RosLogger(const BT::Tree& tree, std::shared_ptr node) : StatusChangeLogger(tree.rootNode()), node_(node) +{} +RosLogger::~RosLogger() +{} + +void RosLogger::callback(Duration timestamp, const TreeNode& node, + NodeStatus prev_status, NodeStatus status) +{ + using namespace std::chrono; + + // get ros node pointer + auto ros_node = node_.lock(); + + if (ros_node) { + + constexpr const char* whitespaces = " "; + constexpr const size_t ws_count = 25; + + double since_epoch = duration(timestamp).count(); + + RCLCPP_DEBUG( + ros_node->get_logger(), "[%.3f]: %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()); + } +} + +void RosLogger::flush() +{ +} + +} // namespace BT \ No newline at end of file