updated ViewSelection

- replaced IceUtil::Time::now with ArmarX TimeUtil
- SaliencyMaps now include more information (time, source)

source/RobotAPI/components/ViewSelection/ViewSelection.cpp

@@ -48,6 +48,9 @@ void ViewSelection::onInitComponent()
 
     std::string graphFileName = "RobotAPI/ViewSelection/graph40k.gra";
 
+    armarx::CMakePackageFinder finder("RobotAPI");
+    ArmarXDataPath::addDataPaths(finder.getDataDir());
+
     if (ArmarXDataPath::getAbsolutePath(graphFileName, graphFileName))
     {
         visibilityMaskGraph = new CIntensityGraph(graphFileName);
@@ -84,7 +87,7 @@ void ViewSelection::onInitComponent()
     sleepingTimeBetweenViewDirectionChanges = getProperty<int>("SleepingTimeBetweenViewDirectionChanges").getValue();
     doAutomaticViewSelection = getProperty<bool>("ActiveAtStartup").getValue();
 
-    timeOfLastViewChange = IceUtil::Time::now();
+    timeOfLastViewChange =  armarx::TimeUtil::GetTime();
 
     // this is robot model specific: offset from the used head coordinate system to the actual
     // head center where the eyes are assumed to be located. Here it is correct for ARMAR-III
@@ -142,13 +145,13 @@ ViewTargetBasePtr ViewSelection::nextAutomaticViewTarget()
 
     // find the direction with highest saliency
     int maxIndex = -1;
-    float maxSaliency = 0;
+    float maxSaliency = std::numeric_limits<float>::min();
     for (size_t i = 0; i < visibilityMaskGraphNodes->size(); i++)
     {
         float saliency = 0.0f;
-        for (const auto & p : saliencyMaps)
+        for (const std::string & n : activeSaliencyMaps)
         {
-            saliency += p.second[i];
+            saliency += saliencyMaps[n]->map[i];
         }
 
         CIntensityNode* visibilityNode = (CIntensityNode*) visibilityMaskGraphNodes->at(i);
@@ -164,7 +167,7 @@ ViewTargetBasePtr ViewSelection::nextAutomaticViewTarget()
     ARMARX_DEBUG << "Highest saliency: " << maxSaliency;
 
     // select a new view if saliency is bigger than threshold (which converges towards 0 over time)
-    int timeSinceLastViewChange = (IceUtil::Time::now() - timeOfLastViewChange).toMilliSeconds();
+    int timeSinceLastViewChange = (armarx::TimeUtil::GetTime() - timeOfLastViewChange).toMilliSeconds();
     float saliencyThreshold = 0;
 
     if (timeSinceLastViewChange > 0)
@@ -182,7 +185,7 @@ ViewTargetBasePtr ViewSelection::nextAutomaticViewTarget()
 
         ViewTargetBasePtr viewTarget = new ViewTargetBase();
         viewTarget->priority = armarx::DEFAULT_VIEWTARGET_PRIORITY; // *  maxSaliency;
-        viewTarget->timeAdded = new TimestampVariant(IceUtil::Time::now());
+        viewTarget->timeAdded = new TimestampVariant(armarx::TimeUtil::GetTime());
         viewTarget->target = viewTargetPositionPtr;
         viewTarget->duration = 0;
 
@@ -242,8 +245,7 @@ void ViewSelection::process()
         ARMARX_INFO << "Looking at target " << viewTargetPositionPtr->output();
         headIKUnitProxy->setHeadTarget(headIKKinematicChainName, viewTargetPositionPtr);
 
-        timeOfLastViewChange = IceUtil::Time::now();
-
+        timeOfLastViewChange = armarx::TimeUtil::GetTime();
 
         float duration = viewTarget->duration;
         if (!viewTarget->duration)
@@ -310,11 +312,11 @@ ViewTargetList ViewSelection::getManualViewTargets(const Ice::Current& c)
 
 
 
-void armarx::ViewSelection::updateSaliencyMap(const string& name, const FloatSequence& map, const Ice::Current&)
+void armarx::ViewSelection::updateSaliencyMap(const SaliencyMapBasePtr& map, const Ice::Current&)
 {
     boost::mutex::scoped_lock lock(manualViewTargetsMutex);
 
-    saliencyMaps[name] = map;
+    saliencyMaps[map->name] = map;
 }
 
 

source/RobotAPI/components/ViewSelection/ViewSelection.h

@@ -28,6 +28,7 @@
 
 
 #include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/time/TimeUtil.h>
 #include <ArmarXCore/observers/variant/TimestampVariant.h>
 #include <ArmarXCore/core/services/tasks/RunningTask.h>
 
@@ -164,7 +165,7 @@ namespace armarx
             doAutomaticViewSelection = false;
         }
 
-        void updateSaliencyMap(const string& name, const FloatSequence& map, const Ice::Current& c = ::Ice::Current());
+        void updateSaliencyMap(const SaliencyMapBasePtr& map, const Ice::Current& c = ::Ice::Current());
 
     private:
 
@@ -197,7 +198,7 @@ namespace armarx
         Eigen::Vector3f offsetToHeadCenter;
 
 
-        std::map<std::string, std::vector<float>> saliencyMaps;
+        std::map<std::string, SaliencyMapBasePtr> saliencyMaps;
 
     };
 

source/RobotAPI/interface/components/ViewSelectionInterface.ice

@@ -52,6 +52,20 @@ module armarx
         string source;
     };
 
+
+
+    ["cpp:virtual"]
+    class SaliencyMapBase
+    {
+        string name;
+
+        FloatSequence map;
+
+        TimestampBase validUntil;
+
+        TimestampBase timeAdded;
+    };
+
     sequence<ViewTargetBase> ViewTargetList;
 
     interface ViewSelectionInterface
@@ -63,7 +77,7 @@ module armarx
         void activateAutomaticViewSelection();
         void deactivateAutomaticViewSelection();
 
-        void updateSaliencyMap(string name, FloatSequence map);
+        void updateSaliencyMap(SaliencyMapBase map);
     };