From f7ae4d4bc7b541a72143389d96a4462d67df24ff Mon Sep 17 00:00:00 2001
From: Markus Grotz <markus.grotz@kit.edu>
Date: Tue, 31 May 2016 14:16:43 +0200
Subject: [PATCH] moved ViewSelection to RobotAPI
---
data/RobotAPI/VariantInfo-RobotAPI.xml | 8 +
source/RobotAPI/applications/CMakeLists.txt | 2 +
.../applications/ViewSelection/CMakeLists.txt | 19 +
.../ViewSelection/ViewSelectionApp.h | 54 +++
.../applications/ViewSelection/main.cpp | 34 ++
source/RobotAPI/components/CMakeLists.txt | 6 +-
.../components/ViewSelection/CMakeLists.txt | 29 ++
.../ViewSelection/ViewSelection.cpp | 329 ++++++++++++++++++
.../components/ViewSelection/ViewSelection.h | 207 +++++++++++
source/RobotAPI/interface/CMakeLists.txt | 3 +
.../components/ViewSelectionInterface.ice | 75 ++++
11 files changed, 762 insertions(+), 4 deletions(-)
create mode 100644 source/RobotAPI/applications/ViewSelection/CMakeLists.txt
create mode 100644 source/RobotAPI/applications/ViewSelection/ViewSelectionApp.h
create mode 100644 source/RobotAPI/applications/ViewSelection/main.cpp
create mode 100644 source/RobotAPI/components/ViewSelection/CMakeLists.txt
create mode 100644 source/RobotAPI/components/ViewSelection/ViewSelection.cpp
create mode 100644 source/RobotAPI/components/ViewSelection/ViewSelection.h
create mode 100755 source/RobotAPI/interface/components/ViewSelectionInterface.ice
diff --git a/data/RobotAPI/VariantInfo-RobotAPI.xml b/data/RobotAPI/VariantInfo-RobotAPI.xml
index d34502bd3..ba6d63e15 100644
--- a/data/RobotAPI/VariantInfo-RobotAPI.xml
+++ b/data/RobotAPI/VariantInfo-RobotAPI.xml
@@ -113,6 +113,14 @@
<onConnect>remoteRobot.reset(new RemoteRobot(robotStateComponent->getSynchronizedRobot()));</onConnect>
<stateMethod header="const VirtualRobot::RobotPtr getRobot() const">return %getContext%->getRobot();</stateMethod>
</Proxy>
+ <Proxy include="RobotAPI/interface/components/ViewSelectionInterface.h"
+ humanName="Automatic View Selection"
+ typeName="ViewSelectionInterfacePrx"
+ memberName="viewSelection"
+ getterName="getViewSelection"
+ propertyName="ViewSelectionName"
+ propertyIsOptional="true"
+ propertyDefaultValue="ViewSelection" />
<Topic include="RobotAPI/interface/visualization/DebugDrawerInterface.h"
humanName="Debug Drawer Topic"
typeName="DebugDrawerInterfacePrx"
diff --git a/source/RobotAPI/applications/CMakeLists.txt b/source/RobotAPI/applications/CMakeLists.txt
index c6e30a4ba..736267ef6 100644
--- a/source/RobotAPI/applications/CMakeLists.txt
+++ b/source/RobotAPI/applications/CMakeLists.txt
@@ -20,3 +20,5 @@ add_subdirectory(ForceTorqueUnitSimulation)
add_subdirectory(XsensIMU)
add_subdirectory(InertialMeasurementUnitObserver)
+
+add_subdirectory(ViewSelection)
diff --git a/source/RobotAPI/applications/ViewSelection/CMakeLists.txt b/source/RobotAPI/applications/ViewSelection/CMakeLists.txt
new file mode 100644
index 000000000..e9617c086
--- /dev/null
+++ b/source/RobotAPI/applications/ViewSelection/CMakeLists.txt
@@ -0,0 +1,19 @@
+armarx_component_set_name("ViewSelectionApp")
+
+find_package(Eigen3 QUIET)
+armarx_build_if(Eigen3_FOUND "Eigen3 not available")
+if(Eigen3_FOUND)
+ include_directories(${Eigen3_INCLUDE_DIR})
+endif()
+
+
+set(COMPONENT_LIBS
+ ViewSelection
+ RobotAPIInterfaces RobotAPICore
+ ArmarXCoreInterfaces ArmarXCore ArmarXCoreStatechart ArmarXCoreObservers
+)
+
+
+set(EXE_SOURCE main.cpp ViewSelectionApp.h)
+
+armarx_add_component_executable("${EXE_SOURCE}")
diff --git a/source/RobotAPI/applications/ViewSelection/ViewSelectionApp.h b/source/RobotAPI/applications/ViewSelection/ViewSelectionApp.h
new file mode 100644
index 000000000..f26a56822
--- /dev/null
+++ b/source/RobotAPI/applications/ViewSelection/ViewSelectionApp.h
@@ -0,0 +1,54 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2015-2016, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotComponents::application::ViewSelection
+ * @author David Schiebener (schiebener qt kit dot edu)
+ * @date 2015
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#ifndef _ARMARX_APPLICATION_RobotComponents_ViewSelection_H
+#define _ARMARX_APPLICATION_RobotComponents_ViewSelection_H
+
+
+#include <ArmarXCore/core/application/Application.h>
+#include <RobotAPI/components/ViewSelection/ViewSelection.h>
+
+namespace armarx
+{
+ /**
+ * @class ViewSelectionApp
+ * @brief A brief description
+ *
+ * Detailed Description
+ */
+ class ViewSelectionApp :
+ virtual public armarx::Application
+ {
+ /**
+ * @see armarx::Application::setup()
+ */
+ void setup(const ManagedIceObjectRegistryInterfacePtr& registry,
+ Ice::PropertiesPtr properties)
+ {
+ registry->addObject(Component::create<ViewSelection>(properties));
+ }
+ };
+}
+
+#endif
diff --git a/source/RobotAPI/applications/ViewSelection/main.cpp b/source/RobotAPI/applications/ViewSelection/main.cpp
new file mode 100644
index 000000000..01e91acc5
--- /dev/null
+++ b/source/RobotAPI/applications/ViewSelection/main.cpp
@@ -0,0 +1,34 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2015-2016, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotComponents::application::ViewSelection
+ * @author David Schiebener (schiebener qt kit dot edu)
+ * @date 2015
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "ViewSelectionApp.h"
+#include <ArmarXCore/core/logging/Logging.h>
+
+int main(int argc, char* argv[])
+{
+ armarx::ApplicationPtr app = armarx::Application::createInstance < armarx::ViewSelectionApp > ();
+ app->setName("ViewSelection");
+
+ return app->main(argc, argv);
+}
diff --git a/source/RobotAPI/components/CMakeLists.txt b/source/RobotAPI/components/CMakeLists.txt
index b596e7006..bf391c4c6 100644
--- a/source/RobotAPI/components/CMakeLists.txt
+++ b/source/RobotAPI/components/CMakeLists.txt
@@ -1,7 +1,5 @@
-
add_subdirectory(units)
add_subdirectory(DebugDrawer)
add_subdirectory(RobotState)
-
-
-add_subdirectory(EarlyVisionGraph)
\ No newline at end of file
+add_subdirectory(EarlyVisionGraph)
+add_subdirectory(ViewSelection)
diff --git a/source/RobotAPI/components/ViewSelection/CMakeLists.txt b/source/RobotAPI/components/ViewSelection/CMakeLists.txt
new file mode 100644
index 000000000..3fb96c045
--- /dev/null
+++ b/source/RobotAPI/components/ViewSelection/CMakeLists.txt
@@ -0,0 +1,29 @@
+armarx_component_set_name("ViewSelection")
+
+
+find_package(Eigen3 QUIET)
+armarx_build_if(Eigen3_FOUND "Eigen3 not available")
+if(Eigen3_FOUND)
+ include_directories(${Eigen3_INCLUDE_DIR})
+endif()
+
+
+set(COMPONENT_LIBS
+ RobotAPIInterfaces RobotAPICore EarlyVisionGraph
+ ArmarXCoreInterfaces ArmarXCore ArmarXCoreStatechart ArmarXCoreObservers
+ ${Simox_LIBRARIES}
+)
+
+set(SOURCES
+./ViewSelection.cpp
+#@TEMPLATE_LINE@@COMPONENT_PATH@/@COMPONENT_NAME@.cpp
+)
+set(HEADERS
+./ViewSelection.h
+#@TEMPLATE_LINE@@COMPONENT_PATH@/@COMPONENT_NAME@.h
+)
+
+armarx_add_component("${SOURCES}" "${HEADERS}")
+
+# add unit tests
+#add_subdirectory(test)
diff --git a/source/RobotAPI/components/ViewSelection/ViewSelection.cpp b/source/RobotAPI/components/ViewSelection/ViewSelection.cpp
new file mode 100644
index 000000000..3ab494165
--- /dev/null
+++ b/source/RobotAPI/components/ViewSelection/ViewSelection.cpp
@@ -0,0 +1,329 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2015-2016, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotComponents::ViewSelection
+ * @author David Schiebener (schiebener at kit dot edu)
+ * @author Markus Grotz ( markus dot grotz at kit dot edu )
+ * @date 2015
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "ViewSelection.h"
+
+#include <ArmarXCore/core/system/ArmarXDataPath.h>
+#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
+
+#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
+
+
+using namespace armarx;
+
+
+void ViewSelection::onInitComponent()
+{
+ usingProxy(getProperty<std::string>("RobotStateComponentName").getValue());
+ usingProxy(getProperty<std::string>("HeadIKUnitName").getValue());
+
+ offeringTopic("DebugDrawerUpdates");
+
+ headIKKinematicChainName = getProperty<std::string>("HeadIKKinematicChainName").getValue();
+ headFrameName = getProperty<std::string>("HeadFrameName").getValue();
+ cameraFrameName = getProperty<std::string>("CameraFrameName").getValue();
+ drawViewDirection = getProperty<bool>("VisualizeViewDirection").getValue();
+
+ std::string graphFileName = "RobotComponents/ViewSelection/graph40k.gra";
+
+ armarx::CMakePackageFinder finder("RobotComponents");
+ ArmarXDataPath::addDataPaths(finder.getDataDir());
+
+ if (ArmarXDataPath::getAbsolutePath(graphFileName, graphFileName))
+ {
+ visibilityMaskGraph = new CIntensityGraph(graphFileName);
+ TNodeList* nodes = visibilityMaskGraph->getNodes();
+ const float maxOverallHeadTiltAngle = getProperty<float>("MaxOverallHeadTiltAngle").getValue();
+ const float borderAreaAngle = 10.0f;
+ const float centralAngle = getProperty<float>("CentralHeadTiltAngle").getValue();
+
+ for (size_t i = 0; i < nodes->size(); i++)
+ {
+ CIntensityNode* node = (CIntensityNode*) nodes->at(i);
+
+ if (fabs(node->getPosition().fPhi - centralAngle) < maxOverallHeadTiltAngle - borderAreaAngle)
+ {
+ node->setIntensity(1.0f);
+ }
+ else if (fabs(node->getPosition().fPhi - centralAngle) < maxOverallHeadTiltAngle)
+ {
+ node->setIntensity(1.0f - (fabs(node->getPosition().fPhi - centralAngle) - (maxOverallHeadTiltAngle - borderAreaAngle)) / borderAreaAngle);
+ }
+ else
+ {
+ node->setIntensity(0.0f);
+ }
+ }
+ }
+ else
+ {
+ ARMARX_ERROR << "Could not find required1 graph file";
+ handleExceptions();
+ }
+
+ sleepingTimeBetweenViewDirectionChanges = getProperty<int>("SleepingTimeBetweenViewDirectionChanges").getValue();
+ doAutomaticViewSelection = getProperty<bool>("ActiveAtStartup").getValue();
+
+ timeOfLastViewChange = IceUtil::Time::now();
+
+ // 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
+ offsetToHeadCenter << 0, 0, 150;
+
+ processorTask = new PeriodicTask<ViewSelection>(this, &ViewSelection::process, 30);
+ processorTask->setDelayWarningTolerance(sleepingTimeBetweenViewDirectionChanges + 100);
+}
+
+
+void ViewSelection::onConnectComponent()
+{
+ robotStateComponent = getProxy<RobotStateComponentInterfacePrx>(getProperty<std::string>("RobotStateComponentName").getValue());
+
+ headIKUnitProxy = getProxy<HeadIKUnitInterfacePrx>(getProperty<std::string>("HeadIKUnitName").getValue());
+ headIKUnitProxy->request();
+
+ drawer = getTopic<DebugDrawerInterfacePrx>("DebugDrawerUpdates");
+
+ processorTask->start();
+}
+
+
+void ViewSelection::onDisconnectComponent()
+{
+ processorTask->stop();
+
+ if (drawer)
+ {
+ drawer->removeArrowDebugLayerVisu("HeadRealViewDirection");
+ }
+
+ try
+ {
+ headIKUnitProxy->release();
+ }
+ catch (...)
+ {
+ ARMARX_WARNING_S << "An exception occured during onDisconnectComponent()";
+ handleExceptions();
+ }
+}
+
+
+void ViewSelection::onExitComponent()
+{
+ delete visibilityMaskGraph;
+}
+
+
+ViewTargetBasePtr ViewSelection::nextAutomaticViewTarget()
+{
+ SharedRobotInterfacePrx robotPrx = robotStateComponent->getSynchronizedRobot();
+ TNodeList* visibilityMaskGraphNodes = visibilityMaskGraph->getNodes();
+
+ // find the direction with highest saliency
+ int maxIndex = -1;
+ float maxSaliency = 0;
+ for (size_t i = 0; i < visibilityMaskGraphNodes->size(); i++)
+ {
+ float saliency = 0.0f;
+ for (const auto & p : saliencyMaps)
+ {
+ saliency += p.second[i];
+ }
+
+ CIntensityNode* visibilityNode = (CIntensityNode*) visibilityMaskGraphNodes->at(i);
+ saliency *= visibilityNode->getIntensity();
+
+ if (saliency > maxSaliency)
+ {
+ maxSaliency = saliency;
+ maxIndex = i;
+ }
+ }
+
+ 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();
+ float saliencyThreshold = 0;
+
+ if (timeSinceLastViewChange > 0)
+ {
+ saliencyThreshold = 2.0f * sleepingTimeBetweenViewDirectionChanges / timeSinceLastViewChange;
+ }
+
+ if (maxSaliency > saliencyThreshold)
+ {
+ Eigen::Vector3f target;
+ MathTools::convert(visibilityMaskGraphNodes->at(maxIndex)->getPosition(), target);
+ const float distanceFactor = 1500;
+ target = distanceFactor * target + offsetToHeadCenter;
+ FramedPositionPtr viewTargetPositionPtr = new FramedPosition(target, headFrameName, robotPrx->getName());
+
+ ViewTargetBasePtr viewTarget = new ViewTargetBase();
+ viewTarget->priority = armarx::DEFAULT_VIEWTARGET_PRIORITY; // * maxSaliency;
+ viewTarget->timeAdded = new TimestampVariant(IceUtil::Time::now());
+ viewTarget->target = viewTargetPositionPtr;
+ viewTarget->duration = 0;
+
+ return viewTarget;
+ }
+
+ return nullptr;
+}
+
+
+
+void ViewSelection::process()
+{
+ ViewTargetBasePtr viewTarget;
+
+ if (doAutomaticViewSelection)
+ {
+ viewTarget = nextAutomaticViewTarget();
+ }
+
+ if (!manualViewTargets.empty())
+ {
+ ScopedLock lock(manualViewTargetsMutex);
+
+ ViewTargetBasePtr manualViewTarget = manualViewTargets.top();
+
+ CompareViewTargets c;
+
+ if (!viewTarget)
+ {
+ viewTarget = manualViewTarget;
+ manualViewTargets.pop();
+ }
+ else if (c(viewTarget, manualViewTarget))
+ {
+ viewTarget = manualViewTarget;
+ manualViewTargets.pop();
+ }
+ }
+
+ if (viewTarget)
+ {
+ SharedRobotInterfacePrx robotPrx = robotStateComponent->getSynchronizedRobot();
+ FramedPositionPtr viewTargetPositionPtr = new FramedPosition(FramedPositionPtr::dynamicCast(viewTarget->target)->toEigen(), viewTarget->target->frame, robotPrx->getName());
+
+ if (drawer && robotPrx->hasRobotNode("Cameras") && drawViewDirection)
+ {
+ float arrowLength = 1500.0f;
+ Vector3Ptr startPos = new Vector3(FramedPosePtr::dynamicCast(robotPrx->getRobotNode("Cameras")->getGlobalPose())->toEigen());
+ FramedDirectionPtr currentDirection = new FramedDirection(Eigen::Vector3f(1, 0, 0), "Cameras", robotPrx->getName());
+ drawer->setArrowDebugLayerVisu("CurrentHeadViewDirection", startPos, currentDirection->toGlobal(robotPrx), DrawColor {0, 0.5, 0.5, 0.1}, arrowLength, 5);
+ Eigen::Vector3f plannedDirectionEigen = viewTargetPositionPtr->toGlobalEigen(robotPrx) - startPos->toEigen();
+ Vector3Ptr plannedDirection = new Vector3(plannedDirectionEigen);
+ drawer->setArrowDebugLayerVisu("PlannedHeadViewDirection", startPos, plannedDirection, DrawColor {0.5, 0.5, 0, 0.1}, arrowLength, 5);
+ }
+
+ ARMARX_INFO << "Looking at target " << viewTargetPositionPtr->output();
+ headIKUnitProxy->setHeadTarget(headIKKinematicChainName, viewTargetPositionPtr);
+
+ timeOfLastViewChange = IceUtil::Time::now();
+
+
+ float duration = viewTarget->duration;
+ if (!viewTarget->duration)
+ {
+ duration = sleepingTimeBetweenViewDirectionChanges;
+ }
+
+
+ boost::this_thread::sleep(boost::posix_time::milliseconds(duration));
+ }
+ else
+ {
+ boost::this_thread::sleep(boost::posix_time::milliseconds(5));
+ }
+}
+
+
+
+void ViewSelection::addManualViewTarget(const FramedPositionBasePtr& target, const Ice::Current& c)
+{
+ boost::mutex::scoped_lock lock(manualViewTargetsMutex);
+
+ //SharedRobotInterfacePrx sharedRobotInterface = robotStateComponent->getRobotSnapshot();
+
+ ViewTargetBasePtr viewTarget = new ViewTargetBase();
+ viewTarget->priority = armarx::DEFAULT_VIEWTARGET_PRIORITY + 1;
+ viewTarget->timeAdded = new TimestampVariant(IceUtil::Time::now());
+ viewTarget->target = target;
+ viewTarget->duration = 0;
+
+ manualViewTargets.push(viewTarget);
+}
+
+void ViewSelection::clearManualViewTargets(const Ice::Current& c)
+{
+ boost::mutex::scoped_lock lock(manualViewTargetsMutex);
+
+ while (!manualViewTargets.empty())
+ {
+ manualViewTargets.pop();
+ }
+
+ // std::priority_queue<ViewTargetBasePtr, vector<ViewTargetBasePtr>, CompareViewTargets> temp;
+ // manualViewTargets.swap(temp);
+}
+
+ViewTargetList ViewSelection::getManualViewTargets(const Ice::Current& c)
+{
+ boost::mutex::scoped_lock lock(manualViewTargetsMutex);
+
+ ViewTargetList result;
+
+ std::priority_queue<ViewTargetBasePtr, vector<ViewTargetBasePtr>, CompareViewTargets> temp(manualViewTargets);
+
+ while (!temp.empty())
+ {
+ result.push_back(temp.top());
+
+ temp.pop();
+ }
+
+ return result;
+}
+
+
+
+void armarx::ViewSelection::updateSaliencyMap(const string& name, const FloatSequence& map, const Ice::Current&)
+{
+ boost::mutex::scoped_lock lock(manualViewTargetsMutex);
+
+ saliencyMaps[name] = map;
+}
+
+
+PropertyDefinitionsPtr ViewSelection::createPropertyDefinitions()
+{
+ return PropertyDefinitionsPtr(new ViewSelectionPropertyDefinitions(getConfigIdentifier()));
+}
+
+
+
diff --git a/source/RobotAPI/components/ViewSelection/ViewSelection.h b/source/RobotAPI/components/ViewSelection/ViewSelection.h
new file mode 100644
index 000000000..ce8977b57
--- /dev/null
+++ b/source/RobotAPI/components/ViewSelection/ViewSelection.h
@@ -0,0 +1,207 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2015-2016, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotComponents::ViewSelection
+ * @author David Schiebener (schiebener at kit dot edu)
+ * @author Markus Grotz ( markus dot grotz at kit dot edu )
+ * @date 2015
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#ifndef _ARMARX_COMPONENT_RobotComponents_ViewSelection_H
+#define _ARMARX_COMPONENT_RobotComponents_ViewSelection_H
+
+
+#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/observers/variant/TimestampVariant.h>
+#include <ArmarXCore/core/services/tasks/RunningTask.h>
+
+#include <RobotAPI/libraries/core/Pose.h>
+#include <RobotAPI/interface/core/RobotState.h>
+
+#include <RobotAPI/interface/components/ViewSelectionInterface.h>
+
+#include <RobotAPI/components/units/HeadIKUnit.h>
+
+#include <RobotAPI/components/EarlyVisionGraph/IntensityGraph.h>
+#include <RobotAPI/components/EarlyVisionGraph/GraphPyramidLookupTable.h>
+#include <RobotAPI/components/EarlyVisionGraph/MathTools.h>
+
+
+#include <Eigen/Geometry>
+
+
+#include <queue>
+
+namespace armarx
+{
+
+ struct CompareViewTargets
+ {
+ bool operator()(ViewTargetBasePtr const& t1, ViewTargetBasePtr const& t2)
+ {
+ if (t1->priority == t2->priority)
+ {
+ return t1->timeAdded->timestamp < t2->timeAdded->timestamp;
+
+ }
+ return t1->priority < t2->priority;
+ }
+ };
+
+
+ /**
+ * @class ViewSelectionPropertyDefinitions
+ * @brief
+ */
+ class ViewSelectionPropertyDefinitions:
+ public ComponentPropertyDefinitions
+ {
+ public:
+ ViewSelectionPropertyDefinitions(std::string prefix):
+ ComponentPropertyDefinitions(prefix)
+ {
+ defineOptionalProperty<std::string>("RobotStateComponentName", "RobotStateComponent", "Name of the robot state component that should be used");
+ defineOptionalProperty<std::string>("HeadIKUnitName", "HeadIKUnit", "Name of the head IK unit component that should be used");
+ defineOptionalProperty<std::string>("HeadIKKinematicChainName", "IKVirtualGaze", "Name of the kinematic chain for the head IK");
+ defineOptionalProperty<std::string>("HeadFrameName", "Head Base", "Name of the frame of the head base in the robot model");
+ defineOptionalProperty<std::string>("CameraFrameName", "VirtualCentralGaze", "Name of the frame of the head base in the robot model");
+ defineOptionalProperty<int>("SleepingTimeBetweenViewDirectionChanges", 2500, "Time between two view changes, to keep the head looking into one direction for a while (in ms)");
+ defineOptionalProperty<bool>("ActiveAtStartup", true, "Decide whether the automatic view selection will be activated (can be changed via the proxy during runtime)");
+ defineOptionalProperty<bool>("VisualizeViewDirection", false, "Draw view ray on DebugLayer.");
+ defineOptionalProperty<float>("MaxOverallHeadTiltAngle", 55.0f, "Maximal angle the head and eyes can look down (in degrees)");
+ defineOptionalProperty<float>("CentralHeadTiltAngle", 110.0f, "Defines the height direction that will be considered 'central' in the reachable area of the head (in degrees). Default is looking 20 degrees downwards");
+ defineOptionalProperty<float>("ProbabilityToLookForALostObject", 0.03f, "Probability that one of the objects that have been seen but could later not been localized again will be included in the view selection");
+ }
+ };
+
+ /**
+ * @defgroup Component-ViewSelection ViewSelection
+ * @ingroup RobotComponents-Components
+ * @brief The ViewSelection component controls the head of the robot with inverse kinematics based on the uncertainty
+ * of the current requested object locations.
+ * The uncertainty of objects grow based on their motion model and the timed passed since the last localization.
+ * It can be activated or deactivated with the Ice interface and given manual target positions to look at.
+ */
+
+ /**
+ * @ingroup Component-ViewSelection
+ * @brief The ViewSelection class
+ */
+ class ViewSelection :
+ virtual public Component,
+ virtual public ViewSelectionInterface
+ {
+ public:
+ /**
+ * @see armarx::ManagedIceObject::getDefaultName()
+ */
+ virtual std::string getDefaultName() const
+ {
+ return "ViewSelection";
+ }
+
+ protected:
+ /**
+ * @see armarx::ManagedIceObject::onInitComponent()
+ */
+ virtual void onInitComponent();
+
+ /**
+ * @see armarx::ManagedIceObject::onConnectComponent()
+ */
+ virtual void onConnectComponent();
+
+ /**
+ * @see armarx::ManagedIceObject::onDisconnectComponent()
+ */
+ virtual void onDisconnectComponent();
+
+ /**
+ * @see armarx::ManagedIceObject::onExitComponent()
+ */
+ virtual void onExitComponent();
+
+ /**
+ * @see PropertyUser::createPropertyDefinitions()
+ */
+ virtual PropertyDefinitionsPtr createPropertyDefinitions();
+
+ virtual void addManualViewTarget(const FramedPositionBasePtr& target, const Ice::Current& c = Ice::Current());
+
+ virtual void clearManualViewTargets(const Ice::Current& c = Ice::Current());
+
+ virtual ViewTargetList getManualViewTargets(const Ice::Current& c = Ice::Current());
+
+ virtual void activateAutomaticViewSelection(const Ice::Current& c = Ice::Current())
+ {
+ boost::mutex::scoped_lock lock(manualViewTargetsMutex);
+
+ ARMARX_INFO << "activating automatic view selection";
+ doAutomaticViewSelection = true;
+ }
+
+ virtual void deactivateAutomaticViewSelection(const Ice::Current& c = Ice::Current())
+ {
+ boost::mutex::scoped_lock lock(manualViewTargetsMutex);
+
+ ARMARX_INFO << "DEactivating automatic view selection";
+ doAutomaticViewSelection = false;
+ }
+
+ void updateSaliencyMap(const string& name, const FloatSequence& map, const Ice::Current& c = ::Ice::Current());
+
+ private:
+
+ void process();
+
+ ViewTargetBasePtr nextAutomaticViewTarget();
+
+ armarx::PeriodicTask<ViewSelection>::pointer_type processorTask;
+
+ RobotStateComponentInterfacePrx robotStateComponent;
+ HeadIKUnitInterfacePrx headIKUnitProxy;
+ DebugDrawerInterfacePrx drawer;
+
+ std::string headIKKinematicChainName;
+ std::string headFrameName;
+ std::string cameraFrameName;
+
+ CIntensityGraph* visibilityMaskGraph;
+
+ float sleepingTimeBetweenViewDirectionChanges;
+ IceUtil::Time timeOfLastViewChange;
+
+ bool drawViewDirection;
+
+ armarx::Mutex manualViewTargetsMutex;
+ std::priority_queue<ViewTargetBasePtr, vector<ViewTargetBasePtr>, CompareViewTargets> manualViewTargets;
+
+ bool doAutomaticViewSelection;
+
+ Eigen::Vector3f offsetToHeadCenter;
+
+
+ std::map<std::string, std::vector<float>> saliencyMaps;
+
+ };
+
+
+}
+
+#endif
diff --git a/source/RobotAPI/interface/CMakeLists.txt b/source/RobotAPI/interface/CMakeLists.txt
index 250737dce..1bd39e90f 100644
--- a/source/RobotAPI/interface/CMakeLists.txt
+++ b/source/RobotAPI/interface/CMakeLists.txt
@@ -36,6 +36,9 @@ set(SLICE_FILES
units/UnitInterface.ice
units/ATINetFTUnit.ice
+
+ components/ViewSelectionInterface.ice
+
visualization/DebugDrawerInterface.ice
)
#core/RobotIK.ice
diff --git a/source/RobotAPI/interface/components/ViewSelectionInterface.ice b/source/RobotAPI/interface/components/ViewSelectionInterface.ice
new file mode 100755
index 000000000..7521a30c8
--- /dev/null
+++ b/source/RobotAPI/interface/components/ViewSelectionInterface.ice
@@ -0,0 +1,75 @@
+/**
+* This file is part of ArmarX.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License version 2 as
+* published by the Free Software Foundation.
+*
+* ArmarX is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @package RobotComponents
+* @author David Schiebener (schiebener at kit dot edu)
+* @author Markus Grotz ( markus dot grotz at kit dot edu )
+* @copyright 2015 Humanoids Group, H2T, KIT
+* @license http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#ifndef _ROBOTCOMPONENTS_COMPONENT_ViewSelection_SLICE_
+#define _ROBOTCOMPONENTS_COMPONENT_ViewSelection_SLICE_
+
+#include <RobotAPI/interface/core/PoseBase.ice>
+#include <RobotAPI/interface/core/FramedPoseBase.ice>
+
+#include <ArmarXCore/interface/observers/Timestamp.ice>
+#include <ArmarXCore/interface/core/BasicTypes.ice>
+
+module armarx
+{
+
+ const int DEFAULT_VIEWTARGET_PRIORITY = 50;
+
+
+ ["cpp:virtual"]
+ class ViewTargetBase
+ {
+ armarx::FramedPositionBase target;
+
+ TimestampBase validUntil;
+
+ TimestampBase timeAdded;
+
+ float duration;
+
+ int priority;
+
+ string source;
+ };
+
+ sequence<ViewTargetBase> ViewTargetList;
+
+ interface ViewSelectionInterface
+ {
+ void addManualViewTarget(armarx::FramedPositionBase target);
+ void clearManualViewTargets();
+ ViewTargetList getManualViewTargets();
+
+ void activateAutomaticViewSelection();
+ void deactivateAutomaticViewSelection();
+
+ void updateSaliencyMap(string name, FloatSequence map);
+ };
+
+
+};
+
+#endif
+
+
+
--
GitLab