MetricCostmapTopoMapPlanner.h

Go to the documentation of this file.

/*
 * Copyright (C) by
 *   Neuroinformatics and Cognitive Robotics Labs (NICR) at TU Ilmenau, GERMANY
 * All rights reserved.
 *
 * Redistribution and modification of this code is strictly prohibited.
 *
 * IN NO EVENT SHALL "NICR" BE LIABLE TO ANY PARTY FOR DIRECT,
 * INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES ARISING OUT OF
 * THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF "NICR" HAS BEEN 
 * ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 * "NICR" SPECIFICALLY DISCLAIM ANY WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 
 * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 
 * PURPOSE. THE SOFTWARE PROVIDED HEREUNDER IS ON AN "AS IS" BASIS, AND 
 * "NICR" HAVE NO OBLIGATION TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, 
 * ENHANCEMENTS OR MODIFICATIONS.
 */

#pragma once

#include <fw/Framework.h>

#include <model/Marker.h>

#include <transform/Pose.h>
#include <maps/CostMap.h>
#include <pilot/AbstractMetricPlanner.h>

#include <topomap/TopoMap.h>
#include <topomap/TopoMapPlanner.h>

#include <topomap/ActionAttribute.h>
#include <topomap/MapAttributes.h>


namespace viros { namespace topomap {


class MetricCostmapTopoMapPlanner
{
    typedef std::vector<std::tuple<GatewayNodePtr, GatewayNodePtr, double>> GatewayHops;

public:

    MetricCostmapTopoMapPlanner();

    template<typename Reflector>
    void reflect(Reflector& r)
    {
        mira::serialization::Array<PlannerContext> plannerCtx(mPlannerCtx,3);
        r.property("PlannerCtx", plannerCtx, "");
        r.property("GatewayWidth", mGatewayWidth, "goal width of gateways in cells", 2);
        r.property("EnableReplanning", mEnableReplanning, "Specifies whether global replanning within the topological structure is enabled.", true);
        r.property("TolerableDetourBeforeReplan", mTolerableDetourBeforeReplan,
                   "The allowed detour in meter caused by significant changes in the environment."
                   "If the detour becomes larger than this threshold, a global replanning"
                   "within the topological structure is triggered after the time span specified"
                   "below. (default 10.0 m)", 10.0f);
        r.property("TimeUntilReplan", mTimeUntilReplan,
                   "Time duration, until replanning is triggered when the current path "
                   "became sub-optimal after significant changes in the environment.",
                   mira::Duration::seconds(2));
        r.property("MaxPlanningTime", mMaxPlanningTime,
                   "How long should we plan until we decide that we can not plan a path",
                   mira::Duration::seconds(30));
        r.property("AllowGoalsWithInfiniteCosts", mAllowGoalsWithInfiniteCosts,
                   "If true planning to goals with infinite costs is allowed. "
                   "This is necessary for moving goals (e.g. follow objective).",
                   false);
        r.member("CachePath", mCachePath,
                 "The path for caching preplanned maps to enable faster bootup. "
                 "The directory will be created if it does not exist.", "cache");
    }

public:

    struct PlannerContext {
        mira::pilot::AbstractMetricPlannerPtr planner;
        mira::maps::CostMap map;
        mira::maps::DirtyRegions dirty;
        Rect2i rect;
        NodePtr node;
        GatewayNodePtr gw;
        bool active;
        boost::optional<double> lastCosts;
        PlannerContext() : active(false) {}

        bool finalApproach() const { return !gw; }

        void reset() {
            node.reset();
            gw.reset();
            active = false;
            lastCosts.reset();
        }

        template<typename Reflector>
        void reflect(Reflector& r)
        {
            r.property("Planner", planner, "");
            r.roproperty("Active", active, "");
        }
    };

    typedef PlannerContext* PlannerContextPtr;

/*
    struct GoalNodeInfo
    {
        GoalNodeInfo() {}
        GoalNodeInfo(NodePtr node, const std::string& frame,
                     const Point2i& goalcell, const Point2f& goalinmap) :
          node(node), frame(frame), goalcell(goalcell), goalinmap(goalinmap) {}
        NodePtr node;
        std::string frame; // frame of the node's map
        Point2i goalcell;
        Point2f goalinmap;
    };
    typedef std::list<GoalNodeInfo> GoalNodeInfoList;
*/
    struct NodePosition
    {
        NodePosition() {}
        NodePosition(NodePtr node, const std::string& frame, const Point2f& pos) :
          node(node), frame(frame), pos(pos) {}
        NodePtr node;
        std::string frame; // frame of the node's map
        Point2f pos;
    };
    typedef std::list<NodePosition> NodePositionList;

    class IEventListener
    {
    public:

        virtual ~IEventListener() {}

        virtual void onNodeChanged(NodePtr currentNode, GatewayNodePtr usedGateway, GatewayNodePtr nextGateway) = 0;
    };

    struct NextSubGoal {
        NodePtr n;
        GatewayNodePtr gn;
        double costs;
        double remainingcosts; // remaining costs to final goal when subgoal is reached
        int hops;

        template<typename Reflector>
        void reflect(Reflector& r)
        {
            r.member("Node", getter(&NextSubGoal::getNodeName, this), setter(&NextSubGoal::dummySetter, this), "");
            r.member("GatewayNode", getter(&NextSubGoal::getGatewayNodeName, this), setter(&NextSubGoal::dummySetter, this), "");
            r.member("Costs", costs, "");
            r.member("RemainingCosts", remainingcosts, "");
            r.member("Hops", hops, "");
        }

        NextSubGoal() :
            costs(std::numeric_limits<double>::max()),
            remainingcosts(std::numeric_limits<double>::max()),
            hops(std::numeric_limits<int>::max()) {}

        void reset()
        {
            n.reset();
            gn.reset();
            costs = std::numeric_limits<double>::max();
            remainingcosts = std::numeric_limits<double>::max();
            hops = std::numeric_limits<int>::max();
        }

        std::string getNodeName() {
            return n ? n->getFullName() : "<null>";
        }
        std::string getGatewayNodeName() {
            return gn ? gn->getFullName() : "<null>";
        }
        void dummySetter(std::string s) {}

    };
    static void printNextSubGoal(const NextSubGoal& nsg,
                                 const std::string& name);

public:

    bool isInFinalApproach() const
    {
        return mCurrentPlanner->active && mCurrentPlanner->finalApproach();
    }

    bool isActive() const;
    double getValue(const Point2f& worldPos) const;
    double getValueFiltered(const Point2f& worldPos) const;

    mira::pilot::AbstractMetricPlanner::Carrot getCarrot(const Point2f& pos, int count, float length = 1e10f) const;

    void resetCostMaps();

    void setEventListener(IEventListener* listener) {
        mEventListener = listener;
    }

    // --- necessary for PilotPathToMovingGoalTopoMap ---
    PlannerContextPtr getCurrentPlanner()
    {
        return mCurrentPlanner;
    }
    PlannerContextPtr getNextPlanner()
    {
        return mNextPlanner;
    }
    // --------------------------------------------------

    // debugging:
    mira::maps::GridMap<double> getNavFunction(bool ignoreDoubleBuffer = false);
    mira::maps::GridMap<uint8,3> dumpNavStatus(bool actualMap = false,
                                               bool costsInGoalRegion = true);

public:

    void initialize(TopoMapPtr map, Authority* authority);

    typedef boost::function<void(mira::pilot::AbstractMetricPlannerPtr, bool)> UpdateGoalFn;
    void setGoal(const NodePositionList& goalNodes, UpdateGoalFn updateGoalFn);

    void update(const maps::CostMap& map);

    void update(const maps::CostMap& map, const maps::DirtyRegions& dirty);

    bool havePlan() const;

    void plan(const mira::maps::CostMap& curCostmap,
              const Point2f& robotPos,
              const NodePositionList& localizedNodes,
              Duration plantime);

    void clearGoal();

    void setAdditionalLinkCosts(const std::string& fromGWName, const std::string& toGWName, double costs) {
        mTopoPlanner.setAdditionalLinkCosts(fromGWName, toGWName, costs);
    }
    void resetAllAdditionalLinkCosts() {
        mTopoPlanner.resetAllAdditionalLinkCosts();
    }

    void setDebugMarkerChannel(Channel<mira::model::Marker> c) {
        mDbgMarkerChannel = c;
    }
    void setDebugNextSubGoalChannel(Channel<NextSubGoal> c) {
        mDbgNextSubGoalChannel = c;
    }

private:

    void drawGateway(mira::model::Marker& marker, std::tuple<GatewayNodePtr, GatewayNodePtr, double> gw);
    bool traverseLinks(GatewayHops& gatewayHops, NodeSet& localizedNodes, GatewayNodePtr node, int hops);

    void planCurrentPlanner();
    bool updateGoalFunction(PlannerContextPtr plannerCtx, bool initial = false);

    void handleReplanningLastCostBased(const mira::maps::CostMap& curCostmap);
    double getCurrentPlannerCosts(const Point2f& p, bool ignoreDoubleBuffer);

    void topologicalPlanning(const NodePositionList& localizedNodes);
    void prepareVisitedList();

    void selectSubGoal(const NodePositionList& localizedNodes);
    void determineLocalizedNodesWithGoal(const NodePositionList& localizedNodes);

    void determineBestTopoSubGoals(const NodePositionList& localizedNodes,
                                   NextSubGoal& oBestSubGoal,
                                   NextSubGoal& oSecondBestSubGoal);
    void determineBestTopoSubGoals(const GatewayNodeSet& gatewayNodes,
                                   const Point2f& robotPos,
                                   NextSubGoal& oBestSubGoal,
                                   NextSubGoal& oSecondBestSubGoal);
    bool isBetterThanSubGoal(double costs, int hops,
                             const NextSubGoal& subGoal);
    void assignToSubGoal(GatewayNodePtr gn,
                         double costs,
                         double remainingcosts,
                         int hops,
                         NextSubGoal& oSubGoal);

    void handleNextPlanner();
    void prepareNextPlanner();

    bool isInGoalNode() const;
    bool isGoalNodeWithLocalization(NodePtr n) const;
    const NodePosition* getNodePosition(NodePtr n, const NodePositionList& nodePosList) const;

private:

    void preparePlannerContext(PlannerContextPtr ctx, const mira::maps::GridMap<double>& costmap, const std::string& frameID, RegionAttributePtr regionattr);
    void setGatewayGoal(PlannerContextPtr ctx, int mapHeight, GatewayPtr gw);

    void computeLeafCosts(NodePtr node);

    void updateNodeCosts(NodePtr node, const Stamped<mira::maps::GridMap<double>>& costmap);
    void computeNodeCosts(NodePtr node, const Stamped<mira::maps::GridMap<double>>& costmap, bool useCache);

    struct ResetCostMapsVisitor;
    friend struct ResetCostMapsVisitor;

    void readNavFuncFromCache(const std::string& gatewayName,
                              const Stamped<mira::maps::GridMap<double>>& costmap,
                              const RegionAttributePtr& regionPtr,
                              maps::CostMap& navFn,
                              std::string& md5sum,
                              std::string& cacheFileName);
    void writeNavFuncToCache(const maps::CostMap& navFn,
                             const std::string& md5sum,
                             std::string& cacheFileName);

private:

    Authority* mAuthority;
    UpdateGoalFn mUpdateGoalFn;
    IEventListener* mEventListener;

    TopoMapPtr mTopoMap;
    int mGatewayWidth;

    TopoMapPlanner mTopoPlanner;
    PlannerContext mPlannerCtx[3];
    PlannerContextPtr mCurrentPlanner;
    PlannerContextPtr mNextPlanner;
    PlannerContextPtr mAuxPlanner;

    NodePositionList mGoalNodes;

    Point2f mCurrentRobotPosition;
    Duration mCurrentPlantime;

    NodePositionList mLocalizedNodesWithGoal;
    TopoMapPlanner::VisitedList mVisitedList;
    NextSubGoal mCurrentSubGoal;
    bool mAllowGoalsWithInfiniteCosts;

    std::string dbgTimeNow;

    bool mEnableReplanning;
    float mTolerableDetourBeforeReplan;
    Duration mTimeUntilReplan;
    Time mReplanTimeoutStart;

    Duration mMaxPlanningTime;
    Time mLastValidPlanningTime;

    Path mCachePath;
    double mMaxCosts;

    Channel<mira::model::Marker> mDbgMarkerChannel;
    Channel<NextSubGoal> mDbgNextSubGoalChannel;
};



} } // namespace