RobotUnitModuleDevices.cpp

Go to the documentation of this file.

/*
 * 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 "RobotUnitModuleDevices.h"
 
#include <ArmarXCore/core/util/StringHelpers.h>
#include <ArmarXCore/core/util/algorithm.h>
 
#include "RobotAPI/components/units/RobotUnit/Devices/GlobalRobotPoseSensorDevice.h"
 
#include "RobotUnitModuleControlThreadDataBuffer.h"
#include "RobotUnitModuleRobotData.h"
 
namespace armarx::RobotUnitModule
{
    const std::string Devices::rtThreadTimingsSensorDeviceName = "RTThreadTimings";
 
    ControlDeviceDescription
    Devices::getControlDeviceDescription(const std::string& name, const Ice::Current&) const
    {
        throwIfInControlThread(BOOST_CURRENT_FUNCTION);
        throwIfDevicesNotReady(__FUNCTION__);
        std::lock_guard<MutexType> guard{controlDevicesMutex};
        if (!controlDevices.has(name))
        {
            std::stringstream ss;
            ss << "getControlDeviceDescription: There is no ControlDevice '" << name
               << "'. There are these ControlDevices: " << controlDevices.keys();
            throw InvalidArgumentException{ss.str()};
        }
        return getControlDeviceDescription(controlDevices.index(name));
    }
 
    ControlDeviceDescriptionSeq
    Devices::getControlDeviceDescriptions(const Ice::Current&) const
    {
        throwIfInControlThread(BOOST_CURRENT_FUNCTION);
        if (!areDevicesReady())
        {
            return {};
        }
        std::lock_guard<MutexType> guard{controlDevicesMutex};
        ControlDeviceDescriptionSeq r;
        r.reserve(getNumberOfControlDevices());
        for (auto idx : getIndices(controlDevices.values()))
        {
            r.emplace_back(getControlDeviceDescription(idx));
        }
        throwIfDevicesNotReady(__FUNCTION__);
        return r;
    }
 
    std::size_t
    Devices::getNumberOfControlDevices() const
    {
        throwIfInControlThread(BOOST_CURRENT_FUNCTION);
        throwIfDevicesNotReady(__FUNCTION__);
        return controlDevices.size();
    }
 
    std::size_t
    Devices::getNumberOfSensorDevices() const
    {
        throwIfInControlThread(BOOST_CURRENT_FUNCTION);
        throwIfDevicesNotReady(__FUNCTION__);
        return sensorDevices.size();
    }
 
    std::size_t
    Devices::getSensorDeviceIndex(const std::string& deviceName) const
    {
        throwIfInControlThread(BOOST_CURRENT_FUNCTION);
        throwIfDevicesNotReady(__FUNCTION__);
        std::lock_guard<MutexType> guard{sensorDevicesMutex};
        ARMARX_CHECK_EXPRESSION(sensorDevices.has(deviceName));
        return sensorDevices.index(deviceName);
    }
 
    std::size_t
    Devices::getControlDeviceIndex(const std::string& deviceName) const
    {
        throwIfInControlThread(BOOST_CURRENT_FUNCTION);
        throwIfDevicesNotReady(__FUNCTION__);
        std::lock_guard<MutexType> guard{controlDevicesMutex};
        ARMARX_CHECK_EXPRESSION(controlDevices.has(deviceName));
        return controlDevices.index(deviceName);
    }
 
    ConstSensorDevicePtr
    Devices::getSensorDevice(const std::string& sensorDeviceName) const
    {
        throwIfInControlThread(BOOST_CURRENT_FUNCTION);
        throwIfDevicesNotReady(__FUNCTION__);
        std::lock_guard<MutexType> guard{sensorDevicesMutex};
        return sensorDevices.at(sensorDeviceName, SensorDevice::NullPtr);
    }
 
    ConstControlDevicePtr
    Devices::getControlDevice(const std::string& deviceName) const
    {
        throwIfInControlThread(BOOST_CURRENT_FUNCTION);
        throwIfDevicesNotReady(__FUNCTION__);
        std::lock_guard<MutexType> guard{controlDevicesMutex};
        return controlDevices.at(deviceName, ControlDevice::NullPtr);
    }
 
    Ice::StringSeq
    Devices::getControlDeviceNames(const Ice::Current&) const
    {
        throwIfInControlThread(BOOST_CURRENT_FUNCTION);
        throwIfDevicesNotReady(__FUNCTION__);
        std::lock_guard<MutexType> guard{controlDevicesMutex};
        return controlDevices.keys();
    }
 
    ControlDeviceDescription
    Devices::getControlDeviceDescription(std::size_t idx) const
    {
        throwIfInControlThread(BOOST_CURRENT_FUNCTION);
        throwIfDevicesNotReady(__FUNCTION__);
        std::lock_guard<MutexType> guard{controlDevicesMutex};
        const ControlDevicePtr& controlDevice = controlDevices.at(idx);
        ControlDeviceDescription data;
        data.deviceName = controlDevice->getDeviceName();
        data.tags.assign(controlDevice->getTags().begin(), controlDevice->getTags().end());
        for (const auto& jointCtrl : controlDevice->getJointControllers())
        {
            data.contolModeToTargetType[jointCtrl->getControlMode()].targetType =
                jointCtrl->getControlTarget()->getControlTargetType();
            data.contolModeToTargetType[jointCtrl->getControlMode()].hardwareControlMode =
                jointCtrl->getHardwareControlMode();
        }
        throwIfDevicesNotReady(__FUNCTION__);
        return data;
    }
 
    ControlDeviceStatus
    Devices::getControlDeviceStatus(std::size_t idx) const
    {
        throwIfInControlThread(BOOST_CURRENT_FUNCTION);
        throwIfDevicesNotReady(__FUNCTION__);
        std::lock_guard<MutexType> guard{controlDevicesMutex};
        const ControlDevicePtr& controlDevice = controlDevices.at(idx);
        ControlDeviceStatus status;
        const auto activeJointCtrl =
            _module<ControlThreadDataBuffer>().getActivatedJointControllers().at(idx);
        status.activeControlMode = activeJointCtrl ? activeJointCtrl->getControlMode()
                                                   : std::string{"!!JointController is nullptr!!"};
        status.deviceName = controlDevice->getDeviceName();
        const auto requestedJointControllers =
            _module<ControlThreadDataBuffer>().copyRequestedJointControllers();
        ARMARX_CHECK_EXPRESSION(requestedJointControllers.at(idx));
        status.requestedControlMode = requestedJointControllers.at(idx)->getControlMode();
        for (const auto& targ :
             _module<ControlThreadDataBuffer>().getSensorAndControlBuffer().control.at(idx))
        {
            status.controlTargetValues[targ->getControlMode()] =
                targ->toVariants(_module<ControlThreadDataBuffer>()
                                     .getSensorAndControlBuffer()
                                     .sensorValuesTimestamp);
        }
        status.timestampUSec = TimeUtil::GetTime(true).toMicroSeconds();
        throwIfDevicesNotReady(__FUNCTION__);
        return status;
    }
 
    ControlDeviceStatus
    Devices::getControlDeviceStatus(const std::string& name, const Ice::Current&) const
    {
        throwIfInControlThread(BOOST_CURRENT_FUNCTION);
        throwIfDevicesNotReady(__FUNCTION__);
        std::lock_guard<MutexType> guard{controlDevicesMutex};
        if (!controlDevices.has(name))
        {
            std::stringstream ss;
            ss << "getControlDeviceStatus: There is no ControlDevice '" << name
               << "'. There are these ControlDevices: " << controlDevices.keys();
            throw InvalidArgumentException{ss.str()};
        }
        return getControlDeviceStatus(controlDevices.index(name));
    }
 
    ControlDeviceStatusSeq
    Devices::getControlDeviceStatuses(const Ice::Current&) const
    {
        throwIfInControlThread(BOOST_CURRENT_FUNCTION);
        if (!areDevicesReady())
        {
            return {};
        }
        std::lock_guard<MutexType> guard{controlDevicesMutex};
        ControlDeviceStatusSeq r;
        r.reserve(getNumberOfControlDevices());
        for (auto idx : getIndices(controlDevices.values()))
        {
            r.emplace_back(getControlDeviceStatus(idx));
        }
        throwIfDevicesNotReady(__FUNCTION__);
        return r;
    }
 
    Ice::StringSeq
    Devices::getSensorDeviceNames(const Ice::Current&) const
    {
        throwIfInControlThread(BOOST_CURRENT_FUNCTION);
        throwIfDevicesNotReady(__FUNCTION__);
        std::lock_guard<MutexType> guard{sensorDevicesMutex};
        return sensorDevices.keys();
    }
 
    SensorDeviceDescription
    Devices::getSensorDeviceDescription(std::size_t idx) const
    {
        throwIfInControlThread(BOOST_CURRENT_FUNCTION);
        throwIfDevicesNotReady(__FUNCTION__);
        std::lock_guard<MutexType> guard{sensorDevicesMutex};
        const SensorDevicePtr& sensorDevice = sensorDevices.at(idx);
        SensorDeviceDescription data;
        data.deviceName = sensorDevice->getDeviceName();
        data.tags.assign(sensorDevice->getTags().begin(), sensorDevice->getTags().end());
        data.sensorValueType = sensorDevice->getSensorValueType();
        throwIfDevicesNotReady(__FUNCTION__);
        return data;
    }
 
    SensorDeviceStatus
    Devices::getSensorDeviceStatus(std::size_t idx) const
    {
        throwIfInControlThread(BOOST_CURRENT_FUNCTION);
        throwIfDevicesNotReady(__FUNCTION__);
        std::lock_guard<MutexType> guard{sensorDevicesMutex};
        const SensorDevicePtr& sensorDevice = sensorDevices.at(idx);
        SensorDeviceStatus status;
        status.deviceName = sensorDevice->getDeviceName();
        status.sensorValue = _module<ControlThreadDataBuffer>()
                                 .getSensorAndControlBuffer()
                                 .sensors.at(idx)
                                 ->toVariants(_module<ControlThreadDataBuffer>()
                                                  .getSensorAndControlBuffer()
                                                  .sensorValuesTimestamp);
        status.timestampUSec = TimeUtil::GetTime(true).toMicroSeconds();
        throwIfDevicesNotReady(__FUNCTION__);
        return status;
    }
 
    SensorDeviceDescription
    Devices::getSensorDeviceDescription(const std::string& name, const Ice::Current&) const
    {
        throwIfInControlThread(BOOST_CURRENT_FUNCTION);
        throwIfDevicesNotReady(__FUNCTION__);
        std::lock_guard<MutexType> guard{sensorDevicesMutex};
        if (!sensorDevices.has(name))
        {
            std::stringstream ss;
            ss << "getSensorDeviceDescription: There is no SensorDevice '" << name
               << "'. There are these SensorDevices: " << sensorDevices.keys();
            throw InvalidArgumentException{ss.str()};
        }
        return getSensorDeviceDescription(sensorDevices.index(name));
    }
 
    SensorDeviceDescriptionSeq
    Devices::getSensorDeviceDescriptions(const Ice::Current&) const
    {
        throwIfInControlThread(BOOST_CURRENT_FUNCTION);
        std::lock_guard<MutexType> guard{sensorDevicesMutex};
        if (!areDevicesReady())
        {
            return {};
        }
        SensorDeviceDescriptionSeq r;
        r.reserve(getNumberOfSensorDevices());
        for (auto idx : getIndices(sensorDevices.values()))
        {
            r.emplace_back(getSensorDeviceDescription(idx));
        }
        return r;
    }
 
    SensorDeviceStatus
    Devices::getSensorDeviceStatus(const std::string& name, const Ice::Current&) const
    {
        throwIfInControlThread(BOOST_CURRENT_FUNCTION);
        throwIfDevicesNotReady(__FUNCTION__);
        std::lock_guard<MutexType> guard{sensorDevicesMutex};
        if (!sensorDevices.has(name))
        {
            std::stringstream ss;
            ss << "getSensorDeviceStatus: There is no SensorDevice '" << name
               << "'. There are these SensorDevices: " << sensorDevices.keys();
            throw InvalidArgumentException{ss.str()};
        }
        return getSensorDeviceStatus(sensorDevices.index(name));
    }
 
    SensorDeviceStatusSeq
    Devices::getSensorDeviceStatuses(const Ice::Current&) const
    {
        throwIfInControlThread(BOOST_CURRENT_FUNCTION);
        if (!areDevicesReady())
        {
            return {};
        }
        std::lock_guard<MutexType> guard{sensorDevicesMutex};
        SensorDeviceStatusSeq r;
        r.reserve(getNumberOfSensorDevices());
        for (auto idx : getIndices(sensorDevices.values()))
        {
            r.emplace_back(getSensorDeviceStatus(idx));
        }
        throwIfDevicesNotReady(__FUNCTION__);
        return r;
    }
 
    void
    Devices::addControlDevice(const ControlDevicePtr& cd)
    {
        throwIfInControlThread(BOOST_CURRENT_FUNCTION);
        ARMARX_DEBUG << "ControlDevice " << &cd;
        throwIfStateIsNot(RobotUnitState::InitializingDevices, __FUNCTION__);
        {
            std::lock_guard<MutexType> guard{controlDevicesMutex};
            //check it
            if (!cd)
            {
                std::stringstream ss;
                ss << "armarx::RobotUnit::addControlDevice: ControlDevice is nullptr";
                ARMARX_ERROR << ss.str();
                throw InvalidArgumentException{ss.str()};
            }
            if (!cd->getJointEmergencyStopController())
            {
                std::stringstream ss;
                ss << "armarx::RobotUnit::addControlDevice: ControlDevice " << cd->getDeviceName()
                   << " has null JointEmergencyStopController (this is not allowed)";
                ARMARX_ERROR << ss.str();
                throw InvalidArgumentException{ss.str()};
            }
            if (!cd->getJointStopMovementController())
            {
                std::stringstream ss;
                ss << "armarx::RobotUnit::addControlDevice: ControlDevice " << cd->getDeviceName()
                   << " has null getJointStopMovementController (this is not allowed)";
                ARMARX_ERROR << ss.str();
                throw InvalidArgumentException{ss.str()};
            }
            //add it
            ARMARX_DEBUG << "Adding the ControlDevice " << cd->getDeviceName() << " " << &cd;
            controlDevices.add(cd->getDeviceName(), cd);
            cd->owner = this;
            ARMARX_INFO << "added ControlDevice " << cd->getDeviceName();
        }
        ARMARX_INFO << "added ControlDevice " << cd->getDeviceName();
        throwIfStateIsNot(RobotUnitState::InitializingDevices, __FUNCTION__);
    }
 
    void
    Devices::addSensorDevice(const SensorDevicePtr& sd)
    {
        throwIfInControlThread(BOOST_CURRENT_FUNCTION);
        ARMARX_DEBUG << "SensorDevice " << &sd;
        throwIfStateIsNot(RobotUnitState::InitializingDevices, __FUNCTION__);
        {
            std::lock_guard<MutexType> guard{sensorDevicesMutex};
            //check it
            if (!sd)
            {
                std::stringstream ss;
                ss << "armarx::RobotUnit::addSensorDevice: SensorDevice is nullptr";
                ARMARX_ERROR << ss.str();
                throw InvalidArgumentException{ss.str()};
            }
            if (!sd->getSensorValue())
            {
                std::stringstream ss;
                ss << "armarx::RobotUnit::addSensorDevice: SensorDevice " << sd->getDeviceName()
                   << " has null SensorValue (this is not allowed)";
                ARMARX_ERROR << ss.str();
                throw InvalidArgumentException{ss.str()};
            }
            //add it
            if (sd->getDeviceName() == rtThreadTimingsSensorDeviceName)
            {
                ARMARX_DEBUG << "Device is the " << rtThreadTimingsSensorDeviceName;
                if (!std::dynamic_pointer_cast<RTThreadTimingsSensorDevice>(sd))
                {
                    throw InvalidArgumentException{
                        "You tried to add a SensorDevice with the name " + sd->getDeviceName() +
                        " which does not derive from RTThreadTimingsSensorDevice. (Don't do this)"};
                }
                //this checks if we already added such a device (do this before setting timingSensorDevice)
                ARMARX_DEBUG << "Adding the SensorDevice " << sd->getDeviceName() << " " << &sd;
                sensorDevices.add(sd->getDeviceName(), sd);
                sd->owner = this;
                rtThreadTimingsSensorDevice =
                    std::dynamic_pointer_cast<RTThreadTimingsSensorDevice>(sd);
            }
            else if (sd->getDeviceName() == GlobalRobotLocalizationSensorDevice::DeviceName())
            {
                ARMARX_DEBUG << "Device is the " << sd->getDeviceName();
                if (!std::dynamic_pointer_cast<GlobalRobotLocalizationSensorDevice>(sd))
                {
                    throw InvalidArgumentException{
                        "You tried to add a SensorDevice with the name " + sd->getDeviceName() +
                        " which does not derive from GlobalRobotLocalizationSensorDevice. (Don't "
                        "do this)"};
                }
                //this checks if we already added such a device (do this before setting timingSensorDevice)
                ARMARX_DEBUG << "Adding the SensorDevice " << sd->getDeviceName() << " " << &sd;
                sensorDevices.add(sd->getDeviceName(), sd);
                sd->owner = this;
                globalRobotLocalizationSensorDevice =
                    sd; //std::dynamic_pointer_cast<GlobalRobotLocalizationSensorDevice>(sd);
            }
            else
            {
                ARMARX_DEBUG << "Adding the SensorDevice " << sd->getDeviceName() << " " << &sd;
                sensorDevices.add(sd->getDeviceName(), sd);
                sd->owner = this;
            }
        }
        ARMARX_INFO << "added SensorDevice " << sd->getDeviceName()
                    << " (valuetype = " << sd->getSensorValueType() << ")";
        throwIfStateIsNot(RobotUnitState::InitializingDevices, __FUNCTION__);
    }
 
    RTThreadTimingsSensorDevicePtr
    Devices::createRTThreadTimingSensorDevice() const
    {
        throwIfInControlThread(BOOST_CURRENT_FUNCTION);
        return std::make_shared<RTThreadTimingsSensorDeviceImpl<>>(rtThreadTimingsSensorDeviceName);
    }
 
    void
    Devices::_postFinishRunning()
    {
        throwIfInControlThread(BOOST_CURRENT_FUNCTION);
        std::lock_guard<MutexType> guardS{sensorDevicesMutex};
        std::lock_guard<MutexType> guardC{controlDevicesMutex};
        controlDevicesConstPtr.clear();
        sensorDevicesConstPtr.clear();
        sensorDevices.clear();
        controlDevices.clear();
    }
 
    std::vector<JointController*>
    Devices::getStopMovementJointControllers() const
    {
        throwIfInControlThread(BOOST_CURRENT_FUNCTION);
        throwIfDevicesNotReady(__FUNCTION__);
        std::lock_guard<MutexType> guard{controlDevicesMutex};
        std::vector<JointController*> controllers;
        controllers.reserve(controlDevices.values().size());
        for (const ControlDevicePtr& dev : controlDevices.values())
        {
            ARMARX_CHECK_NOT_NULL(dev);
            controllers.emplace_back(dev->rtGetJointStopMovementController());
            ARMARX_CHECK_NOT_NULL(controllers.back());
        }
        ARMARX_CHECK_EQUAL(controlDevices.size(), controllers.size());
        throwIfDevicesNotReady(__FUNCTION__);
        return controllers;
    }
 
    std::vector<JointController*>
    Devices::getEmergencyStopJointControllers() const
    {
        throwIfInControlThread(BOOST_CURRENT_FUNCTION);
        throwIfDevicesNotReady(__FUNCTION__);
        std::lock_guard<MutexType> guard{controlDevicesMutex};
        std::vector<JointController*> controllers;
        controllers.reserve(controlDevices.values().size());
        for (const ControlDevicePtr& dev : controlDevices.values())
        {
            ARMARX_CHECK_NOT_NULL(dev);
            controllers.emplace_back(dev->rtGetJointEmergencyStopController());
            ARMARX_CHECK_NOT_NULL(controllers.back());
        }
        ARMARX_CHECK_EQUAL(controlDevices.size(), controllers.size());
        throwIfDevicesNotReady(__FUNCTION__);
        return controllers;
    }
 
    void
    Devices::_preFinishDeviceInitialization()
    {
        throwIfInControlThread(BOOST_CURRENT_FUNCTION);
        std::lock_guard<MutexType> guardS{sensorDevicesMutex};
        std::lock_guard<MutexType> guardC{controlDevicesMutex};
        if (!sensorDevices.has(rtThreadTimingsSensorDeviceName))
        {
            addSensorDevice(createRTThreadTimingSensorDevice());
        }
 
        // this device will be used by the PlatformUnit to make the robot's global pose
        // available to e.g. the NJointControllers.
        addSensorDevice(std::make_shared<GlobalRobotPoseCorrectionSensorDevice>());
        addSensorDevice(std::make_shared<GlobalRobotLocalizationSensorDevice>());
    }
 
    void
    Devices::_postFinishDeviceInitialization()
    {
        ARMARX_TRACE;
        throwIfInControlThread(BOOST_CURRENT_FUNCTION);
        std::lock_guard<MutexType> guardS{sensorDevicesMutex};
        std::lock_guard<MutexType> guardC{controlDevicesMutex};
        ARMARX_DEBUG << "checking " << controlDevices.size() << " ControlDevices:";
        {
            ARMARX_TRACE;
            for (const ControlDevicePtr& controlDevice : controlDevices.values())
            {
                ARMARX_CHECK_EXPRESSION(controlDevice);
                ARMARX_DEBUG << "----" << controlDevice->getDeviceName();
                if (!controlDevice->hasJointController(ControlModes::EmergencyStop))
                {
                    std::stringstream s;
                    s << "ControlDevice " << controlDevice->getDeviceName()
                      << " has no JointController with ControlMode " << ControlModes::EmergencyStop
                      << " (to fix this, add a JointController with this ControlMode to the "
                         "ControlDevice).\nAvailable controllers: "
                      << controlDevice->getControlModes();
                    ARMARX_ERROR << "--------" << s.str();
                    throw LogicError{s.str()};
                }
                if (!controlDevice->hasJointController(ControlModes::StopMovement))
                {
                    std::stringstream s;
                    s << "ControlDevice " << controlDevice->getDeviceName()
                      << " has no JointController with ControlMode \"" << ControlModes::StopMovement
                      << "\" (to fix this, add a JointController with this ControlMode to the "
                         "ControlDevice) \nAvailable controllers: "
                      << controlDevice->getControlModes();
                    ARMARX_ERROR << "--------" << s.str();
                    throw LogicError{s.str()};
                }
            }
        }
        ARMARX_DEBUG << "checking " << controlDevices.size() << " SensorDevices:";
        {
            ARMARX_TRACE;
            for (const SensorDevicePtr& sensorDevice : sensorDevices.values())
            {
                ARMARX_CHECK_EXPRESSION(sensorDevice);
                ARMARX_DEBUG << "----" << sensorDevice->getDeviceName();
                if (!sensorDevice->getSensorValue())
                {
                    std::stringstream s;
                    s << "SensorDevice " << sensorDevice->getSensorValue()
                      << " has null SensorValue";
                    ARMARX_ERROR << "--------" << s.str();
                    throw LogicError{s.str()};
                }
                ARMARX_CHECK_EXPRESSION(sensorDevice);
            }
        }
        ARMARX_DEBUG << "copying device ptrs to const ptr map";
        {
            ARMARX_TRACE;
            for (const auto& dev : controlDevices.values())
            {
                controlDevicesConstPtr[dev->getDeviceName()] = dev;
            }
            for (const auto& dev : sensorDevices.values())
            {
                sensorDevicesConstPtr[dev->getDeviceName()] = dev;
            }
        }
        ARMARX_DEBUG << "copy sensor values";
        {
            ARMARX_TRACE;
            ARMARX_CHECK_EXPRESSION(sensorValues.empty());
            sensorValues.reserve(sensorDevices.values().size());
            for (const SensorDevicePtr& dev : sensorDevices.values())
            {
                ARMARX_CHECK_NOT_NULL(dev);
                sensorValues.emplace_back(dev->getSensorValue());
                ARMARX_CHECK_NOT_NULL(sensorValues.back());
            }
        }
        ARMARX_DEBUG << "copy control targets";
        {
            ARMARX_TRACE;
            ARMARX_CHECK_EXPRESSION(controlTargets.empty());
            controlTargets.reserve(controlDevices.values().size());
            for (const ControlDevicePtr& dev : controlDevices.values())
            {
                ARMARX_CHECK_NOT_NULL(dev);
                controlTargets.emplace_back();
                controlTargets.back().reserve(dev->rtGetJointControllers().size());
                for (JointController* ctrl : dev->rtGetJointControllers())
                {
                    ARMARX_CHECK_NOT_NULL(ctrl);
                    controlTargets.back().emplace_back(ctrl->getControlTarget());
                    ARMARX_CHECK_NOT_NULL(controlTargets.back().back().get());
                }
            }
        }
        ARMARX_DEBUG << "setup ControlDeviceHardwareControlModeGroups";
        {
            ARMARX_TRACE;
            ctrlModeGroups.groupIndices.assign(getNumberOfControlDevices(), IndexSentinel());
 
            if (!ctrlModeGroups.groupsMerged.empty())
            {
                ARMARX_DEBUG << "Remove control devs from ControlDeviceHardwareControlModeGroups ("
                             << ctrlModeGroups.groups.size() << ")";
                const auto groupsMerged = ctrlModeGroups.groupsMerged;
                for (const auto& dev : groupsMerged)
                {
                    if (controlDevices.has(dev))
                    {
                        continue;
                    }
                    ctrlModeGroups.groupsMerged.erase(dev);
                    for (auto& group : ctrlModeGroups.groups)
                    {
                        group.erase(dev);
                    }
                    ARMARX_DEBUG << "----removing nonexistent device: " << dev;
                }
                //remove empty groups
                std::vector<std::set<std::string>> cleanedGroups;
                cleanedGroups.reserve(ctrlModeGroups.groups.size());
                for (auto& group : ctrlModeGroups.groups)
                {
                    const auto sz = group.size();
                    if (sz == 1)
                    {
                        ARMARX_DEBUG << "----removing group with one dev: " << *group.begin();
                    }
                    else if (sz > 1)
                    {
                        cleanedGroups.emplace_back(std::move(group));
                    }
                }
                ctrlModeGroups.groups = cleanedGroups;
                ARMARX_DEBUG << "----number of groups left: " << ctrlModeGroups.groups.size();
            }
            if (!ctrlModeGroups.groupsMerged.empty())
            {
                ARMARX_DEBUG
                    << "Checking control modes for ControlDeviceHardwareControlModeGroups ("
                    << ctrlModeGroups.groups.size() << ")";
                ctrlModeGroups.deviceIndices.resize(ctrlModeGroups.groups.size());
                //iterate over groups
                for (std::size_t groupIdx = 0; groupIdx < ctrlModeGroups.groups.size(); ++groupIdx)
                {
                    const std::set<std::string>& group = ctrlModeGroups.groups.at(groupIdx);
                    ctrlModeGroups.deviceIndices.at(groupIdx).reserve(group.size());
                    ARMARX_CHECK_EXPRESSION(!group.empty());
                    ARMARX_DEBUG << "----Group " << groupIdx << " size: " << group.size();
                    //gets a map of ControlMode->HardwareControlMode for the given device
                    const auto getControlModeToHWControlMode = [&](const std::string& devname)
                    {
                        std::map<std::string, std::string> controlModeToHWControlMode;
                        const ControlDevicePtr& cd = controlDevices.at(devname);
                        for (const auto& jointCtrl : cd->getJointControllers())
                        {
                            controlModeToHWControlMode[jointCtrl->getControlMode()] =
                                jointCtrl->getHardwareControlMode();
                        }
                        return controlModeToHWControlMode;
                    };
                    //get modes of first dev
                    const auto controlModeToHWControlMode =
                        getControlModeToHWControlMode(*group.begin());
                    //check other devs
                    for (const auto& devname : group)
                    {
                        ARMARX_CHECK_EXPRESSION(controlDevices.has(devname))
                            << "The ControlDeviceHardwareControlModeGroups property contains "
                               "device names not existent in the robot: "
                            << devname << "\navailable:\n"
                            << controlDevices.keys();
                        //Assert all devices in a group have the same control modes with the same hw controle modes
                        const auto controlModeToHWControlModeForDevice =
                            getControlModeToHWControlMode(devname);
                        ARMARX_CHECK_EXPRESSION(controlModeToHWControlModeForDevice ==
                                                controlModeToHWControlMode)
                            << "Error for control modes of device '" << devname << "'\n"
                            << "it has the modes: " << controlModeToHWControlModeForDevice
                            << "\n but should have the modes: " << controlModeToHWControlMode;
                        //insert the device index into the device indices
                        const auto devIdx = controlDevices.index(devname);
                        ctrlModeGroups.deviceIndices.at(groupIdx).emplace_back(devIdx);
                        //insert the group index into the group indices (+ check the current group index is the sentinel
                        ARMARX_CHECK_EXPRESSION(ctrlModeGroups.groupIndices.size() > devIdx);
                        ARMARX_CHECK_EXPRESSION(ctrlModeGroups.groupIndices.at(devIdx) ==
                                                IndexSentinel());
                        ctrlModeGroups.groupIndices.at(devIdx) = groupIdx;
                        ARMARX_DEBUG << "------- " << devname;
                    }
                }
            }
        }
        ARMARX_DEBUG << "create mapping from sensor values to robot nodes";
        {
            ARMARX_TRACE;
            ARMARX_CHECK_EXPRESSION(simoxRobotSensorValueMapping.empty());
            VirtualRobot::RobotPtr r = _module<RobotData>().cloneRobot();
            const auto nodes = r->getRobotNodes();
            for (std::size_t idxRobot = 0; idxRobot < nodes.size(); ++idxRobot)
            {
                const VirtualRobot::RobotNodePtr& node = nodes.at(idxRobot);
                if (node->isJoint())
                {
                    const auto& name = node->getName();
                    if (sensorDevices.has(name))
                    {
                        const auto& dev = sensorDevices.at(name);
                        if (dev->getSensorValue()->isA<SensorValue1DoFActuatorPosition>())
                        {
                            SimoxRobotSensorValueMapping m;
                            m.idxRobot = idxRobot;
                            m.idxSens = sensorDevices.index(name);
                            simoxRobotSensorValueMapping.emplace_back(m);
                        }
                        else
                        {
                            ARMARX_WARNING
                                << "SensorValue for SensorDevice " << name << " is of type "
                                << dev->getSensorValueType()
                                << " which does not derive SensorValue1DoFActuatorPosition";
                        }
                    }
                    else
                    {
                        ARMARX_INFO << "No SensorDevice for RobotNode: " << name;
                    }
                }
            }
        }
 
        ARMARX_VERBOSE << "ControlDevices:\n" << controlDevices.keys();
        ARMARX_VERBOSE << "SensorDevices:\n" << sensorDevices.keys();
    }
 
    void
    Devices::_preOnInitRobotUnit()
    {
        throwIfInControlThread(BOOST_CURRENT_FUNCTION);
        //ControlDeviceHardwareControlModeGroups
        const std::string controlDeviceHardwareControlModeGroupsStr =
            getProperty<std::string>("ControlDevices_HardwareControlModeGroups").getValue();
        if (!controlDeviceHardwareControlModeGroupsStr.empty())
        {
            const auto numGroups = std::count(controlDeviceHardwareControlModeGroupsStr.begin(),
                                              controlDeviceHardwareControlModeGroupsStr.end(),
                                              ';') +
                                   1;
            ctrlModeGroups.groups.reserve(numGroups);
            std::vector<std::string> strGroups =
                Split(controlDeviceHardwareControlModeGroupsStr, ";");
            for (const auto& gstr : strGroups)
            {
                bool trimDeviceNames = true;
                std::vector<std::string> strElems = Split(gstr, ",", trimDeviceNames);
                std::set<std::string> group;
                for (auto& device : strElems)
                {
                    ARMARX_CHECK_EXPRESSION(!device.empty())
                        << "The ControlDeviceHardwareControlModeGroups property contains empty "
                           "device names";
                    ARMARX_CHECK_EXPRESSION(!ctrlModeGroups.groupsMerged.count(device))
                        << "The ControlDeviceHardwareControlModeGroups property contains duplicate "
                           "device names: "
                        << device;
                    ctrlModeGroups.groupsMerged.emplace(device);
                    group.emplace(std::move(device));
                }
                if (!group.empty())
                {
                    ARMARX_DEBUG << "adding device group:\n" << ARMARX_STREAM_PRINTER
                    {
                        for (const auto& elem : group)
                        {
                            out << "    " << elem << "\n";
                        }
                    };
                    ctrlModeGroups.groups.emplace_back(std::move(group));
                }
            }
        }
    }
} // namespace armarx::RobotUnitModule