diff --git a/src/systems/user_commands/UserCommands.cc b/src/systems/user_commands/UserCommands.cc index d4929d793f..805279dc31 100644 --- a/src/systems/user_commands/UserCommands.cc +++ b/src/systems/user_commands/UserCommands.cc @@ -18,6 +18,8 @@ */ #include "UserCommands.hh" +#include +#include #ifdef _MSC_VER #pragma warning(push) @@ -143,6 +145,10 @@ class UserCommandBase /// \return True if command was properly executed. public: virtual bool Execute() = 0; + /// \brief Promise that is used to exchange the result of the command + /// execution with the service handler. + public: std::promise promise; + /// \brief Message containing command. protected: google::protobuf::Message *msg{nullptr}; @@ -456,6 +462,15 @@ class gz::sim::systems::UserCommandsPrivate public: template bool ServiceHandler(const InputT &_req, msgs::Boolean &_res); + /// \brief This is similar to \sa ServiceHandler but it blocks the request + /// until the command is actually executed. + /// \tparam CommandT Type for the command that associated with the service. + /// \tparam InputT Type form gz::msgs of the input parameter. + /// \param[in] _req Input parameter message of the service. + /// \param[out] _res Output parameter message of the service. + public: template + bool BlockingServiceHandler(const InputT &_req, msgs::Boolean &_res); + /// \brief Temlpate for advertising services /// \tparam CommandT Type for the command that associated with the service. /// \tparam InputT Type form gz::msgs of the input parameter. @@ -485,6 +500,10 @@ class gz::sim::systems::UserCommandsPrivate /// \brief Mutex to protect pending queue. public: std::mutex pendingMutex; + + /// \brief Global timeout settings for services. + /// \TODO(azeey) Consider making this configurable. + public: const unsigned int kServiceHandlerTimeoutMs{5000}; }; /// \brief Pose3d equality comparison function. @@ -669,7 +688,9 @@ void UserCommands::PreUpdate(const UpdateInfo &/*_info*/, for (auto &cmd : cmds) { // Execute - if (!cmd->Execute()) + bool result = cmd->Execute(); + cmd->promise.set_value(result); + if (!result) continue; // TODO(louise) Update command with current world state @@ -714,8 +735,18 @@ void UserCommandsPrivate::AdvertiseService(const std::string &_topic, { this->node.Advertise( _topic, &UserCommandsPrivate::ServiceHandler, this); + + const auto blockingTopic = _topic + "/blocking"; + this->node.Advertise( + blockingTopic, + &UserCommandsPrivate::BlockingServiceHandler, this); if (_serviceName != nullptr) - gzmsg << _serviceName << " service on [" << _topic << "]" << std::endl; + { + gzmsg << _serviceName << " service on [" << _topic << "] (async)" + << std::endl; + gzmsg << _serviceName << " service on [" << blockingTopic << "] (blocking)" + << std::endl; + } } ////////////////////////////////////////////////// @@ -736,6 +767,31 @@ bool UserCommandsPrivate::ServiceHandler(const InputT &_req, return true; } +////////////////////////////////////////////////// +template +bool UserCommandsPrivate::BlockingServiceHandler(const InputT &_req, + msgs::Boolean &_res) +{ + auto msg = _req.New(); + msg->CopyFrom(_req); + auto cmd = std::make_unique(msg, this->iface); + auto future = cmd->promise.get_future(); + // Push to pending + { + std::lock_guard lock(this->pendingMutex); + this->pendingCmds.push_back(std::move(cmd)); + } + + // This blocks until the command is executed. + if (future.wait_for(std::chrono::milliseconds(kServiceHandlerTimeoutMs)) == + std::future_status::ready) + { + _res.set_data(future.get()); + return true; + } + return false; +} + ////////////////////////////////////////////////// UserCommandBase::UserCommandBase(google::protobuf::Message *_msg, std::shared_ptr &_iface) diff --git a/test/integration/user_commands.cc b/test/integration/user_commands.cc index f506ff0971..477dd98ae8 100644 --- a/test/integration/user_commands.cc +++ b/test/integration/user_commands.cc @@ -15,6 +15,7 @@ * */ +#include #include #include @@ -62,6 +63,32 @@ class UserCommandsTest : public InternalFixture<::testing::Test> { }; +struct AsyncRequestInfo { + bool retval{false}; + msgs::Boolean response; + bool result{false}; +}; + +// This calls a request from a new thread so that the calling function can +// continue even if the request blocks. +template +auto asyncRequest(transport::Node &_node, const std::string &_topic, + const RequestT &_req) +{ + unsigned int timeout = 5000; + auto asyncRetval = std::async(std::launch::async, [&] + { + AsyncRequestInfo info; + info.retval = + _node.Request(_topic, _req, timeout, info.response, info.result); + return info; + }); + // Sleep for a little bit for the async thread to spin up and make the service + // request + GZ_SLEEP_MS(10); + return asyncRetval; +} + ///////////////////////////////////////////////// // See https://github.com/gazebosim/gz-sim/issues/1175 TEST_F(UserCommandsTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(Create)) @@ -137,15 +164,9 @@ TEST_F(UserCommandsTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(Create)) auto pos = pose->mutable_position(); pos->set_z(10); - msgs::Boolean res; - bool result; - unsigned int timeout = 5000; - std::string service{"/world/empty/create"}; - + std::string service{"/world/empty/create/blocking"}; transport::Node node; - EXPECT_TRUE(node.Request(service, req, timeout, res, result)); - EXPECT_TRUE(result); - EXPECT_TRUE(res.data()); + auto requestDataFuture = asyncRequest(node, service, req); // Check entity has not been created yet EXPECT_EQ(kNullEntity, ecm->EntityByComponents(components::Model(), @@ -153,6 +174,13 @@ TEST_F(UserCommandsTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(Create)) // Run an iteration and check it was created server.Run(true, 1, false); + { + auto requestData = requestDataFuture.get(); + EXPECT_TRUE(requestData.retval); + EXPECT_TRUE(requestData.result); + EXPECT_TRUE(requestData.response.data()); + } + EXPECT_EQ(entityCount + 4, ecm->EntityCount()); entityCount = ecm->EntityCount(); @@ -169,12 +197,16 @@ TEST_F(UserCommandsTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(Create)) req.Clear(); req.set_sdf(modelStr); - EXPECT_TRUE(node.Request(service, req, timeout, res, result)); - EXPECT_TRUE(result); - EXPECT_TRUE(res.data()); + requestDataFuture = asyncRequest(node, service, req); // Run an iteration and check it was not created server.Run(true, 1, false); + { + auto requestData = requestDataFuture.get(); + EXPECT_TRUE(requestData.retval); + EXPECT_TRUE(requestData.result); + EXPECT_FALSE(requestData.response.data()); + } EXPECT_EQ(entityCount, ecm->EntityCount()); @@ -183,12 +215,16 @@ TEST_F(UserCommandsTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(Create)) req.set_sdf(modelStr); req.set_allow_renaming(true); - EXPECT_TRUE(node.Request(service, req, timeout, res, result)); - EXPECT_TRUE(result); - EXPECT_TRUE(res.data()); + requestDataFuture = asyncRequest(node, service, req); // Run an iteration and check it was created with a new name server.Run(true, 1, false); + { + auto requestData = requestDataFuture.get(); + EXPECT_TRUE(requestData.retval); + EXPECT_TRUE(requestData.result); + EXPECT_TRUE(requestData.response.data()); + } EXPECT_EQ(entityCount + 4, ecm->EntityCount()); entityCount = ecm->EntityCount(); @@ -202,12 +238,16 @@ TEST_F(UserCommandsTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(Create)) req.set_sdf(modelStr); req.set_name("banana"); - EXPECT_TRUE(node.Request(service, req, timeout, res, result)); - EXPECT_TRUE(result); - EXPECT_TRUE(res.data()); + requestDataFuture = asyncRequest(node, service, req); // Run an iteration and check it was created with given name server.Run(true, 1, false); + { + auto requestData = requestDataFuture.get(); + EXPECT_TRUE(requestData.retval); + EXPECT_TRUE(requestData.result); + EXPECT_TRUE(requestData.response.data()); + } EXPECT_EQ(entityCount + 4, ecm->EntityCount()); entityCount = ecm->EntityCount(); @@ -220,12 +260,16 @@ TEST_F(UserCommandsTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(Create)) req.Clear(); req.set_sdf(lightStr); - EXPECT_TRUE(node.Request(service, req, timeout, res, result)); - EXPECT_TRUE(result); - EXPECT_TRUE(res.data()); + requestDataFuture = asyncRequest(node, service, req); // Run an iteration and check it was created server.Run(true, 1, false); + { + auto requestData = requestDataFuture.get(); + EXPECT_TRUE(requestData.retval); + EXPECT_TRUE(requestData.result); + EXPECT_TRUE(requestData.response.data()); + } EXPECT_EQ(entityCount + 2, ecm->EntityCount()); entityCount = ecm->EntityCount(); @@ -239,12 +283,16 @@ TEST_F(UserCommandsTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(Create)) req.Clear(); req.mutable_light()->set_name("light_test"); req.mutable_light()->set_parent_id(1); - EXPECT_TRUE(node.Request(service, req, timeout, res, result)); - EXPECT_TRUE(result); - EXPECT_TRUE(res.data()); + requestDataFuture = asyncRequest(node, service, req); // Run an iteration and check it was created server.Run(true, 1, false); + { + auto requestData = requestDataFuture.get(); + EXPECT_TRUE(requestData.retval); + EXPECT_TRUE(requestData.result); + EXPECT_TRUE(requestData.response.data()); + } EXPECT_EQ(entityCount + 2, ecm->EntityCount()); entityCount = ecm->EntityCount(); @@ -259,17 +307,13 @@ TEST_F(UserCommandsTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(Create)) req.set_sdf(modelStr); req.set_name("acerola"); - EXPECT_TRUE(node.Request(service, req, timeout, res, result)); - EXPECT_TRUE(result); - EXPECT_TRUE(res.data()); + auto requestDataFuture1 = asyncRequest(node, service, req); req.Clear(); req.set_sdf(modelStr); req.set_name("coconut"); - EXPECT_TRUE(node.Request(service, req, timeout, res, result)); - EXPECT_TRUE(result); - EXPECT_TRUE(res.data()); + auto requestDataFuture2 = asyncRequest(node, service, req); // Check neither exists yet EXPECT_EQ(kNullEntity, ecm->EntityByComponents(components::Model(), @@ -280,6 +324,18 @@ TEST_F(UserCommandsTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(Create)) // Run an iteration and check both models were created server.Run(true, 1, false); + { + auto requestData = requestDataFuture1.get(); + EXPECT_TRUE(requestData.retval); + EXPECT_TRUE(requestData.result); + EXPECT_TRUE(requestData.response.data()); + } + { + auto requestData = requestDataFuture2.get(); + EXPECT_TRUE(requestData.retval); + EXPECT_TRUE(requestData.result); + EXPECT_TRUE(requestData.response.data()); + } EXPECT_EQ(entityCount + 8, ecm->EntityCount()); entityCount = ecm->EntityCount(); @@ -293,12 +349,16 @@ TEST_F(UserCommandsTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(Create)) req.Clear(); req.set_sdf(lightsStr); - EXPECT_TRUE(node.Request(service, req, timeout, res, result)); - EXPECT_TRUE(result); - EXPECT_TRUE(res.data()); + requestDataFuture = asyncRequest(node, service, req); // Run an iteration and check only the 1st was created server.Run(true, 1, false); + { + auto requestData = requestDataFuture.get(); + EXPECT_TRUE(requestData.retval); + EXPECT_TRUE(requestData.result); + EXPECT_TRUE(requestData.response.data()); + } EXPECT_EQ(entityCount + 2, ecm->EntityCount()); entityCount = ecm->EntityCount(); @@ -315,12 +375,16 @@ TEST_F(UserCommandsTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(Create)) req.Clear(); req.set_sdf(badStr); - EXPECT_TRUE(node.Request(service, req, timeout, res, result)); - EXPECT_TRUE(result); - EXPECT_TRUE(res.data()); + requestDataFuture = asyncRequest(node, service, req); // Run an iteration and check nothing was created server.Run(true, 1, false); + { + auto requestData = requestDataFuture.get(); + EXPECT_TRUE(requestData.retval); + EXPECT_TRUE(requestData.result); + EXPECT_FALSE(requestData.response.data()); + } EXPECT_EQ(entityCount, ecm->EntityCount()); @@ -330,12 +394,16 @@ TEST_F(UserCommandsTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(Create)) req.Clear(); req.set_sdf_filename(testModel); - EXPECT_TRUE(node.Request(service, req, timeout, res, result)); - EXPECT_TRUE(result); - EXPECT_TRUE(res.data()); + requestDataFuture = asyncRequest(node, service, req); // Run an iteration and check it was created server.Run(true, 1, false); + { + auto requestData = requestDataFuture.get(); + EXPECT_TRUE(requestData.retval); + EXPECT_TRUE(requestData.result); + EXPECT_TRUE(requestData.response.data()); + } EXPECT_EQ(entityCount + 4, ecm->EntityCount()); EXPECT_NE(kNullEntity, ecm->EntityByComponents(components::Model(),