NJointController.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::NJointController
 * @author     Raphael Grimm ( raphael dot grimm at kit dot edu )
 * @date       2017
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include <atomic>
 
#include <ArmarXCore/core/application/Application.h>
#include <ArmarXCore/core/util/algorithm.h>
 
#include "../RobotUnit.h"
#include "NJointControllerBase.h"
#include "NJointControllerRegistry.h"
 
namespace armarx
{
    RobotUnit*
    getRobotUnit(RobotUnitModule::ControllerManagement* cmngr)
    {
        return cmngr->_modulePtr<RobotUnit>();
    }
} // namespace armarx
 
namespace armarx::RobotUnitModule
{
 
    /**
     * \brief This class allows minimal access to private members of \ref armarx::RobotUnitModule::Devices in a sane fashion for \ref armarx::NJointControllerBase.
     * \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 DevicesAttorneyForNJointController
    {
        friend class ::armarx::NJointControllerBase;
 
        static JointController*
        GetJointController(Devices& d,
                           const std::string& deviceName,
                           const std::string& controlMode)
        {
            if (d.controlDevices.has(deviceName))
            {
                auto& dev = d.controlDevices.at(deviceName);
                if (dev->hasJointController(controlMode))
                {
                    ARMARX_CHECK_NOT_NULL(dev->getJointController(controlMode));
                    ARMARX_CHECK_NOT_NULL(dev->getJointController(controlMode)->getControlTarget());
                    return dev->getJointController(controlMode);
                }
            }
            return nullptr;
        }
    };
} // namespace armarx::RobotUnitModule
 
thread_local bool armarx::NJointControllerRegistryEntry::ConstructorIsRunning_ = false;
 
namespace armarx
{
    const NJointControllerBasePtr NJointControllerBase::NullPtr{nullptr};
 
    NJointControllerDescription
    NJointControllerBase::getControllerDescription(const Ice::Current&) const
    {
        ARMARX_TRACE;
        NJointControllerDescription d;
        d.className = getClassName();
        ARMARX_CHECK_EXPRESSION(getProxy(-1));
        d.controller = NJointControllerInterfacePrx::uncheckedCast(getProxy(-1));
        d.controlModeAssignment = controlDeviceControlModeMap;
        d.instanceName = getInstanceName();
        d.deletable = deletable;
        d.internal = internal;
        return d;
    }
 
    std::optional<std::vector<char>>
    NJointControllerBase::isNotInConflictWith(const std::vector<char>& used) const
    {
        ARMARX_TRACE;
        ARMARX_CHECK_EXPRESSION(used.size() == controlDeviceUsedBitmap.size());
        auto result = used;
        for (std::size_t i = 0; i < controlDeviceUsedBitmap.size(); ++i)
        {
            if (controlDeviceUsedBitmap.at(i))
            {
                if (used.at(i))
                {
                    return std::nullopt;
                }
                result.at(i) = true;
            }
        }
        return result;
    }
 
    NJointControllerStatus
    NJointControllerBase::getControllerStatus(const Ice::Current&) const
    {
        ARMARX_TRACE;
        NJointControllerStatus s;
        s.active = isActive;
        s.instanceName = getInstanceName();
        s.requested = isRequested;
        s.error = deactivatedBecauseOfError;
        s.timestampUSec = std::chrono::duration_cast<std::chrono::microseconds>(
                              std::chrono::high_resolution_clock::now().time_since_epoch())
                              .count();
        return s;
    }
 
    NJointControllerDescriptionWithStatus
    NJointControllerBase::getControllerDescriptionWithStatus(const Ice::Current&) const
    {
        NJointControllerDescriptionWithStatus ds;
        ds.description = getControllerDescription();
        ds.status = getControllerStatus();
        return ds;
    }
 
    //    RobotUnitInterfacePrx NJointControllerBase::getRobotUnit(const Ice::Current&) const
    //    {
    //        return RobotUnitInterfacePrx::uncheckedCast(robotUnit.getProxy(-1));
    //    }
 
    void
    NJointControllerBase::activateController(const Ice::Current&)
    {
        robotUnit.activateNJointController(this);
    }
 
    void
    NJointControllerBase::deactivateController(const Ice::Current&)
    {
        robotUnit.deactivateNJointController(this);
    }
 
    void
    NJointControllerBase::deleteController(const Ice::Current&)
    {
        robotUnit.deleteNJointController(this);
    }
 
    void
    NJointControllerBase::deactivateAndDeleteController(const Ice::Current&)
    {
        robotUnit.deactivateAndDeleteNJointController(this);
    }
 
    void
    NJointControllerBase::publish(const SensorAndControl& sac,
                                  const DebugDrawerInterfacePrx& draw,
                                  const DebugObserverInterfacePrx& observer)
    {
        ARMARX_TRACE;
        const bool active = isActive;
        if (publishActive.exchange(active) != active)
        {
            if (active)
            {
                ARMARX_DEBUG << "activating publishing for " << getInstanceName();
                try
                {
                    onPublishActivation(draw, observer);
                }
                catch (std::exception& e)
                {
                    ARMARX_WARNING << "onPublishActivation of '" << getInstanceName()
                                   << "' threw an exception!\nWhat:\n"
                                   << e.what();
                }
                catch (...)
                {
                    ARMARX_WARNING << "onPublishActivation of '" << getInstanceName()
                                   << "' threw an exception!";
                }
            }
            else
            {
                ARMARX_DEBUG << "deactivating publishing for " << getInstanceName();
                try
                {
                    onPublishDeactivation(draw, observer);
                }
                catch (std::exception& e)
                {
                    ARMARX_WARNING << "onPublishDeactivation of '" << getInstanceName()
                                   << "' threw an exception!\nWhat:\n"
                                   << e.what();
                }
                catch (...)
                {
                    ARMARX_WARNING << "onPublishDeactivation of '" << getInstanceName()
                                   << "' threw an exception!";
                }
            }
        }
        if (active)
        {
            try
            {
                onPublish(sac, draw, observer);
            }
            catch (std::exception& e)
            {
                ARMARX_WARNING << "onPublish of '" << getInstanceName()
                               << "' threw an exception!\nWhat:\n"
                               << e.what();
            }
            catch (...)
            {
                ARMARX_WARNING << "onPublish of '" << getInstanceName() << "' threw an exception!";
            }
        }
    }
 
    void
    NJointControllerBase::deactivatePublish(const DebugDrawerInterfacePrx& draw,
                                            const DebugObserverInterfacePrx& observer)
    {
        ARMARX_TRACE;
        if (publishActive.exchange(false))
        {
            ARMARX_DEBUG << "forced deactivation of publishing for " << getInstanceName();
            onPublishDeactivation(draw, observer);
        }
    }
 
    void
    NJointControllerBase::rtActivateController()
    {
        if (!isActive)
        {
            deactivatedBecauseOfError = false;
            rtPreActivateController();
            isActive = true;
            errorState.store(false);
        }
    }
 
    void
    NJointControllerBase::rtDeactivateController()
    {
        if (isActive)
        {
            isActive = false;
            rtPostDeactivateController();
        }
    }
 
    void
    NJointControllerBase::rtDeactivateControllerBecauseOfError()
    {
        deactivatedBecauseOfError = true;
        rtDeactivateController();
    }
 
    armarx::ConstSensorDevicePtr
    armarx::NJointControllerBase::peekSensorDevice(const std::string& deviceName) const
    {
        ARMARX_CHECK_EXPRESSION(NJointControllerRegistryEntry::ConstructorIsRunning())
            << "The function '" << BOOST_CURRENT_FUNCTION
            << "' must only be called during the construction of an NJointController.";
        return RobotUnitModule::Devices::Instance().getSensorDevice(deviceName);
    }
 
    ConstControlDevicePtr
    NJointControllerBase::peekControlDevice(const std::string& deviceName) const
    {
        ARMARX_CHECK_EXPRESSION(NJointControllerRegistryEntry::ConstructorIsRunning())
            << "The function '" << BOOST_CURRENT_FUNCTION
            << "' must only be called during the construction of an NJointController.";
        return RobotUnitModule::Devices::Instance().getControlDevice(deviceName);
    }
 
    const VirtualRobot::RobotPtr&
    NJointControllerBase::useSynchronizedRtRobot(bool updateCollisionModel)
    {
        ARMARX_CHECK_EXPRESSION(NJointControllerRegistryEntry::ConstructorIsRunning())
            << "The function '" << BOOST_CURRENT_FUNCTION
            << "' must only be called during the construction of an NJointController.";
        ARMARX_CHECK_IS_NULL(rtRobot) << "useSynchronizedRtRobot was already called";
        rtRobot = RobotUnitModule::RobotData::Instance().cloneRobot(updateCollisionModel);
        rtRobotNodes = rtRobot->getRobotNodes();
        return rtRobot;
    }
 
    void
    NJointControllerBase::onInitComponent()
    {
        ARMARX_TRACE;
        onInitNJointController();
    }
 
    void
    NJointControllerBase::onConnectComponent()
    {
        ARMARX_TRACE;
        onConnectNJointController();
    }
 
    void
    NJointControllerBase::onDisconnectComponent()
    {
        ARMARX_TRACE;
        onDisconnectNJointController();
    }
 
    void
    NJointControllerBase::onExitComponent()
    {
        ARMARX_TRACE;
        onExitNJointController();
        // make sure the destructor of the handles does not throw an exception and triggers a termination of the process
        ARMARX_DEBUG << "Deleting thread handles";
        std::unique_lock lock(threadHandlesMutex);
        for (auto& pair : threadHandles)
        {
            try
            {
 
                auto& name = pair.first;
                auto& handle = pair.second;
                if (!handle || !handle->isValid())
                {
                    ARMARX_VERBOSE << "Thread Handle is NULL or invalid - skipping";
                    continue;
                }
                if (handle->isDetached())
                {
                    continue;
                }
                std::future_status status;
                do
                {
                    status = handle->getFuture().wait_for(std::chrono::seconds(3));
                    if (status == std::future_status::timeout)
                    {
                        ARMARX_INFO << "Still waiting for " << name << " thread handle!";
                    }
                    else if (status == std::future_status::ready ||
                             status == std::future_status::deferred)
                    {
                        ARMARX_VERBOSE << "Joining " << name << " task";
                        handle->join();
                        handle.reset();
                    }
                } while (status != std::future_status::ready);
            }
            catch (...)
            {
                handleExceptions();
            }
        }
 
        threadHandles.clear();
    }
 
    ThreadPoolPtr
    NJointControllerBase::getThreadPool() const
    {
        ARMARX_CHECK_EXPRESSION(Application::getInstance());
        return Application::getInstance()->getThreadPool();
    }
 
    const SensorValueBase*
    NJointControllerBase::useSensorValue(const std::string& deviceName) const
    {
        ARMARX_TRACE;
        ARMARX_CHECK_EXPRESSION(NJointControllerRegistryEntry::ConstructorIsRunning())
            << "The function '" << BOOST_CURRENT_FUNCTION
            << "' must only be called during the construction of an NJointController.";
        auto dev = peekSensorDevice(deviceName);
        if (!dev)
        {
            ARMARX_DEBUG << "No sensor device for " << deviceName;
            return nullptr;
        }
        return dev->getSensorValue();
    }
 
    NJointControllerBase::NJointControllerBase() :
        robotUnit(RobotUnitModule::ModuleBase::Instance<::armarx::RobotUnit>()),
        controlDeviceUsedBitmap(robotUnit.getNumberOfControlDevices(), 0)
    {
        controlDeviceUsedIndices.reserve(robotUnit.getNumberOfControlDevices());
    }
 
    NJointControllerBase::~NJointControllerBase()
    {
    }
 
    ControlTargetBase*
    NJointControllerBase::useControlTarget(const std::string& deviceName,
                                           const std::string& controlMode)
    {
        ARMARX_TRACE;
        ARMARX_CHECK_EXPRESSION(NJointControllerRegistryEntry::ConstructorIsRunning())
            << "The function '" << BOOST_CURRENT_FUNCTION
            << "' must only be called during the construction of an NJointController.";
        ARMARX_CHECK_EQUAL(controlDeviceControlModeMap.count(deviceName), 0)
            << BOOST_CURRENT_FUNCTION
            << ": Must not request two ControlTargets for the same device. (got = "
            << controlDeviceControlModeMap.at(deviceName) << ", requested " + controlMode + ") "
            << "(You can only have a ControlDevice in one ControlMode. "
            << "To check all available ControlModes for this device, get the ControlDevice via "
               "peekControlDevice(" +
                   deviceName + ") and querry it)";
 
        //there is a device and a target was requested:
        JointController* const jointCtrl =
            RobotUnitModule::DevicesAttorneyForNJointController::GetJointController(
                RobotUnitModule::Devices::Instance(), deviceName, controlMode);
        if (!jointCtrl)
        {
            return nullptr;
        }
 
        //update meta data structured
        const std::size_t devIndex = robotUnit.getControlDeviceIndex(deviceName);
        controlDeviceUsedJointController[deviceName] = jointCtrl;
 
        controlDeviceControlModeMap[deviceName] = controlMode;
 
        ARMARX_CHECK_EQUAL(0, controlDeviceUsedBitmap.at(devIndex));
        controlDeviceUsedBitmap.at(devIndex) = 1;
        //we want this vector to be sorted
        controlDeviceUsedIndices.insert(std::upper_bound(controlDeviceUsedIndices.begin(),
                                                         controlDeviceUsedIndices.end(),
                                                         devIndex),
                                        devIndex);
 
        return jointCtrl->getControlTarget();
    }
} // namespace armarx