revised ViewSelection

source/RobotAPI/components/ViewSelection/ViewSelection.cpp

@@ -142,6 +142,9 @@ void ViewSelection::onExitComponent()
 
 ViewTargetBasePtr ViewSelection::nextAutomaticViewTarget()
 {
+
+    boost::mutex::scoped_lock lock(syncMutex);
+
     IceUtil::Time currentTime =  armarx::TimeUtil::GetTime();
     std::vector<std::string> activeSaliencyMaps;
     for (const auto & p : saliencyMaps)
@@ -161,6 +164,8 @@ ViewTargetBasePtr ViewSelection::nextAutomaticViewTarget()
 
     }
 
+    hasNewSaliencyMap = false;
+
     if (activeSaliencyMaps.empty())
     {
         return nullptr;
@@ -190,6 +195,9 @@ ViewTargetBasePtr ViewSelection::nextAutomaticViewTarget()
         }
     }
 
+
+    syncMutex.unlock();
+
     ARMARX_DEBUG << "Highest saliency: " << maxSaliency;
 
     // select a new view if saliency is bigger than threshold (which converges towards 0 over time)
@@ -232,6 +240,26 @@ void ViewSelection::process()
         viewTarget = nextAutomaticViewTarget();
     }
 
+    /*
+    //discard outdated view targets
+    IceUtil::Time currentTime = armarx::TimeUtil::GetTime();
+    while (!manualViewTargets.empty())
+    {
+        ViewTargetBasePtr manualViewTarget = manualViewTargets.top();
+        TimestampVariantPtr time = TimestampVariantPtr::dynamicCast(manualViewTarget->validUntil);
+
+        if (time->toTime() < currentTime)
+        {
+            ARMARX_INFO << "view target is no longer valid";
+            manualViewTargets.pop();
+        }
+        else
+        {
+            break;
+        }
+    }
+    */
+
     if (!manualViewTargets.empty())
     {
         ScopedLock lock(manualViewTargetsMutex);
@@ -271,14 +299,15 @@ void ViewSelection::process()
         ARMARX_INFO << "Looking at target " << viewTargetPositionPtr->output();
         headIKUnitProxy->setHeadTarget(headIKKinematicChainName, viewTargetPositionPtr);
 
-        timeOfLastViewChange = armarx::TimeUtil::GetTime();
+        timeOfLastViewChange = TimeUtil::GetTime();
 
-        float duration = viewTarget->duration;
+        long duration = viewTarget->duration;
         if (!viewTarget->duration)
         {
             duration = sleepingTimeBetweenViewDirectionChanges;
         }
 
+        viewSelectionObserver->nextViewTarget(timeOfLastViewChange.toMilliSeconds() + duration);
 
         boost::this_thread::sleep(boost::posix_time::milliseconds(duration));
     }
@@ -303,6 +332,10 @@ void ViewSelection::addManualViewTarget(const FramedPositionBasePtr& target, con
     viewTarget->duration = 0;
 
     manualViewTargets.push(viewTarget);
+
+    boost::mutex::scoped_lock lock2(syncMutex);
+
+    condition.notify_all();
 }
 
 void ViewSelection::clearManualViewTargets(const Ice::Current& c)
@@ -340,9 +373,25 @@ ViewTargetList ViewSelection::getManualViewTargets(const Ice::Current& c)
 
 void armarx::ViewSelection::updateSaliencyMap(const SaliencyMapBasePtr& map, const Ice::Current&)
 {
-    boost::mutex::scoped_lock lock(manualViewTargetsMutex);
+    boost::mutex::scoped_lock lock(syncMutex);
+
+    hasNewSaliencyMap = true;
 
     saliencyMaps[map->name] = map;
+
+    condition.notify_all();
+}
+
+void ViewSelection::removeSaliencyMap(const string& name, const Ice::Current& c)
+{
+    boost::mutex::scoped_lock lock(syncMutex);
+
+    if (saliencyMaps.count(name))
+    {
+        saliencyMaps.erase(name);
+    }
+
+    condition.notify_all();
 }
 
 
@@ -352,4 +401,3 @@ PropertyDefinitionsPtr ViewSelection::createPropertyDefinitions()
 }
 
 
-

source/RobotAPI/components/ViewSelection/ViewSelection.h

@@ -178,6 +178,8 @@ namespace armarx
 
         void updateSaliencyMap(const SaliencyMapBasePtr& map, const Ice::Current& c = ::Ice::Current());
 
+        void removeSaliencyMap(const std::string& name, const Ice::Current& c = ::Ice::Current());
+
     private:
 
         void process();
@@ -210,12 +212,13 @@ namespace armarx
 
         Eigen::Vector3f offsetToHeadCenter;
 
+        boost::condition_variable condition;
+        bool hasNewSaliencyMap;
+        boost::mutex syncMutex;
 
         std::map<std::string, SaliencyMapBasePtr> saliencyMaps;
 
     };
-
-
 }
 
 #endif

source/RobotAPI/interface/components/ViewSelectionInterface.ice

@@ -51,8 +51,6 @@ module armarx
         string source;
     };
 
-
-
     ["cpp:virtual"]
     class SaliencyMapBase
     {
@@ -78,6 +76,7 @@ module armarx
         bool isEnabledAutomaticViewSelection();
 
         void updateSaliencyMap(SaliencyMapBase map);
+        void removeSaliencyMap(string name);
     };
 
 
@@ -86,6 +85,7 @@ module armarx
     {
         void onActivateAutomaticViewSelection();
         void onDeactivateAutomaticViewSelection();
+        void nextViewTarget(long timestamp);
     };
 
 };