diff --git a/.gitignore b/.gitignore
index 1422f5933373b19b58e3759a11b1c1df41bb86c9..055ed57ae3899188b3f99356d1f8a5d1e6261837 100644
--- a/.gitignore
+++ b/.gitignore
@@ -2,6 +2,7 @@
!/build/.gitkeep
!.gitkeep
!.gitignore
+.idea
source/*/Test.h
@@ -13,13 +14,14 @@ source/*/Test.h
.*.kate-swp
.*.swo
*.pyc
+*.orig
.DS_Store
CMakeFiles
CMakeCache.txt
CMakeLists.txt.user*
*.autosave
-*.orig
+*~
*.o
*.os
@@ -38,33 +40,35 @@ moc_*
.cproject
.project
-# clion stuff
-/.idea
-
# MemoryX
.cache
mongod.log*
data/db/
data/dbdump/
+# Test files
+*.roundtriptest
+
# Generated Scenario Files
-scenarios/*/startScenario.sh
-scenarios/*/stopScenario.sh
-scenarios/*/ttyACM*.js
-scenarios/*/ttyACM*.log
-scenarios/*/config/datapath.cfg
-scenarios/*/*/startScenario.sh
-scenarios/*/*/stopScenario.sh
-scenarios/*/*/ttyACM*.js
-scenarios/*/*/ttyACM*.log
-scenarios/*/*/config/datapath.cfg
+startScenario.sh
+stopScenario.sh
+ttyACM*.js
+ttyACM*.log
+datapath*.cfg
*.icegrid.xml
data/RobotAPI/logs
+worktree/
+
+etc/python/armarx.egg-info/
+etc/python/build/*
+etc/python/dist/*
+
.cmake/api/v1/query/cache-v2
.cmake/api/v1/query/codemodel-v2
.cmake/api/v1/query/cmakeFiles-v1
# ArViz Recordings
data/RobotAPI/ArVizStorage/
+
diff --git a/scenarios/ArMemTest/ArMemTest.scx b/scenarios/ArMemTest/ArMemTest.scx
new file mode 100644
index 0000000000000000000000000000000000000000..3d87047035ab68dc65bbc3737b29b2412a1fc406
--- /dev/null
+++ b/scenarios/ArMemTest/ArMemTest.scx
@@ -0,0 +1,5 @@
+<?xml version="1.0" encoding="utf-8"?>
+<scenario name="ArMemTest" creation="2020-08-27.13:58:39" globalConfigName="./config/global.cfg" package="RobotAPI" deploymentType="local" nodeName="NodeMain">
+ <application name="ArMemGlobalStorageApp" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
+</scenario>
+
diff --git a/scenarios/ArMemTest/config/ArMemGlobalStorageApp.cfg b/scenarios/ArMemTest/config/ArMemGlobalStorageApp.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..732d150006239e21446a55b289aba86b35ca3549
--- /dev/null
+++ b/scenarios/ArMemTest/config/ArMemGlobalStorageApp.cfg
@@ -0,0 +1,196 @@
+# ==================================================================
+# ArMemGlobalStorageApp 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: yes
+# - Required: no
+# ArmarX.AdditionalPackages = Default value not mapped.
+
+
+# ArmarX.ApplicationName: Application name
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ApplicationName = ""
+
+
+# ArmarX.ArMemGlobalStorage.EnableProfiling: enable profiler which is used for logging performance events
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ArMemGlobalStorage.EnableProfiling = false
+
+
+# ArmarX.ArMemGlobalStorage.LocalMemoryHostnames: The hostnames of the local memories
+# Attributes:
+# - Default: Default value not mapped.
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ArMemGlobalStorage.LocalMemoryHostnames = Default value not mapped.
+
+
+# ArmarX.ArMemGlobalStorage.MinimumLoggingLevel: Local logging level only for this component
+# Attributes:
+# - Default: Undefined
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
+# ArmarX.ArMemGlobalStorage.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.ArMemGlobalStorage.ObjectName: Name of IceGrid well-known object
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ArMemGlobalStorage.ObjectName = ""
+
+
+# ArmarX.CachePath: Path for cache files. If relative path AND env. variable ARMARX_USER_CONFIG_DIR is set, the cache path will be made relative to ARMARX_USER_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${HOME}/.armarx)
+# Attributes:
+# - Default: mongo/.cache
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.CachePath = mongo/.cache
+
+
+# ArmarX.Config: Comma-separated list of configuration files
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.Config = ""
+
+
+# ArmarX.DataPath: Semicolon-separated search list for data files
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - 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: yes
+# - 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: yes
+# - Required: no
+# ArmarX.DependenciesConfig = ./config/dependencies.cfg
+
+
+# ArmarX.DisableLogging: Turn logging off in whole application
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.DisableLogging = false
+
+
+# ArmarX.EnableProfiling: Enable profiling of CPU load produced by this application
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.EnableProfiling = false
+
+
+# ArmarX.LoadLibraries: Libraries to load at start up of the application. Must be enabled by the Application with enableLibLoading(). Format: PackageName:LibraryName;... or /absolute/path/to/library;...
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.LoadLibraries = ""
+
+
+# ArmarX.LoggingGroup: The logging group is transmitted with every ArmarX log message over Ice in order to group the message in the GUI.
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.LoggingGroup = ""
+
+
+# ArmarX.RedirectStdout: Redirect std::cout and std::cerr to ArmarXLog
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.RedirectStdout = true
+
+
+# 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: yes
+# - Required: no
+# ArmarX.RemoteHandlesDeletionTimeout = 3000
+
+
+# ArmarX.SecondsStartupDelay: The startup will be delayed by this number of seconds (useful for debugging)
+# Attributes:
+# - Default: 0
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.SecondsStartupDelay = 0
+
+
+# ArmarX.StartDebuggerOnCrash: If this application crashes (segmentation fault) qtcreator will attach to this process and start the debugger.
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.StartDebuggerOnCrash = false
+
+
+# ArmarX.ThreadPoolSize: Size of the ArmarX ThreadPool that is always running.
+# Attributes:
+# - Default: 1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ThreadPoolSize = 1
+
+
+# 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: yes
+# - Required: no
+# ArmarX.TopicSuffix = ""
+
+
+# ArmarX.UseTimeServer: Enable using a global Timeserver (e.g. from ArmarXSimulator)
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.UseTimeServer = false
+
+
+# ArmarX.Verbosity: Global logging level for whole application
+# Attributes:
+# - Default: Info
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
+# ArmarX.Verbosity = Info
+
+
diff --git a/scenarios/ArMemTest/config/global.cfg b/scenarios/ArMemTest/config/global.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..32f673479975ccb811f4605afdd8ca6772ca0d66
--- /dev/null
+++ b/scenarios/ArMemTest/config/global.cfg
@@ -0,0 +1,4 @@
+# ==================================================================
+# Global Config from Scenario ArMemTest
+# ==================================================================
+
diff --git a/scenarios/ArVizExample/ArVizExample.scx b/scenarios/ArVizExample/ArVizExample.scx
index bcc46e466e16bf2cb69264abcfbbef745296e3e1..f4df70650c21d77e2f5d1ac810d4c0301c1bba9c 100644
--- a/scenarios/ArVizExample/ArVizExample.scx
+++ b/scenarios/ArVizExample/ArVizExample.scx
@@ -3,5 +3,6 @@
<application name="ArVizExample" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
<application name="ArVizStorage" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
<application name="RobotToArVizApp" instance="" package="RobotAPI" nodeName="" enabled="false" iceAutoRestart="false"/>
+ <application name="DebugObserver" instance="" package="ArmarXCore" nodeName="" enabled="true" iceAutoRestart="false"/>
</scenario>
diff --git a/scenarios/ArVizExample/config/DebugObserver.cfg b/scenarios/ArVizExample/config/DebugObserver.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..4a0b9dac036cd4d103efd7d1b718d508f285d85a
--- /dev/null
+++ b/scenarios/ArVizExample/config/DebugObserver.cfg
@@ -0,0 +1,221 @@
+# ==================================================================
+# DebugObserver 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: yes
+# - Required: no
+# ArmarX.AdditionalPackages = Default value not mapped.
+
+
+# ArmarX.ApplicationName: Application name
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ApplicationName = ""
+
+
+# ArmarX.CachePath: Path for cache files. If relative path AND env. variable ARMARX_USER_CONFIG_DIR is set, the cache path will be made relative to ARMARX_USER_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${HOME}/.armarx)
+# Attributes:
+# - Default: mongo/.cache
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.CachePath = mongo/.cache
+
+
+# ArmarX.Config: Comma-separated list of configuration files
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.Config = ""
+
+
+# ArmarX.DataPath: Semicolon-separated search list for data files
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DataPath = ""
+
+
+# ArmarX.DebugObserver.CreateUpdateFrequenciesChannel: If true, an additional channel is created that shows the update frequency of every other channel in that observer.
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.DebugObserver.CreateUpdateFrequenciesChannel = false
+
+
+# ArmarX.DebugObserver.DebugObserverTopicName: Name of the topic the DebugObserver listens on
+# Attributes:
+# - Default: DebugObserver
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DebugObserver.DebugObserverTopicName = DebugObserver
+
+
+# ArmarX.DebugObserver.EnableProfiling: enable profiler which is used for logging performance events
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.DebugObserver.EnableProfiling = false
+
+
+# ArmarX.DebugObserver.MaxHistoryRecordFrequency: The Observer history is written with this maximum frequency. Everything faster is being skipped.
+# Attributes:
+# - Default: 50
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DebugObserver.MaxHistoryRecordFrequency = 50
+
+
+# ArmarX.DebugObserver.MaxHistorySize: Maximum number of entries in the Observer history
+# Attributes:
+# - Default: 5000
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DebugObserver.MaxHistorySize = 5000
+
+
+# ArmarX.DebugObserver.MinimumLoggingLevel: Local logging level only for this component
+# Attributes:
+# - Default: Undefined
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
+# ArmarX.DebugObserver.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.DebugObserver.ObjectName: Name of IceGrid well-known object
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DebugObserver.ObjectName = ""
+
+
+# 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: yes
+# - 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: yes
+# - Required: no
+# ArmarX.DependenciesConfig = ./config/dependencies.cfg
+
+
+# ArmarX.DisableLogging: Turn logging off in whole application
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.DisableLogging = false
+
+
+# ArmarX.EnableProfiling: Enable profiling of CPU load produced by this application
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.EnableProfiling = false
+
+
+# ArmarX.LoadLibraries: Libraries to load at start up of the application. Must be enabled by the Application with enableLibLoading(). Format: PackageName:LibraryName;... or /absolute/path/to/library;...
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.LoadLibraries = ""
+
+
+# ArmarX.LoggingGroup: The logging group is transmitted with every ArmarX log message over Ice in order to group the message in the GUI.
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.LoggingGroup = ""
+
+
+# ArmarX.RedirectStdout: Redirect std::cout and std::cerr to ArmarXLog
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.RedirectStdout = true
+
+
+# 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: yes
+# - Required: no
+# ArmarX.RemoteHandlesDeletionTimeout = 3000
+
+
+# ArmarX.SecondsStartupDelay: The startup will be delayed by this number of seconds (useful for debugging)
+# Attributes:
+# - Default: 0
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.SecondsStartupDelay = 0
+
+
+# ArmarX.StartDebuggerOnCrash: If this application crashes (segmentation fault) qtcreator will attach to this process and start the debugger.
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.StartDebuggerOnCrash = false
+
+
+# ArmarX.ThreadPoolSize: Size of the ArmarX ThreadPool that is always running.
+# Attributes:
+# - Default: 1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ThreadPoolSize = 1
+
+
+# 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: yes
+# - Required: no
+# ArmarX.TopicSuffix = ""
+
+
+# ArmarX.UseTimeServer: Enable using a global Timeserver (e.g. from ArmarXSimulator)
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.UseTimeServer = false
+
+
+# ArmarX.Verbosity: Global logging level for whole application
+# Attributes:
+# - Default: Info
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
+# ArmarX.Verbosity = Info
+
+
diff --git a/source/RobotAPI/applications/AronArmemGenerator/cxxopts.hpp b/source/RobotAPI/applications/AronArmemGenerator/cxxopts.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..a11201123e2b6c17939c960c9b7c8239fbdc80dc
--- /dev/null
+++ b/source/RobotAPI/applications/AronArmemGenerator/cxxopts.hpp
@@ -0,0 +1,2097 @@
+/*
+
+Copyright (c) 2014, 2015, 2016, 2017 Jarryd Beck
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in
+all copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+THE SOFTWARE.
+
+*/
+
+#ifndef CXXOPTS_HPP_INCLUDED
+#define CXXOPTS_HPP_INCLUDED
+
+#include <cstring>
+#include <cctype>
+#include <exception>
+#include <iostream>
+#include <map>
+#include <memory>
+#include <regex>
+#include <sstream>
+#include <string>
+#include <unordered_map>
+#include <unordered_set>
+#include <vector>
+
+#ifdef __cpp_lib_optional
+#include <optional>
+#define CXXOPTS_HAS_OPTIONAL
+#endif
+
+#define CXXOPTS__VERSION_MAJOR 2
+#define CXXOPTS__VERSION_MINOR 2
+#define CXXOPTS__VERSION_PATCH 0
+
+namespace cxxopts
+{
+ static constexpr struct
+ {
+ uint8_t major, minor, patch;
+ } version =
+ {
+ CXXOPTS__VERSION_MAJOR,
+ CXXOPTS__VERSION_MINOR,
+ CXXOPTS__VERSION_PATCH
+ };
+}
+
+//when we ask cxxopts to use Unicode, help strings are processed using ICU,
+//which results in the correct lengths being computed for strings when they
+//are formatted for the help output
+//it is necessary to make sure that <unicode/unistr.h> can be found by the
+//compiler, and that icu-uc is linked in to the binary.
+
+#ifdef CXXOPTS_USE_UNICODE
+#include <unicode/unistr.h>
+
+namespace cxxopts
+{
+ typedef icu::UnicodeString String;
+
+ inline
+ String
+ toLocalString(std::string s)
+ {
+ return icu::UnicodeString::fromUTF8(std::move(s));
+ }
+
+ class UnicodeStringIterator : public
+ std::iterator<std::forward_iterator_tag, int32_t>
+ {
+ public:
+
+ UnicodeStringIterator(const icu::UnicodeString* string, int32_t pos)
+ : s(string)
+ , i(pos)
+ {
+ }
+
+ value_type
+ operator*() const
+ {
+ return s->char32At(i);
+ }
+
+ bool
+ operator==(const UnicodeStringIterator& rhs) const
+ {
+ return s == rhs.s && i == rhs.i;
+ }
+
+ bool
+ operator!=(const UnicodeStringIterator& rhs) const
+ {
+ return !(*this == rhs);
+ }
+
+ UnicodeStringIterator&
+ operator++()
+ {
+ ++i;
+ return *this;
+ }
+
+ UnicodeStringIterator
+ operator+(int32_t v)
+ {
+ return UnicodeStringIterator(s, i + v);
+ }
+
+ private:
+ const icu::UnicodeString* s;
+ int32_t i;
+ };
+
+ inline
+ String&
+ stringAppend(String& s, String a)
+ {
+ return s.append(std::move(a));
+ }
+
+ inline
+ String&
+ stringAppend(String& s, int n, UChar32 c)
+ {
+ for (int i = 0; i != n; ++i)
+ {
+ s.append(c);
+ }
+
+ return s;
+ }
+
+ template <typename Iterator>
+ String&
+ stringAppend(String& s, Iterator begin, Iterator end)
+ {
+ while (begin != end)
+ {
+ s.append(*begin);
+ ++begin;
+ }
+
+ return s;
+ }
+
+ inline
+ size_t
+ stringLength(const String& s)
+ {
+ return s.length();
+ }
+
+ inline
+ std::string
+ toUTF8String(const String& s)
+ {
+ std::string result;
+ s.toUTF8String(result);
+
+ return result;
+ }
+
+ inline
+ bool
+ empty(const String& s)
+ {
+ return s.isEmpty();
+ }
+}
+
+namespace std
+{
+ inline
+ cxxopts::UnicodeStringIterator
+ begin(const icu::UnicodeString& s)
+ {
+ return cxxopts::UnicodeStringIterator(&s, 0);
+ }
+
+ inline
+ cxxopts::UnicodeStringIterator
+ end(const icu::UnicodeString& s)
+ {
+ return cxxopts::UnicodeStringIterator(&s, s.length());
+ }
+}
+
+//ifdef CXXOPTS_USE_UNICODE
+#else
+
+namespace cxxopts
+{
+ typedef std::string String;
+
+ template <typename T>
+ T
+ toLocalString(T&& t)
+ {
+ return std::forward<T>(t);
+ }
+
+ inline
+ size_t
+ stringLength(const String& s)
+ {
+ return s.length();
+ }
+
+ inline
+ String&
+ stringAppend(String& s, String a)
+ {
+ return s.append(std::move(a));
+ }
+
+ inline
+ String&
+ stringAppend(String& s, size_t n, char c)
+ {
+ return s.append(n, c);
+ }
+
+ template <typename Iterator>
+ String&
+ stringAppend(String& s, Iterator begin, Iterator end)
+ {
+ return s.append(begin, end);
+ }
+
+ template <typename T>
+ std::string
+ toUTF8String(T&& t)
+ {
+ return std::forward<T>(t);
+ }
+
+ inline
+ bool
+ empty(const std::string& s)
+ {
+ return s.empty();
+ }
+}
+
+//ifdef CXXOPTS_USE_UNICODE
+#endif
+
+namespace cxxopts
+{
+ namespace
+ {
+#ifdef _WIN32
+ const std::string LQUOTE("\'");
+ const std::string RQUOTE("\'");
+#else
+ const std::string LQUOTE("‘");
+ const std::string RQUOTE("’");
+#endif
+ }
+
+ class Value : public std::enable_shared_from_this<Value>
+ {
+ public:
+
+ virtual ~Value() = default;
+
+ virtual
+ std::shared_ptr<Value>
+ clone() const = 0;
+
+ virtual void
+ parse(const std::string& text) const = 0;
+
+ virtual void
+ parse() const = 0;
+
+ virtual bool
+ has_default() const = 0;
+
+ virtual bool
+ is_container() const = 0;
+
+ virtual bool
+ has_implicit() const = 0;
+
+ virtual std::string
+ get_default_value() const = 0;
+
+ virtual std::string
+ get_implicit_value() const = 0;
+
+ virtual std::shared_ptr<Value>
+ default_value(const std::string& value) = 0;
+
+ virtual std::shared_ptr<Value>
+ implicit_value(const std::string& value) = 0;
+
+ virtual bool
+ is_boolean() const = 0;
+ };
+
+ class OptionException : public std::exception
+ {
+ public:
+ OptionException(const std::string& message)
+ : m_message(message)
+ {
+ }
+
+ virtual const char*
+ what() const noexcept
+ {
+ return m_message.c_str();
+ }
+
+ private:
+ std::string m_message;
+ };
+
+ class OptionSpecException : public OptionException
+ {
+ public:
+
+ OptionSpecException(const std::string& message)
+ : OptionException(message)
+ {
+ }
+ };
+
+ class OptionParseException : public OptionException
+ {
+ public:
+ OptionParseException(const std::string& message)
+ : OptionException(message)
+ {
+ }
+ };
+
+ class option_exists_error : public OptionSpecException
+ {
+ public:
+ option_exists_error(const std::string& option)
+ : OptionSpecException("Option " + LQUOTE + option + RQUOTE + " already exists")
+ {
+ }
+ };
+
+ class invalid_option_format_error : public OptionSpecException
+ {
+ public:
+ invalid_option_format_error(const std::string& format)
+ : OptionSpecException("Invalid option format " + LQUOTE + format + RQUOTE)
+ {
+ }
+ };
+
+ class option_syntax_exception : public OptionParseException
+ {
+ public:
+ option_syntax_exception(const std::string& text)
+ : OptionParseException("Argument " + LQUOTE + text + RQUOTE +
+ " starts with a - but has incorrect syntax")
+ {
+ }
+ };
+
+ class option_not_exists_exception : public OptionParseException
+ {
+ public:
+ option_not_exists_exception(const std::string& option)
+ : OptionParseException("Option " + LQUOTE + option + RQUOTE + " does not exist")
+ {
+ }
+ };
+
+ class missing_argument_exception : public OptionParseException
+ {
+ public:
+ missing_argument_exception(const std::string& option)
+ : OptionParseException(
+ "Option " + LQUOTE + option + RQUOTE + " is missing an argument"
+ )
+ {
+ }
+ };
+
+ class option_requires_argument_exception : public OptionParseException
+ {
+ public:
+ option_requires_argument_exception(const std::string& option)
+ : OptionParseException(
+ "Option " + LQUOTE + option + RQUOTE + " requires an argument"
+ )
+ {
+ }
+ };
+
+ class option_not_has_argument_exception : public OptionParseException
+ {
+ public:
+ option_not_has_argument_exception
+ (
+ const std::string& option,
+ const std::string& arg
+ )
+ : OptionParseException(
+ "Option " + LQUOTE + option + RQUOTE +
+ " does not take an argument, but argument " +
+ LQUOTE + arg + RQUOTE + " given"
+ )
+ {
+ }
+ };
+
+ class option_not_present_exception : public OptionParseException
+ {
+ public:
+ option_not_present_exception(const std::string& option)
+ : OptionParseException("Option " + LQUOTE + option + RQUOTE + " not present")
+ {
+ }
+ };
+
+ class argument_incorrect_type : public OptionParseException
+ {
+ public:
+ argument_incorrect_type
+ (
+ const std::string& arg
+ )
+ : OptionParseException(
+ "Argument " + LQUOTE + arg + RQUOTE + " failed to parse"
+ )
+ {
+ }
+ };
+
+ class option_required_exception : public OptionParseException
+ {
+ public:
+ option_required_exception(const std::string& option)
+ : OptionParseException(
+ "Option " + LQUOTE + option + RQUOTE + " is required but not present"
+ )
+ {
+ }
+ };
+
+ namespace values
+ {
+ namespace
+ {
+ std::basic_regex<char> integer_pattern
+ ("(-)?(0x)?([0-9a-zA-Z]+)|((0x)?0)");
+ std::basic_regex<char> truthy_pattern
+ ("(t|T)(rue)?");
+ std::basic_regex<char> falsy_pattern
+ ("((f|F)(alse)?)?");
+ }
+
+ namespace detail
+ {
+ template <typename T, bool B>
+ struct SignedCheck;
+
+ template <typename T>
+ struct SignedCheck<T, true>
+ {
+ template <typename U>
+ void
+ operator()(bool negative, U u, const std::string& text)
+ {
+ if (negative)
+ {
+ if (u > static_cast<U>(-(std::numeric_limits<T>::min)()))
+ {
+ throw argument_incorrect_type(text);
+ }
+ }
+ else
+ {
+ if (u > static_cast<U>((std::numeric_limits<T>::max)()))
+ {
+ throw argument_incorrect_type(text);
+ }
+ }
+ }
+ };
+
+ template <typename T>
+ struct SignedCheck<T, false>
+ {
+ template <typename U>
+ void
+ operator()(bool, U, const std::string&) {}
+ };
+
+ template <typename T, typename U>
+ void
+ check_signed_range(bool negative, U value, const std::string& text)
+ {
+ SignedCheck<T, std::numeric_limits<T>::is_signed>()(negative, value, text);
+ }
+ }
+
+ template <typename R, typename T>
+ R
+ checked_negate(T&& t, const std::string&, std::true_type)
+ {
+ // if we got to here, then `t` is a positive number that fits into
+ // `R`. So to avoid MSVC C4146, we first cast it to `R`.
+ // See https://github.com/jarro2783/cxxopts/issues/62 for more details.
+ return -static_cast<R>(t);
+ }
+
+ template <typename R, typename T>
+ T
+ checked_negate(T&&, const std::string& text, std::false_type)
+ {
+ throw argument_incorrect_type(text);
+ }
+
+ template <typename T>
+ void
+ integer_parser(const std::string& text, T& value)
+ {
+ std::smatch match;
+ std::regex_match(text, match, integer_pattern);
+
+ if (match.length() == 0)
+ {
+ throw argument_incorrect_type(text);
+ }
+
+ if (match.length(4) > 0)
+ {
+ value = 0;
+ return;
+ }
+
+ using US = typename std::make_unsigned<T>::type;
+
+ constexpr auto umax = (std::numeric_limits<US>::max)();
+ constexpr bool is_signed = std::numeric_limits<T>::is_signed;
+ const bool negative = match.length(1) > 0;
+ const uint8_t base = match.length(2) > 0 ? 16 : 10;
+
+ auto value_match = match[3];
+
+ US result = 0;
+
+ for (auto iter = value_match.first; iter != value_match.second; ++iter)
+ {
+ US digit = 0;
+
+ if (*iter >= '0' && *iter <= '9')
+ {
+ digit = *iter - '0';
+ }
+ else if (base == 16 && *iter >= 'a' && *iter <= 'f')
+ {
+ digit = *iter - 'a' + 10;
+ }
+ else if (base == 16 && *iter >= 'A' && *iter <= 'F')
+ {
+ digit = *iter - 'A' + 10;
+ }
+ else
+ {
+ throw argument_incorrect_type(text);
+ }
+
+ if (umax - digit < result * base)
+ {
+ throw argument_incorrect_type(text);
+ }
+
+ result = result * base + digit;
+ }
+
+ detail::check_signed_range<T>(negative, result, text);
+
+ if (negative)
+ {
+ value = checked_negate<T>(result,
+ text,
+ std::integral_constant<bool, is_signed>());
+ }
+ else
+ {
+ value = result;
+ }
+ }
+
+ template <typename T>
+ void stringstream_parser(const std::string& text, T& value)
+ {
+ std::stringstream in(text);
+ in >> value;
+ if (!in)
+ {
+ throw argument_incorrect_type(text);
+ }
+ }
+
+ inline
+ void
+ parse_value(const std::string& text, uint8_t& value)
+ {
+ integer_parser(text, value);
+ }
+
+ inline
+ void
+ parse_value(const std::string& text, int8_t& value)
+ {
+ integer_parser(text, value);
+ }
+
+ inline
+ void
+ parse_value(const std::string& text, uint16_t& value)
+ {
+ integer_parser(text, value);
+ }
+
+ inline
+ void
+ parse_value(const std::string& text, int16_t& value)
+ {
+ integer_parser(text, value);
+ }
+
+ inline
+ void
+ parse_value(const std::string& text, uint32_t& value)
+ {
+ integer_parser(text, value);
+ }
+
+ inline
+ void
+ parse_value(const std::string& text, int32_t& value)
+ {
+ integer_parser(text, value);
+ }
+
+ inline
+ void
+ parse_value(const std::string& text, uint64_t& value)
+ {
+ integer_parser(text, value);
+ }
+
+ inline
+ void
+ parse_value(const std::string& text, int64_t& value)
+ {
+ integer_parser(text, value);
+ }
+
+ inline
+ void
+ parse_value(const std::string& text, bool& value)
+ {
+ std::smatch result;
+ std::regex_match(text, result, truthy_pattern);
+
+ if (!result.empty())
+ {
+ value = true;
+ return;
+ }
+
+ std::regex_match(text, result, falsy_pattern);
+ if (!result.empty())
+ {
+ value = false;
+ return;
+ }
+
+ throw argument_incorrect_type(text);
+ }
+
+ inline
+ void
+ parse_value(const std::string& text, std::string& value)
+ {
+ value = text;
+ }
+
+ // The fallback parser. It uses the stringstream parser to parse all types
+ // that have not been overloaded explicitly. It has to be placed in the
+ // source code before all other more specialized templates.
+ template <typename T>
+ void
+ parse_value(const std::string& text, T& value)
+ {
+ stringstream_parser(text, value);
+ }
+
+ template <typename T>
+ void
+ parse_value(const std::string& text, std::vector<T>& value)
+ {
+ T v;
+ parse_value(text, v);
+ value.push_back(v);
+ }
+
+#ifdef CXXOPTS_HAS_OPTIONAL
+ template <typename T>
+ void
+ parse_value(const std::string& text, std::optional<T>& value)
+ {
+ T result;
+ parse_value(text, result);
+ value = std::move(result);
+ }
+#endif
+
+ template <typename T>
+ struct type_is_container
+ {
+ static constexpr bool value = false;
+ };
+
+ template <typename T>
+ struct type_is_container<std::vector<T>>
+ {
+ static constexpr bool value = true;
+ };
+
+ template <typename T>
+ class abstract_value : public Value
+ {
+ using Self = abstract_value<T>;
+
+ public:
+ abstract_value()
+ : m_result(std::make_shared<T>())
+ , m_store(m_result.get())
+ {
+ }
+
+ abstract_value(T* t)
+ : m_store(t)
+ {
+ }
+
+ virtual ~abstract_value() = default;
+
+ abstract_value(const abstract_value& rhs)
+ {
+ if (rhs.m_result)
+ {
+ m_result = std::make_shared<T>();
+ m_store = m_result.get();
+ }
+ else
+ {
+ m_store = rhs.m_store;
+ }
+
+ m_default = rhs.m_default;
+ m_implicit = rhs.m_implicit;
+ m_default_value = rhs.m_default_value;
+ m_implicit_value = rhs.m_implicit_value;
+ }
+
+ void
+ parse(const std::string& text) const
+ {
+ parse_value(text, *m_store);
+ }
+
+ bool
+ is_container() const
+ {
+ return type_is_container<T>::value;
+ }
+
+ void
+ parse() const
+ {
+ parse_value(m_default_value, *m_store);
+ }
+
+ bool
+ has_default() const
+ {
+ return m_default;
+ }
+
+ bool
+ has_implicit() const
+ {
+ return m_implicit;
+ }
+
+ std::shared_ptr<Value>
+ default_value(const std::string& value)
+ {
+ m_default = true;
+ m_default_value = value;
+ return shared_from_this();
+ }
+
+ std::shared_ptr<Value>
+ implicit_value(const std::string& value)
+ {
+ m_implicit = true;
+ m_implicit_value = value;
+ return shared_from_this();
+ }
+
+ std::string
+ get_default_value() const
+ {
+ return m_default_value;
+ }
+
+ std::string
+ get_implicit_value() const
+ {
+ return m_implicit_value;
+ }
+
+ bool
+ is_boolean() const
+ {
+ return std::is_same<T, bool>::value;
+ }
+
+ const T&
+ get() const
+ {
+ if (m_store == nullptr)
+ {
+ return *m_result;
+ }
+ else
+ {
+ return *m_store;
+ }
+ }
+
+ protected:
+ std::shared_ptr<T> m_result;
+ T* m_store;
+
+ bool m_default = false;
+ bool m_implicit = false;
+
+ std::string m_default_value;
+ std::string m_implicit_value;
+ };
+
+ template <typename T>
+ class standard_value : public abstract_value<T>
+ {
+ public:
+ using abstract_value<T>::abstract_value;
+
+ std::shared_ptr<Value>
+ clone() const
+ {
+ return std::make_shared<standard_value<T>>(*this);
+ }
+ };
+
+ template <>
+ class standard_value<bool> : public abstract_value<bool>
+ {
+ public:
+ ~standard_value() = default;
+
+ standard_value()
+ {
+ set_default_and_implicit();
+ }
+
+ standard_value(bool* b)
+ : abstract_value(b)
+ {
+ set_default_and_implicit();
+ }
+
+ std::shared_ptr<Value>
+ clone() const
+ {
+ return std::make_shared<standard_value<bool>>(*this);
+ }
+
+ private:
+
+ void
+ set_default_and_implicit()
+ {
+ m_default = true;
+ m_default_value = "false";
+ m_implicit = true;
+ m_implicit_value = "true";
+ }
+ };
+ }
+
+ template <typename T>
+ std::shared_ptr<Value>
+ value()
+ {
+ return std::make_shared<values::standard_value<T>>();
+ }
+
+ template <typename T>
+ std::shared_ptr<Value>
+ value(T& t)
+ {
+ return std::make_shared<values::standard_value<T>>(&t);
+ }
+
+ class OptionAdder;
+
+ class OptionDetails
+ {
+ public:
+ OptionDetails
+ (
+ const std::string& short_,
+ const std::string& long_,
+ const String& desc,
+ std::shared_ptr<const Value> val
+ )
+ : m_short(short_)
+ , m_long(long_)
+ , m_desc(desc)
+ , m_value(val)
+ , m_count(0)
+ {
+ }
+
+ OptionDetails(const OptionDetails& rhs)
+ : m_desc(rhs.m_desc)
+ , m_count(rhs.m_count)
+ {
+ m_value = rhs.m_value->clone();
+ }
+
+ OptionDetails(OptionDetails&& rhs) = default;
+
+ const String&
+ description() const
+ {
+ return m_desc;
+ }
+
+ const Value& value() const
+ {
+ return *m_value;
+ }
+
+ std::shared_ptr<Value>
+ make_storage() const
+ {
+ return m_value->clone();
+ }
+
+ const std::string&
+ short_name() const
+ {
+ return m_short;
+ }
+
+ const std::string&
+ long_name() const
+ {
+ return m_long;
+ }
+
+ private:
+ std::string m_short;
+ std::string m_long;
+ String m_desc;
+ std::shared_ptr<const Value> m_value;
+ int m_count;
+ };
+
+ struct HelpOptionDetails
+ {
+ std::string s;
+ std::string l;
+ String desc;
+ bool has_default;
+ std::string default_value;
+ bool has_implicit;
+ std::string implicit_value;
+ std::string arg_help;
+ bool is_container;
+ bool is_boolean;
+ };
+
+ struct HelpGroupDetails
+ {
+ std::string name;
+ std::string description;
+ std::vector<HelpOptionDetails> options;
+ };
+
+ class OptionValue
+ {
+ public:
+ void
+ parse
+ (
+ std::shared_ptr<const OptionDetails> details,
+ const std::string& text
+ )
+ {
+ ensure_value(details);
+ ++m_count;
+ m_value->parse(text);
+ }
+
+ void
+ parse_default(std::shared_ptr<const OptionDetails> details)
+ {
+ ensure_value(details);
+ m_value->parse();
+ }
+
+ size_t
+ count() const
+ {
+ return m_count;
+ }
+
+ template <typename T>
+ const T&
+ as() const
+ {
+ if (m_value == nullptr)
+ {
+ throw std::domain_error("No value");
+ }
+
+#ifdef CXXOPTS_NO_RTTI
+ return static_cast<const values::standard_value<T>&>(*m_value).get();
+#else
+ return dynamic_cast<const values::standard_value<T>&>(*m_value).get();
+#endif
+ }
+
+ private:
+ void
+ ensure_value(std::shared_ptr<const OptionDetails> details)
+ {
+ if (m_value == nullptr)
+ {
+ m_value = details->make_storage();
+ }
+ }
+
+ std::shared_ptr<Value> m_value;
+ size_t m_count = 0;
+ };
+
+ class KeyValue
+ {
+ public:
+ KeyValue(std::string key_, std::string value_)
+ : m_key(std::move(key_))
+ , m_value(std::move(value_))
+ {
+ }
+
+ const
+ std::string&
+ key() const
+ {
+ return m_key;
+ }
+
+ const
+ std::string&
+ value() const
+ {
+ return m_value;
+ }
+
+ template <typename T>
+ T
+ as() const
+ {
+ T result;
+ values::parse_value(m_value, result);
+ return result;
+ }
+
+ private:
+ std::string m_key;
+ std::string m_value;
+ };
+
+ class ParseResult
+ {
+ public:
+
+ ParseResult(
+ const std::shared_ptr <
+ std::unordered_map<std::string, std::shared_ptr<OptionDetails>>
+ >,
+ std::vector<std::string>,
+ bool allow_unrecognised,
+ int&, char**&);
+
+ size_t
+ count(const std::string& o) const
+ {
+ auto iter = m_options->find(o);
+ if (iter == m_options->end())
+ {
+ return 0;
+ }
+
+ auto riter = m_results.find(iter->second);
+
+ return riter->second.count();
+ }
+
+ const OptionValue&
+ operator[](const std::string& option) const
+ {
+ auto iter = m_options->find(option);
+
+ if (iter == m_options->end())
+ {
+ throw option_not_present_exception(option);
+ }
+
+ auto riter = m_results.find(iter->second);
+
+ return riter->second;
+ }
+
+ const std::vector<KeyValue>&
+ arguments() const
+ {
+ return m_sequential;
+ }
+
+ private:
+
+ void
+ parse(int& argc, char**& argv);
+
+ void
+ add_to_option(const std::string& option, const std::string& arg);
+
+ bool
+ consume_positional(std::string a);
+
+ void
+ parse_option
+ (
+ std::shared_ptr<OptionDetails> value,
+ const std::string& name,
+ const std::string& arg = ""
+ );
+
+ void
+ parse_default(std::shared_ptr<OptionDetails> details);
+
+ void
+ checked_parse_arg
+ (
+ int argc,
+ char* argv[],
+ int& current,
+ std::shared_ptr<OptionDetails> value,
+ const std::string& name
+ );
+
+ const std::shared_ptr <
+ std::unordered_map<std::string, std::shared_ptr<OptionDetails>>
+ > m_options;
+ std::vector<std::string> m_positional;
+ std::vector<std::string>::iterator m_next_positional;
+ std::unordered_set<std::string> m_positional_set;
+ std::unordered_map<std::shared_ptr<OptionDetails>, OptionValue> m_results;
+
+ bool m_allow_unrecognised;
+
+ std::vector<KeyValue> m_sequential;
+ };
+
+ class Options
+ {
+ typedef std::unordered_map<std::string, std::shared_ptr<OptionDetails>>
+ OptionMap;
+ public:
+
+ Options(std::string program, std::string help_string = "")
+ : m_program(std::move(program))
+ , m_help_string(toLocalString(std::move(help_string)))
+ , m_custom_help("[OPTION...]")
+ , m_positional_help("positional parameters")
+ , m_show_positional(false)
+ , m_allow_unrecognised(false)
+ , m_options(std::make_shared<OptionMap>())
+ , m_next_positional(m_positional.end())
+ {
+ }
+
+ Options&
+ positional_help(std::string help_text)
+ {
+ m_positional_help = std::move(help_text);
+ return *this;
+ }
+
+ Options&
+ custom_help(std::string help_text)
+ {
+ m_custom_help = std::move(help_text);
+ return *this;
+ }
+
+ Options&
+ show_positional_help()
+ {
+ m_show_positional = true;
+ return *this;
+ }
+
+ Options&
+ allow_unrecognised_options()
+ {
+ m_allow_unrecognised = true;
+ return *this;
+ }
+
+ ParseResult
+ parse(int& argc, char**& argv);
+
+ OptionAdder
+ add_options(std::string group = "");
+
+ void
+ add_option
+ (
+ const std::string& group,
+ const std::string& s,
+ const std::string& l,
+ std::string desc,
+ std::shared_ptr<const Value> value,
+ std::string arg_help
+ );
+
+ //parse positional arguments into the given option
+ void
+ parse_positional(std::string option);
+
+ void
+ parse_positional(std::vector<std::string> options);
+
+ void
+ parse_positional(std::initializer_list<std::string> options);
+
+ template <typename Iterator>
+ void
+ parse_positional(Iterator begin, Iterator end)
+ {
+ parse_positional(std::vector<std::string> {begin, end});
+ }
+
+ std::string
+ help(const std::vector<std::string>& groups = {}) const;
+
+ const std::vector<std::string>
+ groups() const;
+
+ const HelpGroupDetails&
+ group_help(const std::string& group) const;
+
+ private:
+
+ void
+ add_one_option
+ (
+ const std::string& option,
+ std::shared_ptr<OptionDetails> details
+ );
+
+ String
+ help_one_group(const std::string& group) const;
+
+ void
+ generate_group_help
+ (
+ String& result,
+ const std::vector<std::string>& groups
+ ) const;
+
+ void
+ generate_all_groups_help(String& result) const;
+
+ std::string m_program;
+ String m_help_string;
+ std::string m_custom_help;
+ std::string m_positional_help;
+ bool m_show_positional;
+ bool m_allow_unrecognised;
+
+ std::shared_ptr<OptionMap> m_options;
+ std::vector<std::string> m_positional;
+ std::vector<std::string>::iterator m_next_positional;
+ std::unordered_set<std::string> m_positional_set;
+
+ //mapping from groups to help options
+ std::map<std::string, HelpGroupDetails> m_help;
+ };
+
+ class OptionAdder
+ {
+ public:
+
+ OptionAdder(Options& options, std::string group)
+ : m_options(options), m_group(std::move(group))
+ {
+ }
+
+ OptionAdder&
+ operator()
+ (
+ const std::string& opts,
+ const std::string& desc,
+ std::shared_ptr<const Value> value
+ = ::cxxopts::value<bool>(),
+ std::string arg_help = ""
+ );
+
+ private:
+ Options& m_options;
+ std::string m_group;
+ };
+
+ namespace
+ {
+ constexpr int OPTION_LONGEST = 30;
+ constexpr int OPTION_DESC_GAP = 2;
+
+ std::basic_regex<char> option_matcher
+ ("--([[:alnum:]][-_[:alnum:]]+)(=(.*))?|-([[:alnum:]]+)");
+
+ std::basic_regex<char> option_specifier
+ ("(([[:alnum:]]),)?[ ]*([[:alnum:]][-_[:alnum:]]*)?");
+
+ String
+ format_option
+ (
+ const HelpOptionDetails& o
+ )
+ {
+ auto& s = o.s;
+ auto& l = o.l;
+
+ String result = " ";
+
+ if (s.size() > 0)
+ {
+ result += "-" + toLocalString(s) + ",";
+ }
+ else
+ {
+ result += " ";
+ }
+
+ if (l.size() > 0)
+ {
+ result += " --" + toLocalString(l);
+ }
+
+ auto arg = o.arg_help.size() > 0 ? toLocalString(o.arg_help) : "arg";
+
+ if (!o.is_boolean)
+ {
+ if (o.has_implicit)
+ {
+ result += " [=" + arg + "(=" + toLocalString(o.implicit_value) + ")]";
+ }
+ else
+ {
+ result += " " + arg;
+ }
+ }
+
+ return result;
+ }
+
+ String
+ format_description
+ (
+ const HelpOptionDetails& o,
+ size_t start,
+ size_t width
+ )
+ {
+ auto desc = o.desc;
+
+ if (o.has_default && (!o.is_boolean || o.default_value != "false"))
+ {
+ desc += toLocalString(" (default: " + o.default_value + ")");
+ }
+
+ String result;
+
+ auto current = std::begin(desc);
+ auto startLine = current;
+ auto lastSpace = current;
+
+ auto size = size_t{};
+
+ while (current != std::end(desc))
+ {
+ if (*current == ' ')
+ {
+ lastSpace = current;
+ }
+
+ if (*current == '\n')
+ {
+ startLine = current + 1;
+ lastSpace = startLine;
+ }
+ else if (size > width)
+ {
+ if (lastSpace == startLine)
+ {
+ stringAppend(result, startLine, current + 1);
+ stringAppend(result, "\n");
+ stringAppend(result, start, ' ');
+ startLine = current + 1;
+ lastSpace = startLine;
+ }
+ else
+ {
+ stringAppend(result, startLine, lastSpace);
+ stringAppend(result, "\n");
+ stringAppend(result, start, ' ');
+ startLine = lastSpace + 1;
+ }
+ size = 0;
+ }
+ else
+ {
+ ++size;
+ }
+
+ ++current;
+ }
+
+ //append whatever is left
+ stringAppend(result, startLine, current);
+
+ return result;
+ }
+ }
+
+ inline
+ ParseResult::ParseResult
+ (
+ const std::shared_ptr <
+ std::unordered_map<std::string, std::shared_ptr<OptionDetails>>
+ > options,
+ std::vector<std::string> positional,
+ bool allow_unrecognised,
+ int& argc, char**& argv
+ )
+ : m_options(options)
+ , m_positional(std::move(positional))
+ , m_next_positional(m_positional.begin())
+ , m_allow_unrecognised(allow_unrecognised)
+ {
+ parse(argc, argv);
+ }
+
+ inline
+ OptionAdder
+ Options::add_options(std::string group)
+ {
+ return OptionAdder(*this, std::move(group));
+ }
+
+ inline
+ OptionAdder&
+ OptionAdder::operator()
+ (
+ const std::string& opts,
+ const std::string& desc,
+ std::shared_ptr<const Value> value,
+ std::string arg_help
+ )
+ {
+ std::match_results<const char*> result;
+ std::regex_match(opts.c_str(), result, option_specifier);
+
+ if (result.empty())
+ {
+ throw invalid_option_format_error(opts);
+ }
+
+ const auto& short_match = result[2];
+ const auto& long_match = result[3];
+
+ if (!short_match.length() && !long_match.length())
+ {
+ throw invalid_option_format_error(opts);
+ }
+ else if (long_match.length() == 1 && short_match.length())
+ {
+ throw invalid_option_format_error(opts);
+ }
+
+ auto option_names = []
+ (
+ const std::sub_match<const char*>& short_,
+ const std::sub_match<const char*>& long_
+ )
+ {
+ if (long_.length() == 1)
+ {
+ return std::make_tuple(long_.str(), short_.str());
+ }
+ else
+ {
+ return std::make_tuple(short_.str(), long_.str());
+ }
+ }
+ (short_match, long_match);
+
+ m_options.add_option
+ (
+ m_group,
+ std::get<0>(option_names),
+ std::get<1>(option_names),
+ desc,
+ value,
+ std::move(arg_help)
+ );
+
+ return *this;
+ }
+
+ inline
+ void
+ ParseResult::parse_default(std::shared_ptr<OptionDetails> details)
+ {
+ m_results[details].parse_default(details);
+ }
+
+ inline
+ void
+ ParseResult::parse_option
+ (
+ std::shared_ptr<OptionDetails> value,
+ const std::string& /*name*/,
+ const std::string& arg
+ )
+ {
+ auto& result = m_results[value];
+ result.parse(value, arg);
+
+ m_sequential.emplace_back(value->long_name(), arg);
+ }
+
+ inline
+ void
+ ParseResult::checked_parse_arg
+ (
+ int argc,
+ char* argv[],
+ int& current,
+ std::shared_ptr<OptionDetails> value,
+ const std::string& name
+ )
+ {
+ if (current + 1 >= argc)
+ {
+ if (value->value().has_implicit())
+ {
+ parse_option(value, name, value->value().get_implicit_value());
+ }
+ else
+ {
+ throw missing_argument_exception(name);
+ }
+ }
+ else
+ {
+ if (value->value().has_implicit())
+ {
+ parse_option(value, name, value->value().get_implicit_value());
+ }
+ else
+ {
+ parse_option(value, name, argv[current + 1]);
+ ++current;
+ }
+ }
+ }
+
+ inline
+ void
+ ParseResult::add_to_option(const std::string& option, const std::string& arg)
+ {
+ auto iter = m_options->find(option);
+
+ if (iter == m_options->end())
+ {
+ throw option_not_exists_exception(option);
+ }
+
+ parse_option(iter->second, option, arg);
+ }
+
+ inline
+ bool
+ ParseResult::consume_positional(std::string a)
+ {
+ while (m_next_positional != m_positional.end())
+ {
+ auto iter = m_options->find(*m_next_positional);
+ if (iter != m_options->end())
+ {
+ auto& result = m_results[iter->second];
+ if (!iter->second->value().is_container())
+ {
+ if (result.count() == 0)
+ {
+ add_to_option(*m_next_positional, a);
+ ++m_next_positional;
+ return true;
+ }
+ else
+ {
+ ++m_next_positional;
+ continue;
+ }
+ }
+ else
+ {
+ add_to_option(*m_next_positional, a);
+ return true;
+ }
+ }
+ ++m_next_positional;
+ }
+
+ return false;
+ }
+
+ inline
+ void
+ Options::parse_positional(std::string option)
+ {
+ parse_positional(std::vector<std::string> {std::move(option)});
+ }
+
+ inline
+ void
+ Options::parse_positional(std::vector<std::string> options)
+ {
+ m_positional = std::move(options);
+ m_next_positional = m_positional.begin();
+
+ m_positional_set.insert(m_positional.begin(), m_positional.end());
+ }
+
+ inline
+ void
+ Options::parse_positional(std::initializer_list<std::string> options)
+ {
+ parse_positional(std::vector<std::string>(std::move(options)));
+ }
+
+ inline
+ ParseResult
+ Options::parse(int& argc, char**& argv)
+ {
+ ParseResult result(m_options, m_positional, m_allow_unrecognised, argc, argv);
+ return result;
+ }
+
+ inline
+ void
+ ParseResult::parse(int& argc, char**& argv)
+ {
+ int current = 1;
+
+ int nextKeep = 1;
+
+ bool consume_remaining = false;
+
+ while (current != argc)
+ {
+ if (strcmp(argv[current], "--") == 0)
+ {
+ consume_remaining = true;
+ ++current;
+ break;
+ }
+
+ std::match_results<const char*> result;
+ std::regex_match(argv[current], result, option_matcher);
+
+ if (result.empty())
+ {
+ //not a flag
+
+ // but if it starts with a `-`, then it's an error
+ if (argv[current][0] == '-' && argv[current][1] != '\0')
+ {
+ throw option_syntax_exception(argv[current]);
+ }
+
+ //if true is returned here then it was consumed, otherwise it is
+ //ignored
+ if (consume_positional(argv[current]))
+ {
+ }
+ else
+ {
+ argv[nextKeep] = argv[current];
+ ++nextKeep;
+ }
+ //if we return from here then it was parsed successfully, so continue
+ }
+ else
+ {
+ //short or long option?
+ if (result[4].length() != 0)
+ {
+ const std::string& s = result[4];
+
+ for (std::size_t i = 0; i != s.size(); ++i)
+ {
+ std::string name(1, s[i]);
+ auto iter = m_options->find(name);
+
+ if (iter == m_options->end())
+ {
+ if (m_allow_unrecognised)
+ {
+ continue;
+ }
+ else
+ {
+ //error
+ throw option_not_exists_exception(name);
+ }
+ }
+
+ auto value = iter->second;
+
+ if (i + 1 == s.size())
+ {
+ //it must be the last argument
+ checked_parse_arg(argc, argv, current, value, name);
+ }
+ else if (value->value().has_implicit())
+ {
+ parse_option(value, name, value->value().get_implicit_value());
+ }
+ else
+ {
+ //error
+ throw option_requires_argument_exception(name);
+ }
+ }
+ }
+ else if (result[1].length() != 0)
+ {
+ const std::string& name = result[1];
+
+ auto iter = m_options->find(name);
+
+ if (iter == m_options->end())
+ {
+ if (m_allow_unrecognised)
+ {
+ // keep unrecognised options in argument list, skip to next argument
+ argv[nextKeep] = argv[current];
+ ++nextKeep;
+ ++current;
+ continue;
+ }
+ else
+ {
+ //error
+ throw option_not_exists_exception(name);
+ }
+ }
+
+ auto opt = iter->second;
+
+ //equals provided for long option?
+ if (result[2].length() != 0)
+ {
+ //parse the option given
+
+ parse_option(opt, name, result[3]);
+ }
+ else
+ {
+ //parse the next argument
+ checked_parse_arg(argc, argv, current, opt, name);
+ }
+ }
+
+ }
+
+ ++current;
+ }
+
+ for (auto& opt : *m_options)
+ {
+ auto& detail = opt.second;
+ auto& value = detail->value();
+
+ auto& store = m_results[detail];
+
+ if (!store.count() && value.has_default())
+ {
+ parse_default(detail);
+ }
+ }
+
+ if (consume_remaining)
+ {
+ while (current < argc)
+ {
+ if (!consume_positional(argv[current]))
+ {
+ break;
+ }
+ ++current;
+ }
+
+ //adjust argv for any that couldn't be swallowed
+ while (current != argc)
+ {
+ argv[nextKeep] = argv[current];
+ ++nextKeep;
+ ++current;
+ }
+ }
+
+ argc = nextKeep;
+
+ }
+
+ inline
+ void
+ Options::add_option
+ (
+ const std::string& group,
+ const std::string& s,
+ const std::string& l,
+ std::string desc,
+ std::shared_ptr<const Value> value,
+ std::string arg_help
+ )
+ {
+ auto stringDesc = toLocalString(std::move(desc));
+ auto option = std::make_shared<OptionDetails>(s, l, stringDesc, value);
+
+ if (s.size() > 0)
+ {
+ add_one_option(s, option);
+ }
+
+ if (l.size() > 0)
+ {
+ add_one_option(l, option);
+ }
+
+ //add the help details
+ auto& options = m_help[group];
+
+ options.options.emplace_back(HelpOptionDetails{s, l, stringDesc,
+ value->has_default(), value->get_default_value(),
+ value->has_implicit(), value->get_implicit_value(),
+ std::move(arg_help),
+ value->is_container(),
+ value->is_boolean()});
+ }
+
+ inline
+ void
+ Options::add_one_option
+ (
+ const std::string& option,
+ std::shared_ptr<OptionDetails> details
+ )
+ {
+ auto in = m_options->emplace(option, details);
+
+ if (!in.second)
+ {
+ throw option_exists_error(option);
+ }
+ }
+
+ inline
+ String
+ Options::help_one_group(const std::string& g) const
+ {
+ typedef std::vector<std::pair<String, String>> OptionHelp;
+
+ auto group = m_help.find(g);
+ if (group == m_help.end())
+ {
+ return "";
+ }
+
+ OptionHelp format;
+
+ size_t longest = 0;
+
+ String result;
+
+ if (!g.empty())
+ {
+ result += toLocalString(" " + g + " options:\n");
+ }
+
+ for (const auto& o : group->second.options)
+ {
+ if (o.is_container &&
+ m_positional_set.find(o.l) != m_positional_set.end() &&
+ !m_show_positional)
+ {
+ continue;
+ }
+
+ auto s = format_option(o);
+ longest = (std::max)(longest, stringLength(s));
+ format.push_back(std::make_pair(s, String()));
+ }
+
+ longest = (std::min)(longest, static_cast<size_t>(OPTION_LONGEST));
+
+ //widest allowed description
+ auto allowed = size_t{76} - longest - OPTION_DESC_GAP;
+
+ auto fiter = format.begin();
+ for (const auto& o : group->second.options)
+ {
+ if (o.is_container &&
+ m_positional_set.find(o.l) != m_positional_set.end() &&
+ !m_show_positional)
+ {
+ continue;
+ }
+
+ auto d = format_description(o, longest + OPTION_DESC_GAP, allowed);
+
+ result += fiter->first;
+ if (stringLength(fiter->first) > longest)
+ {
+ result += '\n';
+ result += toLocalString(std::string(longest + OPTION_DESC_GAP, ' '));
+ }
+ else
+ {
+ result += toLocalString(std::string(longest + OPTION_DESC_GAP -
+ stringLength(fiter->first),
+ ' '));
+ }
+ result += d;
+ result += '\n';
+
+ ++fiter;
+ }
+
+ return result;
+ }
+
+ inline
+ void
+ Options::generate_group_help
+ (
+ String& result,
+ const std::vector<std::string>& print_groups
+ ) const
+ {
+ for (size_t i = 0; i != print_groups.size(); ++i)
+ {
+ const String& group_help_text = help_one_group(print_groups[i]);
+ if (empty(group_help_text))
+ {
+ continue;
+ }
+ result += group_help_text;
+ if (i < print_groups.size() - 1)
+ {
+ result += '\n';
+ }
+ }
+ }
+
+ inline
+ void
+ Options::generate_all_groups_help(String& result) const
+ {
+ std::vector<std::string> all_groups;
+ all_groups.reserve(m_help.size());
+
+ for (auto& group : m_help)
+ {
+ all_groups.push_back(group.first);
+ }
+
+ generate_group_help(result, all_groups);
+ }
+
+ inline
+ std::string
+ Options::help(const std::vector<std::string>& help_groups) const
+ {
+ String result = m_help_string + "\nUsage:\n " +
+ toLocalString(m_program) + " " + toLocalString(m_custom_help);
+
+ if (m_positional.size() > 0 && m_positional_help.size() > 0)
+ {
+ result += " " + toLocalString(m_positional_help);
+ }
+
+ result += "\n\n";
+
+ if (help_groups.size() == 0)
+ {
+ generate_all_groups_help(result);
+ }
+ else
+ {
+ generate_group_help(result, help_groups);
+ }
+
+ return toUTF8String(result);
+ }
+
+ inline
+ const std::vector<std::string>
+ Options::groups() const
+ {
+ std::vector<std::string> g;
+
+ std::transform(
+ m_help.begin(),
+ m_help.end(),
+ std::back_inserter(g),
+ [](const std::map<std::string, HelpGroupDetails>::value_type & pair)
+ {
+ return pair.first;
+ }
+ );
+
+ return g;
+ }
+
+ inline
+ const HelpGroupDetails&
+ Options::group_help(const std::string& group) const
+ {
+ return m_help.at(group);
+ }
+
+}
+
+#endif //CXXOPTS_HPP_INCLUDED
diff --git a/source/RobotAPI/applications/AronArmemGenerator/main.cpp b/source/RobotAPI/applications/AronArmemGenerator/main.cpp
index a42106d717c69867d7441d85c0fb9f12b52df2d3..48ae7dd4dcc75ec8ea5804accbd66bb1c66e5a5b 100644
--- a/source/RobotAPI/applications/AronArmemGenerator/main.cpp
+++ b/source/RobotAPI/applications/AronArmemGenerator/main.cpp
@@ -22,49 +22,165 @@
// STD/STL
#include <iostream>
+#include <filesystem>
+#include <ctime>
// Boost
#include <boost/algorithm/string.hpp>
+// CXXOPTS
+#include <RobotAPI/applications/AronArmemGenerator/cxxopts.hpp>
+
// ArmarX Executable
-#include <ArmarXCore/core/application/Application.h>
-#include <ArmarXCore/core/Component.h>
-#include <ArmarXCore/core/logging/Logging.h>
+//#include <ArmarXCore/core/application/Application.h>
+//#include <ArmarXCore/core/Component.h>
+//#include <ArmarXCore/core/logging/Logging.h>
// ArmarX
#include <ArmarXCore/libraries/cppgen/CppMethod.h>
#include <ArmarXCore/libraries/cppgen/CppClass.h>
#include <RobotAPI/libraries/aron/codegenerator/typeReaders/xmlReader/AronAbstractTypeXMLReader.h>
-#include <RobotAPI/libraries/aron/codegenerator/cppWriters/AronCppWriter.h>
+#include <RobotAPI/libraries/aron/codegenerator/codeWriters/cppWriter/AronCppWriter.h>
using namespace armarx;
using namespace aron;
int main(int argc, char* argv[])
{
- std::string filename = "/home/fabian/Software/ArmarX/RobotAPI/source/RobotAPI/libraries/aron/test/NaturalIK.xml";
- codegeneration::typereader::AronAbstractTypeXMLReader reader;
- reader.parseFile(filename);
- codegeneration::classwriter::AronTypeClassCppWriter writer("AronTestProducer", reader.getWriters(), reader.getReaders());
+ try
+ {
+ cxxopts::Options options("AronArmemCodeGenerator", "An application interface for the aron and armem code generation");
- writer.generateTypeClasses(reader.getProduceTypes());
- std::cout << "TYPES:" << std::endl;
+ options.add_options("General")
+ ("v,verbose", "Enable verbose mode", cxxopts::value<bool>()->default_value("false"))
+ ("h,help", "Print usage");
- for (const auto& c : writer.getTypeClasses())
- {
- CppWriterPtr w = CppWriterPtr(new CppWriter());
- c->writeCpp(w);
- std::cout << w->getString() << std::endl << std::endl;
- }
+ options.add_options("file")
+ ("f,file", "XML file name", cxxopts::value<std::string>()->default_value("/home/fabian/Software/ArmarX/RobotAPI/source/RobotAPI/libraries/aron/test/xmls/NaturalIK.xml"))
+ ("o,output", "The output path", cxxopts::value<std::string>()->default_value("/home/fabian/Software/ArmarX/RobotAPI/source/RobotAPI/libraries/aron/test/generated/"));
+
+ auto result = options.parse(argc, argv);
+
+ if (result.count("help"))
+ {
+ std::cout << options.help() << std::endl;
+ exit(0);
+ }
+
+ bool verbose = result["v"].as<bool>();
+ std::string filename = result["f"].as<std::string>();
+ std::string output = result["o"].as<std::string>();
+
+ codegeneration::typereader::AronAbstractTypeXMLReader reader;
+
+ if (verbose)
+ {
+ std::cout << "Parse the XML file...";
+ }
+
+ std::filesystem::path input_file(filename);
+ if (input_file.extension().string() != ".xml")
+ {
+ std::cerr << "The file you passed has the wrong type." << std::endl;
+ }
+
+ std::string new_name_with_extension = input_file.filename().replace_extension(".h").string();
+
+ reader.parseFile(input_file);
+ if (verbose)
+ {
+ std::cout << " done!" << std::endl;
+ std::cout << "--> Found " << reader.getGenerateTypes().size() << " types." << std::endl;
+ }
+
+ codegeneration::classwriter::AronTypeClassCppWriter writer("AronTestSegment", reader.getWriters(), reader.getReaders(), reader.getUseTypes());
+
+ if (verbose)
+ {
+ std::cout << "Run the type class generator...";
+ }
- //writer.generateConverterMethods(reader.getProduceTypes());
- //std::cout << "CLASS:" << std::endl;
- //std::cout << writer.toString() << std::endl << std::endl;
+ writer.generateTypeClasses(reader.getGenerateTypes());
- //ofstream output;
- //output.open ("/tmp/aronTest.generated.h");
- //output << writer.toString();
- //output.close();*/
+ if (verbose)
+ {
+ std::cout << " done!" << std::endl;
+ std::cout << "--> Found " << writer.getTypeClasses().size() << " type classes." << std::endl;
+ }
+
+ std::ofstream ofs;
+ std::string output_current_file = output + "/" + new_name_with_extension;
+ ofs.open(output_current_file);
+
+ time_t now = time(0);
+ char* dt = ctime(&now);
+
+ ofs << "/* \n\
+ * This file is part of ArmarX. \n\
+ * \n\
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T), \n\
+ * Karlsruhe Institute of Technology (KIT), all rights reserved. \n\
+ * \n\
+ * ArmarX is free software; you can redistribute it and/or modify \n\
+ * it under the terms of the GNU General Public License version 2 as \n\
+ * published by the Free Software Foundation. \n\
+ * \n\
+ * ArmarX is distributed in the hope that it will be useful, but \n\
+ * WITHOUT ANY WARRANTY; without even the implied warranty of \n\
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the \n\
+ * GNU General Public License for more details. \n\
+ * \n\
+ * You should have received a copy of the GNU General Public License \n\
+ * along with this program. If not, see <http://www.gnu.org/licenses/>. \n\
+ * \n\
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt \n\
+ * GNU General Public License \n\
+ ************************************************************************* \n\
+ * WARNING: This file is autogenerated. \n\
+ * Original file: " + filename + " \n\
+ * Timestamp of generation: " + dt + "\
+ * Please do not edit since your changes may be overwritten on the next generation \n\
+ * If you have any questions please contact: Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu) \n\
+ ************************************************************************* \n\
+ */\n\n\n";
+
+ unsigned int i = 0;
+ for (const auto& c : writer.getTypeClasses())
+ {
+ i++;
+ if (verbose)
+ {
+ std::cout << "Exporting class '" << c->getName() << "'...";
+ }
+
+ CppWriterPtr w = CppWriterPtr(new CppWriter());
+ c->writeCpp(w);
+
+ ofs << w->getString();
+
+ if (verbose)
+ {
+ std::cout << " done!" << std::endl;
+ }
+
+ if (i != writer.getTypeClasses().size())
+ {
+ ofs << "\n";
+ ofs << "// *************\n";
+ ofs << "// Here comes another auto-generated class. Don't mind the duplicated includes - everything is guarded!\n";
+ ofs << "// *************\n";
+ ofs << "\n";
+ }
+
+ }
+ ofs.close();
+
+ }
+ catch (const cxxopts::OptionException& e)
+ {
+ std::cerr << "Error in parsing cxxopts options: " << e.what() << std::endl;
+ exit(1);
+ }
return 0;
}
diff --git a/source/RobotAPI/applications/KITHandUnit/CMakeLists.txt b/source/RobotAPI/applications/AronTest/CMakeLists.txt
similarity index 64%
rename from source/RobotAPI/applications/KITHandUnit/CMakeLists.txt
rename to source/RobotAPI/applications/AronTest/CMakeLists.txt
index a7dd589b24ff8824005237c57a281ef5868b889d..c425d85c16ef16e022fcd0a06a19ddbc81a42c60 100644
--- a/source/RobotAPI/applications/KITHandUnit/CMakeLists.txt
+++ b/source/RobotAPI/applications/AronTest/CMakeLists.txt
@@ -1,5 +1,5 @@
-armarx_component_set_name("KITHandUnitApp")
-set(COMPONENT_LIBS KITHandUnit)
+armarx_component_set_name("AronTestApp")
+set(COMPONENT_LIBS AronTest)
armarx_add_component_executable(main.cpp)
#find_package(MyLib QUIET)
@@ -7,5 +7,5 @@ armarx_add_component_executable(main.cpp)
# all target_include_directories must be guarded by if(Xyz_FOUND)
# for multiple libraries write: if(X_FOUND AND Y_FOUND)....
#if(MyLib_FOUND)
-# target_include_directories(KITHandUnit PUBLIC ${MyLib_INCLUDE_DIRS})
+# target_include_directories(AronTest PUBLIC ${MyLib_INCLUDE_DIRS})
#endif()
diff --git a/source/RobotAPI/applications/ArMemGlobalStorage/main.cpp b/source/RobotAPI/applications/AronTest/main.cpp
similarity index 80%
rename from source/RobotAPI/applications/ArMemGlobalStorage/main.cpp
rename to source/RobotAPI/applications/AronTest/main.cpp
index 0bbd86f631cf195d9896394c7e95da7ccd72a068..a38bfa8cf3970438fe8e6554e7114c1f5b17fb8a 100644
--- a/source/RobotAPI/applications/ArMemGlobalStorage/main.cpp
+++ b/source/RobotAPI/applications/AronTest/main.cpp
@@ -13,14 +13,14 @@
* 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::ArMemGlobalStorage
+ * @package RobotAPI::application::AronTest
* @author fabian.peller-konrad@kit.edu ( fabian dot peller-konrad at kit dot edu )
* @date 2020
* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
* GNU General Public License
*/
-#include <RobotAPI/components/ArMemGlobalStorage/ArMemGlobalStorage.h>
+#include <RobotAPI/components/AronTest/AronTest.h>
#include <ArmarXCore/core/application/Application.h>
#include <ArmarXCore/core/Component.h>
@@ -28,5 +28,5 @@
int main(int argc, char* argv[])
{
- return armarx::runSimpleComponentApp < armarx::ArMemGlobalStorage > (argc, argv, "ArMemGlobalStorage");
+ return armarx::runSimpleComponentApp < armarx::AronTest > (argc, argv, "AronTest");
}
diff --git a/source/RobotAPI/applications/CMakeLists.txt b/source/RobotAPI/applications/CMakeLists.txt
index d975f574bcd59af1251b5d109839bba795f79099..68dfd7829691a29eb2a6e70eafc975468909ff2a 100644
--- a/source/RobotAPI/applications/CMakeLists.txt
+++ b/source/RobotAPI/applications/CMakeLists.txt
@@ -1,59 +1,5 @@
-add_subdirectory(ForceTorqueObserver)
-add_subdirectory(HeadIKUnit)
-add_subdirectory(TCPControlUnit)
-
+add_subdirectory(AronArmemGenerator)
add_subdirectory(WeissHapticUnit)
-add_subdirectory(HapticObserver)
-
-
-add_subdirectory(RobotControl)
add_subdirectory(RobotControlUI)
-add_subdirectory(KinematicUnitObserver)
-add_subdirectory(KinematicUnitSimulation)
-add_subdirectory(PlatformUnitSimulation)
-add_subdirectory(PlatformUnitObserver)
add_subdirectory(RobotStateComponent)
-add_subdirectory(HandUnitSimulation)
-add_subdirectory(ForceTorqueUnitSimulation)
-
-
-add_subdirectory(XsensIMU)
-add_subdirectory(InertialMeasurementUnitObserver)
-
-add_subdirectory(ViewSelection)
-
-add_subdirectory(HokuyoLaserUnit)
-add_subdirectory(LaserScannerUnitObserver)
-add_subdirectory(OrientedTactileSensorUnit)
-add_subdirectory(OrientedTactileSensorUnitObserver)
-
-add_subdirectory(OptoForceUnit)
-add_subdirectory(OptoForceUnitObserver)
-
-add_subdirectory(GamepadUnit)
-add_subdirectory(GamepadUnitObserver)
-add_subdirectory(GamepadControlUnit)
-
-add_subdirectory(MetaWearIMU)
-add_subdirectory(MetaWearIMUObserver)
-add_subdirectory(RobotNameService)
-add_subdirectory(SpeechObserver)
-add_subdirectory(DummyTextToSpeech)
-add_subdirectory(KITProstheticHandUnit)
-add_subdirectory(CyberGloveObserver)
-add_subdirectory(RobotHealth)
-add_subdirectory(RobotHealthDummy)
-add_subdirectory(GraspCandidateObserver)
-
-add_subdirectory(FrameTracking)
-
-add_subdirectory(KITHandUnit)
-add_subdirectory(MultiHandUnit)
-add_subdirectory(StatechartExecutorExample)
-
-
-add_subdirectory(NaturalIKTest)
-add_subdirectory(DynamicObstacleManager)
-
-add_subdirectory(ArMemGlobalStorage)
-add_subdirectory(AronArmemGenerator)
+add_subdirectory(TCPControlUnit)
diff --git a/source/RobotAPI/applications/CyberGloveObserver/CMakeLists.txt b/source/RobotAPI/applications/CyberGloveObserver/CMakeLists.txt
deleted file mode 100644
index a639bd1bd8960b9bcd99f866e134c99904ff84dc..0000000000000000000000000000000000000000
--- a/source/RobotAPI/applications/CyberGloveObserver/CMakeLists.txt
+++ /dev/null
@@ -1,3 +0,0 @@
-armarx_component_set_name("CyberGloveObserverApp")
-set(COMPONENT_LIBS CyberGloveObserver)
-armarx_add_component_executable("main.cpp")
diff --git a/source/RobotAPI/applications/CyberGloveObserver/main.cpp b/source/RobotAPI/applications/CyberGloveObserver/main.cpp
deleted file mode 100644
index 7d8b134db7d26732426e1f8303120b2ca688896a..0000000000000000000000000000000000000000
--- a/source/RobotAPI/applications/CyberGloveObserver/main.cpp
+++ /dev/null
@@ -1,32 +0,0 @@
-/*
- * 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::CyberGloveObserver
- * @author JuliaStarke ( julia dot starke at kit dot edu )
- * @date 2018
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#include <RobotAPI/components/CyberGloveObserver/CyberGloveObserver.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::CyberGloveObserver > (argc, argv, "CyberGloveObserver");
-}
diff --git a/source/RobotAPI/applications/DummyTextToSpeech/CMakeLists.txt b/source/RobotAPI/applications/DummyTextToSpeech/CMakeLists.txt
deleted file mode 100644
index 68e57d98b2cda2420d5af15b471f0ed42a8934a5..0000000000000000000000000000000000000000
--- a/source/RobotAPI/applications/DummyTextToSpeech/CMakeLists.txt
+++ /dev/null
@@ -1,3 +0,0 @@
-armarx_component_set_name("DummyTextToSpeechApp")
-set(COMPONENT_LIBS DummyTextToSpeech)
-armarx_add_component_executable("main.cpp")
diff --git a/source/RobotAPI/applications/DummyTextToSpeech/main.cpp b/source/RobotAPI/applications/DummyTextToSpeech/main.cpp
deleted file mode 100644
index 2d852287b667c19549edac56d8e3c2ae2c39d947..0000000000000000000000000000000000000000
--- a/source/RobotAPI/applications/DummyTextToSpeech/main.cpp
+++ /dev/null
@@ -1,32 +0,0 @@
-/*
- * 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::DummyTextToSpeech
- * @author Simon Ottenhaus ( simon dot ottenhaus at kit dot edu )
- * @date 2018
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#include <RobotAPI/components/DummyTextToSpeech/DummyTextToSpeech.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::DummyTextToSpeech > (argc, argv, "DummyTextToSpeech");
-}
diff --git a/source/RobotAPI/applications/DynamicObstacleManager/CMakeLists.txt b/source/RobotAPI/applications/DynamicObstacleManager/CMakeLists.txt
deleted file mode 100644
index e8cd2eda8e4ab5fab7909b9b1a2febf0ccbe4bf4..0000000000000000000000000000000000000000
--- a/source/RobotAPI/applications/DynamicObstacleManager/CMakeLists.txt
+++ /dev/null
@@ -1,7 +0,0 @@
-armarx_component_set_name("DynamicObstacleManager")
-
-set(COMPONENT_LIBS
- DynamicObstacleManager
-)
-
-armarx_add_component_executable("main.cpp")
diff --git a/source/RobotAPI/applications/ForceTorqueObserver/CMakeLists.txt b/source/RobotAPI/applications/ForceTorqueObserver/CMakeLists.txt
deleted file mode 100644
index 35427e462808be20f3b1e0de77293f09b5ad7b75..0000000000000000000000000000000000000000
--- a/source/RobotAPI/applications/ForceTorqueObserver/CMakeLists.txt
+++ /dev/null
@@ -1,6 +0,0 @@
-armarx_component_set_name(ForceTorqueObserver)
-
-set(COMPONENT_LIBS RobotAPIUnits)
-set(EXE_SOURCE main.cpp)
-
-armarx_add_component_executable("${EXE_SOURCE}")
diff --git a/source/RobotAPI/applications/ForceTorqueObserver/main.cpp b/source/RobotAPI/applications/ForceTorqueObserver/main.cpp
deleted file mode 100644
index 5bde42c5f96cebcaaedb5b307a47148a0d640b17..0000000000000000000000000000000000000000
--- a/source/RobotAPI/applications/ForceTorqueObserver/main.cpp
+++ /dev/null
@@ -1,34 +0,0 @@
-/*
- * 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::application::ForceTorqueObserver
- * @author Manfred Kroehnert
- * @date 2014
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#include <RobotAPI/components/units/ForceTorqueObserver.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::ForceTorqueObserver>(argc, argv, "ForceTorqueObserver");
-}
diff --git a/source/RobotAPI/applications/ForceTorqueUnitSimulation/CMakeLists.txt b/source/RobotAPI/applications/ForceTorqueUnitSimulation/CMakeLists.txt
deleted file mode 100644
index 581a8ae8551c0c29d9aa7f0e8876891039414545..0000000000000000000000000000000000000000
--- a/source/RobotAPI/applications/ForceTorqueUnitSimulation/CMakeLists.txt
+++ /dev/null
@@ -1,3 +0,0 @@
-armarx_component_set_name(ForceTorqueUnitSimulation)
-set(COMPONENT_LIBS RobotAPIUnits)
-armarx_add_component_executable("main.cpp")
diff --git a/source/RobotAPI/applications/ForceTorqueUnitSimulation/main.cpp b/source/RobotAPI/applications/ForceTorqueUnitSimulation/main.cpp
deleted file mode 100644
index 26a126b4be868544684291042b31c0d31ee3a4d7..0000000000000000000000000000000000000000
--- a/source/RobotAPI/applications/ForceTorqueUnitSimulation/main.cpp
+++ /dev/null
@@ -1,34 +0,0 @@
-/*
- * 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::application::ForceTorqueUnitSimulation
- * @author Peter Kaiser
- * @date 2014
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#include <RobotAPI/components/units/ForceTorqueUnitSimulation.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::ForceTorqueUnitSimulation>(argc, argv, "ForceTorqueUnitSimulation");
-}
diff --git a/source/RobotAPI/applications/FrameTracking/CMakeLists.txt b/source/RobotAPI/applications/FrameTracking/CMakeLists.txt
deleted file mode 100644
index 69311ee8d5fb8e412d615b156eb93e9a1de31dd2..0000000000000000000000000000000000000000
--- a/source/RobotAPI/applications/FrameTracking/CMakeLists.txt
+++ /dev/null
@@ -1,11 +0,0 @@
-armarx_component_set_name("FrameTrackingApp")
-set(COMPONENT_LIBS FrameTracking)
-armarx_add_component_executable(main.cpp)
-
-#find_package(MyLib QUIET)
-#armarx_build_if(MyLib_FOUND "MyLib not available")
-# all target_include_directories must be guarded by if(Xyz_FOUND)
-# for multiple libraries write: if(X_FOUND AND Y_FOUND)....
-#if(MyLib_FOUND)
-# target_include_directories(FrameTracking PUBLIC ${MyLib_INCLUDE_DIRS})
-#endif()
diff --git a/source/RobotAPI/applications/FrameTracking/main.cpp b/source/RobotAPI/applications/FrameTracking/main.cpp
deleted file mode 100644
index d8a7c016d8a9ea8ba94638e1feaec9f55eb16e6f..0000000000000000000000000000000000000000
--- a/source/RobotAPI/applications/FrameTracking/main.cpp
+++ /dev/null
@@ -1,32 +0,0 @@
-/*
- * 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::FrameTracking
- * @author Adrian Knobloch ( adrian dot knobloch at student dot kit dot edu )
- * @date 2019
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#include <RobotAPI/components/FrameTracking/FrameTracking.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::FrameTracking > (argc, argv, "FrameTracking");
-}
diff --git a/source/RobotAPI/applications/GamepadControlUnit/CMakeLists.txt b/source/RobotAPI/applications/GamepadControlUnit/CMakeLists.txt
deleted file mode 100644
index 8ecc77b4d025f4d539b52a9fac8ca5fb95745de9..0000000000000000000000000000000000000000
--- a/source/RobotAPI/applications/GamepadControlUnit/CMakeLists.txt
+++ /dev/null
@@ -1,11 +0,0 @@
-armarx_component_set_name("GamepadControlUnitApp")
-
-set(COMPONENT_LIBS
- ArmarXCoreInterfaces
- ArmarXCore
- GamepadControlUnit
-)
-
-set(EXE_SOURCE main.cpp)
-
-armarx_add_component_executable("${EXE_SOURCE}")
diff --git a/source/RobotAPI/applications/GamepadUnit/CMakeLists.txt b/source/RobotAPI/applications/GamepadUnit/CMakeLists.txt
deleted file mode 100644
index cfc83f3b16a9d381ba4348ec2825508fc3355033..0000000000000000000000000000000000000000
--- a/source/RobotAPI/applications/GamepadUnit/CMakeLists.txt
+++ /dev/null
@@ -1,11 +0,0 @@
-armarx_component_set_name("GamepadUnitApp")
-
-set(COMPONENT_LIBS
- ArmarXCoreInterfaces
- ArmarXCore
- GamepadUnit
-)
-
-set(EXE_SOURCE main.cpp)
-
-armarx_add_component_executable("${EXE_SOURCE}")
diff --git a/source/RobotAPI/applications/GamepadUnit/main.cpp b/source/RobotAPI/applications/GamepadUnit/main.cpp
deleted file mode 100644
index ec81535eb54772ade3b176f8d0842aa7dabc3b14..0000000000000000000000000000000000000000
--- a/source/RobotAPI/applications/GamepadUnit/main.cpp
+++ /dev/null
@@ -1,32 +0,0 @@
-/*
- * 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::GamepadUnit
- * @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/drivers/GamepadUnit/GamepadUnit.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::GamepadUnit > (argc, argv, "GamepadUnit");
-}
diff --git a/source/RobotAPI/applications/GamepadUnitObserver/CMakeLists.txt b/source/RobotAPI/applications/GamepadUnitObserver/CMakeLists.txt
deleted file mode 100644
index 1fb1582a0ed2be60dfcec7e289e6ab6c3a7076c6..0000000000000000000000000000000000000000
--- a/source/RobotAPI/applications/GamepadUnitObserver/CMakeLists.txt
+++ /dev/null
@@ -1,11 +0,0 @@
-armarx_component_set_name("GamepadUnitObserverApp")
-
-set(COMPONENT_LIBS
- ArmarXCoreInterfaces
- ArmarXCore
- RobotAPIUnits
-)
-
-set(EXE_SOURCE main.cpp)
-
-armarx_add_component_executable("${EXE_SOURCE}")
diff --git a/source/RobotAPI/applications/GamepadUnitObserver/main.cpp b/source/RobotAPI/applications/GamepadUnitObserver/main.cpp
deleted file mode 100644
index 3cf8db15df21ea270e29f6c09f5a55358cc0b876..0000000000000000000000000000000000000000
--- a/source/RobotAPI/applications/GamepadUnitObserver/main.cpp
+++ /dev/null
@@ -1,32 +0,0 @@
-/*
- * 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::GamepadUnitObserver
- * @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/GamepadUnitObserver.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::GamepadUnitObserver > (argc, argv, "GamepadUnitObserver");
-}
diff --git a/source/RobotAPI/applications/GraspCandidateObserver/CMakeLists.txt b/source/RobotAPI/applications/GraspCandidateObserver/CMakeLists.txt
deleted file mode 100644
index 9e4ac83c0d1ccfbd7251fc44ebdc25a4d7c13a62..0000000000000000000000000000000000000000
--- a/source/RobotAPI/applications/GraspCandidateObserver/CMakeLists.txt
+++ /dev/null
@@ -1,25 +0,0 @@
-armarx_component_set_name("GraspCandidateObserverApp")
-
-
-find_package(Eigen3 QUIET)
-armarx_build_if(Eigen3_FOUND "Eigen3 not available")
-
-if (Eigen3_FOUND)
- include_directories(SYSTEM ${Eigen3_INCLUDE_DIR})
-endif()
-
-find_package(Simox ${ArmarX_Simox_VERSION} QUIET)
-armarx_build_if(Simox_FOUND "Simox-VirtualRobot not available")
-if(Simox_FOUND)
- include_directories(${Simox_INCLUDE_DIRS})
-endif()
-
-set(COMPONENT_LIBS
- RobotAPIUnits
- ArmarXCoreInterfaces
- ArmarXCore
-)
-
-set(EXE_SOURCE main.cpp)
-
-armarx_add_component_executable("${EXE_SOURCE}")
diff --git a/source/RobotAPI/applications/GraspCandidateObserver/main.cpp b/source/RobotAPI/applications/GraspCandidateObserver/main.cpp
deleted file mode 100644
index 9de7e0e219db9e8d4a7fb289c9d09902fc8fabe6..0000000000000000000000000000000000000000
--- a/source/RobotAPI/applications/GraspCandidateObserver/main.cpp
+++ /dev/null
@@ -1,32 +0,0 @@
-/*
- * 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::GraspCandidateObserver
- * @author Simon Ottenhaus ( simon dot ottenhaus at kit dot edu )
- * @date 2019
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#include <RobotAPI/components/units/GraspCandidateObserver.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::GraspCandidateObserver > (argc, argv, "GraspCandidateObserver");
-}
diff --git a/source/RobotAPI/applications/HandUnitSimulation/CMakeLists.txt b/source/RobotAPI/applications/HandUnitSimulation/CMakeLists.txt
deleted file mode 100644
index 78cd12fe15ddd8cdcbb47640558cd1cb8c409111..0000000000000000000000000000000000000000
--- a/source/RobotAPI/applications/HandUnitSimulation/CMakeLists.txt
+++ /dev/null
@@ -1,3 +0,0 @@
-armarx_component_set_name("HandUnitSimulation")
-set(COMPONENT_LIBS RobotAPIUnits)
-armarx_add_component_executable("main.cpp")
diff --git a/source/RobotAPI/applications/HandUnitSimulation/main.cpp b/source/RobotAPI/applications/HandUnitSimulation/main.cpp
deleted file mode 100644
index 32bd1b8d3894b856d4cfb3218805f8755d34f606..0000000000000000000000000000000000000000
--- a/source/RobotAPI/applications/HandUnitSimulation/main.cpp
+++ /dev/null
@@ -1,34 +0,0 @@
-/*
- * 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::application::HandUnitSimulation
- * @author David Schiebener ( schiebener at kit dot edu )
- * @date 2014
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#include <RobotAPI/components/units/HandUnitSimulation.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::HandUnitSimulation>(argc, argv, "HandUnitSimulation");
-}
diff --git a/source/RobotAPI/applications/HapticObserver/CMakeLists.txt b/source/RobotAPI/applications/HapticObserver/CMakeLists.txt
deleted file mode 100644
index 7b9bff253f31663346851c504c19d2bde73d32bf..0000000000000000000000000000000000000000
--- a/source/RobotAPI/applications/HapticObserver/CMakeLists.txt
+++ /dev/null
@@ -1,3 +0,0 @@
-armarx_component_set_name("HapticObserverApp")
-set(COMPONENT_LIBS RobotAPIUnits)
-armarx_add_component_executable("main.cpp")
diff --git a/source/RobotAPI/applications/HapticObserver/main.cpp b/source/RobotAPI/applications/HapticObserver/main.cpp
deleted file mode 100644
index c521a8ac8c46219d913e8f427c4e0b30f8ff255c..0000000000000000000000000000000000000000
--- a/source/RobotAPI/applications/HapticObserver/main.cpp
+++ /dev/null
@@ -1,34 +0,0 @@
-/*
- * 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::application::HapticObserver
- * @author Simon Ottenhaus ( simon dot ottenhaus at kit dot edu )
- * @date 2014
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#include <RobotAPI/components/units/HapticObserver.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::HapticObserver>(argc, argv, "HapticObserver");
-}
diff --git a/source/RobotAPI/applications/HeadIKUnit/CMakeLists.txt b/source/RobotAPI/applications/HeadIKUnit/CMakeLists.txt
deleted file mode 100644
index 11d3ea4c233123e7f79fb3d10271b11ce00480a4..0000000000000000000000000000000000000000
--- a/source/RobotAPI/applications/HeadIKUnit/CMakeLists.txt
+++ /dev/null
@@ -1,3 +0,0 @@
-armarx_component_set_name(HeadIKUnit)
-set(COMPONENT_LIBS RobotAPIUnits)
-armarx_add_component_executable("main.cpp")
diff --git a/source/RobotAPI/applications/HeadIKUnit/main.cpp b/source/RobotAPI/applications/HeadIKUnit/main.cpp
deleted file mode 100644
index 83eadc25c567c53051f6c75e5c46cbe98fb70b5e..0000000000000000000000000000000000000000
--- a/source/RobotAPI/applications/HeadIKUnit/main.cpp
+++ /dev/null
@@ -1,34 +0,0 @@
-/*
- * 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::application::HeadIKUnit
- * @author David Schiebener
- * @date 2014
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#include <RobotAPI/components/units/HeadIKUnit.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::HeadIKUnit>(argc, argv, "HeadIKUnit");
-}
diff --git a/source/RobotAPI/applications/HokuyoLaserUnit/CMakeLists.txt b/source/RobotAPI/applications/HokuyoLaserUnit/CMakeLists.txt
deleted file mode 100644
index 9c0a35a345cebaf39288fb22dfdfe2fbb24ac993..0000000000000000000000000000000000000000
--- a/source/RobotAPI/applications/HokuyoLaserUnit/CMakeLists.txt
+++ /dev/null
@@ -1,13 +0,0 @@
-armarx_component_set_name("HokuyoLaserUnitApp")
-
-find_package(HokuyoLaserScannerDriver QUIET)
-
-set(COMPONENT_LIBS
- ArmarXCoreInterfaces
- ArmarXCore
- HokuyoLaserUnit
-)
-
-set(EXE_SOURCE main.cpp)
-
-armarx_add_component_executable("${EXE_SOURCE}")
diff --git a/source/RobotAPI/applications/HokuyoLaserUnit/main.cpp b/source/RobotAPI/applications/HokuyoLaserUnit/main.cpp
deleted file mode 100644
index 111a542188d5d46aa6f5664aa5476d9c7dc6f88e..0000000000000000000000000000000000000000
--- a/source/RobotAPI/applications/HokuyoLaserUnit/main.cpp
+++ /dev/null
@@ -1,32 +0,0 @@
-/*
- * 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::HokyouLaserUnit
- * @author Fabian Paus ( fabian dot paus at kit dot edu )
- * @date 2017
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#include <RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.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::HokuyoLaserUnit > (argc, argv, "HokuyoLaserUnit");
-}
diff --git a/source/RobotAPI/applications/InertialMeasurementUnitObserver/CMakeLists.txt b/source/RobotAPI/applications/InertialMeasurementUnitObserver/CMakeLists.txt
deleted file mode 100644
index 1c7d0f4341b07ac7d6728c19307f41bb1d5dbe88..0000000000000000000000000000000000000000
--- a/source/RobotAPI/applications/InertialMeasurementUnitObserver/CMakeLists.txt
+++ /dev/null
@@ -1,7 +0,0 @@
-armarx_component_set_name("InertialMeasurementUnitObserverApp")
-
-set(COMPONENT_LIBS ArmarXCoreInterfaces ArmarXCore ArmarXCoreObservers RobotAPIUnits)
-
-set(EXE_SOURCE main.cpp)
-
-armarx_add_component_executable("${EXE_SOURCE}")
diff --git a/source/RobotAPI/applications/InertialMeasurementUnitObserver/main.cpp b/source/RobotAPI/applications/InertialMeasurementUnitObserver/main.cpp
deleted file mode 100644
index 593baa1d190a352b0fe2991609304b25555833ba..0000000000000000000000000000000000000000
--- a/source/RobotAPI/applications/InertialMeasurementUnitObserver/main.cpp
+++ /dev/null
@@ -1,34 +0,0 @@
-/*
- * 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::application::InertialMeasurementUnitObserver
- * @author Markus Grotz ( markus-grotz at web dot de )
- * @date 2015
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#include <RobotAPI/components/units/InertialMeasurementUnitObserver.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::InertialMeasurementUnitObserver>(argc, argv, "InertialMeasurementUnitObserver");
-}
diff --git a/source/RobotAPI/applications/KITHandUnit/main.cpp b/source/RobotAPI/applications/KITHandUnit/main.cpp
deleted file mode 100644
index e9dcea70a7cc5d94cbc8e19649306734ae411252..0000000000000000000000000000000000000000
--- a/source/RobotAPI/applications/KITHandUnit/main.cpp
+++ /dev/null
@@ -1,32 +0,0 @@
-/*
- * 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::KITHandUnit
- * @author Stefan Reither ( stefan dot reither at kit dot edu )
- * @date 2019
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#include <RobotAPI/components/KITHandUnit/KITHandUnit.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::KITHandUnit > (argc, argv, "KITHandUnit");
-}
diff --git a/source/RobotAPI/applications/KITProstheticHandObserver/CMakeLists.txt b/source/RobotAPI/applications/KITProstheticHandObserver/CMakeLists.txt
deleted file mode 100644
index ea8df22f5cbce4de23340bd3d3a935c4cc977dd9..0000000000000000000000000000000000000000
--- a/source/RobotAPI/applications/KITProstheticHandObserver/CMakeLists.txt
+++ /dev/null
@@ -1,3 +0,0 @@
-armarx_component_set_name("KITProstheticHandObserverApp")
-set(COMPONENT_LIBS KITProstheticHandObserver)
-armarx_add_component_executable("main.cpp")
diff --git a/source/RobotAPI/applications/KITProstheticHandObserver/main.cpp b/source/RobotAPI/applications/KITProstheticHandObserver/main.cpp
deleted file mode 100644
index 7542d5e2884d7f9bda8103d335c3e61770b27d95..0000000000000000000000000000000000000000
--- a/source/RobotAPI/applications/KITProstheticHandObserver/main.cpp
+++ /dev/null
@@ -1,32 +0,0 @@
-/*
- * 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::KITProstheticHandObserver
- * @author Raphael Grimm ( raphael dot grimm at kit dot edu )
- * @date 2018
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#include <RobotAPI/components/KITProstheticHandObserver/KITProstheticHandObserver.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::KITProstheticHandObserver > (argc, argv, "KITProstheticHandObserver");
-}
diff --git a/source/RobotAPI/applications/KITProstheticHandUnit/CMakeLists.txt b/source/RobotAPI/applications/KITProstheticHandUnit/CMakeLists.txt
deleted file mode 100644
index 695303d1d68fcd4cc1b32a0b46c8f3cfdfb1539d..0000000000000000000000000000000000000000
--- a/source/RobotAPI/applications/KITProstheticHandUnit/CMakeLists.txt
+++ /dev/null
@@ -1,3 +0,0 @@
-armarx_component_set_name("KITProstheticHandUnitApp")
-set(COMPONENT_LIBS KITProstheticHandUnit)
-armarx_add_component_executable("main.cpp")
diff --git a/source/RobotAPI/applications/KITProstheticHandUnit/main.cpp b/source/RobotAPI/applications/KITProstheticHandUnit/main.cpp
deleted file mode 100644
index b464a35118acf0a1e26da8b5840f6f5b77f96c66..0000000000000000000000000000000000000000
--- a/source/RobotAPI/applications/KITProstheticHandUnit/main.cpp
+++ /dev/null
@@ -1,35 +0,0 @@
-/*
- * 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::KITProstheticHandUnit
- * @author Raphael Grimm ( raphael dot grimm at kit dot edu )
- * @date 2018
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#include <RobotAPI/components/KITProstheticHandUnit/KITProstheticHandUnit.h>
-
-#include <ArmarXCore/core/application/Application.h>
-#include <ArmarXCore/core/Component.h>
-#include <ArmarXCore/core/logging/Logging.h>
-
-#include <QCoreApplication>
-
-int main(int argc, char* argv[])
-{
- QCoreApplication a(argc, argv);
- return armarx::runSimpleComponentApp < armarx::KITProstheticHandUnit > (argc, argv, "KITProstheticHandUnit");
-}
diff --git a/source/RobotAPI/applications/KinematicUnitObserver/CMakeLists.txt b/source/RobotAPI/applications/KinematicUnitObserver/CMakeLists.txt
deleted file mode 100644
index 940574be837e22e1d7742a1082a3da03660c0b74..0000000000000000000000000000000000000000
--- a/source/RobotAPI/applications/KinematicUnitObserver/CMakeLists.txt
+++ /dev/null
@@ -1,9 +0,0 @@
-
-armarx_component_set_name(KinematicUnitObserver)
-
-
-set(COMPONENT_LIBS RobotAPIUnits)
-
-set(SOURCES main.cpp)
-
-armarx_add_component_executable("${SOURCES}")
diff --git a/source/RobotAPI/applications/KinematicUnitObserver/config/testProperty b/source/RobotAPI/applications/KinematicUnitObserver/config/testProperty
deleted file mode 100644
index fd3d73770dab1e8e52b2daef51b188df7b83d1da..0000000000000000000000000000000000000000
--- a/source/RobotAPI/applications/KinematicUnitObserver/config/testProperty
+++ /dev/null
@@ -1,7 +0,0 @@
-# test config file for KinematicUnit configured to be a Head
-
-ArmarX.KinematicUnitObserver.RobotFileName=/org/share/home/staff/welke/projects/armarx/data/ArmarIII/ArmarIII-Head.xml # (absolut path required) model XML file path containing a VirtualRobot RobotNodeSet that defines the joints
-ArmarX.KinematicUnitObserver.RobotNodeSetName=Head # node set name
-ArmarX.KinematicUnitObserver.AdapterName=KinematicUnitObserver # name of the ICE adapter
-
-
diff --git a/source/RobotAPI/applications/KinematicUnitObserver/main.cpp b/source/RobotAPI/applications/KinematicUnitObserver/main.cpp
deleted file mode 100644
index 213d65125408c6ae4d7fb90083aef4958b848279..0000000000000000000000000000000000000000
--- a/source/RobotAPI/applications/KinematicUnitObserver/main.cpp
+++ /dev/null
@@ -1,34 +0,0 @@
-/*
- * 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 Core::application::KinematicUnitObserver
- * @author Manfred Kroehnert
- * @date 2014
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#include <RobotAPI/components/units/KinematicUnitObserver.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::KinematicUnitObserver>(argc, argv, "KinematicUnitObserver");
-}
diff --git a/source/RobotAPI/applications/KinematicUnitSimulation/CMakeLists.txt b/source/RobotAPI/applications/KinematicUnitSimulation/CMakeLists.txt
deleted file mode 100644
index da3097f2bd0f6af5d07f2cb6a4a76d380aafa044..0000000000000000000000000000000000000000
--- a/source/RobotAPI/applications/KinematicUnitSimulation/CMakeLists.txt
+++ /dev/null
@@ -1,3 +0,0 @@
-armarx_component_set_name(KinematicUnitSimulation)
-set(COMPONENT_LIBS RobotAPIUnits)
-armarx_add_component_executable("main.cpp")
diff --git a/source/RobotAPI/applications/KinematicUnitSimulation/config/testProperty b/source/RobotAPI/applications/KinematicUnitSimulation/config/testProperty
deleted file mode 100644
index f5a03878511ff258740126bb8b4b916e603cd261..0000000000000000000000000000000000000000
--- a/source/RobotAPI/applications/KinematicUnitSimulation/config/testProperty
+++ /dev/null
@@ -1,5 +0,0 @@
-# test config file for KinematicUnit configured to be a Head
-
-ArmarX.KinematicUnitSimulation.RobotFileName=${ArmarXHome_DIR}/Armar3/data/robotmodel/ArmarIII-Head.xml
-ArmarX.KinematicUnitSimulation.RobotNodeSetName=Head # node set name
-ArmarX.KinematicUnitSimulation.AdapterName=HeadKinematicUnitInstructionChannel # name of the Ice adapter
diff --git a/source/RobotAPI/applications/KinematicUnitSimulation/main.cpp b/source/RobotAPI/applications/KinematicUnitSimulation/main.cpp
deleted file mode 100644
index 766a0c8a0d9c2628516317252259321f14d6e42d..0000000000000000000000000000000000000000
--- a/source/RobotAPI/applications/KinematicUnitSimulation/main.cpp
+++ /dev/null
@@ -1,34 +0,0 @@
-/*
- * 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 Core::application::KinematicUnitSimulation
- * @author Manfred Kroehnert
- * @date 2014
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#include <RobotAPI/components/units/KinematicUnitSimulation.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::KinematicUnitSimulation>(argc, argv, "KinematicUnitSimulation");
-}
diff --git a/source/RobotAPI/applications/LaserScannerUnitObserver/CMakeLists.txt b/source/RobotAPI/applications/LaserScannerUnitObserver/CMakeLists.txt
deleted file mode 100644
index 2e78fb522ca8c291486bd71aa610e92f570e49da..0000000000000000000000000000000000000000
--- a/source/RobotAPI/applications/LaserScannerUnitObserver/CMakeLists.txt
+++ /dev/null
@@ -1,11 +0,0 @@
-armarx_component_set_name("LaserScannerUnitObserverApp")
-
-set(COMPONENT_LIBS
- ArmarXCoreInterfaces
- ArmarXCore
- RobotAPIUnits
-)
-
-set(EXE_SOURCE main.cpp)
-
-armarx_add_component_executable("${EXE_SOURCE}")
diff --git a/source/RobotAPI/applications/LaserScannerUnitObserver/main.cpp b/source/RobotAPI/applications/LaserScannerUnitObserver/main.cpp
deleted file mode 100644
index ecae3d6b3b6a05d96acf8d1e3b661d2ed47e5f58..0000000000000000000000000000000000000000
--- a/source/RobotAPI/applications/LaserScannerUnitObserver/main.cpp
+++ /dev/null
@@ -1,32 +0,0 @@
-/*
- * 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::LaserScannerUnitObserver
- * @author Fabian Paus ( fabian dot paus at kit dot edu )
- * @date 2017
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#include <RobotAPI/components/units/LaserScannerUnitObserver.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::LaserScannerUnitObserver > (argc, argv, "LaserScannerUnitObserver");
-}
diff --git a/source/RobotAPI/applications/MetaWearIMU/CMakeLists.txt b/source/RobotAPI/applications/MetaWearIMU/CMakeLists.txt
deleted file mode 100644
index acf61ea40d6f898ca6beb9c49e4f5def0f1f951a..0000000000000000000000000000000000000000
--- a/source/RobotAPI/applications/MetaWearIMU/CMakeLists.txt
+++ /dev/null
@@ -1,11 +0,0 @@
-armarx_component_set_name("MetaWearIMUApp")
-
-set(COMPONENT_LIBS
- ArmarXCoreInterfaces
- ArmarXCore
- MetaWearIMU
-)
-
-set(EXE_SOURCE main.cpp)
-
-armarx_add_component_executable("${EXE_SOURCE}")
diff --git a/source/RobotAPI/applications/MetaWearIMU/main.cpp b/source/RobotAPI/applications/MetaWearIMU/main.cpp
deleted file mode 100644
index 1f86708a7f1c12cd4f4dd198b6fe40e4664314ff..0000000000000000000000000000000000000000
--- a/source/RobotAPI/applications/MetaWearIMU/main.cpp
+++ /dev/null
@@ -1,32 +0,0 @@
-/*
- * 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::MetaWearIMU
- * @author Lukas Kaul ( lukas dot s dot kaul at gmail dot com )
- * @date 2017
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#include <RobotAPI/drivers/MetaWearIMU/MetaWearIMU.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::MetaWearIMU > (argc, argv, "MetaWearIMU");
-}
diff --git a/source/RobotAPI/applications/MetaWearIMUObserver/CMakeLists.txt b/source/RobotAPI/applications/MetaWearIMUObserver/CMakeLists.txt
deleted file mode 100644
index eb94631d6e3da594b6d0deba7195031d850eee8b..0000000000000000000000000000000000000000
--- a/source/RobotAPI/applications/MetaWearIMUObserver/CMakeLists.txt
+++ /dev/null
@@ -1,11 +0,0 @@
-armarx_component_set_name("MetaWearIMUObserverApp")
-
-set(COMPONENT_LIBS
- ArmarXCoreInterfaces
- ArmarXCore
- RobotAPIUnits
-)
-
-set(EXE_SOURCE main.cpp)
-
-armarx_add_component_executable("${EXE_SOURCE}")
diff --git a/source/RobotAPI/applications/MetaWearIMUObserver/main.cpp b/source/RobotAPI/applications/MetaWearIMUObserver/main.cpp
deleted file mode 100644
index 376425b06be3750b2d16b74a9e156a7753bf4172..0000000000000000000000000000000000000000
--- a/source/RobotAPI/applications/MetaWearIMUObserver/main.cpp
+++ /dev/null
@@ -1,32 +0,0 @@
-/*
- * 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::MetaWearIMUObserver
- * @author Lukas Kaul ( lukas dot s dot kaul at gmail dot com )
- * @date 2017
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#include <RobotAPI/components/units/MetaWearIMUObserver.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::MetaWearIMUObserver > (argc, argv, "MetaWearIMUObserver");
-}
diff --git a/source/RobotAPI/applications/MultiHandUnit/CMakeLists.txt b/source/RobotAPI/applications/MultiHandUnit/CMakeLists.txt
deleted file mode 100644
index 83ad8ec8e46c15ef37af9d9cd991144dddbec1b0..0000000000000000000000000000000000000000
--- a/source/RobotAPI/applications/MultiHandUnit/CMakeLists.txt
+++ /dev/null
@@ -1,11 +0,0 @@
-armarx_component_set_name("MultiHandUnitApp")
-set(COMPONENT_LIBS MultiHandUnit)
-armarx_add_component_executable(main.cpp)
-
-#find_package(MyLib QUIET)
-#armarx_build_if(MyLib_FOUND "MyLib not available")
-# all target_include_directories must be guarded by if(Xyz_FOUND)
-# for multiple libraries write: if(X_FOUND AND Y_FOUND)....
-#if(MyLib_FOUND)
-# target_include_directories(MultiHandUnit PUBLIC ${MyLib_INCLUDE_DIRS})
-#endif()
diff --git a/source/RobotAPI/applications/NaturalIKTest/CMakeLists.txt b/source/RobotAPI/applications/NaturalIKTest/CMakeLists.txt
deleted file mode 100644
index 69f1eb98516cb5ebd6951ac4082e741b976527dd..0000000000000000000000000000000000000000
--- a/source/RobotAPI/applications/NaturalIKTest/CMakeLists.txt
+++ /dev/null
@@ -1,11 +0,0 @@
-armarx_component_set_name("NaturalIKTestApp")
-set(COMPONENT_LIBS NaturalIKTest)
-armarx_add_component_executable(main.cpp)
-
-#find_package(MyLib QUIET)
-#armarx_build_if(MyLib_FOUND "MyLib not available")
-# all target_include_directories must be guarded by if(Xyz_FOUND)
-# for multiple libraries write: if(X_FOUND AND Y_FOUND)....
-#if(MyLib_FOUND)
-# target_include_directories(NaturalIKTest PUBLIC ${MyLib_INCLUDE_DIRS})
-#endif()
diff --git a/source/RobotAPI/applications/OptoForceUnit/CMakeLists.txt b/source/RobotAPI/applications/OptoForceUnit/CMakeLists.txt
deleted file mode 100644
index 314d412e20c3c05830d3cd1c496bf08d9c6753ee..0000000000000000000000000000000000000000
--- a/source/RobotAPI/applications/OptoForceUnit/CMakeLists.txt
+++ /dev/null
@@ -1,3 +0,0 @@
-armarx_component_set_name("OptoForceUnitApp")
-set(COMPONENT_LIBS OptoForceUnit)
-armarx_add_component_executable("main.cpp")
diff --git a/source/RobotAPI/applications/OptoForceUnit/main.cpp b/source/RobotAPI/applications/OptoForceUnit/main.cpp
deleted file mode 100644
index 4c41d5d633660d88e8087cbf7b67320b23c7c642..0000000000000000000000000000000000000000
--- a/source/RobotAPI/applications/OptoForceUnit/main.cpp
+++ /dev/null
@@ -1,32 +0,0 @@
-/*
- * 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::OptoForceUnit
- * @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/drivers/OptoForceUnit/OptoForceUnit.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::OptoForceUnit > (argc, argv, "OptoForceUnit");
-}
diff --git a/source/RobotAPI/applications/OptoForceUnitObserver/CMakeLists.txt b/source/RobotAPI/applications/OptoForceUnitObserver/CMakeLists.txt
deleted file mode 100644
index e9888678a5761884e8ae45d5e5f7bbf172971e7d..0000000000000000000000000000000000000000
--- a/source/RobotAPI/applications/OptoForceUnitObserver/CMakeLists.txt
+++ /dev/null
@@ -1,11 +0,0 @@
-armarx_component_set_name("OptoForceUnitObserverApp")
-
-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
deleted file mode 100644
index 5701513b4255edbcbd3e1646b72f9bceaa7078ea..0000000000000000000000000000000000000000
--- a/source/RobotAPI/applications/OptoForceUnitObserver/main.cpp
+++ /dev/null
@@ -1,32 +0,0 @@
-/*
- * 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/applications/OrientedTactileSensorUnit/CMakeLists.txt b/source/RobotAPI/applications/OrientedTactileSensorUnit/CMakeLists.txt
deleted file mode 100644
index d55194dc8867083cd757c4e1f0047bae582446f8..0000000000000000000000000000000000000000
--- a/source/RobotAPI/applications/OrientedTactileSensorUnit/CMakeLists.txt
+++ /dev/null
@@ -1,3 +0,0 @@
-armarx_component_set_name("OrientedTactileSensorUnitApp")
-set(COMPONENT_LIBS OrientedTactileSensor)
-armarx_add_component_executable("main.cpp")
diff --git a/source/RobotAPI/applications/OrientedTactileSensorUnit/main.cpp b/source/RobotAPI/applications/OrientedTactileSensorUnit/main.cpp
deleted file mode 100644
index f4ab124f78163892921a4330c9680f618be3aef0..0000000000000000000000000000000000000000
--- a/source/RobotAPI/applications/OrientedTactileSensorUnit/main.cpp
+++ /dev/null
@@ -1,9 +0,0 @@
-#include <ArmarXCore/core/application/Application.h>
-#include <RobotAPI/drivers/OrientedTactileSensor/OrientedTactileSensorUnit.h>
-#include <ArmarXCore/core/logging/Logging.h>
-#include <ArmarXCore/core/Component.h>
-
-int main(int argc, char* argv[])
-{
- return armarx::runSimpleComponentApp < armarx::OrientedTactileSensorUnit > (argc, argv, "OrientedTactileSensorUnit");
-}
diff --git a/source/RobotAPI/applications/OrientedTactileSensorUnitObserver/CMakeLists.txt b/source/RobotAPI/applications/OrientedTactileSensorUnitObserver/CMakeLists.txt
deleted file mode 100644
index d2bbae6ff527cfbdc1bc36f77b27c71f0f68967a..0000000000000000000000000000000000000000
--- a/source/RobotAPI/applications/OrientedTactileSensorUnitObserver/CMakeLists.txt
+++ /dev/null
@@ -1,11 +0,0 @@
-armarx_component_set_name("OrientedTactileSensorUnitObserverApp")
-
-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
deleted file mode 100644
index e400516ab5faa9c676abd943be2a35afa25c3db3..0000000000000000000000000000000000000000
--- a/source/RobotAPI/applications/OrientedTactileSensorUnitObserver/main.cpp
+++ /dev/null
@@ -1,32 +0,0 @@
-/*
- * 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/applications/PlatformUnitObserver/CMakeLists.txt b/source/RobotAPI/applications/PlatformUnitObserver/CMakeLists.txt
deleted file mode 100644
index c9deac457ac8eab1bdb0f09419066063739ac163..0000000000000000000000000000000000000000
--- a/source/RobotAPI/applications/PlatformUnitObserver/CMakeLists.txt
+++ /dev/null
@@ -1,8 +0,0 @@
-
-armarx_component_set_name(PlatformUnitObserver)
-
-set(COMPONENT_LIBS RobotAPIUnits)
-
-set(SOURCES main.cpp)
-
-armarx_add_component_executable("${SOURCES}")
diff --git a/source/RobotAPI/applications/PlatformUnitObserver/main.cpp b/source/RobotAPI/applications/PlatformUnitObserver/main.cpp
deleted file mode 100644
index 71e4411ddd80a86b6e282b3368140ee61a462820..0000000000000000000000000000000000000000
--- a/source/RobotAPI/applications/PlatformUnitObserver/main.cpp
+++ /dev/null
@@ -1,34 +0,0 @@
-/*
- * 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 Core::application::PlatformUnitObserver
- * @author Manfred Kroehnert
- * @date 2014
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#include <RobotAPI/components/units/PlatformUnitObserver.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::PlatformUnitObserver>(argc, argv, "PlatformUnitObserver");
-}
diff --git a/source/RobotAPI/applications/PlatformUnitSimulation/CMakeLists.txt b/source/RobotAPI/applications/PlatformUnitSimulation/CMakeLists.txt
deleted file mode 100644
index a6eae5afe96958d0839d314a3a90191ee3c17317..0000000000000000000000000000000000000000
--- a/source/RobotAPI/applications/PlatformUnitSimulation/CMakeLists.txt
+++ /dev/null
@@ -1,8 +0,0 @@
-
-armarx_component_set_name(PlatformUnitSimulation)
-
-set(COMPONENT_LIBS RobotAPIUnits)
-
-set(SOURCES main.cpp)
-
-armarx_add_component_executable("${SOURCES}")
diff --git a/source/RobotAPI/applications/PlatformUnitSimulation/main.cpp b/source/RobotAPI/applications/PlatformUnitSimulation/main.cpp
deleted file mode 100644
index bca561916e0cd5f6c2e240f9af82986ea857fd07..0000000000000000000000000000000000000000
--- a/source/RobotAPI/applications/PlatformUnitSimulation/main.cpp
+++ /dev/null
@@ -1,34 +0,0 @@
-/*
- * 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 Core::application::PlatformUnitSimulation
- * @author Manfred Kroehnert
- * @date 2014
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#include <RobotAPI/components/units/PlatformUnitSimulation.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::PlatformUnitSimulation>(argc, argv, "PlatformUnitSimulation");
-}
diff --git a/source/RobotAPI/applications/RobotControl/CMakeLists.txt b/source/RobotAPI/applications/RobotControl/CMakeLists.txt
deleted file mode 100644
index 798af63ac18774167778230a0e8ffa02380fa42d..0000000000000000000000000000000000000000
--- a/source/RobotAPI/applications/RobotControl/CMakeLists.txt
+++ /dev/null
@@ -1,8 +0,0 @@
-
-armarx_component_set_name(RobotControl)
-
-set(COMPONENT_LIBS RobotAPIOperations)
-
-set(SOURCES main.cpp)
-
-armarx_add_component_executable("${SOURCES}")
diff --git a/source/RobotAPI/applications/RobotControl/main.cpp b/source/RobotAPI/applications/RobotControl/main.cpp
deleted file mode 100644
index 720e6420bb59dd20dcda28e2d3fc16dd827171a2..0000000000000000000000000000000000000000
--- a/source/RobotAPI/applications/RobotControl/main.cpp
+++ /dev/null
@@ -1,34 +0,0 @@
-/*
- * 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 Core::application::RobotControl
- * @author Manfred Kroehnert
- * @date 2014
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#include <RobotAPI/statecharts/operations/RobotControl.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::RobotControl>(argc, argv, "RobotControl");
-}
diff --git a/source/RobotAPI/applications/RobotHealth/CMakeLists.txt b/source/RobotAPI/applications/RobotHealth/CMakeLists.txt
deleted file mode 100644
index 1009dfb456bf010814c9aa55aaf8d07d432a63cb..0000000000000000000000000000000000000000
--- a/source/RobotAPI/applications/RobotHealth/CMakeLists.txt
+++ /dev/null
@@ -1,3 +0,0 @@
-armarx_component_set_name("RobotHealthApp")
-set(COMPONENT_LIBS RobotHealth)
-armarx_add_component_executable("main.cpp")
diff --git a/source/RobotAPI/applications/RobotHealth/main.cpp b/source/RobotAPI/applications/RobotHealth/main.cpp
deleted file mode 100644
index 18f81718c69c04a3f939757936d95d248d58ac00..0000000000000000000000000000000000000000
--- a/source/RobotAPI/applications/RobotHealth/main.cpp
+++ /dev/null
@@ -1,32 +0,0 @@
-/*
- * 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::RobotHealth
- * @author Simon Ottenhaus ( simon dot ottenhaus at kit dot edu )
- * @date 2018
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#include <RobotAPI/components/RobotHealth/RobotHealth.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::RobotHealth > (argc, argv, "RobotHealth");
-}
diff --git a/source/RobotAPI/applications/RobotHealthDummy/CMakeLists.txt b/source/RobotAPI/applications/RobotHealthDummy/CMakeLists.txt
deleted file mode 100644
index 47f7ea2455f4723bdff0e6e8025a0b98e5761a51..0000000000000000000000000000000000000000
--- a/source/RobotAPI/applications/RobotHealthDummy/CMakeLists.txt
+++ /dev/null
@@ -1,3 +0,0 @@
-armarx_component_set_name("RobotHealthDummyApp")
-set(COMPONENT_LIBS RobotHealth)
-armarx_add_component_executable("main.cpp")
diff --git a/source/RobotAPI/applications/RobotHealthDummy/main.cpp b/source/RobotAPI/applications/RobotHealthDummy/main.cpp
deleted file mode 100644
index 60e4ee420854515edd778047657ccf414f20fe61..0000000000000000000000000000000000000000
--- a/source/RobotAPI/applications/RobotHealthDummy/main.cpp
+++ /dev/null
@@ -1,32 +0,0 @@
-/*
- * 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::RobotHealthDummy
- * @author Simon Ottenhaus ( simon dot ottenhaus at kit dot edu )
- * @date 2018
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#include <RobotAPI/components/RobotHealth/RobotHealthDummy.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::RobotHealthDummy > (argc, argv, "RobotHealthDummy");
-}
diff --git a/source/RobotAPI/applications/RobotNameService/CMakeLists.txt b/source/RobotAPI/applications/RobotNameService/CMakeLists.txt
deleted file mode 100644
index 0e89b9bfe5ef9b1a5398da55fd607b2fc143cb33..0000000000000000000000000000000000000000
--- a/source/RobotAPI/applications/RobotNameService/CMakeLists.txt
+++ /dev/null
@@ -1,3 +0,0 @@
-armarx_component_set_name("RobotNameServiceApp")
-set(COMPONENT_LIBS RobotNameService)
-armarx_add_component_executable("main.cpp")
diff --git a/source/RobotAPI/applications/RobotNameService/main.cpp b/source/RobotAPI/applications/RobotNameService/main.cpp
deleted file mode 100644
index 4a8242fe17c0647bcf6dae3232d45de9224e3c10..0000000000000000000000000000000000000000
--- a/source/RobotAPI/applications/RobotNameService/main.cpp
+++ /dev/null
@@ -1,32 +0,0 @@
-/*
- * 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::RobotNameService
- * @author [Author Name] ( [Author Email] )
- * @date 2018
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#include <RobotAPI/components/RobotNameService/RobotNameService.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::RobotNameService > (argc, argv, "RobotNameService");
-}
diff --git a/source/RobotAPI/applications/RobotStateObserver/CMakeLists.txt b/source/RobotAPI/applications/RobotStateObserver/CMakeLists.txt
deleted file mode 100644
index 656f134988a1767c4be01b51484ebf6128da6c00..0000000000000000000000000000000000000000
--- a/source/RobotAPI/applications/RobotStateObserver/CMakeLists.txt
+++ /dev/null
@@ -1,4 +0,0 @@
-armarx_component_set_name(RobotStateObserver)
-set(COMPONENT_LIBS RobotAPICore)
-set(SOURCES main.cpp RobotStateObserverApp.h)
-armarx_add_component_executable("${SOURCES}")
diff --git a/source/RobotAPI/applications/RobotStateObserver/config/testProperty b/source/RobotAPI/applications/RobotStateObserver/config/testProperty
deleted file mode 100644
index fd3d73770dab1e8e52b2daef51b188df7b83d1da..0000000000000000000000000000000000000000
--- a/source/RobotAPI/applications/RobotStateObserver/config/testProperty
+++ /dev/null
@@ -1,7 +0,0 @@
-# test config file for KinematicUnit configured to be a Head
-
-ArmarX.KinematicUnitObserver.RobotFileName=/org/share/home/staff/welke/projects/armarx/data/ArmarIII/ArmarIII-Head.xml # (absolut path required) model XML file path containing a VirtualRobot RobotNodeSet that defines the joints
-ArmarX.KinematicUnitObserver.RobotNodeSetName=Head # node set name
-ArmarX.KinematicUnitObserver.AdapterName=KinematicUnitObserver # name of the ICE adapter
-
-
diff --git a/source/RobotAPI/applications/RobotStateObserver/main.cpp b/source/RobotAPI/applications/RobotStateObserver/main.cpp
deleted file mode 100644
index 1c1a74f270c34d93cc1f0895772be7c738139a50..0000000000000000000000000000000000000000
--- a/source/RobotAPI/applications/RobotStateObserver/main.cpp
+++ /dev/null
@@ -1,34 +0,0 @@
-/*
- * 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 Core::application::RobotStateObserver
- * @author Manfred Kroehnert
- * @date 2014
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#include "RobotStateObserverApp.h"
-#include <ArmarXCore/core/logging/Logging.h>
-
-int main(int argc, char* argv[])
-{
- armarx::ApplicationPtr app = armarx::Application::createInstance<armarx::RobotStateObserverApp>();
- app->setName("RobotStateObserver");
-
- return app->main(argc, argv);
-}
diff --git a/source/RobotAPI/applications/ArMemGlobalStorage/CMakeLists.txt b/source/RobotAPI/applications/SimpleEpisodicMemoryKinematicUnitConnector/CMakeLists.txt
similarity index 52%
rename from source/RobotAPI/applications/ArMemGlobalStorage/CMakeLists.txt
rename to source/RobotAPI/applications/SimpleEpisodicMemoryKinematicUnitConnector/CMakeLists.txt
index 9269db1b62679e046c18f02a6ee6243140a17edf..65606db18b31a615c67032d73d46d70a3fa6ee60 100644
--- a/source/RobotAPI/applications/ArMemGlobalStorage/CMakeLists.txt
+++ b/source/RobotAPI/applications/SimpleEpisodicMemoryKinematicUnitConnector/CMakeLists.txt
@@ -1,5 +1,5 @@
-armarx_component_set_name("ArMemGlobalStorageApp")
-set(COMPONENT_LIBS ArMemGlobalStorage)
+armarx_component_set_name("SimpleEpisodicMemoryKinematicUnitConnectorApp")
+set(COMPONENT_LIBS SimpleEpisodicMemoryKinematicUnitConnector)
armarx_add_component_executable(main.cpp)
#find_package(MyLib QUIET)
@@ -7,5 +7,5 @@ armarx_add_component_executable(main.cpp)
# all target_include_directories must be guarded by if(Xyz_FOUND)
# for multiple libraries write: if(X_FOUND AND Y_FOUND)....
#if(MyLib_FOUND)
-# target_include_directories(ArMemGlobalStorage PUBLIC ${MyLib_INCLUDE_DIRS})
+# target_include_directories(SimpleEpisodicMemoryKinematicUnitConnector PUBLIC ${MyLib_INCLUDE_DIRS})
#endif()
diff --git a/source/RobotAPI/applications/NaturalIKTest/main.cpp b/source/RobotAPI/applications/SimpleEpisodicMemoryKinematicUnitConnector/main.cpp
similarity index 68%
rename from source/RobotAPI/applications/NaturalIKTest/main.cpp
rename to source/RobotAPI/applications/SimpleEpisodicMemoryKinematicUnitConnector/main.cpp
index 5f47c44165584faf07301537cd79c2789379bfea..0c70e4777c39461b2ca0336513436c5117d4731b 100644
--- a/source/RobotAPI/applications/NaturalIKTest/main.cpp
+++ b/source/RobotAPI/applications/SimpleEpisodicMemoryKinematicUnitConnector/main.cpp
@@ -13,14 +13,14 @@
* 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::NaturalIKTest
- * @author Simon Ottenhaus ( simon dot ottenhaus at kit dot edu )
+ * @package RobotAPI::application::SimpleEpisodicMemoryKinematicUnitConnector
+ * @author Fabian PK ( fabian dot peller-konrad at kit dot edu )
* @date 2020
* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
* GNU General Public License
*/
-#include <RobotAPI/components/NaturalIKTest/NaturalIKTest.h>
+#include <RobotAPI/components/SimpleEpisodicMemoryKinematicUnitConnector/SimpleEpisodicMemoryKinematicUnitConnector.h>
#include <ArmarXCore/core/application/Application.h>
#include <ArmarXCore/core/Component.h>
@@ -28,5 +28,5 @@
int main(int argc, char* argv[])
{
- return armarx::runSimpleComponentApp < armarx::NaturalIKTest > (argc, argv, "NaturalIKTest");
+ return armarx::runSimpleComponentApp < armarx::SimpleEpisodicMemoryKinematicUnitConnector > (argc, argv, "SimpleEpisodicMemoryKinematicUnitConnector");
}
diff --git a/source/RobotAPI/applications/SpeechObserver/CMakeLists.txt b/source/RobotAPI/applications/SpeechObserver/CMakeLists.txt
deleted file mode 100644
index 513924b4d11e928fca183e52fc3596f99370a3c9..0000000000000000000000000000000000000000
--- a/source/RobotAPI/applications/SpeechObserver/CMakeLists.txt
+++ /dev/null
@@ -1,12 +0,0 @@
-armarx_component_set_name("SpeechObserverApp")
-
-
-set(COMPONENT_LIBS
- ArmarXCoreInterfaces
- ArmarXCore
- RobotAPIUnits
-)
-
-set(EXE_SOURCE main.cpp)
-
-armarx_add_component_executable("${EXE_SOURCE}")
diff --git a/source/RobotAPI/applications/SpeechObserver/main.cpp b/source/RobotAPI/applications/SpeechObserver/main.cpp
deleted file mode 100644
index 2ea82bf4082f388586e0d61d206954f1e7a81547..0000000000000000000000000000000000000000
--- a/source/RobotAPI/applications/SpeechObserver/main.cpp
+++ /dev/null
@@ -1,32 +0,0 @@
-/*
- * 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::SpeechObserver
- * @author Simon Ottenhaus ( simon dot ottenhaus at kit dot edu )
- * @date 2018
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#include <RobotAPI/components/units/SpeechObserver.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::SpeechObserver > (argc, argv, "SpeechObserver");
-}
diff --git a/source/RobotAPI/applications/StatechartExecutorExample/CMakeLists.txt b/source/RobotAPI/applications/StatechartExecutorExample/CMakeLists.txt
deleted file mode 100644
index 7777d60f18d7420ce0542ea8f7406d2e0a20112e..0000000000000000000000000000000000000000
--- a/source/RobotAPI/applications/StatechartExecutorExample/CMakeLists.txt
+++ /dev/null
@@ -1,3 +0,0 @@
-armarx_component_set_name("StatechartExecutorExampleApp")
-set(COMPONENT_LIBS StatechartExecutorExample)
-armarx_add_component_executable(main.cpp)
diff --git a/source/RobotAPI/applications/StatechartExecutorExample/main.cpp b/source/RobotAPI/applications/StatechartExecutorExample/main.cpp
deleted file mode 100644
index cce20936d9e6abf83b80f764e0d5e9a1b231b0c1..0000000000000000000000000000000000000000
--- a/source/RobotAPI/applications/StatechartExecutorExample/main.cpp
+++ /dev/null
@@ -1,32 +0,0 @@
-/*
- * 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 ArmarXGui::application::StatechartExecutorExample
- * @author Stefan Reither ( stefan dot reither at kit dot edu )
- * @date 2019
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#include <RobotAPI/components/StatechartExecutorExample/StatechartExecutorExample.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::StatechartExecutorExample > (argc, argv, "StatechartExecutorExample");
-}
diff --git a/source/RobotAPI/applications/ViewSelection/CMakeLists.txt b/source/RobotAPI/applications/ViewSelection/CMakeLists.txt
deleted file mode 100644
index feeeadcdd892eca6f0c95247c4f17270006a425e..0000000000000000000000000000000000000000
--- a/source/RobotAPI/applications/ViewSelection/CMakeLists.txt
+++ /dev/null
@@ -1,4 +0,0 @@
-armarx_component_set_name("ViewSelectionApp")
-set(COMPONENT_LIBS ViewSelection)
-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
deleted file mode 100644
index 8d19a221169e041eb312a4b1e17889f238014d99..0000000000000000000000000000000000000000
--- a/source/RobotAPI/applications/ViewSelection/ViewSelectionApp.h
+++ /dev/null
@@ -1,52 +0,0 @@
-/*
- * 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
- */
-
-#pragma once
-
-
-#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) override
- {
- registry->addObject(Component::create<ViewSelection>(properties));
- }
- };
-}
-
diff --git a/source/RobotAPI/applications/ViewSelection/main.cpp b/source/RobotAPI/applications/ViewSelection/main.cpp
deleted file mode 100644
index 01e91acc5430646d786aa12fd1207487e0da7a93..0000000000000000000000000000000000000000
--- a/source/RobotAPI/applications/ViewSelection/main.cpp
+++ /dev/null
@@ -1,34 +0,0 @@
-/*
- * 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/applications/XsensIMU/CMakeLists.txt b/source/RobotAPI/applications/XsensIMU/CMakeLists.txt
deleted file mode 100644
index 2289d9a8e053e175b327cc1bf06b8e89b1d31287..0000000000000000000000000000000000000000
--- a/source/RobotAPI/applications/XsensIMU/CMakeLists.txt
+++ /dev/null
@@ -1,7 +0,0 @@
-armarx_component_set_name("XsensIMUApp")
-
-set(COMPONENT_LIBS ArmarXCoreInterfaces ArmarXCore XsensIMU)
-
-set(EXE_SOURCE main.cpp)
-
-armarx_add_component_executable("${EXE_SOURCE}")
diff --git a/source/RobotAPI/applications/XsensIMU/main.cpp b/source/RobotAPI/applications/XsensIMU/main.cpp
deleted file mode 100644
index 2b5d6083b23b08b01c2b7b3824ab79178ed36a9b..0000000000000000000000000000000000000000
--- a/source/RobotAPI/applications/XsensIMU/main.cpp
+++ /dev/null
@@ -1,34 +0,0 @@
-/*
- * 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::application::XsensIMU
- * @author Markus Grotz ( markus-grotz at web dot de )
- * @date 2015
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#include <RobotAPI/drivers/XsensIMU/XsensIMU.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::XsensIMU>(argc, argv, "XsensIMU");
-}
diff --git a/source/RobotAPI/components/ArMemGlobalStorage/ArMemGlobalStorage.cpp b/source/RobotAPI/components/ArMemGlobalStorage/ArMemGlobalStorage.cpp
index f379578e5fcf7cca267fa0cd5a5052e3e023827a..7ae852eeec344ba3d612068c3eb45aea70b1f66f 100644
--- a/source/RobotAPI/components/ArMemGlobalStorage/ArMemGlobalStorage.cpp
+++ b/source/RobotAPI/components/ArMemGlobalStorage/ArMemGlobalStorage.cpp
@@ -22,42 +22,105 @@
#include "ArMemGlobalStorage.h"
+// STD/STL
+#include <algorithm>
-namespace armarx
-{
-
- std::string ArMemGlobalStorage::getDefaultName() const
- {
- return "ArMemGlobalStorage";
- }
-
-
- void ArMemGlobalStorage::onInitComponent()
- {
-
- }
-
-
- void ArMemGlobalStorage::onConnectComponent()
- {
-
- }
-
-
- void ArMemGlobalStorage::onDisconnectComponent()
- {
+// Boost
+#include <boost/asio.hpp>
- }
+// Simox
+#include <SimoxUtility/algorithm/string.h>
+// ArmarX
+#include <RobotAPI/interface/armem.h>
- void ArMemGlobalStorage::onExitComponent()
+namespace armarx
+{
+ namespace armem
{
- }
-
- armarx::PropertyDefinitionsPtr ArMemGlobalStorage::createPropertyDefinitions()
- {
- PropertyDefinitionsPtr defs{new ComponentPropertyDefinitions{getConfigIdentifier()}};
- return defs;
+ armarx::PropertyDefinitionsPtr ArMemGlobalStorage::createPropertyDefinitions()
+ {
+ PropertyDefinitionsPtr defs{new ComponentPropertyDefinitions{getConfigIdentifier()}};
+
+ defs->optional(local_memory_hostnames, "LocalMemoryHostnames", "The hostnames of the local memories, comma separated");
+ return defs;
+ }
+
+ ArMemGlobalStorage::ArMemGlobalStorage():
+ armarx::Component(),
+ armarx::armem::ArMemBase(),
+ armarx::armem::ArMemGlobalMemoryResolver()
+ {
+ }
+
+ std::string ArMemGlobalStorage::getDefaultName() const
+ {
+ return "ArMemGlobalStorage";
+ }
+
+
+ void ArMemGlobalStorage::onInitComponent()
+ {
+ for (const std::string& hostname : simox::alg::split(local_memory_hostnames, ","))
+ {
+ usingProxy(GenerateLocalMemoryObjectNameFromHostname(hostname));
+ }
+ }
+
+
+ void ArMemGlobalStorage::onConnectComponent()
+ {
+ for (const std::string& hostname : simox::alg::split(local_memory_hostnames, ","))
+ {
+ ArMemLocalMemoryInterfacePrx localMemoryPrx = getProxy<ArMemLocalMemoryInterfacePrx>(GenerateLocalMemoryObjectNameFromHostname(hostname));
+ local_memories[hostname] = localMemoryPrx;
+ }
+ }
+
+
+ void ArMemGlobalStorage::onDisconnectComponent()
+ {
+
+ }
+
+
+ void ArMemGlobalStorage::onExitComponent()
+ {
+
+ }
+
+ std::string ArMemGlobalStorage::getHostnameOfCurrentMachine(const Ice::Current&)
+ {
+ return getMyHostname();
+ }
+
+ ArMemLocalMemoryInterfacePrx ArMemGlobalStorage::getMemoryOfCurrentMachine(const Ice::Current& c)
+ {
+ return getMemoryForHostname(getMyHostname(), c);
+ }
+
+ ArMemLocalMemoryInterfacePrx ArMemGlobalStorage::getMemoryForHostname(const std::string& hostname, const Ice::Current&)
+ {
+ if (local_memories.find(hostname) == local_memories.end())
+ {
+ throw LocalException("The local memory of host '" + hostname + "' does not exist!. Could not return proxy!");
+ }
+ return local_memories[hostname];
+ }
+
+ void ArMemGlobalStorage::dynamicallyRegisterNewLocalMemory(const std::string& hostname, const Ice::Current&)
+ {
+ if (local_memories.find(hostname) == local_memories.end())
+ {
+ ArMemLocalMemoryInterfacePrx localMemoryPrx = getProxy<ArMemLocalMemoryInterfacePrx>(GenerateLocalMemoryObjectNameFromHostname(hostname));
+ local_memories[hostname] = localMemoryPrx;
+ }
+ }
+
+ void ArMemGlobalStorage::exportDataOfAllMemoriesToLocation(const std::string&, const Ice::Current&)
+ {
+ // TODO!! Needs Aron-JSON Export?
+ }
}
}
diff --git a/source/RobotAPI/components/ArMemGlobalStorage/ArMemGlobalStorage.h b/source/RobotAPI/components/ArMemGlobalStorage/ArMemGlobalStorage.h
index 16a5afc2018513d13345f1c509648623772095ff..f3534e096425a2719ec6d460cefafc3de38b15b6 100644
--- a/source/RobotAPI/components/ArMemGlobalStorage/ArMemGlobalStorage.h
+++ b/source/RobotAPI/components/ArMemGlobalStorage/ArMemGlobalStorage.h
@@ -22,25 +22,49 @@
#pragma once
+// STD/STL
+#include <map>
+// Aron
+#include <RobotAPI/interface/aron.h>
+#include <RobotAPI/interface/armem.h>
+
+// ArmarX
#include <ArmarXCore/core/Component.h>
+#include <RobotAPI/libraries/armem/ArMemBase.h>
namespace armarx
{
- class ArMemGlobalStorage :
- virtual public armarx::Component
+ namespace armem
{
- public:
- std::string getDefaultName() const override;
+ class ArMemGlobalStorage :
+ virtual public armarx::Component,
+ virtual public armarx::armem::ArMemBase,
+ virtual public armarx::armem::ArMemGlobalMemoryResolver
+ {
+ public:
+ ArMemGlobalStorage();
+
+ std::string getDefaultName() const override;
+
+ std::string getHostnameOfCurrentMachine(const Ice::Current& = Ice::Current());
+ ArMemLocalMemoryInterfacePrx getMemoryOfCurrentMachine(const Ice::Current& = Ice::Current());
+ ArMemLocalMemoryInterfacePrx getMemoryForHostname(const std::string&, const Ice::Current& = Ice::Current());
+
+ void dynamicallyRegisterNewLocalMemory(const std::string&, const Ice::Current& = Ice::Current());
- protected:
- void onInitComponent() override;
- void onConnectComponent() override;
- void onDisconnectComponent() override;
- void onExitComponent() override;
- armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
+ void exportDataOfAllMemoriesToLocation(const std::string&, const Ice::Current& = Ice::Current());
- private:
+ protected:
+ void onInitComponent() override;
+ void onConnectComponent() override;
+ void onDisconnectComponent() override;
+ void onExitComponent() override;
+ armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
- };
+ private:
+ std::string local_memory_hostnames;
+ std::map<std::string, ArMemLocalMemoryInterfacePrx> local_memories;
+ };
+ }
}
diff --git a/source/RobotAPI/components/ArMemGlobalStorage/CMakeLists.txt b/source/RobotAPI/components/ArMemGlobalStorage/CMakeLists.txt
index b911aa9bf33ea98ceb1b6a896dfb827f3ad5162b..df827fa04cd4f34d7c42c861581f12034698a157 100644
--- a/source/RobotAPI/components/ArMemGlobalStorage/CMakeLists.txt
+++ b/source/RobotAPI/components/ArMemGlobalStorage/CMakeLists.txt
@@ -1,22 +1,19 @@
armarx_component_set_name("ArMemGlobalStorage")
-
set(COMPONENT_LIBS
ArmarXCore
ArmarXCoreInterfaces
+ RobotAPICore
RobotAPIInterfaces
armem
)
-set(SOURCES
- ./ArMemGlobalStorage.cpp
-)
-set(HEADERS
- ./ArMemGlobalStorage.h
-)
+set(SOURCES ArMemGlobalStorage.cpp)
+set(HEADERS ArMemGlobalStorage.h)
armarx_add_component("${SOURCES}" "${HEADERS}")
-
# add unit tests
add_subdirectory(test)
+
+armarx_generate_and_add_component_executable(COMPONENT_NAMESPACE "armarx::armem" APPLICATION_APP_SUFFIX)
diff --git a/source/RobotAPI/components/ArMemGlobalStorage/test/ArMemGlobalStorageTest.cpp b/source/RobotAPI/components/ArMemGlobalStorage/test/ArMemGlobalStorageTest.cpp
index 9b008a1b9accb23c2bb351348044d472dfd7a0cf..f5451b02e484b0fde7ce24805beb9b46cc5c2b60 100644
--- a/source/RobotAPI/components/ArMemGlobalStorage/test/ArMemGlobalStorageTest.cpp
+++ b/source/RobotAPI/components/ArMemGlobalStorage/test/ArMemGlobalStorageTest.cpp
@@ -31,7 +31,7 @@
BOOST_AUTO_TEST_CASE(testExample)
{
- armarx::ArMemGlobalStorage instance;
+ armarx::armem::ArMemGlobalStorage instance;
BOOST_CHECK_EQUAL(true, true);
}
diff --git a/source/RobotAPI/components/ArMemLocalStorage/ArMemLocalStorage.cpp b/source/RobotAPI/components/ArMemLocalStorage/ArMemLocalStorage.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..fb1c918d4f1bd5ca554877e3cf3452e456edfef2
--- /dev/null
+++ b/source/RobotAPI/components/ArMemLocalStorage/ArMemLocalStorage.cpp
@@ -0,0 +1,192 @@
+/*
+ * 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::ArmarXObjects::ArMemLocalStorage
+ * @author fabian.peller-konrad@kit.edu ( fabian dot peller-konrad at kit dot edu )
+ * @date 2020
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+// Header
+#include "ArMemLocalStorage.h"
+
+// Boost
+#include <boost/asio.hpp>
+
+namespace armarx
+{
+
+ namespace armem
+ {
+ armarx::PropertyDefinitionsPtr ArMemLocalStorage::createPropertyDefinitions()
+ {
+ PropertyDefinitionsPtr defs{new ComponentPropertyDefinitions{getConfigIdentifier()}};
+ defs->optional(maximum_segments, "MaximumNumberOfSegments", "Maximum number of segments (<0 means infinite)");
+ defs->optional(maximum_entries_per_segment, "MaximumEntriesPerSegment", "Maximum number of accepted datatypes per segment (<0 means infinite)");
+ defs->optional(maximum_entries_per_datatype, "MaximumEntriesPerDatatype", "Maximum number of accepted commits per datatype (<0 means infinite)");
+
+ return defs;
+ }
+
+ std::string ArMemLocalStorage::getDefaultName() const
+ {
+ std::string hostname = getMyHostname();
+ return GenerateLocalMemoryObjectNameFromHostname(hostname);
+ }
+
+ ArMemLocalStorage::ArMemLocalStorage() :
+ armarx::Component(),
+ armarx::armem::ArMemBase(),
+ maximum_segments(-1),
+ maximum_entries_per_segment(-1),
+ maximum_entries_per_datatype(-1)
+ {
+
+ }
+
+ void ArMemLocalStorage::onInitComponent()
+ {
+
+ }
+
+ void ArMemLocalStorage::onConnectComponent()
+ {
+
+ }
+
+ void ArMemLocalStorage::onDisconnectComponent()
+ {
+
+ }
+
+ void ArMemLocalStorage::onExitComponent()
+ {
+
+ }
+
+ bool ArMemLocalStorage::hasSegment(const std::string& segment) const
+ {
+ auto index_pos = indexed_storage.find(segment);
+ auto timestamped_pos = timestamped_storage.find(segment);
+ auto hashed_pos = hashed_storage.find(segment);
+
+ return index_pos == indexed_storage.end() || timestamped_pos == timestamped_storage.end() || hashed_pos == hashed_storage.end();
+ }
+
+
+ std::string ArMemLocalStorage::commit(const std::string& segment, long sentAt, const std::vector<aron::AronDataPtr>& data, const Ice::Current&)
+ {
+ // Check if segment already exists
+ if (!hasSegment(segment))
+ {
+ // Need to allocate a new storage for a new segment
+ ARMARX_WARNING << "A new segment '" + segment + "' registered to the memory. Allocating space for its members...";
+ indexed_storage[segment] = IndexedDatatypeStorage();
+ timestamped_storage[segment] = TimestampedDataTypeStorage();
+ hashed_storage[segment] = HashedDataTypeStorage();
+ }
+
+ IndexedDatatypeStorage& indexed_segment_storage = indexed_storage[segment];
+ TimestampedDataTypeStorage& timestamped_segment_storage = timestamped_storage[segment];
+ HashedDataTypeStorage& hashed_segment_storage = hashed_storage[segment];
+
+ long now = IceUtil::Time::now().toMilliSeconds();
+ if (sentAt > now)
+ {
+ ARMARX_WARNING << "Received an invalid timestamp. The timestamp when sending data to the segment '" + segment + "' is greater than the storage timestamp (The transmitted time needs to be in milliseconds [ms]). Please check!" ;
+ }
+
+ // Create new memory entry and save result
+ ArMemCommitPtr entryPtr = ArMemCommitPtr(new ArMemCommit());
+ entryPtr->producer = "TODO";
+ entryPtr->stored_in_segment = segment;
+ entryPtr->produce_timestamp_ms = sentAt;
+ entryPtr->stored_in_segment = IceUtil::Time::now().toMilliSeconds();
+ entryPtr->data = data;
+
+ ARMARX_INFO_S << "Saving a new commit to segment '" + segment + "'";
+ std::string string_to_hash = "commit_to_" + segment + "_at_" + std::to_string(now);
+ size_t hash = std::hash<std::string> {}(string_to_hash);
+
+ if (timestamped_segment_storage.find(now) != timestamped_segment_storage.end())
+ {
+ ARMARX_WARNING << "The segment '" + segment + "' already contains a value at timestamp " + std::to_string(now) + ". Overwriting value!";
+ }
+ if (hashed_segment_storage.find(hash) != hashed_segment_storage.end())
+ {
+ ARMARX_WARNING << "The segment '" + segment + "' already contains data at the commit hash " + std::to_string(hash) + ". Overwriting value!";
+ }
+
+ indexed_segment_storage.push_back(entryPtr);
+ timestamped_segment_storage[now] = entryPtr;
+ hashed_segment_storage[hash] = entryPtr;
+ return std::to_string(hash);
+ }
+
+ std::string ArMemLocalStorage::commit_single(const std::string& segment, long generatedTS, const aron::AronDataPtr& x, const Ice::Current& c)
+ {
+ return commit(segment, generatedTS, {x}, c);
+ }
+
+ void ArMemLocalStorage::checkStorageIntegrity() const
+ {
+ if (indexed_storage.size() != timestamped_storage.size() ||
+ indexed_storage.size() != hashed_storage.size() ||
+ timestamped_storage.size() != hashed_storage.size())
+ {
+ throw LocalException("The memory is in an invalid state. The storages have different sizes! Please Check!");
+ }
+ for (const auto& [segment, datatypeStorage] : indexed_storage)
+ {
+ if (!hasSegment(segment))
+ {
+ throw LocalException("The memory is in an invalid state. Not all storages contain the segment '" + segment + "'! Please Check!");
+ }
+ }
+ }
+
+ void ArMemLocalStorage::checkStorageIntegrityForSegment(const std::string& segment) const
+ {
+
+ }
+
+
+ ArMemCommitPtr ArMemLocalStorage::getLatestCommitFromSegment(const std::string& segment, const Ice::Current &)
+ {
+ int index = indexed_storage[segment].size() -1;
+ if(index >= 0)
+ {
+ return indexed_storage[segment][index];
+ }
+ return nullptr;
+ }
+
+ ArMemCommitPtr ArMemLocalStorage::getNextCommitFromSegmentForTimestamp(const std::string &, Ice::Long, const Ice::Current &)
+ {
+ return nullptr;
+ }
+
+ TimestampedArMemCommitList ArMemLocalStorage::getAllCommitsBetweenTimestampsFromSegment(const std::string &, Ice::Long, Ice::Long, const Ice::Current &)
+ {
+ return {};
+ }
+
+ TimestampedArMemCommitList ArMemLocalStorage::getAllCommitsFromSegment(const std::string &, const Ice::Current &)
+ {
+ return {};
+ }
+ }
+}
diff --git a/source/RobotAPI/components/ArMemLocalStorage/ArMemLocalStorage.h b/source/RobotAPI/components/ArMemLocalStorage/ArMemLocalStorage.h
new file mode 100644
index 0000000000000000000000000000000000000000..e0e277b28aa29cae01945d0c5e70de0a4a9af4e7
--- /dev/null
+++ b/source/RobotAPI/components/ArMemLocalStorage/ArMemLocalStorage.h
@@ -0,0 +1,89 @@
+/*
+ * 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::ArmarXObjects::ArMemLocalStorage
+ * @author fabian.peller-konrad@kit.edu ( fabian dot peller-konrad at kit dot edu )
+ * @date 2020
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+// STD/STL
+#include <map>
+
+// Aron
+#include <RobotAPI/interface/aron.h>
+#include <RobotAPI/interface/armem.h>
+
+// ArmarX
+#include <ArmarXCore/core/Component.h>
+#include <RobotAPI/libraries/armem/ArMemBase.h>
+
+namespace armarx
+{
+
+ namespace armem
+ {
+ class ArMemLocalStorage :
+ virtual public armarx::Component,
+ virtual public armarx::armem::ArMemBase,
+ virtual public armarx::armem::ArMemLocalMemoryInterface
+ {
+ public:
+ typedef std::vector<ArMemCommitPtr> IndexedDatatypeStorage;
+ typedef std::map<long, ArMemCommitPtr> TimestampedDataTypeStorage;
+ typedef std::map<std::size_t, ArMemCommitPtr> HashedDataTypeStorage;
+
+ ArMemLocalStorage();
+
+ std::string getDefaultName() const override;
+
+ bool hasSegment(const std::string&) const;
+ long size(const std::string&) const;
+
+ ArMemCommitPtr getLatestCommitFromSegment(const std::string&, const Ice::Current& = Ice::Current());
+ ArMemCommitPtr getNextCommitFromSegmentForTimestamp(const std::string&, long, const Ice::Current& = Ice::Current());
+ TimestampedArMemCommitList getAllCommitsBetweenTimestampsFromSegment(const std::string&, Ice::Long, Ice::Long, const Ice::Current& = Ice::Current());
+ TimestampedArMemCommitList getAllCommitsFromSegment(const std::string&, const Ice::Current& = Ice::Current());
+
+ std::string commit(const std::string& segment, long generatedTS, const std::vector<aron::AronDataPtr>&, const Ice::Current& = Ice::Current());
+ std::string commit_single(const std::string& segment, long generatedTS, const aron::AronDataPtr&, const Ice::Current& = Ice::Current());
+
+ protected:
+ void onInitComponent() override;
+ void onConnectComponent() override;
+ void onDisconnectComponent() override;
+ void onExitComponent() override;
+
+ armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
+
+ private:
+
+ void checkStorageIntegrity() const;
+ void checkStorageIntegrityForSegment(const std::string&) const;
+
+ private:
+ std::map<std::string, IndexedDatatypeStorage> indexed_storage;
+ std::map<std::string, TimestampedDataTypeStorage> timestamped_storage;
+ std::map<std::string, HashedDataTypeStorage> hashed_storage;
+
+ long maximum_segments;
+ long maximum_entries_per_segment;
+ long maximum_entries_per_datatype;
+ };
+ }
+}
diff --git a/source/RobotAPI/components/ArMemLocalStorage/CMakeLists.txt b/source/RobotAPI/components/ArMemLocalStorage/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..38cac48d2c54abd4d58c5a64ea5fe6b178a1b79a
--- /dev/null
+++ b/source/RobotAPI/components/ArMemLocalStorage/CMakeLists.txt
@@ -0,0 +1,34 @@
+armarx_component_set_name("ArMemLocalStorage")
+
+
+set(COMPONENT_LIBS
+ ArmarXCore
+ ArmarXCoreInterfaces
+ RobotAPICore
+ RobotAPIInterfaces
+ armem
+)
+
+set(SOURCES
+ ./ArMemLocalStorage.cpp
+)
+set(HEADERS
+ ./ArMemLocalStorage.h
+)
+
+
+armarx_add_component("${SOURCES}" "${HEADERS}")
+
+#find_package(MyLib QUIET)
+#armarx_build_if(MyLib_FOUND "MyLib not available")
+# all target_include_directories must be guarded by if(Xyz_FOUND)
+# for multiple libraries write: if(X_FOUND AND Y_FOUND)....
+#if(MyLib_FOUND)
+# target_include_directories(ArMemLocalStorage PUBLIC ${MyLib_INCLUDE_DIRS})
+#endif()
+
+# add unit tests
+add_subdirectory(test)
+
+#generate the application
+armarx_generate_and_add_component_executable(COMPONENT_NAMESPACE "armarx::armem" APPLICATION_APP_SUFFIX)
diff --git a/source/RobotAPI/libraries/armem/ArMemWorkingMemory.h b/source/RobotAPI/components/ArMemLocalStorage/test/ArMemLocalStorageTest.cpp
similarity index 60%
rename from source/RobotAPI/libraries/armem/ArMemWorkingMemory.h
rename to source/RobotAPI/components/ArMemLocalStorage/test/ArMemLocalStorageTest.cpp
index d8ac0c02c41b669e732a06a81f3d94e9a9fbd7a4..48a74567fdd35708d0d6e30ee8b25f67f4381be3 100644
--- a/source/RobotAPI/libraries/armem/ArMemWorkingMemory.h
+++ b/source/RobotAPI/components/ArMemLocalStorage/test/ArMemLocalStorageTest.cpp
@@ -13,33 +13,25 @@
* 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::ArmarXObjects::armem
- * @author Simon Ottenhaus ( simon dot ottenhaus at kit dot edu )
+ * @package RobotAPI::ArmarXObjects::ArMemLocalStorage
+ * @author fabian.peller-konrad@kit.edu ( fabian dot peller-konrad at kit dot edu )
* @date 2020
* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
* GNU General Public License
*/
-#pragma once
+#define BOOST_TEST_MODULE RobotAPI::ArmarXObjects::ArMemLocalStorage
+#define ARMARX_BOOST_TEST
-namespace armarx
-{
- /**
- * @defgroup Library-armem armem
- * @ingroup RobotAPI
- * A description of the library armem.
- *
- * @class armem
- * @ingroup Library-armem
- * @brief Brief description of class armem.
- *
- * Detailed description of class armem.
- */
- class armem
- {
- public:
+#include <RobotAPI/Test.h>
+#include <RobotAPI/components/ArMemLocalStorage/ArMemLocalStorage.h>
+
+#include <iostream>
- };
+BOOST_AUTO_TEST_CASE(testExample)
+{
+ armarx::armem::ArMemLocalStorage instance;
+ BOOST_CHECK_EQUAL(true, true);
}
diff --git a/source/RobotAPI/components/ArMemLocalStorage/test/CMakeLists.txt b/source/RobotAPI/components/ArMemLocalStorage/test/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..f72cfa041acff0351e753c642b077c34b3e807b8
--- /dev/null
+++ b/source/RobotAPI/components/ArMemLocalStorage/test/CMakeLists.txt
@@ -0,0 +1,5 @@
+
+# Libs required for the tests
+SET(LIBS ${LIBS} ArmarXCore ArMemLocalStorage)
+
+armarx_add_test(ArMemLocalStorageTest ArMemLocalStorageTest.cpp "${LIBS}")
diff --git a/source/RobotAPI/components/ArViz/CMakeLists.txt b/source/RobotAPI/components/ArViz/CMakeLists.txt
index 59d7a077e5f23def8cb11f55edd15d3877dc4360..76331fc89b2352b1dd7130425f1193d370d5e818 100644
--- a/source/RobotAPI/components/ArViz/CMakeLists.txt
+++ b/source/RobotAPI/components/ArViz/CMakeLists.txt
@@ -4,78 +4,79 @@ set(COMPONENT_LIBS
ArmarXCore
RobotAPICore
RobotAPIInterfaces
+ RobotAPIArmarXObjects
boost_iostreams #compression
)
set(SOURCES
-Client/elements/Mesh.cpp
+ Client/elements/Mesh.cpp
-Coin/ElementVisualizer.cpp
+ Coin/ElementVisualizer.cpp
-Coin/VisualizationRobot.cpp
-Coin/VisualizationObject.cpp
+ Coin/VisualizationRobot.cpp
+ Coin/VisualizationObject.cpp
-Coin/Visualizer.cpp
-Coin/RegisterVisualizationTypes.cpp
+ Coin/Visualizer.cpp
+ Coin/RegisterVisualizationTypes.cpp
-Introspection/ElementJsonSerializers.cpp
-Introspection/exceptions.cpp
+ Introspection/ElementJsonSerializers.cpp
+ Introspection/exceptions.cpp
-Introspection/json_base.cpp
-Introspection/json_elements.cpp
-Introspection/json_layer.cpp
-Introspection/register_element_json_serializers.cpp
+ Introspection/json_base.cpp
+ Introspection/json_elements.cpp
+ Introspection/json_layer.cpp
+ Introspection/register_element_json_serializers.cpp
)
set(HEADERS
-IceConversions.h
-
-Coin/ElementVisualizer.h
-
-# Inventor
-Coin/VisualizationBox.h
-Coin/VisualizationCylinder.h
-Coin/VisualizationCylindroid.h
-Coin/VisualizationEllipsoid.h
-Coin/VisualizationSphere.h
-Coin/VisualizationPose.h
-Coin/VisualizationLine.h
-Coin/VisualizationText.h
-Coin/VisualizationArrow.h
-Coin/VisualizationArrowCircle.h
-Coin/VisualizationPointCloud.h
-Coin/VisualizationPolygon.h
-Coin/VisualizationMesh.h
-# Simox
-Coin/VisualizationRobot.h
-Coin/VisualizationObject.h
-
-Coin/Visualizer.h
-
-# Client
-Client/Layer.h
-Client/Elements.h
-Client/Client.h
-Client/ClientCGALExtensions.h
-Client/Color.h
-
-Client/elements/Color.h
-Client/elements/ElementOps.h
-Client/elements/Mesh.h
-Client/elements/MeshCGALExtensions.h
-Client/elements/PointCloud.h
-
-Client/elements/point_cloud_type_traits.hpp
-
-Introspection/ElementJsonSerializers.h
-Introspection/exceptions.h
-
-Introspection/json_base.h
-Introspection/json_elements.h
-Introspection/json_layer.h
+ IceConversions.h
+
+ Coin/ElementVisualizer.h
+
+ # Inventor
+ Coin/VisualizationBox.h
+ Coin/VisualizationCylinder.h
+ Coin/VisualizationCylindroid.h
+ Coin/VisualizationEllipsoid.h
+ Coin/VisualizationSphere.h
+ Coin/VisualizationPose.h
+ Coin/VisualizationLine.h
+ Coin/VisualizationText.h
+ Coin/VisualizationArrow.h
+ Coin/VisualizationArrowCircle.h
+ Coin/VisualizationPointCloud.h
+ Coin/VisualizationPolygon.h
+ Coin/VisualizationMesh.h
+ # Simox
+ Coin/VisualizationRobot.h
+ Coin/VisualizationObject.h
+
+ Coin/Visualizer.h
+
+ # Client
+ Client/Layer.h
+ Client/Elements.h
+ Client/Client.h
+ Client/ClientCGALExtensions.h
+ Client/Color.h
+
+ Client/elements/Color.h
+ Client/elements/ElementOps.h
+ Client/elements/Mesh.h
+ Client/elements/MeshCGALExtensions.h
+ Client/elements/PointCloud.h
+
+ Client/elements/point_cloud_type_traits.hpp
+
+ Introspection/ElementJsonSerializers.h
+ Introspection/exceptions.h
+
+ Introspection/json_base.h
+ Introspection/json_elements.h
+ Introspection/json_layer.h
)
diff --git a/source/RobotAPI/components/ArViz/Client/Client.h b/source/RobotAPI/components/ArViz/Client/Client.h
index 49b5356eb07cabb999078791c0f675ce29a54e1c..8f2cfb6f2171a310de8f5305c0ffbea0273f09b9 100644
--- a/source/RobotAPI/components/ArViz/Client/Client.h
+++ b/source/RobotAPI/components/ArViz/Client/Client.h
@@ -63,7 +63,6 @@ namespace armarx::viz
l.add(std::forward<Ts>(elems)...);
return l;
}
-
// ////////////////////////////////////////////////////////////////// //
//enqueue
void enqueueLayer(const Layer& l)
diff --git a/source/RobotAPI/components/ArViz/Client/elements/ElementOps.h b/source/RobotAPI/components/ArViz/Client/elements/ElementOps.h
index cfba70de44efdf1ad9d873d33659eee2323413c7..e7b8f0542fecdb99ec55599c9fdf4a371a3c4065 100644
--- a/source/RobotAPI/components/ArViz/Client/elements/ElementOps.h
+++ b/source/RobotAPI/components/ArViz/Client/elements/ElementOps.h
@@ -3,6 +3,7 @@
#pragma once
#include <SimoxUtility/color/GlasbeyLUT.h>
+#include <SimoxUtility/math/convert/rpy_to_quat.h>
#include <RobotAPI/interface/ArViz/Elements.h>
@@ -37,15 +38,18 @@ namespace armarx::viz
}
// TODO: add more overloads
- DerivedT& position(Eigen::Vector3f const& pos)
+ DerivedT& position(float x, float y, float z)
{
auto& pose = data_->pose;
- pose.x = pos.x();
- pose.y = pos.y();
- pose.z = pos.z();
-
+ pose.x = x;
+ pose.y = y;
+ pose.z = z;
return *static_cast<DerivedT*>(this);
}
+ DerivedT& position(Eigen::Vector3f const& pos)
+ {
+ return position(pos.x(), pos.y(), pos.z());
+ }
DerivedT& orientation(Eigen::Quaternionf const& ori)
{
@@ -57,11 +61,14 @@ namespace armarx::viz
return *static_cast<DerivedT*>(this);
}
-
DerivedT& orientation(Eigen::Matrix3f const& ori)
{
return orientation(Eigen::Quaternionf(ori));
}
+ DerivedT& orientation(float r, float p, float y)
+ {
+ return orientation(simox::math::rpy_to_quat(r, p, y));
+ }
DerivedT& pose(Eigen::Matrix4f const& pose)
{
@@ -78,6 +85,22 @@ namespace armarx::viz
return this->position(position).orientation(orientation);
}
+ Eigen::Matrix4f pose() const
+ {
+ auto& p = data_->pose;
+ Eigen::Matrix4f m = Eigen::Matrix4f::Identity();
+ m(0, 3) = p.x;
+ m(1, 3) = p.y;
+ m(2, 3) = p.z;
+ m.topLeftCorner<3, 3>() = Eigen::Quaternionf{p.qw, p.qx, p.qy, p.qz}.toRotationMatrix();
+ return m;
+ }
+
+ DerivedT& transformPose(Eigen::Matrix4f const& p)
+ {
+ return pose(p * pose());
+ }
+
DerivedT& color(Color color)
{
data_->color = color;
diff --git a/source/RobotAPI/components/ArViz/Coin/VisualizationObject.cpp b/source/RobotAPI/components/ArViz/Coin/VisualizationObject.cpp
index 3a066c978c6464055b8968e9c8c893b6b91412f4..10a8bf4858baf5806efc51318a582e457e260e35 100644
--- a/source/RobotAPI/components/ArViz/Coin/VisualizationObject.cpp
+++ b/source/RobotAPI/components/ArViz/Coin/VisualizationObject.cpp
@@ -1,21 +1,50 @@
#include "VisualizationObject.h"
+#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
+
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/system/ArmarXDataPath.h>
+
#include <VirtualRobot/ManipulationObject.h>
#include <VirtualRobot/SceneObject.h>
#include <VirtualRobot/XML/RobotIO.h>
#include <VirtualRobot/XML/ObjectIO.h>
#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualization.h>
-#include <boost/algorithm/string/predicate.hpp>
-
+#include <SimoxUtility/algorithm/string/string_tools.h>
namespace armarx::viz::coin
{
+
namespace
{
+ std::string findObjectInArmarXObjects(const std::string& filename)
+ {
+ IceUtil::Time start = IceUtil::Time::now();
+ std::string objectName = std::filesystem::path(filename).filename().stem();
+ // ARMARX_INFO << "Trying to find object '" << objectName << "' in ArmarXObjects.";
+
+ std::string fullFilename;
+ std::stringstream ss;
+
+ armarx::ObjectFinder objectFinder;
+ if (std::optional<armarx::ObjectInfo> info = objectFinder.findObject("", objectName))
+ {
+ fullFilename = info->simoxXML().absolutePath;
+ ss << "Found '" << objectName << "' in ArmarXObjects as " << *info << " \nat '" << fullFilename << "'.";
+ }
+ else
+ {
+ ss << "Did not find '" << objectName << "' in ArmarXObjects.";
+ }
+ ss << "\n(Lookup took " << (IceUtil::Time::now() - start).toMilliSecondsDouble() << " ms.)";
+ ARMARX_INFO << ss.str();
+
+ return fullFilename;
+ }
+
+
VirtualRobot::ManipulationObjectPtr loadObject(std::string const& project, std::string const& filename)
{
VirtualRobot::ManipulationObjectPtr result;
@@ -26,30 +55,31 @@ namespace armarx::viz::coin
return result;
}
- if (project.size() > 0)
- {
- ARMARX_INFO << "Adding to datapaths of " << project;
- armarx::CMakePackageFinder finder(project);
+ bool checkArmarXObjects = true;
- if (!finder.packageFound())
+ ArmarXDataPath::FindPackageAndAddDataPath(project);
+ std::string fullFilename;
+ if (!ArmarXDataPath::SearchReadableFile(filename, fullFilename))
+ {
+ fullFilename = "";
+ if (checkArmarXObjects)
{
- ARMARX_WARNING << "ArmarX Package " << project << " has not been found!";
+ fullFilename = findObjectInArmarXObjects(filename);
}
- else
+ if (fullFilename.empty())
{
- ARMARX_INFO << "Adding to datapaths: " << finder.getDataDir();
- armarx::ArmarXDataPath::addDataPaths(finder.getDataDir());
+ ARMARX_INFO << deactivateSpam()
+ << "Unable to find readable file for name "
+ << filename;
+ return result;
}
}
- std::string fullFilename = filename;
- ArmarXDataPath::getAbsolutePath(fullFilename, fullFilename);
-
try
{
ARMARX_INFO << "Loading object from " << fullFilename;
- if (boost::ends_with(fullFilename, ".wrl"))
+ if (simox::alg::ends_with(fullFilename, ".wrl"))
{
VirtualRobot::VisualizationFactoryPtr factory = VirtualRobot::VisualizationFactory::fromName("inventor", NULL);
VirtualRobot::VisualizationNodePtr vis = factory->getVisualizationFromFile(fullFilename);
@@ -68,8 +98,6 @@ namespace armarx::viz::coin
<< "\nReason: " << ex.what();
}
-
-
return result;
}
@@ -95,6 +123,7 @@ namespace armarx::viz::coin
result.filename = filename;
result.object = loadObject(project, filename);
+
objectcache.push_back(result);
return result;
@@ -111,7 +140,7 @@ namespace armarx::viz::coin
}
if (!loaded.object)
{
- ARMARX_WARNING << deactivateSpam(10)
+ ARMARX_WARNING << deactivateSpam(120)
<< "Object will not be visualized since it could not be loaded."
<< "\nID: " << element.id
<< "\nProject: " << element.project
diff --git a/source/RobotAPI/components/ArViz/Coin/VisualizationRobot.cpp b/source/RobotAPI/components/ArViz/Coin/VisualizationRobot.cpp
index 0e2ae813fbef99c1ce24e35c8be0be63deeeff8b..bf0ae91ddc2dd48cd7368254c58cb80258d04d3f 100644
--- a/source/RobotAPI/components/ArViz/Coin/VisualizationRobot.cpp
+++ b/source/RobotAPI/components/ArViz/Coin/VisualizationRobot.cpp
@@ -1,3 +1,6 @@
+#include <regex>
+#include <fstream>
+
#include "VisualizationRobot.h"
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
@@ -22,30 +25,20 @@ namespace armarx::viz::coin
return result;
}
- if (project.size() > 0)
+ ArmarXDataPath::FindPackageAndAddDataPath(project);
+ std::string fullFilename;
+ if (!ArmarXDataPath::SearchReadableFile(filename, fullFilename))
{
- ARMARX_INFO << "Adding to datapaths of " << project;
- armarx::CMakePackageFinder finder(project);
-
- if (!finder.packageFound())
- {
- ARMARX_WARNING << "ArmarX Package " << project << " has not been found!";
- }
- else
- {
- ARMARX_INFO << "Adding to datapaths: " << finder.getDataDir();
- armarx::ArmarXDataPath::addDataPaths(finder.getDataDir());
- }
+ ARMARX_INFO << deactivateSpam()
+ << "Unable to find readable file for name "
+ << filename;
+ return result;
}
- std::string fullFilename = filename;
- ArmarXDataPath::getAbsolutePath(fullFilename, fullFilename);
-
- // Always load full model. Visualization can choose between full and collision model
- VirtualRobot::RobotIO::RobotDescription loadMode = VirtualRobot::RobotIO::eFull;
-
try
{
+ // Always load full model. Visualization can choose between full and collision model
+ VirtualRobot::RobotIO::RobotDescription loadMode = VirtualRobot::RobotIO::eFull;
ARMARX_INFO << "Loading robot from " << fullFilename;
result = VirtualRobot::RobotIO::loadRobot(fullFilename, loadMode);
result->setThreadsafe(false);
@@ -75,12 +68,15 @@ namespace armarx::viz::coin
{
if (loaded.project == project && loaded.filename == filename)
{
+ ARMARX_DEBUG << "loading robot from chace " << VAROUT(project) << ", " << VAROUT(filename);
result = loaded;
- result.robot = loaded.robot->clone();
+ //do not scale the robot and do not clone meshes if scaling = 1
+ result.robot = loaded.robot->clone(nullptr, 1, true);
return result;
}
}
+ ARMARX_DEBUG << "loading robot from file " << VAROUT(project) << ", " << VAROUT(filename);
result.project = project;
result.filename = filename;
result.robot = loadRobot(project, filename);
@@ -137,9 +133,9 @@ namespace armarx::viz::coin
if (loadedDrawStyle & data::ModelDrawStyle::OVERRIDE_COLOR)
{
if (loadedColor.r != element.color.r
- || loadedColor.g != element.color.g
- || loadedColor.b != element.color.b
- || loadedColor.a != element.color.a)
+ || loadedColor.g != element.color.g
+ || loadedColor.b != element.color.b
+ || loadedColor.a != element.color.a)
{
int numChildren = node->getNumChildren();
for (int i = 0; i < numChildren; i++)
diff --git a/source/RobotAPI/components/ArViz/Coin/Visualizer.cpp b/source/RobotAPI/components/ArViz/Coin/Visualizer.cpp
index 692800bc7d85488b4af3dc7090c6efabde509911..ffc286590837b87defa10ac9c183092dd185f372 100644
--- a/source/RobotAPI/components/ArViz/Coin/Visualizer.cpp
+++ b/source/RobotAPI/components/ArViz/Coin/Visualizer.cpp
@@ -128,8 +128,13 @@ namespace armarx::viz
}
- void CoinVisualizer::apply(data::LayerUpdate const& update)
+ CoinVisualizer_ApplyTiming CoinVisualizer::apply(data::LayerUpdate const& update)
{
+ IceUtil::Time time_start = IceUtil::Time::now();
+
+ CoinVisualizer_ApplyTiming timing;
+ timing.layerName = update.name;
+
auto layerID = std::make_pair(update.component, update.name);
auto layerIt = layers.find(layerID);
@@ -143,14 +148,19 @@ namespace armarx::viz
layerIt = layers.emplace(layerID, CoinLayer(coinNode)).first;
}
- //IceUtil::Time time_start1 = IceUtil::Time::now();
- //IceUtil::Time time_start = IceUtil::Time::now();
+ IceUtil::Time time_addLayer = IceUtil::Time::now();
+ timing.addLayer = time_addLayer - time_start;
// Add or update the elements in the update
CoinLayer& layer = layerIt->second;
for (auto& updatedElementPtr : update.elements)
{
-
+ if (!updatedElementPtr)
+ {
+ ARMARX_WARNING << deactivateSpam(10) << "Element is null in layer "
+ << update.component << "/" << update.name;
+ continue;
+ }
data::Element const& updatedElement = *updatedElementPtr;
std::type_index elementType = typeid(updatedElement);
@@ -171,10 +181,8 @@ namespace armarx::viz
continue;
}
coin::ElementVisualizer* visualizer = elementVisualizers[visuIndex].get();
- //IceUtil::Time time_findVisu = IceUtil::Time::now();
auto oldElementIter = layer.elements.find(updatedElement.id);
- //IceUtil::Time time_findElement = IceUtil::Time::now();
if (oldElementIter != layer.elements.end())
{
// Element already exists
@@ -182,21 +190,9 @@ namespace armarx::viz
oldElement.wasUpdated = true;
bool updated = visualizer->update(updatedElement, &oldElement);
- IceUtil::Time time_update = IceUtil::Time::now();
if (updated)
{
- // if (update.name == "Example")
- // {
- // ARMARX_INFO << "Elem " << updatedElement.id
- // << "\nFindV: " << (time_findVisu - time_start).toMilliSecondsDouble()
- // << "\nFindE: " << (time_findElement - time_findVisu).toMilliSecondsDouble()
- // << "\nUpdate: " << (time_update - time_findElement).toMilliSecondsDouble()
- // << "\nTotal: " << (time_update - time_start).toMilliSecondsDouble();
- // // ARMARX_INFO << "Elem " << updatedElement.id
- // // << ": " << (time_update - time_start).toMilliSecondsDouble();
- // time_start = time_update;
- // }
layer.elements[updatedElement.id].data = updatedElementPtr;
continue;
}
@@ -221,7 +217,9 @@ namespace armarx::viz
<< "You need to register a visualizer for each type in ArViz/Coin/Visualizer.cpp";
}
}
+
IceUtil::Time time_updates = IceUtil::Time::now();
+ timing.updateElements = time_updates - time_addLayer;
// Remove the elements which were not contained in the update
for (auto iter = layer.elements.begin(); iter != layer.elements.end();)
@@ -238,59 +236,56 @@ namespace armarx::viz
iter = layer.elements.erase(iter);
}
}
- //IceUtil::Time time_remove = IceUtil::Time::now();
- // if (update.name == "Example")
- // {
- // IceUtil::Time duration = IceUtil::Time::now() - time_start1;
- // ARMARX_INFO << "Layer " << update.name
- // << "\nUpdates: " << (time_updates - time_start1).toMilliSecondsDouble()
- // << "\nRemoves: " << (time_remove - time_updates).toMilliSecondsDouble()
- // << "\nTotal: " << (time_remove - time_start1).toMilliSecondsDouble();
- // }
+ IceUtil::Time time_remove = IceUtil::Time::now();
+ timing.removeElements = time_remove - time_updates;
emitLayerUpdated(layerID, layer);
+
+ IceUtil::Time time_end = IceUtil::Time::now();
+ timing.total = time_end - time_start;
+
+ return timing;
}
void CoinVisualizer::update()
{
+ switch (state)
{
- switch (state)
+ case CoinVisualizerState::STARTING:
{
- case CoinVisualizerState::STARTING:
- {
- std::unique_lock<std::mutex> lock(stateMutex);
- storage = stateStorage;
- root->removeAllChildren();
- layers.clear();
- pulledUpdates.revision = 0;
- pulledUpdates.updates.clear();
- updateResult = CoinVisualizerUpdateResult::SUCCESS;
- state = CoinVisualizerState::RUNNING;
- }
- break;
-
- case CoinVisualizerState::RUNNING:
- break;
+ std::unique_lock<std::mutex> lock(stateMutex);
+ storage = stateStorage;
+ root->removeAllChildren();
+ layers.clear();
+ pulledUpdates.revision = 0;
+ pulledUpdates.updates.clear();
+ updateResult = CoinVisualizerUpdateResult::SUCCESS;
+ state = CoinVisualizerState::RUNNING;
+ }
+ break;
- case CoinVisualizerState::STOPPING:
- {
- // After we have reached the STOPPED state, we know that updates are no longer running
- state = CoinVisualizerState::STOPPED;
- return;
- }
+ case CoinVisualizerState::RUNNING:
break;
- case CoinVisualizerState::STOPPED:
- return;
+ case CoinVisualizerState::STOPPING:
+ {
+ // After we have reached the STOPPED state, we know that updates are no longer running
+ state = CoinVisualizerState::STOPPED;
+ return;
}
+ break;
+
+ case CoinVisualizerState::STOPPED:
+ return;
}
switch (updateResult)
{
case CoinVisualizerUpdateResult::SUCCESS:
{
- //IceUtil::Time time_start = IceUtil::Time::now();
+ IceUtil::Time time_start = IceUtil::Time::now();
+ CoinVisualizer_UpdateTiming timing;
// We should restart the pull for updates so it can run in parallel
data::LayerUpdates currentUpdates = pulledUpdates;
@@ -299,27 +294,35 @@ namespace armarx::viz
auto layerIDsBefore = getLayerIDs();
- //IceUtil::Time time_afterPull = IceUtil::Time::now();
+ IceUtil::Time time_pull = IceUtil::Time::now();
+ timing.pull = time_pull - time_start;
+ timing.applies.reserve(currentUpdates.updates.size());
for (data::LayerUpdate const& update : currentUpdates.updates)
{
- apply(update);
+ auto& applyTiming = timing.applies.emplace_back(apply(update));
+ timing.applyTotal.add(applyTiming);
}
- //IceUtil::Time time_afterApply = IceUtil::Time::now();
+ IceUtil::Time time_apply = IceUtil::Time::now();
+ timing.applyTotal.total = time_apply - time_pull;
auto layerIDsAfter = getLayerIDs();
if (layerIDsAfter != layerIDsBefore)
{
emitLayersChanged(layerIDsAfter);
}
- //IceUtil::Time time_afterLayersChanged = IceUtil::Time::now();
-
- // Most of the time is spent in apply
- // ARMARX_INFO << "Updates: " << pulledUpdates.updates.size()
- // << "\nTotal: " << (time_afterLayersChanged - time_start).toMilliSecondsDouble()
- // << "\nPull: " << (time_afterPull - time_start).toMilliSecondsDouble()
- // << "\nApply: " << (time_afterApply - time_afterPull).toMilliSecondsDouble()
- // << "\nLayers: " << (time_afterLayersChanged - time_afterApply).toMilliSecondsDouble();
+ IceUtil::Time time_layersChanged = IceUtil::Time::now();
+ timing.layersChanged = time_layersChanged - time_apply;
+
+ IceUtil::Time time_end = IceUtil::Time::now();
+ timing.total = time_end - time_start;
+
+ {
+ // Copy the timing result
+ std::lock_guard lock(timingMutex);
+ timing.counter = lastTiming.counter + 1;
+ lastTiming = std::move(timing);
+ }
}
break;
case CoinVisualizerUpdateResult::WAITING:
@@ -334,6 +337,7 @@ namespace armarx::viz
}
break;
}
+
}
void CoinVisualizer::onUpdateSuccess(const data::LayerUpdates& updates)
@@ -377,6 +381,12 @@ namespace armarx::viz
}
}
+ CoinVisualizer_UpdateTiming CoinVisualizer::getTiming()
+ {
+ std::lock_guard lock(timingMutex);
+ return lastTiming;
+ }
+
std::vector<CoinLayerID> CoinVisualizer::getLayerIDs()
{
std::vector<CoinLayerID> result;
diff --git a/source/RobotAPI/components/ArViz/Coin/Visualizer.h b/source/RobotAPI/components/ArViz/Coin/Visualizer.h
index 842ad772e6310d35e5b5b44812670d060c7c100a..f3a2248254821f96067f16a6306b02141a1438a6 100644
--- a/source/RobotAPI/components/ArViz/Coin/Visualizer.h
+++ b/source/RobotAPI/components/ArViz/Coin/Visualizer.h
@@ -79,6 +79,37 @@ namespace armarx::viz
FAILURE,
};
+ struct CoinVisualizer_ApplyTiming
+ {
+ std::string layerName;
+
+ IceUtil::Time addLayer = IceUtil::Time::seconds(0);
+ IceUtil::Time updateElements = IceUtil::Time::seconds(0);
+ IceUtil::Time removeElements = IceUtil::Time::seconds(0);
+ IceUtil::Time total = IceUtil::Time::seconds(0);
+
+ void add(CoinVisualizer_ApplyTiming const& other)
+ {
+ addLayer += other.addLayer;
+ updateElements += other.updateElements;
+ removeElements += other.removeElements;
+ total += other.total;
+ }
+ };
+
+ struct CoinVisualizer_UpdateTiming
+ {
+ std::vector<CoinVisualizer_ApplyTiming> applies;
+
+ CoinVisualizer_ApplyTiming applyTotal;
+
+ IceUtil::Time pull = IceUtil::Time::seconds(0);
+ IceUtil::Time layersChanged = IceUtil::Time::seconds(0);
+ IceUtil::Time total = IceUtil::Time::seconds(0);
+
+ int counter = 0;
+ };
+
struct CoinVisualizerWrapper;
class CoinVisualizer
@@ -94,7 +125,7 @@ namespace armarx::viz
void stop();
- void apply(data::LayerUpdate const& update);
+ CoinVisualizer_ApplyTiming apply(data::LayerUpdate const& update);
void update();
@@ -110,6 +141,8 @@ namespace armarx::viz
void showLayer(CoinLayerID const& id, bool visible);
+ CoinVisualizer_UpdateTiming getTiming();
+
std::vector<CoinLayerID> getLayerIDs();
void emitLayersChanged(std::vector<CoinLayerID> const& layerIDs);
void emitLayerUpdated(CoinLayerID const& layerID, CoinLayer const& layer);
@@ -142,5 +175,7 @@ namespace armarx::viz
/// A layer's data has changed.
std::vector<std::function<void(CoinLayerID const& layerID, CoinLayer const& layer)>> layerUpdatedCallbacks;
+ std::mutex timingMutex;
+ CoinVisualizer_UpdateTiming lastTiming;
};
}
diff --git a/source/RobotAPI/components/AronTest/AronTest.cpp b/source/RobotAPI/components/AronTest/AronTest.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..d31afa2d2875b28825e8caf400a0af063343730d
--- /dev/null
+++ b/source/RobotAPI/components/AronTest/AronTest.cpp
@@ -0,0 +1,62 @@
+/*
+ * 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::ArmarXObjects::AronTest
+ * @author fabian.peller-konrad@kit.edu ( fabian dot peller-konrad at kit dot edu )
+ * @date 2020
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "AronTest.h"
+
+
+namespace armarx
+{
+
+ armarx::PropertyDefinitionsPtr AronTest::createPropertyDefinitions()
+ {
+ PropertyDefinitionsPtr defs{new ComponentPropertyDefinitions{getConfigIdentifier()}};
+ return defs;
+ }
+
+ std::string AronTest::getDefaultName() const
+ {
+ return "AronTest";
+ }
+
+
+ void AronTest::onInitComponent()
+ {
+
+ }
+
+
+ void AronTest::onConnectComponent()
+ {
+ }
+
+
+ void AronTest::onDisconnectComponent()
+ {
+
+ }
+
+
+ void AronTest::onExitComponent()
+ {
+
+ }
+}
diff --git a/source/RobotAPI/components/AronTest/AronTest.h b/source/RobotAPI/components/AronTest/AronTest.h
new file mode 100644
index 0000000000000000000000000000000000000000..51bbd65d659bccbb16cc263cf63f4b487df3200b
--- /dev/null
+++ b/source/RobotAPI/components/AronTest/AronTest.h
@@ -0,0 +1,65 @@
+/*
+ * 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::ArmarXObjects::AronTest
+ * @author fabian.peller-konrad@kit.edu ( fabian dot peller-konrad at kit dot edu )
+ * @date 2020
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+
+#include <ArmarXCore/core/Component.h>
+
+#include <ArmarXCore/interface/observers/ObserverInterface.h>
+//#include <RobotAPI/libraries/core/visualization/DebugDrawerTopic.h>
+
+
+namespace armarx
+{
+
+ class AronTest :
+ virtual public armarx::Component
+ {
+ public:
+
+ /// @see armarx::ManagedIceObject::getDefaultName()
+ std::string getDefaultName() const override;
+
+
+ protected:
+
+ /// @see armarx::ManagedIceObject::onInitComponent()
+ void onInitComponent() override;
+
+ /// @see armarx::ManagedIceObject::onConnectComponent()
+ void onConnectComponent() override;
+
+ /// @see armarx::ManagedIceObject::onDisconnectComponent()
+ void onDisconnectComponent() override;
+
+ /// @see armarx::ManagedIceObject::onExitComponent()
+ void onExitComponent() override;
+
+ /// @see PropertyUser::createPropertyDefinitions()
+ armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
+
+
+ private:
+
+ };
+}
diff --git a/source/RobotAPI/components/AronTest/AronTypes.h b/source/RobotAPI/components/AronTest/AronTypes.h
new file mode 100644
index 0000000000000000000000000000000000000000..44c91a5dfd2ea026fb981045503e278ce88989fc
--- /dev/null
+++ b/source/RobotAPI/components/AronTest/AronTypes.h
@@ -0,0 +1,70 @@
+#ifndef ARONTESTPRODUCER_ARON_TYPE_DEFINITION_INCLUDE_GUARD
+#define ARONTESTPRODUCER_ARON_TYPE_DEFINITION_INCLUDE_GUARD
+
+#include <string>
+#include <vector>
+#include <map>
+#include <Eigen/Core>
+#include <RobotAPI/interface/aron.h>
+#include <RobotAPI/libraries/aron/io/AronWriter.h>
+#include <RobotAPI/libraries/aron/io/AronReader.h>
+#include <RobotAPI/libraries/aron/io/classWriters/AronDataNavigatorWriter/AronDataNavigatorWriter.h>
+#include <RobotAPI/libraries/aron/io/classReaders/AronDataNavigatorReader/AronDataNavigatorReader.h>
+
+namespace armarx
+{
+ class NaturalIKResult
+ {
+ public:
+ std::vector<float> jointValues;
+ bool reached;
+ NaturalIKResult()
+ {
+ }
+
+ public:
+ void write(armarx::aron::io::AronWriter& w) const
+ {
+ w.writeStartObject();
+ w.writeKey("jointValues");
+ w.writeStartList();
+ for(unsigned int jointValues_index = 0; jointValues_index < jointValues.size(); ++jointValues_index)
+ {
+ w.writeFloat(jointValues[jointValues_index]);
+ }
+ w.writeEndList();
+ w.writeKey("reached");
+ w.writeBool(reached);
+ w.writeEndObject();
+ }
+ void read(armarx::aron::io::AronReader& r)
+ {
+ r.readStartObject();
+ r.readKey("jointValues");
+ r.readStartList();
+ jointValues.clear();
+ while(!r.readEndList())
+ {
+ float jointValues_iterator = 0;
+ jointValues_iterator = r.readFloat();
+ jointValues.push_back(jointValues_iterator);
+ }
+ r.readKey("reached");
+ reached = r.readBool();
+ r.readEndObject();
+ }
+ armarx::aron::AronDataPtr toAron() const
+ {
+ armarx::aron::io::AronDataNavigatorWriter writer;
+ this->write(writer);
+ return writer.getResult();
+ }
+ void fromAron(const armarx::aron::AronDataPtr& input)
+ {
+ armarx::aron::io::AronDataNavigatorReader reader(input);
+ this->read(reader);
+ }
+ }; // class NaturalIKResult
+} // namespace armarx
+
+#endif // ARONTESTPRODUCER_ARON_TYPE_DEFINITION_INCLUDE_GUARD
diff --git a/source/RobotAPI/components/AronTest/CMakeLists.txt b/source/RobotAPI/components/AronTest/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..d7a589fab5a26e7e85dbc2de159d53cdc09c4b92
--- /dev/null
+++ b/source/RobotAPI/components/AronTest/CMakeLists.txt
@@ -0,0 +1,22 @@
+armarx_component_set_name("AronTest")
+
+
+set(COMPONENT_LIBS
+ ArmarXCore
+ ArmarXCoreInterfaces
+ aron
+)
+
+set(SOURCES
+ ./AronTest.cpp
+)
+set(HEADERS
+ ./AronTest.h
+ ./AronTypes.h
+)
+
+
+armarx_add_component("${SOURCES}" "${HEADERS}")
+
+# add unit tests
+#add_subdirectory(test)
diff --git a/source/RobotAPI/libraries/aron/test/NaturalIK.xml b/source/RobotAPI/components/AronTest/TypeDescription.xml
similarity index 100%
rename from source/RobotAPI/libraries/aron/test/NaturalIK.xml
rename to source/RobotAPI/components/AronTest/TypeDescription.xml
diff --git a/source/RobotAPI/libraries/armem/ArMemWorkingMemory.cpp b/source/RobotAPI/components/AronTest/test/AronTestTest.cpp
similarity index 63%
rename from source/RobotAPI/libraries/armem/ArMemWorkingMemory.cpp
rename to source/RobotAPI/components/AronTest/test/AronTestTest.cpp
index 601a7e37e774a759321cbd5f20004559f7781a85..9e9c6d11cbb9f939ab1f801e7fe833a1a87c6a26 100644
--- a/source/RobotAPI/libraries/armem/ArMemWorkingMemory.cpp
+++ b/source/RobotAPI/components/AronTest/test/AronTestTest.cpp
@@ -13,16 +13,25 @@
* 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::ArmarXObjects::armem
- * @author Simon Ottenhaus ( simon dot ottenhaus at kit dot edu )
+ * @package RobotAPI::ArmarXObjects::AronTest
+ * @author fabian.peller-konrad@kit.edu ( fabian dot peller-konrad at kit dot edu )
* @date 2020
* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
* GNU General Public License
*/
-#include "armem.h"
+#define BOOST_TEST_MODULE RobotAPI::ArmarXObjects::AronTest
-namespace armarx
+#define ARMARX_BOOST_TEST
+
+#include <RobotAPI/Test.h>
+#include <RobotAPI/components/AronTest/AronTest.h>
+
+#include <iostream>
+
+BOOST_AUTO_TEST_CASE(testExample)
{
+ armarx::AronTest instance;
+ BOOST_CHECK_EQUAL(true, true);
}
diff --git a/source/RobotAPI/components/AronTest/test/CMakeLists.txt b/source/RobotAPI/components/AronTest/test/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..3ffa5732dcade5cf542a5789a1c0540408c4f9f8
--- /dev/null
+++ b/source/RobotAPI/components/AronTest/test/CMakeLists.txt
@@ -0,0 +1,5 @@
+
+# Libs required for the tests
+SET(LIBS ${LIBS} ArmarXCore AronTest)
+
+armarx_add_test(AronTestTest AronTestTest.cpp "${LIBS}")
diff --git a/source/RobotAPI/components/CMakeLists.txt b/source/RobotAPI/components/CMakeLists.txt
index 3a49ac17f1df2c6b42e78b974b8589870e1edf12..fb382b82c1c1cf054a24da77b2e2cf5a22540bb4 100644
--- a/source/RobotAPI/components/CMakeLists.txt
+++ b/source/RobotAPI/components/CMakeLists.txt
@@ -24,3 +24,6 @@ add_subdirectory(TopicTimingTest)
add_subdirectory(ViewSelection)
add_subdirectory(ArMemGlobalStorage)
+
+
+add_subdirectory(ArMemLocalStorage)
\ No newline at end of file
diff --git a/source/RobotAPI/components/CyberGloveObserver/CMakeLists.txt b/source/RobotAPI/components/CyberGloveObserver/CMakeLists.txt
index 10645a37cad068afe0c6c1bf44d85f8081521118..1027e5985e7eca3522e6a629aa274fed35644355 100644
--- a/source/RobotAPI/components/CyberGloveObserver/CMakeLists.txt
+++ b/source/RobotAPI/components/CyberGloveObserver/CMakeLists.txt
@@ -3,3 +3,5 @@ set(COMPONENT_LIBS ArmarXCore RobotAPICore ArmarXCoreObservers)
set(SOURCES CyberGloveObserver.cpp)
set(HEADERS CyberGloveObserver.h)
armarx_add_component("${SOURCES}" "${HEADERS}")
+
+armarx_generate_and_add_component_executable(APPLICATION_APP_SUFFIX)
diff --git a/source/RobotAPI/components/DummyTextToSpeech/CMakeLists.txt b/source/RobotAPI/components/DummyTextToSpeech/CMakeLists.txt
index b8f01d7865729cd8fa6194efc10169d153efd1d4..06e452e555081286433dd9fefe4d158204e7e55e 100644
--- a/source/RobotAPI/components/DummyTextToSpeech/CMakeLists.txt
+++ b/source/RobotAPI/components/DummyTextToSpeech/CMakeLists.txt
@@ -6,3 +6,5 @@ armarx_add_component("${SOURCES}" "${HEADERS}")
# add unit tests
add_subdirectory(test)
+
+armarx_generate_and_add_component_executable(APPLICATION_APP_SUFFIX)
diff --git a/source/RobotAPI/components/DynamicObstacleManager/CMakeLists.txt b/source/RobotAPI/components/DynamicObstacleManager/CMakeLists.txt
index d32b8830630d33ae0a1666477e77e62297506c0a..7136f05db5538c47faa2ed7d875a69166172e6db 100644
--- a/source/RobotAPI/components/DynamicObstacleManager/CMakeLists.txt
+++ b/source/RobotAPI/components/DynamicObstacleManager/CMakeLists.txt
@@ -41,13 +41,5 @@ set(HEADERS
armarx_add_component("${SOURCES}" "${HEADERS}")
-#find_package(MyLib QUIET)
-#armarx_build_if(MyLib_FOUND "MyLib not available")
-# all target_include_directories must be guarded by if(Xyz_FOUND)
-# for multiple libraries write: if(X_FOUND AND Y_FOUND)....
-#if(MyLib_FOUND)
-# target_include_directories(DynamicObstacleManager PUBLIC ${MyLib_INCLUDE_DIRS})
-#endif()
-
-# add unit tests
-# add_subdirectory(test)
+armarx_generate_and_add_component_executable()
+
diff --git a/source/RobotAPI/components/FrameTracking/CMakeLists.txt b/source/RobotAPI/components/FrameTracking/CMakeLists.txt
index 54f4372df34157e13fb91d07b87d6806ec7aaadc..ee7ca06b7df533103d908e0fda41b422f317a08a 100644
--- a/source/RobotAPI/components/FrameTracking/CMakeLists.txt
+++ b/source/RobotAPI/components/FrameTracking/CMakeLists.txt
@@ -2,24 +2,12 @@ armarx_component_set_name("FrameTracking")
set(COMPONENT_LIBS ArmarXCore RobotAPIInterfaces RobotAPICore RemoteGui ArmarXCoreObservers ArmarXGuiInterfaces RobotUnit)
-set(SOURCES
-./FrameTracking.cpp
-#@TEMPLATE_LINE@@COMPONENT_PATH@/@COMPONENT_NAME@.cpp
-)
-set(HEADERS
-./FrameTracking.h
-#@TEMPLATE_LINE@@COMPONENT_PATH@/@COMPONENT_NAME@.h
-)
+set(SOURCES FrameTracking.cpp)
+set(HEADERS FrameTracking.h)
armarx_add_component("${SOURCES}" "${HEADERS}")
-#find_package(MyLib QUIET)
-#armarx_build_if(MyLib_FOUND "MyLib not available")
-# all target_include_directories must be guarded by if(Xyz_FOUND)
-# for multiple libraries write: if(X_FOUND AND Y_FOUND)....
-#if(MyLib_FOUND)
-# target_include_directories(FrameTracking PUBLIC ${MyLib_INCLUDE_DIRS})
-#endif()
-
# add unit tests
add_subdirectory(test)
+
+armarx_generate_and_add_component_executable(APPLICATION_APP_SUFFIX)
diff --git a/source/RobotAPI/components/GamepadControlUnit/CMakeLists.txt b/source/RobotAPI/components/GamepadControlUnit/CMakeLists.txt
index 54a4ca048ab3933541b5bbccf11060da7824b6b4..c3b764a153b15400568f67eb1dc9c2813874b921 100644
--- a/source/RobotAPI/components/GamepadControlUnit/CMakeLists.txt
+++ b/source/RobotAPI/components/GamepadControlUnit/CMakeLists.txt
@@ -9,3 +9,5 @@ armarx_add_component("${SOURCES}" "${HEADERS}")
# add unit tests
add_subdirectory(test)
+
+armarx_generate_and_add_component_executable(APPLICATION_APP_SUFFIX)
diff --git a/source/RobotAPI/components/KITHandUnit/CMakeLists.txt b/source/RobotAPI/components/KITHandUnit/CMakeLists.txt
index 31d10218cce049319ec9bd4e72d60f0cca6cbca7..b272a6b26d6e4b35b4d8d618c0c59b8bc82459ab 100644
--- a/source/RobotAPI/components/KITHandUnit/CMakeLists.txt
+++ b/source/RobotAPI/components/KITHandUnit/CMakeLists.txt
@@ -17,3 +17,5 @@ armarx_add_component("${SOURCES}" "${HEADERS}")
# add unit tests
add_subdirectory(test)
+
+armarx_generate_and_add_component_executable(APPLICATION_APP_SUFFIX)
diff --git a/source/RobotAPI/components/KITProstheticHandUnit/CMakeLists.txt b/source/RobotAPI/components/KITProstheticHandUnit/CMakeLists.txt
index 67b35c294a4869b067e4a010583439acf455ad2f..92bc44f64073b1a26ceb15782207265f7bbdcad0 100644
--- a/source/RobotAPI/components/KITProstheticHandUnit/CMakeLists.txt
+++ b/source/RobotAPI/components/KITProstheticHandUnit/CMakeLists.txt
@@ -4,21 +4,16 @@ find_package(Qt5 COMPONENTS Core Bluetooth QUIET)
armarx_build_if(Qt5_FOUND "Qt5 Core or Bluetooth not available")
set(COMPONENT_LIBS
- ArmarXCoreInterfaces
- ArmarXCore
-
RobotAPIUnits
KITProstheticHandDriver
)
-set(SOURCES
- KITProstheticHandUnit.cpp
-)
-set(HEADERS
- KITProstheticHandUnit.h
-)
+set(SOURCES KITProstheticHandUnit.cpp)
+set(HEADERS KITProstheticHandUnit.h)
armarx_add_component("${SOURCES}" "${HEADERS}")
# add unit tests
add_subdirectory(test)
+
+armarx_generate_and_add_component_executable(APPLICATION_APP_SUFFIX)
diff --git a/source/RobotAPI/components/MultiHandUnit/CMakeLists.txt b/source/RobotAPI/components/MultiHandUnit/CMakeLists.txt
index a7f90694cf5d2d6f3b75a9a1b446e6c6c690a6e2..1118d00fce2d52dfbc3ae02d00fb3e0bf121c1ee 100644
--- a/source/RobotAPI/components/MultiHandUnit/CMakeLists.txt
+++ b/source/RobotAPI/components/MultiHandUnit/CMakeLists.txt
@@ -1,9 +1,6 @@
armarx_component_set_name("MultiHandUnit")
-set(COMPONENT_LIBS
- ArmarXCore
- RobotAPICore
-)
+set(COMPONENT_LIBS RobotAPICore)
set(SOURCES MultiHandUnit.cpp)
set(HEADERS MultiHandUnit.h)
@@ -12,3 +9,5 @@ armarx_add_component("${SOURCES}" "${HEADERS}")
# add unit tests
add_subdirectory(test)
+
+armarx_generate_and_add_component_executable(APPLICATION_APP_SUFFIX)
diff --git a/source/RobotAPI/components/NaturalIKTest/CMakeLists.txt b/source/RobotAPI/components/NaturalIKTest/CMakeLists.txt
index 8f4db4a898236c85436645ae747cc281d701011f..dcf81778c35b15b22b99fdcde688e18d7f5af801 100644
--- a/source/RobotAPI/components/NaturalIKTest/CMakeLists.txt
+++ b/source/RobotAPI/components/NaturalIKTest/CMakeLists.txt
@@ -1,6 +1,5 @@
armarx_component_set_name("NaturalIKTest")
-
set(COMPONENT_LIBS
ArmarXCore
ArmarXCoreInterfaces # for DebugObserverInterface
@@ -11,25 +10,12 @@ set(COMPONENT_LIBS
diffik
)
-set(SOURCES
- ./NaturalIKTest.cpp
-#@TEMPLATE_LINE@@COMPONENT_PATH@/@COMPONENT_NAME@.cpp
-)
-set(HEADERS
- ./NaturalIKTest.h
-#@TEMPLATE_LINE@@COMPONENT_PATH@/@COMPONENT_NAME@.h
-)
-
+set(SOURCES NaturalIKTest.cpp)
+set(HEADERS NaturalIKTest.h)
armarx_add_component("${SOURCES}" "${HEADERS}")
-#find_package(MyLib QUIET)
-#armarx_build_if(MyLib_FOUND "MyLib not available")
-# all target_include_directories must be guarded by if(Xyz_FOUND)
-# for multiple libraries write: if(X_FOUND AND Y_FOUND)....
-#if(MyLib_FOUND)
-# target_include_directories(NaturalIKTest PUBLIC ${MyLib_INCLUDE_DIRS})
-#endif()
-
# add unit tests
add_subdirectory(test)
+
+armarx_generate_and_add_component_executable(COMPONENT_NAME NaturalIKTest APPLICATION_APP_SUFFIX)
diff --git a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp
index 71a18f2ff13d9d00e2e8314bd499714f3203e541..8f24af53ed2dca19b6c8b273da70842bbc26dc48 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp
+++ b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp
@@ -71,7 +71,12 @@ namespace armarx
void ObjectPoseObserver::onConnectObserver()
{
- this->robot = RobotState::addRobot("robot", VirtualRobot::RobotIO::RobotDescription::eStructure);
+ // onConnect can be called multiple times, but addRobot will fail if called more than once with the same ID
+ // So we need to always make sure to guard a call to addRobot
+ if (!RobotState::hasRobot("robot"))
+ {
+ robot = RobotState::addRobot("robot", VirtualRobot::RobotIO::RobotDescription::eStructure);
+ }
createRemoteGuiTab();
RemoteGui_startRunningTask();
@@ -119,6 +124,9 @@ namespace armarx
void ObjectPoseObserver::RemoteGui_update()
{
+ // Non-atomic variables need to be guarded by a mutex if accessed by multiple threads
+ std::scoped_lock lock(dataMutex);
+
visu.enabled = tab.visuEnabled.getValue();
visu.inGlobalFrame = tab.visuInGlobalFrame.getValue();
visu.alpha = tab.visuAlpha.getValue();
@@ -143,7 +151,7 @@ namespace armarx
objpose::ObjectPoseSeq objectPoses;
{
std::scoped_lock lock(dataMutex);
- RobotState::synchronizeLocalClone("robot");
+ RobotState::synchronizeLocalClone(robot);
if (robot->hasRobotNode(calibration.robotNode))
{
diff --git a/source/RobotAPI/components/RobotHealth/CMakeLists.txt b/source/RobotAPI/components/RobotHealth/CMakeLists.txt
index 7de0817d36ac6d60f2ade305b9e5c4183ab8b2aa..065edb26d7c32cf48a6cdacfaeabd2baacb12d25 100644
--- a/source/RobotAPI/components/RobotHealth/CMakeLists.txt
+++ b/source/RobotAPI/components/RobotHealth/CMakeLists.txt
@@ -13,3 +13,8 @@ armarx_find_qt_base("QtCore;QtGui;QtOpenGL;QtXml;QtScript;QtDesigner" REQUIRED)
armarx_find_qt(Script)
armarx_add_component("${SOURCES}" "${HEADERS}")
+
+armarx_generate_and_add_component_executable(APPLICATION_APP_SUFFIX)
+armarx_generate_and_add_component_executable(
+ COMPONENT_NAME RobotHealthDummy
+ APPLICATION_APP_SUFFIX)
diff --git a/source/RobotAPI/components/RobotNameService/CMakeLists.txt b/source/RobotAPI/components/RobotNameService/CMakeLists.txt
index 3a7f08440261f548035574caaff233aabde083e8..1205ad25b0f143b5ca6af3719b44d4b3b6e07117 100644
--- a/source/RobotAPI/components/RobotNameService/CMakeLists.txt
+++ b/source/RobotAPI/components/RobotNameService/CMakeLists.txt
@@ -6,3 +6,5 @@ armarx_add_component("${SOURCES}" "${HEADERS}")
# add unit tests
add_subdirectory(test)
+
+armarx_generate_and_add_component_executable(APPLICATION_APP_SUFFIX)
diff --git a/source/RobotAPI/components/SimpleEpisodicMemoryKinematicUnitConnector/CMakeLists.txt b/source/RobotAPI/components/SimpleEpisodicMemoryKinematicUnitConnector/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..f51802b6fb746ec770a1fd994d03b111d8e2324a
--- /dev/null
+++ b/source/RobotAPI/components/SimpleEpisodicMemoryKinematicUnitConnector/CMakeLists.txt
@@ -0,0 +1,31 @@
+armarx_component_set_name("SimpleEpisodicMemoryKinematicUnitConnector")
+
+
+set(COMPONENT_LIBS
+ ArmarXCore
+ ArmarXCoreInterfaces
+
+)
+
+set(SOURCES
+ ./SimpleEpisodicMemoryKinematicUnitConnector.cpp
+#@TEMPLATE_LINE@@COMPONENT_PATH@/@COMPONENT_NAME@.cpp
+)
+set(HEADERS
+ ./SimpleEpisodicMemoryKinematicUnitConnector.h
+#@TEMPLATE_LINE@@COMPONENT_PATH@/@COMPONENT_NAME@.h
+)
+
+
+armarx_add_component("${SOURCES}" "${HEADERS}")
+
+#find_package(MyLib QUIET)
+#armarx_build_if(MyLib_FOUND "MyLib not available")
+# all target_include_directories must be guarded by if(Xyz_FOUND)
+# for multiple libraries write: if(X_FOUND AND Y_FOUND)....
+#if(MyLib_FOUND)
+# target_include_directories(SimpleEpisodicMemoryKinematicUnitConnector PUBLIC ${MyLib_INCLUDE_DIRS})
+#endif()
+
+# add unit tests
+add_subdirectory(test)
diff --git a/source/RobotAPI/components/SimpleEpisodicMemoryKinematicUnitConnector/SimpleEpisodicMemoryKinematicUnitConnector.cpp b/source/RobotAPI/components/SimpleEpisodicMemoryKinematicUnitConnector/SimpleEpisodicMemoryKinematicUnitConnector.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..da712b4aba69056b7ef5dd1f3b9e4aa7663e0e88
--- /dev/null
+++ b/source/RobotAPI/components/SimpleEpisodicMemoryKinematicUnitConnector/SimpleEpisodicMemoryKinematicUnitConnector.cpp
@@ -0,0 +1,69 @@
+/*
+ * 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::ArmarXObjects::SimpleEpisodicMemoryKinematicUnitConnector
+ * @author Fabian PK ( fabian dot peller-konrad at kit dot edu )
+ * @date 2020
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "SimpleEpisodicMemoryKinematicUnitConnector.h"
+
+
+namespace armarx
+{
+
+ std::string SimpleEpisodicMemoryKinematicUnitConnector::getDefaultName() const
+ {
+ return "SimpleEpisodicMemoryKinematicUnitConnector";
+ }
+
+ SimpleEpisodicMemoryKinematicUnitConnector::SimpleEpisodicMemoryKinematicUnitConnector()
+ {}
+
+
+ void SimpleEpisodicMemoryKinematicUnitConnector::onInitComponent()
+ {
+
+ }
+
+
+ void SimpleEpisodicMemoryKinematicUnitConnector::onConnectComponent()
+ {
+
+ }
+
+
+ void SimpleEpisodicMemoryKinematicUnitConnector::onDisconnectComponent()
+ {
+
+ }
+
+
+ void SimpleEpisodicMemoryKinematicUnitConnector::onExitComponent()
+ {
+
+ }
+
+
+
+
+ armarx::PropertyDefinitionsPtr SimpleEpisodicMemoryKinematicUnitConnector::createPropertyDefinitions()
+ {
+ //armarx::PropertyDefinitionsPtr defs = new SimpleEpisodicMemoryKinematicUnitConnectorPropertyDefinitions(getConfigIdentifier());
+ return nullptr;
+ }
+}
diff --git a/source/RobotAPI/components/SimpleEpisodicMemoryKinematicUnitConnector/SimpleEpisodicMemoryKinematicUnitConnector.h b/source/RobotAPI/components/SimpleEpisodicMemoryKinematicUnitConnector/SimpleEpisodicMemoryKinematicUnitConnector.h
new file mode 100644
index 0000000000000000000000000000000000000000..7d9896ffaf1f7578de86d25d64e75c66af2f0297
--- /dev/null
+++ b/source/RobotAPI/components/SimpleEpisodicMemoryKinematicUnitConnector/SimpleEpisodicMemoryKinematicUnitConnector.h
@@ -0,0 +1,59 @@
+/*
+ * 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::ArmarXObjects::SimpleEpisodicMemoryKinematicUnitConnector
+ * @author Fabian PK ( fabian dot peller-konrad at kit dot edu )
+ * @date 2020
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+
+#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
+#include <MemoryX/components/SimpleEpisodicMemory/SimpleEpisodicMemoryConnector.h>
+
+
+namespace armarx
+{
+
+ class SimpleEpisodicMemoryKinematicUnitConnector :
+ public memoryx::SimpleEpisodicMemoryConnector,
+ virtual public armarx::Component
+
+ {
+ public:
+
+ /// @see armarx::ManagedIceObject::getDefaultName()
+ std::string getDefaultName() const override;
+
+ SimpleEpisodicMemoryKinematicUnitConnector();
+
+ protected:
+ void onInitComponent() override;
+ void onConnectComponent() override;
+ void onDisconnectComponent() override;
+ void onExitComponent() override;
+
+ armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
+
+
+ private:
+
+
+ };
+}
diff --git a/source/RobotAPI/components/SimpleEpisodicMemoryKinematicUnitConnector/test/CMakeLists.txt b/source/RobotAPI/components/SimpleEpisodicMemoryKinematicUnitConnector/test/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..39691a0810ed53203efe1749605d57736fadb8a3
--- /dev/null
+++ b/source/RobotAPI/components/SimpleEpisodicMemoryKinematicUnitConnector/test/CMakeLists.txt
@@ -0,0 +1,5 @@
+
+# Libs required for the tests
+SET(LIBS ${LIBS} ArmarXCore SimpleEpisodicMemoryKinematicUnitConnector)
+
+armarx_add_test(SimpleEpisodicMemoryKinematicUnitConnectorTest SimpleEpisodicMemoryKinematicUnitConnectorTest.cpp "${LIBS}")
diff --git a/source/RobotAPI/applications/DynamicObstacleManager/main.cpp b/source/RobotAPI/components/SimpleEpisodicMemoryKinematicUnitConnector/test/SimpleEpisodicMemoryKinematicUnitConnectorTest.cpp
similarity index 56%
rename from source/RobotAPI/applications/DynamicObstacleManager/main.cpp
rename to source/RobotAPI/components/SimpleEpisodicMemoryKinematicUnitConnector/test/SimpleEpisodicMemoryKinematicUnitConnectorTest.cpp
index 2b115f14589702625322d2b461a5adf38aa98643..46d89be8cac117abc4231a5e7b2cdfa5675f772a 100644
--- a/source/RobotAPI/applications/DynamicObstacleManager/main.cpp
+++ b/source/RobotAPI/components/SimpleEpisodicMemoryKinematicUnitConnector/test/SimpleEpisodicMemoryKinematicUnitConnectorTest.cpp
@@ -13,28 +13,25 @@
* 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 Armar6Skills::ArmarXObjects::DynamicObstacleManager
- * @author Fabian Peller-Konrad <fabian.peller-konrad@kit.edu>
+ * @package RobotAPI::ArmarXObjects::SimpleEpisodicMemoryKinematicUnitConnector
+ * @author Fabian PK ( fabian dot peller-konrad at kit dot edu )
* @date 2020
* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
* GNU General Public License
*/
+#define BOOST_TEST_MODULE RobotAPI::ArmarXObjects::SimpleEpisodicMemoryKinematicUnitConnector
-// STD/STL
-#include <string>
+#define ARMARX_BOOST_TEST
-// ArmarX
-#include <ArmarXCore/core/application/Application.h>
-#include <ArmarXCore/core/Component.h>
+#include <RobotAPI/Test.h>
+#include <RobotAPI/components/SimpleEpisodicMemoryKinematicUnitConnector/SimpleEpisodicMemoryKinematicUnitConnector.h>
-// Component
-#include <RobotAPI/components/DynamicObstacleManager/DynamicObstacleManager.h>
+#include <iostream>
-using namespace armarx;
-
-int main(int argc, char* argv[])
+BOOST_AUTO_TEST_CASE(testExample)
{
- const std::string name = DynamicObstacleManager::default_name;
- return runSimpleComponentApp<DynamicObstacleManager>(argc, argv, name);
+ armarx::SimpleEpisodicMemoryKinematicUnitConnector instance;
+
+ BOOST_CHECK_EQUAL(true, true);
}
diff --git a/source/RobotAPI/components/StatechartExecutorExample/CMakeLists.txt b/source/RobotAPI/components/StatechartExecutorExample/CMakeLists.txt
index a557d941fde6d13627ab5d3eaf7957d6424e6998..46da61e1d6d8f17246567cb2a46546eb2f27e58c 100644
--- a/source/RobotAPI/components/StatechartExecutorExample/CMakeLists.txt
+++ b/source/RobotAPI/components/StatechartExecutorExample/CMakeLists.txt
@@ -1,6 +1,5 @@
armarx_component_set_name("StatechartExecutorExample")
-
set(COMPONENT_LIBS
ArmarXCore
ArmarXCoreInterfaces
@@ -9,13 +8,9 @@ set(COMPONENT_LIBS
ArmarXCoreStatechart
)
-set(SOURCES
- ./StatechartExecutorExample.cpp
-)
-set(HEADERS
- ./StatechartExecutorExample.h
-)
-
+set(SOURCES StatechartExecutorExample.cpp)
+set(HEADERS StatechartExecutorExample.h)
armarx_add_component("${SOURCES}" "${HEADERS}")
+armarx_generate_and_add_component_executable(APPLICATION_APP_SUFFIX)
diff --git a/source/RobotAPI/components/StatechartExecutorExample/StatechartExecutorExample.cpp b/source/RobotAPI/components/StatechartExecutorExample/StatechartExecutorExample.cpp
index 775c54a9b4450cd9ea886a42884c1c742d486435..48ee7e41be1f4485ca27eb992a8e4046bbd4b375 100644
--- a/source/RobotAPI/components/StatechartExecutorExample/StatechartExecutorExample.cpp
+++ b/source/RobotAPI/components/StatechartExecutorExample/StatechartExecutorExample.cpp
@@ -68,9 +68,9 @@ namespace armarx
void StatechartExecutorExample::onConnectComponent()
{
- assignProxy(_statechartExecutor, "SimpleStatechartExecutor");
+ getProxy(_statechartExecutor, "SimpleStatechartExecutor");
- assignProxy(_remoteGuiPrx, "RemoteGuiProvider");
+ getProxy(_remoteGuiPrx, "RemoteGuiProvider");
_tabName = "StatechartExecutorExample";
setupRemoteGuiWidget();
_remoteGuiTab = RemoteGui::TabProxy(_remoteGuiPrx, _tabName);
diff --git a/source/RobotAPI/components/ViewSelection/CMakeLists.txt b/source/RobotAPI/components/ViewSelection/CMakeLists.txt
index ecad234c2f621485b413044d1231a60aa725ece9..9fece3cd9ce2faf6ad37df1ce5cfa0a9fbba7862 100644
--- a/source/RobotAPI/components/ViewSelection/CMakeLists.txt
+++ b/source/RobotAPI/components/ViewSelection/CMakeLists.txt
@@ -10,3 +10,4 @@ set(HEADERS ViewSelection.h)
armarx_add_component("${SOURCES}" "${HEADERS}")
+armarx_generate_and_add_component_executable(APPLICATION_APP_SUFFIX)
diff --git a/source/RobotAPI/components/units/CMakeLists.txt b/source/RobotAPI/components/units/CMakeLists.txt
index 430741e730cde5b4751d15963039f180fe57e340..51d1930b83753c195ff7558154092ea025466e4c 100644
--- a/source/RobotAPI/components/units/CMakeLists.txt
+++ b/source/RobotAPI/components/units/CMakeLists.txt
@@ -77,3 +77,21 @@ armarx_add_library("${LIB_NAME}" "${LIB_FILES}" "${LIB_HEADERS}" "${LIBS}")
add_subdirectory(ObstacleAvoidingPlatformUnit)
add_subdirectory(relays)
add_subdirectory(RobotUnit)
+
+armarx_generate_and_add_component_executable(COMPONENT_NAME ForceTorqueObserver )
+armarx_generate_and_add_component_executable(COMPONENT_NAME ForceTorqueUnitSimulation )
+armarx_generate_and_add_component_executable(COMPONENT_NAME GamepadUnitObserver APPLICATION_APP_SUFFIX)
+armarx_generate_and_add_component_executable(COMPONENT_NAME GraspCandidateObserver APPLICATION_APP_SUFFIX)
+armarx_generate_and_add_component_executable(COMPONENT_NAME HandUnitSimulation )
+armarx_generate_and_add_component_executable(COMPONENT_NAME HapticObserver APPLICATION_APP_SUFFIX)
+armarx_generate_and_add_component_executable(COMPONENT_NAME HeadIKUnit )
+armarx_generate_and_add_component_executable(COMPONENT_NAME InertialMeasurementUnitObserver APPLICATION_APP_SUFFIX)
+armarx_generate_and_add_component_executable(COMPONENT_NAME KinematicUnitObserver )
+armarx_generate_and_add_component_executable(COMPONENT_NAME KinematicUnitSimulation )
+armarx_generate_and_add_component_executable(COMPONENT_NAME LaserScannerUnitObserver APPLICATION_APP_SUFFIX)
+armarx_generate_and_add_component_executable(COMPONENT_NAME MetaWearIMUObserver APPLICATION_APP_SUFFIX)
+armarx_generate_and_add_component_executable(COMPONENT_NAME SpeechObserver APPLICATION_APP_SUFFIX)
+armarx_generate_and_add_component_executable(COMPONENT_NAME PlatformUnitSimulation )
+armarx_generate_and_add_component_executable(COMPONENT_NAME PlatformUnitObserver )
+armarx_generate_and_add_component_executable(COMPONENT_NAME OrientedTactileSensorUnitObserver APPLICATION_APP_SUFFIX)
+armarx_generate_and_add_component_executable(COMPONENT_NAME OptoForceUnitObserver APPLICATION_APP_SUFFIX)
diff --git a/source/RobotAPI/components/units/RobotUnit/ControlTargets/ControlTargetBase.h b/source/RobotAPI/components/units/RobotUnit/ControlTargets/ControlTargetBase.h
index 8b69a5d565d944c415be16a0815f26b9d5c1af07..c4e6a36af757d2d020eb6ebb0fdb1a8a2310f5f2 100644
--- a/source/RobotAPI/components/units/RobotUnit/ControlTargets/ControlTargetBase.h
+++ b/source/RobotAPI/components/units/RobotUnit/ControlTargets/ControlTargetBase.h
@@ -80,7 +80,15 @@ namespace armarx
virtual std::size_t getNumberOfDataFields() const = 0;
virtual std::vector<std::string> getDataFieldNames() const = 0;
- virtual std::string getDataFieldAsString(std::size_t i) const = 0;
+ virtual void getDataFieldAs(std::size_t i, bool& out) const = 0;
+ virtual void getDataFieldAs(std::size_t i, Ice::Byte& out) const = 0;
+ virtual void getDataFieldAs(std::size_t i, Ice::Short& out) const = 0;
+ virtual void getDataFieldAs(std::size_t i, Ice::Int& out) const = 0;
+ virtual void getDataFieldAs(std::size_t i, Ice::Long& out) const = 0;
+ virtual void getDataFieldAs(std::size_t i, Ice::Float& out) const = 0;
+ virtual void getDataFieldAs(std::size_t i, Ice::Double& out) const = 0;
+ virtual void getDataFieldAs(std::size_t i, std::string& out) const = 0;
+ virtual const std::type_info& getDataFieldType(std::size_t i) const = 0;
//management functions
template<class T, class = typename std::enable_if<std::is_base_of<ControlTargetBase, T>::value>::type>
@@ -118,9 +126,41 @@ namespace armarx
{ \
return ControlTargetInfo<std::decay<decltype(*this)>::type>::GetNumberOfDataFields(); \
} \
- std::string getDataFieldAsString(std::size_t i) const override \
+ void getDataFieldAs (std::size_t i, bool& out) const override \
{ \
- return ControlTargetInfo<std::decay<decltype(*this)>::type>::GetDataFieldAsString(this, i); \
+ ControlTargetInfo<std::decay<decltype(*this)>::type>::GetDataFieldAs (this, i, out); \
+ } \
+ void getDataFieldAs (std::size_t i, Ice::Byte& out) const override \
+ { \
+ ControlTargetInfo<std::decay<decltype(*this)>::type>::GetDataFieldAs (this, i, out); \
+ } \
+ void getDataFieldAs (std::size_t i, Ice::Short& out) const override \
+ { \
+ ControlTargetInfo<std::decay<decltype(*this)>::type>::GetDataFieldAs (this, i, out); \
+ } \
+ void getDataFieldAs (std::size_t i, Ice::Int& out) const override \
+ { \
+ ControlTargetInfo<std::decay<decltype(*this)>::type>::GetDataFieldAs (this, i, out); \
+ } \
+ void getDataFieldAs (std::size_t i, Ice::Long& out) const override \
+ { \
+ ControlTargetInfo<std::decay<decltype(*this)>::type>::GetDataFieldAs (this, i, out); \
+ } \
+ void getDataFieldAs (std::size_t i, Ice::Float& out) const override \
+ { \
+ ControlTargetInfo<std::decay<decltype(*this)>::type>::GetDataFieldAs (this, i, out); \
+ } \
+ void getDataFieldAs (std::size_t i, Ice::Double& out) const override \
+ { \
+ ControlTargetInfo<std::decay<decltype(*this)>::type>::GetDataFieldAs (this, i, out); \
+ } \
+ void getDataFieldAs (std::size_t i, std::string& out) const override \
+ { \
+ ControlTargetInfo<std::decay<decltype(*this)>::type>::GetDataFieldAs (this, i, out); \
+ } \
+ const std::type_info& getDataFieldType(std::size_t i) const override \
+ { \
+ return ControlTargetInfo<std::decay<decltype(*this)>::type>::GetDataFieldType(i); \
} \
std::vector<std::string> getDataFieldNames() const override \
{ \
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.cpp
index 1f58f79ae6bb6be402f4b22b45bc5e9ebb06ae7a..0994896f9a3510a8c4ce0241d981d0dad2bf03cc 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.cpp
@@ -20,6 +20,12 @@
* GNU General Public License
*/
+#include <regex>
+
+#include <boost/iterator/transform_iterator.hpp>
+
+#include <ArmarXCore/util/CPPUtility/trace.h>
+#include <ArmarXCore/util/CPPUtility/Iterator.h>
#include <ArmarXCore/core/util/FileSystemPathBuilder.h>
#include <ArmarXCore/core/ArmarXManager.h>
@@ -36,6 +42,7 @@ namespace armarx::RobotUnitModule
{
void Logging::addMarkerToRtLog(const RemoteReferenceCounterBasePtr& token, const std::string& marker, const Ice::Current&)
{
+ ARMARX_TRACE;
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
std::lock_guard<std::mutex> guard {rtLoggingMutex};
if (!rtLoggingEntries.count(token->getId()))
@@ -47,6 +54,7 @@ namespace armarx::RobotUnitModule
RemoteReferenceCounterBasePtr Logging::startRtLogging(const std::string& formatString, const Ice::StringSeq& loggingNames, const Ice::Current&)
{
+ ARMARX_TRACE;
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
StringStringDictionary alias;
for (const auto& name : loggingNames)
@@ -58,11 +66,12 @@ namespace armarx::RobotUnitModule
void Logging::stopRtLogging(const RemoteReferenceCounterBasePtr& token, const Ice::Current&)
{
+ ARMARX_TRACE;
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
std::lock_guard<std::mutex> guard {rtLoggingMutex};
if (!rtLoggingEntries.count(token->getId()))
{
- throw InvalidArgumentException {"addMarkerToRtLog called for a nonexistent log"};
+ throw InvalidArgumentException {"stopRtLogging called for a nonexistent log"};
}
ARMARX_DEBUG_S << "RobotUnit: stop RtLogging for file " << rtLoggingEntries.at(token->getId())->filename;
rtLoggingEntries.at(token->getId())->stopLogging = true;
@@ -70,24 +79,39 @@ namespace armarx::RobotUnitModule
Ice::StringSeq Logging::getLoggingNames(const Ice::Current&) const
{
+ ARMARX_TRACE;
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
Ice::StringSeq result;
- for (const auto& strs : sensorDeviceValueLoggingNames)
+ const auto getName = [](const auto & fieldData)
{
- result.insert(result.end(), strs.begin(), strs.end());
+ return fieldData.name;
+ };
+ for (const auto& data : sensorDeviceValueMetaData)
+ {
+ result.insert(
+ result.end(),
+ boost::make_transform_iterator(data.fields.begin(), getName),
+ boost::make_transform_iterator(data.fields.end(), getName));
}
- for (const auto& strvecss : controlDeviceValueLoggingNames)
+ for (const auto& datas : controlDeviceValueMetaData)
{
- for (const auto& strs : strvecss)
+ for (const auto& data : datas)
{
- result.insert(result.end(), strs.begin(), strs.end());
+ result.insert(
+ result.end(),
+ boost::make_transform_iterator(data.fields.begin(), getName),
+ boost::make_transform_iterator(data.fields.end(), getName));
}
}
return result;
}
- RemoteReferenceCounterBasePtr Logging::startRtLoggingWithAliasNames(const std::string& formatString, const StringStringDictionary& aliasNames, const Ice::Current&)
+ RemoteReferenceCounterBasePtr Logging::startRtLoggingWithAliasNames(
+ const std::string& formatString,
+ const StringStringDictionary& aliasNames,
+ const Ice::Current&)
{
+ ARMARX_TRACE;
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
FileSystemPathBuilder pb {formatString};
std::lock_guard<std::mutex> guard {rtLoggingMutex};
@@ -106,22 +130,19 @@ namespace armarx::RobotUnitModule
rtLoggingEntries.erase(pb.getPath());
throw LogicError {"RtLogging could not open filestream for '" + pb.getPath() + "'"};
}
+ ARMARX_INFO << "Start logging to " << e.filename
+ << ". Names (pattern, replacement name): " << aliasNames;
std::stringstream header;
- header << "marker;iteration;timestamp";
+ header << "marker;iteration;timestamp;TimeSinceLastIteration";
auto logDev = [&](const std::string & dev)
{
- for (const auto& pair : aliasNames)
+ ARMARX_TRACE_LITE;
+ for (const auto& [key, value] : aliasNames)
{
- std::string name = pair.first;
- if (boost::starts_with(dev + "$", name))
+ if (MatchName(key, dev))
{
- if ((name.size() > 0 && name.back() == '$'))
- {
- name.resize(name.size() - 1);
- }
- const std::string& alias = pair.second.empty() ? name : pair.second;
- header << ";" << alias << dev.substr(name.size());
+ header << ";" << (value.empty() ? dev : value);
return true;
}
}
@@ -130,40 +151,42 @@ namespace armarx::RobotUnitModule
//get logged sensor device values
{
- e.loggedSensorDeviceValues.reserve(sensorDeviceValueLoggingNames.size());
- for (const auto& svfields : sensorDeviceValueLoggingNames)
+ ARMARX_TRACE;
+ e.loggedSensorDeviceValues.reserve(sensorDeviceValueMetaData.size());
+ for (const auto& valData : sensorDeviceValueMetaData)
{
e.loggedSensorDeviceValues.emplace_back();
auto& svfieldsFlags = e.loggedSensorDeviceValues.back(); //vv
- svfieldsFlags.reserve(svfields.size());
- for (const auto& field : svfields)
+ svfieldsFlags.reserve(valData.fields.size());
+ for (const auto& field : valData.fields)
{
- svfieldsFlags.emplace_back(logDev(field));
+ svfieldsFlags.emplace_back(logDev(field.name));
}
}
}
//get logged control device values
{
- e.loggedControlDeviceValues.reserve(controlDeviceValueLoggingNames.size());
- for (const auto& sjctrls : controlDeviceValueLoggingNames)
+ ARMARX_TRACE;
+ e.loggedControlDeviceValues.reserve(controlDeviceValueMetaData.size());
+ for (const auto& datas : controlDeviceValueMetaData)
{
e.loggedControlDeviceValues.emplace_back();
- auto& sjctrlsFlags = e.loggedControlDeviceValues.back(); //vv
- sjctrlsFlags.reserve(sjctrls.size());
- for (const auto& ctargfields : sjctrls)
+ auto& deviceCtrlFlags = e.loggedControlDeviceValues.back(); //vv
+ deviceCtrlFlags.reserve(datas.size());
+ for (const auto& valData : datas)
{
+ deviceCtrlFlags.emplace_back();
+ auto& ctrlFieldFlags = deviceCtrlFlags.back(); //v
+ ctrlFieldFlags.reserve(valData.fields.size());
- sjctrlsFlags.emplace_back();
- auto& ctargfieldsFlags = sjctrlsFlags.back(); //v
- ctargfieldsFlags.reserve(ctargfields.size());
-
- for (const auto& field : ctargfields)
+ for (const auto& field : valData.fields)
{
- ctargfieldsFlags.emplace_back(logDev(field));
+ ctrlFieldFlags.emplace_back(logDev(field.name));
}
}
}
}
+ ARMARX_TRACE;
//write header
e.stream << header.str() << std::flush; // newline is written at the beginning of each log line
@@ -182,6 +205,7 @@ namespace armarx::RobotUnitModule
void Logging::writeRecentIterationsToFile(const std::string& formatString, const Ice::Current&) const
{
+ ARMARX_TRACE;
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
std::lock_guard<std::mutex> guard {rtLoggingMutex};
FileSystemPathBuilder pb {formatString};
@@ -200,20 +224,20 @@ namespace armarx::RobotUnitModule
//write csv header
{
outCSV << "iteration;timestamp";
- for (const auto& vs : sensorDeviceValueLoggingNames)
+ for (const auto& vs : sensorDeviceValueMetaData)
{
- for (const auto& s : vs)
+ for (const auto& f : vs.fields)
{
- outCSV << ";" << s;
+ outCSV << ";" << f.name;
}
}
- for (const auto& vvs : controlDeviceValueLoggingNames)
+ for (const auto& vvs : controlDeviceValueMetaData)
{
for (const auto& vs : vvs)
{
- for (const auto& s : vs)
+ for (const auto& f : vs.fields)
{
- outCSV << ";" << s;
+ outCSV << ";" << f.name;
}
}
}
@@ -231,7 +255,9 @@ namespace armarx::RobotUnitModule
{
for (std::size_t idxField = 0; idxField < val->getNumberOfDataFields(); ++ idxField)
{
- outCSV << ";" << val->getDataFieldAsString(idxField);
+ std::string s;
+ val->getDataFieldAs(idxField, s);
+ outCSV << ";" << s;
}
}
}
@@ -243,7 +269,9 @@ namespace armarx::RobotUnitModule
{
for (std::size_t idxField = 0; idxField < val->getNumberOfDataFields(); ++ idxField)
{
- outCSV << ";" << val->getDataFieldAsString(idxField);
+ std::string s;
+ val->getDataFieldAs(idxField, s);
+ outCSV << ";" << s;
}
}
}
@@ -275,8 +303,134 @@ namespace armarx::RobotUnitModule
}
}
+ RobotUnitDataStreaming::DataStreamingDescription Logging::startDataStreaming(
+ const RobotUnitDataStreaming::ReceiverPrx& receiver,
+ const RobotUnitDataStreaming::Config& config,
+ const Ice::Current&)
+ {
+ ARMARX_TRACE;
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ if (!receiver)
+ {
+ throw InvalidArgumentException {"Receiver proxy is NULL!"};
+ }
+ std::lock_guard<std::mutex> guard {rtLoggingMutex};
+ if (rtDataStreamingEntry.count(receiver))
+ {
+ throw InvalidArgumentException {"There already is a logger for the given receiver"};
+ }
+
+ RobotUnitDataStreaming::DataStreamingDescription result;
+ DataStreamingEntry& streamingEntry = rtDataStreamingEntry[receiver];
+
+ ARMARX_INFO << "start data streaming to " << receiver->ice_getIdentity().name
+ << ". Values: " << config.loggingNames;
+ auto devMatchesAnyKey = [&](const std::string & dev)
+ {
+ for (const auto& key : config.loggingNames)
+ {
+ if (MatchName(key, dev))
+ {
+ return true;
+ }
+ }
+ return false;
+ };
+
+ const auto handleVal = [&](
+ const ValueMetaData & valData,
+ DataStreamingEntry & streamingEntry,
+ RobotUnitDataStreaming::DataStreamingDescription & descr
+ ) -> std::vector<DataStreamingEntry::OutVal>
+ {
+ ARMARX_TRACE_LITE;
+ std::vector<DataStreamingEntry::OutVal> result;
+ result.resize(valData.fields.size());
+ for (std::size_t i = 0; i < valData.fields.size(); ++i)
+ {
+ if (!devMatchesAnyKey(valData.fields.at(i).name))
+ {
+ continue; //do not add to result and skipp during processing
+ }
+ auto& descrEntr = descr.entries[valData.fields.at(i).name];
+ //formatter failes here!
+ //*INDENT-OFF*
+ #define make_case(Type, TName) \
+ (typeid(Type) == *valData.fields.at(i).type) \
+ { \
+ descrEntr.index = streamingEntry.num##TName##s; \
+ descrEntr.type = RobotUnitDataStreaming::NodeType##TName; \
+ result.at(i).idx = streamingEntry.num##TName##s; \
+ result.at(i).value = DataStreamingEntry::ValueT::TName; \
+ ++streamingEntry.num##TName##s; \
+ }
+ if make_case(bool, Bool)
+ else if make_case(Ice::Byte, Byte)
+ else if make_case(Ice::Short, Short)
+ else if make_case(Ice::Int, Int)
+ else if make_case(Ice::Long, Long)
+ else if make_case(Ice::Float, Float)
+ else if make_case(Ice::Double, Double)
+ else
+ {
+ ARMARX_CHECK_EXPRESSION(false)
+ << "This code sould be unreachable! "
+ "The type of "
+ << valData.fields.at(i).name
+ << " is not handled correctly!";
+ }
+ #undef make_case
+ //*INDENT-ON*
+ }
+ return result;
+ };
+
+ //get logged sensor device values
+ {
+ ARMARX_TRACE;
+ streamingEntry.sensDevs.reserve(sensorDeviceValueMetaData.size());
+ for (const auto& valData : sensorDeviceValueMetaData)
+ {
+ streamingEntry.sensDevs.emplace_back(
+ handleVal(valData, streamingEntry, result));
+ }
+ }
+ //get logged control device values
+ {
+ ARMARX_TRACE;
+ streamingEntry.ctrlDevs.reserve(controlDeviceValueMetaData.size());
+ for (const auto& devData : controlDeviceValueMetaData)
+ {
+ streamingEntry.ctrlDevs.emplace_back();
+ auto& ctrlDevEntrs = streamingEntry.ctrlDevs.back();
+ ctrlDevEntrs.reserve(devData.size());
+ for (const auto& valData : devData)
+ {
+ ctrlDevEntrs.emplace_back(
+ handleVal(valData, streamingEntry, result));
+ }
+ }
+ }
+
+ return result;
+ }
+
+ void Logging::stopDataStreaming(const RobotUnitDataStreaming::ReceiverPrx& receiver, const Ice::Current&)
+ {
+ ARMARX_TRACE;
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ std::lock_guard<std::mutex> guard {rtLoggingMutex};
+ if (!rtDataStreamingEntry.count(receiver))
+ {
+ throw InvalidArgumentException {"stopDataStreaming called for a nonexistent log"};
+ }
+ ARMARX_INFO_S << "RobotUnit: request to stop DataStreaming for " << receiver->ice_id();
+ rtDataStreamingEntry.at(receiver).stopStreaming = true;
+ }
+
void Logging::_preFinishRunning()
{
+ ARMARX_TRACE;
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
defaultLogHandle = nullptr;
if (rtLoggingTask)
@@ -294,6 +448,7 @@ namespace armarx::RobotUnitModule
void Logging::_preFinishControlThreadInitialization()
{
+ ARMARX_TRACE;
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
controlThreadId = LogSender::getThreadId();
ControlThreadOutputBuffer::RtLoggingInstance = &(_module<ControlThreadDataBuffer>().getControlThreadOutputBuffer());
@@ -304,6 +459,7 @@ namespace armarx::RobotUnitModule
void Logging::doLogging()
{
+ ARMARX_TRACE;
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
std::lock_guard<std::mutex> guard {rtLoggingMutex};
const auto now = IceUtil::Time::now();
@@ -318,117 +474,203 @@ namespace armarx::RobotUnitModule
}
//log all
{
+ ARMARX_TRACE;
+ if (!rtLoggingEntries.empty() || !rtDataStreamingEntry.empty())
+ {
+ ARMARX_INFO << deactivateSpam()
+ << "Number of logs " << rtLoggingEntries.size() << '\n'
+ << "Number of streams " << rtDataStreamingEntry.size();
+ }
_module<ControlThreadDataBuffer>().getControlThreadOutputBuffer().foreachNewLoggingEntry(
- [&](const ControlThreadOutputBuffer::Entry & data)
+ [&](const auto & data, auto i, auto num)
+ {
+ doLogging(now, data, i, num);
+ }
+ );
+ }
+ ARMARX_DEBUG_S << ::deactivateSpam() << "the last " << backlog.size() << " iterations are stored";
+ //flush all files
+ {
+ for (auto& pair : rtLoggingEntries)
+ {
+ pair.second->stream << std::flush;
+ }
+ }
+
+ //remove entries
+ {
+ ARMARX_TRACE;
+ std::vector<std::string> toRemove;
+ toRemove.reserve(rtLoggingEntries.size());
+ for (auto& [key, value] : rtLoggingEntries)
{
- //base (marker;iteration;timestamp)
+ if (value->stopLogging)
{
- for (auto& pair : rtLoggingEntries)
- {
- CSVLoggingEntry& e = *pair.second;
- e.stream << "\n"
- << e.marker << ";"
- << data.iteration << ";"
- << data.sensorValuesTimestamp.toMicroSeconds();
- e.marker.clear();
- }
+ //can't remove the current elemet
+ //(this would invalidate the current iterator)
+ toRemove.emplace_back(key);
}
- //sens
+ }
+ for (const auto& rem : toRemove)
+ {
+ rtLoggingEntries.erase(rem);
+ }
+ }
+ //deal with data streaming
+ {
+ ARMARX_TRACE;
+ std::vector<RobotUnitDataStreaming::ReceiverPrx> toRemove;
+ toRemove.reserve(rtDataStreamingEntry.size());
+ for (auto& [prx, data] : rtDataStreamingEntry)
+ {
+ if (data.stopStreaming)
+ {
+ toRemove.emplace_back(prx);
+ }
+ else
{
- //sensors
- for (std::size_t idxDev = 0; idxDev < data.sensors.size(); ++ idxDev)
+ data.send(prx);
+ }
+ }
+ for (const auto& prx : toRemove)
+ {
+ rtDataStreamingEntry.erase(prx);
+ }
+ }
+ }
+
+ void Logging::doLogging(const IceUtil::Time& now, const
+ ControlThreadOutputBuffer::Entry& data,
+ std::size_t i, std::size_t num)
+ {
+ ARMARX_TRACE;
+ //header
+ {
+ ARMARX_TRACE;
+ //base (marker;iteration;timestamp)
+ for (auto& [_, e] : rtLoggingEntries)
+ {
+ e->stream << "\n"
+ << e->marker << ";"
+ << data.iteration << ";"
+ << data.sensorValuesTimestamp.toMicroSeconds() << ";"
+ << data.timeSinceLastIteration.toMicroSeconds();
+ e->marker.clear();
+ }
+ //streaming
+ for (auto& [_, e] : rtDataStreamingEntry)
+ {
+ e.processHeader(data);
+ }
+ }
+ //process devices
+ {
+ //sens
+ {
+ ARMARX_TRACE;
+ //sensors
+ for (std::size_t idxDev = 0; idxDev < data.sensors.size(); ++ idxDev)
+ {
+ const SensorValueBase* val = data.sensors.at(idxDev);
+ //dimensions of sensor value (e.g. vel, tor, f_x, f_y, ...)
+ for (std::size_t idxField = 0; idxField < val->getNumberOfDataFields(); ++ idxField)
{
- const SensorValueBase* val = data.sensors.at(idxDev);
- //dimensions of sensor value (e.g. vel, tor, f_x, f_y, ...)
- for (std::size_t idxField = 0; idxField < val->getNumberOfDataFields(); ++ idxField)
+ std::string str;
+ val->getDataFieldAs(idxField, str);
+ for (auto& [_, entry] : rtLoggingEntries)
{
- const auto str = val->getDataFieldAsString(idxField);
- for (auto& [_, entry] : rtLoggingEntries)
+ if (entry->loggedSensorDeviceValues.at(idxDev).at(idxField))
{
- if (entry->loggedSensorDeviceValues.at(idxDev).at(idxField))
- {
- entry->stream << ";" << str;
- }
+ entry->stream << ";" << str;
}
}
+ for (auto& [_, data] : rtDataStreamingEntry)
+ {
+ data.processSens(*val, idxDev, idxField);
+ }
}
}
- //ctrl
+ }
+ //ctrl
+ {
+ ARMARX_TRACE;
+ //joint controllers
+ for (std::size_t idxDev = 0; idxDev < data.control.size(); ++ idxDev)
{
- //joint controllers
- for (std::size_t idxDev = 0; idxDev < data.control.size(); ++ idxDev)
+ const auto& vals = data.control.at(idxDev);
+ //control value (e.g. v_platform)
+ for (std::size_t idxVal = 0; idxVal < vals.size(); ++idxVal)
{
- const auto& vals = data.control.at(idxDev);
- //control value (e.g. v_platform)
- for (std::size_t idxVal = 0; idxVal < vals.size(); ++idxVal)
+ const ControlTargetBase* val = vals.at(idxVal);
+ //dimensions of control value (e.g. v_platform_x, v_platform_y, v_platform_rotate)
+ for (std::size_t idxField = 0; idxField < val->getNumberOfDataFields(); ++ idxField)
{
- const ControlTargetBase* val = vals.at(idxVal);
- //dimensions of control value (e.g. v_platform_x, v_platform_y, v_platform_rotate)
- for (std::size_t idxField = 0; idxField < val->getNumberOfDataFields(); ++ idxField)
+ std::string str;
+ val->getDataFieldAs(idxField, str);
+ for (auto& [_, entry] : rtLoggingEntries)
{
- const auto str = val->getDataFieldAsString(idxField);
- for (auto& [_, entry] : rtLoggingEntries)
+ if (entry->loggedControlDeviceValues.at(idxDev).at(idxVal).at(idxField))
{
- if (entry->loggedControlDeviceValues.at(idxDev).at(idxVal).at(idxField))
- {
- entry->stream << ";" << str;
- }
+ entry->stream << ";" << str;
}
}
+ for (auto& [_, data] : rtDataStreamingEntry)
+ {
+ data.processCtrl(*val, idxDev, idxVal, idxField);
+ }
}
}
}
- //store data to backlog
+ }
+ }
+ //finish processing
+ {
+ //store data to backlog
+ {
+ ARMARX_TRACE;
+ if (data.writeTimestamp + rtLoggingBacklogRetentionTime >= now)
{
- if (data.writeTimestamp + rtLoggingBacklogRetentionTime >= now)
- {
- backlog.emplace_back(data, true); //true for minimal copy
- }
+ backlog.emplace_back(data, true); //true for minimal copy
}
- //print + reset messages
+ }
+ //print + reset messages
+ {
+ ARMARX_TRACE;
+ for (const ::armarx::detail::RtMessageLogEntryBase* ptr : data.messages.getEntries())
{
- for (const ::armarx::detail::RtMessageLogEntryBase* ptr : data.messages.getEntries())
+ if (!ptr)
{
- if (!ptr)
- {
- break;
- }
- ptr->print(controlThreadId);
+ break;
}
+ ptr->print(controlThreadId);
}
- });
- }
- ARMARX_DEBUG_S << ::deactivateSpam() << "the last " << backlog.size() << " iterations are stored";
- //flush all files
- {
- for (auto& pair : rtLoggingEntries)
- {
- pair.second->stream << std::flush;
}
}
+ }
- //remove entries
+ bool Logging::MatchName(const std::string& pattern, const std::string& name)
+ {
+ ARMARX_TRACE;
+ if (pattern.empty())
{
- std::vector<std::string> toRemove;
- toRemove.reserve(rtLoggingEntries.size());
- for (auto& pair : rtLoggingEntries)
- {
- if (pair.second->stopLogging)
- {
- //can't remove the current elemet
- //(this would invalidate the current iterator)
- toRemove.emplace_back(pair.first);
- }
- }
- for (const auto& rem : toRemove)
- {
- rtLoggingEntries.erase(rem);
- }
+ return false;
+ }
+ static const std::regex pattern_regex{R"(^\^?[- ._*a-zA-Z0-9]+\$?$)"};
+ if (!std::regex_match(pattern, pattern_regex))
+ {
+ throw InvalidArgumentException {"Pattern '" + pattern + "' is invalid"};
}
+ static const std::regex reg_dot{"[.]"};
+ static const std::regex reg_star{"[*]"};
+ const std::string rpl1 = std::regex_replace(pattern, reg_dot, "\\.");
+ const std::string rpl2 = std::regex_replace(rpl1, reg_star, ".*");
+ const std::regex key_regex{rpl2};
+ return std::regex_search(name, key_regex);
}
void Logging::_postOnInitRobotUnit()
{
+ ARMARX_TRACE;
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
rtLoggingTimestep = getProperty<std::size_t>("RTLogging_PeriodMs");
ARMARX_CHECK_LESS(0, rtLoggingTimestep) << "The property RTLoggingPeriodMs must not be 0";
@@ -448,9 +690,11 @@ namespace armarx::RobotUnitModule
void Logging::_postFinishDeviceInitialization()
{
+ ARMARX_TRACE;
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
//init buffer
{
+ ARMARX_TRACE;
std::size_t ctrlThreadPeriodUs = static_cast<std::size_t>(getControlThreadTargetPeriod().toMicroSeconds());
std::size_t logThreadPeriodUs = rtLoggingTimestep * 1000;
std::size_t nBuffers = (logThreadPeriodUs / ctrlThreadPeriodUs + 1) * 100;
@@ -463,51 +707,54 @@ namespace armarx::RobotUnitModule
<< nBuffers << "buffers "
<< "(buffersize = " << bufferSize << " bytes)";
}
- //init logging names
+ //init logging names + field types
{
+ ARMARX_TRACE;
+ const auto makeValueMetaData = [&](auto * val, const std::string & namePre)
+ {
+ ValueMetaData data;
+ const auto names = val->getDataFieldNames();
+ data.fields.resize(names.size());
+ for (const auto& [fieldIdx, fieldName] : MakeIndexedContainer(names))
+ {
+ data.fields.at(fieldIdx).name = namePre + '.' + fieldName;
+ data.fields.at(fieldIdx).type = &(val->getDataFieldType(fieldIdx));
+ }
+ return data;
+ };
+
//sensorDevices
- controlDeviceValueLoggingNames.reserve(_module<Devices>().getControlDevices().size());
+ controlDeviceValueMetaData.reserve(_module<Devices>().getControlDevices().size());
for (const auto& cd : _module<Devices>().getControlDevices().values())
{
- controlDeviceValueLoggingNames.emplace_back();
- auto& namesForDev = controlDeviceValueLoggingNames.back();
- namesForDev.reserve(cd->getJointControllers().size());
+ ARMARX_TRACE;
+ controlDeviceValueMetaData.emplace_back();
+ auto& dataForDev = controlDeviceValueMetaData.back();
+ dataForDev.reserve(cd->getJointControllers().size());
for (auto jointC : cd->getJointControllers())
{
- const auto names = jointC->getControlTarget()->getDataFieldNames();
- namesForDev.emplace_back();
- auto& fullNames = namesForDev.back();
- fullNames.reserve(names.size());
- for (const auto& name : names)
- {
- fullNames.emplace_back(
- "ctrl." +
- cd->getDeviceName() + "." +
- jointC->getControlMode() + "." +
- name);
- }
+ dataForDev.emplace_back(
+ makeValueMetaData(jointC->getControlTarget(),
+ "ctrl." +
+ cd->getDeviceName() + "." +
+ jointC->getControlMode()));
}
}
//sensorDevices
- sensorDeviceValueLoggingNames.reserve(_module<Devices>().getSensorDevices().size());
+ sensorDeviceValueMetaData.reserve(_module<Devices>().getSensorDevices().size());
for (const auto& sd : _module<Devices>().getSensorDevices().values())
{
- const auto names = sd->getSensorValue()->getDataFieldNames();
- sensorDeviceValueLoggingNames.emplace_back();
- auto& fullNames = sensorDeviceValueLoggingNames.back();
- fullNames.reserve(names.size());
- for (const auto& name : names)
- {
- fullNames.emplace_back(
- "sens." +
- sd->getDeviceName() + "." +
- name);
- }
+ ARMARX_TRACE;
+ sensorDeviceValueMetaData.emplace_back(
+ makeValueMetaData(sd->getSensorValue(),
+ "sens." +
+ sd->getDeviceName()));
}
}
//start logging thread is done in rtinit
//maybe add the default log
{
+ ARMARX_TRACE;
const auto loggingpath = getProperty<std::string>("RTLogging_DefaultLog").getValue();
if (!loggingpath.empty())
{
@@ -518,4 +765,117 @@ namespace armarx::RobotUnitModule
rtLoggingTask = new RTLoggingTaskT([&] {doLogging();}, rtLoggingTimestep, false, getName() + "_RTLoggingTask");
rtLoggingTask->setDelayWarningTolerance(rtLoggingTimestep * 10);
}
+
+ void Logging::DataStreamingEntry::processHeader(const ControlThreadOutputBuffer::Entry& e)
+ {
+ ARMARX_TRACE;
+ if (stopStreaming)
+ {
+ return;
+ }
+ result.emplace_back();
+ auto& data = result.back();
+ data.iterationId = e.iteration;
+ data.timestampUSec = e.sensorValuesTimestamp.toMicroSeconds();
+ data.timesSinceLastIterationUSec = e.timeSinceLastIteration.toMicroSeconds();
+
+ data.bools .resize(numBools);
+ data.bytes .resize(numBytes);
+ data.shorts .resize(numShorts);
+ data.ints .resize(numInts);
+ data.longs .resize(numLongs);
+ data.floats .resize(numFloats);
+ data.doubles.resize(numDoubles);
+
+ }
+
+ void WriteTo(const Logging::DataStreamingEntry::OutVal& out,
+ const auto& val,
+ std::size_t fidx,
+ auto& data)
+ {
+ ARMARX_TRACE;
+ using enum_t = Logging::DataStreamingEntry::ValueT;
+ switch (out.value)
+ {
+ case enum_t::Bool :
+ bool b;
+ val.getDataFieldAs(fidx, b);
+ data.bools.at(out.idx) = b;
+ return;
+ case enum_t::Byte :
+ val.getDataFieldAs(fidx, data.bytes.at(out.idx));
+ return;
+ case enum_t::Short :
+ val.getDataFieldAs(fidx, data.shorts.at(out.idx));
+ return;
+ case enum_t::Int :
+ val.getDataFieldAs(fidx, data.ints.at(out.idx));
+ return;
+ case enum_t::Long :
+ val.getDataFieldAs(fidx, data.longs.at(out.idx));
+ return;
+ case enum_t::Float :
+ val.getDataFieldAs(fidx, data.floats.at(out.idx));
+ return;
+ case enum_t::Double :
+ val.getDataFieldAs(fidx, data.doubles.at(out.idx));
+ return;
+ case enum_t::Skipped:
+ return;
+ }
+ }
+
+ void Logging::DataStreamingEntry::processCtrl(
+ const ControlTargetBase& val,
+ std::size_t didx,
+ std::size_t vidx,
+ std::size_t fidx)
+ {
+ ARMARX_TRACE;
+ if (stopStreaming)
+ {
+ return;
+ }
+ auto& data = result.back();
+ const OutVal& o = ctrlDevs.at(didx).at(vidx).at(fidx);
+ WriteTo(o, val, fidx, data);
+ }
+
+ void Logging::DataStreamingEntry::processSens(
+ const SensorValueBase& val,
+ std::size_t didx,
+ std::size_t fidx)
+ {
+ ARMARX_TRACE;
+ if (stopStreaming)
+ {
+ return;
+ }
+ auto& data = result.back();
+ const OutVal& o = sensDevs.at(didx).at(fidx);
+ WriteTo(o, val, fidx, data);
+ }
+
+ void Logging::DataStreamingEntry::send(const RobotUnitDataStreaming::ReceiverPrx& r)
+ {
+ ARMARX_TRACE;
+ try
+ {
+ r->update(result);
+ connectionFailures = 0;
+ result.clear();
+ }
+ catch (...)
+ {
+ ARMARX_TRACE;
+ ++connectionFailures;
+ if (connectionFailures > 10) ///TODO make property
+ {
+ stopStreaming = true;
+ ARMARX_WARNING_S << "DataStreaming Receiver was not reachable for "
+ << connectionFailures << " iterations! Removing receiver";
+ }
+ }
+ }
}
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.h b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.h
index 68cc02b917827edbb5442a7ef28d7e47d50d3776..2df9d88d2d8d497d12985fca320c95a8ea0e5576 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.h
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.h
@@ -150,12 +150,24 @@ namespace armarx::RobotUnitModule
*/
void writeRecentIterationsToFile(const std::string& formatString, const Ice::Current& = Ice::emptyCurrent) const override;
+ RobotUnitDataStreaming::DataStreamingDescription startDataStreaming(
+ const RobotUnitDataStreaming::ReceiverPrx& receiver,
+ const RobotUnitDataStreaming::Config& config,
+ const Ice::Current& = Ice::emptyCurrent) override;
+
+ void stopDataStreaming(
+ const RobotUnitDataStreaming::ReceiverPrx& receiver,
+ const Ice::Current& = Ice::emptyCurrent) override;
// //////////////////////////////////////////////////////////////////////////////////////// //
// //////////////////////////////////// implementation //////////////////////////////////// //
// //////////////////////////////////////////////////////////////////////////////////////// //
private:
/// @brief Executes the logging loop.
void doLogging();
+ void doLogging(const IceUtil::Time& now,
+ const ControlThreadOutputBuffer::Entry& data,
+ std::size_t i, std::size_t num);
+ static bool MatchName(const std::string& pattern, const std::string& name);
// //////////////////////////////////////////////////////////////////////////////////////// //
// ///////////////////////////////////////// Data ///////////////////////////////////////// //
@@ -180,6 +192,54 @@ namespace armarx::RobotUnitModule
std::string marker;
};
+ struct DataStreamingEntry
+ {
+ enum class ValueT
+ {
+ Bool,
+ Byte,
+ Short,
+ Int,
+ Long,
+ Float,
+ Double,
+ Skipped
+ };
+ struct OutVal
+ {
+ std::size_t idx;
+ ValueT value = ValueT::Skipped;
+ };
+
+ bool stopStreaming = false;
+
+ std::size_t numBools = 0;
+ std::size_t numBytes = 0;
+ std::size_t numShorts = 0;
+ std::size_t numInts = 0;
+ std::size_t numLongs = 0;
+ std::size_t numFloats = 0;
+ std::size_t numDoubles = 0;
+
+ bool onlyNewestFrame = false;
+ std::size_t connectionFailures = 0;
+
+ std::vector<std::vector<OutVal>> sensDevs;
+ std::vector<std::vector<std::vector<OutVal>>> ctrlDevs;
+
+ RobotUnitDataStreaming::TimeStepSeq result;
+ void processHeader(const ControlThreadOutputBuffer::Entry& e);
+ void processCtrl(const ControlTargetBase& val,
+ std::size_t didx,
+ std::size_t vidx,
+ std::size_t fidx);
+ void processSens(const SensorValueBase& val,
+ std::size_t didx,
+ std::size_t fidx);
+ void send(const RobotUnitDataStreaming::ReceiverPrx& r);
+ };
+ std::map<RobotUnitDataStreaming::ReceiverPrx, DataStreamingEntry> rtDataStreamingEntry;
+
/// @brief The thread executing the logging functions.
RTLoggingTaskT::pointer_type rtLoggingTask;
/// @brief All entries for logging.
@@ -192,10 +252,21 @@ namespace armarx::RobotUnitModule
Ice::Int controlThreadId {0};
/// @brief Mutex protecting the data structures of this class
mutable std::mutex rtLoggingMutex;
- /// @brief Data field names for all \ref ControlTargetBase "ControlTargets"
- std::vector<std::vector<std::vector<std::string>>> controlDeviceValueLoggingNames;
- /// @brief Data field names for all \ref SensorValueBase "SensorValues"
- std::vector<std::vector< std::string >> sensorDeviceValueLoggingNames;
+
+ struct ValueMetaData
+ {
+ struct FieldMetaData
+ {
+ std::string name;
+ const std::type_info* type;
+ };
+ std::vector<FieldMetaData> fields;
+ };
+
+ /// @brief Data field info for all \ref ControlTargetBase "ControlTargets" (dex x jctrl)
+ std::vector<std::vector<ValueMetaData>> controlDeviceValueMetaData;
+ /// @brief Data field info for all \ref SensorValueBase "SensorValues" (dev)
+ std::vector< ValueMetaData > sensorDeviceValueMetaData;
/// @brief The initial size of the Message entry buffer
std::size_t messageBufferSize {0};
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModulePublisher.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModulePublisher.cpp
index a1c4fb5941d1108f8d5626f7995d690b6733f8e1..763bab62f2773dbb26e45efbc3e38ae7b4839561 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModulePublisher.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModulePublisher.cpp
@@ -182,6 +182,7 @@ namespace armarx::RobotUnitModule
const auto endInner = TimeUtil::GetTime(true);
timingMap["NJointControllerUpdates_" + nJointCtrl->getInstanceName()] = new TimedVariant {TimestampVariant{endInner - begInner}, lastControlThreadTimestamp};
}
+
robotUnitListenerBatchPrx->nJointControllerStatusChanged(allStatus);
debugObserverBatchPrx->ice_flushBatchRequests();
debugDrawerBatchPrx->ice_flushBatchRequests();
@@ -270,11 +271,15 @@ namespace armarx::RobotUnitModule
status.timestampUSec = lastControlThreadTimestamp.toMicroSeconds();
allStatus.emplace_back(status);
}
+
robotUnitListenerBatchPrx->controlDeviceStatusChanged(allStatus);
- if (robotUnitObserver->getState() >= eManagedIceObjectStarted)
+ if (observerPublishControlTargets)
{
- robotUnitObserver->offerOrUpdateDataFieldsFlatCopy(robotUnitObserver->controlDevicesChannel, ctrlDevMap);
- robotUnitObserver->updateChannel(robotUnitObserver->controlDevicesChannel);
+ if (robotUnitObserver->getState() >= eManagedIceObjectStarted)
+ {
+ robotUnitObserver->offerOrUpdateDataFieldsFlatCopy(robotUnitObserver->controlDevicesChannel, ctrlDevMap);
+ robotUnitObserver->updateChannel(robotUnitObserver->controlDevicesChannel);
+ }
}
const auto end = TimeUtil::GetTime(true);
@@ -315,23 +320,27 @@ namespace armarx::RobotUnitModule
allStatus.emplace_back(status);
}
}
+
robotUnitListenerBatchPrx->sensorDeviceStatusChanged(allStatus);
- if (robotUnitObserver->getState() >= eManagedIceObjectStarted)
+ if (observerPublishSensorValues)
{
- std::stringstream s;
- for (auto& pair : sensorDevMap)
- {
- s << pair.first << ": \t" << (pair.second ? pair.second->ice_id() + "\t with datatype \t" + Variant::typeToString(pair.second->getType()) : "NULL") << "\n";
- }
- ARMARX_DEBUG << deactivateSpam(spamdelay) << "Sensor Datafieldnames: " << ARMARX_STREAM_PRINTER
+ if (robotUnitObserver->getState() >= eManagedIceObjectStarted)
{
+ std::stringstream s;
for (auto& pair : sensorDevMap)
{
- out << pair.first << ": " << (pair.second ? pair.second->ice_id() + Variant::typeToString(pair.second->getType()) : "NULL") << "\n";
+ s << pair.first << ": \t" << (pair.second ? pair.second->ice_id() + "\t with datatype \t" + Variant::typeToString(pair.second->getType()) : "NULL") << "\n";
}
- };
- robotUnitObserver->offerOrUpdateDataFieldsFlatCopy(robotUnitObserver->sensorDevicesChannel, sensorDevMap);
- robotUnitObserver->updateChannel(robotUnitObserver->sensorDevicesChannel);
+ ARMARX_DEBUG << deactivateSpam(spamdelay) << "Sensor Datafieldnames: " << ARMARX_STREAM_PRINTER
+ {
+ for (auto& pair : sensorDevMap)
+ {
+ out << pair.first << ": " << (pair.second ? pair.second->ice_id() + Variant::typeToString(pair.second->getType()) : "NULL") << "\n";
+ }
+ };
+ robotUnitObserver->offerOrUpdateDataFieldsFlatCopy(robotUnitObserver->sensorDevicesChannel, sensorDevMap);
+ robotUnitObserver->updateChannel(robotUnitObserver->sensorDevicesChannel);
+ }
}
const auto end = TimeUtil::GetTime(true);
@@ -400,6 +409,8 @@ namespace armarx::RobotUnitModule
observerPublishSensorValues = getProperty<bool>("ObserverPublishSensorValues").getValue();
observerPublishControlTargets = getProperty<bool>("ObserverPublishControlTargets").getValue();
+ observerPublishTimingInformation = getProperty<bool>("ObserverPublishTimingInformation").getValue();
+ observerPublishAdditionalInformation = getProperty<bool>("ObserverPublishAdditionalInformation").getValue();
debugObserverSkipIterations = std::max(1ul, getProperty<std::uint64_t>("ObserverPrintEveryNthIterations").getValue());
publishPeriodMs = std::max(1ul, getProperty<std::size_t>("PublishPeriodMs").getValue());
@@ -428,6 +439,14 @@ namespace armarx::RobotUnitModule
{
observerPublishControlTargets = getProperty<bool>("ObserverPublishControlTargets").getValue();
}
+ if (changedProperties.count("ObserverPublishTimingInformation"))
+ {
+ observerPublishTimingInformation = getProperty<bool>("ObserverPublishTimingInformation").getValue();
+ }
+ if (changedProperties.count("ObserverPublishAdditionalInformation"))
+ {
+ observerPublishAdditionalInformation = getProperty<bool>("ObserverPublishAdditionalInformation").getValue();
+ }
if (changedProperties.count("ObserverPrintEveryNthIterations"))
{
debugObserverSkipIterations = getProperty<std::uint64_t>("ObserverPrintEveryNthIterations").getValue();
@@ -550,7 +569,10 @@ namespace armarx::RobotUnitModule
timingMap["ClassNameUpdates"] = publishNJointClassNames();
timingMap["RobotUnitListenerFlush"] = new TimedVariant
{
- ARMARX_STOPWATCH(TimestampVariant){robotUnitListenerBatchPrx->ice_flushBatchRequests();}, lastControlThreadTimestamp
+ ARMARX_STOPWATCH(TimestampVariant)
+ {
+ robotUnitListenerBatchPrx->ice_flushBatchRequests();
+ }, lastControlThreadTimestamp
};
@@ -559,10 +581,16 @@ namespace armarx::RobotUnitModule
timingMap["LastPublishLoop"] = new TimedVariant {TimestampVariant{lastPublishLoop}, lastControlThreadTimestamp};
if (robotUnitObserver->getState() >= eManagedIceObjectStarted)
{
- robotUnitObserver->offerOrUpdateDataFieldsFlatCopy(robotUnitObserver->additionalChannel, additionalMap);
- robotUnitObserver->updateChannel(robotUnitObserver->additionalChannel);
- robotUnitObserver->offerOrUpdateDataFieldsFlatCopy(robotUnitObserver->timingChannel, timingMap);
- robotUnitObserver->updateChannel(robotUnitObserver->timingChannel);
+ if (observerPublishTimingInformation)
+ {
+ robotUnitObserver->offerOrUpdateDataFieldsFlatCopy(robotUnitObserver->additionalChannel, additionalMap);
+ robotUnitObserver->updateChannel(robotUnitObserver->additionalChannel);
+ }
+ if (observerPublishAdditionalInformation)
+ {
+ robotUnitObserver->offerOrUpdateDataFieldsFlatCopy(robotUnitObserver->timingChannel, timingMap);
+ robotUnitObserver->updateChannel(robotUnitObserver->timingChannel);
+ }
}
}
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModulePublisher.h b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModulePublisher.h
index 15b67d9fae37f0cd7b117759fb157136e76dafec..5844b7ba9efb2bbb4bdcd0f0370ad3def34cddd5 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModulePublisher.h
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModulePublisher.h
@@ -65,6 +65,12 @@ namespace armarx::RobotUnitModule
defineOptionalProperty<bool>(
"ObserverPublishControlTargets", true,
"Whether control targets are send to the observer", PropertyDefinitionBase::eModifiable);
+ defineOptionalProperty<bool>(
+ "ObserverPublishTimingInformation", true,
+ "Whether timing information are send to the observer", PropertyDefinitionBase::eModifiable);
+ defineOptionalProperty<bool>(
+ "ObserverPublishAdditionalInformation", true,
+ "Whether additional information are send to the observer", PropertyDefinitionBase::eModifiable);
defineOptionalProperty<std::string>(
"DebugObserverTopicName", "DebugObserver", "The topic where updates are send to");
defineOptionalProperty<std::uint64_t>(
@@ -258,6 +264,11 @@ namespace armarx::RobotUnitModule
std::atomic_bool observerPublishSensorValues;
/// @brief Whether \ref ControlTargetBase "ControlTargets" should be published to the observers
std::atomic_bool observerPublishControlTargets;
+ /// @brief Whether \ref Timing information should be published to the observers
+ std::atomic_bool observerPublishTimingInformation;
+ /// @brief Whether \ref Additional information should be published to the observers
+ std::atomic_bool observerPublishAdditionalInformation;
+
/// @brief How many iterations of \ref publish shold not publish data to the debug observer.
std::atomic<std::uint64_t> debugObserverSkipIterations;
diff --git a/source/RobotAPI/components/units/RobotUnit/SensorValues/SensorValueBase.h b/source/RobotAPI/components/units/RobotUnit/SensorValues/SensorValueBase.h
index e378603fc90b365fe739a03fd60e76e34d913c9d..18d537b291f6c204036ec14e467a92e9660144df 100644
--- a/source/RobotAPI/components/units/RobotUnit/SensorValues/SensorValueBase.h
+++ b/source/RobotAPI/components/units/RobotUnit/SensorValues/SensorValueBase.h
@@ -91,7 +91,15 @@ namespace armarx
virtual std::size_t getNumberOfDataFields() const = 0;
virtual std::vector<std::string> getDataFieldNames() const = 0;
- virtual std::string getDataFieldAsString(std::size_t i) const = 0;
+ virtual void getDataFieldAs(std::size_t i, bool& out) const = 0;
+ virtual void getDataFieldAs(std::size_t i, Ice::Byte& out) const = 0;
+ virtual void getDataFieldAs(std::size_t i, Ice::Short& out) const = 0;
+ virtual void getDataFieldAs(std::size_t i, Ice::Int& out) const = 0;
+ virtual void getDataFieldAs(std::size_t i, Ice::Long& out) const = 0;
+ virtual void getDataFieldAs(std::size_t i, Ice::Float& out) const = 0;
+ virtual void getDataFieldAs(std::size_t i, Ice::Double& out) const = 0;
+ virtual void getDataFieldAs(std::size_t i, std::string& out) const = 0;
+ virtual const std::type_info& getDataFieldType(std::size_t i) const = 0;
//management functions
template<class T, class = typename std::enable_if<std::is_base_of<SensorValueBase, T>::value>::type>
@@ -130,9 +138,41 @@ namespace armarx
{ \
return SensorValueInfo<std::decay<decltype(*this)>::type>::GetNumberOfDataFields(); \
} \
- std::string getDataFieldAsString(std::size_t i) const override \
+ void getDataFieldAs (std::size_t i, bool& out) const override \
{ \
- return SensorValueInfo<std::decay<decltype(*this)>::type>::GetDataFieldAsString(this, i); \
+ SensorValueInfo<std::decay<decltype(*this)>::type>::GetDataFieldAs (this, i, out); \
+ } \
+ void getDataFieldAs (std::size_t i, Ice::Byte& out) const override \
+ { \
+ SensorValueInfo<std::decay<decltype(*this)>::type>::GetDataFieldAs (this, i, out); \
+ } \
+ void getDataFieldAs (std::size_t i, Ice::Short& out) const override \
+ { \
+ SensorValueInfo<std::decay<decltype(*this)>::type>::GetDataFieldAs (this, i, out); \
+ } \
+ void getDataFieldAs (std::size_t i, Ice::Int& out) const override \
+ { \
+ SensorValueInfo<std::decay<decltype(*this)>::type>::GetDataFieldAs (this, i, out); \
+ } \
+ void getDataFieldAs (std::size_t i, Ice::Long& out) const override \
+ { \
+ SensorValueInfo<std::decay<decltype(*this)>::type>::GetDataFieldAs (this, i, out); \
+ } \
+ void getDataFieldAs (std::size_t i, Ice::Float& out) const override \
+ { \
+ SensorValueInfo<std::decay<decltype(*this)>::type>::GetDataFieldAs (this, i, out); \
+ } \
+ void getDataFieldAs (std::size_t i, Ice::Double& out) const override \
+ { \
+ SensorValueInfo<std::decay<decltype(*this)>::type>::GetDataFieldAs (this, i, out); \
+ } \
+ void getDataFieldAs (std::size_t i, std::string& out) const override \
+ { \
+ SensorValueInfo<std::decay<decltype(*this)>::type>::GetDataFieldAs (this, i, out); \
+ } \
+ const std::type_info& getDataFieldType(std::size_t i) const override \
+ { \
+ return SensorValueInfo<std::decay<decltype(*this)>::type>::GetDataFieldType(i); \
} \
std::vector<std::string> getDataFieldNames() const override \
{ \
diff --git a/source/RobotAPI/components/units/RobotUnit/Units/TrajectoryControllerSubUnit.h b/source/RobotAPI/components/units/RobotUnit/Units/TrajectoryControllerSubUnit.h
index b0b65db9dad4ee9cd94742d27b542cb03719492a..9f48b80609903888caf23163cdefeb1c801ddf74 100644
--- a/source/RobotAPI/components/units/RobotUnit/Units/TrajectoryControllerSubUnit.h
+++ b/source/RobotAPI/components/units/RobotUnit/Units/TrajectoryControllerSubUnit.h
@@ -99,6 +99,9 @@ namespace armarx
void reportJointCurrents(const NameValueMap&, Ice::Long, bool, const Ice::Current&) override {}
void reportJointMotorTemperatures(const NameValueMap&, Ice::Long, bool, const Ice::Current&) override {}
void reportJointStatuses(const NameStatusMap&, Ice::Long, bool, const Ice::Current&) override {}
+
+ void setOffset(const ::Eigen::Matrix4f&, const ::Ice::Current& = ::Ice::emptyCurrent) override {}
+
// ManagedIceObject interface
protected:
void onInitComponent() override;
diff --git a/source/RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.cpp b/source/RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.cpp
index e753f9c4b60b13533dc2cfa20e6494a1d106ba1a..8f038519bc03b014aef5219bb939e1acf03d55af 100644
--- a/source/RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.cpp
@@ -71,7 +71,7 @@ namespace armarx
onePastLoggingReadPosition = writePosition.load();
}
- void ControlThreadOutputBuffer::foreachNewLoggingEntry(std::function<void (const ControlThreadOutputBuffer::Entry&)> consumer)
+ void ControlThreadOutputBuffer::foreachNewLoggingEntry(ConsumerFunctor consumer)
{
ARMARX_CHECK_EXPRESSION(isInitialized);
if (writePosition - onePastLoggingReadPosition >= numEntries)
@@ -87,10 +87,15 @@ namespace armarx
auto numNewEntries = writePosition - onePastLoggingReadPosition;
ARMARX_CHECK_EXPRESSION(numNewEntries < numEntries);
//consume all
- for (; onePastLoggingReadPosition < writePosition; ++onePastLoggingReadPosition)
+ const std::size_t num = writePosition - onePastLoggingReadPosition;
+ for (
+ std::size_t offset = 0;
+ onePastLoggingReadPosition < writePosition;
+ ++onePastLoggingReadPosition, ++offset
+ )
{
auto& entry = entries.at(toBounds(onePastLoggingReadPosition));
- consumer(entry);
+ consumer(entry, offset, num);
entry.messages.reset(messageBufferSize, messageBufferEntries, entry.iteration);
//update how many
diff --git a/source/RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.h b/source/RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.h
index 6412aaba778c5137020f43ae7e9efadf3e69c640..1709023ec2cc788dd4fa9664dbe6739746bad4ae 100644
--- a/source/RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.h
+++ b/source/RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.h
@@ -221,6 +221,7 @@ namespace armarx
{
using Entry = detail::ControlThreadOutputBufferEntry;
using RtMessageLogEntryBase = detail::RtMessageLogEntryBase;
+ using ConsumerFunctor = std::function<void (const Entry&, std::size_t, std::size_t)>;
std::size_t initialize(
std::size_t numEntries,
const KeyValueVector<std::string, ControlDevicePtr>& controlDevices,
@@ -245,7 +246,7 @@ namespace armarx
//logging read
void resetLoggingPosition() const;
- void foreachNewLoggingEntry(std::function<void(const Entry&)> consumer);
+ void foreachNewLoggingEntry(ConsumerFunctor consumer);
std::size_t getNumberOfBytes() const;
diff --git a/source/RobotAPI/components/units/RobotUnit/util/introspection/ClassMemberInfo.h b/source/RobotAPI/components/units/RobotUnit/util/introspection/ClassMemberInfo.h
index 65e8980af45c0a202787d0220e89bf7cf0313ea1..554b1ae3f27be04d0566d8306d8c7859f497f902 100644
--- a/source/RobotAPI/components/units/RobotUnit/util/introspection/ClassMemberInfo.h
+++ b/source/RobotAPI/components/units/RobotUnit/util/introspection/ClassMemberInfo.h
@@ -55,10 +55,39 @@ namespace armarx::introspection
/// @brief Get all entries for member variables
static const KeyValueVector<std::string, Entry>& GetEntries();
static std::size_t GetNumberOfDataFields();
- static std::string GetDataFieldAsString(const ClassType* ptr, std::size_t i);
+ static void GetDataFieldAs(const ClassType* ptr, std::size_t i, bool& out);
+ static void GetDataFieldAs(const ClassType* ptr, std::size_t i, Ice::Byte& out);
+ static void GetDataFieldAs(const ClassType* ptr, std::size_t i, Ice::Short& out);
+ static void GetDataFieldAs(const ClassType* ptr, std::size_t i, Ice::Int& out);
+ static void GetDataFieldAs(const ClassType* ptr, std::size_t i, Ice::Long& out);
+ static void GetDataFieldAs(const ClassType* ptr, std::size_t i, Ice::Float& out);
+ static void GetDataFieldAs(const ClassType* ptr, std::size_t i, Ice::Double& out);
+ static void GetDataFieldAs(const ClassType* ptr, std::size_t i, std::string& out);
+ static const std::type_info& GetDataFieldType(std::size_t i);
static std::vector<std::string> GetDataFieldNames();
static std::map<std::string, VariantBasePtr> ToVariants(const IceUtil::Time& timestamp, const CommonBaseT* ptr);
+ private:
+ template<class T>
+ static auto MakeConverter()
+ {
+ std::vector<std::function<void(const ClassType*, T&)>> functions;
+
+ for (std::size_t idxEntr = 0; idxEntr < GetEntries().size(); ++idxEntr)
+ {
+ for (std::size_t idxField = 0; idxField < GetEntries().at(idxEntr).getNumberOfFields(); ++idxField)
+ {
+ functions.emplace_back(
+ [idxEntr, idxField](const ClassType * classptr, T & out)
+ {
+ const Entry& e = GetEntries().at(idxEntr);
+ e.getDataFieldAs(idxField, classptr, out);
+ });
+ }
+ }
+ ARMARX_CHECK_EQUAL(functions.size(), GetNumberOfDataFields());
+ return functions;
+ }
private:
KeyValueVector<std::string, Entry> entries;
@@ -174,28 +203,81 @@ namespace armarx::introspection
}
template<class CommonBaseT, class ClassT>
- std::string ClassMemberInfo<CommonBaseT, ClassT>::GetDataFieldAsString(const ClassType* ptr, std::size_t i)
+ void ClassMemberInfo<CommonBaseT, ClassT>::GetDataFieldAs(const ClassType* ptr, std::size_t i, bool& out)
+ {
+ static const auto convert = MakeConverter<bool>();
+ ARMARX_CHECK_LESS(i, GetNumberOfDataFields());
+ convert.at(i)(ptr, out);
+ }
+ template<class CommonBaseT, class ClassT>
+ void ClassMemberInfo<CommonBaseT, ClassT>::GetDataFieldAs(const ClassType* ptr, std::size_t i, Ice::Byte& out)
+ {
+ static const auto convert = MakeConverter<Ice::Byte>();
+ ARMARX_CHECK_LESS(i, GetNumberOfDataFields());
+ convert.at(i)(ptr, out);
+ }
+ template<class CommonBaseT, class ClassT>
+ void ClassMemberInfo<CommonBaseT, ClassT>::GetDataFieldAs(const ClassType* ptr, std::size_t i, Ice::Short& out)
+ {
+ static const auto convert = MakeConverter<Ice::Short>();
+ ARMARX_CHECK_LESS(i, GetNumberOfDataFields());
+ convert.at(i)(ptr, out);
+ }
+ template<class CommonBaseT, class ClassT>
+ void ClassMemberInfo<CommonBaseT, ClassT>::GetDataFieldAs(const ClassType* ptr, std::size_t i, Ice::Int& out)
+ {
+ static const auto convert = MakeConverter<Ice::Int>();
+ ARMARX_CHECK_LESS(i, GetNumberOfDataFields());
+ convert.at(i)(ptr, out);
+ }
+ template<class CommonBaseT, class ClassT>
+ void ClassMemberInfo<CommonBaseT, ClassT>::GetDataFieldAs(const ClassType* ptr, std::size_t i, Ice::Long& out)
+ {
+ static const auto convert = MakeConverter<Ice::Long>();
+ ARMARX_CHECK_LESS(i, GetNumberOfDataFields());
+ convert.at(i)(ptr, out);
+ }
+ template<class CommonBaseT, class ClassT>
+ void ClassMemberInfo<CommonBaseT, ClassT>::GetDataFieldAs(const ClassType* ptr, std::size_t i, Ice::Float& out)
+ {
+ static const auto convert = MakeConverter<Ice::Float>();
+ ARMARX_CHECK_LESS(i, GetNumberOfDataFields());
+ convert.at(i)(ptr, out);
+ }
+ template<class CommonBaseT, class ClassT>
+ void ClassMemberInfo<CommonBaseT, ClassT>::GetDataFieldAs(const ClassType* ptr, std::size_t i, Ice::Double& out)
+ {
+ static const auto convert = MakeConverter<Ice::Double>();
+ ARMARX_CHECK_LESS(i, GetNumberOfDataFields());
+ convert.at(i)(ptr, out);
+ }
+ template<class CommonBaseT, class ClassT>
+ void ClassMemberInfo<CommonBaseT, ClassT>::GetDataFieldAs(const ClassType* ptr, std::size_t i, std::string& out)
+ {
+ static const auto convert = MakeConverter<std::string>();
+ ARMARX_CHECK_LESS(i, GetNumberOfDataFields());
+ convert.at(i)(ptr, out);
+ }
+
+ template<class CommonBaseT, class ClassT>
+ const std::type_info& ClassMemberInfo<CommonBaseT, ClassT>::GetDataFieldType(std::size_t i)
{
- static const auto toString = []
+ static const auto convert = []
{
- std::vector<std::function<std::string(const ClassType*)>> functions;
+ std::vector<const std::type_info*> data;
for (std::size_t idxEntr = 0; idxEntr < GetEntries().size(); ++idxEntr)
{
+ const Entry& e = GetEntries().at(idxEntr);
for (std::size_t idxField = 0; idxField < GetEntries().at(idxEntr).getNumberOfFields(); ++idxField)
{
- functions.emplace_back(
- [idxEntr, idxField](const ClassType * classptr)
- {
- const Entry& e = GetEntries().at(idxEntr);
- return e.getFieldAsString(idxField, classptr);
- });
+ data.emplace_back(&e.getFieldTypeId(idxField));
}
}
- ARMARX_CHECK_EQUAL(functions.size(), GetNumberOfDataFields());
- return functions;
+ ARMARX_CHECK_EQUAL(data.size(), GetNumberOfDataFields());
+ return data;
}();
ARMARX_CHECK_LESS(i, GetNumberOfDataFields());
- return toString.at(i)(ptr);
+ return *convert.at(i);
}
}
diff --git a/source/RobotAPI/components/units/RobotUnit/util/introspection/ClassMemberInfoEntry.h b/source/RobotAPI/components/units/RobotUnit/util/introspection/ClassMemberInfoEntry.h
index 1f503de0562a64730dffb2c7f18749a7b7b63862..6188648d33e64d86dbac8a168446bbe5127e6339 100644
--- a/source/RobotAPI/components/units/RobotUnit/util/introspection/ClassMemberInfoEntry.h
+++ b/source/RobotAPI/components/units/RobotUnit/util/introspection/ClassMemberInfoEntry.h
@@ -47,7 +47,7 @@ namespace armarx::introspection
memberName {memberName},
memberTypeName {GetTypeString<MemberType>()},
numberOfFields {DataFieldsInfo<MemberType>::GetNumberOfFields()},
- fieldToString {MakeFieldToStringFunctors<ClassType>(numberOfFields, ptr)},
+ fieldToType {MakeFieldToTypeFunctors<ClassType>(numberOfFields, ptr)},
toVariants_ {MakeToVariantsFunctor<ClassType>(ptr)}
{
ARMARX_CHECK_NOT_EQUAL(0, numberOfFields);
@@ -91,11 +91,58 @@ namespace armarx::introspection
ARMARX_CHECK_LESS(i, numberOfFields);
return fieldNames.at(i);
}
- std::string getFieldAsString(std::size_t i, const CommonBaseType* ptr) const
+ void getDataFieldAs(std::size_t i, const CommonBaseType* ptr, bool& out) const
{
ARMARX_CHECK_NOT_NULL(ptr);
ARMARX_CHECK_LESS(i, numberOfFields);
- return fieldToString.at(i)(ptr);
+ return fieldToType.at(i).toBool(ptr, out);
+ }
+ void getDataFieldAs(std::size_t i, const CommonBaseType* ptr, Ice::Byte& out) const
+ {
+ ARMARX_CHECK_NOT_NULL(ptr);
+ ARMARX_CHECK_LESS(i, numberOfFields);
+ return fieldToType.at(i).toByte(ptr, out);
+ }
+ void getDataFieldAs(std::size_t i, const CommonBaseType* ptr, Ice::Short& out) const
+ {
+ ARMARX_CHECK_NOT_NULL(ptr);
+ ARMARX_CHECK_LESS(i, numberOfFields);
+ return fieldToType.at(i).toShort(ptr, out);
+ }
+ void getDataFieldAs(std::size_t i, const CommonBaseType* ptr, Ice::Int& out) const
+ {
+ ARMARX_CHECK_NOT_NULL(ptr);
+ ARMARX_CHECK_LESS(i, numberOfFields);
+ return fieldToType.at(i).toInt(ptr, out);
+ }
+ void getDataFieldAs(std::size_t i, const CommonBaseType* ptr, Ice::Long& out) const
+ {
+ ARMARX_CHECK_NOT_NULL(ptr);
+ ARMARX_CHECK_LESS(i, numberOfFields);
+ return fieldToType.at(i).toLong(ptr, out);
+ }
+ void getDataFieldAs(std::size_t i, const CommonBaseType* ptr, Ice::Float& out) const
+ {
+ ARMARX_CHECK_NOT_NULL(ptr);
+ ARMARX_CHECK_LESS(i, numberOfFields);
+ return fieldToType.at(i).toFloat(ptr, out);
+ }
+ void getDataFieldAs(std::size_t i, const CommonBaseType* ptr, Ice::Double& out) const
+ {
+ ARMARX_CHECK_NOT_NULL(ptr);
+ ARMARX_CHECK_LESS(i, numberOfFields);
+ return fieldToType.at(i).toDouble(ptr, out);
+ }
+ void getDataFieldAs(std::size_t i, const CommonBaseType* ptr, std::string& out) const
+ {
+ ARMARX_CHECK_NOT_NULL(ptr);
+ ARMARX_CHECK_LESS(i, numberOfFields);
+ return fieldToType.at(i).toString(ptr, out);
+ }
+ const std::type_info& getFieldTypeId(std::size_t i) const
+ {
+ ARMARX_CHECK_LESS(i, numberOfFields);
+ return *fieldToType.at(i).typeId;
}
//variants
@@ -110,25 +157,49 @@ namespace armarx::introspection
const ClassMemberInfoEntry* thisptr,
const IceUtil::Time&,
const CommonBaseType*) >;
- using FieldToStringFunctorType = std::function<std::string(const CommonBaseType*)>;
+ struct FieldToType
+ {
+ template<class T>
+ using FieldToFunctorType = std::function<void(const CommonBaseType*, T&)>;
+ FieldToFunctorType<bool> toBool;
+ FieldToFunctorType<Ice::Byte> toByte;
+ FieldToFunctorType<Ice::Short> toShort;
+ FieldToFunctorType<Ice::Int> toInt;
+ FieldToFunctorType<Ice::Long> toLong;
+ FieldToFunctorType<Ice::Float> toFloat;
+ FieldToFunctorType<Ice::Double> toDouble;
+ FieldToFunctorType<std::string> toString;
+ const std::type_info* typeId;
+ };
template<class ClassType, class MemberType, class MemberPtrClassType>
- static std::vector<FieldToStringFunctorType> MakeFieldToStringFunctors(
+ static std::vector<FieldToType> MakeFieldToTypeFunctors(
std::size_t numberOfFields,
MemberType MemberPtrClassType::* ptr)
{
- std::vector<std::function<std::string(const CommonBaseType*)>> result;
+ std::vector<FieldToType> result;
result.reserve(numberOfFields);
for (std::size_t i = 0; i < numberOfFields; ++i)
{
- result.emplace_back(
- [i, ptr](const CommonBaseType * ptrBase)
- {
- const ClassType* cptr = dynamic_cast<const ClassType*>(ptrBase);
- ARMARX_CHECK_NOT_NULL(cptr);
- return DataFieldsInfo<MemberType>::GetFieldAsString(i, cptr->*ptr);
- }
- );
+ result.emplace_back();
+ auto& fieldData = result.back();
+
+#define make_getter(Type) [i, ptr](const CommonBaseType * ptrBase, Type & out) \
+ { \
+ const ClassType* cptr = dynamic_cast<const ClassType*>(ptrBase); \
+ ARMARX_CHECK_NOT_NULL(cptr); \
+ DataFieldsInfo<MemberType>::GetDataFieldAs(i, cptr->*ptr, out); \
+ }
+ fieldData.toBool = make_getter(bool);
+ fieldData.toByte = make_getter(Ice::Byte);
+ fieldData.toShort = make_getter(Ice::Short);
+ fieldData.toInt = make_getter(Ice::Int);
+ fieldData.toLong = make_getter(Ice::Long);
+ fieldData.toFloat = make_getter(Ice::Float);
+ fieldData.toDouble = make_getter(Ice::Double);
+ fieldData.toString = make_getter(std::string);
+#undef make_getter
+ fieldData.typeId = &DataFieldsInfo<MemberType>::GetDataFieldType(i);
}
return result;
}
@@ -160,7 +231,7 @@ namespace armarx::introspection
const std::string memberTypeName;
//elementar subparts
const std::size_t numberOfFields;
- const std::vector<FieldToStringFunctorType> fieldToString;
+ const std::vector<FieldToType> fieldToType;
std::vector<std::string> fieldNames;
//variants
ToVariantsFunctorType toVariants_;
diff --git a/source/RobotAPI/components/units/RobotUnit/util/introspection/DataFieldsInfo.cpp b/source/RobotAPI/components/units/RobotUnit/util/introspection/DataFieldsInfo.cpp
index cf3a4e9b0ba5db52287a006a879c0b841778bf29..1ce573d80bd57a17cbacd7503435f1d3be67536e 100644
--- a/source/RobotAPI/components/units/RobotUnit/util/introspection/DataFieldsInfo.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/util/introspection/DataFieldsInfo.cpp
@@ -33,15 +33,31 @@
#include <RobotAPI/interface/units/KinematicUnitInterface.h>
#include <RobotAPI/interface/units/KinematicUnitInterfaceStdOverloads.h>
-
+//Eigen::Vector3f
namespace armarx::introspection
{
- std::string DataFieldsInfo<Eigen::Vector3f, void>::GetFieldAsString(std::size_t i, const Eigen::Vector3f& field)
+ std::size_t DataFieldsInfo<Eigen::Vector3f, void>::GetNumberOfFields()
+ {
+ return 3;
+ }
+ void DataFieldsInfo<Eigen::Vector3f, void>::GetDataFieldAs(std::size_t i, const Eigen::Vector3f& field, std::string& out)
{
ARMARX_CHECK_LESS(i, 3);
- return to_string(field(i));
+ out = to_string(field(i));
+ }
+ void DataFieldsInfo<Eigen::Vector3f, void>::GetDataFieldAs(std::size_t i, const Eigen::Vector3f& field, float& out)
+ {
+ ARMARX_CHECK_LESS(i, 3);
+ out = field(i);
+ }
+ const std::type_info& DataFieldsInfo<Eigen::Vector3f, void>::GetDataFieldType(std::size_t i)
+ {
+ return typeid(float);
+ }
+ std::vector<std::string> DataFieldsInfo<Eigen::Vector3f, void>::GetFieldNames()
+ {
+ return {"x", "y", "z"};
}
-
std::map<std::string, VariantBasePtr> DataFieldsInfo<Eigen::Vector3f, void>::ToVariants(
const Eigen::Vector3f& value,
const std::string& name,
@@ -57,125 +73,178 @@ namespace armarx::introspection
return {{name, {new TimedVariant(Vector3{value}, timestamp)}}};
}
}
-
+//Eigen::Vector##Num##Type
namespace armarx::introspection
{
-#define make_DataFieldsInfo_for_eigen_vector(Type,Num) \
- std::string DataFieldsInfo<Eigen::Vector##Num##Type, void>::GetFieldAsString(std::size_t i, const Eigen::Vector##Num##Type& field) \
- { \
- ARMARX_CHECK_LESS(i, Num); \
- return to_string(field(i)); \
- } \
- std::map<std::string, VariantBasePtr> DataFieldsInfo<Eigen::Vector##Num##Type, void>::ToVariants( \
- const Eigen::Vector##Num##Type& value, \
- const std::string& name, \
- const IceUtil::Time& timestamp, \
- const std::string& frame, \
- const std::string& agent) \
- { \
- ARMARX_CHECK_EXPRESSION(frame.empty() && agent.empty()) << "There is no framed version of TimestampVariant"; \
- std::map<std::string, VariantBasePtr> result; \
- for(int i = 0; i < Num; ++i) \
- { \
- result.emplace(name + "." + to_string(i), VariantBasePtr{new TimedVariant(value(i), timestamp)}); \
- } \
- return result; \
- } \
- std::vector<std::string> DataFieldsInfo<Eigen::Vector##Num##Type, void>::GetFieldNames() \
- { \
- std::vector<std::string> result; \
- for(int i = 0; i < Num; ++i) \
- { \
- result.emplace_back(to_string(i)); \
- } \
- return result; \
+#define make_DataFieldsInfo_for_eigen_vector(Type,TypeName,Num) \
+ void DataFieldsInfo<Eigen::Vector##Num##Type, void>::GetDataFieldAs(std::size_t i, const Eigen::Vector##Num##Type& field, std::string& out) \
+ { \
+ ARMARX_CHECK_LESS(i, Num); \
+ out = to_string(field(i)); \
+ } \
+ void DataFieldsInfo<Eigen::Vector##Num##Type, void>::GetDataFieldAs(std::size_t i, const Eigen::Vector##Num##Type& field, Ice::TypeName& out) \
+ { \
+ ARMARX_CHECK_LESS(i, Num); \
+ out = field(i); \
+ } \
+ const std::type_info& DataFieldsInfo<Eigen::Vector##Num##Type, void>::GetDataFieldType(std::size_t i) \
+ { \
+ return typeid(Ice::TypeName); \
+ } \
+ std::map<std::string, VariantBasePtr> DataFieldsInfo<Eigen::Vector##Num##Type, void>::ToVariants( \
+ const Eigen::Vector##Num##Type& value, \
+ const std::string& name, \
+ const IceUtil::Time& timestamp, \
+ const std::string& frame, \
+ const std::string& agent) \
+ { \
+ ARMARX_CHECK_EXPRESSION(frame.empty() && agent.empty()) << "There is no framed version of TimestampVariant"; \
+ std::map<std::string, VariantBasePtr> result; \
+ for(int i = 0; i < Num; ++i) \
+ { \
+ result.emplace(name + "." + to_string(i), VariantBasePtr{new TimedVariant(value(i), timestamp)}); \
+ } \
+ return result; \
+ } \
+ std::vector<std::string> DataFieldsInfo<Eigen::Vector##Num##Type, void>::GetFieldNames() \
+ { \
+ std::vector<std::string> result; \
+ for(int i = 0; i < Num; ++i) \
+ { \
+ result.emplace_back(to_string(i)); \
+ } \
+ return result; \
}
- make_DataFieldsInfo_for_eigen_vector(f, 2)
- make_DataFieldsInfo_for_eigen_vector(f, 4)
- make_DataFieldsInfo_for_eigen_vector(f, 5)
- make_DataFieldsInfo_for_eigen_vector(f, 6)
+ make_DataFieldsInfo_for_eigen_vector(f, Float, 2)
+ make_DataFieldsInfo_for_eigen_vector(f, Float, 4)
+ make_DataFieldsInfo_for_eigen_vector(f, Float, 5)
+ make_DataFieldsInfo_for_eigen_vector(f, Float, 6)
- make_DataFieldsInfo_for_eigen_vector(d, 2)
- make_DataFieldsInfo_for_eigen_vector(d, 3)
- make_DataFieldsInfo_for_eigen_vector(d, 4)
- make_DataFieldsInfo_for_eigen_vector(d, 5)
- make_DataFieldsInfo_for_eigen_vector(d, 6)
+ make_DataFieldsInfo_for_eigen_vector(d, Double, 2)
+ make_DataFieldsInfo_for_eigen_vector(d, Double, 3)
+ make_DataFieldsInfo_for_eigen_vector(d, Double, 4)
+ make_DataFieldsInfo_for_eigen_vector(d, Double, 5)
+ make_DataFieldsInfo_for_eigen_vector(d, Double, 6)
- make_DataFieldsInfo_for_eigen_vector(i, 2)
- make_DataFieldsInfo_for_eigen_vector(i, 3)
- make_DataFieldsInfo_for_eigen_vector(i, 4)
- make_DataFieldsInfo_for_eigen_vector(i, 5)
- make_DataFieldsInfo_for_eigen_vector(i, 6)
+ make_DataFieldsInfo_for_eigen_vector(i, Int, 2)
+ make_DataFieldsInfo_for_eigen_vector(i, Int, 3)
+ make_DataFieldsInfo_for_eigen_vector(i, Int, 4)
+ make_DataFieldsInfo_for_eigen_vector(i, Int, 5)
+ make_DataFieldsInfo_for_eigen_vector(i, Int, 6)
#undef make_DataFieldsInfo_for_eigen_vector
}
-
+//Eigen::Matrix4f
namespace armarx::introspection
{
- std::string DataFieldsInfo<JointStatus, void>::GetFieldAsString(
- std::size_t i, const JointStatus& field)
+ std::size_t DataFieldsInfo<Eigen::Matrix4f, void>::GetNumberOfFields()
+ {
+ return 16;
+ }
+ void DataFieldsInfo<Eigen::Matrix4f, void>::GetDataFieldAs(std::size_t i, const Eigen::Matrix4f& field, float& out)
+ {
+ ARMARX_CHECK_LESS(i, 16);
+ out = field(i / 4, i % 4);
+ }
+ void DataFieldsInfo<Eigen::Matrix4f, void>::GetDataFieldAs(std::size_t i, const Eigen::Matrix4f& field, std::string& out)
+ {
+ ARMARX_CHECK_LESS(i, 16);
+ out = to_string(field(i / 4, i % 4));
+ }
+ const std::type_info& DataFieldsInfo<Eigen::Matrix4f, void>::GetDataFieldType(std::size_t i)
+ {
+ return typeid(float);
+ }
+ std::vector<std::string> DataFieldsInfo<Eigen::Matrix4f, void>::GetFieldNames()
+ {
+ return
+ {
+ "00", "01", "02", "03",
+ "10", "11", "12", "13",
+ "20", "21", "22", "23",
+ "30", "31", "32", "33"
+ };
+ }
+ std::map<std::string, VariantBasePtr> DataFieldsInfo<Eigen::Matrix4f, void>::ToVariants(
+ const Eigen::Matrix4f& value,
+ const std::string& name,
+ const IceUtil::Time& timestamp,
+ const std::string& frame,
+ const std::string& agent)
+ {
+ if (!frame.empty())
+ {
+ return {{name, {new TimedVariant(FramedPose{value, frame, agent}, timestamp)}}};
+ }
+ ARMARX_CHECK_EXPRESSION(agent.empty()) << "No frame but an agent given";
+ return {{name, {new TimedVariant(Pose{value}, timestamp)}}};
+ }
+}
+//Eigen::Quaternionf
+namespace armarx::introspection
+{
+ std::size_t DataFieldsInfo<Eigen::Quaternionf, void>::GetNumberOfFields()
+ {
+ return 4;
+ }
+ void DataFieldsInfo<Eigen::Quaternionf, void>::GetDataFieldAs(std::size_t i, const Eigen::Quaternionf& field, float& out)
{
ARMARX_CHECK_LESS(i, 4);
switch (i)
{
case 0:
- return to_string(field.error);
+ out = field.w();
+ return;
case 1:
- return to_string(field.operation);
+ out = field.x();
+ return;
case 2:
- return to_string(field.enabled);
+ out = field.y();
+ return;
case 3:
- return to_string(field.emergencyStop);
+ out = field.z();
+ return;
}
throw std::logic_error
{
__FILE__ " : " + to_string(__LINE__) +
" : Unreachable code reached"
};
- }
- std::map<std::string, VariantBasePtr> DataFieldsInfo<JointStatus, void>::ToVariants(
- const JointStatus& value,
- const std::string& name,
- const IceUtil::Time& timestamp,
- const std::string& frame,
- const std::string& agent)
- {
- ARMARX_CHECK_EXPRESSION(frame.empty() && agent.empty()) << "There is no framed version for JointStatus";
- static const auto fnames = GetFieldNames();
- return
- {
- {name + fnames.at(0), {new TimedVariant{to_string(value.error), timestamp}}},
- {name + fnames.at(1), {new TimedVariant{to_string(value.operation), timestamp}}},
- {name + fnames.at(2), {new TimedVariant{value.enabled, timestamp}}},
- {name + fnames.at(3), {new TimedVariant{value.emergencyStop, timestamp}}}
- };
}
-}
-
-namespace armarx::introspection
-{
- std::string DataFieldsInfo<Eigen::Quaternionf, void>::GetFieldAsString(
- std::size_t i,
- const Eigen::Quaternionf& field)
+ void DataFieldsInfo<Eigen::Quaternionf, void>::GetDataFieldAs(std::size_t i, const Eigen::Quaternionf& field, std::string& out)
{
ARMARX_CHECK_LESS(i, 4);
switch (i)
{
case 0:
- return to_string(field.w());
+ out = to_string(field.w());
+ return;
case 1:
- return to_string(field.x());
+ out = to_string(field.x());
+ return;
case 2:
- return to_string(field.y());
+ out = to_string(field.y());
+ return;
case 3:
- return to_string(field.z());
+ out = to_string(field.z());
+ return;
}
throw std::logic_error
{
__FILE__ " : " + to_string(__LINE__) +
" : Unreachable code reached"
};
+
+ }
+
+ const std::type_info& DataFieldsInfo<Eigen::Quaternionf, void>::GetDataFieldType(std::size_t i)
+ {
+ return typeid(float);
+ }
+ std::vector<std::string> DataFieldsInfo<Eigen::Quaternionf, void>::GetFieldNames()
+ {
+ return {"qw", "qx", "qy", "qz"};
}
std::map<std::string, VariantBasePtr> DataFieldsInfo<Eigen::Quaternionf, void>::ToVariants(
@@ -193,33 +262,27 @@ namespace armarx::introspection
return {{name, {new TimedVariant(Quaternion{value}, timestamp)}}};
}
}
-
+//std::chrono::microseconds
namespace armarx::introspection
{
- std::string DataFieldsInfo<Eigen::Matrix4f, void>::GetFieldAsString(std::size_t i, const Eigen::Matrix4f& field)
+ std::size_t DataFieldsInfo<std::chrono::microseconds, void>::GetNumberOfFields()
{
- ARMARX_CHECK_LESS(i, 16);
- return to_string(field(i / 4, i % 4));
+ return 1;
}
-
- std::map<std::string, VariantBasePtr> DataFieldsInfo<Eigen::Matrix4f, void>::ToVariants(
- const Eigen::Matrix4f& value,
- const std::string& name,
- const IceUtil::Time& timestamp,
- const std::string& frame,
- const std::string& agent)
+ void DataFieldsInfo<std::chrono::microseconds, void>::GetDataFieldAs(std::size_t i, const std::chrono::microseconds& field, long& out)
{
- if (!frame.empty())
- {
- return {{name, {new TimedVariant(FramedPose{value, frame, agent}, timestamp)}}};
- }
- ARMARX_CHECK_EXPRESSION(agent.empty()) << "No frame but an agent given";
- return {{name, {new TimedVariant(Pose{value}, timestamp)}}};
+ ARMARX_CHECK_EQUAL(i, 0);
+ out = field.count();
+ }
+ void DataFieldsInfo<std::chrono::microseconds, void>::GetDataFieldAs(std::size_t i, const std::chrono::microseconds& field, std::string& out)
+ {
+ ARMARX_CHECK_EQUAL(i, 0);
+ out = to_string(field.count());
+ }
+ const std::type_info& GetDataFieldType(std::size_t i)
+ {
+ return typeid(long);
}
-}
-
-namespace armarx::introspection
-{
std::map<std::string, VariantBasePtr> DataFieldsInfo<std::chrono::microseconds, void>::ToVariants(
std::chrono::microseconds value,
const std::string& name,
@@ -231,6 +294,28 @@ namespace armarx::introspection
return {{name, {new TimedVariant(TimestampVariant{value.count()}, timestamp)}}};
}
+}
+//IceUtil::Time
+namespace armarx::introspection
+{
+ std::size_t DataFieldsInfo<IceUtil::Time, void>::GetNumberOfFields()
+ {
+ return 1;
+ }
+ void DataFieldsInfo<IceUtil::Time, void>::GetDataFieldAs(std::size_t i, const IceUtil::Time& field, long& out)
+ {
+ ARMARX_CHECK_EQUAL(i, 0);
+ out = field.toMicroSeconds();
+ }
+ void DataFieldsInfo<IceUtil::Time, void>::GetDataFieldAs(std::size_t i, const IceUtil::Time& field, std::string& out)
+ {
+ ARMARX_CHECK_EQUAL(i, 0);
+ out = to_string(field.toMicroSeconds());
+ }
+ const std::type_info& DataFieldsInfo<IceUtil::Time, void>::GetDataFieldType(std::size_t i)
+ {
+ return typeid(long);
+ }
std::map<std::string, VariantBasePtr> DataFieldsInfo<IceUtil::Time, void>::ToVariants(
IceUtil::Time value,
const std::string& name,
@@ -242,3 +327,117 @@ namespace armarx::introspection
return {{name, {new TimedVariant(TimestampVariant{value.toMicroSeconds()}, timestamp)}}};
}
}
+//JointStatus
+namespace armarx::introspection
+{
+ std::size_t DataFieldsInfo<JointStatus, void>::GetNumberOfFields()
+ {
+ return 4;
+ }
+ void DataFieldsInfo<JointStatus, void>::GetDataFieldAs(
+ std::size_t i, const JointStatus& field, std::string& out)
+ {
+ ARMARX_CHECK_LESS(i, 4);
+ switch (i)
+ {
+ case 0:
+ out = to_string(field.error);
+ return;
+ case 1:
+ out = to_string(field.operation);
+ return;
+ case 2:
+ out = to_string(field.enabled);
+ return;
+ case 3:
+ out = to_string(field.emergencyStop);
+ return;
+ }
+ throw std::logic_error
+ {
+ __FILE__ " : " + to_string(__LINE__) +
+ " : Unreachable code reached"
+ };
+ }
+ void DataFieldsInfo<JointStatus, void>::GetDataFieldAs(
+ std::size_t i, const JointStatus& field, Ice::Int& out)
+ {
+ ARMARX_CHECK_EXPRESSION(i == 0 || i == 1);
+ switch (i)
+ {
+ case 0:
+ out = field.enabled;
+ return;
+ case 1:
+ out = field.emergencyStop;
+ return;
+ }
+ throw std::logic_error
+ {
+ __FILE__ " : " + to_string(__LINE__) +
+ " : Unreachable code reached"
+ };
+ }
+ void DataFieldsInfo<JointStatus, void>::GetDataFieldAs(
+ std::size_t i, const JointStatus& field, bool& out)
+ {
+ ARMARX_CHECK_EXPRESSION(i == 2 || i == 3);
+ switch (i)
+ {
+ case 2:
+ out = static_cast<Ice::Int>(field.error);
+ return;
+ case 3:
+ out = static_cast<Ice::Int>(field.operation);
+ return;
+ }
+ throw std::logic_error
+ {
+ __FILE__ " : " + to_string(__LINE__) +
+ " : Unreachable code reached"
+ };
+ }
+ const std::type_info& DataFieldsInfo<JointStatus, void>::GetDataFieldType(std::size_t i)
+ {
+ ARMARX_CHECK_LESS(i, 4);
+ switch (i)
+ {
+ case 0:
+ return typeid(Ice::Int);
+ case 1:
+ return typeid(Ice::Int);
+ case 2:
+ return typeid(bool);
+ case 3:
+ return typeid(bool);
+ }
+ throw std::logic_error
+ {
+ __FILE__ " : " + to_string(__LINE__) +
+ " : Unreachable code reached"
+ };
+ }
+
+ std::vector<std::string> DataFieldsInfo<JointStatus, void>::GetFieldNames()
+ {
+ return {"error", "operation", "enabled", "emergencyStop"};
+ }
+
+ std::map<std::string, VariantBasePtr> DataFieldsInfo<JointStatus, void>::ToVariants(
+ const JointStatus& value,
+ const std::string& name,
+ const IceUtil::Time& timestamp,
+ const std::string& frame,
+ const std::string& agent)
+ {
+ ARMARX_CHECK_EXPRESSION(frame.empty() && agent.empty()) << "There is no framed version for JointStatus";
+ static const auto fnames = GetFieldNames();
+ return
+ {
+ {name + fnames.at(0), {new TimedVariant{to_string(value.error), timestamp}}},
+ {name + fnames.at(1), {new TimedVariant{to_string(value.operation), timestamp}}},
+ {name + fnames.at(2), {new TimedVariant{value.enabled, timestamp}}},
+ {name + fnames.at(3), {new TimedVariant{value.emergencyStop, timestamp}}}
+ };
+ }
+}
diff --git a/source/RobotAPI/components/units/RobotUnit/util/introspection/DataFieldsInfo.h b/source/RobotAPI/components/units/RobotUnit/util/introspection/DataFieldsInfo.h
index c122c55b0361fbe82f7e7ce6bbffcb2fdbf9bc84..16d76a7b7bba97433af9a97e26c4fe66c387b051 100644
--- a/source/RobotAPI/components/units/RobotUnit/util/introspection/DataFieldsInfo.h
+++ b/source/RobotAPI/components/units/RobotUnit/util/introspection/DataFieldsInfo.h
@@ -35,7 +35,15 @@ namespace armarx::introspection
{
template<class T, class = void> struct DataFieldsInfo;
// static std::size_t GetNumberOfFields();
- // static std::string GetFieldAsString(std::size_t i, T field);
+ // static void GetDataFieldAs(std::size_t i, T field, bool& out);
+ // static void GetDataFieldAs(std::size_t i, T field, Ice::Byte& out);
+ // static void GetDataFieldAs(std::size_t i, T field, Ice::Short& out);
+ // static void GetDataFieldAs(std::size_t i, T field, Ice::Int& out);
+ // static void GetDataFieldAs(std::size_t i, T field, Ice::Long& out);
+ // static void GetDataFieldAs(std::size_t i, T field, Ice::Float& out);
+ // static void GetDataFieldAs(std::size_t i, T field, Ice::Double& out);
+ // static void GetDataFieldAs(std::size_t i, T field, std::string& out);
+ // static const std::type_info& GetDataFieldType(std::size_t i);
// static std::vector<std::string> GetFieldNames();
// static std::map<std::string, VariantBasePtr> ToVariants(
// T value,
@@ -45,45 +53,106 @@ namespace armarx::introspection
// const std::string& agent = "")
template<class T>
- struct DataFieldsInfo<T, typename std::enable_if<std::is_fundamental<T>::value>::type>
+ struct DataFieldsInfoBase
{
- static std::size_t GetNumberOfFields()
+ static void GetDataFieldAs(std::size_t i, const T& field, bool& out)
{
- return 1;
+ throw std::logic_error {"This function should never be called"};
}
- static std::string GetFieldAsString(std::size_t i, T field)
+ static void GetDataFieldAs(std::size_t i, const T& field, Ice::Byte& out)
{
- ARMARX_CHECK_EQUAL(i, 0);
- return to_string(field);
+ throw std::logic_error {"This function should never be called"};
}
- static std::vector<std::string> GetFieldNames()
+ static void GetDataFieldAs(std::size_t i, const T& field, Ice::Short& out)
{
- throw std::logic_error {"Fundamental types have no field names"};
+ throw std::logic_error {"This function should never be called"};
}
- static std::map<std::string, VariantBasePtr> ToVariants(
- T value,
- const std::string& name,
- const IceUtil::Time& timestamp,
- const std::string& frame = "",
- const std::string& agent = "")
+ static void GetDataFieldAs(std::size_t i, const T& field, Ice::Int& out)
{
- ARMARX_CHECK_EXPRESSION(frame.empty() && agent.empty()) << "There is no framed version for fundamental types";
- return {{name, {new TimedVariant(value, timestamp)}}};
+ throw std::logic_error {"This function should never be called"};
}
- };
-
- template<>
- struct DataFieldsInfo<Eigen::Vector3f, void>
- {
- static std::size_t GetNumberOfFields()
+ static void GetDataFieldAs(std::size_t i, const T& field, Ice::Long& out)
+ {
+ throw std::logic_error {"This function should never be called"};
+ }
+ static void GetDataFieldAs(std::size_t i, const T& field, Ice::Float& out)
+ {
+ throw std::logic_error {"This function should never be called"};
+ }
+ static void GetDataFieldAs(std::size_t i, const T& field, Ice::Double& out)
+ {
+ throw std::logic_error {"This function should never be called"};
+ }
+ static void GetDataFieldAs(std::size_t i, const T& field, std::string& out)
{
- return 3;
+ throw std::logic_error {"This function should never be called"};
}
- static std::string GetFieldAsString(std::size_t i, const Eigen::Vector3f& field);
static std::vector<std::string> GetFieldNames()
{
- return {"x", "y", "z"};
+ throw std::logic_error {"This function should never be called"};
}
+ };
+}
+//build_in_ice_types
+namespace armarx::introspection
+{
+#define make_def_for_build_in_ice_type(Type) \
+ template<> \
+ struct DataFieldsInfo<Type, void> : DataFieldsInfoBase<Type> \
+ { \
+ using DataFieldsInfoBase<Type>::GetDataFieldAs; \
+ static std::size_t GetNumberOfFields() \
+ { \
+ return 1; \
+ } \
+ static void GetDataFieldAs(std::size_t i, const Type& field, std::string& out) \
+ { \
+ ARMARX_CHECK_EQUAL(i, 0); \
+ out = to_string(field); \
+ } \
+ static void GetDataFieldAs(std::size_t i, const Type& field, Type& out) \
+ { \
+ ARMARX_CHECK_EQUAL(i, 0); \
+ out = field; \
+ } \
+ static const std::type_info& GetDataFieldType(std::size_t i) \
+ { \
+ return typeid(Type); \
+ } \
+ static std::map<std::string, VariantBasePtr> ToVariants( \
+ const Type& value, \
+ const std::string& name, \
+ const IceUtil::Time& timestamp, \
+ const std::string& frame = "", \
+ const std::string& agent = "") \
+ { \
+ ARMARX_CHECK_EXPRESSION(frame.empty() && agent.empty()) \
+ << "There is no framed version for build in ice types"; \
+ return {{name, {new TimedVariant(value, timestamp)}}}; \
+ } \
+ }
+ make_def_for_build_in_ice_type(bool);
+ make_def_for_build_in_ice_type(Ice::Byte);
+ make_def_for_build_in_ice_type(Ice::Short);
+ make_def_for_build_in_ice_type(Ice::Int);
+ make_def_for_build_in_ice_type(Ice::Long);
+ make_def_for_build_in_ice_type(Ice::Float);
+ make_def_for_build_in_ice_type(Ice::Double);
+#undef make_def_for_build_in_ice_type
+
+}
+//Eigen::Vector3f
+namespace armarx::introspection
+{
+ template<>
+ struct DataFieldsInfo<Eigen::Vector3f, void> : DataFieldsInfoBase<Eigen::Vector3f>
+ {
+ using DataFieldsInfoBase<Eigen::Vector3f>::GetDataFieldAs;
+ static std::size_t GetNumberOfFields();
+ static void GetDataFieldAs(std::size_t i, const Eigen::Vector3f& field, float& out);
+ static void GetDataFieldAs(std::size_t i, const Eigen::Vector3f& field, std::string& out);
+ static const std::type_info& GetDataFieldType(std::size_t i);
+ static std::vector<std::string> GetFieldNames();
static std::map<std::string, VariantBasePtr> ToVariants(
const Eigen::Vector3f& value,
const std::string& name,
@@ -91,20 +160,24 @@ namespace armarx::introspection
const std::string& frame = "",
const std::string& agent = "");
};
-
-
-
-#define make_DataFieldsInfo_for_eigen_vector(Type,Num) \
- template<> \
- struct DataFieldsInfo<Eigen::Vector##Num##Type, void> \
- { \
- static std::size_t GetNumberOfFields() \
- { \
- return Num; \
- } \
- static std::string GetFieldAsString(std::size_t i, const Eigen::Vector##Num##Type& field); \
- static std::vector<std::string> GetFieldNames(); \
- static std::map<std::string, VariantBasePtr> ToVariants( \
+}
+//Eigen::Vector##Num##Type
+namespace armarx::introspection
+{
+#define make_DataFieldsInfo_for_eigen_vector(Type,TypeName,Num) \
+ template<> \
+ struct DataFieldsInfo<Eigen::Vector##Num##Type, void> : DataFieldsInfoBase<Eigen::Vector##Num##Type> \
+ { \
+ using DataFieldsInfoBase<Eigen::Vector##Num##Type>::GetDataFieldAs; \
+ static std::size_t GetNumberOfFields() \
+ { \
+ return Num; \
+ } \
+ static void GetDataFieldAs(std::size_t i, const Eigen::Vector##Num##Type& field, std::string& out); \
+ static void GetDataFieldAs(std::size_t i, const Eigen::Vector##Num##Type& field, Ice::TypeName& out); \
+ static const std::type_info& GetDataFieldType(std::size_t i); \
+ static std::vector<std::string> GetFieldNames(); \
+ static std::map<std::string, VariantBasePtr> ToVariants( \
const Eigen::Vector##Num##Type& value, \
const std::string& name, \
const IceUtil::Time& timestamp, \
@@ -112,42 +185,37 @@ namespace armarx::introspection
const std::string& agent = ""); \
};
- make_DataFieldsInfo_for_eigen_vector(f, 2)
- make_DataFieldsInfo_for_eigen_vector(f, 4)
- make_DataFieldsInfo_for_eigen_vector(f, 5)
- make_DataFieldsInfo_for_eigen_vector(f, 6)
+ make_DataFieldsInfo_for_eigen_vector(f, Float, 2)
+ make_DataFieldsInfo_for_eigen_vector(f, Float, 4)
+ make_DataFieldsInfo_for_eigen_vector(f, Float, 5)
+ make_DataFieldsInfo_for_eigen_vector(f, Float, 6)
- make_DataFieldsInfo_for_eigen_vector(d, 2)
- make_DataFieldsInfo_for_eigen_vector(d, 3)
- make_DataFieldsInfo_for_eigen_vector(d, 4)
- make_DataFieldsInfo_for_eigen_vector(d, 5)
- make_DataFieldsInfo_for_eigen_vector(d, 6)
+ make_DataFieldsInfo_for_eigen_vector(d, Double, 2)
+ make_DataFieldsInfo_for_eigen_vector(d, Double, 3)
+ make_DataFieldsInfo_for_eigen_vector(d, Double, 4)
+ make_DataFieldsInfo_for_eigen_vector(d, Double, 5)
+ make_DataFieldsInfo_for_eigen_vector(d, Double, 6)
- make_DataFieldsInfo_for_eigen_vector(i, 2)
- make_DataFieldsInfo_for_eigen_vector(i, 3)
- make_DataFieldsInfo_for_eigen_vector(i, 4)
- make_DataFieldsInfo_for_eigen_vector(i, 5)
- make_DataFieldsInfo_for_eigen_vector(i, 6)
+ make_DataFieldsInfo_for_eigen_vector(i, Int, 2)
+ make_DataFieldsInfo_for_eigen_vector(i, Int, 3)
+ make_DataFieldsInfo_for_eigen_vector(i, Int, 4)
+ make_DataFieldsInfo_for_eigen_vector(i, Int, 5)
+ make_DataFieldsInfo_for_eigen_vector(i, Int, 6)
#undef make_DataFieldsInfo_for_eigen_vector
+}
+//Eigen::Matrix4f
+namespace armarx::introspection
+{
template<>
- struct DataFieldsInfo<Eigen::Matrix4f, void>
+ struct DataFieldsInfo<Eigen::Matrix4f, void> : DataFieldsInfoBase<Eigen::Matrix4f>
{
- static std::size_t GetNumberOfFields()
- {
- return 16;
- }
- static std::string GetFieldAsString(std::size_t i, const Eigen::Matrix4f& field);
- static std::vector<std::string> GetFieldNames()
- {
- return
- {
- "00", "01", "02", "03",
- "10", "11", "12", "13",
- "20", "21", "22", "23",
- "30", "31", "32", "33"
- };
- }
+ using DataFieldsInfoBase<Eigen::Matrix4f>::GetDataFieldAs;
+ static std::size_t GetNumberOfFields();
+ static void GetDataFieldAs(std::size_t i, const Eigen::Matrix4f& field, float& out);
+ static void GetDataFieldAs(std::size_t i, const Eigen::Matrix4f& field, std::string& out);
+ static const std::type_info& GetDataFieldType(std::size_t i);
+ static std::vector<std::string> GetFieldNames();
static std::map<std::string, VariantBasePtr> ToVariants(
const Eigen::Matrix4f& value,
const std::string& name,
@@ -155,19 +223,19 @@ namespace armarx::introspection
const std::string& frame = "",
const std::string& agent = "");
};
-
+}
+//Eigen::Quaternionf
+namespace armarx::introspection
+{
template<>
- struct DataFieldsInfo<Eigen::Quaternionf, void>
+ struct DataFieldsInfo<Eigen::Quaternionf, void> : DataFieldsInfoBase<Eigen::Quaternionf>
{
- static std::size_t GetNumberOfFields()
- {
- return 4;
- }
- static std::string GetFieldAsString(std::size_t i, const Eigen::Quaternionf& field);
- static std::vector<std::string> GetFieldNames()
- {
- return {"qw", "qx", "qy", "qz"};
- }
+ using DataFieldsInfoBase<Eigen::Quaternionf>::GetDataFieldAs;
+ static std::size_t GetNumberOfFields();
+ static void GetDataFieldAs(std::size_t i, const Eigen::Quaternionf& field, float& out);
+ static void GetDataFieldAs(std::size_t i, const Eigen::Quaternionf& field, std::string& out);
+ static const std::type_info& GetDataFieldType(std::size_t i);
+ static std::vector<std::string> GetFieldNames();
static std::map<std::string, VariantBasePtr> ToVariants(
const Eigen::Quaternionf& value,
const std::string& name,
@@ -175,22 +243,18 @@ namespace armarx::introspection
const std::string& frame = "",
const std::string& agent = "");
};
-
+}
+//std::chrono::microseconds
+namespace armarx::introspection
+{
template<>
- struct DataFieldsInfo<std::chrono::microseconds, void>
+ struct DataFieldsInfo<std::chrono::microseconds, void> : DataFieldsInfoBase<std::chrono::microseconds>
{
- static std::size_t GetNumberOfFields()
- {
- return 1;
- }
- static std::string GetFieldAsString(std::size_t i, std::chrono::microseconds field)
- {
- return to_string(field.count());
- }
- static std::vector<std::string> GetFieldNames()
- {
- throw std::logic_error {"should never be called"};
- }
+ using DataFieldsInfoBase<std::chrono::microseconds>::GetDataFieldAs;
+ static std::size_t GetNumberOfFields();
+ static void GetDataFieldAs(std::size_t i, const std::chrono::microseconds& field, long& out);
+ static void GetDataFieldAs(std::size_t i, const std::chrono::microseconds& field, std::string& out);
+ static const std::type_info& GetDataFieldType(std::size_t i);
static std::map<std::string, VariantBasePtr> ToVariants(
std::chrono::microseconds value,
const std::string& name,
@@ -198,21 +262,18 @@ namespace armarx::introspection
const std::string& frame = "",
const std::string& agent = "");
};
+}
+//IceUtil::Time
+namespace armarx::introspection
+{
template<>
- struct DataFieldsInfo<IceUtil::Time, void>
+ struct DataFieldsInfo<IceUtil::Time, void> : DataFieldsInfoBase<IceUtil::Time>
{
- static std::size_t GetNumberOfFields()
- {
- return 1;
- }
- static std::string GetFieldAsString(std::size_t i, IceUtil::Time field)
- {
- return to_string(field.toMicroSeconds());
- }
- static std::vector<std::string> GetFieldNames()
- {
- throw std::logic_error {"should never be called"};
- }
+ using DataFieldsInfoBase<IceUtil::Time>::GetDataFieldAs;
+ static std::size_t GetNumberOfFields();
+ static void GetDataFieldAs(std::size_t i, const IceUtil::Time& field, long& out);
+ static void GetDataFieldAs(std::size_t i, const IceUtil::Time& field, std::string& out);
+ static const std::type_info& GetDataFieldType(std::size_t i);
static std::map<std::string, VariantBasePtr> ToVariants(
IceUtil::Time value,
const std::string& name,
@@ -221,26 +282,23 @@ namespace armarx::introspection
const std::string& agent = "");
};
}
-
+//JointStatus
namespace armarx
{
struct JointStatus;
}
-
namespace armarx::introspection
{
template<>
- struct DataFieldsInfo<JointStatus, void>
+ struct DataFieldsInfo<JointStatus, void> : DataFieldsInfoBase<JointStatus>
{
- static std::size_t GetNumberOfFields()
- {
- return 4;
- }
- static std::string GetFieldAsString(std::size_t i, const JointStatus& field);
- static std::vector<std::string> GetFieldNames()
- {
- return {"error", "operation", "enabled", "emergencyStop"};
- }
+ using DataFieldsInfoBase<JointStatus>::GetDataFieldAs;
+ static std::size_t GetNumberOfFields();
+ static void GetDataFieldAs(std::size_t i, const JointStatus& field, std::string& out);
+ static void GetDataFieldAs(std::size_t i, const JointStatus& field, Ice::Int& out);
+ static void GetDataFieldAs(std::size_t i, const JointStatus& field, bool& out);
+ static const std::type_info& GetDataFieldType(std::size_t i);
+ static std::vector<std::string> GetFieldNames();
static std::map<std::string, VariantBasePtr> ToVariants(
const JointStatus& value,
@@ -250,4 +308,3 @@ namespace armarx::introspection
const std::string& agent);
};
}
-
diff --git a/source/RobotAPI/drivers/GamepadUnit/CMakeLists.txt b/source/RobotAPI/drivers/GamepadUnit/CMakeLists.txt
index 26d2a25a68efa5bbaee41fbf4e930c8f155fbd85..dd8ee4c48f70aad699de65a0520440932e724684 100644
--- a/source/RobotAPI/drivers/GamepadUnit/CMakeLists.txt
+++ b/source/RobotAPI/drivers/GamepadUnit/CMakeLists.txt
@@ -9,3 +9,5 @@ armarx_add_component("${SOURCES}" "${HEADERS}")
# add unit tests
add_subdirectory(test)
+
+armarx_generate_and_add_component_executable(APPLICATION_APP_SUFFIX)
diff --git a/source/RobotAPI/drivers/HokuyoLaserUnit/CMakeLists.txt b/source/RobotAPI/drivers/HokuyoLaserUnit/CMakeLists.txt
index fca70b9a3b847f7d8c3c73da2b2570740cbb1454..2edd677f1a3c9bc022d0cc587dff6d1dca3ce833 100644
--- a/source/RobotAPI/drivers/HokuyoLaserUnit/CMakeLists.txt
+++ b/source/RobotAPI/drivers/HokuyoLaserUnit/CMakeLists.txt
@@ -14,3 +14,5 @@ armarx_add_component("${SOURCES}" "${HEADERS}")
# add unit tests
add_subdirectory(test)
+
+armarx_generate_and_add_component_executable(APPLICATION_APP_SUFFIX)
diff --git a/source/RobotAPI/drivers/MetaWearIMU/CMakeLists.txt b/source/RobotAPI/drivers/MetaWearIMU/CMakeLists.txt
index 958bc4a7eafa19275d275edfe2b7290abb5e2f4d..974366720a630284932bc1620bfac3c2281bbabe 100644
--- a/source/RobotAPI/drivers/MetaWearIMU/CMakeLists.txt
+++ b/source/RobotAPI/drivers/MetaWearIMU/CMakeLists.txt
@@ -7,3 +7,4 @@ set(HEADERS MetaWearIMU.h)
armarx_add_component("${SOURCES}" "${HEADERS}")
+armarx_generate_and_add_component_executable(APPLICATION_APP_SUFFIX)
diff --git a/source/RobotAPI/drivers/OptoForceUnit/CMakeLists.txt b/source/RobotAPI/drivers/OptoForceUnit/CMakeLists.txt
index 9fec620570591101578f819a052813f7aec58b12..24b5942e6af3f6dadd175e7ec2616a7df3189d74 100644
--- a/source/RobotAPI/drivers/OptoForceUnit/CMakeLists.txt
+++ b/source/RobotAPI/drivers/OptoForceUnit/CMakeLists.txt
@@ -15,3 +15,5 @@ if(OptoForceOMD_FOUND)
endif()
# add unit tests
add_subdirectory(test)
+
+armarx_generate_and_add_component_executable(APPLICATION_APP_SUFFIX)
diff --git a/source/RobotAPI/drivers/OrientedTactileSensor/CMakeLists.txt b/source/RobotAPI/drivers/OrientedTactileSensor/CMakeLists.txt
index 8db77f68741c2e2f34fe0eb3e2f4284e37df515f..3c8064670b1be73b907c5baee9f3b95bdc26c423 100644
--- a/source/RobotAPI/drivers/OrientedTactileSensor/CMakeLists.txt
+++ b/source/RobotAPI/drivers/OrientedTactileSensor/CMakeLists.txt
@@ -7,3 +7,5 @@ set(LIB_FILES OrientedTactileSensorUnit.cpp)
set(LIB_HEADERS OrientedTactileSensorUnit.h)
armarx_add_library("${LIB_NAME}" "${LIB_FILES}" "${LIB_HEADERS}" "${LIBS}")
+
+armarx_generate_and_add_component_executable(COMPONENT_NAME OrientedTactileSensorUnit APPLICATION_APP_SUFFIX)
diff --git a/source/RobotAPI/drivers/XsensIMU/CMakeLists.txt b/source/RobotAPI/drivers/XsensIMU/CMakeLists.txt
index 23e7c251619ef7e2dbfd53e18ff40d0b77662294..ec20cb89023baf93c67ab53308a3895921145eac 100644
--- a/source/RobotAPI/drivers/XsensIMU/CMakeLists.txt
+++ b/source/RobotAPI/drivers/XsensIMU/CMakeLists.txt
@@ -2,16 +2,11 @@ armarx_component_set_name("XsensIMU")
set(COMPONENT_LIBS ArmarXCore RobotAPIUnits ArmarXCoreObservers LibIMU)
-set(SOURCES
-./XsensIMU.cpp
-#@TEMPLATE_LINE@@COMPONENT_PATH@/@COMPONENT_NAME@.cpp
-)
-set(HEADERS
-./XsensIMU.h
-#@TEMPLATE_LINE@@COMPONENT_PATH@/@COMPONENT_NAME@.h
-)
+set(SOURCES XsensIMU.cpp)
+set(HEADERS XsensIMU.h)
armarx_add_component("${SOURCES}" "${HEADERS}")
-
add_subdirectory(IMU)
+
+armarx_generate_and_add_component_executable(APPLICATION_APP_SUFFIX)
diff --git a/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.cpp b/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.cpp
index e4fa766c3249b699f65fe1105977626adbd22b10..c893f73b6927c28cbb6db2641e890155989d6235 100644
--- a/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.cpp
+++ b/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.cpp
@@ -30,6 +30,8 @@
#include <QMessageBox>
#include <QTimer>
+#include <ArmarXCore/observers/variant/Variant.h>
+
#define ENABLE_INTROSPECTION 0
@@ -37,7 +39,7 @@ namespace armarx
{
struct ArVizWidgetBatchCallback : IceUtil::Shared
{
- class ArVizWidgetController* this_;
+ struct ArVizWidgetController* this_;
void onSuccess(viz::data::RecordingBatch const& batch)
{
@@ -57,6 +59,9 @@ namespace armarx
widget.setupUi(getWidget());
+ timingObserverTimer = new QTimer(this);
+ connect(timingObserverTimer, &QTimer::timeout, this, QOverload<>::of(&This::onTimingObserverUpdate));
+
replayTimer = new QTimer(this);
connect(replayTimer, &QTimer::timeout, this, QOverload<>::of(&This::onReplayTimerTick));
@@ -119,7 +124,10 @@ namespace armarx
{
usingProxy(storageName);
}
- // ARMARX_IMPORTANT << "OnInit: " << storageName;
+ if (debugObserverName.size() > 0)
+ {
+ usingProxy(debugObserverName);
+ }
callbackData = new ArVizWidgetBatchCallback();
callbackData->this_ = this;
@@ -136,6 +144,10 @@ namespace armarx
void ArVizWidgetController::onConnectComponent()
{
getProxy(storage, storageName);
+ // DebugObserver is optional (check for null on every call)
+ getProxy(debugObserver, debugObserverName, false, "", false);
+
+ lastTiming = visualizer.getTiming();
visualizer.startAsync(storage);
// Changes to UI elements are only allowed in the GUI thread
@@ -155,10 +167,13 @@ namespace armarx
onRefreshRecordings();
currentRecordingSelected = false;
changeMode(ArVizWidgetMode::Live);
+
+ timingObserverTimer->start(33);
}
void ArVizWidgetController::onDisconnectGui()
{
+ timingObserverTimer->stop();
changeMode(ArVizWidgetMode::NotConnected);
}
@@ -407,6 +422,23 @@ namespace armarx
layerTreeChanged(nullptr, 0);
}
+ void ArVizWidgetController::onTimingObserverUpdate()
+ {
+ viz::CoinVisualizer_UpdateTiming timing = visualizer.getTiming();
+ if (timing.counter > lastTiming.counter)
+ {
+ if (debugObserver)
+ {
+ timingMap["0.pull"] = new Variant(timing.pull.toMilliSecondsDouble());
+ timingMap["1.apply"] = new Variant(timing.applyTotal.total.toMilliSecondsDouble());
+ timingMap["2.layers"] = new Variant(timing.layersChanged.toMilliSecondsDouble());
+ timingMap["total"] = new Variant(timing.total.toMilliSecondsDouble());
+
+ debugObserver->begin_setDebugChannel("ArViz_Timing", timingMap);
+ }
+ }
+ }
+
void ArVizWidgetController::onStartRecording()
{
std::string recordingID = widget.recordingIdTextBox->text().toStdString();
@@ -909,17 +941,22 @@ namespace armarx
}
const static std::string CONFIG_KEY_STORAGE = "Storage";
+ const static std::string CONFIG_KEY_DEBUG_OBSERVER = "DebugObserver";
void ArVizWidgetController::loadSettings(QSettings* settings)
{
storageName = settings->value(QString::fromStdString(CONFIG_KEY_STORAGE),
"ArVizStorage").toString().toStdString();
+ debugObserverName = settings->value(QString::fromStdString(CONFIG_KEY_DEBUG_OBSERVER),
+ "DebugObserver").toString().toStdString();
}
void ArVizWidgetController::saveSettings(QSettings* settings)
{
settings->setValue(QString::fromStdString(CONFIG_KEY_STORAGE),
QString::fromStdString(storageName));
+ settings->setValue(QString::fromStdString(CONFIG_KEY_DEBUG_OBSERVER),
+ QString::fromStdString(debugObserverName));
}
QPointer<QDialog> armarx::ArVizWidgetController::getConfigDialog(QWidget* parent)
@@ -928,6 +965,7 @@ namespace armarx
{
configDialog = new SimpleConfigDialog(parent);
configDialog->addProxyFinder<armarx::viz::StorageInterfacePrx>({CONFIG_KEY_STORAGE, "ArViz Storage", "ArViz*"});
+ configDialog->addProxyFinder<armarx::DebugObserverInterfacePrx>({CONFIG_KEY_DEBUG_OBSERVER, "Debug observer", "DebugObserver"});
}
return qobject_cast<QDialog*>(configDialog);
}
@@ -937,6 +975,7 @@ namespace armarx
if (configDialog)
{
storageName = configDialog->getProxyName(CONFIG_KEY_STORAGE);
+ debugObserverName = configDialog->getProxyName(CONFIG_KEY_DEBUG_OBSERVER);
}
}
}
diff --git a/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.h b/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.h
index d8c010b550b89fd17117f8d4862100f05aa5b740..4e9c887def80f19350373070799b456dca75d2db 100644
--- a/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.h
+++ b/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.h
@@ -30,6 +30,7 @@
#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXComponentWidgetController.h>
#include <ArmarXGui/libraries/SimpleConfigDialog/SimpleConfigDialog.h>
+#include <ArmarXCore/interface/observers/ObserverInterface.h>
#include <ArmarXCore/core/system/ImportExportComponent.h>
#include <Inventor/nodes/SoSeparator.h>
@@ -69,9 +70,9 @@ namespace armarx
*
* Detailed description
*/
- class ARMARXCOMPONENT_IMPORT_EXPORT
- ArVizWidgetController:
- public armarx::ArmarXComponentWidgetControllerTemplate < ArVizWidgetController >
+ struct ARMARXCOMPONENT_IMPORT_EXPORT
+ ArVizWidgetController
+ : armarx::ArmarXComponentWidgetControllerTemplate < ArVizWidgetController >
{
Q_OBJECT
@@ -139,6 +140,8 @@ namespace armarx
void showAllLayers(bool visible);
void showFilteredLayers(bool visible);
+ void onTimingObserverUpdate();
+
// Record & Replay
@@ -170,11 +173,17 @@ namespace armarx
QPointer<SimpleConfigDialog> configDialog;
+ QTimer* timingObserverTimer;
+ viz::CoinVisualizer_UpdateTiming lastTiming;
+ StringVariantBaseMap timingMap;
+
QTimer* replayTimer;
long replayCurrentTimestamp = 0;
std::string storageName;
armarx::viz::StorageInterfacePrx storage;
+ std::string debugObserverName;
+ DebugObserverInterfacePrx debugObserver;
armarx::viz::CoinVisualizer visualizer;
armarx::LayerInfoTree layerInfoTree;
diff --git a/source/RobotAPI/libraries/aron/types/AronTypeListNavigator.cpp b/source/RobotAPI/gui-plugins/ArVizDrawerGui/ArVizDrawerGuiGuiPlugin.cpp
similarity index 60%
rename from source/RobotAPI/libraries/aron/types/AronTypeListNavigator.cpp
rename to source/RobotAPI/gui-plugins/ArVizDrawerGui/ArVizDrawerGuiGuiPlugin.cpp
index 95f4ec053aee42fd224754b830e26541798d336e..de721d9a03ffbaacffa0afd6173d7b8fb1a29048 100644
--- a/source/RobotAPI/libraries/aron/types/AronTypeListNavigator.cpp
+++ b/source/RobotAPI/gui-plugins/ArVizDrawerGui/ArVizDrawerGuiGuiPlugin.cpp
@@ -1,9 +1,6 @@
/*
* 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.
@@ -16,15 +13,21 @@
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*
- * @author Simon Ottenhaus (simon dot ottenhaus at kit dot edu)
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * \package RobotAPI::gui-plugins::ArVizDrawerGuiGuiPlugin
+ * \author Raphael Grimm ( raphael dot grimm at kit dot edu )
+ * \date 2020
+ * \copyright http://www.gnu.org/licenses/gpl-2.0.txt
* GNU General Public License
*/
-#include "TypeList.h"
+#include "ArVizDrawerGuiGuiPlugin.h"
-using namespace armarx;
+#include "ArVizDrawerGuiWidgetController.h"
-TypeList::TypeList()
+namespace armarx
{
+ ArVizDrawerGuiGuiPlugin::ArVizDrawerGuiGuiPlugin()
+ {
+ addWidget < ArVizDrawerGuiWidgetController > ();
+ }
}
diff --git a/source/RobotAPI/gui-plugins/ArVizDrawerGui/ArVizDrawerGuiGuiPlugin.h b/source/RobotAPI/gui-plugins/ArVizDrawerGui/ArVizDrawerGuiGuiPlugin.h
new file mode 100644
index 0000000000000000000000000000000000000000..91dda180873e5afaa28036839cc53cc163aec25a
--- /dev/null
+++ b/source/RobotAPI/gui-plugins/ArVizDrawerGui/ArVizDrawerGuiGuiPlugin.h
@@ -0,0 +1,50 @@
+/*
+ * 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::gui-plugins::ArVizDrawerGui
+ * \author Raphael Grimm ( raphael dot grimm at kit dot edu )
+ * \date 2020
+ * \copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+#pragma once
+
+#include <ArmarXCore/core/system/ImportExportComponent.h>
+#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
+#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXComponentWidgetController.h>
+
+namespace armarx
+{
+ /**
+ * \class ArVizDrawerGuiGuiPlugin
+ * \ingroup ArmarXGuiPlugins
+ * \brief ArVizDrawerGuiGuiPlugin brief description
+ *
+ * Detailed description
+ */
+ class ARMARXCOMPONENT_IMPORT_EXPORT ArVizDrawerGuiGuiPlugin:
+ public armarx::ArmarXGuiPlugin
+ {
+ Q_OBJECT
+ Q_INTERFACES(ArmarXGuiInterface)
+ Q_PLUGIN_METADATA(IID "ArmarXGuiInterface/1.00")
+ public:
+ /**
+ * All widgets exposed by this plugin are added in the constructor
+ * via calls to addWidget()
+ */
+ ArVizDrawerGuiGuiPlugin();
+ };
+}
diff --git a/source/RobotAPI/gui-plugins/ArVizDrawerGui/ArVizDrawerGuiWidget.ui b/source/RobotAPI/gui-plugins/ArVizDrawerGui/ArVizDrawerGuiWidget.ui
new file mode 100644
index 0000000000000000000000000000000000000000..ebec370784085d1e4ed296206234d499f28cc0fe
--- /dev/null
+++ b/source/RobotAPI/gui-plugins/ArVizDrawerGui/ArVizDrawerGuiWidget.ui
@@ -0,0 +1,119 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<ui version="4.0">
+ <class>ArVizDrawerGuiWidget</class>
+ <widget class="QWidget" name="ArVizDrawerGuiWidget">
+ <property name="geometry">
+ <rect>
+ <x>0</x>
+ <y>0</y>
+ <width>400</width>
+ <height>300</height>
+ </rect>
+ </property>
+ <property name="windowTitle">
+ <string>ArVizDrawerGuiWidget</string>
+ </property>
+ <layout class="QGridLayout" name="gridLayout">
+ <item row="0" column="1">
+ <widget class="QComboBox" name="comboBoxElement"/>
+ </item>
+ <item row="1" column="0" colspan="3">
+ <widget class="QScrollArea" name="scrollArea">
+ <property name="widgetResizable">
+ <bool>true</bool>
+ </property>
+ <widget class="QWidget" name="scrollAreaWidgetContents">
+ <property name="geometry">
+ <rect>
+ <x>0</x>
+ <y>0</y>
+ <width>380</width>
+ <height>249</height>
+ </rect>
+ </property>
+ <layout class="QGridLayout" name="gridLayout_2">
+ <property name="leftMargin">
+ <number>0</number>
+ </property>
+ <property name="topMargin">
+ <number>0</number>
+ </property>
+ <property name="rightMargin">
+ <number>0</number>
+ </property>
+ <property name="bottomMargin">
+ <number>0</number>
+ </property>
+ <property name="spacing">
+ <number>0</number>
+ </property>
+ <item row="0" column="0">
+ <layout class="QVBoxLayout" name="verticalLayoutElements"/>
+ </item>
+ <item row="0" column="1">
+ <spacer name="horizontalSpacer">
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ <property name="sizeHint" stdset="0">
+ <size>
+ <width>354</width>
+ <height>20</height>
+ </size>
+ </property>
+ </spacer>
+ </item>
+ <item row="1" column="0">
+ <spacer name="verticalSpacer">
+ <property name="orientation">
+ <enum>Qt::Vertical</enum>
+ </property>
+ <property name="sizeHint" stdset="0">
+ <size>
+ <width>20</width>
+ <height>223</height>
+ </size>
+ </property>
+ </spacer>
+ </item>
+ </layout>
+ </widget>
+ </widget>
+ </item>
+ <item row="0" column="0">
+ <widget class="QLabel" name="label">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Fixed" vsizetype="Preferred">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="text">
+ <string>Element</string>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="2">
+ <widget class="QPushButton" name="pushButtonElementAdd">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Fixed" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>22</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>+</string>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </widget>
+ <resources/>
+ <connections/>
+</ui>
diff --git a/source/RobotAPI/gui-plugins/ArVizDrawerGui/ArVizDrawerGuiWidgetController.cpp b/source/RobotAPI/gui-plugins/ArVizDrawerGui/ArVizDrawerGuiWidgetController.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..ca4c16d8f017b620919f42f4d4b2a1bf7ce1167d
--- /dev/null
+++ b/source/RobotAPI/gui-plugins/ArVizDrawerGui/ArVizDrawerGuiWidgetController.cpp
@@ -0,0 +1,88 @@
+/*
+ * 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::gui-plugins::ArVizDrawerGuiWidgetController
+ * \author Raphael Grimm ( raphael dot grimm at kit dot edu )
+ * \date 2020
+ * \copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include <string>
+
+#include "ArVizDrawerGuiWidgetController.h"
+#include "Elements/ElementWidgetBase.h"
+#include "Elements/RobotWidget.h"
+
+namespace armarx
+{
+ ArVizDrawerGuiWidgetController::ArVizDrawerGuiWidgetController()
+ {
+ _ui.setupUi(getWidget());
+ connect(_ui.pushButtonElementAdd, &QPushButton::clicked, this,
+ &ArVizDrawerGuiWidgetController::on_pushButtonElementAdd_clicked);
+ //setup factories
+ {
+ _factory["Robot"] = [] { return new RobotWidget; };
+
+ for (const auto& [key, _] : _factory)
+ {
+ _ui.comboBoxElement->addItem(QString::fromStdString(key));
+ }
+ }
+ startTimer(20);
+ }
+
+ void ArVizDrawerGuiWidgetController::onConnectComponent()
+ {
+ _connected = true;
+ }
+ void ArVizDrawerGuiWidgetController::onDisconnectComponent()
+ {
+ _connected = false;
+ }
+
+ void ArVizDrawerGuiWidgetController::on_pushButtonElementAdd_clicked()
+ {
+ const auto name = _ui.comboBoxElement->currentText().toStdString();
+ auto* ptr = _factory.at(name)();
+ _ui.verticalLayoutElements->addWidget(ptr);
+ _elements.emplace(ptr);
+ }
+
+ void ArVizDrawerGuiWidgetController::timerEvent(QTimerEvent*)
+ {
+ if (!_connected)
+ {
+ return;
+ }
+ auto layer = getArvizClient().layer("elements");
+ for (auto* elem : _elements)
+ {
+ if (elem->toDelete())
+ {
+ _ui.verticalLayoutElements->removeWidget(elem);
+ elem->deleteLater();
+ _elements.erase(elem);
+ }
+ else
+ {
+ elem->addTo(layer);
+ }
+ }
+ getArvizClient().commit(layer);
+ }
+}
+
diff --git a/source/RobotAPI/gui-plugins/ArVizDrawerGui/ArVizDrawerGuiWidgetController.h b/source/RobotAPI/gui-plugins/ArVizDrawerGui/ArVizDrawerGuiWidgetController.h
new file mode 100644
index 0000000000000000000000000000000000000000..20382221d2c2810ad7c62cbe5ff3921d688f6f50
--- /dev/null
+++ b/source/RobotAPI/gui-plugins/ArVizDrawerGui/ArVizDrawerGuiWidgetController.h
@@ -0,0 +1,92 @@
+/*
+ * 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::gui-plugins::ArVizDrawerGuiWidgetController
+ * @author Raphael Grimm ( raphael dot grimm at kit dot edu )
+ * @date 2020
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+#pragma once
+
+#include <functional>
+
+#include <ArmarXCore/core/system/ImportExportComponent.h>
+
+#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
+#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXComponentWidgetController.h>
+
+#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
+#include <RobotAPI/gui-plugins/ArVizDrawerGui/ui_ArVizDrawerGuiWidget.h>
+
+namespace armarx
+{
+ class ElementWidgetBase;
+
+ /**
+ \page RobotAPI-GuiPlugins-ArVizDrawerGui ArVizDrawerGui
+ \brief The ArVizDrawerGui allows visualizing ...
+
+ \image html ArVizDrawerGui.png
+ The user can
+
+ API Documentation \ref ArVizDrawerGuiWidgetController
+
+ \see ArVizDrawerGuiGuiPlugin
+ */
+
+ /**
+ * \class ArVizDrawerGuiWidgetController
+ * \brief ArVizDrawerGuiWidgetController brief one line description
+ *
+ * Detailed description
+ */
+ class ARMARXCOMPONENT_IMPORT_EXPORT
+ ArVizDrawerGuiWidgetController:
+ public armarx::ArmarXComponentWidgetControllerTemplate < ArVizDrawerGuiWidgetController >,
+ public virtual ArVizComponentPluginUser
+ {
+ Q_OBJECT
+ public:
+ explicit ArVizDrawerGuiWidgetController();
+ virtual ~ArVizDrawerGuiWidgetController() = default;
+
+ void loadSettings(QSettings* settings) override {}
+ void saveSettings(QSettings* settings) override {}
+
+ static QString GetWidgetName()
+ {
+ return "Visualization.ArVizDrawer";
+ }
+
+ void onInitComponent() override {}
+ void onConnectComponent() override;
+ void onDisconnectComponent() override;
+
+ protected:
+ void timerEvent(QTimerEvent* event) override;
+
+ private slots:
+ void on_pushButtonElementAdd_clicked();
+
+ private:
+ Ui::ArVizDrawerGuiWidget _ui;
+ std::atomic_bool _connected{false};
+ std::set<ElementWidgetBase*> _elements;
+ std::map<std::string, std::function<ElementWidgetBase*()>> _factory;
+ };
+}
+
+
diff --git a/source/RobotAPI/gui-plugins/ArVizDrawerGui/CMakeLists.txt b/source/RobotAPI/gui-plugins/ArVizDrawerGui/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..90084a94160bbe1e60f88d026e1da11bf3b76b47
--- /dev/null
+++ b/source/RobotAPI/gui-plugins/ArVizDrawerGui/CMakeLists.txt
@@ -0,0 +1,30 @@
+set(LIB_NAME "ArVizDrawerGuiGuiPlugin")
+armarx_set_target("${LIB_NAME}")
+
+armarx_build_if(ArmarXGui_FOUND "ArmarXGui not available")
+
+set(SOURCES
+ ArVizDrawerGuiGuiPlugin.cpp
+ ArVizDrawerGuiWidgetController.cpp
+ Elements/RobotWidget.cpp
+)
+
+set(HEADERS
+ ArVizDrawerGuiGuiPlugin.h
+ ArVizDrawerGuiWidgetController.h
+ Elements/ElementWidgetBase.h
+ Elements/RobotWidget.h
+)
+
+set(GUI_MOC_HDRS ${HEADERS})
+
+set(GUI_UIS
+ ArVizDrawerGuiWidget.ui
+ Elements/RobotWidget.ui
+)
+
+set(COMPONENT_LIBS RobotAPIComponentPlugins)
+
+if(ArmarXGui_FOUND)
+ armarx_gui_library("${LIB_NAME}" "${SOURCES}" "${GUI_MOC_HDRS}" "${GUI_UIS}" "" "${COMPONENT_LIBS}")
+endif()
diff --git a/source/RobotAPI/gui-plugins/ArVizDrawerGui/Elements/ElementWidgetBase.h b/source/RobotAPI/gui-plugins/ArVizDrawerGui/Elements/ElementWidgetBase.h
new file mode 100644
index 0000000000000000000000000000000000000000..0d111c37d8af5e7cf528b36aa8fa3d83275601b4
--- /dev/null
+++ b/source/RobotAPI/gui-plugins/ArVizDrawerGui/Elements/ElementWidgetBase.h
@@ -0,0 +1,35 @@
+#pragma once
+
+#include <QWidget>
+
+#include <RobotAPI/components/ArViz/Client/Layer.h>
+#include <RobotAPI/components/ArViz/Client/Elements.h>
+
+namespace armarx
+{
+
+ class ElementWidgetBase : public QWidget
+ {
+ public:
+ ~ElementWidgetBase() override = default;
+ virtual void addTo(viz::Layer& layer) const = 0;
+ virtual bool toDelete() const = 0;
+ };
+
+ template<class UiT>
+ class ElementWidgetBaseTemplate : public ElementWidgetBase
+ {
+ public:
+ ElementWidgetBaseTemplate()
+ {
+ _ui.setupUi(this);
+ _ui.pushButtonDelete->setCheckable(true);
+ _ui.pushButtonDelete->setChecked(false);
+ }
+ bool toDelete() const override
+ {
+ return _ui.pushButtonDelete->isChecked();
+ }
+ UiT _ui;
+ };
+}
diff --git a/source/RobotAPI/gui-plugins/ArVizDrawerGui/Elements/RobotWidget.cpp b/source/RobotAPI/gui-plugins/ArVizDrawerGui/Elements/RobotWidget.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..d9acbc51e7490f2cb8f1578243ab4e5a7c64b49f
--- /dev/null
+++ b/source/RobotAPI/gui-plugins/ArVizDrawerGui/Elements/RobotWidget.cpp
@@ -0,0 +1,18 @@
+#include "RobotWidget.h"
+
+namespace armarx
+{
+ void RobotWidget::addTo(viz::Layer& layer) const
+ {
+ const auto adr = reinterpret_cast<std::intptr_t>(this);
+ layer.add(viz::Robot("Robot_" + std::to_string(adr))
+ .file(_ui.lineEditProject->text().toStdString(),
+ _ui.comboBoxFile->currentText().toStdString())
+ .position(_ui.doubleSpinBoxTX->value(),
+ _ui.doubleSpinBoxTY->value(),
+ _ui.doubleSpinBoxTZ->value())
+ .orientation(_ui.doubleSpinBoxRX->value(),
+ _ui.doubleSpinBoxRY->value(),
+ _ui.doubleSpinBoxRZ->value()));
+ }
+}
diff --git a/source/RobotAPI/gui-plugins/ArVizDrawerGui/Elements/RobotWidget.h b/source/RobotAPI/gui-plugins/ArVizDrawerGui/Elements/RobotWidget.h
new file mode 100644
index 0000000000000000000000000000000000000000..d2b24d8f4e644171361dbf603d2228f24a55055a
--- /dev/null
+++ b/source/RobotAPI/gui-plugins/ArVizDrawerGui/Elements/RobotWidget.h
@@ -0,0 +1,14 @@
+#pragma once
+
+#include "ElementWidgetBase.h"
+
+#include <RobotAPI/gui-plugins/ArVizDrawerGui/ui_RobotWidget.h>
+
+namespace armarx
+{
+ class RobotWidget : public ElementWidgetBaseTemplate<Ui::RobotWidget>
+ {
+ public:
+ void addTo(viz::Layer& layer) const override;
+ };
+}
diff --git a/source/RobotAPI/gui-plugins/ArVizDrawerGui/Elements/RobotWidget.ui b/source/RobotAPI/gui-plugins/ArVizDrawerGui/Elements/RobotWidget.ui
new file mode 100644
index 0000000000000000000000000000000000000000..e8e294d6e0bc0d06f33c7e4fb1048f7d8f87c123
--- /dev/null
+++ b/source/RobotAPI/gui-plugins/ArVizDrawerGui/Elements/RobotWidget.ui
@@ -0,0 +1,198 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<ui version="4.0">
+ <class>RobotWidget</class>
+ <widget class="QWidget" name="RobotWidget">
+ <property name="geometry">
+ <rect>
+ <x>0</x>
+ <y>0</y>
+ <width>547</width>
+ <height>300</height>
+ </rect>
+ </property>
+ <property name="windowTitle">
+ <string>Form</string>
+ </property>
+ <layout class="QGridLayout" name="gridLayout">
+ <property name="leftMargin">
+ <number>0</number>
+ </property>
+ <property name="topMargin">
+ <number>0</number>
+ </property>
+ <property name="rightMargin">
+ <number>0</number>
+ </property>
+ <property name="bottomMargin">
+ <number>0</number>
+ </property>
+ <item row="4" column="3">
+ <widget class="QDoubleSpinBox" name="doubleSpinBoxTZ">
+ <property name="minimum">
+ <double>-100000.000000000000000</double>
+ </property>
+ <property name="maximum">
+ <double>100000.000000000000000</double>
+ </property>
+ <property name="value">
+ <double>0.000000000000000</double>
+ </property>
+ </widget>
+ </item>
+ <item row="3" column="0">
+ <widget class="QLabel" name="label_3">
+ <property name="text">
+ <string>Robot file</string>
+ </property>
+ </widget>
+ </item>
+ <item row="5" column="2">
+ <widget class="QDoubleSpinBox" name="doubleSpinBoxRY">
+ <property name="decimals">
+ <number>3</number>
+ </property>
+ <property name="minimum">
+ <double>-10.000000000000000</double>
+ </property>
+ <property name="maximum">
+ <double>10.000000000000000</double>
+ </property>
+ </widget>
+ </item>
+ <item row="4" column="1">
+ <widget class="QDoubleSpinBox" name="doubleSpinBoxTX">
+ <property name="maximum">
+ <double>100000.000000000000000</double>
+ </property>
+ <property name="value">
+ <double>0.000000000000000</double>
+ </property>
+ </widget>
+ </item>
+ <item row="5" column="3">
+ <widget class="QDoubleSpinBox" name="doubleSpinBoxRZ">
+ <property name="decimals">
+ <number>3</number>
+ </property>
+ <property name="minimum">
+ <double>-10.000000000000000</double>
+ </property>
+ <property name="maximum">
+ <double>10.000000000000000</double>
+ </property>
+ </widget>
+ </item>
+ <item row="5" column="0">
+ <widget class="QLabel" name="label_5">
+ <property name="text">
+ <string>Base RPY</string>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="0">
+ <widget class="QLabel" name="label_2">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Fixed" vsizetype="Preferred">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="text">
+ <string>Project name</string>
+ </property>
+ </widget>
+ </item>
+ <item row="4" column="0">
+ <widget class="QLabel" name="label_4">
+ <property name="text">
+ <string>Base XYZ</string>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="1" colspan="3">
+ <widget class="QLineEdit" name="lineEditProject"/>
+ </item>
+ <item row="3" column="1" colspan="3">
+ <widget class="QComboBox" name="comboBoxFile">
+ <property name="editable">
+ <bool>true</bool>
+ </property>
+ <item>
+ <property name="text">
+ <string>Armar6RT/robotmodel/Armar6-SH/Armar6-SH.xml</string>
+ </property>
+ </item>
+ <item>
+ <property name="text">
+ <string>Armar6RT/robotmodel/Armar6-SH/Armar6-RightHand-v3.xml</string>
+ </property>
+ </item>
+ <item>
+ <property name="text">
+ <string>Armar6RT/robotmodel/Armar6-SH/Armar6-LeftHand-v3.xml</string>
+ </property>
+ </item>
+ </widget>
+ </item>
+ <item row="5" column="1">
+ <widget class="QDoubleSpinBox" name="doubleSpinBoxRX">
+ <property name="decimals">
+ <number>3</number>
+ </property>
+ <property name="minimum">
+ <double>-10.000000000000000</double>
+ </property>
+ <property name="maximum">
+ <double>10.000000000000000</double>
+ </property>
+ </widget>
+ </item>
+ <item row="4" column="2">
+ <widget class="QDoubleSpinBox" name="doubleSpinBoxTY">
+ <property name="minimum">
+ <double>-100000.000000000000000</double>
+ </property>
+ <property name="maximum">
+ <double>100000.000000000000000</double>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="0" colspan="4">
+ <layout class="QHBoxLayout" name="horizontalLayout">
+ <item>
+ <widget class="QLabel" name="label">
+ <property name="text">
+ <string>Robot</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QPushButton" name="pushButtonDelete">
+ <property name="maximumSize">
+ <size>
+ <width>22</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>X</string>
+ </property>
+ <property name="checkable">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="0" column="0" colspan="4">
+ <widget class="Line" name="line">
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </widget>
+ <resources/>
+ <connections/>
+</ui>
diff --git a/source/RobotAPI/gui-plugins/CMakeLists.txt b/source/RobotAPI/gui-plugins/CMakeLists.txt
index b1fa1c8b6ea8a14fcb54a256a29a58ddaed097ba..fa886cea9c7e4df76494bf05d3eb94101c8840c7 100644
--- a/source/RobotAPI/gui-plugins/CMakeLists.txt
+++ b/source/RobotAPI/gui-plugins/CMakeLists.txt
@@ -18,3 +18,5 @@ add_subdirectory(CartesianNaturalPositionController)
add_subdirectory(ObjectPoseGui)
add_subdirectory(CartesianImpedanceController)
+
+add_subdirectory(ArVizDrawerGui)
\ No newline at end of file
diff --git a/source/RobotAPI/gui-plugins/CartesianImpedanceController/CartesianImpedanceControllerWidgetController.h b/source/RobotAPI/gui-plugins/CartesianImpedanceController/CartesianImpedanceControllerWidgetController.h
index 21fa9f38b5ffe50d1b2f4a560e90a2ef337669f8..1618ce6e6d047993ef1360d6ae7203cb79013296 100644
--- a/source/RobotAPI/gui-plugins/CartesianImpedanceController/CartesianImpedanceControllerWidgetController.h
+++ b/source/RobotAPI/gui-plugins/CartesianImpedanceController/CartesianImpedanceControllerWidgetController.h
@@ -22,8 +22,8 @@
#pragma once
#include <RobotAPI/libraries/NJointControllerGuiPluginUtility/NJointControllerGuiPluginBase.h>
-#include <RobotAPI/libraries/NJointControllerGuiPluginUtility/SpinBoxToVector.h>
-#include <RobotAPI/libraries/NJointControllerGuiPluginUtility/SpinBoxToPose.h>
+#include <ArmarXGui/libraries/ArmarXGuiBase/SpinBoxToVector.h>
+#include <ArmarXGui/libraries/ArmarXGuiBase/SpinBoxToPose.h>
#include <RobotAPI/interface/units/RobotUnit/TaskSpaceActiveImpedanceControl.h>
#include <RobotAPI/libraries/RobotAPINJointControllerWidgets/CartesianImpedanceControllerConfigWidget.h>
#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
diff --git a/source/RobotAPI/gui-plugins/RobotUnitPlugin/CMakeLists.txt b/source/RobotAPI/gui-plugins/RobotUnitPlugin/CMakeLists.txt
index 8acf13526082b0f2158ddee8a222583c41e32a78..d8cbfb008050a753e3ed06951d2da13efca1ea6d 100644
--- a/source/RobotAPI/gui-plugins/RobotUnitPlugin/CMakeLists.txt
+++ b/source/RobotAPI/gui-plugins/RobotUnitPlugin/CMakeLists.txt
@@ -49,6 +49,7 @@ set(COMPONENT_LIBS
VariantWidget
RobotAPIVariantWidget
WidgetDescription
+ RobotUnitDataStreamingReceiver
)
if(ArmarXGui_FOUND)
diff --git a/source/RobotAPI/gui-plugins/RobotUnitPlugin/RobotUnitPlugin/RobotUnitPluginWidget.ui b/source/RobotAPI/gui-plugins/RobotUnitPlugin/RobotUnitPlugin/RobotUnitPluginWidget.ui
index 8b4af8524ff6102e7230794f12fdaac64b07a876..c749941ca61a52735aee3922a410b903b8b12839 100644
--- a/source/RobotAPI/gui-plugins/RobotUnitPlugin/RobotUnitPlugin/RobotUnitPluginWidget.ui
+++ b/source/RobotAPI/gui-plugins/RobotUnitPlugin/RobotUnitPlugin/RobotUnitPluginWidget.ui
@@ -142,6 +142,13 @@
</property>
</widget>
</item>
+ <item>
+ <widget class="QCheckBox" name="checkBoxLoggingStream">
+ <property name="text">
+ <string>Stream to local computer</string>
+ </property>
+ </widget>
+ </item>
<item>
<widget class="QPushButton" name="pushButtonLoggingStart">
<property name="text">
diff --git a/source/RobotAPI/gui-plugins/RobotUnitPlugin/RobotUnitPlugin/RobotUnitPluginWidgetController.cpp b/source/RobotAPI/gui-plugins/RobotUnitPlugin/RobotUnitPlugin/RobotUnitPluginWidgetController.cpp
index 9f6229c5105a523109babbc9f2b7121ace039e42..273a2d631d091f03652c326f7b55b5312996153d 100644
--- a/source/RobotAPI/gui-plugins/RobotUnitPlugin/RobotUnitPlugin/RobotUnitPluginWidgetController.cpp
+++ b/source/RobotAPI/gui-plugins/RobotUnitPlugin/RobotUnitPlugin/RobotUnitPluginWidgetController.cpp
@@ -34,6 +34,7 @@
#include <ArmarXCore/util/CPPUtility/Iterator.h>
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/util/FileSystemPathBuilder.h>
using namespace armarx;
@@ -123,22 +124,28 @@ void RobotUnitPluginWidgetController::onInitComponent()
{
usingProxy(robotUnitProxyName);
ARMARX_INFO << "RobotUnitPluginWidgetController::onInitComponent()" << std::flush;
+ QMetaObject::invokeMethod(this, "startOnConnectTimer", Qt::QueuedConnection);
+}
+void RobotUnitPluginWidgetController::onExitComponent()
+{
+ QMetaObject::invokeMethod(this, "stopOnConnectTimer", Qt::QueuedConnection);
}
void RobotUnitPluginWidgetController::onConnectComponent()
{
+ std::lock_guard guard{robotUnitPrxMutex};
robotUnitPrx = getProxy<RobotUnitInterfacePrx>(robotUnitProxyName);
listenerTopicName = robotUnitPrx->getRobotUnitListenerTopicName();
usingTopic(listenerTopicName);
updateToolBarActionCheckedState();
- QMetaObject::invokeMethod(this, "startOnConnectTimer", Qt::QueuedConnection);
+ timerLastIterationRuWasRunning = false;
}
void RobotUnitPluginWidgetController::onDisconnectComponent()
{
+ std::lock_guard guard{robotUnitPrxMutex};
unsubscribeFromTopic(listenerTopicName);
robotUnitPrx = nullptr;
- killTimer(onConnectTimerId);
QMetaObject::invokeMethod(this, "refreshNJointControllersClicked", Qt::QueuedConnection);
QMetaObject::invokeMethod(this, "refreshNJointControllerClassesClicked", Qt::QueuedConnection);
QMetaObject::invokeMethod(this, "refreshControlDevicesClicked", Qt::QueuedConnection);
@@ -219,7 +226,14 @@ QPointer<QWidget> RobotUnitPluginWidgetController::getCustomTitlebarWidget(QWidg
customToolbar->addAction(showLogging);
customToolbar->addSeparator();
- customToolbar->addAction(QIcon(":/icons/document-save.svg"), "Write log", this, SLOT(writeLogClicked()))
+ customToolbar->addAction(
+ QIcon(":/icons/document-save.svg"), "Write log",
+ [&]
+ {
+ std::lock_guard guard{robotUnitPrxMutex};
+ robotUnitPrx->writeRecentIterationsToFile("/tmp/RobotUnitLog-{DateTime}");
+ }
+ )
->setToolTip("Writes the log to /tmp/");
}
updateToolBarActionCheckedState();
@@ -258,32 +272,42 @@ void RobotUnitPluginWidgetController::nJointControllerDeleted(const std::string&
void RobotUnitPluginWidgetController::refreshNJointControllersClicked()
{
+ std::lock_guard guard{robotUnitPrxMutex};
nJointControllers->reset(robotUnitPrx);
}
void RobotUnitPluginWidgetController::refreshNJointControllerClassesClicked()
{
+ std::lock_guard guard{robotUnitPrxMutex};
nJointControllerClasses->reset(robotUnitPrx);
}
void RobotUnitPluginWidgetController::refreshControlDevicesClicked()
{
+ std::lock_guard guard{robotUnitPrxMutex};
controlDevices->reset(robotUnitPrx);
}
void RobotUnitPluginWidgetController::refreshSensorDevicesClicked()
{
+ std::lock_guard guard{robotUnitPrxMutex};
sensorDevices->reset(robotUnitPrx);
}
-void RobotUnitPluginWidgetController::writeLogClicked()
+void RobotUnitPluginWidgetController::startOnConnectTimer()
{
- robotUnitPrx->writeRecentIterationsToFile("/tmp/RobotUnitLog-{DateTime}");
+ if (!timerId)
+ {
+ timerId = startTimer(100);
+ }
}
-
-void RobotUnitPluginWidgetController::startOnConnectTimer()
+void RobotUnitPluginWidgetController::stopOnConnectTimer()
{
- onConnectTimerId = startTimer(100);
+ if (timerId)
+ {
+ killTimer(timerId);
+ timerId = 0;
+ }
}
void RobotUnitPluginWidgetController::updateToolBarActionCheckedState()
@@ -299,23 +323,34 @@ void RobotUnitPluginWidgetController::updateToolBarActionCheckedState()
void armarx::RobotUnitPluginWidgetController::timerEvent(QTimerEvent*)
{
- if (!robotUnitPrx)
+ std::lock_guard guard{robotUnitPrxMutex};
+ if (robotUnitPrx && robotUnitPrx->isRunning())
{
- killTimer(onConnectTimerId);
+ ARMARX_DEBUG << "timerEvent: RobotUnit is running";
+ if (!timerLastIterationRuWasRunning)
+ {
+ refreshNJointControllersClicked();
+ refreshNJointControllerClassesClicked();
+ refreshControlDevicesClicked();
+ refreshSensorDevicesClicked();
+ refreshLogging();
+ }
+ else
+ {
+ loggingData.localLogging();
+ }
+ timerLastIterationRuWasRunning = true;
}
- if (robotUnitPrx->isRunning())
+ else
{
- refreshNJointControllersClicked();
- refreshNJointControllerClassesClicked();
- refreshControlDevicesClicked();
- refreshSensorDevicesClicked();
- refreshLogging();
- killTimer(onConnectTimerId);
+ timerLastIterationRuWasRunning = false;
+ ARMARX_DEBUG << "timerEvent: RobotUnit is not running";
}
}
void armarx::RobotUnitPluginWidgetController::refreshLogging()
{
+ std::lock_guard guard{robotUnitPrxMutex};
on_pushButtonLoggingStop_clicked();
widget.treeWidgetLoggingNames->clear();
@@ -357,7 +392,8 @@ void armarx::RobotUnitPluginWidgetController::refreshLogging()
void armarx::RobotUnitPluginWidgetController::on_pushButtonLoggingStart_clicked()
{
- if (!robotUnitPrx || loggingData.handle)
+ std::lock_guard guard{robotUnitPrxMutex};
+ if (!robotUnitPrx || loggingData.handle || loggingData.streamingHandler)
{
return;
}
@@ -367,7 +403,17 @@ void armarx::RobotUnitPluginWidgetController::on_pushButtonLoggingStart_clicked(
switch (it->checkState(0))
{
case Qt::Checked:
- loggingNames.emplace_back(it->data(0, Qt::ToolTipRole).toString().toStdString());
+ if (it->childCount() == 0)
+ {
+ loggingNames.emplace_back(it->data(0, Qt::ToolTipRole).toString().toStdString());
+ }
+ else
+ {
+ for (int i = 0; i < it->childCount(); ++i)
+ {
+ recurseChildren(it->child(i));
+ }
+ }
break;
case Qt::Unchecked:
break;
@@ -380,31 +426,61 @@ void armarx::RobotUnitPluginWidgetController::on_pushButtonLoggingStart_clicked(
}
};
recurseChildren(loggingData.top);
- ARMARX_INFO << "start logging " << loggingNames;
-
- loggingData.handle = robotUnitPrx->startRtLogging(
- widget.lineEditLoggingPath->text().toStdString(),
- loggingNames);
+ const std::string saveFormatString = widget.lineEditLoggingPath->text().toStdString();
+ if (widget.checkBoxLoggingStream->isChecked())
+ {
+ ARMARX_INFO << "start streaming " << loggingNames;
+ RobotUnitDataStreaming::Config cfg;
+ cfg.loggingNames = loggingNames;
+ loggingData.streamingHandler =
+ make_shared<RobotUnitDataStreamingReceiver>(this, robotUnitPrx, cfg);
+
+ FileSystemPathBuilder pb {saveFormatString};
+ loggingData.logStream.open(pb.getPath());
+ loggingData.logStream << ";iteration;timestamp;TimeSinceLastIteration";
+ const auto& entries = loggingData.streamingHandler->getDataDescription().entries;
+ std::stringstream str;
+ str << "stream " << entries.size() << " values\n";
+ for (const auto& [name, desc] : entries)
+ {
+ loggingData.logStream << ';' << name;
+ str << " " << name
+ << " -> type " << desc.type
+ << ", index " << desc.index << '\n';
+ }
+ loggingData.logStream << std::endl;
+ ARMARX_INFO << str.str();
+ }
+ else
+ {
+ ARMARX_INFO << "start logging " << loggingNames;
+ loggingData.handle = robotUnitPrx->startRtLogging(saveFormatString, loggingNames);
+ widget.pushButtonLoggingMark1->setEnabled(true);
+ widget.pushButtonLoggingMark2->setEnabled(true);
+ widget.pushButtonLoggingMark3->setEnabled(true);
+ }
widget.pushButtonLoggingStart->setEnabled(false);
widget.pushButtonLoggingStop->setEnabled(true);
- widget.pushButtonLoggingMark1->setEnabled(true);
- widget.pushButtonLoggingMark2->setEnabled(true);
- widget.pushButtonLoggingMark3->setEnabled(true);
}
void armarx::RobotUnitPluginWidgetController::on_pushButtonLoggingStop_clicked()
{
+ std::lock_guard guard{robotUnitPrxMutex};
widget.pushButtonLoggingStart->setEnabled(true);
widget.pushButtonLoggingStop->setEnabled(false);
widget.pushButtonLoggingMark1->setEnabled(false);
widget.pushButtonLoggingMark2->setEnabled(false);
widget.pushButtonLoggingMark3->setEnabled(false);
- if (!loggingData.handle)
+ if (loggingData.handle)
{
- return;
+ robotUnitPrx->stopRtLogging(loggingData.handle);
}
- robotUnitPrx->stopRtLogging(loggingData.handle);
- loggingData.handle = nullptr;
+ if (loggingData.logStream.is_open())
+ {
+ loggingData.logStream.close();
+ }
+ loggingData.handle = nullptr;
+ loggingData.streamingHandler = nullptr;
}
void armarx::RobotUnitPluginWidgetController::on_lineEditLoggingFilter_textChanged(const QString& arg1)
@@ -569,3 +645,59 @@ void RobotUnitPluginWidgetController::LoggingData::updateCheckStateDownward(
updateCheckStateDownward(c, state, true);
}
}
+
+void RobotUnitPluginWidgetController::LoggingData::localLogging()
+{
+ if (!streamingHandler)
+ {
+ ARMARX_DEBUG_S << "localLogging -> no local logging";
+ return;
+ }
+ ARMARX_DEBUG_S << "localLogging -> do local logging";
+ auto& data = streamingHandler->getDataBuffer();
+ if (data.empty())
+ {
+ ARMARX_INFO_S << ::deactivateSpam()
+ << "No streaming data received!";
+ return;
+ }
+ ARMARX_INFO_S << ::deactivateSpam(1)
+ << "Writing " << data.size() << " timesteps";
+ const auto& descr = streamingHandler->getDataDescription();
+ for (const auto& step : data)
+ {
+ for (const auto& [name, desc] : descr.entries)
+ {
+ logStream << ';' << step.iterationId
+ << ';' << step.timestampUSec
+ << ';' << step.timesSinceLastIterationUSec;
+ using enum_t = RobotUnitDataStreaming::DataEntryType;
+ switch (desc.type)
+ {
+ case enum_t::NodeTypeBool :
+ logStream << ';' << step.bools .at(desc.index);
+ break;
+ case enum_t::NodeTypeByte :
+ logStream << ';' << step.bytes .at(desc.index);
+ break;
+ case enum_t::NodeTypeShort :
+ logStream << ';' << step.shorts .at(desc.index);
+ break;
+ case enum_t::NodeTypeInt :
+ logStream << ';' << step.ints .at(desc.index);
+ break;
+ case enum_t::NodeTypeLong :
+ logStream << ';' << step.longs .at(desc.index);
+ break;
+ case enum_t::NodeTypeFloat :
+ logStream << ';' << step.floats .at(desc.index);
+ break;
+ case enum_t::NodeTypeDouble:
+ logStream << ';' << step.doubles.at(desc.index);
+ break;
+ };
+ }
+ logStream << std::endl;
+ }
+ data.clear();
+}
diff --git a/source/RobotAPI/gui-plugins/RobotUnitPlugin/RobotUnitPlugin/RobotUnitPluginWidgetController.h b/source/RobotAPI/gui-plugins/RobotUnitPlugin/RobotUnitPlugin/RobotUnitPluginWidgetController.h
index 06270ecbc7e9c7280fac72f6dac959e504db26bd..d1111ffa0913679ff35361e5ca8f5160123f7eff 100644
--- a/source/RobotAPI/gui-plugins/RobotUnitPlugin/RobotUnitPlugin/RobotUnitPluginWidgetController.h
+++ b/source/RobotAPI/gui-plugins/RobotUnitPlugin/RobotUnitPlugin/RobotUnitPluginWidgetController.h
@@ -28,6 +28,7 @@
#include <map>
#include <string>
#include <chrono>
+#include <fstream>
#include <QWidget>
#include <QPushButton>
@@ -45,6 +46,7 @@
#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
#include <RobotAPI/libraries/RobotAPIVariantWidget/RobotAPIVariantWidget.h>
+#include <RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.h>
#include "../QWidgets/NJointControllerClassesWidget.h"
#include "../QWidgets/NJointControllersWidget.h"
@@ -99,6 +101,7 @@ namespace armarx
void onConnectComponent() override;
void onDisconnectComponent() override;
+ void onExitComponent() override;
/**
* @param parent The dialog's parent.
* @return The plugin's config dialog.
@@ -134,9 +137,8 @@ namespace armarx
void refreshSensorDevicesClicked();
void refreshLogging();
- void writeLogClicked();
-
void startOnConnectTimer();
+ void stopOnConnectTimer();
//logging
void on_lineEditLoggingFilter_textChanged(const QString& arg1);
@@ -158,6 +160,7 @@ namespace armarx
QPointer<SimpleConfigDialog> dialog;
std::string robotUnitProxyName;
+ std::recursive_mutex robotUnitPrxMutex;
RobotUnitInterfacePrx robotUnitPrx;
std::string listenerTopicName;
@@ -176,15 +179,19 @@ namespace armarx
struct LoggingData
{
- QTreeWidgetItem* top;
- std::vector<QTreeWidgetItem*> allItems;
- RemoteReferenceCounterBasePtr handle;
+ QTreeWidgetItem* top;
+ std::vector<QTreeWidgetItem*> allItems;
+ RemoteReferenceCounterBasePtr handle;
+ RobotUnitDataStreamingReceiverPtr streamingHandler;
+ std::ofstream logStream;
static void updateCheckStateUpward(QTreeWidgetItem* item, bool recurseParents);
static void updateCheckStateDownward(QTreeWidgetItem* item, Qt::CheckState state, bool recurseChildren);
+ void localLogging();
};
LoggingData loggingData;
- int onConnectTimerId;
+ int timerId = 0;
+ std::atomic_bool timerLastIterationRuWasRunning = false;
};
}
diff --git a/source/RobotAPI/interface/CMakeLists.txt b/source/RobotAPI/interface/CMakeLists.txt
index 07e18bc2313f269eed1cb8fd0735bd1c9fc4e91a..7cae7fe9e851dc8eaa152ea59cb8e1f8bd905adf 100644
--- a/source/RobotAPI/interface/CMakeLists.txt
+++ b/source/RobotAPI/interface/CMakeLists.txt
@@ -98,6 +98,7 @@ set(SLICE_FILES
aron.ice
armem.ice
+ test2.ice
components/ObstacleAvoidance/ObstacleAvoidanceInterface.ice
components/ObstacleAvoidance/ObstacleDetectionInterface.ice
diff --git a/source/RobotAPI/interface/armem.ice b/source/RobotAPI/interface/armem.ice
index 986fc89ab2f020256ef7f6f209b79d05dd324d85..f5ced2684babf02ebe711b89efbe580f82f765a1 100644
--- a/source/RobotAPI/interface/armem.ice
+++ b/source/RobotAPI/interface/armem.ice
@@ -6,25 +6,49 @@ module armarx
{
module armem
{
- dictionary<long, aron::AronData> TimestampedAronDataList;
+ // data in a commit
+ class ArMemCommit
+ {
+ string producer;
+ string stored_in_segment;
+ long produce_timestamp_ms;
+ long storage_timestamp_ms;
+ aron::AronDataList data;
+ };
+
+ // map storageTS to commit
+ dictionary<long, ArMemCommit> TimestampedArMemCommitList;
+
- interface ArMemWorkingMemoryReceiverInterface
+ interface ArMemReceiverInterface
{
- aron::AronData getLatestEntryFromProducer(string producer);
- aron::AronData getNextEntryFromProducerForTimestamp(string producer, long timestamp);
- TimestampedAronDataList getAllEntriesBetweenTimestampsFromProducer(string producer, long timestamp1, long timestamp2);
- TimestampedAronDataList getAllEntriesFromProducer(string producer);
+ ArMemCommit getLatestCommitFromSegment(string segment);
+ ArMemCommit getNextCommitFromSegmentForTimestamp(string segment, long timestamp);
+ TimestampedArMemCommitList getAllCommitsBetweenTimestampsFromSegment(string segment, long timestamp1, long timestamp2);
+ TimestampedArMemCommitList getAllCommitsFromSegment(string segment);
};
- interface ArMemWorkingMemoryProducerInterface
+ interface ArMemProducerInterface
{
- void sendToMemory(string producer, long produceTimestamp, aron::AronData value);
+ string commit(string segment, long produceTimestamp, aron::AronDataList values);
+ string commit_single(string segment, long produceTimestamp, aron::AronData value);
};
- interface ArMemWorkingMemoryInterface
- extends ArMemWorkingMemoryReceiverInterface, ArMemWorkingMemoryProducerInterface
+ interface ArMemLocalMemoryInterface
+ extends ArMemReceiverInterface, ArMemProducerInterface
{
};
+
+ interface ArMemGlobalMemoryResolver
+ {
+ string getHostnameOfCurrentMachine();
+ ArMemLocalMemoryInterface* getMemoryOfCurrentMachine();
+ ArMemLocalMemoryInterface* getMemoryForHostname(string hostname);
+
+ void dynamicallyRegisterNewLocalMemory(string hostname);
+
+ void exportDataOfAllMemoriesToLocation(string location);
+ }
};
};
diff --git a/source/RobotAPI/interface/aron.ice b/source/RobotAPI/interface/aron.ice
index 365bb23f1c0b098df499b3f643ea4d8e2134fc94..5a9f621f41d74b2db44825c5d402bde5494c76c5 100644
--- a/source/RobotAPI/interface/aron.ice
+++ b/source/RobotAPI/interface/aron.ice
@@ -10,39 +10,60 @@ module armarx
{
module aron
{
+ /*************************
+ * General Definitions ***
+ ************************/
+ sequence<byte> AronBlobSequence;
+ sequence<int> AronIntSequence;
- class AronData { };
- sequence<AronData> AronDataList;
- dictionary<string, AronData> AronDataDict;
+ /*************************
+ * Aron Types ************
+ ************************/
+ class AronAbstractType { };
+ sequence<AronAbstractType> AronAbstractTypeList;
+ dictionary<string, AronAbstractType> AronAbstractTypeDict;
- sequence<byte> AronBlobSeq;
+ // External Type
+ class AronExternalType extends AronAbstractType
+ { };
- // List Types
- class AronList extends AronData { AronDataList elements; };
- class AronObject extends AronData { AronDataDict elements; };
- /*class AronBlob extends AronData {
- sequence<int> dimensions;
- string type;
- sequence<byte> data
- };*/
-
- // Basic Types
+ // Container Types
+ class AronListType extends AronAbstractType { AronAbstractType acceptedType; };
+ class AronObjectType extends AronAbstractType { string name; AronAbstractTypeDict elementTypes; };
+ class AronDictType extends AronAbstractType { AronAbstractType acceptedType; };
+ class AronTupleType extends AronAbstractType { AronAbstractTypeList elementTypes; };
+
+ // Complex Types
+ class AronEigenMatrixType extends AronAbstractType { AronIntSequence dimensions; string typeName; };
+ class AronIVTCByteImageType extends AronAbstractType { int width; int height; string typeName; };
+ class AronOpenCVMatType extends AronAbstractType { AronIntSequence dimensions; string typeName; };
+ class AronPCLPointcloudType extends AronAbstractType { int width; int height; string typeName; };
+
+ // Primitive Types
#define RUN_ARON_MACRO(cppType, upperType, lowerType, capsType) \
- class Aron##upperType extends AronData { cppType value; };
+ class Aron##upperType##Type extends AronAbstractType { };
HANDLE_PRIMITIVE_TYPES_WITH_CPP_TYPE
#undef RUN_ARON_MACRO
- class AronAbstractType {
- string cppTypename;
- };
- dictionary<string, AronAbstractType> AronAbstractTypeDict;
- class AronListType extends AronAbstractType { AronAbstractType accepted; };
- class AronObjectType extends AronAbstractType { string name; string declaredIn; AronAbstractTypeDict elements; };
+ /*************************
+ * Aron Data *************
+ ************************/
+ class AronData { };
+ sequence<AronData> AronDataList;
+ dictionary<string, AronData> AronDataDict;
+ // Container Data
+ class AronList extends AronData { AronDataList elements; };
+ class AronDict extends AronData { AronDataDict elements; };
+
+ // Complex Data
+ class AronBlob extends AronData { AronBlobSequence data; };
+
+ // Basic Data
#define RUN_ARON_MACRO(cppType, upperType, lowerType, capsType) \
- class Aron##upperType##Type extends AronAbstractType { };
+ class Aron##upperType extends AronData { cppType value; };
HANDLE_PRIMITIVE_TYPES_WITH_CPP_TYPE
#undef RUN_ARON_MACRO
diff --git a/source/RobotAPI/interface/components/NaturalIKInterface.ice b/source/RobotAPI/interface/components/NaturalIKInterface.ice
index c2067cc9e7079206cd8dc027ad3de8f759b65fcd..a5627f61ef822deaa31e59eb065a2032a877ddcd 100644
--- a/source/RobotAPI/interface/components/NaturalIKInterface.ice
+++ b/source/RobotAPI/interface/components/NaturalIKInterface.ice
@@ -36,6 +36,6 @@ module armarx
interface NaturalIKInterface
{
- NaturalIKResult solveIK(string side, Eigen::Matrix4f target, bool setOrientation, aron::AronObject args);
+ NaturalIKResult solveIK(string side, Eigen::Matrix4f target, bool setOrientation, aron::AronDict args);
};
};
diff --git a/source/RobotAPI/interface/components/TrajectoryPlayerInterface.ice b/source/RobotAPI/interface/components/TrajectoryPlayerInterface.ice
index 3e0f1eded9f05946c493584146891e8bc01632d0..7807f84419960a8219f5a809feccf66e08d6f76a 100644
--- a/source/RobotAPI/interface/components/TrajectoryPlayerInterface.ice
+++ b/source/RobotAPI/interface/components/TrajectoryPlayerInterface.ice
@@ -24,6 +24,8 @@
#include <RobotAPI/interface/core/Trajectory.ice>
#include <RobotAPI/interface/observers/KinematicUnitObserverInterface.ice>
+#include <ArmarXCore/interface/serialization/Eigen.ice>
+#include <RobotAPI/interface/aron.ice>
module armarx
{
@@ -43,6 +45,8 @@ module armarx
bool setJointsInUse(string jointName, bool inUse);
void enableRobotPoseUnit(bool isRobotPose);
+ void setOffset(Eigen::Matrix4f offset);
+
double getCurrentTime();
double getEndTime();
double getTrajEndTime();
diff --git a/source/RobotAPI/interface/test2.ice b/source/RobotAPI/interface/test2.ice
new file mode 100644
index 0000000000000000000000000000000000000000..fee9ebdb587177059032ba24da09ccdfd9a32067
--- /dev/null
+++ b/source/RobotAPI/interface/test2.ice
@@ -0,0 +1,9 @@
+#pragma once
+
+module armarx
+{
+ module test
+ {
+ dictionary<long, int> someTest;
+ };
+};
diff --git a/source/RobotAPI/interface/units/RobotUnit/NJointBimanualObjLevelController.ice b/source/RobotAPI/interface/units/RobotUnit/NJointBimanualObjLevelController.ice
index a00ef88cca841dec09a24ba64e1a3e7059ceb931..7488ad443518fc4fbce80b247a2a0070acafd5e5 100644
--- a/source/RobotAPI/interface/units/RobotUnit/NJointBimanualObjLevelController.ice
+++ b/source/RobotAPI/interface/units/RobotUnit/NJointBimanualObjLevelController.ice
@@ -121,9 +121,151 @@ module armarx
Ice::FloatSeq getCurrentObjVel();
Ice::FloatSeq getCurrentObjForce();
+
+ void setMPWeights(DoubleSeqSeq weights);
+ DoubleSeqSeq getMPWeights();
+ void setMPRotWeights(DoubleSeqSeq weights);
+ DoubleSeqSeq getMPRotWeights();
+ };
+
+ class NJointBimanualObjLevelVelControllerConfig extends NJointControllerConfig
+ {
+
+ // dmp configuration
+ int kernelSize = 10;
+ string dmpMode = "Linear";
+ string dmpType = "Discrete";
+ double timeDuration = 10;
+
+ Ice::FloatSeq leftDesiredJointValues;
+ Ice::FloatSeq rightDesiredJointValues;
+
+ Ice::FloatSeq KpImpedance;
+ Ice::FloatSeq KdImpedance;
+
+ float knull;
+ float dnull;
+
+ float jointVelLimit;
+ float jointLimitAvoidanceKp;
+
+ };
+
+ interface NJointBimanualObjLevelVelControllerInterface extends NJointControllerInterface
+ {
+ void learnDMPFromFiles(Ice::StringSeq trajfiles);
+ bool isFinished();
+ void runDMP(Ice::DoubleSeq goals, double timeDuration);
+ void runDMPWithVirtualStart(Ice::DoubleSeq starts, Ice::DoubleSeq goals, double timeDuration);
+
+ void setGoals(Ice::DoubleSeq goals);
+ void setViaPoints(double u, Ice::DoubleSeq viapoint);
+ void removeAllViaPoints();
+ double getVirtualTime();
+
+ void setKpImpedance(Ice::FloatSeq value);
+ void setKdImpedance(Ice::FloatSeq value);
+
+ Ice::FloatSeq getCurrentObjVel();
+
+ void setMPWeights(DoubleSeqSeq weights);
+ DoubleSeqSeq getMPWeights();
+ void setMPRotWeights(DoubleSeqSeq weights);
+ DoubleSeqSeq getMPRotWeights();
};
+ class NJointBimanualObjLevelMultiMPControllerConfig extends NJointControllerConfig
+ {
+ // dmp configuration
+ int kernelSize = 100;
+ string dmpMode = "MinimumJerk";
+ string dmpObjType = "Discrete";
+ string dmpLeftType = "Discrete";
+ string dmpRightType = "Discrete";
+ float dmpAmplitude = 1.0;
+
+ // phaseStop technique
+ double phaseL = 10;
+ double phaseK = 10;
+ double phaseDist0 = 50;
+ double phaseDist1 = 10;
+ double phaseKpPos = 1;
+ double phaseKpOri = 0.1;
+ double posToOriRatio = 10;
+ double timeDuration = 10;
+
+ Ice::DoubleSeq objInitialPose;
+ Ice::FloatSeq leftDesiredJointValues;
+ Ice::FloatSeq rightDesiredJointValues;
+
+ // impedance, admittance, object motion parameters
+ Ice::FloatSeq KpImpedance;
+ Ice::FloatSeq KdImpedance;
+ Ice::FloatSeq KpAdmittance;
+ Ice::FloatSeq KdAdmittance;
+ Ice::FloatSeq KmAdmittance;
+ Ice::FloatSeq KmPID;
+
+ // pid force controller parameters
+ Ice::FloatSeq targetWrench;
+ Ice::FloatSeq forceP;
+ Ice::FloatSeq forceI;
+ Ice::FloatSeq forceD;
+ Ice::FloatSeq forcePIDLimits;
+
+ float filterCoeff;
+
+
+ float massLeft;
+ Ice::FloatSeq CoMVecLeft;
+ Ice::FloatSeq forceOffsetLeft;
+ Ice::FloatSeq torqueOffsetLeft;
+
+ float massRight;
+ Ice::FloatSeq CoMVecRight;
+ Ice::FloatSeq forceOffsetRight;
+ Ice::FloatSeq torqueOffsetRight;
+
+ float knull;
+ float dnull;
+
+ float torqueLimit;
+
+ Ice::FloatSeq forceThreshold;
+
+ double ftCalibrationTime;
+
+ };
+
+ interface NJointBimanualObjLevelMultiMPControllerInterface extends NJointControllerInterface
+ {
+ void learnDMPFromFiles(Ice::StringSeq trajfiles);
+ void learnMultiDMPFromFiles(Ice::StringSeq objFileNames, Ice::StringSeq leftFileNames, Ice::StringSeq rightFileNames);
+
+ bool isFinished();
+ void runDMP(Ice::DoubleSeq goalObj, Ice::DoubleSeq goalLeft, Ice::DoubleSeq goalRight, double timeDuration);
+ void runDMPWithVirtualStart(Ice::DoubleSeq starts, Ice::DoubleSeq goals, double timeDuration);
+
+// void setViaPoints(double canVal, Ice::DoubleSeq point);
+ void setGoals(Ice::DoubleSeq goals);
+ void setMultiMPGoals(Ice::DoubleSeq goalObj, Ice::DoubleSeq goalLeft, Ice::DoubleSeq goalRight);
+
+ void setViaPoints(double u, Ice::DoubleSeq viapoint);
+ void removeAllViaPoints();
+
+ double getVirtualTime();
+
+ void setKpImpedance(Ice::FloatSeq value);
+ void setKdImpedance(Ice::FloatSeq value);
+ void setKmAdmittance(Ice::FloatSeq value);
+ void setKpAdmittance(Ice::FloatSeq value);
+ void setKdAdmittance(Ice::FloatSeq value);
+ void setAmplitude(double amp);
+
+ Ice::FloatSeq getCurrentObjVel();
+ Ice::FloatSeq getCurrentObjForce();
+ };
};
diff --git a/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice b/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
index 3bc4c6eef67b6e7a9320fd16b2ccf82319268e2b..e2949400f8bcf7bfd03aa679de8406b5ae7f08b0 100644
--- a/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
+++ b/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
@@ -102,6 +102,11 @@ module armarx
string getDMPAsString();
Ice::DoubleSeq createDMPFromString(string dmpString);
+
+ void setMPWeights(DoubleSeqSeq weights);
+ DoubleSeqSeq getMPWeights();
+
+
};
class NJointCCDMPControllerConfig extends NJointControllerConfig
@@ -333,8 +338,64 @@ module armarx
void stopDMP();
void resumeDMP();
+ void setMPWeights(DoubleSeqSeq weights);
+ DoubleSeqSeq getMPWeights();
+
};
+ class NJointTaskSpaceAdaptiveDMPControllerConfig extends NJointControllerConfig
+ {
+
+ // dmp configuration
+ int kernelSize = 100;
+ string dmpMode = "MinimumJerk";
+ string dmpType = "Discrete";
+ double timeDuration;
+ string nodeSetName;
+
+ Ice::FloatSeq Kpos;
+ Ice::FloatSeq Dpos;
+ Ice::FloatSeq Kori;
+ Ice::FloatSeq Dori;
+ Ice::FloatSeq Knull;
+ Ice::FloatSeq Dnull;
+
+ bool useNullSpaceJointDMP;
+ Ice::FloatSeq defaultNullSpaceJointValues;
+
+ float torqueLimit;
+ string forceSensorName;
+ float filterCoeff;
+ };
+
+ interface NJointTaskSpaceAdaptiveDMPControllerInterface extends NJointControllerInterface
+ {
+ void learnDMPFromFiles(Ice::StringSeq trajfiles);
+ void learnJointDMPFromFiles(string jointTrajFile, Ice::FloatSeq currentJVS);
+
+ bool isFinished();
+ void runDMP(Ice::DoubleSeq goals);
+ void runDMPWithTime(Ice::DoubleSeq goals, double timeDuration);
+
+ void setViaPoints(double canVal, Ice::DoubleSeq point);
+ void setGoals(Ice::DoubleSeq goals);
+
+ double getVirtualTime();
+ void setCanVal(double canVal);
+
+ void resetDMP();
+ void stopDMP();
+ void resumeDMP();
+
+ void setKdImpedance(Ice::FloatSeq dampings);
+ void setKpImpedance(Ice::FloatSeq stiffness);
+ void setKpNull(Ice::FloatSeq knull);
+ void setKdNull(Ice::FloatSeq dnull);
+ Ice::FloatSeq getForce();
+ Ice::FloatSeq getVelocityInMM();
+ };
+
+
class NJointPeriodicTSDMPControllerConfig extends NJointControllerConfig
{
@@ -582,6 +643,7 @@ module armarx
int frictionHorizon = 100;
// pid params
+ bool isForceCtrlInForceDir;
bool isForceControlEnabled;
bool isRotControlEnabled;
bool isTorqueControlEnabled;
@@ -639,6 +701,7 @@ module armarx
bool loseContactDetectionEnabled;
int loseContactCounterMax;
float rotAngleSigmoid;
+ bool useDMPInGlobalFrame;
};
@@ -661,6 +724,9 @@ module armarx
Ice::FloatSeq getAnomalyOutput();
void setTrigerAbnormalEvent(bool abnormal);
+
+ void pauseDMP();
+ void resumeDMP();
};
};
diff --git a/source/RobotAPI/interface/units/RobotUnit/RobotUnitInterface.ice b/source/RobotAPI/interface/units/RobotUnit/RobotUnitInterface.ice
index 361cf690a3498a2a9ffcb95527ef27005c54fe86..c7871f0860fbe343a1704b2022f54a8ed6cd8674 100644
--- a/source/RobotAPI/interface/units/RobotUnit/RobotUnitInterface.ice
+++ b/source/RobotAPI/interface/units/RobotUnit/RobotUnitInterface.ice
@@ -46,6 +46,7 @@
#include <ArmarXCore/interface/core/UserException.ice>
#include <ArmarXGui/interface/WidgetDescription.ice>
+//NJointController
module armarx
{
interface NJointControllerInterface;
@@ -61,7 +62,7 @@ module armarx
};
- class NJointControllerConfig{};
+ class NJointControllerConfig {};
struct NJointControllerDescription
{
@@ -117,10 +118,10 @@ module armarx
dictionary<string, NJointControllerInterface*> StringNJointControllerPrxDictionary;
};
+//RobotUnit Utility types - ControlDevice
module armarx
{
dictionary<string, Ice::StringSeq> ControlDeviceNameToControlModesDictionary;
-
dictionary<string, string> ControlDeviceNameToNJointControllerNameDictionary;
struct HWControlModeAndTargetType
@@ -146,8 +147,10 @@ module armarx
long timestampUSec = 0;
};
sequence<ControlDeviceStatus> ControlDeviceStatusSeq;
-
-
+}
+//RobotUnit Utility types - SensorDevice
+module armarx
+{
struct SensorDeviceDescription
{
string deviceName;
@@ -162,15 +165,76 @@ module armarx
long timestampUSec = 0;
};
sequence<SensorDeviceStatus> SensorDeviceStatusSeq;
-
-
+}
+//RobotUnit Utility types - NJointController
+module armarx
+{
struct NJointControllerClassDescription
{
string className;
WidgetDescription::Widget configDescription;
};
sequence<NJointControllerClassDescription> NJointControllerClassDescriptionSeq;
+}
+//RobotUnit Utility types - data streaming
+module armarx
+{
+ module RobotUnitDataStreaming
+ {
+ enum DataEntryType
+ {
+ NodeTypeBool,
+ NodeTypeByte,
+ NodeTypeShort,
+ NodeTypeInt,
+ NodeTypeLong,
+ NodeTypeFloat,
+ NodeTypeDouble
+ };
+
+ struct DataEntry
+ {
+ DataEntryType type;
+ long index = -1;
+ };
+ dictionary<string, DataEntry> StringDataEntryMap;
+
+ struct DataStreamingDescription
+ {
+ StringDataEntryMap entries;
+ };
+
+ struct TimeStep
+ {
+ long iterationId;
+ long timestampUSec;
+ long timesSinceLastIterationUSec;
+ Ice::BoolSeq bools;
+ Ice::ByteSeq bytes;
+ Ice::ShortSeq shorts;
+ Ice::IntSeq ints;
+ Ice::LongSeq longs;
+ Ice::FloatSeq floats;
+ Ice::DoubleSeq doubles;
+ };
+ sequence<TimeStep> TimeStepSeq;
+
+ interface Receiver
+ {
+ void update(TimeStepSeq data);
+ };
+
+ struct Config
+ {
+ Ice::StringSeq loggingNames;
+ };
+ }
+}
+
+//RobotUnit Listener
+module armarx
+{
interface RobotUnitListener
{
void nJointControllerStatusChanged(NJointControllerStatusSeq status);
@@ -180,7 +244,11 @@ module armarx
void nJointControllerCreated(string instanceName);
void nJointControllerDeleted(string instanceName);
};
+}
+//RobotUnit Modules
+module armarx
+{
module RobotUnitModule
{
interface RobotUnitManagementInterface extends AggregatedRobotHealthInterface
@@ -199,6 +267,9 @@ module armarx
void stopRtLogging(RemoteReferenceCounterBase token) throws LogicError;
["cpp:const"] void writeRecentIterationsToFile(string filePathFormatString) throws LogicError, InvalidArgumentException;
+
+ RobotUnitDataStreaming::DataStreamingDescription startDataStreaming(RobotUnitDataStreaming::Receiver* receiver, RobotUnitDataStreaming::Config config);
+ void stopDataStreaming(RobotUnitDataStreaming::Receiver* receiver);
};
interface RobotUnitUnitInterface
{
@@ -210,9 +281,6 @@ module armarx
["cpp:const"] idempotent PlatformUnitInterface* getPlatformUnit();
["cpp:const"] idempotent TCPControlUnitInterface* getTCPControlUnit();
["cpp:const"] idempotent TrajectoryPlayerInterface* getTrajectoryPlayer();
-
-
-
};
interface RobotUnitPublishingInterface
{
@@ -302,16 +370,20 @@ module armarx
["cpp:const"] idempotent EmergencyStopState getRtEmergencyStopState();
};
};
+}
+//RobotUnit
+module armarx
+{
interface RobotUnitInterface extends
- RobotUnitModule::RobotUnitUnitInterface,
- RobotUnitModule::RobotUnitDevicesInterface,
- RobotUnitModule::RobotUnitLoggingInterface,
- RobotUnitModule::RobotUnitPublishingInterface,
- RobotUnitModule::RobotUnitManagementInterface,
- RobotUnitModule::RobotUnitControlThreadInterface,
- RobotUnitModule::RobotUnitControllerManagementInterface,
- RobotUnitModule::RobotUnitSelfCollisionCheckerInterface
+ RobotUnitModule::RobotUnitUnitInterface,
+ RobotUnitModule::RobotUnitDevicesInterface,
+ RobotUnitModule::RobotUnitLoggingInterface,
+ RobotUnitModule::RobotUnitPublishingInterface,
+ RobotUnitModule::RobotUnitManagementInterface,
+ RobotUnitModule::RobotUnitControlThreadInterface,
+ RobotUnitModule::RobotUnitControllerManagementInterface,
+ RobotUnitModule::RobotUnitSelfCollisionCheckerInterface
{
};
};
diff --git a/source/RobotAPI/libraries/CMakeLists.txt b/source/RobotAPI/libraries/CMakeLists.txt
index cea5d29ab918c5a315037290ece8415f8e54bd16..451adc4dc73effa8abae7fde4ffe147e363c8c49 100644
--- a/source/RobotAPI/libraries/CMakeLists.txt
+++ b/source/RobotAPI/libraries/CMakeLists.txt
@@ -21,4 +21,5 @@ add_subdirectory(armem)
add_subdirectory(aron)
add_subdirectory(NJointControllerGuiPluginUtility)
-add_subdirectory(RobotAPINJointControllerWidgets)
\ No newline at end of file
+add_subdirectory(RobotAPINJointControllerWidgets)
+add_subdirectory(RobotUnitDataStreamingReceiver)
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/DMPController/TaskSpaceDMPController.h b/source/RobotAPI/libraries/DMPController/TaskSpaceDMPController.h
index 9cb91dae9d98f8ee798851c37c0d60282a09414c..18072a11b5822596d271a5e0576ac97421ef7d90 100644
--- a/source/RobotAPI/libraries/DMPController/TaskSpaceDMPController.h
+++ b/source/RobotAPI/libraries/DMPController/TaskSpaceDMPController.h
@@ -200,6 +200,58 @@ namespace armarx
this->paused = false;
}
+ void setWeights(const std::vector<std::vector<double> >& weights)
+ {
+ dmpPtr->setWeights(weights);
+ }
+
+ void setTranslWeights(const std::vector<std::vector<double> >& weights)
+ {
+ ARMARX_CHECK_EQUAL(weights.size(), 3);
+
+ for (size_t i = 0; i < 3; ++i)
+ {
+ dmpPtr->setWeights(i, weights[i]);
+ }
+ }
+
+ void setRotWeights(const std::vector<std::vector<double> >& weights)
+ {
+ ARMARX_CHECK_EQUAL(weights.size(), 4);
+
+ for (size_t i = 0; i < 4; ++i)
+ {
+ dmpPtr->setWeights(3 + i, weights[i]);
+ }
+ }
+
+ DMP::DVec2d getWeights()
+ {
+ return dmpPtr->getWeights();
+ }
+
+ DMP::DVec2d getTranslWeights()
+ {
+ DMP::DVec2d res;
+ DMP::DVec2d weights = getWeights();
+ for (size_t i = 0; i < 3; ++i)
+ {
+ res.push_back(weights[i]);
+ }
+ return res;
+ }
+
+ DMP::DVec2d getRotWeights()
+ {
+ DMP::DVec2d res;
+ DMP::DVec2d weights = getWeights();
+ for (size_t i = 3; i < 7; ++i)
+ {
+ res.push_back(weights[i]);
+ }
+ return res;
+ }
+
double canVal;
bool isPhaseStopControl;
std::string dmpName;
diff --git a/source/RobotAPI/libraries/NJointControllerGuiPluginUtility/CMakeLists.txt b/source/RobotAPI/libraries/NJointControllerGuiPluginUtility/CMakeLists.txt
index b044f98be56f2ee7fe799cf3272aac3120d30ccb..94e91cdf9d477c838b7b18f47b56cbcfbb4a5be3 100644
--- a/source/RobotAPI/libraries/NJointControllerGuiPluginUtility/CMakeLists.txt
+++ b/source/RobotAPI/libraries/NJointControllerGuiPluginUtility/CMakeLists.txt
@@ -11,9 +11,8 @@ set(SOURCES
detail/NJointControllerGuiPluginBase.cpp
)
set(HEADERS
- SpinBoxToPose.h
- SpinBoxToVector.h
NJointControllerGuiPluginBase.h
+ detail/NJointControllerGuiPluginBase.h
)
set(GUI_MOC_HDRS detail/NJointControllerGuiPluginBase.h)
set(GUI_UIS)
diff --git a/source/RobotAPI/libraries/NJointControllerGuiPluginUtility/SpinBoxToPose.h b/source/RobotAPI/libraries/NJointControllerGuiPluginUtility/SpinBoxToPose.h
deleted file mode 100644
index c22b4b823b9788e44e2660c8d1f93d6677b1cd9a..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/NJointControllerGuiPluginUtility/SpinBoxToPose.h
+++ /dev/null
@@ -1,115 +0,0 @@
-/*
- * 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::ArmarXObjects::NJointControllerGuiPluginUtility
- * @author Raphael Grimm ( raphael dot grimm at kit dot edu )
- * @date 2020
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#pragma once
-
-#include <SimoxUtility/math/convert.h>
-
-#include "SpinBoxToVector.h"
-
-namespace armarx
-{
- template<class Qwid = QDoubleSpinBox>
- class SpinBoxToPose
- {
- //set
- public:
- void setPos(const Eigen::Vector3f& pos)
- {
- _xyz.set(pos);
- }
- void setOri(const Eigen::Vector3f& rpy)
- {
- _rpy.set(rpy);
- }
- void setOri(const Eigen::Quaternionf& q)
- {
- setOri(simox::math::quat_to_rpy(q));
- }
- void setOri(const Eigen::Matrix3f& m)
- {
- setOri(simox::math::mat3f_to_rpy(m));
- }
- void setPose(const auto& pos, const auto& ori)
- {
- setPos(pos);
- setOri(ori);
- }
- void setPose(const Eigen::Matrix4f& m)
- {
- setPos(simox::math::mat4f_to_pos(m));
- setOri(simox::math::mat4f_to_rpy(m));
- }
- //get
- public:
- Eigen::Vector3f getPos() const
- {
- return _xyz.template get<Eigen::Vector3f>();
- }
- Eigen::Vector3f getRPY() const
- {
- return _rpy.template get<Eigen::Vector3f>();
- }
- Eigen::Quaternionf getQuat() const
- {
- return simox::math::rpy_to_quat(getRPY());
- }
- Eigen::Matrix3f getMat3() const
- {
- return simox::math::rpy_to_mat3f(getRPY());
- }
- Eigen::Matrix4f getMat4() const
- {
- return simox::math::pos_rpy_to_mat4f(getPos(), getRPY());
- }
- //set up
- public:
- void setXYZWidgets(Qwid* x, Qwid* y, Qwid* z)
- {
- _xyz.addWidget(x);
- _xyz.addWidget(y);
- _xyz.addWidget(z);
- }
- void setRPYWidgets(Qwid* r, Qwid* p, Qwid* y)
- {
- _rpy.addWidget(r);
- _rpy.addWidget(p);
- _rpy.addWidget(y);
- }
- SpinBoxToVector<Qwid, 3>& xyzWidgets()
- {
- return _xyz;
- }
- SpinBoxToVector<Qwid, 3>& rpyWidgets()
- {
- return _rpy;
- }
- void setDefaultLimits()
- {
- _xyz.setMinMax(-100'000, 100'000);
- _rpy.setMinMax(-9, 9);
- }
- private:
- SpinBoxToVector<Qwid, 3> _xyz;
- SpinBoxToVector<Qwid, 3> _rpy;
- };
-}
diff --git a/source/RobotAPI/libraries/NJointControllerGuiPluginUtility/SpinBoxToVector.h b/source/RobotAPI/libraries/NJointControllerGuiPluginUtility/SpinBoxToVector.h
deleted file mode 100644
index 3fe18f27c0fb2e32df2f44492cc6b71d1d162369..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/NJointControllerGuiPluginUtility/SpinBoxToVector.h
+++ /dev/null
@@ -1,202 +0,0 @@
-/*
- * 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::ArmarXObjects::NJointControllerGuiPluginUtility
- * @author Raphael Grimm ( raphael dot grimm at kit dot edu )
- * @date 2020
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#pragma once
-
-#include <array>
-#include <vector>
-
-#include <Eigen/Dense>
-
-#include <QDoubleSpinBox>
-
-#include <VirtualRobot/RobotNodeSet.h>
-#include <VirtualRobot/Nodes/RobotNode.h>
-
-#include <ArmarXCore/util/CPPUtility/trace.h>
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-
-namespace armarx
-{
- template<class Qwid = QDoubleSpinBox, std::size_t Size = 0>
- class SpinBoxToVector
- {
- public:
- template<class Scalar, int Rows, int Cols>
- void get(Eigen::Matrix<Scalar, Rows, Cols>& m) const
- {
- ARMARX_TRACE;
- if constexpr(Size)
- {
- ARMARX_CHECK_EQUAL(Size, _widgets.size());
- }
- if constexpr(Rows == -1 && Cols == -1)
- {
- m.resize(_widgets.size(), 1);
- }
- else if constexpr(Rows == 1 && Cols == -1)
- {
- m.resize(_widgets.size());
- }
- else if constexpr(Rows == -1 && Cols == 1)
- {
- m.resize(_widgets.size());
- }
- else if constexpr(Cols == -1)
- {
- m.resize(Rows, _widgets.size() / Rows);
- }
- else if constexpr(Rows == -1)
- {
- m.resize(_widgets.size() / Cols, Cols);
- }
-
- ARMARX_TRACE;
- ARMARX_CHECK_EQUAL(static_cast<std::size_t>(m.size()), _widgets.size());
- for (std::size_t i = 0; i < _widgets.size(); ++i)
- {
- m.data()[i] = _widgets.at(i)->value();
- }
- }
- template<class Scalar>
- void get(std::vector<Scalar>& m) const
- {
- ARMARX_TRACE;
- if constexpr(Size)
- {
- ARMARX_CHECK_EQUAL(Size, _widgets.size());
- }
- m.resize(_widgets.size());
- ARMARX_CHECK_EQUAL(m.size(), _widgets.size());
- for (std::size_t i = 0; i < _widgets.size(); ++i)
- {
- m.at(i) = _widgets.at(i)->value();
- }
- }
- template<class T>
- T get() const
- {
- ARMARX_TRACE;
- if constexpr(Size)
- {
- ARMARX_CHECK_EQUAL(Size, _widgets.size());
- }
- T m;
- get(m);
- return m;
- }
- template<class Scalar, int Rows, int Cols>
- void set(const Eigen::Matrix<Scalar, Rows, Cols>& m)
- {
- ARMARX_TRACE;
- if constexpr(Size)
- {
- ARMARX_CHECK_EQUAL(Size, _widgets.size());
- }
- ARMARX_CHECK_EQUAL(m.size(), _widgets.size());
- for (int i = 0; i < _widgets.size(); ++i)
- {
- _widgets.at(i)->setValue(m.data()[i]);
- }
- }
- void set(const VirtualRobot::RobotNodeSetPtr& set)
- {
- ARMARX_TRACE;
- ARMARX_CHECK_NOT_NULL(set);
- ARMARX_CHECK_EQUAL(set->getSize(), _widgets.size());
- for (std::size_t i = 0; i < _widgets.size(); ++i)
- {
- ARMARX_CHECK_NOT_NULL(set->getNode(i));
- _widgets.at(i)->setValue(set->getNode(i)->getJointValue());
- }
- }
- void set(double d)
- {
- for (auto w : _widgets)
- {
- w->setValue(d);
- }
- }
- public:
- void addWidget(Qwid* s)
- {
- ARMARX_TRACE;
- ARMARX_CHECK_NOT_NULL(s);
- if constexpr(Size)
- {
- ARMARX_CHECK_LESS(_sz, Size);
- _widgets.at(_sz) = s;
- }
- else
- {
- _widgets.emplace_back(s);
- }
- ++_sz;
- }
- void clear()
- {
- ARMARX_TRACE;
- _sz = 0;
- if constexpr(!Size)
- {
- _widgets.clear();
- }
- }
- void visitWidgets(const auto& f)
- {
- for (auto w : _widgets)
- {
- f(w);
- }
- }
- void setMin(auto min)
- {
- for (auto w : _widgets)
- {
- w->setMinimum(min);
- }
- }
- void setMax(auto max)
- {
- for (auto w : _widgets)
- {
- w->setMaximum(max);
- }
- }
- void setMinMax(auto min, auto max)
- {
- setMin(min);
- setMax(max);
- }
- auto& widgets()
- {
- return _widgets;
- }
- private:
- unsigned _sz = 0;
- std::conditional_t <
- Size,
- std::array<Qwid*, Size>,
- std::vector<Qwid*>
- > _widgets;
- };
-}
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.cpp b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.cpp
index f20e5cf17eebc0c4b8bf607571270b4d952f292e..e711da48631a7562fbc748ff2ef5d0d26d6fd84c 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.cpp
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.cpp
@@ -20,6 +20,7 @@
* GNU General Public License
*/
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
+#include <RobotAPI/libraries/core/FramedPose.h>
#include "RobotStateComponentPlugin.h"
@@ -272,6 +273,17 @@ namespace armarx::plugins
{
return _robotStateComponentName;
}
+
+ Eigen::Matrix4f RobotStateComponentPlugin::transformFromTo(
+ const std::string& from,
+ const std::string& to,
+ const VirtualRobot::RobotPtr& rob)
+ {
+ ARMARX_CHECK_NOT_NULL(rob);
+ armarx::FramedPose fp{Eigen::Matrix4f::Identity(), from, rob->getName()};
+ fp.changeFrame(rob, to);
+ return fp.toEigen();
+ }
}
namespace armarx
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.h b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.h
index ddbc64ab666aff4008c77434a86cdb0b62c5d1fe..80e96096b61881ff96df7b50d894cdf50f33d758 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.h
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.h
@@ -88,9 +88,14 @@ namespace armarx::plugins
RobotData getRobotData(const std::string& id) const;
void setRobotRNSAndNode(const std::string& id, const std::string& rnsName, const std::string& nodeName);
+ //querry
+ public:
const RobotStateComponentInterfacePrx& getRobotStateComponent() const;
-
const RobotNameHelper& getRobotNameHelper() const;
+ Eigen::Matrix4f transformFromTo(const std::string& from,
+ const std::string& to,
+ const VirtualRobot::RobotPtr& rob);
+
//sync
public:
bool synchronizeLocalClone(const VirtualRobot::RobotPtr& robot) const;
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllerWidgets/CartesianImpedanceControllerConfigWidget.h b/source/RobotAPI/libraries/RobotAPINJointControllerWidgets/CartesianImpedanceControllerConfigWidget.h
index b29450d2c4afa1699169f06e7e0dde2156a55c38..acf84cb4ede9f5e56d8f278596d0b11c146af5a9 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllerWidgets/CartesianImpedanceControllerConfigWidget.h
+++ b/source/RobotAPI/libraries/RobotAPINJointControllerWidgets/CartesianImpedanceControllerConfigWidget.h
@@ -5,8 +5,8 @@
#include <VirtualRobot/RobotNodeSet.h>
-#include <RobotAPI/libraries/NJointControllerGuiPluginUtility/SpinBoxToVector.h>
-#include <RobotAPI/libraries/NJointControllerGuiPluginUtility/SpinBoxToPose.h>
+#include <ArmarXGui/libraries/ArmarXGuiBase/SpinBoxToVector.h>
+#include <ArmarXGui/libraries/ArmarXGuiBase/SpinBoxToPose.h>
#include <RobotAPI/interface/units/RobotUnit/TaskSpaceActiveImpedanceControl.h>
#include <RobotAPI/libraries/RobotAPINJointControllerWidgets/ui_CartesianImpedanceControllerConfigWidget.h>
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualObjLevelController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualObjLevelController.cpp
index 04843c8950800d4da142cbfe8dcfe0b84773b817..74e8c8dceb83a53134ff155e772fcad08b3bec1f 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualObjLevelController.cpp
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualObjLevelController.cpp
@@ -9,8 +9,7 @@ namespace armarx
NJointBimanualObjLevelController::NJointBimanualObjLevelController(const RobotUnitPtr& robUnit, const armarx::NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&)
{
- ARMARX_INFO << "Preparing ... bimanual ";
- ARMARX_INFO << "I am here";
+ ARMARX_INFO << "Initializing Bimanual Object Level Controller";
useSynchronizedRtRobot();
cfg = NJointBimanualObjLevelControllerConfigPtr::dynamicCast(config);
@@ -278,6 +277,49 @@ namespace armarx
}
+ void NJointBimanualObjLevelController::setMPWeights(const DoubleSeqSeq& weights, const Ice::Current&)
+ {
+ objectDMP->setWeights(weights);
+ }
+
+ DoubleSeqSeq NJointBimanualObjLevelController::getMPWeights(const Ice::Current&)
+ {
+ DMP::DVec2d res = objectDMP->getWeights();
+ DoubleSeqSeq resvec;
+ for (size_t i = 0; i < res.size(); ++i)
+ {
+ std::vector<double> cvec;
+ for (size_t j = 0; j < res[i].size(); ++j)
+ {
+ cvec.push_back(res[i][j]);
+ }
+ resvec.push_back(cvec);
+ }
+
+ return resvec;
+ }
+
+ void NJointBimanualObjLevelController::setMPRotWeights(const DoubleSeqSeq& weights, const Ice::Current&)
+ {
+ objectDMP->setRotWeights(weights);
+ }
+
+ DoubleSeqSeq NJointBimanualObjLevelController::getMPRotWeights(const Ice::Current&)
+ {
+ DMP::DVec2d res = objectDMP->getRotWeights();
+ DoubleSeqSeq resvec;
+ for (size_t i = 0; i < res.size(); ++i)
+ {
+ std::vector<double> cvec;
+ for (size_t j = 0; j < res[i].size(); ++j)
+ {
+ cvec.push_back(res[i][j]);
+ }
+ resvec.push_back(cvec);
+ }
+
+ return resvec;
+ }
void NJointBimanualObjLevelController::rtPreActivateController()
{
@@ -600,7 +642,6 @@ namespace armarx
tcpTargetPoseLeft.block<3, 1>(0, 3) += virtualPose.block<3, 3>(0, 0) * (objCom2TCPLeftInObjFrame - deltaPoseForWrenchControl.block<3, 1>(0, 0));
tcpTargetPoseRight.block<3, 1>(0, 3) += virtualPose.block<3, 3>(0, 0) * (objCom2TCPRightInObjFrame - deltaPoseForWrenchControl.block<3, 1>(6, 0));
-
// --------------------------------------------- Impedance control ---------------------------------------------
Eigen::VectorXf poseError(12);
Eigen::Matrix3f diffMat = tcpTargetPoseLeft.block<3, 3>(0, 0) * currentLeftPose.block<3, 3>(0, 0).transpose();
@@ -702,6 +743,11 @@ namespace armarx
debugOutputData.getWriteBuffer().currentPoseLeft_z = currentLeftPose(2, 3);
+ VirtualRobot::MathTools::Quaternion leftQuat = VirtualRobot::MathTools::eigen4f2quat(currentLeftPose);
+ debugOutputData.getWriteBuffer().leftQuat_w = leftQuat.w;
+ debugOutputData.getWriteBuffer().leftQuat_x = leftQuat.x;
+ debugOutputData.getWriteBuffer().leftQuat_y = leftQuat.y;
+ debugOutputData.getWriteBuffer().leftQuat_z = leftQuat.y;
debugOutputData.getWriteBuffer().modifiedPoseLeft_x = tcpTargetPoseLeft(0, 3);
debugOutputData.getWriteBuffer().modifiedPoseLeft_y = tcpTargetPoseLeft(1, 3);
@@ -711,6 +757,12 @@ namespace armarx
debugOutputData.getWriteBuffer().currentPoseRight_y = currentRightPose(1, 3);
debugOutputData.getWriteBuffer().currentPoseRight_z = currentRightPose(2, 3);
+ VirtualRobot::MathTools::Quaternion rightQuat = VirtualRobot::MathTools::eigen4f2quat(currentRightPose);
+ debugOutputData.getWriteBuffer().rightQuat_w = rightQuat.w;
+ debugOutputData.getWriteBuffer().rightQuat_x = rightQuat.x;
+ debugOutputData.getWriteBuffer().rightQuat_y = rightQuat.y;
+ debugOutputData.getWriteBuffer().rightQuat_z = rightQuat.y;
+
debugOutputData.getWriteBuffer().dmpBoxPose_x = boxPose(0, 3);
debugOutputData.getWriteBuffer().dmpBoxPose_y = boxPose(1, 3);
@@ -1022,6 +1074,14 @@ namespace armarx
datafields["currentPoseLeft_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentPoseLeft_y);
datafields["currentPoseLeft_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentPoseLeft_z);
+ datafields["leftQuat_w"] = new Variant(debugOutputData.getUpToDateReadBuffer().leftQuat_w);
+ datafields["leftQuat_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().leftQuat_x);
+ datafields["leftQuat_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().leftQuat_y);
+ datafields["leftQuat_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().leftQuat_z);
+ datafields["rightQuat_w"] = new Variant(debugOutputData.getUpToDateReadBuffer().rightQuat_w);
+ datafields["rightQuat_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().rightQuat_x);
+ datafields["rightQuat_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().rightQuat_y);
+ datafields["rightQuat_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().rightQuat_z);
datafields["modifiedPoseLeft_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().modifiedPoseLeft_x);
datafields["modifiedPoseLeft_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().modifiedPoseLeft_y);
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualObjLevelController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualObjLevelController.h
index 6d56350dfedc13c3d33e3330dd8266aea13b5002..efb7929b59dcb85fb4dd58d5174c689f5fd55119 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualObjLevelController.h
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualObjLevelController.h
@@ -86,6 +86,11 @@ namespace armarx
std::vector<float> getCurrentObjVel(const Ice::Current&);
std::vector<float> getCurrentObjForce(const Ice::Current&);
+ void setMPWeights(const DoubleSeqSeq& weights, const Ice::Current&);
+ DoubleSeqSeq getMPWeights(const Ice::Current&);
+
+ void setMPRotWeights(const DoubleSeqSeq& weights, const Ice::Current&);
+ DoubleSeqSeq getMPRotWeights(const Ice::Current&);
protected:
virtual void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&);
@@ -134,6 +139,14 @@ namespace armarx
float currentPoseLeft_x;
float currentPoseLeft_y;
float currentPoseLeft_z;
+ float leftQuat_w;
+ float leftQuat_x;
+ float leftQuat_y;
+ float leftQuat_z;
+ float rightQuat_w;
+ float rightQuat_x;
+ float rightQuat_y;
+ float rightQuat_z;
float modifiedPoseLeft_x;
float modifiedPoseLeft_y;
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualObjLevelMultiMPController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualObjLevelMultiMPController.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..26a9107eaa18578a1974c4045e28c9f72fbbde70
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualObjLevelMultiMPController.cpp
@@ -0,0 +1,1305 @@
+#include "NJointBimanualObjLevelMultiMPController.h"
+
+#include <ArmarXCore/core/time/CycleUtil.h>
+#include <ArmarXCore/core/ArmarXObjectScheduler.h>
+
+namespace armarx
+{
+ NJointControllerRegistration<NJointBimanualObjLevelMultiMPController> registrationControllerNJointBimanualObjLevelMultiMPController("NJointBimanualObjLevelMultiMPController");
+
+ NJointBimanualObjLevelMultiMPController::NJointBimanualObjLevelMultiMPController(const RobotUnitPtr& robUnit, const armarx::NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&)
+ {
+ ARMARX_INFO << "Initializing Bimanual Object Level Controller";
+
+ // initialize robot kinematic chain and sensors
+ useSynchronizedRtRobot();
+ cfg = NJointBimanualObjLevelMultiMPControllerConfigPtr::dynamicCast(config);
+
+ leftRNS = rtGetRobot()->getRobotNodeSet("LeftArm");
+
+ for (size_t i = 0; i < leftRNS->getSize(); ++i)
+ {
+ std::string jointName = leftRNS->getNode(i)->getName();
+ leftJointNames.push_back(jointName);
+ ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Torque1DoF);
+ const SensorValueBase* sv = useSensorValue(jointName);
+ leftTargets.push_back(ct->asA<ControlTarget1DoFActuatorTorque>());
+ const SensorValue1DoFActuatorVelocity* velocitySensor = sv->asA<SensorValue1DoFActuatorVelocity>();
+ const SensorValue1DoFActuatorPosition* positionSensor = sv->asA<SensorValue1DoFActuatorPosition>();
+
+ if (!velocitySensor)
+ {
+ ARMARX_WARNING << "No velocitySensor available for " << jointName;
+ }
+ if (!positionSensor)
+ {
+ ARMARX_WARNING << "No positionSensor available for " << jointName;
+ }
+
+
+ leftVelocitySensors.push_back(velocitySensor);
+ leftPositionSensors.push_back(positionSensor);
+
+ };
+
+ rightRNS = rtGetRobot()->getRobotNodeSet("RightArm");
+
+ for (size_t i = 0; i < rightRNS->getSize(); ++i)
+ {
+ std::string jointName = rightRNS->getNode(i)->getName();
+ rightJointNames.push_back(jointName);
+ ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Torque1DoF);
+ const SensorValueBase* sv = useSensorValue(jointName);
+ rightTargets.push_back(ct->asA<ControlTarget1DoFActuatorTorque>());
+
+ const SensorValue1DoFActuatorVelocity* velocitySensor = sv->asA<SensorValue1DoFActuatorVelocity>();
+ const SensorValue1DoFActuatorPosition* positionSensor = sv->asA<SensorValue1DoFActuatorPosition>();
+
+ if (!velocitySensor)
+ {
+ ARMARX_WARNING << "No velocitySensor available for " << jointName;
+ }
+ if (!positionSensor)
+ {
+ ARMARX_WARNING << "No positionSensor available for " << jointName;
+ }
+
+ rightVelocitySensors.push_back(velocitySensor);
+ rightPositionSensors.push_back(positionSensor);
+
+ };
+
+
+ const SensorValueBase* svlf = robUnit->getSensorDevice("FT L")->getSensorValue();
+ leftForceTorque = svlf->asA<SensorValueForceTorque>();
+ const SensorValueBase* svrf = robUnit->getSensorDevice("FT R")->getSensorValue();
+ rightForceTorque = svrf->asA<SensorValueForceTorque>();
+
+ leftIK.reset(new VirtualRobot::DifferentialIK(leftRNS, leftRNS->getRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
+ rightIK.reset(new VirtualRobot::DifferentialIK(rightRNS, rightRNS->getRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
+
+ tcpLeft = leftRNS->getTCP();
+ tcpRight = rightRNS->getTCP();
+
+ /* ----------------- initialize DMP ----------------- */
+ TaskSpaceDMPControllerConfig taskSpaceDMPConfig;
+ taskSpaceDMPConfig.DMPKernelSize = cfg->kernelSize;
+ taskSpaceDMPConfig.DMPMode = cfg->dmpMode;
+ taskSpaceDMPConfig.DMPAmplitude = cfg->dmpAmplitude;
+ taskSpaceDMPConfig.phaseStopParas.goDist = cfg->phaseDist0;
+ taskSpaceDMPConfig.phaseStopParas.backDist = cfg->phaseDist1;
+ taskSpaceDMPConfig.phaseStopParas.Kpos = cfg->phaseKpPos;
+ taskSpaceDMPConfig.phaseStopParas.Dpos = 0;
+ taskSpaceDMPConfig.phaseStopParas.Kori = cfg->phaseKpOri;
+ taskSpaceDMPConfig.phaseStopParas.Dori = 0;
+ taskSpaceDMPConfig.phaseStopParas.mm2radi = cfg->posToOriRatio;
+ taskSpaceDMPConfig.phaseStopParas.maxValue = cfg->phaseL;
+ taskSpaceDMPConfig.phaseStopParas.slop = cfg->phaseK;
+ taskSpaceDMPConfig.motionTimeDuration = cfg->timeDuration;
+
+ taskSpaceDMPConfig.DMPStyle = cfg->dmpObjType;
+ objectDMP.reset(new TaskSpaceDMPController("objDMP", taskSpaceDMPConfig, false));
+ taskSpaceDMPConfig.DMPStyle = cfg->dmpLeftType;
+ leftTCPInObjDMP.reset(new TaskSpaceDMPController("leftDMP", taskSpaceDMPConfig, false));
+ taskSpaceDMPConfig.DMPStyle = cfg->dmpRightType;
+ rightTCPInObjDMP.reset(new TaskSpaceDMPController("rightDMP", taskSpaceDMPConfig, false));
+ ARMARX_IMPORTANT << "dmps initialized";
+
+ /* ----------------- initialize control parameters ----------------- */
+ auto setParamVec = [&](Eigen::VectorXf & param, ::Ice::FloatSeq & cfgParam, const size_t n)
+ {
+ param.setZero(n);
+ ARMARX_CHECK_EQUAL(cfgParam.size(), n);
+
+ for (size_t i = 0; i < n; ++i)
+ {
+ param(i) = cfgParam.at(i);
+ }
+ };
+
+ setParamVec(KpImpedance, cfg->KpImpedance, 12);
+ setParamVec(KdImpedance, cfg->KdImpedance, 12);
+ setParamVec(KpAdmittance, cfg->KpAdmittance, 6);
+ setParamVec(KdAdmittance, cfg->KdAdmittance, 6);
+ setParamVec(KmAdmittance, cfg->KmAdmittance, 6);
+ setParamVec(KmPID, cfg->KmPID, 6);
+
+ ARMARX_INFO << "got controller params";
+
+ /* ------------- initialize default joint values ------------------- */
+ leftDesiredJointValues.resize(leftTargets.size());
+ ARMARX_CHECK_EQUAL(cfg->leftDesiredJointValues.size(), leftTargets.size());
+
+ for (size_t i = 0; i < leftTargets.size(); ++i)
+ {
+ leftDesiredJointValues(i) = cfg->leftDesiredJointValues.at(i);
+ }
+
+ rightDesiredJointValues.resize(rightTargets.size());
+ ARMARX_CHECK_EQUAL(cfg->rightDesiredJointValues.size(), rightTargets.size());
+
+ for (size_t i = 0; i < rightTargets.size(); ++i)
+ {
+ rightDesiredJointValues(i) = cfg->rightDesiredJointValues.at(i);
+ }
+
+ /* ------------- obtain object initial poses ------------------- */
+ // object pose (position in meter)
+ objInitialPose = VirtualRobot::MathTools::quat2eigen4f(cfg->objInitialPose[4], cfg->objInitialPose[5], cfg->objInitialPose[6], cfg->objInitialPose[3]);
+ for (int i = 0; i < 3; ++i)
+ {
+ objInitialPose(i, 3) = cfg->objInitialPose[i];
+ }
+
+ virtualAcc.setZero(6);
+ virtualVel.setZero(6);
+ virtualPose = objInitialPose;
+
+ objTransMatrixInAnchor = Eigen::Matrix4f::Identity();
+
+ // obtain left & right initial pose in the object frame (in meter)
+ leftInitialPose = Eigen::Matrix4f::Identity();
+ rightInitialPose = Eigen::Matrix4f::Identity();
+ Eigen::Matrix4f leftPose = tcpLeft->getPoseInRootFrame();
+ Eigen::Matrix4f rightPose = tcpRight->getPoseInRootFrame();
+ leftInitialPose.block<3, 3>(0, 0) = objInitialPose.block<3, 3>(0, 0).transpose() * leftPose.block<3, 3>(0, 0);
+ leftInitialPose.block<3, 1>(0, 3) = objInitialPose.block<3, 3>(0, 0).transpose() * (leftPose.block<3, 1>(0, 3) * 0.001 - objInitialPose.block<3, 1>(0, 3));
+ rightInitialPose.block<3, 3>(0, 0) = objInitialPose.block<3, 3>(0, 0).transpose() * rightPose.block<3, 3>(0, 0);
+ rightInitialPose.block<3, 1>(0, 3) = objInitialPose.block<3, 3>(0, 0).transpose() * (rightPose.block<3, 1>(0, 3) * 0.001 - objInitialPose.block<3, 1>(0, 3));
+
+ integratedPoseObj = virtualPose;
+ integratedPoseLeft = leftInitialPose; // need to be represented in the object local frame.
+ integratedPoseRight = rightInitialPose;
+ /* ------------- initialize buffers ------------------- */
+ // interface to RT
+ Inferface2rtData interface2rtData;
+ interface2rtData.KpImpedance = KpImpedance;
+ interface2rtData.KdImpedance = KdImpedance;
+ interface2rtData.KmAdmittance = KmAdmittance;
+ interface2rtData.KpAdmittance = KpAdmittance;
+ interface2rtData.KdAdmittance = KdAdmittance;
+ interface2rtBuffer.reinitAllBuffers(interface2rtData);
+
+ // RT to DMP
+ RT2ControlData rt2ControlData;
+ rt2ControlData.deltaT = 0;
+ rt2ControlData.currentTime = 0;
+ rt2ControlData.currentPoseObj = objInitialPose;
+ rt2ControlData.currentTwistObj.setZero();
+ rt2ControlData.currentPoseObj = leftInitialPose;
+ rt2ControlData.currentTwistObj.setZero();
+ rt2ControlData.currentPoseObj = rightInitialPose;
+ rt2ControlData.currentTwistObj.setZero();
+ rt2ControlBuffer.reinitAllBuffers(rt2ControlData);
+
+ // DMP (or interface) to RT Target
+ NJointBimanualObjLevelMultiMPControlData initData;
+ initData.objTargetPose = objInitialPose;
+ initData.objTargetTwist.setZero(6);
+
+ initData.leftTargetPoseInObj = leftInitialPose;
+ initData.leftTargetTwistInObj.setZero(6);
+
+ initData.rightTargetPoseInObj = rightInitialPose;
+ initData.rightTargetTwistInObj.setZero(6);
+ reinitTripleBuffer(initData);
+
+ // Control interface: RT to interface
+ RT2InterfaceData rt2InterfaceData;
+ rt2InterfaceData.currentLeftPoseInObjFrame = leftInitialPose;
+ rt2InterfaceData.currentRightPoseInObjFrame = rightInitialPose;
+ rt2InterfaceData.currentObjPose = objInitialPose;
+ rt2InterfaceData.currentObjForce.setZero();
+ rt2InterfaceData.currentObjVel.setZero();
+ rt2InterfaceBuffer.reinitAllBuffers(rt2InterfaceData);
+
+
+ /* ------------- initialize PID force controller ------------------- */
+ forcePIDControllers.resize(12);
+ for (size_t i = 0; i < 6; i++)
+ {
+ forcePIDControllers.at(i).reset(new PIDController(cfg->forceP[i], cfg->forceI[i], cfg->forceD[i], cfg->forcePIDLimits[i]));
+ forcePIDControllers.at(i + 6).reset(new PIDController(cfg->forceP[i], cfg->forceI[i], cfg->forceD[i], cfg->forcePIDLimits[i]));
+ forcePIDControllers.at(i)->reset();
+ forcePIDControllers.at(i + 6)->reset();
+ }
+
+ /* ------------- initialize force torque sensor related ------------------- */
+ // sensor frame to tcp frame transformation
+ sensorFrame2TcpFrameLeft.setZero();
+ sensorFrame2TcpFrameRight.setZero();
+
+ // filter
+ filterCoeff = cfg->filterCoeff;
+ filteredOldValue.setZero(12);
+
+ // first loop calibrate
+ ftcalibrationTimer = 0;
+ ftOffset.setZero(12);
+
+ // target wrench
+ targetWrench.setZero(cfg->targetWrench.size());
+ for (size_t i = 0; i < cfg->targetWrench.size(); ++i)
+ {
+ targetWrench(i) = cfg->targetWrench[i];
+ }
+
+ /* ------------- static compensation ------------------- */
+ massLeft = cfg->massLeft;
+ CoMVecLeft << cfg->CoMVecLeft[0], cfg->CoMVecLeft[1], cfg->CoMVecLeft[2];
+ forceOffsetLeft << cfg->forceOffsetLeft[0], cfg->forceOffsetLeft[1], cfg->forceOffsetLeft[2];
+ torqueOffsetLeft << cfg->torqueOffsetLeft[0], cfg->torqueOffsetLeft[1], cfg->torqueOffsetLeft[2];
+
+ massRight = cfg->massRight;
+ CoMVecRight << cfg->CoMVecRight[0], cfg->CoMVecRight[1], cfg->CoMVecRight[2];
+ forceOffsetRight << cfg->forceOffsetRight[0], cfg->forceOffsetRight[1], cfg->forceOffsetRight[2];
+ torqueOffsetRight << cfg->torqueOffsetRight[0], cfg->torqueOffsetRight[1], cfg->torqueOffsetRight[2];
+
+ /* ------------- Pose Offset ------------------- */
+ fixedLeftRightRotOffset = Eigen::Matrix3f::Identity();
+ objCom2TCPLeftInObjFrame.setZero();
+ objCom2TCPRightInObjFrame.setZero();
+
+ /* ------------- setup flags ------------------- */
+ firstLoop = true;
+ dmpStarted = false;
+ ARMARX_IMPORTANT << "finished construction!";
+ }
+
+
+ void NJointBimanualObjLevelMultiMPController::rtPreActivateController()
+ {}
+
+ std::string NJointBimanualObjLevelMultiMPController::getClassName(const Ice::Current&) const
+ {
+ return "NJointBimanualObjLevelMultiMPController";
+ }
+
+ void NJointBimanualObjLevelMultiMPController::controllerRun()
+ {
+ if (!rt2ControlBuffer.updateReadBuffer() || !dmpStarted)
+ {
+ return;
+ }
+
+ // get status from RT loop
+ double deltaT = rt2ControlBuffer.getReadBuffer().deltaT;
+ Eigen::Matrix4f currentPoseObj = rt2ControlBuffer.getReadBuffer().currentPoseObj;
+ Eigen::VectorXf currentTwistObj = rt2ControlBuffer.getReadBuffer().currentTwistObj;
+ Eigen::Matrix4f currentPoseLeftInObj = rt2ControlBuffer.getReadBuffer().currentPoseLeftInObj;
+ Eigen::VectorXf currentTwistLeftInObj = rt2ControlBuffer.getReadBuffer().currentTwistLeftInObj;
+ Eigen::Matrix4f currentPoseRightInObj = rt2ControlBuffer.getReadBuffer().currentPoseRightInObj;
+ Eigen::VectorXf currentTwistRightInObj = rt2ControlBuffer.getReadBuffer().currentTwistRightInObj;
+
+ // set can val from outside
+ rightTCPInObjDMP->flow(deltaT, currentPoseRightInObj, currentTwistRightInObj);
+
+ // DMP flow control
+ bool objectDMPFinished = false;
+ bool leftTCPInObjDMPFinished = false;
+ bool rightTCPInObjDMPFinished = false;
+
+ if (objectDMP->canVal > 1e-8 || objectDMP->config.DMPStyle == "Periodic")
+ {
+ objectDMP->flow(deltaT, currentPoseObj, currentTwistObj);
+ getWriterControlStruct().objTargetPose = objectDMP->getTargetPoseMat();
+ getWriterControlStruct().objTargetTwist = objectDMP->getTargetVelocity();
+ }
+ else
+ {
+ objectDMPFinished = true;
+ }
+
+
+ if (leftTCPInObjDMP->canVal > 1e-8 || leftTCPInObjDMP->config.DMPStyle == "Periodic")
+ {
+ leftTCPInObjDMP->flow(deltaT, currentPoseLeftInObj, currentTwistLeftInObj);
+ getWriterControlStruct().leftTargetPoseInObj = leftTCPInObjDMP->getTargetPoseMat();
+ getWriterControlStruct().leftTargetTwistInObj = leftTCPInObjDMP->getTargetVelocity();
+ }
+ else
+ {
+ leftTCPInObjDMPFinished = true;
+ }
+
+ if (rightTCPInObjDMP->canVal > 1e-8 || rightTCPInObjDMP->config.DMPStyle == "Periodic")
+ {
+ rightTCPInObjDMP->flow(deltaT, currentPoseRightInObj, currentTwistRightInObj);
+ getWriterControlStruct().rightTargetPoseInObj = rightTCPInObjDMP->getTargetPoseMat();
+ getWriterControlStruct().rightTargetTwistInObj = rightTCPInObjDMP->getTargetVelocity();
+ }
+ else
+ {
+ rightTCPInObjDMPFinished = true;
+ }
+
+
+ if (objectDMPFinished && leftTCPInObjDMPFinished && rightTCPInObjDMPFinished)
+ {
+ finished = true;
+ dmpStarted = false;
+ }
+
+ // set target to RT loop
+ LockGuardType guard {controlDataMutex};
+ writeControlStruct();
+ }
+
+ void NJointBimanualObjLevelMultiMPController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+ {
+ // ------------------------------------------- current tcp pose -------------------------------------------
+ Eigen::Matrix4f currentLeftPose = tcpLeft->getPoseInRootFrame();
+ Eigen::Matrix4f currentRightPose = tcpRight->getPoseInRootFrame();
+
+ currentLeftPose.block<3, 1>(0, 3) = 0.001 * currentLeftPose.block<3, 1>(0, 3);
+ currentRightPose.block<3, 1>(0, 3) = 0.001 * currentRightPose.block<3, 1>(0, 3);
+
+ double deltaT = timeSinceLastIteration.toSecondsDouble();
+ ftcalibrationTimer += deltaT;
+
+ if (firstLoop)
+ {
+ Eigen::Matrix4f leftPose = currentLeftPose;
+ Eigen::Matrix4f rightPose = currentRightPose;
+
+ Eigen::Vector3f objCom2TCPLeftInGlobal = leftPose.block<3, 1>(0, 3) - virtualPose.block<3, 1>(0, 3);
+ Eigen::Vector3f objCom2TCPRightInGlobal = rightPose.block<3, 1>(0, 3) - virtualPose.block<3, 1>(0, 3);
+
+ // Temporary (until we could detect and set object pose by perception)
+ // T_obj = T_{leftHand} * T_{objTransMatrixInAnchor}
+ objTransMatrixInAnchor.block<3, 3>(0, 0) = leftPose.block<3, 3>(0, 0).transpose() * objInitialPose.block<3, 3>(0, 0);
+ objTransMatrixInAnchor.block<3, 1>(0, 3) = leftPose.block<3, 3>(0, 0).transpose() * (objInitialPose.block<3, 1>(0, 3) - leftPose.block<3, 1>(0, 3));
+
+ objCom2TCPLeftInObjFrame = virtualPose.block<3, 3>(0, 0).transpose() * objCom2TCPLeftInGlobal;
+ objCom2TCPRightInObjFrame = virtualPose.block<3, 3>(0, 0).transpose() * objCom2TCPRightInGlobal;
+
+
+ Eigen::Matrix4f leftSensorFrame = leftRNS->getRobot()->getRobotNode("ArmL8_Wri2")->getPoseInRootFrame();
+ Eigen::Matrix4f rightSensorFrame = rightRNS->getRobot()->getRobotNode("ArmR8_Wri2")->getPoseInRootFrame();
+ leftSensorFrame.block<3, 1>(0, 3) = leftSensorFrame.block<3, 1>(0, 3) * 0.001;
+ rightSensorFrame.block<3, 1>(0, 3) = rightSensorFrame.block<3, 1>(0, 3) * 0.001;
+
+ // sensor frame 2 tcp frame transformation
+ sensorFrame2TcpFrameLeft.block<3, 3>(0, 0) = leftPose.block<3, 3>(0, 0).transpose() * leftSensorFrame.block<3, 3>(0, 0);
+ sensorFrame2TcpFrameRight.block<3, 3>(0, 0) = rightPose.block<3, 3>(0, 0).transpose() * rightSensorFrame.block<3, 3>(0, 0);
+ CoMVecLeft = sensorFrame2TcpFrameLeft.block<3, 3>(0, 0) * CoMVecLeft;
+ CoMVecRight = sensorFrame2TcpFrameRight.block<3, 3>(0, 0) * CoMVecRight;
+ firstLoop = false;
+ }
+
+ Eigen::Matrix4f currentLeftPoseInObjFrame = Eigen::Matrix4f::Identity();
+ Eigen::Matrix4f currentRightPoseInObjFrame = Eigen::Matrix4f::Identity();
+ currentLeftPoseInObjFrame.block<3, 3>(0, 0) = virtualPose.block<3, 3>(0, 0).transpose() * currentLeftPose.block<3, 3>(0, 0);
+ currentLeftPoseInObjFrame.block<3, 1>(0, 3) = virtualPose.block<3, 3>(0, 0).transpose() * (currentLeftPose.block<3, 1>(0, 3) - virtualPose.block<3, 1>(0, 3));
+ currentRightPoseInObjFrame.block<3, 3>(0, 0) = virtualPose.block<3, 3>(0, 0).transpose() * currentRightPose.block<3, 3>(0, 0);
+ currentRightPoseInObjFrame.block<3, 1>(0, 3) = virtualPose.block<3, 3>(0, 0).transpose() * (currentRightPose.block<3, 1>(0, 3) - virtualPose.block<3, 1>(0, 3));
+ rt2InterfaceBuffer.getWriteBuffer().currentLeftPoseInObjFrame = currentLeftPoseInObjFrame;
+ rt2InterfaceBuffer.getWriteBuffer().currentRightPoseInObjFrame = currentRightPoseInObjFrame;
+
+
+ // --------------------------------------------- get control parameters ---------------------------------------
+ KpImpedance = interface2rtBuffer.getUpToDateReadBuffer().KpImpedance;
+ KdImpedance = interface2rtBuffer.getUpToDateReadBuffer().KdImpedance;
+ KmAdmittance = interface2rtBuffer.getUpToDateReadBuffer().KmAdmittance;
+ KpAdmittance = interface2rtBuffer.getUpToDateReadBuffer().KpAdmittance;
+ KdAdmittance = interface2rtBuffer.getUpToDateReadBuffer().KdAdmittance;
+
+ if (ftcalibrationTimer < cfg->ftCalibrationTime)
+ {
+ ftOffset.block<3, 1>(0, 0) = 0.5 * ftOffset.block<3, 1>(0, 0) + 0.5 * leftForceTorque->force;
+ ftOffset.block<3, 1>(3, 0) = 0.5 * ftOffset.block<3, 1>(3, 0) + 0.5 * leftForceTorque->torque;
+ ftOffset.block<3, 1>(6, 0) = 0.5 * ftOffset.block<3, 1>(6, 0) + 0.5 * rightForceTorque->force;
+ ftOffset.block<3, 1>(9, 0) = 0.5 * ftOffset.block<3, 1>(9, 0) + 0.5 * rightForceTorque->torque;
+
+ for (int i = 0; i < KmAdmittance.size(); ++i)
+ {
+ KmAdmittance(i) = 0;
+ }
+ }
+ else
+ {
+ for (int i = 0; i < KmAdmittance.size(); ++i)
+ {
+ KmAdmittance(i) = cfg->KmAdmittance.at(i);
+ }
+ }
+
+ // -------------------------------------------- target wrench ---------------------------------------------
+ Eigen::VectorXf deltaPoseForWrenchControl;
+ deltaPoseForWrenchControl.setZero(12);
+ for (size_t i = 0; i < 12; ++i)
+ {
+ if (KpImpedance(i) == 0)
+ {
+ deltaPoseForWrenchControl(i) = 0;
+ }
+ else
+ {
+ deltaPoseForWrenchControl(i) = targetWrench(i) / KpImpedance(i);
+ if (deltaPoseForWrenchControl(i) > 0.1)
+ {
+ deltaPoseForWrenchControl(i) = 0.1;
+ }
+ else if (deltaPoseForWrenchControl(i) < -0.1)
+ {
+ deltaPoseForWrenchControl(i) = -0.1;
+ }
+ // deltaPoseForWrenchControl(i + 6) = targetWrench(i + 6) / KpImpedance(i);
+ }
+ }
+
+
+ // --------------------------------------------- grasp matrix ---------------------------------------------
+
+ Eigen::Vector3f objCom2TCPLeftInGlobal = currentLeftPose.block<3, 1>(0, 3) - virtualPose.block<3, 1>(0, 3);
+ Eigen::Vector3f objCom2TCPRightInGlobal = currentRightPose.block<3, 1>(0, 3) - virtualPose.block<3, 1>(0, 3);
+
+ Eigen::MatrixXf graspMatrix;
+ graspMatrix.setZero(6, 12);
+ graspMatrix.block<3, 3>(0, 0) = Eigen::MatrixXf::Identity(3, 3);
+ graspMatrix.block<3, 3>(0, 6) = Eigen::MatrixXf::Identity(3, 3);
+ // graspMatrix.block<6, 6>(0, 0) = Eigen::MatrixXf::Identity(6, 6);
+ // graspMatrix.block<6, 6>(0, 6) = Eigen::MatrixXf::Identity(6, 6);
+
+ // Eigen::Vector3f rvec = virtualPose.block<3, 3>(0, 0) * objCom2TCPLeftInObjFrame;
+ graspMatrix.block<3, 3>(3, 0) = skew(objCom2TCPLeftInGlobal);
+
+ // rvec = virtualPose.block<3, 3>(0, 0) * objCom2TCPRightInObjFrame;
+ graspMatrix.block<3, 3>(3, 6) = skew(objCom2TCPRightInGlobal);
+
+ // // projection of grasp matrix
+ // Eigen::MatrixXf pinvG = leftIK->computePseudoInverseJacobianMatrix(graspMatrix, 0);
+ // Eigen::MatrixXf G_range = pinvG * graspMatrix;
+ // Eigen::MatrixXf PG = Eigen::MatrixXf::Identity(12, 12) - G_range;
+ float lambda = 1;
+ Eigen::MatrixXf pinvGraspMatrixT = leftIK->computePseudoInverseJacobianMatrix(graspMatrix.transpose(), lambda);
+
+ // ---------------------------------------------- object pose ----------------------------------------------
+ Eigen::Matrix4f objCurrentPose = currentLeftPose * objTransMatrixInAnchor;
+ Eigen::VectorXf objCurrentTwist;
+ // getObjStatus(objCurrentPose, objCurrentTwist);
+ objCurrentTwist.setZero(6);
+
+ // -------------------------------------- get Jacobian matrix and qpos -------------------------------------
+ Eigen::MatrixXf I = Eigen::MatrixXf::Identity(leftTargets.size(), leftTargets.size());
+ // Jacobian matrices
+ Eigen::MatrixXf jacobiL = leftIK->getJacobianMatrix(tcpLeft, VirtualRobot::IKSolver::CartesianSelection::All);
+ Eigen::MatrixXf jacobiR = rightIK->getJacobianMatrix(tcpRight, VirtualRobot::IKSolver::CartesianSelection::All);
+ jacobiL.block<3, 8>(0, 0) = 0.001 * jacobiL.block<3, 8>(0, 0);
+ jacobiR.block<3, 8>(0, 0) = 0.001 * jacobiR.block<3, 8>(0, 0);
+
+ // qpos, qvel
+ Eigen::VectorXf leftqpos;
+ Eigen::VectorXf leftqvel;
+ leftqpos.resize(leftPositionSensors.size());
+ leftqvel.resize(leftVelocitySensors.size());
+ for (size_t i = 0; i < leftVelocitySensors.size(); ++i)
+ {
+ leftqpos(i) = leftPositionSensors[i]->position;
+ leftqvel(i) = leftVelocitySensors[i]->velocity;
+ }
+
+ Eigen::VectorXf rightqpos;
+ Eigen::VectorXf rightqvel;
+ rightqpos.resize(rightPositionSensors.size());
+ rightqvel.resize(rightVelocitySensors.size());
+
+ for (size_t i = 0; i < rightVelocitySensors.size(); ++i)
+ {
+ rightqpos(i) = rightPositionSensors[i]->position;
+ rightqvel(i) = rightVelocitySensors[i]->velocity;
+ }
+
+ // -------------------------------------- compute TCP and object velocity -------------------------------------
+ Eigen::VectorXf currentLeftTwist = jacobiL * leftqvel;
+ Eigen::VectorXf currentRightTwist = jacobiR * rightqvel;
+
+ Eigen::VectorXf currentTwist(12);
+ currentTwist << currentLeftTwist, currentRightTwist;
+ objCurrentTwist = pinvGraspMatrixT * currentTwist;
+
+ rt2ControlBuffer.getWriteBuffer().currentPoseObj = objCurrentPose;
+ rt2ControlBuffer.getWriteBuffer().currentTwistObj = objCurrentTwist;
+ rt2ControlBuffer.getWriteBuffer().deltaT = deltaT;
+ rt2ControlBuffer.getWriteBuffer().currentTime += deltaT;
+ rt2ControlBuffer.commitWrite();
+
+ // --------------------------------------------- get ft sensor ---------------------------------------------
+ // static compensation
+ Eigen::Vector3f gravity;
+ gravity << 0.0, 0.0, -9.8;
+ Eigen::Vector3f localGravityLeft = currentLeftPose.block<3, 3>(0, 0).transpose() * gravity;
+ Eigen::Vector3f localForceVecLeft = massLeft * localGravityLeft;
+ Eigen::Vector3f localTorqueVecLeft = CoMVecLeft.cross(localForceVecLeft);
+
+ Eigen::Vector3f localGravityRight = currentRightPose.block<3, 3>(0, 0).transpose() * gravity;
+ Eigen::Vector3f localForceVecRight = massRight * localGravityRight;
+ Eigen::Vector3f localTorqueVecRight = CoMVecRight.cross(localForceVecRight);
+
+ // mapping of measured wrenches
+ Eigen::VectorXf wrenchesMeasured(12);
+ wrenchesMeasured << leftForceTorque->force - forceOffsetLeft, leftForceTorque->torque - torqueOffsetLeft, rightForceTorque->force - forceOffsetRight, rightForceTorque->torque - torqueOffsetRight;
+ for (size_t i = 0; i < 12; i++)
+ {
+ wrenchesMeasured(i) = (1 - filterCoeff) * wrenchesMeasured(i) + filterCoeff * filteredOldValue(i);
+ }
+ filteredOldValue = wrenchesMeasured;
+ wrenchesMeasured.block<3, 1>(0, 0) = sensorFrame2TcpFrameLeft.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(0, 0) - localForceVecLeft;
+ wrenchesMeasured.block<3, 1>(3, 0) = sensorFrame2TcpFrameLeft.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(3, 0) - localTorqueVecLeft;
+ wrenchesMeasured.block<3, 1>(6, 0) = sensorFrame2TcpFrameRight.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(6, 0) - localForceVecRight;
+ wrenchesMeasured.block<3, 1>(9, 0) = sensorFrame2TcpFrameRight.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(9, 0) - localTorqueVecRight;
+ // if (wrenchesMeasured.norm() < cfg->forceThreshold)
+ // {
+ // wrenchesMeasured.setZero();
+ // }
+
+ Eigen::VectorXf forceTorqueMeasurementInRoot(12);
+ forceTorqueMeasurementInRoot.block<3, 1>(0, 0) = currentLeftPose.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(0, 0);
+ forceTorqueMeasurementInRoot.block<3, 1>(3, 0) = currentLeftPose.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(3, 0);
+ forceTorqueMeasurementInRoot.block<3, 1>(6, 0) = currentRightPose.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(6, 0);
+ forceTorqueMeasurementInRoot.block<3, 1>(9, 0) = currentRightPose.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(9, 0);
+
+
+ // map to object
+ Eigen::VectorXf objFTValue = graspMatrix * forceTorqueMeasurementInRoot;
+ for (size_t i = 0; i < 6; i++)
+ {
+ if (fabs(objFTValue(i)) < cfg->forceThreshold.at(i))
+ {
+ objFTValue(i) = 0;
+ }
+ else
+ {
+ objFTValue(i) -= cfg->forceThreshold.at(i) * objFTValue(i) / fabs(objFTValue(i));
+ }
+ }
+
+ // pass sensor value to statechart
+ rt2InterfaceBuffer.getWriteBuffer().currentObjPose = objCurrentPose;
+ rt2InterfaceBuffer.getWriteBuffer().currentObjVel = objCurrentTwist.head(3);
+ rt2InterfaceBuffer.getWriteBuffer().currentObjForce = objFTValue.head(3);
+ rt2InterfaceBuffer.commitWrite();
+
+
+ // --------------------------------------------- get MP target ---------------------------------------------
+ Eigen::Matrix4f objTargetPose = rtGetControlStruct().objTargetPose;
+ Eigen::VectorXf objTargetTwist = rtGetControlStruct().objTargetTwist;
+
+ Eigen::Matrix4f leftPoseInObj = rtGetControlStruct().leftTargetPoseInObj;
+ Eigen::VectorXf leftTwistInObj = rtGetControlStruct().leftTargetTwistInObj;
+
+ Eigen::Matrix4f rightPoseInObj = rtGetControlStruct().rightTargetPoseInObj;
+ Eigen::VectorXf rightTwistInObj = rtGetControlStruct().rightTargetTwistInObj;
+
+ // integrate mp target
+ integrateVel2Pose(deltaT, objTargetTwist, integratedPoseObj);
+ integrateVel2Pose(deltaT, leftTwistInObj, integratedPoseLeft);
+ integrateVel2Pose(deltaT, rightTwistInObj, integratedPoseRight);
+
+ // --------------------------------------------- obj admittance control ---------------------------------------------
+ // do admittance control on the object, calculate the virtual pose as attractor for the impedance controller
+ Eigen::VectorXf objPoseError(6);
+ objPoseError.head(3) = virtualPose.block<3, 1>(0, 3) - objTargetPose.block<3, 1>(0, 3);
+ Eigen::Matrix3f objDiffMat = virtualPose.block<3, 3>(0, 0) * objTargetPose.block<3, 3>(0, 0).transpose();
+ objPoseError.tail(3) = VirtualRobot::MathTools::eigen3f2rpy(objDiffMat);
+
+
+ Eigen::VectorXf objAcc;
+ Eigen::VectorXf objVel;
+ objAcc.setZero(6);
+ objVel.setZero(6);
+ for (size_t i = 0; i < 6; i++)
+ {
+ // objAcc(i) = KmAdmittance(i) * objFTValue(i) - KpAdmittance(i) * objPoseError(i) - KdAdmittance(i) * (virtualVel(i) - boxTwist(i));
+ objAcc(i) = KmAdmittance(i) * objFTValue(i) - KpAdmittance(i) * objPoseError(i) - KdAdmittance(i) * virtualVel(i);
+ }
+ objVel = virtualVel + 0.5 * deltaT * (objAcc + virtualAcc);
+ Eigen::VectorXf deltaObjPose = 0.5 * deltaT * (objVel + virtualVel);
+ virtualAcc = objAcc;
+ virtualVel = objVel;
+ virtualPose.block<3, 1>(0, 3) += deltaObjPose.head(3);
+ virtualPose.block<3, 3>(0, 0) = VirtualRobot::MathTools::rpy2eigen3f(deltaObjPose(3), deltaObjPose(4), deltaObjPose(5)) * virtualPose.block<3, 3>(0, 0);
+
+ // --------------------------------------------- convert to tcp pose ---------------------------------------------
+ // [R_v, P_v // 0, 1] * [R_l P_l// 0, 1] = [R_v*R_l R_v * P_l + P_v]
+ Eigen::Matrix4f tcpTargetPoseLeft = virtualPose * leftPoseInObj;
+ Eigen::Matrix4f tcpTargetPoseRight = virtualPose * rightPoseInObj;
+ // tcpTargetPoseLeft.block<3, 1>(0, 3) += virtualPose.block<3, 3>(0, 0) * (objCom2TCPLeftInObjFrame - deltaPoseForWrenchControl.block<3, 1>(0, 0));
+ // tcpTargetPoseRight.block<3, 1>(0, 3) += virtualPose.block<3, 3>(0, 0) * (objCom2TCPRightInObjFrame - deltaPoseForWrenchControl.block<3, 1>(6, 0));
+
+
+ // --------------------------------------------- Impedance control ---------------------------------------------
+ Eigen::VectorXf poseError(12);
+ Eigen::Matrix3f diffMat = tcpTargetPoseLeft.block<3, 3>(0, 0) * currentLeftPose.block<3, 3>(0, 0).transpose();
+ poseError.block<3, 1>(0, 0) = tcpTargetPoseLeft.block<3, 1>(0, 3) - currentLeftPose.block<3, 1>(0, 3);
+ poseError.block<3, 1>(3, 0) = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
+
+ diffMat = tcpTargetPoseRight.block<3, 3>(0, 0) * currentRightPose.block<3, 3>(0, 0).transpose();
+ poseError.block<3, 1>(6, 0) = tcpTargetPoseRight.block<3, 1>(0, 3) - currentRightPose.block<3, 1>(0, 3);
+ poseError.block<3, 1>(9, 0) = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
+
+ Eigen::VectorXf forceImpedance(12);
+ for (size_t i = 0; i < 12; i++)
+ {
+ forceImpedance(i) = KpImpedance(i) * poseError(i) - KdImpedance(i) * currentTwist(i);
+ }
+
+ // --------------------------------------------- Nullspace control ---------------------------------------------
+ Eigen::VectorXf leftNullspaceTorque = cfg->knull * (leftDesiredJointValues - leftqpos) - cfg->dnull * leftqvel;
+ Eigen::VectorXf rightNullspaceTorque = cfg->knull * (rightDesiredJointValues - rightqpos) - cfg->dnull * rightqvel;
+
+ // --------------------------------------------- Set Torque Control Command ---------------------------------------------
+ // float lambda = 1;
+ Eigen::MatrixXf jtpinvL = leftIK->computePseudoInverseJacobianMatrix(jacobiL.transpose(), lambda);
+ Eigen::MatrixXf jtpinvR = rightIK->computePseudoInverseJacobianMatrix(jacobiR.transpose(), lambda);
+ Eigen::VectorXf leftJointDesiredTorques = jacobiL.transpose() * forceImpedance.head(6) + (I - jacobiL.transpose() * jtpinvL) * leftNullspaceTorque;
+ Eigen::VectorXf rightJointDesiredTorques = jacobiR.transpose() * forceImpedance.tail(6) + (I - jacobiR.transpose() * jtpinvR) * rightNullspaceTorque;
+
+ // torque limit
+ for (size_t i = 0; i < leftTargets.size(); ++i)
+ {
+ float desiredTorque = leftJointDesiredTorques(i);
+ if (isnan(desiredTorque))
+ {
+ desiredTorque = 0;
+ }
+ desiredTorque = (desiredTorque > cfg->torqueLimit) ? cfg->torqueLimit : desiredTorque;
+ desiredTorque = (desiredTorque < -cfg->torqueLimit) ? -cfg->torqueLimit : desiredTorque;
+ rt2DebugBuffer.getWriteBuffer().desired_torques[leftJointNames[i]] = leftJointDesiredTorques(i);
+ leftTargets.at(i)->torque = desiredTorque;
+ }
+
+ for (size_t i = 0; i < rightTargets.size(); ++i)
+ {
+ float desiredTorque = rightJointDesiredTorques(i);
+ if (isnan(desiredTorque))
+ {
+ desiredTorque = 0;
+ }
+ desiredTorque = (desiredTorque > cfg->torqueLimit) ? cfg->torqueLimit : desiredTorque;
+ desiredTorque = (desiredTorque < - cfg->torqueLimit) ? - cfg->torqueLimit : desiredTorque;
+ rt2DebugBuffer.getWriteBuffer().desired_torques[rightJointNames[i]] = rightJointDesiredTorques(i);
+ rightTargets.at(i)->torque = desiredTorque;
+ }
+
+ // --------------------------------------------- debug output ---------------------------------------------
+ // impedance control, target TS force
+ rt2DebugBuffer.getWriteBuffer().forceImpedance = forceImpedance;
+ rt2DebugBuffer.getWriteBuffer().poseError = poseError;
+ rt2DebugBuffer.getWriteBuffer().forceTorqueMeasurementInRoot = forceTorqueMeasurementInRoot;
+
+ // object current pose and current twist
+ rt2DebugBuffer.getWriteBuffer().objPoseVec = eigenPose2EigenVec(objCurrentPose);
+ rt2DebugBuffer.getWriteBuffer().objCurrentTwist = objCurrentTwist;
+
+ // object force and torque
+ rt2DebugBuffer.getWriteBuffer().objForceTorque = objFTValue;
+
+ // virtual pose, vel and acc
+ rt2DebugBuffer.getWriteBuffer().virtualPoseVec = eigenPose2EigenVec(virtualPose);
+ rt2DebugBuffer.getWriteBuffer().virtualVel = virtualVel;
+ rt2DebugBuffer.getWriteBuffer().virtualAcc = virtualAcc;
+
+ // integrated pose
+ rt2DebugBuffer.getWriteBuffer().integratedPoseObjVec = eigenPose2EigenVec(integratedPoseObj);
+ rt2DebugBuffer.getWriteBuffer().integratedPoseLeftVec = eigenPose2EigenVec(integratedPoseLeft);
+ rt2DebugBuffer.getWriteBuffer().integratedPoseRightVec = eigenPose2EigenVec(integratedPoseRight);
+
+
+ // hand poses
+ rt2DebugBuffer.getWriteBuffer().targetHandPoseInRootLeft = eigenPose2EigenVec(tcpTargetPoseLeft);
+ rt2DebugBuffer.getWriteBuffer().targetHandPoseInRootRight = eigenPose2EigenVec(tcpTargetPoseRight);
+ rt2DebugBuffer.getWriteBuffer().currentHandPoseInRootLeft = eigenPose2EigenVec(currentLeftPose);
+ rt2DebugBuffer.getWriteBuffer().currentHandPoseInRootRight = eigenPose2EigenVec(currentRightPose);
+
+ // dmp targets
+ rt2DebugBuffer.getWriteBuffer().objTargetPoseVec = eigenPose2EigenVec(objTargetPose);
+ rt2DebugBuffer.getWriteBuffer().leftPoseVecInObj = eigenPose2EigenVec(leftPoseInObj);
+ rt2DebugBuffer.getWriteBuffer().rightPoseVecInObj = eigenPose2EigenVec(rightPoseInObj);
+ rt2DebugBuffer.getWriteBuffer().objTargetTwist = objTargetTwist;
+
+ // parameters
+ rt2DebugBuffer.getWriteBuffer().KpImpedance = KpImpedance;
+ rt2DebugBuffer.getWriteBuffer().KdImpedance = KdImpedance;
+ rt2DebugBuffer.getWriteBuffer().KpAdmittance = KpAdmittance;
+ rt2DebugBuffer.getWriteBuffer().KdAdmittance = KdAdmittance;
+ rt2DebugBuffer.getWriteBuffer().KmAdmittance = KmAdmittance;
+ rt2DebugBuffer.getWriteBuffer().KmPID = KmPID;
+
+ rt2DebugBuffer.commitWrite();
+
+ }
+
+ void NJointBimanualObjLevelMultiMPController::getObjStatus(Eigen::Matrix4f& pose, Eigen::VectorXf& twist)
+ {
+ // this is a temporary function to get status of the object,
+ // in fact, this should be set via the interface function.
+ Eigen::Matrix4f leftPose = tcpLeft->getPoseInRootFrame();
+ leftPose.block<3, 1>(0, 3) *= 0.001;
+ pose = leftPose * objTransMatrixInAnchor;
+ twist.setZero(6);
+ }
+
+ // TODO
+ // void NJointBimanualObjLevelMultiMPController::setObjStatus()
+ // {
+ // // ice interface function, create a buffer for this
+ // }
+
+
+ void NJointBimanualObjLevelMultiMPController::learnDMPFromFiles(const Ice::StringSeq& fileNames, const Ice::Current&)
+ {
+ objectDMP->learnDMPFromFiles(fileNames);
+ }
+
+ void NJointBimanualObjLevelMultiMPController::learnMultiDMPFromFiles(const Ice::StringSeq& objFileNames, const Ice::StringSeq& leftFileNames, const Ice::StringSeq& rightFileNames, const Ice::Current&)
+ {
+ objectDMP->learnDMPFromFiles(objFileNames);
+ leftTCPInObjDMP->learnDMPFromFiles(leftFileNames);
+ rightTCPInObjDMP->learnDMPFromFiles(rightFileNames);
+ }
+
+ void NJointBimanualObjLevelMultiMPController::setGoals(const Ice::DoubleSeq& goals, const Ice::Current& ice)
+ {
+ LockGuardType guard(controllerMutex);
+ objectDMP->setGoalPoseVec(goals);
+ }
+
+ void NJointBimanualObjLevelMultiMPController::setMultiMPGoals(const Ice::DoubleSeq& goalObj, const Ice::DoubleSeq& goalLeft, const Ice::DoubleSeq& goalRight, const Ice::Current& ice)
+ {
+ LockGuardType guard(controllerMutex);
+ objectDMP->setGoalPoseVec(goalObj);
+ leftTCPInObjDMP->setGoalPoseVec(goalLeft);
+ rightTCPInObjDMP->setGoalPoseVec(goalRight);
+ }
+
+ void NJointBimanualObjLevelMultiMPController::integrateVel2Pose(const double deltaT, Eigen::VectorXf& vel, Eigen::Matrix4f& pose)
+ {
+ Eigen::Matrix3f deltaRot = VirtualRobot::MathTools::rpy2eigen3f(vel.tail(3) * static_cast<float>(deltaT));
+ pose.block<3, 3>(0, 0) = deltaRot * pose.block<3, 3>(0, 0);
+ pose.block<3, 1>(0, 3) += vel.head(3) * static_cast<float>(deltaT);
+ }
+
+ std::vector<double> NJointBimanualObjLevelMultiMPController::eigenPose2Vec(const Eigen::Matrix4f& pose)
+ {
+ VirtualRobot::MathTools::Quaternion ori = VirtualRobot::MathTools::eigen4f2quat(pose);
+ std::vector<double> poseVec {pose(0, 3), pose(1, 3), pose(2, 3), ori.w, ori.x, ori.y, ori.z};
+ return poseVec;
+ }
+
+ Eigen::VectorXf NJointBimanualObjLevelMultiMPController::eigenPose2EigenVec(const Eigen::Matrix4f& pose)
+ {
+ VirtualRobot::MathTools::Quaternion ori = VirtualRobot::MathTools::eigen4f2quat(pose);
+ Eigen::VectorXf poseVec;
+ poseVec.setZero(7);
+ poseVec(0) = pose(0, 3);
+ poseVec(1) = pose(1, 3);
+ poseVec(2) = pose(2, 3);
+ poseVec(3) = ori.w;
+ poseVec(4) = ori.x;
+ poseVec(5) = ori.y;
+ poseVec(6) = ori.z;
+ return poseVec;
+ }
+
+ void NJointBimanualObjLevelMultiMPController::runDMP(const Ice::DoubleSeq& goalObj, const Ice::DoubleSeq& goalLeft, const Ice::DoubleSeq& goalRight, Ice::Double timeDuration, const Ice::Current&)
+ {
+ while (!rt2InterfaceBuffer.updateReadBuffer())
+ {
+ usleep(1000);
+ }
+
+ Eigen::Matrix4f leftInitPose = rt2InterfaceBuffer.getUpToDateReadBuffer().currentLeftPoseInObjFrame;
+ Eigen::Matrix4f rightInitPose = rt2InterfaceBuffer.getUpToDateReadBuffer().currentRightPoseInObjFrame;
+ Eigen::Matrix4f objInitPose = rt2InterfaceBuffer.getUpToDateReadBuffer().currentObjPose;
+ std::vector<double> objInitPoseVec = eigenPose2Vec(objInitPose);
+ std::vector<double> leftInitPoseVec = eigenPose2Vec(leftInitPose);
+ std::vector<double> rightInitPoseVec = eigenPose2Vec(rightInitPose);
+ ARMARX_INFO << "get init poses: \n" << VAROUT(objInitPoseVec) << "\n" << VAROUT(leftInitPoseVec) << "\n" << VAROUT(rightInitPoseVec);
+
+ // set the integrated left/right pose when start to run dmp
+ integratedPoseObj = objInitPose;
+ integratedPoseLeft = leftInitPose;
+ integratedPoseRight = rightInitPose;
+
+ objectDMP->prepareExecution(objInitPoseVec, goalObj);
+ objectDMP->canVal = timeDuration;
+ objectDMP->config.motionTimeDuration = timeDuration;
+
+ leftTCPInObjDMP->prepareExecution(leftInitPose, getLocalPose(goalObj, goalLeft));
+ leftTCPInObjDMP->canVal = timeDuration;
+ leftTCPInObjDMP->config.motionTimeDuration = timeDuration;
+
+ rightTCPInObjDMP->prepareExecution(rightInitPose, getLocalPose(goalObj, goalRight));
+ rightTCPInObjDMP->canVal = timeDuration;
+ rightTCPInObjDMP->config.motionTimeDuration = timeDuration;
+ ARMARX_INFO << "DMP prepare execution is done";
+
+ finished = false;
+ dmpStarted = true;
+ }
+
+
+ Eigen::Matrix4f NJointBimanualObjLevelMultiMPController::getLocalPose(const Eigen::Matrix4f& newCoordinate, const Eigen::Matrix4f& globalTargetPose)
+ {
+ Eigen::Matrix4f localPose = Eigen::Matrix4f::Identity();
+
+ localPose.block<3, 3>(0, 0) = newCoordinate.block<3, 3>(0, 0).inverse() * globalTargetPose.block<3, 3>(0, 0);
+ localPose.block<3, 1>(0, 3) = newCoordinate.block<3, 3>(0, 0).inverse() * (globalTargetPose.block<3, 1>(0, 3) - newCoordinate.block<3, 1>(0, 3));
+
+
+ return localPose;
+ }
+
+ Eigen::Matrix4f NJointBimanualObjLevelMultiMPController::getLocalPose(const std::vector<double>& newCoordinateVec, const std::vector<double>& globalTargetPoseVec)
+ {
+ Eigen::Matrix4f newCoordinate = VirtualRobot::MathTools::quat2eigen4f(newCoordinateVec.at(4), newCoordinateVec.at(5), newCoordinateVec.at(6), newCoordinateVec.at(3));
+ newCoordinate(0, 3) = newCoordinateVec.at(0);
+ newCoordinate(1, 3) = newCoordinateVec.at(1);
+ newCoordinate(2, 3) = newCoordinateVec.at(2);
+
+ Eigen::Matrix4f globalTargetPose = VirtualRobot::MathTools::quat2eigen4f(globalTargetPoseVec.at(4), globalTargetPoseVec.at(5), globalTargetPoseVec.at(6), globalTargetPoseVec.at(3));
+ globalTargetPose(0, 3) = globalTargetPoseVec.at(0);
+ globalTargetPose(1, 3) = globalTargetPoseVec.at(1);
+ globalTargetPose(2, 3) = globalTargetPoseVec.at(2);
+
+ return getLocalPose(newCoordinate, globalTargetPose);
+
+ }
+
+ void NJointBimanualObjLevelMultiMPController::runDMPWithVirtualStart(const Ice::DoubleSeq& starts, const Ice::DoubleSeq& goals, Ice::Double timeDuration, const Ice::Current&)
+ {
+ ARMARX_ERROR << "implementation not complete!!!";
+ while (!rt2InterfaceBuffer.updateReadBuffer())
+ {
+ usleep(1000);
+ }
+ ARMARX_IMPORTANT << "obj level control: setup dmp ...";
+ objectDMP->prepareExecution(starts, goals);
+ objectDMP->canVal = timeDuration;
+ objectDMP->config.motionTimeDuration = timeDuration;
+
+ finished = false;
+ dmpStarted = true;
+
+ ARMARX_IMPORTANT << "obj level control: run dmp with virtual start.";
+ }
+
+ void NJointBimanualObjLevelMultiMPController::setViaPoints(Ice::Double u, const Ice::DoubleSeq& viapoint, const Ice::Current&)
+ {
+ // LockGuardType guard(controllerMutex);
+ ARMARX_INFO << "setting via points ";
+ objectDMP->setViaPose(u, viapoint);
+ }
+
+ void NJointBimanualObjLevelMultiMPController::removeAllViaPoints(const Ice::Current&)
+ {
+ objectDMP->removeAllViaPoints();
+ }
+
+ void NJointBimanualObjLevelMultiMPController::setKpImpedance(const Ice::FloatSeq& value, const Ice::Current&)
+ {
+
+ Eigen::VectorXf setpoint;
+ setpoint.setZero(value.size());
+ ARMARX_CHECK_EQUAL(value.size(), 12);
+
+ for (size_t i = 0; i < value.size(); ++i)
+ {
+ setpoint(i) = value.at(i);
+ }
+
+ LockGuardType guard {interfaceDataMutex};
+ interface2rtBuffer.getWriteBuffer().KpImpedance = setpoint;
+ interface2rtBuffer.commitWrite();
+
+ }
+
+ void NJointBimanualObjLevelMultiMPController::setAmplitude(Ice::Double amp, const Ice::Current&)
+ {
+ LockGuardType guard {controllerMutex};
+ if (objectDMP->config.DMPStyle == "Periodic")
+ {
+ objectDMP->setAmplitude(amp);
+ }
+
+ if (leftTCPInObjDMP->config.DMPStyle == "Periodic")
+ {
+ leftTCPInObjDMP->setAmplitude(amp);
+ }
+
+ if (rightTCPInObjDMP->config.DMPStyle == "Periodic")
+ {
+ rightTCPInObjDMP->setAmplitude(amp);
+ }
+ }
+
+ void NJointBimanualObjLevelMultiMPController::setKdImpedance(const Ice::FloatSeq& value, const Ice::Current&)
+ {
+ Eigen::VectorXf setpoint;
+ setpoint.setZero(value.size());
+ ARMARX_CHECK_EQUAL(value.size(), 12);
+
+ for (size_t i = 0; i < value.size(); ++i)
+ {
+ setpoint(i) = value.at(i);
+ }
+
+ LockGuardType guard {interfaceDataMutex};
+ interface2rtBuffer.getWriteBuffer().KdImpedance = setpoint;
+ interface2rtBuffer.commitWrite();
+ }
+
+ void NJointBimanualObjLevelMultiMPController::setKpAdmittance(const Ice::FloatSeq& value, const Ice::Current&)
+ {
+ Eigen::VectorXf setpoint;
+ setpoint.setZero(value.size());
+ ARMARX_CHECK_EQUAL(value.size(), 6);
+
+ for (size_t i = 0; i < value.size(); ++i)
+ {
+ setpoint(i) = value.at(i);
+ }
+
+ LockGuardType guard {interfaceDataMutex};
+ interface2rtBuffer.getWriteBuffer().KpAdmittance = setpoint;
+ interface2rtBuffer.commitWrite();
+ }
+
+ void NJointBimanualObjLevelMultiMPController::setKdAdmittance(const Ice::FloatSeq& value, const Ice::Current&)
+ {
+ Eigen::VectorXf setpoint;
+ setpoint.setZero(value.size());
+ ARMARX_CHECK_EQUAL(value.size(), 6);
+
+ for (size_t i = 0; i < value.size(); ++i)
+ {
+ setpoint(i) = value.at(i);
+ }
+
+ LockGuardType guard {interfaceDataMutex};
+ interface2rtBuffer.getWriteBuffer().KdAdmittance = setpoint;
+ interface2rtBuffer.commitWrite();
+ }
+
+ void NJointBimanualObjLevelMultiMPController::setKmAdmittance(const Ice::FloatSeq& value, const Ice::Current&)
+ {
+ Eigen::VectorXf setpoint;
+ setpoint.setZero(value.size());
+ ARMARX_CHECK_EQUAL(value.size(), 6);
+
+ for (size_t i = 0; i < value.size(); ++i)
+ {
+ setpoint(i) = value.at(i);
+ }
+
+ LockGuardType guard {interfaceDataMutex};
+ interface2rtBuffer.getWriteBuffer().KmAdmittance = setpoint;
+ interface2rtBuffer.commitWrite();
+ }
+
+ std::vector<float> NJointBimanualObjLevelMultiMPController::getCurrentObjVel(const Ice::Current&)
+ {
+ Eigen::Vector3f tvel = rt2InterfaceBuffer.getUpToDateReadBuffer().currentObjVel;
+ std::vector<float> tvelvec = {tvel(0), tvel(1), tvel(2)};
+ return tvelvec;
+ }
+
+ std::vector<float> NJointBimanualObjLevelMultiMPController::getCurrentObjForce(const Ice::Current&)
+ {
+ Eigen::Vector3f fvel = rt2InterfaceBuffer.getUpToDateReadBuffer().currentObjForce;
+ std::vector<float> fvelvec = {fvel(0), fvel(1), fvel(2)};
+ return fvelvec;
+ }
+
+ void NJointBimanualObjLevelMultiMPController::publishVec(const Eigen::VectorXf& bufferVec, const std::string name, StringVariantBaseMap& datafields)
+ {
+ for (int i = 0; i < bufferVec.rows(); ++i)
+ {
+ std::string data_name = name + "_" + std::to_string(i);
+ datafields[data_name] = new Variant(bufferVec(i));
+ }
+ }
+
+ void NJointBimanualObjLevelMultiMPController::onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx& debugObs)
+ {
+
+ StringVariantBaseMap datafields;
+ auto values = rt2DebugBuffer.getUpToDateReadBuffer().desired_torques;
+ for (auto& pair : values)
+ {
+ datafields[pair.first] = new Variant(pair.second);
+ }
+
+ publishVec(rt2DebugBuffer.getUpToDateReadBuffer().forceImpedance, "forceImpedance", datafields);
+ publishVec(rt2DebugBuffer.getUpToDateReadBuffer().forcePID, "forcePID", datafields);
+ publishVec(rt2DebugBuffer.getUpToDateReadBuffer().poseError, "poseError", datafields);
+
+ // ------------------ force torque measurement of hands ------------------
+ publishVec(rt2DebugBuffer.getUpToDateReadBuffer().forceTorqueMeasurementInRoot, "forceTorqueMeasurementInRoot", datafields);
+
+ // ------------------ object force torques ------------------
+ publishVec(rt2DebugBuffer.getUpToDateReadBuffer().objForceTorque, "objForceTorque", datafields);
+
+ // ------------------ virtual pose, vel and acc ------------------
+ publishVec(rt2DebugBuffer.getUpToDateReadBuffer().virtualPoseVec, "virtualPoseVec", datafields);
+ publishVec(rt2DebugBuffer.getUpToDateReadBuffer().virtualVel, "virtualVel", datafields);
+
+ // ------------------ object pose vec, vel ------------------
+ publishVec(rt2DebugBuffer.getUpToDateReadBuffer().objPoseVec, "objPoseVec", datafields);
+ publishVec(rt2DebugBuffer.getUpToDateReadBuffer().objCurrentTwist, "objCurrentTwist", datafields);
+
+ // ------------------ hand poses ------------------
+ publishVec(rt2DebugBuffer.getUpToDateReadBuffer().targetHandPoseInRootLeft, "targetHandPoseInRootLeft", datafields);
+ publishVec(rt2DebugBuffer.getUpToDateReadBuffer().targetHandPoseInRootRight, "targetHandPoseInRootRight", datafields);
+ publishVec(rt2DebugBuffer.getUpToDateReadBuffer().currentHandPoseInRootLeft, "currentHandPoseInRootLeft", datafields);
+ publishVec(rt2DebugBuffer.getUpToDateReadBuffer().currentHandPoseInRootRight, "currentHandPoseInRootRight", datafields);
+
+ // ------------------ dmp targets ------------------
+ publishVec(rt2DebugBuffer.getUpToDateReadBuffer().objTargetPoseVec, "objTargetPoseVec", datafields);
+ publishVec(rt2DebugBuffer.getUpToDateReadBuffer().leftPoseVecInObj, "leftPoseVecInObj", datafields);
+ publishVec(rt2DebugBuffer.getUpToDateReadBuffer().rightPoseVecInObj, "rightPoseVecInObj", datafields);
+ publishVec(rt2DebugBuffer.getUpToDateReadBuffer().objTargetTwist, "objTargetTwist", datafields);
+
+ publishVec(rt2DebugBuffer.getUpToDateReadBuffer().integratedPoseObjVec, "integratedPoseObjVec", datafields);
+ publishVec(rt2DebugBuffer.getUpToDateReadBuffer().integratedPoseLeftVec, "integratedPoseLeftVec", datafields);
+ publishVec(rt2DebugBuffer.getUpToDateReadBuffer().integratedPoseRightVec, "integratedPoseRightVec", datafields);
+
+ // ------------------ controller parameters ------------------
+ publishVec(rt2DebugBuffer.getUpToDateReadBuffer().KpImpedance, "KpImpedance", datafields);
+ publishVec(rt2DebugBuffer.getUpToDateReadBuffer().KdImpedance, "KdImpedance", datafields);
+ publishVec(rt2DebugBuffer.getUpToDateReadBuffer().KpAdmittance, "KpAdmittance", datafields);
+ publishVec(rt2DebugBuffer.getUpToDateReadBuffer().KdAdmittance, "KdAdmittance", datafields);
+ publishVec(rt2DebugBuffer.getUpToDateReadBuffer().KmAdmittance, "KmAdmittance", datafields);
+ publishVec(rt2DebugBuffer.getUpToDateReadBuffer().KmPID, "KmPID", datafields);
+
+ // Eigen::VectorXf forceImpedance = rt2DebugBuffer.getUpToDateReadBuffer().forceImpedance;
+ // for (int i = 0; i < forceImpedance.rows(); ++i)
+ // {
+ // std::stringstream ss;
+ // ss << i;
+ // std::string data_name = "forceImpedance_" + ss.str();
+ // datafields[data_name] = new Variant(forceImpedance(i));
+ // }
+
+ // Eigen::VectorXf forcePID = rt2DebugBuffer.getUpToDateReadBuffer().forcePID;
+ // for (int i = 0; i < forcePID.rows(); ++i)
+ // {
+ // std::stringstream ss;
+ // ss << i;
+ // std::string data_name = "forcePID_" + ss.str();
+ // datafields[data_name] = new Variant(forcePID(i));
+ // }
+
+
+ // Eigen::VectorXf poseError = rt2DebugBuffer.getUpToDateReadBuffer().poseError;
+ // for (int i = 0; i < poseError.rows(); ++i)
+ // {
+ // std::stringstream ss;
+ // ss << i;
+ // std::string data_name = "poseError_" + ss.str();
+ // datafields[data_name] = new Variant(poseError(i));
+ // }
+
+ // // ------------------ force torque measurement of hands ------------------
+ // Eigen::VectorXf forceTorqueMeasurementInRoot = rt2DebugBuffer.getUpToDateReadBuffer().forceTorqueMeasurementInRoot;
+ // for (int i = 0; i < forceTorqueMeasurementInRoot.rows(); ++i)
+ // {
+ // std::stringstream ss;
+ // ss << i;
+ // std::string data_name = "forceTorqueMeasurementInRoot_" + ss.str();
+ // datafields[data_name] = new Variant(forceTorqueMeasurementInRoot(i));
+ // }
+
+ // ------------------ object force torques ------------------
+ // Eigen::VectorXf objForceTorque = rt2DebugBuffer.getUpToDateReadBuffer().objForceTorque;
+ // for (int i = 0; i < objForceTorque.rows(); ++i)
+ // {
+ // std::stringstream ss;
+ // ss << i;
+ // std::string data_name = "objForceTorque_" + ss.str();
+ // datafields[data_name] = new Variant(objForceTorque(i));
+ // }
+
+ // ------------------ virtual pose, vel and acc ------------------
+ // Eigen::VectorXf virtualPoseVec = rt2DebugBuffer.getUpToDateReadBuffer().virtualPoseVec;
+ // for (int i = 0; i < virtualPoseVec.rows(); ++i)
+ // {
+ // std::stringstream ss;
+ // ss << i;
+ // std::string data_name = "virtualPoseVec_" + ss.str();
+ // datafields[data_name] = new Variant(virtualPoseVec(i));
+ // }
+
+ // Eigen::VectorXf virtualVel = rt2DebugBuffer.getUpToDateReadBuffer().virtualVel;
+ // for (int i = 0; i < virtualVel.rows(); ++i)
+ // {
+ // std::stringstream ss;
+ // ss << i;
+ // std::string data_name = "virtualVel_" + ss.str();
+ // datafields[data_name] = new Variant(virtualVel(i));
+ // }
+
+ // ------------------ object pose vec, vel ------------------
+ // Eigen::VectorXf objPoseVec = rt2DebugBuffer.getUpToDateReadBuffer().objPoseVec;
+ // for (int i = 0; i < objPoseVec.rows(); ++i)
+ // {
+ // std::stringstream ss;
+ // ss << i;
+ // std::string data_name = "objPoseVec_" + ss.str();
+ // datafields[data_name] = new Variant(objPoseVec(i));
+ // }
+
+ // Eigen::VectorXf objCurrentTwist = rt2DebugBuffer.getUpToDateReadBuffer().objCurrentTwist;
+ // for (int i = 0; i < objCurrentTwist.rows(); ++i)
+ // {
+ // std::stringstream ss;
+ // ss << i;
+ // std::string data_name = "objCurrentTwist_" + ss.str();
+ // datafields[data_name] = new Variant(objCurrentTwist(i));
+ // }
+
+ // ------------------ hand poses ------------------
+ // Eigen::VectorXf targetHandPoseInRootLeft = rt2DebugBuffer.getUpToDateReadBuffer().targetHandPoseInRootLeft;
+ // for (int i = 0; i < targetHandPoseInRootLeft.rows(); ++i)
+ // {
+ // std::stringstream ss;
+ // ss << i;
+ // std::string data_name = "targetHandPoseInRootLeft_" + ss.str();
+ // datafields[data_name] = new Variant(targetHandPoseInRootLeft(i));
+ // }
+ // Eigen::VectorXf targetHandPoseInRootRight = rt2DebugBuffer.getUpToDateReadBuffer().targetHandPoseInRootRight;
+ // for (int i = 0; i < targetHandPoseInRootRight.rows(); ++i)
+ // {
+ // std::stringstream ss;
+ // ss << i;
+ // std::string data_name = "targetHandPoseInRootRight_" + ss.str();
+ // datafields[data_name] = new Variant(targetHandPoseInRootRight(i));
+ // }
+ // Eigen::VectorXf currentHandPoseInRootLeft = rt2DebugBuffer.getUpToDateReadBuffer().currentHandPoseInRootLeft;
+ // for (int i = 0; i < currentHandPoseInRootLeft.rows(); ++i)
+ // {
+ // std::stringstream ss;
+ // ss << i;
+ // std::string data_name = "currentHandPoseInRootLeft_" + ss.str();
+ // datafields[data_name] = new Variant(currentHandPoseInRootLeft(i));
+ // }
+ // Eigen::VectorXf currentHandPoseInRootRight = rt2DebugBuffer.getUpToDateReadBuffer().currentHandPoseInRootRight;
+ // for (int i = 0; i < currentHandPoseInRootRight.rows(); ++i)
+ // {
+ // std::stringstream ss;
+ // ss << i;
+ // std::string data_name = "currentHandPoseInRootRight_" + ss.str();
+ // datafields[data_name] = new Variant(currentHandPoseInRootRight(i));
+ // }
+ // ------------------ dmp targets ------------------
+ // Eigen::VectorXf objTargetPoseVec = rt2DebugBuffer.getUpToDateReadBuffer().objTargetPoseVec;
+ // for (int i = 0; i < objTargetPoseVec.rows(); ++i)
+ // {
+ // std::stringstream ss;
+ // ss << i;
+ // std::string data_name = "objTargetPoseVec_" + ss.str();
+ // datafields[data_name] = new Variant(objTargetPoseVec(i));
+ // }
+ // Eigen::VectorXf leftPoseVecInObj = rt2DebugBuffer.getUpToDateReadBuffer().leftPoseVecInObj;
+ // for (int i = 0; i < leftPoseVecInObj.rows(); ++i)
+ // {
+ // std::stringstream ss;
+ // ss << i;
+ // std::string data_name = "leftPoseVecInObj_" + ss.str();
+ // datafields[data_name] = new Variant(leftPoseVecInObj(i));
+ // }
+ // Eigen::VectorXf rightPoseVecInObj = rt2DebugBuffer.getUpToDateReadBuffer().rightPoseVecInObj;
+ // for (int i = 0; i < rightPoseVecInObj.rows(); ++i)
+ // {
+ // std::stringstream ss;
+ // ss << i;
+ // std::string data_name = "rightPoseVecInObj_" + ss.str();
+ // datafields[data_name] = new Variant(rightPoseVecInObj(i));
+ // }
+ // Eigen::VectorXf objTargetTwist = rt2DebugBuffer.getUpToDateReadBuffer().objTargetTwist;
+ // for (int i = 0; i < objTargetTwist.rows(); ++i)
+ // {
+ // std::stringstream ss;
+ // ss << i;
+ // std::string data_name = "objTargetTwist_" + ss.str();
+ // datafields[data_name] = new Variant(objTargetTwist(i));
+ // }
+
+ // parameters
+ // Eigen::VectorXf KpImpedance = rt2DebugBuffer.getUpToDateReadBuffer().KpImpedance;
+ // for (int i = 0; i < KpImpedance.rows(); ++i)
+ // {
+ // std::stringstream ss;
+ // ss << i;
+ // std::string data_name = "KpImpedance_" + ss.str();
+ // datafields[data_name] = new Variant(KpImpedance(i));
+ // }
+ // Eigen::VectorXf KdImpedance = rt2DebugBuffer.getUpToDateReadBuffer().KdImpedance;
+ // for (int i = 0; i < KdImpedance.rows(); ++i)
+ // {
+ // std::stringstream ss;
+ // ss << i;
+ // std::string data_name = "KdImpedance_" + ss.str();
+ // datafields[data_name] = new Variant(KdImpedance(i));
+ // }
+ // Eigen::VectorXf KpAdmittance = rt2DebugBuffer.getUpToDateReadBuffer().KpAdmittance;
+ // for (int i = 0; i < KpAdmittance.rows(); ++i)
+ // {
+ // std::stringstream ss;
+ // ss << i;
+ // std::string data_name = "KpAdmittance_" + ss.str();
+ // datafields[data_name] = new Variant(KpAdmittance(i));
+ // }
+ // Eigen::VectorXf KdAdmittance = rt2DebugBuffer.getUpToDateReadBuffer().KdAdmittance;
+ // for (int i = 0; i < KdAdmittance.rows(); ++i)
+ // {
+ // std::stringstream ss;
+ // ss << i;
+ // std::string data_name = "KdAdmittance_" + ss.str();
+ // datafields[data_name] = new Variant(KdAdmittance(i));
+ // }
+ // Eigen::VectorXf KmAdmittance = rt2DebugBuffer.getUpToDateReadBuffer().KmAdmittance;
+ // for (int i = 0; i < KmAdmittance.rows(); ++i)
+ // {
+ // std::stringstream ss;
+ // ss << i;
+ // std::string data_name = "KmAdmittance_" + ss.str();
+ // datafields[data_name] = new Variant(KmAdmittance(i));
+ // }
+ // Eigen::VectorXf KmPID = rt2DebugBuffer.getUpToDateReadBuffer().KmPID;
+ // for (int i = 0; i < KmPID.rows(); ++i)
+ // {
+ // std::stringstream ss;
+ // ss << i;
+ // std::string data_name = "KmPID_" + ss.str();
+ // datafields[data_name] = new Variant(KmPID(i));
+ // }
+
+ debugObs->setDebugChannel("BimanualForceController", datafields);
+ }
+
+ void NJointBimanualObjLevelMultiMPController::onInitNJointController()
+ {
+ ARMARX_INFO << "====== init bimanual multi mp controller ======";
+ runTask("NJointBimanualObjLevelMultiMPController", [&]
+ {
+ CycleUtil c(1);
+ getObjectScheduler()->waitForObjectStateMinimum(eManagedIceObjectStarted);
+ while (getState() == eManagedIceObjectStarted)
+ {
+ if (isControllerActive())
+ {
+ controllerRun();
+ }
+ c.waitForCycleDuration();
+ }
+ });
+ }
+
+ void NJointBimanualObjLevelMultiMPController::onDisconnectNJointController()
+ {
+ ARMARX_INFO << "====== stop bimanual multi mp controller ======";
+ }
+}
+
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualObjLevelMultiMPController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualObjLevelMultiMPController.h
new file mode 100644
index 0000000000000000000000000000000000000000..1fbd032b02b4ce2644e4fc57876e548c531c69aa
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualObjLevelMultiMPController.h
@@ -0,0 +1,297 @@
+#pragma once
+
+#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/IK/DifferentialIK.h>
+
+#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h>
+#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
+#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
+#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
+#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValueForceTorque.h>
+#include <RobotAPI/libraries/DMPController/TaskSpaceDMPController.h>
+#include <RobotAPI/libraries/core/math/MathUtils.h>
+#include <RobotAPI/libraries/core/PIDController.h>
+
+#include <RobotAPI/interface/units/RobotUnit/NJointBimanualObjLevelController.h>
+
+using namespace DMP;
+namespace armarx
+{
+
+
+ TYPEDEF_PTRS_HANDLE(NJointBimanualObjLevelMultiMPController);
+ TYPEDEF_PTRS_HANDLE(NJointBimanualObjLevelMultiMPControlData);
+
+ class NJointBimanualObjLevelMultiMPControlData
+ {
+ public:
+ // control target from Movement Primitives
+ Eigen::Matrix4f objTargetPose;
+ Eigen::VectorXf objTargetTwist;
+
+ Eigen::Matrix4f leftTargetPoseInObj;
+ Eigen::VectorXf leftTargetTwistInObj;
+
+ Eigen::Matrix4f rightTargetPoseInObj;
+ Eigen::VectorXf rightTargetTwistInObj;
+ };
+
+
+ class NJointBimanualObjLevelMultiMPController :
+ public NJointControllerWithTripleBuffer<NJointBimanualObjLevelMultiMPControlData>,
+ public NJointBimanualObjLevelMultiMPControllerInterface
+ {
+ public:
+ // using ConfigPtrT = BimanualForceControllerConfigPtr;
+ NJointBimanualObjLevelMultiMPController(const RobotUnitPtr&, const NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&);
+
+ // NJointControllerInterface interface
+ std::string getClassName(const Ice::Current&) const;
+
+ // NJointController interface
+
+ void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration);
+
+ // NJointCCDMPControllerInterface interface
+ void learnDMPFromFiles(const Ice::StringSeq& fileNames, const Ice::Current&);
+ void learnMultiDMPFromFiles(const Ice::StringSeq& objFileNames, const Ice::StringSeq& leftFileNames, const Ice::StringSeq& rightFileNames, const Ice::Current&);
+
+ bool isFinished(const Ice::Current&)
+ {
+ return finished;
+ }
+ Eigen::Matrix3f skew(Eigen::Vector3f vec)
+ {
+ Eigen::Matrix3f mat = Eigen::MatrixXf::Zero(3, 3);
+ mat(1, 2) = -vec(0);
+ mat(0, 2) = vec(1);
+ mat(0, 1) = -vec(2);
+ mat(2, 1) = vec(0);
+ mat(2, 0) = -vec(1);
+ mat(1, 0) = vec(2);
+ return mat;
+ }
+
+ // void runDMP(const Ice::DoubleSeq& goals, Ice::Double tau, const Ice::Current&);
+ void runDMP(const Ice::DoubleSeq& goalObj, const Ice::DoubleSeq& goalLeft, const Ice::DoubleSeq& goalRight, Ice::Double timeDuration, const Ice::Current&);
+ void runDMPWithVirtualStart(const Ice::DoubleSeq& starts, const Ice::DoubleSeq& goals, Ice::Double timeDuration, const Ice::Current&);
+
+ void setGoals(const Ice::DoubleSeq& goals, const Ice::Current&);
+ void setMultiMPGoals(const Ice::DoubleSeq& goalObj, const Ice::DoubleSeq& goalLeft, const Ice::DoubleSeq& goalRight, const Ice::Current& ice);
+
+ void setViaPoints(Ice::Double u, const Ice::DoubleSeq& viapoint, const Ice::Current&);
+ void removeAllViaPoints(const Ice::Current&);
+
+ double getVirtualTime(const Ice::Current&)
+ {
+ return virtualtimer;
+ }
+
+ void setKpImpedance(const Ice::FloatSeq& value, const Ice::Current&);
+ void setKdImpedance(const Ice::FloatSeq& value, const Ice::Current&);
+ void setKmAdmittance(const Ice::FloatSeq& value, const Ice::Current&);
+ void setKpAdmittance(const Ice::FloatSeq& value, const Ice::Current&);
+ void setKdAdmittance(const Ice::FloatSeq& value, const Ice::Current&);
+
+ std::vector<float> getCurrentObjVel(const Ice::Current&);
+ std::vector<float> getCurrentObjForce(const Ice::Current&);
+
+ void getObjStatus(Eigen::Matrix4f& pose, Eigen::VectorXf& twist);
+ std::vector<double> eigenPose2Vec(const Eigen::Matrix4f& pose);
+ Eigen::VectorXf eigenPose2EigenVec(const Eigen::Matrix4f& pose);
+ Eigen::Matrix4f getLocalPose(const Eigen::Matrix4f& newCoordinate, const Eigen::Matrix4f& globalTargetPose);
+ Eigen::Matrix4f getLocalPose(const std::vector<double>& newCoordinateVec, const std::vector<double>& globalTargetPoseVec);
+ void integrateVel2Pose(const double deltaT, Eigen::VectorXf& vel, Eigen::Matrix4f& pose);
+ void publishVec(const Eigen::VectorXf& bufferVec, const std::string name, StringVariantBaseMap& datafields);
+ void setAmplitude(Ice::Double amp, const Ice::Current&);
+ protected:
+ virtual void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&);
+
+ void onInitNJointController();
+ void onDisconnectNJointController();
+ void controllerRun();
+
+ private:
+ Eigen::VectorXf targetWrench;
+ struct RT2DebugData
+ {
+ StringFloatDictionary desired_torques;
+
+ // dmp targets
+ Eigen::VectorXf objTargetPoseVec;
+ Eigen::VectorXf leftPoseVecInObj;
+ Eigen::VectorXf rightPoseVecInObj;
+ Eigen::VectorXf objTargetTwist;
+
+ // hand poses
+ Eigen::VectorXf targetHandPoseInRootLeft;
+ Eigen::VectorXf targetHandPoseInRootRight;
+ Eigen::VectorXf currentHandPoseInRootLeft;
+ Eigen::VectorXf currentHandPoseInRootRight;
+
+ // object pose, vel and force torque
+ Eigen::VectorXf objForceTorque;
+ Eigen::VectorXf objPoseVec;
+ Eigen::VectorXf objCurrentTwist;
+
+ // virtual pose, vel, acc
+ Eigen::VectorXf virtualPoseVec;
+ Eigen::VectorXf virtualVel;
+ Eigen::VectorXf virtualAcc;
+
+ // integrated pose
+ Eigen::VectorXf integratedPoseObjVec;
+ Eigen::VectorXf integratedPoseLeftVec;
+ Eigen::VectorXf integratedPoseRightVec;
+
+ Eigen::VectorXf poseError;
+
+ // force
+ Eigen::VectorXf forceImpedance;
+ Eigen::VectorXf forcePID;
+ Eigen::VectorXf forcePIDControlValue;
+ Eigen::VectorXf forceTorqueMeasurementInRoot;
+
+ // parameters
+ Eigen::VectorXf KpImpedance;
+ Eigen::VectorXf KdImpedance;
+ Eigen::VectorXf KpAdmittance;
+ Eigen::VectorXf KdAdmittance;
+ Eigen::VectorXf KmAdmittance;
+ Eigen::VectorXf KmPID;
+ };
+ TripleBuffer<RT2DebugData> rt2DebugBuffer;
+
+ struct RT2ControlData
+ {
+ double currentTime;
+ double deltaT;
+ Eigen::Matrix4f currentPoseObj;
+ Eigen::VectorXf currentTwistObj;
+
+ Eigen::Matrix4f currentPoseLeftInObj;
+ Eigen::VectorXf currentTwistLeftInObj;
+
+ Eigen::Matrix4f currentPoseRightInObj;
+ Eigen::VectorXf currentTwistRightInObj;
+ };
+ TripleBuffer<RT2ControlData> rt2ControlBuffer;
+
+ struct RT2InterfaceData
+ {
+ Eigen::Matrix4f currentLeftPoseInObjFrame;
+ Eigen::Matrix4f currentRightPoseInObjFrame;
+ Eigen::Matrix4f currentObjPose;
+ Eigen::Vector3f currentObjVel;
+ Eigen::Vector3f currentObjForce;
+ };
+ TripleBuffer<RT2InterfaceData> rt2InterfaceBuffer;
+
+ struct Inferface2rtData
+ {
+ Eigen::VectorXf KpImpedance;
+ Eigen::VectorXf KdImpedance;
+ Eigen::VectorXf KmAdmittance;
+ Eigen::VectorXf KpAdmittance;
+ Eigen::VectorXf KdAdmittance;
+ };
+ TripleBuffer<Inferface2rtData> interface2rtBuffer;
+
+
+
+ std::vector<ControlTarget1DoFActuatorTorque*> leftTargets;
+ std::vector<const SensorValue1DoFActuatorAcceleration*> leftAccelerationSensors;
+ std::vector<const SensorValue1DoFActuatorVelocity*> leftVelocitySensors;
+ std::vector<const SensorValue1DoFActuatorPosition*> leftPositionSensors;
+
+ std::vector<ControlTarget1DoFActuatorTorque*> rightTargets;
+ std::vector<const SensorValue1DoFActuatorAcceleration*> rightAccelerationSensors;
+ std::vector<const SensorValue1DoFActuatorVelocity*> rightVelocitySensors;
+ std::vector<const SensorValue1DoFActuatorPosition*> rightPositionSensors;
+
+ const SensorValueForceTorque* rightForceTorque;
+ const SensorValueForceTorque* leftForceTorque;
+
+ NJointBimanualObjLevelMultiMPControllerConfigPtr cfg;
+ VirtualRobot::DifferentialIKPtr leftIK;
+ VirtualRobot::DifferentialIKPtr rightIK;
+
+ TaskSpaceDMPControllerPtr objectDMP;
+ TaskSpaceDMPControllerPtr leftTCPInObjDMP;
+ TaskSpaceDMPControllerPtr rightTCPInObjDMP;
+
+ Eigen::Matrix4f objInitialPose; // in root frame
+ Eigen::Matrix4f leftInitialPose; // in obj frame
+ Eigen::Matrix4f rightInitialPose; // in obj frame
+
+ // add integrated pose by accumulating dmp target velocity to initial pose
+ Eigen::Matrix4f integratedPoseObj;
+ Eigen::Matrix4f integratedPoseLeft;
+ Eigen::Matrix4f integratedPoseRight;
+
+ Eigen::Matrix4f objTransMatrixInAnchor;
+
+ double virtualtimer;
+
+ mutable MutexType controllerMutex;
+ mutable MutexType interfaceDataMutex;
+ Eigen::VectorXf leftDesiredJointValues;
+ Eigen::VectorXf rightDesiredJointValues;
+
+ Eigen::VectorXf KpImpedance;
+ Eigen::VectorXf KdImpedance;
+ Eigen::VectorXf KpAdmittance;
+ Eigen::VectorXf KdAdmittance;
+ Eigen::VectorXf KmAdmittance;
+ Eigen::VectorXf KmPID;
+
+ Eigen::VectorXf virtualAcc;
+ Eigen::VectorXf virtualVel;
+ Eigen::Matrix4f virtualPose;
+
+ Eigen::Matrix4f sensorFrame2TcpFrameLeft;
+ Eigen::Matrix4f sensorFrame2TcpFrameRight;
+
+ //static compensation
+ float massLeft;
+ Eigen::Vector3f CoMVecLeft;
+ Eigen::Vector3f forceOffsetLeft;
+ Eigen::Vector3f torqueOffsetLeft;
+
+ float massRight;
+ Eigen::Vector3f CoMVecRight;
+ Eigen::Vector3f forceOffsetRight;
+ Eigen::Vector3f torqueOffsetRight;
+
+ // float knull;
+ // float dnull;
+
+ std::vector<std::string> leftJointNames;
+ std::vector<std::string> rightJointNames;
+
+ // float torqueLimit;
+ VirtualRobot::RobotNodeSetPtr leftRNS;
+ VirtualRobot::RobotNodeSetPtr rightRNS;
+ VirtualRobot::RobotNodePtr tcpLeft;
+ VirtualRobot::RobotNodePtr tcpRight;
+
+ std::vector<PIDControllerPtr> forcePIDControllers;
+
+ // filter parameters
+ float filterCoeff;
+ Eigen::VectorXf filteredOldValue;
+ bool finished;
+ bool dmpStarted;
+ double ftcalibrationTimer;
+ Eigen::VectorXf ftOffset;
+
+ Eigen::Matrix3f fixedLeftRightRotOffset;
+ Eigen::Vector3f objCom2TCPLeftInObjFrame, objCom2TCPRightInObjFrame;
+
+ protected:
+ void rtPreActivateController();
+ bool firstLoop;
+ };
+
+} // namespace armarx
+
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualObjLevelVelController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualObjLevelVelController.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..a57b4b4d3135d5fbd5765816f227eec0a550d69c
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualObjLevelVelController.cpp
@@ -0,0 +1,685 @@
+#include "NJointBimanualObjLevelVelController.h"
+
+#include <ArmarXCore/core/time/CycleUtil.h>
+#include <ArmarXCore/core/ArmarXObjectScheduler.h>
+
+namespace armarx
+{
+ NJointControllerRegistration<NJointBimanualObjLevelVelController> registrationControllerNJointBimanualObjLevelVelController("NJointBimanualObjLevelVelController");
+
+ NJointBimanualObjLevelVelController::NJointBimanualObjLevelVelController(const RobotUnitPtr& robUnit, const armarx::NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&)
+ {
+ ARMARX_INFO << "Preparing ... bimanual ";
+ ARMARX_INFO << "I am here";
+
+ useSynchronizedRtRobot();
+ cfg = NJointBimanualObjLevelVelControllerConfigPtr::dynamicCast(config);
+ // ARMARX_CHECK_EXPRESSION(prov);
+ // RobotUnitPtr robotUnit = RobotUnitPtr::dynamicCast(prov);
+ // ARMARX_CHECK_EXPRESSION(robotUnit);
+ leftRNS = rtGetRobot()->getRobotNodeSet("LeftArm");
+
+ for (size_t i = 0; i < leftRNS->getSize(); ++i)
+ {
+ std::string jointName = leftRNS->getNode(i)->getName();
+ leftJointNames.push_back(jointName);
+ ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Velocity1DoF);
+ const SensorValueBase* sv = useSensorValue(jointName);
+ leftTargets.push_back(ct->asA<ControlTarget1DoFActuatorVelocity>());
+ const SensorValue1DoFActuatorVelocity* velocitySensor = sv->asA<SensorValue1DoFActuatorVelocity>();
+ const SensorValue1DoFActuatorPosition* positionSensor = sv->asA<SensorValue1DoFActuatorPosition>();
+
+ if (!velocitySensor)
+ {
+ ARMARX_WARNING << "No velocitySensor available for " << jointName;
+ }
+ if (!positionSensor)
+ {
+ ARMARX_WARNING << "No positionSensor available for " << jointName;
+ }
+
+
+ leftVelocitySensors.push_back(velocitySensor);
+ leftPositionSensors.push_back(positionSensor);
+
+ };
+
+ rightRNS = rtGetRobot()->getRobotNodeSet("RightArm");
+
+ for (size_t i = 0; i < rightRNS->getSize(); ++i)
+ {
+ std::string jointName = rightRNS->getNode(i)->getName();
+ rightJointNames.push_back(jointName);
+ ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Velocity1DoF);
+ const SensorValueBase* sv = useSensorValue(jointName);
+ rightTargets.push_back(ct->asA<ControlTarget1DoFActuatorVelocity>());
+
+ const SensorValue1DoFActuatorVelocity* velocitySensor = sv->asA<SensorValue1DoFActuatorVelocity>();
+ const SensorValue1DoFActuatorPosition* positionSensor = sv->asA<SensorValue1DoFActuatorPosition>();
+
+ if (!velocitySensor)
+ {
+ ARMARX_WARNING << "No velocitySensor available for " << jointName;
+ }
+ if (!positionSensor)
+ {
+ ARMARX_WARNING << "No positionSensor available for " << jointName;
+ }
+
+ rightVelocitySensors.push_back(velocitySensor);
+ rightPositionSensors.push_back(positionSensor);
+
+ };
+
+ leftIK.reset(new VirtualRobot::DifferentialIK(leftRNS, leftRNS->getRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
+ rightIK.reset(new VirtualRobot::DifferentialIK(rightRNS, rightRNS->getRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
+
+
+ leftTCPController.reset(new CartesianVelocityController(leftRNS, leftRNS->getTCP()));
+ rightTCPController.reset(new CartesianVelocityController(rightRNS, rightRNS->getTCP()));
+
+ double phaseL = 10;
+ double phaseK = 10;
+ double phaseDist0 = 50;
+ double phaseDist1 = 10;
+ double phaseKpPos = 1;
+ double phaseKpOri = 0.1;
+ double posToOriRatio = 10;
+ TaskSpaceDMPControllerConfig taskSpaceDMPConfig;
+ taskSpaceDMPConfig.motionTimeDuration = cfg->timeDuration;
+ taskSpaceDMPConfig.DMPKernelSize = cfg->kernelSize;
+ taskSpaceDMPConfig.DMPMode = cfg->dmpMode;
+ taskSpaceDMPConfig.DMPStyle = cfg->dmpType;
+ taskSpaceDMPConfig.DMPAmplitude = 1.0;
+ taskSpaceDMPConfig.phaseStopParas.Dori = 0;
+ taskSpaceDMPConfig.phaseStopParas.Dpos = 0;
+ taskSpaceDMPConfig.phaseStopParas.goDist = phaseDist0;
+ taskSpaceDMPConfig.phaseStopParas.backDist = phaseDist1;
+ taskSpaceDMPConfig.phaseStopParas.Kpos = phaseKpPos;
+ taskSpaceDMPConfig.phaseStopParas.Kori = phaseKpOri;
+ taskSpaceDMPConfig.phaseStopParas.mm2radi = posToOriRatio;
+ taskSpaceDMPConfig.phaseStopParas.maxValue = phaseL;
+ taskSpaceDMPConfig.phaseStopParas.slop = phaseK;
+
+
+ objectDMP.reset(new TaskSpaceDMPController("boxDMP", taskSpaceDMPConfig, false));
+ ARMARX_IMPORTANT << "dmp finished";
+
+ tcpLeft = leftRNS->getTCP();
+ tcpRight = rightRNS->getTCP();
+
+ //initialize control parameters
+ KpImpedance.setZero(cfg->KpImpedance.size());
+ ARMARX_CHECK_EQUAL(cfg->KpImpedance.size(), 6);
+
+ for (int i = 0; i < KpImpedance.size(); ++i)
+ {
+ KpImpedance(i) = cfg->KpImpedance.at(i);
+ }
+
+ KdImpedance.setZero(cfg->KdImpedance.size());
+ ARMARX_CHECK_EQUAL(cfg->KdImpedance.size(), 6);
+
+ for (int i = 0; i < KdImpedance.size(); ++i)
+ {
+ KdImpedance(i) = cfg->KdImpedance.at(i);
+ }
+
+ Inferface2rtData initInt2rtData;
+ initInt2rtData.KpImpedance = KpImpedance;
+ initInt2rtData.KdImpedance = KdImpedance;
+ interface2rtBuffer.reinitAllBuffers(initInt2rtData);
+
+ leftDesiredJointValues.resize(leftTargets.size());
+ ARMARX_CHECK_EQUAL(cfg->leftDesiredJointValues.size(), leftTargets.size());
+
+ for (size_t i = 0; i < leftTargets.size(); ++i)
+ {
+ leftDesiredJointValues(i) = cfg->leftDesiredJointValues.at(i);
+ }
+
+ rightDesiredJointValues.resize(rightTargets.size());
+ ARMARX_CHECK_EQUAL(cfg->rightDesiredJointValues.size(), rightTargets.size());
+
+ for (size_t i = 0; i < rightTargets.size(); ++i)
+ {
+ rightDesiredJointValues(i) = cfg->rightDesiredJointValues.at(i);
+ }
+
+ virtualPose = Eigen::Matrix4f::Identity();
+ ARMARX_INFO << "got controller params";
+
+ rt2ControlData initSensorData;
+ initSensorData.deltaT = 0;
+ initSensorData.currentTime = 0;
+ initSensorData.currentPose = leftRNS->getTCP()->getPoseInRootFrame();
+ initSensorData.currentTwist.setZero();
+ rt2ControlBuffer.reinitAllBuffers(initSensorData);
+
+
+ ControlInterfaceData initInterfaceData;
+ initInterfaceData.currentLeftPose = tcpLeft->getPoseInRootFrame();
+ initInterfaceData.currentRightPose = tcpRight->getPoseInRootFrame();
+ initInterfaceData.currentObjPose = leftRNS->getTCP()->getPoseInRootFrame();
+ initInterfaceData.currentObjVel.setZero();
+ controlInterfaceBuffer.reinitAllBuffers(initInterfaceData);
+
+
+ leftInitialPose = tcpLeft->getPoseInRootFrame();
+ rightInitialPose = tcpRight->getPoseInRootFrame();
+ leftInitialPose.block<3, 1>(0, 3) = leftInitialPose.block<3, 1>(0, 3);
+ rightInitialPose.block<3, 1>(0, 3) = rightInitialPose.block<3, 1>(0, 3);
+
+
+ NJointBimanualObjLevelVelControlData initData;
+ initData.boxPose = boxInitialPose;
+ reinitTripleBuffer(initData);
+
+ firstLoop = true;
+ ARMARX_INFO << "left initial pose: \n" << leftInitialPose << "\n right initial pose: \n" << rightInitialPose;
+ dmpStarted = false;
+ fixedLeftRightRotOffset = Eigen::Matrix3f::Identity();
+ objCom2TCPLeftInObjFrame.setZero();
+ objCom2TCPRightInObjFrame.setZero();
+
+ }
+
+ void NJointBimanualObjLevelVelController::setMPWeights(const DoubleSeqSeq& weights, const Ice::Current&)
+ {
+ objectDMP->setWeights(weights);
+ }
+
+ DoubleSeqSeq NJointBimanualObjLevelVelController::getMPWeights(const Ice::Current&)
+ {
+ DMP::DVec2d res = objectDMP->getWeights();
+ DoubleSeqSeq resvec;
+ for (size_t i = 0; i < res.size(); ++i)
+ {
+ std::vector<double> cvec;
+ for (size_t j = 0; j < res[i].size(); ++j)
+ {
+ cvec.push_back(res[i][j]);
+ }
+ resvec.push_back(cvec);
+ }
+
+ return resvec;
+ }
+
+ void NJointBimanualObjLevelVelController::setMPRotWeights(const DoubleSeqSeq& weights, const Ice::Current&)
+ {
+ objectDMP->setRotWeights(weights);
+ }
+
+ DoubleSeqSeq NJointBimanualObjLevelVelController::getMPRotWeights(const Ice::Current&)
+ {
+ DMP::DVec2d res = objectDMP->getRotWeights();
+ DoubleSeqSeq resvec;
+ for (size_t i = 0; i < res.size(); ++i)
+ {
+ std::vector<double> cvec;
+ for (size_t j = 0; j < res[i].size(); ++j)
+ {
+ cvec.push_back(res[i][j]);
+ }
+ resvec.push_back(cvec);
+ }
+
+ return resvec;
+ }
+
+ void NJointBimanualObjLevelVelController::rtPreActivateController()
+ {
+ Eigen::Matrix4f boxInitPose = Eigen::Matrix4f::Identity();
+ Eigen::Matrix4f leftPose = tcpLeft->getPoseInRootFrame();
+ Eigen::Matrix4f rightPose = tcpRight->getPoseInRootFrame();
+ leftPose.block<3, 1>(0, 3) = leftPose.block<3, 1>(0, 3) ;
+ rightPose.block<3, 1>(0, 3) = rightPose.block<3, 1>(0, 3) ;
+ boxInitPose.block<3, 1>(0, 3) = 0.5 * (leftPose.block<3, 1>(0, 3) + rightPose.block<3, 1>(0, 3));
+ boxInitPose.block<3, 3>(0, 0) = leftPose.block<3, 3>(0, 0);
+
+ NJointBimanualObjLevelVelControlData initData;
+ initData.boxPose = boxInitPose;
+ reinitTripleBuffer(initData);
+ }
+
+ std::string NJointBimanualObjLevelVelController::getClassName(const Ice::Current&) const
+ {
+ return "NJointBimanualObjLevelVelController";
+ }
+
+ void NJointBimanualObjLevelVelController::controllerRun()
+ {
+ if (!rt2ControlBuffer.updateReadBuffer() || !dmpStarted)
+ {
+ return;
+ }
+
+ double deltaT = rt2ControlBuffer.getReadBuffer().deltaT;
+ Eigen::Matrix4f currentPose = rt2ControlBuffer.getReadBuffer().currentPose;
+ Eigen::VectorXf currentTwist = rt2ControlBuffer.getReadBuffer().currentTwist;
+
+ if (objectDMP->canVal < 1e-8)
+ {
+ finished = true;
+ dmpStarted = false;
+ }
+
+ objectDMP->flow(deltaT, currentPose, currentTwist);
+
+ LockGuardType guard {controlDataMutex};
+ getWriterControlStruct().boxPose = objectDMP->getTargetPoseMat();
+ writeControlStruct();
+
+ }
+
+
+ Eigen::VectorXf NJointBimanualObjLevelVelController::calcIK(VirtualRobot::DifferentialIKPtr ik, const Eigen::MatrixXf& jacobi, const Eigen::VectorXf& cartesianVel, const Eigen::VectorXf& nullspaceVel)
+ {
+ Eigen::FullPivLU<Eigen::MatrixXf> lu_decomp(jacobi);
+
+ Eigen::MatrixXf nullspace = lu_decomp.kernel();
+ Eigen::VectorXf nsv = Eigen::VectorXf::Zero(nullspace.rows());
+ for (int i = 0; i < nullspace.cols(); i++)
+ {
+ float squaredNorm = nullspace.col(i).squaredNorm();
+ // Prevent division by zero
+ if (squaredNorm > 1.0e-32f)
+ {
+ nsv += nullspace.col(i) * nullspace.col(i).dot(nullspaceVel) / nullspace.col(i).squaredNorm();
+ }
+ }
+
+ Eigen::MatrixXf inv = ik->computePseudoInverseJacobianMatrix(jacobi, ik->getJacobiRegularization(VirtualRobot::IKSolver::CartesianSelection::All));
+ Eigen::VectorXf jointVel = inv * cartesianVel;
+ return jointVel;
+ }
+
+ void NJointBimanualObjLevelVelController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+ {
+ Eigen::Matrix4f currentLeftPose = tcpLeft->getPoseInRootFrame();
+ Eigen::Matrix4f currentRightPose = tcpRight->getPoseInRootFrame();
+
+ double deltaT = timeSinceLastIteration.toSecondsDouble();
+
+ if (firstLoop)
+ {
+ Eigen::Matrix4f leftPose = tcpLeft->getPoseInRootFrame();
+ Eigen::Matrix4f rightPose = tcpRight->getPoseInRootFrame();
+
+ leftPose.block<3, 1>(0, 3) = leftPose.block<3, 1>(0, 3);
+ rightPose.block<3, 1>(0, 3) = rightPose.block<3, 1>(0, 3);
+
+ virtualPose.block<3, 1>(0, 3) = 0.5 * (leftPose.block<3, 1>(0, 3) + rightPose.block<3, 1>(0, 3));
+ virtualPose.block<3, 3>(0, 0) = leftPose.block<3, 3>(0, 0);
+ fixedLeftRightRotOffset = virtualPose.block<3, 3>(0, 0).transpose() * rightPose.block<3, 3>(0, 0);
+
+ Eigen::Vector3f objCom2TCPLeftInGlobal = leftPose.block<3, 1>(0, 3) - virtualPose.block<3, 1>(0, 3);
+ Eigen::Vector3f objCom2TCPRightInGlobal = rightPose.block<3, 1>(0, 3) - virtualPose.block<3, 1>(0, 3);
+
+ objCom2TCPLeftInObjFrame = virtualPose.block<3, 3>(0, 0).transpose() * objCom2TCPLeftInGlobal;
+ objCom2TCPRightInObjFrame = virtualPose.block<3, 3>(0, 0).transpose() * objCom2TCPRightInGlobal;
+ firstLoop = false;
+ }
+
+ // --------------------------------------------- get control parameters ---------------------------------------
+ KpImpedance = interface2rtBuffer.getUpToDateReadBuffer().KpImpedance;
+ KdImpedance = interface2rtBuffer.getUpToDateReadBuffer().KdImpedance;
+
+ // --------------------------------------------- grasp matrix ---------------------------------------------
+
+ Eigen::MatrixXf graspMatrix;
+ graspMatrix.setZero(6, 12);
+ graspMatrix.block<3, 3>(0, 0) = Eigen::MatrixXf::Identity(3, 3);
+ graspMatrix.block<3, 3>(0, 6) = Eigen::MatrixXf::Identity(3, 3);
+ Eigen::Vector3f rvec = virtualPose.block<3, 3>(0, 0) * objCom2TCPLeftInObjFrame;
+ graspMatrix.block<3, 3>(3, 0) = skew(rvec);
+
+ rvec = virtualPose.block<3, 3>(0, 0) * objCom2TCPRightInObjFrame;
+ graspMatrix.block<3, 3>(3, 6) = skew(rvec);
+
+ float lambda = 1;
+ Eigen::MatrixXf pinvGraspMatrixT = leftIK->computePseudoInverseJacobianMatrix(graspMatrix.transpose(), lambda);
+
+ // ---------------------------------------------- object pose ----------------------------------------------
+ Eigen::Matrix4f boxCurrentPose = currentLeftPose;
+ boxCurrentPose.block<3, 1>(0, 3) = 0.5 * (currentLeftPose.block<3, 1>(0, 3) + currentRightPose.block<3, 1>(0, 3));
+ Eigen::VectorXf boxCurrentTwist;
+ boxCurrentTwist.setZero(6);
+
+ // -------------------------------------- get Jacobian matrix and qpos -------------------------------------
+ Eigen::MatrixXf I = Eigen::MatrixXf::Identity(leftTargets.size(), leftTargets.size());
+ // Jacobian matrices
+ Eigen::MatrixXf jacobiL = leftIK->getJacobianMatrix(tcpLeft, VirtualRobot::IKSolver::CartesianSelection::All);
+ Eigen::MatrixXf jacobiR = rightIK->getJacobianMatrix(tcpRight, VirtualRobot::IKSolver::CartesianSelection::All);
+
+ // qpos, qvel
+ Eigen::VectorXf leftqpos;
+ Eigen::VectorXf leftqvel;
+ leftqpos.resize(leftPositionSensors.size());
+ leftqvel.resize(leftVelocitySensors.size());
+ for (size_t i = 0; i < leftVelocitySensors.size(); ++i)
+ {
+ leftqpos(i) = leftPositionSensors[i]->position;
+ leftqvel(i) = leftVelocitySensors[i]->velocity;
+ }
+
+ Eigen::VectorXf rightqpos;
+ Eigen::VectorXf rightqvel;
+ rightqpos.resize(rightPositionSensors.size());
+ rightqvel.resize(rightVelocitySensors.size());
+
+ for (size_t i = 0; i < rightVelocitySensors.size(); ++i)
+ {
+ rightqpos(i) = rightPositionSensors[i]->position;
+ rightqvel(i) = rightVelocitySensors[i]->velocity;
+ }
+
+ // -------------------------------------- compute TCP and object velocity -------------------------------------
+ Eigen::VectorXf currentLeftTwist = jacobiL * leftqvel;
+ Eigen::VectorXf currentRightTwist = jacobiR * rightqvel;
+
+ Eigen::VectorXf currentTwist(12);
+ currentTwist << currentLeftTwist, currentRightTwist;
+ boxCurrentTwist = pinvGraspMatrixT * currentTwist;
+
+ rt2ControlBuffer.getWriteBuffer().currentPose = boxCurrentPose;
+ rt2ControlBuffer.getWriteBuffer().currentTwist = boxCurrentTwist;
+ rt2ControlBuffer.getWriteBuffer().deltaT = deltaT;
+ rt2ControlBuffer.getWriteBuffer().currentTime += deltaT;
+ rt2ControlBuffer.commitWrite();
+
+ // pass sensor value to statechart
+ controlInterfaceBuffer.getWriteBuffer().currentObjPose = boxCurrentPose;
+ controlInterfaceBuffer.getWriteBuffer().currentObjVel = boxCurrentTwist.head(3);
+ controlInterfaceBuffer.getWriteBuffer().currentLeftPose = currentLeftPose;
+ controlInterfaceBuffer.getWriteBuffer().currentRightPose = currentRightPose;
+ controlInterfaceBuffer.commitWrite();
+
+
+ // --------------------------------------------- get MP target ---------------------------------------------
+ virtualPose = rtGetControlStruct().boxPose;
+ // Eigen::VectorXf boxTwist = rtGetControlStruct().boxTwist;
+
+ // --------------------------------------------- convert to tcp pose ---------------------------------------------
+ Eigen::Matrix4f tcpTargetPoseLeft = virtualPose;
+ Eigen::Matrix4f tcpTargetPoseRight = virtualPose;
+
+ tcpTargetPoseRight.block<3, 3>(0, 0) = virtualPose.block<3, 3>(0, 0) * fixedLeftRightRotOffset;
+ tcpTargetPoseLeft.block<3, 1>(0, 3) += virtualPose.block<3, 3>(0, 0) * objCom2TCPLeftInObjFrame;
+ tcpTargetPoseRight.block<3, 1>(0, 3) += virtualPose.block<3, 3>(0, 0) * objCom2TCPRightInObjFrame;
+
+ // --------------------------------------------- Velocity control ---------------------------------------------
+
+ Eigen::Matrix3f diffMatLeft = tcpTargetPoseLeft.block<3, 3>(0, 0) * currentLeftPose.block<3, 3>(0, 0).inverse();
+ Eigen::Vector3f errorRPYLeft = VirtualRobot::MathTools::eigen3f2rpy(diffMatLeft);
+ Eigen::Matrix3f diffMatRight = tcpTargetPoseRight.block<3, 3>(0, 0) * currentRightPose.block<3, 3>(0, 0).inverse();
+ Eigen::Vector3f errorRPYRight = VirtualRobot::MathTools::eigen3f2rpy(diffMatRight);
+
+ Eigen::Vector6f leftTargetVel, rightTargetVel;
+ for (size_t i = 0; i < 3; ++i)
+ {
+ leftTargetVel(i) = KpImpedance(i) * (tcpTargetPoseLeft(i, 3) - currentLeftPose(i, 3)) + KdImpedance(i) * (- currentLeftTwist(i));
+ leftTargetVel(i + 3) = KpImpedance(i + 3) * errorRPYLeft(i) + KdImpedance(i + 3) * (- currentLeftTwist(i + 3));
+ rightTargetVel(i) = KpImpedance(i) * (tcpTargetPoseRight(i, 3) - currentRightPose(i, 3)) + KdImpedance(i) * (- currentRightTwist(i));
+ rightTargetVel(i + 3) = KpImpedance(i + 3) * errorRPYRight(i) + KdImpedance(i + 3) * (- currentRightTwist(i + 3));
+ }
+
+
+
+ Eigen::VectorXf leftJointNullSpaceVel = cfg->knull * (leftDesiredJointValues - leftqpos) - cfg->dnull * leftqvel
+ + cfg->jointLimitAvoidanceKp * leftTCPController->calculateJointLimitAvoidance();
+ Eigen::VectorXf leftJointTargetVel = calcIK(leftIK, jacobiL, leftTargetVel, leftJointNullSpaceVel);
+
+
+ Eigen::VectorXf rightJointNullSpaceVel = cfg->knull * (rightDesiredJointValues - rightqpos) - cfg->dnull * rightqvel
+ + cfg->jointLimitAvoidanceKp * rightTCPController->calculateJointLimitAvoidance();
+ Eigen::VectorXf rightJointTargetVel = calcIK(rightIK, jacobiR, rightTargetVel, rightJointNullSpaceVel);
+
+ for (size_t i = 0; i < leftTargets.size(); ++i)
+ {
+ float desiredVel = leftJointTargetVel(i);
+ debugOutputData.getWriteBuffer().desired_vels[leftJointNames[i]] = desiredVel;
+
+ if (fabs(desiredVel) > cfg->jointVelLimit || isnan(desiredVel))
+ {
+ desiredVel = 0.0;
+ }
+
+ leftTargets.at(i)->velocity = desiredVel;
+ }
+
+ for (size_t i = 0; i < rightTargets.size(); ++i)
+ {
+ float desiredVel = rightJointTargetVel(i);
+ debugOutputData.getWriteBuffer().desired_vels[rightJointNames[i]] = desiredVel;
+
+ if (fabs(desiredVel) > cfg->jointVelLimit || isnan(desiredVel))
+ {
+ desiredVel = 0.0;
+ }
+
+ rightTargets.at(i)->velocity = desiredVel;
+ }
+
+
+ // --------------------------------------------- debug output ---------------------------------------------
+ debugOutputData.getWriteBuffer().virtualPose_x = virtualPose(0, 3);
+ debugOutputData.getWriteBuffer().virtualPose_y = virtualPose(1, 3);
+ debugOutputData.getWriteBuffer().virtualPose_z = virtualPose(2, 3);
+
+ debugOutputData.getWriteBuffer().currentPoseLeft_x = currentLeftPose(0, 3);
+ debugOutputData.getWriteBuffer().currentPoseLeft_y = currentLeftPose(1, 3);
+ debugOutputData.getWriteBuffer().currentPoseLeft_z = currentLeftPose(2, 3);
+
+
+ VirtualRobot::MathTools::Quaternion leftQuat = VirtualRobot::MathTools::eigen4f2quat(currentLeftPose);
+ debugOutputData.getWriteBuffer().leftQuat_w = leftQuat.w;
+ debugOutputData.getWriteBuffer().leftQuat_x = leftQuat.x;
+ debugOutputData.getWriteBuffer().leftQuat_y = leftQuat.y;
+ debugOutputData.getWriteBuffer().leftQuat_z = leftQuat.y;
+
+ debugOutputData.getWriteBuffer().currentPoseRight_x = currentRightPose(0, 3);
+ debugOutputData.getWriteBuffer().currentPoseRight_y = currentRightPose(1, 3);
+ debugOutputData.getWriteBuffer().currentPoseRight_z = currentRightPose(2, 3);
+
+ VirtualRobot::MathTools::Quaternion rightQuat = VirtualRobot::MathTools::eigen4f2quat(currentRightPose);
+ debugOutputData.getWriteBuffer().rightQuat_w = rightQuat.w;
+ debugOutputData.getWriteBuffer().rightQuat_x = rightQuat.x;
+ debugOutputData.getWriteBuffer().rightQuat_y = rightQuat.y;
+ debugOutputData.getWriteBuffer().rightQuat_z = rightQuat.y;
+
+
+ debugOutputData.getWriteBuffer().dmpBoxPose_x = virtualPose(0, 3);
+ debugOutputData.getWriteBuffer().dmpBoxPose_y = virtualPose(1, 3);
+ debugOutputData.getWriteBuffer().dmpBoxPose_z = virtualPose(2, 3);
+ debugOutputData.commitWrite();
+
+ }
+
+ void NJointBimanualObjLevelVelController::learnDMPFromFiles(const Ice::StringSeq& fileNames, const Ice::Current&)
+ {
+ objectDMP->learnDMPFromFiles(fileNames);
+ }
+
+
+ void NJointBimanualObjLevelVelController::setGoals(const Ice::DoubleSeq& goals, const Ice::Current& ice)
+ {
+ LockGuardType guard(controllerMutex);
+ objectDMP->setGoalPoseVec(goals);
+
+ }
+
+
+ void NJointBimanualObjLevelVelController::runDMP(const Ice::DoubleSeq& goals, Ice::Double timeDuration, const Ice::Current&)
+ {
+ while (!controlInterfaceBuffer.updateReadBuffer())
+ {
+ usleep(1000);
+ }
+
+ Eigen::Matrix4f leftPose = controlInterfaceBuffer.getUpToDateReadBuffer().currentLeftPose;
+ Eigen::Matrix4f rightPose = controlInterfaceBuffer.getUpToDateReadBuffer().currentRightPose;
+
+ VirtualRobot::MathTools::Quaternion boxOri = VirtualRobot::MathTools::eigen4f2quat(leftPose);
+ Eigen::Vector3f boxPosi = (leftPose.block<3, 1>(0, 3) + rightPose.block<3, 1>(0, 3)) / 2;
+
+
+ std::vector<double> boxInitialPose;
+ for (size_t i = 0; i < 3; ++i)
+ {
+ boxInitialPose.push_back(boxPosi(i)); //Important: mm -> m
+ }
+ boxInitialPose.push_back(boxOri.w);
+ boxInitialPose.push_back(boxOri.x);
+ boxInitialPose.push_back(boxOri.y);
+ boxInitialPose.push_back(boxOri.z);
+
+ objectDMP->prepareExecution(boxInitialPose, goals);
+ objectDMP->canVal = timeDuration;
+ objectDMP->config.motionTimeDuration = timeDuration;
+
+
+ finished = false;
+ dmpStarted = true;
+ }
+
+ void NJointBimanualObjLevelVelController::runDMPWithVirtualStart(const Ice::DoubleSeq& starts, const Ice::DoubleSeq& goals, Ice::Double timeDuration, const Ice::Current&)
+ {
+ while (!controlInterfaceBuffer.updateReadBuffer())
+ {
+ usleep(1000);
+ }
+ ARMARX_IMPORTANT << "obj level control: setup dmp ...";
+ objectDMP->prepareExecution(starts, goals);
+ objectDMP->canVal = timeDuration;
+ objectDMP->config.motionTimeDuration = timeDuration;
+
+ finished = false;
+ dmpStarted = true;
+
+ ARMARX_IMPORTANT << "obj level control: run dmp with virtual start.";
+ }
+
+ void NJointBimanualObjLevelVelController::setViaPoints(Ice::Double u, const Ice::DoubleSeq& viapoint, const Ice::Current&)
+ {
+ // LockGuardType guard(controllerMutex);
+ ARMARX_INFO << "setting via points ";
+ objectDMP->setViaPose(u, viapoint);
+ }
+
+ void NJointBimanualObjLevelVelController::removeAllViaPoints(const Ice::Current&)
+ {
+ objectDMP->removeAllViaPoints();
+ }
+
+ void NJointBimanualObjLevelVelController::setKpImpedance(const Ice::FloatSeq& value, const Ice::Current&)
+ {
+
+ Eigen::VectorXf setpoint;
+ setpoint.setZero(value.size());
+ ARMARX_CHECK_EQUAL(value.size(), 6);
+
+ for (size_t i = 0; i < value.size(); ++i)
+ {
+ setpoint(i) = value.at(i);
+ }
+
+ LockGuardType guard {interfaceDataMutex};
+ interface2rtBuffer.getWriteBuffer().KpImpedance = setpoint;
+ interface2rtBuffer.commitWrite();
+
+ }
+
+ void NJointBimanualObjLevelVelController::setKdImpedance(const Ice::FloatSeq& value, const Ice::Current&)
+ {
+ Eigen::VectorXf setpoint;
+ setpoint.setZero(value.size());
+ ARMARX_CHECK_EQUAL(value.size(), 6);
+
+ for (size_t i = 0; i < value.size(); ++i)
+ {
+ setpoint(i) = value.at(i);
+ }
+
+ LockGuardType guard {interfaceDataMutex};
+ interface2rtBuffer.getWriteBuffer().KdImpedance = setpoint;
+ interface2rtBuffer.commitWrite();
+ }
+
+
+
+ std::vector<float> NJointBimanualObjLevelVelController::getCurrentObjVel(const Ice::Current&)
+ {
+ Eigen::Vector3f tvel = controlInterfaceBuffer.getUpToDateReadBuffer().currentObjVel;
+ std::vector<float> tvelvec = {tvel(0), tvel(1), tvel(2)};
+ return tvelvec;
+ }
+
+
+ void NJointBimanualObjLevelVelController::onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx& debugObs)
+ {
+
+ StringVariantBaseMap datafields;
+ auto values = debugOutputData.getUpToDateReadBuffer().desired_vels;
+ for (auto& pair : values)
+ {
+ datafields[pair.first] = new Variant(pair.second);
+ }
+
+
+ datafields["virtualPose_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().virtualPose_x);
+ datafields["virtualPose_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().virtualPose_y);
+ datafields["virtualPose_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().virtualPose_z);
+
+ datafields["currentPoseLeft_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentPoseLeft_x);
+ datafields["currentPoseLeft_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentPoseLeft_y);
+ datafields["currentPoseLeft_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentPoseLeft_z);
+
+ datafields["leftQuat_w"] = new Variant(debugOutputData.getUpToDateReadBuffer().leftQuat_w);
+ datafields["leftQuat_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().leftQuat_x);
+ datafields["leftQuat_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().leftQuat_y);
+ datafields["leftQuat_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().leftQuat_z);
+
+
+ datafields["currentPoseRight_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentPoseRight_x);
+ datafields["currentPoseRight_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentPoseRight_y);
+ datafields["currentPoseRight_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentPoseRight_z);
+ datafields["rightQuat_w"] = new Variant(debugOutputData.getUpToDateReadBuffer().rightQuat_w);
+ datafields["rightQuat_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().rightQuat_x);
+ datafields["rightQuat_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().rightQuat_y);
+ datafields["rightQuat_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().rightQuat_z);
+
+ datafields["dmpBoxPose_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().dmpBoxPose_x);
+ datafields["dmpBoxPose_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().dmpBoxPose_y);
+ datafields["dmpBoxPose_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().dmpBoxPose_z);
+
+ debugObs->setDebugChannel("BimanualForceController", datafields);
+ }
+
+ void NJointBimanualObjLevelVelController::onInitNJointController()
+ {
+
+
+ ARMARX_INFO << "init ...";
+ runTask("NJointBimanualObjLevelVelController", [&]
+ {
+ CycleUtil c(1);
+ getObjectScheduler()->waitForObjectStateMinimum(eManagedIceObjectStarted);
+ while (getState() == eManagedIceObjectStarted)
+ {
+ if (isControllerActive())
+ {
+ controllerRun();
+ }
+ c.waitForCycleDuration();
+ }
+ });
+ }
+
+ void NJointBimanualObjLevelVelController::onDisconnectNJointController()
+ {
+ ARMARX_INFO << "stopped ...";
+ }
+}
+
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualObjLevelVelController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualObjLevelVelController.h
new file mode 100644
index 0000000000000000000000000000000000000000..8af7f4e21ddf05264782be327f1339080f5bfee7
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualObjLevelVelController.h
@@ -0,0 +1,245 @@
+#pragma once
+
+#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/IK/DifferentialIK.h>
+
+#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h>
+#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
+#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
+#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
+#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValueForceTorque.h>
+#include <RobotAPI/libraries/DMPController/TaskSpaceDMPController.h>
+#include <RobotAPI/libraries/core/math/MathUtils.h>
+#include <RobotAPI/libraries/core/PIDController.h>
+
+#include <RobotAPI/interface/units/RobotUnit/NJointBimanualObjLevelController.h>
+#include <RobotAPI/libraries/core/CartesianVelocityController.h>
+
+using namespace DMP;
+namespace armarx
+{
+
+
+ TYPEDEF_PTRS_HANDLE(NJointBimanualObjLevelVelController);
+ TYPEDEF_PTRS_HANDLE(NJointBimanualObjLevelVelControlData);
+
+ class NJointBimanualObjLevelVelControlData
+ {
+ public:
+ // control target from Movement Primitives
+ Eigen::Matrix4f boxPose;
+ };
+
+
+ class NJointBimanualObjLevelVelController :
+ public NJointControllerWithTripleBuffer<NJointBimanualObjLevelVelControlData>,
+ public NJointBimanualObjLevelVelControllerInterface
+ {
+ public:
+ // using ConfigPtrT = BimanualForceControllerConfigPtr;
+ NJointBimanualObjLevelVelController(const RobotUnitPtr&, const NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&);
+
+ // NJointControllerInterface interface
+ std::string getClassName(const Ice::Current&) const;
+
+ // NJointController interface
+
+ void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration);
+
+ // NJointCCDMPControllerInterface interface
+ void learnDMPFromFiles(const Ice::StringSeq& fileNames, const Ice::Current&);
+ bool isFinished(const Ice::Current&)
+ {
+ return finished;
+ }
+ Eigen::Matrix3f skew(Eigen::Vector3f vec)
+ {
+ Eigen::Matrix3f mat = Eigen::MatrixXf::Zero(3, 3);
+ mat(1, 2) = -vec(0);
+ mat(0, 2) = vec(1);
+ mat(0, 1) = -vec(2);
+ mat(2, 1) = vec(0);
+ mat(2, 0) = -vec(1);
+ mat(1, 0) = vec(2);
+ return mat;
+ }
+
+ // void runDMP(const Ice::DoubleSeq& goals, Ice::Double tau, const Ice::Current&);
+ void runDMP(const Ice::DoubleSeq& goals, Ice::Double timeDuration, const Ice::Current&);
+ void runDMPWithVirtualStart(const Ice::DoubleSeq& starts, const Ice::DoubleSeq& goals, Ice::Double timeDuration, const Ice::Current&);
+
+ void setGoals(const Ice::DoubleSeq& goals, const Ice::Current&);
+ void setViaPoints(Ice::Double u, const Ice::DoubleSeq& viapoint, const Ice::Current&);
+ void removeAllViaPoints(const Ice::Current&);
+
+ double getVirtualTime(const Ice::Current&)
+ {
+ return virtualtimer;
+ }
+
+ void setKpImpedance(const Ice::FloatSeq& value, const Ice::Current&);
+ void setKdImpedance(const Ice::FloatSeq& value, const Ice::Current&);
+
+ std::vector<float> getCurrentObjVel(const Ice::Current&);
+
+ void setMPWeights(const DoubleSeqSeq& weights, const Ice::Current&);
+ DoubleSeqSeq getMPWeights(const Ice::Current&);
+
+ void setMPRotWeights(const DoubleSeqSeq& weights, const Ice::Current&);
+ DoubleSeqSeq getMPRotWeights(const Ice::Current&);
+ Eigen::VectorXf calcIK(VirtualRobot::DifferentialIKPtr ik, const Eigen::MatrixXf& jacobi, const Eigen::VectorXf& cartesianVel, const Eigen::VectorXf& nullspaceVel);
+ protected:
+ virtual void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&);
+
+ void onInitNJointController();
+ void onDisconnectNJointController();
+ void controllerRun();
+
+ private:
+ Eigen::VectorXf targetWrench;
+ struct DebugBufferData
+ {
+ StringFloatDictionary desired_vels;
+
+ float virtualPose_x;
+ float virtualPose_y;
+ float virtualPose_z;
+
+ float currentPoseLeft_x;
+ float currentPoseLeft_y;
+ float currentPoseLeft_z;
+ float leftQuat_w;
+ float leftQuat_x;
+ float leftQuat_y;
+ float leftQuat_z;
+
+ float currentPoseRight_x;
+ float currentPoseRight_y;
+ float currentPoseRight_z;
+ float rightQuat_w;
+ float rightQuat_x;
+ float rightQuat_y;
+ float rightQuat_z;
+
+
+ float dmpBoxPose_x;
+ float dmpBoxPose_y;
+ float dmpBoxPose_z;
+
+ };
+ TripleBuffer<DebugBufferData> debugOutputData;
+
+ struct rt2ControlData
+ {
+ double currentTime;
+ double deltaT;
+ Eigen::Matrix4f currentPose;
+ Eigen::VectorXf currentTwist;
+ };
+ TripleBuffer<rt2ControlData> rt2ControlBuffer;
+
+ struct ControlInterfaceData
+ {
+ Eigen::Matrix4f currentLeftPose;
+ Eigen::Matrix4f currentRightPose;
+ Eigen::Matrix4f currentObjPose;
+ Eigen::Vector3f currentObjVel;
+ };
+
+ TripleBuffer<ControlInterfaceData> controlInterfaceBuffer;
+
+ struct Inferface2rtData
+ {
+ Eigen::VectorXf KpImpedance;
+ Eigen::VectorXf KdImpedance;
+ };
+ TripleBuffer<Inferface2rtData> interface2rtBuffer;
+
+
+
+ std::vector<ControlTarget1DoFActuatorVelocity*> leftTargets;
+ std::vector<const SensorValue1DoFActuatorVelocity*> leftVelocitySensors;
+ std::vector<const SensorValue1DoFActuatorPosition*> leftPositionSensors;
+
+ std::vector<ControlTarget1DoFActuatorVelocity*> rightTargets;
+ std::vector<const SensorValue1DoFActuatorVelocity*> rightVelocitySensors;
+ std::vector<const SensorValue1DoFActuatorPosition*> rightPositionSensors;
+
+ NJointBimanualObjLevelVelControllerConfigPtr cfg;
+ VirtualRobot::DifferentialIKPtr leftIK;
+ VirtualRobot::DifferentialIKPtr rightIK;
+
+ TaskSpaceDMPControllerPtr objectDMP;
+
+
+
+ double virtualtimer;
+
+ mutable MutexType controllerMutex;
+ mutable MutexType interfaceDataMutex;
+ Eigen::VectorXf leftDesiredJointValues;
+ Eigen::VectorXf rightDesiredJointValues;
+
+ Eigen::Matrix4f leftInitialPose;
+ Eigen::Matrix4f rightInitialPose;
+ Eigen::Matrix4f boxInitialPose;
+
+ Eigen::VectorXf KpImpedance;
+ Eigen::VectorXf KdImpedance;
+ Eigen::VectorXf KpAdmittance;
+ Eigen::VectorXf KdAdmittance;
+ Eigen::VectorXf KmAdmittance;
+ Eigen::VectorXf KmPID;
+
+ Eigen::VectorXf virtualAcc;
+ Eigen::VectorXf virtualVel;
+ Eigen::Matrix4f virtualPose;
+
+ Eigen::Matrix4f sensorFrame2TcpFrameLeft;
+ Eigen::Matrix4f sensorFrame2TcpFrameRight;
+
+ //static compensation
+ float massLeft;
+ Eigen::Vector3f CoMVecLeft;
+ Eigen::Vector3f forceOffsetLeft;
+ Eigen::Vector3f torqueOffsetLeft;
+
+ float massRight;
+ Eigen::Vector3f CoMVecRight;
+ Eigen::Vector3f forceOffsetRight;
+ Eigen::Vector3f torqueOffsetRight;
+
+ // float knull;
+ // float dnull;
+
+ std::vector<std::string> leftJointNames;
+ std::vector<std::string> rightJointNames;
+
+ // float torqueLimit;
+ VirtualRobot::RobotNodeSetPtr leftRNS;
+ VirtualRobot::RobotNodeSetPtr rightRNS;
+ VirtualRobot::RobotNodePtr tcpLeft;
+ VirtualRobot::RobotNodePtr tcpRight;
+
+ CartesianVelocityControllerPtr leftTCPController;
+ CartesianVelocityControllerPtr rightTCPController;
+
+ std::vector<PIDControllerPtr> forcePIDControllers;
+
+ // filter parameters
+ float filterCoeff;
+ Eigen::VectorXf filteredOldValue;
+ bool finished;
+ bool dmpStarted;
+ Eigen::VectorXf ftOffset;
+
+ Eigen::Matrix3f fixedLeftRightRotOffset;
+ Eigen::Vector3f objCom2TCPLeftInObjFrame, objCom2TCPRightInObjFrame;
+
+ protected:
+ void rtPreActivateController();
+ bool firstLoop;
+ };
+
+} // namespace armarx
+
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/CMakeLists.txt b/source/RobotAPI/libraries/RobotAPINJointControllers/CMakeLists.txt
index 0d6c896ddb6482e812edeee86f748ca99442e954..b9d558a2e6d4fdc6b6367d3a897f6455244100e5 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/CMakeLists.txt
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/CMakeLists.txt
@@ -34,8 +34,12 @@ list(APPEND LIB_HEADERS
DMPController/NJointPeriodicTSDMPCompliantController.h
DMPController/NJointAdaptiveWipingController.h
DMPController/NJointAnomalyDetectionAdaptiveWipingController.h
+ DMPController/NJointTaskSpaceAdaptiveDMPController.h
BimanualForceControllers/NJointBimanualForceController.h
BimanualForceControllers/NJointBimanualObjLevelController.h
+ BimanualForceControllers/NJointBimanualObjLevelVelController.h
+ BimanualForceControllers/NJointBimanualObjLevelMultiMPController.h
+
)
list(APPEND LIB_FILES
DMPController/NJointJSDMPController.cpp
@@ -50,8 +54,11 @@ list(APPEND LIB_FILES
DMPController/NJointPeriodicTSDMPCompliantController.cpp
DMPController/NJointAdaptiveWipingController.cpp
DMPController/NJointAnomalyDetectionAdaptiveWipingController.cpp
+ DMPController/NJointTaskSpaceAdaptiveDMPController.cpp
BimanualForceControllers/NJointBimanualForceController.cpp
BimanualForceControllers/NJointBimanualObjLevelController.cpp
+ BimanualForceControllers/NJointBimanualObjLevelVelController.cpp
+ BimanualForceControllers/NJointBimanualObjLevelMultiMPController.cpp
)
list(APPEND LIBS ${DMP_LIBRARIES} DMPController)
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointAnomalyDetectionAdaptiveWipingController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointAnomalyDetectionAdaptiveWipingController.cpp
index 2e3ec38e5e4cf64955a6ca6107b32e779aa1418e..65835f6ac7c69d3dd16ae361095bbdecfd848bc1 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointAnomalyDetectionAdaptiveWipingController.cpp
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointAnomalyDetectionAdaptiveWipingController.cpp
@@ -28,6 +28,8 @@ namespace armarx
positionSensors.push_back(positionSensor);
};
+ useDMPInGlobalFrame = cfg->useDMPInGlobalFrame;
+
tcp = rns->getTCP();
// set tcp controller
nodeSetName = cfg->nodeSetName;
@@ -59,6 +61,7 @@ namespace armarx
{
initData.targetTSVel(i) = 0;
}
+ initData.targetTSPose = tcp->getPoseInRootFrame();
reinitTripleBuffer(initData);
firstRun = true;
@@ -82,6 +85,7 @@ namespace armarx
kori << cfg->Kori[0], cfg->Kori[1], cfg->Kori[2];
dori << cfg->Dori[0], cfg->Dori[1], cfg->Dori[2];
+ isForceCtrlInForceDir = cfg->isForceCtrlInForceDir;
isForceControlEnabled = cfg->isForceControlEnabled;
isRotControlEnabled = cfg->isRotControlEnabled;
isTorqueControlEnabled = cfg->isTorqueControlEnabled;
@@ -166,6 +170,7 @@ namespace armarx
abnormalFlag = false;
lastAbnormalFlag = false;
+ positionOffset.setZero();
// toolToFTSensorLink =
// rtGetRobot()->getRobotNode(cfg->forceFrameName)->getPoseInRootFrame().block<3,1>(0, 3) -
@@ -226,10 +231,12 @@ namespace armarx
}
Eigen::VectorXf targetVels(6);
+ Eigen::Matrix4f targetDMPPose;
bool isPhaseStop = rt2CtrlData.getUpToDateReadBuffer().isPhaseStop;
if (isPhaseStop)
{
targetVels.setZero();
+ targetDMPPose = rt2CtrlData.getUpToDateReadBuffer().currentPose;
}
else
{
@@ -242,6 +249,7 @@ namespace armarx
dmpCtrl->flow(deltaT, currentPose, currentTwist);
targetVels = dmpCtrl->getTargetVelocity();
+ targetDMPPose = dmpCtrl->getTargetPoseMat();
debugOutputData.getWriteBuffer().latestTargetVelocities["x_vel"] = targetVels(0);
debugOutputData.getWriteBuffer().latestTargetVelocities["y_vel"] = targetVels(1);
@@ -267,6 +275,7 @@ namespace armarx
}
getWriterControlStruct().canVal = dmpCtrl->canVal;
getWriterControlStruct().targetTSVel = targetVels;
+ getWriterControlStruct().targetTSPose = targetDMPPose;
writeControlStruct();
dmpRunning = true;
@@ -338,11 +347,13 @@ namespace armarx
}
else
{
+ rtUpdateControlStruct();
+ targetVel = rtGetControlStruct().targetTSVel;
+
+
Eigen::Matrix3f currentToolOri = currentPose.block<3, 3>(0, 0) * toolTransform;
/* -------------------------- get target vel from dmp thread --------------------------------- */
- rtUpdateControlStruct();
- targetVel = rtGetControlStruct().targetTSVel;
targetVel(2) = 0;
targetVel.head(3) = currentToolOri * targetVel.head(3);
targetVel.tail(3) = currentToolOri * targetVel.tail(3);
@@ -422,6 +433,9 @@ namespace armarx
adaptKNull *= cfg->adaptRateDecrease;
adaptDNull *= cfg->adaptRateDecrease;
+ positionOffset.setZero();
+ forcePID->reset();
+
// lastDiff = diff;
}
else
@@ -463,6 +477,7 @@ namespace armarx
startLoseContactDetection = true;
loseContactCounter = 0;
forceIntegral = 0;
+
}
if (startLoseContactDetection && loseContactCounter < cfg->loseContactCounterMax)
{
@@ -530,7 +545,14 @@ namespace armarx
/* -------------------------- Force Regulation and Torque PID Controller --------------------------------- */
- forcePID->update(deltaT, forceInToolFrame(2), targetForce);
+ if (isForceCtrlInForceDir)
+ {
+ forcePID->update(deltaT, forceInToolFrame.norm(), targetForce);
+ }
+ else
+ {
+ forcePID->update(deltaT, forceInToolFrame(2), targetForce);
+ }
torquePID->update(deltaT, torqueInToolFrame(1), 0.0);
/* -------------------------- Rotation PID Controller --------------------------------- */
@@ -663,12 +685,39 @@ namespace armarx
// // targetFTInToolFrame.tail(3) += (float)lcrPID->getControlValue() * compensationAxis;
//// targetVel.tail(3) += (float)lcrPID->getControlValue() * compensationAxis;
// }
+
if (isForceControlEnabled)
{
- // targetFTInToolFrame(2) -= (float)forcePID->getControlValue() * forceControlGate;
- targetVel(2) -= (float)forcePID->getControlValue();
- targetVel.head(3) = currentToolOri * targetVel.head(3);
+ if (isForceCtrlInForceDir)
+ {
+ float forcePIDVel = -(float)forcePID->getControlValue();
+ Eigen::Vector3f targetVelInTool;
+ if (forceInToolFrame.norm() < 1e-4)
+ {
+ targetVelInTool << 0, 0, forcePIDVel;
+ }
+ else
+ {
+ targetVelInTool = forceInToolFrame / forceInToolFrame.norm() * forcePIDVel;
+ }
+ targetVel.head(3) += currentToolOri * targetVelInTool;
+ }
+ else
+ {
+ // targetFTInToolFrame(2) -= (float)forcePID->getControlValue() * forceControlGate;
+ Eigen::Vector3f localVel;
+ localVel << 0, 0, -(float)forcePID->getControlValue();
+ positionOffset += currentToolOri * localVel * deltaT;
+
+ targetVel(2) -= (float)forcePID->getControlValue();
+ targetVel.head(3) = currentToolOri * targetVel.head(3);
+ }
+ }
+ else
+ {
+ positionOffset.setZero();
}
+
if (isRotControlEnabled)
{
// targetFTInToolFrame(4) -= (float)rotPID->getControlValue();
@@ -688,42 +737,49 @@ namespace armarx
rt2UserData.getWriteBuffer().currentTcpPose = currentPose;
rt2UserData.getWriteBuffer().tcpTranslVel = currentTwist.head(3);
- rt2UserData.getWriteBuffer().forceOutput = forceInToolFrame;
+ rt2UserData.getWriteBuffer().forceOutput = filteredForceInRoot;
rt2UserData.getWriteBuffer().waitTimeForCalibration += deltaT;
rt2UserData.commitWrite();
/* -------------------------- Integrate Target Velocity 2 Target Pose --------------------------------- */
- targetPose.block<3, 1>(0, 3) = targetPose.block<3, 1>(0, 3) + dTf * targetVel.block<3, 1>(0, 0);
- Eigen::Matrix3f rotMat = VirtualRobot::MathTools::rpy2eigen3f(dTf * targetVel(3), dTf * targetVel(4), dTf * targetVel(5));
- targetPose.block<3, 3>(0, 0) = rotMat * targetPose.block<3, 3>(0, 0);
-
- if (isPhaseStop)
+ if (useDMPInGlobalFrame)
{
- Eigen::Vector2f currentXY = currentPose.block<2, 1>(0, 3);
- if ((lastPosition - currentXY).norm() < cfg->changePositionTolerance)
+ targetPose = rtGetControlStruct().targetTSPose;
+ targetPose.block<3, 1>(0, 3) += positionOffset;
+ }
+ else
+ {
+ targetPose.block<3, 1>(0, 3) = targetPose.block<3, 1>(0, 3) + dTf * targetVel.block<3, 1>(0, 0);
+ Eigen::Matrix3f rotMat = VirtualRobot::MathTools::rpy2eigen3f(dTf * targetVel(3), dTf * targetVel(4), dTf * targetVel(5));
+ targetPose.block<3, 3>(0, 0) = rotMat * targetPose.block<3, 3>(0, 0);
+
+ if (isPhaseStop)
{
- changeTimer += deltaT;
+ Eigen::Vector2f currentXY = currentPose.block<2, 1>(0, 3);
+ if ((lastPosition - currentXY).norm() < cfg->changePositionTolerance)
+ {
+ changeTimer += deltaT;
+ }
+ else
+ {
+ lastPosition = currentPose.block<2, 1>(0, 3);
+ changeTimer = 0;
+ }
+
+ if (changeTimer > cfg->changeTimerThreshold)
+ {
+ targetPose(0, 3) = currentPose(0, 3);
+ targetPose(1, 3) = currentPose(1, 3);
+ isPhaseStop = false;
+ changeTimer = 0;
+ }
}
else
{
- lastPosition = currentPose.block<2, 1>(0, 3);
- changeTimer = 0;
- }
-
- if (changeTimer > cfg->changeTimerThreshold)
- {
- targetPose(0, 3) = currentPose(0, 3);
- targetPose(1, 3) = currentPose(1, 3);
- isPhaseStop = false;
changeTimer = 0;
}
}
- else
- {
- changeTimer = 0;
- }
-
targetPose(0, 3) = targetPose(0, 3) > cfg->ws_x[0] ? targetPose(0, 3) : cfg->ws_x[0];
targetPose(0, 3) = targetPose(0, 3) < cfg->ws_x[1] ? targetPose(0, 3) : cfg->ws_x[1];
@@ -897,8 +953,9 @@ namespace armarx
std::vector<float> NJointAnomalyDetectionAdaptiveWipingController::getAnomalyInput(const Ice::Current&)
{
+ Eigen::Matrix4f tpos = rt2UserData.getUpToDateReadBuffer().currentTcpPose;
Eigen::Vector3f tvel = rt2UserData.getUpToDateReadBuffer().tcpTranslVel;
- std::vector<float> tvelvec = {tvel(0), tvel(1), tvel(2)};
+ std::vector<float> tvelvec = {tvel(0), tvel(1), tvel(2), tpos(0, 3) / 1000, tpos(1, 3) / 1000, tpos(2, 3) / 1000};
return tvelvec;
}
@@ -921,11 +978,26 @@ namespace armarx
datafields["target_y"] = new Variant(targetPoseDebug(1, 3));
datafields["target_z"] = new Variant(targetPoseDebug(2, 3));
+ VirtualRobot::MathTools::Quaternion qtarget = VirtualRobot::MathTools::eigen4f2quat(targetPoseDebug);
+ datafields["target_qw"] = new Variant(qtarget.w);
+ datafields["target_qx"] = new Variant(qtarget.x);
+ datafields["target_qy"] = new Variant(qtarget.y);
+ datafields["target_qz"] = new Variant(qtarget.z);
+
+
Eigen::Matrix4f currentPoseDebug = debugRT.getUpToDateReadBuffer().currentPose;
datafields["current_x"] = new Variant(currentPoseDebug(0, 3));
datafields["current_y"] = new Variant(currentPoseDebug(1, 3));
datafields["current_z"] = new Variant(currentPoseDebug(2, 3));
+
+ VirtualRobot::MathTools::Quaternion qcurrent = VirtualRobot::MathTools::eigen4f2quat(currentPoseDebug);
+ datafields["current_qw"] = new Variant(qcurrent.w);
+ datafields["current_qx"] = new Variant(qcurrent.x);
+ datafields["current_qy"] = new Variant(qcurrent.y);
+ datafields["current_qz"] = new Variant(qcurrent.z);
+
+
Eigen::Vector3f filteredForce = debugRT.getUpToDateReadBuffer().filteredForce;
datafields["filteredforceInTool_x"] = new Variant(filteredForce(0));
datafields["filteredforceInTool_y"] = new Variant(filteredForce(1));
@@ -1055,6 +1127,16 @@ namespace armarx
+ void NJointAnomalyDetectionAdaptiveWipingController::pauseDMP(const Ice::Current&)
+ {
+ dmpCtrl->pauseController();
+ }
+
+ void NJointAnomalyDetectionAdaptiveWipingController::resumeDMP(const Ice::Current&)
+ {
+ dmpCtrl->resumeController();
+ }
+
void NJointAnomalyDetectionAdaptiveWipingController::onDisconnectNJointController()
{
ARMARX_INFO << "stopped ...";
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointAnomalyDetectionAdaptiveWipingController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointAnomalyDetectionAdaptiveWipingController.h
index 495d9ba47d28fd257f6a91c459a71adbd7c4a779..5a34c6833a7b42ebeae47248cc145cf571093f0c 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointAnomalyDetectionAdaptiveWipingController.h
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointAnomalyDetectionAdaptiveWipingController.h
@@ -27,6 +27,7 @@ namespace armarx
{
public:
Eigen::VectorXf targetTSVel;
+ Eigen::Matrix4f targetTSPose;
double canVal;
};
@@ -71,6 +72,8 @@ namespace armarx
std::vector<float> getAnomalyInput(const Ice::Current&);
std::vector<float> getAnomalyOutput(const Ice::Current&);
+ void pauseDMP(const Ice::Current&);
+ void resumeDMP(const Ice::Current&);
protected:
virtual void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&);
@@ -227,6 +230,7 @@ namespace armarx
const SensorValueForceTorque* forceSensor;
// pid controllers
+ bool isForceCtrlInForceDir;
bool isForceControlEnabled;
bool isRotControlEnabled;
bool isTorqueControlEnabled;
@@ -260,11 +264,14 @@ namespace armarx
Eigen::Matrix3f origHandOri;
Eigen::VectorXf qvel_filtered;
+ bool useDMPInGlobalFrame;
+
float lastDiff;
Eigen::Vector2f lastPosition;
double changeTimer;
Eigen::Vector3f toolToFTSensorLink;
+ Eigen::Vector3f positionOffset;
};
} // namespace armarx
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.cpp
index efbb78ce6ac40d4104e059167446028c4a4dceb0..da6292b2769145c6297354dfe8713869955efe76 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.cpp
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.cpp
@@ -596,6 +596,26 @@ namespace armarx
ARMARX_INFO << "stopped ...";
}
+ void NJointTSDMPController::setMPWeights(const DoubleSeqSeq& weights, const Ice::Current&)
+ {
+ dmpCtrl->setWeights(weights);
+ }
+ DoubleSeqSeq NJointTSDMPController::getMPWeights(const Ice::Current&)
+ {
+ DMP::DVec2d res = dmpCtrl->getWeights();
+ DoubleSeqSeq resvec;
+ for (size_t i = 0; i < res.size(); ++i)
+ {
+ std::vector<double> cvec;
+ for (size_t j = 0; j < res[i].size(); ++j)
+ {
+ cvec.push_back(res[i][j]);
+ }
+ resvec.push_back(cvec);
+ }
+
+ return resvec;
+ }
}
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.h
index c8e2ce878b0717abb20fd9e1b15e3aa81966f4a4..106a0c6faf84f36f10ae2c793dac49aeda4cabf6 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.h
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.h
@@ -102,6 +102,11 @@ namespace armarx
// VirtualRobot::IKSolver::CartesianSelection ModeFromIce(const NJointTaskSpaceDMPControllerMode::CartesianSelection mode);
Eigen::VectorXf calcIK(const Eigen::VectorXf& cartesianVel, const Eigen::VectorXf& nullspace, VirtualRobot::IKSolver::CartesianSelection mode);
+
+
+ void setMPWeights(const DoubleSeqSeq& weights, const Ice::Current&);
+ DoubleSeqSeq getMPWeights(const Ice::Current&);
+
protected:
void rtPreActivateController() override;
void rtPostDeactivateController() override;
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceAdaptiveDMPController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceAdaptiveDMPController.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..b9c22ac034eef26cf2dda818121684224b181321
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceAdaptiveDMPController.cpp
@@ -0,0 +1,739 @@
+#include "NJointTaskSpaceAdaptiveDMPController.h"
+
+#include <ArmarXCore/core/ArmarXObjectScheduler.h>
+
+namespace armarx
+{
+ NJointControllerRegistration<NJointTaskSpaceAdaptiveDMPController> registrationControllerNJointTaskSpaceAdaptiveDMPController("NJointTaskSpaceAdaptiveDMPController");
+
+ NJointTaskSpaceAdaptiveDMPController::NJointTaskSpaceAdaptiveDMPController(const RobotUnitPtr& robotUnit, const armarx::NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&)
+ {
+ ARMARX_INFO << "creating impedance dmp controller";
+ cfg = NJointTaskSpaceAdaptiveDMPControllerConfigPtr::dynamicCast(config);
+ useSynchronizedRtRobot();
+ rns = rtGetRobot()->getRobotNodeSet(cfg->nodeSetName);
+ ARMARX_CHECK_EXPRESSION(rns) << cfg->nodeSetName;
+
+ for (size_t i = 0; i < rns->getSize(); ++i)
+ {
+ std::string jointName = rns->getNode(i)->getName();
+ jointNames.push_back(jointName);
+ ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Torque1DoF);
+ const SensorValueBase* sv = useSensorValue(jointName);
+ targets.push_back(ct->asA<ControlTarget1DoFActuatorTorque>());
+ const SensorValue1DoFActuatorVelocity* velocitySensor = sv->asA<SensorValue1DoFActuatorVelocity>();
+ const SensorValue1DoFActuatorPosition* positionSensor = sv->asA<SensorValue1DoFActuatorPosition>();
+
+ if (!velocitySensor)
+ {
+ ARMARX_WARNING << "No velocitySensor available for " << jointName;
+ }
+ if (!positionSensor)
+ {
+ ARMARX_WARNING << "No positionSensor available for " << jointName;
+ }
+
+ velocitySensors.push_back(velocitySensor);
+ positionSensors.push_back(positionSensor);
+ };
+
+
+ const SensorValueBase* svlf = robotUnit->getSensorDevice(cfg->forceSensorName)->getSensorValue();
+ forceSensor = svlf->asA<SensorValueForceTorque>();
+
+ tcp = rns->getTCP();
+ ik.reset(new VirtualRobot::DifferentialIK(rns, rtGetRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
+ numOfJoints = targets.size();
+ // set DMP
+ double phaseL = 1000;
+ double phaseK = 1000;
+ double phaseDist0 = 10000;
+ double phaseDist1 = 10000;
+ double posToOriRatio = 10;
+
+ TaskSpaceDMPControllerConfig taskSpaceDMPConfig;
+ taskSpaceDMPConfig.motionTimeDuration = cfg->timeDuration;
+ taskSpaceDMPConfig.DMPKernelSize = cfg->kernelSize;
+ taskSpaceDMPConfig.DMPMode = cfg->dmpMode;
+ taskSpaceDMPConfig.DMPStyle = cfg->dmpType;
+ taskSpaceDMPConfig.DMPAmplitude = 1.0;
+ taskSpaceDMPConfig.phaseStopParas.goDist = phaseDist0;
+ taskSpaceDMPConfig.phaseStopParas.backDist = phaseDist1;
+ taskSpaceDMPConfig.phaseStopParas.Kpos = 0;
+ taskSpaceDMPConfig.phaseStopParas.Dpos = 0;
+ taskSpaceDMPConfig.phaseStopParas.Kori = 0;
+ taskSpaceDMPConfig.phaseStopParas.Dori = 0;
+ taskSpaceDMPConfig.phaseStopParas.mm2radi = posToOriRatio;
+ taskSpaceDMPConfig.phaseStopParas.maxValue = phaseL;
+ taskSpaceDMPConfig.phaseStopParas.slop = phaseK;
+
+ dmpCtrl.reset(new TaskSpaceDMPController("DMPController", taskSpaceDMPConfig, false));
+ finished = false;
+
+ useNullSpaceJointDMP = cfg->useNullSpaceJointDMP;
+ nullSpaceJointDMPPtr.reset(new DMP::UMIDMP(100));
+
+ isNullSpaceJointDMPLearned = false;
+
+ defaultNullSpaceJointValues.resize(targets.size());
+ ARMARX_CHECK_EQUAL(cfg->defaultNullSpaceJointValues.size(), targets.size());
+
+ for (size_t i = 0; i < targets.size(); ++i)
+ {
+ defaultNullSpaceJointValues(i) = cfg->defaultNullSpaceJointValues.at(i);
+ }
+
+
+ kpos << cfg->Kpos[0], cfg->Kpos[1], cfg->Kpos[2];
+ dpos << cfg->Dpos[0], cfg->Dpos[1], cfg->Dpos[2];
+ kori << cfg->Kori[0], cfg->Kori[1], cfg->Kori[2];
+ dori << cfg->Dori[0], cfg->Dori[1], cfg->Dori[2];
+
+
+ ARMARX_CHECK_EQUAL(cfg->Knull.size(), targets.size());
+ ARMARX_CHECK_EQUAL(cfg->Dnull.size(), targets.size());
+
+ knull.setZero(targets.size());
+ dnull.setZero(targets.size());
+
+ for (size_t i = 0; i < targets.size(); ++i)
+ {
+ knull(i) = cfg->Knull.at(i);
+ dnull(i) = cfg->Dnull.at(i);
+ }
+
+ torqueLimit = cfg->torqueLimit;
+ timeDuration = cfg->timeDuration;
+
+ NJointTaskSpaceAdaptiveDMPControllerInterfaceData initInterfaceData;
+ initInterfaceData.currentTcpPose = Eigen::Matrix4f::Identity();
+ initInterfaceData.currentForce = Eigen::Vector3f::Zero();
+ initInterfaceData.currentVel.setZero(6);
+ interfaceData.reinitAllBuffers(initInterfaceData);
+
+ NJointTaskSpaceAdaptiveDMPControllerSensorData initControllerSensorData;
+ initControllerSensorData.currentPose = Eigen::Matrix4f::Identity();
+ initControllerSensorData.currentTime = 0;
+ initControllerSensorData.deltaT = 0;
+ initControllerSensorData.currentTwist.setZero();
+ controllerSensorData.reinitAllBuffers(initControllerSensorData);
+
+ //initialize control parameters
+ Eigen::VectorXf KpImpedance;
+ KpImpedance.setZero(6);
+
+ for (int i = 0; i < 3; ++i)
+ {
+ KpImpedance(i) = cfg->Kpos.at(i);
+ KpImpedance(i + 3) = cfg->Kori.at(i);
+ }
+
+ Eigen::VectorXf KdImpedance;
+ KdImpedance.setZero(6);
+
+ for (int i = 0; i < 3; ++i)
+ {
+ KdImpedance(i) = cfg->Dpos.at(i);
+ KdImpedance(i + 3) = cfg->Dori.at(i);
+
+ }
+
+ Inferface2rtData initInt2rtData;
+ initInt2rtData.KpImpedance = KpImpedance;
+ initInt2rtData.KdImpedance = KdImpedance;
+ initInt2rtData.Knull = knull;
+ initInt2rtData.Dnull = dnull;
+ interface2rtBuffer.reinitAllBuffers(initInt2rtData);
+
+
+ Interface2CtrlData initInt2CtrlData;
+ initInt2CtrlData.canVal = 1.0;
+ interface2CtrlBuffer.reinitAllBuffers(initInt2CtrlData);
+
+ firstRun = true;
+ forceOffset.setZero(3);
+ filteredForce.setZero(3);
+
+ ARMARX_INFO << "Finished controller constructor ";
+ }
+
+ std::string NJointTaskSpaceAdaptiveDMPController::getClassName(const Ice::Current&) const
+ {
+ return "NJointTaskSpaceAdaptiveDMPController";
+
+ }
+
+
+ void NJointTaskSpaceAdaptiveDMPController::rtPreActivateController()
+ {
+ NJointTaskSpaceAdaptiveDMPControllerControlData initData;
+ initData.targetPose = tcp->getPoseInRootFrame();
+ initData.targetVel.resize(6);
+ initData.targetVel.setZero();
+ initData.desiredNullSpaceJointValues = defaultNullSpaceJointValues;
+ reinitTripleBuffer(initData);
+
+
+ }
+
+
+ void NJointTaskSpaceAdaptiveDMPController::controllerRun()
+ {
+
+
+ if (!dmpCtrl)
+ {
+ return;
+ }
+
+ if (!controllerSensorData.updateReadBuffer())
+ {
+ return;
+ }
+
+
+ double deltaT = 0.001; //controllerSensorData.getReadBuffer().deltaT;
+ Eigen::Matrix4f currentPose = controllerSensorData.getReadBuffer().currentPose;
+ Eigen::VectorXf currentTwist = controllerSensorData.getReadBuffer().currentTwist;
+
+ if (interface2CtrlBuffer.updateReadBuffer())
+ {
+ dmpCtrl->canVal = interface2CtrlBuffer.getUpToDateReadBuffer().canVal;
+ }
+
+ if (!started)
+ {
+ LockGuardType guard {controlDataMutex};
+ getWriterControlStruct().desiredNullSpaceJointValues = defaultNullSpaceJointValues;
+ getWriterControlStruct().targetVel.setZero(6);
+ getWriterControlStruct().targetPose = currentPose;
+ getWriterControlStruct().canVal = 1.0;
+ getWriterControlStruct().mpcFactor = 0.0;
+ writeControlStruct();
+ }
+ else
+ {
+ if (stopped)
+ {
+
+ LockGuardType guard {controlDataMutex};
+ getWriterControlStruct().desiredNullSpaceJointValues = defaultNullSpaceJointValues;
+ getWriterControlStruct().targetVel.setZero(6);
+ getWriterControlStruct().targetPose = oldPose;
+ getWriterControlStruct().canVal = dmpCtrl->canVal;
+ getWriterControlStruct().mpcFactor = dmpCtrl->debugData.mpcFactor;
+ writeControlStruct();
+ }
+ else
+ {
+ if (dmpCtrl->canVal < 1e-8)
+ {
+ finished = true;
+ LockGuardType guard {controlDataMutex};
+ getWriterControlStruct().targetVel.setZero();
+ writeControlStruct();
+ return;
+ }
+
+ dmpCtrl->flow(deltaT, currentPose, currentTwist);
+
+ Eigen::VectorXf desiredNullSpaceJointValues(jointNames.size());
+ if (useNullSpaceJointDMP && isNullSpaceJointDMPLearned)
+ {
+ DMP::DVec targetJointState;
+ currentJointState = nullSpaceJointDMPPtr->calculateDirectlyVelocity(currentJointState, dmpCtrl->canVal / timeDuration, deltaT / timeDuration, targetJointState);
+
+ if (targetJointState.size() == jointNames.size())
+ {
+ for (size_t i = 0; i < targetJointState.size(); ++i)
+ {
+ desiredNullSpaceJointValues(i) = targetJointState[i];
+ }
+ }
+ else
+ {
+ desiredNullSpaceJointValues = defaultNullSpaceJointValues;
+ }
+ }
+ else
+ {
+ desiredNullSpaceJointValues = defaultNullSpaceJointValues;
+ }
+
+ LockGuardType guard {controlDataMutex};
+ getWriterControlStruct().desiredNullSpaceJointValues = desiredNullSpaceJointValues;
+ getWriterControlStruct().targetVel = dmpCtrl->getTargetVelocity();
+ getWriterControlStruct().targetPose = dmpCtrl->getTargetPoseMat();
+ getWriterControlStruct().canVal = dmpCtrl->canVal;
+ getWriterControlStruct().mpcFactor = dmpCtrl->debugData.mpcFactor;
+
+ writeControlStruct();
+ }
+ }
+ }
+
+ void NJointTaskSpaceAdaptiveDMPController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+ {
+ double deltaT = timeSinceLastIteration.toSecondsDouble();
+ Eigen::Matrix4f currentPose = tcp->getPoseInRootFrame();
+
+ Eigen::MatrixXf jacobi = ik->getJacobianMatrix(tcp, VirtualRobot::IKSolver::CartesianSelection::All);
+
+ Eigen::VectorXf qpos(positionSensors.size());
+ Eigen::VectorXf qvel(velocitySensors.size());
+ for (size_t i = 0; i < positionSensors.size(); ++i)
+ {
+ qpos(i) = positionSensors[i]->position;
+ qvel(i) = velocitySensors[i]->velocity;
+ }
+
+ Eigen::VectorXf currentTwist = jacobi * qvel;
+
+ controllerSensorData.getWriteBuffer().currentPose = currentPose;
+ controllerSensorData.getWriteBuffer().currentTwist = currentTwist;
+ controllerSensorData.getWriteBuffer().deltaT = deltaT;
+ controllerSensorData.getWriteBuffer().currentTime += deltaT;
+ controllerSensorData.commitWrite();
+
+
+ Eigen::Matrix4f targetPose;
+ Eigen::VectorXf targetVel;
+ Eigen::VectorXf desiredNullSpaceJointValues;
+ if (firstRun || !started)
+ {
+ firstRun = false;
+ targetPose = currentPose;
+ targetVel.setZero(6);
+ desiredNullSpaceJointValues = defaultNullSpaceJointValues;
+ forceOffset = 0.1 * forceOffset + 0.9 * forceSensor->force;
+ }
+ else
+ {
+ filteredForce = (1 - cfg->filterCoeff) * filteredForce + cfg->filterCoeff * (forceSensor->force - forceOffset);
+ targetPose = rtGetControlStruct().targetPose;
+ targetVel = rtGetControlStruct().targetVel;
+ desiredNullSpaceJointValues = rtGetControlStruct().desiredNullSpaceJointValues;
+ }
+
+ interfaceData.getWriteBuffer().currentTcpPose = currentPose;
+ interfaceData.getWriteBuffer().currentForce = filteredForce;
+ interfaceData.getWriteBuffer().currentVel = currentTwist;
+ interfaceData.commitWrite();
+
+
+ kpos = interface2rtBuffer.getUpToDateReadBuffer().KpImpedance.head(3);
+ kori = interface2rtBuffer.getUpToDateReadBuffer().KpImpedance.tail(3);
+ dpos = interface2rtBuffer.getUpToDateReadBuffer().KdImpedance.head(3);
+ dori = interface2rtBuffer.getUpToDateReadBuffer().KdImpedance.tail(3);
+ knull = interface2rtBuffer.getUpToDateReadBuffer().Knull;
+ dnull = interface2rtBuffer.getUpToDateReadBuffer().Dnull;
+
+ /* calculate torques in meter */
+ jacobi.block(0, 0, 3, numOfJoints) = 0.001 * jacobi.block(0, 0, 3, numOfJoints); // convert mm to m
+ Eigen::Vector6f jointControlWrench;
+ {
+ Eigen::Vector3f targetTCPLinearVelocity;
+ targetTCPLinearVelocity << 0.001 * targetVel(0), 0.001 * targetVel(1), 0.001 * targetVel(2);
+ Eigen::Vector3f currentTCPLinearVelocity;
+ currentTCPLinearVelocity << 0.001 * currentTwist(0), 0.001 * currentTwist(1), 0.001 * currentTwist(2);
+ Eigen::Vector3f currentTCPPosition = currentPose.block<3, 1>(0, 3);
+ Eigen::Vector3f desiredPosition = targetPose.block<3, 1>(0, 3);
+ Eigen::Vector3f tcpDesiredForce = 0.001 * kpos.cwiseProduct(desiredPosition - currentTCPPosition) + dpos.cwiseProduct(targetTCPLinearVelocity - currentTCPLinearVelocity);
+
+ Eigen::Vector3f currentTCPAngularVelocity;
+ currentTCPAngularVelocity << currentTwist(3), currentTwist(4), currentTwist(5);
+ Eigen::Matrix3f currentRotMat = currentPose.block<3, 3>(0, 0);
+ Eigen::Matrix3f diffMat = targetPose.block<3, 3>(0, 0) * currentRotMat.inverse();
+ Eigen::Vector3f rpy = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
+ Eigen::Vector3f tcpDesiredTorque = kori.cwiseProduct(rpy) - dori.cwiseProduct(currentTCPAngularVelocity);
+ jointControlWrench << tcpDesiredForce, tcpDesiredTorque;
+ }
+
+ Eigen::MatrixXf I = Eigen::MatrixXf::Identity(targets.size(), targets.size());
+
+ Eigen::VectorXf nullspaceTorque = knull.cwiseProduct(desiredNullSpaceJointValues - qpos) - dnull.cwiseProduct(qvel);
+ Eigen::MatrixXf jtpinv = ik->computePseudoInverseJacobianMatrix(jacobi.transpose(), 2.0);
+ Eigen::VectorXf jointDesiredTorques = jacobi.transpose() * jointControlWrench + (I - jacobi.transpose() * jtpinv) * nullspaceTorque;
+
+ // torque limit
+ ARMARX_CHECK_EXPRESSION(!targets.empty());
+ ARMARX_CHECK_LESS(targets.size(), 1000);
+ for (size_t i = 0; i < targets.size(); ++i)
+ {
+ float desiredTorque = jointDesiredTorques(i);
+
+ if (isnan(desiredTorque))
+ {
+ desiredTorque = 0;
+ }
+
+ desiredTorque = (desiredTorque > torqueLimit) ? torqueLimit : desiredTorque;
+ desiredTorque = (desiredTorque < -torqueLimit) ? -torqueLimit : desiredTorque;
+
+ debugOutputData.getWriteBuffer().desired_torques[jointNames[i]] = jointDesiredTorques(i);
+ debugOutputData.getWriteBuffer().desired_nullspaceJoint[jointNames[i]] = desiredNullSpaceJointValues(i);
+
+ targets.at(i)->torque = desiredTorque;
+ if (!targets.at(i)->isValid())
+ {
+ ARMARX_INFO << deactivateSpam(1) << "Torque controller target is invalid - setting to zero! set value: " << targets.at(i)->torque;
+ targets.at(i)->torque = 0.0f;
+ }
+ }
+
+
+ debugOutputData.getWriteBuffer().forceDesired_x = jointControlWrench(0);
+ debugOutputData.getWriteBuffer().forceDesired_y = jointControlWrench(1);
+ debugOutputData.getWriteBuffer().forceDesired_z = jointControlWrench(2);
+ debugOutputData.getWriteBuffer().forceDesired_rx = jointControlWrench(3);
+ debugOutputData.getWriteBuffer().forceDesired_ry = jointControlWrench(4);
+ debugOutputData.getWriteBuffer().forceDesired_rz = jointControlWrench(5);
+
+ debugOutputData.getWriteBuffer().impedanceKp_x = kpos(0);
+ debugOutputData.getWriteBuffer().impedanceKp_y = kpos(1);
+ debugOutputData.getWriteBuffer().impedanceKp_z = kpos(2);
+ debugOutputData.getWriteBuffer().impedanceKp_rx = kori(0);
+ debugOutputData.getWriteBuffer().impedanceKp_ry = kori(1);
+ debugOutputData.getWriteBuffer().impedanceKp_rz = kori(2);
+
+ debugOutputData.getWriteBuffer().forceInRoot_x = filteredForce(0);
+ debugOutputData.getWriteBuffer().forceInRoot_y = filteredForce(1);
+ debugOutputData.getWriteBuffer().forceInRoot_z = filteredForce(2);
+ // debugOutputData.getWriteBuffer().torqueInRoot_x = filteredForce(3);
+ // debugOutputData.getWriteBuffer().torqueInRoot_y = filteredForce(4);
+ // debugOutputData.getWriteBuffer().torqueInRoot_z = filteredForce(5);
+
+ debugOutputData.getWriteBuffer().vel_x = currentTwist(0);
+ debugOutputData.getWriteBuffer().vel_y = currentTwist(1);
+ debugOutputData.getWriteBuffer().vel_z = currentTwist(2);
+
+ // debugOutputData.getWriteBuffer().currentCanVal = rtGetControlStruct().canVal;
+ // debugOutputData.getWriteBuffer().mpcfactor = rtGetControlStruct().mpcFactor;
+
+ debugOutputData.getWriteBuffer().targetPose_x = targetPose(0, 3);
+ debugOutputData.getWriteBuffer().targetPose_y = targetPose(1, 3);
+ debugOutputData.getWriteBuffer().targetPose_z = targetPose(2, 3);
+ VirtualRobot::MathTools::Quaternion targetQuat = VirtualRobot::MathTools::eigen4f2quat(targetPose);
+ debugOutputData.getWriteBuffer().targetPose_qw = targetQuat.w;
+ debugOutputData.getWriteBuffer().targetPose_qx = targetQuat.x;
+ debugOutputData.getWriteBuffer().targetPose_qy = targetQuat.y;
+ debugOutputData.getWriteBuffer().targetPose_qz = targetQuat.z;
+
+ debugOutputData.getWriteBuffer().currentPose_x = currentPose(0, 3);
+ debugOutputData.getWriteBuffer().currentPose_y = currentPose(1, 3);
+ debugOutputData.getWriteBuffer().currentPose_z = currentPose(2, 3);
+ VirtualRobot::MathTools::Quaternion currentQuat = VirtualRobot::MathTools::eigen4f2quat(currentPose);
+ debugOutputData.getWriteBuffer().currentPose_qw = currentQuat.w;
+ debugOutputData.getWriteBuffer().currentPose_qx = currentQuat.x;
+ debugOutputData.getWriteBuffer().currentPose_qy = currentQuat.y;
+ debugOutputData.getWriteBuffer().currentPose_qz = currentQuat.z;
+ debugOutputData.getWriteBuffer().deltaT = deltaT;
+
+ debugOutputData.commitWrite();
+
+ }
+
+
+ void NJointTaskSpaceAdaptiveDMPController::learnDMPFromFiles(const Ice::StringSeq& fileNames, const Ice::Current&)
+ {
+ dmpCtrl->learnDMPFromFiles(fileNames);
+ ARMARX_INFO << "Learned DMP ... ";
+ }
+
+ void NJointTaskSpaceAdaptiveDMPController::setViaPoints(Ice::Double u, const Ice::DoubleSeq& viapoint, const Ice::Current&)
+ {
+ LockGuardType guard(controllerMutex);
+ ARMARX_INFO << "setting via points ";
+ dmpCtrl->setViaPose(u, viapoint);
+
+ }
+
+ void NJointTaskSpaceAdaptiveDMPController::setGoals(const Ice::DoubleSeq& goals, const Ice::Current& ice)
+ {
+ dmpCtrl->setGoalPoseVec(goals);
+
+ }
+
+ void NJointTaskSpaceAdaptiveDMPController::setCanVal(double canVal, const Ice::Current&)
+ {
+ LockGuardType guard(int2ctrlMutex);
+ interface2CtrlBuffer.getWriteBuffer().canVal = canVal;
+ interface2CtrlBuffer.commitWrite();
+ }
+
+
+ void NJointTaskSpaceAdaptiveDMPController::learnJointDMPFromFiles(const std::string& fileName, const Ice::FloatSeq& currentJVS, const Ice::Current&)
+ {
+ DMP::Vec<DMP::SampledTrajectoryV2 > trajs;
+ DMP::DVec ratios;
+ DMP::SampledTrajectoryV2 traj;
+ traj.readFromCSVFile(fileName);
+ traj = DMP::SampledTrajectoryV2::normalizeTimestamps(traj, 0, 1);
+ if (traj.dim() != jointNames.size())
+ {
+ isNullSpaceJointDMPLearned = false;
+ return;
+ }
+
+ DMP::DVec goal;
+ goal.resize(traj.dim());
+ currentJointState.resize(traj.dim());
+
+ for (size_t i = 0; i < goal.size(); ++i)
+ {
+ goal.at(i) = traj.rbegin()->getPosition(i);
+ currentJointState.at(i).pos = currentJVS.at(i);
+ currentJointState.at(i).vel = 0;
+ }
+
+ trajs.push_back(traj);
+ nullSpaceJointDMPPtr->learnFromTrajectories(trajs);
+
+ // prepare exeuction of joint dmp
+ nullSpaceJointDMPPtr->prepareExecution(goal, currentJointState, 1.0, 1.0);
+ ARMARX_INFO << "prepared nullspace joint dmp";
+ isNullSpaceJointDMPLearned = true;
+ }
+
+
+ void NJointTaskSpaceAdaptiveDMPController::resetDMP(const Ice::Current&)
+ {
+ if (started)
+ {
+ ARMARX_INFO << "Cannot reset running DMP";
+ }
+ firstRun = true;
+ }
+
+ void NJointTaskSpaceAdaptiveDMPController::setKdImpedance(const Ice::FloatSeq& value, const Ice::Current&)
+ {
+ Eigen::VectorXf setpoint;
+ setpoint.setZero(value.size());
+ ARMARX_CHECK_EQUAL(value.size(), 6);
+
+ for (size_t i = 0; i < value.size(); ++i)
+ {
+ setpoint(i) = value.at(i);
+ }
+
+ LockGuardType guard {interfaceDataMutex};
+ interface2rtBuffer.getWriteBuffer().KdImpedance = setpoint;
+ interface2rtBuffer.commitWrite();
+ }
+
+ void NJointTaskSpaceAdaptiveDMPController::setKpImpedance(const Ice::FloatSeq& value, const Ice::Current&)
+ {
+ Eigen::VectorXf setpoint;
+ setpoint.setZero(value.size());
+ ARMARX_CHECK_EQUAL(value.size(), 6);
+
+ for (size_t i = 0; i < value.size(); ++i)
+ {
+ setpoint(i) = value.at(i);
+ }
+
+ LockGuardType guard {interfaceDataMutex};
+ interface2rtBuffer.getWriteBuffer().KpImpedance = setpoint;
+ interface2rtBuffer.commitWrite();
+
+ }
+
+ void NJointTaskSpaceAdaptiveDMPController::setKpNull(const Ice::FloatSeq& value, const Ice::Current&)
+ {
+ Eigen::VectorXf setpoint;
+ setpoint.setZero(value.size());
+ ARMARX_CHECK_EQUAL(value.size(), rns->getSize());
+
+ for (size_t i = 0; i < value.size(); ++i)
+ {
+ setpoint(i) = value.at(i);
+ }
+
+ LockGuardType guard {interfaceDataMutex};
+ interface2rtBuffer.getWriteBuffer().Knull = setpoint;
+ interface2rtBuffer.commitWrite();
+
+ }
+
+ void NJointTaskSpaceAdaptiveDMPController::setKdNull(const Ice::FloatSeq& value, const Ice::Current&)
+ {
+ Eigen::VectorXf setpoint;
+ setpoint.setZero(value.size());
+ ARMARX_CHECK_EQUAL(value.size(), rns->getSize());
+
+ for (size_t i = 0; i < value.size(); ++i)
+ {
+ setpoint(i) = value.at(i);
+ }
+
+ LockGuardType guard {interfaceDataMutex};
+ interface2rtBuffer.getWriteBuffer().Dnull = setpoint;
+ interface2rtBuffer.commitWrite();
+
+ }
+
+ Ice::FloatSeq NJointTaskSpaceAdaptiveDMPController::getForce(const Ice::Current&)
+ {
+ Eigen::Vector3f force = interfaceData.getUpToDateReadBuffer().currentForce;
+ std::vector<float> forceVec = {force(0), force(1), force(2)};
+ return forceVec;
+ }
+
+ Ice::FloatSeq NJointTaskSpaceAdaptiveDMPController::getVelocityInMM(const Ice::Current&)
+ {
+ Eigen::VectorXf currentVel = interfaceData.getUpToDateReadBuffer().currentVel;
+ std::vector<float> tvelvec = {currentVel(0), currentVel(1), currentVel(2), currentVel(3), currentVel(4), currentVel(5)};
+ return tvelvec;
+ }
+
+ void NJointTaskSpaceAdaptiveDMPController::stopDMP(const Ice::Current&)
+ {
+ oldPose = interfaceData.getUpToDateReadBuffer().currentTcpPose;
+ stopped = true;
+ }
+
+ void NJointTaskSpaceAdaptiveDMPController::resumeDMP(const Ice::Current&)
+ {
+ stopped = false;
+ }
+
+
+ void NJointTaskSpaceAdaptiveDMPController::runDMPWithTime(const Ice::DoubleSeq& goals, Ice::Double timeDuration, const Ice::Current&)
+ {
+ firstRun = true;
+ while (firstRun)
+ {
+ usleep(100);
+ }
+
+ Eigen::Matrix4f pose = interfaceData.getUpToDateReadBuffer().currentTcpPose;
+ dmpCtrl->prepareExecution(dmpCtrl->eigen4f2vec(pose), goals);
+
+ dmpCtrl->canVal = timeDuration;
+ dmpCtrl->config.motionTimeDuration = timeDuration;
+
+ finished = false;
+
+ if (isNullSpaceJointDMPLearned && useNullSpaceJointDMP)
+ {
+ ARMARX_INFO << "Using Null Space Joint DMP";
+ }
+
+ started = true;
+ stopped = false;
+ // controllerTask->start();
+ }
+
+ void NJointTaskSpaceAdaptiveDMPController::runDMP(const Ice::DoubleSeq& goals, const Ice::Current&)
+ {
+ firstRun = true;
+ while (firstRun)
+ {
+ usleep(100);
+ }
+
+ Eigen::Matrix4f pose = interfaceData.getUpToDateReadBuffer().currentTcpPose;
+ dmpCtrl->prepareExecution(dmpCtrl->eigen4f2vec(pose), goals);
+
+ finished = false;
+
+ if (isNullSpaceJointDMPLearned && useNullSpaceJointDMP)
+ {
+ ARMARX_INFO << "Using Null Space Joint DMP";
+ }
+
+ started = true;
+ stopped = false;
+ // controllerTask->start();
+ }
+
+
+
+ void NJointTaskSpaceAdaptiveDMPController::onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx& debugObs)
+ {
+ StringVariantBaseMap datafields;
+ auto values = debugOutputData.getUpToDateReadBuffer().desired_torques;
+ for (auto& pair : values)
+ {
+ datafields["torqueDesired_" + pair.first] = new Variant(pair.second);
+ }
+
+ auto values_null = debugOutputData.getUpToDateReadBuffer().desired_nullspaceJoint;
+ for (auto& pair : values_null)
+ {
+ datafields["nullspaceDesired_" + pair.first] = new Variant(pair.second);
+ }
+
+ datafields["canVal"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentCanVal);
+ datafields["mpcfactor"] = new Variant(debugOutputData.getUpToDateReadBuffer().mpcfactor);
+ datafields["targetPose_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().targetPose_x);
+ datafields["targetPose_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().targetPose_y);
+ datafields["targetPose_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().targetPose_z);
+ datafields["targetPose_qw"] = new Variant(debugOutputData.getUpToDateReadBuffer().targetPose_qw);
+ datafields["targetPose_qx"] = new Variant(debugOutputData.getUpToDateReadBuffer().targetPose_qx);
+ datafields["targetPose_qy"] = new Variant(debugOutputData.getUpToDateReadBuffer().targetPose_qy);
+ datafields["targetPose_qz"] = new Variant(debugOutputData.getUpToDateReadBuffer().targetPose_qz);
+
+ datafields["impedanceKp_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().impedanceKp_x);
+ datafields["impedanceKp_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().impedanceKp_y);
+ datafields["impedanceKp_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().impedanceKp_z);
+ datafields["impedanceKp_rx"] = new Variant(debugOutputData.getUpToDateReadBuffer().impedanceKp_rx);
+ datafields["impedanceKp_ry"] = new Variant(debugOutputData.getUpToDateReadBuffer().impedanceKp_ry);
+ datafields["impedanceKp_rz"] = new Variant(debugOutputData.getUpToDateReadBuffer().impedanceKp_rz);
+
+ datafields["currentPose_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentPose_x);
+ datafields["currentPose_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentPose_y);
+ datafields["currentPose_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentPose_z);
+ datafields["currentPose_qw"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentPose_qw);
+ datafields["currentPose_qx"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentPose_qx);
+ datafields["currentPose_qy"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentPose_qy);
+ datafields["currentPose_qz"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentPose_qz);
+
+ datafields["forceDesired_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().forceDesired_x);
+ datafields["forceDesired_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().forceDesired_y);
+ datafields["forceDesired_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().forceDesired_z);
+ datafields["forceDesired_rx"] = new Variant(debugOutputData.getUpToDateReadBuffer().forceDesired_rx);
+ datafields["forceDesired_ry"] = new Variant(debugOutputData.getUpToDateReadBuffer().forceDesired_ry);
+ datafields["forceDesired_rz"] = new Variant(debugOutputData.getUpToDateReadBuffer().forceDesired_rz);
+
+ datafields["forceInRoot_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().forceInRoot_x);
+ datafields["forceInRoot_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().forceInRoot_y);
+ datafields["forceInRoot_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().forceInRoot_z);
+
+ datafields["vel_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().vel_x);
+ datafields["vel_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().vel_y);
+ datafields["vel_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().vel_z);
+
+ datafields["deltaT"] = new Variant(debugOutputData.getUpToDateReadBuffer().deltaT);
+
+ std::string channelName = cfg->nodeSetName + "_TaskSpaceImpedanceControl";
+ debugObs->setDebugChannel(channelName, datafields);
+ }
+
+ void NJointTaskSpaceAdaptiveDMPController::onInitNJointController()
+ {
+ ARMARX_INFO << "init ...";
+ // controllerTask = new PeriodicTask<NJointTaskSpaceAdaptiveDMPController>(this, &NJointTaskSpaceAdaptiveDMPController::controllerRun, 1);
+ runTask("NJointTaskSpaceAdaptiveDMPController", [&]
+ {
+ CycleUtil c(1);
+ getObjectScheduler()->waitForObjectStateMinimum(eManagedIceObjectStarted);
+ while (getState() == eManagedIceObjectStarted)
+ {
+ if (isControllerActive())
+ {
+ controllerRun();
+ }
+ c.waitForCycleDuration();
+ }
+ });
+ }
+
+ void NJointTaskSpaceAdaptiveDMPController::onDisconnectNJointController()
+ {
+ // controllerTask->stop();
+ }
+
+
+
+}
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceAdaptiveDMPController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceAdaptiveDMPController.h
new file mode 100644
index 0000000000000000000000000000000000000000..d8d4dfd08096060b3d4ecaaf0e7d137f972c7de3
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceAdaptiveDMPController.h
@@ -0,0 +1,245 @@
+
+#pragma once
+
+#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h>
+#include <VirtualRobot/Robot.h>
+#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
+#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
+#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
+#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValueForceTorque.h>
+#include <VirtualRobot/IK/DifferentialIK.h>
+#include <RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.h>
+#include <RobotAPI/libraries/DMPController/TaskSpaceDMPController.h>
+#include <dmp/representation/dmp/umidmp.h>
+#include <ArmarXCore/core/time/CycleUtil.h>
+
+namespace armarx
+{
+ TYPEDEF_PTRS_HANDLE(NJointTaskSpaceAdaptiveDMPController);
+ TYPEDEF_PTRS_HANDLE(NJointTaskSpaceAdaptiveDMPControllerControlData);
+
+ class NJointTaskSpaceAdaptiveDMPControllerControlData
+ {
+ public:
+ Eigen::VectorXf targetVel;
+ Eigen::Matrix4f targetPose;
+ Eigen::VectorXf desiredNullSpaceJointValues;
+ double canVal;
+ double mpcFactor;
+ };
+
+
+
+ /**
+ * @brief The NJointTaskSpaceAdaptiveDMPController class
+ * @ingroup Library-RobotUnit-NJointControllers
+ */
+ class NJointTaskSpaceAdaptiveDMPController :
+ public NJointControllerWithTripleBuffer<NJointTaskSpaceAdaptiveDMPControllerControlData>,
+ public NJointTaskSpaceAdaptiveDMPControllerInterface
+ {
+ public:
+ using ConfigPtrT = NJointTaskSpaceAdaptiveDMPControllerConfigPtr;
+ NJointTaskSpaceAdaptiveDMPController(const RobotUnitPtr& robotUnit, const NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&);
+
+ // NJointControllerInterface interface
+ std::string getClassName(const Ice::Current&) const;
+
+ // NJointController interface
+
+ void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration);
+
+ // NJointTaskSpaceAdaptiveDMPControllerInterface interface
+ void learnDMPFromFiles(const Ice::StringSeq& fileNames, const Ice::Current&);
+ bool isFinished(const Ice::Current&)
+ {
+ return finished;
+ }
+
+ void setViaPoints(Ice::Double u, const Ice::DoubleSeq& viapoint, const Ice::Current&);
+ void setGoals(const Ice::DoubleSeq& goals, const Ice::Current&);
+
+ void learnJointDMPFromFiles(const std::string& fileName, const Ice::FloatSeq& currentJVS, const Ice::Current&);
+ void runDMP(const Ice::DoubleSeq& goals, const Ice::Current&);
+ void runDMPWithTime(const Ice::DoubleSeq& goals, Ice::Double timeDuration, const Ice::Current&);
+
+ Ice::Double getVirtualTime(const Ice::Current&)
+ {
+ return dmpCtrl->canVal;
+ }
+
+ void stopDMP(const Ice::Current&);
+ void resumeDMP(const Ice::Current&);
+ void resetDMP(const Ice::Current&);
+
+ void setKdImpedance(const Ice::FloatSeq& dampings, const Ice::Current&);
+ void setKpImpedance(const Ice::FloatSeq& stiffness, const Ice::Current&);
+
+ void setKdNull(const Ice::FloatSeq& dnull, const Ice::Current&);
+ void setKpNull(const Ice::FloatSeq& knull, const Ice::Current&);
+ Ice::FloatSeq getForce(const Ice::Current&);
+ Ice::FloatSeq getVelocityInMM(const Ice::Current&);
+ void setCanVal(double canVal, const Ice::Current&);
+
+ protected:
+ virtual void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&);
+
+ void onInitNJointController();
+ void onDisconnectNJointController();
+ void controllerRun();
+
+ private:
+ struct DebugBufferData
+ {
+ double currentCanVal;
+ double mpcfactor;
+ float targetPose_x;
+ float targetPose_y;
+ float targetPose_z;
+ float targetPose_qw;
+ float targetPose_qx;
+ float targetPose_qy;
+ float targetPose_qz;
+
+ float currentPose_x;
+ float currentPose_y;
+ float currentPose_z;
+ float currentPose_qw;
+ float currentPose_qx;
+ float currentPose_qy;
+ float currentPose_qz;
+
+ StringFloatDictionary desired_torques;
+ StringFloatDictionary desired_nullspaceJoint;
+ float forceDesired_x;
+ float forceDesired_y;
+ float forceDesired_z;
+ float forceDesired_rx;
+ float forceDesired_ry;
+ float forceDesired_rz;
+
+ float impedanceKp_x;
+ float impedanceKp_y;
+ float impedanceKp_z;
+ float impedanceKp_rx;
+ float impedanceKp_ry;
+ float impedanceKp_rz;
+
+ float forceInRoot_x;
+ float forceInRoot_y;
+ float forceInRoot_z;
+ // float torqueInRoot_x;
+ // float torqueInRoot_y;
+ // float torqueInRoot_z;
+
+ float vel_x;
+ float vel_y;
+ float vel_z;
+
+ float deltaT;
+
+ };
+
+ TripleBuffer<DebugBufferData> debugOutputData;
+
+ struct NJointTaskSpaceAdaptiveDMPControllerSensorData
+ {
+ double currentTime;
+ double deltaT;
+ Eigen::Matrix4f currentPose;
+ Eigen::VectorXf currentTwist;
+ };
+ TripleBuffer<NJointTaskSpaceAdaptiveDMPControllerSensorData> controllerSensorData;
+
+ struct NJointTaskSpaceAdaptiveDMPControllerInterfaceData
+ {
+ Eigen::Matrix4f currentTcpPose;
+ Eigen::VectorXf currentVel;
+ Eigen::Vector3f currentForce;
+
+ };
+
+ TripleBuffer<NJointTaskSpaceAdaptiveDMPControllerInterfaceData> interfaceData;
+
+
+ struct Inferface2rtData
+ {
+ Eigen::VectorXf KpImpedance;
+ Eigen::VectorXf KdImpedance;
+ Eigen::VectorXf Knull;
+ Eigen::VectorXf Dnull;
+ };
+ TripleBuffer<Inferface2rtData> interface2rtBuffer;
+
+ struct Interface2CtrlData
+ {
+ double canVal;
+ };
+ TripleBuffer<Interface2CtrlData> interface2CtrlBuffer;
+
+
+ DMP::Vec<DMP::DMPState> currentJointState;
+ DMP::UMIDMPPtr nullSpaceJointDMPPtr;
+
+ TaskSpaceDMPControllerPtr dmpCtrl;
+
+ std::vector<const SensorValue1DoFActuatorTorque*> torqueSensors;
+ std::vector<const SensorValue1DoFActuatorVelocity*> velocitySensors;
+ std::vector<const SensorValue1DoFActuatorPosition*> positionSensors;
+ std::vector<ControlTarget1DoFActuatorTorque*> targets;
+
+ // velocity ik controller parameters
+ // dmp parameters
+ double timeDuration;
+ bool finished;
+ VirtualRobot::RobotNodeSetPtr rns;
+
+ // phaseStop parameters
+ double phaseL;
+ double phaseK;
+ double phaseDist0;
+ double phaseDist1;
+ double posToOriRatio;
+
+
+ NJointTaskSpaceAdaptiveDMPControllerConfigPtr cfg;
+ VirtualRobot::DifferentialIKPtr ik;
+ VirtualRobot::RobotNodePtr tcp;
+
+ float torqueLimit;
+
+ Eigen::Vector3f kpos;
+ Eigen::Vector3f kori;
+ Eigen::Vector3f dpos;
+ Eigen::Vector3f dori;
+ Eigen::VectorXf knull;
+ Eigen::VectorXf dnull;
+ int numOfJoints;
+
+ bool useNullSpaceJointDMP;
+ bool isNullSpaceJointDMPLearned;
+
+
+ Eigen::VectorXf defaultNullSpaceJointValues;
+ std::vector<std::string> jointNames;
+ mutable MutexType controllerMutex;
+ PeriodicTask<NJointTaskSpaceAdaptiveDMPController>::pointer_type controllerTask;
+ bool firstRun;
+ bool started = false;
+ bool stopped = false;
+ Eigen::Vector3f forceOffset;
+
+ Eigen::Matrix4f oldPose;
+ const SensorValueForceTorque* forceSensor;
+ Eigen::Vector3f filteredForce;
+
+ mutable MutexType interfaceDataMutex;
+ mutable MutexType int2ctrlMutex;
+
+ // NJointController interface
+ protected:
+ void rtPreActivateController();
+ };
+
+} // namespace armarx
+
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp
index e52e8c26f863b165dca8605226747e66e68a7aee..0ed7f2136377c71399c71fec0310fbec12baba85 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp
@@ -553,6 +553,28 @@ namespace armarx
// controllerTask->stop();
}
+ void NJointTaskSpaceImpedanceDMPController::setMPWeights(const DoubleSeqSeq& weights, const Ice::Current&)
+ {
+ dmpCtrl->setWeights(weights);
+ }
+
+ DoubleSeqSeq NJointTaskSpaceImpedanceDMPController::getMPWeights(const Ice::Current&)
+ {
+ DMP::DVec2d res = dmpCtrl->getWeights();
+ DoubleSeqSeq resvec;
+ for (size_t i = 0; i < res.size(); ++i)
+ {
+ std::vector<double> cvec;
+ for (size_t j = 0; j < res[i].size(); ++j)
+ {
+ cvec.push_back(res[i][j]);
+ }
+ resvec.push_back(cvec);
+ }
+
+ return resvec;
+ }
+
}
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.h
index 25f6792ca5b9eb84fcf1853dff4890850d66cd9e..412a46f20b5ce93eddfed0d68854d4a3c03bc30a 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.h
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.h
@@ -70,6 +70,10 @@ namespace armarx
void stopDMP(const Ice::Current&);
void resumeDMP(const Ice::Current&);
void resetDMP(const Ice::Current&);
+
+ void setMPWeights(const DoubleSeqSeq& weights, const Ice::Current&);
+ DoubleSeqSeq getMPWeights(const Ice::Current&);
+
protected:
virtual void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&);
diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.cpp b/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.cpp
index a9ecec5e4f8965566355d6ef98f60bea206ee42e..349eca66f847b5d3000b6418241f2fd304b3f304 100644
--- a/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.cpp
+++ b/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.cpp
@@ -25,6 +25,8 @@
#include <ArmarXCore/core/util/StringHelpers.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/system/ArmarXDataPath.h>
+#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
#include <ArmarXCore/statechart/xmlstates/profiles/StatechartProfiles.h>
#include <ArmarXCore/util/CPPUtility/trace.h>
@@ -32,7 +34,6 @@
namespace armarx
{
-
void RobotNameHelper::writeRobotInfoNode(
const RobotInfoNodePtr& n,
std::ostream& str,
@@ -225,12 +226,26 @@ namespace armarx
return select("HandModelPath");
}
+ std::string RobotNameHelper::Arm::getAbsoluteHandModelPath() const
+ {
+ ArmarXDataPath::FindPackageAndAddDataPath(getHandModelPackage());
+ auto path = getHandModelPath();
+ return ArmarXDataPath::getAbsolutePath(path, path) ?
+ path : "";
+ }
+
std::string RobotNameHelper::Arm::getHandModelPackage() const
{
ARMARX_TRACE;
return select("HandModelPackage");
}
+ std::string RobotNameHelper::Arm::getPalmCollisionModel() const
+ {
+ ARMARX_TRACE;
+ return select("PalmCollisionModel");
+ }
+
RobotNameHelper::RobotArm RobotNameHelper::Arm::addRobot(const VirtualRobot::RobotPtr& robot) const
{
ARMARX_TRACE;
@@ -258,24 +273,35 @@ namespace armarx
VirtualRobot::RobotNodeSetPtr RobotNameHelper::RobotArm::getKinematicChain() const
{
ARMARX_TRACE;
+ ARMARX_CHECK_NOT_NULL(robot);
return robot->getRobotNodeSet(arm.getKinematicChain());
}
VirtualRobot::RobotNodeSetPtr RobotNameHelper::RobotArm::getTorsoKinematicChain() const
{
ARMARX_TRACE;
+ ARMARX_CHECK_NOT_NULL(robot);
return robot->getRobotNodeSet(arm.getTorsoKinematicChain());
}
VirtualRobot::RobotNodePtr RobotNameHelper::RobotArm::getTCP() const
{
ARMARX_TRACE;
+ ARMARX_CHECK_NOT_NULL(robot);
return robot->getRobotNode(arm.getTCP());
}
+ VirtualRobot::RobotNodePtr RobotNameHelper::RobotArm::getPalmCollisionModel() const
+ {
+ ARMARX_TRACE;
+ ARMARX_CHECK_NOT_NULL(robot);
+ return robot->getRobotNode(arm.getPalmCollisionModel());
+ }
+
VirtualRobot::RobotNodePtr RobotNameHelper::RobotArm::getHandRootNode() const
{
ARMARX_TRACE;
+ ARMARX_CHECK_NOT_NULL(robot);
return robot->getRobotNode(arm.getHandRootNode());
}
@@ -289,7 +315,12 @@ namespace armarx
return tcp->getPoseInRootFrame().inverse() * hand->getPoseInRootFrame();
}
- RobotNameHelper::Arm RobotNameHelper::RobotArm::getArm() const
+ const VirtualRobot::RobotPtr& RobotNameHelper::RobotArm::getRobot() const
+ {
+ return robot;
+ }
+
+ const RobotNameHelper::Arm& RobotNameHelper::RobotArm::getArm() const
{
return arm;
}
@@ -297,7 +328,7 @@ namespace armarx
RobotNameHelper::RobotArm::RobotArm(const Arm& arm, const VirtualRobot::RobotPtr& robot)
: arm(arm), robot(robot)
{
-
+ ARMARX_CHECK_NOT_NULL(robot);
}
const std::vector<std::string>& RobotNameHelper::getProfiles() const
diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.h b/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.h
index 703c0ae83c286d4c3273d19e1dadd11044e1a71b..b4db8b858c70273d24e1f4e6b615037aa49c2faf 100644
--- a/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.h
+++ b/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.h
@@ -74,7 +74,9 @@ namespace armarx
std::string getHandControllerName() const;
std::string getHandRootNode() const;
std::string getHandModelPath() const;
+ std::string getAbsoluteHandModelPath() const;
std::string getHandModelPackage() const;
+ std::string getPalmCollisionModel() const;
RobotArm addRobot(const VirtualRobot::RobotPtr& robot) const;
Arm(const std::shared_ptr<const RobotNameHelper>& rnh, const std::string& side);
@@ -103,8 +105,10 @@ namespace armarx
VirtualRobot::RobotNodeSetPtr getTorsoKinematicChain() const;
VirtualRobot::RobotNodePtr getTCP() const;
VirtualRobot::RobotNodePtr getHandRootNode() const;
+ VirtualRobot::RobotNodePtr getPalmCollisionModel() const;
Eigen::Matrix4f getTcp2HandRootTransform() const;
- Arm getArm() const;
+ const Arm& getArm() const;
+ const VirtualRobot::RobotPtr& getRobot() const;
RobotArm(const Arm& arm, const VirtualRobot::RobotPtr& robot);
RobotArm() = default;
@@ -114,7 +118,7 @@ namespace armarx
RobotArm& operator=(const RobotArm&) = default;
private:
- const Arm arm;
+ Arm arm;
VirtualRobot::RobotPtr robot;
};
@@ -123,6 +127,10 @@ namespace armarx
static RobotNameHelperPtr Create(const RobotInfoNodePtr& robotInfo, const StatechartProfilePtr& profile);
RobotNameHelper(const RobotInfoNodePtr& robotInfo, const StatechartProfilePtr& profile = nullptr);
+ RobotNameHelper(RobotNameHelper&&) = default;
+ RobotNameHelper(const RobotNameHelper&) = default;
+ RobotNameHelper& operator=(RobotNameHelper&&) = default;
+ RobotNameHelper& operator=(const RobotNameHelper&) = default;
Arm getArm(const std::string& side) const;
RobotArm getRobotArm(const std::string& side, const VirtualRobot::RobotPtr& robot) const;
diff --git a/source/RobotAPI/libraries/RobotUnitDataStreamingReceiver/CMakeLists.txt b/source/RobotAPI/libraries/RobotUnitDataStreamingReceiver/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..8457edd93471fb5d96a8ec99b0e27f103d17a367
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotUnitDataStreamingReceiver/CMakeLists.txt
@@ -0,0 +1,17 @@
+set(LIB_NAME RobotUnitDataStreamingReceiver)
+
+armarx_component_set_name("${LIB_NAME}")
+armarx_set_target("Library: ${LIB_NAME}")
+
+set(LIBS
+ RobotAPIInterfaces
+ RobotAPICore
+)
+
+set(LIB_FILES RobotUnitDataStreamingReceiver.cpp)
+set(LIB_HEADERS RobotUnitDataStreamingReceiver.h )
+
+armarx_add_library("${LIB_NAME}" "${LIB_FILES}" "${LIB_HEADERS}" "${LIBS}")
+
+# add unit tests
+add_subdirectory(test)
diff --git a/source/RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.cpp b/source/RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..1021205caf6ade46e5d5330df8f5591591b01430
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.cpp
@@ -0,0 +1,127 @@
+/*
+ * 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::ArmarXObjects::RobotUnitDataStreamingReceiver
+ * @author Raphael Grimm ( raphael dot grimm at kit dot edu )
+ * @date 2020
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include <Ice/ObjectAdapter.h>
+
+#include <ArmarXCore/core/ArmarXManager.h>
+
+#include "RobotUnitDataStreamingReceiver.h"
+
+namespace armarx::detail::RobotUnitDataStreamingReceiver
+{
+ class Receiver :
+ virtual public ManagedIceObject,
+ virtual public RobotUnitDataStreaming::Receiver
+ {
+ public:
+ std::string getDefaultName() const override
+ {
+ return "RobotUnitDataStreamingReceiver";
+ }
+ void onInitComponent() override {}
+ void onConnectComponent() override {}
+ void onExitComponent() override {}
+
+ void update(const RobotUnitDataStreaming::TimeStepSeq& data, const Ice::Current&) override
+ {
+ std::lock_guard{_data_mutex};
+ ARMARX_INFO << deactivateSpam()
+ << "received " << data.size() << " timesteps";
+ _data.emplace_back(data);
+ }
+
+ std::mutex _data_mutex;
+ std::deque<RobotUnitDataStreaming::TimeStepSeq> _data;
+ Ice::Identity _identity;
+ };
+}
+
+namespace armarx
+{
+ RobotUnitDataStreamingReceiver::RobotUnitDataStreamingReceiver(
+ const ManagedIceObjectPtr& obj,
+ const RobotUnitInterfacePrx& ru,
+ const RobotUnitDataStreaming::Config& cfg
+ ) : _obj{obj}, _ru{ru}
+ {
+ ARMARX_CHECK_NOT_NULL(_obj);
+ ARMARX_CHECK_NOT_NULL(_ru);
+ _receiver = make_shared<detail::RobotUnitDataStreamingReceiver::Receiver>();
+
+ _receiver->_identity.name =
+ _obj->getName() + "_RobotUnitDataStreamingReceiver_" +
+ std::to_string(clock_t::now().time_since_epoch().count());
+
+ auto adapter = _obj->getArmarXManager()->getAdapter();
+ _proxy = RobotUnitDataStreaming::ReceiverPrx::uncheckedCast(
+ adapter->add(_receiver, _receiver->_identity));
+
+ _description = _ru->startDataStreaming(_proxy, cfg);
+ }
+
+ RobotUnitDataStreamingReceiver::~RobotUnitDataStreamingReceiver()
+ {
+ if (_proxy)
+ {
+ if (!_description.entries.empty())
+ {
+ _ru->stopDataStreaming(_proxy);
+ }
+ auto adapter = _obj->getArmarXManager()->getAdapter();
+ adapter->remove(_receiver->_identity);
+ }
+ _proxy = nullptr;
+ _receiver = nullptr;
+ }
+
+ std::deque<RobotUnitDataStreaming::TimeStep>& RobotUnitDataStreamingReceiver::getDataBuffer()
+ {
+ ARMARX_CHECK_NOT_NULL(_receiver);
+ std::deque<RobotUnitDataStreaming::TimeStepSeq> data;
+ {
+ std::lock_guard{_receiver->_data_mutex};
+ std::swap(data, _receiver->_data);
+ }
+ for (auto& chunk : data)
+ {
+ for (auto& step : chunk)
+ {
+ if (
+ _last_iteration_id != -1 &&
+ _last_iteration_id + 1 != step.iterationId
+ )
+ {
+ ARMARX_ERROR << "Missing Iterations or iterations out of order!"
+ " This should not happen";
+ }
+ _last_iteration_id = step.iterationId;
+ _data_buffer.emplace_back(std::move(step));
+ }
+ }
+ return _data_buffer;
+ }
+
+ const RobotUnitDataStreaming::DataStreamingDescription& RobotUnitDataStreamingReceiver::getDataDescription() const
+ {
+ return _description;
+ }
+}
diff --git a/source/RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.h b/source/RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.h
new file mode 100644
index 0000000000000000000000000000000000000000..5f577c0119c1d100227ed4b6f3fb3e2696bccf66
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.h
@@ -0,0 +1,72 @@
+/*
+ * 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::ArmarXObjects::RobotUnitDataStreamingReceiver
+ * @author Raphael Grimm ( raphael dot grimm at kit dot edu )
+ * @date 2020
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <mutex>
+
+#include <ArmarXCore/core/ManagedIceObject.h>
+
+#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
+
+namespace armarx::detail::RobotUnitDataStreamingReceiver
+{
+ TYPEDEF_PTRS_HANDLE(Receiver);
+}
+namespace armarx
+{
+ TYPEDEF_PTRS_SHARED(RobotUnitDataStreamingReceiver);
+ /**
+ * @defgroup Library-RobotUnitDataStreamingReceiver RobotUnitDataStreamingReceiver
+ * @ingroup RobotAPI
+ * A description of the library RobotUnitDataStreamingReceiver.
+ *
+ * @class RobotUnitDataStreamingReceiver
+ * @ingroup Library-RobotUnitDataStreamingReceiver
+ * @brief Brief description of class RobotUnitDataStreamingReceiver.
+ *
+ * Detailed description of class RobotUnitDataStreamingReceiver.
+ */
+ class RobotUnitDataStreamingReceiver
+ {
+ public:
+ using clock_t = std::chrono::high_resolution_clock;
+
+ RobotUnitDataStreamingReceiver(
+ const ManagedIceObjectPtr& obj,
+ const RobotUnitInterfacePrx& ru,
+ const RobotUnitDataStreaming::Config& cfg);
+ ~RobotUnitDataStreamingReceiver();
+
+ std::deque<RobotUnitDataStreaming::TimeStep>& getDataBuffer();
+ const RobotUnitDataStreaming::DataStreamingDescription& getDataDescription() const;
+
+ private:
+ ManagedIceObjectPtr _obj;
+ RobotUnitInterfacePrx _ru;
+ detail::RobotUnitDataStreamingReceiver::ReceiverPtr _receiver;
+ RobotUnitDataStreaming::ReceiverPrx _proxy;
+ RobotUnitDataStreaming::DataStreamingDescription _description;
+ std::deque<RobotUnitDataStreaming::TimeStep> _data_buffer;
+ long _last_iteration_id = -1;
+ };
+}
diff --git a/source/RobotAPI/libraries/RobotUnitDataStreamingReceiver/test/CMakeLists.txt b/source/RobotAPI/libraries/RobotUnitDataStreamingReceiver/test/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..b60f2c1a3675c36e1d30a36fe47e4256e4c33845
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotUnitDataStreamingReceiver/test/CMakeLists.txt
@@ -0,0 +1,5 @@
+
+# Libs required for the tests
+SET(LIBS ${LIBS} ArmarXCore RobotUnitDataStreamingReceiver)
+
+armarx_add_test(RobotUnitDataStreamingReceiverTest RobotUnitDataStreamingReceiverTest.cpp "${LIBS}")
\ No newline at end of file
diff --git a/source/RobotAPI/applications/MultiHandUnit/main.cpp b/source/RobotAPI/libraries/RobotUnitDataStreamingReceiver/test/RobotUnitDataStreamingReceiverTest.cpp
similarity index 66%
rename from source/RobotAPI/applications/MultiHandUnit/main.cpp
rename to source/RobotAPI/libraries/RobotUnitDataStreamingReceiver/test/RobotUnitDataStreamingReceiverTest.cpp
index b284606d543efcd6326d2bfd03e645c5b8e61e8b..08ac432e58223900d4f06545c71130570bd89d65 100644
--- a/source/RobotAPI/applications/MultiHandUnit/main.cpp
+++ b/source/RobotAPI/libraries/RobotUnitDataStreamingReceiver/test/RobotUnitDataStreamingReceiverTest.cpp
@@ -13,20 +13,24 @@
* 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::MultiHandUnit
+ * @package RobotAPI::ArmarXObjects::RobotUnitDataStreamingReceiver
* @author Raphael Grimm ( raphael dot grimm at kit dot edu )
- * @date 2019
+ * @date 2020
* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
* GNU General Public License
*/
-#include <RobotAPI/components/MultiHandUnit/MultiHandUnit.h>
+#define BOOST_TEST_MODULE RobotAPI::ArmarXLibraries::RobotUnitDataStreamingReceiver
-#include <ArmarXCore/core/application/Application.h>
-#include <ArmarXCore/core/Component.h>
-#include <ArmarXCore/core/logging/Logging.h>
+#define ARMARX_BOOST_TEST
-int main(int argc, char* argv[])
+#include <RobotAPI/Test.h>
+#include "../RobotUnitDataStreamingReceiver.h"
+
+#include <iostream>
+
+BOOST_AUTO_TEST_CASE(testExample)
{
- return armarx::runSimpleComponentApp < armarx::MultiHandUnit > (argc, argv, "MultiHandUnit");
+
+ BOOST_CHECK_EQUAL(true, true);
}
diff --git a/source/RobotAPI/libraries/armem/ArMemBase.cpp b/source/RobotAPI/libraries/armem/ArMemBase.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..5048a42d4c4a424ff509e59b6a1dc3a1ddf63494
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/ArMemBase.cpp
@@ -0,0 +1,54 @@
+/*
+ * 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::ArmarXObjects::armem
+ * @author Simon Ottenhaus ( simon dot ottenhaus at kit dot edu )
+ * @date 2020
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+// Header
+#include "ArMemBase.h"
+
+
+namespace armarx
+{
+ namespace armem
+ {
+ const std::string ArMemBase::DEFAULT_LOCAL_MEMORY_NAME_PREFIX = "ArMemLocalStorage";
+
+ ArMemBase::ArMemBase()
+ {
+ my_hostname = getMyHostname();
+ }
+
+ std::string ArMemBase::GenerateLocalMemoryObjectNameFromHostname(const std::string& hostname)
+ {
+ return DEFAULT_LOCAL_MEMORY_NAME_PREFIX + "__" + hostname;
+ }
+
+ std::string ArMemBase::getMyHostname() const
+ {
+ return my_hostname;
+ }
+
+ std::string ArMemBase::GetMyHostname()
+ {
+ boost::asio::io_service io_service;
+ return boost::asio::ip::host_name();
+ }
+ }
+}
diff --git a/source/RobotAPI/applications/GamepadControlUnit/main.cpp b/source/RobotAPI/libraries/armem/ArMemBase.h
similarity index 53%
rename from source/RobotAPI/applications/GamepadControlUnit/main.cpp
rename to source/RobotAPI/libraries/armem/ArMemBase.h
index a2c5a7e4296a36c3012bbd7bb720120ef3e72c3d..851b2b869aa70a9271d3bc54021582d77977af57 100644
--- a/source/RobotAPI/applications/GamepadControlUnit/main.cpp
+++ b/source/RobotAPI/libraries/armem/ArMemBase.h
@@ -13,20 +13,45 @@
* 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::GamepadControlUnit
+ * @package RobotAPI::ArmarXObjects::armem
* @author Simon Ottenhaus ( simon dot ottenhaus at kit dot edu )
- * @date 2017
+ * @date 2020
* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
* GNU General Public License
*/
-#include <RobotAPI/components/GamepadControlUnit/GamepadControlUnit.h>
+#pragma once
-#include <ArmarXCore/core/application/Application.h>
-#include <ArmarXCore/core/Component.h>
-#include <ArmarXCore/core/logging/Logging.h>
+// STD/STL
+#include <string>
-int main(int argc, char* argv[])
+// Boost
+#include <boost/asio.hpp>
+
+
+namespace armarx
{
- return armarx::runSimpleComponentApp < armarx::GamepadControlUnit > (argc, argv, "GamepadControlUnit");
+ namespace armem
+ {
+
+ class ArMemBase;
+ typedef std::shared_ptr<ArMemBase> ArMemBasePtr;
+
+ class ArMemBase
+ {
+
+ public:
+ ArMemBase();
+
+ std::string getMyHostname() const;
+ static std::string GetMyHostname();
+
+ static std::string GenerateLocalMemoryObjectNameFromHostname(const std::string&);
+
+ private:
+ static const std::string DEFAULT_LOCAL_MEMORY_NAME_PREFIX;
+ std::string my_hostname;
+ };
+
+ }
}
diff --git a/source/RobotAPI/libraries/armem/ArMemWorkingMemoryBase.cpp b/source/RobotAPI/libraries/armem/ArMemWorkingMemoryBase.cpp
deleted file mode 100644
index 15ea5dcac2ddbcefc059805a45ca969b393c97c3..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/ArMemWorkingMemoryBase.cpp
+++ /dev/null
@@ -1,84 +0,0 @@
-/*
- * 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::ArmarXObjects::armem
- * @author Simon Ottenhaus ( simon dot ottenhaus at kit dot edu )
- * @date 2020
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-// Header
-#include "ArMemWorkingMemoryBase.h"
-
-// STD/STL
-#include <vector>
-#include <map>
-#include <memory>
-#include <unordered_map>
-
-// ArmarX
-#include <ArmarXCore/core/logging/Logging.h>
-
-namespace armarx
-{
- namespace armem
- {
-
- ArMemWorkingMemoryBase::ArMemWorkingMemoryBase() :
- maximumEntriesPerDatatype(-1),
- maximumEntriesPerProducer(-1)
- {
-
- }
-
- void ArMemWorkingMemoryBase::store(const std::string& producer, const std::string& datatype, long sentAt, const aron::data::AronDataNavigatorPtr& data)
- {
- if (storage.find(producer) == storage.end())
- {
- // Need to allocate a new storage for a new producer
- ARMARX_WARNING_S << "A new producer '" + producer + "' registered to the memory. Allocating space for its members...";
- storage[producer] = ProducerDatatypeStorage();
- }
-
- ProducerDatatypeStorage& producer_storage = storage[producer];
- if (producer_storage.find(datatype) == producer_storage.end())
- {
- // Need to allocate a new storage for a new producer
- ARMARX_WARNING_S << "A new datatype '" + datatype + "' registered to the memory to producer '" + producer + "'. Allocating space for its members...";
- producer_storage[datatype] = SingleDatatypeStorage();
- }
-
- SingleDatatypeStorage& datatype_storage = producer_storage[datatype];
-
- long now = IceUtil::Time::now().toMilliSeconds();
- if (sentAt > now)
- {
- ARMARX_WARNING_S << "Received an invalid timestamp. The timestamp when sending data of type '" + datatype + "' from the producer '" + producer + "' is greater than the storage timestamp (The transmitted time needs to be in milliseconds [ms]). Please check!" ;
- }
-
- // Create new memory entry and save result
- ArMemEntryPtr entryPtr = ArMemEntryPtr(new ArMemEntry());
- entryPtr->producer = producer;
- entryPtr->datatype = datatype;
- entryPtr->produceTimestampMs = sentAt;
- entryPtr->storageTimestampMs = IceUtil::Time::now().toMilliSeconds();
- entryPtr->value = data;
-
- ARMARX_INFO_S << "Saving a new entry of type '" + datatype + "' for producer '" + producer + "'";
- datatype_storage.push_back(entryPtr);
- }
- }
-}
diff --git a/source/RobotAPI/libraries/armem/ArMemWorkingMemoryBase.h b/source/RobotAPI/libraries/armem/ArMemWorkingMemoryBase.h
deleted file mode 100644
index df8bb735e6cf2c5f8ab94a4d985d47c36705a240..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/ArMemWorkingMemoryBase.h
+++ /dev/null
@@ -1,75 +0,0 @@
-/*
- * 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::ArmarXObjects::armem
- * @author Simon Ottenhaus ( simon dot ottenhaus at kit dot edu )
- * @date 2020
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#pragma once
-
-// STD/STL
-#include <vector>
-#include <map>
-#include <memory>
-#include <unordered_map>
-
-// ArmarX
-#include <RobotAPI/interface/armem.h>
-#include <RobotAPI/libraries/aron/data/AronDataNavigator.h>
-
-
-namespace armarx
-{
- namespace armem
- {
-
- class ArMemWorkingMemoryBase;
- typedef std::shared_ptr<ArMemWorkingMemoryBase> ArMemWorkingMemoryBasePtr;
-
- struct ArMemEntry;
- typedef std::shared_ptr<ArMemEntry> ArMemEntryPtr;
-
- struct ArMemEntry
- {
- std::string producer;
- std::string datatype;
- long produceTimestampMs;
- long storageTimestampMs;
- armarx::aron::data::AronDataNavigatorPtr value;
- };
-
- class ArMemWorkingMemoryBase
- {
- typedef std::vector<armem::ArMemEntryPtr> SingleDatatypeStorage; // make map to access via timestamp?
- typedef std::unordered_map<std::string, SingleDatatypeStorage> ProducerDatatypeStorage;
- typedef std::unordered_map<std::string, ProducerDatatypeStorage> TotalDataTypeStorage;
-
- public:
- ArMemWorkingMemoryBase();
-
- void store(const std::string&, const std::string&, long, const aron::data::AronDataNavigatorPtr&);
-
-
- private:
- TotalDataTypeStorage storage;
- long maximumEntriesPerDatatype;
- long maximumEntriesPerProducer;
- };
-
- }
-}
diff --git a/source/RobotAPI/libraries/armem/CMakeLists.txt b/source/RobotAPI/libraries/armem/CMakeLists.txt
index 5de144492fd19df6489a4b3e01f150fc8770fdc9..121a15c6ef5cb56d312fa9d7871212fa6e2db9a8 100644
--- a/source/RobotAPI/libraries/armem/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem/CMakeLists.txt
@@ -3,38 +3,17 @@ set(LIB_NAME armem)
armarx_component_set_name("${LIB_NAME}")
armarx_set_target("Library: ${LIB_NAME}")
-find_package(Eigen3 QUIET)
-armarx_build_if(Eigen3_FOUND "Eigen3 not available")
-if(Eigen3_FOUND)
- include_directories(${Eigen3_INCLUDE_DIR})
-endif()
-
-find_package(Simox QUIET)
-armarx_build_if(Simox_FOUND "Simox not available")
-if(Simox_FOUND)
- include_directories(${Simox_INCLUDE_DIR})
-endif()
-
-find_package(OpenCV QUIET)
-armarx_build_if(OpenCV_FOUND "OpenCV not available")
-if(OpenCV_FOUND)
- include_directories(${OpenCV_INCLUDE_DIRS})
-endif()
-
set(LIBS
ArmarXCoreInterfaces
ArmarXCore
- RobotAPIInterfaces
- ${OpenCV_LIBS}
- ${Simox_LIBS}
)
set(LIB_FILES
- ./ArMemWorkingMemoryBase.cpp
+ ./ArMemBase.cpp
)
set(LIB_HEADERS
- ./ArMemWorkingMemoryBase.h
+ ./ArMemBase.h
)
armarx_add_library("${LIB_NAME}" "${LIB_FILES}" "${LIB_HEADERS}" "${LIBS}")
diff --git a/source/RobotAPI/libraries/aron/AronConcepts.h b/source/RobotAPI/libraries/aron/AronConcepts.h
index a73834410728e2667e2540358096dedb272b7498..c8c1c14bfbd09a8dc8ad0edfeaf38ffb4028259c 100644
--- a/source/RobotAPI/libraries/aron/AronConcepts.h
+++ b/source/RobotAPI/libraries/aron/AronConcepts.h
@@ -59,7 +59,7 @@ namespace armarx
#define RUN_ARON_MACRO(upperType, lowerType, capsType) \
|| (std::is_same<AronData, Aron##upperType>::value)
- HANDLE_ALL_ARON_TYPES_WITHOUT_CPP_TYPE
+ HANDLE_ALL_ARON_DATA_WITHOUT_CPP_TYPE
#undef RUN_ARON_MACRO
;
@@ -89,9 +89,11 @@ namespace armarx
{
class AronDataNavigator;
- class AronObjectDataNavigator;
+#define RUN_ARON_MACRO(upperType, lowerType, capsType) \
+ class Aron##upperType##DataNavigator;
- class AronListDataNavigator;
+ HANDLE_CONTAINER_DATA
+#undef RUN_ARON_MACRO
template<typename AronDataType, typename CppType> //requires AreAronPrimitiveDependent<AronDataType, CppType>
class AronPrimitiveDataNavigator;
@@ -101,7 +103,7 @@ namespace armarx
#define RUN_ARON_MACRO(upperType, lowerType, capsType) \
|| (std::is_same<AronDataNavigator, Aron##upperType##DataNavigator>::value)
- HANDLE_CONTAINER_TYPES
+ HANDLE_CONTAINER_DATA
#undef RUN_ARON_MACRO
#define RUN_ARON_MACRO(cppType, upperType, lowerType, capsType) \
@@ -117,9 +119,11 @@ namespace armarx
{
class AronAbstractTypeNavigator;
- class AronObjectTypeNavigator;
+#define RUN_ARON_MACRO(upperType, lowerType, capsType) \
+ class Aron##upperType##TypeNavigator;
- class AronListTypeNavigator;
+ HANDLE_CONTAINER_TYPES
+#undef RUN_ARON_MACRO
template<typename AronAbstractTypeType, typename AronDataType, typename CppType> //requires AreAronAbstractTypeAndPrimitiveDependent<AronAbstractTypeType, AronDataType, CppType>
class AronPrimitiveTypeNavigator;
@@ -139,5 +143,6 @@ namespace armarx
#undef RUN_ARON_MACRO
;
}
+
}
}
diff --git a/source/RobotAPI/libraries/aron/AronConfig.h b/source/RobotAPI/libraries/aron/AronConfig.h
index 4ba21f81a57f6de4883df055e27454a788e1789b..4f8622a817fd73a425a84ffb71b3d438275ae8a5 100644
--- a/source/RobotAPI/libraries/aron/AronConfig.h
+++ b/source/RobotAPI/libraries/aron/AronConfig.h
@@ -24,28 +24,54 @@
#pragma once
/**
- * This file contains the general structure of AronPrimitives.
- * A primitive is every class, that only contains one field (named value) with a corresponsing type.
- * Examples include integer types, float types but also more complex classes such as Eigen::MatrixXf.
+ * This file contains the general structure of all accepted AronTypes.
*
- * If you want to store a new class directly to the memory, please make sure that the type definition is available
- * in aron.ice and insert the new primitive here. Everything else is done automatically.
+ * In general we distinguish we distinguish between container(complex) and primitive types.
+ * Complex types include Objects, Lists, Dicts, Pairs and Blobs.
+ * Pimitive types on the other hand include int, long, float, double, string and bools.
*
- * Please enter the types in the following way:
- * {[cppType without std], uppercase first letter, all lowercase, all uppercase}
+ * A primitive always has only one member containing the value of the aron. A container type can have multiple members.
*
- * Sometimes ice does not slice the aron.ice file when adding or changing a primitive type here. In that case you need to add an empty
- * character or line to the ice file, to reset its modification date
+ * Note that every AronData must have a corresponding AronAbstractType (both declared in aron.ice).
+ * The AronData is a copy of cpp values while the AronAbstractType describes the type structure.
+ *
+ * In the following we descripe the types in more detail:
+ * AronData:
+ * |
+ * |->Container:
+ * | |
+ * | |->AronObject:
+ * | | This container represents cpp classes. It contains a std::map mapping from a membername (std::string) to AronData.
+ * | | It is a recursive structure that again can contain other complex or primitive types.
+ * | |
+ * | |->AronList:
+ * This container represents a std::vector<T> where T can be any datatype convertable to Aron.
+ * TODO
*/
+/************************************
+ * TYPES ****************************
+ ***********************************/
// All containertypes go here
#define HANDLE_CONTAINER_TYPES \
RUN_ARON_MACRO(Object, object, OBJECT) \
- RUN_ARON_MACRO(List, list, LIST)
+ RUN_ARON_MACRO(List, list, LIST) \
+ RUN_ARON_MACRO(Dict, dict, DICT) \
+ RUN_ARON_MACRO(Tuple, tuple, TUPLE) \
+
+// All complex types go here
+#define HANDLE_COMPLEX_TYPES \
+ RUN_ARON_MACRO(EigenMatrix, eigenmatrix, EIGEN_MATRIX) \
+ RUN_ARON_MACRO(IVTCByteImage, ivtcbyteimage, IVT_CBYTE_IMAGE) \
+ RUN_ARON_MACRO(OpenCVMat, opencvmat, OPENCV_MAT) \
+ RUN_ARON_MACRO(PCLPointcloud, pclpointcloud, PCL_POINTCLOUD)
+// All external types go here (usually only one)
+#define HANDLE_EXTERNAL_TYPES \
+ RUN_ARON_MACRO(External, external, EXTERNAL)
-// All primitive types go here
+// All primitive types go here. They must have a cpp type
#define HANDLE_PRIMITIVE_TYPES_WITH_CPP_TYPE \
RUN_ARON_MACRO(int, Int, int, INT) \
RUN_ARON_MACRO(long, Long, long, LONG) \
@@ -54,6 +80,7 @@
RUN_ARON_MACRO(string, String, string, STRING) \
RUN_ARON_MACRO(bool, Bool, bool, BOOL)
+// All primitive types without cpp type
#define HANDLE_PRIMITIVE_TYPES_WITHOUT_CPP_TYPE \
RUN_ARON_MACRO(Int, int, INT) \
RUN_ARON_MACRO(Long, long, LONG) \
@@ -62,8 +89,28 @@
RUN_ARON_MACRO(String, string, STRING) \
RUN_ARON_MACRO(Bool, bool, BOOL)
-
-// Combined
+// All combined
#define HANDLE_ALL_ARON_TYPES_WITHOUT_CPP_TYPE \
HANDLE_CONTAINER_TYPES \
+ HANDLE_EXTERNAL_TYPES \
+ HANDLE_COMPLEX_TYPES \
+ HANDLE_PRIMITIVE_TYPES_WITHOUT_CPP_TYPE
+
+
+/************************************
+ * DATA *****************************
+ ***********************************/
+
+// All containerdata go here
+#define HANDLE_CONTAINER_DATA \
+ RUN_ARON_MACRO(List, list, LIST) \
+ RUN_ARON_MACRO(Dict, dict, DICT)
+
+#define HANDLE_COMPLEX_DATA \
+ RUN_ARON_MACRO(Blob, blob, BLOB)
+
+// All three combined
+#define HANDLE_ALL_ARON_DATA_WITHOUT_CPP_TYPE \
+ HANDLE_CONTAINER_DATA \
+ HANDLE_COMPLEX_DATA \
HANDLE_PRIMITIVE_TYPES_WITHOUT_CPP_TYPE
diff --git a/source/RobotAPI/libraries/aron/AronDescriptor.h b/source/RobotAPI/libraries/aron/AronDescriptor.h
new file mode 100644
index 0000000000000000000000000000000000000000..d982112115cb0ed8a50255b7fdb06cfa68077fa0
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/AronDescriptor.h
@@ -0,0 +1,104 @@
+/*
+ * 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/>.
+ *
+ * @author Simon Ottenhaus (simon dot ottenhaus at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+// STD/STL
+#include <memory>
+#include <typeindex>
+#include <typeinfo>
+
+// ArmarX
+#include <ArmarXCore/core/exceptions/Exception.h>
+
+#include <RobotAPI/interface/aron.h>
+#include <RobotAPI/libraries/aron/AronConfig.h>
+
+namespace armarx
+{
+ namespace aron
+ {
+ /*enum AronContainerTypeDescriptor
+ {
+ #define RUN_ARON_MACRO(upperType, lowerType, capsType) \
+ eAron##upperType##TypeContainer,
+
+ HANDLE_CONTAINER_TYPES
+ #undef RUN_ARON_MACRO
+ };
+
+ enum AronContainerDataDescriptor
+ {
+ #define RUN_ARON_MACRO(upperType, lowerType, capsType) \
+ eAron##upperType##DataContainer,
+
+ HANDLE_CONTAINER_DATA
+ #undef RUN_ARON_MACRO
+ };
+
+ enum AronComplexTypeDescriptor
+ {
+ #define RUN_ARON_MACRO(upperType, lowerType, capsType) \
+ eAron##upperType##ComplexType,
+
+ HANDLE_COMPLEX_TYPES
+ #undef RUN_ARON_MACRO
+ };
+
+ enum AronComplexDataDescriptor
+ {
+ #define RUN_ARON_MACRO(upperType, lowerType, capsType) \
+ eAron##upperType##ComplexData,
+
+ HANDLE_COMPLEX_DATA
+ #undef RUN_ARON_MACRO
+ };
+
+ enum AronPrimitiveDescriptor
+ {
+ #define RUN_ARON_MACRO(upperType, lowerType, capsType) \
+ eAron##upperType##Primitive,
+
+ HANDLE_PRIMITIVE_TYPES_WITHOUT_CPP_TYPE
+ #undef RUN_ARON_MACRO
+ };*/
+
+ enum AronTypeDescriptor
+ {
+#define RUN_ARON_MACRO(upperType, lowerType, capsType) \
+ eAron##upperType##Type,
+
+ HANDLE_ALL_ARON_TYPES_WITHOUT_CPP_TYPE
+#undef RUN_ARON_MACRO
+ };
+
+ enum AronDataDescriptor
+ {
+#define RUN_ARON_MACRO(upperType, lowerType, capsType) \
+ eAron##upperType##Data,
+
+ HANDLE_ALL_ARON_DATA_WITHOUT_CPP_TYPE
+#undef RUN_ARON_MACRO
+ };
+ }
+}
diff --git a/source/RobotAPI/libraries/aron/AronFactory.h b/source/RobotAPI/libraries/aron/AronFactory.h
index 2f899c92769ded0a966ca327403ec3cd13f7a76f..22741b952827ea2a2bef43ec328c4163c0d9d3ef 100644
--- a/source/RobotAPI/libraries/aron/AronFactory.h
+++ b/source/RobotAPI/libraries/aron/AronFactory.h
@@ -45,8 +45,8 @@ namespace armarx
public:
AronFactory() = default;
- virtual typename Output::PointerType create(const Input&, const AronNavigatorPath&) const = 0;
- virtual typename Output::PointerType createSpecific(const Input&, const AronNavigatorPath&) const = 0;
+ virtual Output create(const Input&, const AronNavigatorPath&) const = 0;
+ virtual Output createSpecific(const Input&, const AronNavigatorPath&) const = 0;
};
}
}
diff --git a/source/RobotAPI/libraries/aron/AronNavigator.h b/source/RobotAPI/libraries/aron/AronNavigator.h
index 5ab1da05597422cc9d078dd3d70702a976a10eae..2235a9c0a3ae5d3468249941efd810c89355f8c4 100644
--- a/source/RobotAPI/libraries/aron/AronNavigator.h
+++ b/source/RobotAPI/libraries/aron/AronNavigator.h
@@ -32,46 +32,58 @@
#include <ArmarXCore/core/exceptions/Exception.h>
#include <RobotAPI/libraries/aron/AronConcepts.h>
+#include <RobotAPI/libraries/aron/AronDescriptor.h>
+#include <RobotAPI/libraries/aron/AronResolver.h>
#include <RobotAPI/libraries/aron/AronNavigatorPath.h>
namespace armarx
{
namespace aron
{
- template <typename Aron> //requires isAron<Aron>
+ template <typename Descriptor, typename Aron> //requires isAron<Aron> && isTypeDescriptor<Descriptor>
+ class AronNavigator;
+
+ template <typename Descriptor, typename Aron> //requires isAron<Aron> && isTypeDescriptor<Descriptor>
+ using AronNavigatorPtr = std::shared_ptr<AronNavigator<Descriptor, Aron>>;
+
+ template <typename Descriptor, typename Aron> //requires isAron<Aron> && isTypeDescriptor<Descriptor>
class AronNavigator
{
public:
- AronNavigator() = default;
- AronNavigator(const AronNavigatorPath& path) :
- path(path)
+ // constructors
+ AronNavigator(const Descriptor& descriptor) :
+ descriptor(descriptor)
{
-
}
- virtual typename Aron::PointerType getResult() const = 0;
-
- AronNavigatorPath getPath() const
+ // public member functions
+ Descriptor getDescriptor() const
{
- return path;
+ return descriptor;
}
- std::string pathToString() const
+ // virual definitions
+ virtual typename Aron::PointerType getResult() const = 0;
+
+ // static methods
+ template<typename AronNavigatorType> //requires isAronNavigator<AronNavigatorType>
+ static typename AronNavigatorType::PointerType FromAronPtr(const typename Aron::PointerType& n)
{
- return path.toString();
+ return AronNavigatorType::DynamicCast(AronNavigatorType::FromAronPtr(n));
}
protected:
- void checkPtr(const typename Aron::PointerType& data, const std::string& message) const
+ static void checkAronPtrForNull(const typename Aron::PointerType& data, const std::string& message)
{
if (data.get() == nullptr)
{
- throw LocalException(message + " - The path was: " + pathToString());
+ throw LocalException("Could not cast an AronPtr. The Ptr was NULL. " + message);
}
}
private:
- AronNavigatorPath path;
+ // members
+ const Descriptor descriptor;
};
}
}
diff --git a/source/RobotAPI/libraries/aron/AronNavigatorPath.cpp b/source/RobotAPI/libraries/aron/AronNavigatorPath.cpp
index 533152596b7ef15bb62cd4514319afd99c19b2f4..971b893c3f040bd0d8de0bec935115288f28f401 100644
--- a/source/RobotAPI/libraries/aron/AronNavigatorPath.cpp
+++ b/source/RobotAPI/libraries/aron/AronNavigatorPath.cpp
@@ -33,7 +33,14 @@ namespace armarx
namespace aron
{
- AronNavigatorPath::AronNavigatorPath()
+ AronNavigatorPath::AronNavigatorPath() :
+ delimeter("/")
+ {
+
+ }
+
+ AronNavigatorPath::AronNavigatorPath(const std::string& d):
+ delimeter(d)
{
}
diff --git a/source/RobotAPI/libraries/aron/AronNavigatorPath.h b/source/RobotAPI/libraries/aron/AronNavigatorPath.h
index eab900d47e92471a5616733599319429921fdc5c..f4a8d63416a4be44a75a8fd648cb559822d70a93 100644
--- a/source/RobotAPI/libraries/aron/AronNavigatorPath.h
+++ b/source/RobotAPI/libraries/aron/AronNavigatorPath.h
@@ -38,18 +38,21 @@ namespace armarx
{
public:
AronNavigatorPath();
+ AronNavigatorPath(const std::string&);
AronNavigatorPath(const AronNavigatorPath&);
AronNavigatorPath(const AronNavigatorPath&, const std::string&);
AronNavigatorPath(const AronNavigatorPath&, const std::string&, const std::string&);
- void appendPath(const std::string& str);
-
std::vector<std::string> getPath() const;
std::string getLastElement() const;
std::string toString() const;
private:
+ void appendPath(const std::string&);
+
+ private:
+ std::string delimeter;
std::vector<std::string> path;
};
}
diff --git a/source/RobotAPI/libraries/aron/AronResolver.cpp b/source/RobotAPI/libraries/aron/AronResolver.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..d7f2af025868fedb031cb766be4c7c32d2b20ed4
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/AronResolver.cpp
@@ -0,0 +1,92 @@
+/*
+ * 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/>.
+ *
+ * @author Simon Ottenhaus (simon dot ottenhaus at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+// Boost
+#include <boost/core/demangle.hpp>
+
+// Header
+#include "AronResolver.h"
+
+namespace armarx
+{
+ namespace aron
+ {
+ AronTypeDescriptor AronResolver::GetTypeForAronAbstractType(const AronAbstractTypePtr& a)
+ {
+#define RUN_ARON_MACRO(upperType, lowerType, capsType) \
+ { \
+ Aron##upperType##TypePtr aron = Aron##upperType##TypePtr::dynamicCast(a); \
+ if(aron.get() != nullptr) \
+ { \
+ return eAron##upperType##Type; \
+ } \
+ }
+ HANDLE_ALL_ARON_TYPES_WITHOUT_CPP_TYPE
+#undef RUN_ARON_MACRO
+ throw LocalException("AronTypeResolver: Could not cast AronAbstractType to a valid type.");
+ }
+
+ AronTypeDescriptor AronResolver::GetTypeForAronAbstractTypeId(const std::type_index& a)
+ {
+#define RUN_ARON_MACRO(upperType, lowerType, capsType) \
+ { \
+ if(a == std::type_index(typeid(Aron##upperType##Type))) \
+ { \
+ return eAron##upperType##Type; \
+ } \
+ }
+ HANDLE_ALL_ARON_TYPES_WITHOUT_CPP_TYPE
+#undef RUN_ARON_MACRO
+ throw LocalException("AronTypeResolver: Could not cast AronAbstractTypeId to a valid type. The typeid was " + boost::core::demangle(a.name()));
+ }
+
+ AronDataDescriptor AronResolver::GetTypeForAronData(const AronDataPtr& a)
+ {
+#define RUN_ARON_MACRO(upperType, lowerType, capsType) \
+ { \
+ Aron##upperType##Ptr aron = Aron##upperType##Ptr::dynamicCast(a); \
+ if(aron.get() != nullptr) \
+ { \
+ return eAron##upperType##Data; \
+ } \
+ }
+ HANDLE_ALL_ARON_DATA_WITHOUT_CPP_TYPE
+#undef RUN_ARON_MACRO
+ throw LocalException("AronTypeResolver: Could not cast AronData to a valid type.");
+ }
+
+ AronDataDescriptor AronResolver::GetTypeForAronDataTypeId(const std::type_index& a)
+ {
+#define RUN_ARON_MACRO(upperType, lowerType, capsType) \
+ { \
+ if(a == std::type_index(typeid(Aron##upperType))) \
+ { \
+ return eAron##upperType##Data; \
+ } \
+ }
+ HANDLE_ALL_ARON_DATA_WITHOUT_CPP_TYPE
+#undef RUN_ARON_MACRO
+ throw LocalException("AronTypeResolver: Could not cast AronDataTypeId to a valid type. The typeid was " + boost::core::demangle(a.name()));
+ }
+ }
+}
diff --git a/source/RobotAPI/libraries/aron/AronResolver.h b/source/RobotAPI/libraries/aron/AronResolver.h
new file mode 100644
index 0000000000000000000000000000000000000000..2c1b79f0f0ab485abcdd77a91349e84b2982ffe7
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/AronResolver.h
@@ -0,0 +1,57 @@
+/*
+ * 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/>.
+ *
+ * @author Simon Ottenhaus (simon dot ottenhaus at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+// STD/STL
+#include <memory>
+#include <typeindex>
+#include <typeinfo>
+
+// ArmarX
+#include <ArmarXCore/core/exceptions/Exception.h>
+
+#include <RobotAPI/interface/aron.h>
+#include <RobotAPI/libraries/aron/AronConfig.h>
+#include <RobotAPI/libraries/aron/AronDescriptor.h>
+
+namespace armarx
+{
+ namespace aron
+ {
+ class AronResolver
+ {
+ private:
+ AronResolver() = default;
+
+ public:
+ static AronTypeDescriptor GetTypeForAronAbstractType(const AronAbstractTypePtr& a);
+
+ static AronTypeDescriptor GetTypeForAronAbstractTypeId(const std::type_index& a);
+
+ static AronDataDescriptor GetTypeForAronData(const AronDataPtr& a);
+
+ static AronDataDescriptor GetTypeForAronDataTypeId(const std::type_index& a);
+ };
+ }
+}
diff --git a/source/RobotAPI/libraries/aron/AronTypeDescriptor.h b/source/RobotAPI/libraries/aron/AronTypeDescriptor.h
deleted file mode 100644
index ce7f22cdb917b8eac803a22507aad93de73e7c20..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/AronTypeDescriptor.h
+++ /dev/null
@@ -1,145 +0,0 @@
-/*
- * 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/>.
- *
- * @author Simon Ottenhaus (simon dot ottenhaus at kit dot edu)
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#pragma once
-
-// STD/STL
-#include <memory>
-#include <typeindex>
-#include <typeinfo>
-
-// ArmarX
-#include <ArmarXCore/core/exceptions/Exception.h>
-
-#include <RobotAPI/interface/aron.h>
-#include <RobotAPI/libraries/aron/AronConfig.h>
-
-namespace armarx
-{
- namespace aron
- {
- class AronTypeDescriptor;
- typedef std::shared_ptr<AronTypeDescriptor> AronTypeDescriptorPtr;
-
- class AronTypeDescriptor
- {
- public:
- enum Value
- {
-#define RUN_ARON_MACRO(upperType, lowerType, capsType) \
- eAron##upperType,
-
- HANDLE_ALL_ARON_TYPES_WITHOUT_CPP_TYPE
-#undef RUN_ARON_MACRO
- };
-
- protected:
- AronTypeDescriptor() = default;
-
- public:
- constexpr AronTypeDescriptor(const Value type) : value(type) { }
-
- operator Value() const
- {
- return value;
- }
- Value getValue() const
- {
- return value;
- }
-
- explicit operator bool() = delete;
- constexpr bool operator==(const AronTypeDescriptor& a) const
- {
- return value == a.value;
- }
- constexpr bool operator!=(const AronTypeDescriptor& a) const
- {
- return value != a.value;
- }
-
-
- static AronTypeDescriptorPtr GetTypeForAronAbstractType(const AronAbstractTypePtr& a)
- {
-#define RUN_ARON_MACRO(upperType, lowerType, capsType) \
- { \
- Aron##upperType##TypePtr aron = Aron##upperType##TypePtr::dynamicCast(a); \
- if(aron.get() != nullptr) \
- { \
- return AronTypeDescriptorPtr(new AronTypeDescriptor(eAron##upperType)); \
- } \
- }
- HANDLE_ALL_ARON_TYPES_WITHOUT_CPP_TYPE
-#undef RUN_ARON_MACRO
- throw LocalException("AronTypeDescriptor: Could not cast AronAbstractType to a valid type.");
- }
-
- static AronTypeDescriptorPtr GetTypeForAronAbstractTypeId(const std::type_index& a)
- {
-#define RUN_ARON_MACRO(upperType, lowerType, capsType) \
- { \
- if(a == std::type_index(typeid(Aron##upperType##Type))) \
- { \
- return AronTypeDescriptorPtr(new AronTypeDescriptor(eAron##upperType)); \
- } \
- }
- HANDLE_ALL_ARON_TYPES_WITHOUT_CPP_TYPE
-#undef RUN_ARON_MACRO
- throw LocalException("AronTypeDescriptor: Could not cast AronAbstractTypeId to a valid type.");
- }
-
- static AronTypeDescriptorPtr GetTypeForAronData(const AronDataPtr& a)
- {
-#define RUN_ARON_MACRO(upperType, lowerType, capsType) \
- { \
- Aron##upperType##Ptr aron = Aron##upperType##Ptr::dynamicCast(a); \
- if(aron.get() != nullptr) \
- { \
- return AronTypeDescriptorPtr(new AronTypeDescriptor(eAron##upperType)); \
- } \
- }
- HANDLE_ALL_ARON_TYPES_WITHOUT_CPP_TYPE
-#undef RUN_ARON_MACRO
- throw LocalException("AronTypeDescriptor: Could not cast AronData to a valid type.");
- }
-
- static AronTypeDescriptorPtr GetTypeForAronDataTypeId(const std::type_index& a)
- {
-#define RUN_ARON_MACRO(upperType, lowerType, capsType) \
- { \
- if(a == std::type_index(typeid(Aron##upperType))) \
- { \
- return AronTypeDescriptorPtr(new AronTypeDescriptor(eAron##upperType)); \
- } \
- }
- HANDLE_ALL_ARON_TYPES_WITHOUT_CPP_TYPE
-#undef RUN_ARON_MACRO
- throw LocalException("AronTypeDescriptor: Could not cast AronDataTypeId to a valid type.");
- }
-
- private:
- Value value;
-
- };
- }
-}
diff --git a/source/RobotAPI/libraries/aron/CMakeLists.txt b/source/RobotAPI/libraries/aron/CMakeLists.txt
index e1b760772df67bd80391a4c2fd6ca3ea50b256b8..1d44e52999ea6df921df8e451ef4927f782f0541 100644
--- a/source/RobotAPI/libraries/aron/CMakeLists.txt
+++ b/source/RobotAPI/libraries/aron/CMakeLists.txt
@@ -25,19 +25,27 @@ set(LIBS
)
set(LIB_FILES
+ AronResolver.cpp
AronNavigatorPath.cpp
data/AronDataNavigator.cpp
- data/AronObjectDataNavigator.cpp
- data/AronPrimitiveDataNavigator.cpp
data/AronListDataNavigator.cpp
+ data/AronDictDataNavigator.cpp
+ data/AronBlobDataNavigator.cpp
+ data/AronPrimitiveDataNavigator.cpp
data/AronDataNavigatorFactory.cpp
types/AronAbstractTypeNavigator.cpp
types/AronObjectTypeNavigator.cpp
types/AronListTypeNavigator.cpp
+ types/AronDictTypeNavigator.cpp
+ types/AronTupleTypeNavigator.cpp
+ types/AronEigenMatrixTypeNavigator.cpp
+ types/AronIVTCByteImageTypeNavigator.cpp
+ types/AronOpenCVMatTypeNavigator.cpp
+ types/AronPCLPointcloudTypeNavigator.cpp
types/AronPrimitiveTypeNavigator.cpp
- types/AronAbstractTypeNavigatorFactory.cpp
+ types/AronExternalTypeNavigator.cpp
io/classWriters/AronDataNavigatorWriter/AronDataNavigatorWriterToken.cpp
io/classWriters/AronDataNavigatorWriter/AronDataNavigatorWriter.cpp
@@ -45,7 +53,7 @@ set(LIB_FILES
io/classReaders/AronDataNavigatorReader/AronDataNavigatorReaderToken.cpp
io/classReaders/AronDataNavigatorReader/AronDataNavigatorReader.cpp
- codegenerator/cppWriters/AronCppWriter.cpp
+ codegenerator/codeWriters/cppWriter/AronCppWriter.cpp
codegenerator/typeReaders/xmlReader/AronAbstractTypeXMLData.cpp
codegenerator/typeReaders/xmlReader/AronAbstractTypeXMLReaderFactory.cpp
@@ -58,20 +66,30 @@ set(LIB_HEADERS
AronNavigator.h
AronFactory.h
- AronTypeDescriptor.h
+ AronDescriptor.h
+ AronResolver.h
AronNavigatorPath.h
+ data/AronAllDataNavigators.h
data/AronDataNavigator.h
- data/AronObjectDataNavigator.h
data/AronListDataNavigator.h
+ data/AronDictDataNavigator.h
+ data/AronBlobDataNavigator.h
data/AronPrimitiveDataNavigator.h
data/AronDataNavigatorFactory.h
+ types/AronAllTypeNavigators.h
types/AronAbstractTypeNavigator.h
types/AronObjectTypeNavigator.h
types/AronListTypeNavigator.h
+ types/AronDictTypeNavigator.h
+ types/AronTupleTypeNavigator.h
+ types/AronEigenMatrixTypeNavigator.h
+ types/AronIVTCByteImageTypeNavigator.h
+ types/AronOpenCVMatTypeNavigator.h
+ types/AronPCLPointcloudTypeNavigator.h
types/AronPrimitiveTypeNavigator.h
- types/AronAbstractTypeNavigatorFactory.h
+ types/AronExternalTypeNavigator.h
io/AronWriter.h
io/classWriters/AronDataWriter.h
@@ -89,9 +107,10 @@ set(LIB_HEADERS
codegenerator/AronCppClass.h
codegenerator/AronWriterInfo.h
codegenerator/AronReaderInfo.h
- codegenerator/AronCppWriterBase.h
+ codegenerator/AronUseTypeInfo.h
+ codegenerator/AronCodeWriter.h
- codegenerator/cppWriters/AronCppWriter.h
+ codegenerator/codeWriters/cppWriter/AronCppWriter.h
codegenerator/typeReaders/AronAbstractTypeReader.h
codegenerator/typeReaders/xmlReader/AronAbstractTypeXMLData.h
diff --git a/source/RobotAPI/libraries/aron/codegenerator/AronCppWriterBase.h b/source/RobotAPI/libraries/aron/codegenerator/AronCodeWriter.h
similarity index 100%
rename from source/RobotAPI/libraries/aron/codegenerator/AronCppWriterBase.h
rename to source/RobotAPI/libraries/aron/codegenerator/AronCodeWriter.h
diff --git a/source/RobotAPI/libraries/aron/codegenerator/AronCppClass.h b/source/RobotAPI/libraries/aron/codegenerator/AronCppClass.h
index ce45063d0d198594df2fb41edd0fec2eae9db544..978faaf299898ec961ea3e397dd59c44c0ce3117 100644
--- a/source/RobotAPI/libraries/aron/codegenerator/AronCppClass.h
+++ b/source/RobotAPI/libraries/aron/codegenerator/AronCppClass.h
@@ -16,7 +16,7 @@
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*
- * @author Simon Ottenhaus (simon dot ottenhaus at kit dot edu)
+ * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
* GNU General Public License
*/
@@ -30,7 +30,9 @@
// ArmarX
#include <ArmarXCore/core/exceptions/Exception.h>
-#include <RobotAPI/libraries/aron/AronConfig.h>
+#include <RobotAPI/libraries/aron/AronConcepts.h>
+#include <RobotAPI/libraries/aron/io/AronReader.h>
+#include <RobotAPI/libraries/aron/io/AronWriter.h>
using namespace std;
@@ -48,12 +50,19 @@ namespace armarx
public:
using PointerType = AronCppClassPtr;
-#define RUN_ARON_MACRO(cppType, upperType, lowerType, capsType) \
- virtual void setMember##upperType(const std::string&, const cppType&) = 0; \
+ public:
+ virtual void initialize() = 0;
+ virtual void reset() = 0;
+ virtual void read(armarx::aron::io::AronReader& r) = 0;
+ virtual void write(armarx::aron::io::AronWriter& w) const = 0;
- HANDLE_PRIMITIVE_TYPES_WITH_CPP_TYPE
-#undef RUN_ARON_MACRO
+ AronAbstractTypePtr getAronAbstractType() const
+ {
+ return aronType;
+ }
+ protected:
+ AronAbstractTypePtr aronType;
};
}
}
diff --git a/source/RobotAPI/applications/RobotStateObserver/RobotStateObserverApp.h b/source/RobotAPI/libraries/aron/codegenerator/AronUseTypeInfo.h
similarity index 53%
rename from source/RobotAPI/applications/RobotStateObserver/RobotStateObserverApp.h
rename to source/RobotAPI/libraries/aron/codegenerator/AronUseTypeInfo.h
index e3538fda2a18bf3bb490de05ac06a9a01a6b7efe..f36668acf7dc938270040580c48d4fd7a21829f6 100644
--- a/source/RobotAPI/applications/RobotStateObserver/RobotStateObserverApp.h
+++ b/source/RobotAPI/libraries/aron/codegenerator/AronUseTypeInfo.h
@@ -1,7 +1,8 @@
/*
* This file is part of ArmarX.
*
- * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
+ * 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
@@ -15,31 +16,38 @@
* 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 ArmarXCore::applications
- * @author Kai Welke (welke dot at kit dot edu)
- * @date 2012
+ * @author Simon Ottenhaus (simon dot ottenhaus at kit dot edu)
* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
* GNU General Public License
*/
+#pragma once
-#include <ArmarXCore/core/application/Application.h>
-#include <RobotAPI/libraries/core/remoterobot/RobotStateObserver.h>
+// STD/STL
+#include <memory>
+#include <string>
namespace armarx
{
- /**
- * Application for testing armarx::RobotStateObserver
- */
- class RobotStateObserverApp :
- virtual public armarx::Application
+ namespace aron
{
- /**
- * @see armarx::Application::setup()
- */
- void setup(const ManagedIceObjectRegistryInterfacePtr& registry, Ice::PropertiesPtr properties)
+ namespace codegeneration
{
- registry->addObject(Component::create<RobotStateObserver>(properties));
+ class AronUseTypeInfo;
+ typedef std::shared_ptr<AronUseTypeInfo> AronUseTypeInfoPtr;
+
+ class AronUseTypeInfo
+ {
+ public:
+ std::string typeName;
+ std::string include;
+
+ auto operator<(const AronUseTypeInfo& s) const
+ {
+ // The methodName must be unique
+ return typeName < s.typeName;
+ }
+ };
}
- };
+ }
}
diff --git a/source/RobotAPI/libraries/aron/codegenerator/codeWriters/cppWriter/AronCppWriter.cpp b/source/RobotAPI/libraries/aron/codegenerator/codeWriters/cppWriter/AronCppWriter.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..a327def7f088f156497861902a3f185f33e510c3
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/codegenerator/codeWriters/cppWriter/AronCppWriter.cpp
@@ -0,0 +1,181 @@
+/*
+ * 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/>.
+ *
+ * @author Simon Ottenhaus (simon dot ottenhaus at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+
+// Boost
+#include <boost/algorithm/string.hpp>
+
+// Header
+#include "AronCppWriter.h"
+
+// ArmarX
+#include <ArmarXCore/core/exceptions/Exception.h>
+
+namespace armarx
+{
+ namespace aron
+ {
+ namespace codegeneration
+ {
+ namespace classwriter
+ {
+ AronTypeClassCppWriter::AronTypeClassCppWriter(const std::string& producerName, const std::set<AronWriterInfo>& writers, const std::set<AronReaderInfo>& readers, const std::vector<AronUseTypeInfo>& useTypes) :
+ producerName(producerName),
+ writers(writers),
+ readers(readers)
+ {
+ additionalIncludes.push_back("<RobotAPI/libraries/aron/codegenerator/AronCppClass.h>");
+ for (const auto& useType : useTypes)
+ {
+ additionalIncludes.push_back(useType.include);
+ }
+ }
+
+ std::vector<CppClassPtr> AronTypeClassCppWriter::getTypeClasses() const
+ {
+ return typeClasses;
+ }
+
+ void AronTypeClassCppWriter::generateTypeClasses(const GenerateTypePtrList& produceTypes)
+ {
+ for (const auto& [produceObj, localProduceTypes] : produceTypes)
+ {
+ // Convert to Object type and create class object
+ if (produceObj == nullptr)
+ {
+ throw LocalException("An received produce type is not an object. You can only produce objects!");
+ }
+
+ CppClassPtr c = setupCppClassFromObjectType(produceObj);
+ c->addInherit("virtual public armarx::aron::codegeneration::AronCppClass");
+
+ // Add includes and guard
+ setupIncludes(c, boost::algorithm::to_upper_copy(producerName) + "__" + boost::algorithm::to_upper_copy(c->getName()) + "__ARON_TYPE_DEFINITION_INCLUDE_GUARD");
+
+ // setup inner classes
+ for(const auto& localProduceObj : localProduceTypes)
+ {
+ CppClassPtr inner = setupCppClassFromObjectType(localProduceObj);
+
+ c->addInnerClass(inner);
+ }
+
+ // Generic methods
+ CppMethodPtr reset = produceObj->toResetMethod();
+ c->addMethod(reset);
+
+ CppMethodPtr setup = produceObj->toInitializeMethod();
+ c->addMethod(setup);
+
+ CppMethodPtr write = produceObj->toWriteMethod();
+ c->addMethod(write);
+
+ CppMethodPtr read = produceObj->toReadMethod();
+ c->addMethod(read);
+
+ // Writermethods
+ for (const AronWriterInfo& info : writers)
+ {
+ c->addInclude(info.include);
+ CppMethodPtr convert = produceObj->toSpecializedWriterMethod(info.returnType, info.methodName, info.writerClassType);
+ c->addMethod(convert);
+ }
+
+ // Add methods to set the member variables
+ for (const AronReaderInfo& info : readers)
+ {
+ c->addInclude(info.include);
+ CppMethodPtr convert = produceObj->toSpecializedReaderMethod(info.argumentType, info.methodName, info.readerClassType);
+ c->addMethod(convert);
+ }
+
+ typeClasses.push_back(c);
+ }
+ }
+
+ CppClassPtr AronTypeClassCppWriter::setupCppClassFromObjectType(const types::AronObjectTypeNavigatorPtr& o) const
+ {
+ std::vector<std::string> split;
+ std::string cppTypename = o->getFullCppTypename();
+ boost::algorithm::split(split, cppTypename, boost::is_any_of("::"), boost::token_compress_on);
+ if (split.size() < 1)
+ {
+ throw LocalException("The cpp name of an inner type was empty. Please check the type definition file if all object names are set correctly.");
+ }
+
+ std::vector<std::string> namespaces(split);
+ namespaces.pop_back();
+
+ CppClassPtr c = CppClassPtr(new CppClass(namespaces, split[split.size() - 1]));
+
+ for (const auto& [key, child] : o->getChildren())
+ {
+ c->addPublicField(child->getFullCppTypename() + " " + key + ";");
+ }
+
+ // operator==
+ std::stringstream doc;
+ doc << "@brief operator==() - This method checks whether all values equal another instance. \n";
+ doc << "@param i - The other instance\n";
+ doc << "@return - true, if all members are the same, false otherwise";
+ CppMethodPtr equals = CppMethodPtr(new CppMethod("bool operator==(const " + o->getFullCppTypename() + "& i) const", doc.str()));
+
+ for (const auto& [key, child] : o->getChildren())
+ {
+ equals->addLine("if ( not (" + key + " == i." + key + "))");
+ equals->addLine(" return false;");
+ }
+ equals->addLine("return true;");
+ c->addMethod(equals);
+
+ return c;
+ }
+
+ void AronTypeClassCppWriter::setupIncludes(CppClassPtr& c, const std::string& guard) const
+ {
+ c->addClassDoc("Autogenerated class. Please do not edit. Instead, edit the corresponding .xml file.");
+ c->setIncludeGuard(guard);
+ c->addInclude("<string>");
+ c->addInclude("<vector>");
+ c->addInclude("<map>");
+
+ c->addInclude("<Eigen/Core>");
+ c->addInclude("<RobotAPI/interface/aron.h>");
+ for (const std::string& s : additionalIncludes)
+ {
+ c->addInclude(s);
+ }
+ CppBlockPtr ctorBlock = CppBlockPtr(new CppBlock());
+ ctorBlock->addLine("initialize();");
+ ctorBlock->addLine("reset();");
+ c->addCtor("", ctorBlock);
+ }
+ }
+ }
+ }
+}
+
+
+
+
+
diff --git a/source/RobotAPI/libraries/aron/codegenerator/cppWriters/AronCppWriter.h b/source/RobotAPI/libraries/aron/codegenerator/codeWriters/cppWriter/AronCppWriter.h
similarity index 78%
rename from source/RobotAPI/libraries/aron/codegenerator/cppWriters/AronCppWriter.h
rename to source/RobotAPI/libraries/aron/codegenerator/codeWriters/cppWriter/AronCppWriter.h
index 23d37d97ba7ead12f20ab83499a2653bfcc150fe..0c7098924705596571878c2cc36d7fde32d4a168 100644
--- a/source/RobotAPI/libraries/aron/codegenerator/cppWriters/AronCppWriter.h
+++ b/source/RobotAPI/libraries/aron/codegenerator/codeWriters/cppWriter/AronCppWriter.h
@@ -40,6 +40,7 @@
#include <RobotAPI/libraries/aron/codegenerator/AronWriterInfo.h>
#include <RobotAPI/libraries/aron/codegenerator/AronReaderInfo.h>
+#include <RobotAPI/libraries/aron/codegenerator/AronUseTypeInfo.h>
namespace armarx
{
@@ -55,22 +56,23 @@ namespace armarx
class AronTypeClassCppWriter
{
public:
- typedef std::vector<types::AronAbstractTypeNavigatorPtr> ProduceTypePtrList;
+ typedef std::vector<std::pair<types::AronObjectTypeNavigatorPtr, std::vector<types::AronObjectTypeNavigatorPtr>>> GenerateTypePtrList;
- AronTypeClassCppWriter(const std::string&, const std::set<AronWriterInfo>&, const std::set<AronReaderInfo>&);
+ AronTypeClassCppWriter(const std::string&, const std::set<AronWriterInfo>&, const std::set<AronReaderInfo>&, const std::vector<AronUseTypeInfo>&);
- void generateTypeClasses(const ProduceTypePtrList&);
+ void generateTypeClasses(const GenerateTypePtrList&);
std::vector<CppClassPtr> getTypeClasses() const;
private:
- void setupCppClass(CppClassPtr&, const std::string&, const std::vector<std::string>&) const;
+ CppClassPtr setupCppClassFromObjectType(const types::AronObjectTypeNavigatorPtr&) const;
+ void setupIncludes(CppClassPtr&, const std::string&) const;
private:
std::string producerName;
std::set<AronWriterInfo> writers;
std::set<AronReaderInfo> readers;
+ std::vector<std::string> additionalIncludes;
std::vector<CppClassPtr> typeClasses;
-
};
}
}
diff --git a/source/RobotAPI/libraries/aron/codegenerator/cppWriters/AronCppWriter.cpp b/source/RobotAPI/libraries/aron/codegenerator/cppWriters/AronCppWriter.cpp
deleted file mode 100644
index c118a1f15c940cee52d1cb8285f03026070ce4ca..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/codegenerator/cppWriters/AronCppWriter.cpp
+++ /dev/null
@@ -1,135 +0,0 @@
-/*
- * 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/>.
- *
- * @author Simon Ottenhaus (simon dot ottenhaus at kit dot edu)
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-
-// Boost
-#include <boost/algorithm/string.hpp>
-
-// Header
-#include "AronCppWriter.h"
-
-// ArmarX
-#include <ArmarXCore/core/exceptions/Exception.h>
-
-namespace armarx
-{
- namespace aron
- {
- namespace codegeneration
- {
- namespace classwriter
- {
- AronTypeClassCppWriter::AronTypeClassCppWriter(const std::string& producerName, const std::set<AronWriterInfo>& writers, const std::set<AronReaderInfo>& readers) :
- producerName(producerName),
- writers(writers),
- readers(readers)
- {
- }
-
- std::vector<CppClassPtr> AronTypeClassCppWriter::getTypeClasses() const
- {
- return typeClasses;
- }
-
- void AronTypeClassCppWriter::generateTypeClasses(const ProduceTypePtrList& produceTypes)
- {
- for (const types::AronAbstractTypeNavigatorPtr& produceType : produceTypes)
- {
- // Convert to Object type and create class object
- types::AronObjectTypeNavigatorPtr produceObj = std::dynamic_pointer_cast<types::AronObjectTypeNavigator>(produceType);
- if (produceObj == nullptr)
- {
- throw LocalException("An received produce type is not an object. You can only produce objects!");
- }
-
- std::vector<std::string> split;
- std::string cppTypename = produceObj->getFullCppTypename();
- boost::algorithm::split(split, cppTypename, boost::is_any_of("::"), boost::token_compress_on);
- if (split.size() < 1)
- {
- throw LocalException("The cpp name of a type was empty. Please check the type definition file if all object names are set correctly.");
- }
-
- std::vector<std::string> namespaces(split);
- namespaces.pop_back();
- CppClassPtr c = CppClassPtr(new CppClass(namespaces, split[split.size() - 1]));
-
- for (const auto& [key, child] : produceObj->getChildren())
- {
- c->addPublicField(child->getFullCppTypename() + " " + key + ";");
- }
-
- // Type definition
- setupCppClass(c, boost::algorithm::to_upper_copy(producerName) + "_ARON_TYPE_DEFINITION_INCLUDE_GUARD", {"<RobotAPI/libraries/aron/io/AronWriter.h>", "<RobotAPI/libraries/aron/io/AronReader.h>"});
-
- // Generic methods
- CppMethodPtr write = produceType->toWriteMethod();
- c->addMethod(write);
-
- CppMethodPtr read = produceType->toReadMethod();
- c->addMethod(read);
-
- // Writermethods
- for(const AronWriterInfo& info : writers)
- {
- c->addInclude(info.include);
- CppMethodPtr convert = produceType->toSpecializedWriterMethod(info.returnType, info.methodName, info.writerClassType);
- c->addMethod(convert);
- }
-
- // Add methods to set the member variables
- for(const AronReaderInfo& info : readers)
- {
- c->addInclude(info.include);
- CppMethodPtr convert = produceType->toSpecializedReaderMethod(info.argumentType, info.methodName, info.readerClassType);
- c->addMethod(convert);
- }
-
- typeClasses.push_back(c);
- }
- }
-
- void AronTypeClassCppWriter::setupCppClass(CppClassPtr& c, const std::string& guard, const std::vector<std::string>& includes) const
- {
- c->setIncludeGuard(guard);
- c->addInclude("<string>");
- c->addInclude("<vector>");
- c->addInclude("<map>");
-
- c->addInclude("<Eigen/Core>");
- c->addInclude("<RobotAPI/interface/aron.h>");
- for(const std::string& s : includes)
- {
- c->addInclude(s);
- }
- c->addCtor("");
- }
- }
- }
- }
-}
-
-
-
-
-
diff --git a/source/RobotAPI/libraries/aron/codegenerator/typeReaders/AronAbstractTypeReader.h b/source/RobotAPI/libraries/aron/codegenerator/typeReaders/AronAbstractTypeReader.h
index 30185c31bf3873d951e1f751dea2992eabf48c45..747d0331172d45953306e21d8822c7d3d62c9546 100644
--- a/source/RobotAPI/libraries/aron/codegenerator/typeReaders/AronAbstractTypeReader.h
+++ b/source/RobotAPI/libraries/aron/codegenerator/typeReaders/AronAbstractTypeReader.h
@@ -25,11 +25,13 @@
// STD/STL
#include <memory>
+#include <filesystem>
// ArmarX
-#include <RobotAPI/libraries/aron/types/AronAbstractTypeNavigator.h>
+#include <RobotAPI/libraries/aron/types/AronObjectTypeNavigator.h>
#include <RobotAPI/libraries/aron/codegenerator/AronReaderInfo.h>
#include <RobotAPI/libraries/aron/codegenerator/AronWriterInfo.h>
+#include <RobotAPI/libraries/aron/codegenerator/AronUseTypeInfo.h>
namespace armarx
{
@@ -59,6 +61,7 @@ namespace armarx
toAron.include = "<RobotAPI/libraries/aron/io/classWriters/AronDataNavigatorWriter/AronDataNavigatorWriter.h>";
writers.insert(toAron);
+ // The fromAron method is visible by deafult
AronReaderInfo fromAron;
fromAron.methodName = "fromAron";
fromAron.argumentType = "armarx::aron::AronDataPtr";
@@ -69,6 +72,7 @@ namespace armarx
virtual void parse(const Input&) = 0;
virtual void parseFile(const std::string& filename) = 0;
+ virtual void parseFile(const std::filesystem::path& file) = 0;
std::set<AronWriterInfo> getWriters() const
{
@@ -79,20 +83,20 @@ namespace armarx
return readers;
}
- std::vector<types::AronAbstractTypeNavigatorPtr> getReceiveTypes() const
+ std::vector<AronUseTypeInfo> getUseTypes() const
{
- return receiveTypes;
+ return useTypes;
}
- std::vector<types::AronAbstractTypeNavigatorPtr> getProduceTypes() const
+ std::vector<std::pair<types::AronObjectTypeNavigatorPtr, std::vector<types::AronObjectTypeNavigatorPtr>>> getGenerateTypes() const
{
- return produceTypes;
+ return generateTypes;
}
protected:
std::set<AronReaderInfo> readers;
std::set<AronWriterInfo> writers;
- std::vector<types::AronAbstractTypeNavigatorPtr> receiveTypes;
- std::vector<types::AronAbstractTypeNavigatorPtr> produceTypes;
+ std::vector<AronUseTypeInfo> useTypes;
+ std::vector<std::pair<types::AronObjectTypeNavigatorPtr, std::vector<types::AronObjectTypeNavigatorPtr>>> generateTypes;
};
}
}
diff --git a/source/RobotAPI/libraries/aron/codegenerator/typeReaders/xmlReader/AronAbstractTypeXMLData.cpp b/source/RobotAPI/libraries/aron/codegenerator/typeReaders/xmlReader/AronAbstractTypeXMLData.cpp
index 3fda8197b6c4fd9203b4d4522d1a9f1f8cce8573..689acfd6d31b424d53e8a30fe15dbe815e60f558 100644
--- a/source/RobotAPI/libraries/aron/codegenerator/typeReaders/xmlReader/AronAbstractTypeXMLData.cpp
+++ b/source/RobotAPI/libraries/aron/codegenerator/typeReaders/xmlReader/AronAbstractTypeXMLData.cpp
@@ -39,14 +39,14 @@ namespace armarx
namespace aron
{
namespace codegeneration
- {
+ {
namespace typereader
{
const std::string AronAbstractTypeXMLReaderData::ARON_TYPE_DEFINITION_TAG = "arontypedefinition";
const std::string AronAbstractTypeXMLReaderData::ARON_SERIALIZERS_TAG = "serializers";
const std::string AronAbstractTypeXMLReaderData::ARON_USE_TYPES_TAG = "usetypes";
- const std::string AronAbstractTypeXMLReaderData::ARON_PRODUCE_TYPES_TAG = "producetypes";
+ const std::string AronAbstractTypeXMLReaderData::ARON_PRODUCE_TYPES_TAG = "generatetypes";
const std::string AronAbstractTypeXMLReaderData::ARON_SERIALIZER_WRITERS_TAG = "writers";
const std::string AronAbstractTypeXMLReaderData::ARON_SERIALIZER_READERS_TAG = "readers";
@@ -54,52 +54,64 @@ namespace armarx
const std::string AronAbstractTypeXMLReaderData::ARON_SERIALIZER_WRITER_TAG = "writer";
const std::string AronAbstractTypeXMLReaderData::ARON_SERIALIZER_READER_TAG = "reader";
const std::string AronAbstractTypeXMLReaderData::ARON_USE_TYPE_TAG = "usetype";
- const std::string AronAbstractTypeXMLReaderData::ARON_PRODUCE_TYPE_TAG = "producetype";
+ const std::string AronAbstractTypeXMLReaderData::ARON_PRODUCE_TYPE_TAG = "generatetype";
const std::string AronAbstractTypeXMLReaderData::ARON_USE_TYPE_INCLUDE_ATTRIBUTE_NAME = "include";
+ const std::string AronAbstractTypeXMLReaderData::ARON_USE_TYPE_NAME_ATTRIBUTE_NAME = "name";
const std::string AronAbstractTypeXMLReaderData::ARON_SERIALIZER_METHOD_ATTRIBUTE_NAME = "method";
const std::string AronAbstractTypeXMLReaderData::ARON_SERIALIZER_METHOD_RETURN_ATTRIBUTE_NAME = "return";
const std::string AronAbstractTypeXMLReaderData::ARON_SERIALIZER_METHOD_ARGUMENT_ATTRIBUTE_NAME = "argumenttype";
const std::string AronAbstractTypeXMLReaderData::ARON_SERIALIZER_WRITER_NAME_ATTRIBUTE_NAME = "writer";
const std::string AronAbstractTypeXMLReaderData::ARON_SERIALIZER_READER_NAME_ATTRIBUTE_NAME = "reader";
const std::string AronAbstractTypeXMLReaderData::ARON_SERIALIZER_INCLUDE_ATTRIBUTE_NAME = "include";
+ const std::string AronAbstractTypeXMLReaderData::ARON_PRODUCE_EXTERNAL_NAME_ATTRIBUTE_NAME = "name";
const std::string AronAbstractTypeXMLReaderData::ARON_PRODUCE_OBJECT_NAME_ATTRIBUTE_NAME = "name";
+ const std::string AronAbstractTypeXMLReaderData::ARON_PRODUCE_OBJECT_TYPE_ATTRIBUTE_NAME = "type";
const std::string AronAbstractTypeXMLReaderData::ARON_PRODUCE_OBJECT_CHILD_KEY_ATTRIBUTE_NAME = "key";
const std::string AronAbstractTypeXMLReaderData::ARON_PRODUCE_LIST_TYPE_ATTRIBUTE_NAME = "listtype";
+ const std::string AronAbstractTypeXMLReaderData::ARON_PRODUCE_DICT_TYPE_ATTRIBUTE_NAME = "dicttype";
+ const std::string AronAbstractTypeXMLReaderData::ARON_PRODUCE_TUPLE_TYPE_ATTRIBUTE_NAME = "tupletype";
const std::string AronAbstractTypeXMLReaderData::ARON_PRODUCE_OBJECT_CHILD_TAG = "objectchild";
const std::string AronAbstractTypeXMLReaderData::ARON_PRODUCE_LIST_TYPE_TAG = "listtype";
+ const std::string AronAbstractTypeXMLReaderData::ARON_PRODUCE_DICT_TYPE_TAG = "dicttype";
+ const std::string AronAbstractTypeXMLReaderData::ARON_PRODUCE_TUPLE_TYPE_TAG = "tupletype";
// Top Level type tags
- #define RUN_ARON_MACRO(upperType, lowerType, capsType) \
- const std::string AronAbstractTypeXMLReaderData::ARON_PRODUCE_##capsType##_TAG = #lowerType;
+#define RUN_ARON_MACRO(upperType, lowerType, capsType) \
+ const std::string AronAbstractTypeXMLReaderData::ARON_PRODUCE_##capsType##_TAG = #lowerType;
- HANDLE_ALL_ARON_TYPES_WITHOUT_CPP_TYPE
- #undef RUN_ARON_MACRO
+ HANDLE_ALL_ARON_TYPES_WITHOUT_CPP_TYPE
+#undef RUN_ARON_MACRO
- void AronAbstractTypeXMLReaderData::checkAttribute(const RapidXmlReaderNode& node, const std::string& att)
+ void AronAbstractTypeXMLReaderData::CheckAttribute(const RapidXmlReaderNode& node, const std::string& att)
{
- if(!node.has_attribute(att.c_str()))
+ if (!node.has_attribute(att.c_str()))
{
- throw LocalException("A <"+node.name()+">-tag does not have the correct attribute. Expected: " + att);
+ throw LocalException("A <" + node.name() + ">-tag does not have the correct attribute. Expected: " + att);
}
}
- void AronAbstractTypeXMLReaderData::checkTagName(const RapidXmlReaderNode& node, const std::string& name)
+ bool AronAbstractTypeXMLReaderData::HasTagName(const RapidXmlReaderNode& node, const std::string& name)
+ {
+ return (boost::algorithm::to_lower_copy(name) == boost::algorithm::to_lower_copy(node.name()));
+ }
+
+ void AronAbstractTypeXMLReaderData::CheckTagName(const RapidXmlReaderNode& node, const std::string& name)
{
- if(!(boost::algorithm::to_lower_copy(name) == boost::algorithm::to_lower_copy(node.name())))
+ if (!(HasTagName(node, name)))
{
- throw LocalException("The node <"+node.name()+"> has the wrong tag. Expected: <" + name + ">");
+ throw LocalException("The node <" + node.name() + "> has the wrong tag. Expected: <" + name + ">");
}
}
- void AronAbstractTypeXMLReaderData::checkChildSize(const RapidXmlReaderNode& node, const size_t size)
+ void AronAbstractTypeXMLReaderData::CheckChildSize(const RapidXmlReaderNode& node, const size_t size)
{
std::vector<RapidXmlReaderNode> children = node.nodes();
- if(children.size() != size)
+ if (children.size() != size)
{
- throw LocalException("The node <"+node.name()+"> can only have exactly "+std::to_string(size)+" children. Found " + std::to_string(children.size()) + ".");
+ throw LocalException("The node <" + node.name() + "> can only have exactly " + std::to_string(size) + " children. Found " + std::to_string(children.size()) + ".");
}
}
}
diff --git a/source/RobotAPI/libraries/aron/codegenerator/typeReaders/xmlReader/AronAbstractTypeXMLData.h b/source/RobotAPI/libraries/aron/codegenerator/typeReaders/xmlReader/AronAbstractTypeXMLData.h
index 204e9343b5f96018d65543d3db35b3499207c8e0..0b37d9c998c2f02a6a2eb65f46cb3057606f0182 100644
--- a/source/RobotAPI/libraries/aron/codegenerator/typeReaders/xmlReader/AronAbstractTypeXMLData.h
+++ b/source/RobotAPI/libraries/aron/codegenerator/typeReaders/xmlReader/AronAbstractTypeXMLData.h
@@ -72,24 +72,32 @@ namespace armarx
const static std::string ARON_SERIALIZER_READER_NAME_ATTRIBUTE_NAME;
const static std::string ARON_SERIALIZER_INCLUDE_ATTRIBUTE_NAME;
const static std::string ARON_USE_TYPE_INCLUDE_ATTRIBUTE_NAME;
+ const static std::string ARON_USE_TYPE_NAME_ATTRIBUTE_NAME;
+ const static std::string ARON_PRODUCE_EXTERNAL_NAME_ATTRIBUTE_NAME;
const static std::string ARON_PRODUCE_OBJECT_NAME_ATTRIBUTE_NAME;
+ const static std::string ARON_PRODUCE_OBJECT_TYPE_ATTRIBUTE_NAME;
const static std::string ARON_PRODUCE_OBJECT_CHILD_KEY_ATTRIBUTE_NAME;
const static std::string ARON_PRODUCE_LIST_TYPE_ATTRIBUTE_NAME;
+ const static std::string ARON_PRODUCE_DICT_TYPE_ATTRIBUTE_NAME;
+ const static std::string ARON_PRODUCE_TUPLE_TYPE_ATTRIBUTE_NAME;
// Second level tags. Only important if in specific top level tag
const static std::string ARON_PRODUCE_OBJECT_CHILD_TAG;
const static std::string ARON_PRODUCE_LIST_TYPE_TAG;
+ const static std::string ARON_PRODUCE_DICT_TYPE_TAG;
+ const static std::string ARON_PRODUCE_TUPLE_TYPE_TAG;
// Top Level type tags
- #define RUN_ARON_MACRO(upperType, lowerType, capsType) \
- const static std::string ARON_PRODUCE_##capsType##_TAG;
+#define RUN_ARON_MACRO(upperType, lowerType, capsType) \
+ const static std::string ARON_PRODUCE_##capsType##_TAG;
- HANDLE_ALL_ARON_TYPES_WITHOUT_CPP_TYPE
- #undef RUN_ARON_MACRO
+ HANDLE_ALL_ARON_TYPES_WITHOUT_CPP_TYPE
+#undef RUN_ARON_MACRO
- static void checkAttribute(const RapidXmlReaderNode& node, const std::string& att);
- static void checkTagName(const RapidXmlReaderNode& node, const std::string& name);
- static void checkChildSize(const RapidXmlReaderNode& node, const size_t size);
+ static void CheckAttribute(const RapidXmlReaderNode& node, const std::string& att);
+ static bool HasTagName(const RapidXmlReaderNode& node, const std::string& name);
+ static void CheckTagName(const RapidXmlReaderNode& node, const std::string& name);
+ static void CheckChildSize(const RapidXmlReaderNode& node, const size_t size);
};
}
}
diff --git a/source/RobotAPI/libraries/aron/codegenerator/typeReaders/xmlReader/AronAbstractTypeXMLReader.cpp b/source/RobotAPI/libraries/aron/codegenerator/typeReaders/xmlReader/AronAbstractTypeXMLReader.cpp
index 4dd038abce4257ce4fbbd29b588188741b20f1d1..9667584ebd658df1cb7f60ed9dc031a0684a93ec 100644
--- a/source/RobotAPI/libraries/aron/codegenerator/typeReaders/xmlReader/AronAbstractTypeXMLReader.cpp
+++ b/source/RobotAPI/libraries/aron/codegenerator/typeReaders/xmlReader/AronAbstractTypeXMLReader.cpp
@@ -42,7 +42,7 @@ namespace armarx
{
namespace typereader
{
- const AronAbstractTypeXMLReaderFactory AronAbstractTypeXMLReader::Factory;
+ const AronObjectTypeXMLReaderFactory AronAbstractTypeXMLReader::Factory;
void AronAbstractTypeXMLReader::parse(const std::string& xml)
{
@@ -56,63 +56,77 @@ namespace armarx
parse(reader);
}
+ void AronAbstractTypeXMLReader::parseFile(const std::filesystem::path& file)
+ {
+ RapidXmlReaderPtr reader = RapidXmlReader::FromFile(file.string());
+ parse(reader);
+ }
+
void AronAbstractTypeXMLReader::parse(const RapidXmlReaderPtr& reader)
{
RapidXmlReaderNode root = reader->getRoot();
// Check Header
- AronAbstractTypeXMLReaderData::checkTagName(root, AronAbstractTypeXMLReaderData::ARON_TYPE_DEFINITION_TAG);
+ AronAbstractTypeXMLReaderData::CheckTagName(root, AronAbstractTypeXMLReaderData::ARON_TYPE_DEFINITION_TAG);
- AronAbstractTypeXMLReaderData::checkChildSize(root, 3);
+ AronAbstractTypeXMLReaderData::CheckChildSize(root, 3);
std::vector<RapidXmlReaderNode> children = root.nodes();
- AronAbstractTypeXMLReaderData::checkTagName(children[0], AronAbstractTypeXMLReaderData::ARON_SERIALIZERS_TAG);
- AronAbstractTypeXMLReaderData::checkTagName(children[1], AronAbstractTypeXMLReaderData::ARON_USE_TYPES_TAG);
- AronAbstractTypeXMLReaderData::checkTagName(children[2], AronAbstractTypeXMLReaderData::ARON_PRODUCE_TYPES_TAG);
-
+ AronAbstractTypeXMLReaderData::CheckTagName(children[0], AronAbstractTypeXMLReaderData::ARON_SERIALIZERS_TAG);
+ AronAbstractTypeXMLReaderData::CheckTagName(children[1], AronAbstractTypeXMLReaderData::ARON_USE_TYPES_TAG);
+ AronAbstractTypeXMLReaderData::CheckTagName(children[2], AronAbstractTypeXMLReaderData::ARON_PRODUCE_TYPES_TAG);
// Check Serializers
- AronAbstractTypeXMLReaderData::checkChildSize(children[0], 2);
+ AronAbstractTypeXMLReaderData::CheckChildSize(children[0], 2);
std::vector<RapidXmlReaderNode> serializers = children[0].nodes();
- AronAbstractTypeXMLReaderData::checkTagName(serializers[0], AronAbstractTypeXMLReaderData::ARON_SERIALIZER_WRITERS_TAG);
- AronAbstractTypeXMLReaderData::checkTagName(serializers[1], AronAbstractTypeXMLReaderData::ARON_SERIALIZER_READERS_TAG);
+ AronAbstractTypeXMLReaderData::CheckTagName(serializers[0], AronAbstractTypeXMLReaderData::ARON_SERIALIZER_WRITERS_TAG);
+ AronAbstractTypeXMLReaderData::CheckTagName(serializers[1], AronAbstractTypeXMLReaderData::ARON_SERIALIZER_READERS_TAG);
- for(const auto& serializer : serializers[0].nodes())
+ for (const auto& serializer : serializers[0].nodes())
{
this->writers.insert(readWriter(serializer));
}
- for(const auto& serializer : serializers[1].nodes())
+ for (const auto& serializer : serializers[1].nodes())
{
this->readers.insert(readReader(serializer));
}
// Check UseTypes
- for(const auto& receiveType : children[1].nodes())
+ for (const auto& receiveType : children[1].nodes())
+ {
+ this->useTypes.push_back(readUseType(receiveType));
+ }
+
+ // Add UseTypes to Factory
+ std::vector<std::string> useObjectNames;
+ for (const auto& useType : useTypes)
{
- this->receiveTypes.push_back(readReceiveType(receiveType));
+ useObjectNames.push_back(useType.typeName);
}
+ Factory.AddTopLevelObjectNames(useObjectNames);
// Check ProduceTypes
- for(const auto& produceType : children[2].nodes())
+ for (const auto& produceType : children[2].nodes())
{
- this->produceTypes.push_back(readProduceType(produceType));
+ readGenerateType(produceType);
}
+ generateTypes = Factory.GetAllProducedTopLevelObjects();
}
AronWriterInfo AronAbstractTypeXMLReader::readWriter(const RapidXmlReaderNode& node) const
{
- AronAbstractTypeXMLReaderData::checkTagName(node, AronAbstractTypeXMLReaderData::ARON_SERIALIZER_WRITER_TAG);
+ AronAbstractTypeXMLReaderData::CheckTagName(node, AronAbstractTypeXMLReaderData::ARON_SERIALIZER_WRITER_TAG);
- AronAbstractTypeXMLReaderData::checkAttribute(node, AronAbstractTypeXMLReaderData::ARON_SERIALIZER_METHOD_ATTRIBUTE_NAME);
+ AronAbstractTypeXMLReaderData::CheckAttribute(node, AronAbstractTypeXMLReaderData::ARON_SERIALIZER_METHOD_ATTRIBUTE_NAME);
const std::string methodname = node.attribute_value(AronAbstractTypeXMLReaderData::ARON_SERIALIZER_METHOD_ATTRIBUTE_NAME.c_str());
- AronAbstractTypeXMLReaderData::checkAttribute(node, AronAbstractTypeXMLReaderData::ARON_SERIALIZER_METHOD_RETURN_ATTRIBUTE_NAME);
+ AronAbstractTypeXMLReaderData::CheckAttribute(node, AronAbstractTypeXMLReaderData::ARON_SERIALIZER_METHOD_RETURN_ATTRIBUTE_NAME);
const std::string returntype = node.attribute_value(AronAbstractTypeXMLReaderData::ARON_SERIALIZER_METHOD_RETURN_ATTRIBUTE_NAME.c_str());
- AronAbstractTypeXMLReaderData::checkAttribute(node, AronAbstractTypeXMLReaderData::ARON_SERIALIZER_WRITER_NAME_ATTRIBUTE_NAME);
+ AronAbstractTypeXMLReaderData::CheckAttribute(node, AronAbstractTypeXMLReaderData::ARON_SERIALIZER_WRITER_NAME_ATTRIBUTE_NAME);
const std::string writer = node.attribute_value(AronAbstractTypeXMLReaderData::ARON_SERIALIZER_WRITER_NAME_ATTRIBUTE_NAME.c_str());
- AronAbstractTypeXMLReaderData::checkAttribute(node, AronAbstractTypeXMLReaderData::ARON_SERIALIZER_INCLUDE_ATTRIBUTE_NAME);
+ AronAbstractTypeXMLReaderData::CheckAttribute(node, AronAbstractTypeXMLReaderData::ARON_SERIALIZER_INCLUDE_ATTRIBUTE_NAME);
const std::string include = node.attribute_value(AronAbstractTypeXMLReaderData::ARON_SERIALIZER_INCLUDE_ATTRIBUTE_NAME.c_str());
AronWriterInfo info;
@@ -126,18 +140,18 @@ namespace armarx
AronReaderInfo AronAbstractTypeXMLReader::readReader(const RapidXmlReaderNode& node) const
{
- AronAbstractTypeXMLReaderData::checkTagName(node, AronAbstractTypeXMLReaderData::ARON_SERIALIZER_READER_TAG);
+ AronAbstractTypeXMLReaderData::CheckTagName(node, AronAbstractTypeXMLReaderData::ARON_SERIALIZER_READER_TAG);
- AronAbstractTypeXMLReaderData::checkAttribute(node, AronAbstractTypeXMLReaderData::ARON_SERIALIZER_METHOD_ATTRIBUTE_NAME);
+ AronAbstractTypeXMLReaderData::CheckAttribute(node, AronAbstractTypeXMLReaderData::ARON_SERIALIZER_METHOD_ATTRIBUTE_NAME);
const std::string methodname = node.attribute_value(AronAbstractTypeXMLReaderData::ARON_SERIALIZER_METHOD_ATTRIBUTE_NAME.c_str());
- AronAbstractTypeXMLReaderData::checkAttribute(node, AronAbstractTypeXMLReaderData::ARON_SERIALIZER_METHOD_ARGUMENT_ATTRIBUTE_NAME);
+ AronAbstractTypeXMLReaderData::CheckAttribute(node, AronAbstractTypeXMLReaderData::ARON_SERIALIZER_METHOD_ARGUMENT_ATTRIBUTE_NAME);
const std::string returntype = node.attribute_value(AronAbstractTypeXMLReaderData::ARON_SERIALIZER_METHOD_ARGUMENT_ATTRIBUTE_NAME.c_str());
- AronAbstractTypeXMLReaderData::checkAttribute(node, AronAbstractTypeXMLReaderData::ARON_SERIALIZER_READER_NAME_ATTRIBUTE_NAME);
+ AronAbstractTypeXMLReaderData::CheckAttribute(node, AronAbstractTypeXMLReaderData::ARON_SERIALIZER_READER_NAME_ATTRIBUTE_NAME);
const std::string reader = node.attribute_value(AronAbstractTypeXMLReaderData::ARON_SERIALIZER_READER_NAME_ATTRIBUTE_NAME.c_str());
- AronAbstractTypeXMLReaderData::checkAttribute(node, AronAbstractTypeXMLReaderData::ARON_SERIALIZER_INCLUDE_ATTRIBUTE_NAME);
+ AronAbstractTypeXMLReaderData::CheckAttribute(node, AronAbstractTypeXMLReaderData::ARON_SERIALIZER_INCLUDE_ATTRIBUTE_NAME);
const std::string include = node.attribute_value(AronAbstractTypeXMLReaderData::ARON_SERIALIZER_INCLUDE_ATTRIBUTE_NAME.c_str());
AronReaderInfo info;
@@ -149,27 +163,27 @@ namespace armarx
return info;
}
- types::AronAbstractTypeNavigatorPtr AronAbstractTypeXMLReader::readReceiveType(const RapidXmlReaderNode& node) const
+ AronUseTypeInfo AronAbstractTypeXMLReader::readUseType(const RapidXmlReaderNode& node) const
{
- AronAbstractTypeXMLReaderData::checkTagName(node, AronAbstractTypeXMLReaderData::ARON_USE_TYPE_TAG);
+ AronAbstractTypeXMLReaderData::CheckTagName(node, AronAbstractTypeXMLReaderData::ARON_USE_TYPE_TAG);
- AronAbstractTypeXMLReaderData::checkAttribute(node, AronAbstractTypeXMLReaderData::ARON_USE_TYPE_INCLUDE_ATTRIBUTE_NAME);
+ AronAbstractTypeXMLReaderData::CheckAttribute(node, AronAbstractTypeXMLReaderData::ARON_USE_TYPE_INCLUDE_ATTRIBUTE_NAME);
+ const std::string include = node.attribute_value(AronAbstractTypeXMLReaderData::ARON_USE_TYPE_INCLUDE_ATTRIBUTE_NAME.c_str());
- return nullptr;
- }
-
- types::AronAbstractTypeNavigatorPtr AronAbstractTypeXMLReader::readProduceType(const RapidXmlReaderNode& node) const
- {
- AronAbstractTypeXMLReaderData::checkTagName(node, AronAbstractTypeXMLReaderData::ARON_PRODUCE_TYPE_TAG);
+ AronAbstractTypeXMLReaderData::CheckAttribute(node, AronAbstractTypeXMLReaderData::ARON_USE_TYPE_NAME_ATTRIBUTE_NAME);
+ const std::string name = node.attribute_value(AronAbstractTypeXMLReaderData::ARON_USE_TYPE_NAME_ATTRIBUTE_NAME.c_str());
- AronAbstractTypeXMLReaderData::checkChildSize(node, 1);
- std::vector<RapidXmlReaderNode> children = node.nodes();
+ AronUseTypeInfo info;
+ info.typeName = name;
+ info.include = include;
- RapidXmlReaderNode child = children[0];
+ return info;
+ }
- // We only accept objects on the first level
- AronAbstractTypeXMLReaderData::checkTagName(child, AronAbstractTypeXMLReaderData::ARON_PRODUCE_OBJECT_TAG);
- return Factory.create(child, AronNavigatorPath());
+ types::AronObjectTypeNavigatorPtr AronAbstractTypeXMLReader::readGenerateType(const RapidXmlReaderNode& node) const
+ {
+ AronAbstractTypeXMLReaderData::CheckTagName(node, AronAbstractTypeXMLReaderData::ARON_PRODUCE_TYPE_TAG);
+ return types::AronObjectTypeNavigator::DynamicCast(Factory.createSpecific(node, AronNavigatorPath()));
}
}
}
diff --git a/source/RobotAPI/libraries/aron/codegenerator/typeReaders/xmlReader/AronAbstractTypeXMLReader.h b/source/RobotAPI/libraries/aron/codegenerator/typeReaders/xmlReader/AronAbstractTypeXMLReader.h
index bd43eda8f5a1c0e602ca5a96b9556c0b54cdffa7..2b4186f7d0d3c02ecc4cf701586d30bf082dc693 100644
--- a/source/RobotAPI/libraries/aron/codegenerator/typeReaders/xmlReader/AronAbstractTypeXMLReader.h
+++ b/source/RobotAPI/libraries/aron/codegenerator/typeReaders/xmlReader/AronAbstractTypeXMLReader.h
@@ -25,6 +25,7 @@
// STD/STL
#include <memory>
+#include <filesystem>
// Base Class
#include <RobotAPI/libraries/aron/codegenerator/typeReaders/AronAbstractTypeReader.h>
@@ -32,7 +33,7 @@
// ArmarX
#include <ArmarXCore/core/rapidxml/wrapper/RapidXmlReader.h>
-#include <RobotAPI/libraries/aron/types/AronAbstractTypeNavigator.h>
+#include <RobotAPI/libraries/aron/types/AronObjectTypeNavigator.h>
#include <RobotAPI/libraries/aron/codegenerator/typeReaders/xmlReader/AronAbstractTypeXMLReaderFactory.h>
namespace armarx
@@ -47,24 +48,25 @@ namespace armarx
typedef std::shared_ptr<AronAbstractTypeXMLReader> AronAbstractTypeXMLReaderPtr;
class AronAbstractTypeXMLReader :
- virtual public AronAbstractTypeReader<std::string>
+ virtual public AronAbstractTypeReader<std::string>
{
public:
AronAbstractTypeXMLReader() = default;
virtual void parse(const std::string&) override;
virtual void parseFile(const std::string&) override;
+ virtual void parseFile(const std::filesystem::path&) override;
private:
void parse(const RapidXmlReaderPtr& node);
AronWriterInfo readWriter(const RapidXmlReaderNode& node) const;
AronReaderInfo readReader(const RapidXmlReaderNode& node) const;
- types::AronAbstractTypeNavigatorPtr readReceiveType(const RapidXmlReaderNode& node) const;
- types::AronAbstractTypeNavigatorPtr readProduceType(const RapidXmlReaderNode& node) const;
+ AronUseTypeInfo readUseType(const RapidXmlReaderNode& node) const;
+ types::AronObjectTypeNavigatorPtr readGenerateType(const RapidXmlReaderNode& node) const;
private:
- static const AronAbstractTypeXMLReaderFactory Factory;
+ static const AronObjectTypeXMLReaderFactory Factory;
};
}
}
diff --git a/source/RobotAPI/libraries/aron/codegenerator/typeReaders/xmlReader/AronAbstractTypeXMLReaderFactory.cpp b/source/RobotAPI/libraries/aron/codegenerator/typeReaders/xmlReader/AronAbstractTypeXMLReaderFactory.cpp
index b13f04d18d2e7c8ad7a81cbc8eb61e53cdf93c29..1d7452b590c0aaf30b58fadf5957562f07b28c12 100644
--- a/source/RobotAPI/libraries/aron/codegenerator/typeReaders/xmlReader/AronAbstractTypeXMLReaderFactory.cpp
+++ b/source/RobotAPI/libraries/aron/codegenerator/typeReaders/xmlReader/AronAbstractTypeXMLReaderFactory.cpp
@@ -42,18 +42,27 @@ namespace armarx
{
namespace typereader
{
+ std::string AronAbstractTypeXMLReaderFactory::CurrentTopLevelObjectName = "";
+ std::vector<std::pair<std::string, std::vector<std::string>>> AronAbstractTypeXMLReaderFactory::AllTopLevelObjectNames = {};
+ std::vector<std::string> AronAbstractTypeXMLReaderFactory::AllLocalObjectNames = {};
+ std::vector<std::pair<types::AronObjectTypeNavigatorPtr, std::vector<types::AronObjectTypeNavigatorPtr>>> AronAbstractTypeXMLReaderFactory::AllTopLevelProducedObjects = {};
+ std::vector<types::AronObjectTypeNavigatorPtr> AronAbstractTypeXMLReaderFactory::AllLocalProducedObjects = {};
+
const std::map<std::string, AronAbstractTypeXMLReaderFactoryPtr> AronAbstractTypeXMLReaderFactory::Factories =
{
#define RUN_ARON_MACRO(upperType, lowerType, capsType) \
{AronAbstractTypeXMLReaderData::ARON_PRODUCE_##capsType##_TAG, AronAbstractTypeXMLReaderFactoryPtr(new Aron##upperType##TypeXMLReaderFactory())}, \
- HANDLE_CONTAINER_TYPES
+ HANDLE_CONTAINER_TYPES
+ HANDLE_EXTERNAL_TYPES
+ //HANDLE_COMPLEX_TYPES
#undef RUN_ARON_MACRO
+
#define RUN_ARON_MACRO(cppType, upperType, lowerType, capsType) \
{AronAbstractTypeXMLReaderData::ARON_PRODUCE_##capsType##_TAG, AronAbstractTypeXMLReaderFactoryPtr(new AronPrimitiveTypeXMLReaderFactory<Aron##upperType##Type, Aron##upperType, cppType>())}, \
- HANDLE_PRIMITIVE_TYPES_WITH_CPP_TYPE
+ HANDLE_PRIMITIVE_TYPES_WITH_CPP_TYPE
#undef RUN_ARON_MACRO
};
@@ -73,23 +82,77 @@ namespace armarx
return factory_iterator->second->createSpecific(node, path);
}
- types::AronAbstractTypeNavigatorPtr AronAbstractTypeXMLReaderFactory::createSpecific(const RapidXmlReaderNode&, const AronNavigatorPath&) const
+ std::vector<std::pair<types::AronObjectTypeNavigatorPtr, std::vector<types::AronObjectTypeNavigatorPtr>>> AronAbstractTypeXMLReaderFactory::GetAllProducedTopLevelObjects()
+ {
+ return AllTopLevelProducedObjects;
+ }
+
+ void AronAbstractTypeXMLReaderFactory::AddTopLevelObjectNames(const std::vector<std::string>& names)
+ {
+ for (const auto& name : names)
+ {
+ AddTopLevelObjectName(name);
+ }
+ }
+
+ void AronAbstractTypeXMLReaderFactory::AddTopLevelObjectName(const std::string& name)
{
- throw LocalException("Called disallowed method of an AronDataNavigatorFactory. Use child class instead!");
+ AllTopLevelObjectNames.push_back(std::make_pair(name, std::vector<std::string>()));
}
+ // External
+ types::AronAbstractTypeNavigatorPtr AronExternalTypeXMLReaderFactory::createSpecific(const RapidXmlReaderNode& node, const AronNavigatorPath& path) const
+ {
+ AronAbstractTypeXMLReaderData::CheckAttribute(node, AronAbstractTypeXMLReaderData::ARON_PRODUCE_EXTERNAL_NAME_ATTRIBUTE_NAME);
+ const std::string name = node.attribute_value(AronAbstractTypeXMLReaderData::ARON_PRODUCE_EXTERNAL_NAME_ATTRIBUTE_NAME.c_str());
+
+ // check whether type exists
+ for (const auto& [x, _] : AllTopLevelObjectNames)
+ {
+ if (name == x)
+ {
+ types::AronExternalTypeNavigatorPtr aronExternalType = types::AronExternalTypeNavigatorPtr(new types::AronExternalTypeNavigator(name, path));
+ return aronExternalType;
+ }
+ }
+
+ // check whether type locally exists
+ if (std::find(AllLocalObjectNames.begin(), AllLocalObjectNames.end(), name) != AllLocalObjectNames.end())
+ {
+ types::AronExternalTypeNavigatorPtr aronExternalType = types::AronExternalTypeNavigatorPtr(new types::AronExternalTypeNavigator(name, path));
+ return aronExternalType;
+ }
+
+ throw LocalException("AronExternalTypeXMLReaderFactory: A name could not be found. Perhaps you need to change the order of your types or you have to add another UseType!");
+ }
+
+ // Object
types::AronAbstractTypeNavigatorPtr AronObjectTypeXMLReaderFactory::createSpecific(const RapidXmlReaderNode& node, const AronNavigatorPath& path) const
{
- AronAbstractTypeXMLReaderData::checkAttribute(node, AronAbstractTypeXMLReaderData::ARON_PRODUCE_OBJECT_NAME_ATTRIBUTE_NAME);
+ AronAbstractTypeXMLReaderData::CheckAttribute(node, AronAbstractTypeXMLReaderData::ARON_PRODUCE_OBJECT_NAME_ATTRIBUTE_NAME);
const std::string name = node.attribute_value(AronAbstractTypeXMLReaderData::ARON_PRODUCE_OBJECT_NAME_ATTRIBUTE_NAME.c_str());
+ if (AronAbstractTypeXMLReaderData::HasTagName(node, AronAbstractTypeXMLReaderData::ARON_PRODUCE_TYPE_TAG))
+ {
+ // we are in a top level object
+ // set the new name
+ CurrentTopLevelObjectName = name;
+ AllLocalObjectNames.clear();
+ AllLocalProducedObjects.clear();
+ }
+ else if (name.find("::") != std::string::npos)
+ {
+ throw LocalException("AronObjectTypeXMLReaderFactory: A local class must not have a namespace!");
+ }
+
types::AronObjectTypeNavigatorPtr aronObjectType = types::AronObjectTypeNavigatorPtr(new types::AronObjectTypeNavigator(name, path));
- for(const RapidXmlReaderNode& objectChild : node.nodes()) {
- AronAbstractTypeXMLReaderData::checkTagName(objectChild, AronAbstractTypeXMLReaderData::ARON_PRODUCE_OBJECT_CHILD_TAG);
- AronAbstractTypeXMLReaderData::checkChildSize(objectChild, 1);
+ for (const RapidXmlReaderNode& objectChild : node.nodes())
+ {
+ AronAbstractTypeXMLReaderData::CheckTagName(objectChild, AronAbstractTypeXMLReaderData::ARON_PRODUCE_OBJECT_CHILD_TAG);
+ AronAbstractTypeXMLReaderData::CheckChildSize(objectChild, 1);
- AronAbstractTypeXMLReaderData::checkAttribute(objectChild, AronAbstractTypeXMLReaderData::ARON_PRODUCE_OBJECT_CHILD_KEY_ATTRIBUTE_NAME);
+ AronAbstractTypeXMLReaderData::CheckAttribute(objectChild, AronAbstractTypeXMLReaderData::ARON_PRODUCE_OBJECT_CHILD_KEY_ATTRIBUTE_NAME);
const std::string key = objectChild.attribute_value(AronAbstractTypeXMLReaderData::ARON_PRODUCE_OBJECT_CHILD_KEY_ATTRIBUTE_NAME.c_str());
std::vector<RapidXmlReaderNode> children = objectChild.nodes();
@@ -97,34 +160,118 @@ namespace armarx
aronObjectType->addElement(key, childNavigator);
}
+ if (AronAbstractTypeXMLReaderData::HasTagName(node, AronAbstractTypeXMLReaderData::ARON_PRODUCE_TYPE_TAG))
+ {
+ // Finishing top level object
+ AllTopLevelObjectNames.push_back(std::make_pair(name, AllLocalObjectNames));
+ AllTopLevelProducedObjects.push_back(std::make_pair(aronObjectType, AllLocalProducedObjects));
+ }
+ else
+ {
+ std::cout << "add low level " << name;
+ AllLocalObjectNames.push_back(aronObjectType->getFullCppTypename());
+ AllLocalProducedObjects.push_back(aronObjectType);
+ }
+
return aronObjectType;
}
+ // List
types::AronAbstractTypeNavigatorPtr AronListTypeXMLReaderFactory::createSpecific(const RapidXmlReaderNode& node, const AronNavigatorPath& path) const
{
- if(node.has_attribute(AronAbstractTypeXMLReaderData::ARON_PRODUCE_LIST_TYPE_ATTRIBUTE_NAME.c_str()))
+ if (node.has_attribute(AronAbstractTypeXMLReaderData::ARON_PRODUCE_LIST_TYPE_ATTRIBUTE_NAME.c_str()))
{
- AronAbstractTypeXMLReaderData::checkAttribute(node, AronAbstractTypeXMLReaderData::ARON_PRODUCE_LIST_TYPE_ATTRIBUTE_NAME);
+ AronAbstractTypeXMLReaderData::CheckChildSize(node, 0);
const std::string acceptedType = node.attribute_value(AronAbstractTypeXMLReaderData::ARON_PRODUCE_LIST_TYPE_ATTRIBUTE_NAME.c_str());
- return types::AronAbstractTypeNavigatorPtr(new types::AronListTypeNavigator(acceptedType, path));
+ return types::AronAbstractTypeNavigator::GetAronAbstractTypePtrByName(acceptedType);
}
else
{
- // Create childtype
- AronAbstractTypeXMLReaderData::checkChildSize(node, 1);
+ AronAbstractTypeXMLReaderData::CheckChildSize(node, 1);
std::vector<RapidXmlReaderNode> nodeChildren = node.nodes();
const RapidXmlReaderNode listTypeDeclarationNode = nodeChildren[0];
- AronAbstractTypeXMLReaderData::checkChildSize(listTypeDeclarationNode, 1);
+ AronAbstractTypeXMLReaderData::CheckTagName(listTypeDeclarationNode, AronAbstractTypeXMLReaderData::ARON_PRODUCE_LIST_TYPE_TAG);
+
+ AronAbstractTypeXMLReaderData::CheckChildSize(listTypeDeclarationNode, 1);
std::vector<RapidXmlReaderNode> listTypeNodeChildren = listTypeDeclarationNode.nodes();
const RapidXmlReaderNode typeNode = listTypeNodeChildren[0];
-
types::AronAbstractTypeNavigatorPtr type = create(typeNode, AronNavigatorPath(path, "type"));
return types::AronAbstractTypeNavigatorPtr(new types::AronListTypeNavigator(type, path));
}
}
+
+ // Dict
+ types::AronAbstractTypeNavigatorPtr AronDictTypeXMLReaderFactory::createSpecific(const RapidXmlReaderNode& node, const AronNavigatorPath& path) const
+ {
+ AronAbstractTypeXMLReaderData::CheckChildSize(node, 1);
+ std::vector<RapidXmlReaderNode> nodeChildren = node.nodes();
+ const RapidXmlReaderNode dictTypeDeclarationNode = nodeChildren[0];
+
+ if (dictTypeDeclarationNode.has_attribute(AronAbstractTypeXMLReaderData::ARON_PRODUCE_DICT_TYPE_ATTRIBUTE_NAME.c_str()))
+ {
+ AronAbstractTypeXMLReaderData::CheckChildSize(dictTypeDeclarationNode, 0);
+ const std::string acceptedType = dictTypeDeclarationNode.attribute_value(AronAbstractTypeXMLReaderData::ARON_PRODUCE_DICT_TYPE_ATTRIBUTE_NAME.c_str());
+ return types::AronAbstractTypeNavigator::GetAronAbstractTypePtrByName(acceptedType);
+ }
+ else
+ {
+ AronAbstractTypeXMLReaderData::CheckTagName(dictTypeDeclarationNode, AronAbstractTypeXMLReaderData::ARON_PRODUCE_DICT_TYPE_TAG);
+
+ AronAbstractTypeXMLReaderData::CheckChildSize(dictTypeDeclarationNode, 1);
+ std::vector<RapidXmlReaderNode> dictTypeNodeChildren = dictTypeDeclarationNode.nodes();
+ const RapidXmlReaderNode typeNode = dictTypeNodeChildren[0];
+ types::AronAbstractTypeNavigatorPtr type = create(typeNode, AronNavigatorPath(path, "type"));
+ return types::AronAbstractTypeNavigatorPtr(new types::AronDictTypeNavigator(type, path));
+ }
+ }
+
+ // Tuple
+ types::AronAbstractTypeNavigatorPtr AronTupleTypeXMLReaderFactory::createSpecific(const RapidXmlReaderNode& node, const AronNavigatorPath& path) const
+ {
+ std::vector<types::AronAbstractTypeNavigatorPtr> types;
+ std::vector<RapidXmlReaderNode> nodeChildren = node.nodes();
+
+ if (nodeChildren.size() < 2)
+ {
+ throw LocalException("AronTupleTypeXMLReaderFactory: A tuple must have at least two types.");
+ }
+
+ unsigned int i = 0;
+ for (const RapidXmlReaderNode& tupleTypeDeclarationNode : nodeChildren)
+ {
+ AronAbstractTypeXMLReaderData::CheckTagName(tupleTypeDeclarationNode, AronAbstractTypeXMLReaderData::ARON_PRODUCE_TUPLE_TYPE_TAG);
+
+ if (tupleTypeDeclarationNode.has_attribute(AronAbstractTypeXMLReaderData::ARON_PRODUCE_TUPLE_TYPE_ATTRIBUTE_NAME.c_str()))
+ {
+ AronAbstractTypeXMLReaderData::CheckChildSize(tupleTypeDeclarationNode, 0);
+
+ const std::string typeName = tupleTypeDeclarationNode.attribute_value(AronAbstractTypeXMLReaderData::ARON_PRODUCE_TUPLE_TYPE_ATTRIBUTE_NAME.c_str());
+ types::AronAbstractTypeNavigatorPtr type = types::AronAbstractTypeNavigator::GetAronAbstractTypePtrByName(typeName);
+ }
+ else
+ {
+ AronAbstractTypeXMLReaderData::CheckChildSize(tupleTypeDeclarationNode, 1);
+
+ std::vector<RapidXmlReaderNode> typeNodeChildren = tupleTypeDeclarationNode.nodes();
+ const RapidXmlReaderNode typeNode = typeNodeChildren[0];
+
+ types::AronAbstractTypeNavigatorPtr type = create(typeNode, AronNavigatorPath(path, "<" + std::to_string(i++) + ">"));
+ types.push_back(type);
+ }
+ }
+ return types::AronAbstractTypeNavigatorPtr(new types::AronTupleTypeNavigator(types, path));
+ }
+
+ // Blob
+ /*types::AronAbstractTypeNavigatorPtr AronBlobTypeXMLReaderFactory::createSpecific(const RapidXmlReaderNode& node, const AronNavigatorPath& path) const
+ {
+ AronAbstractTypeXMLReaderData::checkChildSize(node, 0);
+
+ return types::AronAbstractTypeNavigatorPtr(new types::AronBlobTypeNavigator("TODO", path));
+ }*/
}
}
}
diff --git a/source/RobotAPI/libraries/aron/codegenerator/typeReaders/xmlReader/AronAbstractTypeXMLReaderFactory.h b/source/RobotAPI/libraries/aron/codegenerator/typeReaders/xmlReader/AronAbstractTypeXMLReaderFactory.h
index 241b2c209284052b79f49a8bda67705e8fa577fb..a5797edc87fda519976797070a9b615d1ac7fa91 100644
--- a/source/RobotAPI/libraries/aron/codegenerator/typeReaders/xmlReader/AronAbstractTypeXMLReaderFactory.h
+++ b/source/RobotAPI/libraries/aron/codegenerator/typeReaders/xmlReader/AronAbstractTypeXMLReaderFactory.h
@@ -33,7 +33,7 @@
#include <ArmarXCore/core/rapidxml/wrapper/RapidXmlReader.h>
#include <RobotAPI/libraries/aron/AronFactory.h>
-#include <RobotAPI/libraries/aron/types/AronAbstractTypeNavigator.h>
+#include <RobotAPI/libraries/aron/types/AronAllTypeNavigators.h>
using namespace std;
@@ -49,42 +49,48 @@ namespace armarx
typedef std::shared_ptr<AronAbstractTypeXMLReaderFactory> AronAbstractTypeXMLReaderFactoryPtr;
class AronAbstractTypeXMLReaderFactory :
- virtual public AronFactory<RapidXmlReaderNode, types::AronAbstractTypeNavigator>
+ virtual public AronFactory<RapidXmlReaderNode, types::AronAbstractTypeNavigatorPtr>
{
public:
AronAbstractTypeXMLReaderFactory() = default;
virtual types::AronAbstractTypeNavigatorPtr create(const RapidXmlReaderNode&, const AronNavigatorPath&) const override;
- virtual types::AronAbstractTypeNavigatorPtr createSpecific(const RapidXmlReaderNode&, const AronNavigatorPath&) const override;
+ virtual types::AronAbstractTypeNavigatorPtr createSpecific(const RapidXmlReaderNode&, const AronNavigatorPath&) const = 0;
+
+ static std::vector<std::pair<types::AronObjectTypeNavigatorPtr, std::vector<types::AronObjectTypeNavigatorPtr>>> GetAllProducedTopLevelObjects();
+
+ static void AddTopLevelObjectNames(const std::vector<std::string>&);
+ static void AddTopLevelObjectName(const std::string&);
+
+ protected:
+ static std::string CurrentTopLevelObjectName;
+ static std::vector<std::pair<std::string, std::vector<std::string>>> AllTopLevelObjectNames;
+ static std::vector<std::string> AllLocalObjectNames;
+ static std::vector<std::pair<types::AronObjectTypeNavigatorPtr, std::vector<types::AronObjectTypeNavigatorPtr>>> AllTopLevelProducedObjects;
+ static std::vector<types::AronObjectTypeNavigatorPtr> AllLocalProducedObjects;
private:
static const std::map<std::string, AronAbstractTypeXMLReaderFactoryPtr> Factories;
};
- // Object
- class AronObjectTypeXMLReaderFactory :
- virtual public AronAbstractTypeXMLReaderFactory
- {
- public:
- AronObjectTypeXMLReaderFactory() = default;
-
- virtual types::AronAbstractTypeNavigatorPtr createSpecific(const RapidXmlReaderNode&, const AronNavigatorPath&) const override;
- };
+#define RUN_ARON_MACRO(upperType, lowerType, capsType) \
+ class Aron##upperType##TypeXMLReaderFactory : \
+ virtual public AronAbstractTypeXMLReaderFactory \
+ { \
+ public: \
+ Aron##upperType##TypeXMLReaderFactory() = default; \
+ virtual types::AronAbstractTypeNavigatorPtr createSpecific(const RapidXmlReaderNode&, const AronNavigatorPath&) const override; \
+ };
- // List
- class AronListTypeXMLReaderFactory :
- virtual public AronAbstractTypeXMLReaderFactory
- {
- public:
- AronListTypeXMLReaderFactory() = default;
+ HANDLE_CONTAINER_TYPES
+ HANDLE_EXTERNAL_TYPES
+#undef RUN_ARON_MACRO
- virtual types::AronAbstractTypeNavigatorPtr createSpecific(const RapidXmlReaderNode&, const AronNavigatorPath&) const override;
- };
// Primitives
template<typename AronAbstractTypeType, typename AronDataType, typename CppType> //requires AreAronAbstractTypeAndPrimitiveDependent<AronType, AronDataType, CppType>
class AronPrimitiveTypeXMLReaderFactory :
- virtual public AronAbstractTypeXMLReaderFactory
+ virtual public AronAbstractTypeXMLReaderFactory
{
public:
AronPrimitiveTypeXMLReaderFactory() = default;
diff --git a/source/RobotAPI/libraries/aron/types/AronTypeList.h b/source/RobotAPI/libraries/aron/data/AronAllDataNavigators.h
similarity index 68%
rename from source/RobotAPI/libraries/aron/types/AronTypeList.h
rename to source/RobotAPI/libraries/aron/data/AronAllDataNavigators.h
index 57942ccac14cde6842789c9096864794a259e789..8ce6f2eff60aef01aabf3fd6ea1bd02c4aace913 100644
--- a/source/RobotAPI/libraries/aron/types/AronTypeList.h
+++ b/source/RobotAPI/libraries/aron/data/AronAllDataNavigators.h
@@ -23,17 +23,24 @@
#pragma once
+// STD/STL
#include <memory>
+// ArmarX
+#include <RobotAPI/interface/aron.h>
+#include <RobotAPI/libraries/aron/data/AronDataNavigator.h>
+#include <RobotAPI/libraries/aron/data/AronBlobDataNavigator.h>
+#include <RobotAPI/libraries/aron/data/AronDictDataNavigator.h>
+#include <RobotAPI/libraries/aron/data/AronListDataNavigator.h>
+#include <RobotAPI/libraries/aron/data/AronPrimitiveDataNavigator.h>
+
namespace armarx
{
- using TypeListPtr = std::shared_ptr<class TypeList>;
-
- class TypeList
+ namespace aron
{
- public:
- TypeList();
+ namespace data
+ {
- private:
- };
+ }
+ }
}
diff --git a/source/RobotAPI/libraries/aron/data/AronBlobDataNavigator.cpp b/source/RobotAPI/libraries/aron/data/AronBlobDataNavigator.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..0958b148bd9cf34258961ff3d1cc660f704aa1ed
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/data/AronBlobDataNavigator.cpp
@@ -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/>.
+ *
+ * @author Simon Ottenhaus (simon dot ottenhaus at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+// Header
+#include "AronBlobDataNavigator.h"
+
+// ArmarX
+#include <ArmarXCore/core/exceptions/Exception.h>
+#include <RobotAPI/libraries/aron/data/AronDataNavigatorFactory.h>
+
+
+
+namespace armarx
+{
+ namespace aron
+ {
+ namespace data
+ {
+ // constructors
+ AronBlobDataNavigator::AronBlobDataNavigator(const AronNavigatorPath& path) :
+ AronNavigator<AronDataDescriptor, AronData>::AronNavigator(AronDataDescriptor::eAronBlobData),
+ AronDataNavigator(AronDataDescriptor::eAronBlobData, path),
+ aron(new AronBlob())
+ {
+
+ }
+
+ AronBlobDataNavigator::AronBlobDataNavigator(const AronBlobPtr& o, const AronNavigatorPath& path) :
+ AronNavigator<AronDataDescriptor, AronData>::AronNavigator(AronDataDescriptor::eAronBlobData),
+ AronDataNavigator(AronDataDescriptor::eAronBlobData, path),
+ aron(o)
+ {
+
+ }
+
+ // static methods
+ AronBlobDataNavigatorPtr AronBlobDataNavigator::DynamicCast(const AronDataNavigatorPtr& n)
+ {
+ AronBlobDataNavigatorPtr casted = std::dynamic_pointer_cast<AronBlobDataNavigator>(n);
+ checkDataNavigatorPtrForNull(casted, "");
+ return casted;
+ }
+
+ // public member functions
+ unsigned char* AronBlobDataNavigator::getData() const
+ {
+ return aron->data.data();
+ }
+
+ void AronBlobDataNavigator::setData(unsigned int elements, const unsigned char* src)
+ {
+ aron->data = std::vector<unsigned char>(elements);
+ memcpy(aron->data.data(), src, elements);
+ }
+
+ // virtual implementations
+ AronDataPtr AronBlobDataNavigator::getResult() const
+ {
+ return aron;
+ }
+
+ std::vector<AronDataNavigatorPtr> AronBlobDataNavigator::getChildren() const
+ {
+ return {};
+ }
+
+ size_t AronBlobDataNavigator::childrenSize() const
+ {
+ return 0;
+ }
+ }
+ }
+}
diff --git a/source/RobotAPI/libraries/aron/data/AronObjectDataNavigator.h b/source/RobotAPI/libraries/aron/data/AronBlobDataNavigator.h
similarity index 58%
rename from source/RobotAPI/libraries/aron/data/AronObjectDataNavigator.h
rename to source/RobotAPI/libraries/aron/data/AronBlobDataNavigator.h
index 9f4cba9886a5fca1133797fa8806f3aa43754507..8085b82e4c0c58cbb10e1ab2ad9f019cb29d2f78 100644
--- a/source/RobotAPI/libraries/aron/data/AronObjectDataNavigator.h
+++ b/source/RobotAPI/libraries/aron/data/AronBlobDataNavigator.h
@@ -37,26 +37,34 @@ namespace armarx
{
namespace data
{
- class AronObjectDataNavigator;
- typedef std::shared_ptr<AronObjectDataNavigator> AronObjectDataNavigatorPtr;
+ class AronBlobDataNavigator;
+ typedef std::shared_ptr<AronBlobDataNavigator> AronBlobDataNavigatorPtr;
- class AronObjectDataNavigator :
+ class AronBlobDataNavigator :
virtual public AronDataNavigator
{
public:
- using PointerType = AronObjectDataNavigatorPtr;
+ using PointerType = AronBlobDataNavigatorPtr;
- AronObjectDataNavigator(const AronNavigatorPath& path);
- AronObjectDataNavigator(const AronObjectPtr&, const AronNavigatorPath& path);
+ public:
+ // constructors
+ AronBlobDataNavigator(const AronNavigatorPath& path);
+ AronBlobDataNavigator(const AronBlobPtr&, const AronNavigatorPath& path);
+
+ // static methods
+ static AronBlobDataNavigatorPtr DynamicCast(const AronDataNavigatorPtr& n);
- void addElement(const std::string& key, const AronDataNavigatorPtr&);
- AronDataNavigatorPtr getElement(const std::string& key);
- size_t size() const;
+ // public member functions
+ unsigned char* getData() const;
+ void setData(unsigned int, const unsigned char*);
+ // virtual implementations
virtual AronDataPtr getResult() const override;
+ virtual std::vector<AronDataNavigatorPtr> getChildren() const override;
+ virtual size_t childrenSize() const override;
private:
- AronObjectPtr aron;
+ AronBlobPtr aron;
};
}
}
diff --git a/source/RobotAPI/libraries/aron/data/AronDataNavigator.cpp b/source/RobotAPI/libraries/aron/data/AronDataNavigator.cpp
index 8e5f0665b55bac9979b2addffc3fd22849f869b9..ce099bec77a17bdc6e80a3fe50546c726ba926a1 100644
--- a/source/RobotAPI/libraries/aron/data/AronDataNavigator.cpp
+++ b/source/RobotAPI/libraries/aron/data/AronDataNavigator.cpp
@@ -35,18 +35,29 @@ namespace armarx
{
namespace data
{
+ // static data members
const AronDataNavigatorFactoryPtr AronDataNavigator::Factory = AronDataNavigatorFactoryPtr(new AronDataNavigatorFactory());
- AronDataNavigator::AronDataNavigator(const AronNavigatorPath& path) :
- AronNavigator<AronData>(path)
+ // constructors
+ AronDataNavigator::AronDataNavigator(const AronDataDescriptor& descriptor, const AronNavigatorPath& path) :
+ AronNavigator<AronDataDescriptor, AronData>::AronNavigator(descriptor),
+ path(path)
{
-
}
- AronDataNavigatorPtr AronDataNavigator::FromAronDataPtr(const AronDataPtr& a, const AronNavigatorPath& path)
+ // static methods
+ AronDataNavigatorPtr AronDataNavigator::FromAronPtr(const AronDataPtr& a, const AronNavigatorPath& path)
{
return Factory->create(a, path);
}
+
+ void AronDataNavigator::checkDataNavigatorPtrForNull(const AronDataNavigatorPtr& data, const std::string& message)
+ {
+ if (data.get() == nullptr)
+ {
+ throw LocalException("Could not cast an AronDataNavigatorPtr. The Ptr was NULL. " + message);
+ }
+ }
}
}
}
diff --git a/source/RobotAPI/libraries/aron/data/AronDataNavigator.h b/source/RobotAPI/libraries/aron/data/AronDataNavigator.h
index e935ad20889507e52b65a527d8bc3817fd23f2f2..d5778608091fee0bcbaa75ed3662540ae5dae8aa 100644
--- a/source/RobotAPI/libraries/aron/data/AronDataNavigator.h
+++ b/source/RobotAPI/libraries/aron/data/AronDataNavigator.h
@@ -32,6 +32,7 @@
#include <ArmarXCore/core/exceptions/Exception.h>
#include <RobotAPI/interface/aron.h>
#include <RobotAPI/libraries/aron/AronNavigator.h>
+#include <RobotAPI/libraries/aron/AronNavigatorPath.h>
namespace armarx
{
@@ -46,24 +47,46 @@ namespace armarx
typedef std::shared_ptr<AronDataNavigator> AronDataNavigatorPtr;
class AronDataNavigator :
- virtual public AronNavigator<AronData>
+ virtual public AronNavigator<AronDataDescriptor, AronData>
{
public:
using PointerType = AronDataNavigatorPtr;
- AronDataNavigator(const AronNavigatorPath& path);
- virtual AronDataPtr getResult() const override = 0;
+ public:
+ // constructors
+ AronDataNavigator(const AronDataDescriptor&, const AronNavigatorPath&);
+
+ // static methods
+ static AronDataNavigatorPtr FromAronPtr(const AronDataPtr&, const AronNavigatorPath&);
+
+ template<typename AronNavigatorType>
+ static typename AronNavigatorType::PointerType DynamicCast(const AronDataNavigatorPtr& n)
+ {
+ return AronNavigatorType::DynamicCast(n);
+ }
+ // virtual definitions of base class
+ virtual AronDataPtr getResult() const override = 0;
- static AronDataNavigatorPtr FromAronDataPtr(const AronDataPtr&, const AronNavigatorPath&);
+ // virtual definitions
+ virtual std::vector<AronDataNavigatorPtr> getChildren() const = 0;
+ virtual size_t childrenSize() const = 0;
- template<typename AronDataNavigatorType>
- static typename AronDataNavigatorType::PointerType DynamicCast(const AronDataNavigatorPtr& n)
+ // Getter/Setter
+ AronNavigatorPath getPath() const
{
- return std::dynamic_pointer_cast<AronDataNavigatorType>(n);
+ return path;
}
+ std::string pathToString() const
+ {
+ return path.toString();
+ }
+
+ protected:
+ static void checkDataNavigatorPtrForNull(const AronDataNavigatorPtr& data, const std::string& message);
private:
+ AronNavigatorPath path;
static const AronDataNavigatorFactoryPtr Factory;
};
}
diff --git a/source/RobotAPI/libraries/aron/data/AronDataNavigatorFactory.cpp b/source/RobotAPI/libraries/aron/data/AronDataNavigatorFactory.cpp
index 46f4b239a70e0fb0701fbd0cdd1675c96cac5fdd..8f259df8678d7e2f398dc30dedf858d314a6dd86 100644
--- a/source/RobotAPI/libraries/aron/data/AronDataNavigatorFactory.cpp
+++ b/source/RobotAPI/libraries/aron/data/AronDataNavigatorFactory.cpp
@@ -35,30 +35,38 @@ namespace armarx
namespace data
{
- const std::map<AronTypeDescriptor::Value, AronDataNavigatorFactoryPtr> AronDataNavigatorFactory::Factories =
+ // Map types to factories
+ const std::map<AronDataDescriptor, AronDataNavigatorFactoryPtr> AronDataNavigatorFactory::Factories =
{
#define RUN_ARON_MACRO(upperType, lowerType, capsType) \
- {AronTypeDescriptor::GetTypeForAronDataTypeId(typeid(Aron##upperType))->getValue(), AronDataNavigatorFactoryPtr(new Aron##upperType##DataNavigatorFactory())}, \
+ {AronResolver::GetTypeForAronDataTypeId(typeid(Aron##upperType)), AronDataNavigatorFactoryPtr(new Aron##upperType##DataNavigatorFactory())}, \
- HANDLE_CONTAINER_TYPES
+ HANDLE_CONTAINER_DATA
#undef RUN_ARON_MACRO
+ /*#define RUN_ARON_MACRO(upperType, lowerType, capsType) \
+ {AronTypeResolver::GetTypeForAronDataTypeId(typeid(Aron##upperType##Type)), AronDataNavigatorFactoryPtrnew Aron##upperType##TypeNavigatorFactory())}, \
+
+ HANDLE_COMPLEX_DATA
+ #undef RUN_ARON_MACRO*/
+
#define RUN_ARON_MACRO(cppType, upperType, lowerType, capsType) \
- {AronTypeDescriptor::GetTypeForAronDataTypeId(typeid(Aron##upperType))->getValue(), AronDataNavigatorFactoryPtr(new AronPrimitiveDataNavigatorFactory<Aron##upperType, cppType>())}, \
+ {AronResolver::GetTypeForAronDataTypeId(typeid(Aron##upperType)), AronDataNavigatorFactoryPtr(new AronPrimitiveDataNavigatorFactory<Aron##upperType, cppType>())}, \
- HANDLE_PRIMITIVE_TYPES_WITH_CPP_TYPE
+ HANDLE_PRIMITIVE_TYPES_WITH_CPP_TYPE
#undef RUN_ARON_MACRO
};
+ // Access method
AronDataNavigatorPtr AronDataNavigatorFactory::create(const AronDataPtr& aron, const AronNavigatorPath& path) const
{
- if(!aron)
+ if (!aron)
{
throw LocalException("AronDataNavigatorFactory: An AronDataPtr is NULL! Cannot create navigator!");
}
- auto factory_iterator = Factories.find(AronTypeDescriptor::GetTypeForAronData(aron)->getValue());
- if(factory_iterator == Factories.end())
+ auto factory_iterator = Factories.find(AronResolver::GetTypeForAronData(aron));
+ if (factory_iterator == Factories.end())
{
throw LocalException("AronDataNavigatorFactory: Cannot find the desired factory. Cannot create navigator!");
}
@@ -70,17 +78,29 @@ namespace armarx
throw LocalException("AronDataNavigatorFactory: Called disallowed method of an AronDataNavigatorFactory. Use child class instead!");
}
- AronDataNavigatorPtr AronObjectDataNavigatorFactory::createSpecific(const AronDataPtr& aron, const AronNavigatorPath& path) const
- {
- AronObjectPtr aronObject = AronObjectPtr::dynamicCast(aron);
- return AronObjectDataNavigatorPtr(new AronObjectDataNavigator(aronObject, path));
- }
+ // Container Factories
+#define RUN_ARON_MACRO(upperType, lowerType, capsType) \
+ AronDataNavigatorPtr Aron##upperType##DataNavigatorFactory::createSpecific(const AronDataPtr& aron, const AronNavigatorPath& path) const \
+ { \
+ Aron##upperType##Ptr aronCasted = Aron##upperType##Ptr::dynamicCast(aron); \
+ return AronDataNavigatorPtr(new Aron##upperType##DataNavigator(aronCasted, path)); \
+ }
+
+ HANDLE_CONTAINER_DATA
+#undef RUN_ARON_MACRO
+
+
+ // Complex Factories
+#define RUN_ARON_MACRO(upperType, lowerType, capsType) \
+ AronDataNavigatorPtr Aron##upperType##DataNavigatorFactory::createSpecific(const AronDataPtr& aron, const AronNavigatorPath& path) const \
+ { \
+ Aron##upperType##Ptr aronCasted = Aron##upperType##Ptr::dynamicCast(aron); \
+ return AronDataNavigatorPtr(new Aron##upperType##DataNavigator(aronCasted, path)); \
+ }
+
+ HANDLE_COMPLEX_DATA
+#undef RUN_ARON_MACRO
- AronDataNavigatorPtr AronListDataNavigatorFactory::createSpecific(const AronDataPtr& aron, const AronNavigatorPath& path) const
- {
- AronListPtr aronList = AronListPtr::dynamicCast(aron);
- return AronListDataNavigatorPtr(new AronListDataNavigator(aronList, path));
- }
}
}
}
diff --git a/source/RobotAPI/libraries/aron/data/AronDataNavigatorFactory.h b/source/RobotAPI/libraries/aron/data/AronDataNavigatorFactory.h
index 186233043af15b9f9be1ee16ca17c72d31055d8f..f25547e1824d93563ee05ad563e8ece837fb0aff 100644
--- a/source/RobotAPI/libraries/aron/data/AronDataNavigatorFactory.h
+++ b/source/RobotAPI/libraries/aron/data/AronDataNavigatorFactory.h
@@ -30,11 +30,12 @@
// ArmarX
#include <RobotAPI/libraries/aron/AronFactory.h>
#include <RobotAPI/libraries/aron/data/AronDataNavigator.h>
-#include <RobotAPI/libraries/aron/data/AronObjectDataNavigator.h>
#include <RobotAPI/libraries/aron/data/AronListDataNavigator.h>
+#include <RobotAPI/libraries/aron/data/AronDictDataNavigator.h>
+#include <RobotAPI/libraries/aron/data/AronBlobDataNavigator.h>
#include <RobotAPI/libraries/aron/data/AronPrimitiveDataNavigator.h>
-#include <RobotAPI/libraries/aron/AronTypeDescriptor.h>
+#include <RobotAPI/libraries/aron/AronDescriptor.h>
namespace armarx
{
@@ -46,7 +47,7 @@ namespace armarx
typedef std::shared_ptr<AronDataNavigatorFactory> AronDataNavigatorFactoryPtr;
class AronDataNavigatorFactory :
- virtual public AronFactory<AronDataPtr, AronDataNavigator>
+ virtual public AronFactory<AronDataPtr, AronDataNavigatorPtr>
{
public:
AronDataNavigatorFactory() = default;
@@ -54,31 +55,39 @@ namespace armarx
virtual AronDataNavigatorPtr createSpecific(const AronDataPtr&, const AronNavigatorPath&) const override;
private:
- static const std::map<AronTypeDescriptor::Value, AronDataNavigatorFactoryPtr> Factories;
+ static const std::map<AronDataDescriptor, AronDataNavigatorFactoryPtr> Factories;
};
- // Object
- class AronObjectDataNavigatorFactory :
- virtual public AronDataNavigatorFactory
- {
- public:
- AronObjectDataNavigatorFactory() = default;
- virtual AronDataNavigatorPtr createSpecific(const AronDataPtr&, const AronNavigatorPath&) const override;
- };
+ // Container Factories
+#define RUN_ARON_MACRO(upperType, lowerType, capsType) \
+ class Aron##upperType##DataNavigatorFactory : \
+ virtual public AronDataNavigatorFactory \
+ { \
+ public: \
+ Aron##upperType##DataNavigatorFactory() = default; \
+ virtual AronDataNavigatorPtr createSpecific(const AronDataPtr&, const AronNavigatorPath&) const override; \
+ };
- // List
- class AronListDataNavigatorFactory :
- virtual public AronDataNavigatorFactory
- {
- public:
- AronListDataNavigatorFactory() = default;
- virtual AronDataNavigatorPtr createSpecific(const AronDataPtr&, const AronNavigatorPath&) const override;
- };
+ HANDLE_CONTAINER_DATA
+#undef RUN_ARON_MACRO
+
+ // Complex factories
+#define RUN_ARON_MACRO(upperType, lowerType, capsType) \
+ class Aron##upperType##DataNavigatorFactory : \
+ virtual public AronDataNavigatorFactory \
+ { \
+ public: \
+ Aron##upperType##DataNavigatorFactory() = default; \
+ virtual AronDataNavigatorPtr createSpecific(const AronDataPtr&, const AronNavigatorPath&) const override; \
+ };
+
+ HANDLE_COMPLEX_DATA
+#undef RUN_ARON_MACRO
- // Primitive
+ // Primitive Factories
template<typename AronDataType, typename CppType> //requires AreAronPrimitiveDependent<AronDataType, CppType>
class AronPrimitiveDataNavigatorFactory :
- virtual public AronDataNavigatorFactory
+ virtual public AronDataNavigatorFactory
{
public:
AronPrimitiveDataNavigatorFactory() = default;
diff --git a/source/RobotAPI/libraries/aron/data/AronDictDataNavigator.cpp b/source/RobotAPI/libraries/aron/data/AronDictDataNavigator.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..503d257c3b07e863d106fa4794b740bce70a96b5
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/data/AronDictDataNavigator.cpp
@@ -0,0 +1,117 @@
+/*
+ * 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/>.
+ *
+ * @author Simon Ottenhaus (simon dot ottenhaus at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+// Header
+#include "AronDictDataNavigator.h"
+
+// ArmarX
+#include <ArmarXCore/core/exceptions/Exception.h>
+#include <RobotAPI/libraries/aron/data/AronDataNavigatorFactory.h>
+
+namespace armarx
+{
+ namespace aron
+ {
+ namespace data
+ {
+
+ // constructors
+ AronDictDataNavigator::AronDictDataNavigator(const AronNavigatorPath& path) :
+ AronNavigator<AronDataDescriptor, AronData>::AronNavigator(AronDataDescriptor::eAronDictData),
+ AronDataNavigator(AronDataDescriptor::eAronDictData, path),
+ aron(new AronDict())
+ {
+
+ }
+
+ AronDictDataNavigator::AronDictDataNavigator(const AronDictPtr& o, const AronNavigatorPath& path) :
+ AronNavigator<AronDataDescriptor, AronData>::AronNavigator(AronDataDescriptor::eAronDictData),
+ AronDataNavigator(AronDataDescriptor::eAronDictData, path),
+ aron(o)
+ {
+ checkAronPtrForNull(aron, pathToString());
+
+ for (const auto& [key, dataPtr] : aron->elements)
+ {
+ childrenNavigators[key] = FromAronPtr(dataPtr, AronNavigatorPath(path, "[" + key + "]"));
+ }
+ }
+
+ // static methods
+ AronDictDataNavigatorPtr AronDictDataNavigator::DynamicCast(const AronDataNavigatorPtr& n)
+ {
+ AronDictDataNavigatorPtr casted = std::dynamic_pointer_cast<AronDictDataNavigator>(n);
+ checkDataNavigatorPtrForNull(casted, "");
+ return casted;
+ }
+
+ // public member functions
+ std::vector<std::string> AronDictDataNavigator::getAllKeys() const
+ {
+ std::vector<std::string> ret;
+ for (const auto& [key, _] : childrenNavigators)
+ {
+ ret.push_back(key);
+ }
+ return ret;
+ }
+
+ void AronDictDataNavigator::addElement(const std::string& key, const AronDataNavigatorPtr& data)
+ {
+ this->childrenNavigators[key] = data;
+ this->aron->elements[key] = data->getResult();
+ }
+
+ AronDataNavigatorPtr AronDictDataNavigator::getElement(const std::string& key) const
+ {
+ auto it = childrenNavigators.find(key);
+ if (it == childrenNavigators.end())
+ {
+ throw LocalException("AronDictDataNavigator: Could not find key '" + key + "'. The path was: " + pathToString());
+ }
+ return it->second;
+ }
+
+ // virtual implementations
+ AronDataPtr AronDictDataNavigator::getResult() const
+ {
+ return aron;
+ }
+
+ std::vector<AronDataNavigatorPtr> AronDictDataNavigator::getChildren() const
+ {
+ std::vector<AronDataNavigatorPtr> ret(childrenNavigators.size());
+ for (const auto& [key, nav] : childrenNavigators)
+ {
+ ret.push_back(nav);
+ }
+ return ret;
+ }
+
+ size_t AronDictDataNavigator::childrenSize() const
+ {
+ return childrenNavigators.size();
+ }
+ }
+ }
+}
diff --git a/source/RobotAPI/libraries/aron/data/AronDictDataNavigator.h b/source/RobotAPI/libraries/aron/data/AronDictDataNavigator.h
new file mode 100644
index 0000000000000000000000000000000000000000..881346cd04d5c639c8299233511432497b5a10ce
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/data/AronDictDataNavigator.h
@@ -0,0 +1,75 @@
+/*
+ * 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/>.
+ *
+ * @author Simon Ottenhaus (simon dot ottenhaus at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+// STD/STL
+#include <memory>
+#include <map>
+
+// ArmarX
+#include <RobotAPI/interface/aron.h>
+#include <RobotAPI/libraries/aron/data/AronDataNavigator.h>
+#include <RobotAPI/libraries/aron/types/AronDictTypeNavigator.h>
+
+namespace armarx
+{
+ namespace aron
+ {
+ namespace data
+ {
+ class AronDictDataNavigator;
+ typedef std::shared_ptr<AronDictDataNavigator> AronDictDataNavigatorPtr;
+
+ class AronDictDataNavigator :
+ virtual public AronDataNavigator
+ {
+ public:
+ using PointerType = AronDictDataNavigatorPtr;
+
+ public:
+ // constructors
+ AronDictDataNavigator(const AronNavigatorPath& path);
+ AronDictDataNavigator(const AronDictPtr&, const AronNavigatorPath& path);
+
+ // static methods
+ static AronDictDataNavigatorPtr DynamicCast(const AronDataNavigatorPtr& n);
+
+ // public member functions
+ std::vector<std::string> getAllKeys() const;
+ void addElement(const std::string& key, const AronDataNavigatorPtr&);
+ AronDataNavigatorPtr getElement(const std::string&) const;
+
+ // virtual implementations
+ virtual AronDataPtr getResult() const override;
+ virtual std::vector<AronDataNavigatorPtr> getChildren() const override;
+ virtual size_t childrenSize() const override;
+
+ private:
+ // members
+ std::map<std::string, AronDataNavigatorPtr> childrenNavigators;
+ AronDictPtr aron;
+ };
+ }
+ }
+}
diff --git a/source/RobotAPI/libraries/aron/data/AronListDataNavigator.cpp b/source/RobotAPI/libraries/aron/data/AronListDataNavigator.cpp
index c750af7d3fb1b4eac4d5ded112b98fce894c4d92..84bb812d8d12c8014558e109124c104230358641 100644
--- a/source/RobotAPI/libraries/aron/data/AronListDataNavigator.cpp
+++ b/source/RobotAPI/libraries/aron/data/AronListDataNavigator.cpp
@@ -35,42 +35,67 @@ namespace armarx
{
namespace data
{
+ // constructors
AronListDataNavigator::AronListDataNavigator(const AronNavigatorPath& path) :
- AronDataNavigator(path),
+ AronNavigator<AronDataDescriptor, AronData>::AronNavigator(AronDataDescriptor::eAronListData),
+ AronDataNavigator(AronDataDescriptor::eAronListData, path),
aron(new AronList())
{
}
AronListDataNavigator::AronListDataNavigator(const AronListPtr& l, const AronNavigatorPath& path) :
- AronDataNavigator(path),
+ AronNavigator<AronDataDescriptor, AronData>::AronNavigator(AronDataDescriptor::eAronListData),
+ AronDataNavigator(AronDataDescriptor::eAronListData, path),
aron(l)
{
+ checkAronPtrForNull(aron, pathToString());
+ unsigned int i = 0;
+ for (const auto& dataPtr : l->elements)
+ {
+ childrenNavigators.push_back(FromAronPtr(dataPtr, AronNavigatorPath(path, "[" + std::to_string(i++) + "]")));
+ }
+ }
+
+ // static methods
+ AronListDataNavigatorPtr AronListDataNavigator::DynamicCast(const AronDataNavigatorPtr& n)
+ {
+ AronListDataNavigatorPtr casted = std::dynamic_pointer_cast<AronListDataNavigator>(n);
+ checkDataNavigatorPtrForNull(casted, "");
+ return casted;
}
- void AronListDataNavigator::addData(const AronDataNavigatorPtr& n)
+ // public member functions
+ void AronListDataNavigator::addElement(const AronDataNavigatorPtr& n)
{
+ childrenNavigators.push_back(n);
aron->elements.push_back(n->getResult());
}
- AronDataNavigatorPtr AronListDataNavigator::getData(unsigned int i)
+ AronDataNavigatorPtr AronListDataNavigator::getElement(unsigned int i) const
{
- if(i > this->size())
+ if (i >= childrenNavigators.size())
{
throw LocalException("AronListDataNavigator: Index (" + std::to_string(i) + ") out of bounds. The path is " + pathToString());
}
- return FromAronDataPtr(aron->elements[i], AronNavigatorPath());
+ return childrenNavigators[i];
+ }
+
+ // virtual implementations
+ AronDataPtr AronListDataNavigator::getResult() const
+ {
+ return aron;
}
- size_t AronListDataNavigator::size() const
+ std::vector<AronDataNavigatorPtr> AronListDataNavigator::getChildren() const
{
- return aron->elements.size();
+ return childrenNavigators;
}
- AronDataPtr AronListDataNavigator::getResult() const
+ size_t AronListDataNavigator::childrenSize() const
{
- return aron;
+ return childrenNavigators.size();
}
}
}
diff --git a/source/RobotAPI/libraries/aron/data/AronListDataNavigator.h b/source/RobotAPI/libraries/aron/data/AronListDataNavigator.h
index f25a79cc9bc1c0f0f45c5e6dc14404d3f97e53d4..a143dd66438bfe379f1f4382bccf9f59a09edd8d 100644
--- a/source/RobotAPI/libraries/aron/data/AronListDataNavigator.h
+++ b/source/RobotAPI/libraries/aron/data/AronListDataNavigator.h
@@ -29,6 +29,7 @@
// ArmarX
#include <RobotAPI/interface/aron.h>
#include <RobotAPI/libraries/aron/data/AronDataNavigator.h>
+#include <RobotAPI/libraries/aron/types/AronListTypeNavigator.h>
namespace armarx
{
@@ -45,16 +46,25 @@ namespace armarx
public:
using PointerType = AronListDataNavigatorPtr;
+ public:
+ // constructors
AronListDataNavigator(const AronNavigatorPath& path);
AronListDataNavigator(const AronListPtr&, const AronNavigatorPath& path);
- void addData(const AronDataNavigatorPtr&);
- AronDataNavigatorPtr getData(unsigned int);
- size_t size() const;
+ // static methods
+ static AronListDataNavigatorPtr DynamicCast(const AronDataNavigatorPtr& n);
+
+ // public member functions
+ void addElement(const AronDataNavigatorPtr&);
+ AronDataNavigatorPtr getElement(unsigned int) const;
+ // virtual implementations
virtual AronDataPtr getResult() const override;
+ virtual std::vector<AronDataNavigatorPtr> getChildren() const override;
+ virtual size_t childrenSize() const override;
private:
+ std::vector<AronDataNavigatorPtr> childrenNavigators;
AronListPtr aron;
};
}
diff --git a/source/RobotAPI/libraries/aron/data/AronObjectDataNavigator.cpp b/source/RobotAPI/libraries/aron/data/AronObjectDataNavigator.cpp
deleted file mode 100644
index f04baed9f41d0df72bf9cefa28c746e81c598289..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/data/AronObjectDataNavigator.cpp
+++ /dev/null
@@ -1,79 +0,0 @@
-/*
- * 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/>.
- *
- * @author Simon Ottenhaus (simon dot ottenhaus at kit dot edu)
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-// Header
-#include "AronObjectDataNavigator.h"
-
-// ArmarX
-#include <ArmarXCore/core/exceptions/Exception.h>
-#include <RobotAPI/libraries/aron/data/AronDataNavigatorFactory.h>
-
-
-
-namespace armarx
-{
- namespace aron
- {
- namespace data
- {
- AronObjectDataNavigator::AronObjectDataNavigator(const AronNavigatorPath& path) :
- AronDataNavigator(path),
- aron(new AronObject())
- {
-
- }
-
- AronObjectDataNavigator::AronObjectDataNavigator(const AronObjectPtr& o, const AronNavigatorPath& path) :
- AronDataNavigator(path),
- aron(o)
- {
-
- }
-
- void AronObjectDataNavigator::addElement(const std::string& key, const AronDataNavigatorPtr& data)
- {
- this->aron->elements[key] = data->getResult();
- }
-
- AronDataNavigatorPtr AronObjectDataNavigator::getElement(const std::string& key)
- {
- auto it = aron->elements.find(key);
- if(it == aron->elements.end())
- {
- throw LocalException("AronObjectDataNavigator: Could not find key '" + key + "'. The path was: " + pathToString());
- }
- return FromAronDataPtr(aron->elements[key], AronNavigatorPath());
- }
-
- AronDataPtr AronObjectDataNavigator::getResult() const
- {
- return aron;
- }
-
- size_t AronObjectDataNavigator::size() const
- {
- return aron->elements.size();
- }
- }
- }
-}
diff --git a/source/RobotAPI/libraries/aron/data/AronPrimitiveDataNavigator.h b/source/RobotAPI/libraries/aron/data/AronPrimitiveDataNavigator.h
index 9907a9e0f5f3e0876b8ffaf5ab71347b3a5e76cd..f71ca0ac5f6babd16fdeb32d1e00cbca596fd584 100644
--- a/source/RobotAPI/libraries/aron/data/AronPrimitiveDataNavigator.h
+++ b/source/RobotAPI/libraries/aron/data/AronPrimitiveDataNavigator.h
@@ -50,19 +50,32 @@ namespace armarx
public:
using PointerType = AronPrimitiveDataNavigatorPtr<AronDataType, CppType>;
+ public:
+ // constructors
AronPrimitiveDataNavigator(const AronNavigatorPath& path):
- AronDataNavigator(path),
+ AronNavigator<AronDataDescriptor, AronData>::AronNavigator(AronResolver::GetTypeForAronDataTypeId(typeid(AronDataType))),
+ AronDataNavigator(AronResolver::GetTypeForAronDataTypeId(typeid(AronDataType)), path),
aron(new AronDataType())
{
}
AronPrimitiveDataNavigator(const typename AronDataType::PointerType& p, const AronNavigatorPath& path) :
- AronDataNavigator(path),
+ AronNavigator<AronDataDescriptor, AronData>::AronNavigator(AronResolver::GetTypeForAronDataTypeId(typeid(AronDataType))),
+ AronDataNavigator(AronResolver::GetTypeForAronDataTypeId(typeid(AronDataType)), path),
aron(p)
{
+ checkAronPtrForNull(aron, pathToString());
+ }
+ // static methods
+ static AronPrimitiveDataNavigatorPtr<AronDataType, CppType> DynamicCast(const AronDataNavigatorPtr& n)
+ {
+ AronPrimitiveDataNavigatorPtr<AronDataType, CppType> casted = std::dynamic_pointer_cast<AronPrimitiveDataNavigator<AronDataType, CppType>>(n);
+ checkDataNavigatorPtrForNull(casted, "");
+ return casted;
}
+ // public member functions
void setValue(const CppType& x)
{
aron->value = x;
@@ -73,12 +86,24 @@ namespace armarx
return aron->value;
}
+ // virtual implementations
virtual AronDataPtr getResult() const override
{
return aron;
}
+ virtual std::vector<AronDataNavigatorPtr> getChildren() const override
+ {
+ return {};
+ }
+
+ virtual size_t childrenSize() const override
+ {
+ return 0;
+ }
+
private:
+ // members
typename AronDataType::PointerType aron;
};
}
diff --git a/source/RobotAPI/libraries/aron/io/AronReader.h b/source/RobotAPI/libraries/aron/io/AronReader.h
index af73563c282e7c4fb376442990f2e6af3088e093..3cf0b1d863b59ca611286473e0b5226f17963eaf 100644
--- a/source/RobotAPI/libraries/aron/io/AronReader.h
+++ b/source/RobotAPI/libraries/aron/io/AronReader.h
@@ -46,19 +46,26 @@ namespace armarx
AronReader() = default;
#define RUN_ARON_MACRO(upperType, lowerType, capsType) \
- virtual bool readStart##upperType() = 0; \
- virtual bool readEnd##upperType() = 0; \
+ virtual bool readStart##upperType() = 0; \
+ virtual bool readEnd##upperType() = 0; \
- HANDLE_CONTAINER_TYPES
+ HANDLE_CONTAINER_DATA
+#undef RUN_ARON_MACRO
+
+#define RUN_ARON_MACRO(upperType, lowerType, capsType) \
+ virtual unsigned char* read##upperType() = 0; \
+
+ HANDLE_COMPLEX_DATA
#undef RUN_ARON_MACRO
#define RUN_ARON_MACRO(cppType, upperType, lowerType, capsType) \
- virtual cppType read##upperType() = 0; \
+ virtual cppType read##upperType() = 0; \
- HANDLE_PRIMITIVE_TYPES_WITH_CPP_TYPE
+ HANDLE_PRIMITIVE_TYPES_WITH_CPP_TYPE
#undef RUN_ARON_MACRO
- virtual bool readKey(const std::string&) = 0;
+ virtual std::string readKey() = 0;
+ virtual bool readMember(const std::string&) = 0;
};
}
}
diff --git a/source/RobotAPI/libraries/aron/io/AronWriter.h b/source/RobotAPI/libraries/aron/io/AronWriter.h
index 461edc7b2a002b3d5c589164daea3b0a3ad26859..4b21db40cc152f45f5d85cd1aa86a6142b578981 100644
--- a/source/RobotAPI/libraries/aron/io/AronWriter.h
+++ b/source/RobotAPI/libraries/aron/io/AronWriter.h
@@ -46,20 +46,25 @@ namespace armarx
AronWriter() = default;
#define RUN_ARON_MACRO(upperType, lowerType, capsType) \
- virtual bool writeStart##upperType() = 0; \
- virtual bool writeEnd##upperType() = 0; \
+ virtual bool writeStart##upperType() = 0; \
+ virtual bool writeEnd##upperType() = 0; \
- HANDLE_CONTAINER_TYPES
+ HANDLE_CONTAINER_DATA
+#undef RUN_ARON_MACRO
+
+#define RUN_ARON_MACRO(upperType, lowerType, capsType) \
+ virtual bool write##upperType(const unsigned int, const unsigned char*) = 0; \
+
+ HANDLE_COMPLEX_DATA
#undef RUN_ARON_MACRO
#define RUN_ARON_MACRO(cppType, upperType, lowerType, capsType) \
- virtual bool write##upperType(const cppType&) = 0; \
+ virtual bool write##upperType(const cppType&) = 0; \
- HANDLE_PRIMITIVE_TYPES_WITH_CPP_TYPE
+ HANDLE_PRIMITIVE_TYPES_WITH_CPP_TYPE
#undef RUN_ARON_MACRO
virtual bool writeKey(const std::string&) = 0;
-
};
}
}
diff --git a/source/RobotAPI/libraries/aron/io/classReaders/AronDataNavigatorReader/AronDataNavigatorReader.cpp b/source/RobotAPI/libraries/aron/io/classReaders/AronDataNavigatorReader/AronDataNavigatorReader.cpp
index c8eb78c905ce39764c4c710975edb87fec4834c0..0d17764e02dd6806b4fd560783b71cd9b722e37f 100644
--- a/source/RobotAPI/libraries/aron/io/classReaders/AronDataNavigatorReader/AronDataNavigatorReader.cpp
+++ b/source/RobotAPI/libraries/aron/io/classReaders/AronDataNavigatorReader/AronDataNavigatorReader.cpp
@@ -26,7 +26,6 @@
// ArmarX
#include <RobotAPI/libraries/aron/data/AronListDataNavigator.h>
-#include <RobotAPI/libraries/aron/data/AronObjectDataNavigator.h>
#include <RobotAPI/libraries/aron/data/AronPrimitiveDataNavigator.h>
using namespace std;
@@ -40,63 +39,76 @@ namespace armarx
AronDataNavigatorReader::AronDataNavigatorReader(const AronDataPtr& aron) :
AronDataReader<AronDataPtr>(aron),
- input(data::AronDataNavigator::FromAronDataPtr(aron, AronNavigatorPath())),
- readInitialStartObject(false)
+ input(data::AronDataNavigator::FromAronPtr(aron, AronNavigatorPath())),
+ readInitialStart(false)
{
- AronDataNavigatorReaderTokenPtr token = AronDataNavigatorReaderTokenPtr(new AronDataNavigatorReaderToken(AronDataNavigatorReaderToken::AronReaderLastType::ARON_OBJECT, input));
- stack.push(token);
+
}
bool AronDataNavigatorReader::readStartList()
{
+ if (!readInitialStart)
+ {
+ data::AronListDataNavigatorPtr l = data::AronListDataNavigator::DynamicCast(input);
+ AronDataNavigatorReaderTokenPtr token = AronDataNavigatorReaderTokenPtr(new AronDataNavigatorReaderToken(l));
+ stack.push(token);
+
+ readInitialStart = true;
+ return true;
+ }
AronDataNavigatorReaderTokenPtr lastToken = stack.top();
- data::AronListDataNavigatorPtr list = data::AronDataNavigator::DynamicCast<data::AronListDataNavigator>(lastToken->getElement());
+ data::AronListDataNavigatorPtr list = data::AronDataNavigator::DynamicCast<data::AronListDataNavigator>(lastToken->getElementAndIncreaseCnt());
- AronDataNavigatorReaderTokenPtr newToken = AronDataNavigatorReaderTokenPtr(
- new AronDataNavigatorReaderToken(AronDataNavigatorReaderToken::AronReaderLastType::ARON_LIST, list));
+ AronDataNavigatorReaderTokenPtr newToken = AronDataNavigatorReaderTokenPtr(new AronDataNavigatorReaderToken(list));
stack.push(newToken);
return true;
}
- bool AronDataNavigatorReader::readStartObject()
+
+ bool AronDataNavigatorReader::readStartDict()
{
- if(!readInitialStartObject)
+ if (!readInitialStart)
{
- // We skip the first startObject because the obj is already added to the stack
+ data::AronDictDataNavigatorPtr obj = data::AronDictDataNavigator::DynamicCast(input);
+ AronDataNavigatorReaderTokenPtr token = AronDataNavigatorReaderTokenPtr(new AronDataNavigatorReaderToken(obj));
+ stack.push(token);
+
+ readInitialStart = true;
return true;
}
AronDataNavigatorReaderTokenPtr lastToken = stack.top();
- data::AronObjectDataNavigatorPtr object = data::AronDataNavigator::DynamicCast<data::AronObjectDataNavigator>(lastToken->getElement());
- AronDataNavigatorReaderTokenPtr newToken = AronDataNavigatorReaderTokenPtr(
- new AronDataNavigatorReaderToken(AronDataNavigatorReaderToken::AronReaderLastType::ARON_OBJECT, object));
+ data::AronDictDataNavigatorPtr dict = data::AronDataNavigator::DynamicCast<data::AronDictDataNavigator>(lastToken->getElementAndIncreaseCnt());
+
+ AronDataNavigatorReaderTokenPtr newToken = AronDataNavigatorReaderTokenPtr(new AronDataNavigatorReaderToken(dict));
stack.push(newToken);
return true;
}
- bool AronDataNavigatorReader::readEndObject()
+
+ bool AronDataNavigatorReader::readEndList()
{
AronDataNavigatorReaderTokenPtr token = stack.top();
- if(!token->isObject())
+ if (token->getDescriptor() != AronDataDescriptor::eAronListData)
{
- throw LocalException("AronDataNavigatorReader: A token in the stack has the wrong type. Expected Object!");
- return false;
+ throw LocalException("AronDataNavigatorReader: A token in the stack has the wrong type. Expected List!");
}
- if(!token->checkedAllChildren())
+ if (token->checkedAllChildren())
{
- throw LocalException("AronDataNavigatorReader: Try to end an object but we did not access all its children!");
+ stack.pop();
+ return true;
}
- stack.pop();
- return true;
+ return false;
}
- bool AronDataNavigatorReader::readEndList()
+
+ bool AronDataNavigatorReader::readEndDict()
{
AronDataNavigatorReaderTokenPtr token = stack.top();
- if(!token->isList())
+ if (token->getDescriptor() != AronDataDescriptor::eAronDictData)
{
- throw LocalException("AronDataNavigatorReader: A token in the stack has the wrong type. Expected List!");
+ throw LocalException("AronDataNavigatorReader: A token in the stack has the wrong type. Expected Dict!");
return false;
}
- if(token->checkedAllChildren())
+ if (token->checkedAllChildren())
{
stack.pop();
return true;
@@ -104,27 +116,39 @@ namespace armarx
return false;
}
+ // Complex Data
+ unsigned char* AronDataNavigatorReader::readBlob()
+ {
+ AronDataNavigatorReaderTokenPtr token = stack.top();
+ data::AronDataNavigatorPtr nav = token->getElementAndIncreaseCnt();
+ data::AronBlobDataNavigatorPtr casted = data::AronBlobDataNavigator::DynamicCast(nav);
+ return casted->getData();
+ }
+
+ // Read primitives
#define RUN_ARON_MACRO(cppType, upperType, lowerType, capsType) \
cppType AronDataNavigatorReader::read##upperType() \
{ \
AronDataNavigatorReaderTokenPtr token = stack.top(); \
- data::AronDataNavigatorPtr nav = token->getElement(); \
- data::AronPrimitiveDataNavigatorPtr<Aron##upperType, cppType> casted = checkAndCast<data::AronPrimitiveDataNavigator<Aron##upperType, cppType>>(nav); \
+ data::AronDataNavigatorPtr nav = token->getElementAndIncreaseCnt(); \
+ data::AronPrimitiveDataNavigatorPtr<Aron##upperType, cppType> casted = data::AronPrimitiveDataNavigator<Aron##upperType, cppType>::DynamicCast(nav); \
return casted->getValue(); \
}
- HANDLE_PRIMITIVE_TYPES_WITH_CPP_TYPE
+ HANDLE_PRIMITIVE_TYPES_WITH_CPP_TYPE
#undef RUN_ARON_MACRO
- bool AronDataNavigatorReader::readKey(const std::string& k)
+ // ReadKey
+ std::string AronDataNavigatorReader::readKey()
{
AronDataNavigatorReaderTokenPtr token = stack.top();
- if(!token->isObject())
- {
- throw LocalException("AronDataNavigatorReader: A token in the stack has the wrong type. Expected Object!");
- return false;
- }
- token->setKey(k);
+ return token->getCurrentKey();
+ }
+
+ bool AronDataNavigatorReader::readMember(const std::string& k)
+ {
+ AronDataNavigatorReaderTokenPtr token = stack.top();
+ token->addCurrentKey(k);
return true;
}
}
diff --git a/source/RobotAPI/libraries/aron/io/classReaders/AronDataNavigatorReader/AronDataNavigatorReader.h b/source/RobotAPI/libraries/aron/io/classReaders/AronDataNavigatorReader/AronDataNavigatorReader.h
index a9d0268d76ac48736852d07d2fea5590ae9b4604..dab876b9a000ee887bb9373359da6bc06eef85db 100644
--- a/source/RobotAPI/libraries/aron/io/classReaders/AronDataNavigatorReader/AronDataNavigatorReader.h
+++ b/source/RobotAPI/libraries/aron/io/classReaders/AronDataNavigatorReader/AronDataNavigatorReader.h
@@ -44,44 +44,41 @@ namespace armarx
typedef std::shared_ptr<AronDataNavigatorReader> AronDataNavigatorReaderPtr;
class AronDataNavigatorReader :
- virtual public AronDataReader<AronDataPtr>
+ virtual public AronDataReader<AronDataPtr>
{
public:
using PointerType = AronDataNavigatorReaderPtr;
+ public:
+ // constructors
AronDataNavigatorReader(const AronDataPtr& aron);
#define RUN_ARON_MACRO(upperType, lowerType, capsType) \
- virtual bool readStart##upperType() override; \
- virtual bool readEnd##upperType() override; \
+ virtual bool readStart##upperType() override; \
+ virtual bool readEnd##upperType() override; \
- HANDLE_CONTAINER_TYPES
+ HANDLE_CONTAINER_DATA
#undef RUN_ARON_MACRO
-#define RUN_ARON_MACRO(cppType, upperType, lowerType, capsType) \
- virtual cppType read##upperType() override; \
+#define RUN_ARON_MACRO(upperType, lowerType, capsType) \
+ virtual unsigned char* read##upperType() override; \
- HANDLE_PRIMITIVE_TYPES_WITH_CPP_TYPE
+ HANDLE_COMPLEX_DATA
#undef RUN_ARON_MACRO
- virtual bool readKey(const std::string&) override;
+#define RUN_ARON_MACRO(cppType, upperType, lowerType, capsType) \
+ virtual cppType read##upperType() override; \
- private:
- template <typename AronDataNavigatorType>
- typename AronDataNavigatorType::PointerType checkAndCast(const data::AronDataNavigatorPtr& ptr) const
- {
- typename AronDataNavigatorType::PointerType casted = std::dynamic_pointer_cast<AronDataNavigatorType>(ptr);
- if(casted == nullptr)
- {
- throw LocalException("Could not cast a pointer to " + std::string(typeid(AronDataNavigatorType).name()) + ". The result is NULL.");
- }
- return casted;
- }
+ HANDLE_PRIMITIVE_TYPES_WITH_CPP_TYPE
+#undef RUN_ARON_MACRO
+
+ virtual std::string readKey() override;
+ virtual bool readMember(const std::string&) override;
private:
data::AronDataNavigatorPtr input;
std::stack<AronDataNavigatorReaderTokenPtr> stack;
- bool readInitialStartObject;
+ bool readInitialStart;
};
}
}
diff --git a/source/RobotAPI/libraries/aron/io/classReaders/AronDataNavigatorReader/AronDataNavigatorReaderToken.cpp b/source/RobotAPI/libraries/aron/io/classReaders/AronDataNavigatorReader/AronDataNavigatorReaderToken.cpp
index 6f73c4ee3c4f0f3d22788f4cacc0b7a19e19845c..3033ce1dee353d4460fc35a4fd2e1f44d3e74ca0 100644
--- a/source/RobotAPI/libraries/aron/io/classReaders/AronDataNavigatorReader/AronDataNavigatorReaderToken.cpp
+++ b/source/RobotAPI/libraries/aron/io/classReaders/AronDataNavigatorReader/AronDataNavigatorReaderToken.cpp
@@ -31,83 +31,76 @@ namespace armarx
namespace io
{
- AronDataNavigatorReaderToken::AronDataNavigatorReaderToken(const AronReaderLastType t, const data::AronDataNavigatorPtr& data) :
- object(nullptr),
- list(nullptr),
- key(""),
- checked(0),
- type(t)
+ // constructors
+ AronDataNavigatorReaderToken::AronDataNavigatorReaderToken(const data::AronDataNavigatorPtr& data) :
+ data(data),
+ cnt(0),
+ key("")
{
- switch (t)
+ if (getDescriptor() == AronDataDescriptor::eAronDictData)
{
- case ARON_OBJECT:
- object = data::AronDataNavigator::DynamicCast<data::AronObjectDataNavigator>(data);
- break;
- case ARON_LIST:
- list = data::AronDataNavigator::DynamicCast<data::AronListDataNavigator>(data);
- break;
- default:
- throw LocalException("AronDataNavigatorReaderToken: A Token does not have a valid type set.");
+ allKeys = data::AronDictDataNavigator::DynamicCast(data)->getAllKeys();
}
}
- bool AronDataNavigatorReaderToken::isObject() const
+ // public member functions
+ AronDataDescriptor AronDataNavigatorReaderToken::getDescriptor() const
{
- return type == ARON_OBJECT;
- }
- bool AronDataNavigatorReaderToken::isList() const
- {
- return type == ARON_LIST;
+ return data->getDescriptor();
}
- void AronDataNavigatorReaderToken::setKey(const std::string& k)
+ bool AronDataNavigatorReaderToken::checkedAllChildren() const
{
- key = k;
+ return cnt >= data->childrenSize();
}
- bool AronDataNavigatorReaderToken::checkedAllChildren() const
+ // List and Tuple member functions
+ unsigned int AronDataNavigatorReaderToken::getCurrentIndex() const
{
- if(type == ARON_OBJECT)
+ if (getDescriptor() != AronDataDescriptor::eAronListData)
{
- if(checked > object->size())
- {
- throw LocalException("AronDataNavigatorReaderToken: Checked too many children. Cannot guarantee correct shape of object. Abort.");
- }
- return checked == object->size();
- }
- else if(type == ARON_LIST)
- {
- if(checked > list->size())
- {
- throw LocalException("AronDataNavigatorReaderToken: Checked too many elements. Cannot guarantee correct shape of list. Abort.");
- }
- return checked == list->size();
- }
- else
- {
- throw LocalException("AronDataNavigatorReaderToken: A Token does not have a valid type set.");
+ throw LocalException("AronDataNavigatorReaderToken: Cant get index of a non-list token.");
}
+ return cnt;
}
- data::AronDataNavigatorPtr AronDataNavigatorReaderToken::getElement()
+ // Dict member functions
+ std::string AronDataNavigatorReaderToken::getCurrentKey() const
{
- if(checkedAllChildren())
+ if (cnt >= allKeys.size())
{
- throw LocalException("AronDataNavigatorReaderToken: Already checked all children. Cannot get another one. Abort.");
+ throw LocalException("AronDataNavigatorReaderToken: Cant get key at index " + std::to_string(cnt) + " of vector with size " + std::to_string(allKeys.size()) + ".");
}
+ return allKeys[cnt];
+ }
- if(type == ARON_OBJECT)
- {
- checked++;
- return object->getElement(key);
- }
- else if(type == ARON_LIST)
- {
- return list->getData(checked++);
- }
- else
+ // Dict member functions
+ void AronDataNavigatorReaderToken::addCurrentKey(const std::string& k)
+ {
+ allKeys.push_back(k);
+ }
+
+ // virtual definitions
+ data::AronDataNavigatorPtr AronDataNavigatorReaderToken::getElementAndIncreaseCnt()
+ {
+ switch (getDescriptor())
{
- throw LocalException("AronDataNavigatorReaderToken: A Token does not have a valid type set.");
+ case eAronDictData:
+ {
+ data::AronDictDataNavigatorPtr casted = data::AronDictDataNavigator::DynamicCast(data);
+ data::AronDataNavigatorPtr ret = casted->getElement(getCurrentKey());
+ cnt++;
+ return ret;
+ }
+ case eAronListData:
+ {
+ data::AronListDataNavigatorPtr casted = data::AronListDataNavigator::DynamicCast(data);
+ data::AronDataNavigatorPtr ret = casted->getElement(getCurrentIndex());
+ cnt++;
+ return ret;
+ }
+ default:
+ throw LocalException("AronDataNavigatorReaderToken: Could not resove a type of a navigator. Allowed are only container and complex types due to performance.");
}
}
}
diff --git a/source/RobotAPI/libraries/aron/io/classReaders/AronDataNavigatorReader/AronDataNavigatorReaderToken.h b/source/RobotAPI/libraries/aron/io/classReaders/AronDataNavigatorReader/AronDataNavigatorReaderToken.h
index 7aa8bb62680d182320e6a0d0188d6238a659b7f0..63bc723df7199b9ccbdab92a2314c31eb4b0f71a 100644
--- a/source/RobotAPI/libraries/aron/io/classReaders/AronDataNavigatorReader/AronDataNavigatorReaderToken.h
+++ b/source/RobotAPI/libraries/aron/io/classReaders/AronDataNavigatorReader/AronDataNavigatorReaderToken.h
@@ -26,8 +26,9 @@
// ArmarX
#include <RobotAPI/libraries/aron/AronConfig.h>
-#include <RobotAPI/libraries/aron/data/AronObjectDataNavigator.h>
#include <RobotAPI/libraries/aron/data/AronListDataNavigator.h>
+#include <RobotAPI/libraries/aron/data/AronDictDataNavigator.h>
+#include <RobotAPI/libraries/aron/data/AronBlobDataNavigator.h>
using namespace std;
@@ -37,36 +38,40 @@ namespace armarx
{
namespace io
{
- class AronDataNavigatorReaderToken;
- typedef std::shared_ptr<AronDataNavigatorReaderToken> AronDataNavigatorReaderTokenPtr;
+ // BaseClass
+ class AronDataNavigatorReaderToken;
+ typedef std::shared_ptr<AronDataNavigatorReaderToken> AronDataNavigatorReaderTokenPtr;
- class AronDataNavigatorReaderToken
- {
- public:
- using PointerType = AronDataNavigatorReaderTokenPtr;
-
- enum AronReaderLastType
+ class AronDataNavigatorReaderToken
{
- ARON_OBJECT,
- ARON_LIST
- };
+ public:
+ using PointerType = AronDataNavigatorReaderTokenPtr;
- AronDataNavigatorReaderToken(const AronReaderLastType, const data::AronDataNavigatorPtr& data);
+ public:
+ // constructors
+ AronDataNavigatorReaderToken(const data::AronDataNavigatorPtr& data);
- bool isObject() const;
- bool isList() const;
+ // public member functions
+ AronDataDescriptor getDescriptor() const;
+ bool checkedAllChildren() const;
+ data::AronDataNavigatorPtr getElementAndIncreaseCnt();
- void setKey(const std::string& key);
- bool checkedAllChildren() const;
- data::AronDataNavigatorPtr getElement();
+ // List and Tuple member functions
+ unsigned int getCurrentIndex() const;
- private:
- data::AronObjectDataNavigatorPtr object;
- data::AronListDataNavigatorPtr list;
- std::string key;
- unsigned int checked;
- const AronReaderLastType type;
- };
+ // Dict and object member functions
+ void addCurrentKey(const std::string&);
+ std::string getCurrentKey() const;
+
+ private:
+ // members
+ data::AronDataNavigatorPtr data;
+ unsigned int cnt;
+
+ // dict and object members
+ std::string key;
+ std::vector<std::string> allKeys;
+ };
}
}
}
diff --git a/source/RobotAPI/libraries/aron/io/classWriters/AronDataNavigatorWriter/AronDataNavigatorWriter.cpp b/source/RobotAPI/libraries/aron/io/classWriters/AronDataNavigatorWriter/AronDataNavigatorWriter.cpp
index f617819169b892d9faca004c78e09d9bda1410df..dd5d3f6551e5c8d48000670ef4d02de074fac18c 100644
--- a/source/RobotAPI/libraries/aron/io/classWriters/AronDataNavigatorWriter/AronDataNavigatorWriter.cpp
+++ b/source/RobotAPI/libraries/aron/io/classWriters/AronDataNavigatorWriter/AronDataNavigatorWriter.cpp
@@ -26,9 +26,13 @@
// ArmarX
#include <RobotAPI/libraries/aron/data/AronListDataNavigator.h>
-#include <RobotAPI/libraries/aron/data/AronObjectDataNavigator.h>
#include <RobotAPI/libraries/aron/data/AronPrimitiveDataNavigator.h>
+#include <RobotAPI/libraries/aron/types/AronListTypeNavigator.h>
+#include <RobotAPI/libraries/aron/types/AronObjectTypeNavigator.h>
+#include <RobotAPI/libraries/aron/types/AronTupleTypeNavigator.h>
+#include <RobotAPI/libraries/aron/types/AronPrimitiveTypeNavigator.h>
+
using namespace std;
namespace armarx
@@ -38,53 +42,83 @@ namespace armarx
namespace io
{
+ AronDataNavigatorWriter::AronDataNavigatorWriter() :
+ wroteInitialStartContainer(false)
+ {
+
+ }
+
bool AronDataNavigatorWriter::writeStartList()
{
- AronDataNavigatorWriterTokenPtr token = AronDataNavigatorWriterTokenPtr(
- new AronDataNavigatorWriterToken(AronDataNavigatorWriterToken::AronWriterLastType::ARON_LIST));
- stack.push(token);
+ AronNavigatorPath path;
+ if (!wroteInitialStartContainer)
+ {
+ wroteInitialStartContainer = true;
+ }
+ else
+ {
+ AronDataNavigatorWriterTokenPtr token = stack.top();
+ path = AronNavigatorPath(token->getData()->getPath(), token->getAccessor());
+ }
+
+ data::AronDataNavigatorPtr data = data::AronDataNavigatorPtr(new data::AronListDataNavigator(path));
+ AronDataNavigatorWriterTokenPtr token2 = AronDataNavigatorWriterTokenPtr(new AronDataNavigatorWriterToken(data));
+ stack.push(token2);
return true;
}
- bool AronDataNavigatorWriter::writeStartObject()
+
+ bool AronDataNavigatorWriter::writeStartDict()
{
- AronDataNavigatorWriterTokenPtr token = AronDataNavigatorWriterTokenPtr(
- new AronDataNavigatorWriterToken(AronDataNavigatorWriterToken::AronWriterLastType::ARON_OBJECT));
+ AronNavigatorPath path;
+ if (!wroteInitialStartContainer)
+ {
+ wroteInitialStartContainer = true;
+ }
+ else
+ {
+ AronDataNavigatorWriterTokenPtr token = stack.top();
+ path = AronNavigatorPath(token->getData()->getPath(), token->getAccessor());
+ }
+ data::AronDataNavigatorPtr data = data::AronDataNavigatorPtr(new data::AronDictDataNavigator(path));
+ AronDataNavigatorWriterTokenPtr token = AronDataNavigatorWriterTokenPtr(new AronDataNavigatorWriterToken(data));
stack.push(token);
return true;
}
- bool AronDataNavigatorWriter::writeEndObject()
+
+ bool AronDataNavigatorWriter::writeEndList()
{
AronDataNavigatorWriterTokenPtr token = stack.top();
- if(!token->isObject())
- {
- throw LocalException("AronDataNavigatorWriter: A token in the stack has the wrong type. Expected Object!");
- return false;
- }
+ lastRemovedElement = token->getData();
+ stack.pop();
- if(stack.size() > 1) // We do not remove the last object since we want to return it
+ if (stack.size() > 0)
{
- stack.pop();
- data::AronDataNavigatorPtr tokenData = token->getData();
-
AronDataNavigatorWriterTokenPtr prevToken = stack.top();
- prevToken->addElement(tokenData);
+ prevToken->addElement(lastRemovedElement);
}
return true;
}
- bool AronDataNavigatorWriter::writeEndList()
+
+ bool AronDataNavigatorWriter::writeEndDict()
{
AronDataNavigatorWriterTokenPtr token = stack.top();
- if(!token->isList())
+ lastRemovedElement = token->getData();
+ stack.pop();
+
+ if (stack.size() > 0)
{
- throw LocalException("AronDataNavigatorWriter: A token in the stack has the wrong type. Expected List!");
- return false;
+ AronDataNavigatorWriterTokenPtr prevToken = stack.top();
+ prevToken->addElement(lastRemovedElement);
}
+ return true;
+ }
- stack.pop();
- data::AronDataNavigatorPtr tokenData = token->getData();
-
- AronDataNavigatorWriterTokenPtr prevToken = stack.top();
- prevToken->addElement(tokenData);
+ // Complex Types
+ bool AronDataNavigatorWriter::writeBlob(const unsigned int elements, const unsigned char* data)
+ {
+ AronDataNavigatorWriterTokenPtr token = stack.top();
+ data::AronBlobDataNavigatorPtr aron(new data::AronBlobDataNavigator(AronNavigatorPath()));
+ aron->setData(elements, data);
return true;
}
@@ -98,28 +132,23 @@ namespace armarx
return true; \
}
- HANDLE_PRIMITIVE_TYPES_WITH_CPP_TYPE
+ HANDLE_PRIMITIVE_TYPES_WITH_CPP_TYPE
#undef RUN_ARON_MACRO
bool AronDataNavigatorWriter::writeKey(const std::string& k)
{
AronDataNavigatorWriterTokenPtr token = stack.top();
- if(!token->isObject())
- {
- throw LocalException("AronDataNavigatorWriter: A token in the stack has the wrong type. Expected Object!");
- return false;
- }
- token->setKey(k);
+ token->setCurrentKey(k);
return true;
}
AronDataPtr AronDataNavigatorWriter::getResult() const
{
- if(stack.size() != 1)
+ if (lastRemovedElement == nullptr)
{
- throw LocalException("AronDataNavigatorWriter: The stacksize is greater 1. When getting the result the stack size must be 1!");
+ throw LocalException("AronDataNavigatorWriter: Could not return NULL. Perhaps you never called a writeEnd* method?");
}
- return stack.top()->getData()->getResult();
+ return lastRemovedElement->getResult();
}
}
}
diff --git a/source/RobotAPI/libraries/aron/io/classWriters/AronDataNavigatorWriter/AronDataNavigatorWriter.h b/source/RobotAPI/libraries/aron/io/classWriters/AronDataNavigatorWriter/AronDataNavigatorWriter.h
index 28fdb2dd3d2150cb177fbff6623e0b7c42e9790c..977b90e0f4cbf40501a6a76de45ec35006337b1d 100644
--- a/source/RobotAPI/libraries/aron/io/classWriters/AronDataNavigatorWriter/AronDataNavigatorWriter.h
+++ b/source/RobotAPI/libraries/aron/io/classWriters/AronDataNavigatorWriter/AronDataNavigatorWriter.h
@@ -44,27 +44,37 @@ namespace armarx
typedef std::shared_ptr<AronDataNavigatorWriter> AronDataNavigatorWriterPtr;
class AronDataNavigatorWriter :
- virtual public AronDataWriter<AronDataPtr>
+ virtual public AronDataWriter<AronDataPtr>
{
public:
- AronDataNavigatorWriter() = default;
-
- virtual bool writeStartList() override;
- virtual bool writeStartObject() override;
- virtual bool writeEndObject() override;
- virtual bool writeEndList() override;
- virtual bool writeInt(const int &) override;
- virtual bool writeLong(const long &) override;
- virtual bool writeFloat(const float &) override;
- virtual bool writeDouble(const double &) override;
- virtual bool writeString(const std::string &) override;
- virtual bool writeBool(const bool &) override;
- virtual bool writeKey(const std::string&) override;
+ AronDataNavigatorWriter();
+
+#define RUN_ARON_MACRO(upperType, lowerType, capsType) \
+ virtual bool writeStart##upperType() override; \
+ virtual bool writeEnd##upperType() override;
+
+ HANDLE_CONTAINER_DATA
+#undef RUN_ARON_MACRO
+
+#define RUN_ARON_MACRO(upperType, lowerType, capsType) \
+ virtual bool write##upperType(const unsigned int, const unsigned char*) override; \
+
+ HANDLE_COMPLEX_DATA
+#undef RUN_ARON_MACRO
+
+#define RUN_ARON_MACRO(cppType, upperType, lowerType, capsType) \
+ virtual bool write##upperType(const cppType&) override;
+
+ HANDLE_PRIMITIVE_TYPES_WITH_CPP_TYPE
+#undef RUN_ARON_MACRO
+
+ virtual bool writeKey(const std::string&);
virtual AronDataPtr getResult() const override;
private:
- data::AronDataNavigatorPtr data;
+ bool wroteInitialStartContainer;
+ data::AronDataNavigatorPtr lastRemovedElement;
std::stack<AronDataNavigatorWriterTokenPtr> stack;
};
}
diff --git a/source/RobotAPI/libraries/aron/io/classWriters/AronDataNavigatorWriter/AronDataNavigatorWriterToken.cpp b/source/RobotAPI/libraries/aron/io/classWriters/AronDataNavigatorWriter/AronDataNavigatorWriterToken.cpp
index 82da72ad34bf2864f5ad60187f475891a59c33ba..6ffecc3fffc34cbc9617a0ece525a3d435e7045c 100644
--- a/source/RobotAPI/libraries/aron/io/classWriters/AronDataNavigatorWriter/AronDataNavigatorWriterToken.cpp
+++ b/source/RobotAPI/libraries/aron/io/classWriters/AronDataNavigatorWriter/AronDataNavigatorWriterToken.cpp
@@ -30,69 +30,87 @@ namespace armarx
{
namespace io
{
- AronDataNavigatorWriterToken::AronDataNavigatorWriterToken(AronWriterLastType t) :
- object(nullptr),
- list(nullptr),
- key(""),
- type(t)
+ // constructor
+ AronDataNavigatorWriterToken::AronDataNavigatorWriterToken(const data::AronDataNavigatorPtr& data) :
+ data(data),
+ currentIndex(0),
+ currentKey("")
{
- switch (t)
- {
- case ARON_OBJECT:
- object = data::AronObjectDataNavigatorPtr(new data::AronObjectDataNavigator(AronNavigatorPath()));
- break;
- case ARON_LIST:
- list = data::AronListDataNavigatorPtr(new data::AronListDataNavigator(AronNavigatorPath()));
- break;
- default:
- throw LocalException("An AronDataNavigatorToken does not have a valid type set.");
- }
+
}
- bool AronDataNavigatorWriterToken::isObject() const
+ AronDataDescriptor AronDataNavigatorWriterToken::getDescriptor() const
{
- return type == ARON_OBJECT;
+ return data->getDescriptor();
}
- bool AronDataNavigatorWriterToken::isList() const
+
+ void AronDataNavigatorWriterToken::addElement(const data::AronDataNavigatorPtr& n)
{
- return type == ARON_LIST;
+ switch (getDescriptor())
+ {
+ case eAronDictData:
+ {
+ data::AronDictDataNavigatorPtr casted = data::AronDictDataNavigator::DynamicCast(data);
+ casted->addElement(getCurrentKey(), n);
+ break;
+ }
+ case eAronListData:
+ {
+ data::AronListDataNavigatorPtr casted = data::AronListDataNavigator::DynamicCast(data);
+ casted->addElement(n);
+ currentIndex++;
+ break;
+ }
+ default:
+ throw LocalException("AronDataNavigatorWriterToken: Could not resove a type of a navigator. Allowed are only container and complex types due to performance. The path was: " + data->pathToString());
+ }
}
- void AronDataNavigatorWriterToken::setKey(const std::string& k)
+ data::AronDataNavigatorPtr AronDataNavigatorWriterToken::getData() const
{
- key = k;
+ return data;
}
- void AronDataNavigatorWriterToken::addElement(const data::AronDataNavigatorPtr& n)
+ std::string AronDataNavigatorWriterToken::getAccessor() const
{
- if(type == ARON_OBJECT)
- {
- object->addElement(key, n);
- }
- else if(type == ARON_LIST)
- {
- list->addData(n);
- }
- else
+ switch (getDescriptor())
{
- throw LocalException("An AronDataNavigatorToken does not have a valid type set.");
+ case eAronDictData:
+ return currentKey;
+ case eAronListData:
+ return std::to_string(currentIndex);
+ default:
+ throw LocalException("AronDataNavigatorWriterToken: Could not resove a type of a navigator. Allowed are only container and complex types due to performance. The path was: " + data->pathToString());
}
}
- data::AronDataNavigatorPtr AronDataNavigatorWriterToken::getData() const
+ // List and Tuple member functions
+ unsigned int AronDataNavigatorWriterToken::getCurrentIndex() const
{
- if(type == ARON_OBJECT)
+ if (getDescriptor() != AronDataDescriptor::eAronListData)
{
- return object;
+ throw LocalException("AronDataNavigatorWriterToken: Cant get index of a non-list token. The path was: " + data->pathToString());
}
- else if(type == ARON_LIST)
+ return currentIndex;
+ }
+
+ // Dict and object member functions
+ void AronDataNavigatorWriterToken::setCurrentKey(const std::string& k)
+ {
+ if (getDescriptor() != AronDataDescriptor::eAronDictData)
{
- return list;
+ throw LocalException("AronDataNavigatorWriterToken: Cant set key of a non-dict token. The path was: " + data->pathToString());
}
- else
+ currentKey = k;
+ }
+
+ std::string AronDataNavigatorWriterToken::getCurrentKey() const
+ {
+ if (getDescriptor() != AronDataDescriptor::eAronDictData)
{
- throw LocalException("An AronDataNavigatorToken does not have a valid type set.");
+ throw LocalException("AronDataNavigatorWriterToken: Cant get key of a non-dict token. The path was: " + data->pathToString());
}
+ return currentKey;
}
}
}
diff --git a/source/RobotAPI/libraries/aron/io/classWriters/AronDataNavigatorWriter/AronDataNavigatorWriterToken.h b/source/RobotAPI/libraries/aron/io/classWriters/AronDataNavigatorWriter/AronDataNavigatorWriterToken.h
index 9254e7d3ffc59114a7e2d45f8cedb7dd7d34b49c..454d8b886384e8a7a81501f1a99c6c2307eb8499 100644
--- a/source/RobotAPI/libraries/aron/io/classWriters/AronDataNavigatorWriter/AronDataNavigatorWriterToken.h
+++ b/source/RobotAPI/libraries/aron/io/classWriters/AronDataNavigatorWriter/AronDataNavigatorWriterToken.h
@@ -26,8 +26,9 @@
// ArmarX
#include <RobotAPI/libraries/aron/AronConfig.h>
-#include <RobotAPI/libraries/aron/data/AronObjectDataNavigator.h>
#include <RobotAPI/libraries/aron/data/AronListDataNavigator.h>
+#include <RobotAPI/libraries/aron/data/AronDictDataNavigator.h>
+#include <RobotAPI/libraries/aron/data/AronBlobDataNavigator.h>
namespace armarx
{
@@ -43,26 +44,32 @@ namespace armarx
public:
using PointerType = AronDataNavigatorWriterTokenPtr;
- enum AronWriterLastType
- {
- ARON_OBJECT,
- ARON_LIST
- };
-
- AronDataNavigatorWriterToken(AronWriterLastType);
-
- bool isObject() const;
- bool isList() const;
+ public:
+ // constructor
+ AronDataNavigatorWriterToken(const data::AronDataNavigatorPtr&);
- void setKey(const std::string& key);
+ // public member functions
+ AronDataDescriptor getDescriptor() const;
void addElement(const data::AronDataNavigatorPtr&);
data::AronDataNavigatorPtr getData() const;
+ std::string getAccessor() const;
+
+ // List and Tuple member functions
+ unsigned int getCurrentIndex() const;
+
+ // Dict and object member functions
+ void setCurrentKey(const std::string&);
+ std::string getCurrentKey() const;
private:
- data::AronObjectDataNavigatorPtr object;
- data::AronListDataNavigatorPtr list;
- std::string key;
- const AronWriterLastType type;
+ // members
+ data::AronDataNavigatorPtr data;
+
+ // list index
+ unsigned int currentIndex;
+
+ // dict members
+ std::string currentKey;
};
}
}
diff --git a/source/RobotAPI/libraries/aron/test/AronTypes.h b/source/RobotAPI/libraries/aron/test/AronTypes.h
index 2dba4e6faf2fc44e740a9cc731035a696e50f164..9d40b5d56087ee9d3830267eef521d93780a835b 100644
--- a/source/RobotAPI/libraries/aron/test/AronTypes.h
+++ b/source/RobotAPI/libraries/aron/test/AronTypes.h
@@ -6,59 +6,106 @@
#include <map>
#include <Eigen/Core>
#include <RobotAPI/interface/aron.h>
-#include <RobotAPI/libraries/aron/io/AronWriter.h>
-#include <RobotAPI/libraries/aron/io/AronReader.h>
+#include <RobotAPI/libraries/aron/codegenerator/AronCppClass.h>
#include <RobotAPI/libraries/aron/io/classWriters/AronDataNavigatorWriter/AronDataNavigatorWriter.h>
#include <RobotAPI/libraries/aron/io/classReaders/AronDataNavigatorReader/AronDataNavigatorReader.h>
namespace armarx
{
class NaturalIKResult
+ : virtual public armarx::aron::codegeneration::AronCppClass
{
public:
std::vector<float> jointValues;
bool reached;
+
+ public:
NaturalIKResult()
{
+ reset();
}
public:
- void write(armarx::aron::io::AronWriter& w) const
+ /**
+ * @brief reset() - This method resets all member variables to default.
+ * @return - nothing
+ */
+ virtual void reset() override
+ {
+ jointValues.clear();
+ reached = {};
+ }
+ /**
+ * @brief reset() - This method sets special base-class members.
+ * @return - nothing
+ */
+ virtual void setup() override
{
- w.writeStartObject();
+ // setup aronType
+ std::map<std::string, armarx::aron::AronAbstractType::PointerType> aronTypeRoot_objectiteratorcontainer;
+ armarx::aron::AronFloatType::PointerType aronTypeRoot_jointValues_listtype = armarx::aron::AronFloatType::PointerType(new armarx::aron::AronFloatType("float"));
+ armarx::aron::AronListType::PointerType aronTypeRoot_jointValues = armarx::aron::AronListType::PointerType(new armarx::aron::AronListType("std::vector<float>", aronTypeRoot_jointValues_listtype));
+ aronTypeRoot_objectiteratorcontainer["jointValues"] = (aronTypeRoot_jointValues);
+ armarx::aron::AronBoolType::PointerType aronTypeRoot_reached = armarx::aron::AronBoolType::PointerType(new armarx::aron::AronBoolType("bool"));
+ aronTypeRoot_objectiteratorcontainer["reached"] = (aronTypeRoot_reached);
+ armarx::aron::AronObjectType::PointerType aronTypeRoot = armarx::aron::AronObjectType::PointerType(new armarx::aron::AronObjectType("armarx::NaturalIKResult", aronTypeRoot_objectiteratorcontainer));
+ aronType = aronTypeRoot;
+ }
+ /**
+ * @brief read() - This method returns a new type from the member data types using a writer implementation.
+ * @param w - The writer implementation
+ * @return - the result of the writer implementation
+ */
+ virtual void write(armarx::aron::io::AronWriter& w) const override
+ {
+ w.writeStartDict();
w.writeKey("jointValues");
w.writeStartList();
- for (unsigned int jointValues_index = 0; jointValues_index < jointValues.size(); ++jointValues_index)
+ for (unsigned int jointValues_listiterator = 0; jointValues_listiterator < jointValues.size(); ++jointValues_listiterator)
{
- w.writeFloat(jointValues[jointValues_index]);
+ w.writeFloat(jointValues[jointValues_listiterator]);
}
w.writeEndList();
w.writeKey("reached");
w.writeBool(reached);
- w.writeEndObject();
+ w.writeEndDict();
}
- void read(armarx::aron::io::AronReader& r)
+ /**
+ * @brief read() - This method sets the struct members to new values given in a reader implementation.
+ * @param r - The reader implementation
+ * @return - nothing
+ */
+ virtual void read(armarx::aron::io::AronReader& r) override
{
- r.readStartObject();
- r.readKey("jointValues");
+ reset();
+
+ r.readStartDict();
+ r.readMember("jointValues");
r.readStartList();
- jointValues.clear();
while (!r.readEndList())
{
- float jointValues_iterator = 0;
- jointValues_iterator = r.readFloat();
- jointValues.push_back(jointValues_iterator);
+ float jointValues_listiterator;
+ jointValues_listiterator = r.readFloat();
+ jointValues.push_back(jointValues_listiterator);
}
- r.readKey("reached");
+ r.readMember("reached");
reached = r.readBool();
- r.readEndObject();
+ r.readEndDict();
}
+ /**
+ * @brief specializedWrite() - This method returns a new type from the member data types using a writer implementation.
+ * @return - the result of the writer implementation
+ */
armarx::aron::AronDataPtr toAron() const
{
armarx::aron::io::AronDataNavigatorWriter writer;
this->write(writer);
return writer.getResult();
}
+ /**
+ * @brief specializedRead() - This method sets the struct members to new values given in a reader implementation.
+ * @return - nothing
+ */
void fromAron(const armarx::aron::AronDataPtr& input)
{
armarx::aron::io::AronDataNavigatorReader reader(input);
@@ -68,3 +115,4 @@ namespace armarx
} // namespace armarx
#endif // ARONTESTPRODUCER_ARON_TYPE_DEFINITION_INCLUDE_GUARD
+
diff --git a/source/RobotAPI/libraries/aron/test/aronTest.cpp b/source/RobotAPI/libraries/aron/test/aronTest.cpp
index 553672b30ff3fd32371d287e236eaa0f7b40fd8a..db02761681325ad3816923a7f0094e9252de72bb 100644
--- a/source/RobotAPI/libraries/aron/test/aronTest.cpp
+++ b/source/RobotAPI/libraries/aron/test/aronTest.cpp
@@ -26,6 +26,8 @@
// STD/STL
#include <iostream>
+#include <cstdlib>
+#include <ctime>
// Boost
#include <boost/algorithm/string.hpp>
@@ -38,71 +40,325 @@
#include <ArmarXCore/libraries/cppgen/CppClass.h>
// Generated File
-#include "AronTypes.h"
+#include "generated/NaturalIK.h"
+#include "generated/ListTest.h"
+#include "generated/DictTest.h"
+#include "generated/PrimitiveTest.h"
+#include "generated/ObjectTest.h"
+#include <Eigen/Core>
+
using namespace armarx;
using namespace aron;
-BOOST_AUTO_TEST_CASE(testExample)
+int generateRandom(int max, int min)
{
- NaturalIKResult k;
- k.reached = true;
- k.jointValues = std::vector<float>({1.0f, 2.0f, 3.0f});
+ std::srand(std::time(nullptr));
+ int random = std::rand() % max + min;
+ return random;
+}
+
+std::string generateRandomWord()
+{
+ vector<string> words = {"Lorem", "ipsum", "dolor", "sit", "amet", "consetetur", "sadipscing", "elitr"
+ "sed", "diam", "nonumy", "eirmod", "tempor", "invidunt", "ut", "labore", "et"
+ "dolore", "magna", "aliquyam", "eratsed"
+ };
+
+ int i = generateRandom(words.size(), 0);
+ return words[i];
+}
- // check toAron
+AronDataPtr generateAronDataFromType(const AronAbstractTypePtr& type)
+{
+ {
+ AronObjectTypePtr t = AronObjectTypePtr::dynamicCast(type);
+ if (t)
+ {
+ AronDictPtr d = AronDictPtr(new AronDict());
+ for (const auto& [k, tt] : t->elementTypes)
+ {
+ d->elements[k] = generateAronDataFromType(tt);
+ }
+ return d;
+ }
+ }
+ {
+ AronDictTypePtr t = AronDictTypePtr::dynamicCast(type);
+ if (t)
+ {
+ return new AronDict();
+ }
+ }
+ {
+ AronListTypePtr t = AronListTypePtr::dynamicCast(type);
+ if (t)
+ {
+ return new AronList();
+ }
+ }
+ {
+ AronTupleTypePtr t = AronTupleTypePtr::dynamicCast(type);
+ if (t)
+ {
+ return new AronList();
+ }
+ }
+ {
+ AronEigenMatrixTypePtr t = AronEigenMatrixTypePtr::dynamicCast(type);
+ if (t)
+ {
+ return new AronBlob();
+ }
+ }
{
- AronDataPtr aronNaturalIKResultData = k.toAron();
- AronObjectPtr aronNaturalIKResult = AronObjectPtr::dynamicCast(aronNaturalIKResultData);
- BOOST_CHECK_NE(aronNaturalIKResult.get(), nullptr);
+ AronIntTypePtr t = AronIntTypePtr::dynamicCast(type);
+ if (t)
+ {
+ return new AronInt();
+ }
+ }
+ {
+ AronLongTypePtr t = AronLongTypePtr::dynamicCast(type);
+ if (t)
+ {
+ return new AronLong();
+ }
+ }
+ {
+ AronFloatTypePtr t = AronFloatTypePtr::dynamicCast(type);
+ if (t)
+ {
+ return new AronFloat();
+ }
+ }
+ {
+ AronDoubleTypePtr t = AronDoubleTypePtr::dynamicCast(type);
+ if (t)
+ {
+ return new AronDouble();
+ }
+ }
+ {
+ AronStringTypePtr t = AronStringTypePtr::dynamicCast(type);
+ if (t)
+ {
+ return new AronString();
+ }
+ }
+ {
+ AronBoolTypePtr t = AronBoolTypePtr::dynamicCast(type);
+ if (t)
+ {
+ return new AronBool();
+ }
+ }
+ throw LocalException("generateAronDataFromType: No valid type found!");
+}
- AronDataPtr aronReachedData = aronNaturalIKResult->elements["reached"];
- AronBoolPtr aronReached = AronBoolPtr::dynamicCast(aronReachedData);
- BOOST_CHECK_NE(aronReached.get(), nullptr);
- BOOST_CHECK_EQUAL(aronReached->value, true);
+void initializeRandomly(AronDataPtr& data, const AronAbstractTypePtr& type)
+{
+ BOOST_CHECK_NE(data.get(), nullptr);
+ BOOST_CHECK_NE(type.get(), nullptr);
- AronDataPtr aronJointValuesData = aronNaturalIKResult->elements["jointValues"];
- AronListPtr aronJointValues = AronListPtr::dynamicCast(aronJointValuesData);
- BOOST_CHECK_NE(aronJointValues.get(), nullptr);
- BOOST_CHECK_EQUAL(aronJointValues->elements.size(), k.jointValues.size());
+ // Containers
+ {
+ AronObjectTypePtr objectType = AronObjectTypePtr::dynamicCast(type);
+ if (objectType)
+ {
+ AronDictPtr dict = AronDictPtr::dynamicCast(data);
+ BOOST_CHECK_NE(dict.get(), nullptr);
+ for (auto& [key, nextData] : dict->elements)
+ {
+ initializeRandomly(nextData, objectType->elementTypes[key]);
+ }
+ return;
+ }
+ }
+
+ {
+ AronListTypePtr listType = AronListTypePtr::dynamicCast(type);
+ if (listType)
+ {
+ AronListPtr list = AronListPtr::dynamicCast(data);
+ BOOST_CHECK_NE(list.get(), nullptr);
+
+ list->elements.clear();
+ int numElements = generateRandom(26, 0);
+ for (int i = 0; i < numElements; ++i)
+ {
+ AronDataPtr newData = generateAronDataFromType(listType->acceptedType);
+ initializeRandomly(newData, listType->acceptedType);
+ list->elements.push_back(newData);
+ }
+ return;
+ }
+ }
- for (unsigned int i = 0; i < k.jointValues.size(); ++i)
+ {
+ AronDictTypePtr dictType = AronDictTypePtr::dynamicCast(type);
+ if (dictType)
{
- const AronDataPtr& aronJointValuesElementData = aronJointValues->elements[i];
- AronFloatPtr aronJointValuesElement = AronFloatPtr::dynamicCast(aronJointValuesElementData);
- BOOST_CHECK_NE(aronJointValuesElement.get(), nullptr);
- BOOST_CHECK_EQUAL(aronJointValuesElement->value, k.jointValues[i]);
+ AronDictPtr dict = AronDictPtr::dynamicCast(data);
+ BOOST_CHECK_NE(dict.get(), nullptr);
+
+ dict->elements.clear();
+ int numElements = generateRandom(26, 0);
+ for (int i = 0; i < numElements; ++i)
+ {
+ std::string key = generateRandomWord();
+ AronDataPtr newData = generateAronDataFromType(dictType->acceptedType);
+ initializeRandomly(newData, dictType->acceptedType);
+ dict->elements[key] = newData;
+ }
+ return;
}
}
+ {
+ AronTupleTypePtr tupleType = AronTupleTypePtr::dynamicCast(type);
+ if (tupleType)
+ {
+ AronListPtr list = AronListPtr::dynamicCast(data);
+ BOOST_CHECK_NE(list.get(), nullptr);
+ BOOST_CHECK_EQUAL(list->elements.size(), tupleType->elementTypes.size());
+
+ int i = 0;
+ for (auto& nextData : list->elements)
+ {
+ initializeRandomly(nextData, tupleType->elementTypes[i++]);
+ }
+ return;
+ }
+ }
- // check fromAron
+ // TODO Complex
{
- AronObjectPtr aronNaturalIKResult = AronObjectPtr(new AronObject());
- AronBoolPtr aronReached = AronBoolPtr(new AronBool());
- aronReached->value = false;
- aronNaturalIKResult->elements["reached"] = aronReached;
+ AronEigenMatrixTypePtr EigenMatrixType = AronEigenMatrixTypePtr::dynamicCast(type);
+ if (EigenMatrixType)
+ {
+ AronBlobPtr blob = AronBlobPtr::dynamicCast(data);
+ BOOST_CHECK_NE(blob.get(), nullptr);
+ return;
+ }
+ }
+
- AronListPtr aronJointValues = AronListPtr(new AronList());
- for (unsigned int i = 4; i < 8; ++i)
+ // Primitives
+ {
+ AronIntTypePtr primitiveType = AronIntTypePtr::dynamicCast(type);
+ if (primitiveType)
{
- AronFloatPtr aronJointValuesElement = AronFloatPtr(new AronFloat());
- aronJointValuesElement->value = 1.0f * i;
+ AronIntPtr primitiveData = AronIntPtr::dynamicCast(data);
+ BOOST_CHECK_NE(primitiveData.get(), nullptr);
- aronJointValues->elements.push_back(aronJointValuesElement);
+ primitiveData->value = generateRandom(123456789, -123456789);
+ return;
}
- aronNaturalIKResult->elements["jointValues"] = aronJointValues;
+ }
+ {
+ AronLongTypePtr primitiveType = AronLongTypePtr::dynamicCast(type);
+ if (primitiveType)
+ {
+ AronLongPtr primitiveData = AronLongPtr::dynamicCast(data);
+ BOOST_CHECK_NE(primitiveData.get(), nullptr);
- k.fromAron(aronNaturalIKResult);
- BOOST_CHECK_EQUAL(k.reached, false);
+ primitiveData->value = generateRandom(123456789, -123456789);
+ return;
+ }
+ }
+ {
+ AronFloatTypePtr primitiveType = AronFloatTypePtr::dynamicCast(type);
+ if (primitiveType)
+ {
+ AronFloatPtr primitiveData = AronFloatPtr::dynamicCast(data);
+ BOOST_CHECK_NE(primitiveData.get(), nullptr);
- BOOST_CHECK_EQUAL(k.jointValues.size(), 4);
- for (unsigned int i = 4; i < 8; ++i)
+ primitiveData->value = generateRandom(123456789, -123456789);
+ return;
+ }
+ }
+ {
+ AronDoubleTypePtr primitiveType = AronDoubleTypePtr::dynamicCast(type);
+ if (primitiveType)
+ {
+ AronDoublePtr primitiveData = AronDoublePtr::dynamicCast(data);
+ BOOST_CHECK_NE(primitiveData.get(), nullptr);
+
+ primitiveData->value = generateRandom(123456789, -123456789);
+ return;
+ }
+ }
+ {
+ AronStringTypePtr primitiveType = AronStringTypePtr::dynamicCast(type);
+ if (primitiveType)
+ {
+ AronStringPtr primitiveData = AronStringPtr::dynamicCast(data);
+ BOOST_CHECK_NE(primitiveData.get(), nullptr);
+
+ primitiveData->value = generateRandomWord();
+ return;
+ }
+ }
+ {
+ AronBoolTypePtr primitiveType = AronBoolTypePtr::dynamicCast(type);
+ if (primitiveType)
{
- std::cout << "found values: " << k.jointValues[i - 4] << std::endl;
- BOOST_CHECK_EQUAL(k.jointValues[i - 4], 1.0f * i);
+ AronBoolPtr primitiveData = AronBoolPtr::dynamicCast(data);
+ BOOST_CHECK_NE(primitiveData.get(), nullptr);
+
+ primitiveData->value = generateRandom(1, 0);
+ return;
}
}
+ throw LocalException("initializeRandomly: No valid type found!");
+}
+
+template <typename T>
+void runTestWithInstances(T& k, T& k2)
+{
+ AronAbstractTypePtr k_type = k.getAronAbstractType();
+ BOOST_CHECK_NE(k_type.get(), nullptr);
+
+ AronAbstractTypePtr k2_type = k2.getAronAbstractType();
+ BOOST_CHECK_NE(k_type.get(), nullptr);
+
+ AronDataPtr k_aron = k.toAron();
+ BOOST_CHECK_NE(k_aron.get(), nullptr);
+ initializeRandomly(k_aron, k_type);
+
+ k2.fromAron(k_aron);
+ AronDataPtr k2_aron = k2.toAron();
+ BOOST_CHECK_NE(k2_aron.get(), nullptr);
+
+ k.fromAron(k2_aron);
+ BOOST_CHECK_EQUAL((k == k2), true);
+}
+
+BOOST_AUTO_TEST_CASE(AronTests)
+{
+ NaturalIKResult k;
+ NaturalIKResult k2;
+ runTestWithInstances<NaturalIKResult>(k, k2);
+
+ ListTest l;
+ ListTest l2;
+ runTestWithInstances<ListTest>(l, l2);
+
+ DictTest d;
+ DictTest d2;
+ runTestWithInstances<DictTest>(d, d2);
+
+ PrimitiveTest p;
+ PrimitiveTest p2;
+ runTestWithInstances<PrimitiveTest>(p, p2);
+
+ SimpleClass o1;
+ SimpleClass o12;
+ runTestWithInstances<SimpleClass>(o1, o12);
- BOOST_CHECK_EQUAL(true, true);
+ ObjectTest o2;
+ ObjectTest o22;
+ runTestWithInstances<ObjectTest>(o2, o22);
}
diff --git a/source/RobotAPI/libraries/aron/test/generated/DictTest.h b/source/RobotAPI/libraries/aron/test/generated/DictTest.h
new file mode 100644
index 0000000000000000000000000000000000000000..47247cd011ea77c1ce70c0b32af5ad6ca728fe16
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/test/generated/DictTest.h
@@ -0,0 +1,152 @@
+/*
+ * 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/>.
+ *
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ *************************************************************************
+ * WARNING: This file is autogenerated.
+ * Original file: /home/fabian/Software/ArmarX/RobotAPI/source/RobotAPI/libraries/aron/test/xmls/DictTest.xml
+ * Please do not edit since your changes may be overwritten on the next generation
+ * If you have any questions please contact: Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
+ *************************************************************************
+ */
+
+
+#ifndef ARONTESTSEGMENT__DICTTEST__ARON_TYPE_DEFINITION_INCLUDE_GUARD
+#define ARONTESTSEGMENT__DICTTEST__ARON_TYPE_DEFINITION_INCLUDE_GUARD
+
+#include <string>
+#include <vector>
+#include <map>
+#include <Eigen/Core>
+#include <RobotAPI/interface/aron.h>
+#include <RobotAPI/libraries/aron/codegenerator/AronCppClass.h>
+#include <RobotAPI/libraries/aron/io/classWriters/AronDataNavigatorWriter/AronDataNavigatorWriter.h>
+#include <RobotAPI/libraries/aron/io/classReaders/AronDataNavigatorReader/AronDataNavigatorReader.h>
+
+namespace armarx
+{
+ class DictTest
+ : virtual public armarx::aron::codegeneration::AronCppClass
+ {
+ public:
+ std::map<std::string, float> dict;
+
+ public:
+ DictTest()
+ {
+ initialize();
+ reset();
+ }
+
+ public:
+ /**
+ * @brief operator==() - This method checks whether all values equal another instance.
+ * @param i - The other instance
+ * @return - true, if all members are the same, false otherwise
+ */
+ bool operator==(const armarx::DictTest& i) const
+ {
+ if ( not (dict == i.dict))
+ return false;
+ return true;
+ }
+ /**
+ * @brief reset() - This method resets all member variables to default.
+ * @return - nothing
+ */
+ virtual void reset() override
+ {
+ dict.clear();
+ }
+ /**
+ * @brief initialize() - This method initializeses special base-class members.
+ * @return - nothing
+ */
+ virtual void initialize() override
+ {
+ // setup aronType
+ std::map<std::string, armarx::aron::AronAbstractType::PointerType> aronTypeRoot_objectiteratorcontainer;
+ armarx::aron::AronFloatType::PointerType aronTypeRoot_dict_dicttype = armarx::aron::AronFloatType::PointerType(new armarx::aron::AronFloatType());
+ armarx::aron::AronDictType::PointerType aronTypeRoot_dict = armarx::aron::AronDictType::PointerType(new armarx::aron::AronDictType(aronTypeRoot_dict_dicttype));
+ aronTypeRoot_objectiteratorcontainer["dict"] = aronTypeRoot_dict;
+ armarx::aron::AronObjectType::PointerType aronTypeRoot = armarx::aron::AronObjectType::PointerType(new armarx::aron::AronObjectType("armarx::DictTest", aronTypeRoot_objectiteratorcontainer));
+ aronType = aronTypeRoot;
+ }
+ /**
+ * @brief write() - This method returns a new type from the member data types using a writer implementation.
+ * @param w - The writer implementation
+ * @return - the result of the writer implementation
+ */
+ virtual void write(armarx::aron::io::AronWriter& w) const override
+ {
+ w.writeStartDict();
+ w.writeKey("dict");
+ w.writeStartDict();
+ for (const auto& [dict_dictkey, dict_dictval] : dict)
+ {
+ w.writeKey(dict_dictkey);
+ w.writeFloat(dict_dictval);
+ }
+ w.writeEndDict();
+ w.writeEndDict();
+ }
+ /**
+ * @brief read() - This method sets the struct members to new values given in a reader implementation.
+ * @param r - The reader implementation
+ * @return - nothing
+ */
+ virtual void read(armarx::aron::io::AronReader& r) override
+ {
+ reset();
+
+ r.readStartDict();
+ r.readMember("dict");
+ r.readStartDict();
+ while(!r.readEndDict())
+ {
+ std::string dict_dictkey = r.readKey();
+ float dict_dictiterator;
+ dict_dictiterator = r.readFloat();
+ dict[dict_dictkey] = dict_dictiterator;
+ }
+ r.readEndDict();
+ }
+ /**
+ * @brief specializedWrite() - This method returns a new type from the member data types using a writer implementation.
+ * @return - the result of the writer implementation
+ */
+ armarx::aron::AronDataPtr toAron() const
+ {
+ armarx::aron::io::AronDataNavigatorWriter writer;
+ this->write(writer);
+ return writer.getResult();
+ }
+ /**
+ * @brief specializedRead() - This method sets the struct members to new values given in a reader implementation.
+ * @return - nothing
+ */
+ void fromAron(const armarx::aron::AronDataPtr& input)
+ {
+ armarx::aron::io::AronDataNavigatorReader reader(input);
+ this->read(reader);
+ }
+ }; // class DictTest
+} // namespace armarx
+
+#endif // ARONTESTSEGMENT__DICTTEST__ARON_TYPE_DEFINITION_INCLUDE_GUARD
diff --git a/source/RobotAPI/libraries/aron/test/generated/ListTest.h b/source/RobotAPI/libraries/aron/test/generated/ListTest.h
new file mode 100644
index 0000000000000000000000000000000000000000..e325c483e728f39b2b400c1524389b8a7096c852
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/test/generated/ListTest.h
@@ -0,0 +1,351 @@
+/*
+ * 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/>.
+ *
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ *************************************************************************
+ * WARNING: This file is autogenerated.
+ * Original file: /home/fabian/Software/ArmarX/RobotAPI/source/RobotAPI/libraries/aron/test/xmls/ListTest.xml
+ * Please do not edit since your changes may be overwritten on the next generation
+ * If you have any questions please contact: Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
+ *************************************************************************
+ */
+
+
+#ifndef ARONTESTSEGMENT__LISTTEST__ARON_TYPE_DEFINITION_INCLUDE_GUARD
+#define ARONTESTSEGMENT__LISTTEST__ARON_TYPE_DEFINITION_INCLUDE_GUARD
+
+#include <string>
+#include <vector>
+#include <map>
+#include <Eigen/Core>
+#include <RobotAPI/interface/aron.h>
+#include <RobotAPI/libraries/aron/codegenerator/AronCppClass.h>
+#include <RobotAPI/libraries/aron/io/classWriters/AronDataNavigatorWriter/AronDataNavigatorWriter.h>
+#include <RobotAPI/libraries/aron/io/classReaders/AronDataNavigatorReader/AronDataNavigatorReader.h>
+
+namespace armarx
+{
+ class ListTest
+ : virtual public armarx::aron::codegeneration::AronCppClass
+ {
+ protected:
+ class ListClass
+ {
+ public:
+ bool element0;
+ bool element1;
+ bool element2;
+
+ public:
+ /**
+ * @brief operator==() - This method checks whether all values equal another instance.
+ * @param i - The other instance
+ * @return - true, if all members are the same, false otherwise
+ */
+ bool operator==(const ListClass& i) const
+ {
+ if ( not (element0 == i.element0))
+ return false;
+ if ( not (element1 == i.element1))
+ return false;
+ if ( not (element2 == i.element2))
+ return false;
+ return true;
+ }
+ }; // class ListClass
+
+ public:
+ std::vector<bool> anotherboolList;
+ std::vector<bool> boolList;
+ std::vector<double> doubleList;
+ std::vector<float> floatList;
+ std::vector<int> intList;
+ std::vector<long> longList;
+ std::vector<ListClass> objectList;
+ std::vector<std::string> stringList;
+
+ public:
+ ListTest()
+ {
+ initialize();
+ reset();
+ }
+
+ public:
+ /**
+ * @brief operator==() - This method checks whether all values equal another instance.
+ * @param i - The other instance
+ * @return - true, if all members are the same, false otherwise
+ */
+ bool operator==(const armarx::ListTest& i) const
+ {
+ if ( not (anotherboolList == i.anotherboolList))
+ return false;
+ if ( not (boolList == i.boolList))
+ return false;
+ if ( not (doubleList == i.doubleList))
+ return false;
+ if ( not (floatList == i.floatList))
+ return false;
+ if ( not (intList == i.intList))
+ return false;
+ if ( not (longList == i.longList))
+ return false;
+ if ( not (objectList == i.objectList))
+ return false;
+ if ( not (stringList == i.stringList))
+ return false;
+ return true;
+ }
+ /**
+ * @brief reset() - This method resets all member variables to default.
+ * @return - nothing
+ */
+ virtual void reset() override
+ {
+ anotherboolList.clear();
+ boolList.clear();
+ doubleList.clear();
+ floatList.clear();
+ intList.clear();
+ longList.clear();
+ objectList.clear();
+ stringList.clear();
+ }
+ /**
+ * @brief initialize() - This method initializeses special base-class members.
+ * @return - nothing
+ */
+ virtual void initialize() override
+ {
+ // setup aronType
+ std::map<std::string, armarx::aron::AronAbstractType::PointerType> aronTypeRoot_objectiteratorcontainer;
+ armarx::aron::AronBoolType::PointerType aronTypeRoot_anotherboolList_listtype = armarx::aron::AronBoolType::PointerType(new armarx::aron::AronBoolType());
+ armarx::aron::AronListType::PointerType aronTypeRoot_anotherboolList = armarx::aron::AronListType::PointerType(new armarx::aron::AronListType(aronTypeRoot_anotherboolList_listtype));
+ aronTypeRoot_objectiteratorcontainer["anotherboolList"] = aronTypeRoot_anotherboolList;
+ armarx::aron::AronBoolType::PointerType aronTypeRoot_boolList_listtype = armarx::aron::AronBoolType::PointerType(new armarx::aron::AronBoolType());
+ armarx::aron::AronListType::PointerType aronTypeRoot_boolList = armarx::aron::AronListType::PointerType(new armarx::aron::AronListType(aronTypeRoot_boolList_listtype));
+ aronTypeRoot_objectiteratorcontainer["boolList"] = aronTypeRoot_boolList;
+ armarx::aron::AronDoubleType::PointerType aronTypeRoot_doubleList_listtype = armarx::aron::AronDoubleType::PointerType(new armarx::aron::AronDoubleType());
+ armarx::aron::AronListType::PointerType aronTypeRoot_doubleList = armarx::aron::AronListType::PointerType(new armarx::aron::AronListType(aronTypeRoot_doubleList_listtype));
+ aronTypeRoot_objectiteratorcontainer["doubleList"] = aronTypeRoot_doubleList;
+ armarx::aron::AronFloatType::PointerType aronTypeRoot_floatList_listtype = armarx::aron::AronFloatType::PointerType(new armarx::aron::AronFloatType());
+ armarx::aron::AronListType::PointerType aronTypeRoot_floatList = armarx::aron::AronListType::PointerType(new armarx::aron::AronListType(aronTypeRoot_floatList_listtype));
+ aronTypeRoot_objectiteratorcontainer["floatList"] = aronTypeRoot_floatList;
+ armarx::aron::AronIntType::PointerType aronTypeRoot_intList_listtype = armarx::aron::AronIntType::PointerType(new armarx::aron::AronIntType());
+ armarx::aron::AronListType::PointerType aronTypeRoot_intList = armarx::aron::AronListType::PointerType(new armarx::aron::AronListType(aronTypeRoot_intList_listtype));
+ aronTypeRoot_objectiteratorcontainer["intList"] = aronTypeRoot_intList;
+ armarx::aron::AronLongType::PointerType aronTypeRoot_longList_listtype = armarx::aron::AronLongType::PointerType(new armarx::aron::AronLongType());
+ armarx::aron::AronListType::PointerType aronTypeRoot_longList = armarx::aron::AronListType::PointerType(new armarx::aron::AronListType(aronTypeRoot_longList_listtype));
+ aronTypeRoot_objectiteratorcontainer["longList"] = aronTypeRoot_longList;
+ std::map<std::string, armarx::aron::AronAbstractType::PointerType> aronTypeRoot_objectList_listtype_objectiteratorcontainer;
+ armarx::aron::AronBoolType::PointerType aronTypeRoot_objectList_listtype_element0 = armarx::aron::AronBoolType::PointerType(new armarx::aron::AronBoolType());
+ aronTypeRoot_objectList_listtype_objectiteratorcontainer["element0"] = aronTypeRoot_objectList_listtype_element0;
+ armarx::aron::AronBoolType::PointerType aronTypeRoot_objectList_listtype_element1 = armarx::aron::AronBoolType::PointerType(new armarx::aron::AronBoolType());
+ aronTypeRoot_objectList_listtype_objectiteratorcontainer["element1"] = aronTypeRoot_objectList_listtype_element1;
+ armarx::aron::AronBoolType::PointerType aronTypeRoot_objectList_listtype_element2 = armarx::aron::AronBoolType::PointerType(new armarx::aron::AronBoolType());
+ aronTypeRoot_objectList_listtype_objectiteratorcontainer["element2"] = aronTypeRoot_objectList_listtype_element2;
+ armarx::aron::AronObjectType::PointerType aronTypeRoot_objectList_listtype = armarx::aron::AronObjectType::PointerType(new armarx::aron::AronObjectType("ListClass", aronTypeRoot_objectList_listtype_objectiteratorcontainer));
+ armarx::aron::AronListType::PointerType aronTypeRoot_objectList = armarx::aron::AronListType::PointerType(new armarx::aron::AronListType(aronTypeRoot_objectList_listtype));
+ aronTypeRoot_objectiteratorcontainer["objectList"] = aronTypeRoot_objectList;
+ armarx::aron::AronStringType::PointerType aronTypeRoot_stringList_listtype = armarx::aron::AronStringType::PointerType(new armarx::aron::AronStringType());
+ armarx::aron::AronListType::PointerType aronTypeRoot_stringList = armarx::aron::AronListType::PointerType(new armarx::aron::AronListType(aronTypeRoot_stringList_listtype));
+ aronTypeRoot_objectiteratorcontainer["stringList"] = aronTypeRoot_stringList;
+ armarx::aron::AronObjectType::PointerType aronTypeRoot = armarx::aron::AronObjectType::PointerType(new armarx::aron::AronObjectType("armarx::ListTest", aronTypeRoot_objectiteratorcontainer));
+ aronType = aronTypeRoot;
+ }
+ /**
+ * @brief write() - This method returns a new type from the member data types using a writer implementation.
+ * @param w - The writer implementation
+ * @return - the result of the writer implementation
+ */
+ virtual void write(armarx::aron::io::AronWriter& w) const override
+ {
+ w.writeStartDict();
+ w.writeKey("anotherboolList");
+ w.writeStartList();
+ for(unsigned int anotherboolList_listiterator = 0; anotherboolList_listiterator < anotherboolList.size(); ++anotherboolList_listiterator)
+ {
+ w.writeBool(anotherboolList[anotherboolList_listiterator]);
+ }
+ w.writeEndList();
+ w.writeKey("boolList");
+ w.writeStartList();
+ for(unsigned int boolList_listiterator = 0; boolList_listiterator < boolList.size(); ++boolList_listiterator)
+ {
+ w.writeBool(boolList[boolList_listiterator]);
+ }
+ w.writeEndList();
+ w.writeKey("doubleList");
+ w.writeStartList();
+ for(unsigned int doubleList_listiterator = 0; doubleList_listiterator < doubleList.size(); ++doubleList_listiterator)
+ {
+ w.writeDouble(doubleList[doubleList_listiterator]);
+ }
+ w.writeEndList();
+ w.writeKey("floatList");
+ w.writeStartList();
+ for(unsigned int floatList_listiterator = 0; floatList_listiterator < floatList.size(); ++floatList_listiterator)
+ {
+ w.writeFloat(floatList[floatList_listiterator]);
+ }
+ w.writeEndList();
+ w.writeKey("intList");
+ w.writeStartList();
+ for(unsigned int intList_listiterator = 0; intList_listiterator < intList.size(); ++intList_listiterator)
+ {
+ w.writeInt(intList[intList_listiterator]);
+ }
+ w.writeEndList();
+ w.writeKey("longList");
+ w.writeStartList();
+ for(unsigned int longList_listiterator = 0; longList_listiterator < longList.size(); ++longList_listiterator)
+ {
+ w.writeLong(longList[longList_listiterator]);
+ }
+ w.writeEndList();
+ w.writeKey("objectList");
+ w.writeStartList();
+ for(unsigned int objectList_listiterator = 0; objectList_listiterator < objectList.size(); ++objectList_listiterator)
+ {
+ w.writeStartDict();
+ w.writeKey("element0");
+ w.writeBool(objectList[objectList_listiterator].element0);
+ w.writeKey("element1");
+ w.writeBool(objectList[objectList_listiterator].element1);
+ w.writeKey("element2");
+ w.writeBool(objectList[objectList_listiterator].element2);
+ w.writeEndDict();
+ }
+ w.writeEndList();
+ w.writeKey("stringList");
+ w.writeStartList();
+ for(unsigned int stringList_listiterator = 0; stringList_listiterator < stringList.size(); ++stringList_listiterator)
+ {
+ w.writeString(stringList[stringList_listiterator]);
+ }
+ w.writeEndList();
+ w.writeEndDict();
+ }
+ /**
+ * @brief read() - This method sets the struct members to new values given in a reader implementation.
+ * @param r - The reader implementation
+ * @return - nothing
+ */
+ virtual void read(armarx::aron::io::AronReader& r) override
+ {
+ reset();
+
+ r.readStartDict();
+ r.readMember("anotherboolList");
+ r.readStartList();
+ while(!r.readEndList())
+ {
+ bool anotherboolList_listiterator;
+ anotherboolList_listiterator = r.readBool();
+ anotherboolList.push_back(anotherboolList_listiterator);
+ }
+ r.readMember("boolList");
+ r.readStartList();
+ while(!r.readEndList())
+ {
+ bool boolList_listiterator;
+ boolList_listiterator = r.readBool();
+ boolList.push_back(boolList_listiterator);
+ }
+ r.readMember("doubleList");
+ r.readStartList();
+ while(!r.readEndList())
+ {
+ double doubleList_listiterator;
+ doubleList_listiterator = r.readDouble();
+ doubleList.push_back(doubleList_listiterator);
+ }
+ r.readMember("floatList");
+ r.readStartList();
+ while(!r.readEndList())
+ {
+ float floatList_listiterator;
+ floatList_listiterator = r.readFloat();
+ floatList.push_back(floatList_listiterator);
+ }
+ r.readMember("intList");
+ r.readStartList();
+ while(!r.readEndList())
+ {
+ int intList_listiterator;
+ intList_listiterator = r.readInt();
+ intList.push_back(intList_listiterator);
+ }
+ r.readMember("longList");
+ r.readStartList();
+ while(!r.readEndList())
+ {
+ long longList_listiterator;
+ longList_listiterator = r.readLong();
+ longList.push_back(longList_listiterator);
+ }
+ r.readMember("objectList");
+ r.readStartList();
+ while(!r.readEndList())
+ {
+ ListClass objectList_listiterator;
+ r.readStartDict();
+ r.readMember("element0");
+ objectList_listiterator.element0 = r.readBool();
+ r.readMember("element1");
+ objectList_listiterator.element1 = r.readBool();
+ r.readMember("element2");
+ objectList_listiterator.element2 = r.readBool();
+ r.readEndDict();
+ objectList.push_back(objectList_listiterator);
+ }
+ r.readMember("stringList");
+ r.readStartList();
+ while(!r.readEndList())
+ {
+ std::string stringList_listiterator;
+ stringList_listiterator = r.readString();
+ stringList.push_back(stringList_listiterator);
+ }
+ r.readEndDict();
+ }
+ /**
+ * @brief specializedWrite() - This method returns a new type from the member data types using a writer implementation.
+ * @return - the result of the writer implementation
+ */
+ armarx::aron::AronDataPtr toAron() const
+ {
+ armarx::aron::io::AronDataNavigatorWriter writer;
+ this->write(writer);
+ return writer.getResult();
+ }
+ /**
+ * @brief specializedRead() - This method sets the struct members to new values given in a reader implementation.
+ * @return - nothing
+ */
+ void fromAron(const armarx::aron::AronDataPtr& input)
+ {
+ armarx::aron::io::AronDataNavigatorReader reader(input);
+ this->read(reader);
+ }
+ }; // class ListTest
+} // namespace armarx
+
+#endif // ARONTESTSEGMENT__LISTTEST__ARON_TYPE_DEFINITION_INCLUDE_GUARD
diff --git a/source/RobotAPI/libraries/aron/test/generated/NaturalIK.h b/source/RobotAPI/libraries/aron/test/generated/NaturalIK.h
new file mode 100644
index 0000000000000000000000000000000000000000..9298574ba3d2438941fb320624576be0f0f8bc34
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/test/generated/NaturalIK.h
@@ -0,0 +1,160 @@
+/*
+ * 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/>.
+ *
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ *************************************************************************
+ * WARNING: This file is autogenerated.
+ * Original file: /home/fabian/Software/ArmarX/RobotAPI/source/RobotAPI/libraries/aron/test/xmls/NaturalIK.xml
+ * Please do not edit since your changes may be overwritten on the next generation
+ * If you have any questions please contact: Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
+ *************************************************************************
+ */
+
+
+#ifndef ARONTESTSEGMENT__NATURALIKRESULT__ARON_TYPE_DEFINITION_INCLUDE_GUARD
+#define ARONTESTSEGMENT__NATURALIKRESULT__ARON_TYPE_DEFINITION_INCLUDE_GUARD
+
+#include <string>
+#include <vector>
+#include <map>
+#include <Eigen/Core>
+#include <RobotAPI/interface/aron.h>
+#include <RobotAPI/libraries/aron/codegenerator/AronCppClass.h>
+#include <RobotAPI/libraries/aron/io/classWriters/AronDataNavigatorWriter/AronDataNavigatorWriter.h>
+#include <RobotAPI/libraries/aron/io/classReaders/AronDataNavigatorReader/AronDataNavigatorReader.h>
+
+namespace armarx
+{
+ class NaturalIKResult
+ : virtual public armarx::aron::codegeneration::AronCppClass
+ {
+ public:
+ std::vector<float> jointValues;
+ bool reached;
+
+ public:
+ NaturalIKResult()
+ {
+ initialize();
+ reset();
+ }
+
+ public:
+ /**
+ * @brief operator==() - This method checks whether all values equal another instance.
+ * @param i - The other instance
+ * @return - true, if all members are the same, false otherwise
+ */
+ bool operator==(const armarx::NaturalIKResult& i) const
+ {
+ if ( not (jointValues == i.jointValues))
+ return false;
+ if ( not (reached == i.reached))
+ return false;
+ return true;
+ }
+ /**
+ * @brief reset() - This method resets all member variables to default.
+ * @return - nothing
+ */
+ virtual void reset() override
+ {
+ jointValues.clear();
+ reached = {};
+ }
+ /**
+ * @brief initialize() - This method initializeses special base-class members.
+ * @return - nothing
+ */
+ virtual void initialize() override
+ {
+ // setup aronType
+ std::map<std::string, armarx::aron::AronAbstractType::PointerType> aronTypeRoot_objectiteratorcontainer;
+ armarx::aron::AronFloatType::PointerType aronTypeRoot_jointValues_listtype = armarx::aron::AronFloatType::PointerType(new armarx::aron::AronFloatType());
+ armarx::aron::AronListType::PointerType aronTypeRoot_jointValues = armarx::aron::AronListType::PointerType(new armarx::aron::AronListType(aronTypeRoot_jointValues_listtype));
+ aronTypeRoot_objectiteratorcontainer["jointValues"] = aronTypeRoot_jointValues;
+ armarx::aron::AronBoolType::PointerType aronTypeRoot_reached = armarx::aron::AronBoolType::PointerType(new armarx::aron::AronBoolType());
+ aronTypeRoot_objectiteratorcontainer["reached"] = aronTypeRoot_reached;
+ armarx::aron::AronObjectType::PointerType aronTypeRoot = armarx::aron::AronObjectType::PointerType(new armarx::aron::AronObjectType("armarx::NaturalIKResult", aronTypeRoot_objectiteratorcontainer));
+ aronType = aronTypeRoot;
+ }
+ /**
+ * @brief write() - This method returns a new type from the member data types using a writer implementation.
+ * @param w - The writer implementation
+ * @return - the result of the writer implementation
+ */
+ virtual void write(armarx::aron::io::AronWriter& w) const override
+ {
+ w.writeStartDict();
+ w.writeKey("jointValues");
+ w.writeStartList();
+ for(unsigned int jointValues_listiterator = 0; jointValues_listiterator < jointValues.size(); ++jointValues_listiterator)
+ {
+ w.writeFloat(jointValues[jointValues_listiterator]);
+ }
+ w.writeEndList();
+ w.writeKey("reached");
+ w.writeBool(reached);
+ w.writeEndDict();
+ }
+ /**
+ * @brief read() - This method sets the struct members to new values given in a reader implementation.
+ * @param r - The reader implementation
+ * @return - nothing
+ */
+ virtual void read(armarx::aron::io::AronReader& r) override
+ {
+ reset();
+
+ r.readStartDict();
+ r.readMember("jointValues");
+ r.readStartList();
+ while(!r.readEndList())
+ {
+ float jointValues_listiterator;
+ jointValues_listiterator = r.readFloat();
+ jointValues.push_back(jointValues_listiterator);
+ }
+ r.readMember("reached");
+ reached = r.readBool();
+ r.readEndDict();
+ }
+ /**
+ * @brief specializedWrite() - This method returns a new type from the member data types using a writer implementation.
+ * @return - the result of the writer implementation
+ */
+ armarx::aron::AronDataPtr toAron() const
+ {
+ armarx::aron::io::AronDataNavigatorWriter writer;
+ this->write(writer);
+ return writer.getResult();
+ }
+ /**
+ * @brief specializedRead() - This method sets the struct members to new values given in a reader implementation.
+ * @return - nothing
+ */
+ void fromAron(const armarx::aron::AronDataPtr& input)
+ {
+ armarx::aron::io::AronDataNavigatorReader reader(input);
+ this->read(reader);
+ }
+ }; // class NaturalIKResult
+} // namespace armarx
+
+#endif // ARONTESTSEGMENT__NATURALIKRESULT__ARON_TYPE_DEFINITION_INCLUDE_GUARD
diff --git a/source/RobotAPI/libraries/aron/test/generated/ObjectTest.h b/source/RobotAPI/libraries/aron/test/generated/ObjectTest.h
new file mode 100644
index 0000000000000000000000000000000000000000..09fc9ac3fd0215122885d69afd86d917b39a49f8
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/test/generated/ObjectTest.h
@@ -0,0 +1,340 @@
+/*
+ * 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/>.
+ *
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ *************************************************************************
+ * WARNING: This file is autogenerated
+ * Original file: /home/fabian/Software/ArmarX/RobotAPI/source/RobotAPI/libraries/aron/test/xmls/ObjectTest.xml
+ * Timestamp of generation: Tue Jul 28 18:11:50 2020
+ * Please do not edit since your changes may be overwritten on the next generation
+ * If you have any questions please contact: Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
+ *************************************************************************
+ */
+
+
+#ifndef ARONTESTSEGMENT__SIMPLECLASS__ARON_TYPE_DEFINITION_INCLUDE_GUARD
+#define ARONTESTSEGMENT__SIMPLECLASS__ARON_TYPE_DEFINITION_INCLUDE_GUARD
+
+#include <string>
+#include <vector>
+#include <map>
+#include <Eigen/Core>
+#include <RobotAPI/interface/aron.h>
+#include <RobotAPI/libraries/aron/codegenerator/AronCppClass.h>
+#include <RobotAPI/libraries/aron/io/classWriters/AronDataNavigatorWriter/AronDataNavigatorWriter.h>
+#include <RobotAPI/libraries/aron/io/classReaders/AronDataNavigatorReader/AronDataNavigatorReader.h>
+
+namespace armarx
+{
+ class SimpleClass
+ : virtual public armarx::aron::codegeneration::AronCppClass
+ {
+ public:
+ bool the_bool;
+
+ public:
+ SimpleClass()
+ {
+ initialize();
+ reset();
+ }
+
+ public:
+ /**
+ * @brief operator==() - This method checks whether all values equal another instance.
+ * @param i - The other instance
+ * @return - true, if all members are the same, false otherwise
+ */
+ bool operator==(const armarx::SimpleClass& i) const
+ {
+ if ( not (the_bool == i.the_bool))
+ return false;
+ return true;
+ }
+ /**
+ * @brief reset() - This method resets all member variables to default.
+ * @return - nothing
+ */
+ virtual void reset() override
+ {
+ the_bool = {};
+ }
+ /**
+ * @brief initialize() - This method initializeses special base-class members.
+ * @return - nothing
+ */
+ virtual void initialize() override
+ {
+ // setup aronType
+ std::map<std::string, armarx::aron::AronAbstractType::PointerType> aronTypeRoot_objectiteratorcontainer;
+ armarx::aron::AronBoolType::PointerType aronTypeRoot_the_bool = armarx::aron::AronBoolType::PointerType(new armarx::aron::AronBoolType());
+ aronTypeRoot_objectiteratorcontainer["the_bool"] = aronTypeRoot_the_bool;
+ armarx::aron::AronObjectType::PointerType aronTypeRoot = armarx::aron::AronObjectType::PointerType(new armarx::aron::AronObjectType("armarx::SimpleClass", aronTypeRoot_objectiteratorcontainer));
+ aronType = aronTypeRoot;
+ }
+ /**
+ * @brief write() - This method returns a new type from the member data types using a writer implementation.
+ * @param w - The writer implementation
+ * @return - the result of the writer implementation
+ */
+ virtual void write(armarx::aron::io::AronWriter& w) const override
+ {
+ w.writeStartDict();
+ w.writeKey("the_bool");
+ w.writeBool(the_bool);
+ w.writeEndDict();
+ }
+ /**
+ * @brief read() - This method sets the struct members to new values given in a reader implementation.
+ * @param r - The reader implementation
+ * @return - nothing
+ */
+ virtual void read(armarx::aron::io::AronReader& r) override
+ {
+ reset();
+
+ r.readStartDict();
+ r.readMember("the_bool");
+ the_bool = r.readBool();
+ r.readEndDict();
+ }
+ /**
+ * @brief specializedWrite() - This method returns a new type from the member data types using a writer implementation.
+ * @return - the result of the writer implementation
+ */
+ armarx::aron::AronDataPtr toAron() const
+ {
+ armarx::aron::io::AronDataNavigatorWriter writer;
+ this->write(writer);
+ return writer.getResult();
+ }
+ /**
+ * @brief specializedRead() - This method sets the struct members to new values given in a reader implementation.
+ * @return - nothing
+ */
+ void fromAron(const armarx::aron::AronDataPtr& input)
+ {
+ armarx::aron::io::AronDataNavigatorReader reader(input);
+ this->read(reader);
+ }
+ }; // class SimpleClass
+} // namespace armarx
+
+#endif // ARONTESTSEGMENT__SIMPLECLASS__ARON_TYPE_DEFINITION_INCLUDE_GUARD
+
+// *************
+// Here comes another auto-generated class. Don't mind the duplicated includes - everything is guarded!
+// *************
+
+#ifndef ARONTESTSEGMENT__OBJECTTEST__ARON_TYPE_DEFINITION_INCLUDE_GUARD
+#define ARONTESTSEGMENT__OBJECTTEST__ARON_TYPE_DEFINITION_INCLUDE_GUARD
+
+#include <string>
+#include <vector>
+#include <map>
+#include <Eigen/Core>
+#include <RobotAPI/interface/aron.h>
+#include <RobotAPI/libraries/aron/codegenerator/AronCppClass.h>
+#include <RobotAPI/libraries/aron/io/classWriters/AronDataNavigatorWriter/AronDataNavigatorWriter.h>
+#include <RobotAPI/libraries/aron/io/classReaders/AronDataNavigatorReader/AronDataNavigatorReader.h>
+
+namespace armarx
+{
+ class ObjectTest
+ : virtual public armarx::aron::codegeneration::AronCppClass
+ {
+ protected:
+ class OtherFancyObject
+ {
+ public:
+ bool blarg;
+ int other_blarg;
+
+ public:
+ /**
+ * @brief operator==() - This method checks whether all values equal another instance.
+ * @param i - The other instance
+ * @return - true, if all members are the same, false otherwise
+ */
+ bool operator==(const OtherFancyObject& i) const
+ {
+ if ( not (blarg == i.blarg))
+ return false;
+ if ( not (other_blarg == i.other_blarg))
+ return false;
+ return true;
+ }
+ }; // class OtherFancyObject
+
+ class VisionClass
+ {
+ public:
+ bool blarg;
+ int other_object;
+
+ public:
+ /**
+ * @brief operator==() - This method checks whether all values equal another instance.
+ * @param i - The other instance
+ * @return - true, if all members are the same, false otherwise
+ */
+ bool operator==(const VisionClass& i) const
+ {
+ if ( not (blarg == i.blarg))
+ return false;
+ if ( not (other_object == i.other_object))
+ return false;
+ return true;
+ }
+ }; // class VisionClass
+
+ public:
+ VisionClass another_local_object;
+ OtherFancyObject local_object;
+
+ public:
+ ObjectTest()
+ {
+ initialize();
+ reset();
+ }
+
+ public:
+ /**
+ * @brief operator==() - This method checks whether all values equal another instance.
+ * @param i - The other instance
+ * @return - true, if all members are the same, false otherwise
+ */
+ bool operator==(const armarx::ObjectTest& i) const
+ {
+ if ( not (another_local_object == i.another_local_object))
+ return false;
+ if ( not (local_object == i.local_object))
+ return false;
+ return true;
+ }
+ /**
+ * @brief reset() - This method resets all member variables to default.
+ * @return - nothing
+ */
+ virtual void reset() override
+ {
+ another_local_object.blarg = {};
+ another_local_object.other_object = {};
+ local_object.blarg = {};
+ local_object.other_blarg = {};
+ }
+ /**
+ * @brief initialize() - This method initializeses special base-class members.
+ * @return - nothing
+ */
+ virtual void initialize() override
+ {
+ // setup aronType
+ std::map<std::string, armarx::aron::AronAbstractType::PointerType> aronTypeRoot_objectiteratorcontainer;
+ std::map<std::string, armarx::aron::AronAbstractType::PointerType> aronTypeRoot_another_local_object_objectiteratorcontainer;
+ armarx::aron::AronBoolType::PointerType aronTypeRoot_another_local_object_blarg = armarx::aron::AronBoolType::PointerType(new armarx::aron::AronBoolType());
+ aronTypeRoot_another_local_object_objectiteratorcontainer["blarg"] = aronTypeRoot_another_local_object_blarg;
+ armarx::aron::AronIntType::PointerType aronTypeRoot_another_local_object_other_object = armarx::aron::AronIntType::PointerType(new armarx::aron::AronIntType());
+ aronTypeRoot_another_local_object_objectiteratorcontainer["other_object"] = aronTypeRoot_another_local_object_other_object;
+ armarx::aron::AronObjectType::PointerType aronTypeRoot_another_local_object = armarx::aron::AronObjectType::PointerType(new armarx::aron::AronObjectType("VisionClass", aronTypeRoot_another_local_object_objectiteratorcontainer));
+ aronTypeRoot_objectiteratorcontainer["another_local_object"] = aronTypeRoot_another_local_object;
+ std::map<std::string, armarx::aron::AronAbstractType::PointerType> aronTypeRoot_local_object_objectiteratorcontainer;
+ armarx::aron::AronBoolType::PointerType aronTypeRoot_local_object_blarg = armarx::aron::AronBoolType::PointerType(new armarx::aron::AronBoolType());
+ aronTypeRoot_local_object_objectiteratorcontainer["blarg"] = aronTypeRoot_local_object_blarg;
+ armarx::aron::AronIntType::PointerType aronTypeRoot_local_object_other_blarg = armarx::aron::AronIntType::PointerType(new armarx::aron::AronIntType());
+ aronTypeRoot_local_object_objectiteratorcontainer["other_blarg"] = aronTypeRoot_local_object_other_blarg;
+ armarx::aron::AronObjectType::PointerType aronTypeRoot_local_object = armarx::aron::AronObjectType::PointerType(new armarx::aron::AronObjectType("OtherFancyObject", aronTypeRoot_local_object_objectiteratorcontainer));
+ aronTypeRoot_objectiteratorcontainer["local_object"] = aronTypeRoot_local_object;
+ armarx::aron::AronObjectType::PointerType aronTypeRoot = armarx::aron::AronObjectType::PointerType(new armarx::aron::AronObjectType("armarx::ObjectTest", aronTypeRoot_objectiteratorcontainer));
+ aronType = aronTypeRoot;
+ }
+ /**
+ * @brief write() - This method returns a new type from the member data types using a writer implementation.
+ * @param w - The writer implementation
+ * @return - the result of the writer implementation
+ */
+ virtual void write(armarx::aron::io::AronWriter& w) const override
+ {
+ w.writeStartDict();
+ w.writeKey("another_local_object");
+ w.writeStartDict();
+ w.writeKey("blarg");
+ w.writeBool(another_local_object.blarg);
+ w.writeKey("other_object");
+ w.writeInt(another_local_object.other_object);
+ w.writeEndDict();
+ w.writeKey("local_object");
+ w.writeStartDict();
+ w.writeKey("blarg");
+ w.writeBool(local_object.blarg);
+ w.writeKey("other_blarg");
+ w.writeInt(local_object.other_blarg);
+ w.writeEndDict();
+ w.writeEndDict();
+ }
+ /**
+ * @brief read() - This method sets the struct members to new values given in a reader implementation.
+ * @param r - The reader implementation
+ * @return - nothing
+ */
+ virtual void read(armarx::aron::io::AronReader& r) override
+ {
+ reset();
+
+ r.readStartDict();
+ r.readMember("another_local_object");
+ r.readStartDict();
+ r.readMember("blarg");
+ another_local_object.blarg = r.readBool();
+ r.readMember("other_object");
+ another_local_object.other_object = r.readInt();
+ r.readEndDict();
+ r.readMember("local_object");
+ r.readStartDict();
+ r.readMember("blarg");
+ local_object.blarg = r.readBool();
+ r.readMember("other_blarg");
+ local_object.other_blarg = r.readInt();
+ r.readEndDict();
+ r.readEndDict();
+ }
+ /**
+ * @brief specializedWrite() - This method returns a new type from the member data types using a writer implementation.
+ * @return - the result of the writer implementation
+ */
+ armarx::aron::AronDataPtr toAron() const
+ {
+ armarx::aron::io::AronDataNavigatorWriter writer;
+ this->write(writer);
+ return writer.getResult();
+ }
+ /**
+ * @brief specializedRead() - This method sets the struct members to new values given in a reader implementation.
+ * @return - nothing
+ */
+ void fromAron(const armarx::aron::AronDataPtr& input)
+ {
+ armarx::aron::io::AronDataNavigatorReader reader(input);
+ this->read(reader);
+ }
+ }; // class ObjectTest
+} // namespace armarx
+
+#endif // ARONTESTSEGMENT__OBJECTTEST__ARON_TYPE_DEFINITION_INCLUDE_GUARD
diff --git a/source/RobotAPI/libraries/aron/test/generated/PrimitiveTest.h b/source/RobotAPI/libraries/aron/test/generated/PrimitiveTest.h
new file mode 100644
index 0000000000000000000000000000000000000000..8505b602f7d6dd57a0d6e6a03795e1383b8c6228
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/test/generated/PrimitiveTest.h
@@ -0,0 +1,188 @@
+/*
+ * 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/>.
+ *
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ *************************************************************************
+ * WARNING: This file is autogenerated.
+ * Original file: /home/fabian/Software/ArmarX/RobotAPI/source/RobotAPI/libraries/aron/test/xmls/PrimitiveTest.xml
+ * Please do not edit since your changes may be overwritten on the next generation
+ * If you have any questions please contact: Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
+ *************************************************************************
+ */
+
+
+#ifndef ARONTESTSEGMENT__PRIMITIVETEST__ARON_TYPE_DEFINITION_INCLUDE_GUARD
+#define ARONTESTSEGMENT__PRIMITIVETEST__ARON_TYPE_DEFINITION_INCLUDE_GUARD
+
+#include <string>
+#include <vector>
+#include <map>
+#include <Eigen/Core>
+#include <RobotAPI/interface/aron.h>
+#include <RobotAPI/libraries/aron/codegenerator/AronCppClass.h>
+#include <RobotAPI/libraries/aron/io/classWriters/AronDataNavigatorWriter/AronDataNavigatorWriter.h>
+#include <RobotAPI/libraries/aron/io/classReaders/AronDataNavigatorReader/AronDataNavigatorReader.h>
+
+namespace armarx
+{
+ class PrimitiveTest
+ : virtual public armarx::aron::codegeneration::AronCppClass
+ {
+ public:
+ bool the_bool;
+ double the_double;
+ float the_float;
+ int the_int;
+ long the_long;
+ std::string the_string;
+
+ public:
+ PrimitiveTest()
+ {
+ initialize();
+ reset();
+ }
+
+ public:
+ /**
+ * @brief operator==() - This method checks whether all values equal another instance.
+ * @param i - The other instance
+ * @return - true, if all members are the same, false otherwise
+ */
+ bool operator==(const armarx::PrimitiveTest& i) const
+ {
+ if ( not (the_bool == i.the_bool))
+ return false;
+ if ( not (the_double == i.the_double))
+ return false;
+ if ( not (the_float == i.the_float))
+ return false;
+ if ( not (the_int == i.the_int))
+ return false;
+ if ( not (the_long == i.the_long))
+ return false;
+ if ( not (the_string == i.the_string))
+ return false;
+ return true;
+ }
+ /**
+ * @brief reset() - This method resets all member variables to default.
+ * @return - nothing
+ */
+ virtual void reset() override
+ {
+ the_bool = {};
+ the_double = {};
+ the_float = {};
+ the_int = {};
+ the_long = {};
+ the_string = {};
+ }
+ /**
+ * @brief initialize() - This method initializeses special base-class members.
+ * @return - nothing
+ */
+ virtual void initialize() override
+ {
+ // setup aronType
+ std::map<std::string, armarx::aron::AronAbstractType::PointerType> aronTypeRoot_objectiteratorcontainer;
+ armarx::aron::AronBoolType::PointerType aronTypeRoot_the_bool = armarx::aron::AronBoolType::PointerType(new armarx::aron::AronBoolType());
+ aronTypeRoot_objectiteratorcontainer["the_bool"] = aronTypeRoot_the_bool;
+ armarx::aron::AronDoubleType::PointerType aronTypeRoot_the_double = armarx::aron::AronDoubleType::PointerType(new armarx::aron::AronDoubleType());
+ aronTypeRoot_objectiteratorcontainer["the_double"] = aronTypeRoot_the_double;
+ armarx::aron::AronFloatType::PointerType aronTypeRoot_the_float = armarx::aron::AronFloatType::PointerType(new armarx::aron::AronFloatType());
+ aronTypeRoot_objectiteratorcontainer["the_float"] = aronTypeRoot_the_float;
+ armarx::aron::AronIntType::PointerType aronTypeRoot_the_int = armarx::aron::AronIntType::PointerType(new armarx::aron::AronIntType());
+ aronTypeRoot_objectiteratorcontainer["the_int"] = aronTypeRoot_the_int;
+ armarx::aron::AronLongType::PointerType aronTypeRoot_the_long = armarx::aron::AronLongType::PointerType(new armarx::aron::AronLongType());
+ aronTypeRoot_objectiteratorcontainer["the_long"] = aronTypeRoot_the_long;
+ armarx::aron::AronStringType::PointerType aronTypeRoot_the_string = armarx::aron::AronStringType::PointerType(new armarx::aron::AronStringType());
+ aronTypeRoot_objectiteratorcontainer["the_string"] = aronTypeRoot_the_string;
+ armarx::aron::AronObjectType::PointerType aronTypeRoot = armarx::aron::AronObjectType::PointerType(new armarx::aron::AronObjectType("armarx::PrimitiveTest", aronTypeRoot_objectiteratorcontainer));
+ aronType = aronTypeRoot;
+ }
+ /**
+ * @brief write() - This method returns a new type from the member data types using a writer implementation.
+ * @param w - The writer implementation
+ * @return - the result of the writer implementation
+ */
+ virtual void write(armarx::aron::io::AronWriter& w) const override
+ {
+ w.writeStartDict();
+ w.writeKey("the_bool");
+ w.writeBool(the_bool);
+ w.writeKey("the_double");
+ w.writeDouble(the_double);
+ w.writeKey("the_float");
+ w.writeFloat(the_float);
+ w.writeKey("the_int");
+ w.writeInt(the_int);
+ w.writeKey("the_long");
+ w.writeLong(the_long);
+ w.writeKey("the_string");
+ w.writeString(the_string);
+ w.writeEndDict();
+ }
+ /**
+ * @brief read() - This method sets the struct members to new values given in a reader implementation.
+ * @param r - The reader implementation
+ * @return - nothing
+ */
+ virtual void read(armarx::aron::io::AronReader& r) override
+ {
+ reset();
+
+ r.readStartDict();
+ r.readMember("the_bool");
+ the_bool = r.readBool();
+ r.readMember("the_double");
+ the_double = r.readDouble();
+ r.readMember("the_float");
+ the_float = r.readFloat();
+ r.readMember("the_int");
+ the_int = r.readInt();
+ r.readMember("the_long");
+ the_long = r.readLong();
+ r.readMember("the_string");
+ the_string = r.readString();
+ r.readEndDict();
+ }
+ /**
+ * @brief specializedWrite() - This method returns a new type from the member data types using a writer implementation.
+ * @return - the result of the writer implementation
+ */
+ armarx::aron::AronDataPtr toAron() const
+ {
+ armarx::aron::io::AronDataNavigatorWriter writer;
+ this->write(writer);
+ return writer.getResult();
+ }
+ /**
+ * @brief specializedRead() - This method sets the struct members to new values given in a reader implementation.
+ * @return - nothing
+ */
+ void fromAron(const armarx::aron::AronDataPtr& input)
+ {
+ armarx::aron::io::AronDataNavigatorReader reader(input);
+ this->read(reader);
+ }
+ }; // class PrimitiveTest
+} // namespace armarx
+
+#endif // ARONTESTSEGMENT__PRIMITIVETEST__ARON_TYPE_DEFINITION_INCLUDE_GUARD
diff --git a/source/RobotAPI/libraries/aron/test/generated/output.generated.h b/source/RobotAPI/libraries/aron/test/generated/output.generated.h
new file mode 100644
index 0000000000000000000000000000000000000000..e78beefa4467582905e32f2b8fa14b9c84bf5e4c
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/test/generated/output.generated.h
@@ -0,0 +1,383 @@
+#ifndef ARONTESTPRODUCER__LISTCLASS__ARON_TYPE_DEFINITION_INCLUDE_GUARD
+#define ARONTESTPRODUCER__LISTCLASS__ARON_TYPE_DEFINITION_INCLUDE_GUARD
+
+#include <string>
+#include <vector>
+#include <map>
+#include <Eigen/Core>
+#include <RobotAPI/interface/aron.h>
+#include <RobotAPI/libraries/aron/codegenerator/AronCppClass.h>
+#include <RobotAPI/libraries/aron/io/classWriters/AronDataNavigatorWriter/AronDataNavigatorWriter.h>
+#include <RobotAPI/libraries/aron/io/classReaders/AronDataNavigatorReader/AronDataNavigatorReader.h>
+
+namespace armarx
+{
+ class ListClass
+ : virtual public armarx::aron::codegeneration::AronCppClass
+ {
+ public:
+ bool element0;
+ bool element1;
+ bool element2;
+
+ public:
+ ListClass()
+ {
+ reset();
+ }
+
+ public:
+ /**
+ * @brief reset() - This method resets all member variables to default.
+ * @return - nothing
+ */
+ virtual void reset() override
+ {
+ element0 = {};
+ element1 = {};
+ element2 = {};
+ }
+ /**
+ * @brief reset() - This method sets special base-class members.
+ * @return - nothing
+ */
+ virtual void setup() override
+ {
+ // setup aronType
+ std::map<std::string, armarx::aron::AronAbstractType::PointerType> aronTypeRoot_objectiteratorcontainer;
+ armarx::aron::AronBoolType::PointerType aronTypeRoot_element0 = armarx::aron::AronBoolType::PointerType(new armarx::aron::AronBoolType());
+ aronTypeRoot_objectiteratorcontainer["element0"] = (aronTypeRoot_element0);
+ armarx::aron::AronBoolType::PointerType aronTypeRoot_element1 = armarx::aron::AronBoolType::PointerType(new armarx::aron::AronBoolType());
+ aronTypeRoot_objectiteratorcontainer["element1"] = (aronTypeRoot_element1);
+ armarx::aron::AronBoolType::PointerType aronTypeRoot_element2 = armarx::aron::AronBoolType::PointerType(new armarx::aron::AronBoolType());
+ aronTypeRoot_objectiteratorcontainer["element2"] = (aronTypeRoot_element2);
+ armarx::aron::AronObjectType::PointerType aronTypeRoot = armarx::aron::AronObjectType::PointerType(new armarx::aron::AronObjectType("armarx::ListClass", aronTypeRoot_objectiteratorcontainer));
+ aronType = aronTypeRoot;
+ }
+ /**
+ * @brief read() - This method returns a new type from the member data types using a writer implementation.
+ * @param w - The writer implementation
+ * @return - the result of the writer implementation
+ */
+ virtual void write(armarx::aron::io::AronWriter& w) const override
+ {
+ w.writeStartDict();
+ w.writeKey("element0");
+ w.writeBool(element0);
+ w.writeKey("element1");
+ w.writeBool(element1);
+ w.writeKey("element2");
+ w.writeBool(element2);
+ w.writeEndDict();
+ }
+ /**
+ * @brief read() - This method sets the struct members to new values given in a reader implementation.
+ * @param r - The reader implementation
+ * @return - nothing
+ */
+ virtual void read(armarx::aron::io::AronReader& r) override
+ {
+ reset();
+
+ r.readStartDict();
+ r.readMember("element0");
+ element0 = r.readBool();
+ r.readMember("element1");
+ element1 = r.readBool();
+ r.readMember("element2");
+ element2 = r.readBool();
+ r.readEndDict();
+ }
+ /**
+ * @brief specializedWrite() - This method returns a new type from the member data types using a writer implementation.
+ * @return - the result of the writer implementation
+ */
+ armarx::aron::AronDataPtr toAron() const
+ {
+ armarx::aron::io::AronDataNavigatorWriter writer;
+ this->write(writer);
+ return writer.getResult();
+ }
+ /**
+ * @brief specializedRead() - This method sets the struct members to new values given in a reader implementation.
+ * @return - nothing
+ */
+ void fromAron(const armarx::aron::AronDataPtr& input)
+ {
+ armarx::aron::io::AronDataNavigatorReader reader(input);
+ this->read(reader);
+ }
+ }; // class ListClass
+} // namespace armarx
+
+#endif // ARONTESTPRODUCER__LISTCLASS__ARON_TYPE_DEFINITION_INCLUDE_GUARD
+#ifndef ARONTESTPRODUCER__LISTTEST__ARON_TYPE_DEFINITION_INCLUDE_GUARD
+#define ARONTESTPRODUCER__LISTTEST__ARON_TYPE_DEFINITION_INCLUDE_GUARD
+
+#include <string>
+#include <vector>
+#include <map>
+#include <Eigen/Core>
+#include <RobotAPI/interface/aron.h>
+#include <RobotAPI/libraries/aron/codegenerator/AronCppClass.h>
+#include <RobotAPI/libraries/aron/io/classWriters/AronDataNavigatorWriter/AronDataNavigatorWriter.h>
+#include <RobotAPI/libraries/aron/io/classReaders/AronDataNavigatorReader/AronDataNavigatorReader.h>
+
+namespace armarx
+{
+ class ListTest
+ : virtual public armarx::aron::codegeneration::AronCppClass
+ {
+ public:
+ std::vector<bool> anotherboolList;
+ std::vector<bool> boolList;
+ std::vector<double> doubleList;
+ std::vector<float> floatList;
+ std::vector<int> intList;
+ std::vector<long> longList;
+ std::vector<armarx::ListClass> objectList;
+ std::vector<std::string> stringList;
+
+ public:
+ ListTest()
+ {
+ reset();
+ }
+
+ public:
+ /**
+ * @brief reset() - This method resets all member variables to default.
+ * @return - nothing
+ */
+ virtual void reset() override
+ {
+ anotherboolList.clear();
+ boolList.clear();
+ doubleList.clear();
+ floatList.clear();
+ intList.clear();
+ longList.clear();
+ objectList.clear();
+ stringList.clear();
+ }
+ /**
+ * @brief reset() - This method sets special base-class members.
+ * @return - nothing
+ */
+ virtual void setup() override
+ {
+ // setup aronType
+ std::map<std::string, armarx::aron::AronAbstractType::PointerType> aronTypeRoot_objectiteratorcontainer;
+ armarx::aron::AronBoolType::PointerType aronTypeRoot_anotherboolList_listtype = armarx::aron::AronBoolType::PointerType(new armarx::aron::AronBoolType());
+ armarx::aron::AronListType::PointerType aronTypeRoot_anotherboolList = armarx::aron::AronListType::PointerType(new armarx::aron::AronListType(aronTypeRoot_anotherboolList_listtype));
+ aronTypeRoot_objectiteratorcontainer["anotherboolList"] = (aronTypeRoot_anotherboolList);
+ armarx::aron::AronBoolType::PointerType aronTypeRoot_boolList_listtype = armarx::aron::AronBoolType::PointerType(new armarx::aron::AronBoolType());
+ armarx::aron::AronListType::PointerType aronTypeRoot_boolList = armarx::aron::AronListType::PointerType(new armarx::aron::AronListType(aronTypeRoot_boolList_listtype));
+ aronTypeRoot_objectiteratorcontainer["boolList"] = (aronTypeRoot_boolList);
+ armarx::aron::AronDoubleType::PointerType aronTypeRoot_doubleList_listtype = armarx::aron::AronDoubleType::PointerType(new armarx::aron::AronDoubleType());
+ armarx::aron::AronListType::PointerType aronTypeRoot_doubleList = armarx::aron::AronListType::PointerType(new armarx::aron::AronListType(aronTypeRoot_doubleList_listtype));
+ aronTypeRoot_objectiteratorcontainer["doubleList"] = (aronTypeRoot_doubleList);
+ armarx::aron::AronFloatType::PointerType aronTypeRoot_floatList_listtype = armarx::aron::AronFloatType::PointerType(new armarx::aron::AronFloatType());
+ armarx::aron::AronListType::PointerType aronTypeRoot_floatList = armarx::aron::AronListType::PointerType(new armarx::aron::AronListType(aronTypeRoot_floatList_listtype));
+ aronTypeRoot_objectiteratorcontainer["floatList"] = (aronTypeRoot_floatList);
+ armarx::aron::AronIntType::PointerType aronTypeRoot_intList_listtype = armarx::aron::AronIntType::PointerType(new armarx::aron::AronIntType());
+ armarx::aron::AronListType::PointerType aronTypeRoot_intList = armarx::aron::AronListType::PointerType(new armarx::aron::AronListType(aronTypeRoot_intList_listtype));
+ aronTypeRoot_objectiteratorcontainer["intList"] = (aronTypeRoot_intList);
+ armarx::aron::AronLongType::PointerType aronTypeRoot_longList_listtype = armarx::aron::AronLongType::PointerType(new armarx::aron::AronLongType());
+ armarx::aron::AronListType::PointerType aronTypeRoot_longList = armarx::aron::AronListType::PointerType(new armarx::aron::AronListType(aronTypeRoot_longList_listtype));
+ aronTypeRoot_objectiteratorcontainer["longList"] = (aronTypeRoot_longList);
+ std::map<std::string, armarx::aron::AronAbstractType::PointerType> aronTypeRoot_objectList_listtype_objectiteratorcontainer;
+ armarx::aron::AronBoolType::PointerType aronTypeRoot_objectList_listtype_element0 = armarx::aron::AronBoolType::PointerType(new armarx::aron::AronBoolType());
+ aronTypeRoot_objectList_listtype_objectiteratorcontainer["element0"] = (aronTypeRoot_objectList_listtype_element0);
+ armarx::aron::AronBoolType::PointerType aronTypeRoot_objectList_listtype_element1 = armarx::aron::AronBoolType::PointerType(new armarx::aron::AronBoolType());
+ aronTypeRoot_objectList_listtype_objectiteratorcontainer["element1"] = (aronTypeRoot_objectList_listtype_element1);
+ armarx::aron::AronBoolType::PointerType aronTypeRoot_objectList_listtype_element2 = armarx::aron::AronBoolType::PointerType(new armarx::aron::AronBoolType());
+ aronTypeRoot_objectList_listtype_objectiteratorcontainer["element2"] = (aronTypeRoot_objectList_listtype_element2);
+ armarx::aron::AronObjectType::PointerType aronTypeRoot_objectList_listtype = armarx::aron::AronObjectType::PointerType(new armarx::aron::AronObjectType("armarx::ListClass", aronTypeRoot_objectList_listtype_objectiteratorcontainer));
+ armarx::aron::AronListType::PointerType aronTypeRoot_objectList = armarx::aron::AronListType::PointerType(new armarx::aron::AronListType(aronTypeRoot_objectList_listtype));
+ aronTypeRoot_objectiteratorcontainer["objectList"] = (aronTypeRoot_objectList);
+ armarx::aron::AronStringType::PointerType aronTypeRoot_stringList_listtype = armarx::aron::AronStringType::PointerType(new armarx::aron::AronStringType());
+ armarx::aron::AronListType::PointerType aronTypeRoot_stringList = armarx::aron::AronListType::PointerType(new armarx::aron::AronListType(aronTypeRoot_stringList_listtype));
+ aronTypeRoot_objectiteratorcontainer["stringList"] = (aronTypeRoot_stringList);
+ armarx::aron::AronObjectType::PointerType aronTypeRoot = armarx::aron::AronObjectType::PointerType(new armarx::aron::AronObjectType("armarx::ListTest", aronTypeRoot_objectiteratorcontainer));
+ aronType = aronTypeRoot;
+ }
+ /**
+ * @brief read() - This method returns a new type from the member data types using a writer implementation.
+ * @param w - The writer implementation
+ * @return - the result of the writer implementation
+ */
+ virtual void write(armarx::aron::io::AronWriter& w) const override
+ {
+ w.writeStartDict();
+ w.writeKey("anotherboolList");
+ w.writeStartList();
+ for(unsigned int anotherboolList_listiterator = 0; anotherboolList_listiterator < anotherboolList.size(); ++anotherboolList_listiterator)
+ {
+ w.writeBool(anotherboolList[anotherboolList_listiterator]);
+ }
+ w.writeEndList();
+ w.writeKey("boolList");
+ w.writeStartList();
+ for(unsigned int boolList_listiterator = 0; boolList_listiterator < boolList.size(); ++boolList_listiterator)
+ {
+ w.writeBool(boolList[boolList_listiterator]);
+ }
+ w.writeEndList();
+ w.writeKey("doubleList");
+ w.writeStartList();
+ for(unsigned int doubleList_listiterator = 0; doubleList_listiterator < doubleList.size(); ++doubleList_listiterator)
+ {
+ w.writeDouble(doubleList[doubleList_listiterator]);
+ }
+ w.writeEndList();
+ w.writeKey("floatList");
+ w.writeStartList();
+ for(unsigned int floatList_listiterator = 0; floatList_listiterator < floatList.size(); ++floatList_listiterator)
+ {
+ w.writeFloat(floatList[floatList_listiterator]);
+ }
+ w.writeEndList();
+ w.writeKey("intList");
+ w.writeStartList();
+ for(unsigned int intList_listiterator = 0; intList_listiterator < intList.size(); ++intList_listiterator)
+ {
+ w.writeInt(intList[intList_listiterator]);
+ }
+ w.writeEndList();
+ w.writeKey("longList");
+ w.writeStartList();
+ for(unsigned int longList_listiterator = 0; longList_listiterator < longList.size(); ++longList_listiterator)
+ {
+ w.writeLong(longList[longList_listiterator]);
+ }
+ w.writeEndList();
+ w.writeKey("objectList");
+ w.writeStartList();
+ for(unsigned int objectList_listiterator = 0; objectList_listiterator < objectList.size(); ++objectList_listiterator)
+ {
+ w.writeStartDict();
+ w.writeKey("element0");
+ w.writeBool(objectList[objectList_listiterator].element0);
+ w.writeKey("element1");
+ w.writeBool(objectList[objectList_listiterator].element1);
+ w.writeKey("element2");
+ w.writeBool(objectList[objectList_listiterator].element2);
+ w.writeEndDict();
+ }
+ w.writeEndList();
+ w.writeKey("stringList");
+ w.writeStartList();
+ for(unsigned int stringList_listiterator = 0; stringList_listiterator < stringList.size(); ++stringList_listiterator)
+ {
+ w.writeString(stringList[stringList_listiterator]);
+ }
+ w.writeEndList();
+ w.writeEndDict();
+ }
+ /**
+ * @brief read() - This method sets the struct members to new values given in a reader implementation.
+ * @param r - The reader implementation
+ * @return - nothing
+ */
+ virtual void read(armarx::aron::io::AronReader& r) override
+ {
+ reset();
+
+ r.readStartDict();
+ r.readMember("anotherboolList");
+ r.readStartList();
+ while(!r.readEndList())
+ {
+ bool anotherboolList_listiterator;
+ anotherboolList_listiterator = r.readBool();
+ anotherboolList.push_back(anotherboolList_listiterator);
+ }
+ r.readMember("boolList");
+ r.readStartList();
+ while(!r.readEndList())
+ {
+ bool boolList_listiterator;
+ boolList_listiterator = r.readBool();
+ boolList.push_back(boolList_listiterator);
+ }
+ r.readMember("doubleList");
+ r.readStartList();
+ while(!r.readEndList())
+ {
+ double doubleList_listiterator;
+ doubleList_listiterator = r.readDouble();
+ doubleList.push_back(doubleList_listiterator);
+ }
+ r.readMember("floatList");
+ r.readStartList();
+ while(!r.readEndList())
+ {
+ float floatList_listiterator;
+ floatList_listiterator = r.readFloat();
+ floatList.push_back(floatList_listiterator);
+ }
+ r.readMember("intList");
+ r.readStartList();
+ while(!r.readEndList())
+ {
+ int intList_listiterator;
+ intList_listiterator = r.readInt();
+ intList.push_back(intList_listiterator);
+ }
+ r.readMember("longList");
+ r.readStartList();
+ while(!r.readEndList())
+ {
+ long longList_listiterator;
+ longList_listiterator = r.readLong();
+ longList.push_back(longList_listiterator);
+ }
+ r.readMember("objectList");
+ r.readStartList();
+ while(!r.readEndList())
+ {
+ armarx::ListClass objectList_listiterator;
+ r.readStartDict();
+ r.readMember("element0");
+ objectList_listiterator.element0 = r.readBool();
+ r.readMember("element1");
+ objectList_listiterator.element1 = r.readBool();
+ r.readMember("element2");
+ objectList_listiterator.element2 = r.readBool();
+ r.readEndDict();
+ objectList.push_back(objectList_listiterator);
+ }
+ r.readMember("stringList");
+ r.readStartList();
+ while(!r.readEndList())
+ {
+ std::string stringList_listiterator;
+ stringList_listiterator = r.readString();
+ stringList.push_back(stringList_listiterator);
+ }
+ r.readEndDict();
+ }
+ /**
+ * @brief specializedWrite() - This method returns a new type from the member data types using a writer implementation.
+ * @return - the result of the writer implementation
+ */
+ armarx::aron::AronDataPtr toAron() const
+ {
+ armarx::aron::io::AronDataNavigatorWriter writer;
+ this->write(writer);
+ return writer.getResult();
+ }
+ /**
+ * @brief specializedRead() - This method sets the struct members to new values given in a reader implementation.
+ * @return - nothing
+ */
+ void fromAron(const armarx::aron::AronDataPtr& input)
+ {
+ armarx::aron::io::AronDataNavigatorReader reader(input);
+ this->read(reader);
+ }
+ }; // class ListTest
+} // namespace armarx
+
+#endif // ARONTESTPRODUCER__LISTTEST__ARON_TYPE_DEFINITION_INCLUDE_GUARD
diff --git a/source/RobotAPI/libraries/aron/test/new file b/source/RobotAPI/libraries/aron/test/new file
new file mode 100644
index 0000000000000000000000000000000000000000..ba64cf781d81b2af9376fd00053927a706395cf9
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/test/new file
@@ -0,0 +1,51 @@
+#include <string.h>
+#include <iostream>
+#include <vector>
+
+typedef unsigned char byte;
+
+int main()
+{
+ // Test input vector
+ std::vector<float> fvec = {1000.0f, 2000.0f, 3000.0f};
+
+ // convert float vector to byte vector (e.g. create AronBlob)
+ float* fvec_data = fvec.data();
+ byte* fvec_data_as_byte = reinterpret_cast<byte*>(fvec_data);
+ std::vector<byte> fvec_byte(fvec_data_as_byte, fvec_data_as_byte + (sizeof(float) * fvec.size()));
+
+ // Test byte vector by casting back
+ byte* fvec_byte_data = fvec_byte.data();
+ float* fvec_byte_data_as_float = reinterpret_cast<float*>(fvec_byte_data);
+ std::vector<float> fvec_byte_test(fvec_byte_data_as_float, fvec_byte_data_as_float + fvec.size());
+
+ for(const auto& i : fvec_byte_test)
+ std::cout << i << std::endl;
+
+ // Create new vector to copy values into (e.g. readAronBlob)
+ unsigned long byte_size = fvec_byte.size() / sizeof(float);
+ unsigned long byte_size_error = fvec_byte.size() % sizeof(float);
+
+ if(byte_size_error != 0)
+ {
+ // throw LocalException("error!");
+ std::cout << "ERROR! WRONG SIZE!" << std::endl;
+ exit(1);
+ }
+
+ std::vector<float> fvec2(fvec_byte.size() / sizeof(float));
+
+ // convert new float vector to byte* to first element in order to copy the values
+ float* fvec2_data = fvec2.data();
+ byte* fvec2_data_as_byte = reinterpret_cast<byte*>(fvec2_data);
+
+ // copy the values
+ memcpy(fvec2_data_as_byte, fvec_byte.data(), (sizeof(float) * fvec.size()));
+
+ // print new element
+ for(const auto& i : fvec2)
+ std::cout << i << std::endl;
+
+
+ return 0;
+}
diff --git a/source/RobotAPI/libraries/aron/test/xmls/DictTest.xml b/source/RobotAPI/libraries/aron/test/xmls/DictTest.xml
new file mode 100644
index 0000000000000000000000000000000000000000..5757b9392ee398c23ba3215c038c55d133fb085a
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/test/xmls/DictTest.xml
@@ -0,0 +1,23 @@
+<!--Some fancy comment -->
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+ <Serializers>
+ <Writers>
+ </Writers>
+ <Readers>
+ </Readers>
+ </Serializers>
+ <UseTypes>
+ </UseTypes>
+ <GenerateTypes>
+ <GenerateType name='armarx::DictTest'>
+ <objectchild key='dict'>
+ <Dict>
+ <DictType>
+ <Float />
+ </DictType>
+ </Dict>
+ </ObjectChild>
+ </GenerateType>
+ </GenerateTypes>
+</AronTypeDefinition>
diff --git a/source/RobotAPI/libraries/aron/test/xmls/HumanPose.xml b/source/RobotAPI/libraries/aron/test/xmls/HumanPose.xml
new file mode 100644
index 0000000000000000000000000000000000000000..ab39fae3b5fd5be0de040c47fd77fd4eadf77254
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/test/xmls/HumanPose.xml
@@ -0,0 +1,35 @@
+<!--This class contains the data structure for NaturalIKResults -->
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+
+ <Serializers>
+
+ <Writers>
+ </Writers>
+
+ <Readers>
+ </Readers>
+
+ </Serializers>
+
+ <UseTypes>
+ </UseTypes>
+
+ <GenerateTypes>
+ <GenerateType name='armarx::NaturalIKResult'>
+
+ <ObjectChild key='reached'>
+ <Bool />
+ </ObjectChild>
+
+ <ObjectChild key='jointValues'>
+ <List>
+ <ListType>
+ <Float />
+ </ListType>
+ </List>
+ </ObjectChild>
+
+ </GenerateType>
+ </GenerateTypes>
+</AronTypeDefinition>
diff --git a/source/RobotAPI/libraries/aron/test/xmls/ListTest.xml b/source/RobotAPI/libraries/aron/test/xmls/ListTest.xml
new file mode 100644
index 0000000000000000000000000000000000000000..3cb8666d888ae9742027dcae7ab1ad55a94521d1
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/test/xmls/ListTest.xml
@@ -0,0 +1,82 @@
+<!--Some fancy comment -->
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+ <Serializers>
+ <Writers>
+ </Writers>
+ <Readers>
+ </Readers>
+ </Serializers>
+ <UseTypes>
+ </UseTypes>
+ <GenerateTypes>
+ <GenerateType name='armarx::ListTest'>
+ <ObjectChild key='floatList'>
+ <List>
+ <ListType>
+ <Float />
+ </ListType>
+ </List>
+ </ObjectChild>
+ <ObjectChild key='intList'>
+ <List>
+ <ListType>
+ <Int />
+ </ListType>
+ </List>
+ </ObjectChild>
+ <ObjectChild key='doubleList'>
+ <List>
+ <ListType>
+ <Double />
+ </ListType>
+ </List>
+ </ObjectChild>
+ <ObjectChild key='longList'>
+ <List>
+ <ListType>
+ <Long />
+ </ListType>
+ </List>
+ </ObjectChild>
+ <ObjectChild key='stringList'>
+ <List>
+ <ListType>
+ <String />
+ </ListType>
+ </List>
+ </ObjectChild>
+ <ObjectChild key='boolList'>
+ <List>
+ <ListType>
+ <Bool />
+ </ListType>
+ </List>
+ </ObjectChild>
+ <ObjectChild key='anotherboolList'>
+ <List>
+ <ListType>
+ <Bool />
+ </ListType>
+ </List>
+ </ObjectChild>
+ <ObjectChild key='objectList'>
+ <List>
+ <ListType>
+ <Object name='ListClass'>
+ <ObjectChild key='element0'>
+ <Bool />
+ </ObjectChild>
+ <ObjectChild key='element1'>
+ <Bool />
+ </ObjectChild>
+ <ObjectChild key='element2'>
+ <Bool />
+ </ObjectChild>
+ </Object>
+ </ListType>
+ </List>
+ </ObjectChild>
+ </GenerateType>
+ </GenerateTypes>
+</AronTypeDefinition>
diff --git a/source/RobotAPI/libraries/aron/test/xmls/NaturalIK.xml b/source/RobotAPI/libraries/aron/test/xmls/NaturalIK.xml
new file mode 100644
index 0000000000000000000000000000000000000000..ab39fae3b5fd5be0de040c47fd77fd4eadf77254
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/test/xmls/NaturalIK.xml
@@ -0,0 +1,35 @@
+<!--This class contains the data structure for NaturalIKResults -->
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+
+ <Serializers>
+
+ <Writers>
+ </Writers>
+
+ <Readers>
+ </Readers>
+
+ </Serializers>
+
+ <UseTypes>
+ </UseTypes>
+
+ <GenerateTypes>
+ <GenerateType name='armarx::NaturalIKResult'>
+
+ <ObjectChild key='reached'>
+ <Bool />
+ </ObjectChild>
+
+ <ObjectChild key='jointValues'>
+ <List>
+ <ListType>
+ <Float />
+ </ListType>
+ </List>
+ </ObjectChild>
+
+ </GenerateType>
+ </GenerateTypes>
+</AronTypeDefinition>
diff --git a/source/RobotAPI/libraries/aron/test/xmls/ObjectTest.xml b/source/RobotAPI/libraries/aron/test/xmls/ObjectTest.xml
new file mode 100644
index 0000000000000000000000000000000000000000..19d48b8e5ff3986d22825b7aa1e9a6dbb39d6a25
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/test/xmls/ObjectTest.xml
@@ -0,0 +1,44 @@
+<!--Some fancy comment -->
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+ <Serializers>
+ <Writers>
+ </Writers>
+ <Readers>
+ </Readers>
+ </Serializers>
+ <UseTypes>
+ </UseTypes>
+ <GenerateTypes>
+
+ <GenerateType name='armarx::SimpleClass'>
+ <objectchild key='the_bool'>
+ <bool />
+ </objectchild>
+ </GenerateType>
+
+ <GenerateType name='armarx::ObjectTest'>
+ <objectchild key='local_object'>
+ <object name='OtherFancyObject'>
+ <objectchild key='blarg'>
+ <bool />
+ </objectchild>
+ <objectchild key='other_blarg'>
+ <int />
+ </objectchild>
+ </object>
+ </ObjectChild>
+ <objectchild key='another_local_object'>
+ <object name='VisionClass'>
+ <objectchild key='blarg'>
+ <bool />
+ </objectchild>
+ <objectchild key='other_object'>
+ <int />
+ </objectchild>
+ </object>
+ </ObjectChild>
+ </GenerateType>
+
+ </GenerateTypes>
+</AronTypeDefinition>
diff --git a/source/RobotAPI/libraries/aron/test/xmls/PrimitiveTest.xml b/source/RobotAPI/libraries/aron/test/xmls/PrimitiveTest.xml
new file mode 100644
index 0000000000000000000000000000000000000000..4bf6e14022c300e226ba8aad827dd086752928e0
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/test/xmls/PrimitiveTest.xml
@@ -0,0 +1,34 @@
+<!--Some fancy comment -->
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+ <Serializers>
+ <Writers>
+ </Writers>
+ <Readers>
+ </Readers>
+ </Serializers>
+ <UseTypes>
+ </UseTypes>
+ <GenerateTypes>
+ <GenerateType name='armarx::PrimitiveTest'>
+ <objectchild key='the_int'>
+ <int />
+ </ObjectChild>
+ <objectchild key='the_long'>
+ <long />
+ </ObjectChild>
+ <objectchild key='the_float'>
+ <float />
+ </ObjectChild>
+ <objectchild key='the_double'>
+ <double />
+ </ObjectChild>
+ <objectchild key='the_string'>
+ <string />
+ </ObjectChild>
+ <objectchild key='the_bool'>
+ <bool />
+ </ObjectChild>
+ </GenerateType>
+ </GenerateTypes>
+</AronTypeDefinition>
diff --git a/source/RobotAPI/libraries/aron/types/AronAbstractTypeNavigator.cpp b/source/RobotAPI/libraries/aron/types/AronAbstractTypeNavigator.cpp
index c6eaff4cdbfe35ecc092fda1bebed01583cb5bb6..6c7e59e441368fd479631fe7e29d1684e1e21ced 100644
--- a/source/RobotAPI/libraries/aron/types/AronAbstractTypeNavigator.cpp
+++ b/source/RobotAPI/libraries/aron/types/AronAbstractTypeNavigator.cpp
@@ -41,19 +41,37 @@ namespace armarx
{
namespace types
{
+ // constantes
const std::string AronAbstractTypeNavigator::ARON_DATA_NAME = boost::core::demangle(typeid(AronData).name());
const std::string AronAbstractTypeNavigator::ARON_DATA_PTR_NAME = AronAbstractTypeNavigator::ARON_DATA_NAME + "::PointerType";
- const AronAbstractTypeNavigatorFactoryPtr AronAbstractTypeNavigator::Factory = AronAbstractTypeNavigatorFactoryPtr(new AronAbstractTypeNavigatorFactory());
- std::map<std::string, AronAbstractTypeNavigatorPtr> AronAbstractTypeNavigator::AllAronAbstractTypes = {};
+ const std::string AronAbstractTypeNavigator::ARON_TYPE_NAME = boost::core::demangle(typeid(AronAbstractType).name());
+ const std::string AronAbstractTypeNavigator::ARON_TYPE_PTR_NAME = AronAbstractTypeNavigator::ARON_TYPE_NAME + "::PointerType";
+ std::map<std::string, AronObjectTypeNavigatorPtr> AronAbstractTypeNavigator::AllAronObjectTypes = {};
- AronAbstractTypeNavigator::AronAbstractTypeNavigator(const std::string& cpp, const std::string& aron, const AronNavigatorPath& path) :
- AronNavigator<AronAbstractType>(path),
- cppTypename(cpp),
- aronTypename(aron)
+
+ namespace
+ {
+ // anonymous namespace
+ std::string resolveCppTypeName(const std::string& realTypeName)
+ {
+ if (realTypeName == boost::core::demangle(typeid(std::string).name()))
+ {
+ return "std::string";
+ }
+ return realTypeName;
+ }
+ }
+
+ // constructors
+ AronAbstractTypeNavigator::AronAbstractTypeNavigator(const AronTypeDescriptor& d, const std::string& cpp, const std::string& aron, const std::string& type) :
+ AronNavigator<AronTypeDescriptor, AronAbstractType>::AronNavigator(d),
+ cppTypename(resolveCppTypeName(cpp)),
+ aronTypename(aron),
+ aronTypeTypename(type)
{
- AllAronAbstractTypes[boost::algorithm::to_lower_copy(getFullCppTypename())] = AronAbstractTypeNavigatorPtr(this);
+
}
bool AronAbstractTypeNavigator::operator<(const AronAbstractTypeNavigator& other) const
@@ -66,10 +84,7 @@ namespace armarx
{
return cppTypename;
}
- std::string AronAbstractTypeNavigator::getFullCppPtrTypename() const
- {
- return "std::shared_ptr<" + cppTypename + ">";
- }
+
std::string AronAbstractTypeNavigator::getCppTypename() const
{
std::vector<std::string> split;
@@ -79,33 +94,45 @@ namespace armarx
type[0] = toupper(type[0]);
return type;
}
- std::string AronAbstractTypeNavigator::getCppPtrTypename() const
- {
- // TODO
- return "";
- }
std::string AronAbstractTypeNavigator::getFullAronTypename() const
{
return aronTypename;
}
+
+ std::string AronAbstractTypeNavigator::getAronTypename() const
+ {
+ std::vector<std::string> split;
+ std::string name = getFullAronTypename();
+ boost::algorithm::split(split, name, boost::is_any_of("::"));
+ std::string type = split[split.size() - 1];
+ type[0] = toupper(type[0]);
+ return type;
+ }
+
std::string AronAbstractTypeNavigator::getFullAronPtrTypename() const
{
- return aronTypename + "::PointerType";
+ return getFullAronTypename() + "::PointerType";
}
- std::string AronAbstractTypeNavigator::getAronTypename() const
+
+ std::string AronAbstractTypeNavigator::getFullAronTypeTypename() const
+ {
+ return aronTypeTypename;
+ }
+
+ std::string AronAbstractTypeNavigator::getAronTypeTypename() const
{
std::vector<std::string> split;
- std::string name = getFullAronTypename();
+ std::string name = getFullAronTypeTypename();
boost::algorithm::split(split, name, boost::is_any_of("::"));
std::string type = split[split.size() - 1];
type[0] = toupper(type[0]);
return type;
}
- std::string AronAbstractTypeNavigator::getAronPtrTypename() const
+
+ std::string AronAbstractTypeNavigator::getFullAronTypePtrTypename() const
{
- // TODO
- return "";
+ return getFullAronTypeTypename() + "::PointerType";
}
std::string AronAbstractTypeNavigator::escapeAccessor(const std::string& accessor)
@@ -118,9 +145,44 @@ namespace armarx
return escaped_accessor;
}
+ CppMethodPtr AronAbstractTypeNavigator::toResetMethod() const
+ {
+ std::stringstream doc;
+ doc << "@brief reset() - This method resets all member variables to default. \n";
+ doc << "@return - nothing";
+
+ CppMethodPtr m = CppMethodPtr(new CppMethod("virtual void reset() override", doc.str()));
+ CppBlockPtr b = this->getResetBlock("");
+ m->setBlock(b);
+ return m;
+ }
+
+ CppMethodPtr AronAbstractTypeNavigator::toInitializeMethod() const
+ {
+ std::stringstream doc;
+ doc << "@brief initialize() - This method initializeses special base-class members. \n";
+ doc << "@return - nothing";
+
+ CppMethodPtr m = CppMethodPtr(new CppMethod("virtual void initialize() override", doc.str()));
+ CppBlockPtr b = CppBlockPtr(new CppBlock());
+ b->addLine("// setup aronType");
+ CppBlockPtr b2 = this->getInitializeAronTypeBlock("aronTypeRoot");
+ b = CppBlock::mergeBlocks(b, b2);
+ b->addLine("aronType = aronTypeRoot;");
+
+
+ m->setBlock(b);
+ return m;
+ }
+
CppMethodPtr AronAbstractTypeNavigator::toWriteMethod() const
{
- CppMethodPtr m = CppMethodPtr(new CppMethod("void write(armarx::aron::io::AronWriter& w) const", ""));
+ std::stringstream doc;
+ doc << "@brief write() - This method returns a new type from the member data types using a writer implementation. \n";
+ doc << "@param w - The writer implementation\n";
+ doc << "@return - the result of the writer implementation";
+
+ CppMethodPtr m = CppMethodPtr(new CppMethod("virtual void write(armarx::aron::io::AronWriter& w) const override", doc.str()));
CppBlockPtr b = this->getWriteBlock("");
m->setBlock(b);
return m;
@@ -128,15 +190,28 @@ namespace armarx
CppMethodPtr AronAbstractTypeNavigator::toReadMethod() const
{
- CppMethodPtr m = CppMethodPtr(new CppMethod("void read(armarx::aron::io::AronReader& r)", ""));
- CppBlockPtr b = this->getReadBlock("");
+ std::stringstream doc;
+ doc << "@brief read() - This method sets the struct members to new values given in a reader implementation. \n";
+ doc << "@param r - The reader implementation\n";
+ doc << "@return - nothing";
+
+ CppMethodPtr m = CppMethodPtr(new CppMethod("virtual void read(armarx::aron::io::AronReader& r) override", doc.str()));
+ CppBlockPtr b = CppBlockPtr(new CppBlock());
+ b->addLine("reset();");
+ b->addLine("");
+ CppBlockPtr b2 = this->getReadBlock("");
+ b = CppBlock::mergeBlocks(b, b2);
m->setBlock(b);
return m;
}
CppMethodPtr AronAbstractTypeNavigator::toSpecializedWriterMethod(const std::string& returnname, const std::string& methodname, const std::string& writerName) const
{
- CppMethodPtr m = CppMethodPtr(new CppMethod(returnname + " " + methodname + "() const"));
+ std::stringstream doc;
+ doc << "@brief specializedWrite() - This method returns a new type from the member data types using a writer implementation. \n";
+ doc << "@return - the result of the writer implementation";
+
+ CppMethodPtr m = CppMethodPtr(new CppMethod(returnname + " " + methodname + "() const", doc.str()));
m->addLine(writerName + " writer;");
m->addLine("this->write(writer);");
m->addLine("return writer.getResult();");
@@ -145,27 +220,44 @@ namespace armarx
CppMethodPtr AronAbstractTypeNavigator::toSpecializedReaderMethod(const std::string& argumentname, const std::string& methodname, const std::string& readerName) const
{
- CppMethodPtr m = CppMethodPtr(new CppMethod("void " + methodname + "(const " + argumentname + "& input)"));
+ std::stringstream doc;
+ doc << "@brief specializedRead() - This method sets the struct members to new values given in a reader implementation. \n";
+ doc << "@return - nothing";
+
+ CppMethodPtr m = CppMethodPtr(new CppMethod("void " + methodname + "(const " + argumentname + "& input)", doc.str()));
m->addLine(readerName + " reader(input);");
m->addLine("this->read(reader);");
return m;
}
+ // static methods
+ AronAbstractTypeNavigatorPtr AronAbstractTypeNavigator::GetAronAbstractTypePtrByName(const std::string& name)
+ {
+ const std::string lower_name = boost::algorithm::to_lower_copy(name);
+ auto it = AllAronObjectTypes.find(lower_name);
+ if (it == AllAronObjectTypes.end())
+ {
+ throw LocalException("AronAbstractTypeNavigator: Could not find a valid object type for name '" + lower_name + "', (original: " + name + ").");
+ }
+ return it->second;
+ }
- AronAbstractTypeNavigatorPtr AronAbstractTypeNavigator::FromAronAbstractTypePtr(const AronAbstractTypePtr& a, const AronNavigatorPath& path)
+ void AronAbstractTypeNavigator::AddObjectTypeToAllObjectTypesList(const AronObjectTypeNavigatorPtr& n)
{
- return Factory->create(a, path);
+ checkAbstractTypeNavigatorPtrForNull(n, "");
+ if (AllAronObjectTypes.find(boost::algorithm::to_lower_copy(n->getFullCppTypename())) != AllAronObjectTypes.end())
+ {
+ throw LocalException("AronAbstractTypeNavigator: The object type '" + n->getFullCppTypename() + "' has already beed added to the AllAronAbstractTypes.");
+ }
+ AllAronObjectTypes[boost::algorithm::to_lower_copy(n->getFullCppTypename())] = n;
}
- AronAbstractTypeNavigatorPtr AronAbstractTypeNavigator::GetAronAbstractTypePtrByName(const std::string& name)
+ void AronAbstractTypeNavigator::checkAbstractTypeNavigatorPtrForNull(const AronAbstractTypeNavigatorPtr& data, const std::string& message)
{
- const std::string lower_name = boost::algorithm::to_lower_copy(name);
- auto it = AllAronAbstractTypes.find(lower_name);
- if (it == AllAronAbstractTypes.end())
+ if (data.get() == nullptr)
{
- throw LocalException("AronAbstractTypeNavigator: Could not find a valid type for name '" + lower_name + "', (original: " + name + ").");
+ throw LocalException("AronAbstractTypeNavigator: Could not cast an AronAbstractTypeNavigatorPtr. The Ptr was NULL. " + message);
}
- return it->second;
}
}
}
diff --git a/source/RobotAPI/libraries/aron/types/AronAbstractTypeNavigator.h b/source/RobotAPI/libraries/aron/types/AronAbstractTypeNavigator.h
index 74af63a23e595a3734dcfdf5e359de2971a43a1b..61c7798d1718b6bd78a09281cd0dbfdf08d9a55a 100644
--- a/source/RobotAPI/libraries/aron/types/AronAbstractTypeNavigator.h
+++ b/source/RobotAPI/libraries/aron/types/AronAbstractTypeNavigator.h
@@ -49,35 +49,47 @@ namespace armarx
class AronAbstractTypeNavigatorFactory;
typedef std::shared_ptr<AronAbstractTypeNavigatorFactory> AronAbstractTypeNavigatorFactoryPtr;
+ class AronObjectTypeNavigator;
+ typedef std::shared_ptr<AronObjectTypeNavigator> AronObjectTypeNavigatorPtr;
+
class AronAbstractTypeNavigator;
typedef std::shared_ptr<AronAbstractTypeNavigator> AronAbstractTypeNavigatorPtr;
class AronAbstractTypeNavigator :
- virtual public AronNavigator<AronAbstractType>
+ virtual public AronNavigator<AronTypeDescriptor, AronAbstractType>
{
public:
using PointerType = AronAbstractTypeNavigatorPtr;
- AronAbstractTypeNavigator(const std::string&, const std::string&, const AronNavigatorPath& path);
+ public:
+ // constructors
+ AronAbstractTypeNavigator(const AronTypeDescriptor&, const std::string&, const std::string&, const std::string&);
+ // operators
bool operator<(const AronAbstractTypeNavigator&) const;
+ // public member methods
std::string getFullCppTypename() const;
- std::string getFullCppPtrTypename() const;
-
std::string getCppTypename() const;
- std::string getCppPtrTypename() const;
std::string getFullAronTypename() const;
+ std::string getAronTypename() const;
std::string getFullAronPtrTypename() const;
- std::string getAronTypename() const;
- std::string getAronPtrTypename() const;
+ std::string getFullAronTypeTypename() const;
+ std::string getAronTypeTypename() const;
+ std::string getFullAronTypePtrTypename() const;
- std::string getDefaultInitialization() const;
+ CppMethodPtr toSpecializedWriterMethod(const std::string&, const std::string&, const std::string&) const;
+ CppMethodPtr toSpecializedReaderMethod(const std::string&, const std::string&, const std::string&) const;
- static std::string escapeAccessor(const std::string&);
+ // virtual override definitions
+ CppMethodPtr toResetMethod() const;
+ virtual CppBlockPtr getResetBlock(const std::string&) const = 0;
+
+ CppMethodPtr toInitializeMethod() const;
+ virtual CppBlockPtr getInitializeAronTypeBlock(const std::string&) const = 0;
CppMethodPtr toWriteMethod() const;
virtual CppBlockPtr getWriteBlock(const std::string&) const = 0;
@@ -85,31 +97,38 @@ namespace armarx
CppMethodPtr toReadMethod() const;
virtual CppBlockPtr getReadBlock(const std::string&) const = 0;
- CppMethodPtr toSpecializedWriterMethod(const std::string&, const std::string&, const std::string&) const;
- CppMethodPtr toSpecializedReaderMethod(const std::string&, const std::string&, const std::string&) const;
-
virtual AronAbstractTypePtr getResult() const override = 0;
-
- static AronAbstractTypeNavigatorPtr FromAronAbstractTypePtr(const AronAbstractTypePtr&, const AronNavigatorPath& path);
+ // static methods
static AronAbstractTypeNavigatorPtr GetAronAbstractTypePtrByName(const std::string& name);
+ static void AddObjectTypeToAllObjectTypesList(const AronObjectTypeNavigatorPtr&);
template<typename AronAbstractTypeNavigatorClass>
static typename AronAbstractTypeNavigatorClass::PointerType DynamicCast(const AronAbstractTypeNavigatorPtr& n)
{
- return std::dynamic_pointer_cast<AronAbstractTypeNavigatorClass>(n);
+ return AronAbstractTypeNavigatorClass::DynamicCast(n);
}
+ protected:
+ static void checkAbstractTypeNavigatorPtrForNull(const AronAbstractTypeNavigatorPtr& data, const std::string& message);
+ static std::string escapeAccessor(const std::string&);
+
public:
+ // public constantes
const static std::string ARON_DATA_NAME;
const static std::string ARON_DATA_PTR_NAME;
+ const static std::string ARON_TYPE_NAME;
+ const static std::string ARON_TYPE_PTR_NAME;
+
private:
+ // members
std::string cppTypename;
std::string aronTypename;
+ std::string aronTypeTypename;
- const static AronAbstractTypeNavigatorFactoryPtr Factory;
- static std::map<std::string, AronAbstractTypeNavigatorPtr> AllAronAbstractTypes;
+ // static members
+ static std::map<std::string, AronObjectTypeNavigatorPtr> AllAronObjectTypes;
};
}
}
diff --git a/source/RobotAPI/libraries/aron/types/AronAbstractTypeNavigatorFactory.cpp b/source/RobotAPI/libraries/aron/types/AronAbstractTypeNavigatorFactory.cpp
index 57681d8686be3ef9af4ae5b10819b4df3743277d..beb4f4e64c0d055561acb05f20ef9faad9a3f651 100644
--- a/source/RobotAPI/libraries/aron/types/AronAbstractTypeNavigatorFactory.cpp
+++ b/source/RobotAPI/libraries/aron/types/AronAbstractTypeNavigatorFactory.cpp
@@ -35,16 +35,22 @@ namespace armarx
{
namespace types
{
- const std::map<AronTypeDescriptor::Value, AronAbstractTypeNavigatorFactoryPtr> AronAbstractTypeNavigatorFactory::Factories =
+ const std::map<AronTypeDescriptor, AronAbstractTypeNavigatorFactoryPtr> AronAbstractTypeNavigatorFactory::Factories =
{
#define RUN_ARON_MACRO(upperType, lowerType, capsType) \
- {AronTypeDescriptor::GetTypeForAronAbstractTypeId(typeid(Aron##upperType##Type))->getValue(), AronAbstractTypeNavigatorFactoryPtr(new Aron##upperType##TypeNavigatorFactory())}, \
+ {AronResolver::GetTypeForAronAbstractTypeId(typeid(Aron##upperType##Type)), AronAbstractTypeNavigatorFactoryPtr(new Aron##upperType##TypeNavigatorFactory())}, \
HANDLE_CONTAINER_TYPES
#undef RUN_ARON_MACRO
+ /*#define RUN_ARON_MACRO(upperType, lowerType, capsType) \
+ {AronTypeResolver::GetTypeForAronAbstractTypeId(typeid(Aron##upperType##Type)), AronAbstractTypeNavigatorFactoryPtr(new Aron##upperType##TypeNavigatorFactory())}, \
+
+ HANDLE_COMPLEX_TYPES
+ #undef RUN_ARON_MACRO*/
+
#define RUN_ARON_MACRO(cppType, upperType, lowerType, capsType) \
- {AronTypeDescriptor::GetTypeForAronAbstractTypeId(typeid(Aron##upperType##Type))->getValue(), AronAbstractTypeNavigatorFactoryPtr(new AronPrimitiveTypeNavigatorFactory<Aron##upperType##Type, Aron##upperType, cppType>())}, \
+ {AronResolver::GetTypeForAronAbstractTypeId(typeid(Aron##upperType##Type)), AronAbstractTypeNavigatorFactoryPtr(new AronPrimitiveTypeNavigatorFactory<Aron##upperType##Type, Aron##upperType, cppType>())}, \
HANDLE_PRIMITIVE_TYPES_WITH_CPP_TYPE
#undef RUN_ARON_MACRO
@@ -57,7 +63,7 @@ namespace armarx
throw LocalException("AronAbstractTypeNavigatorFactory: An AronDataPtr is NULL! Cannot create navigator!");
}
- auto factory_iterator = Factories.find(AronTypeDescriptor::GetTypeForAronAbstractType(aron)->getValue());
+ auto factory_iterator = Factories.find(AronResolver::GetTypeForAronAbstractType(aron));
if (factory_iterator == Factories.end())
{
throw LocalException("AronAbstractTypeNavigatorFactory: Cannot find the desired factory. Cannot create navigator!");
@@ -70,16 +76,27 @@ namespace armarx
throw LocalException("AronAbstractTypeNavigatorFactory: Called disallowed method of an AronDataNavigatorFactory. Use child class instead!");
}
- AronAbstractTypeNavigatorPtr AronObjectTypeNavigatorFactory::createSpecific(const AronAbstractTypePtr& aron, const AronNavigatorPath& path) const
- {
- return AronObjectTypeNavigatorPtr(new AronObjectTypeNavigator(aron->cppTypename, path));
- }
+ // Container factories
+#define RUN_ARON_MACRO(upperType, lowerType, capsType) \
+ AronAbstractTypeNavigatorPtr Aron##upperType##TypeNavigatorFactory::createSpecific(const AronAbstractTypePtr& aron, const AronNavigatorPath& path) const \
+ { \
+ Aron##upperType##TypePtr aronCastedType = Aron##upperType##TypePtr::dynamicCast(aron); \
+ return AronAbstractTypeNavigatorPtr(new Aron##upperType##TypeNavigator(aronCastedType, path)); \
+ }
- AronAbstractTypeNavigatorPtr AronListTypeNavigatorFactory::createSpecific(const AronAbstractTypePtr& aron, const AronNavigatorPath& path) const
- {
- AronListTypePtr aronListType = AronListTypePtr::dynamicCast(aron);
- return AronListTypeNavigatorPtr(new AronListTypeNavigator(aronListType->accepted->cppTypename, path));
- }
+ HANDLE_CONTAINER_TYPES
+#undef RUN_ARON_MACRO
+
+ // Complex factories
+#define RUN_ARON_MACRO(upperType, lowerType, capsType) \
+ AronAbstractTypeNavigatorPtr Aron##upperType##TypeNavigatorFactory::createSpecific(const AronAbstractTypePtr& aron, const AronNavigatorPath& path) const \
+ { \
+ Aron##upperType##TypePtr aronCastedType = Aron##upperType##TypePtr::dynamicCast(aron); \
+ return AronAbstractTypeNavigatorPtr(new Aron##upperType##TypeNavigator(aronCastedType, path)); \
+ }
+
+ HANDLE_COMPLEX_TYPES
+#undef RUN_ARON_MACRO
}
}
}
diff --git a/source/RobotAPI/libraries/aron/types/AronAbstractTypeNavigatorFactory.h b/source/RobotAPI/libraries/aron/types/AronAbstractTypeNavigatorFactory.h
index bed6d9e8cfe05854ef573c1a00288f08555d14df..d83280e515a3a9da0b827096daae9dbc41c66997 100644
--- a/source/RobotAPI/libraries/aron/types/AronAbstractTypeNavigatorFactory.h
+++ b/source/RobotAPI/libraries/aron/types/AronAbstractTypeNavigatorFactory.h
@@ -29,12 +29,9 @@
// ArmarXCore
#include <RobotAPI/libraries/aron/AronFactory.h>
-#include <RobotAPI/libraries/aron/types/AronAbstractTypeNavigator.h>
-#include <RobotAPI/libraries/aron/types/AronObjectTypeNavigator.h>
-#include <RobotAPI/libraries/aron/types/AronListTypeNavigator.h>
-#include <RobotAPI/libraries/aron/types/AronPrimitiveTypeNavigator.h>
+#include <RobotAPI/libraries/aron/types/AronAllTypeNavigators.h>
-#include <RobotAPI/libraries/aron/AronTypeDescriptor.h>
+#include <RobotAPI/libraries/aron/AronDescriptor.h>
namespace armarx
{
@@ -46,7 +43,7 @@ namespace armarx
typedef std::shared_ptr<AronAbstractTypeNavigatorFactory> AronAbstractTypeNavigatorFactoryPtr;
class AronAbstractTypeNavigatorFactory :
- virtual public AronFactory<AronAbstractTypePtr, AronAbstractTypeNavigator>
+ virtual public AronFactory<AronAbstractTypePtr, AronAbstractTypeNavigatorPtr>
{
public:
AronAbstractTypeNavigatorFactory() = default;
@@ -54,26 +51,34 @@ namespace armarx
virtual AronAbstractTypeNavigatorPtr createSpecific(const AronAbstractTypePtr&, const AronNavigatorPath&) const override;
private:
- const static std::map<AronTypeDescriptor::Value, AronAbstractTypeNavigatorFactoryPtr> Factories;
+ const static std::map<AronTypeDescriptor, AronAbstractTypeNavigatorFactoryPtr> Factories;
};
- // Object
- class AronObjectTypeNavigatorFactory :
- virtual public AronAbstractTypeNavigatorFactory
- {
- public:
- AronObjectTypeNavigatorFactory() = default;
- virtual AronAbstractTypeNavigatorPtr createSpecific(const AronAbstractTypePtr&, const AronNavigatorPath&) const override;
- };
+ // Container factories
+#define RUN_ARON_MACRO(upperType, lowerType, capsType) \
+ class Aron##upperType##TypeNavigatorFactory : \
+ virtual public AronAbstractTypeNavigatorFactory \
+ { \
+ public: \
+ Aron##upperType##TypeNavigatorFactory() = default; \
+ virtual AronAbstractTypeNavigatorPtr createSpecific(const AronAbstractTypePtr&, const AronNavigatorPath&) const override; \
+ };
- // List
- class AronListTypeNavigatorFactory :
- virtual public AronAbstractTypeNavigatorFactory
- {
- public:
- AronListTypeNavigatorFactory() = default;
- virtual AronAbstractTypeNavigatorPtr createSpecific(const AronAbstractTypePtr&, const AronNavigatorPath&) const override;
- };
+ HANDLE_CONTAINER_TYPES
+#undef RUN_ARON_MACRO
+
+ // Complex factories
+#define RUN_ARON_MACRO(upperType, lowerType, capsType) \
+ class Aron##upperType##TypeNavigatorFactory : \
+ virtual public AronAbstractTypeNavigatorFactory \
+ { \
+ public: \
+ Aron##upperType##TypeNavigatorFactory() = default; \
+ virtual AronAbstractTypeNavigatorPtr createSpecific(const AronAbstractTypePtr&, const AronNavigatorPath&) const override; \
+ };
+
+ HANDLE_COMPLEX_TYPES
+#undef RUN_ARON_MACRO
// Primitive
template<typename AronAbstractTypeType, typename AronDataType, typename CppType> //requires AreAronAbstractTypeAndPrimitiveDependent<AronType, AronDataType, CppType>
diff --git a/source/RobotAPI/libraries/aron/types/AronAllTypeNavigators.h b/source/RobotAPI/libraries/aron/types/AronAllTypeNavigators.h
new file mode 100644
index 0000000000000000000000000000000000000000..bc0cbd71560e0af462b653934ad764437d5425c2
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/types/AronAllTypeNavigators.h
@@ -0,0 +1,53 @@
+/*
+ * 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/>.
+ *
+ * @author Simon Ottenhaus (simon dot ottenhaus at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+// STD/STL
+#include <string>
+#include <map>
+
+// ArmarX
+#include <RobotAPI/interface/aron.h>
+#include <RobotAPI/libraries/aron/types/AronAbstractTypeNavigator.h>
+#include <RobotAPI/libraries/aron/types/AronDictTypeNavigator.h>
+#include <RobotAPI/libraries/aron/types/AronEigenMatrixTypeNavigator.h>
+#include <RobotAPI/libraries/aron/types/AronIVTCByteImageTypeNavigator.h>
+#include <RobotAPI/libraries/aron/types/AronListTypeNavigator.h>
+#include <RobotAPI/libraries/aron/types/AronObjectTypeNavigator.h>
+#include <RobotAPI/libraries/aron/types/AronOpenCVMatTypeNavigator.h>
+#include <RobotAPI/libraries/aron/types/AronPCLPointcloudTypeNavigator.h>
+#include <RobotAPI/libraries/aron/types/AronTupleTypeNavigator.h>
+#include <RobotAPI/libraries/aron/types/AronPrimitiveTypeNavigator.h>
+#include <RobotAPI/libraries/aron/types/AronExternalTypeNavigator.h>
+
+namespace armarx
+{
+ namespace aron
+ {
+ namespace types
+ {
+
+ }
+ }
+}
diff --git a/source/RobotAPI/libraries/aron/types/AronDictTypeNavigator.cpp b/source/RobotAPI/libraries/aron/types/AronDictTypeNavigator.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..00e521c7bc5fa99f6c1b5c9f010b95fb8258d20a
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/types/AronDictTypeNavigator.cpp
@@ -0,0 +1,133 @@
+/*
+ * 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/>.
+ *
+ * @author Simon Ottenhaus (simon dot ottenhaus at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+// Boost
+#include <boost/algorithm/string.hpp>
+
+// Header
+#include "AronDictTypeNavigator.h"
+
+
+namespace armarx
+{
+ namespace aron
+ {
+ namespace types
+ {
+ // constructors
+ AronDictTypeNavigator::AronDictTypeNavigator(const std::string& acceptedTypename, const AronNavigatorPath& path) :
+ AronNavigator<AronTypeDescriptor, AronAbstractType>::AronNavigator(AronTypeDescriptor::eAronDictType),
+ AronAbstractTypeNavigator(AronTypeDescriptor::eAronDictType, "std::map<std::string, " + acceptedTypename + ">", boost::core::demangle(typeid(AronDict).name()), boost::core::demangle(typeid(AronDictType).name())),
+ acceptedTypeNavigator(GetAronAbstractTypePtrByName(acceptedTypename)),
+ type(new AronDictType())
+ {
+ type->acceptedType = acceptedTypeNavigator->getResult();
+ }
+
+ AronDictTypeNavigator::AronDictTypeNavigator(const AronAbstractTypeNavigatorPtr& acceptedType, const AronNavigatorPath& path) :
+ AronNavigator<AronTypeDescriptor, AronAbstractType>::AronNavigator(AronTypeDescriptor::eAronDictType),
+ AronAbstractTypeNavigator(AronTypeDescriptor::eAronDictType, "std::map<std::string, " + acceptedType->getFullCppTypename() + ">", boost::core::demangle(typeid(AronDict).name()), boost::core::demangle(typeid(AronDictType).name())),
+ acceptedTypeNavigator(acceptedType),
+ type(new AronDictType())
+ {
+ type->acceptedType = acceptedTypeNavigator->getResult();
+ }
+
+ // static methods
+ AronDictTypeNavigatorPtr AronDictTypeNavigator::DynamicCast(const AronAbstractTypeNavigatorPtr& n)
+ {
+ AronDictTypeNavigatorPtr casted = std::dynamic_pointer_cast<AronDictTypeNavigator>(n);
+ checkAbstractTypeNavigatorPtrForNull(casted, "");
+ return casted;
+ }
+
+ // virtual implementations
+ AronAbstractTypePtr AronDictTypeNavigator::getResult() const
+ {
+ return type;
+ }
+
+ CppBlockPtr AronDictTypeNavigator::getResetBlock(const std::string& accessor) const
+ {
+ CppBlockPtr b = CppBlockPtr(new CppBlock());
+ b->addLine(accessor + ".clear();");
+ return b;
+ }
+
+ CppBlockPtr AronDictTypeNavigator::getInitializeAronTypeBlock(const std::string& accessor) const
+ {
+ CppBlockPtr b = CppBlockPtr(new CppBlock());
+
+ std::string escaped_accessor = escapeAccessor(accessor);
+ std::string accessor_iterator = escaped_accessor + "_dicttype";
+
+ CppBlockPtr b2 = acceptedTypeNavigator->getInitializeAronTypeBlock(accessor_iterator);
+ b = CppBlock::mergeBlocks(b, b2);
+
+ b->addLine(getFullAronTypePtrTypename() + " " + accessor + " = " + getFullAronTypePtrTypename() + "(new " + getFullAronTypeTypename() + "(" + accessor_iterator + "));");
+ return b;
+ }
+
+ CppBlockPtr AronDictTypeNavigator::getWriteBlock(const std::string& accessor) const
+ {
+ CppBlockPtr b = CppBlockPtr(new CppBlock());
+ b->addLine("w.writeStartDict();");
+
+ std::string escaped_accessor = escapeAccessor(accessor);
+ std::string accessor_iterator_key = escaped_accessor + "_dictkey";
+ std::string accessor_iterator_val = escaped_accessor + "_dictval";
+
+ b->addLine("for (const auto& [" + accessor_iterator_key + ", " + accessor_iterator_val + "] : " + accessor + ") ");
+
+ CppBlockPtr b2 = CppBlockPtr(new CppBlock());
+ b2->addLine("w.writeKey(" + accessor_iterator_key + ");");
+ b2 = CppBlock::mergeBlocks(b2, acceptedTypeNavigator->getWriteBlock(accessor_iterator_val));
+ b->addBlock(b2);
+
+ b->addLine("w.writeEndDict();");
+ return b;
+ }
+
+ CppBlockPtr AronDictTypeNavigator::getReadBlock(const std::string& accessor) const
+ {
+ CppBlockPtr b = CppBlockPtr(new CppBlock());
+ b->addLine("r.readStartDict();");
+
+ std::string escaped_accessor = escapeAccessor(accessor);
+ std::string accessor_iterator = escaped_accessor + "_dictiterator";
+ std::string accessor_iterator_key = escaped_accessor + "_dictkey";
+
+ b->addLine("while(!r.readEndDict())");
+ CppBlockPtr b2 = CppBlockPtr(new CppBlock());
+ b2->addLine("std::string " + accessor_iterator_key + " = r.readKey();");
+ b2->addLine(acceptedTypeNavigator->getFullCppTypename() + " " + accessor_iterator + ";");
+ b2 = CppBlock::mergeBlocks(b2, acceptedTypeNavigator->getReadBlock(accessor_iterator));
+ b2->addLine(accessor + "[" + accessor_iterator_key + "] = " + accessor_iterator + ";");
+
+ b->addBlock(b2);
+ return b;
+ }
+ }
+ }
+}
+
diff --git a/source/RobotAPI/libraries/aron/types/AronDictTypeNavigator.h b/source/RobotAPI/libraries/aron/types/AronDictTypeNavigator.h
new file mode 100644
index 0000000000000000000000000000000000000000..b92bf96ce844510e6109388aea9deedc4893a66b
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/types/AronDictTypeNavigator.h
@@ -0,0 +1,73 @@
+/*
+ * 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/>.
+ *
+ * @author Simon Ottenhaus (simon dot ottenhaus at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+// STD/STL
+#include <string>
+#include <map>
+
+// Base Class
+#include "AronAbstractTypeNavigator.h"
+
+
+namespace armarx
+{
+ namespace aron
+ {
+ namespace types
+ {
+
+ class AronDictTypeNavigator;
+ typedef std::shared_ptr<AronDictTypeNavigator> AronDictTypeNavigatorPtr;
+
+ class AronDictTypeNavigator :
+ virtual public AronAbstractTypeNavigator
+ {
+ public:
+ using PointerType = AronDictTypeNavigatorPtr;
+
+ public:
+ // constructors
+ AronDictTypeNavigator(const AronAbstractTypeNavigatorPtr&, const AronNavigatorPath& path);
+ AronDictTypeNavigator(const std::string&, const AronNavigatorPath& path);
+
+ // static methods
+ static AronDictTypeNavigatorPtr DynamicCast(const AronAbstractTypeNavigatorPtr& n);
+
+ // virtual implementations
+ virtual AronAbstractTypePtr getResult() const override;
+
+ virtual CppBlockPtr getInitializeAronTypeBlock(const std::string&) const override;
+ virtual CppBlockPtr getResetBlock(const std::string&) const override;
+ virtual CppBlockPtr getWriteBlock(const std::string&) const override;
+ virtual CppBlockPtr getReadBlock(const std::string&) const override;
+
+ private:
+ // members
+ AronAbstractTypeNavigatorPtr acceptedTypeNavigator;
+ AronDictTypePtr type;
+ };
+ }
+ }
+}
diff --git a/source/RobotAPI/libraries/aron/types/AronEigenMatrixTypeNavigator.cpp b/source/RobotAPI/libraries/aron/types/AronEigenMatrixTypeNavigator.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..3d7b1091b75b22d8b87faf7eb62bc7f15d4c08d1
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/types/AronEigenMatrixTypeNavigator.cpp
@@ -0,0 +1,140 @@
+/*
+ * 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/>.
+ *
+ * @author Simon Ottenhaus (simon dot ottenhaus at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+// Boost
+#include <boost/algorithm/string.hpp>
+
+// Header
+#include "AronEigenMatrixTypeNavigator.h"
+
+namespace armarx
+{
+ namespace aron
+ {
+ namespace types
+ {
+
+ const std::set<std::string> AronEigenMatrixTypeNavigator::acceptedTypes =
+ {
+ "short",
+ "int",
+ "long",
+ "float",
+ "double",
+ //"std::complex<float>",
+ //"std::complex<double>"
+ };
+
+ namespace
+ {
+ // anonymous helper functions
+ std::string implodeDimensionVector(const std::vector<int>& dimensions)
+ {
+ if (dimensions.size() == 0)
+ {
+ return "";
+ }
+
+ std::stringstream ss;
+ for (unsigned int i = 0; i < dimensions.size() - 1; ++i)
+ {
+ ss << dimensions[i] << ", ";
+ }
+ ss << dimensions[dimensions.size() - 1];
+ return ss.str();
+ }
+ }
+
+ // constructors
+ AronEigenMatrixTypeNavigator::AronEigenMatrixTypeNavigator(const std::vector<int>& dims, const std::string& t, const AronNavigatorPath& path) :
+ AronNavigator<AronTypeDescriptor, AronAbstractType>::AronNavigator(AronTypeDescriptor::eAronEigenMatrixType),
+ AronAbstractTypeNavigator(AronTypeDescriptor::eAronEigenMatrixType, "::Eigen::Matrix<" + t + ", " + implodeDimensionVector(dims) + ">", boost::core::demangle(typeid(AronBlob).name()), boost::core::demangle(typeid(AronEigenMatrixType).name())),
+ dimensions(dims),
+ type(new AronEigenMatrixType())
+ {
+ type->dimensions = dims;
+ type->typeName = t;
+
+ bool found = false;
+ for (const auto& typeName : acceptedTypes)
+ {
+ if (typeName == t)
+ {
+ this->usedType = typeName;
+ found = true;
+ break;
+ }
+ }
+
+ if (!found)
+ {
+ throw LocalException("AronEigenMatrixTypeNavigator: Could not find the typename '" + t + "' in accepted types.");
+ }
+ }
+
+ // static methods
+ AronEigenMatrixTypeNavigatorPtr AronEigenMatrixTypeNavigator::DynamicCast(const AronAbstractTypeNavigatorPtr& n)
+ {
+ AronEigenMatrixTypeNavigatorPtr casted = std::dynamic_pointer_cast<AronEigenMatrixTypeNavigator>(n);
+ checkAbstractTypeNavigatorPtrForNull(casted, "");
+ return casted;
+ }
+
+ // virtual implementations
+ AronAbstractTypePtr AronEigenMatrixTypeNavigator::getResult() const
+ {
+ return type;
+ }
+
+ CppBlockPtr AronEigenMatrixTypeNavigator::getResetBlock(const std::string& accessor) const
+ {
+ CppBlockPtr b = CppBlockPtr(new CppBlock());
+ b->addLine(accessor + " = {};");
+ return b;
+ }
+
+ CppBlockPtr AronEigenMatrixTypeNavigator::getInitializeAronTypeBlock(const std::string& accessor) const
+ {
+ CppBlockPtr b = CppBlockPtr(new CppBlock());
+ b->addLine(getFullAronTypePtrTypename() + " " + accessor + " = " + getFullAronTypePtrTypename() + "(new " + getFullAronTypeTypename() + "());");
+ return b;
+ }
+
+ CppBlockPtr AronEigenMatrixTypeNavigator::getWriteBlock(const std::string& accessor) const
+ {
+ CppBlockPtr b = CppBlockPtr(new CppBlock());
+ b->addLine("w.writeBlob(reinterpret_cast<unsigned char*>(" + accessor + ".data()));");
+ return b;
+ }
+
+ CppBlockPtr AronEigenMatrixTypeNavigator::getReadBlock(const std::string& accessor) const
+ {
+ CppBlockPtr b = CppBlockPtr(new CppBlock());
+ //
+ b->addLine(accessor + " = " + getFullCppTypename() + "(reinterpret_cast<" + usedType + "*>(r.readBlob())); // Eigen already performs memcpy");
+ return b;
+ }
+ }
+ }
+}
+
diff --git a/source/RobotAPI/libraries/aron/types/AronEigenMatrixTypeNavigator.h b/source/RobotAPI/libraries/aron/types/AronEigenMatrixTypeNavigator.h
new file mode 100644
index 0000000000000000000000000000000000000000..a5bd0012ef83374b4f7b5a51a17140c5f19a2cc7
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/types/AronEigenMatrixTypeNavigator.h
@@ -0,0 +1,73 @@
+/*
+ * 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/>.
+ *
+ * @author Simon Ottenhaus (simon dot ottenhaus at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+// STD/STL
+#include <string>
+#include <map>
+
+// Base Class
+#include "AronAbstractTypeNavigator.h"
+
+namespace armarx
+{
+ namespace aron
+ {
+ namespace types
+ {
+
+ class AronEigenMatrixTypeNavigator;
+ typedef std::shared_ptr<AronEigenMatrixTypeNavigator> AronEigenMatrixTypeNavigatorPtr;
+
+ class AronEigenMatrixTypeNavigator :
+ virtual public AronAbstractTypeNavigator
+ {
+ public:
+ using PointerType = AronEigenMatrixTypeNavigatorPtr;
+
+ public:
+ // constructors
+ AronEigenMatrixTypeNavigator(const std::vector<int>&, const std::string&, const AronNavigatorPath& path);
+
+ // static methods
+ static AronEigenMatrixTypeNavigatorPtr DynamicCast(const AronAbstractTypeNavigatorPtr& n);
+
+ // virtual implementations
+ virtual AronAbstractTypePtr getResult() const override;
+
+ virtual CppBlockPtr getInitializeAronTypeBlock(const std::string&) const override;
+ virtual CppBlockPtr getResetBlock(const std::string&) const override;
+ virtual CppBlockPtr getWriteBlock(const std::string&) const override;
+ virtual CppBlockPtr getReadBlock(const std::string&) const override;
+
+ private:
+ // members
+ static const std::set<std::string> acceptedTypes;
+ std::string usedType;
+ std::vector<int> dimensions;
+ AronEigenMatrixTypePtr type;
+ };
+ }
+ }
+}
diff --git a/source/RobotAPI/libraries/aron/types/AronExternalTypeNavigator.cpp b/source/RobotAPI/libraries/aron/types/AronExternalTypeNavigator.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..abb4fde18ba1ed4f869c5750632ad1c97f2ae54d
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/types/AronExternalTypeNavigator.cpp
@@ -0,0 +1,88 @@
+/*
+ * 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/>.
+ *
+ * @author Simon Ottenhaus (simon dot ottenhaus at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+// Boost
+#include <boost/algorithm/string/replace.hpp>
+
+// Header
+#include "AronExternalTypeNavigator.h"
+
+namespace armarx
+{
+ namespace aron
+ {
+ namespace types
+ {
+ // constructors
+ AronExternalTypeNavigator::AronExternalTypeNavigator(const std::string& name, const AronNavigatorPath& path) :
+ AronNavigator<AronTypeDescriptor, AronAbstractType>::AronNavigator(AronTypeDescriptor::eAronListType),
+ AronAbstractTypeNavigator(AronTypeDescriptor::eAronExternalType, name, boost::core::demangle(typeid(AronData).name()), boost::core::demangle(typeid(AronExternalType).name())),
+ type(new AronExternalType())
+ {
+ }
+
+ // static methods
+ AronExternalTypeNavigatorPtr AronExternalTypeNavigator::DynamicCast(const AronAbstractTypeNavigatorPtr& n)
+ {
+ AronExternalTypeNavigatorPtr casted = std::dynamic_pointer_cast<AronExternalTypeNavigator>(n);
+ checkAbstractTypeNavigatorPtrForNull(casted, "");
+ return casted;
+ }
+
+ // virtual implementations
+ AronAbstractTypePtr AronExternalTypeNavigator::getResult() const
+ {
+ throw LocalException("AronExternalTypeNavigator: Called invalid function getResult. Nothing to get!");
+ return type;
+ }
+
+ CppBlockPtr AronExternalTypeNavigator::getResetBlock(const std::string& accessor) const
+ {
+ CppBlockPtr b = CppBlockPtr(new CppBlock());
+ b->addLine(accessor + ".reset();");
+ return b;
+ }
+
+ CppBlockPtr AronExternalTypeNavigator::getInitializeAronTypeBlock(const std::string& accessor) const
+ {
+ CppBlockPtr b = CppBlockPtr(new CppBlock());
+ b->addLine(getFullAronTypePtrTypename() + " " + accessor + " = " + getFullAronTypePtrTypename() + "(new " + getFullAronTypeTypename() + "());");
+ return b;
+ }
+
+ CppBlockPtr AronExternalTypeNavigator::getWriteBlock(const std::string& accessor) const
+ {
+ CppBlockPtr b = CppBlockPtr(new CppBlock());
+ b->addLine(accessor + ".write(w);");
+ return b;
+ }
+
+ CppBlockPtr AronExternalTypeNavigator::getReadBlock(const std::string& accessor) const
+ {
+ CppBlockPtr b = CppBlockPtr(new CppBlock());
+ b->addLine(accessor + ".read(w);");
+ return b;
+ }
+ }
+ }
+}
diff --git a/source/RobotAPI/libraries/aron/types/AronExternalTypeNavigator.h b/source/RobotAPI/libraries/aron/types/AronExternalTypeNavigator.h
new file mode 100644
index 0000000000000000000000000000000000000000..c6c6c381761be93e1edc4ffb99b6a2490b727009
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/types/AronExternalTypeNavigator.h
@@ -0,0 +1,70 @@
+/*
+ * 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/>.
+ *
+ * @author Simon Ottenhaus (simon dot ottenhaus at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+// STD/STL
+#include <string>
+#include <vector>
+
+// Base Class
+#include "AronAbstractTypeNavigator.h"
+
+namespace armarx
+{
+ namespace aron
+ {
+ namespace types
+ {
+ class AronExternalTypeNavigator;
+ typedef std::shared_ptr<AronExternalTypeNavigator> AronExternalTypeNavigatorPtr;
+
+ class AronExternalTypeNavigator :
+ public AronAbstractTypeNavigator
+ {
+ public:
+ using PointerType = AronExternalTypeNavigatorPtr;
+
+ public:
+ // constructors
+ AronExternalTypeNavigator(const std::string&, const AronNavigatorPath& path);
+
+ // static methods
+ static AronExternalTypeNavigatorPtr DynamicCast(const AronAbstractTypeNavigatorPtr& n);
+
+ // virtual implementations
+ virtual AronAbstractTypePtr getResult() const override;
+
+ virtual CppBlockPtr getInitializeAronTypeBlock(const std::string&) const override;
+ virtual CppBlockPtr getResetBlock(const std::string&) const override;
+ virtual CppBlockPtr getWriteBlock(const std::string&) const override;
+ virtual CppBlockPtr getReadBlock(const std::string&) const override;
+
+ private:
+ // members
+ std::string name;
+ AronExternalTypePtr type;
+ };
+ }
+ }
+}
diff --git a/source/RobotAPI/libraries/aron/types/AronIVTCByteImageTypeNavigator.cpp b/source/RobotAPI/libraries/aron/types/AronIVTCByteImageTypeNavigator.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..4487ce55ccda3e875364b986ac210c2c822dd36f
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/types/AronIVTCByteImageTypeNavigator.cpp
@@ -0,0 +1,119 @@
+/*
+ * 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/>.
+ *
+ * @author Simon Ottenhaus (simon dot ottenhaus at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+// Boost
+#include <boost/algorithm/string.hpp>
+
+// Header
+#include "AronIVTCByteImageTypeNavigator.h"
+
+namespace armarx
+{
+ namespace aron
+ {
+ namespace types
+ {
+
+ const std::set<std::string> AronIVTCByteImageTypeNavigator::acceptedTypes =
+ {
+ {"CByteImage::ImageType::eGrayScale"},
+ {"CByteImage::ImageType::eRGB24"},
+ {"CByteImage::ImageType::eRGB24Split"}
+ };
+
+ // constructors
+ AronIVTCByteImageTypeNavigator::AronIVTCByteImageTypeNavigator(const unsigned int w, const unsigned int h, const std::string& t, const AronNavigatorPath& path) :
+ AronNavigator<AronTypeDescriptor, AronAbstractType>::AronNavigator(AronTypeDescriptor::eAronIVTCByteImageType),
+ AronAbstractTypeNavigator(AronTypeDescriptor::eAronIVTCByteImageType, "CByteImage", boost::core::demangle(typeid(AronBlob).name()), boost::core::demangle(typeid(AronIVTCByteImageType).name())),
+ width(w),
+ height(h),
+ type(new AronIVTCByteImageType())
+ {
+ type->width = w;
+ type->height = h;
+ type->typeName = t;
+
+ bool found = false;
+ for (const auto& typeName : acceptedTypes)
+ {
+ if (typeName == t)
+ {
+ this->usedType = typeName;
+ found = true;
+ break;
+ }
+ }
+
+ if (!found)
+ {
+ throw LocalException("AronIVTCByteImageTypeNavigator: Could not find the typename '" + t + "' in accepted types.");
+ }
+ }
+
+ // static methods
+ AronIVTCByteImageTypeNavigatorPtr AronIVTCByteImageTypeNavigator::DynamicCast(const AronAbstractTypeNavigatorPtr& n)
+ {
+ AronIVTCByteImageTypeNavigatorPtr casted = std::dynamic_pointer_cast<AronIVTCByteImageTypeNavigator>(n);
+ checkAbstractTypeNavigatorPtrForNull(casted, "");
+ return casted;
+ }
+
+ // virtual implementations
+ AronAbstractTypePtr AronIVTCByteImageTypeNavigator::getResult() const
+ {
+ return type;
+ }
+
+ CppBlockPtr AronIVTCByteImageTypeNavigator::getResetBlock(const std::string& accessor) const
+ {
+ CppBlockPtr b = CppBlockPtr(new CppBlock());
+ b->addLine(accessor + " = " + getFullCppTypename() + "();");
+ return b;
+ }
+
+ CppBlockPtr AronIVTCByteImageTypeNavigator::getInitializeAronTypeBlock(const std::string& accessor) const
+ {
+ CppBlockPtr b = CppBlockPtr(new CppBlock());
+ b->addLine(getFullAronTypePtrTypename() + " " + accessor + " = " + getFullAronTypePtrTypename() + "(new " + getFullAronTypeTypename() + "());");
+ return b;
+ }
+
+ CppBlockPtr AronIVTCByteImageTypeNavigator::getWriteBlock(const std::string& accessor) const
+ {
+ CppBlockPtr b = CppBlockPtr(new CppBlock());
+ b->addLine("w.writeBlob(reinterpret_cast<unsigned char*>(" + accessor + ".pixels));");
+ return b;
+ }
+
+ CppBlockPtr AronIVTCByteImageTypeNavigator::getReadBlock(const std::string& accessor) const
+ {
+ CppBlockPtr b = CppBlockPtr(new CppBlock());
+ //CByteImage(int nImageWidth, int nImageHeight, ImageType imageType, bool bHeaderOnly = false);
+ b->addLine(accessor + " = " + getFullCppTypename() + "(" + std::to_string(width) + ", " + std::to_string(height) + ", " + usedType + ", false);");
+ b->addLine("memcpy(" + accessor + ".pixels, reinterpret_cast<unsiged char*>(r.readBlob()), " + accessor + ".width * " + accessor + ".height * " + accessor + ".bytesPerPixel);");
+ return b;
+ }
+ }
+ }
+}
+
diff --git a/source/RobotAPI/libraries/aron/types/AronIVTCByteImageTypeNavigator.h b/source/RobotAPI/libraries/aron/types/AronIVTCByteImageTypeNavigator.h
new file mode 100644
index 0000000000000000000000000000000000000000..a3c69efe4becbe58d489e5114d22192842cab2b5
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/types/AronIVTCByteImageTypeNavigator.h
@@ -0,0 +1,74 @@
+/*
+ * 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/>.
+ *
+ * @author Simon Ottenhaus (simon dot ottenhaus at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+// STD/STL
+#include <string>
+#include <map>
+
+// Base Class
+#include "AronAbstractTypeNavigator.h"
+
+namespace armarx
+{
+ namespace aron
+ {
+ namespace types
+ {
+
+ class AronIVTCByteImageTypeNavigator;
+ typedef std::shared_ptr<AronIVTCByteImageTypeNavigator> AronIVTCByteImageTypeNavigatorPtr;
+
+ class AronIVTCByteImageTypeNavigator :
+ virtual public AronAbstractTypeNavigator
+ {
+ public:
+ using PointerType = AronIVTCByteImageTypeNavigatorPtr;
+
+ public:
+ // constructors
+ AronIVTCByteImageTypeNavigator(const unsigned int w, const unsigned int h, const std::string&, const AronNavigatorPath& path);
+
+ // static methods
+ static AronIVTCByteImageTypeNavigatorPtr DynamicCast(const AronAbstractTypeNavigatorPtr& n);
+
+ // virtual implementations
+ virtual AronAbstractTypePtr getResult() const override;
+
+ virtual CppBlockPtr getInitializeAronTypeBlock(const std::string&) const override;
+ virtual CppBlockPtr getResetBlock(const std::string&) const override;
+ virtual CppBlockPtr getWriteBlock(const std::string&) const override;
+ virtual CppBlockPtr getReadBlock(const std::string&) const override;
+
+ private:
+ // members
+ static const std::set<std::string> acceptedTypes;
+ std::string usedType;
+ unsigned int width;
+ unsigned int height;
+ AronIVTCByteImageTypePtr type;
+ };
+ }
+ }
+}
diff --git a/source/RobotAPI/libraries/aron/types/AronListTypeNavigator.cpp b/source/RobotAPI/libraries/aron/types/AronListTypeNavigator.cpp
index 84c1efdebee5a54bab809b220d371076a2dc7781..e3cc04075ee91dcc184ee93c88756877144fdb70 100644
--- a/source/RobotAPI/libraries/aron/types/AronListTypeNavigator.cpp
+++ b/source/RobotAPI/libraries/aron/types/AronListTypeNavigator.cpp
@@ -33,35 +33,67 @@ namespace armarx
{
namespace types
{
+ // constructors
AronListTypeNavigator::AronListTypeNavigator(const AronAbstractTypeNavigatorPtr& acceptedType, const AronNavigatorPath& path) :
- AronAbstractTypeNavigator("std::vector<" + acceptedType->getFullCppTypename() + ">", boost::core::demangle(typeid(AronList).name()), path),
+ AronNavigator<AronTypeDescriptor, AronAbstractType>::AronNavigator(AronTypeDescriptor::eAronListType),
+ AronAbstractTypeNavigator(AronTypeDescriptor::eAronListType, "std::vector<" + acceptedType->getFullCppTypename() + ">", boost::core::demangle(typeid(AronList).name()), boost::core::demangle(typeid(AronListType).name())),
acceptedTypeNavigator(acceptedType),
type(new AronListType())
{
- type->cppTypename = getFullCppTypename();
- type->accepted = acceptedTypeNavigator->getResult();
+ type->acceptedType = acceptedTypeNavigator->getResult();
}
+
AronListTypeNavigator::AronListTypeNavigator(const std::string& acceptedTypename, const AronNavigatorPath& path) :
- AronAbstractTypeNavigator("std::vector<" + acceptedTypename + ">", boost::core::demangle(typeid(AronList).name()), path),
+ AronNavigator<AronTypeDescriptor, AronAbstractType>::AronNavigator(AronTypeDescriptor::eAronListType),
+ AronAbstractTypeNavigator(AronTypeDescriptor::eAronListType, "std::vector<" + acceptedTypename + ">", boost::core::demangle(typeid(AronList).name()), boost::core::demangle(typeid(AronListType).name())),
acceptedTypeNavigator(GetAronAbstractTypePtrByName(acceptedTypename)),
type(new AronListType())
{
- type->cppTypename = getFullCppTypename();
- type->accepted = acceptedTypeNavigator->getResult();
+ type->acceptedType = acceptedTypeNavigator->getResult();
}
+ // static methods
+ AronListTypeNavigatorPtr AronListTypeNavigator::DynamicCast(const AronAbstractTypeNavigatorPtr& n)
+ {
+ AronListTypeNavigatorPtr casted = std::dynamic_pointer_cast<AronListTypeNavigator>(n);
+ checkAbstractTypeNavigatorPtrForNull(casted, "");
+ return casted;
+ }
+
+ // virtual implementations
AronAbstractTypePtr AronListTypeNavigator::getResult() const
{
return type;
}
+ CppBlockPtr AronListTypeNavigator::getResetBlock(const std::string& accessor) const
+ {
+ CppBlockPtr b = CppBlockPtr(new CppBlock());
+ b->addLine(accessor + ".clear();");
+ return b;
+ }
+
+ CppBlockPtr AronListTypeNavigator::getInitializeAronTypeBlock(const std::string& accessor) const
+ {
+ CppBlockPtr b = CppBlockPtr(new CppBlock());
+
+ std::string escaped_accessor = escapeAccessor(accessor);
+ std::string accessor_iterator = escaped_accessor + "_listtype";
+
+ CppBlockPtr b2 = acceptedTypeNavigator->getInitializeAronTypeBlock(accessor_iterator);
+ b = CppBlock::mergeBlocks(b, b2);
+
+ b->addLine(getFullAronTypePtrTypename() + " " + accessor + " = " + getFullAronTypePtrTypename() + "(new " + getFullAronTypeTypename() + "(" + accessor_iterator + "));");
+ return b;
+ }
+
CppBlockPtr AronListTypeNavigator::getWriteBlock(const std::string& accessor) const
{
CppBlockPtr b = CppBlockPtr(new CppBlock());
b->addLine("w.writeStartList();");
std::string escaped_accessor = escapeAccessor(accessor);
- std::string accessor_iterator = escaped_accessor + "_index";
+ std::string accessor_iterator = escaped_accessor + "_listiterator";
b->addLine("for(unsigned int " + accessor_iterator + " = 0; " + accessor_iterator + " < " + accessor + ".size(); ++" + accessor_iterator + ")");
CppBlockPtr b2 = acceptedTypeNavigator->getWriteBlock(accessor + "[" + accessor_iterator + "]");
@@ -74,14 +106,13 @@ namespace armarx
{
CppBlockPtr b = CppBlockPtr(new CppBlock());
b->addLine("r.readStartList();");
- b->addLine(accessor + ".clear();");
std::string escaped_accessor = escapeAccessor(accessor);
- std::string accessor_iterator = escaped_accessor + "_iterator";
+ std::string accessor_iterator = escaped_accessor + "_listiterator";
b->addLine("while(!r.readEndList())");
CppBlockPtr b2 = CppBlockPtr(new CppBlock());
- b2->addLine(acceptedTypeNavigator->getFullCppTypename() + " " + accessor_iterator + " = 0;");
+ b2->addLine(acceptedTypeNavigator->getFullCppTypename() + " " + accessor_iterator + ";");
b2 = CppBlock::mergeBlocks(b2, acceptedTypeNavigator->getReadBlock(accessor_iterator));
b2->addLine(accessor + ".push_back(" + accessor_iterator + ");");
diff --git a/source/RobotAPI/libraries/aron/types/AronListTypeNavigator.h b/source/RobotAPI/libraries/aron/types/AronListTypeNavigator.h
index 1d59da0968449157ef1f266cbcdc9af2c365a6b0..04e39056c4c61be8b4d49b7c9ae56e1036cf2f10 100644
--- a/source/RobotAPI/libraries/aron/types/AronListTypeNavigator.h
+++ b/source/RobotAPI/libraries/aron/types/AronListTypeNavigator.h
@@ -45,15 +45,24 @@ namespace armarx
public:
using PointerType = AronListTypeNavigatorPtr;
+ public:
+ // constructors
AronListTypeNavigator(const AronAbstractTypeNavigatorPtr&, const AronNavigatorPath& path);
AronListTypeNavigator(const std::string&, const AronNavigatorPath& path);
+ // static methods
+ static AronListTypeNavigatorPtr DynamicCast(const AronAbstractTypeNavigatorPtr& n);
+
+ // virtual implementations
virtual AronAbstractTypePtr getResult() const override;
+ virtual CppBlockPtr getInitializeAronTypeBlock(const std::string&) const override;
+ virtual CppBlockPtr getResetBlock(const std::string&) const override;
virtual CppBlockPtr getWriteBlock(const std::string&) const override;
virtual CppBlockPtr getReadBlock(const std::string&) const override;
private:
+ // members
AronAbstractTypeNavigatorPtr acceptedTypeNavigator;
AronListTypePtr type;
};
diff --git a/source/RobotAPI/libraries/aron/types/AronObjectTypeNavigator.cpp b/source/RobotAPI/libraries/aron/types/AronObjectTypeNavigator.cpp
index b674d6dd6dccd4cf7547ea3080cdd73519deb6a9..d28e5503c9083846e879c772b42269d8dd25a21e 100644
--- a/source/RobotAPI/libraries/aron/types/AronObjectTypeNavigator.cpp
+++ b/source/RobotAPI/libraries/aron/types/AronObjectTypeNavigator.cpp
@@ -35,17 +35,44 @@ namespace armarx
namespace types
{
+ // constructors
+ AronObjectTypeNavigator::AronObjectTypeNavigator(const std::string& cppName, const std::map<std::string, AronAbstractTypeNavigatorPtr>& children, const AronNavigatorPath& path) :
+ AronNavigator<AronTypeDescriptor, AronAbstractType>::AronNavigator(AronTypeDescriptor::eAronObjectType),
+ AronAbstractTypeNavigator(AronTypeDescriptor::eAronObjectType, cppName, boost::core::demangle(typeid(AronDict).name()), boost::core::demangle(typeid(AronObjectType).name())),
+ childrenNavigators(children),
+ type(new AronObjectType())
+ {
+ AddObjectTypeToAllObjectTypesList(AronObjectTypeNavigatorPtr(this));
+
+ for (const auto& [key, nav] : childrenNavigators)
+ {
+ type->elementTypes[key] = nav->getResult();
+ }
+ }
+
AronObjectTypeNavigator::AronObjectTypeNavigator(const std::string& cppName, const AronNavigatorPath& path) :
- AronAbstractTypeNavigator(cppName, boost::core::demangle(typeid(AronObject).name()), path),
+ AronNavigator<AronTypeDescriptor, AronAbstractType>::AronNavigator(AronTypeDescriptor::eAronObjectType),
+ AronAbstractTypeNavigator(AronTypeDescriptor::eAronObjectType, cppName, boost::core::demangle(typeid(AronDict).name()), boost::core::demangle(typeid(AronObjectType).name())),
type(new AronObjectType())
{
- type->cppTypename = getCppTypename();
+ AddObjectTypeToAllObjectTypesList(AronObjectTypeNavigatorPtr(this));
+
+ type->name = getFullCppTypename();
}
+ // static methods
+ AronObjectTypeNavigatorPtr AronObjectTypeNavigator::DynamicCast(const AronAbstractTypeNavigatorPtr& n)
+ {
+ AronObjectTypeNavigatorPtr casted = std::dynamic_pointer_cast<AronObjectTypeNavigator>(n);
+ checkAbstractTypeNavigatorPtrForNull(casted, "");
+ return casted;
+ }
+
+ // public member functions
void AronObjectTypeNavigator::addElement(const std::string& k, const AronAbstractTypeNavigatorPtr& v)
{
childrenNavigators[k] = v;
- type->elements[k] = v->getResult();
+ type->elementTypes[k] = v->getResult();
}
std::map<std::string, AronAbstractTypeNavigatorPtr> AronObjectTypeNavigator::getChildren() const
@@ -53,36 +80,69 @@ namespace armarx
return childrenNavigators;
}
+ // virtual implementations
AronAbstractTypePtr AronObjectTypeNavigator::getResult() const
{
return type;
}
+ CppBlockPtr AronObjectTypeNavigator::getResetBlock(const std::string& accessor) const
+ {
+ CppBlockPtr b = CppBlockPtr(new CppBlock());
+ for (const auto& [key, child] : childrenNavigators)
+ {
+ CppBlockPtr b2 = child->getResetBlock((accessor != "" ? (accessor + "." + key) : key));
+ b = CppBlock::mergeBlocks(b, b2);
+ }
+ return b;
+ }
+
+ CppBlockPtr AronObjectTypeNavigator::getInitializeAronTypeBlock(const std::string& accessor) const
+ {
+ CppBlockPtr b = CppBlockPtr(new CppBlock());
+
+ std::string escaped_accessor = escapeAccessor(accessor);
+ std::string accessor_iterator_container = escaped_accessor + "_objectiteratorcontainer";
+
+ b->addLine("std::map<std::string, " + ARON_TYPE_PTR_NAME + "> " + accessor_iterator_container + ";");
+
+ for (const auto& [key, child] : childrenNavigators)
+ {
+ std::string accessor_iterator = escaped_accessor + "_" + key;
+ CppBlockPtr b2 = child->getInitializeAronTypeBlock(accessor_iterator);
+ b = CppBlock::mergeBlocks(b, b2);
+
+ b->addLine(accessor_iterator_container + "[\"" + key + "\"] = " + accessor_iterator + ";");
+ }
+ b->addLine(getFullAronTypePtrTypename() + " " + accessor + " = " + getFullAronTypePtrTypename() + "(new " + getFullAronTypeTypename() + "(\"" + getFullCppTypename() + "\", " + accessor_iterator_container + "));");
+ return b;
+ }
+
CppBlockPtr AronObjectTypeNavigator::getWriteBlock(const std::string& accessor) const
{
CppBlockPtr b = CppBlockPtr(new CppBlock());
- b->addLine("w.writeStartObject();");
+ b->addLine("w.writeStartDict();");
for (const auto& [key, child] : childrenNavigators)
{
b->addLine("w.writeKey(\"" + key + "\");");
CppBlockPtr b2 = child->getWriteBlock((accessor != "" ? (accessor + "." + key) : key));
b = CppBlock::mergeBlocks(b, b2);
}
- b->addLine("w.writeEndObject();");
+ b->addLine("w.writeEndDict();");
return b;
}
CppBlockPtr AronObjectTypeNavigator::getReadBlock(const std::string& accessor) const
{
CppBlockPtr b = CppBlockPtr(new CppBlock());
- b->addLine("r.readStartObject();");
+ b->addLine("r.readStartDict();");
for (const auto& [key, child] : childrenNavigators)
{
- b->addLine("r.readKey(\"" + key + "\");");
+ b->addLine("r.readMember(\"" + key + "\");");
CppBlockPtr b2 = child->getReadBlock((accessor != "" ? (accessor + "." + key) : key));
b = CppBlock::mergeBlocks(b, b2);
}
- b->addLine("r.readEndObject();");
+ b->addLine("r.readEndDict();");
return b;
}
}
diff --git a/source/RobotAPI/libraries/aron/types/AronObjectTypeNavigator.h b/source/RobotAPI/libraries/aron/types/AronObjectTypeNavigator.h
index 5a4543bf8242e35dae0998db9771cf35a64fd98b..c9ac78785dd0c7b555a2ad0ad5bf4da2565efda8 100644
--- a/source/RobotAPI/libraries/aron/types/AronObjectTypeNavigator.h
+++ b/source/RobotAPI/libraries/aron/types/AronObjectTypeNavigator.h
@@ -47,17 +47,28 @@ namespace armarx
public:
using PointerType = AronObjectTypeNavigatorPtr;
- AronObjectTypeNavigator(const std::string&, const AronNavigatorPath& path);
+ public:
+ // constructors
+ AronObjectTypeNavigator(const std::string&, const std::map<std::string, AronAbstractTypeNavigatorPtr>&, const AronNavigatorPath&);
+ AronObjectTypeNavigator(const std::string&, const AronNavigatorPath&);
+ // public member functions
void addElement(const std::string& k, const AronAbstractTypeNavigatorPtr& v);
std::map<std::string, AronAbstractTypeNavigatorPtr> getChildren() const;
+ // static methods
+ static AronObjectTypeNavigatorPtr DynamicCast(const AronAbstractTypeNavigatorPtr& n);
+
+ // virtual implementations
virtual AronAbstractTypePtr getResult() const override;
+ virtual CppBlockPtr getInitializeAronTypeBlock(const std::string&) const override;
+ virtual CppBlockPtr getResetBlock(const std::string&) const override;
virtual CppBlockPtr getWriteBlock(const std::string&) const override;
virtual CppBlockPtr getReadBlock(const std::string&) const override;
private:
+ // members
std::map<std::string, AronAbstractTypeNavigatorPtr> childrenNavigators;
AronObjectTypePtr type;
};
diff --git a/source/RobotAPI/libraries/aron/types/AronOpenCVMatTypeNavigator.cpp b/source/RobotAPI/libraries/aron/types/AronOpenCVMatTypeNavigator.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..ffe078bd6b28f201f9b2743e3e5c3f97e6d057db
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/types/AronOpenCVMatTypeNavigator.cpp
@@ -0,0 +1,117 @@
+/*
+ * 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/>.
+ *
+ * @author Simon Ottenhaus (simon dot ottenhaus at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+// Boost
+#include <boost/algorithm/string.hpp>
+
+// Header
+#include "AronOpenCVMatTypeNavigator.h"
+
+namespace armarx
+{
+ namespace aron
+ {
+ namespace types
+ {
+
+ const std::set<std::string> AronOpenCVMatTypeNavigator::acceptedTypes =
+ {
+ "",
+ "",
+ "",
+ ""
+ };
+
+ // constructors
+ AronOpenCVMatTypeNavigator::AronOpenCVMatTypeNavigator(const std::vector<int>& dims, const std::string& t, const AronNavigatorPath& path) :
+ AronNavigator<AronTypeDescriptor, AronAbstractType>::AronNavigator(AronTypeDescriptor::eAronOpenCVMatType),
+ AronAbstractTypeNavigator(AronTypeDescriptor::eAronOpenCVMatType, "cv::Mat", boost::core::demangle(typeid(AronBlob).name()), boost::core::demangle(typeid(AronOpenCVMatType).name())),
+ dimensions(dims),
+ type(new AronOpenCVMatType())
+ {
+ type->dimensions = dims;
+ type->typeName = t;
+
+ bool found = false;
+ for (const auto& typeName : acceptedTypes)
+ {
+ if (typeName == t)
+ {
+ this->usedType = typeName;
+ found = true;
+ break;
+ }
+ }
+
+ if (!found)
+ {
+ throw LocalException("AronEigenTypeNavigator: Could not find the typename '" + t + "' in accepted types.");
+ }
+ }
+
+ // static methods
+ AronOpenCVMatTypeNavigatorPtr AronOpenCVMatTypeNavigator::DynamicCast(const AronAbstractTypeNavigatorPtr& n)
+ {
+ AronOpenCVMatTypeNavigatorPtr casted = std::dynamic_pointer_cast<AronOpenCVMatTypeNavigator>(n);
+ checkAbstractTypeNavigatorPtrForNull(casted, "");
+ return casted;
+ }
+
+ // virtual implementations
+ AronAbstractTypePtr AronOpenCVMatTypeNavigator::getResult() const
+ {
+ return type;
+ }
+
+ CppBlockPtr AronOpenCVMatTypeNavigator::getResetBlock(const std::string& accessor) const
+ {
+ CppBlockPtr b = CppBlockPtr(new CppBlock());
+ b->addLine(accessor + " = " + getFullCppTypename() + "();");
+ return b;
+ }
+
+ CppBlockPtr AronOpenCVMatTypeNavigator::getInitializeAronTypeBlock(const std::string& accessor) const
+ {
+ CppBlockPtr b = CppBlockPtr(new CppBlock());
+ b->addLine(getFullAronTypePtrTypename() + " " + accessor + " = " + getFullAronTypePtrTypename() + "(new " + getFullAronTypeTypename() + "());");
+ return b;
+ }
+
+ CppBlockPtr AronOpenCVMatTypeNavigator::getWriteBlock(const std::string& accessor) const
+ {
+ CppBlockPtr b = CppBlockPtr(new CppBlock());
+ b->addLine("w.writeBlob(reinterpret_cast<unsigned char*>(" + accessor + ".pixels));");
+ return b;
+ }
+
+ CppBlockPtr AronOpenCVMatTypeNavigator::getReadBlock(const std::string& accessor) const
+ {
+ CppBlockPtr b = CppBlockPtr(new CppBlock());
+ //
+ //b->addLine(accessor + " = " + getFullCppTypename() + "({" + implodeDimensionVector(dimensions) + "}, " + usedType + ", reinterpret_cast<unsigned char*>(r.readBlob()), 0);");
+ return b;
+ }
+ }
+ }
+}
+
diff --git a/source/RobotAPI/libraries/aron/types/AronOpenCVMatTypeNavigator.h b/source/RobotAPI/libraries/aron/types/AronOpenCVMatTypeNavigator.h
new file mode 100644
index 0000000000000000000000000000000000000000..caeeeba4297b4fd6edca9fd53b5deef4c72825e0
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/types/AronOpenCVMatTypeNavigator.h
@@ -0,0 +1,73 @@
+/*
+ * 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/>.
+ *
+ * @author Simon Ottenhaus (simon dot ottenhaus at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+// STD/STL
+#include <string>
+#include <map>
+
+// Base Class
+#include "AronAbstractTypeNavigator.h"
+
+namespace armarx
+{
+ namespace aron
+ {
+ namespace types
+ {
+
+ class AronOpenCVMatTypeNavigator;
+ typedef std::shared_ptr<AronOpenCVMatTypeNavigator> AronOpenCVMatTypeNavigatorPtr;
+
+ class AronOpenCVMatTypeNavigator :
+ virtual public AronAbstractTypeNavigator
+ {
+ public:
+ using PointerType = AronOpenCVMatTypeNavigatorPtr;
+
+ public:
+ // constructors
+ AronOpenCVMatTypeNavigator(const std::vector<int>&, const std::string&, const AronNavigatorPath& path);
+
+ // static methods
+ static AronOpenCVMatTypeNavigatorPtr DynamicCast(const AronAbstractTypeNavigatorPtr& n);
+
+ // virtual implementations
+ virtual AronAbstractTypePtr getResult() const override;
+
+ virtual CppBlockPtr getInitializeAronTypeBlock(const std::string&) const override;
+ virtual CppBlockPtr getResetBlock(const std::string&) const override;
+ virtual CppBlockPtr getWriteBlock(const std::string&) const override;
+ virtual CppBlockPtr getReadBlock(const std::string&) const override;
+
+ private:
+ // members
+ static const std::set<std::string> acceptedTypes;
+ std::string usedType;
+ std::vector<int> dimensions;
+ AronOpenCVMatTypePtr type;
+ };
+ }
+ }
+}
diff --git a/source/RobotAPI/libraries/aron/types/AronPCLPointcloudTypeNavigator.cpp b/source/RobotAPI/libraries/aron/types/AronPCLPointcloudTypeNavigator.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..177e580c03e7aee2bc7507782a6e6add68aa95f1
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/types/AronPCLPointcloudTypeNavigator.cpp
@@ -0,0 +1,118 @@
+/*
+ * 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/>.
+ *
+ * @author Simon Ottenhaus (simon dot ottenhaus at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+// Boost
+#include <boost/algorithm/string.hpp>
+
+// Header
+#include "AronPCLPointcloudTypeNavigator.h"
+
+namespace armarx
+{
+ namespace aron
+ {
+ namespace types
+ {
+
+ const std::vector<std::string> AronPCLPointcloudTypeNavigator::acceptedTypes =
+ {
+ "pcl::PointXYZRGBL",
+ "",
+ "",
+ ""
+ };
+
+ // constructors
+ AronPCLPointcloudTypeNavigator::AronPCLPointcloudTypeNavigator(const unsigned int w, const unsigned int h, const std::string& t, const AronNavigatorPath& path) :
+ AronNavigator<AronTypeDescriptor, AronAbstractType>::AronNavigator(AronTypeDescriptor::eAronPCLPointcloudType),
+ AronAbstractTypeNavigator(AronTypeDescriptor::eAronPCLPointcloudType, "pcl::PointCloud<" + t + ">", boost::core::demangle(typeid(AronBlob).name()), boost::core::demangle(typeid(AronPCLPointcloudType).name())),
+ width(w),
+ height(h),
+ type(new AronPCLPointcloudType())
+ {
+ type->typeName = t;
+ type->width = w;
+ type->height = h;
+
+ bool found = false;
+ for (const auto& typeName : acceptedTypes)
+ {
+ if (typeName == t)
+ {
+ this->usedType = typeName;
+ found = true;
+ break;
+ }
+ }
+
+ if (!found)
+ {
+ throw LocalException("AronEigenTypeNavigator: Could not find the typename '" + t + "' in accepted types.");
+ }
+ }
+
+ // static methods
+ AronPCLPointcloudTypeNavigatorPtr AronPCLPointcloudTypeNavigator::DynamicCast(const AronAbstractTypeNavigatorPtr& n)
+ {
+ AronPCLPointcloudTypeNavigatorPtr casted = std::dynamic_pointer_cast<AronPCLPointcloudTypeNavigator>(n);
+ checkAbstractTypeNavigatorPtrForNull(casted, "");
+ return casted;
+ }
+
+ // virtual implementations
+ AronAbstractTypePtr AronPCLPointcloudTypeNavigator::getResult() const
+ {
+ return type;
+ }
+
+ CppBlockPtr AronPCLPointcloudTypeNavigator::getResetBlock(const std::string& accessor) const
+ {
+ CppBlockPtr b = CppBlockPtr(new CppBlock());
+ return b;
+ }
+
+ CppBlockPtr AronPCLPointcloudTypeNavigator::getInitializeAronTypeBlock(const std::string& accessor) const
+ {
+ CppBlockPtr b = CppBlockPtr(new CppBlock());
+ return b;
+ }
+
+ CppBlockPtr AronPCLPointcloudTypeNavigator::getWriteBlock(const std::string& accessor) const
+ {
+ CppBlockPtr b = CppBlockPtr(new CppBlock());
+ b->addLine("w.writeBlob(reinterpret_cast<unsigned char*>(" + accessor + ".points.data()));");
+ return b;
+ }
+
+ CppBlockPtr AronPCLPointcloudTypeNavigator::getReadBlock(const std::string& accessor) const
+ {
+ CppBlockPtr b = CppBlockPtr(new CppBlock());
+ //PointCloud (uint32_t width_, uint32_t height_, const PointT& value_ = PointT ())
+ b->addLine(accessor + " = " + getFullCppTypename() + "(" + std::to_string(width) + ", " + std::to_string(height) + ", " + usedType + ");");
+ b->addLine("memcpy(" + accessor + ".points.data(), reinterpret_cast<" + usedType + "*>(r.readBlob()), " + accessor + ".width * " + accessor + ".height);");
+ return b;
+ }
+ }
+ }
+}
+
diff --git a/source/RobotAPI/libraries/aron/types/AronPCLPointcloudTypeNavigator.h b/source/RobotAPI/libraries/aron/types/AronPCLPointcloudTypeNavigator.h
new file mode 100644
index 0000000000000000000000000000000000000000..8737f4745afa33e18378ba7052b0458cbaf28f16
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/types/AronPCLPointcloudTypeNavigator.h
@@ -0,0 +1,74 @@
+/*
+ * 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/>.
+ *
+ * @author Simon Ottenhaus (simon dot ottenhaus at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+// STD/STL
+#include <string>
+#include <map>
+
+// Base Class
+#include "AronAbstractTypeNavigator.h"
+
+namespace armarx
+{
+ namespace aron
+ {
+ namespace types
+ {
+
+ class AronPCLPointcloudTypeNavigator;
+ typedef std::shared_ptr<AronPCLPointcloudTypeNavigator> AronPCLPointcloudTypeNavigatorPtr;
+
+ class AronPCLPointcloudTypeNavigator :
+ virtual public AronAbstractTypeNavigator
+ {
+ public:
+ using PointerType = AronPCLPointcloudTypeNavigatorPtr;
+
+ public:
+ // constructors
+ AronPCLPointcloudTypeNavigator(const unsigned int, const unsigned int, const std::string&, const AronNavigatorPath& path);
+
+ // static methods
+ static AronPCLPointcloudTypeNavigatorPtr DynamicCast(const AronAbstractTypeNavigatorPtr& n);
+
+ // virtual implementations
+ virtual AronAbstractTypePtr getResult() const override;
+
+ virtual CppBlockPtr getInitializeAronTypeBlock(const std::string&) const override;
+ virtual CppBlockPtr getResetBlock(const std::string&) const override;
+ virtual CppBlockPtr getWriteBlock(const std::string&) const override;
+ virtual CppBlockPtr getReadBlock(const std::string&) const override;
+
+ private:
+ // members
+ static const std::vector<std::string> acceptedTypes;
+ std::string usedType;
+ unsigned int width;
+ unsigned int height;
+ AronPCLPointcloudTypePtr type;
+ };
+ }
+ }
+}
diff --git a/source/RobotAPI/libraries/aron/types/AronPrimitiveTypeNavigator.h b/source/RobotAPI/libraries/aron/types/AronPrimitiveTypeNavigator.h
index c70b4e4f247276fdec4d197ce39ff483dc85bdc0..495474a20b59b2eb77cf0690f842a91c09a31c60 100644
--- a/source/RobotAPI/libraries/aron/types/AronPrimitiveTypeNavigator.h
+++ b/source/RobotAPI/libraries/aron/types/AronPrimitiveTypeNavigator.h
@@ -51,26 +51,52 @@ namespace armarx
public:
using PointerType = AronPrimitiveTypeNavigatorPtr<AronAbstractTypeType, AronDataType, CppType>;
+ public:
+ // constructors
AronPrimitiveTypeNavigator(const AronNavigatorPath& path) :
- AronAbstractTypeNavigator(boost::core::demangle(typeid(CppType).name()), boost::core::demangle(typeid(AronDataType).name()), path),
+ AronNavigator<AronTypeDescriptor, AronAbstractType>::AronNavigator(AronResolver::GetTypeForAronAbstractTypeId(typeid(AronAbstractTypeType))),
+ AronAbstractTypeNavigator(AronResolver::GetTypeForAronAbstractTypeId(typeid(AronAbstractTypeType)), boost::core::demangle(typeid(CppType).name()), boost::core::demangle(typeid(AronDataType).name()), boost::core::demangle(typeid(AronAbstractTypeType).name())),
type(new AronAbstractTypeType())
{
- type->cppTypename = getCppTypename();
+
+ }
+
+ // static methods
+ AronPrimitiveTypeNavigatorPtr<AronAbstractTypeType, AronDataType, CppType> DynamicCast(const AronAbstractTypeNavigatorPtr& n)
+ {
+ AronPrimitiveTypeNavigatorPtr<AronAbstractTypeType, AronDataType, CppType> casted = std::dynamic_pointer_cast<AronPrimitiveTypeNavigator<AronAbstractTypeType, AronDataType, CppType>>(n);
+ checkAbstractTypeNavigatorPtrForNull(casted, "");
+ return casted;
}
+ // virtual implementations
virtual AronAbstractTypePtr getResult() const override
{
return type;
}
- virtual CppBlockPtr getWriteBlock(const std::string& accessor) const
+ virtual CppBlockPtr getResetBlock(const std::string& accessor) const override
+ {
+ CppBlockPtr b = CppBlockPtr(new CppBlock());
+ b->addLine(accessor + " = {};");
+ return b;
+ }
+
+ virtual CppBlockPtr getInitializeAronTypeBlock(const std::string& accessor) const override
+ {
+ CppBlockPtr b = CppBlockPtr(new CppBlock());
+ b->addLine(getFullAronTypePtrTypename() + " " + accessor + " = " + getFullAronTypePtrTypename() + "(new " + getFullAronTypeTypename() + "());");
+ return b;
+ }
+
+ virtual CppBlockPtr getWriteBlock(const std::string& accessor) const override
{
CppBlockPtr b = CppBlockPtr(new CppBlock());
b->addLine("w.write" + getCppTypename() + "(" + accessor + ");");
return b;
}
- virtual CppBlockPtr getReadBlock(const std::string& accessor) const
+ virtual CppBlockPtr getReadBlock(const std::string& accessor) const override
{
CppBlockPtr b = CppBlockPtr(new CppBlock());
b->addLine(accessor + " = r.read" + getCppTypename() + "();");
@@ -78,6 +104,7 @@ namespace armarx
}
private:
+ // members
typename AronAbstractTypeType::PointerType type;
};
}
diff --git a/source/RobotAPI/libraries/aron/types/AronTupleTypeNavigator.cpp b/source/RobotAPI/libraries/aron/types/AronTupleTypeNavigator.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..3324161bbf02684bdf1a5b1f007fb53257b78882
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/types/AronTupleTypeNavigator.cpp
@@ -0,0 +1,167 @@
+/*
+ * 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/>.
+ *
+ * @author Simon Ottenhaus (simon dot ottenhaus at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+// Boost
+#include <boost/algorithm/string.hpp>
+
+// Header
+#include "AronTupleTypeNavigator.h"
+
+
+namespace armarx
+{
+ namespace aron
+ {
+ namespace types
+ {
+ namespace
+ {
+ // anonymous helper functions
+ std::vector<std::string> getAronTupleTypeNames(const std::vector<AronAbstractTypeNavigatorPtr>& arons)
+ {
+ std::vector<std::string> names;
+ for (const auto& aron : arons)
+ {
+ names.push_back(aron->getFullCppTypename());
+ }
+ return names;
+ }
+ }
+
+ // constructors
+ AronTupleTypeNavigator::AronTupleTypeNavigator(const std::vector<std::string>& acceptedCppTypenames, const AronNavigatorPath& path) :
+ AronNavigator<AronTypeDescriptor, AronAbstractType>::AronNavigator(AronTypeDescriptor::eAronTupleType),
+ AronAbstractTypeNavigator(AronTypeDescriptor::eAronTupleType, "std::tuple<" + boost::algorithm::join(acceptedCppTypenames, ", ") + ">", boost::core::demangle(typeid(AronList).name()), boost::core::demangle(typeid(AronTupleType).name())),
+ type(new AronTupleType())
+ {
+ for (const std::string& typeName : acceptedCppTypenames)
+ {
+ AronAbstractTypeNavigatorPtr nav = GetAronAbstractTypePtrByName(typeName);
+ acceptedTypeNavigators.push_back(nav);
+ type->elementTypes.push_back(nav->getResult());
+ }
+ }
+
+ AronTupleTypeNavigator::AronTupleTypeNavigator(const std::vector<AronAbstractTypeNavigatorPtr>& acceptedTypeNavigators, const AronNavigatorPath& path) :
+ AronNavigator<AronTypeDescriptor, AronAbstractType>::AronNavigator(AronTypeDescriptor::eAronTupleType),
+ AronAbstractTypeNavigator(AronTypeDescriptor::eAronTupleType, "std::tuple<" + boost::algorithm::join(getAronTupleTypeNames(acceptedTypeNavigators), ", ") + ">", boost::core::demangle(typeid(AronList).name()), boost::core::demangle(typeid(AronTupleType).name())),
+ acceptedTypeNavigators(acceptedTypeNavigators),
+ type(new AronTupleType())
+ {
+
+ }
+
+ // static methods
+ AronTupleTypeNavigatorPtr AronTupleTypeNavigator::DynamicCast(const AronAbstractTypeNavigatorPtr& n)
+ {
+ AronTupleTypeNavigatorPtr casted = std::dynamic_pointer_cast<AronTupleTypeNavigator>(n);
+ checkAbstractTypeNavigatorPtrForNull(casted, "");
+ return casted;
+ }
+
+ // public member functions
+ void AronTupleTypeNavigator::addElement(const AronAbstractTypeNavigatorPtr& v)
+ {
+ acceptedTypeNavigators.push_back(v);
+ type->elementTypes.push_back(v->getResult());
+ }
+
+ std::vector<AronAbstractTypeNavigatorPtr> AronTupleTypeNavigator::getElements() const
+ {
+ return acceptedTypeNavigators;
+ }
+
+ // virtual implementations
+ AronAbstractTypePtr AronTupleTypeNavigator::getResult() const
+ {
+ return type;
+ }
+
+ CppBlockPtr AronTupleTypeNavigator::getInitializeAronTypeBlock(const std::string& accessor) const
+ {
+ CppBlockPtr b = CppBlockPtr(new CppBlock());
+
+ std::string escaped_accessor = escapeAccessor(accessor);
+ std::string accessor_iterator_container = escaped_accessor + "_tupleiteratorcontainer";
+
+ b->addLine("std::vector<" + getFullAronTypePtrTypename() + "> " + accessor_iterator_container + ";");
+
+ unsigned int i = 0;
+ for (const auto& child : acceptedTypeNavigators)
+ {
+ std::string accessor_iterator = escaped_accessor + "_tupleiterator" + std::to_string(i++);
+ CppBlockPtr b2 = child->getInitializeAronTypeBlock(accessor_iterator);
+ b = CppBlock::mergeBlocks(b, b2);
+
+ b->addLine(accessor_iterator_container + ".push_back(" + accessor_iterator + ");");
+ }
+ b->addLine(getFullAronTypePtrTypename() + " " + accessor + " = " + getFullAronTypePtrTypename() + "(new " + getFullAronTypeTypename() + "(" + accessor_iterator_container + "));");
+ return b;
+ }
+
+ CppBlockPtr AronTupleTypeNavigator::getResetBlock(const std::string& accessor) const
+ {
+ CppBlockPtr b = CppBlockPtr(new CppBlock());
+
+ unsigned int i = 0;
+ for (const auto& child : acceptedTypeNavigators)
+ {
+ CppBlockPtr b2 = child->getResetBlock("std::get<" + std::to_string(i++) + ">(" + accessor + ")");
+ b = CppBlock::mergeBlocks(b, b2);
+ }
+ return b;
+ }
+
+ CppBlockPtr AronTupleTypeNavigator::getWriteBlock(const std::string& accessor) const
+ {
+ CppBlockPtr b = CppBlockPtr(new CppBlock());
+ b->addLine("w.writeStartList();");
+
+ unsigned int i = 0;
+ for (const auto& type : acceptedTypeNavigators)
+ {
+ CppBlockPtr b2 = type->getWriteBlock("std::get<" + std::to_string(i++) + ">(" + accessor + ")");
+ b = CppBlock::mergeBlocks(b, b2);
+ }
+ b->addLine("w.writeEndList();");
+ return b;
+ }
+
+ CppBlockPtr AronTupleTypeNavigator::getReadBlock(const std::string& accessor) const
+ {
+ CppBlockPtr b = CppBlockPtr(new CppBlock());
+ b->addLine("r.readStartList();");
+
+ unsigned int i = 0;
+ for (const auto& type : acceptedTypeNavigators)
+ {
+ CppBlockPtr b2 = type->getReadBlock("std::get<" + std::to_string(i++) + ">(" + accessor + ")");
+ b = CppBlock::mergeBlocks(b, b2);
+ }
+ b->addLine("r.readEndList();");
+ return b;
+ }
+ }
+ }
+}
+
diff --git a/source/RobotAPI/libraries/aron/types/AronTupleTypeNavigator.h b/source/RobotAPI/libraries/aron/types/AronTupleTypeNavigator.h
new file mode 100644
index 0000000000000000000000000000000000000000..32808ba09b9ceebb101aad13f3134430e1375b57
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/types/AronTupleTypeNavigator.h
@@ -0,0 +1,77 @@
+/*
+ * 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/>.
+ *
+ * @author Simon Ottenhaus (simon dot ottenhaus at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+// STD/STL
+#include <string>
+#include <map>
+
+// Base Class
+#include "AronAbstractTypeNavigator.h"
+
+namespace armarx
+{
+ namespace aron
+ {
+ namespace types
+ {
+
+ class AronTupleTypeNavigator;
+ typedef std::shared_ptr<AronTupleTypeNavigator> AronTupleTypeNavigatorPtr;
+
+ class AronTupleTypeNavigator :
+ virtual public AronAbstractTypeNavigator
+ {
+ public:
+ using PointerType = AronTupleTypeNavigatorPtr;
+
+ public:
+ // constructors
+ AronTupleTypeNavigator(const std::vector<std::string>&, const AronNavigatorPath& path);
+ AronTupleTypeNavigator(const std::vector<AronAbstractTypeNavigatorPtr>&, const AronNavigatorPath& path);
+
+ // static methods
+ static AronTupleTypeNavigatorPtr DynamicCast(const AronAbstractTypeNavigatorPtr& n);
+
+ // public member functions
+ void addElement(const AronAbstractTypeNavigatorPtr&);
+ std::vector<AronAbstractTypeNavigatorPtr> getElements() const;
+
+ // virtual implementations
+ virtual AronAbstractTypePtr getResult() const override;
+
+ virtual CppBlockPtr getInitializeAronTypeBlock(const std::string&) const override;
+ virtual CppBlockPtr getResetBlock(const std::string&) const override;
+ virtual CppBlockPtr getWriteBlock(const std::string&) const override;
+ virtual CppBlockPtr getReadBlock(const std::string&) const override;
+
+
+ private:
+ // members
+ std::vector<AronAbstractTypeNavigatorPtr> acceptedTypeNavigators;
+ AronTupleTypePtr type;
+ };
+ }
+ }
+}
diff --git a/source/RobotAPI/statecharts/operations/CMakeLists.txt b/source/RobotAPI/statecharts/operations/CMakeLists.txt
index d9de1b3d03dd5337a0791bd6c280b03d8142e01a..4c5dbf0b75803a3df189be3ed2105f265d287d85 100644
--- a/source/RobotAPI/statecharts/operations/CMakeLists.txt
+++ b/source/RobotAPI/statecharts/operations/CMakeLists.txt
@@ -8,3 +8,5 @@ set(LIB_FILES RobotControl.cpp)
set(LIB_HEADERS RobotControl.h)
armarx_add_library("${LIB_NAME}" "${LIB_FILES}" "${LIB_HEADERS}" "${LIBS}")
+
+armarx_generate_and_add_component_executable(COMPONENT_NAME RobotControl)