Implement observer (WIP)

source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp

@@ -22,9 +22,29 @@
 
 #include "ObjectPoseObserver.h"
 
+#include <SimoxUtility/meta/EnumNames.hpp>
+
+#include <RobotAPI/libraries/core/Pose.h>
+
+
+std::ostream& armarx::objpose::operator<<(std::ostream& os, const ObjectID& id)
+{
+    return os << "'" << id.project << "/" << id.name << "'";
+}
+
+
 
 namespace armarx
 {
+
+    const simox::meta::EnumNames<objpose::ObjectTypeEnum> ObjectTypeEnumNames =
+    {
+        { objpose::ObjectTypeEnum::AnyObject, "AnyObject" },
+        { objpose::ObjectTypeEnum::KnownObject, "KnownObject" },
+        { objpose::ObjectTypeEnum::UnknownObject, "UnknownObject" }
+    };
+
+
     ObjectPoseObserverPropertyDefinitions::ObjectPoseObserverPropertyDefinitions(std::string prefix) :
         armarx::ObserverPropertyDefinitions(prefix)
     {
@@ -34,6 +54,11 @@ namespace armarx
     {
         armarx::PropertyDefinitionsPtr defs(new ObjectPoseObserverPropertyDefinitions(getConfigIdentifier()));
 
+        defs->defineOptionalProperty<std::string>("ObjectPoseTopicName", "ObjectPoseTopic", "Name of the Object Pose Topic.");
+
+        defs->optional(visu.enabled, "visu.enabled", "Enable or disable visualization of objects.");
+        defs->optional(visu.inGlobalFrame, "visu.inGlobalFrame", "If true, show global poses. If false, show poses in robot frame.");
+
         return defs;
     }
 
@@ -45,6 +70,7 @@ namespace armarx
 
     void ObjectPoseObserver::onInitObserver()
     {
+        usingTopicFromProperty("ObjectPoseTopicName");
     }
 
     void ObjectPoseObserver::onConnectObserver()
@@ -60,46 +86,111 @@ namespace armarx
     }
 
 
-    void ObjectPoseObserver::reportProviderInfo(const std::string& providerName, const objpose::ProviderInfo& info, const Ice::Current&)
+    void ObjectPoseObserver::reportProviderAvailable(const std::string& providerName, const Ice::Current&)
     {
+        objpose::ObjectPoseProviderPrx provider = getProviderProxy(providerName);
+        if (!provider)
+        {
+            ARMARX_WARNING << "Received availability signal from provider '" << providerName << "', "
+                           << "but could not get provider proxy.";
+            return;
+        }
+        objpose::ProviderInfo info = provider->getProviderInfo();
+
+        {
+            std::scoped_lock lock(dataMutex);
+            if (providers.count(providerName) == 0)
+            {
+                ARMARX_VERBOSE << "New provider '" << providerName << "' available.";
+            }
+            providers[providerName] = info;
 
+            if (updateCounters.count(providerName) == 0)
+            {
+                updateCounters[providerName] = 0;
+            }
+        }
+
+        if (!existsChannel(providerName))
+        {
+            offerChannel(providerName, "Channel of provider '" + providerName + "'.");
+        }
+        offerOrUpdateDataField(providerName, "objectType", ObjectTypeEnumNames.to_name(info.objectType), "");
     }
 
-    void ObjectPoseObserver::reportObjectPoses(const std::string& providerName, const objpose::ObjectPoseSeq& candidates, const Ice::Current&)
+    void ObjectPoseObserver::reportObjectPoses(const std::string& providerName, const objpose::ObjectPoseSeq& objectPoses, const Ice::Current&)
     {
-
+        std::scoped_lock lock(dataMutex);
+        ARMARX_INFO << "received object poses from " << providerName;
+        this->objectPoses[providerName] = objectPoses;
+        handleProviderUpdate(providerName);
     }
 
-    objpose::InfoMap ObjectPoseObserver::getAvailableProvidersWithInfo(const Ice::Current&)
+
+    objpose::ObjectPoseSeq ObjectPoseObserver::getObjectPoses(const Ice::Current&)
     {
         return {};
     }
 
-    Ice::StringSeq ObjectPoseObserver::getAvailableProviderNames(const Ice::Current&)
+    objpose::ObjectPoseSeq ObjectPoseObserver::getObjectPosesByProvider(const std::string& providerName, const Ice::Current&)
     {
         return {};
     }
 
-    objpose::ProviderInfo ObjectPoseObserver::getProviderInfo(const std::string& providerName, const Ice::Current&)
+
+    void ObjectPoseObserver::requestObjects(const objpose::ObjectIDSeq& objectIDs, Ice::Long relativeTimeoutMS, const Ice::Current&)
+    {
+        std::map<std::string, objpose::ObjectIDSeq> requests;
+
+        {
+            std::scoped_lock lock(dataMutex);
+            for (const auto& objectID : objectIDs)
+            {
+                for (const auto& [name, info] : providers)
+                {
+                    // ToDo: optimize look up by using sets.
+                    if (std::find(info.supportedObjects.begin(), info.supportedObjects.end(), objectID) != info.supportedObjects.end())
+                    {
+                        requests[name].push_back(objectID);
+                    }
+                }
+            }
+        }
+
+        for (const auto& [providerName, objects] : requests)
+        {
+            objpose::ObjectPoseProviderPrx provider = getProviderProxy(providerName);
+            if (provider)
+            {
+                ARMARX_VERBOSE << "Requesting provider '" << providerName << "' for objects " << objects;
+                provider->requestObjects(objects, relativeTimeoutMS);
+            }
+        }
+    }
+
+
+    objpose::ProviderInfoMap ObjectPoseObserver::getAvailableProvidersWithInfo(const Ice::Current&)
     {
         return {};
     }
 
-    bool ObjectPoseObserver::hasProvider(const std::string& providerName, const Ice::Current&)
+    Ice::StringSeq ObjectPoseObserver::getAvailableProviderNames(const Ice::Current&)
     {
         return {};
     }
 
-    objpose::ObjectPoseSeq ObjectPoseObserver::getObjectPoses(const Ice::Current&)
+    objpose::ProviderInfo ObjectPoseObserver::getProviderInfo(const std::string& providerName, const Ice::Current&)
     {
         return {};
     }
 
-    objpose::ObjectPoseSeq ObjectPoseObserver::getObjectPosesByProvider(const std::string& providerName, const Ice::Current&)
+    bool ObjectPoseObserver::hasProvider(const std::string& providerName, const Ice::Current&)
     {
         return {};
     }
 
+
+
     Ice::Int ObjectPoseObserver::getUpdateCounterByProvider(const std::string& providerName, const Ice::Current&)
     {
         return {};
@@ -110,9 +201,63 @@ namespace armarx
         return {};
     }
 
-    bool ObjectPoseObserver::setProviderConfig(const std::string& providerName, const StringVariantBaseMap& config, const Ice::Current&)
+
+    void ObjectPoseObserver::handleProviderUpdate(const std::string& providerName)
     {
-        return {};
+        // Initialized to 0 on first access.
+        updateCounters[providerName]++;
+        if (providers.count(providerName) == 0)
+        {
+            providers[providerName] = objpose::ProviderInfo();
+        }
+
+        if (!existsChannel(providerName))
+        {
+            offerChannel(providerName, "Channel of provider '" + providerName + "'.");
+        }
+        offerOrUpdateDataField(providerName, "updateCounter", Variant(updateCounters.at(providerName)), "Counter incremented for each update.");
+        offerOrUpdateDataField(providerName, "candidateCount", Variant(int(objectPoses.at(providerName).size())), "Number of provided object poses.");
+
+        if (visu.enabled)
+        {
+            visProviderUpdate(providerName);
+        }
+    }
+
+
+    void ObjectPoseObserver::visProviderUpdate(const std::string& providerName)
+    {
+        viz::Layer layer = arviz.layer(providerName);
+
+        for (const objpose::ObjectPose& objectPose : objectPoses.at(providerName))
+        {
+            const objpose::ObjectID id = objectPose.objectID;
+            std::string key = id.project + "/" + id.name;
+
+            std::optional<ObjectInfo> objectInfo = objectFinder.findObject(id.project, id.name);
+            if (!objectInfo)
+            {
+                ARMARX_WARNING << "Cannot visualize object '" << key << "'.";
+                continue;
+            }
+
+            Eigen::Matrix4f pose = armarx::PosePtr::dynamicCast(objectPose.objectPose)->toEigen();
+            if (visu.inGlobalFrame)
+            {
+                // pose = armarx::PosePtr::dynamicCast(objectPose.robotPose)->toEigen() * pose;
+            }
+
+            layer.add(viz::Object(id.project + "/" + id.name)
+                      .file("", objectInfo->getSimoxXML()).pose(pose));
+        }
+
+        arviz.commit(layer);
+    }
+
+    objpose::ObjectPoseProviderPrx ObjectPoseObserver::getProviderProxy(const std::string& providerName)
+    {
+        return getProxy<objpose::ObjectPoseProviderPrx>(providerName, false, "", false);
     }
 
 }
+

source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.h

@@ -22,15 +22,24 @@
 
 #pragma once
 
+#include <mutex>
 
 #include <ArmarXCore/observers/Observer.h>
 
 #include <RobotAPI/interface/objectpose/ObjectPoseObserver.h>
 #include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
 
+#include "ObjectFinder.h"
+
 #define ICE_CURRENT_ARG const Ice::Current& = Ice::emptyCurrent
 
 
+namespace armarx::objpose
+{
+    std::ostream& operator<<(std::ostream& os, const ObjectID& id);
+}
+
+
 namespace armarx
 {
     /**
@@ -38,7 +47,7 @@ namespace armarx
      * @brief Property definitions of `ObjectPoseObserver`.
      */
     class ObjectPoseObserverPropertyDefinitions :
-            public ObserverPropertyDefinitions
+        public ObserverPropertyDefinitions
     {
     public:
         ObjectPoseObserverPropertyDefinitions(std::string prefix);
@@ -58,9 +67,9 @@ namespace armarx
      * Detailed description of class ObjectPoseObserver.
      */
     class ObjectPoseObserver :
-            virtual public Observer
-            , virtual public objpose::ObjectPoseObserverInterface
-            , virtual public armarx::ArVizComponentPluginUser
+        virtual public Observer
+        , virtual public objpose::ObjectPoseObserverInterface
+        , virtual public armarx::ArVizComponentPluginUser
     {
     public:
 
@@ -70,22 +79,25 @@ namespace armarx
 
         // ObjectPoseTopic interface
     public:
-        void reportProviderInfo(const std::string& providerName, const objpose::ProviderInfo& info, ICE_CURRENT_ARG) override;
-        void reportObjectPoses(const std::string& providerName, const objpose::ObjectPoseSeq& candidates, ICE_CURRENT_ARG) override;
+        void reportProviderAvailable(const std::string& providerName, ICE_CURRENT_ARG) override;
+        void reportObjectPoses(const std::string& providerName, const objpose::ObjectPoseSeq& objectPoses, ICE_CURRENT_ARG) override;
 
         // ObjectPoseObserverInterface interface
     public:
-        objpose::InfoMap getAvailableProvidersWithInfo(ICE_CURRENT_ARG) override;
+
+        objpose::ObjectPoseSeq getObjectPoses(ICE_CURRENT_ARG) override;
+        objpose::ObjectPoseSeq getObjectPosesByProvider(const std::string& providerName, ICE_CURRENT_ARG) override;
+
+        void requestObjects(const objpose::ObjectIDSeq& objectIDs, Ice::Long relativeTimeoutMS, ICE_CURRENT_ARG) override;
+
         Ice::StringSeq getAvailableProviderNames(ICE_CURRENT_ARG) override;
         objpose::ProviderInfo getProviderInfo(const std::string& providerName, ICE_CURRENT_ARG) override;
+        objpose::ProviderInfoMap getAvailableProvidersWithInfo(ICE_CURRENT_ARG) override;
         bool hasProvider(const std::string& providerName, ICE_CURRENT_ARG) override;
 
-        objpose::ObjectPoseSeq getObjectPoses(ICE_CURRENT_ARG) override;
-        objpose::ObjectPoseSeq getObjectPosesByProvider(const std::string& providerName, ICE_CURRENT_ARG) override;
 
         Ice::Int getUpdateCounterByProvider(const std::string& providerName, ICE_CURRENT_ARG) override;
         StringIntDictionary getAllUpdateCounters(ICE_CURRENT_ARG) override;
-        bool setProviderConfig(const std::string& providerName, const StringVariantBaseMap& config, ICE_CURRENT_ARG) override;
 
 
     protected:
@@ -98,9 +110,34 @@ namespace armarx
 
         armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
 
+    private:
+
+        void handleProviderUpdate(const std::string& providerName);
+        void visProviderUpdate(const std::string& providerName);
+
+        objpose::ObjectPoseProviderPrx getProviderProxy(const std::string& providerName);
+
 
     private:
 
+        Ice::StringSeq getAvailableProviderNames();
+
+
+        std::mutex dataMutex;
+        objpose::ProviderInfoMap providers;
+
+        std::map<std::string, objpose::ObjectPoseSeq> objectPoses;
+        std::map<std::string, int> updateCounters;
+
+
+        ObjectFinder objectFinder;
+
+        struct Visu
+        {
+            bool enabled = false;
+            bool inGlobalFrame = true;
+        };
+        Visu visu;
 
     };