MovePlatformToLandmark.cpp

/**
* This file is part of ArmarX.
*
* ArmarX is free software; you can redistribute it and/or modify
* it under the terms of the GNU Lesser General Public License as
* published by the Free Software Foundation; either version 2 of
* the License, or (at your option) any later version.
*
* 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 Lesser 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::MovePlatformToLandmark
* @author     Valerij Wittenbeck
* @date       2014

* @copyright  http://www.gnu.org/licenses/gpl.txt
*             GNU General Public License
*/

#include "MovePlatformToLandmark.h"
//#include "../core/RobotStatechartContext.h"

#include <Core/observers/variant/ChannelRef.h>
#include <Core/observers/variant/SingleTypeVariantList.h>
#include <Core/robotstate/remote/ArmarPose.h>
#include <Core/core/ManagedIceObject.h>
#include <Core/robotstate/remote/RobotStateObjectFactories.h>
#include <Core/core/CoreObjectFactories.h>
#include <Core/observers/ObserverObjectFactories.h>
#include <Core/interface/units/PlatformUnitInterface.h>

#include <boost/algorithm/string.hpp>

namespace armarx
{


    void MovePlatformToLandmarkStateChart::onInitRemoteStateOfferer()
    {
        addState<StatechartMovePlatformToLandmark>("MovePlatformToLandmarkStateChart");
    }

    void MovePlatformToLandmarkStateChart::onConnectRemoteStateOfferer()
    {

    }

    // ****************************************************************
    // Implementation of StatechartMovePlatformToLandmark
    // ****************************************************************
    void StatechartMovePlatformToLandmark::defineParameters()
    {
        setConfigFile("RobotAPI/scenarios/MovePlatformToLandmarkTest/configs/MovePlatformToLandmarkExampleGraph.xml");

        addToInput("targetLandmark", VariantType::String, false);
        addToInput("landmarkNodes", VariantType::List(VariantType::FramedVector3), false);
        addToInput("landmarkEdges", VariantType::List(VariantType::String), false);
        addToInput("positionalAccuracy", VariantType::Float, true);
        addToInput("orientationalAccuracy", VariantType::Float, true);
        addToInput("timeoutMoveTo", VariantType::Int, false);
    }

    void StatechartMovePlatformToLandmark::defineSubstates()
    {
        RemoteStatePtr stateMoveToLocation = addRemoteState("MovePlatformStateChart","MovePlatformStateChartStateOfferer");
        StatePtr stateCalcPath = addState<StateCalculatePath>("stateCalculatePath");
        StatePtr stateSuccess = addState<SuccessState>("stateSuccess");
        StatePtr stateFailure = addState<FailureState>("stateFailure");

        setInitState(stateCalcPath, ParameterMapping::createMapping()->mapFromParent("*","*"));
        //transitions
        addTransition<EvPathFound>(stateCalcPath, stateMoveToLocation, ParameterMapping::createMapping()
                                   ->mapFromParent("*","*")
                                   ->mapFromOutput("targetPositions")
                                   );
        addTransition<EvNoPathFound>(stateCalcPath, stateFailure);
        addTransition<EvSuccess>(stateMoveToLocation, stateSuccess);
        addTransition<EvFailure>(stateMoveToLocation, stateFailure);
    }

    void StatechartMovePlatformToLandmark::onEnter()
    {
        ARMARX_LOG << eVERBOSE << "Entering StatechartMovePlatformToLandmark";
    }

    void StatechartMovePlatformToLandmark::onExit()
    {
        ARMARX_LOG << eVERBOSE << "Exiting StatechartMovePlatformToLandmark...";
    }

    // ****************************************************************
    // Implementation of StateCalculatePath
    // ****************************************************************

    void StateCalculatePath::defineParameters()
    {
        addToInput("targetLandmark", VariantType::String, false);
        addToInput("landmarkNodes", VariantType::List(VariantType::FramedVector3), false);
        addToInput("landmarkEdges", VariantType::List(VariantType::String), false);
        addToOutput("targetPositions", VariantType::List(VariantType::Vector3), false);
    }

    void StateCalculatePath::onEnter()
    {
        ARMARX_LOG << eVERBOSE << "Entering StateCalculatePath" << flush;

        //read the nodes into a tag to node map
        SingleTypeVariantListPtr landmarkNodes = getInput<SingleTypeVariantList>("landmarkNodes");
        std::map<std::string, NodePtr> nameToNodeMap;
        for (int i = 0; i < landmarkNodes->getSize(); i++) {
            FramedVector3Ptr fv = landmarkNodes->getVariant(i)->get<FramedVector3>();
            NodePtr n = NodePtr(new Node());
            n->name = fv->frame;
            n->framedPos = fv;
            nameToNodeMap[fv->frame] = n;
        }

        ARMARX_LOG << eVERBOSE << "StateCalculatePath:onEnter(): Retrieving edges" << flush;
        //process the edges by adding the nodes to each other's successors
        SingleTypeVariantListPtr landmarkEdges = getInput<SingleTypeVariantList>("landmarkEdges");
        for (int i = 0; i < landmarkEdges->getSize(); i++) {
            std::string edge = landmarkEdges->getVariant(i)->getString();
            std::vector<std::string> edgeNodes;
            boost::split(edgeNodes, edge, boost::is_any_of(";"), boost::token_compress_on);
            NodePtr left = nameToNodeMap[edgeNodes[0]];
            NodePtr right = nameToNodeMap[edgeNodes[1]];
            left->successors.push_back(right);
            right->successors.push_back(left);
        }

        ARMARX_LOG << eVERBOSE << "StateCalculatePath:onEnter(): Getting current pose from PlatformContext" << flush;
        PlatformContext* context = getContext<PlatformContext>();
        ChannelRefPtr poseRef = context->getChannelRef(context->getPlatformUnitObserverName(), "platformPose");
        const float platformPositionX = context->getDatafieldRef(poseRef, "positionX")->getDataField()->getFloat();
        const float platformPositionY = context->getDatafieldRef(poseRef, "positionY")->getDataField()->getFloat();

        //has trouble resolving the observer?
//        const float platformPositionX = context->getDataFromObserver(DataFieldIdentifierPtr(new DataFieldIdentifier("platformPose.positionX")))->getFloat();
//        const float platformPositionY = context->getDataFromObserver(DataFieldIdentifierPtr(new DataFieldIdentifier("platformPose.positionY")))->getFloat();

        std::map<std::string, NodePtr>::iterator it = nameToNodeMap.begin();
        if (it == nameToNodeMap.end()) {
            ARMARX_LOG << eWARN << "No nodes provided";
            sendEvent<EvNoPathFound>();
            return;
        }
        ARMARX_LOG << eVERBOSE << "StateCalculatePath:onEnter(): Finding closest point in graph" << flush;
        NodePtr closest = it->second;
        float minDist = getDist(platformPositionX, platformPositionY, closest->framedPos->x, closest->framedPos->y);
        it++;
        for (; it != nameToNodeMap.end(); it++) {
            const float dist = getDist(platformPositionX, platformPositionY, it->second->framedPos->x, it->second->framedPos->y);
            if (dist < minDist) {
                closest = it->second;
                minDist = dist;
            }
        }

        ARMARX_LOG << eVERBOSE << "StateCalculatePath:onEnter(): Starting point: " << closest->name << flush;

        std::string landmark = getInput<std::string>("targetLandmark");
        NodePtr goal = nameToNodeMap[landmark];
        if (!goal) {
            ARMARX_LOG << eWARN << "Target landmark doesn't exist in graph" << flush;
            sendEvent<EvNoPathFound>();
            return;
        }
        ARMARX_LOG << eVERBOSE << "StateCalculatePath:onEnter(): Looking for path" << flush;
        std::list<NodePtr> path = aStar(closest, goal);
        if (path.empty()) {
            ARMARX_LOG << eWARN << "No path found" << flush;
            sendEvent<EvNoPathFound>();
            return;
        }
        SingleTypeVariantList targetPositions(VariantType::Vector3);
        const float goalAlpha = path.back()->framedPos->z;
        for (std::list<NodePtr>::iterator it = path.begin(); it != path.end(); it++) {
            Vector3 v;
            v.x = (*it)->framedPos->x;
            v.y = (*it)->framedPos->y;
            v.z = goalAlpha;
            targetPositions.addVariant(v);
        }
        setOutput("targetPositions", targetPositions);
        sendEvent<EvPathFound>();
        ARMARX_LOG << eVERBOSE << "StateCalculatePath: Done onEnter()" << flush;
    }

    void StateCalculatePath::onExit()
    {
        ARMARX_LOG << eVERBOSE << "StateCalculatePath: Done onExit()";
    }

    //http://en.wikipedia.org/wiki/A*_search_algorithm
    std::list<StateCalculatePath::NodePtr> StateCalculatePath::aStar(StateCalculatePath::NodePtr start, StateCalculatePath::NodePtr goal) {
        std::list<NodePtr> path;

        std::vector<NodePtr> closedSet;
        std::vector<NodePtr> openSet;
        openSet.push_back(start);
        std::map<NodePtr, float> gScore;
        gScore[start] = 0;
        std::map<NodePtr, float> fScore;
        fScore[start] = gScore[start] + heuristic(start, goal);
        std::map<NodePtr, NodePtr> cameFrom;
        cameFrom[goal] = start; //in case start==goal

        while (!openSet.empty()) {
            float lowestScore = fScore[openSet[0]];
            std::vector<NodePtr>::iterator currentIT = openSet.begin();
            for (std::vector<NodePtr>::iterator it = openSet.begin() + 1; it != openSet.end(); it++) {
                if (fScore[*it] < lowestScore) {
                    lowestScore = fScore[*it];
                    currentIT = it;
                }
            }
            NodePtr current = *currentIT;
            if (current == goal) {
                NodePtr cameFromNode = goal;
                while (cameFromNode != start) {
                    path.push_front(cameFromNode);
                    cameFromNode = cameFrom[cameFromNode];
                }
                path.push_front(start);
                break;
            }
            openSet.erase(currentIT);
            closedSet.push_back(current);

            for (size_t i = 0; i < current->successors.size(); i++) {
                NodePtr neighbor = current->successors[i];
                if (std::find(closedSet.begin(), closedSet.end(), neighbor) != closedSet.end()) continue;

                float tentativeGScore = gScore[current] + heuristic(current, neighbor);
                bool notInOS = std::find(openSet.begin(), openSet.end(), neighbor) == openSet.end();
                if (notInOS || tentativeGScore < gScore[neighbor]) {
                    cameFrom[neighbor] = current;
                    gScore[neighbor] = tentativeGScore;
                    fScore[neighbor] = tentativeGScore + heuristic(neighbor, goal);
                    if (notInOS) {
                        openSet.push_back(neighbor);
                    }
                }
            }
        }
        return path;
    }
}