Skip to content

Commit 42c9cac

Browse files
azeeyazeey
committed
Fix UserCommands services so that they return the actual status the executed command (#2999)
Previously, the services immediately returned true after adding the command to the pending commands queue which meant that the return statuses did not reflect the success or failure of the subsequent execution of the commands. --------- Signed-off-by: Addisu Z. Taddese <[email protected]> (cherry picked from commit 590e0f4)
1 parent a1eb1e0 commit 42c9cac

File tree

2 files changed

+164
-40
lines changed

2 files changed

+164
-40
lines changed

‎src/systems/user_commands/UserCommands.cc‎

Lines changed: 58 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -18,6 +18,8 @@
1818
*/
1919

2020
#include "UserCommands.hh"
21+
#include <chrono>
22+
#include <future>
2123

2224
#ifdef _MSC_VER
2325
#pragma warning(push)
@@ -143,6 +145,10 @@ class UserCommandBase
143145
/// \return True if command was properly executed.
144146
public: virtual bool Execute() = 0;
145147

148+
/// \brief Promise that is used to exchange the result of the command
149+
/// execution with the service handler.
150+
public: std::promise<bool> promise;
151+
146152
/// \brief Message containing command.
147153
protected: google::protobuf::Message *msg{nullptr};
148154

@@ -456,6 +462,15 @@ class gz::sim::systems::UserCommandsPrivate
456462
public: template <typename CommandT, typename InputT>
457463
bool ServiceHandler(const InputT &_req, msgs::Boolean &_res);
458464

465+
/// \brief This is similar to \sa ServiceHandler but it blocks the request
466+
/// until the command is actually executed.
467+
/// \tparam CommandT Type for the command that associated with the service.
468+
/// \tparam InputT Type form gz::msgs of the input parameter.
469+
/// \param[in] _req Input parameter message of the service.
470+
/// \param[out] _res Output parameter message of the service.
471+
public: template <typename CommandT, typename InputT>
472+
bool BlockingServiceHandler(const InputT &_req, msgs::Boolean &_res);
473+
459474
/// \brief Temlpate for advertising services
460475
/// \tparam CommandT Type for the command that associated with the service.
461476
/// \tparam InputT Type form gz::msgs of the input parameter.
@@ -485,6 +500,10 @@ class gz::sim::systems::UserCommandsPrivate
485500

486501
/// \brief Mutex to protect pending queue.
487502
public: std::mutex pendingMutex;
503+
504+
/// \brief Global timeout settings for services.
505+
/// \TODO(azeey) Consider making this configurable.
506+
public: const unsigned int kServiceHandlerTimeoutMs{5000};
488507
};
489508

490509
/// \brief Pose3d equality comparison function.
@@ -676,7 +695,9 @@ void UserCommands::PreUpdate(const UpdateInfo &/*_info*/,
676695
for (auto &cmd : cmds)
677696
{
678697
// Execute
679-
if (!cmd->Execute())
698+
bool result = cmd->Execute();
699+
cmd->promise.set_value(result);
700+
if (!result)
680701
continue;
681702

682703
// TODO(louise) Update command with current world state
@@ -721,8 +742,18 @@ void UserCommandsPrivate::AdvertiseService(const std::string &_topic,
721742
{
722743
this->node.Advertise(
723744
_topic, &UserCommandsPrivate::ServiceHandler<CommandT, InputT>, this);
745+
746+
const auto blockingTopic = _topic + "/blocking";
747+
this->node.Advertise(
748+
blockingTopic,
749+
&UserCommandsPrivate::BlockingServiceHandler<CommandT, InputT>, this);
724750
if (_serviceName != nullptr)
725-
gzmsg << _serviceName << " service on [" << _topic << "]" << std::endl;
751+
{
752+
gzmsg << _serviceName << " service on [" << _topic << "] (async)"
753+
<< std::endl;
754+
gzmsg << _serviceName << " service on [" << blockingTopic << "] (blocking)"
755+
<< std::endl;
756+
}
726757
}
727758

728759
//////////////////////////////////////////////////
@@ -743,6 +774,31 @@ bool UserCommandsPrivate::ServiceHandler(const InputT &_req,
743774
return true;
744775
}
745776

777+
//////////////////////////////////////////////////
778+
template <typename CommandT, typename InputT>
779+
bool UserCommandsPrivate::BlockingServiceHandler(const InputT &_req,
780+
msgs::Boolean &_res)
781+
{
782+
auto msg = _req.New();
783+
msg->CopyFrom(_req);
784+
auto cmd = std::make_unique<CommandT>(msg, this->iface);
785+
auto future = cmd->promise.get_future();
786+
// Push to pending
787+
{
788+
std::lock_guard<std::mutex> lock(this->pendingMutex);
789+
this->pendingCmds.push_back(std::move(cmd));
790+
}
791+
792+
// This blocks until the command is executed.
793+
if (future.wait_for(std::chrono::milliseconds(kServiceHandlerTimeoutMs)) ==
794+
std::future_status::ready)
795+
{
796+
_res.set_data(future.get());
797+
return true;
798+
}
799+
return false;
800+
}
801+
746802
//////////////////////////////////////////////////
747803
UserCommandBase::UserCommandBase(google::protobuf::Message *_msg,
748804
std::shared_ptr<UserCommandsInterface> &_iface)

‎test/integration/user_commands.cc‎

Lines changed: 106 additions & 38 deletions
Original file line numberDiff line numberDiff line change
@@ -15,6 +15,7 @@
1515
*
1616
*/
1717

18+
#include <future>
1819
#include <string>
1920

2021
#include <gtest/gtest.h>
@@ -62,6 +63,32 @@ class UserCommandsTest : public InternalFixture<::testing::Test>
6263
{
6364
};
6465

66+
struct AsyncRequestInfo {
67+
bool retval{false};
68+
msgs::Boolean response;
69+
bool result{false};
70+
};
71+
72+
// This calls a request from a new thread so that the calling function can
73+
// continue even if the request blocks.
74+
template <typename RequestT>
75+
auto asyncRequest(transport::Node &_node, const std::string &_topic,
76+
const RequestT &_req)
77+
{
78+
unsigned int timeout = 5000;
79+
auto asyncRetval = std::async(std::launch::async, [&]
80+
{
81+
AsyncRequestInfo info;
82+
info.retval =
83+
_node.Request(_topic, _req, timeout, info.response, info.result);
84+
return info;
85+
});
86+
// Sleep for a little bit for the async thread to spin up and make the service
87+
// request
88+
GZ_SLEEP_MS(10);
89+
return asyncRetval;
90+
}
91+
6592
/////////////////////////////////////////////////
6693
// See https://github.com/gazebosim/gz-sim/issues/1175
6794
TEST_F(UserCommandsTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(Create))
@@ -137,22 +164,23 @@ TEST_F(UserCommandsTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(Create))
137164
auto pos = pose->mutable_position();
138165
pos->set_z(10);
139166

140-
msgs::Boolean res;
141-
bool result;
142-
unsigned int timeout = 5000;
143-
std::string service{"/world/empty/create"};
144-
167+
std::string service{"/world/empty/create/blocking"};
145168
transport::Node node;
146-
EXPECT_TRUE(node.Request(service, req, timeout, res, result));
147-
EXPECT_TRUE(result);
148-
EXPECT_TRUE(res.data());
169+
auto requestDataFuture = asyncRequest(node, service, req);
149170

150171
// Check entity has not been created yet
151172
EXPECT_EQ(kNullEntity, ecm->EntityByComponents(components::Model(),
152173
components::Name("spawned_model")));
153174

154175
// Run an iteration and check it was created
155176
server.Run(true, 1, false);
177+
{
178+
auto requestData = requestDataFuture.get();
179+
EXPECT_TRUE(requestData.retval);
180+
EXPECT_TRUE(requestData.result);
181+
EXPECT_TRUE(requestData.response.data());
182+
}
183+
156184
EXPECT_EQ(entityCount + 4, ecm->EntityCount());
157185
entityCount = ecm->EntityCount();
158186

@@ -169,12 +197,16 @@ TEST_F(UserCommandsTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(Create))
169197
req.Clear();
170198
req.set_sdf(modelStr);
171199

172-
EXPECT_TRUE(node.Request(service, req, timeout, res, result));
173-
EXPECT_TRUE(result);
174-
EXPECT_TRUE(res.data());
200+
requestDataFuture = asyncRequest(node, service, req);
175201

176202
// Run an iteration and check it was not created
177203
server.Run(true, 1, false);
204+
{
205+
auto requestData = requestDataFuture.get();
206+
EXPECT_TRUE(requestData.retval);
207+
EXPECT_TRUE(requestData.result);
208+
EXPECT_FALSE(requestData.response.data());
209+
}
178210

179211
EXPECT_EQ(entityCount, ecm->EntityCount());
180212

@@ -183,12 +215,16 @@ TEST_F(UserCommandsTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(Create))
183215
req.set_sdf(modelStr);
184216
req.set_allow_renaming(true);
185217

186-
EXPECT_TRUE(node.Request(service, req, timeout, res, result));
187-
EXPECT_TRUE(result);
188-
EXPECT_TRUE(res.data());
218+
requestDataFuture = asyncRequest(node, service, req);
189219

190220
// Run an iteration and check it was created with a new name
191221
server.Run(true, 1, false);
222+
{
223+
auto requestData = requestDataFuture.get();
224+
EXPECT_TRUE(requestData.retval);
225+
EXPECT_TRUE(requestData.result);
226+
EXPECT_TRUE(requestData.response.data());
227+
}
192228

193229
EXPECT_EQ(entityCount + 4, ecm->EntityCount());
194230
entityCount = ecm->EntityCount();
@@ -202,12 +238,16 @@ TEST_F(UserCommandsTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(Create))
202238
req.set_sdf(modelStr);
203239
req.set_name("banana");
204240

205-
EXPECT_TRUE(node.Request(service, req, timeout, res, result));
206-
EXPECT_TRUE(result);
207-
EXPECT_TRUE(res.data());
241+
requestDataFuture = asyncRequest(node, service, req);
208242

209243
// Run an iteration and check it was created with given name
210244
server.Run(true, 1, false);
245+
{
246+
auto requestData = requestDataFuture.get();
247+
EXPECT_TRUE(requestData.retval);
248+
EXPECT_TRUE(requestData.result);
249+
EXPECT_TRUE(requestData.response.data());
250+
}
211251

212252
EXPECT_EQ(entityCount + 4, ecm->EntityCount());
213253
entityCount = ecm->EntityCount();
@@ -220,12 +260,16 @@ TEST_F(UserCommandsTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(Create))
220260
req.Clear();
221261
req.set_sdf(lightStr);
222262

223-
EXPECT_TRUE(node.Request(service, req, timeout, res, result));
224-
EXPECT_TRUE(result);
225-
EXPECT_TRUE(res.data());
263+
requestDataFuture = asyncRequest(node, service, req);
226264

227265
// Run an iteration and check it was created
228266
server.Run(true, 1, false);
267+
{
268+
auto requestData = requestDataFuture.get();
269+
EXPECT_TRUE(requestData.retval);
270+
EXPECT_TRUE(requestData.result);
271+
EXPECT_TRUE(requestData.response.data());
272+
}
229273

230274
EXPECT_EQ(entityCount + 2, ecm->EntityCount());
231275
entityCount = ecm->EntityCount();
@@ -239,12 +283,16 @@ TEST_F(UserCommandsTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(Create))
239283
req.Clear();
240284
req.mutable_light()->set_name("light_test");
241285
req.mutable_light()->set_parent_id(1);
242-
EXPECT_TRUE(node.Request(service, req, timeout, res, result));
243-
EXPECT_TRUE(result);
244-
EXPECT_TRUE(res.data());
286+
requestDataFuture = asyncRequest(node, service, req);
245287

246288
// Run an iteration and check it was created
247289
server.Run(true, 1, false);
290+
{
291+
auto requestData = requestDataFuture.get();
292+
EXPECT_TRUE(requestData.retval);
293+
EXPECT_TRUE(requestData.result);
294+
EXPECT_TRUE(requestData.response.data());
295+
}
248296

249297
EXPECT_EQ(entityCount + 2, ecm->EntityCount());
250298
entityCount = ecm->EntityCount();
@@ -259,17 +307,13 @@ TEST_F(UserCommandsTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(Create))
259307
req.set_sdf(modelStr);
260308
req.set_name("acerola");
261309

262-
EXPECT_TRUE(node.Request(service, req, timeout, res, result));
263-
EXPECT_TRUE(result);
264-
EXPECT_TRUE(res.data());
310+
auto requestDataFuture1 = asyncRequest(node, service, req);
265311

266312
req.Clear();
267313
req.set_sdf(modelStr);
268314
req.set_name("coconut");
269315

270-
EXPECT_TRUE(node.Request(service, req, timeout, res, result));
271-
EXPECT_TRUE(result);
272-
EXPECT_TRUE(res.data());
316+
auto requestDataFuture2 = asyncRequest(node, service, req);
273317

274318
// Check neither exists yet
275319
EXPECT_EQ(kNullEntity, ecm->EntityByComponents(components::Model(),
@@ -280,6 +324,18 @@ TEST_F(UserCommandsTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(Create))
280324

281325
// Run an iteration and check both models were created
282326
server.Run(true, 1, false);
327+
{
328+
auto requestData = requestDataFuture1.get();
329+
EXPECT_TRUE(requestData.retval);
330+
EXPECT_TRUE(requestData.result);
331+
EXPECT_TRUE(requestData.response.data());
332+
}
333+
{
334+
auto requestData = requestDataFuture2.get();
335+
EXPECT_TRUE(requestData.retval);
336+
EXPECT_TRUE(requestData.result);
337+
EXPECT_TRUE(requestData.response.data());
338+
}
283339

284340
EXPECT_EQ(entityCount + 8, ecm->EntityCount());
285341
entityCount = ecm->EntityCount();
@@ -293,12 +349,16 @@ TEST_F(UserCommandsTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(Create))
293349
req.Clear();
294350
req.set_sdf(lightsStr);
295351

296-
EXPECT_TRUE(node.Request(service, req, timeout, res, result));
297-
EXPECT_TRUE(result);
298-
EXPECT_TRUE(res.data());
352+
requestDataFuture = asyncRequest(node, service, req);
299353

300354
// Run an iteration and check only the 1st was created
301355
server.Run(true, 1, false);
356+
{
357+
auto requestData = requestDataFuture.get();
358+
EXPECT_TRUE(requestData.retval);
359+
EXPECT_TRUE(requestData.result);
360+
EXPECT_TRUE(requestData.response.data());
361+
}
302362

303363
EXPECT_EQ(entityCount + 2, ecm->EntityCount());
304364
entityCount = ecm->EntityCount();
@@ -315,12 +375,16 @@ TEST_F(UserCommandsTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(Create))
315375
req.Clear();
316376
req.set_sdf(badStr);
317377

318-
EXPECT_TRUE(node.Request(service, req, timeout, res, result));
319-
EXPECT_TRUE(result);
320-
EXPECT_TRUE(res.data());
378+
requestDataFuture = asyncRequest(node, service, req);
321379

322380
// Run an iteration and check nothing was created
323381
server.Run(true, 1, false);
382+
{
383+
auto requestData = requestDataFuture.get();
384+
EXPECT_TRUE(requestData.retval);
385+
EXPECT_TRUE(requestData.result);
386+
EXPECT_FALSE(requestData.response.data());
387+
}
324388

325389
EXPECT_EQ(entityCount, ecm->EntityCount());
326390

@@ -330,12 +394,16 @@ TEST_F(UserCommandsTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(Create))
330394
req.Clear();
331395
req.set_sdf_filename(testModel);
332396

333-
EXPECT_TRUE(node.Request(service, req, timeout, res, result));
334-
EXPECT_TRUE(result);
335-
EXPECT_TRUE(res.data());
397+
requestDataFuture = asyncRequest(node, service, req);
336398

337399
// Run an iteration and check it was created
338400
server.Run(true, 1, false);
401+
{
402+
auto requestData = requestDataFuture.get();
403+
EXPECT_TRUE(requestData.retval);
404+
EXPECT_TRUE(requestData.result);
405+
EXPECT_TRUE(requestData.response.data());
406+
}
339407
EXPECT_EQ(entityCount + 4, ecm->EntityCount());
340408

341409
EXPECT_NE(kNullEntity, ecm->EntityByComponents(components::Model(),

0 commit comments

Comments
 (0)