EmergencyStop.h
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 ArmarXCore::ArmarXObjects::EmergencyStop
17  * @author Stefan Reither ( stef dot reither at web dot de )
18  * @date 2016
19  * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
20  * GNU General Public License
21  */
22 
23 #pragma once
24 
25 #include <atomic>
26 #include <shared_mutex>
27 
29 #include <ArmarXCore/core/time.h>
30 #include <ArmarXCore/interface/components/EmergencyStopInterface.h>
31 
32 namespace armarx
33 {
34  /**
35  * @class EmergencyStopPropertyNodeDefinitions
36  * @brief
37  */
39  {
40  public:
43  {
44  defineOptionalProperty<std::string>("EmergencyStopProxy",
45  "EmergencyStopMaster",
46  "The name of the emergencyStop-proxy.");
47  defineOptionalProperty<std::string>(
48  "EmergencyStopTopic",
49  "EmergencyStop",
50  "The name of the topic over which changes of the emergencyStopState are sent.");
51  }
52  };
53 
55  {
56  public:
59  {
60  defineOptionalProperty<std::string>(
61  "EmergencyStopTopic",
62  "EmergencyStop",
63  "The name of the topic over which changes of the emergencyStopState are sent.");
64  }
65  };
66 
67  /**
68  * @defgroup Component-EmergencyStop EmergencyStop
69  * @brief This component provides a software emergency-stop mechanism.
70  * @ingroup ArmarXCore-Components
71  *
72  * The mechanism consists of one EmergencyStopMaster and one or more EmergencyStopNode(s).
73  * Every node connects to the master and listens to any changes of the EmergencyStopState send over the specified topic.
74  * <br>
75  * The state of the nodes is always identical to the state of the master and can be used in the component that started such a node.
76  * Furthermore there is the possibility to register functions of the component directly in the node, which then will be executed immediately
77  * when the master changes into the specified state.
78  *
79  * **Properties:**
80  *
81  * In most usecases there is no need to change any of the two optional properties: *EmergencyStopProxy*
82  * and *EmergencyStopTopic*. With their standard values the functionality of one master and many nodes and the button
83  * in the ArmarXGui is guaranteed. Only in case there are multiple masters with different states, these additional masters have to
84  * get a different proxy-name and topic-name.
85  *
86  * **Usage:**
87  *
88  * Example implementation of an EmergencyStopNode in a component:
89  *
90  * KinematicUnitArmar4.h:
91  * \verbatim
92  #include <ArmarXCore/components/EmergencyStop/EmergencyStop.h>
93  class KinematicUnitArmar4
94  {
95  ...
96  armarx::EmergencyStopNode<KinematicUnitArmar4>::EmergencyStopNodePtr emergencyStopNode;
97  ...
98  }
99  \endverbatim
100  *
101  * KinematicUnitArmar4.cpp:
102  * \verbatim
103  void KinematicUnitArmar4::onInitKinematicUnit()
104  {
105  //
106  //
107  //
108 
109  // Creating an EmergencyStopNode with this class (KinematicUnitArmar4) as template-typ
110  emergencyStopNode = armarx::Component::create < armarx::EmergencyStopNode<KinematicUnitArmar4> >();
111 
112  // Adding the created node to the ArmarXManager, it is important to use an UUID in the name, because there might
113  // be several nodes
114  this->getArmarXManager()->addObject(emergencyStopNode, false, emergencyStopNode->getDefaultName() + IceUtil::generateUUID());
115 
116  // Waiting for the node-component to be started
117  emergencyStopNode->getObjectScheduler()->waitForObjectState(armarx::ManagedIceObjectState::eManagedIceObjectStarted);
118 
119  // Register a function of this class in the node
120  emergencyStopNode->registerCallbackFunction(this, &KinematicUnitArmar4::emergencyStopActive, armarx::EmergencyStopState::eEmergencyStopActive);
121 
122  // Getting the current state of the EmergencyStop
123  EmergencyStopState state = emergencyStopNode->getEmergencyStopState();
124  if (state == armarx::EmergencyStopState::eEmergencyStopActive)
125  {
126  // do something
127  }
128  }
129 
130  void KinematicUnitArmar4::emergencyStopActive()
131  {
132  // do something else
133  }
134  \endverbatim
135  *
136  *
137  *
138  *
139  * @class EmergencyStopMaster
140  * @ingroup Component-EmergencyStop
141  * @brief The EmergencyStopMaster stores the current state of the EmergencyStop and broadcasts any changes to that state
142  * over the specified topic.
143  */
145  virtual public armarx::Component,
146  virtual public armarx::EmergencyStopMasterInterface
147  {
148  public:
149  /**
150  * @brief Sets the state of the EmergencyStop and sends the new state immediatly over the specified topic
151  * @param state the new state
152  */
153  void setEmergencyStopState(EmergencyStopState state,
154  const ::Ice::Current& = Ice::emptyCurrent) override;
155 
156  /**
157  * @brief Try set the SS2 emergency stop state if it is safe.
158  *
159  * It is always safe to swith into active SS2 state. It is only safe to release SS2 state
160  * if it has been active for a specified amount of time in order to avoid bouncing.
161  *
162  * @return Assumed SS2 state after the operation.
163  */
164  EmergencyStopState
165  trySetEmergencyStopState(EmergencyStopState targetState,
166  const ::Ice::Current& = Ice::emptyCurrent) override;
167 
168  /**
169  * @return the current EmergencyStopState
170  */
171  EmergencyStopState
172  getEmergencyStopState(const ::Ice::Current& = Ice::emptyCurrent) const override;
173 
174  protected:
175  /**
176  * @see armarx::ManagedIceObject::onInitComponent()
177  */
178  void onInitComponent() override;
179 
180  /**
181  * @see armarx::ManagedIceObject::onConnectComponent()
182  */
183  void onConnectComponent() override;
184 
185  /**
186  * @see PropertyUser::createPropertyDefinitions()
187  */
189 
190  /**
191  * @see armarx::ManagedIceObject::getDefaultName()
192  */
193  std::string
194  getDefaultName() const override
195  {
196  return "EmergencyStopMaster";
197  }
198 
199  private:
200  EmergencyStopListenerPrx emergencyStopTopic;
201  mutable std::shared_mutex ss2StateMutex;
202  armarx::core::time::DateTime lastSS2Activation;
203  EmergencyStopState currentSS2State;
204  //emergency_stop::SS2_ThreadSafe emergencyStop;
205  };
206 
207  /**
208  * @class EmergencyStopNode
209  * @ingroup Component-EmergencyStop
210  * @brief This component listens on the specified topic (default value: *EmergencyStop*) and changes its state corresponding to the
211  * state send over the topic. Furthermore it calls registered functions if their expected state is equal to the one that has been sent.
212  * <br>
213  * The template-type of an instantiation of this class has to be equal to the type of the object to which any registered function belong.
214  */
215  template <class T>
217  virtual public armarx::Component,
218  virtual public armarx::EmergencyStopNodeInterface
219  {
220  typedef void (T::*method_type)(void);
221 
222  struct CallbackFunction
223  {
224  T* parent;
225  method_type function;
226  };
227 
228  using CallbackFunctionsMap = std::multimap<EmergencyStopState, CallbackFunction>;
229 
230  // EmergencyStopListener interface
231  public:
232  /**
233  * @brief registerCallbackFunction Registers the given function to be called, if the reported emergencyStopState is equal to the given one.
234  * @param parent the object the given function belongs to
235  * @param callbackFunction the function to be called when the state of the EmergencyStopMaster changes into the given one.
236  * Only functions with return-type void can be registered.
237  * @param state the state into which the EmergencyStopMaster has to change to call the given function
238  */
239  void
240  registerCallbackFunction(T* parent, method_type callbackFunction, EmergencyStopState state)
241  {
242  CallbackFunction cb;
243  cb.function = callbackFunction;
244  cb.parent = parent;
245 
246  callbackFunctions.insert(std::pair<EmergencyStopState, CallbackFunction>(state, cb));
247 
248  ARMARX_INFO << "Registered function: " << callbackFunction << " - " << state;
249  }
250 
251  /**
252  * @brief unregisterCallbackFunction Unregisters the given function for the given state. If no registered function fits to the
253  * given parameter, nothing happens
254  * @param parent the object the given function belongs to
255  * @param callbackFunction the function to be unregistered
256  * @param state the corresponding state
257  */
258  void
260  method_type callbackFunction,
261  EmergencyStopState state)
262  {
263  std::pair<typename CallbackFunctionsMap::iterator,
264  typename CallbackFunctionsMap::iterator>
265  range;
266  range = callbackFunctions.equal_range(state);
267 
268  for (typename CallbackFunctionsMap::iterator it = range.first; it != range.second;
269  /* no increment */)
270  {
271  if ((it->second).parent == parent && (it->second).function == callbackFunction)
272  {
273  callbackFunctions.erase(it++);
274  }
275  else
276  {
277  ++it;
278  }
279  }
280  }
281 
282  /**
283  * @return the current EmergencyStopState
284  */
285  EmergencyStopState
287  {
288  return this->currentState;
289  }
290 
292 
293  std::string
294  getDefaultName() const override
295  {
296  return "EmergencyStopNode";
297  }
298 
299  private:
300  void
301  callback(CallbackFunction callback)
302  {
303  T* parent = callback.parent;
304  method_type function = callback.function;
305 
306  try
307  {
308  (parent->*function)();
309  }
310  catch (...)
311  {
313  }
314  }
315 
316  void
317  reportEmergencyStopState(EmergencyStopState state, const Ice::Current&) override
318  {
319  currentState = state;
320 
321  std::pair<typename CallbackFunctionsMap::iterator,
322  typename CallbackFunctionsMap::iterator>
323  range;
324  range = callbackFunctions.equal_range(state);
325 
326  for (typename CallbackFunctionsMap::iterator it = range.first; it != range.second; ++it)
327  {
328  callback(it->second);
329  }
330  }
331 
332  CallbackFunctionsMap callbackFunctions;
333  std::atomic<EmergencyStopState> currentState;
334  // ManagedIceObject interface
335  protected:
336  void
337  onInitComponent() override
338  {
339  usingTopic(getProperty<std::string>("EmergencyStopTopic").getValue());
340  }
341 
342  void
344  {
345  EmergencyStopMasterInterfacePrx stopMaster = getProxy<EmergencyStopMasterInterfacePrx>(
346  getProperty<std::string>("EmergencyStopProxy").getValue(), false, "", false);
347  if (stopMaster)
348  {
349  currentState = stopMaster->getEmergencyStopState();
350  }
351  else
352  {
353  currentState = eEmergencyStopInactive;
354  }
355  }
356 
359  {
362  }
363  };
364 } // namespace armarx
armarx::EmergencyStopNode
This component listens on the specified topic (default value: EmergencyStop) and changes its state co...
Definition: EmergencyStop.h:216
armarx::EmergencyStopPropertyMasterDefinitions::EmergencyStopPropertyMasterDefinitions
EmergencyStopPropertyMasterDefinitions(std::string prefix)
Definition: EmergencyStop.h:57
armarx::EmergencyStopMaster::trySetEmergencyStopState
EmergencyStopState trySetEmergencyStopState(EmergencyStopState targetState, const ::Ice::Current &=Ice::emptyCurrent) override
Try set the SS2 emergency stop state if it is safe.
Definition: EmergencyStop.cpp:46
armarx::PropertyDefinitionContainer::prefix
std::string prefix
Prefix of the properties such as namespace, domain, component name, etc.
Definition: PropertyDefinitionContainer.h:333
armarx::EmergencyStopNode::getDefaultName
std::string getDefaultName() const override
Retrieve default name of component.
Definition: EmergencyStop.h:294
IceInternal::Handle
Definition: forward_declarations.h:8
armarx::EmergencyStopMaster::getDefaultName
std::string getDefaultName() const override
Definition: EmergencyStop.h:194
armarx::EmergencyStopNode::registerCallbackFunction
void registerCallbackFunction(T *parent, method_type callbackFunction, EmergencyStopState state)
registerCallbackFunction Registers the given function to be called, if the reported emergencyStopStat...
Definition: EmergencyStop.h:240
armarx::EmergencyStopNodePropertyDefinitions
Definition: EmergencyStop.h:38
armarx::EmergencyStopNodePropertyDefinitions::EmergencyStopNodePropertyDefinitions
EmergencyStopNodePropertyDefinitions(std::string prefix)
Definition: EmergencyStop.h:41
armarx::EmergencyStopMaster::getEmergencyStopState
EmergencyStopState getEmergencyStopState(const ::Ice::Current &=Ice::emptyCurrent) const override
Definition: EmergencyStop.cpp:63
armarx::EmergencyStopPropertyMasterDefinitions
Definition: EmergencyStop.h:54
armarx::EmergencyStopNode::onInitComponent
void onInitComponent() override
Pure virtual hook for the subclass.
Definition: EmergencyStop.h:337
Component.h
armarx::Component
Baseclass for all ArmarX ManagedIceObjects requiring properties.
Definition: Component.h:95
armarx::core::time::DateTime
Represents a point in time.
Definition: DateTime.h:24
armarx::ManagedIceObject::usingTopic
void usingTopic(const std::string &name, bool orderedPublishing=false)
Registers a proxy for subscription after initialization.
Definition: ManagedIceObject.cpp:248
armarx::Component::getConfigIdentifier
std::string getConfigIdentifier()
Retrieve config identifier for this component as set in constructor.
Definition: Component.cpp:74
armarx::EmergencyStopNode::onConnectComponent
void onConnectComponent() override
Pure virtual hook for the subclass.
Definition: EmergencyStop.h:343
armarx::EmergencyStopMaster
The EmergencyStopMaster stores the current state of the EmergencyStop and broadcasts any changes to t...
Definition: EmergencyStop.h:144
armarx::ComponentPropertyDefinitions
Default component property definition container.
Definition: Component.h:70
ARMARX_INFO
#define ARMARX_INFO
Definition: Logging.h:174
IceUtil::Handle< class PropertyDefinitionContainer >
armarx::EmergencyStopNode::createPropertyDefinitions
armarx::PropertyDefinitionsPtr createPropertyDefinitions() override
Creates the property definition container.
Definition: EmergencyStop.h:358
time.h
armarx::EmergencyStopMaster::onConnectComponent
void onConnectComponent() override
Definition: EmergencyStop.cpp:78
armarx::EmergencyStopMaster::onInitComponent
void onInitComponent() override
Definition: EmergencyStop.cpp:71
T
float T
Definition: UnscentedKalmanFilterTest.cpp:35
armarx::handleExceptions
void handleExceptions()
Definition: Exception.cpp:141
armarx::EmergencyStopMaster::createPropertyDefinitions
armarx::PropertyDefinitionsPtr createPropertyDefinitions() override
Definition: EmergencyStop.cpp:88
armarx::PropertyDefinitionsPtr
IceUtil::Handle< class PropertyDefinitionContainer > PropertyDefinitionsPtr
PropertyDefinitions smart pointer type.
Definition: forward_declarations.h:34
armarx::EmergencyStopNode::getEmergencyStopState
EmergencyStopState getEmergencyStopState() const
Definition: EmergencyStop.h:286
armarx::EmergencyStopNode::unregisterCallbackFunction
void unregisterCallbackFunction(T *parent, method_type callbackFunction, EmergencyStopState state)
unregisterCallbackFunction Unregisters the given function for the given state.
Definition: EmergencyStop.h:259
armarx
This file offers overloads of toIce() and fromIce() functions for STL container types.
Definition: ArmarXTimeserver.cpp:28
armarx::EmergencyStopMaster::setEmergencyStopState
void setEmergencyStopState(EmergencyStopState state, const ::Ice::Current &=Ice::emptyCurrent) override
Sets the state of the EmergencyStop and sends the new state immediatly over the specified topic.
Definition: EmergencyStop.cpp:31