prediction_helpers.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     phesch ( ulila at student dot kit dot edu )
 * @date       2022
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#pragma once
 
#include <functional>
#include <vector>
 
#include <ArmarXCore/core/time.h>
 
#include <RobotAPI/libraries/armem/core/MemoryID.h>
#include <RobotAPI/libraries/armem/server/wm/memory_definitions.h>
#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
 
namespace armarx::armem
{
    /**
     * Holds info on snapshot data extracted from a time range.
     * Used for performing predictions.
     */
    template <typename DataType, typename LatestType>
    struct SnapshotRangeInfo
    {
        bool success = false;
        std::string errorMessage = "";
 
        std::vector<double> timestampsSec = {};
        std::vector<DataType> values = {};
        LatestType latestValue;
    };
 
    /*!
     * @brief Get data points for the snapshots of an entity in a given time range.
     *
     * The resulting `SnapshotRangeInfo` is useful for performing predictions.
     * Data from the individual snapshots is given to the conversion functions
     * before being stored in the result.
     * Timestamps in the `timestampsSec` vector are relative to the endTime parameter
     * to this function.
     * Data from a snapshot is always retrieved from the instance with index 0.
     * If some data cannot be retrieved, `success` is set to `false`
     * and the `errorMessage` in the return value is set accordingly.
     *
     * @param segment the segment to get the snapshot data from
     * @param entityID the entity to get the data for
     * @param startTime the beginning of the time range
     * @param endTime the end of the time range. Timestamps are relative to this value
     * @param dictToData the conversion function from `DictPtr` to `DataType`
     * @param dictToLatest the conversion function from `DictPtr` to `LatestType`
     * @return the corresponding `LatestSnapshotInfo`
     */
    template <typename SegmentType, typename DataType, typename LatestType>
    SnapshotRangeInfo<DataType, LatestType>
    getSnapshotsInRange(const SegmentType* segment,
                        const MemoryID& entityID,
                        const DateTime& startTime,
                        const DateTime& endTime,
                        std::function<DataType(const aron::data::DictPtr&)> dictToData,
                        std::function<LatestType(const aron::data::DictPtr&)> dictToLatest)
    {
        SnapshotRangeInfo<DataType, LatestType> result;
        result.success = false;
 
        const server::wm::Entity* entity = segment->findEntity(entityID);
        if (entity == nullptr)
        {
            std::stringstream sstream;
            sstream << "Could not find entity with ID " << entityID << ".";
            result.errorMessage = sstream.str();
            return result;
        }
 
        const int instanceIndex = 0;
        bool querySuccess = true;
        entity->forEachSnapshotInTimeRange(
            startTime,
            endTime,
            [&](const wm::EntitySnapshot& snapshot)
            {
                const auto* instance = snapshot.findInstance(instanceIndex);
                if (instance)
                {
                    result.timestampsSec.push_back(
                        (instance->id().timestamp - endTime).toSecondsDouble());
                    result.values.emplace_back(dictToData(instance->data()));
                }
                else
                {
                    std::stringstream sstream;
                    sstream << "Could not find instance with index " << instanceIndex
                            << " in snapshot " << snapshot.id() << ".";
                    result.errorMessage = sstream.str();
                    querySuccess = false;
                }
            });
 
        if (querySuccess)
        {
            aron::data::DictPtr latest = entity->findLatestInstanceData(instanceIndex);
            if (latest)
            {
                result.success = true;
                result.latestValue = dictToLatest(latest);
            }
            else
            {
                std::stringstream sstream;
                sstream << "Could not find instance with index " << instanceIndex << " for entity "
                        << entity->id() << ".";
                result.errorMessage = sstream.str();
            }
        }
 
        return result;
    }
 
} // namespace armarx::armem