20 #include <gmock/gmock.h> 21 #include <gtest/gtest.h> 25 #include <std_srvs/SetBool.h> 27 #include <pilz_msgs/GetSpeedOverride.h> 28 #include <pilz_msgs/OperationModes.h> 38 using ::testing::DoAll;
40 using ::testing::Return;
41 using ::testing::SetArgReferee;
43 using pilz_msgs::OperationModes;
48 void SetUp()
override;
49 void TearDown()
override;
51 MOCK_METHOD1(monitorCartesianSpeed,
bool(
const bool));
54 std::unique_ptr<OperationModeSetupExecutor>
executor_;
61 OperationModes op_mode;
63 op_mode.value = OperationModes::UNKNOWN;
65 ON_CALL(*
this, monitorCartesianSpeed(_)).WillByDefault(Return(
true));
68 std::bind(&OperationModeSetupExecutorTest::monitorCartesianSpeed,
this, std::placeholders::_1)));
94 OperationModes op_mode;
96 op_mode.value = OperationModes::T1;
98 EXPECT_CALL(*
this, monitorCartesianSpeed(
true)).WillOnce(Return(
true));
100 executor_->updateOperationMode(op_mode);
105 OperationModes op_mode2;
107 op_mode2.value = OperationModes::AUTO;
109 EXPECT_CALL(*
this, monitorCartesianSpeed(
false)).WillOnce(Return(
true));
111 executor_->updateOperationMode(op_mode2);
132 OperationModes op_mode;
134 op_mode.value = OperationModes::T1;
136 EXPECT_CALL(*
this, monitorCartesianSpeed(
true)).WillOnce(Return(
true));
138 executor_->updateOperationMode(op_mode);
143 OperationModes op_mode2;
145 op_mode2.value = op_mode.value;
147 EXPECT_CALL(*
this, monitorCartesianSpeed(
true)).WillRepeatedly(Return(
true));
149 executor_->updateOperationMode(op_mode2);
170 OperationModes op_mode;
172 op_mode.value = OperationModes::T1;
174 EXPECT_CALL(*
this, monitorCartesianSpeed(
true)).WillOnce(Return(
true));
176 executor_->updateOperationMode(op_mode);
181 OperationModes op_mode2;
182 op_mode2.time_stamp = op_mode.time_stamp;
183 op_mode2.value = OperationModes::AUTO;
185 EXPECT_CALL(*
this, monitorCartesianSpeed(_)).Times(0);
187 executor_->updateOperationMode(op_mode2);
206 OperationModes op_mode;
208 op_mode.value = OperationModes::UNKNOWN;
210 EXPECT_CALL(*
this, monitorCartesianSpeed(_)).Times(0);
212 executor_->updateOperationMode(op_mode);
218 MOCK_METHOD1(
call,
bool(std_srvs::SetBool& srv));
219 MOCK_METHOD0(getService, std::string());
222 MATCHER_P(IsSpeedMonitoringSettingCorrect, active_flag,
"")
224 return arg.request.data == active_flag;
233 const bool exp_active_flag{
false };
234 std_srvs::SetBool exp_srv;
235 exp_srv.request.data = exp_active_flag;
238 EXPECT_CALL(mock, getService()).WillRepeatedly(Return(
"TestServiceName"));
239 EXPECT_CALL(mock,
call(IsSpeedMonitoringSettingCorrect(exp_active_flag))).Times(1).WillOnce(Return(
true));
241 EXPECT_TRUE(monitorCartesianSpeedSrv<MonitorCartesianSpeedServiceMock>(mock, exp_active_flag));
250 EXPECT_CALL(mock, getService()).WillRepeatedly(Return(
"TestServiceName"));
251 EXPECT_CALL(mock,
call(_)).Times(1).WillOnce(Return(
false));
253 const bool exp_active_flag{
false };
254 EXPECT_FALSE(monitorCartesianSpeedSrv<MonitorCartesianSpeedServiceMock>(mock, exp_active_flag));
259 public ::testing::WithParamInterface<std::pair<pilz_msgs::OperationModes::_value_type, double>>
263 return GetParam().first;
268 return GetParam().second;
273 typedef ::testing::NiceMock<OperationModeSetupExecutorTestSpeedOverride>
287 OperationModes op_mode;
289 op_mode.value = GetParam().first;
291 executor_->updateOperationMode(op_mode);
293 auto req = pilz_msgs::GetSpeedOverrideRequest();
294 auto res = pilz_msgs::GetSpeedOverrideResponse();
295 executor_->getSpeedOverride(req, res);
296 EXPECT_EQ(res.speed_override, GetParam().second);
300 ::testing::Values(std::pair<OperationModes::_value_type, double>(OperationModes::UNKNOWN, 0.0),
301 std::pair<OperationModes::_value_type, double>(OperationModes::T1, 0.1),
302 std::pair<OperationModes::_value_type, double>(OperationModes::AUTO, 1.0)), );
306 int main(
int argc,
char* argv[])
308 testing::InitGoogleTest(&argc, argv);
309 return RUN_ALL_TESTS();
double getSpeedOverride()
bool call(const std::string &service_name, MReq &req, MRes &res)
OperationModes::_value_type getMode()
INSTANTIATE_TEST_CASE_P(SpeedOverrideModeTests, OperationModeSetupExecutorTestSpeedOverrideNice, ::testing::Values(std::pair< OperationModes::_value_type, double >(OperationModes::UNKNOWN, 0.0), std::pair< OperationModes::_value_type, double >(OperationModes::T1, 0.1), std::pair< OperationModes::_value_type, double >(OperationModes::AUTO, 1.0)),)
Activates speed monitoring and sets the speed override based on the current operation mode...
std::unique_ptr< OperationModeSetupExecutor > executor_
int main(int argc, char *argv[])
TEST_P(OperationModeSetupExecutorTestSpeedOverrideNice, testSpeedOverride)
MATCHER_P(IsSpeedMonitoringSettingCorrect, active_flag, "")
TEST_F(OperationModeSetupExecutorTest, testUpdateOperationMode)
::testing::NiceMock< OperationModeSetupExecutorTestSpeedOverride > OperationModeSetupExecutorTestSpeedOverrideNice