AbstractDynamicWindow.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_ABSTRACTDYNAMICWINDOW_H_
#define _MIRA_PILOT_ABSTRACTDYNAMICWINDOW_H_

#include <serialization/adapters/std/list>

#include <pilot/AbstractObjectiveBasedPlanner.h>
#include <pilot/Objective.h>
#include <pilot/TrajectoryFamily.h>

#include <fw/Channel.h>


namespace mira { namespace pilot {


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

    AbstractDynamicWindow();

protected:

    virtual void initialize();

public: // implements AbstractPlanner

    virtual Velocity2 planMotion();

public:

    struct Cell
    {
        Cell() :
            vote(Objective::Vote::ALLOW),
            naySayer(0) {}

        Cell(const Velocity2& v, const Velocity2& r) :
            vOff(v), radius(r),
            vote(Objective::Vote::ALLOW),
            naySayer(0) {}

        template<typename Reflector>
        void reflect(Reflector& r)
        {
            r.member("VelocityOffset", vOff, "");
            r.member("Radius", radius, "");
            r.member("Vote", vote, "");
            r.member("Votes", votes, "");
            r.member("NaySayer", naySayer, "");
        }

        // relative velocity offset to center of current window
        Velocity2 vOff;

        // the radius (size) of the cell
        Velocity2 radius;
        Objective::Vote vote; // the overall vote/cost for choosing this velocity
        std::map<uint32, Objective::Vote> votes; // The votes of all objectives sorted by their id

        int naySayer; // id of objective that said "NOT_ADMISSABLE"
    };

    struct BestAction {
        BestAction() {}
        BestAction(const Velocity2& iV, Objective::Vote iVote, const robot::PoseVelocityTrajectory& iTrajectory) :
            v(iV), vote(iVote), trajectory(iTrajectory) {}

        template<typename Reflector>
        void reflect(Reflector& r)
        {
            r.member("Velocity", v, "");
            r.member("Vote", vote, "");
        }

        Velocity2 v;
        Objective::Vote vote;
        robot::PoseVelocityTrajectory trajectory;
    };

    struct Config
    {
        template<typename Reflector>
        void reflect(Reflector& r)
        {
            r.member("BestAction", bestAction, "");
            r.member("Objectives", objectives, "");
            r.member("Cells", cells, "");
            r.member("CurrentVelocity", curV, "");
        }

        boost::optional<BestAction> bestAction;
        std::list<Cell> cells;
        Velocity2 curV;
        std::list<std::string> objectives;
    };

protected:

    Config mConfig;

protected:

    virtual void buildCellLut() = 0;

private:
    void initializeCells(float dt);

    Channel<Config> mDbgDynamicWindow;
    Channel<TrajectoryFamily> mDbgTrajectories;
};

}}

#endif