SegmentAdapter.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::armem_objects::SegmentAdapter
 * @author     Rainer Kartmann ( rainer dot kartmann at kit dot edu )
 * @date       2021
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "SegmentAdapter.h"
 
#include <SimoxUtility/algorithm/get_map_keys_values.h>
#include <VirtualRobot/Robot.h>
 
#include "ArmarXCore/core/time/Clock.h"
#include <ArmarXCore/core/ice_conversions/ice_conversions_templates.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/time/CycleUtil.h>
#include <ArmarXCore/core/time/ice_conversions.h>
#include <ArmarXCore/observers/variant/Variant.h>
 
#include <RobotAPI/libraries/ArmarXObjects/ObjectID.h>
#include <RobotAPI/libraries/ArmarXObjects/aron_conversions.h>
#include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h>
#include <RobotAPI/libraries/ArmarXObjects/predictions.h>
#include <RobotAPI/libraries/armem_objects/aron/ObjectInstance.aron.generated.h>
#include <RobotAPI/libraries/aron/core/aron_conversions.h>
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
 
namespace armarx::armem::server::obj::instance
{
 
    SegmentAdapter::SegmentAdapter(MemoryToIceAdapter& iceMemory) : segment(iceMemory)
    {
    }
 
    std::string
    SegmentAdapter::getName() const
    {
        return Logging::tag.tagName;
    }
 
    void
    SegmentAdapter::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
    {
        calibration.defineProperties(defs, prefix + "calibration.");
        segment.defineProperties(defs, prefix);
        robotHead.defineProperties(defs, prefix + "head.");
        visu.defineProperties(defs, prefix + "visu.");
    }
 
    void
    SegmentAdapter::init()
    {
        segment.setTag(getName());
        segment.decay.setTag(getName());
        segment.setPredictionEngines(predictionEngines);
        robotHead.setTag(getName());
        visu.setTag(getName());
 
        segment.init();
    }
 
    void
    SegmentAdapter::connect(robot_state::VirtualRobotReader* virtualRobotReader,
                            KinematicUnitObserverInterfacePrx kinematicUnitObserver,
                            viz::Client arviz,
                            DebugObserverInterfacePrx debugObserver)
    {
        this->debugObserver = debugObserver;
        this->arviz = arviz;
 
        segment.robots.reader = virtualRobotReader;
 
        robotHead.kinematicUnitObserver = kinematicUnitObserver;
        robotHead.debugObserver = debugObserver;
        robotHead.fetchDatafields();
 
        visu.arviz = arviz;
 
        if (!visu.updateTask)
        {
            visu.updateTask = new SimpleRunningTask<>([this]() { this->visualizeRun(); });
            visu.updateTask->start();
        }
 
        segment.connect(arviz);
    }
 
    void
    SegmentAdapter::reportProviderAvailable(const std::string& providerName,
                                            const objpose::ProviderInfo& info,
                                            const Ice::Current&)
    {
        ARMARX_IMPORTANT << "Provider `" << providerName << "` is (now) available.";
        updateProviderInfo(providerName, info);
    }
 
    void
    SegmentAdapter::reportObjectPoses(const std::string& providerName,
                                      const objpose::data::ProvidedObjectPoseSeq& providedPoses,
                                      const Ice::Current&)
    {
        ARMARX_VERBOSE << "Received " << providedPoses.size() << " object poses from provider '"
                       << providerName << "'.";
        updateObjectPoses(providerName, providedPoses);
    }
 
    void
    SegmentAdapter::updateProviderInfo(const std::string& providerName,
                                       const objpose::ProviderInfo& info)
    {
        if (!info.proxy)
        {
            ARMARX_WARNING << "Received availability signal by provider '" << providerName << "' "
                           << "with invalid provider proxy.\nIgnoring provider '" << providerName
                           << "'.";
            return;
        }
        segment.doLocked(
            [this, &providerName, &info]()
            {
                std::stringstream ss;
                for (const auto& id : info.supportedObjects)
                {
                    ss << "- " << id << "\n";
                }
                ARMARX_VERBOSE << "Provider '" << providerName << "' available.\n"
                               << "Supported objects: \n"
                               << ss.str();
                segment.providers[providerName] = info;
            });
    }
 
    void
    SegmentAdapter::updateObjectPoses(const std::string& providerName,
                                      const objpose::data::ProvidedObjectPoseSeq& providedPoses)
    {
        TIMING_START(tReportObjectPoses);
 
        RobotHeadMovement::Discard discard;
        {
            std::scoped_lock lock(robotHeadMutex);
            discard = robotHead.getDiscard();
        }
        if (debugObserver)
        {
            StringVariantBaseMap map;
            map["Discarding All Updates"] = new Variant(discard.all ? 1.f : 0.f);
            if (discard.all)
            {
                map["Proportion Updated Poses"] = new Variant(0.f);
            }
            debugObserver->setDebugChannel(getName(), map);
        }
 
        if (discard.all)
        {
            return;
        }
 
        segment.doLocked(
            [&]()
            {
                const auto timestamp = armarx::Clock::Now();
 
                TIMING_START(tCommitObjectPoses);
                Segment::CommitStats stats = segment.commitObjectPoses(
                    providerName, providedPoses, calibration, discard.updatesUntil);
                TIMING_END_STREAM(tCommitObjectPoses, ARMARX_VERBOSE);
 
                if (debugObserver)
                {
                    debugObserver->setDebugChannel(
                        getName(),
                        {{"Discarding All Updates", new Variant(discard.all ? 1 : 0)},
                         {"Proportion Updated Poses",
                          new Variant(static_cast<float>(stats.numUpdated) /
                                      providedPoses.size())}});
                }
 
                handleProviderUpdate(providerName);
 
                TIMING_END_STREAM(tReportObjectPoses, ARMARX_VERBOSE);
                if (debugObserver)
                {
                    debugObserver->setDebugChannel(
                        getName(),
                        {
                            {"t ReportObjectPoses [ms]",
                             new Variant(tReportObjectPoses.toMilliSecondsDouble())},
                            {"t MemorySetObjectPoses [ms]",
                             new Variant(tCommitObjectPoses.toMilliSecondsDouble())},
                        });
                }
            });
    }
 
    void
    SegmentAdapter::handleProviderUpdate(const std::string& providerName)
    {
        // Initialized to 0 on first access.
        if (segment.providers.count(providerName) == 0)
        {
            segment.providers[providerName] = objpose::ProviderInfo();
        }
    }
 
    objpose::data::ObjectPoseSeq
    SegmentAdapter::getObjectPoses(const Ice::Current&)
    {
        TIMING_START(tGetObjectPoses);
 
        const Time now = Time::Now();
        const objpose::ObjectPoseSeq objectPoses = segment.doLocked(
            [this, &now]() { return simox::alg::get_values(segment.getObjectPoses(now)); });
        const objpose::data::ObjectPoseSeq result = objpose::toIce(objectPoses);
 
        TIMING_END_STREAM(tGetObjectPoses, ARMARX_VERBOSE);
 
        if (debugObserver)
        {
            debugObserver->setDebugChannel(
                getName(),
                {
                    {"t GetObjectPoses() [ms]",
                     new Variant(tGetObjectPoses.toMilliSecondsDouble())},
                });
        }
 
        return result;
    }
 
    objpose::data::ObjectPoseSeq
    SegmentAdapter::getObjectPosesByProvider(const std::string& providerName, const Ice::Current&)
    {
        TIMING_START(GetObjectPoses);
 
        const Time now = Time::Now();
        const objpose::ObjectPoseSeq objectPoses = segment.doLocked(
            [this, &now, &providerName]() {
                return simox::alg::get_values(segment.getObjectPosesByProvider(providerName, now));
            });
        const objpose::data::ObjectPoseSeq result = objpose::toIce(objectPoses);
 
        TIMING_END_STREAM(GetObjectPoses, ARMARX_VERBOSE);
 
        if (debugObserver)
        {
            debugObserver->setDebugChannel(getName(),
                                           {
                                               {"t GetObjectPosesByProvider() [ms]",
                                                new Variant(GetObjectPoses.toMilliSecondsDouble())},
                                           });
        }
 
        return result;
    }
 
    objpose::observer::RequestObjectsOutput
    SegmentAdapter::requestObjects(const objpose::observer::RequestObjectsInput& input,
                                   const Ice::Current&)
    {
        ARMARX_INFO << "Requesting objects ...";
 
        std::map<std::string, objpose::provider::RequestObjectsInput> providerRequests;
        std::map<std::string, objpose::ObjectPoseProviderPrx> proxies;
 
        objpose::observer::RequestObjectsOutput output;
 
        auto updateProxy = [&](const std::string& providerName)
        {
            if (proxies.count(providerName) == 0)
            {
                if (auto it = segment.providers.find(providerName); it != segment.providers.end())
                {
                    proxies[providerName] = it->second.proxy;
                }
                else
                {
                    ARMARX_ERROR << "No proxy for provider ' " << providerName << "'.";
                    proxies[providerName] = nullptr;
                }
            }
        };
 
        if (not input.provider.empty())
        {
            providerRequests[input.provider] = input.request;
            updateProxy(input.provider);
        }
        else
        {
            segment.doLocked(
                [&]()
                {
                    ARMARX_INFO << "Known providers are: "
                                << simox::alg::get_keys(segment.providers);
 
                    for (const auto& objectID : input.request.objectIDs)
                    {
                        bool found = true;
                        for (const auto& [providerName, info] : segment.providers)
                        {
                            // ToDo: optimize look up.
                            if (std::find(info.supportedObjects.begin(),
                                          info.supportedObjects.end(),
                                          objectID) != info.supportedObjects.end())
                            {
                                ARMARX_INFO << "Found provider for " << objectID << ": "
                                            << providerName;
                                providerRequests[providerName].relativeTimeoutMS =
                                    input.request.relativeTimeoutMS;
                                providerRequests[providerName].objectIDs.push_back(objectID);
                                updateProxy(providerName);
                                break;
                            }
                        }
                        if (!found)
                        {
                            ARMARX_ERROR << "Did not find a provider for " << objectID << ".";
                            output.results[objectID].providerName = "";
                        }
                    }
                });
        }
 
        ARMARX_INFO << VAROUT(simox::alg::get_keys(providerRequests));
 
        for (const auto& [providerName, request] : providerRequests)
        {
            if (objpose::ObjectPoseProviderPrx proxy = proxies.at(providerName); proxy)
            {
                ARMARX_INFO << "Requesting " << request.objectIDs.size() << " objects by provider '"
                            << providerName << "' for " << request.relativeTimeoutMS << " ms.";
                objpose::provider::RequestObjectsOutput providerOutput =
                    proxy->requestObjects(request);
 
                int successful = 0;
                for (const auto& [objectID, result] : providerOutput.results)
                {
                    objpose::observer::ObjectRequestResult& res = output.results[objectID];
                    res.providerName = providerName;
                    res.result = result;
                    successful += int(result.success);
                }
                ARMARX_INFO << successful << " of " << request.objectIDs.size()
                            << " object requests successful.";
            }
        }
        return output;
    }
 
    objpose::ProviderInfoMap
    SegmentAdapter::getAvailableProvidersInfo(const Ice::Current&)
    {
        return segment.doLocked([this]() { return segment.providers; });
    }
 
    Ice::StringSeq
    SegmentAdapter::getAvailableProviderNames(const Ice::Current&)
    {
        return segment.doLocked([this]() { return simox::alg::get_keys(segment.providers); });
    }
 
    objpose::ProviderInfo
    SegmentAdapter::getProviderInfo(const std::string& providerName, const Ice::Current&)
    {
        return segment.doLocked([this, &providerName]()
                                { return segment.getProviderInfo(providerName); });
    }
 
    bool
    SegmentAdapter::hasProvider(const std::string& providerName, const Ice::Current&)
    {
        return segment.doLocked([this, &providerName]()
                                { return segment.providers.count(providerName) > 0; });
    }
 
    objpose::AttachObjectToRobotNodeOutput
    SegmentAdapter::attachObjectToRobotNode(const objpose::AttachObjectToRobotNodeInput& input,
                                            const Ice::Current&)
    {
        return segment.doLocked([this, &input]()
                                { return segment.attachObjectToRobotNode(input); });
    }
 
    objpose::DetachObjectFromRobotNodeOutput
    SegmentAdapter::detachObjectFromRobotNode(const objpose::DetachObjectFromRobotNodeInput& input,
                                              const Ice::Current&)
    {
        return segment.doLocked([this, &input]()
                                { return segment.detachObjectFromRobotNode(input); });
    }
 
    objpose::DetachAllObjectsFromRobotNodesOutput
    SegmentAdapter::detachAllObjectsFromRobotNodes(
        const objpose::DetachAllObjectsFromRobotNodesInput& input,
        const Ice::Current&)
    {
        return segment.doLocked([this, &input]()
                                { return segment.detachAllObjectsFromRobotNodes(input); });
    }
 
    objpose::AgentFramesSeq
    SegmentAdapter::getAttachableFrames(const Ice::Current&)
    {
        return segment.doLocked(
            [this]()
            {
                objpose::AgentFramesSeq output;
                for (const auto& [name, agent] : segment.robots.loaded)
                {
                    objpose::AgentFrames& frames = output.emplace_back();
                    frames.agent = agent->getName();
                    frames.frames = agent->getRobotNodeNames();
                }
                return output;
            });
    }
 
    objpose::SignalHeadMovementOutput
    SegmentAdapter::signalHeadMovement(const objpose::SignalHeadMovementInput& input,
                                       const Ice::Current&)
    {
        std::scoped_lock lock(robotHeadMutex);
        return robotHead.signalHeadMovement(input);
    }
 
    objpose::ObjectPosePredictionResultSeq
    SegmentAdapter::predictObjectPoses(const objpose::ObjectPosePredictionRequestSeq& requests,
                                       const Ice::Current&)
    {
        objpose::ObjectPosePredictionResultSeq results;
        std::vector<std::map<DateTime, objpose::ObjectPose>> poses;
        std::vector<objpose::ObjectPose> latestPoses;
 
        segment.doLocked(
            [this, &requests, &results, &poses, &latestPoses]()
            {
                for (const auto& request : requests)
                {
                    auto& result = results.emplace_back();
                    const ObjectID objID = armarx::fromIce(request.objectID);
 
                    const Duration timeWindow = armarx::fromIce<Duration>(request.timeWindow);
 
                    const wm::Entity* entity = segment.findObjectEntity(objID);
                    // Use result.success as a marker for whether to continue later
                    result.success = false;
                    poses.emplace_back();
                    latestPoses.emplace_back();
                    if (!entity)
                    {
                        std::stringstream sstream;
                        sstream << "Could not find object with ID " << objID << ".";
                        result.errorMessage = sstream.str();
                        continue;
                    }
 
                    const auto dto = entity->findLatestInstanceDataAs<arondto::ObjectInstance>();
                    if (!dto)
                    {
                        std::stringstream sstream;
                        sstream << "No snapshots of the object " << objID << " were found."
                                << " Cannot make a prediction.";
                        result.errorMessage = sstream.str();
                        continue;
                    }
 
                    result.success = true;
                    poses.back() = segment.getObjectPosesInRange(
                        *entity, Time::Now() - timeWindow, Time::Invalid());
                    latestPoses.back() = aron::fromAron<objpose::ObjectPose>(dto.value().pose);
                }
            });
 
        for (size_t i = 0; i < requests.size(); ++i)
        {
            const objpose::ObjectPosePredictionRequest& request = requests.at(i);
            objpose::ObjectPosePredictionResult& result = results.at(i);
 
            if (result.success)
            {
                armem::PredictionSettings settings =
                    armem::PredictionSettings::fromIce(request.settings);
 
                if (settings.predictionEngineID.empty() or
                    settings.predictionEngineID == linearPredictionEngineID)
                {
                    result = objpose::predictObjectPoseLinear(
                        poses.at(i),
                        armarx::fromIce<DateTime>(request.timestamp),
                        latestPoses.at(i));
                }
                else
                {
                    result.errorMessage = "Prediction engine '" + settings.predictionEngineID +
                                          "' not available for object pose prediction.";
                    result.success = false;
                }
            }
        }
        return results;
    }
 
    armem::prediction::data::PredictionEngineSeq
    SegmentAdapter::getAvailableObjectPoseEngines(const Ice::Current&)
    {
        return armarx::toIce<armem::prediction::data::PredictionEngineSeq>(predictionEngines);
    }
 
    void
    SegmentAdapter::visualizeRun()
    {
        ObjectFinder objectFinder;
 
        CycleUtil cycle(static_cast<int>(1000 / visu.frequencyHz));
        while (visu.updateTask && !visu.updateTask->isStopped())
        {
            {
                std::scoped_lock lock(visuMutex);
 
                if (visu.enabled)
                {
                    TIMING_START(tVisu);
 
                    objpose::ObjectPoseMap objectPoses;
                    std::map<ObjectID, std::map<DateTime, objpose::ObjectPose>> poseHistories;
                    visu.minConfidence = -1;
 
                    segment.doLocked(
                        [this, &objectPoses, &poseHistories, &objectFinder]()
                        {
                            const Time now = Time::Now();
 
                            // Also include decayed objects in result
                            // Store original setting.
                            const float minConf = segment.decay.removeObjectsBelowConfidence;
                            segment.decay.removeObjectsBelowConfidence = -1;
                            // Get result.
                            objectPoses = segment.getObjectPoses(now);
                            // Restore original setting.
                            segment.decay.removeObjectsBelowConfidence = minConf;
 
                            objectFinder = segment.objectFinder;
                            if (segment.decay.enabled)
                            {
                                visu.minConfidence = segment.decay.removeObjectsBelowConfidence;
                            }
 
                            // Get histories.
                            for (const auto& [id, objectPose] : objectPoses)
                            {
                                const wm::Entity* entity = segment.findObjectEntity(id);
                                if (entity != nullptr)
                                {
                                    poseHistories[id] = instance::Segment::getObjectPosesInRange(
                                        *entity,
                                        Time::Now() - Duration::SecondsDouble(
                                                          visu.linearPredictions.timeWindowSeconds),
                                        Time::Invalid());
                                }
                            }
                        });
 
                    const std::vector<viz::Layer> layers =
                        visu.visualizeCommit(objectPoses, poseHistories, objectFinder);
                    arviz.commit(layers);
 
 
                    TIMING_END_STREAM(tVisu, ARMARX_VERBOSE);
 
                    if (debugObserver)
                    {
                        armarx::StringVariantBaseMap debugValues;
                        debugValues["t Visualize [ms]"] = new Variant(tVisu.toMilliSecondsDouble());
                        for (const auto& [id, pose] : objectPoses)
                        {
                            debugValues["confidence(" + id.str() + ")"] =
                                new Variant(pose.confidence);
                        }
                        debugObserver->setDebugChannel(getName(), debugValues);
                    }
                }
            }
            cycle.waitForCycleDuration();
        }
    }
 
    const std::string SegmentAdapter::linearPredictionEngineID = "Linear Position Regression";
    const std::vector<PredictionEngine> SegmentAdapter::predictionEngines{
        {linearPredictionEngineID}};
 
    void
    SegmentAdapter::RemoteGui::setup(const SegmentAdapter& adapter)
    {
        using namespace armarx::RemoteGui::Client;
 
        this->visu.setup(adapter.visu);
        this->segment.setup(adapter.segment);
        this->decay.setup(adapter.segment.decay);
        this->robotHead.setup(adapter.robotHead);
 
        layout = VBoxLayout{this->visu.group,
                            this->segment.group,
                            this->decay.group,
                            this->robotHead.group,
                            VSpacer()};
 
        group = {};
        group.setLabel("Instance");
        group.addChild(layout);
    }
 
    void
    SegmentAdapter::RemoteGui::update(SegmentAdapter& adapter)
    {
        // Non-atomic variables need to be guarded by a mutex if accessed by multiple threads
        {
            std::scoped_lock lock(adapter.visuMutex);
            this->visu.update(adapter.visu);
        }
        this->segment.update(adapter.segment);
        {
            adapter.segment.doLocked([this, &adapter]()
                                     { this->decay.update(adapter.segment.decay); });
        }
        {
            std::scoped_lock lock(adapter.robotHeadMutex);
            this->robotHead.update(adapter.robotHead);
        }
    }
 
} // namespace armarx::armem::server::obj::instance