TopoMapPlanner.h

Go to the documentation of this file.

/*
Copyright (c) 2013,
    Neuroinformatics and Cognitive Robotics Labs at TU Ilmenau, Germany

All rights reserved.

Copying, resale, or redistribution, with or without modification, is strictly
prohibited.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES,
EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/

#pragma once

#include <topomap/TopoMap.h>

namespace viros { namespace topomap {


class TopoMapPlanner
{

public:

    TopoMapPlanner();

public:

    void setLeafCostFunction(boost::function<void(NodePtr)> fn);


    void precomputeCosts(TopoMapPtr map);

    void propagateUpdatedCosts(NodePtr node);

    void setConnectivityViolated(NodePtr node, bool isViolated=true);

    bool isConnectivityViolated(NodePtr node) const;

    void clearAllConnecitvityViolations(TopoMapPtr map);

    void setAdditionalLinkCosts(const std::string& fromGWName, const std::string& toGWName, double costs);

    void resetAllAdditionalLinkCosts();

private:

    void precomputeCosts(NodePtr node);

    void computeIntraLinkCosts(NodePtr node);

public:
    struct GatewayCostPair {
        GatewayNodePtr node;
        double costs;
        int hops;

        GatewayCostPair() {}
        GatewayCostPair(GatewayNodePtr n, double c, int h=0) :
            node(n), costs(c), hops(h) {}

        bool operator<(const GatewayCostPair& other) const {
            return costs>other.costs;
            // those with smaller costs first: hence we need to swap the
            // cmp operator as priority_queue keeps the "largest" element on top
            // of the queue
        }
    };
    typedef std::priority_queue<GatewayCostPair> OpenList;

    struct CostInfo {
        double costs;
        int hops;

        CostInfo() {}
        CostInfo(double c, int h) : costs(c), hops(h) {}

    };
    typedef std::map<GatewayNodePtr,CostInfo> VisitedList;

    void prepareWavefront(OpenList& Q, const VisitedList& V);
    bool propagateWavefront(OpenList& Q, VisitedList& V, const NodeSet& localizedNodes = NodeSet(), const NodeSet& ancestors = NodeSet());

    static double infinity() {
        return std::numeric_limits<double>::max();
    }

public:

    struct DbgHopInfo {
        GatewayNodePtr gw;
        double costs;
        int hops;

        DbgHopInfo() {}
        DbgHopInfo(GatewayNodePtr gw, double costs, int hops) : gw(gw), costs(costs), hops(hops) {}
    };

    std::list<DbgHopInfo> dbgHops;
    bool dbgEnabled;

private:

    boost::function<void(NodePtr)> mLeafCostFunction;

    typedef std::map<std::string, double> AdditionalCostMap;
    AdditionalCostMap mAdditionalLinkCosts;
};



} } // namespace