RobotUnitModulePublisher.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 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 #pragma once
24 
27 
28 #include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
29 #include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
30 
31 #include "../RobotUnitObserver.h"
32 #include "RobotUnitModuleBase.h"
33 
34 namespace armarx::detail
35 {
36  struct ControlThreadOutputBufferEntry;
37 }
38 
39 namespace armarx
40 {
41  TYPEDEF_PTRS_HANDLE(RobotUnitObserver);
42  using SensorAndControl = detail::ControlThreadOutputBufferEntry;
43 } // namespace armarx
44 
46 {
48  {
49  public:
51  {
52  defineOptionalProperty<std::string>("RobotUnitListenerTopicName",
53  "RobotUnitListenerTopic",
54  "The topic receiving events for RobotUnitListener");
55  defineOptionalProperty<std::string>("DebugDrawerUpdatesTopicName",
56  "DebugDrawerUpdates",
57  "The topic receiving events for debug drawing");
58  defineOptionalProperty<std::size_t>(
59  "PublishPeriodMs", 10, "Milliseconds between each publish");
60 
61  defineOptionalProperty<bool>("ObserverEnablePublishing",
62  true,
63  "Whether the publishing thread is started or not",
65  defineOptionalProperty<bool>("ObserverPublishSensorValues",
66  true,
67  "Whether sensor values are send to the observer",
69  defineOptionalProperty<bool>("ObserverPublishControlTargets",
70  true,
71  "Whether control targets are send to the observer",
73  defineOptionalProperty<bool>("ObserverPublishTimingInformation",
74  true,
75  "Whether timing information are send to the observer",
77  defineOptionalProperty<bool>("ObserverPublishAdditionalInformation",
78  true,
79  "Whether additional information are send to the observer",
81  defineOptionalProperty<std::string>(
82  "DebugObserverTopicName", "DebugObserver", "The topic where updates are send to");
83  defineOptionalProperty<std::uint64_t>(
84  "ObserverPrintEveryNthIterations",
85  1,
86  "Only every nth iteration data is send to the debug observer",
88  }
89  };
90 
91  /**
92  * @ingroup Library-RobotUnit-Modules
93  * @brief This \ref ModuleBase "Module" manages publishing of all data to Topics,
94  * updating of all units managed by \ref Units "the Units module" and calling of \ref NJointControllerBase hooks.
95  *
96  * @see ModuleBase
97  */
98  class Publisher : virtual public ModuleBase, virtual public RobotUnitPublishingInterface
99  {
100  friend class ModuleBase;
101 
102  public:
103  /**
104  * @brief Returns the singleton instance of this class
105  * @return The singleton instance of this class
106  */
107  static Publisher&
109  {
110  return ModuleBase::Instance<Publisher>();
111  }
112 
113  // //////////////////////////////////////////////////////////////////////////////////////// //
114  // ///////////////////////////////// RobotUnitModule hooks //////////////////////////////// //
115  // //////////////////////////////////////////////////////////////////////////////////////// //
116  private:
117  /// @see ModuleBase::_icePropertiesInitialized
118  void _icePropertiesInitialized();
119  /// @see ModuleBase::_componentPropertiesUpdated
120  void _componentPropertiesUpdated(const std::set<std::string>& changedProperties);
121 
122  /// @see ModuleBase::_preOnInitRobotUnit
123  void _preOnInitRobotUnit();
124  /// @see ModuleBase::_preOnConnectRobotUnit
125  void _preOnConnectRobotUnit();
126  /// @see ModuleBase::_postFinishControlThreadInitialization
127  void _postFinishControlThreadInitialization();
128  /// @see ModuleBase::_preFinishRunning
129  void _preFinishRunning();
130  // //////////////////////////////////////////////////////////////////////////////////////// //
131  // ///////////////////////////////////// ice interface //////////////////////////////////// //
132  // //////////////////////////////////////////////////////////////////////////////////////// //
133  public:
134  /**
135  * @brief Returns the name of the used DebugDrawerTopic
136  * @return The name of the used DebugDrawerTopic
137  */
138  std::string getDebugDrawerTopicName(const Ice::Current& = Ice::emptyCurrent) const override;
139  /**
140  * @brief Returns the name of the used DebugObserverTopic
141  * @return The name of the used DebugObserverTopic
142  */
143  std::string
144  getDebugObserverTopicName(const Ice::Current& = Ice::emptyCurrent) const override;
145  /**
146  * @brief Returns the name of the used RobotUnitListenerTopic
147  * @return The name of the used RobotUnitListenerTopic
148  */
149  std::string
150  getRobotUnitListenerTopicName(const Ice::Current& = Ice::emptyCurrent) const override;
151 
152  /**
153  * @brief Returns the used DebugDrawerProxy
154  * @return The used DebugDrawerProxy
155  */
157  getDebugDrawerProxy(const Ice::Current& = Ice::emptyCurrent) const override;
158  /**
159  * @brief Returns the used RobotUnitListenerProxy
160  * @return The used RobotUnitListenerProxy
161  */
162  RobotUnitListenerPrx
163  getRobotUnitListenerProxy(const Ice::Current& = Ice::emptyCurrent) const override;
164  /**
165  * @brief Returns the used DebugObserverProxy
166  * @return The used DebugObserverProxy
167  */
169  getDebugObserverProxy(const Ice::Current& = Ice::emptyCurrent) const override;
170  // //////////////////////////////////////////////////////////////////////////////////////// //
171  // /////////////////////////////////// Module interface /////////////////////////////////// //
172  // //////////////////////////////////////////////////////////////////////////////////////// //
173  protected:
174  /**
175  * @brief Publishes data
176  * @param additionalMap Data published to the RobotUnitObserver's additional channel (This field is used by deriving classes)
177  * @param timingMap Data published to the RobotUnitObserver's timing channel (This field is used by deriving classes)
178  */
179  virtual void publish(StringVariantBaseMap additionalMap = {},
180  StringVariantBaseMap timingMap = {});
181  // //////////////////////////////////////////////////////////////////////////////////////// //
182  // //////////////////////////////////// implementation //////////////////////////////////// //
183  // //////////////////////////////////////////////////////////////////////////////////////// //
184  private:
185  /**
186  * @brief Returns the used RobotUnitObserver
187  * @return The used RobotUnitObserver
188  */
189  const RobotUnitObserverPtr&
190  getRobotUnitObserver() const
191  {
192  ARMARX_CHECK_EXPRESSION(robotUnitObserver);
193  return robotUnitObserver;
194  }
195 
196  /**
197  * @brief Publishes updtes about new classes od \ref NJointControllerBase "NJointControllers"
198  * @return The time required by this function as a \ref Variant
199  */
200  TimedVariantPtr publishNJointClassNames();
201  /**
202  * @brief Publishes updates of \ref NJointControllerBase "NJointControllers" ([de]activate + publish hooks)
203  * @param timingMap Timings of this publish iteration (out param)
204  * @param controlThreadOutputBuffer The output of the published \ref ControlThread iteration
205  * @return The time required by this function as a \ref Variant
206  * @see NJointControllerBase::onPublish
207  */
209  publishNJointControllerUpdates(StringVariantBaseMap& timingMap,
210  const SensorAndControl& controlThreadOutputBuffer);
211  /**
212  * @brief Updates all sub units and publishes the timings of these updates
213  * @param timingMap Timings of this publish iteration (out param)
214  * @param controlThreadOutputBuffer The output of the published \ref ControlThread iteration
215  * @param activatedControllers The \ref JointController "JointControllers" and \ref NJointControllerBase "NJointControllers"
216  * active in the published \ref ControlThread iteration
217  * @return The time required by this function as a \ref Variant
218  */
219  TimedVariantPtr publishUnitUpdates(StringVariantBaseMap& timingMap,
220  const SensorAndControl& controlThreadOutputBuffer,
221  const JointAndNJointControllers& activatedControllers);
222  /**
223  * @brief Publishes data about updates of \ref JointController "JointControllers" and their \ref ControlTargetBase "ControlTargets"
224  * @param controlThreadOutputBuffer The output of the published \ref ControlThread iteration
225  * @param haveSensorAndControlValsChanged Whether \ref ControlTargetBase "ControlTargets" were updated by the \ref ControlThread
226  * @param publishToObserver Whether data should be published to observers
227  * @param activatedControllers The \ref JointController "JointControllers" and \ref NJointControllerBase "NJointControllers"
228  * active in the published \ref ControlThread iteration
229  * @param requestedJointControllers
230  * @return The time required by this function as a \ref Variant
231  */
233  publishControlUpdates(const SensorAndControl& controlThreadOutputBuffer,
234  bool haveSensorAndControlValsChanged,
235  bool publishToObserver,
236  const JointAndNJointControllers& activatedControllers,
237  const std::vector<JointController*>& requestedJointControllers);
238  /**
239  * @brief Publishes Updates about \ref SensorValueBase "SensorValues" (To \ref RobotUnitListener and \ref RobotUnitObserver)
240  * @param publishToObserver Whether data should be published to observers
241  * @param controlThreadOutputBuffer The output of the published \ref ControlThread iteration
242  * @return The time required by this function as a \ref Variant
243  */
244  TimedVariantPtr publishSensorUpdates(bool publishToObserver,
245  const SensorAndControl& controlThreadOutputBuffer);
246  // //////////////////////////////////////////////////////////////////////////////////////// //
247  // ///////////////////////////////////////// Data ///////////////////////////////////////// //
248  // //////////////////////////////////////////////////////////////////////////////////////// //
249  private:
250  static constexpr int spamdelay = 30;
251 
252  using PublisherTaskT = SimplePeriodicTask<std::function<void(void)>>;
253 
254  /// @brief The name of the used RobotUnitListenerTopic
255  std::string robotUnitListenerTopicName;
256  /// @brief The name of the used DebugDrawerTopic
257  std::string debugDrawerUpdatesTopicName;
258  /// @brief The name of the used DebugObserverTopic
259  std::string debugObserverTopicName;
260 
261  /// @brief The used DebugDrawerProxy
262  DebugDrawerInterfacePrx debugDrawerPrx;
263  /// @brief The used DebugObserverProxy
264  DebugObserverInterfacePrx debugObserverPrx;
265 
266  /// @brief the number of calles to \ref publish
267  std::uint64_t publishIterationCount{0};
268 
269  /// @brief The time required by the last iteration of \ref publish to publish sensor data
270  IceUtil::Time publishNewSensorDataTime;
271  /// @brief The thread executing the publisher loop
272  PublisherTaskT::pointer_type publisherTask; ///TODO use std thread
273  /// @brief The already reported classes of \ref NJointControllerBase "NJointControllers"
274  std::set<std::string> lastReportedClasses;
275 
276  /// @brief Whether \ref SensorValueBase "SensorValues" should be published to the observers
277  std::atomic_bool observerPublishSensorValues;
278  /// @brief Whether the publishing thread should be started or not
279  std::atomic_bool observerEnablePublishing;
280  /// @brief Whether \ref ControlTargetBase "ControlTargets" should be published to the observers
281  std::atomic_bool observerPublishControlTargets;
282  /// @brief Whether \ref Timing information should be published to the observers
283  std::atomic_bool observerPublishTimingInformation;
284  /// @brief Whether \ref Additional information should be published to the observers
285  std::atomic_bool observerPublishAdditionalInformation;
286 
287  /// @brief How many iterations of \ref publish shold not publish data to the debug observer.
288  std::atomic<std::uint64_t> debugObserverSkipIterations;
289 
290  /// @brief The time required by the last iteration of \ref publish
291  /// \warning May only be accessed by the publish thread.
292  IceUtil::Time lastPublishLoop;
293 
294  /// @brief The used RobotUnitObserver
295  RobotUnitObserverPtr robotUnitObserver;
296 
297  /// @brief The period of the publisher loop
298  std::size_t publishPeriodMs{1};
299 
300  /// @brief A proxy to the used RobotUnitListener topic
301  RobotUnitListenerPrx robotUnitListenerPrx;
302  /// @brief A batch proxy to the used RobotUnitListener topic
303  /// \warning May only be accessed by the publish thread.
304  RobotUnitListenerPrx robotUnitListenerBatchPrx;
305 
306  /// \warning May only be accessed by the publish thread.
307  IceUtil::Time lastControlThreadTimestamp;
308  };
309 } // namespace armarx::RobotUnitModule
DebugObserver.h
armarx::StringVariantBaseMap
std::map< std::string, VariantBasePtr > StringVariantBaseMap
Definition: ManagedIceObject.h:111
armarx::TYPEDEF_PTRS_HANDLE
TYPEDEF_PTRS_HANDLE(NJointCartesianNaturalPositionController)
armarx::RobotUnitModule::Publisher::Instance
static Publisher & Instance()
Returns the singleton instance of this class.
Definition: RobotUnitModulePublisher.h:108
armarx::RobotUnitModule::Publisher::getRobotUnitListenerTopicName
std::string getRobotUnitListenerTopicName(const Ice::Current &=Ice::emptyCurrent) const override
Returns the name of the used RobotUnitListenerTopic.
Definition: RobotUnitModulePublisher.cpp:400
RobotUnitModuleBase.h
armarx::RobotUnitModule::Publisher::getDebugObserverTopicName
std::string getDebugObserverTopicName(const Ice::Current &=Ice::emptyCurrent) const override
Returns the name of the used DebugObserverTopic.
Definition: RobotUnitModulePublisher.cpp:416
armarx::PropertyDefinitionContainer::prefix
std::string prefix
Prefix of the properties such as namespace, domain, component name, etc.
Definition: PropertyDefinitionContainer.h:333
armarx::DebugObserverInterfacePrx
::IceInternal::ProxyHandle<::IceProxy::armarx::DebugObserverInterface > DebugObserverInterfacePrx
Definition: JointController.h:44
armarx::SensorAndControl
detail::ControlThreadOutputBufferEntry SensorAndControl
Definition: NJointControllerBase.h:72
armarx::RobotUnitModule::Publisher::publish
virtual void publish(StringVariantBaseMap additionalMap={}, StringVariantBaseMap timingMap={})
Publishes data.
Definition: RobotUnitModulePublisher.cpp:587
armarx::RobotUnitModule::Publisher::getDebugDrawerProxy
DebugDrawerInterfacePrx getDebugDrawerProxy(const Ice::Current &=Ice::emptyCurrent) const override
Returns the used DebugDrawerProxy.
Definition: RobotUnitModulePublisher.cpp:442
armarx::detail
Definition: ApplicationNetworkStats.cpp:34
TaskUtil.h
armarx::armem::Time
armarx::core::time::DateTime Time
Definition: forward_declarations.h:13
armarx::RobotUnitModule::PublisherPropertyDefinitions
Definition: RobotUnitModulePublisher.h:47
armarx::RobotUnitModule::Publisher
This Module manages publishing of all data to Topics, updating of all units managed by the Units modu...
Definition: RobotUnitModulePublisher.h:98
armarx::TimedVariantPtr
IceInternal::Handle< TimedVariant > TimedVariantPtr
Definition: TimedVariant.h:38
armarx::SimplePeriodicTask
SimplePeriodicTask(Ts...) -> SimplePeriodicTask< std::function< void(void)>>
armarx::RobotUnitModule::Publisher::getDebugDrawerTopicName
std::string getDebugDrawerTopicName(const Ice::Current &=Ice::emptyCurrent) const override
Returns the name of the used DebugDrawerTopic.
Definition: RobotUnitModulePublisher.cpp:408
armarx::RobotUnitModule::Publisher::getDebugObserverProxy
DebugObserverInterfacePrx getDebugObserverProxy(const Ice::Current &=Ice::emptyCurrent) const override
Returns the used DebugObserverProxy.
Definition: RobotUnitModulePublisher.cpp:433
ARMARX_CHECK_EXPRESSION
#define ARMARX_CHECK_EXPRESSION(expression)
This macro evaluates the expression and if it turns out to be false it will throw an ExpressionExcept...
Definition: ExpressionException.h:73
armarx::RobotUnitModule::PublisherPropertyDefinitions::PublisherPropertyDefinitions
PublisherPropertyDefinitions(std::string prefix)
Definition: RobotUnitModulePublisher.h:50
IceInternal::ProxyHandle<::IceProxy::armarx::DebugDrawerInterface >
armarx::RobotUnitModule
Definition: ControlDevice.h:34
armarx::RobotUnitModule::Publisher::getRobotUnitListenerProxy
RobotUnitListenerPrx getRobotUnitListenerProxy(const Ice::Current &=Ice::emptyCurrent) const override
Returns the used RobotUnitListenerProxy.
Definition: RobotUnitModulePublisher.cpp:424
armarx::DebugDrawerInterfacePrx
::IceInternal::ProxyHandle<::IceProxy::armarx::DebugDrawerInterface > DebugDrawerInterfacePrx
Definition: JointController.h:40
armarx::PropertyDefinitionBase::eModifiable
@ eModifiable
Definition: PropertyDefinitionInterface.h:57
armarx::RobotUnitModule::ModuleBasePropertyDefinitions
Definition: RobotUnitModuleBase.h:107
armarx
This file offers overloads of toIce() and fromIce() functions for STL container types.
Definition: ArmarXTimeserver.cpp:28
armarx::RobotUnitModule::ModuleBase
Base class for all RobotUnitModules.
Definition: RobotUnitModuleBase.h:183