SimulationObjectPoseProvider.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    armarx
 * @author     Philip Scherer ( ulila@student.kit.edu )
 * @date       2022
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "SimulationObjectPoseProvider.h"
 
#include <algorithm>
#include <experimental/map>
 
#include <SimoxUtility/algorithm/string.h>
 
#include <ArmarXCore/core/time/DateTime.h>
#include <ArmarXCore/core/time/Metronome.h>
 
#include <RobotAPI/libraries/ArmarXObjects/ProvidedObjectPose.h>
#include <RobotAPI/libraries/core/FramedPose.h>
#include <RobotAPI/libraries/core/Pose.h>
 
#include <range/v3/range/conversion.hpp>
#include <range/v3/view/filter.hpp>
#include <range/v3/view/transform.hpp>
 
namespace armarx
{
    const std::string SimulationObjectPoseProvider::defaultName = "SimulationObjectPoseProvider";
 
    armarx::PropertyDefinitionsPtr
    SimulationObjectPoseProvider::createPropertyDefinitions()
    {
        const armarx::PropertyDefinitionsPtr def =
            new armarx::ComponentPropertyDefinitions(getConfigIdentifier());
 
        // Publish to a topic (passing the TopicListenerPrx).
        // def->topic(myTopicListener);
 
        // Subscribe to a topic (passing the topic name).
        // def->topic<PlatformUnitListener>("MyTopic");
 
        def->component(simulatorPrx, "Simulator");
 
        def->optional(properties.updateFrequency,
                      "UpdateFrequency",
                      "Frequency at which to update the objects in the ObjectMemory.");
 
        def->optional(properties.requestAllEntities,
                      "p.requestAllEntities",
                      "True if all entities should be requested all the time.");
 
        def->optional(properties.initiallyRequestedEntities,
                      "p.initiallyRequestedEntities",
                      "All entities (comma separated) that should be requested from the beginning "
                      "(e.g. 'Kitchen/multivitamin-juice/0'). "
                      "If you want an entity to be localized for n seconds append ':n'. If you "
                      "want to see all active entities, set the component to DEBUG.");
 
        return def;
    }
 
    void
    SimulationObjectPoseProvider::onInitComponent()
    {
        // Topics and properties defined above are automagically registered.
 
        // Keep debug observer data until calling `sendDebugObserverBatch()`.
        // (Requies the armarx::DebugObserverComponentPluginUser.)
        // setDebugObserverBatchModeEnabled(true);
 
        // setup the active requests
        for (const auto& entity : simox::alg::split(properties.initiallyRequestedEntities, ","))
        {
            const auto tokens = simox::alg::split(entity, ":");
 
            ARMARX_CHECK(not tokens.empty());
            ARMARX_CHECK(tokens.size() <= 2)
                << "Could not use multiple durations for localization request '" << entity << "'";
 
            const auto& objectName = tokens[0];
 
            using DateTime = armarx::core::time::DateTime;
            using Duration = core::time::Duration;
            const auto until = tokens.size() == 1
                                   ? DateTime::Invalid()
                                   : DateTime::Now() + Duration::Seconds(std::stoi(tokens.at(1)));
 
            activeRequests.insert({objectName, until});
        }
    }
 
    void
    SimulationObjectPoseProvider::onConnectComponent()
    {
        // Do things after connecting to topics and components.
 
        poseProviderTask = new SimpleRunningTask<>([this]() { this->poseProviderTaskRun(); });
        poseProviderTask->start();
    }
 
    void
    SimulationObjectPoseProvider::onDisconnectComponent()
    {
    }
 
    void
    SimulationObjectPoseProvider::onExitComponent()
    {
    }
 
    std::string
    SimulationObjectPoseProvider::getDefaultName() const
    {
        return SimulationObjectPoseProvider::defaultName;
    }
 
    std::string
    SimulationObjectPoseProvider::GetDefaultName()
    {
        return SimulationObjectPoseProvider::defaultName;
    }
 
    objpose::provider::RequestObjectsOutput
    SimulationObjectPoseProvider::requestObjects(
        const objpose::provider::RequestObjectsInput& input,
        const Ice::Current& /*unused*/)
    {
        if (properties.requestAllEntities)
        {
            ARMARX_WARNING << "All entities are requested by default. "
                              "Requesting an object does not have an effect.";
            // The method will still finish to generate a return object.
        }
 
        const std::lock_guard l(knownObjectsMutex);
        const std::lock_guard l2(activeRequestsMutex);
 
        auto now = armarx::core::time::DateTime::Now();
 
        objpose::provider::RequestObjectsOutput output;
        for (const auto& id : input.objectIDs)
        {
            const std::string entityId = id.dataset + "/" + id.className + "/" + id.instanceName;
            if (knownObjects.find(entityId) != knownObjects.end())
            {
                if (activeRequests.find(entityId) != activeRequests.end())
                {
                    ARMARX_INFO << "Object '" << entityId
                                << "' is already requested. Updating the request time.";
                }
                activeRequests[entityId] =
                    now + armarx::core::time::Duration::MilliSeconds(input.relativeTimeoutMS);
                output.results[id].success = true;
            }
            else
            {
                std::vector<std::string> allNames;
                allNames.reserve(knownObjects.size());
                for (const auto& [name, _] : knownObjects)
                {
                    allNames.push_back(name);
                }
                ARMARX_WARNING << "Could not query unknown object '" << entityId
                               << "'. All known objects are: " << allNames;
                output.results[id].success = false;
            }
        }
        return output;
    }
 
    objpose::ProviderInfo
    SimulationObjectPoseProvider::getProviderInfo(const Ice::Current& /*unused*/)
    {
        objpose::ProviderInfo info;
        info.objectType = objpose::KnownObject;
        info.proxy = getProxy<objpose::ObjectPoseProviderPrx>();
        info.supportedObjects = {};
        return info;
    }
 
    void
    SimulationObjectPoseProvider::poseProviderTaskRun()
    {
        Metronome metronome(Frequency::Hertz(properties.updateFrequency));
 
        while (poseProviderTask and not poseProviderTask->isStopped())
        {
            metronome.waitForNextTick();
            const DateTime now = DateTime::Now();
 
            // update scene objects
            armarx::SceneVisuData sceneData;
            try
            {
                sceneData = simulatorPrx->getScene();
                for (const auto& object : sceneData.objects)
                {
                    ARMARX_DEBUG << "Found object in scene: " << object.name;
                }
            }
            catch (const Ice::LocalException& e)
            {
                ARMARX_INFO << "Could not get object poses from simulator: " << e.what();
                continue;
            }
 
            ARMARX_DEBUG << deactivateSpam(10) << "Received " << sceneData.objects.size()
                         << " objects and " << sceneData.robots.size() << " robots.";
 
            std::vector<objpose::ProvidedObjectPose> providedObjects;
 
            if (properties.requestAllEntities)
            {
                // Convert all poses directly
                providedObjects = getAllPoses(sceneData, now);
            }
            else
            {
                // Update known objects
                updateKnownObjects(sceneData);
                removeExpiredRequests(now);
                providedObjects = getRequestedPoses(now);
            }
 
            // Report the poses
            try
            {
                ARMARX_DEBUG << deactivateSpam(10) << "Reporting " << providedObjects.size()
                             << " object poses";
                objectPoseTopic->reportObjectPoses(getName(), objpose::toIce(providedObjects));
            }
            catch (const Ice::LocalException& e)
            {
                ARMARX_INFO << "Could not report object poses to object memory: " << e.what();
                continue;
            }
        }
    }
 
    std::vector<objpose::ProvidedObjectPose>
    SimulationObjectPoseProvider::getAllPoses(const armarx::SceneVisuData& sceneData,
                                              const DateTime& time) const
    {
        auto convertPoses = [this, time](const auto& simObject)
        { return objectPoseFromVisuData(simObject, time); };
 
        return sceneData.objects | ranges::views::transform(convertPoses) | ranges::to_vector;
    }
 
    void
    SimulationObjectPoseProvider::updateKnownObjects(const armarx::SceneVisuData& sceneData)
    {
        const std::scoped_lock l(knownObjectsMutex);
 
        knownObjects.clear();
        for (const auto& object : sceneData.objects)
        {
            knownObjects.insert({object.name, object});
        }
    }
 
    void
    SimulationObjectPoseProvider::removeExpiredRequests(const DateTime& time)
    {
        const std::scoped_lock l2(activeRequestsMutex);
 
        auto isTimeout = [time](const auto& request)
        {
            const auto& provideUntilTime = request.second;
            return (not provideUntilTime.isInvalid()) and time > provideUntilTime;
        };
 
        std::experimental::erase_if(activeRequests, isTimeout);
    }
 
    std::vector<objpose::ProvidedObjectPose>
    SimulationObjectPoseProvider::getRequestedPoses(const DateTime& time) const
    {
        const std::scoped_lock l(knownObjectsMutex);
        const std::scoped_lock l2(activeRequestsMutex);
 
        auto isRequestedObjectKnown = [this](const auto& request)
        {
            const auto& objectName = request.first;
            return this->knownObjects.find(objectName) != this->knownObjects.end();
        };
 
        auto getPoseFromRequest = [this, time](const auto& request)
        {
            const auto& object = this->knownObjects.at(request.first);
            return objectPoseFromVisuData(object, time);
        };
 
        return activeRequests | ranges::views::filter(isRequestedObjectKnown) |
               ranges::views::transform(getPoseFromRequest) | ranges::to_vector;
    }
 
    objpose::ProvidedObjectPose
    SimulationObjectPoseProvider::objectPoseFromVisuData(const armarx::ObjectVisuData& visuData,
                                                         const DateTime& time) const
    {
        objpose::ProvidedObjectPose pose;
 
        pose.providerName = getName();
        pose.objectType = objpose::ObjectType::KnownObject;
        pose.isStatic = visuData.staticObject;
 
        pose.objectID = armarx::ObjectID(visuData.name);
 
        pose.objectPose = armarx::fromIce(visuData.objectPoses.at(visuData.name));
        pose.objectPoseFrame = armarx::GlobalFrame;
 
        pose.confidence = 1;
        pose.timestamp = time;
 
        return pose;
    }
} // namespace armarx