From 8d9719318999ef0da376432e67e519c9828ce6bb Mon Sep 17 00:00:00 2001
From: Rainer Kartmann <rainer.kartmann@kit.edu>
Date: Thu, 14 May 2020 09:09:25 +0200
Subject: [PATCH] Implement observer (WIP)
---
.../ObjectPoseObserver/ObjectPoseObserver.cpp | 167 ++++++++++++++++--
.../ObjectPoseObserver/ObjectPoseObserver.h | 57 ++++--
2 files changed, 203 insertions(+), 21 deletions(-)
diff --git a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp
index 4bfd62e21..cc982097f 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp
+++ b/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);
}
}
+
diff --git a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.h b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.h
index e00b4bc45..4b9d84788 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.h
+++ b/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;
};
--
GitLab