TimeQueue.h

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * ArmarX is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * ArmarX is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 *
 * @author     Fabian Reister ( fabian dot reister at kit dot edu )
 * @date       2021
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#pragma once
 
#include <algorithm>
#include <deque>
#include <iterator>
#include <mutex>
#include <shared_mutex>
#include <vector>
 
#include <Eigen/Core>
#include <Eigen/Geometry>
 
#include <SimoxUtility/math/pose/interpolate.h>
 
#include "ArmarXCore/core/logging/LogSender.h"
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
 
namespace armarx::localization_and_mapping::cartographer_adapter
{
 
    //    struct PoseStamped
    //    {
    //        Eigen::Isometry3f pose;
    //        int64_t timestamp;
    //    };
 
    template <typename MessageType>
    class TimeQueue
    {
 
    public:
        TimeQueue(const std::size_t maxQueueSize) : maxQueueSize(maxQueueSize)
        {
        }
 
        TimeQueue(TimeQueue&& other) noexcept :
            queue(std::move(other.queue)), maxQueueSize(other.maxQueueSize)
        {
        }
 
        // using MessagePtr = std::shared_ptr<MessageType>;
        // using MessageConstPtr = std::shared_ptr<const MessageType>;
 
        void
        insert(MessageType message)
        {
            ARMARX_TRACE;
            ARMARX_DEBUG << "Locking insert()";
            std::unique_lock g{mtx};
 
            // if (not queue.empty())
            // {
            //     ARMARX_CHECK_GREATER(message.timestamp, queue.back().timestamp)
            //         << "Poses have to be passed in with increasing timestamps";
            // }
 
            ARMARX_TRACE;
 
            queue.emplace_back(std::move(message));
 
            // only keep the last x seconds specified by duration
            //trimUntil(poseStamped.timestamp - duration);
 
            while (queue.size() > maxQueueSize)
            {
                queue.pop_front();
            }
        }
 
        std::vector<int64_t>
        timestamps() const
        {
            ARMARX_TRACE;
            ARMARX_DEBUG << "Locking timestamps()";
 
            std::vector<int64_t> ts;
            ts.reserve(queue.size());
 
            std::shared_lock g{mtx};
            std::transform(queue.begin(),
                           queue.end(),
                           std::back_inserter(ts),
                           [](const MessageType& msg) { return msg.timestamp; });
 
            return ts;
        }
 
        const MessageType&
        lookupAt(int64_t referenceTimestamp) const
        {
            ARMARX_TRACE;
            ARMARX_DEBUG << "Locking lookupAt()";
            std::shared_lock g{mtx};
 
            // ARMARX_CHECK(_has(referenceTimestamp))
            //     << "Cannot perform lookup of timestamp " << referenceTimestamp;
 
            // const auto poseNextIt = findFirstElementAtOrAfter(referenceTimestamp);
            const auto poseIt = findFirstElementAtOrBefore(referenceTimestamp);
 
            // TODO(fabian.reister): check if poseBeforeIt is valid and compare dt's
 
            return *poseIt;
        }
 
        bool
        has(int64_t timestamp) const
        {
            ARMARX_TRACE;
            ARMARX_DEBUG << "Locking has()";
            std::shared_lock g{mtx};
            return _has(timestamp);
        }
 
        void
        trimUntil(int64_t timestamp)
        {
            ARMARX_TRACE;
            ARMARX_DEBUG << "Locking trimUntil()";
 
            std::unique_lock g{mtx};
 
            while (not queue.empty())
            {
                if (queue.front().timestamp <= timestamp)
                {
                    queue.pop_front();
                }
 
                // We reached a valid entry. Done.
                break;
            }
        }
 
        void
        clear()
        {
            std::lock_guard g{mtx};
            queue.clear();
        }
 
        std::size_t
        size() const noexcept
        {
            std::lock_guard g{mtx};
            return queue.size();
        }
 
        using QueueType = std::deque<MessageType>;
        using QueueIterator = typename QueueType::iterator;
        using QueueConstIterator = typename QueueType::const_iterator;
        using QueueConstReverseIterator = typename QueueType::const_reverse_iterator;
 
 
    protected:
        QueueConstIterator
        findFirstElementAtOrAfter(int64_t timestamp) const
        {
            auto timestampBeyond = [timestamp](const MessageType& poseStamped)
            { return poseStamped.timestamp >= timestamp; };
 
            const auto poseNextIt = std::find_if(queue.begin(), queue.end(), timestampBeyond);
            return poseNextIt;
        }
 
        QueueConstReverseIterator
        findFirstElementAtOrBefore(int64_t timestamp) const
        {
            auto timestampBefore = [timestamp](const MessageType& poseStamped)
            { return poseStamped.timestamp <= timestamp; };
 
            const auto poseBeforeIt = std::find_if(queue.rbegin(), queue.rend(), timestampBefore);
            return poseBeforeIt;
        }
 
        bool
        _has(int64_t timestamp) const
        {
            // Cannot perform lookup when queue is empty
            if (queue.empty())
            {
                return false;
            }
 
            // Cannot perform lookup into the future!
            if (queue.back().timestamp < timestamp)
            {
                // ARMARX_DEBUG << "Cannot perform lookup into future";
                return false;
            }
 
            // Cannot perform lookup. Timestamp too old
            if (queue.front().timestamp > timestamp)
            {
                // ARMARX_DEBUG << "Cannot perform lookup into past";
                return false;
            }
 
            // => now we know that there is an element right after and before the timestamp within our queue
            return true;
        }
 
 
    protected:
        QueueType queue;
 
        mutable std::shared_mutex mtx;
 
    private:
        void
        trimUntilPriv(const size_t pos)
        {
            if (pos >= queue.size())
            {
                // TODO(fabian.reister): warning
                return;
            }
 
            if (pos == 0)
            {
                queue.clear();
            }
 
            queue.erase(queue.begin(), queue.begin() + pos);
        }
 
        const std::size_t maxQueueSize;
    };
 
} // namespace armarx::localization_and_mapping::cartographer_adapter