From 2e8382474bb49166deb483f2376cc9168b8ce6b9 Mon Sep 17 00:00:00 2001 From: Liam Han Date: Mon, 25 Jul 2022 19:15:27 +0000 Subject: [PATCH 01/26] initial commit to allow plugin to call a service Signed-off-by: Liam Han --- .../triggered_publisher/TriggeredPublisher.cc | 132 ++++++++++++++++-- .../triggered_publisher/TriggeredPublisher.hh | 54 ++++++- 2 files changed, 171 insertions(+), 15 deletions(-) diff --git a/src/systems/triggered_publisher/TriggeredPublisher.cc b/src/systems/triggered_publisher/TriggeredPublisher.cc index 94a63b7ac8..bea774b3e0 100644 --- a/src/systems/triggered_publisher/TriggeredPublisher.cc +++ b/src/systems/triggered_publisher/TriggeredPublisher.cc @@ -477,11 +477,13 @@ std::unique_ptr InputMatcher::Create( TriggeredPublisher::~TriggeredPublisher() { this->done = true; + this->s_done = true; this->newMatchSignal.notify_one(); + this->serviceMatchSignal.notify_one(); if (this->workerThread.joinable()) - { this->workerThread.join(); - } + if (this->serviceWorkerThread.joinable()) + this->serviceWorkerThread.join(); } ////////////////////////////////////////////////// @@ -596,33 +598,81 @@ void TriggeredPublisher::Configure(const Entity &, } } else - { ignerr << "No ouptut specified" << std::endl; - return; + + if (sdfClone->HasElement("service")) + { + for (auto serviceElem = sdfClone->GetElement("service"); serviceElem; + serviceElem = serviceElem->GetNextElement("service")) + { + ServiceOutputInfo s_info; + s_info.servName = serviceElem->Get("name"); + if (s_info.servName.empty()) + { + ignerr << "Service name cannot be empty\n"; + } + s_info.reqType = serviceElem->Get("reqType"); + if (s_info.reqType.empty()) + { + ignerr << "Service request type cannot be empty\n"; + } + s_info.repType = serviceElem->Get("repType"); + if (s_info.repType.empty()) + { + ignerr << "Service response type cannot be empty\n"; + } + s_info.reqMsg = serviceElem->Get("reqMsg"); + if (s_info.reqMsg.empty()) + { + ignerr << "Service request string cannot be empty\n"; + } + std::string _timeout = serviceElem->Get("timeout"); + if (_timeout.empty()) + { + ignwarn << "Timeout value not specified for service name [" + << s_info.servName << "] with service type [" + << s_info.reqType << "]. Using default value of 3000\n"; + // Use default timeout value of 3000ms + s_info.timeout = 3000; + } + else + s_info.timeout = std::stoi(_timeout); + + this->serviceOutputInfo.push_back(std::move(s_info)); + } } + else + ignwarn << "No service specified" << std::endl; auto msgCb = std::function( [this](const auto &_msg) { if (this->MatchInput(_msg)) { + if (this->delay > 0ms) + { + std::lock_guard lock(this->publishQueueMutex); + this->publishQueue.push_back(this->delay); + } + else { - if (this->delay > 0ms) { - std::lock_guard lock(this->publishQueueMutex); - this->publishQueue.push_back(this->delay); + std::lock_guard lock(this->publishCountMutex); + ++this->publishCount; } - else + this->newMatchSignal.notify_one(); + } + if (this->serviceOutputInfo.size() > 0) + { { - { - std::lock_guard lock(this->publishCountMutex); - ++this->publishCount; - } - this->newMatchSignal.notify_one(); + std::lock_guard lock(this->serviceCountMutex); + ++this->serviceCount; } + this->serviceMatchSignal.notify_one(); } } }); + if (!this->node.Subscribe(this->inputTopic, msgCb)) { ignerr << "Input subscriber could not be created for topic [" @@ -643,8 +693,64 @@ void TriggeredPublisher::Configure(const Entity &, this->workerThread = std::thread(std::bind(&TriggeredPublisher::DoWork, this)); + this->serviceWorkerThread = + std::thread(std::bind(&TriggeredPublisher::DoServiceWork, this)); +} + +////////////////////////////////////////////////// +void TriggeredPublisher::DoServiceWork() +{ + while (!this->s_done) + { + std::size_t pending{0}; + { + using namespace std::chrono_literals; + std::unique_lock lock(this->serviceCountMutex); + this->serviceMatchSignal.wait_for(lock, 1s, + [this] + { + return (this->serviceCount > 0) || this->s_done; + }); + + if (this->serviceCount == 0 || this->s_done) + continue; + + std::swap(pending, this->serviceCount); + } + for (auto &s_info : this->serviceOutputInfo) + { + for (std::size_t i = 0; i < pending; ++i) + { + bool result; + auto req = msgs::Factory::New(s_info.reqType, s_info.reqMsg); + if (!req) + { + ignerr << "Unable to create request for type[" + << s_info.reqType << "].\n"; + return; + } + auto rep = msgs::Factory::New(s_info.repType); + if (!rep) + { + ignerr << "Unable to create response for type[" + << s_info.repType << "].\n"; + return; + } + bool executed = this->node.Request(s_info.servName, *req, + s_info.timeout, *rep, result); + if (executed) + { + if (!result) + ignerr << "Service call [" << s_info.servName << "] failed\n"; + } + else + ignerr << "Service call [" << s_info.servName << "] timed out\n"; + } + } + } } + ////////////////////////////////////////////////// void TriggeredPublisher::DoWork() { diff --git a/src/systems/triggered_publisher/TriggeredPublisher.hh b/src/systems/triggered_publisher/TriggeredPublisher.hh index f524b119ab..74877c1831 100644 --- a/src/systems/triggered_publisher/TriggeredPublisher.hh +++ b/src/systems/triggered_publisher/TriggeredPublisher.hh @@ -36,8 +36,8 @@ namespace systems class InputMatcher; /// \brief The triggered publisher system publishes a user specified message - /// on an output topic in response to an input message that matches user - /// specified criteria. An optional simulation time delay can be used + /// on an output topic or service in response to an input message that matches + /// user specified criteria. An optional simulation time delay can be used /// delay message publication. /// /// ## System Parameters @@ -78,6 +78,16 @@ namespace systems /// ``: Integer number of milliseconds, in simulation time, to /// delay publication. /// + /// - ``: Contains configuration for service to call: Multiple + /// `` tags are possible. A service will be called for each input + /// that matches. + /// * Attributes: + /// * `name`: Service name (eg. `/world/triggered_publisher/set_pose`) + /// * `reqType`: Service request message type (eg. ignition.msgs.Pose) + /// * `repType`: Service respond message type (eg. ignition.msgs.Boolean) + /// * `timeout`: Service request timeout + /// * `reqMsg`: String used to construct the service protobuf message. + /// /// Examples: /// 1. Any receipt of a Boolean messages on the input topic triggers an output /// \code{.xml} @@ -182,6 +192,9 @@ namespace systems /// \brief Thread that handles publishing output messages public: void DoWork(); + /// \brief Thread that handles calling services + public: void DoServiceWork(); + /// \brief Helper function that calls Match on every InputMatcher available /// \param[in] _inputMsg Input message /// \return True if all of the matchers return true @@ -210,31 +223,68 @@ namespace systems transport::Node::Publisher pub; }; + /// \brief Class that holds necessary bits for each specified service output + private: struct ServiceOutputInfo + { + /// \brief Service name + std::string servName; + + /// \brief Service request type + std::string reqType; + + /// \brief Service response type + std::string repType; + + /// \brief Service request message + std::string reqMsg; + + /// \brief Service request timeout + unsigned int timeout; + }; + /// \brief List of InputMatchers private: std::vector> matchers; /// \brief List of outputs private: std::vector outputInfo; + /// \brief List of service outputs + private: std::vector serviceOutputInfo; + /// \brief Ignition communication node. private: transport::Node node; /// \brief Counter that tells the publisher how many times to publish private: std::size_t publishCount{0}; + /// \brief Counter that tells how many times to call the service + private: std::size_t serviceCount{0}; + /// \brief Mutex to synchronize access to publishCount private: std::mutex publishCountMutex; + /// \brief Mutex to synchronize access to serviceCount + private: std::mutex serviceCountMutex; + /// \brief Condition variable to signal that new matches have occured private: std::condition_variable newMatchSignal; + /// \brief Condition variable to signal to call a service + private: std::condition_variable serviceMatchSignal; + /// \brief Thread handle for worker thread private: std::thread workerThread; + /// \brief Thread handle for service worker thread + private: std::thread serviceWorkerThread; + /// \brief Flag for when the system is done and the worker thread should /// stop private: std::atomic done{false}; + /// \brief Flag used for the service worker thread + private: std::atomic s_done{false}; + /// \brief Publish delay time. This is in simulation time. private: std::chrono::steady_clock::duration delay{0}; From 48a1a1155ab5f5502e1a9996b6350cfc6835327d Mon Sep 17 00:00:00 2001 From: Liam Han Date: Mon, 25 Jul 2022 19:21:51 +0000 Subject: [PATCH 02/26] adding tutorial and modifying the world sdf Signed-off-by: Liam Han --- examples/worlds/triggered_publisher.sdf | 8 +++++++ tutorials/triggered_publisher.md | 28 ++++++++++++++++++++++--- 2 files changed, 33 insertions(+), 3 deletions(-) diff --git a/examples/worlds/triggered_publisher.sdf b/examples/worlds/triggered_publisher.sdf index 303f78beca..e643ca2ce9 100644 --- a/examples/worlds/triggered_publisher.sdf +++ b/examples/worlds/triggered_publisher.sdf @@ -454,6 +454,14 @@ start falling. linear: {x: 3} + + + + linear: {x: 0} + + + data: true diff --git a/tutorials/triggered_publisher.md b/tutorials/triggered_publisher.md index d07c74c318..6cd5b33bb7 100644 --- a/tutorials/triggered_publisher.md +++ b/tutorials/triggered_publisher.md @@ -1,8 +1,8 @@ \page triggeredpublisher Triggered Publisher The `TriggeredPublisher` system publishes a user specified message on an output -topic in response to an input message that matches user specified criteria. The -system works by checking the input against a set of Matchers. Matchers +topic or service in response to an input message that matches user specified criteria. +The system works by checking the input against a set of Matchers. Matchers contain string representations of protobuf messages which are compared for equality or containment with the input message. Matchers can match the whole input message or only a specific field inside the message. @@ -11,7 +11,8 @@ This tutorial describes how the Triggered Publisher system can be used to cause a box to fall from its initial position by detaching a detachable joint in response to the motion of a vehicle. The tutorial also covers how Triggered Publisher systems can be chained together by showing how the falling of the box -can trigger another box to fall. The finished world SDFormat file for this +can trigger another box to fall. Last, it'll cover how service call can be triggered +to reset the robot pose. The finished world SDFormat file for this tutorial can be found in [examples/worlds/triggered_publisher.sdf](https://github.com/ignitionrobotics/ign-gazebo/blob/ign-gazebo2/examples/worlds/triggered_publisher.sdf) @@ -263,3 +264,24 @@ and publish the start message ``` ign topic -t "/start" -m ignition.msgs.Empty -p " " ``` + +Once both boxes have fallen, we can publish a message to invoke a service call +to reset the robot position as well as set the speed to 0. As shown below, the +`` sets the linear x speed to 0 and `` tag contains metadata +to invoke a service call to `/world/triggered_publisher/set_pose`. The `reqMsg` +is expressed in the human-readable form of Google Protobuf meesages. Multiple +services can be used and it can also be used with the `` tag. + +```xml + + + + linear: {x: 0} + + + + +``` From 8b1fb70dddf65b1d2be9d2dab953e81c213ee484 Mon Sep 17 00:00:00 2001 From: Liam Han Date: Tue, 26 Jul 2022 22:12:11 +0000 Subject: [PATCH 03/26] added test for single input and single service output Signed-off-by: Liam Han --- test/integration/triggered_publisher.cc | 45 +++++++++++++++++++++++++ test/worlds/triggered_publisher.sdf | 6 ++++ 2 files changed, 51 insertions(+) diff --git a/test/integration/triggered_publisher.cc b/test/integration/triggered_publisher.cc index 9590a7c421..04ab70f436 100644 --- a/test/integration/triggered_publisher.cc +++ b/test/integration/triggered_publisher.cc @@ -637,3 +637,48 @@ TEST_F(TriggeredPublisherTest, EXPECT_EQ(0u, recvCount); } + + +// TODO: add test for no service call (aka timeout) +// TODO: add test for multiple services +// TODO: add test for correct service (x) +// TODO: add more Request options (some requests take different param) + +///////////////////////////////////////////////// +TEST_F(TriggeredPublisherTest, + IGN_UTILS_TEST_DISABLED_ON_WIN32(InputMessagesTriggerServices)) +{ + transport::Node node; + auto inputPub = node.Advertise("/in_14"); + std::atomic recvCount{0}; + + // Responser (https://gazebosim.org/api/transport/11.0/services.html) + + auto srvEchoCb = std::function( + [&recvCount](const auto &_req, auto &_rep) + { + ++recvCount; + _rep.set_data(_req.data()); + EXPECT_TRUE(_req.data() == "test"); + if (_req.data() != "test") + return false; + return true; + }); + + // Advertise a dummy service + std::string service = "/srv-test"; + node.Advertise(service, srvEchoCb); + + const std::size_t pubCount{10}; + for (std::size_t i = 0; i < pubCount; ++i) + { + EXPECT_TRUE(inputPub.Publish(msgs::Empty())); + IGN_SLEEP_MS(100); + } + + waitUntil(5000, [&]{return pubCount == recvCount;}); + EXPECT_EQ(pubCount, recvCount); +} + +///////////////////////////////////////////////// diff --git a/test/worlds/triggered_publisher.sdf b/test/worlds/triggered_publisher.sdf index 7c9f21c568..c83817c75e 100644 --- a/test/worlds/triggered_publisher.sdf +++ b/test/worlds/triggered_publisher.sdf @@ -166,6 +166,12 @@ + + + + + From 3361e9a6350680ae040fcb391cbb5f30d2a7c869 Mon Sep 17 00:00:00 2001 From: Liam Han Date: Tue, 26 Jul 2022 23:03:34 +0000 Subject: [PATCH 04/26] added test for single input and multiple service output Signed-off-by: Liam Han --- test/integration/triggered_publisher.cc | 49 +++++++++++++++++++++++++ 1 file changed, 49 insertions(+) diff --git a/test/integration/triggered_publisher.cc b/test/integration/triggered_publisher.cc index 04ab70f436..a87977371c 100644 --- a/test/integration/triggered_publisher.cc +++ b/test/integration/triggered_publisher.cc @@ -682,3 +682,52 @@ TEST_F(TriggeredPublisherTest, } ///////////////////////////////////////////////// +TEST_F(TriggeredPublisherTest, + IGN_UTILS_TEST_DISABLED_ON_WIN32(MultipleServiceForOneInput)) +{ + transport::Node node; + auto inputPub = node.Advertise("/in_15"); + std::mutex recvMsgMutex; + std::vector recvMsgs0; + std::vector recvMsgs1; + auto cbCreator = [&recvMsgMutex](std::vector &_msgVector) + { + return std::function( + [&_msgVector, &recvMsgMutex](const auto &_req, auto &_rep) + { + std::lock_guard lock(recvMsgMutex); + _msgVector.push_back(_req.data()); + if (_req.data() || !_req.data()) + return true; + return false; + }); + }; + + auto msgCb0 = cbCreator(recvMsgs0); + auto msgCb1 = cbCreator(recvMsgs1); + + // Advertise two dummy services + node.Advertise("/srv-test-0", msgCb0); + node.Advertise("/srv-test-1", msgCb1); + + const int pubCount{10}; + for (int i = 0; i < pubCount; ++i) + { + EXPECT_TRUE(inputPub.Publish(msgs::Empty())); + IGN_SLEEP_MS(10); + } + + waitUntil(5000, [&] + { + std::lock_guard lock(recvMsgMutex); + return static_cast(pubCount) == recvMsgs0.size() && + static_cast(pubCount) == recvMsgs1.size(); + }); + + EXPECT_EQ(static_cast(pubCount), recvMsgs0.size()); + EXPECT_EQ(static_cast(pubCount), recvMsgs1.size()); + + // The plugin has two outputs. We expect 10 messages in each output topic + EXPECT_EQ(pubCount, std::count(recvMsgs0.begin(), recvMsgs0.end(), true)); + EXPECT_EQ(pubCount, std::count(recvMsgs1.begin(), recvMsgs1.end(), false)); +} From 8209f6b92730acc27d666329dbef33b472662947 Mon Sep 17 00:00:00 2001 From: Liam Han Date: Tue, 26 Jul 2022 23:59:10 +0000 Subject: [PATCH 05/26] added test for invalid matching service name => timeout Signed-off-by: Liam Han --- test/integration/triggered_publisher.cc | 73 ++++++++++++++++++++----- test/worlds/triggered_publisher.sdf | 8 +++ 2 files changed, 68 insertions(+), 13 deletions(-) diff --git a/test/integration/triggered_publisher.cc b/test/integration/triggered_publisher.cc index a87977371c..79532722a2 100644 --- a/test/integration/triggered_publisher.cc +++ b/test/integration/triggered_publisher.cc @@ -638,12 +638,51 @@ TEST_F(TriggeredPublisherTest, EXPECT_EQ(0u, recvCount); } +///////////////////////////////////////////////// +// Test for invalid service name. It'll timeout +// when matching service name can't be found. +///////////////////////////////////////////////// +TEST_F(TriggeredPublisherTest, + IGN_UTILS_TEST_DISABLED_ON_WIN32(InvalidServiceName)) +{ + transport::Node node; + auto inputPub = node.Advertise("/in_14"); + std::atomic recvCount{0}; -// TODO: add test for no service call (aka timeout) -// TODO: add test for multiple services -// TODO: add test for correct service (x) -// TODO: add more Request options (some requests take different param) + auto srvEchoCb = std::function( + [&recvCount](const auto &_req, auto &_rep) + { + EXPECT_TRUE(_req.data() == "test"); + if (_req.data() == "test") + { + ++recvCount; + _rep.set_data(_req.data()); + return true; + } + return false; + }); + + // Advertise a dummy service + std::string service = "/srv-dummy-test"; + node.Advertise(service, srvEchoCb); + + const std::size_t pubCount{10}; + for (std::size_t i = 0; i < pubCount; ++i) + { + EXPECT_TRUE(inputPub.Publish(msgs::Empty())); + IGN_SLEEP_MS(100); + } + + waitUntil(5000, [&]{return recvCount == 0u;}); + EXPECT_EQ(recvCount, 0u); +} +///////////////////////////////////////////////// +// Test for triggering a service call in response +// to input ign msg by publishing 10 times. Service +// call will also occur 10 times. It'll compare +// pubCount and recvCount. ///////////////////////////////////////////////// TEST_F(TriggeredPublisherTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(InputMessagesTriggerServices)) @@ -652,18 +691,18 @@ TEST_F(TriggeredPublisherTest, auto inputPub = node.Advertise("/in_14"); std::atomic recvCount{0}; - // Responser (https://gazebosim.org/api/transport/11.0/services.html) - auto srvEchoCb = std::function( [&recvCount](const auto &_req, auto &_rep) { - ++recvCount; - _rep.set_data(_req.data()); EXPECT_TRUE(_req.data() == "test"); - if (_req.data() != "test") - return false; - return true; + if (_req.data() == "test") + { + ++recvCount; + _rep.set_data(_req.data()); + return true; + } + return false; }); // Advertise a dummy service @@ -681,6 +720,12 @@ TEST_F(TriggeredPublisherTest, EXPECT_EQ(pubCount, recvCount); } +///////////////////////////////////////////////// +// Test for triggering multiple services in response +// to a input ign msg by publishing 10 times. Service +// call will also occur 10 times for each service. +// It'll compare pubCount and recvCount for each +// services. ///////////////////////////////////////////////// TEST_F(TriggeredPublisherTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(MultipleServiceForOneInput)) @@ -696,9 +741,11 @@ TEST_F(TriggeredPublisherTest, [&_msgVector, &recvMsgMutex](const auto &_req, auto &_rep) { std::lock_guard lock(recvMsgMutex); - _msgVector.push_back(_req.data()); if (_req.data() || !_req.data()) - return true; + { + _msgVector.push_back(_req.data()); + return true; + } return false; }); }; diff --git a/test/worlds/triggered_publisher.sdf b/test/worlds/triggered_publisher.sdf index c83817c75e..34eaa1d04b 100644 --- a/test/worlds/triggered_publisher.sdf +++ b/test/worlds/triggered_publisher.sdf @@ -172,6 +172,14 @@ + + + + + + From 8cee834ebbf8f184a8db97f551b842291c503898 Mon Sep 17 00:00:00 2001 From: Liam Han Date: Wed, 27 Jul 2022 00:50:50 +0000 Subject: [PATCH 06/26] modified variables the camelCase Signed-off-by: Liam Han --- .../triggered_publisher/TriggeredPublisher.cc | 58 +++++++++---------- .../triggered_publisher/TriggeredPublisher.hh | 2 +- 2 files changed, 30 insertions(+), 30 deletions(-) diff --git a/src/systems/triggered_publisher/TriggeredPublisher.cc b/src/systems/triggered_publisher/TriggeredPublisher.cc index bea774b3e0..eec9936021 100644 --- a/src/systems/triggered_publisher/TriggeredPublisher.cc +++ b/src/systems/triggered_publisher/TriggeredPublisher.cc @@ -477,7 +477,7 @@ std::unique_ptr InputMatcher::Create( TriggeredPublisher::~TriggeredPublisher() { this->done = true; - this->s_done = true; + this->srvDone = true; this->newMatchSignal.notify_one(); this->serviceMatchSignal.notify_one(); if (this->workerThread.joinable()) @@ -605,40 +605,40 @@ void TriggeredPublisher::Configure(const Entity &, for (auto serviceElem = sdfClone->GetElement("service"); serviceElem; serviceElem = serviceElem->GetNextElement("service")) { - ServiceOutputInfo s_info; - s_info.servName = serviceElem->Get("name"); - if (s_info.servName.empty()) + ServiceOutputInfo serviceInfo; + serviceInfo.servName = serviceElem->Get("name"); + if (serviceInfo.servName.empty()) { ignerr << "Service name cannot be empty\n"; } - s_info.reqType = serviceElem->Get("reqType"); - if (s_info.reqType.empty()) + serviceInfo.reqType = serviceElem->Get("reqType"); + if (serviceInfo.reqType.empty()) { ignerr << "Service request type cannot be empty\n"; } - s_info.repType = serviceElem->Get("repType"); - if (s_info.repType.empty()) + serviceInfo.repType = serviceElem->Get("repType"); + if (serviceInfo.repType.empty()) { ignerr << "Service response type cannot be empty\n"; } - s_info.reqMsg = serviceElem->Get("reqMsg"); - if (s_info.reqMsg.empty()) + serviceInfo.reqMsg = serviceElem->Get("reqMsg"); + if (serviceInfo.reqMsg.empty()) { ignerr << "Service request string cannot be empty\n"; } - std::string _timeout = serviceElem->Get("timeout"); - if (_timeout.empty()) + std::string timeoutInfo = serviceElem->Get("timeout"); + if (timeoutInfo.empty()) { ignwarn << "Timeout value not specified for service name [" - << s_info.servName << "] with service type [" - << s_info.reqType << "]. Using default value of 3000\n"; + << serviceInfo.servName << "] with service type [" + << serviceInfo.reqType << "]. Using default value of 3000\n"; // Use default timeout value of 3000ms - s_info.timeout = 3000; + serviceInfo.timeout = 3000; } else - s_info.timeout = std::stoi(_timeout); + serviceInfo.timeout = std::stoi(timeoutInfo); - this->serviceOutputInfo.push_back(std::move(s_info)); + this->serviceOutputInfo.push_back(std::move(serviceInfo)); } } else @@ -700,7 +700,7 @@ void TriggeredPublisher::Configure(const Entity &, ////////////////////////////////////////////////// void TriggeredPublisher::DoServiceWork() { - while (!this->s_done) + while (!this->srvDone) { std::size_t pending{0}; { @@ -709,42 +709,42 @@ void TriggeredPublisher::DoServiceWork() this->serviceMatchSignal.wait_for(lock, 1s, [this] { - return (this->serviceCount > 0) || this->s_done; + return (this->serviceCount > 0) || this->srvDone; }); - if (this->serviceCount == 0 || this->s_done) + if (this->serviceCount == 0 || this->srvDone) continue; std::swap(pending, this->serviceCount); } - for (auto &s_info : this->serviceOutputInfo) + for (auto &serviceInfo : this->serviceOutputInfo) { for (std::size_t i = 0; i < pending; ++i) { bool result; - auto req = msgs::Factory::New(s_info.reqType, s_info.reqMsg); + auto req = msgs::Factory::New(serviceInfo.reqType, serviceInfo.reqMsg); if (!req) { ignerr << "Unable to create request for type[" - << s_info.reqType << "].\n"; + << serviceInfo.reqType << "].\n"; return; } - auto rep = msgs::Factory::New(s_info.repType); + auto rep = msgs::Factory::New(serviceInfo.repType); if (!rep) { ignerr << "Unable to create response for type[" - << s_info.repType << "].\n"; + << serviceInfo.repType << "].\n"; return; } - bool executed = this->node.Request(s_info.servName, *req, - s_info.timeout, *rep, result); + bool executed = this->node.Request(serviceInfo.servName, *req, + serviceInfo.timeout, *rep, result); if (executed) { if (!result) - ignerr << "Service call [" << s_info.servName << "] failed\n"; + ignerr << "Service call [" << serviceInfo.servName << "] failed\n"; } else - ignerr << "Service call [" << s_info.servName << "] timed out\n"; + ignerr << "Service call [" << serviceInfo.servName << "] timed out\n"; } } } diff --git a/src/systems/triggered_publisher/TriggeredPublisher.hh b/src/systems/triggered_publisher/TriggeredPublisher.hh index 74877c1831..7f30da15b7 100644 --- a/src/systems/triggered_publisher/TriggeredPublisher.hh +++ b/src/systems/triggered_publisher/TriggeredPublisher.hh @@ -283,7 +283,7 @@ namespace systems private: std::atomic done{false}; /// \brief Flag used for the service worker thread - private: std::atomic s_done{false}; + private: std::atomic srvDone{false}; /// \brief Publish delay time. This is in simulation time. private: std::chrono::steady_clock::duration delay{0}; From 4d4d38441d16f37443b4a4e6fedf7ff28e2ed3cc Mon Sep 17 00:00:00 2001 From: Liam Han Date: Thu, 28 Jul 2022 04:14:47 +0000 Subject: [PATCH 07/26] fixed typo, indentation, grammar, lines that exceeded 80 char Signed-off-by: Liam Han --- examples/worlds/triggered_publisher.sdf | 33 +++-- .../triggered_publisher/TriggeredPublisher.cc | 97 +++++++++------ .../triggered_publisher/TriggeredPublisher.hh | 10 +- test/worlds/triggered_publisher.sdf | 114 +++++++++++++----- tutorials/triggered_publisher.md | 33 ++--- 5 files changed, 198 insertions(+), 89 deletions(-) diff --git a/examples/worlds/triggered_publisher.sdf b/examples/worlds/triggered_publisher.sdf index e643ca2ce9..42718d613a 100644 --- a/examples/worlds/triggered_publisher.sdf +++ b/examples/worlds/triggered_publisher.sdf @@ -387,13 +387,17 @@ start falling. true - + body box1 box_body /box1/detach - + body box2 box_body @@ -448,27 +452,40 @@ start falling. - + linear: {x: 3} - + linear: {x: 0} - + + - + data: true - + -7.5 diff --git a/src/systems/triggered_publisher/TriggeredPublisher.cc b/src/systems/triggered_publisher/TriggeredPublisher.cc index eec9936021..58c7495628 100644 --- a/src/systems/triggered_publisher/TriggeredPublisher.cc +++ b/src/systems/triggered_publisher/TriggeredPublisher.cc @@ -252,7 +252,9 @@ FullMatcher::FullMatcher(const std::string &_msgType, bool _logicType, : InputMatcher(_msgType), logicType(_logicType) { if (nullptr == this->matchMsg || !this->matchMsg->IsInitialized()) + { return; + } this->valid = google::protobuf::TextFormat::ParseFromString( _matchString, this->matchMsg.get()); @@ -273,7 +275,9 @@ FieldMatcher::FieldMatcher(const std::string &_msgType, bool _logicType, fieldName(_fieldName) { if (nullptr == this->matchMsg || !this->matchMsg->IsInitialized()) + { return; + } transport::ProtoMsg *matcherSubMsg{nullptr}; if (!FindFieldSubMessage(this->matchMsg.get(), _fieldName, @@ -294,7 +298,9 @@ FieldMatcher::FieldMatcher(const std::string &_msgType, bool _logicType, } if (nullptr == matcherSubMsg) + { return; + } bool result = google::protobuf::TextFormat::ParseFieldValueFromString( _fieldString, this->fieldDescMatcher.back(), matcherSubMsg); @@ -481,9 +487,13 @@ TriggeredPublisher::~TriggeredPublisher() this->newMatchSignal.notify_one(); this->serviceMatchSignal.notify_one(); if (this->workerThread.joinable()) + { this->workerThread.join(); + } if (this->serviceWorkerThread.joinable()) + { this->serviceWorkerThread.join(); + } } ////////////////////////////////////////////////// @@ -550,7 +560,9 @@ void TriggeredPublisher::Configure(const Entity &, { int ms = sdfClone->Get("delay_ms"); if (ms > 0) + { this->delay = std::chrono::milliseconds(ms); + } } if (sdfClone->HasElement("output")) @@ -598,51 +610,57 @@ void TriggeredPublisher::Configure(const Entity &, } } else + { ignerr << "No ouptut specified" << std::endl; + } if (sdfClone->HasElement("service")) { for (auto serviceElem = sdfClone->GetElement("service"); serviceElem; serviceElem = serviceElem->GetNextElement("service")) { - ServiceOutputInfo serviceInfo; - serviceInfo.servName = serviceElem->Get("name"); - if (serviceInfo.servName.empty()) - { - ignerr << "Service name cannot be empty\n"; - } - serviceInfo.reqType = serviceElem->Get("reqType"); - if (serviceInfo.reqType.empty()) - { - ignerr << "Service request type cannot be empty\n"; - } - serviceInfo.repType = serviceElem->Get("repType"); - if (serviceInfo.repType.empty()) - { - ignerr << "Service response type cannot be empty\n"; - } - serviceInfo.reqMsg = serviceElem->Get("reqMsg"); - if (serviceInfo.reqMsg.empty()) - { - ignerr << "Service request string cannot be empty\n"; - } - std::string timeoutInfo = serviceElem->Get("timeout"); - if (timeoutInfo.empty()) - { - ignwarn << "Timeout value not specified for service name [" - << serviceInfo.servName << "] with service type [" - << serviceInfo.reqType << "]. Using default value of 3000\n"; - // Use default timeout value of 3000ms - serviceInfo.timeout = 3000; - } - else - serviceInfo.timeout = std::stoi(timeoutInfo); + ServiceOutputInfo serviceInfo; + serviceInfo.servName = serviceElem->Get("name"); + if (serviceInfo.servName.empty()) + { + ignerr << "Service name cannot be empty\n"; + } + serviceInfo.reqType = serviceElem->Get("reqType"); + if (serviceInfo.reqType.empty()) + { + ignerr << "Service request type cannot be empty\n"; + } + serviceInfo.repType = serviceElem->Get("repType"); + if (serviceInfo.repType.empty()) + { + ignerr << "Service response type cannot be empty\n"; + } + serviceInfo.reqMsg = serviceElem->Get("reqMsg"); + if (serviceInfo.reqMsg.empty()) + { + ignerr << "Service request string cannot be empty\n"; + } + std::string timeoutInfo = serviceElem->Get("timeout"); + if (timeoutInfo.empty()) + { + ignwarn << "Timeout value not specified for service name [" + << serviceInfo.servName << "] with service type [" + << serviceInfo.reqType << "]. Using default value of 3000\n"; + // Use default timeout value of 3000ms + serviceInfo.timeout = 3000; + } + else + { + serviceInfo.timeout = std::stoi(timeoutInfo); + } - this->serviceOutputInfo.push_back(std::move(serviceInfo)); + this->serviceOutputInfo.push_back(std::move(serviceInfo)); } } else + { ignwarn << "No service specified" << std::endl; + } auto msgCb = std::function( [this](const auto &_msg) @@ -713,7 +731,9 @@ void TriggeredPublisher::DoServiceWork() }); if (this->serviceCount == 0 || this->srvDone) + { continue; + } std::swap(pending, this->serviceCount); } @@ -729,6 +749,7 @@ void TriggeredPublisher::DoServiceWork() << serviceInfo.reqType << "].\n"; return; } + auto rep = msgs::Factory::New(serviceInfo.repType); if (!rep) { @@ -736,21 +757,25 @@ void TriggeredPublisher::DoServiceWork() << serviceInfo.repType << "].\n"; return; } + bool executed = this->node.Request(serviceInfo.servName, *req, serviceInfo.timeout, *rep, result); if (executed) { if (!result) + { ignerr << "Service call [" << serviceInfo.servName << "] failed\n"; + } } else + { ignerr << "Service call [" << serviceInfo.servName << "] timed out\n"; + } } } } } - ////////////////////////////////////////////////// void TriggeredPublisher::DoWork() { @@ -767,7 +792,9 @@ void TriggeredPublisher::DoWork() }); if (this->publishCount == 0 || this->done) + { continue; + } std::swap(pending, this->publishCount); } @@ -818,7 +845,9 @@ void TriggeredPublisher::PreUpdate(const ignition::gazebo::UpdateInfo &_info, } if (notify) + { this->newMatchSignal.notify_one(); + } } ////////////////////////////////////////////////// diff --git a/src/systems/triggered_publisher/TriggeredPublisher.hh b/src/systems/triggered_publisher/TriggeredPublisher.hh index 7f30da15b7..e075ec1a8a 100644 --- a/src/systems/triggered_publisher/TriggeredPublisher.hh +++ b/src/systems/triggered_publisher/TriggeredPublisher.hh @@ -36,9 +36,11 @@ namespace systems class InputMatcher; /// \brief The triggered publisher system publishes a user specified message - /// on an output topic or service in response to an input message that matches - /// user specified criteria. An optional simulation time delay can be used - /// delay message publication. + /// on an output topic in response to an input message that matches user + /// specified criteria. It can also call a user specified service as an + /// output in response to an input message. An optional simulation time delay + /// can be used delay message + /// publication. /// /// ## System Parameters /// @@ -75,7 +77,7 @@ namespace systems /// the human-readable representation of a protobuf message as used by /// `ign topic` for publishing messages /// - /// ``: Integer number of milliseconds, in simulation time, to + /// - ``: Integer number of milliseconds, in simulation time, to /// delay publication. /// /// - ``: Contains configuration for service to call: Multiple diff --git a/test/worlds/triggered_publisher.sdf b/test/worlds/triggered_publisher.sdf index 34eaa1d04b..09892482fb 100644 --- a/test/worlds/triggered_publisher.sdf +++ b/test/worlds/triggered_publisher.sdf @@ -5,19 +5,25 @@ 0 - + - + data: true - + data: false @@ -27,7 +33,9 @@ - + data: true @@ -36,7 +44,9 @@ - + data: 0 @@ -45,7 +55,9 @@ - + data: 0 @@ -54,7 +66,9 @@ - + data: -5 @@ -75,7 +89,9 @@ - + 1.0 2.0 @@ -83,7 +99,9 @@ - + 1.0 2.0 @@ -91,7 +109,9 @@ - + { @@ -102,7 +122,9 @@ - + { @@ -120,65 +142,99 @@ - + data: 0, data: 1 - + data: 0.5 - + 0.5 - + "value1" - + 1000 - - + + data: 0, data: 1 - + - + - - - - + + + + + - - + + + + diff --git a/tutorials/triggered_publisher.md b/tutorials/triggered_publisher.md index 6cd5b33bb7..358bff46b7 100644 --- a/tutorials/triggered_publisher.md +++ b/tutorials/triggered_publisher.md @@ -1,18 +1,20 @@ \page triggeredpublisher Triggered Publisher The `TriggeredPublisher` system publishes a user specified message on an output -topic or service in response to an input message that matches user specified criteria. -The system works by checking the input against a set of Matchers. Matchers -contain string representations of protobuf messages which are compared for -equality or containment with the input message. Matchers can match the whole -input message or only a specific field inside the message. +topic in response to an input message that matches user specified criteria. It +can also call a user specified service as an output in response to an input +message. The system works by checking the input against a set of Matchers. +Matchers contain string representations of protobuf messages which are compared +for equality or containment with the input message. Matchers can match the +whole input message or only a specific field inside the message. + This tutorial describes how the Triggered Publisher system can be used to cause a box to fall from its initial position by detaching a detachable joint in response to the motion of a vehicle. The tutorial also covers how Triggered Publisher systems can be chained together by showing how the falling of the box -can trigger another box to fall. Last, it'll cover how service call can be triggered -to reset the robot pose. The finished world SDFormat file for this +can trigger another box to fall. Last, it covers how a service call can be +triggered to reset the robot pose. The finished world SDFormat file for this tutorial can be found in [examples/worlds/triggered_publisher.sdf](https://github.com/ignitionrobotics/ign-gazebo/blob/ign-gazebo2/examples/worlds/triggered_publisher.sdf) @@ -267,10 +269,10 @@ ign topic -t "/start" -m ignition.msgs.Empty -p " " Once both boxes have fallen, we can publish a message to invoke a service call to reset the robot position as well as set the speed to 0. As shown below, the -`` sets the linear x speed to 0 and `` tag contains metadata -to invoke a service call to `/world/triggered_publisher/set_pose`. The `reqMsg` -is expressed in the human-readable form of Google Protobuf meesages. Multiple -services can be used and it can also be used with the `` tag. +`` sets the linear x speed to 0, and the `` tag contains +metadata to invoke a service call to `/world/triggered_publisher/set_pose`. The +`reqMsg` is expressed in the human-readable form of Google Protobuf meesages. +Multiple `` tags can be used as well as with the `` tag. ```xml ` tag. linear: {x: 0} - + ``` From 55e1035369257e0626ef2fbb1e2546500f6d8dd4 Mon Sep 17 00:00:00 2001 From: Liam Han Date: Thu, 28 Jul 2022 19:43:39 +0000 Subject: [PATCH 08/26] fixing ubuntu bionic ci issue Signed-off-by: Liam Han --- src/systems/triggered_publisher/TriggeredPublisher.cc | 6 ++++-- test/integration/triggered_publisher.cc | 4 ++-- 2 files changed, 6 insertions(+), 4 deletions(-) diff --git a/src/systems/triggered_publisher/TriggeredPublisher.cc b/src/systems/triggered_publisher/TriggeredPublisher.cc index 58c7495628..f4468f9495 100644 --- a/src/systems/triggered_publisher/TriggeredPublisher.cc +++ b/src/systems/triggered_publisher/TriggeredPublisher.cc @@ -764,12 +764,14 @@ void TriggeredPublisher::DoServiceWork() { if (!result) { - ignerr << "Service call [" << serviceInfo.servName << "] failed\n"; + ignerr << "Service call [" << serviceInfo.servName + << "] failed\n"; } } else { - ignerr << "Service call [" << serviceInfo.servName << "] timed out\n"; + ignerr << "Service call [" << serviceInfo.servName + << "] timed out\n"; } } } diff --git a/test/integration/triggered_publisher.cc b/test/integration/triggered_publisher.cc index 79532722a2..0c2d98ec27 100644 --- a/test/integration/triggered_publisher.cc +++ b/test/integration/triggered_publisher.cc @@ -653,7 +653,7 @@ TEST_F(TriggeredPublisherTest, msgs::StringMsg &)>( [&recvCount](const auto &_req, auto &_rep) { - EXPECT_TRUE(_req.data() == "test"); + EXPECT_EQ(_req.data() == "test"); if (_req.data() == "test") { ++recvCount; @@ -695,7 +695,7 @@ TEST_F(TriggeredPublisherTest, msgs::StringMsg &)>( [&recvCount](const auto &_req, auto &_rep) { - EXPECT_TRUE(_req.data() == "test"); + EXPECT_EQ(_req.data() == "test"); if (_req.data() == "test") { ++recvCount; From e0f56ea86c2e745f65f93d1eb17bfcd134ed0fae Mon Sep 17 00:00:00 2001 From: Liam Han Date: Thu, 28 Jul 2022 21:47:58 +0000 Subject: [PATCH 09/26] silly syntax mistake on expect_eq Signed-off-by: Liam Han --- test/integration/triggered_publisher.cc | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/test/integration/triggered_publisher.cc b/test/integration/triggered_publisher.cc index 0c2d98ec27..5f887a421d 100644 --- a/test/integration/triggered_publisher.cc +++ b/test/integration/triggered_publisher.cc @@ -653,7 +653,7 @@ TEST_F(TriggeredPublisherTest, msgs::StringMsg &)>( [&recvCount](const auto &_req, auto &_rep) { - EXPECT_EQ(_req.data() == "test"); + EXPECT_EQ(_req.data(), "test"); if (_req.data() == "test") { ++recvCount; @@ -695,7 +695,7 @@ TEST_F(TriggeredPublisherTest, msgs::StringMsg &)>( [&recvCount](const auto &_req, auto &_rep) { - EXPECT_EQ(_req.data() == "test"); + EXPECT_EQ(_req.data(), "test"); if (_req.data() == "test") { ++recvCount; From a2200575df246603d839d03012806c599cb16ad3 Mon Sep 17 00:00:00 2001 From: Liam Han Date: Mon, 1 Aug 2022 19:39:25 +0000 Subject: [PATCH 10/26] added three more test cases that addesses incorrect response type, incorrect request type and false result Signed-off-by: Liam Han --- .../triggered_publisher/TriggeredPublisher.cc | 25 +++--- test/integration/triggered_publisher.cc | 82 +++++++++++++++++++ test/worlds/triggered_publisher.sdf | 39 +++++++++ 3 files changed, 135 insertions(+), 11 deletions(-) diff --git a/src/systems/triggered_publisher/TriggeredPublisher.cc b/src/systems/triggered_publisher/TriggeredPublisher.cc index f4468f9495..826d773f84 100644 --- a/src/systems/triggered_publisher/TriggeredPublisher.cc +++ b/src/systems/triggered_publisher/TriggeredPublisher.cc @@ -624,36 +624,34 @@ void TriggeredPublisher::Configure(const Entity &, if (serviceInfo.servName.empty()) { ignerr << "Service name cannot be empty\n"; + return; } serviceInfo.reqType = serviceElem->Get("reqType"); if (serviceInfo.reqType.empty()) { ignerr << "Service request type cannot be empty\n"; + return; } serviceInfo.repType = serviceElem->Get("repType"); if (serviceInfo.repType.empty()) { ignerr << "Service response type cannot be empty\n"; + return; } serviceInfo.reqMsg = serviceElem->Get("reqMsg"); if (serviceInfo.reqMsg.empty()) { ignerr << "Service request string cannot be empty\n"; + return; } std::string timeoutInfo = serviceElem->Get("timeout"); if (timeoutInfo.empty()) { - ignwarn << "Timeout value not specified for service name [" - << serviceInfo.servName << "] with service type [" - << serviceInfo.reqType << "]. Using default value of 3000\n"; - // Use default timeout value of 3000ms - serviceInfo.timeout = 3000; - } - else - { - serviceInfo.timeout = std::stoi(timeoutInfo); + ignwarn << "Timeout value cannot be empty\n"; + return; } + serviceInfo.timeout = std::stoi(timeoutInfo); this->serviceOutputInfo.push_back(std::move(serviceInfo)); } } @@ -745,7 +743,7 @@ void TriggeredPublisher::DoServiceWork() auto req = msgs::Factory::New(serviceInfo.reqType, serviceInfo.reqMsg); if (!req) { - ignerr << "Unable to create request for type[" + ignerr << "Unable to create request for type [" << serviceInfo.reqType << "].\n"; return; } @@ -753,7 +751,7 @@ void TriggeredPublisher::DoServiceWork() auto rep = msgs::Factory::New(serviceInfo.repType); if (!rep) { - ignerr << "Unable to create response for type[" + ignerr << "Unable to create response for type [" << serviceInfo.repType << "].\n"; return; } @@ -767,6 +765,11 @@ void TriggeredPublisher::DoServiceWork() ignerr << "Service call [" << serviceInfo.servName << "] failed\n"; } + else + { + ignmsg << "Service call [" << serviceInfo.servName + << "] succeeded\n"; + } } else { diff --git a/test/integration/triggered_publisher.cc b/test/integration/triggered_publisher.cc index 5f887a421d..3014de6052 100644 --- a/test/integration/triggered_publisher.cc +++ b/test/integration/triggered_publisher.cc @@ -778,3 +778,85 @@ TEST_F(TriggeredPublisherTest, EXPECT_EQ(pubCount, std::count(recvMsgs0.begin(), recvMsgs0.end(), true)); EXPECT_EQ(pubCount, std::count(recvMsgs1.begin(), recvMsgs1.end(), false)); } + +///////////////////////////////////////////////// +// Test for triggering a service call with incorrect +// request type or reseponse type. The server callback +// will not be triggered hence the recvCount will be 0 +///////////////////////////////////////////////// +TEST_F(TriggeredPublisherTest, + IGN_UTILS_TEST_DISABLED_ON_WIN32(WrongRequestOrResponseType)) +{ + transport::Node node; + auto inputPub = node.Advertise("/in_16"); + std::atomic recvCount{0}; + + auto srvEchoCb = std::function( + [&recvCount](const auto &_req, auto &_rep) + { + EXPECT_EQ(_req.data(), "test"); + if (_req.data() == "test") + { + ++recvCount; + _rep.set_data(_req.data()); + return true; + } + return false; + }); + + // Advertise a dummy service + std::string service = "/srv-test"; + node.Advertise(service, srvEchoCb); + + const std::size_t pubCount{10}; + for (std::size_t i = 0; i < pubCount; ++i) + { + EXPECT_TRUE(inputPub.Publish(msgs::Empty())); + IGN_SLEEP_MS(100); + } + + waitUntil(5000, [&]{return recvCount == 0u;}); + EXPECT_EQ(0u, recvCount); +} + +///////////////////////////////////////////////// +// Test for triggering a service call that'll return +// False result. The server callback will be triggered +// but it'll +1 to the recvCout but return False. +///////////////////////////////////////////////// +TEST_F(TriggeredPublisherTest, + IGN_UTILS_TEST_DISABLED_ON_WIN32(FailedReesultServiceCall)) +{ + transport::Node node; + auto inputPub = node.Advertise("/in_17"); + std::atomic recvCount{0}; + + auto srvEchoCb = std::function( + [&recvCount](const auto &_req, auto &_rep) + { + EXPECT_EQ(_req.data(), "test"); + if (_req.data() == "test") + { + ++recvCount; + _rep.set_data(_req.data()); + // return True was substitued with False + return false; + } + }); + + // Advertise a dummy service + std::string service = "/srv-test"; + node.Advertise(service, srvEchoCb); + + const std::size_t pubCount{10}; + for (std::size_t i = 0; i < pubCount; ++i) + { + EXPECT_TRUE(inputPub.Publish(msgs::Empty())); + IGN_SLEEP_MS(100); + } + + waitUntil(5000, [&]{return pubCount == recvCount;}); + EXPECT_EQ(pubCount, recvCount); +} diff --git a/test/worlds/triggered_publisher.sdf b/test/worlds/triggered_publisher.sdf index 09892482fb..c355bf4c9a 100644 --- a/test/worlds/triggered_publisher.sdf +++ b/test/worlds/triggered_publisher.sdf @@ -236,6 +236,45 @@ reqMsg="data: false"> + + + + + + + + + + + + + + + + + + From cc4b6659b7560afff2c1d0026ee1c7673158c85b Mon Sep 17 00:00:00 2001 From: Liam Han Date: Wed, 3 Aug 2022 11:17:02 +0000 Subject: [PATCH 11/26] WIP: major restructuring and currently working. Requires more cleanup and test Signed-off-by: Liam Han --- .../triggered_publisher/TriggeredPublisher.cc | 116 +- .../triggered_publisher/TriggeredPublisher.hh | 98 +- test/integration/triggered_publisher.cc | 1527 +++++++++-------- test/worlds/triggered_publisher.sdf | 65 +- 4 files changed, 1029 insertions(+), 777 deletions(-) diff --git a/src/systems/triggered_publisher/TriggeredPublisher.cc b/src/systems/triggered_publisher/TriggeredPublisher.cc index 826d773f84..90d1722819 100644 --- a/src/systems/triggered_publisher/TriggeredPublisher.cc +++ b/src/systems/triggered_publisher/TriggeredPublisher.cc @@ -479,6 +479,28 @@ std::unique_ptr InputMatcher::Create( return matcher; } +////////////////////////////////////////////////// + +void TriggeredPublisher::ResponseHandler::operator() (const msgs::StringMsg &msg, ServiceOutputInfo &srvInfo) +{ + ignmsg << "Service call [" << srvInfo.servName + << "] succeeded. Response message [" << std::boolalpha << msg.data() + << "]\n"; +} +void TriggeredPublisher::ResponseHandler::operator() (const msgs::Boolean &msg, ServiceOutputInfo &srvInfo) +{ + ignmsg << "Service call [" << srvInfo.servName + << "] succeeded. Response message [" << std::boolalpha << msg.data() + << "]\n"; +} +void TriggeredPublisher::ResponseHandler::operator() (const msgs::Empty &msg, ServiceOutputInfo &srvInfo) +{ + ignmsg << "Service call [" << srvInfo.servName + << "] succeeded. Response message [" << msg.unused() + << "]\n"; +} +// NOTE: add more msgs here + ////////////////////////////////////////////////// TriggeredPublisher::~TriggeredPublisher() { @@ -629,14 +651,13 @@ void TriggeredPublisher::Configure(const Entity &, serviceInfo.reqType = serviceElem->Get("reqType"); if (serviceInfo.reqType.empty()) { - ignerr << "Service request type cannot be empty\n"; - return; + ignwarn << "Service request type cannot be empty\n"; } serviceInfo.repType = serviceElem->Get("repType"); if (serviceInfo.repType.empty()) { - ignerr << "Service response type cannot be empty\n"; - return; + ignwarn << "Service response type cannot be empty\n"; + //todo: change to warn } serviceInfo.reqMsg = serviceElem->Get("reqMsg"); if (serviceInfo.reqMsg.empty()) @@ -644,14 +665,14 @@ void TriggeredPublisher::Configure(const Entity &, ignerr << "Service request string cannot be empty\n"; return; } - std::string timeoutInfo = serviceElem->Get("timeout"); - if (timeoutInfo.empty()) - { - ignwarn << "Timeout value cannot be empty\n"; - return; - } - - serviceInfo.timeout = std::stoi(timeoutInfo); + // std::string timeoutInfo = serviceElem->Get("timeout"); + // if (timeoutInfo.empty()) + // { + // ignwarn << "Timeout value cannot be empty\n"; + // return; + // } + + // serviceInfo.timeout = std::stoi(timeoutInfo); this->serviceOutputInfo.push_back(std::move(serviceInfo)); } } @@ -678,11 +699,14 @@ void TriggeredPublisher::Configure(const Entity &, } this->newMatchSignal.notify_one(); } + // only perform when service tag is specified in sdf + ignerr<<"out\n"; if (this->serviceOutputInfo.size() > 0) { { + ignerr<<"in\n"; std::lock_guard lock(this->serviceCountMutex); - ++this->serviceCount; + this->callService = true; } this->serviceMatchSignal.notify_one(); } @@ -714,69 +738,43 @@ void TriggeredPublisher::Configure(const Entity &, } ////////////////////////////////////////////////// + +#define HANDLE_REQUEST(type, typeString) \ +if(serviceInfo.reqType == typeString) \ +{ \ + if(serviceInfo.reqType.empty()) \ + { \ + this->HandleNoInputRequest(serviceInfo); \ + } \ + else \ + { \ + this->HandleRequest(serviceInfo); \ + } \ +} + void TriggeredPublisher::DoServiceWork() { while (!this->srvDone) { - std::size_t pending{0}; { using namespace std::chrono_literals; std::unique_lock lock(this->serviceCountMutex); - this->serviceMatchSignal.wait_for(lock, 1s, + this->serviceMatchSignal.wait(lock, [this] { - return (this->serviceCount > 0) || this->srvDone; + return (this->callService) || this->srvDone; }); - if (this->serviceCount == 0 || this->srvDone) + if (this->srvDone) { continue; } - - std::swap(pending, this->serviceCount); } + ignerr <<"================================\n"; for (auto &serviceInfo : this->serviceOutputInfo) { - for (std::size_t i = 0; i < pending; ++i) - { - bool result; - auto req = msgs::Factory::New(serviceInfo.reqType, serviceInfo.reqMsg); - if (!req) - { - ignerr << "Unable to create request for type [" - << serviceInfo.reqType << "].\n"; - return; - } - - auto rep = msgs::Factory::New(serviceInfo.repType); - if (!rep) - { - ignerr << "Unable to create response for type [" - << serviceInfo.repType << "].\n"; - return; - } - - bool executed = this->node.Request(serviceInfo.servName, *req, - serviceInfo.timeout, *rep, result); - if (executed) - { - if (!result) - { - ignerr << "Service call [" << serviceInfo.servName - << "] failed\n"; - } - else - { - ignmsg << "Service call [" << serviceInfo.servName - << "] succeeded\n"; - } - } - else - { - ignerr << "Service call [" << serviceInfo.servName - << "] timed out\n"; - } - } + HANDLE_REQUEST(msgs::StringMsg, "ignition.msgs.StringMsg"); + HANDLE_REQUEST(msgs::Boolean, "ignition.msgs.Boolean"); } } } diff --git a/src/systems/triggered_publisher/TriggeredPublisher.hh b/src/systems/triggered_publisher/TriggeredPublisher.hh index e075ec1a8a..1bd3c191bf 100644 --- a/src/systems/triggered_publisher/TriggeredPublisher.hh +++ b/src/systems/triggered_publisher/TriggeredPublisher.hh @@ -239,9 +239,6 @@ namespace systems /// \brief Service request message std::string reqMsg; - - /// \brief Service request timeout - unsigned int timeout; }; /// \brief List of InputMatchers @@ -260,7 +257,8 @@ namespace systems private: std::size_t publishCount{0}; /// \brief Counter that tells how many times to call the service - private: std::size_t serviceCount{0}; + //private: std::size_t serviceCount{0}; + private: bool callService{false}; /// \brief Mutex to synchronize access to publishCount private: std::mutex publishCountMutex; @@ -295,8 +293,98 @@ namespace systems /// \brief Mutex to synchronize access to publishQueue private: std::mutex publishQueueMutex; + + private: class ResponseHandler + { + public: virtual void operator() (const msgs::StringMsg &msg, + ServiceOutputInfo &srvInfo); + public: virtual void operator() (const msgs::Boolean &msg, + ServiceOutputInfo &srvInfo); + public: virtual void operator() (const msgs::Empty &msg, + ServiceOutputInfo &srvInfo); + //NOTE: add more msgs here + }; + + private: template + std::function ReplyCallback( + ServiceOutputInfo& serviceInfo) + { + return std::function( + [&serviceInfo] (const ReplyT &msg, const bool res){ + if (res) + { + ResponseHandler()(msg, serviceInfo); + } + else + { + ignerr << "Service call [" << serviceInfo.servName + << "] failed\n"; + } + }); + } + + private: template + bool HandleResponse(ServiceOutputInfo& serviceInfo, RequestT& req) + { + auto reqCb = ReplyCallback(serviceInfo); + return this->node.Request(serviceInfo.servName, req, reqCb); + } + + private: template + bool HandleNoInputRequest(ServiceOutputInfo& serviceInfo) + { + auto reqCb = ReplyCallback(serviceInfo); + return this->node.Request(serviceInfo.servName, reqCb); + } + + private: template + bool HandleNoReply(ServiceOutputInfo& serviceInfo) + { + auto reqCb = ReplyCallback(serviceInfo); + return this->node.Request(serviceInfo.servName, reqCb); + } + + #define HANDLE_REPLY(repT, typeString) \ + if(serviceInfo.repType == typeString) \ + { \ + if(serviceInfo.repType.empty()) \ + { \ + executed = HandleNoReply(serviceInfo); \ + } \ + else \ + { \ + executed = HandleResponse(serviceInfo, *req); \ + } \ + } + private: template + void HandleRequest(ServiceOutputInfo& serviceInfo) + { + bool executed {false}; + auto req = msgs::Factory::New(serviceInfo.reqType, + serviceInfo.reqMsg); + if (!req) + { + ignerr << "Unable to create request for type [" + << serviceInfo.reqType << "].\n"; + return; + } + + HANDLE_REPLY(msgs::StringMsg, "ignition.msgs.StringMsg"); + HANDLE_REPLY(msgs::Boolean, "ignition.msgs.Boolean"); + + if (!executed) + { + ignerr << "Service call [" << serviceInfo.servName + << "] timed out\n"; + } + { + std::lock_guard lock(this->serviceCountMutex); + this->callService = false; + } + ignerr << "=====================\n"; + } }; - } +} } } } diff --git a/test/integration/triggered_publisher.cc b/test/integration/triggered_publisher.cc index 3014de6052..784863a54a 100644 --- a/test/integration/triggered_publisher.cc +++ b/test/integration/triggered_publisher.cc @@ -88,595 +88,785 @@ bool waitUntil(int _timeoutMs, Pred _pred) return false; } -///////////////////////////////////////////////// -/// Check that empty message types do not need any data to be specified in the -/// configuration -// See https://github.com/ignitionrobotics/ign-gazebo/issues/1175 -TEST_F(TriggeredPublisherTest, - IGN_UTILS_TEST_DISABLED_ON_WIN32(EmptyInputEmptyOutput)) -{ - transport::Node node; - auto inputPub = node.Advertise("/in_0"); - std::atomic recvCount{0}; - auto msgCb = std::function( - [&recvCount](const auto &) - { - ++recvCount; - }); - node.Subscribe("/out_0", msgCb); - IGN_SLEEP_MS(100ms); - - const std::size_t pubCount{10}; - for (std::size_t i = 0; i < pubCount; ++i) - { - EXPECT_TRUE(inputPub.Publish(msgs::Empty())); - } - waitUntil(5000, [&]{return pubCount == recvCount;}); - - EXPECT_EQ(pubCount, recvCount); -} - -///////////////////////////////////////////////// -TEST_F(TriggeredPublisherTest, - IGN_UTILS_TEST_DISABLED_ON_WIN32(WrongInputMessageTypeDoesNotMatch)) -{ - transport::Node node; - auto inputPub = node.Advertise("/in_0"); - std::atomic recvCount{0}; - auto msgCb = std::function( - [&recvCount](const auto &) - { - ++recvCount; - }); - node.Subscribe("/out_0", msgCb); - - const std::size_t pubCount{10}; - for (std::size_t i = 0; i < pubCount; ++i) - { - EXPECT_TRUE(inputPub.Publish(msgs::Boolean())); - } - - waitUntil(5000, [&]{return 0u == recvCount;}); - EXPECT_EQ(0u, recvCount); -} - -///////////////////////////////////////////////// -TEST_F(TriggeredPublisherTest, - IGN_UTILS_TEST_DISABLED_ON_WIN32(InputMessagesTriggerOutputs)) -{ - transport::Node node; - auto inputPub = node.Advertise("/in_1"); - std::atomic recvCount{0}; - auto msgCb = std::function( - [&recvCount](const auto &_msg) - { - EXPECT_TRUE(_msg.data()); - ++recvCount; - }); - node.Subscribe("/out_1", msgCb); - - const std::size_t pubCount{10}; - for (std::size_t i = 0; i < pubCount; ++i) - { - EXPECT_TRUE(inputPub.Publish(msgs::Empty())); - IGN_SLEEP_MS(10); - } - - waitUntil(5000, [&]{return pubCount == recvCount;}); - EXPECT_EQ(pubCount, recvCount); -} - -///////////////////////////////////////////////// -TEST_F(TriggeredPublisherTest, - IGN_UTILS_TEST_DISABLED_ON_WIN32(MultipleOutputsForOneInput)) -{ - transport::Node node; - auto inputPub = node.Advertise("/in_2"); - std::mutex recvMsgMutex; - std::vector recvMsgs0; - std::vector recvMsgs1; - auto cbCreator = [&recvMsgMutex](std::vector &_msgVector) - { - return std::function( - [&_msgVector, &recvMsgMutex](const msgs::Boolean &_msg) - { - std::lock_guard lock(recvMsgMutex); - _msgVector.push_back(_msg.data()); - }); - }; - - auto msgCb0 = cbCreator(recvMsgs0); - auto msgCb1 = cbCreator(recvMsgs1); - node.Subscribe("/out_2_0", msgCb0); - node.Subscribe("/out_2_1", msgCb1); - - const int pubCount{10}; - for (int i = 0; i < pubCount; ++i) - { - EXPECT_TRUE(inputPub.Publish(msgs::Empty())); - IGN_SLEEP_MS(10); - } - - waitUntil(5000, [&] - { - std::lock_guard lock(recvMsgMutex); - return static_cast(pubCount) == recvMsgs0.size() && - static_cast(pubCount) == recvMsgs1.size(); - }); - - EXPECT_EQ(static_cast(pubCount), recvMsgs0.size()); - EXPECT_EQ(static_cast(pubCount), recvMsgs1.size()); - - // The plugin has two outputs. We expect 10 messages in each output topic - EXPECT_EQ(pubCount, std::count(recvMsgs0.begin(), recvMsgs0.end(), false)); - EXPECT_EQ(pubCount, std::count(recvMsgs1.begin(), recvMsgs1.end(), true)); -} - -///////////////////////////////////////////////// -TEST_F(TriggeredPublisherTest, - IGN_UTILS_TEST_DISABLED_ON_WIN32(ExactMatchBooleanInputs)) -{ - transport::Node node; - auto inputPub = node.Advertise("/in_3"); - std::atomic recvCount{0}; - auto msgCb = std::function( - [&recvCount](const auto &) - { - ++recvCount; - }); - node.Subscribe("/out_3", msgCb); - - const std::size_t pubCount{10}; - const std::size_t trueCount{5}; - for (std::size_t i = 0; i < pubCount; ++i) - { - if (i < trueCount) - { - EXPECT_TRUE(inputPub.Publish(msgs::Convert(true))); - } - else - { - EXPECT_TRUE(inputPub.Publish(msgs::Convert(false))); - } - IGN_SLEEP_MS(10); - } - - // The matcher filters out false messages and the inputs consist of 5 true and - // 5 false messages, so we expect 5 output messages - EXPECT_EQ(trueCount, recvCount); -} - -///////////////////////////////////////////////// -TEST_F(TriggeredPublisherTest, - IGN_UTILS_TEST_DISABLED_ON_WIN32(MatchersWithLogicTypeAttribute)) -{ - transport::Node node; - auto inputPub = node.Advertise("/in_4"); - std::atomic recvCount[2]{0, 0}; - - auto cbCreator = [](std::atomic &_counter) - { - return std::function( - [&_counter](const msgs::Empty &) - { - ++_counter; - }); - }; - - auto msgCb0 = cbCreator(recvCount[0]); - auto msgCb1 = cbCreator(recvCount[1]); - node.Subscribe("/out_4_0", msgCb0); - node.Subscribe("/out_4_1", msgCb1); - - const int pubCount{10}; - for (int i = 0; i < pubCount; ++i) - { - EXPECT_TRUE(inputPub.Publish( - msgs::Convert(static_cast(i - pubCount / 2)))); - IGN_SLEEP_MS(10); - } - // The negative matcher filters out 0 so we expect 9 output messages from the - // 10 inputs - EXPECT_EQ(9u, recvCount[0]); - - // The positive matcher only accepts the input value 0 - EXPECT_EQ(1u, recvCount[1]); -} - -///////////////////////////////////////////////// -TEST_F(TriggeredPublisherTest, - IGN_UTILS_TEST_DISABLED_ON_WIN32(MultipleMatchersAreAnded)) -{ - transport::Node node; - auto inputPub = node.Advertise("/in_5"); - std::atomic recvCount{0}; - auto msgCb = std::function( - [&recvCount](const auto &) - { - ++recvCount; - }); - node.Subscribe("/out_5", msgCb); - - const int pubCount{10}; - for (int i = 0; i < pubCount; ++i) - { - EXPECT_TRUE(inputPub.Publish( - msgs::Convert(static_cast(i - pubCount / 2)))); - IGN_SLEEP_MS(10); - } - // The matcher filters out negative numbers and the input is [-5,4], so we - // expect 5 output messages. - EXPECT_EQ(5u, recvCount); -} - -///////////////////////////////////////////////// -TEST_F(TriggeredPublisherTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(FieldMatchers)) -{ - transport::Node node; - auto inputPub = node.Advertise("/in_6"); - std::atomic recvCount[2]{0, 0}; - - auto cbCreator = [](std::atomic &_counter) - { - return std::function( - [&_counter](const msgs::Empty &) - { - ++_counter; - }); - }; - - auto msgCb0 = cbCreator(recvCount[0]); - auto msgCb1 = cbCreator(recvCount[1]); - node.Subscribe("/out_6_0", msgCb0); - node.Subscribe("/out_6_1", msgCb1); - - const int pubCount{10}; - msgs::Vector2d msg; - msg.set_x(1.0); - for (int i = 0; i < pubCount; ++i) - { - msg.set_y(static_cast(i)); - EXPECT_TRUE(inputPub.Publish(msg)); - IGN_SLEEP_MS(10); - } - - // The first plugin matches x==1 and y==2 which only once out of the 10 inputs - EXPECT_EQ(1u, recvCount[0]); - // The second plugin matches x==1 and y!=2 which occurs 9 out of the 10 inputs - EXPECT_EQ(9u, recvCount[1]); -} - -///////////////////////////////////////////////// -/// Tests that if the specified field is a repeated field, a partial match is -/// used when comparing against the input. -TEST_F(TriggeredPublisherTest, - IGN_UTILS_TEST_DISABLED_ON_WIN32( - FieldMatchersWithRepeatedFieldsUsePartialMatches)) -{ - transport::Node node; - auto inputPub = node.Advertise("/in_7"); - std::atomic recvCount{0}; - auto msgCb = std::function( - [&recvCount](const auto &) - { - ++recvCount; - }); - node.Subscribe("/out_7", msgCb); +/////////////////////////////////////////////////// +///// Check that empty message types do not need any data to be specified in the +///// configuration +//// See https://github.com/ignitionrobotics/ign-gazebo/issues/1175 +//TEST_F(TriggeredPublisherTest, +// IGN_UTILS_TEST_DISABLED_ON_WIN32(EmptyInputEmptyOutput)) +//{ +// transport::Node node; +// auto inputPub = node.Advertise("/in_0"); +// std::atomic recvCount{0}; +// auto msgCb = std::function( +// [&recvCount](const auto &) +// { +// ++recvCount; +// }); +// node.Subscribe("/out_0", msgCb); +// IGN_SLEEP_MS(100ms); +// +// const std::size_t pubCount{10}; +// for (std::size_t i = 0; i < pubCount; ++i) +// { +// EXPECT_TRUE(inputPub.Publish(msgs::Empty())); +// } +// waitUntil(5000, [&]{return pubCount == recvCount;}); +// +// EXPECT_EQ(pubCount, recvCount); +//} +// +/////////////////////////////////////////////////// +//TEST_F(TriggeredPublisherTest, +// IGN_UTILS_TEST_DISABLED_ON_WIN32(WrongInputMessageTypeDoesNotMatch)) +//{ +// transport::Node node; +// auto inputPub = node.Advertise("/in_0"); +// std::atomic recvCount{0}; +// auto msgCb = std::function( +// [&recvCount](const auto &) +// { +// ++recvCount; +// }); +// node.Subscribe("/out_0", msgCb); +// +// const std::size_t pubCount{10}; +// for (std::size_t i = 0; i < pubCount; ++i) +// { +// EXPECT_TRUE(inputPub.Publish(msgs::Boolean())); +// } +// +// waitUntil(5000, [&]{return 0u == recvCount;}); +// EXPECT_EQ(0u, recvCount); +//} +// +/////////////////////////////////////////////////// +//TEST_F(TriggeredPublisherTest, +// IGN_UTILS_TEST_DISABLED_ON_WIN32(InputMessagesTriggerOutputs)) +//{ +// transport::Node node; +// auto inputPub = node.Advertise("/in_1"); +// std::atomic recvCount{0}; +// auto msgCb = std::function( +// [&recvCount](const auto &_msg) +// { +// EXPECT_TRUE(_msg.data()); +// ++recvCount; +// }); +// node.Subscribe("/out_1", msgCb); +// +// const std::size_t pubCount{10}; +// for (std::size_t i = 0; i < pubCount; ++i) +// { +// EXPECT_TRUE(inputPub.Publish(msgs::Empty())); +// IGN_SLEEP_MS(10); +// } +// +// waitUntil(5000, [&]{return pubCount == recvCount;}); +// EXPECT_EQ(pubCount, recvCount); +//} +// +/////////////////////////////////////////////////// +//TEST_F(TriggeredPublisherTest, +// IGN_UTILS_TEST_DISABLED_ON_WIN32(MultipleOutputsForOneInput)) +//{ +// transport::Node node; +// auto inputPub = node.Advertise("/in_2"); +// std::mutex recvMsgMutex; +// std::vector recvMsgs0; +// std::vector recvMsgs1; +// auto cbCreator = [&recvMsgMutex](std::vector &_msgVector) +// { +// return std::function( +// [&_msgVector, &recvMsgMutex](const msgs::Boolean &_msg) +// { +// std::lock_guard lock(recvMsgMutex); +// _msgVector.push_back(_msg.data()); +// }); +// }; +// +// auto msgCb0 = cbCreator(recvMsgs0); +// auto msgCb1 = cbCreator(recvMsgs1); +// node.Subscribe("/out_2_0", msgCb0); +// node.Subscribe("/out_2_1", msgCb1); +// +// const int pubCount{10}; +// for (int i = 0; i < pubCount; ++i) +// { +// EXPECT_TRUE(inputPub.Publish(msgs::Empty())); +// IGN_SLEEP_MS(10); +// } +// +// waitUntil(5000, [&] +// { +// std::lock_guard lock(recvMsgMutex); +// return static_cast(pubCount) == recvMsgs0.size() && +// static_cast(pubCount) == recvMsgs1.size(); +// }); +// +// EXPECT_EQ(static_cast(pubCount), recvMsgs0.size()); +// EXPECT_EQ(static_cast(pubCount), recvMsgs1.size()); +// +// // The plugin has two outputs. We expect 10 messages in each output topic +// EXPECT_EQ(pubCount, std::count(recvMsgs0.begin(), recvMsgs0.end(), false)); +// EXPECT_EQ(pubCount, std::count(recvMsgs1.begin(), recvMsgs1.end(), true)); +//} +// +/////////////////////////////////////////////////// +//TEST_F(TriggeredPublisherTest, +// IGN_UTILS_TEST_DISABLED_ON_WIN32(ExactMatchBooleanInputs)) +//{ +// transport::Node node; +// auto inputPub = node.Advertise("/in_3"); +// std::atomic recvCount{0}; +// auto msgCb = std::function( +// [&recvCount](const auto &) +// { +// ++recvCount; +// }); +// node.Subscribe("/out_3", msgCb); +// +// const std::size_t pubCount{10}; +// const std::size_t trueCount{5}; +// for (std::size_t i = 0; i < pubCount; ++i) +// { +// if (i < trueCount) +// { +// EXPECT_TRUE(inputPub.Publish(msgs::Convert(true))); +// } +// else +// { +// EXPECT_TRUE(inputPub.Publish(msgs::Convert(false))); +// } +// IGN_SLEEP_MS(10); +// } +// +// // The matcher filters out false messages and the inputs consist of 5 true and +// // 5 false messages, so we expect 5 output messages +// EXPECT_EQ(trueCount, recvCount); +//} +// +/////////////////////////////////////////////////// +//TEST_F(TriggeredPublisherTest, +// IGN_UTILS_TEST_DISABLED_ON_WIN32(MatchersWithLogicTypeAttribute)) +//{ +// transport::Node node; +// auto inputPub = node.Advertise("/in_4"); +// std::atomic recvCount[2]{0, 0}; +// +// auto cbCreator = [](std::atomic &_counter) +// { +// return std::function( +// [&_counter](const msgs::Empty &) +// { +// ++_counter; +// }); +// }; +// +// auto msgCb0 = cbCreator(recvCount[0]); +// auto msgCb1 = cbCreator(recvCount[1]); +// node.Subscribe("/out_4_0", msgCb0); +// node.Subscribe("/out_4_1", msgCb1); +// +// const int pubCount{10}; +// for (int i = 0; i < pubCount; ++i) +// { +// EXPECT_TRUE(inputPub.Publish( +// msgs::Convert(static_cast(i - pubCount / 2)))); +// IGN_SLEEP_MS(10); +// } +// // The negative matcher filters out 0 so we expect 9 output messages from the +// // 10 inputs +// EXPECT_EQ(9u, recvCount[0]); +// +// // The positive matcher only accepts the input value 0 +// EXPECT_EQ(1u, recvCount[1]); +//} +// +/////////////////////////////////////////////////// +//TEST_F(TriggeredPublisherTest, +// IGN_UTILS_TEST_DISABLED_ON_WIN32(MultipleMatchersAreAnded)) +//{ +// transport::Node node; +// auto inputPub = node.Advertise("/in_5"); +// std::atomic recvCount{0}; +// auto msgCb = std::function( +// [&recvCount](const auto &) +// { +// ++recvCount; +// }); +// node.Subscribe("/out_5", msgCb); +// +// const int pubCount{10}; +// for (int i = 0; i < pubCount; ++i) +// { +// EXPECT_TRUE(inputPub.Publish( +// msgs::Convert(static_cast(i - pubCount / 2)))); +// IGN_SLEEP_MS(10); +// } +// // The matcher filters out negative numbers and the input is [-5,4], so we +// // expect 5 output messages. +// EXPECT_EQ(5u, recvCount); +//} +// +/////////////////////////////////////////////////// +//TEST_F(TriggeredPublisherTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(FieldMatchers)) +//{ +// transport::Node node; +// auto inputPub = node.Advertise("/in_6"); +// std::atomic recvCount[2]{0, 0}; +// +// auto cbCreator = [](std::atomic &_counter) +// { +// return std::function( +// [&_counter](const msgs::Empty &) +// { +// ++_counter; +// }); +// }; +// +// auto msgCb0 = cbCreator(recvCount[0]); +// auto msgCb1 = cbCreator(recvCount[1]); +// node.Subscribe("/out_6_0", msgCb0); +// node.Subscribe("/out_6_1", msgCb1); +// +// const int pubCount{10}; +// msgs::Vector2d msg; +// msg.set_x(1.0); +// for (int i = 0; i < pubCount; ++i) +// { +// msg.set_y(static_cast(i)); +// EXPECT_TRUE(inputPub.Publish(msg)); +// IGN_SLEEP_MS(10); +// } +// +// // The first plugin matches x==1 and y==2 which only once out of the 10 inputs +// EXPECT_EQ(1u, recvCount[0]); +// // The second plugin matches x==1 and y!=2 which occurs 9 out of the 10 inputs +// EXPECT_EQ(9u, recvCount[1]); +//} +// +/////////////////////////////////////////////////// +///// Tests that if the specified field is a repeated field, a partial match is +///// used when comparing against the input. +//TEST_F(TriggeredPublisherTest, +// IGN_UTILS_TEST_DISABLED_ON_WIN32( +// FieldMatchersWithRepeatedFieldsUsePartialMatches)) +//{ +// transport::Node node; +// auto inputPub = node.Advertise("/in_7"); +// std::atomic recvCount{0}; +// auto msgCb = std::function( +// [&recvCount](const auto &) +// { +// ++recvCount; +// }); +// node.Subscribe("/out_7", msgCb); +// +// const int pubCount{10}; +// for (int i = 0; i < pubCount; ++i) +// { +// msgs::Pose poseMsg; +// auto *frame = poseMsg.mutable_header()->add_data(); +// frame->set_key("frame_id"); +// frame->add_value(std::string("frame") + std::to_string(i)); +// +// auto *time = poseMsg.mutable_header()->mutable_stamp(); +// time->set_sec(10); +// +// auto *other = poseMsg.mutable_header()->add_data(); +// other->set_key("other_key"); +// other->add_value("other_value"); +// EXPECT_TRUE(inputPub.Publish(poseMsg)); +// IGN_SLEEP_MS(10); +// } +// +// // The matcher filters out frame ids that are not frame0, so we expect 1 +// // output. Even though the data field contains other key-value pairs, since +// // repeated fields use partial matching, the matcher will match one of the +// // inputs. +// EXPECT_EQ(1u, recvCount); +//} +// +//TEST_F(TriggeredPublisherTest, +// IGN_UTILS_TEST_DISABLED_ON_WIN32(WrongInputWhenRepeatedSubFieldExpected)) +//{ +// transport::Node node; +// auto inputPub = node.Advertise("/in_7"); +// std::atomic recvCount{0}; +// auto msgCb = std::function( +// [&recvCount](const auto &) +// { +// ++recvCount; +// }); +// node.Subscribe("/out_7", msgCb); +// IGN_SLEEP_MS(10); +// +// const int pubCount{10}; +// msgs::Empty msg; +// for (int i = 0; i < pubCount; ++i) +// { +// EXPECT_TRUE(inputPub.Publish(msg)); +// IGN_SLEEP_MS(10); +// } +// +// EXPECT_EQ(0u, recvCount); +//} +// +/////////////////////////////////////////////////// +///// Tests that field matchers can be used to do a full match with repeated +///// fields by specifying the containing field of the repeated field in the +///// "field" attribute and setting the desired values of the repeated field in +///// the value of the tag. +//TEST_F(TriggeredPublisherTest, IGN_UTILS_TEST_DISABLED_ON_WIN32( +// FieldMatchersWithRepeatedFieldsInValueUseFullMatches)) +//{ +// transport::Node node; +// auto inputPub = node.Advertise("/in_8"); +// std::atomic recvCount{0}; +// auto msgCb = std::function( +// [&recvCount](const auto &) +// { +// ++recvCount; +// }); +// node.Subscribe("/out_8", msgCb); +// IGN_SLEEP_MS(10); +// +// const int pubCount{10}; +// for (int i = 0; i < pubCount; ++i) +// { +// msgs::Pose poseMsg; +// auto *frame = poseMsg.mutable_header()->add_data(); +// frame->set_key("frame_id"); +// frame->add_value("frame0"); +// +// if (i < 5) +// { +// auto *other = poseMsg.mutable_header()->add_data(); +// other->set_key("other_key"); +// other->add_value("other_value"); +// } +// EXPECT_TRUE(inputPub.Publish(poseMsg)); +// IGN_SLEEP_MS(10); +// } +// +// // Since the field specified in "field" is not a repeated field, a full match +// // is required to trigger an output. Only the first 5 input messages have the +// // second "data" entry, so the expected recvCount is 5. +// EXPECT_EQ(5u, recvCount); +//} +// +/////////////////////////////////////////////////// +///// Tests that full matchers can be used with repeated fields by specifying the +///// desired values of the repeated field in the value of the tag. The +///// message created from the value of must be a full match of the input. +//TEST_F(TriggeredPublisherTest, IGN_UTILS_TEST_DISABLED_ON_WIN32( +// FullMatchersWithRepeatedFieldsInValueUseFullMatches)) +//{ +// transport::Node node; +// auto inputPub = node.Advertise("/in_9"); +// std::atomic recvCount{0}; +// auto msgCb = std::function( +// [&recvCount](const auto &) +// { +// ++recvCount; +// }); +// node.Subscribe("/out_9", msgCb); +// IGN_SLEEP_MS(10); +// +// const int pubCount{10}; +// msgs::Int32_V msg; +// for (int i = 0; i < pubCount; ++i) +// { +// msg.add_data(i); +// EXPECT_TRUE(inputPub.Publish(msg)); +// IGN_SLEEP_MS(10); +// } +// +// // The input contains an increasing sets of sequences, {0}, {0,1}, {0,1,2}... +// // The matcher only matches {0,1} +// EXPECT_EQ(1u, recvCount); +//} +// +//TEST_F(TriggeredPublisherTest, +// IGN_UTILS_TEST_DISABLED_ON_WIN32(FullMatchersAcceptToleranceParam)) +//{ +// transport::Node node; +// auto inputPub = node.Advertise("/in_10"); +// std::atomic recvCount{0}; +// auto msgCb = std::function( +// [&recvCount](const auto &) +// { +// ++recvCount; +// }); +// node.Subscribe("/out_10", msgCb); +// IGN_SLEEP_MS(10); +// +// const int pubCount{10}; +// msgs::Float msg; +// for (int i = 0; i < pubCount; ++i) +// { +// msg.set_data(static_cast(i)* 0.1); +// EXPECT_TRUE(inputPub.Publish(msg)); +// IGN_SLEEP_MS(10); +// } +// +// // The input contains the sequence {0, 0.1, 0.2, ...}, the matcher is set to +// // match 0.5 with a tolerance of 0.15, so it should match {0.4, 0.5, 0.6} +// EXPECT_EQ(3u, recvCount); +//} +// +//TEST_F(TriggeredPublisherTest, +// IGN_UTILS_TEST_DISABLED_ON_WIN32(FieldMatchersAcceptToleranceParam)) +//{ +// transport::Node node; +// auto inputPub = node.Advertise("/in_11"); +// std::atomic recvCount{0}; +// auto msgCb = std::function( +// [&recvCount](const auto &) +// { +// ++recvCount; +// }); +// node.Subscribe("/out_11", msgCb); +// IGN_SLEEP_MS(10); +// +// const int pubCount{10}; +// msgs::Pose msg; +// for (int i = 0; i < pubCount; ++i) +// { +// msg.mutable_position()->set_x(0.1); +// msg.mutable_position()->set_z(static_cast(i)* 0.1); +// EXPECT_TRUE(inputPub.Publish(msg)); +// IGN_SLEEP_MS(10); +// } +// +// // The input contains the sequence {0, 0.1, 0.2, ...} in position.z, the +// // matcher is set to match 0.5 with a tolerance of 0.15, so it should match +// // {0.4, 0.5, 0.6} +// EXPECT_EQ(3u, recvCount); +//} +// +//TEST_F(TriggeredPublisherTest, +// IGN_UTILS_TEST_DISABLED_ON_WIN32(SubfieldsOfRepeatedFieldsNotSupported)) +//{ +// transport::Node node; +// auto inputPub = node.Advertise("/in_12"); +// std::atomic recvCount{0}; +// auto msgCb = std::function( +// [&recvCount](const auto &) +// { +// ++recvCount; +// }); +// node.Subscribe("/out_12", msgCb); +// IGN_SLEEP_MS(10); +// +// const int pubCount{10}; +// for (int i = 0; i < pubCount; ++i) +// { +// msgs::Header msg; +// auto *data = msg.add_data(); +// data->set_key("key1"); +// data->add_value("value1"); +// +// EXPECT_TRUE(inputPub.Publish(msg)); +// IGN_SLEEP_MS(10); +// } +// +// // Subfields of repeated fiealds are not supported, so no output should be +// // triggered. +// EXPECT_EQ(0u, recvCount); +//} +// +//TEST_F(TriggeredPublisherTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(TriggerDelay)) +//{ +// transport::Node node; +// auto inputPub = node.Advertise("/in_13"); +// std::atomic recvCount{0}; +// auto msgCb = std::function( +// [&recvCount](const auto &) +// { +// ++recvCount; +// }); +// node.Subscribe("/out_13", msgCb); +// IGN_SLEEP_MS(100ms); +// +// const std::size_t pubCount{10}; +// for (std::size_t i = 0; i < pubCount; ++i) +// { +// EXPECT_TRUE(inputPub.Publish(msgs::Empty())); +// } +// waitUntil(1000, [&]{return pubCount == recvCount;}); +// +// // Delay has been specified, but simulation is not running. No messages +// // should have been received. +// EXPECT_EQ(0u, recvCount); +// +// // The simulation delay is 1000ms, which is equal to 1000 steps. Run +// // for 999 steps, and the count should still be zero. Take one additional +// // step and all the messages should arrive. +// this->server->Run(true, 999, false); +// waitUntil(1000, [&]{return pubCount == recvCount;}); +// EXPECT_EQ(0u, recvCount); +// this->server->Run(true, 1, false); +// waitUntil(1000, [&]{return pubCount == recvCount;}); +// EXPECT_EQ(pubCount, recvCount); +//} +// +//TEST_F(TriggeredPublisherTest, +// IGN_UTILS_TEST_DISABLED_ON_WIN32(WrongInputWhenRepeatedFieldExpected)) +//{ +// transport::Node node; +// auto inputPub = node.Advertise("/invalid_topic"); +// std::atomic recvCount{0}; +// auto msgCb = std::function( +// [&recvCount](const auto &) +// { +// ++recvCount; +// }); +// node.Subscribe("/out_9", msgCb); +// IGN_SLEEP_MS(10); +// +// const int pubCount{10}; +// msgs::Int32 msg; +// for (int i = 0; i < pubCount; ++i) +// { +// msg.set_data(i); +// EXPECT_TRUE(inputPub.Publish(msg)); +// IGN_SLEEP_MS(10); +// } +// +// EXPECT_EQ(0u, recvCount); +//} +// +/////////////////////////////////////////////////// +//// Test for invalid service name. It'll timeout +//// when matching service name can't be found. +/////////////////////////////////////////////////// +//TEST_F(TriggeredPublisherTest, +// IGN_UTILS_TEST_DISABLED_ON_WIN32(InvalidServiceName)) +//{ +// transport::Node node; +// auto inputPub = node.Advertise("/in_14"); +// std::atomic recvCount{0}; +// +// auto srvEchoCb = std::function( +// [&recvCount](const auto &_req, auto &_rep) +// { +// EXPECT_EQ(_req.data(), "test"); +// if (_req.data() == "test") +// { +// ++recvCount; +// _rep.set_data(_req.data()); +// return true; +// } +// return false; +// }); +// +// // Advertise a dummy service +// std::string service = "/srv-dummy-test"; +// node.Advertise(service, srvEchoCb); +// +// const std::size_t pubCount{10}; +// for (std::size_t i = 0; i < pubCount; ++i) +// { +// EXPECT_TRUE(inputPub.Publish(msgs::Empty())); +// IGN_SLEEP_MS(100); +// } +// +// waitUntil(5000, [&]{return recvCount == 0u;}); +// EXPECT_EQ(recvCount, 0u); +//} +// +/////////////////////////////////////////////////// +//// Test for triggering a service call in response +//// to input ign msg by publishing 10 times. Service +//// call will also occur 10 times. It'll compare +//// pubCount and recvCount. +/////////////////////////////////////////////////// +//TEST_F(TriggeredPublisherTest, +// IGN_UTILS_TEST_DISABLED_ON_WIN32(InputMessagesTriggerServices)) +//{ +// transport::Node node; +// auto inputPub = node.Advertise("/in_14"); +// std::atomic recvCount{0}; +// +// auto srvEchoCb = std::function( +// [&recvCount](const auto &_req, auto &_rep) +// { +// EXPECT_EQ(_req.data(), "test"); +// if (_req.data() == "test") +// { +// ++recvCount; +// _rep.set_data(_req.data()); +// return true; +// } +// return false; +// }); +// +// // Advertise a dummy service +// std::string service = "/srv-test"; +// node.Advertise(service, srvEchoCb); +// +// const std::size_t pubCount{10}; +// for (std::size_t i = 0; i < pubCount; ++i) +// { +// EXPECT_TRUE(inputPub.Publish(msgs::Empty())); +// IGN_SLEEP_MS(100); +// } +// +// waitUntil(5000, [&]{return pubCount == recvCount;}); +// EXPECT_EQ(pubCount, recvCount); +//} +// +/////////////////////////////////////////////////// +//// Test for triggering multiple services in response +//// to a input ign msg by publishing 10 times. Service +//// call will also occur 10 times for each service. +//// It'll compare pubCount and recvCount for each +//// services. +/////////////////////////////////////////////////// +//TEST_F(TriggeredPublisherTest, +// IGN_UTILS_TEST_DISABLED_ON_WIN32(MultipleServiceForOneInput)) +//{ +// transport::Node node; +// auto inputPub = node.Advertise("/in_15"); +// std::mutex recvMsgMutex; +// std::vector recvMsgs0; +// std::vector recvMsgs1; +// auto cbCreator = [&recvMsgMutex](std::vector &_msgVector) +// { +// return std::function( +// [&_msgVector, &recvMsgMutex](const auto &_req, auto &_rep) +// { +// std::lock_guard lock(recvMsgMutex); +// if (_req.data() || !_req.data()) +// { +// _msgVector.push_back(_req.data()); +// return true; +// } +// return false; +// }); +// }; +// +// auto msgCb0 = cbCreator(recvMsgs0); +// auto msgCb1 = cbCreator(recvMsgs1); +// +// // Advertise two dummy services +// node.Advertise("/srv-test-0", msgCb0); +// node.Advertise("/srv-test-1", msgCb1); +// +// const int pubCount{10}; +// for (int i = 0; i < pubCount; ++i) +// { +// EXPECT_TRUE(inputPub.Publish(msgs::Empty())); +// IGN_SLEEP_MS(10); +// } +// +// waitUntil(5000, [&] +// { +// std::lock_guard lock(recvMsgMutex); +// return static_cast(pubCount) == recvMsgs0.size() && +// static_cast(pubCount) == recvMsgs1.size(); +// }); +// +// EXPECT_EQ(static_cast(pubCount), recvMsgs0.size()); +// EXPECT_EQ(static_cast(pubCount), recvMsgs1.size()); +// +// // The plugin has two outputs. We expect 10 messages in each output topic +// EXPECT_EQ(pubCount, std::count(recvMsgs0.begin(), recvMsgs0.end(), true)); +// EXPECT_EQ(pubCount, std::count(recvMsgs1.begin(), recvMsgs1.end(), false)); +//} +// +/////////////////////////////////////////////////// +//// Test for triggering a service call with incorrect +//// request type or reseponse type. The server callback +//// will not be triggered hence the recvCount will be 0 +/////////////////////////////////////////////////// +//TEST_F(TriggeredPublisherTest, +// IGN_UTILS_TEST_DISABLED_ON_WIN32(WrongRequestOrResponseType)) +//{ +// transport::Node node; +// auto inputPub = node.Advertise("/in_16"); +// std::atomic recvCount{0}; +// +// auto srvEchoCb = std::function( +// [&recvCount](const auto &_req, auto &_rep) +// { +// EXPECT_EQ(_req.data(), "test"); +// if (_req.data() == "test") +// { +// ++recvCount; +// _rep.set_data(_req.data()); +// return true; +// } +// return false; +// }); +// +// // Advertise a dummy service +// std::string service = "/srv-test"; +// node.Advertise(service, srvEchoCb); +// +// const std::size_t pubCount{10}; +// for (std::size_t i = 0; i < pubCount; ++i) +// { +// EXPECT_TRUE(inputPub.Publish(msgs::Empty())); +// IGN_SLEEP_MS(100); +// } +// +// waitUntil(5000, [&]{return recvCount == 0u;}); +// EXPECT_EQ(0u, recvCount); +//} +// +/////////////////////////////////////////////////// +//// Test for triggering a service call that'll return +//// False result. The server callback will be triggered +//// but it'll +1 to the recvCout but return False. +/////////////////////////////////////////////////// +//TEST_F(TriggeredPublisherTest, +// IGN_UTILS_TEST_DISABLED_ON_WIN32(FailedReesultServiceCall)) +//{ +// transport::Node node; +// auto inputPub = node.Advertise("/in_17"); +// std::atomic recvCount{0}; +// +// auto srvEchoCb = std::function( +// [&recvCount](const auto &_req, auto &_rep) +// { +// EXPECT_EQ(_req.data(), "test"); +// if (_req.data() == "test") +// { +// ++recvCount; +// _rep.set_data(_req.data()); +// // return True was substitued with False +// return false; +// } +// }); +// +// // Advertise a dummy service +// std::string service = "/srv-test"; +// node.Advertise(service, srvEchoCb); +// +// const std::size_t pubCount{10}; +// for (std::size_t i = 0; i < pubCount; ++i) +// { +// EXPECT_TRUE(inputPub.Publish(msgs::Empty())); +// IGN_SLEEP_MS(100); +// } +// +// waitUntil(5000, [&]{return pubCount == recvCount;}); +// EXPECT_EQ(pubCount, recvCount); +//} - const int pubCount{10}; - for (int i = 0; i < pubCount; ++i) - { - msgs::Pose poseMsg; - auto *frame = poseMsg.mutable_header()->add_data(); - frame->set_key("frame_id"); - frame->add_value(std::string("frame") + std::to_string(i)); - auto *time = poseMsg.mutable_header()->mutable_stamp(); - time->set_sec(10); - auto *other = poseMsg.mutable_header()->add_data(); - other->set_key("other_key"); - other->add_value("other_value"); - EXPECT_TRUE(inputPub.Publish(poseMsg)); - IGN_SLEEP_MS(10); - } - // The matcher filters out frame ids that are not frame0, so we expect 1 - // output. Even though the data field contains other key-value pairs, since - // repeated fields use partial matching, the matcher will match one of the - // inputs. - EXPECT_EQ(1u, recvCount); -} -TEST_F(TriggeredPublisherTest, - IGN_UTILS_TEST_DISABLED_ON_WIN32(WrongInputWhenRepeatedSubFieldExpected)) -{ - transport::Node node; - auto inputPub = node.Advertise("/in_7"); - std::atomic recvCount{0}; - auto msgCb = std::function( - [&recvCount](const auto &) - { - ++recvCount; - }); - node.Subscribe("/out_7", msgCb); - IGN_SLEEP_MS(10); - const int pubCount{10}; - msgs::Empty msg; - for (int i = 0; i < pubCount; ++i) - { - EXPECT_TRUE(inputPub.Publish(msg)); - IGN_SLEEP_MS(10); - } - EXPECT_EQ(0u, recvCount); -} - -///////////////////////////////////////////////// -/// Tests that field matchers can be used to do a full match with repeated -/// fields by specifying the containing field of the repeated field in the -/// "field" attribute and setting the desired values of the repeated field in -/// the value of the tag. -TEST_F(TriggeredPublisherTest, IGN_UTILS_TEST_DISABLED_ON_WIN32( - FieldMatchersWithRepeatedFieldsInValueUseFullMatches)) -{ - transport::Node node; - auto inputPub = node.Advertise("/in_8"); - std::atomic recvCount{0}; - auto msgCb = std::function( - [&recvCount](const auto &) - { - ++recvCount; - }); - node.Subscribe("/out_8", msgCb); - IGN_SLEEP_MS(10); - - const int pubCount{10}; - for (int i = 0; i < pubCount; ++i) - { - msgs::Pose poseMsg; - auto *frame = poseMsg.mutable_header()->add_data(); - frame->set_key("frame_id"); - frame->add_value("frame0"); - - if (i < 5) - { - auto *other = poseMsg.mutable_header()->add_data(); - other->set_key("other_key"); - other->add_value("other_value"); - } - EXPECT_TRUE(inputPub.Publish(poseMsg)); - IGN_SLEEP_MS(10); - } - - // Since the field specified in "field" is not a repeated field, a full match - // is required to trigger an output. Only the first 5 input messages have the - // second "data" entry, so the expected recvCount is 5. - EXPECT_EQ(5u, recvCount); -} - -///////////////////////////////////////////////// -/// Tests that full matchers can be used with repeated fields by specifying the -/// desired values of the repeated field in the value of the tag. The -/// message created from the value of must be a full match of the input. -TEST_F(TriggeredPublisherTest, IGN_UTILS_TEST_DISABLED_ON_WIN32( - FullMatchersWithRepeatedFieldsInValueUseFullMatches)) -{ - transport::Node node; - auto inputPub = node.Advertise("/in_9"); - std::atomic recvCount{0}; - auto msgCb = std::function( - [&recvCount](const auto &) - { - ++recvCount; - }); - node.Subscribe("/out_9", msgCb); - IGN_SLEEP_MS(10); - - const int pubCount{10}; - msgs::Int32_V msg; - for (int i = 0; i < pubCount; ++i) - { - msg.add_data(i); - EXPECT_TRUE(inputPub.Publish(msg)); - IGN_SLEEP_MS(10); - } - - // The input contains an increasing sets of sequences, {0}, {0,1}, {0,1,2}... - // The matcher only matches {0,1} - EXPECT_EQ(1u, recvCount); -} - -TEST_F(TriggeredPublisherTest, - IGN_UTILS_TEST_DISABLED_ON_WIN32(FullMatchersAcceptToleranceParam)) -{ - transport::Node node; - auto inputPub = node.Advertise("/in_10"); - std::atomic recvCount{0}; - auto msgCb = std::function( - [&recvCount](const auto &) - { - ++recvCount; - }); - node.Subscribe("/out_10", msgCb); - IGN_SLEEP_MS(10); - - const int pubCount{10}; - msgs::Float msg; - for (int i = 0; i < pubCount; ++i) - { - msg.set_data(static_cast(i)* 0.1); - EXPECT_TRUE(inputPub.Publish(msg)); - IGN_SLEEP_MS(10); - } - - // The input contains the sequence {0, 0.1, 0.2, ...}, the matcher is set to - // match 0.5 with a tolerance of 0.15, so it should match {0.4, 0.5, 0.6} - EXPECT_EQ(3u, recvCount); -} - -TEST_F(TriggeredPublisherTest, - IGN_UTILS_TEST_DISABLED_ON_WIN32(FieldMatchersAcceptToleranceParam)) -{ - transport::Node node; - auto inputPub = node.Advertise("/in_11"); - std::atomic recvCount{0}; - auto msgCb = std::function( - [&recvCount](const auto &) - { - ++recvCount; - }); - node.Subscribe("/out_11", msgCb); - IGN_SLEEP_MS(10); - - const int pubCount{10}; - msgs::Pose msg; - for (int i = 0; i < pubCount; ++i) - { - msg.mutable_position()->set_x(0.1); - msg.mutable_position()->set_z(static_cast(i)* 0.1); - EXPECT_TRUE(inputPub.Publish(msg)); - IGN_SLEEP_MS(10); - } - - // The input contains the sequence {0, 0.1, 0.2, ...} in position.z, the - // matcher is set to match 0.5 with a tolerance of 0.15, so it should match - // {0.4, 0.5, 0.6} - EXPECT_EQ(3u, recvCount); -} - -TEST_F(TriggeredPublisherTest, - IGN_UTILS_TEST_DISABLED_ON_WIN32(SubfieldsOfRepeatedFieldsNotSupported)) -{ - transport::Node node; - auto inputPub = node.Advertise("/in_12"); - std::atomic recvCount{0}; - auto msgCb = std::function( - [&recvCount](const auto &) - { - ++recvCount; - }); - node.Subscribe("/out_12", msgCb); - IGN_SLEEP_MS(10); - - const int pubCount{10}; - for (int i = 0; i < pubCount; ++i) - { - msgs::Header msg; - auto *data = msg.add_data(); - data->set_key("key1"); - data->add_value("value1"); - - EXPECT_TRUE(inputPub.Publish(msg)); - IGN_SLEEP_MS(10); - } - - // Subfields of repeated fiealds are not supported, so no output should be - // triggered. - EXPECT_EQ(0u, recvCount); -} - -TEST_F(TriggeredPublisherTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(TriggerDelay)) -{ - transport::Node node; - auto inputPub = node.Advertise("/in_13"); - std::atomic recvCount{0}; - auto msgCb = std::function( - [&recvCount](const auto &) - { - ++recvCount; - }); - node.Subscribe("/out_13", msgCb); - IGN_SLEEP_MS(100ms); - - const std::size_t pubCount{10}; - for (std::size_t i = 0; i < pubCount; ++i) - { - EXPECT_TRUE(inputPub.Publish(msgs::Empty())); - } - waitUntil(1000, [&]{return pubCount == recvCount;}); - - // Delay has been specified, but simulation is not running. No messages - // should have been received. - EXPECT_EQ(0u, recvCount); - - // The simulation delay is 1000ms, which is equal to 1000 steps. Run - // for 999 steps, and the count should still be zero. Take one additional - // step and all the messages should arrive. - this->server->Run(true, 999, false); - waitUntil(1000, [&]{return pubCount == recvCount;}); - EXPECT_EQ(0u, recvCount); - this->server->Run(true, 1, false); - waitUntil(1000, [&]{return pubCount == recvCount;}); - EXPECT_EQ(pubCount, recvCount); -} - -TEST_F(TriggeredPublisherTest, - IGN_UTILS_TEST_DISABLED_ON_WIN32(WrongInputWhenRepeatedFieldExpected)) -{ - transport::Node node; - auto inputPub = node.Advertise("/invalid_topic"); - std::atomic recvCount{0}; - auto msgCb = std::function( - [&recvCount](const auto &) - { - ++recvCount; - }); - node.Subscribe("/out_9", msgCb); - IGN_SLEEP_MS(10); - - const int pubCount{10}; - msgs::Int32 msg; - for (int i = 0; i < pubCount; ++i) - { - msg.set_data(i); - EXPECT_TRUE(inputPub.Publish(msg)); - IGN_SLEEP_MS(10); - } - - EXPECT_EQ(0u, recvCount); -} - -///////////////////////////////////////////////// -// Test for invalid service name. It'll timeout -// when matching service name can't be found. -///////////////////////////////////////////////// -TEST_F(TriggeredPublisherTest, - IGN_UTILS_TEST_DISABLED_ON_WIN32(InvalidServiceName)) -{ - transport::Node node; - auto inputPub = node.Advertise("/in_14"); - std::atomic recvCount{0}; - - auto srvEchoCb = std::function( - [&recvCount](const auto &_req, auto &_rep) - { - EXPECT_EQ(_req.data(), "test"); - if (_req.data() == "test") - { - ++recvCount; - _rep.set_data(_req.data()); - return true; - } - return false; - }); - - // Advertise a dummy service - std::string service = "/srv-dummy-test"; - node.Advertise(service, srvEchoCb); - - const std::size_t pubCount{10}; - for (std::size_t i = 0; i < pubCount; ++i) - { - EXPECT_TRUE(inputPub.Publish(msgs::Empty())); - IGN_SLEEP_MS(100); - } - - waitUntil(5000, [&]{return recvCount == 0u;}); - EXPECT_EQ(recvCount, 0u); -} ///////////////////////////////////////////////// // Test for triggering a service call in response @@ -685,175 +875,98 @@ TEST_F(TriggeredPublisherTest, // pubCount and recvCount. ///////////////////////////////////////////////// TEST_F(TriggeredPublisherTest, - IGN_UTILS_TEST_DISABLED_ON_WIN32(InputMessagesTriggerServices)) + IGN_UTILS_TEST_DISABLED_ON_WIN32(CallingServiceInProgress)) { transport::Node node; - auto inputPub = node.Advertise("/in_14"); + auto inputPub = node.Advertise("/in_20"); std::atomic recvCount{0}; +//TODO: change recvCount to be two different entitites +// TODO: take out timeout - auto srvEchoCb = std::function( [&recvCount](const auto &_req, auto &_rep) { + ignerr <<"IN srv-0 CALLBACK\n"; EXPECT_EQ(_req.data(), "test"); if (_req.data() == "test") { - ++recvCount; + // ++recvCount; _rep.set_data(_req.data()); + ignerr <<"starting to wait\n"; + std::this_thread::sleep_for(std::chrono::milliseconds(200)); + // IGN_SLEEP_MS(1200); + ignerr <<"3seconds passed\n"; return true; } return false; }); - // Advertise a dummy service - std::string service = "/srv-test"; - node.Advertise(service, srvEchoCb); - - const std::size_t pubCount{10}; - for (std::size_t i = 0; i < pubCount; ++i) - { - EXPECT_TRUE(inputPub.Publish(msgs::Empty())); - IGN_SLEEP_MS(100); - } - - waitUntil(5000, [&]{return pubCount == recvCount;}); - EXPECT_EQ(pubCount, recvCount); -} - -///////////////////////////////////////////////// -// Test for triggering multiple services in response -// to a input ign msg by publishing 10 times. Service -// call will also occur 10 times for each service. -// It'll compare pubCount and recvCount for each -// services. -///////////////////////////////////////////////// -TEST_F(TriggeredPublisherTest, - IGN_UTILS_TEST_DISABLED_ON_WIN32(MultipleServiceForOneInput)) -{ - transport::Node node; - auto inputPub = node.Advertise("/in_15"); - std::mutex recvMsgMutex; - std::vector recvMsgs0; - std::vector recvMsgs1; - auto cbCreator = [&recvMsgMutex](std::vector &_msgVector) - { - return std::function( - [&_msgVector, &recvMsgMutex](const auto &_req, auto &_rep) + auto srvCb1 = std::function( + [&recvCount](const auto &_req, auto &_rep) { - std::lock_guard lock(recvMsgMutex); - if (_req.data() || !_req.data()) + ignerr <<"IN srv-1 CALLBACK\n"; + EXPECT_EQ(_req.data(), "test2"); + if (_req.data() == "test2") { - _msgVector.push_back(_req.data()); + ++recvCount; + _rep.set_data(false); + ignerr <<"starting to wait\n"; + IGN_SLEEP_MS(100); + ignerr <<"3seconds passed\n"; return true; } return false; }); - }; - - auto msgCb0 = cbCreator(recvMsgs0); - auto msgCb1 = cbCreator(recvMsgs1); - - // Advertise two dummy services - node.Advertise("/srv-test-0", msgCb0); - node.Advertise("/srv-test-1", msgCb1); - - const int pubCount{10}; - for (int i = 0; i < pubCount; ++i) - { - EXPECT_TRUE(inputPub.Publish(msgs::Empty())); - IGN_SLEEP_MS(10); - } - - waitUntil(5000, [&] - { - std::lock_guard lock(recvMsgMutex); - return static_cast(pubCount) == recvMsgs0.size() && - static_cast(pubCount) == recvMsgs1.size(); - }); - - EXPECT_EQ(static_cast(pubCount), recvMsgs0.size()); - EXPECT_EQ(static_cast(pubCount), recvMsgs1.size()); - - // The plugin has two outputs. We expect 10 messages in each output topic - EXPECT_EQ(pubCount, std::count(recvMsgs0.begin(), recvMsgs0.end(), true)); - EXPECT_EQ(pubCount, std::count(recvMsgs1.begin(), recvMsgs1.end(), false)); -} - -///////////////////////////////////////////////// -// Test for triggering a service call with incorrect -// request type or reseponse type. The server callback -// will not be triggered hence the recvCount will be 0 -///////////////////////////////////////////////// -TEST_F(TriggeredPublisherTest, - IGN_UTILS_TEST_DISABLED_ON_WIN32(WrongRequestOrResponseType)) -{ - transport::Node node; - auto inputPub = node.Advertise("/in_16"); - std::atomic recvCount{0}; - - auto srvEchoCb = std::function( + auto srvCb2 = std::function( [&recvCount](const auto &_req, auto &_rep) { - EXPECT_EQ(_req.data(), "test"); - if (_req.data() == "test") + ignerr <<"IN CALLBACK - 4\n"; + EXPECT_EQ(_req.data(), true); + if (_req.data() == true) { ++recvCount; - _rep.set_data(_req.data()); + _rep.set_data("meh"); + ignerr <<"starting to wait\n"; + IGN_SLEEP_MS(200); + ignerr <<"3seconds passed\n"; return true; } return false; }); - // Advertise a dummy service - std::string service = "/srv-test"; - node.Advertise(service, srvEchoCb); - - const std::size_t pubCount{10}; - for (std::size_t i = 0; i < pubCount; ++i) - { - EXPECT_TRUE(inputPub.Publish(msgs::Empty())); - IGN_SLEEP_MS(100); - } - - waitUntil(5000, [&]{return recvCount == 0u;}); - EXPECT_EQ(0u, recvCount); -} - -///////////////////////////////////////////////// -// Test for triggering a service call that'll return -// False result. The server callback will be triggered -// but it'll +1 to the recvCout but return False. -///////////////////////////////////////////////// -TEST_F(TriggeredPublisherTest, - IGN_UTILS_TEST_DISABLED_ON_WIN32(FailedReesultServiceCall)) -{ - transport::Node node; - auto inputPub = node.Advertise("/in_17"); - std::atomic recvCount{0}; - - auto srvEchoCb = std::function( - [&recvCount](const auto &_req, auto &_rep) + auto srvCb3 = std::function( + [&recvCount](const auto &_req) { + ignerr <<"IN CALLBACK - 3\n"; EXPECT_EQ(_req.data(), "test"); if (_req.data() == "test") { ++recvCount; - _rep.set_data(_req.data()); - // return True was substitued with False - return false; + ignerr <<"starting to wait\n"; + IGN_SLEEP_MS(100); + ignerr <<"3seconds passed\n"; + ignerr <<"DONE\n"; } }); - // Advertise a dummy service - std::string service = "/srv-test"; - node.Advertise(service, srvEchoCb); + // std::string service = "/srv-test-th"; + node.Advertise("srv-0", srvCb0); + node.Advertise("srv-1", srvCb1); + node.Advertise("srv-2", srvCb2); + node.Advertise("srv-3", srvCb3); + //node.Advertise("srv-0", srvEchoCb2); +// node.Advertise(service, srvEchoCb1); + +// node.Advertise("/test1", srvE); const std::size_t pubCount{10}; for (std::size_t i = 0; i < pubCount; ++i) { EXPECT_TRUE(inputPub.Publish(msgs::Empty())); + ignerr <<"publishing " << i < @@ -225,14 +224,12 @@ name="/srv-test-0" reqType="ignition.msgs.Boolean" repType="ignition.msgs.Boolean" - timeout="100" reqMsg="data: true"> @@ -245,7 +242,6 @@ name="/srv-test" reqType="ignition.msgs.InvalidReqType" repType="ignition.msgs.StringMsg" - timeout="100" reqMsg="data: 'test'"> @@ -258,7 +254,6 @@ name="/srv-test" reqType="ignition.msgs.StringMsg" repType="ignition.msgs.InvalidRepType" - timeout="100" reqMsg="data: 'test'"> @@ -271,7 +266,65 @@ name="/srv-test" reqType="ignition.msgs.StringMsg" repType="ignition.msgs.StringMsg" - timeout="100" + reqMsg="data: 'test'"> + + + + + + + + + + + + + + + + + + + + + + + From d5b016aeb78c5f14715b25aca6f4f7f2a033e4f0 Mon Sep 17 00:00:00 2001 From: Liam Han Date: Wed, 3 Aug 2022 11:50:35 +0000 Subject: [PATCH 12/26] WIP: fixed preprocessor define bug Signed-off-by: Liam Han --- .../triggered_publisher/TriggeredPublisher.cc | 21 +++++++--------- .../triggered_publisher/TriggeredPublisher.hh | 25 +++++++++---------- test/integration/triggered_publisher.cc | 20 +++++++++++++-- test/worlds/triggered_publisher.sdf | 3 +-- 4 files changed, 40 insertions(+), 29 deletions(-) diff --git a/src/systems/triggered_publisher/TriggeredPublisher.cc b/src/systems/triggered_publisher/TriggeredPublisher.cc index 90d1722819..f043702623 100644 --- a/src/systems/triggered_publisher/TriggeredPublisher.cc +++ b/src/systems/triggered_publisher/TriggeredPublisher.cc @@ -662,8 +662,7 @@ void TriggeredPublisher::Configure(const Entity &, serviceInfo.reqMsg = serviceElem->Get("reqMsg"); if (serviceInfo.reqMsg.empty()) { - ignerr << "Service request string cannot be empty\n"; - return; + ignwarn << "Service request string cannot be empty\n"; } // std::string timeoutInfo = serviceElem->Get("timeout"); // if (timeoutInfo.empty()) @@ -740,17 +739,14 @@ void TriggeredPublisher::Configure(const Entity &, ////////////////////////////////////////////////// #define HANDLE_REQUEST(type, typeString) \ -if(serviceInfo.reqType == typeString) \ +if (serviceInfo.reqType == typeString) \ { \ - if(serviceInfo.reqType.empty()) \ - { \ - this->HandleNoInputRequest(serviceInfo); \ - } \ - else \ - { \ - this->HandleRequest(serviceInfo); \ - } \ -} + this->HandleRequest(serviceInfo); \ +} \ +else if (serviceInfo.reqType.empty()) \ +{ \ + this->HandleNoInputRequest(serviceInfo); \ +} \ void TriggeredPublisher::DoServiceWork() { @@ -775,6 +771,7 @@ void TriggeredPublisher::DoServiceWork() { HANDLE_REQUEST(msgs::StringMsg, "ignition.msgs.StringMsg"); HANDLE_REQUEST(msgs::Boolean, "ignition.msgs.Boolean"); + HANDLE_REQUEST(msgs::Empty, "ignition.msgs.Empty"); } } } diff --git a/src/systems/triggered_publisher/TriggeredPublisher.hh b/src/systems/triggered_publisher/TriggeredPublisher.hh index 1bd3c191bf..a55977135f 100644 --- a/src/systems/triggered_publisher/TriggeredPublisher.hh +++ b/src/systems/triggered_publisher/TriggeredPublisher.hh @@ -337,25 +337,23 @@ namespace systems return this->node.Request(serviceInfo.servName, reqCb); } - private: template - bool HandleNoReply(ServiceOutputInfo& serviceInfo) + private: template + bool HandleNoReply(ServiceOutputInfo& serviceInfo, RequestT& req) { - auto reqCb = ReplyCallback(serviceInfo); - return this->node.Request(serviceInfo.servName, reqCb); + // auto reqCb = ReplyCallback(serviceInfo); + return this->node.Request(serviceInfo.servName, req); } #define HANDLE_REPLY(repT, typeString) \ if(serviceInfo.repType == typeString) \ { \ - if(serviceInfo.repType.empty()) \ - { \ - executed = HandleNoReply(serviceInfo); \ - } \ - else \ - { \ - executed = HandleResponse(serviceInfo, *req); \ - } \ - } + executed = HandleResponse(serviceInfo, *req); \ + } \ + else if (serviceInfo.repType.empty()) \ + { \ + executed = HandleNoReply(serviceInfo, *req); \ + } \ + private: template void HandleRequest(ServiceOutputInfo& serviceInfo) { @@ -371,6 +369,7 @@ namespace systems HANDLE_REPLY(msgs::StringMsg, "ignition.msgs.StringMsg"); HANDLE_REPLY(msgs::Boolean, "ignition.msgs.Boolean"); + HANDLE_REPLY(msgs::Empty, "ignition.msgs.Empty"); if (!executed) { diff --git a/test/integration/triggered_publisher.cc b/test/integration/triggered_publisher.cc index 784863a54a..37dbfee9b1 100644 --- a/test/integration/triggered_publisher.cc +++ b/test/integration/triggered_publisher.cc @@ -923,7 +923,7 @@ TEST_F(TriggeredPublisherTest, msgs::StringMsg &)>( [&recvCount](const auto &_req, auto &_rep) { - ignerr <<"IN CALLBACK - 4\n"; + ignerr <<"IN srv-2 CALLBACK\n"; EXPECT_EQ(_req.data(), true); if (_req.data() == true) { @@ -940,7 +940,7 @@ TEST_F(TriggeredPublisherTest, auto srvCb3 = std::function( [&recvCount](const auto &_req) { - ignerr <<"IN CALLBACK - 3\n"; + ignerr <<"IN srv-3 CALLBACK\n"; EXPECT_EQ(_req.data(), "test"); if (_req.data() == "test") { @@ -951,12 +951,28 @@ TEST_F(TriggeredPublisherTest, ignerr <<"DONE\n"; } }); + + auto srvCb4 = std::function( + [&recvCount](auto &rep) + { + ignerr <<"IN srv-4 CALLBACK\n"; + { + ++recvCount; + rep.set_data("callback srv-4"); + ignerr <<"starting to wait\n"; + IGN_SLEEP_MS(100); + ignerr <<"3seconds passed\n"; + ignerr <<"DONE4444\n"; + return true; + } + }); // Advertise a dummy service // std::string service = "/srv-test-th"; node.Advertise("srv-0", srvCb0); node.Advertise("srv-1", srvCb1); node.Advertise("srv-2", srvCb2); node.Advertise("srv-3", srvCb3); + node.Advertise("srv-4", srvCb4); //node.Advertise("srv-0", srvEchoCb2); // node.Advertise(service, srvEchoCb1); diff --git a/test/worlds/triggered_publisher.sdf b/test/worlds/triggered_publisher.sdf index 8bdcebd26c..f9b36ec390 100644 --- a/test/worlds/triggered_publisher.sdf +++ b/test/worlds/triggered_publisher.sdf @@ -324,8 +324,7 @@ + repType="ignition.msgs.StringMsg"> From a5673857db0125e1d5762653327ec84b505de08d Mon Sep 17 00:00:00 2001 From: Liam Han Date: Thu, 4 Aug 2022 03:51:36 +0000 Subject: [PATCH 13/26] WIP: working but extremely convoluted Signed-off-by: Liam Han --- .../triggered_publisher/TriggeredPublisher.cc | 66 +-- .../triggered_publisher/TriggeredPublisher.hh | 85 +++- test/integration/triggered_publisher.cc | 396 ++++++++++++------ test/worlds/triggered_publisher.sdf | 56 ++- 4 files changed, 411 insertions(+), 192 deletions(-) diff --git a/src/systems/triggered_publisher/TriggeredPublisher.cc b/src/systems/triggered_publisher/TriggeredPublisher.cc index f043702623..4915f5092c 100644 --- a/src/systems/triggered_publisher/TriggeredPublisher.cc +++ b/src/systems/triggered_publisher/TriggeredPublisher.cc @@ -479,28 +479,6 @@ std::unique_ptr InputMatcher::Create( return matcher; } -////////////////////////////////////////////////// - -void TriggeredPublisher::ResponseHandler::operator() (const msgs::StringMsg &msg, ServiceOutputInfo &srvInfo) -{ - ignmsg << "Service call [" << srvInfo.servName - << "] succeeded. Response message [" << std::boolalpha << msg.data() - << "]\n"; -} -void TriggeredPublisher::ResponseHandler::operator() (const msgs::Boolean &msg, ServiceOutputInfo &srvInfo) -{ - ignmsg << "Service call [" << srvInfo.servName - << "] succeeded. Response message [" << std::boolalpha << msg.data() - << "]\n"; -} -void TriggeredPublisher::ResponseHandler::operator() (const msgs::Empty &msg, ServiceOutputInfo &srvInfo) -{ - ignmsg << "Service call [" << srvInfo.servName - << "] succeeded. Response message [" << msg.unused() - << "]\n"; -} -// NOTE: add more msgs here - ////////////////////////////////////////////////// TriggeredPublisher::~TriggeredPublisher() { @@ -651,27 +629,27 @@ void TriggeredPublisher::Configure(const Entity &, serviceInfo.reqType = serviceElem->Get("reqType"); if (serviceInfo.reqType.empty()) { - ignwarn << "Service request type cannot be empty\n"; + ignwarn << "Service request type is empty\n"; } serviceInfo.repType = serviceElem->Get("repType"); if (serviceInfo.repType.empty()) { - ignwarn << "Service response type cannot be empty\n"; - //todo: change to warn + ignwarn << "Service reply type is empty\n"; } serviceInfo.reqMsg = serviceElem->Get("reqMsg"); if (serviceInfo.reqMsg.empty()) { - ignwarn << "Service request string cannot be empty\n"; + ignwarn << "Service request string is empty\n"; } - // std::string timeoutInfo = serviceElem->Get("timeout"); - // if (timeoutInfo.empty()) - // { - // ignwarn << "Timeout value cannot be empty\n"; - // return; - // } - - // serviceInfo.timeout = std::stoi(timeoutInfo); + // serviceInfo.timeout = serviceElem->Get("timeout"); + // if (serviceInfo.reqMsg.empty()) + // { + // ignwarn << "Service timeout is empty\n"; + // } + // else + // { + // serviceInfo.timeout = std::stoi(serviceInfo.timeout); + // } this->serviceOutputInfo.push_back(std::move(serviceInfo)); } } @@ -699,11 +677,9 @@ void TriggeredPublisher::Configure(const Entity &, this->newMatchSignal.notify_one(); } // only perform when service tag is specified in sdf - ignerr<<"out\n"; if (this->serviceOutputInfo.size() > 0) { { - ignerr<<"in\n"; std::lock_guard lock(this->serviceCountMutex); this->callService = true; } @@ -738,16 +714,15 @@ void TriggeredPublisher::Configure(const Entity &, ////////////////////////////////////////////////// -#define HANDLE_REQUEST(type, typeString) \ -if (serviceInfo.reqType == typeString) \ +#define HANDLE_REQUEST(requestT, typeString) \ +if (((serviceInfo.reqType == typeString) | serviceInfo.reqType.empty()) && !isProcessing) \ { \ - this->HandleRequest(serviceInfo); \ -} \ -else if (serviceInfo.reqType.empty()) \ -{ \ - this->HandleNoInputRequest(serviceInfo); \ + this->HandleRequest(serviceInfo); \ + isProcessing = true; \ } \ + //this->HandleNoRequestMsg(serviceInfo); + void TriggeredPublisher::DoServiceWork() { while (!this->srvDone) @@ -766,12 +741,15 @@ void TriggeredPublisher::DoServiceWork() continue; } } - ignerr <<"================================\n"; + bool isProcessing {false}; for (auto &serviceInfo : this->serviceOutputInfo) { + HANDLE_REQUEST(msgs::Pose, "ignition.msgs.Pose"); HANDLE_REQUEST(msgs::StringMsg, "ignition.msgs.StringMsg"); HANDLE_REQUEST(msgs::Boolean, "ignition.msgs.Boolean"); HANDLE_REQUEST(msgs::Empty, "ignition.msgs.Empty"); + ignerr<<"-------\n"; + //NOTE: add more protobuf msgs for the Request } } } diff --git a/src/systems/triggered_publisher/TriggeredPublisher.hh b/src/systems/triggered_publisher/TriggeredPublisher.hh index a55977135f..e2039c6941 100644 --- a/src/systems/triggered_publisher/TriggeredPublisher.hh +++ b/src/systems/triggered_publisher/TriggeredPublisher.hh @@ -234,7 +234,7 @@ namespace systems /// \brief Service request type std::string reqType; - /// \brief Service response type + /// \brief Service reply type std::string repType; /// \brief Service request message @@ -294,15 +294,37 @@ namespace systems /// \brief Mutex to synchronize access to publishQueue private: std::mutex publishQueueMutex; - private: class ResponseHandler + private: class ReplyHandler { public: virtual void operator() (const msgs::StringMsg &msg, - ServiceOutputInfo &srvInfo); + ServiceOutputInfo &srvInfo) + { + ignmsg << "Service call [" << srvInfo.servName + << "] succeeded. Response message [" << std::boolalpha + << msg.data() << "]\n"; + } public: virtual void operator() (const msgs::Boolean &msg, - ServiceOutputInfo &srvInfo); + ServiceOutputInfo &srvInfo) + { + ignmsg << "Service call [" << srvInfo.servName + << "] succeeded. Response message [" << std::boolalpha + << msg.data() << "]\n"; + } public: virtual void operator() (const msgs::Empty &msg, - ServiceOutputInfo &srvInfo); - //NOTE: add more msgs here + ServiceOutputInfo &srvInfo) + { + ignmsg << "Service call [" << srvInfo.servName + << "] succeeded. Response message [" << msg.unused() + << "]\n"; + } + public: virtual void operator() (const msgs::Pose &msg, + ServiceOutputInfo &srvInfo) + { + ignmsg << "Service call [" << srvInfo.servName + << "] succeeded. Response message [" + << "]\n"; + } + //NOTE: add more replys for different types }; private: template @@ -313,7 +335,7 @@ namespace systems [&serviceInfo] (const ReplyT &msg, const bool res){ if (res) { - ResponseHandler()(msg, serviceInfo); + ReplyHandler()(msg, serviceInfo); } else { @@ -324,15 +346,19 @@ namespace systems } private: template - bool HandleResponse(ServiceOutputInfo& serviceInfo, RequestT& req) + bool HandleReply(ServiceOutputInfo& serviceInfo, RequestT& req) { + // Non-blocking request with request type and callback auto reqCb = ReplyCallback(serviceInfo); return this->node.Request(serviceInfo.servName, req, reqCb); } - + private: template - bool HandleNoInputRequest(ServiceOutputInfo& serviceInfo) + bool HandleNoRequestMsg(ServiceOutputInfo& serviceInfo) { + // Non-blocking request with no request type + //ignerr<<"here\n"; + auto reqCb = ReplyCallback(serviceInfo); return this->node.Request(serviceInfo.servName, reqCb); } @@ -340,36 +366,50 @@ namespace systems private: template bool HandleNoReply(ServiceOutputInfo& serviceInfo, RequestT& req) { - // auto reqCb = ReplyCallback(serviceInfo); + // Non-blocking request with no callback return this->node.Request(serviceInfo.servName, req); } #define HANDLE_REPLY(repT, typeString) \ - if(serviceInfo.repType == typeString) \ + if (((serviceInfo.repType == typeString) && \ + (serviceInfo.reqType.empty())) && !isProcessing) \ { \ - executed = HandleResponse(serviceInfo, *req); \ + executed = HandleNoRequestMsg(serviceInfo); \ + isProcessing = true; \ } \ - else if (serviceInfo.repType.empty()) \ + else if (serviceInfo.repType.empty() && !isProcessing) \ { \ - executed = HandleNoReply(serviceInfo, *req); \ + executed = HandleNoReply(serviceInfo, *req); \ + isProcessing = true; \ + } \ + else if((serviceInfo.repType == typeString) && !isProcessing)\ + { \ + executed = HandleReply(serviceInfo, *req); \ + isProcessing = true; \ } \ private: template void HandleRequest(ServiceOutputInfo& serviceInfo) { bool executed {false}; - auto req = msgs::Factory::New(serviceInfo.reqType, - serviceInfo.reqMsg); - if (!req) + bool isProcessing {false}; + // msgs::Factory::New req; + std::unique_ptr req; + if (!serviceInfo.reqType.empty()) { - ignerr << "Unable to create request for type [" - << serviceInfo.reqType << "].\n"; - return; + req = msgs::Factory::New(serviceInfo.reqType, + serviceInfo.reqMsg); + if (!req) + { + ignerr << "Unable to create request for type [" + << serviceInfo.reqType << "].\n"; + return; + } } - HANDLE_REPLY(msgs::StringMsg, "ignition.msgs.StringMsg"); HANDLE_REPLY(msgs::Boolean, "ignition.msgs.Boolean"); HANDLE_REPLY(msgs::Empty, "ignition.msgs.Empty"); + //NOTE: add more protobuf msgs for the Reply if (!executed) { @@ -380,7 +420,6 @@ namespace systems std::lock_guard lock(this->serviceCountMutex); this->callService = false; } - ignerr << "=====================\n"; } }; } diff --git a/test/integration/triggered_publisher.cc b/test/integration/triggered_publisher.cc index 37dbfee9b1..a5256f0a9c 100644 --- a/test/integration/triggered_publisher.cc +++ b/test/integration/triggered_publisher.cc @@ -651,13 +651,13 @@ bool waitUntil(int _timeoutMs, Pred _pred) // // auto srvEchoCb = std::function( -// [&recvCount](const auto &_req, auto &_rep) +// [&recvCount](const auto &req, auto &rep) // { -// EXPECT_EQ(_req.data(), "test"); -// if (_req.data() == "test") +// EXPECT_EQ(req.data(), "test"); +// if (req.data() == "test") // { // ++recvCount; -// _rep.set_data(_req.data()); +// rep.set_data(req.data()); // return true; // } // return false; @@ -693,13 +693,13 @@ bool waitUntil(int _timeoutMs, Pred _pred) // // auto srvEchoCb = std::function( -// [&recvCount](const auto &_req, auto &_rep) +// [&recvCount](const auto &req, auto &rep) // { -// EXPECT_EQ(_req.data(), "test"); -// if (_req.data() == "test") +// EXPECT_EQ(req.data(), "test"); +// if (req.data() == "test") // { // ++recvCount; -// _rep.set_data(_req.data()); +// rep.set_data(req.data()); // return true; // } // return false; @@ -738,12 +738,12 @@ bool waitUntil(int _timeoutMs, Pred _pred) // auto cbCreator = [&recvMsgMutex](std::vector &_msgVector) // { // return std::function( -// [&_msgVector, &recvMsgMutex](const auto &_req, auto &_rep) +// [&_msgVector, &recvMsgMutex](const auto &req, auto &rep) // { // std::lock_guard lock(recvMsgMutex); -// if (_req.data() || !_req.data()) +// if (req.data() || !req.data()) // { -// _msgVector.push_back(_req.data()); +// _msgVector.push_back(req.data()); // return true; // } // return false; @@ -793,13 +793,13 @@ bool waitUntil(int _timeoutMs, Pred _pred) // // auto srvEchoCb = std::function( -// [&recvCount](const auto &_req, auto &_rep) +// [&recvCount](const auto &req, auto &rep) // { -// EXPECT_EQ(_req.data(), "test"); -// if (_req.data() == "test") +// EXPECT_EQ(req.data(), "test"); +// if (req.data() == "test") // { // ++recvCount; -// _rep.set_data(_req.data()); +// rep.set_data(req.data()); // return true; // } // return false; @@ -834,13 +834,13 @@ bool waitUntil(int _timeoutMs, Pred _pred) // // auto srvEchoCb = std::function( -// [&recvCount](const auto &_req, auto &_rep) +// [&recvCount](const auto &req, auto &rep) // { -// EXPECT_EQ(_req.data(), "test"); -// if (_req.data() == "test") +// EXPECT_EQ(req.data(), "test"); +// if (req.data() == "test") // { // ++recvCount; -// _rep.set_data(_req.data()); +// rep.set_data(req.data()); // // return True was substitued with False // return false; // } @@ -868,124 +868,276 @@ bool waitUntil(int _timeoutMs, Pred _pred) +/////////////////////////////////////////////////// +//// Test for triggering a service call with same +//// Request and Reply type +//// --- +//// Request Type: msgs::StringMsg +//// Reply Type: msgs::StringMsg +/////////////////////////////////////////////////// +//TEST_F(TriggeredPublisherTest, +// IGN_UTILS_TEST_DISABLED_ON_WIN32(CallingSameRequestReplyTypeServ)) +//{ +// transport::Node node; +// auto inputPub = node.Advertise("/in_20"); +// std::atomic recvCount{0}; +// +// auto srvCb = std::function([&recvCount](const auto &req, auto &rep) +// { +// EXPECT_EQ(req.data(), "srv-0"); +// if (req.data() == "srv-0") +// { +// ++recvCount; +// rep.set_data(req.data()); +// return true; +// } +// return false; +// }); +// node.Advertise("srv-0", srvCb); +// +// const std::size_t pubCount{10}; +// for (std::size_t i = 0; i < pubCount; ++i) +// { +// EXPECT_TRUE(inputPub.Publish(msgs::Empty())); +// IGN_SLEEP_MS(100); +// } +// +// waitUntil(5000, [&]{return pubCount == recvCount;}); +// EXPECT_EQ(pubCount, recvCount); +//} +// +// +/////////////////////////////////////////////////// +//// Test for triggering a service call with different +//// Request and Reply type +//// --- +//// Request Type: msgs::StringMsg +//// Reply Type: msgs::Boolean +/////////////////////////////////////////////////// +//TEST_F(TriggeredPublisherTest, +// IGN_UTILS_TEST_DISABLED_ON_WIN32(CallingDifferntRequestReplyTypeServ1)) +//{ +// transport::Node node; +// auto inputPub = node.Advertise("/in_21"); +// std::atomic recvCount{0}; +// +// auto srvCb = std::function([&recvCount](const auto &req, auto &rep) +// { +// EXPECT_EQ(req.data(), "srv-1"); +// if (req.data() == "srv-1") +// { +// ++recvCount; +// rep.set_data(true); +// return true; +// } +// return false; +// }); +// node.Advertise("srv-1", srvCb); +// +// const std::size_t pubCount{10}; +// for (std::size_t i = 0; i < pubCount; ++i) +// { +// EXPECT_TRUE(inputPub.Publish(msgs::Empty())); +// IGN_SLEEP_MS(100); +// } +// +// waitUntil(5000, [&]{return pubCount == recvCount;}); +// EXPECT_EQ(pubCount, recvCount); +//} +// +/////////////////////////////////////////////////// +//// Test for triggering a service call with different +//// Request and Reply type +//// --- +//// Request Type: msgs::Boolean +//// Reply Type: msgs::StringMsg +/////////////////////////////////////////////////// +//TEST_F(TriggeredPublisherTest, +// IGN_UTILS_TEST_DISABLED_ON_WIN32(CallingDifferntRequestReplyTypeServ2)) +//{ +// transport::Node node; +// auto inputPub = node.Advertise("/in_22"); +// std::atomic recvCount{0}; +// +// auto srvCb = std::function([&recvCount](const auto &req, auto &rep) +// { +// EXPECT_EQ(req.data(), true); +// if (req.data() == true) +// { +// ++recvCount; +// rep.set_data("srv-2"); +// return true; +// } +// return false; +// }); +// node.Advertise("srv-2", srvCb); +// +// const std::size_t pubCount{10}; +// for (std::size_t i = 0; i < pubCount; ++i) +// { +// EXPECT_TRUE(inputPub.Publish(msgs::Empty())); +// IGN_SLEEP_MS(100); +// } +// +// waitUntil(5000, [&]{return pubCount == recvCount;}); +// EXPECT_EQ(pubCount, recvCount); +//} +// +/////////////////////////////////////////////////// +//// Test for triggering a service call with no +//// Reply type specified +//// --- +//// Request Type: msgs::StringMsg +//// Reply Type: null +/////////////////////////////////////////////////// +//TEST_F(TriggeredPublisherTest, +// IGN_UTILS_TEST_DISABLED_ON_WIN32(CallingNoReplyTypeService)) +//{ +// transport::Node node; +// auto inputPub = node.Advertise("/in_23"); +// std::atomic recvCount{0}; +// +// auto srvCb = std::function +// ([&recvCount](const auto &req) +// { +// EXPECT_EQ(req.data(), "srv-3"); +// if (req.data() == "srv-3") +// { +// ++recvCount; +// } +// }); +// node.Advertise("srv-3", srvCb); +// +// const std::size_t pubCount{10}; +// for (std::size_t i = 0; i < pubCount; ++i) +// { +// EXPECT_TRUE(inputPub.Publish(msgs::Empty())); +// IGN_SLEEP_MS(100); +// } +// +// waitUntil(5000, [&]{return pubCount == recvCount;}); +// EXPECT_EQ(pubCount, recvCount); +//} + + + ///////////////////////////////////////////////// -// Test for triggering a service call in response -// to input ign msg by publishing 10 times. Service -// call will also occur 10 times. It'll compare -// pubCount and recvCount. +// Test for triggering a service call with no +// Request type +// --- +// Request Type: null +// Reply Type: msgs::StringMsg ///////////////////////////////////////////////// TEST_F(TriggeredPublisherTest, - IGN_UTILS_TEST_DISABLED_ON_WIN32(CallingServiceInProgress)) + IGN_UTILS_TEST_DISABLED_ON_WIN32(CallingNoRequestTypeServ)) { transport::Node node; - auto inputPub = node.Advertise("/in_20"); + auto inputPub = node.Advertise("/in_24"); std::atomic recvCount{0}; -//TODO: change recvCount to be two different entitites -// TODO: take out timeout - - auto srvCb0 = std::function( - [&recvCount](const auto &_req, auto &_rep) - { - ignerr <<"IN srv-0 CALLBACK\n"; - EXPECT_EQ(_req.data(), "test"); - if (_req.data() == "test") - { - // ++recvCount; - _rep.set_data(_req.data()); - ignerr <<"starting to wait\n"; - std::this_thread::sleep_for(std::chrono::milliseconds(200)); - // IGN_SLEEP_MS(1200); - ignerr <<"3seconds passed\n"; - return true; - } - return false; - }); - auto srvCb1 = std::function( - [&recvCount](const auto &_req, auto &_rep) - { - ignerr <<"IN srv-1 CALLBACK\n"; - EXPECT_EQ(_req.data(), "test2"); - if (_req.data() == "test2") - { - ++recvCount; - _rep.set_data(false); - ignerr <<"starting to wait\n"; - IGN_SLEEP_MS(100); - ignerr <<"3seconds passed\n"; - return true; - } - return false; - }); - auto srvCb2 = std::function( - [&recvCount](const auto &_req, auto &_rep) - { - ignerr <<"IN srv-2 CALLBACK\n"; - EXPECT_EQ(_req.data(), true); - if (_req.data() == true) - { - ++recvCount; - _rep.set_data("meh"); - ignerr <<"starting to wait\n"; - IGN_SLEEP_MS(200); - ignerr <<"3seconds passed\n"; - return true; - } - return false; - }); - - auto srvCb3 = std::function( - [&recvCount](const auto &_req) - { - ignerr <<"IN srv-3 CALLBACK\n"; - EXPECT_EQ(_req.data(), "test"); - if (_req.data() == "test") - { - ++recvCount; - ignerr <<"starting to wait\n"; - IGN_SLEEP_MS(100); - ignerr <<"3seconds passed\n"; - ignerr <<"DONE\n"; - } - }); - - auto srvCb4 = std::function( - [&recvCount](auto &rep) - { - ignerr <<"IN srv-4 CALLBACK\n"; - { - ++recvCount; - rep.set_data("callback srv-4"); - ignerr <<"starting to wait\n"; - IGN_SLEEP_MS(100); - ignerr <<"3seconds passed\n"; - ignerr <<"DONE4444\n"; - return true; - } - }); - // Advertise a dummy service - // std::string service = "/srv-test-th"; - node.Advertise("srv-0", srvCb0); - node.Advertise("srv-1", srvCb1); - node.Advertise("srv-2", srvCb2); - node.Advertise("srv-3", srvCb3); - node.Advertise("srv-4", srvCb4); - //node.Advertise("srv-0", srvEchoCb2); -// node.Advertise(service, srvEchoCb1); - -// node.Advertise("/test1", srvE); + auto srvCb = std::function + ([&recvCount](auto &rep) + { + ++recvCount; + rep.set_data("srv-4"); + return true; + }); + node.Advertise("srv-4", srvCb); const std::size_t pubCount{10}; for (std::size_t i = 0; i < pubCount; ++i) { EXPECT_TRUE(inputPub.Publish(msgs::Empty())); - ignerr <<"publishing " << i <("/in_25"); +// std::atomic recvCount{0}; +// +// auto srvCb = std::function +// ([&recvCount](const auto &req, auto &rep) +// { +// EXPECT_EQ(req.unused(), true); +// if (req.unused() == true) +// { +// ++recvCount; +// rep.set_unused(false); +// return true; +// } +// return false; +// }); +// node.Advertise("srv-5", srvCb); +// +// const std::size_t pubCount{10}; +// for (std::size_t i = 0; i < pubCount; ++i) +// { +// EXPECT_TRUE(inputPub.Publish(msgs::Empty())); +// IGN_SLEEP_MS(100); +// } +// +// waitUntil(5000, [&]{return pubCount == recvCount;}); +// EXPECT_EQ(pubCount, recvCount); +//} +// +/////////////////////////////////////////////////// +//// Test for triggering a service call for Pose +//// Request and Boolean Reply type +//// --- +//// Request Type: msgs::Pose +//// Reply Type: msgs::StringMsg +/////////////////////////////////////////////////// +//TEST_F(TriggeredPublisherTest, +// IGN_UTILS_TEST_DISABLED_ON_WIN32(CallingPoseReqTypeServ)) +//{ +// transport::Node node; +// auto inputPub = node.Advertise("/in_26"); +// std::atomic recvCount{0}; +// +// auto srvCb = std::function([&recvCount](const auto &req, auto &rep) +// { +// EXPECT_EQ(req.name(), "blue_vehicle"); +// EXPECT_EQ(req.id(), 8); +// EXPECT_EQ(req.position().x(), -3); +// EXPECT_EQ(req.position().z(), 0.325); +// +// if (req.name() == "blue_vehicle" && req.id() == 8 && +// req.position().x() == -3 && req.position().z() == 0.325) +// { +// ++recvCount; +// rep.set_data(true); +// return true; +// } +// return false; +// }); +// node.Advertise("srv-6", srvCb); +// +// const std::size_t pubCount{10}; +// for (std::size_t i = 0; i < pubCount; ++i) +// { +// EXPECT_TRUE(inputPub.Publish(msgs::Empty())); +// IGN_SLEEP_MS(100); +// } +// +// waitUntil(5000, [&]{return pubCount == recvCount;}); +// EXPECT_EQ(pubCount, recvCount); +//} diff --git a/test/worlds/triggered_publisher.sdf b/test/worlds/triggered_publisher.sdf index f9b36ec390..d1dad06e45 100644 --- a/test/worlds/triggered_publisher.sdf +++ b/test/worlds/triggered_publisher.sdf @@ -293,6 +293,7 @@ reqMsg="data: false"> + @@ -301,32 +302,81 @@ name="/srv-0" reqType="ignition.msgs.StringMsg" repType="ignition.msgs.StringMsg" - reqMsg="data: 'test'"> + reqMsg="data: 'srv-0'"> + + + + + reqMsg="data: 'srv-1'"> + + + + + + + + + reqMsg="data: 'srv-3'"> + + + + + + + + + + + + + + + + + From 6aca06fcaccdaadbb1df2fcc26195f32e1696d4e Mon Sep 17 00:00:00 2001 From: Liam Han Date: Thu, 4 Aug 2022 09:18:49 +0000 Subject: [PATCH 14/26] WIP major modification but a lot of errors and tests failed Signed-off-by: Liam Han --- .../triggered_publisher/TriggeredPublisher.cc | 33 +- .../triggered_publisher/TriggeredPublisher.hh | 136 +- test/integration/triggered_publisher.cc | 2035 +++++++++-------- test/worlds/triggered_publisher.sdf | 2 +- 4 files changed, 1150 insertions(+), 1056 deletions(-) diff --git a/src/systems/triggered_publisher/TriggeredPublisher.cc b/src/systems/triggered_publisher/TriggeredPublisher.cc index 4915f5092c..547564468c 100644 --- a/src/systems/triggered_publisher/TriggeredPublisher.cc +++ b/src/systems/triggered_publisher/TriggeredPublisher.cc @@ -641,15 +641,6 @@ void TriggeredPublisher::Configure(const Entity &, { ignwarn << "Service request string is empty\n"; } - // serviceInfo.timeout = serviceElem->Get("timeout"); - // if (serviceInfo.reqMsg.empty()) - // { - // ignwarn << "Service timeout is empty\n"; - // } - // else - // { - // serviceInfo.timeout = std::stoi(serviceInfo.timeout); - // } this->serviceOutputInfo.push_back(std::move(serviceInfo)); } } @@ -711,7 +702,6 @@ void TriggeredPublisher::Configure(const Entity &, this->serviceWorkerThread = std::thread(std::bind(&TriggeredPublisher::DoServiceWork, this)); } - ////////////////////////////////////////////////// #define HANDLE_REQUEST(requestT, typeString) \ @@ -721,8 +711,6 @@ if (((serviceInfo.reqType == typeString) | serviceInfo.reqType.empty()) && !isPr isProcessing = true; \ } \ - //this->HandleNoRequestMsg(serviceInfo); - void TriggeredPublisher::DoServiceWork() { while (!this->srvDone) @@ -741,15 +729,24 @@ void TriggeredPublisher::DoServiceWork() continue; } } - bool isProcessing {false}; for (auto &serviceInfo : this->serviceOutputInfo) { - HANDLE_REQUEST(msgs::Pose, "ignition.msgs.Pose"); - HANDLE_REQUEST(msgs::StringMsg, "ignition.msgs.StringMsg"); - HANDLE_REQUEST(msgs::Boolean, "ignition.msgs.Boolean"); - HANDLE_REQUEST(msgs::Empty, "ignition.msgs.Empty"); - ignerr<<"-------\n"; + // bool isProcessing {false}; + // HANDLE_REQUEST(msgs::Pose, "ignition.msgs.Pose"); + // HANDLE_REQUEST(msgs::StringMsg, "ignition.msgs.StringMsg"); + // HANDLE_REQUEST(msgs::Boolean, "ignition.msgs.Boolean"); + // HANDLE_REQUEST(msgs::Empty, "ignition.msgs.Empty"); //NOTE: add more protobuf msgs for the Request + + //CheckServRepType(msgs::Pose, "ignition.msgs.Pose"); + //CheckServRepType(msgs::StringMsg, "ignition.msgs.StringMsg"); + //CheckServRepType(msgs::Boolean, "ignition.msgs.Boolean"); + //CheckServRepType(msgs::Empty, "ignition.msgs.Empty"); + + Logic(serviceInfo, "ignition.msgs.Pose"); + Logic(serviceInfo, "ignition.msgs.StringMsg"); + Logic(serviceInfo, "ignition.msgs.Boolean"); + Logic(serviceInfo, "ignition.msgs.Empty"); } } } diff --git a/src/systems/triggered_publisher/TriggeredPublisher.hh b/src/systems/triggered_publisher/TriggeredPublisher.hh index e2039c6941..d61bab4b92 100644 --- a/src/systems/triggered_publisher/TriggeredPublisher.hh +++ b/src/systems/triggered_publisher/TriggeredPublisher.hh @@ -86,8 +86,6 @@ namespace systems /// * Attributes: /// * `name`: Service name (eg. `/world/triggered_publisher/set_pose`) /// * `reqType`: Service request message type (eg. ignition.msgs.Pose) - /// * `repType`: Service respond message type (eg. ignition.msgs.Boolean) - /// * `timeout`: Service request timeout /// * `reqMsg`: String used to construct the service protobuf message. /// /// Examples: @@ -239,6 +237,10 @@ namespace systems /// \brief Service request message std::string reqMsg; + + + bool reqTypeMatchFound{false}; + bool repTypeMatchFound{false}; }; /// \brief List of InputMatchers @@ -322,7 +324,10 @@ namespace systems { ignmsg << "Service call [" << srvInfo.servName << "] succeeded. Response message [" - << "]\n"; + << "name: " << msg.name() << "pos: {x: " + << msg.position().x() << ", y: " + << msg.position().y() << ", z: " + << msg.position().z() << "}]\n"; } //NOTE: add more replys for different types }; @@ -388,28 +393,87 @@ namespace systems isProcessing = true; \ } \ - private: template - void HandleRequest(ServiceOutputInfo& serviceInfo) + // private: template + // void HandleRequest(ServiceOutputInfo& serviceInfo) + // { + // bool executed {false}; + // bool isProcessing {false}; + // // msgs::Factory::New req; + // std::unique_ptr req; + // if (!serviceInfo.reqType.empty()) + // { + // req = msgs::Factory::New(serviceInfo.reqType, + // serviceInfo.reqMsg); + // if (!req) + // { + // ignerr << "Unable to create request for type [" + // << serviceInfo.reqType << "].\n"; + // return; + // } + // } + // HANDLE_REPLY(msgs::StringMsg, "ignition.msgs.StringMsg"); + // HANDLE_REPLY(msgs::Boolean, "ignition.msgs.Boolean"); + // HANDLE_REPLY(msgs::Empty, "ignition.msgs.Empty"); + // //NOTE: add more protobuf msgs for the Reply + + // if (!executed) + // { + // ignerr << "Service call [" << serviceInfo.servName + // << "] timed out\n"; + // } + // { + // std::lock_guard lock(this->serviceCountMutex); + // this->callService = false; + // } + // } + + + + + private: template + bool CallRequest(ServiceOutputInfo& serviceInfo, + std::string repTypeString, + const std::unique_ptr& req) { - bool executed {false}; - bool isProcessing {false}; - // msgs::Factory::New req; - std::unique_ptr req; - if (!serviceInfo.reqType.empty()) + if (serviceInfo.repTypeMatchFound) { - req = msgs::Factory::New(serviceInfo.reqType, - serviceInfo.reqMsg); - if (!req) + return false; + } + if (serviceInfo.repType == repTypeString) + { + serviceInfo.repTypeMatchFound = true; + + if (serviceInfo.reqType.empty()) { - ignerr << "Unable to create request for type [" - << serviceInfo.reqType << "].\n"; - return; + return HandleNoRequestMsg(serviceInfo); + } + + else if (serviceInfo.repType.empty()) + { + return HandleNoReply(serviceInfo, *req); + } + + else if (!serviceInfo.reqType.empty() && !serviceInfo.repType.empty()) + { + return HandleReply(serviceInfo, *req); } } - HANDLE_REPLY(msgs::StringMsg, "ignition.msgs.StringMsg"); - HANDLE_REPLY(msgs::Boolean, "ignition.msgs.Boolean"); - HANDLE_REPLY(msgs::Empty, "ignition.msgs.Empty"); - //NOTE: add more protobuf msgs for the Reply + return false; + } + + // function that takees reqT and repT template and figures out which fucntions to call + private: template + void Logic(ServiceOutputInfo& serviceInfo, std::string repTypeString) + { + bool executed {false}; + std::unique_ptr req; + bool result = CreateReqMsg(serviceInfo, repTypeString, req); + if (!result) + return; + + executed = CallRequest(serviceInfo, "ignition.msgs.StringMsg", req); + executed = CallRequest(serviceInfo, "ignition.msgs.Boolean", req); + executed = CallRequest(serviceInfo, "ignition.msgs.Empty", req); if (!executed) { @@ -421,8 +485,38 @@ namespace systems this->callService = false; } } + + + private: template + bool CreateReqMsg(ServiceOutputInfo& serviceInfo, + std::string reqTypeString, + std::unique_ptr& r) + { + if (serviceInfo.reqTypeMatchFound) + { + return false; + } + // std::unique_ptr req; + if (serviceInfo.reqType == reqTypeString) + {// if reqMsg missingjust create one without it + r = msgs::Factory::New(serviceInfo.reqType, + serviceInfo.reqMsg); + } + else if (serviceInfo.reqType.empty()) + { + return true; +//set serviceinfo requesttypeenum to e.g.2 + } + return false; + } + }; -} + + //method that just creates msgType unique ptr => will require a map + // mapping of string and unq ptr + + +}// } } } diff --git a/test/integration/triggered_publisher.cc b/test/integration/triggered_publisher.cc index a5256f0a9c..134128dcca 100644 --- a/test/integration/triggered_publisher.cc +++ b/test/integration/triggered_publisher.cc @@ -88,939 +88,941 @@ bool waitUntil(int _timeoutMs, Pred _pred) return false; } -/////////////////////////////////////////////////// -///// Check that empty message types do not need any data to be specified in the -///// configuration -//// See https://github.com/ignitionrobotics/ign-gazebo/issues/1175 -//TEST_F(TriggeredPublisherTest, -// IGN_UTILS_TEST_DISABLED_ON_WIN32(EmptyInputEmptyOutput)) -//{ -// transport::Node node; -// auto inputPub = node.Advertise("/in_0"); -// std::atomic recvCount{0}; -// auto msgCb = std::function( -// [&recvCount](const auto &) -// { -// ++recvCount; -// }); -// node.Subscribe("/out_0", msgCb); -// IGN_SLEEP_MS(100ms); -// -// const std::size_t pubCount{10}; -// for (std::size_t i = 0; i < pubCount; ++i) -// { -// EXPECT_TRUE(inputPub.Publish(msgs::Empty())); -// } -// waitUntil(5000, [&]{return pubCount == recvCount;}); -// -// EXPECT_EQ(pubCount, recvCount); -//} -// -/////////////////////////////////////////////////// -//TEST_F(TriggeredPublisherTest, -// IGN_UTILS_TEST_DISABLED_ON_WIN32(WrongInputMessageTypeDoesNotMatch)) -//{ -// transport::Node node; -// auto inputPub = node.Advertise("/in_0"); -// std::atomic recvCount{0}; -// auto msgCb = std::function( -// [&recvCount](const auto &) -// { -// ++recvCount; -// }); -// node.Subscribe("/out_0", msgCb); -// -// const std::size_t pubCount{10}; -// for (std::size_t i = 0; i < pubCount; ++i) -// { -// EXPECT_TRUE(inputPub.Publish(msgs::Boolean())); -// } -// -// waitUntil(5000, [&]{return 0u == recvCount;}); -// EXPECT_EQ(0u, recvCount); -//} -// -/////////////////////////////////////////////////// -//TEST_F(TriggeredPublisherTest, -// IGN_UTILS_TEST_DISABLED_ON_WIN32(InputMessagesTriggerOutputs)) -//{ -// transport::Node node; -// auto inputPub = node.Advertise("/in_1"); -// std::atomic recvCount{0}; -// auto msgCb = std::function( -// [&recvCount](const auto &_msg) -// { -// EXPECT_TRUE(_msg.data()); -// ++recvCount; -// }); -// node.Subscribe("/out_1", msgCb); -// -// const std::size_t pubCount{10}; -// for (std::size_t i = 0; i < pubCount; ++i) -// { -// EXPECT_TRUE(inputPub.Publish(msgs::Empty())); -// IGN_SLEEP_MS(10); -// } -// -// waitUntil(5000, [&]{return pubCount == recvCount;}); -// EXPECT_EQ(pubCount, recvCount); -//} -// -/////////////////////////////////////////////////// -//TEST_F(TriggeredPublisherTest, -// IGN_UTILS_TEST_DISABLED_ON_WIN32(MultipleOutputsForOneInput)) -//{ -// transport::Node node; -// auto inputPub = node.Advertise("/in_2"); -// std::mutex recvMsgMutex; -// std::vector recvMsgs0; -// std::vector recvMsgs1; -// auto cbCreator = [&recvMsgMutex](std::vector &_msgVector) -// { -// return std::function( -// [&_msgVector, &recvMsgMutex](const msgs::Boolean &_msg) -// { -// std::lock_guard lock(recvMsgMutex); -// _msgVector.push_back(_msg.data()); -// }); -// }; -// -// auto msgCb0 = cbCreator(recvMsgs0); -// auto msgCb1 = cbCreator(recvMsgs1); -// node.Subscribe("/out_2_0", msgCb0); -// node.Subscribe("/out_2_1", msgCb1); -// -// const int pubCount{10}; -// for (int i = 0; i < pubCount; ++i) -// { -// EXPECT_TRUE(inputPub.Publish(msgs::Empty())); -// IGN_SLEEP_MS(10); -// } -// -// waitUntil(5000, [&] -// { -// std::lock_guard lock(recvMsgMutex); -// return static_cast(pubCount) == recvMsgs0.size() && -// static_cast(pubCount) == recvMsgs1.size(); -// }); -// -// EXPECT_EQ(static_cast(pubCount), recvMsgs0.size()); -// EXPECT_EQ(static_cast(pubCount), recvMsgs1.size()); -// -// // The plugin has two outputs. We expect 10 messages in each output topic -// EXPECT_EQ(pubCount, std::count(recvMsgs0.begin(), recvMsgs0.end(), false)); -// EXPECT_EQ(pubCount, std::count(recvMsgs1.begin(), recvMsgs1.end(), true)); -//} -// -/////////////////////////////////////////////////// -//TEST_F(TriggeredPublisherTest, -// IGN_UTILS_TEST_DISABLED_ON_WIN32(ExactMatchBooleanInputs)) -//{ -// transport::Node node; -// auto inputPub = node.Advertise("/in_3"); -// std::atomic recvCount{0}; -// auto msgCb = std::function( -// [&recvCount](const auto &) -// { -// ++recvCount; -// }); -// node.Subscribe("/out_3", msgCb); -// -// const std::size_t pubCount{10}; -// const std::size_t trueCount{5}; -// for (std::size_t i = 0; i < pubCount; ++i) -// { -// if (i < trueCount) -// { -// EXPECT_TRUE(inputPub.Publish(msgs::Convert(true))); -// } -// else -// { -// EXPECT_TRUE(inputPub.Publish(msgs::Convert(false))); -// } -// IGN_SLEEP_MS(10); -// } -// -// // The matcher filters out false messages and the inputs consist of 5 true and -// // 5 false messages, so we expect 5 output messages -// EXPECT_EQ(trueCount, recvCount); -//} -// -/////////////////////////////////////////////////// -//TEST_F(TriggeredPublisherTest, -// IGN_UTILS_TEST_DISABLED_ON_WIN32(MatchersWithLogicTypeAttribute)) -//{ -// transport::Node node; -// auto inputPub = node.Advertise("/in_4"); -// std::atomic recvCount[2]{0, 0}; -// -// auto cbCreator = [](std::atomic &_counter) -// { -// return std::function( -// [&_counter](const msgs::Empty &) -// { -// ++_counter; -// }); -// }; -// -// auto msgCb0 = cbCreator(recvCount[0]); -// auto msgCb1 = cbCreator(recvCount[1]); -// node.Subscribe("/out_4_0", msgCb0); -// node.Subscribe("/out_4_1", msgCb1); -// -// const int pubCount{10}; -// for (int i = 0; i < pubCount; ++i) -// { -// EXPECT_TRUE(inputPub.Publish( -// msgs::Convert(static_cast(i - pubCount / 2)))); -// IGN_SLEEP_MS(10); -// } -// // The negative matcher filters out 0 so we expect 9 output messages from the -// // 10 inputs -// EXPECT_EQ(9u, recvCount[0]); -// -// // The positive matcher only accepts the input value 0 -// EXPECT_EQ(1u, recvCount[1]); -//} -// -/////////////////////////////////////////////////// -//TEST_F(TriggeredPublisherTest, -// IGN_UTILS_TEST_DISABLED_ON_WIN32(MultipleMatchersAreAnded)) -//{ -// transport::Node node; -// auto inputPub = node.Advertise("/in_5"); -// std::atomic recvCount{0}; -// auto msgCb = std::function( -// [&recvCount](const auto &) -// { -// ++recvCount; -// }); -// node.Subscribe("/out_5", msgCb); -// -// const int pubCount{10}; -// for (int i = 0; i < pubCount; ++i) -// { -// EXPECT_TRUE(inputPub.Publish( -// msgs::Convert(static_cast(i - pubCount / 2)))); -// IGN_SLEEP_MS(10); -// } -// // The matcher filters out negative numbers and the input is [-5,4], so we -// // expect 5 output messages. -// EXPECT_EQ(5u, recvCount); -//} -// -/////////////////////////////////////////////////// -//TEST_F(TriggeredPublisherTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(FieldMatchers)) -//{ -// transport::Node node; -// auto inputPub = node.Advertise("/in_6"); -// std::atomic recvCount[2]{0, 0}; -// -// auto cbCreator = [](std::atomic &_counter) -// { -// return std::function( -// [&_counter](const msgs::Empty &) -// { -// ++_counter; -// }); -// }; -// -// auto msgCb0 = cbCreator(recvCount[0]); -// auto msgCb1 = cbCreator(recvCount[1]); -// node.Subscribe("/out_6_0", msgCb0); -// node.Subscribe("/out_6_1", msgCb1); -// -// const int pubCount{10}; -// msgs::Vector2d msg; -// msg.set_x(1.0); -// for (int i = 0; i < pubCount; ++i) -// { -// msg.set_y(static_cast(i)); -// EXPECT_TRUE(inputPub.Publish(msg)); -// IGN_SLEEP_MS(10); -// } -// -// // The first plugin matches x==1 and y==2 which only once out of the 10 inputs -// EXPECT_EQ(1u, recvCount[0]); -// // The second plugin matches x==1 and y!=2 which occurs 9 out of the 10 inputs -// EXPECT_EQ(9u, recvCount[1]); -//} -// -/////////////////////////////////////////////////// -///// Tests that if the specified field is a repeated field, a partial match is -///// used when comparing against the input. -//TEST_F(TriggeredPublisherTest, -// IGN_UTILS_TEST_DISABLED_ON_WIN32( -// FieldMatchersWithRepeatedFieldsUsePartialMatches)) -//{ -// transport::Node node; -// auto inputPub = node.Advertise("/in_7"); -// std::atomic recvCount{0}; -// auto msgCb = std::function( -// [&recvCount](const auto &) -// { -// ++recvCount; -// }); -// node.Subscribe("/out_7", msgCb); -// -// const int pubCount{10}; -// for (int i = 0; i < pubCount; ++i) -// { -// msgs::Pose poseMsg; -// auto *frame = poseMsg.mutable_header()->add_data(); -// frame->set_key("frame_id"); -// frame->add_value(std::string("frame") + std::to_string(i)); -// -// auto *time = poseMsg.mutable_header()->mutable_stamp(); -// time->set_sec(10); -// -// auto *other = poseMsg.mutable_header()->add_data(); -// other->set_key("other_key"); -// other->add_value("other_value"); -// EXPECT_TRUE(inputPub.Publish(poseMsg)); -// IGN_SLEEP_MS(10); -// } -// -// // The matcher filters out frame ids that are not frame0, so we expect 1 -// // output. Even though the data field contains other key-value pairs, since -// // repeated fields use partial matching, the matcher will match one of the -// // inputs. -// EXPECT_EQ(1u, recvCount); -//} -// -//TEST_F(TriggeredPublisherTest, -// IGN_UTILS_TEST_DISABLED_ON_WIN32(WrongInputWhenRepeatedSubFieldExpected)) -//{ -// transport::Node node; -// auto inputPub = node.Advertise("/in_7"); -// std::atomic recvCount{0}; -// auto msgCb = std::function( -// [&recvCount](const auto &) -// { -// ++recvCount; -// }); -// node.Subscribe("/out_7", msgCb); -// IGN_SLEEP_MS(10); -// -// const int pubCount{10}; -// msgs::Empty msg; -// for (int i = 0; i < pubCount; ++i) -// { -// EXPECT_TRUE(inputPub.Publish(msg)); -// IGN_SLEEP_MS(10); -// } -// -// EXPECT_EQ(0u, recvCount); -//} -// -/////////////////////////////////////////////////// -///// Tests that field matchers can be used to do a full match with repeated -///// fields by specifying the containing field of the repeated field in the -///// "field" attribute and setting the desired values of the repeated field in -///// the value of the tag. -//TEST_F(TriggeredPublisherTest, IGN_UTILS_TEST_DISABLED_ON_WIN32( -// FieldMatchersWithRepeatedFieldsInValueUseFullMatches)) -//{ -// transport::Node node; -// auto inputPub = node.Advertise("/in_8"); -// std::atomic recvCount{0}; -// auto msgCb = std::function( -// [&recvCount](const auto &) -// { -// ++recvCount; -// }); -// node.Subscribe("/out_8", msgCb); -// IGN_SLEEP_MS(10); -// -// const int pubCount{10}; -// for (int i = 0; i < pubCount; ++i) -// { -// msgs::Pose poseMsg; -// auto *frame = poseMsg.mutable_header()->add_data(); -// frame->set_key("frame_id"); -// frame->add_value("frame0"); -// -// if (i < 5) -// { -// auto *other = poseMsg.mutable_header()->add_data(); -// other->set_key("other_key"); -// other->add_value("other_value"); -// } -// EXPECT_TRUE(inputPub.Publish(poseMsg)); -// IGN_SLEEP_MS(10); -// } -// -// // Since the field specified in "field" is not a repeated field, a full match -// // is required to trigger an output. Only the first 5 input messages have the -// // second "data" entry, so the expected recvCount is 5. -// EXPECT_EQ(5u, recvCount); -//} -// -/////////////////////////////////////////////////// -///// Tests that full matchers can be used with repeated fields by specifying the -///// desired values of the repeated field in the value of the tag. The -///// message created from the value of must be a full match of the input. -//TEST_F(TriggeredPublisherTest, IGN_UTILS_TEST_DISABLED_ON_WIN32( -// FullMatchersWithRepeatedFieldsInValueUseFullMatches)) -//{ -// transport::Node node; -// auto inputPub = node.Advertise("/in_9"); -// std::atomic recvCount{0}; -// auto msgCb = std::function( -// [&recvCount](const auto &) -// { -// ++recvCount; -// }); -// node.Subscribe("/out_9", msgCb); -// IGN_SLEEP_MS(10); -// -// const int pubCount{10}; -// msgs::Int32_V msg; -// for (int i = 0; i < pubCount; ++i) -// { -// msg.add_data(i); -// EXPECT_TRUE(inputPub.Publish(msg)); -// IGN_SLEEP_MS(10); -// } -// -// // The input contains an increasing sets of sequences, {0}, {0,1}, {0,1,2}... -// // The matcher only matches {0,1} -// EXPECT_EQ(1u, recvCount); -//} -// -//TEST_F(TriggeredPublisherTest, -// IGN_UTILS_TEST_DISABLED_ON_WIN32(FullMatchersAcceptToleranceParam)) -//{ -// transport::Node node; -// auto inputPub = node.Advertise("/in_10"); -// std::atomic recvCount{0}; -// auto msgCb = std::function( -// [&recvCount](const auto &) -// { -// ++recvCount; -// }); -// node.Subscribe("/out_10", msgCb); -// IGN_SLEEP_MS(10); -// -// const int pubCount{10}; -// msgs::Float msg; -// for (int i = 0; i < pubCount; ++i) -// { -// msg.set_data(static_cast(i)* 0.1); -// EXPECT_TRUE(inputPub.Publish(msg)); -// IGN_SLEEP_MS(10); -// } -// -// // The input contains the sequence {0, 0.1, 0.2, ...}, the matcher is set to -// // match 0.5 with a tolerance of 0.15, so it should match {0.4, 0.5, 0.6} -// EXPECT_EQ(3u, recvCount); -//} -// -//TEST_F(TriggeredPublisherTest, -// IGN_UTILS_TEST_DISABLED_ON_WIN32(FieldMatchersAcceptToleranceParam)) -//{ -// transport::Node node; -// auto inputPub = node.Advertise("/in_11"); -// std::atomic recvCount{0}; -// auto msgCb = std::function( -// [&recvCount](const auto &) -// { -// ++recvCount; -// }); -// node.Subscribe("/out_11", msgCb); -// IGN_SLEEP_MS(10); -// -// const int pubCount{10}; -// msgs::Pose msg; -// for (int i = 0; i < pubCount; ++i) -// { -// msg.mutable_position()->set_x(0.1); -// msg.mutable_position()->set_z(static_cast(i)* 0.1); -// EXPECT_TRUE(inputPub.Publish(msg)); -// IGN_SLEEP_MS(10); -// } -// -// // The input contains the sequence {0, 0.1, 0.2, ...} in position.z, the -// // matcher is set to match 0.5 with a tolerance of 0.15, so it should match -// // {0.4, 0.5, 0.6} -// EXPECT_EQ(3u, recvCount); -//} -// -//TEST_F(TriggeredPublisherTest, -// IGN_UTILS_TEST_DISABLED_ON_WIN32(SubfieldsOfRepeatedFieldsNotSupported)) -//{ -// transport::Node node; -// auto inputPub = node.Advertise("/in_12"); -// std::atomic recvCount{0}; -// auto msgCb = std::function( -// [&recvCount](const auto &) -// { -// ++recvCount; -// }); -// node.Subscribe("/out_12", msgCb); -// IGN_SLEEP_MS(10); -// -// const int pubCount{10}; -// for (int i = 0; i < pubCount; ++i) -// { -// msgs::Header msg; -// auto *data = msg.add_data(); -// data->set_key("key1"); -// data->add_value("value1"); -// -// EXPECT_TRUE(inputPub.Publish(msg)); -// IGN_SLEEP_MS(10); -// } -// -// // Subfields of repeated fiealds are not supported, so no output should be -// // triggered. -// EXPECT_EQ(0u, recvCount); -//} -// -//TEST_F(TriggeredPublisherTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(TriggerDelay)) -//{ -// transport::Node node; -// auto inputPub = node.Advertise("/in_13"); -// std::atomic recvCount{0}; -// auto msgCb = std::function( -// [&recvCount](const auto &) -// { -// ++recvCount; -// }); -// node.Subscribe("/out_13", msgCb); -// IGN_SLEEP_MS(100ms); -// -// const std::size_t pubCount{10}; -// for (std::size_t i = 0; i < pubCount; ++i) -// { -// EXPECT_TRUE(inputPub.Publish(msgs::Empty())); -// } -// waitUntil(1000, [&]{return pubCount == recvCount;}); -// -// // Delay has been specified, but simulation is not running. No messages -// // should have been received. -// EXPECT_EQ(0u, recvCount); -// -// // The simulation delay is 1000ms, which is equal to 1000 steps. Run -// // for 999 steps, and the count should still be zero. Take one additional -// // step and all the messages should arrive. -// this->server->Run(true, 999, false); -// waitUntil(1000, [&]{return pubCount == recvCount;}); -// EXPECT_EQ(0u, recvCount); -// this->server->Run(true, 1, false); -// waitUntil(1000, [&]{return pubCount == recvCount;}); -// EXPECT_EQ(pubCount, recvCount); -//} -// -//TEST_F(TriggeredPublisherTest, -// IGN_UTILS_TEST_DISABLED_ON_WIN32(WrongInputWhenRepeatedFieldExpected)) -//{ -// transport::Node node; -// auto inputPub = node.Advertise("/invalid_topic"); -// std::atomic recvCount{0}; -// auto msgCb = std::function( -// [&recvCount](const auto &) -// { -// ++recvCount; -// }); -// node.Subscribe("/out_9", msgCb); -// IGN_SLEEP_MS(10); -// -// const int pubCount{10}; -// msgs::Int32 msg; -// for (int i = 0; i < pubCount; ++i) -// { -// msg.set_data(i); -// EXPECT_TRUE(inputPub.Publish(msg)); -// IGN_SLEEP_MS(10); -// } -// -// EXPECT_EQ(0u, recvCount); -//} -// -/////////////////////////////////////////////////// -//// Test for invalid service name. It'll timeout -//// when matching service name can't be found. -/////////////////////////////////////////////////// -//TEST_F(TriggeredPublisherTest, -// IGN_UTILS_TEST_DISABLED_ON_WIN32(InvalidServiceName)) -//{ -// transport::Node node; -// auto inputPub = node.Advertise("/in_14"); -// std::atomic recvCount{0}; -// -// auto srvEchoCb = std::function( -// [&recvCount](const auto &req, auto &rep) -// { -// EXPECT_EQ(req.data(), "test"); -// if (req.data() == "test") -// { -// ++recvCount; -// rep.set_data(req.data()); -// return true; -// } -// return false; -// }); -// -// // Advertise a dummy service -// std::string service = "/srv-dummy-test"; -// node.Advertise(service, srvEchoCb); -// -// const std::size_t pubCount{10}; -// for (std::size_t i = 0; i < pubCount; ++i) -// { -// EXPECT_TRUE(inputPub.Publish(msgs::Empty())); -// IGN_SLEEP_MS(100); -// } -// -// waitUntil(5000, [&]{return recvCount == 0u;}); -// EXPECT_EQ(recvCount, 0u); -//} -// -/////////////////////////////////////////////////// -//// Test for triggering a service call in response -//// to input ign msg by publishing 10 times. Service -//// call will also occur 10 times. It'll compare -//// pubCount and recvCount. -/////////////////////////////////////////////////// -//TEST_F(TriggeredPublisherTest, -// IGN_UTILS_TEST_DISABLED_ON_WIN32(InputMessagesTriggerServices)) -//{ -// transport::Node node; -// auto inputPub = node.Advertise("/in_14"); -// std::atomic recvCount{0}; -// -// auto srvEchoCb = std::function( -// [&recvCount](const auto &req, auto &rep) -// { -// EXPECT_EQ(req.data(), "test"); -// if (req.data() == "test") -// { -// ++recvCount; -// rep.set_data(req.data()); -// return true; -// } -// return false; -// }); -// -// // Advertise a dummy service -// std::string service = "/srv-test"; -// node.Advertise(service, srvEchoCb); -// -// const std::size_t pubCount{10}; -// for (std::size_t i = 0; i < pubCount; ++i) -// { -// EXPECT_TRUE(inputPub.Publish(msgs::Empty())); -// IGN_SLEEP_MS(100); -// } -// -// waitUntil(5000, [&]{return pubCount == recvCount;}); -// EXPECT_EQ(pubCount, recvCount); -//} -// -/////////////////////////////////////////////////// -//// Test for triggering multiple services in response -//// to a input ign msg by publishing 10 times. Service -//// call will also occur 10 times for each service. -//// It'll compare pubCount and recvCount for each -//// services. -/////////////////////////////////////////////////// -//TEST_F(TriggeredPublisherTest, -// IGN_UTILS_TEST_DISABLED_ON_WIN32(MultipleServiceForOneInput)) -//{ -// transport::Node node; -// auto inputPub = node.Advertise("/in_15"); -// std::mutex recvMsgMutex; -// std::vector recvMsgs0; -// std::vector recvMsgs1; -// auto cbCreator = [&recvMsgMutex](std::vector &_msgVector) -// { -// return std::function( -// [&_msgVector, &recvMsgMutex](const auto &req, auto &rep) -// { -// std::lock_guard lock(recvMsgMutex); -// if (req.data() || !req.data()) -// { -// _msgVector.push_back(req.data()); -// return true; -// } -// return false; -// }); -// }; -// -// auto msgCb0 = cbCreator(recvMsgs0); -// auto msgCb1 = cbCreator(recvMsgs1); -// -// // Advertise two dummy services -// node.Advertise("/srv-test-0", msgCb0); -// node.Advertise("/srv-test-1", msgCb1); -// -// const int pubCount{10}; -// for (int i = 0; i < pubCount; ++i) -// { -// EXPECT_TRUE(inputPub.Publish(msgs::Empty())); -// IGN_SLEEP_MS(10); -// } -// -// waitUntil(5000, [&] -// { -// std::lock_guard lock(recvMsgMutex); -// return static_cast(pubCount) == recvMsgs0.size() && -// static_cast(pubCount) == recvMsgs1.size(); -// }); -// -// EXPECT_EQ(static_cast(pubCount), recvMsgs0.size()); -// EXPECT_EQ(static_cast(pubCount), recvMsgs1.size()); -// -// // The plugin has two outputs. We expect 10 messages in each output topic -// EXPECT_EQ(pubCount, std::count(recvMsgs0.begin(), recvMsgs0.end(), true)); -// EXPECT_EQ(pubCount, std::count(recvMsgs1.begin(), recvMsgs1.end(), false)); -//} -// -/////////////////////////////////////////////////// -//// Test for triggering a service call with incorrect -//// request type or reseponse type. The server callback -//// will not be triggered hence the recvCount will be 0 -/////////////////////////////////////////////////// -//TEST_F(TriggeredPublisherTest, -// IGN_UTILS_TEST_DISABLED_ON_WIN32(WrongRequestOrResponseType)) -//{ -// transport::Node node; -// auto inputPub = node.Advertise("/in_16"); -// std::atomic recvCount{0}; -// -// auto srvEchoCb = std::function( -// [&recvCount](const auto &req, auto &rep) -// { -// EXPECT_EQ(req.data(), "test"); -// if (req.data() == "test") -// { -// ++recvCount; -// rep.set_data(req.data()); -// return true; -// } -// return false; -// }); -// -// // Advertise a dummy service -// std::string service = "/srv-test"; -// node.Advertise(service, srvEchoCb); -// -// const std::size_t pubCount{10}; -// for (std::size_t i = 0; i < pubCount; ++i) -// { -// EXPECT_TRUE(inputPub.Publish(msgs::Empty())); -// IGN_SLEEP_MS(100); -// } -// -// waitUntil(5000, [&]{return recvCount == 0u;}); -// EXPECT_EQ(0u, recvCount); -//} -// -/////////////////////////////////////////////////// -//// Test for triggering a service call that'll return -//// False result. The server callback will be triggered -//// but it'll +1 to the recvCout but return False. -/////////////////////////////////////////////////// -//TEST_F(TriggeredPublisherTest, -// IGN_UTILS_TEST_DISABLED_ON_WIN32(FailedReesultServiceCall)) -//{ -// transport::Node node; -// auto inputPub = node.Advertise("/in_17"); -// std::atomic recvCount{0}; -// -// auto srvEchoCb = std::function( -// [&recvCount](const auto &req, auto &rep) -// { -// EXPECT_EQ(req.data(), "test"); -// if (req.data() == "test") -// { -// ++recvCount; -// rep.set_data(req.data()); -// // return True was substitued with False -// return false; -// } -// }); -// -// // Advertise a dummy service -// std::string service = "/srv-test"; -// node.Advertise(service, srvEchoCb); -// -// const std::size_t pubCount{10}; -// for (std::size_t i = 0; i < pubCount; ++i) -// { -// EXPECT_TRUE(inputPub.Publish(msgs::Empty())); -// IGN_SLEEP_MS(100); -// } -// -// waitUntil(5000, [&]{return pubCount == recvCount;}); -// EXPECT_EQ(pubCount, recvCount); -//} - - - - - - - - -/////////////////////////////////////////////////// -//// Test for triggering a service call with same -//// Request and Reply type -//// --- -//// Request Type: msgs::StringMsg -//// Reply Type: msgs::StringMsg -/////////////////////////////////////////////////// -//TEST_F(TriggeredPublisherTest, -// IGN_UTILS_TEST_DISABLED_ON_WIN32(CallingSameRequestReplyTypeServ)) -//{ -// transport::Node node; -// auto inputPub = node.Advertise("/in_20"); -// std::atomic recvCount{0}; -// -// auto srvCb = std::function([&recvCount](const auto &req, auto &rep) -// { -// EXPECT_EQ(req.data(), "srv-0"); -// if (req.data() == "srv-0") -// { -// ++recvCount; -// rep.set_data(req.data()); -// return true; -// } -// return false; -// }); -// node.Advertise("srv-0", srvCb); -// -// const std::size_t pubCount{10}; -// for (std::size_t i = 0; i < pubCount; ++i) -// { -// EXPECT_TRUE(inputPub.Publish(msgs::Empty())); -// IGN_SLEEP_MS(100); -// } -// -// waitUntil(5000, [&]{return pubCount == recvCount;}); -// EXPECT_EQ(pubCount, recvCount); -//} -// -// -/////////////////////////////////////////////////// -//// Test for triggering a service call with different -//// Request and Reply type -//// --- -//// Request Type: msgs::StringMsg -//// Reply Type: msgs::Boolean -/////////////////////////////////////////////////// -//TEST_F(TriggeredPublisherTest, -// IGN_UTILS_TEST_DISABLED_ON_WIN32(CallingDifferntRequestReplyTypeServ1)) -//{ -// transport::Node node; -// auto inputPub = node.Advertise("/in_21"); -// std::atomic recvCount{0}; -// -// auto srvCb = std::function([&recvCount](const auto &req, auto &rep) -// { -// EXPECT_EQ(req.data(), "srv-1"); -// if (req.data() == "srv-1") -// { -// ++recvCount; -// rep.set_data(true); -// return true; -// } -// return false; -// }); -// node.Advertise("srv-1", srvCb); -// -// const std::size_t pubCount{10}; -// for (std::size_t i = 0; i < pubCount; ++i) -// { -// EXPECT_TRUE(inputPub.Publish(msgs::Empty())); -// IGN_SLEEP_MS(100); -// } -// -// waitUntil(5000, [&]{return pubCount == recvCount;}); -// EXPECT_EQ(pubCount, recvCount); -//} -// -/////////////////////////////////////////////////// -//// Test for triggering a service call with different -//// Request and Reply type -//// --- -//// Request Type: msgs::Boolean -//// Reply Type: msgs::StringMsg -/////////////////////////////////////////////////// -//TEST_F(TriggeredPublisherTest, -// IGN_UTILS_TEST_DISABLED_ON_WIN32(CallingDifferntRequestReplyTypeServ2)) -//{ -// transport::Node node; -// auto inputPub = node.Advertise("/in_22"); -// std::atomic recvCount{0}; -// -// auto srvCb = std::function([&recvCount](const auto &req, auto &rep) -// { -// EXPECT_EQ(req.data(), true); -// if (req.data() == true) -// { -// ++recvCount; -// rep.set_data("srv-2"); -// return true; -// } -// return false; -// }); -// node.Advertise("srv-2", srvCb); -// -// const std::size_t pubCount{10}; -// for (std::size_t i = 0; i < pubCount; ++i) -// { -// EXPECT_TRUE(inputPub.Publish(msgs::Empty())); -// IGN_SLEEP_MS(100); -// } -// -// waitUntil(5000, [&]{return pubCount == recvCount;}); -// EXPECT_EQ(pubCount, recvCount); -//} -// -/////////////////////////////////////////////////// -//// Test for triggering a service call with no -//// Reply type specified -//// --- -//// Request Type: msgs::StringMsg -//// Reply Type: null -/////////////////////////////////////////////////// -//TEST_F(TriggeredPublisherTest, -// IGN_UTILS_TEST_DISABLED_ON_WIN32(CallingNoReplyTypeService)) -//{ -// transport::Node node; -// auto inputPub = node.Advertise("/in_23"); -// std::atomic recvCount{0}; -// -// auto srvCb = std::function -// ([&recvCount](const auto &req) -// { -// EXPECT_EQ(req.data(), "srv-3"); -// if (req.data() == "srv-3") -// { -// ++recvCount; -// } -// }); -// node.Advertise("srv-3", srvCb); -// -// const std::size_t pubCount{10}; -// for (std::size_t i = 0; i < pubCount; ++i) -// { -// EXPECT_TRUE(inputPub.Publish(msgs::Empty())); -// IGN_SLEEP_MS(100); -// } -// -// waitUntil(5000, [&]{return pubCount == recvCount;}); -// EXPECT_EQ(pubCount, recvCount); -//} +///////////////////////////////////////////////// +/// Check that empty message types do not need any data to be specified in the +/// configuration +// See https://github.com/ignitionrobotics/ign-gazebo/issues/1175 +TEST_F(TriggeredPublisherTest, + IGN_UTILS_TEST_DISABLED_ON_WIN32(EmptyInputEmptyOutput)) +{ + transport::Node node; + auto inputPub = node.Advertise("/in_0"); + std::atomic recvCount{0}; + auto msgCb = std::function( + [&recvCount](const auto &) + { + ++recvCount; + }); + node.Subscribe("/out_0", msgCb); + IGN_SLEEP_MS(100ms); + + const std::size_t pubCount{10}; + for (std::size_t i = 0; i < pubCount; ++i) + { + EXPECT_TRUE(inputPub.Publish(msgs::Empty())); + } + waitUntil(5000, [&]{return pubCount == recvCount;}); + + EXPECT_EQ(pubCount, recvCount); +} + +///////////////////////////////////////////////// +TEST_F(TriggeredPublisherTest, + IGN_UTILS_TEST_DISABLED_ON_WIN32(WrongInputMessageTypeDoesNotMatch)) +{ + transport::Node node; + auto inputPub = node.Advertise("/in_0"); + std::atomic recvCount{0}; + auto msgCb = std::function( + [&recvCount](const auto &) + { + ++recvCount; + }); + node.Subscribe("/out_0", msgCb); + + const std::size_t pubCount{10}; + for (std::size_t i = 0; i < pubCount; ++i) + { + EXPECT_TRUE(inputPub.Publish(msgs::Boolean())); + } + + waitUntil(5000, [&]{return 0u == recvCount;}); + EXPECT_EQ(0u, recvCount); +} + +///////////////////////////////////////////////// +TEST_F(TriggeredPublisherTest, + IGN_UTILS_TEST_DISABLED_ON_WIN32(InputMessagesTriggerOutputs)) +{ + transport::Node node; + auto inputPub = node.Advertise("/in_1"); + std::atomic recvCount{0}; + auto msgCb = std::function( + [&recvCount](const auto &_msg) + { + EXPECT_TRUE(_msg.data()); + ++recvCount; + }); + node.Subscribe("/out_1", msgCb); + + const std::size_t pubCount{10}; + for (std::size_t i = 0; i < pubCount; ++i) + { + EXPECT_TRUE(inputPub.Publish(msgs::Empty())); + IGN_SLEEP_MS(10); + } + + waitUntil(5000, [&]{return pubCount == recvCount;}); + EXPECT_EQ(pubCount, recvCount); +} + +///////////////////////////////////////////////// +TEST_F(TriggeredPublisherTest, + IGN_UTILS_TEST_DISABLED_ON_WIN32(MultipleOutputsForOneInput)) +{ + transport::Node node; + auto inputPub = node.Advertise("/in_2"); + std::mutex recvMsgMutex; + std::vector recvMsgs0; + std::vector recvMsgs1; + auto cbCreator = [&recvMsgMutex](std::vector &_msgVector) + { + return std::function( + [&_msgVector, &recvMsgMutex](const msgs::Boolean &_msg) + { + std::lock_guard lock(recvMsgMutex); + _msgVector.push_back(_msg.data()); + }); + }; + + auto msgCb0 = cbCreator(recvMsgs0); + auto msgCb1 = cbCreator(recvMsgs1); + node.Subscribe("/out_2_0", msgCb0); + node.Subscribe("/out_2_1", msgCb1); + + const int pubCount{10}; + for (int i = 0; i < pubCount; ++i) + { + EXPECT_TRUE(inputPub.Publish(msgs::Empty())); + IGN_SLEEP_MS(10); + } + + waitUntil(5000, [&] + { + std::lock_guard lock(recvMsgMutex); + return static_cast(pubCount) == recvMsgs0.size() && + static_cast(pubCount) == recvMsgs1.size(); + }); + + EXPECT_EQ(static_cast(pubCount), recvMsgs0.size()); + EXPECT_EQ(static_cast(pubCount), recvMsgs1.size()); + + // The plugin has two outputs. We expect 10 messages in each output topic + EXPECT_EQ(pubCount, std::count(recvMsgs0.begin(), recvMsgs0.end(), false)); + EXPECT_EQ(pubCount, std::count(recvMsgs1.begin(), recvMsgs1.end(), true)); +} + +///////////////////////////////////////////////// +TEST_F(TriggeredPublisherTest, + IGN_UTILS_TEST_DISABLED_ON_WIN32(ExactMatchBooleanInputs)) +{ + transport::Node node; + auto inputPub = node.Advertise("/in_3"); + std::atomic recvCount{0}; + auto msgCb = std::function( + [&recvCount](const auto &) + { + ++recvCount; + }); + node.Subscribe("/out_3", msgCb); + + const std::size_t pubCount{10}; + const std::size_t trueCount{5}; + for (std::size_t i = 0; i < pubCount; ++i) + { + if (i < trueCount) + { + EXPECT_TRUE(inputPub.Publish(msgs::Convert(true))); + } + else + { + EXPECT_TRUE(inputPub.Publish(msgs::Convert(false))); + } + IGN_SLEEP_MS(10); + } + + // The matcher filters out false messages and the inputs consist of 5 true and + // 5 false messages, so we expect 5 output messages + EXPECT_EQ(trueCount, recvCount); +} + +///////////////////////////////////////////////// +TEST_F(TriggeredPublisherTest, + IGN_UTILS_TEST_DISABLED_ON_WIN32(MatchersWithLogicTypeAttribute)) +{ + transport::Node node; + auto inputPub = node.Advertise("/in_4"); + std::atomic recvCount[2]{0, 0}; + + auto cbCreator = [](std::atomic &_counter) + { + return std::function( + [&_counter](const msgs::Empty &) + { + ++_counter; + }); + }; + + auto msgCb0 = cbCreator(recvCount[0]); + auto msgCb1 = cbCreator(recvCount[1]); + node.Subscribe("/out_4_0", msgCb0); + node.Subscribe("/out_4_1", msgCb1); + + const int pubCount{10}; + for (int i = 0; i < pubCount; ++i) + { + EXPECT_TRUE(inputPub.Publish( + msgs::Convert(static_cast(i - pubCount / 2)))); + IGN_SLEEP_MS(10); + } + // The negative matcher filters out 0 so we expect 9 output messages from the + // 10 inputs + EXPECT_EQ(9u, recvCount[0]); + + // The positive matcher only accepts the input value 0 + EXPECT_EQ(1u, recvCount[1]); +} + +///////////////////////////////////////////////// +TEST_F(TriggeredPublisherTest, + IGN_UTILS_TEST_DISABLED_ON_WIN32(MultipleMatchersAreAnded)) +{ + transport::Node node; + auto inputPub = node.Advertise("/in_5"); + std::atomic recvCount{0}; + auto msgCb = std::function( + [&recvCount](const auto &) + { + ++recvCount; + }); + node.Subscribe("/out_5", msgCb); + + const int pubCount{10}; + for (int i = 0; i < pubCount; ++i) + { + EXPECT_TRUE(inputPub.Publish( + msgs::Convert(static_cast(i - pubCount / 2)))); + IGN_SLEEP_MS(10); + } + // The matcher filters out negative numbers and the input is [-5,4], so we + // expect 5 output messages. + EXPECT_EQ(5u, recvCount); +} + +///////////////////////////////////////////////// +TEST_F(TriggeredPublisherTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(FieldMatchers)) +{ + transport::Node node; + auto inputPub = node.Advertise("/in_6"); + std::atomic recvCount[2]{0, 0}; + + auto cbCreator = [](std::atomic &_counter) + { + return std::function( + [&_counter](const msgs::Empty &) + { + ++_counter; + }); + }; + + auto msgCb0 = cbCreator(recvCount[0]); + auto msgCb1 = cbCreator(recvCount[1]); + node.Subscribe("/out_6_0", msgCb0); + node.Subscribe("/out_6_1", msgCb1); + + const int pubCount{10}; + msgs::Vector2d msg; + msg.set_x(1.0); + for (int i = 0; i < pubCount; ++i) + { + msg.set_y(static_cast(i)); + EXPECT_TRUE(inputPub.Publish(msg)); + IGN_SLEEP_MS(10); + } + + // The first plugin matches x==1 and y==2 which only once out of the 10 inputs + EXPECT_EQ(1u, recvCount[0]); + // The second plugin matches x==1 and y!=2 which occurs 9 out of the 10 inputs + EXPECT_EQ(9u, recvCount[1]); +} + +///////////////////////////////////////////////// +/// Tests that if the specified field is a repeated field, a partial match is +/// used when comparing against the input. +TEST_F(TriggeredPublisherTest, + IGN_UTILS_TEST_DISABLED_ON_WIN32( + FieldMatchersWithRepeatedFieldsUsePartialMatches)) +{ + transport::Node node; + auto inputPub = node.Advertise("/in_7"); + std::atomic recvCount{0}; + auto msgCb = std::function( + [&recvCount](const auto &) + { + ++recvCount; + }); + node.Subscribe("/out_7", msgCb); + + const int pubCount{10}; + for (int i = 0; i < pubCount; ++i) + { + msgs::Pose poseMsg; + auto *frame = poseMsg.mutable_header()->add_data(); + frame->set_key("frame_id"); + frame->add_value(std::string("frame") + std::to_string(i)); + + auto *time = poseMsg.mutable_header()->mutable_stamp(); + time->set_sec(10); + + auto *other = poseMsg.mutable_header()->add_data(); + other->set_key("other_key"); + other->add_value("other_value"); + EXPECT_TRUE(inputPub.Publish(poseMsg)); + IGN_SLEEP_MS(10); + } + + // The matcher filters out frame ids that are not frame0, so we expect 1 + // output. Even though the data field contains other key-value pairs, since + // repeated fields use partial matching, the matcher will match one of the + // inputs. + EXPECT_EQ(1u, recvCount); +} + +TEST_F(TriggeredPublisherTest, + IGN_UTILS_TEST_DISABLED_ON_WIN32(WrongInputWhenRepeatedSubFieldExpected)) +{ + transport::Node node; + auto inputPub = node.Advertise("/in_7"); + std::atomic recvCount{0}; + auto msgCb = std::function( + [&recvCount](const auto &) + { + ++recvCount; + }); + node.Subscribe("/out_7", msgCb); + IGN_SLEEP_MS(10); + + const int pubCount{10}; + msgs::Empty msg; + for (int i = 0; i < pubCount; ++i) + { + EXPECT_TRUE(inputPub.Publish(msg)); + IGN_SLEEP_MS(10); + } + + EXPECT_EQ(0u, recvCount); +} + +///////////////////////////////////////////////// +/// Tests that field matchers can be used to do a full match with repeated +/// fields by specifying the containing field of the repeated field in the +/// "field" attribute and setting the desired values of the repeated field in +/// the value of the tag. +TEST_F(TriggeredPublisherTest, IGN_UTILS_TEST_DISABLED_ON_WIN32( + FieldMatchersWithRepeatedFieldsInValueUseFullMatches)) +{ + transport::Node node; + auto inputPub = node.Advertise("/in_8"); + std::atomic recvCount{0}; + auto msgCb = std::function( + [&recvCount](const auto &) + { + ++recvCount; + }); + node.Subscribe("/out_8", msgCb); + IGN_SLEEP_MS(10); + + const int pubCount{10}; + for (int i = 0; i < pubCount; ++i) + { + msgs::Pose poseMsg; + auto *frame = poseMsg.mutable_header()->add_data(); + frame->set_key("frame_id"); + frame->add_value("frame0"); + + if (i < 5) + { + auto *other = poseMsg.mutable_header()->add_data(); + other->set_key("other_key"); + other->add_value("other_value"); + } + EXPECT_TRUE(inputPub.Publish(poseMsg)); + IGN_SLEEP_MS(10); + } + + // Since the field specified in "field" is not a repeated field, a full match + // is required to trigger an output. Only the first 5 input messages have the + // second "data" entry, so the expected recvCount is 5. + EXPECT_EQ(5u, recvCount); +} + +///////////////////////////////////////////////// +/// Tests that full matchers can be used with repeated fields by specifying the +/// desired values of the repeated field in the value of the tag. The +/// message created from the value of must be a full match of the input. +TEST_F(TriggeredPublisherTest, IGN_UTILS_TEST_DISABLED_ON_WIN32( + FullMatchersWithRepeatedFieldsInValueUseFullMatches)) +{ + transport::Node node; + auto inputPub = node.Advertise("/in_9"); + std::atomic recvCount{0}; + auto msgCb = std::function( + [&recvCount](const auto &) + { + ++recvCount; + }); + node.Subscribe("/out_9", msgCb); + IGN_SLEEP_MS(10); + + const int pubCount{10}; + msgs::Int32_V msg; + for (int i = 0; i < pubCount; ++i) + { + msg.add_data(i); + EXPECT_TRUE(inputPub.Publish(msg)); + IGN_SLEEP_MS(10); + } + + // The input contains an increasing sets of sequences, {0}, {0,1}, {0,1,2}... + // The matcher only matches {0,1} + EXPECT_EQ(1u, recvCount); +} + +TEST_F(TriggeredPublisherTest, + IGN_UTILS_TEST_DISABLED_ON_WIN32(FullMatchersAcceptToleranceParam)) +{ + transport::Node node; + auto inputPub = node.Advertise("/in_10"); + std::atomic recvCount{0}; + auto msgCb = std::function( + [&recvCount](const auto &) + { + ++recvCount; + }); + node.Subscribe("/out_10", msgCb); + IGN_SLEEP_MS(10); + + const int pubCount{10}; + msgs::Float msg; + for (int i = 0; i < pubCount; ++i) + { + msg.set_data(static_cast(i)* 0.1); + EXPECT_TRUE(inputPub.Publish(msg)); + IGN_SLEEP_MS(10); + } + + // The input contains the sequence {0, 0.1, 0.2, ...}, the matcher is set to + // match 0.5 with a tolerance of 0.15, so it should match {0.4, 0.5, 0.6} + EXPECT_EQ(3u, recvCount); +} + +TEST_F(TriggeredPublisherTest, + IGN_UTILS_TEST_DISABLED_ON_WIN32(FieldMatchersAcceptToleranceParam)) +{ + transport::Node node; + auto inputPub = node.Advertise("/in_11"); + std::atomic recvCount{0}; + auto msgCb = std::function( + [&recvCount](const auto &) + { + ++recvCount; + }); + node.Subscribe("/out_11", msgCb); + IGN_SLEEP_MS(10); + + const int pubCount{10}; + msgs::Pose msg; + for (int i = 0; i < pubCount; ++i) + { + msg.mutable_position()->set_x(0.1); + msg.mutable_position()->set_z(static_cast(i)* 0.1); + EXPECT_TRUE(inputPub.Publish(msg)); + IGN_SLEEP_MS(10); + } + + // The input contains the sequence {0, 0.1, 0.2, ...} in position.z, the + // matcher is set to match 0.5 with a tolerance of 0.15, so it should match + // {0.4, 0.5, 0.6} + EXPECT_EQ(3u, recvCount); +} + +TEST_F(TriggeredPublisherTest, + IGN_UTILS_TEST_DISABLED_ON_WIN32(SubfieldsOfRepeatedFieldsNotSupported)) +{ + transport::Node node; + auto inputPub = node.Advertise("/in_12"); + std::atomic recvCount{0}; + auto msgCb = std::function( + [&recvCount](const auto &) + { + ++recvCount; + }); + node.Subscribe("/out_12", msgCb); + IGN_SLEEP_MS(10); + + const int pubCount{10}; + for (int i = 0; i < pubCount; ++i) + { + msgs::Header msg; + auto *data = msg.add_data(); + data->set_key("key1"); + data->add_value("value1"); + + EXPECT_TRUE(inputPub.Publish(msg)); + IGN_SLEEP_MS(10); + } + + // Subfields of repeated fiealds are not supported, so no output should be + // triggered. + EXPECT_EQ(0u, recvCount); +} + +TEST_F(TriggeredPublisherTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(TriggerDelay)) +{ + transport::Node node; + auto inputPub = node.Advertise("/in_13"); + std::atomic recvCount{0}; + auto msgCb = std::function( + [&recvCount](const auto &) + { + ++recvCount; + }); + node.Subscribe("/out_13", msgCb); + IGN_SLEEP_MS(100ms); + + const std::size_t pubCount{10}; + for (std::size_t i = 0; i < pubCount; ++i) + { + EXPECT_TRUE(inputPub.Publish(msgs::Empty())); + } + waitUntil(1000, [&]{return pubCount == recvCount;}); + + // Delay has been specified, but simulation is not running. No messages + // should have been received. + EXPECT_EQ(0u, recvCount); + + // The simulation delay is 1000ms, which is equal to 1000 steps. Run + // for 999 steps, and the count should still be zero. Take one additional + // step and all the messages should arrive. + this->server->Run(true, 999, false); + waitUntil(1000, [&]{return pubCount == recvCount;}); + EXPECT_EQ(0u, recvCount); + this->server->Run(true, 1, false); + waitUntil(1000, [&]{return pubCount == recvCount;}); + EXPECT_EQ(pubCount, recvCount); +} + +TEST_F(TriggeredPublisherTest, + IGN_UTILS_TEST_DISABLED_ON_WIN32(WrongInputWhenRepeatedFieldExpected)) +{ + transport::Node node; + auto inputPub = node.Advertise("/invalid_topic"); + std::atomic recvCount{0}; + auto msgCb = std::function( + [&recvCount](const auto &) + { + ++recvCount; + }); + node.Subscribe("/out_9", msgCb); + IGN_SLEEP_MS(10); + + const int pubCount{10}; + msgs::Int32 msg; + for (int i = 0; i < pubCount; ++i) + { + msg.set_data(i); + EXPECT_TRUE(inputPub.Publish(msg)); + IGN_SLEEP_MS(10); + } + + EXPECT_EQ(0u, recvCount); +} + +///////////////////////////////////////////////// +// Test for invalid service name. It'll timeout +// when matching service name can't be found. +///////////////////////////////////////////////// +TEST_F(TriggeredPublisherTest, + IGN_UTILS_TEST_DISABLED_ON_WIN32(InvalidServiceName)) +{ + transport::Node node; + auto inputPub = node.Advertise("/in_14"); + std::atomic recvCount{0}; + + auto srvEchoCb = std::function( + [&recvCount](const auto &req, auto &rep) + { + EXPECT_EQ(req.data(), "test"); + if (req.data() == "test") + { + ++recvCount; + rep.set_data(req.data()); + return true; + } + return false; + }); + + // Advertise a dummy service + std::string service = "/srv-dummy-test"; + node.Advertise(service, srvEchoCb); + + const std::size_t pubCount{10}; + for (std::size_t i = 0; i < pubCount; ++i) + { + EXPECT_TRUE(inputPub.Publish(msgs::Empty())); + IGN_SLEEP_MS(100); + } + + waitUntil(5000, [&]{return recvCount == 0u;}); + EXPECT_EQ(recvCount, 0u); +} + +///////////////////////////////////////////////// +// Test for triggering a service call in response +// to input ign msg by publishing 10 times. Service +// call will also occur 10 times. It'll compare +// pubCount and recvCount. +///////////////////////////////////////////////// +TEST_F(TriggeredPublisherTest, + IGN_UTILS_TEST_DISABLED_ON_WIN32(InputMessagesTriggerServices)) +{ + transport::Node node; + auto inputPub = node.Advertise("/in_14"); + std::atomic recvCount{0}; + + auto srvEchoCb = std::function( + [&recvCount](const auto &req, auto &rep) + { + EXPECT_EQ(req.data(), "test"); + if (req.data() == "test") + { + ++recvCount; + rep.set_data(req.data()); + return true; + } + return false; + }); + + // Advertise a dummy service + std::string service = "/srv-test"; + node.Advertise(service, srvEchoCb); + + const std::size_t pubCount{10}; + for (std::size_t i = 0; i < pubCount; ++i) + { + EXPECT_TRUE(inputPub.Publish(msgs::Empty())); + IGN_SLEEP_MS(100); + } + + waitUntil(5000, [&]{return pubCount == recvCount;}); + EXPECT_EQ(pubCount, recvCount); +} + +///////////////////////////////////////////////// +// Test for triggering multiple services in response +// to a input ign msg by publishing 10 times. Service +// call will also occur 10 times for each service. +// It'll compare pubCount and recvCount for each +// services. +///////////////////////////////////////////////// +TEST_F(TriggeredPublisherTest, + IGN_UTILS_TEST_DISABLED_ON_WIN32(MultipleServiceForOneInput)) +{ + transport::Node node; + auto inputPub = node.Advertise("/in_15"); + std::mutex recvMsgMutex; + std::vector recvMsgs0; + std::vector recvMsgs1; + auto cbCreator = [&recvMsgMutex](std::vector &_msgVector) + { + return std::function( + [&_msgVector, &recvMsgMutex](const auto &req, auto &rep) + { + std::lock_guard lock(recvMsgMutex); + if (req.data() || !req.data()) + { + rep.set_data(true); + _msgVector.push_back(req.data()); + return true; + } + return false; + }); + }; + + auto msgCb0 = cbCreator(recvMsgs0); + auto msgCb1 = cbCreator(recvMsgs1); + + // Advertise two dummy services + node.Advertise("/srv-test-0", msgCb0); + node.Advertise("/srv-test-1", msgCb1); + + const int pubCount{10}; + for (int i = 0; i < pubCount; ++i) + { + EXPECT_TRUE(inputPub.Publish(msgs::Empty())); + IGN_SLEEP_MS(10); + } + + waitUntil(5000, [&] + { + std::lock_guard lock(recvMsgMutex); + return static_cast(pubCount) == recvMsgs0.size() && + static_cast(pubCount) == recvMsgs1.size(); + }); + + EXPECT_EQ(static_cast(pubCount), recvMsgs0.size()); + EXPECT_EQ(static_cast(pubCount), recvMsgs1.size()); + + // The plugin has two outputs. We expect 10 messages in each output topic + EXPECT_EQ(pubCount, std::count(recvMsgs0.begin(), recvMsgs0.end(), true)); + EXPECT_EQ(pubCount, std::count(recvMsgs1.begin(), recvMsgs1.end(), false)); +} + +///////////////////////////////////////////////// +// Test for triggering a service call with incorrect +// request type or reseponse type. The server callback +// will not be triggered hence the recvCount will be 0 +///////////////////////////////////////////////// +TEST_F(TriggeredPublisherTest, + IGN_UTILS_TEST_DISABLED_ON_WIN32(WrongRequestOrResponseType)) +{ + transport::Node node; + auto inputPub = node.Advertise("/in_16"); + std::atomic recvCount{0}; + + auto srvEchoCb = std::function( + [&recvCount](const auto &req, auto &rep) + { + EXPECT_EQ(req.data(), "test"); + if (req.data() == "test") + { + ++recvCount; + rep.set_data(req.data()); + return true; + } + return false; + }); + + // Advertise a dummy service + std::string service = "/srv-test"; + node.Advertise(service, srvEchoCb); + + const std::size_t pubCount{10}; + for (std::size_t i = 0; i < pubCount; ++i) + { + EXPECT_TRUE(inputPub.Publish(msgs::Empty())); + IGN_SLEEP_MS(100); + } + + waitUntil(5000, [&]{return recvCount == 0u;}); + EXPECT_EQ(0u, recvCount); +} + +///////////////////////////////////////////////// +// Test for triggering a service call that'll return +// False result. The server callback will be triggered +// but it'll +1 to the recvCout but return False. +///////////////////////////////////////////////// +TEST_F(TriggeredPublisherTest, + IGN_UTILS_TEST_DISABLED_ON_WIN32(FailedReesultServiceCall)) +{ + transport::Node node; + auto inputPub = node.Advertise("/in_17"); + std::atomic recvCount{0}; + + auto srvEchoCb = std::function( + [&recvCount](const auto &req, auto &rep) + { + EXPECT_EQ(req.data(), "test"); + if (req.data() == "test") + { + ++recvCount; + rep.set_data(req.data()); + // return True was substitued with False + return false; + } + return false; + }); + + // Advertise a dummy service + std::string service = "/srv-test"; + node.Advertise(service, srvEchoCb); + + const std::size_t pubCount{10}; + for (std::size_t i = 0; i < pubCount; ++i) + { + EXPECT_TRUE(inputPub.Publish(msgs::Empty())); + IGN_SLEEP_MS(100); + } + + waitUntil(5000, [&]{return pubCount == recvCount;}); + EXPECT_EQ(pubCount, recvCount); +} + + + + + + + + +///////////////////////////////////////////////// +// Test for triggering a service call with same +// Request and Reply type +// --- +// Request Type: msgs::StringMsg +// Reply Type: msgs::StringMsg +///////////////////////////////////////////////// +TEST_F(TriggeredPublisherTest, + IGN_UTILS_TEST_DISABLED_ON_WIN32(CallingSameRequestReplyTypeServ)) +{ + transport::Node node; + auto inputPub = node.Advertise("/in_20"); + std::atomic recvCount{0}; + + auto srvCb = std::function([&recvCount](const auto &req, auto &rep) + { + EXPECT_EQ(req.data(), "srv-0"); + if (req.data() == "srv-0") + { + ++recvCount; + rep.set_data(req.data()); + return true; + } + return false; + }); + node.Advertise("srv-0", srvCb); + + const std::size_t pubCount{10}; + for (std::size_t i = 0; i < pubCount; ++i) + { + EXPECT_TRUE(inputPub.Publish(msgs::Empty())); + IGN_SLEEP_MS(100); + } + + waitUntil(5000, [&]{return pubCount == recvCount;}); + EXPECT_EQ(pubCount, recvCount); +} + + +///////////////////////////////////////////////// +// Test for triggering a service call with different +// Request and Reply type +// --- +// Request Type: msgs::StringMsg +// Reply Type: msgs::Boolean +///////////////////////////////////////////////// +TEST_F(TriggeredPublisherTest, + IGN_UTILS_TEST_DISABLED_ON_WIN32(CallingDifferntRequestReplyTypeServ1)) +{ + transport::Node node; + auto inputPub = node.Advertise("/in_21"); + std::atomic recvCount{0}; + + auto srvCb = std::function([&recvCount](const auto &req, auto &rep) + { + EXPECT_EQ(req.data(), "srv-1"); + if (req.data() == "srv-1") + { + ++recvCount; + rep.set_data(true); + return true; + } + return false; + }); + node.Advertise("srv-1", srvCb); + + const std::size_t pubCount{10}; + for (std::size_t i = 0; i < pubCount; ++i) + { + EXPECT_TRUE(inputPub.Publish(msgs::Empty())); + IGN_SLEEP_MS(100); + } + + waitUntil(5000, [&]{return pubCount == recvCount;}); + EXPECT_EQ(pubCount, recvCount); +} + +///////////////////////////////////////////////// +// Test for triggering a service call with different +// Request and Reply type +// --- +// Request Type: msgs::Boolean +// Reply Type: msgs::StringMsg +///////////////////////////////////////////////// +TEST_F(TriggeredPublisherTest, + IGN_UTILS_TEST_DISABLED_ON_WIN32(CallingDifferntRequestReplyTypeServ2)) +{ + transport::Node node; + auto inputPub = node.Advertise("/in_22"); + std::atomic recvCount{0}; + + auto srvCb = std::function([&recvCount](const auto &req, auto &rep) + { + EXPECT_EQ(req.data(), true); + if (req.data() == true) + { + ++recvCount; + rep.set_data("srv-2"); + return true; + } + return false; + }); + node.Advertise("srv-2", srvCb); + + const std::size_t pubCount{10}; + for (std::size_t i = 0; i < pubCount; ++i) + { + EXPECT_TRUE(inputPub.Publish(msgs::Empty())); + IGN_SLEEP_MS(100); + } + + waitUntil(5000, [&]{return pubCount == recvCount;}); + EXPECT_EQ(pubCount, recvCount); +} + +///////////////////////////////////////////////// +// Test for triggering a service call with no +// Reply type specified +// --- +// Request Type: msgs::StringMsg +// Reply Type: null +///////////////////////////////////////////////// +TEST_F(TriggeredPublisherTest, + IGN_UTILS_TEST_DISABLED_ON_WIN32(CallingNoReplyTypeService)) +{ + transport::Node node; + auto inputPub = node.Advertise("/in_23"); + std::atomic recvCount{0}; + + auto srvCb = std::function + ([&recvCount](const auto &req) + { + EXPECT_EQ(req.data(), "srv-3"); + if (req.data() == "srv-3") + { + ++recvCount; + } + }); + node.Advertise("srv-3", srvCb); + + const std::size_t pubCount{10}; + for (std::size_t i = 0; i < pubCount; ++i) + { + EXPECT_TRUE(inputPub.Publish(msgs::Empty())); + IGN_SLEEP_MS(100); + } + + waitUntil(5000, [&]{return pubCount == recvCount;}); + EXPECT_EQ(pubCount, recvCount); +} @@ -1051,7 +1053,6 @@ TEST_F(TriggeredPublisherTest, for (std::size_t i = 0; i < pubCount; ++i) { EXPECT_TRUE(inputPub.Publish(msgs::Empty())); - ignerr <<"publing..\n"; IGN_SLEEP_MS(100); } @@ -1059,85 +1060,87 @@ TEST_F(TriggeredPublisherTest, EXPECT_EQ(pubCount, recvCount); } -/////////////////////////////////////////////////// -//// Test for triggering a service call with different -//// Request and Reply type -//// --- -//// Request Type: Empty -//// Reply Type: Empty -/////////////////////////////////////////////////// -//TEST_F(TriggeredPublisherTest, -// IGN_UTILS_TEST_DISABLED_ON_WIN32(CallingEmptyRequestTypeServ)) -//{ -// transport::Node node; -// auto inputPub = node.Advertise("/in_25"); -// std::atomic recvCount{0}; -// -// auto srvCb = std::function -// ([&recvCount](const auto &req, auto &rep) -// { -// EXPECT_EQ(req.unused(), true); -// if (req.unused() == true) -// { -// ++recvCount; -// rep.set_unused(false); -// return true; -// } -// return false; -// }); -// node.Advertise("srv-5", srvCb); -// -// const std::size_t pubCount{10}; -// for (std::size_t i = 0; i < pubCount; ++i) -// { -// EXPECT_TRUE(inputPub.Publish(msgs::Empty())); -// IGN_SLEEP_MS(100); -// } -// -// waitUntil(5000, [&]{return pubCount == recvCount;}); -// EXPECT_EQ(pubCount, recvCount); -//} -// -/////////////////////////////////////////////////// -//// Test for triggering a service call for Pose -//// Request and Boolean Reply type -//// --- -//// Request Type: msgs::Pose -//// Reply Type: msgs::StringMsg -/////////////////////////////////////////////////// -//TEST_F(TriggeredPublisherTest, -// IGN_UTILS_TEST_DISABLED_ON_WIN32(CallingPoseReqTypeServ)) -//{ -// transport::Node node; -// auto inputPub = node.Advertise("/in_26"); -// std::atomic recvCount{0}; -// -// auto srvCb = std::function([&recvCount](const auto &req, auto &rep) -// { -// EXPECT_EQ(req.name(), "blue_vehicle"); -// EXPECT_EQ(req.id(), 8); -// EXPECT_EQ(req.position().x(), -3); -// EXPECT_EQ(req.position().z(), 0.325); -// -// if (req.name() == "blue_vehicle" && req.id() == 8 && -// req.position().x() == -3 && req.position().z() == 0.325) -// { -// ++recvCount; -// rep.set_data(true); -// return true; -// } -// return false; -// }); -// node.Advertise("srv-6", srvCb); -// -// const std::size_t pubCount{10}; -// for (std::size_t i = 0; i < pubCount; ++i) -// { -// EXPECT_TRUE(inputPub.Publish(msgs::Empty())); -// IGN_SLEEP_MS(100); -// } -// -// waitUntil(5000, [&]{return pubCount == recvCount;}); -// EXPECT_EQ(pubCount, recvCount); -//} +///////////////////////////////////////////////// +// Test for triggering a service call with different +// Request and Reply type +// --- +// Request Type: Empty +// Reply Type: Empty +///////////////////////////////////////////////// +TEST_F(TriggeredPublisherTest, + IGN_UTILS_TEST_DISABLED_ON_WIN32(CallingEmptyRequestTypeServ)) +{ + transport::Node node; + auto inputPub = node.Advertise("/in_25"); + std::atomic recvCount{0}; + + auto srvCb = std::function + ([&recvCount](const auto &req, auto &rep) + { + EXPECT_EQ(req.unused(), true); + if (req.unused() == true) + { + ++recvCount; + rep.set_unused(false); + return true; + } + return false; + }); + node.Advertise("srv-5", srvCb); + + const std::size_t pubCount{10}; + for (std::size_t i = 0; i < pubCount; ++i) + { + EXPECT_TRUE(inputPub.Publish(msgs::Empty())); + IGN_SLEEP_MS(100); + } + + waitUntil(5000, [&]{return pubCount == recvCount;}); + EXPECT_EQ(pubCount, recvCount); +} + +///////////////////////////////////////////////// +// Test for triggering a service call for Pose +// Request and Boolean Reply type +// --- +// Request Type: msgs::Pose +// Reply Type: msgs::StringMsg +///////////////////////////////////////////////// +TEST_F(TriggeredPublisherTest, + IGN_UTILS_TEST_DISABLED_ON_WIN32(CallingPoseReqTypeServ)) +{ + transport::Node node; + auto inputPub = node.Advertise("/in_26"); + std::atomic recvCount{0}; + + auto srvCb = std::function([&recvCount](const auto &req, auto &rep) + { + EXPECT_EQ(req.name(), "blue_vehicle"); + EXPECT_FLOAT_EQ(req.id(), 8); + EXPECT_FLOAT_EQ(req.position().x(), 3); + EXPECT_FLOAT_EQ(req.position().z(), 0.325); + float tol = 0.001; + if (req.name() == "blue_vehicle" && + fabs(req.id() - 8) < tol && + fabs(req.position().x() - 3) < tol && + fabs(req.position().z() - 0.325) + reqMsg="name: 'blue_vehicle', id: 8, position: {x: 3, z: 0.325}"> From aa5090b1ca58d9ab86b0c2e5e4d90d8ae7c0981b Mon Sep 17 00:00:00 2001 From: Liam Han Date: Thu, 4 Aug 2022 10:20:18 +0000 Subject: [PATCH 15/26] stable version: had to revert back to previous work. all tests passed Signed-off-by: Liam Han --- .../triggered_publisher/TriggeredPublisher.cc | 23 +-- .../triggered_publisher/TriggeredPublisher.hh | 164 ++++-------------- 2 files changed, 41 insertions(+), 146 deletions(-) diff --git a/src/systems/triggered_publisher/TriggeredPublisher.cc b/src/systems/triggered_publisher/TriggeredPublisher.cc index 547564468c..7641882000 100644 --- a/src/systems/triggered_publisher/TriggeredPublisher.cc +++ b/src/systems/triggered_publisher/TriggeredPublisher.cc @@ -705,7 +705,8 @@ void TriggeredPublisher::Configure(const Entity &, ////////////////////////////////////////////////// #define HANDLE_REQUEST(requestT, typeString) \ -if (((serviceInfo.reqType == typeString) | serviceInfo.reqType.empty()) && !isProcessing) \ +if (((serviceInfo.reqType == typeString) | serviceInfo.reqType.empty()) \ + && !isProcessing) \ { \ this->HandleRequest(serviceInfo); \ isProcessing = true; \ @@ -731,22 +732,12 @@ void TriggeredPublisher::DoServiceWork() } for (auto &serviceInfo : this->serviceOutputInfo) { - // bool isProcessing {false}; - // HANDLE_REQUEST(msgs::Pose, "ignition.msgs.Pose"); - // HANDLE_REQUEST(msgs::StringMsg, "ignition.msgs.StringMsg"); - // HANDLE_REQUEST(msgs::Boolean, "ignition.msgs.Boolean"); - // HANDLE_REQUEST(msgs::Empty, "ignition.msgs.Empty"); + bool isProcessing {false}; + HANDLE_REQUEST(msgs::Pose, "ignition.msgs.Pose"); + HANDLE_REQUEST(msgs::StringMsg, "ignition.msgs.StringMsg"); + HANDLE_REQUEST(msgs::Boolean, "ignition.msgs.Boolean"); + HANDLE_REQUEST(msgs::Empty, "ignition.msgs.Empty"); //NOTE: add more protobuf msgs for the Request - - //CheckServRepType(msgs::Pose, "ignition.msgs.Pose"); - //CheckServRepType(msgs::StringMsg, "ignition.msgs.StringMsg"); - //CheckServRepType(msgs::Boolean, "ignition.msgs.Boolean"); - //CheckServRepType(msgs::Empty, "ignition.msgs.Empty"); - - Logic(serviceInfo, "ignition.msgs.Pose"); - Logic(serviceInfo, "ignition.msgs.StringMsg"); - Logic(serviceInfo, "ignition.msgs.Boolean"); - Logic(serviceInfo, "ignition.msgs.Empty"); } } } diff --git a/src/systems/triggered_publisher/TriggeredPublisher.hh b/src/systems/triggered_publisher/TriggeredPublisher.hh index d61bab4b92..7085604e17 100644 --- a/src/systems/triggered_publisher/TriggeredPublisher.hh +++ b/src/systems/triggered_publisher/TriggeredPublisher.hh @@ -237,10 +237,6 @@ namespace systems /// \brief Service request message std::string reqMsg; - - - bool reqTypeMatchFound{false}; - bool repTypeMatchFound{false}; }; /// \brief List of InputMatchers @@ -361,9 +357,6 @@ namespace systems private: template bool HandleNoRequestMsg(ServiceOutputInfo& serviceInfo) { - // Non-blocking request with no request type - //ignerr<<"here\n"; - auto reqCb = ReplyCallback(serviceInfo); return this->node.Request(serviceInfo.servName, reqCb); } @@ -376,104 +369,45 @@ namespace systems } #define HANDLE_REPLY(repT, typeString) \ - if (((serviceInfo.repType == typeString) && \ - (serviceInfo.reqType.empty())) && !isProcessing) \ - { \ - executed = HandleNoRequestMsg(serviceInfo); \ - isProcessing = true; \ - } \ + if (serviceInfo.repType == typeString && \ + serviceInfo.reqType.empty() && !isProcessing) \ + { \ + executed = HandleNoRequestMsg(serviceInfo); \ + isProcessing = true; \ + } \ else if (serviceInfo.repType.empty() && !isProcessing) \ - { \ - executed = HandleNoReply(serviceInfo, *req); \ - isProcessing = true; \ - } \ - else if((serviceInfo.repType == typeString) && !isProcessing)\ - { \ - executed = HandleReply(serviceInfo, *req); \ - isProcessing = true; \ - } \ - - // private: template - // void HandleRequest(ServiceOutputInfo& serviceInfo) - // { - // bool executed {false}; - // bool isProcessing {false}; - // // msgs::Factory::New req; - // std::unique_ptr req; - // if (!serviceInfo.reqType.empty()) - // { - // req = msgs::Factory::New(serviceInfo.reqType, - // serviceInfo.reqMsg); - // if (!req) - // { - // ignerr << "Unable to create request for type [" - // << serviceInfo.reqType << "].\n"; - // return; - // } - // } - // HANDLE_REPLY(msgs::StringMsg, "ignition.msgs.StringMsg"); - // HANDLE_REPLY(msgs::Boolean, "ignition.msgs.Boolean"); - // HANDLE_REPLY(msgs::Empty, "ignition.msgs.Empty"); - // //NOTE: add more protobuf msgs for the Reply - - // if (!executed) - // { - // ignerr << "Service call [" << serviceInfo.servName - // << "] timed out\n"; - // } - // { - // std::lock_guard lock(this->serviceCountMutex); - // this->callService = false; - // } - // } - - - - - private: template - bool CallRequest(ServiceOutputInfo& serviceInfo, - std::string repTypeString, - const std::unique_ptr& req) - { - if (serviceInfo.repTypeMatchFound) - { - return false; - } - if (serviceInfo.repType == repTypeString) - { - serviceInfo.repTypeMatchFound = true; - - if (serviceInfo.reqType.empty()) - { - return HandleNoRequestMsg(serviceInfo); - } + { \ + executed = HandleNoReply(serviceInfo, *req); \ + isProcessing = true; \ + } \ + else if (serviceInfo.repType == typeString && !isProcessing) \ + { \ + executed = HandleReply(serviceInfo, *req); \ + isProcessing = true; \ + } \ - else if (serviceInfo.repType.empty()) - { - return HandleNoReply(serviceInfo, *req); - } - - else if (!serviceInfo.reqType.empty() && !serviceInfo.repType.empty()) - { - return HandleReply(serviceInfo, *req); - } - } - return false; - } - - // function that takees reqT and repT template and figures out which fucntions to call private: template - void Logic(ServiceOutputInfo& serviceInfo, std::string repTypeString) + void HandleRequest(ServiceOutputInfo& serviceInfo) { bool executed {false}; - std::unique_ptr req; - bool result = CreateReqMsg(serviceInfo, repTypeString, req); - if (!result) - return; + bool isProcessing {false}; - executed = CallRequest(serviceInfo, "ignition.msgs.StringMsg", req); - executed = CallRequest(serviceInfo, "ignition.msgs.Boolean", req); - executed = CallRequest(serviceInfo, "ignition.msgs.Empty", req); + std::unique_ptr req; + if (!serviceInfo.reqType.empty()) + { + req = msgs::Factory::New(serviceInfo.reqType, + serviceInfo.reqMsg); + if (!req) + { + ignerr << "Unable to create request for type [" + << serviceInfo.reqType << "].\n"; + return; + } + } + HANDLE_REPLY(msgs::StringMsg, "ignition.msgs.StringMsg"); + HANDLE_REPLY(msgs::Boolean, "ignition.msgs.Boolean"); + HANDLE_REPLY(msgs::Empty, "ignition.msgs.Empty"); + //NOTE: add more protobuf msgs for the Reply if (!executed) { @@ -485,38 +419,8 @@ namespace systems this->callService = false; } } - - - private: template - bool CreateReqMsg(ServiceOutputInfo& serviceInfo, - std::string reqTypeString, - std::unique_ptr& r) - { - if (serviceInfo.reqTypeMatchFound) - { - return false; - } - // std::unique_ptr req; - if (serviceInfo.reqType == reqTypeString) - {// if reqMsg missingjust create one without it - r = msgs::Factory::New(serviceInfo.reqType, - serviceInfo.reqMsg); - } - else if (serviceInfo.reqType.empty()) - { - return true; -//set serviceinfo requesttypeenum to e.g.2 - } - return false; - } - }; - - //method that just creates msgType unique ptr => will require a map - // mapping of string and unq ptr - - -}// +} } } } From 759c7fd23952bda95c345a02bb2003646fa908c8 Mon Sep 17 00:00:00 2001 From: Liam Han Date: Mon, 8 Aug 2022 00:39:32 +0000 Subject: [PATCH 16/26] modified to use blocking Request method as well as reduce a service worker thread to just one thread with the publisher. all tests passed Signed-off-by: Liam Han --- .../triggered_publisher/TriggeredPublisher.cc | 153 +++++---- .../triggered_publisher/TriggeredPublisher.hh | 27 +- test/integration/triggered_publisher.cc | 301 ++++-------------- test/worlds/triggered_publisher.sdf | 99 +----- 4 files changed, 160 insertions(+), 420 deletions(-) diff --git a/src/systems/triggered_publisher/TriggeredPublisher.cc b/src/systems/triggered_publisher/TriggeredPublisher.cc index 547564468c..7ea8ae356a 100644 --- a/src/systems/triggered_publisher/TriggeredPublisher.cc +++ b/src/systems/triggered_publisher/TriggeredPublisher.cc @@ -483,17 +483,11 @@ std::unique_ptr InputMatcher::Create( TriggeredPublisher::~TriggeredPublisher() { this->done = true; - this->srvDone = true; this->newMatchSignal.notify_one(); - this->serviceMatchSignal.notify_one(); if (this->workerThread.joinable()) { this->workerThread.join(); } - if (this->serviceWorkerThread.joinable()) - { - this->serviceWorkerThread.join(); - } } ////////////////////////////////////////////////// @@ -609,17 +603,13 @@ void TriggeredPublisher::Configure(const Entity &, } } } - else - { - ignerr << "No ouptut specified" << std::endl; - } if (sdfClone->HasElement("service")) { for (auto serviceElem = sdfClone->GetElement("service"); serviceElem; serviceElem = serviceElem->GetNextElement("service")) { - ServiceOutputInfo serviceInfo; + ServOutputInfo serviceInfo; serviceInfo.servName = serviceElem->Get("name"); if (serviceInfo.servName.empty()) { @@ -639,14 +629,24 @@ void TriggeredPublisher::Configure(const Entity &, serviceInfo.reqMsg = serviceElem->Get("reqMsg"); if (serviceInfo.reqMsg.empty()) { - ignwarn << "Service request string is empty\n"; + ignerr << "Service request string cannot be empty\n"; + return; } - this->serviceOutputInfo.push_back(std::move(serviceInfo)); + std::string timeoutInfo = serviceElem->Get("timeout"); + if (timeoutInfo.empty()) + { + ignerr << "Timeout value cannot be empty\n"; + return; + } + + serviceInfo.timeout = std::stoi(timeoutInfo); + this->servOutputInfo.push_back(std::move(serviceInfo)); } } - else + if (!sdfClone->HasElement("service") && !sdfClone->HasElement("output")) { - ignwarn << "No service specified" << std::endl; + ignerr << "No output and service specified." << std::endl; + return; } auto msgCb = std::function( @@ -667,14 +667,13 @@ void TriggeredPublisher::Configure(const Entity &, } this->newMatchSignal.notify_one(); } - // only perform when service tag is specified in sdf - if (this->serviceOutputInfo.size() > 0) + if (this->servOutputInfo.size() > 0) { { - std::lock_guard lock(this->serviceCountMutex); - this->callService = true; + std::lock_guard lock(this->triggerServMutex); + this->triggerServ = true; } - this->serviceMatchSignal.notify_one(); + this->newMatchSignal.notify_one(); } } }); @@ -699,58 +698,72 @@ void TriggeredPublisher::Configure(const Entity &, this->workerThread = std::thread(std::bind(&TriggeredPublisher::DoWork, this)); - this->serviceWorkerThread = - std::thread(std::bind(&TriggeredPublisher::DoServiceWork, this)); } -////////////////////////////////////////////////// - -#define HANDLE_REQUEST(requestT, typeString) \ -if (((serviceInfo.reqType == typeString) | serviceInfo.reqType.empty()) && !isProcessing) \ -{ \ - this->HandleRequest(serviceInfo); \ - isProcessing = true; \ -} \ -void TriggeredPublisher::DoServiceWork() +void TriggeredPublisher::PublishMsg(std::size_t pending) { - while (!this->srvDone) - { + for (auto &info : this->outputInfo) { - using namespace std::chrono_literals; - std::unique_lock lock(this->serviceCountMutex); - this->serviceMatchSignal.wait(lock, - [this] - { - return (this->callService) || this->srvDone; - }); - - if (this->srvDone) + for (std::size_t i = 0; i < pending; ++i) { - continue; + info.pub.Publish(*info.msgData); } } - for (auto &serviceInfo : this->serviceOutputInfo) + +} + +void TriggeredPublisher::CallService() +{ + for (auto &serviceInfo : this->servOutputInfo) { - // bool isProcessing {false}; - // HANDLE_REQUEST(msgs::Pose, "ignition.msgs.Pose"); - // HANDLE_REQUEST(msgs::StringMsg, "ignition.msgs.StringMsg"); - // HANDLE_REQUEST(msgs::Boolean, "ignition.msgs.Boolean"); - // HANDLE_REQUEST(msgs::Empty, "ignition.msgs.Empty"); - //NOTE: add more protobuf msgs for the Request - - //CheckServRepType(msgs::Pose, "ignition.msgs.Pose"); - //CheckServRepType(msgs::StringMsg, "ignition.msgs.StringMsg"); - //CheckServRepType(msgs::Boolean, "ignition.msgs.Boolean"); - //CheckServRepType(msgs::Empty, "ignition.msgs.Empty"); - - Logic(serviceInfo, "ignition.msgs.Pose"); - Logic(serviceInfo, "ignition.msgs.StringMsg"); - Logic(serviceInfo, "ignition.msgs.Boolean"); - Logic(serviceInfo, "ignition.msgs.Empty"); + bool result; + auto req = msgs::Factory::New(serviceInfo.reqType, + serviceInfo.reqMsg); + if (!req) + { + ignerr << "Unable to create request for type [" + << serviceInfo.reqType << "].\n"; + return; + } + + auto rep = msgs::Factory::New(serviceInfo.repType); + if (!rep) + { + ignerr << "Unable to create response for type [" + << serviceInfo.repType << "].\n"; + return; + } + + bool executed = this->node.Request(serviceInfo.servName, + *req, + serviceInfo.timeout, + *rep, + result); + if (executed) + { + if (!result) + { + ignerr << "Service call [" << serviceInfo.servName + << "] failed\n"; + } + else + { + ignmsg << "Service call [" << serviceInfo.servName + << "] succeeded\n"; + } + } + else + { + ignerr << "Service call [" << serviceInfo.servName + << "] timed out\n"; + } + { + std::lock_guard lock(this->triggerServMutex); + this->triggerServ = false; + } } - } -} +} ////////////////////////////////////////////////// void TriggeredPublisher::DoWork() { @@ -763,7 +776,7 @@ void TriggeredPublisher::DoWork() this->newMatchSignal.wait_for(lock, 1s, [this] { - return (this->publishCount > 0) || this->done; + return (this->publishCount > 0) || this-> triggerServ || this->done; }); if (this->publishCount == 0 || this->done) @@ -771,14 +784,14 @@ void TriggeredPublisher::DoWork() continue; } - std::swap(pending, this->publishCount); - } - - for (auto &info : this->outputInfo) - { - for (std::size_t i = 0; i < pending; ++i) + if (this->publishCount >0) { - info.pub.Publish(*info.msgData); + std::swap(pending, this->publishCount); + PublishMsg(pending); + } + if (this->triggerServ) + { + CallService(); } } } diff --git a/src/systems/triggered_publisher/TriggeredPublisher.hh b/src/systems/triggered_publisher/TriggeredPublisher.hh index d61bab4b92..600d33e187 100644 --- a/src/systems/triggered_publisher/TriggeredPublisher.hh +++ b/src/systems/triggered_publisher/TriggeredPublisher.hh @@ -192,8 +192,11 @@ namespace systems /// \brief Thread that handles publishing output messages public: void DoWork(); - /// \brief Thread that handles calling services - public: void DoServiceWork(); + /// \brief Method that calls a service + private: void CallService(); + + /// \brief Method that publishes a message + private: void PublishMsg(std::size_t pending); /// \brief Helper function that calls Match on every InputMatcher available /// \param[in] _inputMsg Input message @@ -224,7 +227,7 @@ namespace systems }; /// \brief Class that holds necessary bits for each specified service output - private: struct ServiceOutputInfo + private: struct ServOutputInfo { /// \brief Service name std::string servName; @@ -250,7 +253,7 @@ namespace systems private: std::vector outputInfo; /// \brief List of service outputs - private: std::vector serviceOutputInfo; + private: std::vector servOutputInfo; /// \brief Ignition communication node. private: transport::Node node; @@ -258,35 +261,25 @@ namespace systems /// \brief Counter that tells the publisher how many times to publish private: std::size_t publishCount{0}; - /// \brief Counter that tells how many times to call the service - //private: std::size_t serviceCount{0}; - private: bool callService{false}; + /// \brief Flag to trigger calling a service in the DoWork thread + private: bool triggerServ{false}; /// \brief Mutex to synchronize access to publishCount private: std::mutex publishCountMutex; /// \brief Mutex to synchronize access to serviceCount - private: std::mutex serviceCountMutex; + private: std::mutex triggerServMutex; /// \brief Condition variable to signal that new matches have occured private: std::condition_variable newMatchSignal; - /// \brief Condition variable to signal to call a service - private: std::condition_variable serviceMatchSignal; - /// \brief Thread handle for worker thread private: std::thread workerThread; - /// \brief Thread handle for service worker thread - private: std::thread serviceWorkerThread; - /// \brief Flag for when the system is done and the worker thread should /// stop private: std::atomic done{false}; - /// \brief Flag used for the service worker thread - private: std::atomic srvDone{false}; - /// \brief Publish delay time. This is in simulation time. private: std::chrono::steady_clock::duration delay{0}; diff --git a/test/integration/triggered_publisher.cc b/test/integration/triggered_publisher.cc index 134128dcca..347e015ec8 100644 --- a/test/integration/triggered_publisher.cc +++ b/test/integration/triggered_publisher.cc @@ -641,6 +641,9 @@ TEST_F(TriggeredPublisherTest, ///////////////////////////////////////////////// // Test for invalid service name. It'll timeout // when matching service name can't be found. +// --- +// Request Type: msgs::StringMsg +// Reply Type: msgs::StringMsg ///////////////////////////////////////////////// TEST_F(TriggeredPublisherTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(InvalidServiceName)) @@ -683,6 +686,9 @@ TEST_F(TriggeredPublisherTest, // to input ign msg by publishing 10 times. Service // call will also occur 10 times. It'll compare // pubCount and recvCount. +// --- +// Request Type: msgs::StringMsg +// Reply Type: msgs::StringMsg ///////////////////////////////////////////////// TEST_F(TriggeredPublisherTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(InputMessagesTriggerServices)) @@ -726,6 +732,9 @@ TEST_F(TriggeredPublisherTest, // call will also occur 10 times for each service. // It'll compare pubCount and recvCount for each // services. +// --- +// Request Type: msgs::Boolean +// Reply Type: msgs::Boolean ///////////////////////////////////////////////// TEST_F(TriggeredPublisherTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(MultipleServiceForOneInput)) @@ -782,8 +791,10 @@ TEST_F(TriggeredPublisherTest, ///////////////////////////////////////////////// // Test for triggering a service call with incorrect -// request type or reseponse type. The server callback -// will not be triggered hence the recvCount will be 0 +// request type or reseponse type specified in sdf. +// --- +// Request Type: msgs::StringMsg +// Reply Type: msgs::StringMsg ///////////////////////////////////////////////// TEST_F(TriggeredPublisherTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(WrongRequestOrResponseType)) @@ -823,8 +834,10 @@ TEST_F(TriggeredPublisherTest, ///////////////////////////////////////////////// // Test for triggering a service call that'll return -// False result. The server callback will be triggered -// but it'll +1 to the recvCout but return False. +// False result. +// --- +// Request Type: msgs::StringMsg +// Reply Type: msgs::StringMsg ///////////////////////////////////////////////// TEST_F(TriggeredPublisherTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(FailedReesultServiceCall)) @@ -863,191 +876,37 @@ TEST_F(TriggeredPublisherTest, EXPECT_EQ(pubCount, recvCount); } - - - - - - - -///////////////////////////////////////////////// -// Test for triggering a service call with same -// Request and Reply type -// --- -// Request Type: msgs::StringMsg -// Reply Type: msgs::StringMsg -///////////////////////////////////////////////// -TEST_F(TriggeredPublisherTest, - IGN_UTILS_TEST_DISABLED_ON_WIN32(CallingSameRequestReplyTypeServ)) -{ - transport::Node node; - auto inputPub = node.Advertise("/in_20"); - std::atomic recvCount{0}; - - auto srvCb = std::function([&recvCount](const auto &req, auto &rep) - { - EXPECT_EQ(req.data(), "srv-0"); - if (req.data() == "srv-0") - { - ++recvCount; - rep.set_data(req.data()); - return true; - } - return false; - }); - node.Advertise("srv-0", srvCb); - - const std::size_t pubCount{10}; - for (std::size_t i = 0; i < pubCount; ++i) - { - EXPECT_TRUE(inputPub.Publish(msgs::Empty())); - IGN_SLEEP_MS(100); - } - - waitUntil(5000, [&]{return pubCount == recvCount;}); - EXPECT_EQ(pubCount, recvCount); -} - - ///////////////////////////////////////////////// // Test for triggering a service call with different -// Request and Reply type +// ignition msg type // --- -// Request Type: msgs::StringMsg -// Reply Type: msgs::Boolean -///////////////////////////////////////////////// -TEST_F(TriggeredPublisherTest, - IGN_UTILS_TEST_DISABLED_ON_WIN32(CallingDifferntRequestReplyTypeServ1)) -{ - transport::Node node; - auto inputPub = node.Advertise("/in_21"); - std::atomic recvCount{0}; - - auto srvCb = std::function([&recvCount](const auto &req, auto &rep) - { - EXPECT_EQ(req.data(), "srv-1"); - if (req.data() == "srv-1") - { - ++recvCount; - rep.set_data(true); - return true; - } - return false; - }); - node.Advertise("srv-1", srvCb); - - const std::size_t pubCount{10}; - for (std::size_t i = 0; i < pubCount; ++i) - { - EXPECT_TRUE(inputPub.Publish(msgs::Empty())); - IGN_SLEEP_MS(100); - } - - waitUntil(5000, [&]{return pubCount == recvCount;}); - EXPECT_EQ(pubCount, recvCount); -} - -///////////////////////////////////////////////// -// Test for triggering a service call with different -// Request and Reply type -// --- -// Request Type: msgs::Boolean +// Request Type: msgs::Empty // Reply Type: msgs::StringMsg ///////////////////////////////////////////////// TEST_F(TriggeredPublisherTest, - IGN_UTILS_TEST_DISABLED_ON_WIN32(CallingDifferntRequestReplyTypeServ2)) + IGN_UTILS_TEST_DISABLED_ON_WIN32(EmptyReqStringMsgRep)) { transport::Node node; - auto inputPub = node.Advertise("/in_22"); + auto inputPub = node.Advertise("/in_18"); std::atomic recvCount{0}; - auto srvCb = std::function([&recvCount](const auto &req, auto &rep) - { - EXPECT_EQ(req.data(), true); - if (req.data() == true) - { - ++recvCount; - rep.set_data("srv-2"); - return true; - } - return false; - }); - node.Advertise("srv-2", srvCb); - - const std::size_t pubCount{10}; - for (std::size_t i = 0; i < pubCount; ++i) - { - EXPECT_TRUE(inputPub.Publish(msgs::Empty())); - IGN_SLEEP_MS(100); - } - - waitUntil(5000, [&]{return pubCount == recvCount;}); - EXPECT_EQ(pubCount, recvCount); -} - -///////////////////////////////////////////////// -// Test for triggering a service call with no -// Reply type specified -// --- -// Request Type: msgs::StringMsg -// Reply Type: null -///////////////////////////////////////////////// -TEST_F(TriggeredPublisherTest, - IGN_UTILS_TEST_DISABLED_ON_WIN32(CallingNoReplyTypeService)) -{ - transport::Node node; - auto inputPub = node.Advertise("/in_23"); - std::atomic recvCount{0}; - - auto srvCb = std::function - ([&recvCount](const auto &req) - { - EXPECT_EQ(req.data(), "srv-3"); - if (req.data() == "srv-3") - { - ++recvCount; - } - }); - node.Advertise("srv-3", srvCb); - - const std::size_t pubCount{10}; - for (std::size_t i = 0; i < pubCount; ++i) - { - EXPECT_TRUE(inputPub.Publish(msgs::Empty())); - IGN_SLEEP_MS(100); - } - - waitUntil(5000, [&]{return pubCount == recvCount;}); - EXPECT_EQ(pubCount, recvCount); -} - - - -///////////////////////////////////////////////// -// Test for triggering a service call with no -// Request type -// --- -// Request Type: null -// Reply Type: msgs::StringMsg -///////////////////////////////////////////////// -TEST_F(TriggeredPublisherTest, - IGN_UTILS_TEST_DISABLED_ON_WIN32(CallingNoRequestTypeServ)) -{ - transport::Node node; - auto inputPub = node.Advertise("/in_24"); - std::atomic recvCount{0}; + auto srvEchoCb = std::function( + [&recvCount](const auto &req, auto &rep) + { + EXPECT_EQ(req.unused(), true); + if (req.unused() == true) + { + ++recvCount; + rep.set_data("srv-empty"); + return true; + } + return false; + }); - auto srvCb = std::function - ([&recvCount](auto &rep) - { - ++recvCount; - rep.set_data("srv-4"); - return true; - }); - node.Advertise("srv-4", srvCb); + // Advertise a dummy service + std::string service = "/srv-empty-req-test"; + node.Advertise(service, srvEchoCb); const std::size_t pubCount{10}; for (std::size_t i = 0; i < pubCount; ++i) @@ -1062,77 +921,35 @@ TEST_F(TriggeredPublisherTest, ///////////////////////////////////////////////// // Test for triggering a service call with different -// Request and Reply type +// ignition msg type // --- -// Request Type: Empty -// Reply Type: Empty +// Request Type: msgs::StringMsg +// Reply Type: msgs::Empty ///////////////////////////////////////////////// TEST_F(TriggeredPublisherTest, - IGN_UTILS_TEST_DISABLED_ON_WIN32(CallingEmptyRequestTypeServ)) + IGN_UTILS_TEST_DISABLED_ON_WIN32(StringMsgReqEmptyRep)) { transport::Node node; - auto inputPub = node.Advertise("/in_25"); + auto inputPub = node.Advertise("/in_19"); std::atomic recvCount{0}; - auto srvCb = std::function - ([&recvCount](const auto &req, auto &rep) - { - EXPECT_EQ(req.unused(), true); - if (req.unused() == true) - { - ++recvCount; - rep.set_unused(false); - return true; - } - return false; - }); - node.Advertise("srv-5", srvCb); - - const std::size_t pubCount{10}; - for (std::size_t i = 0; i < pubCount; ++i) - { - EXPECT_TRUE(inputPub.Publish(msgs::Empty())); - IGN_SLEEP_MS(100); - } - - waitUntil(5000, [&]{return pubCount == recvCount;}); - EXPECT_EQ(pubCount, recvCount); -} - -///////////////////////////////////////////////// -// Test for triggering a service call for Pose -// Request and Boolean Reply type -// --- -// Request Type: msgs::Pose -// Reply Type: msgs::StringMsg -///////////////////////////////////////////////// -TEST_F(TriggeredPublisherTest, - IGN_UTILS_TEST_DISABLED_ON_WIN32(CallingPoseReqTypeServ)) -{ - transport::Node node; - auto inputPub = node.Advertise("/in_26"); - std::atomic recvCount{0}; + auto srvEchoCb = std::function( + [&recvCount](const auto &req, auto &rep) + { + EXPECT_EQ(req.data(), "test"); + if (req.data() == "test") + { + ++recvCount; + rep.set_unused(true); + return true; + } + return false; + }); - auto srvCb = std::function([&recvCount](const auto &req, auto &rep) - { - EXPECT_EQ(req.name(), "blue_vehicle"); - EXPECT_FLOAT_EQ(req.id(), 8); - EXPECT_FLOAT_EQ(req.position().x(), 3); - EXPECT_FLOAT_EQ(req.position().z(), 0.325); - float tol = 0.001; - if (req.name() == "blue_vehicle" && - fabs(req.id() - 8) < tol && - fabs(req.position().x() - 3) < tol && - fabs(req.position().z() - 0.325) - - - - - - - - - - - - - - - - - - - - - - + timeout="100" + reqMsg="unused: true"> - + - - - - - - - - - - - - - - - - - - + timeout="100" + reqMsg="data: 'test'"> - - From 8295ebb67e745572c0350467fd05291a8072f869 Mon Sep 17 00:00:00 2001 From: Liam Han Date: Thu, 4 Aug 2022 10:20:18 +0000 Subject: [PATCH 17/26] stable version: had to revert back to previous work. all tests passed Signed-off-by: Liam Han --- .../triggered_publisher/TriggeredPublisher.cc | 2 +- .../triggered_publisher/TriggeredPublisher.hh | 164 ++++-------------- 2 files changed, 35 insertions(+), 131 deletions(-) diff --git a/src/systems/triggered_publisher/TriggeredPublisher.cc b/src/systems/triggered_publisher/TriggeredPublisher.cc index 7ea8ae356a..52d62ed8ae 100644 --- a/src/systems/triggered_publisher/TriggeredPublisher.cc +++ b/src/systems/triggered_publisher/TriggeredPublisher.cc @@ -762,7 +762,7 @@ void TriggeredPublisher::CallService() this->triggerServ = false; } } - + } } ////////////////////////////////////////////////// void TriggeredPublisher::DoWork() diff --git a/src/systems/triggered_publisher/TriggeredPublisher.hh b/src/systems/triggered_publisher/TriggeredPublisher.hh index 600d33e187..17e8eb9a48 100644 --- a/src/systems/triggered_publisher/TriggeredPublisher.hh +++ b/src/systems/triggered_publisher/TriggeredPublisher.hh @@ -240,10 +240,6 @@ namespace systems /// \brief Service request message std::string reqMsg; - - - bool reqTypeMatchFound{false}; - bool repTypeMatchFound{false}; }; /// \brief List of InputMatchers @@ -354,9 +350,6 @@ namespace systems private: template bool HandleNoRequestMsg(ServiceOutputInfo& serviceInfo) { - // Non-blocking request with no request type - //ignerr<<"here\n"; - auto reqCb = ReplyCallback(serviceInfo); return this->node.Request(serviceInfo.servName, reqCb); } @@ -369,104 +362,45 @@ namespace systems } #define HANDLE_REPLY(repT, typeString) \ - if (((serviceInfo.repType == typeString) && \ - (serviceInfo.reqType.empty())) && !isProcessing) \ - { \ - executed = HandleNoRequestMsg(serviceInfo); \ - isProcessing = true; \ - } \ + if (serviceInfo.repType == typeString && \ + serviceInfo.reqType.empty() && !isProcessing) \ + { \ + executed = HandleNoRequestMsg(serviceInfo); \ + isProcessing = true; \ + } \ else if (serviceInfo.repType.empty() && !isProcessing) \ - { \ - executed = HandleNoReply(serviceInfo, *req); \ - isProcessing = true; \ - } \ - else if((serviceInfo.repType == typeString) && !isProcessing)\ - { \ - executed = HandleReply(serviceInfo, *req); \ - isProcessing = true; \ - } \ - - // private: template - // void HandleRequest(ServiceOutputInfo& serviceInfo) - // { - // bool executed {false}; - // bool isProcessing {false}; - // // msgs::Factory::New req; - // std::unique_ptr req; - // if (!serviceInfo.reqType.empty()) - // { - // req = msgs::Factory::New(serviceInfo.reqType, - // serviceInfo.reqMsg); - // if (!req) - // { - // ignerr << "Unable to create request for type [" - // << serviceInfo.reqType << "].\n"; - // return; - // } - // } - // HANDLE_REPLY(msgs::StringMsg, "ignition.msgs.StringMsg"); - // HANDLE_REPLY(msgs::Boolean, "ignition.msgs.Boolean"); - // HANDLE_REPLY(msgs::Empty, "ignition.msgs.Empty"); - // //NOTE: add more protobuf msgs for the Reply - - // if (!executed) - // { - // ignerr << "Service call [" << serviceInfo.servName - // << "] timed out\n"; - // } - // { - // std::lock_guard lock(this->serviceCountMutex); - // this->callService = false; - // } - // } - - - - - private: template - bool CallRequest(ServiceOutputInfo& serviceInfo, - std::string repTypeString, - const std::unique_ptr& req) - { - if (serviceInfo.repTypeMatchFound) - { - return false; - } - if (serviceInfo.repType == repTypeString) - { - serviceInfo.repTypeMatchFound = true; - - if (serviceInfo.reqType.empty()) - { - return HandleNoRequestMsg(serviceInfo); - } + { \ + executed = HandleNoReply(serviceInfo, *req); \ + isProcessing = true; \ + } \ + else if (serviceInfo.repType == typeString && !isProcessing) \ + { \ + executed = HandleReply(serviceInfo, *req); \ + isProcessing = true; \ + } \ - else if (serviceInfo.repType.empty()) - { - return HandleNoReply(serviceInfo, *req); - } - - else if (!serviceInfo.reqType.empty() && !serviceInfo.repType.empty()) - { - return HandleReply(serviceInfo, *req); - } - } - return false; - } - - // function that takees reqT and repT template and figures out which fucntions to call private: template - void Logic(ServiceOutputInfo& serviceInfo, std::string repTypeString) + void HandleRequest(ServiceOutputInfo& serviceInfo) { bool executed {false}; - std::unique_ptr req; - bool result = CreateReqMsg(serviceInfo, repTypeString, req); - if (!result) - return; + bool isProcessing {false}; - executed = CallRequest(serviceInfo, "ignition.msgs.StringMsg", req); - executed = CallRequest(serviceInfo, "ignition.msgs.Boolean", req); - executed = CallRequest(serviceInfo, "ignition.msgs.Empty", req); + std::unique_ptr req; + if (!serviceInfo.reqType.empty()) + { + req = msgs::Factory::New(serviceInfo.reqType, + serviceInfo.reqMsg); + if (!req) + { + ignerr << "Unable to create request for type [" + << serviceInfo.reqType << "].\n"; + return; + } + } + HANDLE_REPLY(msgs::StringMsg, "ignition.msgs.StringMsg"); + HANDLE_REPLY(msgs::Boolean, "ignition.msgs.Boolean"); + HANDLE_REPLY(msgs::Empty, "ignition.msgs.Empty"); + //NOTE: add more protobuf msgs for the Reply if (!executed) { @@ -478,38 +412,8 @@ namespace systems this->callService = false; } } - - - private: template - bool CreateReqMsg(ServiceOutputInfo& serviceInfo, - std::string reqTypeString, - std::unique_ptr& r) - { - if (serviceInfo.reqTypeMatchFound) - { - return false; - } - // std::unique_ptr req; - if (serviceInfo.reqType == reqTypeString) - {// if reqMsg missingjust create one without it - r = msgs::Factory::New(serviceInfo.reqType, - serviceInfo.reqMsg); - } - else if (serviceInfo.reqType.empty()) - { - return true; -//set serviceinfo requesttypeenum to e.g.2 - } - return false; - } - }; - - //method that just creates msgType unique ptr => will require a map - // mapping of string and unq ptr - - -}// +} } } } From 10d946f26086efd42287e655c37aaff5a05bfc74 Mon Sep 17 00:00:00 2001 From: Liam Han Date: Mon, 8 Aug 2022 06:20:20 +0000 Subject: [PATCH 18/26] successfully reverted and tested Signed-off-by: Liam Han --- .../triggered_publisher/TriggeredPublisher.cc | 111 ++++++-------- .../triggered_publisher/TriggeredPublisher.hh | 138 +----------------- test/worlds/triggered_publisher.sdf | 6 + 3 files changed, 60 insertions(+), 195 deletions(-) diff --git a/src/systems/triggered_publisher/TriggeredPublisher.cc b/src/systems/triggered_publisher/TriggeredPublisher.cc index 4b0644a655..ddb0b8e735 100644 --- a/src/systems/triggered_publisher/TriggeredPublisher.cc +++ b/src/systems/triggered_publisher/TriggeredPublisher.cc @@ -619,12 +619,14 @@ void TriggeredPublisher::Configure(const Entity &, serviceInfo.reqType = serviceElem->Get("reqType"); if (serviceInfo.reqType.empty()) { - ignwarn << "Service request type is empty\n"; + ignerr << "Service request type cannot be empty\n"; + return; } serviceInfo.repType = serviceElem->Get("repType"); if (serviceInfo.repType.empty()) { - ignwarn << "Service reply type is empty\n"; + ignerr << "Service reply type cannot be empty\n"; + return; } serviceInfo.reqMsg = serviceElem->Get("reqMsg"); if (serviceInfo.reqMsg.empty()) @@ -645,7 +647,8 @@ void TriggeredPublisher::Configure(const Entity &, } if (!sdfClone->HasElement("service") && !sdfClone->HasElement("output")) { - ignerr << "No output and service specified." << std::endl; + ignerr << "No output and service specified. Make sure to specify at least" + "one of them." << std::endl; return; } @@ -699,19 +702,8 @@ void TriggeredPublisher::Configure(const Entity &, this->workerThread = std::thread(std::bind(&TriggeredPublisher::DoWork, this)); } -<<<<<<< HEAD -======= -////////////////////////////////////////////////// - -#define HANDLE_REQUEST(requestT, typeString) \ -if (((serviceInfo.reqType == typeString) | serviceInfo.reqType.empty()) \ - && !isProcessing) \ -{ \ - this->HandleRequest(serviceInfo); \ - isProcessing = true; \ -} \ ->>>>>>> aa5090b1ca58d9ab86b0c2e5e4d90d8ae7c0981b +////////////////////////////////////////////////// void TriggeredPublisher::PublishMsg(std::size_t pending) { for (auto &info : this->outputInfo) @@ -721,70 +713,61 @@ void TriggeredPublisher::PublishMsg(std::size_t pending) info.pub.Publish(*info.msgData); } } - } +////////////////////////////////////////////////// void TriggeredPublisher::CallService() { - for (auto &serviceInfo : this->servOutputInfo) + for (auto &serviceInfo : this->servOutputInfo) + { + bool result; + auto req = msgs::Factory::New(serviceInfo.reqType, + serviceInfo.reqMsg); + if (!req) { -<<<<<<< HEAD - bool result; - auto req = msgs::Factory::New(serviceInfo.reqType, - serviceInfo.reqMsg); - if (!req) - { - ignerr << "Unable to create request for type [" - << serviceInfo.reqType << "].\n"; - return; - } + ignerr << "Unable to create request for type [" + << serviceInfo.reqType << "].\n"; + return; + } - auto rep = msgs::Factory::New(serviceInfo.repType); - if (!rep) - { - ignerr << "Unable to create response for type [" - << serviceInfo.repType << "].\n"; - return; - } + auto rep = msgs::Factory::New(serviceInfo.repType); + if (!rep) + { + ignerr << "Unable to create response for type [" + << serviceInfo.repType << "].\n"; + return; + } - bool executed = this->node.Request(serviceInfo.servName, - *req, - serviceInfo.timeout, - *rep, - result); - if (executed) - { - if (!result) - { - ignerr << "Service call [" << serviceInfo.servName - << "] failed\n"; - } - else - { - ignmsg << "Service call [" << serviceInfo.servName - << "] succeeded\n"; - } - } - else + bool executed = this->node.Request(serviceInfo.servName, + *req, + serviceInfo.timeout, + *rep, + result); + if (executed) + { + if (!result) { ignerr << "Service call [" << serviceInfo.servName - << "] timed out\n"; + << "] failed\n"; } + else { - std::lock_guard lock(this->triggerServMutex); - this->triggerServ = false; + ignmsg << "Service call [" << serviceInfo.servName + << "] succeeded\n"; } -======= - bool isProcessing {false}; - HANDLE_REQUEST(msgs::Pose, "ignition.msgs.Pose"); - HANDLE_REQUEST(msgs::StringMsg, "ignition.msgs.StringMsg"); - HANDLE_REQUEST(msgs::Boolean, "ignition.msgs.Boolean"); - HANDLE_REQUEST(msgs::Empty, "ignition.msgs.Empty"); - //NOTE: add more protobuf msgs for the Request ->>>>>>> aa5090b1ca58d9ab86b0c2e5e4d90d8ae7c0981b + } + else + { + ignerr << "Service call [" << serviceInfo.servName + << "] timed out\n"; + } + { + std::lock_guard lock(this->triggerServMutex); + this->triggerServ = false; } } } + ////////////////////////////////////////////////// void TriggeredPublisher::DoWork() { diff --git a/src/systems/triggered_publisher/TriggeredPublisher.hh b/src/systems/triggered_publisher/TriggeredPublisher.hh index 17e8eb9a48..122a9ba3b4 100644 --- a/src/systems/triggered_publisher/TriggeredPublisher.hh +++ b/src/systems/triggered_publisher/TriggeredPublisher.hh @@ -38,8 +38,8 @@ namespace systems /// \brief The triggered publisher system publishes a user specified message /// on an output topic in response to an input message that matches user /// specified criteria. It can also call a user specified service as an - /// output in response to an input message. An optional simulation time delay - /// can be used delay message + /// output in response to an input message. It currently supports blocking + /// service call. An optional simulation time delay can be used delay message /// publication. /// /// ## System Parameters @@ -85,7 +85,9 @@ namespace systems /// that matches. /// * Attributes: /// * `name`: Service name (eg. `/world/triggered_publisher/set_pose`) + /// * `timeout`: Service timeout /// * `reqType`: Service request message type (eg. ignition.msgs.Pose) + /// * `repType`: Service response message type (eg. ignition.msgs.Empty) /// * `reqMsg`: String used to construct the service protobuf message. /// /// Examples: @@ -240,6 +242,9 @@ namespace systems /// \brief Service request message std::string reqMsg; + + /// \brief Serivce timeout + int timeout; }; /// \brief List of InputMatchers @@ -284,138 +289,9 @@ namespace systems /// \brief Mutex to synchronize access to publishQueue private: std::mutex publishQueueMutex; - - private: class ReplyHandler - { - public: virtual void operator() (const msgs::StringMsg &msg, - ServiceOutputInfo &srvInfo) - { - ignmsg << "Service call [" << srvInfo.servName - << "] succeeded. Response message [" << std::boolalpha - << msg.data() << "]\n"; - } - public: virtual void operator() (const msgs::Boolean &msg, - ServiceOutputInfo &srvInfo) - { - ignmsg << "Service call [" << srvInfo.servName - << "] succeeded. Response message [" << std::boolalpha - << msg.data() << "]\n"; - } - public: virtual void operator() (const msgs::Empty &msg, - ServiceOutputInfo &srvInfo) - { - ignmsg << "Service call [" << srvInfo.servName - << "] succeeded. Response message [" << msg.unused() - << "]\n"; - } - public: virtual void operator() (const msgs::Pose &msg, - ServiceOutputInfo &srvInfo) - { - ignmsg << "Service call [" << srvInfo.servName - << "] succeeded. Response message [" - << "name: " << msg.name() << "pos: {x: " - << msg.position().x() << ", y: " - << msg.position().y() << ", z: " - << msg.position().z() << "}]\n"; - } - //NOTE: add more replys for different types - }; - - private: template - std::function ReplyCallback( - ServiceOutputInfo& serviceInfo) - { - return std::function( - [&serviceInfo] (const ReplyT &msg, const bool res){ - if (res) - { - ReplyHandler()(msg, serviceInfo); - } - else - { - ignerr << "Service call [" << serviceInfo.servName - << "] failed\n"; - } - }); - } - - private: template - bool HandleReply(ServiceOutputInfo& serviceInfo, RequestT& req) - { - // Non-blocking request with request type and callback - auto reqCb = ReplyCallback(serviceInfo); - return this->node.Request(serviceInfo.servName, req, reqCb); - } - - private: template - bool HandleNoRequestMsg(ServiceOutputInfo& serviceInfo) - { - auto reqCb = ReplyCallback(serviceInfo); - return this->node.Request(serviceInfo.servName, reqCb); - } - - private: template - bool HandleNoReply(ServiceOutputInfo& serviceInfo, RequestT& req) - { - // Non-blocking request with no callback - return this->node.Request(serviceInfo.servName, req); - } - - #define HANDLE_REPLY(repT, typeString) \ - if (serviceInfo.repType == typeString && \ - serviceInfo.reqType.empty() && !isProcessing) \ - { \ - executed = HandleNoRequestMsg(serviceInfo); \ - isProcessing = true; \ - } \ - else if (serviceInfo.repType.empty() && !isProcessing) \ - { \ - executed = HandleNoReply(serviceInfo, *req); \ - isProcessing = true; \ - } \ - else if (serviceInfo.repType == typeString && !isProcessing) \ - { \ - executed = HandleReply(serviceInfo, *req); \ - isProcessing = true; \ - } \ - - private: template - void HandleRequest(ServiceOutputInfo& serviceInfo) - { - bool executed {false}; - bool isProcessing {false}; - - std::unique_ptr req; - if (!serviceInfo.reqType.empty()) - { - req = msgs::Factory::New(serviceInfo.reqType, - serviceInfo.reqMsg); - if (!req) - { - ignerr << "Unable to create request for type [" - << serviceInfo.reqType << "].\n"; - return; - } - } - HANDLE_REPLY(msgs::StringMsg, "ignition.msgs.StringMsg"); - HANDLE_REPLY(msgs::Boolean, "ignition.msgs.Boolean"); - HANDLE_REPLY(msgs::Empty, "ignition.msgs.Empty"); - //NOTE: add more protobuf msgs for the Reply - - if (!executed) - { - ignerr << "Service call [" << serviceInfo.servName - << "] timed out\n"; - } - { - std::lock_guard lock(this->serviceCountMutex); - this->callService = false; - } - } }; } } } } - #endif diff --git a/test/worlds/triggered_publisher.sdf b/test/worlds/triggered_publisher.sdf index d97dfb0b6a..5d21cbf855 100644 --- a/test/worlds/triggered_publisher.sdf +++ b/test/worlds/triggered_publisher.sdf @@ -212,6 +212,7 @@ name="/srv-test" reqType="ignition.msgs.StringMsg" repType="ignition.msgs.StringMsg" + timeout="100" reqMsg="data: 'test'"> @@ -224,12 +225,14 @@ name="/srv-test-0" reqType="ignition.msgs.Boolean" repType="ignition.msgs.Boolean" + timeout="100" reqMsg="data: true"> @@ -242,6 +245,7 @@ name="/srv-test" reqType="ignition.msgs.InvalidReqType" repType="ignition.msgs.StringMsg" + timeout="100" reqMsg="data: 'test'"> @@ -254,6 +258,7 @@ name="/srv-test" reqType="ignition.msgs.StringMsg" repType="ignition.msgs.InvalidRepType" + timeout="100" reqMsg="data: 'test'"> @@ -266,6 +271,7 @@ name="/srv-test" reqType="ignition.msgs.StringMsg" repType="ignition.msgs.StringMsg" + timeout="100" reqMsg="data: 'test'"> From a811126a032635d62ae29085f136f2518d5ef422 Mon Sep 17 00:00:00 2001 From: Liam Han Date: Sat, 13 Aug 2022 16:15:28 +0000 Subject: [PATCH 19/26] fixing PR suggestions Signed-off-by: Liam Han --- examples/worlds/triggered_publisher.sdf | 2 +- .../triggered_publisher/TriggeredPublisher.cc | 54 +++--- .../triggered_publisher/TriggeredPublisher.hh | 2 +- test/integration/triggered_publisher.cc | 173 +++++++----------- test/worlds/triggered_publisher.sdf | 25 +-- tutorials/triggered_publisher.md | 2 +- 6 files changed, 95 insertions(+), 163 deletions(-) diff --git a/examples/worlds/triggered_publisher.sdf b/examples/worlds/triggered_publisher.sdf index 42718d613a..a990a7a182 100644 --- a/examples/worlds/triggered_publisher.sdf +++ b/examples/worlds/triggered_publisher.sdf @@ -472,7 +472,7 @@ start falling. reqType="ignition.msgs.Pose" repType="ignition.msgs.Boolean" timeout="3000" - reqMsg="name: 'blue_vehicle', id:8, position: {x:-3, z:0.325}"> + reqMsg="name: 'blue_vehicle', id: 8, position: {x: -3, z: 0.325}"> GetNextElement("service")) { ServOutputInfo serviceInfo; - serviceInfo.servName = serviceElem->Get("name"); - if (serviceInfo.servName.empty()) + serviceInfo.srvName = serviceElem->Get("name"); + if (serviceInfo.srvName.empty()) { ignerr << "Service name cannot be empty\n"; return; @@ -631,7 +631,7 @@ void TriggeredPublisher::Configure(const Entity &, serviceInfo.reqMsg = serviceElem->Get("reqMsg"); if (serviceInfo.reqMsg.empty()) { - ignerr << "Service request string cannot be empty\n"; + ignerr << "Service request message cannot be empty\n"; return; } std::string timeoutInfo = serviceElem->Get("timeout"); @@ -706,13 +706,13 @@ void TriggeredPublisher::Configure(const Entity &, ////////////////////////////////////////////////// void TriggeredPublisher::PublishMsg(std::size_t pending) { - for (auto &info : this->outputInfo) + for (auto &info : this->outputInfo) + { + for (std::size_t i = 0; i < pending; ++i) { - for (std::size_t i = 0; i < pending; ++i) - { - info.pub.Publish(*info.msgData); - } + info.pub.Publish(*info.msgData); } + } } ////////////////////////////////////////////////// @@ -721,8 +721,7 @@ void TriggeredPublisher::CallService() for (auto &serviceInfo : this->servOutputInfo) { bool result; - auto req = msgs::Factory::New(serviceInfo.reqType, - serviceInfo.reqMsg); + auto req = msgs::Factory::New(serviceInfo.reqType, serviceInfo.reqMsg); if (!req) { ignerr << "Unable to create request for type [" @@ -738,28 +737,22 @@ void TriggeredPublisher::CallService() return; } - bool executed = this->node.Request(serviceInfo.servName, - *req, - serviceInfo.timeout, - *rep, - result); + bool executed = this->node.Request(serviceInfo.srvName, *req, + serviceInfo.timeout, *rep, result); if (executed) { if (!result) { - ignerr << "Service call [" << serviceInfo.servName - << "] failed\n"; + ignerr << "Service call [" << serviceInfo.srvName << "] failed\n"; } else { - ignmsg << "Service call [" << serviceInfo.servName - << "] succeeded\n"; + ignmsg << "Service call [" << serviceInfo.srvName << "] succeeded\n"; } } else { - ignerr << "Service call [" << serviceInfo.servName - << "] timed out\n"; + ignerr << "Service call [" << serviceInfo.srvName << "] timed out\n"; } { std::lock_guard lock(this->triggerServMutex); @@ -788,15 +781,16 @@ void TriggeredPublisher::DoWork() continue; } - if (this->publishCount >0) - { - std::swap(pending, this->publishCount); - PublishMsg(pending); - } - if (this->triggerServ) - { - CallService(); - } + std::swap(pending, this->publishCount); + } + + if (pending > 0) + { + PublishMsg(pending); + } + if (this->triggerServ) + { + CallService(); } } } diff --git a/src/systems/triggered_publisher/TriggeredPublisher.hh b/src/systems/triggered_publisher/TriggeredPublisher.hh index 122a9ba3b4..dd36524b4d 100644 --- a/src/systems/triggered_publisher/TriggeredPublisher.hh +++ b/src/systems/triggered_publisher/TriggeredPublisher.hh @@ -232,7 +232,7 @@ namespace systems private: struct ServOutputInfo { /// \brief Service name - std::string servName; + std::string srvName; /// \brief Service request type std::string reqType; diff --git a/test/integration/triggered_publisher.cc b/test/integration/triggered_publisher.cc index 347e015ec8..e8a95de9f8 100644 --- a/test/integration/triggered_publisher.cc +++ b/test/integration/triggered_publisher.cc @@ -639,12 +639,14 @@ TEST_F(TriggeredPublisherTest, } ///////////////////////////////////////////////// -// Test for invalid service name. It'll timeout -// when matching service name can't be found. -// --- -// Request Type: msgs::StringMsg -// Reply Type: msgs::StringMsg -///////////////////////////////////////////////// +/// Test for invalid service name. A service, `/srv-dummy-test` is advertised +/// and the callback is also provided in this test. Everytime an input msg is +/// published to `/in_14` topic, triggered_publisher plugin will call the +/// service `srv-test`, specified in the test/worlds/triggered_publisher.sdf. +/// However, since the two service names do not match, the callback provided in +/// this test will not be invoked. Therefore, the pubCount, which is set to 10, +/// will not equal to recvCount. The recvCount will be 0, since the callback +/// isn't invoked. TEST_F(TriggeredPublisherTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(InvalidServiceName)) { @@ -655,16 +657,16 @@ TEST_F(TriggeredPublisherTest, auto srvEchoCb = std::function( [&recvCount](const auto &req, auto &rep) + { + EXPECT_EQ(req.data(), "test"); + if (req.data() == "test") { - EXPECT_EQ(req.data(), "test"); - if (req.data() == "test") - { - ++recvCount; - rep.set_data(req.data()); - return true; - } - return false; - }); + ++recvCount; + rep.set_data(req.data()); + return true; + } + return false; + }); // Advertise a dummy service std::string service = "/srv-dummy-test"; @@ -682,14 +684,15 @@ TEST_F(TriggeredPublisherTest, } ///////////////////////////////////////////////// -// Test for triggering a service call in response -// to input ign msg by publishing 10 times. Service -// call will also occur 10 times. It'll compare -// pubCount and recvCount. -// --- -// Request Type: msgs::StringMsg -// Reply Type: msgs::StringMsg -///////////////////////////////////////////////// +/// Test for triggering a service call in response to an input msg. A service, +/// `srv-test` is advertised and the callback is also provided in this test. +/// Everytime an input msg is published to `/in_14` topic, triggered_publisher +/// plugin will call the service `/srv-test`, specified in the test/worlds/ +/// triggered_publisher.sdf. This time, the name of the services match. By +/// publishing input msg 10 times, the service callback will also be invoked 10 +/// times. The `pubCount` is set to 10 and recvCount is increased everytime +/// request data matches the string "test" inside the service callback. For a +/// successful test, the pubCount will equal to recvCount. TEST_F(TriggeredPublisherTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(InputMessagesTriggerServices)) { @@ -711,7 +714,6 @@ TEST_F(TriggeredPublisherTest, return false; }); - // Advertise a dummy service std::string service = "/srv-test"; node.Advertise(service, srvEchoCb); @@ -727,15 +729,14 @@ TEST_F(TriggeredPublisherTest, } ///////////////////////////////////////////////// -// Test for triggering multiple services in response -// to a input ign msg by publishing 10 times. Service -// call will also occur 10 times for each service. -// It'll compare pubCount and recvCount for each -// services. -// --- -// Request Type: msgs::Boolean -// Reply Type: msgs::Boolean -///////////////////////////////////////////////// +/// Test for triggering multiple services (in sequence) in response to an input +/// msg by publishing 10 times. Two services, `srv-test-0` and `srv-test-1` are +/// specified in the test/worlds/triggered_publisher.sdf. Everytime an input msg +/// is published, triggered_publisher will call the service and invoke the +/// callback. std::vector is passed as a reference and will be populated with +/// the request message, which will be a boolean value of `true`. If successful, +/// `recvMsg0` and `recvMsg1` vectors should both have a size of 10 with all +/// true boolean values. TEST_F(TriggeredPublisherTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(MultipleServiceForOneInput)) { @@ -750,7 +751,7 @@ TEST_F(TriggeredPublisherTest, [&_msgVector, &recvMsgMutex](const auto &req, auto &rep) { std::lock_guard lock(recvMsgMutex); - if (req.data() || !req.data()) + if (req.data()) { rep.set_data(true); _msgVector.push_back(req.data()); @@ -775,27 +776,25 @@ TEST_F(TriggeredPublisherTest, } waitUntil(5000, [&] - { - std::lock_guard lock(recvMsgMutex); - return static_cast(pubCount) == recvMsgs0.size() && - static_cast(pubCount) == recvMsgs1.size(); - }); + { + std::lock_guard lock(recvMsgMutex); + return static_cast(pubCount) == recvMsgs0.size() && + static_cast(pubCount) == recvMsgs1.size(); + }); EXPECT_EQ(static_cast(pubCount), recvMsgs0.size()); EXPECT_EQ(static_cast(pubCount), recvMsgs1.size()); // The plugin has two outputs. We expect 10 messages in each output topic EXPECT_EQ(pubCount, std::count(recvMsgs0.begin(), recvMsgs0.end(), true)); - EXPECT_EQ(pubCount, std::count(recvMsgs1.begin(), recvMsgs1.end(), false)); + EXPECT_EQ(pubCount, std::count(recvMsgs1.begin(), recvMsgs1.end(), true)); } ///////////////////////////////////////////////// -// Test for triggering a service call with incorrect -// request type or reseponse type specified in sdf. -// --- -// Request Type: msgs::StringMsg -// Reply Type: msgs::StringMsg -///////////////////////////////////////////////// +/// Test for triggering a service call with incorrect request type or reply +/// type specified in test/worlds/triggered_publisher.sdf. `InvalidReqType` and +/// `InvalidRepType` do not exist. Hence, the callback will never be invoked and +/// the recvCount will be 0. TEST_F(TriggeredPublisherTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(WrongRequestOrResponseType)) { @@ -833,69 +832,23 @@ TEST_F(TriggeredPublisherTest, } ///////////////////////////////////////////////// -// Test for triggering a service call that'll return -// False result. -// --- -// Request Type: msgs::StringMsg -// Reply Type: msgs::StringMsg -///////////////////////////////////////////////// -TEST_F(TriggeredPublisherTest, - IGN_UTILS_TEST_DISABLED_ON_WIN32(FailedReesultServiceCall)) -{ - transport::Node node; - auto inputPub = node.Advertise("/in_17"); - std::atomic recvCount{0}; - - auto srvEchoCb = std::function( - [&recvCount](const auto &req, auto &rep) - { - EXPECT_EQ(req.data(), "test"); - if (req.data() == "test") - { - ++recvCount; - rep.set_data(req.data()); - // return True was substitued with False - return false; - } - return false; - }); - - // Advertise a dummy service - std::string service = "/srv-test"; - node.Advertise(service, srvEchoCb); - - const std::size_t pubCount{10}; - for (std::size_t i = 0; i < pubCount; ++i) - { - EXPECT_TRUE(inputPub.Publish(msgs::Empty())); - IGN_SLEEP_MS(100); - } - - waitUntil(5000, [&]{return pubCount == recvCount;}); - EXPECT_EQ(pubCount, recvCount); -} - -///////////////////////////////////////////////// -// Test for triggering a service call with different -// ignition msg type -// --- -// Request Type: msgs::Empty -// Reply Type: msgs::StringMsg -///////////////////////////////////////////////// +/// Test for triggering a service call with different request (Boolean) and +/// reply type (StringMsg). Check `InputMessagesTriggerServices` test for more +/// details on how the test works. This test is very similar except that it has +/// different request and reply type. TEST_F(TriggeredPublisherTest, - IGN_UTILS_TEST_DISABLED_ON_WIN32(EmptyReqStringMsgRep)) + IGN_UTILS_TEST_DISABLED_ON_WIN32(BooleanReqStringMsgRep)) { transport::Node node; auto inputPub = node.Advertise("/in_18"); std::atomic recvCount{0}; - auto srvEchoCb = std::function( [&recvCount](const auto &req, auto &rep) { - EXPECT_EQ(req.unused(), true); - if (req.unused() == true) + EXPECT_EQ(req.data(), true); + if (req.data() == true) { ++recvCount; rep.set_data("srv-empty"); @@ -905,7 +858,7 @@ TEST_F(TriggeredPublisherTest, }); // Advertise a dummy service - std::string service = "/srv-empty-req-test"; + std::string service = "/srv-diff-type-0"; node.Advertise(service, srvEchoCb); const std::size_t pubCount{10}; @@ -920,35 +873,33 @@ TEST_F(TriggeredPublisherTest, } ///////////////////////////////////////////////// -// Test for triggering a service call with different -// ignition msg type -// --- -// Request Type: msgs::StringMsg -// Reply Type: msgs::Empty -///////////////////////////////////////////////// +/// Test for triggering a service call with different request (StringMsg) and +/// reply type (Boolean). Check `InputMessagesTriggerServices` test for more +/// details on how the test works. This test is very similar except that it has +/// different request and reply type. TEST_F(TriggeredPublisherTest, - IGN_UTILS_TEST_DISABLED_ON_WIN32(StringMsgReqEmptyRep)) + IGN_UTILS_TEST_DISABLED_ON_WIN32(StringMsgReqBooleanRep)) { transport::Node node; auto inputPub = node.Advertise("/in_19"); std::atomic recvCount{0}; auto srvEchoCb = std::function( + msgs::Boolean &)>( [&recvCount](const auto &req, auto &rep) { EXPECT_EQ(req.data(), "test"); if (req.data() == "test") { ++recvCount; - rep.set_unused(true); + rep.set_data(true); return true; } return false; }); // Advertise a dummy service - std::string service = "/srv-empty-rep-test"; + std::string service = "/srv-diff-type-1"; node.Advertise(service, srvEchoCb); const std::size_t pubCount{10}; diff --git a/test/worlds/triggered_publisher.sdf b/test/worlds/triggered_publisher.sdf index 5d21cbf855..2ce7ab2394 100644 --- a/test/worlds/triggered_publisher.sdf +++ b/test/worlds/triggered_publisher.sdf @@ -233,7 +233,7 @@ reqType="ignition.msgs.Boolean" repType="ignition.msgs.Boolean" timeout="100" - reqMsg="data: false"> + reqMsg="data: true"> @@ -263,29 +263,16 @@ - - - - - - + reqMsg="data: true"> @@ -294,9 +281,9 @@ name="ignition::gazebo::systems::TriggeredPublisher"> diff --git a/tutorials/triggered_publisher.md b/tutorials/triggered_publisher.md index 358bff46b7..ff4759c3ef 100644 --- a/tutorials/triggered_publisher.md +++ b/tutorials/triggered_publisher.md @@ -2,7 +2,7 @@ The `TriggeredPublisher` system publishes a user specified message on an output topic in response to an input message that matches user specified criteria. It -can also call a user specified service as an output in response to an input +can also call a user specified service in response to an input message. The system works by checking the input against a set of Matchers. Matchers contain string representations of protobuf messages which are compared for equality or containment with the input message. Matchers can match the From d64b17e014373780267c9313c7c6d8bc86cca2be Mon Sep 17 00:00:00 2001 From: Liam Han Date: Tue, 23 Aug 2022 17:02:20 +0000 Subject: [PATCH 20/26] changed string with 'serv' to 'srv' and included to the header Signed-off-by: Liam Han --- .../triggered_publisher/TriggeredPublisher.cc | 20 +++++++++---------- .../triggered_publisher/TriggeredPublisher.hh | 10 +++++----- 2 files changed, 15 insertions(+), 15 deletions(-) diff --git a/src/systems/triggered_publisher/TriggeredPublisher.cc b/src/systems/triggered_publisher/TriggeredPublisher.cc index 27e9e6bcba..1b2036fb10 100644 --- a/src/systems/triggered_publisher/TriggeredPublisher.cc +++ b/src/systems/triggered_publisher/TriggeredPublisher.cc @@ -609,7 +609,7 @@ void TriggeredPublisher::Configure(const Entity &, for (auto serviceElem = sdfClone->GetElement("service"); serviceElem; serviceElem = serviceElem->GetNextElement("service")) { - ServOutputInfo serviceInfo; + SrvOutputInfo serviceInfo; serviceInfo.srvName = serviceElem->Get("name"); if (serviceInfo.srvName.empty()) { @@ -642,7 +642,7 @@ void TriggeredPublisher::Configure(const Entity &, } serviceInfo.timeout = std::stoi(timeoutInfo); - this->servOutputInfo.push_back(std::move(serviceInfo)); + this->srvOutputInfo.push_back(std::move(serviceInfo)); } } if (!sdfClone->HasElement("service") && !sdfClone->HasElement("output")) @@ -670,11 +670,11 @@ void TriggeredPublisher::Configure(const Entity &, } this->newMatchSignal.notify_one(); } - if (this->servOutputInfo.size() > 0) + if (this->srvOutputInfo.size() > 0) { { - std::lock_guard lock(this->triggerServMutex); - this->triggerServ = true; + std::lock_guard lock(this->triggerSrvMutex); + this->triggerSrv = true; } this->newMatchSignal.notify_one(); } @@ -718,7 +718,7 @@ void TriggeredPublisher::PublishMsg(std::size_t pending) ////////////////////////////////////////////////// void TriggeredPublisher::CallService() { - for (auto &serviceInfo : this->servOutputInfo) + for (auto &serviceInfo : this->srvOutputInfo) { bool result; auto req = msgs::Factory::New(serviceInfo.reqType, serviceInfo.reqMsg); @@ -755,8 +755,8 @@ void TriggeredPublisher::CallService() ignerr << "Service call [" << serviceInfo.srvName << "] timed out\n"; } { - std::lock_guard lock(this->triggerServMutex); - this->triggerServ = false; + std::lock_guard lock(this->triggerSrvMutex); + this->triggerSrv = false; } } } @@ -773,7 +773,7 @@ void TriggeredPublisher::DoWork() this->newMatchSignal.wait_for(lock, 1s, [this] { - return (this->publishCount > 0) || this-> triggerServ || this->done; + return (this->publishCount > 0) || this-> triggerSrv || this->done; }); if (this->publishCount == 0 || this->done) @@ -788,7 +788,7 @@ void TriggeredPublisher::DoWork() { PublishMsg(pending); } - if (this->triggerServ) + if (this->triggerSrv) { CallService(); } diff --git a/src/systems/triggered_publisher/TriggeredPublisher.hh b/src/systems/triggered_publisher/TriggeredPublisher.hh index dd36524b4d..88bce55a57 100644 --- a/src/systems/triggered_publisher/TriggeredPublisher.hh +++ b/src/systems/triggered_publisher/TriggeredPublisher.hh @@ -20,7 +20,7 @@ #include #include #include - +#include #include #include "ignition/gazebo/System.hh" @@ -229,7 +229,7 @@ namespace systems }; /// \brief Class that holds necessary bits for each specified service output - private: struct ServOutputInfo + private: struct SrvOutputInfo { /// \brief Service name std::string srvName; @@ -254,7 +254,7 @@ namespace systems private: std::vector outputInfo; /// \brief List of service outputs - private: std::vector servOutputInfo; + private: std::vector srvOutputInfo; /// \brief Ignition communication node. private: transport::Node node; @@ -263,13 +263,13 @@ namespace systems private: std::size_t publishCount{0}; /// \brief Flag to trigger calling a service in the DoWork thread - private: bool triggerServ{false}; + private: bool triggerSrv{false}; /// \brief Mutex to synchronize access to publishCount private: std::mutex publishCountMutex; /// \brief Mutex to synchronize access to serviceCount - private: std::mutex triggerServMutex; + private: std::mutex triggerSrvMutex; /// \brief Condition variable to signal that new matches have occured private: std::condition_variable newMatchSignal; From 33a2a736cb452b80768d6c42ff69f76aadc379f9 Mon Sep 17 00:00:00 2001 From: Liam Han Date: Tue, 30 Aug 2022 07:52:06 +0000 Subject: [PATCH 21/26] fixed indentation and removed rep.set_data since it's unused on the client service Signed-off-by: Liam Han --- test/integration/triggered_publisher.cc | 72 ++++++++++++------------- 1 file changed, 33 insertions(+), 39 deletions(-) diff --git a/test/integration/triggered_publisher.cc b/test/integration/triggered_publisher.cc index e8a95de9f8..8a2170ae92 100644 --- a/test/integration/triggered_publisher.cc +++ b/test/integration/triggered_publisher.cc @@ -662,7 +662,6 @@ TEST_F(TriggeredPublisherTest, if (req.data() == "test") { ++recvCount; - rep.set_data(req.data()); return true; } return false; @@ -703,16 +702,15 @@ TEST_F(TriggeredPublisherTest, auto srvEchoCb = std::function( [&recvCount](const auto &req, auto &rep) - { - EXPECT_EQ(req.data(), "test"); - if (req.data() == "test") - { - ++recvCount; - rep.set_data(req.data()); - return true; - } - return false; - }); + { + EXPECT_EQ(req.data(), "test"); + if (req.data() == "test") + { + ++recvCount; + return true; + } + return false; + }); std::string service = "/srv-test"; node.Advertise(service, srvEchoCb); @@ -753,7 +751,6 @@ TEST_F(TriggeredPublisherTest, std::lock_guard lock(recvMsgMutex); if (req.data()) { - rep.set_data(true); _msgVector.push_back(req.data()); return true; } @@ -805,16 +802,15 @@ TEST_F(TriggeredPublisherTest, auto srvEchoCb = std::function( [&recvCount](const auto &req, auto &rep) + { + EXPECT_EQ(req.data(), "test"); + if (req.data() == "test") { - EXPECT_EQ(req.data(), "test"); - if (req.data() == "test") - { - ++recvCount; - rep.set_data(req.data()); - return true; - } - return false; - }); + ++recvCount; + return true; + } + return false; + }); // Advertise a dummy service std::string service = "/srv-test"; @@ -846,16 +842,15 @@ TEST_F(TriggeredPublisherTest, auto srvEchoCb = std::function( [&recvCount](const auto &req, auto &rep) + { + EXPECT_EQ(req.data(), true); + if (req.data() == true) { - EXPECT_EQ(req.data(), true); - if (req.data() == true) - { - ++recvCount; - rep.set_data("srv-empty"); - return true; - } - return false; - }); + ++recvCount; + return true; + } + return false; + }); // Advertise a dummy service std::string service = "/srv-diff-type-0"; @@ -887,16 +882,15 @@ TEST_F(TriggeredPublisherTest, auto srvEchoCb = std::function( [&recvCount](const auto &req, auto &rep) + { + EXPECT_EQ(req.data(), "test"); + if (req.data() == "test") { - EXPECT_EQ(req.data(), "test"); - if (req.data() == "test") - { - ++recvCount; - rep.set_data(true); - return true; - } - return false; - }); + ++recvCount; + return true; + } + return false; + }); // Advertise a dummy service std::string service = "/srv-diff-type-1"; From 7b5b33fedef4c15f26af78c0960051b6c370fbd6 Mon Sep 17 00:00:00 2001 From: Liam Han Date: Tue, 30 Aug 2022 08:06:25 +0000 Subject: [PATCH 22/26] getting rid of the id Signed-off-by: Liam Han --- examples/worlds/triggered_publisher.sdf | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/worlds/triggered_publisher.sdf b/examples/worlds/triggered_publisher.sdf index a990a7a182..98b7c52c64 100644 --- a/examples/worlds/triggered_publisher.sdf +++ b/examples/worlds/triggered_publisher.sdf @@ -472,7 +472,7 @@ start falling. reqType="ignition.msgs.Pose" repType="ignition.msgs.Boolean" timeout="3000" - reqMsg="name: 'blue_vehicle', id: 8, position: {x: -3, z: 0.325}"> + reqMsg="name: 'blue_vehicle', position: {x: -3, z: 0.325}"> Date: Wed, 31 Aug 2022 00:29:35 +0000 Subject: [PATCH 23/26] fixed race condition resulting seldom test failure Signed-off-by: Liam Han --- .../triggered_publisher/TriggeredPublisher.cc | 32 +++++++++---------- test/integration/triggered_publisher.cc | 2 +- 2 files changed, 16 insertions(+), 18 deletions(-) diff --git a/src/systems/triggered_publisher/TriggeredPublisher.cc b/src/systems/triggered_publisher/TriggeredPublisher.cc index 1b2036fb10..9a9ea37bfc 100644 --- a/src/systems/triggered_publisher/TriggeredPublisher.cc +++ b/src/systems/triggered_publisher/TriggeredPublisher.cc @@ -672,11 +672,8 @@ void TriggeredPublisher::Configure(const Entity &, } if (this->srvOutputInfo.size() > 0) { - { - std::lock_guard lock(this->triggerSrvMutex); - this->triggerSrv = true; - } - this->newMatchSignal.notify_one(); + std::lock_guard lock(this->triggerSrvMutex); + this->triggerSrv = true; } } }); @@ -754,10 +751,6 @@ void TriggeredPublisher::CallService() { ignerr << "Service call [" << serviceInfo.srvName << "] timed out\n"; } - { - std::lock_guard lock(this->triggerSrvMutex); - this->triggerSrv = false; - } } } @@ -766,6 +759,7 @@ void TriggeredPublisher::DoWork() { while (!this->done) { + // check whether to publish a msg by checking publishCount std::size_t pending{0}; { using namespace std::chrono_literals; @@ -773,25 +767,29 @@ void TriggeredPublisher::DoWork() this->newMatchSignal.wait_for(lock, 1s, [this] { - return (this->publishCount > 0) || this-> triggerSrv || this->done; + return (this->publishCount > 0) || this->done; }); if (this->publishCount == 0 || this->done) { continue; } - std::swap(pending, this->publishCount); } - if (pending > 0) - { - PublishMsg(pending); - } - if (this->triggerSrv) + PublishMsg(pending); + + // check whether to call a service by checking triggerSrv + bool callSrv{false}; { - CallService(); + std::lock_guard lock(this->triggerSrvMutex); + if (!this->triggerSrv || this->done){ + continue; + } + std::swap(callSrv, this->triggerSrv); } + + CallService(); } } diff --git a/test/integration/triggered_publisher.cc b/test/integration/triggered_publisher.cc index 8a2170ae92..5a6d551950 100644 --- a/test/integration/triggered_publisher.cc +++ b/test/integration/triggered_publisher.cc @@ -769,7 +769,7 @@ TEST_F(TriggeredPublisherTest, for (int i = 0; i < pubCount; ++i) { EXPECT_TRUE(inputPub.Publish(msgs::Empty())); - IGN_SLEEP_MS(10); + IGN_SLEEP_MS(100); } waitUntil(5000, [&] From 0569e1017a09fdf972c3a5620b943aef3e962791 Mon Sep 17 00:00:00 2001 From: Liam Han Date: Wed, 31 Aug 2022 04:03:33 +0000 Subject: [PATCH 24/26] changed from triggerSrv to serviceCount. This compensates for the two threads running at different rate Signed-off-by: Liam Han --- .../triggered_publisher/TriggeredPublisher.cc | 67 ++++++++++--------- .../triggered_publisher/TriggeredPublisher.hh | 8 +-- 2 files changed, 39 insertions(+), 36 deletions(-) diff --git a/src/systems/triggered_publisher/TriggeredPublisher.cc b/src/systems/triggered_publisher/TriggeredPublisher.cc index 9a9ea37bfc..cc7a4f88ad 100644 --- a/src/systems/triggered_publisher/TriggeredPublisher.cc +++ b/src/systems/triggered_publisher/TriggeredPublisher.cc @@ -673,7 +673,7 @@ void TriggeredPublisher::Configure(const Entity &, if (this->srvOutputInfo.size() > 0) { std::lock_guard lock(this->triggerSrvMutex); - this->triggerSrv = true; + ++this->serviceCount; } } }); @@ -713,44 +713,47 @@ void TriggeredPublisher::PublishMsg(std::size_t pending) } ////////////////////////////////////////////////// -void TriggeredPublisher::CallService() +void TriggeredPublisher::CallService(std::size_t pendingSrv) { for (auto &serviceInfo : this->srvOutputInfo) { - bool result; - auto req = msgs::Factory::New(serviceInfo.reqType, serviceInfo.reqMsg); - if (!req) + for (std::size_t i = 0; i < pendingSrv; ++i) { - ignerr << "Unable to create request for type [" - << serviceInfo.reqType << "].\n"; - return; - } + bool result; + auto req = msgs::Factory::New(serviceInfo.reqType, serviceInfo.reqMsg); + if (!req) + { + ignerr << "Unable to create request for type [" + << serviceInfo.reqType << "].\n"; + return; + } - auto rep = msgs::Factory::New(serviceInfo.repType); - if (!rep) - { - ignerr << "Unable to create response for type [" - << serviceInfo.repType << "].\n"; - return; - } + auto rep = msgs::Factory::New(serviceInfo.repType); + if (!rep) + { + ignerr << "Unable to create response for type [" + << serviceInfo.repType << "].\n"; + return; + } - bool executed = this->node.Request(serviceInfo.srvName, *req, - serviceInfo.timeout, *rep, result); - if (executed) - { - if (!result) + bool executed = this->node.Request(serviceInfo.srvName, *req, + serviceInfo.timeout, *rep, result); + if (executed) { - ignerr << "Service call [" << serviceInfo.srvName << "] failed\n"; + if (!result) + { + ignerr << "Service call [" << serviceInfo.srvName << "] failed\n"; + } + else + { + ignmsg << "Service call [" << serviceInfo.srvName << "] succeeded\n"; + } } else { - ignmsg << "Service call [" << serviceInfo.srvName << "] succeeded\n"; + ignerr << "Service call [" << serviceInfo.srvName << "] timed out\n"; } } - else - { - ignerr << "Service call [" << serviceInfo.srvName << "] timed out\n"; - } } } @@ -779,17 +782,17 @@ void TriggeredPublisher::DoWork() PublishMsg(pending); - // check whether to call a service by checking triggerSrv - bool callSrv{false}; + // check whether to call a service by checking serviceCount + std::size_t pendingSrv{0}; { std::lock_guard lock(this->triggerSrvMutex); - if (!this->triggerSrv || this->done){ + if (this->serviceCount == 0 || this->done){ continue; } - std::swap(callSrv, this->triggerSrv); + std::swap(pendingSrv, this->serviceCount); } - CallService(); + CallService(pendingSrv); } } diff --git a/src/systems/triggered_publisher/TriggeredPublisher.hh b/src/systems/triggered_publisher/TriggeredPublisher.hh index 88bce55a57..2560e67722 100644 --- a/src/systems/triggered_publisher/TriggeredPublisher.hh +++ b/src/systems/triggered_publisher/TriggeredPublisher.hh @@ -195,7 +195,7 @@ namespace systems public: void DoWork(); /// \brief Method that calls a service - private: void CallService(); + private: void CallService(std::size_t pendingSrv); /// \brief Method that publishes a message private: void PublishMsg(std::size_t pending); @@ -259,12 +259,12 @@ namespace systems /// \brief Ignition communication node. private: transport::Node node; + /// \brief Counter that tells how manny times to call the service + private: std::size_t serviceCount{0}; + /// \brief Counter that tells the publisher how many times to publish private: std::size_t publishCount{0}; - /// \brief Flag to trigger calling a service in the DoWork thread - private: bool triggerSrv{false}; - /// \brief Mutex to synchronize access to publishCount private: std::mutex publishCountMutex; From fced8fb485000b803cc63f4af97bd362d748dc14 Mon Sep 17 00:00:00 2001 From: Mabel Zhang Date: Fri, 2 Sep 2022 20:15:43 -0400 Subject: [PATCH 25/26] braces indentation Signed-off-by: Mabel Zhang --- test/integration/triggered_publisher.cc | 66 ++++++++++++------------- 1 file changed, 33 insertions(+), 33 deletions(-) diff --git a/test/integration/triggered_publisher.cc b/test/integration/triggered_publisher.cc index 5a6d551950..fdd05935d7 100644 --- a/test/integration/triggered_publisher.cc +++ b/test/integration/triggered_publisher.cc @@ -702,15 +702,15 @@ TEST_F(TriggeredPublisherTest, auto srvEchoCb = std::function( [&recvCount](const auto &req, auto &rep) - { - EXPECT_EQ(req.data(), "test"); - if (req.data() == "test") + { + EXPECT_EQ(req.data(), "test"); + if (req.data() == "test") { ++recvCount; return true; } - return false; - }); + return false; + }); std::string service = "/srv-test"; node.Advertise(service, srvEchoCb); @@ -750,10 +750,10 @@ TEST_F(TriggeredPublisherTest, { std::lock_guard lock(recvMsgMutex); if (req.data()) - { - _msgVector.push_back(req.data()); - return true; - } + { + _msgVector.push_back(req.data()); + return true; + } return false; }); }; @@ -802,15 +802,15 @@ TEST_F(TriggeredPublisherTest, auto srvEchoCb = std::function( [&recvCount](const auto &req, auto &rep) - { - EXPECT_EQ(req.data(), "test"); - if (req.data() == "test") { - ++recvCount; - return true; - } - return false; - }); + EXPECT_EQ(req.data(), "test"); + if (req.data() == "test") + { + ++recvCount; + return true; + } + return false; + }); // Advertise a dummy service std::string service = "/srv-test"; @@ -842,15 +842,15 @@ TEST_F(TriggeredPublisherTest, auto srvEchoCb = std::function( [&recvCount](const auto &req, auto &rep) - { - EXPECT_EQ(req.data(), true); - if (req.data() == true) { - ++recvCount; - return true; - } - return false; - }); + EXPECT_EQ(req.data(), true); + if (req.data() == true) + { + ++recvCount; + return true; + } + return false; + }); // Advertise a dummy service std::string service = "/srv-diff-type-0"; @@ -882,15 +882,15 @@ TEST_F(TriggeredPublisherTest, auto srvEchoCb = std::function( [&recvCount](const auto &req, auto &rep) - { - EXPECT_EQ(req.data(), "test"); - if (req.data() == "test") { - ++recvCount; - return true; - } - return false; - }); + EXPECT_EQ(req.data(), "test"); + if (req.data() == "test") + { + ++recvCount; + return true; + } + return false; + }); // Advertise a dummy service std::string service = "/srv-diff-type-1"; From a88d88e5c39c5d62ac1f82877d462a29a6fc444b Mon Sep 17 00:00:00 2001 From: Liam Han Date: Wed, 7 Sep 2022 05:34:33 +0000 Subject: [PATCH 26/26] addressing gnu c compiler (gcc) warnings Signed-off-by: Liam Han --- test/integration/triggered_publisher.cc | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/test/integration/triggered_publisher.cc b/test/integration/triggered_publisher.cc index fdd05935d7..3ae1070d12 100644 --- a/test/integration/triggered_publisher.cc +++ b/test/integration/triggered_publisher.cc @@ -656,7 +656,7 @@ TEST_F(TriggeredPublisherTest, auto srvEchoCb = std::function( - [&recvCount](const auto &req, auto &rep) + [&recvCount](const auto &req, auto &) { EXPECT_EQ(req.data(), "test"); if (req.data() == "test") @@ -701,7 +701,7 @@ TEST_F(TriggeredPublisherTest, auto srvEchoCb = std::function( - [&recvCount](const auto &req, auto &rep) + [&recvCount](const auto &req, auto &) { EXPECT_EQ(req.data(), "test"); if (req.data() == "test") @@ -746,7 +746,7 @@ TEST_F(TriggeredPublisherTest, auto cbCreator = [&recvMsgMutex](std::vector &_msgVector) { return std::function( - [&_msgVector, &recvMsgMutex](const auto &req, auto &rep) + [&_msgVector, &recvMsgMutex](const auto &req, auto &) { std::lock_guard lock(recvMsgMutex); if (req.data()) @@ -801,7 +801,7 @@ TEST_F(TriggeredPublisherTest, auto srvEchoCb = std::function( - [&recvCount](const auto &req, auto &rep) + [&recvCount](const auto &req, auto &) { EXPECT_EQ(req.data(), "test"); if (req.data() == "test") @@ -841,7 +841,7 @@ TEST_F(TriggeredPublisherTest, auto srvEchoCb = std::function( - [&recvCount](const auto &req, auto &rep) + [&recvCount](const auto &req, auto &) { EXPECT_EQ(req.data(), true); if (req.data() == true) @@ -881,7 +881,7 @@ TEST_F(TriggeredPublisherTest, auto srvEchoCb = std::function( - [&recvCount](const auto &req, auto &rep) + [&recvCount](const auto &req, auto &) { EXPECT_EQ(req.data(), "test"); if (req.data() == "test")