WorkingMemoryObjectPoseProvider.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    MemoryX::ArmarXObjects::WorkingMemoryObjectPoseProvider
 * @author     Rainer Kartmann ( rainer dot kartmann at kit dot edu )
 * @date       2020
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "WorkingMemoryObjectPoseProvider.h"
 
#include <SimoxUtility/algorithm/string.h>
#include <SimoxUtility/json.h>
#include <VirtualRobot/CollisionDetection/CollisionModel.h>
#include <VirtualRobot/ManipulationObject.h>
 
#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/core/time/CycleUtil.h>
#include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
#include <ArmarXCore/core/time/TimeUtil.h>
#include <ArmarXCore/core/time/ice_conversions.h>
 
#include <RobotAPI/libraries/ArmarXObjects/ObjectID.h>
#include <RobotAPI/libraries/ArmarXObjects/ProvidedObjectPose.h>
#include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h>
#include <RobotAPI/libraries/core/Pose.h>
 
#include <MemoryX/core/MemoryXCoreObjectFactories.h>
#include <MemoryX/libraries/memorytypes/entity/ObjectInstance.h>
 
void
memoryx::to_json(nlohmann::json& j, const Config& config)
{
    j["objectNames"] = config.objectNames;
}
 
void
memoryx::from_json(const nlohmann::json& j, Config& config)
{
    j.at("objectNames").get_to(config.objectNames);
}
 
namespace memoryx
{
 
    armarx::PropertyDefinitionsPtr
    WorkingMemoryObjectPoseProvider::createPropertyDefinitions()
    {
        armarx::PropertyDefinitionsPtr defs(
            new armarx::ComponentPropertyDefinitions(getConfigIdentifier()));
 
        defs->component(workingMemory);
        defs->component(robotStateComponent);
        defs->component(priorKnowledge);
 
        defs->optional(updateFrequency, "UpdateFrequency", "Target number of updates per second.");
        defs->optional(configFile, "ConfigFile", "Path to the config file.");
        defs->optional(loadObjectDatasetsStr,
                       "LoadDatasets",
                       "Only load the files for the following datasets, separated by ;. Load all "
                       "if input is empty.");
 
        return defs;
    }
 
    std::string
    WorkingMemoryObjectPoseProvider::getDefaultName() const
    {
        return "WorkingMemoryObjectPoseProvider";
    }
 
    std::string
    WorkingMemoryObjectPoseProvider::GetDefaultName()
    {
        return "WorkingMemoryObjectPoseProvider";
    }
 
    void
    WorkingMemoryObjectPoseProvider::onInitComponent()
    {
        configFile = armarx::ArmarXDataPath::resolvePath(configFile);
        ARMARX_INFO << "Loading config file '" << configFile << "' ...";
        if (std::filesystem::is_regular_file(configFile))
        {
            const nlohmann::json j = nlohmann::read_json(configFile);
            j.get_to(config);
        }
        else
        {
            ARMARX_INFO << "No config file at '" << configFile << "'";
        }
    }
 
    void
    WorkingMemoryObjectPoseProvider::onConnectComponent()
    {
        ARMARX_CHECK_NOT_NULL(workingMemory);
        ARMARX_CHECK_NOT_NULL(robotStateComponent);
        ARMARX_CHECK_NOT_NULL(priorKnowledge);
 
        ARMARX_IMPORTANT << "Loading workingmemory and priorknowledge entites and files. This may "
                            "take a while....";
        attachments.initFromProxies(workingMemory, robotStateComponent);
 
        std::vector<std::string> loadDatasets = simox::alg::split(loadObjectDatasetsStr, ";");
 
        objectClassSegment.initFromProxy(priorKnowledge, loadDatasets);
        ARMARX_IMPORTANT << "... done loading!";
 
        // A periodic task logs an important info when the cycle time is not met.
        // To avoid this, we use a running task.
        task = new armarx::SimpleRunningTask<>(
            [this]()
            {
                armarx::CycleUtil cycle(int(1000 / updateFrequency));
 
                while (task && !task->isStopped())
                {
                    provideObjectInstancesPoses();
                    cycle.waitForCycleDuration();
                }
            });
 
        task->start();
    }
 
    void
    WorkingMemoryObjectPoseProvider::onDisconnectComponent()
    {
        if (task)
        {
            task->stop();
        }
    }
 
    void
    WorkingMemoryObjectPoseProvider::onExitComponent()
    {
    }
 
    void
    WorkingMemoryObjectPoseProvider::provideObjectInstancesPoses()
    {
        std::scoped_lock lock(mutex);
        std::vector<ObjectInstancePtr> instances = attachments.queryObjects();
        provideObjectInstancesPoses(instances);
    }
 
    void
    WorkingMemoryObjectPoseProvider::provideObjectInstancesPoses(
        const std::vector<ObjectInstancePtr>& objectInstances)
    {
        armarx::objpose::data::ProvidedObjectPoseSeq objectPoses;
 
        for (const auto& instance : objectInstances)
        {
            objectPoses.push_back(toProvidedObjectPose(instance).toIce());
        }
 
        objectPoseTopic->reportObjectPoses(getName(), objectPoses);
    }
 
    armarx::objpose::ProvidedObjectPose
    WorkingMemoryObjectPoseProvider::toProvidedObjectPose(const ObjectInstancePtr& instance)
    {
        armarx::objpose::ProvidedObjectPose pose;
 
        pose.objectType = armarx::objpose::KnownObject;
        std::string className = instance->getMostProbableClass();
        if (auto it = config.objectNames.find(className); it != config.objectNames.end())
        {
            ARMARX_VERBOSE << deactivateSpam(60) << "Replacing class '" << className << "' by '"
                           << it->second << "'";
            className = it->second;
        }
        pose.objectID = armarx::ObjectID(className);
        pose.objectID.setInstanceName(instance->getId());
 
        pose.objectPose = instance->getPose()->toEigen();
        pose.objectPoseFrame = instance->getPose()->getFrame();
        if (pose.objectPoseFrame.empty())
        {
            pose.objectPoseFrame = armarx::GlobalFrame;
        }
 
        pose.confidence = instance->getExistenceCertainty();
        if (instance->hasLocalizationTimestamp())
        {
            fromIce(instance->getLocalizationTimestamp(), pose.timestamp);
        }
        else
        {
            pose.timestamp = armarx::DateTime::Now();
        }
 
        std::optional<ObjectClassWrapper> objectClass = objectClassSegment.getClass(className);
        //ARMARX_IMPORTANT << "Looking for class: " << className;
        if (objectClass)
        {
            VirtualRobot::CollisionModelPtr collisionModel =
                objectClass->manipulationObject->getCollisionModel();
            VirtualRobot::BoundingBox bb = collisionModel->getBoundingBox(false);
            Eigen::Vector3f bbMin = bb.getMin();
            Eigen::Vector3f bbMax = bb.getMax();
            Eigen::Vector3f extents = bbMax - bbMin;
            //ARMARX_IMPORTANT << "Bounding box: " << extents.transpose();
 
            pose.localOOBB = simox::OrientedBoxf(
                (0.5 * (bbMin + bbMax)).eval(), Eigen::Quaternionf::Identity(), extents);
        }
 
        pose.providerName = getName();
 
        return pose;
    }
 
    armarx::objpose::ProviderInfo
    WorkingMemoryObjectPoseProvider::getProviderInfo(const Ice::Current&)
    {
        armarx::objpose::ProviderInfo info;
 
        return info;
    }
 
    void
    WorkingMemoryObjectPoseProvider::attachObjectToRobotNode(
        const AttachObjectToRobotNodeInput& attachment,
        const Ice::Current&)
    {
        std::scoped_lock lock(mutex);
        attachments.attachObjectToRobotNode(attachment);
    }
 
    void
    WorkingMemoryObjectPoseProvider::detachObjectFromRobotNode(
        const DetachObjectFromRobotNodeInput& detachment,
        const Ice::Current&)
    {
        std::scoped_lock lock(mutex);
        attachments.detachObjectFromRobotNode(detachment);
    }
 
} // namespace memoryx
 
ARMARX_REGISTER_COMPONENT_EXECUTABLE(::memoryx::WorkingMemoryObjectPoseProvider,
                                     ::memoryx::WorkingMemoryObjectPoseProvider::GetDefaultName());