RobotUnitComponentPlugin.cpp

Go to the documentation of this file.

#include "RobotUnitComponentPlugin.h"
 
#include <thread>
 
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/util/CPPUtility/Pointer.h>
 
namespace armarx::plugins
{
    RobotUnitDataStreamingReceiverPtr
    RobotUnitComponentPlugin::startDataStreaming(const RobotUnitDataStreaming::Config& cfg)
    {
        //ok to create smart ptr from parent, since ice handels this
        return make_shared<RobotUnitDataStreamingReceiver>(&parent(), getRobotUnit(), cfg);
    }
 
    void
    RobotUnitComponentPlugin::postOnDisconnectComponent()
    {
        if (!_ctrls.empty())
        {
            ARMARX_CHECK_NOT_NULL(_robotUnit);
            std::vector<std::string> cs(_ctrls.begin(), _ctrls.end());
            _robotUnit->deactivateAndDeleteNJointControllers(cs);
        }
        _robotUnit = nullptr;
    }
 
    NJointControllerInterfacePrx
    RobotUnitComponentPlugin::createNJointController(
        const std::string& className,
        const std::string& instanceName,
        const armarx::NJointControllerConfigPtr& config,
        bool doManageController)
    {
        ARMARX_CHECK_NOT_NULL(_robotUnit);
        ARMARX_INFO << ARMARX_STREAM_PRINTER
        {
            out << "creating ";
            if (doManageController)
            {
                out << "and managing ";
            }
            out << " controller '" << instanceName << "' of class '" << instanceName << "'";
        };
        const auto prx =
            _robotUnit->createOrReplaceNJointController(className, instanceName, config);
        ARMARX_CHECK_NOT_NULL(prx);
        if (doManageController)
        {
            manageController(instanceName);
        }
        return prx;
    }
 
    void
    RobotUnitComponentPlugin::manageController(const armarx::NJointControllerInterfacePrx& ctrl)
    {
        ARMARX_CHECK_NOT_NULL(ctrl);
        manageController(ctrl->getInstanceName());
    }
 
    void
    RobotUnitComponentPlugin::manageController(const std::string& ctrl)
    {
        _ctrls.emplace(ctrl);
    }
 
    void
    RobotUnitComponentPlugin::preOnInitComponent()
    {
        if (_robotUnitName.empty())
        {
            parent<Component>().getProperty(_robotUnitName, PROPERTY_NAME);
        }
 
        if (not _isRobotUnitOptionalDependency)
        {
            parent<Component>().usingProxy(_robotUnitName);
        }
    }
 
    void
    RobotUnitComponentPlugin::preOnConnectComponent()
    {
        if (not _robotUnitName.empty())
        {
            parent<Component>().getProxy(_robotUnit, _robotUnitName);
        }
    }
 
    void
    RobotUnitComponentPlugin::postCreatePropertyDefinitions(
        armarx::PropertyDefinitionsPtr& properties)
    {
        if (!properties->hasDefinition(PROPERTY_NAME))
        {
            if (_isRobotUnitOptionalDependency)
            {
                properties->defineOptionalProperty<std::string>(
                    PROPERTY_NAME, "", "Name of the RobotUnit");
            }
            else
            {
                properties->defineRequiredProperty<std::string>(PROPERTY_NAME,
                                                                "Name of the RobotUnit");
            }
        }
    }
 
    RobotUnitInterfacePrx
    RobotUnitComponentPlugin::getRobotUnit() const
    {
        return _robotUnit;
    }
 
    void
    RobotUnitComponentPlugin::setRobotUnitName(const std::string& name)
    {
        ARMARX_CHECK_EMPTY(_robotUnitName);
        _robotUnitName = name;
    }
 
    const std::string&
    RobotUnitComponentPlugin::getRobotUnitName() const
    {
        return _robotUnitName;
    }
 
    bool
    RobotUnitComponentPlugin::hasRobotUnitName() const
    {
        return not _robotUnitName.empty();
    }
 
    void
    RobotUnitComponentPlugin::setRobotUnitAsOptionalDependency(bool isOptional)
    {
        _isRobotUnitOptionalDependency = isOptional;
    }
 
    void
    RobotUnitComponentPlugin::waitUntilRobotUnitIsRunning(const std::function<bool()>& termCond)
    {
        ARMARX_INFO << "Waiting until robot unit is running ...";
 
        if (not hasRobotUnitName())
        {
            ARMARX_ERROR << "Could not wait for a robotUnit without a name!";
            return;
        }
 
        parent<Component>().usingProxy(_robotUnitName);
        while (not termCond() and not robotUnitIsRunning())
        {
            ARMARX_INFO << deactivateSpam() << "Still waiting for robot unit to start...";
            std::this_thread::sleep_for(std::chrono::milliseconds(100));
        }
 
        ARMARX_INFO << "Robot unit is up and running.";
    }
 
    bool
    RobotUnitComponentPlugin::robotUnitIsRunning() const
    {
        if (not hasRobotUnitName())
        {
            // An empty robotUnit can never run
            return false;
        }
        return not isNullptr(getRobotUnit()) and getRobotUnit()->isRunning();
    }
 
 
} // namespace armarx::plugins
 
namespace armarx
{
    RobotUnitComponentPluginUser::RobotUnitComponentPluginUser()
    {
        addPlugin(plugin);
    }
 
    RobotUnitInterfacePrx
    RobotUnitComponentPluginUser::getRobotUnit() const
    {
        return plugin->getRobotUnit();
    }
 
    plugins::RobotUnitComponentPlugin&
    RobotUnitComponentPluginUser::getRobotUnitComponentPlugin()
    {
        return *plugin;
    }
 
    void
    RobotUnitComponentPluginUser::waitUntilRobotUnitIsRunning(
        const std::function<bool()>& termCond) const
    {
        ARMARX_INFO << "Waiting until robot unit is running ...";
 
        while ((not termCond()) and
               ((isNullptr(getRobotUnit()) or (not getRobotUnit()->isRunning()))))
        {
            std::this_thread::sleep_for(std::chrono::milliseconds(100));
        }
 
        ARMARX_INFO << "Robot unit is up and running.";
    }
} // namespace armarx