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

Progress bar for ros2 bag play #1836

Merged
merged 33 commits into from
Feb 24, 2025
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
33 commits
Select commit Hold shift + click to select a range
a3c7c47
Squashed commits of the initial nicolaloi progress bar implementation
morlov-apexai Jan 11, 2025
1258839
Change type of progress_bar_update_rate to int32_t
morlov-apexai Jan 11, 2025
2b75b6f
Remove "in_burst_mode" from play_next() method
morlov-apexai Jan 11, 2025
fa4b220
Move progress bar to a separate PlayerProgressBar class
morlov-apexai Jan 25, 2025
5f13cca
Move PlayerStatus inside PlayerProgressBar
morlov-apexai Jan 26, 2025
fa7b943
Move PlayerProgressBar class to a separate h(c)pp files
morlov-apexai Jan 26, 2025
c6b0379
Use Pimpl design pattern for PlayerProgressBar class
morlov-apexai Jan 26, 2025
b526d78
Replace logger and std::cout with reference to the std::ostream
morlov-apexai Jan 26, 2025
8949780
Use std::stringstream and rdbuf() to avoid extra data copy to string
morlov-apexai Jan 26, 2025
ae26cb6
Change type of the `progress_bar_separation_lines` to the `uint32_t`
morlov-apexai Jan 26, 2025
3990386
Simplify logic by using update period in milliseconds
morlov-apexai Jan 26, 2025
cb95bf8
Regenerate pyi stub files
morlov-apexai Jan 26, 2025
6472ce3
Add test_player_progress_bar.cpp with TODOs
morlov-apexai Jan 26, 2025
5d8978e
Add some unit tests
nicolaloi Jan 30, 2025
d804c6c
Cleanup mess with ANSI control codes
morlov-apexai Feb 6, 2025
1d43918
Remove padding before leading seconds in timestamp value
morlov-apexai Feb 6, 2025
3631125
Remove redundant test cases and extra scope parenthesis
morlov-apexai Feb 6, 2025
95e4545
Implement "update_with_limited_rate_with_zero_timestamp" test
morlov-apexai Feb 6, 2025
74db33a
Sweep cleanup in test_player_progress_bar.cpp
morlov-apexai Feb 6, 2025
a6d02d2
Use get_message_order_timestamp(..) with update_with_limited_rate(..)
morlov-apexai Feb 7, 2025
4d3180c
Disable progress bar by default on rosbag2_transport layer
morlov-apexai Feb 19, 2025
9554b72
Add line cleanup ANSI codes before printing progress bar info
morlov-apexai Feb 19, 2025
9a0f4f8
Adjust API to properly handle progress bar update with play_next() call
morlov-apexai Feb 19, 2025
5e3cabe
Use std::atomic for current_player_status_
morlov-apexai Feb 19, 2025
47c1cf0
Remove line clear ANSI code from separation lines
morlov-apexai Feb 19, 2025
858da22
update README and docs
nicolaloi Feb 21, 2025
dda2c4e
Revert "Remove line clear ANSI code from separation lines"
MichaelOrlov Feb 23, 2025
1d18d09
Correct wording in README.md and `ros2 bag play` options
MichaelOrlov Feb 23, 2025
deed06a
Rename progress-bar CLI option to the progress-bar-update-rate
MichaelOrlov Feb 23, 2025
ec5dbfd
Rename progress-bar CLI option to the progress-bar-update-rate
MichaelOrlov Feb 23, 2025
3505215
Add progress bar options to the get_play_options_from_node_params parser
MichaelOrlov Feb 23, 2025
6839d06
Address test failures on Windows
MichaelOrlov Feb 24, 2025
30976c8
Enable progress bar by default for composable player node
MichaelOrlov Feb 24, 2025
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
14 changes: 12 additions & 2 deletions README.md
Original file line number Diff line number Diff line change
Expand Up @@ -135,16 +135,19 @@ When you have a recorded bag, you can use Rosbag2 to play it back:
$ ros2 bag play <bag>
```

The bag argument can be a directory containing `metadata.yaml` and one or more storage files, or to a single storage file such as `.mcap` or `.db3`.
The bag argument can be a directory containing `metadata.yaml` and one or more storage files, or
to a single storage file such as `.mcap` or `.db3`.
The Player will automatically detect which storage implementation to use for playing.
A progress bar to track the playback progress will be displayed in the terminal by default.

To play back multiple bags:

```
$ ros2 bag play <bag1> -i <bag2> -i <bag3>
```

Messages from all provided bags will be played in order, based on their original recording reception timestamps.
Messages from all provided bags will be played in order, based on their original recording
reception timestamps.

Options:

Expand All @@ -164,6 +167,13 @@ Options:
The reference to use for bag message chronological ordering.
Choices: reception timestamp (`received`), publication timestamp (`sent`).
Default: reception timestamp.
* `--progress-bar-update-rate [Hz]`:
Print a progress bar for the playback with a specified maximum update rate in times per second
(Hz). Negative values mark an update for every published message, while a zero value disables
the progress bar. Default is 3 Hz.
* `--progress-bar-separation-lines`:
The number of lines to separate the progress bar from the rest of the playback player output.
It prevents mixing external log messages with the progress bar string. Default to 2.

For more options, run with `--help`.

Expand Down
21 changes: 21 additions & 0 deletions ros2bag/ros2bag/verb/play.py
Original file line number Diff line number Diff line change
Expand Up @@ -181,6 +181,24 @@ def add_arguments(self, parser, cli_name): # noqa: D102
'--log-level', type=str, default='info',
choices=['debug', 'info', 'warn', 'error', 'fatal'],
help='Logging level.')
progress_bar_group = parser.add_argument_group('Progress bar', 'Settings for progress bar')
progress_bar_group.add_argument(
'--progress-bar-update-rate', type=int, metavar='Hz', default=3,
help='Print a progress bar for the playback with a specified maximum update rate in '
'times per second (Hz). '
'Default is %(default)d Hz. '
'Negative values mark an update for every published message, while '
' a zero value disables the progress bar.')
progress_bar_group.add_argument(
'--progress-bar-separation-lines', type=check_not_negative_int, default=2,
metavar='NUM_SEPARATION_LINES',
help=' Then number of lines to separate the progress bar from the rest of the '
'playback player output. Negative values are invalid. Default is %(default)d. '
'This option makes sense only if the progress bar is enabled. '
'Large values are useful if external log messages from other nodes are shorter '
'than the progress bar string and are printed at a rate higher than the '
'progress bar update rate. In these cases, a larger value will avoid '
'the external log message to be mixed with the progress bar string.')

def get_playback_until_from_arg_group(self, playback_until_sec, playback_until_nsec) -> int:
nano_scale = 1000 * 1000 * 1000
Expand Down Expand Up @@ -292,6 +310,9 @@ def main(self, *, args): # noqa: D102
'sent': MessageOrder.SENT_TIMESTAMP,
}.get(args.message_order)

play_options.progress_bar_update_rate = args.progress_bar_update_rate
play_options.progress_bar_separation_lines = args.progress_bar_separation_lines

player = Player(args.log_level)
try:
player.play(storage_options, play_options)
Expand Down
2 changes: 2 additions & 0 deletions rosbag2_py/rosbag2_py/_transport.pyi
Original file line number Diff line number Diff line change
Expand Up @@ -34,6 +34,8 @@ class PlayOptions:
node_prefix: str
playback_duration: float
playback_until_timestamp: int
progress_bar_separation_lines: int
progress_bar_update_rate: int
publish_service_requests: bool
rate: float
read_ahead_queue_size: int
Expand Down
2 changes: 2 additions & 0 deletions rosbag2_py/src/rosbag2_py/_transport.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -556,6 +556,8 @@ PYBIND11_MODULE(_transport, m) {
"playback_until_timestamp",
&PlayOptions::getPlaybackUntilTimestamp,
&PlayOptions::setPlaybackUntilTimestamp)
.def_readwrite("progress_bar_update_rate", &PlayOptions::progress_bar_update_rate)
.def_readwrite("progress_bar_separation_lines", &PlayOptions::progress_bar_separation_lines)
.def_readwrite("wait_acked_timeout", &PlayOptions::wait_acked_timeout)
.def_readwrite("disable_loan_message", &PlayOptions::disable_loan_message)
.def_readwrite("publish_service_requests", &PlayOptions::publish_service_requests)
Expand Down
4 changes: 4 additions & 0 deletions rosbag2_transport/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -46,6 +46,7 @@ add_library(${PROJECT_NAME} SHARED
src/rosbag2_transport/bag_rewrite.cpp
src/rosbag2_transport/player.cpp
src/rosbag2_transport/play_options.cpp
src/rosbag2_transport/player_progress_bar.cpp
src/rosbag2_transport/player_service_client.cpp
src/rosbag2_transport/reader_writer_factory.cpp
src/rosbag2_transport/recorder.cpp
Expand Down Expand Up @@ -534,6 +535,9 @@ if(BUILD_TESTING)
rcpputils::rcpputils
rosbag2_test_common::rosbag2_test_common
)

ament_add_gmock(test_player_progress_bar test/rosbag2_transport/test_player_progress_bar.cpp)
target_link_libraries(test_player_progress_bar ${PROJECT_NAME})
endif()

ament_package()
9 changes: 9 additions & 0 deletions rosbag2_transport/include/rosbag2_transport/play_options.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -137,6 +137,15 @@ struct PlayOptions
// replayed messages may not be correctly ordered. A possible solution could be to increase the
// read_ahead_queue_size value to buffer (and order) more messages.
MessageOrder message_order = MessageOrder::RECEIVED_TIMESTAMP;

/// The progress bar maximum update rate in times per second (Hz).
/// If update rate equal 0 the progress bar will be disabled and will not output any information.
/// If update rate less than 0 the progress bar will be updated for every update(..) and
/// update_with_limited_rate(..) calls.
int32_t progress_bar_update_rate = 0;

// Number of separation lines to print in between the playback output and the progress bar.
uint32_t progress_bar_separation_lines = 3;
};

} // namespace rosbag2_transport
Expand Down
Original file line number Diff line number Diff line change
@@ -0,0 +1,96 @@
// Copyright 2025 Open Source Robotics Foundation, Inc.
//
// 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 ROSBAG2_TRANSPORT__PLAYER_PROGRESS_BAR_HPP_
#define ROSBAG2_TRANSPORT__PLAYER_PROGRESS_BAR_HPP_

#include <memory>
#include <string>

#include "rcutils/time.h"
#include "rosbag2_transport/visibility_control.hpp"

namespace rosbag2_transport
{
class PlayerProgressBarImpl;

class ROSBAG2_TRANSPORT_PUBLIC PlayerProgressBar
{
public:
enum class PlayerStatus : char
{
BURST = 'B',
DELAYED = 'D',
PAUSED = 'P',
RUNNING = 'R',
STOPPED = 'S',
};

/// PlayerProgressBar constructor
/// \param output_stream Reference to output stream where progress bar and help information
/// will be printed out. Could be std::cout, std::cerr or std::ostringstream for tests.
/// \param starting_time Time stamp of the first message in the bag.
/// \param ending_time Time stamp of the last message in the bag.
/// \param progress_bar_update_rate The progress bar maximum update rate in times per second (Hz).
/// If update rate equal 0 the progress bar will be disabled and will not output any information.
/// If update rate less than 0 the progress bar will be updated for every update(..) and
/// update_with_limited_rate(..) calls.
/// \param progress_bar_separation_lines Number of separation lines to print in between the
/// playback output and the progress bar.
explicit PlayerProgressBar(
std::ostream & output_stream,
rcutils_time_point_value_t starting_time,
rcutils_time_point_value_t ending_time,
int32_t progress_bar_update_rate = 0,
uint32_t progress_bar_separation_lines = 3);

virtual ~PlayerProgressBar();

/// \brief Prints help string about player progress bar.
/// Expected to be printed once at the beginning.
void print_help_str() const;

/// \brief Updates progress bar with the specified timestamp and player status, taking into
/// account the update rate set by the user.
/// \note The function should be used for regular progress bar updates, for example
/// after the publishing the next message.
/// \warning This function is not thread safe and shall not be called concurrently from multiple
/// threads.
/// \param status The player status to be updated on progress bar.
/// \param timestamp Timestamp of the last published message.
void update_with_limited_rate(
const PlayerStatus & status,
const rcutils_time_point_value_t & timestamp);

/// \brief Updates progress bar with the specified player status, irrespective to the update rate
/// set by the user.
/// \note The function should be called for extraordinary progress bar updates, for example
/// when player changed its internal status or a log message is printed, and we want to 'redraw'
/// the progress bar.
/// \param status The player status to be updated on progress bar.
/// \param timestamp Timestamp of the last published message. If timestamp is less than 0 the
/// last published timestamp will be taken from the inner progress bar cache.
void update(const PlayerStatus & status, const rcutils_time_point_value_t & timestamp = -1);

/// \brief Getter function for current player status.
/// \return Current player status.
PlayerStatus get_player_status();

private:
std::unique_ptr<PlayerProgressBarImpl> pimpl_;
};

} // namespace rosbag2_transport

#endif // ROSBAG2_TRANSPORT__PLAYER_PROGRESS_BAR_HPP_
Original file line number Diff line number Diff line change
Expand Up @@ -252,6 +252,13 @@ PlayOptions get_play_options_from_node_params(rclcpp::Node & node)
message_order.c_str());
}

play_options.progress_bar_update_rate = param_utils::declare_integer_node_params<int32_t>(
node, "play.progress_bar_update_rate", std::numeric_limits<int32_t>::min(),
std::numeric_limits<int32_t>::max(), 3);

play_options.progress_bar_separation_lines = param_utils::declare_integer_node_params<uint32_t>(
node, "play.progress_bar_separation_lines", 0, std::numeric_limits<uint32_t>::max(), 2);

return play_options;
}

Expand Down
9 changes: 9 additions & 0 deletions rosbag2_transport/src/rosbag2_transport/play_options.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -59,6 +59,9 @@ Node convert<rosbag2_transport::PlayOptions>::encode(

node["disable_loan_message"] = play_options.disable_loan_message;

node["progress_bar_update_rate"] = play_options.progress_bar_update_rate;
node["progress_bar_separation_lines"] = play_options.progress_bar_separation_lines;

return node;
}

Expand Down Expand Up @@ -114,6 +117,12 @@ bool convert<rosbag2_transport::PlayOptions>::decode(

optional_assign<bool>(node, "disable_loan_message", play_options.disable_loan_message);

optional_assign<int32_t>(
node, "progress_bar_update_rate", play_options.progress_bar_update_rate);

optional_assign<uint32_t>(
node, "progress_bar_separation_lines", play_options.progress_bar_separation_lines);

return true;
}

Expand Down
Loading
Loading