ArVizStorage.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/>.
 *
 * @package    RobotAPI::ArmarXObjects::ArViz
 * @author     Fabian Paus ( fabian dot paus at kit dot edu )
 * @date       2019
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "ArVizStorage.h"
 
#include <algorithm>
#include <optional>
 
#include <SimoxUtility/json/json.hpp>
 
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/core/time.h>
#include <ArmarXCore/core/time/DateTime.h>
#include <ArmarXCore/core/time/Duration.h>
#include <ArmarXCore/core/util/IceBlobToObject.h>
#include <ArmarXCore/core/util/ObjectToIceBlob.h>
 
namespace armarx
{
    static std::filesystem::path
    getAbsolutePath(const std::filesystem::path& path)
    {
        if (path.is_absolute())
        {
            return path;
        }
        else
        {
            std::string absolute;
            if (ArmarXDataPath::getAbsolutePath(path.string(), absolute))
            {
                return absolute;
            }
            else
            {
                ARMARX_WARNING_S << "Could not resolve relative file as package data file: "
                                 << path;
                return path;
            }
        }
    }
 
    std::string
    ArVizStorage::getDefaultName() const
    {
        return "ArVizStorage";
    }
 
    armarx::PropertyDefinitionsPtr
    ArVizStorage::createPropertyDefinitions()
    {
        armarx::PropertyDefinitionsPtr defs(
            new ComponentPropertyDefinitions(getConfigIdentifier()));
 
        defs->optional(
            properties_.topicName, "TopicName", "Layer updates are sent over this topic.");
 
        defs->optional(properties_.maxHistorySize,
                       "MaxHistorySize",
                       "How many layer updates are saved in the history until they are compressed")
            .setMin(0);
 
        defs->optional(properties_.historyPath,
                       "HistoryPath",
                       "Destination path where the history is serialized to");
 
 
        defs->optional(properties_.componentWarnFrequency, "ComponentWarnFrequency", 
            "Define a frequency in Hz above which the compnent raises a warning. As you should not send data at a too high rate.");
 
        return defs;
    }
 
    void
    ArVizStorage::onInitComponent()
    {
        properties_.historyPath = getAbsolutePath(properties_.historyPath);
        if (!std::filesystem::exists(properties_.historyPath))
        {
            ARMARX_INFO << "Creating history path: " << properties_.historyPath;
            std::error_code error;
            std::filesystem::create_directory(properties_.historyPath, error);
            if (error)
            {
                ARMARX_WARNING << "Could not create directory for history: \n" << error.message();
            }
        }
 
        usingTopic(properties_.topicName);
    }
 
    void
    ArVizStorage::onConnectComponent()
    {
        revision = 0;
        currentState.clear();
        history.clear();
        recordingInitialState.clear();
 
        recordingBuffer.clear();
        recordingMetaData.id = "";
    }
 
    void
    ArVizStorage::onDisconnectComponent()
    {
        if (recordingTask)
        {
            recordingTask->stop();
            recordingTask = nullptr;
        }
    }
 
    void
    ArVizStorage::onExitComponent()
    {
    }
 
    void
    ArVizStorage::updateLayers(viz::data::LayerUpdateSeq const& updates, const Ice::Current&)
    {
        std::unique_lock<std::mutex> lock(historyMutex);
 
        revision += 1;
 
        IceUtil::Time now = IceUtil::Time::now();
        long nowInMicroSeconds = now.toMicroSeconds();
 
        if (not updates.empty())
        {
            const std::string& componentName = updates.front().component;
 
            auto& history = updateHistoryForComponents[componentName];
            history.push_back(armarx::Clock::Now());
 
            const auto maxHistoryDur = armarx::Duration::SecondsDouble(1);
 
            const DateTime referenceNow = Clock::Now();
            const auto isOutdated = [&referenceNow,
                                     &maxHistoryDur](const DateTime& timestamp) -> bool
            { return (referenceNow - timestamp) > maxHistoryDur; };
 
            // trim history
            history.erase(std::remove_if(history.begin(), history.end(), isOutdated),
                          history.end());
 
            ARMARX_VERBOSE << deactivateSpam(1, componentName) << componentName << ": "
                           << history.size() / maxHistoryDur.toSecondsDouble() << " Hz";
 
            if (history.size() > properties_.componentWarnFrequency)
            {
                ARMARX_WARNING << deactivateSpam(10) << "Component `" << componentName << "`"
                               << "sends data at a too high rate ("
                               << history.size() / maxHistoryDur.toSecondsDouble() << ")" 
                               << deactivateSpam(10, componentName);
            }
        }
 
 
        for (auto& update : updates)
        {
            if (update.component.empty())
            {
                ARMARX_INFO << deactivateSpam(120)
                            << "Discarding ArViz update with empty component name. Check whether "
                            << "you correctly create your ArViz client (`armarx::viz::Client`) "
                            << "in your code.";
                continue;
            }
 
            auto& historyEntry = history.emplace_back();
            historyEntry.revision = revision;
            historyEntry.timestampInMicroseconds = nowInMicroSeconds;
            historyEntry.update = update;
 
            // Insert or create the layer
            bool found = false;
            for (auto& layer : currentState)
            {
                if (layer.update.component == update.component && layer.update.name == update.name)
                {
                    layer = historyEntry;
                    found = true;
                    break;
                }
            }
            if (!found)
            {
                currentState.push_back(historyEntry);
            }
        }
 
        long currentHistorySize = history.size();
        if (currentHistorySize >= properties_.maxHistorySize)
        {
            {
                std::unique_lock<std::mutex> lock(recordingMutex);
                if (recordingMetaData.id.size() > 0)
                {
                    auto& newBatch = recordingBuffer.emplace_back();
                    newBatch.initialState = recordingInitialState;
                    newBatch.updates = std::move(history);
                    recordingInitialState = currentState;
 
                    recordingCondition.notify_one();
                }
            }
            history.clear();
        }
    }
 
    viz::data::CommitResult
    ArVizStorage::commitAndReceiveInteractions(viz::data::CommitInput const& input,
                                               const Ice::Current&)
    {
        viz::data::CommitResult result;
 
        {
            std::unique_lock<std::mutex> lock(historyMutex);
 
            revision += 1;
            result.revision = revision;
 
            IceUtil::Time now = IceUtil::Time::now();
            long nowInMicroSeconds = now.toMicroSeconds();
 
            // Insert updates into the history and update the current state
            for (viz::data::LayerUpdate const& update : input.updates)
            {
                viz::data::TimestampedLayerUpdate& historyEntry = history.emplace_back();
                historyEntry.revision = revision;
                historyEntry.timestampInMicroseconds = nowInMicroSeconds;
                historyEntry.update = update;
 
                // Insert or create the layer
                bool found = false;
                for (viz::data::TimestampedLayerUpdate& layer : currentState)
                {
                    if (layer.update.component == update.component &&
                        layer.update.name == update.name)
                    {
                        layer = historyEntry;
                        found = true;
                        break;
                    }
                }
                if (!found)
                {
                    currentState.push_back(historyEntry);
                }
            }
 
            // Trim the history if max size has been exceeded
            long currentHistorySize = history.size();
            if (currentHistorySize >= properties_.maxHistorySize)
            {
                {
                    std::unique_lock<std::mutex> lock(recordingMutex);
                    if (recordingMetaData.id.size() > 0)
                    {
                        auto& newBatch = recordingBuffer.emplace_back();
                        newBatch.initialState = recordingInitialState;
                        newBatch.updates = std::move(history);
                        recordingInitialState = currentState;
 
                        recordingCondition.notify_one();
                    }
                }
                history.clear();
            }
 
            // Find relevant interactions
            if (input.interactionComponent.size() > 0)
            {
                auto interactionsEnd = interactionBuffer.end();
                auto foundInteractionsBegin =
                    std::partition(interactionBuffer.begin(),
                                   interactionsEnd,
                                   [&input](viz::data::InteractionFeedback const& interaction)
                                   {
                                       if (interaction.component == input.interactionComponent)
                                       {
                                           for (std::string const& layer : input.interactionLayers)
                                           {
                                               if (interaction.layer == layer)
                                               {
                                                   return false;
                                               }
                                           }
                                       }
                                       return true;
                                   });
 
                result.interactions.assign(foundInteractionsBegin, interactionsEnd);
                interactionBuffer.erase(foundInteractionsBegin, interactionsEnd);
            }
        }
 
        return result;
    }
 
    viz::data::LayerUpdates
    ArVizStorage::pullUpdatesSince(Ice::Long revision, const Ice::Current&)
    {
        viz::data::LayerUpdates result;
 
        std::unique_lock<std::mutex> lock(historyMutex);
 
        result.updates.reserve(currentState.size());
        for (auto& layer : currentState)
        {
            if (layer.revision > revision)
            {
                result.updates.push_back(layer.update);
            }
        }
        result.revision = this->revision;
 
        return result;
    }
 
    static const int ALL_TRANSFORM_FLAGS =
        viz::data::InteractionFeedbackType::TRANSFORM_BEGIN_FLAG |
        viz::data::InteractionFeedbackType::TRANSFORM_DURING_FLAG |
        viz::data::InteractionFeedbackType::TRANSFORM_END_FLAG;
 
    viz::data::LayerUpdates
    ArVizStorage::pullUpdatesSinceAndSendInteractions(
        Ice::Long revision,
        viz::data::InteractionFeedbackSeq const& interactions,
        const Ice::Current& c)
    {
        viz::data::LayerUpdates result;
 
        std::unique_lock<std::mutex> lock(historyMutex);
 
        for (viz::data::InteractionFeedback const& interaction : interactions)
        {
            for (viz::data::InteractionFeedback& entry : interactionBuffer)
            {
                if (entry.component == interaction.component && entry.layer == interaction.layer &&
                    entry.element == interaction.element)
                {
                    int previousTransformFlags = entry.type & ALL_TRANSFORM_FLAGS;
                    int interactionTransformFlags = interaction.type & ALL_TRANSFORM_FLAGS;
 
                    entry = interaction;
                    // Keep the previous transform flags.
                    // Blindly overwriting the entry might skip over the begin or during events.
                    if (previousTransformFlags != 0 && interactionTransformFlags != 0)
                    {
                        entry.type |= previousTransformFlags;
                    }
                    goto for_end;
                }
            }
 
            // Interaction did not exist, add it to the buffer
            interactionBuffer.push_back(interaction);
 
        for_end:;
        }
 
        result.updates.reserve(currentState.size());
        for (viz::data::TimestampedLayerUpdate& layer : currentState)
        {
            if (layer.revision > revision)
            {
                result.updates.push_back(layer.update);
            }
        }
        result.revision = this->revision;
 
        return result;
    }
 
    void
    ArVizStorage::record()
    {
        while (!recordingTask->isStopped())
        {
            std::unique_lock<std::mutex> lock(recordingMutex);
            while (!recordingTask->isStopped() && recordingBuffer.empty())
            {
                recordingCondition.wait_for(lock, std::chrono::milliseconds(10));
            }
            for (auto& batch : recordingBuffer)
            {
                recordBatch(batch);
            }
            recordingBuffer.clear();
        }
    }
} // namespace armarx
 
namespace armarx::viz::data
{
 
    void
    to_json(nlohmann::json& j, RecordingBatchHeader const& batch)
    {
        j["index"] = batch.index;
        j["firstRevision"] = batch.firstRevision;
        j["lastRevision"] = batch.lastRevision;
        j["firstTimestampInMicroSeconds"] = batch.firstTimestampInMicroSeconds;
        j["lastTimestampInMicroSeconds"] = batch.lastTimestampInMicroSeconds;
    }
 
    void
    from_json(nlohmann::json const& j, RecordingBatchHeader& batch)
    {
        batch.index = j["index"];
        batch.firstRevision = j["firstRevision"];
        batch.lastRevision = j["lastRevision"];
        batch.firstTimestampInMicroSeconds = j["firstTimestampInMicroSeconds"];
        batch.lastTimestampInMicroSeconds = j["lastTimestampInMicroSeconds"];
    }
 
    void
    to_json(nlohmann::json& j, Recording const& recording)
    {
        j["id"] = recording.id;
        j["firstRevision"] = recording.firstRevision;
        j["lastRevision"] = recording.lastRevision;
        j["firstTimestampInMicroSeconds"] = recording.firstTimestampInMicroSeconds;
        j["lastTimestampInMicroSeconds"] = recording.lastTimestampInMicroSeconds;
        j["batchHeaders"] = recording.batchHeaders;
    }
 
    void
    from_json(nlohmann::json const& j, Recording& recording)
    {
        recording.id = j["id"];
        recording.firstRevision = j["firstRevision"];
        recording.lastRevision = j["lastRevision"];
        recording.firstTimestampInMicroSeconds = j["firstTimestampInMicroSeconds"];
        recording.lastTimestampInMicroSeconds = j["lastTimestampInMicroSeconds"];
        j["batchHeaders"].get_to(recording.batchHeaders);
    }
 
} // namespace armarx::viz::data
 
static bool
writeCompleteFile(std::string const& filename, const void* data, std::size_t size)
{
    FILE* file = fopen(filename.c_str(), "wb");
    if (!file)
    {
        return false;
    }
    std::size_t written = std::fwrite(data, 1, size, file);
    if (written != size)
    {
        return false;
    }
    std::fclose(file);
    return true;
}
 
static std::string
readCompleteFile(std::filesystem::path const& path)
{
    FILE* f = fopen(path.string().c_str(), "rb");
    fseek(f, 0, SEEK_END);
    long fsize = ftell(f);
    fseek(f, 0, SEEK_SET); /* same as rewind(f); */
 
    std::string result(fsize, '\0');
    std::size_t read = fread(result.data(), 1, fsize, f);
    result.resize(read);
    fclose(f);
 
    return result;
}
 
static std::optional<armarx::viz::data::Recording>
readRecordingInfo(std::filesystem::path const& recordingDirectory)
{
    std::optional<::armarx::viz::data::Recording> result;
 
    std::filesystem::path recordingFilePath = recordingDirectory / "recording.json";
    if (!std::filesystem::exists(recordingFilePath))
    {
        ARMARX_INFO << "No recording.json found in directory: " << recordingDirectory;
        return result;
    }
 
    try
    {
        std::string recordingString = readCompleteFile(recordingFilePath);
        nlohmann::json recordingJson = nlohmann::json::parse(recordingString);
 
        ::armarx::viz::data::Recording recording;
        recordingJson.get_to(recording);
 
        result = std::move(recording);
        return result;
    }
    catch (std::exception const& ex)
    {
        ARMARX_WARNING << "Could not parse JSON file: " << recordingFilePath
                       << "\nReason: " << ex.what();
        return result;
    }
}
 
static std::string
batchFileName(armarx::viz::data::RecordingBatchHeader const& batchHeader)
{
    return std::to_string(batchHeader.firstRevision) + ".bin";
}
 
void
armarx::ArVizStorage::recordBatch(armarx::viz::data::RecordingBatch& batch)
{
    if (batch.updates.empty())
    {
        return;
    }
 
    auto& first = batch.updates.front();
    auto& last = batch.updates.back();
 
    batch.header.index = -1; // TODO: Should we save the index?
    batch.header.firstRevision = first.revision;
    batch.header.lastRevision = last.revision;
    batch.header.firstTimestampInMicroSeconds = first.timestampInMicroseconds;
    batch.header.lastTimestampInMicroSeconds = last.timestampInMicroseconds;
    if (firstBatch)
    {
        batch.initialState = currentState;
        firstBatch = false;
    }
 
 
    std::string filename = batchFileName(batch.header);
    std::filesystem::path filePath = recordingPath / filename;
 
    ObjectToIceBlobSerializer ser{batch};
    // Save the batch to a new file
    if (!writeCompleteFile(filePath.string(), ser.begin(), ser.size()))
    {
        ARMARX_WARNING << "Could not write history file: " << filePath;
        return;
    }
 
    // Update the meta data
    if (recordingMetaData.firstRevision < 0)
    {
        recordingMetaData.firstRevision = first.revision;
    }
    recordingMetaData.lastRevision = last.revision;
 
    if (recordingMetaData.firstTimestampInMicroSeconds < 0)
    {
        recordingMetaData.firstTimestampInMicroSeconds = first.timestampInMicroseconds;
    }
    recordingMetaData.lastTimestampInMicroSeconds = last.timestampInMicroseconds;
 
    armarx::viz::data::RecordingBatchHeader& newBatch =
        recordingMetaData.batchHeaders.emplace_back();
    newBatch.index = recordingMetaData.batchHeaders.size() - 1;
    newBatch.firstRevision = first.revision;
    newBatch.lastRevision = last.revision;
    newBatch.firstTimestampInMicroSeconds = first.timestampInMicroseconds;
    newBatch.lastTimestampInMicroSeconds = last.timestampInMicroseconds;
 
    // Save the meta data to a (potentially existing) json file
    nlohmann::json j = recordingMetaData;
    std::string jString = j.dump(2);
    std::filesystem::path recordingFile = recordingPath / "recording.json";
    if (!writeCompleteFile(recordingFile.string(), jString.data(), jString.size()))
    {
        ARMARX_WARNING << "Could not write recording file: " << recordingFile;
        return;
    }
 
    ARMARX_INFO << "Recorded ArViz batch to: " << filePath;
}
 
std::string
armarx::ArVizStorage::startRecording(std::string const& newRecordingPrefix, const Ice::Current&)
{
    {
        std::unique_lock<std::mutex> lock(recordingMutex);
        if (recordingMetaData.id.size() > 0)
        {
            ARMARX_WARNING
                << "Could not start recording with prefix " << newRecordingPrefix
                << "\nbecause there is already a recording running for the recording ID: "
                << recordingMetaData.id;
            return recordingMetaData.id;
        }
 
        IceUtil::Time now = IceUtil::Time::now();
        std::ostringstream id;
        id << newRecordingPrefix << '_' << now.toString("%Y-%m-%d_%H-%M-%S");
        std::string newRecordingID = id.str();
 
        recordingPath = properties_.historyPath / newRecordingID;
        if (!std::filesystem::exists(recordingPath))
        {
            ARMARX_INFO << "Creating directory for recording with ID '" << newRecordingID
                        << "'\nPath: " << recordingPath;
            std::filesystem::create_directory(recordingPath);
        }
 
        recordingBuffer.clear();
 
        recordingMetaData.id = newRecordingID;
        {
            std::unique_lock<std::mutex> lock(historyMutex);
            if (history.size() > 0)
            {
                auto& mostRecent = history.back();
                recordingMetaData.firstRevision = mostRecent.revision;
                recordingMetaData.firstTimestampInMicroSeconds = mostRecent.timestampInMicroseconds;
            }
        }
        recordingMetaData.lastRevision = 0;
        recordingMetaData.lastTimestampInMicroSeconds = 0;
        recordingMetaData.batchHeaders.clear();
    }
 
    firstBatch = true;
    recordingTask = new RunningTask<ArVizStorage>(this, &ArVizStorage::record);
    recordingTask->start();
 
    return "";
}
 
void
armarx::ArVizStorage::stopRecording(const Ice::Current&)
{
    if (!recordingTask)
    {
        return;
    }
 
    recordingTask->stop();
    recordingTask = nullptr;
 
    std::unique_lock<std::mutex> lock(recordingMutex);
 
    viz::data::RecordingBatch lastBatch;
    lastBatch.initialState = recordingInitialState;
    lastBatch.updates = std::move(history);
    recordBatch(lastBatch);
 
    recordingMetaData.id = "";
    recordingMetaData.firstRevision = -1;
    recordingMetaData.firstTimestampInMicroSeconds = -1;
}
 
armarx::viz::data::RecordingsInfo
armarx::ArVizStorage::getAllRecordings(const Ice::Current&)
{
    viz::data::RecordingsInfo recordingsInfo;
    viz::data::RecordingSeq result;
 
    for (std::filesystem::directory_entry const& entry :
         std::filesystem::directory_iterator(properties_.historyPath))
    {
        ARMARX_DEBUG << "Checking: " << entry.path();
 
        if (!entry.is_directory())
        {
            continue;
        }
 
        std::optional<viz::data::Recording> recording = readRecordingInfo(entry.path());
        if (recording)
        {
            result.push_back(std::move(*recording));
        }
    }
 
    recordingsInfo.recordings = result;
    recordingsInfo.recordingsPath = properties_.historyPath;
 
    return recordingsInfo;
}
 
armarx::viz::data::RecordingBatch
armarx::ArVizStorage::getRecordingBatch(std::string const& recordingID,
                                        Ice::Long batchIndex,
                                        const Ice::Current&)
{
    viz::data::RecordingBatch result;
    result.header.index = -1;
 
    std::filesystem::path recordingPath = properties_.historyPath / recordingID;
    std::optional<viz::data::Recording> recording = readRecordingInfo(recordingPath);
    if (!recording)
    {
        ARMARX_WARNING << "Could not read recording information for '" << recordingID << "'"
                       << "\nPath: " << recordingPath;
        return result;
    }
 
    if (batchIndex < 0 || batchIndex >= (long)recording->batchHeaders.size())
    {
        ARMARX_WARNING << "Batch index is not valid. Index = " << batchIndex
                       << "Batch count: " << recording->batchHeaders.size();
        return result;
    }
 
    viz::data::RecordingBatchHeader const& batchHeader = recording->batchHeaders[batchIndex];
    std::filesystem::path batchFile = recordingPath / batchFileName(batchHeader);
    if (!std::filesystem::exists(batchFile))
    {
        ARMARX_WARNING << "Could not find batch file for recording '" << recordingID
                       << "' with index " << batchIndex << "\nPath: " << batchFile;
        return result;
    }
 
    iceBlobToObject(result, readCompleteFile(batchFile));
 
    result.header.index = batchIndex;
 
    return result;
}