AbstractObjectiveBasedPlanner.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_ABSTRACTOBJECTIVEBASEDPLANNER_H_
#define _MIRA_PILOT_ABSTRACTOBJECTIVEBASEDPLANNER_H_

#include <serialization/adapters/std/list>

#include <robot/UnicycleBasedRobotModel.h>

#include <pilot/AbstractPlanner.h>
#include <pilot/Objective.h>

// for debugging
#include <image/Img.h>

#include <fw/Channel.h>


namespace mira { namespace pilot {


class AbstractObjectiveBasedPlanner : public AbstractPlanner
{
    MIRA_ABSTRACT_OBJECT(AbstractObjectiveBasedPlanner)
public:

    template<typename Reflector>
    void reflect(Reflector& r)
    {
        r.property("PlanningHorizon", mPlanningHorizon,
                   "The planning horizon [ms]. It should be equal or"
                   "larger than the Pilots cycle time", Duration::milliseconds(250));
        r.property("MinLookAhead", mMinLookAhead,
                   "The min. lookahead time [ms]",Duration::seconds(2));
        r.property("MaxLookAhead", mMaxLookAhead,
                   "The max. lookahead time [ms]",Duration::seconds(4));
        r.property("TrajectorySamples", mTrajectorySamples,
                   "Number of samples along the predicted trajectory (default: 10)", 10);
        r.property("MaxNoCommandTime", mMaxNoCommandTime,
                   "How long is it allowed to have no valid velocity command", Duration::seconds(10));
        r.property("Objectives", mObjectives, "The objectives");

        r.method("enableObjective", &AbstractObjectiveBasedPlanner::enableObjective, this,
                 "Enable/Disable an objective which is specified by its class name",
                 "name", "The objective's name", "PathObjective",
                 "enable", "true/false to enable/disable", true);
    }

protected:

    virtual void initialize();
    virtual void resume();
    virtual void pause();

public: // implements AbstractPlanner

    virtual void setTask(boost::shared_ptr<navigation::Task> task);

    virtual bool reachedTasks();

    virtual bool satisfied();

    virtual void resetDissatisfaction();

    virtual std::list<DissatisfactionReason> getDissatisfactionReasons();

    // must be implemented in derived classes
    // virtual Velocity2 planMotion() = 0;

    virtual void update();

    virtual void plan(Duration plantime);

    virtual void updateRobotModel();

public:
    

    void addObjective(boost::shared_ptr<Objective> o, float weight);

    boost::shared_ptr<Objective> getObjective(const std::string& name);


    //boost::shared_ptr<IObjective> getObjective(const std::string& name);


    void enableObjective(const std::string& name, bool enable);

    robot::UnicycleBasedRobotModelPtr getRobotModel() { return mRobotModel; }

protected:

    struct ObjectiveInfo
    {
        ObjectiveInfo() : weight(0.0f), disabled(false) {}
        ObjectiveInfo(boost::shared_ptr<Objective> o, float w) :
            objective(o), weight(w), disabled(false) {}

        template<typename Reflector>
        void reflect(Reflector& r)
        {
            if(objective)
                r.itemName(objective->getItemName());
            r.roproperty("Has Work", getter<bool>(&ObjectiveInfo::hasWork, this),
                         "Do the objective has some work");
            r.roproperty("Reached Task", getter<bool>(&ObjectiveInfo::reachedTask, this),
                         "Does the objective has reached its task?");
            r.roproperty("Satisfaction", getter<Objective::Satisfaction>(&ObjectiveInfo::getSatisfaction, this),
                         "Satisfaction of this objective", PropertyHints::enumeration("INTOLERABLE;DISSATISFIED;DONT_CARE;SATISFIED"));

            r.property("Disabled",  disabled,  "If true, the objective is disabled",false);
            r.property("Weight",    weight,    "The weight for this objective",1.0f);
            r.property("Objective", objective, "The objective");
        }

        Objective::Satisfaction getSatisfaction() {
            return objective ? objective->getSatisfaction() : Objective::DONT_CARE;
        }

        void resetDissatisfaction() {
            if (objective)
                objective->resetDissatisfaction();
        }

        bool hasWork() const {
            return objective ? objective->hasWork() : false;
        }

        bool reachedTask() const {
            return objective ? objective->reachedTask() : true;
        }

        bool isActive() const { return !disabled && objective->hasWork(); }

        boost::shared_ptr<Objective> objective;
        float weight;
        bool disabled;
    };

    typedef std::list<ObjectiveInfo> Objectives;
    Objectives mObjectives;

protected:

    Duration mPlanningHorizon;
    Duration mMinLookAhead;
    Duration mMaxLookAhead;
    int mTrajectorySamples;

    Duration mMaxNoCommandTime;
    Time mLastCommand;

    robot::UnicycleBasedRobotModelPtr mRobotModel;

protected:

    void prepareVote(float dt);

    void informDecision(const robot::PoseVelocityTrajectory& chosen, float dt);

    robot::PoseVelocityTrajectory generateTrajectory(Velocity2 v,
                                         const Velocity2& targetv,
                                         float lookAheadTime);

    virtual void updateRobotModel(bool updateObjectives);
};

}}

#endif