21 #include <gtest/gtest.h> 22 #include <gmock/gmock.h> 24 #include <std_srvs/Trigger.h> 27 #include <prbt_hardware_support/BrakeTestErrorCodes.h> 37 using std::placeholders::_1;
42 MOCK_METHOD0(detectMotion,
bool());
43 MOCK_METHOD0(holdController,
void());
44 MOCK_METHOD0(executeBrakeTest, BrakeTest::Response());
45 MOCK_METHOD0(unholdController,
void());
46 MOCK_METHOD1(sendBrakeTestResult,
bool(
const bool brake_test_result));
64 TEST(BrakeTestExecutorTest, testBrakeTestTriggeringRobotNotMoving)
67 BrakeTestExecutor brake_test_executor(std::bind(&SystemMock::detectMotion, &mock),
68 std::bind(&SystemMock::holdController, &mock),
69 std::bind(&SystemMock::executeBrakeTest, &mock),
70 std::bind(&SystemMock::unholdController, &mock),
71 std::bind(&SystemMock::sendBrakeTestResult, &mock, _1));
76 EXPECT_CALL(mock, detectMotion()).Times(1).WillOnce(Return(
false));
77 EXPECT_CALL(mock, holdController()).Times(1);
78 EXPECT_CALL(mock, executeBrakeTest())
80 .WillOnce(testing::Invoke(
82 BrakeTest::Response res;
86 EXPECT_CALL(mock, unholdController()).Times(1);
87 EXPECT_CALL(mock, sendBrakeTestResult(_)).Times(1).WillOnce(Return(
true));
90 BrakeTest brake_test_srv;
91 EXPECT_TRUE(brake_test_executor.
executeBrakeTest(brake_test_srv.request, brake_test_srv.response)) <<
"Failed to call brake test service.";
92 EXPECT_TRUE(brake_test_srv.response.success) <<
"Brake tests failed unexpectedly. Message: " << brake_test_srv.response.error_msg;
107 TEST(BrakeTestExecutorTest, testBrakeTestServiceWithRobotMotion)
110 BrakeTestExecutor brake_test_executor(std::bind(&SystemMock::detectMotion, &mock),
111 std::bind(&SystemMock::holdController, &mock),
112 std::bind(&SystemMock::executeBrakeTest, &mock),
113 std::bind(&SystemMock::unholdController, &mock),
114 std::bind(&SystemMock::sendBrakeTestResult, &mock, _1));
116 EXPECT_CALL(mock, detectMotion()).Times(1).WillOnce(Return(
true));
117 EXPECT_CALL(mock, holdController()).Times(0);
118 EXPECT_CALL(mock, executeBrakeTest()).Times(0);
119 EXPECT_CALL(mock, unholdController()).Times(0);
120 EXPECT_CALL(mock, sendBrakeTestResult(_)).Times(0);
123 BrakeTest brake_test_srv;
124 EXPECT_TRUE(brake_test_executor.
executeBrakeTest(brake_test_srv.request, brake_test_srv.response));
125 EXPECT_FALSE(brake_test_srv.response.success) <<
"Brake tests was unexpectedly successful";
126 EXPECT_EQ(BrakeTestErrorCodes::ROBOT_MOTION_DETECTED, brake_test_srv.response.error_code.value);
144 TEST(BrakeTestExecutorTest, testBrakeTestServiceTriggerFails)
147 BrakeTestExecutor brake_test_executor(std::bind(&SystemMock::detectMotion, &mock),
148 std::bind(&SystemMock::holdController, &mock),
149 std::bind(&SystemMock::executeBrakeTest, &mock),
150 std::bind(&SystemMock::unholdController, &mock),
151 std::bind(&SystemMock::sendBrakeTestResult, &mock, _1));
156 EXPECT_CALL(mock, detectMotion()).Times(1).WillOnce(Return(
false));
157 EXPECT_CALL(mock, holdController()).Times(1);
158 EXPECT_CALL(mock, executeBrakeTest())
160 .WillOnce(testing::Invoke([]()
162 BrakeTest::Response res;
164 res.error_msg =
"Test error message";
165 res.error_code.value = BrakeTestErrorCodes::FAILURE;
168 EXPECT_CALL(mock, unholdController()).Times(1);
169 EXPECT_CALL(mock, sendBrakeTestResult(_)).Times(1).WillOnce(Return(
true));
172 BrakeTest brake_test_srv;
173 EXPECT_TRUE(brake_test_executor.
executeBrakeTest(brake_test_srv.request, brake_test_srv.response));
174 EXPECT_FALSE(brake_test_srv.response.success);
188 TEST(BrakeTestExecutorTest, testBrakeTestResultServiceFails)
191 BrakeTestExecutor brake_test_executor(std::bind(&SystemMock::detectMotion, &mock),
192 std::bind(&SystemMock::holdController, &mock),
193 std::bind(&SystemMock::executeBrakeTest, &mock),
194 std::bind(&SystemMock::unholdController, &mock),
195 std::bind(&SystemMock::sendBrakeTestResult, &mock, _1));
200 EXPECT_CALL(mock, detectMotion()).Times(1).WillOnce(Return(
false));
201 EXPECT_CALL(mock, holdController()).Times(1);
202 EXPECT_CALL(mock, executeBrakeTest())
204 .WillOnce(testing::Invoke([]()
206 BrakeTest::Response res;
210 EXPECT_CALL(mock, unholdController()).Times(1);
211 EXPECT_CALL(mock, sendBrakeTestResult(_)).Times(1).WillOnce(Return(
false));
214 BrakeTest brake_test_srv;
215 EXPECT_TRUE(brake_test_executor.
executeBrakeTest(brake_test_srv.request, brake_test_srv.response));
216 EXPECT_FALSE(brake_test_srv.response.success);
217 EXPECT_EQ(brake_test_srv.response.error_code.value, BrakeTestErrorCodes::FAILURE);
224 TEST(BrakeTestExecutorTest, testMissingHoldFunc)
229 std::bind(&SystemMock::executeBrakeTest, &mock),
230 std::bind(&SystemMock::unholdController, &mock),
231 std::bind(&SystemMock::sendBrakeTestResult, &mock, _1)),
239 TEST(BrakeTestExecutorTest, testMissingUnholdFunc)
243 std::bind(&SystemMock::holdController, &mock),
244 std::bind(&SystemMock::executeBrakeTest, &mock),
246 std::bind(&SystemMock::sendBrakeTestResult, &mock, _1)),
254 TEST(BrakeTestExecutorTest, testMissingDetectMotionFunc)
258 std::bind(&SystemMock::holdController, &mock),
259 std::bind(&SystemMock::executeBrakeTest, &mock),
260 std::bind(&SystemMock::unholdController, &mock),
261 std::bind(&SystemMock::sendBrakeTestResult, &mock, _1)),
269 TEST(BrakeTestExecutorTest, testMissingExecuteBrakeTestFunc)
273 std::bind(&SystemMock::holdController, &mock),
275 std::bind(&SystemMock::unholdController, &mock),
276 std::bind(&SystemMock::sendBrakeTestResult, &mock, _1)),
284 TEST(BrakeTestExecutorTest, testMissingSendBrakeTestResultFunc)
288 std::bind(&SystemMock::holdController, &mock),
289 std::bind(&SystemMock::executeBrakeTest, &mock),
290 std::bind(&SystemMock::unholdController, &mock),
299 TEST(BrakeTestExecutorTest, testDtorBrakeTestExecutorException)
307 MOCK_METHOD1(
call,
bool(std_srvs::Trigger& srv));
308 MOCK_METHOD0(getService, std::string());
314 TEST(BrakeTestExecutorTest, testTriggerServiceCallFailure)
317 EXPECT_CALL(mock, getService()).WillRepeatedly(Return(
"TestServiceName"));
318 EXPECT_CALL(mock,
call(_)).Times(1).WillOnce(Return(
false));
320 triggerServiceCall<TriggerServiceMock>(mock);
327 TEST(BrakeTestExecutorTest, testTriggerServiceCallResponseFalse)
330 EXPECT_CALL(mock, getService()).WillRepeatedly(Return(
"TestServiceName"));
331 std_srvs::Trigger exp_srv;
332 exp_srv.response.success =
false;
333 EXPECT_CALL(mock,
call(_)).Times(1).WillOnce(DoAll(SetArgReferee<0>(exp_srv), Return(
true)));
335 triggerServiceCall<TriggerServiceMock>(mock);
341 MOCK_METHOD1(
call,
bool(BrakeTest& srv));
342 MOCK_METHOD0(getService, std::string());
350 TEST(BrakeTestExecutorTest, testExecuteBrakeTestCallFailure)
353 EXPECT_CALL(mock, getService()).WillRepeatedly(Return(
"TestServiceName"));
354 EXPECT_CALL(mock,
call(_)).Times(1).WillOnce(Return(
false));
356 BrakeTest::Response res {executeBrakeTestCall<BrakeTestServiceMock>(mock)};
357 EXPECT_FALSE(res.success);
358 EXPECT_EQ(res.error_code.value, BrakeTestErrorCodes::TRIGGER_BRAKETEST_SERVICE_FAILURE);
364 MOCK_METHOD1(
call,
bool(SendBrakeTestResult& srv));
365 MOCK_METHOD0(getService, std::string());
371 TEST(BrakeTestExecutorTest, testSendBrakeTestResultCallFailure)
374 EXPECT_CALL(mock, getService()).WillRepeatedly(Return(
"TestServiceName"));
375 EXPECT_CALL(mock,
call(_)).Times(1).WillOnce(Return(
false));
377 EXPECT_FALSE(sendBrakeTestResultCall<SendBrakeTestResltServiceMock>(mock,
true));
380 MATCHER(IsRequestResultFalse,
"") {
return !arg.request.result; }
385 TEST(BrakeTestExecutorTest, testSendBrakeTestResultCallSuccess)
388 EXPECT_CALL(mock, getService()).WillRepeatedly(Return(
"TestServiceName"));
389 EXPECT_CALL(mock,
call(IsRequestResultFalse())).Times(1).WillOnce(Return(
true));
391 EXPECT_TRUE(sendBrakeTestResultCall<SendBrakeTestResltServiceMock>(mock,
false));
396 int main(
int argc,
char *argv[])
398 testing::InitGoogleMock(&argc, argv);
399 return RUN_ALL_TESTS();
bool call(const std::string &service_name, MReq &req, MRes &res)
bool executeBrakeTest(BrakeTest::Request &req, BrakeTest::Response &response)
MATCHER(IsRequestResultFalse,"")
Triggers execution of brake tests only if the controller is not executing a trajectory.
TEST(BrakeTestExecutorTest, testBrakeTestTriggeringRobotNotMoving)
int main(int argc, char *argv[])