PathFollowTask.h

Go to the documentation of this file.

/*
 * Copyright (C) 2012 by
 *   MetraLabs GmbH (MLAB), GERMANY
 * and
 *   Neuroinformatics and Cognitive Robotics Labs (NICR) at TU Ilmenau, GERMANY
 * All rights reserved.
 *
 * Contact: info@mira-project.org
 *
 * Commercial Usage:
 *   Licensees holding valid commercial licenses may use this file in
 *   accordance with the commercial license agreement provided with the
 *   software or, alternatively, in accordance with the terms contained in
 *   a written agreement between you and MLAB or NICR.
 *
 * GNU General Public License Usage:
 *   Alternatively, this file may be used under the terms of the GNU
 *   General Public License version 3.0 as published by the Free Software
 *   Foundation and appearing in the file LICENSE.GPL3 included in the
 *   packaging of this file. Please review the following information to
 *   ensure the GNU General Public License version 3.0 requirements will be
 *   met: http://www.gnu.org/copyleft/gpl.html.
 *   Alternatively you may (at your option) use any later version of the GNU
 *   General Public License if such license has been publicly approved by
 *   MLAB and NICR (or its successors, if any).
 *
 * 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_PATHFOLLOWTASK_H_
#define _MIRA_PATHFOLLOWTASK_H_

#include <geometry/Point.h>
#include <navigation/Task.h>
#include <transform/Pose.h>

namespace mira { namespace navigation {


class PathFollowBaseTaskCommon : public SubTask
{
public:

    PathFollowBaseTaskCommon() :
        precision(0.1f),
        goalTranslationTolerance(0.1f),
        goalOrientationTolerance(Degreef(10.0f))
    {}

    PathFollowBaseTaskCommon(float iPrecision, float iGoalTransTolerance, Anglef iGoalOrientTolerance) :
        precision(iPrecision),
        goalTranslationTolerance(iGoalTransTolerance),
        goalOrientationTolerance(iGoalOrientTolerance)
    {}

    // keep using Angle for serialization compatibility
    template<typename Reflector>
    void reflectTolerance(Reflector& r) {
        if (Reflector::isReadOnly::value) {
            Anglef a(goalOrientationTolerance);
            r.property("GoalOrientationTolerance", a,
                       "The orientation tolerance for reaching the goal point [deg]",
                       PropertyHint(), REFLECT_CTRLFLAG_TEMP_TRACKING);
        } else {
            Anglef a;
            r.property("GoalOrientationTolerance", a,
                       "The orientation tolerance for reaching the goal point [deg]",
                       Degreef(10.0f),
                       PropertyHint(), REFLECT_CTRLFLAG_TEMP_TRACKING);
            goalOrientationTolerance = a.rad();
        }
    }

    void reflectTolerance(PropertySerializer& r) {
        r.property("GoalOrientationTolerance",
                   rad2degGetter(goalOrientationTolerance),
                   deg2radSetter(goalOrientationTolerance, Deg2RadNonNegative),
                   "The orientation tolerance for reaching the goal point [deg]",
                   10.f, PropertyHints::minimum(0.f));
    }


    template<typename Reflector>
    void reflect(Reflector& r)
    {
        MIRA_REFLECT_BASE(r, SubTask);
        r.property("Precision", precision,
                   "The precision for following the path (max. distance from path) [m]", 0.25f);
        r.property("GoalTranslationTolerance", goalTranslationTolerance,
                   "The position tolerance for reaching the goal point [m]", 0.1f);
        reflectTolerance(r);
        r.property("FrameID", frame,
                   "The coordinate frame of the points", "/GlobalFrame");
    }

public:
    float precision;

    float goalTranslationTolerance;

    float goalOrientationTolerance;

    std::string frame;
};


template <typename T>
class PathFollowBaseTask : public PathFollowBaseTaskCommon
{
public:

    PathFollowBaseTask() : PathFollowBaseTaskCommon()
    {}

    PathFollowBaseTask(float iPrecision, float iGoalTransTolerance, Anglef iGoalOrientTolerance) :
        PathFollowBaseTaskCommon(iPrecision, iGoalTransTolerance, iGoalOrientTolerance)
    {}

    template<typename Reflector>
    void reflect(Reflector& r)
    {
        MIRA_REFLECT_BASE(r, PathFollowBaseTaskCommon);
        r.property("Points", points,
                   "Control points of the spline [m]");
    }

    virtual bool reachedPosition(const typename T::value_type& pos) const { return true; }
    virtual bool reachedOrientation(const typename T::value_type& pos) const { return true; }
    virtual bool reached(const typename T::value_type& pos) const
    {
        return reachedPosition(pos) && reachedOrientation(pos);
    }

public:
    T points;
};


class PathFollowTask : public PathFollowBaseTask<std::vector<Pose2>>
{
    MIRA_OBJECT(PathFollowTask);
public:

    typedef PathFollowBaseTask<std::vector<Pose2>> Base;

    PathFollowTask() {}

    PathFollowTask(float iPrecision, float iGoalTransTolerance, Anglef iGoalOrientTolerance) :
        Base(iPrecision, iGoalTransTolerance, iGoalOrientTolerance)
    {}

    bool reachedPosition(const Pose2& pos) const
    {
        return reached(pos.t);
    }

    bool reached(const Point2f& pos) const
    {
        if (points.empty())
            return true;

        return (pos - points.back().t).norm() <= goalTranslationTolerance;
    }

    bool reachedOrientation(const Pose2& pos) const
    {
        return reached(pos.phi());
    }

    bool reached(float orientation) const
    {
        if (points.empty())
            return true;

        return std::abs(smallestAngleDifference(orientation, points.back().phi())) <= goalOrientationTolerance;
    }
};

class VelocityWaypoint
{
public:
    VelocityWaypoint() {}
    VelocityWaypoint(const Point2f& p, float v) :
        pos(p), vT(v) {}

    template<typename Reflector>
    void reflect(Reflector& r)
    {
        r.member("Pos", pos, "Position");
        r.member("vT", vT, "Velocity");
    }

    Point2f pos;
    float vT;
};


class VelocityWaypointTask : public PathFollowBaseTask<std::vector<VelocityWaypoint>>
{
    MIRA_OBJECT(VelocityWaypointTask);
public:
    typedef PathFollowBaseTask<std::vector<VelocityWaypoint>> Base;

    VelocityWaypointTask() {}

    VelocityWaypointTask(float iGoalTransTolerance) :
        Base(0.0f, iGoalTransTolerance, Anglef(0.0f))
    {}

    bool reachedPosition(const VelocityWaypoint& pos) const
    {
        return reached(pos.pos);
    }

    bool reached(const Point2f& pos) const
    {
        if (points.empty())
            return true;

        return (pos - points.back().pos).norm() <= goalTranslationTolerance;
    }

};


}} // namespace

#endif