Skip to content

Commit 9ed6efc

Browse files
committed
feat: add qos handler in pub/sub
Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com>
1 parent aebbcc5 commit 9ed6efc

File tree

1 file changed

+30
-12
lines changed

1 file changed

+30
-12
lines changed

common/autoware_test_utils/include/autoware_test_utils/autoware_test_utils.hpp

+30-12
Original file line numberDiff line numberDiff line change
@@ -51,6 +51,7 @@
5151
#include <string>
5252
#include <unordered_map>
5353
#include <vector>
54+
#include <optional>
5455

5556
namespace autoware::test_utils
5657
{
@@ -402,6 +403,7 @@ void createPublisherWithQoS(
402403
rclcpp::Node::SharedPtr test_node, std::string topic_name,
403404
std::shared_ptr<rclcpp::Publisher<T>> & publisher)
404405
{
406+
// override QoS settings for specific message types
405407
if constexpr (
406408
std::is_same_v<T, LaneletRoute> || std::is_same_v<T, LaneletMapBin> ||
407409
std::is_same_v<T, OperationModeState>) {
@@ -410,7 +412,7 @@ void createPublisherWithQoS(
410412
qos.transient_local();
411413
publisher = rclcpp::create_publisher<T>(test_node, topic_name, qos);
412414
} else {
413-
publisher = rclcpp::create_publisher<T>(test_node, topic_name, 1);
415+
// publisher is already created. Do nothing
414416
}
415417
}
416418

@@ -443,18 +445,23 @@ void setPublisher(
443445
* @param topic_name The name of the topic to subscribe to.
444446
* @param callback The callback function to call when a message is received.
445447
* @param subscriber A reference to the subscription to be created.
448+
* @param qos The QoS settings for the subscription (optional).
446449
*/
447450
template <typename T>
448451
void createSubscription(
449-
rclcpp::Node::SharedPtr test_node, std::string topic_name,
452+
rclcpp::Node::SharedPtr test_node, const std::string & topic_name,
450453
std::function<void(const typename T::ConstSharedPtr)> callback,
451-
std::shared_ptr<rclcpp::Subscription<T>> & subscriber)
454+
std::shared_ptr<rclcpp::Subscription<T>> & subscriber,
455+
std::optional<rclcpp::QoS> qos = std::nullopt)
452456
{
453-
if constexpr (std::is_same_v<T, Trajectory>) {
454-
subscriber = test_node->create_subscription<T>(topic_name, rclcpp::QoS{1}, callback);
455-
} else {
456-
subscriber = test_node->create_subscription<T>(topic_name, 10, callback);
457+
if (!qos.has_value()) {
458+
if constexpr (std::is_same_v<T, Trajectory>) {
459+
qos = rclcpp::QoS{1};
460+
} else {
461+
qos = rclcpp::QoS{10};
462+
}
457463
}
464+
subscriber = test_node->create_subscription<T>(topic_name, *qos, callback);
458465
}
459466

460467
/**
@@ -467,14 +474,16 @@ void createSubscription(
467474
* @param topic_name The name of the topic to subscribe to.
468475
* @param subscriber A reference to the subscription to be set.
469476
* @param count A reference to a counter that increments on message receipt.
477+
* @param qos The QoS settings for the subscription (optional).
470478
*/
471479
template <typename T>
472480
void setSubscriber(
473481
rclcpp::Node::SharedPtr test_node, std::string topic_name,
474-
std::shared_ptr<rclcpp::Subscription<T>> & subscriber, size_t & count)
482+
std::shared_ptr<rclcpp::Subscription<T>> & subscriber, size_t & count,
483+
std::optional<rclcpp::QoS> qos = std::nullopt)
475484
{
476485
createSubscription(
477-
test_node, topic_name, [&count](const typename T::ConstSharedPtr) { count++; }, subscriber);
486+
test_node, topic_name, [&count](const typename T::ConstSharedPtr) { count++; }, subscriber, qos);
478487
}
479488

480489
/**
@@ -534,9 +543,18 @@ class AutowareTestManager
534543
template <typename MessageType>
535544
void test_pub_msg(
536545
rclcpp::Node::SharedPtr target_node, const std::string & topic_name, MessageType & msg)
546+
{
547+
rclcpp::QoS qos(rclcpp::KeepLast(10));
548+
test_pub_msg(target_node, topic_name, msg, qos);
549+
}
550+
551+
template <typename MessageType>
552+
void test_pub_msg(
553+
rclcpp::Node::SharedPtr target_node, const std::string & topic_name, MessageType & msg,
554+
rclcpp::QoS qos)
537555
{
538556
if (publishers_.find(topic_name) == publishers_.end()) {
539-
auto publisher = test_node_->create_publisher<MessageType>(topic_name, 10);
557+
auto publisher = test_node_->create_publisher<MessageType>(topic_name, qos);
540558
publishers_[topic_name] = std::static_pointer_cast<rclcpp::PublisherBase>(publisher);
541559
}
542560

@@ -550,12 +568,12 @@ class AutowareTestManager
550568
template <typename MessageType>
551569
void set_subscriber(
552570
const std::string & topic_name,
553-
std::function<void(const typename MessageType::ConstSharedPtr)> callback)
571+
std::function<void(const typename MessageType::ConstSharedPtr)> callback, std::optional<rclcpp::QoS> qos = std::nullopt)
554572
{
555573
if (subscribers_.find(topic_name) == subscribers_.end()) {
556574
std::shared_ptr<rclcpp::Subscription<MessageType>> subscriber;
557575
autoware::test_utils::createSubscription<MessageType>(
558-
test_node_, topic_name, callback, subscriber);
576+
test_node_, topic_name, callback, subscriber, qos);
559577
subscribers_[topic_name] = std::static_pointer_cast<rclcpp::SubscriptionBase>(subscriber);
560578
} else {
561579
RCLCPP_WARN(test_node_->get_logger(), "Subscriber %s already set.", topic_name.c_str());

0 commit comments

Comments
 (0)