hwi_update_test.cpp
Go to the documentation of this file.
1 // Copyright (C) 2019, Pal Robotics S.L.
3 //
4 // Redistribution and use in source and binary forms, with or without
5 // modification, are permitted provided that the following conditions are met:
6 // * Redistributions of source code must retain the above copyright notice,
7 // this list of conditions and the following disclaimer.
8 // * Redistributions in binary form must reproduce the above copyright
9 // notice, this list of conditions and the following disclaimer in the
10 // documentation and/or other materials provided with the distribution.
11 // * Neither the name of Pal Robotics S.L. nor the names of its
12 // contributors may be used to endorse or promote products derived from
13 // this software without specific prior written permission.
14 //
15 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
16 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
17 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
18 // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
19 // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
20 // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
21 // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
22 // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
23 // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
24 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
25 // POSSIBILITY OF SUCH DAMAGE.
27 
29 
32 #include <controller_manager_msgs/SwitchController.h>
35 
36 #include <ros/ros.h>
37 
38 #include <gmock/gmock.h>
39 
40 #include <memory>
41 #include <functional>
42 
43 using ::testing::StrictMock;
44 using ::testing::_;
45 using ::testing::AtLeast;
46 using ::testing::AnyNumber;
47 using ::testing::DoAll;
48 using ::testing::InvokeWithoutArgs;
49 using ::testing::Return;
50 
52 {
53 public:
54 
57  bool(const std::list<hardware_interface::ControllerInfo> &));
58  MOCK_METHOD2(prepareSwitch, bool(const std::list<hardware_interface::ControllerInfo> &,
59  const std::list<hardware_interface::ControllerInfo> &));
60  MOCK_METHOD2(doSwitch, void(const std::list<hardware_interface::ControllerInfo> &,
61  const std::list<hardware_interface::ControllerInfo> &));
64  MOCK_METHOD2(read, void(const ros::Time &time, const ros::Duration &period));
65  MOCK_METHOD2(write, void(const ros::Time &time, const ros::Duration &period));
66 };
67 
69 {
70 public:
71  ControllerLoaderMock() : ControllerLoaderInterface("ControllerLoaderMock") {}
72 
73  MOCK_METHOD1(createInstance,
75  MOCK_METHOD0(getDeclaredClasses, std::vector<std::string>(void));
76  MOCK_METHOD0(reload, void(void));
77 };
78 
80 {
81 public:
82 
83  void initializeState()
84  {
85  state_ = INITIALIZED;
86  }
87 
88  MOCK_METHOD1(starting, void(const ros::Time &));
89  MOCK_METHOD2(update, void(const ros::Time &, const ros::Duration &));
90  MOCK_METHOD1(stopping, void(const ros::Time &));
91  MOCK_METHOD1(waiting, void(const ros::Time &));
92  MOCK_METHOD1(aborting, void(const ros::Time &));
93  MOCK_METHOD4(initRequest, bool(hardware_interface::RobotHW *, ros::NodeHandle &,
94  ros::NodeHandle &, ClaimedResources &));
95 };
96 
97 class ControllerManagerTest : public ::testing::Test
98 {
99 public:
100  void SetUp() override
101  {
102  prepareMocks();
103  loadControllers();
104  }
105 
107  {
108  // prepare robot hw mock
109  hw_mock_.reset(new StrictMock<RobotHWMock>);
110 
111  // prepare controller loader mock
112  ctrl_loader_mock_ = new StrictMock<ControllerLoaderMock>();
113  ctrl_loader_mock_ptr_.reset(ctrl_loader_mock_);
114 
115  // prepare controllers mocks
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_);
120 
121  // create controller manager
122  cm_.reset(new controller_manager::ControllerManager(hw_mock_.get()));
123  }
124 
126  {
127  // register mock controller loader
128  cm_->registerControllerLoader(ctrl_loader_mock_ptr_);
129 
130  const std::vector<std::string> types = { "ControllerMock" };
131  EXPECT_CALL(*ctrl_loader_mock_, getDeclaredClasses()).Times(2).WillRepeatedly(Return(types));
132  EXPECT_CALL(*ctrl_loader_mock_, createInstance("ControllerMock"))
133  .Times(2)
134  .WillOnce(Return(ctrl_1_mock_ptr_))
135  .WillOnce(Return(ctrl_2_mock_ptr_));
136 
137  EXPECT_CALL(*ctrl_1_mock_, initRequest(_, _, _, _))
138  .Times(1)
139  .WillOnce(DoAll(InvokeWithoutArgs(ctrl_1_mock_, &ControllerMock::initializeState),
140  Return(true)));
141  EXPECT_CALL(*ctrl_2_mock_, initRequest(_, _, _, _))
142  .Times(1)
143  .WillOnce(DoAll(InvokeWithoutArgs(ctrl_2_mock_, &ControllerMock::initializeState),
144  Return(true)));
145 
146  // load mock controllers
147  ASSERT_TRUE(cm_->loadController("mock_ctrl_1"));
148  ASSERT_TRUE(cm_->loadController("mock_ctrl_2"));
149  }
150 
151  // robot hw mock
152  std::unique_ptr<StrictMock<RobotHWMock>> hw_mock_;
153 
154  // controller loader mock
155  StrictMock<ControllerLoaderMock> *ctrl_loader_mock_;
157 
158  // controllers mocks
159  StrictMock<ControllerMock> *ctrl_1_mock_;
160  StrictMock<ControllerMock> *ctrl_2_mock_;
163 
164  // controller manager
165  std::shared_ptr<controller_manager::ControllerManager> cm_;
166 };
167 
168 void update(std::shared_ptr<controller_manager::ControllerManager> cm, const ros::TimerEvent &e)
169 {
170  cm->update(e.current_real, e.current_real - e.last_real);
171 }
172 
174 {
175  const ros::Duration period(1.0);
176 
177  EXPECT_CALL(*hw_mock_, doSwitch(_, _)).Times(0);
178 
179  cm_->update(ros::Time::now(), period);
180 }
181 
183 {
184  // only way to trigger switch is through switchController(...) which in turn waits
185  // for update(...) to finish the switch, hence the update on a timer
186  ros::NodeHandle node_handle;
187  ros::Timer timer = node_handle.createTimer(ros::Duration(0.01),
188  std::bind(update, cm_, std::placeholders::_1));
189 
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);
193  EXPECT_CALL(*hw_mock_, switchResult()).Times(1).WillOnce(Return(RobotHWMock::DONE));
194 
195  // switch with no controllers at all
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));
199 }
200 
201 TEST_F(ControllerManagerTest, SwitchControllersTest)
202 {
203  // only way to trigger switch is through switchController(...) which in turn waits for
204  // update(...) to finish the switch, hence the update on a timer
205  ros::NodeHandle node_handle;
206  ros::Timer timer = node_handle.createTimer(ros::Duration(0.01),
207  std::bind(update, cm_, std::placeholders::_1));
208 
209  // one call for each controller
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);
213  EXPECT_CALL(*hw_mock_, switchResult()).Times(2).WillRepeatedly(Return(RobotHWMock::DONE));
214 
215  const int strictness = controller_manager_msgs::SwitchController::Request::STRICT;
216  std::vector<std::string> start_controllers, stop_controllers;
217 
218  // called at least once, otherwise can't perform second switch
219  EXPECT_CALL(*ctrl_1_mock_, update(_, _)).Times(AtLeast(1));
220  // this may or may not be called depending on the timing of the update timer
221  EXPECT_CALL(*ctrl_2_mock_, update(_, _)).Times(AnyNumber());
222 
223  // start controller
224  EXPECT_CALL(*ctrl_1_mock_, starting(_)).Times(1);
225 
226  start_controllers = { "mock_ctrl_1" };
227  ASSERT_TRUE(cm_->switchController(start_controllers, stop_controllers, strictness));
228  ASSERT_TRUE(ctrl_1_mock_->isRunning());
229 
230  // stop and start controllers
231  EXPECT_CALL(*ctrl_1_mock_, stopping(_)).Times(1);
232  EXPECT_CALL(*ctrl_2_mock_, starting(_)).Times(1);
233 
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());
239 }
240 
241 TEST_F(ControllerManagerTest, SwitchControllersAbortTest)
242 {
243  // only way to trigger switch is through switchController(...) which in turn waits for
244  // update(...) to finish the switch, hence the update on a timer
245  ros::NodeHandle node_handle;
246  ros::Timer timer = node_handle.createTimer(ros::Duration(0.01),
247  std::bind(update, cm_, std::placeholders::_1));
248 
249  // one call for each controller
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);
253  EXPECT_CALL(*hw_mock_, switchResult()).Times(2).WillRepeatedly(Return(RobotHWMock::ERROR));
254 
255  const int strictness = controller_manager_msgs::SwitchController::Request::STRICT;
256  std::vector<std::string> start_controllers, stop_controllers;
257 
258  // controllers are aborted before they can update
259  EXPECT_CALL(*ctrl_1_mock_, update(_, _)).Times(0);
260  EXPECT_CALL(*ctrl_2_mock_, update(_, _)).Times(0);
261 
262  // abort controller
263  EXPECT_CALL(*ctrl_1_mock_, aborting(_)).Times(1);
264  EXPECT_CALL(*ctrl_2_mock_, aborting(_)).Times(1);
265 
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());
270 }
271 
272 TEST_F(ControllerManagerTest, SwitchControllersTimeoutTest)
273 {
274  // only way to trigger switch is through switchController(...) which in turn waits for
275  // update(...) to finish the switch, hence the update on a timer
276  ros::NodeHandle node_handle;
277  ros::Timer timer = node_handle.createTimer(ros::Duration(0.01),
278  std::bind(update, cm_, std::placeholders::_1));
279 
280  // one call for each controller
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);
284  EXPECT_CALL(*hw_mock_, switchResult()).WillRepeatedly(Return(RobotHWMock::ONGOING));
285 
286  const int strictness = controller_manager_msgs::SwitchController::Request::STRICT;
287  std::vector<std::string> start_controllers, stop_controllers;
288 
289  // controllers are never started
290  EXPECT_CALL(*ctrl_1_mock_, update(_, _)).Times(0);
291  EXPECT_CALL(*ctrl_2_mock_, update(_, _)).Times(0);
292 
293  // called while hardware interfaces are ONGOING
294  EXPECT_CALL(*ctrl_1_mock_, waiting(_)).Times(AtLeast(1));
295  EXPECT_CALL(*ctrl_2_mock_, waiting(_)).Times(AtLeast(1));
296 
297  // abort controller
298  EXPECT_CALL(*ctrl_1_mock_, aborting(_)).Times(1);
299  EXPECT_CALL(*ctrl_2_mock_, aborting(_)).Times(1);
300 
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());
305 }
306 
307 TEST_F(ControllerManagerTest, ControllerUpdatesTest)
308 {
309  // only way to trigger switch is through switchController(...) which in turn waits for
310  // update(...) to finish the switch, hence the update on a timer
311  ros::NodeHandle node_handle;
312  ros::Timer timer = node_handle.createTimer(ros::Duration(0.01),
313  std::bind(update, cm_, std::placeholders::_1));
314 
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())
319  .Times(11)
320  .WillOnce(Return(RobotHWMock::ONGOING))
321  .WillOnce(Return(RobotHWMock::ONGOING))
322  .WillOnce(Return(RobotHWMock::ONGOING))
323  .WillOnce(Return(RobotHWMock::ONGOING))
324  .WillOnce(Return(RobotHWMock::ONGOING))
325  .WillOnce(Return(RobotHWMock::ONGOING))
326  .WillOnce(Return(RobotHWMock::ONGOING))
327  .WillOnce(Return(RobotHWMock::ONGOING))
328  .WillOnce(Return(RobotHWMock::ONGOING))
329  .WillOnce(Return(RobotHWMock::ONGOING))
330  .WillOnce(Return(RobotHWMock::DONE));
331 
332  const int strictness = controller_manager_msgs::SwitchController::Request::STRICT;
333  std::vector<std::string> start_controllers, stop_controllers;
334 
335  // controller started
336  EXPECT_CALL(*ctrl_1_mock_, update(_, _)).Times(AtLeast(5));
337  // called 5 times, once for each ONGOING
338  EXPECT_CALL(*ctrl_1_mock_, waiting(_)).Times(5);
339  // controller not started
340  EXPECT_CALL(*ctrl_2_mock_, update(_, _)).Times(0);
341 
342  // start controller
343  EXPECT_CALL(*ctrl_1_mock_, starting(_)).Times(1);
344 
345  start_controllers = { "mock_ctrl_1" };
346  ASSERT_TRUE(cm_->switchController(start_controllers, stop_controllers, strictness));
347  ASSERT_TRUE(ctrl_1_mock_->isRunning());
348 
349  timer.stop();
350 
351  const ros::Duration period(1.0);
352  for (unsigned int i = 0; i < 5; ++i)
353  {
354  cm_->update(ros::Time::now(), period);
355  }
356 }
357 
358 TEST_F(ControllerManagerTest, SwitchControllersAsapTest)
359 {
360  // only way to trigger switch is through switchController(...) which in turn waits for
361  // update(...) to finish the switch, hence the update on a timer
362  ros::NodeHandle node_handle;
363  ros::Timer timer = node_handle.createTimer(ros::Duration(0.01),
364  std::bind(update, cm_, std::placeholders::_1));
365 
366  // one call for each controller
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(_))
372  .Times(12)
373  .WillOnce(Return(RobotHWMock::DONE))
374  .WillOnce(Return(RobotHWMock::ONGOING))
375  .WillOnce(Return(RobotHWMock::ONGOING))
376  .WillOnce(Return(RobotHWMock::ONGOING))
377  .WillOnce(Return(RobotHWMock::ONGOING))
378  .WillOnce(Return(RobotHWMock::ONGOING))
379  .WillOnce(Return(RobotHWMock::ONGOING))
380  .WillOnce(Return(RobotHWMock::ONGOING))
381  .WillOnce(Return(RobotHWMock::ONGOING))
382  .WillOnce(Return(RobotHWMock::ONGOING))
383  .WillOnce(Return(RobotHWMock::ONGOING))
384  .WillOnce(Return(RobotHWMock::DONE));
385 
386  const int strictness = controller_manager_msgs::SwitchController::Request::STRICT;
387  std::vector<std::string> start_controllers, stop_controllers;
388 
389  // called at least 5 times, once for each ONGOING
390  EXPECT_CALL(*ctrl_1_mock_, update(_, _)).Times(AtLeast(5));
391  // this may or may not be called depending on the timing of the update timer
392  EXPECT_CALL(*ctrl_2_mock_, update(_, _)).Times(AnyNumber());
393 
394  // both controllers are started
395  EXPECT_CALL(*ctrl_1_mock_, starting(_)).Times(1);
396  EXPECT_CALL(*ctrl_2_mock_, starting(_)).Times(1);
397  // called 5 times, once for each ONGOING
398  EXPECT_CALL(*ctrl_2_mock_, waiting(_)).Times(5);
399 
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());
404 }
405 
406 TEST_F(ControllerManagerTest, SwitchControllersAsapAbortTest)
407 {
408  // only way to trigger switch is through switchController(...) which in turn waits for
409  // update(...) to finish the switch, hence the update on a timer
410  ros::NodeHandle node_handle;
411  ros::Timer timer = node_handle.createTimer(ros::Duration(0.01),
412  std::bind(update, cm_, std::placeholders::_1));
413 
414  // one call for each controller
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(_))
420  .Times(13)
421  .WillOnce(Return(RobotHWMock::DONE))
422  .WillOnce(Return(RobotHWMock::ONGOING))
423  .WillOnce(Return(RobotHWMock::ONGOING))
424  .WillOnce(Return(RobotHWMock::ONGOING))
425  .WillOnce(Return(RobotHWMock::ONGOING))
426  .WillOnce(Return(RobotHWMock::ONGOING))
427  .WillOnce(Return(RobotHWMock::ONGOING))
428  .WillOnce(Return(RobotHWMock::ONGOING))
429  .WillOnce(Return(RobotHWMock::ONGOING))
430  .WillOnce(Return(RobotHWMock::ONGOING))
431  .WillOnce(Return(RobotHWMock::ONGOING))
432  .WillOnce(Return(RobotHWMock::ERROR))
433  .WillOnce(Return(RobotHWMock::ERROR));
434 
435  const int strictness = controller_manager_msgs::SwitchController::Request::STRICT;
436  std::vector<std::string> start_controllers, stop_controllers;
437 
438  // called at least 5 times, once for each ONGOING
439  EXPECT_CALL(*ctrl_1_mock_, update(_, _)).Times(AtLeast(5));
440  // this may or may not be called depending on the timing of the update timer
441  EXPECT_CALL(*ctrl_2_mock_, update(_, _)).Times(AnyNumber());
442 
443  // one controller manages to start, the other is aborted
444  EXPECT_CALL(*ctrl_1_mock_, starting(_)).Times(1);
445  EXPECT_CALL(*ctrl_2_mock_, aborting(_)).Times(1);
446  // called 5 times, once for each ONGOING
447  EXPECT_CALL(*ctrl_2_mock_, waiting(_)).Times(5);
448 
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());
453 }
454 
455 TEST_F(ControllerManagerTest, SwitchControllersAsapTimeoutTest)
456 {
457  // only way to trigger switch is through switchController(...) which in turn waits for
458  // update(...) to finish the switch, hence the update on a timer
459  ros::NodeHandle node_handle;
460  ros::Timer timer = node_handle.createTimer(ros::Duration(0.01),
461  std::bind(update, cm_, std::placeholders::_1));
462 
463  // one call for each controller
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(_))
469  .WillOnce(Return(RobotHWMock::DONE))
470  .WillRepeatedly(Return(RobotHWMock::ONGOING));
471 
472  const int strictness = controller_manager_msgs::SwitchController::Request::STRICT;
473  std::vector<std::string> start_controllers, stop_controllers;
474 
475  EXPECT_CALL(*ctrl_1_mock_, update(_, _)).Times(AtLeast(1));
476  // this may or may not be called depending on the timing of the update timer
477  EXPECT_CALL(*ctrl_2_mock_, update(_, _)).Times(AnyNumber());
478 
479  // one controller manages to start, the other is aborted
480  EXPECT_CALL(*ctrl_1_mock_, starting(_)).Times(1);
481  EXPECT_CALL(*ctrl_2_mock_, aborting(_)).Times(1);
482 
483  EXPECT_CALL(*ctrl_2_mock_, waiting(_)).Times(AtLeast(1));
484 
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());
489 }
490 
491 int main(int argc, char **argv)
492 {
493  testing::InitGoogleTest(&argc, argv);
494  ros::init(argc, argv, "controller_manager_update_test");
495 
496  ros::AsyncSpinner spinner(1);
497  spinner.start();
498  int ret = RUN_ALL_TESTS();
499  return ret;
500 }
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
void stop()
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
TEST_F(ControllerManagerTest, NoSwitchTest)
void initializeState()
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)
static Time now()
int main(int argc, char **argv)
MOCK_CONST_METHOD1(checkForConflict, bool(const std::list< hardware_interface::ControllerInfo > &))
StrictMock< ControllerMock > * ctrl_1_mock_
virtual SwitchState switchResult() const
controller_interface::ControllerBaseSharedPtr ctrl_1_mock_ptr_
virtual bool init(ros::NodeHandle &, ros::NodeHandle &)


controller_manager
Author(s): Wim Meeussen, Mathias Lüdtke
autogenerated on Mon Feb 28 2022 23:30:17