Objective.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_OBJECTIVE_H_
#define _MIRA_PILOT_OBJECTIVE_H_

#include <factory/Factory.h>

#include <transform/Velocity.h>

#include <fw/Framework.h>

#include <navigation/Task.h>

#include <pilot/DissatisfactionReasons.h>
#include <pilot/Module.h>
#include <robot/Trajectory.h>

namespace mira { namespace pilot {


class Task; // forward decl
class Pilot;

class Objective : public Module
{
    MIRA_OBJECT(Objective);
public:

    enum Permit
    {
        ADMISSIBLE = 0,
        NOT_ADMISSIBLE = 1,
        NOT_REACHABLE = 2
    };

    enum Satisfaction
    {
        INTOLERABLE = 0,
        DISSATISFIED = 1,
        DONT_CARE = 2,
        SATISFIED = 3
    };

    struct Vote
    {
        static const Vote UNREACHABLE;
        static const Vote DENY;
        static const Vote ALLOW;

        Vote() :
            permit(ADMISSIBLE),
            costs(0.0) {}

        Vote(Permit p, double c) :
            permit(p),
            costs(c) {}

        template<typename Reflector>
        void reflect(Reflector& r)
        {
            r.member("Permit", permit, "");
            r.member("Costs", costs, "");
        }

        bool operator<(const Vote& rhs) {
            if(permit==ADMISSIBLE && rhs.permit!=ADMISSIBLE)
                return true;
            if(permit==ADMISSIBLE && rhs.permit==ADMISSIBLE)
                return costs < rhs.costs;
            return false;
        }

        Permit permit;
        double costs;
    };

public:

    Objective() :
        mHaveWork(false),
        mSatisfaction(DONT_CARE),
        mDissatisfactionReason(DissatisfactionReason::NO_REASON) {}
    virtual ~Objective() {}

    template<typename Reflector>
    void reflect(Reflector& r)
    {
        MIRA_REFLECT_BASE(r, Module);
    }

public:

    bool hasWork() const { return mHaveWork; }

    virtual Satisfaction getSatisfaction() {
        return mSatisfaction;
    }

    virtual void resetDissatisfaction() {
        mSatisfaction = DONT_CARE;
        mDissatisfactionReason = DissatisfactionReason::NO_REASON;
    }

    virtual DissatisfactionReason getDissatisfactionReason() {
        return mDissatisfactionReason;
    }

    virtual void prepareVote(float lookAheadTime) {};

    virtual Vote vote(const Velocity2& v, float lookAheadTime) {
        MIRA_THROW(XNotImplemented, "You need to implement ONE of the vote() methods");
    }

    virtual Vote vote(const Velocity2& v, const robot::PoseVelocityTrajectory& trajectory, float lookAheadTime) {
        return vote(v,lookAheadTime);
    }

    virtual Vote vote(const robot::PoseVelocityTrajectory& trajectory, float lookAheadTime) {
        assert(!trajectory.empty());
        return vote(trajectory.back().v, trajectory, lookAheadTime);
    }

    virtual void informDecision(const Velocity2& chosen, float dt) {}

    virtual void informDecision(const robot::PoseVelocityTrajectory& trajectory, float dt) {
        assert(!trajectory.empty());
        informDecision(trajectory.back().v, dt);
    };

    virtual void setTask(navigation::TaskPtr task) {};

    virtual bool reachedTask() { return true; }

    virtual void update() {}

    virtual void plan(Duration plantime) {}

    virtual void updateRobotModel() {}

protected:

    void setHaveWork(bool haveWork) {
        mHaveWork = haveWork;
    }

    void setSatisfaction(Satisfaction satisfaction, DissatisfactionReason reason=DissatisfactionReason::NO_REASON) {
        mSatisfaction = satisfaction;
        mDissatisfactionReason = reason;
    }

private:

    bool mHaveWork;
    Satisfaction mSatisfaction;
    DissatisfactionReason mDissatisfactionReason;
};



}} // namespace

#endif