51
51
#include < string>
52
52
#include < unordered_map>
53
53
#include < vector>
54
+ #include < optional>
54
55
55
56
namespace autoware ::test_utils
56
57
{
@@ -402,6 +403,7 @@ void createPublisherWithQoS(
402
403
rclcpp::Node::SharedPtr test_node, std::string topic_name,
403
404
std::shared_ptr<rclcpp::Publisher<T>> & publisher)
404
405
{
406
+ // override QoS settings for specific message types
405
407
if constexpr (
406
408
std::is_same_v<T, LaneletRoute> || std::is_same_v<T, LaneletMapBin> ||
407
409
std::is_same_v<T, OperationModeState>) {
@@ -410,7 +412,7 @@ void createPublisherWithQoS(
410
412
qos.transient_local ();
411
413
publisher = rclcpp::create_publisher<T>(test_node, topic_name, qos);
412
414
} else {
413
- publisher = rclcpp::create_publisher<T>(test_node, topic_name, 1 );
415
+ // publisher is already created. Do nothing
414
416
}
415
417
}
416
418
@@ -443,18 +445,23 @@ void setPublisher(
443
445
* @param topic_name The name of the topic to subscribe to.
444
446
* @param callback The callback function to call when a message is received.
445
447
* @param subscriber A reference to the subscription to be created.
448
+ * @param qos The QoS settings for the subscription (optional).
446
449
*/
447
450
template <typename T>
448
451
void createSubscription (
449
- rclcpp::Node::SharedPtr test_node, std::string topic_name,
452
+ rclcpp::Node::SharedPtr test_node, const std::string & topic_name,
450
453
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)
452
456
{
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
+ }
457
463
}
464
+ subscriber = test_node->create_subscription <T>(topic_name, *qos, callback);
458
465
}
459
466
460
467
/* *
@@ -467,14 +474,16 @@ void createSubscription(
467
474
* @param topic_name The name of the topic to subscribe to.
468
475
* @param subscriber A reference to the subscription to be set.
469
476
* @param count A reference to a counter that increments on message receipt.
477
+ * @param qos The QoS settings for the subscription (optional).
470
478
*/
471
479
template <typename T>
472
480
void setSubscriber (
473
481
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)
475
484
{
476
485
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 );
478
487
}
479
488
480
489
/* *
@@ -534,9 +543,18 @@ class AutowareTestManager
534
543
template <typename MessageType>
535
544
void test_pub_msg (
536
545
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)
537
555
{
538
556
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 );
540
558
publishers_[topic_name] = std::static_pointer_cast<rclcpp::PublisherBase>(publisher);
541
559
}
542
560
@@ -550,12 +568,12 @@ class AutowareTestManager
550
568
template <typename MessageType>
551
569
void set_subscriber (
552
570
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 )
554
572
{
555
573
if (subscribers_.find (topic_name) == subscribers_.end ()) {
556
574
std::shared_ptr<rclcpp::Subscription<MessageType>> subscriber;
557
575
autoware::test_utils::createSubscription<MessageType>(
558
- test_node_, topic_name, callback, subscriber);
576
+ test_node_, topic_name, callback, subscriber, qos );
559
577
subscribers_[topic_name] = std::static_pointer_cast<rclcpp::SubscriptionBase>(subscriber);
560
578
} else {
561
579
RCLCPP_WARN (test_node_->get_logger (), " Subscriber %s already set." , topic_name.c_str ());
0 commit comments