MemoryToDebugObserver.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/>.
 *
 * @package    RobotAPI::ArmarXObjects::MemoryToDebugObserver
 * @author     Rainer Kartmann ( rainer dot kartmann at kit dot edu )
 * @date       2023
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#pragma once
 
#include <map>
#include <set>
#include <string>
 
#include <SimoxUtility/json/json.h>
 
#include <ArmarXCore/libraries/DebugObserverHelper/DebugObserverHelper.h>
 
#include <RobotAPI/interface/armem/mns/MemoryNameSystemInterface.h>
#include <RobotAPI/libraries/armem/client/Reader.h>
#include <RobotAPI/libraries/armem/client/plugins/PluginUser.h>
#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
 
namespace armarx::aron::data
{
    class NDArray;
}
 
namespace armarx::armem::client::util
{
    /**
     * @brief ID of an ARON value in the memory.
     */
    struct MemoryValueID
    {
        armem::MemoryID entityID;
        aron::Path aronPath;
    };
 
    bool operator==(const MemoryValueID& lhs, const MemoryValueID& rhs);
    bool operator<(const MemoryValueID& lhs, const MemoryValueID& rhs);
 
    /**
     * @brief Transfers data from memory servers to the DebugObserver.
     *
     * Transfers data from the memory system to the \ref Component-DebugObserver "Debug Observer",
     * allowing to visualize them in the \ref ArmarXGui-GuiPlugins-PlotterPlugin "Live Plotter".
     */
    class MemoryToDebugObserver
    {
    public:
        /**
         * @brief Configuration.
         *
         * Can be converted to and from JSON.
         */
        struct Properties
        {
            std::vector<MemoryValueID> plottedValues;
        };
 
        /**
         * @brief Required services.
         */
        struct Services
        {
            MemoryNameSystem& memoryNameSystem;
            armarx::DebugObserverHelper& debugObserver;
        };
 
        /**
         * @brief Constructor.
         */
        MemoryToDebugObserver(const Properties& properties, const Services& services);
 
        /**
         * @brief Query values from the memory and send them to the debug observer.
         */
        void pollOnce();
 
        /**
         * @brief Query the given values from the memory and send them to the debug observer.
         *
         * Allows polling a dynamically changing set of values without reconstructing this object
         * (and thereby keeping the cache of memory readers).
         *
         * @return The datafields that were sent, grouped by channel name. Lets callers detect which
         * datafields are no longer being sent (e.g. because a value was removed) and remove them.
         */
        std::map<std::string, std::set<std::string>>
        pollOnce(const std::vector<MemoryValueID>& values);
 
        static std::string makeChannelName(const armem::MemoryID& memoryID);
        static std::string makeDatafieldName(const aron::Path& path);
 
        /**
         * @brief Decompose a numeric NDArray (vector, matrix, pose) into scalar datafields.
         *
         * Returns a list of (datafield name suffix, value) pairs to append to the base datafield
         * name. Vectors use x/y/z/w (or indices), matrices use row/column indices, and 4x4
         * homogeneous transforms are converted to x, y, z and roll, pitch, yaw. Returns an empty
         * list if the array is not a plottable numeric vector/matrix (e.g. an image).
         */
        static std::vector<std::pair<std::string, double>>
        decomposeNDArray(const aron::data::NDArray& array, int maxSize = 16);
 
    private:
        armem::client::Reader* getReader(const armem::MemoryID& memoryID);
 
        Properties properties;
        Services services;
 
        std::map<armem::MemoryID, armem::client::Reader> memoryReaders;
    };
 
    void to_json(simox::json::json& j, const MemoryValueID& id);
    void from_json(const simox::json::json& j, MemoryValueID& id);
 
    void to_json(simox::json::json& j, const MemoryToDebugObserver::Properties& p);
    void from_json(const simox::json::json& j, MemoryToDebugObserver::Properties& p);
 
} // namespace armarx::armem::client::util