ros_topic_logger.hpp

// Copyright (c) 2019 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
 
#ifndef NAV2_BEHAVIOR_TREE__ROS_TOPIC_LOGGER_HPP_
#define NAV2_BEHAVIOR_TREE__ROS_TOPIC_LOGGER_HPP_
 
#include <vector>
#include <memory>
#include <utility>
#include <string>
 
#include "behaviortree_cpp/loggers/abstract_logger.h"
#include "rclcpp/clock.hpp"
#include "rclcpp/logger.hpp"
#include "rclcpp/logging.hpp"
#include "nav2_msgs/msg/behavior_tree_log.hpp"
#include "nav2_msgs/msg/behavior_tree_status_change.hpp"
#include "tf2/time.hpp"
#include "tf2_ros/buffer_interface.hpp"
#include "nav2_ros_common/interface_factories.hpp"
 
namespace nav2_behavior_tree
{
 
class RosTopicLogger : public BT::StatusChangeLogger
{
public:
  RosTopicLogger(
    const nav2::LifecycleNode::WeakPtr & ros_node,
    const BT::Tree & tree,
    bool log_idle = true)
  : StatusChangeLogger(tree.rootNode())
  {
    auto node = ros_node.lock();
    clock_ = node->get_clock();
    logger_ = node->get_logger().get_child("ros_topic_logger");
    log_pub_ = nav2::interfaces::create_publisher<nav2_msgs::msg::BehaviorTreeLog>(
      node, "behavior_tree_log");
    log_pub_->on_activate();
    enableTransitionToIdle(log_idle);
  }
 
  void callback(
    BT::Duration timestamp,
    const BT::TreeNode & node,
    BT::NodeStatus prev_status,
    BT::NodeStatus status) override
  {
    nav2_msgs::msg::BehaviorTreeStatusChange event;
 
    // BT timestamps are a duration since the epoch. Need to convert to a time_point
    // before converting to a msg.
    event.timestamp = tf2_ros::toMsg(tf2::TimePoint(timestamp));
    event.node_name = node.name();
    event.uid = node.UID();
    event.previous_status = toStr(prev_status, false);
    event.current_status = toStr(status, false);
    event_log_.push_back(std::move(event));
 
    auto prev_pad = std::string(kStatusWidth - toStr(prev_status, false).size(), ' ');
    auto curr_pad = std::string(kStatusWidth - toStr(status, false).size(), ' ');
    RCLCPP_DEBUG(
      logger_, "[%.3f]: %s%s -> %s%s  %s",
      std::chrono::duration<double>(timestamp).count(),
      toStr(prev_status, true).c_str(), prev_pad.c_str(),
      toStr(status, true).c_str(), curr_pad.c_str(),
      node.name().c_str() );
  }
 
  void flush() override
  {
    if (!event_log_.empty()) {
      auto log_msg = std::make_unique<nav2_msgs::msg::BehaviorTreeLog>();
      log_msg->timestamp = clock_->now();
      log_msg->event_log = event_log_;
      log_pub_->publish(std::move(log_msg));
      event_log_.clear();
    }
  }
 
protected:
  // Longest BT status string is 7 chars (RUNNING, SUCCESS, FAILURE); pad shorter ones (IDLE)
  static constexpr size_t kStatusWidth = 7;
  rclcpp::Clock::SharedPtr clock_;
  rclcpp::Logger logger_{rclcpp::get_logger("bt_navigator")};
  nav2::Publisher<nav2_msgs::msg::BehaviorTreeLog>::SharedPtr log_pub_;
  std::vector<nav2_msgs::msg::BehaviorTreeStatusChange> event_log_;
};
 
}   // namespace nav2_behavior_tree
 
#endif   // NAV2_BEHAVIOR_TREE__ROS_TOPIC_LOGGER_HPP_