EmergencyStop.h

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    ArmarXCore::ArmarXObjects::EmergencyStop
 * @author     Stefan Reither ( stef dot reither at web dot de )
 * @date       2016
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#pragma once
 
#include <atomic>
#include <shared_mutex>
 
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/time.h>
#include <ArmarXCore/interface/components/EmergencyStopInterface.h>
 
namespace armarx
{
    /**
     * @class EmergencyStopPropertyNodeDefinitions
     * @brief
     */
    class EmergencyStopNodePropertyDefinitions : public armarx::ComponentPropertyDefinitions
    {
    public:
        EmergencyStopNodePropertyDefinitions(std::string prefix) :
            armarx::ComponentPropertyDefinitions(prefix)
        {
            defineOptionalProperty<std::string>("EmergencyStopProxy",
                                                "EmergencyStopMaster",
                                                "The name of the emergencyStop-proxy.");
            defineOptionalProperty<std::string>(
                "EmergencyStopTopic",
                "EmergencyStop",
                "The name of the topic over which changes of the emergencyStopState are sent.");
        }
    };
 
    class EmergencyStopPropertyMasterDefinitions : public armarx::ComponentPropertyDefinitions
    {
    public:
        EmergencyStopPropertyMasterDefinitions(std::string prefix) :
            armarx::ComponentPropertyDefinitions(prefix)
        {
            defineOptionalProperty<std::string>(
                "EmergencyStopTopic",
                "EmergencyStop",
                "The name of the topic over which changes of the emergencyStopState are sent.");
        }
    };
 
    /**
     * @defgroup Component-EmergencyStop EmergencyStop
     * @brief This component provides a software emergency-stop mechanism.
     * @ingroup ArmarXCore-Components
     *
     * The mechanism consists of one EmergencyStopMaster and one or more EmergencyStopNode(s).
     * Every node connects to the master and listens to any changes of the EmergencyStopState send over the specified topic.
     * <br>
     * The state of the nodes is always identical to the state of the master and can be used in the component that started such a node.
     * Furthermore there is the possibility to register functions of the component directly  in the node, which then will be executed immediately
     * when the master changes into the specified state.
     *
     * **Properties:**
     *
     * In most usecases there is no need to change any of the two optional properties: *EmergencyStopProxy*
     * and *EmergencyStopTopic*. With their standard values the functionality of one master and many nodes and the button
     * in the ArmarXGui is guaranteed. Only in case there are multiple masters with different states, these additional masters have to
     * get a different proxy-name and topic-name.
     *
     * **Usage:**
     *
     * Example implementation of an EmergencyStopNode in a component:
     *
     * KinematicUnitArmar4.h:
     * \verbatim
       #include <ArmarXCore/components/EmergencyStop/EmergencyStop.h>
       class KinematicUnitArmar4
       {
            ...
            armarx::EmergencyStopNode<KinematicUnitArmar4>::EmergencyStopNodePtr emergencyStopNode;
            ...
        }
       \endverbatim
     *
     * KinematicUnitArmar4.cpp:
     * \verbatim
       void KinematicUnitArmar4::onInitKinematicUnit()
       {
            //
            //
            //
 
            // Creating an EmergencyStopNode with this class (KinematicUnitArmar4) as template-typ
            emergencyStopNode = armarx::Component::create < armarx::EmergencyStopNode<KinematicUnitArmar4> >();
 
            // Adding the created node to the ArmarXManager, it is important to use an UUID in the name, because there might
            // be several nodes
            this->getArmarXManager()->addObject(emergencyStopNode, false, emergencyStopNode->getDefaultName() + IceUtil::generateUUID());
 
            // Waiting for the node-component to be started
            emergencyStopNode->getObjectScheduler()->waitForObjectState(armarx::ManagedIceObjectState::eManagedIceObjectStarted);
 
            // Register a function of this class in the node
            emergencyStopNode->registerCallbackFunction(this, &KinematicUnitArmar4::emergencyStopActive, armarx::EmergencyStopState::eEmergencyStopActive);
 
            // Getting the current state of the EmergencyStop
            EmergencyStopState state = emergencyStopNode->getEmergencyStopState();
            if (state == armarx::EmergencyStopState::eEmergencyStopActive)
            {
                // do something
            }
       }
 
       void KinematicUnitArmar4::emergencyStopActive()
       {
            // do something else
       }
       \endverbatim
     *
     *
     *
     *
     * @class EmergencyStopMaster
     * @ingroup Component-EmergencyStop
     * @brief The EmergencyStopMaster stores the current state of the EmergencyStop and broadcasts any changes to that state
     * over the specified topic.
     */
    class EmergencyStopMaster :
        virtual public armarx::Component,
        virtual public armarx::EmergencyStopMasterInterface
    {
    public:
        /**
         * @brief Sets the state of the EmergencyStop and sends the new state immediatly over the specified topic
         * @param state the new state
         */
        void setEmergencyStopState(EmergencyStopState state,
                                   const ::Ice::Current& = Ice::emptyCurrent) override;
 
        /**
         * @brief Try set the SS2 emergency stop state if it is safe.
         *
         * It is always safe to swith into active SS2 state.  It is only safe to release SS2 state
         * if it has been active for a specified amount of time in order to avoid bouncing.
         *
         * @return Assumed SS2 state after the operation.
         */
        EmergencyStopState
        trySetEmergencyStopState(EmergencyStopState targetState,
                                 const ::Ice::Current& = Ice::emptyCurrent) override;
 
        /**
         * @return the current EmergencyStopState
         */
        EmergencyStopState
        getEmergencyStopState(const ::Ice::Current& = Ice::emptyCurrent) const override;
 
    protected:
        /**
         * @see armarx::ManagedIceObject::onInitComponent()
         */
        void onInitComponent() override;
 
        /**
         * @see armarx::ManagedIceObject::onConnectComponent()
         */
        void onConnectComponent() override;
 
        /**
         * @see PropertyUser::createPropertyDefinitions()
         */
        armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
 
        /**
         * @see armarx::ManagedIceObject::getDefaultName()
         */
        std::string
        getDefaultName() const override
        {
            return "EmergencyStopMaster";
        }
 
    private:
        EmergencyStopListenerPrx emergencyStopTopic;
        mutable std::shared_mutex ss2StateMutex;
        armarx::core::time::DateTime lastSS2Activation;
        EmergencyStopState currentSS2State;
        //emergency_stop::SS2_ThreadSafe emergencyStop;
    };
 
    /**
     * @class EmergencyStopNode
     * @ingroup Component-EmergencyStop
     * @brief This component listens on the specified topic (default value: *EmergencyStop*) and changes its state corresponding to the
     * state send over the topic. Furthermore it calls registered functions if their expected state is equal to the one that has been sent.
     * <br>
     * The template-type of an instantiation of this class has to be equal to the type of the object to which any registered function belong.
     */
    template <class T>
    class EmergencyStopNode :
        virtual public armarx::Component,
        virtual public armarx::EmergencyStopNodeInterface
    {
        typedef void (T::*method_type)(void);
 
        struct CallbackFunction
        {
            T* parent;
            method_type function;
        };
 
        using CallbackFunctionsMap = std::multimap<EmergencyStopState, CallbackFunction>;
 
        // EmergencyStopListener interface
    public:
        /**
         * @brief registerCallbackFunction Registers the given function to be called, if the reported emergencyStopState is equal to the given one.
         * @param parent the object the given function belongs to
         * @param callbackFunction the function to be called when the state of the EmergencyStopMaster changes into the given one.
         * Only functions with return-type void can be registered.
         * @param state the state into which the EmergencyStopMaster has to change to call the given function
         */
        void
        registerCallbackFunction(T* parent, method_type callbackFunction, EmergencyStopState state)
        {
            CallbackFunction cb;
            cb.function = callbackFunction;
            cb.parent = parent;
 
            callbackFunctions.insert(std::pair<EmergencyStopState, CallbackFunction>(state, cb));
 
            ARMARX_INFO << "Registered function: " << callbackFunction << " - " << state;
        }
 
        /**
         * @brief unregisterCallbackFunction Unregisters the given function for the given state. If no registered function fits to the
         * given parameter, nothing happens
         * @param parent the object the given function belongs to
         * @param callbackFunction the function to be unregistered
         * @param state the corresponding state
         */
        void
        unregisterCallbackFunction(T* parent,
                                   method_type callbackFunction,
                                   EmergencyStopState state)
        {
            std::pair<typename CallbackFunctionsMap::iterator,
                      typename CallbackFunctionsMap::iterator>
                range;
            range = callbackFunctions.equal_range(state);
 
            for (typename CallbackFunctionsMap::iterator it = range.first; it != range.second;
                 /* no increment */)
            {
                if ((it->second).parent == parent && (it->second).function == callbackFunction)
                {
                    callbackFunctions.erase(it++);
                }
                else
                {
                    ++it;
                }
            }
        }
 
        /**
         * @return the current EmergencyStopState
         */
        EmergencyStopState
        getEmergencyStopState() const
        {
            return this->currentState;
        }
 
        using EmergencyStopNodePtr = IceInternal::Handle<EmergencyStopNode<T>>;
 
        std::string
        getDefaultName() const override
        {
            return "EmergencyStopNode";
        }
 
    private:
        void
        callback(CallbackFunction callback)
        {
            T* parent = callback.parent;
            method_type function = callback.function;
 
            try
            {
                (parent->*function)();
            }
            catch (...)
            {
                handleExceptions();
            }
        }
 
        void
        reportEmergencyStopState(EmergencyStopState state, const Ice::Current&) override
        {
            currentState = state;
 
            std::pair<typename CallbackFunctionsMap::iterator,
                      typename CallbackFunctionsMap::iterator>
                range;
            range = callbackFunctions.equal_range(state);
 
            for (typename CallbackFunctionsMap::iterator it = range.first; it != range.second; ++it)
            {
                callback(it->second);
            }
        }
 
        CallbackFunctionsMap callbackFunctions;
        std::atomic<EmergencyStopState> currentState;
        // ManagedIceObject interface
    protected:
        void
        onInitComponent() override
        {
            usingTopic(getProperty<std::string>("EmergencyStopTopic").getValue());
        }
 
        void
        onConnectComponent() override
        {
            EmergencyStopMasterInterfacePrx stopMaster = getProxy<EmergencyStopMasterInterfacePrx>(
                getProperty<std::string>("EmergencyStopProxy").getValue(), false, "", false);
            if (stopMaster)
            {
                currentState = stopMaster->getEmergencyStopState();
            }
            else
            {
                currentState = eEmergencyStopInactive;
            }
        }
 
        armarx::PropertyDefinitionsPtr
        createPropertyDefinitions() override
        {
            return armarx::PropertyDefinitionsPtr(
                new EmergencyStopNodePropertyDefinitions(Component::getConfigIdentifier()));
        }
    };
} // namespace armarx