diff --git a/scenarios/OptoForceUnit/OptoForceUnit.scx b/scenarios/OptoForceUnit/OptoForceUnit.scx
index b61734dea6b15f026b6b46b0c95e46b04fd185f7..217d563cb8db1edbd5df43ab05e530c71b970069 100644
--- a/scenarios/OptoForceUnit/OptoForceUnit.scx
+++ b/scenarios/OptoForceUnit/OptoForceUnit.scx
@@ -1,5 +1,6 @@
<?xml version="1.0" encoding="utf-8"?>
-<scenario name="OptoForceUnit" lastChange="2017-03-03.15:11:46" creation="2017-03-03.15:11:40" globalConfigName="./config/global.cfg" package="RobotAPI">
+<scenario name="OptoForceUnit" lastChange="2017-03-06.18:50:02" creation="2017-03-03.15:11:40" globalConfigName="./config/global.cfg" package="RobotAPI">
<application name="OptoForceUnitApp" instance="" package="RobotAPI"/>
+ <application name="OptoForceUnitObserverApp" instance="" package="RobotAPI"/>
</scenario>
diff --git a/scenarios/OptoForceUnit/config/OptoForceUnitApp.cfg b/scenarios/OptoForceUnit/config/OptoForceUnitApp.cfg
index f5d0795f711190daabfc601e025ad26da69a7add..d36d393fe461df989545cbfd931333583a68c0aa 100644
--- a/scenarios/OptoForceUnit/config/OptoForceUnitApp.cfg
+++ b/scenarios/OptoForceUnit/config/OptoForceUnitApp.cfg
@@ -146,11 +146,6 @@
# ArmarX.Verbosity = Info
-# Ice.Config: Custom Property
-# Attributes:
-# - Default: ::NOT_DEFINED::
-# - Case sensitivity: no
-# - Required: no
-# Ice.Config = ::NOT_DEFINED::
+
diff --git a/scenarios/OptoForceUnit/config/OptoForceUnitObserverApp.cfg b/scenarios/OptoForceUnit/config/OptoForceUnitObserverApp.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..f834041ce30668ad17b90bc1f64f5efdbf7954d1
--- /dev/null
+++ b/scenarios/OptoForceUnit/config/OptoForceUnitObserverApp.cfg
@@ -0,0 +1,196 @@
+# ==================================================================
+# OptoForceUnitObserverApp properties
+# ==================================================================
+
+# ArmarX.AdditionalPackages: List of additional ArmarX packages which should be in the list of default packages. If you have custom packages, which should be found by the gui or other apps, specify them here. Comma separated List.
+# Attributes:
+# - Default: Default value not mapped.
+# - Case sensitivity: no
+# - Required: no
+# ArmarX.AdditionalPackages = Default value not mapped.
+
+
+# ArmarX.ApplicationName: Application name
+# Attributes:
+# - Default: ""
+# - Case sensitivity: no
+# - Required: no
+# ArmarX.ApplicationName = ""
+
+
+# ArmarX.CachePath: Path for cache files
+# Attributes:
+# - Default: ${HOME}/.armarx/mongo/.cache
+# - Case sensitivity: no
+# - Required: no
+# ArmarX.CachePath = ${HOME}/.armarx/mongo/.cache
+
+
+# ArmarX.Config: Comma-separated list of configuration files
+# Attributes:
+# - Default: ""
+# - Case sensitivity: no
+# - Required: no
+# ArmarX.Config = ""
+
+
+# ArmarX.DataPath: Semicolon-separated search list for data files
+# Attributes:
+# - Default: ""
+# - Case sensitivity: no
+# - Required: no
+# ArmarX.DataPath = ""
+
+
+# ArmarX.DefaultPackages: List of ArmarX packages which are accessible by default. Comma separated List. If you want to add your own packages and use all default ArmarX packages, use the property 'AdditionalPackages'.
+# Attributes:
+# - Default: Default value not mapped.
+# - Case sensitivity: no
+# - Required: no
+# ArmarX.DefaultPackages = Default value not mapped.
+
+
+# ArmarX.DependenciesConfig: Path to the (usually generated) config file containing all data paths of all dependent projects. This property usually does not need to be edited.
+# Attributes:
+# - Default: ./config/dependencies.cfg
+# - Case sensitivity: no
+# - Required: no
+# ArmarX.DependenciesConfig = ./config/dependencies.cfg
+
+
+# ArmarX.DisableLogging: Turn logging off in whole application
+# Attributes:
+# - Default: 0
+# - Case sensitivity: no
+# - Required: no
+# ArmarX.DisableLogging = 0
+
+
+# ArmarX.EnableProfiling: Enable profiling of CPU load produced by this application
+# Attributes:
+# - Default: 0
+# - Case sensitivity: no
+# - Required: no
+# ArmarX.EnableProfiling = 0
+
+
+# ArmarX.OptoForceUnitObserver.CreateUpdateFrequenciesChannel: If true, an additional channel is created that shows the update frequency of every other channel in that observer.
+# Attributes:
+# - Default: 0
+# - Case sensitivity: no
+# - Required: no
+# ArmarX.OptoForceUnitObserver.CreateUpdateFrequenciesChannel = 0
+
+
+# ArmarX.OptoForceUnitObserver.DebugDrawerTopic: Name of the DebugDrawerTopic
+# Attributes:
+# - Default: DebugDrawerUpdates
+# - Case sensitivity: no
+# - Required: no
+# ArmarX.OptoForceUnitObserver.DebugDrawerTopic = DebugDrawerUpdates
+
+
+# ArmarX.OptoForceUnitObserver.EnableProfiling: enable profiler which is used for logging performance events
+# Attributes:
+# - Default: 0
+# - Case sensitivity: no
+# - Required: no
+# ArmarX.OptoForceUnitObserver.EnableProfiling = 0
+
+
+# ArmarX.OptoForceUnitObserver.MaxHistoryRecordFrequency: The Observer history is written with this maximum frequency. Everything faster is being skipped.
+# Attributes:
+# - Default: 50
+# - Case sensitivity: no
+# - Required: no
+# ArmarX.OptoForceUnitObserver.MaxHistoryRecordFrequency = 50
+
+
+# ArmarX.OptoForceUnitObserver.MaxHistorySize: Maximum number of entries in the Observer history
+# Attributes:
+# - Default: 5000
+# - Case sensitivity: no
+# - Required: no
+# ArmarX.OptoForceUnitObserver.MaxHistorySize = 5000
+
+
+# ArmarX.OptoForceUnitObserver.MinimumLoggingLevel: Local logging level only for this component
+# Attributes:
+# - Default: Undefined
+# - Case sensitivity: no
+# - Required: no
+# ArmarX.OptoForceUnitObserver.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.OptoForceUnitObserver.ObjectName: Name of IceGrid well-known object
+# Attributes:
+# - Default: ""
+# - Case sensitivity: no
+# - Required: no
+# ArmarX.OptoForceUnitObserver.ObjectName = ""
+
+
+# ArmarX.OptoForceUnitObserver.OptoForceTopicName: Name of the OptoForce Topic
+# Attributes:
+# - Default: OptoForceValues
+# - Case sensitivity: no
+# - Required: no
+# ArmarX.OptoForceUnitObserver.OptoForceTopicName = OptoForceValues
+
+
+# ArmarX.RedirectStdout: Redirect std::cout and std::cerr to ArmarXLog
+# Attributes:
+# - Default: 1
+# - Case sensitivity: no
+# - Required: no
+# ArmarX.RedirectStdout = 1
+
+
+# ArmarX.RemoteHandlesDeletionTimeout: The timeout (in ms) before a remote handle deletes the managed object after the use count reached 0. This time can be used by a client to increment the count again (may be required when transmitting remote handles)
+# Attributes:
+# - Default: 3000
+# - Case sensitivity: no
+# - Required: no
+# ArmarX.RemoteHandlesDeletionTimeout = 3000
+
+
+# ArmarX.StartDebuggerOnCrash: If this application crashes (segmentation fault) qtcreator will attach to this process and start the debugger.
+# Attributes:
+# - Default: 0
+# - Case sensitivity: no
+# - Required: no
+# ArmarX.StartDebuggerOnCrash = 0
+
+
+# ArmarX.TopicSuffix: Suffix appended to all topic names for outgoing topics. This is mainly used to direct all topics to another name for TopicReplaying purposes.
+# Attributes:
+# - Default: ""
+# - Case sensitivity: no
+# - Required: no
+# ArmarX.TopicSuffix = ""
+
+
+# ArmarX.UseTimeServer: Enable using a global Timeserver (e.g. from ArmarXSimulator)
+# Attributes:
+# - Default: 0
+# - Case sensitivity: no
+# - Required: no
+# ArmarX.UseTimeServer = 0
+
+
+# ArmarX.Verbosity: Global logging level for whole application
+# Attributes:
+# - Default: Info
+# - Case sensitivity: no
+# - Required: no
+# ArmarX.Verbosity = Info
+
+
+# Ice.Config: Custom Property
+# Attributes:
+# - Default: ::NOT_DEFINED::
+# - Case sensitivity: no
+# - Required: no
+# Ice.Config = ::NOT_DEFINED::
+
+
diff --git a/source/RobotAPI/applications/CMakeLists.txt b/source/RobotAPI/applications/CMakeLists.txt
index e61b41ad472eef26e14cbb05a80ed58e5ce41763..77c4ef61a2e3f924f98de566c587b2fc78751b93 100644
--- a/source/RobotAPI/applications/CMakeLists.txt
+++ b/source/RobotAPI/applications/CMakeLists.txt
@@ -23,3 +23,5 @@ add_subdirectory(InertialMeasurementUnitObserver)
add_subdirectory(ViewSelection)
add_subdirectory(OptoForceUnit)
+
+add_subdirectory(OptoForceUnitObserver)
\ No newline at end of file
diff --git a/source/RobotAPI/applications/OptoForceUnitObserver/CMakeLists.txt b/source/RobotAPI/applications/OptoForceUnitObserver/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..e0f6eb95c87fc537c7fe05f4679c8fb220d01d74
--- /dev/null
+++ b/source/RobotAPI/applications/OptoForceUnitObserver/CMakeLists.txt
@@ -0,0 +1,20 @@
+armarx_component_set_name("OptoForceUnitObserverApp")
+
+#find_package(MyLib QUIET)
+#armarx_build_if(MyLib_FOUND "MyLib not available")
+#
+# all include_directories must be guarded by if(Xyz_FOUND)
+# for multiple libraries write: if(X_FOUND AND Y_FOUND)....
+#if(MyLib_FOUND)
+# include_directories(${MyLib_INCLUDE_DIRS})
+#endif()
+
+set(COMPONENT_LIBS
+ ArmarXCoreInterfaces
+ ArmarXCore
+ RobotAPIUnits
+)
+
+set(EXE_SOURCE main.cpp)
+
+armarx_add_component_executable("${EXE_SOURCE}")
diff --git a/source/RobotAPI/applications/OptoForceUnitObserver/main.cpp b/source/RobotAPI/applications/OptoForceUnitObserver/main.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..5701513b4255edbcbd3e1646b72f9bceaa7078ea
--- /dev/null
+++ b/source/RobotAPI/applications/OptoForceUnitObserver/main.cpp
@@ -0,0 +1,32 @@
+/*
+ * 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 RobotAPI::application::OptoForceUnitObserver
+ * @author Simon Ottenhaus ( simon dot ottenhaus at kit dot edu )
+ * @date 2017
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include <RobotAPI/components/units/OptoForceUnitObserver.h>
+
+#include <ArmarXCore/core/application/Application.h>
+#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/logging/Logging.h>
+
+int main(int argc, char* argv[])
+{
+ return armarx::runSimpleComponentApp < armarx::OptoForceUnitObserver > (argc, argv, "OptoForceUnitObserver");
+}
diff --git a/source/RobotAPI/components/units/CMakeLists.txt b/source/RobotAPI/components/units/CMakeLists.txt
index 90567cf96caddb0d174100b504237265888f06a2..79c9f3287e330cd411a7cff12b4070f53c22f4ee 100644
--- a/source/RobotAPI/components/units/CMakeLists.txt
+++ b/source/RobotAPI/components/units/CMakeLists.txt
@@ -32,6 +32,7 @@ set(LIB_HEADERS
HapticObserver.h
InertialMeasurementUnit.h
InertialMeasurementUnitObserver.h
+ OptoForceUnitObserver.h
TCPControlUnit.h
TCPControlUnitObserver.h
@@ -57,6 +58,7 @@ set(LIB_FILES
HapticObserver.cpp
InertialMeasurementUnit.cpp
InertialMeasurementUnitObserver.cpp
+ OptoForceUnitObserver.cpp
TCPControlUnit.cpp
TCPControlUnitObserver.cpp
diff --git a/source/RobotAPI/components/units/OptoForceUnitObserver.cpp b/source/RobotAPI/components/units/OptoForceUnitObserver.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..f70392b3024b5b5e37b7e940e6eff2864ce619c9
--- /dev/null
+++ b/source/RobotAPI/components/units/OptoForceUnitObserver.cpp
@@ -0,0 +1,146 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2011-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
+ * @author
+ * @date
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+#include "OptoForceUnitObserver.h"
+
+
+#include <ArmarXCore/observers/checks/ConditionCheckEquals.h>
+#include <ArmarXCore/observers/checks/ConditionCheckLarger.h>
+#include <ArmarXCore/observers/checks/ConditionCheckSmaller.h>
+#include <ArmarXCore/observers/checks/ConditionCheckUpdated.h>
+
+#include <RobotAPI/libraries/core/Pose.h>
+
+#include <ArmarXCore/observers/variant/TimestampVariant.h>
+
+
+
+using namespace armarx;
+
+
+void OptoForceUnitObserver::onInitObserver()
+{
+ usingTopic(getProperty<std::string>("OptoForceTopicName").getValue());
+
+ //offerConditionCheck("updated", new ConditionCheckUpdated());
+ //offerConditionCheck("larger", new ConditionCheckLarger());
+ //offerConditionCheck("equals", new ConditionCheckEquals());
+ //offerConditionCheck("smaller", new ConditionCheckSmaller());
+
+ offeringTopic(getProperty<std::string>("DebugDrawerTopic").getValue());
+}
+
+
+
+void OptoForceUnitObserver::onConnectObserver()
+{
+ debugDrawerPrx = getTopic<DebugDrawerInterfacePrx>(getProperty<std::string>("DebugDrawerTopic").getValue());
+}
+
+
+void OptoForceUnitObserver::onExitObserver()
+{
+ //debugDrawerPrx->removePoseVisu("IMU", "orientation");
+ //debugDrawerPrx->removeLineVisu("IMU", "acceleration");
+}
+
+void OptoForceUnitObserver::reportSensorValues(const std::string& device, const std::string& name, float fx, float fy, float fz, const TimestampBasePtr& timestamp, const Ice::Current& c)
+{
+ ScopedLock lock(dataMutex);
+ TimestampVariantPtr timestampPtr = TimestampVariantPtr::dynamicCast(timestamp);
+
+ if (!existsChannel(device))
+ {
+ offerChannel(device, "Force data");
+ }
+
+ offerOrUpdateDataField(device, "name", Variant(name), "Name of the sensor");
+ offerOrUpdateDataField(device, "fx", Variant(fx), "Force x");
+ offerOrUpdateDataField(device, "fy", Variant(fy), "Force y");
+ offerOrUpdateDataField(device, "fz", Variant(fz), "Force z");
+ offerOrUpdateDataField(device, "timestamp", timestampPtr, "Timestamp");
+ offerOrUpdateDataField(device, "force", Vector3(fx, fy, fz), "Force");
+
+ updateChannel(device);
+}
+
+/*void OptoForceUnitObserver::reportSensorValues(const std::string& device, const std::string& name, const IMUData& values, const TimestampBasePtr& timestamp, const Ice::Current& c)
+{
+ ScopedLock lock(dataMutex);
+
+ TimestampVariantPtr timestampPtr = TimestampVariantPtr::dynamicCast(timestamp);
+
+ Vector3Ptr acceleration = new Vector3(values.acceleration.at(0), values.acceleration.at(1), values.acceleration.at(2));
+ Vector3Ptr gyroscopeRotation = new Vector3(values.gyroscopeRotation.at(0), values.gyroscopeRotation.at(1), values.gyroscopeRotation.at(2));
+ Vector3Ptr magneticRotation = new Vector3(values.magneticRotation.at(0), values.magneticRotation.at(1), values.magneticRotation.at(2));
+ QuaternionPtr orientationQuaternion = new Quaternion(values.orientationQuaternion.at(0), values.orientationQuaternion.at(1), values.orientationQuaternion.at(2), values.orientationQuaternion.at(3));
+
+ if (!existsChannel(device))
+ {
+ offerChannel(device, "IMU data");
+ }
+
+ offerOrUpdateDataField(device, "name", Variant(name), "Name of the IMU sensor");
+ offerValue(device, "acceleration", acceleration);
+ offerValue(device, "gyroscopeRotation", gyroscopeRotation);
+ offerValue(device, "magneticRotation", magneticRotation);
+ offerValue(device, "acceleration", acceleration);
+ offerOrUpdateDataField(device, "orientationQuaternion", orientationQuaternion, "orientation quaternion values");
+ offerOrUpdateDataField(device, "timestamp", timestampPtr, "Timestamp");
+
+ updateChannel(device);
+
+ Eigen::Vector3f zero;
+ zero.setZero();
+
+ DrawColor color;
+ color.r = 1;
+ color.g = 1;
+ color.b = 0;
+ color.a = 0.5;
+
+ Eigen::Vector3f ac = acceleration->toEigen();
+ ac *= 10;
+
+ debugDrawerPrx->setLineVisu("IMU", "acceleration", new Vector3(), new Vector3(ac), 2.0f, color);
+
+ PosePtr posePtr = new Pose(orientationQuaternion->toEigen(), zero);
+ debugDrawerPrx->setPoseVisu("IMU", "orientation", posePtr);
+ debugDrawerPrx->setBoxDebugLayerVisu("floor", new Pose(), new Vector3(5000, 5000, 1), DrawColor {0.7f, 0.7f, 0.7f, 1.0f});
+}*/
+
+void OptoForceUnitObserver::offerValue(std::string device, std::string fieldName, Vector3Ptr vec)
+{
+ offerOrUpdateDataField(device, fieldName, vec, fieldName + " values");
+ offerOrUpdateDataField(device, fieldName + "_x", vec->x, fieldName + "_x value");
+ offerOrUpdateDataField(device, fieldName + "_y", vec->y, fieldName + "_y value");
+ offerOrUpdateDataField(device, fieldName + "_z", vec->z, fieldName + "_z value");
+
+}
+
+
+PropertyDefinitionsPtr OptoForceUnitObserver::createPropertyDefinitions()
+{
+ return PropertyDefinitionsPtr(new OptoForceUnitObserverPropertyDefinitions(getConfigIdentifier()));
+}
+
diff --git a/source/RobotAPI/components/units/OptoForceUnitObserver.h b/source/RobotAPI/components/units/OptoForceUnitObserver.h
new file mode 100644
index 0000000000000000000000000000000000000000..8d06c99dd98adf6c77351d6cbeb7db2772c12835
--- /dev/null
+++ b/source/RobotAPI/components/units/OptoForceUnitObserver.h
@@ -0,0 +1,93 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-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 RobotAPI::units
+ * @author David Schiebener <schiebener at kit dot edu>
+ * @date 2014
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#ifndef _ARMARX_ROBOTAPI_IMU_OBSERVER_H
+#define _ARMARX_ROBOTAPI_IMU_OBSERVER_H
+
+#include <RobotAPI/interface/units/OptoForceUnit.h>
+#include <ArmarXCore/observers/Observer.h>
+#include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
+#include <RobotAPI/libraries/core/Pose.h>
+
+
+namespace armarx
+{
+ /**
+ * \class OptoForceUnitObserverPropertyDefinitions
+ * \brief
+ */
+ class OptoForceUnitObserverPropertyDefinitions:
+ public ObserverPropertyDefinitions
+ {
+ public:
+ OptoForceUnitObserverPropertyDefinitions(std::string prefix):
+ ObserverPropertyDefinitions(prefix)
+ {
+ defineOptionalProperty<std::string>("OptoForceTopicName", "OptoForceValues", "Name of the OptoForce Topic");
+ defineOptionalProperty<std::string>("DebugDrawerTopic", "DebugDrawerUpdates", "Name of the DebugDrawerTopic");
+ }
+ };
+
+
+ /**
+ * \class OptoForceUnitObserver
+ * \ingroup RobotAPI-SensorActorUnits-observers
+ * \brief Observer monitoring @IMU sensor values
+ *
+ * The OptoForceUnitObserver monitors @IMU sensor values published by OptoForceUnit-implementations and offers condition checks on these values.
+ * Available condition checks are: *updated*, *larger*, *equals* and *smaller*.
+ */
+ class OptoForceUnitObserver :
+ virtual public Observer,
+ virtual public OptoForceUnitObserverInterface
+ {
+ public:
+ OptoForceUnitObserver() {}
+
+ virtual std::string getDefaultName() const
+ {
+ return "OptoForceUnitObserver";
+ }
+ virtual void onInitObserver();
+ virtual void onConnectObserver();
+ virtual void onExitObserver();
+
+ void reportSensorValues(const std::string& device, const std::string& name, float fx, float fy, float fz, const TimestampBasePtr& timestamp, const Ice::Current& c = ::Ice::Current());
+
+ /**
+ * @see PropertyUser::createPropertyDefinitions()
+ */
+ virtual PropertyDefinitionsPtr createPropertyDefinitions();
+
+
+ private:
+ Mutex dataMutex;
+ DebugDrawerInterfacePrx debugDrawerPrx;
+
+
+ void offerValue(std::string device, std::string fieldName, Vector3Ptr vec);
+ };
+}
+
+#endif
diff --git a/source/RobotAPI/drivers/OptoForceUnit/CMakeLists.txt b/source/RobotAPI/drivers/OptoForceUnit/CMakeLists.txt
index 6bb74c019e89e71368d3694936559db19f1da014..b1a56eba222d6b24adbcf6099a195c108059aad9 100644
--- a/source/RobotAPI/drivers/OptoForceUnit/CMakeLists.txt
+++ b/source/RobotAPI/drivers/OptoForceUnit/CMakeLists.txt
@@ -10,7 +10,7 @@ endif()
#message("OptoForceOMD_INCLUDE_DIR: " ${OptoForceOMD_INCLUDE_DIR})
#message("OptoForceOMD_LIBRARIES: " ${OptoForceOMD_LIBRARIES})
-set(COMPONENT_LIBS ArmarXCoreInterfaces ArmarXCore ${OptoForceOMD_LIBRARIES})
+set(COMPONENT_LIBS ArmarXCoreInterfaces ArmarXCore ArmarXCoreEigen3Variants RobotAPIInterfaces ${OptoForceOMD_LIBRARIES})
set(SOURCES
./OptoForceUnit.cpp
diff --git a/source/RobotAPI/drivers/OptoForceUnit/OptoForceUnit.cpp b/source/RobotAPI/drivers/OptoForceUnit/OptoForceUnit.cpp
index da4adfece6bcd40ef282c1535793552f447830dc..9f5f7bb5c1a90e77b4e78046237081d8b5e27383 100644
--- a/source/RobotAPI/drivers/OptoForceUnit/OptoForceUnit.cpp
+++ b/source/RobotAPI/drivers/OptoForceUnit/OptoForceUnit.cpp
@@ -22,12 +22,16 @@
#include "OptoForceUnit.h"
+#include <ArmarXCore/observers/variant/TimestampVariant.h>
+
using namespace armarx;
void OptoForceUnit::onInitComponent()
{
+ offeringTopic(getProperty<std::string>("OptoForceTopicName").getValue());
+
OPort* portlist = ports.listPorts(true);
ARMARX_INFO << "Found " << ports.getLastSize() << " Optoforce device(s).";
@@ -70,7 +74,7 @@ void OptoForceUnit::onInitComponent()
void OptoForceUnit::onConnectComponent()
{
-
+ topicPrx = getTopic<OptoForceUnitListenerPrx>(getProperty<std::string>("OptoForceTopicName").getValue());
}
void OptoForceUnit::run()
@@ -89,6 +93,8 @@ void OptoForceUnit::run()
ARMARX_IMPORTANT << "filter: " << filter;
ARMARX_IMPORTANT << "mode: " << mode;
+ OptoForceUnitListenerPrx batchPrx = topicPrx->ice_batchOneway();
+
while(readTask->isRunning())
{
OptoPackage* pa = 0;
@@ -113,18 +119,28 @@ void OptoForceUnit::run()
//optoForceOutputFile << IceUtil::Time::now().toDateTime() << " OptoForce: ";
// get values of first DAQ - 4 OptoForce sensors connected
+ IceUtil::Time now = IceUtil::Time::now();
+ TimestampVariantPtr nowTimestamp = new TimestampVariant(now);
+
+
for (int k = 0; k < daq.getSensorSize(); k++)
{
ARMARX_INFO << "Sensor " << k;
for (int i = 0; i < size; i++)
{
- ARMARX_INFO << "x: " << pa[k * size + i].x << " y: " << pa[k * size + i].y << " z: " << pa[k * size + i].z << " s1: " << pa[k * size + i].s1 << " s2: " << pa[k * size + i].s2 << " s3: " << pa[k * size + i].s3 << " s4: " << pa[k * size + i].s4 << " TEMP: " << pa[k * size + i].temp << flush;
- //optoForceOutputFile << "x: " << pa[k * size + i].x << " y: " << pa[k * size + i].y << " z: " << pa[k * size + i].z << " ";
+ float x = pa[k * size + i].x;
+ float y = pa[k * size + i].y;
+ float z = pa[k * size + i].z;
+ //ARMARX_INFO << "x: " << pa[k * size + i].x << " y: " << pa[k * size + i].y << " z: " << pa[k * size + i].z << " s1: " << pa[k * size + i].s1 << " s2: " << pa[k * size + i].s2 << " s3: " << pa[k * size + i].s3 << " s4: " << pa[k * size + i].s4 << " TEMP: " << pa[k * size + i].temp << flush;
+ //optoForceOutputFile << "x: " << pa[k * size + i].x << " y: " << pa[k * size + i].y << " z: " << pa[k * size + i].z << " ";
+ batchPrx->reportSensorValues("sensor" + std::to_string(k), "sensor" + std::to_string(k), x, y, z, nowTimestamp);
}
}
+ batchPrx->ice_flushBatchRequests();
+
/*// get values of second DAQ - 3 OptoForce sensors connected
//needs to be (daq2.getSensorSize()-1) to read out only 3 ports
for (int k = 0; k < (daq2.getSensorSize() - 1) ; k++)
diff --git a/source/RobotAPI/drivers/OptoForceUnit/OptoForceUnit.h b/source/RobotAPI/drivers/OptoForceUnit/OptoForceUnit.h
index 92d760307041030ae2747d8499dd43b6eee2fdf0..a4ac6efb12c9403e297a123f1166f48f8a3ee8c7 100644
--- a/source/RobotAPI/drivers/OptoForceUnit/OptoForceUnit.h
+++ b/source/RobotAPI/drivers/OptoForceUnit/OptoForceUnit.h
@@ -26,6 +26,7 @@
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/services/tasks/RunningTask.h>
+#include <RobotAPI/interface/units/OptoForceUnit.h>
#include <opto.h>
namespace armarx
@@ -42,7 +43,7 @@ namespace armarx
armarx::ComponentPropertyDefinitions(prefix)
{
//defineRequiredProperty<std::string>("PropertyName", "Description");
- //defineOptionalProperty<std::string>("PropertyName", "DefaultValue", "Description");
+ defineOptionalProperty<std::string>("OptoForceTopicName", "OptoForceValues", "Name of the OptoForce Topic");
}
};
@@ -98,6 +99,8 @@ namespace armarx
private:
void run();
+ OptoForceUnitListenerPrx topicPrx;
+
OptoPorts ports;
OptoDAQ daq;
RunningTask<OptoForceUnit>::pointer_type readTask;
diff --git a/source/RobotAPI/interface/CMakeLists.txt b/source/RobotAPI/interface/CMakeLists.txt
index 1ccbfdb6b7cd84c766f83d2e9bdbb6e2d1612c5c..ba7831b0ec40a504b3d159c45297b08a8fcbccd8 100644
--- a/source/RobotAPI/interface/CMakeLists.txt
+++ b/source/RobotAPI/interface/CMakeLists.txt
@@ -23,6 +23,7 @@ set(SLICE_FILES
units/ForceTorqueUnit.ice
units/InertialMeasurementUnit.ice
+ units/OptoForceUnit.ice
units/HandUnitInterface.ice
units/HapticUnit.ice
units/WeissHapticUnit.ice
diff --git a/source/RobotAPI/interface/units/OptoForceUnit.ice b/source/RobotAPI/interface/units/OptoForceUnit.ice
new file mode 100644
index 0000000000000000000000000000000000000000..5f1017e7b3db1173cbd767b21a498fff8990318d
--- /dev/null
+++ b/source/RobotAPI/interface/units/OptoForceUnit.ice
@@ -0,0 +1,71 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-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 RobotAPI
+ * @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_ROBOTAPI_UNITS_INERTIALMEASUREMENTUNIT_SLICE_
+#define _ARMARX_ROBOTAPI_UNITS_INERTIALMEASUREMENTUNIT_SLICE_
+
+
+#include <RobotAPI/interface/units/UnitInterface.ice>
+#include <RobotAPI/interface/core/RobotState.ice>
+
+#include <ArmarXCore/interface/core/UserException.ice>
+#include <ArmarXCore/interface/core/BasicTypes.ice>
+#include <ArmarXCore/interface/observers/VariantBase.ice>
+#include <ArmarXCore/interface/observers/Matrix.ice>
+#include <ArmarXCore/interface/observers/Timestamp.ice>
+#include <ArmarXCore/interface/observers/ObserverInterface.ice>
+
+
+
+module armarx
+{
+ /**
+ * Implements an interface to an OptoForceUnit.
+ **/
+ interface OptoForceUnitInterface extends armarx::SensorActorUnitInterface
+ {
+ };
+ /**
+ * Implements an interface to an OptoForceUnitListener.
+ **/
+ interface OptoForceUnitListener
+ {
+ /**
+ * reportSensorValues reports the IMU sensor values from a given sensor device.
+ * @param device Name of IMU sensor device.
+ * @param values IMU sensor data.
+ * @param timestamp Timestamp of the measurement.
+ **/
+ void reportSensorValues(string device, string name, float fx, float fy, float fz, TimestampBase timestamp);
+ };
+ /**
+ * Implements an interface to an OptoForceUnitObserver.
+ **/
+ interface OptoForceUnitObserverInterface extends ObserverInterface, OptoForceUnitListener
+ {
+ };
+
+};
+
+#endif