diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModulePublisher.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModulePublisher.cpp
index dc4b109411a3d8e21c60208a8e4f44ea86fe644d..b6aa80e65dd186fe7ae931e7846ce038b2a47523 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModulePublisher.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModulePublisher.cpp
@@ -29,21 +29,6 @@ namespace armarx
{
namespace RobotUnitModule
{
- namespace detail
- {
- std::function<std::string(const std::string&)> makeDebugObserverNameFixer(const std::string& prefix)
- {
- return
- [prefix](const std::string & name)
- {
- std::string s = prefix + name;
- std::replace(s.begin(), s.end(), '.', '_');
- std::replace(s.begin(), s.end(), ':', '_');
- return s;
- };
- }
- }
-
/**
* \brief This class allows minimal access to private members of \ref NJointController in a sane fashion for \ref Publisher.
* \warning !! DO NOT ADD ANYTHING IF YOU DO NOT KNOW WAHT YOU ARE DOING! IF YOU DO SOMETHING WRONG YOU WILL CAUSE UNDEFINED BEHAVIOUR !!
@@ -135,6 +120,185 @@ namespace armarx
return ControllerManagementAttorneyForPublisher::GetNJointControllers(this);
}
+ TimedVariantPtr Publisher::publishNJointClassNames(IceUtil::Time timestamp, RobotUnitListenerPrx &listenerPrx)
+ {
+ const auto beg = TimeUtil::GetTime(true);
+
+ const auto classNames = NJointControllerRegistry::getKeys();
+ if (lastReportedClasses.size() != classNames.size())
+ {
+ for (const auto& name : classNames)
+ {
+ if (!lastReportedClasses.count(name))
+ {
+ listenerPrx->nJointControllerClassAdded(name);
+ lastReportedClasses.emplace(name);
+ }
+ }
+ }
+
+ const auto end = TimeUtil::GetTime(true);
+ return new TimedVariant{TimestampVariant{end-beg}, timestamp};
+ }
+
+ TimedVariantPtr Publisher::publishNJointControllerUpdates(
+ IceUtil::Time timestamp,
+ RobotUnitListenerPrx& listenerPrx,
+ StringVariantBaseMap& timingMap,
+ const SensorAndControl& controlThreadOutputBuffer,
+ DebugObserverInterfacePrx& debugObserverBatchPrx
+ )
+ {
+ const auto beg = TimeUtil::GetTime(true);
+
+ for (const auto& pair : getNJointControllers())
+ {
+ const auto begInner = TimeUtil::GetTime(true);
+ const NJointControllerPtr& nJointCtrl = pair.second;
+
+ //run some hook for active (used for visu)
+ NJointControllerAttorneyForPublisher::Publish(nJointCtrl, controlThreadOutputBuffer, debugDrawerPrx, debugObserverBatchPrx);
+ if (
+ NJointControllerAttorneyForPublisher::GetStatusReportedActive(nJointCtrl) != nJointCtrl->isControllerActive() ||
+ NJointControllerAttorneyForPublisher::GetStatusReportedRequested(nJointCtrl) != nJointCtrl->isControllerRequested()
+ )
+ {
+ NJointControllerAttorneyForPublisher::UpdateStatusReported(nJointCtrl);
+ listenerPrx->nJointControllerStatusChanged(nJointCtrl->getControllerStatus());
+ }
+
+ const auto endInner = TimeUtil::GetTime(true);
+ timingMap["NJointControllerUpdates_" + nJointCtrl->getInstanceName()] = new TimedVariant{TimestampVariant{endInner-begInner}, timestamp};
+ }
+
+ const auto end = TimeUtil::GetTime(true);
+ return new TimedVariant{TimestampVariant{end-beg}, timestamp};
+ }
+
+ TimedVariantPtr Publisher::publishUnitUpdates(
+ IceUtil::Time timestamp,
+ StringVariantBaseMap &timingMap,
+ const SensorAndControl& controlThreadOutputBuffer,
+ const JointAndNJointControllers& activatedControllers)
+ {
+ const auto beg = TimeUtil::GetTime(true);
+
+ ARMARX_DEBUG << deactivateSpam(spamdelay) << "updating units with new sensor values";
+ publishNewSensorDataTime = TimeUtil::GetTime(true);
+ for (const RobotUnitSubUnitPtr& rsu : UnitsAttorneyForPublisher::GetSubUnits(this))
+ {
+ if (rsu && rsu->getObjectScheduler() && rsu->getProxy())
+ {
+ const auto begInner = TimeUtil::GetTime(true);
+ rsu->update(controlThreadOutputBuffer, activatedControllers);
+ const auto endInner = TimeUtil::GetTime(true);
+ timingMap["UnitUpdate_" + rsu->getName()] = new TimedVariant{TimestampVariant{endInner-begInner}, timestamp};
+ }
+ }
+
+ const auto end = TimeUtil::GetTime(true);
+ return new TimedVariant{TimestampVariant{end-beg}, timestamp};
+ }
+
+ TimedVariantPtr Publisher::publishControlUpdates(
+ IceUtil::Time timestamp,
+ RobotUnitListenerPrx& listenerPrx,
+ const SensorAndControl& controlThreadOutputBuffer,
+ bool haveSensorAndControlValsChanged,
+ bool publishToObserver,
+ const JointAndNJointControllers& activatedControllers,
+ const std::vector<JointController*>& requestedJointControllers)
+ {
+ const auto beg = TimeUtil::GetTime(true);
+
+
+ StringVariantBaseMap ctrlDevMap;
+ for (std::size_t ctrlidx = 0 ; ctrlidx < _module<Devices>().getNumberOfControlDevices(); ++ctrlidx)
+ {
+ const ControlDevice& ctrlDev = *(_module<Devices>().getControlDevices().at(ctrlidx));
+
+ StringToStringVariantBaseMapMap variants;
+ ControlDeviceStatus status;
+ const auto activeJointCtrl = activatedControllers.jointControllers.at(ctrlidx);
+ status.activeControlMode = activeJointCtrl ? activeJointCtrl->getControlMode() : std::string {"!!JointController is nullptr!!"};
+ status.deviceName = ctrlDev.getDeviceName();
+
+ for (const auto& ctrlVal : controlThreadOutputBuffer.control.at(ctrlidx))
+ {
+ const auto& controlMode = ctrlVal->getControlMode();
+ variants[controlMode] = ctrlVal->toVariants(timestamp);
+ if (
+ haveSensorAndControlValsChanged &&
+ !variants[controlMode].empty() &&
+ observerPublishControlTargets &&
+ publishToObserver
+ )
+ {
+ for(const auto& pair : variants[controlMode])
+ {
+ ctrlDevMap[status.deviceName + "_" + pair.first] = pair.second;
+ }
+ }
+ }
+
+ ARMARX_CHECK_EXPRESSION(requestedJointControllers.at(ctrlidx));
+ status.requestedControlMode = requestedJointControllers.at(ctrlidx)->getControlMode();
+ status.controlTargetValues = std::move(variants);
+ status.timestampUSec = MicroNow().count();
+ listenerPrx->controlDeviceStatusChanged(status);
+ }
+ if (robotUnitObserver->getState() >= eManagedIceObjectStarted)
+ {
+ robotUnitObserver->offerOrUpdateDataFieldsFlatCopy(robotUnitObserver->controlDevicesChannel, ctrlDevMap);
+ robotUnitObserver->updateChannel(robotUnitObserver->controlDevicesChannel);
+ }
+
+ const auto end = TimeUtil::GetTime(true);
+ return new TimedVariant{TimestampVariant{end-beg}, timestamp};
+ }
+
+ TimedVariantPtr Publisher::publishSensorUpdates(
+ IceUtil::Time timestamp,
+ RobotUnitListenerPrx &listenerPrx,
+ bool publishToObserver,
+ const SensorAndControl &controlThreadOutputBuffer)
+ {
+ const auto beg = TimeUtil::GetTime(true);
+
+ StringVariantBaseMap sensorDevMap;
+ for (std::size_t sensidx = 0 ; sensidx < _module<Devices>().getNumberOfSensorDevices(); ++sensidx)
+ {
+ const SensorValueBase& sensVal = *(controlThreadOutputBuffer.sensors.at(sensidx));
+ auto variants = sensVal.toVariants(timestamp);
+
+ if (!variants.empty())
+ {
+ SensorDeviceStatus status;
+ status.deviceName = DevicesAttorneyForPublisher::GetSensorDeviceName(this, sensidx);
+
+ if (observerPublishSensorValues && publishToObserver)
+ {
+ for(const auto& pair : variants)
+ {
+ sensorDevMap[status.deviceName + "_" + pair.first] = pair.second;
+ }
+ }
+
+ status.sensorValue = std::move(variants);
+ status.timestampUSec = MicroNow().count();
+ listenerPrx->sensorDeviceStatusChanged(status);
+ }
+ }
+ if (robotUnitObserver->getState() >= eManagedIceObjectStarted)
+ {
+ robotUnitObserver->offerOrUpdateDataFieldsFlatCopy(robotUnitObserver->sensorDevicesChannel, sensorDevMap);
+ robotUnitObserver->updateChannel(robotUnitObserver->sensorDevicesChannel);
+ }
+
+ const auto end = TimeUtil::GetTime(true);
+ return new TimedVariant{TimestampVariant{end-beg}, timestamp};
+ }
+
std::string Publisher::getRobotUnitListenerTopicName(const Ice::Current &) const
{
return robotUnitListenerTopicName;
@@ -241,7 +405,7 @@ namespace armarx
void Publisher::_postFinishControlThreadInitialization()
{
//start publisher
- publishNewSensorDataTime = TimeUtil::GetTime();
+ publishNewSensorDataTime = TimeUtil::GetTime(true);
publisherTask = new PublisherTaskT([&] {publish({});}, publishPeriodMs, false, getName() + "_PublisherTask");
ARMARX_INFO << "starting publisher with timestep " << publishPeriodMs;
publisherTask->setDelayWarningTolerance(10 * publishPeriodMs);
@@ -251,7 +415,6 @@ namespace armarx
void Publisher::publish(StringVariantBaseMap additionalMap, StringVariantBaseMap timingMap)
{
auto begPublish = Now();
- static const int spamdelay = 30;
if (getRobotUnitState() != RobotUnitState::Running)
{
@@ -283,19 +446,13 @@ namespace armarx
const bool haveActivatedControllersChanged = _module<ControlThreadDataBuffer>().activatedControllersChanged();
const bool haveSensorAndControlValsChanged = _module<ControlThreadDataBuffer>().sensorAndControlBufferChanged();
//setup datastructures
- const auto& controlThreadOutputBuffer = _module<ControlThreadDataBuffer>().getSensorAndControlBuffer();
- const auto& activatedControllers = _module<ControlThreadDataBuffer>().getActivatedControllers();
+ const auto& controlThreadOutputBuffer = _module<ControlThreadDataBuffer>().getSensorAndControlBuffer(); ///TODO copy buffer
+ const auto activatedControllers = _module<ControlThreadDataBuffer>().getActivatedControllers();
const auto requestedJointControllers = _module<ControlThreadDataBuffer>().copyRequestedJointControllers();
const auto timestamp = controlThreadOutputBuffer.sensorValuesTimestamp;
- const auto numSensorDevices = _module<Devices>().getNumberOfSensorDevices();
- const auto numControlDevices = _module<Devices>().getNumberOfControlDevices();
-
const bool publishToObserver = !(publishIterationCount % debugObserverSkipIterations);
- const bool debugObserverPublishControlTargetsThisIteration = observerPublishControlTargets;
- const bool debugObserverPublishSensorValuesThisIteration = observerPublishSensorValues;
- StringVariantBaseMap ctrlDevMap, sensorDevMap;
//publish publishing meta state
additionalMap["haveActivatedControllersChanged"] = new TimedVariant {haveActivatedControllersChanged, timestamp};
additionalMap["haveSensorAndControlValsChanged"] = new TimedVariant {haveSensorAndControlValsChanged, timestamp};
@@ -304,56 +461,9 @@ namespace armarx
if (haveSensorAndControlValsChanged)
{
//update units
- timingMap["publishTimings_UnitUpdate"] = new TimedVariant
- {
- ARMARX_STOPWATCH(TimestampVariant)
- {
- ARMARX_DEBUG << deactivateSpam(spamdelay) << "updating units with new sensor values";
- publishNewSensorDataTime = TimeUtil::GetTime();
- for (const RobotUnitSubUnitPtr& rsu : UnitsAttorneyForPublisher::GetSubUnits(this))
- {
- if (rsu && rsu->getObjectScheduler() && rsu->getProxy())
- {
- timingMap["publishTimings_UnitUpdate_" + rsu->getName()] = new TimedVariant
- {
- ARMARX_STOPWATCH(TimestampVariant){rsu->update(controlThreadOutputBuffer, activatedControllers);},
- timestamp
- };
- }
- }
- },
- timestamp
- };
+ timingMap["UnitUpdate"] = publishUnitUpdates(timestamp, timingMap, controlThreadOutputBuffer, activatedControllers);
//publish sensor updates
-
- timingMap["publishTimings_SensorUpdates"] = new TimedVariant
- {
- ARMARX_STOPWATCH(TimestampVariant)
- {
- for (std::size_t sensidx = 0 ; sensidx < numSensorDevices; ++sensidx)
- {
- const SensorValueBase& sensVal = *(controlThreadOutputBuffer.sensors.at(sensidx));
- auto variants = sensVal.toVariants(timestamp);
-
- if (!variants.empty())
- {
- SensorDeviceStatus status;
- status.deviceName = DevicesAttorneyForPublisher::GetSensorDeviceName(this, sensidx);
- status.sensorValue = std::move(variants);
- status.timestampUSec = MicroNow().count();
- if (debugObserverPublishSensorValuesThisIteration && publishToObserver)
- {
- transformMapKeys(
- variants, sensorDevMap,
- detail::makeDebugObserverNameFixer(status.deviceName + "_" + sensVal.getSensorValueType(true) + "_")
- );
- }
- listenerBatchPrx->sensorDeviceStatusChanged(status);
- }
- }
- },
- timestamp
- };
+ timingMap["SensorUpdates"] = publishSensorUpdates(timestamp, listenerBatchPrx, publishToObserver, controlThreadOutputBuffer);
}
else
{
@@ -366,126 +476,32 @@ namespace armarx
}
if (haveSensorAndControlValsChanged || haveActivatedControllersChanged)
{
- timingMap["publishTimings_ControlUpdates"] = new TimedVariant
- {
- ARMARX_STOPWATCH(TimestampVariant)
- {
- for (std::size_t ctrlidx = 0 ; ctrlidx < numControlDevices; ++ctrlidx)
- {
- const ControlDevice& ctrlDev = *(_module<Devices>().getControlDevices().at(ctrlidx));
-
- StringToStringVariantBaseMapMap variants;
- for (const auto& ctrlVal : controlThreadOutputBuffer.control.at(ctrlidx))
- {
- const auto& controlMode = ctrlVal->getControlMode();
- variants[controlMode] = ctrlVal->toVariants(timestamp);
- if (
- haveSensorAndControlValsChanged &&
- !variants[controlMode].empty() &&
- debugObserverPublishControlTargetsThisIteration &&
- publishToObserver
- )
- {
- transformMapKeys(
- variants[controlMode], ctrlDevMap,
- detail::makeDebugObserverNameFixer(ctrlDev.getDeviceName() + "_" + controlMode + "_")
- );
- }
- }
- ControlDeviceStatus status;
- const auto activeJointCtrl = activatedControllers.jointControllers.at(ctrlidx);
- status.activeControlMode = activeJointCtrl ? activeJointCtrl->getControlMode() : std::string {"!!JointController is nullptr!!"};
- status.deviceName = ctrlDev.getDeviceName();
- ARMARX_CHECK_EXPRESSION(requestedJointControllers.at(ctrlidx));
- status.requestedControlMode = requestedJointControllers.at(ctrlidx)->getControlMode();
- status.controlTargetValues = std::move(variants);
- status.timestampUSec = MicroNow().count();
- listenerBatchPrx->controlDeviceStatusChanged(status);
- }
- },
- timestamp
- };
+ timingMap["ControlUpdates"] = publishControlUpdates(
+ timestamp, listenerBatchPrx, controlThreadOutputBuffer,
+ haveSensorAndControlValsChanged, publishToObserver,
+ activatedControllers, requestedJointControllers);
}
//call publish hook + publish NJointController changes
- timingMap["publishTimings_NJointControllerUpdates"] = new TimedVariant
- {
- ARMARX_STOPWATCH(TimestampVariant)
- {
- for (const auto& pair : getNJointControllers())
- {
- const NJointControllerPtr& nJointCtrl = pair.second;
- timingMap["publishTimings_NJointControllerUpdates_" + nJointCtrl->getInstanceName()] = new TimedVariant
- {
- ARMARX_STOPWATCH(TimestampVariant)
- {
- //run some hook for active (used for visu)
- NJointControllerAttorneyForPublisher::Publish(nJointCtrl, controlThreadOutputBuffer, debugDrawerPrx, debugObserverBatchPrx);
- if (
- NJointControllerAttorneyForPublisher::GetStatusReportedActive(nJointCtrl) != nJointCtrl->isControllerActive() ||
- NJointControllerAttorneyForPublisher::GetStatusReportedRequested(nJointCtrl) != nJointCtrl->isControllerRequested()
- )
- {
- NJointControllerAttorneyForPublisher::UpdateStatusReported(nJointCtrl);
- listenerBatchPrx->nJointControllerStatusChanged(nJointCtrl->getControllerStatus());
- }
- },
- timestamp
- };
- }
- },
- timestamp
- };
+ timingMap["NJointControllerUpdates"] = publishNJointControllerUpdates(timestamp, listenerBatchPrx, timingMap, controlThreadOutputBuffer, debugObserverBatchPrx);
//report new class names
- timingMap["publishTimings_ClassNameUpdates"] = new TimedVariant
- {
- ARMARX_STOPWATCH(TimestampVariant)
- {
- const auto classNames = NJointControllerRegistry::getKeys();
- if (lastReportedClasses.size() != classNames.size())
- {
- for (const auto& name : classNames)
- {
- if (!lastReportedClasses.count(name))
- {
- listenerBatchPrx->nJointControllerClassAdded(name);
- lastReportedClasses.emplace(name);
- }
- }
- }
- },
- timestamp
- };
- timingMap["publishTimings_RobotUnitListenerFlush"] = new TimedVariant
+ timingMap["ClassNameUpdates"] = publishNJointClassNames(timestamp, listenerBatchPrx);
+ timingMap["RobotUnitListenerFlush"] = new TimedVariant
{
ARMARX_STOPWATCH(TimestampVariant){listenerBatchPrx->ice_flushBatchRequests();}, timestamp
};
if (publishToObserver)
{
- timingMap["publishTimings_LastRobotUnitObserverFlush"] =
- new TimedVariant {TimestampVariant{lastRobotUnitObserverFlush.count()}, timestamp};
- timingMap["publishTimings_LastPublishLoop"] = new TimedVariant {TimestampVariant{lastPublishLoop.count()}, timestamp};
- lastRobotUnitObserverFlush = ARMARX_STOPWATCH()
+ timingMap["LastPublishLoop"] = new TimedVariant {TimestampVariant{lastPublishLoop.count()}, timestamp};
+ if (robotUnitObserver->getState() >= eManagedIceObjectStarted)
{
- if (robotUnitObserver->getState() >= eManagedIceObjectStarted)
- {
- robotUnitObserver->offerOrUpdateDataFieldsFlatCopy(robotUnitObserver->additionalChannel, additionalMap);
- robotUnitObserver->updateChannel(robotUnitObserver->additionalChannel);
- robotUnitObserver->offerOrUpdateDataFieldsFlatCopy(robotUnitObserver->timingChannel, timingMap);
- robotUnitObserver->updateChannel(robotUnitObserver->timingChannel);
- robotUnitObserver->offerOrUpdateDataFieldsFlatCopy(robotUnitObserver->controlDevicesChannel, ctrlDevMap);
- robotUnitObserver->updateChannel(robotUnitObserver->controlDevicesChannel);
- robotUnitObserver->offerOrUpdateDataFieldsFlatCopy(robotUnitObserver->sensorDevicesChannel, sensorDevMap);
- robotUnitObserver->updateChannel(robotUnitObserver->sensorDevicesChannel);
-
- }
- debugObserverBatchPrx->ice_flushBatchRequests();
- };
- }
- else
- {
- lastRobotUnitObserverFlush = std::chrono::microseconds {0};
+ robotUnitObserver->offerOrUpdateDataFieldsFlatCopy(robotUnitObserver->additionalChannel, additionalMap);
+ robotUnitObserver->updateChannel(robotUnitObserver->additionalChannel);
+ robotUnitObserver->offerOrUpdateDataFieldsFlatCopy(robotUnitObserver->timingChannel, timingMap);
+ robotUnitObserver->updateChannel(robotUnitObserver->timingChannel);
+ }
+ debugObserverBatchPrx->ice_flushBatchRequests();
}
//warn if there are unset jointCtrl controllers
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModulePublisher.h b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModulePublisher.h
index 21cb1a18a5a794de8139bda0ffb72d4d50de62e6..c59dcd9a5039bb5e8742e96d8146c0c98e5b5757 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModulePublisher.h
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModulePublisher.h
@@ -34,6 +34,13 @@
namespace armarx
{
TYPEDEF_PTRS_HANDLE(RobotUnitObserver);
+
+ namespace detail
+ {
+ struct ControlThreadOutputBufferEntry;
+ }
+ using SensorAndControl = detail::ControlThreadOutputBufferEntry;
+
namespace RobotUnitModule
{
class PublisherPropertyDefinitions: public ModuleBasePropertyDefinitions
@@ -163,10 +170,84 @@ namespace armarx
ARMARX_CHECK_EXPRESSION(robotUnitObserver);
return robotUnitObserver;
}
+ /**
+ * @brief Publishes updtes about new classes od \ref NJointController "NJointControllers"
+ * @param timestamp The timestamp of the published \ref ControlThread iteration
+ * @param listenerPrx The proxy to the \ref RobotUnitListener topic
+ * @return The time required by this function as a \ref Variant
+ */
+ TimedVariantPtr publishNJointClassNames(
+ IceUtil::Time timestamp,
+ RobotUnitListenerPrx& listenerPrx);
+ /**
+ * @brief Publishes updates of \ref NJointController "NJointControllers" ([de]activate + publish hooks)
+ * @param timestamp The timestamp of the published \ref ControlThread iteration
+ * @param listenerPrx The proxy to the \ref RobotUnitListener topic
+ * @param timingMap Timings of this publish iteration (out param)
+ * @param controlThreadOutputBuffer The output of the published \ref ControlThread iteration
+ * @param debugObserverBatchPrx Proxy to the \ref DebugObserver's topic
+ * @return The time required by this function as a \ref Variant
+ * @see NJointController::onPublish
+ */
+ TimedVariantPtr publishNJointControllerUpdates(
+ IceUtil::Time timestamp,
+ RobotUnitListenerPrx& listenerPrx,
+ StringVariantBaseMap& timingMap,
+ const SensorAndControl& controlThreadOutputBuffer,
+ DebugObserverInterfacePrx& debugObserverBatchPrx);
+ /**
+ * @brief Updates all sub units and publishes the timings of these updates
+ * @param timestamp The timestamp of the published \ref ControlThread iteration
+ * @param timingMap Timings of this publish iteration (out param)
+ * @param controlThreadOutputBuffer The output of the published \ref ControlThread iteration
+ * @param activatedControllers The \ref JointController "JointControllers" and \ref NJointController "NJointControllers"
+ * active in the published \ref ControlThread iteration
+ * @return The time required by this function as a \ref Variant
+ */
+ TimedVariantPtr publishUnitUpdates(
+ IceUtil::Time timestamp,
+ StringVariantBaseMap &timingMap,
+ const SensorAndControl& controlThreadOutputBuffer,
+ const JointAndNJointControllers& activatedControllers);
+ /**
+ * @brief Publishes data about updates of \ref JointController "JointControllers" and their \ref ControlTargetBase "ControlTargets"
+ * @param timestamp The timestamp of the published \ref ControlThread iteration
+ * @param listenerPrx The proxy to the \ref RobotUnitListener topic
+ * @param controlThreadOutputBuffer The output of the published \ref ControlThread iteration
+ * @param haveSensorAndControlValsChanged Whether \ref ControlTargetBase "ControlTargets" were updated by the \ref ControlThread
+ * @param publishToObserver Whether data should be published to observers
+ * @param activatedControllers The \ref JointController "JointControllers" and \ref NJointController "NJointControllers"
+ * active in the published \ref ControlThread iteration
+ * @param requestedJointControllers
+ * @return The time required by this function as a \ref Variant
+ */
+ TimedVariantPtr publishControlUpdates(
+ IceUtil::Time timestamp,
+ RobotUnitListenerPrx& listenerPrx,
+ const SensorAndControl& controlThreadOutputBuffer,
+ bool haveSensorAndControlValsChanged,
+ bool publishToObserver,
+ const JointAndNJointControllers& activatedControllers,
+ const std::vector<JointController*>& requestedJointControllers);
+ /**
+ * @brief Publishes Updates about \ref SensorValueBase "SensorValues" (To \ref RobotUnitListener and \ref RobotUnitObserver)
+ * @param timestamp The timestamp of the published \ref ControlThread iteration
+ * @param listenerPrx The proxy to the \ref RobotUnitListener topic
+ * @param publishToObserver Whether data should be published to observers
+ * @param controlThreadOutputBuffer The output of the published \ref ControlThread iteration
+ * @return The time required by this function as a \ref Variant
+ */
+ TimedVariantPtr publishSensorUpdates(
+ IceUtil::Time timestamp,
+ RobotUnitListenerPrx &listenerPrx,
+ bool publishToObserver,
+ const SensorAndControl& controlThreadOutputBuffer);
// //////////////////////////////////////////////////////////////////////////////////////// //
// ///////////////////////////////////////// Data ///////////////////////////////////////// //
// //////////////////////////////////////////////////////////////////////////////////////// //
private:
+ static constexpr int spamdelay = 30;
+
using PublisherTaskT = SimplePeriodicTask<std::function<void(void)>>;
/// @brief The name of the used RobotUnitListenerTopic
@@ -198,8 +279,6 @@ namespace armarx
/// @brief How many iterations of \ref publish shold not publish data to the debug observer.
std::atomic<std::uint64_t> debugObserverSkipIterations;
- /// @brief The time required by the last iteration of \ref publish to flush the debug observer
- std::chrono::microseconds lastRobotUnitObserverFlush;
/// @brief The time required by the last iteration of \ref publish
std::chrono::microseconds lastPublishLoop;