player.cpp

// Copyright 2018, Bosch Software Innovations GmbH.

//

// 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.

#include "rosbag2_transport/player.hpp"

#include "rosbag2_transport/record_h264_codec.hpp"

#include <algorithm>

#include <chrono>

#include <memory>

#include <queue>

#include <string>

#include <unordered_map>

#include <utility>

#include <vector>

#include <unistd.h>

#include "rcl/graph.h"

#include "rclcpp/rclcpp.hpp"

#include "rcutils/time.h"

#include "rosbag2_cpp/clocks/time_controller_clock.hpp"

#include "rosbag2_cpp/reader.hpp"

#include "rosbag2_cpp/typesupport_helpers.hpp"

#include "rosbag2_storage/storage_filter.hpp"

#include "qos.hpp"

namespace

{

/**

* Trivial std::unique_lock wrapper providing constructor that allows Clang Thread Safety Analysis.

* The std::unique_lock does not have these annotations.

*/

class RCPPUTILS_TSA_SCOPED_CAPABILITY TSAUniqueLock : public std::unique_lock<std::mutex>

{

public:

explicit TSAUniqueLock(std::mutex & mu) RCPPUTILS_TSA_ACQUIRE(mu)

: std::unique_lock<std::mutex>(mu)

{}

~TSAUniqueLock() RCPPUTILS_TSA_RELEASE() {}

};

/**

* Determine which QoS to offer for a topic.

* The priority of the profile selected is:

* 1. The override specified in play_options (if one exists for the topic).

* 2. A profile automatically adapted to the recorded QoS profiles of publishers on the topic.

*

* \param topic_name The full name of the topic, with namespace (ex. /arm/joint_status).

* \param topic_qos_profile_overrides A map of topic to QoS profile overrides.

* @return The QoS profile to be used for subscribing.

*/

rclcpp::QoS publisher_qos_for_topic(

const rosbag2_storage::TopicMetadata & topic,

const std::unordered_map<std::string, rclcpp::QoS> & topic_qos_profile_overrides,

const rclcpp::Logger & logger)

{

using rosbag2_transport::Rosbag2QoS;

auto qos_it = topic_qos_profile_overrides.find(topic.name);

if (qos_it != topic_qos_profile_overrides.end()) {

RCLCPP_INFO_STREAM(

logger,

"Overriding QoS profile for topic " << topic.name);

return Rosbag2QoS{qos_it->second};

} else if (topic.offered_qos_profiles.empty()) {

return Rosbag2QoS{};

}

const auto profiles_yaml = YAML::Load(topic.offered_qos_profiles);

const auto offered_qos_profiles = profiles_yaml.as<std::vector<Rosbag2QoS>>();

return Rosbag2QoS::adapt_offer_to_recorded_offers(topic.name, offered_qos_profiles);

}

} // namespace

namespace rosbag2_transport

{

int topic_n = 0;

int special_topic_num = 0;

int pub_topic_num = 0;

 

bool comp_min_element(std::pair<uint8_t, rosbag2_storage::SerializedBagMessageSharedPtr> el1,

std::pair<uint8_t, rosbag2_storage::SerializedBagMessageSharedPtr> el2)

{

if(el1.second == nullptr) {

return false;

}

if(el2.second == nullptr) {

return true;

}

return el1.second->time_stamp < el2.second->time_stamp;

}

Player::Player(const std::string & node_name, const rclcpp::NodeOptions & node_options)

: rclcpp::Node(node_name, node_options)

{

// TODO(karsten1987): Use this constructor later with parameter parsing.

// The reader, storage_options as well as play_options can be loaded via parameter.

// That way, the player can be used as a simple component in a component manager.

throw rclcpp::exceptions::UnimplementedError();

}

Player::Player(

const rosbag2_storage::StorageOptions & storage_options,

const rosbag2_transport::PlayOptions & play_options,

const std::string & node_name,

const rclcpp::NodeOptions & node_options)

: Player(std::make_unique<rosbag2_cpp::Reader>(),

storage_options, play_options,

node_name, node_options)

{}

Player::Player(

std::unique_ptr<rosbag2_cpp::Reader> reader,

const rosbag2_storage::StorageOptions & storage_options,

const rosbag2_transport::PlayOptions & play_options,

const std::string & node_name,

const rclcpp::NodeOptions & node_options)

: Player(std::move(reader),

// only call KeyboardHandler when using default keyboard handler implementation

std::shared_ptr<KeyboardHandler>(new KeyboardHandler()),

storage_options, play_options,

node_name, node_options)

{}

Player::Player(

std::unique_ptr<rosbag2_cpp::Reader> reader,

std::shared_ptr<KeyboardHandler> keyboard_handler,

const rosbag2_storage::StorageOptions & storage_options,

const rosbag2_transport::PlayOptions & play_options,

const std::string & node_name,

const rclcpp::NodeOptions & node_options)

: rclcpp::Node(

node_name,

rclcpp::NodeOptions(node_options).arguments(play_options.topic_remapping_options)),

storage_options_(storage_options),

play_options_(play_options),

keyboard_handler_(keyboard_handler)

{

{

std::lock_guard<std::mutex> lk(reader_mutex_);

reader_ = std::move(reader);

// keep reader open until player is destroyed

reader_->open(storage_options_, {"", rmw_get_serialization_format()});

auto metadata = reader_->get_metadata();

starting_time_ = std::chrono::duration_cast<std::chrono::nanoseconds>(

metadata.starting_time.time_since_epoch()).count();

// If a non-default (positive) starting time offset is provided in PlayOptions,

// then add the offset to the starting time obtained from reader metadata

if (play_options_.start_offset < 0) {

RCLCPP_WARN_STREAM(

get_logger(),

"Invalid start offset value: " <<

RCUTILS_NS_TO_S(static_cast<double>(play_options_.start_offset)) <<

". Negative start offset ignored.");

} else {

starting_time_ += play_options_.start_offset;

}

clock_ = std::make_unique<rosbag2_cpp::TimeControllerClock>(

starting_time_, std::chrono::steady_clock::now,

std::chrono::milliseconds{100}, play_options_.start_paused);

set_rate(play_options_.rate);

topic_qos_profile_overrides_ = play_options_.topic_qos_profile_overrides;

prepare_publishers();

}

srv_pause_ = create_service<rosbag2_interfaces::srv::Pause>(

"~/pause",

[this](

const std::shared_ptr<rmw_request_id_t>/* request_header */,

const std::shared_ptr<rosbag2_interfaces::srv::Pause::Request>/* request */,

const std::shared_ptr<rosbag2_interfaces::srv::Pause::Response>/* response */)

{

pause();

});

srv_resume_ = create_service<rosbag2_interfaces::srv::Resume>(

"~/resume",

[this](

const std::shared_ptr<rmw_request_id_t>/* request_header */,

const std::shared_ptr<rosbag2_interfaces::srv::Resume::Request>/* request */,

const std::shared_ptr<rosbag2_interfaces::srv::Resume::Response>/* response */)

{

resume();

});

srv_toggle_paused_ = create_service<rosbag2_interfaces::srv::TogglePaused>(

"~/toggle_paused",

[this](

const std::shared_ptr<rmw_request_id_t>/* request_header */,

const std::shared_ptr<rosbag2_interfaces::srv::TogglePaused::Request>/* request */,

const std::shared_ptr<rosbag2_interfaces::srv::TogglePaused::Response>/* response */)

{

toggle_paused();

});

srv_is_paused_ = create_service<rosbag2_interfaces::srv::IsPaused>(

"~/is_paused",

[this](

const std::shared_ptr<rmw_request_id_t>/* request_header */,

const std::shared_ptr<rosbag2_interfaces::srv::IsPaused::Request>/* request */,

const std::shared_ptr<rosbag2_interfaces::srv::IsPaused::Response> response)

{

response->paused = is_paused();

});

srv_get_rate_ = create_service<rosbag2_interfaces::srv::GetRate>(

"~/get_rate",

[this](

const std::shared_ptr<rmw_request_id_t>/* request_header */,

const std::shared_ptr<rosbag2_interfaces::srv::GetRate::Request>/* request */,

const std::shared_ptr<rosbag2_interfaces::srv::GetRate::Response> response)

{

response->rate = get_rate();

});

srv_set_rate_ = create_service<rosbag2_interfaces::srv::SetRate>(

"~/set_rate",

[this](

const std::shared_ptr<rmw_request_id_t>/* request_header */,

const std::shared_ptr<rosbag2_interfaces::srv::SetRate::Request> request,

const std::shared_ptr<rosbag2_interfaces::srv::SetRate::Response> response)

{

response->success = set_rate(request->rate);

});

srv_play_next_ = create_service<rosbag2_interfaces::srv::PlayNext>(

"~/play_next",

[this](

const std::shared_ptr<rmw_request_id_t>/* request_header */,

const std::shared_ptr<rosbag2_interfaces::srv::PlayNext::Request>/* request */,

const std::shared_ptr<rosbag2_interfaces::srv::PlayNext::Response> response)

{

response->success = play_next();

});

add_keyboard_callbacks();

}

Player::Player(

rosbag2_storage::BagMetadata & metadata,

std::vector<std::string>& bag_files,

std::shared_ptr<KeyboardHandler> keyboard_handler,

const rosbag2_storage::StorageOptions & storage_options,

const rosbag2_transport::PlayOptions & play_options,

const std::string & node_name,

const rclcpp::NodeOptions & node_options)

: rclcpp::Node(

node_name,

rclcpp::NodeOptions(node_options).arguments(play_options.topic_remapping_options)),

md_(metadata),

storage_options_(storage_options),

play_options_(play_options),

keyboard_handler_(keyboard_handler)

{

// new_bags_.reserve(256);

for(size_t i = 0; i < bag_files.size(); ++i) {

initial_bags_.push_back(bag_files[i]);

}

all_bags_.swap(bag_files);

{

std::lock_guard<std::mutex> lk(reader_mutex_);

starting_time_ = std::chrono::duration_cast<std::chrono::nanoseconds>(

md_.starting_time.time_since_epoch()).count();

if (play_options_.start_offset < 0) {

RCLCPP_WARN_STREAM(

get_logger(),

"Invalid start offset value: " <<

RCUTILS_NS_TO_S(static_cast<double>(play_options_.start_offset)) <<

". Negative start offset ignored.");

} else {

starting_time_ += play_options_.start_offset;

}

clock_ = std::make_unique<rosbag2_cpp::TimeControllerClock>(

starting_time_, std::chrono::steady_clock::now,

std::chrono::milliseconds{100}, play_options_.start_paused);

set_rate(play_options_.rate);

topic_qos_profile_overrides_ = play_options_.topic_qos_profile_overrides;

prepare_metaddata_publishers();

}

create_addbag_subscriber();

srv_pause_ = create_service<rosbag2_interfaces::srv::Pause>(

"~/pause",

[this](

const std::shared_ptr<rmw_request_id_t>/* request_header */,

const std::shared_ptr<rosbag2_interfaces::srv::Pause::Request>/* request */,

const std::shared_ptr<rosbag2_interfaces::srv::Pause::Response>/* response */)

{

pause();

});

srv_resume_ = create_service<rosbag2_interfaces::srv::Resume>(

"~/resume",

[this](

const std::shared_ptr<rmw_request_id_t>/* request_header */,

const std::shared_ptr<rosbag2_interfaces::srv::Resume::Request>/* request */,

const std::shared_ptr<rosbag2_interfaces::srv::Resume::Response>/* response */)

{

resume();

});

srv_toggle_paused_ = create_service<rosbag2_interfaces::srv::TogglePaused>(

"~/toggle_paused",

[this](

const std::shared_ptr<rmw_request_id_t>/* request_header */,

const std::shared_ptr<rosbag2_interfaces::srv::TogglePaused::Request>/* request */,

const std::shared_ptr<rosbag2_interfaces::srv::TogglePaused::Response>/* response */)

{

toggle_paused();

});

srv_is_paused_ = create_service<rosbag2_interfaces::srv::IsPaused>(

"~/is_paused",

[this](

const std::shared_ptr<rmw_request_id_t>/* request_header */,

const std::shared_ptr<rosbag2_interfaces::srv::IsPaused::Request>/* request */,

const std::shared_ptr<rosbag2_interfaces::srv::IsPaused::Response> response)

{

response->paused = is_paused();

});

srv_get_rate_ = create_service<rosbag2_interfaces::srv::GetRate>(

"~/get_rate",

[this](

const std::shared_ptr<rmw_request_id_t>/* request_header */,

const std::shared_ptr<rosbag2_interfaces::srv::GetRate::Request>/* request */,

const std::shared_ptr<rosbag2_interfaces::srv::GetRate::Response> response)

{

response->rate = get_rate();

});

srv_set_rate_ = create_service<rosbag2_interfaces::srv::SetRate>(

"~/set_rate",

[this](

const std::shared_ptr<rmw_request_id_t>/* request_header */,

const std::shared_ptr<rosbag2_interfaces::srv::SetRate::Request> request,

const std::shared_ptr<rosbag2_interfaces::srv::SetRate::Response> response)

{

response->success = set_rate(request->rate);

});

srv_play_next_ = create_service<rosbag2_interfaces::srv::PlayNext>(

"~/play_next",

[this](

const std::shared_ptr<rmw_request_id_t>/* request_header */,

const std::shared_ptr<rosbag2_interfaces::srv::PlayNext::Request>/* request */,

const std::shared_ptr<rosbag2_interfaces::srv::PlayNext::Response> response)

{

response->success = play_next();

});

add_keyboard_callbacks();

}

void Player::prepare_metaddata_publishers()

{

rosbag2_storage::StorageFilter storage_filter;

storage_filter.topics = play_options_.topics_to_filter;

// reader_->set_filter(storage_filter);

// Create /clock publisher

if (play_options_.clock_publish_frequency > 0.f) {

const auto publish_period = std::chrono::nanoseconds(

static_cast<uint64_t>(RCUTILS_S_TO_NS(1) / play_options_.clock_publish_frequency));

// NOTE: PlayerClock does not own this publisher because rosbag2_cpp

// should not own transport-based functionality

clock_publisher_ = this->create_publisher<rosgraph_msgs::msg::Clock>(

"/clock", rclcpp::ClockQoS());

clock_publish_timer_ = this->create_wall_timer(

publish_period, [this]() {

auto msg = rosgraph_msgs::msg::Clock();

msg.clock = rclcpp::Time(clock_->now());

clock_publisher_->publish(msg);

});

}

// Create topic publishers

// auto topics = reader_->get_all_topics_and_types();

std::vector<rosbag2_storage::TopicMetadata> topics_metadat;

for (const auto & topic_information : md_.topics_with_message_count) {

topics_metadat.push_back(topic_information.topic_metadata);

}

for (const auto & topic : topics_metadat) {

if (publishers_.find(topic.name) != publishers_.end()) {

continue;

}

// filter topics to add publishers if necessary

auto & filter_topics = storage_filter.topics;

if (!filter_topics.empty()) {

auto iter = std::find(filter_topics.begin(), filter_topics.end(), topic.name);

if (iter == filter_topics.end()) {

continue;

}

}

auto topic_qos = publisher_qos_for_topic(

topic, topic_qos_profile_overrides_,

this->get_logger());

try {

publishers_.insert(

std::make_pair(

topic.name, this->create_generic_publisher(topic.name, topic.type, topic_qos)));

} catch (const std::runtime_error & e) {

// using a warning log seems better than adding a new option

// to ignore some unknown message type library

RCLCPP_WARN(

this->get_logger(),

"Ignoring a topic '%s', reason: %s.", topic.name.c_str(), e.what());

}

}

}

void Player::create_add_topic_publisher(rosbag2_storage::BagMetadata& metadata)

{

std::vector<rosbag2_storage::TopicMetadata> topics_metadat;

for (const auto & topic_information : metadata.topics_with_message_count) {

topics_metadat.push_back(topic_information.topic_metadata);

}

rosbag2_storage::StorageFilter storage_filter;

storage_filter.topics = play_options_.topics_to_filter;

for (const auto & topic : topics_metadat) {

if (publishers_.find(topic.name) != publishers_.end()) {

continue;

}

// filter topics to add publishers if necessary

auto & filter_topics = storage_filter.topics;

if (!filter_topics.empty()) {

auto iter = std::find(filter_topics.begin(), filter_topics.end(), topic.name);

if (iter == filter_topics.end()) {

continue;

}

}

auto topic_qos = publisher_qos_for_topic(

topic, topic_qos_profile_overrides_,

this->get_logger());

try {

publishers_.insert(

std::make_pair(

topic.name, this->create_generic_publisher(topic.name, topic.type, topic_qos)));

} catch (const std::runtime_error & e) {

// using a warning log seems better than adding a new option

// to ignore some unknown message type library

RCLCPP_WARN(

this->get_logger(),

"Ignoring a topic '%s', reason: %s.", topic.name.c_str(), e.what());

}

}

}

void Player::create_addbag_subscriber() {

addbag_subscriber_ = this->create_subscription<std_msgs::msg::String>(

"/rosbag2_player/add_db3",

10,

std::bind(&Player::addtopic_callback, this, std::placeholders::_1)

);

}

void Player::addtopic_callback(const std_msgs::msg::String::SharedPtr msg)

{

std::cout << "Enter in Player::addtopic_callback" << std::endl;

if(all_bags_.end() != std::find(all_bags_.begin(), all_bags_.end(), msg->data)) {

std::cout << "received New bag file: " << msg->data.c_str() << ", ignore " << std::endl;

return;

}

std::cout << "New bag file: " << msg->data.c_str() << std::endl;

all_bags_.push_back(msg->data);

rosbag2_storage::StorageOptions storage_options;

storage_options.uri = msg->data;

storage_options.storage_id="sqlite3";

auto reader = ReaderWriterFactory::make_reader(storage_options);

reader->open(storage_options);

{

std::lock_guard<std::mutex> lc(bag_mutex_);

int index = new_bag_index_.load();

new_bag_info bag_info(reader, storage_options.uri);

new_bags_.emplace_back(std::move(bag_info));

}

auto bag_topics_and_types = new_bags_[new_bag_index_].reader_->get_all_topics_and_types();

for (const auto & topic_metadata : bag_topics_and_types) {

// const std::string & topic_name = topic_metadata.name;

if(close_writer != nullptr) {

close_writer->add_topic_with_info(topic_metadata);

}

// filtered_outputs.try_emplace(topic_name);

// filtered_outputs[topic_name].push_back(writer.get());

}

rosbag2_storage::BagMetadata metadata = new_bags_[new_bag_index_].reader_->get_metadata();

create_add_topic_publisher(metadata);

++new_bag_index_;

new_bag_.store(true);

bag_cv_.notify_one();

}

void Player::delete_played_bag(std::string bag_file)

{

if (unlink(bag_file.c_str()) == 0) {

std::cout << "File " << bag_file << " deleted successfully\n";

} else {

if (errno == ENOENT) {

std::cout << "File " << bag_file << " does not exist\n";

} else {

std::cout << "File " << bag_file << " Error deleting file\n";

}

}

}

int new_bag_number = 0;

void Player::write_new_bag_to_cache(rosbag2_cpp::Writer* writer)

{

std::shared_ptr<rosbag2_storage::SerializedBagMessage> bag_msg;

std::cout << "Enter in write_new_bag_to_cache, new_bag_index_ is: " << new_bag_index_.load() << std::endl;

for(size_t index = 0; index < new_bag_index_.load(); ++index) {

if(new_bags_[index].is_writer) {

continue;

}

while(new_bags_[index].reader_->has_next())

{

++new_bag_number;

++topic_n;

bag_msg = new_bags_[index].reader_->read_next();

writer->write(bag_msg);

}

new_bags_[index].is_writer = true;

// delete_played_bag(new_bags_[index].bag_uri);

}

std::cout << "leave write_new_bag_to_cache, add new bag messages: " << new_bag_number << std::endl;

}

Player::~Player()

{

// remove callbacks on key_codes to prevent race conditions

// Note: keyboard_handler handles locks between removing & executing callbacks

for (auto cb_handle : keyboard_callbacks_) {

keyboard_handler_->delete_key_press_callback(cb_handle);

}

// closes reader

std::lock_guard<std::mutex> lk(reader_mutex_);

if (reader_) {

reader_->close();

}

}

rosbag2_cpp::Reader * Player::release_reader()

{

reader_->close();

return reader_.release();

}

const std::chrono::milliseconds

Player::queue_read_wait_period_ = std::chrono::milliseconds(100);

bool Player::is_storage_completely_loaded() const

{

if (storage_loading_future_.valid() &&

storage_loading_future_.wait_for(std::chrono::seconds(0)) == std::future_status::ready)

{

storage_loading_future_.get();

}

return !storage_loading_future_.valid();

}

std::unordered_map<std::string, std::vector<rosbag2_cpp::Writer *>>

Player::get_topic_and_writer(

const std::vector<std::unique_ptr<rosbag2_cpp::Reader>> & input_bags,

const std::vector<std::pair<std::unique_ptr<rosbag2_cpp::Writer>, rosbag2_transport::RecordOptions>> & output_bags)

{

std::unordered_map<std::string, std::vector<rosbag2_cpp::Writer *>> filtered_outputs;

for (const auto & [writer, record_options] : output_bags) {

for (const auto & input_bag : input_bags) {

auto bag_topics_and_types = input_bag->get_all_topics_and_types();

for (const auto & topic_metadata : bag_topics_and_types) {

const std::string & topic_name = topic_metadata.name;

writer->add_topic_with_info(topic_metadata);

filtered_outputs.try_emplace(topic_name);

filtered_outputs[topic_name].push_back(writer.get());

}

}

}

return filtered_outputs;

}

void Player::perform_rewrite_parallel(

const std::vector<std::unique_ptr<rosbag2_cpp::Reader>> & input_bags,

const std::vector<std::pair<std::unique_ptr<rosbag2_cpp::Writer>, rosbag2_transport::RecordOptions>> & output_bags

)

{

if (input_bags.empty() || output_bags.empty()) {

throw std::runtime_error("Must provide at least one input and one output bag to rewrite.");

}

close_writer = output_bags[0].first.get();

std::unordered_map<std::string, std::vector<rosbag2_cpp::Writer*>> topic_outputs = get_topic_and_writer(input_bags, output_bags);

std::vector<std::shared_ptr<rosbag2_storage::SerializedBagMessage>> next_messages;

next_messages.resize(input_bags.size(), nullptr);

std::shared_ptr<rosbag2_storage::SerializedBagMessage> next_msg;

uint8_t has_message = 0;

int8_t bags_size = input_bags.size() - 1;

uint8_t size_bit = 0;

while (bags_size >= 0) {

size_bit |= 1 << bags_size;

--bags_size;

}

int temp = 0;

std::map<uint8_t, rosbag2_storage::SerializedBagMessageSharedPtr> next_bags_msgs;

for(size_t index = 0; index < input_bags.size(); ++index) {

next_bags_msgs.insert({index, nullptr});

}

while (has_message < size_bit)

{

for (size_t i = 0; i < input_bags.size(); i++)

{

if(next_bags_msgs.at(i) == nullptr)

{

if(input_bags.at(i)->has_next()) {

next_bags_msgs.at(i) = input_bags.at(i)->read_next();

}

else {

temp = 1 << i;

has_message |= temp;

}

}

}

auto it = std::min_element(next_bags_msgs.begin(), next_bags_msgs.end(), comp_min_element);

next_msg = it->second;

if(next_msg == nullptr) {

continue;

}

auto topic_writers = topic_outputs.find(next_msg->topic_name);

if (topic_writers != topic_outputs.end()) {

// if(next_msg->topic_name == "/driver/radar_right_front_object")

// std::cout << "time stamp is: " << next_msg->time_stamp << ", topic is: next_msg->topic_name: " << next_msg->topic_name << std::endl;

close_writer->write(next_msg);

++topic_n;

// std::cout << "writer the " << topic_n << " topic: " << next_msg->topic_name << std::endl;

// for (auto writer : topic_writers->second) {

// }

// close_writer = topic_writers->second[0];

}

it->second = nullptr;

}

// for(auto i : initial_bags_) {

// // delete_played_bag(i);

// }

write_new_bag_to_cache(close_writer);

// while (true)

// {

// // std::cout << "is_waiting_ ..." << std::endl;

// is_waiting_.store(true);

// std::unique_lock<std::mutex> lc(bag_mutex_);

// bag_cv_.wait(lc, [this](){

// return new_bag_.load() || !is_in_play_;

// });

// if(!is_in_play_) {

// break;

// }

// size_t index = new_bag_index_.load() - 1;

// while(new_bags_[index].reader_->has_next())

// {

// next_msg = new_bags_[index].reader_->read_next();

// close_writer->write(next_msg);

// new_bags_[index].is_writer = true;

// }

// new_bag_.store(false);

// }

// close_writer->close_cache_consumer();

std::cout << "end of writer topic, number is: " << topic_n << std::endl;

}

// void Player::play(bool h264_enable)

void Player::play()

{

std::cout << "Enter in Player::play, start play\n";

is_in_play_ = true;

float delay;

if (play_options_.delay >= 0.0) {

delay = play_options_.delay;

} else {

RCLCPP_WARN(

this->get_logger(),

"Invalid delay value: %f. Delay is disabled.",

play_options_.delay);

delay = 0.0;

}

RCLCPP_INFO_STREAM(get_logger(), "Start to play...");

try {

do {

if (delay > 0.0) {

RCLCPP_INFO_STREAM(this->get_logger(), "Sleep " << delay << " sec");

std::chrono::duration<float> duration(delay);

std::this_thread::sleep_for(duration);

}

reader_->open(storage_options_, {"", rmw_get_serialization_format()});

const auto starting_time = std::chrono::duration_cast<std::chrono::nanoseconds>(

reader_->get_metadata().starting_time.time_since_epoch()).count();

clock_->jump(starting_time);

storage_loading_future_ = std::async(

std::launch::async,

[this]() {load_storage_content();});

// is_h264_decode_enabled_.store(h264_enable);

wait_for_filled_queue();

play_messages_from_queue();

reader_->close();

} while (rclcpp::ok() && play_options_.loop);

} catch (std::runtime_error & e) {

RCLCPP_ERROR(this->get_logger(), "Failed to play: %s", e.what());

}

is_in_play_ = false;

}

using namespace std::chrono_literals;

void Player::play_with_cache(std::vector<std::unique_ptr<rosbag2_cpp::Reader>>& input_bags,

std::vector<std::pair<std::unique_ptr<rosbag2_cpp::Writer>, rosbag2_transport::RecordOptions>>& output_bags)

{

std::cout << "Enter in Player::play_with_cache\n";

is_in_play_ = true;

float delay;

if (play_options_.delay >= 0.0) {

delay = play_options_.delay;

} else {

RCLCPP_WARN(

this->get_logger(),

"Invalid delay value: %f. Delay is disabled.",

play_options_.delay);

delay = 0.0;

}

RCLCPP_INFO_STREAM(get_logger(), "Start to play...");

try {

do {

if (delay > 0.0) {

RCLCPP_INFO_STREAM(this->get_logger(), "Sleep " << delay << " sec");

std::chrono::duration<float> duration(delay);

std::this_thread::sleep_for(duration);

}

const auto starting_time = std::chrono::duration_cast<std::chrono::nanoseconds>(

md_.starting_time.time_since_epoch()).count();

clock_->jump(starting_time);

storage_loading_future_ = std::async(std::launch::async,

[this, &input_bags, &output_bags] () {

perform_rewrite_parallel(input_bags, output_bags);

}

);

wait_for_filled_queue();

play_messages_from_buffer();

} while (rclcpp::ok() && play_options_.loop);

} catch (std::runtime_error & e) {

RCLCPP_ERROR(this->get_logger(), "Failed to play: %s", e.what());

}

is_in_play_ = false;

new_bag_.store(true);

bag_cv_.notify_one();

// std::cout << "End of play_with_cache, pub_topic_num is: " << pub_topic_num << std::endl;

}

void Player::pause()

{

clock_->pause();

RCLCPP_INFO_STREAM(get_logger(), "Pausing play. Press space to continue ...");

}

void Player::resume()

{

clock_->resume();

RCLCPP_INFO_STREAM(get_logger(), "Resuming play. Press space to pause ...");

}

void Player::toggle_paused()

{

is_paused() ? resume() : pause();

}

bool Player::is_paused() const

{

return clock_->is_paused();

}

double Player::get_rate() const

{

return clock_->get_rate();

}

bool Player::set_rate(double rate)

{

bool ok = clock_->set_rate(rate);

if (ok) {

RCLCPP_INFO_STREAM(get_logger(), "Set rate to " << rate);

} else {

RCLCPP_WARN_STREAM(get_logger(), "Failed to set rate to invalid value " << rate);

}

return ok;

}

rosbag2_storage::SerializedBagMessageSharedPtr * Player::peek_next_message_from_queue()

{

rosbag2_storage::SerializedBagMessageSharedPtr * message_ptr = message_queue_.peek();

if (message_ptr == nullptr && !is_storage_completely_loaded() && rclcpp::ok()) {

RCLCPP_WARN(

this->get_logger(),

"Message queue starved. Messages will be delayed. Consider "

"increasing the --read-ahead-queue-size option.");

while (message_ptr == nullptr && !is_storage_completely_loaded() && rclcpp::ok()) {

std::this_thread::sleep_for(std::chrono::microseconds(100));

message_ptr = message_queue_.peek();

}

}

return message_ptr;

}

rosbag2_storage::SerializedBagMessageSharedPtr * Player::peek_next_message_from_buffer()

{

rosbag2_storage::SerializedBagMessageSharedPtr * message_ptr = message_queue_.peek();

if (message_ptr == nullptr && !is_waiting_.load() && rclcpp::ok()) {

RCLCPP_WARN(

this->get_logger(),

"Message queue starved. Messages will be delayed. Consider "

"increasing the --read-ahead-queue-size option.");

while (message_ptr == nullptr && !is_waiting_.load() && rclcpp::ok()) {

std::this_thread::sleep_for(std::chrono::microseconds(100));

message_ptr = message_queue_.peek();

}

}

return message_ptr;

}

bool Player::play_next()

{

// Temporary take over playback from play_messages_from_queue()

std::lock_guard<std::mutex> lk(skip_message_in_main_play_loop_mutex_);

if (!clock_->is_paused() || !is_in_play_) {

return false;

}

skip_message_in_main_play_loop_ = true;

rosbag2_storage::SerializedBagMessageSharedPtr * message_ptr = peek_next_message_from_queue();

bool next_message_published = false;

while (message_ptr != nullptr && !next_message_published) {

{

rosbag2_storage::SerializedBagMessageSharedPtr message = *message_ptr;

next_message_published = publish_message(message);

clock_->jump(message->time_stamp);

}

message_queue_.pop();

message_ptr = peek_next_message_from_queue();

}

return next_message_published;

}

void Player::wait_for_filled_queue() const

{

while (

message_queue_.size_approx() < play_options_.read_ahead_queue_size &&

!is_storage_completely_loaded() && rclcpp::ok())

{

std::this_thread::sleep_for(queue_read_wait_period_);

}

}

void Player::load_storage_content()

{

auto queue_lower_boundary =

static_cast<size_t>(play_options_.read_ahead_queue_size * read_ahead_lower_bound_percentage_);

auto queue_upper_boundary = play_options_.read_ahead_queue_size;

while (reader_->has_next() && rclcpp::ok()) {

if (message_queue_.size_approx() < queue_lower_boundary) {

enqueue_up_to_boundary(queue_upper_boundary);

} else {

std::this_thread::sleep_for(std::chrono::milliseconds(1));

}

}

}

void Player::load_message(rosbag2_storage::SerializedBagMessageSharedPtr message)

{

// std::cout << "Enter in load_message\n";

auto queue_lower_boundary =

static_cast<size_t>(play_options_.read_ahead_queue_size * read_ahead_lower_bound_percentage_);

size_t size = message_queue_.size_approx();

if (rclcpp::ok()) {

if(message_queue_.size_approx() > play_options_.read_ahead_queue_size * 2) {

std::this_thread::sleep_for(std::chrono::milliseconds(1));

}

message_queue_.enqueue(message);

}

std::cout << "add message to queue: " << ++message_count_ << ", message size is: " << size+1 << std::endl;

}

void Player::enqueue_up_to_boundary(uint64_t boundary)

{

rosbag2_storage::SerializedBagMessageSharedPtr message;

for (size_t i = message_queue_.size_approx(); i < boundary; i++) {

if (!reader_->has_next()) {

break;

}

message = reader_->read_next();

message_queue_.enqueue(message);

}

}

bool Player::is_key_frame(const std::vector<uint8_t>& data)

{

if(data.size() < 5){

return false;

}

int32_t nal_type = data.at(4) & 0x1F;

if(data.at(0) == 0 && data.at(1) == 0 && data.at(2) == 0 && data.at(3) == 1){

if(nal_type == 5 || nal_type == 7 || nal_type == 8){

RCLCPP_INFO(rclcpp::get_logger("rosbag2_transport"), "Found I frame at 4!!!!");

return true;

}

}

if(data.at(0) == 0 && data.at(1) == 0 && data.at(2) == 1){

nal_type = data.at(3) & 0x1f;

if(nal_type == 5 || nal_type == 7 || nal_type == 8){

RCLCPP_INFO(rclcpp::get_logger("rosbag2_transport"), "Found I frame at 3!!!!");

return true;

}

}

return false;

}

void Player::play_messages_from_queue()

{

playing_messages_from_queue_ = true;

// Note: We need to use message_queue_.peek() instead of message_queue_.try_dequeue(message)

// to support play_next() API logic.

rosbag2_storage::SerializedBagMessageSharedPtr * message_ptr = peek_next_message_from_queue();

while (message_ptr != nullptr && rclcpp::ok()) {

{

rosbag2_storage::SerializedBagMessageSharedPtr message = *message_ptr;

// Do not move on until sleep_until returns true

// It will always sleep, so this is not a tight busy loop on pause

while (rclcpp::ok() && !clock_->sleep_until(message->time_stamp))

{

}

if (rclcpp::ok()) {

{

std::lock_guard<std::mutex> lk(skip_message_in_main_play_loop_mutex_);

if (skip_message_in_main_play_loop_) {

skip_message_in_main_play_loop_ = false;

message_ptr = peek_next_message_from_queue();

continue;

}

}

// std::cout << "play_messages_from_queue..."<<std::endl;

//h264 feature: decode h264 data from storage before playing

std::shared_ptr<sensor_msgs::msg::Image> ros_msg = std::make_shared<sensor_msgs::msg::Image>();

rclcpp::Serialization<sensor_msgs::msg::Image> ser;

// if(is_h264_decode_enabled_.load()

// && message->topic_name.find("/driver/camera_") != std::string::npos

// && message->topic_name.find("_image") != std::string::npos)

// {

// auto serialize_msg = rclcpp::SerializedMessage(*message->serialized_data);

// ser.deserialize_message(&serialize_msg, ros_msg.get());

// // static bool iframe_found = false;

// // if(!iframe_found && is_key_frame(ros_msg->data)){

// // iframe_found = true;

// // }

// // if(!iframe_found){

// // RCLCPP_INFO(rclcpp::get_logger("rosbag2_transport"), "waiting for I frame..........!!!!, data size: %d", ros_msg->data.size());

// // message_queue_.pop();

// // message_ptr = peek_next_message_from_queue();

// // continue;

// // }

// if(RecordCodecH264::decode(message->topic_name, ros_msg)){

// ser.serialize_message(ros_msg.get(), &serialize_msg);

// *message->serialized_data = serialize_msg.release_rcl_serialized_message();

// }

// else{

// RCLCPP_ERROR(rclcpp::get_logger("rosbag2_transport"), "Failed to decode h264 image of topic: %s", message->topic_name);

// message_queue_.pop();

// message_ptr = peek_next_message_from_queue();

// continue;

// }

// }

publish_message(message);

}

message_queue_.pop();

message_ptr = peek_next_message_from_queue();

}

}

playing_messages_from_queue_ = false;

}

int publish_count = 0;

void Player::play_messages_from_buffer()

{

std::cout << "Player::play_messages_from_buffer" << std::endl;

playing_messages_from_queue_ = true;

// Note: We need to use message_queue_.peek() instead of message_queue_.try_dequeue(message)

// to support play_next() API logic.

rosbag2_storage::SerializedBagMessageSharedPtr * message_ptr = peek_next_message_from_buffer();

while (message_ptr != nullptr && rclcpp::ok()) {

{

rosbag2_storage::SerializedBagMessageSharedPtr message = *message_ptr;

// Do not move on until sleep_until returns true

// It will always sleep, so this is not a tight busy loop on pause

while (rclcpp::ok() && !clock_->sleep_until(message->time_stamp))

{

}

if (rclcpp::ok()) {

{

std::lock_guard<std::mutex> lk(skip_message_in_main_play_loop_mutex_);

if (skip_message_in_main_play_loop_) {

skip_message_in_main_play_loop_ = false;

message_ptr = peek_next_message_from_buffer();

continue;

}

}

std::shared_ptr<sensor_msgs::msg::Image> ros_msg = std::make_shared<sensor_msgs::msg::Image>();

rclcpp::Serialization<sensor_msgs::msg::Image> ser;

std::cout << "***** publish_message *****" << std::endl;

publish_message(message);

}

// std::cout << "----get next message_queue_ msg----pop "<<std::endl;

message_queue_.pop();

auto size = message_queue_.size_approx();

std::cout << "----message_queue_ size---- "<<message_queue_.size_approx()<<std::endl;

if(size > 0){

message_ptr = peek_next_message_from_buffer();

}else{

break;

}

// message_ptr = peek_next_message_from_buffer();

// std::cout << "----get next message_queue_ msg---- "<<std::endl;

}

}

playing_messages_from_queue_ = false;

std::cout << "publish_count is: " << publish_count << std::endl;

}

void Player::prepare_publishers()

{

rosbag2_storage::StorageFilter storage_filter;

storage_filter.topics = play_options_.topics_to_filter;

reader_->set_filter(storage_filter);

// Create /clock publisher

if (play_options_.clock_publish_frequency > 0.f) {

const auto publish_period = std::chrono::nanoseconds(

static_cast<uint64_t>(RCUTILS_S_TO_NS(1) / play_options_.clock_publish_frequency));

// NOTE: PlayerClock does not own this publisher because rosbag2_cpp

// should not own transport-based functionality

clock_publisher_ = this->create_publisher<rosgraph_msgs::msg::Clock>(

"/clock", rclcpp::ClockQoS());

clock_publish_timer_ = this->create_wall_timer(

publish_period, [this]() {

auto msg = rosgraph_msgs::msg::Clock();

msg.clock = rclcpp::Time(clock_->now());

clock_publisher_->publish(msg);

});

}

// Create topic publishers

auto topics = reader_->get_all_topics_and_types();

for (const auto & topic : topics) {

if (publishers_.find(topic.name) != publishers_.end()) {

continue;

}

// filter topics to add publishers if necessary

auto & filter_topics = storage_filter.topics;

if (!filter_topics.empty()) {

auto iter = std::find(filter_topics.begin(), filter_topics.end(), topic.name);

if (iter == filter_topics.end()) {

continue;

}

}

auto topic_qos = publisher_qos_for_topic(

topic, topic_qos_profile_overrides_,

this->get_logger());

try {

publishers_.insert(

std::make_pair(

topic.name, this->create_generic_publisher(topic.name, topic.type, topic_qos)));

} catch (const std::runtime_error & e) {

// using a warning log seems better than adding a new option

// to ignore some unknown message type library

RCLCPP_WARN(

this->get_logger(),

"Ignoring a topic '%s', reason: %s.", topic.name.c_str(), e.what());

}

}

}

bool Player::publish_message(rosbag2_storage::SerializedBagMessageSharedPtr message)

{

bool message_published = false;

auto publisher_iter = publishers_.find(message->topic_name);

if (publisher_iter != publishers_.end()) {

if(publisher_iter->second == nullptr){

std::cout << "null publisher ptr..." <<std::endl;

}else

{

std::cout << "start publish, topic name is: " << message->topic_name << ", topic count is: " << ++publish_count << std::endl;

// ++publish_count;

publisher_iter->second->publish(rclcpp::SerializedMessage(*message->serialized_data));

message_published = true;

}

}

return message_published;

}

void Player::add_key_callback(

KeyboardHandler::KeyCode key,

const std::function<void()> & cb,

const std::string & op_name)

{

std::string key_str = enum_key_code_to_str(key);

if (key == KeyboardHandler::KeyCode::UNKNOWN) {

RCLCPP_ERROR_STREAM(

get_logger(),

"Invalid key binding " << key_str << " for " << op_name);

throw std::invalid_argument("Invalid key binding.");

}

keyboard_callbacks_.push_back(

keyboard_handler_->add_key_press_callback(

[cb](KeyboardHandler::KeyCode /*key_code*/,

KeyboardHandler::KeyModifiers /*key_modifiers*/) {cb();},

key));

// show instructions

RCLCPP_INFO_STREAM(

get_logger(),

"Press " << key_str << " for " << op_name);

}

void Player::add_keyboard_callbacks()

{

// skip if disabled

if (play_options_.disable_keyboard_controls) {

return;

}

RCLCPP_INFO_STREAM(get_logger(), "Adding keyboard callbacks.");

// check keybindings

add_key_callback(

play_options_.pause_resume_toggle_key,

[this]() {toggle_paused();},

"Pause/Resume"

);

add_key_callback(

play_options_.play_next_key,

[this]() {play_next();},

"Play Next Message"

);

add_key_callback(

play_options_.increase_rate_key,

[this]() {set_rate(get_rate() * 1.1);},

"Increase Rate 10%"

);

add_key_callback(

play_options_.decrease_rate_key,

[this]() {set_rate(get_rate() * 0.9);},

"Decrease Rate 10%"

);

}

} // namespace rosbag2_transport