Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Add --snapshot-mode argument to the "record" verb #851

Merged
merged 3 commits into from
Sep 25, 2021
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
20 changes: 11 additions & 9 deletions docs/design/rosbag2_snapshot_mode.md
Original file line number Diff line number Diff line change
Expand Up @@ -18,7 +18,7 @@ $ ros2 bag record --max-cache-size 100000 --snapshot-mode [topics [topics ...]]

Triggering a snapshot via CLI:
```
$ ros2 service call ~/snapshot rosbag2_interfaces/TakeSnapshot
$ ros2 service call /rosbag2_recorder/snapshot rosbag2_interfaces/Snapshot
```

Triggering a snapshot with a keyboard shortcut:
Expand All @@ -29,22 +29,24 @@ Triggering a snapshot with a keyboard shortcut:

* Add a `--snapshot-mode` flag to the `record` verb in `ros2bag` that will be passed via `StorageOptions`.

* Create a new class `CircularBufferMessageCache` which maintains two circular buffers sized according to the existing `—max-cache-size` parameter. During snapshot mode, this class will keep adding messages to the same circular buffer until triggered to switch to the secondary buffer by the `Recorder` class.
* Create a new class `CircularMessageCache` which maintains two circular buffers, individally implemented in a class `MessageCacheCircularBuffer` and sized according to the existing `—max-cache-size` parameter. During snapshot mode, `CircularMessageCache` will keep adding messages to the same circular buffer until triggered to switch to the secondary buffer by the `Recorder` class.

* Create a service `~/snapshot` in the `rosbag2_recorder` node that will listen for snapshot requests. Upon receiving a service call, it will trigger a buffer flip in `CircularBufferMessageCache` and call the new `take_snapeshot` function in `SequentialWriter` (detailed below) to write the snapshot messages to storage.
* Create a service `~/snapshot` in the `rosbag2_recorder` node that will listen for snapshot requests. Upon receiving a service call, it will trigger a buffer flip in `CircularMessageCache` and call the new `take_snapeshot` function in `SequentialWriter` (detailed below) to write the snapshot messages to storage.

* Modify `SequentialWriter` to use `CircularBufferMessageCache` when snapshot mode is enabled instead of `MessageCache` and `CacheConsumer`.
* Modify `SequentialWriter` to use `CircularMessageCache` when snapshot mode is enabled instead of `MessageCache` and `CacheConsumer`.

* Add a new `take_snapshot` function to `SequentialWriter` that will be invoked by the `rosbag2_recorder` node's snapshot service to write the snapshot data to the rosbag, close the rosbag, and open a new rosbag for a future snapshot.
* Add a new `take_snapshot` function to `SequentialWriter` that will be invoked by the `rosbag2_recorder` node's snapshot service to write the snapshot data to the rosbag.

* Implement the `~/snapshot` service interface as `TakeSnapshot.srv` in `rosbag2_interfaces`. It won’t require any arguments (similar to `Resume.srv`).
* Modify `BaseWriterInterface` to include a pure virutal function `take_snapshot` to ensure that all future writers are compatible with snapshot mode.

* Implement the `~/snapshot` service interface as `Snapshot.srv` in `rosbag2_interfaces`. It won’t require any arguments (similar to `Resume.srv`).


## Implementation Breakdown
The snapshot feature implementation could be broken down into the following PRs:

* Implement `CircularBufferMessageCache` and add corresponding tests.
* Integrate `CircularBufferMessageCache` into `SequentialWriter` and update its tests.
* Create the `~/snapshot` service in the `Recorder` class, add corresponding tests, and create the `TakeSnapshot.srv` service interface.
* Implement `CircularMessageCache` and `MessageCacheCircularBuffer`. Then add corresponding tests.
* Integrate `CircularMessageCache` into `SequentialWriter` and update its tests.
* Create the `~/snapshot` service in the `Recorder` class, add corresponding tests, and create the `Snapshot.srv` service interface.
* Add `--snapshot-mode` to the `record` verb and update `StorageOptions`
* Add keyboard shortcut for triggering a snapshot in snapshot mode
6 changes: 6 additions & 0 deletions ros2bag/ros2bag/verb/record.py
Original file line number Diff line number Diff line change
Expand Up @@ -114,6 +114,11 @@ def add_arguments(self, parser, cli_name): # noqa: D102
help='Number of files or messages that may be compressed in parallel. '
'Default is 0, which will be interpreted as the number of CPU cores.'
)
parser.add_argument(
'--snapshot-mode', action='store_true',
help='Enable snapshot mode. Messages will not be written to the bagfile until '
'the "/rosbag2_recorder/snapshot" service is called.'
)
parser.add_argument(
'--include-hidden-topics', action='store_true',
help='record also hidden topics.'
Expand Down Expand Up @@ -190,6 +195,7 @@ def main(self, *, args): # noqa: D102
max_cache_size=args.max_cache_size,
storage_preset_profile=args.storage_preset_profile,
storage_config_uri=storage_config_file,
snapshot_mode=args.snapshot_mode
)
record_options = RecordOptions()
record_options.all = args.all
Expand Down
10 changes: 7 additions & 3 deletions rosbag2_py/src/rosbag2_py/_storage.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -43,14 +43,15 @@ PYBIND11_MODULE(_storage, m) {
pybind11::class_<rosbag2_storage::StorageOptions>(m, "StorageOptions")
.def(
pybind11::init<
std::string, std::string, uint64_t, uint64_t, uint64_t, std::string, std::string>(),
std::string, std::string, uint64_t, uint64_t, uint64_t, std::string, std::string, bool>(),
pybind11::arg("uri"),
pybind11::arg("storage_id"),
pybind11::arg("max_bagfile_size") = 0,
pybind11::arg("max_bagfile_duration") = 0,
pybind11::arg("max_cache_size") = 0,
pybind11::arg("storage_preset_profile") = "",
pybind11::arg("storage_config_uri") = "")
pybind11::arg("storage_config_uri") = "",
pybind11::arg("snapshot_mode") = false)
.def_readwrite("uri", &rosbag2_storage::StorageOptions::uri)
.def_readwrite("storage_id", &rosbag2_storage::StorageOptions::storage_id)
.def_readwrite(
Expand All @@ -67,7 +68,10 @@ PYBIND11_MODULE(_storage, m) {
&rosbag2_storage::StorageOptions::storage_preset_profile)
.def_readwrite(
"storage_config_uri",
&rosbag2_storage::StorageOptions::storage_config_uri);
&rosbag2_storage::StorageOptions::storage_config_uri)
.def_readwrite(
"snapshot_mode",
&rosbag2_storage::StorageOptions::snapshot_mode);

pybind11::class_<rosbag2_storage::StorageFilter>(m, "StorageFilter")
.def(
Expand Down