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 <Eigen/Geometry>
 
#include <pcl/point_types.h>
 
#include <opencv2/core/mat.hpp>
 
#include <IceUtil/Time.h>
 
#include <SimoxUtility/math/pose/pose.h>
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/VirtualRobot.h>
#include <VirtualRobot/XML/RobotIO.h>
 
#include "ArmarXCore/core/time/Clock.h"
#include <ArmarXCore/core/PackagePath.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
#include <ArmarXCore/core/time/CycleUtil.h>
 
#include "RobotAPI/libraries/ArmarXObjects/ObjectID.h"
#include <RobotAPI/libraries/armem/core/Time.h>
#include <RobotAPI/libraries/armem_objects/aron/FamiliarObjectInstance.aron.generated.h>
#include <RobotAPI/libraries/armem_objects/types.h>
// #include <RobotAPI/libraries/armem_objects/aron_conversions.h>
// #include <RobotAPI/libraries/armem/core/aron_conversions.h>
#include <RobotAPI/libraries/ArmarXObjects/aron_conversions.h>
 
namespace armarx::familiar_objects
{
    FamiliarObjectDetectionExample::FamiliarObjectDetectionExample()
    {
        addPlugin(familiarObjectInstanceReaderPlugin);
 
        p.exemplaryFamiliarObjectID.dataset = "myDataset";
        p.exemplaryFamiliarObjectID.className = "sphere";
        p.exemplaryFamiliarObjectID.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->component(familiarObjectPoseStoragePrx, "ObjectMemory");
 
        return defs;
    }
 
    std::string
    FamiliarObjectDetectionExample::getDefaultName() const
    {
        return "FamiliarObjectDetectionExample";
    }
 
    void
    FamiliarObjectDetectionExample::onInitComponent()
    {
    }
 
    void
    FamiliarObjectDetectionExample::onConnectComponent()
    {
        //
        //  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();
    }
 
    void
    FamiliarObjectDetectionExample::storeExemplaryFamiliarObjectInMemory() const
    {
 
        ::armarx::objpose::ProvidedFamiliarObjectPoseSeq data;
 
        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 int numPoints = 100;
 
        // x in red
        for (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 (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 (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);
 
        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()
    {
        // task->stop();
    }
 
    void
    FamiliarObjectDetectionExample::onExitComponent()
    {
    }
 
 
} // namespace armarx::familiar_objects