LivePlotController.cpp

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/>.
 */
 
#include "LivePlotController.h"
 
#include <algorithm>
 
#include <QThread>
#include <QTimer>
 
#include <ArmarXCore/libraries/DebugObserverHelper/DebugObserverHelper.h>
 
namespace armarx::armem::gui::controller
{
    static constexpr int livePlotPollFrequencyHz = 30;
 
    LivePlotController::LivePlotController(
        std::shared_ptr<armem::gui::model::MemoryViewerModel> model)
    {
        Logging::setTag("LivePlotController");
        _model = std::move(model);
        startWorker();
    }
 
    LivePlotController::~LivePlotController()
    {
        if (_workerThread)
        {
            _workerThread->quit();
            _workerThread->wait();
        }
        removeAllChannels();
    }
 
    void
    LivePlotController::startWorker()
    {
        _workerThread = new QThread();
        _worker = new LivePlotWorker(_model, this);
        _worker->moveToThread(_workerThread);
 
        connect(_workerThread, &QThread::started, _worker, &LivePlotWorker::init);
        connect(_workerThread, &QThread::finished, _worker, &LivePlotWorker::cleanup);
        connect(_workerThread, &QThread::finished, _worker, &QObject::deleteLater);
        connect(_workerThread, &QThread::finished, _workerThread, &QObject::deleteLater);
 
        connect(this, &This::startWorkerSignal, _worker, &LivePlotWorker::onConnect);
        connect(this, &This::stopWorkerSignal, _worker, &LivePlotWorker::onDisconnect);
 
        _workerThread->start();
    }
 
    bool
    LivePlotController::hasDebugObserver() const
    {
        return bool(_model->debugObserver());
    }
 
    bool
    LivePlotController::isPlotted(const MemoryValueID& id) const
    {
        std::lock_guard lock(_valuesMutex);
        return std::find(_values.begin(), _values.end(), id) != _values.end();
    }
 
    bool
    LivePlotController::allPlotted(const std::vector<MemoryValueID>& ids) const
    {
        if (ids.empty())
        {
            return false;
        }
        std::lock_guard lock(_valuesMutex);
        return std::all_of(ids.begin(),
                           ids.end(),
                           [this](const MemoryValueID& id)
                           { return std::find(_values.begin(), _values.end(), id) != _values.end(); });
    }
 
    void
    LivePlotController::setPlotted(const std::vector<MemoryValueID>& ids, bool enable)
    {
        {
            std::lock_guard lock(_valuesMutex);
            for (const MemoryValueID& id : ids)
            {
                auto it = std::find(_values.begin(), _values.end(), id);
                if (enable && it == _values.end())
                {
                    _values.push_back(id);
                    _usedChannels.insert(
                        armem::client::util::MemoryToDebugObserver::makeChannelName(id.entityID));
                }
                else if (not enable && it != _values.end())
                {
                    _values.erase(it);
                }
            }
        }
 
        // Deactivated values are removed from the observer by the worker, which detects on its
        // next poll that they are no longer being sent (see LivePlotWorker::removeStaleDatafields).
        emit valuesChanged();
    }
 
    std::vector<LivePlotController::MemoryValueID>
    LivePlotController::valuesSnapshot() const
    {
        std::lock_guard lock(_valuesMutex);
        return _values;
    }
 
    void
    LivePlotController::onConnect()
    {
        emit startWorkerSignal();
    }
 
    void
    LivePlotController::onDisconnect()
    {
        emit stopWorkerSignal();
    }
 
    void
    LivePlotController::removeAllChannels()
    {
        DebugObserverInterfacePrx observer = _model->debugObserver();
        if (not observer)
        {
            return;
        }
 
        std::set<std::string> channels;
        {
            std::lock_guard lock(_valuesMutex);
            channels = std::move(_usedChannels);
            _usedChannels.clear();
            _values.clear();
        }
 
        for (const std::string& channel : channels)
        {
            try
            {
                observer->removeDebugChannel(channel);
            }
            catch (...)
            {
                // Observer may already be gone; nothing we can do.
            }
        }
    }
 
    LivePlotWorker::LivePlotWorker(std::shared_ptr<armem::gui::model::MemoryViewerModel> model,
                                   LivePlotController* controller) :
        _controller(controller)
    {
        Logging::setTag("LivePlotWorker");
        _model = std::move(model);
    }
 
    void
    LivePlotWorker::init()
    {
        // Timers must be created in their own thread, so not in the constructor.
        _timer = new QTimer(this);
        _timer->setInterval(1000 / livePlotPollFrequencyHz);
        connect(_timer, &QTimer::timeout, this, &This::onPoll);
    }
 
    void
    LivePlotWorker::cleanup()
    {
        if (_timer)
        {
            _timer->stop();
            _timer->deleteLater();
            _timer = nullptr;
        }
        _memoryToDebugObserver.reset();
        _debugObserverHelper.reset();
        _lastPushed.clear();
    }
 
    void
    LivePlotWorker::onConnect()
    {
        // The DebugObserver is optional in the MemoryViewer; without it we cannot plot.
        if (not _model->debugObserver())
        {
            return;
        }
 
        _debugObserverHelper =
            std::make_unique<armarx::DebugObserverHelper>(_model->debugObserver(), true);
 
        armem::client::util::MemoryToDebugObserver::Services services{
            .memoryNameSystem = _model->mns(),
            .debugObserver = *_debugObserverHelper,
        };
        _memoryToDebugObserver = std::make_unique<armem::client::util::MemoryToDebugObserver>(
            armem::client::util::MemoryToDebugObserver::Properties{}, services);
 
        if (_timer)
        {
            _timer->start();
        }
    }
 
    void
    LivePlotWorker::onDisconnect()
    {
        if (_timer)
        {
            _timer->stop();
        }
        _memoryToDebugObserver.reset();
        _debugObserverHelper.reset();
        _lastPushed.clear();
    }
 
    void
    LivePlotWorker::onPoll()
    {
        if (not _memoryToDebugObserver)
        {
            return;
        }
 
        std::map<std::string, std::set<std::string>> current;
        try
        {
            current = _memoryToDebugObserver->pollOnce(_controller->valuesSnapshot());
        }
        catch (const std::exception& e)
        {
            // Keep _lastPushed so a transient failure does not remove still-active datafields.
            ARMARX_INFO << deactivateSpam(10)
                        << "Failed to send memory values to the debug observer: " << e.what();
            return;
        }
        catch (...)
        {
            ARMARX_INFO << deactivateSpam(10)
                        << "Failed to send memory values to the debug observer (unknown error).";
            return;
        }
 
        removeStaleDatafields(current);
        _lastPushed = std::move(current);
    }
 
    void
    LivePlotWorker::removeStaleDatafields(
        const std::map<std::string, std::set<std::string>>& current)
    {
        DebugObserverInterfacePrx observer = _model->debugObserver();
        if (not observer)
        {
            return;
        }
 
        static const std::set<std::string> empty;
        for (const auto& [channel, lastFields] : _lastPushed)
        {
            const auto it = current.find(channel);
            const std::set<std::string>& currentFields = it != current.end() ? it->second : empty;
 
            for (const std::string& field : lastFields)
            {
                if (currentFields.find(field) == currentFields.end())
                {
                    try
                    {
                        observer->removeDebugDatafield(channel, field);
                    }
                    catch (...)
                    {
                    }
                }
            }
 
            if (currentFields.empty())
            {
                try
                {
                    observer->removeDebugChannel(channel);
                }
                catch (...)
                {
                }
            }
        }
    }
 
} // namespace armarx::armem::gui::controller