RobotUnitModuleControlThreadDataBuffer.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 "RobotUnitModuleControlThreadDataBuffer.h"
 
#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerBase.h>
 
#include "RobotUnitModuleControllerManagement.h"
#include "RobotUnitModuleDevices.h"
 
namespace armarx::RobotUnitModule
{
    /**
     * \brief This class allows minimal access to private members of \ref Devices in a sane fashion for \ref ControlThreadDataBuffer.
     * \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 DevicesAttorneyForControlThreadDataBuffer
    {
        friend class ControlThreadDataBuffer;
 
        static std::vector<JointController*>
        GetStopMovementJointControllers(ControlThreadDataBuffer* p)
        {
            return p->_module<Devices>().getStopMovementJointControllers();
        }
    };
 
    /**
     * \brief This class allows minimal access to private members of \ref ControllerManagement in a sane fashion for \ref ControlThreadDataBuffer.
     * \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 ControllerManagementAttorneyForControlThreadDataBuffer
    {
        friend class ControlThreadDataBuffer;
 
        static void
        UpdateNJointControllerRequestedState(ControlThreadDataBuffer* p,
                                             const std::set<NJointControllerBasePtr>& request)
        {
            p->_module<ControllerManagement>().updateNJointControllerRequestedState(request);
        }
    };
} // namespace armarx::RobotUnitModule
 
namespace armarx::RobotUnitModule
{
    JointAndNJointControllers
    ControlThreadDataBuffer::getActivatedControllers() const
    {
        throwIfInControlThread(BOOST_CURRENT_FUNCTION);
        throwIfDevicesNotReady(__FUNCTION__);
        std::lock_guard<std::recursive_mutex> guard{controllersActivatedMutex};
        return controllersActivated.getReadBuffer();
    }
 
    std::vector<JointController*>
    ControlThreadDataBuffer::getActivatedJointControllers() const
    {
        throwIfInControlThread(BOOST_CURRENT_FUNCTION);
        throwIfDevicesNotReady(__FUNCTION__);
        std::lock_guard<std::recursive_mutex> guard{controllersActivatedMutex};
        return controllersActivated.getReadBuffer().jointControllers;
    }
 
    std::vector<NJointControllerBase*>
    ControlThreadDataBuffer::getActivatedNJointControllers() const
    {
        throwIfInControlThread(BOOST_CURRENT_FUNCTION);
        throwIfDevicesNotReady(__FUNCTION__);
        std::lock_guard<std::recursive_mutex> guard{controllersActivatedMutex};
        return controllersActivated.getReadBuffer().nJointControllers;
    }
 
    bool
    ControlThreadDataBuffer::activatedControllersChanged() const
    {
        throwIfInControlThread(BOOST_CURRENT_FUNCTION);
        throwIfDevicesNotReady(__FUNCTION__);
        std::lock_guard<std::recursive_mutex> guard{controllersActivatedMutex};
        return controllersActivated.updateReadBuffer();
    }
 
    JointAndNJointControllers
    ControlThreadDataBuffer::copyRequestedControllers() const
    {
        throwIfInControlThread(BOOST_CURRENT_FUNCTION);
        throwIfDevicesNotReady(__FUNCTION__);
        std::lock_guard<std::recursive_mutex> guard{controllersRequestedMutex};
        return controllersRequested.getWriteBuffer();
    }
 
    std::vector<JointController*>
    ControlThreadDataBuffer::copyRequestedJointControllers() const
    {
        throwIfInControlThread(BOOST_CURRENT_FUNCTION);
        throwIfDevicesNotReady(__FUNCTION__);
        std::lock_guard<std::recursive_mutex> guard{controllersRequestedMutex};
        return controllersRequested.getWriteBuffer().jointControllers;
    }
 
    std::vector<NJointControllerBase*>
    ControlThreadDataBuffer::copyRequestedNJointControllers() const
    {
        throwIfInControlThread(BOOST_CURRENT_FUNCTION);
        throwIfDevicesNotReady(__FUNCTION__);
        std::lock_guard<std::recursive_mutex> guard{controllersRequestedMutex};
        return controllersRequested.getWriteBuffer().nJointControllers;
    }
 
    bool
    ControlThreadDataBuffer::sensorAndControlBufferChanged() const
    {
        throwIfInControlThread(BOOST_CURRENT_FUNCTION);
        throwIfDevicesNotReady(__FUNCTION__);
        return controlThreadOutputBuffer.updateReadBuffer();
    }
 
    const SensorAndControl&
    ControlThreadDataBuffer::getSensorAndControlBuffer() const
    {
        throwIfInControlThread(BOOST_CURRENT_FUNCTION);
        throwIfDevicesNotReady(__FUNCTION__);
        return controlThreadOutputBuffer.getReadBuffer();
    }
 
    void
    ControlThreadDataBuffer::writeRequestedControllers(JointAndNJointControllers&& setOfControllers)
    {
        throwIfInControlThread(BOOST_CURRENT_FUNCTION);
        throwIfDevicesNotReady(__FUNCTION__);
        //check NJoint
        const auto& nJointCtrls = setOfControllers.nJointControllers;
        std::set<NJointControllerBasePtr> nJointSet{nJointCtrls.begin(), nJointCtrls.end()};
        nJointSet.erase(nullptr);
        //# NJoint
        const std::size_t nNJointCtrls =
            std::count_if(nJointCtrls.begin(),
                          nJointCtrls.end(),
                          [](const NJointControllerBasePtr& p) { return p; });
 
        ARMARX_CHECK_EXPRESSION(nNJointCtrls == nJointSet.size());
        ARMARX_CHECK_EXPRESSION(nJointCtrls.size() ==
                                _module<Devices>().getNumberOfControlDevices());
        //first nNJointCtrls not null
        ARMARX_CHECK_EXPRESSION(std::all_of(nJointCtrls.begin(),
                                            nJointCtrls.begin() + nNJointCtrls,
                                            [](NJointControllerBase* p) { return p; }));
        //last getNumberOfControlDevices()-nNJointCtrls null
        ARMARX_CHECK_EXPRESSION(std::all_of(nJointCtrls.begin() + nNJointCtrls,
                                            nJointCtrls.end(),
                                            [](NJointControllerBase* p) { return !p; }));
        //conflict free and sorted
        ARMARX_CHECK_EXPRESSION(std::is_sorted(
            nJointCtrls.begin(), nJointCtrls.end(), std::greater<NJointControllerBase*>{}));
        ARMARX_CHECK_EXPRESSION(NJointControllerBase::AreNotInConflict(
            nJointCtrls.begin(), nJointCtrls.begin() + nNJointCtrls));
 
        //check JointCtrl
        const auto& jointCtrls = setOfControllers.jointControllers;
        ARMARX_CHECK_EXPRESSION(jointCtrls.size() ==
                                _module<Devices>().getNumberOfControlDevices());
        ARMARX_CHECK_EXPRESSION(std::all_of(
            jointCtrls.begin(), jointCtrls.end(), [](JointController* p) { return p; }));
 
        //check groups
        {
            ARMARX_DEBUG << "check groups";
            std::size_t grpIdx = 0;
            for (const auto& group : _module<Devices>().getControlModeGroups().groups)
            {
                ARMARX_CHECK_EXPRESSION(!group.empty());
                const auto hwMode =
                    setOfControllers.jointControllers
                        .at(_module<Devices>().getControlDeviceIndex(*group.begin()))
                        ->getHardwareControlMode();
                ARMARX_DEBUG << "---- group " << grpIdx << " mode '" << hwMode << "'";
                for (const auto& other : group)
                {
                    const auto& dev = _module<Devices>().getControlDeviceIndex(other);
                    ARMARX_CHECK_EXPRESSION(dev);
                    const auto otherHwMode =
                        setOfControllers.jointControllers.at(dev)->getHardwareControlMode();
                    ARMARX_DEBUG << "-------- '" << other << "'' mode '" << otherHwMode << "'";
                    ARMARX_CHECK_EXPRESSION(otherHwMode == hwMode);
                }
                ++grpIdx;
            }
        }
        //setup assignement
        {
            ARMARX_DEBUG << "setup assignement index";
            setOfControllers.resetAssignement();
            for (std::size_t nJointCtrlIndex = 0;
                 nJointCtrlIndex < setOfControllers.nJointControllers.size();
                 ++nJointCtrlIndex)
            {
                const NJointControllerBase* nJoint =
                    setOfControllers.nJointControllers.at(nJointCtrlIndex);
                if (!nJoint)
                {
                    break;
                }
                ARMARX_DEBUG << "---- " << nJoint->getInstanceName();
                for (std::size_t jointIndex : nJoint->getControlDeviceUsedIndices())
                {
                    ARMARX_DEBUG << "-------- Index " << jointIndex << ": "
                                 << setOfControllers.jointToNJointControllerAssignement.at(
                                        jointIndex)
                                 << "-> " << nJointCtrlIndex;
                    ARMARX_CHECK_EXPRESSION(setOfControllers.jointToNJointControllerAssignement.at(
                                                jointIndex) == IndexSentinel());
                    setOfControllers.jointToNJointControllerAssignement.at(jointIndex) =
                        nJointCtrlIndex;
                }
            }
        }
 
        {
            ARMARX_DEBUG << "call checkSetOfControllersToActivate";
            checkSetOfControllersToActivate(setOfControllers);
        }
        //set requested state
        ControllerManagementAttorneyForControlThreadDataBuffer::
            UpdateNJointControllerRequestedState(this, nJointSet);
 
        ARMARX_CHECK_EQUAL(_module<Devices>().getNumberOfControlDevices(),
                           setOfControllers.jointControllers.size());
        ARMARX_CHECK_EQUAL(_module<Devices>().getNumberOfControlDevices(),
                           setOfControllers.nJointControllers.size());
        ARMARX_CHECK_EQUAL(_module<Devices>().getNumberOfControlDevices(),
                           setOfControllers.jointToNJointControllerAssignement.size());
        {
            std::lock_guard<std::recursive_mutex> guard{controllersRequestedMutex};
            controllersRequested.getWriteBuffer() = std::move(setOfControllers);
            controllersRequested.commitWrite();
        }
        ARMARX_DEBUG
            << "writeRequestedControllers(JointAndNJointControllers&& setOfControllers)...done!";
    }
 
    void
    ControlThreadDataBuffer::setActivateControllersRequest(
        std::set<NJointControllerBasePtr, std::greater<NJointControllerBasePtr>> ctrls)
    {
        throwIfInControlThread(BOOST_CURRENT_FUNCTION);
        std::lock_guard<std::recursive_mutex> guardRequested{controllersRequestedMutex};
        throwIfDevicesNotReady(__FUNCTION__);
        //erase nullptr
        ctrls.erase(nullptr);
        ARMARX_CHECK_EXPRESSION(ctrls.size() <= _module<Devices>().getNumberOfControlDevices());
 
        const std::set<NJointControllerBasePtr, std::greater<NJointControllerBasePtr>>
            setOfRequestedCtrls{controllersRequested.getWriteBuffer().nJointControllers.begin(),
                                controllersRequested.getWriteBuffer().nJointControllers.end()};
        if (setOfRequestedCtrls == ctrls)
        {
            //redirty the flag to swap in the same set again
            controllersRequested.commitWrite();
            return;
        }
        JointAndNJointControllers request{_module<Devices>().getNumberOfControlDevices()};
        std::size_t idx = 0;
        for (const NJointControllerBasePtr& nJointCtrl : ctrls)
        {
            request.nJointControllers.at(idx++) = nJointCtrl.get();
            //add Joint for this ctrl
            for (const auto& cd2cm : nJointCtrl->getControlDeviceUsedControlModeMap())
            {
                std::size_t jointCtrlIndex =
                    _module<Devices>().getControlDevices().index(cd2cm.first);
                if (request.jointControllers.at(jointCtrlIndex))
                {
                    std::stringstream ss;
                    ss << "RobotUnit:setActivateControllersRequest controllers to activate are in "
                          "conflict!\ncontrollers:";
                    for (const auto& ctrl : ctrls)
                    {
                        ss << "\n" << ctrl->getInstanceName();
                    }
                    ARMARX_ERROR << ss.str();
                    throw InvalidArgumentException{ss.str()};
                }
                request.jointControllers.at(jointCtrlIndex) =
                    _module<Devices>()
                        .getControlDevices()
                        .at(jointCtrlIndex)
                        ->getJointController(cd2cm.second);
            }
        }
 
        std::size_t jointControllersCount = request.jointControllers.size();
        for (std::size_t i = 0; i < jointControllersCount; ++i)
        {
            JointController*& jointCtrl = request.jointControllers.at(i);
            if (!jointCtrl)
            {
                JointController* jointCtrlOld =
                    controllersRequested.getWriteBuffer().jointControllers.at(i);
                if (jointCtrlOld ==
                    _module<Devices>().getControlDevices().at(i)->getJointStopMovementController())
                {
                    //don't replace this controller with emergency stop
                    jointCtrl = jointCtrlOld;
                }
                else
                {
                    //no one controls this device -> emergency stop
                    jointCtrl = _module<Devices>()
                                    .getControlDevices()
                                    .at(i)
                                    ->getJointEmergencyStopController();
                }
            }
        }
        ARMARX_DEBUG << "wrote requested controllers";
        ARMARX_INFO << "requested controllers:\n" << ARMARX_STREAM_PRINTER
        {
            for (std::size_t i = 0; i < request.nJointControllers.size(); ++i)
            {
                const auto& ctrl = request.nJointControllers.at(i);
                if (ctrl)
                {
                    out << i << "\t'" << ctrl->getInstanceName() << "' \t of class '"
                        << ctrl->getClassName() << "'\n";
                }
            }
        };
        writeRequestedControllers(std::move(request));
    }
 
    void
    ControlThreadDataBuffer::updateVirtualRobot(
        const VirtualRobot::RobotPtr& robot,
        const std::vector<VirtualRobot::RobotNodePtr>& nodes) const
    {
        throwIfInControlThread(BOOST_CURRENT_FUNCTION);
        _module<Devices>().updateVirtualRobotFromSensorValues(
            robot, nodes, controlThreadOutputBuffer.getReadBuffer().sensors);
    }
 
    void
    ControlThreadDataBuffer::updateVirtualRobot(const VirtualRobot::RobotPtr& robot) const
    {
        throwIfInControlThread(BOOST_CURRENT_FUNCTION);
        _module<Devices>().updateVirtualRobotFromSensorValues(
            robot, controlThreadOutputBuffer.getReadBuffer().sensors);
    }
 
    void
    ControlThreadDataBuffer::_postFinishDeviceInitialization()
    {
        throwIfInControlThread(BOOST_CURRENT_FUNCTION);
        ARMARX_DEBUG << "initializing buffers:";
        {
            ARMARX_DEBUG << "----initializing controller buffers for "
                         << _module<Devices>().getNumberOfControlDevices() << " control devices";
 
            JointAndNJointControllers ctrlinitReq(_module<Devices>().getNumberOfControlDevices());
            ARMARX_CHECK_EQUAL(_module<Devices>().getNumberOfControlDevices(),
                               ctrlinitReq.jointControllers.size());
            ARMARX_CHECK_EQUAL(_module<Devices>().getNumberOfControlDevices(),
                               ctrlinitReq.nJointControllers.size());
            ARMARX_CHECK_EQUAL(_module<Devices>().getNumberOfControlDevices(),
                               ctrlinitReq.jointToNJointControllerAssignement.size());
            controllersActivated.reinitAllBuffers(ctrlinitReq);
 
            ctrlinitReq.jointControllers =
                DevicesAttorneyForControlThreadDataBuffer::GetStopMovementJointControllers(this);
            ARMARX_CHECK_EQUAL(_module<Devices>().getNumberOfControlDevices(),
                               ctrlinitReq.jointControllers.size());
            ARMARX_CHECK_EQUAL(_module<Devices>().getNumberOfControlDevices(),
                               ctrlinitReq.nJointControllers.size());
            ARMARX_CHECK_EQUAL(_module<Devices>().getNumberOfControlDevices(),
                               ctrlinitReq.jointToNJointControllerAssignement.size());
            controllersRequested.reinitAllBuffers(ctrlinitReq);
            controllersRequested
                .commitWrite(); //to make sure the diry bit is activated (to trigger rt switch)
        }
    }
} // namespace armarx::RobotUnitModule