From c703ea191102978d7e2f3ca4c4d0f0a93b71fc31 Mon Sep 17 00:00:00 2001
From: Markus Grotz <markus.grotz@kit.edu>
Date: Fri, 24 Jun 2016 14:58:10 +0200
Subject: [PATCH] revised ViewSelection
---
.../ViewSelection/ViewSelection.cpp | 56 +++++++++++++++++--
.../components/ViewSelection/ViewSelection.h | 7 ++-
.../components/ViewSelectionInterface.ice | 4 +-
3 files changed, 59 insertions(+), 8 deletions(-)
diff --git a/source/RobotAPI/components/ViewSelection/ViewSelection.cpp b/source/RobotAPI/components/ViewSelection/ViewSelection.cpp
index 30a85a193..4cfaeae09 100644
--- a/source/RobotAPI/components/ViewSelection/ViewSelection.cpp
+++ b/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()
}
-
diff --git a/source/RobotAPI/components/ViewSelection/ViewSelection.h b/source/RobotAPI/components/ViewSelection/ViewSelection.h
index 496e20250..d10ee5d6b 100644
--- a/source/RobotAPI/components/ViewSelection/ViewSelection.h
+++ b/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
diff --git a/source/RobotAPI/interface/components/ViewSelectionInterface.ice b/source/RobotAPI/interface/components/ViewSelectionInterface.ice
index cb9a18032..8db92bbd9 100755
--- a/source/RobotAPI/interface/components/ViewSelectionInterface.ice
+++ b/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);
};
};
--
GitLab