MessageQueue.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 Humanoids Group, H2T, KIT
 * @license    http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#pragma once
 
#include <atomic>
#include <condition_variable>
#include <cstddef>
#include <functional>
#include <mutex>
#include <optional>
#include <queue>
#include <thread>
 
#include "ArmarXCore/core/services/tasks/RunningTask.h"
#include <ArmarXCore/core/logging/Logging.h>
 
namespace armarx::localization_and_mapping::cartographer_adapter
{
 
    // https://embeddedartistry.com/blog/2017/02/08/implementing-an-asynchronous-dispatch-queue/
 
    /**
    * @brief The purpose of the MessageQueue is to provide a convenient way to process incoming messages sequentially.
    *
    * Althought, the main idea is to pass in ICE messages, the MessageQueue can be used for any data.
    *
    * Usage example:
    *
    *      struct ExampleMessage{
    *          int x, y;
    *      }
    *
    *      class Consumer{
    *          public:
    *
    *              void processMessage(const ExampleMessagePtr& message){
    *                  // your implementation
    *              }
    *      }
    *
    *      Consumer message_consumer;
    *      MessageQueue<ExampleMessage> message_queue(&Consumer::processMessage, message_consumer);
    *
    *      message_queue.push({1, 2});
    *      message_queue.push({3, 4});
    *
    */
    template <class MessageT>
    class MessageQueue
    {
 
    public:
        using MessageType = MessageT;
 
        using Callback = std::function<void(MessageT)>;
 
        MessageQueue() :
            workerThread(new RunningTask<MessageQueue>(this, &MessageQueue::run, "worker")){};
 
        MessageQueue(const MessageQueue&) = delete;
 
        template <class... Args>
        MessageQueue(Args... args) :
            messageCallback(std::bind(args..., std::placeholders::_1)),
            workerThread(new RunningTask<MessageQueue>(this, &MessageQueue::run, "worker"))
        {
            workerThread->start();
        }
 
        ~MessageQueue()
        {
            // trigger thread to stop
            ok.store(false);
 
            cv.notify_all();
 
            if (workerThread)
            {
                workerThread->stop();
                workerThread->join();
            }
 
            // ARMARX_INFO << "Waiting for workerThread to return";
            // workerThread.join();
        }
 
        template <class... Args>
        void
        connect(Args... args)
        {
            messageCallback = std::bind(args..., std::placeholders::_1);
 
            if (not workerThread->isRunning())
            {
                workerThread->start();
            }
        }
 
        /**
        * @brief push data into the queue.
        * @param data
        */
        void
        push(MessageT data)
        {
            ARMARX_TRACE;
            if (!processMessages.load())
            {
                return;
            }
 
            ARMARX_TRACE;
            {
                std::lock_guard g{mtx};
                messageQueue.push(data);
 
                // limit queue size
                const size_t queueSize = messageQueue.size();
                if ((maxQueueSize > 0) and (queueSize > maxQueueSize))
                {
                    const size_t elementsToRemove = queueSize - maxQueueSize;
 
                    // TODO(fabian.reister): is there a better way?
                    for (size_t i = 0; i < elementsToRemove; i++)
                    {
                        messageQueue.pop();
                    }
                }
 
                ARMARX_VERBOSE << deactivateSpam(1) << "Message queue size is " << queueSize;
            }
            cv.notify_all(); // for run() and waitUntilProcessed()
        }
 
        /**
        * @brief run the worker thread function sending out messages via callback
        */
        void
        run()
        {
            ARMARX_TRACE;
 
            while (ok.load())
            {
                // Get new data.
                std::optional<MessageT> message;
                {
                    ARMARX_TRACE;
                    // Wait until we have data
                    std::unique_lock<std::mutex> lock(mtx);
                    cv.wait(lock, [this] { return not messageQueue.empty(); });
                    // after wait, we own the lock
                    if (not messageQueue.empty())
                    {
                        message = messageQueue.front();
                        messageQueue.pop();
                    }
                }
                if (message and processMessages.load())
                {
                    ARMARX_TRACE;
                    messageCallback(message.value());
                }
                // else: drop the message if the signal has changed
            }
 
            ARMARX_DEBUG << "[Message queue] Worker thread has finished.";
        }
 
        [[nodiscard]] std::size_t
        size() const
        {
            ARMARX_DEBUG << "Locking size()";
 
            std::lock_guard g{mtx};
            return messageQueue.size();
        }
 
        void
        enable()
        {
            ARMARX_TRACE;
            processMessages.store(true);
        }
 
        void
        disable()
        {
            ARMARX_TRACE;
            processMessages.store(false);
        }
 
        void
        waitUntilProcessed()
        {
            ARMARX_DEBUG << "Locking waitUntilProcessed()";
 
            ARMARX_TRACE;
            processMessages.store(false);
 
            std::unique_lock<std::mutex> lock(mtx);
 
            cv.wait(lock, [this] { return messageQueue.empty(); });
        }
 
        void
        clear()
        {
            ARMARX_DEBUG << "Locking clear()";
 
            ARMARX_TRACE;
            std::lock_guard g{mtx};
 
            messageQueue = std::queue<MessageT>();
        }
 
        void
        setQueueSize(const size_t maxQueueSize) noexcept
        {
            ARMARX_TRACE;
            this->maxQueueSize = maxQueueSize;
        }
 
    private:
        mutable std::mutex mtx;
        std::queue<MessageT> messageQueue;
 
        std::condition_variable cv;
 
        Callback messageCallback;
 
        // std::thread workerThread;
 
        typename armarx::RunningTask<MessageQueue>::pointer_type workerThread;
 
        std::atomic<bool> processMessages{true};
        std::atomic<bool> ok{true};
 
        size_t maxQueueSize{0};
    };
 
} // namespace armarx::localization_and_mapping::cartographer_adapter