revised ViewSelection

source/RobotAPI/components/ViewSelection/ViewSelection.cpp

@@ -141,12 +141,11 @@ void ViewSelection::onExitComponent()
 }
 
 
-ViewTargetBasePtr ViewSelection::nextAutomaticViewTarget()
+void ViewSelection::getActiveSaliencyMaps(std::vector<std::string>& activeSaliencyMaps)
 {
     boost::mutex::scoped_lock lock(syncMutex);
 
     IceUtil::Time currentTime =  armarx::TimeUtil::GetTime();
-    std::vector<std::string> activeSaliencyMaps;
     for (const auto & p : saliencyMaps)
     {
         if (p.second->validUntil)
@@ -161,20 +160,19 @@ ViewTargetBasePtr ViewSelection::nextAutomaticViewTarget()
         {
             activeSaliencyMaps.push_back(p.second->name);
         }
-
     }
+}
 
-    hasNewSaliencyMap = false;
+ViewTargetBasePtr ViewSelection::nextAutomaticViewTarget()
+{
+    std::vector<std::string> activeSaliencyMaps;
+    getActiveSaliencyMaps(activeSaliencyMaps);
 
     if (activeSaliencyMaps.empty())
     {
         return nullptr;
     }
 
-    if (visuSaliencyThreshold > 0.0)
-    {
-        drawSaliencySphere(activeSaliencyMaps);
-    }
 
     SharedRobotInterfacePrx robotPrx = robotStateComponent->getSynchronizedRobot();
     TNodeList* visibilityMaskGraphNodes = visibilityMaskGraph->getNodes();
@@ -183,6 +181,10 @@ ViewTargetBasePtr ViewSelection::nextAutomaticViewTarget()
     int maxIndex = -1;
     float maxSaliency = std::numeric_limits<float>::min();
 
+    boost::mutex::scoped_lock lock(syncMutex);
+
+    hasNewSaliencyMap = false;
+
     for (size_t i = 0; i < visibilityMaskGraphNodes->size(); i++)
     {
         float saliency = 0.0f;
@@ -193,12 +195,9 @@ ViewTargetBasePtr ViewSelection::nextAutomaticViewTarget()
 
         saliency /= activeSaliencyMaps.size();
 
-
-
         CIntensityNode* visibilityNode = (CIntensityNode*) visibilityMaskGraphNodes->at(i);
         saliency *= visibilityNode->getIntensity();
 
-
         if (saliency > maxSaliency)
         {
             maxSaliency = saliency;
@@ -234,7 +233,14 @@ ViewTargetBasePtr ViewSelection::nextAutomaticViewTarget()
         viewTarget->target = viewTargetPositionPtr;
         viewTarget->duration = 0;
 
+
+        if (visuSaliencyThreshold > 0.0)
+        {
+            drawSaliencySphere(activeSaliencyMaps);
+        }
+
         return viewTarget;
+
     }
 
     return nullptr;
@@ -244,6 +250,12 @@ ViewTargetBasePtr ViewSelection::nextAutomaticViewTarget()
 
 void ViewSelection::process()
 {
+    /*
+    while(getState() < eManagedIceObjectExiting)
+    {
+    }
+    */
+
     ViewTargetBasePtr viewTarget;
 
     if (doAutomaticViewSelection)
@@ -294,6 +306,7 @@ void ViewSelection::process()
     if (viewTarget)
     {
         SharedRobotInterfacePrx robotPrx = robotStateComponent->getSynchronizedRobot();
+        // FramedPositionPtr viewTargetPositionPtr = FramedPositionPtr::dynamicCast(viewTarget->target);
         FramedPositionPtr viewTargetPositionPtr = new FramedPosition(FramedPositionPtr::dynamicCast(viewTarget->target)->toEigen(), viewTarget->target->frame, robotPrx->getName());
 
         if (drawer && robotPrx->hasRobotNode("Cameras") && drawViewDirection)
@@ -401,13 +414,12 @@ void ViewSelection::drawSaliencySphere(const ::Ice::StringSeq& names, const Ice:
 {
     ARMARX_LOG << "visualizing saliency map";
 
-    //drawer->clearLayer("saliencyMap");
+    drawer->clearLayer("saliencyMap");
 
     SharedRobotInterfacePrx robotPrx = robotStateComponent->getSynchronizedRobot();
 
     Eigen::Matrix4f cameraToGlobal = FramedPosePtr::dynamicCast(robotPrx->getRobotNode(headFrameName)->getGlobalPose())->toEigen();
 
-    IceUtil::Time currentTime =  armarx::TimeUtil::GetTime();
     std::vector<std::string> activeSaliencyMaps;
 
     if (names.size())
@@ -419,23 +431,17 @@ void ViewSelection::drawSaliencySphere(const ::Ice::StringSeq& names, const Ice:
     }
     else
     {
-        for (const auto & p : saliencyMaps)
-        {
-            if (p.second->validUntil)
-            {
-                TimestampVariantPtr time = TimestampVariantPtr::dynamicCast(p.second->validUntil);
-                if (time->toTime() > currentTime)
-                {
-                    activeSaliencyMaps.push_back(p.second->name);
-                }
-            }
-            else if ((currentTime - TimestampVariantPtr::dynamicCast(p.second->timeAdded)->toTime()) < IceUtil::Time::seconds(5))
-            {
-                activeSaliencyMaps.push_back(p.second->name);
-            }
-        }
+        getActiveSaliencyMaps(activeSaliencyMaps);
     }
 
+    if (!activeSaliencyMaps.size())
+    {
+        return;
+    }
+
+
+    boost::mutex::scoped_lock lock(syncMutex);
+
     TNodeList* visibilityMaskGraphNodes = visibilityMaskGraph->getNodes();
     DebugDrawer24BitColoredPointCloud cloud;
     cloud.points.reserve(visibilityMaskGraphNodes->size());
@@ -499,7 +505,6 @@ void ViewSelection::removeSaliencyMap(const string& name, const Ice::Current& c)
 
 Ice::StringSeq ViewSelection::getSaliencyMapNames(const Ice::Current& c)
 {
-
     std::vector<std::string> names;
 
     boost::mutex::scoped_lock lock(syncMutex);

source/RobotAPI/components/ViewSelection/ViewSelection.h

@@ -194,6 +194,8 @@ namespace armarx
 
         ViewTargetBasePtr nextAutomaticViewTarget();
 
+        void getActiveSaliencyMaps(std::vector<std::string>& activeSaliencyMaps);
+
         armarx::PeriodicTask<ViewSelection>::pointer_type processorTask;
 
         RobotStateComponentInterfacePrx robotStateComponent;