RobotUnitModuleBase.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 "RobotUnitModuleBase.h"
 
#include <sstream>
 
#include <VirtualRobot/Robot.h>
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/util/Preprocessor.h>
#include <ArmarXCore/util/CPPUtility/trace.h>
 
#include "RobotUnitModules.h"
 
namespace armarx
{
    std::string
    to_string(RobotUnitState s)
    {
        switch (s)
        {
            case armarx::RobotUnitState::InitializingComponent:
                return "RobotUnitState::InitializingComponent";
            case armarx::RobotUnitState::InitializingDevices:
                return "RobotUnitState::InitializingDevices";
            case armarx::RobotUnitState::InitializingUnits:
                return "RobotUnitState::InitializingUnits";
            case armarx::RobotUnitState::InitializingControlThread:
                return "RobotUnitState::InitializingControlThread";
            case armarx::RobotUnitState::Running:
                return "RobotUnitState::Running";
            case armarx::RobotUnitState::Exiting:
                return "RobotUnitState::Exiting";
        }
        throw std::invalid_argument{"Unknown state " + to_string(static_cast<std::size_t>(s)) +
                                    "\nStacktrace\n" + LogSender::CreateBackTrace()};
    }
} // namespace armarx
 
namespace armarx::RobotUnitModule
{
    // //////////////////////////////////////////////////////////////////////////// //
    // /////////////////////////// call for each module /////////////////////////// //
    // //////////////////////////////////////////////////////////////////////////// //
#define cast_to_and_call(Type, fn, rethrow)                                                        \
    ARMARX_TRACE;                                                                                  \
    try                                                                                            \
    {                                                                                              \
        dynamic_cast<Type*>(this)->Type::fn;                                                       \
    }                                                                                              \
    catch (Ice::Exception & e)                                                                     \
    {                                                                                              \
        ARMARX_ERROR << "exception in " << #Type "::" #fn << "!\nwhat:\n"                          \
                     << e.what() << "\n\tid   : " << e.ice_id() << "\n\tfile : " << e.ice_file()   \
                     << "\n\tline : " << e.ice_line() << "\n\tstack: " << e.ice_stackTrace();      \
        if (rethrow)                                                                               \
        {                                                                                          \
            throw;                                                                                 \
        }                                                                                          \
    }                                                                                              \
    catch (std::exception & e)                                                                     \
    {                                                                                              \
        ARMARX_ERROR << "exception in " << #Type "::" #fn << "!\nwhat:\n" << e.what();             \
        if (rethrow)                                                                               \
        {                                                                                          \
            throw;                                                                                 \
        }                                                                                          \
    }                                                                                              \
    catch (...)                                                                                    \
    {                                                                                              \
        ARMARX_ERROR << "exception in " << #Type "::" #fn << "!";                                  \
        if (rethrow)                                                                               \
        {                                                                                          \
            throw;                                                                                 \
        }                                                                                          \
    }
 
#define for_each_module_apply(r, data, elem) data(elem)
#define for_each_module(macro)                                                                     \
    BOOST_PP_SEQ_FOR_EACH(for_each_module_apply, macro, BOOST_PP_VARIADIC_TO_SEQ(RobotUnitModules))
 
#define check_base(Type)                                                                           \
    static_assert(std::is_base_of<ModuleBase, Type>::value,                                        \
                  "The RobotUnitModule '" #Type "' has to derived ModuleBase");
    for_each_module(check_base)
#undef check_base
 
        void ModuleBase::checkDerivedClasses() const
    {
#define check_deriving(Type)                                                                       \
    ARMARX_CHECK_NOT_NULL(dynamic_cast<const Type*>(this))                                         \
        << "This class does not derive from " << GetTypeString<Type>();
        for_each_module(check_deriving)
#undef check_deriving
    }
 
    // //////////////////////////////////////////////////////////////////////////// //
    // ManagedIceObject life cycle
    void
    ModuleBase::onInitComponent()
    {
        ARMARX_TRACE;
        checkDerivedClasses();
        auto guard = getGuard();
        throwIfStateIsNot(RobotUnitState::InitializingComponent, __FUNCTION__);
        ARMARX_IMPORTANT << "Starting initialization of RobotUnitModule '" << getName();
        cast_to_and_call(::armarx::RobotUnitModule::RobotData, _initVirtualRobot(), true);
 
#define call_module_hook(Type)                                                                     \
    cast_to_and_call(::armarx::RobotUnitModule::Type, _preOnInitRobotUnit(), true)
        for_each_module(call_module_hook)
#undef call_module_hook
 
            onInitRobotUnit();
 
#define call_module_hook(Type)                                                                     \
    cast_to_and_call(::armarx::RobotUnitModule::Type, _postOnInitRobotUnit(), true)
        for_each_module(call_module_hook)
#undef call_module_hook
 
            finishComponentInitialization();
    }
 
    void
    ModuleBase::onConnectComponent()
    {
        ARMARX_TRACE;
        checkDerivedClasses();
 
#define call_module_hook(Type)                                                                     \
    cast_to_and_call(::armarx::RobotUnitModule::Type, _preOnConnectRobotUnit(), true)
        for_each_module(call_module_hook)
#undef call_module_hook
 
            onConnectRobotUnit();
 
#define call_module_hook(Type)                                                                     \
    cast_to_and_call(::armarx::RobotUnitModule::Type, _postOnConnectRobotUnit(), true)
        for_each_module(call_module_hook)
#undef call_module_hook
    }
 
    void
    ModuleBase::onDisconnectComponent()
    {
        ARMARX_TRACE;
        checkDerivedClasses();
 
#define call_module_hook(Type)                                                                     \
    cast_to_and_call(::armarx::RobotUnitModule::Type, _preOnDisconnectRobotUnit(), true)
        for_each_module(call_module_hook)
#undef call_module_hook
 
            onDisconnectRobotUnit();
 
#define call_module_hook(Type)                                                                     \
    cast_to_and_call(::armarx::RobotUnitModule::Type, _postOnDisconnectRobotUnit(), true)
        for_each_module(call_module_hook)
#undef call_module_hook
    }
 
    void
    ModuleBase::onExitComponent()
    {
        ARMARX_TRACE;
        checkDerivedClasses();
        auto guard = getGuard();
 
        finishRunning();
 
#define call_module_hook(Type)                                                                     \
    cast_to_and_call(::armarx::RobotUnitModule::Type, _preOnExitRobotUnit(), false)
        for_each_module(call_module_hook)
#undef call_module_hook
 
            onExitRobotUnit();
 
#define call_module_hook(Type)                                                                     \
    cast_to_and_call(::armarx::RobotUnitModule::Type, _postOnExitRobotUnit(), false)
        for_each_module(call_module_hook)
#undef call_module_hook
    }
 
    // //////////////////////////////////////////////////////////////////////////// //
    //  other ManagedIceObject / Component methods
    void
    ModuleBase::componentPropertiesUpdated(const std::set<std::string>& changedProperties)
    {
#define call_module_hook(Type)                                                                     \
    cast_to_and_call(                                                                              \
        ::armarx::RobotUnitModule::Type, _componentPropertiesUpdated(changedProperties), true)
        for_each_module(call_module_hook)
#undef call_module_hook
    }
 
    void
    ModuleBase::icePropertiesInitialized()
    {
#define call_module_hook(Type)                                                                     \
    cast_to_and_call(::armarx::RobotUnitModule::Type, _icePropertiesInitialized(), true)
        for_each_module(call_module_hook)
#undef call_module_hook
    }
 
    // //////////////////////////////////////////////////////////////////////////// //
    // RobotUnit life cycle
    void
    ModuleBase::finishComponentInitialization()
    {
        ARMARX_TRACE;
        checkDerivedClasses();
        auto guard = getGuard();
        throwIfStateIsNot(RobotUnitState::InitializingComponent, __FUNCTION__);
 
#define call_module_hook(Type)                                                                     \
    cast_to_and_call(::armarx::RobotUnitModule::Type, _preFinishComponentInitialization(), true)
        for_each_module(call_module_hook)
#undef call_module_hook
 
            setRobotUnitState(RobotUnitState::InitializingDevices);
 
#define call_module_hook(Type)                                                                     \
    cast_to_and_call(::armarx::RobotUnitModule::Type, _postFinishComponentInitialization(), true)
        for_each_module(call_module_hook)
#undef call_module_hook
    }
 
    void
    ModuleBase::finishDeviceInitialization()
    {
        ARMARX_TRACE;
        checkDerivedClasses();
        auto guard = getGuard();
        throwIfStateIsNot(RobotUnitState::InitializingDevices, __FUNCTION__);
 
#define call_module_hook(Type)                                                                     \
    cast_to_and_call(::armarx::RobotUnitModule::Type, _preFinishDeviceInitialization(), true)
        for_each_module(call_module_hook)
#undef call_module_hook
 
            setRobotUnitState(RobotUnitState::InitializingUnits);
 
#define call_module_hook(Type)                                                                     \
    cast_to_and_call(::armarx::RobotUnitModule::Type, _postFinishDeviceInitialization(), true)
        for_each_module(call_module_hook)
#undef call_module_hook
    }
 
    void
    ModuleBase::finishUnitInitialization()
    {
        ARMARX_TRACE;
        checkDerivedClasses();
        auto guard = getGuard();
        throwIfStateIsNot(RobotUnitState::InitializingUnits, __FUNCTION__);
 
#define call_module_hook(Type)                                                                     \
    cast_to_and_call(::armarx::RobotUnitModule::Type, _preFinishUnitInitialization(), true)
        for_each_module(call_module_hook)
#undef call_module_hook
 
            setRobotUnitState(RobotUnitState::InitializingControlThread);
 
#define call_module_hook(Type)                                                                     \
    cast_to_and_call(::armarx::RobotUnitModule::Type, _postFinishUnitInitialization(), true)
        for_each_module(call_module_hook)
#undef call_module_hook
    }
 
    void
    ModuleBase::finishControlThreadInitialization()
    {
        ARMARX_TRACE;
        checkDerivedClasses();
        auto guard = getGuard();
        throwIfStateIsNot(RobotUnitState::InitializingControlThread, __FUNCTION__);
 
#define call_module_hook(Type)                                                                     \
    cast_to_and_call(::armarx::RobotUnitModule::Type, _preFinishControlThreadInitialization(), true)
        for_each_module(call_module_hook)
#undef call_module_hook
 
            setRobotUnitState(RobotUnitState::Running);
 
#define call_module_hook(Type)                                                                     \
    cast_to_and_call(                                                                              \
        ::armarx::RobotUnitModule::Type, _postFinishControlThreadInitialization(), true)
        for_each_module(call_module_hook)
#undef call_module_hook
    }
 
    void
    ModuleBase::finishRunning()
    {
        ARMARX_TRACE;
        checkDerivedClasses();
        shutDown();
        GuardType guard{dataMutex}; //DO NOT USE getGuard here!
        if (getRobotUnitState() != RobotUnitState::Running)
        {
            ARMARX_ERROR << "called finishRunning when state was "
                         << to_string(getRobotUnitState());
        }
 
#define call_module_hook(Type)                                                                     \
    cast_to_and_call(::armarx::RobotUnitModule::Type, _preFinishRunning(), false)
        for_each_module(call_module_hook)
#undef call_module_hook
 
            setRobotUnitState(RobotUnitState::Exiting);
        joinControlThread();
 
#define call_module_hook(Type)                                                                     \
    cast_to_and_call(::armarx::RobotUnitModule::Type, _postFinishRunning(), false)
        for_each_module(call_module_hook)
#undef call_module_hook
    }
 
#undef for_each_module_apply
#undef for_each_module
#undef cast_to_and_call
    // //////////////////////////////////////////////////////////////////////////// //
    // ////////////////////////////////// other /////////////////////////////////// //
    // //////////////////////////////////////////////////////////////////////////// //
 
    const std::set<RobotUnitState> ModuleBase::DevicesReadyStates{
        RobotUnitState::InitializingUnits,
        RobotUnitState::InitializingControlThread,
        RobotUnitState::Running};
 
    void
    ModuleBase::setRobotUnitState(RobotUnitState to)
    {
        ARMARX_TRACE;
        auto guard = getGuard();
 
        const auto transitionErrorMessage = [=, this](RobotUnitState expectedFrom)
        {
            return "Can't switch to state " + to_string(to) + " from " + to_string(state) +
                   "! The expected source state is " + to_string(expectedFrom) + ".";
        };
        const auto transitionErrorThrow = [=, this](RobotUnitState expectedFrom)
        {
            if (state != expectedFrom)
            {
                const auto msg = transitionErrorMessage(expectedFrom);
                ARMARX_ERROR << msg;
                throw std::invalid_argument{msg};
            }
        };
 
        switch (to)
        {
            case armarx::RobotUnitState::InitializingComponent:
                throw std::invalid_argument{
                    "Can't switch to state RobotUnitState::InitializingComponent"};
            case armarx::RobotUnitState::InitializingDevices:
                transitionErrorThrow(RobotUnitState::InitializingComponent);
                break;
            case RobotUnitState::InitializingUnits:
                transitionErrorThrow(RobotUnitState::InitializingDevices);
                break;
            case RobotUnitState::InitializingControlThread:
                transitionErrorThrow(RobotUnitState::InitializingUnits);
                break;
            case RobotUnitState::Running:
                transitionErrorThrow(RobotUnitState::InitializingControlThread);
                break;
            case RobotUnitState::Exiting:
                if (state != RobotUnitState::Running)
                {
                    ARMARX_ERROR << transitionErrorMessage(RobotUnitState::Running);
                }
                break;
            default:
                throw std::invalid_argument{"setRobotUnitState: Unknown target state " +
                                            to_string(static_cast<std::size_t>(to)) +
                                            "\nStacktrace\n" + LogSender::CreateBackTrace()};
        }
        ARMARX_INFO << "switching state from " << to_string(state) << " to " << to_string(to);
        state = to;
    }
 
    bool
    ModuleBase::areDevicesReady() const
    {
        return DevicesReadyStates.count(state);
    }
 
    bool
    ModuleBase::inControlThread() const
    {
        return getRobotUnitState() == RobotUnitState::Running &&
               std::this_thread::get_id() == _module<ControlThread>().getControlThreadId();
    }
 
    void
    ModuleBase::throwIfInControlThread(const std::string& fnc) const
    {
        ARMARX_TRACE;
        if (inControlThread())
        {
            std::stringstream str;
            str << "Called function '" << fnc << "' in the Control Thread\nStack trace:\n"
                << LogSender::CreateBackTrace();
            ARMARX_ERROR << str.str();
            throw LogicError{str.str()};
        }
    }
 
    ModuleBase::GuardType
    ModuleBase::getGuard() const
    {
        ARMARX_TRACE;
        if (inControlThread())
        {
            throw LogicError{"Attempted to lock mutex in Control Thread\nStack trace:\n" +
                             LogSender::CreateBackTrace()};
        }
        GuardType guard{dataMutex, std::defer_lock};
        if (guard.try_lock())
        {
            return guard;
        }
        while (!guard.try_lock_for(std::chrono::microseconds{100}))
        {
            if (isShuttingDown())
            {
                throw LogicError{"Attempting to lock mutex during shutdown\nStacktrace\n" +
                                 LogSender::CreateBackTrace()};
            }
        }
 
        return guard;
    }
 
    void
    ModuleBase::throwIfStateIsNot(const std::set<RobotUnitState>& stateSet,
                                  const std::string& fnc,
                                  bool onlyWarn) const
    {
        ARMARX_TRACE;
        if (!stateSet.count(state))
        {
            std::stringstream ss;
            ss << fnc << ": Can't be called if state is not in {";
            bool fst = true;
            for (RobotUnitState st : stateSet)
            {
                if (!fst)
                {
                    ss << ", ";
                }
                ss << st;
                fst = false;
            }
            ss << "} (current state " << getRobotUnitState() << ")\n"
               << "Stacktrace:\n"
               << LogSender::CreateBackTrace();
            ARMARX_ERROR << ss.str();
            if (!onlyWarn)
            {
                throw LogicError{ss.str()};
            }
        }
    }
 
    void
    ModuleBase::throwIfStateIsNot(RobotUnitState s, const std::string& fnc, bool onlyWarn) const
    {
        throwIfStateIsNot(std::set<RobotUnitState>{s}, fnc, onlyWarn);
    }
 
    void
    ModuleBase::throwIfStateIs(const std::set<RobotUnitState>& stateSet,
                               const std::string& fnc,
                               bool onlyWarn) const
    {
        ARMARX_TRACE;
        if (stateSet.count(state))
        {
            std::stringstream ss;
            ss << fnc << ": Can't be called if state is in {";
            bool fst = true;
            for (RobotUnitState st : stateSet)
            {
                if (!fst)
                {
                    ss << ", ";
                }
                ss << st;
                fst = false;
            }
            ss << "} (current state " << getRobotUnitState() << ")\n"
               << "Stacktrace:\n"
               << LogSender::CreateBackTrace();
            ARMARX_ERROR << ss.str();
            if (!onlyWarn)
            {
                throw LogicError{ss.str()};
            }
        }
    }
 
    void
    ModuleBase::throwIfStateIs(RobotUnitState s, const std::string& fnc, bool onlyWarn) const
    {
        throwIfStateIs(std::set<RobotUnitState>{s}, fnc, onlyWarn);
    }
 
    void
    ModuleBase::throwIfDevicesNotReady(const std::string& fnc) const
    {
        throwIfStateIsNot(DevicesReadyStates, fnc);
    }
 
    std::atomic<ModuleBase*> ModuleBase::Instance_{nullptr};
 
    ModuleBase::ModuleBase()
    {
        ARMARX_TRACE;
        if (Instance_ && this != Instance_)
        {
            ARMARX_FATAL << "This class is a Singleton. It was already instantiated. (Instance = "
                         << Instance_ << ", this = " << this << ")";
            std::terminate();
        }
        Instance_ = this;
    }
 
} // namespace armarx::RobotUnitModule