From b6ebf6bee28c99845d6cb57dab7cf337b92d6fc6 Mon Sep 17 00:00:00 2001
From: Markus Grotz <markus.grotz@kit.edu>
Date: Tue, 7 Jun 2016 11:56:18 +0200
Subject: [PATCH] updated ViewSelection
- replaced IceUtil::Time::now with ArmarX TimeUtil
- SaliencyMaps now include more information (time, source)
---
.../ViewSelection/ViewSelection.cpp | 22 ++++++++++---------
.../components/ViewSelection/ViewSelection.h | 5 +++--
.../components/ViewSelectionInterface.ice | 16 +++++++++++++-
3 files changed, 30 insertions(+), 13 deletions(-)
diff --git a/source/RobotAPI/components/ViewSelection/ViewSelection.cpp b/source/RobotAPI/components/ViewSelection/ViewSelection.cpp
index 79700054c..34c80fd1f 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 ce8977b57..40d200901 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 7521a30c8..52c991bd7 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);
};
--
GitLab