32 #include <controller_manager_msgs/SwitchController.h>
38 #include <gmock/gmock.h>
43 using ::testing::StrictMock;
45 using ::testing::AtLeast;
46 using ::testing::AnyNumber;
47 using ::testing::DoAll;
48 using ::testing::InvokeWithoutArgs;
49 using ::testing::Return;
57 bool(
const std::list<hardware_interface::ControllerInfo> &));
59 const std::list<hardware_interface::ControllerInfo> &));
61 const std::list<hardware_interface::ControllerInfo> &));
109 hw_mock_.reset(
new StrictMock<RobotHWMock>);
130 const std::vector<std::string> types = {
"ControllerMock" };
131 EXPECT_CALL(*
ctrl_loader_mock_, getDeclaredClasses()).Times(2).WillRepeatedly(Return(types));
147 ASSERT_TRUE(
cm_->loadController(
"mock_ctrl_1"));
148 ASSERT_TRUE(
cm_->loadController(
"mock_ctrl_2"));
165 std::shared_ptr<controller_manager::ControllerManager>
cm_;
177 EXPECT_CALL(*hw_mock_, doSwitch(_, _)).Times(0);
188 std::bind(
update, cm_, std::placeholders::_1));
190 EXPECT_CALL(*hw_mock_, checkForConflict(_)).Times(1).WillOnce(Return(
false));
191 EXPECT_CALL(*hw_mock_, prepareSwitch(_, _)).Times(1).WillOnce(Return(
true));
192 EXPECT_CALL(*hw_mock_, doSwitch(_, _)).Times(1);
196 const std::vector<std::string> start_controllers, stop_controllers;
197 const int strictness = controller_manager_msgs::SwitchController::Request::STRICT;
198 ASSERT_TRUE(cm_->switchController(start_controllers, stop_controllers, strictness));
207 std::bind(
update, cm_, std::placeholders::_1));
210 EXPECT_CALL(*hw_mock_, checkForConflict(_)).Times(2).WillRepeatedly(Return(
false));
211 EXPECT_CALL(*hw_mock_, prepareSwitch(_, _)).Times(2).WillRepeatedly(Return(
true));
212 EXPECT_CALL(*hw_mock_, doSwitch(_, _)).Times(2);
215 const int strictness = controller_manager_msgs::SwitchController::Request::STRICT;
216 std::vector<std::string> start_controllers, stop_controllers;
219 EXPECT_CALL(*ctrl_1_mock_,
update(_, _)).Times(AtLeast(1));
221 EXPECT_CALL(*ctrl_2_mock_,
update(_, _)).Times(AnyNumber());
224 EXPECT_CALL(*ctrl_1_mock_, starting(_)).Times(1);
226 start_controllers = {
"mock_ctrl_1" };
227 ASSERT_TRUE(cm_->switchController(start_controllers, stop_controllers, strictness));
228 ASSERT_TRUE(ctrl_1_mock_->isRunning());
231 EXPECT_CALL(*ctrl_1_mock_, stopping(_)).Times(1);
232 EXPECT_CALL(*ctrl_2_mock_, starting(_)).Times(1);
234 start_controllers = {
"mock_ctrl_2" };
235 stop_controllers = {
"mock_ctrl_1" };
236 ASSERT_TRUE(cm_->switchController(start_controllers, stop_controllers, strictness));
237 ASSERT_TRUE(ctrl_1_mock_->isStopped());
238 ASSERT_TRUE(ctrl_2_mock_->isRunning());
247 std::bind(
update, cm_, std::placeholders::_1));
250 EXPECT_CALL(*hw_mock_, checkForConflict(_)).Times(1).WillOnce(Return(
false));
251 EXPECT_CALL(*hw_mock_, prepareSwitch(_, _)).Times(1).WillOnce(Return(
true));
252 EXPECT_CALL(*hw_mock_, doSwitch(_, _)).Times(1);
255 const int strictness = controller_manager_msgs::SwitchController::Request::STRICT;
256 std::vector<std::string> start_controllers, stop_controllers;
259 EXPECT_CALL(*ctrl_1_mock_,
update(_, _)).Times(0);
260 EXPECT_CALL(*ctrl_2_mock_,
update(_, _)).Times(0);
263 EXPECT_CALL(*ctrl_1_mock_, aborting(_)).Times(1);
264 EXPECT_CALL(*ctrl_2_mock_, aborting(_)).Times(1);
266 start_controllers = {
"mock_ctrl_1",
"mock_ctrl_2" };
267 ASSERT_TRUE(cm_->switchController(start_controllers, stop_controllers, strictness));
268 ASSERT_TRUE(ctrl_1_mock_->isAborted());
269 ASSERT_TRUE(ctrl_2_mock_->isAborted());
278 std::bind(
update, cm_, std::placeholders::_1));
281 EXPECT_CALL(*hw_mock_, checkForConflict(_)).Times(1).WillOnce(Return(
false));
282 EXPECT_CALL(*hw_mock_, prepareSwitch(_, _)).Times(1).WillOnce(Return(
true));
283 EXPECT_CALL(*hw_mock_, doSwitch(_, _)).Times(1);
286 const int strictness = controller_manager_msgs::SwitchController::Request::STRICT;
287 std::vector<std::string> start_controllers, stop_controllers;
290 EXPECT_CALL(*ctrl_1_mock_,
update(_, _)).Times(0);
291 EXPECT_CALL(*ctrl_2_mock_,
update(_, _)).Times(0);
294 EXPECT_CALL(*ctrl_1_mock_, waiting(_)).Times(AtLeast(1));
295 EXPECT_CALL(*ctrl_2_mock_, waiting(_)).Times(AtLeast(1));
298 EXPECT_CALL(*ctrl_1_mock_, aborting(_)).Times(1);
299 EXPECT_CALL(*ctrl_2_mock_, aborting(_)).Times(1);
301 start_controllers = {
"mock_ctrl_1",
"mock_ctrl_2" };
302 ASSERT_TRUE(cm_->switchController(start_controllers, stop_controllers, strictness,
false, 1.0));
303 ASSERT_TRUE(ctrl_1_mock_->isAborted());
304 ASSERT_TRUE(ctrl_2_mock_->isAborted());
313 std::bind(
update, cm_, std::placeholders::_1));
315 EXPECT_CALL(*hw_mock_, checkForConflict(_)).Times(1).WillOnce(Return(
false));
316 EXPECT_CALL(*hw_mock_, prepareSwitch(_, _)).Times(1).WillOnce(Return(
true));
317 EXPECT_CALL(*hw_mock_, doSwitch(_, _)).Times(1);
318 EXPECT_CALL(*hw_mock_, switchResult())
332 const int strictness = controller_manager_msgs::SwitchController::Request::STRICT;
333 std::vector<std::string> start_controllers, stop_controllers;
336 EXPECT_CALL(*ctrl_1_mock_,
update(_, _)).Times(AtLeast(5));
338 EXPECT_CALL(*ctrl_1_mock_, waiting(_)).Times(5);
340 EXPECT_CALL(*ctrl_2_mock_,
update(_, _)).Times(0);
343 EXPECT_CALL(*ctrl_1_mock_, starting(_)).Times(1);
345 start_controllers = {
"mock_ctrl_1" };
346 ASSERT_TRUE(cm_->switchController(start_controllers, stop_controllers, strictness));
347 ASSERT_TRUE(ctrl_1_mock_->isRunning());
352 for (
unsigned int i = 0; i < 5; ++i)
364 std::bind(
update, cm_, std::placeholders::_1));
367 EXPECT_CALL(*hw_mock_, checkForConflict(_)).Times(1).WillRepeatedly(Return(
false));
368 EXPECT_CALL(*hw_mock_, prepareSwitch(_, _)).Times(1).WillRepeatedly(Return(
true));
369 EXPECT_CALL(*hw_mock_, doSwitch(_, _)).Times(1);
370 EXPECT_CALL(*hw_mock_, switchResult()).Times(0);
371 EXPECT_CALL(*hw_mock_, switchResult(_))
386 const int strictness = controller_manager_msgs::SwitchController::Request::STRICT;
387 std::vector<std::string> start_controllers, stop_controllers;
390 EXPECT_CALL(*ctrl_1_mock_,
update(_, _)).Times(AtLeast(5));
392 EXPECT_CALL(*ctrl_2_mock_,
update(_, _)).Times(AnyNumber());
395 EXPECT_CALL(*ctrl_1_mock_, starting(_)).Times(1);
396 EXPECT_CALL(*ctrl_2_mock_, starting(_)).Times(1);
398 EXPECT_CALL(*ctrl_2_mock_, waiting(_)).Times(5);
400 start_controllers = {
"mock_ctrl_1",
"mock_ctrl_2" };
401 ASSERT_TRUE(cm_->switchController(start_controllers, stop_controllers, strictness,
true));
402 ASSERT_TRUE(ctrl_1_mock_->isRunning());
403 ASSERT_TRUE(ctrl_2_mock_->isRunning());
412 std::bind(
update, cm_, std::placeholders::_1));
415 EXPECT_CALL(*hw_mock_, checkForConflict(_)).Times(1).WillRepeatedly(Return(
false));
416 EXPECT_CALL(*hw_mock_, prepareSwitch(_, _)).Times(1).WillRepeatedly(Return(
true));
417 EXPECT_CALL(*hw_mock_, doSwitch(_, _)).Times(1);
418 EXPECT_CALL(*hw_mock_, switchResult()).Times(0);
419 EXPECT_CALL(*hw_mock_, switchResult(_))
435 const int strictness = controller_manager_msgs::SwitchController::Request::STRICT;
436 std::vector<std::string> start_controllers, stop_controllers;
439 EXPECT_CALL(*ctrl_1_mock_,
update(_, _)).Times(AtLeast(5));
441 EXPECT_CALL(*ctrl_2_mock_,
update(_, _)).Times(AnyNumber());
444 EXPECT_CALL(*ctrl_1_mock_, starting(_)).Times(1);
445 EXPECT_CALL(*ctrl_2_mock_, aborting(_)).Times(1);
447 EXPECT_CALL(*ctrl_2_mock_, waiting(_)).Times(5);
449 start_controllers = {
"mock_ctrl_1",
"mock_ctrl_2" };
450 ASSERT_TRUE(cm_->switchController(start_controllers, stop_controllers, strictness,
true));
451 ASSERT_TRUE(ctrl_1_mock_->isRunning());
452 ASSERT_TRUE(ctrl_2_mock_->isAborted());
461 std::bind(
update, cm_, std::placeholders::_1));
464 EXPECT_CALL(*hw_mock_, checkForConflict(_)).Times(1).WillRepeatedly(Return(
false));
465 EXPECT_CALL(*hw_mock_, prepareSwitch(_, _)).Times(1).WillRepeatedly(Return(
true));
466 EXPECT_CALL(*hw_mock_, doSwitch(_, _)).Times(1);
467 EXPECT_CALL(*hw_mock_, switchResult()).Times(0);
468 EXPECT_CALL(*hw_mock_, switchResult(_))
472 const int strictness = controller_manager_msgs::SwitchController::Request::STRICT;
473 std::vector<std::string> start_controllers, stop_controllers;
475 EXPECT_CALL(*ctrl_1_mock_,
update(_, _)).Times(AtLeast(1));
477 EXPECT_CALL(*ctrl_2_mock_,
update(_, _)).Times(AnyNumber());
480 EXPECT_CALL(*ctrl_1_mock_, starting(_)).Times(1);
481 EXPECT_CALL(*ctrl_2_mock_, aborting(_)).Times(1);
483 EXPECT_CALL(*ctrl_2_mock_, waiting(_)).Times(AtLeast(1));
485 start_controllers = {
"mock_ctrl_1",
"mock_ctrl_2" };
486 ASSERT_TRUE(cm_->switchController(start_controllers, stop_controllers, strictness,
true, 1.0));
487 ASSERT_TRUE(ctrl_1_mock_->isRunning());
488 ASSERT_TRUE(ctrl_2_mock_->isAborted());
491 int main(
int argc,
char **argv)
493 testing::InitGoogleTest(&argc, argv);
494 ros::init(argc, argv,
"controller_manager_update_test");
498 int ret = RUN_ALL_TESTS();