RobotUnitModulePublisher.cpp

/*
 * This file is part of ArmarX.
 *
 * ArmarX is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * ArmarX is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 *
 * @package    RobotAPI::ArmarXObjects::RobotUnit
 * @author     Raphael Grimm ( raphael dot grimm at kit dot edu )
 * @date       2018
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */

#include "RobotUnitModulePublisher.h"

#include "ArmarXCore/core/exceptions/local/ExpressionException.h"
#include "ArmarXCore/core/logging/Logging.h"
#include "ArmarXCore/core/util/StringHelpers.h"
#include <ArmarXCore/core/ArmarXObjectScheduler.h>

#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerBase.h>
#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h>
#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.h>
#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThreadDataBuffer.h>
#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControllerManagement.h>
#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.h>
#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.h>
#include <RobotAPI/components/units/RobotUnit/Units/RobotUnitSubUnit.h>
#include <RobotAPI/components/units/RobotUnit/util/NonRtTiming.h>

namespace armarx::RobotUnitModule
{
    /**
     * \brief This class allows minimal access to private members of \ref NJointControllerBase 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 !!
     */
    class NJointControllerAttorneyForPublisher
    {
        friend class Publisher;

        static bool
        GetStatusReportedActive(const NJointControllerBasePtr& nJointCtrl)
        {
            return nJointCtrl->statusReportedActive;
        }

        static bool
        GetStatusReportedRequested(const NJointControllerBasePtr& nJointCtrl)
        {
            return nJointCtrl->statusReportedRequested;
        }

        static void
        UpdateStatusReported(const NJointControllerBasePtr& nJointCtrl)
        {
            nJointCtrl->statusReportedActive = nJointCtrl->isActive.load();
            nJointCtrl->statusReportedRequested = nJointCtrl->isRequested.load();
        }

        static void
        Publish(const NJointControllerBasePtr& nJointCtrl,
                const SensorAndControl& sac,
                const DebugDrawerInterfacePrx& draw,
                const DebugObserverInterfacePrx& observer)
        {
            nJointCtrl->publish(sac, draw, observer);
        }

        static void
        DeactivatePublishing(const NJointControllerBasePtr& nJointCtrl,
                             const DebugDrawerInterfacePrx& draw,
                             const DebugObserverInterfacePrx& observer)
        {
            nJointCtrl->deactivatePublish(draw, observer);
        }
    };

    /**
     * \brief This class allows minimal access to private members of \ref Devices 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 !!
     */
    class DevicesAttorneyForPublisher
    {
        friend class Publisher;

        static const std::string&
        GetSensorDeviceName(Publisher* p, std::size_t idx)
        {
            return p->_module<Devices>().sensorDevices.at(idx)->getDeviceName();
        }
    };

    /**
     * \brief This class allows minimal access to private members of \ref Units 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 !!
     */
    class UnitsAttorneyForPublisher
    {
        friend class Publisher;

        static const std::vector<RobotUnitSubUnitPtr>&
        GetSubUnits(Publisher* p)
        {
            return p->_module<Units>().subUnits;
        }
    };

    /**
     * \brief This class allows minimal access to private members of \ref ControllerManagement 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 !!
     */
    class ControllerManagementAttorneyForPublisher
    {
        friend class Publisher;

        static const std::map<std::string, NJointControllerBasePtr>&
        GetNJointControllers(Publisher* p)
        {
            return p->_module<ControllerManagement>().nJointControllers;
        }

        static void
        RemoveNJointControllersToBeDeleted(Publisher* p, bool blocking, RobotUnitListenerPrx l)
        {
            p->_module<ControllerManagement>().removeNJointControllersToBeDeleted(blocking, l);
        }
    };

    /**
    * \brief This class allows minimal access to private members of \ref ControlThread in a sane fashion for \ref Publisher.
    * \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 !!
    */
    class ControlThreadAttorneyForPublisher
    {
        friend class Publisher;

        static void
        ProcessEmergencyStopRequest(Publisher* p)
        {
            return p->_module<ControlThread>().processEmergencyStopRequest();
        }
    };
} // namespace armarx::RobotUnitModule

namespace armarx::RobotUnitModule
{
    TimedVariantPtr
    Publisher::publishNJointClassNames()
    {
        ARMARX_TRACE;
        throwIfInControlThread(BOOST_CURRENT_FUNCTION);
        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))
                {
                    robotUnitListenerBatchPrx->nJointControllerClassAdded(name);
                    lastReportedClasses.emplace(name);
                }
            }
        }

        const auto end = TimeUtil::GetTime(true);
        return new TimedVariant{TimestampVariant{end - beg}, lastControlThreadTimestamp};
    }

    TimedVariantPtr
    Publisher::publishNJointControllerUpdates(StringVariantBaseMap& timingMap,
                                              const SensorAndControl& controlThreadOutputBuffer)
    {
        ARMARX_TRACE;
        throwIfInControlThread(BOOST_CURRENT_FUNCTION);
        const auto beg = TimeUtil::GetTime(true);

        //publish controller updates
        auto debugDrawerBatchPrx = debugDrawerPrx->ice_batchOneway();
        auto debugObserverBatchPrx = debugObserverPrx->ice_batchOneway();
        NJointControllerStatusSeq allStatus;
        for (const auto& pair :
             ControllerManagementAttorneyForPublisher::GetNJointControllers(this))
        {
            const auto begInner = TimeUtil::GetTime(true);
            const NJointControllerBasePtr& nJointCtrl = pair.second;

            //run some hook for active (used for visu)
            NJointControllerAttorneyForPublisher::Publish(
                nJointCtrl, controlThreadOutputBuffer, debugDrawerBatchPrx, debugObserverBatchPrx);
            if (NJointControllerAttorneyForPublisher::GetStatusReportedActive(nJointCtrl) !=
                    nJointCtrl->isControllerActive() ||
                NJointControllerAttorneyForPublisher::GetStatusReportedRequested(nJointCtrl) !=
                    nJointCtrl->isControllerRequested())
            {
                NJointControllerAttorneyForPublisher::UpdateStatusReported(nJointCtrl);
                allStatus.emplace_back(nJointCtrl->getControllerStatus());
            }

            const auto endInner = TimeUtil::GetTime(true);
            timingMap["NJointControllerUpdates_" + nJointCtrl->getInstanceName()] =
                new TimedVariant{TimestampVariant{endInner - begInner}, lastControlThreadTimestamp};
        }

        robotUnitListenerBatchPrx->nJointControllerStatusChanged(allStatus);
        debugObserverBatchPrx->ice_flushBatchRequests();
        debugDrawerBatchPrx->ice_flushBatchRequests();

        const auto end = TimeUtil::GetTime(true);
        return new TimedVariant{TimestampVariant{end - beg}, lastControlThreadTimestamp};
    }

    TimedVariantPtr
    Publisher::publishUnitUpdates(StringVariantBaseMap& timingMap,
                                  const SensorAndControl& controlThreadOutputBuffer,
                                  const JointAndNJointControllers& activatedControllers)
    {
        ARMARX_TRACE;
        throwIfInControlThread(BOOST_CURRENT_FUNCTION);
        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}, lastControlThreadTimestamp};
            }
        }

        const auto end = TimeUtil::GetTime(true);
        return new TimedVariant{TimestampVariant{end - beg}, lastControlThreadTimestamp};
    }

    TimedVariantPtr
    Publisher::publishControlUpdates(const SensorAndControl& controlThreadOutputBuffer,
                                     bool haveSensorAndControlValsChanged,
                                     bool publishToObserver,
                                     const JointAndNJointControllers& activatedControllers,
                                     const std::vector<JointController*>& requestedJointControllers)
    {
        ARMARX_TRACE;
        throwIfInControlThread(BOOST_CURRENT_FUNCTION);
        const auto beg = armarx::rtNow();


        StringVariantBaseMap ctrlDevMap;
        ControlDeviceStatusSeq allStatus;
        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();
            if (activeJointCtrl)
            {
                auto additionalDatafields =
                    activeJointCtrl->publish(debugDrawerPrx, debugObserverPrx);
                for (auto& pair : additionalDatafields)
                {
                    ctrlDevMap[ctrlDev.getDeviceName() + "_" + activeJointCtrl->getControlMode() +
                               "_" + pair.first] = pair.second;
                }
            }

            for (const auto& ctrlVal : controlThreadOutputBuffer.control.at(ctrlidx))
            {
                const auto& controlMode = ctrlVal->getControlMode();
                variants[controlMode] = ctrlVal->toVariants(lastControlThreadTimestamp);
                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 = lastControlThreadTimestamp.toMicroSeconds();
            allStatus.emplace_back(status);
        }

        robotUnitListenerBatchPrx->controlDeviceStatusChanged(allStatus);
        if (observerPublishControlTargets)
        {
            if (robotUnitObserver->getState() >= eManagedIceObjectStarted)
            {
                robotUnitObserver->offerOrUpdateDataFieldsFlatCopy_async(
                    robotUnitObserver->controlDevicesChannel, std::move(ctrlDevMap));
            }
        }

        const auto end = armarx::rtNow();
        return new TimedVariant{TimestampVariant{end - beg}, lastControlThreadTimestamp};
    }

    TimedVariantPtr
    Publisher::publishSensorUpdates(bool publishToObserver,
                                    const SensorAndControl& controlThreadOutputBuffer)
    {
        ARMARX_TRACE;
        throwIfInControlThread(BOOST_CURRENT_FUNCTION);
        const auto beg = TimeUtil::GetTime(true);

        StringVariantBaseMap sensorDevMap;
        const auto numSensorDev = _module<Devices>().getNumberOfSensorDevices();
        ARMARX_CHECK_EQUAL(controlThreadOutputBuffer.sensors.size(),
                           _module<Devices>().getNumberOfSensorDevices());
        SensorDeviceStatusSeq allStatus;
        for (std::size_t sensidx = 0; sensidx < numSensorDev; ++sensidx)
        {
            ARMARX_TRACE;
            const SensorValueBase& sensVal = *(controlThreadOutputBuffer.sensors.at(sensidx));
            auto variants = sensVal.toVariants(lastControlThreadTimestamp);

            if (!variants.empty())
            {
                SensorDeviceStatus status;
                status.deviceName = DevicesAttorneyForPublisher::GetSensorDeviceName(this, sensidx);

                ARMARX_TRACE;
                if (observerPublishSensorValues && publishToObserver)
                {
                    for (const auto& pair : variants)
                    {
                        sensorDevMap[status.deviceName + "_" + pair.first] = pair.second;
                    }
                }

                status.sensorValue = std::move(variants);
                status.timestampUSec = lastControlThreadTimestamp.toMicroSeconds();
                allStatus.emplace_back(status);
            }
        }

        ARMARX_TRACE;
        robotUnitListenerBatchPrx->sensorDeviceStatusChanged(allStatus);
        if (observerPublishSensorValues)
        {
            ARMARX_TRACE;
            if (robotUnitObserver->getState() >= eManagedIceObjectStarted)
            {
                std::stringstream s;
                for (auto& pair : sensorDevMap)
                {
                    s << pair.first << ": \t"
                      << (pair.second ? pair.second->ice_id() + "\t with datatype \t" +
                                            Variant::typeToString(pair.second->getType())
                                      : "NULL")
                      << "\n";
                }
                ARMARX_DEBUG << deactivateSpam(spamdelay)
                             << "Sensor Datafieldnames: " << ARMARX_STREAM_PRINTER
                {
                    for (auto& pair : sensorDevMap)
                    {
                        out << pair.first << ": "
                            << (pair.second ? pair.second->ice_id() +
                                                  Variant::typeToString(pair.second->getType())
                                            : "NULL")
                            << "\n";
                    }
                };
                robotUnitObserver->offerOrUpdateDataFieldsFlatCopy_async(
                    robotUnitObserver->sensorDevicesChannel, std::move(sensorDevMap));
            }
        }
        ARMARX_TRACE;

        const auto end = TimeUtil::GetTime(true);
        return new TimedVariant{TimestampVariant{end - beg}, lastControlThreadTimestamp};
    }

    std::string
    Publisher::getRobotUnitListenerTopicName(const Ice::Current&) const
    {
        ARMARX_TRACE;
        throwIfInControlThread(BOOST_CURRENT_FUNCTION);
        return robotUnitListenerTopicName;
    }

    std::string
    Publisher::getDebugDrawerTopicName(const Ice::Current&) const
    {
        ARMARX_TRACE;
        throwIfInControlThread(BOOST_CURRENT_FUNCTION);
        return debugDrawerUpdatesTopicName;
    }

    std::string
    Publisher::getDebugObserverTopicName(const Ice::Current&) const
    {
        ARMARX_TRACE;
        throwIfInControlThread(BOOST_CURRENT_FUNCTION);
        return debugObserverTopicName;
    }

    RobotUnitListenerPrx
    Publisher::getRobotUnitListenerProxy(const Ice::Current&) const
    {
        ARMARX_TRACE;
        throwIfInControlThread(BOOST_CURRENT_FUNCTION);
        ARMARX_CHECK_EXPRESSION(robotUnitListenerPrx);
        return robotUnitListenerPrx;
    }

    DebugObserverInterfacePrx
    Publisher::getDebugObserverProxy(const Ice::Current&) const
    {
        ARMARX_TRACE;
        throwIfInControlThread(BOOST_CURRENT_FUNCTION);
        ARMARX_CHECK_EXPRESSION(debugObserverPrx);
        return debugObserverPrx;
    }

    DebugDrawerInterfacePrx
    Publisher::getDebugDrawerProxy(const Ice::Current&) const
    {
        ARMARX_TRACE;
        throwIfInControlThread(BOOST_CURRENT_FUNCTION);
        ARMARX_CHECK_EXPRESSION(debugDrawerPrx);
        return debugDrawerPrx;
    }

    void
    Publisher::_preOnConnectRobotUnit()
    {
        ARMARX_TRACE;
        throwIfInControlThread(BOOST_CURRENT_FUNCTION);
        ARMARX_DEBUG << "retrieving RobotUnitListenerPrx for topic "
                     << getRobotUnitListenerTopicName();
        robotUnitListenerPrx = getTopic<RobotUnitListenerPrx>(getRobotUnitListenerTopicName());
        robotUnitListenerBatchPrx = robotUnitListenerPrx->ice_batchOneway();

        ARMARX_DEBUG << "retrieving DebugDrawerInterfacePrx for topic "
                     << getDebugDrawerTopicName();
        debugDrawerPrx = getTopic<DebugDrawerInterfacePrx>(getDebugDrawerTopicName());

        ARMARX_DEBUG << "retrieving DebugObserverInterfacePrx for topic "
                     << getDebugObserverTopicName();
        debugObserverPrx = getTopic<DebugObserverInterfacePrx>(getDebugObserverTopicName());
    }

    void
    Publisher::_preOnInitRobotUnit()
    {
        ARMARX_TRACE;
        throwIfInControlThread(BOOST_CURRENT_FUNCTION);
        robotUnitListenerTopicName =
            getProperty<std::string>("RobotUnitListenerTopicName").getValue();
        debugDrawerUpdatesTopicName =
            getProperty<std::string>("DebugDrawerUpdatesTopicName").getValue();
        debugObserverTopicName = getProperty<std::string>("DebugObserverTopicName").getValue();

        observerEnablePublishing = getProperty<bool>("ObserverEnablePublishing").getValue();
        observerPublishSensorValues = getProperty<bool>("ObserverPublishSensorValues").getValue();
        observerPublishControlTargets =
            getProperty<bool>("ObserverPublishControlTargets").getValue();
        observerPublishTimingInformation =
            getProperty<bool>("ObserverPublishTimingInformation").getValue();
        observerPublishAdditionalInformation =
            getProperty<bool>("ObserverPublishAdditionalInformation").getValue();
        debugObserverSkipIterations =
            std::max(1ul, getProperty<std::uint64_t>("ObserverPrintEveryNthIterations").getValue());

        publishPeriodMs = std::max(1ul, getProperty<std::size_t>("PublishPeriodMs").getValue());

        offeringTopic(getRobotUnitListenerTopicName());
        offeringTopic(getDebugDrawerTopicName());
        offeringTopic(getDebugObserverTopicName());
        getArmarXManager()->addObject(robotUnitObserver);
    }

    void
    Publisher::_icePropertiesInitialized()
    {
        ARMARX_TRACE;
        throwIfInControlThread(BOOST_CURRENT_FUNCTION);
        robotUnitObserver = Component::create<RobotUnitObserver, RobotUnitObserverPtr>(
            getIceProperties(), "", getConfigDomain());
        addPropertyUser(PropertyUserPtr::dynamicCast(robotUnitObserver));
    }

    void
    Publisher::_componentPropertiesUpdated(const std::set<std::string>& changedProperties)
    {
        ARMARX_TRACE;
        throwIfInControlThread(BOOST_CURRENT_FUNCTION);
        if (changedProperties.count("ObserverPublishSensorValues"))
        {
            observerPublishSensorValues =
                getProperty<bool>("ObserverPublishSensorValues").getValue();
        }
        if (changedProperties.count("ObserverPublishControlTargets"))
        {
            observerPublishControlTargets =
                getProperty<bool>("ObserverPublishControlTargets").getValue();
        }
        if (changedProperties.count("ObserverPublishTimingInformation"))
        {
            observerPublishTimingInformation =
                getProperty<bool>("ObserverPublishTimingInformation").getValue();
        }
        if (changedProperties.count("ObserverPublishAdditionalInformation"))
        {
            observerPublishAdditionalInformation =
                getProperty<bool>("ObserverPublishAdditionalInformation").getValue();
        }
        if (changedProperties.count("ObserverPrintEveryNthIterations"))
        {
            debugObserverSkipIterations =
                getProperty<std::uint64_t>("ObserverPrintEveryNthIterations").getValue();
        }
    }

    void
    Publisher::_preFinishRunning()
    {
        ARMARX_TRACE;
        throwIfInControlThread(BOOST_CURRENT_FUNCTION);
        if (publisherTask)
        {
            ARMARX_DEBUG << "shutting down publisher task";
            publisherTask->stop();
            std::this_thread::sleep_for(std::chrono::milliseconds{10});
            while (publisherTask->isFunctionExecuting() || publisherTask->isRunning())
            {
                ARMARX_FATAL << deactivateSpam(0.1)
                             << "PUBLISHER TASK IS RUNNING AFTER IT WAS STOPPED!";
            }
            publisherTask = nullptr;
            //since the drawer queues draw events and we want to clear the layers, we have to sleep here
            std::this_thread::sleep_for(std::chrono::milliseconds{100});
            ARMARX_DEBUG << "shutting down publisher task done";
        }
        for (const auto& pair :
             ControllerManagementAttorneyForPublisher::GetNJointControllers(this))
        {
            ARMARX_DEBUG << "forcing deactivation of publishing for " << pair.first;
            NJointControllerAttorneyForPublisher::DeactivatePublishing(
                pair.second, debugDrawerPrx, debugObserverPrx);
        }
    }

    void
    Publisher::_postFinishControlThreadInitialization()
    {
        ARMARX_TRACE;
        //start publisher
        publishNewSensorDataTime = TimeUtil::GetTime(true);
        publisherTask = new PublisherTaskT(
            [&] { publish({}); }, publishPeriodMs, false, getName() + "_PublisherTask");
        ARMARX_INFO << "starting publisher with timestep " << publishPeriodMs;
        publisherTask->setDelayWarningTolerance(10 * publishPeriodMs);
        if (observerEnablePublishing)
        {
            publisherTask->start();
        }
    }

    void
    Publisher::publish(StringVariantBaseMap additionalMap, StringVariantBaseMap timingMap)
    {
        ARMARX_TRACE;
        throwIfInControlThread(BOOST_CURRENT_FUNCTION);
        auto begPublish = TimeUtil::GetTime(true);

        if (getRobotUnitState() != RobotUnitState::Running)
        {
            ARMARX_DEBUG << deactivateSpam(spamdelay) << "skipping publishing in state "
                         << getRobotUnitState();
            return;
        }
        if (isShuttingDown())
        {
            return;
        }
        GuardType guard;
        try
        {
            guard = getGuard();
        }
        catch (...)
        {
            //shutting down
            return;
        }
        throwIfStateIsNot(RobotUnitState::Running, __FUNCTION__);

        //maybe change the emergency stop state
        ControlThreadAttorneyForPublisher::ProcessEmergencyStopRequest(this);

        //get batch proxies
        //delete NJoint queued for deletion
        ControllerManagementAttorneyForPublisher::RemoveNJointControllersToBeDeleted(
            this, false, robotUnitListenerBatchPrx);
        //swap buffers in
        const bool haveActivatedControllersChanged =
            _module<ControlThreadDataBuffer>().activatedControllersChanged();
        const bool haveSensorAndControlValsChanged =
            _module<ControlThreadDataBuffer>().sensorAndControlBufferChanged();
        //setup datastructures
        const auto& controlThreadOutputBuffer =
            _module<ControlThreadDataBuffer>().getSensorAndControlBuffer(); ///TODO copy buffer

        const auto activatedControllers =
            _module<ControlThreadDataBuffer>().getActivatedControllers();
        const auto requestedJointControllers =
            _module<ControlThreadDataBuffer>().copyRequestedJointControllers();

        // controlThreadOutputBuffer.sensorValuesTimestamp is in MONOTONIC_RAW (not relative to epoch).
        // We have to map it to be relative to epoch (REAL_TIME).
        lastControlThreadTimestamp =
            armarx::mapRtTimestampToNonRtTimestamp(controlThreadOutputBuffer.sensorValuesTimestamp);

        const bool publishToObserver = !(publishIterationCount % debugObserverSkipIterations);
        //publish publishing meta state
        additionalMap["haveActivatedControllersChanged"] =
            new TimedVariant{haveActivatedControllersChanged, lastControlThreadTimestamp};
        additionalMap["haveSensorAndControlValsChanged"] =
            new TimedVariant{haveSensorAndControlValsChanged, lastControlThreadTimestamp};
        additionalMap["publishToObserver"] =
            new TimedVariant{publishToObserver, lastControlThreadTimestamp};
        additionalMap["SoftwareEmergencyStopState"] =
            new TimedVariant{_module<Units>().getEmergencyStopMaster()->getEmergencyStopState() ==
                                     EmergencyStopState::eEmergencyStopActive
                                 ? "EmergencyStopActive"
                                 : "EmergencyStopInactive",
                             lastControlThreadTimestamp};


        //update
        if (haveSensorAndControlValsChanged)
        {
            timingMap["UnitUpdate"] =
                publishUnitUpdates(timingMap, controlThreadOutputBuffer, activatedControllers);
            timingMap["SensorUpdates"] =
                publishSensorUpdates(publishToObserver, controlThreadOutputBuffer);
        }
        else
        {
            const double sensSecondsAgo =
                (TimeUtil::GetTime() - publishNewSensorDataTime).toSecondsDouble();
            if (sensSecondsAgo > 1)
            {
                ARMARX_WARNING << deactivateSpam(spamdelay)
                               << "armarx::RobotUnit::publish: Last sensor value update is "
                               << sensSecondsAgo << " seconds ago!";
            }
        }
        if (haveSensorAndControlValsChanged || haveActivatedControllersChanged)
        {
            timingMap["ControlUpdates"] = publishControlUpdates(controlThreadOutputBuffer,
                                                                haveSensorAndControlValsChanged,
                                                                publishToObserver,
                                                                activatedControllers,
                                                                requestedJointControllers);
        }
        //call publish hook + publish NJointControllerBase changes
        timingMap["NJointControllerUpdates"] =
            publishNJointControllerUpdates(timingMap, controlThreadOutputBuffer);

        //report new class names
        timingMap["ClassNameUpdates"] = publishNJointClassNames();
        timingMap["RobotUnitListenerFlush"] = new TimedVariant{
            ARMARX_STOPWATCH(TimestampVariant){robotUnitListenerBatchPrx->ice_flushBatchRequests();
    }

    , lastControlThreadTimestamp
}; // namespace armarx::RobotUnitModule

if (publishToObserver)
{
    timingMap["LastPublishLoop"] =
        new TimedVariant{TimestampVariant{lastPublishLoop}, lastControlThreadTimestamp};
    if (robotUnitObserver->getState() >= eManagedIceObjectStarted)
    {
        if (observerPublishTimingInformation)
        {
            robotUnitObserver->offerOrUpdateDataFieldsFlatCopy_async(
                robotUnitObserver->additionalChannel, std::move(additionalMap));
        }
        if (observerPublishAdditionalInformation)
        {
            robotUnitObserver->offerOrUpdateDataFieldsFlatCopy_async(
                robotUnitObserver->timingChannel, std::move(timingMap));
        }
    }
}

//warn if there are unset jointCtrl controllers
{
    const auto& jointCtrls = _module<ControlThreadDataBuffer>().getActivatedJointControllers();
    if (std::any_of(jointCtrls.begin(),
                    jointCtrls.end(),
                    [](const JointController* jointCtrl) { return !jointCtrl; }))
    {
        ARMARX_WARNING << deactivateSpam(5)
                       << "armarx::RobotUnit::publish: Some activated JointControllers are "
                          "reported to be nullptr! "
                       << "(did you forget to call rtSwitchControllerSetup()?)";
    }
}
++publishIterationCount;
lastPublishLoop = TimeUtil::GetTime(true) - begPublish;
}
}