From d1fc46dceb872674cba71ae8c5b7acf0962697db Mon Sep 17 00:00:00 2001 From: Miaofei Date: Thu, 9 May 2019 15:12:34 -0700 Subject: [PATCH] update to be compatible with latest rclcpp API Signed-off-by: Miaofei --- .../test_quality_of_service/qos_test_node.hpp | 6 +++- .../qos_test_publisher.hpp | 4 ++- .../qos_test_subscriber.hpp | 4 ++- .../test/qos_test_node.cpp | 4 ++- .../test/qos_test_publisher.cpp | 6 ++-- .../test/qos_test_subscriber.cpp | 5 +-- .../test/test_deadline.cpp | 30 +++++++--------- .../test/test_lifespan.cpp | 19 ++++------- .../test/test_liveliness.cpp | 34 ++++++++----------- 9 files changed, 55 insertions(+), 57 deletions(-) diff --git a/test_quality_of_service/include/test_quality_of_service/qos_test_node.hpp b/test_quality_of_service/include/test_quality_of_service/qos_test_node.hpp index ce631629..fc3fbb82 100644 --- a/test_quality_of_service/include/test_quality_of_service/qos_test_node.hpp +++ b/test_quality_of_service/include/test_quality_of_service/qos_test_node.hpp @@ -27,7 +27,8 @@ class QosTestNode : public rclcpp::Node public: QosTestNode( const std::string & name, - const std::string & topic); + const std::string & topic, + const rclcpp::QoS & qos_options); virtual ~QosTestNode(); @@ -57,10 +58,13 @@ class QosTestNode : public rclcpp::Node * \return the incremented count */ int increment_count(); + /// name of this publisher (Node) const std::string name_; /// topic name to publish const std::string topic_; + /// qos options needed for QoS settings + const rclcpp::QoS qos_options_; private: /// simple counter used for this node's measurements diff --git a/test_quality_of_service/include/test_quality_of_service/qos_test_publisher.hpp b/test_quality_of_service/include/test_quality_of_service/qos_test_publisher.hpp index d3af1880..9d689d2d 100644 --- a/test_quality_of_service/include/test_quality_of_service/qos_test_publisher.hpp +++ b/test_quality_of_service/include/test_quality_of_service/qos_test_publisher.hpp @@ -31,6 +31,7 @@ class QosTestPublisher : public QosTestNode QosTestPublisher( const std::string & name, const std::string & topic, + const rclcpp::QoS & qos_options, const rclcpp::PublisherOptions & publisher_options, const std::chrono::milliseconds & publish_period); @@ -46,12 +47,13 @@ class QosTestPublisher : public QosTestNode void setup_start() override; void setup_stop() override; - /// publisher options needed for QoS settings + /// publisher options needed for QoS callbacks const rclcpp::PublisherOptions publisher_options_; /// publishing period const std::chrono::milliseconds publish_period_; /// the timer of this publisher rclcpp::TimerBase::SharedPtr timer_; + rclcpp::Publisher::SharedPtr publisher_; }; diff --git a/test_quality_of_service/include/test_quality_of_service/qos_test_subscriber.hpp b/test_quality_of_service/include/test_quality_of_service/qos_test_subscriber.hpp index 145f4343..8a22503f 100644 --- a/test_quality_of_service/include/test_quality_of_service/qos_test_subscriber.hpp +++ b/test_quality_of_service/include/test_quality_of_service/qos_test_subscriber.hpp @@ -30,6 +30,7 @@ class QosTestSubscriber : public QosTestNode QosTestSubscriber( const std::string & name, const std::string & topic, + const rclcpp::QoS & qos_options, const rclcpp::SubscriptionOptions & sub_options); virtual ~QosTestSubscriber() = default; @@ -45,8 +46,9 @@ class QosTestSubscriber : public QosTestNode void setup_stop() override; private: - /// subscription options needed for QoS settings + /// subscription options needed for QoS callbacks const rclcpp::SubscriptionOptions sub_options_; + rclcpp::Subscription::SharedPtr subscription_; }; diff --git a/test_quality_of_service/test/qos_test_node.cpp b/test_quality_of_service/test/qos_test_node.cpp index 43564043..cad54630 100644 --- a/test_quality_of_service/test/qos_test_node.cpp +++ b/test_quality_of_service/test/qos_test_node.cpp @@ -22,10 +22,12 @@ QosTestNode::QosTestNode( const std::string & name, - const std::string & topic) + const std::string & topic, + const rclcpp::QoS & qos_options) : Node(name), name_(name), topic_(topic), + qos_options_(qos_options), count_(0), started_(false) {} diff --git a/test_quality_of_service/test/qos_test_publisher.cpp b/test_quality_of_service/test/qos_test_publisher.cpp index 5cabf0a0..ec304f73 100644 --- a/test_quality_of_service/test/qos_test_publisher.cpp +++ b/test_quality_of_service/test/qos_test_publisher.cpp @@ -20,15 +20,17 @@ QosTestPublisher::QosTestPublisher( const std::string & name, const std::string & topic, + const rclcpp::QoS & qos_options, const rclcpp::PublisherOptions & publisher_options, const std::chrono::milliseconds & publish_period) -: QosTestNode(name, topic), +: QosTestNode(name, topic, qos_options), publisher_options_(publisher_options), publish_period_(publish_period), timer_(nullptr) { RCLCPP_INFO(this->get_logger(), "created publisher %s %s\n", name.c_str(), topic.c_str()); - publisher_ = this->create_publisher(topic_, 10, publisher_options_); + publisher_ = this->create_publisher( + topic_, qos_options_, publisher_options_); } void QosTestPublisher::publish_message() diff --git a/test_quality_of_service/test/qos_test_subscriber.cpp b/test_quality_of_service/test/qos_test_subscriber.cpp index 70381d8c..3d212bd3 100644 --- a/test_quality_of_service/test/qos_test_subscriber.cpp +++ b/test_quality_of_service/test/qos_test_subscriber.cpp @@ -27,8 +27,9 @@ using std::placeholders::_1; QosTestSubscriber::QosTestSubscriber( const std::string & name, const std::string & topic, + const rclcpp::QoS & qos_options, const rclcpp::SubscriptionOptions & sub_options) -: QosTestNode(name, topic), +: QosTestNode(name, topic, qos_options), sub_options_(sub_options), subscription_(nullptr) { @@ -48,8 +49,8 @@ void QosTestSubscriber::setup_start() if (!subscription_) { subscription_ = create_subscription( topic_, + qos_options_, std::bind(&QosTestSubscriber::listen_to_message, this, _1), - 10, sub_options_); } } diff --git a/test_quality_of_service/test/test_deadline.cpp b/test_quality_of_service/test/test_deadline.cpp index 3fec865a..cbbfa184 100644 --- a/test_quality_of_service/test/test_deadline.cpp +++ b/test_quality_of_service/test/test_deadline.cpp @@ -34,19 +34,17 @@ using namespace std::chrono_literals; TEST_F(QosRclcppTestFixture, test_deadline_no_publisher) { const std::chrono::milliseconds deadline_duration = 1s; const std::chrono::milliseconds test_duration = 10500ms; - const std::tuple deadline_duration_tuple = convert_chrono_milliseconds_to_size_t( - deadline_duration); const int expected_number_of_deadline_callbacks = test_duration / deadline_duration; int total_number_of_subscriber_deadline_events = 0; int last_sub_count = 0; - rmw_qos_profile_t qos_profile = rmw_qos_profile_default; - std::tie(qos_profile.deadline.sec, qos_profile.deadline.nsec) = deadline_duration_tuple; + // define qos profile + rclcpp::QoS qos_profile(10); + qos_profile.deadline(deadline_duration); // setup subscription options and callback rclcpp::SubscriptionOptions subscriber_options; - subscriber_options.qos_profile = qos_profile; subscriber_options.event_callbacks.deadline_callback = [&last_sub_count, &total_number_of_subscriber_deadline_events](rclcpp::QOSDeadlineRequestedInfo & event) -> void @@ -63,9 +61,10 @@ TEST_F(QosRclcppTestFixture, test_deadline_no_publisher) { const std::string topic("test_deadline_no_publisher"); // register a publisher for the topic but don't publish anything or use QoS options - publisher = std::make_shared("publisher", topic, publisher_options, - test_duration); - subscriber = std::make_shared("subscriber", topic, subscriber_options); + publisher = std::make_shared( + "publisher", topic, qos_profile, publisher_options, test_duration); + subscriber = std::make_shared( + "subscriber", topic, qos_profile, subscriber_options); executor->add_node(subscriber); subscriber->start(); @@ -83,8 +82,6 @@ TEST_F(QosRclcppTestFixture, test_deadline_no_publisher) { TEST_F(QosRclcppTestFixture, test_deadline) { int expected_number_of_events = 5; const std::chrono::milliseconds deadline_duration = 1s; - const std::tuple deadline_duration_tuple = convert_chrono_milliseconds_to_size_t( - deadline_duration); const std::chrono::milliseconds max_test_length = 10s; const std::chrono::milliseconds publish_rate = deadline_duration / expected_number_of_events; @@ -94,12 +91,11 @@ TEST_F(QosRclcppTestFixture, test_deadline) { int last_pub_count = 0; int last_sub_count = 0; - rmw_qos_profile_t qos_profile = rmw_qos_profile_default; - std::tie(qos_profile.deadline.sec, qos_profile.deadline.nsec) = deadline_duration_tuple; + rclcpp::QoS qos_profile(10); + qos_profile.deadline(deadline_duration); // setup subscription options and callback rclcpp::SubscriptionOptions subscriber_options; - subscriber_options.qos_profile = qos_profile; subscriber_options.event_callbacks.deadline_callback = [&last_sub_count, &total_number_of_subscriber_deadline_events](rclcpp::QOSDeadlineRequestedInfo & event) -> void @@ -113,7 +109,6 @@ TEST_F(QosRclcppTestFixture, test_deadline) { // setup publishing options and callback rclcpp::PublisherOptions publisher_options; - publisher_options.qos_profile = qos_profile; publisher_options.event_callbacks.deadline_callback = [&last_pub_count, &total_number_of_publisher_deadline_events](rclcpp::QOSDeadlineOfferedInfo & event) -> void @@ -127,9 +122,10 @@ TEST_F(QosRclcppTestFixture, test_deadline) { const std::string topic("test_deadline"); - publisher = std::make_shared("publisher", topic, publisher_options, - publish_rate); - subscriber = std::make_shared("subscriber", topic, subscriber_options); + publisher = std::make_shared( + "publisher", topic, qos_profile, publisher_options, publish_rate); + subscriber = std::make_shared( + "subscriber", topic, qos_profile, subscriber_options); // toggle publishing on and off to force deadline events rclcpp::TimerBase::SharedPtr toggle_publisher_timer = subscriber->create_wall_timer( diff --git a/test_quality_of_service/test/test_lifespan.cpp b/test_quality_of_service/test/test_lifespan.cpp index 159929e5..64bf5858 100644 --- a/test_quality_of_service/test/test_lifespan.cpp +++ b/test_quality_of_service/test/test_lifespan.cpp @@ -32,33 +32,28 @@ using namespace std::chrono_literals; TEST_F(QosRclcppTestFixture, test_deadline) { const std::chrono::milliseconds lifespan_duration = 1s; - const std::tuple message_lifespan = convert_chrono_milliseconds_to_size_t( - lifespan_duration); const int history = 2; const std::chrono::milliseconds max_test_length = 11s; const int expected_published = max_test_length / lifespan_duration * 2; const std::chrono::milliseconds publish_period = 500ms; // define qos profile - rmw_qos_profile_t qos_profile = rmw_qos_profile_default; - - qos_profile.depth = history; - qos_profile.durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL; - std::tie(qos_profile.deadline.sec, qos_profile.deadline.nsec) = message_lifespan; + rclcpp::QoS qos_profile(history); + qos_profile.durability(RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL); + qos_profile.lifespan(lifespan_duration); // subscription options rclcpp::SubscriptionOptions subscriber_options; - subscriber_options.qos_profile = qos_profile; // publisher options rclcpp::PublisherOptions publisher_options; - publisher_options.qos_profile = qos_profile; std::string topic = "test_lifespan"; - publisher = std::make_shared("publisher", topic, publisher_options, - publish_period); - subscriber = std::make_shared("subscriber", topic, subscriber_options); + publisher = std::make_shared( + "publisher", topic, qos_profile, publisher_options, publish_period); + subscriber = std::make_shared( + "subscriber", topic, qos_profile, subscriber_options); int timer_fired_count = 0; // toggle publishing on and off to force deadline events diff --git a/test_quality_of_service/test/test_liveliness.cpp b/test_quality_of_service/test/test_liveliness.cpp index 96e9d87c..50f169d6 100644 --- a/test_quality_of_service/test/test_liveliness.cpp +++ b/test_quality_of_service/test/test_liveliness.cpp @@ -40,21 +40,17 @@ TEST_F(QosRclcppTestFixture, test_automatic_liveliness_changed) { const std::chrono::milliseconds kill_publisher_after = 2s; const std::chrono::milliseconds publish_period = 200ms; int number_of_published_messages = 0; - const std::tuple liveliness_lease_tuple = - convert_chrono_milliseconds_to_size_t(1s); + const std::chrono::milliseconds lease_duration = 1s; int total_number_of_liveliness_events = 0; - rmw_qos_profile_t qos_profile = rmw_qos_profile_default; - // define liveliness policy - qos_profile.liveliness = RMW_QOS_POLICY_LIVELINESS_AUTOMATIC; - - std::tie(qos_profile.deadline.sec, qos_profile.deadline.nsec) = liveliness_lease_tuple; + // define qos profile + rclcpp::QoS qos_profile(10); + qos_profile.liveliness(RMW_QOS_POLICY_LIVELINESS_AUTOMATIC); + qos_profile.liveliness_lease_duration(lease_duration); // subscription options rclcpp::SubscriptionOptions subscriber_options; - subscriber_options.qos_profile = qos_profile; - // subscriber Liveliness callback event subscriber_options.event_callbacks.liveliness_callback = [&total_number_of_liveliness_events](rclcpp::QOSLivelinessChangedInfo & event) -> void @@ -80,20 +76,18 @@ TEST_F(QosRclcppTestFixture, test_automatic_liveliness_changed) { // publisher options rclcpp::PublisherOptions publisher_options; - publisher_options.qos_profile = qos_profile; - -// publisher_options.event_callbacks.liveliness_callback = -// [](rclcpp::QOSLivelinessLostInfo & event) -> void -// { -// RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "=====QOSLivelinessLostInfo callback fired"); -// }; + publisher_options.event_callbacks.liveliness_callback = + [](rclcpp::QOSLivelinessLostInfo & /*event*/) -> void + { + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "=====QOSLivelinessLostInfo callback fired"); + }; std::string topic("test_automatic_liveliness_changed"); - publisher = std::make_shared("publisher", topic, publisher_options, - publish_period); - - subscriber = std::make_shared("subscriber", topic, subscriber_options); + publisher = std::make_shared( + "publisher", topic, qos_profile, publisher_options, publish_period); + subscriber = std::make_shared( + "subscriber", topic, qos_profile, subscriber_options); int timer_fired_count = 0; // kill the publisher after a predetermined amount of time