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> &));
75 MOCK_METHOD0(getDeclaredClasses, std::vector<std::string>(
void));
76 MOCK_METHOD0(reload,
void(
void));
83 void initializeState()
88 MOCK_METHOD1(starting,
void(
const ros::Time &));
90 MOCK_METHOD1(stopping,
void(
const ros::Time &));
91 MOCK_METHOD1(waiting,
void(
const ros::Time &));
92 MOCK_METHOD1(aborting,
void(
const ros::Time &));
109 hw_mock_.reset(
new StrictMock<RobotHWMock>);
112 ctrl_loader_mock_ =
new StrictMock<ControllerLoaderMock>();
113 ctrl_loader_mock_ptr_.reset(ctrl_loader_mock_);
116 ctrl_1_mock_ =
new StrictMock<ControllerMock>();
117 ctrl_2_mock_ =
new StrictMock<ControllerMock>();
118 ctrl_1_mock_ptr_.reset(ctrl_1_mock_);
119 ctrl_2_mock_ptr_.reset(ctrl_2_mock_);
128 cm_->registerControllerLoader(ctrl_loader_mock_ptr_);
130 const std::vector<std::string> types = {
"ControllerMock" };
131 EXPECT_CALL(*ctrl_loader_mock_, getDeclaredClasses()).Times(2).WillRepeatedly(Return(types));
134 .WillOnce(Return(ctrl_1_mock_ptr_))
135 .WillOnce(Return(ctrl_2_mock_ptr_));
137 EXPECT_CALL(*ctrl_1_mock_, initRequest(_, _, _, _))
141 EXPECT_CALL(*ctrl_2_mock_, initRequest(_, _, _, _))
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);
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);
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);
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);
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();
ROS Controller Manager and Runner.
MOCK_CONST_METHOD0(switchResult, SwitchState(void))
std::unique_ptr< StrictMock< RobotHWMock > > hw_mock_
std::shared_ptr< ControllerBase > ControllerBaseSharedPtr
virtual void doSwitch(const std::list< ControllerInfo > &, const std::list< ControllerInfo > &)
Timer createTimer(Rate r, Handler h, Obj o, bool oneshot=false, bool autostart=true) const
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
TEST_F(ControllerManagerTest, NoSwitchTest)
virtual bool checkForConflict(const std::list< ControllerInfo > &info) const
virtual void read(const ros::Time &, const ros::Duration &)
void update(std::shared_ptr< controller_manager::ControllerManager > cm, const ros::TimerEvent &e)
MOCK_METHOD2(init, bool(ros::NodeHandle &, ros::NodeHandle &))
controller_manager::ControllerLoaderInterfaceSharedPtr ctrl_loader_mock_ptr_
std::shared_ptr< ControllerLoaderInterface > ControllerLoaderInterfaceSharedPtr
virtual bool prepareSwitch(const std::list< ControllerInfo > &, const std::list< ControllerInfo > &)
virtual void write(const ros::Time &, const ros::Duration &)
StrictMock< ControllerMock > * ctrl_2_mock_
std::shared_ptr< controller_manager::ControllerManager > cm_
controller_interface::ControllerBaseSharedPtr ctrl_2_mock_ptr_
StrictMock< ControllerLoaderMock > * ctrl_loader_mock_
Base * createInstance(const std::string &derived_class_name, ClassLoader *loader)
int main(int argc, char **argv)
MOCK_CONST_METHOD1(checkForConflict, bool(const std::list< hardware_interface::ControllerInfo > &))
Abstract Controller Loader Interface.
StrictMock< ControllerMock > * ctrl_1_mock_
virtual SwitchState switchResult() const
controller_interface::ControllerBaseSharedPtr ctrl_1_mock_ptr_
virtual bool init(ros::NodeHandle &, ros::NodeHandle &)