diff --git a/source/RobotAPI/components/ViewSelection/ViewSelection.cpp b/source/RobotAPI/components/ViewSelection/ViewSelection.cpp index 79700054c9abaa3f060ac9b6d65de9766a5e7a84..34c80fd1fdb4bd9b9d8d49f5f6323866f1bfce37 100644 --- a/source/RobotAPI/components/ViewSelection/ViewSelection.cpp +++ b/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; } diff --git a/source/RobotAPI/components/ViewSelection/ViewSelection.h b/source/RobotAPI/components/ViewSelection/ViewSelection.h index ce8977b57ed01e24180a81ad384d1576b43bd909..40d20090178faee01ba73d07d799aea35a71fa1c 100644 --- a/source/RobotAPI/components/ViewSelection/ViewSelection.h +++ b/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; }; diff --git a/source/RobotAPI/interface/components/ViewSelectionInterface.ice b/source/RobotAPI/interface/components/ViewSelectionInterface.ice index 7521a30c834262116fbeaa8f6f13d0ce9e90d2c6..52c991bd72a26ba991ec4c8b2b036444eb8385fc 100755 --- a/source/RobotAPI/interface/components/ViewSelectionInterface.ice +++ b/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); };