51 GetStatusReportedActive(
const NJointControllerBasePtr& nJointCtrl)
53 return nJointCtrl->statusReportedActive;
57 GetStatusReportedRequested(
const NJointControllerBasePtr& nJointCtrl)
59 return nJointCtrl->statusReportedRequested;
63 UpdateStatusReported(
const NJointControllerBasePtr& nJointCtrl)
65 nJointCtrl->statusReportedActive = nJointCtrl->isActive.load();
66 nJointCtrl->statusReportedRequested = nJointCtrl->isRequested.load();
70 Publish(
const NJointControllerBasePtr& nJointCtrl,
75 nJointCtrl->
publish(sac, draw, observer);
79 DeactivatePublishing(
const NJointControllerBasePtr& nJointCtrl,
83 nJointCtrl->deactivatePublish(draw, observer);
95 static const std::string&
96 GetSensorDeviceName(
Publisher* p, std::size_t idx)
98 return p->
_module<
Devices>().sensorDevices.at(idx)->getDeviceName();
110 static const std::vector<RobotUnitSubUnitPtr>&
125 static const std::map<std::string, NJointControllerBasePtr>&
132 RemoveNJointControllersToBeDeleted(
Publisher* p,
bool blocking, RobotUnitListenerPrx l)
147 ProcessEmergencyStopRequest(
Publisher* p)
157 Publisher::publishNJointClassNames()
164 if (lastReportedClasses.size() != classNames.size())
166 for (
const auto& name : classNames)
168 if (!lastReportedClasses.count(name))
170 robotUnitListenerBatchPrx->nJointControllerClassAdded(name);
171 lastReportedClasses.emplace(name);
177 return new TimedVariant{TimestampVariant{end - beg}, lastControlThreadTimestamp};
189 auto debugDrawerBatchPrx = debugDrawerPrx->ice_batchOneway();
190 auto debugObserverBatchPrx = debugObserverPrx->ice_batchOneway();
191 NJointControllerStatusSeq allStatus;
192 for (
const auto& pair :
193 ControllerManagementAttorneyForPublisher::GetNJointControllers(
this))
196 const NJointControllerBasePtr& nJointCtrl = pair.second;
199 NJointControllerAttorneyForPublisher::Publish(
200 nJointCtrl, controlThreadOutputBuffer, debugDrawerBatchPrx, debugObserverBatchPrx);
201 if (NJointControllerAttorneyForPublisher::GetStatusReportedActive(nJointCtrl) !=
202 nJointCtrl->isControllerActive() ||
203 NJointControllerAttorneyForPublisher::GetStatusReportedRequested(nJointCtrl) !=
204 nJointCtrl->isControllerRequested())
206 NJointControllerAttorneyForPublisher::UpdateStatusReported(nJointCtrl);
207 allStatus.emplace_back(nJointCtrl->getControllerStatus());
211 timingMap[
"NJointControllerUpdates_" + nJointCtrl->getInstanceName()] =
212 new TimedVariant{TimestampVariant{endInner - begInner}, lastControlThreadTimestamp};
215 robotUnitListenerBatchPrx->nJointControllerStatusChanged(allStatus);
216 debugObserverBatchPrx->ice_flushBatchRequests();
217 debugDrawerBatchPrx->ice_flushBatchRequests();
220 return new TimedVariant{TimestampVariant{end - beg}, lastControlThreadTimestamp};
226 const JointAndNJointControllers& activatedControllers)
234 for (
const RobotUnitSubUnitPtr& rsu : UnitsAttorneyForPublisher::GetSubUnits(
this))
236 if (rsu && rsu->getObjectScheduler() && rsu->getProxy())
239 rsu->update(controlThreadOutputBuffer, activatedControllers);
241 timingMap[
"UnitUpdate_" + rsu->getName()] =
new TimedVariant{
242 TimestampVariant{endInner - begInner}, lastControlThreadTimestamp};
247 return new TimedVariant{TimestampVariant{end - beg}, lastControlThreadTimestamp};
251 Publisher::publishControlUpdates(
const SensorAndControl& controlThreadOutputBuffer,
252 bool haveSensorAndControlValsChanged,
253 bool publishToObserver,
254 const JointAndNJointControllers& activatedControllers,
255 const std::vector<JointController*>& requestedJointControllers)
263 ControlDeviceStatusSeq allStatus;
264 for (std::size_t ctrlidx = 0; ctrlidx < _module<Devices>().getNumberOfControlDevices();
267 const ControlDevice& ctrlDev = *(_module<Devices>().getControlDevices().at(ctrlidx));
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();
278 auto additionalDatafields =
279 activeJointCtrl->publish(debugDrawerPrx, debugObserverPrx);
280 for (
auto& pair : additionalDatafields)
282 ctrlDevMap[ctrlDev.getDeviceName() +
"_" + activeJointCtrl->getControlMode() +
283 "_" + pair.first] = pair.second;
287 for (
const auto& ctrlVal : controlThreadOutputBuffer.control.at(ctrlidx))
289 const auto& controlMode = ctrlVal->getControlMode();
290 variants[controlMode] = ctrlVal->toVariants(lastControlThreadTimestamp);
291 if (haveSensorAndControlValsChanged && !variants[controlMode].
empty() &&
292 observerPublishControlTargets && publishToObserver)
294 for (
const auto& pair : variants[controlMode])
296 ctrlDevMap[
status.deviceName +
"_" + pair.first] = pair.second;
302 status.requestedControlMode = requestedJointControllers.at(ctrlidx)->getControlMode();
303 status.controlTargetValues = std::move(variants);
304 status.timestampUSec = lastControlThreadTimestamp.toMicroSeconds();
305 allStatus.emplace_back(
status);
308 robotUnitListenerBatchPrx->controlDeviceStatusChanged(allStatus);
309 if (observerPublishControlTargets)
311 if (robotUnitObserver->getState() >= eManagedIceObjectStarted)
313 robotUnitObserver->offerOrUpdateDataFieldsFlatCopy_async(
314 robotUnitObserver->controlDevicesChannel, std::move(ctrlDevMap));
319 return new TimedVariant{TimestampVariant{end - beg}, lastControlThreadTimestamp};
323 Publisher::publishSensorUpdates(
bool publishToObserver,
331 const auto numSensorDev = _module<Devices>().getNumberOfSensorDevices();
333 _module<Devices>().getNumberOfSensorDevices());
334 SensorDeviceStatusSeq allStatus;
335 for (std::size_t sensidx = 0; sensidx < numSensorDev; ++sensidx)
338 const SensorValueBase& sensVal = *(controlThreadOutputBuffer.sensors.at(sensidx));
339 auto variants = sensVal.toVariants(lastControlThreadTimestamp);
341 if (!variants.empty())
343 SensorDeviceStatus
status;
344 status.deviceName = DevicesAttorneyForPublisher::GetSensorDeviceName(
this, sensidx);
347 if (observerPublishSensorValues && publishToObserver)
349 for (
const auto& pair : variants)
351 sensorDevMap[
status.deviceName +
"_" + pair.first] = pair.second;
355 status.sensorValue = std::move(variants);
356 status.timestampUSec = lastControlThreadTimestamp.toMicroSeconds();
357 allStatus.emplace_back(
status);
362 robotUnitListenerBatchPrx->sensorDeviceStatusChanged(allStatus);
363 if (observerPublishSensorValues)
366 if (robotUnitObserver->getState() >= eManagedIceObjectStarted)
369 for (
auto& pair : sensorDevMap)
371 s << pair.first <<
": \t"
372 << (pair.second ? pair.second->ice_id() +
"\t with datatype \t" +
380 for (
auto& pair : sensorDevMap)
382 out << pair.first <<
": "
383 << (pair.second ? pair.second->ice_id() +
389 robotUnitObserver->offerOrUpdateDataFieldsFlatCopy_async(
390 robotUnitObserver->sensorDevicesChannel, std::move(sensorDevMap));
396 return new TimedVariant{TimestampVariant{end - beg}, lastControlThreadTimestamp};
404 return robotUnitListenerTopicName;
412 return debugDrawerUpdatesTopicName;
420 return debugObserverTopicName;
429 return robotUnitListenerPrx;
438 return debugObserverPrx;
447 return debugDrawerPrx;
451 Publisher::_preOnConnectRobotUnit()
455 ARMARX_DEBUG <<
"retrieving RobotUnitListenerPrx for topic "
458 robotUnitListenerBatchPrx = robotUnitListenerPrx->ice_batchOneway();
460 ARMARX_DEBUG <<
"retrieving DebugDrawerInterfacePrx for topic "
464 ARMARX_DEBUG <<
"retrieving DebugObserverInterfacePrx for topic "
470 Publisher::_preOnInitRobotUnit()
474 robotUnitListenerTopicName =
475 getProperty<std::string>(
"RobotUnitListenerTopicName").getValue();
476 debugDrawerUpdatesTopicName =
477 getProperty<std::string>(
"DebugDrawerUpdatesTopicName").getValue();
478 debugObserverTopicName = getProperty<std::string>(
"DebugObserverTopicName").getValue();
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());
491 publishPeriodMs =
std::max(1ul, getProperty<std::size_t>(
"PublishPeriodMs").getValue());
500 Publisher::_icePropertiesInitialized()
504 robotUnitObserver = Component::create<RobotUnitObserver, RobotUnitObserverPtr>(
510 Publisher::_componentPropertiesUpdated(
const std::set<std::string>& changedProperties)
514 if (changedProperties.count(
"ObserverPublishSensorValues"))
516 observerPublishSensorValues =
517 getProperty<bool>(
"ObserverPublishSensorValues").getValue();
519 if (changedProperties.count(
"ObserverPublishControlTargets"))
521 observerPublishControlTargets =
522 getProperty<bool>(
"ObserverPublishControlTargets").getValue();
524 if (changedProperties.count(
"ObserverPublishTimingInformation"))
526 observerPublishTimingInformation =
527 getProperty<bool>(
"ObserverPublishTimingInformation").getValue();
529 if (changedProperties.count(
"ObserverPublishAdditionalInformation"))
531 observerPublishAdditionalInformation =
532 getProperty<bool>(
"ObserverPublishAdditionalInformation").getValue();
534 if (changedProperties.count(
"ObserverPrintEveryNthIterations"))
536 debugObserverSkipIterations =
537 getProperty<std::uint64_t>(
"ObserverPrintEveryNthIterations").getValue();
542 Publisher::_preFinishRunning()
549 publisherTask->stop();
550 std::this_thread::sleep_for(std::chrono::milliseconds{10});
551 while (publisherTask->isFunctionExecuting() || publisherTask->isRunning())
554 <<
"PUBLISHER TASK IS RUNNING AFTER IT WAS STOPPED!";
556 publisherTask =
nullptr;
558 std::this_thread::sleep_for(std::chrono::milliseconds{100});
561 for (
const auto& pair :
562 ControllerManagementAttorneyForPublisher::GetNJointControllers(
this))
564 ARMARX_DEBUG <<
"forcing deactivation of publishing for " << pair.first;
565 NJointControllerAttorneyForPublisher::DeactivatePublishing(
566 pair.second, debugDrawerPrx, debugObserverPrx);
571 Publisher::_postFinishControlThreadInitialization()
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)
582 publisherTask->start();
616 ControlThreadAttorneyForPublisher::ProcessEmergencyStopRequest(
this);
620 ControllerManagementAttorneyForPublisher::RemoveNJointControllersToBeDeleted(
621 this,
false, robotUnitListenerBatchPrx);
623 const bool haveActivatedControllersChanged =
624 _module<ControlThreadDataBuffer>().activatedControllersChanged();
625 const bool haveSensorAndControlValsChanged =
626 _module<ControlThreadDataBuffer>().sensorAndControlBufferChanged();
628 const auto& controlThreadOutputBuffer =
629 _module<ControlThreadDataBuffer>().getSensorAndControlBuffer();
631 const auto activatedControllers =
632 _module<ControlThreadDataBuffer>().getActivatedControllers();
633 const auto requestedJointControllers =
634 _module<ControlThreadDataBuffer>().copyRequestedJointControllers();
638 lastControlThreadTimestamp =
641 const bool publishToObserver = !(publishIterationCount % debugObserverSkipIterations);
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};
658 if (haveSensorAndControlValsChanged)
660 timingMap[
"UnitUpdate"] =
661 publishUnitUpdates(timingMap, controlThreadOutputBuffer, activatedControllers);
662 timingMap[
"SensorUpdates"] =
663 publishSensorUpdates(publishToObserver, controlThreadOutputBuffer);
667 const double sensSecondsAgo =
669 if (sensSecondsAgo > 1)
672 <<
"armarx::RobotUnit::publish: Last sensor value update is "
673 << sensSecondsAgo <<
" seconds ago!";
676 if (haveSensorAndControlValsChanged || haveActivatedControllersChanged)
678 timingMap[
"ControlUpdates"] = publishControlUpdates(controlThreadOutputBuffer,
679 haveSensorAndControlValsChanged,
681 activatedControllers,
682 requestedJointControllers);
685 timingMap[
"NJointControllerUpdates"] =
686 publishNJointControllerUpdates(timingMap, controlThreadOutputBuffer);
689 timingMap[
"ClassNameUpdates"] = publishNJointClassNames();
694 , lastControlThreadTimestamp
697 if (publishToObserver)
699 timingMap[
"LastPublishLoop"] =
701 if (robotUnitObserver->getState() >= eManagedIceObjectStarted)
703 if (observerPublishTimingInformation)
705 robotUnitObserver->offerOrUpdateDataFieldsFlatCopy_async(
706 robotUnitObserver->additionalChannel, std::move(additionalMap));
708 if (observerPublishAdditionalInformation)
710 robotUnitObserver->offerOrUpdateDataFieldsFlatCopy_async(
711 robotUnitObserver->timingChannel, std::move(timingMap));
718 const auto& jointCtrls = _module<ControlThreadDataBuffer>().getActivatedJointControllers();
719 if (std::any_of(jointCtrls.begin(),
724 <<
"armarx::RobotUnit::publish: Some activated JointControllers are "
725 "reported to be nullptr! "
726 <<
"(did you forget to call rtSwitchControllerSetup()?)";
729 ++publishIterationCount;