RobotUnitModuleBase.cpp
Go to the documentation of this file.
1 /*
2  * This file is part of ArmarX.
3  *
4  * ArmarX is free software; you can redistribute it and/or modify
5  * it under the terms of the GNU General Public License version 2 as
6  * published by the Free Software Foundation.
7  *
8  * ArmarX is distributed in the hope that it will be useful, but
9  * WITHOUT ANY WARRANTY; without even the implied warranty of
10  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
11  * GNU General Public License for more details.
12  *
13  * You should have received a copy of the GNU General Public License
14  * along with this program. If not, see <http://www.gnu.org/licenses/>.
15  *
16  * @package RobotAPI::ArmarXObjects::RobotUnit
17  * @author Raphael Grimm ( raphael dot grimm at kit dot edu )
18  * @date 2018
19  * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
20  * GNU General Public License
21  */
22 
23 #include "RobotUnitModuleBase.h"
24 
25 #include <sstream>
26 
30 
31 #include "RobotUnitModules.h"
32 
33 namespace armarx
34 {
35  std::string
37  {
38  switch (s)
39  {
41  return "RobotUnitState::InitializingComponent";
43  return "RobotUnitState::InitializingDevices";
45  return "RobotUnitState::InitializingUnits";
47  return "RobotUnitState::InitializingControlThread";
49  return "RobotUnitState::Running";
51  return "RobotUnitState::Exiting";
52  }
53  throw std::invalid_argument{"Unknown state " + to_string(static_cast<std::size_t>(s)) +
54  "\nStacktrace\n" + LogSender::CreateBackTrace()};
55  }
56 } // namespace armarx
57 
59 {
60  // //////////////////////////////////////////////////////////////////////////// //
61  // /////////////////////////// call for each module /////////////////////////// //
62  // //////////////////////////////////////////////////////////////////////////// //
63 #define cast_to_and_call(Type, fn, rethrow) \
64  ARMARX_TRACE; \
65  try \
66  { \
67  dynamic_cast<Type*>(this)->Type::fn; \
68  } \
69  catch (Ice::Exception & e) \
70  { \
71  ARMARX_ERROR << "exception in " << #Type "::" #fn << "!\nwhat:\n" \
72  << e.what() << "\n\tid : " << e.ice_id() << "\n\tfile : " << e.ice_file() \
73  << "\n\tline : " << e.ice_line() << "\n\tstack: " << e.ice_stackTrace(); \
74  if (rethrow) \
75  { \
76  throw; \
77  } \
78  } \
79  catch (std::exception & e) \
80  { \
81  ARMARX_ERROR << "exception in " << #Type "::" #fn << "!\nwhat:\n" << e.what(); \
82  if (rethrow) \
83  { \
84  throw; \
85  } \
86  } \
87  catch (...) \
88  { \
89  ARMARX_ERROR << "exception in " << #Type "::" #fn << "!"; \
90  if (rethrow) \
91  { \
92  throw; \
93  } \
94  }
95 
96 #define for_each_module_apply(r, data, elem) data(elem)
97 #define for_each_module(macro) \
98  BOOST_PP_SEQ_FOR_EACH(for_each_module_apply, macro, BOOST_PP_VARIADIC_TO_SEQ(RobotUnitModules))
99 
100 #define check_base(Type) \
101  static_assert(std::is_base_of<ModuleBase, Type>::value, \
102  "The RobotUnitModule '" #Type "' has to derived ModuleBase");
104 #undef check_base
105 
107  {
108 #define check_deriving(Type) \
109  ARMARX_CHECK_NOT_NULL(dynamic_cast<const Type*>(this)) \
110  << "This class does not derive from " << GetTypeString<Type>();
112 #undef check_deriving
113  }
114 
115  // //////////////////////////////////////////////////////////////////////////// //
116  // ManagedIceObject life cycle
117  void
119  {
120  ARMARX_TRACE;
122  auto guard = getGuard();
124  ARMARX_IMPORTANT << "Starting initialization of RobotUnitModule '" << getName();
125  cast_to_and_call(::armarx::RobotUnitModule::RobotData, _initVirtualRobot(), true);
126 
127 #define call_module_hook(Type) \
128  cast_to_and_call(::armarx::RobotUnitModule::Type, _preOnInitRobotUnit(), true)
130 #undef call_module_hook
131 
132  onInitRobotUnit();
133 
134 #define call_module_hook(Type) \
135  cast_to_and_call(::armarx::RobotUnitModule::Type, _postOnInitRobotUnit(), true)
137 #undef call_module_hook
138 
140  }
141 
142  void
144  {
145  ARMARX_TRACE;
147 
148 #define call_module_hook(Type) \
149  cast_to_and_call(::armarx::RobotUnitModule::Type, _preOnConnectRobotUnit(), true)
151 #undef call_module_hook
152 
154 
155 #define call_module_hook(Type) \
156  cast_to_and_call(::armarx::RobotUnitModule::Type, _postOnConnectRobotUnit(), true)
158 #undef call_module_hook
159  }
160 
161  void
163  {
164  ARMARX_TRACE;
166 
167 #define call_module_hook(Type) \
168  cast_to_and_call(::armarx::RobotUnitModule::Type, _preOnDisconnectRobotUnit(), true)
170 #undef call_module_hook
171 
173 
174 #define call_module_hook(Type) \
175  cast_to_and_call(::armarx::RobotUnitModule::Type, _postOnDisconnectRobotUnit(), true)
177 #undef call_module_hook
178  }
179 
180  void
182  {
183  ARMARX_TRACE;
185  auto guard = getGuard();
186 
187  finishRunning();
188 
189 #define call_module_hook(Type) \
190  cast_to_and_call(::armarx::RobotUnitModule::Type, _preOnExitRobotUnit(), false)
192 #undef call_module_hook
193 
194  onExitRobotUnit();
195 
196 #define call_module_hook(Type) \
197  cast_to_and_call(::armarx::RobotUnitModule::Type, _postOnExitRobotUnit(), false)
199 #undef call_module_hook
200  }
201 
202  // //////////////////////////////////////////////////////////////////////////// //
203  // other ManagedIceObject / Component methods
204  void
205  ModuleBase::componentPropertiesUpdated(const std::set<std::string>& changedProperties)
206  {
207 #define call_module_hook(Type) \
208  cast_to_and_call( \
209  ::armarx::RobotUnitModule::Type, _componentPropertiesUpdated(changedProperties), true)
211 #undef call_module_hook
212  }
213 
214  void
216  {
217 #define call_module_hook(Type) \
218  cast_to_and_call(::armarx::RobotUnitModule::Type, _icePropertiesInitialized(), true)
220 #undef call_module_hook
221  }
222 
223  // //////////////////////////////////////////////////////////////////////////// //
224  // RobotUnit life cycle
225  void
227  {
228  ARMARX_TRACE;
230  auto guard = getGuard();
232 
233 #define call_module_hook(Type) \
234  cast_to_and_call(::armarx::RobotUnitModule::Type, _preFinishComponentInitialization(), true)
236 #undef call_module_hook
237 
238  setRobotUnitState(RobotUnitState::InitializingDevices);
239 
240 #define call_module_hook(Type) \
241  cast_to_and_call(::armarx::RobotUnitModule::Type, _postFinishComponentInitialization(), true)
243 #undef call_module_hook
244  }
245 
246  void
248  {
249  ARMARX_TRACE;
251  auto guard = getGuard();
253 
254 #define call_module_hook(Type) \
255  cast_to_and_call(::armarx::RobotUnitModule::Type, _preFinishDeviceInitialization(), true)
257 #undef call_module_hook
258 
259  setRobotUnitState(RobotUnitState::InitializingUnits);
260 
261 #define call_module_hook(Type) \
262  cast_to_and_call(::armarx::RobotUnitModule::Type, _postFinishDeviceInitialization(), true)
264 #undef call_module_hook
265  }
266 
267  void
269  {
270  ARMARX_TRACE;
272  auto guard = getGuard();
274 
275 #define call_module_hook(Type) \
276  cast_to_and_call(::armarx::RobotUnitModule::Type, _preFinishUnitInitialization(), true)
278 #undef call_module_hook
279 
281 
282 #define call_module_hook(Type) \
283  cast_to_and_call(::armarx::RobotUnitModule::Type, _postFinishUnitInitialization(), true)
285 #undef call_module_hook
286  }
287 
288  void
290  {
291  ARMARX_TRACE;
293  auto guard = getGuard();
295 
296 #define call_module_hook(Type) \
297  cast_to_and_call(::armarx::RobotUnitModule::Type, _preFinishControlThreadInitialization(), true)
299 #undef call_module_hook
300 
301  setRobotUnitState(RobotUnitState::Running);
302 
303 #define call_module_hook(Type) \
304  cast_to_and_call( \
305  ::armarx::RobotUnitModule::Type, _postFinishControlThreadInitialization(), true)
307 #undef call_module_hook
308  }
309 
310  void
312  {
313  ARMARX_TRACE;
315  shutDown();
316  GuardType guard{dataMutex}; //DO NOT USE getGuard here!
318  {
319  ARMARX_ERROR << "called finishRunning when state was "
321  }
322 
323 #define call_module_hook(Type) \
324  cast_to_and_call(::armarx::RobotUnitModule::Type, _preFinishRunning(), false)
326 #undef call_module_hook
327 
328  setRobotUnitState(RobotUnitState::Exiting);
330 
331 #define call_module_hook(Type) \
332  cast_to_and_call(::armarx::RobotUnitModule::Type, _postFinishRunning(), false)
334 #undef call_module_hook
335  }
336 
337 #undef for_each_module_apply
338 #undef for_each_module
339 #undef cast_to_and_call
340  // //////////////////////////////////////////////////////////////////////////// //
341  // ////////////////////////////////// other /////////////////////////////////// //
342  // //////////////////////////////////////////////////////////////////////////// //
343 
344  const std::set<RobotUnitState> ModuleBase::DevicesReadyStates{
348 
349  void
350  ModuleBase::setRobotUnitState(RobotUnitState to)
351  {
352  ARMARX_TRACE;
353  auto guard = getGuard();
354 
355  const auto transitionErrorMessage = [=, this](RobotUnitState expectedFrom)
356  {
357  return "Can't switch to state " + to_string(to) + " from " + to_string(state) +
358  "! The expected source state is " + to_string(expectedFrom) + ".";
359  };
360  const auto transitionErrorThrow = [=, this](RobotUnitState expectedFrom)
361  {
362  if (state != expectedFrom)
363  {
364  const auto msg = transitionErrorMessage(expectedFrom);
365  ARMARX_ERROR << msg;
366  throw std::invalid_argument{msg};
367  }
368  };
369 
370  switch (to)
371  {
373  throw std::invalid_argument{
374  "Can't switch to state RobotUnitState::InitializingComponent"};
376  transitionErrorThrow(RobotUnitState::InitializingComponent);
377  break;
379  transitionErrorThrow(RobotUnitState::InitializingDevices);
380  break;
382  transitionErrorThrow(RobotUnitState::InitializingUnits);
383  break;
385  transitionErrorThrow(RobotUnitState::InitializingControlThread);
386  break;
388  if (state != RobotUnitState::Running)
389  {
390  ARMARX_ERROR << transitionErrorMessage(RobotUnitState::Running);
391  }
392  break;
393  default:
394  throw std::invalid_argument{"setRobotUnitState: Unknown target state " +
395  to_string(static_cast<std::size_t>(to)) +
396  "\nStacktrace\n" + LogSender::CreateBackTrace()};
397  }
398  ARMARX_INFO << "switching state from " << to_string(state) << " to " << to_string(to);
399  state = to;
400  }
401 
402  bool
404  {
405  return DevicesReadyStates.count(state);
406  }
407 
408  bool
410  {
412  std::this_thread::get_id() == _module<ControlThread>().getControlThreadId();
413  }
414 
415  void
416  ModuleBase::throwIfInControlThread(const std::string& fnc) const
417  {
418  ARMARX_TRACE;
419  if (inControlThread())
420  {
421  std::stringstream str;
422  str << "Called function '" << fnc << "' in the Control Thread\nStack trace:\n"
424  ARMARX_ERROR << str.str();
425  throw LogicError{str.str()};
426  }
427  }
428 
431  {
432  ARMARX_TRACE;
433  if (inControlThread())
434  {
435  throw LogicError{"Attempted to lock mutex in Control Thread\nStack trace:\n" +
437  }
438  GuardType guard{dataMutex, std::defer_lock};
439  if (guard.try_lock())
440  {
441  return guard;
442  }
443  while (!guard.try_lock_for(std::chrono::microseconds{100}))
444  {
445  if (isShuttingDown())
446  {
447  throw LogicError{"Attempting to lock mutex during shutdown\nStacktrace\n" +
449  }
450  }
451 
452  return guard;
453  }
454 
455  void
456  ModuleBase::throwIfStateIsNot(const std::set<RobotUnitState>& stateSet,
457  const std::string& fnc,
458  bool onlyWarn) const
459  {
460  ARMARX_TRACE;
461  if (!stateSet.count(state))
462  {
463  std::stringstream ss;
464  ss << fnc << ": Can't be called if state is not in {";
465  bool fst = true;
466  for (RobotUnitState st : stateSet)
467  {
468  if (!fst)
469  {
470  ss << ", ";
471  }
472  ss << st;
473  fst = false;
474  }
475  ss << "} (current state " << getRobotUnitState() << ")\n"
476  << "Stacktrace:\n"
478  ARMARX_ERROR << ss.str();
479  if (!onlyWarn)
480  {
481  throw LogicError{ss.str()};
482  }
483  }
484  }
485 
486  void
487  ModuleBase::throwIfStateIsNot(RobotUnitState s, const std::string& fnc, bool onlyWarn) const
488  {
489  throwIfStateIsNot(std::set<RobotUnitState>{s}, fnc, onlyWarn);
490  }
491 
492  void
493  ModuleBase::throwIfStateIs(const std::set<RobotUnitState>& stateSet,
494  const std::string& fnc,
495  bool onlyWarn) const
496  {
497  ARMARX_TRACE;
498  if (stateSet.count(state))
499  {
500  std::stringstream ss;
501  ss << fnc << ": Can't be called if state is in {";
502  bool fst = true;
503  for (RobotUnitState st : stateSet)
504  {
505  if (!fst)
506  {
507  ss << ", ";
508  }
509  ss << st;
510  fst = false;
511  }
512  ss << "} (current state " << getRobotUnitState() << ")\n"
513  << "Stacktrace:\n"
515  ARMARX_ERROR << ss.str();
516  if (!onlyWarn)
517  {
518  throw LogicError{ss.str()};
519  }
520  }
521  }
522 
523  void
524  ModuleBase::throwIfStateIs(RobotUnitState s, const std::string& fnc, bool onlyWarn) const
525  {
526  throwIfStateIs(std::set<RobotUnitState>{s}, fnc, onlyWarn);
527  }
528 
529  void
530  ModuleBase::throwIfDevicesNotReady(const std::string& fnc) const
531  {
532  throwIfStateIsNot(DevicesReadyStates, fnc);
533  }
534 
535  std::atomic<ModuleBase*> ModuleBase::Instance_{nullptr};
536 
538  {
539  ARMARX_TRACE;
540  if (Instance_ && this != Instance_)
541  {
542  ARMARX_FATAL << "This class is a Singleton. It was already instantiated. (Instance = "
543  << Instance_ << ", this = " << this << ")";
544  std::terminate();
545  }
546  Instance_ = this;
547  }
548 
549 } // namespace armarx::RobotUnitModule
armarx::RobotUnitModule::ModuleBase::isShuttingDown
bool isShuttingDown() const
Returns whether the RobotUnit is shutting down.
Definition: RobotUnitModuleBase.h:647
armarx::RobotUnitModule::ModuleBase::onConnectRobotUnit
virtual void onConnectRobotUnit()
called in onConnectComponent
Definition: RobotUnitModuleBase.h:681
str
std::string str(const T &t)
Definition: UserAssistedSegmenterGuiWidgetController.cpp:42
armarx::RobotUnitModule::ModuleBase::onExitComponent
void onExitComponent() final override
Definition: RobotUnitModuleBase.cpp:181
ARMARX_IMPORTANT
#define ARMARX_IMPORTANT
Definition: Logging.h:183
RobotUnitModules.h
RobotUnitModuleBase.h
trace.h
armarx::RobotUnitModule::ModuleBase::throwIfStateIs
void throwIfStateIs(const std::set< RobotUnitState > &stateSet, const std::string &fnc, bool onlyWarn=false) const
Throws an exception if the current state is in.
Definition: RobotUnitModuleBase.cpp:493
call_module_hook
#define call_module_hook(Type)
cast_to_and_call
#define cast_to_and_call(Type, fn, rethrow)
Definition: RobotUnitModuleBase.cpp:63
armarx::RobotUnitModule::ModuleBase::getRobotUnitState
RobotUnitState getRobotUnitState() const
Returns the RobotUnit's State.
Definition: RobotUnitModuleBase.ipp:29
armarx::RobotUnitModule::ModuleBase::onInitRobotUnit
virtual void onInitRobotUnit()
called in onInitComponent
Definition: RobotUnitModuleBase.h:671
ARMARX_FATAL
#define ARMARX_FATAL
Definition: Logging.h:192
armarx::RobotUnitModule::ModuleBase::checkDerivedClasses
void checkDerivedClasses() const
Checks whether the implementing class derives all modules.
armarx::RobotUnitModule::ModuleBase::finishUnitInitialization
virtual void finishUnitInitialization()
Transition RobotUnitState::InitializingUnits -> RobotUnitState::WaitingForRTThreadInitialization.
Definition: RobotUnitModuleBase.cpp:268
armarx::RobotUnitState::InitializingUnits
@ InitializingUnits
armarx::RobotUnitModule::for_each_module
for_each_module(check_base) void ModuleBase
Definition: RobotUnitModuleBase.cpp:103
armarx::RobotUnitModule::ModuleBase::ModuleBase
ModuleBase()
Ctor.
Definition: RobotUnitModuleBase.cpp:537
ARMARX_TRACE
#define ARMARX_TRACE
Definition: trace.h:69
armarx::RobotUnitModule::ModuleBase::inControlThread
bool inControlThread() const
Returns whether the current thread is the ControlThread.
Definition: RobotUnitModuleBase.cpp:409
armarx::RobotUnitModule::ModuleBase::shutDown
void shutDown()
Requests the RobotUnit to shut down.
Definition: RobotUnitModuleBase.h:657
armarx::RobotUnitState::Running
@ Running
armarx::RobotUnitModule::ModuleBase::onDisconnectComponent
void onDisconnectComponent() final override
Definition: RobotUnitModuleBase.cpp:162
armarx::RobotUnitModule::ModuleBase::componentPropertiesUpdated
void componentPropertiesUpdated(const std::set< std::string > &changedProperties) override
Definition: RobotUnitModuleBase.cpp:205
boost::process::posix::terminate
void terminate(const Process &p)
Definition: terminate.hpp:20
armarx::RobotUnitModule::ModuleBase::finishControlThreadInitialization
virtual void finishControlThreadInitialization()
Transition RobotUnitState::InitializingControlThread -> RobotUnitState::Running.
Definition: RobotUnitModuleBase.cpp:289
armarx::RobotUnitModule::ModuleBase::getGuard
GuardType getGuard() const
Returns a guard to the RobotUnits mutex.
Definition: RobotUnitModuleBase.cpp:430
armarx::LogSender::CreateBackTrace
static std::string CreateBackTrace(int linesToSkip=1)
Definition: LogSender.cpp:508
check_base
#define check_base(Type)
Definition: RobotUnitModuleBase.cpp:100
armarx::RobotUnitModule::ModuleBase::onExitRobotUnit
virtual void onExitRobotUnit()
called in onExitComponent before calling finishRunning
Definition: RobotUnitModuleBase.h:702
armarx::RobotUnitModule::ModuleBase::onDisconnectRobotUnit
virtual void onDisconnectRobotUnit()
called in onDisconnecComponent
Definition: RobotUnitModuleBase.h:691
ARMARX_ERROR
#define ARMARX_ERROR
Definition: Logging.h:189
armarx::RobotUnitModule::ModuleBase::finishRunning
virtual void finishRunning()
Transition RobotUnitState::Running -> RobotUnitState::Exiting.
Definition: RobotUnitModuleBase.cpp:311
armarx::to_string
const std::string & to_string(const std::string &s)
Definition: StringHelpers.h:40
armarx::RobotUnitModule::ModuleBase::joinControlThread
virtual void joinControlThread()=0
Implementations have to join their ControlThread in this hook. (used by RobotUnit::finishRunning())
armarx::RobotUnitState::InitializingControlThread
@ InitializingControlThread
ExpressionException.h
check_deriving
#define check_deriving(Type)
Preprocessor.h
ARMARX_INFO
#define ARMARX_INFO
Definition: Logging.h:174
armarx::RobotUnitModule::ModuleBase::areDevicesReady
bool areDevicesReady() const
Returns whether Devices are ready.
Definition: RobotUnitModuleBase.cpp:403
armarx::RobotUnitState::InitializingDevices
@ InitializingDevices
armarx::RobotUnitModule::ModuleBase::icePropertiesInitialized
void icePropertiesInitialized() override
Definition: RobotUnitModuleBase.cpp:215
armarx::RobotUnitModule::ModuleBase::GuardType
std::unique_lock< MutexType > GuardType
Definition: RobotUnitModuleBase.h:226
armarx::RobotUnitState::InitializingComponent
@ InitializingComponent
armarx::RobotUnitModule::ModuleBase::throwIfInControlThread
void throwIfInControlThread(const std::string &fnc) const
Throws if the current thread is the ControlThread.
Definition: RobotUnitModuleBase.cpp:416
armarx::RobotUnitState
RobotUnitState
The current state of the multi step initialization of a RobotUnit.
Definition: RobotUnitModuleBase.h:74
armarx::RobotUnitModule::ModuleBase::throwIfDevicesNotReady
void throwIfDevicesNotReady(const std::string &fnc) const
Throws if the Devices are not ready.
Definition: RobotUnitModuleBase.cpp:530
armarx::ManagedIceObject::getName
std::string getName() const
Retrieve name of object.
Definition: ManagedIceObject.cpp:107
armarx::RobotUnitModule::ModuleBase::throwIfStateIsNot
void throwIfStateIsNot(const std::set< RobotUnitState > &stateSet, const std::string &fnc, bool onlyWarn=false) const
Throws an exception if the current state is not in.
Definition: RobotUnitModuleBase.cpp:456
armarx::RobotUnitModule
Definition: ControlDevice.h:34
armarx::RobotUnitModule::ModuleBase::finishComponentInitialization
virtual void finishComponentInitialization()
Transition RobotUnitState::InitializingComponent -> RobotUnitState::InitializingDevices.
Definition: RobotUnitModuleBase.cpp:226
armarx::RobotUnitModule::ModuleBase::onInitComponent
void onInitComponent() final override
armarx::RobotUnitModule::RobotData
This Module holds all high-level data about the robot.
Definition: RobotUnitModuleRobotData.h:52
armarx::RobotUnitModule::ModuleBase::finishDeviceInitialization
virtual void finishDeviceInitialization()
Transition RobotUnitState::InitializingDevices -> RobotUnitState::InitializingUnits.
Definition: RobotUnitModuleBase.cpp:247
armarx::ctrlutil::s
double s(double t, double s0, double v0, double a0, double j)
Definition: CtrlUtil.h:33
armarx::RobotUnitState::Exiting
@ Exiting
armarx
This file offers overloads of toIce() and fromIce() functions for STL container types.
Definition: ArmarXTimeserver.cpp:28
armarx::RobotUnitModule::ModuleBase::onConnectComponent
void onConnectComponent() final override
Definition: RobotUnitModuleBase.cpp:143