RobotUnitModuleUnits.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 "RobotUnitModuleUnits.h"
 
#include <ArmarXCore/core/IceGridAdmin.h>
#include <ArmarXCore/core/IceManager.h>
#include <ArmarXCore/core/time.h>
 
#include <RobotAPI/components/units/RobotUnit/Devices/GlobalRobotPoseSensorDevice.h>
#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformUnitVelocityPassThroughController.h>
#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformVelocityControllerWithRamp.h>
#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValueBattery.h>
#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValueHolonomicPlatform.h>
#include <RobotAPI/components/units/RobotUnit/Units/DiagnosticsSubUnit.h>
#include <RobotAPI/components/units/RobotUnit/Units/ForceTorqueSubUnit.h>
#include <RobotAPI/components/units/RobotUnit/Units/InertialMeasurementSubUnit.h>
#include <RobotAPI/components/units/RobotUnit/Units/KinematicSubUnit.h>
#include <RobotAPI/components/units/RobotUnit/Units/LocalizationSubUnit.h>
#include <RobotAPI/components/units/RobotUnit/Units/PlatformSubUnit.h>
#include <RobotAPI/components/units/RobotUnit/Units/TCPControllerSubUnit.h>
#include <RobotAPI/components/units/RobotUnit/Units/TrajectoryControllerSubUnit.h>
 
namespace armarx::RobotUnitModule
{
    /**
    * \brief This class allows minimal access to private members of \ref ControlThread in a sane fashion for \ref RobotUnitEmergencyStopMaster.
    * \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 ControlThreadAttorneyForRobotUnitEmergencyStopMaster
    {
        friend class RobotUnitEmergencyStopMaster;
 
        static void
        SetEmergencyStopStateNoReportToTopic(ControlThread* controlThreadModule,
                                             EmergencyStopState state)
        {
            controlThreadModule->setEmergencyStopStateNoReportToTopic(state);
        }
    };
} // namespace armarx::RobotUnitModule
 
namespace armarx::RobotUnitModule
{
    class RobotUnitEmergencyStopMaster :
        virtual public ManagedIceObject,
        virtual public EmergencyStopMasterInterface
    {
    public:
        RobotUnitEmergencyStopMaster(ControlThread* controlThreadModule,
                                     std::string emergencyStopTopicName) :
            controlThreadModule{controlThreadModule}, emergencyStopTopicName{emergencyStopTopicName}
        {
            ARMARX_CHECK_NOT_NULL(controlThreadModule);
            ARMARX_CHECK_EXPRESSION(!emergencyStopTopicName.empty());
        }
 
        void
        setEmergencyStopState(EmergencyStopState state,
                              const Ice::Current& = Ice::emptyCurrent) final override
        {
            EmergencyStopState currentState = getEmergencyStopState();
 
            const std::unique_lock lock{ss2StateMutex};
 
            if (currentState == state)
            {
                return;
            }
 
            currentState = state;
 
            if (currentState == EmergencyStopState::eEmergencyStopActive)
            {
                lastSS2Activation = armarx::core::time::DateTime::Now();
            }
 
            ControlThreadAttorneyForRobotUnitEmergencyStopMaster::
                SetEmergencyStopStateNoReportToTopic(controlThreadModule, currentState);
            emergencyStopTopic->reportEmergencyStopState(currentState);
        }
 
        EmergencyStopState
        trySetEmergencyStopState(EmergencyStopState targetState,
                                 const ::Ice::Current&) final override
        {
            armarx::core::time::Duration safeDuration =
                armarx::core::time::Duration::MilliSeconds(500);
 
            // If the SS2 should be released (target state = inactive) we check whether the last
            // activation happened at least before safeDuration.
            if (targetState == EmergencyStopState::eEmergencyStopInactive)
            {
                if (armarx::core::time::DateTime::Now() > lastSS2Activation + safeDuration)
                {
                    setEmergencyStopState(targetState);
                }
            }
            else
            {
                setEmergencyStopState(targetState);
            }
 
            return getEmergencyStopState();
        }
 
        EmergencyStopState
        getEmergencyStopState(const Ice::Current& = Ice::emptyCurrent) const final override
        {
            const std::shared_lock lock{ss2StateMutex};
            return controlThreadModule->getEmergencyStopState();
        }
 
    protected:
        void
        onInitComponent() final override
        {
            armarx::ManagedIceObject::offeringTopic(emergencyStopTopicName);
        }
 
        void
        onConnectComponent() final override
        {
            emergencyStopTopic = armarx::ManagedIceObject::getTopic<EmergencyStopListenerPrx>(
                emergencyStopTopicName);
            setEmergencyStopState(EmergencyStopState::eEmergencyStopInactive);
        }
 
        std::string
        getDefaultName() const final override
        {
            return "EmergencyStopMaster";
        }
 
    protected:
        ControlThread* const controlThreadModule;
        const std::string emergencyStopTopicName;
        EmergencyStopListenerPrx emergencyStopTopic;
        mutable std::shared_mutex ss2StateMutex;
        armarx::core::time::DateTime lastSS2Activation;
    };
} // namespace armarx::RobotUnitModule
 
namespace armarx::RobotUnitModule
{
    Ice::ObjectProxySeq
    Units::getUnits(const Ice::Current&) const
    {
        throwIfInControlThread(BOOST_CURRENT_FUNCTION);
        std::vector<ManagedIceObjectPtr> unitsCopy;
        {
            auto guard = getGuard();
            //copy to keep lock retention time low
            unitsCopy = units;
        }
        Ice::ObjectProxySeq r;
        r.reserve(unitsCopy.size());
        for (const ManagedIceObjectPtr& unit : unitsCopy)
        {
            r.emplace_back(unit->getProxy(-1, true));
        }
        return r;
    }
 
    Ice::ObjectPrx
    Units::getUnit(const std::string& staticIceId, const Ice::Current&) const
    {
        throwIfInControlThread(BOOST_CURRENT_FUNCTION);
        //no lock required
        ManagedIceObjectPtr unit = getUnit(staticIceId);
        if (unit)
        {
            return unit->getProxy(-1, true);
        }
        return nullptr;
    }
 
    const EmergencyStopMasterInterfacePtr&
    Units::getEmergencyStopMaster() const
    {
        throwIfInControlThread(BOOST_CURRENT_FUNCTION);
        return emergencyStopMaster;
    }
 
    void
    Units::initializeDefaultUnits()
    {
        ARMARX_TRACE;
        throwIfInControlThread(BOOST_CURRENT_FUNCTION);
        auto beg = TimeUtil::GetTime(true);
        {
            ARMARX_TRACE;
            auto guard = getGuard();
 
            ARMARX_TRACE;
            throwIfStateIsNot(RobotUnitState::InitializingUnits, __FUNCTION__);
            ARMARX_INFO << "initializing default units";
 
            ARMARX_TRACE;
            initializeDiagnosticsUnit();
            ARMARX_DEBUG << "Diagnostics unit initialized.";
 
            ARMARX_TRACE;
            initializeKinematicUnit();
            ARMARX_DEBUG << "KinematicUnit initialized";
 
            ARMARX_TRACE;
            if (_module<RobotData>().arePlatformAndLocalizationUnitsEnabled())
            {
                ARMARX_DEBUG << "initializing LocalizationUnit";
                initializeLocalizationUnit();
                ARMARX_DEBUG << "LocalizationUnit initialized";
            }
 
            ARMARX_TRACE;
            if (_module<RobotData>().arePlatformAndLocalizationUnitsEnabled())
            {
                ARMARX_DEBUG << "initializing PlatformUnit";
                initializePlatformUnit();
                ARMARX_DEBUG << "PlatformUnit initialized";
            }
 
            ARMARX_TRACE;
            initializeForceTorqueUnit();
            ARMARX_DEBUG << "ForceTorqueUnit initialized";
 
            ARMARX_TRACE;
            initializeInertialMeasurementUnit();
            ARMARX_DEBUG << "InertialMeasurementUnit initialized";
 
            ARMARX_TRACE;
            initializeTrajectoryControllerUnit();
            ARMARX_DEBUG << "TrajectoryControllerUnit initialized";
 
            ARMARX_TRACE;
            initializeTcpControllerUnit();
            ARMARX_DEBUG << "TcpControllerUnit initialized";
 
            ARMARX_TRACE;
        }
        ARMARX_INFO << "initializing default units...done! "
                    << (TimeUtil::GetTime(true) - beg).toMicroSeconds() << " us";
    }
 
    void
    Units::initializeDiagnosticsUnit()
    {
        throwIfInControlThread(BOOST_CURRENT_FUNCTION);
        using UnitT = DiagnosticsSubUnit;
        using IfaceT = DiagnosticsUnitInterface;
 
        auto guard = getGuard();
        throwIfStateIsNot(RobotUnitState::InitializingUnits, __FUNCTION__);
        // Check if unit is already added.
        if (getUnit(IfaceT::ice_staticId()))
        {
            return;
        }
 
        // Check if it is required.
        std::map<std::string, std::size_t> batteryDevs;
        for (auto i : getIndices(_module<Devices>().getSensorDevices().keys()))
        {
            const SensorDevicePtr& sensorDevice = _module<Devices>().getSensorDevices().at(i);
            if (sensorDevice->getSensorValue()->isA<SensorValueBattery>())
            {
                batteryDevs[sensorDevice->getDeviceName()] = i;
            }
        }
 
        const std::string configName = getProperty<std::string>("DiagnosticsUnitName");
        const std::string confPre = getConfigDomain() + "." + configName + ".";
        Ice::PropertiesPtr properties = getIceProperties()->clone();
 
        // Create and add unit.
        IceInternal::Handle<UnitT> unit =
            Component::create<UnitT>(properties, configName, getConfigDomain());
        unit->setBatteryManagementDevices(batteryDevs);
        addUnit(std::move(unit));
    }
 
    void
    Units::initializeKinematicUnit()
    {
        throwIfInControlThread(BOOST_CURRENT_FUNCTION);
        using IfaceT = KinematicUnitInterface;
 
        auto guard = getGuard();
        throwIfStateIsNot(RobotUnitState::InitializingUnits, __FUNCTION__);
        //check if unit is already added
        if (getUnit(IfaceT::ice_staticId()))
        {
            return;
        }
        auto unit = createKinematicSubUnit(getIceProperties()->clone());
        //add
        if (unit)
        {
            addUnit(std::move(unit));
        }
    }
 
    ManagedIceObjectPtr
    Units::createKinematicSubUnit(const Ice::PropertiesPtr& properties,
                                  const std::string& positionControlMode,
                                  const std::string& velocityControlMode,
                                  const std::string& torqueControlMode,
                                  const std::string& pwmControlMode)
    {
        ARMARX_TRACE;
        throwIfInControlThread(BOOST_CURRENT_FUNCTION);
        /// TODO move init code to Kinematic sub unit
        using UnitT = KinematicSubUnit;
 
        //add ctrl et al
        ARMARX_DEBUG << "createKinematicSubUnit...";
        std::map<std::string, UnitT::ActuatorData> devs;
        for (const ControlDevicePtr& controlDevice :
             _module<Devices>().getControlDevices().values())
        {
            ARMARX_CHECK_NOT_NULL(controlDevice);
            const auto& controlDeviceName = controlDevice->getDeviceName();
            ARMARX_DEBUG << "looking at device " << controlDeviceName
                         << controlDevice->getJointControllerToTargetTypeNameMap();
            if (!controlDevice->hasTag(ControlDeviceTags::CreateNoDefaultController))
            {
                UnitT::ActuatorData ad;
                ad.name = controlDeviceName;
                ARMARX_DEBUG << "has sensor device: "
                             << _module<Devices>().getSensorDevices().has(controlDeviceName);
                ad.sensorDeviceIndex =
                    _module<Devices>().getSensorDevices().has(controlDeviceName)
                        ? _module<Devices>().getSensorDevices().index(controlDeviceName)
                        : std::numeric_limits<std::size_t>::max();
 
                const auto init_controller =
                    [&](const std::string& requestedControlMode, auto& ctrl, const auto* tpptr)
                {
                    auto controlMode = requestedControlMode;
                    using CtargT = std::remove_const_t<std::remove_reference_t<decltype(*tpptr)>>;
                    NJointKinematicUnitPassThroughControllerPtr& pt = ctrl;
 
                    auto testMode = [&](const auto& m)
                    {
                        JointController* jointCtrl = controlDevice->getJointController(m);
                        return jointCtrl && jointCtrl->getControlTarget()->isA<CtargT>();
                    };
 
                    if (!testMode(controlMode))
                    {
                        controlMode = "";
                        //try to find the correct mode (maybe the specified target was derived!
                        //this only works, if exactly one controller provides this mode
                        const JointController* selected_ctrl{nullptr};
                        auto jointControllers = controlDevice->getJointControllers();
                        for (const auto* jointController : jointControllers)
                        {
                            if (jointController->getControlTarget()->isA<CtargT>())
                            {
                                if (selected_ctrl)
                                {
                                    selected_ctrl = nullptr;
                                    ARMARX_INFO << "Autodetected two controllers supporting "
                                                << requestedControlMode << "! autoselection failed";
                                    break;
                                }
                                selected_ctrl = jointController;
                            }
                        }
                        if (selected_ctrl)
                        {
                            controlMode = selected_ctrl->getControlMode();
                            ARMARX_INFO << "Autodetected controller with mode " << controlMode
                                        << "(the requested mode was " << requestedControlMode
                                        << ")";
                        }
                    }
 
                    if (!controlMode.empty())
                    {
                        NJointKinematicUnitPassThroughControllerConfigPtr config =
                            new NJointKinematicUnitPassThroughControllerConfig;
                        config->controlMode = controlMode;
                        config->deviceName = controlDeviceName;
                        auto ctrl_name = getName() + "_" + "NJointKU_PTCtrl_" + controlDeviceName +
                                         "_" + controlMode;
                        std::replace(ctrl_name.begin(), ctrl_name.end(), '.', '_');
                        std::replace(ctrl_name.begin(), ctrl_name.end(), '-', '_');
                        std::replace(ctrl_name.begin(), ctrl_name.end(), ':', '_');
                        const NJointControllerBasePtr& nJointCtrl =
                            _module<ControllerManagement>().createNJointController(
                                "NJointKinematicUnitPassThroughController",
                                ctrl_name,
                                config,
                                false,
                                true);
                        pt = NJointKinematicUnitPassThroughControllerPtr::dynamicCast(nJointCtrl);
                        ARMARX_CHECK_EXPRESSION(pt);
                    }
                };
                init_controller(
                    positionControlMode, ad.ctrlPos, (ControlTarget1DoFActuatorPosition*)0);
                init_controller(
                    velocityControlMode, ad.ctrlVel, (ControlTarget1DoFActuatorVelocity*)0);
                init_controller(torqueControlMode, ad.ctrlTor, (ControlTarget1DoFActuatorTorque*)0);
                if (!ad.ctrlTor)
                {
                    init_controller(pwmControlMode, ad.ctrlTor, (ControlTarget1DoFActuatorPWM*)0);
                }
 
                if (ad.ctrlPos || ad.ctrlTor || ad.ctrlVel)
                {
                    ARMARX_DEBUG << "created controllers for " << controlDeviceName
                                 << " pos/tor/vel " << ad.ctrlPos.get() << " / " << ad.ctrlTor.get()
                                 << " / " << ad.ctrlVel.get();
                    devs[controlDeviceName] = std::move(ad);
                }
            }
        }
        if (devs.empty())
        {
            ARMARX_IMPORTANT << "No joint devices found - skipping adding of KinematicUnit";
            return nullptr;
        }
        ARMARX_IMPORTANT << "Found " << devs.size() << " joint devices - adding KinematicUnit";
        ARMARX_CHECK_EXPRESSION(!devs.empty())
            << "no 1DoF ControlDevice found with matching SensorDevice";
        //add it
        const std::string configName = getProperty<std::string>("KinematicUnitName");
        const std::string confPre = getConfigDomain() + "." + configName + ".";
        //properties->setProperty(confPre + "MinimumLoggingLevel", getProperty<std::string>("MinimumLoggingLevel").getValue());
        //fill properties
        properties->setProperty(confPre + "RobotNodeSetName",
                                _module<RobotData>().getRobotNodetSeName());
        properties->setProperty(confPre + "RobotFileName", _module<RobotData>().getRobotFileName());
        properties->setProperty(confPre + "RobotFileNameProject",
                                _module<RobotData>().getRobotProjectName());
        properties->setProperty(confPre + "TopicPrefix",
                                getProperty<std::string>("KinematicUnitNameTopicPrefix"));
 
        ARMARX_DEBUG << "creating unit " << configName
                     << " using these properties: " << properties->getPropertiesForPrefix("");
        IceInternal::Handle<UnitT> unit =
            Component::create<UnitT>(properties, configName, getConfigDomain());
 
        //fill devices (sensor + controller)
        unit->setupData(getProperty<std::string>("RobotFileName").getValue(),
                        _module<RobotData>().cloneRobot(),
                        std::move(devs),
                        _module<Devices>().getControlModeGroups().groups,
                        _module<Devices>().getControlModeGroups().groupsMerged,
                        dynamic_cast<RobotUnit*>(this));
        return unit;
    }
 
    void
    Units::initializePlatformUnit()
    {
        throwIfInControlThread(BOOST_CURRENT_FUNCTION);
        using UnitT = PlatformSubUnit;
        using IfaceT = PlatformUnitInterface;
 
        ARMARX_TRACE;
 
        auto guard = getGuard();
        throwIfStateIsNot(RobotUnitState::InitializingUnits, __FUNCTION__);
        //check if unit is already added
        if (getUnit(IfaceT::ice_staticId()))
        {
            return;
        }
        ARMARX_TRACE;
        //is there a platform dev?
        if (_module<RobotData>().getRobotPlatformName().empty())
        {
            ARMARX_INFO << "no platform unit created since no platform name was given";
            return;
        }
        ARMARX_TRACE;
        if (!_module<Devices>().getControlDevices().has(
                _module<RobotData>().getRobotPlatformName()))
        {
            ARMARX_WARNING
                << "no platform unit created since the platform control device with name '"
                << _module<RobotData>().getRobotPlatformName() << "' was not found";
            return;
        }
        ARMARX_TRACE;
        const ControlDevicePtr& controlDevice =
            _module<Devices>().getControlDevices().at(_module<RobotData>().getRobotPlatformName());
        const SensorDevicePtr& sensorDevice =
            _module<Devices>().getSensorDevices().at(_module<RobotData>().getRobotPlatformName());
        JointController* jointVel =
            controlDevice->getJointController(ControlModes::HolonomicPlatformVelocity);
        ARMARX_TRACE;
        ARMARX_CHECK_EXPRESSION(jointVel);
        ARMARX_CHECK_EXPRESSION(
            sensorDevice->getSensorValue()->asA<SensorValueHolonomicPlatform>());
        //add it
        ARMARX_TRACE;
        const std::string configName = getProperty<std::string>("PlatformUnitName");
        const std::string confPre = getConfigDomain() + "." + configName + ".";
        Ice::PropertiesPtr properties = getIceProperties()->clone();
        //properties->setProperty(confPre + "MinimumLoggingLevel", getProperty<std::string>("MinimumLoggingLevel").getValue());
        //fill properties
        properties->setProperty(confPre + "PlatformName",
                                _module<RobotData>().getRobotPlatformInstanceName());
        ARMARX_DEBUG << "creating unit " << configName
                     << " using these properties: " << properties->getPropertiesForPrefix("");
        ARMARX_TRACE;
        IceInternal::Handle<UnitT> unit =
            Component::create<UnitT>(properties, configName, getConfigDomain());
        //config
        ARMARX_TRACE;
        const NJointHolonomicPlatformVelocityControllerTypes platformControllerType =
            getProperty<NJointHolonomicPlatformVelocityControllerTypes>(
                "PlatformUnitVelocityControllerType");
        if (platformControllerType ==
            NJointHolonomicPlatformVelocityControllerTypes::PassThroughController)
        {
            NJointHolonomicPlatformUnitVelocityPassThroughControllerConfigPtr config =
                new NJointHolonomicPlatformUnitVelocityPassThroughControllerConfig;
            config->initialVelocityX = 0;
            config->initialVelocityY = 0;
            config->initialVelocityRotation = 0;
            config->platformName = _module<RobotData>().getRobotPlatformName();
 
            auto ctrl = NJointHolonomicPlatformUnitVelocityPassThroughControllerPtr::dynamicCast(
                _module<ControllerManagement>().createNJointController(
                    "NJointHolonomicPlatformUnitVelocityPassThroughController",
                    getName() + "_" + configName + "_VelPTContoller",
                    config,
                    false,
                    true));
            ARMARX_TRACE;
            ARMARX_CHECK_EXPRESSION(ctrl);
            unit->pt = ctrl;
            ARMARX_TRACE;
        }
        else if (platformControllerType ==
                 NJointHolonomicPlatformVelocityControllerTypes::ControllerWithRamp)
        {
            NJointHolonomicPlatformVelocityControllerWithRampConfigPtr config =
                new NJointHolonomicPlatformVelocityControllerWithRampConfig;
            config->platformName = _module<RobotData>().getRobotPlatformName();
 
            config->maxPositionAcceleration =
                getProperty<float>("PlatformUnitMaximumPositionAcceleration");
            config->maxOrientationAcceleration =
                getProperty<float>("PlatformUnitMaximumOrientationAcceleration");
 
            auto ctrl = NJointHolonomicPlatformVelocityControllerWithRampPtr::dynamicCast(
                _module<ControllerManagement>().createNJointController(
                    "NJointHolonomicPlatformVelocityControllerWithRamp",
                    getName() + "_" + configName + "_VelPTContoller",
                    config,
                    false,
                    true));
            ARMARX_TRACE;
            ARMARX_CHECK_EXPRESSION(ctrl);
            unit->pt = ctrl;
            ARMARX_TRACE;
        }
        else
        {
            ARMARX_ERROR << "Invalid Platform velocity controller specified '"
                         << NJointHolonomicPlatformVelocityControllerTypesNames.to_name(
                                platformControllerType)
                         << "'";
        }
 
        NJointHolonomicPlatformRelativePositionControllerConfigPtr configRelativePositionCtrlCfg =
            new NJointHolonomicPlatformRelativePositionControllerConfig;
        configRelativePositionCtrlCfg->platformName = _module<RobotData>().getRobotPlatformName();
        auto ctrlRelativePosition =
            NJointHolonomicPlatformRelativePositionControllerPtr::dynamicCast(
                _module<ControllerManagement>().createNJointController(
                    "NJointHolonomicPlatformRelativePositionController",
                    getName() + "_" + configName + "_RelativePositionContoller",
                    configRelativePositionCtrlCfg,
                    false,
                    true));
        ARMARX_CHECK_EXPRESSION(ctrlRelativePosition);
        unit->relativePosCtrl = ctrlRelativePosition;
 
        // unit->platformSensorIndex = _module<Devices>().getSensorDevices().index(_module<RobotData>().getRobotPlatformName());
 
        NJointHolonomicPlatformGlobalPositionControllerConfigPtr configGlobalPositionCtrlCfg =
            new NJointHolonomicPlatformGlobalPositionControllerConfig;
        configGlobalPositionCtrlCfg->platformName = _module<RobotData>().getRobotPlatformName();
        auto ctrlGlobalPosition = NJointHolonomicPlatformGlobalPositionControllerPtr::dynamicCast(
            _module<ControllerManagement>().createNJointController(
                "NJointHolonomicPlatformGlobalPositionController",
                getName() + "_" + configName + "_GlobalPositionContoller",
                configGlobalPositionCtrlCfg,
                false,
                true));
        ARMARX_TRACE;
        ARMARX_CHECK_EXPRESSION(ctrlGlobalPosition);
        unit->globalPosCtrl = ctrlGlobalPosition;
        ARMARX_TRACE;
 
        unit->platformSensorIndex = _module<Devices>().getSensorDevices().index(
            _module<RobotData>().getRobotPlatformName());
        //add
        addUnit(std::move(unit));
    }
 
    void
    Units::initializeLocalizationUnit()
    {
        ARMARX_TRACE;
        ARMARX_DEBUG << "initializeLocalizationUnit";
 
        throwIfInControlThread(BOOST_CURRENT_FUNCTION);
        using UnitT = LocalizationSubUnit;
        using IfaceT = LocalizationUnitInterface;
 
        ARMARX_TRACE;
        auto guard = getGuard();
        throwIfStateIsNot(RobotUnitState::InitializingUnits, __FUNCTION__);
        //check if unit is already added
        if (getUnit(IfaceT::ice_staticId()))
        {
            return;
        }
        ARMARX_TRACE;
 
        if (_module<Devices>().getSensorDevices().count(
                _module<RobotData>().getRobotPlatformName()) == 0)
        {
            ARMARX_WARNING << "No sensor devices found for robot platform `"
                           << _module<RobotData>().getRobotPlatformName() << "`.";
            return;
        }
 
        ARMARX_DEBUG << "Getting device SensorValueHolonomicPlatformRelativePosition";
        ARMARX_CHECK(
            _module<Devices>().getSensorDevices().has(_module<RobotData>().getRobotPlatformName()))
            << _module<RobotData>().getRobotPlatformName();
        const SensorDevicePtr& sensorDeviceRelativePosition =
            _module<Devices>().getSensorDevices().at(_module<RobotData>().getRobotPlatformName());
        ARMARX_CHECK_EXPRESSION(sensorDeviceRelativePosition->getSensorValue()
                                    ->asA<SensorValueHolonomicPlatformRelativePosition>());
 
        ARMARX_TRACE;
        // const SensorDevicePtr& sensorDevicePoseCorrection = _module<Devices>().getSensorDevices().at(GlobalRobotPoseCorrectionSensorDevice::DeviceName());
        // ARMARX_CHECK_EXPRESSION(sensorDeviceRelativePosition->getSensorValue()->asA<SensorValueGlobalPoseCorrection>());
 
 
        //add it
        const std::string configName = "LocalizationUnit";
        const std::string confPre = getConfigDomain() + "." + configName + ".";
        Ice::PropertiesPtr properties = getIceProperties()->clone();
        //properties->setProperty(confPre + "MinimumLoggingLevel", getProperty<std::string>("MinimumLoggingLevel").getValue());
        //fill properties
        // properties->setProperty(confPre + "PlatformName", _module<RobotData>().getRobotPlatformInstanceName());
        ARMARX_DEBUG << "creating unit " << configName
                     << " using these properties: " << properties->getPropertiesForPrefix("");
        IceInternal::Handle<UnitT> unit =
            Component::create<UnitT>(properties, configName, getConfigDomain());
        //config
 
        ARMARX_DEBUG << "Setting device GlobalRobotPoseCorrectionSensorDevice";
        unit->globalPositionCorrectionSensorDevice =
            dynamic_cast<decltype(unit->globalPositionCorrectionSensorDevice)>(
                _module<Devices>()
                    .getSensorDevices()
                    .at(GlobalRobotPoseCorrectionSensorDevice::DeviceName())
                    .get());
 
 
        const SensorDevicePtr& sensorDeviceGlobalRobotLocalization =
            _module<Devices>().getSensorDevices().at(
                GlobalRobotLocalizationSensorDevice::DeviceName());
        ARMARX_CHECK_EXPRESSION(sensorDeviceGlobalRobotLocalization->getSensorValue()
                                    ->asA<GlobalRobotLocalizationSensorDevice::SensorValueType>());
 
        dynamic_cast<GlobalRobotLocalizationSensorDevice&>(*sensorDeviceGlobalRobotLocalization)
            .sensorRelativePosition = sensorDeviceRelativePosition->getSensorValue()
                                          ->asA<SensorValueHolonomicPlatformRelativePosition>();
        dynamic_cast<GlobalRobotLocalizationSensorDevice&>(*sensorDeviceGlobalRobotLocalization)
            .sensorGlobalPositionCorrection =
            unit->globalPositionCorrectionSensorDevice->getSensorValue()
                ->asA<SensorValueGlobalPoseCorrection>();
 
        //add
        addUnit(unit);
    }
 
    void
    Units::initializeForceTorqueUnit()
    {
        throwIfInControlThread(BOOST_CURRENT_FUNCTION);
        using UnitT = ForceTorqueSubUnit;
        using IfaceT = ForceTorqueUnitInterface;
 
        auto guard = getGuard();
        throwIfStateIsNot(RobotUnitState::InitializingUnits, __FUNCTION__);
        //check if unit is already added
        if (getUnit(IfaceT::ice_staticId()))
        {
            return;
        }
        //check if it is required
        std::vector<ForceTorqueSubUnit::DeviceData> ftdevs;
        for (const SensorDevicePtr& ftSensorDevice : _module<Devices>().getSensorDevices().values())
        {
            if (ftSensorDevice->getSensorValue()->isA<SensorValueForceTorque>())
            {
                ForceTorqueSubUnit::DeviceData ftSensorData;
                ftSensorData.sensorIndex =
                    _module<Devices>().getSensorDevices().index(ftSensorDevice->getDeviceName());
                ftSensorData.deviceName = ftSensorDevice->getDeviceName();
                ftSensorData.frame = ftSensorDevice->getReportingFrame();
                ARMARX_CHECK_EXPRESSION(!ftSensorData.frame.empty())
                    << "The sensor device '" << ftSensorData.deviceName
                    << "' (index: " << ftSensorData.sensorIndex
                    << ") reports force torque values but returns an empty string as reporting "
                       "frame";
                ARMARX_CHECK_EXPRESSION(unitCreateRobot->hasRobotNode(ftSensorData.frame))
                    << "The sensor device '" << ftSensorData.deviceName
                    << "' (index: " << ftSensorData.sensorIndex
                    << ") reports force torque values but returns a reporting frame '"
                    << ftSensorData.frame << "' which is not present in the robot '"
                    << _module<RobotData>().getRobotName() << "'";
                ftdevs.emplace_back(std::move(ftSensorData));
            }
        }
        if (ftdevs.empty())
        {
            ARMARX_IMPORTANT
                << "no force torque unit created since there are no force torque sensor devices";
            return;
        }
        //add it
        const std::string configName = getProperty<std::string>("ForceTorqueUnitName");
        const std::string confPre = getConfigDomain() + "." + configName + ".";
        Ice::PropertiesPtr properties = getIceProperties()->clone();
        //properties->setProperty(confPre + "MinimumLoggingLevel", getProperty<std::string>("MinimumLoggingLevel").getValue());
        //fill properties
        properties->setProperty(confPre + "AgentName", _module<RobotData>().getRobotName());
        properties->setProperty(confPre + "ForceTorqueTopicName",
                                getProperty<std::string>("ForceTorqueTopicName").getValue());
        ARMARX_DEBUG << "creating unit " << configName
                     << " using these properties: " << properties->getPropertiesForPrefix("");
        IceInternal::Handle<UnitT> unit =
            Component::create<UnitT>(properties, configName, getConfigDomain());
        unit->devs = ftdevs;
        //add
        addUnit(std::move(unit));
    }
 
    void
    Units::initializeInertialMeasurementUnit()
    {
        throwIfInControlThread(BOOST_CURRENT_FUNCTION);
        using UnitT = InertialMeasurementSubUnit;
        using IfaceT = InertialMeasurementUnitInterface;
 
        auto guard = getGuard();
        throwIfStateIsNot(RobotUnitState::InitializingUnits, __FUNCTION__);
        //check if unit is already added
        if (getUnit(IfaceT::ice_staticId()))
        {
            return;
        }
        //check if it is required
        std::map<std::string, std::size_t> imudevs;
        for (auto i : getIndices(_module<Devices>().getSensorDevices().keys()))
        {
            const SensorDevicePtr& sensorDevice = _module<Devices>().getSensorDevices().at(i);
            if (sensorDevice->getSensorValue()->isA<SensorValueIMU>())
            {
                imudevs[sensorDevice->getDeviceName()] = i;
            }
        }
        if (imudevs.empty())
        {
            ARMARX_IMPORTANT
                << "no inertial measurement unit created since there are no imu sensor devices";
            return;
        }
        //add it
        const std::string configName = getProperty<std::string>("InertialMeasurementUnitName");
        const std::string confPre = getConfigDomain() + "." + configName + ".";
        Ice::PropertiesPtr properties = getIceProperties()->clone();
        //properties->setProperty(confPre + "MinimumLoggingLevel", getProperty<std::string>("MinimumLoggingLevel").getValue());
        //fill properties
        properties->setProperty(confPre + "IMUTopicName",
                                getProperty<std::string>("IMUTopicName").getValue());
        IceInternal::Handle<UnitT> unit =
            Component::create<UnitT>(properties, configName, getConfigDomain());
        unit->devs = imudevs;
        //add
        addUnit(std::move(unit));
    }
 
    void
    Units::initializeTrajectoryControllerUnit()
    {
        throwIfInControlThread(BOOST_CURRENT_FUNCTION);
        auto guard = getGuard();
        throwIfStateIsNot(RobotUnitState::InitializingUnits, __FUNCTION__);
        using UnitT = TrajectoryControllerSubUnit;
        if (!getProperty<bool>("CreateTrajectoryPlayer").getValue())
        {
            return;
        }
 
        //check if unit is already added
        if (getUnit(UnitT::ice_staticId()) || !trajectoryControllerSubUnit)
        {
            return;
        }
        (TrajectoryControllerSubUnitPtr::dynamicCast(trajectoryControllerSubUnit))
            ->setup(_modulePtr<RobotUnit>());
        addUnit(trajectoryControllerSubUnit);
        trajectoryControllerSubUnit = nullptr;
    }
 
    void
    Units::initializeTcpControllerUnit()
    {
        throwIfInControlThread(BOOST_CURRENT_FUNCTION);
        auto guard = getGuard();
        throwIfStateIsNot(RobotUnitState::InitializingUnits, __FUNCTION__);
 
        if (!getProperty<bool>("CreateTCPControlUnit").getValue())
        {
            return;
        }
        using UnitT = TCPControllerSubUnit;
 
        //check if unit is already added
        if (getUnit(UnitT::ice_staticId()))
        {
            return;
        }
        (TCPControllerSubUnitPtr::dynamicCast(tcpControllerSubUnit))
            ->setup(_modulePtr<RobotUnit>(), _module<RobotData>().cloneRobot());
        addUnit(tcpControllerSubUnit);
        tcpControllerSubUnit = nullptr;
    }
 
    void
    Units::addUnit(ManagedIceObjectPtr unit)
    {
        throwIfInControlThread(BOOST_CURRENT_FUNCTION);
        ARMARX_CHECK_NOT_NULL(unit);
        auto guard = getGuard();
        throwIfStateIsNot(RobotUnitState::InitializingUnits, __FUNCTION__);
        getArmarXManager()->addObjectAsync(unit, "", true, false);
        //maybe add it to the sub units
        RobotUnitSubUnitPtr rsu = RobotUnitSubUnitPtr::dynamicCast(unit);
        if (rsu)
        {
            subUnits.emplace_back(std::move(rsu));
        }
        units.emplace_back(std::move(unit));
    }
 
    void
    Units::_icePropertiesInitialized()
    {
        throwIfInControlThread(BOOST_CURRENT_FUNCTION);
        Ice::PropertiesPtr properties = getIceProperties()->clone();
        const std::string& configDomain = "ArmarX";
        // create TCPControlSubUnit
 
        {
            const std::string configNameTCPControlUnit =
                getProperty<std::string>("TCPControlUnitName");
            tcpControllerSubUnit = Component::create<TCPControllerSubUnit, TCPControllerSubUnitPtr>(
                properties, configNameTCPControlUnit, configDomain);
            addPropertyUser(PropertyUserPtr::dynamicCast(tcpControllerSubUnit));
        }
 
        // create TrajectoryControllerSubUnit
 
        {
            const std::string configNameTrajectoryControllerUnit =
                getProperty<std::string>("TrajectoryControllerUnitName");
            const std::string confPre =
                configDomain + "." + configNameTrajectoryControllerUnit + ".";
            properties->setProperty(confPre + "KinematicUnitName",
                                    getProperty<std::string>("KinematicUnitName").getValue());
            trajectoryControllerSubUnit =
                Component::create<TrajectoryControllerSubUnit, TrajectoryControllerSubUnitPtr>(
                    properties, configNameTrajectoryControllerUnit, configDomain);
            addPropertyUser(PropertyUserPtr::dynamicCast(trajectoryControllerSubUnit));
        }
    }
 
    void
    Units::_preFinishRunning()
    {
        throwIfInControlThread(BOOST_CURRENT_FUNCTION);
        //remove all units
        subUnits.clear();
        for (ManagedIceObjectPtr& unit : units)
        {
            getArmarXManager()->removeObjectBlocking(unit->getName());
        }
        units.clear();
    }
 
    void
    Units::_preOnInitRobotUnit()
    {
        throwIfInControlThread(BOOST_CURRENT_FUNCTION);
        unitCreateRobot = _module<RobotData>().cloneRobot();
        ARMARX_DEBUG << "add emergency stop master";
        {
            emergencyStopMaster = new RobotUnitEmergencyStopMaster{
                &_module<ControlThread>(),
                getProperty<std::string>("EmergencyStopTopic").getValue()};
            ManagedIceObjectPtr obj = ManagedIceObjectPtr::dynamicCast(emergencyStopMaster);
            ARMARX_CHECK_EXPRESSION(obj) << "RobotUnitEmergencyStopMaster";
            getArmarXManager()->addObject(
                obj, getProperty<std::string>("EmergencyStopMasterName").getValue(), true);
            auto prx = obj->getProxy(-1);
            try
            {
                getArmarXManager()->getIceManager()->getIceGridSession()->getAdmin()->addObject(
                    prx);
            }
            catch (const IceGrid::ObjectExistsException&)
            {
                getArmarXManager()->getIceManager()->getIceGridSession()->getAdmin()->updateObject(
                    prx);
            }
            catch (std::exception& e)
            {
                ARMARX_WARNING << "add the robot unit as emergency stop master to the ice grid "
                                  "admin!\nThere was an exception:\n"
                               << e.what();
            }
        }
        ARMARX_DEBUG << "add emergency stop master...done!";
    }
 
    void
    Units::_postOnExitRobotUnit()
    {
        throwIfInControlThread(BOOST_CURRENT_FUNCTION);
        ARMARX_DEBUG << "remove EmergencyStopMaster";
        try
        {
            getArmarXManager()->removeObjectBlocking(
                ManagedIceObjectPtr::dynamicCast(emergencyStopMaster));
            getArmarXManager()->getIceManager()->removeObject(
                getProperty<std::string>("EmergencyStopMasterName").getValue());
        }
        catch (...)
        {
        }
        ARMARX_DEBUG << "remove EmergencyStopMaster...done!";
    }
 
    const ManagedIceObjectPtr&
    Units::getUnit(const std::string& staticIceId) const
    {
        throwIfInControlThread(BOOST_CURRENT_FUNCTION);
        auto guard = getGuard();
        for (const ManagedIceObjectPtr& unit : units)
        {
            if (unit->ice_isA(staticIceId))
            {
                return unit;
            }
        }
        return ManagedIceObject::NullPtr;
    }
} // namespace armarx::RobotUnitModule