Skip to content

Commit 775db2f

Browse files
committed
feat(tier4_autoware_utils): default QoS setting of polling subscriber
Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>
1 parent fd89ca2 commit 775db2f

File tree

1 file changed

+15
-3
lines changed

1 file changed

+15
-3
lines changed

common/tier4_autoware_utils/include/tier4_autoware_utils/ros/polling_subscriber.hpp

+15-3
Original file line numberDiff line numberDiff line change
@@ -30,24 +30,36 @@ class InterProcessPollingSubscriber
3030
std::optional<T> data_;
3131

3232
public:
33-
explicit InterProcessPollingSubscriber(rclcpp::Node * node, const std::string & topic_name)
33+
explicit InterProcessPollingSubscriber(
34+
rclcpp::Node * node, const std::string & topic_name, const rclcpp::QoS & qos = rclcpp::QoS{1})
3435
{
3536
auto noexec_callback_group =
3637
node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive, false);
3738
auto noexec_subscription_options = rclcpp::SubscriptionOptions();
3839
noexec_subscription_options.callback_group = noexec_callback_group;
3940

4041
subscriber_ = node->create_subscription<T>(
41-
topic_name, rclcpp::QoS{1},
42+
topic_name, qos,
4243
[node]([[maybe_unused]] const typename T::ConstSharedPtr msg) { assert(false); },
4344
noexec_subscription_options);
45+
if (qos.get_rmw_qos_profile().depth > 1) {
46+
RCLCPP_WARN(
47+
node->get_logger(),
48+
"InterProcessPollingSubscriber will be used with depth > 1, which may cause inefficient "
49+
"serialization while updateLatestData()");
50+
}
4451
};
4552
bool updateLatestData()
4653
{
4754
rclcpp::MessageInfo message_info;
4855
T tmp;
4956
// The queue size (QoS) must be 1 to get the last message data.
50-
if (subscriber_->take(tmp, message_info)) {
57+
bool is_latest_message_consumed = false;
58+
// pop the queue until latest data
59+
while (subscriber_->take(tmp, message_info)) {
60+
is_latest_message_consumed = true;
61+
}
62+
if (is_latest_message_consumed) {
5163
data_ = tmp;
5264
}
5365
return data_.has_value();

0 commit comments

Comments
 (0)