RobotControl.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    ArmarX::
* @author     Mirko Waechter ( mirko.waechter at kit dot edu)
* @date       2012
* @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
*             GNU General Public License
*/
 
#include "RobotControl.h"
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/observers/variant/ChannelRef.h>
#include <ArmarXCore/statechart/DynamicRemoteState.h>
 
#include <RobotAPI/components/units/KinematicUnitObserver.h>
 
namespace armarx
{
    void
    RobotControl::onInitRemoteStateOfferer()
    {
        addState<Statechart_Robot>("RobotControl");
    }
 
    void
    RobotControl::onConnectRemoteStateOfferer()
    {
        startRobotStatechart();
    }
 
    void
    RobotControl::onExitRemoteStateOfferer()
    {
        removeInstance(stateId);
        robotFunctionalState = NULL;
    }
 
    // this function creates an instance of the robot-statechart,
    // so there is always one running-instance active that can
    // be controlled by the GUI
    void
    RobotControl::startRobotStatechart()
    {
 
 
        task = new RunningTask<RobotControl>(this, &RobotControl::createStaticInstance);
        task->start();
    }
 
    void
    RobotControl::hardReset(const Ice::Current&)
    {
        removeInstance(robotFunctionalState->getLocalUniqueId());
        startRobotStatechart();
    }
 
    PropertyDefinitionsPtr
    RobotControl::createPropertyDefinitions()
    {
        return PropertyDefinitionsPtr(new RobotControlContextProperties(getConfigIdentifier()));
    }
 
    void
    RobotControl::createStaticInstance()
    {
        getObjectScheduler()->waitForObjectState(eManagedIceObjectStarted, 5000);
        stateId = createRemoteStateInstance("RobotControl", NULL, "RobotStatechart", "");
        std::map<int, StateBasePtr> stateList = getChildStatesByName(stateId, "Functional");
        ARMARX_INFO << "Functional State Id:" << stateList.begin()->first << flush;
        assert(stateList.size() > 0);
        int robotFunctionalStateId = stateList.begin()->first;
        robotFunctionalState = stateList.begin()->second;
        callRemoteState(stateId, StringVariantContainerBaseMap());
 
        const std::string proxyName = getProperty<std::string>("XMLStatechartProfile").getValue() +
                                      getProperty<std::string>("proxyName").getValue();
        const std::string stateName = getProperty<std::string>("stateName").getValue();
        ARMARX_IMPORTANT << VAROUT(proxyName) << VAROUT(stateName);
 
        if (!proxyName.empty() && !stateName.empty())
        {
            auto statebase = robotFunctionalState->findSubstateByName("DynamicRemoteState");
            ARMARX_CHECK_EXPRESSION(statebase);
            DynamicRemoteStatePtr state = DynamicRemoteStatePtr::dynamicCast(statebase);
 
            if (!state)
            {
                ARMARX_ERROR << "dynamic state pointer null";
                return;
            }
 
            state->getObjectScheduler()->waitForObjectState(eManagedIceObjectStarted, 5000);
            EventPtr evt = createEvent<EvLoadScenario>();
            evt->eventReceiverName = "toAll";
            evt->properties["proxyName"] = new SingleVariant(proxyName);
            evt->properties["stateName"] = new SingleVariant(stateName);
            issueEvent(robotFunctionalStateId, evt);
        }
    }
 
    void
    Statechart_Robot::defineState()
    {
    }
 
    void
    Statechart_Robot::defineSubstates()
    {
        //add substates
        StateBasePtr stateFunctional = setInitState(addState<StateRobotControl>("Functional"));
        StateBasePtr stateStopped = addState<State>("Stopped");
        StateBasePtr stateFailure = addState<State>("Failure");
 
        // add transitions
        addTransition<Failure>(stateFunctional, stateFailure);
        addTransition<EvStopRobot>(stateFunctional, stateStopped);
        addTransition<EvStartRobot>(stateStopped, stateFunctional);
        addTransition<EvStartRobot>(stateFailure, stateFunctional);
    }
 
    void
    StateRobotControl::defineSubstates()
    {
        // add substates
        StateBasePtr stateIdling = addState<State>("Idling");
        StateBasePtr stateRobotPreInitialized =
            addState<RobotPreInitialized>("RobotPreInitialized");
        StateBasePtr stateRobotInitialized = addState<State>("RobotInitialized");
        StateBasePtr stateScenarioRunning = addDynamicRemoteState("DynamicScenarioState");
        StateBasePtr stateFatalError = addState<FailureState>("FatalError");
 
        setInitState(stateRobotInitialized);
        // add transitions
        addTransition<EvInit>(stateIdling, stateRobotPreInitialized);
        addTransition<EvInitialized>(stateRobotPreInitialized, stateRobotInitialized);
        addTransition<EvLoadScenario>(stateRobotInitialized,
                                      stateScenarioRunning,
                                      PM::createMapping()->mapFromEvent("*", "*"));
        addTransition<EvInit>(stateScenarioRunning, stateRobotPreInitialized);
        addTransition<Success>(stateScenarioRunning, stateRobotInitialized);
 
        // failure transitions
        addTransition<EvLoadingFailed>(stateScenarioRunning, stateFatalError);
        addTransition<EvConnectionLost>(stateScenarioRunning, stateFatalError);
        addTransition<EvInitFailed>(stateRobotPreInitialized, stateFatalError);
        addTransitionFromAllStates<Failure>(stateFatalError);
    }
 
    void
    StateRobotControl::onEnter()
    {
        // install global running conditions for the robot (e.g. temperature < xx°C)
        //        KinematicUnitObserverInterfacePrx prx = getContext()->getProxy<KinematicUnitObserverInterfacePrx>("KinematicUnitObserver");
        //        installCondition<Failure>(Expression::create()
        //                         ->add(channels::KinematicUnitObserver::jointTemperatures.getDatafield("KinematicUnitObserver", "NECK_JOINT0"), checks::KinematicUnitObserver::larger->getCheckStr(),  50.f));
    }
 
    RobotPreInitialized::RobotPreInitialized()
    {
    }
 
    void
    RobotPreInitialized::onEnter()
    {
        // issue init on remote components and install condition "RobotInitialized"
    }
 
} // namespace armarx