ControlDevice.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
 * @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 "ControlDevice.h"
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
 
namespace armarx
{
    const ControlDevicePtr ControlDevice::NullPtr{nullptr};
 
    JointController*
    ControlDevice::getJointEmergencyStopController()
    {
        ARMARX_CHECK_EXPRESSION(jointEmergencyStopController)
            << "ControlDevice::getJointEmergencyStopController called for a ControlDevice ('"
            << getDeviceName()
            << "') without a JointController "
               "with the ControlMode ControlModes::EmergencyStop"
               " (add a JointController with this ControlMode)";
        return jointEmergencyStopController;
    }
 
    JointController*
    ControlDevice::getJointStopMovementController()
    {
        ARMARX_CHECK_EXPRESSION(jointStopMovementController)
            << "ControlDevice::getJointStopMovementController called for a ControlDevice ('"
            << getDeviceName()
            << "') without a JointController "
               "with the ControlMode ControlModes::StopMovement"
               " (add a JointController with this ControlMode)";
        return jointStopMovementController;
    }
 
    void
    ControlDevice::rtSetActiveJointController(JointController* jointCtrl)
    {
        ARMARX_CHECK_EXPRESSION(jointCtrl)
            << "Called ControlDevice::rtSetActiveJointController with a nullptr (Don't do this)";
        if (activeJointController == jointCtrl)
        {
            return;
        }
        if (jointCtrl->parent != this)
        {
            throw std::logic_error{
                "ControlDevice(" + getDeviceName() +
                ")::rtSetActiveJointController: "
                "tried to switch to a joint controller from a different ControlDevice "
                "(You should only call ControlDevice::rtSetActiveJointController with "
                "JointControllers added to the ControlDevice via "
                "ControlDevice::addJointController)"};
        }
        if (activeJointController)
        {
            activeJointController->rtDeactivate();
        }
        jointCtrl->rtActivate();
        activeJointController = jointCtrl;
    }
 
    void
    ControlDevice::rtRun(const IceUtil::Time& sensorValuesTimestamp,
                         const IceUtil::Time& timeSinceLastIteration)
    {
        auto activejointCtrl = rtGetActiveJointController();
        ARMARX_CHECK_EXPRESSION(activejointCtrl);
        activejointCtrl->rtRun(sensorValuesTimestamp, timeSinceLastIteration);
        rtWriteTargetValues(sensorValuesTimestamp, timeSinceLastIteration);
    }
 
    void
    ControlDevice::addJointController(JointController* jointCtrl)
    {
        ARMARX_CHECK_IS_NULL(owner) << "The ControlDevice '" << getDeviceName()
                                    << "' was already added to a RobotUnit! Call "
                                       "addJointController before calling addControlDevice.";
        ARMARX_DEBUG << "adding Controller " << jointCtrl;
        if (!jointCtrl)
        {
            throw std::invalid_argument{"ControlDevice(" + getDeviceName() +
                                        ")::addJointController: joint controller is nullptr (Don't "
                                        "try nullptrs as JointController)"};
        }
        if (jointCtrl->parent)
        {
            throw std::logic_error{
                "ControlDevice(" + getDeviceName() +
                ")::addJointController: tried to add a joint controller multiple times" +
                "(Don't try to add a JointController multiple to a ControlDevice)"};
        }
        ARMARX_DEBUG << "getControlMode of " << jointCtrl;
        const std::string& mode = jointCtrl->getControlMode();
        ARMARX_VERBOSE << "adding joint controller for device '" << getDeviceName()
                       << "' with mode '" << mode << "'";
        if (jointControllers.has(mode))
        {
            throw std::invalid_argument{
                "ControlDevice(" + getDeviceName() +
                ")::addJointController: joint controller for mode " + mode + " was already added" +
                "(Don't try to add multiple JointController with the same ControlMode)"};
        }
        jointCtrl->parent = this;
        ARMARX_DEBUG << "resetting target";
        jointCtrl->rtResetTarget();
        ARMARX_DEBUG << "resetting target...done!";
        if (mode == ControlModes::EmergencyStop)
        {
            jointEmergencyStopController = jointCtrl;
        }
        if (mode == ControlModes::StopMovement)
        {
            jointStopMovementController = jointCtrl;
        }
        if (!jointCtrl->getControlTarget())
        {
            throw std::logic_error{
                "ControlDevice(" + getDeviceName() +
                ")::addJointController: The JointController has no ControlTarget. (mode = " + mode +
                ")" + "(Don't try to add JointController without a ControlTarget)"};
        }
        const std::hash<std::string> hash{};
        jointCtrl->controlModeHash = hash(jointCtrl->getControlMode());
        jointCtrl->hardwareControlModeHash = hash(jointCtrl->getHardwareControlMode());
        jointControllers.add(mode, std::move(jointCtrl));
        ARMARX_DEBUG << "adding Controller " << jointCtrl << "...done";
        ARMARX_CHECK_IS_NULL(owner);
    }
 
    RobotUnitModule::Devices*
    ControlDevice::getOwner() const
    {
        return owner;
    }
 
    std::map<std::string, std::string>
    ControlDevice::getJointControllerToTargetTypeNameMap() const
    {
        std::map<std::string, std::string> map;
        for (const auto& name : jointControllers.keys())
        {
            map[name] = jointControllers.at(name)->getControlTarget()->getControlTargetType();
        }
        return map;
    }
 
} // namespace armarx