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

Update PlayOptions::delay to rclcpp::Duration to get nanosecond resolution #843

Merged
merged 4 commits into from
Aug 13, 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
4 changes: 2 additions & 2 deletions ros2bag/ros2bag/verb/play.py
Original file line number Diff line number Diff line change
Expand Up @@ -83,8 +83,8 @@ def add_arguments(self, parser, cli_name): # noqa: D102
help='Publish to /clock at a specific frequency in Hz, to act as a ROS Time Source. '
'Value must be positive. Defaults to not publishing.')
parser.add_argument(
'-d', '--delay', type=float, default=0.0,
help='Sleep SEC seconds before play. Valid range > 0.0')
'-d', '--delay', type=positive_float, default=0.0,
help='Sleep duration before play (each loop), in seconds. Negative durations invalid.')

def main(self, *, args): # noqa: D102
qos_profile_overrides = {} # Specify a valid default
Expand Down
21 changes: 17 additions & 4 deletions rosbag2_py/src/rosbag2_py/_transport.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -67,14 +67,24 @@ template<class T>
struct OptionsWrapper : public T
{
public:
void setTopicQoSProfileOverrides(
const py::dict & overrides)
void setDelay(double delay)
{
this->delay = rclcpp::Duration::from_nanoseconds(
static_cast<rcl_duration_value_t>(RCUTILS_S_TO_NS(delay)));
}

double getDelay() const
{
return RCUTILS_NS_TO_S(static_cast<double>(this->delay.nanoseconds()));
}

void setTopicQoSProfileOverrides(const py::dict & overrides)
{
py_dict = overrides;
this->topic_qos_profile_overrides = qos_map_from_py_dict(overrides);
}

const py::dict & getTopicQoSProfileOverrides()
const py::dict & getTopicQoSProfileOverrides() const
{
return py_dict;
}
Expand Down Expand Up @@ -225,7 +235,10 @@ PYBIND11_MODULE(_transport, m) {
.def_readwrite("loop", &PlayOptions::loop)
.def_readwrite("topic_remapping_options", &PlayOptions::topic_remapping_options)
.def_readwrite("clock_publish_frequency", &PlayOptions::clock_publish_frequency)
.def_readwrite("delay", &PlayOptions::delay)
.def_property(
"delay",
&PlayOptions::getDelay,
&PlayOptions::setDelay)
;

py::class_<RecordOptions>(m, "RecordOptions")
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -20,6 +20,7 @@
#include <unordered_map>
#include <vector>

#include "rclcpp/duration.hpp"
#include "rclcpp/qos.hpp"

namespace rosbag2_transport
Expand All @@ -45,8 +46,8 @@ struct PlayOptions
// 0 (or negative) means that no publisher will be created
double clock_publish_frequency = 0.0;

// Sleep SEC seconds before play. Valid range > 0.0.
float delay = 0.0;
// Sleep before play. Negative durations invalid. Will delay at the beginning of each loop.
rclcpp::Duration delay = rclcpp::Duration(0, 0);
};

} // namespace rosbag2_transport
Expand Down
16 changes: 8 additions & 8 deletions rosbag2_transport/src/rosbag2_transport/player.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -212,20 +212,20 @@ bool Player::is_storage_completely_loaded() const

void Player::play()
{
float delay;
if (play_options_.delay >= 0.0) {
rclcpp::Duration delay(0, 0);
if (play_options_.delay >= rclcpp::Duration(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_WARN_STREAM(
this->get_logger(),
"Invalid delay value: " << play_options_.delay.nanoseconds() << ". Delay is disabled.");
}

try {
do {
if (delay > 0.0) {
RCLCPP_INFO_STREAM(this->get_logger(), "Sleep " << delay << " sec");
std::chrono::duration<float> duration(delay);
if (delay > rclcpp::Duration(0, 0)) {
RCLCPP_INFO_STREAM(this->get_logger(), "Sleep " << delay.nanoseconds() << " ns");
std::chrono::nanoseconds duration(delay.nanoseconds());
std::this_thread::sleep_for(duration);
}
reader_->open(storage_options_, {"", rmw_get_serialization_format()});
Expand Down
4 changes: 2 additions & 2 deletions rosbag2_transport/test/rosbag2_transport/test_play_loop.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -44,7 +44,7 @@ TEST_F(RosBag2PlayTestFixture, play_bag_file_twice) {
const float rate = 1.0;
const bool loop_playback = false;
double clock_publish_frequency = 0.0;
const float delay = 1.0;
const rclcpp::Duration delay(1, 0);

auto primitive_message1 = get_messages_basic_types()[0];
primitive_message1->int32_value = test_value;
Expand Down Expand Up @@ -104,7 +104,7 @@ TEST_F(RosBag2PlayTestFixture, messages_played_in_loop) {
const float rate = 1.0;
const bool loop_playback = true;
const double clock_publish_frequency = 0.0;
const float delay = 1.0;
const rclcpp::Duration delay(1, 0);

auto primitive_message1 = get_messages_basic_types()[0];
primitive_message1->int32_value = test_value;
Expand Down
32 changes: 17 additions & 15 deletions rosbag2_transport/test/rosbag2_transport/test_play_timing.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -194,24 +194,23 @@ TEST_F(PlayerTestFixture, playing_respects_delay)
auto primitive_message2 = get_messages_strings()[0];
primitive_message2->string_value = "Hello World 2";

auto message_time_difference = std::chrono::seconds(1);
auto message_time_difference = rclcpp::Duration(1, 0);
auto topics_and_types =
std::vector<rosbag2_storage::TopicMetadata>{{"topic1", "test_msgs/Strings", "", ""}};
std::vector<std::shared_ptr<rosbag2_storage::SerializedBagMessage>> messages =
{serialize_test_message("topic1", 0, primitive_message1),
serialize_test_message("topic1", 0, primitive_message2)};

messages[0]->time_stamp = 100;
messages[1]->time_stamp =
messages[0]->time_stamp + std::chrono::nanoseconds(message_time_difference).count();
messages[1]->time_stamp = messages[0]->time_stamp + message_time_difference.nanoseconds();

float delay_margin = 1.0;
rclcpp::Duration delay_margin(1, 0);

// Sleep 5.0 seconds before play
{
play_options_.delay = 5.0;
std::chrono::duration<float> delay(play_options_.delay);
std::chrono::duration<float> delay_uppper(play_options_.delay + delay_margin);
play_options_.delay = rclcpp::Duration(5, 0);
auto lower_expected_duration = message_time_difference + play_options_.delay;
auto upper_expected_duration = message_time_difference + play_options_.delay + delay_margin;

auto prepared_mock_reader = std::make_unique<MockSequentialReader>();
prepared_mock_reader->prepare(messages, topics_and_types);
Expand All @@ -220,16 +219,18 @@ TEST_F(PlayerTestFixture, playing_respects_delay)
std::move(reader), storage_options_, play_options_);
auto start = std::chrono::steady_clock::now();
player->play();
auto replay_time = std::chrono::steady_clock::now() - start;

ASSERT_THAT(replay_time, Gt(message_time_difference + delay));
ASSERT_THAT(replay_time, Lt(message_time_difference + delay_uppper));
auto replay_time = std::chrono::steady_clock::now() - start;
auto replay_nanos = std::chrono::nanoseconds(replay_time).count();
EXPECT_THAT(replay_nanos, Gt(lower_expected_duration.nanoseconds()));
EXPECT_THAT(replay_nanos, Lt(upper_expected_duration.nanoseconds()));
}

// Invalid value should result in playing at default delay 0.0
{
play_options_.delay = -5.0;
std::chrono::duration<float> delay_uppper(delay_margin);
play_options_.delay = rclcpp::Duration(-5, 0);
auto lower_expected_duration = message_time_difference;
auto upper_expected_duration = message_time_difference + delay_margin;

auto prepared_mock_reader = std::make_unique<MockSequentialReader>();
prepared_mock_reader->prepare(messages, topics_and_types);
Expand All @@ -238,9 +239,10 @@ TEST_F(PlayerTestFixture, playing_respects_delay)
std::move(reader), storage_options_, play_options_);
auto start = std::chrono::steady_clock::now();
player->play();
auto replay_time = std::chrono::steady_clock::now() - start;

ASSERT_THAT(replay_time, Gt(message_time_difference));
ASSERT_THAT(replay_time, Lt(message_time_difference + delay_uppper));
auto replay_time = std::chrono::steady_clock::now() - start;
auto replay_nanos = std::chrono::nanoseconds(replay_time).count();
EXPECT_THAT(replay_nanos, Gt(lower_expected_duration.nanoseconds()));
EXPECT_THAT(replay_nanos, Lt(upper_expected_duration.nanoseconds()));
}
}