Program Listing for File playable_bag.h

↰ Return to documentation for file (/tmp/ws/src/cartographer_ros/cartographer_ros/include/cartographer_ros/playable_bag.h)

/*
 * Copyright 2018 The Cartographer Authors
 *
 * 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 CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_PLAYABLE_BAG_H
#define CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_PLAYABLE_BAG_H

#include <functional>
#include <queue>

#include "cartographer_ros_msgs/msg/bagfile_progress.hpp"
#include <rclcpp/rclcpp.hpp>
#include <rosbag2_cpp/reader.hpp>
#include <rosbag2_cpp/readers/sequential_reader.hpp>
#include <rosbag2_cpp/writer.hpp>
#include <rosbag2_cpp/writers/sequential_writer.hpp>
#include "tf2_ros/buffer.h"

namespace cartographer_ros {

class PlayableBag {
 public:
  // Handles messages early, i.e. when they are about to enter the buffer.
  // Returns a boolean indicating whether the message should enter the buffer.
  using FilteringEarlyMessageHandler =
      std::function<bool /* forward_message_to_buffer */ (
          std::shared_ptr<rosbag2_storage::SerializedBagMessage>)>;

  PlayableBag(const std::string& bag_filename, int bag_id,
              rclcpp::Duration buffer_delay,
              FilteringEarlyMessageHandler filtering_early_message_handler);

  rclcpp::Time PeekMessageTime() const;
  rosbag2_storage::SerializedBagMessage GetNextMessage(
      cartographer_ros_msgs::msg::BagfileProgress* progress);
  bool IsMessageAvailable() const;
  std::tuple<rclcpp::Time, rclcpp::Time> GetBeginEndTime() const;

  int bag_id() const;
  std::set<std::string> topics() const { return topics_; }
  double duration_in_seconds() const { return duration_in_seconds_; }
  bool finished() const { return finished_; }
  rosbag2_storage::BagMetadata bag_metadata;

 private:
  void AdvanceOneMessage();
  void AdvanceUntilMessageAvailable();

  std::unique_ptr<rosbag2_cpp::Reader> bag_reader_;
  bool finished_;
  const int bag_id_;
  const std::string bag_filename_;
  double duration_in_seconds_;
  int message_counter_;
  std::deque<rosbag2_storage::SerializedBagMessage> buffered_messages_;
  const rclcpp::Duration  buffer_delay_;
  FilteringEarlyMessageHandler filtering_early_message_handler_;
  std::set<std::string> topics_;
};

class PlayableBagMultiplexer {
 public:
  PlayableBagMultiplexer(rclcpp::Node::SharedPtr node);
  void AddPlayableBag(PlayableBag playable_bag);

  // Returns the next message from the multiplexed (merge-sorted) message
  // stream, along with the bag id corresponding to the message, and whether
  // this was the last message in that bag.
  std::tuple<rosbag2_storage::SerializedBagMessage, int, std::string, bool> GetNextMessage();

  bool IsMessageAvailable() const;
  rclcpp::Time PeekMessageTime() const;

  std::set<std::string> topics() const { return topics_; }

 private:
  struct BagMessageItem {
    rclcpp::Time message_timestamp;
    int bag_index;
    struct TimestampIsGreater {
      bool operator()(const BagMessageItem& l, const BagMessageItem& r) {
        return l.message_timestamp > r.message_timestamp;
      }
    };
  };

  rclcpp::Node::SharedPtr node_;
  // Publishes information about the bag-file(s) processing and its progress
  rclcpp::Publisher<cartographer_ros_msgs::msg::BagfileProgress>::SharedPtr bag_progress_pub_;
  // Map between bagfile id and the last time when its progress was published
  std::map<int, rclcpp::Time> bag_progress_time_map_;
  // The time interval of publishing bag-file(s) processing in seconds
  double progress_pub_interval_;

  std::vector<PlayableBag> playable_bags_;
  std::priority_queue<BagMessageItem, std::vector<BagMessageItem>,
                      BagMessageItem::TimestampIsGreater>
      next_message_queue_;
  std::set<std::string> topics_;
};

}  // namespace cartographer_ros

#endif  // CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_PLAYABLE_BAG_H