ros2/rosbag2

rosbag2_transport::Recorder duration 0

bernatgaston opened this issue · 2 comments

Description

When using rosbag2_transport::Recorder from c++ library, the record starts but does not record anything. metadata duration is set to 0. I was guessing it was due to some parameter but I tried all that matter and the result is the same.

Expected Behavior

Record the subscribed topics as if your do ros2 bag record. This is the output of the metadata file if I do the same as the program does but from the cli.
ros2 bag record -a

rosbag2_bagfile_information: version: 5 storage_identifier: sqlite3 duration: nanoseconds: 16397558235 starting_time: nanoseconds_since_epoch: 1705586119189521394 message_count: 408 ....

Actual Behavior

[atlas_diagnostics-6] stdin is not a terminal device. Keyboard handling disabled.[INFO] [1705594489.438030834] [atlas.rosbag2_recorder]: Press SPACE for pausing/resuming
[atlas_diagnostics-6] [INFO] [1705594489.439090446] [rosbag2_storage]: Opened database '/tmp/rosbag_test_1/rosbag_test_1_0.db3' for READ_WRITE.
[atlas_diagnostics-6] [INFO] [1705594489.439415754] [atlas.rosbag2_recorder]: Listening for topics...
[atlas_diagnostics-6] [INFO] [1705594489.439423707] [atlas.rosbag2_recorder]: Event publisher thread: Starting
[atlas_diagnostics-6] [INFO] [1705594489.441240469] [atlas.rosbag2_recorder]: Subscribed to topic '/rosout'
[atlas_diagnostics-6] [INFO] [1705594489.442035187] [atlas.rosbag2_recorder]: Subscribed to topic '/parameter_events'
[atlas_diagnostics-6] [INFO] [1705594489.442579507] [atlas.rosbag2_recorder]: Subscribed to topic '/atlas/tf'
[atlas_diagnostics-6] [INFO] [1705594489.443045806] [atlas.rosbag2_recorder]: Subscribed to topic '/atlas/joy'
[atlas_diagnostics-6] [INFO] [1705594489.443581241] [atlas.rosbag2_recorder]: Subscribed to topic '/atlas/tf_static'
[atlas_diagnostics-6] [INFO] [1705594489.443987340] [atlas.rosbag2_recorder]: Subscribed to topic '/atlas/events/write_split'
[atlas_diagnostics-6] [INFO] [1705594489.444356789] [atlas.rosbag2_recorder]: Subscribed to topic '/atlas/Diagnostics/status'
[atlas_diagnostics-6] [INFO] [1705594489.444788244] [atlas.rosbag2_recorder]: Subscribed to topic '/atlas/Description/robot_description'
[atlas_diagnostics-6] [INFO] [1705594489.444847581] [atlas.rosbag2_recorder]: Recording...
[atlas_diagnostics-6] [INFO] [1705594489.445997596] [atlas.rosbag2_recorder]: Subscribed to topic '/atlas/Joystick/status'

....... (after 30 seconds).....

[atlas_diagnostics-6] [INFO] [1705594500.936705236] [atlas.Diagnostics]: Stopping recorder
[atlas_diagnostics-6] [INFO] [1705594500.986895716] [atlas.rosbag2_recorder]: Event publisher thread: Exiting
But then the metadata file is
rosbag2_bagfile_information: version: 5 storage_identifier: sqlite3 duration: nanoseconds: 0 starting_time: nanoseconds_since_epoch: 9223372036854775807 message_count: 0 ...

To Reproduce

** Steps to reproduce the behavior, e.g.

#include <iostream>
#include <memory>
#include <rosbag2_cpp/reader.hpp>
#include <rosbag2_cpp/writer.hpp>
#include <rosbag2_storage/storage_options.hpp>
#include <rosbag2_transport/record_options.hpp>
#include <rosbag2_transport/recorder.hpp>
#include <rclcpp/rclcpp.hpp>
#include <chrono>
#include <thread>`

class NodeClass : public rclcpp::Node {
public:
    // Constructor
    NodeClass() : Node("node_class") {
        // Additional constructor logic if needed
    }

// Member function to start recording
    bool StartRosbag() {
        // Create a ROS 2 bag recorder
        rosbag2_storage::StorageOptions storage_options;
        storage_options.uri = "/tmp/rosbag_test_1";
        storage_options.snapshot_mode = false;
        storage_options.max_bagfile_duration = 30;
        
        rosbag2_transport::RecordOptions record_options;
        record_options.rmw_serialization_format = "cdr";
        record_options.all = true;
        record_options.start_paused = false;
        
        std::shared_ptr<rosbag2_cpp::Writer> writer = std::make_shared<rosbag2_cpp::Writer>();
        
        rclcpp::NodeOptions node_options;
        recorder_ = std::make_unique<rosbag2_transport::Recorder>(writer, storage_options, record_options, "rosbag2_recorder", node_options);

        // Start recording
        try {
            recorder_->record();
        } catch (std::exception &e) {
            RCLCPP_ERROR(get_logger(), "Error while starting the rosbag: %s", e.what());
            return false;
        }
        
        return true;
    }

// Member function to stop recording
    void StopRosbag() {
        // Add a delay of 1 second
        std::this_thread::sleep_for(std::chrono::seconds(1));
        
        RCLCPP_INFO(this->get_logger(), "Stopping recorder");
        recorder_->stop();
    }`

private:
    std::unique_ptr<rosbag2_transport::Recorder> recorder_;
};

int main() {
    // Example usage of NodeClass
    NodeClass node;
    node.StartRosbag();
    
    // Add a delay of 1 second
    std::this_thread::sleep_for(std::chrono::seconds(1));

    // Do some other work
    node.StopRosbag();

    return 0;
}

System (please complete the following information)

  • OS: [e.g. Ubuntu Bionic] Ubuntu 21.04
  • ROS 2 Distro: [e.g. Dashing] Humble
  • Install Method: [APT, release archive, source] APT
  • Version: [release, branch, commit hash, patch release number]

Additional context

** Add any other context about the problem here **

I tried hard to put all the code in code mode but unfortunately I was unable to do it, sorry for that

@bernatgaston The reason why it is not recording is because there is no executor in your example that is supposed to process incoming messages from the subscriptions and call callbacks.
To make your example work you will need to add a recorder to the singletreaded executor and run executer's spin() method in a separate thread.
Please refer to rosbag2_py package to see how we do this in our rosbag2 CLI handler

void record(
const rosbag2_storage::StorageOptions & storage_options,
RecordOptions & record_options)
{
auto old_sigterm_handler = std::signal(
SIGTERM, [](int /* signal */) {
rosbag2_py::Recorder::cancel();
});
auto old_sigint_handler = std::signal(
SIGINT, [](int /* signal */) {
rosbag2_py::Recorder::cancel();
});
try {
exit_ = false;
auto exec = std::make_unique<rclcpp::executors::SingleThreadedExecutor>();
if (record_options.rmw_serialization_format.empty()) {
record_options.rmw_serialization_format = std::string(rmw_get_serialization_format());
}
auto writer = rosbag2_transport::ReaderWriterFactory::make_writer(record_options);
auto recorder = std::make_shared<rosbag2_transport::Recorder>(
std::move(writer), storage_options, record_options);
recorder->record();
exec->add_node(recorder);
// Run exec->spin() in a separate thread, because we need to call exec->cancel() after
// recorder->stop() to be able to send notifications about bag split and close.
auto spin_thread = std::thread(
[&exec]() {
exec->spin();
});
{
// Release the GIL for long-running record, so that calling Python code
// can use other threads
py::gil_scoped_release release;
std::unique_lock<std::mutex> lock(wait_for_exit_mutex_);
wait_for_exit_cv_.wait(lock, [] {return rosbag2_py::Recorder::exit_.load();});
recorder->stop();
}
exec->cancel();
if (spin_thread.joinable()) {
spin_thread.join();
}
exec->remove_node(recorder);
} catch (...) {