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 
26 #include <ArmarXCore/interface/components/EmergencyStopInterface.h>
27 #include <atomic>
28 
29 namespace armarx
30 {
31  /**
32  * @class EmergencyStopPropertyNodeDefinitions
33  * @brief
34  */
37  {
38  public:
41  {
42  defineOptionalProperty<std::string>("EmergencyStopProxy", "EmergencyStopMaster", "The name of the emergencyStop-proxy.");
43  defineOptionalProperty<std::string>("EmergencyStopTopic", "EmergencyStop", "The name of the topic over which changes of the emergencyStopState are sent.");
44  }
45  };
46 
49  {
50  public:
53  {
54  defineOptionalProperty<std::string>("EmergencyStopTopic", "EmergencyStop", "The name of the topic over which changes of the emergencyStopState are sent.");
55  }
56  };
57 
58  /**
59  * @defgroup Component-EmergencyStop EmergencyStop
60  * @brief This component provides a software emergency-stop mechanism.
61  * @ingroup ArmarXCore-Components
62  *
63  * The mechanism consists of one EmergencyStopMaster and one or more EmergencyStopNode(s).
64  * Every node connects to the master and listens to any changes of the EmergencyStopState send over the specified topic.
65  * <br>
66  * 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.
67  * Furthermore there is the possibility to register functions of the component directly in the node, which then will be executed immediately
68  * when the master changes into the specified state.
69  *
70  * **Properties:**
71  *
72  * In most usecases there is no need to change any of the two optional properties: *EmergencyStopProxy*
73  * and *EmergencyStopTopic*. With their standard values the functionality of one master and many nodes and the button
74  * in the ArmarXGui is guaranteed. Only in case there are multiple masters with different states, these additional masters have to
75  * get a different proxy-name and topic-name.
76  *
77  * **Usage:**
78  *
79  * Example implementation of an EmergencyStopNode in a component:
80  *
81  * KinematicUnitArmar4.h:
82  * \verbatim
83  #include <ArmarXCore/components/EmergencyStop/EmergencyStop.h>
84  class KinematicUnitArmar4
85  {
86  ...
87  armarx::EmergencyStopNode<KinematicUnitArmar4>::EmergencyStopNodePtr emergencyStopNode;
88  ...
89  }
90  \endverbatim
91  *
92  * KinematicUnitArmar4.cpp:
93  * \verbatim
94  void KinematicUnitArmar4::onInitKinematicUnit()
95  {
96  //
97  //
98  //
99 
100  // Creating an EmergencyStopNode with this class (KinematicUnitArmar4) as template-typ
101  emergencyStopNode = armarx::Component::create < armarx::EmergencyStopNode<KinematicUnitArmar4> >();
102 
103  // Adding the created node to the ArmarXManager, it is important to use an UUID in the name, because there might
104  // be several nodes
105  this->getArmarXManager()->addObject(emergencyStopNode, false, emergencyStopNode->getDefaultName() + IceUtil::generateUUID());
106 
107  // Waiting for the node-component to be started
108  emergencyStopNode->getObjectScheduler()->waitForObjectState(armarx::ManagedIceObjectState::eManagedIceObjectStarted);
109 
110  // Register a function of this class in the node
111  emergencyStopNode->registerCallbackFunction(this, &KinematicUnitArmar4::emergencyStopActive, armarx::EmergencyStopState::eEmergencyStopActive);
112 
113  // Getting the current state of the EmergencyStop
114  EmergencyStopState state = emergencyStopNode->getEmergencyStopState();
115  if (state == armarx::EmergencyStopState::eEmergencyStopActive)
116  {
117  // do something
118  }
119  }
120 
121  void KinematicUnitArmar4::emergencyStopActive()
122  {
123  // do something else
124  }
125  \endverbatim
126  *
127  *
128  *
129  *
130  * @class EmergencyStopMaster
131  * @ingroup Component-EmergencyStop
132  * @brief The EmergencyStopMaster stores the current state of the EmergencyStop and broadcasts any changes to that state
133  * over the specified topic.
134  */
136  virtual public armarx::Component,
137  virtual public armarx::EmergencyStopMasterInterface
138  {
139  public:
140 
141  /**
142  * @brief Sets the state of the EmergencyStop and sends the new state immediatly over the specified topic
143  * @param state the new state
144  */
145  void setEmergencyStopState(EmergencyStopState state, const ::Ice::Current& = Ice::emptyCurrent) override;
146  /**
147  * @return the current EmergencyStopState
148  */
149  EmergencyStopState getEmergencyStopState(const ::Ice::Current& = Ice::emptyCurrent) const override;
150 
151  protected:
152  /**
153  * @see armarx::ManagedIceObject::onInitComponent()
154  */
155  void onInitComponent() override;
156 
157  /**
158  * @see armarx::ManagedIceObject::onConnectComponent()
159  */
160  void onConnectComponent() override;
161 
162  /**
163  * @see PropertyUser::createPropertyDefinitions()
164  */
166 
167  /**
168  * @see armarx::ManagedIceObject::getDefaultName()
169  */
170  std::string getDefaultName() const override
171  {
172  return "EmergencyStopMaster";
173  }
174 
175  private:
176  EmergencyStopListenerPrx emergencyStopTopic;
177  std::atomic<EmergencyStopState> emergencyStopState;
178 
179  };
180 
181  /**
182  * @class EmergencyStopNode
183  * @ingroup Component-EmergencyStop
184  * @brief This component listens on the specified topic (default value: *EmergencyStop*) and changes its state corresponding to the
185  * state send over the topic. Furthermore it calls registered functions if their expected state is equal to the one that has been sent.
186  * <br>
187  * 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.
188  */
189  template <class T>
191  virtual public armarx::Component,
192  virtual public armarx::EmergencyStopNodeInterface
193  {
194  typedef void (T::*method_type)(void);
195 
196  struct CallbackFunction
197  {
198  T* parent;
199  method_type function;
200  };
201 
202  using CallbackFunctionsMap = std::multimap<EmergencyStopState, CallbackFunction>;
203 
204  // EmergencyStopListener interface
205  public:
206 
207  /**
208  * @brief registerCallbackFunction Registers the given function to be called, if the reported emergencyStopState is equal to the given one.
209  * @param parent the object the given function belongs to
210  * @param callbackFunction the function to be called when the state of the EmergencyStopMaster changes into the given one.
211  * Only functions with return-type void can be registered.
212  * @param state the state into which the EmergencyStopMaster has to change to call the given function
213  */
214  void registerCallbackFunction(T* parent, method_type callbackFunction, EmergencyStopState state)
215  {
216  CallbackFunction cb;
217  cb.function = callbackFunction;
218  cb.parent = parent;
219 
220  callbackFunctions.insert(std::pair<EmergencyStopState, CallbackFunction>(state, cb));
221 
222  ARMARX_INFO << "Registered function: " << callbackFunction << " - " << state;
223  }
224 
225  /**
226  * @brief unregisterCallbackFunction Unregisters the given function for the given state. If no registered function fits to the
227  * given parameter, nothing happens
228  * @param parent the object the given function belongs to
229  * @param callbackFunction the function to be unregistered
230  * @param state the corresponding state
231  */
232  void unregisterCallbackFunction(T* parent, method_type callbackFunction, EmergencyStopState state)
233  {
234  std::pair<typename CallbackFunctionsMap::iterator, typename CallbackFunctionsMap::iterator> range;
235  range = callbackFunctions.equal_range(state);
236 
237  for (typename CallbackFunctionsMap::iterator it = range.first; it != range.second; /* no increment */)
238  {
239  if ((it->second).parent == parent && (it->second).function == callbackFunction)
240  {
241  callbackFunctions.erase(it++);
242  }
243  else
244  {
245  ++it;
246  }
247  }
248  }
249 
250  /**
251  * @return the current EmergencyStopState
252  */
253  EmergencyStopState getEmergencyStopState() const
254  {
255  return this->currentState;
256  }
257 
259 
260  std::string getDefaultName() const override
261  {
262  return "EmergencyStopNode";
263  }
264 
265  private:
266  void callback(CallbackFunction callback)
267  {
268  T* parent = callback.parent;
269  method_type function = callback.function;
270 
271  try
272  {
273  (parent->*function)();
274  }
275  catch (...)
276  {
278  }
279  }
280 
281  void reportEmergencyStopState(EmergencyStopState state, const Ice::Current&) override
282  {
283  currentState = state;
284 
285  std::pair<typename CallbackFunctionsMap::iterator, typename CallbackFunctionsMap::iterator> range;
286  range = callbackFunctions.equal_range(state);
287 
288  for (typename CallbackFunctionsMap::iterator it = range.first; it != range.second; ++it)
289  {
290  callback(it->second);
291  }
292  }
293 
294  CallbackFunctionsMap callbackFunctions;
295  std::atomic<EmergencyStopState> currentState;
296  // ManagedIceObject interface
297  protected:
298  void onInitComponent() override
299  {
300  usingTopic(getProperty<std::string>("EmergencyStopTopic").getValue());
301  }
302 
303  void onConnectComponent() override
304  {
305  EmergencyStopMasterInterfacePrx stopMaster = getProxy<EmergencyStopMasterInterfacePrx>(getProperty<std::string>("EmergencyStopProxy").getValue(), false, "", false);
306  if (stopMaster)
307  {
308  currentState = stopMaster->getEmergencyStopState();
309  }
310  else
311  {
312  currentState = eEmergencyStopInactive;
313  }
314  }
315 
317  {
320 
321  }
322 
323  };
324 }
325 
armarx::EmergencyStopNode
This component listens on the specified topic (default value: EmergencyStop) and changes its state co...
Definition: EmergencyStop.h:190
armarx::EmergencyStopPropertyMasterDefinitions::EmergencyStopPropertyMasterDefinitions
EmergencyStopPropertyMasterDefinitions(std::string prefix)
Definition: EmergencyStop.h:51
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:260
IceInternal::Handle
Definition: forward_declarations.h:8
armarx::EmergencyStopMaster::getDefaultName
std::string getDefaultName() const override
Definition: EmergencyStop.h:170
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:214
armarx::EmergencyStopNodePropertyDefinitions
Definition: EmergencyStop.h:35
armarx::EmergencyStopNodePropertyDefinitions::EmergencyStopNodePropertyDefinitions
EmergencyStopNodePropertyDefinitions(std::string prefix)
Definition: EmergencyStop.h:39
armarx::EmergencyStopMaster::getEmergencyStopState
EmergencyStopState getEmergencyStopState(const ::Ice::Current &=Ice::emptyCurrent) const override
Definition: EmergencyStop.cpp:48
armarx::EmergencyStopPropertyMasterDefinitions
Definition: EmergencyStop.h:47
armarx::EmergencyStopNode::onInitComponent
void onInitComponent() override
Pure virtual hook for the subclass.
Definition: EmergencyStop.h:298
Component.h
armarx::Component
Baseclass for all ArmarX ManagedIceObjects requiring properties.
Definition: Component.h:95
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:303
armarx::EmergencyStopMaster
The EmergencyStopMaster stores the current state of the EmergencyStop and broadcasts any changes to t...
Definition: EmergencyStop.h:135
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:316
armarx::EmergencyStopMaster::onConnectComponent
void onConnectComponent() override
Definition: EmergencyStop.cpp:59
armarx::EmergencyStopMaster::onInitComponent
void onInitComponent() override
Definition: EmergencyStop.cpp:53
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:67
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:253
armarx::EmergencyStopNode::unregisterCallbackFunction
void unregisterCallbackFunction(T *parent, method_type callbackFunction, EmergencyStopState state)
unregisterCallbackFunction Unregisters the given function for the given state.
Definition: EmergencyStop.h:232
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:28