RobotUnitModulePublisher.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 
24 
29 
39 
41 {
42  /**
43  * \brief This class allows minimal access to private members of \ref NJointControllerBase in a sane fashion for \ref Publisher.
44  * \warning !! DO NOT ADD ANYTHING IF YOU DO NOT KNOW WAHT YOU ARE DOING! IF YOU DO SOMETHING WRONG YOU WILL CAUSE UNDEFINED BEHAVIOUR !!
45  */
47  {
48  friend class Publisher;
49 
50  static bool
51  GetStatusReportedActive(const NJointControllerBasePtr& nJointCtrl)
52  {
53  return nJointCtrl->statusReportedActive;
54  }
55 
56  static bool
57  GetStatusReportedRequested(const NJointControllerBasePtr& nJointCtrl)
58  {
59  return nJointCtrl->statusReportedRequested;
60  }
61 
62  static void
63  UpdateStatusReported(const NJointControllerBasePtr& nJointCtrl)
64  {
65  nJointCtrl->statusReportedActive = nJointCtrl->isActive.load();
66  nJointCtrl->statusReportedRequested = nJointCtrl->isRequested.load();
67  }
68 
69  static void
70  Publish(const NJointControllerBasePtr& nJointCtrl,
71  const SensorAndControl& sac,
72  const DebugDrawerInterfacePrx& draw,
73  const DebugObserverInterfacePrx& observer)
74  {
75  nJointCtrl->publish(sac, draw, observer);
76  }
77 
78  static void
79  DeactivatePublishing(const NJointControllerBasePtr& nJointCtrl,
80  const DebugDrawerInterfacePrx& draw,
81  const DebugObserverInterfacePrx& observer)
82  {
83  nJointCtrl->deactivatePublish(draw, observer);
84  }
85  };
86 
87  /**
88  * \brief This class allows minimal access to private members of \ref Devices in a sane fashion for \ref Publisher.
89  * \warning !! DO NOT ADD ANYTHING IF YOU DO NOT KNOW WAHT YOU ARE DOING! IF YOU DO SOMETHING WRONG YOU WILL CAUSE UNDEFINED BEHAVIOUR !!
90  */
92  {
93  friend class Publisher;
94 
95  static const std::string&
96  GetSensorDeviceName(Publisher* p, std::size_t idx)
97  {
98  return p->_module<Devices>().sensorDevices.at(idx)->getDeviceName();
99  }
100  };
101 
102  /**
103  * \brief This class allows minimal access to private members of \ref Units in a sane fashion for \ref Publisher.
104  * \warning !! DO NOT ADD ANYTHING IF YOU DO NOT KNOW WAHT YOU ARE DOING! IF YOU DO SOMETHING WRONG YOU WILL CAUSE UNDEFINED BEHAVIOUR !!
105  */
107  {
108  friend class Publisher;
109 
110  static const std::vector<RobotUnitSubUnitPtr>&
111  GetSubUnits(Publisher* p)
112  {
113  return p->_module<Units>().subUnits;
114  }
115  };
116 
117  /**
118  * \brief This class allows minimal access to private members of \ref ControllerManagement in a sane fashion for \ref Publisher.
119  * \warning !! DO NOT ADD ANYTHING IF YOU DO NOT KNOW WAHT YOU ARE DOING! IF YOU DO SOMETHING WRONG YOU WILL CAUSE UNDEFINED BEHAVIOUR !!
120  */
122  {
123  friend class Publisher;
124 
125  static const std::map<std::string, NJointControllerBasePtr>&
126  GetNJointControllers(Publisher* p)
127  {
128  return p->_module<ControllerManagement>().nJointControllers;
129  }
130 
131  static void
132  RemoveNJointControllersToBeDeleted(Publisher* p, bool blocking, RobotUnitListenerPrx l)
133  {
134  p->_module<ControllerManagement>().removeNJointControllersToBeDeleted(blocking, l);
135  }
136  };
137 
138  /**
139  * \brief This class allows minimal access to private members of \ref ControlThread in a sane fashion for \ref Publisher.
140  * \warning !! DO NOT ADD ADDITIONAL FRIENDS IF YOU DO NOT KNOW WAHT YOU ARE DOING! IF YOU DO SOMETHING WRONG YOU WILL CAUSE UNDEFINED BEHAVIOUR !!
141  */
143  {
144  friend class Publisher;
145 
146  static void
147  ProcessEmergencyStopRequest(Publisher* p)
148  {
149  return p->_module<ControlThread>().processEmergencyStopRequest();
150  }
151  };
152 } // namespace armarx::RobotUnitModule
153 
154 namespace armarx::RobotUnitModule
155 {
157  Publisher::publishNJointClassNames()
158  {
159  ARMARX_TRACE;
160  throwIfInControlThread(BOOST_CURRENT_FUNCTION);
161  const auto beg = TimeUtil::GetTime(true);
162 
163  const auto classNames = NJointControllerRegistry::getKeys();
164  if (lastReportedClasses.size() != classNames.size())
165  {
166  for (const auto& name : classNames)
167  {
168  if (!lastReportedClasses.count(name))
169  {
170  robotUnitListenerBatchPrx->nJointControllerClassAdded(name);
171  lastReportedClasses.emplace(name);
172  }
173  }
174  }
175 
176  const auto end = TimeUtil::GetTime(true);
177  return new TimedVariant{TimestampVariant{end - beg}, lastControlThreadTimestamp};
178  }
179 
181  Publisher::publishNJointControllerUpdates(StringVariantBaseMap& timingMap,
182  const SensorAndControl& controlThreadOutputBuffer)
183  {
184  ARMARX_TRACE;
185  throwIfInControlThread(BOOST_CURRENT_FUNCTION);
186  const auto beg = TimeUtil::GetTime(true);
187 
188  //publish controller updates
189  auto debugDrawerBatchPrx = debugDrawerPrx->ice_batchOneway();
190  auto debugObserverBatchPrx = debugObserverPrx->ice_batchOneway();
191  NJointControllerStatusSeq allStatus;
192  for (const auto& pair :
193  ControllerManagementAttorneyForPublisher::GetNJointControllers(this))
194  {
195  const auto begInner = TimeUtil::GetTime(true);
196  const NJointControllerBasePtr& nJointCtrl = pair.second;
197 
198  //run some hook for active (used for visu)
199  NJointControllerAttorneyForPublisher::Publish(
200  nJointCtrl, controlThreadOutputBuffer, debugDrawerBatchPrx, debugObserverBatchPrx);
201  if (NJointControllerAttorneyForPublisher::GetStatusReportedActive(nJointCtrl) !=
202  nJointCtrl->isControllerActive() ||
203  NJointControllerAttorneyForPublisher::GetStatusReportedRequested(nJointCtrl) !=
204  nJointCtrl->isControllerRequested())
205  {
206  NJointControllerAttorneyForPublisher::UpdateStatusReported(nJointCtrl);
207  allStatus.emplace_back(nJointCtrl->getControllerStatus());
208  }
209 
210  const auto endInner = TimeUtil::GetTime(true);
211  timingMap["NJointControllerUpdates_" + nJointCtrl->getInstanceName()] =
212  new TimedVariant{TimestampVariant{endInner - begInner}, lastControlThreadTimestamp};
213  }
214 
215  robotUnitListenerBatchPrx->nJointControllerStatusChanged(allStatus);
216  debugObserverBatchPrx->ice_flushBatchRequests();
217  debugDrawerBatchPrx->ice_flushBatchRequests();
218 
219  const auto end = TimeUtil::GetTime(true);
220  return new TimedVariant{TimestampVariant{end - beg}, lastControlThreadTimestamp};
221  }
222 
224  Publisher::publishUnitUpdates(StringVariantBaseMap& timingMap,
225  const SensorAndControl& controlThreadOutputBuffer,
226  const JointAndNJointControllers& activatedControllers)
227  {
228  ARMARX_TRACE;
229  throwIfInControlThread(BOOST_CURRENT_FUNCTION);
230  const auto beg = TimeUtil::GetTime(true);
231 
232  ARMARX_DEBUG << deactivateSpam(spamdelay) << "updating units with new sensor values";
233  publishNewSensorDataTime = TimeUtil::GetTime(true);
234  for (const RobotUnitSubUnitPtr& rsu : UnitsAttorneyForPublisher::GetSubUnits(this))
235  {
236  if (rsu && rsu->getObjectScheduler() && rsu->getProxy())
237  {
238  const auto begInner = TimeUtil::GetTime(true);
239  rsu->update(controlThreadOutputBuffer, activatedControllers);
240  const auto endInner = TimeUtil::GetTime(true);
241  timingMap["UnitUpdate_" + rsu->getName()] = new TimedVariant{
242  TimestampVariant{endInner - begInner}, lastControlThreadTimestamp};
243  }
244  }
245 
246  const auto end = TimeUtil::GetTime(true);
247  return new TimedVariant{TimestampVariant{end - beg}, lastControlThreadTimestamp};
248  }
249 
251  Publisher::publishControlUpdates(const SensorAndControl& controlThreadOutputBuffer,
252  bool haveSensorAndControlValsChanged,
253  bool publishToObserver,
254  const JointAndNJointControllers& activatedControllers,
255  const std::vector<JointController*>& requestedJointControllers)
256  {
257  ARMARX_TRACE;
258  throwIfInControlThread(BOOST_CURRENT_FUNCTION);
259  const auto beg = armarx::rtNow();
260 
261 
262  StringVariantBaseMap ctrlDevMap;
263  ControlDeviceStatusSeq allStatus;
264  for (std::size_t ctrlidx = 0; ctrlidx < _module<Devices>().getNumberOfControlDevices();
265  ++ctrlidx)
266  {
267  const ControlDevice& ctrlDev = *(_module<Devices>().getControlDevices().at(ctrlidx));
268 
269  StringToStringVariantBaseMapMap variants;
270  ControlDeviceStatus status;
271  const auto activeJointCtrl = activatedControllers.jointControllers.at(ctrlidx);
272  status.activeControlMode = activeJointCtrl
273  ? activeJointCtrl->getControlMode()
274  : std::string{"!!JointController is nullptr!!"};
275  status.deviceName = ctrlDev.getDeviceName();
276  if (activeJointCtrl)
277  {
278  auto additionalDatafields =
279  activeJointCtrl->publish(debugDrawerPrx, debugObserverPrx);
280  for (auto& pair : additionalDatafields)
281  {
282  ctrlDevMap[ctrlDev.getDeviceName() + "_" + activeJointCtrl->getControlMode() +
283  "_" + pair.first] = pair.second;
284  }
285  }
286 
287  for (const auto& ctrlVal : controlThreadOutputBuffer.control.at(ctrlidx))
288  {
289  const auto& controlMode = ctrlVal->getControlMode();
290  variants[controlMode] = ctrlVal->toVariants(lastControlThreadTimestamp);
291  if (haveSensorAndControlValsChanged && !variants[controlMode].empty() &&
292  observerPublishControlTargets && publishToObserver)
293  {
294  for (const auto& pair : variants[controlMode])
295  {
296  ctrlDevMap[status.deviceName + "_" + pair.first] = pair.second;
297  }
298  }
299  }
300 
301  ARMARX_CHECK_EXPRESSION(requestedJointControllers.at(ctrlidx));
302  status.requestedControlMode = requestedJointControllers.at(ctrlidx)->getControlMode();
303  status.controlTargetValues = std::move(variants);
304  status.timestampUSec = lastControlThreadTimestamp.toMicroSeconds();
305  allStatus.emplace_back(status);
306  }
307 
308  robotUnitListenerBatchPrx->controlDeviceStatusChanged(allStatus);
309  if (observerPublishControlTargets)
310  {
311  if (robotUnitObserver->getState() >= eManagedIceObjectStarted)
312  {
313  robotUnitObserver->offerOrUpdateDataFieldsFlatCopy_async(
314  robotUnitObserver->controlDevicesChannel, std::move(ctrlDevMap));
315  }
316  }
317 
318  const auto end = armarx::rtNow();
319  return new TimedVariant{TimestampVariant{end - beg}, lastControlThreadTimestamp};
320  }
321 
323  Publisher::publishSensorUpdates(bool publishToObserver,
324  const SensorAndControl& controlThreadOutputBuffer)
325  {
326  ARMARX_TRACE;
327  throwIfInControlThread(BOOST_CURRENT_FUNCTION);
328  const auto beg = TimeUtil::GetTime(true);
329 
330  StringVariantBaseMap sensorDevMap;
331  const auto numSensorDev = _module<Devices>().getNumberOfSensorDevices();
332  ARMARX_CHECK_EQUAL(controlThreadOutputBuffer.sensors.size(),
333  _module<Devices>().getNumberOfSensorDevices());
334  SensorDeviceStatusSeq allStatus;
335  for (std::size_t sensidx = 0; sensidx < numSensorDev; ++sensidx)
336  {
337  ARMARX_TRACE;
338  const SensorValueBase& sensVal = *(controlThreadOutputBuffer.sensors.at(sensidx));
339  auto variants = sensVal.toVariants(lastControlThreadTimestamp);
340 
341  if (!variants.empty())
342  {
343  SensorDeviceStatus status;
344  status.deviceName = DevicesAttorneyForPublisher::GetSensorDeviceName(this, sensidx);
345 
346  ARMARX_TRACE;
347  if (observerPublishSensorValues && publishToObserver)
348  {
349  for (const auto& pair : variants)
350  {
351  sensorDevMap[status.deviceName + "_" + pair.first] = pair.second;
352  }
353  }
354 
355  status.sensorValue = std::move(variants);
356  status.timestampUSec = lastControlThreadTimestamp.toMicroSeconds();
357  allStatus.emplace_back(status);
358  }
359  }
360 
361  ARMARX_TRACE;
362  robotUnitListenerBatchPrx->sensorDeviceStatusChanged(allStatus);
363  if (observerPublishSensorValues)
364  {
365  ARMARX_TRACE;
366  if (robotUnitObserver->getState() >= eManagedIceObjectStarted)
367  {
368  std::stringstream s;
369  for (auto& pair : sensorDevMap)
370  {
371  s << pair.first << ": \t"
372  << (pair.second ? pair.second->ice_id() + "\t with datatype \t" +
373  Variant::typeToString(pair.second->getType())
374  : "NULL")
375  << "\n";
376  }
377  ARMARX_DEBUG << deactivateSpam(spamdelay)
378  << "Sensor Datafieldnames: " << ARMARX_STREAM_PRINTER
379  {
380  for (auto& pair : sensorDevMap)
381  {
382  out << pair.first << ": "
383  << (pair.second ? pair.second->ice_id() +
384  Variant::typeToString(pair.second->getType())
385  : "NULL")
386  << "\n";
387  }
388  };
389  robotUnitObserver->offerOrUpdateDataFieldsFlatCopy_async(
390  robotUnitObserver->sensorDevicesChannel, std::move(sensorDevMap));
391  }
392  }
393  ARMARX_TRACE;
394 
395  const auto end = TimeUtil::GetTime(true);
396  return new TimedVariant{TimestampVariant{end - beg}, lastControlThreadTimestamp};
397  }
398 
399  std::string
400  Publisher::getRobotUnitListenerTopicName(const Ice::Current&) const
401  {
402  ARMARX_TRACE;
403  throwIfInControlThread(BOOST_CURRENT_FUNCTION);
404  return robotUnitListenerTopicName;
405  }
406 
407  std::string
408  Publisher::getDebugDrawerTopicName(const Ice::Current&) const
409  {
410  ARMARX_TRACE;
411  throwIfInControlThread(BOOST_CURRENT_FUNCTION);
412  return debugDrawerUpdatesTopicName;
413  }
414 
415  std::string
416  Publisher::getDebugObserverTopicName(const Ice::Current&) const
417  {
418  ARMARX_TRACE;
419  throwIfInControlThread(BOOST_CURRENT_FUNCTION);
420  return debugObserverTopicName;
421  }
422 
423  RobotUnitListenerPrx
424  Publisher::getRobotUnitListenerProxy(const Ice::Current&) const
425  {
426  ARMARX_TRACE;
427  throwIfInControlThread(BOOST_CURRENT_FUNCTION);
428  ARMARX_CHECK_EXPRESSION(robotUnitListenerPrx);
429  return robotUnitListenerPrx;
430  }
431 
433  Publisher::getDebugObserverProxy(const Ice::Current&) const
434  {
435  ARMARX_TRACE;
436  throwIfInControlThread(BOOST_CURRENT_FUNCTION);
437  ARMARX_CHECK_EXPRESSION(debugObserverPrx);
438  return debugObserverPrx;
439  }
440 
442  Publisher::getDebugDrawerProxy(const Ice::Current&) const
443  {
444  ARMARX_TRACE;
445  throwIfInControlThread(BOOST_CURRENT_FUNCTION);
446  ARMARX_CHECK_EXPRESSION(debugDrawerPrx);
447  return debugDrawerPrx;
448  }
449 
450  void
451  Publisher::_preOnConnectRobotUnit()
452  {
453  ARMARX_TRACE;
454  throwIfInControlThread(BOOST_CURRENT_FUNCTION);
455  ARMARX_DEBUG << "retrieving RobotUnitListenerPrx for topic "
457  robotUnitListenerPrx = getTopic<RobotUnitListenerPrx>(getRobotUnitListenerTopicName());
458  robotUnitListenerBatchPrx = robotUnitListenerPrx->ice_batchOneway();
459 
460  ARMARX_DEBUG << "retrieving DebugDrawerInterfacePrx for topic "
462  debugDrawerPrx = getTopic<DebugDrawerInterfacePrx>(getDebugDrawerTopicName());
463 
464  ARMARX_DEBUG << "retrieving DebugObserverInterfacePrx for topic "
466  debugObserverPrx = getTopic<DebugObserverInterfacePrx>(getDebugObserverTopicName());
467  }
468 
469  void
470  Publisher::_preOnInitRobotUnit()
471  {
472  ARMARX_TRACE;
473  throwIfInControlThread(BOOST_CURRENT_FUNCTION);
474  robotUnitListenerTopicName =
475  getProperty<std::string>("RobotUnitListenerTopicName").getValue();
476  debugDrawerUpdatesTopicName =
477  getProperty<std::string>("DebugDrawerUpdatesTopicName").getValue();
478  debugObserverTopicName = getProperty<std::string>("DebugObserverTopicName").getValue();
479 
480  observerEnablePublishing = getProperty<bool>("ObserverEnablePublishing").getValue();
481  observerPublishSensorValues = getProperty<bool>("ObserverPublishSensorValues").getValue();
482  observerPublishControlTargets =
483  getProperty<bool>("ObserverPublishControlTargets").getValue();
484  observerPublishTimingInformation =
485  getProperty<bool>("ObserverPublishTimingInformation").getValue();
486  observerPublishAdditionalInformation =
487  getProperty<bool>("ObserverPublishAdditionalInformation").getValue();
488  debugObserverSkipIterations =
489  std::max(1ul, getProperty<std::uint64_t>("ObserverPrintEveryNthIterations").getValue());
490 
491  publishPeriodMs = std::max(1ul, getProperty<std::size_t>("PublishPeriodMs").getValue());
492 
496  getArmarXManager()->addObject(robotUnitObserver);
497  }
498 
499  void
500  Publisher::_icePropertiesInitialized()
501  {
502  ARMARX_TRACE;
503  throwIfInControlThread(BOOST_CURRENT_FUNCTION);
504  robotUnitObserver = Component::create<RobotUnitObserver, RobotUnitObserverPtr>(
506  addPropertyUser(PropertyUserPtr::dynamicCast(robotUnitObserver));
507  }
508 
509  void
510  Publisher::_componentPropertiesUpdated(const std::set<std::string>& changedProperties)
511  {
512  ARMARX_TRACE;
513  throwIfInControlThread(BOOST_CURRENT_FUNCTION);
514  if (changedProperties.count("ObserverPublishSensorValues"))
515  {
516  observerPublishSensorValues =
517  getProperty<bool>("ObserverPublishSensorValues").getValue();
518  }
519  if (changedProperties.count("ObserverPublishControlTargets"))
520  {
521  observerPublishControlTargets =
522  getProperty<bool>("ObserverPublishControlTargets").getValue();
523  }
524  if (changedProperties.count("ObserverPublishTimingInformation"))
525  {
526  observerPublishTimingInformation =
527  getProperty<bool>("ObserverPublishTimingInformation").getValue();
528  }
529  if (changedProperties.count("ObserverPublishAdditionalInformation"))
530  {
531  observerPublishAdditionalInformation =
532  getProperty<bool>("ObserverPublishAdditionalInformation").getValue();
533  }
534  if (changedProperties.count("ObserverPrintEveryNthIterations"))
535  {
536  debugObserverSkipIterations =
537  getProperty<std::uint64_t>("ObserverPrintEveryNthIterations").getValue();
538  }
539  }
540 
541  void
542  Publisher::_preFinishRunning()
543  {
544  ARMARX_TRACE;
545  throwIfInControlThread(BOOST_CURRENT_FUNCTION);
546  if (publisherTask)
547  {
548  ARMARX_DEBUG << "shutting down publisher task";
549  publisherTask->stop();
550  std::this_thread::sleep_for(std::chrono::milliseconds{10});
551  while (publisherTask->isFunctionExecuting() || publisherTask->isRunning())
552  {
554  << "PUBLISHER TASK IS RUNNING AFTER IT WAS STOPPED!";
555  }
556  publisherTask = nullptr;
557  //since the drawer queues draw events and we want to clear the layers, we have to sleep here
558  std::this_thread::sleep_for(std::chrono::milliseconds{100});
559  ARMARX_DEBUG << "shutting down publisher task done";
560  }
561  for (const auto& pair :
562  ControllerManagementAttorneyForPublisher::GetNJointControllers(this))
563  {
564  ARMARX_DEBUG << "forcing deactivation of publishing for " << pair.first;
565  NJointControllerAttorneyForPublisher::DeactivatePublishing(
566  pair.second, debugDrawerPrx, debugObserverPrx);
567  }
568  }
569 
570  void
571  Publisher::_postFinishControlThreadInitialization()
572  {
573  ARMARX_TRACE;
574  //start publisher
575  publishNewSensorDataTime = TimeUtil::GetTime(true);
576  publisherTask = new PublisherTaskT(
577  [&] { publish({}); }, publishPeriodMs, false, getName() + "_PublisherTask");
578  ARMARX_INFO << "starting publisher with timestep " << publishPeriodMs;
579  publisherTask->setDelayWarningTolerance(10 * publishPeriodMs);
580  if (observerEnablePublishing)
581  {
582  publisherTask->start();
583  }
584  }
585 
586  void
588  {
589  ARMARX_TRACE;
590  throwIfInControlThread(BOOST_CURRENT_FUNCTION);
591  auto begPublish = TimeUtil::GetTime(true);
592 
594  {
595  ARMARX_DEBUG << deactivateSpam(spamdelay) << "skipping publishing in state "
596  << getRobotUnitState();
597  return;
598  }
599  if (isShuttingDown())
600  {
601  return;
602  }
603  GuardType guard;
604  try
605  {
606  guard = getGuard();
607  }
608  catch (...)
609  {
610  //shutting down
611  return;
612  }
614 
615  //maybe change the emergency stop state
616  ControlThreadAttorneyForPublisher::ProcessEmergencyStopRequest(this);
617 
618  //get batch proxies
619  //delete NJoint queued for deletion
620  ControllerManagementAttorneyForPublisher::RemoveNJointControllersToBeDeleted(
621  this, false, robotUnitListenerBatchPrx);
622  //swap buffers in
623  const bool haveActivatedControllersChanged =
624  _module<ControlThreadDataBuffer>().activatedControllersChanged();
625  const bool haveSensorAndControlValsChanged =
626  _module<ControlThreadDataBuffer>().sensorAndControlBufferChanged();
627  //setup datastructures
628  const auto& controlThreadOutputBuffer =
629  _module<ControlThreadDataBuffer>().getSensorAndControlBuffer(); ///TODO copy buffer
630 
631  const auto activatedControllers =
632  _module<ControlThreadDataBuffer>().getActivatedControllers();
633  const auto requestedJointControllers =
634  _module<ControlThreadDataBuffer>().copyRequestedJointControllers();
635 
636  // controlThreadOutputBuffer.sensorValuesTimestamp is in MONOTONIC_RAW (not relative to epoch).
637  // We have to map it to be relative to epoch (REAL_TIME).
638  lastControlThreadTimestamp =
639  armarx::mapRtTimestampToNonRtTimestamp(controlThreadOutputBuffer.sensorValuesTimestamp);
640 
641  const bool publishToObserver = !(publishIterationCount % debugObserverSkipIterations);
642  //publish publishing meta state
643  additionalMap["haveActivatedControllersChanged"] =
644  new TimedVariant{haveActivatedControllersChanged, lastControlThreadTimestamp};
645  additionalMap["haveSensorAndControlValsChanged"] =
646  new TimedVariant{haveSensorAndControlValsChanged, lastControlThreadTimestamp};
647  additionalMap["publishToObserver"] =
648  new TimedVariant{publishToObserver, lastControlThreadTimestamp};
649  additionalMap["SoftwareEmergencyStopState"] =
650  new TimedVariant{_module<Units>().getEmergencyStopMaster()->getEmergencyStopState() ==
651  EmergencyStopState::eEmergencyStopActive
652  ? "EmergencyStopActive"
653  : "EmergencyStopInactive",
654  lastControlThreadTimestamp};
655 
656 
657  //update
658  if (haveSensorAndControlValsChanged)
659  {
660  timingMap["UnitUpdate"] =
661  publishUnitUpdates(timingMap, controlThreadOutputBuffer, activatedControllers);
662  timingMap["SensorUpdates"] =
663  publishSensorUpdates(publishToObserver, controlThreadOutputBuffer);
664  }
665  else
666  {
667  const double sensSecondsAgo =
668  (TimeUtil::GetTime() - publishNewSensorDataTime).toSecondsDouble();
669  if (sensSecondsAgo > 1)
670  {
671  ARMARX_WARNING << deactivateSpam(spamdelay)
672  << "armarx::RobotUnit::publish: Last sensor value update is "
673  << sensSecondsAgo << " seconds ago!";
674  }
675  }
676  if (haveSensorAndControlValsChanged || haveActivatedControllersChanged)
677  {
678  timingMap["ControlUpdates"] = publishControlUpdates(controlThreadOutputBuffer,
679  haveSensorAndControlValsChanged,
680  publishToObserver,
681  activatedControllers,
682  requestedJointControllers);
683  }
684  //call publish hook + publish NJointControllerBase changes
685  timingMap["NJointControllerUpdates"] =
686  publishNJointControllerUpdates(timingMap, controlThreadOutputBuffer);
687 
688  //report new class names
689  timingMap["ClassNameUpdates"] = publishNJointClassNames();
690  timingMap["RobotUnitListenerFlush"] = new TimedVariant{
691  ARMARX_STOPWATCH(TimestampVariant){robotUnitListenerBatchPrx->ice_flushBatchRequests();
692  }
693 
694  , lastControlThreadTimestamp
695 }; // namespace armarx::RobotUnitModule
696 
697 if (publishToObserver)
698 {
699  timingMap["LastPublishLoop"] =
700  new TimedVariant{TimestampVariant{lastPublishLoop}, lastControlThreadTimestamp};
701  if (robotUnitObserver->getState() >= eManagedIceObjectStarted)
702  {
703  if (observerPublishTimingInformation)
704  {
705  robotUnitObserver->offerOrUpdateDataFieldsFlatCopy_async(
706  robotUnitObserver->additionalChannel, std::move(additionalMap));
707  }
708  if (observerPublishAdditionalInformation)
709  {
710  robotUnitObserver->offerOrUpdateDataFieldsFlatCopy_async(
711  robotUnitObserver->timingChannel, std::move(timingMap));
712  }
713  }
714 }
715 
716 //warn if there are unset jointCtrl controllers
717 {
718  const auto& jointCtrls = _module<ControlThreadDataBuffer>().getActivatedJointControllers();
719  if (std::any_of(jointCtrls.begin(),
720  jointCtrls.end(),
721  [](const JointController* jointCtrl) { return !jointCtrl; }))
722  {
724  << "armarx::RobotUnit::publish: Some activated JointControllers are "
725  "reported to be nullptr! "
726  << "(did you forget to call rtSwitchControllerSetup()?)";
727  }
728 }
729 ++publishIterationCount;
730 lastPublishLoop = TimeUtil::GetTime(true) - begPublish;
731 }
732 }
armarx::RobotUnitModule::ModuleBase::isShuttingDown
bool isShuttingDown() const
Returns whether the RobotUnit is shutting down.
Definition: RobotUnitModuleBase.h:647
armarx::RobotUnitModule::ControllerManagementAttorneyForPublisher
This class allows minimal access to private members of ControllerManagement in a sane fashion for Pub...
Definition: RobotUnitModulePublisher.cpp:121
RobotUnitModuleControllerManagement.h
RobotUnitModuleUnits.h
armarx::RobotUnitModule::DevicesAttorneyForPublisher
This class allows minimal access to private members of Devices in a sane fashion for Publisher.
Definition: RobotUnitModulePublisher.cpp:91
armarx::StringVariantBaseMap
std::map< std::string, VariantBasePtr > StringVariantBaseMap
Definition: ManagedIceObject.h:111
armarx::Component::addPropertyUser
void addPropertyUser(const PropertyUserPtr &subPropertyUser)
Add additional property users here that should show up in the application help text.
Definition: Component.cpp:140
NJointControllerBase.h
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
armarx::RobotUnitModule::Units
This Module manages all Units of a RobotUnit.
Definition: RobotUnitModuleUnits.h:123
armarx::ManagedIceObject::getArmarXManager
ArmarXManagerPtr getArmarXManager() const
Returns the ArmarX manager used to add and remove components.
Definition: ManagedIceObject.cpp:348
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::TimestampVariant
Definition: TimestampVariant.h:54
armarx::DebugObserverInterfacePrx
::IceInternal::ProxyHandle<::IceProxy::armarx::DebugObserverInterface > DebugObserverInterfacePrx
Definition: JointController.h:44
armarx::JointController
The JointController class represents one joint in one control mode.
Definition: JointController.h:51
armarx::RobotUnitModule::ControlThreadAttorneyForPublisher
This class allows minimal access to private members of ControlThread in a sane fashion for Publisher.
Definition: RobotUnitModulePublisher.cpp:142
armarx::SensorAndControl
detail::ControlThreadOutputBufferEntry SensorAndControl
Definition: NJointControllerBase.h:72
armarx::RobotUnitModule::ModuleBase::getRobotUnitState
RobotUnitState getRobotUnitState() const
Returns the RobotUnit's State.
Definition: RobotUnitModuleBase.ipp:29
StringHelpers.h
ARMARX_STOPWATCH
#define ARMARX_STOPWATCH(...)
Definition: Time.h:145
armarx::RobotUnitModule::Publisher::publish
virtual void publish(StringVariantBaseMap additionalMap={}, StringVariantBaseMap timingMap={})
Publishes data.
Definition: RobotUnitModulePublisher.cpp:587
cxxopts::empty
bool empty(const std::string &s)
Definition: cxxopts.hpp:255
ARMARX_FATAL
#define ARMARX_FATAL
Definition: Logging.h:192
NonRtTiming.h
ARMARX_TRACE
#define ARMARX_TRACE
Definition: trace.h:69
armarx::status
status
Definition: FiniteStateMachine.h:259
armarx::RobotUnitModule::ControlThread
This Module manages the ControlThread.
Definition: RobotUnitModuleControlThread.h:83
armarx::Registrar::getKeys
static std::vector< KeyType > getKeys()
Retrieves the list of all registered elements.
Definition: Registrar.h:115
armarx::RobotUnitState::Running
@ Running
RobotUnitModulePublisher.h
armarx::RobotUnitModule::NJointControllerAttorneyForPublisher
This class allows minimal access to private members of NJointControllerBase in a sane fashion for Pub...
Definition: RobotUnitModulePublisher.cpp:46
RobotUnitModuleDevices.h
RobotUnitModuleControlThread.h
ARMARX_DEBUG
#define ARMARX_DEBUG
Definition: Logging.h:177
armarx::RobotUnitModule::Publisher::getDebugDrawerProxy
DebugDrawerInterfacePrx getDebugDrawerProxy(const Ice::Current &=Ice::emptyCurrent) const override
Returns the used DebugDrawerProxy.
Definition: RobotUnitModulePublisher.cpp:442
armarx::RobotUnitModule::UnitsAttorneyForPublisher
This class allows minimal access to private members of Units in a sane fashion for Publisher.
Definition: RobotUnitModulePublisher.cpp:106
RobotUnitModuleControlThreadDataBuffer.h
armarx::Variant::typeToString
static std::string typeToString(VariantTypeId typeId)
Return the name of the registered type typeId.
Definition: Variant.cpp:732
armarx::RobotUnitModule::ModuleBase::getGuard
GuardType getGuard() const
Returns a guard to the RobotUnits mutex.
Definition: RobotUnitModuleBase.cpp:430
RobotUnitSubUnit.h
ArmarXObjectScheduler.h
max
T max(T t1, T t2)
Definition: gdiam.h:48
armarx::Component::getConfigDomain
std::string getConfigDomain()
Retrieve config domain for this component as set in constructor.
Definition: Component.cpp:60
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::TimeUtil::GetTime
static IceUtil::Time GetTime(TimeMode timeMode=TimeMode::VirtualTime)
Get the current time.
Definition: TimeUtil.cpp:42
ExpressionException.h
armarx::TimedVariantPtr
IceInternal::Handle< TimedVariant > TimedVariantPtr
Definition: TimedVariant.h:38
armarx::RobotUnitModule::Devices
This Module manages sensor and control devices for a RobotUnit and only allows save and sane access.
Definition: RobotUnitModuleDevices.h:67
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_INFO
#define ARMARX_INFO
Definition: Logging.h:174
armarx::ManagedIceObject::offeringTopic
void offeringTopic(const std::string &name)
Registers a topic for retrival after initialization.
Definition: ManagedIceObject.cpp:290
IceInternal::ProxyHandle<::IceProxy::armarx::DebugObserverInterface >
armarx::RobotUnitModule::ControllerManagement
This Module manages NJointControllers.
Definition: RobotUnitModuleControllerManagement.h:39
armarx::RobotUnitModule::ModuleBase::GuardType
std::unique_lock< MutexType > GuardType
Definition: RobotUnitModuleBase.h:226
armarx::RobotUnitModule::ModuleBase::_module
T & _module()
Returns this as ref to the given type.
Definition: RobotUnitModuleBase.h:249
armarx::RobotUnitModule::ModuleBase::throwIfInControlThread
void throwIfInControlThread(const std::string &fnc) const
Throws if the current thread is the ControlThread.
Definition: RobotUnitModuleBase.cpp:416
armarx::Logging::deactivateSpam
SpamFilterDataPtr deactivateSpam(float deactivationDurationSec=10.0f, const std::string &identifier="", bool deactivate=true) const
disables the logging for the current line for the given amount of seconds.
Definition: Logging.cpp:92
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::PropertyUser::getIceProperties
Ice::PropertiesPtr getIceProperties() const
Returns the set of Ice properties.
Definition: PropertyUser.cpp:229
armarx::RobotUnitModule::Publisher::getRobotUnitListenerProxy
RobotUnitListenerPrx getRobotUnitListenerProxy(const Ice::Current &=Ice::emptyCurrent) const override
Returns the used RobotUnitListenerProxy.
Definition: RobotUnitModulePublisher.cpp:424
armarx::mapRtTimestampToNonRtTimestamp
IceUtil::Time mapRtTimestampToNonRtTimestamp(const IceUtil::Time &time_monotic_raw)
Definition: NonRtTiming.h:46
Logging.h
ARMARX_CHECK_EQUAL
#define ARMARX_CHECK_EQUAL(lhs, rhs)
This macro evaluates whether lhs is equal (==) rhs and if it turns out to be false it will throw an E...
Definition: ExpressionException.h:130
ARMARX_WARNING
#define ARMARX_WARNING
Definition: Logging.h:186
armarx::DebugDrawerInterfacePrx
::IceInternal::ProxyHandle<::IceProxy::armarx::DebugDrawerInterface > DebugDrawerInterfacePrx
Definition: JointController.h:40
armarx::ctrlutil::s
double s(double t, double s0, double v0, double a0, double j)
Definition: CtrlUtil.h:33
NJointControllerRegistry.h
armarx::TimedVariant
Definition: TimedVariant.h:40
ARMARX_STREAM_PRINTER
#define ARMARX_STREAM_PRINTER
use this macro to write output code that is executed when printed and thus not executed if the debug ...
Definition: Logging.h:304
armarx::rtNow
IceUtil::Time rtNow()
Definition: RtTiming.h:40