FamiliarObjectDetectionExample.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/>.
 *
 * @author     Fabian Reister ( fabian dot reister at kit dot edu )
 * @date       2024
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "FamiliarObjectDetectionExample.h"
 
#include <map>
#include <optional>
#include <string>
#include <vector>
 
#include <Eigen/Geometry>
 
#include <pcl/point_types.h>
#include <VirtualRobot/ManipulationObject.h>
 
#include "ArmarXCore/core/time/Clock.h"
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
#include <ArmarXCore/core/util/StringHelpers.h>
 
#include "RobotAPI/libraries/ArmarXObjects/ObjectFinder.h"
#include "RobotAPI/libraries/ArmarXObjects/ObjectID.h"
#include "RobotAPI/libraries/ArmarXObjects/aron_conversions/armarx.h"
#include "RobotAPI/libraries/armem_objects/client/familiar_object_instance/ObjectReader.h"
#include "RobotAPI/libraries/armem_objects/utils.h"
#include <RobotAPI/interface/objectpose/FamiliarObjectPoseStorageInterface.h>
#include <RobotAPI/libraries/armem_objects/aron/FamiliarObjectInstance.aron.generated.h>
 
namespace armarx::familiar_objects
{
 
    FamiliarObjectDetectionExample::FamiliarObjectDetectionExample()
    {
        addPlugin(familiarObjectInstanceReaderPlugin);
 
        p.exemplaryFamiliarObjectID.dataset = "myDataset";
        p.exemplaryFamiliarObjectID.className = "sphere";
        p.exemplaryFamiliarObjectID.instanceName = "0";
 
        p.knownObjectID.dataset = "myDataset";
        p.knownObjectID.className = "sphere";
        p.knownObjectID.instanceName = "0";
    }
 
    armarx::PropertyDefinitionsPtr
    FamiliarObjectDetectionExample::createPropertyDefinitions()
    {
        armarx::PropertyDefinitionsPtr defs =
            new ComponentPropertyDefinitions(getConfigIdentifier());
 
        defs->required(p.robotName, "p.robotName");
 
        defs->optional(p.exemplaryFamiliarObjectID.dataset, "p.objectId.dataset");
        defs->optional(p.exemplaryFamiliarObjectID.className, "p.objectId.className");
        defs->optional(p.exemplaryFamiliarObjectID.instanceName, "p.objectId.instanceName");
 
        defs->optional(p.knownObjectID.dataset, "p.knownObjectID.dataset");
        defs->optional(p.knownObjectID.className, "p.knownObjectID.className");
        defs->optional(p.knownObjectID.instanceName, "p.knownObjectID.instanceName");
 
        defs->optional(p.knownObjectAsFamID.dataset, "p.knownObjectAsFamID.dataset");
        defs->optional(p.knownObjectAsFamID.className, "p.knownObjectAsFamID.className");
        defs->optional(p.knownObjectAsFamID.instanceName, "p.knownObjectAsFamID.instanceName");
 
        defs->optional(p.objectShiftX, "p.objectShiftX", "");
        defs->optional(p.objectShiftY, "p.objectShiftY", "");
        defs->optional(p.nFamObjects, "p.nFamObjects", "Number of familiar objects to create");            
 
        defs->component(familiarObjectPoseStoragePrx, "ObjectMemory");
 
        return defs;
    }
 
    std::string
    FamiliarObjectDetectionExample::getDefaultName() const
    {
        return "FamiliarObjectDetectionExample";
    }
 
    void
    FamiliarObjectDetectionExample::onInitComponent()
    {
    }
 
    void
    FamiliarObjectDetectionExample::run()
    {
        //
        //  First, we create a familiar object and send it to the memory.
        //
 
        ARMARX_IMPORTANT << "Storing exemplary familiar object in memory";
        storeExemplaryFamiliarObjectInMemory();
 
        //
        // Then, we read the familiar object from the memory.
        //
        ARMARX_IMPORTANT << "Reading familiar object from memory";
        readExemplaryFamiliarObjectFromMemory();
 
        ARMARX_IMPORTANT << "Storing known object as familiar object in memory";
        storeKnownObjectAsFamiliarObjectInMemory();
    }
 
    void
    FamiliarObjectDetectionExample::onConnectComponent()
    {
        detectTask =
            new PeriodicTask<FamiliarObjectDetectionExample>(this,
                                                             &FamiliarObjectDetectionExample::run,
                                                             1'000); // every 5 seconds
 
        detectTask->start();
    }
 
    void
    FamiliarObjectDetectionExample::storeKnownObjectAsFamiliarObjectInMemory() const
    {
        const auto client = getClient();
 
        armarx::ObjectID knownObjectID;
        fromAron(p.knownObjectID, knownObjectID);
        const auto knownObjectPose = client.fetchObjectPose(knownObjectID);
 
        ARMARX_CHECK(knownObjectPose.has_value())
            << "Known object " << QUOTED(knownObjectID) << " not found in ObjectMemory.";
 
        ObjectFinder finder;
        auto manipulationObject = finder.loadManipulationObject(knownObjectPose.value());
        ARMARX_CHECK_NOT_NULL(manipulationObject)
            << "Could not load ManipulationObject for known object " << QUOTED(knownObjectID.str())
            << ".";
 
        manipulationObject->setGlobalPose(knownObjectPose->objectPoseGlobal);
 
        ObjectID famObjectID;
        fromAron(p.knownObjectAsFamID, famObjectID);
 
        const auto familiarObject =
            armem::objects::createFamiliarObjectInstanceFromManipulationObject(*manipulationObject,
                                                                               famObjectID);
 
        ::armarx::objpose::ProvidedFamiliarObjectPoseSeq data;
 
        for (unsigned int i = 0; i < p.nFamObjects; i++)
        {
            auto famObj = familiarObject;
            famObj.objectID.instanceName = std::to_string(i);
 
            // shift each object by 50mm in x direction
            famObj.poseGlobal->pose.block(0, 3, 3, 1) +=
                Eigen::Vector3f(static_cast<float>(i) * p.objectShiftX, static_cast<float>(i) * p.objectShiftY, 0.0f);
 
            data.push_back(famObj.toAronDTO());
        }
 
        // data.push_back(familiarObject.toAronDTO());
 
        ARMARX_INFO << "Sending " << data.size() << " familiar object(s) to the memory";
        familiarObjectPoseStoragePrx->reportFamiliarObjectPoses(getName(), data);
    }
 
    void
    FamiliarObjectDetectionExample::storeExemplaryFamiliarObjectInMemory() const
    {
        armem::arondto::FamiliarObjectInstance familiarObject;
 
        familiarObject.timestamp = armarx::Clock::Now();
 
        familiarObject.poseSensFrame.pose =
            Eigen::Isometry3f{Eigen::Translation3f{0, 0, 1000}}.matrix();
        familiarObject.poseSensFrame.header.frame = "DepthCameraSim";
        familiarObject.poseSensFrame.header.agent = p.robotName;
        // familiarObject.poseSensFrame.header.frame = "AzureKinect_RGB";
 
 
        familiarObject.objectID = p.exemplaryFamiliarObjectID;
 
        familiarObject.confidence = 1.0;
 
        // familiarObject.bounding_box =
 
        // familiarObject.depth_image_patch = cv::Mat1f(cv::Size(20, 20));
        // familiarObject.rgb_image_patch = cv::Mat3b(cv::Size(20, 20));
 
        const unsigned int numPoints = 100;
 
        // x in red
        for (unsigned int i = 0; i < numPoints; i++)
        {
            pcl::PointXYZRGBA point;
            point.x = static_cast<float>(i) - numPoints / 2;
            point.y = 0;
            point.z = 0;
            point.r = 255;
            point.g = 0;
            point.b = 0;
            familiarObject.points.points.push_back(point);
        }
 
        // y in green
        for (unsigned int i = 0; i < numPoints; i++)
        {
            pcl::PointXYZRGBA point;
            point.x = 0;
            point.y = static_cast<float>(i) - numPoints / 2;
            point.z = 0;
            point.r = 0;
            point.g = 255;
            point.b = 0;
            familiarObject.points.points.push_back(point);
        }
 
        // z in blue
        for (unsigned int i = 0; i < numPoints; i++)
        {
            pcl::PointXYZRGBA point;
            point.y = 0;
            point.y = 0;
            point.z = static_cast<float>(i) - numPoints / 2;
            point.r = 0;
            point.g = 0;
            point.b = 255;
            familiarObject.points.points.push_back(point);
        }
 
        familiarObject.points.header.frame_id = "DepthCameraSim";
        familiarObject.points.is_dense = true;
        familiarObject.points.width = familiarObject.points.points.size();
        familiarObject.points.height = 1;
 
        familiarObject.bounding_box.center.setZero();
        familiarObject.bounding_box.extents.setConstant(numPoints);
 
        ::armarx::objpose::ProvidedFamiliarObjectPoseSeq data;
        data.push_back(familiarObject.toAronDTO());
 
        ARMARX_INFO << "Sending " << data.size() << " familiar object to the memory";
        familiarObjectPoseStoragePrx->reportFamiliarObjectPoses(getName(), data);
    }
 
    void
    FamiliarObjectDetectionExample::readExemplaryFamiliarObjectFromMemory() const
    {
        ARMARX_CHECK_NOT_NULL(familiarObjectInstanceReaderPlugin);
 
        const armem::obj::familiar_object_instance::Reader& familiarObjectInstanceReader =
            familiarObjectInstanceReaderPlugin->get();
 
 
        //
        // 1. Read from all providers
        //
        ARMARX_IMPORTANT << "Reading from all providers";
        {
 
            const auto allFamiliarObjectInstances =
                familiarObjectInstanceReader.queryAllLatestFamiliarObjectInstances();
 
            // print
            for (const auto& [providerName, instances] : allFamiliarObjectInstances)
            {
                ARMARX_INFO << "Provider name: " << providerName;
                for (const auto& instance : instances)
                {
                    ARMARX_INFO << "- Instance: " << instance.objectID.dataset << "/"
                                << instance.objectID.className << "/"
                                << instance.objectID.instanceName;
                }
            }
        }
 
        //
        // 2. Read from a specific provider
        //
        ARMARX_IMPORTANT << "Reading from a specific provider";
        {
            const std::map<std::string, std::vector<armem::arondto::FamiliarObjectInstance>>
                familiarObjectInstances =
                    familiarObjectInstanceReader.queryAllLatestFamiliarObjectInstances(getName());
 
            ARMARX_INFO << "Provider name: " << getName();
 
            ARMARX_CHECK_EQUAL(familiarObjectInstances.size(), 1);
            ARMARX_CHECK_EQUAL(familiarObjectInstances.begin()->first, getName());
 
            const auto& thisFamiliarObjectInstances = familiarObjectInstances.begin()->second;
            // print
            for (const auto& instance : thisFamiliarObjectInstances)
            {
                ARMARX_INFO << "- Instance: " << instance.objectID.dataset << "/"
                            << instance.objectID.className << "/" << instance.objectID.instanceName;
            }
        }
 
        //
        // 3. Read all instances of a specific class
        //
        ARMARX_IMPORTANT << "Reading all instances of a specific class";
        {
            armarx::ObjectID objectId;
            fromAron(p.exemplaryFamiliarObjectID, objectId);
 
            const auto instances =
                familiarObjectInstanceReader.queryLatestFamiliarObjectInstancesFromClass(
                    objectId.getClassID());
 
            for (const auto& [instanceName, instancesForProvider] : instances)
            {
                for (const auto& instance : instancesForProvider)
                {
                    ARMARX_INFO << "- Instance: " << instance.objectID.dataset << "/"
                                << instance.objectID.className << "/"
                                << instance.objectID.instanceName;
                }
            }
        }
 
        //
        // 4. Read a specific instance
        //
        ARMARX_IMPORTANT << "Reading a specific instance";
        {
            armarx::ObjectID objectId;
            fromAron(p.exemplaryFamiliarObjectID, objectId);
 
            const std::optional<std::map<std::string, armem::arondto::FamiliarObjectInstance>>
                instances =
                    familiarObjectInstanceReader.queryLatestFamiliarObjectInstance(objectId);
 
            ARMARX_CHECK(instances.has_value());
 
            for (const auto& [instanceName, instance] : instances.value())
            {
                ARMARX_INFO << "- Instance: " << instance.objectID.dataset << "/"
                            << instance.objectID.className << "/" << instance.objectID.instanceName;
            }
        }
    }
 
    void
    FamiliarObjectDetectionExample::onDisconnectComponent()
    {
        if (detectTask)
        {
            detectTask->stop();
            detectTask = nullptr;
        }
    }
 
    void
    FamiliarObjectDetectionExample::onExitComponent()
    {
    }
 
 
} // namespace armarx::familiar_objects