added Grasp Candidate Observer to VariantInfo

data/RobotAPI/VariantInfo-RobotAPI.xml

@@ -228,12 +228,23 @@
         </Topic>
 
         <Topic include="RobotAPI/interface/speech/SpeechInterface.h"
-            humanName="Text to Speech"
+            humanName="Text to Speech Topic"
             typeName="TextListenerInterfacePrx"
             memberName="textToSpeech"
             getterName="getTextToSpeech"
             propertyName="TextToSpeechTopicName"
             propertyIsOptional="true"
             propertyDefaultValue="TextToSpeech" />
+
+
+        <Proxy include="RobotAPI/interface/units/GraspCandidateObserverInterface.h"
+            humanName="Grasp Candidate Observer"
+            typeName="GraspCandidateObserverInterfacePrx"
+            memberName="graspCandidateObserver"
+            getterName="getGraspCandidateObserver"
+            propertyName="GraspCandidateObserverName"
+            propertyIsOptional="true"
+            propertyDefaultValue="GraspCandidateObserver" />
+
     </Lib>
 </VariantInfo>

source/RobotAPI/applications/CMakeLists.txt

@@ -42,4 +42,5 @@ add_subdirectory(DummyTextToSpeech)
 add_subdirectory(ProsthesisObserver)
 add_subdirectory(CyberGloveObserver)
 add_subdirectory(RobotHealth)
-add_subdirectory(RobotHealthDummy)
\ No newline at end of file
+add_subdirectory(RobotHealthDummy)
+add_subdirectory(GraspCandidateObserver)
\ No newline at end of file

source/RobotAPI/applications/GraspCandidateObserver/CMakeLists.txt

+armarx_component_set_name("GraspCandidateObserverApp")
+
+
+find_package(Eigen3 QUIET)
+armarx_build_if(Eigen3_FOUND "Eigen3 not available")
+
+if (Eigen3_FOUND)
+    include_directories(SYSTEM ${Eigen3_INCLUDE_DIR})
+endif()
+
+find_package(Simox ${ArmarX_Simox_VERSION} QUIET)
+armarx_build_if(Simox_FOUND "Simox-VirtualRobot not available")
+if(Simox_FOUND)
+    include_directories(${Simox_INCLUDE_DIRS})
+endif()
+
+set(COMPONENT_LIBS
+    RobotAPIUnits
+    ArmarXCoreInterfaces
+    ArmarXCore
+)
+
+set(EXE_SOURCE main.cpp)
+
+armarx_add_component_executable("${EXE_SOURCE}")

source/RobotAPI/applications/GraspCandidateObserver/main.cpp

+/*
+ * 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    RobotAPI::application::GraspCandidateObserver
+ * @author     Simon Ottenhaus ( simon dot ottenhaus at kit dot edu )
+ * @date       2019
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#include <RobotAPI/components/units/GraspCandidateObserver.h>
+
+#include <ArmarXCore/core/application/Application.h>
+#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/logging/Logging.h>
+
+int main(int argc, char* argv[])
+{
+    return armarx::runSimpleComponentApp < armarx::GraspCandidateObserver > (argc, argv, "GraspCandidateObserver");
+}

source/RobotAPI/components/units/GraspCandidateObserver.cpp

@@ -44,7 +44,7 @@ GraspCandidateObserver::GraspCandidateObserver()
 
 void GraspCandidateObserver::onInitObserver()
 {
-    usingTopic(getProperty<std::string>("GraspCandidateTopicName").getValue());
+    usingTopic(getProperty<std::string>("GraspCandidatesTopicName").getValue());
     offeringTopic(getProperty<std::string>("ConfigTopicName").getValue());
 
 
@@ -52,7 +52,7 @@ void GraspCandidateObserver::onInitObserver()
 
 void GraspCandidateObserver::onConnectObserver()
 {
-    configTopic = getTopic<GraspCandidateProviderConfigListenerPrx>(getProperty<std::string>("ConfigTopicName").getValue());
+    configTopic = getTopic<GraspCandidateProviderInterfacePrx>(getProperty<std::string>("ConfigTopicName").getValue());
 }
 
 PropertyDefinitionsPtr GraspCandidateObserver::createPropertyDefinitions()
@@ -67,28 +67,60 @@ bool GraspCandidateObserver::FilterMatches(const CandidateFilterConditionPtr& fi
     return true;
 }
 
+std::string GraspCandidateObserver::ObjectTypeToString(ObjectTypeEnum type)
+{
+    switch (type)
+    {
+        case grasping::AnyObject:
+            return "AnyObject";
+        case grasping::KnownObject:
+            return "KnownObject";
+        case grasping::UnknownObject:
+            return "UnknownObject";
+        default:
+            return "ERROR";
+    }
+}
+
 void GraspCandidateObserver::reportGraspCandidates(const std::string& providerName, const GraspCandidateSeq& candidates, const Ice::Current&)
 {
     ScopedLock lock(dataMutex);
     this->candidates[providerName] = candidates;
-    if(updateCounters.count(providerName) == 0)
+    if (updateCounters.count(providerName) == 0)
     {
         updateCounters[providerName] = 0;
     }
-    if(providers.count(providerName) == 0)
+    updateCounters[providerName]++;
+    if (providers.count(providerName) == 0)
     {
         providers[providerName] = new ProviderInfo();
     }
+
+
+    if (!existsChannel(providerName))
+    {
+        offerChannel(providerName, "Channel of " + providerName);
+    }
+    offerOrUpdateDataField(providerName, "updateCounter", Variant(updateCounters[providerName]), "Counter that increases for each update");
+    offerOrUpdateDataField(providerName, "candidateCount", Variant((int)candidates.size()), "Number of provided candiates");
+
 }
 
 void GraspCandidateObserver::reportProviderInfo(const std::string& providerName, const ProviderInfoPtr& info, const Ice::Current&)
 {
     ScopedLock lock(dataMutex);
     providers[providerName] = info;
-    if(updateCounters.count(providerName) == 0)
+    if (updateCounters.count(providerName) == 0)
     {
         updateCounters[providerName] = 0;
     }
+
+
+    if (!existsChannel(providerName))
+    {
+        offerChannel(providerName, "Channel of " + providerName);
+    }
+    offerOrUpdateDataField(providerName, "", ObjectTypeToString(info->objectType), "");
 }
 
 InfoMap GraspCandidateObserver::getAvailableProvidersWithInfo(const Ice::Current&)
@@ -124,22 +156,22 @@ GraspCandidateSeq GraspCandidateObserver::getAllCandidates(const Ice::Current&)
 {
     ScopedLock lock(dataMutex);
     GraspCandidateSeq all;
-    for(const std::pair<std::string, grasping::GraspCandidateSeq>& pair : candidates)
+    for (const std::pair<std::string, grasping::GraspCandidateSeq>& pair : candidates)
     {
         all.insert(all.end(), pair.second.begin(), pair.second.end());
     }
     return all;
 }
 
-GraspCandidateSeq GraspCandidateObserver::getCandidatesByFilter(const CandidateFilterConditionPtr&filter, const Ice::Current&)
+GraspCandidateSeq GraspCandidateObserver::getCandidatesByFilter(const CandidateFilterConditionPtr& filter, const Ice::Current&)
 {
     ScopedLock lock(dataMutex);
     GraspCandidateSeq matching;
-    for(const std::pair<std::string, grasping::GraspCandidateSeq>& pair : candidates)
+    for (const std::pair<std::string, grasping::GraspCandidateSeq>& pair : candidates)
     {
-        for(const grasping::GraspCandidatePtr& candidate : pair.second)
+        for (const grasping::GraspCandidatePtr& candidate : pair.second)
         {
-            if(FilterMatches(filter, candidate))
+            if (FilterMatches(filter, candidate))
             {
                 matching.push_back(candidate);
             }
@@ -164,7 +196,7 @@ IntMap GraspCandidateObserver::getAllUpdateCounters(const Ice::Current& provider
 bool GraspCandidateObserver::setProviderConfig(const std::string& providerName, const ConfigMap& config, const Ice::Current&)
 {
     ScopedLock lock(dataMutex);
-    if(providers.count(providerName) == 0)
+    if (providers.count(providerName) == 0)
     {
         return false;
     }
@@ -179,7 +211,7 @@ bool GraspCandidateObserver::hasProvider(const std::string& providerName)
 }
 void GraspCandidateObserver::checkHasProvider(const std::string& providerName)
 {
-    if(!hasProvider(providerName))
+    if (!hasProvider(providerName))
     {
         throw LocalException("Unknown provider name '") << providerName << "'. Available providers: " << getAvailableProviderNames();
     }
@@ -187,7 +219,7 @@ void GraspCandidateObserver::checkHasProvider(const std::string& providerName)
 StringSeq GraspCandidateObserver::getAvailableProviderNames()
 {
     StringSeq names;
-    for(const std::pair<std::string, ProviderInfoPtr>& pair : providers)
+    for (const std::pair<std::string, ProviderInfoPtr>& pair : providers)
     {
         names.push_back(pair.first);
     }

source/RobotAPI/components/units/GraspCandidateObserver.h

@@ -39,9 +39,8 @@ namespace armarx
         GraspCandidateObserverPropertyDefinitions(std::string prefix):
             ObserverPropertyDefinitions(prefix)
         {
-            defineOptionalProperty<std::string>("GraspCandidateTopicName", "GraspCandidateUpdates", "Name of the Grasp Candidate Topic");
-            defineOptionalProperty<std::string>("ConfigTopicName", "GraspCandidateProviderConfigUpdates", "Name of the Grasp Candidate Topic");
-
+            defineOptionalProperty<std::string>("GraspCandidatesTopicName", "GraspCandidatesTopic", "Name of the Grasp Candidate Topic");
+            defineOptionalProperty<std::string>("ConfigTopicName", "GraspCandidateProviderConfigTopic", "Name of the Grasp Candidate Provider Config Topic");
         }
     };
 
@@ -71,36 +70,37 @@ namespace armarx
 
     public:
         static bool FilterMatches(const grasping::CandidateFilterConditionPtr& filter, const grasping::GraspCandidatePtr& candidate);
+        static std::string ObjectTypeToString(grasping::ObjectTypeEnum type);
 
         // GraspCandidateProviderListener interface
     public:
-        void reportGraspCandidates(const std::string&providerName, const grasping::GraspCandidateSeq&candidates, const Ice::Current&) override;
-        void reportProviderInfo(const std::string&providerName, const grasping::ProviderInfoPtr&info, const Ice::Current&) override;
+        void reportGraspCandidates(const std::string& providerName, const grasping::GraspCandidateSeq& candidates, const Ice::Current&) override;
+        void reportProviderInfo(const std::string& providerName, const grasping::ProviderInfoPtr& info, const Ice::Current&) override;
 
         // GraspCandidateObserverInterface interface
     public:
         grasping::InfoMap getAvailableProvidersWithInfo(const Ice::Current&) override;
         grasping::StringSeq getAvailableProviderNames(const Ice::Current&) override;
-        grasping::ProviderInfoPtr getProviderInfo(const std::string&providerName, const Ice::Current&) override;
-        bool hasProvider(const std::string&providerName, const Ice::Current&c) override;
+        grasping::ProviderInfoPtr getProviderInfo(const std::string& providerName, const Ice::Current&) override;
+        bool hasProvider(const std::string& providerName, const Ice::Current& c) override;
         grasping::GraspCandidateSeq getAllCandidates(const Ice::Current&) override;
         grasping::GraspCandidateSeq getCandidatesByFilter(const grasping::CandidateFilterConditionPtr& filter, const Ice::Current&) override;
-        Ice::Int getUpdateCounterByProvider(const std::string&providerName, const Ice::Current&) override;
-        grasping::IntMap getAllUpdateCounters(const Ice::Current&providerName) override;
-        bool setProviderConfig(const std::string&providerName, const grasping::ConfigMap&config, const Ice::Current&) override;
+        Ice::Int getUpdateCounterByProvider(const std::string& providerName, const Ice::Current&) override;
+        grasping::IntMap getAllUpdateCounters(const Ice::Current& providerName) override;
+        bool setProviderConfig(const std::string& providerName, const grasping::ConfigMap& config, const Ice::Current&) override;
 
 
 
     private:
-        bool hasProvider(const std::string&providerName);
-        void checkHasProvider(const std::string&providerName);
+        bool hasProvider(const std::string& providerName);
+        void checkHasProvider(const std::string& providerName);
         grasping::StringSeq getAvailableProviderNames();
         Mutex dataMutex;
         std::map<std::string, grasping::GraspCandidateSeq> candidates;
         grasping::InfoMap providers;
         std::map<std::string, int> updateCounters;
 
-        grasping::GraspCandidateProviderConfigListenerPrx configTopic;
+        grasping::GraspCandidateProviderInterfacePrx configTopic;
 
     };
 

source/RobotAPI/interface/observers/GraspCandidateObserverInterface.ice

@@ -44,7 +44,7 @@ module armarx
         sequence<string> StringSeq;
         dictionary<string, int> IntMap;
 
-        interface GraspCandidateObserverInterface extends ObserverInterface, GraspCandidateProviderListener
+        interface GraspCandidateObserverInterface extends ObserverInterface, GraspCandidatesTopicInterface
         {
             InfoMap getAvailableProvidersWithInfo();
             StringSeq getAvailableProviderNames();

source/RobotAPI/interface/units/GraspCandidateProviderInterface.ice

@@ -44,9 +44,9 @@ module armarx
         class BoundingBox
         {
             Vector3Base center;
-            Vector3Base len1;
-            Vector3Base len2;
-            Vector3Base len3;
+            Vector3Base ha1;
+            Vector3Base ha2;
+            Vector3Base ha3;
         };
 
         class GraspCandidateSourceInfo
@@ -91,12 +91,12 @@ module armarx
         dictionary<string, ProviderInfo> InfoMap;
 
 
-        interface GraspCandidateProviderListener
+        interface GraspCandidatesTopicInterface
         {
-            void reportGraspCandidates(string providerName, GraspCandidateSeq candidates);
             void reportProviderInfo(string providerName, ProviderInfo info);
+            void reportGraspCandidates(string providerName, GraspCandidateSeq candidates);
         };
-        interface GraspCandidateProviderConfigListener
+        interface GraspCandidateProviderInterface
         {
             void requestGraspCandidateGeneration(string providerName, int timeoutMS);
             void setGraspCandidateGenerationConfig(string providerName, ConfigMap config);