Trajectory.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.
 */

#pragma once

#include <vector>

#include <transform/Pose.h>
#include <transform/Velocity.h>
#include <utils/Time.h>

namespace mira { namespace robot {


template<int D>
class PoseTrajectorySample
{
public:
    typedef RigidTransform<float, D> Pose;

public:

    PoseTrajectorySample() {}

    PoseTrajectorySample(const Time& iT, const Pose& iP) :
        t(iT),p(iP) {}

    template<typename Reflector>
    void reflect(Reflector& r)
    {
        r.member("Timestamp", t, "");
        p.reflect(r);
    }

public:
    Time t;
    Pose p;
};

class PoseVelocityTrajectorySample
{
public:

    PoseVelocityTrajectorySample() {}

    PoseVelocityTrajectorySample(const Pose2& iP, const Velocity2& iV, float iDT) :
        p(iP), v(iV), dt(iDT) {}

    template<typename Reflector>
    void reflect(Reflector& r)
    {
        r.member("Pose", p, "");
        r.member("Velocity", v, "");
        r.member("DeltaTime", dt, "");
    }

public:
    Pose2 p;
    Velocity2 v;
    float dt;
};

typedef std::vector<PoseVelocityTrajectorySample, Eigen::aligned_allocator<PoseVelocityTrajectorySample>> PoseVelocityTrajectory;

template <int D>
class PoseTrajectory : public std::vector<PoseTrajectorySample<D>,Eigen::aligned_allocator<PoseTrajectorySample<D>>>
{
public:
    typedef std::vector<PoseTrajectorySample<D>,Eigen::aligned_allocator<PoseTrajectorySample<D>>> Base;

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

    typedef PoseTrajectorySample<D> Sample;
    typedef RigidTransform<float, D> Pose;

    void push_back(const Time& t, const Pose& p) {
        Base::push_back(Sample(t,p));
    }

    // need to "inherit" the method explicitly, as we overloaded it above :(
    void push_back(const Sample& s) {
        Base::push_back(s);
    }

    typedef typename Base::iterator iterator;
    typedef typename Base::const_iterator const_iterator;
    iterator find(const Time& t, iterator start) {
        for(iterator it=start; it!=this->end(); ++it) {
            if(it->t >= t)
                return it;
        }
        return this->end(); // not found
    }
    iterator find(const Time& t) {
        return find(t, this->begin());
    }
};

typedef PoseTrajectory<2> PoseTrajectory2;
typedef PoseTrajectory<3> PoseTrajectory3;


} } // namespace