MetricPlannerBase.h

Go to the documentation of this file.

/*
 * Copyright (C) by
 *   MetraLabs GmbH (MLAB), GERMANY
 *  and
 *   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 "MLAB" OR "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 "MLAB" OR
 * "NICR" HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 * "MLAB" AND "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 "MLAB" AND "NICR" HAVE NO OBLIGATION TO
 * PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS OR MODIFICATIONS.
 */

#ifndef _MIRA_PILOT_METRICPLANNERBASE_H_
#define _MIRA_PILOT_METRICPLANNERBASE_H_

#include <pilot/AbstractMetricPlanner.h>

namespace mira { namespace pilot {


inline double plannerDefaultInfinity()
{
#ifdef WIN32
    // Under some circumstances, Visual C++ does not seem to like the
    // "proper" way, but sometimes it does. Anyway, this workaround
    // should do the trick. Note that initializing a non-integer static
    // const is not supposed to be standard either...
    return DBL_MAX;
#else
    return std::numeric_limits<double>::max();
#endif
}

class MetricPlannerBase : public AbstractMetricPlanner
{
    MIRA_ABSTRACT_OBJECT(MetricPlannerBase)
public:

    MetricPlannerBase(bool useDoubleBuffer = false,
                      bool persistentDoubleBuffer = false);
    virtual ~MetricPlannerBase() {}

    template <typename Reflector>
    void reflect(Reflector& r)
    {
        MIRA_REFLECT_BASE(r, mira::pilot::AbstractMetricPlanner);

        r.member("UseDoubleBuffer",
                 getter(&MetricPlannerBase::getUseDoubleBuffer, this),
                 setter(&MetricPlannerBase::setUseDoubleBuffer, this),
                 "Use double buffering for the planner map", false, REFLECT_CTRLFLAG_MEMBER_AS_ROPROPERTY);
        r.property("PersistentDoubleBuffer",
                   getter(&MetricPlannerBase::getPersistentDoubleBuffer, this),
                   setter(&MetricPlannerBase::setPersistentDoubleBuffer, this),
                   "Do NOT invalidate double buffer when a new goal is set",
                   false);
        r.property("CarrotFromDoubleBuffer", mGetCarrotFromDoubleBuffer,
                   "Whether getCarrot should be performed on the DoubleBuffer, if active and valid.",
                   false);
        r.property("MaxPlanningTime", mMaxPlanningTime,
                   "How long should we plan until we decide that we can not plan a path",
                   Duration::seconds(30));
        r.property("PathLostTimeout", mPathLostTimeout,
                   "If the planner has no path since the specified time (but had one before) XPathLost is thrown",
                   Duration::seconds(10));
        r.property("Navfscale", mNavfscale, "The color scaling of the path costs during visualization", 0.02);
    }

public:

    virtual void setOnewayMap(const maps::OccupancyGrid& onewayMap) {
        MIRA_THROW(XNotImplemented, "MetricPlannerBase::setOnewayMap() not overridden by concrete subclass!")
    }

    void setUseDoubleBuffer(bool useDoubleBuffer) {
        mUseDoubleBuffer = useDoubleBuffer;
        if (!useDoubleBuffer)
            mDoubleBufferValid = false;
    }

    void setPersistentDoubleBuffer(bool persistentDoubleBuffer) {
        mPersistentDoubleBuffer = persistentDoubleBuffer;
    }

public:

    virtual Point2f getGradient(const Point2f& worldPos) const;

    Point2f getGradientMap(const Point2i& mapPos) const;

    virtual Point2f getCurrentGradientMap(const Point2i& mapPos) const;

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

    virtual double infinity() const;

    double getValue(const Point2f& worldPos, bool ignoreDoubleBuffer = false) const;

    double getValueMap(const Point2i& mapPos, bool ignoreDoubleBuffer = false) const;

    virtual double getCurrentValueMap(const Point2i& mapPos) const = 0;

    virtual double getValueFiltered(const Point2f& worldPos, bool ignoreDoubleBuffer = false) const;

    virtual Carrot getCarrot(const Point2f& pos, int maxCount, float maxDistance = 1e10f) const;

    virtual Carrot getCurrentCarrot(const Point2f& pos, int maxCount, float maxDistance = 1e10f) const = 0;

    virtual Carrot getDoubleBufferCarrot(const Point2f& pos, int maxCount, float maxDistance = 1e10f) const;

    virtual Pose2 getCurrentGoal(const Point2f& pos) const;

public:

    Point2i world2map(const Point2f& p) const {
        if(mCostMap.empty())
            MIRA_THROW(XLogical, "No costmap created yet. Call update() first.")
        return mCostMap.world2map(p);
    }

    Point2f map2world(const Point2i& p) const {
        if (mCostMap.empty())
            MIRA_THROW(XLogical, "No costmap created yet. Call update() first.")
        return mCostMap.map2world(p);
    }

    float getCellSize() const {
        if (mCostMap.empty())
            MIRA_THROW(XLogical, "No costmap created yet. Call update() first.")
        return mCostMap.getCellSize();
    }

    Rect2f getRegion() const {
        if (mCostMap.empty())
            MIRA_THROW(XLogical, "No costmap created yet. Call update() first.")
        return mCostMap.getRegion();
    }

    Rect2i getMapRegion() const {
        if (mCostMap.empty())
            MIRA_THROW(XLogical, "No costmap created yet. Call update() first.")
        return mCostMap.getMapRegion();
    }

    bool getUseDoubleBuffer() const {
        return mUseDoubleBuffer;
    }

    bool getPersistentDoubleBuffer() const {
        return mPersistentDoubleBuffer;
    }

    Duration getAccumulatedPlanningTime() const {
        return mAccumulatedPlanningTime;
    }

public:

    void setGoal(const Point2f& goal, float radius,
                 bool clearDoubleBuffer = true, bool prepareNew = true);

    void setGoal(const Polygon2i& polygon, int radius = 1,
                 bool clearDoubleBuffer = true, bool prepareNew = true);

    void setGoal(const Polygon2f& polygon, float radius = 0.1f,
                 bool clearDoubleBuffer = true, bool prepareNew = true);

    void setGoal(const Polygon2i& polygon, int radius, CostFn& costFn,
                 bool clearDoubleBuffer = true, bool prepareNew = true);

    void setGoal(const Polygon2f& polygon, float radius, CostFn& costFn,
                 bool clearDoubleBuffer = true, bool prepareNew = true);

    float defaultCostFn(int x, int y, float r);

protected:

    virtual bool isValid() const;

    virtual void reset();

    virtual void resetDoubleBuffer(bool createNew);

    virtual void copyDoubleBuffer() = 0;

    void prepareNewGoal(bool clearDoubleBuffer);

    virtual void removeGoals() = 0;

    virtual void addGoal(const Point2i& p, float cost) = 0;

    virtual void freeCell(const Point2i& p) = 0;

    virtual void dirtyCell(const Point2i& p, double value) = 0;

    AbstractMetricPlanner::Carrot getCarrotFromMap(const maps::CostMap& map, const Point2f& pos,
                                                   int maxCount, float maxDistance) const;

    int computeStableGradientFromMap(const maps::CostMap& map, int ix, int iy, double baseVal, double stepsize,
                                     double& dx, double& dy, double& gx, double& gy) const;

public:

    void plan(const Point2f& pos, Duration plantime);

    bool havePlan() const { return mHadValidPlan; }

    bool havePlanSinceUpdate() const { return mHadValidPlanSinceUpdate; }

    Time lastValidPlan() const { return mLastValidPlan; }

    void planTime(Duration plantime);

    void planSteps(int steps);

    void resetPlanningTimeout();

    maps::CostMap getNavFunction(bool ignoreDoubleBuffer = false) const;

    virtual maps::CostMap getCurrentNavFunction() const = 0;

public:

    maps::GridMap<uint8,3> dumpNavStatus(bool useDoubleBuffer = false,
                                         bool costsInGoalRegion = true);

    maps::GridMap<uint8,3> showNavFunctionNearGoal();

public:

    Status getStatus(const Point2f& p) const;

    virtual Status getStatusMap(const Point2i& p) const;

protected:

    virtual Img8U1 getStatusField() const;

    double filteredCorner(double self, double xnb,
                          double ynb, double diagnb) const;

protected:

    Size2i mSize;
    maps::CostMap mCostMap;

    // the buffer is used to copy complete paths when after the planning wave
    // reached the robot
    maps::CostMap mDoubleBuffer;   // buffer for nav function

    // last point of time, where planning was started or a valid plan existed
    // internally used as timeout when no valid plan exists
    Time mLastPlan;

    // last point of time, where a valid plan existed
    // equals mLastPlan if havePlan()
    // is older than mLastPlan if !havePlan()
    // externally provided, because this is the age of the double buffer
    Time mLastValidPlan;

    // How much time has been spent planning until we reach the goal
    Duration mAccumulatedPlanningTime;

    Duration mMaxPlanningTime;
    Duration mPathLostTimeout;
    bool mHadValidPlan;
    bool mHadValidPlanSinceUpdate;
    bool mPathLostReported;

    float mNavfscale;

    // in order to achieve subcell nav function resolution at goal proximity
    // (enabling to reach the goal point and not just the goal cell):
    // nav function is overlaid with pyramidal cost function in goal point's
    // 3x3 cell neighborhood (pyramid base = 3x3 cell area,
    // pyramid tip = goal point)
    bool mHasPointGoal;
    Point2f mPointGoal;
    Point2i mPointGoalCell;
    Eigen::VectorXf mPointGoalPyrBorder;
    Eigen::VectorXf mPointGoalPyrSlope;

    bool mUseDoubleBuffer;
    bool mGetCarrotFromDoubleBuffer;

    // To ensure, that we only access the double buffer when the double buffer
    // contains valid data, this flag will be set to true if mUseDoubleBuffer
    // is true AND the function copyDoubleBuffer was called. Otherwise, the
    // double buffer might be (still) invalid or infinity and therefore will
    // not be accessed.
    bool mDoubleBufferValid;

    // Usually, the double buffer should be invalidated when setting a new goal.
    // However, for a continuously moving target, it is reasonable to keep the
    // double buffer, in order to follow the path to the previous goal position
    // while planning to the new goal. This can be achieved with this
    // flag.
    bool mPersistentDoubleBuffer;
};


}}

#endif