-
Notifications
You must be signed in to change notification settings - Fork 248
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
Make sure published messages are acknowledged for play mode #951
Changes from 3 commits
File filter
Filter by extension
Conversations
Jump to
Diff view
Diff view
There are no files selected for viewing
Original file line number | Diff line number | Diff line change | ||||
---|---|---|---|---|---|---|
|
@@ -229,6 +229,30 @@ void Player::play() | |||||
std::lock_guard<std::mutex> lk(ready_to_play_from_queue_mutex_); | ||||||
is_ready_to_play_from_queue_ = false; | ||||||
ready_to_play_from_queue_cv_.notify_all(); | ||||||
|
||||||
// Wait for all published messages to be acknowledged. | ||||||
if (play_options_.wait_acked_timeout >= 0) { | ||||||
std::chrono::milliseconds timeout(play_options_.wait_acked_timeout); | ||||||
if (timeout == std::chrono::milliseconds(0)) { | ||||||
timeout = std::chrono::milliseconds(-1); | ||||||
} | ||||||
for (auto pub : publishers_) { | ||||||
try { | ||||||
if (!pub.second->wait_for_all_acked(timeout)) { | ||||||
RCLCPP_ERROR( | ||||||
get_logger(), | ||||||
"Timeout to wait all published messages acknowledged for topic %s", | ||||||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. I feel like this reads a little better.
Suggested change
|
||||||
pub.first.c_str()); | ||||||
} | ||||||
} catch (std::exception & e) { | ||||||
RCLCPP_ERROR( | ||||||
get_logger(), | ||||||
"Failed to wait all published messages acknowledged for topic %s : %s", | ||||||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. Another small wordage suggestion.
Suggested change
|
||||||
pub.first.c_str(), | ||||||
e.what()); | ||||||
} | ||||||
} | ||||||
} | ||||||
} | ||||||
|
||||||
void Player::pause() | ||||||
|
@@ -463,6 +487,7 @@ void Player::prepare_publishers() | |||||
|
||||||
// Create topic publishers | ||||||
auto topics = reader_->get_all_topics_and_types(); | ||||||
std::string topic_without_support_acked; | ||||||
for (const auto & topic : topics) { | ||||||
if (publishers_.find(topic.name) != publishers_.end()) { | ||||||
continue; | ||||||
|
@@ -483,6 +508,11 @@ void Player::prepare_publishers() | |||||
publishers_.insert( | ||||||
std::make_pair( | ||||||
topic.name, create_generic_publisher(topic.name, topic.type, topic_qos))); | ||||||
if (play_options_.wait_acked_timeout >= 0 && | ||||||
topic_qos.reliability() == rclcpp::ReliabilityPolicy::BestEffort) | ||||||
{ | ||||||
topic_without_support_acked += topic.name + ", "; | ||||||
} | ||||||
} catch (const std::runtime_error & e) { | ||||||
// using a warning log seems better than adding a new option | ||||||
// to ignore some unknown message type library | ||||||
|
@@ -491,6 +521,18 @@ void Player::prepare_publishers() | |||||
"Ignoring a topic '%s', reason: %s.", topic.name.c_str(), e.what()); | ||||||
} | ||||||
} | ||||||
|
||||||
if (!topic_without_support_acked.empty()) { | ||||||
// remove the last ", " | ||||||
topic_without_support_acked.erase( | ||||||
topic_without_support_acked.end() - 2, | ||||||
topic_without_support_acked.end()); | ||||||
|
||||||
RCLCPP_WARN( | ||||||
get_logger(), | ||||||
"--wait-for-all-acked is invaild for below topics since reliability of QOS is BestEffort.\n" | ||||||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. Very minor change here.
Suggested change
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. Thanks for your comments. |
||||||
"%s", topic_without_support_acked.c_str()); | ||||||
} | ||||||
} | ||||||
|
||||||
bool Player::publish_message(rosbag2_storage::SerializedBagMessageSharedPtr message) | ||||||
|
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
I feel like this flows a little better, though I tend to be a bit wordy.