From 763e652891bd32110c365e85620bfe2b6b673c03 Mon Sep 17 00:00:00 2001
From: andreeatulbure <andreea_tulbure@yahoo.de>
Date: Mon, 27 Feb 2017 14:25:12 +0100
Subject: [PATCH] added observer
---
.../OrientedTactileSensor.scx | 6 +
.../config/OrientedTactileSensorUnitApp.cfg | 167 +++++++++++++++
.../OrientedTactileSensorUnitObserverApp.cfg | 191 ++++++++++++++++++
.../OrientedTactileSensor/config/global.cfg | 7 +
source/RobotAPI/applications/CMakeLists.txt | 2 +
.../CMakeLists.txt | 20 ++
.../main.cpp | 32 +++
.../RobotAPI/components/units/CMakeLists.txt | 2 +
.../OrientedTactileSensorUnitObserver.cpp | 105 ++++++++++
.../units/OrientedTactileSensorUnitObserver.h | 89 ++++++++
.../OrientedTactileSensorUnit.cpp | 39 ++--
.../OrientedTactileSensorUnit.h | 9 +-
.../units/OrientedTactileSensorUnit.ice | 9 +-
13 files changed, 642 insertions(+), 36 deletions(-)
create mode 100644 scenarios/OrientedTactileSensor/OrientedTactileSensor.scx
create mode 100644 scenarios/OrientedTactileSensor/config/OrientedTactileSensorUnitApp.cfg
create mode 100644 scenarios/OrientedTactileSensor/config/OrientedTactileSensorUnitObserverApp.cfg
create mode 100644 scenarios/OrientedTactileSensor/config/global.cfg
create mode 100644 source/RobotAPI/applications/OrientedTactileSensorUnitObserver/CMakeLists.txt
create mode 100644 source/RobotAPI/applications/OrientedTactileSensorUnitObserver/main.cpp
create mode 100644 source/RobotAPI/components/units/OrientedTactileSensorUnitObserver.cpp
create mode 100644 source/RobotAPI/components/units/OrientedTactileSensorUnitObserver.h
diff --git a/scenarios/OrientedTactileSensor/OrientedTactileSensor.scx b/scenarios/OrientedTactileSensor/OrientedTactileSensor.scx
new file mode 100644
index 000000000..ec0c5693b
--- /dev/null
+++ b/scenarios/OrientedTactileSensor/OrientedTactileSensor.scx
@@ -0,0 +1,6 @@
+<?xml version="1.0" encoding="utf-8"?>
+<scenario name="OrientedTactileSensor" lastChange="2017-02-27.01:54:44 PM" creation="2017-02-27.01:48:55 PM" globalConfigName="./config/global.cfg" package="RobotAPI">
+ <application name="OrientedTactileSensorUnitApp" instance="" package="RobotAPI"/>
+ <application name="OrientedTactileSensorUnitObserverApp" instance="" package="RobotAPI"/>
+</scenario>
+
diff --git a/scenarios/OrientedTactileSensor/config/OrientedTactileSensorUnitApp.cfg b/scenarios/OrientedTactileSensor/config/OrientedTactileSensorUnitApp.cfg
new file mode 100644
index 000000000..fe53aea1b
--- /dev/null
+++ b/scenarios/OrientedTactileSensor/config/OrientedTactileSensorUnitApp.cfg
@@ -0,0 +1,167 @@
+# ==================================================================
+# OrientedTactileSensorUnitApp 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.OrientedTactileSensorUnit.EnableProfiling: enable profiler which is used for logging performance events
+# Attributes:
+# - Default: 0
+# - Case sensitivity: no
+# - Required: no
+# ArmarX.OrientedTactileSensorUnit.EnableProfiling = 0
+
+
+# ArmarX.OrientedTactileSensorUnit.MinimumLoggingLevel: Local logging level only for this component
+# Attributes:
+# - Default: Undefined
+# - Case sensitivity: no
+# - Required: no
+# ArmarX.OrientedTactileSensorUnit.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.OrientedTactileSensorUnit.ObjectName: Name of IceGrid well-known object
+# Attributes:
+# - Default: ""
+# - Case sensitivity: no
+# - Required: no
+# ArmarX.OrientedTactileSensorUnit.ObjectName = ""
+
+
+# ArmarX.OrientedTactileSensorUnit.SerialInterfaceDevice: The serial device the arduino is connected to.
+# Attributes:
+# - Default: /dev/ttyACM0
+# - Case sensitivity: no
+# - Required: no
+ArmarX.OrientedTactileSensorUnit.SerialInterfaceDevice = /dev/ttyACM1
+
+
+# ArmarX.OrientedTactileSensorUnit.TopicName: Name of the topic on which the sensor values are provided
+# Attributes:
+# - Default: OrientedTactileSensorValues
+# - Case sensitivity: no
+# - Required: no
+# ArmarX.OrientedTactileSensorUnit.TopicName = OrientedTactileSensorValues
+
+
+# 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
+
+
+
+
+
diff --git a/scenarios/OrientedTactileSensor/config/OrientedTactileSensorUnitObserverApp.cfg b/scenarios/OrientedTactileSensor/config/OrientedTactileSensorUnitObserverApp.cfg
new file mode 100644
index 000000000..5f35ab285
--- /dev/null
+++ b/scenarios/OrientedTactileSensor/config/OrientedTactileSensorUnitObserverApp.cfg
@@ -0,0 +1,191 @@
+# ==================================================================
+# OrientedTactileSensorUnitObserverApp 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.OrientedTactileSensorUnitObserver.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.OrientedTactileSensorUnitObserver.CreateUpdateFrequenciesChannel = 0
+
+
+# ArmarX.OrientedTactileSensorUnitObserver.DebugDrawerTopic: Name of the DebugDrawerTopic
+# Attributes:
+# - Default: DebugDrawerUpdates
+# - Case sensitivity: no
+# - Required: no
+# ArmarX.OrientedTactileSensorUnitObserver.DebugDrawerTopic = DebugDrawerUpdates
+
+
+# ArmarX.OrientedTactileSensorUnitObserver.EnableProfiling: enable profiler which is used for logging performance events
+# Attributes:
+# - Default: 0
+# - Case sensitivity: no
+# - Required: no
+# ArmarX.OrientedTactileSensorUnitObserver.EnableProfiling = 0
+
+
+# ArmarX.OrientedTactileSensorUnitObserver.MaxHistoryRecordFrequency: The Observer history is written with this maximum frequency. Everything faster is being skipped.
+# Attributes:
+# - Default: 50
+# - Case sensitivity: no
+# - Required: no
+# ArmarX.OrientedTactileSensorUnitObserver.MaxHistoryRecordFrequency = 50
+
+
+# ArmarX.OrientedTactileSensorUnitObserver.MaxHistorySize: Maximum number of entries in the Observer history
+# Attributes:
+# - Default: 5000
+# - Case sensitivity: no
+# - Required: no
+# ArmarX.OrientedTactileSensorUnitObserver.MaxHistorySize = 5000
+
+
+# ArmarX.OrientedTactileSensorUnitObserver.MinimumLoggingLevel: Local logging level only for this component
+# Attributes:
+# - Default: Undefined
+# - Case sensitivity: no
+# - Required: no
+# ArmarX.OrientedTactileSensorUnitObserver.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.OrientedTactileSensorUnitObserver.ObjectName: Name of IceGrid well-known object
+# Attributes:
+# - Default: ""
+# - Case sensitivity: no
+# - Required: no
+# ArmarX.OrientedTactileSensorUnitObserver.ObjectName = ""
+
+
+# ArmarX.OrientedTactileSensorUnitObserver.SensorTopicName: Name of the Sensor Topic.
+# Attributes:
+# - Default: OrientedTactileSensorValues
+# - Case sensitivity: no
+# - Required: no
+# ArmarX.OrientedTactileSensorUnitObserver.SensorTopicName = OrientedTactileSensorValues
+
+
+# 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
+
+
+
+
+
diff --git a/scenarios/OrientedTactileSensor/config/global.cfg b/scenarios/OrientedTactileSensor/config/global.cfg
new file mode 100644
index 000000000..e3da6095b
--- /dev/null
+++ b/scenarios/OrientedTactileSensor/config/global.cfg
@@ -0,0 +1,7 @@
+# ==================================================================
+# Global Config from Scenario OrientedTactileSensor
+# ==================================================================
+
+
+
+
diff --git a/source/RobotAPI/applications/CMakeLists.txt b/source/RobotAPI/applications/CMakeLists.txt
index 76c27df1b..1f4d55e28 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(OrientedTactileSensorUnit)
+
+add_subdirectory(OrientedTactileSensorUnitObserver)
\ No newline at end of file
diff --git a/source/RobotAPI/applications/OrientedTactileSensorUnitObserver/CMakeLists.txt b/source/RobotAPI/applications/OrientedTactileSensorUnitObserver/CMakeLists.txt
new file mode 100644
index 000000000..ec058b57f
--- /dev/null
+++ b/source/RobotAPI/applications/OrientedTactileSensorUnitObserver/CMakeLists.txt
@@ -0,0 +1,20 @@
+armarx_component_set_name("OrientedTactileSensorUnitObserverApp")
+
+#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/OrientedTactileSensorUnitObserver/main.cpp b/source/RobotAPI/applications/OrientedTactileSensorUnitObserver/main.cpp
new file mode 100644
index 000000000..e400516ab
--- /dev/null
+++ b/source/RobotAPI/applications/OrientedTactileSensorUnitObserver/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::OrientedTactileSensorUnitObserver
+ * @author andreeatulbure ( andreea_tulbure at yahoo dot de )
+ * @date 2017
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include <RobotAPI/components/units/OrientedTactileSensorUnitObserver.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::OrientedTactileSensorUnitObserver > (argc, argv, "OrientedTactileSensorUnitObserver");
+}
diff --git a/source/RobotAPI/components/units/CMakeLists.txt b/source/RobotAPI/components/units/CMakeLists.txt
index 90567cf96..66291132a 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
+ OrientedTactileSensorUnitObserver.h
TCPControlUnit.h
TCPControlUnitObserver.h
@@ -57,6 +58,7 @@ set(LIB_FILES
HapticObserver.cpp
InertialMeasurementUnit.cpp
InertialMeasurementUnitObserver.cpp
+ OrientedTactileSensorUnitObserver.cpp
TCPControlUnit.cpp
TCPControlUnitObserver.cpp
diff --git a/source/RobotAPI/components/units/OrientedTactileSensorUnitObserver.cpp b/source/RobotAPI/components/units/OrientedTactileSensorUnitObserver.cpp
new file mode 100644
index 000000000..98f60e6c9
--- /dev/null
+++ b/source/RobotAPI/components/units/OrientedTactileSensorUnitObserver.cpp
@@ -0,0 +1,105 @@
+/*
+ * 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 "OrientedTactileSensorUnitObserver.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 OrientedTactileSensorUnitObserver::onInitObserver()
+{
+ usingTopic(getProperty<std::string>("SensorTopicName").getValue());
+ offeringTopic(getProperty<std::string>("DebugDrawerTopic").getValue());
+}
+
+
+
+void OrientedTactileSensorUnitObserver::onConnectObserver()
+{
+ debugDrawerPrx = getTopic<DebugDrawerInterfacePrx>(getProperty<std::string>("DebugDrawerTopic").getValue());
+}
+
+
+void OrientedTactileSensorUnitObserver::onExitObserver()
+{
+ //debugDrawerPrx->removePoseVisu("IMU", "orientation");
+ //debugDrawerPrx->removeLineVisu("IMU", "acceleration");
+}
+
+void OrientedTactileSensorUnitObserver::reportSensorValues(int id, float pressure, float qw, float qx, float qy, float qz, const TimestampBasePtr& timestamp, const Ice::Current&)
+{
+ ScopedLock lock(dataMutex);
+ TimestampVariantPtr timestampPtr = TimestampVariantPtr::dynamicCast(timestamp);
+
+ std::stringstream ss;
+ ss << "sensor" << id;
+ std::string channelName = ss.str();
+
+ if (!existsChannel(channelName))
+ {
+ offerChannel(channelName, "Sensor Data");
+ }
+
+ offerOrUpdateDataField(channelName, "pressure", Variant(pressure), "current pressure");
+ QuaternionPtr orientationQuaternion = new Quaternion(qw, qx, qy, qz);
+ offerOrUpdateDataField(channelName, "orientation", orientationQuaternion, "current oriantation");
+}
+/* TODO: for integration with debug drawer
+ 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});
+*/
+
+
+PropertyDefinitionsPtr OrientedTactileSensorUnitObserver::createPropertyDefinitions()
+{
+ return PropertyDefinitionsPtr(new OrientedTactileSensorUnitObserverPropertyDefinitions(getConfigIdentifier()));
+}
diff --git a/source/RobotAPI/components/units/OrientedTactileSensorUnitObserver.h b/source/RobotAPI/components/units/OrientedTactileSensorUnitObserver.h
new file mode 100644
index 000000000..2b95757d8
--- /dev/null
+++ b/source/RobotAPI/components/units/OrientedTactileSensorUnitObserver.h
@@ -0,0 +1,89 @@
+/*
+ * 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/OrientedTactileSensorUnit.h>
+#include <ArmarXCore/observers/Observer.h>
+#include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
+#include <RobotAPI/libraries/core/Pose.h>
+
+
+namespace armarx
+{
+ /**
+ * \class OrientedTactileSensorUnitObserverPropertyDefinitions
+ * \brief
+ */
+ class OrientedTactileSensorUnitObserverPropertyDefinitions:
+ public ObserverPropertyDefinitions
+ {
+ public:
+ OrientedTactileSensorUnitObserverPropertyDefinitions(std::string prefix):
+ ObserverPropertyDefinitions(prefix)
+ {
+ defineOptionalProperty<std::string>("SensorTopicName", "OrientedTactileSensorValues", "Name of the Sensor Topic.");
+ defineOptionalProperty<std::string>("DebugDrawerTopic", "DebugDrawerUpdates", "Name of the DebugDrawerTopic");
+ }
+ };
+
+
+ /**
+ * \class OrientedTactileSensorUnitObserver
+ * \ingroup RobotAPI-SensorActorUnits-observers
+ * \brief Observer monitoring @IMU sensor values
+ *
+ * The OrientedTactileSensorUnitObserver monitors @IMU sensor values published by OrientedTactileSensorUnit-implementations and offers condition checks on these values.
+ */
+ class OrientedTactileSensorUnitObserver :
+ virtual public Observer,
+ virtual public OrientedTactileSensorUnitObserverInterface
+ {
+ public:
+ OrientedTactileSensorUnitObserver() {}
+
+ virtual std::string getDefaultName() const
+ {
+ return "OrientedTactileSensorUnitObserver";
+ }
+ virtual void onInitObserver();
+ virtual void onConnectObserver();
+ virtual void onExitObserver();
+
+ void reportSensorValues(int id, float pressure, float qw, float qx, float qy, float qz, const TimestampBasePtr& timestamp, const Ice::Current&);
+
+ /**
+ * @see PropertyUser::createPropertyDefinitions()
+ */
+ virtual PropertyDefinitionsPtr createPropertyDefinitions();
+
+
+ private:
+ Mutex dataMutex;
+ DebugDrawerInterfacePrx debugDrawerPrx;
+ };
+}
+
+#endif
diff --git a/source/RobotAPI/drivers/OrientedTactileSensor/OrientedTactileSensorUnit.cpp b/source/RobotAPI/drivers/OrientedTactileSensor/OrientedTactileSensorUnit.cpp
index 758e770e9..747d27439 100644
--- a/source/RobotAPI/drivers/OrientedTactileSensor/OrientedTactileSensorUnit.cpp
+++ b/source/RobotAPI/drivers/OrientedTactileSensor/OrientedTactileSensorUnit.cpp
@@ -3,6 +3,7 @@
#include <sys/ioctl.h>
#include <fcntl.h>
#include <math.h>
+#include <ArmarXCore/observers/variant/TimestampVariant.h>
using namespace armarx;
@@ -10,10 +11,10 @@ OrientedTactileSensorUnit::OrientedTactileSensorUnit()
{
}
-
void OrientedTactileSensorUnit::onInitComponent()
{
- offeringTopic("OrientedTactileSensorUnit");
+ std::string topicName = getProperty<std::string>("TopicName").getValue();
+ offeringTopic(topicName);
//open serial port
std::string portname = getProperty<std::string>("SerialInterfaceDevice").getValue();
@@ -21,19 +22,25 @@ void OrientedTactileSensorUnit::onInitComponent()
fd = open(portname.c_str(), O_RDONLY | O_NOCTTY);
struct termios toptions;
+
/* Get currently set options for the tty */
tcgetattr(fd, &toptions);
+
/* Set custom options */
cfsetispeed(&toptions, B115200);
cfsetospeed(&toptions, B115200);
+
/* 8 bits, no parity, no stop bits */
toptions.c_lflag = 0;
toptions.c_iflag = 0;
toptions.c_oflag = 0;
+
/* commit the options */
tcsetattr(fd, TCSANOW, &toptions);
+
/* Wait for the Arduino to reset */
usleep(1000 * 1000);
+
/* Flush anything already in the serial buffer */
tcflush(fd, TCIFLUSH);
@@ -67,10 +74,8 @@ void OrientedTactileSensorUnit::onInitComponent()
void OrientedTactileSensorUnit::onConnectComponent()
{
- //std::string topicName = getProperty<std::string>("ArduinoIMUUnit").getValue();
std::string topicName = getProperty<std::string>("TopicName").getValue();
- listenerOrientedTactileSensor = getTopic<OrientedTactileSensorUnitListenerPrx>(topicName);
- ARMARX_INFO << "Topic Name given" << flush;
+ topicPrx = getTopic<OrientedTactileSensorUnitListenerPrx>(topicName);
}
PropertyDefinitionsPtr OrientedTactileSensorUnit::createPropertyDefinitions()
@@ -85,25 +90,13 @@ void OrientedTactileSensorUnit::run()
{
std::string line;
getline(arduino, line, '\n');
- //ARMARX_IMPORTANT << line;
- getValues(line.c_str());
- /*if (recording)
+ SensorData data = getValues(line.c_str());
+ IceUtil::Time now = IceUtil::Time::now();
+ TimestampVariantPtr nowTimestamp = new TimestampVariant(now);
+ if (topicPrx)
{
- ScopedLock lock(fileMutex);
-
-
- // get sensor data from serial
- getValues(line.c_str());
-
- outputFile << IceUtil::Time::now().toDateTime() << " " << "Id: " << sensorData[0] << " Pressure: " << sensorData[1] << " IMU: " << sensorData[2] << " " << sensorData[3] << " " << sensorData[4] << " " << sensorData[5] << "\n";
- ARMARX_INFO << "Id: " << sensorData[0] << " Pressure: " << sensorData[1] << " IMU: " << sensorData[2] << " " << sensorData[3] << " " << sensorData[4] << " " << sensorData[5] << "\n";
- //outputFile << IceUtil::Time::now().toDateTime() << " " << line << "\n";
- //ARMARX_INFO << line;
-
- outputFile.flush();
-
- //listenerIMU->reportForceSensorValues(sensorData[0], sensorData[1], sensorData[2], sensorData[3], sensorData[4], sensorData[5]);
- }*/
+ topicPrx->reportSensorValues(data.id, data.pressure, data.qw, data.qx, data.qy, data.qz, nowTimestamp);
+ }
}
}
diff --git a/source/RobotAPI/drivers/OrientedTactileSensor/OrientedTactileSensorUnit.h b/source/RobotAPI/drivers/OrientedTactileSensor/OrientedTactileSensorUnit.h
index 481c4ec20..e5739e7ca 100644
--- a/source/RobotAPI/drivers/OrientedTactileSensor/OrientedTactileSensorUnit.h
+++ b/source/RobotAPI/drivers/OrientedTactileSensor/OrientedTactileSensorUnit.h
@@ -28,7 +28,7 @@ namespace armarx
defineOptionalProperty<std::string>(
"TopicName",
- "OrientedTactileSensorUnit",
+ "OrientedTactileSensorValues",
"Name of the topic on which the sensor values are provided");
}
@@ -41,7 +41,7 @@ namespace armarx
*/
class OrientedTactileSensorUnit:
virtual public armarx::Component
- //virtual public MMMCaptureSensorInterface
+ //TODO: needs interface to send calibration data
{
public:
OrientedTactileSensorUnit();
@@ -66,12 +66,9 @@ namespace armarx
private:
std::fstream arduino;
- //std::ofstream outputFile;
- //Mutex fileMutex;
RunningTask<OrientedTactileSensorUnit>::pointer_type readTask;
- OrientedTactileSensorUnitListenerPrx listenerOrientedTactileSensor;
+ OrientedTactileSensorUnitListenerPrx topicPrx;
- //void periodicExec();
void run();
SensorData getValues(std::string s);
int fd;
diff --git a/source/RobotAPI/interface/units/OrientedTactileSensorUnit.ice b/source/RobotAPI/interface/units/OrientedTactileSensorUnit.ice
index a198be582..d41e55367 100644
--- a/source/RobotAPI/interface/units/OrientedTactileSensorUnit.ice
+++ b/source/RobotAPI/interface/units/OrientedTactileSensorUnit.ice
@@ -25,6 +25,7 @@
#include <RobotAPI/interface/core/PoseBase.ice>
#include <RobotAPI/interface/units/UnitInterface.ice>
+#include <ArmarXCore/interface/observers/Timestamp.ice>
#include <ArmarXCore/interface/core/UserException.ice>
module armarx
@@ -40,17 +41,11 @@ module armarx
interface OrientedTactileSensorUnitInterface extends SensorActorUnitInterface
{
- //void startRecording(string customName);
-
- //void stopRecording();
-
- bool isComponentOnline();
};
interface OrientedTactileSensorUnitListener
{
- void reportSensorValues(int id, float pressure, float qw, float qx, float qy, float qz);
-
+ void reportSensorValues(int id, float pressure, float qw, float qx, float qy, float qz, TimestampBase timestamp);
};
--
GitLab