ControllerWrapper.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/>.
 *
 * @author     Fabian Reister ( fabian dot reister at kit dot edu )
 * @date       2024
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "ControllerWrapper.h"
 
#include <ArmarXCore/core/exceptions/LocalException.h>
 
#include <RobotAPI/libraries/aron/core/codegeneration/cpp/AronGeneratedClass.h>
#include <RobotAPI/libraries/aron/core/codegeneration/cpp/AronGeneratedObject.h>
 
#include <armarx/control/client/ControllerDescription.h>
#include <armarx/control/interface/ConfigurableNJointControllerInterface.h>
#include <armarx/control/memory/config/Writer.h>
#include <armarx/control/memory/constants.h>
 
namespace armarx::control::client::detail
{
 
 
    ControllerWrapperBase::ControllerWrapperBase(const ControllerPrx& controller,
                                                 const std::string& controllerName,
                                                 const std::string& controllerTypeName,
                                                 const memory::config::Writer& configWriter) :
        controller_(controller),
        controllerName_(controllerName),
        controllerTypeName_(controllerTypeName),
        configWriter_(configWriter)
    {
    }
 
    ControllerWrapperBase::~ControllerWrapperBase()
    {
        // deactivate the controller
        if (not daemonized_)
        {
            try
            {
                ARMARX_INFO << "Deactivating and deleting controller `" << controllerName_ << "`";
                controller_->deactivateAndDeleteController();
            }
            // Swallow any exception: destructors must not throw (especially not during
            // stack unwinding, where it would call std::terminate). The downside is that
            // if the RPC fails here, the controller may be leaked on the RobotUnit side
            // and never actually deleted.
            // TODO: fix this properly — ensure the controller is reliably deactivated
            // and deleted on the RobotUnit even when the wrapper is destroyed during
            // an exception (e.g. retry, defer cleanup to a non-throwing path, or make
            // the server-side call idempotent and infallible).
            catch (...)
            {
                ARMARX_WARNING << "Exception while deactivating and deleting controller `"
                               << controllerName_
                               << "`. The controller may not have been deleted on the "
                                  "RobotUnit: "
                               << GetHandledExceptionString();
            }
        }
    }
 
    void
    ControllerWrapperBase::activate()
    {
        ARMARX_CHECK_EXPRESSION(controller_);
        ARMARX_INFO << "Activating controller `" << controllerName_ << "`";
        controller_->activateController();
    }
 
    bool
    ControllerWrapperBase::isActive()
    {
        ARMARX_CHECK_EXPRESSION(controller_);
        return controller_->isControllerActive();
    }
 
    void
    ControllerWrapperBase::deactivate()
    {
        ARMARX_CHECK_EXPRESSION(controller_);
        ARMARX_INFO << "Deactivating controller `" << controllerName_ << "`";
        controller_->deactivateController();
    }
 
    void
    ControllerWrapperBase::daemonize(const bool daemonize)
    {
        ARMARX_INFO << "Daemonizing controller `" << controllerName_ << "`";
        daemonized_ = daemonize;
    }
 
    ControllerWrapperBase::ControllerPrx&
    ControllerWrapperBase::ctrl()
    {
        ARMARX_CHECK_EXPRESSION(controller_);
        return controller_;
    }
 
    void
    ControllerWrapperBase::updateConfig(
        const armarx::aron::codegenerator::cpp::AronGeneratedObjectBase& config)
    {
        ARMARX_VERBOSE << "Updating config for controller `" << controllerName_ << "`";
 
        ARMARX_CHECK_NOT_NULL(controller_);
        auto start = armarx::DateTime::Now();
        auto cfgDTO = config.toAronDTO();
        ARMARX_DEBUG << "--> config.toAronDTO() of control cfg took "
                     << (armarx::DateTime::Now() - start).toSecondsDouble() << " sec.";
        start = armarx::DateTime::Now();
        controller_->updateConfig(cfgDTO);
        ARMARX_DEBUG
            << "--> sending the converted AronDTO (controller_->updateConfig(cfgDTO)) took "
            << (armarx::DateTime::Now() - start).toSecondsDouble() << " sec.";
 
        // controller_->updateConfig(config.toAronDTO());
 
        // store the config also in the memory (logging only)
        start = armarx::DateTime::Now();
        configWriter_.storeConfig(controllerTypeName_, controllerName_, config.toAron());
        ARMARX_DEBUG << "--> store config in memory (configWriter_.storeConfig) took "
                     << (armarx::DateTime::Now() - start).toSecondsDouble() << " sec.";
    }
} // namespace armarx::control::client::detail