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 <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
#include <RobotAPI/libraries/ArmarXObjects/aron_conversions.h>
#include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h>
#include <RobotAPI/libraries/ArmarXObjects/ObjectID.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 "ArmarXCore/core/time/Clock.h"
#include <ArmarXCore/core/ice_conversions/ice_conversions_templates.h>
#include <ArmarXCore/core/time/CycleUtil.h>
#include <ArmarXCore/core/time/ice_conversions.h>
#include <ArmarXCore/observers/variant/Variant.h>
 
#include <VirtualRobot/Robot.h>
 
#include <SimoxUtility/algorithm/get_map_keys_values.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&)
    {
        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&)
    {
        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 (input.provider.size() > 0)
        {
            providerRequests[input.provider] = input.request;
            updateProxy(input.provider);
        }
        else
        {
            segment.doLocked([&]()
            {
                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())
                        {
                            providerRequests[providerName].objectIDs.push_back(objectID);
                            updateProxy(providerName);
                            break;
                        }
                    }
                    if (!found)
                    {
                        ARMARX_ERROR << "Did not find a provider for " << objectID << ".";
                        output.results[objectID].providerName = "";
                    }
                }
            });
        }
 
        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);
        }
    }
 
}