diff --git a/.gitlab/CODEOWNERS b/.gitlab/CODEOWNERS
index a50d21fe1c21109729acf45e579328b68e740f75..af86e2ce7810f25704e7afbbbb3072e53e36b487 100644
--- a/.gitlab/CODEOWNERS
+++ b/.gitlab/CODEOWNERS
@@ -1,3 +1,11 @@
+# ARON
+
+/source/RobotAPI/libraries/aron/ @fratty @dreher
+
+/source/RobotAPI/interface/aron/ @fratty @dreher
+/source/RobotAPI/interface/aron.ice @fratty @dreher
+
+
# ArMem
/source/RobotAPI/components/armem/ @fratty @RainerKartmann @dreher
diff --git a/CMakeLists.txt b/CMakeLists.txt
index c7bd1763dfe4608861a977d61c0dfb04a4dc3670..578b8978679e99bf5342adf6a7e5f5ed04538542 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -22,3 +22,21 @@ add_subdirectory(source)
install_project()
add_subdirectory(scenarios)
+
+
+if (TRUE)
+ find_package(libmongocxx QUIET)
+ find_package(libbsoncxx QUIET)
+ if (NOT libbsoncxx_FOUND OR NOT libbsoncxx_FOUND)
+ string(ASCII 27 Esc)
+ set(BoldYellow "${Esc}[1;33m")
+ set(ColourReset "${Esc}[m")
+
+ message(WARNING "${BoldYellow}\
+ The packages libbsoncxx or libbsoncxx not found. \
+ They are required to build the memory system (armem and related libraries). \
+ Please use this installation script to install libmongocxx and libbsoncxx:
+ cd ${PROJECT_SOURCE_DIR}/etc/mongocxx/ && ./install_mongocxx.sh ~/repos \
+ ${ColourReset}")
+ endif()
+endif()
diff --git a/data/RobotAPI/VariantInfo-RobotAPI.xml b/data/RobotAPI/VariantInfo-RobotAPI.xml
index fac11b677edbf7fc26cd2538e453981601e94f13..9d9342ab21b51d11fd188a70c5e4cf6066afff24 100644
--- a/data/RobotAPI/VariantInfo-RobotAPI.xml
+++ b/data/RobotAPI/VariantInfo-RobotAPI.xml
@@ -43,10 +43,18 @@
<Proxy include="RobotAPI/interface/units/PlatformUnitInterface.h"
humanName="Platform Unit (obstacle avoiding)"
typeName="PlatformUnitInterfacePrx"
- memberName="obstacleAvoidingPlatformUnit"
- getterName="getObstacleAvoidingPlatformUnit"
- propertyName="ObstacleAvoidingPlatformUnitName"
+ memberName="platformUnit1"
+ getterName="getPlatformUnit1"
+ propertyName="PlatformUnitName1"
propertyDefaultValue="ObstacleAvoidingPlatformUnit"
+ propertyIsOptional="true" />
+ <Proxy include="RobotAPI/interface/units/PlatformUnitInterface.h"
+ humanName="Platform Unit (obstacle aware)"
+ typeName="PlatformUnitInterfacePrx"
+ memberName="platformUnit2"
+ getterName="getPlatformUnit2"
+ propertyName="PlatformUnitName2"
+ propertyDefaultValue="ObstacleAwarePlatformUnit"
propertyIsOptional="true" />
<Proxy include="RobotAPI/interface/observers/PlatformUnitObserverInterface.h"
humanName="Platform Unit Observer"
diff --git a/etc/cmake/Findlibbson-1.0.cmake b/etc/cmake/Findlibbson-1.0.cmake
new file mode 100644
index 0000000000000000000000000000000000000000..e1f1df468c48dc8a7d44ba8328958fac8171a7e8
--- /dev/null
+++ b/etc/cmake/Findlibbson-1.0.cmake
@@ -0,0 +1,48 @@
+# see https://github.com/intersvyaz/zerod/blob/master/cmake/modules/FindLibbson.cmake
+# - Find libbson
+# Find the native libbson includes and library.
+# Once done this will define
+#
+# LIBBSON_INCLUDE_DIRS - where to find bson.h, etc.
+# LIBBSON_LIBRARIES - List of libraries when using libbson.
+# LIBBSON_FOUND - True if libbson found.
+#
+# LIBBSON_VERSION_STRING - The version of libbson found (x.y.z)
+# LIBBSON_VERSION_MAJOR - The major version
+# LIBBSON_VERSION_MINOR - The minor version
+# LIBBSON_VERSION_MICRO - The micro version
+
+FIND_PATH(LIBBSON_INCLUDE_DIR NAMES bson.h PATH_SUFFIXES libbson-1.0)
+FIND_LIBRARY(LIBBSON_LIBRARY NAMES bson-1.0)
+
+MARK_AS_ADVANCED(LIBBSON_LIBRARY LIBBSON_INCLUDE_DIR)
+
+IF(LIBBSON_INCLUDE_DIR AND EXISTS "${LIBBSON_INCLUDE_DIR}/bson-version.h")
+ # Read and parse version header file for version number
+ file(READ "${LIBBSON_INCLUDE_DIR}/bson-version.h" _libbson_HEADER_CONTENTS)
+ IF(_libbson_HEADER_CONTENTS MATCHES ".*BSON_MAJOR_VERSION.*")
+ #define BSON_MAJOR_VERSION
+ string(REGEX REPLACE ".*#define +BSON_MAJOR_VERSION +\\(([0-9]+)\\).*" "\\1" LIBBSON_VERSION_MAJOR "${_libbson_HEADER_CONTENTS}")
+ string(REGEX REPLACE ".*#define +BSON_MINOR_VERSION +\\(([0-9]+)\\).*" "\\1" LIBBSON_VERSION_MINOR "${_libbson_HEADER_CONTENTS}")
+ string(REGEX REPLACE ".*#define +BSON_MICRO_VERSION +\\(([0-9]+)\\).*" "\\1" LIBBSON_VERSION_MICRO "${_libbson_HEADER_CONTENTS}")
+ ELSE()
+ SET(LIBBSON_VERSION_MAJOR 0)
+ SET(LIBBSON_VERSION_MINOR 0)
+ SET(LIBBSON_VERSION_MICRO 0)
+ ENDIF()
+
+ SET(LIBBSON_VERSION_STRING "${LIBBSON_VERSION_MAJOR}.${LIBBSON_VERSION_MINOR}.${LIBBSON_VERSION_MICRO}")
+ENDIF()
+
+# handle the QUIETLY and REQUIRED arguments and set LIBBSON_FOUND to TRUE if
+# all listed variables are TRUE
+INCLUDE(FindPackageHandleStandardArgs)
+FIND_PACKAGE_HANDLE_STANDARD_ARGS(libbson-1.0
+ REQUIRED_VARS LIBBSON_LIBRARY LIBBSON_INCLUDE_DIR
+ VERSION_VAR LIBBSON_VERSION_STRING
+)
+
+IF(LIBBSON_FOUND)
+ SET(LIBBSON_INCLUDE_DIRS ${LIBBSON_INCLUDE_DIR})
+ SET(LIBBSON_LIBRARIES ${LIBBSON_LIBRARY})
+ENDIF()
diff --git a/etc/cmake/Findlibmongoc-1.0 b/etc/cmake/Findlibmongoc-1.0
new file mode 100644
index 0000000000000000000000000000000000000000..594e87aeadc5cb31066934b8ad48dfa25ac29f8e
--- /dev/null
+++ b/etc/cmake/Findlibmongoc-1.0
@@ -0,0 +1,47 @@
+# see https://github.com/intersvyaz/freeradius-mongodb/blob/master/cmake/modules/FindLibmongoc.cmake
+# - Find libmongoc
+# Find the native libmongoc includes and library.
+# Once done this will define
+#
+# LIBMONGOC_INCLUDE_DIRS - where to find mongoc.h, etc.
+# LIBMONGOC_LIBRARIES - List of libraries when using libmongoc.
+# LIBMONGOC_FOUND - True if libmongoc found.
+#
+# LIBMONGOC_VERSION_STRING - The version of libmongoc found (x.y.z)
+# LIBMONGOC_VERSION_MAJOR - The major version
+# LIBMONGOC_VERSION_MINOR - The minor version
+# LIBMONGOC_VERSION_MICRO - The micro version
+
+FIND_PATH(LIBMONGOC_INCLUDE_DIR NAMES mongoc.h PATH_SUFFIXES libmongoc-1.0)
+FIND_LIBRARY(LIBMONGOC_LIBRARY NAMES mongoc-1.0)
+
+MARK_AS_ADVANCED(LIBMONGOC_LIBRARY LIBMONGOC_INCLUDE_DIR)
+
+IF(LIBMONGOC_INCLUDE_DIR AND EXISTS "${LIBMONGOC_INCLUDE_DIR}/mongoc-version.h")
+ # Read and parse version header file for version number
+ file(READ "${LIBMONGOC_INCLUDE_DIR}/mongoc-version.h" _libmongoc_HEADER_CONTENTS)
+ IF(_libmongoc_HEADER_CONTENTS MATCHES ".*MONGOC_MAJOR_VERSION.*")
+ string(REGEX REPLACE ".*#define +MONGOC_MAJOR_VERSION +\\(([0-9]+)\\).*" "\\1" LIBMONGOC_VERSION_MAJOR "${_libmongoc_HEADER_CONTENTS}")
+ string(REGEX REPLACE ".*#define +MONGOC_MINOR_VERSION +\\(([0-9]+)\\).*" "\\1" LIBMONGOC_VERSION_MINOR "${_libmongoc_HEADER_CONTENTS}")
+ string(REGEX REPLACE ".*#define +MONGOC_MICRO_VERSION +\\(([0-9]+)\\).*" "\\1" LIBMONGOC_VERSION_MICRO "${_libmongoc_HEADER_CONTENTS}")
+ ELSE()
+ SET(LIBMONGOC_VERSION_MAJOR 0)
+ SET(LIBMONGOC_VERSION_MINOR 0)
+ SET(LIBMONGOC_VERSION_MICRO 0)
+ ENDIF()
+
+ SET(LIBMONGOC_VERSION_STRING "${LIBMONGOC_VERSION_MAJOR}.${LIBMONGOC_VERSION_MINOR}.${LIBMONGOC_VERSION_MICRO}")
+ENDIF()
+
+# handle the QUIETLY and REQUIRED arguments and set LIBMONGOC_FOUND to TRUE if
+# all listed variables are TRUE
+INCLUDE(FindPackageHandleStandardArgs)
+FIND_PACKAGE_HANDLE_STANDARD_ARGS(Libmongoc
+ REQUIRED_VARS LIBMONGOC_LIBRARY LIBMONGOC_INCLUDE_DIR
+ VERSION_VAR LIBMONGOC_VERSION_STRING
+)
+
+IF(LIBMONGOC_FOUND)
+ SET(LIBMONGOC_INCLUDE_DIRS ${LIBMONGOC_INCLUDE_DIR})
+ SET(LIBMONGOC_LIBRARIES ${LIBMONGOC_LIBRARY})
+ENDIF()
diff --git a/etc/mongocxx/install_mongocxx.sh b/etc/mongocxx/install_mongocxx.sh
new file mode 100755
index 0000000000000000000000000000000000000000..8820931ce0ab923c8a2ff451ee84f62c9b62918c
--- /dev/null
+++ b/etc/mongocxx/install_mongocxx.sh
@@ -0,0 +1,164 @@
+#!/bin/bash
+
+# By Fabian Peller 2021-06-10;
+# Extended by Rainer Kartmann 2021-06-23;
+
+
+if [ "$#" -ne 1 ]; then
+ echo "Usage: $0 target-directory" >&2
+ echo -e " target-directory \t Directory where mongocxx should be placed (e.g. ~/repos)"
+ exit 1
+fi
+
+
+# Stolen from mycroft-core/dev_setup.sh // Rainer Kartmann
+function found_exe() {
+ hash "$1" 2>/dev/null
+}
+
+# If tput is available and can handle multiple colors
+if found_exe tput ; then
+ if [[ $(tput colors) != "-1" && -z $CI ]]; then
+ GREEN=$(tput setaf 2)
+ BLUE=$(tput setaf 4)
+ CYAN=$(tput setaf 6)
+ YELLOW=$(tput setaf 3)
+ RED=$(tput setaf 1)
+ RESET=$(tput sgr0)
+ HIGHLIGHT=$YELLOW
+ fi
+fi
+
+
+function get_YN() {
+ # Loop until the user hits the Y or the N key
+ echo -e -n "Choice [${CYAN}Y${RESET}/${CYAN}N${RESET}]: "
+ while true; do
+ read -N1 -s key
+ case $key in
+ [Yy])
+ return 0
+ ;;
+ [Nn])
+ return 1
+ ;;
+ esac
+ done
+}
+
+
+extractDir="$1"
+sourceDir="$extractDir/mongo-cxx-driver-r3.2.1"
+installDir="$extractDir/mongo-cxx-driver-r3.2.1/build/install"
+scriptDir="$(readlink -f "$(dirname "$0")")"
+pwd="$(pwd)"
+
+
+# You first have to install libbson-dev and libmongoc-dev (not necessary on the lab pcs)
+#sudo apt install libbson-dev libmongoc-dev
+
+# Then you have to update the symlinks (not necessary on the lab pcs)
+# Update the symlinks because the default mongoc bson cmake configs are shitty
+#sudo mkdir -p /usr/lib/include/
+#sudo ln -s /usr/include/libbson-1.0/ /usr/lib/include/libbson-1.0
+#sudo ln -s /usr/include/libbson-1.0/ /usr/lib/include/libbson-1.0
+#sudo ln -s /usr/include/libmongoc-1.0/ /usr/lib/include/libmongoc-1.0
+#sudo ln -s /usr/lib /usr/lib/lib
+
+
+function install_deps()
+{
+ APT_PACKAGE_LIST=(libbson-dev libmongoc-dev)
+
+ declare -a installed_packages
+ declare -a missing_packages
+
+ for pkg in "${APT_PACKAGE_LIST[@]}"; do
+ if dpkg -V "$pkg" > /dev/null 2>&1 ; then
+ installed_packages+=($pkg)
+ else
+ missing_packages+=($pkg)
+ fi
+ done
+
+ if [ ${#installed_packages[*]} -ne 0 ]; then
+ echo "$GREEN
+The following apt packages are already installed: "
+ echo "${installed_packages[*]} $RESET"
+ fi
+
+ if [ ${#missing_packages[*]} -eq 0 ]; then
+
+ echo "$GREEN
+No missing packages. $RESET"
+ echo ""
+
+ else
+
+ echo "$YELLOW
+The following apt packages still need to be installed: "
+ echo "${missing_packages[*]} $RESET"
+
+ echo "
+Do you want to install these packages and update some required
+symlinks in /usr/lib/include/? (requires sudo)
+Y)es, install missing packages via apt-get and update symlinks
+N)o, don't install or I don't have sudo permissions"
+
+ if get_YN ; then
+ echo -e "$HIGHLIGHT Y - installing missing packages $RESET"
+ sudo apt-get install ${missing_packages[*]}
+
+ function verbose_ln()
+ {
+ echo "Updating symlink $2 -> $1"
+ sudo ln -s "$1" "$2"
+ }
+
+ sudo mkdir -p /usr/lib/include/
+ verbose_ln /usr/include/libbson-1.0/ /usr/lib/include/libbson-1.0
+ verbose_ln /usr/include/libbson-1.0/ /usr/lib/include/libbson-1.0
+ verbose_ln /usr/include/libmongoc-1.0/ /usr/lib/include/libmongoc-1.0
+ verbose_ln /usr/lib /usr/lib/lib
+
+ else
+ echo -e "$HIGHLIGHT N - not installing missing pacakges $RESET"
+ echo ""
+ fi
+
+ fi
+}
+
+install_deps
+
+
+# Then you can build the mongocxx driver
+mkdir $extractDir &> /dev/null || true
+tar -xvf mongo-cxx-driver-r3.2.1.tar.gz -C $extractDir
+cd $sourceDir
+mkdir build &> /dev/null || true
+cd build
+cmake ..
+cmake .. -DBSON_LIBRARY="/usr/lib/x86_64-linux-gnu/libbson-1.0.so" -DMONGOC_LIBRARY="/usr/lib/x86_64-linux-gnu/libmongoc-1.0.so" -DCMAKE_INSTALL_PREFIX="$installDir"
+# install to $extractDir/build/install
+make -j`nproc`
+make install
+
+
+# Finally, tell RobotAPI where to find mongocxx
+echo "$HIGHLIGHT
+You now need to cmake RobotAPI with:
+cmake -DCMAKE_PREFIX_PATH=$installDir .. $RESET"
+
+echo "
+Should I do that for you?
+Y)es, run cmake in RobotAPI
+N)o, I'll do it myself"
+if get_YN ; then
+
+ echo -e "$HIGHLIGHT Y - running cmake in RobotAPI $RESET"
+ armarx-dev exec --no-deps RobotAPI "cd build && cmake -DCMAKE_PREFIX_PATH=$installDir ."
+
+else
+ echo -e "$HIGHLIGHT N - not running cmake in RobotAPI $RESET"
+fi
diff --git a/etc/mongocxx/mongo-cxx-driver-r3.2.1.tar.gz b/etc/mongocxx/mongo-cxx-driver-r3.2.1.tar.gz
new file mode 100644
index 0000000000000000000000000000000000000000..c7ce0e9b587972ef723eaaf04dbdb49d7ffe5888
Binary files /dev/null and b/etc/mongocxx/mongo-cxx-driver-r3.2.1.tar.gz differ
diff --git a/scenarios/ArMemCore/ArMemCore.scx b/scenarios/ArMemCore/ArMemCore.scx
new file mode 100644
index 0000000000000000000000000000000000000000..c58f240d86e1aa7b8d01b04f6d15d0504eba8d21
--- /dev/null
+++ b/scenarios/ArMemCore/ArMemCore.scx
@@ -0,0 +1,8 @@
+<?xml version="1.0" encoding="utf-8"?>
+<scenario name="ArMemCore" creation="2021-06-21.11:36:43" globalConfigName="./config/global.cfg" package="RobotAPI" deploymentType="local" nodeName="NodeMain">
+ <application name="DebugObserver" instance="" package="ArmarXCore" nodeName="" enabled="true" iceAutoRestart="false"/>
+ <application name="MemoryNameSystem" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
+ <application name="RemoteGuiProviderApp" instance="" package="ArmarXGui" nodeName="" enabled="true" iceAutoRestart="false"/>
+ <application name="ArVizStorage" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
+</scenario>
+
diff --git a/scenarios/ArMemCore/config/ArVizStorage.cfg b/scenarios/ArMemCore/config/ArVizStorage.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..0dcbd8d5775b8c0f562bba62b5f2fe282e2da6d0
--- /dev/null
+++ b/scenarios/ArMemCore/config/ArVizStorage.cfg
@@ -0,0 +1,212 @@
+# ==================================================================
+# ArVizStorage 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.ArVizStorage.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.ArVizStorage.EnableProfiling = false
+
+
+# ArmarX.ArVizStorage.HistoryPath: Destination path where the history are serialized to
+# Attributes:
+# - Default: RobotAPI/ArVizStorage
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ArVizStorage.HistoryPath = RobotAPI/ArVizStorage
+
+
+# ArmarX.ArVizStorage.MaxHistorySize: How many layer updates are saved in the history until they are compressed
+# Attributes:
+# - Default: 1000
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ArVizStorage.MaxHistorySize = 1000
+
+
+# ArmarX.ArVizStorage.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.ArVizStorage.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.ArVizStorage.ObjectName: Name of IceGrid well-known object
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ArVizStorage.ObjectName = ""
+
+
+# ArmarX.ArVizStorage.TopicName: Layer updates are sent over this topic.
+# Attributes:
+# - Default: ArVizTopic
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ArVizStorage.TopicName = ArVizTopic
+
+
+# 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/ArMemCore/config/DebugObserver.cfg b/scenarios/ArMemCore/config/DebugObserver.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..4a0b9dac036cd4d103efd7d1b718d508f285d85a
--- /dev/null
+++ b/scenarios/ArMemCore/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/scenarios/ArMemCore/config/MemoryNameSystem.cfg b/scenarios/ArMemCore/config/MemoryNameSystem.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..7dd22218243ca4f9e67e843da8b42916f3b8568a
--- /dev/null
+++ b/scenarios/ArMemCore/config/MemoryNameSystem.cfg
@@ -0,0 +1,196 @@
+# ==================================================================
+# MemoryNameSystem 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.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.MemoryNameSystem.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.MemoryNameSystem.EnableProfiling = false
+
+
+# ArmarX.MemoryNameSystem.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.MemoryNameSystem.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.MemoryNameSystem.ObjectName: Name of IceGrid well-known object
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.MemoryNameSystem.ObjectName = ""
+
+
+# ArmarX.MemoryNameSystem.RemoteGuiName: Name of the remote gui provider
+# Attributes:
+# - Default: RemoteGuiProvider
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.MemoryNameSystem.RemoteGuiName = RemoteGuiProvider
+
+
+# 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/ArMemCore/config/RemoteGuiProviderApp.cfg b/scenarios/ArMemCore/config/RemoteGuiProviderApp.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..4fd690cefd94559b207493cf40e346a3e47f3b12
--- /dev/null
+++ b/scenarios/ArMemCore/config/RemoteGuiProviderApp.cfg
@@ -0,0 +1,196 @@
+# ==================================================================
+# RemoteGuiProviderApp 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.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.RemoteGuiProvider.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.RemoteGuiProvider.EnableProfiling = false
+
+
+# ArmarX.RemoteGuiProvider.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.RemoteGuiProvider.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.RemoteGuiProvider.ObjectName: Name of IceGrid well-known object
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RemoteGuiProvider.ObjectName = ""
+
+
+# ArmarX.RemoteGuiProvider.TopicName: Name of the topic on which updates to the remote state are reported.
+# Attributes:
+# - Default: RemoteGuiTopic
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RemoteGuiProvider.TopicName = RemoteGuiTopic
+
+
+# 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/ArMemCore/config/global.cfg b/scenarios/ArMemCore/config/global.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..7a27480632a19b090c2a5d877135779187e46554
--- /dev/null
+++ b/scenarios/ArMemCore/config/global.cfg
@@ -0,0 +1,4 @@
+# ==================================================================
+# Global Config from Scenario ArMemCore
+# ==================================================================
+
diff --git a/scenarios/ArMemExample/config/ExampleMemory.cfg b/scenarios/ArMemExample/config/ExampleMemory.cfg
index d9e24e657c2b57a11bac7d34b2df65dff2e9eb8d..117331158ed0929bccc03456cfe93ab1281d1d3f 100644
--- a/scenarios/ArMemExample/config/ExampleMemory.cfg
+++ b/scenarios/ArMemExample/config/ExampleMemory.cfg
@@ -84,21 +84,53 @@ ArmarX.ArMemExampleMemory.tpc.pub.MemoryListener = MemoryUpdates
# ArmarX.EnableProfiling = false
-# ArmarX.ExampleMemory.:
+# ArmarX.ExampleMemory.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.ExampleMemory.EnableProfiling = false
+
+
+# ArmarX.ExampleMemory.Longtermmemorydatabase:
+# Attributes:
+# - Default: Test
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ExampleMemory.Longtermmemorydatabase = Test
+
+
+# ArmarX.ExampleMemory.Longtermmemoryhost:
+# Attributes:
+# - Default: localhost
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ExampleMemory.Longtermmemoryhost = localhost
+
+
+# ArmarX.ExampleMemory.Longtermmemorypassword:
# Attributes:
# - Default: ""
# - Case sensitivity: yes
# - Required: no
-# ArmarX.ExampleMemory. = ""
+# ArmarX.ExampleMemory.Longtermmemorypassword = ""
-# ArmarX.ExampleMemory.EnableProfiling: enable profiler which is used for logging performance events
+# ArmarX.ExampleMemory.Longtermmemoryport:
# Attributes:
-# - Default: false
+# - Default: 27017
# - Case sensitivity: yes
# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.ExampleMemory.EnableProfiling = false
+# ArmarX.ExampleMemory.Longtermmemoryport = 27017
+
+
+# ArmarX.ExampleMemory.Longtermmemoryuser:
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ExampleMemory.Longtermmemoryuser = ""
# ArmarX.ExampleMemory.MinimumLoggingLevel: Local logging level only for this component
diff --git a/scenarios/ArMemExample/config/ExampleMemoryClient.cfg b/scenarios/ArMemExample/config/ExampleMemoryClient.cfg
index 10325be750b47ccdfa1827265cce4d881d58d2ed..fa8e5a747031ddbb8e87a5c5d5b60446d2393c2c 100644
--- a/scenarios/ArMemExample/config/ExampleMemoryClient.cfg
+++ b/scenarios/ArMemExample/config/ExampleMemoryClient.cfg
@@ -118,12 +118,20 @@ ArmarX.ArMemExampleClient.tpc.sub.MemoryListener = MemoryUpdates
# ArmarX.ExampleMemoryClient.RemoteGuiName = RemoteGuiProvider
-# ArmarX.ExampleMemoryClient.mem.MemoryName: Name of the memory to use.
+# ArmarX.ExampleMemoryClient.ex.CommitFrequency: Frequency in which example data is commited.
+# Attributes:
+# - Default: 10
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ExampleMemoryClient.ex.CommitFrequency = 10
+
+
+# ArmarX.ExampleMemoryClient.mem.UsedMemoryName: Name of the memory to use.
# Attributes:
# - Default: Example
# - Case sensitivity: yes
# - Required: no
-# ArmarX.ExampleMemoryClient.mem.MemoryName = Example
+# ArmarX.ExampleMemoryClient.mem.UsedMemoryName = Example
# ArmarX.ExampleMemoryClient.mns.MemoryNameSystemEnabled: Whether to use (and depend on) the Memory Name System (MNS).
diff --git a/scenarios/ArMemObjectMemory/ArMemObjectMemory.scx b/scenarios/ArMemObjectMemory/ArMemObjectMemory.scx
index e6f93e12cda04bf7a80e6a41daada5189f2ef787..9e30d4e3984f80d64e40d72fdb21b7a071c8031a 100644
--- a/scenarios/ArMemObjectMemory/ArMemObjectMemory.scx
+++ b/scenarios/ArMemObjectMemory/ArMemObjectMemory.scx
@@ -1,14 +1,14 @@
<?xml version="1.0" encoding="utf-8"?>
<scenario name="ArMemObjectMemory" creation="2021-04-22.11:29:22" globalConfigName="./config/global.cfg" package="RobotAPI" deploymentType="local" nodeName="NodeMain">
<application name="ObjectMemory" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
- <application name="MemoryNameSystem" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
- <application name="DebugObserver" instance="" package="ArmarXCore" nodeName="" enabled="true" iceAutoRestart="false"/>
- <application name="RemoteGuiProviderApp" instance="" package="ArmarXGui" nodeName="" enabled="true" iceAutoRestart="false"/>
- <application name="ObjectPoseProviderExample" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
- <application name="ArVizStorage" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
+ <application name="MemoryNameSystem" instance="" package="RobotAPI" nodeName="" enabled="false" iceAutoRestart="false"/>
+ <application name="DebugObserver" instance="" package="ArmarXCore" nodeName="" enabled="false" iceAutoRestart="false"/>
+ <application name="RemoteGuiProviderApp" instance="" package="ArmarXGui" nodeName="" enabled="false" iceAutoRestart="false"/>
+ <application name="ObjectPoseProviderExample" instance="" package="RobotAPI" nodeName="" enabled="false" iceAutoRestart="false"/>
+ <application name="ArVizStorage" instance="" package="RobotAPI" nodeName="" enabled="false" iceAutoRestart="false"/>
<application name="RobotStateComponent" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
<application name="RobotToArVizApp" instance="" package="RobotAPI" nodeName="" enabled="false" iceAutoRestart="false"/>
- <application name="ObjectPoseClientExample" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
+ <application name="ObjectPoseClientExample" instance="" package="RobotAPI" nodeName="" enabled="false" iceAutoRestart="false"/>
<application name="ArticulatedObjectLocalizerExample" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
</scenario>
diff --git a/scenarios/ArMemObjectMemory/config/ArticulatedObjectLocalizerExample.cfg b/scenarios/ArMemObjectMemory/config/ArticulatedObjectLocalizerExample.cfg
index 82769e0f22bfa6de0ca678be094d396df07c89e0..9d225df2f4efadbb731a79fd45dbf852189ec80b 100644
--- a/scenarios/ArMemObjectMemory/config/ArticulatedObjectLocalizerExample.cfg
+++ b/scenarios/ArMemObjectMemory/config/ArticulatedObjectLocalizerExample.cfg
@@ -46,10 +46,10 @@
# ArmarX.ArticulatedObjectLocalizerExample.mem.obj.articulated.CoreSegment: Name of the memory core segment to use for object classes.
# Attributes:
-# - Default: ArticulatedObjectClass
+# - Default: Class
# - Case sensitivity: yes
# - Required: no
-# ArmarX.ArticulatedObjectLocalizerExample.mem.obj.articulated.CoreSegment = ArticulatedObjectClass
+# ArmarX.ArticulatedObjectLocalizerExample.mem.obj.articulated.CoreSegment = Class
# ArmarX.ArticulatedObjectLocalizerExample.mem.obj.articulated.MemoryName:
@@ -62,10 +62,10 @@
# ArmarX.ArticulatedObjectLocalizerExample.mem.obj.articulated.ProviderName:
# Attributes:
-# - Default: ArmarXObjects
+# - Default: PriorKnowledgeData
# - Case sensitivity: yes
# - Required: no
-ArmarX.ArticulatedObjectLocalizerExample.mem.obj.articulated.ProviderName = ExampleProvider
+# ArmarX.ArticulatedObjectLocalizerExample.mem.obj.articulated.ProviderName = PriorKnowledgeData
# ArmarX.ArticulatedObjectLocalizerExample.mns.MemoryNameSystemEnabled: Whether to use (and depend on) the Memory Name System (MNS).
diff --git a/scenarios/ArMemObjectMemory/config/ArticulatedObjectLocalizerExample.instance1.cfg b/scenarios/ArMemObjectMemory/config/ArticulatedObjectLocalizerExample.instance1.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..9d225df2f4efadbb731a79fd45dbf852189ec80b
--- /dev/null
+++ b/scenarios/ArMemObjectMemory/config/ArticulatedObjectLocalizerExample.instance1.cfg
@@ -0,0 +1,254 @@
+# ==================================================================
+# ArticulatedObjectLocalizerExample 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.ArticulatedObjectLocalizerExample.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.ArticulatedObjectLocalizerExample.EnableProfiling = false
+
+
+# ArmarX.ArticulatedObjectLocalizerExample.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.ArticulatedObjectLocalizerExample.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.ArticulatedObjectLocalizerExample.ObjectName: Name of IceGrid well-known object
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ArticulatedObjectLocalizerExample.ObjectName = ""
+
+
+# ArmarX.ArticulatedObjectLocalizerExample.mem.obj.articulated.CoreSegment: Name of the memory core segment to use for object classes.
+# Attributes:
+# - Default: Class
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ArticulatedObjectLocalizerExample.mem.obj.articulated.CoreSegment = Class
+
+
+# ArmarX.ArticulatedObjectLocalizerExample.mem.obj.articulated.MemoryName:
+# Attributes:
+# - Default: Object
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ArticulatedObjectLocalizerExample.mem.obj.articulated.MemoryName = Object
+
+
+# ArmarX.ArticulatedObjectLocalizerExample.mem.obj.articulated.ProviderName:
+# Attributes:
+# - Default: PriorKnowledgeData
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ArticulatedObjectLocalizerExample.mem.obj.articulated.ProviderName = PriorKnowledgeData
+
+
+# ArmarX.ArticulatedObjectLocalizerExample.mns.MemoryNameSystemEnabled: Whether to use (and depend on) the Memory Name System (MNS).
+# Set to false to use this memory as a stand-alone.
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ArticulatedObjectLocalizerExample.mns.MemoryNameSystemEnabled = true
+
+
+# ArmarX.ArticulatedObjectLocalizerExample.mns.MemoryNameSystemName: Name of the Memory Name System (MNS) component.
+# Attributes:
+# - Default: MemoryNameSystem
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ArticulatedObjectLocalizerExample.mns.MemoryNameSystemName = MemoryNameSystem
+
+
+# ArmarX.ArticulatedObjectLocalizerExample.tpc.pub.DebugObserver: Name of the `DebugObserver` topic to publish data to.
+# Attributes:
+# - Default: DebugObserver
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ArticulatedObjectLocalizerExample.tpc.pub.DebugObserver = DebugObserver
+
+
+# ArmarX.ArticulatedObjectLocalizerExample.tpc.sub.MemoryListener: Name of the `MemoryListener` topic to subscribe to.
+# Attributes:
+# - Default: MemoryUpdates
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ArticulatedObjectLocalizerExample.tpc.sub.MemoryListener = MemoryUpdates
+
+
+# ArmarX.ArticulatedObjectLocalizerExample.updateFrequency: Memory update frequency (write).
+# Attributes:
+# - Default: 25
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ArticulatedObjectLocalizerExample.updateFrequency = 25
+
+
+# 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/ArMemObjectMemory/config/ObjectMemory.cfg b/scenarios/ArMemObjectMemory/config/ObjectMemory.cfg
index df483eb31778702f71c8a1ff6c13bf260f86c605..e67a4c41162632026046dc8402466d2deb7ee233 100644
--- a/scenarios/ArMemObjectMemory/config/ObjectMemory.cfg
+++ b/scenarios/ArMemObjectMemory/config/ObjectMemory.cfg
@@ -7,7 +7,7 @@
# - Default: Default value not mapped.
# - Case sensitivity: yes
# - Required: no
-ArmarX.AdditionalPackages = ArmarXObjects
+# ArmarX.AdditionalPackages = Default value not mapped.
# ArmarX.ApplicationName: Application name
@@ -92,14 +92,6 @@ ArmarX.AdditionalPackages = ArmarXObjects
# ArmarX.LoggingGroup = ""
-# ArmarX.ObjectMemory.:
-# Attributes:
-# - Default: ""
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.ObjectMemory. = ""
-
-
# ArmarX.ObjectMemory.ArVizTopicName: Name of the ArViz topic
# Attributes:
# - Default: ArVizTopic
@@ -158,6 +150,55 @@ ArmarX.AdditionalPackages = ArmarXObjects
# ArmarX.ObjectMemory.cmp.KinematicUnitObserverName = KinematicUnitObserver
+# ArmarX.ObjectMemory.ltm.00_enabled:
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ObjectMemory.ltm.00_enabled = true
+
+
+# ArmarX.ObjectMemory.ltm.10_host:
+# Attributes:
+# - Default: localhost
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.ltm.10_host = localhost
+
+
+# ArmarX.ObjectMemory.ltm.11_port:
+# Attributes:
+# - Default: 25270
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.ltm.11_port = 25270
+
+
+# ArmarX.ObjectMemory.ltm.20_user:
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.ltm.20_user = ""
+
+
+# ArmarX.ObjectMemory.ltm.21_password:
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.ltm.21_password = ""
+
+
+# ArmarX.ObjectMemory.ltm.22_database:
+# Attributes:
+# - Default: Test
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.ltm.22_database = Test
+
+
# ArmarX.ObjectMemory.mem.MemoryName: Name of this memory server.
# Attributes:
# - Default: Object
@@ -193,10 +234,10 @@ ArmarX.AdditionalPackages = ArmarXObjects
# ArmarX.ObjectMemory.mem.articulated.cls.ObjectsPackage: Name of the objects package to load from.
# Attributes:
-# - Default: ArmarXObjects
+# - Default: PriorKnowledgeData
# - Case sensitivity: yes
# - Required: no
-# ArmarX.ObjectMemory.mem.articulated.cls.ObjectsPackage = ArmarXObjects
+# ArmarX.ObjectMemory.mem.articulated.cls.ObjectsPackage = PriorKnowledgeData
# ArmarX.ObjectMemory.mem.articulated.inst.CoreSegmentName: Name of the object instance core segment.
@@ -241,20 +282,19 @@ ArmarX.AdditionalPackages = ArmarXObjects
# ArmarX.ObjectMemory.mem.cls.Floor.EntityName: Object class entity of the floor.
# Attributes:
-# - Default: Environment/floor-20x20
+# - Default: Building/floor-20x20
# - Case sensitivity: yes
# - Required: no
-# ArmarX.ObjectMemory.mem.cls.Floor.EntityName = Environment/floor-20x20
+# ArmarX.ObjectMemory.mem.cls.Floor.EntityName = Building/floor-20x20
# ArmarX.ObjectMemory.mem.cls.Floor.Height: Height (z) of the floor plane.
# Set slightly below 0 to avoid z-fighting when drawing planes on the ground.
# Attributes:
-# - Default: true
+# - Default: -1
# - Case sensitivity: yes
# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.ObjectMemory.mem.cls.Floor.Height = true
+# ArmarX.ObjectMemory.mem.cls.Floor.Height = -1
# ArmarX.ObjectMemory.mem.cls.Floor.LayerName: Layer to draw the floor on.
@@ -293,10 +333,10 @@ ArmarX.AdditionalPackages = ArmarXObjects
# ArmarX.ObjectMemory.mem.cls.ObjectsPackage: Name of the objects package to load from.
# Attributes:
-# - Default: ArmarXObjects
+# - Default: PriorKnowledgeData
# - Case sensitivity: yes
# - Required: no
-# ArmarX.ObjectMemory.mem.cls.ObjectsPackage = ArmarXObjects
+# ArmarX.ObjectMemory.mem.cls.ObjectsPackage = PriorKnowledgeData
# ArmarX.ObjectMemory.mem.inst.CoreSegmentName: Name of the object instance core segment.
@@ -415,6 +455,32 @@ ArmarX.AdditionalPackages = ArmarXObjects
# ArmarX.ObjectMemory.mem.inst.head.maxJointVelocity = 0.0500000007
+# ArmarX.ObjectMemory.mem.inst.scene.10_Package: ArmarX package containing the scene snapshots.
+# Scene snapshots are expected to be located in Package/data/Package/Scenes/*.json.
+# Attributes:
+# - Default: PriorKnowledgeData
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.inst.scene.10_Package = PriorKnowledgeData
+
+
+# ArmarX.ObjectMemory.mem.inst.scene.11_Directory: Directory in Package/data/Package/ containing the scene snapshots.
+# Attributes:
+# - Default: scenes
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.inst.scene.11_Directory = scenes
+
+
+# ArmarX.ObjectMemory.mem.inst.scene.12_SnapshotToLoad: Scene snapshot to load on startup (e.g. 'Scene_2021-06-24_20-20-03').
+# You can also specify paths relative to 'Package/scenes/'.
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.inst.scene.12_SnapshotToLoad = ""
+
+
# ArmarX.ObjectMemory.mem.inst.visu.alpha: Alpha of objects (1 = solid, 0 = transparent).
# Attributes:
# - Default: 1
@@ -484,6 +550,15 @@ ArmarX.AdditionalPackages = ArmarXObjects
# ArmarX.ObjectMemory.mem.inst.visu.oobbs = false
+# ArmarX.ObjectMemory.mem.inst.visu.useArticulatedModels: Prefer articulated object models if available.
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ObjectMemory.mem.inst.visu.useArticulatedModels = true
+
+
# ArmarX.ObjectMemory.mns.MemoryNameSystemEnabled: Whether to use (and depend on) the Memory Name System (MNS).
# Set to false to use this memory as a stand-alone.
# Attributes:
@@ -518,6 +593,14 @@ ArmarX.AdditionalPackages = ArmarXObjects
# ArmarX.ObjectMemory.tpc.pub.MemoryListener = MemoryUpdates
+# ArmarX.ObjectMemory.tpc.sub.MemoryListener: Name of the `MemoryListener` topic to subscribe to.
+# Attributes:
+# - Default: MemoryUpdates
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.tpc.sub.MemoryListener = MemoryUpdates
+
+
# ArmarX.ObjectMemory.tpc.sub.ObjectPoseTopic: Name of the `ObjectPoseTopic` topic to subscribe to.
# Attributes:
# - Default: ObjectPoseTopic
diff --git a/scenarios/ArMemObjectMemory/config/ObjectPoseProviderExample.cfg b/scenarios/ArMemObjectMemory/config/ObjectPoseProviderExample.cfg
index 45075e6114eae82fe89f50317c0bced824c96b46..76fbf317102eb56ab01bfd0037544c69f85c6c27 100644
--- a/scenarios/ArMemObjectMemory/config/ObjectPoseProviderExample.cfg
+++ b/scenarios/ArMemObjectMemory/config/ObjectPoseProviderExample.cfg
@@ -135,6 +135,15 @@
# ArmarX.ObjectPoseProviderExample.Objects = KIT/Amicelli, KIT/YellowSaltCylinder
+# ArmarX.ObjectPoseProviderExample.SingleShot: If true, publishes only one snapshot.
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ObjectPoseProviderExample.SingleShot = false
+
+
# ArmarX.ObjectPoseProviderExample.tpc.pub.DebugObserver: Name of the `DebugObserver` topic to publish data to.
# Attributes:
# - Default: DebugObserver
diff --git a/scenarios/dasd/config/DynamicObstacleManager.cfg b/scenarios/dasd/config/DynamicObstacleManager.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..c2e4f0a514d269ca6fdc5cb0daa164c2be4e6cd3
--- /dev/null
+++ b/scenarios/dasd/config/DynamicObstacleManager.cfg
@@ -0,0 +1,327 @@
+# ==================================================================
+# DynamicObstacleManager 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.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.DynamicObstacleManager.AllowInRobotSpawning: Allow obstacles to spawn inside the robot
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.DynamicObstacleManager.AllowInRobotSpawning = false
+
+
+# ArmarX.DynamicObstacleManager.ArVizTopicName: Name of the ArViz topic
+# Attributes:
+# - Default: ArVizTopic
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DynamicObstacleManager.ArVizTopicName = ArVizTopic
+
+
+# ArmarX.DynamicObstacleManager.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.DynamicObstacleManager.EnableProfiling = false
+
+
+# ArmarX.DynamicObstacleManager.EnableRemove: Delete Obstacles when value < 0
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+ArmarX.DynamicObstacleManager.EnableRemove = true
+
+
+# ArmarX.DynamicObstacleManager.LineSecurityMargin: Security margin of line obstacles
+# Attributes:
+# - Default: 400
+# - Case sensitivity: yes
+# - Required: no
+ArmarX.DynamicObstacleManager.LineSecurityMargin = 250
+
+
+# ArmarX.DynamicObstacleManager.LineThickness: The thickness of line obstacles
+# Attributes:
+# - Default: 200
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DynamicObstacleManager.LineThickness = 200
+
+
+# ArmarX.DynamicObstacleManager.MaxLengthOfLines: Maximum length of lines in mm
+# Attributes:
+# - Default: 10000
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DynamicObstacleManager.MaxLengthOfLines = 10000
+
+
+# ArmarX.DynamicObstacleManager.MaxObstacleSize: The maximal obstacle size in mm.
+# Attributes:
+# - Default: 600
+# - Case sensitivity: yes
+# - Required: no
+ArmarX.DynamicObstacleManager.MaxObstacleSize = 400
+
+
+# ArmarX.DynamicObstacleManager.MaxObstacleValue: Maximum value for the obstacles
+# Attributes:
+# - Default: 5000
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DynamicObstacleManager.MaxObstacleValue = 5000
+
+
+# ArmarX.DynamicObstacleManager.MinLengthOfLines: Minimum length of lines in mm
+# Attributes:
+# - Default: 50
+# - Case sensitivity: yes
+# - Required: no
+ArmarX.DynamicObstacleManager.MinLengthOfLines = 50
+
+
+# ArmarX.DynamicObstacleManager.MinObstacleSize: The minimal obstacle size in mm.
+# Attributes:
+# - Default: 100
+# - Case sensitivity: yes
+# - Required: no
+ArmarX.DynamicObstacleManager.MinObstacleSize = 5
+
+
+# ArmarX.DynamicObstacleManager.MinObstacleValueForAccepting: Minimum value for the obstacles to get accepted
+# Attributes:
+# - Default: 1000
+# - Case sensitivity: yes
+# - Required: no
+ArmarX.DynamicObstacleManager.MinObstacleValueForAccepting = 5
+
+
+# ArmarX.DynamicObstacleManager.MinSampleRatioPerEllipsis: Minimum percentage of samples which have to be in an elllipsis to be considered as known obsacle
+# Attributes:
+# - Default: 0.699999988
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DynamicObstacleManager.MinSampleRatioPerEllipsis = 0.699999988
+
+
+# ArmarX.DynamicObstacleManager.MinSampleRatioPerLineSegment: Minimum percentage of samples which have to be in an elllipsis to be considered as known obsacle
+# Attributes:
+# - Default: 0.899999976
+# - Case sensitivity: yes
+# - Required: no
+ArmarX.DynamicObstacleManager.MinSampleRatioPerLineSegment = 0.5
+
+
+# ArmarX.DynamicObstacleManager.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.DynamicObstacleManager.MinimumLoggingLevel = Debug
+
+
+# ArmarX.DynamicObstacleManager.ObjectName: Name of IceGrid well-known object
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DynamicObstacleManager.ObjectName = ""
+
+
+# ArmarX.DynamicObstacleManager.ObstacleAvoidanceName: The name of the used obstacle avoidance interface
+# Attributes:
+# - Default: PlatformObstacleAvoidance
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DynamicObstacleManager.ObstacleAvoidanceName = PlatformObstacleAvoidance
+
+
+# ArmarX.DynamicObstacleManager.ObstacleSecurityMargin: Security margin of ellipsis obstacles
+# Attributes:
+# - Default: 500
+# - Case sensitivity: yes
+# - Required: no
+ArmarX.DynamicObstacleManager.ObstacleSecurityMargin = 300
+
+
+# ArmarX.DynamicObstacleManager.OnlyVisualizeObstacles: Connection to obstacle avoidance
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.DynamicObstacleManager.OnlyVisualizeObstacles = false
+
+
+# ArmarX.DynamicObstacleManager.UpdateInterval: The interval to check the obstacles
+# Attributes:
+# - Default: 500
+# - Case sensitivity: yes
+# - Required: no
+ArmarX.DynamicObstacleManager.UpdateInterval = 350
+
+
+# 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/dasd/config/ObstacleAwarePlatformUnit.cfg b/scenarios/dasd/config/ObstacleAwarePlatformUnit.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..c397d978b79078ebfd9908eb5bfd7c9a3e103c59
--- /dev/null
+++ b/scenarios/dasd/config/ObstacleAwarePlatformUnit.cfg
@@ -0,0 +1,332 @@
+# ==================================================================
+# ObstacleAwarePlatformUnit 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.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.ObstacleAwarePlatformUnit.ArVizTopicName: Name of the ArViz topic
+# Attributes:
+# - Default: ArVizTopic
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObstacleAwarePlatformUnit.ArVizTopicName = ArVizTopic
+
+
+# ArmarX.ObstacleAwarePlatformUnit.DebugObserverTopicName: Name of the topic the DebugObserver listens on
+# Attributes:
+# - Default: DebugObserver
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObstacleAwarePlatformUnit.DebugObserverTopicName = DebugObserver
+
+
+# ArmarX.ObstacleAwarePlatformUnit.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.ObstacleAwarePlatformUnit.EnableProfiling = false
+
+
+# ArmarX.ObstacleAwarePlatformUnit.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.ObstacleAwarePlatformUnit.MinimumLoggingLevel = Debug
+
+
+# ArmarX.ObstacleAwarePlatformUnit.ObjectName: Name of IceGrid well-known object
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObstacleAwarePlatformUnit.ObjectName = ""
+
+
+# ArmarX.ObstacleAwarePlatformUnit.PlatformName: Name of the platform (will publish values on PlatformName + 'State')
+# Attributes:
+# - Default: Platform
+# - Case sensitivity: yes
+# - Required: no
+ArmarX.ObstacleAwarePlatformUnit.PlatformName = Armar6PlatformUnit
+
+
+# ArmarX.ObstacleAwarePlatformUnit.RemoteStateComponentName: Name of the robot state component
+# Attributes:
+# - Default: RobotStateComponent
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObstacleAwarePlatformUnit.RemoteStateComponentName = RobotStateComponent
+
+
+# ArmarX.ObstacleAwarePlatformUnit.cmp.DynamicObstacleManager: Ice object name of the `DynamicObstacleManager` component.
+# Attributes:
+# - Default: ObstacleAvoidingPlatformUnit
+# - Case sensitivity: yes
+# - Required: no
+ArmarX.ObstacleAwarePlatformUnit.cmp.DynamicObstacleManager = DynamicObstacleManager
+
+
+# ArmarX.ObstacleAwarePlatformUnit.cmp.PlatformUnit: Ice object name of the `PlatformUnit` component.
+# Attributes:
+# - Default: Platform
+# - Case sensitivity: yes
+# - Required: no
+ArmarX.ObstacleAwarePlatformUnit.cmp.PlatformUnit = Armar6PlatformUnit
+
+
+# ArmarX.ObstacleAwarePlatformUnit.cmp.RobotStateComponent: Ice object name of the `RobotStateComponent` component.
+# Attributes:
+# - Default: RobotStateComponent
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObstacleAwarePlatformUnit.cmp.RobotStateComponent = RobotStateComponent
+
+
+# ArmarX.ObstacleAwarePlatformUnit.control.pos.kp:
+# Attributes:
+# - Default: 3.5
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObstacleAwarePlatformUnit.control.pos.kp = 3.5
+
+
+# ArmarX.ObstacleAwarePlatformUnit.control.pose.cycle_time: Control loop cycle time.
+# Attributes:
+# - Default: 10 ms
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObstacleAwarePlatformUnit.control.pose.cycle_time = 10 ms
+
+
+# ArmarX.ObstacleAwarePlatformUnit.control.rot.kd:
+# Attributes:
+# - Default: 0
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObstacleAwarePlatformUnit.control.rot.kd = 0
+
+
+# ArmarX.ObstacleAwarePlatformUnit.control.rot.ki:
+# Attributes:
+# - Default: 9.00000014e-05
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObstacleAwarePlatformUnit.control.rot.ki = 9.00000014e-05
+
+
+# ArmarX.ObstacleAwarePlatformUnit.control.rot.kp:
+# Attributes:
+# - Default: 1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObstacleAwarePlatformUnit.control.rot.kp = 1
+
+
+# ArmarX.ObstacleAwarePlatformUnit.doa.agent_safety_margin:
+# Attributes:
+# - Default: 0
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObstacleAwarePlatformUnit.doa.agent_safety_margin = 0
+
+
+# ArmarX.ObstacleAwarePlatformUnit.min_vel_general: Velocity in [mm/s] the robot should at least set on general
+# Attributes:
+# - Default: 100
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObstacleAwarePlatformUnit.min_vel_general = 100
+
+
+# ArmarX.ObstacleAwarePlatformUnit.min_vel_near_target: Velocity in [mm/s] the robot should at least set when near the target
+# Attributes:
+# - Default: 50
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObstacleAwarePlatformUnit.min_vel_near_target = 50
+
+
+# ArmarX.ObstacleAwarePlatformUnit.pos_near_threshold: Distance in [mm] after which the robot is considered to be near the target for min velocity, smoothing, etc.
+# Attributes:
+# - Default: 250
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObstacleAwarePlatformUnit.pos_near_threshold = 250
+
+
+# ArmarX.ObstacleAwarePlatformUnit.tpc.pub.GlobalRobotPoseLocalization: Name of the `GlobalRobotPoseLocalization` topic to publish data to.
+# Attributes:
+# - Default: GlobalRobotPoseLocalization
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObstacleAwarePlatformUnit.tpc.pub.GlobalRobotPoseLocalization = GlobalRobotPoseLocalization
+
+
+# ArmarX.ObstacleAwarePlatformUnit.tpc.pub.Odometry: Name of the `Odometry` topic to publish data to.
+# Attributes:
+# - Default: Odometry
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObstacleAwarePlatformUnit.tpc.pub.Odometry = Odometry
+
+
+# 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/dasd/config/global.cfg b/scenarios/dasd/config/global.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..897f1c82a0578538e7b463f4246587ba134e24de
--- /dev/null
+++ b/scenarios/dasd/config/global.cfg
@@ -0,0 +1,4 @@
+# ==================================================================
+# Global Config from Scenario dasd
+# ==================================================================
+
diff --git a/scenarios/dasd/dasd.scx b/scenarios/dasd/dasd.scx
new file mode 100644
index 0000000000000000000000000000000000000000..b12d5a0c9d6feb2eb4f19076d28212d9294eff02
--- /dev/null
+++ b/scenarios/dasd/dasd.scx
@@ -0,0 +1,6 @@
+<?xml version="1.0" encoding="utf-8"?>
+<scenario name="dasd" creation="2021-06-07.18:00:09" globalConfigName="./config/global.cfg" package="RobotAPI" deploymentType="local" nodeName="NodeMain">
+ <application name="ObstacleAwarePlatformUnit" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
+ <application name="DynamicObstacleManager" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
+</scenario>
+
diff --git a/source/RobotAPI/applications/AronCodeGenerator/main.cpp b/source/RobotAPI/applications/AronCodeGenerator/main.cpp
index 3de3dc06515dcc47c36657e51edb56baddb76823..fb8a2287f38cadcdbd71175a1e50ca171e7d5461 100644
--- a/source/RobotAPI/applications/AronCodeGenerator/main.cpp
+++ b/source/RobotAPI/applications/AronCodeGenerator/main.cpp
@@ -144,7 +144,7 @@ int main(int argc, char* argv[])
}
}
- cppcodegenerator::Writer writer("AronTestSegment", reader.getCodeIncludes());
+ cppserializer::Writer writer("AronTestSegment", reader.getCodeIncludes());
if (verbose)
{
diff --git a/source/RobotAPI/components/ArViz/ArVizStorage.cpp b/source/RobotAPI/components/ArViz/ArVizStorage.cpp
index 2b27537f1a3c0cd903b360b271ad7b02b94459ef..b0b29f25911e2899083979af9b94987dd7b70bc8 100644
--- a/source/RobotAPI/components/ArViz/ArVizStorage.cpp
+++ b/source/RobotAPI/components/ArViz/ArVizStorage.cpp
@@ -169,7 +169,7 @@ namespace armarx
- viz::data::LayerUpdates armarx::ArVizStorage::pullUpdatesSince(Ice::Long revision, const Ice::Current&)
+ viz::data::LayerUpdates ArVizStorage::pullUpdatesSince(Ice::Long revision, const Ice::Current&)
{
viz::data::LayerUpdates result;
diff --git a/source/RobotAPI/components/ArViz/CMakeLists.txt b/source/RobotAPI/components/ArViz/CMakeLists.txt
index 23bc568751839f9758cd09597b2b4f58b8b5e419..9a5a3adcbfedd3a756b8231b61db7591ad66047b 100644
--- a/source/RobotAPI/components/ArViz/CMakeLists.txt
+++ b/source/RobotAPI/components/ArViz/CMakeLists.txt
@@ -15,12 +15,16 @@ set(SOURCES
Client/elements/Mesh.cpp
Client/elements/Robot.cpp
Client/elements/RobotHand.cpp
+ Client/elements/Line.cpp
+ Client/elements/Path.cpp
+
Client/drawer/ArVizDrawerBase.cpp
Client/ScopedClient.cpp
Coin/ElementVisualizer.cpp
Coin/VisualizationRobot.cpp
+ Coin/VisualizationPath.cpp
Coin/VisualizationObject.cpp
Coin/Visualizer.cpp
@@ -49,6 +53,7 @@ set(HEADERS
Coin/VisualizationEllipsoid.h
Coin/VisualizationSphere.h
Coin/VisualizationPose.h
+ Coin/VisualizationPath.h
Coin/VisualizationLine.h
Coin/VisualizationText.h
Coin/VisualizationArrow.h
@@ -77,6 +82,8 @@ set(HEADERS
Client/elements/PointCloud.h
Client/elements/Robot.h
Client/elements/RobotHand.h
+ Client/elements/Line.h
+ Client/elements/Path.h
Client/drawer/ArVizDrawerBase.h
diff --git a/source/RobotAPI/components/ArViz/Client/Elements.cpp b/source/RobotAPI/components/ArViz/Client/Elements.cpp
index 3ce3ea6f48c69658788abc3ba2ed00c6a81e1002..daec8bc655e0f6431dc531f0401478838e7f053b 100644
--- a/source/RobotAPI/components/ArViz/Client/Elements.cpp
+++ b/source/RobotAPI/components/ArViz/Client/Elements.cpp
@@ -9,15 +9,18 @@ namespace armarx::viz
{
const std::string Object::DefaultObjectsPackage = armarx::ObjectFinder::DefaultObjectsPackageName;
+ const std::string Object::DefaultRelativeObjectsDirectory = armarx::ObjectFinder::DefaultObjectsDirectory;
- Object& Object::fileByObjectFinder(const std::string& objectID, const std::string& objectsPackage)
+ Object& Object::fileByObjectFinder(const std::string& objectID, const std::string& objectsPackage,
+ const std::string& relativeObjectsDirectory)
{
- return this->fileByObjectFinder(armarx::ObjectID(objectID), objectsPackage);
+ return this->fileByObjectFinder(armarx::ObjectID(objectID), objectsPackage, relativeObjectsDirectory);
}
- Object& Object::fileByObjectFinder(const armarx::ObjectID& objectID, const std::string& objectsPackage)
+ Object& Object::fileByObjectFinder(const armarx::ObjectID& objectID, const std::string& objectsPackage,
+ const std::string& relativeObjectsDirectory)
{
- ObjectInfo info(objectsPackage, "", objectID);
+ ObjectInfo info(objectsPackage, "", relativeObjectsDirectory, objectID);
armarx::PackageFileLocation file = info.simoxXML();
return this->file(file.package, file.relativePath);
}
diff --git a/source/RobotAPI/components/ArViz/Client/Elements.h b/source/RobotAPI/components/ArViz/Client/Elements.h
index 66f07992e0ba2dc0c382b3a9742a5413c9f192b6..e9157a24d9f60b9ba71d70cccd2141b0258665c3 100644
--- a/source/RobotAPI/components/ArViz/Client/Elements.h
+++ b/source/RobotAPI/components/ArViz/Client/Elements.h
@@ -461,6 +461,7 @@ namespace armarx::viz
{
private:
static const std::string DefaultObjectsPackage;
+ static const std::string DefaultRelativeObjectsDirectory;
public:
using ElementOps::ElementOps;
@@ -483,8 +484,12 @@ namespace armarx::viz
* @param objectsPackage The objects package ("ArmarXObjects" by default)
* @see <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
*/
- Object& fileByObjectFinder(const armarx::ObjectID& objectID, const std::string& objectsPackage = DefaultObjectsPackage);
- Object& fileByObjectFinder(const std::string& objectID, const std::string& objectsPackage = DefaultObjectsPackage);
+ Object& fileByObjectFinder(const armarx::ObjectID& objectID,
+ const std::string& objectsPackage = DefaultObjectsPackage,
+ const std::string& relativeObjectsDirectory = DefaultRelativeObjectsDirectory);
+ Object& fileByObjectFinder(const std::string& objectID,
+ const std::string& objectsPackage = DefaultObjectsPackage,
+ const std::string& relativeObjectsDirectory = DefaultRelativeObjectsDirectory);
Object& alpha(float alpha);
diff --git a/source/RobotAPI/components/ArViz/Client/drawer/ArVizDrawerBase.cpp b/source/RobotAPI/components/ArViz/Client/drawer/ArVizDrawerBase.cpp
index a19032cd088ccdf49e16af5ea394d73a92b30878..9d2c8e7ed36b6a494556ecdd55e652aea93b7e77 100644
--- a/source/RobotAPI/components/ArViz/Client/drawer/ArVizDrawerBase.cpp
+++ b/source/RobotAPI/components/ArViz/Client/drawer/ArVizDrawerBase.cpp
@@ -6,21 +6,27 @@
namespace armarx
{
- ArVizDrawerBase::ArVizDrawerBase(armarx::viz::Client &arviz) : arvizClient(arviz) {}
+ ArVizDrawerBase::ArVizDrawerBase(armarx::viz::Client& arviz) : arvizClient(arviz) {}
ArVizDrawerBase::~ArVizDrawerBase() = default;
- viz::Client &ArVizDrawerBase::arviz() { return arvizClient; }
+ viz::ScopedClient& ArVizDrawerBase::arviz()
+ {
+ return arvizClient;
+ }
- const viz::Client &ArVizDrawerBase::arviz() const { return arvizClient; }
+ const viz::ScopedClient& ArVizDrawerBase::arviz() const
+ {
+ return arvizClient;
+ }
- void ArVizDrawerBase::commit(const viz::Layer &layer)
+ void ArVizDrawerBase::commit(const viz::Layer& layer)
{
std::lock_guard guard{layerMtx};
arvizClient.commit(layer);
}
- void ArVizDrawerBase::commit(const std::vector<viz::Layer> &layers)
+ void ArVizDrawerBase::commit(const std::vector<viz::Layer>& layers)
{
std::lock_guard guard{layerMtx};
arvizClient.commit(layers);
diff --git a/source/RobotAPI/components/ArViz/Client/drawer/ArVizDrawerBase.h b/source/RobotAPI/components/ArViz/Client/drawer/ArVizDrawerBase.h
index f7c813be72283ec4899f402a1133c41ab5a381e2..e302acc5c27e025aac9c103793f7a5ed726d76d9 100644
--- a/source/RobotAPI/components/ArViz/Client/drawer/ArVizDrawerBase.h
+++ b/source/RobotAPI/components/ArViz/Client/drawer/ArVizDrawerBase.h
@@ -24,6 +24,8 @@
#include <mutex>
#include <vector>
+#include "RobotAPI/components/ArViz/Client/ScopedClient.h"
+
namespace armarx
{
// forward declaration
@@ -42,19 +44,19 @@ namespace armarx
*/
class ArVizDrawerBase
{
- public:
- ArVizDrawerBase(armarx::viz::Client &arviz);
+ public:
+ ArVizDrawerBase(armarx::viz::Client& arviz);
virtual ~ArVizDrawerBase();
- protected:
- viz::Client &arviz();
- const viz::Client &arviz() const;
+ protected:
+ viz::ScopedClient& arviz();
+ const viz::ScopedClient& arviz() const;
- void commit(const viz::Layer &layer);
- void commit(const std::vector<viz::Layer> &layers);
+ void commit(const viz::Layer& layer);
+ void commit(const std::vector<viz::Layer>& layers);
- private:
- viz::Client &arvizClient;
+ private:
+ viz::ScopedClient arvizClient;
std::mutex layerMtx;
};
diff --git a/source/RobotAPI/components/ArViz/Client/elements/Line.cpp b/source/RobotAPI/components/ArViz/Client/elements/Line.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..985162f1726a66c88c4a310e6b287641294fa9f9
--- /dev/null
+++ b/source/RobotAPI/components/ArViz/Client/elements/Line.cpp
@@ -0,0 +1,20 @@
+#include "Line.h"
+
+#include "ArmarXCore/interface/core/BasicVectorTypesHelpers.h"
+
+namespace armarx::viz
+{
+ Line& Line::lineWidth(float w)
+ {
+ data_->lineWidth = w;
+
+ return *this;
+ }
+ Line& Line::fromTo(Eigen::Vector3f from, Eigen::Vector3f to)
+ {
+ data_->from = ToBasicVectorType(from);
+ data_->to = ToBasicVectorType(to);
+
+ return *this;
+ }
+} // namespace armarx::viz
\ No newline at end of file
diff --git a/source/RobotAPI/components/ArViz/Client/elements/Line.h b/source/RobotAPI/components/ArViz/Client/elements/Line.h
new file mode 100644
index 0000000000000000000000000000000000000000..6f6d9427e05b9d91606e8ecdfa8a2164f4847515
--- /dev/null
+++ b/source/RobotAPI/components/ArViz/Client/elements/Line.h
@@ -0,0 +1,38 @@
+/*
+ * 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/>.
+ *
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include "RobotAPI/components/ArViz/Client/elements/ElementOps.h"
+#include <RobotAPI/interface/ArViz/Elements.h>
+
+namespace armarx::viz
+{
+ class Line : public ElementOps<Line, data::ElementLine>
+ {
+ public:
+ using ElementOps::ElementOps;
+
+ Line& lineWidth(float w);
+
+ Line& fromTo(Eigen::Vector3f from, Eigen::Vector3f to);
+ };
+} // namespace armarx::viz
diff --git a/source/RobotAPI/components/ArViz/Client/elements/Path.cpp b/source/RobotAPI/components/ArViz/Client/elements/Path.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..cf5e9899e943dd4076c2dd21ca9fe982b3c1a9bc
--- /dev/null
+++ b/source/RobotAPI/components/ArViz/Client/elements/Path.cpp
@@ -0,0 +1,49 @@
+#include "Path.h"
+
+#include <iterator>
+
+#include <ArmarXCore/interface/core/BasicVectorTypes.h>
+#include <ArmarXCore/interface/core/BasicVectorTypesHelpers.h>
+
+namespace armarx::viz
+{
+
+ Path& Path::clear()
+ {
+ data_->points.clear();
+
+ return *this;
+ }
+
+ Path& Path::width(float w)
+ {
+ data_->lineWidth = w;
+
+ return *this;
+ }
+
+ Path& Path::points(std::vector<Eigen::Vector3f> const& ps)
+ {
+ auto& points = data_->points;
+ points.clear();
+ points.reserve(ps.size());
+
+ std::transform(ps.begin(),
+ ps.end(),
+ std::back_inserter(points),
+ [](const auto & e)
+ {
+ return ToBasicVectorType(e);
+ });
+
+ return *this;
+ }
+
+ Path& Path::addPoint(Eigen::Vector3f p)
+ {
+ data_->points.emplace_back(ToBasicVectorType(p));
+
+ return *this;
+ }
+
+} // namespace armarx::viz
\ No newline at end of file
diff --git a/source/RobotAPI/components/ArViz/Client/elements/Path.h b/source/RobotAPI/components/ArViz/Client/elements/Path.h
new file mode 100644
index 0000000000000000000000000000000000000000..7cfbecbd973a194cef0e0e350a5c7c3c1aedbc13
--- /dev/null
+++ b/source/RobotAPI/components/ArViz/Client/elements/Path.h
@@ -0,0 +1,49 @@
+
+
+/*
+ * 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/>.
+ *
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include "RobotAPI/components/ArViz/Client/elements/ElementOps.h"
+#include <RobotAPI/interface/ArViz/Elements.h>
+
+namespace armarx::viz
+{
+ class Path : public ElementOps<Path, data::ElementPath>
+ {
+ public:
+ using ElementOps::ElementOps;
+
+ Path& clear();
+
+ Path& width(float w);
+
+ Path& points(std::vector<Eigen::Vector3f> const& ps);
+
+ Path& addPoint(Eigen::Vector3f p);
+ };
+} // namespace armarx::viz
+
+
+
+
+
diff --git a/source/RobotAPI/components/ArViz/Coin/RegisterVisualizationTypes.cpp b/source/RobotAPI/components/ArViz/Coin/RegisterVisualizationTypes.cpp
index 33febe24be91fd428f06762cbcfc6d7e48f101a0..5ca14001aff6c7be187ad4c7736e86fd3ab6b3c6 100644
--- a/source/RobotAPI/components/ArViz/Coin/RegisterVisualizationTypes.cpp
+++ b/source/RobotAPI/components/ArViz/Coin/RegisterVisualizationTypes.cpp
@@ -15,13 +15,14 @@
#include "VisualizationMesh.h"
#include "VisualizationRobot.h"
#include "VisualizationObject.h"
+#include "VisualizationPath.h"
void armarx::viz::CoinVisualizer::registerVisualizationTypes()
{
using namespace armarx::viz::coin;
- elementVisualizers.reserve(15);
+ elementVisualizers.reserve(16);
registerVisualizerFor<VisualizationBox>();
registerVisualizerFor<VisualizationCylinder>();
@@ -38,4 +39,5 @@ void armarx::viz::CoinVisualizer::registerVisualizationTypes()
registerVisualizerFor<VisualizationMesh>();
registerVisualizerFor<VisualizationRobot>();
registerVisualizerFor<VisualizationObject>();
+ registerVisualizerFor<VisualizationPath>();
}
diff --git a/source/RobotAPI/components/ArViz/Coin/VisualizationPath.cpp b/source/RobotAPI/components/ArViz/Coin/VisualizationPath.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..a0190abd75a8438a2371c728a580837d68c3eecc
--- /dev/null
+++ b/source/RobotAPI/components/ArViz/Coin/VisualizationPath.cpp
@@ -0,0 +1,97 @@
+#include "VisualizationPath.h"
+
+#include <algorithm>
+
+#include <Inventor/SbVec3f.h>
+#include <Inventor/nodes/SoCoordinate3.h>
+#include <Inventor/nodes/SoDrawStyle.h>
+#include <Inventor/nodes/SoLineSet.h>
+#include <Inventor/nodes/SoPointSet.h>
+
+namespace armarx::viz::coin
+{
+
+ VisualizationPath::VisualizationPath()
+ {
+ coordinate3 = new SoCoordinate3;
+
+ // create line around polygon
+ SoSeparator* lineSep = new SoSeparator;
+
+ lineMaterial = new SoMaterial;
+ lineSep->addChild(lineMaterial);
+ lineSep->addChild(coordinate3);
+
+ lineStyle = new SoDrawStyle();
+ lineSep->addChild(lineStyle);
+
+ lineSet = new SoLineSet;
+ lineSep->addChild(lineSet);
+
+ node->addChild(coordinate3);
+ node->addChild(lineSep);
+
+ // points (use the following, if the points should be drawn in a different color)
+
+ // pclMat = new SoMaterial;
+
+ // SoMaterialBinding* pclMatBind = new SoMaterialBinding;
+ // pclMatBind->value = SoMaterialBinding::PER_PART;
+
+ pclCoords = new SoCoordinate3;
+ pclStyle = new SoDrawStyle;
+
+ // node->addChild(pclMat);
+ // node->addChild(pclMatBind);
+ node->addChild(pclCoords);
+ node->addChild(pclStyle);
+ node->addChild(new SoPointSet);
+ }
+
+ bool VisualizationPath::update(ElementType const& element)
+ {
+ // set position
+ coordinate3->point.setValuesPointer(element.points.size(),
+ reinterpret_cast<const float*>(element.points.data()));
+
+ // set color
+ const auto lineColor = element.color;
+
+ constexpr float toUnit = 1.0F / 255.0F;
+
+ const auto color = SbVec3f(lineColor.r, lineColor.g, lineColor.b) * toUnit;
+ const float transparency = 1.0F - static_cast<float>(lineColor.a) * toUnit;
+
+ lineMaterial->diffuseColor.setValue(color);
+ lineMaterial->ambientColor.setValue(color);
+ lineMaterial->transparency.setValue(transparency);
+
+ if (element.lineWidth > 0.0F)
+ {
+ lineStyle->lineWidth.setValue(element.lineWidth);
+ }
+ else
+ {
+ lineStyle->style = SoDrawStyleElement::INVISIBLE;
+ }
+
+ const int pointSize = static_cast<int>(element.points.size());
+ lineSet->numVertices.set1Value(0, pointSize);
+
+ // points at each node
+ // const std::vector<SbVec3f> colorData(element.points.size(), color);
+
+ // pclMat->diffuseColor.setValuesPointer(colorData.size(),
+ // reinterpret_cast<const float*>(colorData.data()));
+ // pclMat->ambientColor.setValuesPointer(colorData.size(),
+ // reinterpret_cast<const float*>(colorData.data()));
+ // pclMat->transparency = transparency;
+
+ pclCoords->point.setValuesPointer(element.points.size(),
+ reinterpret_cast<const float*>(element.points.data()));
+
+ pclStyle->pointSize = element.lineWidth + 5;
+
+ return true;
+ }
+} // namespace armarx::viz::coin
\ No newline at end of file
diff --git a/source/RobotAPI/components/ArViz/Coin/VisualizationPath.h b/source/RobotAPI/components/ArViz/Coin/VisualizationPath.h
new file mode 100644
index 0000000000000000000000000000000000000000..487ee9df58baeb6b91e91de6fdb3be873511a0af
--- /dev/null
+++ b/source/RobotAPI/components/ArViz/Coin/VisualizationPath.h
@@ -0,0 +1,55 @@
+/*
+ * 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/>.
+ *
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include "ElementVisualizer.h"
+
+#include <RobotAPI/interface/ArViz/Elements.h>
+
+class SoCoordinate3;
+class SoDrawStyle;
+class SoLineSet;
+
+namespace armarx::viz::coin
+{
+ struct VisualizationPath : TypedElementVisualization<SoSeparator>
+ {
+ using ElementType = data::ElementPath;
+
+ VisualizationPath();
+
+ bool update(ElementType const& element);
+
+ /// lines
+ SoCoordinate3* coordinate3;
+ SoDrawStyle* lineStyle;
+ SoLineSet* lineSet;
+ SoMaterial* lineMaterial;
+
+
+ /// points
+ // SoMaterial* pclMat;
+ SoCoordinate3* pclCoords;
+ SoDrawStyle* pclStyle;
+
+ };
+} // namespace armarx::viz::coin
diff --git a/source/RobotAPI/components/ArViz/Coin/Visualizer.cpp b/source/RobotAPI/components/ArViz/Coin/Visualizer.cpp
index 96ad7fed33e9be2813679f6ab20405f85300fc57..5f99047a649e80be511efd37c1cfbc8ac438031a 100644
--- a/source/RobotAPI/components/ArViz/Coin/Visualizer.cpp
+++ b/source/RobotAPI/components/ArViz/Coin/Visualizer.cpp
@@ -51,11 +51,11 @@ namespace armarx::viz
return "UNKNOWN";
}
- static void updateVisualizationCB(void* data, SoSensor* sensor)
- {
- auto* visu = static_cast<CoinVisualizer*>(data);
- visu->update();
- }
+ // static void updateVisualizationCB(void* data, SoSensor* sensor)
+ // {
+ // auto* visu = static_cast<CoinVisualizer*>(data);
+ // visu->update();
+ // }
struct TimedBlock
{
@@ -89,10 +89,10 @@ namespace armarx::viz
&CoinVisualizerWrapper::onUpdateFailure);
root = new SoSeparator;
- timerSensor = new SoTimerSensor(updateVisualizationCB, this);
+ //timerSensor = new SoTimerSensor(updateVisualizationCB, this);
- float cycleTimeMS = 33.0f;
- timerSensor->setInterval(SbTime(cycleTimeMS / 1000.0f));
+ //float cycleTimeMS = 33.0f;
+ //timerSensor->setInterval(SbTime(cycleTimeMS / 1000.0f));
}
CoinVisualizer::~CoinVisualizer()
@@ -115,8 +115,8 @@ namespace armarx::viz
state = CoinVisualizerState::STARTING;
stateStorage = storage;
- SoSensorManager* sensor_mgr = SoDB::getSensorManager();
- sensor_mgr->insertTimerSensor(timerSensor);
+ //SoSensorManager* sensor_mgr = SoDB::getSensorManager();
+ //sensor_mgr->insertTimerSensor(timerSensor);
}
void CoinVisualizer::stop()
@@ -272,6 +272,10 @@ namespace armarx::viz
void CoinVisualizer::update()
{
+ {
+ std::lock_guard lock(timingMutex);
+ lastTiming.updateToggle = (lastTiming.updateToggle + 1) % 10;
+ }
switch (state)
{
case CoinVisualizerState::STARTING:
@@ -312,6 +316,7 @@ namespace armarx::viz
// We should restart the pull for updates so it can run in parallel
data::LayerUpdates currentUpdates = pulledUpdates;
updateResult = CoinVisualizerUpdateResult::WAITING;
+ timing.waitStart = time_start;
storage->begin_pullUpdatesSince(currentUpdates.revision, callback);
auto layerIDsBefore = getLayerIDs();
@@ -343,6 +348,7 @@ namespace armarx::viz
// Copy the timing result
std::lock_guard lock(timingMutex);
timing.counter = lastTiming.counter + 1;
+ timing.updateToggle = lastTiming.updateToggle;
lastTiming = std::move(timing);
}
}
@@ -350,7 +356,13 @@ namespace armarx::viz
case CoinVisualizerUpdateResult::WAITING:
{
// Still waiting for result
- } break;
+ {
+ // Copy the timing result
+ std::lock_guard lock(timingMutex);
+ lastTiming.waitDuration = IceUtil::Time::now() - lastTiming.waitStart;
+ }
+ }
+ break;
case CoinVisualizerUpdateResult::FAILURE:
{
std::unique_lock<std::mutex> lock(stateMutex);
diff --git a/source/RobotAPI/components/ArViz/Coin/Visualizer.h b/source/RobotAPI/components/ArViz/Coin/Visualizer.h
index 5cb71b0fecc568e06b42ae49d8f8e9525104feea..49ad2a51bd6f70dfddac1785296b8abc07ba08c9 100644
--- a/source/RobotAPI/components/ArViz/Coin/Visualizer.h
+++ b/source/RobotAPI/components/ArViz/Coin/Visualizer.h
@@ -174,6 +174,11 @@ namespace armarx::viz
IceUtil::Time layersChanged = IceUtil::Time::seconds(0);
IceUtil::Time total = IceUtil::Time::seconds(0);
+ IceUtil::Time waitStart = IceUtil::Time::seconds(0);
+ IceUtil::Time waitDuration = IceUtil::Time::seconds(0);
+
+ int updateToggle = 0;
+
int counter = 0;
};
diff --git a/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.cpp b/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.cpp
index d1e48cdea6aa941033225c291e50a3da312f9475..487c9c6fc4c732fc1d4fe2f126724cb74c8dd810 100644
--- a/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.cpp
+++ b/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.cpp
@@ -20,8 +20,6 @@
*/
#include "ArticulatedObjectLocalizerExample.h"
-#include "ArmarXCore/core/services/tasks/PeriodicTask.h"
-
#include <memory>
@@ -29,29 +27,32 @@
#include <IceUtil/Time.h>
+#include <SimoxUtility/math/pose/pose.h>
#include <VirtualRobot/Robot.h>
-#include <VirtualRobot/XML/RobotIO.h>
#include <VirtualRobot/VirtualRobot.h>
+#include <VirtualRobot/XML/RobotIO.h>
-#include <ArmarXCore/core/logging/Logging.h>
+#include "ArmarXCore/core/services/tasks/PeriodicTask.h"
#include <ArmarXCore/core/PackagePath.h>
-#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/core/time/CycleUtil.h>
#include <RobotAPI/libraries/armem/client/query/Builder.h>
#include <RobotAPI/libraries/armem/client/query/query_fns.h>
+#include <RobotAPI/libraries/armem/core/Time.h>
#include <RobotAPI/libraries/armem/core/workingmemory/ice_conversions.h>
#include <RobotAPI/libraries/armem/server/MemoryRemoteGui.h>
-#include <RobotAPI/libraries/armem/core/Time.h>
#include <RobotAPI/libraries/armem_objects/types.h>
-
namespace armarx::articulated_object
{
ArticulatedObjectLocalizerExample::ArticulatedObjectLocalizerExample() :
- articulatedObjectWriter(new ::armarx::armem::articulated_object::Writer(*this)),
- articulatedObjectReader(new ::armarx::armem::articulated_object::Reader(*this)) {}
+ articulatedObjectWriter(new ::armarx::armem::articulated_object::ArticulatedObjectWriter(memoryNameSystem)),
+ articulatedObjectReader(new ::armarx::armem::articulated_object::ArticulatedObjectReader(memoryNameSystem))
+ {
+ }
armarx::PropertyDefinitionsPtr ArticulatedObjectLocalizerExample::createPropertyDefinitions()
{
@@ -62,6 +63,7 @@ namespace armarx::articulated_object
defs->optional(p.updateFrequency, "updateFrequency", "Memory update frequency (write).");
+ // Reader will override some properties of writer.
articulatedObjectWriter->registerPropertyDefinitions(defs);
articulatedObjectReader->registerPropertyDefinitions(defs);
@@ -73,7 +75,11 @@ namespace armarx::articulated_object
return "ArticulatedObjectLocalizerExample";
}
- void ArticulatedObjectLocalizerExample::onInitComponent() {}
+ void ArticulatedObjectLocalizerExample::onInitComponent()
+ {
+ // Reader overwrote property registered property of articulatedObjectWriter.
+ articulatedObjectWriter->setProviderName(articulatedObjectReader->getProviderName());
+ }
void ArticulatedObjectLocalizerExample::onConnectComponent()
{
@@ -83,7 +89,8 @@ namespace armarx::articulated_object
ARMARX_IMPORTANT << "Running example.";
start = armem::Time::now();
- task = new PeriodicTask<ArticulatedObjectLocalizerExample>(this, &ArticulatedObjectLocalizerExample::run, 1000 / p.updateFrequency);
+ task = new PeriodicTask<ArticulatedObjectLocalizerExample>(
+ this, &ArticulatedObjectLocalizerExample::run, 1000.f / p.updateFrequency);
task->start();
}
@@ -92,20 +99,23 @@ namespace armarx::articulated_object
task->stop();
}
- void ArticulatedObjectLocalizerExample::onExitComponent() {}
+ void ArticulatedObjectLocalizerExample::onExitComponent()
+ {
+ }
VirtualRobot::RobotPtr ArticulatedObjectLocalizerExample::createDishwasher()
{
- const std::string dishwasherName = "CupboardWithDishwasher";
+ const std::string dishwasherName = "Kitchen/mobile-dishwasher";
const auto descriptions = articulatedObjectReader->queryDescriptions(IceUtil::Time::now());
ARMARX_INFO << "Found " << descriptions.size() << " articulated object descriptions";
- const auto it = std::find_if(descriptions.begin(), descriptions.end(), [&](const armem::articulated_object::ArticulatedObjectDescription & desc) -> bool
- {
- return desc.name == dishwasherName;
- });
+ const auto it = std::find_if(
+ descriptions.begin(),
+ descriptions.end(),
+ [&](const armem::articulated_object::ArticulatedObjectDescription & desc) -> bool
+ { return desc.name == dishwasherName; });
if (it == descriptions.end())
{
@@ -113,30 +123,23 @@ namespace armarx::articulated_object
return nullptr;
}
- return VirtualRobot::RobotIO::loadRobot(ArmarXDataPath::resolvePath(it->xml.serialize().path), VirtualRobot::RobotIO::eStructure);
- }
-
- armem::articulated_object::ArticulatedObject convert(const VirtualRobot::Robot& obj, const armem::Time& timestamp)
- {
- ARMARX_DEBUG << "Filename is " << obj.getFilename();
+ auto obj =
+ VirtualRobot::RobotIO::loadRobot(ArmarXDataPath::resolvePath(it->xml.serialize().path),
+ VirtualRobot::RobotIO::eStructure);
- return
- armem::articulated_object::ArticulatedObject
+ if (not obj)
{
- .description = {
- .name = obj.getName(),
- .xml = PackagePath(armarx::ArmarXDataPath::getProject({"ArmarXObjects"}, obj.getFilename()), obj.getFilename())
- },
- .instance = "", // TODO(fabian.reister):
- .config = {
- .timestamp = timestamp,
- .globalPose = Eigen::Affine3f(obj.getRootNode()->getGlobalPose()),
- .jointMap = obj.getJointValues()
- },
- .timestamp = timestamp
- };
+ return nullptr;
+ }
+
+ obj->setName("MobileDishwasher0");
+ obj->setType(it->name);
+
+ return obj;
}
+
+
void ArticulatedObjectLocalizerExample::run()
{
if (dishwasher == nullptr)
@@ -152,21 +155,19 @@ namespace armarx::articulated_object
ARMARX_DEBUG << "Reporting articulated objects";
const IceUtil::Time now = TimeUtil::GetTime();
- const float t = float((now - start).toSecondsDouble());
+ const float t = float((now - start).toSecondsDouble());
// move joints at certain frequency
const float k = (1 + std::sin(t / (M_2_PIf32))) / 2; // in [0,1]
- const std::map<std::string, float> jointValues
- {
- {"dishwasher_door_joint", M_PIf32 / 2 * k},
- {"drawer_joint", 350 * k}
- };
+ const std::map<std::string, float> jointValues{{"dishwasher_door_joint", M_PIf32 / 2 * k},
+ {"drawer_joint", 350 * k}};
+ dishwasher->setGlobalPose(simox::math::pose(Eigen::Vector3f(1000, 0, 0)));
dishwasher->setJointValues(jointValues);
- armarx::armem::articulated_object::ArticulatedObject armemDishwasher = convert(*dishwasher, IceUtil::Time::now());
- articulatedObjectWriter->store(armemDishwasher);
+
+ articulatedObjectWriter->storeArticulatedObject(dishwasher, IceUtil::Time::now());
}
} // namespace armarx::articulated_object
diff --git a/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.h b/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.h
index 75c0764cfb9c6103a82c68c0faba2b984ffabb86..f85a804e97337e1193ca1f38048beb103e65ce59 100644
--- a/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.h
+++ b/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.h
@@ -1,24 +1,22 @@
#pragma once
+#include <VirtualRobot/VirtualRobot.h>
-// ArmarX
#include "ArmarXCore/core/services/tasks/PeriodicTask.h"
-#include "RobotAPI/libraries/armem/core/Time.h"
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/interface/observers/ObserverInterface.h>
#include <ArmarXCore/util/tasks.h>
+
#include <ArmarXGui/libraries/ArmarXGuiComponentPlugins/LightweightRemoteGuiComponentPlugin.h>
-// RobotAPI
-#include <RobotAPI/interface/armem/server/MemoryInterface.h>
+#include "RobotAPI/libraries/armem/core/Time.h"
#include <RobotAPI/interface/armem/mns/MemoryNameSystemInterface.h>
+#include <RobotAPI/interface/armem/server/MemoryInterface.h>
#include <RobotAPI/libraries/armem/client/ComponentPlugin.h>
#include <RobotAPI/libraries/armem/core/workingmemory/Memory.h>
-
-#include <RobotAPI/libraries/armem_objects/client/articulated_object/Writer.h>
-#include <RobotAPI/libraries/armem_objects/client/articulated_object/Reader.h>
-#include <VirtualRobot/VirtualRobot.h>
+#include <RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectReader.h>
+#include <RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectWriter.h>
namespace armarx::articulated_object
{
@@ -45,7 +43,6 @@ namespace armarx::articulated_object
std::string getDefaultName() const override;
protected:
-
armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
void onInitComponent() override;
@@ -55,9 +52,7 @@ namespace armarx::articulated_object
void run();
-
private:
-
VirtualRobot::RobotPtr createDishwasher();
std::shared_ptr<VirtualRobot::Robot> dishwasher;
@@ -68,14 +63,14 @@ namespace armarx::articulated_object
armarx::DebugObserverInterfacePrx debugObserver;
- std::unique_ptr<::armarx::armem::articulated_object::Writer> articulatedObjectWriter;
- std::unique_ptr<::armarx::armem::articulated_object::Reader> articulatedObjectReader;
+ std::unique_ptr<::armarx::armem::articulated_object::ArticulatedObjectWriter> articulatedObjectWriter;
+ std::unique_ptr<::armarx::armem::articulated_object::ArticulatedObjectReader>
+ articulatedObjectReader;
struct Properties
{
float updateFrequency{25.F};
} p;
-
};
-} // namespace armarx::articulated_object
+} // namespace armarx::articulated_object
diff --git a/source/RobotAPI/components/DynamicObstacleManager/DynamicObstacleManager.cpp b/source/RobotAPI/components/DynamicObstacleManager/DynamicObstacleManager.cpp
index e712e30ccb2978fdc7caa0210b919773b7dce46b..f15496da384cd8c25169bdcc5decf93b730cff6e 100644
--- a/source/RobotAPI/components/DynamicObstacleManager/DynamicObstacleManager.cpp
+++ b/source/RobotAPI/components/DynamicObstacleManager/DynamicObstacleManager.cpp
@@ -22,6 +22,8 @@
#include "DynamicObstacleManager.h"
+#include <VirtualRobot/MathTools.h>
+
// STD/STL
#include <string>
#include <vector>
@@ -118,6 +120,7 @@ namespace armarx
void DynamicObstacleManager::add_decayable_line_segment(const Eigen::Vector2f& line_start, const Eigen::Vector2f& line_end, const Ice::Current&)
{
+ TIMING_START(add_decayable_line_segment);
const Eigen::Vector2f difference_vec = line_end - line_start;
const float length = difference_vec.norm();
const Eigen::Vector2f center_vec = line_start + 0.5 * difference_vec;
@@ -136,10 +139,10 @@ namespace armarx
return;
}
-
{
std::shared_lock<std::shared_mutex> l(m_managed_obstacles_mutex);
+ TIMING_START(add_decayable_line_segment_mutex);
// First check own obstacles
for (ManagedObstaclePtr& managed_obstacle : m_managed_obstacles)
{
@@ -155,13 +158,15 @@ namespace armarx
managed_obstacle->line_matches.push_back(line_start);
managed_obstacle->line_matches.push_back(line_end);
managed_obstacle->m_last_matched = IceUtil::Time::now();
+ TIMING_END(add_decayable_line_segment_mutex);
return;
}
}
+ TIMING_END(add_decayable_line_segment_mutex);
}
// Second check the obstacles from the obstacle avoidance
- for (const auto& published_obstacle : m_obstacle_detection->getObstacles())
+ /*for (const auto& published_obstacle : m_obstacle_detection->getObstacles())
{
float coverage = ManagedObstacle::line_segment_ellipsis_coverage(
{published_obstacle.posX, published_obstacle.posY}, published_obstacle.axisLengthX, published_obstacle.axisLengthY, published_obstacle.yaw,
@@ -172,7 +177,7 @@ namespace armarx
ARMARX_DEBUG << "Found the obstacle in the static obstacle list! Matching name: " << published_obstacle.name;
return;
}
- }
+ }*/
ARMARX_DEBUG << " No matching obstacle found. Create new obstacle and add to list";
ManagedObstaclePtr new_managed_obstacle(new ManagedObstacle());
@@ -198,6 +203,15 @@ namespace armarx
std::lock_guard<std::shared_mutex> l(m_managed_obstacles_mutex);
m_managed_obstacles.push_back(new_managed_obstacle);
}
+ TIMING_END(add_decayable_line_segment);
+ }
+
+ void DynamicObstacleManager::add_decayable_line_segments(const dynamicobstaclemanager::LineSegments& lines, const Ice::Current& c)
+ {
+ for (const auto& line : lines)
+ {
+ add_decayable_line_segment(line.lineStart, line.lineEnd, c);
+ }
}
@@ -218,7 +232,7 @@ namespace armarx
std::lock_guard l(m_managed_obstacles_mutex);
ARMARX_DEBUG << "Remove Obstacle " << name << " from obstacle map and from obstacledetection";
- for (ManagedObstaclePtr& managed_obstacle : m_managed_obstacles)
+ for (ManagedObstaclePtr managed_obstacle : m_managed_obstacles)
{
if (managed_obstacle->m_obstacle.name == name)
{
@@ -240,6 +254,53 @@ namespace armarx
}
+ float
+ DynamicObstacleManager::distanceToObstacle(const Eigen::Vector2f& agentPosition, float safetyRadius, const Eigen::Vector2f& goal, const Ice::Current&)
+ {
+ std::shared_lock<std::shared_mutex> l{m_managed_obstacles_mutex};
+
+ const Eigen::Vector2f diff = goal - agentPosition;
+ const Eigen::Vector2f orthogonal_normalized = Eigen::Vector2f(Eigen::Rotation2Df(M_PI_2f32) * diff).normalized();
+
+ const float sample_step = 5; // in [mm], sample step size towards goal.
+ const float distance_to_goal = diff.norm() + safetyRadius;
+
+ float current_distance = safetyRadius;
+
+ while (current_distance < distance_to_goal)
+ {
+ for (const auto man_obstacle : m_managed_obstacles)
+ {
+ Eigen::Vector2f sample = agentPosition + ((goal - agentPosition).normalized() * current_distance);
+ Eigen::Vector2f sample_left = sample + (orthogonal_normalized * 250);
+ Eigen::Vector2f sample_right = sample - (orthogonal_normalized * 250);
+
+ obstacledetection::Obstacle obstacle = man_obstacle->m_obstacle;
+ Eigen::Vector2f obstacle_origin{obstacle.posX, obstacle.posY};
+
+ if (ManagedObstacle::point_ellipsis_coverage(obstacle_origin, obstacle.axisLengthX, obstacle.axisLengthY, obstacle.yaw, sample))
+ {
+ return current_distance - safetyRadius;
+ }
+
+ if (ManagedObstacle::point_ellipsis_coverage(obstacle_origin, obstacle.axisLengthX, obstacle.axisLengthY, obstacle.yaw, sample_left))
+ {
+ return current_distance - safetyRadius;
+ }
+
+ if (ManagedObstacle::point_ellipsis_coverage(obstacle_origin, obstacle.axisLengthX, obstacle.axisLengthY, obstacle.yaw, sample_right))
+ {
+ return current_distance - safetyRadius;
+ }
+ }
+
+ current_distance += sample_step;
+ }
+
+ return std::numeric_limits<float>::infinity();
+ }
+
+
void DynamicObstacleManager::directly_update_obstacle(const std::string& name, const Eigen::Vector2f& obs_pos, float e_rx, float e_ry, float e_yaw, const Ice::Current&)
{
obstacledetection::Obstacle new_unmanaged_obstacle;
@@ -283,7 +344,6 @@ namespace armarx
}
}
-
// Update obstacle avoidance
int checked_obstacles = 0;
bool updated = false;
diff --git a/source/RobotAPI/components/DynamicObstacleManager/DynamicObstacleManager.h b/source/RobotAPI/components/DynamicObstacleManager/DynamicObstacleManager.h
index 1629b7efe3102dccf85521ac0fac6379e09225cc..5f2d8065a307706ea16789930c12f5165d6044eb 100644
--- a/source/RobotAPI/components/DynamicObstacleManager/DynamicObstacleManager.h
+++ b/source/RobotAPI/components/DynamicObstacleManager/DynamicObstacleManager.h
@@ -69,10 +69,12 @@ namespace armarx
void add_decayable_obstacle(const Eigen::Vector2f&, float, float, float, const Ice::Current& = Ice::Current()) override;
void add_decayable_line_segment(const Eigen::Vector2f&, const Eigen::Vector2f&, const Ice::Current& = Ice::Current()) override;
+ void add_decayable_line_segments(const dynamicobstaclemanager::LineSegments& lines, const Ice::Current& = Ice::Current()) override;
void remove_all_decayable_obstacles(const Ice::Current& = Ice::Current()) override;
void directly_update_obstacle(const std::string& name, const Eigen::Vector2f&, float, float, float, const Ice::Current& = Ice::Current()) override;
void remove_obstacle(const std::string& name, const Ice::Current& = Ice::Current()) override;
void wait_unitl_obstacles_are_ready(const Ice::Current& = Ice::Current()) override;
+ float distanceToObstacle(const Eigen::Vector2f& agentPosition, float safetyRadius, const Eigen::Vector2f& goal, const Ice::Current& = Ice::Current()) override;
protected:
void onInitComponent() override;
diff --git a/source/RobotAPI/components/DynamicObstacleManager/ManagedObstacle.cpp b/source/RobotAPI/components/DynamicObstacleManager/ManagedObstacle.cpp
index fe21cc4e46558b3a70102592ee77ef56715421f8..f718a45434ec538fbd5d2541fb9413568f3e79f0 100644
--- a/source/RobotAPI/components/DynamicObstacleManager/ManagedObstacle.cpp
+++ b/source/RobotAPI/components/DynamicObstacleManager/ManagedObstacle.cpp
@@ -94,10 +94,25 @@ namespace armarx
float ManagedObstacle::line_segment_ellipsis_coverage(const Eigen::Vector2f e_origin, const float e_rx, const float e_ry, const float e_yaw, const Eigen::Vector2f line_seg_start, const Eigen::Vector2f line_seg_end)
{
// Again we discretize the line into n points and we check the coverage of those points
- const unsigned int SAMPLES = 40;
const Eigen::Vector2f difference_vec = line_seg_end - line_seg_start;
const Eigen::Vector2f difference_vec_normed = difference_vec.normalized();
const float distance = difference_vec.norm();
+ const unsigned int SAMPLES = std::max(distance / 10.0, 40.0);
+
+ const Vector2f difference_start_origin = e_origin - line_seg_start;
+ const Vector2f difference_end_origin = e_origin - line_seg_end;
+
+ if (difference_start_origin.norm() > std::max(e_rx, e_ry) && distance < std::max(e_rx, e_ry))
+ {
+ return 0.0;
+ }
+
+ if (difference_end_origin.norm() > std::max(e_rx, e_ry) && distance < std::max(e_rx, e_ry))
+ {
+ return 0.0;
+ }
+
+
const float step_size = distance / SAMPLES;
const Eigen::Vector2f dir = difference_vec_normed * step_size;
diff --git a/source/RobotAPI/components/KITProstheticHandUnit/CMakeLists.txt b/source/RobotAPI/components/KITProstheticHandUnit/CMakeLists.txt
index 92bc44f64073b1a26ceb15782207265f7bbdcad0..7becda0cb17dca65fb607f7c518a2d2a12302078 100644
--- a/source/RobotAPI/components/KITProstheticHandUnit/CMakeLists.txt
+++ b/source/RobotAPI/components/KITProstheticHandUnit/CMakeLists.txt
@@ -4,6 +4,7 @@ find_package(Qt5 COMPONENTS Core Bluetooth QUIET)
armarx_build_if(Qt5_FOUND "Qt5 Core or Bluetooth not available")
set(COMPONENT_LIBS
+ RemoteGui
RobotAPIUnits
KITProstheticHandDriver
)
diff --git a/source/RobotAPI/components/ObjectPoseProviderExample/ObjectPoseProviderExample.cpp b/source/RobotAPI/components/ObjectPoseProviderExample/ObjectPoseProviderExample.cpp
index c0ef77b2f4b728d245c446b9782336843157d9e6..a87d465a301b423defc89260a52cb15164bcd37d 100644
--- a/source/RobotAPI/components/ObjectPoseProviderExample/ObjectPoseProviderExample.cpp
+++ b/source/RobotAPI/components/ObjectPoseProviderExample/ObjectPoseProviderExample.cpp
@@ -42,6 +42,8 @@ namespace armarx
defs->optional(initialObjectIDs, "Objects", "Object IDs of objects to be tracked.")
.map(simox::alg::join(initialObjectIDs, ", "), initialObjectIDs);
+ defs->optional(singleShot, "SingleShot", "If true, publishes only one snapshot.");
+
return defs;
}
@@ -172,6 +174,11 @@ namespace armarx
ARMARX_VERBOSE << "Reporting " << poses.size() << " object poses";
objectPoseTopic->reportObjectPoses(getName(), poses);
+ if (singleShot)
+ {
+ break;
+ }
+
cycle.waitForCycleDuration();
}
}
diff --git a/source/RobotAPI/components/ObjectPoseProviderExample/ObjectPoseProviderExample.h b/source/RobotAPI/components/ObjectPoseProviderExample/ObjectPoseProviderExample.h
index 7ed814eb06c219f3280f1d5d4e9473ef56344fb0..8de0dac4589f9ab3333becd9a464a0ef236231f5 100644
--- a/source/RobotAPI/components/ObjectPoseProviderExample/ObjectPoseProviderExample.h
+++ b/source/RobotAPI/components/ObjectPoseProviderExample/ObjectPoseProviderExample.h
@@ -102,6 +102,8 @@ namespace armarx
armarx::ObjectFinder objectFinder;
std::vector<std::string> initialObjectIDs = { "KIT/Amicelli", "KIT/YellowSaltCylinder" };
+ bool singleShot = false;
+
armarx::SimpleRunningTask<>::pointer_type poseEstimationTask;
diff --git a/source/RobotAPI/components/SkillObserver/SkillObserver.h b/source/RobotAPI/components/SkillObserver/SkillObserver.h
index 44afdb0070538c416d06d2a938eef09d6d54693b..54fc0ca89f9d57cba62f2a5ca2a65b452db3d8ca 100644
--- a/source/RobotAPI/components/SkillObserver/SkillObserver.h
+++ b/source/RobotAPI/components/SkillObserver/SkillObserver.h
@@ -22,6 +22,7 @@
#pragma once
+#include <mutex>
#include <thread>
#include <ArmarXCore/core/Component.h>
diff --git a/source/RobotAPI/components/armem/client/ExampleMemoryClient/ExampleMemoryClient.cpp b/source/RobotAPI/components/armem/client/ExampleMemoryClient/ExampleMemoryClient.cpp
index c1ebb7ba455032cf3d48fa32b40081a6a2fcdb3d..8ae42e00bd5385886696dd6ead35feb927665947 100644
--- a/source/RobotAPI/components/armem/client/ExampleMemoryClient/ExampleMemoryClient.cpp
+++ b/source/RobotAPI/components/armem/client/ExampleMemoryClient/ExampleMemoryClient.cpp
@@ -32,6 +32,8 @@
#include <RobotAPI/libraries/armem/server/MemoryRemoteGui.h>
#include <RobotAPI/libraries/armem/client/query/Builder.h>
#include <RobotAPI/libraries/armem/client/query/query_fns.h>
+#include <RobotAPI/libraries/armem/core/error.h>
+#include <RobotAPI/libraries/armem/core/aron_conversions.h>
#include <RobotAPI/libraries/armem/core/workingmemory/ice_conversions.h>
#include <RobotAPI/components/armem/server/ExampleMemory/aron/ExampleData.aron.generated.h>
@@ -39,22 +41,20 @@
namespace armarx
{
- ExampleMemoryClientPropertyDefinitions::ExampleMemoryClientPropertyDefinitions(std::string prefix) :
- armarx::ComponentPropertyDefinitions(prefix)
- {
- }
armarx::PropertyDefinitionsPtr ExampleMemoryClient::createPropertyDefinitions()
{
- armarx::PropertyDefinitionsPtr defs = new ExampleMemoryClientPropertyDefinitions(getConfigIdentifier());
+ armarx::PropertyDefinitionsPtr defs = new ComponentPropertyDefinitions(getConfigIdentifier());
defs->topic(debugObserver);
- defs->optional(memoryName, "mem.MemoryName", "Name of the memory to use.");
+ defs->optional(p.usedMemoryName, "mem.UsedMemoryName", "Name of the memory to use.");
+ defs->optional(p.commitFrequency, "ex.CommitFrequency", "Frequency in which example data is commited.");
return defs;
}
+
std::string ExampleMemoryClient::getDefaultName() const
{
return "ExampleMemoryClient";
@@ -72,27 +72,32 @@ namespace armarx
RemoteGui_startRunningTask();
// Wait for the memory to become available and add it as dependency.
- ARMARX_IMPORTANT << "Waiting for memory '" << memoryName << "' ...";
- auto result = useMemory(memoryName);
- if (!result.success)
+ ARMARX_IMPORTANT << "Waiting for memory '" << p.usedMemoryName << "' ...";
+ try
{
- ARMARX_ERROR << result.errorMessage;
+ memoryReader = memoryNameSystem.useReader(p.usedMemoryName);
+ memoryWriter = memoryNameSystem.useWriter(p.usedMemoryName);
+ }
+ catch (const armem::error::CouldNotResolveMemoryServer& e)
+ {
+ ARMARX_ERROR << e.what();
+ return;
}
// Add a provider segment to commit to.
- providerID = addProviderSegment();
+ exampleProviderID = addProviderSegment();
// Construct the entity ID.
- entityID = providerID.withEntityName("example_entity");
+ exampleEntityID = exampleProviderID.withEntityName("example_entity");
// Subscribe to example_entity updates
// Using a lambda:
- memoryReader.subscribe(entityID, [&](const armem::MemoryID & entityID, const std::vector<armem::MemoryID>& snapshotIDs)
+ memoryNameSystem.subscribe(exampleEntityID, [&](const armem::MemoryID & exampleEntityID, const std::vector<armem::MemoryID>& snapshotIDs)
{
- ARMARX_INFO << "Entity " << entityID << " was updated by " << snapshotIDs.size() << " snapshots.";
+ ARMARX_INFO << "Entity " << exampleEntityID << " was updated by " << snapshotIDs.size() << " snapshots.";
});
// Using a member function:
- memoryReader.subscribe(entityID, this, &ExampleMemoryClient::example_entityUpdated);
+ memoryNameSystem.subscribe(exampleEntityID, this, &ExampleMemoryClient::processExampleEntityUpdate);
task = new RunningTask<ExampleMemoryClient>(this, &ExampleMemoryClient::run);
@@ -117,10 +122,10 @@ namespace armarx
run_started = IceUtil::Time::now();
std::srand(std::time(nullptr));
- armem::MemoryID snapshotID = commitSingleSnapshot(entityID);
+ armem::MemoryID snapshotID = commitSingleSnapshot(exampleEntityID);
if (true)
{
- commitMultipleSnapshots(entityID, 3);
+ commitMultipleSnapshots(exampleEntityID, 3);
}
if (true)
{
@@ -135,15 +140,19 @@ namespace armarx
commitExampleData();
queryExampleData();
}
+ if (true)
+ {
+ commitExamplesWithLinks();
+ }
- CycleUtil c(100);
+ CycleUtil c(static_cast<int>(1000 / p.commitFrequency));
int i = 0;
while (!task->isStopped())
{
commitExampleData();
if (i++ < 10000)
{
- commitSingleSnapshot(entityID);
+ commitSingleSnapshot(exampleEntityID);
}
c.waitForCycleDuration();
}
@@ -163,6 +172,7 @@ namespace armarx
return armem::MemoryID(result.segmentID);
}
+
armem::MemoryID ExampleMemoryClient::commitSingleSnapshot(const armem::MemoryID& entityID)
{
// Prepare the update with some empty instances.
@@ -179,7 +189,7 @@ namespace armarx
auto cos = std::make_shared<aron::datanavigator::FloatNavigator>(std::cos(diff));
auto sqrt = std::make_shared<aron::datanavigator::DoubleNavigator>(std::sqrt(diff));
- auto lin = std::make_shared<aron::datanavigator::LongNavigator>((long)(diff * 1000));
+ auto lin = std::make_shared<aron::datanavigator::LongNavigator>(static_cast<long>(diff * 1000));
auto rand = std::make_shared<aron::datanavigator::IntNavigator>(std::rand());
dict1->addElement("sin", sin);
@@ -206,13 +216,14 @@ namespace armarx
return updateResult.snapshotID;
}
+
void ExampleMemoryClient::commitMultipleSnapshots(const armem::MemoryID& entityID, int num)
{
// Commit a number of updates with different timestamps and number of instances.
armem::Commit commit;
for (int i = 0; i < num; ++i)
{
- armem::EntityUpdate& update = commit.updates.emplace_back();
+ armem::EntityUpdate& update = commit.add();
update.entityID = entityID;
update.timeCreated = armem::Time::now() + armem::Time::seconds(i);
for (int j = 0; j < i; ++j)
@@ -290,6 +301,7 @@ namespace armarx
}
}
+
void ExampleMemoryClient::queryExactSnapshot(const armem::MemoryID& snapshotID)
{
ARMARX_IMPORTANT
@@ -299,11 +311,7 @@ namespace armarx
namespace qf = armem::client::query_fns;
armem::client::query::Builder qb;
- qb
- .coreSegments(qf::withID(snapshotID))
- .providerSegments(qf::withID(snapshotID))
- .entities(qf::withID(snapshotID))
- .snapshots(qf::withID(snapshotID));
+ qb.singleEntitySnapshot(snapshotID);
armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
ARMARX_INFO << qResult;
@@ -335,23 +343,25 @@ namespace armarx
ARMARX_ERROR << addSegmentResult.errorMessage;
return;
}
- armem::MemoryID providerID = armem::MemoryID(addSegmentResult.segmentID);
+ exampleDataProviderID = armem::MemoryID(addSegmentResult.segmentID);
- armem::Time time = armem::Time::now();
+ const armem::Time time = armem::Time::now();
armem::Commit commit;
+
//default
{
- armem::EntityUpdate& update = commit.updates.emplace_back();
- update.entityID = providerID.withEntityName("default");
+ armem::EntityUpdate& update = commit.add();
+ update.entityID = exampleDataProviderID.withEntityName("default");
update.timeCreated = time;
- armem::example::ExampleData primitive;
- update.instancesData = { primitive.toAron() };
+ armem::example::ExampleData data;
+ toAron(data.memoryLink, armem::MemoryID());
+ update.instancesData = { data.toAron() };
}
//the answer
{
- armem::EntityUpdate& update = commit.updates.emplace_back();
- update.entityID = providerID.withEntityName("the answer");
+ armem::EntityUpdate& update = commit.add();
+ update.entityID = exampleDataProviderID.withEntityName("the answer");
update.timeCreated = time;
armem::example::ExampleData data;
@@ -378,7 +388,11 @@ namespace armarx
{ "two", 2 },
{ "three", 3 },
};
+
+ toAron(data.memoryLink, armem::MemoryID());
+
{
+ data.the_ivt_image = std::make_shared<CByteImage>();
CByteImage& image = *data.the_ivt_image;
image.Set(20, 10, CByteImage::ImageType::eRGB24);
simox::ColorMap cmap = simox::color::cmaps::plasma();
@@ -400,24 +414,29 @@ namespace armarx
}
armem::CommitResult commitResult = memoryWriter.commit(commit);
- if (!commitResult.allSuccess())
+ if (commitResult.allSuccess())
+ {
+ armem::fromIce(commitResult.results.at(1).snapshotID, theAnswerSnapshotID);
+ }
+ else
{
ARMARX_WARNING << commitResult.allErrorMessages();
}
}
+
void ExampleMemoryClient::queryExampleData()
{
// Query all entities from provider.
armem::client::query::Builder qb;
qb
- .coreSegments().withID(providerID)
- .providerSegments().withID(providerID)
+ .coreSegments().withID(exampleProviderID)
+ .providerSegments().withID(exampleProviderID)
.entities().all()
.snapshots().all();
armem::client::QueryResult result = memoryReader.query(qb.buildQueryInput());
- if (result)
+ if (result.success)
{
tab.queryResult = std::move(result.memory);
tab.rebuild = true;
@@ -429,8 +448,65 @@ namespace armarx
}
+ void ExampleMemoryClient::commitExamplesWithLinks()
+ {
+ const armem::Time time = armem::Time::now();
+
+ armem::Commit commit;
+ {
+ armem::EntityUpdate& update = commit.add();
+ update.entityID = exampleDataProviderID.withEntityName("link to the_answer");
+ update.timeCreated = time;
+
+ armem::example::ExampleData data;
+ armem::toAron(data.memoryLink, theAnswerSnapshotID);
+
+ update.instancesData = { data.toAron() };
+ }
+ {
+ armem::EntityUpdate& update = commit.add();
+ update.entityID = exampleDataProviderID.withEntityName("link to self");
+ update.timeCreated = time;
+
+ armem::example::ExampleData data;
+ armem::toAron(data.memoryLink, update.entityID.withTimestamp(time));
+
+ update.instancesData = { data.toAron() };
+ }
+
+ {
+ armem::EntityUpdate& update = commit.add();
+ update.entityID = exampleDataProviderID.withEntityName("link to previous snapshot");
+ update.timeCreated = time - armem::Time::seconds(1); // 1 sec in the past
+
+ armem::example::ExampleData data;
+ armem::toAron(data.memoryLink, armem::MemoryID()); // First entry - invalid link
+
+ update.instancesData = { data.toAron() };
+ }
+ {
+ armem::EntityUpdate& update = commit.add();
+ update.entityID = exampleDataProviderID.withEntityName("link to previous snapshot");
+ update.timeCreated = time;
+
+ armem::example::ExampleData data;
+ armem::toAron(data.memoryLink, update.entityID.withTimestamp(time - armem::Time::seconds(1)));
+
+ update.instancesData = { data.toAron() };
+ }
+
+ ARMARX_CHECK_EQUAL(commit.updates.size(), 4);
+ armem::CommitResult commitResult = memoryWriter.commit(commit);
+
+ if (!commitResult.allSuccess() || commitResult.results.size() != commit.updates.size())
+ {
+ ARMARX_WARNING << commitResult.allErrorMessages();
+ }
+ }
+
- void ExampleMemoryClient::example_entityUpdated(const armem::MemoryID& subscriptionID, const std::vector<armem::MemoryID>& snapshotIDs)
+ void ExampleMemoryClient::processExampleEntityUpdate(
+ const armem::MemoryID& subscriptionID, const std::vector<armem::MemoryID>& snapshotIDs)
{
std::stringstream ss;
ss << "example_entity got updated: " << subscriptionID << "\n";
diff --git a/source/RobotAPI/components/armem/client/ExampleMemoryClient/ExampleMemoryClient.h b/source/RobotAPI/components/armem/client/ExampleMemoryClient/ExampleMemoryClient.h
index 774a4cbb84bc06496471e214d1cdb7935af1418f..5680e316a6228021932afea5d73abd02fdcda6d8 100644
--- a/source/RobotAPI/components/armem/client/ExampleMemoryClient/ExampleMemoryClient.h
+++ b/source/RobotAPI/components/armem/client/ExampleMemoryClient/ExampleMemoryClient.h
@@ -39,28 +39,18 @@
namespace armarx
{
- /**
- * @class ExampleClientPropertyDefinitions
- * @brief Property definitions of `ExampleClient`.
- */
- class ExampleMemoryClientPropertyDefinitions :
- public armarx::ComponentPropertyDefinitions
- {
- public:
- ExampleMemoryClientPropertyDefinitions(std::string prefix);
- };
-
/**
* @defgroup Component-ExampleClient ExampleClient
* @ingroup RobotAPI-Components
- * A description of the component ExampleClient.
+ *
+ * An example for an ArMem Memory Client.
*
* @class ExampleClient
* @ingroup Component-ExampleClient
* @brief Brief description of class ExampleClient.
*
- * Detailed description of class ExampleClient.
+ * Connects to the example memory, and commits and queries example data.
*/
class ExampleMemoryClient :
virtual public armarx::Component,
@@ -93,35 +83,48 @@ namespace armarx
private:
- armem::MemoryID providerID;
- armem::MemoryID entityID;
-
// Callback for updates on `example_entity`.
- void example_entityUpdated(const armem::MemoryID& id, const std::vector<armem::MemoryID>& snapshotIDs);
+ void processExampleEntityUpdate(const armem::MemoryID& id, const std::vector<armem::MemoryID>& snapshotIDs);
// Examples
void waitForMemory();
armem::MemoryID addProviderSegment();
- armem::MemoryID commitSingleSnapshot(const armem::MemoryID& entityID);
- void commitMultipleSnapshots(const armem::MemoryID& entityID, int num = 3);
+ armem::MemoryID commitSingleSnapshot(const armem::MemoryID& exampleEntityID);
+ void commitMultipleSnapshots(const armem::MemoryID& exampleEntityID, int num = 3);
- void queryLatestSnapshot(const armem::MemoryID& entityID);
+ void queryLatestSnapshot(const armem::MemoryID& exampleEntityID);
void queryExactSnapshot(const armem::MemoryID& snapshotID);
void commitExampleData();
void queryExampleData();
+ void commitExamplesWithLinks();
+
+
private:
+ struct Properties
+ {
+ std::string usedMemoryName = "Example";
+ float commitFrequency = 10;
+ };
+ Properties p;
+
+
+ armem::MemoryID exampleProviderID;
+ armem::MemoryID exampleEntityID;
+
+ armem::MemoryID exampleDataProviderID;
+ armem::MemoryID theAnswerSnapshotID;
+
+
IceUtil::Time run_started;
armarx::RunningTask<ExampleMemoryClient>::pointer_type task;
armarx::DebugObserverInterfacePrx debugObserver;
- std::string memoryName = "Example";
-
struct RemoteGuiTab : RemoteGui::Client::Tab
{
std::atomic_bool rebuild = false;
diff --git a/source/RobotAPI/components/armem/client/VirtualRobotReaderExampleClient/VirtualRobotReaderExampleClient.cpp b/source/RobotAPI/components/armem/client/VirtualRobotReaderExampleClient/VirtualRobotReaderExampleClient.cpp
index 85ca85f48ca62c7bc686bd9286d52fae5480ebc7..6650e573785cce732f0f2bbe7c128aa34fc58626 100644
--- a/source/RobotAPI/components/armem/client/VirtualRobotReaderExampleClient/VirtualRobotReaderExampleClient.cpp
+++ b/source/RobotAPI/components/armem/client/VirtualRobotReaderExampleClient/VirtualRobotReaderExampleClient.cpp
@@ -31,7 +31,7 @@
namespace armarx::robot_state
{
VirtualRobotReaderExampleClient::VirtualRobotReaderExampleClient() :
- virtualRobotReader(*this) {}
+ virtualRobotReader(this->memoryNameSystem) {}
armarx::PropertyDefinitionsPtr VirtualRobotReaderExampleClient::createPropertyDefinitions()
{
diff --git a/source/RobotAPI/components/armem/server/ExampleMemory/ExampleMemory.cpp b/source/RobotAPI/components/armem/server/ExampleMemory/ExampleMemory.cpp
index 8bd030bb8883e73910b182d7aad66feda0dab96f..3ed4b4612c5dd3f6e0afbd71630dba7a4753f460 100644
--- a/source/RobotAPI/components/armem/server/ExampleMemory/ExampleMemory.cpp
+++ b/source/RobotAPI/components/armem/server/ExampleMemory/ExampleMemory.cpp
@@ -61,9 +61,10 @@ namespace armarx
void ExampleMemory::onInitComponent()
{
workingMemory.name() = p.memoryName;
+ longtermMemory.name() = p.memoryName;
// Usually, the memory server will specify a number of core segments with a specific aron type.
- workingMemory.addCoreSegment("ExampleData", armem::example::ExampleData::toInitialAronType());
+ workingMemory.addCoreSegment("ExampleData", armem::example::ExampleData::toAronType());
// For illustration purposes, we add more segments (without types).
bool trim = true;
diff --git a/source/RobotAPI/components/armem/server/ExampleMemory/aron/ExampleData.xml b/source/RobotAPI/components/armem/server/ExampleMemory/aron/ExampleData.xml
index 7acb4e6d45870a95e90848fbb36479e5b7dee193..edd34d36d5d05c01ce7e40b2f1cbdc9213c2c2f6 100644
--- a/source/RobotAPI/components/armem/server/ExampleMemory/aron/ExampleData.xml
+++ b/source/RobotAPI/components/armem/server/ExampleMemory/aron/ExampleData.xml
@@ -1,12 +1,20 @@
<!--Some fancy comment -->
<?xml version="1.0" encoding="UTF-8" ?>
<AronTypeDefinition>
+
<CodeIncludes>
<Include include="<Eigen/Core>" />
<Include include="<Image/ByteImage.h>" />
+ <Include include="<RobotAPI/libraries/armem/aron/MemoryID.aron.generated.h>" />
</CodeIncludes>
+
+ <AronIncludes>
+ <Include include="<RobotAPI/libraries/armem/aron/MemoryID.xml>" />
+ </AronIncludes>
+
<GenerateTypes>
<Object name='armarx::armem::example::ExampleData'>
+
<ObjectChild key='the_int'>
<Int />
</ObjectChild>
@@ -33,7 +41,7 @@
<EigenMatrix rows="4" cols="4" type="float" />
</ObjectChild>
<ObjectChild key='the_ivt_image'>
- <IVTCByteImage type="GrayScale" />
+ <IVTCByteImage type="GrayScale" shared_ptr="true"/>
</ObjectChild>
<ObjectChild key='the_float_list'>
@@ -79,6 +87,11 @@
</Dict>
</ObjectChild>
+ <ObjectChild key="memoryLink">
+ <armarx::armem::arondto::MemoryID />
+ </ObjectChild>
+
</Object>
</GenerateTypes>
+
</AronTypeDefinition>
diff --git a/source/RobotAPI/components/armem/server/ExampleMemory/test/ExampleMemoryTest.cpp b/source/RobotAPI/components/armem/server/ExampleMemory/test/ExampleMemoryTest.cpp
index b269993f79bd5c5d39c590e1359cb5790fd9b4dc..c5ba35a13aa4ec127f64d0992021d29262d54407 100644
--- a/source/RobotAPI/components/armem/server/ExampleMemory/test/ExampleMemoryTest.cpp
+++ b/source/RobotAPI/components/armem/server/ExampleMemory/test/ExampleMemoryTest.cpp
@@ -39,7 +39,7 @@ namespace armem = armarx::armem;
BOOST_AUTO_TEST_CASE(test_ExampleData_type)
{
- armarx::aron::typenavigator::ObjectNavigatorPtr type = ExampleData::toInitialAronType();
+ armarx::aron::typenavigator::ObjectNavigatorPtr type = ExampleData::toAronType();
BOOST_CHECK_EQUAL(type->childrenSize(), 15);
diff --git a/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp b/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp
index b6d997f6882daa1b9ee95d97fb4502ae181aa9ab..91ea5b23400f568891326f6dd6c79b5e603d229c 100644
--- a/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp
+++ b/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp
@@ -54,9 +54,6 @@ namespace armarx::armem::server::obj
classSegment.defineProperties(defs, prefix + "cls.");
instance::SegmentAdapter::defineProperties(defs, prefix + "inst.");
- articulatedObjectInstanceSegment.defineProperties(defs, prefix + "articulated.inst.");
- articulatedObjectClassSegment.defineProperties(defs, prefix + "articulated.cls.");
-
attachmentSegment.defineProperties(defs, prefix + "attachments.");
return defs;
@@ -68,10 +65,6 @@ namespace armarx::armem::server::obj
server::ComponentPluginUser::workingMemoryMutex),
classSegment(server::ComponentPluginUser::iceMemory,
server::ComponentPluginUser::workingMemoryMutex),
- articulatedObjectClassSegment(server::ComponentPluginUser::iceMemory,
- server::ComponentPluginUser::workingMemoryMutex),
- articulatedObjectInstanceSegment(server::ComponentPluginUser::iceMemory,
- server::ComponentPluginUser::workingMemoryMutex),
attachmentSegment(server::ComponentPluginUser::iceMemory,
server::ComponentPluginUser::workingMemoryMutex)
{
@@ -86,8 +79,6 @@ namespace armarx::armem::server::obj
{
workingMemory.name() = defaultMemoryName;
- instance::SegmentAdapter::init();
-
const auto initSegmentWithCatch = [&](const std::string & segmentName, const auto&& fn)
{
try
@@ -108,27 +99,19 @@ namespace armarx::armem::server::obj
}
};
+ // Class segment needs to be initialized before instance segment,
+ // as the instance segment may refer to and search through the class segment.
initSegmentWithCatch("class", [&]()
{
classSegment.init();
});
- initSegmentWithCatch("articulated object class", [&]()
- {
- articulatedObjectClassSegment.init();
- });
-
- initSegmentWithCatch("articulated object instance", [&]()
- {
- articulatedObjectInstanceSegment.setArticulatedObjectClassSegment(articulatedObjectClassSegment);
- articulatedObjectInstanceSegment.init();
- });
+ instance::SegmentAdapter::init();
initSegmentWithCatch("attachment", [&]()
{
attachmentSegment.init();
});
-
}
void ObjectMemory::onConnectComponent()
@@ -144,6 +127,9 @@ namespace armarx::armem::server::obj
getProxyFromProperty(kinematicUnitObserver, "cmp.KinematicUnitObserverName", false, "", false);
+ // Create first to use the original values.
+ createRemoteGuiTab();
+
instance::SegmentAdapter::connect(
robotStateComponent,
robot,
@@ -155,21 +141,11 @@ namespace armarx::armem::server::obj
ArVizComponentPluginUser::getArvizClient()
);
- articulatedObjectClassSegment.connect(
- ArVizComponentPluginUser::getArvizClient()
- );
-
- articulatedObjectInstanceSegment.connect(
- ArVizComponentPluginUser::getArvizClient()
- );
-
attachmentSegment.connect(
ArVizComponentPluginUser::getArvizClient()
);
- createRemoteGuiTab();
RemoteGui_startRunningTask();
-
}
void ObjectMemory::onDisconnectComponent()
@@ -185,28 +161,34 @@ namespace armarx::armem::server::obj
{
using namespace armarx::RemoteGui::Client;
- tab.instance.setup(*this);
- tab.clazz.setup(classSegment);
+ tab.reset(new RemoteGuiTab);
+
+ tab->instance.setup(*this);
+ tab->clazz.setup(classSegment);
HBoxLayout segments =
{
- tab.instance.group,
- tab.clazz.group
+ tab->instance.group,
+ tab->clazz.group
};
VBoxLayout root =
{
segments,
VSpacer()
};
- RemoteGui_createTab(Component::getName(), root, &tab);
+ RemoteGui_createTab(Component::getName(), root, tab.get());
}
+
void ObjectMemory::RemoteGui_update()
{
- // Non-atomic variables need to be guarded by a mutex if accessed by multiple threads
- tab.instance.update(*this);
- tab.clazz.update(classSegment);
+ tab->instance.update(*this);
+ tab->clazz.update(classSegment);
+
+ if (tab->clazz.data.rebuild)
+ {
+ createRemoteGuiTab();
+ }
}
}
-
diff --git a/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.h b/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.h
index 2dfd668d1e43df914ff7b2f02205e699ad2ea5e9..f29b2cef93efb2aa35a7983f4dce87d3db854b80 100644
--- a/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.h
+++ b/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.h
@@ -111,8 +111,6 @@ namespace armarx::armem::server::obj
clazz::Segment classSegment;
- articulated_object_class::Segment articulatedObjectClassSegment;
- articulated_object_instance::Segment articulatedObjectInstanceSegment;
attachments::Segment attachmentSegment;
// associations::Segment associationsSegment;
@@ -122,7 +120,7 @@ namespace armarx::armem::server::obj
instance::SegmentAdapter::RemoteGui instance;
clazz::Segment::RemoteGui clazz;
};
- RemoteGuiTab tab;
+ std::unique_ptr<RemoteGuiTab> tab;
};
diff --git a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp
index 2a3d01143be737e9afb7ce5c27253e2cb6d4891e..111cced66ba9f678ba852d1005a10ded834d0946 100644
--- a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp
+++ b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp
@@ -49,7 +49,6 @@
#include <RobotAPI/libraries/armem_robot_state/aron/Transform.aron.generated.h>
#include <RobotAPI/libraries/core/FramedPose.h>
#include <RobotAPI/libraries/armem/core/Time.h>
-#include <RobotAPI/libraries/aron/core/navigator/data/primitive/Primitive.h>
#include <RobotAPI/libraries/core/Pose.h>
namespace armarx::armem::server::robot_state
@@ -71,9 +70,15 @@ namespace armarx::armem::server::robot_state
armarx::PropertyDefinitionsPtr RobotStateMemory::createPropertyDefinitions()
{
+ armarx::PropertyDefinitionsPtr defs = new ComponentPropertyDefinitions(getConfigIdentifier());
+
+ const std::string prefix = "mem.";
+
+ workingMemory.name() = "RobotState";
+ defs->optional(workingMemory.name(), prefix + "MemoryName", "Name of this memory server.");
+
const std::string robotUnitPrefix{sensorValuePrefix};
- armarx::PropertyDefinitionsPtr defs = new ComponentPropertyDefinitions(getConfigIdentifier());
defs->optional(robotUnitSensorPrefix, robotUnitPrefix + "SensorValuePrefix", "Prefix of all sensor values");
defs->optional(robotUnitMemoryBatchSize, robotUnitPrefix + "MemoryBatchSize", "The size of the entity snapshot to send to the memory. Min is 1");
defs->optional(robotUnitPollFrequency, robotUnitPrefix + "UpdateFrequency", "The frequency in Hz to store values. All other values get discarded. Min is 1, max is " + std::to_string(ROBOT_UNIT_MAXIMUM_FREQUENCY));
@@ -84,6 +89,8 @@ namespace armarx::armem::server::robot_state
localizationSegment.defineProperties(defs);
commonVisu.defineProperties(defs);
+ setRobotUnitAsOptionalDependency();
+
return defs;
}
@@ -124,12 +131,20 @@ namespace armarx::armem::server::robot_state
ArmarXDataPath::getAbsolutePath(robotUnitConfigPath, robotUnitConfigPath, includePaths);
- workingMemory.name() = workingMemoryName;
+ // workingMemory.name() = workingMemoryName;
}
void RobotStateMemory::onConnectComponent()
{
+
+ if (getRobotUnit())
+ {
+ waitUntilRobotUnitIsRunning();
+ }
+
+ ARMARX_CHECK_NOT_NULL(getRobotUnit()->getKinematicUnit());
+
descriptionSegment.connect(getArvizClient(), getRobotUnit());
proprioceptionSegment.connect(getArvizClient(), getRobotUnit());
diff --git a/source/RobotAPI/components/armem/server/SkillsMemory/SkillsMemory.cpp b/source/RobotAPI/components/armem/server/SkillsMemory/SkillsMemory.cpp
index 8a03cc15970b9da91dd3699ca883c94db13db522..c0a9fd2f33211d9a130f236b9350836b75b51466 100644
--- a/source/RobotAPI/components/armem/server/SkillsMemory/SkillsMemory.cpp
+++ b/source/RobotAPI/components/armem/server/SkillsMemory/SkillsMemory.cpp
@@ -70,8 +70,8 @@ namespace armarx
workingMemory.name() = p.memoryName;
{
- armarx::armem::wm::CoreSegment& c = workingMemory.addCoreSegment(p.statechartCoreSegmentName, armarx::armem::arondto::Statechart::Transition::toInitialAronType());
- c.addProviderSegment("Transitions", armarx::armem::arondto::Statechart::Transition::toInitialAronType());
+ armarx::armem::wm::CoreSegment& c = workingMemory.addCoreSegment(p.statechartCoreSegmentName, armarx::armem::arondto::Statechart::Transition::toAronType());
+ c.addProviderSegment("Transitions", armarx::armem::arondto::Statechart::Transition::toAronType());
}
}
diff --git a/source/RobotAPI/components/units/CMakeLists.txt b/source/RobotAPI/components/units/CMakeLists.txt
index 51d1930b83753c195ff7558154092ea025466e4c..3fa53590170e0ff9d288f7f130adae9814ee7446 100644
--- a/source/RobotAPI/components/units/CMakeLists.txt
+++ b/source/RobotAPI/components/units/CMakeLists.txt
@@ -75,6 +75,7 @@ set(LIB_FILES
armarx_add_library("${LIB_NAME}" "${LIB_FILES}" "${LIB_HEADERS}" "${LIBS}")
add_subdirectory(ObstacleAvoidingPlatformUnit)
+add_subdirectory(ObstacleAwarePlatformUnit)
add_subdirectory(relays)
add_subdirectory(RobotUnit)
diff --git a/source/RobotAPI/components/units/KinematicUnitObserver.cpp b/source/RobotAPI/components/units/KinematicUnitObserver.cpp
index c1796936e47e97e06b6a923a4ca6832bcdf81f40..1dacd798cc3feb30b0fb04b283b0d9ebb3eb2ae4 100644
--- a/source/RobotAPI/components/units/KinematicUnitObserver.cpp
+++ b/source/RobotAPI/components/units/KinematicUnitObserver.cpp
@@ -297,7 +297,7 @@ namespace armarx
if (aValueChanged || newChannel)
{
- boost::unordered_map< ::std::string, ::armarx::VariantBasePtr> map;
+ std::unordered_map< ::std::string, ::armarx::VariantBasePtr> map;
if (timestamp < 0)
{
for (const auto& it : nameValueMap)
diff --git a/source/RobotAPI/components/units/ObstacleAwarePlatformUnit/CMakeLists.txt b/source/RobotAPI/components/units/ObstacleAwarePlatformUnit/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..524b650bf5a3767a92a457ab97b26ffcdd903a40
--- /dev/null
+++ b/source/RobotAPI/components/units/ObstacleAwarePlatformUnit/CMakeLists.txt
@@ -0,0 +1,10 @@
+armarx_add_component(
+ COMPONENT_NAME ObstacleAwarePlatformUnit
+ HEADERS ObstacleAwarePlatformUnit.h
+ SOURCES ObstacleAwarePlatformUnit.cpp
+ COMPONENT_LIBS ArmarXCoreComponentPlugins
+ RobotAPICore
+ RobotAPIComponentPlugins
+ RobotUnit
+)
+armarx_add_component_executable("main.cpp")
diff --git a/source/RobotAPI/components/units/ObstacleAwarePlatformUnit/ObstacleAwarePlatformUnit.cpp b/source/RobotAPI/components/units/ObstacleAwarePlatformUnit/ObstacleAwarePlatformUnit.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..fc0274a7eb932135018cf356ca39105ea70890c6
--- /dev/null
+++ b/source/RobotAPI/components/units/ObstacleAwarePlatformUnit/ObstacleAwarePlatformUnit.cpp
@@ -0,0 +1,750 @@
+/*
+ * 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::ObstacleAwarePlatformUnit
+ * @author Christian R. G. Dreher <c.dreher@kit.edu>
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+
+#include <RobotAPI/components/units/ObstacleAwarePlatformUnit/ObstacleAwarePlatformUnit.h>
+
+
+// STD/STL
+#include <algorithm>
+#include <cmath>
+#include <limits>
+#include <numeric>
+
+// Eigen
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+
+// Simox
+#include <SimoxUtility/math.h>
+
+// ArmarX
+#include <ArmarXCore/observers/variant/Variant.h>
+
+
+namespace
+{
+
+ void
+ invalidate(float& v)
+ {
+ v = std::numeric_limits<float>::infinity();
+ }
+
+
+ void
+ invalidate(Eigen::Vector2f& v)
+ {
+ v.x() = std::numeric_limits<float>::infinity();
+ v.y() = std::numeric_limits<float>::infinity();
+ }
+
+
+ void
+ invalidate(Eigen::Vector3f& v)
+ {
+ v.x() = std::numeric_limits<float>::infinity();
+ v.y() = std::numeric_limits<float>::infinity();
+ v.z() = std::numeric_limits<float>::infinity();
+ }
+
+ template<typename T>
+ void invalidate(std::deque<T>& d)
+ {
+ d.clear();
+ }
+
+
+ std::string
+ to_string(armarx::ObstacleAwarePlatformUnit::control_mode mode)
+ {
+ switch (mode)
+ {
+ case armarx::ObstacleAwarePlatformUnit::control_mode::position:
+ return "position";
+ case armarx::ObstacleAwarePlatformUnit::control_mode::velocity:
+ return "velocity";
+ case armarx::ObstacleAwarePlatformUnit::control_mode::none:
+ default:
+ return "unset";
+ }
+ }
+
+}
+
+
+const std::string
+armarx::ObstacleAwarePlatformUnit::default_name = "ObstacleAwarePlatformUnit";
+
+
+armarx::ObstacleAwarePlatformUnit::ObstacleAwarePlatformUnit() = default;
+
+
+armarx::ObstacleAwarePlatformUnit::~ObstacleAwarePlatformUnit() = default;
+
+
+void
+armarx::ObstacleAwarePlatformUnit::onInitPlatformUnit()
+{
+ ARMARX_DEBUG << "Initializing " << getName() << ".";
+
+ ARMARX_DEBUG << "Initialized " << getName() << ".";
+}
+
+
+void
+armarx::ObstacleAwarePlatformUnit::onStartPlatformUnit()
+{
+ ARMARX_DEBUG << "Starting " << getName() << ".";
+
+ if (!hasRobot("robot"))
+ {
+ m_robot = addRobot("robot", VirtualRobot::RobotIO::eStructure);
+ }
+
+ invalidate(m_control_data.target_vel);
+ invalidate(m_control_data.target_rot_vel);
+ invalidate(m_control_data.target_pos);
+ invalidate(m_control_data.target_ori);
+ invalidate(m_viz.start);
+
+ ARMARX_DEBUG << "Started " << getName() << ".";
+}
+
+
+void
+armarx::ObstacleAwarePlatformUnit::onExitPlatformUnit()
+{
+ ARMARX_DEBUG << "Exiting " << getName() << ".";
+
+ schedule_high_level_control_loop(control_mode::none);
+
+ ARMARX_DEBUG << "Exited " << getName() << ".";
+}
+
+
+std::string
+armarx::ObstacleAwarePlatformUnit::getDefaultName()
+const
+{
+ return default_name;
+}
+
+
+void
+armarx::ObstacleAwarePlatformUnit::moveTo(
+ const float target_pos_x,
+ const float target_pos_y,
+ const float target_ori,
+ const float pos_reached_threshold,
+ const float ori_reached_threshold,
+ const Ice::Current&)
+{
+ using namespace simox::math;
+
+ std::scoped_lock l{m_control_data.mutex};
+
+ m_control_data.target_pos = Eigen::Vector2f{target_pos_x, target_pos_y};
+ m_control_data.target_ori = periodic_clamp<float>(target_ori, -M_PI, M_PI);
+ m_control_data.pos_reached_threshold = pos_reached_threshold;
+ m_control_data.ori_reached_threshold = ori_reached_threshold;
+
+ invalidate(m_control_data.target_vel);
+ invalidate(m_control_data.target_rot_vel);
+
+ m_rot_pid_controller.reset();
+
+ schedule_high_level_control_loop(control_mode::position);
+}
+
+
+void
+armarx::ObstacleAwarePlatformUnit::move(
+ const float target_vel_x,
+ const float target_vel_y,
+ const float target_rot_vel,
+ const Ice::Current&)
+{
+ using namespace simox::math;
+
+ std::scoped_lock l{m_control_data.mutex};
+
+ m_control_data.target_vel = Eigen::Vector2f{target_vel_x, target_vel_y};
+ m_control_data.target_rot_vel = periodic_clamp<float>(target_rot_vel, -M_PI, M_PI);
+
+ invalidate(m_control_data.target_pos);
+ invalidate(m_control_data.target_ori);
+
+ ARMARX_CHECK(m_control_data.target_vel.allFinite());
+ ARMARX_CHECK(std::isfinite(m_control_data.target_rot_vel));
+
+ schedule_high_level_control_loop(control_mode::velocity);
+}
+
+
+void
+armarx::ObstacleAwarePlatformUnit::moveRelative(
+ const float target_pos_delta_x,
+ const float target_pos_delta_y,
+ const float target_delta_ori,
+ const float pos_reached_threshold,
+ const float ori_reached_threshold,
+ const Ice::Current& current)
+{
+ using namespace simox::math;
+
+ // Transform relative to global.
+ std::unique_lock lock{m_control_data.mutex};
+ synchronizeLocalClone(m_robot);
+ const Eigen::Vector2f agent_pos = m_robot->getGlobalPosition().head<2>();
+ const float agent_ori = mat4f_to_rpy(m_robot->getGlobalPose()).z();
+ lock.unlock();
+
+ // Use moveTo.
+ moveTo(
+ agent_pos.x() + target_pos_delta_x,
+ agent_pos.y() + target_pos_delta_y,
+ agent_ori + target_delta_ori,
+ pos_reached_threshold,
+ ori_reached_threshold,
+ current);
+}
+
+
+void
+armarx::ObstacleAwarePlatformUnit::setMaxVelocities(
+ float max_pos_vel,
+ float max_rot_vel,
+ const Ice::Current&)
+{
+ // ensure positiveness
+ max_pos_vel = std::max(max_pos_vel, 0.F);
+ max_rot_vel = std::max(max_rot_vel, 0.F);
+
+ std::scoped_lock l{m_control_data.mutex};
+ m_control_data.max_vel = max_pos_vel;
+ m_control_data.max_rot_vel = max_rot_vel;
+ m_platform->setMaxVelocities(max_pos_vel, max_rot_vel);
+}
+
+
+void
+armarx::ObstacleAwarePlatformUnit::stopPlatform(const Ice::Current&)
+{
+ schedule_high_level_control_loop(control_mode::none);
+ m_platform->stopPlatform();
+}
+
+
+void
+armarx::ObstacleAwarePlatformUnit::schedule_high_level_control_loop(control_mode mode)
+{
+ std::scoped_lock l{m_control_loop.mutex};
+
+ // If the control mode didn't change, there's nothing to do.
+ if (m_control_loop.mode == mode)
+ {
+ return;
+ }
+
+ // If a control loop is runnung, stop it and wait for termination.
+ if (m_control_loop.mode != mode and m_control_loop.task)
+ {
+ ARMARX_VERBOSE << "Stopping " << ::to_string(m_control_loop.mode) << " control.";
+ const bool join = true;
+ m_control_loop.task->stop(join);
+ }
+
+ // If the new control mode is none, no new control loop needs to be created.
+ if (mode == control_mode::none)
+ {
+ ARMARX_VERBOSE << "Going into stand-by.";
+ return;
+ }
+
+ // Start next control loop.
+ ARMARX_VERBOSE << "Starting " << ::to_string(mode) << " control.";
+ m_control_loop.mode = mode;
+ m_control_loop.task = new RunningTask<ObstacleAwarePlatformUnit>(
+ this,
+ &ObstacleAwarePlatformUnit::high_level_control_loop);
+ m_control_loop.task->start();
+}
+
+
+void
+armarx::ObstacleAwarePlatformUnit::high_level_control_loop()
+{
+ const control_mode mode = m_control_loop.mode;
+ ARMARX_VERBOSE << "Started " << ::to_string(mode) << " control.";
+
+ try
+ {
+ CycleUtil cu{m_control_loop.cycle_time};
+ while (not m_control_loop.task->isStopped())
+ {
+ const velocities vels = get_velocities();
+
+ // Debug.
+ StringVariantBaseMap m;
+ m["err_dist"] = new Variant{vels.err_dist};
+ m["err_angular_dist"] = new Variant{vels.err_angular_dist};
+
+ m["target_global_x"] = new Variant{vels.target_global.x()};
+ m["target_global_y"] = new Variant{vels.target_global.y()};
+ m["target_global_abs"] = new Variant{vels.target_global.norm()};
+
+ m["target_local_x"] = new Variant{vels.target_local.x()};
+ m["target_local_y"] = new Variant{vels.target_local.y()};
+ m["target_local_abs"] = new Variant(vels.target_local.norm());
+ m["target_rot"] = new Variant{vels.target_rot};
+
+ m["modulated_global_x"] = new Variant{vels.modulated_global.x()};
+ m["modulated_global_y"] = new Variant{vels.modulated_global.y()};
+ m["modulated_global_abs"] = new Variant{vels.modulated_global.norm()};
+
+ m["modulated_local_x"] = new Variant{vels.modulated_local.x()};
+ m["modulated_local_y"] = new Variant{vels.modulated_local.y()};
+ m["modulated_local_abs"] = new Variant{vels.modulated_local.norm()};
+
+// m["max_rot_vel"] = new Variant{m_control_data.max_rot_vel};
+// m["max_lin_vel"] = new Variant{m_control_data.max_vel};
+
+// m["rot_desired"] = new Variant{m_control_data.target_ori};
+// m["rot_measured"] = new Variant{m_control_data.agent_ori};
+
+// m["pos_x_desired"] = new Variant{m_control_data.target_pos.x()};
+// m["pos_x_measured"] = new Variant{m_control_data.agent_pos.y()};
+
+// m["pos_y_desired"] = new Variant{m_control_data.target_pos.x()};
+// m["pos_y_measured"] = new Variant{m_control_data.agent_pos.y()};
+
+ setDebugObserverChannel("ObstacleAwarePlatformCtrl", m);
+
+ m_platform->move(vels.modulated_local.x(), vels.modulated_local.y(), vels.target_rot);
+ visualize(vels);
+ cu.waitForCycleDuration();
+ }
+ }
+ catch (const std::exception& e)
+ {
+ ARMARX_ERROR << "Error occured while running control loop.\n"
+ << e.what();
+ }
+ catch (...)
+ {
+ ARMARX_ERROR << "Unknown error occured while running control loop.";
+ }
+
+ m_platform->move(0, 0, 0);
+ m_platform->stopPlatform();
+ m_control_loop.mode = control_mode::none;
+
+ visualize();
+
+ ARMARX_VERBOSE << "Stopped " << ::to_string(mode) << " control.";
+}
+
+
+armarx::ObstacleAwarePlatformUnit::velocities
+armarx::ObstacleAwarePlatformUnit::get_velocities()
+{
+ using namespace simox::math;
+
+ // Acquire control_data mutex to ensure data remains consistent.
+ std::scoped_lock l{m_control_data.mutex};
+ // Update agent and get target velocities.
+ update_agent_dependent_values();
+ const Eigen::Vector2f target_vel = get_target_velocity();
+ const float target_rot_vel = get_target_rotational_velocity();
+
+ const Eigen::Vector2f extents = Eigen::Affine3f(m_robot->getRobotNode("PlatformCornerFrontLeft")->getPoseInRootFrame()).translation().head<2>();
+
+ // Apply obstacle avoidance and apply post processing to get the modulated velocity.
+ const Eigen::Vector2f modulated_vel = [this, &target_vel, &extents]
+ {
+ const VirtualRobot::Circle circle = VirtualRobot::projectedBoundingCircle(*m_robot);
+ ARMARX_DEBUG << "Circle approximation: " << circle.radius;
+ m_viz.boundingCircle = circle;
+ const float distance = m_obsman->distanceToObstacle(m_control_data.agent_pos /*circle.center*/, circle.radius, m_control_data.target_pos);
+
+ ARMARX_DEBUG << "Distance to obstacle: " << distance;
+
+ // https://www.wolframalpha.com/input/?i=line+through+%281000%2C+0%29+and+%282000%2C+1%29
+ float modifier = std::clamp((distance / 1000) - 1., 0.0, 1.0);
+
+ const Eigen::Vector2f vel = modifier * target_vel;
+ return vel.norm() > 20 ? vel : Eigen::Vector2f{0, 0};
+ }();
+
+ ARMARX_CHECK(modulated_vel.allFinite()) << "Velocity contains non-finite values.";
+ ARMARX_CHECK(std::isfinite(target_rot_vel)) << "Rotation velocity is non-finite.";
+
+ ARMARX_DEBUG << "Target velocity: " << target_vel.transpose()
+ << "; norm: " << target_vel.norm() << "; " << target_rot_vel;
+ ARMARX_DEBUG << "Modulated velocity: " << modulated_vel.transpose() << modulated_vel.norm();
+
+ const auto r = Eigen::Rotation2D(m_control_data.agent_ori).toRotationMatrix().inverse();
+
+ velocities vels;
+ vels.target_local = r * target_vel;
+ vels.target_global = target_vel;
+ vels.modulated_local = r * modulated_vel;
+ vels.modulated_global = modulated_vel;
+ vels.target_rot = target_rot_vel;
+ vels.err_dist = m_control_data.target_dist;
+ vels.err_angular_dist = m_control_data.target_angular_dist;
+
+ return vels;
+}
+
+
+Eigen::Vector2f
+armarx::ObstacleAwarePlatformUnit::get_target_velocity()
+const
+{
+ using namespace simox::math;
+
+ Eigen::Vector2f uncapped_target_vel = Eigen::Vector2f::Zero();
+
+ if (m_control_loop.mode == control_mode::position /*and not target_position_reached()*/)
+ {
+ uncapped_target_vel =
+ (m_control_data.target_pos - m_control_data.agent_pos) * m_control_data.kp;
+ }
+ else if (m_control_loop.mode == control_mode::velocity)
+ {
+ uncapped_target_vel = m_control_data.target_vel;
+ }
+
+ ARMARX_CHECK(uncapped_target_vel.allFinite());
+
+ return norm_max(uncapped_target_vel, m_control_data.max_vel);
+}
+
+
+float
+armarx::ObstacleAwarePlatformUnit::get_target_rotational_velocity()
+const
+{
+ using namespace simox::math;
+
+ float uncapped_target_rot_vel = 0;
+
+ if (m_control_loop.mode == control_mode::position /*and not target_orientation_reached()*/)
+ {
+ m_rot_pid_controller.update(m_control_data.agent_ori, m_control_data.target_ori);
+ uncapped_target_rot_vel = m_rot_pid_controller.getControlValue();
+ }
+ else if (m_control_loop.mode == control_mode::velocity)
+ {
+ uncapped_target_rot_vel = m_control_data.target_rot_vel;
+ }
+
+ ARMARX_CHECK(std::isfinite(uncapped_target_rot_vel));
+
+ return std::clamp(uncapped_target_rot_vel, -m_control_data.max_rot_vel, m_control_data.max_rot_vel);
+
+ // return std::copysign(std::min(std::fabs(uncapped_target_rot_vel), m_control_data.max_rot_vel),
+ // uncapped_target_rot_vel);
+}
+
+
+void
+armarx::ObstacleAwarePlatformUnit::update_agent_dependent_values()
+{
+ using namespace simox::math;
+
+ synchronizeLocalClone(m_robot);
+ m_control_data.agent_pos = m_robot->getGlobalPosition().head<2>();
+ m_control_data.agent_ori =
+ periodic_clamp<float>(mat4f_to_rpy(m_robot->getGlobalPose()).z(), -M_PI, M_PI);
+
+ ARMARX_CHECK_GREATER(m_control_data.agent_ori, -M_PI);
+ ARMARX_CHECK_LESS(m_control_data.agent_ori, M_PI);
+
+ // Update distances in position mode.
+ if (m_control_loop.mode == control_mode::position)
+ {
+ ARMARX_CHECK_GREATER(m_control_data.target_ori, -M_PI);
+ ARMARX_CHECK_LESS(m_control_data.target_ori, M_PI);
+ ARMARX_CHECK(m_control_data.target_pos.allFinite());
+ ARMARX_CHECK(std::isfinite(m_control_data.target_ori));
+
+ m_control_data.target_dist =
+ (m_control_data.target_pos - m_control_data.agent_pos).norm();
+ m_control_data.target_angular_dist =
+ periodic_clamp<float>(m_control_data.target_ori - m_control_data.agent_ori,
+ -M_PI, M_PI);
+
+ ARMARX_CHECK_GREATER(m_control_data.target_angular_dist, -M_PI);
+ ARMARX_CHECK_LESS(m_control_data.target_angular_dist, M_PI);
+
+ ARMARX_DEBUG << "Distance to target: " << m_control_data.target_dist << " mm and "
+ << m_control_data.target_angular_dist << " rad.";
+ }
+ // Otherwise invalidate them.
+ else
+ {
+ invalidate(m_control_data.target_dist);
+ invalidate(m_control_data.target_angular_dist);
+ }
+}
+
+
+bool
+armarx::ObstacleAwarePlatformUnit::target_position_reached()
+const
+{
+ if (m_control_loop.mode == control_mode::position)
+ {
+ return m_control_data.target_dist < m_control_data.pos_reached_threshold;
+ }
+
+ // Cannot reach any target in non-position mode.
+ return false;
+}
+
+
+bool
+armarx::ObstacleAwarePlatformUnit::target_orientation_reached()
+const
+{
+ if (m_control_loop.mode == control_mode::position)
+ {
+ return std::fabs(m_control_data.target_angular_dist) < m_control_data.ori_reached_threshold;
+ }
+
+ // Cannot reach any target in non-position mode.
+ return false;
+}
+
+
+bool
+armarx::ObstacleAwarePlatformUnit::target_reached()
+const
+{
+ if (m_control_loop.mode == control_mode::position)
+ {
+ return target_position_reached() and target_orientation_reached();
+ }
+
+ return false;
+}
+
+
+bool
+armarx::ObstacleAwarePlatformUnit::is_near_target(const Eigen::Vector2f& control_velocity)
+const noexcept
+{
+ if (m_control_data.target_dist < m_control_data.pos_near_threshold)
+ {
+ const Eigen::Vector2f target_direction = m_control_data.target_pos - m_control_data.agent_pos;
+ const Eigen::Vector2f control_direction = control_velocity / control_velocity.norm();
+
+ const float sim = simox::math::cosine_similarity(target_direction, control_direction);
+
+ // if almost pointing into same direction
+ if (sim > cos(M_PI_4f32))
+ {
+ return true;
+ }
+ }
+
+ return false;
+}
+
+
+void
+armarx::ObstacleAwarePlatformUnit::visualize()
+{
+ const Eigen::Vector2f zero = Eigen::Vector2f::Zero();
+ velocities vels;
+ vels.target_local = zero;
+ vels.target_global = zero;
+ vels.modulated_local = zero;
+ vels.modulated_global = zero;
+ vels.target_rot = 0;
+
+ visualize(vels);
+}
+
+
+void
+armarx::ObstacleAwarePlatformUnit::visualize(const velocities& vels)
+{
+ using namespace armarx::viz;
+
+ if (not m_viz.enabled)
+ {
+ return;
+ }
+
+ Eigen::Vector3f agent_pos{m_control_data.agent_pos.x(), m_control_data.agent_pos.y(), 0};
+
+ // Progress. Only draw in position control mode.
+ Layer l_prog = arviz.layer("progress");
+ if (m_control_loop.mode == control_mode::position)
+ {
+ const float min_keypoint_dist = 50;
+
+ {
+ l_prog.add(Cylinder{"boundingCylinder"}
+ .position(agent_pos)
+ .color(Color::cyan(255, 64))
+ .pose(simox::math::pos_rpy_to_mat4f(agent_pos, -M_PI_2, 0, 0) * Eigen::Isometry3f(Eigen::Translation3f(0, -1000, 0)).matrix())
+ .radius(m_viz.boundingCircle.radius)
+ .height(2000));
+ }
+
+ // If no start is set, use current agent pos.
+ if (not m_viz.start.allFinite())
+ {
+ m_viz.start = agent_pos;
+ }
+
+ const Eigen::Vector3f& last_keypoint_pos =
+ m_viz.path.size() >= 1 ? m_viz.path.back() : m_viz.start;
+
+ // Keep a certain distance between path keypoints.
+ if ((last_keypoint_pos - agent_pos).norm() > min_keypoint_dist)
+ {
+ m_viz.path.push_back(agent_pos);
+ }
+
+ // Draw path.
+ if (not target_reached())
+ {
+ // Start.
+ l_prog.add(Sphere{"start"}
+ .position(m_viz.start)
+ .color(Color::cyan(255, 64))
+ .radius(40));
+
+ // Path.
+ for (unsigned i = 0; i < m_viz.path.size(); ++i)
+ {
+ l_prog.add(Sphere{"path_" + std::to_string(i + 1)}
+ .position(m_viz.path[i])
+ .color(Color::magenta(255, 128))
+ .radius(20));
+ }
+
+ // Goal.
+ const Eigen::Vector3f target{m_control_data.target_pos.x(),
+ m_control_data.target_pos.y(),
+ 0};
+ l_prog.add(Arrow{"goal"}
+ .fromTo(target + Eigen::Vector3f{0, 0, 220},
+ target + Eigen::Vector3f{0, 0, 40})
+ .color(Color::red(255, 128))
+ .width(20));
+ }
+ else
+ {
+ m_viz.path.clear();
+ invalidate(m_viz.start);
+ }
+ }
+
+ // Velocities.
+ Layer l_vels = arviz.layer("velocities");
+ if (m_control_loop.mode != control_mode::none)
+ {
+ const float min_velocity = 15;
+ const Eigen::Vector3f from1{agent_pos + Eigen::Vector3f{0, 0, 2000}};
+ const Eigen::Vector3f from2 = from1 + Eigen::Vector3f{0, 0, 200};
+ const Eigen::Vector3f original{vels.target_global.x(), vels.target_global.y(), 0};
+ const Eigen::Vector3f modulated{vels.modulated_global.x(), vels.modulated_global.y(), 0};
+
+ if (original.norm() > min_velocity)
+ {
+ l_vels.add(Arrow{"original"}
+ .fromTo(from1, from1 + original * 5)
+ .color(Color::green(255, 128))
+ .width(25));
+ }
+
+ if (modulated.norm() > min_velocity)
+ {
+ l_vels.add(Arrow{"modulated"}
+ .fromTo(from2, from2 + modulated * 5)
+ .color(Color::cyan(255, 128))
+ .width(25));
+ }
+
+ // TODO rotation arrow
+ }
+
+ // Agent.
+ Layer l_agnt = arviz.layer("agent");
+ if (m_control_loop.mode != control_mode::none)
+ {
+ l_agnt.add(Sphere{"agent"}
+ .position(agent_pos)
+ .color(Color::red(255, 128))
+ .radius(50));
+
+ // Agent safety margin.
+ if (m_control_data.agent_safety_margin > 0)
+ {
+ l_agnt.add(Cylinder{"agent_safety_margin"}
+ .pose(simox::math::pos_rpy_to_mat4f(agent_pos, -M_PI_2, 0, 0))
+ .color(Color::red(255, 64))
+ .radius(m_control_data.agent_safety_margin)
+ .height(2));
+ }
+ }
+
+ arviz.commit({l_prog, l_vels, l_agnt});
+}
+
+
+armarx::PropertyDefinitionsPtr
+armarx::ObstacleAwarePlatformUnit::createPropertyDefinitions()
+{
+ PropertyDefinitionsPtr def = PlatformUnit::createPropertyDefinitions();
+
+ def->component(m_platform, "Platform");
+ def->component(m_obsman, "ObstacleAvoidingPlatformUnit");
+
+ // Settings.
+ def->optional(m_control_data.min_vel_near_target, "min_vel_near_target", "Velocity in [mm/s] "
+ "the robot should at least set when near the target");
+ def->optional(m_control_data.min_vel_general, "min_vel_general", "Velocity in [mm/s] the robot "
+ "should at least set on general");
+ def->optional(m_control_data.pos_near_threshold, "pos_near_threshold", "Distance in [mm] after "
+ "which the robot is considered to be near the target for min velocity, "
+ "smoothing, etc.");
+
+ // Control parameters.
+ def->optional(m_control_data.kp, "control.pos.kp");
+ def->optional(m_rot_pid_controller.Kp, "control.rot.kp");
+ def->optional(m_rot_pid_controller.Ki, "control.rot.ki");
+ def->optional(m_rot_pid_controller.Kd, "control.rot.kd");
+ def->optional(m_control_loop.cycle_time, "control.pose.cycle_time", "Control loop cycle time.");
+
+ // Obstacle avoidance parameter.
+ def->optional(m_control_data.agent_safety_margin, "doa.agent_safety_margin");
+
+ return def;
+}
diff --git a/source/RobotAPI/components/units/ObstacleAwarePlatformUnit/ObstacleAwarePlatformUnit.h b/source/RobotAPI/components/units/ObstacleAwarePlatformUnit/ObstacleAwarePlatformUnit.h
new file mode 100644
index 0000000000000000000000000000000000000000..1bc78b92b4ba4edd0811d7324d2a2f811aa2ee15
--- /dev/null
+++ b/source/RobotAPI/components/units/ObstacleAwarePlatformUnit/ObstacleAwarePlatformUnit.h
@@ -0,0 +1,259 @@
+/*
+ * 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::ObstacleAwarePlatformUnit
+ * @author Christian R. G. Dreher <c.dreher@kit.edu>
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+
+#pragma once
+
+
+// STD/STL
+#include <deque>
+#include <string>
+#include <tuple>
+#include <mutex>
+#include <vector>
+
+// Eigen
+#include <Eigen/Core>
+
+// Ice
+#include <IceUtil/Time.h>
+
+// Simox
+#include <VirtualRobot/Nodes/RobotNode.h>
+#include <VirtualRobot/Safety.h>
+
+// ArmarX
+#include <ArmarXCore/libraries/ArmarXCoreComponentPlugins/DebugObserverComponentPlugin.h>
+#include <ArmarXCore/util/tasks.h>
+#include <ArmarXCore/util/time.h>
+
+// RobotAPI
+#include <RobotAPI/components/units/PlatformUnit.h>
+#include <RobotAPI/interface/components/ObstacleAvoidance/DynamicObstacleManagerInterface.h>
+#include <RobotAPI/libraries/core/PIDController.h>
+#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
+#include <RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.h>
+
+
+namespace armarx
+{
+
+ class ObstacleAwarePlatformUnit :
+ virtual public PlatformUnit,
+ virtual public RobotStateComponentPluginUser,
+ virtual public ArVizComponentPluginUser,
+ virtual public DebugObserverComponentPluginUser
+ {
+
+ public:
+
+ enum class control_mode
+ {
+ position,
+ velocity,
+ none
+ };
+
+ private:
+
+ struct control_loop
+ {
+ std::mutex mutex;
+ control_mode mode = control_mode::none;
+ RunningTask<ObstacleAwarePlatformUnit>::pointer_type task;
+ IceUtil::Time cycle_time = IceUtil::Time::milliSeconds(10);
+ };
+
+ struct control_data
+ {
+ std::mutex mutex;
+ Eigen::Vector2f target_pos;
+ float target_ori;
+ Eigen::Vector2f target_vel;
+ float target_rot_vel;
+ float target_dist;
+ float target_angular_dist;
+ Eigen::Vector2f agent_pos;
+ float agent_ori;
+ double agent_safety_margin = 0;
+ float min_vel_near_target = 50;
+ float min_vel_general = 100;
+ float max_vel = 200;
+ float max_rot_vel = 0.3;
+ float pos_near_threshold = 250;
+ float pos_reached_threshold = 8;
+ float ori_reached_threshold = 0.1;
+ float kp = 3.5;
+ };
+
+ struct visualization
+ {
+ Eigen::Vector3f start;
+ std::vector<Eigen::Vector3f> path;
+ bool enabled = true;
+ VirtualRobot::Circle boundingCircle;
+ };
+
+ struct velocities
+ {
+ Eigen::Vector2f target_local;
+ Eigen::Vector2f modulated_local;
+ Eigen::Vector2f target_global;
+ Eigen::Vector2f modulated_global;
+ float target_rot;
+ float err_dist;
+ float err_angular_dist;
+ };
+
+ public:
+
+ ObstacleAwarePlatformUnit();
+
+ ~ObstacleAwarePlatformUnit()
+ override;
+
+ std::string
+ getDefaultName()
+ const override;
+
+ void
+ moveTo(
+ float target_pos_x,
+ float target_pos_y,
+ float target_ori,
+ float pos_reached_threshold,
+ float ori_reached_threshold,
+ const Ice::Current& = Ice::Current{})
+ override;
+
+ void
+ move(
+ float target_vel_x,
+ float target_vel_y,
+ float target_rot_vel,
+ const Ice::Current& = Ice::Current{})
+ override;
+
+ void
+ moveRelative(
+ float target_pos_delta_x,
+ float target_pos_delta_y,
+ float target_delta_ori,
+ float pos_reached_threshold,
+ float ori_reached_threshold,
+ const Ice::Current& = Ice::Current{})
+ override;
+
+ void
+ setMaxVelocities(
+ float max_pos_vel,
+ float max_rot_vel,
+ const Ice::Current& = Ice::Current{})
+ override;
+
+ void
+ stopPlatform(const Ice::Current& = Ice::Current{})
+ override;
+
+ protected:
+
+ void
+ onInitPlatformUnit()
+ override;
+
+ void
+ onStartPlatformUnit()
+ override;
+
+ void
+ onExitPlatformUnit()
+ override;
+
+ PropertyDefinitionsPtr
+ createPropertyDefinitions()
+ override;
+
+ private:
+
+ void
+ schedule_high_level_control_loop(control_mode mode);
+
+ void
+ high_level_control_loop();
+
+ velocities
+ get_velocities();
+
+ void
+ update_agent_dependent_values();
+
+ Eigen::Vector2f
+ get_target_velocity()
+ const;
+
+ float
+ get_target_rotational_velocity()
+ const;
+
+ bool
+ target_position_reached()
+ const;
+
+ bool
+ target_orientation_reached()
+ const;
+
+ bool
+ target_reached()
+ const;
+
+ void
+ visualize();
+
+ void
+ visualize(const velocities& vels);
+
+ bool
+ is_near_target(const Eigen::Vector2f& control_velocity)
+ const
+ noexcept;
+
+ public:
+
+ static const std::string default_name;
+
+ private:
+
+ PlatformUnitInterfacePrx m_platform;
+ VirtualRobot::RobotPtr m_robot;
+ DynamicObstacleManagerInterfacePrx m_obsman;
+
+ ObstacleAwarePlatformUnit::control_loop m_control_loop;
+ ObstacleAwarePlatformUnit::control_data m_control_data;
+
+ mutable PIDController m_rot_pid_controller{1.0, 0.00009, 0.0, std::numeric_limits<double>::max(), std::numeric_limits<double>::max(), true};
+
+ visualization m_viz;
+
+ };
+
+}
diff --git a/source/RobotAPI/components/units/ObstacleAwarePlatformUnit/main.cpp b/source/RobotAPI/components/units/ObstacleAwarePlatformUnit/main.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..f168208f6734ea228aa848dcf0c5d0976370bae7
--- /dev/null
+++ b/source/RobotAPI/components/units/ObstacleAwarePlatformUnit/main.cpp
@@ -0,0 +1,40 @@
+/*
+ * 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::ObstacleAwarePlatformUnit
+ * @author Christian R. G. Dreher <c.dreher@kit.edu>
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+
+// STD/STL
+#include <string>
+
+// ArmarX
+#include <ArmarXCore/core/application/Application.h>
+#include <ArmarXCore/core/Component.h>
+
+// RobotAPI
+#include <RobotAPI/components/units/ObstacleAwarePlatformUnit/ObstacleAwarePlatformUnit.h>
+
+
+int main(int argc, char* argv[])
+{
+ using namespace armarx;
+ const std::string name = ObstacleAwarePlatformUnit::default_name;
+ return runSimpleComponentApp<ObstacleAwarePlatformUnit>(argc, argv, name);
+}
diff --git a/source/RobotAPI/components/units/RobotUnit/BasicControllers.cpp b/source/RobotAPI/components/units/RobotUnit/BasicControllers.cpp
index 0ec81ab3895bc1037141ee6798c01effcbee45eb..e106a9dbb943dabd9bd23a05e9d8320b6e415b82 100644
--- a/source/RobotAPI/components/units/RobotUnit/BasicControllers.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/BasicControllers.cpp
@@ -1419,7 +1419,7 @@ namespace armarx
auto min_jerk = [&](double t, double tf, double s0, double v0, double a0, double xf = 0, double t0 = 0.0)
{
double D = tf - t0;
- BOOST_ASSERT(D != 0.0);
+ ARMARX_CHECK(D != 0.0);
double D2 = D * D;
double tau = (t - t0) / D;
double b0 = s0;
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.cpp
index bc246422b98fbd89feac994a2ace52645532e22a..77f9c8d8b49f9f192eb63ab64fe9d5b308b9e22e 100755
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.cpp
@@ -23,6 +23,10 @@
*/
#include "NJointHolonomicPlatformGlobalPositionController.h"
+#include <cmath>
+
+#include <SimoxUtility/math/periodic/periodic_clamp.h>
+
namespace armarx
{
NJointControllerRegistration<NJointHolonomicPlatformGlobalPositionController>
@@ -34,7 +38,7 @@ namespace armarx
const NJointHolonomicPlatformGlobalPositionControllerConfigPtr& cfg,
const VirtualRobot::RobotPtr&) :
pid(cfg->p, cfg->i, cfg->d, cfg->maxVelocity, cfg->maxAcceleration),
- opid(cfg->p_rot, cfg->i_rot, cfg->d_rot, cfg->maxRotationVelocity, cfg->maxRotationAcceleration)
+ opid(cfg->p_rot, cfg->i_rot, cfg->d_rot, cfg->maxRotationVelocity, cfg->maxRotationAcceleration, true)
{
const SensorValueBase* sv = useSensorValue(cfg->platformName);
@@ -78,24 +82,14 @@ namespace armarx
return;
}
- float relativeOrientation = currentOrientation - rtGetControlStruct().startOrientation;
- Eigen::Vector2f relativeCurrentPosition = currentPosition - rtGetControlStruct().startPosition;
-
- Eigen::Vector2f updatedPosition = rtGetControlStruct().globalPosition;// + relativeCurrentPosition;
- float updatedOrientation = rtGetControlStruct().globalOrientation;//+ relativeOrientation;
-
- float relativeGlobalOrientation = rtGetControlStruct().globalOrientation - getWriterControlStruct().startOrientation;
- relativeGlobalOrientation = std::atan2(std::sin(relativeGlobalOrientation), std::cos(relativeGlobalOrientation));
-
- float relativeTargetOrientation = rtGetControlStruct().targetOrientation - getWriterControlStruct().startOrientation;
- relativeTargetOrientation = std::atan2(std::sin(relativeTargetOrientation), std::cos(relativeTargetOrientation));
+ const float measuredOrientation = rtGetControlStruct().globalOrientation;
+ pid.update(timeSinceLastIteration.toSecondsDouble(), rtGetControlStruct().globalPosition, rtGetControlStruct().target);
+ opid.update(timeSinceLastIteration.toSecondsDouble(), static_cast<double>(measuredOrientation), rtGetControlStruct().targetOrientation);
- pid.update(timeSinceLastIteration.toSecondsDouble(), updatedPosition, rtGetControlStruct().target);
- //opid.update(timeSinceLastIteration.toSecondsDouble(), static_cast<double>(updatedOrientation), rtGetControlStruct().targetOrientation);
- opid.update(timeSinceLastIteration.toSecondsDouble(), static_cast<double>(relativeGlobalOrientation), relativeTargetOrientation);
+ const Eigen::Rotation2Df global_R_local(-measuredOrientation);
- Eigen::Vector2f velocities = Eigen::Rotation2Df(-updatedOrientation) * Eigen::Vector2f(pid.getControlValue()[0], pid.getControlValue()[1]);
+ Eigen::Vector2f velocities = global_R_local * pid.getControlValue();
target->velocityX = velocities.x();
target->velocityY = velocities.y();
target->velocityRotation = static_cast<float>(opid.getControlValue());
@@ -112,7 +106,7 @@ namespace armarx
std::lock_guard<std::recursive_mutex> lock(controlDataMutex);
getWriterControlStruct().target << x, y;
- getWriterControlStruct().targetOrientation = std::atan2(std::sin(yaw), std::cos(yaw));
+ getWriterControlStruct().targetOrientation = simox::math::periodic_clamp(yaw, -M_PIf32, M_PIf32);
getWriterControlStruct().translationAccuracy = translationAccuracy;
getWriterControlStruct().rotationAccuracy = rotationAccuracy;
getWriterControlStruct().newTargetSet = true;
@@ -125,7 +119,7 @@ namespace armarx
// ..todo: check if norm is too large
getWriterControlStruct().globalPosition << currentPose.x, currentPose.y;
- getWriterControlStruct().globalOrientation = currentPose.rotationAroundZ;
+ getWriterControlStruct().globalOrientation = simox::math::periodic_clamp(currentPose.rotationAroundZ, -M_PIf32, M_PIf32);
getWriterControlStruct().startPosition = currentPosition;
getWriterControlStruct().startOrientation = currentOrientation;
diff --git a/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.cpp b/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.cpp
index 44d9c0aa5a71bee4a4095077a3aca2525cb0437a..4f7372a5e178ed8ec895049ef094af354d297ee2 100644
--- a/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.cpp
+++ b/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.cpp
@@ -65,6 +65,9 @@ namespace armarx
widget.setupUi(getWidget());
+ updateTimer = new QTimer(this);
+ connect(updateTimer, &QTimer::timeout, this, QOverload<>::of(&This::onUpdate));
+
timingObserverTimer = new QTimer(this);
connect(timingObserverTimer, &QTimer::timeout, this, QOverload<>::of(&This::onTimingObserverUpdate));
@@ -186,6 +189,7 @@ namespace armarx
changeMode(ArVizWidgetMode::Live);
timingObserverTimer->start(33);
+ updateTimer->start(33);
}
void ArVizWidgetController::onDisconnectGui()
@@ -440,10 +444,15 @@ namespace armarx
layerTreeChanged(nullptr, 0);
}
+ void ArVizWidgetController::onUpdate()
+ {
+ visualizer.update();
+ }
+
void ArVizWidgetController::onTimingObserverUpdate()
{
viz::CoinVisualizer_UpdateTiming timing = visualizer.getTiming();
- if (timing.counter > lastTiming.counter)
+ //if (timing.counter > lastTiming.counter)
{
if (debugObserver)
{
@@ -453,6 +462,8 @@ namespace armarx
timingMap["1.2 apply, updateElements (ms)"] = new Variant(timing.applyTotal.updateElements.toMilliSecondsDouble());
timingMap["1.3 apply, removeElements (ms)"] = new Variant(timing.applyTotal.removeElements.toMilliSecondsDouble());
timingMap["2. layers (ms)"] = new Variant(timing.layersChanged.toMilliSecondsDouble());
+ timingMap["3. wait duration (ms)"] = new Variant(timing.waitDuration.toMilliSecondsDouble());
+ timingMap["4. update toggle"] = new Variant(timing.updateToggle);
timingMap["total (ms)"] = new Variant(timing.total.toMilliSecondsDouble());
timings.push_back(timing.total.toMilliSecondsDouble());
diff --git a/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.h b/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.h
index 7a3a0bfa34330ecf32509b24d2e1d420e4016d0f..31383a63ea78c510ec781951ff56b5fdbcafee06 100644
--- a/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.h
+++ b/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.h
@@ -140,6 +140,7 @@ namespace armarx
void showAllLayers(bool visible);
void showFilteredLayers(bool visible);
+ void onUpdate();
void onTimingObserverUpdate();
@@ -174,6 +175,7 @@ namespace armarx
QPointer<SimpleConfigDialog> configDialog;
+ QTimer* updateTimer;
QTimer* timingObserverTimer;
viz::CoinVisualizer_UpdateTiming lastTiming;
StringVariantBaseMap timingMap;
diff --git a/source/RobotAPI/interface/ArViz/Elements.ice b/source/RobotAPI/interface/ArViz/Elements.ice
index 0adf0413b30d5985851b86dab12580d0a314acbe..bebbb3f414be2184f51fdb8518cdcd368da33ebc 100644
--- a/source/RobotAPI/interface/ArViz/Elements.ice
+++ b/source/RobotAPI/interface/ArViz/Elements.ice
@@ -100,6 +100,13 @@ module data
float lineWidth = 0.0f;
};
+ class ElementPath extends Element
+ {
+ Vector3fSeq points;
+
+ float lineWidth = 10.0f;
+ };
+
class ElementArrow extends Element
{
float length = 100.0f;
diff --git a/source/RobotAPI/interface/armem/query.ice b/source/RobotAPI/interface/armem/query.ice
index bfe0764272dbee12270de46cfe7cf0b90f851bb0..2a0eb6e217a769fe4bab33dd0ab896e2faeda8e5 100644
--- a/source/RobotAPI/interface/armem/query.ice
+++ b/source/RobotAPI/interface/armem/query.ice
@@ -11,9 +11,17 @@ module armarx
{
module data
{
+ enum QueryTarget
+ {
+ WMAndLTM,
+ WM,
+ LTM // only for debug. remove later
+ };
+
/// Which entity snapshots to get from an entity?
class EntityQuery
{
+ QueryTarget target = QueryTarget::WMAndLTM;
};
sequence<EntityQuery> EntityQuerySeq;
@@ -107,6 +115,7 @@ module armarx
class ProviderSegmentQuery
{
EntityQuerySeq entityQueries;
+ QueryTarget target = QueryTarget::WMAndLTM;
};
sequence<ProviderSegmentQuery> ProviderSegmentQuerySeq;
module provider
@@ -129,6 +138,7 @@ module armarx
class CoreSegmentQuery
{
ProviderSegmentQuerySeq providerSegmentQueries;
+ QueryTarget target = QueryTarget::WMAndLTM;
};
sequence<CoreSegmentQuery> CoreSegmentQuerySeq;
@@ -152,6 +162,7 @@ module armarx
class MemoryQuery
{
CoreSegmentQuerySeq coreSegmentQueries;
+ QueryTarget target = QueryTarget::WMAndLTM;
};
sequence<MemoryQuery> MemoryQuerySeq;
dictionary<string, MemoryQuerySeq> MemoryQueriesDict;
@@ -177,6 +188,7 @@ module armarx
{
/// Dict of memory name to
MemoryQueriesDict memoryQueries;
+ QueryTarget target = QueryTarget::WMAndLTM;
};
struct Input
diff --git a/source/RobotAPI/interface/armem/server/MemoryInterface.ice b/source/RobotAPI/interface/armem/server/MemoryInterface.ice
index a4c8ba2f7602be3eccf333584cffe7a773486f03..ec83f439d8a91e8e57fe4c1767a8ba27bfab7c5d 100644
--- a/source/RobotAPI/interface/armem/server/MemoryInterface.ice
+++ b/source/RobotAPI/interface/armem/server/MemoryInterface.ice
@@ -6,6 +6,8 @@
#include <RobotAPI/interface/armem/server/ReadingMemoryInterface.ice>
#include <RobotAPI/interface/armem/server/WritingMemoryInterface.ice>
+#include <RobotAPI/interface/armem/client/MemoryListenerInterface.ice>
+
module armarx
{
@@ -21,7 +23,10 @@ module armarx
{
};
- interface MemoryInterface extends WorkingMemoryInterface, LongTermMemoryInterface
+ interface MemoryInterface extends
+ WorkingMemoryInterface,
+ LongTermMemoryInterface,
+ client::MemoryListenerInterface
{
};
};
diff --git a/source/RobotAPI/interface/aron/Aron.ice b/source/RobotAPI/interface/aron/Aron.ice
index d01b0d60aa39d65b39d14fbc1abb8100a9ec0014..dc88e0484490476691ee98e3c3bf338964c02e6d 100644
--- a/source/RobotAPI/interface/aron/Aron.ice
+++ b/source/RobotAPI/interface/aron/Aron.ice
@@ -24,11 +24,16 @@
RUN_ARON_MACRO(String, string, STRING) \
RUN_ARON_MACRO(Bool, bool, BOOL)
+#define ARON_VERSION "beta 0.2.2"
+
+
module armarx
{
module aron
{
+ const string VERSION = ARON_VERSION;
+
/*************************
* General Definitions ***
************************/
@@ -43,23 +48,36 @@ module armarx
************************/
module type
{
- class AronType { };
+ enum Maybe
+ {
+ eNone,
+ eOptional,
+ eRawPointer,
+ eUniquePointer,
+ eSharedPointer
+ };
+
+ class AronType {
+ //string VERSION = ARON_VERSION;
+ Maybe maybe = Maybe::eNone; // Every type can have a maye type flag or not
+ }
sequence<AronType> AronTypeList;
dictionary<string, AronType> AronTypeDict;
// Container types
- class AronList extends AronType { AronType acceptedType; };
- class AronTuple extends AronType { AronTypeList elementTypes; };
- class AronPair extends AronType { AronType acceptedType1; AronType acceptedType2; };
- class AronObject extends AronType { AronObject parent; string objectName; AronTypeDict elementTypes; };
- class AronDict extends AronType { AronType acceptedType; };
+ class AronList extends AronType { AronType acceptedType; }
+ class AronTuple extends AronType { AronTypeList elementTypes; }
+ class AronPair extends AronType { AronType acceptedType1; AronType acceptedType2; }
+ class AronObject extends AronType { AronObject parent; string objectName; AronTypeDict elementTypes; }
+ class AronDict extends AronType { AronType acceptedType; }
// Complex Types (serialize to ndarray)
- class AronEigenMatrix extends AronType { AronIntSequence dimensions; string typeName; };
+ class AronNDArray extends AronType { AronIntSequence dimensions; int elementSize; string typeName; }
+ class AronEigenMatrix extends AronType { int rows; int cols; string typeName; }
class AronEigenQuaternion extends AronType { string typeName; }
- class AronIVTCByteImage extends AronType { int width; int height; string typeName; }
- class AronOpenCVMat extends AronType { AronIntSequence dimensions; string typeName; }
- class AronPCLPointCloud extends AronType { int width; int height; string typeName; }
+ class AronIVTCByteImage extends AronType { }
+ class AronOpenCVMat extends AronType { }
+ class AronPCLPointCloud extends AronType { string typeName; }
class AronPosition extends AronType { }
class AronOrientation extends AronType { }
class AronPose extends AronType { }
@@ -81,7 +99,9 @@ module armarx
************************/
module data
{
- class AronData { };
+ class AronData {
+ //string VERSION = ARON_VERSION;
+ };
sequence<AronData> AronDataList;
dictionary<string, AronData> AronDataDict;
diff --git a/source/RobotAPI/interface/components/ObstacleAvoidance/DynamicObstacleManagerInterface.ice b/source/RobotAPI/interface/components/ObstacleAvoidance/DynamicObstacleManagerInterface.ice
index d21a3c55b7574a459f0be3e2577870cf7640f5e3..5ff2649fae12cb906f5821f73ba1fb1fd21647fe 100644
--- a/source/RobotAPI/interface/components/ObstacleAvoidance/DynamicObstacleManagerInterface.ice
+++ b/source/RobotAPI/interface/components/ObstacleAvoidance/DynamicObstacleManagerInterface.ice
@@ -30,6 +30,18 @@
module armarx
{
+ module dynamicobstaclemanager
+ {
+ struct LineSegment
+ {
+ Eigen::Vector2f lineStart;
+ Eigen::Vector2f lineEnd;
+ };
+
+ sequence<LineSegment> LineSegments;
+ }
+
+
interface DynamicObstacleManagerInterface
{
void
@@ -38,6 +50,9 @@ module armarx
void
add_decayable_line_segment(Eigen::Vector2f line_start, Eigen::Vector2f line_end);
+ void
+ add_decayable_line_segments(dynamicobstaclemanager::LineSegments lines);
+
void
remove_all_decayable_obstacles();
@@ -48,6 +63,8 @@ module armarx
remove_obstacle(string name);
void wait_unitl_obstacles_are_ready();
+
+ float distanceToObstacle(Eigen::Vector2f agentPosition, float safetyRadius, Eigen::Vector2f goal);
};
};
diff --git a/source/RobotAPI/interface/objectpose/object_pose_types.ice b/source/RobotAPI/interface/objectpose/object_pose_types.ice
index 385da35e387d04b21aaea7e4845b073f9c977ee4..0dc650c9b7bb6bf9b139714015ea55120a409f0d 100644
--- a/source/RobotAPI/interface/objectpose/object_pose_types.ice
+++ b/source/RobotAPI/interface/objectpose/object_pose_types.ice
@@ -29,6 +29,10 @@
module armarx
{
+ // Originally defined in <RobotAPI/interface/units/KinematicUnitInterface.ice>
+ dictionary<string, float> NameValueMap;
+
+
// A struct's name cannot cannot differ only in capitalization from its immediately enclosing module name.
module objpose
{
@@ -64,6 +68,8 @@ module armarx
string providerName;
/// Known or unknown object.
ObjectTypeEnum objectType = AnyObject;
+ /// Whether object is static. Static objects don't decay.
+ bool isStatic = false;
/// The object ID, i.e. dataset and name.
armarx::data::ObjectID objectID;
@@ -72,6 +78,9 @@ module armarx
PoseBase objectPose;
string objectPoseFrame;
+ /// The object's joint values if it is articulated.
+ NameValueMap objectJointValues;
+
/// Confidence in [0, 1] (1 = full, 0 = none).
float confidence = 0;
/// Source timestamp.
@@ -92,6 +101,8 @@ module armarx
string providerName;
/// Known or unknown object.
ObjectTypeEnum objectType = AnyObject;
+ /// Whether object is static. Static objects don't decay.
+ bool isStatic = false;
/// The object ID, i.e. dataset and name.
armarx::data::ObjectID objectID;
@@ -101,6 +112,10 @@ module armarx
PoseBase objectPoseOriginal;
string objectPoseOriginalFrame;
+ /// The object's joint values if it is articulated.
+ NameValueMap objectJointValues;
+
+
StringFloatDictionary robotConfig;
PoseBase robotPose;
diff --git a/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice b/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
index 878f0b28bf5318095f28006dfd6e219a4e68217f..5be7b7967c044268043390727cdc95b7ac434590 100644
--- a/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
+++ b/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
@@ -329,6 +329,7 @@ module armarx
{
void learnDMPFromFiles(Ice::StringSeq trajfiles);
void learnJointDMPFromFiles(string jointTrajFile, Ice::FloatSeq currentJVS);
+ void setUseNullSpaceJointDMP(bool enable);
bool isFinished();
void runDMP(Ice::DoubleSeq goals);
@@ -336,6 +337,7 @@ module armarx
void setViaPoints(double canVal, Ice::DoubleSeq point);
void setGoals(Ice::DoubleSeq goals);
+ void setDefaultNullSpaceJointValues(Eigen::VectorXf jointValues);
double getVirtualTime();
@@ -347,10 +349,12 @@ module armarx
void setMPWeights(DoubleSeqSeq weights);
DoubleSeqSeq getMPWeights();
- void setLinearVelocityKd(Ice::FloatSeq kd);
- void setLinearVelocityKp(Ice::FloatSeq kp);
- void setAngularVelocityKd(Ice::FloatSeq kd);
- void setAngularVelocityKp(Ice::FloatSeq kp);
+ void setLinearVelocityKd(Eigen::Vector3f kd);
+ void setLinearVelocityKp(Eigen::Vector3f kp);
+ void setAngularVelocityKd(Eigen::Vector3f kd);
+ void setAngularVelocityKp(Eigen::Vector3f kp);
+ void setNullspaceVelocityKd(Eigen::VectorXf jointValues);
+ void setNullspaceVelocityKp(Eigen::VectorXf jointValues);
void enableForceStop();
void disableForceStop();
diff --git a/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCAT.cpp b/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCAT.cpp
index ac5fe6828a68ed2cb600a0bbef9d8cb2ac7df674..bb239e656673d84e2849ae1485de41f744889ccf 100644
--- a/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCAT.cpp
+++ b/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCAT.cpp
@@ -785,7 +785,7 @@ void EtherCAT::readErrorCounters()
//not used, only confusing info...
//uint16_t configAddr = ecx_APRDw(ecx_context.port, ADPh, ECT_REG_STADR, 100000);
- for (int i = 0; i < 4; i++)
+ for (int i = 0; i < 2; i++)
{
// Error handling taken from
@@ -809,14 +809,14 @@ void EtherCAT::readErrorCounters()
}
else if (ret1 > 0 && ret2 > 0 && ret3 > 0 && ret4 > 0)
{
- ARMARX_INFO << "no errors for Slavenumber " << slaveIndex << " port:" << i << "\tname: " << name;
+ ARMARX_DEBUG << "no errors for Slavenumber " << slaveIndex << " port:" << i << "\tname: " << name;
}
}
IceUtil::Time elapsed = (IceUtil::Time::now(IceUtil::Time::Monotonic) - start);
if (elapsed.toMilliSeconds() > 10)
{
updatePDO();
- ARMARX_INFO << "Updated BUS to prevent timeout, " << elapsed << " has passed since last bus update.";
+ ARMARX_DEBUG << "Updated BUS to prevent timeout, " << elapsed << " has passed since last bus update.";
start = IceUtil::Time::now(IceUtil::Time::Monotonic);
}
}
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.cpp b/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.cpp
index 8e86d96dc398d707ab4e8629e4b971519db8dc65..92a5760ae47f099c3408606903efb25db007f764 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.cpp
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.cpp
@@ -1,23 +1,24 @@
#include <VirtualRobot/XML/ObjectIO.h>
-#include <boost/algorithm/string.hpp>
+#include <set>
#include <SimoxUtility/algorithm/string.h>
#include <SimoxUtility/filesystem/list_directory.h>
-#include <VirtualRobot/XML/RobotIO.h>
-
#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
#include "ObjectFinder.h"
+
namespace armarx
{
namespace fs = std::filesystem;
- ObjectFinder::ObjectFinder(const std::string& objectsPackageName) : packageName(objectsPackageName)
+
+ ObjectFinder::ObjectFinder(const std::string& objectsPackageName, const ObjectFinder::path& relObjectsDir) :
+ packageName(objectsPackageName), relObjectsDir(relObjectsDir)
{
Logging::setTag("ObjectFinder");
}
@@ -25,7 +26,7 @@ namespace armarx
void ObjectFinder::setPath(const std::string& path)
{
packageName = path;
- packageDataDir.clear();
+ absPackageDataDir.clear();
}
std::string ObjectFinder::getPackageName() const
@@ -35,11 +36,11 @@ namespace armarx
void ObjectFinder::init() const
{
- if (packageDataDir.empty())
+ if (absPackageDataDir.empty())
{
CMakePackageFinder packageFinder(packageName);
- packageDataDir = packageFinder.getDataDir();
- if (packageDataDir.empty())
+ absPackageDataDir = packageFinder.getDataDir();
+ if (absPackageDataDir.empty())
{
ARMARX_WARNING << "Could not find package '" << packageName << "'.";
// throw LocalException() << "Could not find package '" << packageName << "'.";
@@ -49,12 +50,18 @@ namespace armarx
ARMARX_VERBOSE << "Objects root directory: " << _rootDirAbs();
// make sure this data path is available => e.g. for findArticulatedObjects
- armarx::ArmarXDataPath::addDataPaths(std::vector<std::string> {packageDataDir});
+ armarx::ArmarXDataPath::addDataPaths(std::vector<std::string> {absPackageDataDir});
}
}
}
+ bool ObjectFinder::isDatasetDirValid(const path& path) const
+ {
+ return std::filesystem::is_directory(path);
+ }
+
+
std::optional<ObjectInfo> ObjectFinder::findObject(const std::string& dataset, const std::string& name) const
{
init();
@@ -64,15 +71,15 @@ namespace armarx
}
if (!dataset.empty())
{
- return ObjectInfo(packageName, packageDataDir, dataset, name);
+ return ObjectInfo(packageName, absPackageDataDir, relObjectsDir, dataset, name);
}
// Search for object in datasets.
- const auto& datasets = getDatasets();
- for (const path& dataset : datasets)
+ const std::vector<std::string>& datasets = getDatasets();
+ for (const std::string& dataset : datasets)
{
if (fs::is_directory(_rootDirAbs() / dataset / name))
{
- return ObjectInfo(packageName, packageDataDir, dataset, name);
+ return ObjectInfo(packageName, absPackageDataDir, relObjectsDir, dataset, name);
}
}
@@ -106,7 +113,7 @@ namespace armarx
{
// init(); // Done by called methods.
std::vector<std::string> datasets;
- for (const auto& dir : getDatasetDirectories())
+ for (const path& dir : getDatasetDirectories())
{
datasets.push_back(dir.filename());
}
@@ -123,9 +130,9 @@ namespace armarx
const bool local = false;
std::vector<path> dirs = simox::fs::list_directory(_rootDirAbs(), local);
std::vector<path> datasetDirs;
- for (const auto& p : dirs)
+ for (const path& p : dirs)
{
- if (std::filesystem::is_directory(p))
+ if (isDatasetDirValid(p))
{
datasetDirs.push_back(p);
}
@@ -144,7 +151,7 @@ namespace armarx
std::vector<ObjectInfo> objects;
for (const path& datasetDir : simox::fs::list_directory(_rootDirAbs(), local))
{
- if (fs::is_directory(_rootDirAbs() / datasetDir))
+ if (isDatasetDirValid(_rootDirAbs() / datasetDir))
{
std::vector<ObjectInfo> dataset = findAllObjectsOfDataset(datasetDir, checkPaths);
for (const auto& o : dataset)
@@ -169,7 +176,7 @@ namespace armarx
std::vector<armem::articulated_object::ArticulatedObjectDescription> objects;
for (const path& datasetDir : simox::fs::list_directory(_rootDirAbs(), local))
{
- if (fs::is_directory(_rootDirAbs() / datasetDir))
+ if (isDatasetDirValid(_rootDirAbs() / datasetDir))
{
const auto dataset = findAllArticulatedObjectsOfDataset(datasetDir, checkPaths);
objects.insert(objects.end(), dataset.begin(), dataset.end());
@@ -211,7 +218,7 @@ namespace armarx
{
if (fs::is_directory(datasetDir / dir))
{
- ObjectInfo object(packageName, packageDataDir, dataset, dir.filename());
+ ObjectInfo object(packageName, absPackageDataDir, relObjectsDir, dataset, dir.filename());
if (!checkPaths || object.checkPaths())
{
objects.push_back(object);
@@ -221,7 +228,9 @@ namespace armarx
return objects;
}
- std::unordered_map<std::string, std::vector<armem::articulated_object::ArticulatedObjectDescription>> ObjectFinder::findAllArticulatedObjectsByDataset(bool checkPaths) const
+
+ std::unordered_map<std::string, std::vector<armem::articulated_object::ArticulatedObjectDescription>>
+ ObjectFinder::findAllArticulatedObjectsByDataset(bool checkPaths) const
{
init();
if (!_ready())
@@ -234,7 +243,7 @@ namespace armarx
std::unordered_map<std::string, std::vector<armem::articulated_object::ArticulatedObjectDescription>> datasets;
for (const path& datasetDir : simox::fs::list_directory(_rootDirAbs(), local))
{
- if (fs::is_directory(_rootDirAbs() / datasetDir))
+ if (isDatasetDirValid(_rootDirAbs() / datasetDir))
{
const auto dataset = findAllArticulatedObjectsOfDataset(datasetDir, checkPaths);
datasets[datasetDir] = dataset;
@@ -244,7 +253,8 @@ namespace armarx
}
- std::vector<armem::articulated_object::ArticulatedObjectDescription> ObjectFinder::findAllArticulatedObjectsOfDataset(const std::string& dataset, bool checkPaths) const
+ std::vector<armem::articulated_object::ArticulatedObjectDescription>
+ ObjectFinder::findAllArticulatedObjectsOfDataset(const std::string& dataset, bool checkPaths) const
{
init();
if (!_ready())
@@ -252,58 +262,29 @@ namespace armarx
return {};
}
path datasetDir = _rootDirAbs() / dataset;
- if (!fs::is_directory(datasetDir))
+ if (!isDatasetDirValid(datasetDir))
{
ARMARX_WARNING << "Expected dataset directory for dataset '" << dataset << "': \n"
<< datasetDir;
return {};
}
- const std::vector<std::string> validExtensions{".urdf", ".xml"};
-
- const auto hasValidExtension = [&](const std::string & file) -> bool
- {
- return std::find(validExtensions.begin(), validExtensions.end(), boost::algorithm::to_lower_copy(std::filesystem::path(file).extension().string())) != validExtensions.end();
- };
-
std::vector<armem::articulated_object::ArticulatedObjectDescription> objects;
const bool local = true;
for (const path& dir : simox::fs::list_directory(datasetDir, local))
{
- if (not fs::is_directory(datasetDir / dir))
- {
- continue;
- }
-
- for (const auto& file : std::filesystem::directory_iterator(datasetDir / dir))
+ if (fs::is_directory(datasetDir / dir))
{
- const std::string xml = std::filesystem::path(file).string();
-
- if (hasValidExtension(xml))
+ ObjectInfo object(packageName, absPackageDataDir, relObjectsDir, dataset, dir.filename());
+ std::optional<PackageFileLocation> modelFile = object.getArticulatedModel();
+ if (modelFile.has_value())
{
- try
- {
- const auto robot = VirtualRobot::RobotIO::loadRobot(xml, VirtualRobot::RobotIO::RobotDescription::eStructure);
- if (robot != nullptr && robot->isPassive())
- {
- const std::string relativeXMLPath = armarx::ArmarXDataPath::getRelativeArmarXPath(xml);
-
- objects.emplace_back(armem::articulated_object::ArticulatedObjectDescription
- {
- .name = robot->getName(),
- .xml = {packageName, relativeXMLPath}
- // .dataset = dataset
- });
- }
- }
- catch (const armarx::LocalException& ex)
+ objects.emplace_back(armem::articulated_object::ArticulatedObjectDescription
{
- ARMARX_WARNING << ex.what();
- }
- catch (...)
- {
-
- }
+ .name = object.idStr(),
+ .xml = {modelFile->package, modelFile->relativePath}
+ // .dataset = dataset
+ });
}
}
}
@@ -392,7 +373,7 @@ namespace armarx
ObjectFinder::path ObjectFinder::_rootDirAbs() const
{
- return packageDataDir / packageName;
+ return absPackageDataDir / packageName / relObjectsDir;
}
ObjectFinder::path ObjectFinder::_rootDirRel() const
@@ -402,7 +383,7 @@ namespace armarx
bool ObjectFinder::_ready() const
{
- return !packageDataDir.empty();
+ return !absPackageDataDir.empty();
}
}
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.h b/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.h
index 2b342f3ac1151d0589bbc4b0212c3aed2fde798d..ea3a7e9cb08f423716931086f309e6dabeedd1b9 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.h
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.h
@@ -7,32 +7,40 @@
#include <ArmarXCore/core/logging/Logging.h>
+#include <RobotAPI/libraries/armem_objects/types.h>
+
#include "ObjectInfo.h"
#include "ObjectPose.h"
-#include <RobotAPI/libraries/armem_objects/types.h>
namespace armarx
{
/**
- * @brief Used to find objects in the ArmarX objects repository [1].
+ * @brief Used to find objects in the ArmarX objects repository [1] (formerly [2]).
*
- * @see [1] https://gitlab.com/ArmarX/ArmarXObjects
+ * @see [1] https://gitlab.com/ArmarX/PriorKnowledgeData
+ * @see [2] https://gitlab.com/ArmarX/ArmarXObjects
*/
class ObjectFinder : Logging
{
public:
+
using path = std::filesystem::path;
- inline static const std::string DefaultObjectsPackageName = "ArmarXObjects";
+ inline static const std::string DefaultObjectsPackageName = "PriorKnowledgeData";
+ inline static const std::string DefaultObjectsDirectory = "objects";
+
public:
- ObjectFinder(const std::string& objectsPackageName = DefaultObjectsPackageName);
+
+ ObjectFinder(const std::string& objectsPackageName = DefaultObjectsPackageName,
+ const path& relObjectsDir = DefaultObjectsDirectory);
ObjectFinder(ObjectFinder&&) = default;
ObjectFinder(const ObjectFinder&) = default;
ObjectFinder& operator=(ObjectFinder&&) = default;
ObjectFinder& operator=(const ObjectFinder&) = default;
+
void setPath(const std::string& path);
std::string getPackageName() const;
@@ -89,14 +97,18 @@ namespace armarx
private:
+
void init() const;
+ bool isDatasetDirValid(const std::filesystem::path& path) const;
path _rootDirAbs() const;
path _rootDirRel() const;
bool _ready() const;
+
private:
+
/// Name of package containing the object models (ArmarXObjects by default).
mutable std::string packageName;
@@ -104,6 +116,10 @@ namespace armarx
* @brief Absolute path to data directory (e.g. "/.../repos/ArmarXObjects/data").
* Empty if package could not be found.
*/
- mutable path packageDataDir;
+ mutable path absPackageDataDir;
+
+ /// Path to the directory containing objects in the package's data directory.
+ path relObjectsDir;
+
};
}
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectID.cpp b/source/RobotAPI/libraries/ArmarXObjects/ObjectID.cpp
index adb8260199706797be7b36b65e4fd504ad2e8a63..f2d76a501ecf8e1f53dcf9bf80a2e8a0f66d7536 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectID.cpp
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectID.cpp
@@ -57,6 +57,11 @@ namespace armarx
return _className == rhs._className && _dataset == rhs._dataset;
}
+ ObjectID ObjectID::withInstanceName(const std::string& instanceName) const
+ {
+ return ObjectID(_dataset, _className, instanceName);
+ }
+
bool ObjectID::operator==(const ObjectID& rhs) const
{
return _className == rhs._className
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectID.h b/source/RobotAPI/libraries/ArmarXObjects/ObjectID.h
index 73e2c6ca5f8d272fd90257fd5aedfd839266174d..389b203a6ecc67f2f6cd074ce103c5b52687a2be 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectID.h
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectID.h
@@ -43,6 +43,8 @@ namespace armarx
/// Indicates whether dataset and class name are equal.
bool equalClass(const ObjectID& rhs) const;
+ ObjectID withInstanceName(const std::string& instanceName) const;
+
/// Indicates whether dataset, class name and instance name are equal.
bool operator==(const ObjectID& rhs) const;
inline bool operator!=(const ObjectID& rhs) const
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.cpp b/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.cpp
index 348a5b25419813a22717f71c92d0c3484042361c..47728cbd7249f4983717f9a1dbb079e79215fadc 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.cpp
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.cpp
@@ -14,14 +14,17 @@ namespace armarx
ObjectInfo::ObjectInfo(const std::string& packageName, const ObjectInfo::path& packageDataDir,
- const ObjectID& id) :
- _packageName(packageName), _packageDataDir(packageDataDir), _id(id)
+ const path& relObjectsPath, const ObjectID& id) :
+ _packageName(packageName), _absPackageDataDir(packageDataDir),
+ _relObjectsPath(relObjectsPath), _id(id)
{
}
ObjectInfo::ObjectInfo(const std::string& packageName, const ObjectInfo::path& packageDataDir,
- const std::string& dataset, const std::string& name) :
- _packageName(packageName), _packageDataDir(packageDataDir), _id(dataset, name)
+ const path& relObjectsPath,
+ const std::string& dataset, const std::string& className) :
+ _packageName(packageName), _absPackageDataDir(packageDataDir),
+ _relObjectsPath(relObjectsPath), _id(dataset, className)
{
}
@@ -40,7 +43,7 @@ namespace armarx
return _id.dataset();
}
- std::string ObjectInfo::name() const
+ std::string ObjectInfo::className() const
{
return _id.className();
}
@@ -57,7 +60,7 @@ namespace armarx
ObjectInfo::path ObjectInfo::objectDirectory() const
{
- return path(_packageName) / _id.dataset() / _id.className();
+ return path(_packageName) / _relObjectsPath / _id.dataset() / _id.className();
}
PackageFileLocation ObjectInfo::file(const std::string& _extension, const std::string& suffix) const
@@ -72,15 +75,49 @@ namespace armarx
PackageFileLocation loc;
loc.package = _packageName;
loc.relativePath = objectDirectory() / filename;
- loc.absolutePath = _packageDataDir / loc.relativePath;
+ loc.absolutePath = _absPackageDataDir / loc.relativePath;
return loc;
}
+
PackageFileLocation ObjectInfo::simoxXML() const
{
return file(".xml");
}
+ PackageFileLocation ObjectInfo::articulatedSimoxXML() const
+ {
+ return file(".xml", "_articulated");
+ }
+
+ PackageFileLocation ObjectInfo::articulatedUrdf() const
+ {
+ return file(".urdf", "_articulated");
+ }
+
+ std::optional<PackageFileLocation> ObjectInfo::getArticulatedModel() const
+ {
+ if (fs::is_regular_file(articulatedSimoxXML().absolutePath))
+ {
+ return articulatedSimoxXML();
+ }
+ else if (fs::is_regular_file(articulatedUrdf().absolutePath))
+ {
+ return articulatedUrdf();
+ }
+ else
+ {
+ return std::nullopt;
+ }
+ }
+
+
+
+ PackageFileLocation ObjectInfo::meshWrl() const
+ {
+ return file(".wrl");
+ }
+
PackageFileLocation ObjectInfo::wavefrontObj() const
{
return file(".obj");
@@ -232,15 +269,15 @@ namespace armarx
{
if (_logError)
{
- ARMARX_WARNING << "Expected simox object file for object '" << *this << "': " << simoxXML().absolutePath;
+ ARMARX_WARNING << "Expected simox object file for object " << *this << ": " << simoxXML().absolutePath;
}
result = false;
}
- if (!fs::is_regular_file(wavefrontObj().absolutePath))
+ if (false and not fs::is_regular_file(wavefrontObj().absolutePath))
{
if (_logError)
{
- ARMARX_WARNING << "Expected wavefront object file (.obj) for object '" << *this << "': " << wavefrontObj().absolutePath;
+ ARMARX_WARNING << "Expected wavefront object file (.obj) for object " << *this << ": " << wavefrontObj().absolutePath;
}
result = false;
}
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.h b/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.h
index 634944e4cc52379977f784c370f48af1e84ed28d..ebd0fc51b4ddb660613dd0c5f18aa322c2942795 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.h
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.h
@@ -41,10 +41,19 @@ namespace armarx
public:
+ /**
+ * @brief ObjectInfo
+ *
+ * @param packageName The ArmarX package.
+ * @param absPackageDataDir Absolute path to the package's data directory.
+ * @param localObjectsPath The path where objects are stored in the data directory.
+ * @param id The object class ID (with dataset and class name).
+ */
+ ObjectInfo(const std::string& packageName, const path& absPackageDataDir,
+ const path& relObjectsPath, const ObjectID& id);
ObjectInfo(const std::string& packageName, const path& packageDataDir,
- const ObjectID& id);
- ObjectInfo(const std::string& packageName, const path& packageDataDir,
- const std::string& dataset, const std::string& name);
+ const path& relObjectsPath,
+ const std::string& dataset, const std::string& className);
virtual ~ObjectInfo() = default;
@@ -56,14 +65,30 @@ namespace armarx
std::string package() const;
std::string dataset() const;
- std::string name() const;
+ std::string className() const;
+ [[deprecated("This function is deprecated. Use className() instead.")]]
+ std::string name() const
+ {
+ return className();
+ }
+
/// Return "dataset/name".
ObjectID id() const;
std::string idStr() const;
+
PackageFileLocation file(const std::string& extension, const std::string& suffix = "") const;
+
PackageFileLocation simoxXML() const;
+
+ PackageFileLocation articulatedSimoxXML() const;
+ PackageFileLocation articulatedUrdf() const;
+ /// Return the articulated Simox XML or URDF, if one exists.
+ std::optional<PackageFileLocation> getArticulatedModel() const;
+
+
+ PackageFileLocation meshWrl() const;
PackageFileLocation wavefrontObj() const;
PackageFileLocation boundingBoxJson() const;
@@ -113,7 +138,8 @@ namespace armarx
private:
std::string _packageName;
- path _packageDataDir;
+ path _absPackageDataDir;
+ path _relObjectsPath;
ObjectID _id;
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.cpp b/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.cpp
index 171da9c1a660b8257b2abc3f1b1f05ff6c0deb98..51fb57e6896277be3538672fce00c1096bc60951 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.cpp
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.cpp
@@ -29,6 +29,7 @@ namespace armarx::objpose
{
providerName = ice.providerName;
objectType = ice.objectType;
+ isStatic = ice.isStatic;
armarx::fromIce(ice.objectID, objectID);
objectPoseRobot = ::armarx::fromIce(ice.objectPoseRobot);
@@ -36,6 +37,8 @@ namespace armarx::objpose
objectPoseOriginal = ::armarx::fromIce(ice.objectPoseOriginal);
objectPoseOriginalFrame = ice.objectPoseOriginalFrame;
+ objectJointValues = ice.objectJointValues;
+
robotConfig = ice.robotConfig;
robotPose = ::armarx::fromIce(ice.robotPose);
@@ -58,12 +61,14 @@ namespace armarx::objpose
{
ice.providerName = providerName;
ice.objectType = objectType;
+ ice.isStatic = isStatic;
armarx::toIce(ice.objectID, objectID);
ice.objectPoseRobot = new Pose(objectPoseRobot);
ice.objectPoseGlobal = new Pose(objectPoseGlobal);
ice.objectPoseOriginal = new Pose(objectPoseOriginal);
ice.objectPoseOriginalFrame = objectPoseOriginalFrame;
+ ice.objectJointValues = objectJointValues;
ice.robotConfig = robotConfig;
ice.robotPose = new Pose(robotPose);
@@ -80,10 +85,12 @@ namespace armarx::objpose
{
providerName = provided.providerName;
objectType = provided.objectType;
+ isStatic = provided.isStatic;
armarx::fromIce(provided.objectID, objectID);
objectPoseOriginal = ::armarx::fromIce(provided.objectPose);
objectPoseOriginalFrame = provided.objectPoseFrame;
+ objectJointValues = provided.objectJointValues;
armarx::FramedPose framed(objectPoseOriginal, objectPoseOriginalFrame, robot->getName());
framed.changeFrame(robot, robot->getRootNode()->getName());
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.h b/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.h
index 023d6351f3bcf2533188e0df197b7f8e61806388..23d54901fbf7fa5db2c222a09c268da111a22512 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.h
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.h
@@ -1,11 +1,15 @@
#pragma once
#include <optional>
+#include <map>
+#include <vector>
+
#include <Eigen/Core>
#include <SimoxUtility/shapes/OrientedBox.h>
#include <VirtualRobot/VirtualRobot.h>
+#include "ArmarXCore/core/PackagePath.h"
#include <IceUtil/Time.h>
@@ -47,6 +51,8 @@ namespace armarx::objpose
std::string providerName;
/// Known or unknown object.
ObjectTypeEnum objectType = AnyObject;
+ /// Whether object is static. Static objects don't decay.
+ bool isStatic = false;
/// The object ID, i.e. dataset, class name and instance name.
armarx::ObjectID objectID;
@@ -56,6 +62,9 @@ namespace armarx::objpose
Eigen::Matrix4f objectPoseOriginal;
std::string objectPoseOriginalFrame;
+ /// The object's joint values if it is articulated.
+ std::map<std::string, float> objectJointValues;
+
std::map<std::string, float> robotConfig;
Eigen::Matrix4f robotPose;
@@ -88,8 +97,13 @@ namespace armarx::objpose
* @param agent The agent/robot.
*/
void updateAttached(VirtualRobot::RobotPtr agent);
+
+
+ std::optional<PackagePath> articulatedSimoxXmlPath;
+
};
using ObjectPoseSeq = std::vector<ObjectPose>;
+ using ObjectPoseMap = std::map<ObjectID, ObjectPose>;
void fromIce(const data::ObjectAttachmentInfo& ice, ObjectAttachmentInfo& attachment);
diff --git a/source/RobotAPI/libraries/ArmarXObjects/aron/ObjectPose.xml b/source/RobotAPI/libraries/ArmarXObjects/aron/ObjectPose.xml
index e9701dc24c9a891449c9025a514322eaa615b52f..ba26c1a618746e41823850dc99a93b670bc2a7e9 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/aron/ObjectPose.xml
+++ b/source/RobotAPI/libraries/ArmarXObjects/aron/ObjectPose.xml
@@ -38,6 +38,10 @@ ARON DTO of armarx::objpose::ObjectPose.
<armarx::objpose::arondto::ObjectType />
</ObjectChild>
+ <ObjectChild key='isStatic'>
+ <bool />
+ </ObjectChild>
+
<ObjectChild key='objectID'>
<armarx::arondto::ObjectID />
</ObjectChild>
@@ -58,6 +62,13 @@ ARON DTO of armarx::objpose::ObjectPose.
<string />
</ObjectChild>
+ <!-- The object's joint values if it is articulated. -->
+ <ObjectChild key='objectJointValues'>
+ <Dict>
+ <Float />
+ </Dict>
+ </ObjectChild>
+
<ObjectChild key='robotConfig'>
<Dict>
<Float />
diff --git a/source/RobotAPI/libraries/ArmarXObjects/aron_conversions/objpose.cpp b/source/RobotAPI/libraries/ArmarXObjects/aron_conversions/objpose.cpp
index 2de8a3da0096d0c2edf7d103c944edbb681eb56d..a639223a050b5473c69302816ee1a46adceb505a 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/aron_conversions/objpose.cpp
+++ b/source/RobotAPI/libraries/ArmarXObjects/aron_conversions/objpose.cpp
@@ -2,8 +2,13 @@
#include <ArmarXCore/core/exceptions/local/UnexpectedEnumValueException.h>
+
#include <RobotAPI/libraries/aron/common/aron_conversions.h>
+
+#include <RobotAPI/libraries/ArmarXObjects/ObjectPose.h>
#include <RobotAPI/libraries/ArmarXObjects/aron_conversions.h>
+#include <RobotAPI/libraries/ArmarXObjects/aron/ObjectType.aron.generated.h>
+#include <RobotAPI/libraries/ArmarXObjects/aron/ObjectPose.aron.generated.h>
void armarx::objpose::fromAron(const arondto::ObjectAttachmentInfo& dto, ObjectAttachmentInfo& bo)
@@ -56,14 +61,15 @@ void armarx::objpose::toAron(arondto::ObjectType& dto, const ObjectTypeEnum& bo)
void armarx::objpose::fromAron(const arondto::ObjectPose& dto, ObjectPose& bo)
{
bo.providerName = dto.providerName;
-
fromAron(dto.objectType, bo.objectType);
+ bo.isStatic = dto.isStatic;
fromAron(dto.objectID, bo.objectID);
bo.objectPoseRobot = dto.objectPoseRobot;
bo.objectPoseGlobal = dto.objectPoseGlobal;
bo.objectPoseOriginal = dto.objectPoseOriginal;
bo.objectPoseOriginalFrame = dto.objectPoseOriginalFrame;
+ bo.objectJointValues = dto.objectJointValues;
bo.robotConfig = dto.robotConfig;
bo.robotPose = dto.robotPose;
@@ -80,7 +86,7 @@ void armarx::objpose::fromAron(const arondto::ObjectPose& dto, ObjectPose& bo)
bo.confidence = dto.confidence;
- bo.timestamp = IceUtil::Time::microSeconds(dto.timestamp);
+ bo.timestamp = dto.timestamp;
if (dto.localOOBBValid)
{
@@ -97,14 +103,15 @@ void armarx::objpose::fromAron(const arondto::ObjectPose& dto, ObjectPose& bo)
void armarx::objpose::toAron(arondto::ObjectPose& dto, const ObjectPose& bo)
{
dto.providerName = bo.providerName;
-
toAron(dto.objectType, bo.objectType);
+ dto.isStatic = bo.isStatic;
toAron(dto.objectID, bo.objectID);
dto.objectPoseRobot = bo.objectPoseRobot;
dto.objectPoseGlobal = bo.objectPoseGlobal;
dto.objectPoseOriginal = bo.objectPoseOriginal;
dto.objectPoseOriginalFrame = bo.objectPoseOriginalFrame;
+ dto.objectJointValues = bo.objectJointValues;
dto.robotConfig = bo.robotConfig;
dto.robotPose = bo.robotPose;
@@ -122,7 +129,7 @@ void armarx::objpose::toAron(arondto::ObjectPose& dto, const ObjectPose& bo)
dto.confidence = bo.confidence;
- dto.timestamp = bo.timestamp.toMicroSeconds();
+ dto.timestamp = bo.timestamp;
if (bo.localOOBB)
{
diff --git a/source/RobotAPI/libraries/ArmarXObjects/aron_conversions/objpose.h b/source/RobotAPI/libraries/ArmarXObjects/aron_conversions/objpose.h
index afe31e500b7ba9a39b76dc33b0eeda0916cc849b..2e3f5901bfaf45f0cf131f609d46a18d67ace532 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/aron_conversions/objpose.h
+++ b/source/RobotAPI/libraries/ArmarXObjects/aron_conversions/objpose.h
@@ -1,14 +1,20 @@
#pragma once
-#include <RobotAPI/libraries/ArmarXObjects/ObjectPose.h>
-#include <RobotAPI/libraries/ArmarXObjects/aron/ObjectPose.aron.generated.h>
-
#include <RobotAPI/interface/objectpose/object_pose_types.h>
-#include <RobotAPI/libraries/ArmarXObjects/aron/ObjectType.aron.generated.h>
+namespace armarx::objpose::arondto
+{
+ class ObjectAttachmentInfo;
+ class ObjectType;
+ class ObjectPose;
+}
namespace armarx::objpose
{
+ class ObjectAttachmentInfo;
+ class ObjectPose;
+
+
void fromAron(const arondto::ObjectAttachmentInfo& dto, ObjectAttachmentInfo& bo);
void toAron(arondto::ObjectAttachmentInfo& dto, const ObjectAttachmentInfo& bo);
diff --git a/source/RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseClientPlugin.cpp b/source/RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseClientPlugin.cpp
index b12572855775c7c9f3f11f4f2d571da7ff36a14f..9bf87346d6014d3add6edad66bbda0bfa84019ad 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseClientPlugin.cpp
+++ b/source/RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseClientPlugin.cpp
@@ -1,5 +1,8 @@
#include "ObjectPoseClientPlugin.h"
+#include <ArmarXCore/core/Component.h>
+
+
namespace armarx::plugins
{
void ObjectPoseClientPlugin::postCreatePropertyDefinitions(PropertyDefinitionsPtr& properties)
diff --git a/source/RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseClientPlugin.h b/source/RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseClientPlugin.h
index 996f699acd35dd50afa095f3bdaa66d9d2a7e061..769d1176dd1fc8db6063fe867a3aec1ff3e1b90d 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseClientPlugin.h
+++ b/source/RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseClientPlugin.h
@@ -1,11 +1,12 @@
#pragma once
-#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/ComponentPlugin.h>
#include <RobotAPI/interface/objectpose/ObjectPoseStorageInterface.h>
#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
#include <RobotAPI/libraries/ArmarXObjects/ObjectPose.h>
+
namespace armarx::plugins
{
class ObjectPoseClientPlugin : public ComponentPlugin
@@ -51,6 +52,9 @@ namespace armarx::plugins
};
}
+
+#include <ArmarXCore/core/ManagedIceObject.h>
+
namespace armarx
{
/**
diff --git a/source/RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseProviderPlugin.cpp b/source/RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseProviderPlugin.cpp
index 63ccab6e140afa4bb8fdd174d0523e7c3b0cd78d..6aef209e68f06d66eae2a5a9a94fde6d084e04bd 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseProviderPlugin.cpp
+++ b/source/RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseProviderPlugin.cpp
@@ -1,5 +1,8 @@
#include "ObjectPoseProviderPlugin.h"
+#include <ArmarXCore/core/Component.h>
+
+
namespace armarx::plugins
{
diff --git a/source/RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseProviderPlugin.h b/source/RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseProviderPlugin.h
index 2f7c87ef560cad2e4a0b20257387f52b8bd6835d..1d0191ed6c32cc912efcb683155d465bdbfa0ff2 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseProviderPlugin.h
+++ b/source/RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseProviderPlugin.h
@@ -1,6 +1,6 @@
#pragma once
-#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/ComponentPlugin.h>
#include <RobotAPI/interface/objectpose/ObjectPoseProvider.h>
@@ -32,6 +32,8 @@ namespace armarx::plugins
}
+#include <ArmarXCore/core/ManagedIceObject.h>
+
namespace armarx
{
diff --git a/source/RobotAPI/libraries/ArmarXObjects/test/ArmarXObjectsTest.cpp b/source/RobotAPI/libraries/ArmarXObjects/test/ArmarXObjectsTest.cpp
index 0af92b11dcd764b7ab376f07a5869cb1962e5f4f..85ca31bec07bbe8af4108a64a24805da44dd19d5 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/test/ArmarXObjectsTest.cpp
+++ b/source/RobotAPI/libraries/ArmarXObjects/test/ArmarXObjectsTest.cpp
@@ -29,8 +29,80 @@
#include <iostream>
-BOOST_AUTO_TEST_CASE(testExample)
+#include <RobotAPI/libraries/ArmarXObjects/aron/ObjectPose.aron.generated.h>
+#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
+
+
+namespace fs = std::filesystem;
+
+
+BOOST_AUTO_TEST_SUITE(arondto_ObjectPose_test)
+
+
+BOOST_AUTO_TEST_CASE(test_ObjectType_copy_assignment)
+{
+ BOOST_TEST_MESSAGE("Constructor");
+ armarx::objpose::arondto::ObjectType lhs, rhs;
+
+ BOOST_TEST_MESSAGE("Assignment");
+ BOOST_CHECK_NO_THROW(lhs = rhs);
+
+ BOOST_TEST_MESSAGE("Done");
+}
+
+BOOST_AUTO_TEST_CASE(test_ObjectAttachmentInfo_copy_assignment)
+{
+ BOOST_TEST_MESSAGE("Constructor");
+ armarx::objpose::arondto::ObjectAttachmentInfo lhs, rhs;
+
+ BOOST_TEST_MESSAGE("Assignment");
+ BOOST_CHECK_NO_THROW(lhs = rhs);
+
+ BOOST_TEST_MESSAGE("Done");
+}
+
+BOOST_AUTO_TEST_CASE(test_ObjectPose_copy_assignment)
{
+ BOOST_TEST_MESSAGE("Constructor");
+ armarx::objpose::arondto::ObjectPose lhs, rhs;
- BOOST_CHECK_EQUAL(true, true);
+ BOOST_TEST_MESSAGE("Assignment");
+ BOOST_CHECK_NO_THROW(lhs = rhs);
+
+ BOOST_TEST_MESSAGE("Done");
+}
+
+
+BOOST_AUTO_TEST_SUITE_END()
+
+
+
+BOOST_AUTO_TEST_SUITE(ObjectFinderTest)
+
+
+BOOST_AUTO_TEST_CASE(test_find)
+{
+ using namespace armarx;
+
+ ObjectFinder finder;
+
+ bool checkPaths = false;
+ std::vector<ObjectInfo> objects = finder.findAllObjects(checkPaths);
+ BOOST_CHECK_GT(objects.size(), 0);
+
+ for (const ObjectInfo& object : objects)
+ {
+ fs::path simoxXML = object.simoxXML().absolutePath;
+ fs::path objectDir = simoxXML.parent_path();
+ BOOST_TEST_CONTEXT("Object: " << object.id() << " at " << objectDir)
+ {
+ BOOST_CHECK(fs::is_directory(objectDir));
+ BOOST_CHECK(fs::is_regular_file(simoxXML)
+ || fs::is_regular_file(object.articulatedSimoxXML().absolutePath));
+ }
+ }
}
+
+
+
+BOOST_AUTO_TEST_SUITE_END()
diff --git a/source/RobotAPI/libraries/CMakeLists.txt b/source/RobotAPI/libraries/CMakeLists.txt
index 5df6b028f0272c51a0538392375c27421d19dc06..02e5c638bfdfc145c271a995e016ce0c5f15ba01 100644
--- a/source/RobotAPI/libraries/CMakeLists.txt
+++ b/source/RobotAPI/libraries/CMakeLists.txt
@@ -22,7 +22,7 @@ add_subdirectory(armem_gui)
add_subdirectory(armem_objects)
add_subdirectory(armem_robot)
add_subdirectory(armem_robot_state)
-add_subdirectory(armem_robot_mapping)
+add_subdirectory(armem_vision)
add_subdirectory(armem_skills)
add_subdirectory(aron)
diff --git a/source/RobotAPI/libraries/ControllerUIUtility/CartesianWaypointControllerConfig/RemoteGui.cpp b/source/RobotAPI/libraries/ControllerUIUtility/CartesianWaypointControllerConfig/RemoteGui.cpp
index 92005dea9a32122b5e447d60436332286e1625dd..a568893eb8448825f471ceb44f31944e3bdab56b 100644
--- a/source/RobotAPI/libraries/ControllerUIUtility/CartesianWaypointControllerConfig/RemoteGui.cpp
+++ b/source/RobotAPI/libraries/ControllerUIUtility/CartesianWaypointControllerConfig/RemoteGui.cpp
@@ -155,28 +155,25 @@ namespace armarx::RemoteGui
.addChild(new RemoteGui::HSpacer)
);
}
-}
-namespace armarx::RemoteGui::detail::_getValue
-{
- void GetValueOutputParameter <CartesianWaypointControllerConfig, void>::call(
- CartesianWaypointControllerConfig& cfg,
- RemoteGui::ValueMap const& values, std::string const& name)
+
+ void getValueFromMap(CartesianWaypointControllerConfig& cfg,
+ RemoteGui::ValueMap const& values, std::string const& name)
{
- cfg.maxPositionAcceleration = getValue<float>(values, name + "_maxAcc_Pos");
- cfg.maxOrientationAcceleration = getValue<float>(values, name + "_maxAcc_Ori");
- cfg.maxNullspaceAcceleration = getValue<float>(values, name + "_maxAcc_Null");
+ getValueFromMap(cfg.maxPositionAcceleration, values, name + "_maxAcc_Pos");
+ getValueFromMap(cfg.maxOrientationAcceleration, values, name + "_maxAcc_Ori");
+ getValueFromMap(cfg.maxNullspaceAcceleration, values, name + "_maxAcc_Null");
- cfg.kpJointLimitAvoidance = getValue<float>(values, name + "_JointLimitAvoidance_KP");
- cfg.jointLimitAvoidanceScale = getValue<float>(values, name + "_JointLimitAvoidance_Scale");
+ getValueFromMap(cfg.kpJointLimitAvoidance, values, name + "_JointLimitAvoidance_KP");
+ getValueFromMap(cfg.jointLimitAvoidanceScale, values, name + "_JointLimitAvoidance_Scale");
- cfg.thresholdOrientationNear = getValue<float>(values, name + "_Thresholds_Ori_Near");
- cfg.thresholdOrientationReached = getValue<float>(values, name + "_Thresholds_Ori_Reached");
- cfg.thresholdPositionNear = getValue<float>(values, name + "_Thresholds_Pos_Near");
- cfg.thresholdPositionReached = getValue<float>(values, name + "_Thresholds_Pos_Reached");
+ getValueFromMap(cfg.thresholdOrientationNear, values, name + "_Thresholds_Ori_Near");
+ getValueFromMap(cfg.thresholdOrientationReached, values, name + "_Thresholds_Ori_Reached");
+ getValueFromMap(cfg.thresholdPositionNear, values, name + "_Thresholds_Pos_Near");
+ getValueFromMap(cfg.thresholdPositionReached, values, name + "_Thresholds_Pos_Reached");
- cfg.maxOriVel = getValue<float>(values, name + "_Max_vel_ori");
- cfg.maxPosVel = getValue<float>(values, name + "_Max_vel_pos");
- cfg.kpOri = getValue<float>(values, name + "_KP_ori");
- cfg.kpPos = getValue<float>(values, name + "_KP_pos");
+ getValueFromMap(cfg.maxOriVel, values, name + "_Max_vel_ori");
+ getValueFromMap(cfg.maxPosVel, values, name + "_Max_vel_pos");
+ getValueFromMap(cfg.kpOri, values, name + "_KP_ori");
+ getValueFromMap(cfg.kpPos, values, name + "_KP_pos");
}
}
diff --git a/source/RobotAPI/libraries/ControllerUIUtility/CartesianWaypointControllerConfig/RemoteGui.h b/source/RobotAPI/libraries/ControllerUIUtility/CartesianWaypointControllerConfig/RemoteGui.h
index ca43720fa07154d3067941a7f9ec496f2e43bd26..238ba2ae38f1f20aeba20604c62c86f1e432488b 100644
--- a/source/RobotAPI/libraries/ControllerUIUtility/CartesianWaypointControllerConfig/RemoteGui.h
+++ b/source/RobotAPI/libraries/ControllerUIUtility/CartesianWaypointControllerConfig/RemoteGui.h
@@ -31,15 +31,7 @@ namespace armarx::RemoteGui
detail::GroupBoxBuilder makeConfigGui(
const std::string& name,
const CartesianWaypointControllerConfig& val);
-}
-namespace armarx::RemoteGui::detail::_getValue
-{
- template<>
- struct GetValueOutputParameter <CartesianWaypointControllerConfig, void> : std::true_type
- {
- static void call(
- CartesianWaypointControllerConfig& cfg,
- RemoteGui::ValueMap const& values, std::string const& name);
- };
+ void getValueFromMap(CartesianWaypointControllerConfig& cfg,
+ RemoteGui::ValueMap const& values, std::string const& name);
}
diff --git a/source/RobotAPI/libraries/ControllerUIUtility/NJointCartesianWaypointControllerConfig/RemoteGui.cpp b/source/RobotAPI/libraries/ControllerUIUtility/NJointCartesianWaypointControllerConfig/RemoteGui.cpp
index fece7f9254cc646114adec271f5c931a92b47432..dc6aadb5ad0b72d8ebe19ad8e8f1b21b3a8b7d9b 100644
--- a/source/RobotAPI/libraries/ControllerUIUtility/NJointCartesianWaypointControllerConfig/RemoteGui.cpp
+++ b/source/RobotAPI/libraries/ControllerUIUtility/NJointCartesianWaypointControllerConfig/RemoteGui.cpp
@@ -49,16 +49,13 @@ namespace armarx::RemoteGui
return builder;
}
-}
-namespace armarx::RemoteGui::detail::_getValue
-{
- void GetValueOutputParameter <NJointCartesianWaypointControllerRuntimeConfig, void>::call(
- NJointCartesianWaypointControllerRuntimeConfig& cfg,
- RemoteGui::ValueMap const& values, std::string const& name)
+
+ void getValueFromMap(NJointCartesianWaypointControllerRuntimeConfig& cfg,
+ RemoteGui::ValueMap const& values, std::string const& name)
{
- getValue(cfg.wpCfg, values, name);
- getValue(cfg.forceThreshold, values, name + "_forceThreshold");
- getValue(cfg.forceThresholdInRobotRootZ, values, name + "_forceThresholdInRobotRootZ");
- getValue(cfg.optimizeNullspaceIfTargetWasReached, values, name + "_optimizeNullspaceIfTargetWasReached");
+ getValueFromMap(cfg.wpCfg, values, name);
+ getValueFromMap(cfg.forceThreshold, values, name + "_forceThreshold");
+ getValueFromMap(cfg.forceThresholdInRobotRootZ, values, name + "_forceThresholdInRobotRootZ");
+ getValueFromMap(cfg.optimizeNullspaceIfTargetWasReached, values, name + "_optimizeNullspaceIfTargetWasReached");
}
}
diff --git a/source/RobotAPI/libraries/ControllerUIUtility/NJointCartesianWaypointControllerConfig/RemoteGui.h b/source/RobotAPI/libraries/ControllerUIUtility/NJointCartesianWaypointControllerConfig/RemoteGui.h
index f06371d4fe96ec51fcafbf80fb9a999e86f697f3..f8b06a1a00c52963ff590e196ba95c97bdb3cefb 100644
--- a/source/RobotAPI/libraries/ControllerUIUtility/NJointCartesianWaypointControllerConfig/RemoteGui.h
+++ b/source/RobotAPI/libraries/ControllerUIUtility/NJointCartesianWaypointControllerConfig/RemoteGui.h
@@ -32,19 +32,7 @@ namespace armarx::RemoteGui
const std::string& name,
const NJointCartesianWaypointControllerRuntimeConfig& val);
- template <>
- NJointCartesianWaypointControllerRuntimeConfig getValue<NJointCartesianWaypointControllerRuntimeConfig>(
- ValueMap const& values,
- std::string const& name);
-}
-namespace armarx::RemoteGui::detail::_getValue
-{
- template<>
- struct GetValueOutputParameter <NJointCartesianWaypointControllerRuntimeConfig, void> : std::true_type
- {
- static void call(
- NJointCartesianWaypointControllerRuntimeConfig& cfg,
- RemoteGui::ValueMap const& values, std::string const& name);
- };
+ void getValueFromMap(NJointCartesianWaypointControllerRuntimeConfig& cfg,
+ RemoteGui::ValueMap const& values, std::string const& name);
}
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.cpp b/source/RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.cpp
index b0079227d85264a7aafea4cbf5a8795d1d606cc0..859389a12506d1a73734a9111dcae884164ea4d0 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.cpp
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.cpp
@@ -1,5 +1,8 @@
#include "ArVizComponentPlugin.h"
+#include <ArmarXCore/core/Component.h>
+
+
namespace armarx::plugins
{
std::string ArVizComponentPlugin::getTopicName()
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h b/source/RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h
index a4388cb604ea784494091be344b89320a317ad5b..6ce746eddb21cf676150d615cb2d9d57f2211d13 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h
@@ -1,6 +1,6 @@
#pragma once
-#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/ComponentPlugin.h>
#include <RobotAPI/components/ArViz/Client/Client.h>
@@ -27,6 +27,9 @@ namespace armarx::plugins
};
}
+
+#include <ArmarXCore/core/ManagedIceObject.h>
+
namespace armarx
{
/**
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/CartesianPositionControlComponentPlugin.cpp b/source/RobotAPI/libraries/RobotAPIComponentPlugins/CartesianPositionControlComponentPlugin.cpp
index 6b403e45ccfea923d1ead7b8e73fa5e7dbcf0de2..9d241c0c94832026b41a328aa215b7008e583db8 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/CartesianPositionControlComponentPlugin.cpp
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/CartesianPositionControlComponentPlugin.cpp
@@ -1,5 +1,8 @@
#include "CartesianPositionControlComponentPlugin.h"
+#include <ArmarXCore/core/Component.h>
+
+
namespace armarx
{
namespace plugins
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/CartesianPositionControlComponentPlugin.h b/source/RobotAPI/libraries/RobotAPIComponentPlugins/CartesianPositionControlComponentPlugin.h
index 0d53885cc44d3d0d42f4357c6c7b7cf0cfa7db5f..3a6dfebd06e39743a612ed9cfa3480fbd453d492 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/CartesianPositionControlComponentPlugin.h
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/CartesianPositionControlComponentPlugin.h
@@ -1,6 +1,6 @@
#pragma once
-#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/ComponentPlugin.h>
#include <RobotAPI/interface/components/CartesianPositionControlInterface.h>
@@ -30,6 +30,9 @@ namespace armarx
}
}
+
+#include <ArmarXCore/core/ManagedIceObject.h>
+
namespace armarx
{
/**
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/DebugDrawerHelperComponentPlugin.cpp b/source/RobotAPI/libraries/RobotAPIComponentPlugins/DebugDrawerHelperComponentPlugin.cpp
index 5fd0cd798b0e16567b57b443bff1381467a88a89..bd995800595c1715f71723f42a5c59dfee4dc063 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/DebugDrawerHelperComponentPlugin.cpp
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/DebugDrawerHelperComponentPlugin.cpp
@@ -21,9 +21,11 @@
*/
#include <ArmarXCore/util/CPPUtility/trace.h>
+#include <ArmarXCore/core/Component.h>
#include "DebugDrawerHelperComponentPlugin.h"
+
namespace armarx::plugins
{
DebugDrawerHelperComponentPlugin::DebugDrawerHelperComponentPlugin(ManagedIceObject& parent, std::string pre) :
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/DebugDrawerHelperComponentPlugin.h b/source/RobotAPI/libraries/RobotAPIComponentPlugins/DebugDrawerHelperComponentPlugin.h
index 1cf37ba70ec4000b61db4a3d8872e6871d6c7c2a..59ed86bb95d3c66aa02c6ad935066e8d97fc53e3 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/DebugDrawerHelperComponentPlugin.h
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/DebugDrawerHelperComponentPlugin.h
@@ -22,7 +22,7 @@
#pragma once
-#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/ComponentPlugin.h>
#include <RobotAPI/components/DebugDrawer/DebugDrawerHelper.h>
@@ -82,6 +82,9 @@ namespace armarx::plugins
};
}
+
+#include <ArmarXCore/core/ManagedIceObject.h>
+
namespace armarx
{
class DebugDrawerHelperComponentPluginUser : virtual public ManagedIceObject
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/FrameTrackingComponentPlugin.cpp b/source/RobotAPI/libraries/RobotAPIComponentPlugins/FrameTrackingComponentPlugin.cpp
index f5ba6417ad1ee3c5a683a33209678c3465c76277..1ca9b39590f7f49df239495cce72356223b70514 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/FrameTrackingComponentPlugin.cpp
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/FrameTrackingComponentPlugin.cpp
@@ -1,5 +1,8 @@
#include "FrameTrackingComponentPlugin.h"
+#include <ArmarXCore/core/Component.h>
+
+
namespace armarx
{
namespace plugins
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/FrameTrackingComponentPlugin.h b/source/RobotAPI/libraries/RobotAPIComponentPlugins/FrameTrackingComponentPlugin.h
index 82b1b593d1c457b1b2c636a292ee0ce8aebce477..8364375ea697ba9129d3699edc00c0dede740810 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/FrameTrackingComponentPlugin.h
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/FrameTrackingComponentPlugin.h
@@ -1,6 +1,6 @@
#pragma once
-#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/ComponentPlugin.h>
#include <RobotAPI/interface/components/FrameTrackingInterface.h>
@@ -30,6 +30,9 @@ namespace armarx
}
}
+
+#include <ArmarXCore/core/ManagedIceObject.h>
+
namespace armarx
{
/**
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/GraspCandidateObserverComponentPlugin.cpp b/source/RobotAPI/libraries/RobotAPIComponentPlugins/GraspCandidateObserverComponentPlugin.cpp
index 47494bcad7cba1f390206cbb513a12f8fe1fa838..6ab08acaee80d8896636f3c16c81bb38822365a5 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/GraspCandidateObserverComponentPlugin.cpp
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/GraspCandidateObserverComponentPlugin.cpp
@@ -1,5 +1,7 @@
#include <SimoxUtility/algorithm/string/string_tools.h>
+#include <ArmarXCore/core/Component.h>
+
#include "GraspCandidateObserverComponentPlugin.h"
namespace armarx
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/GraspCandidateObserverComponentPlugin.h b/source/RobotAPI/libraries/RobotAPIComponentPlugins/GraspCandidateObserverComponentPlugin.h
index e2f134ef78f79da7ea1e7298a969d984cd3a4123..a47f7877514702b1d4c719cb87947fea09ed5c90 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/GraspCandidateObserverComponentPlugin.h
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/GraspCandidateObserverComponentPlugin.h
@@ -1,6 +1,6 @@
#pragma once
-#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/ComponentPlugin.h>
#include <RobotAPI/interface/observers/GraspCandidateObserverInterface.h>
namespace armarx
@@ -36,6 +36,9 @@ namespace armarx
}
}
+
+#include <ArmarXCore/core/ManagedIceObject.h>
+
namespace armarx
{
/**
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/HandUnitComponentPlugin.cpp b/source/RobotAPI/libraries/RobotAPIComponentPlugins/HandUnitComponentPlugin.cpp
index c99b6a9c2dafb560e00e86c28eee0108b650c888..df3ed14e186ef4bced826f2e4cc9e581529ae5af 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/HandUnitComponentPlugin.cpp
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/HandUnitComponentPlugin.cpp
@@ -1,5 +1,8 @@
#include "HandUnitComponentPlugin.h"
+#include <ArmarXCore/core/Component.h>
+
+
namespace armarx
{
namespace plugins
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/HandUnitComponentPlugin.h b/source/RobotAPI/libraries/RobotAPIComponentPlugins/HandUnitComponentPlugin.h
index 4cd3593cb95abd83e023e8aa5642b68e23bea3fb..6cfe35ef754764c1c657ddefd7ec5bee811339c0 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/HandUnitComponentPlugin.h
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/HandUnitComponentPlugin.h
@@ -1,6 +1,6 @@
#pragma once
-#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/ComponentPlugin.h>
#include <RobotAPI/interface/units/HandUnitInterface.h>
@@ -30,6 +30,9 @@ namespace armarx
}
}
+
+#include <ArmarXCore/core/ManagedIceObject.h>
+
namespace armarx
{
/**
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/KinematicUnitComponentPlugin.cpp b/source/RobotAPI/libraries/RobotAPIComponentPlugins/KinematicUnitComponentPlugin.cpp
index ba8ba16d24d40bbb2312690ef87b4dd9f5619cd5..59dc918a1be3688d70795a5175c90ca28c5da099 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/KinematicUnitComponentPlugin.cpp
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/KinematicUnitComponentPlugin.cpp
@@ -1,5 +1,8 @@
#include "KinematicUnitComponentPlugin.h"
+#include <ArmarXCore/core/Component.h>
+
+
namespace armarx
{
namespace plugins
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/KinematicUnitComponentPlugin.h b/source/RobotAPI/libraries/RobotAPIComponentPlugins/KinematicUnitComponentPlugin.h
index e3b4062b59500f26314a6ccdb6e5480dd7d34a2b..2b3486826b6b84883e28c86598f3e05ea04c2674 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/KinematicUnitComponentPlugin.h
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/KinematicUnitComponentPlugin.h
@@ -1,6 +1,6 @@
#pragma once
-#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/ComponentPlugin.h>
#include <RobotAPI/interface/units/KinematicUnitInterface.h>
@@ -30,6 +30,9 @@ namespace armarx
}
}
+
+#include <ArmarXCore/core/ManagedIceObject.h>
+
namespace armarx
{
/**
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/NaturalIKComponentPlugin.cpp b/source/RobotAPI/libraries/RobotAPIComponentPlugins/NaturalIKComponentPlugin.cpp
index 008d497652fbd0888ef82eb1e5b99650bda11a25..fd592a9250cb0bb99caef834243ff4cf5bbaab23 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/NaturalIKComponentPlugin.cpp
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/NaturalIKComponentPlugin.cpp
@@ -1,5 +1,8 @@
#include "NaturalIKComponentPlugin.h"
+#include <ArmarXCore/core/Component.h>
+
+
namespace armarx
{
namespace plugins
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/NaturalIKComponentPlugin.h b/source/RobotAPI/libraries/RobotAPIComponentPlugins/NaturalIKComponentPlugin.h
index 0532416d50ae164baf5645d0cbc30ff50f3e9f48..c7e1c9cf61c21bfa986b3a0c827a6b371aba3048 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/NaturalIKComponentPlugin.h
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/NaturalIKComponentPlugin.h
@@ -1,6 +1,6 @@
#pragma once
-#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/ComponentPlugin.h>
#include <RobotAPI/interface/components/NaturalIKInterface.h>
@@ -30,6 +30,9 @@ namespace armarx
}
}
+
+#include <ArmarXCore/core/ManagedIceObject.h>
+
namespace armarx
{
/**
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/PlatformUnitComponentPlugin.cpp b/source/RobotAPI/libraries/RobotAPIComponentPlugins/PlatformUnitComponentPlugin.cpp
index f40b6cd34a5f5a8f028b714cc7a1517d590f2b3c..affec3b906414a2616c3e786aa0f88e6fa7b4d44 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/PlatformUnitComponentPlugin.cpp
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/PlatformUnitComponentPlugin.cpp
@@ -1,5 +1,8 @@
#include "PlatformUnitComponentPlugin.h"
+#include <ArmarXCore/core/Component.h>
+
+
namespace armarx
{
namespace plugins
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/PlatformUnitComponentPlugin.h b/source/RobotAPI/libraries/RobotAPIComponentPlugins/PlatformUnitComponentPlugin.h
index d2cec0f45b698f813d12a10920a122efbb63d00b..67b0edbc320bf724e321e2c32631b37ac5ebc92f 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/PlatformUnitComponentPlugin.h
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/PlatformUnitComponentPlugin.h
@@ -1,6 +1,6 @@
#pragma once
-#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/ComponentPlugin.h>
#include <RobotAPI/interface/units/PlatformUnitInterface.h>
@@ -30,6 +30,9 @@ namespace armarx
}
}
+
+#include <ArmarXCore/core/ManagedIceObject.h>
+
namespace armarx
{
/**
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.cpp b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.cpp
index 026024a9fb704181271f09f69ec4b94306a3d295..b9ad4f67a8e71d645fc5c868912a0ea9e271c1c9 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.cpp
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.cpp
@@ -19,11 +19,15 @@
* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
* GNU General Public License
*/
+
+#include <ArmarXCore/core/Component.h>
+
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
#include <RobotAPI/libraries/core/FramedPose.h>
#include "RobotStateComponentPlugin.h"
+
namespace armarx::plugins
{
const RobotNameHelper& RobotStateComponentPlugin::getRobotNameHelper() const
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.h b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.h
index d60ed34f0964b4dc8592c88b055fb32e7bab4608..d3a1ba42bd9a21279268f24780572957b1d80aa4 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.h
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.h
@@ -27,12 +27,13 @@
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/XML/RobotIO.h>
-#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/ComponentPlugin.h>
#include <RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.h>
#include <RobotAPI/interface/core/RobotState.h>
#include <RobotAPI/libraries/diffik/SimpleDiffIK.h>
+
namespace armarx::plugins
{
/**
@@ -158,6 +159,9 @@ namespace armarx::plugins
};
}
+
+#include <ArmarXCore/core/ManagedIceObject.h>
+
namespace armarx
{
class RobotStateComponentPluginUser : virtual public ManagedIceObject
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.cpp b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.cpp
index 7c120b3cc4db3aabbf6b184e5d747d3014ae0a61..4e6e7966cc4c71e819da47cd9a7ee06ccd536775 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.cpp
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.cpp
@@ -1,5 +1,11 @@
#include "RobotUnitComponentPlugin.h"
+#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/util/CPPUtility/Pointer.h>
+
+#include <thread>
+
+
namespace armarx::plugins
{
RobotUnitDataStreamingReceiverPtr
@@ -61,27 +67,25 @@ namespace armarx::plugins
void RobotUnitComponentPlugin::preOnInitComponent()
{
- if (!_deactivated)
+ if (_robotUnitName.empty())
+ {
+ parent<Component>().getProperty(_robotUnitName, PROPERTY_NAME);
+ }
+
+ if (not _isRobotUnitOptionalDependency)
{
- if (_robotUnitName.empty())
- {
- parent<Component>().getProperty(_robotUnitName, PROPERTY_NAME);
- }
parent<Component>().usingProxy(_robotUnitName);
}
}
void RobotUnitComponentPlugin::preOnConnectComponent()
{
- if (!_deactivated)
- {
- parent<Component>().getProxy(_robotUnit, _robotUnitName);
- }
+ parent<Component>().getProxy(_robotUnit, _robotUnitName);
}
void RobotUnitComponentPlugin::postCreatePropertyDefinitions(armarx::PropertyDefinitionsPtr& properties)
{
- if (!_deactivated && !properties->hasDefinition(PROPERTY_NAME))
+ if (!properties->hasDefinition(PROPERTY_NAME))
{
properties->defineRequiredProperty<std::string>(
PROPERTY_NAME,
@@ -96,13 +100,11 @@ namespace armarx::plugins
void RobotUnitComponentPlugin::setRobotUnitName(const std::string& name)
{
- if (!_deactivated)
- {
- ARMARX_CHECK_NOT_EMPTY(name);
- ARMARX_CHECK_EMPTY(_robotUnitName);
- _robotUnitName = name;
- }
+ ARMARX_CHECK_NOT_EMPTY(name);
+ ARMARX_CHECK_EMPTY(_robotUnitName);
+ _robotUnitName = name;
}
+
const std::string& RobotUnitComponentPlugin::getRobotUnitName() const
{
return _robotUnitName;
@@ -110,9 +112,13 @@ namespace armarx::plugins
void RobotUnitComponentPlugin::deactivate()
{
- _deactivated = true;
+ _isRobotUnitOptionalDependency = true;
}
-}
+
+
+
+
+} // namespace armarx::plugins
namespace armarx
{
@@ -130,6 +136,18 @@ namespace armarx
{
return *plugin;
}
+
+ void RobotUnitComponentPluginUser::waitUntilRobotUnitIsRunning(const std::function<bool()>& termCond) const
+ {
+ ARMARX_INFO << "Waiting until robot unit is running ...";
+
+ while ((not termCond()) and ((isNullptr(getRobotUnit()) or (not getRobotUnit()->isRunning()))))
+ {
+ std::this_thread::sleep_for(std::chrono::milliseconds(100));
+ }
+
+ ARMARX_INFO << "Robot unit is up and running.";
+ }
}
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.h b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.h
index 18616020e0683a70e6fcb0f95c7e7a09939db357..80357a96e9a3bb6c889d7b73435baef90a385b54 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.h
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.h
@@ -1,6 +1,6 @@
#pragma once
-#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/ComponentPlugin.h>
#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
#include <RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.h>
@@ -29,6 +29,12 @@ namespace armarx
void deactivate();
+ void setRobotUnitAsOptionalDependency(bool isOptional = true)
+ {
+
+ }
+
+
//controllers
public:
template<class PrxT>
@@ -63,12 +69,15 @@ namespace armarx
static constexpr const char* PROPERTY_NAME = "RobotUnitName";
RobotUnitInterfacePrx _robotUnit;
std::string _robotUnitName;
- bool _deactivated = false;
+ bool _isRobotUnitOptionalDependency = false;
std::set<std::string> _ctrls;
};
}
}
+
+#include <ArmarXCore/core/ManagedIceObject.h>
+
namespace armarx
{
/**
@@ -78,7 +87,25 @@ namespace armarx
{
public:
RobotUnitComponentPluginUser();
+
RobotUnitInterfacePrx getRobotUnit() const;
+
+ void setRobotUnitAsOptionalDependency(bool isOptional = true)
+ {
+ plugin->setRobotUnitAsOptionalDependency(isOptional);
+ }
+
+ /**
+ * @brief Waits until the robot unit is running.
+ *
+ * Although the robot unit proxy might be initialized (\see getRobotUnit()), the robot unit might
+ * not be fully initialized.
+ *
+ * @param termCond Termination condition. If it evaluates to true, waitUntilRobotUnitIsRunning returns without waiting
+ * for the robot unit to become available.
+ */
+ void waitUntilRobotUnitIsRunning(const std::function<bool()>& termCond = [] {return false;}) const;
+
plugins::RobotUnitComponentPlugin& getRobotUnitComponentPlugin();
private:
armarx::plugins::RobotUnitComponentPlugin* plugin = nullptr;
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitObserverComponentPlugin.cpp b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitObserverComponentPlugin.cpp
index 284533cf16e62834d85ff33e73d36b158943acef..0b5805c209fe40ab96eaafbeb2f573ca91a9ba9e 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitObserverComponentPlugin.cpp
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitObserverComponentPlugin.cpp
@@ -1,5 +1,8 @@
#include "RobotUnitObserverComponentPlugin.h"
+#include <ArmarXCore/core/Component.h>
+
+
namespace armarx
{
namespace plugins
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitObserverComponentPlugin.h b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitObserverComponentPlugin.h
index 0ebeff910700102871aa6229259e05f2efec866d..27b70a5064ad2e6be651e04b2cffe5f7d2d0360f 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitObserverComponentPlugin.h
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitObserverComponentPlugin.h
@@ -1,6 +1,6 @@
#pragma once
-#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/ComponentPlugin.h>
#include <ArmarXCore/interface/observers/ObserverInterface.h>
@@ -30,6 +30,9 @@ namespace armarx
}
}
+
+#include <ArmarXCore/core/ManagedIceObject.h>
+
namespace armarx
{
/**
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/TrajectoryPlayerComponentPlugin.cpp b/source/RobotAPI/libraries/RobotAPIComponentPlugins/TrajectoryPlayerComponentPlugin.cpp
index 1de30ae07c9f8218666121c28350f32c1ca1f5dc..28a6479b0ab5f844e5a2e68c14fdf84df66e3c67 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/TrajectoryPlayerComponentPlugin.cpp
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/TrajectoryPlayerComponentPlugin.cpp
@@ -1,5 +1,8 @@
#include "TrajectoryPlayerComponentPlugin.h"
+#include <ArmarXCore/core/Component.h>
+
+
namespace armarx
{
namespace plugins
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/TrajectoryPlayerComponentPlugin.h b/source/RobotAPI/libraries/RobotAPIComponentPlugins/TrajectoryPlayerComponentPlugin.h
index 3fc52df857f7a7225eacb25103dbd7fa16de4af5..28d57ca0488a8318ca154add6ec749b5380f8e50 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/TrajectoryPlayerComponentPlugin.h
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/TrajectoryPlayerComponentPlugin.h
@@ -1,6 +1,6 @@
#pragma once
-#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/ComponentPlugin.h>
#include <RobotAPI/interface/components/TrajectoryPlayerInterface.h>
@@ -30,6 +30,9 @@ namespace armarx
}
}
+
+#include <ArmarXCore/core/ManagedIceObject.h>
+
namespace armarx
{
/**
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp
index 945b5d69585de2af97f7237d0107de03861fdf8c..1c02bc7a4c1849e44e62ef2b88dc5163efc35cfa 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp
@@ -4,9 +4,12 @@
namespace armarx
{
- NJointControllerRegistration<NJointTaskSpaceImpedanceDMPController> registrationControllerNJointTaskSpaceImpedanceDMPController("NJointTaskSpaceImpedanceDMPController");
+ NJointControllerRegistration<NJointTaskSpaceImpedanceDMPController> registrationControllerNJointTaskSpaceImpedanceDMPController(
+ "NJointTaskSpaceImpedanceDMPController");
- NJointTaskSpaceImpedanceDMPController::NJointTaskSpaceImpedanceDMPController(const RobotUnitPtr& robotUnit, const armarx::NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&)
+ NJointTaskSpaceImpedanceDMPController::NJointTaskSpaceImpedanceDMPController(const RobotUnitPtr& robotUnit,
+ const armarx::NJointControllerConfigPtr& config,
+ const VirtualRobot::RobotPtr&)
{
ARMARX_INFO << "creating impedance dmp controller";
cfg = NJointTaskSpaceImpedanceDMPControllerConfigPtr::dynamicCast(config);
@@ -46,6 +49,8 @@ namespace armarx
tcp = rns->getTCP();
ik.reset(new VirtualRobot::DifferentialIK(rns, rtGetRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
+ ik->setDampedSvdLambda(0.0001);
+
numOfJoints = targets.size();
// set DMP
TaskSpaceDMPControllerConfig taskSpaceDMPConfig;
@@ -72,35 +77,35 @@ namespace armarx
isNullSpaceJointDMPLearned = false;
- defaultNullSpaceJointValues.resize(targets.size());
+ Eigen::VectorXf nullspaceValues(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);
+ nullspaceValues(i) = cfg->defaultNullSpaceJointValues.at(i);
}
-
+ defaultNullSpaceJointValues.reinitAllBuffers(nullspaceValues);
Eigen::Vector3f kpos(cfg->Kpos[0], cfg->Kpos[1], cfg->Kpos[2]);
Eigen::Vector3f dpos(cfg->Dpos[0], cfg->Dpos[1], cfg->Dpos[2]);
Eigen::Vector3f kori(cfg->Kori[0], cfg->Kori[1], cfg->Kori[2]);
Eigen::Vector3f dori(cfg->Dori[0], cfg->Dori[1], cfg->Dori[2]);
-
- CtrlParams initParams = {kpos, dpos, kori, dori};
- ctrlParams.reinitAllBuffers(initParams);
+ Eigen::VectorXf knull(targets.size());
+ Eigen::VectorXf dnull(targets.size());
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);
}
+ CtrlParams initParams = {kpos, dpos, kori, dori, knull, dnull};
+ ctrlParams.reinitAllBuffers(initParams);
+
torqueLimit = cfg->torqueLimit;
timeDuration = cfg->timeDuration;
@@ -133,7 +138,7 @@ namespace armarx
initData.targetPose = tcp->getPoseInRootFrame();
initData.targetVel.resize(6);
initData.targetVel.setZero();
- initData.desiredNullSpaceJointValues = defaultNullSpaceJointValues;
+ initData.desiredNullSpaceJointValues = defaultNullSpaceJointValues.getUpToDateReadBuffer();
reinitTripleBuffer(initData);
@@ -159,8 +164,8 @@ namespace armarx
if (!started)
{
- LockGuardType guard {controlDataMutex};
- getWriterControlStruct().desiredNullSpaceJointValues = defaultNullSpaceJointValues;
+ LockGuardType guard{controlDataMutex};
+ getWriterControlStruct().desiredNullSpaceJointValues = defaultNullSpaceJointValues.getUpToDateReadBuffer();
getWriterControlStruct().targetVel.setZero(6);
getWriterControlStruct().targetPose = currentPose;
getWriterControlStruct().canVal = 1.0;
@@ -172,8 +177,8 @@ namespace armarx
if (stopped)
{
- LockGuardType guard {controlDataMutex};
- getWriterControlStruct().desiredNullSpaceJointValues = defaultNullSpaceJointValues;
+ LockGuardType guard{controlDataMutex};
+ getWriterControlStruct().desiredNullSpaceJointValues = defaultNullSpaceJointValues.getUpToDateReadBuffer();
getWriterControlStruct().targetVel.setZero(6);
getWriterControlStruct().targetPose = oldPose;
getWriterControlStruct().canVal = dmpCtrl->canVal;
@@ -185,7 +190,7 @@ namespace armarx
if (dmpCtrl->canVal < 1e-8)
{
finished = true;
- LockGuardType guard {controlDataMutex};
+ LockGuardType guard{controlDataMutex};
getWriterControlStruct().targetVel.setZero();
writeControlStruct();
return;
@@ -197,7 +202,10 @@ namespace armarx
if (useNullSpaceJointDMP && isNullSpaceJointDMPLearned)
{
DMP::DVec targetJointState;
- currentJointState = nullSpaceJointDMPPtr->calculateDirectlyVelocity(currentJointState, dmpCtrl->canVal / timeDuration, deltaT / timeDuration, targetJointState);
+ currentJointState = nullSpaceJointDMPPtr->calculateDirectlyVelocity(currentJointState,
+ dmpCtrl->canVal / timeDuration,
+ deltaT / timeDuration,
+ targetJointState);
if (targetJointState.size() == jointNames.size())
{
@@ -208,15 +216,15 @@ namespace armarx
}
else
{
- desiredNullSpaceJointValues = defaultNullSpaceJointValues;
+ desiredNullSpaceJointValues = defaultNullSpaceJointValues.getUpToDateReadBuffer();
}
}
else
{
- desiredNullSpaceJointValues = defaultNullSpaceJointValues;
+ desiredNullSpaceJointValues = defaultNullSpaceJointValues.getUpToDateReadBuffer();
}
- LockGuardType guard {controlDataMutex};
+ LockGuardType guard{controlDataMutex};
getWriterControlStruct().desiredNullSpaceJointValues = desiredNullSpaceJointValues;
getWriterControlStruct().targetVel = dmpCtrl->getTargetVelocity();
getWriterControlStruct().targetPose = dmpCtrl->getTargetPoseMat();
@@ -228,7 +236,8 @@ namespace armarx
}
}
- void NJointTaskSpaceImpedanceDMPController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+ void NJointTaskSpaceImpedanceDMPController::rtRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration)
{
double deltaT = timeSinceLastIteration.toSecondsDouble();
Eigen::Matrix4f targetPose;
@@ -273,37 +282,40 @@ namespace armarx
jacobi.block(0, 0, 3, numOfJoints) = 0.001 * jacobi.block(0, 0, 3, numOfJoints); // convert mm to m
-
-
Eigen::Vector3f kpos = ctrlParams.getUpToDateReadBuffer().kpos;
Eigen::Vector3f dpos = ctrlParams.getUpToDateReadBuffer().dpos;
Eigen::Vector3f kori = ctrlParams.getUpToDateReadBuffer().kori;
Eigen::Vector3f dori = ctrlParams.getUpToDateReadBuffer().dori;
+ Eigen::VectorXf knull = ctrlParams.getUpToDateReadBuffer().knull;
+ Eigen::VectorXf dnull = ctrlParams.getUpToDateReadBuffer().dnull;
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);
+ 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 tcpDesiredForce = 0.001 * kpos.cwiseProduct(desiredPosition - currentTCPPosition) +
+ dpos.cwiseProduct(targetTCPLinearVelocity - currentTCPLinearVelocity);
Eigen::Vector3f currentTCPAngularVelocity;
- currentTCPAngularVelocity << currentTwist(3), currentTwist(4), currentTwist(5);
+ 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;
+ jointControlWrench << tcpDesiredForce, tcpDesiredTorque;
}
Eigen::MatrixXf I = Eigen::MatrixXf::Identity(targets.size(), targets.size());
- Eigen::VectorXf nullspaceTorque = knull.cwiseProduct(desiredNullSpaceJointValues - qpos) - dnull.cwiseProduct(qvel);
+ 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;
+ Eigen::VectorXf jointDesiredTorques =
+ jacobi.transpose() * jointControlWrench + (I - jacobi.transpose() * jtpinv) * nullspaceTorque;
@@ -321,7 +333,7 @@ namespace armarx
desiredTorque = 0;
}
- desiredTorque = (desiredTorque > torqueLimit) ? torqueLimit : desiredTorque;
+ desiredTorque = (desiredTorque > torqueLimit) ? torqueLimit : desiredTorque;
desiredTorque = (desiredTorque < -torqueLimit) ? -torqueLimit : desiredTorque;
debugOutputData.getWriteBuffer().desired_torques[jointNames[i]] = jointDesiredTorques(i);
@@ -330,7 +342,8 @@ namespace armarx
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;
+ ARMARX_INFO << deactivateSpam(1) << "Torque controller target is invalid - setting to zero! set value: "
+ << targets.at(i)->torque;
targets.at(i)->torque = 0.0f;
}
}
@@ -354,6 +367,7 @@ namespace armarx
debugOutputData.getWriteBuffer().targetPose_qx = targetQuat.x;
debugOutputData.getWriteBuffer().targetPose_qy = targetQuat.y;
debugOutputData.getWriteBuffer().targetPose_qz = targetQuat.z;
+ debugOutputData.getWriteBuffer().currentCanVal = rtGetControlStruct().canVal;
debugOutputData.getWriteBuffer().currentPose_x = currentPose(0, 3);
debugOutputData.getWriteBuffer().currentPose_y = currentPose(1, 3);
@@ -365,6 +379,26 @@ namespace armarx
debugOutputData.getWriteBuffer().currentPose_qz = currentQuat.z;
debugOutputData.getWriteBuffer().deltaT = deltaT;
+ debugOutputData.getWriteBuffer().currentKpos_x = kpos.x();
+ debugOutputData.getWriteBuffer().currentKpos_y = kpos.y();
+ debugOutputData.getWriteBuffer().currentKpos_z = kpos.z();
+ debugOutputData.getWriteBuffer().currentKori_x = kori.x();
+ debugOutputData.getWriteBuffer().currentKori_y = kori.y();
+ debugOutputData.getWriteBuffer().currentKori_z = kori.z();
+ debugOutputData.getWriteBuffer().currentKnull_x = knull.x();
+ debugOutputData.getWriteBuffer().currentKnull_y = knull.y();
+ debugOutputData.getWriteBuffer().currentKnull_z = knull.z();
+
+ debugOutputData.getWriteBuffer().currentDpos_x = dpos.x();
+ debugOutputData.getWriteBuffer().currentDpos_y = dpos.y();
+ debugOutputData.getWriteBuffer().currentDpos_z = dpos.z();
+ debugOutputData.getWriteBuffer().currentDori_x = dori.x();
+ debugOutputData.getWriteBuffer().currentDori_y = dori.y();
+ debugOutputData.getWriteBuffer().currentDori_z = dori.z();
+ debugOutputData.getWriteBuffer().currentDnull_x = dnull.x();
+ debugOutputData.getWriteBuffer().currentDnull_y = dnull.y();
+ debugOutputData.getWriteBuffer().currentDnull_z = dnull.z();
+
debugOutputData.commitWrite();
}
@@ -376,7 +410,8 @@ namespace armarx
ARMARX_INFO << "Learned DMP ... ";
}
- void NJointTaskSpaceImpedanceDMPController::setViaPoints(Ice::Double u, const Ice::DoubleSeq& viapoint, const Ice::Current&)
+ void NJointTaskSpaceImpedanceDMPController::setViaPoints(Ice::Double u, const Ice::DoubleSeq& viapoint,
+ const Ice::Current&)
{
LockGuardType guard(controllerMutex);
ARMARX_INFO << "setting via points ";
@@ -390,9 +425,11 @@ namespace armarx
}
- void NJointTaskSpaceImpedanceDMPController::learnJointDMPFromFiles(const std::string& fileName, const Ice::FloatSeq& currentJVS, const Ice::Current&)
+ void NJointTaskSpaceImpedanceDMPController::learnJointDMPFromFiles(const std::string& fileName,
+ const Ice::FloatSeq& currentJVS,
+ const Ice::Current&)
{
- DMP::Vec<DMP::SampledTrajectoryV2 > trajs;
+ DMP::Vec<DMP::SampledTrajectoryV2> trajs;
DMP::DVec ratios;
DMP::SampledTrajectoryV2 traj;
traj.readFromCSVFile(fileName);
@@ -444,8 +481,14 @@ namespace armarx
stopped = false;
}
+ void NJointTaskSpaceImpedanceDMPController::setUseNullSpaceJointDMP(bool enable, const Ice::Current&)
+ {
+ useNullSpaceJointDMP = enable;
+ }
+
- void NJointTaskSpaceImpedanceDMPController::runDMPWithTime(const Ice::DoubleSeq& goals, Ice::Double timeDuration, const Ice::Current&)
+ void NJointTaskSpaceImpedanceDMPController::runDMPWithTime(const Ice::DoubleSeq& goals, Ice::Double timeDuration,
+ const Ice::Current&)
{
dmpCtrl->canVal = timeDuration;
dmpCtrl->config.motionTimeDuration = timeDuration;
@@ -478,8 +521,8 @@ namespace armarx
}
-
- void NJointTaskSpaceImpedanceDMPController::onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx& debugObs)
+ void NJointTaskSpaceImpedanceDMPController::onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&,
+ const DebugObserverInterfacePrx& debugObs)
{
StringVariantBaseMap datafields;
auto values = debugOutputData.getUpToDateReadBuffer().desired_torques;
@@ -512,6 +555,26 @@ namespace armarx
datafields["currentPose_qy"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentPose_qy);
datafields["currentPose_qz"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentPose_qz);
+ datafields["currentKpos_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentKpos_x);
+ datafields["currentKpos_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentKpos_y);
+ datafields["currentKpos_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentKpos_z);
+ datafields["currentKori_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentKori_x);
+ datafields["currentKori_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentKori_y);
+ datafields["currentKori_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentKori_z);
+ datafields["currentKnull_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentKnull_x);
+ datafields["currentKnull_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentKnull_y);
+ datafields["currentKnull_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentKnull_z);
+
+ datafields["currentDpos_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentDpos_x);
+ datafields["currentDpos_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentDpos_y);
+ datafields["currentDpos_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentDpos_z);
+ datafields["currentDori_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentDori_x);
+ datafields["currentDori_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentDori_y);
+ datafields["currentDori_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentDori_z);
+ datafields["currentDnull_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentDnull_x);
+ datafields["currentDnull_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentDnull_y);
+ datafields["currentDnull_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentDnull_z);
+
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);
@@ -573,45 +636,76 @@ namespace armarx
void NJointTaskSpaceImpedanceDMPController::removeAllViaPoints(const Ice::Current&)
{
- LockGuardType guard {controllerMutex};
+ LockGuardType guard{controllerMutex};
ARMARX_INFO << "setting via points ";
dmpCtrl->removeAllViaPoints();
}
- void NJointTaskSpaceImpedanceDMPController::setLinearVelocityKd(const Ice::FloatSeq& kd, const Ice::Current&)
+ void NJointTaskSpaceImpedanceDMPController::setLinearVelocityKd(const Eigen::Vector3f& kd, const Ice::Current&)
{
ARMARX_CHECK_EQUAL(kd.size(), 3);
+ ARMARX_INFO << "set linear kd " << VAROUT(kd);
LockGuardType guard(controllerMutex);
- ctrlParams.getWriteBuffer().dpos << kd[0], kd[1], kd[2];
+ ctrlParams.getWriteBuffer().dpos = kd;
ctrlParams.commitWrite();
}
- void NJointTaskSpaceImpedanceDMPController::setLinearVelocityKp(const Ice::FloatSeq& kp, const Ice::Current&)
+ void NJointTaskSpaceImpedanceDMPController::setLinearVelocityKp(const Eigen::Vector3f& kp, const Ice::Current&)
{
ARMARX_CHECK_EQUAL(kp.size(), 3);
+ ARMARX_INFO << "set linear kp " << VAROUT(kp);
LockGuardType guard(controllerMutex);
- ctrlParams.getWriteBuffer().kpos << kp[0], kp[1], kp[2];
+ ctrlParams.getWriteBuffer().kpos = kp;
ctrlParams.commitWrite();
}
- void NJointTaskSpaceImpedanceDMPController::setAngularVelocityKd(const Ice::FloatSeq& kd, const Ice::Current&)
+ void NJointTaskSpaceImpedanceDMPController::setAngularVelocityKd(const Eigen::Vector3f& kd, const Ice::Current&)
{
ARMARX_CHECK_EQUAL(kd.size(), 3);
+ ARMARX_INFO << "set angular kd " << VAROUT(kd);
LockGuardType guard(controllerMutex);
- ctrlParams.getWriteBuffer().dori << kd[0], kd[1], kd[2];
+ ctrlParams.getWriteBuffer().dori = kd;
ctrlParams.commitWrite();
-
}
- void NJointTaskSpaceImpedanceDMPController::setAngularVelocityKp(const Ice::FloatSeq& kp, const Ice::Current&)
+ void NJointTaskSpaceImpedanceDMPController::setAngularVelocityKp(const Eigen::Vector3f& kp, const Ice::Current&)
{
ARMARX_CHECK_EQUAL(kp.size(), 3);
+ ARMARX_INFO << "set angular kp " << VAROUT(kp);
+ LockGuardType guard(controllerMutex);
+ ctrlParams.getWriteBuffer().kori = kp;
+ ctrlParams.commitWrite();
+
+
+ }
+
+ void NJointTaskSpaceImpedanceDMPController::setNullspaceVelocityKd(const Eigen::VectorXf& kd, const Ice::Current&)
+ {
+ ARMARX_CHECK_EQUAL(kd.size(), targets.size());
+ ARMARX_INFO << "set nullspace kd " << VAROUT(kd);
LockGuardType guard(controllerMutex);
- ctrlParams.getWriteBuffer().kori << kp[0], kp[1], kp[2];
+ ctrlParams.getWriteBuffer().dnull = kd;
ctrlParams.commitWrite();
+ }
+
+ void NJointTaskSpaceImpedanceDMPController::setNullspaceVelocityKp(const Eigen::VectorXf& kp, const Ice::Current&)
+ {
+ ARMARX_CHECK_EQUAL(kp.size(), targets.size());
+ ARMARX_INFO << "set linear kp " << VAROUT(kp);
+ LockGuardType guard(controllerMutex);
+ ctrlParams.getWriteBuffer().knull = kp;
+ ctrlParams.commitWrite();
+ }
+ void NJointTaskSpaceImpedanceDMPController::setDefaultNullSpaceJointValues(const Eigen::VectorXf& jointValues,
+ const Ice::Current&)
+ {
+ ARMARX_CHECK_EQUAL(jointValues.size(), targets.size());
+ defaultNullSpaceJointValues.getWriteBuffer() = jointValues;
+ defaultNullSpaceJointValues.commitWrite();
+
}
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.h
index d68faf54980d663be30cad8d6cd6e1e6555e6c2a..9c15961eb71a8977e1934ffbc2a0a89de94fa1c3 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.h
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.h
@@ -42,44 +42,48 @@ namespace armarx
NJointTaskSpaceImpedanceDMPController(const RobotUnitPtr& robotUnit, const NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&);
// NJointControllerInterface interface
- std::string getClassName(const Ice::Current&) const;
+ std::string getClassName(const Ice::Current&) const override;
// NJointController interface
- void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration);
+ void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
// NJointTaskSpaceImpedanceDMPControllerInterface interface
- void learnDMPFromFiles(const Ice::StringSeq& fileNames, const Ice::Current&);
- bool isFinished(const Ice::Current&)
+ void learnDMPFromFiles(const Ice::StringSeq& fileNames, const Ice::Current&) override;
+ bool isFinished(const Ice::Current&) override
{
return finished;
}
- void setViaPoints(Ice::Double u, const Ice::DoubleSeq& viapoint, const Ice::Current&);
- void setGoals(const Ice::DoubleSeq& goals, const Ice::Current&);
+ void setViaPoints(Ice::Double u, const Ice::DoubleSeq& viapoint, const Ice::Current&) override;
+ void setGoals(const Ice::DoubleSeq& goals, const Ice::Current&) override;
- 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&);
+ void learnJointDMPFromFiles(const std::string& fileName, const Ice::FloatSeq& currentJVS, const Ice::Current&) override;
+ void runDMP(const Ice::DoubleSeq& goals, const Ice::Current&) override;
+ void runDMPWithTime(const Ice::DoubleSeq& goals, Ice::Double timeDuration, const Ice::Current&) override;
- Ice::Double getVirtualTime(const Ice::Current&)
+ Ice::Double getVirtualTime(const Ice::Current&) override
{
return dmpCtrl->canVal;
}
- void stopDMP(const Ice::Current&);
- void resumeDMP(const Ice::Current&);
- void resetDMP(const Ice::Current&);
+ void stopDMP(const Ice::Current&) override;
+ void resumeDMP(const Ice::Current&) override;
+ void resetDMP(const Ice::Current&) override;
- void setMPWeights(const DoubleSeqSeq& weights, const Ice::Current&);
- DoubleSeqSeq getMPWeights(const Ice::Current&);
+ void setMPWeights(const DoubleSeqSeq& weights, const Ice::Current&) override;
+ DoubleSeqSeq getMPWeights(const Ice::Current&) override;
void removeAllViaPoints(const Ice::Current&) override;
- void setLinearVelocityKd(const Ice::FloatSeq& kd, const Ice::Current&) override;
- void setLinearVelocityKp(const Ice::FloatSeq& kp, const Ice::Current&)override;
- void setAngularVelocityKd(const Ice::FloatSeq& kd, const Ice::Current&)override;
- void setAngularVelocityKp(const Ice::FloatSeq& kp, const Ice::Current&)override;
+ void setLinearVelocityKd(const Eigen::Vector3f& kd, const Ice::Current&) override;
+ void setLinearVelocityKp(const Eigen::Vector3f& kp, const Ice::Current&) override;
+ void setAngularVelocityKd(const Eigen::Vector3f& kd, const Ice::Current&) override;
+ void setAngularVelocityKp(const Eigen::Vector3f& kp, const Ice::Current&) override;
+ void setNullspaceVelocityKd(const Eigen::VectorXf& kd, const Ice::Current&) override;
+ void setNullspaceVelocityKp(const Eigen::VectorXf& kp, const Ice::Current&) override;
+ void setUseNullSpaceJointDMP(bool enable, const Ice::Current&) override;
+ void setDefaultNullSpaceJointValues(const Eigen::VectorXf& jointValues, const Ice::Current&) override;
void enableForceStop(const Ice::Current&) override
{
@@ -94,10 +98,10 @@ namespace armarx
this->forceThreshold = forceThreshold;
}
protected:
- virtual void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&);
+ virtual void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&) override;
- void onInitNJointController();
- void onDisconnectNJointController();
+ void onInitNJointController() override;
+ void onDisconnectNJointController() override;
void controllerRun();
private:
@@ -121,6 +125,26 @@ namespace armarx
float currentPose_qy;
float currentPose_qz;
+ float currentKpos_x;
+ float currentKpos_y;
+ float currentKpos_z;
+ float currentKori_x;
+ float currentKori_y;
+ float currentKori_z;
+ float currentKnull_x;
+ float currentKnull_y;
+ float currentKnull_z;
+
+ float currentDpos_x;
+ float currentDpos_y;
+ float currentDpos_z;
+ float currentDori_x;
+ float currentDori_y;
+ float currentDori_z;
+ float currentDnull_x;
+ float currentDnull_y;
+ float currentDnull_z;
+
StringFloatDictionary desired_torques;
StringFloatDictionary desired_nullspaceJoint;
float forceDesired_x;
@@ -132,9 +156,11 @@ namespace armarx
float deltaT;
+
+
};
- TripleBuffer<DebugBufferData> debugOutputData;
+ WriteBufferedTripleBuffer<DebugBufferData> debugOutputData;
struct NJointTaskSpaceImpedanceDMPControllerSensorData
{
@@ -143,14 +169,14 @@ namespace armarx
Eigen::Matrix4f currentPose;
Eigen::VectorXf currentTwist;
};
- TripleBuffer<NJointTaskSpaceImpedanceDMPControllerSensorData> controllerSensorData;
+ WriteBufferedTripleBuffer<NJointTaskSpaceImpedanceDMPControllerSensorData> controllerSensorData;
struct NJointTaskSpaceImpedanceDMPControllerInterfaceData
{
Eigen::Matrix4f currentTcpPose;
};
- TripleBuffer<NJointTaskSpaceImpedanceDMPControllerInterfaceData> interfaceData;
+ WriteBufferedTripleBuffer<NJointTaskSpaceImpedanceDMPControllerInterfaceData> interfaceData;
struct CtrlParams
{
@@ -158,9 +184,11 @@ namespace armarx
Eigen::Vector3f dpos;
Eigen::Vector3f kori;
Eigen::Vector3f dori;
+ Eigen::VectorXf knull;
+ Eigen::VectorXf dnull;
};
- TripleBuffer<CtrlParams> ctrlParams;
+ WriteBufferedTripleBuffer<CtrlParams> ctrlParams;
@@ -198,15 +226,15 @@ namespace armarx
// Eigen::Vector3f kori;
// Eigen::Vector3f dpos;
// Eigen::Vector3f dori;
- Eigen::VectorXf knull;
- Eigen::VectorXf dnull;
+ // Eigen::VectorXf knull;
+ // Eigen::VectorXf dnull;
int numOfJoints;
- bool useNullSpaceJointDMP;
+ std::atomic_bool useNullSpaceJointDMP;
bool isNullSpaceJointDMPLearned;
- Eigen::VectorXf defaultNullSpaceJointValues;
+ WriteBufferedTripleBuffer<Eigen::VectorXf> defaultNullSpaceJointValues;
std::vector<std::string> jointNames;
mutable MutexType controllerMutex;
PeriodicTask<NJointTaskSpaceImpedanceDMPController>::pointer_type controllerTask;
diff --git a/source/RobotAPI/libraries/armem/CMakeLists.txt b/source/RobotAPI/libraries/armem/CMakeLists.txt
index eb83670b3cf4633d3fe2ce6bf52e61256a8d7931..38d9515cc25818652636ca4b8c5e969eaaf9519a 100644
--- a/source/RobotAPI/libraries/armem/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem/CMakeLists.txt
@@ -3,6 +3,22 @@ set(LIB_NAME armem)
armarx_component_set_name("${LIB_NAME}")
armarx_set_target("Library: ${LIB_NAME}")
+SET(INSTALL_SCRIPT_MSG
+ "Please use the installation script in RobotAPI/etc/mongocxx to install libmongocxx and libbsoncxx."
+)
+
+find_package(libmongocxx QUIET)
+armarx_build_if(libmongocxx_FOUND "libmongocxx not available. ${INSTALL_SCRIPT_MSG}")
+find_package(libbsoncxx QUIET)
+armarx_build_if(libbsoncxx_FOUND "libbsoncxx not available. ${INSTALL_SCRIPT_MSG}")
+
+
+#message("LIBMONGOCXX:")
+#message("${LIBMONGOCXX_FOUND}")
+#message("${LIBMONGOCXX_INCLUDE_DIRS}")
+#message("${LIBMONGOCXX_LIBRARIES}")
+
+
set(LIBS
ArmarXCoreInterfaces ArmarXCore
RemoteGui
@@ -14,10 +30,12 @@ set(LIBS
set(LIB_FILES
core/Commit.cpp
core/MemoryID.cpp
+ core/MemoryID_operators.cpp
core/SuccessHeader.cpp
core/Time.cpp
core/ice_conversions.cpp
core/aron_conversions.cpp
+ core/json_conversions.cpp
core/base/detail/MemoryItem.cpp
core/base/detail/MaxHistorySize.cpp
@@ -40,8 +58,11 @@ set(LIB_FILES
core/workingmemory/EntitySnapshot.cpp
core/workingmemory/Memory.cpp
core/workingmemory/ProviderSegment.cpp
- core/workingmemory/Visitor.cpp
core/workingmemory/ice_conversions.cpp
+ core/workingmemory/json_conversions.cpp
+ core/workingmemory/entityInstance_conversions.cpp
+ core/workingmemory/visitor/Visitor.cpp
+ core/workingmemory/visitor/FunctionalVisitor.cpp
core/longtermmemory/CoreSegment.cpp
core/longtermmemory/Entity.cpp
@@ -49,6 +70,7 @@ set(LIB_FILES
core/longtermmemory/EntitySnapshot.cpp
core/longtermmemory/Memory.cpp
core/longtermmemory/ProviderSegment.cpp
+ core/longtermmemory/mongodb/MongoDBConnectionManager.cpp
core/diskmemory/TypeIO.cpp
core/diskmemory/CoreSegment.cpp
@@ -59,13 +81,20 @@ set(LIB_FILES
core/diskmemory/ProviderSegment.cpp
core/error/ArMemError.cpp
+ core/error/mns.cpp
client/ComponentPlugin.cpp
+ client/MemoryNameSystem.cpp
+ client/MemoryNameSystemComponentPlugin.cpp
client/Reader.cpp
client/ReaderComponentPlugin.cpp
client/Writer.cpp
client/WriterComponentPlugin.cpp
+ client/util/MemoryListener.cpp
+ client/util/SimpleReaderBase.cpp
+ client/util/SimpleWriterBase.cpp
+
client/Query.cpp
client/query/Builder.cpp
client/query/selectors.cpp
@@ -94,7 +123,6 @@ set(LIB_FILES
server/query_proc/longtermmemory/MemoryQueryProcessor.cpp
mns/MemoryNameSystem.cpp
- mns/ClientPlugin.cpp
mns/ComponentPlugin.cpp
util/util.cpp
@@ -105,14 +133,17 @@ set(LIB_HEADERS
core/Commit.h
core/DataMode.h
core/MemoryID.h
+ core/MemoryID_operators.h
core/SuccessHeader.h
core/Time.h
core/aron_conversions.h
+ core/json_conversions.h
core/ice_conversions.h
core/ice_conversions_templates.h
core/error.h
core/error/ArMemError.h
+ core/error/mns.h
core/base/detail/MemoryItem.h
core/base/detail/MaxHistorySize.h
@@ -134,8 +165,12 @@ set(LIB_HEADERS
core/workingmemory/EntitySnapshot.h
core/workingmemory/Memory.h
core/workingmemory/ProviderSegment.h
- core/workingmemory/Visitor.h
core/workingmemory/ice_conversions.h
+ core/workingmemory/json_conversions.h
+ core/workingmemory/entityInstance_conversions.h
+ core/workingmemory/visitor.h
+ core/workingmemory/visitor/Visitor.h
+ core/workingmemory/visitor/FunctionalVisitor.h
core/longtermmemory/CoreSegment.h
core/longtermmemory/Entity.h
@@ -158,11 +193,14 @@ set(LIB_HEADERS
client.h
client/ComponentPlugin.h
+ client/MemoryNameSystem.h
+ client/MemoryNameSystemComponentPlugin.h
client/Reader.h
client/ReaderComponentPlugin.h
client/Writer.h
client/WriterComponentPlugin.h
+ client/query.h
client/Query.h
client/query/Builder.h
client/query/query_fns.h
@@ -170,6 +208,10 @@ set(LIB_HEADERS
client/query/detail/NameSelectorOps.h
client/query/detail/SelectorOps.h
+ client/util/MemoryListener.h
+ client/util/SimpleReaderBase.h
+ client/util/SimpleWriterBase.h
+
server.h
server/ComponentPlugin.h
server/MemoryToIceAdapter.h
@@ -197,26 +239,38 @@ set(LIB_HEADERS
mns.h
mns/MemoryNameSystem.h
- mns/ClientPlugin.h
mns/ComponentPlugin.h
util/util.h
)
-armarx_add_library("${LIB_NAME}" "${LIB_FILES}" "${LIB_HEADERS}" "${LIBS}")
-
-
-armarx_enable_aron_file_generation_for_target(
- TARGET_NAME
- ${LIB_NAME}
+armarx_add_library(
+ LIB_NAME
+ "${LIB_NAME}"
+ SOURCES
+ "${LIB_FILES}"
+ HEADERS
+ "${LIB_HEADERS}"
+ LIBS
+ "${LIBS}"
ARON_FILES
aron/MemoryID.xml
-)
+ )
+
+
+#armarx_enable_aron_file_generation_for_target(
+# TARGET_NAME
+# ${LIB_NAME}
+# ARON_FILES
+# aron/MemoryID.xml
+#)
add_library(RobotAPI::armem ALIAS "${LIB_NAME}")
+target_include_directories("${LIB_NAME}" PUBLIC ${LIBMONGOCXX_INCLUDE_DIRS})
+target_include_directories("${LIB_NAME}" PUBLIC ${LIBBSONCXX_INCLUDE_DIRS})
# add unit tests
add_subdirectory(test)
diff --git a/source/RobotAPI/libraries/armem/client.h b/source/RobotAPI/libraries/armem/client.h
index baf0ca232e5447048939aa062ee122a1d4f936b2..834d62ab45d88d18e4277929c47acd55c5825263 100644
--- a/source/RobotAPI/libraries/armem/client.h
+++ b/source/RobotAPI/libraries/armem/client.h
@@ -1,6 +1,8 @@
#pragma once
#include "client/ComponentPlugin.h"
+#include "client/MemoryNameSystem.h"
+#include "client/MemoryNameSystemComponentPlugin.h"
#include "client/Query.h"
#include "client/query/Builder.h"
#include "client/query/query_fns.h"
diff --git a/source/RobotAPI/libraries/armem/client/ComponentPlugin.cpp b/source/RobotAPI/libraries/armem/client/ComponentPlugin.cpp
index ba836f47f31f1dd5e21cb5101fbd7c4997ca4836..a68e742cd24489694da91ef73ff557a6e8be1294 100644
--- a/source/RobotAPI/libraries/armem/client/ComponentPlugin.cpp
+++ b/source/RobotAPI/libraries/armem/client/ComponentPlugin.cpp
@@ -21,20 +21,20 @@ armarx::armem::client::ComponentPluginUser::~ComponentPluginUser()
void
-armarx::armem::client::ComponentPluginUser::setMemory(server::MemoryInterfacePrx memory)
+armarx::armem::client::ComponentPluginUser::setMemoryServer(server::MemoryInterfacePrx memory)
{
- setReadingMemory(memory);
- setWritingMemory(memory);
+ setReadingMemoryServer(memory);
+ setWritingMemoryServer(memory);
}
armarx::armem::data::WaitForMemoryResult
-armarx::armem::client::ComponentPluginUser::useMemory(const std::string& memoryName)
+armarx::armem::client::ComponentPluginUser::useMemoryServer(const std::string& memoryName)
{
- armem::data::WaitForMemoryResult result = mns::plugins::ClientPluginUserBase::useMemory(memoryName);
+ armem::data::WaitForMemoryResult result = MemoryNameSystemComponentPluginUser::useMemoryServer(memoryName);
if (result.proxy)
{
- setMemory(result.proxy);
+ setMemoryServer(result.proxy);
}
return result;
}
diff --git a/source/RobotAPI/libraries/armem/client/ComponentPlugin.h b/source/RobotAPI/libraries/armem/client/ComponentPlugin.h
index 7fc432536b62411a912d7829bc40fd82efa5a855..3bdbde93bd046bcceb361d94183be5fd1c09db23 100644
--- a/source/RobotAPI/libraries/armem/client/ComponentPlugin.h
+++ b/source/RobotAPI/libraries/armem/client/ComponentPlugin.h
@@ -32,14 +32,15 @@ namespace armarx::armem::client
* @brief Resolve the given memory name, add it as dependency and update the readers and writers.
* @param The memory's name.
* @return The result of `waitForMemory()`.
- * @see `armem::mns::plugins::ClientPluginUserBase::useMemory()`
+ * @see `armem::MemoryNameSystemComponentPluginUser::useMemory()`
*/
- virtual armem::data::WaitForMemoryResult useMemory(const std::string& memoryName) override;
- using mns::plugins::ClientPluginUserBase::useMemory;
+ virtual armem::data::WaitForMemoryResult useMemoryServer(const std::string& memoryName) override;
+ using MemoryNameSystemComponentPluginUser::useMemoryServer;
+
protected:
- void setMemory(server::MemoryInterfacePrx memory);
+ void setMemoryServer(server::MemoryInterfacePrx memory);
};
diff --git a/source/RobotAPI/libraries/armem/client/MemoryNameSystem.cpp b/source/RobotAPI/libraries/armem/client/MemoryNameSystem.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..4395b55fd45e6bdb019ee1285fbc5cc3dac648f9
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/client/MemoryNameSystem.cpp
@@ -0,0 +1,368 @@
+#include "MemoryNameSystem.h"
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/ManagedIceObject.h>
+
+#include <RobotAPI/libraries/armem/core/error.h>
+#include <RobotAPI/libraries/armem/client/Reader.h>
+#include <RobotAPI/libraries/armem/client/Writer.h>
+
+#include <SimoxUtility/algorithm/string/string_tools.h>
+
+namespace armarx::armem::client
+{
+
+ MemoryNameSystem::MemoryNameSystem()
+ {
+ }
+
+
+ MemoryNameSystem::MemoryNameSystem(mns::MemoryNameSystemInterfacePrx mns, ManagedIceObject* component) :
+ mns(mns), component(component)
+ {
+ }
+
+
+ void MemoryNameSystem::update()
+ {
+ ARMARX_CHECK_NOT_NULL(mns);
+
+ data::GetAllRegisteredMemoriesResult result;
+ try
+ {
+ result = mns->getAllRegisteredMemories();
+ }
+ catch (const Ice::NotRegisteredException& e)
+ {
+ throw error::MemoryNameSystemQueryFailed(e.what());
+ }
+
+ if (result.success)
+ {
+ this->servers = result.proxies;
+ }
+ else
+ {
+ throw error::MemoryNameSystemQueryFailed(result.errorMessage);
+ }
+ }
+
+
+ server::MemoryInterfacePrx MemoryNameSystem::resolveServer(const MemoryID& memoryID)
+ {
+ if (auto it = servers.find(memoryID.memoryName); it != servers.end())
+ {
+ return it->second;
+ }
+ else
+ {
+ update();
+ if (auto it = servers.find(memoryID.memoryName); it != servers.end())
+ {
+ return it->second;
+ }
+ else
+ {
+ throw error::CouldNotResolveMemoryServer(memoryID);
+ }
+ }
+ }
+
+
+ server::MemoryInterfacePrx MemoryNameSystem::waitForServer(const MemoryID& memoryID, Time timeout)
+ {
+ if (auto it = servers.find(memoryID.memoryName); it != servers.end())
+ {
+ return it->second;
+ }
+ else
+ {
+ armem::data::WaitForMemoryInput input;
+ input.name = memoryID.memoryName;
+ input.timeoutMilliSeconds = timeout.toMilliSeconds();
+
+ ARMARX_CHECK_NOT_NULL(mns);
+ armem::data::WaitForMemoryResult result = mns->waitForMemory(input);
+ if (result.success)
+ {
+ return result.proxy;
+ }
+ else
+ {
+ throw error::CouldNotResolveMemoryServer(memoryID, result.errorMessage);
+ }
+ }
+ }
+
+ server::MemoryInterfacePrx MemoryNameSystem::useServer(const MemoryID& memoryID)
+ {
+ ARMARX_CHECK_NOT_NULL(component)
+ << "Owning component not set when using a memory server. \n"
+ << "When calling `armem::mns::MemoryNameSystem::useServer()`, the owning component which should "
+ << "receive the dependency to the memory server must be set beforehand. \n\n"
+ << "Use `armem::mns::MemoryNameSystem::setComponent()` or pass the component on construction "
+ << "before calling useServer().";
+ return useServer(memoryID, *component);
+ }
+
+
+ server::MemoryInterfacePrx MemoryNameSystem::useServer(const MemoryID& memoryID, ManagedIceObject& component)
+ {
+ server::MemoryInterfacePrx server = waitForServer(memoryID);
+ // Add dependency.
+ component.usingProxy(server->ice_getIdentity().name);
+ return server;
+ }
+
+
+ server::MemoryInterfacePrx MemoryNameSystem::useServer(const std::string& memoryName)
+ {
+ return useServer(MemoryID().withMemoryName(memoryName));
+ }
+
+
+ server::MemoryInterfacePrx MemoryNameSystem::useServer(const std::string& memoryName, ManagedIceObject& component)
+ {
+ return useServer(MemoryID().withMemoryName(memoryName), component);
+ }
+
+
+ Reader MemoryNameSystem::getReader(const MemoryID& memoryID)
+ {
+ return Reader(resolveServer(memoryID));
+ }
+
+
+ Reader MemoryNameSystem::useReader(const MemoryID& memoryID)
+ {
+ return Reader(useServer(memoryID));
+ }
+
+
+ Reader MemoryNameSystem::useReader(const MemoryID& memoryID, ManagedIceObject& component)
+ {
+ return Reader(useServer(memoryID, component));
+ }
+
+
+ Reader MemoryNameSystem::useReader(const std::string& memoryName)
+ {
+ return useReader(MemoryID().withMemoryName(memoryName));
+ }
+
+
+ Reader MemoryNameSystem::useReader(const std::string& memoryName, ManagedIceObject& component)
+ {
+ return useReader(MemoryID().withMemoryName(memoryName), component);
+ }
+
+
+ template <class ClientT>
+ std::map<std::string, ClientT> MemoryNameSystem::_getAllClients() const
+ {
+ std::map<std::string, ClientT> result;
+ for (const auto& [name, server] : servers)
+ {
+ result[name] = ClientT(server);
+ }
+ return result;
+ }
+
+
+ template <class ClientT>
+ std::map<std::string, ClientT> MemoryNameSystem::_getAllClients(bool update)
+ {
+ if (update)
+ {
+ this->update();
+ }
+ return const_cast<const MemoryNameSystem&>(*this)._getAllClients<ClientT>();
+ }
+
+
+ std::map<std::string, Reader> MemoryNameSystem::getAllReaders(bool update)
+ {
+ return _getAllClients<Reader>(update);
+ }
+
+
+ std::map<std::string, Reader> MemoryNameSystem::getAllReaders() const
+ {
+ return _getAllClients<Reader>();
+ }
+
+
+ Writer MemoryNameSystem::getWriter(const MemoryID& memoryID)
+ {
+ return Writer(resolveServer(memoryID));
+ }
+
+
+ Writer MemoryNameSystem::useWriter(const MemoryID& memoryID)
+ {
+ return Writer(useServer(memoryID));
+ }
+
+
+ Writer MemoryNameSystem::useWriter(const MemoryID& memoryID, ManagedIceObject& component)
+ {
+ return Writer(useServer(memoryID, component));
+ }
+
+
+ Writer MemoryNameSystem::useWriter(const std::string& memoryName)
+ {
+ return useWriter(MemoryID().withMemoryName(memoryName));
+ }
+
+
+ Writer MemoryNameSystem::useWriter(const std::string& memoryName, ManagedIceObject& component)
+ {
+ return useWriter(MemoryID().withMemoryName(memoryName), component);
+ }
+
+
+ std::map<std::string, Writer> MemoryNameSystem::getAllWriters(bool update)
+ {
+ return _getAllClients<Writer>(update);
+ }
+
+
+ std::map<std::string, Writer> MemoryNameSystem::getAllWriters() const
+ {
+ return _getAllClients<Writer>();
+ }
+
+
+ std::optional<wm::EntityInstance> MemoryNameSystem::resolveEntityInstance(const MemoryID& id)
+ {
+ auto result = resolveEntityInstances({id});
+ if (result.size() > 0)
+ {
+ return result.begin()->second;
+ }
+ else
+ {
+ return std::nullopt;
+ }
+ }
+
+
+ std::map<MemoryID, wm::EntityInstance> MemoryNameSystem::resolveEntityInstances(const std::vector<MemoryID>& ids)
+ {
+ std::stringstream errors;
+ int errorCounter = 0;
+
+ std::map<std::string, std::vector<MemoryID>> idsPerMemory;
+ for (const auto& id : ids)
+ {
+ idsPerMemory[id.memoryName].push_back(id);
+ }
+
+ std::map<MemoryID, wm::EntityInstance> result;
+ for (const auto& [memoryName, ids] : idsPerMemory)
+ {
+ Reader reader = getReader(MemoryID().withMemoryName(memoryName));
+ QueryResult queryResult = reader.queryMemoryIDs(ids);
+ if (queryResult.success)
+ {
+ for (const MemoryID& id : ids)
+ {
+ try
+ {
+ if (id.hasInstanceIndex())
+ {
+ result[id] = queryResult.memory.getEntityInstance(id);
+ }
+ else if (id.hasTimestamp())
+ {
+ result[id] = queryResult.memory.getEntitySnapshot(id).getInstance(0);
+ }
+ else if (id.hasEntityName())
+ {
+ result[id] = queryResult.memory.getEntity(id).getLatestSnapshot().getInstance(0);
+ }
+ else
+ {
+ std::stringstream ss;
+ ss << "MemoryNameSystem::" << __FUNCTION__ << "requires IDs to be entity, snapshot or instance IDs,"
+ << "but ID has no entity name.";
+ throw error::InvalidMemoryID(id, ss.str());
+ }
+ }
+ catch (const error::ArMemError& e)
+ {
+ errors << "\n#" << ++errorCounter << "\n"
+ << "Failed to retrieve " << id << " from query result: \n" << e.what();
+ }
+ }
+ }
+ else
+ {
+ errors << "\n# " << ++errorCounter << "\n"
+ << "Failed to query '" << memoryName << "': \n" << queryResult.errorMessage;
+ }
+ }
+
+ if (errors.str().size() > 0)
+ {
+ ARMARX_INFO << "MemoryNameSystem::" << __FUNCTION__ << ": The following errors may affect your result: "
+ << "\n\n" << errors.str() << "\n\n"
+ << "When querying entity instances: \n- "
+ << simox::alg::join(simox::alg::multi_to_string(ids), "\n- ");
+ }
+
+ return result;
+ }
+
+
+ void MemoryNameSystem::registerServer(const MemoryID& memoryID, server::MemoryInterfacePrx proxy)
+ {
+ data::RegisterMemoryInput input;
+ input.name = memoryID.memoryName;
+ input.proxy = proxy;
+ ARMARX_CHECK_NOT_NULL(input.proxy);
+
+ ARMARX_CHECK_NOT_NULL(mns);
+ data::RegisterMemoryResult result = mns->registerMemory(input);
+ if (!result.success)
+ {
+ throw error::ServerRegistrationOrRemovalFailed("register", memoryID, result.errorMessage);
+ }
+ }
+
+
+ void MemoryNameSystem::removeServer(const MemoryID& memoryID)
+ {
+ data::RemoveMemoryInput input;
+ input.name = memoryID.memoryName;
+
+ ARMARX_CHECK_NOT_NULL(mns);
+ data::RemoveMemoryResult result = mns->removeMemory(input);
+ if (!result.success)
+ {
+ throw error::ServerRegistrationOrRemovalFailed("remove", memoryID, result.errorMessage);
+ }
+ }
+
+
+ mns::MemoryNameSystemInterfacePrx MemoryNameSystem::getMemoryNameSystem() const
+ {
+ return mns;
+ }
+
+
+ void MemoryNameSystem::getMemoryNameSystem(mns::MemoryNameSystemInterfacePrx mns)
+ {
+ this->mns = mns;
+ }
+
+ void MemoryNameSystem::setComponent(ManagedIceObject* component)
+ {
+ this->component = component;
+ }
+
+}
+
+
+
diff --git a/source/RobotAPI/libraries/armem/client/MemoryNameSystem.h b/source/RobotAPI/libraries/armem/client/MemoryNameSystem.h
new file mode 100644
index 0000000000000000000000000000000000000000..d981bab0383f0d04fdc221fbbd2cbd062d061c53
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/client/MemoryNameSystem.h
@@ -0,0 +1,270 @@
+/*
+ * 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::libraries::armem
+ * @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
+ * @date 2020
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <map>
+#include <optional>
+
+#include <RobotAPI/interface/armem/mns/MemoryNameSystemInterface.h>
+#include <RobotAPI/interface/armem/server/MemoryInterface.h>
+
+#include <RobotAPI/libraries/armem/core/MemoryID.h>
+#include <RobotAPI/libraries/armem/client/util/MemoryListener.h>
+
+
+namespace armarx
+{
+ class ManagedIceObject;
+}
+namespace armarx::armem::client
+{
+ class Reader;
+ class Writer;
+}
+
+namespace armarx::armem::wm
+{
+ class EntityInstance;
+}
+
+namespace armarx::armem::client
+{
+
+ /**
+ * @brief The memory name system (MNS) client.
+ *
+ * This client class serves provides the MNS interface and is a local cache
+ * of the MNS registry. It can be used to resolve memory servers by their
+ * memory ID and to construct `client::Readers` and `client::Writers`.
+ *
+ * During server resolution, it first consults the locally cached registry.
+ * If the memory server is not known locally, the local registry is
+ * updated to that of the remote MNS before trying again.
+ *
+ * In addition, the MNS client can be used to send queries over multiple
+ * memory servers, as well as retrieving the data for arbitrary entity or
+ * entity snapshot IDs.
+ */
+ class MemoryNameSystem : public util::MemoryListener
+ {
+ public:
+
+ MemoryNameSystem();
+
+ /**
+ * @brief Construct an MNS client.
+ *
+ * @param mns The MNS proxy.
+ * @param component The owning component. When `using` a memory server,
+ * dependencies will be added to this component.
+ */
+ MemoryNameSystem(mns::MemoryNameSystemInterfacePrx mns,
+ ManagedIceObject* component = nullptr);
+
+
+ mns::MemoryNameSystemInterfacePrx getMemoryNameSystem() const;
+ void getMemoryNameSystem(mns::MemoryNameSystemInterfacePrx mns);
+
+ void setComponent(ManagedIceObject* component);
+
+
+ // Name Resolution
+
+ /**
+ * @brief Update the internal registry to the data in the MNS.
+ *
+ * @throw `error::MemoryNameSystemQueryFailed` If the call to the MNS failed.
+ */
+ void update();
+
+ /**
+ * @brief Resolve the given memory server for the given memory ID.
+ *
+ * @param memoryID The memory ID.
+ * @return The memory server proxy.
+ *
+ * @throw `error::CouldNotResolveMemoryServer` If the memory name could not be resolved.
+ */
+ server::MemoryInterfacePrx resolveServer(const MemoryID& memoryID);
+
+ /**
+ * @brief Wait for the given memory server.
+ *
+ * @param memoryID The memory ID.
+ * @param timeout How long to wait at maximum. Negative values indicate infinite wait.
+ * @return The memory server proxy.
+ *
+ * @throw `error::CouldNotResolveMemoryServer` If the memory name could not be resolved.
+ */
+ server::MemoryInterfacePrx waitForServer(const MemoryID& memoryID, Time timeout = Time::milliSeconds(-1));
+
+ /**
+ * @brief Wait for the given memory server and add a dependency to the memory server.
+ *
+ * @param memoryID The memory ID.
+ * @param component The component that should depend on the memory server.
+ * @return The memory server proxy.
+ *
+ * @throw `error::CouldNotResolveMemoryServer` If the memory name could not be resolved.
+ */
+ server::MemoryInterfacePrx useServer(const MemoryID& memoryID);
+ server::MemoryInterfacePrx useServer(const MemoryID& memoryID, ManagedIceObject& component);
+ server::MemoryInterfacePrx useServer(const std::string& memoryName);
+ server::MemoryInterfacePrx useServer(const std::string& memoryName, ManagedIceObject& component);
+
+
+ // Reader/Writer construction
+
+ /**
+ * @brief Get a reader to the given memory name.
+ *
+ * @param memoryID The memory ID.
+ * @return The reader.
+ *
+ * @throw `error::CouldNotResolveMemoryServer` If the memory name could not be resolved.
+ */
+ Reader getReader(const MemoryID& memoryID);
+
+ /// Use a memory server and get a reader for it.
+ Reader useReader(const MemoryID& memoryID);
+ Reader useReader(const MemoryID& memoryID, ManagedIceObject& component);
+ Reader useReader(const std::string& memoryName);
+ Reader useReader(const std::string& memoryName, ManagedIceObject& component);
+
+ /**
+ * @brief Get Readers for all registered servers.
+ *
+ * @param update If true, perform an update first.
+ * @return The readers.
+ */
+ std::map<std::string, Reader> getAllReaders(bool update = true);
+ /**
+ * @brief Get Readers for all registered servers (without updating).
+ *
+ * @return The readers.
+ */
+ std::map<std::string, Reader> getAllReaders() const;
+
+
+ /**
+ * @brief Get a writer to the given memory name.
+ *
+ * @param memoryID The memory ID.
+ * @return The writer.
+ *
+ * @throw `error::CouldNotResolveMemoryServer` If the memory name could not be resolved.
+ */
+ Writer getWriter(const MemoryID& memoryID);
+
+ /// Use a memory server and get a writer for it.
+ Writer useWriter(const MemoryID& memoryID);
+ Writer useWriter(const MemoryID& memoryID, ManagedIceObject& component);
+ Writer useWriter(const std::string& memoryName);
+ Writer useWriter(const std::string& memoryName, ManagedIceObject& component);
+
+ /**
+ * @brief Get Writers for all registered servers.
+ *
+ * @param update If true, perform an update first.
+ * @return The readers.
+ */
+ std::map<std::string, Writer> getAllWriters(bool update = true);
+ /**
+ * @brief Get Writers for all registered servers (without updating).
+ *
+ * @return The readers.
+ */
+ std::map<std::string, Writer> getAllWriters() const;
+
+
+ // ToDo: commit() and query()
+
+ /**
+ * @brief Resolve a memory ID to an EntityInstance.
+ *
+ * The ID can refer to an entity, an entity snapshot, or an entity
+ * instance. When not referring to an entity instance, the latest
+ * snapshot and first instance will be queried.
+ *
+ * @param id The entity, snapshot or instance ID.
+ * @return
+ */
+ std::optional<wm::EntityInstance> resolveEntityInstance(const MemoryID& id);
+
+ std::map<MemoryID, wm::EntityInstance> resolveEntityInstances(const std::vector<MemoryID>& ids);
+
+
+
+ // Registration - only for memory servers
+
+ /**
+ * @brief Register a memory server in the MNS.
+ *
+ * @param memoryID The memoryID.
+ * @param server The memory server proxy.
+ *
+ * @throw `error::ServerRegistrationOrRemovalFailed` If the registration failed.
+ */
+ void registerServer(const MemoryID& memoryID, server::MemoryInterfacePrx server);
+
+ /**
+ * @brief Remove a memory server from the MNS.
+ *
+ * @param memoryID The memoryID.
+ *
+ * @throw `error::ServerRegistrationOrRemovalFailed` If the removal failed.
+ */
+ void removeServer(const MemoryID& memoryID);
+
+
+
+ // Operators
+
+ /// Indicate whether the proxy is set.
+ inline operator bool() const
+ {
+ return bool(mns);
+ }
+
+
+ private:
+
+ template <class ClientT>
+ std::map<std::string, ClientT> _getAllClients(bool update);
+ template <class ClientT>
+ std::map<std::string, ClientT> _getAllClients() const;
+
+
+ /// The MNS proxy.
+ mns::MemoryNameSystemInterfacePrx mns = nullptr;
+
+ /// The component to which dependencies will be added.
+ ManagedIceObject* component = nullptr;
+
+ /// The registered memory servers.
+ std::map<std::string, server::MemoryInterfacePrx> servers;
+
+ };
+
+
+}
diff --git a/source/RobotAPI/libraries/armem/client/MemoryNameSystemComponentPlugin.cpp b/source/RobotAPI/libraries/armem/client/MemoryNameSystemComponentPlugin.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..60fae2cf9b37e450fcfbaee806af8115f496c678
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/client/MemoryNameSystemComponentPlugin.cpp
@@ -0,0 +1,154 @@
+#include "MemoryNameSystemComponentPlugin.h"
+
+#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+#include <RobotAPI/libraries/armem/core/error.h>
+#include <RobotAPI/libraries/armem/core/ice_conversions.h>
+
+
+
+namespace armarx::armem::client::plugins
+{
+
+ MemoryNameSystemComponentPlugin::~MemoryNameSystemComponentPlugin()
+ {}
+
+
+ void MemoryNameSystemComponentPlugin::postCreatePropertyDefinitions(armarx::PropertyDefinitionsPtr& properties)
+ {
+ if (!properties->hasDefinition(makePropertyName(PROPERTY_MNS_NAME_NAME)))
+ {
+ properties->defineOptionalProperty<std::string>(
+ makePropertyName(PROPERTY_MNS_NAME_NAME),
+ parent<MemoryNameSystemComponentPluginUser>().memoryNameSystemName,
+ "Name of the Memory Name System (MNS) component.");
+ }
+ if (!properties->hasDefinition(makePropertyName(PROPERTY_MNS_ENABLED_NAME)))
+ {
+ properties->defineOptionalProperty<bool>(
+ makePropertyName(PROPERTY_MNS_ENABLED_NAME),
+ parent<MemoryNameSystemComponentPluginUser>().memoryNameSystemEnabled,
+ "Whether to use (and depend on) the Memory Name System (MNS)."
+ "\nSet to false to use this memory as a stand-alone.");
+ }
+
+ // Subscribe topics by single servers, use this as a prefix.
+ properties->topic<MemoryListenerInterface>("MemoryUpdates");
+ }
+
+ void MemoryNameSystemComponentPlugin::preOnInitComponent()
+ {
+ if (isMemoryNameSystemEnabled())
+ {
+ parent().usingProxy(getMemoryNameSystemName());
+ }
+ }
+
+ void MemoryNameSystemComponentPlugin::preOnConnectComponent()
+ {
+ if (isMemoryNameSystemEnabled())
+ {
+ ARMARX_DEBUG << "Creating MemoryNameSystem client with owning component '" << parent().getName() << "'.";
+ parent<MemoryNameSystemComponentPluginUser>().memoryNameSystem =
+ MemoryNameSystem(getMemoryNameSystem(), &parent());
+ }
+ }
+
+ bool MemoryNameSystemComponentPlugin::isMemoryNameSystemEnabled()
+ {
+ return parentDerives<Component>() ?
+ parent<Component>().getProperty<bool>(makePropertyName(PROPERTY_MNS_ENABLED_NAME)) :
+ parent<MemoryNameSystemComponentPluginUser>().memoryNameSystemEnabled;
+ }
+
+ std::string MemoryNameSystemComponentPlugin::getMemoryNameSystemName()
+ {
+ return parentDerives<Component>() ?
+ parent<Component>().getProperty<std::string>(makePropertyName(PROPERTY_MNS_NAME_NAME)) :
+ std::string{parent<MemoryNameSystemComponentPluginUser>().memoryNameSystemName};
+ }
+
+ mns::MemoryNameSystemInterfacePrx MemoryNameSystemComponentPlugin::getMemoryNameSystem()
+ {
+ return isMemoryNameSystemEnabled() && parentDerives<Component>()
+ ? parent<Component>().getProxy<mns::MemoryNameSystemInterfacePrx>(getMemoryNameSystemName())
+ : nullptr;
+ }
+
+}
+
+namespace armarx::armem::client
+{
+
+ MemoryNameSystemComponentPluginUser::MemoryNameSystemComponentPluginUser()
+ {
+ addPlugin(plugin);
+ }
+
+ MemoryNameSystemComponentPluginUser::~MemoryNameSystemComponentPluginUser()
+ {
+ }
+
+ void MemoryNameSystemComponentPluginUser::memoryUpdated(
+ const std::vector<data::MemoryID>& updatedSnapshotIDs, const Ice::Current&)
+ {
+ memoryNameSystem.updated(updatedSnapshotIDs);
+ }
+
+ armem::data::WaitForMemoryResult MemoryNameSystemComponentPluginUser::useMemoryServer(const MemoryID& id)
+ {
+ armem::data::WaitForMemoryResult result;
+ try
+ {
+ // Add dependency.
+ result.proxy = memoryNameSystem.useServer(id);
+ result.success = true;
+ }
+ catch (const armem::error::CouldNotResolveMemoryServer& e)
+ {
+ result.success = false;
+ result.errorMessage = e.what();
+ }
+ return result;
+ }
+
+ armem::data::WaitForMemoryResult MemoryNameSystemComponentPluginUser::useMemoryServer(const std::string& memoryName)
+ {
+ return useMemoryServer(MemoryID().withMemoryName(memoryName));
+ }
+
+ armem::data::WaitForMemoryResult MemoryNameSystemComponentPluginUser::waitForMemoryServer(const std::string& memoryName)
+ {
+ armem::data::WaitForMemoryResult result;
+ try
+ {
+ result.proxy = memoryNameSystem.waitForServer(MemoryID().withMemoryName(memoryName));
+ result.success = true;
+ }
+ catch (const armem::error::CouldNotResolveMemoryServer& e)
+ {
+ result.success = false;
+ result.errorMessage = e.what();
+ }
+ return result;
+ }
+
+ armem::data::ResolveMemoryNameResult MemoryNameSystemComponentPluginUser::resolveMemoryServer(const std::string& memoryName)
+ {
+ armem::data::ResolveMemoryNameResult result;
+ try
+ {
+ result.proxy = memoryNameSystem.resolveServer(MemoryID().withMemoryName(memoryName));
+ result.success = true;
+ }
+ catch (const armem::error::CouldNotResolveMemoryServer& e)
+ {
+ result.success = false;
+ result.errorMessage = e.what();
+ }
+ return result;
+ }
+
+}
+
diff --git a/source/RobotAPI/libraries/armem/client/MemoryNameSystemComponentPlugin.h b/source/RobotAPI/libraries/armem/client/MemoryNameSystemComponentPlugin.h
new file mode 100644
index 0000000000000000000000000000000000000000..a6a2434a68a033d3e540729c08e83c90babda847
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/client/MemoryNameSystemComponentPlugin.h
@@ -0,0 +1,101 @@
+#pragma once
+
+#include <ArmarXCore/core/ComponentPlugin.h>
+
+#include <RobotAPI/interface/armem/mns/MemoryNameSystemInterface.h>
+#include <RobotAPI/interface/armem/client/MemoryListenerInterface.h>
+#include <RobotAPI/libraries/armem/core/MemoryID.h>
+#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
+
+
+namespace armarx::armem::mns
+{
+ class MemoryNameSystemComponentPluginUser;
+}
+
+
+namespace armarx::armem::client::plugins
+{
+ /**
+ * @brief A base plugin offering optional access and dependency
+ * to the Memory Name System (MNS).
+ */
+ class MemoryNameSystemComponentPlugin : public ComponentPlugin
+ {
+ public:
+
+ using ComponentPlugin::ComponentPlugin;
+ virtual ~MemoryNameSystemComponentPlugin() override;
+
+ void postCreatePropertyDefinitions(PropertyDefinitionsPtr& properties) override;
+
+ void preOnInitComponent() override;
+ void preOnConnectComponent() override;
+
+ /**
+ * @brief Indicate whether the Memory Name System (MNS) is enabled.
+ */
+ bool isMemoryNameSystemEnabled();
+ /**
+ * @brief Get the name of the MNS component.
+ */
+ std::string getMemoryNameSystemName();
+
+ /**
+ * @brief Get the MNS proxy.
+ * @return The MNS proxy when MNS is enabled, nullptr when MNS is disabled.
+ */
+ mns::MemoryNameSystemInterfacePrx getMemoryNameSystem();
+
+ public:
+ static constexpr const char* PROPERTY_MNS_ENABLED_NAME = "mns.MemoryNameSystemEnabled";
+ static constexpr const char* PROPERTY_MNS_NAME_NAME = "mns.MemoryNameSystemName";
+ };
+
+}
+
+
+#include <ArmarXCore/core/ManagedIceObject.h>
+
+namespace armarx::armem::client
+{
+
+ class MemoryNameSystemComponentPluginUser :
+ virtual public ManagedIceObject,
+ virtual public MemoryListenerInterface
+ {
+ protected:
+
+ MemoryNameSystemComponentPluginUser();
+ virtual ~MemoryNameSystemComponentPluginUser() override;
+
+
+ virtual void memoryUpdated(const std::vector<data::MemoryID>& updatedSnapshotIDs, const Ice::Current& current) override;
+
+
+ [[deprecated("Use memoryNameSystem member instead of function inherited by MemoryNameSystemComponentPluginUser.")]]
+ armem::data::WaitForMemoryResult waitForMemoryServer(const std::string& memoryName);
+ [[deprecated("Use memoryNameSystem member instead of function inherited by MemoryNameSystemComponentPluginUser.")]]
+ armem::data::ResolveMemoryNameResult resolveMemoryServer(const std::string& memoryName);
+
+ [[deprecated("Use memoryNameSystem member instead of function inherited by MemoryNameSystemComponentPluginUser.")]]
+ armem::data::WaitForMemoryResult useMemoryServer(const MemoryID& id);
+ [[deprecated("Use memoryNameSystem member instead of function inherited by MemoryNameSystemComponentPluginUser.")]]
+ virtual armem::data::WaitForMemoryResult useMemoryServer(const std::string& memoryName);
+
+
+ public:
+
+ /// Only valid when enabled.
+ MemoryNameSystem memoryNameSystem;
+
+ bool memoryNameSystemEnabled = true;
+ std::string memoryNameSystemName = "MemoryNameSystem";
+
+
+ private:
+
+ plugins::MemoryNameSystemComponentPlugin* plugin = nullptr;
+
+ };
+}
diff --git a/source/RobotAPI/libraries/armem/client/Query.h b/source/RobotAPI/libraries/armem/client/Query.h
index b22c880013ad594acad439f3941eaff324763287..4f5a87bdccfc84458210d0811b02d99275b54ae4 100644
--- a/source/RobotAPI/libraries/armem/client/Query.h
+++ b/source/RobotAPI/libraries/armem/client/Query.h
@@ -9,8 +9,16 @@
#include <RobotAPI/libraries/armem/core/workingmemory/Memory.h>
+namespace armarx::armem::client::query
+{
+ // #include <RobotAPI/libraries/armem/client/query/Builder.h>
+ class Builder;
+}
+
namespace armarx::armem::client
{
+ using QueryBuilder = query::Builder;
+
/**
* @brief An update of an entity for a specific point in time.
diff --git a/source/RobotAPI/libraries/armem/client/Reader.cpp b/source/RobotAPI/libraries/armem/client/Reader.cpp
index 011948cc29860ed26b63dfdf93e59358c8180e08..a589512f13bcdaaa2536f597a54474eb72f96574 100644
--- a/source/RobotAPI/libraries/armem/client/Reader.cpp
+++ b/source/RobotAPI/libraries/armem/client/Reader.cpp
@@ -1,7 +1,16 @@
#include "Reader.h"
+#include <sstream>
+
#include <ArmarXCore/core/logging/Logging.h>
+#include <RobotAPI/libraries/armem/core/MemoryID_operators.h>
+#include <RobotAPI/libraries/armem/core/longtermmemory/Memory.h>
+#include <RobotAPI/libraries/armem/core/workingmemory/Memory.h>
+#include <RobotAPI/libraries/armem/core/workingmemory/visitor.h>
+#include <RobotAPI/libraries/armem/core/workingmemory/ice_conversions.h>
+#include <RobotAPI/libraries/armem/util/util.h>
+
#include "query/Builder.h"
#include "query/query_fns.h"
@@ -20,6 +29,7 @@ namespace armarx::armem::client
return QueryResult::fromIce(query(input.toIce()));
}
+
armem::query::data::Result
Reader::query(const armem::query::data::Input& input) const
{
@@ -50,11 +60,13 @@ namespace armarx::armem::client
return result;
}
+
QueryResult Reader::query(armem::query::data::MemoryQueryPtr query, DataMode dataMode) const
{
return this->query(armem::query::data::MemoryQuerySeq{query}, dataMode);
}
+
QueryResult Reader::query(const armem::query::data::MemoryQuerySeq& queries, DataMode dataMode) const
{
QueryInput input;
@@ -63,79 +75,161 @@ namespace armarx::armem::client
return this->query(input);
}
- QueryResult Reader::getAll(DataMode dataMode) const
- {
- using namespace client::query_fns;
-
- query::Builder qb(dataMode);
- qb.coreSegments(all()).providerSegments(all()).entities(all()).snapshots(all());
- return this->query(qb.buildQueryInput());
+ QueryResult Reader::query(const QueryBuilder& queryBuilder) const
+ {
+ return this->query(queryBuilder.buildQueryInput());
}
- QueryResult Reader::getLatestSnapshots(DataMode dataMode) const
+
+ QueryResult Reader::queryMemoryIDs(const std::vector<MemoryID>& ids, DataMode dataMode) const
{
using namespace client::query_fns;
query::Builder qb(dataMode);
- qb.coreSegments(all()).providerSegments(all()).entities(all()).snapshots(latest());
+ for (const MemoryID& id : ids)
+ {
+ query::EntitySelector& entity = qb.coreSegments(withID(id)).providerSegments(withID(id)).entities(withID(id));
- return this->query(qb.buildQueryInput());
+ if (id.hasTimestamp())
+ {
+ entity.snapshots(withID(id));
+ }
+ else
+ {
+ entity.snapshots(latest());
+ }
+ }
+ return query(qb);
}
- void
- Reader::updated(const std::vector<MemoryID>& updatedSnapshotIDs) const
+
+
+ std::optional<wm::EntitySnapshot> Reader::getLatestSnapshotOf(const std::vector<MemoryID>& _snapshotIDs) const
{
- for (const auto& [subscription, callbacks] : this->callbacks)
- {
- std::vector<MemoryID> matchingSnapshotIDs;
+ std::vector<MemoryID> snapshotIDs = _snapshotIDs;
- for (const MemoryID& updatedSnapshotID : updatedSnapshotIDs)
+ client::QueryResult result = this->queryMemoryIDs(snapshotIDs);
+ if (result.success)
+ {
+ std::sort(snapshotIDs.begin(), snapshotIDs.end(), compareTimestampDecreasing);
+ for (const MemoryID& snapshotID : snapshotIDs)
{
- if (contains(subscription, updatedSnapshotID))
+ try
{
- matchingSnapshotIDs.push_back(updatedSnapshotID);
+ wm::EntitySnapshot& snapshot = result.memory.getEntitySnapshot(snapshotID);
+ return snapshot;
}
- }
-
- if (not matchingSnapshotIDs.empty())
- {
- for (auto& callback : callbacks)
+ catch (const armem::error::ArMemError&)
{
- callback(subscription, matchingSnapshotIDs);
}
}
+ return std::nullopt;
+ }
+ else
+ {
+ ARMARX_INFO << "Error querying " << snapshotIDs.size() << " STT snapshots:\n"
+ << result.errorMessage;
+ return std::nullopt;
}
}
- data::StoreResult
- Reader::readAndStore(const data::StoreInput& input) const
+
+ QueryResult Reader::getLatestSnapshotsIn(const MemoryID& id, DataMode dataMode) const
{
- server::StoringMemoryInterfacePrx storingMemoryPrx = server::StoringMemoryInterfacePrx::checkedCast(memoryPrx);
- if (storingMemoryPrx)
+ using namespace client::query_fns;
+ if (!id.isWellDefined())
{
- return storingMemoryPrx->store(input);
+ throw armem::error::InvalidMemoryID(id, "ID must be well defined, but was not.");
+ }
+
+ query::Builder qb(dataMode);
+ query::CoreSegmentSelector& core =
+ id.hasCoreSegmentName()
+ ? qb.coreSegments(withID(id))
+ : qb.coreSegments(all());
+ query::ProviderSegmentSelector& prov =
+ id.hasProviderSegmentName()
+ ? core.providerSegments(withID(id))
+ : core.providerSegments(all());
+ query::EntitySelector& entity =
+ id.hasEntityName()
+ ? prov.entities(withID(id))
+ : prov.entities(all());
+ entity.snapshots(latest());
+
+ return query(qb);
+ }
+
+
+ struct FindLatestSnapshotVisitor : public wm::Visitor
+ {
+ std::optional<wm::EntitySnapshot> latest = std::nullopt;
+
+ bool visitEnter(const wm::EntitySnapshot& snapshot) override;
+ };
+ bool FindLatestSnapshotVisitor::visitEnter(const wm::EntitySnapshot& snapshot)
+ {
+ if (not latest.has_value() or snapshot.time() < latest->time())
+ {
+ latest = snapshot;
+ }
+ return true;
+ }
+
+
+ std::optional<wm::EntitySnapshot> Reader::getLatestSnapshotIn(const MemoryID& id, DataMode dataMode) const
+ {
+ client::QueryResult result = getLatestSnapshotsIn(id, dataMode);
+ if (result.success)
+ {
+ FindLatestSnapshotVisitor visitor;
+ visitor.applyTo(result.memory);
+ return visitor.latest;
}
else
{
- ARMARX_WARNING << "Could not store a query into the LTM. It seems like the Memory does not implement the StoringMemoryInterface.";
- return {};
+ ARMARX_INFO << "Error querying latest snapshot in " << id;
+ return std::nullopt;
}
}
- void
- Reader::subscribe(const MemoryID& id, callback callback)
+ QueryResult Reader::getAllLatestSnapshots(DataMode dataMode) const
+ {
+ using namespace client::query_fns;
+
+ query::Builder qb(dataMode);
+ qb.coreSegments(all()).providerSegments(all()).entities(all()).snapshots(latest());
+
+ return this->query(qb);
+ }
+
+
+ QueryResult Reader::getAll(DataMode dataMode) const
{
- callbacks[id].push_back(callback);
+ using namespace client::query_fns;
+
+ query::Builder qb(dataMode);
+ qb.coreSegments(all()).providerSegments(all()).entities(all()).snapshots(all());
+
+ return this->query(qb);
}
- void Reader::subscribe(const MemoryID& subscriptionID, callback_updated_only callback)
+
+ data::StoreResult
+ Reader::readAndStore(const data::StoreInput& input) const
{
- subscribe(subscriptionID, [callback](const MemoryID&, const std::vector<MemoryID>& updatedSnapshotIDs)
+ server::StoringMemoryInterfacePrx storingMemoryPrx = server::StoringMemoryInterfacePrx::checkedCast(memoryPrx);
+ if (storingMemoryPrx)
+ {
+ return storingMemoryPrx->store(input);
+ }
+ else
{
- callback(updatedSnapshotIDs);
- });
+ ARMARX_WARNING << "Could not store a query into the LTM. It seems like the Memory does not implement the StoringMemoryInterface.";
+ return {};
+ }
}
@@ -144,5 +238,4 @@ namespace armarx::armem::client
{
this->memoryPrx = memory;
}
-
}
diff --git a/source/RobotAPI/libraries/armem/client/Reader.h b/source/RobotAPI/libraries/armem/client/Reader.h
index 5cbeff84a3b6f979cd39d87b8271ff7e81d0c01b..1d69491d2cf33e7bdf460634ef69bc26db4bb52b 100644
--- a/source/RobotAPI/libraries/armem/client/Reader.h
+++ b/source/RobotAPI/libraries/armem/client/Reader.h
@@ -2,17 +2,14 @@
// STD/STL
-#include <functional>
-#include <unordered_map>
+#include <optional>
#include <vector>
// RobotAPI
#include <RobotAPI/interface/armem/server/ReadingMemoryInterface.h>
#include <RobotAPI/interface/armem/server/StoringMemoryInterface.h>
-#include <RobotAPI/interface/armem/client/MemoryListenerInterface.h>
-#include <RobotAPI/libraries/armem/core/workingmemory/ice_conversions.h>
-#include <RobotAPI/libraries/armem/core/workingmemory/Memory.h>
-#include <RobotAPI/libraries/armem/core/longtermmemory/Memory.h>
+#include <RobotAPI/libraries/armem/core/workingmemory/EntitySnapshot.h>
+// #include <RobotAPI/libraries/armem/core/workingmemory/ice_conversions.h>
#include "Query.h"
@@ -26,15 +23,6 @@ namespace armarx::armem::client
class Reader
{
- using callback = std::function<void(const MemoryID& subscriptionID, const std::vector<MemoryID>& updatedSnapshotIDs)>;
- using callback_updated_only = std::function<void(const std::vector<MemoryID>& updatedSnapshotIDs)>;
-
- template <class CalleeT>
- using member_callback = void(CalleeT::*)(const MemoryID& subscriptionID, const std::vector<MemoryID>& updatedSnapshotIDs);
- template <class CalleeT>
- using member_callback_updated_only = void(CalleeT::*)(const std::vector<MemoryID>& updatedSnapshotIDs);
-
-
public:
/**
@@ -45,47 +33,83 @@ namespace armarx::armem::client
void setReadingMemory(server::ReadingMemoryInterfacePrx memory);
+ /// Perform a query.
QueryResult query(const QueryInput& input) const;
armem::query::data::Result query(const armem::query::data::Input& input) const;
QueryResult query(armem::query::data::MemoryQueryPtr query, DataMode dataMode = DataMode::WithData) const;
QueryResult query(const armem::query::data::MemoryQuerySeq& queries, DataMode dataMode = DataMode::WithData) const;
+ QueryResult query(const QueryBuilder& queryBuilder) const;
- QueryResult getAll(DataMode dataMode = DataMode::WithData) const;
- QueryResult getLatestSnapshots(DataMode dataMode = DataMode::WithData) const;
- //data::StoreResult readAndStore(data::StoreInputSeq& input);
- data::StoreResult readAndStore(const data::StoreInput& input) const;
+ /**
+ * @brief Qeury a specific set of memory IDs.
+ *
+ * Each ID can refer to an entity, a snapshot or an instance. When not
+ * referring to an entity instance, the latest snapshot and first
+ * instance will be queried, respectively.
+ *
+ * All memory IDs must refer to the memory this reader is reading from.
+ * If an ID refers to another memory, the query will not find it and it
+ * will not be part of the result.
+ *
+ * @param ids The entity, snapshot or instance IDs.
+ * @param dataMode Whether to include instance data or just meta data.
+ * @return The query result.
+ */
+ QueryResult
+ queryMemoryIDs(const std::vector<MemoryID>& ids, DataMode dataMode = DataMode::WithData) const;
+
- void subscribe(const MemoryID& subscriptionID, callback callback);
- void subscribe(const MemoryID& subscriptionID, callback_updated_only callback);
/**
- * Subscribe with a class member function:
- * @code
- * reader.subscribe(entityID, this, &This::myCallback);
- * @endcode
+ * @brief Query the given snapshot and return the latest existing snapshot.
+ * @param snapshotIDs The snapshot (or entity) IDs.
+ * @return The latest snapshot contained in the query result, if any.
*/
- template <class CalleeT>
- void subscribe(const MemoryID& subscriptionID, CalleeT* callee, member_callback<CalleeT> callback)
- {
- auto cb = [callee, callback](const MemoryID & subscriptionID, const std::vector<MemoryID>& updatedSnapshotIDs)
- {
- (callee->*callback)(subscriptionID, updatedSnapshotIDs);
- };
- subscribe(subscriptionID, cb);
- }
- template <class CalleeT>
- void subscribe(const MemoryID& subscriptionID, CalleeT* callee, member_callback_updated_only<CalleeT> callback)
- {
- auto cb = [callee, callback](const MemoryID&, const std::vector<MemoryID>& updatedSnapshotIDs)
- {
- (callee->*callback)(updatedSnapshotIDs);
- };
- subscribe(subscriptionID, cb);
- }
- /// Function handling updates from the MemoryListener ice topic.
- void updated(const std::vector<MemoryID>& updatedIDs) const;
+ std::optional<wm::EntitySnapshot>
+ getLatestSnapshotOf(const std::vector<MemoryID>& snapshotIDs) const;
+
+
+ /**
+ * @brief Get the latest snapshots under the given memory ID.
+ * @param id A memory, core segment, provider segment or entity ID.
+ * @param dataMode With or without data.
+ * @return The query result.
+ */
+ QueryResult
+ getLatestSnapshotsIn(const MemoryID& id, DataMode dataMode = DataMode::WithData) const;
+
+ /**
+ * @brief Get the latest snapshot under the given memory ID.
+ * @param id A memory, core segment, provider segment or entity ID.
+ * @param dataMode With or without data.
+ * @return The latest contained snapshot, if any.
+ */
+ std::optional<wm::EntitySnapshot>
+ getLatestSnapshotIn(const MemoryID& id, DataMode dataMode = DataMode::WithData) const;
+
+
+ /**
+ * @brief Get all latest snapshots in the memory.
+ * @param dataMode With or without data.
+ * @return The query result.
+ */
+ QueryResult
+ getAllLatestSnapshots(DataMode dataMode = DataMode::WithData) const;
+
+
+ /**
+ * @brief Get the whole memory content.
+ * @param dataMode With or without data.
+ * @return The query result.
+ */
+ QueryResult
+ getAll(DataMode dataMode = DataMode::WithData) const;
+
+
+ //data::StoreResult readAndStore(data::StoreInputSeq& input);
+ data::StoreResult readAndStore(const data::StoreInput& input) const;
inline operator bool() const
@@ -97,10 +121,6 @@ namespace armarx::armem::client
server::ReadingMemoryInterfacePrx memoryPrx;
- private:
-
- std::unordered_map<MemoryID, std::vector<callback>> callbacks;
-
};
}
diff --git a/source/RobotAPI/libraries/armem/client/ReaderComponentPlugin.cpp b/source/RobotAPI/libraries/armem/client/ReaderComponentPlugin.cpp
index 2e73e852b9ec168e1e4442c00f7668b5cabb9f64..3204c83cc094f3895e4ace10a4b91c9cdfeb1070 100644
--- a/source/RobotAPI/libraries/armem/client/ReaderComponentPlugin.cpp
+++ b/source/RobotAPI/libraries/armem/client/ReaderComponentPlugin.cpp
@@ -10,16 +10,16 @@
namespace armarx::armem::client::plugins
{
- ReaderComponentPlugin::~ReaderComponentPlugin()
+ ReaderComponentPlugin::ReaderComponentPlugin(ManagedIceObject& parent, std::string pre) :
+ ComponentPlugin(parent, pre)
{
- // pass
+ addPlugin(mnsPlugin, pre);
+ addPluginDependency(mnsPlugin);
}
- void
- ReaderComponentPlugin::postCreatePropertyDefinitions(PropertyDefinitionsPtr& properties)
+
+ ReaderComponentPlugin::~ReaderComponentPlugin()
{
- ClientPlugin::postCreatePropertyDefinitions(properties);
- properties->topic<MemoryListenerInterface>("MemoryUpdates");
}
}
@@ -35,32 +35,22 @@ namespace armarx::armem::client
ReaderComponentPluginUser::~ReaderComponentPluginUser()
{
- // pass
- }
-
-
- void
- ReaderComponentPluginUser::memoryUpdated(
- const std::vector<data::MemoryID>& updatedSnapshotIDsIce, const Ice::Current&)
- {
- std::vector<MemoryID> updatedSnapshotIDs;
- fromIce(updatedSnapshotIDsIce, updatedSnapshotIDs);
- memoryReader.updated(updatedSnapshotIDs);
}
void
- ReaderComponentPluginUser::setReadingMemory(server::ReadingMemoryInterfacePrx memory)
+ ReaderComponentPluginUser::setReadingMemoryServer(server::ReadingMemoryInterfacePrx memory)
{
memoryReader.setReadingMemory(memory);
}
- armem::data::WaitForMemoryResult ReaderComponentPluginUser::useMemory(const std::string& memoryName)
+
+ armem::data::WaitForMemoryResult ReaderComponentPluginUser::useMemoryServer(const std::string& memoryName)
{
- armem::data::WaitForMemoryResult result = mns::plugins::ClientPluginUserBase::useMemory(memoryName);
+ armem::data::WaitForMemoryResult result = MemoryNameSystemComponentPluginUser::useMemoryServer(memoryName);
if (result.proxy)
{
- setReadingMemory(result.proxy);
+ setReadingMemoryServer(result.proxy);
}
return result;
}
diff --git a/source/RobotAPI/libraries/armem/client/ReaderComponentPlugin.h b/source/RobotAPI/libraries/armem/client/ReaderComponentPlugin.h
index 8c7858bfc6e5c14ca5f756512d000928f14b8502..0a2e37ba953e024858090cc64d6510dfebfd8a77 100644
--- a/source/RobotAPI/libraries/armem/client/ReaderComponentPlugin.h
+++ b/source/RobotAPI/libraries/armem/client/ReaderComponentPlugin.h
@@ -11,25 +11,26 @@
#include <RobotAPI/libraries/aron/core/navigator/data/AllNavigators.h>
#include <RobotAPI/interface/armem/mns/MemoryNameSystemInterface.h>
#include <RobotAPI/interface/armem/server/ReadingMemoryInterface.h>
-#include <RobotAPI/interface/armem/client/MemoryListenerInterface.h>
#include <RobotAPI/libraries/armem/client/Reader.h>
-#include <RobotAPI/libraries/armem/mns/ClientPlugin.h>
+#include <RobotAPI/libraries/armem/client/MemoryNameSystemComponentPlugin.h>
namespace armarx::armem::client::plugins
{
- class ReaderComponentPlugin :
- public mns::plugins::ClientPlugin
+ class ReaderComponentPlugin : public ComponentPlugin
{
public:
- using mns::plugins::ClientPlugin::ClientPlugin;
+ ReaderComponentPlugin(ManagedIceObject& parent, std::string pre);
~ReaderComponentPlugin() override;
- void postCreatePropertyDefinitions(PropertyDefinitionsPtr& properties) override;
+
+ private:
+
+ plugins::MemoryNameSystemComponentPlugin* mnsPlugin = nullptr;
};
@@ -41,8 +42,7 @@ namespace armarx::armem::client
class ReaderComponentPluginUser :
virtual public ManagedIceObject,
- virtual public mns::plugins::ClientPluginUserBase,
- virtual public MemoryListenerInterface
+ virtual public MemoryNameSystemComponentPluginUser
{
public:
@@ -50,18 +50,14 @@ namespace armarx::armem::client
ReaderComponentPluginUser();
~ReaderComponentPluginUser() override;
- virtual armem::data::WaitForMemoryResult useMemory(const std::string& memoryName) override;
- using mns::plugins::ClientPluginUserBase::useMemory;
+ [[deprecated("Use memoryNameSystem member instead of function inherited by ReaderComponentPluginUser.")]]
+ virtual armem::data::WaitForMemoryResult useMemoryServer(const std::string& memoryName) override;
+ using MemoryNameSystemComponentPluginUser::useMemoryServer;
- virtual void memoryUpdated(const std::vector<data::MemoryID>& updatedSnapshotIDs, const Ice::Current& current) override;
protected:
- void setReadingMemory(server::ReadingMemoryInterfacePrx memory);
-
-
-
- protected:
+ void setReadingMemoryServer(server::ReadingMemoryInterfacePrx memory);
Reader memoryReader;
diff --git a/source/RobotAPI/libraries/armem/client/Writer.cpp b/source/RobotAPI/libraries/armem/client/Writer.cpp
index 93dbb39ae33d6e614fb0f4e95de58e0522a99b20..2021f3a189367dc9134d60ce1dd597cb8513dfaf 100644
--- a/source/RobotAPI/libraries/armem/client/Writer.cpp
+++ b/source/RobotAPI/libraries/armem/client/Writer.cpp
@@ -41,21 +41,14 @@ namespace armarx::armem::client
}
- CommitResult Writer::commit(const Commit& _commit)
+ CommitResult Writer::commit(const Commit& commit)
{
ARMARX_CHECK_NOT_NULL(memory);
- Commit commit = _commit;
-
- Time timeSent = armem::Time::now();
- for (EntityUpdate& update : commit.updates)
- {
- update.timeSent = timeSent;
- }
data::Commit commitIce;
toIce(commitIce, commit);
- data::CommitResult resultIce = this->commit(commitIce);
+ data::CommitResult resultIce = this->_commit(commitIce);
armem::CommitResult result;
fromIce(resultIce, result);
@@ -64,10 +57,60 @@ namespace armarx::armem::client
}
- data::CommitResult Writer::commit(const data::Commit& commit)
+ data::CommitResult Writer::commit(const data::Commit& _commit)
+ {
+ data::Commit commit = _commit;
+ return this->_commit(commit);
+ }
+
+
+ EntityUpdateResult Writer::commit(const EntityUpdate& update)
+ {
+ armem::Commit commit;
+ commit.updates.push_back(update);
+
+ armem::CommitResult result = this->commit(commit);
+ ARMARX_CHECK_EQUAL(result.results.size(), 1);
+ return result.results.at(0);
+ }
+
+ EntityUpdateResult Writer::commit(
+ const MemoryID& entityID,
+ const std::vector<aron::datanavigator::DictNavigatorPtr>& instancesData,
+ Time timeCreated)
+ {
+ EntityUpdate update;
+ update.entityID = entityID;
+ update.instancesData = instancesData;
+ update.timeCreated = timeCreated;
+ return commit(update);
+ }
+
+ void
+ Writer::setWritingMemory(server::WritingMemoryInterfacePrx memory)
+ {
+ this->memory = memory;
+ }
+
+ data::CommitResult Writer::_commit(data::Commit& commit)
{
ARMARX_CHECK_NOT_NULL(memory);
+ /*
+ * This function sets the `timeSent` of each `EntityUpdate` before
+ * sending the data. To allow that, `commit` needs to bo non-const.
+ * Otherwise, the function would need to make a copy of `commit`,
+ * which is not necessary in all cases; e.g. when called by
+ * `commit(Const Commit&)`, which converts the commit to ice types
+ * anyway.
+ */
+
+ const Time timeSent = armem::Time::now();
+ for (data::EntityUpdate& update : commit.updates)
+ {
+ update.timeSentMicroSeconds = timeSent.toMicroSeconds();
+ }
+
data::CommitResult result;
auto handleError = [&commit, &result](const std::string & what)
{
@@ -99,35 +142,6 @@ namespace armarx::armem::client
return result;
}
-
-
- EntityUpdateResult Writer::commit(const EntityUpdate& update)
- {
- armem::Commit commit;
- commit.updates.push_back(update);
-
- armem::CommitResult result = this->commit(commit);
- ARMARX_CHECK_EQUAL(result.results.size(), 1);
- return result.results.at(0);
- }
-
- EntityUpdateResult Writer::commit(
- const MemoryID& entityID,
- const std::vector<aron::datanavigator::DictNavigatorPtr>& instancesData,
- Time timeCreated)
- {
- EntityUpdate update;
- update.entityID = entityID;
- update.instancesData = instancesData;
- update.timeCreated = timeCreated;
- return commit(update);
- }
-
- void
- Writer::setWritingMemory(server::WritingMemoryInterfacePrx memory)
- {
- this->memory = memory;
- }
}
diff --git a/source/RobotAPI/libraries/armem/client/Writer.h b/source/RobotAPI/libraries/armem/client/Writer.h
index 9c6e8851e4fe3792ad728e90eaa9319a88568e0c..dd953fa368d9a46363677d3bc39bc71f8da2a9ce 100644
--- a/source/RobotAPI/libraries/armem/client/Writer.h
+++ b/source/RobotAPI/libraries/armem/client/Writer.h
@@ -50,6 +50,10 @@ namespace armarx::armem::client
const std::vector<aron::datanavigator::DictNavigatorPtr>& instancesData,
Time timeCreated);
+ // with bare-ice types
+ data::CommitResult commit(const data::Commit& commit);
+
+
void setWritingMemory(server::WritingMemoryInterfacePrx memory);
operator bool() const
@@ -59,7 +63,9 @@ namespace armarx::armem::client
private:
- data::CommitResult commit(const data::Commit& commit);
+ /// Sets `timeSent` on all entity updates and performs the commit,
+ data::CommitResult _commit(data::Commit& commit);
+
public:
diff --git a/source/RobotAPI/libraries/armem/client/WriterComponentPlugin.cpp b/source/RobotAPI/libraries/armem/client/WriterComponentPlugin.cpp
index 04014c77a338c1f6fc836aa8ea1f64f92feb42e9..093f29e6fa2178f19ae13983e21772f17400856d 100644
--- a/source/RobotAPI/libraries/armem/client/WriterComponentPlugin.cpp
+++ b/source/RobotAPI/libraries/armem/client/WriterComponentPlugin.cpp
@@ -10,9 +10,15 @@
namespace armarx::armem::client::plugins
{
+ WriterComponentPlugin::WriterComponentPlugin(ManagedIceObject& parent, std::string pre) :
+ ComponentPlugin(parent, pre)
+ {
+ addPlugin(mnsPlugin, pre);
+ addPluginDependency(mnsPlugin);
+ }
+
WriterComponentPlugin::~WriterComponentPlugin()
{
- // pass
}
}
@@ -32,17 +38,17 @@ namespace armarx::armem::client
void
- WriterComponentPluginUser::setWritingMemory(server::WritingMemoryInterfacePrx memory)
+ WriterComponentPluginUser::setWritingMemoryServer(server::WritingMemoryInterfacePrx memory)
{
memoryWriter.setWritingMemory(memory);
}
- armem::data::WaitForMemoryResult WriterComponentPluginUser::useMemory(const std::string& memoryName)
+ armem::data::WaitForMemoryResult WriterComponentPluginUser::useMemoryServer(const std::string& memoryName)
{
- armem::data::WaitForMemoryResult result = mns::plugins::ClientPluginUserBase::useMemory(memoryName);
+ armem::data::WaitForMemoryResult result = MemoryNameSystemComponentPluginUser::useMemoryServer(memoryName);
if (result.proxy)
{
- setWritingMemory(result.proxy);
+ setWritingMemoryServer(result.proxy);
}
return result;
}
diff --git a/source/RobotAPI/libraries/armem/client/WriterComponentPlugin.h b/source/RobotAPI/libraries/armem/client/WriterComponentPlugin.h
index 0f5f21f808fb646fe8af27192d04c82a5739426e..435881b48b27b9053dd5da5e87b794d3561f3b3c 100644
--- a/source/RobotAPI/libraries/armem/client/WriterComponentPlugin.h
+++ b/source/RobotAPI/libraries/armem/client/WriterComponentPlugin.h
@@ -10,20 +10,24 @@
#include <RobotAPI/interface/armem/mns/MemoryNameSystemInterface.h>
#include <RobotAPI/libraries/armem/client/Writer.h>
-#include <RobotAPI/libraries/armem/mns/ClientPlugin.h>
+#include <RobotAPI/libraries/armem/client/MemoryNameSystemComponentPlugin.h>
namespace armarx::armem::client::plugins
{
- class WriterComponentPlugin : public mns::plugins::ClientPlugin
+ class WriterComponentPlugin : public ComponentPlugin
{
public:
- using mns::plugins::ClientPlugin::ClientPlugin;
+ WriterComponentPlugin(ManagedIceObject& parent, std::string pre);
~WriterComponentPlugin() override;
+ private:
+
+ plugins::MemoryNameSystemComponentPlugin* mnsPlugin = nullptr;
+
};
}
@@ -32,7 +36,7 @@ namespace armarx::armem::client
{
class WriterComponentPluginUser :
virtual public ManagedIceObject,
- virtual public mns::plugins::ClientPluginUserBase
+ virtual public MemoryNameSystemComponentPluginUser
{
public:
@@ -40,15 +44,17 @@ namespace armarx::armem::client
WriterComponentPluginUser();
~WriterComponentPluginUser() override;
- virtual armem::data::WaitForMemoryResult useMemory(const std::string& memoryName) override;
- using mns::plugins::ClientPluginUserBase::useMemory;
+ virtual armem::data::WaitForMemoryResult useMemoryServer(const std::string& memoryName) override;
+ using MemoryNameSystemComponentPluginUser::useMemoryServer;
+
protected:
- void setWritingMemory(server::WritingMemoryInterfacePrx memory);
+ void setWritingMemoryServer(server::WritingMemoryInterfacePrx memory);
Writer memoryWriter;
+
private:
plugins::WriterComponentPlugin* plugin = nullptr;
diff --git a/source/RobotAPI/libraries/armem/client/query.h b/source/RobotAPI/libraries/armem/client/query.h
new file mode 100644
index 0000000000000000000000000000000000000000..99606089e06952dd2f5006b628c69f3644acd674
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/client/query.h
@@ -0,0 +1,6 @@
+#pragma once
+
+#include "Query.h"
+#include "query/Builder.h"
+#include "query/query_fns.h"
+#include "query/selectors.h"
diff --git a/source/RobotAPI/libraries/armem/client/query/Builder.cpp b/source/RobotAPI/libraries/armem/client/query/Builder.cpp
index e7ce598a68d393d6da88a9e4ec52a801d10999c5..5979ea365a863dc0ce1c0c0b91a0d77fc3f331e9 100644
--- a/source/RobotAPI/libraries/armem/client/query/Builder.cpp
+++ b/source/RobotAPI/libraries/armem/client/query/Builder.cpp
@@ -8,6 +8,12 @@ namespace armarx::armem::client::query
{
}
+ Builder& Builder::queryTarget(const armem::query::data::QueryTarget target)
+ {
+ this->_target = target;
+ return *this;
+ }
+
QueryInput Builder::buildQueryInput() const
{
QueryInput input;
@@ -27,6 +33,7 @@ namespace armarx::armem::client::query
{
for (const armem::query::data::MemoryQueryPtr& query : child.buildQueries())
{
+ query->target = _target;
memoryQueries.push_back(query);
}
}
diff --git a/source/RobotAPI/libraries/armem/client/query/Builder.h b/source/RobotAPI/libraries/armem/client/query/Builder.h
index 7f4c2b7c63440f8182d2e489e74ea33ab98e8317..eae09294320f216aa3e996821c429376ff7675a0 100644
--- a/source/RobotAPI/libraries/armem/client/query/Builder.h
+++ b/source/RobotAPI/libraries/armem/client/query/Builder.h
@@ -28,7 +28,7 @@ namespace armarx::armem::client::query
public:
Builder(DataMode dataMode = DataMode::WithData);
-
+ Builder& queryTarget(const armem::query::data::QueryTarget target);
/// Start specifying core segments.
CoreSegmentSelector& coreSegments();
@@ -66,7 +66,7 @@ namespace armarx::armem::client::query
public:
-
+ armem::query::data::QueryTarget _target = armem::query::data::QueryTarget::WMAndLTM;
DataMode dataMode;
};
diff --git a/source/RobotAPI/libraries/armem/client/util/MemoryListener.cpp b/source/RobotAPI/libraries/armem/client/util/MemoryListener.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..3318b9ed385ff337b13871fb017fcb521af7fd74
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/client/util/MemoryListener.cpp
@@ -0,0 +1,90 @@
+#include "MemoryListener.h"
+
+#include <sstream>
+
+#include <ArmarXCore/core/logging/Logging.h>
+#include <RobotAPI/libraries/armem/core/ice_conversions.h>
+#include <RobotAPI/libraries/armem/core/error.h>
+
+
+namespace armarx::armem::client::util
+{
+
+ MemoryListener::MemoryListener()
+ {
+ }
+
+
+ void
+ MemoryListener::updated(const std::vector<data::MemoryID>& updatedSnapshotIDs) const
+ {
+ std::vector<MemoryID> bos;
+ fromIce(updatedSnapshotIDs, bos);
+ updated(bos);
+ }
+
+
+ void
+ MemoryListener::updated(const std::vector<MemoryID>& updatedSnapshotIDs) const
+ {
+ std::stringstream error;
+
+ for (const auto& [subscription, subCallbacks] : this->callbacks)
+ {
+ std::vector<MemoryID> matchingSnapshotIDs;
+
+ for (const MemoryID& updatedSnapshotID : updatedSnapshotIDs)
+ {
+ try
+ {
+ if (contains(subscription, updatedSnapshotID))
+ {
+ matchingSnapshotIDs.push_back(updatedSnapshotID);
+ }
+ }
+ catch (const armem::error::InvalidMemoryID& e)
+ {
+ // Log to debug, but ignore otherwise
+ error << "Error when comparing subscribed ID " << subscription
+ << " with updated ID " << updatedSnapshotID << ":\n"
+ << e.what() << "\n\n";
+ }
+ }
+
+ if (not matchingSnapshotIDs.empty())
+ {
+ ARMARX_DEBUG << "Calling " << subCallbacks.size() << " callbacks"
+ << " subscribing " << subscription
+ << " with " << matchingSnapshotIDs.size() << " snapshot IDs ...";
+ for (auto& callback : subCallbacks)
+ {
+ callback(subscription, matchingSnapshotIDs);
+ }
+ }
+ }
+ if (error.str().size() > 0)
+ {
+ ARMARX_VERBOSE << "The following issues were encountered during MemoryListener::" << __FUNCTION__ << "(): \n\n"
+ << error.str();
+ }
+ }
+
+
+ void
+ MemoryListener::subscribe(const MemoryID& id, Callback callback)
+ {
+ callbacks[id].push_back(callback);
+ }
+
+
+ void
+ MemoryListener::subscribe(const MemoryID& subscriptionID, CallbackUpdatedOnly callback)
+ {
+ subscribe(subscriptionID, [callback](const MemoryID&, const std::vector<MemoryID>& updatedSnapshotIDs)
+ {
+ callback(updatedSnapshotIDs);
+ });
+ }
+
+
+}
diff --git a/source/RobotAPI/libraries/armem/client/util/MemoryListener.h b/source/RobotAPI/libraries/armem/client/util/MemoryListener.h
new file mode 100644
index 0000000000000000000000000000000000000000..2d530097ce6394ddcf4cc815f8e44f93e057d916
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/client/util/MemoryListener.h
@@ -0,0 +1,78 @@
+#pragma once
+
+
+// STD/STL
+#include <functional>
+#include <unordered_map>
+#include <vector>
+
+// RobotAPI
+#include <RobotAPI/interface/armem/client/MemoryListenerInterface.h>
+#include <RobotAPI/libraries/armem/core/MemoryID.h>
+
+
+namespace armarx::armem::client::util
+{
+
+ /**
+ * @brief Handles update signals from the memory system and distributes it
+ * to its subsribers.
+ */
+ class MemoryListener
+ {
+
+ using Callback = std::function<void(const MemoryID& subscriptionID, const std::vector<MemoryID>& updatedSnapshotIDs)>;
+ using CallbackUpdatedOnly = std::function<void(const std::vector<MemoryID>& updatedSnapshotIDs)>;
+
+ template <class CalleeT>
+ using MemberCallback = void(CalleeT::*)(const MemoryID& subscriptionID, const std::vector<MemoryID>& updatedSnapshotIDs);
+ template <class CalleeT>
+ using MemberCallbackUpdatedOnly = void(CalleeT::*)(const std::vector<MemoryID>& updatedSnapshotIDs);
+
+
+ public:
+
+ MemoryListener();
+
+
+ void subscribe(const MemoryID& subscriptionID, Callback Callback);
+ void subscribe(const MemoryID& subscriptionID, CallbackUpdatedOnly Callback);
+
+ /**
+ * Subscribe with a class member function:
+ * @code
+ * listener.subscribe(entityID, this, &This::myCallback);
+ * @endcode
+ */
+ template <class CalleeT>
+ void subscribe(const MemoryID& subscriptionID, CalleeT* callee, MemberCallback<CalleeT> callback)
+ {
+ auto cb = [callee, callback](const MemoryID & subscriptionID, const std::vector<MemoryID>& updatedSnapshotIDs)
+ {
+ (callee->*callback)(subscriptionID, updatedSnapshotIDs);
+ };
+ subscribe(subscriptionID, cb);
+ }
+ template <class CalleeT>
+ void subscribe(const MemoryID& subscriptionID, CalleeT* callee, MemberCallbackUpdatedOnly<CalleeT> callback)
+ {
+ auto cb = [callee, callback](const MemoryID&, const std::vector<MemoryID>& updatedSnapshotIDs)
+ {
+ (callee->*callback)(updatedSnapshotIDs);
+ };
+ subscribe(subscriptionID, cb);
+ }
+
+
+ /// Function handling updates from the MemoryListener ice topic.
+ void updated(const std::vector<MemoryID>& updatedIDs) const;
+ void updated(const std::vector<data::MemoryID>& updatedIDs) const;
+
+
+ protected:
+
+ std::unordered_map<MemoryID, std::vector<Callback>> callbacks;
+
+ };
+
+}
diff --git a/source/RobotAPI/libraries/armem/client/util/SimpleReaderBase.cpp b/source/RobotAPI/libraries/armem/client/util/SimpleReaderBase.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..ce25109502b1da09cb01399396a5c7bca92ed50f
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/client/util/SimpleReaderBase.cpp
@@ -0,0 +1,63 @@
+#include "SimpleReaderBase.h"
+
+#include <RobotAPI/libraries/armem/core/error.h>
+#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
+
+
+namespace armarx::armem::client::util
+{
+
+ SimpleReaderBase::SimpleReaderBase(MemoryNameSystem& memoryNameSystem) :
+ memoryNameSystem(memoryNameSystem)
+ {
+ }
+
+
+ void SimpleReaderBase::registerPropertyDefinitions(
+ armarx::PropertyDefinitionsPtr& def)
+ {
+ ARMARX_DEBUG << "Writer: registerPropertyDefinitions";
+
+ const std::string prefix = propertyPrefix();
+
+ def->optional(props.memoryName, prefix + "Memory");
+ def->optional(props.coreSegmentName, prefix + "CoreSegment");
+ }
+
+
+ void SimpleReaderBase::connect()
+ {
+ // Wait for the memory to become available and add it as dependency.
+ ARMARX_IMPORTANT << "SimpleReaderBase: Waiting for memory '" << props.memoryName
+ << "' ...";
+ try
+ {
+ memoryReaderClient = memoryNameSystem.useReader(MemoryID().withMemoryName(props.memoryName));
+ ARMARX_IMPORTANT << "SimpleReaderBase: Connected to memory '" << props.memoryName << "'";
+ }
+ catch (const armem::error::CouldNotResolveMemoryServer& e)
+ {
+ ARMARX_ERROR << e.what();
+ return;
+ }
+ }
+
+
+ std::mutex& SimpleReaderBase::memoryReaderMutex()
+ {
+ return memoryMutex;
+ }
+
+
+ const armem::client::Reader& SimpleReaderBase::memoryReader() const
+ {
+ return memoryReaderClient;
+ }
+
+
+ const SimpleReaderBase::Properties& SimpleReaderBase::properties() const
+ {
+ return props;
+ }
+
+} // namespace armarx::armem::client::util
diff --git a/source/RobotAPI/libraries/armem/client/util/SimpleReaderBase.h b/source/RobotAPI/libraries/armem/client/util/SimpleReaderBase.h
new file mode 100644
index 0000000000000000000000000000000000000000..a78db401d3f578d7be9f9c86542a37f72326aabc
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/client/util/SimpleReaderBase.h
@@ -0,0 +1,78 @@
+/*
+ * 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/>.
+ *
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <mutex>
+
+#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
+
+#include <RobotAPI/libraries/armem/client/Reader.h>
+
+
+namespace armarx::armem::client
+{
+ class MemoryNameSystem;
+}
+
+namespace armarx::armem::client::util
+{
+
+ class SimpleReaderBase
+ {
+ public:
+
+ SimpleReaderBase(MemoryNameSystem& memoryNameSystem);
+ virtual ~SimpleReaderBase() = default;
+
+ void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def);
+ void connect();
+
+
+ protected:
+
+ // Properties
+ struct Properties
+ {
+ std::string memoryName = "";
+ std::string coreSegmentName = "";
+ };
+
+ const Properties& properties() const;
+
+ virtual std::string propertyPrefix() const = 0;
+ virtual Properties defaultProperties() const = 0;
+
+ std::mutex& memoryReaderMutex();
+ const armem::client::Reader& memoryReader() const;
+
+
+ private:
+
+ Properties props;
+
+ armem::client::Reader memoryReaderClient;
+ std::mutex memoryMutex;
+
+ MemoryNameSystem& memoryNameSystem;
+ };
+
+} // namespace armarx::armem::client::util
diff --git a/source/RobotAPI/libraries/armem/client/util/SimpleWriterBase.cpp b/source/RobotAPI/libraries/armem/client/util/SimpleWriterBase.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..ca399bf8f64a2fd347d17c31587d33639cded032
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/client/util/SimpleWriterBase.cpp
@@ -0,0 +1,68 @@
+#include "SimpleWriterBase.h"
+
+#include <RobotAPI/libraries/armem/core/error.h>
+#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
+
+
+namespace armarx::armem::client::util
+{
+ SimpleWriterBase::SimpleWriterBase(MemoryNameSystem& memoryNameSystem) :
+ memoryNameSystem(memoryNameSystem)
+ {
+ }
+
+
+ void
+ SimpleWriterBase::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
+ {
+ ARMARX_DEBUG << "Writer: registerPropertyDefinitions";
+
+ const std::string prefix = propertyPrefix();
+
+ props = defaultProperties();
+
+ def->optional(props.memoryName, prefix + "Memory");
+ def->optional(props.coreSegmentName, prefix + "CoreSegment");
+
+ def->required(props.providerName,
+ prefix + "Provider",
+ "Name of this provider");
+ }
+
+
+ void SimpleWriterBase::connect()
+ {
+ // Wait for the memory to become available and add it as dependency.
+ ARMARX_IMPORTANT << "SimpleWriterBase: Waiting for memory '"
+ << props.memoryName << "' ...";
+ try
+ {
+ memoryWriterClient = memoryNameSystem.useWriter(MemoryID().withMemoryName(props.memoryName));
+ ARMARX_IMPORTANT << "SimpleWriterBase: Connected to memory '" << props.memoryName << "'";
+ }
+ catch (const armem::error::CouldNotResolveMemoryServer& e)
+ {
+ ARMARX_ERROR << e.what();
+ return;
+ }
+ }
+
+
+ std::mutex& SimpleWriterBase::memoryWriterMutex()
+ {
+ return memoryMutex;
+ }
+
+
+ armem::client::Writer& SimpleWriterBase::memoryWriter()
+ {
+ return memoryWriterClient;
+ }
+
+
+ const SimpleWriterBase::Properties& SimpleWriterBase::properties() const
+ {
+ return props;
+ }
+
+} // namespace armarx::armem::client::util
diff --git a/source/RobotAPI/libraries/armem/client/util/SimpleWriterBase.h b/source/RobotAPI/libraries/armem/client/util/SimpleWriterBase.h
new file mode 100644
index 0000000000000000000000000000000000000000..1413299a3081f36dfaa83cce425c50994e7018c2
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/client/util/SimpleWriterBase.h
@@ -0,0 +1,79 @@
+/*
+ * 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/>.
+ *
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <mutex>
+
+#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
+
+#include <RobotAPI/libraries/armem/client/Writer.h>
+
+
+namespace armarx::armem::client
+{
+ class MemoryNameSystem;
+}
+
+namespace armarx::armem::client::util
+{
+
+ class SimpleWriterBase
+ {
+ public:
+
+ SimpleWriterBase(MemoryNameSystem& memoryNameSystem);
+ virtual ~SimpleWriterBase() = default;
+
+ void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def);
+ void connect();
+
+
+ protected:
+
+ struct Properties
+ {
+ std::string memoryName = "";
+ std::string coreSegmentName = "";
+ std::string providerName = ""; // required property
+ };
+
+ const Properties& properties() const;
+
+ virtual std::string propertyPrefix() const = 0;
+ virtual Properties defaultProperties() const = 0;
+
+ std::mutex& memoryWriterMutex();
+ armem::client::Writer& memoryWriter();
+
+
+ private:
+
+ Properties props;
+
+ armem::client::Writer memoryWriterClient;
+ std::mutex memoryMutex;
+
+ MemoryNameSystem& memoryNameSystem;
+
+ };
+
+} // namespace armarx::armem::client::util
diff --git a/source/RobotAPI/libraries/armem/core/Commit.cpp b/source/RobotAPI/libraries/armem/core/Commit.cpp
index 5c1946ed0f55591457c7ce7fcf7a8a5b9a325ee4..fed7ed660b450bb0865a018f9c8d7119dd96a7b4 100644
--- a/source/RobotAPI/libraries/armem/core/Commit.cpp
+++ b/source/RobotAPI/libraries/armem/core/Commit.cpp
@@ -86,4 +86,17 @@ namespace armarx::armem
return updates.emplace_back(update);
}
+ void Commit::add(const std::vector<EntityUpdate>& updates)
+ {
+ for (const auto& update : updates)
+ {
+ add(update);
+ }
+ }
+
+ void Commit::append(const Commit& c)
+ {
+ add(c.updates);
+ }
+
}
diff --git a/source/RobotAPI/libraries/armem/core/Commit.h b/source/RobotAPI/libraries/armem/core/Commit.h
index 48707600ab242ad3a624c6e26965acdef18839fa..fc69a4b53dcaecc52d703aa42c00405f4162825f 100644
--- a/source/RobotAPI/libraries/armem/core/Commit.h
+++ b/source/RobotAPI/libraries/armem/core/Commit.h
@@ -10,6 +10,16 @@
namespace armarx::armem
{
+
+ /**
+ * @brief The type of an update
+ */
+ enum class UpdateType
+ {
+ UpdatedExisting,
+ InsertedNew
+ };
+
/**
* @brief An update of an entity for a specific point in time.
*/
@@ -87,6 +97,8 @@ namespace armarx::armem
EntityUpdate& add();
EntityUpdate& add(const EntityUpdate& update);
+ void add(const std::vector<EntityUpdate>& updates);
+ void append(const Commit& c);
friend std::ostream& operator<<(std::ostream& os, const Commit& rhs);
};
diff --git a/source/RobotAPI/libraries/armem/core/MemoryID.cpp b/source/RobotAPI/libraries/armem/core/MemoryID.cpp
index 61dd4836ca9f16ffd6b36342facbd2f610c75ff3..f8d7c832401a4ebf335347c67745c1ce82af4e03 100644
--- a/source/RobotAPI/libraries/armem/core/MemoryID.cpp
+++ b/source/RobotAPI/libraries/armem/core/MemoryID.cpp
@@ -80,6 +80,23 @@ namespace armarx::armem
}
+ MemoryID::MemoryID(
+ const std::string& memoryName,
+ const std::string& coreSegmentName,
+ const std::string& providerSegmentName,
+ const std::string& entityName,
+ Time timestamp,
+ int instanceIndex) :
+ memoryName(memoryName),
+ coreSegmentName(coreSegmentName),
+ providerSegmentName(providerSegmentName),
+ entityName(entityName),
+ timestamp(timestamp),
+ instanceIndex(instanceIndex)
+ {
+ }
+
+
std::string MemoryID::str(bool escapeDelimiters) const
{
return str(delimiter, escapeDelimiters);
@@ -418,13 +435,13 @@ namespace armarx::armem
if (!general.isWellDefined())
{
std::stringstream ss;
- ss << "ID `general` is not well-defined, which is required for `" << __FUNCTION__ << "()`.";
+ ss << "\nGeneral ID " << general << " is not well-defined, which is required for `" << __FUNCTION__ << "()`.";
throw error::InvalidMemoryID(general, ss.str());
}
if (!specific.isWellDefined())
{
std::stringstream ss;
- ss << "ID `specific` is not well-defined, which is required for `" << __FUNCTION__ << "()`.";
+ ss << "\nSpecific ID " << specific << " is not well-defined, which is required for `" << __FUNCTION__ << "()`.";
throw error::InvalidMemoryID(specific, ss.str());
}
diff --git a/source/RobotAPI/libraries/armem/core/MemoryID.h b/source/RobotAPI/libraries/armem/core/MemoryID.h
index 7ddf2130801767eabb60c01caba1d2dc427caba3..f9d3d5b58ca4670a8abb4333e417a9a2109843ae 100644
--- a/source/RobotAPI/libraries/armem/core/MemoryID.h
+++ b/source/RobotAPI/libraries/armem/core/MemoryID.h
@@ -63,6 +63,13 @@ namespace armarx::armem
/// (Re-)Construct a memory ID from a string representation as returned by `str()`.
explicit MemoryID(const std::string& string);
+ MemoryID(const std::string& memoryName,
+ const std::string& coreSegmentName,
+ const std::string& providerSegmentName = "",
+ const std::string& entityName = "",
+ Time timestamp = Time::microSeconds(-1),
+ int instanceIndex = -1);
+
/**
* @brief Indicate whether this ID is well-defined.
diff --git a/source/RobotAPI/libraries/armem/core/MemoryID_operators.cpp b/source/RobotAPI/libraries/armem/core/MemoryID_operators.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..8a1e8aa17bf66356061696eee972e587eef96e61
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/MemoryID_operators.cpp
@@ -0,0 +1,26 @@
+#include "MemoryID_operators.h"
+
+#include "MemoryID.h"
+
+
+std::ostream& armarx::armem::operator<<(std::ostream& os, const std::vector<MemoryID>& rhs)
+{
+ os << "std::vector<MemoryID> with " << rhs.size() << " entries:";
+ for (size_t i = 0; i < rhs.size(); ++i)
+ {
+ os << "\n\t[" << i << "] " << rhs[i];
+ }
+ return os;
+}
+
+
+bool armarx::armem::compareTimestamp(const MemoryID& lhs, const MemoryID& rhs)
+{
+ return lhs.timestamp < rhs.timestamp;
+}
+
+
+bool armarx::armem::compareTimestampDecreasing(const MemoryID& lhs, const MemoryID& rhs)
+{
+ return lhs.timestamp > rhs.timestamp;
+}
diff --git a/source/RobotAPI/libraries/armem/core/MemoryID_operators.h b/source/RobotAPI/libraries/armem/core/MemoryID_operators.h
new file mode 100644
index 0000000000000000000000000000000000000000..7508c81e8ce2245e3b6924f7a706b0eff4390e7e
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/MemoryID_operators.h
@@ -0,0 +1,22 @@
+#pragma once
+
+// #include "MemoryID.h"
+
+#include <vector>
+#include <ostream>
+
+
+namespace armarx::armem
+{
+ class MemoryID;
+
+ std::ostream& operator<<(std::ostream& os, const std::vector<MemoryID>& rhs);
+
+ /// lhs.timestamp < rhs.timstamp
+ bool compareTimestamp(const MemoryID& lhs, const MemoryID& rhs);
+ /// lhs.timestamp > rhs.timstamp
+ bool compareTimestampDecreasing(const MemoryID& lhs, const MemoryID& rhs);
+
+}
+
+
diff --git a/source/RobotAPI/libraries/armem/core/aron_conversions.cpp b/source/RobotAPI/libraries/armem/core/aron_conversions.cpp
index ed2197e66fc41d424cc793980a771684e12d3de1..1f8e33c403eaeb3570d6848616d556772170e929 100644
--- a/source/RobotAPI/libraries/armem/core/aron_conversions.cpp
+++ b/source/RobotAPI/libraries/armem/core/aron_conversions.cpp
@@ -7,7 +7,7 @@ void armarx::armem::fromAron(const arondto::MemoryID& dto, MemoryID& bo)
bo.coreSegmentName = dto.coreSegmentName;
bo.providerSegmentName = dto.providerSegmentName;
bo.entityName = dto.entityName;
- bo.timestamp = IceUtil::Time::microSeconds(dto.timestamp);
+ bo.timestamp = dto.timestamp;
bo.instanceIndex = dto.instanceIndex;
}
@@ -17,7 +17,7 @@ void armarx::armem::toAron(arondto::MemoryID& dto, const MemoryID& bo)
dto.coreSegmentName = bo.coreSegmentName;
dto.providerSegmentName = bo.providerSegmentName;
dto.entityName = bo.entityName;
- dto.timestamp = bo.timestamp.toMicroSeconds();
+ dto.timestamp = bo.timestamp;
dto.instanceIndex = bo.instanceIndex;
}
diff --git a/source/RobotAPI/libraries/armem/core/base/CoreSegmentBase.h b/source/RobotAPI/libraries/armem/core/base/CoreSegmentBase.h
index ead811e5a744b9208a30eb7761c381b040cd7e90..6dc77c57625b0a8ebd33200885f06045eef49bbf 100644
--- a/source/RobotAPI/libraries/armem/core/base/CoreSegmentBase.h
+++ b/source/RobotAPI/libraries/armem/core/base/CoreSegmentBase.h
@@ -33,6 +33,22 @@ namespace armarx::armem::base
using EntitySnapshotT = typename EntityT::EntitySnapshotT;
using EntityInstanceT = typename EntitySnapshotT::EntityInstanceT;
+ struct UpdateResult
+ {
+ armarx::armem::UpdateType coreSegmentUpdateType;
+ armarx::armem::UpdateType providerSegmentUpdateType;
+ armarx::armem::UpdateType entityUpdateType;
+ MemoryID id;
+ std::vector<EntitySnapshotT> removedSnapshots;
+
+ UpdateResult() = default;
+ UpdateResult(const typename ProviderSegmentT::UpdateResult& c) :
+ providerSegmentUpdateType(c.providerSegmentUpdateType),
+ entityUpdateType(c.entityUpdateType),
+ id(c.id),
+ removedSnapshots(c.removedSnapshots)
+ {}
+ };
public:
@@ -69,13 +85,13 @@ namespace armarx::armem::base
}
- inline const std::map<std::string, ProviderSegmentT>& providerSegments() const
+ inline const auto& providerSegments() const
{
return this->_container;
}
- inline std::map<std::string, ProviderSegmentT>& providerSegments()
+ inline auto& providerSegments()
{
- return const_cast<std::map<std::string, ProviderSegmentT>&>(const_cast<const CoreSegmentBase*>(this)->providerSegments());
+ return this->_container;
}
@@ -98,7 +114,7 @@ namespace armarx::armem::base
}
else
{
- throw armem::error::MissingEntry("provider segment", name, getLevelName(), this->getKeyString());
+ throw armem::error::MissingEntry::create<ProviderSegmentT>(name, *this);
}
}
@@ -129,26 +145,60 @@ namespace armarx::armem::base
}
}
- virtual MemoryID update(const EntityUpdate& update) override
+ /**
+ * @brief Updates an entity's history.
+ *
+ * Missing entity entries are added before updating.
+ */
+ UpdateResult update(const EntityUpdate& update)
{
this->_checkContainerName(update.entityID.coreSegmentName, this->name());
+ ProviderSegmentT* provSeg;
+ UpdateType updateType = UpdateType::InsertedNew;
+
auto it = this->_container.find(update.entityID.providerSegmentName);
- if (it != this->_container.end())
+ if (it == this->_container.end())
{
- return it->second.update(update);
+ if (_addMissingProviderSegmentDuringUpdate)
+ {
+ // Insert into map.
+ provSeg = &addProviderSegment(update.entityID.providerSegmentName);
+ provSeg->setMaxHistorySize(_maxHistorySize);
+ updateType = UpdateType::InsertedNew;
+ }
+ else
+ {
+ throw error::MissingEntry::create<EntitySnapshotT>(update.entityID.providerSegmentName, *this);
+ }
}
else
{
- if (_addMissingProviderSegmentDuringUpdate)
+ provSeg = &it->second;
+ updateType = UpdateType::UpdatedExisting;
+ }
+
+ // Update entry.
+ UpdateResult ret(provSeg->update(update));
+ ret.coreSegmentUpdateType = updateType;
+ return ret;
+ }
+
+ void append(const _Derived& m)
+ {
+ ARMARX_INFO << "CoreSegment: Merge name '" << m.name() << "' into '" << name() << "'";
+
+ for (const auto& [k, s] : m.container())
+ {
+ if (const auto& it = this->_container.find(k); it != this->_container.end())
{
- // Add the missing provider segment (with this core segment's type).
- ProviderSegmentT& provSeg = addProviderSegment(update.entityID.providerSegmentName);
- return provSeg.update(update);
+ // segment already exists
+ it->second.append(s);
}
else
{
- throw armem::error::MissingEntry("provider segment", update.entityID.providerSegmentName, getLevelName(), this->getKeyString());
+ auto wms = this->_container.emplace(k, this->id().withProviderSegmentName(k));
+ wms.first->second.append(s);
}
}
}
diff --git a/source/RobotAPI/libraries/armem/core/base/EntityBase.h b/source/RobotAPI/libraries/armem/core/base/EntityBase.h
index 39dcc12c92902bad4c7a2e0c9cb7385de8e0a40c..ab227c33ee321475f129d8f864b5cbf31e3f4845 100644
--- a/source/RobotAPI/libraries/armem/core/base/EntityBase.h
+++ b/source/RobotAPI/libraries/armem/core/base/EntityBase.h
@@ -52,6 +52,14 @@ namespace armarx::armem::base
using EntitySnapshotT = _EntitySnapshotT;
using EntityInstanceT = typename EntitySnapshotT::EntityInstanceT;
+ struct UpdateResult
+ {
+ armarx::armem::UpdateType entityUpdateType;
+ MemoryID id;
+ std::vector<EntitySnapshotT> removedSnapshots;
+
+ UpdateResult() = default;
+ };
public:
@@ -119,7 +127,12 @@ namespace armarx::armem::base
/**
* @brief Indicates whether a history entry for the given time exists.
*/
- bool hasSnapshot(Time time) const
+ virtual bool hasSnapshot(const Time& time)
+ {
+ return const_cast<const EntityBase*>(this)->hasSnapshot(time);
+ }
+
+ virtual bool hasSnapshot(const Time& time) const
{
return this->_container.count(time) > 0;
}
@@ -128,6 +141,11 @@ namespace armarx::armem::base
* @brief Get the latest timestamp.
* @throw `armem::error::EntityHistoryEmpty` If the history is empty.
*/
+ Time getLatestTimestamp()
+ {
+ return const_cast<const EntityBase*>(this)->getLatestTimestamp();
+ }
+
Time getLatestTimestamp() const
{
return getLatestItem().first;
@@ -136,7 +154,12 @@ namespace armarx::armem::base
/**
* @brief Get all timestamps in the history.
*/
- std::vector<Time> getTimestamps() const
+ virtual std::vector<Time> getTimestamps()
+ {
+ return const_cast<const EntityBase*>(this)->getTimestamps();
+ }
+
+ virtual std::vector<Time> getTimestamps() const
{
return simox::alg::get_keys(this->_container);
}
@@ -148,14 +171,13 @@ namespace armarx::armem::base
* @return The snapshot, if it exists.
*
* @throws `armem::error::MissingEntry` If there is no such entry.
- * @throws `armem::error::MissingData` If the entry has no data.
*/
- EntitySnapshotT& getSnapshot(Time time)
+ virtual EntitySnapshotT& getSnapshot(const Time& time)
{
return const_cast<EntitySnapshotT&>(const_cast<const EntityBase*>(this)->getSnapshot(time));
}
- const EntitySnapshotT& getSnapshot(Time time) const
+ virtual const EntitySnapshotT& getSnapshot(const Time& time) const
{
auto it = this->_container.find(time);
if (it != this->_container.end())
@@ -164,7 +186,7 @@ namespace armarx::armem::base
}
else
{
- throw error::MissingEntry("entity snapshot", toDateTimeMilliSeconds(time), getLevelName(), this->id().str());
+ throw armem::error::MissingEntry::create<EntitySnapshotT>(toDateTimeMilliSeconds(time), *this);
}
}
@@ -175,7 +197,7 @@ namespace armarx::armem::base
const EntitySnapshotT& getSnapshot(const MemoryID& id) const
{
- checkEntityName(id.entityName);
+ this->_checkContainerName(id.entityName, this->name());
return getSnapshot(id.timestamp);
}
@@ -183,7 +205,6 @@ namespace armarx::armem::base
* @brief Return the snapshot with the most recent timestamp.
* @return The latest snapshot.
* @throw `armem::error::EntityHistoryEmpty` If the history is empty.
- * @throw `armem::error::MissingData` If the latest snapshot has no data.
*/
EntitySnapshotT& getLatestSnapshot()
{
@@ -195,16 +216,153 @@ namespace armarx::armem::base
return getLatestItem().second;
}
+ /**
+ * @brief Return the lastest snapshot before or at time.
+ * @param time The time.
+ * @return The latest snapshot.
+ * @throw `armem::error::EntityHistoryEmpty` If the history is empty.
+ * @throw `armem::error::MissingEntry` If no such snapshot
+ */
+ virtual const EntitySnapshotT& getLatestSnapshotBeforeOrAt(const Time& time) const
+ {
+ // first element equal or greater
+ typename std::map<Time, EntitySnapshotT>::const_iterator refIt = this->_container.upper_bound(time);
+
+ if (refIt == this->_container.begin())
+ {
+ if (refIt->first == time)
+ {
+ return refIt->second;
+ }
+ throw error::MissingEntry::create<EntitySnapshotT>(toDateTimeMilliSeconds(time), *this);
+ }
+
+ // last element less than
+ typename std::map<Time, EntitySnapshotT>::const_iterator refItPrev = std::prev(refIt);
+
+ // ... or we return the element before if possible
+ if (refItPrev != this->_container.begin())
+ {
+ return refItPrev->second;
+ }
+
+ throw error::MissingEntry::create<EntitySnapshotT>(toDateTimeMilliSeconds(time), *this);
+ }
+
+ /**
+ * @brief Return the lastest snapshot before time.
+ * @param time The time.
+ * @return The latest snapshot.
+ * @throw `armem::error::EntityHistoryEmpty` If the history is empty.
+ * @throw `armem::error::MissingEntry` If no such snapshot
+ */
+ virtual const EntitySnapshotT& getLatestSnapshotBefore(const Time& time) const
+ {
+ // first element equal or greater
+ typename std::map<Time, EntitySnapshotT>::const_iterator refIt = this->_container.upper_bound(time);
+
+ if (refIt == this->_container.begin())
+ {
+ throw error::MissingEntry::create<EntitySnapshotT>(toDateTimeMilliSeconds(time), *this);
+ }
+
+ // last element less than
+ typename std::map<Time, EntitySnapshotT>::const_iterator refItPrev = std::prev(refIt);
+
+ // we return the element before if possible
+ if (refItPrev != this->_container.begin())
+ {
+ return refItPrev->second;
+ }
+
+ throw error::MissingEntry::create<EntitySnapshotT>(toDateTimeMilliSeconds(time), *this);
+ }
+
+ /**
+ * @brief Return all snapshots before or at time.
+ * @param time The time.
+ * @return The latest snapshots.
+ * @throw `armem::error::EntityHistoryEmpty` If the history is empty.
+ * @throw `armem::error::MissingEntry` If no such snapshots
+ */
+ virtual const std::vector<std::reference_wrapper<const EntitySnapshotT>> getSnapshotsBefore(const Time& time) const
+ {
+ std::vector<std::reference_wrapper<const EntitySnapshotT>> ret;
+ const EntitySnapshotT& latest = getLatestSnapshotBefore(time);
+
+ for (const auto& [timestamp, snapshot] : this->_container)
+ {
+ ret.emplace_back(snapshot);
+ if (timestamp == latest.id().timestamp)
+ {
+ break;
+ }
+ }
+ return ret;
+ }
+
+ /**
+ * @brief Return first snapshot after or at time.
+ * @param time The time.
+ * @return The first snapshot.
+ * @throw `armem::error::EntityHistoryEmpty` If the history is empty.
+ * @throw `armem::error::MissingEntry` If no such snapshot
+ */
+ virtual const EntitySnapshotT& getFirstSnapshotAfterOrAt(const Time& time) const
+ {
+ // first element equal or greater
+ typename std::map<Time, EntitySnapshotT>::const_iterator refIt = this->_container.upper_bound(time);
+
+ if (refIt == this->_container.end())
+ {
+ throw error::MissingEntry::create<EntitySnapshotT>(toDateTimeMilliSeconds(time), *this);
+ }
+ return refIt->second;
+ }
+
+ /**
+ * @brief Return first snapshot after time.
+ * @param time The time.
+ * @return The first snapshot.
+ * @throw `armem::error::EntityHistoryEmpty` If the history is empty.
+ * @throw `armem::error::MissingEntry` If no such snapshot
+ */
+ virtual const EntitySnapshotT& getFirstSnapshotAfter(const Time& time) const
+ {
+ // first element equal or greater
+ typename std::map<Time, EntitySnapshotT>::const_iterator refIt = this->_container.upper_bound(time);
+
+ if (refIt == this->_container.end())
+ {
+ throw error::MissingEntry::create<EntitySnapshotT>(toDateTimeMilliSeconds(time), *this);
+ }
+
+ if (refIt->first > time)
+ {
+ return refIt->second;
+ }
+
+ // first element greater than
+ typename std::map<Time, EntitySnapshotT>::const_iterator refItNext = std::next(refIt);
+
+ if (refItNext != this->_container.end())
+ {
+ return refItNext->second;
+ }
+
+ throw error::MissingEntry::create<EntitySnapshotT>(toDateTimeMilliSeconds(time), *this);
+ }
+
/**
* @brief Add the given update to this entity's history.
* @param update The update.
* @return The snapshot ID of the update.
*/
- virtual MemoryID update(const EntityUpdate& update)
+ UpdateResult update(const EntityUpdate& update)
{
- checkEntityName(update.entityID.entityName);
- this->id() = update.entityID;
+ this->_checkContainerName(update.entityID.entityName, this->name());
+ UpdateResult ret;
EntitySnapshotT* snapshot;
@@ -213,16 +371,39 @@ namespace armarx::armem::base
{
// Insert into history.
snapshot = &addSnapshot(update.timeCreated);
- truncateHistoryToSize();
+ ret.removedSnapshots = truncate();
+ ret.entityUpdateType = UpdateType::InsertedNew;
}
else
{
snapshot = &it->second;
+ ret.entityUpdateType = UpdateType::UpdatedExisting;
}
// Update entry.
- snapshot->update(update);
+ snapshot->update(update, this->id());
+ ret.id = snapshot->id();
- return snapshot->id();
+ return ret;
+ }
+
+ void append(const DerivedT& m)
+ {
+ ARMARX_INFO << "Entity: Merge name '" << m.name() << "' into '" << name() << "'";
+
+ for (const auto& [k, s] : m.container())
+ {
+ if (const auto& it = this->_container.find(k); it != this->_container.end())
+ {
+ // segment already exists
+ // We assume that a snapshot does not change, so ignore
+ }
+ else
+ {
+ auto snapshot = s.copy();
+ snapshot.id() = this->id().withTimestamp(k); // update id (e.g. memory name) if necessary
+ this->_container.insert(std::make_pair(k, std::move(snapshot)));
+ }
+ }
}
/**
@@ -256,7 +437,7 @@ namespace armarx::armem::base
void setMaxHistorySize(long maxSize) override
{
MaxHistorySize::setMaxHistorySize(maxSize);
- truncateHistoryToSize();
+ truncate();
}
std::string getKeyString() const override
@@ -269,27 +450,22 @@ namespace armarx::armem::base
}
- private:
+ protected:
/// If maximum size is set, ensure `history`'s is not higher.
- void truncateHistoryToSize()
+ std::vector<EntitySnapshotT> truncate()
{
+ std::vector<EntitySnapshotT> removedElements;
if (_maxHistorySize >= 0)
{
while (this->_container.size() > size_t(_maxHistorySize))
{
+ removedElements.push_back(std::move(this->_container.begin()->second));
this->_container.erase(this->_container.begin());
}
ARMARX_CHECK_LESS_EQUAL(this->_container.size(), _maxHistorySize);
}
- }
-
- void checkEntityName(const std::string& name) const
- {
- if (name != this->name())
- {
- throw armem::error::ContainerNameMismatch(name, getLevelName(), getKeyString());
- }
+ return removedElements;
}
/**
@@ -297,7 +473,7 @@ namespace armarx::armem::base
* @return The latest snapshot.
* @throw `armem::error::EntityHistoryEmpty` If the history is empty.
*/
- const typename std::map<Time, EntitySnapshotT>::value_type& getLatestItem() const
+ virtual const typename std::map<Time, EntitySnapshotT>::value_type& getLatestItem() const
{
if (this->_container.empty())
{
diff --git a/source/RobotAPI/libraries/armem/core/base/EntityInstanceBase.h b/source/RobotAPI/libraries/armem/core/base/EntityInstanceBase.h
index aab70193435c7eec54b9e2127e7c57528ebba842..8dd474c667687e6235a96a191e577163ca194cba 100644
--- a/source/RobotAPI/libraries/armem/core/base/EntityInstanceBase.h
+++ b/source/RobotAPI/libraries/armem/core/base/EntityInstanceBase.h
@@ -53,7 +53,6 @@ namespace armarx::armem::base
*/
virtual void update(const EntityUpdate& update, int index) = 0;
-
virtual bool equalsDeep(const DerivedT& other) const = 0;
virtual DerivedT copy() const
diff --git a/source/RobotAPI/libraries/armem/core/base/EntitySnapshotBase.h b/source/RobotAPI/libraries/armem/core/base/EntitySnapshotBase.h
index defdf1c049fa5274b0d112f42d78037730d9eff5..a74f41d3c0a9efd34d00b2f6fa6849bafd9dca1e 100644
--- a/source/RobotAPI/libraries/armem/core/base/EntitySnapshotBase.h
+++ b/source/RobotAPI/libraries/armem/core/base/EntitySnapshotBase.h
@@ -49,7 +49,7 @@ namespace armarx::armem::base
EntitySnapshotBase& operator=(EntitySnapshotBase&& other) = default;
- virtual bool equalsDeep(const EntitySnapshotBase& other) const
+ bool equalsDeep(const EntitySnapshotBase& other) const
{
//std::cout << "EntitySnapshot::equalsDeep" << std::endl;
if (this->size() != other.size())
@@ -131,7 +131,7 @@ namespace armarx::armem::base
}
else
{
- throw armem::error::MissingEntry(EntityInstanceT().getLevelName(), std::to_string(index), getLevelName(), toDateTimeMilliSeconds(time()));
+ throw armem::error::MissingEntry::create<EntityInstanceT>(std::to_string(index), *this);
}
}
diff --git a/source/RobotAPI/libraries/armem/core/base/MemoryBase.h b/source/RobotAPI/libraries/armem/core/base/MemoryBase.h
index 713ba580089443b9fc2ca6f982e72970aa28dde7..d7c8b8cd203fc8b140cbed0b959110f0868a4b83 100644
--- a/source/RobotAPI/libraries/armem/core/base/MemoryBase.h
+++ b/source/RobotAPI/libraries/armem/core/base/MemoryBase.h
@@ -33,6 +33,25 @@ namespace armarx::armem::base
using EntityInstanceT = typename EntitySnapshotT::EntityInstanceT;
+ struct UpdateResult
+ {
+ armarx::armem::UpdateType memoryUpdateType;
+ armarx::armem::UpdateType coreSegmentUpdateType;
+ armarx::armem::UpdateType providerSegmentUpdateType;
+ armarx::armem::UpdateType entityUpdateType;
+ MemoryID id;
+ std::vector<EntitySnapshotT> removedSnapshots;
+
+ UpdateResult() = default;
+ UpdateResult(const typename CoreSegmentT::UpdateResult& c) :
+ coreSegmentUpdateType(c.coreSegmentUpdateType),
+ providerSegmentUpdateType(c.providerSegmentUpdateType),
+ entityUpdateType(c.entityUpdateType),
+ id(c.id),
+ removedSnapshots(c.removedSnapshots)
+ {}
+ };
+
public:
MemoryBase()
@@ -63,13 +82,13 @@ namespace armarx::armem::base
}
- inline const std::map<std::string, CoreSegmentT>& coreSegments() const
+ inline auto& coreSegments() const
{
return this->_container;
}
- inline std::map<std::string, CoreSegmentT>& coreSegments()
+ inline auto& coreSegments()
{
- return const_cast<std::map<std::string, CoreSegmentT>&>(const_cast<const MemoryBase*>(this)->coreSegments());
+ return this->_container;
}
@@ -92,10 +111,35 @@ namespace armarx::armem::base
}
else
{
- throw armem::error::MissingEntry(CoreSegmentT().getLevelName(), name, getLevelName(), this->name());
+ throw armem::error::MissingEntry::create<CoreSegmentT>(name, *this);
+ }
+ }
+
+
+ bool hasProviderSegment(const MemoryID& providerSegmentID) const
+ {
+ auto it = this->_container.find(providerSegmentID.coreSegmentName);
+ if (it != this->_container.end())
+ {
+ return it->second.hasProviderSegment(providerSegmentID.providerSegmentName);
+ }
+ else
+ {
+ return false;
}
}
+ ProviderSegmentT& getProviderSegment(const MemoryID& providerSegmentID)
+ {
+ return getCoreSegment(providerSegmentID.coreSegmentName).getProviderSegment(providerSegmentID.providerSegmentName);
+ }
+
+ const ProviderSegmentT& getProviderSegment(const MemoryID& providerSegmentID) const
+ {
+ return getCoreSegment(providerSegmentID.coreSegmentName).getProviderSegment(providerSegmentID.providerSegmentName);
+ }
+
+
using Base::getEntity;
const EntityT& getEntity(const MemoryID& id) const override
{
@@ -173,18 +217,64 @@ namespace armarx::armem::base
return segments;
}
- virtual MemoryID update(const EntityUpdate& update) override
+
+ /**
+ * @brief Store all updates in `commit`.
+ * @param commit The commit.
+ * @return The resulting memory IDs.
+ */
+ std::vector<UpdateResult> update(const Commit& commit)
+ {
+ std::vector<UpdateResult> results;
+ for (const auto& update : commit.updates)
+ {
+ results.push_back(this->update(update));
+ }
+ return results;
+ }
+
+ /**
+ * @brief Store the given update.
+ * @param update The update.
+ * @return The resulting entity snapshot's ID.
+ */
+ UpdateResult update(const EntityUpdate& update)
{
this->_checkContainerName(update.entityID.memoryName, this->name());
auto it = this->_container.find(update.entityID.coreSegmentName);
if (it != this->_container.end())
{
- return it->second.update(update);
+ UpdateResult ret(it->second.update(update));
+ ret.memoryUpdateType = UpdateType::UpdatedExisting;
+ return ret;
}
else
{
- throw armem::error::MissingEntry(CoreSegmentT().getLevelName(), update.entityID.coreSegmentName, getLevelName(), this->name());
+ throw armem::error::MissingEntry::create<CoreSegmentT>(update.entityID.coreSegmentName, *this);
+ }
+ }
+
+ /**
+ * @brief Merge another memory into this one. Append all data
+ * @param m The other memory
+ */
+ void append(const _Derived& m)
+ {
+ ARMARX_INFO << "Memory: Merge name '" << m.name() << "' into '" << name() << "'";
+
+ for (const auto& [k, s] : m.container())
+ {
+ if (const auto& it = this->_container.find(k); it != this->_container.end())
+ {
+ // segment already exists
+ it->second.append(s);
+ }
+ else
+ {
+ auto wms = this->_container.emplace(k, this->id().withCoreSegmentName(k));
+ wms.first->second.append(s);
+ }
}
}
diff --git a/source/RobotAPI/libraries/armem/core/base/ProviderSegmentBase.h b/source/RobotAPI/libraries/armem/core/base/ProviderSegmentBase.h
index 503650498d9afb5ddcdbbf22ecb84baaae3201f8..57ff3210244fa516551f4906a2ea07c2cc81f09d 100644
--- a/source/RobotAPI/libraries/armem/core/base/ProviderSegmentBase.h
+++ b/source/RobotAPI/libraries/armem/core/base/ProviderSegmentBase.h
@@ -32,6 +32,20 @@ namespace armarx::armem::base
using EntitySnapshotT = typename EntityT::EntitySnapshotT;
using EntityInstanceT = typename EntitySnapshotT::EntityInstanceT;
+ struct UpdateResult
+ {
+ armarx::armem::UpdateType providerSegmentUpdateType;
+ armarx::armem::UpdateType entityUpdateType;
+ MemoryID id;
+ std::vector<EntitySnapshotT> removedSnapshots;
+
+ UpdateResult() = default;
+ UpdateResult(const typename EntityT::UpdateResult& c) :
+ entityUpdateType(c.entityUpdateType),
+ id(c.id),
+ removedSnapshots(c.removedSnapshots)
+ {}
+ };
public:
@@ -69,13 +83,13 @@ namespace armarx::armem::base
}
- inline const std::map<std::string, EntityT>& entities() const
+ inline const auto& entities() const
{
return this->_container;
}
- inline std::map<std::string, EntityT>& entities()
+ inline auto& entities()
{
- return const_cast<std::map<std::string, EntityT>&>(const_cast<const ProviderSegmentBase*>(this)->entities());
+ return this->_container;
}
@@ -105,7 +119,7 @@ namespace armarx::armem::base
}
else
{
- throw error::MissingEntry("entity", name, getLevelName(), this->name());
+ throw armem::error::MissingEntry::create<EntityT>(name, *this);
}
}
@@ -128,24 +142,49 @@ namespace armarx::armem::base
*
* Missing entity entries are added before updating.
*/
- virtual MemoryID update(const EntityUpdate& update) override
+ UpdateResult update(const EntityUpdate& update)
{
this->_checkContainerName(update.entityID.providerSegmentName, this->name());
EntityT* entity;
- auto it = this->_container.find(update.entityID.providerSegmentName);
+ UpdateType updateType = UpdateType::InsertedNew;
+
+ auto it = this->_container.find(update.entityID.entityName);
if (it == this->_container.end())
{
// Add entity entry.
entity = &addEntity(update.entityID.entityName);
entity->setMaxHistorySize(_maxHistorySize);
+ updateType = UpdateType::InsertedNew;
}
else
{
entity = &it->second;
+ updateType = UpdateType::UpdatedExisting;
}
// Update entity.
- return entity->update(update);
+ UpdateResult ret(entity->update(update));
+ ret.providerSegmentUpdateType = updateType;
+ return ret;
+ }
+
+ void append(const _Derived& m)
+ {
+ ARMARX_INFO << "ProviderSegment: Merge name '" << m.name() << "' into '" << name() << "'";
+
+ for (const auto& [k, s] : m.container())
+ {
+ if (const auto& it = this->_container.find(k); it != this->_container.end())
+ {
+ // segment already exists
+ it->second.append(s);
+ }
+ else
+ {
+ auto wms = this->_container.emplace(k, this->id().withEntityName(k));
+ wms.first->second.append(s);
+ }
+ }
}
/// Add an empty entity with the given name.
diff --git a/source/RobotAPI/libraries/armem/core/base/detail/EntityContainerBase.h b/source/RobotAPI/libraries/armem/core/base/detail/EntityContainerBase.h
index 09733b648d096c7e33179c2903f3dbe241dca8d2..95993d560c6435fd147ddfc7a2a7aa9267d94855 100644
--- a/source/RobotAPI/libraries/armem/core/base/detail/EntityContainerBase.h
+++ b/source/RobotAPI/libraries/armem/core/base/detail/EntityContainerBase.h
@@ -32,42 +32,18 @@ namespace armarx::armem::base::detail
using EntitySnapshotT = typename EntityT::EntitySnapshotT;
using EntityInstanceT = typename EntitySnapshotT::EntityInstanceT;
-
public:
using Base::MemoryContainerBase;
using Base::operator=;
-
- /**
- * @brief Store all updates in `commit`.
- * @param commit The commit.
- * @return The resulting memory IDs.
- */
- std::vector<MemoryID> update(const Commit& commit)
- {
- std::vector<MemoryID> ids;
- for (const auto& update : commit.updates)
- {
- ids.push_back(this->update(update));
- }
- return ids;
- }
- /**
- * @brief Store the given update.
- * @param update The update.
- * @return The resulting entity snapshot's ID.
- */
- virtual MemoryID update(const EntityUpdate& update) = 0;
-
-
/**
* @brief Retrieve an entity.
* @param id The entity ID.
* @return The entity.
* @throw An exception deriving from `armem::error::ArMemError` if the entity is missing.
*/
- virtual EntityT& getEntity(const MemoryID& id)
+ EntityT& getEntity(const MemoryID& id)
{
return const_cast<EntityT&>(const_cast<const EntityContainerBase*>(this)->getEntity(id));
}
@@ -97,12 +73,12 @@ namespace armarx::armem::base::detail
* @return The entity snapshot.
* @throw An exception deriving from `armem::error::ArMemError` if the snapshot is missing.
*/
- virtual EntitySnapshotT& getEntitySnapshot(const MemoryID& id)
+ EntitySnapshotT& getEntitySnapshot(const MemoryID& id)
{
return const_cast<EntitySnapshotT&>(const_cast<const EntityContainerBase*>(this)->getEntitySnapshot(id));
}
- virtual const EntitySnapshotT& getEntitySnapshot(const MemoryID& id) const
+ const EntitySnapshotT& getEntitySnapshot(const MemoryID& id) const
{
const EntityT& entity = getEntity(id);
@@ -116,12 +92,12 @@ namespace armarx::armem::base::detail
}
}
- virtual EntityInstanceT& getEntityInstance(const MemoryID& id)
+ EntityInstanceT& getEntityInstance(const MemoryID& id)
{
return getEntitySnapshot(id).getInstance(id);
}
- virtual const EntityInstanceT& getEntityInstance(const MemoryID& id) const
+ const EntityInstanceT& getEntityInstance(const MemoryID& id) const
{
return getEntitySnapshot(id).getInstance(id);
}
diff --git a/source/RobotAPI/libraries/armem/core/base/detail/MaxHistorySize.h b/source/RobotAPI/libraries/armem/core/base/detail/MaxHistorySize.h
index 69539df5c4063b8d27cd751b79ee82eb90ff5b7e..1fc07ed85e54716d24d2447fa17cb95b291f2766 100644
--- a/source/RobotAPI/libraries/armem/core/base/detail/MaxHistorySize.h
+++ b/source/RobotAPI/libraries/armem/core/base/detail/MaxHistorySize.h
@@ -4,6 +4,7 @@
namespace armarx::armem::base::detail
{
+ // TODO: Replace by ConstrainedHistorySize (not only max entries, e.g. delete oldest / delete least accessed / ...)
class MaxHistorySize
{
public:
diff --git a/source/RobotAPI/libraries/armem/core/base/detail/MemoryContainerBase.h b/source/RobotAPI/libraries/armem/core/base/detail/MemoryContainerBase.h
index b2155c70dbf645ebe0e2783fd3189bf9b58800af..1a6a216653d243da9613ec967363dc5d44b5c503 100644
--- a/source/RobotAPI/libraries/armem/core/base/detail/MemoryContainerBase.h
+++ b/source/RobotAPI/libraries/armem/core/base/detail/MemoryContainerBase.h
@@ -7,7 +7,6 @@
namespace armarx::armem::base::detail
{
-
/**
* @class Provides default implmentations of `MemoryContainer`, as well as
* iterators (which requires a template).
@@ -19,7 +18,6 @@ namespace armarx::armem::base::detail
using Base = MemoryItem;
public:
-
using DerivedT = _DerivedT;
using ContainerT = _ContainerT;
@@ -42,15 +40,19 @@ namespace armarx::armem::base::detail
// Container methods
- virtual bool empty() const
+ bool empty() const
{
return _container.empty();
}
- virtual std::size_t size() const
+ std::size_t size() const
{
return _container.size();
}
- virtual void clear()
+ bool hasData() const
+ {
+ return size() > 0;
+ }
+ void clear()
{
return _container.clear();
}
@@ -81,10 +83,8 @@ namespace armarx::armem::base::detail
return _container;
}
-
// Copying
-
- virtual DerivedT copy() const
+ DerivedT copy() const
{
DerivedT d;
this->_copySelf(d);
@@ -92,7 +92,7 @@ namespace armarx::armem::base::detail
}
/// Make a copy not containing any elements.
- virtual DerivedT copyEmpty() const
+ DerivedT copyEmpty() const
{
DerivedT d;
this->_copySelfEmpty(d);
@@ -128,7 +128,7 @@ namespace armarx::armem::base::detail
protected:
- ContainerT _container;
+ mutable ContainerT _container;
};
diff --git a/source/RobotAPI/libraries/armem/core/diskmemory/CoreSegment.cpp b/source/RobotAPI/libraries/armem/core/diskmemory/CoreSegment.cpp
index 6a26be9c7915a54b081e5d03b4f67cb6ae9eeaee..dd25b0219a6e0b7559c875ab98446d86ec3fa60f 100644
--- a/source/RobotAPI/libraries/armem/core/diskmemory/CoreSegment.cpp
+++ b/source/RobotAPI/libraries/armem/core/diskmemory/CoreSegment.cpp
@@ -58,7 +58,7 @@ namespace armarx::armem::d_ltm
if (d.is_directory())
{
std::string k = d.path().filename();
- auto wms = _container.emplace(std::make_pair(k, id().withProviderSegmentName(k)));
+ auto wms = _container.emplace(k, id().withProviderSegmentName(k));
wms.first->second.reload(p_ptr);
}
@@ -87,7 +87,7 @@ namespace armarx::armem::d_ltm
else
{
std::filesystem::create_directory(_fullPath() / k);
- auto wms = _container.emplace(std::make_pair(k, id().withProviderSegmentName(k)));
+ auto wms = _container.emplace(k, id().withProviderSegmentName(k));
wms.first->second.path = path;
wms.first->second.append(s);
}
diff --git a/source/RobotAPI/libraries/armem/core/diskmemory/EntityInstance.cpp b/source/RobotAPI/libraries/armem/core/diskmemory/EntityInstance.cpp
index 49bfe419d8a8cf1052fad7e47a0ecba9e355c529..4da151bd537f98e0e5693d948f202d68859f0d76 100644
--- a/source/RobotAPI/libraries/armem/core/diskmemory/EntityInstance.cpp
+++ b/source/RobotAPI/libraries/armem/core/diskmemory/EntityInstance.cpp
@@ -4,12 +4,9 @@
#include <fstream>
#include "../../core/error.h"
+#include "ArmarXCore/core/exceptions/LocalException.h"
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-#include <RobotAPI/libraries/aron/core/io/dataIO/converter/Converter.h>
-#include <RobotAPI/libraries/aron/core/io/dataIO/visitor/Visitor.h>
-#include <RobotAPI/libraries/aron/core/io/dataIO/reader/nlohmannJSON/NlohmannJSONReader.h>
-#include <RobotAPI/libraries/aron/core/io/dataIO/writer/nlohmannJSON/NlohmannJSONWriter.h>
namespace armarx::armem::d_ltm
{
@@ -69,13 +66,13 @@ namespace armarx::armem::d_ltm
{
std::ifstream ifs(d);
std::string file_content((std::istreambuf_iterator<char>(ifs)), (std::istreambuf_iterator<char>()));
- aron::dataIO::reader::NlohmannJSONReader dataReader(file_content);
- aron::dataIO::writer::NavigatorWriter navWriter;
- aron::dataIO::Converter::ReadAndConvert(dataReader, navWriter, expectedStructure);
-
- aron::datanavigator::DictNavigatorPtr aron = aron::datanavigator::DictNavigator::DynamicCastAndCheck(navWriter.getResult());
- return unwrapData(aron);
+ nlohmann::json j(file_content);
+ auto aron = std::make_shared<aron::datanavigator::DictNavigator>();
+ to_aron(aron, j, expectedStructure);
+ wm::EntityInstance e(id());
+ from_aron(aron, e);
+ return e;
}
else
{
@@ -103,7 +100,16 @@ namespace armarx::armem::d_ltm
void EntityInstance::setTo(const wm::EntityInstance& m)
{
std::filesystem::path p = _fullPath();
- std::filesystem::create_directories(p);
+
+ try
+ {
+ std::filesystem::create_directories(p);
+ }
+ catch (...)
+ {
+ ARMARX_WARNING << GetHandledExceptionString();
+ return;
+ }
std::filesystem::path d = p / (std::string(DATA_FILENAME) + ".json");
@@ -115,71 +121,12 @@ namespace armarx::armem::d_ltm
std::ofstream ofs;
ofs.open(d);
- aron::datanavigator::DictNavigatorPtr aron = wrapData(m);
- aron::dataIO::writer::NlohmannJSONWriter dataWriter;
- aron::dataIO::Visitor::VisitAndSetup(dataWriter, aron);
- std::string new_file_full_content = dataWriter.getResult().dump(2);
+ auto aron = std::make_shared<aron::datanavigator::DictNavigator>();
+ to_aron(aron, m);
+ nlohmann::json j;
+ from_aron(aron, j);
- ofs << new_file_full_content;
+ ofs << j.dump(2);
ofs.close();
}
-
-
- wm::EntityInstance EntityInstance::unwrapData(const aron::datanavigator::DictNavigatorPtr& dataWrapped) const
- {
- wm::EntityInstance e(id());
- wm::EntityInstanceMetadata& metadata = e.metadata();
-
- if (dataWrapped->hasElement(DATA_WRAPPER_DATA_FIELD))
- {
- aron::datanavigator::DictNavigatorPtr data = aron::datanavigator::DictNavigator::DynamicCastAndCheck(dataWrapped->getElement(DATA_WRAPPER_DATA_FIELD));
- e.setData(data);
- }
-
- auto timeCreated = aron::datanavigator::LongNavigator::DynamicCastAndCheck(dataWrapped->getElement(DATA_WRAPPER_TIME_CREATED_FIELD));
- metadata.timeCreated = Time::microSeconds(timeCreated->toAronLongPtr()->value);
-
- auto timeSent = aron::datanavigator::LongNavigator::DynamicCastAndCheck(dataWrapped->getElement(DATA_WRAPPER_TIME_SENT_FIELD));
- metadata.timeSent = Time::microSeconds(timeSent->toAronLongPtr()->value);
-
- auto timeArrived = aron::datanavigator::LongNavigator::DynamicCastAndCheck(dataWrapped->getElement(DATA_WRAPPER_TIME_ARRIVED_FIELD));
- metadata.timeArrived = Time::microSeconds(timeArrived->toAronLongPtr()->value);
-
- auto confidence = aron::datanavigator::DoubleNavigator::DynamicCastAndCheck(dataWrapped->getElement(DATA_WRAPPER_CONFIDENCE_FIELD));
- metadata.confidence = static_cast<float>(confidence->toAronDoublePtr()->value);
-
- return e;
- }
-
- aron::datanavigator::DictNavigatorPtr EntityInstance::wrapData(const wm::EntityInstance& e) const
- {
- auto dataWrapped = std::make_shared<aron::datanavigator::DictNavigator>();
- if (e.data())
- {
- dataWrapped->addElement(DATA_WRAPPER_DATA_FIELD, e.data());
- }
-
- auto timeWrapped = std::make_shared<aron::datanavigator::LongNavigator>();
- timeWrapped->setValue(Time::now().toMicroSeconds());
- dataWrapped->addElement(DATA_WRAPPER_TIME_STORED_FIELD, timeWrapped);
-
- const wm::EntityInstanceMetadata& metadata = e.metadata();
- auto timeCreated = std::make_shared<aron::datanavigator::LongNavigator>();
- timeCreated->setValue(metadata.timeCreated.toMicroSeconds());
- dataWrapped->addElement(DATA_WRAPPER_TIME_CREATED_FIELD, timeCreated);
-
- auto timeSent = std::make_shared<aron::datanavigator::LongNavigator>();
- timeSent->setValue(metadata.timeSent.toMicroSeconds());
- dataWrapped->addElement(DATA_WRAPPER_TIME_SENT_FIELD, timeSent);
-
- auto timeArrived = std::make_shared<aron::datanavigator::LongNavigator>();
- timeArrived->setValue(metadata.timeArrived.toMicroSeconds());
- dataWrapped->addElement(DATA_WRAPPER_TIME_ARRIVED_FIELD, timeArrived);
-
- auto confidence = std::make_shared<aron::datanavigator::DoubleNavigator>();
- confidence->setValue(static_cast<double>(metadata.confidence));
- dataWrapped->addElement(DATA_WRAPPER_CONFIDENCE_FIELD, confidence);
-
- return dataWrapped;
- }
}
diff --git a/source/RobotAPI/libraries/armem/core/diskmemory/EntityInstance.h b/source/RobotAPI/libraries/armem/core/diskmemory/EntityInstance.h
index 9ee48c55b33dca02c88f57de1ac30426f8d5ae37..89b96c011fcccb6bfc5471850185b0cea49f77b0 100644
--- a/source/RobotAPI/libraries/armem/core/diskmemory/EntityInstance.h
+++ b/source/RobotAPI/libraries/armem/core/diskmemory/EntityInstance.h
@@ -5,6 +5,9 @@
#include "../base/EntityInstanceBase.h"
#include "../workingmemory/EntityInstance.h"
+#include "../workingmemory/entityInstance_conversions.h"
+#include "../workingmemory/json_conversions.h"
+
namespace armarx::armem::d_ltm
{
@@ -51,19 +54,10 @@ namespace armarx::armem::d_ltm
std::filesystem::path _fullPath() const;
std::filesystem::path _fullPath(const std::filesystem::path&) const;
- wm::EntityInstance unwrapData(const aron::datanavigator::DictNavigatorPtr&) const;
- aron::datanavigator::DictNavigatorPtr wrapData(const wm::EntityInstance&) const;
-
public:
std::shared_ptr<std::filesystem::path> path;
private:
static const constexpr char* DATA_FILENAME = "data";
- static constexpr const char* DATA_WRAPPER_DATA_FIELD = "__ARON_DATA";
- static constexpr const char* DATA_WRAPPER_TIME_STORED_FIELD = "__WRITER_METADATA__TIME_STORED";
- static constexpr const char* DATA_WRAPPER_TIME_CREATED_FIELD = "__ENTITY_METADATA__TIME_CREATED";
- static constexpr const char* DATA_WRAPPER_TIME_SENT_FIELD = "__ENTITY_METADATA__TIME_SENT";
- static constexpr const char* DATA_WRAPPER_TIME_ARRIVED_FIELD = "__ENTITY_METADATA__TIME_ARRIVED";
- static constexpr const char* DATA_WRAPPER_CONFIDENCE_FIELD = "__ENTITY_METADATA__CONFIDENCE";
};
}
diff --git a/source/RobotAPI/libraries/armem/core/diskmemory/EntitySnapshot.cpp b/source/RobotAPI/libraries/armem/core/diskmemory/EntitySnapshot.cpp
index 199e4f2c9b9eed9d9b099e773d96ba0a03fd7d16..cc1124ab2335d1acf69550350a0bc980c0af7b7f 100644
--- a/source/RobotAPI/libraries/armem/core/diskmemory/EntitySnapshot.cpp
+++ b/source/RobotAPI/libraries/armem/core/diskmemory/EntitySnapshot.cpp
@@ -69,7 +69,15 @@ namespace armarx::armem::d_ltm
void EntitySnapshot::setTo(const wm::EntitySnapshot& m)
{
- std::filesystem::create_directories(_fullPath());
+ try
+ {
+ std::filesystem::create_directories(_fullPath());
+ }
+ catch (...)
+ {
+ ARMARX_WARNING << GetHandledExceptionString();
+ return;
+ }
// We remove the contente here and reset it with new values
_container.clear();
@@ -77,7 +85,15 @@ namespace armarx::armem::d_ltm
int i = 0;
for (const auto& s : m.instances())
{
- std::filesystem::create_directory(_fullPath() / std::to_string(i));
+ try
+ {
+ std::filesystem::create_directory(_fullPath() / std::to_string(i));
+ }
+ catch (...)
+ {
+ ARMARX_WARNING << GetHandledExceptionString();
+ continue;;
+ }
auto wms = _container.emplace_back(id().withInstanceIndex(i++));
wms.path = path;
diff --git a/source/RobotAPI/libraries/armem/core/diskmemory/Memory.cpp b/source/RobotAPI/libraries/armem/core/diskmemory/Memory.cpp
index 874d189d6a22a79c9e015d800c292f9af075cfea..1038f5bfbed5d2f19b06819471e65cb5d0aab042 100644
--- a/source/RobotAPI/libraries/armem/core/diskmemory/Memory.cpp
+++ b/source/RobotAPI/libraries/armem/core/diskmemory/Memory.cpp
@@ -55,7 +55,7 @@ namespace armarx::armem::d_ltm
if (d.is_directory())
{
std::string k = d.path().filename();
- auto wms = _container.emplace(std::make_pair(k, id().withCoreSegmentName(k)));
+ auto wms = _container.emplace(k, id().withCoreSegmentName(k));
wms.first->second.reload(path);
}
}
@@ -75,7 +75,7 @@ namespace armarx::armem::d_ltm
{
std::filesystem::create_directory(_fullPath() / k);
- auto wms = _container.emplace(std::make_pair(k, id().withCoreSegmentName(k)));
+ auto wms = _container.emplace(k, id().withCoreSegmentName(k));
wms.first->second.path = path;
wms.first->second.append(s);
}
diff --git a/source/RobotAPI/libraries/armem/core/diskmemory/ProviderSegment.cpp b/source/RobotAPI/libraries/armem/core/diskmemory/ProviderSegment.cpp
index 2d5ed3d4a588827137b33e69ecaec3be1a4c31fc..1132b2fca64d906276c60886e5d50486b7126c8b 100644
--- a/source/RobotAPI/libraries/armem/core/diskmemory/ProviderSegment.cpp
+++ b/source/RobotAPI/libraries/armem/core/diskmemory/ProviderSegment.cpp
@@ -64,7 +64,7 @@ namespace armarx::armem::d_ltm
if (d.is_directory())
{
std::string k = d.path().filename();
- auto wms = _container.emplace(std::make_pair(k, id().withEntityName(k)));
+ auto wms = _container.emplace(k, id().withEntityName(k));
wms.first->second.reload(p_ptr);
}
@@ -81,7 +81,18 @@ namespace armarx::armem::d_ltm
void ProviderSegment::append(const wm::ProviderSegment& m)
{
- std::filesystem::create_directories(_fullPath());
+
+ try
+ {
+ std::filesystem::create_directories(_fullPath());
+
+ }
+ catch (...)
+ {
+ ARMARX_WARNING << GetHandledExceptionString();
+ return;
+ }
+
TypeIO::writeAronType(_aronType, _fullPath());
for (const auto& [k, s] : m.container())
@@ -92,8 +103,20 @@ namespace armarx::armem::d_ltm
}
else
{
- std::filesystem::create_directory(_fullPath() / k);
- auto wms = _container.emplace(std::make_pair(k, id().withEntityName(k)));
+
+ try
+ {
+ std::filesystem::create_directory(_fullPath() / k);
+ continue;
+
+ }
+ catch (...)
+ {
+ ARMARX_WARNING << GetHandledExceptionString();
+ return;
+ }
+
+ auto wms = _container.emplace(k, id().withEntityName(k));
wms.first->second.path = path;
wms.first->second.append(s);
}
diff --git a/source/RobotAPI/libraries/armem/core/error.h b/source/RobotAPI/libraries/armem/core/error.h
index 36b7459745b3c0c7887151c9469b3ec54820e297..31bef6c2746eb276f23cb9102c61c295810c39d7 100644
--- a/source/RobotAPI/libraries/armem/core/error.h
+++ b/source/RobotAPI/libraries/armem/core/error.h
@@ -1,6 +1,7 @@
#pragma once
#include "error/ArMemError.h"
+#include "error/mns.h"
namespace armarx::armem::error
diff --git a/source/RobotAPI/libraries/armem/core/error/ArMemError.cpp b/source/RobotAPI/libraries/armem/core/error/ArMemError.cpp
index c44a40758a73ba64d8827c6f7a751b9df8fc00c3..8936fab6fe953bc3b7fee870a97ffee598234ea7 100644
--- a/source/RobotAPI/libraries/armem/core/error/ArMemError.cpp
+++ b/source/RobotAPI/libraries/armem/core/error/ArMemError.cpp
@@ -4,6 +4,8 @@
#include <SimoxUtility/algorithm/string/string_tools.h>
+#include "../MemoryID.h"
+
namespace armarx::armem::error
{
@@ -63,17 +65,19 @@ namespace armarx::armem::error
MissingEntry::MissingEntry(const std::string& missingTerm, const std::string& missingName,
- const std::string& ownTerm, const std::string& ownName) :
- ArMemError(makeMsg(missingTerm, missingName, ownTerm, ownName))
+ const std::string& containerTerm, const std::string& containerName,
+ size_t size) :
+ ArMemError(makeMsg(missingTerm, missingName, containerTerm, containerName, size))
{
}
std::string MissingEntry::makeMsg(const std::string& missingTerm, const std::string& missingName,
- const std::string& ownTerm, const std::string& ownName)
+ const std::string& containerTerm, const std::string& containerName,
+ size_t size)
{
std::stringstream ss;
ss << "No " << missingTerm << " with name '" << missingName << "' "
- << "in " << ownTerm << " '" << ownName << "'.";
+ << "in " << containerTerm << " '" << containerName << "' (with size " << size << ").";
return ss.str();
}
diff --git a/source/RobotAPI/libraries/armem/core/error/ArMemError.h b/source/RobotAPI/libraries/armem/core/error/ArMemError.h
index 3456c7a350835e6c67ca1d439d2fc0c44a8be507..e6843d13be5bc07e71d90b00cf6a911ca717e524 100644
--- a/source/RobotAPI/libraries/armem/core/error/ArMemError.h
+++ b/source/RobotAPI/libraries/armem/core/error/ArMemError.h
@@ -1,10 +1,14 @@
#pragma once
#include <stdexcept>
+
#include <SimoxUtility/meta/type_name.h>
-#include "../MemoryID.h"
+namespace armarx::armem
+{
+ class MemoryID;
+}
namespace armarx::armem::error
{
@@ -76,11 +80,22 @@ namespace armarx::armem::error
class MissingEntry : public ArMemError
{
public:
+
+ template <class MissingT, class ContainerT>
+ static MissingEntry create(const std::string& missingKey, const ContainerT& container)
+ {
+ return MissingEntry(MissingT().getLevelName(), missingKey,
+ container.getLevelName(), container.getKeyString(), container.size());
+ }
+
+
MissingEntry(const std::string& missingTerm, const std::string& missingName,
- const std::string& ownTerm, const std::string& ownName);
+ const std::string& containerTerm, const std::string& containerName,
+ size_t containerSize);
static std::string makeMsg(const std::string& missingTerm, const std::string& missingName,
- const std::string& ownTerm, const std::string& ownName);
+ const std::string& containerTerm, const std::string& containerName,
+ size_t size);
};
diff --git a/source/RobotAPI/libraries/armem/core/error/mns.cpp b/source/RobotAPI/libraries/armem/core/error/mns.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..da8b8f4855ac67e575cb1e9a677eea277dc1cc58
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/error/mns.cpp
@@ -0,0 +1,64 @@
+#include "mns.h"
+
+#include <sstream>
+
+#include "../MemoryID.h"
+
+
+namespace armarx::armem::error
+{
+
+ MemoryNameSystemQueryFailed::MemoryNameSystemQueryFailed(const std::string& function, const std::string& errorMessage) :
+ ArMemError(makeMsg(function, errorMessage))
+ {
+ }
+
+ std::string MemoryNameSystemQueryFailed::makeMsg(const std::string& function, const std::string& errorMessage)
+ {
+ std::stringstream ss;
+ ss << "Failed to call '" << function << "' on the memory name system.\n";
+ if (not errorMessage.empty())
+ {
+ ss << "\n" << errorMessage;
+ }
+ return ss.str();
+ }
+
+
+
+ CouldNotResolveMemoryServer::CouldNotResolveMemoryServer(const MemoryID& memoryID, const std::string& errorMessage) :
+ ArMemError(makeMsg(memoryID, errorMessage))
+ {
+ }
+
+ std::string CouldNotResolveMemoryServer::makeMsg(const MemoryID& memoryID, const std::string& errorMessage)
+ {
+ std::stringstream ss;
+ ss << "Could not resolve the memory name '" << memoryID << "'."
+ << "\nMemory server for '" << memoryID << "' is not registered.";
+ if (not errorMessage.empty())
+ {
+ ss << "\n" << errorMessage;
+ }
+ return ss.str();
+ }
+
+
+ ServerRegistrationOrRemovalFailed::ServerRegistrationOrRemovalFailed(const std::string& verb, const MemoryID& memoryID, const std::string& errorMessage) :
+ ArMemError(makeMsg(verb, memoryID, errorMessage))
+ {
+
+ }
+
+ std::string ServerRegistrationOrRemovalFailed::makeMsg(const std::string& verb, const MemoryID& memoryID, const std::string& errorMessage)
+ {
+ std::stringstream ss;
+ ss << "Failed to " << verb << " memory server for '" << memoryID << "' in the Memory Name System (MNS).";
+ if (not errorMessage.empty())
+ {
+ ss << "\n" << errorMessage;
+ }
+ return ss.str();
+ }
+
+}
diff --git a/source/RobotAPI/libraries/armem/core/error/mns.h b/source/RobotAPI/libraries/armem/core/error/mns.h
new file mode 100644
index 0000000000000000000000000000000000000000..428df8cae66494f72b1a56267a97b3942f63f7a6
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/error/mns.h
@@ -0,0 +1,52 @@
+#pragma once
+
+#include "ArMemError.h"
+
+
+namespace armarx::armem::error
+{
+
+ /**
+ * @brief Indicates that a query to the Memory Name System failed.
+ */
+ class MemoryNameSystemQueryFailed : public ArMemError
+ {
+ public:
+
+ MemoryNameSystemQueryFailed(const std::string& function, const std::string& errorMessage = "");
+
+ static std::string makeMsg(const std::string& function, const std::string& errorMessage = "");
+
+ };
+
+
+ /**
+ * @brief Indicates that a query to the Memory Name System failed.
+ */
+ class CouldNotResolveMemoryServer : public ArMemError
+ {
+ public:
+
+ CouldNotResolveMemoryServer(const MemoryID& memoryID, const std::string& errorMessage = "");
+
+ static std::string makeMsg(const MemoryID& memoryID, const std::string& errorMessage = "");
+
+ };
+
+
+ /**
+ * @brief Indicates that a query to the Memory Name System failed.
+ */
+ class ServerRegistrationOrRemovalFailed : public ArMemError
+ {
+ public:
+
+ ServerRegistrationOrRemovalFailed(const std::string& verb, const MemoryID& memoryID, const std::string& errorMessage = "");
+
+ static std::string makeMsg(const std::string& verb, const MemoryID& memoryID, const std::string& errorMessage = "");
+
+ };
+
+
+}
+
diff --git a/source/RobotAPI/libraries/armem/core/json_conversions.cpp b/source/RobotAPI/libraries/armem/core/json_conversions.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..e4da9d89b5e04351e5e528132406a962470364e3
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/json_conversions.cpp
@@ -0,0 +1,26 @@
+#include "json_conversions.h"
+
+#include <RobotAPI/libraries/armem/core/MemoryID.h>
+
+
+void armarx::armem::to_json(nlohmann::json& j, const MemoryID& bo)
+{
+ j["memoryName"] = bo.memoryName;
+ j["coreSegmentName"] = bo.coreSegmentName;
+ j["providerSegmentName"] = bo.providerSegmentName;
+ j["entityName"] = bo.entityName;
+ j["timestamp_usec"] = bo.timestamp.toMicroSeconds();
+ j["timestamp_datetime"] = toDateTimeMilliSeconds(bo.timestamp);
+ j["instanceIndex"] = bo.instanceIndex;
+}
+
+void armarx::armem::from_json(const nlohmann::json& j, MemoryID& bo)
+{
+ j.at("memoryName").get_to(bo.memoryName);
+ j.at("coreSegmentName").get_to(bo.coreSegmentName);
+ j.at("providerSegmentName").get_to(bo.providerSegmentName);
+ j.at("entityName").get_to(bo.entityName);
+ bo.timestamp = Time::microSeconds(j.at("timestamp_usec").get<int64_t>());
+ // j.at("timestamp_datetime").get_to(toDateTimeMilliSeconds(bo.timestamp));
+ j.at("instanceIndex").get_to(bo.instanceIndex);
+}
diff --git a/source/RobotAPI/libraries/armem/core/json_conversions.h b/source/RobotAPI/libraries/armem/core/json_conversions.h
new file mode 100644
index 0000000000000000000000000000000000000000..86e508c228fe57063216c2f1c6fa7710066992bc
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/json_conversions.h
@@ -0,0 +1,12 @@
+#pragma once
+
+#include <SimoxUtility/json/json.hpp>
+
+
+namespace armarx::armem
+{
+ class MemoryID;
+
+ void to_json(nlohmann::json& j, const MemoryID& bo);
+ void from_json(const nlohmann::json& j, MemoryID& bo);
+}
diff --git a/source/RobotAPI/libraries/armem/core/longtermmemory/CoreSegment.cpp b/source/RobotAPI/libraries/armem/core/longtermmemory/CoreSegment.cpp
index 78f275fe1a8b1285b852431acaa67dc13963874d..a64a5982e143e2627718f5039d3559b2d3479348 100644
--- a/source/RobotAPI/libraries/armem/core/longtermmemory/CoreSegment.cpp
+++ b/source/RobotAPI/libraries/armem/core/longtermmemory/CoreSegment.cpp
@@ -10,7 +10,9 @@ namespace armarx::armem::ltm
wm::CoreSegment CoreSegment::convert() const
{
- wm::CoreSegment m;
+ ARMARX_INFO << "CoreSegment: Converting with connection to: " << dbsettings.toString();
+
+ wm::CoreSegment m(id());
for (const auto& [_, s] : _container)
{
m.addProviderSegment(s.convert());
@@ -18,8 +20,50 @@ namespace armarx::armem::ltm
return m;
}
+ void CoreSegment::reload()
+ {
+ _container.clear();
+
+ ARMARX_INFO << "CoreSegment: (Re)Establishing connection to: " << dbsettings.toString();
+
+ mongocxx::client& client = MongoDBConnectionManager::EstablishConnection(dbsettings);
+ mongocxx::database db = client[dbsettings.database];
+ mongocxx::collection coll = db[id().str()];
+
+
+ mongocxx::cursor cursor = coll.find({});
+ for (auto doc : cursor)
+ {
+ nlohmann::json json = nlohmann::json::parse(bsoncxx::to_json(doc));
+ ARMARX_INFO << "CoreSegment: Found foreign key: " << json.at("foreign_key");
+
+ MemoryID i((std::string) json.at("foreign_key"));
+ if (i.coreSegmentName != id().coreSegmentName)
+ {
+ throw error::InvalidMemoryID(i, "A MemoryID in mongodb was invalid. Found the wrong coreSegment name. Expected " + id().coreSegmentName);
+ }
+
+ std::string k = i.providerSegmentName;
+
+ if (const auto& it = _container.find(k); it != _container.end())
+ {
+ throw error::ArMemError("Somehow after clearing the container a key k = " + k + " was found. Do you have double entries in mongodb?");
+ }
+ else
+ {
+ auto wms = _container.emplace(k, id().withProviderSegmentName(k));
+ wms.first->second.dbsettings = dbsettings;
+ wms.first->second.reload();
+ }
+ }
+ }
+
void CoreSegment::append(const wm::CoreSegment& m)
{
+ mongocxx::client& client = MongoDBConnectionManager::EstablishConnection(dbsettings);
+ mongocxx::database db = client[dbsettings.database];
+ mongocxx::collection coll = db[id().str()];
+
for (const auto& [k, s] : m.container())
{
if (const auto& it = _container.find(k); it != _container.end())
@@ -28,7 +72,14 @@ namespace armarx::armem::ltm
}
else
{
- auto wms = _container.emplace(std::make_pair(k, id().withCoreSegmentName(k)));
+ auto builder = bsoncxx::builder::stream::document{};
+ bsoncxx::document::value foreign_key = builder
+ << "foreign_key" << s.id().withProviderSegmentName(k).str()
+ << bsoncxx::builder::stream::finalize;
+ coll.insert_one(foreign_key.view());
+
+ auto wms = _container.emplace(k, id().withProviderSegmentName(k));
+ wms.first->second.dbsettings = dbsettings;
wms.first->second.append(s);
}
}
diff --git a/source/RobotAPI/libraries/armem/core/longtermmemory/CoreSegment.h b/source/RobotAPI/libraries/armem/core/longtermmemory/CoreSegment.h
index 67f9da6f465820534a4214cbb423e14916819daf..3a7225058a265949de50e98a63d1d3c6ce5bf371 100644
--- a/source/RobotAPI/libraries/armem/core/longtermmemory/CoreSegment.h
+++ b/source/RobotAPI/libraries/armem/core/longtermmemory/CoreSegment.h
@@ -28,7 +28,24 @@ namespace armarx::armem::ltm
wm::CoreSegment convert() const;
// MongoDB connection
+ void reload();
void append(const wm::CoreSegment&);
+
+ protected:
+ virtual void _copySelf(CoreSegment& other) const override
+ {
+ Base::_copySelf(other);
+ other.dbsettings = dbsettings;
+ }
+
+ virtual void _copySelfEmpty(CoreSegment& other) const override
+ {
+ Base::_copySelfEmpty(other);
+ other.dbsettings = dbsettings;
+ }
+
+ public:
+ MongoDBConnectionManager::MongoDBSettings dbsettings;
};
}
diff --git a/source/RobotAPI/libraries/armem/core/longtermmemory/Entity.cpp b/source/RobotAPI/libraries/armem/core/longtermmemory/Entity.cpp
index 15e232c01fb0edb2ce6e0bffec0f2f0b2c0a3110..3865e6df0dfb2112d14d5ee23c40e3a740626307 100644
--- a/source/RobotAPI/libraries/armem/core/longtermmemory/Entity.cpp
+++ b/source/RobotAPI/libraries/armem/core/longtermmemory/Entity.cpp
@@ -5,7 +5,9 @@ namespace armarx::armem::ltm
wm::Entity Entity::convert() const
{
- wm::Entity m;
+ ARMARX_INFO << "Entity: Converting with connection to: " << dbsettings.toString();
+
+ wm::Entity m(id());
for (const auto& [_, s] : _container)
{
m.addSnapshot(s.convert());
@@ -13,19 +15,137 @@ namespace armarx::armem::ltm
return m;
}
+ void Entity::reload()
+ {
+ _container.clear();
+
+ ARMARX_INFO << "Entity: (Re)Establishing connection to: " << dbsettings.toString();
+
+ mongocxx::client& client = MongoDBConnectionManager::EstablishConnection(dbsettings);
+ mongocxx::database db = client[dbsettings.database];
+ mongocxx::collection coll = db[id().str()];
+
+ mongocxx::cursor cursor = coll.find({});
+ int i = 0;
+ for (auto doc : cursor)
+ {
+ if (i > MAX_HISTORY_SIZE)
+ {
+ // TODO: Add logic for most queried data?
+ break;
+ }
+ nlohmann::json json = nlohmann::json::parse(bsoncxx::to_json(doc));
+
+ auto k = armem::Time::microSeconds(json.at("timestamp"));
+ //ARMARX_INFO << "Entity: Found timestamp: " << std::to_string(k.toMicroSeconds());
+
+ if (const auto& it = _container.find(k); it != _container.end())
+ {
+ throw error::ArMemError("Somehow after clearing the container a key k = " + std::to_string(k.toMicroSeconds()) + " was found. Do you have double entries in mongodb?");
+ }
+ else
+ {
+ auto wms = _container.emplace(std::make_pair(k, id().withTimestamp(k)));
+ wms.first->second.dbsettings = dbsettings;
+ wms.first->second.reload();
+ }
+ ++i;
+ }
+ }
+
void Entity::append(const wm::Entity& m)
{
+ mongocxx::client& client = MongoDBConnectionManager::EstablishConnection(dbsettings);
+ mongocxx::database db = client[dbsettings.database];
+ mongocxx::collection coll = db[id().str()];
+
for (const auto& [k, s] : m.container())
{
if (const auto& it = _container.find(k); it != _container.end())
{
- it->second.setTo(s);
+ // segment already exists
+ // We assume that a snapshot does not change, so ignore
}
else
{
auto wms = _container.emplace(std::make_pair(k, id().withTimestamp(k)));
- wms.first->second.setTo(s);
+ wms.first->second.dbsettings = dbsettings;
+ wms.first->second.setTo(s, k);
+ //truncateHistoryToSize();
}
}
}
+
+ bool Entity::hasSnapshot(const Time& time) const
+ {
+ // check cache
+ if (Base::hasSnapshot(time))
+ {
+ return true;
+ }
+ // check mongodb
+ mongocxx::client& client = MongoDBConnectionManager::EstablishConnection(dbsettings);
+ mongocxx::database db = client[dbsettings.database];
+ mongocxx::collection coll = db[id().str()];
+
+ bsoncxx::stdx::optional<bsoncxx::document::value> maybe_result = coll.find_one(document{} << "timestamp" << time.toMicroSeconds() << finalize);
+ if (!maybe_result)
+ {
+ return false;
+ }
+
+ auto json = nlohmann::json::parse(bsoncxx::to_json(*maybe_result));
+ auto id = MemoryID::fromString(json["id"].get<std::string>());
+ auto instances = json["instances"];
+ EntitySnapshot snapshot(id);
+ snapshot.dbsettings = dbsettings;
+
+ for (unsigned int i = 0; i < instances.size(); ++i)
+ {
+ EntityInstance instance(id.withInstanceIndex(i));
+ snapshot.addInstance(instance);
+ }
+
+ _container.insert({time, snapshot});
+ //truncateHistoryToSize();
+ return true;
+ }
+
+ std::vector<Time> Entity::getTimestamps() const
+ {
+ // get from cache
+ std::vector<Time> ret; // = Base::getTimestamps();
+
+ // get missing from mongodb
+ mongocxx::client& client = MongoDBConnectionManager::EstablishConnection(dbsettings);
+ mongocxx::database db = client[dbsettings.database];
+ mongocxx::collection coll = db[id().str()];
+
+ auto cursor = coll.find({});
+ for (auto doc : cursor)
+ {
+ auto json = nlohmann::json::parse(bsoncxx::to_json(doc));
+ auto ts = json["timestamp"].get<long>();
+ ret.push_back(Time::microSeconds(ts));
+ }
+ return ret;
+ }
+
+ const EntitySnapshot& Entity::getSnapshot(const Time& time) const
+ {
+ if (!hasSnapshot(time))
+ {
+ throw error::MissingEntry::create<EntitySnapshotT>(toDateTimeMilliSeconds(time), *this);
+ }
+
+ // the above command already puts the reference to the cache
+ return Base::getSnapshot(time);
+ }
+
+ const std::map<Time, EntitySnapshot>::value_type& Entity::getLatestItem() const
+ {
+ // Directly query mongodb (cache cant know whether it is the latest or not)
+ // TODO
+ return Base::getLatestItem();
+ }
}
diff --git a/source/RobotAPI/libraries/armem/core/longtermmemory/Entity.h b/source/RobotAPI/libraries/armem/core/longtermmemory/Entity.h
index ecdf297c13fae4d24b070f2af13fd6c84ed93392..a5bca08c7ee0b7aff2a79d2c45aa37fc4baec1c4 100644
--- a/source/RobotAPI/libraries/armem/core/longtermmemory/Entity.h
+++ b/source/RobotAPI/libraries/armem/core/longtermmemory/Entity.h
@@ -38,12 +38,46 @@ namespace armarx::armem::ltm
using Base::EntityBase;
using Base::operator=;
+ Entity()
+ {
+ // the history of snapshots is just a cache of frequently used elements
+ setMaxHistorySize(MAX_HISTORY_SIZE);
+ }
// Conversion
wm::Entity convert() const;
// MongoDB connection
+ void reload();
void append(const wm::Entity&);
+
+ // virtual overrides for LTM lookups
+ virtual bool hasSnapshot(const Time& time) const override;
+ virtual std::vector<Time> getTimestamps() const override;
+ virtual const EntitySnapshot& getSnapshot(const Time& time) const override;
+
+ protected:
+ virtual void _copySelf(Entity& other) const override
+ {
+ Base::_copySelf(other);
+ other.dbsettings = dbsettings;
+ }
+
+ virtual void _copySelfEmpty(Entity& other) const override
+ {
+ Base::_copySelfEmpty(other);
+ other.dbsettings = dbsettings;
+ }
+
+ protected:
+ // virtual overrides for LTM storage
+ virtual const std::map<Time, EntitySnapshot>::value_type& getLatestItem() const override;
+
+ public:
+ MongoDBConnectionManager::MongoDBSettings dbsettings;
+
+ private:
+ const static constexpr int MAX_HISTORY_SIZE = 200;
};
}
diff --git a/source/RobotAPI/libraries/armem/core/longtermmemory/EntityInstance.cpp b/source/RobotAPI/libraries/armem/core/longtermmemory/EntityInstance.cpp
index 6332be7ea52f3c547101ae3ee196a524a36aab17..5e44714904efe74f920b891b665e13497b749508 100644
--- a/source/RobotAPI/libraries/armem/core/longtermmemory/EntityInstance.cpp
+++ b/source/RobotAPI/libraries/armem/core/longtermmemory/EntityInstance.cpp
@@ -1,5 +1,7 @@
#include "EntityInstance.h"
+#include "../../core/error.h"
+
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
namespace armarx::armem::ltm
@@ -42,15 +44,4 @@ namespace armarx::armem::ltm
Base::_copySelf(other);
other._metadata = _metadata;
}
-
- wm::EntityInstance EntityInstance::convert() const
- {
- wm::EntityInstance m;
- return m;
- }
-
- void EntityInstance::setTo(const wm::EntityInstance& m)
- {
- ARMARX_IMPORTANT << "Longtermmemory received an entity instance: " << m.id().str();
- }
}
diff --git a/source/RobotAPI/libraries/armem/core/longtermmemory/EntityInstance.h b/source/RobotAPI/libraries/armem/core/longtermmemory/EntityInstance.h
index f78668e5e1a4e1506c2b63c8d2721126e293d6c9..e95ae277e83b2a927172ad1e663c42a700fef849 100644
--- a/source/RobotAPI/libraries/armem/core/longtermmemory/EntityInstance.h
+++ b/source/RobotAPI/libraries/armem/core/longtermmemory/EntityInstance.h
@@ -3,6 +3,10 @@
#include "../base/EntityInstanceBase.h"
#include "../workingmemory/EntityInstance.h"
+#include "mongodb/MongoDBConnectionManager.h"
+
+#include "../workingmemory/entityInstance_conversions.h"
+#include "../workingmemory/json_conversions.h"
namespace armarx::armem::ltm
{
@@ -69,12 +73,6 @@ namespace armarx::armem::ltm
virtual EntityInstance copy() const override;
- // Conversion
- wm::EntityInstance convert() const;
-
- // MongoDB connection
- void setTo(const wm::EntityInstance&);
-
protected:
virtual void _copySelf(EntityInstance& other) const override;
diff --git a/source/RobotAPI/libraries/armem/core/longtermmemory/EntitySnapshot.cpp b/source/RobotAPI/libraries/armem/core/longtermmemory/EntitySnapshot.cpp
index 73d4932155562ffe213504ce3b7bccfc93690b51..47a680731f9794c99433a114dd94a9ad22f5cbea 100644
--- a/source/RobotAPI/libraries/armem/core/longtermmemory/EntitySnapshot.cpp
+++ b/source/RobotAPI/libraries/armem/core/longtermmemory/EntitySnapshot.cpp
@@ -2,32 +2,110 @@
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include "../workingmemory/entityInstance_conversions.h"
+#include "../workingmemory/json_conversions.h"
+
#include "error.h"
namespace armarx::armem::ltm
{
- wm::EntitySnapshot EntitySnapshot::convert() const
+ wm::EntitySnapshot EntitySnapshot::convert(const aron::typenavigator::NavigatorPtr& expectedStructure) const
{
- wm::EntitySnapshot m;
- for (const auto& s : _container)
+ ARMARX_INFO << "EntitySnapshot: Converting with connection to: " << dbsettings.toString();
+
+ mongocxx::client& client = MongoDBConnectionManager::EstablishConnection(dbsettings);
+ mongocxx::database db = client[dbsettings.database];
+ mongocxx::collection coll = db[id().getEntityID().str()];
+
+ auto res = coll.find_one(document{} << "id" << id().getEntitySnapshotID().str() << finalize);
+
+ if (!res)
+ {
+ throw error::ArMemError("Could not load an instance from the memory '" + id().getEntityID().str() + "'. Tried to access: " + id().getEntitySnapshotID().str());
+ }
+
+ nlohmann::json json = nlohmann::json::parse(bsoncxx::to_json(*res));
+ nlohmann::json instances = json["instances"];
+
+ if (instances.size() != _container.size())
{
- m.addInstance(s.convert());
+ throw error::ArMemError("The size of the mongodb entity entry at id " + id().getEntitySnapshotID().str() + " has wrong size. Expected: " + std::to_string(_container.size()) + " but got: " + std::to_string(instances.size()));
+ }
+
+ wm::EntitySnapshot m(id());
+ for (unsigned int i = 0; i < _container.size(); ++i)
+ {
+ nlohmann::json doc = instances[i++];
+
+ auto aron = std::make_shared<aron::datanavigator::DictNavigator>();
+ to_aron(aron, doc, expectedStructure);
+ wm::EntityInstance e(id());
+ from_aron(aron, e);
+ m.addInstance(e);
}
return m;
}
- void EntitySnapshot::setTo(const wm::EntitySnapshot& m)
+ void EntitySnapshot::reload()
+ {
+ _container.clear();
+
+ ARMARX_INFO << "EntitySnapshot: (Re)Establishing connection to: " << dbsettings.toString();
+
+ mongocxx::client& client = MongoDBConnectionManager::EstablishConnection(dbsettings);
+ mongocxx::database db = client[dbsettings.database];
+ mongocxx::collection coll = db[id().getEntityID().str()];
+
+ auto res = coll.find_one(document{} << "id" << id().getEntitySnapshotID().str() << finalize);
+
+ if (!res)
+ {
+ throw error::ArMemError("Could not load an instance from the memory '" + id().getEntityID().str() + "'. Tried to access: " + id().getEntitySnapshotID().str());
+ }
+
+ nlohmann::json json = nlohmann::json::parse(bsoncxx::to_json(*res));
+ for (unsigned int i = 0; i < json.at("instances").size(); ++i)
+ {
+ auto wms = _container.emplace_back(id().withInstanceIndex(i));
+ }
+
+ ARMARX_INFO << "Entity '" + id().str() + "': Found instances in LTM: " << json.at("instances").size();
+ }
+
+ void EntitySnapshot::setTo(const wm::EntitySnapshot& m, const armem::Time& t)
{
// We remove the contente here and reset it with new values
_container.clear();
+ mongocxx::client& client = MongoDBConnectionManager::EstablishConnection(dbsettings);
+ mongocxx::database db = client[dbsettings.database];
+ mongocxx::collection coll = db[id().getEntityID().str()];
+
+ bsoncxx::builder::stream::document builder{};
+ auto in_array = builder
+ << "id" << id().getEntitySnapshotID().str()
+ << "timestamp" << t.toMicroSeconds()
+ << "instances";
+ auto array_builder = bsoncxx::builder::basic::array{};
+
int i = 0;
for (const auto& s : m.container())
{
auto wms = _container.emplace_back(id().withInstanceIndex(i++));
- wms.setTo(s);
+
+ auto aron = std::make_shared<aron::datanavigator::DictNavigator>();
+ to_aron(aron, s);
+ nlohmann::json j;
+ from_aron(aron, j);
+
+ auto doc_value = bsoncxx::from_json(j.dump(2));
+ array_builder.append(doc_value);
}
+
+ auto after_array = in_array << array_builder;
+ bsoncxx::document::value doc = after_array << bsoncxx::builder::stream::finalize;
+ coll.insert_one(doc.view());
}
}
diff --git a/source/RobotAPI/libraries/armem/core/longtermmemory/EntitySnapshot.h b/source/RobotAPI/libraries/armem/core/longtermmemory/EntitySnapshot.h
index 79c0341643c4619270e6b264454ce50dffd0a202..02361d2e708365303fbce81f56eaa9b49fb07a61 100644
--- a/source/RobotAPI/libraries/armem/core/longtermmemory/EntitySnapshot.h
+++ b/source/RobotAPI/libraries/armem/core/longtermmemory/EntitySnapshot.h
@@ -25,9 +25,26 @@ namespace armarx::armem::ltm
// Conversion
- wm::EntitySnapshot convert() const;
+ wm::EntitySnapshot convert(const aron::typenavigator::NavigatorPtr& = nullptr) const;
// MongoDB connection
- void setTo(const wm::EntitySnapshot&);
+ void reload();
+ void setTo(const wm::EntitySnapshot&, const armem::Time& t);
+
+ protected:
+ virtual void _copySelf(EntitySnapshot& other) const override
+ {
+ Base::_copySelf(other);
+ other.dbsettings = dbsettings;
+ }
+
+ virtual void _copySelfEmpty(EntitySnapshot& other) const override
+ {
+ Base::_copySelfEmpty(other);
+ other.dbsettings = dbsettings;
+ }
+
+ public:
+ MongoDBConnectionManager::MongoDBSettings dbsettings;
};
}
diff --git a/source/RobotAPI/libraries/armem/core/longtermmemory/Memory.cpp b/source/RobotAPI/libraries/armem/core/longtermmemory/Memory.cpp
index ac204816011ee0b760fa65a9fecea330edcadf97..fbea444e9d44008407703fe7b7f8b007179726b6 100644
--- a/source/RobotAPI/libraries/armem/core/longtermmemory/Memory.cpp
+++ b/source/RobotAPI/libraries/armem/core/longtermmemory/Memory.cpp
@@ -1,5 +1,6 @@
#include "Memory.h"
+#include <ArmarXCore/core/time/TimeUtil.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
@@ -9,35 +10,121 @@
namespace armarx::armem::ltm
{
+ bool Memory::checkConnection() const
+ {
+ // Check connection:
+ if (!MongoDBConnectionManager::ConnectionIsValid(dbsettings))
+ {
+ ARMARX_WARNING << deactivateSpam("ConnectionIsNotValid")
+ << "The connection to mongocxx for memory '" << name() << "' is not valid. Settings are: " << dbsettings.toString()
+ << "\nTo start it, run e.g.: \n"
+ << "mongod --port " << dbsettings.port << " --dbpath \"/tmp/\""
+ << "\n\n";
+ return false;
+ }
+ ARMARX_INFO << "Checking connection";
+ return true;
+ }
+
wm::Memory Memory::convert() const
{
- wm::Memory m;
+ if (!checkConnection())
+ {
+ wm::Memory m(id());
+ return m;
+ }
+
+ ARMARX_INFO << "Converting with connection to: " << dbsettings.toString();
+
+ TIMING_START(LTM_Convert);
+
+ wm::Memory m(id());
for (const auto& [_, s] : _container)
{
m.addCoreSegment(s.convert());
}
+
+ TIMING_END(LTM_Convert);
return m;
}
- void Memory::reload(const MongoDBConnectionManager::MongoDBSettings& settings)
+ void Memory::reload()
{
- dbsettings = settings;
- std::cout << "Setting connection to: " << settings.uniqueString() << std::endl;
+ if (!checkConnection())
+ {
+ return;
+ }
+
+ ARMARX_INFO << "(Re)Establishing connection to: " << dbsettings.toString();
+ _container.clear();
+
+ mongocxx::client& client = MongoDBConnectionManager::EstablishConnection(dbsettings);
+ mongocxx::database db = client[dbsettings.database];
+ mongocxx::collection coll = db[id().str()];
+
+ mongocxx::cursor cursor = coll.find({});
+ for (auto doc : cursor)
+ {
+ nlohmann::json json = nlohmann::json::parse(bsoncxx::to_json(doc));
+ ARMARX_INFO << "Memory: Found foreign key: " << json.at("foreign_key");
+
+ MemoryID i((std::string) json.at("foreign_key"));
+ if (i.memoryName != id().memoryName)
+ {
+ throw error::InvalidMemoryID(i, "A MemoryID in mongodb was invalid. Found the wrong memory name. Expected " + id().memoryName);
+ }
+
+ std::string k = i.coreSegmentName;
+
+ if (const auto& it = _container.find(k); it != _container.end())
+ {
+ throw error::ArMemError("Somehow after clearing the container a key k = " + k + " was found. Do you have double entries in mongodb?");
+ }
+ else
+ {
+ auto wms = _container.emplace(k, id().withCoreSegmentName(k));
+ wms.first->second.dbsettings = dbsettings;
+ wms.first->second.reload();
+ }
+ }
}
void Memory::append(const wm::Memory& m)
{
+ if (!checkConnection())
+ {
+ return;
+ }
+
+ ARMARX_INFO << "Merge memory with name '" << m.name() << "' into the LTM with name '" << name() << "'";
+
+ TIMING_START(LTM_Append);
+
+ mongocxx::client& client = MongoDBConnectionManager::EstablishConnection(dbsettings);
+ mongocxx::database db = client[dbsettings.database];
+ mongocxx::collection coll = db[id().str()];
+
for (const auto& [k, s] : m.container())
{
if (const auto& it = _container.find(k); it != _container.end())
{
+ // TODO check if foreign key exists
it->second.append(s);
}
else
{
- auto wms = _container.emplace(std::make_pair(k, id().withCoreSegmentName(k)));
+ auto builder = bsoncxx::builder::stream::document{};
+ bsoncxx::document::value foreign_key = builder
+ << "foreign_key" << s.id().withCoreSegmentName(k).str()
+ << bsoncxx::builder::stream::finalize;
+ coll.insert_one(foreign_key.view());
+
+ auto wms = _container.emplace(k, id().withCoreSegmentName(k));
+ wms.first->second.dbsettings = dbsettings;
wms.first->second.append(s);
}
}
+
+ TIMING_END(LTM_Append);
}
}
diff --git a/source/RobotAPI/libraries/armem/core/longtermmemory/Memory.h b/source/RobotAPI/libraries/armem/core/longtermmemory/Memory.h
index a381972a8c6670f10f286b5ecee82757c1be87d1..86c00cc618e23620540428ba3b024743ecc272ee 100644
--- a/source/RobotAPI/libraries/armem/core/longtermmemory/Memory.h
+++ b/source/RobotAPI/libraries/armem/core/longtermmemory/Memory.h
@@ -5,7 +5,6 @@
#include "CoreSegment.h"
#include "../workingmemory/Memory.h"
-#include "mongodb/MongoDBConnectionManager.h"
namespace armarx::armem::ltm
{
@@ -22,16 +21,57 @@ namespace armarx::armem::ltm
using Base::MemoryBase;
using Base::operator=;
+ enum class PeriodicTransferMode
+ {
+ MANUAL,
+ TRANSFER_PERIODIC_KEEP,
+ TRANSFER_PERIODIC_DELETE,
+ };
+
+ enum class FilledTransferMode
+ {
+ TRANSFER_LATEST,
+ TRANSFER_LEAST_USED
+ };
+
+ struct PeriodicTransfer
+ {
+ PeriodicTransferMode mode = PeriodicTransferMode::MANUAL;
+ };
+
+ struct FilledTransfer
+ {
+ FilledTransferMode mode = FilledTransferMode::TRANSFER_LATEST;
+ };
+
// Conversion
wm::Memory convert() const;
// MongoDB connection
- void reload(const MongoDBConnectionManager::MongoDBSettings&);
+ void reload();
void append(const wm::Memory&);
+ protected:
+ virtual void _copySelf(Memory& other) const override
+ {
+ Base::_copySelf(other);
+ other.dbsettings = dbsettings;
+ }
+
+ virtual void _copySelfEmpty(Memory& other) const override
+ {
+ Base::_copySelfEmpty(other);
+ other.dbsettings = dbsettings;
+ }
+
+ private:
+ bool checkConnection() const;
public:
MongoDBConnectionManager::MongoDBSettings dbsettings;
+
+ PeriodicTransfer periodic_transfer;
+ FilledTransferMode filled_transfer;
};
}
diff --git a/source/RobotAPI/libraries/armem/core/longtermmemory/ProviderSegment.cpp b/source/RobotAPI/libraries/armem/core/longtermmemory/ProviderSegment.cpp
index 38f2a142989324cab5aa9a47f2f80d7e91fa9b24..aac80459904cae59550ecdf33f7057f92ed03aa4 100644
--- a/source/RobotAPI/libraries/armem/core/longtermmemory/ProviderSegment.cpp
+++ b/source/RobotAPI/libraries/armem/core/longtermmemory/ProviderSegment.cpp
@@ -10,7 +10,9 @@ namespace armarx::armem::ltm
wm::ProviderSegment ProviderSegment::convert() const
{
- wm::ProviderSegment m;
+ ARMARX_INFO << "ProviderSegment: Converting with connection to: " << dbsettings.toString();
+
+ wm::ProviderSegment m(id());
for (const auto& [_, s] : _container)
{
m.addEntity(s.convert());
@@ -18,8 +20,49 @@ namespace armarx::armem::ltm
return m;
}
+ void ProviderSegment::reload()
+ {
+ _container.clear();
+
+ ARMARX_INFO << "ProviderSegment: (Re)Establishing connection to: " << dbsettings.toString();
+
+ mongocxx::client& client = MongoDBConnectionManager::EstablishConnection(dbsettings);
+ mongocxx::database db = client[dbsettings.database];
+ mongocxx::collection coll = db[id().str()];
+
+ mongocxx::cursor cursor = coll.find({});
+ for (auto doc : cursor)
+ {
+ nlohmann::json json = nlohmann::json::parse(bsoncxx::to_json(doc));
+ ARMARX_INFO << "ProviderSegment: Found foreign key: " << json.at("foreign_key");
+
+ MemoryID i((std::string) json.at("foreign_key"));
+ if (i.providerSegmentName != id().providerSegmentName)
+ {
+ throw error::InvalidMemoryID(i, "A MemoryID in mongodb was invalid. Found the wrong providerSegment name. Expected " + id().providerSegmentName);
+ }
+
+ std::string k = i.entityName;
+
+ if (const auto& it = _container.find(k); it != _container.end())
+ {
+ throw error::ArMemError("Somehow after clearing the container a key k = " + k + " was found. Do you have double entries in mongodb?");
+ }
+ else
+ {
+ auto wms = _container.emplace(k, id().withEntityName(k));
+ wms.first->second.dbsettings = dbsettings;
+ wms.first->second.reload();
+ }
+ }
+ }
+
void ProviderSegment::append(const wm::ProviderSegment& m)
{
+ mongocxx::client& client = MongoDBConnectionManager::EstablishConnection(dbsettings);
+ mongocxx::database db = client[dbsettings.database];
+ mongocxx::collection coll = db[id().str()];
+
for (const auto& [k, s] : m.container())
{
if (const auto& it = _container.find(k); it != _container.end())
@@ -28,7 +71,14 @@ namespace armarx::armem::ltm
}
else
{
- auto wms = _container.emplace(std::make_pair(k, id().withEntityName(k)));
+ auto builder = bsoncxx::builder::stream::document{};
+ bsoncxx::document::value foreign_key = builder
+ << "foreign_key" << s.id().withEntityName(k).str()
+ << bsoncxx::builder::stream::finalize;
+ coll.insert_one(foreign_key.view());
+
+ auto wms = _container.emplace(k, id().withEntityName(k));
+ wms.first->second.dbsettings = dbsettings;
wms.first->second.append(s);
}
}
diff --git a/source/RobotAPI/libraries/armem/core/longtermmemory/ProviderSegment.h b/source/RobotAPI/libraries/armem/core/longtermmemory/ProviderSegment.h
index 4e81db8a9270f58d44845d08f298b37d59424323..bb49f232e9fe231e1cd500e3f9739ddaa0d7d83f 100644
--- a/source/RobotAPI/libraries/armem/core/longtermmemory/ProviderSegment.h
+++ b/source/RobotAPI/libraries/armem/core/longtermmemory/ProviderSegment.h
@@ -28,7 +28,24 @@ namespace armarx::armem::ltm
wm::ProviderSegment convert() const;
// MongoDB connection
+ void reload();
void append(const wm::ProviderSegment&);
+
+ protected:
+ virtual void _copySelf(ProviderSegment& other) const override
+ {
+ Base::_copySelf(other);
+ other.dbsettings = dbsettings;
+ }
+
+ virtual void _copySelfEmpty(ProviderSegment& other) const override
+ {
+ Base::_copySelfEmpty(other);
+ other.dbsettings = dbsettings;
+ }
+
+ public:
+ MongoDBConnectionManager::MongoDBSettings dbsettings;
};
}
diff --git a/source/RobotAPI/libraries/armem/core/longtermmemory/mongodb/MongoDBConnectionManager.cpp b/source/RobotAPI/libraries/armem/core/longtermmemory/mongodb/MongoDBConnectionManager.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..6ad44836900e04765790a9a3c2a19881e1288656
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/longtermmemory/mongodb/MongoDBConnectionManager.cpp
@@ -0,0 +1,76 @@
+#include "MongoDBConnectionManager.h"
+
+namespace armarx::armem::ltm
+{
+ bool MongoDBConnectionManager::initialized = false;
+ std::map<std::string, mongocxx::client> MongoDBConnectionManager::Connections = {};
+
+
+ void MongoDBConnectionManager::initialize_if()
+ {
+ if (!initialized)
+ {
+ initialized = true;
+ mongocxx::instance instance{}; // This should be done only once.
+ }
+ }
+
+ mongocxx::client& MongoDBConnectionManager::EstablishConnection(const MongoDBSettings& settings)
+ {
+ initialize_if();
+
+ const auto uri_str = settings.uri();
+ const auto& it = Connections.find(uri_str);
+ if (it == Connections.end())
+ {
+ mongocxx::uri uri(uri_str);
+ auto con = Connections.emplace(uri_str, mongocxx::client(uri));
+ return con.first->second;
+ }
+ else
+ {
+ // A connection already exists. We do not need to open another one.
+ return it->second;
+ }
+ }
+
+ bool MongoDBConnectionManager::ConnectionIsValid(const MongoDBSettings& settings, bool force)
+ {
+ initialize_if();
+
+ try
+ {
+ if (!force)
+ {
+ const auto uri_str = settings.uri();
+ const auto& it = Connections.find(uri_str);
+ if (it != Connections.end())
+ {
+ auto admin = it->second["admin"];
+ auto result = admin.run_command(bsoncxx::builder::basic::make_document(bsoncxx::builder::basic::kvp("isMaster", 1)));
+ return true;
+ }
+ }
+
+ const auto uri_str = settings.uri();
+ mongocxx::uri uri(uri_str);
+ auto client = mongocxx::client(uri);
+ auto admin = client["admin"];
+ auto result = admin.run_command(bsoncxx::builder::basic::make_document(bsoncxx::builder::basic::kvp("isMaster", 1)));
+ return true;
+ }
+ catch (const std::exception& xcp)
+ {
+ return false;
+ }
+ }
+
+ bool MongoDBConnectionManager::ConnectionExists(const MongoDBSettings& settings)
+ {
+ initialize_if();
+
+ const auto uri_str = settings.uri();
+ const auto& it = Connections.find(uri_str);
+ return it != Connections.end();
+ }
+}
diff --git a/source/RobotAPI/libraries/armem/core/longtermmemory/mongodb/MongoDBConnectionManager.h b/source/RobotAPI/libraries/armem/core/longtermmemory/mongodb/MongoDBConnectionManager.h
index 02c25965f9c6ae0c12f528c9e2a5c241a10a26d3..0cd3cb5ba8f6c575d6d3dd9357f16884f508914c 100644
--- a/source/RobotAPI/libraries/armem/core/longtermmemory/mongodb/MongoDBConnectionManager.h
+++ b/source/RobotAPI/libraries/armem/core/longtermmemory/mongodb/MongoDBConnectionManager.h
@@ -4,10 +4,29 @@
#include <vector>
#include <map>
#include <memory>
+#include <iostream>
+
+#include <bsoncxx/json.hpp>
+#include <mongocxx/client.hpp>
+#include <mongocxx/stdx.hpp>
+#include <mongocxx/uri.hpp>
+#include <mongocxx/instance.hpp>
+#include <bsoncxx/builder/stream/helpers.hpp>
+#include <bsoncxx/builder/stream/document.hpp>
+#include <bsoncxx/builder/stream/array.hpp>
+
namespace armarx::armem::ltm
{
+ using bsoncxx::builder::stream::close_array;
+ using bsoncxx::builder::stream::close_document;
+ using bsoncxx::builder::stream::document;
+ using bsoncxx::builder::stream::finalize;
+ using bsoncxx::builder::stream::open_array;
+ using bsoncxx::builder::stream::open_document;
+
+
/**
* @brief Data of a memory consisting of multiple core segments.
*/
@@ -17,38 +36,40 @@ namespace armarx::armem::ltm
struct MongoDBSettings
{
std::string host = "localhost";
- std::string user = "root";
+ unsigned int port = 25270;
+ std::string user = "";
std::string password = "";
+ std::string database = "Test";
+
bool isSet() const
{
// we always need a user and a host
- return !host.empty() and !user.empty();
+ return !host.empty() and port != 0 and !user.empty();
}
- std::string uniqueString() const
+ std::string uri() const
{
- return host + "::" + user;
+ return "mongodb://" + host + ":" + std::to_string(port) + user;
}
- };
- static int EstablishConnection(const MongoDBSettings& settings)
- {
- const auto str_rep = settings.uniqueString();
- const auto& it = Connections.find(str_rep);
- if (it == Connections.end())
+ std::string toString() const
{
- auto con = Connections.emplace(str_rep, 0);
- return con.first->second;
+ return uri() + ":" + password + "/" + database;
}
- else
- {
- // A connection already exists. We do not need to open another one.
- return it->second;
- }
- }
+ };
+
+ static mongocxx::client& EstablishConnection(const MongoDBSettings& settings);
+ static bool ConnectionIsValid(const MongoDBSettings& settings, bool force = false);
+ static bool ConnectionExists(const MongoDBSettings& settings);
private:
- static std::map<std::string, int> Connections;
+ static void initialize_if();
+
+
+ private:
+ static bool initialized;
+ static std::map<std::string, mongocxx::client> Connections;
+
};
}
diff --git a/source/RobotAPI/libraries/armem/core/workingmemory/CoreSegment.cpp b/source/RobotAPI/libraries/armem/core/workingmemory/CoreSegment.cpp
index b4341c9da6b9bf6c1c9be6e1141ff712d9e00132..285235b76e397952301c23d8218abb11f1f7aeaf 100644
--- a/source/RobotAPI/libraries/armem/core/workingmemory/CoreSegment.cpp
+++ b/source/RobotAPI/libraries/armem/core/workingmemory/CoreSegment.cpp
@@ -7,6 +7,17 @@
namespace armarx::armem::wm
{
+
+ Commit CoreSegment::toCommit() const
+ {
+ Commit c;
+ for (const auto& [k, s] : _container)
+ {
+ c.append(s.toCommit());
+ }
+ return c;
+ }
+
void CoreSegment::_copySelfWithoutData(CoreSegment& other) const
{
other.id() = _id;
diff --git a/source/RobotAPI/libraries/armem/core/workingmemory/CoreSegment.h b/source/RobotAPI/libraries/armem/core/workingmemory/CoreSegment.h
index 1eaf327338929b1290e476951aa69bdde0d3723d..7847bbb71cc6a1a269556aeec07738c396e42a68 100644
--- a/source/RobotAPI/libraries/armem/core/workingmemory/CoreSegment.h
+++ b/source/RobotAPI/libraries/armem/core/workingmemory/CoreSegment.h
@@ -27,8 +27,11 @@ namespace armarx::armem::wm
CoreSegment& operator=(const CoreSegment& other) = default;
CoreSegment& operator=(CoreSegment&& other) = default;
- virtual ~CoreSegment() override = default;
-
+ /**
+ * @brief Convert the content of this segmnet into a commit
+ * @return The resulting commit
+ */
+ Commit toCommit() const;
protected:
diff --git a/source/RobotAPI/libraries/armem/core/workingmemory/Entity.cpp b/source/RobotAPI/libraries/armem/core/workingmemory/Entity.cpp
index 022864c19d9ea98583d9d3998bec46643921cdc8..e704e96d5454d27bfb095120c1901e000611122a 100644
--- a/source/RobotAPI/libraries/armem/core/workingmemory/Entity.cpp
+++ b/source/RobotAPI/libraries/armem/core/workingmemory/Entity.cpp
@@ -2,6 +2,20 @@
namespace armarx::armem::wm
{
+
+ Commit Entity::toCommit() const
+ {
+ Commit c;
+ for (const auto& [k, s] : _container)
+ {
+ EntityUpdate& up = c.add();
+ up.entityID = id();
+ up.timeCreated = k;
+ up.instancesData = s.getAronData();
+ }
+ return c;
+ }
+
void Entity::_copySelfWithoutData(Entity& other) const
{
other.id() = _id;
diff --git a/source/RobotAPI/libraries/armem/core/workingmemory/Entity.h b/source/RobotAPI/libraries/armem/core/workingmemory/Entity.h
index 6a31286722e0e78d53423f6aa6b517583f83ca09..a6a7621e21272db6447e9e09b695900eefd263c1 100644
--- a/source/RobotAPI/libraries/armem/core/workingmemory/Entity.h
+++ b/source/RobotAPI/libraries/armem/core/workingmemory/Entity.h
@@ -42,8 +42,11 @@ namespace armarx::armem::wm
Entity& operator=(const Entity& other) = default;
Entity& operator=(Entity&& other) = default;
- virtual ~Entity() override = default;
-
+ /**
+ * @brief Convert the content of this segmnet into a commit
+ * @return The resulting commit
+ */
+ Commit toCommit() const;
protected:
diff --git a/source/RobotAPI/libraries/armem/core/workingmemory/EntityInstance.cpp b/source/RobotAPI/libraries/armem/core/workingmemory/EntityInstance.cpp
index 95e508bde1412a87c865b2720bd227ec58595114..6149449c77533abd656a460097969f0066d8d88f 100644
--- a/source/RobotAPI/libraries/armem/core/workingmemory/EntityInstance.cpp
+++ b/source/RobotAPI/libraries/armem/core/workingmemory/EntityInstance.cpp
@@ -10,7 +10,7 @@ std::ostream& armarx::armem::wm::operator<<(std::ostream& os, const EntityInstan
<< "\n- t_sent = \t" << armem::toStringMicroSeconds(d.timeSent) << " us"
<< "\n- t_arrived = \t" << armem::toStringMicroSeconds(d.timeArrived) << " us"
<< "\n- confidence = \t" << d.confidence << " us"
- ;
+ ;
return os;
}
@@ -28,9 +28,17 @@ namespace armarx::armem::wm
bool EntityInstance::equalsDeep(const EntityInstance& other) const
{
- return id() == other.id()
- && _metadata == other.metadata()
- && _data->equalsDeep(other.data());
+ if (_data and other.data())
+ {
+ return id() == other.id()
+ && _metadata == other.metadata()
+ && *_data == *other.data();
+ }
+ if (_data or other.data())
+ {
+ return false;
+ }
+ return id() == other.id() && _metadata == other.metadata();
}
void EntityInstance::update(const EntityUpdate& update, int index)
diff --git a/source/RobotAPI/libraries/armem/core/workingmemory/EntitySnapshot.cpp b/source/RobotAPI/libraries/armem/core/workingmemory/EntitySnapshot.cpp
index fb68d050a0ad19e1e2627e4cb5732a23a2ce4493..76d5400131a7ec6a57478f13afb0dfc38a4da174 100644
--- a/source/RobotAPI/libraries/armem/core/workingmemory/EntitySnapshot.cpp
+++ b/source/RobotAPI/libraries/armem/core/workingmemory/EntitySnapshot.cpp
@@ -7,6 +7,16 @@
namespace armarx::armem::wm
{
+ std::vector<aron::datanavigator::DictNavigatorPtr> EntitySnapshot::getAronData() const
+ {
+ std::vector<aron::datanavigator::DictNavigatorPtr> ret;
+ for (const auto& aron : _container)
+ {
+ ret.push_back(aron.data());
+ }
+ return ret;
+ }
+
void EntitySnapshot::_copySelfWithoutData(EntitySnapshot& other) const
{
other.id() = _id;
diff --git a/source/RobotAPI/libraries/armem/core/workingmemory/EntitySnapshot.h b/source/RobotAPI/libraries/armem/core/workingmemory/EntitySnapshot.h
index ee0c78c5d201ed2e9fb1fe5c333efc728a9dccfc..3eedb48d0b4aaa5191541d8ca1586ffc59ab5998 100644
--- a/source/RobotAPI/libraries/armem/core/workingmemory/EntitySnapshot.h
+++ b/source/RobotAPI/libraries/armem/core/workingmemory/EntitySnapshot.h
@@ -23,6 +23,7 @@ namespace armarx::armem::wm
using Base::EntitySnapshotBase;
using Base::operator=;
+ std::vector<aron::datanavigator::DictNavigatorPtr> getAronData() const;
protected:
diff --git a/source/RobotAPI/libraries/armem/core/workingmemory/Memory.cpp b/source/RobotAPI/libraries/armem/core/workingmemory/Memory.cpp
index b7af1a6456687b1741710d46fa157b9091295db7..3d2efd2249f969308b126608d01533500f3bfa73 100644
--- a/source/RobotAPI/libraries/armem/core/workingmemory/Memory.cpp
+++ b/source/RobotAPI/libraries/armem/core/workingmemory/Memory.cpp
@@ -3,6 +3,16 @@
namespace armarx::armem::wm
{
+ Commit Memory::toCommit() const
+ {
+ Commit c;
+ for (const auto& [k, s] : _container)
+ {
+ c.append(s.toCommit());
+ }
+ return c;
+ }
+
void Memory::_copySelfWithoutData(Memory& other) const
{
other.id() = _id;
diff --git a/source/RobotAPI/libraries/armem/core/workingmemory/Memory.h b/source/RobotAPI/libraries/armem/core/workingmemory/Memory.h
index e8b7cb35349837820c3dd47f2d584ef6b9760461..3c64c0841b46a5c95f7f7365af26a1b20226f4dc 100644
--- a/source/RobotAPI/libraries/armem/core/workingmemory/Memory.h
+++ b/source/RobotAPI/libraries/armem/core/workingmemory/Memory.h
@@ -27,6 +27,11 @@ namespace armarx::armem::wm
Memory& operator=(const Memory& other) = default;
Memory& operator=(Memory&& other) = default;
+ /**
+ * @brief Convert the content of this memory into a commit
+ * @return The resulting commit
+ */
+ Commit toCommit() const;
protected:
diff --git a/source/RobotAPI/libraries/armem/core/workingmemory/ProviderSegment.cpp b/source/RobotAPI/libraries/armem/core/workingmemory/ProviderSegment.cpp
index 517b35bfad666cdac02041694e787523c4c1e894..bd03cf21cd8b616a3afcc4d531377288e55c5f2f 100644
--- a/source/RobotAPI/libraries/armem/core/workingmemory/ProviderSegment.cpp
+++ b/source/RobotAPI/libraries/armem/core/workingmemory/ProviderSegment.cpp
@@ -8,6 +8,16 @@
namespace armarx::armem::wm
{
+ Commit ProviderSegment::toCommit() const
+ {
+ Commit c;
+ for (const auto& [k, s] : _container)
+ {
+ c.append(s.toCommit());
+ }
+ return c;
+ }
+
void ProviderSegment::_copySelfWithoutData(ProviderSegment& other) const
{
other.id() = _id;
diff --git a/source/RobotAPI/libraries/armem/core/workingmemory/ProviderSegment.h b/source/RobotAPI/libraries/armem/core/workingmemory/ProviderSegment.h
index 0b703617d9b1b642153da505882e71aab8aa3723..7d1e9db335ee39433351a8e15113059836bf825d 100644
--- a/source/RobotAPI/libraries/armem/core/workingmemory/ProviderSegment.h
+++ b/source/RobotAPI/libraries/armem/core/workingmemory/ProviderSegment.h
@@ -28,7 +28,11 @@ namespace armarx::armem::wm
ProviderSegment& operator=(const ProviderSegment& other) = default;
ProviderSegment& operator=(ProviderSegment&& other) = default;
- virtual ~ProviderSegment() override = default;
+ /**
+ * @brief Convert the content of this segmnet into a commit
+ * @return The resulting commit
+ */
+ Commit toCommit() const;
protected:
diff --git a/source/RobotAPI/libraries/armem/core/workingmemory/entityInstance_conversions.cpp b/source/RobotAPI/libraries/armem/core/workingmemory/entityInstance_conversions.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..31538f8d8e2f1eeaf932ed16e50b7040520f0a99
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/workingmemory/entityInstance_conversions.cpp
@@ -0,0 +1,64 @@
+#include "json_conversions.h"
+
+namespace armarx::armem
+{
+
+ constexpr const char* DATA_WRAPPER_DATA_FIELD = "__ARON_DATA";
+ constexpr const char* DATA_WRAPPER_TIME_STORED_FIELD = "__WRITER_METADATA__TIME_STORED";
+ constexpr const char* DATA_WRAPPER_TIME_CREATED_FIELD = "__ENTITY_METADATA__TIME_CREATED";
+ constexpr const char* DATA_WRAPPER_TIME_SENT_FIELD = "__ENTITY_METADATA__TIME_SENT";
+ constexpr const char* DATA_WRAPPER_TIME_ARRIVED_FIELD = "__ENTITY_METADATA__TIME_ARRIVED";
+ constexpr const char* DATA_WRAPPER_CONFIDENCE_FIELD = "__ENTITY_METADATA__CONFIDENCE";
+
+ void from_aron(const aron::datanavigator::DictNavigatorPtr& dataWrapped, wm::EntityInstance& e)
+ {
+ wm::EntityInstanceMetadata& metadata = e.metadata();
+
+ if (dataWrapped->hasElement(DATA_WRAPPER_DATA_FIELD))
+ {
+ aron::datanavigator::DictNavigatorPtr data = aron::datanavigator::DictNavigator::DynamicCastAndCheck(dataWrapped->getElement(DATA_WRAPPER_DATA_FIELD));
+ e.setData(data);
+ }
+
+ auto timeCreated = aron::datanavigator::LongNavigator::DynamicCastAndCheck(dataWrapped->getElement(DATA_WRAPPER_TIME_CREATED_FIELD));
+ metadata.timeCreated = Time::microSeconds(timeCreated->toAronLongPtr()->value);
+
+ auto timeSent = aron::datanavigator::LongNavigator::DynamicCastAndCheck(dataWrapped->getElement(DATA_WRAPPER_TIME_SENT_FIELD));
+ metadata.timeSent = Time::microSeconds(timeSent->toAronLongPtr()->value);
+
+ auto timeArrived = aron::datanavigator::LongNavigator::DynamicCastAndCheck(dataWrapped->getElement(DATA_WRAPPER_TIME_ARRIVED_FIELD));
+ metadata.timeArrived = Time::microSeconds(timeArrived->toAronLongPtr()->value);
+
+ auto confidence = aron::datanavigator::DoubleNavigator::DynamicCastAndCheck(dataWrapped->getElement(DATA_WRAPPER_CONFIDENCE_FIELD));
+ metadata.confidence = static_cast<float>(confidence->toAronDoublePtr()->value);
+ }
+
+ void to_aron(aron::datanavigator::DictNavigatorPtr& a, const wm::EntityInstance& e)
+ {
+ if (e.data())
+ {
+ a->addElement(DATA_WRAPPER_DATA_FIELD, e.data());
+ }
+
+ auto timeWrapped = std::make_shared<aron::datanavigator::LongNavigator>();
+ timeWrapped->setValue(Time::now().toMicroSeconds());
+ a->addElement(DATA_WRAPPER_TIME_STORED_FIELD, timeWrapped);
+
+ const wm::EntityInstanceMetadata& metadata = e.metadata();
+ auto timeCreated = std::make_shared<aron::datanavigator::LongNavigator>();
+ timeCreated->setValue(metadata.timeCreated.toMicroSeconds());
+ a->addElement(DATA_WRAPPER_TIME_CREATED_FIELD, timeCreated);
+
+ auto timeSent = std::make_shared<aron::datanavigator::LongNavigator>();
+ timeSent->setValue(metadata.timeSent.toMicroSeconds());
+ a->addElement(DATA_WRAPPER_TIME_SENT_FIELD, timeSent);
+
+ auto timeArrived = std::make_shared<aron::datanavigator::LongNavigator>();
+ timeArrived->setValue(metadata.timeArrived.toMicroSeconds());
+ a->addElement(DATA_WRAPPER_TIME_ARRIVED_FIELD, timeArrived);
+
+ auto confidence = std::make_shared<aron::datanavigator::DoubleNavigator>();
+ confidence->setValue(static_cast<double>(metadata.confidence));
+ a->addElement(DATA_WRAPPER_CONFIDENCE_FIELD, confidence);
+ }
+}
diff --git a/source/RobotAPI/libraries/armem/core/workingmemory/entityInstance_conversions.h b/source/RobotAPI/libraries/armem/core/workingmemory/entityInstance_conversions.h
new file mode 100644
index 0000000000000000000000000000000000000000..f3b25566f110dca844e24dcac4c997f724c74548
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/workingmemory/entityInstance_conversions.h
@@ -0,0 +1,9 @@
+#pragma once
+
+#include "Memory.h"
+
+namespace armarx::armem
+{
+ void from_aron(const aron::datanavigator::DictNavigatorPtr&, wm::EntityInstance&);
+ void to_aron(aron::datanavigator::DictNavigatorPtr&, const wm::EntityInstance&);
+}
diff --git a/source/RobotAPI/libraries/armem/core/workingmemory/ice_conversions.cpp b/source/RobotAPI/libraries/armem/core/workingmemory/ice_conversions.cpp
index 53378a6976889f4169be8ea35d4f8560df9bf96f..bfc049995a6553e53a78be00294cb9c162e80a0d 100644
--- a/source/RobotAPI/libraries/armem/core/workingmemory/ice_conversions.cpp
+++ b/source/RobotAPI/libraries/armem/core/workingmemory/ice_conversions.cpp
@@ -72,7 +72,7 @@ namespace armarx::armem
if (providerSegment.hasAronType())
{
- ice.aronType = providerSegment.aronType()->getResult();
+ ice.aronType = providerSegment.aronType()->toAronPtr();
}
ARMARX_CHECK(!providerSegment.aronType() || ice.aronType);
toIce(ice.entities, providerSegment.entities());
@@ -95,7 +95,7 @@ namespace armarx::armem
if (coreSegment.hasAronType())
{
- ice.aronType = coreSegment.aronType()->getResult();
+ ice.aronType = coreSegment.aronType()->toAronPtr();
}
ARMARX_CHECK(!coreSegment.aronType() || ice.aronType);
toIce(ice.providerSegments, coreSegment.providerSegments());
diff --git a/source/RobotAPI/libraries/armem/core/workingmemory/json_conversions.cpp b/source/RobotAPI/libraries/armem/core/workingmemory/json_conversions.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..e7499d8b42edf895bbbdb2f0d06a1fe3ed2ee823
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/workingmemory/json_conversions.cpp
@@ -0,0 +1,22 @@
+#include "json_conversions.h"
+
+#include <RobotAPI/libraries/aron/core/Debug.h>
+
+namespace armarx::armem
+{
+
+ void from_aron(const aron::datanavigator::DictNavigatorPtr& aron, nlohmann::json& j)
+ {
+ aron::dataIO::writer::NlohmannJSONWriter dataWriter;
+ aron::dataIO::Visitor::VisitAndSetup(dataWriter, aron);
+ j = dataWriter.getResult();
+ }
+
+ void to_aron(aron::datanavigator::DictNavigatorPtr& a, const nlohmann::json& e, const aron::typenavigator::NavigatorPtr& expectedStructure)
+ {
+ aron::dataIO::reader::NlohmannJSONReader dataReader(e);
+ aron::dataIO::writer::NavigatorWriter navWriter;
+ aron::dataIO::Converter::ReadAndConvert(dataReader, navWriter, expectedStructure);
+ a = aron::datanavigator::DictNavigator::DynamicCastAndCheck(navWriter.getResult());
+ }
+}
diff --git a/source/RobotAPI/libraries/armem/core/workingmemory/json_conversions.h b/source/RobotAPI/libraries/armem/core/workingmemory/json_conversions.h
new file mode 100644
index 0000000000000000000000000000000000000000..d132d88f7d7fb694be8b3dc086c87dcafa276e6a
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/workingmemory/json_conversions.h
@@ -0,0 +1,15 @@
+#pragma once
+
+#include "Memory.h"
+
+#include <RobotAPI/libraries/aron/core/io/dataIO/converter/Converter.h>
+#include <RobotAPI/libraries/aron/core/io/dataIO/visitor/Visitor.h>
+#include <RobotAPI/libraries/aron/core/io/dataIO/reader/nlohmannJSON/NlohmannJSONReader.h>
+#include <RobotAPI/libraries/aron/core/io/dataIO/writer/nlohmannJSON/NlohmannJSONWriter.h>
+
+
+namespace armarx::armem
+{
+ void from_aron(const aron::datanavigator::DictNavigatorPtr&, nlohmann::json&);
+ void to_aron(aron::datanavigator::DictNavigatorPtr&, const nlohmann::json&, const aron::typenavigator::NavigatorPtr& expectedStructure = nullptr);
+}
diff --git a/source/RobotAPI/libraries/armem/core/workingmemory/visitor.h b/source/RobotAPI/libraries/armem/core/workingmemory/visitor.h
new file mode 100644
index 0000000000000000000000000000000000000000..815ee1b34908dc5916e7d909e709bc735409f63c
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/workingmemory/visitor.h
@@ -0,0 +1,4 @@
+#pragma once
+
+#include "visitor/FunctionalVisitor.h"
+#include "visitor/Visitor.h"
diff --git a/source/RobotAPI/libraries/armem/core/workingmemory/visitor/FunctionalVisitor.cpp b/source/RobotAPI/libraries/armem/core/workingmemory/visitor/FunctionalVisitor.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..53016fc6c83d9ef6e24a2e75942a12bced7d0eaa
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/workingmemory/visitor/FunctionalVisitor.cpp
@@ -0,0 +1,23 @@
+#include "FunctionalVisitor.h"
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+#include <RobotAPI/libraries/armem/core/workingmemory/Memory.h>
+#include <RobotAPI/libraries/armem/core/error.h>
+
+
+namespace armarx::armem::wm
+{
+
+ FunctionalVisitor::FunctionalVisitor()
+ {
+ }
+
+
+ FunctionalVisitor::~FunctionalVisitor()
+ {
+ }
+
+
+
+}
diff --git a/source/RobotAPI/libraries/armem/core/workingmemory/visitor/FunctionalVisitor.h b/source/RobotAPI/libraries/armem/core/workingmemory/visitor/FunctionalVisitor.h
new file mode 100644
index 0000000000000000000000000000000000000000..1c1b46e138c7eb2d713ba15b76ba77a2588ea0ce
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/workingmemory/visitor/FunctionalVisitor.h
@@ -0,0 +1,101 @@
+#pragma once
+
+#include <functional>
+
+#include "Visitor.h"
+
+
+namespace armarx::armem::wm
+{
+
+ /**
+ * @brief A `Visitor` which can be parametrized by `std::function`
+ * instead of inheriting and overriding.
+ */
+ class FunctionalVisitor : public Visitor
+ {
+ public:
+
+ FunctionalVisitor();
+ virtual ~FunctionalVisitor() override;
+
+
+ bool visitEnter(Memory& memory) override
+ {
+ return memoryFn ? memoryFn(memory) : Visitor::visitEnter(memory);
+ }
+ bool visitEnter(CoreSegment& coreSegment) override
+ {
+ return coreSegmentFn ? coreSegmentFn(coreSegment) : Visitor::visitEnter(coreSegment);
+ }
+ bool visitEnter(ProviderSegment& providerSegment) override
+ {
+ return providerSegmentFn ? providerSegmentFn(providerSegment) : Visitor::visitEnter(providerSegment);
+ }
+ bool visitEnter(Entity& entity) override
+ {
+ return entityFn ? entityFn(entity) : Visitor::visitEnter(entity);
+ }
+ bool visitEnter(EntitySnapshot& snapshot) override
+ {
+ return snapshotFn ? snapshotFn(snapshot) : Visitor::visitEnter(snapshot);
+ }
+
+ bool visit(EntityInstance& instance) override
+ {
+ return instanceFn ? instanceFn(instance) : Visitor::visit(instance);
+ }
+
+
+
+ // Const versions
+
+
+ bool visitEnter(const Memory& memory) override
+ {
+ return memoryFn ? memoryConstFn(memory) : Visitor::visitEnter(memory);
+ }
+ bool visitEnter(const CoreSegment& coreSegment) override
+ {
+ return coreSegmentFn ? coreSegmentConstFn(coreSegment) : Visitor::visitEnter(coreSegment);
+ }
+ bool visitEnter(const ProviderSegment& providerSegment) override
+ {
+ return providerSegmentFn ? providerSegmentConstFn(providerSegment) : Visitor::visitEnter(providerSegment);
+ }
+ bool visitEnter(const Entity& entity) override
+ {
+ return entityFn ? entityConstFn(entity) : Visitor::visitEnter(entity);
+ }
+ bool visitEnter(const EntitySnapshot& snapshot) override
+ {
+ return memoryFn ? snapshotConstFn(snapshot) : Visitor::visitEnter(snapshot);
+ }
+
+ bool visit(const EntityInstance& instance) override
+ {
+ return instanceFn ? instanceConstFn(instance) : Visitor::visit(instance);
+ }
+
+
+ std::function<bool(Memory& memory)> memoryFn;
+ std::function<bool(const Memory& memory)> memoryConstFn;
+
+ std::function<bool(CoreSegment& coreSegment)> coreSegmentFn;
+ std::function<bool(const CoreSegment& coreSegment)> coreSegmentConstFn;
+
+ std::function<bool(ProviderSegment& providerSegment)> providerSegmentFn;
+ std::function<bool(const ProviderSegment& providerSegment)> providerSegmentConstFn;
+
+ std::function<bool(Entity& entity)> entityFn;
+ std::function<bool(const Entity& entity)> entityConstFn;
+
+ std::function<bool(EntitySnapshot& snapshot)> snapshotFn;
+ std::function<bool(const EntitySnapshot& snapshot)> snapshotConstFn;
+
+ std::function<bool(EntityInstance& instance)> instanceFn;
+ std::function<bool(const EntityInstance& instance)> instanceConstFn;
+
+ };
+
+}
diff --git a/source/RobotAPI/libraries/armem/core/workingmemory/Visitor.cpp b/source/RobotAPI/libraries/armem/core/workingmemory/visitor/Visitor.cpp
similarity index 100%
rename from source/RobotAPI/libraries/armem/core/workingmemory/Visitor.cpp
rename to source/RobotAPI/libraries/armem/core/workingmemory/visitor/Visitor.cpp
diff --git a/source/RobotAPI/libraries/armem/core/workingmemory/Visitor.h b/source/RobotAPI/libraries/armem/core/workingmemory/visitor/Visitor.h
similarity index 98%
rename from source/RobotAPI/libraries/armem/core/workingmemory/Visitor.h
rename to source/RobotAPI/libraries/armem/core/workingmemory/visitor/Visitor.h
index c0a3854a242c030113eeb46409c172563660294a..fa789fcf0b6876ec5d9fa0ff1a3944b4e3bf3fd3 100644
--- a/source/RobotAPI/libraries/armem/core/workingmemory/Visitor.h
+++ b/source/RobotAPI/libraries/armem/core/workingmemory/visitor/Visitor.h
@@ -12,7 +12,7 @@ namespace armarx::armem::wm
/**
- * @brief A visitor for the hierarchical Memory data structure.
+ * @brief A visitor for the hierarchical memory data structure.
*/
class Visitor
{
diff --git a/source/RobotAPI/libraries/armem/mns/ClientPlugin.cpp b/source/RobotAPI/libraries/armem/mns/ClientPlugin.cpp
deleted file mode 100644
index f7bba9445c73a7c13a3374969d631530145d9c14..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/mns/ClientPlugin.cpp
+++ /dev/null
@@ -1,127 +0,0 @@
-#include "ClientPlugin.h"
-
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-
-#include "../error.h"
-
-
-
-namespace armarx::armem::mns::plugins
-{
-
- ClientPlugin::~ClientPlugin()
- {}
-
-
- void ClientPlugin::postCreatePropertyDefinitions(armarx::PropertyDefinitionsPtr& properties)
- {
- if (!properties->hasDefinition(makePropertyName(PROPERTY_MNS_NAME_NAME)))
- {
- properties->defineOptionalProperty<std::string>(
- makePropertyName(PROPERTY_MNS_NAME_NAME),
- parent<ClientPluginUserBase>().memoryNameSystemName,
- "Name of the Memory Name System (MNS) component.");
- }
- if (!properties->hasDefinition(makePropertyName(PROPERTY_MNS_ENABLED_NAME)))
- {
- properties->defineOptionalProperty<bool>(
- makePropertyName(PROPERTY_MNS_ENABLED_NAME),
- parent<ClientPluginUserBase>().memoryNameSystemEnabled,
- "Whether to use (and depend on) the Memory Name System (MNS)."
- "\nSet to false to use this memory as a stand-alone.");
- }
- }
-
- void ClientPlugin::preOnInitComponent()
- {
- if (isMemoryNameSystemEnabled())
- {
- parent().usingProxy(getMemoryNameSystemName());
- }
- }
-
- void ClientPlugin::preOnConnectComponent()
- {
- if (isMemoryNameSystemEnabled())
- {
- parent<ClientPluginUserBase>().memoryNameSystem = getMemoryNameSystem();
- }
- }
-
- bool ClientPlugin::isMemoryNameSystemEnabled()
- {
- return parentDerives<Component>() ?
- parent<Component>().getProperty<bool>(makePropertyName(PROPERTY_MNS_ENABLED_NAME)) :
- parent<ClientPluginUserBase>().memoryNameSystemEnabled;
- }
-
- std::string ClientPlugin::getMemoryNameSystemName()
- {
- return parentDerives<Component>() ?
- parent<Component>().getProperty<std::string>(makePropertyName(PROPERTY_MNS_NAME_NAME)) :
- std::string{parent<ClientPluginUserBase>().memoryNameSystemName};
- }
-
- mns::MemoryNameSystemInterfacePrx ClientPlugin::getMemoryNameSystem()
- {
- return isMemoryNameSystemEnabled() && parentDerives<Component>()
- ? parent<Component>().getProxy<MemoryNameSystemInterfacePrx>(getMemoryNameSystemName())
- : nullptr;
- }
-
- ClientPluginUserBase::~ClientPluginUserBase()
- {
- }
-
- armem::data::WaitForMemoryResult ClientPluginUserBase::useMemory(const MemoryID& id)
- {
- return useMemory(id.memoryName);
- }
-
- armem::data::WaitForMemoryResult ClientPluginUserBase::useMemory(const std::string& memoryName)
- {
- armem::data::WaitForMemoryInput input;
- input.name = memoryName;
-
- armem::data::WaitForMemoryResult result = memoryNameSystem->waitForMemory(input);
- if (result.success)
- {
- if (Component* comp = dynamic_cast<Component*>(this))
- {
- // Add dependency.
- comp->usingProxy(result.proxy->ice_getIdentity().name);
- }
- }
- return result;
- }
-
- armem::data::WaitForMemoryResult ClientPluginUserBase::waitForMemory(const std::string& memoryName)
- {
- if (!memoryNameSystem)
- {
- return {};
- }
- armem::data::WaitForMemoryInput input;
- input.name = memoryName;
- return memoryNameSystem->waitForMemory(input);
- }
-
- armem::data::ResolveMemoryNameResult ClientPluginUserBase::resolveMemoryName(const std::string& memoryName)
- {
- if (!memoryNameSystem)
- {
- return {};
- }
- armem::data::ResolveMemoryNameInput input;
- input.name = memoryName;
- return memoryNameSystem->resolveMemoryName(input);
- }
-
- bool ClientPluginUserBase::isMemoryAvailable(const std::string& memoryName)
- {
- armem::data::ResolveMemoryNameResult result = resolveMemoryName(memoryName);
- return result.success && result.proxy;
- }
-
-}
-
diff --git a/source/RobotAPI/libraries/armem/mns/ClientPlugin.h b/source/RobotAPI/libraries/armem/mns/ClientPlugin.h
deleted file mode 100644
index 4ebc9aad33392ffdf48c019882b14217638174cf..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/mns/ClientPlugin.h
+++ /dev/null
@@ -1,82 +0,0 @@
-#pragma once
-
-#include <ArmarXCore/core/Component.h>
-
-#include <RobotAPI/interface/armem/mns/MemoryNameSystemInterface.h>
-#include <RobotAPI/libraries/armem/core/MemoryID.h>
-
-
-namespace armarx::armem::mns
-{
- class ClientPluginUserBase;
-}
-
-
-namespace armarx::armem::mns::plugins
-{
- /**
- * @brief A base plugin offering optional access and dependency
- * to the Memory Name System (MNS).
- */
- class ClientPlugin : public ComponentPlugin
- {
- public:
-
- using ComponentPlugin::ComponentPlugin;
- virtual ~ClientPlugin() override;
-
- void postCreatePropertyDefinitions(PropertyDefinitionsPtr& properties) override;
-
- void preOnInitComponent() override;
- void preOnConnectComponent() override;
-
- /**
- * @brief Indicate whether the Memory Name System (MNS) is enabled.
- */
- bool isMemoryNameSystemEnabled();
- /**
- * @brief Get the name of the MNS component.
- */
- std::string getMemoryNameSystemName();
-
- /**
- * @brief Get the MNS proxy.
- * @return The MNS proxy when MNS is enabled, nullptr when MNS is disabled.
- */
- mns::MemoryNameSystemInterfacePrx getMemoryNameSystem();
-
- public:
- static constexpr const char* PROPERTY_MNS_ENABLED_NAME = "mns.MemoryNameSystemEnabled";
- static constexpr const char* PROPERTY_MNS_NAME_NAME = "mns.MemoryNameSystemName";
- };
-
-
- /**
- * @brief Base class for users of the `ClientPlugin`.
- * This is itself not a usable plugin user (hence still in the plugins namespace).
- */
- class ClientPluginUserBase
- {
- protected:
-
- virtual ~ClientPluginUserBase();
-
-
- armem::data::WaitForMemoryResult waitForMemory(const std::string& memoryName);
- armem::data::ResolveMemoryNameResult resolveMemoryName(const std::string& memoryName);
- bool isMemoryAvailable(const std::string& memoryName);
-
- armem::data::WaitForMemoryResult useMemory(const MemoryID& id);
- virtual armem::data::WaitForMemoryResult useMemory(const std::string& memoryName);
-
-
- public:
-
- /// Only set when enabled.
- mns::MemoryNameSystemInterfacePrx memoryNameSystem = nullptr;
-
- bool memoryNameSystemEnabled = true;
- std::string memoryNameSystemName = "MemoryNameSystem";
-
- };
-}
diff --git a/source/RobotAPI/libraries/armem/server/ComponentPlugin.cpp b/source/RobotAPI/libraries/armem/server/ComponentPlugin.cpp
index 536c72f49d6ccb4c656f37c345dc6a4ec70b4f17..7b3a1d05817c9c25675abe2e5b8ab011aba1d92d 100644
--- a/source/RobotAPI/libraries/armem/server/ComponentPlugin.cpp
+++ b/source/RobotAPI/libraries/armem/server/ComponentPlugin.cpp
@@ -2,12 +2,13 @@
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-#include "../error.h"
+#include <RobotAPI/libraries/armem/core/error.h>
#include "MemoryToIceAdapter.h"
//#include <RobotAPI/libraries/armem/core/io/diskWriter/NlohmannJSON/NlohmannJSONDiskWriter.h>
+
namespace armarx::armem::server::plugins
{
ComponentPlugin::~ComponentPlugin()
@@ -16,12 +17,17 @@ namespace armarx::armem::server::plugins
void ComponentPlugin::postCreatePropertyDefinitions(PropertyDefinitionsPtr& properties)
{
- ClientPlugin::postCreatePropertyDefinitions(properties);
- properties->topic(memoryListener, this->parent<ComponentPluginUser>().memoryListenerDefaultName);
+ MemoryNameSystemComponentPlugin::postCreatePropertyDefinitions(properties);
- properties->optional(longTermMemoryDatabaseHost, this->parent<ComponentPluginUser>().longTermMemoryDatabaseHostDefault);
- properties->optional(longTermMemoryDatabaseUser, this->parent<ComponentPluginUser>().longTermMemoryDatabaseUserDefault);
- properties->optional(longTermMemoryDatabasePassword, this->parent<ComponentPluginUser>().longTermMemoryDatabasePasswordDefault);
+ ComponentPluginUser& parent = this->parent<ComponentPluginUser>();
+ properties->topic(memoryListener, parent.memoryListenerDefaultName);
+
+ properties->optional(parent.longtermMemoryEnabled, "ltm.00_enabled");
+ properties->optional(parent.longtermMemory.dbsettings.host, "ltm.10_host");
+ properties->optional(parent.longtermMemory.dbsettings.port, "ltm.11_port");
+ properties->optional(parent.longtermMemory.dbsettings.user, "ltm.20_user");
+ properties->optional(parent.longtermMemory.dbsettings.password, "ltm.21_password");
+ properties->optional(parent.longtermMemory.dbsettings.database, "ltm.22_database");
}
@@ -36,12 +42,10 @@ namespace armarx::armem::server::plugins
parent.iceMemory.setMemoryListener(memoryListener);
// establishing connection to ltm and mongodb
- ltm::MongoDBConnectionManager::MongoDBSettings settings;
- settings.host = longTermMemoryDatabaseHost;
- settings.user = longTermMemoryDatabaseUser;
- settings.password = longTermMemoryDatabasePassword;
-
- parent.longtermMemory.reload(settings);
+ if (parent.longtermMemoryEnabled)
+ {
+ parent.longtermMemory.reload();
+ }
}
@@ -57,19 +61,22 @@ namespace armarx::armem::server::plugins
data::RegisterMemoryResult ComponentPlugin::registerMemory(ComponentPluginUser& parent)
{
- data::RegisterMemoryInput input;
- input.name = parent.workingMemory.name();
- input.proxy = MemoryInterfacePrx::checkedCast(parent.getProxy());
- ARMARX_CHECK_NOT_NULL(input.proxy);
- data::RegisterMemoryResult result = parent.memoryNameSystem->registerMemory(input);
- if (result.success)
+ MemoryID id = MemoryID().withMemoryName(parent.workingMemory.name());
+ MemoryInterfacePrx proxy = MemoryInterfacePrx::checkedCast(parent.getProxy());
+ ARMARX_CHECK_NOT_NULL(proxy);
+
+ data::RegisterMemoryResult result;
+ try
{
- ARMARX_DEBUG << "Registered memory '" << input.name << "' in the Memory Name System (MNS).";
+ parent.memoryNameSystem.registerServer(id, proxy);
+ result.success = true;
+ ARMARX_DEBUG << "Registered memory server for " << id << " in the Memory Name System (MNS).";
}
- else
+ catch (const armem::error::ServerRegistrationOrRemovalFailed& e)
{
- ARMARX_WARNING << "Failed to register this memory in the Memory Name System (MNS):"
- << "\n" << result.errorMessage;
+ result.success = false;
+ result.errorMessage = e.what();
+ ARMARX_WARNING << e.what();
}
return result;
}
@@ -77,21 +84,20 @@ namespace armarx::armem::server::plugins
data::RemoveMemoryResult ComponentPlugin::removeMemory(ComponentPluginUser& parent)
{
+ MemoryID id = MemoryID().withMemoryName(parent.workingMemory.name());
+
data::RemoveMemoryResult result;
try
{
- data::RemoveMemoryInput input;
- input.name = parent.workingMemory.name();
- result = parent.memoryNameSystem->removeMemory(input);
- if (result.success)
- {
- ARMARX_DEBUG << "Removed memory '" << input.name << "' from the Memory Name System (MNS).";
- }
- else
- {
- ARMARX_WARNING << "Failed to remove this memory in the Memory Name System (MNS):"
- << "\n" << result.errorMessage;
- }
+ parent.memoryNameSystem.removeServer(id);
+ result.success = true;
+ ARMARX_DEBUG << "Removed memory server for " << id << " from the Memory Name System (MNS).";
+ }
+ catch (const armem::error::ServerRegistrationOrRemovalFailed& e)
+ {
+ result.success = false;
+ result.errorMessage = e.what();
+ ARMARX_WARNING << e.what();
}
catch (const Ice::NotRegisteredException&)
{
@@ -145,8 +151,7 @@ namespace armarx::armem::server
// LTM LOADING
data::StoreResult ComponentPluginUser::store(const data::StoreInput& input, const Ice::Current&)
{
- std::scoped_lock lock(workingMemoryMutex);
- std::scoped_lock lock2(longtermMemoryMutex);
+ std::scoped_lock lock(workingMemoryMutex, longtermMemoryMutex);
return iceMemory.store(input);
}
diff --git a/source/RobotAPI/libraries/armem/server/ComponentPlugin.h b/source/RobotAPI/libraries/armem/server/ComponentPlugin.h
index fc2154f59a0d640a2b6d6e07f5df51465caa2812..5f9ef1d574df06b1644d733e494273ce0660dbbf 100644
--- a/source/RobotAPI/libraries/armem/server/ComponentPlugin.h
+++ b/source/RobotAPI/libraries/armem/server/ComponentPlugin.h
@@ -8,10 +8,10 @@
#include <RobotAPI/interface/armem/client/MemoryListenerInterface.h>
#include <RobotAPI/interface/armem/mns/MemoryNameSystemInterface.h>
-#include "../core/workingmemory/Memory.h"
-#include "../core/longtermmemory/Memory.h"
+#include <RobotAPI/libraries/armem/core/workingmemory/Memory.h>
+#include <RobotAPI/libraries/armem/core/longtermmemory/Memory.h>
-#include "../mns/ClientPlugin.h"
+#include <RobotAPI/libraries/armem/client/MemoryNameSystemComponentPlugin.h>
#include "MemoryToIceAdapter.h"
@@ -24,11 +24,12 @@ namespace armarx::armem::server
namespace armarx::armem::server::plugins
{
- class ComponentPlugin : public mns::plugins::ClientPlugin
+ class ComponentPlugin : public client::plugins::MemoryNameSystemComponentPlugin
{
+ using Base = client::plugins::MemoryNameSystemComponentPlugin;
public:
- using ClientPlugin::ClientPlugin;
+ using Base::MemoryNameSystemComponentPlugin;
virtual ~ComponentPlugin() override;
void postCreatePropertyDefinitions(PropertyDefinitionsPtr& properties) override;
@@ -67,7 +68,7 @@ namespace armarx::armem::server
class ComponentPluginUser :
virtual public ManagedIceObject
, virtual public MemoryInterface
- , virtual public mns::plugins::ClientPluginUserBase
+ , virtual public client::MemoryNameSystemComponentPluginUser
{
public:
@@ -99,14 +100,12 @@ namespace armarx::armem::server
wm::Memory workingMemory;
std::mutex workingMemoryMutex;
+ bool longtermMemoryEnabled = true;
ltm::Memory longtermMemory;
std::mutex longtermMemoryMutex;
/// property defaults
std::string memoryListenerDefaultName = "MemoryUpdates";
- std::string longTermMemoryDatabaseHostDefault = "";
- std::string longTermMemoryDatabaseUserDefault = "";
- std::string longTermMemoryDatabasePasswordDefault = "";
/// Helps connecting `memory` to ice. Used to handle Ice callbacks.
MemoryToIceAdapter iceMemory { &workingMemory, &longtermMemory};
diff --git a/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.cpp b/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.cpp
index c1ae33fbe624fe2558c8f2dfb11d2ae380fb192a..ebfaa65cb2874649dc5f737998cefee3a02ff6ea 100644
--- a/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.cpp
+++ b/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.cpp
@@ -1,13 +1,13 @@
#include "MemoryToIceAdapter.h"
+#include "ArmarXCore/core/logging/Logging.h"
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include "../error.h"
#include "../core/workingmemory/ice_conversions.h"
#include "query_proc/workingmemory/MemoryQueryProcessor.h"
+#include "query_proc/longtermmemory/MemoryQueryProcessor.h"
-//#include "../core/io/diskWriter/NlohmannJSON/NlohmannJSONDiskWriter.h"
-//#include "../core/io/diskReader/NlohmannJSON/NlohmannJSONDiskReader.h"
namespace armarx::armem::server
{
@@ -151,16 +151,22 @@ namespace armarx::armem::server
EntityUpdateResult& result = commitResult.results.emplace_back();
try
{
- MemoryID snapshotID = workingMemory->update(update);
+ auto updateResult = workingMemory->update(update);
result.success = true;
- result.snapshotID = snapshotID;
+ result.snapshotID = updateResult.id;
result.timeArrived = update.timeArrived;
+ // TODO: Add if and param here (fabian.peller)
+ for (const auto& snapshot : updateResult.removedSnapshots)
+ {
+ ARMARX_DEBUG << "The id " << snapshot.id() << " was removed from wm";
+ }
+
if (publishUpdates)
{
data::MemoryID& id = updatedIDs.emplace_back();
- armem::toIce(id, snapshotID);
+ armem::toIce(id, result.snapshotID);
}
}
catch (const error::ArMemError& e)
@@ -195,13 +201,47 @@ namespace armarx::armem::server
{
ARMARX_CHECK_NOT_NULL(workingMemory);
- armem::wm::query_proc::MemoryQueryProcessor processor(
+ armem::wm::query_proc::MemoryQueryProcessor wm_processor(
input.withData ? armem::DataMode::WithData
: armem::DataMode::NoData);
- return processor.process(input, *workingMemory);
+
+ armem::query::data::Result result;
+ wm::Memory wm_result = wm_processor.process(input, *workingMemory, /* ignore if: */ { query::data::QueryTarget::LTM });
+
+ armem::ltm::query_proc::MemoryQueryProcessor ltm_processor;
+ ltm::Memory ltm_result = ltm_processor.process(input, *longtermMemory, /* ignore if: */ { query::data::QueryTarget::WM });
+
+
+ if (ltm_result.hasData())
+ {
+ // ATTENTION: This code block moves data from LTM back into WM.
+ // However, since some segments are constrained, the WM might send data back to LTM.
+ // This may also affect the data returned by the current query.
+ // However, this is expected behavior, since we copy the data in the processor (copyEmpty) we can safely return the copy and
+ // remove the original memory reference from WM here.
+ wm::Memory ltm_converted = ltm_result.convert();
+ wm_result.append(ltm_converted);
+
+ // query again to limit output size (TODO: Skip if querytype is all)
+ wm::Memory merged_result = wm_processor.process(input, wm_result);
+ result.memory = toIce<data::MemoryPtr>(merged_result);
+
+ // also move results of ltm to wm
+ //this->commit(ltm_converted.toCommit());
+
+ // mark removed entries of wm in viewer
+ // TODO
+ }
+ else
+ {
+ result.memory = toIce<data::MemoryPtr>(wm_result);
+ }
+
+ result.success = true;
+ return result;
}
- // LTM LOADING
+ // LTM LOADING FROM LTM
query::data::Result MemoryToIceAdapter::load(const armem::query::data::Input& query)
{
ARMARX_CHECK_NOT_NULL(longtermMemory);
@@ -218,19 +258,23 @@ namespace armarx::armem::server
ARMARX_CHECK_NOT_NULL(longtermMemory);
data::StoreResult output;
+ for (const auto& query : input.query.memoryQueries)
+ {
+ // force query to only query WM
+ // TODO: Think about other query class (fabian.peller)
+ query->target = query::data::QueryTarget::WM;
+ }
+
armem::query::data::Result queryResult = this->query(input.query);
+ output.success = queryResult.success;
+
if (queryResult.success)
{
wm::Memory m;
fromIce(queryResult.memory, m);
longtermMemory->append(m);
}
- else
- {
- output.success = false;
- }
- ARMARX_IMPORTANT << "Finsihed";
return output;
}
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/base/BaseQueryProcessorBase.h b/source/RobotAPI/libraries/armem/server/query_proc/base/BaseQueryProcessorBase.h
index 3ea2b0bdd45230dab810bd628cf338d2b685662b..69a305e45befca5a9189c6fb264985126d16fbf3 100644
--- a/source/RobotAPI/libraries/armem/server/query_proc/base/BaseQueryProcessorBase.h
+++ b/source/RobotAPI/libraries/armem/server/query_proc/base/BaseQueryProcessorBase.h
@@ -2,8 +2,6 @@
#include <RobotAPI/interface/armem/query.h>
-#include <RobotAPI/libraries/armem/core/DataMode.h>
-
namespace armarx::armem::base::query_proc
{
@@ -20,34 +18,42 @@ namespace armarx::armem::base::query_proc
public:
- DataT process(const QueryT& query, const DataT& data) const
+ DataT process(const QueryT& query, const DataT& data, const std::vector<query::data::QueryTarget>& ignoreTargets = {}) const
{
DataT result = data.copyEmpty();
+
+ if (std::find(ignoreTargets.begin(), ignoreTargets.end(), query.target) != ignoreTargets.end())
+ {
+ return result;
+ }
this->process(result, query, data);
return result;
}
- DataT process(const QueryPtrT& query, const DataT& data) const
+ DataT process(const QueryPtrT& query, const DataT& data, const std::vector<query::data::QueryTarget>& ignoreTargets = {}) const
{
- return this->process(*query, *data);
+ return this->process(*query, *data, ignoreTargets);
}
- DataT process(const QuerySeqT& queries, const DataT& data) const
+ DataT process(const QuerySeqT& queries, const DataT& data, const std::vector<query::data::QueryTarget>& ignoreTargets = {}) const
{
DataT result = data.copyEmpty();
- this->process(result, queries, data);
+ this->process(result, queries, data, ignoreTargets);
return result;
}
- void process(DataT& result, const QuerySeqT& queries, const DataT& data) const
+ void process(DataT& result, const QuerySeqT& queries, const DataT& data, const std::vector<query::data::QueryTarget>& ignoreTargets = {}) const
{
for (const auto& query : queries)
{
+ if (std::find(ignoreTargets.begin(), ignoreTargets.end(), query->target) != ignoreTargets.end())
+ {
+ continue;
+ }
this->process(result, *query, data);
}
}
virtual void process(DataT& result, const QueryT& query, const DataT& data) const = 0;
};
-
}
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/base/EntityQueryProcessorBase.h b/source/RobotAPI/libraries/armem/server/query_proc/base/EntityQueryProcessorBase.h
index 2a4308ebdc837255a9f6a816dc62880dc09e8c21..40113207f194c76301fbe8768a6d42c22f8b01db 100644
--- a/source/RobotAPI/libraries/armem/server/query_proc/base/EntityQueryProcessorBase.h
+++ b/source/RobotAPI/libraries/armem/server/query_proc/base/EntityQueryProcessorBase.h
@@ -96,19 +96,37 @@ namespace armarx::armem::base::query_proc
}
catch (const armem::error::EntityHistoryEmpty&)
{
- // Leave empty.
+ if (false)
+ {
+ ARMARX_IMPORTANT << "Failed to retrieve latest snapshot from entity " << entity.id() << ". "
+ << "Entity has not timestamps.";
+ }
}
}
else
{
- Time time = fromIce<Time>(query.timestamp);
+ const Time time = fromIce<Time>(query.timestamp);
try
{
- addResultSnapshot(result, entity.getSnapshot(time));
+ auto snapshot = entity.getSnapshot(time);
+ addResultSnapshot(result, snapshot);
}
catch (const armem::error::MissingEntry&)
{
// Leave empty.
+ if (false)
+ {
+ std::stringstream ss;
+ ss << "Failed to retrieve snapshot with timestamp "
+ << armem::toDateTimeMilliSeconds(time)
+ << " from entity " << entity.id() << ".\n"
+ << "Entity has timestamps: ";
+ for (const Time& t : entity.getTimestamps())
+ {
+ ss << "\n- " << armem::toDateTimeMilliSeconds(t);
+ }
+ ARMARX_IMPORTANT << ss.str();
+ }
}
}
}
@@ -170,61 +188,6 @@ namespace armarx::armem::base::query_proc
}
- inline auto lastElementBeforeOrAt(const auto& history, const auto timestamp) const
- {
- // first element equal or greater
- typename std::map<Time, EntitySnapshotT>::const_iterator refItFwd = history.upper_bound(timestamp);
-
- // last element less than
- typename std::map<Time, EntitySnapshotT>::const_iterator refItFwdLt = std::prev(refItFwd);
-
- // last element not greater than => if this is invalid, we have no suitable elements
- typename std::map<Time, EntitySnapshotT>::const_reverse_iterator refIt(refItFwd);
- if (refIt == history.rend())
- {
- return history.end();
- }
-
- // now either refItFwd is a perfect match ...
- if (refItFwd->first == timestamp)
- {
- return refItFwd;
- }
-
- // ... or we return the element before if possible
- if (refItFwdLt != history.begin())
- {
- return refItFwdLt;
- }
-
- return history.end();
- }
-
- inline auto lastElementBefore(const auto& history, const auto& timestamp) const
- {
- // first element equal or greater
- typename std::map<Time, EntitySnapshotT>::const_iterator refItFwd = history.upper_bound(timestamp);
-
- // last element less than
- typename std::map<Time, EntitySnapshotT>::const_iterator refItFwdLt = std::prev(refItFwd);
-
- // last element not greater than => if this is invalid, we have no suitable elements
- typename std::map<Time, EntitySnapshotT>::const_reverse_iterator refIt(refItFwd);
- if (refIt == history.rend())
- {
- return history.end();
- }
-
- // we return the element before if possible
- if (refItFwdLt != history.begin())
- {
- return refItFwdLt;
- }
-
- return history.end();
- }
-
-
void process(_EntityT& result,
const armem::query::data::entity::BeforeOrAtTime& query,
const _EntityT& entity) const
@@ -232,12 +195,15 @@ namespace armarx::armem::base::query_proc
const auto referenceTimestamp = fromIce<Time>(query.timestamp);
ARMARX_CHECK(referenceTimestamp.toMicroSeconds() >= 0) << "Reference timestamp is negative!";
- const auto beforeOrAt = lastElementBeforeOrAt(entity.history(), referenceTimestamp);
-
- if (beforeOrAt != entity.history().end())
+ try
{
+ auto beforeOrAt = entity.getLatestSnapshotBeforeOrAt(referenceTimestamp);
addResultSnapshot(result, beforeOrAt);
}
+ catch (const armem::error::MissingEntry&)
+ {
+ // Leave empty.
+ }
}
@@ -250,31 +216,24 @@ namespace armarx::armem::base::query_proc
const auto maxEntries = fromIce<std::int64_t>(query.maxEntries);
- const auto itBefore = lastElementBefore(entity.history(), referenceTimestamp);
- if (itBefore == entity.history().end())
+ try
{
- ARMARX_WARNING << "No suitable element found";
- return;
- }
-
- const auto nEntriesBefore = std::distance(entity.history().begin(), itBefore);
+ const auto before = entity.getSnapshotsBefore(referenceTimestamp);
- const int nEntries = [&]()
- {
- // see query.ice
- if (maxEntries > 0)
+ int i = 0;
+ for (const auto& snapshot : before)
{
- return std::min(nEntriesBefore, maxEntries);
+ if (maxEntries >= 0 && i >= maxEntries)
+ {
+ break;
+ }
+ addResultSnapshot(result, snapshot);
+ ++i;
}
-
- return nEntriesBefore; // all elements before timestamp
}
- ();
-
- auto it = itBefore;
- for (std::int64_t i = 0; i < nEntries; i++, --it)
+ catch (const error::MissingEntry&)
{
- addResultSnapshot(result, it);
+ // Leave empty.
}
}
@@ -282,7 +241,6 @@ namespace armarx::armem::base::query_proc
const armem::query::data::entity::TimeApprox& query,
const _EntityT& entity) const
{
-
const auto referenceTimestamp = fromIce<Time>(query.timestamp);
ARMARX_CHECK(referenceTimestamp.toMicroSeconds() >= 0) << "Reference timestamp is negative!";
@@ -300,67 +258,37 @@ namespace armarx::armem::base::query_proc
return std::abs(t.toMicroSeconds() - referenceTimestampMicroSeconds) <= epsDuration;
};
- // first element before or at timestamp
- const auto beforeOrAt = lastElementBeforeOrAt(entity.history(), referenceTimestamp);
- const bool isBeforeOrAtValid = beforeOrAt != entity.history().end();
- const auto isPerfectMatch = isBeforeOrAtValid && beforeOrAt->first == referenceTimestamp;
-
- // first element greater
- const auto after = entity.history().upper_bound(referenceTimestamp);
- const bool isAfterValid = after != entity.history().end();
-
- // if both are invalid, there is nothing to be gained here.
- if ((not isBeforeOrAtValid) and (not isAfterValid))
- {
- const armem::Time dt = referenceTimestamp - entity.getLatestTimestamp();
-
- ARMARX_WARNING << "Lookup " << dt.toMilliSeconds() << " ms into the future.";
- return;
- }
- // -> now one or both are valid ...
-
- // is 'before' a perfect match?
- if(isPerfectMatch)
- {
- addResultSnapshot(result, beforeOrAt);
- return;
- }
-
- // only 'before' valid?
- if (not isAfterValid)
+ try
{
- if (isInRange(beforeOrAt->first))
+ // last element before or at timestamp
+ auto beforeOrAt = entity.getLatestSnapshotBeforeOrAt(referenceTimestamp);
+ const auto timestampOfMatchBefore = beforeOrAt.id().timestamp;
+ const auto isPerfectMatch = timestampOfMatchBefore == referenceTimestamp;
+ if (isInRange(timestampOfMatchBefore))
{
addResultSnapshot(result, beforeOrAt);
}
- return;
- }
- // only 'after' valid?
- if (not isBeforeOrAtValid)
- {
- if (isInRange(after->first))
+ // earsly stop, not necessary to also get the next since the match is perfect
+ if (isPerfectMatch)
+ {
+ return;
+ }
+
+ // first element after or at timestamp (or at because of fewer checks, we can assure that there is not element at)
+ const auto after = entity.getFirstSnapshotAfterOrAt(referenceTimestamp);
+ const auto timestampOfMatchAfter = after.id().timestamp;
+ if (isInRange(timestampOfMatchAfter))
{
addResultSnapshot(result, after);
}
- return;
}
- // -> now both are valid
-
- // return both (if in range) => user can interpolate
- if (isInRange(beforeOrAt->first))
+ catch (const armem::error::MissingEntry&)
{
- addResultSnapshot(result, beforeOrAt);
- }
- if (isInRange(after->first))
- {
- addResultSnapshot(result, after);
}
-
}
-
static size_t negativeIndexSemantics(long index, size_t size)
{
const size_t max = size > 0 ? size - 1 : 0;
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/base/MemoryQueryProcessorBase.h b/source/RobotAPI/libraries/armem/server/query_proc/base/MemoryQueryProcessorBase.h
index c50086b553164b70f10ed1559a18420649eb3892..5de194d7ba2debada4ef9363005b1a7f2e927e94 100644
--- a/source/RobotAPI/libraries/armem/server/query_proc/base/MemoryQueryProcessorBase.h
+++ b/source/RobotAPI/libraries/armem/server/query_proc/base/MemoryQueryProcessorBase.h
@@ -32,6 +32,13 @@ namespace armarx::armem::base::query_proc
using EntityT = typename ProviderSegmentT::EntityT;
using EntitySnapshotT = typename EntityT::EntitySnapshotT;
+
+ using Base::process;
+ _MemoryT process(const armem::query::data::Input& input, const _MemoryT& memory, const std::vector<query::data::QueryTarget>& ignoreTargets = {}) const
+ {
+ return this->process(input.memoryQueries, memory, ignoreTargets);
+ }
+
void process(_MemoryT& result,
const armem::query::data::MemoryQuery& query,
const _MemoryT& memory) const override
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/longtermmemory/EntityQueryProcessor.h b/source/RobotAPI/libraries/armem/server/query_proc/longtermmemory/EntityQueryProcessor.h
index 5ec4c5dde481e16f0af30589d57ff2a447d81221..952d759c8552c7939f6e08266e7feb880a85438e 100644
--- a/source/RobotAPI/libraries/armem/server/query_proc/longtermmemory/EntityQueryProcessor.h
+++ b/source/RobotAPI/libraries/armem/server/query_proc/longtermmemory/EntityQueryProcessor.h
@@ -31,6 +31,7 @@ namespace armarx::armem::ltm::query_proc
void addResultSnapshot(ltm::Entity& result, const ltm::EntitySnapshot& snapshot) const override
{
+ ARMARX_INFO << "addResultSnapshot" << __LINE__;
result.addSnapshot(snapshot.copy());
}
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/MemoryQueryProcessor.h b/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/MemoryQueryProcessor.h
index b017c3b561569c1da5d3d7f661ee604bbb1cae12..b0616c658a7915568fd910a8f35c9d4701aa1d0b 100644
--- a/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/MemoryQueryProcessor.h
+++ b/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/MemoryQueryProcessor.h
@@ -21,28 +21,6 @@ namespace armarx::armem::wm::query_proc
Base(dataMode), coreSegmentProcessor(dataMode)
{}
- armem::query::data::Result process(const armem::query::data::Input& input,
- const wm::Memory& memory) const
- {
- armem::query::data::Result result;
- result.memory = processToIce(input.memoryQueries, memory);
- result.success = true;
- return result;
- }
-
- using Base::process;
- data::MemoryPtr processToIce(const armem::query::data::MemoryQuery& query,
- const wm::Memory& memory) const
- {
- return toIce<data::MemoryPtr>(process(query, memory));
- }
-
- data::MemoryPtr processToIce(const armem::query::data::MemoryQuerySeq& queries,
- const wm::Memory& memory) const
- {
- return toIce<data::MemoryPtr>(process(queries, memory));
- }
-
protected:
virtual CoreSegmentT coreSegmentProcessorProcess(const armem::query::data::CoreSegmentQuerySeq& q, const CoreSegmentT& s) const override
{
diff --git a/source/RobotAPI/libraries/armem/test/ArMemLTMTest.cpp b/source/RobotAPI/libraries/armem/test/ArMemLTMTest.cpp
index f6271553eddfe787162841ba7c428f7f74aa1626..24c0a22203aae81bfc52802a9f48fbb9755c9c5f 100644
--- a/source/RobotAPI/libraries/armem/test/ArMemLTMTest.cpp
+++ b/source/RobotAPI/libraries/armem/test/ArMemLTMTest.cpp
@@ -114,7 +114,7 @@ namespace ArMemLTMTest
for (unsigned int j = 0; j < numInstances; ++j)
{
- aron::datanavigator::DictNavigatorPtr m = aron::datanavigator::DictNavigator::DynamicCastAndCheck(r.generateAronDataFromType(t));
+ aron::datanavigator::DictNavigatorPtr m = aron::datanavigator::DictNavigator::DynamicCastAndCheck(r.generateEmptyAronDataFromType(t));
r.initializeRandomly(m, t);
q.push_back(m);
}
diff --git a/source/RobotAPI/libraries/armem/test/ArMemMemoryTest.cpp b/source/RobotAPI/libraries/armem/test/ArMemMemoryTest.cpp
index 7588230d8bab0f9fc42093c01382497ddcfc402b..984c1f8e508ec631527f53d5fdff8ea32081ad6e 100644
--- a/source/RobotAPI/libraries/armem/test/ArMemMemoryTest.cpp
+++ b/source/RobotAPI/libraries/armem/test/ArMemMemoryTest.cpp
@@ -627,8 +627,9 @@ BOOST_AUTO_TEST_CASE(test_segment_setup)
{
update.entityID = armem::MemoryID::fromString("Memory/OtherCoreSegment");
BOOST_CHECK_THROW(coreSegment.update(update), armem::error::ContainerNameMismatch);
- update.entityID = armem::MemoryID::fromString("Memory/ImageRGB/MissingProvider");
- BOOST_CHECK_THROW(coreSegment.update(update), armem::error::MissingEntry);
+ // This adds the segment now (changed default behavior)
+ //update.entityID = armem::MemoryID::fromString("Memory/ImageRGB/MissingProvider");
+ //BOOST_CHECK_THROW(coreSegment.update(update), armem::error::MissingEntry);
}
armem::wm::ProviderSegment& providerSegment = coreSegment.addProviderSegment("SomeRGBImageProvider");
diff --git a/source/RobotAPI/libraries/armem/test/ArMemQueryTest.cpp b/source/RobotAPI/libraries/armem/test/ArMemQueryTest.cpp
index 789600c2fe82857b865c845d6892fc2da44d3d6a..b2d07571d619531f45381d1238ca8201b8e0175e 100644
--- a/source/RobotAPI/libraries/armem/test/ArMemQueryTest.cpp
+++ b/source/RobotAPI/libraries/armem/test/ArMemQueryTest.cpp
@@ -134,7 +134,7 @@ BOOST_AUTO_TEST_CASE(test_entity_Single_latest)
BOOST_AUTO_TEST_CASE(test_entity_Single_existing)
{
- addResults(query::entity::Single { 3000 });
+ addResults(query::entity::Single { query::QueryTarget::WMAndLTM, 3000 });
BOOST_REQUIRE_GT(results.size(), 0);
for (const armem::wm::Entity& result : results)
@@ -148,7 +148,7 @@ BOOST_AUTO_TEST_CASE(test_entity_Single_existing)
BOOST_AUTO_TEST_CASE(test_entity_Single_non_existing)
{
- addResults(query::entity::Single { 3500 });
+ addResults(query::entity::Single { query::QueryTarget::WMAndLTM, 3500 });
BOOST_REQUIRE_GT(results.size(), 0);
for (const armem::wm::Entity& result : results)
@@ -182,7 +182,7 @@ BOOST_AUTO_TEST_CASE(test_entity_All)
BOOST_AUTO_TEST_CASE(test_entity_TimeRange_slice)
{
- addResults(query::entity::TimeRange{ 1500, 3500 });
+ addResults(query::entity::TimeRange{ query::QueryTarget::WMAndLTM, 1500, 3500 });
BOOST_REQUIRE_GT(results.size(), 0);
for (const armem::wm::Entity& result : results)
@@ -203,7 +203,7 @@ BOOST_AUTO_TEST_CASE(test_entity_TimeRange_slice)
BOOST_AUTO_TEST_CASE(test_entity_TimeRange_exact)
{
- addResults(query::entity::TimeRange{ 2000, 4000 });
+ addResults(query::entity::TimeRange{ query::QueryTarget::WMAndLTM, 2000, 4000 });
BOOST_REQUIRE_GT(results.size(), 0);
for (const armem::wm::Entity& result : results)
@@ -223,8 +223,8 @@ BOOST_AUTO_TEST_CASE(test_entity_TimeRange_exact)
BOOST_AUTO_TEST_CASE(test_entity_TimeRange_all)
{
- addResults(query::entity::TimeRange{ 0, 10000 });
- addResults(query::entity::TimeRange{ -1, -1 });
+ addResults(query::entity::TimeRange{ query::QueryTarget::WMAndLTM, 0, 10000 });
+ addResults(query::entity::TimeRange{ query::QueryTarget::WMAndLTM, -1, -1 });
BOOST_REQUIRE_GT(results.size(), 0);
for (const armem::wm::Entity& result : results)
@@ -242,8 +242,8 @@ BOOST_AUTO_TEST_CASE(test_entity_TimeRange_all)
BOOST_AUTO_TEST_CASE(test_entity_TimeRange_empty)
{
- addResults(query::entity::TimeRange{ 2400, 2600 });
- addResults(query::entity::TimeRange{ 6000, 1000 });
+ addResults(query::entity::TimeRange{ query::QueryTarget::WMAndLTM, 2400, 2600 });
+ addResults(query::entity::TimeRange{ query::QueryTarget::WMAndLTM, 6000, 1000 });
BOOST_REQUIRE_GT(results.size(), 0);
for (const armem::wm::Entity& result : results)
@@ -257,7 +257,7 @@ BOOST_AUTO_TEST_CASE(test_entity_TimeRange_empty)
BOOST_AUTO_TEST_CASE(test_entity_TimeRange_from_start)
{
- addResults(query::entity::TimeRange{ -1, 2500 });
+ addResults(query::entity::TimeRange{ query::QueryTarget::WMAndLTM, -1, 2500 });
BOOST_REQUIRE_GT(results.size(), 0);
for (const armem::wm::Entity& result : results)
@@ -278,7 +278,7 @@ BOOST_AUTO_TEST_CASE(test_entity_TimeRange_from_start)
BOOST_AUTO_TEST_CASE(test_entity_TimeRange_to_end)
{
- addResults(query::entity::TimeRange{ 2500, -1 });
+ addResults(query::entity::TimeRange{ query::QueryTarget::WMAndLTM, 2500, -1 });
BOOST_REQUIRE_GT(results.size(), 0);
for (const armem::wm::Entity& result : results)
@@ -302,7 +302,7 @@ BOOST_AUTO_TEST_CASE(test_entity_TimeRange_to_end)
BOOST_AUTO_TEST_CASE(test_entity_BeforeTime_1)
{
BOOST_REQUIRE_EQUAL(entity.size(), 5);
- addResults(query::entity::BeforeTime{ 3500, 1 });
+ addResults(query::entity::BeforeTime{ query::QueryTarget::WMAndLTM, 3500, 1 });
BOOST_REQUIRE_EQUAL(results.size(), 2);
for (const auto& result : results)
@@ -318,7 +318,7 @@ BOOST_AUTO_TEST_CASE(test_entity_BeforeTime_1)
BOOST_AUTO_TEST_CASE(test_entity_BeforeTime_2)
{
BOOST_REQUIRE_EQUAL(entity.size(), 5);
- addResults(query::entity::BeforeTime{ 3500, 2});
+ addResults(query::entity::BeforeTime{ query::QueryTarget::WMAndLTM, 3500, 2});
BOOST_REQUIRE_EQUAL(results.size(), 2);
for (const auto& result : results)
@@ -343,7 +343,7 @@ BOOST_AUTO_TEST_CASE(test_entity_BeforeTime_2)
BOOST_AUTO_TEST_CASE(test_entity_BeforeOrAtTime_before)
{
BOOST_REQUIRE_EQUAL(entity.size(), 5);
- addResults(query::entity::BeforeOrAtTime{ 3500 });
+ addResults(query::entity::BeforeOrAtTime{ query::QueryTarget::WMAndLTM, 3500 });
BOOST_REQUIRE_EQUAL(results.size(), 2);
for (const auto& result : results)
@@ -358,7 +358,7 @@ BOOST_AUTO_TEST_CASE(test_entity_BeforeOrAtTime_before)
BOOST_AUTO_TEST_CASE(test_entity_BeforeOrAtTime_at)
{
BOOST_REQUIRE_EQUAL(entity.size(), 5);
- addResults(query::entity::BeforeOrAtTime{ 3000 });
+ addResults(query::entity::BeforeOrAtTime{ query::QueryTarget::WMAndLTM, 3000 });
BOOST_REQUIRE_EQUAL(results.size(), 2);
for (const auto& result : results)
@@ -373,7 +373,7 @@ BOOST_AUTO_TEST_CASE(test_entity_BeforeOrAtTime_at)
BOOST_AUTO_TEST_CASE(test_entity_BeforeOrAtTime_lookup_past)
{
BOOST_REQUIRE_EQUAL(entity.size(), 5);
- addResults(query::entity::BeforeOrAtTime{ 1 });
+ addResults(query::entity::BeforeOrAtTime{ query::QueryTarget::WMAndLTM, 1 });
BOOST_REQUIRE_EQUAL(results.size(), 2);
for (const auto& result : results)
@@ -393,7 +393,7 @@ BOOST_AUTO_TEST_CASE(test_entity_BeforeOrAtTime_lookup_past)
BOOST_AUTO_TEST_CASE(test_entity_TimeApprox_no_limit)
{
BOOST_REQUIRE_EQUAL(entity.size(), 5);
- addResults(query::entity::TimeApprox{ 3500, -1});
+ addResults(query::entity::TimeApprox{ query::QueryTarget::WMAndLTM, 3500, -1});
BOOST_REQUIRE_EQUAL(results.size(), 2);
for (const auto& result : results)
@@ -419,7 +419,7 @@ BOOST_AUTO_TEST_CASE(test_entity_TimeApprox_no_limit)
BOOST_AUTO_TEST_CASE(test_entity_TimeApprox_limit_600)
{
BOOST_REQUIRE_EQUAL(entity.size(), 5);
- addResults(query::entity::TimeApprox{ 3500, 600});
+ addResults(query::entity::TimeApprox{ query::QueryTarget::WMAndLTM, 3500, 600});
BOOST_REQUIRE_EQUAL(results.size(), 2);
for (const auto& result : results)
@@ -445,7 +445,7 @@ BOOST_AUTO_TEST_CASE(test_entity_TimeApprox_limit_600)
BOOST_AUTO_TEST_CASE(test_entity_TimeApprox_limit_too_small)
{
BOOST_REQUIRE_EQUAL(entity.size(), 5);
- addResults(query::entity::TimeApprox{ 3500, 100});
+ addResults(query::entity::TimeApprox{ query::QueryTarget::WMAndLTM, 3500, 100});
BOOST_REQUIRE_EQUAL(results.size(), 2);
for (const auto& result : results)
@@ -464,7 +464,7 @@ BOOST_AUTO_TEST_CASE(test_entity_TimeApprox_limit_too_small)
BOOST_AUTO_TEST_CASE(test_entity_TimeApprox_limit_only_next)
{
BOOST_REQUIRE_EQUAL(entity.size(), 5);
- addResults(query::entity::TimeApprox{ 3700, 400});
+ addResults(query::entity::TimeApprox{ query::QueryTarget::WMAndLTM, 3700, 400});
BOOST_REQUIRE_EQUAL(results.size(), 2);
for (const auto& result : results)
@@ -489,7 +489,7 @@ BOOST_AUTO_TEST_CASE(test_entity_TimeApprox_limit_only_next)
BOOST_AUTO_TEST_CASE(test_entity_TimeApprox_limit_only_previous)
{
BOOST_REQUIRE_EQUAL(entity.size(), 5);
- addResults(query::entity::TimeApprox{ 3300, 400});
+ addResults(query::entity::TimeApprox{ query::QueryTarget::WMAndLTM, 3300, 400});
BOOST_REQUIRE_EQUAL(results.size(), 2);
for (const auto& result : results)
@@ -514,7 +514,7 @@ BOOST_AUTO_TEST_CASE(test_entity_TimeApprox_limit_only_previous)
BOOST_AUTO_TEST_CASE(test_entity_TimeApprox_perfect_match)
{
BOOST_REQUIRE_EQUAL(entity.size(), 5);
- addResults(query::entity::TimeApprox{ 3000, -1});
+ addResults(query::entity::TimeApprox{ query::QueryTarget::WMAndLTM, 3000, -1});
BOOST_REQUIRE_EQUAL(results.size(), 2);
for (const auto& result : results)
@@ -539,7 +539,7 @@ BOOST_AUTO_TEST_CASE(test_entity_TimeApprox_perfect_match)
BOOST_AUTO_TEST_CASE(test_entity_TimeApprox_lookup_past)
{
BOOST_REQUIRE_EQUAL(entity.size(), 5);
- addResults(query::entity::TimeApprox{ 1, 1});
+ addResults(query::entity::TimeApprox{ query::QueryTarget::WMAndLTM, 1, 1});
BOOST_REQUIRE_EQUAL(results.size(), 2);
for (const auto& result : results)
@@ -557,7 +557,7 @@ BOOST_AUTO_TEST_CASE(test_entity_TimeApprox_lookup_past)
BOOST_AUTO_TEST_CASE(test_entity_TimeApprox_lookup_future)
{
BOOST_REQUIRE_EQUAL(entity.size(), 5);
- addResults(query::entity::TimeApprox{ 10'000, 1});
+ addResults(query::entity::TimeApprox{ query::QueryTarget::WMAndLTM, 10'000, 1});
BOOST_REQUIRE_EQUAL(results.size(), 2);
for (const auto& result : results)
@@ -575,7 +575,7 @@ BOOST_AUTO_TEST_CASE(test_entity_TimeApprox_lookup_future)
BOOST_AUTO_TEST_CASE(test_entity_TimeApprox_lookup_future_valid)
{
BOOST_REQUIRE_EQUAL(entity.size(), 5);
- addResults(query::entity::TimeApprox{ 10'000, -1});
+ addResults(query::entity::TimeApprox{ query::QueryTarget::WMAndLTM, 10'000, -1});
BOOST_REQUIRE_EQUAL(results.size(), 2);
for (const auto& result : results)
@@ -594,7 +594,7 @@ BOOST_AUTO_TEST_CASE(test_entity_TimeApprox_lookup_future_valid)
BOOST_AUTO_TEST_CASE(test_entity_TimeApprox_lookup_invalid_timestamp)
{
- BOOST_REQUIRE_THROW(addResults(query::entity::TimeApprox{ -1, 1}), ::armarx::LocalException);
+ BOOST_REQUIRE_THROW(addResults(query::entity::TimeApprox{ query::QueryTarget::WMAndLTM, -1, 1}), ::armarx::LocalException);
}
@@ -634,7 +634,7 @@ BOOST_AUTO_TEST_CASE(test_negative_index_semantics)
BOOST_AUTO_TEST_CASE(test_entity_IndexRange_all_default)
{
addResults(query::entity::IndexRange());
- addResults(query::entity::IndexRange(0, -1));
+ addResults(query::entity::IndexRange(query::QueryTarget::WMAndLTM, 0, -1));
BOOST_REQUIRE_GT(results.size(), 0);
for (const armem::wm::Entity& result : results)
@@ -654,10 +654,10 @@ BOOST_AUTO_TEST_CASE(test_entity_IndexRange_all_default)
BOOST_AUTO_TEST_CASE(test_entity_IndexRange_slice)
{
BOOST_REQUIRE_EQUAL(entity.size(), 5);
- addResults(query::entity::IndexRange{ 1, 3 }); // => [1, 2, 3]
- addResults(query::entity::IndexRange{ 1, -2 }); // 5 - 2 = 3
- addResults(query::entity::IndexRange{ -4, 3 }); // 5 - 4 = 1
- addResults(query::entity::IndexRange{ -4, -2 });
+ addResults(query::entity::IndexRange{ query::QueryTarget::WMAndLTM, 1, 3 }); // => [1, 2, 3]
+ addResults(query::entity::IndexRange{ query::QueryTarget::WMAndLTM, 1, -2 }); // 5 - 2 = 3
+ addResults(query::entity::IndexRange{ query::QueryTarget::WMAndLTM, -4, 3 }); // 5 - 4 = 1
+ addResults(query::entity::IndexRange{ query::QueryTarget::WMAndLTM, -4, -2 });
BOOST_REQUIRE_GT(results.size(), 0);
for (const armem::wm::Entity& result : results)
@@ -679,12 +679,12 @@ BOOST_AUTO_TEST_CASE(test_entity_IndexRange_slice)
BOOST_AUTO_TEST_CASE(test_entity_IndexRange_empty_range)
{
BOOST_REQUIRE_EQUAL(entity.size(), 5);
- addResults(query::entity::IndexRange{ 1, 0 });
- addResults(query::entity::IndexRange{ 2, 1 });
- addResults(query::entity::IndexRange{ 5, 3 });
- addResults(query::entity::IndexRange{ 4, -3 }); // 5-3 = 2
- addResults(query::entity::IndexRange{ 3, -3 });
- addResults(query::entity::IndexRange{ 1, -5 });
+ addResults(query::entity::IndexRange{ query::QueryTarget::WMAndLTM, 1, 0 });
+ addResults(query::entity::IndexRange{ query::QueryTarget::WMAndLTM, 2, 1 });
+ addResults(query::entity::IndexRange{ query::QueryTarget::WMAndLTM, 5, 3 });
+ addResults(query::entity::IndexRange{ query::QueryTarget::WMAndLTM, 4, -3 }); // 5-3 = 2
+ addResults(query::entity::IndexRange{ query::QueryTarget::WMAndLTM, 3, -3 });
+ addResults(query::entity::IndexRange{ query::QueryTarget::WMAndLTM, 1, -5 });
BOOST_REQUIRE_GT(results.size(), 0);
@@ -701,12 +701,12 @@ BOOST_AUTO_TEST_CASE(test_entity_IndexRange_empty_entity)
{
entity.clear();
BOOST_REQUIRE_EQUAL(entity.size(), 0);
- addResults(query::entity::IndexRange{ 0, 0 });
- addResults(query::entity::IndexRange{ 0, 10 });
- addResults(query::entity::IndexRange{-10, -1 });
- addResults(query::entity::IndexRange{ 2, 5 });
- addResults(query::entity::IndexRange{ 3, -3 });
- addResults(query::entity::IndexRange{ -1, 10 });
+ addResults(query::entity::IndexRange{ query::QueryTarget::WMAndLTM, 0, 0 });
+ addResults(query::entity::IndexRange{ query::QueryTarget::WMAndLTM, 0, 10 });
+ addResults(query::entity::IndexRange{query::QueryTarget::WMAndLTM, -10, -1 });
+ addResults(query::entity::IndexRange{ query::QueryTarget::WMAndLTM, 2, 5 });
+ addResults(query::entity::IndexRange{ query::QueryTarget::WMAndLTM, 3, -3 });
+ addResults(query::entity::IndexRange{ query::QueryTarget::WMAndLTM, -1, 10 });
BOOST_REQUIRE_GT(results.size(), 0);
diff --git a/source/RobotAPI/libraries/armem/util/util.h b/source/RobotAPI/libraries/armem/util/util.h
index 16da9f1709528dc7b9c7d484be494a2f1e6da851..f160efcd49b3f3ddc3ec93aa98b07e5ff80c8dc6 100644
--- a/source/RobotAPI/libraries/armem/util/util.h
+++ b/source/RobotAPI/libraries/armem/util/util.h
@@ -43,7 +43,7 @@ namespace armarx::armem
template <typename AronClass>
std::optional<AronClass> tryCast(const wm::EntityInstance& item)
{
- static_assert(std::is_base_of<armarx::aron::cppcodegenerator::AronCppClass,
+ static_assert(std::is_base_of<armarx::aron::cppserializer::AronCppClass,
AronClass>::value);
try
@@ -69,7 +69,7 @@ namespace armarx::armem
std::vector<AronClass>
allOfType(const std::map<std::string, wm::Entity>& entities)
{
- static_assert(std::is_base_of<armarx::aron::cppcodegenerator::AronCppClass,
+ static_assert(std::is_base_of<armarx::aron::cppserializer::AronCppClass,
AronClass>::value);
std::vector<AronClass> outV;
@@ -116,7 +116,7 @@ namespace armarx::armem
auto transformAllOfType(const std::map<std::string, wm::Entity>& entities,
auto pred) -> std::vector<decltype(pred(AronClass()))>
{
- static_assert(std::is_base_of<armarx::aron::cppcodegenerator::AronCppClass,
+ static_assert(std::is_base_of<armarx::aron::cppserializer::AronCppClass,
AronClass>::value);
std::vector<decltype(pred(AronClass()))> outV;
diff --git a/source/RobotAPI/libraries/armem_gui/CMakeLists.txt b/source/RobotAPI/libraries/armem_gui/CMakeLists.txt
index d90a90aa04a3fcc2c3d891bfab8727a991522f44..696c6db777d8512d49168682c8a368243f2b930d 100644
--- a/source/RobotAPI/libraries/armem_gui/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem_gui/CMakeLists.txt
@@ -14,16 +14,19 @@ set(LIBRARIES
)
set(SOURCES
- MemoryViewer.cpp
- PeriodicUpdateWidget.cpp
- LTMControlWidget.cpp
gui_utils.cpp
lifecycle.cpp
+ MemoryControlWidget.cpp
+ MemoryViewer.cpp
+ PeriodicUpdateWidget.cpp
+
instance/GroupBox.cpp
instance/ImageView.cpp
instance/InstanceView.cpp
+ instance/MemoryIDTreeWidgetItem.cpp
+ instance/WidgetsWithToolbar.cpp
instance/sanitize_typename.cpp
instance/serialize_path.cpp
@@ -46,17 +49,23 @@ set(SOURCES
query_widgets/SnapshotSelectorWidget.cpp
)
set(HEADERS
+
+ gui_utils.h
+ lifecycle.h
+
+ MemoryControlWidget.h
MemoryViewer.h
+ PeriodicUpdateWidget.h
+ TreeWidgetBuilder.h
PeriodicUpdateWidget.h
- LTMControlWidget.h
TreeWidgetBuilder.h
- gui_utils.h
- lifecycle.h
instance/GroupBox.h
instance/ImageView.h
instance/InstanceView.h
+ instance/MemoryIDTreeWidgetItem.h
+ instance/WidgetsWithToolbar.h
instance/sanitize_typename.h
instance/serialize_path.h
diff --git a/source/RobotAPI/libraries/armem_gui/LTMControlWidget.cpp b/source/RobotAPI/libraries/armem_gui/MemoryControlWidget.cpp
similarity index 79%
rename from source/RobotAPI/libraries/armem_gui/LTMControlWidget.cpp
rename to source/RobotAPI/libraries/armem_gui/MemoryControlWidget.cpp
index 257e43d3a3f0df224b4632449c97aa4336c9f184..c68673377563e83019640755699187ebee39a418 100644
--- a/source/RobotAPI/libraries/armem_gui/LTMControlWidget.cpp
+++ b/source/RobotAPI/libraries/armem_gui/MemoryControlWidget.cpp
@@ -1,4 +1,4 @@
-#include "LTMControlWidget.h"
+#include "MemoryControlWidget.h"
#include <QPushButton>
#include <QLineEdit>
@@ -11,7 +11,7 @@
namespace armarx::armem::gui
{
- LTMControlWidget::LTMControlWidget()
+ MemoryControlWidget::MemoryControlWidget()
{
setSizePolicy(QSizePolicy::Policy::Minimum, QSizePolicy::Policy::Fixed);
@@ -27,8 +27,8 @@ namespace armarx::armem::gui
_storeButton = new QPushButton("Store in LTM", this);
layout->addWidget(_lineEdit);
- layout->addWidget(_storeButton);
layout->addWidget(_exportHereButton);
+ layout->addWidget(_storeButton);
// Private connections.
@@ -37,22 +37,22 @@ namespace armarx::armem::gui
connect(_exportHereButton, &QPushButton::pressed, this, &This::exportHere);
}
- QLineEdit* LTMControlWidget::pathInputBox()
+ QLineEdit* MemoryControlWidget::pathInputBox()
{
return _lineEdit;
}
- QString LTMControlWidget::getEnteredPath()
+ QString MemoryControlWidget::getEnteredPath()
{
return _lineEdit->text();
}
- QPushButton* LTMControlWidget::storeButton()
+ QPushButton* MemoryControlWidget::storeButton()
{
return _storeButton;
}
- QPushButton* LTMControlWidget::exportHereButton()
+ QPushButton* MemoryControlWidget::exportHereButton()
{
return _exportHereButton;
}
diff --git a/source/RobotAPI/libraries/armem_gui/LTMControlWidget.h b/source/RobotAPI/libraries/armem_gui/MemoryControlWidget.h
similarity index 71%
rename from source/RobotAPI/libraries/armem_gui/LTMControlWidget.h
rename to source/RobotAPI/libraries/armem_gui/MemoryControlWidget.h
index 9c827b01173aa7b724fcfe7e700691d5a093e36e..4f8acc62ac9c8918f887703d9188e76cfc9f8584 100644
--- a/source/RobotAPI/libraries/armem_gui/LTMControlWidget.h
+++ b/source/RobotAPI/libraries/armem_gui/MemoryControlWidget.h
@@ -8,19 +8,20 @@ class QLineEdit;
namespace armarx::armem::gui
{
- class LTMControlWidget : public QWidget
+ class MemoryControlWidget : public QWidget
{
Q_OBJECT
- using This = LTMControlWidget;
+ using This = MemoryControlWidget;
public:
- LTMControlWidget();
+ MemoryControlWidget();
QLineEdit* pathInputBox();
QString getEnteredPath();
QPushButton* storeButton();
+ QPushButton* loadButton();
QPushButton* exportHereButton();
public slots:
@@ -28,6 +29,7 @@ namespace armarx::armem::gui
signals:
void store();
+ void load();
void exportHere();
private slots:
@@ -41,6 +43,7 @@ namespace armarx::armem::gui
QLineEdit* _lineEdit;
QPushButton* _storeButton;
+ QPushButton* _loadButton;
QPushButton* _exportHereButton;
};
diff --git a/source/RobotAPI/libraries/armem_gui/MemoryViewer.cpp b/source/RobotAPI/libraries/armem_gui/MemoryViewer.cpp
index 447b35332a60a3442b63e07a2c5e129b5b7b83d1..6d711ac1dfee696b069a0408a001f5d98a08f2cd 100644
--- a/source/RobotAPI/libraries/armem_gui/MemoryViewer.cpp
+++ b/source/RobotAPI/libraries/armem_gui/MemoryViewer.cpp
@@ -14,9 +14,10 @@
#include <SimoxUtility/algorithm/get_map_keys_values.h>
#include <QBoxLayout>
-#include <QDialog>
#include <QCheckBox>
+#include <QDialog>
#include <QGroupBox>
+#include <QMenu>
#include <QLabel>
#include <QLayout>
#include <QSettings>
@@ -28,7 +29,7 @@ namespace armarx::armem::gui
{
MemoryViewer::MemoryViewer(
QBoxLayout* updateWidgetLayout,
- QBoxLayout* ltmControlWidgetLayout,
+ QBoxLayout* memoryControlWidgetLayout,
QGroupBox* memoryGroupBox, QLayout* memoryGroupBoxParentLayout,
QGroupBox* instanceGroupBox, QLayout* instanceGroupBoxParentLayout,
QLabel* statusLabel
@@ -39,17 +40,29 @@ namespace armarx::armem::gui
this->statusLabel = statusLabel;
this->statusLabel->clear();
+ statusLabel->setContextMenuPolicy(Qt::CustomContextMenu);
+ connect(statusLabel, &QLabel::customContextMenuRequested, [statusLabel](const QPoint & pos)
+ {
+ QMenu menu(statusLabel);
+ menu.addAction("Clear status", [statusLabel]()
+ {
+ statusLabel->clear();
+ });
+ menu.exec(statusLabel->mapToGlobal(pos));
+ });
+
+
// Update timer
this->updateWidgetLayout = updateWidgetLayout;
updateWidget = new armem::gui::PeriodicUpdateWidget(2.0, 60);
updateWidgetLayout->insertWidget(0, updateWidget);
// LTM Control
- if (ltmControlWidgetLayout)
+ if (memoryControlWidgetLayout)
{
- this->ltmControlWidgetLayout = ltmControlWidgetLayout;
- ltmControlWidget = new armem::gui::LTMControlWidget();
- ltmControlWidgetLayout->addWidget(ltmControlWidget);
+ this->memoryControlWidgetLayout = memoryControlWidgetLayout;
+ memoryControlWidget = new armem::gui::MemoryControlWidget();
+ memoryControlWidgetLayout->addWidget(memoryControlWidget);
}
// Memory View
@@ -66,8 +79,8 @@ namespace armarx::armem::gui
// Connections
//connect(this, &This::connected, this, &This::updateMemory);
- connect(ltmControlWidget, &armem::gui::LTMControlWidget::store, this, &This::store);
- connect(ltmControlWidget, &armem::gui::LTMControlWidget::exportHere, this, &This::exportHere);
+ connect(memoryControlWidget, &armem::gui::MemoryControlWidget::exportHere, this, &This::exportHere);
+ connect(memoryControlWidget, &armem::gui::MemoryControlWidget::store, this, &This::store);
connect(this, &This::connected, this, &This::updateMemories);
connect(updateWidget, &armem::gui::PeriodicUpdateWidget::update, this, &This::updateMemories);
@@ -77,6 +90,7 @@ namespace armarx::armem::gui
connect(memoryGroup->tree(), &armem::gui::MemoryTreeWidget::updated, this, &This::memoryTreeUpdated);
connect(instanceGroup, &armem::gui::InstanceGroupBox::viewUpdated, this, &This::instanceTreeUpdated);
+ connect(instanceGroup->view, &armem::gui::InstanceView::memoryIdResolutionRequested, this, &This::resolveMemoryID);
}
void MemoryViewer::setLogTag(const std::string& tag)
@@ -102,13 +116,12 @@ namespace armarx::armem::gui
{
if (!mnsName.empty())
{
- component.getProxy(mns, mnsName);
- auto res = mns->getAllRegisteredMemories();
- for (auto& [name, proxy] : res.proxies)
- {
- armem::client::Reader memoryReader{proxy};
- memoryReaders[name] = memoryReader;
- }
+ armem::mns::MemoryNameSystemInterfacePrx mnsProxy;
+ component.getProxy(mnsProxy, mnsName);
+ mns = client::MemoryNameSystem(mnsProxy);
+
+ const bool update = true;
+ memoryReaders = mns.getAllReaders(update);
}
// DebugObserver is optional (check for null on every call)
if (!debugObserverName.empty())
@@ -127,6 +140,27 @@ namespace armarx::armem::gui
emit disconnected();
}
+ const armem::wm::Memory* MemoryViewer::getSingleMemoryData(const std::string& memoryName)
+ {
+ auto it = memoryData.find(memoryName);
+ if (it == memoryData.end())
+ {
+ std::stringstream ss;
+ ss << "Memory name '" << memoryName << "' is unknown. Known are: "
+ << simox::alg::get_keys(memoryData);
+ statusLabel->setText(QString::fromStdString(ss.str()));
+ return nullptr;
+ }
+ else if (not it->second.has_value())
+ {
+ return nullptr;
+ }
+ else
+ {
+ return &it->second.value();
+ }
+ }
+
void MemoryViewer::store()
{
TIMING_START(MemoryStore);
@@ -144,7 +178,7 @@ namespace armarx::armem::gui
void MemoryViewer::exportHere()
{
TIMING_START(MemoryExport);
- QString qs = ltmControlWidget->getEnteredPath();
+ QString qs = memoryControlWidget->getEnteredPath();
std::string utf8_text = qs.toUtf8().constData();
ARMARX_IMPORTANT << "Exporting all memories at '" << utf8_text << "'.";
@@ -177,11 +211,7 @@ namespace armarx::armem::gui
memoryReaders.clear();
memoryData.clear();
- for (auto& [name, proxy] : mns->getAllRegisteredMemories().proxies)
- {
- armem::client::Reader memoryReader{proxy};
- memoryReaders[name] = memoryReader;
- }
+ memoryReaders = mns.getAllReaders(true);
bool dataChanged = false;
@@ -232,17 +262,7 @@ namespace armarx::armem::gui
void MemoryViewer::updateInstanceTree(const armem::MemoryID& selectedID)
{
- if (memoryData.find(selectedID.memoryName) == memoryData.end())
- {
- std::stringstream ss;
- ss << "Memory name '" << selectedID.memoryName << "' is unknown. Known are: "
- << simox::alg::get_keys(memoryData);
- statusLabel->setText(QString::fromStdString(ss.str()));
- return;
- }
-
- const std::optional<armem::wm::Memory>& data = memoryData.at(selectedID.memoryName);
-
+ const armem::wm::Memory* data = getSingleMemoryData(selectedID.memoryName);
if (data)
{
if (!selectedID.hasEntityName())
@@ -289,6 +309,74 @@ namespace armarx::armem::gui
}
}
+ void MemoryViewer::resolveMemoryID(const MemoryID& id)
+ {
+ // ARMARX_IMPORTANT << "Resolving memory ID: " << id;
+
+ auto handleError = [this](const std::string & msg)
+ {
+ statusLabel->setText(QString::fromStdString(msg));
+ ARMARX_WARNING << msg;
+ };
+
+ if (id.memoryName.empty())
+ {
+ handleError("Memory name is empty.");
+ }
+
+ aron::typenavigator::ObjectNavigatorPtr segmentType;
+ std::optional<wm::EntityInstance> instance;
+ try
+ {
+ const wm::Memory* data = getSingleMemoryData(id.memoryName);
+ if (data)
+ {
+ segmentType = data->getProviderSegment(id).aronType();
+
+ if (id.hasInstanceIndex())
+ {
+ instance = data->getEntityInstance(id);
+ }
+ else if (id.hasTimestamp())
+ {
+ instance = data->getEntitySnapshot(id).getInstance(0);
+ }
+ else
+ {
+ instance = data->getEntity(id).getLatestSnapshot().getInstance(0);
+ }
+ }
+ }
+ catch (const armem::error::ArMemError& e)
+ {
+ // May be handled by remote lookup
+ (void) e;
+ }
+
+ if (!instance)
+ {
+ try
+ {
+ // Resolve remotely (may still fail, returns an optional).
+ instance = mns.resolveEntityInstance(id);
+ }
+ catch (const armem::error::ArMemError& e)
+ {
+ ARMARX_WARNING << e.what();
+ statusLabel->setText(e.what());
+ }
+ }
+
+ if (instance)
+ {
+ instanceGroup->view->addInstanceView(*instance, segmentType);
+ }
+ else
+ {
+ // ToDo: Propagate error back to highlight selected entry in red
+ }
+ }
+
void MemoryViewer::updateMemoryTree()
{
std::map<std::string, const armem::wm::Memory*> convMap;
diff --git a/source/RobotAPI/libraries/armem_gui/MemoryViewer.h b/source/RobotAPI/libraries/armem_gui/MemoryViewer.h
index 19ab5000d9d662080b80e621144650fd887a0bcd..22455503637f55114516bf6e93c6772cd18c179e 100644
--- a/source/RobotAPI/libraries/armem_gui/MemoryViewer.h
+++ b/source/RobotAPI/libraries/armem_gui/MemoryViewer.h
@@ -8,12 +8,14 @@
#include <ArmarXCore/core/logging/Logging.h>
#include <RobotAPI/interface/armem/mns/MemoryNameSystemInterface.h>
+#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
#include <RobotAPI/libraries/armem/client/Reader.h>
#include <RobotAPI/libraries/armem_gui/lifecycle.h>
#include <RobotAPI/libraries/armem_gui/instance/GroupBox.h>
#include <RobotAPI/libraries/armem_gui/memory/GroupBox.h>
#include <RobotAPI/libraries/armem_gui/PeriodicUpdateWidget.h>
-#include <RobotAPI/libraries/armem_gui/LTMControlWidget.h>
+#include <RobotAPI/libraries/armem_gui/MemoryControlWidget.h>
+
class QBoxLayout;
class QDialog;
@@ -63,14 +65,15 @@ namespace armarx::armem::gui
public slots:
+ void updateMemories();
+ void updateInstanceTree(const armem::MemoryID& selectedID);
+
+ void resolveMemoryID(const MemoryID& id);
+
// LTMControlWidget
void store();
void exportHere();
- // Other
- void updateMemories();
- void updateInstanceTree(const armem::MemoryID& selectedID);
-
signals:
@@ -97,11 +100,13 @@ namespace armarx::armem::gui
void onConnect(ManagedIceObject& component);
void onDisconnect(ManagedIceObject& component);
+ const armem::wm::Memory* getSingleMemoryData(const std::string& memoryName);
+
public:
std::string mnsName;
- armem::mns::MemoryNameSystemInterfacePrx mns;
+ armem::client::MemoryNameSystem mns;
std::map<std::string, armem::client::Reader> memoryReaders;
std::map<std::string, std::optional<armem::wm::Memory>> memoryData;
@@ -109,8 +114,8 @@ namespace armarx::armem::gui
QLayout* updateWidgetLayout = nullptr;
armem::gui::PeriodicUpdateWidget* updateWidget = nullptr;
- QLayout* ltmControlWidgetLayout = nullptr;
- armem::gui::LTMControlWidget* ltmControlWidget = nullptr;
+ QLayout* memoryControlWidgetLayout = nullptr;
+ armem::gui::MemoryControlWidget* memoryControlWidget = nullptr;
armem::gui::MemoryGroupBox* memoryGroup = nullptr;
diff --git a/source/RobotAPI/libraries/armem_gui/instance/GroupBox.cpp b/source/RobotAPI/libraries/armem_gui/instance/GroupBox.cpp
index b33c381970dfe3f3eb0ce0b0b7be9e9599f75f7f..ff828d4437f042c7857332ec3957f9b5f55c60a5 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/GroupBox.cpp
+++ b/source/RobotAPI/libraries/armem_gui/instance/GroupBox.cpp
@@ -19,10 +19,11 @@ namespace armarx::armem::gui::instance
useTypeInfoCheckBox->setChecked(true);
QHBoxLayout* checkBoxLayout = new QHBoxLayout();
+ checkBoxLayout->setDirection(QBoxLayout::RightToLeft);
checkBoxLayout->addWidget(useTypeInfoCheckBox);
- layout->addLayout(checkBoxLayout);
layout->addWidget(view);
+ layout->addLayout(checkBoxLayout);
this->setTitle("Instance View (select an entity instance on the left)");
const int margin = 3;
diff --git a/source/RobotAPI/libraries/armem_gui/instance/InstanceView.cpp b/source/RobotAPI/libraries/armem_gui/instance/InstanceView.cpp
index 429d2f3603967f64728e59113841b85c862b8d46..2b4ab121dabc7d52188f1d10b8adb501f2c72d32 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/InstanceView.cpp
+++ b/source/RobotAPI/libraries/armem_gui/instance/InstanceView.cpp
@@ -1,6 +1,8 @@
#include "InstanceView.h"
#include <QAction>
+#include <QApplication>
+#include <QClipboard>
#include <QGroupBox>
#include <QHBoxLayout>
#include <QHeaderView>
@@ -19,18 +21,27 @@
#include <RobotAPI/libraries/aron/core/navigator/data/complex/NDArray.h>
#include <RobotAPI/libraries/aron/core/navigator/type/container/Object.h>
+#include <RobotAPI/libraries/armem/aron/MemoryID.aron.generated.h>
+#include <RobotAPI/libraries/armem/core/aron_conversions.h>
+
#include <RobotAPI/libraries/armem_gui/gui_utils.h>
#include <RobotAPI/libraries/armem_gui/instance/ImageView.h>
+#include <RobotAPI/libraries/armem_gui/instance/sanitize_typename.h>
#include <RobotAPI/libraries/armem_gui/instance/serialize_path.h>
#include <RobotAPI/libraries/armem_gui/instance/tree_builders/DataTreeBuilder.h>
#include <RobotAPI/libraries/armem_gui/instance/tree_builders/TypedDataTreeBuilder.h>
+#include "MemoryIDTreeWidgetItem.h"
+#include "WidgetsWithToolbar.h"
+
namespace armarx::armem::gui::instance
{
InstanceView::InstanceView()
{
+ Logging::setTag("InstanceView");
+
QLayout* layout = new QVBoxLayout();
this->setLayout(layout);
int margin = 3;
@@ -50,15 +61,10 @@ namespace armarx::armem::gui::instance
tree->setHeaderLabels(columns);
tree->header()->resizeSection(int(Columns::KEY), 250);
- tree->header()->resizeSection(int(Columns::VALUE), 150);
+ tree->header()->resizeSection(int(Columns::VALUE), 200);
- treeItemInstanceID = new QTreeWidgetItem({"Instance ID"});
- treeItemInstanceID->addChild(new QTreeWidgetItem({"Memory Name"}));
- treeItemInstanceID->addChild(new QTreeWidgetItem({"Core Segment Name"}));
- treeItemInstanceID->addChild(new QTreeWidgetItem({"Provider Segment Name"}));
- treeItemInstanceID->addChild(new QTreeWidgetItem({"Entity Name"}));
- treeItemInstanceID->addChild(new QTreeWidgetItem({"Timestamp"}));
- treeItemInstanceID->addChild(new QTreeWidgetItem({"Instance Index"}));
+ treeItemInstanceID = new MemoryIDTreeWidgetItem({"Instance ID"});
+ treeItemInstanceID->addKeyChildren();
treeItemMetadata = new QTreeWidgetItem({"Metadata"});
treeItemMetadata->addChild(new QTreeWidgetItem({"Confidence"}));
@@ -74,6 +80,7 @@ namespace armarx::armem::gui::instance
{
item->setExpanded(true);
}
+ treeItemMetadata->setExpanded(false);
tree->setContextMenuPolicy(Qt::CustomContextMenu);
connect(tree, &QTreeWidget::customContextMenuRequested, this, &This::prepareTreeContextMenu);
@@ -100,7 +107,7 @@ namespace armarx::armem::gui::instance
instance = &memory.getEntityInstance(id);
if (useTypeInfo)
{
- aronType = memory.getCoreSegment(id.coreSegmentName).getProviderSegment(id.providerSegmentName).aronType();
+ aronType = memory.getProviderSegment(id).aronType();
}
}
catch (const armem::error::ArMemError& e)
@@ -122,6 +129,7 @@ namespace armarx::armem::gui::instance
update();
}
+
void InstanceView::update()
{
if (currentInstance)
@@ -136,18 +144,34 @@ namespace armarx::armem::gui::instance
}
+ void InstanceView::addInstanceView(const wm::EntityInstance& instance, aron::typenavigator::ObjectNavigatorPtr aronType)
+ {
+ ARMARX_IMPORTANT << "Adding instance view with toolbar for instance: " << instance.id();
+ InstanceView* view = new InstanceView;
+ view->setStatusLabel(statusLabel);
+ view->setUseTypeInfo(useTypeInfo);
+
+ WidgetsWithToolbar* child = new WidgetsWithToolbar();
+ child->addWidget(view);
+
+
+ splitter->addWidget(child);
+
+ // Propagate this signal upwards.
+ connect(view, &InstanceView::memoryIdResolutionRequested, this, &This::memoryIdResolutionRequested);
+
+ view->update(instance, aronType);
+ }
+
+
void InstanceView::updateInstanceID(const MemoryID& id)
{
- const std::vector<std::string> items = id.getAllItems();
- ARMARX_CHECK_EQUAL(treeItemInstanceID->childCount(), static_cast<int>(items.size()));
- int i = 0;
- for (const std::string& item : items)
- {
- treeItemInstanceID->child(i++)->setText(int(Columns::VALUE), QString::fromStdString(item));
- }
+ treeItemInstanceID->setInstanceID(id, int(Columns::VALUE));
}
- void InstanceView::updateData(const aron::datanavigator::DictNavigatorPtr& data, aron::typenavigator::ObjectNavigatorPtr aronType)
+
+ void InstanceView::updateData(
+ const aron::datanavigator::DictNavigatorPtr& data, aron::typenavigator::ObjectNavigatorPtr aronType)
{
if (!data)
{
@@ -157,6 +181,8 @@ namespace armarx::armem::gui::instance
}
else if (useTypeInfo && aronType)
{
+ treeItemData->setText(int(Columns::TYPE), QString::fromStdString(sanitizeTypeName(aronType->getName())));
+
TypedDataTreeBuilder builder;
builder.setColumns(int(Columns::KEY), int(Columns::VALUE), int(Columns::TYPE));
builder.updateTree(treeItemData, *aronType, *data);
@@ -165,7 +191,7 @@ namespace armarx::armem::gui::instance
{
DataTreeBuilder builder;
builder.setColumns(int(Columns::KEY), int(Columns::VALUE), int(Columns::TYPE));
- builder.updateTree(treeItemData, *data);
+ builder.updateTree(treeItemData, data);
}
treeItemData->setExpanded(true);
}
@@ -196,24 +222,43 @@ namespace armarx::armem::gui::instance
}
+ aron::Path InstanceView::getElementPath(const QTreeWidgetItem* item) const
+ {
+ QStringList qpath = item->data(int(Columns::KEY), Qt::UserRole).toStringList();
+ aron::Path path = deserializePath(qpath);
+ return path;
+ }
+
+
void InstanceView::prepareTreeContextMenu(const QPoint& pos)
{
- QTreeWidgetItem* item = tree->itemAt(pos);
- if (item == nullptr || item->parent() == nullptr)
+ const QTreeWidgetItem* item = tree->itemAt(pos);
+ if (item == nullptr)
{
- // No item or top level item => no context menu.
- return;
+ return; // No item => no context menu.
}
QMenu menu(this);
+ if (item == this->treeItemInstanceID && currentInstance.has_value())
+ {
+ if (QAction* action = makeActionCopyMemoryID(currentInstance->id()))
+ {
+ menu.addAction(action);
+ }
+ }
+ else if (item->parent() == nullptr)
+ {
+ return; // Other top level item => no context menu.
+ }
+
+ // Type descriptor based actions
aron::type::Descriptor type = static_cast<aron::type::Descriptor>(item->data(int(Columns::TYPE), Qt::UserRole).toInt());
switch (type)
{
case aron::type::Descriptor::eIVTCByteImage:
{
- QStringList qpath = item->data(int(Columns::KEY), Qt::UserRole).toStringList();
- aron::Path path = deserializePath(qpath);
+ const aron::Path path = getElementPath(item);
QAction* viewAction = new QAction("Show image");
menu.addAction(viewAction);
@@ -224,12 +269,118 @@ namespace armarx::armem::gui::instance
}
break;
default:
- return;
+ break;
+ }
+
+ // Type name based actions
+ const std::string typeName = item->text(int(Columns::TYPE)).toStdString();
+ if (typeName == instance::sanitizedMemoryIDTypeName)
+ {
+ const aron::Path path = getElementPath(item);
+
+ if (std::optional<MemoryID> id = getElementMemoryID(path))
+ {
+ if (QAction* action = makeActionCopyMemoryID(id.value()))
+ {
+ menu.addAction(action);
+ }
+ if (QAction* action = makeActionResolveMemoryID(id.value()))
+ {
+ menu.addAction(action);
+ }
+ }
+ }
+
+
+ if (menu.actions().size() > 0)
+ {
+ menu.exec(tree->mapToGlobal(pos));
+ }
+ }
+
+
+ std::optional<MemoryID> InstanceView::getElementMemoryID(const aron::Path& elementPath)
+ {
+ aron::datanavigator::NavigatorPtr element;
+ try
+ {
+ element = currentInstance->data()->navigateAbsolute(elementPath);
+ }
+ // This can happen when the underlying entity structure changes (a new entity has been selected).
+ catch (const aron::error::AronException&)
+ {
+ // showErrorMessage(e.what());
+ return std::nullopt;
+ }
+ catch (const armarx::LocalException& e)
+ {
+ showErrorMessage(e.what());
+ return std::nullopt;
+ }
+
+ std::stringstream couldNotParseMsg;
+ couldNotParseMsg << "Element " << elementPath.toString() << " could not be parsed as MemoryID.";
+
+ auto dictElement = std::dynamic_pointer_cast<aron::datanavigator::DictNavigator>(element);
+ if (!dictElement)
+ {
+ showErrorMessage(couldNotParseMsg.str() + " (Failed to cast to DictNavigator.)");
+ return std::nullopt;
}
- menu.exec(tree->mapToGlobal(pos));
+ try
+ {
+ arondto::MemoryID dto;
+ dto.fromAron(dictElement);
+
+ MemoryID id;
+ armem::fromAron(dto, id);
+ return id;
+ }
+ catch (const armarx::aron::error::AronException&)
+ {
+ showErrorMessage(couldNotParseMsg.str());
+ return std::nullopt;
+ }
}
+
+ QAction* InstanceView::makeActionResolveMemoryID(const MemoryID& id)
+ {
+ QAction* action = new QAction("Resolve memory ID");
+
+ if (not(id.hasEntityName() and id.isWellDefined()))
+ {
+ action->setDisabled(true);
+ action->setText(action->text() + " (incomplete Memory ID)");
+ }
+ connect(action, &QAction::triggered, [this, id]()
+ {
+ // ARMARX_IMPORTANT << "emit memoryIdResolutionRequested(id = " << id << ")";
+ emit memoryIdResolutionRequested(id);
+ });
+
+ return action;
+ }
+
+ QAction* InstanceView::makeActionCopyMemoryID(const MemoryID& id)
+ {
+ QAction* action = new QAction("Copy memory ID to clipboard");
+
+ connect(action, &QAction::triggered, [/*this,*/ id]() // `this` for ARMARX_IMPORTANT
+ {
+ const QString idStr = QString::fromStdString(id.str());
+
+ // ARMARX_IMPORTANT << "Copy '" << idStr.toStdString() << "' to clipboard.";
+ QClipboard* clipboard = QApplication::clipboard();
+ clipboard->setText(idStr);
+ QApplication::processEvents();
+ });
+
+ return action;
+ }
+
+
void InstanceView::showImageView(const aron::Path& elementPath)
{
if (!currentInstance)
@@ -238,7 +389,18 @@ namespace armarx::armem::gui::instance
}
if (!imageView)
{
- imageView = ImageView(splitter);
+ WidgetsWithToolbar* toolbar = new WidgetsWithToolbar();
+
+ imageView = new ImageView();
+ imageView->toolbar = toolbar;
+ toolbar->addWidget(imageView);
+
+ splitter->addWidget(toolbar);
+
+ connect(toolbar, &WidgetsWithToolbar::closing, [this]()
+ {
+ imageView = nullptr;
+ });
}
imageView->elementPath = elementPath;
updateImageView(currentInstance->data());
@@ -246,11 +408,11 @@ namespace armarx::armem::gui::instance
void InstanceView::removeImageView()
{
- imageView->group->hide();
- imageView->group->deleteLater();
- imageView = std::nullopt;
+ imageView->toolbar->close();
+ imageView = nullptr;
}
+
void InstanceView::updateImageView(const aron::datanavigator::DictNavigatorPtr& data)
{
using aron::datanavigator::NDArrayNavigator;
@@ -317,28 +479,25 @@ namespace armarx::armem::gui::instance
std::stringstream title;
title << "Image element '" << imageView->elementPath.toString() << "'"; // of entity instance " << currentInstance->id();
- imageView->group->setTitle(QString::fromStdString(title.str()));
+ imageView->setTitle(QString::fromStdString(title.str()));
imageView->view->setImage(QImage(imageData->getData(), shape.at(0), shape.at(1), format));
}
- InstanceView::ImageView::ImageView(QSplitter* parent)
+ InstanceView::ImageView::ImageView()
{
- group = new QGroupBox();
- parent->addWidget(group);
-
- group->setLayout(new QHBoxLayout());
+ setLayout(new QHBoxLayout());
int margin = 2;
- group->layout()->setContentsMargins(margin, margin, margin, margin);
+ layout()->setContentsMargins(margin, margin, margin, margin);
if (false)
{
- QFont font = group->font();
+ QFont font = this->font();
font.setPointSizeF(font.pointSize() * 0.75);
- group->setFont(font);
+ setFont(font);
}
view = new instance::ImageView();
- group->layout()->addWidget(view);
+ layout()->addWidget(view);
}
}
diff --git a/source/RobotAPI/libraries/armem_gui/instance/InstanceView.h b/source/RobotAPI/libraries/armem_gui/instance/InstanceView.h
index 0b0a0ee2655f506fa09f15b99dd0edc149e88470..26f9d3be5fccfdce4fdb17a9ddbe430c2f334433 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/InstanceView.h
+++ b/source/RobotAPI/libraries/armem_gui/instance/InstanceView.h
@@ -3,6 +3,7 @@
#include <optional>
#include <QWidget>
+#include <QGroupBox>
#include <RobotAPI/libraries/aron/core/navigator/type/container/Object.h>
@@ -19,9 +20,11 @@ class QTreeWidgetItem;
namespace armarx::armem::gui::instance
{
class ImageView;
+ class MemoryIDTreeWidgetItem;
+ class WidgetsWithToolbar;
- class InstanceView : public QWidget
+ class InstanceView : public QWidget, public Logging
{
Q_OBJECT
using This = InstanceView;
@@ -38,16 +41,20 @@ namespace armarx::armem::gui::instance
void update(const wm::EntityInstance& instance, aron::typenavigator::ObjectNavigatorPtr aronType = nullptr);
void update();
+ void addInstanceView(const wm::EntityInstance& instance, aron::typenavigator::ObjectNavigatorPtr aronType = nullptr);
+
signals:
void updated();
void instanceSelected(const MemoryID& id);
+ void memoryIdResolutionRequested(const MemoryID& id);
private slots:
void prepareTreeContextMenu(const QPoint& pos);
+
void showImageView(const aron::Path& elementPath);
void removeImageView();
@@ -61,6 +68,12 @@ namespace armarx::armem::gui::instance
void showErrorMessage(const std::string& message);
+ aron::Path getElementPath(const QTreeWidgetItem* item) const;
+ std::optional<MemoryID> getElementMemoryID(const aron::Path& elementPath);
+
+ QAction* makeActionResolveMemoryID(const MemoryID& id);
+ QAction* makeActionCopyMemoryID(const MemoryID& id);
+
private:
@@ -78,21 +91,22 @@ namespace armarx::armem::gui::instance
QSplitter* splitter;
QTreeWidget* tree;
- QTreeWidgetItem* treeItemInstanceID;
+ MemoryIDTreeWidgetItem* treeItemInstanceID;
QTreeWidgetItem* treeItemMetadata;
QTreeWidgetItem* treeItemData;
- class ImageView
+ class ImageView : public QGroupBox
{
public:
- ImageView(QSplitter* parent);
+ ImageView();
- QGroupBox* group;
instance::ImageView* view;
aron::Path elementPath;
+
+ WidgetsWithToolbar* toolbar;
};
- std::optional<ImageView> imageView;
+ ImageView* imageView = nullptr;
QLabel* statusLabel = nullptr;
diff --git a/source/RobotAPI/libraries/armem_gui/instance/MemoryIDTreeWidgetItem.cpp b/source/RobotAPI/libraries/armem_gui/instance/MemoryIDTreeWidgetItem.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..b8f568c79cdd1d84fa803df9ffaee737e24c9ea2
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_gui/instance/MemoryIDTreeWidgetItem.cpp
@@ -0,0 +1,41 @@
+#include "MemoryIDTreeWidgetItem.h"
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+#include <RobotAPI/libraries/armem/core/MemoryID.h>
+
+
+namespace armarx::armem::gui::instance
+{
+
+ void MemoryIDTreeWidgetItem::addKeyChildren()
+ {
+ addChild(new QTreeWidgetItem({"Memory Name"}));
+ addChild(new QTreeWidgetItem({"Core Segment Name"}));
+ addChild(new QTreeWidgetItem({"Provider Segment Name"}));
+ addChild(new QTreeWidgetItem({"Entity Name"}));
+ addChild(new QTreeWidgetItem({"Timestamp"}));
+ addChild(new QTreeWidgetItem({"Instance Index"}));
+ }
+
+
+ void MemoryIDTreeWidgetItem::setInstanceID(const MemoryID& id, int valueColumn)
+ {
+ setText(valueColumn, QString::fromStdString(id.str()));
+
+ const std::vector<std::string> items = id.getAllItems();
+ ARMARX_CHECK_EQUAL(childCount(), static_cast<int>(items.size()));
+ int i = 0;
+ for (const std::string& item : items)
+ {
+ child(i++)->setText(valueColumn, QString::fromStdString(item));
+ }
+ // Timestamp in human-readable format
+ if (id.hasTimestamp())
+ {
+ child(4)->setText(valueColumn, QString::fromStdString(toDateTimeMilliSeconds(id.timestamp)));
+ }
+ }
+
+}
+
diff --git a/source/RobotAPI/libraries/armem_gui/instance/MemoryIDTreeWidgetItem.h b/source/RobotAPI/libraries/armem_gui/instance/MemoryIDTreeWidgetItem.h
new file mode 100644
index 0000000000000000000000000000000000000000..a011a59aff48b8725e42aac1c7eab71ac5ca8676
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_gui/instance/MemoryIDTreeWidgetItem.h
@@ -0,0 +1,27 @@
+#pragma once
+
+#include <QTreeWidgetItem>
+
+
+namespace armarx::armem
+{
+ class MemoryID;
+}
+
+namespace armarx::armem::gui::instance
+{
+
+ class MemoryIDTreeWidgetItem : public QTreeWidgetItem
+ {
+ public:
+
+ using QTreeWidgetItem::QTreeWidgetItem;
+
+ void addKeyChildren();
+ void setInstanceID(const MemoryID& id, int valueColumn = 1);
+
+
+ };
+
+}
+
diff --git a/source/RobotAPI/libraries/armem_gui/instance/WidgetsWithToolbar.cpp b/source/RobotAPI/libraries/armem_gui/instance/WidgetsWithToolbar.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..b41ce7649aa21303658ed3a400f691e5efca1d0e
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_gui/instance/WidgetsWithToolbar.cpp
@@ -0,0 +1,80 @@
+/*
+* 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 ArmarX::
+* @author Rainer Kartmann ( rainer dot kartmann at kit dot edu)
+* @date 2021
+* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#include "WidgetsWithToolbar.h"
+
+#include <QAction>
+#include <QHBoxLayout>
+#include <QPushButton>
+#include <QToolBar>
+#include <QVBoxLayout>
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+#include "InstanceView.h"
+
+
+namespace armarx::armem::gui::instance
+{
+
+ WidgetsWithToolbar::WidgetsWithToolbar(QWidget* parent) : QWidget(parent)
+ {
+ const int margin = 0;
+ this->setContentsMargins(margin, margin, margin, margin);
+
+ _layout = new QHBoxLayout();
+ this->setLayout(_layout);
+ _layout->setContentsMargins(margin, margin, margin, margin);
+ _layout->setSpacing(0);
+
+
+ toolbar = new QToolBar();
+ toolbar->setOrientation(Qt::Orientation::Vertical);
+ toolbar->setContentsMargins(margin, margin, margin, margin);
+
+ QAction* action = toolbar->addAction(QIcon(":/icons/dialog-close.ico"), "Close", [this]()
+ {
+ this->close();
+ });
+ action->setToolTip("Remove this instance view");
+
+ _layout->addWidget(toolbar);
+ }
+
+
+ void WidgetsWithToolbar::addWidget(QWidget* widget)
+ {
+ ARMARX_CHECK_GREATER_EQUAL(_layout->count(), 1);
+ _layout->insertWidget(_layout->count() - 1, widget);
+ }
+
+ void WidgetsWithToolbar::close()
+ {
+ // ARMARX_IMPORTANT << "Closing instance view ...";
+ emit closing();
+
+ this->hide();
+ this->deleteLater();
+ }
+
+}
+
diff --git a/source/RobotAPI/libraries/armem_gui/instance/WidgetsWithToolbar.h b/source/RobotAPI/libraries/armem_gui/instance/WidgetsWithToolbar.h
new file mode 100644
index 0000000000000000000000000000000000000000..8d2f2a00aa0d2ee74d47f95ce88e49b85b3392e4
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_gui/instance/WidgetsWithToolbar.h
@@ -0,0 +1,76 @@
+/*
+* 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 ArmarX::
+* @author Rainer Kartmann ( rainer dot kartmann at kit dot edu)
+* @date 2021
+* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#pragma once
+
+#include <QWidget>
+
+class QHBoxLayout;
+class QVBoxLayout;
+class QToolBar;
+
+
+namespace armarx::armem::gui::instance
+{
+ class WidgetsWithToolbar : public QWidget
+ {
+ Q_OBJECT
+ using This = WidgetsWithToolbar;
+
+ public:
+
+ WidgetsWithToolbar(QWidget* parent = nullptr);
+
+ void addWidget(QWidget* widget);
+
+
+ public slots:
+
+ void close();
+
+
+ signals:
+
+ void closing();
+
+
+ protected slots:
+
+ signals:
+
+
+ protected:
+
+
+ public:
+
+ QToolBar* toolbar;
+
+
+ private:
+
+ QHBoxLayout* _layout;
+
+ };
+}
+
+
diff --git a/source/RobotAPI/libraries/armem_gui/instance/sanitize_typename.cpp b/source/RobotAPI/libraries/armem_gui/instance/sanitize_typename.cpp
index 9db04814561de68c4e7390093a7a7b3659f0219c..0180b51961c795ca4a45fd748078cb5cda1ca663 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/sanitize_typename.cpp
+++ b/source/RobotAPI/libraries/armem_gui/instance/sanitize_typename.cpp
@@ -7,13 +7,14 @@
#include <RobotAPI/libraries/armem/aron/MemoryID.aron.generated.h>
-static const std::string MemoryIDTypeName = armarx::armem::arondto::MemoryID::toInitialAronType()->getName();
+const std::string armarx::armem::gui::instance::rawMemoryIDTypeName = armarx::armem::arondto::MemoryID::toAronType()->getName();
+const std::string armarx::armem::gui::instance::sanitizedMemoryIDTypeName = "MemoryID";
std::string armarx::armem::gui::instance::sanitizeTypeName(const std::string& typeName)
{
- if (typeName == MemoryIDTypeName)
+ if (typeName == rawMemoryIDTypeName)
{
- return "MemoryID";
+ return sanitizedMemoryIDTypeName;
}
namespace s = simox::alg;
diff --git a/source/RobotAPI/libraries/armem_gui/instance/sanitize_typename.h b/source/RobotAPI/libraries/armem_gui/instance/sanitize_typename.h
index 80c77acf229ecb6d70b55b6ac1c5b951a0cb89bd..80bc8fe789e5d2f6a442632f3c88b93020ddbb02 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/sanitize_typename.h
+++ b/source/RobotAPI/libraries/armem_gui/instance/sanitize_typename.h
@@ -6,6 +6,9 @@
namespace armarx::armem::gui::instance
{
+ extern const std::string rawMemoryIDTypeName;
+ extern const std::string sanitizedMemoryIDTypeName;
+
std::string sanitizeTypeName(const std::string& typeName);
}
diff --git a/source/RobotAPI/libraries/armem_gui/instance/tree_builders/DataTreeBuilder.cpp b/source/RobotAPI/libraries/armem_gui/instance/tree_builders/DataTreeBuilder.cpp
index 07ccac8866d870b153fba28915006d3dfd46cdae..a2811c0bc5a01ef29fbb9cc41d7b5f47118f5a9a 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/tree_builders/DataTreeBuilder.cpp
+++ b/source/RobotAPI/libraries/armem_gui/instance/tree_builders/DataTreeBuilder.cpp
@@ -12,27 +12,27 @@ namespace armarx::armem::gui::instance
{
}
- void DataTreeBuilder::updateTree(QTreeWidgetItem* parent, aron::datanavigator::DictNavigator& data)
+ void DataTreeBuilder::updateTree(QTreeWidgetItem* parent, const aron::datanavigator::DictNavigatorPtr& data)
{
DictBuilder builder = getDictBuilder();
builder.setUpdateItemFn([this, &data](const std::string & key, QTreeWidgetItem * item)
{
- auto child = data.getElement(key);
- this->update(item, key, *child);
+ auto child = data->getElement(key);
+ this->update(item, key, child);
return true;
});
- builder.updateTree(parent, data.getAllKeys());
+ builder.updateTree(parent, data->getAllKeys());
}
- void DataTreeBuilder::updateTree(QTreeWidgetItem* parent, aron::datanavigator::ListNavigator& data)
+ void DataTreeBuilder::updateTree(QTreeWidgetItem* parent, const aron::datanavigator::ListNavigatorPtr& data)
{
- auto children = data.getChildren();
+ auto children = data->getChildren();
ListBuilder builder = getListBuilder();
builder.setUpdateItemFn([this, &children](size_t key, QTreeWidgetItem * item)
{
- this->update(item, std::to_string(key), *children.at(key));
+ this->update(item, std::to_string(key), children.at(key));
return true;
});
@@ -40,17 +40,23 @@ namespace armarx::armem::gui::instance
}
- void DataTreeBuilder::update(QTreeWidgetItem* item, const std::string& key, aron::datanavigator::Navigator& data)
+ void DataTreeBuilder::update(QTreeWidgetItem* item, const std::string& key, const aron::datanavigator::NavigatorPtr& data)
{
- this->setRowTexts(item, key, data);
+ if (!data)
+ {
+ this->setRowTexts(item, key, "null");
+ return;
+ }
+
+ this->setRowTexts(item, key, *data);
- if (auto cast = dynamic_cast<aron::datanavigator::DictNavigator*>(&data))
+ if (auto cast = aron::datanavigator::DictNavigator::DynamicCast(data))
{
- updateTree(item, *cast);
+ updateTree(item, cast);
}
- else if (auto cast = dynamic_cast<aron::datanavigator::ListNavigator*>(&data))
+ else if (auto cast = aron::datanavigator::ListNavigator::DynamicCast(data))
{
- updateTree(item, *cast);
+ updateTree(item, cast);
}
}
diff --git a/source/RobotAPI/libraries/armem_gui/instance/tree_builders/DataTreeBuilder.h b/source/RobotAPI/libraries/armem_gui/instance/tree_builders/DataTreeBuilder.h
index a9aa8471e5d4720c8e747844deb45459d68b1b90..cd408a34c3d3ebe2d36e5efe4cc7435ebefb0d94 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/tree_builders/DataTreeBuilder.h
+++ b/source/RobotAPI/libraries/armem_gui/instance/tree_builders/DataTreeBuilder.h
@@ -17,13 +17,13 @@ namespace armarx::armem::gui::instance
DataTreeBuilder();
- void updateTree(QTreeWidgetItem* parent, aron::datanavigator::DictNavigator& data);
- void updateTree(QTreeWidgetItem* parent, aron::datanavigator::ListNavigator& data);
+ void updateTree(QTreeWidgetItem* parent, const aron::datanavigator::DictNavigatorPtr& data);
+ void updateTree(QTreeWidgetItem* parent, const aron::datanavigator::ListNavigatorPtr& data);
protected:
- void update(QTreeWidgetItem* item, const std::string& key, aron::datanavigator::Navigator& data);
+ void update(QTreeWidgetItem* item, const std::string& key, const aron::datanavigator::NavigatorPtr& data);
};
diff --git a/source/RobotAPI/libraries/armem_gui/instance/tree_builders/TypedDataTreeBuilder.cpp b/source/RobotAPI/libraries/armem_gui/instance/tree_builders/TypedDataTreeBuilder.cpp
index 5be64a3f5657893e2c6f36e4695eff4a08c9dc9e..56d204abf42573540c363c0973f4bf10db357317 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/tree_builders/TypedDataTreeBuilder.cpp
+++ b/source/RobotAPI/libraries/armem_gui/instance/tree_builders/TypedDataTreeBuilder.cpp
@@ -2,10 +2,15 @@
#include <QTreeWidgetItem>
+#include <RobotAPI/libraries/aron/common/aron_conversions.h>
+#include <RobotAPI/libraries/armem/core/aron_conversions.h>
+#include <RobotAPI/libraries/armem/aron/MemoryID.aron.generated.h>
+
#include <RobotAPI/libraries/armem_gui/TreeWidgetBuilder.h>
#include <RobotAPI/libraries/armem_gui/instance/serialize_path.h>
#include <RobotAPI/libraries/armem_gui/instance/sanitize_typename.h>
#include <RobotAPI/libraries/armem_gui/instance/display_visitors/TypedDataDisplayVisitor.h>
+#include <RobotAPI/libraries/armem_gui/instance/MemoryIDTreeWidgetItem.h>
namespace armarx::armem::gui::instance
@@ -39,12 +44,26 @@ namespace armarx::armem::gui::instance
}
}
+
void TypedDataTreeBuilder::updateTree(
QTreeWidgetItem* parent,
aron::typenavigator::ObjectNavigator& type,
aron::datanavigator::DictNavigator& data)
{
DictBuilder builder = getDictBuilder();
+ builder.setMakeItemFn([this, &type](const std::string & key) -> QTreeWidgetItem*
+ {
+ if (type.getMemberType(key)->getName() == instance::rawMemoryIDTypeName)
+ {
+ MemoryIDTreeWidgetItem* item = new MemoryIDTreeWidgetItem({QString::fromStdString(key)});
+ item->addKeyChildren();
+ return item;
+ }
+ else
+ {
+ return this->makeItem(key);
+ }
+ });
builder.setUpdateItemFn([this, &type, &data](const std::string & key, QTreeWidgetItem * item)
{
auto childType = type.getMemberType(key);
@@ -85,6 +104,7 @@ namespace armarx::armem::gui::instance
}
}
+
void TypedDataTreeBuilder::updateTree(
QTreeWidgetItem* parent,
aron::typenavigator::PairNavigator& type,
@@ -109,6 +129,7 @@ namespace armarx::armem::gui::instance
builder.updateTree(parent, getIndex(data.childrenSize()));
}
+
void TypedDataTreeBuilder::updateTree(
QTreeWidgetItem* parent,
aron::typenavigator::TupleNavigator& type,
@@ -139,37 +160,54 @@ namespace armarx::armem::gui::instance
aron::typenavigator::Navigator& type,
aron::datanavigator::Navigator& data)
{
- using namespace aron::typenavigator;
+ using namespace aron;
- const std::string value = armarx::aron::TypedDataDisplayVisitor::getValue(type, data);
- const std::string typeName = sanitizeTypeName(type.getName());
+ const std::string value = aron::TypedDataDisplayVisitor::getValue(type, data);
+ const std::string typeName = instance::sanitizeTypeName(type.getName());
setRowTexts(item, key, value, typeName);
- item->setData(columnKey, Qt::UserRole, serializePath(data.getPath()));
+ item->setData(columnKey, Qt::UserRole, instance::serializePath(data.getPath()));
item->setData(columnType, Qt::UserRole, static_cast<int>(type.getDescriptor()));
- if (auto t = dynamic_cast<ObjectNavigator*>(&type))
+ if (typeName == sanitizedMemoryIDTypeName)
+ {
+ MemoryIDTreeWidgetItem* memoryIDItem = dynamic_cast<MemoryIDTreeWidgetItem*>(item);
+ datanavigator::DictNavigator* dictData = dynamic_cast<datanavigator::DictNavigator*>(&data);
+ if (memoryIDItem && dictData)
+ {
+ arondto::MemoryID dto;
+ // Because fromAron does not take refs -.-
+ dto.fromAron(std::make_shared<datanavigator::DictNavigator>(*dictData));
+
+ MemoryID id = aron::fromAron<MemoryID>(dto);
+ memoryIDItem->setInstanceID(id);
+ return; // Done, no recursion.
+ }
+ }
+
+ if (auto t = dynamic_cast<typenavigator::ObjectNavigator*>(&type))
{
- _updateTree<aron::datanavigator::DictNavigator>(item, *t, data);
+ _updateTree<datanavigator::DictNavigator>(item, *t, data);
}
- else if (auto t = dynamic_cast<DictNavigator*>(&type))
+ else if (auto t = dynamic_cast<typenavigator::DictNavigator*>(&type))
{
- _updateTree<aron::datanavigator::DictNavigator>(item, *t, data);
+ _updateTree<datanavigator::DictNavigator>(item, *t, data);
}
- else if (auto t = dynamic_cast<ListNavigator*>(&type))
+ else if (auto t = dynamic_cast<typenavigator::ListNavigator*>(&type))
{
- _updateTree<aron::datanavigator::ListNavigator>(item, *t, data);
+ _updateTree<datanavigator::ListNavigator>(item, *t, data);
}
- else if (auto t = dynamic_cast<PairNavigator*>(&type))
+ else if (auto t = dynamic_cast<typenavigator::PairNavigator*>(&type))
{
- _updateTree<aron::datanavigator::ListNavigator>(item, *t, data);
+ _updateTree<datanavigator::ListNavigator>(item, *t, data);
}
- else if (auto t = dynamic_cast<TupleNavigator*>(&type))
+ else if (auto t = dynamic_cast<typenavigator::TupleNavigator*>(&type))
{
- _updateTree<aron::datanavigator::ListNavigator>(item, *t, data);
+ _updateTree<datanavigator::ListNavigator>(item, *t, data);
}
}
+
template <class DataT, class TypeT>
void TypedDataTreeBuilder::_updateTree(QTreeWidgetItem* item, TypeT& type, aron::datanavigator::Navigator& data)
{
diff --git a/source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeTypedDataVisitor.h b/source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeTypedDataVisitor.h
index fb1c1baa557167432ffaa19f46e71d223a5c080d..49885f7eb3d66e561e0a83c496524907afd5d049 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeTypedDataVisitor.h
+++ b/source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeTypedDataVisitor.h
@@ -13,6 +13,8 @@
#include <RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeDataVisitorBase.h>
+#include <SimoxUtility/algorithm/string/string_tools.h>
+
namespace armarx::armem::gui::instance
{
diff --git a/source/RobotAPI/libraries/armem_gui/lifecycle.cpp b/source/RobotAPI/libraries/armem_gui/lifecycle.cpp
index 508ec50924baacdfe90df941a8f55ac730620837..ce054ff30d516dd5bb64c120b197fd2f50887ae1 100644
--- a/source/RobotAPI/libraries/armem_gui/lifecycle.cpp
+++ b/source/RobotAPI/libraries/armem_gui/lifecycle.cpp
@@ -58,16 +58,31 @@ namespace armarx::gui
void LifecycleClient::onInit(ManagedIceObject& component)
{
(void) component;
+ onInit();
}
void LifecycleClient::onConnect(ManagedIceObject& component)
{
(void) component;
+ onConnect();
}
void LifecycleClient::onDisconnect(ManagedIceObject& component)
{
(void) component;
+ onDisconnect();
+ }
+
+ void LifecycleClient::onInit()
+ {
+ }
+
+ void LifecycleClient::onConnect()
+ {
+ }
+
+ void LifecycleClient::onDisconnect()
+ {
}
}
diff --git a/source/RobotAPI/libraries/armem_gui/lifecycle.h b/source/RobotAPI/libraries/armem_gui/lifecycle.h
index 0bc404387b3165841526c12a09a9fa6bbad0fdd9..cb5cb3ddfc4d2f2fc3f1b96709db0a3bc49c245e 100644
--- a/source/RobotAPI/libraries/armem_gui/lifecycle.h
+++ b/source/RobotAPI/libraries/armem_gui/lifecycle.h
@@ -70,6 +70,11 @@ namespace armarx::gui
virtual void onConnect(ManagedIceObject& component);
virtual void onDisconnect(ManagedIceObject& component);
+ // Alternative override
+ virtual void onInit();
+ virtual void onConnect();
+ virtual void onDisconnect();
+
};
diff --git a/source/RobotAPI/libraries/armem_gui/memory/TreeWidget.cpp b/source/RobotAPI/libraries/armem_gui/memory/TreeWidget.cpp
index 4dc890e600b5419db408d3110455a19fd96f2e68..1d4a2b908eba38fbb4be2d028030598c220aae44 100644
--- a/source/RobotAPI/libraries/armem_gui/memory/TreeWidget.cpp
+++ b/source/RobotAPI/libraries/armem_gui/memory/TreeWidget.cpp
@@ -34,7 +34,7 @@ namespace armarx::armem::gui::memory
header()->setMinimumSectionSize(25);
header()->resizeSection(int(Columns::KEY), 250);
- header()->resizeSection(int(Columns::SIZE), 30);
+ header()->resizeSection(int(Columns::SIZE), 40);
header()->setTextElideMode(Qt::TextElideMode::ElideRight);
diff --git a/source/RobotAPI/libraries/armem_gui/query_widgets/QueryWidget.cpp b/source/RobotAPI/libraries/armem_gui/query_widgets/QueryWidget.cpp
index 6ac42aee3f0870279f8f8fe823fdb9ca5340c4c7..ae9af3c213fbbea4d4a44852e8e02d0af15b8de9 100644
--- a/source/RobotAPI/libraries/armem_gui/query_widgets/QueryWidget.cpp
+++ b/source/RobotAPI/libraries/armem_gui/query_widgets/QueryWidget.cpp
@@ -45,6 +45,7 @@ namespace armarx::armem::gui
{
armem::client::query::Builder qb(dataMode());
qb
+ .queryTarget(_snapshotSelectorWidget->queryTarget())
.coreSegments().all()
.providerSegments().all()
.entities().all()
diff --git a/source/RobotAPI/libraries/armem_gui/query_widgets/SnapshotForm.cpp b/source/RobotAPI/libraries/armem_gui/query_widgets/SnapshotForm.cpp
index 8e00d3811f24d12146df6aa0c782af2437a71711..8d60c2252d4e9d6593a228cad75075cc41a21e5c 100644
--- a/source/RobotAPI/libraries/armem_gui/query_widgets/SnapshotForm.cpp
+++ b/source/RobotAPI/libraries/armem_gui/query_widgets/SnapshotForm.cpp
@@ -42,6 +42,22 @@ namespace armarx::armem::gui
}
+ // Source: https://stackoverflow.com/a/26538572
+ class LeadingZeroSpinBox : public QSpinBox
+ {
+ using QSpinBox::QSpinBox;
+
+ int numDigits = 6;
+ int base = 10;
+
+ virtual QString textFromValue(int value) const;
+ };
+ QString LeadingZeroSpinBox::textFromValue(int value) const
+ {
+ return QString("%1").arg(value, numDigits, base, QChar('0'));
+ }
+
+
SnapshotFormSingle::SnapshotFormSingle()
{
const QDateTime now = QDateTime::currentDateTime();
@@ -52,25 +68,41 @@ namespace armarx::armem::gui
dateTime->setDisabled(true);
setDateTimeDisplayFormat(dateTime);
+ microseconds = new LeadingZeroSpinBox();
+ microseconds->setDisabled(true);
+ microseconds->setMinimum(0);
+ microseconds->setMaximum(1000 * 1000 - 1);
+ microseconds->setSingleStep(1);
+ microseconds->setValue(static_cast<int>(now.toMSecsSinceEpoch() % 1000) * 1000);
+
+ QHBoxLayout* timestampLayout = new QHBoxLayout();
+ timestampLayout->addWidget(dateTime);
+ timestampLayout->addWidget(microseconds);
+
latest = new QCheckBox("Latest");
latest->setChecked(true);
QGridLayout* layout = new QGridLayout(this);
layout->addWidget(label, 0, 0);
- layout->addWidget(dateTime, 0, 1);
+ layout->addLayout(timestampLayout, 0, 1);
layout->addWidget(latest, 0, 2);
connect(latest, &QCheckBox::toggled, dateTime, &QDateTimeEdit::setDisabled);
+ connect(latest, &QCheckBox::toggled, microseconds, &QSpinBox::setDisabled);
connect(dateTime, &QDateTimeEdit::dateTimeChanged, this, &SnapshotForm::queryChanged);
+ connect(microseconds, QOverload<int>::of(&QSpinBox::valueChanged), this, &SnapshotForm::queryChanged);
connect(latest, &QCheckBox::toggled, this, &SnapshotForm::queryChanged);
}
+
void SnapshotFormSingle::fillEntitySelector(client::query::SnapshotSelector& selector)
{
- selector.atTime(latest->isChecked()
- ? Time::microSeconds(-1)
- : Time::milliSeconds(dateTime->dateTime().toMSecsSinceEpoch()));
+ const Time time = latest->isChecked()
+ ? Time::microSeconds(-1)
+ : (Time::seconds(dateTime->dateTime().toSecsSinceEpoch()))
+ + Time::microSeconds(microseconds->value());
+ selector.atTime(time);
}
@@ -159,7 +191,7 @@ namespace armarx::armem::gui
sb->setMinimum(-1000);
sb->setMaximum(1000);
}
- firstSpinBox->setValue(-10);
+ firstSpinBox->setValue(-2);
lastSpinBox->setValue(-1);
QString tooltip = "Python index semantics: Negative indices count from the end (-1 is the last entry).";
diff --git a/source/RobotAPI/libraries/armem_gui/query_widgets/SnapshotForm.h b/source/RobotAPI/libraries/armem_gui/query_widgets/SnapshotForm.h
index 4fa9b3a8d9148048747174c8c72ed155953dc8b3..0ca9950b33a0585c97a8157bf17ad3c272fc8abc 100644
--- a/source/RobotAPI/libraries/armem_gui/query_widgets/SnapshotForm.h
+++ b/source/RobotAPI/libraries/armem_gui/query_widgets/SnapshotForm.h
@@ -59,6 +59,7 @@ namespace armarx::armem::gui
private:
QLabel* label;
QDateTimeEdit* dateTime;
+ QSpinBox* microseconds;
QCheckBox* latest;
};
diff --git a/source/RobotAPI/libraries/armem_gui/query_widgets/SnapshotSelectorWidget.cpp b/source/RobotAPI/libraries/armem_gui/query_widgets/SnapshotSelectorWidget.cpp
index 762421d132997ffaec145d170cc71b708e2cb414..87d94376e673492d07a7fe2f0206359259ea327a 100644
--- a/source/RobotAPI/libraries/armem_gui/query_widgets/SnapshotSelectorWidget.cpp
+++ b/source/RobotAPI/libraries/armem_gui/query_widgets/SnapshotSelectorWidget.cpp
@@ -15,6 +15,11 @@ namespace armarx::armem::gui
return _selector;
}
+ query::data::QueryTarget SnapshotSelectorWidget::queryTarget() const
+ {
+ return _queryTarget;
+ }
+
SnapshotSelectorWidget::SnapshotSelectorWidget()
{
@@ -22,19 +27,30 @@ namespace armarx::armem::gui
setLayout(_pageLayout);
connect(this, &This::queryOutdated, this, &This::updateSelector);
+ connect(this, &This::queryTargetOutdated, this, &This::updateQueryTarget);
{
QHBoxLayout* typeLayout = new QHBoxLayout();
+ // snapshot select box
_queryComboBox = new QComboBox();
_queryComboBox->addItems({"All", "Single", "Time Range", "Index Range"});
_queryComboBox->setCurrentIndex(3);
typeLayout->addWidget(_queryComboBox);
- connect(_queryComboBox, &QComboBox::currentTextChanged, this, &This::showSelectedForm);
+ connect(_queryComboBox, &QComboBox::currentTextChanged, this, &This::showSelectedFormForQuery);
connect(_queryComboBox, &QComboBox::currentTextChanged, this, &This::queryOutdated);
+ // query type select box
+ _queryTargetComboBox = new QComboBox();
+ _queryTargetComboBox->addItems({"WM", "WM & LTM", "LTM (debug only)"});
+ _queryTargetComboBox->setCurrentIndex(0);
+
+ typeLayout->addWidget(_queryTargetComboBox);
+
+ connect(_queryTargetComboBox, &QComboBox::currentTextChanged, this, &This::queryTargetOutdated);
+
_pageLayout->addLayout(typeLayout);
}
@@ -45,8 +61,14 @@ namespace armarx::armem::gui
addForm("Time Range", new SnapshotFormTimeRange());
addForm("Index Range", new SnapshotFormIndexRange());
- showSelectedForm(_queryComboBox->currentText());
+ // Add query targets
+ _queryTargets.insert({"WM", query::data::QueryTarget::WM});
+ _queryTargets.insert({"WM & LTM", query::data::QueryTarget::WMAndLTM});
+ _queryTargets.insert({"LTM (debug only)", query::data::QueryTarget::LTM});
+
+ showSelectedFormForQuery(_queryComboBox->currentText());
updateSelector();
+ updateQueryTarget();
}
void SnapshotSelectorWidget::updateSelector()
@@ -55,7 +77,13 @@ namespace armarx::armem::gui
emit queryChanged();
}
- void SnapshotSelectorWidget::showSelectedForm(QString selected)
+ void SnapshotSelectorWidget::updateQueryTarget()
+ {
+ this->_queryTarget = _queryTargets.at(_queryTargetComboBox->currentText());
+ emit queryTargetChanged();
+ }
+
+ void SnapshotSelectorWidget::showSelectedFormForQuery(QString selected)
{
for (auto& [name, form] : _queryForms)
{
diff --git a/source/RobotAPI/libraries/armem_gui/query_widgets/SnapshotSelectorWidget.h b/source/RobotAPI/libraries/armem_gui/query_widgets/SnapshotSelectorWidget.h
index 2241b67e810292b685fe0837f43ea621cfb2fb93..92db5fbb13c74b011ca0f36a64fbf82a58536717 100644
--- a/source/RobotAPI/libraries/armem_gui/query_widgets/SnapshotSelectorWidget.h
+++ b/source/RobotAPI/libraries/armem_gui/query_widgets/SnapshotSelectorWidget.h
@@ -36,6 +36,7 @@ namespace armarx::armem::gui
armem::DataMode dataMode() const;
client::query::SnapshotSelector& selector();
+ query::data::QueryTarget queryTarget() const;
public slots:
@@ -43,19 +44,22 @@ namespace armarx::armem::gui
signals:
void queryChanged();
+ void queryTargetChanged();
private slots:
void updateSelector();
+ void updateQueryTarget();
void hideAllForms();
- void showSelectedForm(QString selected);
+ void showSelectedFormForQuery(QString selected);
signals:
void queryOutdated();
+ void queryTargetOutdated();
private:
@@ -66,11 +70,14 @@ namespace armarx::armem::gui
public:
client::query::SnapshotSelector _selector;
+ query::data::QueryTarget _queryTarget;
QVBoxLayout* _pageLayout;
QComboBox* _queryComboBox;
+ QComboBox* _queryTargetComboBox;
/// The forms for the different query types. Hidden when not selected.
std::map<QString, SnapshotForm*> _queryForms;
+ std::map<QString, query::data::QueryTarget> _queryTargets;
};
diff --git a/source/RobotAPI/libraries/armem_objects/CMakeLists.txt b/source/RobotAPI/libraries/armem_objects/CMakeLists.txt
index 8c0f625f022e3efeb7cf7763841a25e6469b6ac8..54eb919e1804ce099efe5ade4ea14294a202e0bb 100644
--- a/source/RobotAPI/libraries/armem_objects/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem_objects/CMakeLists.txt
@@ -20,6 +20,8 @@ armarx_add_library(
aron_conversions.h
aron_forward_declarations.h
+ SceneSnapshot.h
+
server/class/FloorVis.h
server/class/Segment.h
@@ -28,17 +30,21 @@ armarx_add_library(
server/instance/Decay.h
server/instance/RobotHeadMovement.h
server/instance/Visu.h
+ server/instance/ArticulatedObjectVisu.h
- server/articulated_object_class/Segment.h
- server/articulated_object_instance/Segment.h
+ # server/articulated_object_class/Segment.h
+ # server/articulated_object_instance/Segment.h
# server/articulated_object/SegmentAdapter.h
- server/articulated_object_instance/Visu.h
+ # server/articulated_object_instance/Visu.h
server/attachments/Segment.h
client/articulated_object/Reader.h
client/articulated_object/Writer.h
+ client/articulated_object/ArticulatedObjectReader.h
+ client/articulated_object/ArticulatedObjectWriter.h
client/articulated_object/interfaces.h
+ client/articulated_object/utils.h
client/attachment/Reader.h
client/attachment/Writer.h
@@ -46,6 +52,8 @@ armarx_add_library(
SOURCES
aron_conversions.cpp
+ SceneSnapshot.cpp
+
server/class/FloorVis.cpp
server/class/Segment.cpp
@@ -54,17 +62,21 @@ armarx_add_library(
server/instance/Decay.cpp
server/instance/RobotHeadMovement.cpp
server/instance/Visu.cpp
+ server/instance/ArticulatedObjectVisu.cpp
server/articulated_object_class/Segment.cpp
- server/articulated_object_instance/Segment.cpp
+ # server/articulated_object_instance/Segment.cpp
# server/articulated_object/SegmentAdapter.cpp
- server/articulated_object_instance/Visu.cpp
+ # server/articulated_object_instance/Visu.cpp
server/attachments/Segment.cpp
client/articulated_object/Reader.cpp
client/articulated_object/Writer.cpp
+ client/articulated_object/ArticulatedObjectReader.cpp
+ client/articulated_object/ArticulatedObjectWriter.cpp
+ client/articulated_object/utils.cpp
client/attachment/Reader.cpp
client/attachment/Writer.cpp
diff --git a/source/RobotAPI/libraries/armem_objects/SceneSnapshot.cpp b/source/RobotAPI/libraries/armem_objects/SceneSnapshot.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..d3ece87d9746198af2f5b9acc5b81cce38d189aa
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/SceneSnapshot.cpp
@@ -0,0 +1,90 @@
+#include "SceneSnapshot.h"
+
+#include <SimoxUtility/json.h>
+
+#include <RobotAPI/libraries/ArmarXObjects/ObjectID.h>
+#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
+
+// #include <RobotAPI/libraries/armem/core/json_conversions.h>
+
+
+namespace armarx::armem::obj
+{
+
+ ObjectID SceneSnapshot::Object::getClassID() const
+ {
+ return ObjectID(className);
+ }
+
+
+ ObjectID SceneSnapshot::Object::getClassID(ObjectFinder& finder) const
+ {
+ ObjectID id = getClassID();
+ if (id.dataset().empty())
+ {
+ if (std::optional<ObjectInfo> info = finder.findObject(id.className()))
+ {
+ return info->id();
+ }
+ }
+ return id;
+ }
+
+
+ ObjectID SceneSnapshot::Object::getObjectID() const
+ {
+ return getClassID().withInstanceName(instanceName);
+ }
+
+
+ ObjectID SceneSnapshot::Object::getObjectID(ObjectFinder& finder) const
+ {
+ return getClassID(finder).withInstanceName(instanceName);
+ }
+
+}
+
+
+void armarx::armem::obj::to_json(nlohmann::json& j, const SceneSnapshot::Object& rhs)
+{
+ // j["instanceID"] = rhs.instanceID;
+ j["class"] = rhs.className;
+ j["instanceName"] = rhs.instanceName;
+ j["collection"] = rhs.collection;
+ j["position"] = rhs.position;
+ j["orientation"] = rhs.orientation;
+ j["jointValues"] = rhs.jointValues;
+}
+
+
+void armarx::armem::obj::from_json(const nlohmann::json& j, SceneSnapshot::Object& rhs)
+{
+ // j.at("instanceID").get_to(rhs.instanceID);
+ j.at("class").get_to(rhs.className);
+ if (j.count("instanceName"))
+ {
+ j["instanceName"].get_to(rhs.instanceName);
+ }
+ j.at("collection").get_to(rhs.collection);
+ j.at("position").get_to(rhs.position);
+ j.at("orientation").get_to(rhs.orientation);
+ if (j.count("jointValues"))
+ {
+ j.at("jointValues").get_to(rhs.jointValues);
+ }
+}
+
+
+
+void armarx::armem::obj::to_json(nlohmann::json& j, const SceneSnapshot& rhs)
+{
+ j["objects"] = rhs.objects;
+}
+
+
+void armarx::armem::obj::from_json(const nlohmann::json& j, SceneSnapshot& rhs)
+{
+ j.at("objects").get_to(rhs.objects);
+}
+
+
diff --git a/source/RobotAPI/libraries/armem_objects/SceneSnapshot.h b/source/RobotAPI/libraries/armem_objects/SceneSnapshot.h
new file mode 100644
index 0000000000000000000000000000000000000000..3e24156490544ebc537372a0d204c052ae8b2428
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/SceneSnapshot.h
@@ -0,0 +1,64 @@
+/*
+ * 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::ObjectMemory
+ * @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+#pragma once
+
+#include <SimoxUtility/json/json.hpp>
+
+#include <Eigen/Geometry>
+
+
+namespace armarx
+{
+ class ObjectID;
+ class ObjectFinder;
+}
+namespace armarx::armem::obj
+{
+
+ struct SceneSnapshot
+ {
+ struct Object
+ {
+ std::string className;
+ std::string instanceName;
+ std::string collection;
+
+ Eigen::Vector3f position = Eigen::Vector3f::Zero();
+ Eigen::Quaternionf orientation = Eigen::Quaternionf::Identity();
+
+ std::map<std::string, float> jointValues;
+
+ ObjectID getClassID() const;
+ ObjectID getClassID(ObjectFinder& finder) const;
+ ObjectID getObjectID() const;
+ ObjectID getObjectID(ObjectFinder& finder) const;
+ };
+ std::vector<Object> objects;
+ };
+
+ void to_json(nlohmann::json& j, const SceneSnapshot::Object& rhs);
+ void from_json(const nlohmann::json& j, SceneSnapshot::Object& rhs);
+
+ void to_json(nlohmann::json& j, const SceneSnapshot& rhs);
+ void from_json(const nlohmann::json& j, SceneSnapshot& rhs);
+
+}
diff --git a/source/RobotAPI/libraries/armem_objects/aron/ObjectClass.xml b/source/RobotAPI/libraries/armem_objects/aron/ObjectClass.xml
index 650082eb4ef8b924c8368fdd6cdf8da4578e105a..cb3c418e68b53cb24b781b22ff057386f995d2dc 100644
--- a/source/RobotAPI/libraries/armem_objects/aron/ObjectClass.xml
+++ b/source/RobotAPI/libraries/armem_objects/aron/ObjectClass.xml
@@ -31,6 +31,10 @@ Core segment type of Object/Class.
<armarx::arondto::PackagePath />
</ObjectChild>
+ <ObjectChild key="articulatedSimoxXmlPath">
+ <armarx::arondto::PackagePath />
+ </ObjectChild>
+
<ObjectChild key="meshWrlPath">
<armarx::arondto::PackagePath />
</ObjectChild>
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectReader.cpp b/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectReader.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c973cf24801967244589677f9b044be099f5c306
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectReader.cpp
@@ -0,0 +1,61 @@
+#include "ArticulatedObjectReader.h"
+
+#include <mutex>
+#include <optional>
+
+#include <VirtualRobot/XML/RobotIO.h>
+
+#include <ArmarXCore/core/PackagePath.h>
+#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/system/ArmarXDataPath.h>
+
+#include "RobotAPI/libraries/ArmarXObjects/ObjectInfo.h"
+#include "RobotAPI/libraries/ArmarXObjects/ObjectPose.h"
+#include "RobotAPI/libraries/armem_objects/aron_conversions.h"
+#include <RobotAPI/libraries/ArmarXObjects/aron/ObjectPose.aron.generated.h>
+#include <RobotAPI/libraries/armem/client/query/Builder.h>
+#include <RobotAPI/libraries/armem/core/Time.h>
+#include <RobotAPI/libraries/armem/core/error.h>
+#include <RobotAPI/libraries/armem/core/workingmemory/CoreSegment.h>
+#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot/aron_conversions.h>
+#include <RobotAPI/libraries/armem_robot/robot_conversions.h>
+
+namespace armarx::armem::articulated_object
+{
+
+ VirtualRobot::RobotPtr ArticulatedObjectReader::getArticulatedObject(const std::string& name,
+ const armem::Time& timestamp)
+ {
+ const auto descriptions = queryDescriptions(IceUtil::Time::now());
+
+ ARMARX_INFO << "Found " << descriptions.size() << " articulated object descriptions";
+
+ const auto it = std::find_if(
+ descriptions.begin(),
+ descriptions.end(),
+ [&](const armem::articulated_object::ArticulatedObjectDescription & desc) -> bool
+ { return desc.name == name; });
+
+ if (it == descriptions.end())
+ {
+ ARMARX_WARNING << "Articulated object " << name << " not (yet) available";
+ return nullptr;
+ }
+
+ auto obj =
+ VirtualRobot::RobotIO::loadRobot(ArmarXDataPath::resolvePath(it->xml.serialize().path),
+ VirtualRobot::RobotIO::eStructure);
+
+ if (not obj)
+ {
+ return nullptr;
+ }
+
+ obj->setName("");
+ obj->setType(it->name);
+
+ return obj;
+ }
+
+} // namespace armarx::armem::articulated_object
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectReader.h b/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectReader.h
new file mode 100644
index 0000000000000000000000000000000000000000..5d918ddf474bfb110afaede2c4ce765ae446e931
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectReader.h
@@ -0,0 +1,16 @@
+#pragma once
+
+#include "Reader.h"
+
+namespace armarx::armem::articulated_object
+{
+
+ class ArticulatedObjectReader : virtual public Reader
+ {
+ public:
+ using Reader::Reader;
+
+ VirtualRobot::RobotPtr getArticulatedObject(const std::string& name,
+ const armem::Time& timestamp);
+ };
+} // namespace armarx::armem::articulated_object
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectWriter.cpp b/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectWriter.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..f86f5f90d7787078572c9efa4b20cd5142100273
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectWriter.cpp
@@ -0,0 +1,49 @@
+#include "ArticulatedObjectWriter.h"
+
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+
+#include "ArmarXCore/core/exceptions/local/ExpressionException.h"
+#include <ArmarXCore/core/system/ArmarXDataPath.h>
+
+#include <VirtualRobot/Robot.h>
+
+namespace armarx::armem::articulated_object
+{
+ armem::articulated_object::ArticulatedObject convert(const VirtualRobot::Robot& obj,
+ const armem::Time& timestamp)
+ {
+ ARMARX_DEBUG << "Filename is " << obj.getFilename();
+
+ // TODO(fabian.reister): remove "PriorKnowledgeData" below
+
+ return armem::articulated_object::ArticulatedObject
+ {
+ .description = {
+ .name = obj.getType(),
+ .xml = PackagePath(armarx::ArmarXDataPath::getProject(
+ {"PriorKnowledgeData"}, obj.getFilename()),
+ obj.getFilename())
+ },
+ .instance = "", // TODO(fabian.reister):
+ .config = {
+ .timestamp = timestamp,
+ .globalPose = Eigen::Affine3f(obj.getRootNode()->getGlobalPose()),
+ .jointMap = obj.getJointValues()
+ },
+ .timestamp = timestamp};
+ }
+
+ bool
+ ArticulatedObjectWriter::storeArticulatedObject(const VirtualRobot::RobotPtr& articulatedObject,
+ const armem::Time& timestamp)
+ {
+
+ ARMARX_CHECK_NOT_NULL(articulatedObject);
+
+ armarx::armem::articulated_object::ArticulatedObject armemArticulatedObject =
+ convert(*articulatedObject, IceUtil::Time::now());
+
+ return store(armemArticulatedObject);
+ }
+} // namespace armarx::armem::articulated_object
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectWriter.h b/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectWriter.h
new file mode 100644
index 0000000000000000000000000000000000000000..ff39d70f049fa670e41157dbb79608b16d98b66c
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectWriter.h
@@ -0,0 +1,21 @@
+
+
+#pragma once
+
+#include <VirtualRobot/VirtualRobot.h>
+
+#include "Writer.h"
+
+namespace armarx::armem::articulated_object
+{
+
+ class ArticulatedObjectWriter : virtual public Writer
+ {
+ public:
+ using Writer::Writer;
+
+ bool
+ storeArticulatedObject(const VirtualRobot::RobotPtr& articulatedObject,
+ const armem::Time& timestamp);
+ };
+} // namespace armarx::armem::articulated_object
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.cpp b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.cpp
index d11b7421313e4586e4347b31c9f8c79c09f169f5..d0bfb35708104712d2bebc33b4d7676b999ddac5 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.cpp
@@ -3,22 +3,32 @@
#include <mutex>
#include <optional>
-#include "ArmarXCore/core/logging/Logging.h"
#include <ArmarXCore/core/PackagePath.h>
-
-#include "RobotAPI/libraries/armem/client/query/Builder.h"
-#include "RobotAPI/libraries/armem/core/Time.h"
-#include "RobotAPI/libraries/armem/core/workingmemory/CoreSegment.h"
-#include "RobotAPI/libraries/armem_robot/aron_conversions.h"
-#include "RobotAPI/libraries/armem_robot/robot_conversions.h"
+#include <ArmarXCore/core/logging/Logging.h>
+
+#include "RobotAPI/libraries/ArmarXObjects/ObjectInfo.h"
+#include "RobotAPI/libraries/ArmarXObjects/ObjectPose.h"
+#include "RobotAPI/libraries/armem_objects/aron_conversions.h"
+#include <RobotAPI/libraries/ArmarXObjects/aron/ObjectPose.aron.generated.h>
+#include <RobotAPI/libraries/armem/client/query/Builder.h>
+#include <RobotAPI/libraries/armem/core/Time.h>
+#include <RobotAPI/libraries/armem/core/error.h>
+#include <RobotAPI/libraries/armem/core/workingmemory/CoreSegment.h>
#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot/aron_conversions.h>
+#include <RobotAPI/libraries/armem_robot/robot_conversions.h>
+
+#include "utils.h"
namespace fs = ::std::filesystem;
namespace armarx::armem::articulated_object
{
- Reader::Reader(armem::ClientReaderComponentPluginUser& component) : component(component) {}
+ Reader::Reader(armem::client::MemoryNameSystem& memoryNameSystem) :
+ memoryNameSystem(memoryNameSystem)
+ {
+ }
void Reader::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
{
@@ -41,23 +51,23 @@ namespace armarx::armem::articulated_object
{
// Wait for the memory to become available and add it as dependency.
ARMARX_IMPORTANT << "Reader: Waiting for memory '" << properties.memoryName << "' ...";
- auto result = component.useMemory(properties.memoryName);
- if (not result.success)
+ try
+ {
+ memoryReader = memoryNameSystem.useReader(properties.memoryName);
+ ARMARX_IMPORTANT << "Reader: Connected to memory '" << properties.memoryName << "'";
+ }
+ catch (const armem::error::CouldNotResolveMemoryServer& e)
{
- ARMARX_ERROR << result.errorMessage;
+ ARMARX_ERROR << e.what();
return;
}
- ARMARX_IMPORTANT << "Reader: Connected to memory '" << properties.memoryName;
-
- memoryReader.setReadingMemory(result.proxy);
-
armem::MemoryID id = armem::MemoryID();
id.memoryName = properties.memoryName;
id.coreSegmentName = properties.coreClassSegmentName;
// listen to all provider segments!
- memoryReader.subscribe(id, this, &Reader::updateKnownObjects);
+ memoryNameSystem.subscribe(id, this, &Reader::updateKnownObjects);
}
void Reader::updateKnownObject(const armem::MemoryID& snapshotId)
@@ -159,6 +169,16 @@ namespace armarx::armem::articulated_object
return getRobotDescriptions(qResult.memory);
}
+ std::string Reader::getProviderName() const
+ {
+ return properties.providerName;
+ }
+
+ void Reader::setProviderName(const std::string& providerName)
+ {
+ this->properties.providerName = providerName;
+ }
+
std::optional<robot::RobotDescription> Reader::queryDescription(const std::string& name,
const armem::Time& timestamp)
{
@@ -222,7 +242,7 @@ namespace armarx::armem::articulated_object
.getCoreSegment(properties.coreInstanceSegmentName);
// clang-format on
- for (const auto &[_, providerSegment] : coreSegment.providerSegments())
+ for (const auto& [_, providerSegment] : coreSegment.providerSegments())
{
const auto entities = simox::alg::get_values(providerSegment.entities());
@@ -250,16 +270,16 @@ namespace armarx::armem::articulated_object
return std::nullopt;
}
+
std::optional<robot::RobotDescription>
Reader::getRobotDescription(const armarx::armem::wm::Memory& memory) const
{
// clang-format off
const armem::wm::CoreSegment& coreSegment = memory
.getCoreSegment(properties.coreClassSegmentName);
- // .getProviderSegment(properties.providerName); // TODO(fabian.reister): all
// clang-format on
- for (const auto &[_, providerSegment] : coreSegment.providerSegments())
+ for (const auto& [_, providerSegment] : coreSegment.providerSegments())
{
const auto entities = simox::alg::get_values(providerSegment.entities());
@@ -267,7 +287,6 @@ namespace armarx::armem::articulated_object
{
ARMARX_WARNING << "No entity found";
continue;
- // return std::nullopt;
}
const auto entitySnapshots = simox::alg::get_values(entities.front().history());
@@ -276,13 +295,13 @@ namespace armarx::armem::articulated_object
{
ARMARX_WARNING << "No entity snapshots found";
continue;
- // return std::nullopt;
}
- // TODO(fabian.reister): check if 0 available
- const armem::wm::EntityInstance& instance = entitySnapshots.front().getInstance(0);
-
- return robot::convertRobotDescription(instance);
+ if (entitySnapshots.front().hasInstance(0))
+ {
+ const armem::wm::EntityInstance& instance = entitySnapshots.front().getInstance(0);
+ return convertRobotDescription(instance);
+ }
}
return std::nullopt;
@@ -296,9 +315,9 @@ namespace armarx::armem::articulated_object
const armem::wm::CoreSegment& coreSegment =
memory.getCoreSegment(properties.coreClassSegmentName);
- for (const auto &[providerName, providerSegment] : coreSegment.providerSegments())
+ for (const auto& [providerName, providerSegment] : coreSegment.providerSegments())
{
- for (const auto &[name, entity] : providerSegment.entities())
+ for (const auto& [name, entity] : providerSegment.entities())
{
if (entity.empty())
{
@@ -309,7 +328,7 @@ namespace armarx::armem::articulated_object
const auto entitySnapshots = simox::alg::get_values(entity.history());
const armem::wm::EntityInstance& instance = entitySnapshots.front().getInstance(0);
- const auto robotDescription = robot::convertRobotDescription(instance);
+ const auto robotDescription = convertRobotDescription(instance);
if (robotDescription)
{
@@ -321,4 +340,4 @@ namespace armarx::armem::articulated_object
return descriptions;
}
-} // namespace armarx::armem::articulated_object
\ No newline at end of file
+} // namespace armarx::armem::articulated_object
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.h b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.h
index 21a4591cd2f1f5c9a125177e50285b3802f561b9..f5dac4035dfb4c8ccab3e82e78af035d64fd981f 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.h
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.h
@@ -24,18 +24,22 @@
#include <mutex>
#include <optional>
-#include "RobotAPI/libraries/armem/client.h"
-#include "RobotAPI/libraries/armem/client/Reader.h"
+#include <VirtualRobot/VirtualRobot.h>
+
+#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
+
+#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
+#include <RobotAPI/libraries/armem/client/Reader.h>
#include "interfaces.h"
namespace armarx::armem::articulated_object
{
- class Reader:
- virtual public ReaderInterface
+
+ class Reader : virtual public ReaderInterface
{
public:
- Reader(armem::ClientReaderComponentPluginUser& component);
+ Reader(armem::client::MemoryNameSystem& memoryNameSystem);
virtual ~Reader() = default;
void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def);
@@ -43,40 +47,51 @@ namespace armarx::armem::articulated_object
void synchronize(ArticulatedObject& obj, const armem::Time& timestamp) override;
- std::optional<ArticulatedObject> get(const std::string& name, const armem::Time& timestamp) override;
- ArticulatedObject get(const ArticulatedObjectDescription& description, const armem::Time& timestamp) override;
+ std::optional<ArticulatedObject> get(const std::string& name,
+ const armem::Time& timestamp) override;
+ ArticulatedObject get(const ArticulatedObjectDescription& description,
+ const armem::Time& timestamp) override;
- std::optional<robot::RobotState> queryState(const robot::RobotDescription& description, const armem::Time& timestamp);
- std::optional<robot::RobotDescription> queryDescription(const std::string& name, const armem::Time& timestamp);
+ std::optional<robot::RobotState> queryState(const robot::RobotDescription& description,
+ const armem::Time& timestamp);
+ std::optional<robot::RobotDescription> queryDescription(const std::string& name,
+ const armem::Time& timestamp);
std::vector<robot::RobotDescription> queryDescriptions(const armem::Time& timestamp);
+ std::string getProviderName() const;
+ void setProviderName(const std::string& providerName);
+
// TODO(fabian.reister): register property defs
protected:
- std::optional<robot::RobotState> getRobotState(const armarx::armem::wm::Memory& memory) const;
- std::optional<robot::RobotDescription> getRobotDescription(const armarx::armem::wm::Memory& memory) const;
- std::vector<robot::RobotDescription> getRobotDescriptions(const armarx::armem::wm::Memory& memory) const;
+ std::optional<robot::RobotState>
+ getRobotState(const armarx::armem::wm::Memory& memory) const;
+ std::optional<robot::RobotDescription>
+ getRobotDescription(const armarx::armem::wm::Memory& memory) const;
+ std::vector<robot::RobotDescription>
+ getRobotDescriptions(const armarx::armem::wm::Memory& memory) const;
private:
- void updateKnownObjects(const armem::MemoryID& subscriptionID, const std::vector<armem::MemoryID>& snapshotIDs);
+ void updateKnownObjects(const armem::MemoryID& subscriptionID,
+ const std::vector<armem::MemoryID>& snapshotIDs);
void updateKnownObject(const armem::MemoryID& snapshotId);
struct Properties
{
- std::string memoryName = "Object";
- std::string coreInstanceSegmentName = "ArticulatedObjectInstance";
- std::string coreClassSegmentName = "ArticulatedObjectClass";
- std::string providerName = "ArmarXObjects";
+ std::string memoryName = "Object";
+ std::string coreInstanceSegmentName = "Instance";
+ std::string coreClassSegmentName = "Class";
+ std::string providerName = "PriorKnowledgeData";
} properties;
const std::string propertyPrefix = "mem.obj.articulated.";
+ armem::client::MemoryNameSystem& memoryNameSystem;
+
armem::client::Reader memoryReader;
std::mutex memoryWriterMutex;
-
- armem::ClientReaderComponentPluginUser& component;
};
-} // namespace armarx::armem::articulated_object
\ No newline at end of file
+} // namespace armarx::armem::articulated_object
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.cpp b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.cpp
index 9062747de124ac3db5ba37e3f14f2b8cdfc7b692..237bc31cc8827f0fb9f80284bf0c639c23d093ec 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.cpp
@@ -1,24 +1,38 @@
#include "Writer.h"
-#include <IceUtil/Time.h>
-#include <SimoxUtility/algorithm/get_map_keys_values.h>
#include <mutex>
#include <optional>
-#include "ArmarXCore/core/logging/Logging.h"
+#include <IceUtil/Time.h>
-#include "RobotAPI/libraries/armem/core/MemoryID.h"
-#include "RobotAPI/libraries/armem_objects/aron_conversions.h"
-#include "RobotAPI/libraries/armem_robot/aron_conversions.h"
-#include <RobotAPI/libraries/armem_robot/aron/RobotDescription.aron.generated.h>
-#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
+#include <SimoxUtility/algorithm/get_map_keys_values.h>
+
+#include "ArmarXCore/core/exceptions/LocalException.h"
+#include <ArmarXCore/core/logging/Logging.h>
+
+#include "RobotAPI/libraries/ArmarXObjects/ObjectID.h"
+#include "RobotAPI/libraries/ArmarXObjects/ObjectInfo.h"
+#include <RobotAPI/libraries/ArmarXObjects/aron/ObjectID.aron.generated.h>
+#include <RobotAPI/libraries/armem/client/query.h>
+#include <RobotAPI/libraries/armem/core/MemoryID.h>
#include <RobotAPI/libraries/armem/core/aron_conversions.h>
-#include "RobotAPI/libraries/armem_robot/robot_conversions.h"
+#include <RobotAPI/libraries/armem/core/error.h>
+#include <RobotAPI/libraries/armem_objects/aron/ObjectClass.aron.generated.h>
+#include <RobotAPI/libraries/armem_objects/aron/ObjectInstance.aron.generated.h>
+#include <RobotAPI/libraries/armem_objects/aron_conversions.h>
+#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot/aron/RobotDescription.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot/aron_conversions.h>
+#include <RobotAPI/libraries/armem_robot/robot_conversions.h>
+#include "utils.h"
namespace armarx::armem::articulated_object
{
- Writer::Writer(armem::ClientComponentPluginUser& component): component(component) {}
+ Writer::Writer(armem::client::MemoryNameSystem& memoryNameSystem) :
+ memoryNameSystem(memoryNameSystem)
+ {
+ }
void Writer::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
{
@@ -34,6 +48,8 @@ namespace armarx::armem::articulated_object
def->optional(properties.coreClassSegmentName,
prefix + "CoreSegment",
"Name of the memory core segment to use for object classes.");
+
+ ARMARX_IMPORTANT << "Writer: add property '" << prefix << "ProviderName'";
def->required(properties.providerName, prefix + "ProviderName", "Name of this provider");
}
@@ -41,19 +57,20 @@ namespace armarx::armem::articulated_object
{
// Wait for the memory to become available and add it as dependency.
ARMARX_IMPORTANT << "Writer: Waiting for memory '" << properties.memoryName << "' ...";
- auto result = component.useMemory(properties.memoryName);
- if (not result.success)
+ try
{
- ARMARX_ERROR << result.errorMessage;
+ memoryWriter = memoryNameSystem.useWriter(properties.memoryName);
+ memoryReader = memoryNameSystem.useReader(properties.memoryName);
+ ARMARX_IMPORTANT << "Writer: Connected to memory '" << properties.memoryName << "'";
+ }
+ catch (const armem::error::CouldNotResolveMemoryServer& e)
+ {
+ ARMARX_ERROR << e.what();
return;
}
- ARMARX_IMPORTANT << "Writer: Connected to memory '" << properties.memoryName;
-
- memoryWriter.setWritingMemory(result.proxy);
- memoryReader.setReadingMemory(result.proxy);
-
- const auto resultCoreClassSegment = memoryWriter.addSegment(properties.coreClassSegmentName, properties.providerName);
+ const auto resultCoreClassSegment =
+ memoryWriter.addSegment(properties.coreClassSegmentName, properties.providerName);
const auto resultCoreInstanceSegmentName =
memoryWriter.addSegment(properties.coreInstanceSegmentName, properties.providerName);
@@ -64,7 +81,7 @@ namespace armarx::armem::articulated_object
id.setCoreSegmentID(refId); // listen to all provider segments!
updateKnownObjects();
- memoryReader.subscribe(id, this, &Writer::updateKnownObjects);
+ memoryNameSystem.subscribe(id, this, &Writer::updateKnownObjects);
}
void Writer::updateKnownObject(const armem::MemoryID& snapshotId)
@@ -76,7 +93,8 @@ namespace armarx::armem::articulated_object
// TODO(fabian.reister): implement
}
- void Writer::updateKnownObjects(const armem::MemoryID& subscriptionID, const std::vector<armem::MemoryID>& snapshotIDs)
+ void Writer::updateKnownObjects(const armem::MemoryID& subscriptionID,
+ const std::vector<armem::MemoryID>& snapshotIDs)
{
ARMARX_INFO << "New objects available!";
updateKnownObjects();
@@ -85,6 +103,8 @@ namespace armarx::armem::articulated_object
void Writer::updateKnownObjects()
{
knownObjects = queryDescriptions(IceUtil::Time::now());
+
+ ARMARX_INFO << "Known articulated objects " << simox::alg::get_keys(knownObjects);
}
std::optional<armem::MemoryID> Writer::storeOrGetClass(const ArticulatedObject& obj)
@@ -97,6 +117,8 @@ namespace armarx::armem::articulated_object
return objectId->second;
}
+ throw LocalException("articulated object class " + obj.description.name + " not found");
+
// otherwise create
if (properties.allowClassCreation)
{
@@ -113,7 +135,8 @@ namespace armarx::armem::articulated_object
ARMARX_DEBUG << "Trying to create core segment + provider segment";
// TODO(fabian.reister): variable provider segment
- const auto result = memoryWriter.addSegment(properties.coreClassSegmentName, properties.providerName);
+ const auto result =
+ memoryWriter.addSegment(properties.coreClassSegmentName, properties.providerName);
if (not result.success)
{
@@ -125,9 +148,7 @@ namespace armarx::armem::articulated_object
const auto providerId = armem::MemoryID(result.segmentID);
const auto entityID =
- providerId
- .withEntityName(obj.description.name)
- .withTimestamp(timestamp);
+ providerId.withEntityName(obj.description.name).withTimestamp(timestamp);
armem::EntityUpdate update;
update.entityID = entityID;
@@ -155,47 +176,61 @@ namespace armarx::armem::articulated_object
return updateResult.snapshotID;
}
- bool Writer::storeInstance(const ArticulatedObject& obj)
+ std::string Writer::getProviderName() const
{
- std::lock_guard g{memoryWriterMutex};
-
- ARMARX_DEBUG << "Trying to create core segment + provider segment";
+ return properties.providerName;
+ }
- const auto result =
- memoryWriter.addSegment(properties.coreInstanceSegmentName, properties.providerName);
+ void Writer::setProviderName(const std::string& providerName)
+ {
+ this->properties.providerName = providerName;
+ }
- if (not result.success)
- {
- ARMARX_ERROR << "Creating core segment failed. Reason: " << result.errorMessage;
- return false;
- }
+ bool Writer::storeInstance(const ArticulatedObject& obj)
+ {
+ std::lock_guard g{memoryWriterMutex};
const auto& timestamp = obj.timestamp;
- const auto providerId = armem::MemoryID(result.segmentID);
- const auto entityID =
- providerId
- .withEntityName(obj.description.name)
- .withTimestamp(timestamp);
+ const auto providerId = armem::MemoryID()
+ .withMemoryName(properties.memoryName)
+ .withCoreSegmentName(properties.coreInstanceSegmentName)
+ .withProviderSegmentName(properties.providerName);
armem::EntityUpdate update;
- update.entityID = entityID;
+ update.entityID = providerId.withEntityName(obj.description.name);
+ // .withTimestamp(timestamp); // You only need to specify the entity ID, not the snapshot ID
+
+ // arondto::Robot aronArticulatedObject;
+ // robot::toAron(aronArticulatedObject, obj);
- arondto::Robot aronArticulatedObject;
- robot::toAron(aronArticulatedObject, obj);
+ arondto::ObjectInstance objectInstance;
+ toAron(objectInstance, obj.config);
- const auto descriptionId = storeOrGetClass(obj);
+ const auto classId = storeOrGetClass(obj);
- if (not descriptionId)
+ if (not classId)
{
ARMARX_WARNING << "Could not get class for object " << obj.description.name;
return false;
}
// install memory link
- toAron(aronArticulatedObject.description, *descriptionId);
+ toAron(objectInstance.classID, *classId);
+
+ armem::MemoryID id;
+ id.setEntityID(classId->getEntityID());
+
+ armarx::ObjectID objectId(id.entityName);
+
+ armarx::arondto::ObjectID cs;
+ cs.className = objectId.className();
+ cs.instanceName = objectId.instanceName();
+ cs.dataset = objectId.dataset();
- update.instancesData = {aronArticulatedObject.toAron()};
+ objectInstance.pose.objectID = cs;
+
+ update.instancesData = {objectInstance.toAron()};
update.timeCreated = timestamp;
ARMARX_DEBUG << "Committing " << update << " at time " << timestamp;
@@ -222,18 +257,12 @@ namespace armarx::armem::articulated_object
return false;
}
- // TODO(fabian.reister): integrate memory link
return storeInstance(obj);
}
- // const std::string& Writer::getPropertyPrefix() const
- // {
- // return propertyPrefix;
- // }
-
-
// TODO this is a duplicate
- std::optional<robot::RobotDescription> Writer::getRobotDescription(const armarx::armem::wm::Memory& memory) const
+ std::optional<robot::RobotDescription>
+ Writer::getRobotDescription(const armarx::armem::wm::Memory& memory) const
{
// clang-format off
const armem::wm::ProviderSegment& providerSegment = memory
@@ -259,14 +288,16 @@ namespace armarx::armem::articulated_object
// TODO(fabian.reister): check if 0 available
const armem::wm::EntityInstance& instance = entitySnapshots.front().getInstance(0);
- return robot::convertRobotDescription(instance);
+ return convertRobotDescription(instance);
}
- std::unordered_map<std::string, armem::MemoryID>Writer::getRobotDescriptions(const armarx::armem::wm::Memory& memory) const
+ std::unordered_map<std::string, armem::MemoryID>
+ Writer::getRobotDescriptions(const armarx::armem::wm::Memory& memory) const
{
std::unordered_map<std::string, armem::MemoryID> descriptions;
- const armem::wm::CoreSegment& coreSegment = memory.getCoreSegment(properties.coreClassSegmentName);
+ const armem::wm::CoreSegment& coreSegment =
+ memory.getCoreSegment(properties.coreClassSegmentName);
for (const auto& [providerName, providerSegment] : coreSegment.providerSegments())
{
@@ -278,11 +309,11 @@ namespace armarx::armem::articulated_object
continue;
}
- const auto entitySnapshots = simox::alg::get_values(entity.history());
+ const auto entitySnapshots = simox::alg::get_values(entity.history());
const armem::wm::EntitySnapshot& sn = entitySnapshots.front();
const armem::wm::EntityInstance& instance = sn.getInstance(0);
- const auto robotDescription = robot::convertRobotDescription(instance);
+ const auto robotDescription = convertRobotDescription(instance);
if (robotDescription)
{
@@ -290,14 +321,13 @@ namespace armarx::armem::articulated_object
descriptions.insert({robotDescription->name, snapshotID});
}
}
-
}
return descriptions;
}
-
- std::unordered_map<std::string, armem::MemoryID> Writer::queryDescriptions(const armem::Time& timestamp)
+ std::unordered_map<std::string, armem::MemoryID>
+ Writer::queryDescriptions(const armem::Time& timestamp)
{
// Query all entities from provider.
armem::client::query::Builder qb;
@@ -322,5 +352,4 @@ namespace armarx::armem::articulated_object
return getRobotDescriptions(qResult.memory);
}
-
-} // namespace armarx::armem::articulated_object
\ No newline at end of file
+} // namespace armarx::armem::articulated_object
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.h b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.h
index f2031dd402e546a21a80a17c28f8b2e4107f9059..95495220780d95278685c8c349462708aa822a3e 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.h
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.h
@@ -23,9 +23,11 @@
#include <mutex>
-#include "RobotAPI/libraries/armem/client/Reader.h"
-#include "RobotAPI/libraries/armem/client/Writer.h"
-#include "RobotAPI/libraries/armem/client.h"
+#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
+
+#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
+#include <RobotAPI/libraries/armem/client/Reader.h>
+#include <RobotAPI/libraries/armem/client/Writer.h>
#include "interfaces.h"
@@ -37,10 +39,9 @@ namespace armarx::armem::articulated_object
virtual public WriterInterface
{
public:
- Writer(armem::ClientComponentPluginUser& component);
+ Writer(armem::client::MemoryNameSystem& memoryNameSystem);
virtual ~Writer() = default;
-
void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def);
void connect();
@@ -52,7 +53,12 @@ namespace armarx::armem::articulated_object
// const std::string& getPropertyPrefix() const override;
+ std::string getProviderName() const;
+ void setProviderName(const std::string& providerName);
+
+
private:
+
std::optional<armem::MemoryID> storeOrGetClass(const ArticulatedObject& obj);
void updateKnownObjects(const armem::MemoryID& subscriptionID, const std::vector<armem::MemoryID>& snapshotIDs);
@@ -68,15 +74,17 @@ namespace armarx::armem::articulated_object
struct Properties
{
std::string memoryName = "Object";
- std::string coreInstanceSegmentName = "ArticulatedObjectInstance";
- std::string coreClassSegmentName = "ArticulatedObjectClass";
- std::string providerName;
+ std::string coreInstanceSegmentName = "Instance";
+ std::string coreClassSegmentName = "Class";
+ std::string providerName = "";
bool allowClassCreation = false;
} properties;
const std::string propertyPrefix = "mem.obj.articulated.";
+ armem::client::MemoryNameSystem& memoryNameSystem;
+
armem::client::Writer memoryWriter;
std::mutex memoryWriterMutex;
@@ -85,9 +93,7 @@ namespace armarx::armem::articulated_object
// key: name of object: RobotDescription::name
std::unordered_map<std::string, MemoryID> knownObjects;
-
- armem::ClientComponentPluginUser& component;
};
-} // namespace armarx::armem::articulated_object
\ No newline at end of file
+} // namespace armarx::armem::articulated_object
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/utils.cpp b/source/RobotAPI/libraries/armem_objects/client/articulated_object/utils.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c6da5e109ff356ecbe4a4f2b569c332a5e34cfcb
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/utils.cpp
@@ -0,0 +1,34 @@
+#include "utils.h"
+
+#include <RobotAPI/libraries/armem_robot/aron_conversions.h>
+
+namespace armarx::armem::articulated_object
+{
+ std::optional<robot::RobotDescription>
+ convertRobotDescription(const armem::wm::EntityInstance& instance)
+ {
+
+ arondto::ObjectClass aronObjectInfo;
+ try
+ {
+ aronObjectInfo.fromAron(instance.data());
+ }
+ catch (...)
+ {
+ ARMARX_WARNING << "Conversion to ObjectPose failed!";
+ return std::nullopt;
+ }
+
+ robot::RobotDescription robotDescription;
+ fromAron(aronObjectInfo, robotDescription);
+
+ // check if robot description is valid
+ const auto xml = robotDescription.xml.serialize();
+ if (robotDescription.name.empty() or xml.package.empty() or xml.path.empty())
+ {
+ return std::nullopt;
+ }
+
+ return robotDescription;
+ }
+} // namespace armarx::armem::articulated_object
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/utils.h b/source/RobotAPI/libraries/armem_objects/client/articulated_object/utils.h
new file mode 100644
index 0000000000000000000000000000000000000000..2e51a9e4b336bd4550027227d30a0236efa0f217
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/utils.h
@@ -0,0 +1,16 @@
+#pragma once
+
+#include <optional>
+
+#include "RobotAPI/libraries/armem/core/workingmemory/EntityInstance.h"
+#include "RobotAPI/libraries/armem_robot/types.h"
+#include <RobotAPI/libraries/armem_objects/aron/ObjectClass.aron.generated.h>
+
+
+namespace armarx::armem::articulated_object
+{
+
+ std::optional<robot::RobotDescription>
+ convertRobotDescription(const armem::wm::EntityInstance& instance);
+
+} // namespace armarx::armem::articulated_object
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/armem_objects/client/attachment/Reader.cpp b/source/RobotAPI/libraries/armem_objects/client/attachment/Reader.cpp
index e3ce169b9ef41292b7ad565f94b74faf141ee7ad..f723f30b2eb2d704dcfae73f47e9351cf659b7ad 100644
--- a/source/RobotAPI/libraries/armem_objects/client/attachment/Reader.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/attachment/Reader.cpp
@@ -3,19 +3,21 @@
#include <mutex>
#include <optional>
-#include "ArmarXCore/core/logging/Logging.h"
+#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/PackagePath.h>
-#include "RobotAPI/libraries/armem/core/Time.h"
-#include "RobotAPI/libraries/armem/client/query/Builder.h"
-#include "RobotAPI/libraries/armem/core/workingmemory/CoreSegment.h"
-#include "RobotAPI/libraries/armem/util/util.h"
-#include "RobotAPI/libraries/armem_robot/robot_conversions.h"
-#include "RobotAPI/libraries/armem_robot/aron_conversions.h"
+#include <RobotAPI/libraries/armem/core/Time.h>
+#include <RobotAPI/libraries/armem/core/error.h>
+#include <RobotAPI/libraries/armem/client/query/Builder.h>
+#include <RobotAPI/libraries/armem/core/workingmemory/CoreSegment.h>
+#include <RobotAPI/libraries/armem/util/util.h>
+#include <RobotAPI/libraries/armem_robot/robot_conversions.h>
+#include <RobotAPI/libraries/armem_robot/aron_conversions.h>
#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
#include <RobotAPI/libraries/armem_objects/aron/Attachment.aron.generated.h>
#include <RobotAPI/libraries/armem_objects/aron_conversions.h>
-#include "RobotAPI/libraries/aron/common/aron_conversions.h"
+#include <RobotAPI/libraries/aron/common/aron_conversions.h>
+
namespace armarx::armem::attachment
{
@@ -72,7 +74,9 @@ namespace armarx::armem::attachment
}
} // namespace
- Reader::Reader(armem::ClientReaderComponentPluginUser& component) : component(component) {}
+ Reader::Reader(armem::client::MemoryNameSystem& memoryNameSystem) :
+ memoryNameSystem(memoryNameSystem)
+ {}
void Reader::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
{
@@ -92,16 +96,16 @@ namespace armarx::armem::attachment
{
// Wait for the memory to become available and add it as dependency.
ARMARX_IMPORTANT << "Reader: Waiting for memory '" << properties.memoryName << "' ...";
- auto result = component.useMemory(properties.memoryName);
- if (not result.success)
+ try
+ {
+ memoryReader = memoryNameSystem.useReader(properties.memoryName);
+ ARMARX_IMPORTANT << "Reader: Connected to memory '" << properties.memoryName << "'";
+ }
+ catch (const armem::error::CouldNotResolveMemoryServer& e)
{
- ARMARX_ERROR << result.errorMessage;
+ ARMARX_ERROR << e.what();
return;
}
-
- ARMARX_IMPORTANT << "Reader: Connected to memory '" << properties.memoryName;
-
- memoryReader.setReadingMemory(result.proxy);
}
@@ -207,4 +211,4 @@ namespace armarx::armem::attachment
-} // namespace armarx::armem::attachment
\ No newline at end of file
+} // namespace armarx::armem::attachment
diff --git a/source/RobotAPI/libraries/armem_objects/client/attachment/Reader.h b/source/RobotAPI/libraries/armem_objects/client/attachment/Reader.h
index bcfb3d1a135a2446f255a0038a2aaad85c2a3412..1080156c8bbbdb95afc62c0467d53dbe700c49f9 100644
--- a/source/RobotAPI/libraries/armem_objects/client/attachment/Reader.h
+++ b/source/RobotAPI/libraries/armem_objects/client/attachment/Reader.h
@@ -24,16 +24,19 @@
#include <mutex>
#include <optional>
-#include "RobotAPI/libraries/armem/client.h"
-#include "RobotAPI/libraries/armem/client/Reader.h"
-#include "RobotAPI/libraries/armem_objects/types.h"
+#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
+
+#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
+#include <RobotAPI/libraries/armem/client/Reader.h>
+#include <RobotAPI/libraries/armem_objects/types.h>
+
namespace armarx::armem::attachment
{
class Reader
{
public:
- Reader(armem::ClientReaderComponentPluginUser& component);
+ Reader(armem::client::MemoryNameSystem& memoryNameSystem);
virtual ~Reader() = default;
void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def);
@@ -56,11 +59,10 @@ namespace armarx::armem::attachment
const std::string propertyPrefix = "mem.obj.attachment.";
+ armem::client::MemoryNameSystem& memoryNameSystem;
armem::client::Reader memoryReader;
std::mutex memoryWriterMutex;
-
- armem::ClientReaderComponentPluginUser& component;
};
-} // namespace armarx::armem::attachment
\ No newline at end of file
+} // namespace armarx::armem::attachment
diff --git a/source/RobotAPI/libraries/armem_objects/client/attachment/Writer.cpp b/source/RobotAPI/libraries/armem_objects/client/attachment/Writer.cpp
index f4d16070b84e4d29aca1742ce0356fb0c69ea5b0..69f5806c2bb6dc4cbfae1f196d4f967c04ab9a12 100644
--- a/source/RobotAPI/libraries/armem_objects/client/attachment/Writer.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/attachment/Writer.cpp
@@ -5,20 +5,23 @@
#include <mutex>
#include <optional>
-#include "ArmarXCore/core/logging/Logging.h"
+#include <ArmarXCore/core/logging/Logging.h>
-#include "RobotAPI/libraries/armem/core/MemoryID.h"
-#include "RobotAPI/libraries/armem_objects/aron_conversions.h"
-#include "RobotAPI/libraries/armem_robot/aron_conversions.h"
+#include <RobotAPI/libraries/armem/core/MemoryID.h>
+#include <RobotAPI/libraries/armem/core/error.h>
+#include <RobotAPI/libraries/armem_objects/aron_conversions.h>
+#include <RobotAPI/libraries/armem_robot/aron_conversions.h>
#include <RobotAPI/libraries/armem_robot/aron/RobotDescription.aron.generated.h>
#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
#include <RobotAPI/libraries/armem/core/aron_conversions.h>
-#include "RobotAPI/libraries/armem_robot/robot_conversions.h"
+#include <RobotAPI/libraries/armem_robot/robot_conversions.h>
namespace armarx::armem::attachment
{
- Writer::Writer(armem::ClientComponentPluginUser& component): component(component) {}
+ Writer::Writer(armem::client::MemoryNameSystem& memoryNameSystem) :
+ memoryNameSystem(memoryNameSystem)
+ {}
void Writer::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
{
@@ -38,17 +41,17 @@ namespace armarx::armem::attachment
{
// Wait for the memory to become available and add it as dependency.
ARMARX_IMPORTANT << "Writer: Waiting for memory '" << properties.memoryName << "' ...";
- auto result = component.useMemory(properties.memoryName);
- if (not result.success)
+ try
+ {
+ memoryWriter = memoryNameSystem.useWriter(properties.memoryName);
+ memoryReader = memoryNameSystem.useReader(properties.memoryName);
+ ARMARX_IMPORTANT << "Writer: Connected to memory '" << properties.memoryName << "'";
+ }
+ catch (const armem::error::CouldNotResolveMemoryServer& e)
{
- ARMARX_ERROR << result.errorMessage;
+ ARMARX_ERROR << e.what();
return;
}
-
- ARMARX_IMPORTANT << "Writer: Connected to memory '" << properties.memoryName;
-
- memoryWriter.setWritingMemory(result.proxy);
- memoryReader.setReadingMemory(result.proxy);
}
@@ -141,4 +144,4 @@ namespace armarx::armem::attachment
-} // namespace armarx::armem::attachment
\ No newline at end of file
+} // namespace armarx::armem::attachment
diff --git a/source/RobotAPI/libraries/armem_objects/client/attachment/Writer.h b/source/RobotAPI/libraries/armem_objects/client/attachment/Writer.h
index f20421d4b27d9e2fc10e106e238a9684fca9d2cf..754f4f28041429cd1bb93c9bd3b962335c984530 100644
--- a/source/RobotAPI/libraries/armem_objects/client/attachment/Writer.h
+++ b/source/RobotAPI/libraries/armem_objects/client/attachment/Writer.h
@@ -23,13 +23,14 @@
#include <mutex>
-#include "RobotAPI/libraries/armem/client/Reader.h"
-#include "RobotAPI/libraries/armem/client/Writer.h"
-#include "RobotAPI/libraries/armem/client.h"
+#include <RobotAPI/libraries/armem/client/Reader.h>
+#include <RobotAPI/libraries/armem/client/Writer.h>
+#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
-#include "RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h"
+#include <RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h>
+
+#include <RobotAPI/libraries/armem_objects/types.h>
-#include "RobotAPI/libraries/armem_objects/types.h"
namespace armarx::armem::attachment
{
@@ -37,7 +38,7 @@ namespace armarx::armem::attachment
class Writer
{
public:
- Writer(armem::ClientComponentPluginUser& component);
+ Writer(armem::client::MemoryNameSystem& memoryNameSystem);
virtual ~Writer() = default;
@@ -60,14 +61,14 @@ namespace armarx::armem::attachment
const std::string propertyPrefix = "mem.obj.articulated.";
+ armem::client::MemoryNameSystem& memoryNameSystem;
+
armem::client::Writer memoryWriter;
std::mutex memoryWriterMutex;
armem::client::Reader memoryReader;
std::mutex memoryReaderMutex;
-
- armem::ClientComponentPluginUser& component;
};
-} // namespace armarx::armem::attachment
\ No newline at end of file
+} // namespace armarx::armem::attachment
diff --git a/source/RobotAPI/libraries/armem_objects/server/articulated_object_class/Segment.cpp b/source/RobotAPI/libraries/armem_objects/server/articulated_object_class/Segment.cpp
index 422b8e6be1fa3b50fe0ffc154ce2612528c3a578..425b5af02da13e58f4a654a0784029f5fbe9a9e0 100644
--- a/source/RobotAPI/libraries/armem_objects/server/articulated_object_class/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/articulated_object_class/Segment.cpp
@@ -9,7 +9,7 @@
#include "RobotAPI/libraries/aron/common/aron_conversions.h"
#include <RobotAPI/libraries/armem/core/aron_conversions.h>
-#include <RobotAPI/libraries/armem/core/workingmemory/Visitor.h>
+#include <RobotAPI/libraries/armem/core/workingmemory/visitor.h>
#include "RobotAPI/libraries/armem/core/MemoryID.h"
#include <RobotAPI/libraries/armem/client/Writer.h>
#include <RobotAPI/libraries/armem/client/query/Builder.h>
@@ -48,7 +48,7 @@ namespace armarx::armem::server::obj::articulated_object_class
{
ARMARX_CHECK_NOT_NULL(iceMemory.workingMemory);
- coreSegment = &iceMemory.workingMemory->addCoreSegment(p.coreClassSegmentName, arondto::RobotDescription::toInitialAronType());
+ coreSegment = &iceMemory.workingMemory->addCoreSegment(p.coreClassSegmentName, arondto::RobotDescription::toAronType());
coreSegment->setMaxHistorySize(p.maxHistorySize);
if (p.loadFromObjectsPackage)
@@ -89,7 +89,7 @@ namespace armarx::armem::server::obj::articulated_object_class
for (const armem::articulated_object::ArticulatedObjectDescription& desc : descriptions)
{
EntityUpdate& update = commit.updates.emplace_back();
- update.entityID = providerID.withEntityName(datasetName + "/" + desc.name);
+ update.entityID = providerID.withEntityName(desc.name);
update.timeArrived = update.timeCreated = update.timeSent = now;
arondto::RobotDescription aronRobotDescription;
diff --git a/source/RobotAPI/libraries/armem_objects/server/articulated_object_instance/Segment.cpp b/source/RobotAPI/libraries/armem_objects/server/articulated_object_instance/Segment.cpp
index c7fa562a61f81e8a09889182efe136a963da1a2a..dba8c91a83e3449fd0dc2a4bdb8ef79632a572e0 100644
--- a/source/RobotAPI/libraries/armem_objects/server/articulated_object_instance/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/articulated_object_instance/Segment.cpp
@@ -49,7 +49,7 @@ namespace armarx::armem::server::obj::articulated_object_instance
{
ARMARX_CHECK_NOT_NULL(iceMemory.workingMemory);
- coreSegment = &iceMemory.workingMemory->addCoreSegment(p.coreInstanceSegmentName, arondto::Robot::toInitialAronType());
+ coreSegment = &iceMemory.workingMemory->addCoreSegment(p.coreInstanceSegmentName, arondto::Robot::toAronType());
coreSegment->setMaxHistorySize(p.maxHistorySize);
}
diff --git a/source/RobotAPI/libraries/armem_objects/server/attachments/Segment.cpp b/source/RobotAPI/libraries/armem_objects/server/attachments/Segment.cpp
index 338bc804b12c0569542e57ed283741e4145893a5..f2490c841d0042a133fb1c8cf58740eb2af614cd 100644
--- a/source/RobotAPI/libraries/armem_objects/server/attachments/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/attachments/Segment.cpp
@@ -9,7 +9,7 @@
#include "RobotAPI/libraries/aron/common/aron_conversions.h"
#include <RobotAPI/libraries/armem/core/aron_conversions.h>
-#include <RobotAPI/libraries/armem/core/workingmemory/Visitor.h>
+#include <RobotAPI/libraries/armem/core/workingmemory/visitor.h>
#include "RobotAPI/libraries/armem/core/MemoryID.h"
#include <RobotAPI/libraries/armem/client/Writer.h>
#include <RobotAPI/libraries/armem/client/query/Builder.h>
@@ -23,7 +23,6 @@
namespace armarx::armem::server::obj::attachments
{
-
Segment::Segment(armem::server::MemoryToIceAdapter& memoryToIceAdapter, std::mutex& memoryMutex) :
iceMemory(memoryToIceAdapter),
memoryMutex(memoryMutex)
@@ -43,7 +42,7 @@ namespace armarx::armem::server::obj::attachments
{
ARMARX_CHECK_NOT_NULL(iceMemory.workingMemory);
- coreSegment = &iceMemory.workingMemory->addCoreSegment(p.coreClassSegmentName, arondto::Robot::toInitialAronType());
+ coreSegment = &iceMemory.workingMemory->addCoreSegment(p.coreClassSegmentName, arondto::Robot::toAronType());
coreSegment->setMaxHistorySize(p.maxHistorySize);
}
diff --git a/source/RobotAPI/libraries/armem_objects/server/class/FloorVis.h b/source/RobotAPI/libraries/armem_objects/server/class/FloorVis.h
index 4c3be719fce4148970fa0ef16d8465d2df7a10d3..dcce63977438bdd0d646b50aa8e5dd9679354df8 100644
--- a/source/RobotAPI/libraries/armem_objects/server/class/FloorVis.h
+++ b/source/RobotAPI/libraries/armem_objects/server/class/FloorVis.h
@@ -41,9 +41,9 @@ namespace armarx::armem::server::obj::clazz
{
bool show = true;
- std::string entityName = "Environment/floor-20x20";
+ std::string entityName = "Building/floor-20x20";
std::string layerName = "Floor";
- bool height = 1;
+ float height = -1;
void define(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "");
};
diff --git a/source/RobotAPI/libraries/armem_objects/server/class/Segment.cpp b/source/RobotAPI/libraries/armem_objects/server/class/Segment.cpp
index 93989e18d6c5cd39d0f878cd2caea652fadb41b3..3e6ecfed37e3d75d60b3d509f97520c7627f0fcc 100644
--- a/source/RobotAPI/libraries/armem_objects/server/class/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/class/Segment.cpp
@@ -42,7 +42,7 @@ namespace armarx::armem::server::obj::clazz
{
ARMARX_CHECK_NOT_NULL(iceMemory.workingMemory);
- coreSegment = &iceMemory.workingMemory->addCoreSegment(p.coreSegmentName, arondto::ObjectClass::toInitialAronType());
+ coreSegment = &iceMemory.workingMemory->addCoreSegment(p.coreSegmentName, arondto::ObjectClass::toAronType());
coreSegment->setMaxHistorySize(p.maxHistorySize);
if (p.loadFromObjectsPackage)
@@ -79,7 +79,10 @@ namespace armarx::armem::server::obj::clazz
std::vector<ObjectInfo> infos = objectFinder.findAllObjects(checkPaths);
const MemoryID providerID = coreSegment->id().withProviderSegmentName(objectFinder.getPackageName());
- coreSegment->addProviderSegment(providerID.providerSegmentName);
+ if (not coreSegment->hasProviderSegment(providerID.providerSegmentName))
+ {
+ coreSegment->addProviderSegment(providerID.providerSegmentName);
+ }
ARMARX_INFO << "Loading up to " << infos.size() << " object classes from '"
<< objectFinder.getPackageName() << "' ...";
@@ -169,18 +172,23 @@ namespace armarx::armem::server::obj::clazz
toAron(data.id, info.id());
- if (fs::is_regular_file(info.simoxXML().absolutePath))
- {
- toAron(data.simoxXmlPath, info.simoxXML());
- }
- if (fs::is_regular_file(info.wavefrontObj().absolutePath))
+ auto setPathIfExists = [](
+ armarx::arondto::PackagePath & aron,
+ const PackageFileLocation & location)
{
- toAron(data.meshObjPath, info.wavefrontObj());
- }
- if (fs::is_regular_file(info.file(".wrl").absolutePath))
- {
- toAron(data.meshWrlPath, info.file(".wrl"));
- }
+ if (fs::is_regular_file(location.absolutePath))
+ {
+ toAron(aron, location);
+ }
+ else
+ {
+ toAron(aron, PackageFileLocation());
+ }
+ };
+ setPathIfExists(data.simoxXmlPath, info.simoxXML());
+ setPathIfExists(data.articulatedSimoxXmlPath, info.articulatedSimoxXML());
+ setPathIfExists(data.meshObjPath, info.wavefrontObj());
+ setPathIfExists(data.meshWrlPath, info.meshWrl());
auto aabb = info.loadAABB();
toAron(data.aabb, aabb ? aabb.value() : simox::AxisAlignedBoundingBox());
@@ -225,12 +233,16 @@ namespace armarx::armem::server::obj::clazz
{
using namespace armarx::RemoteGui::Client;
+ reloadButton.setLabel("Reload");
+
maxHistorySize.setValue(std::max(1, int(segment.p.maxHistorySize)));
maxHistorySize.setRange(1, 1e6);
infiniteHistory.setValue(segment.p.maxHistorySize == -1);
GridLayout grid;
int row = 0;
+ grid.add(reloadButton, {row, 0}, {1, 2});
+ row++;
grid.add(Label("Max History Size"), {row, 0}).add(maxHistorySize, {row, 1});
row++;
grid.add(Label("Infinite History Size"), {row, 0}).add(infiniteHistory, {row, 1});
@@ -243,6 +255,12 @@ namespace armarx::armem::server::obj::clazz
void Segment::RemoteGui::Data::update(Segment& segment)
{
+ if (reloadButton.wasClicked())
+ {
+ std::scoped_lock lock(segment.memoryMutex);
+ segment.loadByObjectFinder();
+ rebuild = true;
+ }
if (infiniteHistory.hasValueChanged() || maxHistorySize.hasValueChanged())
{
std::scoped_lock lock(segment.memoryMutex);
diff --git a/source/RobotAPI/libraries/armem_objects/server/class/Segment.h b/source/RobotAPI/libraries/armem_objects/server/class/Segment.h
index 7356e6a1d9e490840f7c19b5e41e27506ddf2ecf..88eaedbefb6529b873dfe6033e6a2857672c0586 100644
--- a/source/RobotAPI/libraries/armem_objects/server/class/Segment.h
+++ b/source/RobotAPI/libraries/armem_objects/server/class/Segment.h
@@ -76,11 +76,14 @@ namespace armarx::armem::server::obj::clazz
{
armarx::RemoteGui::Client::GroupBox group;
+ armarx::RemoteGui::Client::Button reloadButton;
armarx::RemoteGui::Client::IntSpinBox maxHistorySize;
armarx::RemoteGui::Client::CheckBox infiniteHistory;
void setup(const Segment& segment);
void update(Segment& segment);
+
+ bool rebuild = false;
};
Data data;
diff --git a/source/RobotAPI/libraries/armem_objects/server/articulated_object_instance/Visu.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/ArticulatedObjectVisu.cpp
similarity index 73%
rename from source/RobotAPI/libraries/armem_objects/server/articulated_object_instance/Visu.cpp
rename to source/RobotAPI/libraries/armem_objects/server/instance/ArticulatedObjectVisu.cpp
index d1ee962acf267c3263676772816ba7d79f8e46cb..f1b90e5daec4edf42590ce0d8aad7a4aa9d83838 100644
--- a/source/RobotAPI/libraries/armem_objects/server/articulated_object_instance/Visu.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/ArticulatedObjectVisu.cpp
@@ -1,31 +1,30 @@
-#include "Visu.h"
+#include "ArticulatedObjectVisu.h"
#include <algorithm>
+#include <SimoxUtility/math/pose.h>
+
+#include "ArmarXCore/core/services/tasks/PeriodicTask.h"
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/time/CycleUtil.h>
#include <ArmarXCore/core/time/TimeUtil.h>
-#include <SimoxUtility/math/pose.h>
-
#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
-#include "ArmarXCore/core/services/tasks/PeriodicTask.h"
#include "Segment.h"
-namespace armarx::armem::server::obj::articulated_object_instance
+namespace armarx::armem::server::obj::instance
{
- void Visu::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
+ void ArticulatedObjectVisu::defineProperties(armarx::PropertyDefinitionsPtr defs,
+ const std::string& prefix)
{
- defs->optional(p.enabled, prefix + "enabled",
- "Enable or disable visualization of objects.");
- defs->optional(p.frequencyHz, prefix + "frequenzyHz",
- "Frequency of visualization.");
+ defs->optional(
+ p.enabled, prefix + "enabled", "Enable or disable visualization of objects.");
+ defs->optional(p.frequencyHz, prefix + "frequenzyHz", "Frequency of visualization.");
}
-
- viz::Layer Visu::visualizeProvider(
+ viz::Layer ArticulatedObjectVisu::visualizeProvider(
const std::string& providerName,
const armarx::armem::articulated_object::ArticulatedObjects& objects) const
{
@@ -36,12 +35,13 @@ namespace armarx::armem::server::obj::articulated_object_instance
return layer;
}
-
- void Visu::visualizeObjects(viz::Layer& layer, const armarx::armem::articulated_object::ArticulatedObjects& objects) const
+ void ArticulatedObjectVisu::visualizeObjects(
+ viz::Layer& layer,
+ const armarx::armem::articulated_object::ArticulatedObjects& objects) const
{
- const auto visualizeObject = [&](const armarx::armem::articulated_object::ArticulatedObject & obj)
+ const auto visualizeObject =
+ [&](const armarx::armem::articulated_object::ArticulatedObject & obj)
{
-
const auto xmlPath = obj.description.xml.serialize();
// clang-format off
@@ -58,40 +58,40 @@ namespace armarx::armem::server::obj::articulated_object_instance
};
std::for_each(objects.begin(), objects.end(), visualizeObject);
-
}
- void Visu::init()
+ void ArticulatedObjectVisu::init()
{
- updateTask = new PeriodicTask<Visu>(this, &Visu::visualizeRun, 1000 / p.frequencyHz);
+ updateTask = new PeriodicTask<ArticulatedObjectVisu>(this, &ArticulatedObjectVisu::visualizeRun, 1000 / p.frequencyHz);
+
+ ARMARX_INFO << "ArticulatedObjectVisu: init";
updateTask->start();
}
-
- void Visu::visualizeRun()
+ void ArticulatedObjectVisu::visualizeRun()
{
// std::scoped_lock lock(visuMutex);
- ARMARX_DEBUG << "Update task";
+ ARMARX_INFO << "Update task";
- if (not p.enabled)
- {
- return;
- }
+ // if (not p.enabled)
+ // {
+ // return;
+ // }
// TIMING_START(Visu);
const auto articulatedObjects = segment.getArticulatedObjects();
- ARMARX_DEBUG << "Found " << articulatedObjects.size() << " articulated objects";
+ ARMARX_INFO << "Found " << articulatedObjects.size() << " articulated objects";
viz::Layer layer = arviz.layer("ArticulatedObjectInstances");
- ARMARX_DEBUG << "visualizing objects";
+ ARMARX_INFO << "visualizing objects";
visualizeObjects(layer, articulatedObjects);
- ARMARX_DEBUG << "Committing objects";
+ ARMARX_INFO << "Committing objects";
arviz.commit({layer});
- ARMARX_DEBUG << "Done committing";
+ ARMARX_INFO << "Done committing";
// TIMING_END_STREAM(Visu, ARMARX_VERBOSE);
@@ -102,7 +102,6 @@ namespace armarx::armem::server::obj::articulated_object_instance
// { "t Visualize [ms]", new Variant(Visu.toMilliSecondsDouble()) },
// });
// }
-
}
// void Visu::RemoteGui::setup(const Visu& visu)
@@ -155,6 +154,4 @@ namespace armarx::armem::server::obj::articulated_object_instance
// visu.objectFramesScale = objectFramesScale.getValue();
// }
-
-
-} // namespace armarx::armem::server::obj::articulated_object_instance
+} // namespace armarx::armem::server::obj::instance
diff --git a/source/RobotAPI/libraries/armem_objects/server/articulated_object_instance/Visu.h b/source/RobotAPI/libraries/armem_objects/server/instance/ArticulatedObjectVisu.h
similarity index 81%
rename from source/RobotAPI/libraries/armem_objects/server/articulated_object_instance/Visu.h
rename to source/RobotAPI/libraries/armem_objects/server/instance/ArticulatedObjectVisu.h
index 3590270fe545c830b4907082dd127d8a44598587..9c66fd7fb613d41e36f65fe1d8273dc8bd8b0e13 100644
--- a/source/RobotAPI/libraries/armem_objects/server/articulated_object_instance/Visu.h
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/ArticulatedObjectVisu.h
@@ -27,56 +27,53 @@
// #include <ArmarXGui/libraries/RemoteGui/Client/Widgets.h>
#include <RobotAPI/components/ArViz/Client/Client.h>
-
#include <RobotAPI/libraries/armem_objects/types.h>
-
namespace armarx
{
class ObjectFinder;
}
-namespace armarx::armem::server::obj::articulated_object_instance
+namespace armarx::armem::server::obj::instance
{
class Segment;
/**
* @brief Visualizes articulated objects
*/
- class Visu : public armarx::Logging
+ class ArticulatedObjectVisu : public armarx::Logging
{
public:
+ ArticulatedObjectVisu(const viz::Client& arviz, Segment& segment) :
+ arviz(arviz), segment(segment)
+ {
+ }
- Visu(const viz::Client& arviz, const Segment& segment): arviz(arviz), segment(segment) {}
-
- void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "visu.");
+ void defineProperties(armarx::PropertyDefinitionsPtr defs,
+ const std::string& prefix = "visu.");
void init();
protected:
viz::Layer visualizeProvider(
const std::string& providerName,
- const armarx::armem::articulated_object::ArticulatedObjects& objects
- ) const;
+ const armarx::armem::articulated_object::ArticulatedObjects& objects) const;
void visualizeObjects(
viz::Layer& layer,
- const armarx::armem::articulated_object::ArticulatedObjects& objects
- ) const;
-
+ const armarx::armem::articulated_object::ArticulatedObjects& objects) const;
private:
viz::Client arviz;
- const Segment& segment;
+ Segment& segment;
struct Properties
{
- bool enabled = true;
+ bool enabled = true;
float frequencyHz = 25;
} p;
-
- PeriodicTask<Visu>::pointer_type updateTask;
+ PeriodicTask<ArticulatedObjectVisu>::pointer_type updateTask;
void visualizeRun();
// struct RemoteGui
@@ -95,7 +92,6 @@ namespace armarx::armem::server::obj::articulated_object_instance
// // void setup(const Visu& visu);
// // void update(Visu& visu);
// };
-
};
-} // namespace armarx::armem::server::obj::articulated_object_instance
+} // namespace armarx::armem::server::obj::instance
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Decay.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/Decay.cpp
index f07f526d60dc409bc2bbf7e2dc37389237d9ec96..df3728fb3bbc6149fb329e60a3a94083caa6a441 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/Decay.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/Decay.cpp
@@ -26,28 +26,27 @@ namespace armarx::armem::server::obj::instance
void Decay::updateConfidence(objpose::ObjectPose& pose, IceUtil::Time now) const
{
- float confidence = calculateConfidence(pose.timestamp, now);
- pose.confidence = confidence;
+ if (pose.attachment or pose.isStatic)
+ {
+ pose.confidence = 1.0;
+ }
+ else
+ {
+ pose.confidence = calculateConfidence(pose.timestamp, now);
+ }
}
void Decay::updateConfidences(objpose::ObjectPoseSeq& objectPoses, IceUtil::Time now) const
{
for (objpose::ObjectPose& pose : objectPoses)
{
- if (pose.attachment)
- {
- pose.confidence = 1.0;
- }
- else
- {
- updateConfidence(pose, now);
- }
+ updateConfidence(pose, now);
}
}
float Decay::calculateConfidence(IceUtil::Time localization, IceUtil::Time now) const
{
- float duration = (now - localization).toSeconds();
+ const float duration = static_cast<float>((now - localization).toSecondsDouble());
if (duration < delaySeconds)
{
return maxConfidence;
@@ -88,17 +87,17 @@ namespace armarx::armem::server::obj::instance
}
{
maxConfidence.setRange(0, 1);
- maxConfidence.setSteps(20);
+ maxConfidence.setSteps(100);
maxConfidence.setValue(decay.maxConfidence);
}
{
minConfidence.setRange(0, 1);
- minConfidence.setSteps(20);
+ minConfidence.setSteps(100);
minConfidence.setValue(decay.minConfidence);
}
{
removeObjectsBelowConfidence.setRange(0, 1);
- removeObjectsBelowConfidence.setSteps(20);
+ removeObjectsBelowConfidence.setSteps(100);
removeObjectsBelowConfidence.setValue(decay.removeObjectsBelowConfidence);
}
@@ -128,6 +127,7 @@ namespace armarx::armem::server::obj::instance
decay.durationSeconds = durationSeconds.getValue();
decay.maxConfidence = maxConfidence.getValue();
decay.minConfidence = minConfidence.getValue();
+ decay.removeObjectsBelowConfidence = removeObjectsBelowConfidence.getValue();
}
}
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
index aa2125dada86d895e7fe063cbdc20501623576b7..db706075c9bd5431fefd2b04b7634cf5e8ff6747 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
@@ -1,26 +1,44 @@
#include "Segment.h"
+#include <RobotAPI/libraries/armem_objects/aron/ObjectClass.aron.generated.h>
#include <RobotAPI/libraries/armem_objects/aron_conversions.h>
+#include <RobotAPI/libraries/armem_objects/SceneSnapshot.h>
#include <RobotAPI/libraries/armem/core/aron_conversions.h>
-#include <RobotAPI/libraries/armem/core/workingmemory/Visitor.h>
+#include <RobotAPI/libraries/armem/core/error.h>
+#include <RobotAPI/libraries/armem/core/workingmemory/visitor.h>
#include <RobotAPI/libraries/armem/client/Writer.h>
#include <RobotAPI/libraries/armem/client/query/Builder.h>
#include <RobotAPI/libraries/armem/client/query/query_fns.h>
+#include <RobotAPI/libraries/armem/util/util.h>
+
+#include <RobotAPI/libraries/aron/common/aron_conversions.h>
#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
#include <RobotAPI/libraries/ArmarXObjects/aron_conversions.h>
#include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h>
#include <RobotAPI/libraries/ArmarXObjects/aron/ObjectPose.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot/aron_conversions.h>
+
#include <RobotAPI/libraries/core/FramedPose.h>
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
#include <ArmarXCore/core/time/TimeUtil.h>
+#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
+
+#include <SimoxUtility/algorithm/get_map_keys_values.h>
+#include <SimoxUtility/json.h>
+#include <SimoxUtility/math/pose/pose.h>
+
+
+#include <Eigen/Geometry>
+#include <IceUtil/Time.h>
#include <sstream>
+
namespace armarx::armem::server::obj::instance
{
@@ -37,6 +55,7 @@ namespace armarx::armem::server::obj::instance
{
try
{
+ objectInfo->setLogError(false); // Don't log missing files
return objectInfo->loadOOBB();
}
catch (const std::ios_base::failure& e)
@@ -59,6 +78,7 @@ namespace armarx::armem::server::obj::instance
});
}
+
void Segment::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
{
defs->optional(p.coreSegmentName, prefix + "CoreSegmentName", "Name of the object instance core segment.");
@@ -67,15 +87,39 @@ namespace armarx::armem::server::obj::instance
"If true, no new snapshots are stored while an object is attached to a robot node.\n"
"If false, new snapshots are stored, but the attachment is kept in the new snapshots.");
+ defs->optional(p.sceneSnapshotsPackage, prefix + "scene.10_Package",
+ "ArmarX package containing the scene snapshots.\n"
+ "Scene snapshots are expected to be located in Package/data/Package/Scenes/*.json.");
+ defs->optional(p.sceneSnapshotsDirectory, prefix + "scene.11_Directory",
+ "Directory in Package/data/Package/ containing the scene snapshots.");
+ defs->optional(p.sceneSnapshotToLoad, prefix + "scene.12_SnapshotToLoad",
+ "Scene snapshot to load on startup (e.g. 'Scene_2021-06-24_20-20-03').\n"
+ "You can also specify paths relative to 'Package/scenes/'.");
+
decay.defineProperties(defs, prefix + "decay.");
}
+
void Segment::init()
{
ARMARX_CHECK_NOT_NULL(iceMemory.workingMemory);
- coreSegment = &iceMemory.workingMemory->addCoreSegment(p.coreSegmentName, arondto::ObjectInstance::toInitialAronType());
+ coreSegment = &iceMemory.workingMemory->addCoreSegment(p.coreSegmentName, arondto::ObjectInstance::toAronType());
coreSegment->setMaxHistorySize(p.maxHistorySize);
+
+
+ if (not p.sceneSnapshotToLoad.empty())
+ {
+ bool lockMemory = false;
+ commitSceneSnapshotFromFilename(p.sceneSnapshotToLoad, lockMemory);
+ }
+ }
+
+ void Segment::connect(viz::Client arviz)
+ {
+ // ARMARX_INFO << "ArticulatedObjectVisu";
+ // this->visu = std::make_unique<ArticulatedObjectVisu>(arviz, *this);
+ // visu->init();
}
@@ -156,41 +200,45 @@ namespace armarx::armem::server::obj::instance
}
}
- commitObjectPoses(providerName, newObjectPoses);
+ commitObjectPoses(newObjectPoses, providerName);
return stats;
}
- void Segment::commitObjectPoses(const std::string& providerName, const ObjectPoseSeq& objectPoses)
+
+ void Segment::commitObjectPoses(const ObjectPoseSeq& objectPoses, const std::string& providerName)
{
ARMARX_CHECK_NOT_NULL(coreSegment);
+ Time now = TimeUtil::GetTime();
- // Update memory.
- const MemoryID providerSegmentID = coreSegment->id().withProviderSegmentName(providerName);
- if (!coreSegment->hasProviderSegment(providerSegmentID.providerSegmentName))
- {
- coreSegment->addProviderSegment(providerSegmentID.providerSegmentName);
- }
+ const MemoryID coreSegmentID = coreSegment->id();
Commit commit;
for (const objpose::ObjectPose& pose : objectPoses)
{
EntityUpdate& update = commit.updates.emplace_back();
- {
- update.entityID = providerSegmentID.withEntityName(pose.objectID.str());
- update.timeArrived = TimeUtil::GetTime();
- update.timeCreated = pose.timestamp;
- update.confidence = pose.confidence;
- arondto::ObjectInstance dto;
- toAron(dto, pose);
- // Search for object class.
- if (auto instance = findClassInstance(pose.objectID))
- {
- toAron(dto.classID, instance->id());
- }
- update.instancesData.push_back(dto.toAron());
+ const MemoryID providerID = coreSegmentID.withProviderSegmentName(
+ providerName.empty() ? pose.providerName : providerName);
+
+ update.entityID = providerID.withEntityName(pose.objectID.str());
+ update.timeArrived = now;
+ update.timeCreated = pose.timestamp;
+ update.confidence = pose.confidence;
+
+ arondto::ObjectInstance dto;
+ toAron(dto, pose);
+ // Search for object class.
+ if (auto instance = findClassInstance(pose.objectID))
+ {
+ toAron(dto.classID, instance->id());
+ }
+ else
+ {
+ toAron(dto.classID, MemoryID());
}
+ toAron(dto.sourceID, MemoryID());
+ update.instancesData.push_back(dto.toAron());
}
iceMemory.commit(commit);
@@ -203,30 +251,34 @@ namespace armarx::armem::server::obj::instance
return *coreSegment;
}
+
const wm::CoreSegment& Segment::getCoreSegment() const
{
ARMARX_CHECK_NOT_NULL(coreSegment);
return *coreSegment;
}
- objpose::ObjectPoseSeq Segment::getObjectPoses(IceUtil::Time now)
+
+ objpose::ObjectPoseMap Segment::getObjectPoses(IceUtil::Time now)
{
- ObjectPoseSeq objectPoses = getLatestObjectPoses();
+ ObjectPoseMap objectPoses = getLatestObjectPoses();
updateObjectPoses(objectPoses, now);
return filterObjectPoses(objectPoses);
}
- objpose::ObjectPoseSeq Segment::getObjectPosesByProvider(
+
+ objpose::ObjectPoseMap Segment::getObjectPosesByProvider(
const std::string& providerName,
IceUtil::Time now)
{
ARMARX_CHECK_NOT_NULL(coreSegment);
- ObjectPoseSeq objectPoses = getLatestObjectPoses(coreSegment->getProviderSegment(providerName));
+ ObjectPoseMap objectPoses = getLatestObjectPoses(coreSegment->getProviderSegment(providerName));
updateObjectPoses(objectPoses, now);
return filterObjectPoses(objectPoses);
}
+
armem::wm::Entity* Segment::findObjectEntity(const ObjectID& objectID, const std::string& providerName)
{
ARMARX_CHECK_NOT_NULL(coreSegment);
@@ -258,11 +310,11 @@ namespace armarx::armem::server::obj::instance
}
- void Segment::updateObjectPoses(ObjectPoseSeq& objectPoses, IceUtil::Time now)
+ void Segment::updateObjectPoses(ObjectPoseMap& objectPoses, IceUtil::Time now)
{
bool agentSynchronized = false;
- for (ObjectPose& objectPose : objectPoses)
+ for (auto& [id, objectPose] : objectPoses)
{
updateObjectPose(objectPose, now, robot, agentSynchronized);
}
@@ -270,14 +322,14 @@ namespace armarx::armem::server::obj::instance
void Segment::updateObjectPoses(
- ObjectPoseSeq& objectPoses,
+ ObjectPoseMap& objectPoses,
IceUtil::Time now,
VirtualRobot::RobotPtr agent,
bool& agentSynchronized) const
{
- for (ObjectPose& pose : objectPoses)
+ for (auto& [id, objectPose] : objectPoses)
{
- updateObjectPose(pose, now, agent, agentSynchronized);
+ updateObjectPose(objectPose, now, agent, agentSynchronized);
}
}
@@ -297,14 +349,14 @@ namespace armarx::armem::server::obj::instance
}
- objpose::ObjectPoseSeq Segment::filterObjectPoses(const ObjectPoseSeq& objectPoses) const
+ Segment::ObjectPoseMap Segment::filterObjectPoses(const ObjectPoseMap& objectPoses) const
{
- ObjectPoseSeq result;
- for (const ObjectPose& objectPose : objectPoses)
+ ObjectPoseMap result;
+ for (const auto& [id, objectPose] : objectPoses)
{
if (!(decay.enabled && objectPose.confidence < decay.removeObjectsBelowConfidence))
{
- result.push_back(objectPose);
+ result[id] = objectPose;
}
}
return result;
@@ -329,26 +381,30 @@ namespace armarx::armem::server::obj::instance
objectPose.updateAttached(agent);
}
- objpose::ObjectPoseSeq Segment::getLatestObjectPoses() const
+
+ objpose::ObjectPoseMap Segment::getLatestObjectPoses() const
{
ARMARX_CHECK_NOT_NULL(coreSegment);
return getLatestObjectPoses(*coreSegment);
}
- objpose::ObjectPoseSeq Segment::getLatestObjectPoses(const armem::wm::CoreSegment& coreSeg)
+
+ objpose::ObjectPoseMap Segment::getLatestObjectPoses(const armem::wm::CoreSegment& coreSeg)
{
- ObjectPoseSeq result;
+ ObjectPoseMap result;
getLatestObjectPoses(coreSeg, result);
return result;
}
- objpose::ObjectPoseSeq Segment::getLatestObjectPoses(const armem::wm::ProviderSegment& provSeg)
+
+ objpose::ObjectPoseMap Segment::getLatestObjectPoses(const armem::wm::ProviderSegment& provSeg)
{
- ObjectPoseSeq result;
+ ObjectPoseMap result;
getLatestObjectPoses(provSeg, result);
return result;
}
+
objpose::ObjectPose Segment::getLatestObjectPose(const armem::wm::Entity& entity)
{
ObjectPose result;
@@ -356,7 +412,8 @@ namespace armarx::armem::server::obj::instance
return result;
}
- void Segment::getLatestObjectPoses(const armem::wm::CoreSegment& coreSeg, ObjectPoseSeq& out)
+
+ void Segment::getLatestObjectPoses(const armem::wm::CoreSegment& coreSeg, ObjectPoseMap& out)
{
for (const auto& [_, provSegment] : coreSeg)
{
@@ -364,18 +421,29 @@ namespace armarx::armem::server::obj::instance
}
}
- void Segment::getLatestObjectPoses(const armem::wm::ProviderSegment& provSegment, ObjectPoseSeq& out)
+
+ void Segment::getLatestObjectPoses(const armem::wm::ProviderSegment& provSegment, ObjectPoseMap& out)
{
for (const auto& [_, entity] : provSegment)
{
if (!entity.empty())
{
- ObjectPose& pose = out.emplace_back();
- getLatestObjectPose(entity, pose);
+ ObjectPose pose = getLatestObjectPose(entity);
+ // Try to insert. Fails and returns false if an entry already exists.
+ const auto [it, success] = out.insert({pose.objectID, pose});
+ if (!success)
+ {
+ // An entry with that ID already exists. We keep the newest.
+ if (it->second.timestamp < pose.timestamp)
+ {
+ it->second = pose;
+ }
+ }
}
}
}
+
void Segment::getLatestObjectPose(const armem::wm::Entity& entity, ObjectPose& out)
{
for (const armem::wm::EntityInstance& instance : entity.getLatestSnapshot())
@@ -402,11 +470,71 @@ namespace armarx::armem::server::obj::instance
return data;
}
+
+ ::armarx::armem::articulated_object::ArticulatedObjects Segment::getArticulatedObjects()
+ {
+ objpose::ObjectPoseMap objectPoses = getObjectPoses(IceUtil::Time::now());
+
+ ARMARX_INFO << "Found " << objectPoses.size() << " object poses";
+
+ ::armarx::armem::articulated_object::ArticulatedObjects objects;
+ for (const auto&[objectId, objectPose] : objectPoses)
+ {
+ armem::articulated_object::ArticulatedObject articulatedObject;
+ articulatedObject.config.jointMap = objectPose.objectJointValues;
+ articulatedObject.config.globalPose = objectPose.objectPoseGlobal;
+ articulatedObject.config.timestamp = objectPose.timestamp;
+ articulatedObject.instance = objectPose.objectID.instanceName();
+ articulatedObject.timestamp = objectPose.timestamp;
+
+
+ ARMARX_INFO << "Object id is " << objectId.str();
+ ARMARX_INFO << "Object id for objectPose is " << objectPose.objectID.str();
+
+ // Search for object class.
+ if (auto classInstance = findClassInstance(objectPose.objectID))
+ {
+ arondto::ObjectClass dto;
+
+ try
+ {
+ dto.fromAron(classInstance->data());
+ robot::RobotDescription description;
+
+ fromAron(dto, description);
+ articulatedObject.description = description;
+
+ }
+ catch (...)
+ {
+ ARMARX_WARNING << "Conversion failed!";
+ continue;
+ }
+ }
+ else
+ {
+ ARMARX_WARNING << "Class instance not found!";
+ continue;
+ }
+
+
+
+ if (not articulatedObject.config.jointMap.empty())
+ {
+ objects.push_back(articulatedObject);
+ }
+ }
+
+ return objects;
+ }
+
+
std::optional<simox::OrientedBoxf> Segment::getObjectOOBB(const ObjectID& id)
{
return oobbCache.get(id);
}
+
objpose::ProviderInfo Segment::getProviderInfo(const std::string& providerName)
{
try
@@ -427,7 +555,6 @@ namespace armarx::armem::server::obj::instance
}
-
objpose::AttachObjectToRobotNodeOutput
Segment::attachObjectToRobotNode(const objpose::AttachObjectToRobotNodeInput& input)
{
@@ -517,6 +644,7 @@ namespace armarx::armem::server::obj::instance
return output;
}
+
objpose::DetachObjectFromRobotNodeOutput Segment::detachObjectFromRobotNode(
const objpose::DetachObjectFromRobotNodeInput& input)
{
@@ -581,6 +709,7 @@ namespace armarx::armem::server::obj::instance
virtual bool visitEnter(armem::wm::Entity& entity) override;
};
+
bool DetachVisitor::visitEnter(armem::wm::Entity& entity)
{
const arondto::ObjectInstance data = owner.getLatestInstanceData(entity);
@@ -590,7 +719,6 @@ namespace armarx::armem::server::obj::instance
// Store non-attached pose in new snapshot.
owner.storeDetachedSnapshot(entity, data, now, commitAttachedPose);
}
-
return false; // Stop descending.
}
@@ -613,6 +741,7 @@ namespace armarx::armem::server::obj::instance
return output;
}
+
void Segment::storeDetachedSnapshot(
armem::wm::Entity& entity,
const arondto::ObjectInstance& data,
@@ -648,25 +777,213 @@ namespace armarx::armem::server::obj::instance
}
- std::optional<wm::EntityInstance> Segment::findClassInstance(const ObjectID& objectID)
+ std::optional<wm::EntityInstance> Segment::findClassInstance(const ObjectID& objectID) const
{
const ObjectID classID = { objectID.dataset(), objectID.className() };
try
{
for (const auto& [_, provSeg] : iceMemory.workingMemory->getCoreSegment("Class"))
{
- return provSeg.getEntity(classID.str()).getLatestSnapshot().getInstance(0);
+ if (provSeg.hasEntity(classID.str()))
+ {
+ return provSeg.getEntity(classID.str()).getLatestSnapshot().getInstance(0);
+ }
}
+
+ ARMARX_WARNING << "No provider segment for classID " << classID.str() << " found";
return std::nullopt;
}
- catch (const armem::error::ArMemError&)
+ catch (const armem::error::ArMemError& e)
{
// Some segment or entity did not exist.
+ ARMARX_WARNING << e.what();
return std::nullopt;
}
}
+ void Segment::storeScene(const std::string& filename, const armem::obj::SceneSnapshot& scene)
+ {
+ if (const std::optional<std::filesystem::path> path = resolveSceneFilename(filename))
+ {
+ const nlohmann::json j = scene;
+ ARMARX_INFO << "Storing scene snapshot at: \n" << path.value() << "\n\n" << j.dump(2);
+ try
+ {
+ nlohmann::write_json(path.value(), j, 2);
+ }
+ catch (const std::ios_base::failure& e)
+ {
+ ARMARX_WARNING << "Storing scene snapshot failed: \n" << e.what();
+ }
+ }
+ }
+
+
+ std::optional<armem::obj::SceneSnapshot> Segment::loadScene(const std::string& filename)
+ {
+ if (const std::optional<std::filesystem::path> path = resolveSceneFilename(filename))
+ {
+ return loadScene(path.value());
+ }
+ else
+ {
+ return std::nullopt;
+ }
+ }
+
+
+ std::optional<armem::obj::SceneSnapshot> Segment::loadScene(const std::filesystem::path& path)
+ {
+ ARMARX_INFO << "Loading scene snapshot from: \n" << path;
+ nlohmann::json j;
+ try
+ {
+ j = nlohmann::read_json(path);
+ }
+ catch (const std::ios_base::failure& e)
+ {
+ ARMARX_WARNING << "Loading scene snapshot failed: \n" << e.what();
+ return std::nullopt;
+ }
+
+ armem::obj::SceneSnapshot snapshot = j;
+ return snapshot;
+ }
+
+
+ const std::string Segment::timestampPlaceholder = "%TIMESTAMP";
+
+
+ std::optional<std::filesystem::path> Segment::resolveSceneFilename(const std::string& _filename)
+ {
+ std::string filename = _filename;
+
+ filename = simox::alg::replace_all(filename, timestampPlaceholder,
+ Time::now().toString("%Y-%m-%d_%H-%M-%S"));
+ if (not simox::alg::ends_with(filename, ".json"))
+ {
+ filename += ".json";
+ }
+
+ if (!finder)
+ {
+ finder.reset(new CMakePackageFinder(p.sceneSnapshotsPackage));
+ if (not finder->packageFound())
+ {
+ ARMARX_WARNING << "Could not find CMake package " << p.sceneSnapshotsPackage << ".";
+ }
+ }
+ if (finder->packageFound())
+ {
+ std::filesystem::path dataDir = finder->getDataDir();
+ std::filesystem::path path = dataDir / p.sceneSnapshotsPackage / p.sceneSnapshotsDirectory / filename;
+ return path;
+ }
+ else
+ {
+ return std::nullopt;
+ }
+ }
+
+
+ armem::obj::SceneSnapshot Segment::getSceneSnapshot() const
+ {
+ armem::obj::SceneSnapshot scene;
+
+ wm::FunctionalVisitor visitor;
+ visitor.entityFn = [&scene](wm::Entity & entity)
+ {
+ try
+ {
+ const wm::EntityInstance& entityInstance = entity.getLatestSnapshot().getInstance(0);
+ std::optional<arondto::ObjectInstance> objectInstance = tryCast<arondto::ObjectInstance>(entityInstance);
+ if (objectInstance)
+ {
+ armem::obj::SceneSnapshot::Object& object = scene.objects.emplace_back();
+ // object.instanceID = entityInstance.id();
+ object.className = ObjectID(objectInstance->classID.entityName).getClassID().str();
+ object.collection = "";
+ object.position = simox::math::position(objectInstance->pose.objectPoseGlobal);
+ object.orientation = simox::math::orientation(objectInstance->pose.objectPoseGlobal);
+ }
+ }
+ catch (const armem::error::ArMemError& e)
+ {
+ ARMARX_WARNING_S << e.what();
+ }
+ return false;
+ };
+
+ visitor.applyTo(*coreSegment);
+ return scene;
+ }
+
+
+ void Segment::commitSceneSnapshot(const armem::obj::SceneSnapshot& scene, const std::string& sceneName)
+ {
+ const Time now = TimeUtil::GetTime();
+ std::map<ObjectID, int> idCounters;
+
+ objpose::ObjectPoseSeq objectPoses;
+
+ for (const auto& object : scene.objects)
+ {
+ const ObjectID classID = object.getClassID(objectFinder);
+
+ objpose::ObjectPose& pose = objectPoses.emplace_back();
+
+ pose.providerName = sceneName;
+ pose.objectType = objpose::ObjectTypeEnum::KnownObject;
+ pose.isStatic = true; // Objects loaded from prior knowledge are considerd static to exclude them from decay.
+ pose.objectID = classID.withInstanceName(
+ object.instanceName.empty()
+ ? std::to_string(idCounters[classID]++)
+ : object.instanceName
+ );
+
+ pose.objectPoseGlobal = simox::math::pose(object.position, object.orientation);
+ pose.objectPoseRobot = pose.objectPoseGlobal;
+ pose.objectPoseOriginal = pose.objectPoseGlobal;
+ pose.objectPoseOriginalFrame = armarx::GlobalFrame;
+
+ pose.objectJointValues = object.jointValues;
+
+ pose.robotConfig = {};
+ pose.robotPose = Eigen::Matrix4f::Identity();
+
+ pose.confidence = 1.0;
+ pose.localOOBB = getObjectOOBB(classID);
+ pose.timestamp = now;
+ }
+
+ commitObjectPoses(objectPoses);
+ }
+
+
+ void Segment::commitSceneSnapshotFromFilename(const std::string& filename, bool lockMemory)
+ {
+ ARMARX_INFO << "Loading scene snapshot '" << filename << "' ...";
+ if (auto path = resolveSceneFilename(filename))
+ {
+ if (const auto snapshot = loadScene(path.value()))
+ {
+ std::filesystem::path filename = path->filename();
+ filename.replace_extension(); // Removes extension
+
+ if (lockMemory)
+ {
+ std::scoped_lock lock(memoryMutex);
+ commitSceneSnapshot(snapshot.value(), filename.string());
+ }
+ else
+ {
+ commitSceneSnapshot(snapshot.value(), filename.string());
+ }
+ }
+ }
+ }
+
void Segment::RemoteGui::setup(const Segment& data)
{
@@ -677,8 +994,26 @@ namespace armarx::armem::server::obj::instance
infiniteHistory.setValue(data.p.maxHistorySize == -1);
discardSnapshotsWhileAttached.setValue(data.p.discardSnapshotsWhileAttached);
+ storeLoadLine.setValue("Scene_" + timestampPlaceholder);
+ loadButton.setLabel("Load Scene");
+ storeButton.setLabel("Store Scene");
+
+ HBoxLayout storeLoadLineLayout(
+ {
+ Label(data.p.sceneSnapshotsPackage + "/" + data.p.sceneSnapshotsDirectory + "/"),
+ storeLoadLine,
+ Label(".json")
+ });
+ HBoxLayout storeLoadButtonsLayout({loadButton, storeButton});
+
GridLayout grid;
int row = 0;
+
+ grid.add(storeLoadLineLayout, {row, 0}, {1, 2});
+ row++;
+ grid.add(storeLoadButtonsLayout, {row, 0}, {1, 2});
+ row++;
+
grid.add(Label("Max History Size"), {row, 0}).add(maxHistorySize, {row, 1});
row++;
grid.add(Label("Infinite History Size"), {row, 0}).add(infiniteHistory, {row, 1});
@@ -690,8 +1025,25 @@ namespace armarx::armem::server::obj::instance
group.addChild(grid);
}
+
void Segment::RemoteGui::update(Segment& data)
{
+ if (loadButton.wasClicked())
+ {
+ bool lockMemory = true;
+ data.commitSceneSnapshotFromFilename(storeLoadLine.getValue(), lockMemory);
+ }
+
+ if (storeButton.wasClicked())
+ {
+ armem::obj::SceneSnapshot scene;
+ {
+ std::scoped_lock lock(data.memoryMutex);
+ scene = data.getSceneSnapshot();
+ }
+ data.storeScene(storeLoadLine.getValue(), scene);
+ }
+
if (infiniteHistory.hasValueChanged() || maxHistorySize.hasValueChanged()
|| discardSnapshotsWhileAttached.hasValueChanged())
{
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.h b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.h
index 5b7c268bb28530b82906bc07cee31431d6738f5c..3ec6a2f22f41569945cc0c0ed3f5fd0a4021241e 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.h
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.h
@@ -1,14 +1,16 @@
#pragma once
+#include <filesystem>
#include <map>
-#include <string>
#include <optional>
+#include <string>
#include <SimoxUtility/caching/CacheMap.h>
#include <SimoxUtility/shapes/OrientedBox.h>
#include <ArmarXCore/core/logging/Logging.h>
+#include "RobotAPI/components/ArViz/Client/Client.h"
#include <RobotAPI/interface/core/RobotState.h>
#include <RobotAPI/interface/objectpose/ObjectPoseStorageInterface.h>
@@ -22,6 +24,13 @@
#include "Decay.h"
+#include "ArticulatedObjectVisu.h"
+
+
+namespace armarx::armem::obj
+{
+ struct SceneSnapshot;
+}
namespace armarx::armem::server::obj::instance
{
@@ -36,6 +45,7 @@ namespace armarx::armem::server::obj::instance
};
using ObjectPose = objpose::ObjectPose;
using ObjectPoseSeq = objpose::ObjectPoseSeq;
+ using ObjectPoseMap = std::map<ObjectID, ObjectPose>;
public:
@@ -48,20 +58,23 @@ namespace armarx::armem::server::obj::instance
void init();
+ void connect(viz::Client arviz);
+
+
CommitStats commitObjectPoses(
const std::string& providerName,
const objpose::data::ProvidedObjectPoseSeq& providedPoses,
std::optional<Time> discardUpdatesUntil = std::nullopt);
- void commitObjectPoses(const std::string& providerName, const ObjectPoseSeq& objectPoses);
+ void commitObjectPoses(const ObjectPoseSeq& objectPoses, const std::string& providerName = "");
wm::CoreSegment& getCoreSegment();
const wm::CoreSegment& getCoreSegment() const;
- objpose::ObjectPoseSeq getObjectPoses(IceUtil::Time now);
- objpose::ObjectPoseSeq getObjectPosesByProvider(const std::string& providerName, IceUtil::Time now);
+ objpose::ObjectPoseMap getObjectPoses(IceUtil::Time now);
+ objpose::ObjectPoseMap getObjectPosesByProvider(const std::string& providerName, IceUtil::Time now);
wm::Entity* findObjectEntity(const ObjectID& objectID, const std::string& providerName = "");
std::optional<simox::OrientedBoxf> getObjectOOBB(const ObjectID& id);
@@ -87,26 +100,29 @@ namespace armarx::armem::server::obj::instance
bool& synchronized) const;
- static ObjectPoseSeq getLatestObjectPoses(const wm::CoreSegment& coreSeg);
- static ObjectPoseSeq getLatestObjectPoses(const wm::ProviderSegment& provSeg);
+ static ObjectPoseMap getLatestObjectPoses(const wm::CoreSegment& coreSeg);
+ static ObjectPoseMap getLatestObjectPoses(const wm::ProviderSegment& provSeg);
static ObjectPose getLatestObjectPose(const wm::Entity& entity);
- static void getLatestObjectPoses(const wm::CoreSegment& coreSeg, ObjectPoseSeq& out);
- static void getLatestObjectPoses(const wm::ProviderSegment& provSeg, ObjectPoseSeq& out);
+ static void getLatestObjectPoses(const wm::CoreSegment& coreSeg, ObjectPoseMap& out);
+ static void getLatestObjectPoses(const wm::ProviderSegment& provSeg, ObjectPoseMap& out);
static void getLatestObjectPose(const wm::Entity& entity, ObjectPose& out);
static arondto::ObjectInstance getLatestInstanceData(const wm::Entity& entity);
+ ::armarx::armem::articulated_object::ArticulatedObjects getArticulatedObjects();
+
+
private:
- ObjectPoseSeq getLatestObjectPoses() const;
+ ObjectPoseMap getLatestObjectPoses() const;
void updateObjectPoses(
- ObjectPoseSeq& objectPoses,
+ ObjectPoseMap& objectPoses,
IceUtil::Time now);
void updateObjectPoses(
- ObjectPoseSeq& objectPoses,
+ ObjectPoseMap& objectPoses,
IceUtil::Time now,
VirtualRobot::RobotPtr agent,
bool& agentSynchronized
@@ -119,7 +135,7 @@ namespace armarx::armem::server::obj::instance
) const;
- ObjectPoseSeq filterObjectPoses(const ObjectPoseSeq& objectPoses) const;
+ ObjectPoseMap filterObjectPoses(const ObjectPoseMap& objectPoses) const;
void storeDetachedSnapshot(
@@ -129,12 +145,24 @@ namespace armarx::armem::server::obj::instance
bool commitAttachedPose);
- std::optional<wm::EntityInstance> findClassInstance(const ObjectID& objectID);
+ std::optional<wm::EntityInstance> findClassInstance(const ObjectID& objectID) const;
friend struct DetachVisitor;
+ private:
+
+ void storeScene(const std::string& filename, const armem::obj::SceneSnapshot& scene);
+ std::optional<armem::obj::SceneSnapshot> loadScene(const std::string& filename);
+ std::optional<armem::obj::SceneSnapshot> loadScene(const std::filesystem::path& path);
+ std::optional<std::filesystem::path> resolveSceneFilename(const std::string& filename);
+
+ armem::obj::SceneSnapshot getSceneSnapshot() const;
+ void commitSceneSnapshot(const armem::obj::SceneSnapshot& scene, const std::string& sceneName);
+ void commitSceneSnapshotFromFilename(const std::string& filename, bool lockMemory);
+
+
public:
RobotStateComponentInterfacePrx robotStateComponent;
@@ -162,6 +190,11 @@ namespace armarx::armem::server::obj::instance
// -1 would be infinite, but this can let the RAM grow quickly.
long maxHistorySize = 25;
bool discardSnapshotsWhileAttached = true;
+
+ /// Package containing the scene snapshots
+ std::string sceneSnapshotsPackage = armarx::ObjectFinder::DefaultObjectsPackageName;
+ std::string sceneSnapshotsDirectory = "scenes";
+ std::string sceneSnapshotToLoad = "";
};
Properties p;
@@ -172,6 +205,9 @@ namespace armarx::armem::server::obj::instance
/// Class name -> dataset name.
simox::caching::CacheMap<std::string, std::string> classNameToDatasetCache;
+ std::unique_ptr<CMakePackageFinder> finder;
+
+ static const std::string timestampPlaceholder;
public:
@@ -179,6 +215,10 @@ namespace armarx::armem::server::obj::instance
{
armarx::RemoteGui::Client::GroupBox group;
+ armarx::RemoteGui::Client::LineEdit storeLoadLine;
+ armarx::RemoteGui::Client::Button storeButton;
+ armarx::RemoteGui::Client::Button loadButton;
+
armarx::RemoteGui::Client::IntSpinBox maxHistorySize;
armarx::RemoteGui::Client::CheckBox infiniteHistory;
armarx::RemoteGui::Client::CheckBox discardSnapshotsWhileAttached;
@@ -187,6 +227,9 @@ namespace armarx::armem::server::obj::instance
void update(Segment& data);
};
+ private:
+ std::unique_ptr<ArticulatedObjectVisu> visu;
+
};
}
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.cpp
index 3773c087bf70fa68981be86696b01c6fae7645d6..e0577877f2ac9e9c7e97e8aaa50d99c438fe4c9f 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.cpp
@@ -93,6 +93,8 @@ namespace armarx::armem::server::obj::instance
});
visu.updateTask->start();
}
+
+ segment.connect(arviz);
}
void SegmentAdapter::reportProviderAvailable(const std::string& providerName, const objpose::ProviderInfo& info, const Ice::Current&)
@@ -217,7 +219,7 @@ namespace armarx::armem::server::obj::instance
TIMING_END_STREAM(tGetObjectPosesLock, ARMARX_VERBOSE);
const IceUtil::Time now = TimeUtil::GetTime();
- const objpose::data::ObjectPoseSeq result = objpose::toIce(segment.getObjectPoses(now));
+ const objpose::data::ObjectPoseSeq result = objpose::toIce(simox::alg::get_values(segment.getObjectPoses(now)));
TIMING_END_STREAM(tGetObjectPoses, ARMARX_VERBOSE);
@@ -242,7 +244,7 @@ namespace armarx::armem::server::obj::instance
TIMING_END_STREAM(GetObjectPosesLock, ARMARX_VERBOSE);
const IceUtil::Time now = TimeUtil::GetTime();
- const objpose::data::ObjectPoseSeq result = objpose::toIce(segment.getObjectPosesByProvider(providerName, now));
+ const objpose::data::ObjectPoseSeq result = objpose::toIce(simox::alg::get_values(segment.getObjectPosesByProvider(providerName, now)));
TIMING_END_STREAM(GetObjectPoses, ARMARX_VERBOSE);
@@ -406,6 +408,8 @@ namespace armarx::armem::server::obj::instance
void SegmentAdapter::visualizeRun()
{
+ ObjectFinder objectFinder;
+
CycleUtil cycle(static_cast<int>(1000 / visu.frequencyHz));
while (visu.updateTask && !visu.updateTask->isStopped())
{
@@ -414,33 +418,45 @@ namespace armarx::armem::server::obj::instance
if (visu.enabled)
{
- TIMING_START(Visu);
+ TIMING_START(tVisu);
- objpose::ObjectPoseSeq objectPoses;
- ObjectFinder objectFinder;
+ objpose::ObjectPoseMap objectPoses;
visu.minConfidence = -1;
{
std::scoped_lock lock(memoryMutex);
const IceUtil::Time now = TimeUtil::GetTime();
+
+ // Also include decayed objects in result
+ // Store original setting.
+ const float minConf = segment.decay.removeObjectsBelowConfidence;
+ segment.decay.removeObjectsBelowConfidence = -1;
+ // Get result.
objectPoses = segment.getObjectPoses(now);
+ // Restore original setting.
+ segment.decay.removeObjectsBelowConfidence = minConf;
+
objectFinder = segment.objectFinder;
if (segment.decay.enabled)
{
visu.minConfidence = segment.decay.removeObjectsBelowConfidence;
}
}
+
const std::vector<viz::Layer> layers = visu.visualizeCommit(objectPoses, objectFinder);
arviz.commit(layers);
- TIMING_END_STREAM(Visu, ARMARX_VERBOSE);
+ TIMING_END_STREAM(tVisu, ARMARX_VERBOSE);
if (debugObserver)
{
- debugObserver->setDebugChannel(getName(),
+ armarx::StringVariantBaseMap debugValues;
+ debugValues["t Visualize [ms]"] = new Variant(tVisu.toMilliSecondsDouble());
+ for (const auto& [id, pose] : objectPoses)
{
- { "t Visualize [ms]", new Variant(Visu.toMilliSecondsDouble()) },
- });
+ debugValues["confidence(" + id.str() + ")"] = new Variant(pose.confidence);
+ }
+ debugObserver->setDebugChannel(getName(), debugValues);
}
}
}
@@ -484,9 +500,9 @@ namespace armarx::armem::server::obj::instance
std::scoped_lock lock(adapter.visuMutex);
this->visu.update(adapter.visu);
}
+ this->segment.update(adapter.segment);
{
std::scoped_lock lock(adapter.memoryMutex);
- this->segment.update(adapter.segment);
this->decay.update(adapter.segment.decay);
}
{
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Visu.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/Visu.cpp
index 5cc251ca984e22a0129411b6b05a59db79d8a543..a0c8812a6751f0612ffb254a2cb43bdd714e812d 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/Visu.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/Visu.cpp
@@ -28,6 +28,8 @@ namespace armarx::armem::server::obj::instance
"Enable showing object frames.");
defs->optional(objectFramesScale, prefix + "objectFramesScale",
"Scaling of object frames.");
+ defs->optional(useArticulatedModels, prefix + "useArticulatedModels",
+ "Prefer articulated object models if available.");
}
@@ -43,30 +45,46 @@ namespace armarx::armem::server::obj::instance
return layers;
}
+
std::vector<viz::Layer> Visu::visualizeCommit(
const objpose::ObjectPoseSeq& objectPoses,
const ObjectFinder& objectFinder) const
{
- std::map<std::string, viz::Layer> layers;
-
- auto getLayer = [this, &layers](const std::string & providerName) -> viz::Layer &
+ std::map<std::string, viz::Layer> stage;
+ for (const objpose::ObjectPose& objectPose : objectPoses)
{
- auto it = layers.find(providerName);
- if (it == layers.end())
- {
- it = layers.emplace(providerName, arviz.layer(providerName)).first;
- }
- return it->second;
- };
+ visualizeObjectPose(getLayer(objectPose.providerName, stage), objectPose, objectFinder);
+ }
+ return simox::alg::get_values(stage);
+ }
- for (const objpose::ObjectPose& objectPose : objectPoses)
+
+ std::vector<viz::Layer> Visu::visualizeCommit(
+ const objpose::ObjectPoseMap& objectPoses,
+ const ObjectFinder& objectFinder) const
+ {
+ std::map<std::string, viz::Layer> stage;
+ for (const auto& [id, objectPose] : objectPoses)
{
- visualizeObjectPose(getLayer(objectPose.providerName), objectPose, objectFinder);
+ visualizeObjectPose(getLayer(objectPose.providerName, stage), objectPose, objectFinder);
}
+ return simox::alg::get_values(stage);
+ }
- return simox::alg::get_values(layers);
+
+ viz::Layer& Visu::getLayer(
+ const std::string& providerName,
+ std::map<std::string, viz::Layer>& stage) const
+ {
+ auto it = stage.find(providerName);
+ if (it == stage.end())
+ {
+ it = stage.emplace(providerName, arviz.layer(providerName)).first;
+ }
+ return it->second;
}
+
viz::Layer Visu::visualizeProvider(
const std::string& providerName,
const objpose::ObjectPoseSeq& objectPoses,
@@ -85,8 +103,8 @@ namespace armarx::armem::server::obj::instance
const objpose::ObjectPose& objectPose,
const ObjectFinder& objectFinder) const
{
- const bool show = not(objectPose.confidence < minConfidence);
- if (!show)
+ const bool show = objectPose.confidence >= minConfidence;
+ if (not show)
{
return;
}
@@ -95,25 +113,44 @@ namespace armarx::armem::server::obj::instance
const Eigen::Matrix4f pose = inGlobalFrame ? objectPose.objectPoseGlobal : objectPose.objectPoseRobot;
{
- viz::Object object(key);
- object.pose(pose);
- if (std::optional<ObjectInfo> objectInfo = objectFinder.findObject(id))
- {
- object.file(objectInfo->package(), objectInfo->simoxXML().relativePath);
- }
- else
- {
- object.fileByObjectFinder(id);
- }
- if (alphaByConfidence && objectPose.confidence < 1.0f)
+ std::optional<ObjectInfo> objectInfo = objectFinder.findObject(id);
+
+ bool done = false;
+ if (useArticulatedModels && objectInfo)
{
- object.overrideColor(simox::Color::white().with_alpha(objectPose.confidence));
+ if (std::optional<PackageFileLocation> model = objectInfo->getArticulatedModel())
+ {
+ viz::Robot robot(key);
+ robot.pose(pose);
+ robot.file(model->package, model->relativePath);
+ robot.joints(objectPose.objectJointValues);
+
+ layer.add(robot);
+ done = true;
+ }
}
- else if (alpha < 1)
+ if (!done)
{
- object.overrideColor(simox::Color::white().with_alpha(alpha));
+ viz::Object object(key);
+ object.pose(pose);
+ if (objectInfo)
+ {
+ object.file(objectInfo->package(), objectInfo->simoxXML().relativePath);
+ }
+ else
+ {
+ object.fileByObjectFinder(id);
+ }
+ if (alphaByConfidence && objectPose.confidence < 1.0f)
+ {
+ object.overrideColor(simox::Color::white().with_alpha(objectPose.confidence));
+ }
+ else if (alpha < 1)
+ {
+ object.overrideColor(simox::Color::white().with_alpha(alpha));
+ }
+ layer.add(object);
}
- layer.add(object);
}
if (oobbs && objectPose.localOOBB)
@@ -148,6 +185,7 @@ namespace armarx::armem::server::obj::instance
objectFramesScale.setSteps(int(10 * max));
objectFramesScale.setValue(visu.objectFramesScale);
}
+ useArticulatedModels.setValue(visu.useArticulatedModels);
GridLayout grid;
int row = 0;
@@ -164,6 +202,8 @@ namespace armarx::armem::server::obj::instance
grid.add(Label("Object Frames"), {row, 0}).add(objectFrames, {row, 1});
grid.add(Label("Scale:"), {row, 2}).add(objectFramesScale, {row, 3});
row++;
+ grid.add(Label("Use Articulated Models"), {row, 0}).add(useArticulatedModels, {row, 1});
+ row++;
group.setLabel("Visualization");
group.addChild(grid);
@@ -178,6 +218,7 @@ namespace armarx::armem::server::obj::instance
visu.oobbs = oobbs.getValue();
visu.objectFrames = objectFrames.getValue();
visu.objectFramesScale = objectFramesScale.getValue();
+ visu.useArticulatedModels = useArticulatedModels.getValue();
}
}
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Visu.h b/source/RobotAPI/libraries/armem_objects/server/instance/Visu.h
index 1cd1d41c835e56e820ce3db8b3397b1e117d8202..01e7f059817d4050fc9728a4b09d95d22c9a572f 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/Visu.h
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/Visu.h
@@ -37,6 +37,10 @@ namespace armarx::armem::server::obj::instance
const objpose::ObjectPoseSeq& objectPoses,
const ObjectFinder& objectFinder
) const;
+ std::vector<viz::Layer> visualizeCommit(
+ const objpose::ObjectPoseMap& objectPoses,
+ const ObjectFinder& objectFinder
+ ) const;
viz::Layer visualizeProvider(
const std::string& providerName,
@@ -52,6 +56,11 @@ namespace armarx::armem::server::obj::instance
) const;
+ private:
+
+ viz::Layer& getLayer(const std::string& providerName, std::map<std::string, viz::Layer>& stage) const;
+
+
public:
viz::Client arviz;
@@ -67,6 +76,9 @@ namespace armarx::armem::server::obj::instance
bool objectFrames = false;
float objectFramesScale = 1.0;
+ /// Prefer articulated models if available.
+ bool useArticulatedModels = true;
+
SimpleRunningTask<>::pointer_type updateTask;
@@ -83,6 +95,9 @@ namespace armarx::armem::server::obj::instance
armarx::RemoteGui::Client::CheckBox objectFrames;
armarx::RemoteGui::Client::FloatSpinBox objectFramesScale;
+ armarx::RemoteGui::Client::CheckBox useArticulatedModels;
+
+
void setup(const Visu& visu);
void update(Visu& visu);
};
diff --git a/source/RobotAPI/libraries/armem_objects/test/ArMemObjectsTest.cpp b/source/RobotAPI/libraries/armem_objects/test/ArMemObjectsTest.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..3624c35b18ff85c1b71bdf58ad5a37c245f7959e
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/test/ArMemObjectsTest.cpp
@@ -0,0 +1,36 @@
+/*
+ * 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_gui
+ * @author Rainer Kartmann ( rainer dot kartmann 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::ArmarXLibraries::armem_objects
+
+#define ARMARX_BOOST_TEST
+
+#include <RobotAPI/Test.h>
+
+#include <iostream>
+
+
+
+BOOST_AUTO_TEST_CASE(test_armem_objects)
+{
+ BOOST_CHECK(true);
+}
diff --git a/source/RobotAPI/libraries/armem_objects/test/CMakeLists.txt b/source/RobotAPI/libraries/armem_objects/test/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..bb67820c6e69342d583d11dc158448d7bc532ea6
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/test/CMakeLists.txt
@@ -0,0 +1,5 @@
+
+# Libs required for the tests
+SET(LIBS ${LIBS} ArmarXCore "${LIB_NAME}")
+
+armarx_add_test(ArMemObjectsTest ArMemObjectsTest.cpp "${LIBS}")
diff --git a/source/RobotAPI/libraries/armem_robot/aron_conversions.cpp b/source/RobotAPI/libraries/armem_robot/aron_conversions.cpp
index 6a56d19f385af667974269c72c0de37af406da9f..da72cc16f995f239c207dfb38dcaa6cf533fb74b 100644
--- a/source/RobotAPI/libraries/armem_robot/aron_conversions.cpp
+++ b/source/RobotAPI/libraries/armem_robot/aron_conversions.cpp
@@ -1,18 +1,19 @@
#include "aron_conversions.h"
+#include <RobotAPI/libraries/aron/common/aron_conversions.h>
+
+#include <RobotAPI/libraries/ArmarXObjects/ObjectID.h>
+#include <RobotAPI/libraries/ArmarXObjects/aron_conversions.h>
+
#include <RobotAPI/libraries/aron/common/aron_conversions/core.h>
#include <RobotAPI/libraries/aron/common/aron_conversions/stl.h>
#include <RobotAPI/libraries/aron/common/aron_conversions/armarx.h>
+
namespace armarx::armem::robot
{
/* to be moved */
- void fromAron(const long& dto, IceUtil::Time& time)
- {
- time = IceUtil::Time::microSeconds(dto);
- }
-
void toAron(long& dto, const IceUtil::Time& time)
{
dto = time.toMicroSeconds();
@@ -51,16 +52,60 @@ namespace armarx::armem::robot
void fromAron(const arondto::RobotState& dto, RobotState& bo)
{
- fromAron(dto.timestamp, bo.timestamp);
+ bo.timestamp = dto.timestamp;
bo.globalPose.matrix() = dto.globalPose;
bo.jointMap = dto.jointMap;
}
void toAron(arondto::RobotState& dto, const RobotState& bo)
{
- toAron(dto.timestamp, bo.timestamp);
+ dto.timestamp = bo.timestamp;
dto.globalPose = bo.globalPose.matrix();
dto.jointMap = bo.jointMap;
}
} // namespace armarx::armem::robot
+
+
+namespace armarx::armem
+{
+
+ void robot::fromAron(const arondto::ObjectClass& dto, RobotDescription& bo)
+ {
+ bo.name = aron::fromAron<armarx::ObjectID>(dto.id).str();
+ fromAron(dto.articulatedSimoxXmlPath, bo.xml);
+ }
+
+ void robot::toAron(arondto::ObjectClass& dto, const RobotDescription& bo)
+ {
+ toAron(dto.id, ObjectID(bo.name));
+ toAron(dto.articulatedSimoxXmlPath, bo.xml);
+ }
+
+
+ void robot::fromAron(const arondto::ObjectInstance& dto, RobotState& bo)
+ {
+ fromAron(dto.pose, bo);
+ }
+
+ void robot::toAron(arondto::ObjectInstance& dto, const RobotState& bo)
+ {
+ toAron(dto.pose, bo);
+ }
+
+
+ void robot::fromAron(const objpose::arondto::ObjectPose& dto, RobotState& bo)
+ {
+ bo.timestamp = dto.timestamp;
+ bo.globalPose = dto.objectPoseGlobal;
+ bo.jointMap = dto.objectJointValues;
+ }
+
+ void robot::toAron(objpose::arondto::ObjectPose& dto, const RobotState& bo)
+ {
+ dto.timestamp = bo.timestamp;
+ dto.objectPoseGlobal = bo.globalPose.matrix();
+ dto.objectJointValues = bo.jointMap;
+ }
+
+} // namespace armarx::armem
diff --git a/source/RobotAPI/libraries/armem_robot/aron_conversions.h b/source/RobotAPI/libraries/armem_robot/aron_conversions.h
index 70e06ce8eedb7de9538f5249b3eedf797aec6060..9c124d758a9189b1ec709cca9808b430bbc63a94 100644
--- a/source/RobotAPI/libraries/armem_robot/aron_conversions.h
+++ b/source/RobotAPI/libraries/armem_robot/aron_conversions.h
@@ -1,11 +1,15 @@
#pragma once
-#include <RobotAPI/libraries/armem_robot/types.h>
+#include <RobotAPI/libraries/armem_objects/aron/ObjectClass.aron.generated.h>
+#include <RobotAPI/libraries/armem_objects/aron/ObjectInstance.aron.generated.h>
+#include <RobotAPI/libraries/ArmarXObjects/aron/ObjectPose.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot/types.h>
#include <RobotAPI/libraries/armem_robot/aron/RobotDescription.aron.generated.h>
#include <RobotAPI/libraries/armem_robot/aron/RobotState.aron.generated.h>
#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
+
namespace armarx::armem::robot
{
// TODO move the following
@@ -22,4 +26,14 @@ namespace armarx::armem::robot
void fromAron(const arondto::RobotState& dto, RobotState& bo);
void toAron(arondto::RobotState& dto, const RobotState& bo);
+
+ void fromAron(const arondto::ObjectClass& dto, RobotDescription& bo);
+ void toAron(arondto::ObjectClass& dto, const RobotDescription& bo);
+
+ void fromAron(const arondto::ObjectInstance& dto, RobotState& bo);
+ void toAron(arondto::ObjectInstance& dto, const RobotState& bo);
+
+ void fromAron(const objpose::arondto::ObjectPose& dto, RobotState& bo);
+ void toAron(objpose::arondto::ObjectPose& dto, const RobotState& bo);
+
} // namespace armarx::armem::robot
diff --git a/source/RobotAPI/libraries/armem_robot_mapping/CMakeLists.txt b/source/RobotAPI/libraries/armem_robot_mapping/CMakeLists.txt
deleted file mode 100644
index 145ced819b68db659a92e232747c645c24037ce5..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem_robot_mapping/CMakeLists.txt
+++ /dev/null
@@ -1,34 +0,0 @@
-set(LIB_NAME armem_robot_mapping)
-
-armarx_component_set_name("${LIB_NAME}")
-armarx_set_target("Library: ${LIB_NAME}")
-
-armarx_add_library(
- LIBS
- # ArmarX
- ArmarXCore
- # This package
- RobotAPICore
- RobotAPI::armem
- # System / External
- Eigen3::Eigen
- HEADERS
- ./aron_conversions.h
- ./MappingDataWriter.h
- ./MappingDataReader.h
- SOURCES
- ./aron_conversions.cpp
- ./MappingDataWriter.cpp
- ./MappingDataReader.cpp
-)
-
-
-armarx_enable_aron_file_generation_for_target(
- TARGET_NAME
- "${LIB_NAME}"
- ARON_FILES
- aron/LaserScan.xml
-)
-
-
-add_library(RobotAPI::armem_robot_mapping ALIAS armem_robot_mapping)
diff --git a/source/RobotAPI/libraries/armem_robot_mapping/MappingDataWriter.cpp b/source/RobotAPI/libraries/armem_robot_mapping/MappingDataWriter.cpp
deleted file mode 100644
index 351d71398a6959f346aadf45c6c604edcd865a53..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem_robot_mapping/MappingDataWriter.cpp
+++ /dev/null
@@ -1,96 +0,0 @@
-#include "MappingDataWriter.h"
-
-#include "RobotAPI/libraries/armem_robot_mapping/aron_conversions.h"
-#include <RobotAPI/libraries/armem_robot_mapping/aron/LaserScan.aron.generated.h>
-
-
-namespace armarx::armem
-{
-
- MappingDataWriter::MappingDataWriter(armem::ClientWriterComponentPluginUser& component)
- : component(component) {}
-
- MappingDataWriter::~MappingDataWriter() = default;
-
- void MappingDataWriter::registerPropertyDefinitions(
- armarx::PropertyDefinitionsPtr& def)
- {
- ARMARX_DEBUG << "TransformWriter: registerPropertyDefinitions";
-
- const std::string prefix = propertyPrefix;
-
- def->optional(properties.mappingMemoryName, prefix + "MappingMemoryName",
- "Name of the mapping memory core segment to use.");
-
- def->optional(properties.memoryName, prefix + "MemoryName");
- }
-
- void MappingDataWriter::connect()
- {
- // Wait for the memory to become available and add it as dependency.
- ARMARX_IMPORTANT << "MappingDataWriter: Waiting for memory '" << properties.memoryName
- << "' ...";
- auto result = component.useMemory(properties.memoryName);
- if (not result.success)
- {
- ARMARX_ERROR << result.errorMessage;
- return;
- }
-
- ARMARX_IMPORTANT << "TransformWriter: Connected to memory '" << properties.memoryName;
-
- memoryWriter.setWritingMemory(result.proxy);
- }
-
-
- bool MappingDataWriter::storeSensorData(const LaserScan& laserScan,
- const std::string& frame,
- const std::string& agentName,
- const std::int64_t& timestamp)
- {
- std::lock_guard g{memoryWriterMutex};
-
- const auto result =
- memoryWriter.addSegment(properties.mappingMemoryName, agentName);
-
- if (not result.success)
- {
- ARMARX_ERROR << result.errorMessage;
-
- // TODO(fabian.reister): message
- return false;
- }
-
- const auto iceTimestamp = Time::microSeconds(timestamp);
-
- const auto providerId = armem::MemoryID(result.segmentID);
- const auto entityID =
- providerId.withEntityName(frame).withTimestamp(iceTimestamp);
-
- armem::EntityUpdate update;
- update.entityID = entityID;
-
- arondto::LaserScanStamped aronSensorData;
- // currently only sets the header
- toAron(laserScan, iceTimestamp, frame, agentName, aronSensorData);
-
- auto dict = aronSensorData.toAron();
- dict->addElement("scan", toAron(laserScan));
-
- update.instancesData = {dict};
- update.timeCreated = iceTimestamp;
-
- ARMARX_DEBUG << "Committing " << update << " at time " << iceTimestamp;
- armem::EntityUpdateResult updateResult = memoryWriter.commit(update);
-
- ARMARX_DEBUG << updateResult;
-
- if (not updateResult.success)
- {
- ARMARX_ERROR << updateResult.errorMessage;
- }
-
- return updateResult.success;
- }
-
-} // namespace armarx::armem
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/armem_robot_mapping/aron_conversions.h b/source/RobotAPI/libraries/armem_robot_mapping/aron_conversions.h
deleted file mode 100644
index 7ecffc237d480fd33c30226ef7f1cf02c09308de..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem_robot_mapping/aron_conversions.h
+++ /dev/null
@@ -1,70 +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/>.
- *
- * @author Fabian Reister ( fabian dot reister at kit dot edu )
- * @date 2021
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-
-#pragma once
-
-#include <RobotAPI/interface/units/LaserScannerUnit.h>
-#include <RobotAPI/libraries/aron/converter/common/VectorConverter.h>
-#include <RobotAPI/libraries/aron/core/navigator/data/complex/NDArray.h>
-#include <RobotAPI/libraries/armem/core/Time.h>
-
-namespace armarx
-{
-
- namespace arondto
- {
- struct LaserScanStamped;
- } // namespace aron
-
- // struct LaserScan;
- struct LaserScanStamped;
-
- void fromAron(
- const arondto::LaserScanStamped& aronLaserScan,
- LaserScan& laserScan,
- std::int64_t& timestamp,
- std::string& frame,
- std::string& agentName);
-
-
- template<typename T>
- auto fromAron(const aron::datanavigator::NDArrayNavigatorPtr& navigator)
- {
- return aron::converter::AronVectorConverter::ConvertToVector<T>(navigator);
- }
-
- void fromAron(const arondto::LaserScanStamped& aronLaserScan, LaserScanStamped& laserScan);
-
- void toAron(
- const LaserScan& laserScan,
- const armem::Time& timestamp,
- const std::string& frame,
- const std::string& agentName,
- arondto::LaserScanStamped& aronLaserScan);
-
- inline aron::datanavigator::NDArrayNavigatorPtr toAron(const LaserScan& laserScan)
- {
- return aron::converter::AronVectorConverter::ConvertFromVector(laserScan);
- }
-
-
-} // namespace armarx
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/armem_robot_state/aron_conversions.cpp b/source/RobotAPI/libraries/armem_robot_state/aron_conversions.cpp
index 27fecee2e4f4ae9137a4e7f6281bef679d308da4..785d25bdca4757ebe1c07f0bab767aa08a02a24d 100644
--- a/source/RobotAPI/libraries/armem_robot_state/aron_conversions.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/aron_conversions.cpp
@@ -38,7 +38,7 @@ namespace armarx::armem
aron::toAron(dto.parentFrame, bo.parentFrame);
aron::toAron(dto.frame, bo.frame);
aron::toAron(dto.agent, bo.agent);
- armarx::toAron(dto.timestamp, bo.timestamp);
+ dto.timestamp = bo.timestamp;
}
void fromAron(const arondto::TransformHeader& dto, robot_state::TransformHeader& bo)
@@ -46,7 +46,7 @@ namespace armarx::armem
aron::fromAron(dto.parentFrame, bo.parentFrame);
aron::fromAron(dto.frame, bo.frame);
aron::fromAron(dto.agent, bo.agent);
- armarx::fromAron(dto.timestamp, bo.timestamp);
+ bo.timestamp = dto.timestamp;
}
/* JointState */
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp
index 2d8319dc9f6517c41e9a38225ad4dbd11b9ae607..7a500d1391c0f0942795b49af65740f350eecbed 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp
@@ -3,25 +3,27 @@
#include <mutex>
#include <optional>
-#include "ArmarXCore/core/exceptions/LocalException.h"
-#include "ArmarXCore/core/logging/Logging.h"
+#include <ArmarXCore/core/exceptions/LocalException.h>
+#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/PackagePath.h>
-#include "RobotAPI/libraries/armem/client/query/Builder.h"
-#include "RobotAPI/libraries/armem/core/Time.h"
-#include "RobotAPI/libraries/armem/core/workingmemory/CoreSegment.h"
-#include "RobotAPI/libraries/armem_robot/aron_conversions.h"
-#include "RobotAPI/libraries/armem_robot/robot_conversions.h"
+#include <RobotAPI/libraries/armem/client/query/Builder.h>
+#include <RobotAPI/libraries/armem/core/error.h>
+#include <RobotAPI/libraries/armem/core/Time.h>
+#include <RobotAPI/libraries/armem/core/workingmemory/CoreSegment.h>
+#include <RobotAPI/libraries/armem_robot/aron_conversions.h>
+#include <RobotAPI/libraries/armem_robot/robot_conversions.h>
#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/aron/JointState.aron.generated.h>
+
namespace fs = ::std::filesystem;
namespace armarx::armem::robot_state
{
- RobotReader::RobotReader(armem::ClientReaderComponentPluginUser& component) :
- memoryClient(component), transformReader(component)
+ RobotReader::RobotReader(armem::client::MemoryNameSystem& memoryNameSystem) :
+ memoryNameSystem(memoryNameSystem), transformReader(memoryNameSystem)
{
}
@@ -42,16 +44,16 @@ namespace armarx::armem::robot_state
// Wait for the memory to become available and add it as dependency.
ARMARX_IMPORTANT << "RobotReader: Waiting for memory '" << properties.memoryName << "' ...";
- auto result = memoryClient.useMemory(properties.memoryName);
- if (not result.success)
+ try
+ {
+ memoryReader = memoryNameSystem.useReader(properties.memoryName);
+ ARMARX_IMPORTANT << "RobotReader: Connected to memory '" << properties.memoryName << "'";
+ }
+ catch (const armem::error::CouldNotResolveMemoryServer& e)
{
- ARMARX_ERROR << result.errorMessage;
+ ARMARX_ERROR << e.what();
return;
}
-
- ARMARX_IMPORTANT << "RobotReader: Connected to memory '" << properties.memoryName;
-
- memoryReader.setReadingMemory(result.proxy);
}
std::optional<robot::Robot> RobotReader::get(const std::string& name,
@@ -242,7 +244,7 @@ namespace armarx::armem::robot_state
template <typename AronClass>
std::optional<AronClass> tryCast(const wm::EntityInstance& item)
{
- static_assert(std::is_base_of<armarx::aron::cppcodegenerator::AronCppClass,
+ static_assert(std::is_base_of<armarx::aron::cppserializer::AronCppClass,
AronClass>::value);
try
@@ -327,4 +329,4 @@ namespace armarx::armem::robot_state
return robot::convertRobotDescription(instance);
}
-} // namespace armarx::armem::robot_state
\ No newline at end of file
+} // namespace armarx::armem::robot_state
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h
index b5bd01e5d182a88cc6d36e8c8112e1683c017d16..13b5572d382059bcff17ce2bb5bfb04cc819b04b 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h
@@ -24,13 +24,14 @@
#include <mutex>
#include <optional>
-#include "RobotAPI/libraries/armem/client.h"
-#include "RobotAPI/libraries/armem/client/Reader.h"
+#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
+#include <RobotAPI/libraries/armem/client/Reader.h>
-#include "RobotAPI/libraries/armem_robot/client/interfaces.h"
-#include "RobotAPI/libraries/armem_robot/types.h"
+#include <RobotAPI/libraries/armem_robot/client/interfaces.h>
+#include <RobotAPI/libraries/armem_robot/types.h>
+
+#include <RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.h>
-#include "RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.h"
namespace armarx::armem::robot_state
{
@@ -43,7 +44,7 @@ namespace armarx::armem::robot_state
class RobotReader : virtual public robot::ReaderInterface
{
public:
- RobotReader(armem::ClientReaderComponentPluginUser& component);
+ RobotReader(armem::client::MemoryNameSystem& memoryNameSystem);
virtual ~RobotReader() = default;
void connect();
@@ -89,12 +90,11 @@ namespace armarx::armem::robot_state
const std::string propertyPrefix = "mem.robot_state.";
+ armem::client::MemoryNameSystem& memoryNameSystem;
armem::client::Reader memoryReader;
std::mutex memoryWriterMutex;
- armem::ClientReaderComponentPluginUser& memoryClient;
-
client::robot_state::localization::TransformReader transformReader;
};
-} // namespace armarx::armem::robot_state
\ No newline at end of file
+} // namespace armarx::armem::robot_state
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.cpp b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.cpp
index a3655fee1ae2597b768424f0b89fd581a1bb0680..10e13611f3fd8608a5d0b5027fd92b960c3a7754 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.cpp
@@ -14,8 +14,8 @@
namespace armarx::armem::robot_state
{
- VirtualRobotReader::VirtualRobotReader(armem::ClientReaderComponentPluginUser& component) :
- RobotReader(component)
+ VirtualRobotReader::VirtualRobotReader(armem::client::MemoryNameSystem& memoryNameSystem) :
+ RobotReader(memoryNameSystem)
{
}
@@ -83,4 +83,4 @@ namespace armarx::armem::robot_state
}
-} // namespace armarx::armem::robot_state
\ No newline at end of file
+} // namespace armarx::armem::robot_state
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h
index a280e71b6132f065aec28a008ebb5ab803845dad..e5945103cf4956fae90644b630cc61132624227b 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h
@@ -27,6 +27,7 @@
#include <VirtualRobot/VirtualRobot.h>
#include <VirtualRobot/XML/RobotIO.h>
+
namespace armarx::armem::robot_state
{
/**
@@ -40,7 +41,7 @@ namespace armarx::armem::robot_state
class VirtualRobotReader : virtual public RobotReader
{
public:
- VirtualRobotReader(armem::ClientReaderComponentPluginUser& component);
+ VirtualRobotReader(armem::client::MemoryNameSystem& memoryNameSystem);
virtual ~VirtualRobotReader() = default;
void connect();
@@ -61,4 +62,4 @@ namespace armarx::armem::robot_state
VirtualRobot::RobotIO::RobotDescription::eStructure);
};
-} // namespace armarx::armem::robot_state
\ No newline at end of file
+} // namespace armarx::armem::robot_state
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.cpp b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.cpp
index 8a37b2dec14cfc6721b7458fce984dac59bdfb21..6aa026431e7d9efd0128a9007d23918b291b54bc 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.cpp
@@ -47,6 +47,7 @@
// this package
#include <RobotAPI/libraries/armem/client/query/Builder.h>
#include <RobotAPI/libraries/armem/client/query/query_fns.h>
+#include <RobotAPI/libraries/armem/core/error.h>
#include <RobotAPI/libraries/armem/core/workingmemory/Memory.h>
#include <RobotAPI/libraries/armem/core/workingmemory/ProviderSegment.h>
#include <RobotAPI/libraries/armem/core/workingmemory/ice_conversions.h>
@@ -61,7 +62,8 @@
namespace armarx::armem::client::robot_state::localization
{
- TransformReader::TransformReader(armem::ClientReaderComponentPluginUser& memoryClient) : memoryClient(memoryClient) {}
+ TransformReader::TransformReader(armem::client::MemoryNameSystem& memoryNameSystem) :
+ memoryNameSystem(memoryNameSystem) {}
TransformReader::~TransformReader() = default;
@@ -83,16 +85,16 @@ namespace armarx::armem::client::robot_state::localization
// Wait for the memory to become available and add it as dependency.
ARMARX_IMPORTANT << "TransformReader: Waiting for memory '" << properties.memoryName
<< "' ...";
- auto result = memoryClient.useMemory(properties.memoryName);
- if (not result.success)
+ try
{
- ARMARX_ERROR << result.errorMessage;
+ memoryReader = memoryNameSystem.useReader(properties.memoryName);
+ ARMARX_IMPORTANT << "TransformReader: Connected to memory '" << properties.memoryName;
+ }
+ catch (const armem::error::CouldNotResolveMemoryServer& e)
+ {
+ ARMARX_ERROR << e.what();
return;
}
-
- ARMARX_IMPORTANT << "TransformReader: Connected to memory '" << properties.memoryName;
-
- memoryReader.setReadingMemory(result.proxy);
}
TransformResult TransformReader::getGlobalPose(const std::string& agentName,
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.h b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.h
index 2d1d98ddf82c5c137b934b3be91079bb87298503..433694ec819859bdcda209dd57aad21d22512116 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.h
+++ b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.h
@@ -22,10 +22,9 @@
#pragma once
+#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
#include <RobotAPI/libraries/armem/client/Reader.h>
-#include <RobotAPI/libraries/armem/client.h>
-
#include "interfaces.h"
namespace armarx::armem::client::robot_state::localization
@@ -45,7 +44,7 @@ namespace armarx::armem::client::robot_state::localization
virtual public TransformReaderInterface
{
public:
- TransformReader(armem::ClientReaderComponentPluginUser& memoryClient);
+ TransformReader(armem::client::MemoryNameSystem& memoryNameSystem);
~TransformReader() override;
@@ -61,19 +60,17 @@ namespace armarx::armem::client::robot_state::localization
private:
+ armem::client::MemoryNameSystem& memoryNameSystem;
armem::client::Reader memoryReader;
// Properties
struct Properties
{
- std::string memoryName = "RobotStateMemory";
+ std::string memoryName = "RobotState";
std::string localizationSegment = "Localization";
} properties;
const std::string propertyPrefix = "mem.robot_state.";
-
- armem::ClientReaderComponentPluginUser& memoryClient;
-
};
} // namespace armarx::armem::client::robot_state::localization
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformWriter.cpp b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformWriter.cpp
index 2327c141840bc74f8b43ab14df03b03f2b98ccdf..47660fc439e597f4c8f59bbcff666223c5964e18 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformWriter.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformWriter.cpp
@@ -46,14 +46,16 @@
#include <RobotAPI/libraries/armem_robot_state/aron/Transform.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/aron_conversions.h>
-#include "RobotAPI/libraries/armem/core/MemoryID.h"
+#include <RobotAPI/libraries/armem/core/MemoryID.h>
+#include <RobotAPI/libraries/armem/core/error.h>
namespace armarx::armem::client::robot_state::localization
{
- TransformWriter::TransformWriter(armem::ClientWriterComponentPluginUser& memoryClient) : memoryClient(memoryClient) {}
+ TransformWriter::TransformWriter(armem::client::MemoryNameSystem& memoryNameSystem) :
+ memoryNameSystem(memoryNameSystem) {}
TransformWriter::~TransformWriter() = default;
@@ -73,16 +75,16 @@ namespace armarx::armem::client::robot_state::localization
// Wait for the memory to become available and add it as dependency.
ARMARX_IMPORTANT << "TransformWriter: Waiting for memory '" << properties.memoryName
<< "' ...";
- auto result = memoryClient.useMemory(properties.memoryName);
- if (not result.success)
+ try
{
- ARMARX_ERROR << result.errorMessage;
+ memoryWriter = memoryNameSystem.useWriter(properties.memoryName);
+ ARMARX_IMPORTANT << "TransformWriter: Connected to memory '" << properties.memoryName << "'";
+ }
+ catch (const armem::error::CouldNotResolveMemoryServer& e)
+ {
+ ARMARX_ERROR << e.what();
return;
}
-
- ARMARX_IMPORTANT << "TransformWriter: Connected to memory '" << properties.memoryName;
-
- memoryWriter.setWritingMemory(result.proxy);
}
bool TransformWriter::commitTransform(const ::armarx::armem::robot_state::Transform& transform)
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformWriter.h b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformWriter.h
index aa9e295bf19b34229697ad43725884469c9d1172..299c79b69222accb8290a7064229a9dfe59e8572 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformWriter.h
+++ b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformWriter.h
@@ -24,13 +24,12 @@
#include <mutex>
+#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
#include <RobotAPI/libraries/armem/client/Writer.h>
-// #include <RobotAPI/libraries/armem/client/MemoryConnector.h>
-
-#include <RobotAPI/libraries/armem/client.h>
#include "interfaces.h"
+
namespace armarx::armem::client::robot_state::localization
{
@@ -47,11 +46,10 @@ namespace armarx::armem::client::robot_state::localization
*/
class TransformWriter :
virtual public TransformWriterInterface
- // virtual public ::armarx::armem::MemoryConnector
{
public:
- TransformWriter(armem::ClientWriterComponentPluginUser& memoryClient);
+ TransformWriter(armem::client::MemoryNameSystem& memoryNameSystem);
~TransformWriter() override;
// TransformWriterInterface
@@ -64,19 +62,19 @@ namespace armarx::armem::client::robot_state::localization
bool commitTransform(const ::armarx::armem::robot_state::Transform& transform) override;
private:
+
+ armem::client::MemoryNameSystem& memoryNameSystem;
armem::client::Writer memoryWriter;
std::mutex memoryWriterMutex;
// Properties
struct Properties
{
- std::string memoryName = "RobotStateMemory";
+ std::string memoryName = "RobotState";
std::string localizationSegment = "Localization";
} properties;
const std::string propertyPrefix = "mem.robot_state.";
-
- armem::ClientWriterComponentPluginUser& memoryClient;
};
} // namespace armarx::armem::client::robot_state::localization
diff --git a/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.cpp b/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.cpp
index c8db83835ccc53f21e416b723745892a1455f801..a59aa5a4c6cdd3ebe552522c0ffd532e873da82a 100644
--- a/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.cpp
@@ -20,6 +20,44 @@
namespace armarx::armem::common::robot_state::localization
{
+ TransformChainResult TransformHelper::lookupTransformChain(const armem::wm::CoreSegment& localizationCoreSegment,
+ const TransformQuery& query)
+ {
+ const std::vector<std::string> tfChain =
+ buildTransformChain(localizationCoreSegment, query);
+
+ if (tfChain.empty())
+ {
+ return {.header = query.header,
+ .transforms = std::vector<Eigen::Affine3f>{},
+ .status = TransformChainResult::Status::ErrorFrameNotAvailable,
+ .errorMessage = "Cannot create tf lookup chain '" +
+ query.header.parentFrame + " -> " + query.header.frame +
+ "'"};
+ }
+
+ const std::vector<Eigen::Affine3f> transforms = obtainTransforms(
+ localizationCoreSegment, tfChain, query.header.agent, query.header.timestamp);
+
+ if (transforms.empty())
+ {
+ ARMARX_WARNING << deactivateSpam(1) << "No transform available.";
+ return {.header = query.header,
+ .transforms = {},
+ .status = TransformChainResult::Status::ErrorFrameNotAvailable,
+ .errorMessage = "Error in TF loookup: '" + query.header.parentFrame +
+ " -> " + query.header.frame +
+ "'. No memory data in time range."};
+ }
+
+
+ ARMARX_DEBUG << "Found valid transform";
+
+ return {.header = query.header,
+ .transforms = transforms,
+ .status = TransformChainResult::Status::Success};
+ }
+
TransformResult TransformHelper::lookupTransform(const armem::wm::CoreSegment& localizationCoreSegment,
const TransformQuery& query)
{
@@ -284,14 +322,17 @@ namespace armarx::armem::common::robot_state::localization
std::vector<::armarx::armem::robot_state::Transform> transforms;
transforms.reserve(entity.history().size());
- const auto entitySnapshots = simox::alg::get_values(entity.history());
+ // const auto entitySnapshots = simox::alg::get_values(entity.history());
+
+ const std::vector<wm::EntitySnapshot> entitySnapshots = {entity.getLatestSnapshot()};
+
std::transform(
entitySnapshots.begin(),
entitySnapshots.end(),
std::back_inserter(transforms),
- [](const auto & entity)
+ [](const auto & entitySnapshot)
{
- return convertEntityToTransform(entity.getInstance(0));
+ return convertEntityToTransform(entitySnapshot.getInstance(0));
});
ARMARX_DEBUG << "obtaining transform";
@@ -312,11 +353,11 @@ namespace armarx::armem::common::robot_state::localization
{
ARMARX_DEBUG << "empty transform";
- throw armem::error::MissingEntry("foo", "bar", "foo2", "bar2");
+ throw armem::error::MissingEntry("foo", "bar", "foo2", "bar2", 0);
}
ARMARX_DEBUG << "single transform";
return transforms.front().transform;
}
-} // namespace armarx::armem::common::robot_state::localization
\ No newline at end of file
+} // namespace armarx::armem::common::robot_state::localization
diff --git a/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.h b/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.h
index 8045c552e839b657c47dca5aabe6f76e0c384d0c..cafbedcbca60f859f6fe158190a5985350ddad65 100644
--- a/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.h
+++ b/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.h
@@ -46,6 +46,10 @@ namespace armarx::armem::common::robot_state::localization
lookupTransform(const armem::wm::CoreSegment& localizationCoreSegment,
const TransformQuery& query);
+ static TransformChainResult
+ lookupTransformChain(const armem::wm::CoreSegment& localizationCoreSegment,
+ const TransformQuery& query);
+
private:
static std::vector<std::string>
buildTransformChain(const armem::wm::CoreSegment& localizationCoreSegment,
diff --git a/source/RobotAPI/libraries/armem_robot_state/common/localization/types.h b/source/RobotAPI/libraries/armem_robot_state/common/localization/types.h
index 7f0c7c3396c7688ebe50b854579b107e9cd55b6f..e16d599ba8fdd21f653a951ea16fdda6571bf243 100644
--- a/source/RobotAPI/libraries/armem_robot_state/common/localization/types.h
+++ b/source/RobotAPI/libraries/armem_robot_state/common/localization/types.h
@@ -26,6 +26,7 @@
#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
+#include <Eigen/src/Geometry/Transform.h>
#include <RobotAPI/libraries/armem_robot_state/types.h>
namespace armarx::armem::common::robot_state::localization
@@ -50,6 +51,27 @@ namespace armarx::armem::common::robot_state::localization
std::string errorMessage = "";
};
+ struct TransformChainResult
+ {
+ ::armarx::armem::robot_state::TransformHeader header;
+ std::vector<Eigen::Affine3f> transforms;
+
+ enum class Status
+ {
+ Success,
+ Error,
+ ErrorLookupIntoFuture,
+ ErrorFrameNotAvailable
+ } status;
+
+ explicit operator bool() const
+ {
+ return status == Status::Success;
+ }
+
+ std::string errorMessage = "";
+ };
+
struct TransformQuery
{
::armarx::armem::robot_state::TransformHeader header;
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.cpp b/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.cpp
index 73f17915af1c3497fb8b5627d629fd7b2373a90c..9762ea0339755892ac79ef62dbfcc88353995b00 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.cpp
@@ -1,5 +1,6 @@
#include "Visu.h"
+#include <Eigen/src/Geometry/Transform.h>
#include <algorithm>
#include <Eigen/Geometry>
@@ -13,34 +14,31 @@
#include "ArmarXCore/core/time/CycleUtil.h"
#include <ArmarXCore/core/time/TimeUtil.h>
+#include "RobotAPI/components/ArViz/Client/Elements.h"
+#include "RobotAPI/libraries/armem/core/Time.h"
#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
#include <exception>
-#include "RobotAPI/libraries/armem/core/Time.h"
+#include <string>
+#include "../description/Segment.h"
#include "../localization/Segment.h"
#include "../proprioception/Segment.h"
-#include "../description/Segment.h"
namespace armarx::armem::server::robot_state
{
void Visu::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
{
- defs->optional(p.enabled, prefix + "enabled",
- "Enable or disable visualization of objects.");
- defs->optional(p.frequencyHz, prefix + "frequenzyHz",
- "Frequency of visualization.");
+ defs->optional(
+ p.enabled, prefix + "enabled", "Enable or disable visualization of objects.");
+ defs->optional(p.frequencyHz, prefix + "frequenzyHz", "Frequency of visualization.");
}
- void Visu::visualizeRobots(
- viz::Layer& layer,
- const robot::Robots& robots
- )
+ void Visu::visualizeRobots(viz::Layer& layer, const robot::Robots& robots)
{
const auto visualizeRobot = [&](const robot::Robot & robot)
{
-
const auto xmlPath = robot.description.xml.serialize();
// clang-format off
@@ -57,14 +55,33 @@ namespace armarx::armem::server::robot_state
};
std::for_each(robots.begin(), robots.end(), visualizeRobot);
-
}
- void Visu::init()
+ void Visu::visualizeFrames(
+ viz::Layer& layerFrames,
+ const std::unordered_map<std::string, std::vector<Eigen::Affine3f>>& frames)
{
+ for (const auto& [robotName, robotFrames] : frames)
+ {
+ Eigen::Affine3f pose = Eigen::Affine3f::Identity();
+ int i = 0;
+ for (const auto& frame : robotFrames)
+ {
+ Eigen::Affine3f from = pose;
+
+ pose = pose * frame; // to
+
+ Eigen::Affine3f to = pose;
+
+ const auto arr = viz::Arrow(robotName + std::to_string(++i)).fromTo(from.translation(), to.translation());
+ layerFrames.add(arr);
+ }
+ }
}
+ void Visu::init() {}
+
void Visu::connect(const viz::Client& arviz)
{
this->arviz = arviz;
@@ -126,54 +143,54 @@ namespace armarx::armem::server::robot_state
// visu.objectFramesScale = objectFramesScale.getValue();
// }
- robot::Robots combine(const std::unordered_map<std::string, robot::RobotDescription>& robotDescriptions,
- const std::unordered_map<std::string, Eigen::Affine3f>& globalRobotPoseMap,
- const std::unordered_map<std::string, std::map<std::string, float>>& robotJointPositionMap)
+ robot::Robots combine(
+ const std::unordered_map<std::string, robot::RobotDescription>& robotDescriptions,
+ const std::unordered_map<std::string, Eigen::Affine3f>& globalRobotPoseMap,
+ const std::unordered_map<std::string, std::map<std::string, float>>& robotJointPositionMap)
{
robot::Robots robots;
- for (const auto& [robotName, robotDescription] : robotDescriptions)
+ for (const auto &[robotName, robotDescription] : robotDescriptions)
{
const auto& globalPose = globalRobotPoseMap.find(robotName);
if (globalPose == globalRobotPoseMap.end())
{
- ARMARX_WARNING << deactivateSpam(10) << "No global pose for robot '" << robotName << "'";
+ ARMARX_WARNING << deactivateSpam(10) << "No global pose for robot '" << robotName
+ << "'";
continue;
}
const auto& jointMap = robotJointPositionMap.find(robotName);
if (jointMap == robotJointPositionMap.end())
{
- ARMARX_WARNING << deactivateSpam(10) << "No joint positions for robot '" << robotName << "'";
+ ARMARX_WARNING << deactivateSpam(10) << "No joint positions for robot '"
+ << robotName << "'";
continue;
}
- ARMARX_DEBUG << "Found the following joints for robot " << robotName << ": " << simox::alg::get_keys(jointMap->second);
+ ARMARX_DEBUG << "Found the following joints for robot " << robotName << ": "
+ << simox::alg::get_keys(jointMap->second);
// TODO(fabian.reister): based on data
const armem::Time timestamp = IceUtil::Time::now();
- robots.emplace_back(
- robot::Robot
+ robots.emplace_back(robot::Robot
{
.description = robotDescription,
- .instance = "", // TODO(fabian.reister): set this properly
- .config = {
- .timestamp = timestamp,
+ .instance = "", // TODO(fabian.reister): set this properly
+ .config = {
+ .timestamp = timestamp,
.globalPose = globalPose->second,
- .jointMap = jointMap->second
+ .jointMap = jointMap->second
},
- .timestamp = timestamp,
- }
- );
+ .timestamp = timestamp,
+ });
}
return robots;
-
}
-
void Visu::visualizeRun()
{
@@ -194,10 +211,14 @@ namespace armarx::armem::server::robot_state
try
{
- const auto robotDescriptions = descriptionSegment.getRobotDescriptions(timestamp);
- const auto globalRobotPoseMap = localizationSegment.getRobotGlobalPoses(timestamp);
- const auto robotJointPositionMap = proprioceptionSegment.getRobotJointPositions(timestamp);
+ const auto robotDescriptions =
+ descriptionSegment.getRobotDescriptions(timestamp);
+ const auto globalRobotPoseMap =
+ localizationSegment.getRobotGlobalPoses(timestamp);
+ const auto robotJointPositionMap =
+ proprioceptionSegment.getRobotJointPositions(timestamp);
+ const auto frames = localizationSegment.getRobotFramePoses(timestamp);
// we need all 3 informations:
// - robot description
@@ -205,7 +226,8 @@ namespace armarx::armem::server::robot_state
// - joint positions
// => this is nothing but a armem::Robot
- const robot::Robots robots = combine(robotDescriptions, globalRobotPoseMap, robotJointPositionMap);
+ const robot::Robots robots =
+ combine(robotDescriptions, globalRobotPoseMap, robotJointPositionMap);
viz::Layer layer = arviz.layer("Robots");
ARMARX_DEBUG << "visualizing robots";
@@ -213,10 +235,16 @@ namespace armarx::armem::server::robot_state
ARMARX_DEBUG << "Committing robots";
- arviz.commit({layer});
+ viz::Layer layerFrames = arviz.layer("Frames");
- ARMARX_DEBUG << "Done committing";
+ ARMARX_DEBUG << "visualizing frames";
+ visualizeFrames(layerFrames, frames);
+
+ ARMARX_DEBUG << "Committing frames";
+ arviz.commit({layer, layerFrames});
+
+ ARMARX_DEBUG << "Done committing";
TIMING_END_STREAM(Visu, ARMARX_VERBOSE);
@@ -227,7 +255,6 @@ namespace armarx::armem::server::robot_state
// { "t Visualize [ms]", new Variant(Visu.toMilliSecondsDouble()) },
// });
// }
-
}
catch (const std::exception& ex)
{
@@ -239,13 +266,10 @@ namespace armarx::armem::server::robot_state
ARMARX_WARNING << "Unknown exception";
continue;
}
-
}
}
cycle.waitForCycleDuration();
}
}
-
-
-} // namespace armarx::armem::server::robot_state
+} // namespace armarx::armem::server::robot_state
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.h b/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.h
index dad4e49feb530109596a57fab7b52742b421e15c..7e4b3814ce3a2c03f4ea8fb38846d4378c3a18c6 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.h
+++ b/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.h
@@ -83,6 +83,10 @@ namespace armarx::armem::server::robot_state
const robot::Robots& robots
);
+ static void visualizeFrames(
+ viz::Layer& layerFrames,
+ const std::unordered_map<std::string, std::vector<Eigen::Affine3f>>& frames);
+
private:
viz::Client arviz;
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.cpp b/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.cpp
index 826c5b484768686e2edb8cee409df4136bcc85ab..d50d965608f7cf4ad8541467d75442303a293211 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.cpp
@@ -18,7 +18,7 @@
#include <RobotAPI/libraries/armem/client/query/Builder.h>
#include <RobotAPI/libraries/armem/client/query/query_fns.h>
#include <RobotAPI/libraries/armem/core/aron_conversions.h>
-#include <RobotAPI/libraries/armem/core/workingmemory/Visitor.h>
+#include <RobotAPI/libraries/armem/core/workingmemory/visitor.h>
#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
@@ -55,7 +55,7 @@ namespace armarx::armem::server::robot_state::description
ARMARX_INFO << "Adding core segment '" << p.coreSegment << "'";
coreSegment = &iceMemory.workingMemory->addCoreSegment(
- p.coreSegment, arondto::RobotDescription::toInitialAronType());
+ p.coreSegment, arondto::RobotDescription::toAronType());
coreSegment->setMaxHistorySize(p.maxHistorySize);
}
@@ -73,7 +73,7 @@ namespace armarx::armem::server::robot_state::description
const Time now = TimeUtil::GetTime();
const MemoryID providerID = coreSegment->id().withProviderSegmentName(robotDescription.name);
- coreSegment->addProviderSegment(providerID.providerSegmentName, arondto::RobotDescription::toInitialAronType());
+ coreSegment->addProviderSegment(providerID.providerSegmentName, arondto::RobotDescription::toAronType());
EntityUpdate update;
update.entityID = providerID.withEntityName("description");
@@ -99,25 +99,8 @@ namespace armarx::armem::server::robot_state::description
void Segment::updateRobotDescription()
{
ARMARX_CHECK_NOT_NULL(robotUnit);
+ auto kinematicUnit = robotUnit->getKinematicUnit();
- const auto waitForKinematicUnit = [&]()
- {
- while (true)
- {
- auto kinematicUnit = robotUnit->getKinematicUnit();
-
- if (kinematicUnit)
- {
- ARMARX_INFO << "Kinematic unit is now available.";
- return kinematicUnit;
- }
-
- ARMARX_INFO << "Waiting for kinematic unit ...";
- std::this_thread::sleep_for(std::chrono::seconds(1));
- }
- };
-
- auto kinematicUnit = waitForKinematicUnit();
ARMARX_CHECK_NOT_NULL(kinematicUnit);
const auto robotName = kinematicUnit->getRobotName();
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.cpp b/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.cpp
index 3137127aa112528fdf39351b9bfc52e4c4ece3b8..e0dcf6c2bcd9d86012d98cf5aded6fee585ed866 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.cpp
@@ -19,24 +19,22 @@
#include <RobotAPI/libraries/aron/common/aron_conversions.h>
-#include <RobotAPI/libraries/armem/core/aron_conversions.h>
-#include <RobotAPI/libraries/armem/core/MemoryID.h>
-#include <RobotAPI/libraries/armem/core/Time.h>
#include <RobotAPI/libraries/armem/client/Writer.h>
#include <RobotAPI/libraries/armem/client/query/Builder.h>
#include <RobotAPI/libraries/armem/client/query/query_fns.h>
+#include <RobotAPI/libraries/armem/core/MemoryID.h>
+#include <RobotAPI/libraries/armem/core/Time.h>
#include <RobotAPI/libraries/armem/core/aron_conversions.h>
-#include <RobotAPI/libraries/armem/core/workingmemory/Visitor.h>
+#include <RobotAPI/libraries/armem/core/workingmemory/visitor.h>
#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
-#include <RobotAPI/libraries/armem_robot/robot_conversions.h>
#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot/robot_conversions.h>
#include <RobotAPI/libraries/armem_robot_state/aron/Transform.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/aron_conversions.h>
-#include <RobotAPI/libraries/armem_robot_state/common/localization/types.h>
#include <RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.h>
-
+#include <RobotAPI/libraries/armem_robot_state/common/localization/types.h>
namespace armarx::armem::server::robot_state::localization
{
@@ -67,7 +65,7 @@ namespace armarx::armem::server::robot_state::localization
ARMARX_INFO << "Adding core segment '" << p.coreSegment << "'";
coreSegment = &iceMemory.workingMemory->addCoreSegment(
- p.coreSegment, arondto::Transform::toInitialAronType());
+ p.coreSegment, arondto::Transform::toAronType());
coreSegment->setMaxHistorySize(p.maxHistorySize);
}
@@ -76,7 +74,45 @@ namespace armarx::armem::server::robot_state::localization
// this->visu = std::make_unique<Visu>(arviz, *this);
}
- std::unordered_map<std::string, Eigen::Affine3f> Segment::getRobotGlobalPoses(const armem::Time& timestamp) const
+ std::unordered_map<std::string, std::vector<Eigen::Affine3f>>
+ Segment::getRobotFramePoses(const armem::Time& timestamp) const
+ {
+ using common::robot_state::localization::TransformHelper;
+ using common::robot_state::localization::TransformQuery;
+
+ std::unordered_map<std::string, std::vector<Eigen::Affine3f>> frames;
+
+ const auto& localizationCoreSegment =
+ iceMemory.workingMemory->getCoreSegment(p.coreSegment);
+ const auto knownRobots = simox::alg::get_keys(localizationCoreSegment);
+
+ for (const auto& robotName : knownRobots)
+ {
+
+ TransformQuery query{.header{.parentFrame = GlobalFrame,
+ .frame = "root", // TODO, FIXME
+ .agent = robotName,
+ .timestamp = timestamp}};
+
+ const auto result = TransformHelper::lookupTransformChain(localizationCoreSegment, query);
+
+ if (not result)
+ {
+ // TODO
+ continue;
+ }
+
+ frames.emplace(robotName, result.transforms);
+ }
+
+ ARMARX_INFO << deactivateSpam(10)
+ << "Number of known robot pose chains: " << frames.size();
+
+ return frames;
+ }
+
+ std::unordered_map<std::string, Eigen::Affine3f>
+ Segment::getRobotGlobalPoses(const armem::Time& timestamp) const
{
using common::robot_state::localization::TransformHelper;
@@ -91,15 +127,12 @@ namespace armarx::armem::server::robot_state::localization
for (const auto& robotName : knownRobots)
{
- TransformQuery query{.header{
- .parentFrame = GlobalFrame,
- .frame = "root", // TODO, FIXME
- .agent = robotName,
- .timestamp = timestamp
- }};
+ TransformQuery query{.header{.parentFrame = GlobalFrame,
+ .frame = "root", // TODO, FIXME
+ .agent = robotName,
+ .timestamp = timestamp}};
- const auto result =
- TransformHelper::lookupTransform(localizationCoreSegment, query);
+ const auto result = TransformHelper::lookupTransform(localizationCoreSegment, query);
if (not result)
{
@@ -124,14 +157,14 @@ namespace armarx::armem::server::robot_state::localization
coreSegment->id().withProviderSegmentName(transform.header.agent);
if (not coreSegment->hasProviderSegment(providerID.providerSegmentName))
{
- coreSegment->addProviderSegment(providerID.providerSegmentName);
+ coreSegment->addProviderSegment(providerID.providerSegmentName, arondto::Transform::toAronType());
}
Commit commit;
EntityUpdate& update = commit.updates.emplace_back();
- update.entityID = providerID.withEntityName(transform.header.parentFrame + "," +
- transform.header.frame);
+ update.entityID =
+ providerID.withEntityName(transform.header.parentFrame + "," + transform.header.frame);
update.timeArrived = update.timeCreated = update.timeSent = timestamp;
arondto::Transform aronTransform;
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.h b/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.h
index c23e9bcdce71f64288cec1a53c5c82d816327f92..307eaae17babf66aa9d0a1e759cbc348cbb05008 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.h
+++ b/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.h
@@ -71,6 +71,7 @@ namespace armarx::armem::server::robot_state::localization
void init();
std::unordered_map<std::string, Eigen::Affine3f> getRobotGlobalPoses(const armem::Time& timestamp) const;
+ std::unordered_map<std::string, std::vector<Eigen::Affine3f>> getRobotFramePoses(const armem::Time& timestamp) const;
bool storeTransform(const armarx::armem::robot_state::Transform& transform);
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.cpp b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.cpp
index 5ec63c12d9c3f6485839ff138b1ebe409dd1adc8..ac5e5e1c36e66d47251700ec6565a9fb61ce5430 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.cpp
@@ -10,7 +10,7 @@
#include "RobotAPI/libraries/aron/common/aron_conversions.h"
#include <RobotAPI/libraries/armem/core/aron_conversions.h>
-#include <RobotAPI/libraries/armem/core/workingmemory/Visitor.h>
+#include <RobotAPI/libraries/armem/core/workingmemory/visitor.h>
#include "RobotAPI/libraries/armem/core/MemoryID.h"
#include <RobotAPI/libraries/armem/client/Writer.h>
#include <RobotAPI/libraries/armem/client/query/Builder.h>
@@ -93,21 +93,29 @@ namespace armarx::armem::server::robot_state::proprioception
{
std::unordered_map<std::string, std::map<std::string, float>> jointMap;
+ std::lock_guard g{memoryMutex};
+
for (const auto& [robotName, provSeg] : iceMemory.workingMemory->getCoreSegment(p.coreSegment))
{
for (const auto& [name, entity] : provSeg.entities())
{
- const auto& entityInstance = entity.getLatestSnapshot().getInstance(0);
+ try
+ {
+ const auto& entityInstance = entity.getLatestSnapshot().getInstance(0);
+ const auto jointState = tryCast<armarx::armem::arondto::JointState>(entityInstance);
- const auto jointState = tryCast<armarx::armem::arondto::JointState>(entityInstance);
+ if (not jointState)
+ {
+ // ARMARX_WARNING << "Could not convert entity instance to 'JointState'";
+ continue;
+ }
+ jointMap[robotName].emplace(jointState->name, jointState->position);
- if (not jointState)
- {
- // ARMARX_WARNING << "Could not convert entity instance to 'JointState'";
- continue;
}
+ catch (...) // empty history etc
+ {
- jointMap[robotName].emplace(jointState->name, jointState->position);
+ }
}
}
diff --git a/source/RobotAPI/libraries/armem_vision/CMakeLists.txt b/source/RobotAPI/libraries/armem_vision/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..1767c9cebd878b1d15cad6f8043a2bd7cf932814
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_vision/CMakeLists.txt
@@ -0,0 +1,43 @@
+set(LIB_NAME armem_vision)
+
+armarx_component_set_name("${LIB_NAME}")
+armarx_set_target("Library: ${LIB_NAME}")
+
+armarx_add_library(
+ LIBS
+ # ArmarX
+ ArmarXCore
+ # This package
+ RobotAPI::Core
+ RobotAPI::armem
+ aroncommon
+ # System / External
+ Eigen3::Eigen
+ HEADERS
+ ./aron_conversions.h
+ ./client/laser_scans/Reader.h
+ ./client/laser_scans/Writer.h
+ ./client/occupancy_grid/Reader.h
+ ./client/occupancy_grid/Writer.h
+ SOURCES
+ ./aron_conversions.cpp
+ ./client/laser_scans/Reader.cpp
+ ./client/laser_scans/Writer.cpp
+ ./client/occupancy_grid/Reader.cpp
+ ./client/occupancy_grid/Writer.cpp
+ ./OccupancyGridHelper.cpp
+)
+
+armarx_enable_aron_file_generation_for_target(
+ TARGET_NAME
+ "${LIB_NAME}"
+ ARON_FILES
+ aron/LaserScan.xml
+ aron/OccupancyGrid.xml
+)
+
+add_library(
+ RobotAPI::armem_vision
+ ALIAS
+ armem_vision
+)
diff --git a/source/RobotAPI/libraries/armem_vision/OccupancyGridHelper.cpp b/source/RobotAPI/libraries/armem_vision/OccupancyGridHelper.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..f88cc75454fd6f83545c3207c1e58642bedcc225
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_vision/OccupancyGridHelper.cpp
@@ -0,0 +1,39 @@
+#include "OccupancyGridHelper.h"
+
+#include "types.h"
+
+namespace armarx
+{
+ OccupancyGridHelper::OccupancyGridHelper(const OccupancyGrid& occupancyGrid,
+ const Params& params) :
+ occupancyGrid(occupancyGrid), params(params)
+ {
+ }
+
+ OccupancyGridHelper::BinaryArray OccupancyGridHelper::knownCells() const
+ {
+ return (occupancyGrid.grid > 0.F).cast<bool>();
+ }
+
+ OccupancyGridHelper::BinaryArray OccupancyGridHelper::freespace() const
+ {
+ // matrix1 = matrix1 .unaryExpr(std::ptr_fun(ReplaceNanWithValue<1>));
+ // return (occupancyGrid.grid ).cast<bool>();
+
+ const auto isFree = [&](OccupancyGrid::CellType p) -> float
+ { return static_cast<float>(p < params.freespaceThreshold and p > 0.F); };
+
+ // TODO(fabian.reister): which one to choose?
+ // return occupancyGrid.grid.unaryExpr(isFree).cast<bool>();
+ return occupancyGrid.grid.unaryViewExpr(isFree).cast<bool>();
+ }
+
+ OccupancyGridHelper::BinaryArray OccupancyGridHelper::obstacles() const
+ {
+ const auto isOccupied = [&](OccupancyGrid::CellType p) -> float
+ { return static_cast<float>(p > params.occupiedThreshold); };
+
+ return occupancyGrid.grid.unaryViewExpr(isOccupied).cast<bool>();
+ }
+
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/armem_vision/OccupancyGridHelper.h b/source/RobotAPI/libraries/armem_vision/OccupancyGridHelper.h
new file mode 100644
index 0000000000000000000000000000000000000000..17628ef34fc4ef30fc2b46dd8f031f2fc46dbda3
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_vision/OccupancyGridHelper.h
@@ -0,0 +1,40 @@
+#pragma once
+
+#include <Eigen/Core>
+
+namespace armarx::armem
+{
+ struct OccupancyGrid;
+}
+
+namespace armarx
+{
+ using armarx::armem::OccupancyGrid;
+
+ namespace detail
+ {
+ struct OccupancyGridHelperParams
+ {
+ float freespaceThreshold = 0.45F;
+ float occupiedThreshold = 0.55F;
+ };
+ }
+
+ class OccupancyGridHelper
+ {
+ public:
+ using Params = detail::OccupancyGridHelperParams;
+
+ OccupancyGridHelper(const OccupancyGrid& occupancyGrid, const Params& params);
+
+ using BinaryArray = Eigen::Array<bool, Eigen::Dynamic, Eigen::Dynamic>;
+
+ BinaryArray knownCells() const;
+ BinaryArray freespace() const;
+ BinaryArray obstacles() const;
+
+ private:
+ const OccupancyGrid& occupancyGrid;
+ const Params params;
+ };
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/armem_robot_mapping/aron/LaserScan.xml b/source/RobotAPI/libraries/armem_vision/aron/LaserScan.xml
similarity index 83%
rename from source/RobotAPI/libraries/armem_robot_mapping/aron/LaserScan.xml
rename to source/RobotAPI/libraries/armem_vision/aron/LaserScan.xml
index 659df4a1536b0f81b81ba64f77e4049ba42fdd5c..f2d11c2e4111dfd0c24896ba807aef6435b7014b 100644
--- a/source/RobotAPI/libraries/armem_robot_mapping/aron/LaserScan.xml
+++ b/source/RobotAPI/libraries/armem_vision/aron/LaserScan.xml
@@ -8,7 +8,7 @@
<GenerateTypes>
- <Object name='armarx::arondto::LaserScannerInfo'>
+ <Object name='armarx::armem::arondto::LaserScannerInfo'>
<ObjectChild key='device'>
<string />
</ObjectChild>
@@ -26,7 +26,7 @@
</ObjectChild>
</Object>
- <Object name="armarx::arondto::SensorHeader">
+ <Object name="armarx::armem::arondto::SensorHeader">
<ObjectChild key="agent">
<string/>
</ObjectChild>
@@ -39,9 +39,9 @@
</Object>
- <Object name='armarx::arondto::LaserScanStamped'>
+ <Object name='armarx::armem::arondto::LaserScanStamped'>
<ObjectChild key="header">
- <armarx::arondto::SensorHeader />
+ <armarx::armem::arondto::SensorHeader />
</ObjectChild>
<!--
diff --git a/source/RobotAPI/libraries/armem_vision/aron/OccupancyGrid.xml b/source/RobotAPI/libraries/armem_vision/aron/OccupancyGrid.xml
new file mode 100644
index 0000000000000000000000000000000000000000..0c508a4e2138b4b04c126287bf46d8826fb3da6f
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_vision/aron/OccupancyGrid.xml
@@ -0,0 +1,30 @@
+<!--Some fancy comment -->
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+ <CodeIncludes>
+ </CodeIncludes>
+ <AronIncludes>
+ </AronIncludes>
+
+ <GenerateTypes>
+
+ <Object name='armarx::armem::arondto::OccupancyGrid'>
+ <ObjectChild key='resolution'>
+ <float />
+ </ObjectChild>
+ <ObjectChild key='frame'>
+ <string />
+ </ObjectChild>
+ <ObjectChild key='pose'>
+ <Pose />
+ </ObjectChild>
+
+ <!--
+ <ObjectChild key='grid'>
+ <NdArray />
+ </ObjectChild> -->
+ </Object>
+
+
+ </GenerateTypes>
+</AronTypeDefinition>
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/armem_robot_mapping/aron_conversions.cpp b/source/RobotAPI/libraries/armem_vision/aron_conversions.cpp
similarity index 63%
rename from source/RobotAPI/libraries/armem_robot_mapping/aron_conversions.cpp
rename to source/RobotAPI/libraries/armem_vision/aron_conversions.cpp
index b37a88381383b41ea6c3ca9bd3075a50e8b733ef..47ada08bb62a3ae78fe9520255be23b1f92e7612 100644
--- a/source/RobotAPI/libraries/armem_robot_mapping/aron_conversions.cpp
+++ b/source/RobotAPI/libraries/armem_vision/aron_conversions.cpp
@@ -5,29 +5,22 @@
#include <iterator>
#include <RobotAPI/interface/units/LaserScannerUnit.h>
-#include <RobotAPI/libraries/armem_robot_mapping/aron/LaserScan.aron.generated.h>
+#include <RobotAPI/libraries/armem_vision/aron/LaserScan.aron.generated.h>
+#include <RobotAPI/libraries/aron/common/aron_conversions.h>
#include <RobotAPI/libraries/aron/converter/common/Converter.h>
#include <RobotAPI/libraries/aron/core/navigator/data/complex/NDArray.h>
#include "types.h"
-
-namespace armarx
+namespace armarx::armem
{
/************ fromAron ************/
-
- armem::Time timeFromAron(const int64_t timestamp)
- {
- return armem::Time::microSeconds(timestamp);
- }
-
SensorHeader fromAron(const arondto::SensorHeader& aronSensorHeader)
{
-
- return {.agent = aronSensorHeader.agent,
- .frame = aronSensorHeader.frame,
- .timestamp = timeFromAron(aronSensorHeader.timestamp)};
+ return {.agent = aronSensorHeader.agent,
+ .frame = aronSensorHeader.frame,
+ .timestamp = aronSensorHeader.timestamp};
}
void fromAron(const arondto::LaserScanStamped& aronLaserScan,
@@ -37,8 +30,10 @@ namespace armarx
// laserScan.data = fromAron(aronLaserScan.data);
}
- void fromAron(const arondto::LaserScanStamped& aronLaserScan, LaserScan& laserScan,
- std::int64_t& timestamp, std::string& frame,
+ void fromAron(const arondto::LaserScanStamped& aronLaserScan,
+ LaserScan& laserScan,
+ std::int64_t& timestamp,
+ std::string& frame,
std::string& agentName)
{
const auto header = fromAron(aronLaserScan.header);
@@ -46,14 +41,12 @@ namespace armarx
// laserScan = fromAron(aronLaserScan.data);
timestamp = header.timestamp.toMicroSeconds();
- frame = header.frame;
+ frame = header.frame;
agentName = header.agent;
}
-
/************ toAron ************/
-
// auto toAron(const LaserScan& laserScan, aron::LaserScan& aronLaserScan)
// {
// aronLaserScan.scan = toAron(laserScan);
@@ -68,9 +61,9 @@ namespace armarx
{
arondto::SensorHeader aronSensorHeader;
- aronSensorHeader.agent = sensorHeader.agent;
- aronSensorHeader.frame = sensorHeader.frame;
- aronSensorHeader.timestamp = toAron(sensorHeader.timestamp);
+ aronSensorHeader.agent = sensorHeader.agent;
+ aronSensorHeader.frame = sensorHeader.frame;
+ aronSensorHeader.timestamp = sensorHeader.timestamp;
return aronSensorHeader;
}
@@ -92,9 +85,26 @@ namespace armarx
{
.agent = agentName, .frame = frame, .timestamp = timestamp};
- const LaserScanStamped laserScanStamped{.header = header, .data = laserScan};
+ const LaserScanStamped laserScanStamped{.header = header,
+ .data = laserScan};
toAron(laserScanStamped, aronLaserScanStamped);
}
-} // namespace armarx
\ No newline at end of file
+ void toAron(arondto::OccupancyGrid& dto, const OccupancyGrid& bo)
+ {
+ aron::toAron(dto.frame, bo.frame);
+ aron::toAron(dto.pose, bo.pose);
+ aron::toAron(dto.resolution, bo.resolution);
+ // bo.grid is NdArray -> need special handling.
+ }
+
+ void fromAron(const arondto::OccupancyGrid& dto, OccupancyGrid& bo)
+ {
+ aron::fromAron(dto.frame, bo.frame);
+ aron::fromAron(dto.pose, bo.pose);
+ aron::fromAron(dto.resolution, bo.resolution);
+ // bo.grid is NdArray -> need special handling.
+ }
+
+} // namespace armarx::armem
diff --git a/source/RobotAPI/libraries/armem_vision/aron_conversions.h b/source/RobotAPI/libraries/armem_vision/aron_conversions.h
new file mode 100644
index 0000000000000000000000000000000000000000..db97ad5076457805d953100201821f912a046291
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_vision/aron_conversions.h
@@ -0,0 +1,82 @@
+/*
+ * 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/>.
+ *
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include "RobotAPI/libraries/armem_vision/types.h"
+#include <RobotAPI/interface/units/LaserScannerUnit.h>
+#include <RobotAPI/libraries/armem/core/Time.h>
+#include <RobotAPI/libraries/armem_vision/aron/OccupancyGrid.aron.generated.h>
+#include <RobotAPI/libraries/aron/converter/common/VectorConverter.h>
+#include <RobotAPI/libraries/aron/converter/eigen/EigenConverter.h>
+#include <RobotAPI/libraries/aron/core/navigator/data/complex/NDArray.h>
+
+namespace armarx::armem
+{
+
+ namespace arondto
+ {
+ struct LaserScanStamped;
+ } // namespace arondto
+
+ // struct LaserScan;
+ struct LaserScanStamped;
+
+ void fromAron(const arondto::LaserScanStamped& aronLaserScan,
+ LaserScan& laserScan,
+ std::int64_t& timestamp,
+ std::string& frame,
+ std::string& agentName);
+
+ template <typename T>
+ auto fromAron(const aron::datanavigator::NDArrayNavigatorPtr& navigator)
+ {
+ return aron::converter::AronVectorConverter::ConvertToVector<T>(
+ navigator);
+ }
+
+ void fromAron(const arondto::LaserScanStamped& aronLaserScan,
+ LaserScanStamped& laserScan);
+
+ void toAron(const LaserScan& laserScan,
+ const armem::Time& timestamp,
+ const std::string& frame,
+ const std::string& agentName,
+ arondto::LaserScanStamped& aronLaserScan);
+
+ inline aron::datanavigator::NDArrayNavigatorPtr
+ toAron(const LaserScan& laserScan)
+ {
+ using aron::converter::AronVectorConverter;
+ return AronVectorConverter::ConvertFromVector(laserScan);
+ }
+
+ // OccupancyGrid
+ void toAron(arondto::OccupancyGrid& dto, const OccupancyGrid& bo);
+ void fromAron(const arondto::OccupancyGrid& dto, OccupancyGrid& bo);
+
+ inline aron::datanavigator::NDArrayNavigatorPtr
+ toAron(const OccupancyGrid::Grid& grid)
+ {
+ return aron::converter::AronEigenConverter::ConvertFromArray(grid);
+ }
+
+} // namespace armarx::armem
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/armem_robot_mapping/MappingDataReader.cpp b/source/RobotAPI/libraries/armem_vision/client/laser_scans/Reader.cpp
similarity index 63%
rename from source/RobotAPI/libraries/armem_robot_mapping/MappingDataReader.cpp
rename to source/RobotAPI/libraries/armem_vision/client/laser_scans/Reader.cpp
index fd8250c5b2b086f3869729e2bf6836e5e3c5dc32..9c46b0c4729dfe6ca038c1f31ef81255f56d4968 100644
--- a/source/RobotAPI/libraries/armem_robot_mapping/MappingDataReader.cpp
+++ b/source/RobotAPI/libraries/armem_vision/client/laser_scans/Reader.cpp
@@ -1,31 +1,32 @@
-#include "MappingDataReader.h"
+#include "Reader.h"
// STD / STL
-#include <cstring>
-#include <vector>
#include <algorithm>
+#include <cstring>
#include <map>
#include <optional>
#include <ostream>
-#include <type_traits>
#include <utility>
+#include <vector>
+
+#include <type_traits>
// ICE
-#include <IceUtil/Time.h>
#include <IceUtil/Handle.h>
+#include <IceUtil/Time.h>
// Simox
#include <SimoxUtility/algorithm/get_map_keys_values.h>
// ArmarXCore
-#include <ArmarXCore/core/logging/Logging.h>
-#include <ArmarXCore/core/logging/LogSender.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/logging/LogSender.h>
+#include <ArmarXCore/core/logging/Logging.h>
// RobotAPI Interfaces
-#include <RobotAPI/interface/units/LaserScannerUnit.h>
#include <RobotAPI/interface/armem/mns/MemoryNameSystemInterface.h>
#include <RobotAPI/interface/armem/server/ReadingMemoryInterface.h>
+#include <RobotAPI/interface/units/LaserScannerUnit.h>
// RobotAPI Aron
#include <RobotAPI/libraries/aron/core/Exception.h>
@@ -37,71 +38,72 @@
#include <RobotAPI/libraries/armem/client/query/Builder.h>
#include <RobotAPI/libraries/armem/client/query/selectors.h>
+#include <RobotAPI/libraries/armem/core/error.h>
#include <RobotAPI/libraries/armem/core/workingmemory/Memory.h>
#include <RobotAPI/libraries/armem/core/workingmemory/CoreSegment.h>
#include <RobotAPI/libraries/armem/core/workingmemory/ProviderSegment.h>
#include <RobotAPI/libraries/armem/core/workingmemory/Entity.h>
#include <RobotAPI/libraries/armem/core/workingmemory/EntityInstance.h>
-
#include <RobotAPI/libraries/armem/util/util.h>
+#include <RobotAPI/libraries/armem_vision/aron/LaserScan.aron.generated.h>
+#include <RobotAPI/libraries/armem_vision/aron_conversions.h>
+#include <RobotAPI/libraries/armem_vision/types.h>
-#include <RobotAPI/libraries/armem_robot_mapping/aron/LaserScan.aron.generated.h>
-#include <RobotAPI/libraries/armem_robot_mapping/aron_conversions.h>
-#include <RobotAPI/libraries/armem_robot_mapping/types.h>
-
-namespace armarx::armem
+namespace armarx::armem::vision::laser_scans::client
{
- MappingDataReader::MappingDataReader(armem::ClientReaderComponentPluginUser& memoryClient)
- : memoryClient(memoryClient) {}
+ Reader::Reader(armem::client::MemoryNameSystem& memoryNameSystem) :
+ memoryNameSystem(memoryNameSystem)
+ {
+ }
+ Reader::~Reader() = default;
- MappingDataReader::~MappingDataReader() = default;
- void MappingDataReader::registerPropertyDefinitions(
- armarx::PropertyDefinitionsPtr& def)
+ void
+ Reader::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
{
ARMARX_DEBUG << "TransformReader: registerPropertyDefinitions";
registerPropertyDefinitions(def);
const std::string prefix = propertyPrefix;
- def->optional(properties.mappingMemoryName, prefix + "MappingMemoryName",
+ def->optional(properties.coreSegmentName,
+ prefix + "CoreSegment",
"Name of the mapping memory core segment to use.");
def->optional(properties.memoryName, prefix + "MemoryName");
}
- void MappingDataReader::connect()
+ void Reader::connect()
{
// Wait for the memory to become available and add it as dependency.
- ARMARX_IMPORTANT << "TransformReader: Waiting for memory '"
+ ARMARX_IMPORTANT << "MappingDataReader: Waiting for memory '"
<< properties.memoryName << "' ...";
- auto result = memoryClient.useMemory(properties.memoryName);
- if (not result.success)
+ try
{
- ARMARX_ERROR << result.errorMessage;
+ memoryReader = memoryNameSystem.useReader(MemoryID().withMemoryName(properties.memoryName));
+ ARMARX_IMPORTANT << "MappingDataReader: Connected to memory '" << properties.memoryName << "'";
+ }
+ catch (const armem::error::CouldNotResolveMemoryServer& e)
+ {
+ ARMARX_ERROR << e.what();
return;
}
-
- ARMARX_IMPORTANT << "TransformReader: Connected to memory '"
- << properties.memoryName;
-
- memoryReader.setReadingMemory(result.proxy);
}
- armem::client::query::Builder
- MappingDataReader::buildQuery(const Query& query) const
+ armarx::armem::client::query::Builder
+ Reader::buildQuery(const Query& query) const
{
- armem::client::query::Builder qb;
+ armarx::armem::client::query::Builder qb;
ARMARX_INFO << "Query for agent: " << query.agent
- << " memory name: " << properties.mappingMemoryName;
+ << " memory name: " << properties.memoryName;
if (query.sensorList.empty()) // all sensors
{
// clang-format off
qb
- .coreSegments().withName(properties.mappingMemoryName)
+ .coreSegments().withName(properties.memoryName)
.providerSegments().withName(query.agent)
.entities().all()
.snapshots().timeRange(query.timeRange.min, query.timeRange.max);
@@ -111,7 +113,7 @@ namespace armarx::armem
{
// clang-format off
qb
- .coreSegments().withName(properties.mappingMemoryName)
+ .coreSegments().withName(properties.memoryName)
.providerSegments().withName(query.agent)
.entities().withNames(query.sensorList)
.snapshots().timeRange(query.timeRange.min, query.timeRange.max);
@@ -119,10 +121,10 @@ namespace armarx::armem
}
return qb;
-
}
- std::vector<LaserScanStamped> asLaserScans(const std::map<std::string, wm::Entity>& entities)
+ std::vector<LaserScanStamped>
+ asLaserScans(const std::map<std::string, wm::Entity>& entities)
{
std::vector<LaserScanStamped> outV;
@@ -131,25 +133,27 @@ namespace armarx::armem
ARMARX_WARNING << "No entities!";
}
- const auto convert = [](const arondto::LaserScanStamped & aronLaserScanStamped, const wm::EntityInstance & ei) -> LaserScanStamped
+ const auto convert =
+ [](const arondto::LaserScanStamped & aronLaserScanStamped,
+ const wm::EntityInstance & ei) -> LaserScanStamped
{
LaserScanStamped laserScanStamped;
fromAron(aronLaserScanStamped, laserScanStamped);
- const auto ndArrayNavigator = aron::datanavigator::NDArrayNavigator::DynamicCast(ei.data()->getElement("scan"));
+ const auto ndArrayNavigator =
+ aron::datanavigator::NDArrayNavigator::DynamicCast(
+ ei.data()->getElement("scan"));
ARMARX_CHECK_NOT_NULL(ndArrayNavigator);
laserScanStamped.data = fromAron<LaserScanStep>(ndArrayNavigator);
ARMARX_IMPORTANT << "4";
-
return laserScanStamped;
-
};
// loop over all entities and their snapshots
- for (const auto &[s, entity] : entities)
+ for (const auto& [s, entity] : entities)
{
if (entity.empty())
{
@@ -158,11 +162,12 @@ namespace armarx::armem
ARMARX_DEBUG << "History size: " << entity.size();
- for (const auto &[ss, entitySnapshot] : entity)
+ for (const auto& [ss, entitySnapshot] : entity)
{
for (const auto& entityInstance : entitySnapshot.instances())
{
- const auto o = tryCast<arondto::LaserScanStamped>(entityInstance);
+ const auto o =
+ tryCast<arondto::LaserScanStamped>(entityInstance);
if (o)
{
@@ -175,8 +180,7 @@ namespace armarx::armem
return outV;
}
- MappingDataReader::Result
- MappingDataReader::queryData(const Query& query) const
+ Reader::Result Reader::queryData(const Query& query) const
{
const auto qb = buildQuery(query);
@@ -191,25 +195,25 @@ namespace armarx::armem
{
ARMARX_WARNING << "Failed to query data from memory: "
<< qResult.errorMessage;
- return {.laserScans = {},
- .sensors = {},
- .status = Result::Status::Error,
+ return {.laserScans = {},
+ .sensors = {},
+ .status = Result::Status::Error,
.errorMessage = qResult.errorMessage};
}
// now create result from memory
const auto& entities =
- qResult.memory.getCoreSegment(properties.mappingMemoryName)
+ qResult.memory.getCoreSegment(properties.memoryName)
.getProviderSegment(query.agent)
.entities();
const auto laserScans = asLaserScans(entities);
- const auto sensors = simox::alg::get_keys(entities);
+ const auto sensors = simox::alg::get_keys(entities);
- return {.laserScans = laserScans,
- .sensors = sensors,
- .status = Result::Status::Success,
+ return {.laserScans = laserScans,
+ .sensors = sensors,
+ .status = Result::Status::Success,
.errorMessage = ""};
}
-} // namespace armarx::armem
+} // namespace armarx::armem::vision::laser_scans::client
diff --git a/source/RobotAPI/libraries/armem_robot_mapping/MappingDataReader.h b/source/RobotAPI/libraries/armem_vision/client/laser_scans/Reader.h
similarity index 78%
rename from source/RobotAPI/libraries/armem_robot_mapping/MappingDataReader.h
rename to source/RobotAPI/libraries/armem_vision/client/laser_scans/Reader.h
index aee783764f4b5720e4ae0d8d69fbf6faed297e98..138717c42257e02275a15785940783330126ee4d 100644
--- a/source/RobotAPI/libraries/armem_robot_mapping/MappingDataReader.h
+++ b/source/RobotAPI/libraries/armem_vision/client/laser_scans/Reader.h
@@ -21,7 +21,7 @@
#pragma once
-#include <stdint.h>
+#include <cstdint>
#include <string>
#include <vector>
@@ -30,8 +30,8 @@
#include <RobotAPI/libraries/armem/client.h>
#include <RobotAPI/libraries/armem/client/Reader.h>
#include <RobotAPI/libraries/armem/client/query/Builder.h>
-#include <RobotAPI/libraries/armem_robot_mapping/types.h>
-
+#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
+#include <RobotAPI/libraries/armem_vision/types.h>
namespace armarx
@@ -39,7 +39,7 @@ namespace armarx
class ManagedIceObject;
}
-namespace armarx::armem
+namespace armarx::armem::vision::laser_scans::client
{
struct TimeRange
@@ -59,16 +59,15 @@ namespace armarx::armem
*
* Detailed description of class ExampleClient.
*/
- class MappingDataReader
+ class Reader
{
public:
- MappingDataReader(armem::ClientReaderComponentPluginUser& memoryClient);
- virtual ~MappingDataReader();
+ Reader(armem::client::MemoryNameSystem& memoryNameSystem);
+ virtual ~Reader();
void connect();
-
struct Query
{
std::string agent;
@@ -77,7 +76,6 @@ namespace armarx::armem
// if empty, all sensors will be queried
std::vector<std::string> sensorList;
-
};
struct Result
@@ -94,32 +92,29 @@ namespace armarx::armem
} status;
std::string errorMessage;
-
};
Result queryData(const Query& query) const;
void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def);
-
- client::query::Builder buildQuery(const Query& query) const ;
-
+ armarx::armem::client::query::Builder
+ buildQuery(const Query& query) const;
private:
+ armem::client::MemoryNameSystem& memoryNameSystem;
armem::client::Reader memoryReader;
// Properties
struct Properties
{
- std::string memoryName = "RobotState";
- std::string mappingMemoryName = "Mapping";
+ std::string memoryName = "Vision";
+ std::string coreSegmentName = "LaserScans";
} properties;
+ const std::string propertyPrefix = "mem.vision.";
- const std::string propertyPrefix = "mem.mapping.";
-
- armem::ClientReaderComponentPluginUser& memoryClient;
};
-} // namespace armarx::armem
+} // namespace armarx::armem::vision::laser_scans::client
diff --git a/source/RobotAPI/libraries/armem_vision/client/laser_scans/Writer.cpp b/source/RobotAPI/libraries/armem_vision/client/laser_scans/Writer.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..7358e504ffecf3a2a710bd77b1631437e3818a42
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_vision/client/laser_scans/Writer.cpp
@@ -0,0 +1,100 @@
+#include "Writer.h"
+
+#include <RobotAPI/libraries/armem/core/error.h>
+#include "RobotAPI/libraries/armem_vision/aron_conversions.h"
+#include <RobotAPI/libraries/armem_vision/aron/LaserScan.aron.generated.h>
+
+
+namespace armarx::armem::vision::laser_scans::client
+{
+
+ Writer::Writer(armem::client::MemoryNameSystem& memoryNameSystem)
+ : memoryNameSystem(memoryNameSystem) {}
+ Writer::~Writer() = default;
+
+
+ void
+ Writer::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
+ {
+ ARMARX_DEBUG << "LaserScansWriter: registerPropertyDefinitions";
+
+ const std::string prefix = propertyPrefix;
+
+ def->optional(properties.coreSegmentName,
+ prefix + "CoreSegment",
+ "Name of the mapping memory core segment to use.");
+
+ def->optional(properties.memoryName, prefix + "MemoryName");
+ }
+
+ void Writer::connect()
+ {
+ // Wait for the memory to become available and add it as dependency.
+ ARMARX_IMPORTANT << "LaserScansWriter: Waiting for memory '"
+ << properties.memoryName << "' ...";
+ try
+ {
+ memoryWriter = memoryNameSystem.useWriter(MemoryID().withMemoryName(properties.memoryName));
+ ARMARX_IMPORTANT << "MappingDataWriter: Connected to memory '" << properties.memoryName << "'";
+ }
+ catch (const armem::error::CouldNotResolveMemoryServer& e)
+ {
+ ARMARX_ERROR << e.what();
+ return;
+ }
+
+ ARMARX_IMPORTANT << "LaserScansWriter: Connected to memory '"
+ << properties.memoryName;
+ }
+
+ bool Writer::storeSensorData(const LaserScan& laserScan,
+ const std::string& frame,
+ const std::string& agentName,
+ const std::int64_t& timestamp)
+ {
+ std::lock_guard g{memoryWriterMutex};
+
+ const auto result =
+ memoryWriter.addSegment(properties.memoryName, agentName);
+
+ if (not result.success)
+ {
+ ARMARX_ERROR << result.errorMessage;
+
+ // TODO(fabian.reister): message
+ return false;
+ }
+
+ const auto iceTimestamp = Time::microSeconds(timestamp);
+
+ const auto providerId = armem::MemoryID(result.segmentID);
+ const auto entityID =
+ providerId.withEntityName(frame).withTimestamp(iceTimestamp);
+
+ armem::EntityUpdate update;
+ update.entityID = entityID;
+
+ arondto::LaserScanStamped aronSensorData;
+ // currently only sets the header
+ toAron(laserScan, iceTimestamp, frame, agentName, aronSensorData);
+
+ auto dict = aronSensorData.toAron();
+ dict->addElement("scan", toAron(laserScan));
+
+ update.instancesData = {dict};
+ update.timeCreated = iceTimestamp;
+
+ ARMARX_DEBUG << "Committing " << update << " at time " << iceTimestamp;
+ armem::EntityUpdateResult updateResult = memoryWriter.commit(update);
+
+ ARMARX_DEBUG << updateResult;
+
+ if (not updateResult.success)
+ {
+ ARMARX_ERROR << updateResult.errorMessage;
+ }
+
+ return updateResult.success;
+ }
+
+} // namespace armarx::armem::vision::laser_scans::client
diff --git a/source/RobotAPI/libraries/armem_robot_mapping/MappingDataWriter.h b/source/RobotAPI/libraries/armem_vision/client/laser_scans/Writer.h
similarity index 65%
rename from source/RobotAPI/libraries/armem_robot_mapping/MappingDataWriter.h
rename to source/RobotAPI/libraries/armem_vision/client/laser_scans/Writer.h
index e5de996746b7d54b46faa891cf2cd26007fbaf02..afc383634b87a31fc84a841a5658f83ec845ee27 100644
--- a/source/RobotAPI/libraries/armem_robot_mapping/MappingDataWriter.h
+++ b/source/RobotAPI/libraries/armem_vision/client/laser_scans/Writer.h
@@ -24,13 +24,14 @@
#include <mutex>
-#include <RobotAPI/interface/units/LaserScannerUnit.h>
+#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
+#include <RobotAPI/interface/units/LaserScannerUnit.h>
#include <RobotAPI/libraries/armem/client/Writer.h>
-#include <RobotAPI/libraries/armem/client.h>
+#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
-namespace armarx::armem
+namespace armarx::armem::vision::laser_scans::client
{
/**
@@ -44,11 +45,13 @@ namespace armarx::armem
*
* Detailed description of class ExampleClient.
*/
- class MappingDataWriter
+ class Writer
{
public:
- MappingDataWriter(armem::ClientWriterComponentPluginUser& component);
- virtual ~MappingDataWriter();
+
+ Writer(armem::client::MemoryNameSystem& memoryNameSystem);
+ virtual ~Writer();
+
void connect();
@@ -57,27 +60,29 @@ namespace armarx::armem
// void connect() override;
/// to be called in Component::addPropertyDefinitions
- void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def) /*override*/;
+ void registerPropertyDefinitions(
+ armarx::PropertyDefinitionsPtr& def) /*override*/;
- bool storeSensorData(const LaserScan& laserScan, const std::string& frame, const std::string& agentName, const std::int64_t& timestamp);
+ bool storeSensorData(const LaserScan& laserScan,
+ const std::string& frame,
+ const std::string& agentName,
+ const std::int64_t& timestamp);
private:
+ armem::client::MemoryNameSystem& memoryNameSystem;
armem::client::Writer memoryWriter;
// Properties
struct Properties
{
- std::string memoryName = "RobotState";
- std::string mappingMemoryName = "Mapping";
+ std::string memoryName = "Vision";
+ std::string coreSegmentName = "LaserScans";
} properties;
std::mutex memoryWriterMutex;
+ const std::string propertyPrefix = "mem.vision.";
- const std::string propertyPrefix = "mem.mapping.";
-
- armem::ClientWriterComponentPluginUser& component;
};
-
-} // namespace armarx::armem
+} // namespace armarx::armem::vision::laser_scans::client
diff --git a/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Reader.cpp b/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Reader.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..ba7edf547d1d45873609d723b92934b18d7f5c88
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Reader.cpp
@@ -0,0 +1,146 @@
+#include "Reader.h"
+
+// STD / STL
+#include <algorithm>
+#include <cstring>
+#include <map>
+#include <optional>
+#include <ostream>
+#include <utility>
+#include <vector>
+
+#include <type_traits>
+
+// ICE
+#include <IceUtil/Handle.h>
+#include <IceUtil/Time.h>
+
+// Simox
+#include <SimoxUtility/algorithm/get_map_keys_values.h>
+
+// ArmarXCore
+#include "ArmarXCore/core/exceptions/LocalException.h"
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/logging/LogSender.h>
+#include <ArmarXCore/core/logging/Logging.h>
+
+// RobotAPI Interfaces
+#include "RobotAPI/libraries/aron/converter/eigen/EigenConverter.h"
+#include <RobotAPI/interface/armem/mns/MemoryNameSystemInterface.h>
+#include <RobotAPI/interface/armem/server/ReadingMemoryInterface.h>
+#include <RobotAPI/interface/units/LaserScannerUnit.h>
+
+// RobotAPI Aron
+#include <RobotAPI/libraries/armem_vision/aron/OccupancyGrid.aron.generated.h>
+#include <RobotAPI/libraries/aron/core/Exception.h>
+#include <RobotAPI/libraries/aron/core/navigator/data/complex/NDArray.h>
+
+// RobotAPI Armem
+#include <RobotAPI/libraries/armem/client/Query.h>
+#include <RobotAPI/libraries/armem/client/Reader.h>
+#include <RobotAPI/libraries/armem/client/query/Builder.h>
+#include <RobotAPI/libraries/armem/client/query/selectors.h>
+#include <RobotAPI/libraries/armem/core/workingmemory/CoreSegment.h>
+#include <RobotAPI/libraries/armem/core/workingmemory/Entity.h>
+#include <RobotAPI/libraries/armem/core/workingmemory/EntityInstance.h>
+#include <RobotAPI/libraries/armem/core/workingmemory/Memory.h>
+#include <RobotAPI/libraries/armem/core/workingmemory/ProviderSegment.h>
+#include <RobotAPI/libraries/armem/util/util.h>
+#include <RobotAPI/libraries/armem_vision/aron/LaserScan.aron.generated.h>
+#include <RobotAPI/libraries/armem_vision/aron_conversions.h>
+#include <RobotAPI/libraries/armem_vision/types.h>
+
+namespace armarx::armem::vision::occupancy_grid::client
+{
+
+ armarx::armem::client::query::Builder Reader::buildQuery(const Query& query) const
+ {
+ armarx::armem::client::query::Builder qb;
+
+ // clang-format off
+ qb
+ .coreSegments().withName(properties().memoryName)
+ .providerSegments().withName(query.providerName)
+ .entities().all()
+ .snapshots().beforeOrAtTime(query.timestamp);
+ // clang-format on
+
+ return qb;
+ }
+
+ OccupancyGrid asOccupancyGrid(const std::map<std::string, wm::Entity>& entities)
+ {
+ ARMARX_CHECK(not entities.empty()) << "No entities";
+ ARMARX_CHECK(entities.size() == 1) << "There should be only one entity!";
+
+ const wm::Entity& entity = entities.begin()->second;
+ ARMARX_CHECK(not entity.empty()) << "No snapshots";
+
+ const auto& entitySnapshot = entity.getLatestSnapshot();
+ ARMARX_CHECK(not entitySnapshot.empty()) << "No entity snapshot instances";
+
+ const auto& entityInstance = entitySnapshot.instances().front();
+
+ const auto aronDto = tryCast<arondto::OccupancyGrid>(entityInstance);
+ ARMARX_CHECK(aronDto) << "Failed casting to OccupancyGrid";
+
+ OccupancyGrid occupancyGrid;
+ fromAron(*aronDto, occupancyGrid);
+
+ // direct access to grid data
+ const auto ndArrayNavigator = aron::datanavigator::NDArrayNavigator::DynamicCast(
+ entityInstance.data()->getElement("grid"));
+ ARMARX_CHECK_NOT_NULL(ndArrayNavigator);
+
+ occupancyGrid.grid = aron::converter::AronEigenConverter::ConvertToArray<float>(*ndArrayNavigator);
+
+ return occupancyGrid;
+ }
+
+ Reader::Result Reader::query(const Query& query) const
+ {
+ const auto qb = buildQuery(query);
+
+ ARMARX_IMPORTANT << "[MappingDataReader] query ... ";
+
+ const armem::client::QueryResult qResult =
+ memoryReader().query(qb.buildQueryInput());
+
+ ARMARX_DEBUG << "[MappingDataReader] result: " << qResult;
+
+ if (not qResult.success)
+ {
+ ARMARX_WARNING << "Failed to query data from memory: "
+ << qResult.errorMessage;
+ return {.occupancyGrid = std::nullopt,
+ .status = Result::Status::Error,
+ .errorMessage = qResult.errorMessage};
+ }
+
+ // now create result from memory
+ const auto& entities = qResult.memory.getCoreSegment(properties().memoryName)
+ .getProviderSegment(query.providerName)
+ .entities();
+
+ if (entities.empty())
+ {
+ ARMARX_WARNING << "No entities.";
+ return {.occupancyGrid = std::nullopt,
+ .status = Result::Status::NoData,
+ .errorMessage = "No entities"};
+ }
+
+ try
+ {
+ const auto occupancyGrid = asOccupancyGrid(entities);
+ return Result{.occupancyGrid = occupancyGrid,
+ .status = Result::Status::Success};
+ }
+ catch (...)
+ {
+ return Result{.status = Result::Status::Error,
+ .errorMessage = GetHandledExceptionString()};
+ }
+ }
+
+} // namespace armarx::armem::vision::occupancy_grid::client
diff --git a/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Reader.h b/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Reader.h
new file mode 100644
index 0000000000000000000000000000000000000000..a43c2c7c1151223358d5b1ab608f824fd3cdf66f
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Reader.h
@@ -0,0 +1,74 @@
+/*
+ * 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/>.
+ *
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <mutex>
+
+#include "RobotAPI/libraries/armem/client/util/SimpleReaderBase.h"
+#include "RobotAPI/libraries/armem/core/Time.h"
+#include "RobotAPI/libraries/armem_vision/types.h"
+#include <RobotAPI/libraries/armem/client/query/Builder.h>
+
+namespace armarx::armem::vision::occupancy_grid::client
+{
+
+ class Reader : virtual public armarx::armem::client::util::SimpleReaderBase
+ {
+ public:
+ using armarx::armem::client::util::SimpleReaderBase::SimpleReaderBase;
+ ~Reader() override;
+
+ struct Query
+ {
+ std::string providerName;
+ armem::Time timestamp;
+ };
+
+ struct Result
+ {
+ std::optional<OccupancyGrid> occupancyGrid = std::nullopt;
+
+ enum Status
+ {
+ Success,
+ NoData,
+ Error
+ } status;
+
+ std::string errorMessage = "";
+
+ operator bool() const noexcept
+ {
+ return status == Status::Success;
+ }
+ };
+
+ Result query(const Query& query) const;
+
+ ::armarx::armem::client::query::Builder buildQuery(const Query& query) const;
+
+ protected:
+ std::string propertyPrefix() const override;
+ Properties defaultProperties() const override;
+ };
+
+} // namespace armarx::armem::vision::occupancy_grid::client
diff --git a/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Writer.cpp b/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Writer.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..f2930f14f479d9ae68ee222f2643edf327392a43
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Writer.cpp
@@ -0,0 +1,70 @@
+#include "Writer.h"
+
+#include "RobotAPI/libraries/armem_vision/aron_conversions.h"
+#include <RobotAPI/libraries/armem_vision/aron/OccupancyGrid.aron.generated.h>
+
+namespace armarx::armem::vision::occupancy_grid::client
+{
+ Writer::~Writer() = default;
+
+ bool Writer::store(const OccupancyGrid& grid,
+ const std::string& frame,
+ const std::string& agentName,
+ const std::int64_t& timestamp)
+ {
+ std::lock_guard g{memoryWriterMutex()};
+
+ const auto result = memoryWriter().addSegment(properties().coreSegmentName, agentName);
+
+ if (not result.success)
+ {
+ ARMARX_ERROR << result.errorMessage;
+
+ // TODO(fabian.reister): message
+ return false;
+ }
+
+ const auto iceTimestamp = Time::microSeconds(timestamp);
+
+ const auto providerId = armem::MemoryID(result.segmentID);
+ const auto entityID = providerId.withEntityName(frame).withTimestamp(iceTimestamp);
+
+ armem::EntityUpdate update;
+ update.entityID = entityID;
+
+ arondto::OccupancyGrid aronGrid;
+ // currently only sets the header
+ toAron(aronGrid, grid);
+
+ auto dict = aronGrid.toAron();
+ dict->addElement("grid", toAron(grid.grid));
+
+ update.instancesData = {dict};
+ update.timeCreated = iceTimestamp;
+
+ ARMARX_DEBUG << "Committing " << update << " at time " << iceTimestamp;
+ armem::EntityUpdateResult updateResult = memoryWriter().commit(update);
+
+ ARMARX_DEBUG << updateResult;
+
+ if (not updateResult.success)
+ {
+ ARMARX_ERROR << updateResult.errorMessage;
+ }
+
+ return updateResult.success;
+ }
+
+ std::string Writer::propertyPrefix() const
+ {
+ return "mem.vision.occupancy_grid.";
+ }
+
+ armarx::armem::client::util::SimpleWriterBase::SimpleWriterBase::Properties
+ Writer::defaultProperties() const
+ {
+
+ return SimpleWriterBase::Properties{.memoryName = "Vision",
+ .coreSegmentName = "OccupancyGrid"};
+ }
+} // namespace armarx::armem::vision::occupancy_grid::client
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Writer.h b/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Writer.h
new file mode 100644
index 0000000000000000000000000000000000000000..bf1268444c333d10bb3c2f9efb68da11fe697cc8
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Writer.h
@@ -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::
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <mutex>
+
+#include "RobotAPI/libraries/armem/client/util/SimpleWriterBase.h"
+#include "RobotAPI/libraries/armem_vision/types.h"
+
+namespace armarx::armem::vision::occupancy_grid::client
+{
+
+ /**
+ * @defgroup Component-ExampleClient ExampleClient
+ * @ingroup RobotAPI-Components
+ * A description of the component ExampleClient.
+ *
+ * @class ExampleClient
+ * @ingroup Component-ExampleClient
+ * @brief Brief description of class ExampleClient.
+ *
+ * Detailed description of class ExampleClient.
+ */
+ class Writer : virtual public armarx::armem::client::util::SimpleWriterBase
+ {
+ public:
+ using armarx::armem::client::util::SimpleWriterBase::SimpleWriterBase;
+ ~Writer() override;
+
+ bool store(const OccupancyGrid& grid,
+ const std::string& frame,
+ const std::string& agentName,
+ const std::int64_t& timestamp);
+
+ protected:
+ std::string propertyPrefix() const override;
+ Properties defaultProperties() const override;
+ };
+
+
+
+} // namespace armarx::armem::vision::occupancy_grid::client
diff --git a/source/RobotAPI/libraries/armem_robot_mapping/types.h b/source/RobotAPI/libraries/armem_vision/types.h
similarity index 73%
rename from source/RobotAPI/libraries/armem_robot_mapping/types.h
rename to source/RobotAPI/libraries/armem_vision/types.h
index d822597e103177b0833014d25f0530e25f2b2075..dd975e9b1e6e76484c3077bebf9dc99c9f3a8d85 100644
--- a/source/RobotAPI/libraries/armem_robot_mapping/types.h
+++ b/source/RobotAPI/libraries/armem_vision/types.h
@@ -21,10 +21,12 @@
#pragma once
-#include <RobotAPI/libraries/armem/core/Time.h>
+#include <vector>
+
#include <RobotAPI/interface/units/LaserScannerUnit.h>
+#include <RobotAPI/libraries/armem/core/Time.h>
-namespace armarx
+namespace armarx::armem
{
struct SensorHeader
@@ -40,4 +42,20 @@ namespace armarx
LaserScan data;
};
-} // namespace armarx
+ // template<typename _ValueT = float>
+ struct OccupancyGrid
+ {
+ float resolution;
+
+ std::string frame;
+ Eigen::Affine3f pose;
+
+ // using ValueType = _ValueT;
+ using CellType = float;
+ using Grid = Eigen::Array<CellType, Eigen::Dynamic, Eigen::Dynamic>;
+
+ Grid grid;
+ };
+
+
+} // namespace armarx::armem
diff --git a/source/RobotAPI/libraries/aron/converter/eigen/EigenConverter.h b/source/RobotAPI/libraries/aron/converter/eigen/EigenConverter.h
index 787ae30a23f097821989fcd9658a3fa31d6b4761..4a5a02e56edc8fae399b2a216b4ee77a6f4536d8 100644
--- a/source/RobotAPI/libraries/aron/converter/eigen/EigenConverter.h
+++ b/source/RobotAPI/libraries/aron/converter/eigen/EigenConverter.h
@@ -28,6 +28,7 @@
// Eigen
#include <Eigen/Geometry>
#include <Eigen/Core>
+#include <Eigen/src/Core/util/Constants.h>
// ArmarX
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
@@ -103,9 +104,26 @@ namespace armarx::aron::converter
return ConvertToMatrix<T, Rows, Cols>(*nav);
}
- template<typename T, int Rows, int Cols>
+ template<typename T>
+ static Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> ConvertToDynamicMatrix(const datanavigator::NDArrayNavigator& nav)
+ {
+ const auto dims = nav.getDimensions();
+
+ using MatrixT = Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>;
+
+ MatrixT ret;
+ memcpy(reinterpret_cast<unsigned char*>(ret.data()), nav.getData(), std::accumulate(std::begin(dims), std::end(dims), 1, std::multiplies<>()));
+ return ret;
+ }
+
+ template<typename T, int Rows = Eigen::Dynamic, int Cols = Eigen::Dynamic>
static Eigen::Matrix<T, Rows, Cols> ConvertToMatrix(const datanavigator::NDArrayNavigator& nav)
{
+ if constexpr(Rows == Eigen::Dynamic and Cols == Eigen::Dynamic)
+ {
+ return ConvertToDynamicMatrix<T>(nav);
+ }
+
checkDimensions(nav, {Rows, Cols, sizeof(T)}, "ConvertToMatrix");
auto dims = nav.getDimensions();
@@ -114,6 +132,61 @@ namespace armarx::aron::converter
return ret;
}
+ template<typename T>
+ static datanavigator::NDArrayNavigatorPtr ConvertFromMatrix(const Eigen::Matrix < T, Eigen::Dynamic, Eigen::Dynamic >& mat)
+ {
+ datanavigator::NDArrayNavigatorPtr ndArr(new datanavigator::NDArrayNavigator);
+
+ ndArr->setDimensions({static_cast<int>(mat.rows()), static_cast<int>(mat.cols())});
+ ndArr->setData(sizeof(T) * mat.size(), reinterpret_cast <const unsigned char* >(mat.data()));
+
+ return ndArr;
+ }
+
+
+ // Eigen::Array
+
+ template<typename T>
+ static Eigen::Array<T, Eigen::Dynamic, Eigen::Dynamic> ConvertToDynamicArray(const datanavigator::NDArrayNavigator& nav)
+ {
+ const auto dims = nav.getDimensions();
+
+ using ArrayT = Eigen::Array<T, Eigen::Dynamic, Eigen::Dynamic>;
+
+ ArrayT ret;
+ memcpy(reinterpret_cast<unsigned char*>(ret.data()), nav.getData(), std::accumulate(std::begin(dims), std::end(dims), 1, std::multiplies<>()));
+ return ret;
+ }
+
+ template<typename T, int Rows = Eigen::Dynamic, int Cols = Eigen::Dynamic>
+ static Eigen::Matrix<T, Rows, Cols> ConvertToArray(const datanavigator::NDArrayNavigator& nav)
+ {
+ if constexpr(Rows == Eigen::Dynamic and Cols == Eigen::Dynamic)
+ {
+ return ConvertToDynamicArray<T>(nav);
+ }
+
+ checkDimensions(nav, {Rows, Cols, sizeof(T)}, "ConvertToMatrix");
+ auto dims = nav.getDimensions();
+
+ Eigen::Array<T, Rows, Cols> ret;
+ memcpy(reinterpret_cast<unsigned char*>(ret.data()), nav.getData(), std::accumulate(std::begin(dims), std::end(dims), 1, std::multiplies<int>()));
+ return ret;
+ }
+
+ template<typename T>
+ static datanavigator::NDArrayNavigatorPtr ConvertFromArray(const Eigen::Array < T, Eigen::Dynamic, Eigen::Dynamic >& mat)
+ {
+ datanavigator::NDArrayNavigatorPtr ndArr(new datanavigator::NDArrayNavigator);
+
+ ndArr->setDimensions({static_cast<int>(mat.rows()), static_cast<int>(mat.cols())});
+ ndArr->setData(sizeof(T) * mat.size(), reinterpret_cast <const unsigned char* >(mat.data()));
+
+ return ndArr;
+ }
+
+
+
private:
/**
diff --git a/source/RobotAPI/libraries/aron/core/CMakeLists.txt b/source/RobotAPI/libraries/aron/core/CMakeLists.txt
index c136ba4ac8ebdf2b9195252b75c00844e9b56e49..af2caf05737aec188ccae47c5554f7776a27defd 100644
--- a/source/RobotAPI/libraries/aron/core/CMakeLists.txt
+++ b/source/RobotAPI/libraries/aron/core/CMakeLists.txt
@@ -3,26 +3,15 @@ set(LIB_NAME aron)
armarx_component_set_name("${LIB_NAME}")
armarx_set_target("Library: ${LIB_NAME}")
-find_package(Eigen3 QUIET)
-armarx_build_if(Eigen3_FOUND "Eigen3 not available")
-
find_package(Simox QUIET)
armarx_build_if(Simox_FOUND "Simox not available")
-find_package(PCL QUIET COMPONENTS io common)
-armarx_build_if(PCL_FOUND "PCL not available")
-
-
set(LIBS
ArmarXCoreInterfaces
ArmarXCore
RobotAPIInterfaces
cppgen
Simox::SimoxUtility
-
- # System libraries
- Eigen3::Eigen
- # PCLInterface
)
set(LIB_FILES
@@ -30,18 +19,30 @@ set(LIB_FILES
Path.cpp
navigator/data/Navigator.cpp
+ navigator/data/detail/NavigatorBase.cpp
+ navigator/data/detail/PrimitiveNavigatorBase.cpp
navigator/data/container/List.cpp
navigator/data/container/Dict.cpp
navigator/data/complex/NDArray.cpp
- navigator/data/primitive/Primitive.cpp
+ navigator/data/primitive/Int.cpp
+ navigator/data/primitive/Long.cpp
+ navigator/data/primitive/Float.cpp
+ navigator/data/primitive/Double.cpp
+ navigator/data/primitive/String.cpp
+ navigator/data/primitive/Bool.cpp
navigator/data/NavigatorFactory.cpp
navigator/type/Navigator.cpp
+ navigator/type/detail/NavigatorBase.cpp
+ navigator/type/detail/ContainerNavigatorBase.cpp
+ navigator/type/detail/NDArrayNavigatorBase.cpp
+ navigator/type/detail/PrimitiveNavigatorBase.cpp
navigator/type/container/Object.cpp
navigator/type/container/List.cpp
navigator/type/container/Dict.cpp
navigator/type/container/Tuple.cpp
navigator/type/container/Pair.cpp
+ navigator/type/ndarray/NDArray.cpp
navigator/type/ndarray/EigenMatrix.cpp
navigator/type/ndarray/EigenQuaternion.cpp
navigator/type/ndarray/IVTCByteImage.cpp
@@ -51,24 +52,34 @@ set(LIB_FILES
navigator/type/ndarray/Pose.cpp
navigator/type/ndarray/Orientation.cpp
navigator/type/enum/IntEnum.cpp
- navigator/type/primitive/Primitive.cpp
+ navigator/type/primitive/Int.cpp
+ navigator/type/primitive/Long.cpp
+ navigator/type/primitive/Float.cpp
+ navigator/type/primitive/Double.cpp
+ navigator/type/primitive/String.cpp
+ navigator/type/primitive/Bool.cpp
+ navigator/type/primitive/Time.cpp
navigator/type/NavigatorFactory.cpp
navigator/visitors/DataVisitor.cpp
navigator/visitors/TypedDataVisitor.cpp
+ io/dataIO/Writer.cpp
io/dataIO/writer/navigator/NavigatorWriter.cpp
io/dataIO/writer/nlohmannJSON/NlohmannJSONWriter.cpp
+ io/dataIO/Reader.cpp
io/dataIO/reader/navigator/NavigatorReader.cpp
io/dataIO/reader/nlohmannJSON/NlohmannJSONReader.cpp
io/dataIO/visitor/Visitor.cpp
io/dataIO/converter/Converter.cpp
+ io/typeIO/Writer.cpp
io/typeIO/writer/navigator/NavigatorWriter.cpp
io/typeIO/writer/nlohmannJSON/NlohmannJSONWriter.cpp
+ io/typeIO/Reader.cpp
io/typeIO/reader/navigator/NavigatorReader.cpp
io/typeIO/reader/nlohmannJSON/NlohmannJSONReader.cpp
@@ -77,6 +88,10 @@ set(LIB_FILES
codegenerator/codeWriter/cpp/Writer.cpp
codegenerator/codeWriter/cpp/serializer/Serializer.cpp
+ codegenerator/codeWriter/cpp/serializer/detail/SerializerBase.cpp
+ codegenerator/codeWriter/cpp/serializer/detail/ContainerSerializerBase.cpp
+ codegenerator/codeWriter/cpp/serializer/detail/NDArraySerializerBase.cpp
+ codegenerator/codeWriter/cpp/serializer/detail/PrimitiveSerializerBase.cpp
codegenerator/codeWriter/cpp/serializer/toplevel/ObjectClass.cpp
codegenerator/codeWriter/cpp/serializer/toplevel/IntEnumClass.cpp
codegenerator/codeWriter/cpp/serializer/container/Dict.cpp
@@ -84,6 +99,7 @@ set(LIB_FILES
codegenerator/codeWriter/cpp/serializer/container/Object.cpp
codegenerator/codeWriter/cpp/serializer/container/Tuple.cpp
codegenerator/codeWriter/cpp/serializer/container/Pair.cpp
+ codegenerator/codeWriter/cpp/serializer/ndarray/NDArray.cpp
codegenerator/codeWriter/cpp/serializer/ndarray/EigenMatrix.cpp
codegenerator/codeWriter/cpp/serializer/ndarray/EigenQuaternion.cpp
codegenerator/codeWriter/cpp/serializer/ndarray/IVTCByteImage.cpp
@@ -93,7 +109,13 @@ set(LIB_FILES
codegenerator/codeWriter/cpp/serializer/ndarray/Orientation.cpp
codegenerator/codeWriter/cpp/serializer/ndarray/Pose.cpp
codegenerator/codeWriter/cpp/serializer/enum/IntEnum.cpp
- codegenerator/codeWriter/cpp/serializer/primitive/Primitive.cpp
+ codegenerator/codeWriter/cpp/serializer/primitive/Int.cpp
+ codegenerator/codeWriter/cpp/serializer/primitive/Long.cpp
+ codegenerator/codeWriter/cpp/serializer/primitive/Float.cpp
+ codegenerator/codeWriter/cpp/serializer/primitive/Double.cpp
+ codegenerator/codeWriter/cpp/serializer/primitive/String.cpp
+ codegenerator/codeWriter/cpp/serializer/primitive/Bool.cpp
+ codegenerator/codeWriter/cpp/serializer/primitive/Time.cpp
codegenerator/codeWriter/cpp/serializer/SerializerFactory.cpp
codegenerator/typeReader/xml/Data.cpp
@@ -116,19 +138,31 @@ set(LIB_HEADERS
navigator/NavigatorFactory.h
navigator/data/Navigator.h
+ navigator/data/detail/NavigatorBase.h
+ navigator/data/detail/PrimitiveNavigatorBase.h
navigator/data/container/List.h
navigator/data/container/Dict.h
navigator/data/complex/NDArray.h
- navigator/data/primitive/Primitive.h
+ navigator/data/primitive/Int.h
+ navigator/data/primitive/Long.h
+ navigator/data/primitive/Float.h
+ navigator/data/primitive/Double.h
+ navigator/data/primitive/String.h
+ navigator/data/primitive/Bool.h
navigator/data/AllNavigators.h
navigator/data/NavigatorFactory.h
navigator/type/Navigator.h
+ navigator/type/detail/NavigatorBase.h
+ navigator/type/detail/ContainerNavigatorBase.h
+ navigator/type/detail/NDArrayNavigatorBase.h
+ navigator/type/detail/PrimitiveNavigatorBase.h
navigator/type/container/Object.h
navigator/type/container/List.h
navigator/type/container/Dict.h
navigator/type/container/Tuple.h
navigator/type/container/Pair.h
+ navigator/type/ndarray/NDArray.h
navigator/type/ndarray/EigenMatrix.h
navigator/type/ndarray/EigenQuaternion.h
navigator/type/ndarray/IVTCByteImage.h
@@ -138,7 +172,13 @@ set(LIB_HEADERS
navigator/type/ndarray/Pose.h
navigator/type/ndarray/Orientation.h
navigator/type/enum/IntEnum.h
- navigator/type/primitive/Primitive.h
+ navigator/type/primitive/Int.h
+ navigator/type/primitive/Long.h
+ navigator/type/primitive/Float.h
+ navigator/type/primitive/Double.h
+ navigator/type/primitive/String.h
+ navigator/type/primitive/Bool.h
+ navigator/type/primitive/Time.h
navigator/type/AllNavigators.h
navigator/type/NavigatorFactory.h
@@ -192,6 +232,10 @@ set(LIB_HEADERS
codegenerator/codeWriter/cpp/AronCppClass.h
codegenerator/codeWriter/cpp/Writer.h
codegenerator/codeWriter/cpp/serializer/Serializer.h
+ codegenerator/codeWriter/cpp/serializer/detail/SerializerBase.h
+ codegenerator/codeWriter/cpp/serializer/detail/ContainerSerializerBase.h
+ codegenerator/codeWriter/cpp/serializer/detail/NDArraySerializerBase.h
+ codegenerator/codeWriter/cpp/serializer/detail/PrimitiveSerializerBase.h
codegenerator/codeWriter/cpp/serializer/toplevel/ObjectClass.h
codegenerator/codeWriter/cpp/serializer/toplevel/IntEnumClass.h
codegenerator/codeWriter/cpp/serializer/container/Dict.h
@@ -199,6 +243,7 @@ set(LIB_HEADERS
codegenerator/codeWriter/cpp/serializer/container/Object.h
codegenerator/codeWriter/cpp/serializer/container/Tuple.h
codegenerator/codeWriter/cpp/serializer/container/Pair.h
+ codegenerator/codeWriter/cpp/serializer/ndarray/NDArray.h
codegenerator/codeWriter/cpp/serializer/ndarray/EigenMatrix.h
codegenerator/codeWriter/cpp/serializer/ndarray/EigenQuaternion.h
codegenerator/codeWriter/cpp/serializer/ndarray/IVTCByteImage.h
@@ -208,7 +253,13 @@ set(LIB_HEADERS
codegenerator/codeWriter/cpp/serializer/ndarray/Orientation.h
codegenerator/codeWriter/cpp/serializer/ndarray/Pose.h
codegenerator/codeWriter/cpp/serializer/enum/IntEnum.h
- codegenerator/codeWriter/cpp/serializer/primitive/Primitive.h
+ codegenerator/codeWriter/cpp/serializer/primitive/Int.h
+ codegenerator/codeWriter/cpp/serializer/primitive/Long.h
+ codegenerator/codeWriter/cpp/serializer/primitive/Float.h
+ codegenerator/codeWriter/cpp/serializer/primitive/Double.h
+ codegenerator/codeWriter/cpp/serializer/primitive/String.h
+ codegenerator/codeWriter/cpp/serializer/primitive/Bool.h
+ codegenerator/codeWriter/cpp/serializer/primitive/Time.h
codegenerator/codeWriter/cpp/serializer/AllSerializers.h
codegenerator/codeWriter/cpp/serializer/SerializerFactory.h
@@ -219,16 +270,18 @@ set(LIB_HEADERS
)
-armarx_add_library("${LIB_NAME}" "${LIB_FILES}" "${LIB_HEADERS}" "${LIBS}")
-
-
-if(PCL_FOUND)
-target_include_directories("${LIB_NAME}"
- SYSTEM PUBLIC
- "${PCL_INCLUDE_DIRS}"
+armarx_add_library(
+ LIB_NAME
+ "${LIB_NAME}"
+ SOURCES
+ "${LIB_FILES}"
+ HEADERS
+ "${LIB_HEADERS}"
+ LIBS
+ "${LIBS}"
)
-endif()
+add_library(RobotAPI::aron ALIAS "${LIB_NAME}")
# add unit tests
add_subdirectory(test)
diff --git a/source/RobotAPI/libraries/aron/core/Config.h b/source/RobotAPI/libraries/aron/core/Config.h
index fc05237a22dab5dc57cb4f44b6d4b9616cc379d2..7db99c2f1aaa298eb50f076a371677408dac113e 100644
--- a/source/RobotAPI/libraries/aron/core/Config.h
+++ b/source/RobotAPI/libraries/aron/core/Config.h
@@ -86,6 +86,7 @@ namespace armarx
// All complex types go here
#define HANDLE_NDARRAY_TYPES \
+ RUN_ARON_MACRO(NDArray, ndarray, NDARRAY) \
RUN_ARON_MACRO(EigenMatrix, eigenmatrix, EIGEN_MATRIX) \
RUN_ARON_MACRO(EigenQuaternion, eigenquaternion, EIGEN_QUATERNION) \
RUN_ARON_MACRO(IVTCByteImage, ivtcbyteimage, IVT_CBYTE_IMAGE) \
@@ -164,6 +165,7 @@ namespace armarx
RUN_ARON_MACRO(Dict, dict, DICT, Dict, dict, DICT)
#define HANDLE_COMPLEX_CORRESPONDING \
+ RUN_ARON_MACRO(NDArray, ndarray, NDARRAY, NDArray, ndarray, NDARRAY) \
RUN_ARON_MACRO(EigenMatrix, eigenmatrix, EIGEN_MATRIX, NDArray, ndarray, NDARRAY) \
RUN_ARON_MACRO(EigenQuaternion, eigenmatrix, EIGEN_MATRIX, NDArray, ndarray, NDARRAY) \
RUN_ARON_MACRO(IVTCByteImage, ivtcbyteimage, IVT_CBYTE_IMAGE, NDArray, ndarray, NDARRAY) \
diff --git a/source/RobotAPI/libraries/aron/core/Debug.h b/source/RobotAPI/libraries/aron/core/Debug.h
index 28b3958646b984914a6092d272d5b39d3a7dd2ba..672b47deeb901506af09f0b2cd49a998ede7f95c 100644
--- a/source/RobotAPI/libraries/aron/core/Debug.h
+++ b/source/RobotAPI/libraries/aron/core/Debug.h
@@ -46,6 +46,11 @@ namespace armarx::aron
static std::string AronDataPtrToString(const data::AronDataPtr& data)
{
+ if (!data)
+ {
+ return "";
+ }
+
dataIO::writer::NlohmannJSONWriter w;
dataIO::Visitor::VisitAndSetup(w, data);
return w.getResult().dump(2);
@@ -53,10 +58,11 @@ namespace armarx::aron
static std::string NavigatorPtrToString(const datanavigator::NavigatorPtr& data)
{
- if (data == nullptr)
+ if (!data)
{
return "";
}
+
dataIO::writer::NlohmannJSONWriter w;
dataIO::Visitor::VisitAndSetup(w, data);
return w.getResult().dump(2);
@@ -71,10 +77,6 @@ namespace armarx::aron
static std::string NavigatorPtrToString(const typenavigator::NavigatorPtr& data)
{
- if (data == nullptr)
- {
- return "";
- }
typeIO::writer::NlohmannJSONWriter w;
typeIO::Visitor::VisitAndSetup(w, data);
return w.getResult().dump(2);
diff --git a/source/RobotAPI/libraries/aron/core/Descriptor.h b/source/RobotAPI/libraries/aron/core/Descriptor.h
index 26e9e24ba442a6d04f7590181e8736ace884ab29..bcf2d0c3173dfc12493d0e5dac0155da81217c57 100644
--- a/source/RobotAPI/libraries/aron/core/Descriptor.h
+++ b/source/RobotAPI/libraries/aron/core/Descriptor.h
@@ -31,10 +31,20 @@
#include <string>
// ArmarX
+#include <RobotAPI/interface/aron.h>
#include <RobotAPI/libraries/aron/core/Config.h>
namespace armarx::aron::type
{
+ const std::vector<type::Maybe> ALL_MAYBE_TYPES =
+ {
+ type::Maybe::eNone,
+ type::Maybe::eOptional,
+ type::Maybe::eRawPointer,
+ type::Maybe::eSharedPointer,
+ type::Maybe::eUniquePointer
+ };
+
enum class Descriptor
{
#define RUN_ARON_MACRO(upperType, lowerType, capsType) \
@@ -45,7 +55,17 @@ namespace armarx::aron::type
eUnknown = -1
};
- const std::map<type::Descriptor, std::string> _descriptorstring =
+ const std::vector<type::Descriptor> ALL_ARON_TYPES =
+ {
+#define RUN_ARON_MACRO(upperType, lowerType, capsType) \
+ type::Descriptor::e##upperType,
+
+ HANDLE_ALL_ARON_TYPES
+#undef RUN_ARON_MACRO
+ Descriptor::eUnknown
+ };
+
+ const std::map<type::Descriptor, std::string> _descriptor2string =
{
#define RUN_ARON_MACRO(upperType, lowerType, capsType) \
{ Descriptor::e##upperType, "armarx::aron::type::Descriptor::e" + std::string(#upperType) },
@@ -57,7 +77,47 @@ namespace armarx::aron::type
inline std::string DESCRIPTOR_TO_STRING(const type::Descriptor d)
{
- return type::_descriptorstring.at(d);
+ return type::_descriptor2string.at(d);
+ }
+
+ const std::map<type::Maybe, std::string> _maybe2string =
+ {
+ {type::Maybe::eNone, "armarx::aron::type::Maybe::eNone"},
+ {type::Maybe::eOptional, "armarx::aron::type::Maybe::eOptional"},
+ {type::Maybe::eRawPointer, "armarx::aron::type::Maybe::eRawPointer"},
+ {type::Maybe::eSharedPointer, "armarx::aron::type::Maybe::eSharedPointer"},
+ {type::Maybe::eUniquePointer, "armarx::aron::type::Maybe::eUniquePointer"}
+ };
+
+ inline std::string MAYBE_TO_STRING(const type::Maybe d)
+ {
+ return type::_maybe2string.at(d);
+ }
+
+ const std::map<std::string, type::Descriptor> _string2descriptor =
+ {
+#define RUN_ARON_MACRO(upperType, lowerType, capsType) \
+ { "armarx::aron::type::Descriptor::e" + std::string(#upperType), Descriptor::e##upperType }, \
+ { std::string(#upperType), Descriptor::e##upperType }, \
+ { std::string(#lowerType), Descriptor::e##upperType }, \
+ { std::string(#capsType), Descriptor::e##upperType },
+
+ HANDLE_ALL_ARON_TYPES
+#undef RUN_ARON_MACRO
+ {"armarx::aron::type::Descriptor::eUnknown", Descriptor::eUnknown},
+ {"Unknown", Descriptor::eUnknown},
+ {"unknown", Descriptor::eUnknown},
+ {"UNKNOWN", Descriptor::eUnknown}
+ };
+
+ inline type::Descriptor STRING_TO_DESCRIPTOR(const std::string& s)
+ {
+ return type::_string2descriptor.at(s);
+ }
+
+ inline type::Descriptor DESCRIPTOR_FROM_STRING(const std::string& s)
+ {
+ return STRING_TO_DESCRIPTOR(s);
}
}
@@ -73,7 +133,17 @@ namespace armarx::aron::data
eUnknown = -1
};
- const std::map<data::Descriptor, std::string> _descriptorstring =
+ const std::vector<data::Descriptor> ALL_ARON_DATA =
+ {
+#define RUN_ARON_MACRO(upperType, lowerType, capsType) \
+ data::Descriptor::e##upperType,
+
+ HANDLE_ALL_ARON_DATA
+#undef RUN_ARON_MACRO
+ Descriptor::eUnknown
+ };
+
+ const std::map<data::Descriptor, std::string> _descriptor2string =
{
#define RUN_ARON_MACRO(upperType, lowerType, capsType) \
{ Descriptor::e##upperType, "armarx::aron::data::Descriptor::e" + std::string(#upperType) },
@@ -85,6 +155,32 @@ namespace armarx::aron::data
inline std::string DESCRIPTOR_TO_STRING(const data::Descriptor d)
{
- return data::_descriptorstring.at(d);
+ return data::_descriptor2string.at(d);
+ }
+
+ const std::map<std::string, data::Descriptor> _string2descriptor =
+ {
+#define RUN_ARON_MACRO(upperType, lowerType, capsType) \
+ { "armarx::aron::data::Descriptor::e" + std::string(#upperType), Descriptor::e##upperType }, \
+ { std::string(#upperType), Descriptor::e##upperType }, \
+ { std::string(#lowerType), Descriptor::e##upperType }, \
+ { std::string(#capsType), Descriptor::e##upperType },
+
+ HANDLE_ALL_ARON_DATA
+#undef RUN_ARON_MACRO
+ {"armarx::aron::data::Descriptor::eUnknown", Descriptor::eUnknown},
+ {"Unknown", Descriptor::eUnknown},
+ {"unknown", Descriptor::eUnknown},
+ {"UNKNOWN", Descriptor::eUnknown}
+ };
+
+ inline data::Descriptor STRING_TO_DESCRIPTOR(const std::string& s)
+ {
+ return data::_string2descriptor.at(s);
+ }
+
+ inline data::Descriptor DESCRIPTOR_FROM_STRING(const std::string& s)
+ {
+ return STRING_TO_DESCRIPTOR(s);
}
}
diff --git a/source/RobotAPI/libraries/aron/core/Exception.h b/source/RobotAPI/libraries/aron/core/Exception.h
index 13044c9f973b43478e9e753ddc9554f1e8509139..025328edaa8360ed6a31864b91d4dcb030c8ff60 100644
--- a/source/RobotAPI/libraries/aron/core/Exception.h
+++ b/source/RobotAPI/libraries/aron/core/Exception.h
@@ -27,6 +27,7 @@
// ArmarX
#include <RobotAPI/interface/aron.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/exceptions/Exception.h>
#include <RobotAPI/libraries/aron/core/Path.h>
#include <RobotAPI/libraries/aron/core/Descriptor.h>
@@ -123,6 +124,24 @@ namespace armarx::aron::error
}
};
+ class MaybeNotValidException :
+ public AronException
+ {
+ public:
+ MaybeNotValidException() = delete;
+ MaybeNotValidException(const std::string& caller, const std::string& method, const std::string& reason, const type::Maybe maybe) :
+ AronException(caller, method, reason + ". The name of the maybe-type was: " + type::MAYBE_TO_STRING(maybe))
+ {
+
+ }
+
+ MaybeNotValidException(const std::string& caller, const std::string& method, const std::string& reason, const type::Maybe maybe, const Path& path) :
+ AronException(caller, method, reason + ". The name of the maybe-type was: " + type::MAYBE_TO_STRING(maybe), path)
+ {
+
+ }
+ };
+
class StringNotValidException :
public AronException
{
@@ -139,6 +158,18 @@ namespace armarx::aron::error
{
}
+
+ StringNotValidException(const std::string& caller, const std::string& method, const std::string& reason, const std::string& input, const std::string& expectation) :
+ AronException(caller, method, reason + ". Got: " + input + ". Expected: " + expectation)
+ {
+
+ }
+
+ StringNotValidException(const std::string& caller, const std::string& method, const std::string& reason, const std::string& input, const std::string& expectation, const Path& path) :
+ AronException(caller, method, reason + ". Got: " + input + ". Expected: " + expectation, path)
+ {
+
+ }
};
class IndexNotValidException :
diff --git a/source/RobotAPI/libraries/aron/core/Randomizer.h b/source/RobotAPI/libraries/aron/core/Randomizer.h
index 315dbed6511f2933fda4dff223a87c6f473b0592..bdc05099f108f0abaecb8db68cc58f47b8688a6c 100644
--- a/source/RobotAPI/libraries/aron/core/Randomizer.h
+++ b/source/RobotAPI/libraries/aron/core/Randomizer.h
@@ -39,38 +39,33 @@ namespace armarx::aron
public:
Randomizer()
{
- initialize_random();
+ initializeSelf();
};
- typenavigator::NavigatorPtr generateRandomType(bool mustBeObject = false) const
+ typenavigator::NavigatorPtr generateRandomType(bool allowMaybeType = false, bool firstObject = false) const
{
type::Descriptor nextType = type::Descriptor::eObject;
- if (!mustBeObject)
+ if (!firstObject)
{
- std::vector<type::Descriptor> descriptors =
- {
-#define RUN_ARON_MACRO(upperType, lowerType, capsType) \
- type::Descriptor::e##upperType,
-
- HANDLE_ALL_ARON_TYPES
-#undef RUN_ARON_MACRO
- };
+ nextType = getRandomElement(type::ALL_ARON_TYPES);
+ }
- int randomType = generateRandom(descriptors.size(), 0);
- nextType = descriptors[randomType];
+ type::Maybe nextMaybeType = type::Maybe::eNone;
+ if (allowMaybeType && !firstObject)
+ {
+ nextMaybeType = getRandomElement(type::ALL_MAYBE_TYPES);
}
switch (nextType)
{
case type::Descriptor::eObject:
{
- typenavigator::ObjectNavigatorPtr t = typenavigator::ObjectNavigatorPtr(new typenavigator::ObjectNavigator(Path()));
+ auto t = std::make_shared<typenavigator::ObjectNavigator>();
std::string objectName = generateRandomWord();
t->setObjectName(objectName);
- std::vector<std::string> usedNames({objectName});
int members = generateRandom(4, 2);
- std::set<std::string> usedKeys;
+ std::set<std::string> usedKeys = {objectName};
for (int i = 0; i < members; ++i)
{
std::string key = generateRandomWord(usedKeys);
@@ -81,9 +76,25 @@ namespace armarx::aron
}
return t;
}
+ case type::Descriptor::eIntEnum:
+ {
+ auto t = std::make_shared<typenavigator::IntEnumNavigator>();
+ std::string intEnumName = generateRandomWord();
+ t->setEnumName(intEnumName);
+
+ std::set<std::string> usedKeys = {intEnumName};
+ for (int i = 0; i < generateRandom(5, 2); ++i)
+ {
+ std::string key = generateRandomWord(usedKeys);
+ usedKeys.insert(key);
+
+ t->addAcceptedValue(key, i);
+ }
+ return t;
+ }
case type::Descriptor::eDict:
{
- typenavigator::DictNavigatorPtr t = typenavigator::DictNavigatorPtr(new typenavigator::DictNavigator());
+ auto t = std::make_shared<typenavigator::DictNavigator>();
typenavigator::NavigatorPtr a = generateRandomType(false);
t->setAcceptedType(a);
@@ -91,7 +102,7 @@ namespace armarx::aron
}
case type::Descriptor::eTuple:
{
- typenavigator::TupleNavigatorPtr t = typenavigator::TupleNavigatorPtr(new typenavigator::TupleNavigator());
+ auto t = std::make_shared<typenavigator::TupleNavigator>();
int members = generateRandom(4, 2);
for (int i = 0; i < members; ++i)
@@ -103,7 +114,7 @@ namespace armarx::aron
}
case type::Descriptor::eList:
{
- typenavigator::ListNavigatorPtr t = typenavigator::ListNavigatorPtr(new typenavigator::ListNavigator());
+ auto t = std::make_shared<typenavigator::ListNavigator>();
typenavigator::NavigatorPtr a = generateRandomType(false);
t->setAcceptedType(a);
@@ -111,7 +122,7 @@ namespace armarx::aron
}
case type::Descriptor::ePair:
{
- typenavigator::PairNavigatorPtr t = typenavigator::PairNavigatorPtr(new typenavigator::PairNavigator());
+ auto t = std::make_shared<typenavigator::PairNavigator>();
typenavigator::NavigatorPtr a = generateRandomType(false);
typenavigator::NavigatorPtr b = generateRandomType(false);
@@ -119,15 +130,61 @@ namespace armarx::aron
t->setSecondAcceptedType(b);
return t;
}
+ case type::Descriptor::eNDArray:
+ {
+ // TODO (fabian.peller)
+ // fall through EigenMatrix
+ }
case type::Descriptor::eEigenMatrix:
+ {
+ auto t = std::make_shared<typenavigator::EigenMatrixNavigator>();
+ auto type = getRandomKey(t->ACCEPTED_TYPES);
+ t->setTypename(type);
+ t->setRows(generateRandom(10, 1));
+ t->setRows(generateRandom(10, 1));
+ return t;
+ }
case type::Descriptor::eEigenQuaternion:
+ {
+ auto t = std::make_shared<typenavigator::EigenQuaternionNavigator>();
+ auto type = getRandomKey(t->ACCEPTED_TYPES);
+ t->setTypename(type);
+ return t;
+ }
case type::Descriptor::eIVTCByteImage:
+ {
+ auto t = std::make_shared<typenavigator::IVTCByteImageNavigator>();
+ auto type = getRandomKey(t->ACCEPTED_TYPES);
+ return t;
+ }
case type::Descriptor::eOpenCVMat:
+ {
+ auto t = std::make_shared<typenavigator::OpenCVMatNavigator>();
+ auto type = getRandomKey(t->ACCEPTED_TYPES);
+ return t;
+ }
case type::Descriptor::ePCLPointCloud:
+ {
+ auto t = std::make_shared<typenavigator::PCLPointCloudNavigator>();
+ auto type = getRandomKey(t->ACCEPTED_TYPES);
+ t->setTypename(type);
+ return t;
+ }
case type::Descriptor::ePosition:
+ {
+ auto t = std::make_shared<typenavigator::PositionNavigator>();
+ return t;
+ }
case type::Descriptor::eOrientation:
+ {
+ auto t = std::make_shared<typenavigator::PositionNavigator>();
+ return t;
+ }
case type::Descriptor::ePose:
-
+ {
+ auto t = std::make_shared<typenavigator::PositionNavigator>();
+ return t;
+ }
case type::Descriptor::eInt:
{
auto t = std::make_shared<typenavigator::IntNavigator>();
@@ -163,15 +220,23 @@ namespace armarx::aron
auto t = std::make_shared<typenavigator::TimeNavigator>();
return t;
}
- default:
+ case type::Descriptor::eUnknown:
{
throw error::DescriptorNotValidException("Randomizer", "generateRandomType", "No valid type found!", nextType);
}
}
}
- datanavigator::NavigatorPtr generateAronDataFromType(const typenavigator::NavigatorPtr& type) const
+ datanavigator::NavigatorPtr generateEmptyAronDataFromType(const typenavigator::NavigatorPtr& type, bool ignore_maybe = false) const
{
+ if (type->getMaybe() != type::Maybe::eNone && !ignore_maybe)
+ {
+ if (fiftyPercentChance())
+ {
+ return nullptr;
+ }
+ }
+
const type::Descriptor desc = type->getDescriptor();
switch (desc)
{
@@ -179,25 +244,32 @@ namespace armarx::aron
case type::Descriptor::eObject:
{
typenavigator::ObjectNavigatorPtr t = typenavigator::ObjectNavigator::DynamicCastAndCheck(type);
- datanavigator::DictNavigatorPtr d = datanavigator::DictNavigatorPtr(new datanavigator::DictNavigator());
+ auto d = std::make_shared<datanavigator::DictNavigator>();
for (const auto& [k, tt] : t->getMemberTypes())
{
- d->addElement(k, generateAronDataFromType(tt));
+ d->addElement(k, generateEmptyAronDataFromType(tt));
}
return d;
}
+ case type::Descriptor::eIntEnum:
+ {
+ auto t = typenavigator::IntEnumNavigator::DynamicCastAndCheck(type);
+ auto d = std::make_shared<datanavigator::IntNavigator>();
+ return d;
+ }
+
// here all totally random
case type::Descriptor::eDict:
{
- typenavigator::DictNavigatorPtr t = typenavigator::DictNavigator::DynamicCastAndCheck(type);
+ auto t = typenavigator::DictNavigator::DynamicCastAndCheck(type);
return datanavigator::NavigatorPtr(new datanavigator::DictNavigator());
}
#define RUN_ARON_MACRO(upperType, lowerType, capsType) \
case type::Descriptor::e##upperType: \
{ \
- typenavigator::upperType##NavigatorPtr t = typenavigator::upperType##Navigator::DynamicCastAndCheck(type); \
+ auto t = typenavigator::upperType##Navigator::DynamicCastAndCheck(type); \
return datanavigator::NavigatorPtr(new datanavigator::ListNavigator()); \
}
@@ -207,9 +279,8 @@ case type::Descriptor::e##upperType: \
#define RUN_ARON_MACRO(upperType, lowerType, capsType) \
case type::Descriptor::e##upperType: \
{ \
- typenavigator::upperType##NavigatorPtr t = typenavigator::upperType##Navigator::DynamicCastAndCheck(type); \
- datanavigator::NDArrayNavigatorPtr ndarray = datanavigator::NDArrayNavigatorPtr(new datanavigator::NDArrayNavigator()); \
- return ndarray; \
+ auto t = typenavigator::upperType##Navigator::DynamicCastAndCheck(type); \
+ return datanavigator::NDArrayNavigatorPtr(new datanavigator::NDArrayNavigator()); \
}
HANDLE_NDARRAY_TYPES
@@ -218,21 +289,33 @@ case type::Descriptor::e##upperType: \
#define RUN_ARON_MACRO(upperType, lowerType, capsType, upperData, lowerData, capsData) \
case type::Descriptor::e##upperType: \
{ \
- typenavigator::upperType##NavigatorPtr t = typenavigator::upperType##Navigator::DynamicCastAndCheck(type); \
+ auto t = typenavigator::upperType##Navigator::DynamicCastAndCheck(type); \
return datanavigator::NavigatorPtr(new datanavigator::upperData##Navigator()); \
}
HANDLE_PRIMITIVE_CORRESPONDING
#undef RUN_ARON_MACRO
- default:
+ case type::Descriptor::eUnknown:
{
throw error::DescriptorNotValidException("Randomizer", "generateAronDataFromType", "No valid type found!", desc);
}
}
+ // end of non void....... dont know why
+ return nullptr;
}
void initializeRandomly(datanavigator::NavigatorPtr& data, const typenavigator::NavigatorPtr& type) const
{
+ data = generateEmptyAronDataFromType(type);
+ if (!data)
+ {
+ if (type->getMaybe() == type::Maybe::eNone)
+ {
+ throw error::AronException("Randomizer", "initializeRandomly", "The data is null but the type is not a maybe type. This is not valid.");
+ }
+ return;
+ }
+
// Containers
type::Descriptor desc = type->getDescriptor();
switch (desc)
@@ -250,31 +333,62 @@ case type::Descriptor::e##upperType: \
HANDLE_ALL_CORRESPONDING
#undef RUN_ARON_MACRO
- default:
+ case type::Descriptor::eUnknown:
{
throw error::DescriptorNotValidException("Randomizer", "initializeRandomly", "No valid type found!", desc);
}
}
}
+ // generate i in [min, max)
int generateRandom(int max, int min) const
{
- max += 1;
+ if (max < min)
+ {
+ throw error::IndexNotValidException("AronRandomizer", "generateRandom", "The max value is lower than the min value", max, min);
+ }
int random = (std::rand() % (max - min)) + min;
return random;
}
+ template <class T>
+ T getRandomElement(const std::vector<T>& vec) const
+ {
+ int i = generateRandom(vec.size(), 0);
+ return vec.at(i);
+ }
+
+ template <class T>
+ std::string getRandomKey(const std::map<std::string, T>& m) const
+ {
+ std::vector<std::string> keys;
+ for (const auto [k, _] : m)
+ {
+ keys.push_back(k);
+ }
+ return getRandomElement(keys);
+ }
+
+ bool fiftyPercentChance() const
+ {
+ return generateRandom(2, 0);
+ }
+
std::string generateRandomWord(const std::set<std::string>& usedKeys = {}) const
{
+ // https://randomwordgenerator.com/
+ // script to convert random words into string:
+ // jQuery("#result li i").remove(); var str = ""; jQuery("#result li").each(function() {str += jQuery(this).html() + ", " }); console.log(str);
std::vector<string> words =
{
- "Lorem", "ipsum", "dolor", "sit", "amet", "consetetur", "sadipscing", "elitr"
- "sed", "diam", "nonumy", "eirmod", "tempor", "invidunt", "ut", "labore", "et"
- "dolore", "magna", "aliquyam", "eratsed"
+ "jaw", "river", "bow", "profession", "heat", "interference", "slave", "population", "deport", "redeem", "digress", "penny", "cousin", "beef", "Bible", "fuss",
+ "urgency", "tasty", "innovation", "upset", "gold", "day", "remunerate", "strain", "district", "panel", "choke", "rack", "parameter", "despair", "extort", "country",
+ "hesitate", "record", "brand", "confusion", "discreet", "accept", "lifestyle", "option", "corn", "charity", "miss", "viable", "glance", "norm", "meet", "bird",
+ "ribbon", "guideline"
};
- int i = generateRandom(words.size() - 1, 0);
- std::string key = words[i];
+ int i = generateRandom(words.size(), 0);
+ std::string key = words.at(i);
while (usedKeys.count(key) > 0)
{
@@ -289,18 +403,11 @@ case type::Descriptor::e##upperType: \
std::vector<unsigned char> new_blob(size, 0);
for (unsigned int i = 0; i < size; ++i)
{
- new_blob[i] = (generateRandom(127, 0));
+ new_blob[i] = (generateRandom(128, 0));
}
return new_blob;
}
- private:
- void initialize_random() const
- {
- std::srand(std::time(nullptr));
- }
-
- public:
void initializeRandomly(datanavigator::DictNavigatorPtr& data, const typenavigator::ObjectNavigatorPtr& type) const
{
for (auto& [key, nextData] : data->getElements())
@@ -318,7 +425,7 @@ case type::Descriptor::e##upperType: \
{
std::string key = generateRandomWord(usedKeys);
usedKeys.insert(key);
- datanavigator::NavigatorPtr newData = generateAronDataFromType(type->getAcceptedType());
+ datanavigator::NavigatorPtr newData = generateEmptyAronDataFromType(type->getAcceptedType());
initializeRandomly(newData, type->getAcceptedType());
data->addElement(key, newData);
}
@@ -347,7 +454,7 @@ case type::Descriptor::e##upperType: \
int numElements = generateRandom(5, 1);
for (int i = 0; i < numElements; ++i)
{
- datanavigator::NavigatorPtr newData = generateAronDataFromType(type->getAcceptedType());
+ datanavigator::NavigatorPtr newData = generateEmptyAronDataFromType(type->getAcceptedType());
initializeRandomly(newData, type->getAcceptedType());
data->addElement(newData);
}
@@ -383,12 +490,18 @@ case type::Descriptor::e##upperType: \
void initializeRandomly(datanavigator::BoolNavigatorPtr& data, const typenavigator::NavigatorPtr& type) const
{
- data->setValue(generateRandom(1, 0));
+ data->setValue(fiftyPercentChance());
}
void initializeRandomly(datanavigator::StringNavigatorPtr& data, const typenavigator::NavigatorPtr& type) const
{
data->setValue(generateRandomWord());
}
+
+ private:
+ void initializeSelf() const
+ {
+ std::srand(std::time(nullptr));
+ }
};
}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/CodeWriter.h b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/CodeWriter.h
index f2f9b12c22c9a8bcf3ca88ddc4ba19f6f8168b14..eec1b3db30bf39bf726f24b7c150e8102d409e60 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/CodeWriter.h
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/CodeWriter.h
@@ -33,10 +33,7 @@
// ArmarX
#include <ArmarXCore/libraries/cppgen/MetaClass.h>
-#include <RobotAPI/libraries/aron/core/navigator/type/Navigator.h>
-#include <RobotAPI/libraries/aron/core/navigator/type/container/Object.h>
-#include <RobotAPI/libraries/aron/core/navigator/type/container/List.h>
-#include <RobotAPI/libraries/aron/core/navigator/type/primitive/Primitive.h>
+#include <RobotAPI/libraries/aron/core/navigator/type/AllNavigators.h>
#include <RobotAPI/libraries/aron/core/codegenerator/WriterInfo.h>
#include <RobotAPI/libraries/aron/core/codegenerator/ReaderInfo.h>
@@ -73,8 +70,6 @@ namespace armarx::aron::codegenerator
std::vector<codegeneratorhelper::WriterInfoPtr> dataWriters;
std::vector<codegeneratorhelper::ReaderInfoPtr> dataReaders;
std::vector<codegeneratorhelper::WriterInfoPtr> initialTypeWriters;
- std::vector<codegeneratorhelper::WriterInfoPtr> currentTypeWriters;
- std::vector<codegeneratorhelper::ReaderInfoPtr> typeReaders;
std::vector<std::string> additionalIncludes;
};
}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/AronCppClass.h b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/AronCppClass.h
index 7098b08c6630be31bbb44c0fe098d09071e68b45..7249a77170fd0f5c5e930d3d41ae465502ad3e96 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/AronCppClass.h
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/AronCppClass.h
@@ -35,7 +35,7 @@
#include <RobotAPI/libraries/aron/core/io/typeIO/Writer.h>
-namespace armarx::aron::cppcodegenerator
+namespace armarx::aron::cppserializer
{
class AronCppClass;
typedef std::shared_ptr<AronCppClass> AronCppClassPtr;
@@ -47,14 +47,17 @@ namespace armarx::aron::cppcodegenerator
public:
AronCppClass() = default;
- virtual void reset() = 0;
- virtual void initialize() = 0;
- virtual void read(armarx::aron::dataIO::ReaderInterface& r) = 0;
+
+ /// Reset all member values of this class to default (as stated in the XML). This may mean that maybe types are null or false and images may be created as headers_only
+ virtual void resetHard() = 0;
+
+ /// Reset all member values of this class softly, meaning if a maybe type has a value, we reset only the value (not the full maybe type) and if an image has data (width, height) we keep the data and width and height and only reset teh pixel values
+ virtual void resetSoft() = 0;
+
+ /// Set members according to loaded data in ReaderInterface. If skip_first_readStartDict is true, we skip the first call of r.readStartDict() (It was already called outside. This is important for nested classes)
+ virtual void read(armarx::aron::dataIO::ReaderInterface& r, bool skip_first_readStartDict) = 0;
+
+ /// Set the data in WriterInterface to the values of this class. If a maybe type is null or false, this method calls w.writeNull().
virtual void write(armarx::aron::dataIO::WriterInterface& w) const = 0;
- virtual void writeCurrentType(armarx::aron::typeIO::WriterInterface& w) const = 0;
- static void writeType(armarx::aron::typeIO::WriterInterface& w)
- {
- throw error::AronException("AronCppClass", "writeType", "You are not allowed to call this method directly. Each class must inheriting from AronCppClass must implement this static method!");
- };
};
}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/Writer.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/Writer.cpp
index c0d2067811d86fd394dd29be507a9af7fcb96ba5..86dc2874398a8bde89ae9224e4cdb8a6f47a83c7 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/Writer.cpp
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/Writer.cpp
@@ -30,7 +30,7 @@
// ArmarX
-namespace armarx::aron::cppcodegenerator
+namespace armarx::aron::cppserializer
{
Writer::Writer(const std::string& producerName, const std::vector<std::string>& additionalIncludesFromXMLFile) :
CodeWriter(producerName, additionalIncludesFromXMLFile)
@@ -77,18 +77,11 @@ namespace armarx::aron::cppcodegenerator
{
// The toAron Serializer is visible by default
codegeneratorhelper::WriterInfoPtr toAronType = codegeneratorhelper::WriterInfoPtr(new codegeneratorhelper::WriterInfo());
- toAronType->methodName = "toInitialAronType";
+ toAronType->methodName = "toAronType";
toAronType->returnType = "armarx::aron::typenavigator::ObjectNavigatorPtr";
toAronType->writerClassType = "armarx::aron::typeIO::writer::NavigatorWriter";
toAronType->include = "<RobotAPI/libraries/aron/core/io/typeIO/writer/navigator/NavigatorWriter.h>";
initialTypeWriters.push_back(toAronType);
-
- // The toAron Serializer is visible by default
- codegeneratorhelper::WriterInfoPtr toAronType2 = codegeneratorhelper::WriterInfoPtr(new codegeneratorhelper::WriterInfo());
- toAronType2->methodName = "toCurrentAronType";
- toAronType2->returnType = "armarx::aron::typenavigator::ObjectNavigatorPtr";
- toAronType2->writerClassType = "armarx::aron::typeIO::writer::NavigatorWriter";
- currentTypeWriters.push_back(toAronType2);
}
void Writer::generateTypeObjects(const std::vector<codegeneratorhelper::GenerateObjectInfoPtr>& generateObjects)
@@ -97,7 +90,7 @@ namespace armarx::aron::cppcodegenerator
{
const auto& nav = publicGenerateObjectType->correspondingType;
- //std::cout << "Generating: " << publicGenerateObjectType->correspondingObjectType->getName() << std::endl;
+ //std::cout << "Generating: " << nav->getName() << std::endl;
// Convert to Object type and create class object
if (nav == nullptr)
@@ -117,7 +110,7 @@ namespace armarx::aron::cppcodegenerator
}
else
{
- c->addInherit("virtual public armarx::aron::cppcodegenerator::AronCppClass");
+ c->addInherit("public armarx::aron::cppserializer::AronCppClass");
c->addInclude("<RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/AronCppClass.h>");
}
@@ -151,20 +144,16 @@ namespace armarx::aron::cppcodegenerator
c->addMethod(equals);
//std::cout << "Generate reset method" << std::endl;
- CppMethodPtr reset = serializer->toResetMethod();
- c->addMethod(reset);
+ CppMethodPtr resetHard = serializer->toResetHardMethod();
+ c->addMethod(resetHard);
//std::cout << "Generate init method" << std::endl;
- CppMethodPtr setup = serializer->toInitializeMethod();
- c->addMethod(setup);
+ CppMethodPtr resetSoft = serializer->toResetSoftMethod();
+ c->addMethod(resetSoft);
//std::cout << "Generate writeInit method" << std::endl;
- CppMethodPtr writeInitialType = serializer->toWriteInitialTypeMethod();
- c->addMethod(writeInitialType);
-
- //std::cout << "Generate writeCurrent method" << std::endl;
- CppMethodPtr writeCurrentType = serializer->toWriteCurrentTypeMethod();
- c->addMethod(writeCurrentType);
+ CppMethodPtr writeType = serializer->toWriteTypeMethod();
+ c->addMethod(writeType);
//std::cout << "Generate write method" << std::endl;
CppMethodPtr write = serializer->toWriteMethod();
@@ -185,6 +174,8 @@ namespace armarx::aron::cppcodegenerator
c->addMethod(convert);
}
+ //std::cout << "set member vars" << std::endl;
+
// Add methods to set the member variables
for (const codegeneratorhelper::ReaderInfoPtr& info : dataReaders)
{
@@ -196,38 +187,20 @@ namespace armarx::aron::cppcodegenerator
c->addMethod(convert);
}
- // Typewritermethods
- for (const codegeneratorhelper::WriterInfoPtr& info : initialTypeWriters)
- {
- if (!info->include.empty())
- {
- c->addInclude(info->include);
- }
- CppMethodPtr convert = serializer->toSpecializedInitialTypeWriterMethod(info->returnType, info->methodName, info->writerClassType, "armarx::aron::typenavigator::ObjectNavigator::DynamicCast");
- c->addMethod(convert);
- }
+ //std::cout << "type writer methods" << std::endl;
// Typewritermethods
- for (const codegeneratorhelper::WriterInfoPtr& info : currentTypeWriters)
+ for (const codegeneratorhelper::WriterInfoPtr& info : initialTypeWriters)
{
if (!info->include.empty())
{
c->addInclude(info->include);
}
- CppMethodPtr convert = serializer->toSpecializedCurrentTypeWriterMethod(info->returnType, info->methodName, info->writerClassType, "armarx::aron::typenavigator::ObjectNavigator::DynamicCast");
+ CppMethodPtr convert = serializer->toSpecializedTypeWriterMethod(info->returnType, info->methodName, info->writerClassType, "armarx::aron::typenavigator::ObjectNavigator::DynamicCast");
c->addMethod(convert);
}
- // Add methods to set the member variables
- for (const codegeneratorhelper::ReaderInfoPtr& info : typeReaders)
- {
- if (!info->include.empty())
- {
- c->addInclude(info->include);
- }
- CppMethodPtr convert = serializer->toSpecializedCurrentTypeReaderMethod(info->argumentType, info->methodName, info->readerClassType);
- c->addMethod(convert);
- }
+ //std::cout << "push back" << std::endl;
typeClasses.push_back(c);
}
@@ -259,7 +232,7 @@ namespace armarx::aron::cppcodegenerator
c->addInclude("<map>");
c->addInclude("<RobotAPI/interface/aron.h>");
- c->addInherit("virtual public armarx::aron::cppcodegenerator::AronCppClass");
+ c->addInherit("virtual public armarx::aron::cppserializer::AronCppClass");
c->addInclude("<RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/AronCppClass.h>");
// ctor
@@ -295,21 +268,17 @@ namespace armarx::aron::cppcodegenerator
c->addMethod(equals);
//std::cout << "Generate reset method" << std::endl;
- CppMethodPtr reset = serializer->toResetMethod();
- c->addMethod(reset);
+ CppMethodPtr resetHard = serializer->toResetHardMethod();
+ c->addMethod(resetHard);
//std::cout << "Generate init method" << std::endl;
- CppMethodPtr setup = serializer->toInitializeMethod();
- c->addMethod(setup);
+ CppMethodPtr resetSoft = serializer->toResetSoftMethod();
+ c->addMethod(resetSoft);
//std::cout << "Generate writeInit method" << std::endl;
- CppMethodPtr writeInitialType = serializer->toWriteInitialTypeMethod();
+ CppMethodPtr writeInitialType = serializer->toWriteTypeMethod();
c->addMethod(writeInitialType);
- //std::cout << "Generate writeCurrent method" << std::endl;
- CppMethodPtr writeCurrentType = serializer->toWriteCurrentTypeMethod();
- c->addMethod(writeCurrentType);
-
//std::cout << "Generate write method" << std::endl;
CppMethodPtr write = serializer->toWriteMethod();
c->addMethod(write);
@@ -341,24 +310,21 @@ namespace armarx::aron::cppcodegenerator
}
else
{
- c->addInherit("virtual public armarx::aron::cppcodegenerator::AronCppClass");
+ c->addInherit("virtual public armarx::aron::cppserializer::AronCppClass");
}
CppMethodPtr equals = serializer->toEqualsMethod();
c->addMethod(equals);
- CppMethodPtr reset = serializer->toResetMethod();
- c->addMethod(reset);
+ CppMethodPtr resetHard = serializer->toResetHardMethod();
+ c->addMethod(resetHard);
- CppMethodPtr setup = serializer->toInitializeMethod();
- c->addMethod(setup);
+ CppMethodPtr resetSoft = serializer->toResetSoftMethod();
+ c->addMethod(resetSoft);
- CppMethodPtr writeInitialType = serializer->toWriteInitialTypeMethod();
+ CppMethodPtr writeInitialType = serializer->toWriteTypeMethod();
c->addMethod(writeInitialType);
- CppMethodPtr writeCurrentType = serializer->toWriteCurrentTypeMethod();
- c->addMethod(writeCurrentType);
-
CppMethodPtr write = serializer->toWriteMethod();
c->addMethod(write);
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/Writer.h b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/Writer.h
index 91dd59689dfb678b45ac6b5190ea1b8a04f05cd4..d9b27c9fe82987a78724f7d837d9d3343a4af943 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/Writer.h
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/Writer.h
@@ -40,7 +40,7 @@
#include <RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/toplevel/ObjectClass.h>
#include <RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/toplevel/IntEnumClass.h>
-namespace armarx::aron::cppcodegenerator
+namespace armarx::aron::cppserializer
{
class Writer;
typedef std::shared_ptr<Writer> WriterPtr;
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/AllSerializers.h b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/AllSerializers.h
index d4c7f75c8d1498eec120718bde8c0194e9153277..dd01bd127edd43a72cdafa370d96b68d8ebd77fa 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/AllSerializers.h
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/AllSerializers.h
@@ -5,6 +5,7 @@
#include "container/Dict.h"
#include "container/Tuple.h"
#include "container/Pair.h"
+#include "ndarray/NDArray.h"
#include "ndarray/EigenMatrix.h"
#include "ndarray/EigenQuaternion.h"
#include "ndarray/IVTCByteImage.h"
@@ -14,4 +15,10 @@
#include "ndarray/Orientation.h"
#include "ndarray/Pose.h"
#include "enum/IntEnum.h"
-#include "primitive/Primitive.h"
+#include "primitive/Int.h"
+#include "primitive/Long.h"
+#include "primitive/Float.h"
+#include "primitive/Double.h"
+#include "primitive/String.h"
+#include "primitive/Bool.h"
+#include "primitive/Time.h"
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/Serializer.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/Serializer.cpp
index 3b18a9f8c7c220e125fe828ce52868fdabab4d83..19220470403b00537971203580f336acb6105aa2 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/Serializer.cpp
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/Serializer.cpp
@@ -29,50 +29,307 @@
// ArmarX
#include <RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/SerializerFactory.h>
-namespace armarx::aron::cppcodegenerator
+namespace armarx::aron::cppserializer
{
// constantes
const std::string Serializer::ARON_DATA_NAME = simox::meta::get_type_name(typeid(armarx::aron::data::AronData));
const std::string Serializer::ARON_DATA_PTR_NAME = Serializer::ARON_DATA_NAME + "::PointerType";
+ const std::string Serializer::READ_START_RETURN_TYPE_ACCESSOR = "_return_type";
+
const std::string Serializer::ARON_TYPE_NAME = simox::meta::get_type_name(typeid(armarx::aron::type::AronType));
const std::string Serializer::ARON_TYPE_PTR_NAME = Serializer::ARON_TYPE_NAME + "::PointerType";
- const std::map<std::string, std::string> Serializer::RESOLVE_TYPENAMES =
- {
- {"string", "std::string"},
- };
-
const std::map<std::string, std::string> Serializer::ESCAPE_ACCESSORS =
{
- {"->", "_ptr_"},
- {".", "_dot_"},
- {"[", "_lbr_"},
- {"]", "_rbr_"},
+ {"->", "__ptr__"},
+ {".", "__dot__"},
+ {"[", "__lbrC__"},
+ {"]", "__rbrC__"},
+ {"(", "__lbrR__"},
+ {")", "__rbrR__"},
+ {"*", "__ast__"},
};
const SerializerFactoryPtr Serializer::FACTORY = SerializerFactoryPtr(new SerializerFactory());
+ // constructors
+ Serializer::Serializer(const std::string& cppName, const std::string& aronDataTypename, const std::string& aronTypeTypename) :
+ cppTypename(cppName),
+ aronDataTypename(aronDataTypename),
+ aronTypeTypename(aronTypeTypename)
+ {
+
+ }
+
// static methods
- std::string Serializer::ResolveCppTypename(const std::string& enteredTypeName)
+ std::string Serializer::ResolveMaybeAccessor(const std::string& s, const typenavigator::NavigatorPtr& t)
{
- auto i = Serializer::RESOLVE_TYPENAMES.find(enteredTypeName);
- if (i != Serializer::RESOLVE_TYPENAMES.end())
+ if (t->getMaybe() == type::Maybe::eOptional)
+ {
+ return s + ".value()";
+ }
+ if (t->getMaybe() == type::Maybe::eRawPointer || t->getMaybe() == type::Maybe::eSharedPointer || t->getMaybe() == type::Maybe::eUniquePointer)
{
- return i->second;
+ return "*" + s;
}
- return enteredTypeName;
+ return s;
}
- std::vector<std::string> Serializer::ResolveCppTypenames(const std::vector<std::string>& enteredTypeNames)
+ CppBlockPtr Serializer::ResolveMaybeWriteBlock(const std::string& accessor, const CppBlockPtr& block_if_data, const typenavigator::NavigatorPtr& typenavigator)
{
- std::vector<std::string> ret;
- for (const auto& t : enteredTypeNames)
+ CppBlockPtr b = std::make_shared<CppBlock>();
+
+ if (!accessor.empty())
{
- ret.push_back(Serializer::ResolveCppTypename(t));
+ b->addLine("// Generate WriteInterface API calls for " + accessor);
}
- return ret;
+ else
+ {
+ b->addLine("// Generate WriteInterface API calls for the top level object " + ExtractCppTypename(typenavigator));
+ }
+
+ if (typenavigator->getMaybe() != type::Maybe::eNone)
+ {
+ switch (typenavigator->getMaybe())
+ {
+ case type::Maybe::eOptional:
+ {
+ b->addLine("if (" + accessor + ".has_value())");
+ b->addBlock(block_if_data);
+ break;
+ }
+ case type::Maybe::eRawPointer:
+ {
+ b->addLine("if (" + accessor + " != NULL)");
+ b->addBlock(block_if_data);
+ break;
+ }
+ case type::Maybe::eSharedPointer:
+ [[fallthrough]];
+ case type::Maybe::eUniquePointer:
+ {
+ b->addLine("if (" + accessor + " != nullptr)");
+ b->addBlock(block_if_data);
+ break;
+ }
+ default:
+ {
+ throw error::MaybeNotValidException("Serializer", "ResolveMaybeWriteBlock", "Unkown Maybe type (perhaps forgot to add in switch case?)", typenavigator->getMaybe(), typenavigator->getPath());
+ }
+ }
+
+ b->addLine("else");
+ CppBlockPtr block_if_no_data = std::make_shared<CppBlock>();
+ block_if_no_data->addLine("w.writeNull(); // The maybe type '" + accessor + "' has no data, so we simply write NULL (resulting in a AronData nullhandle)");
+ b->addBlock(block_if_no_data);
+ return b;
+ }
+ b->appendBlock(block_if_data);
+ return b;
+ }
+
+ CppBlockPtr Serializer::ResolveMaybeReadBlock(const std::string& accessor, const std::string& readElement, const CppBlockPtr& block_if_data, const typenavigator::NavigatorPtr& typenavigator)
+ {
+ std::string escaped_accessor = EscapeAccessor(accessor);
+ std::string read_start_result_accessor = escaped_accessor + READ_START_RETURN_TYPE_ACCESSOR;
+
+ CppBlockPtr resolved_block = std::make_shared<CppBlock>();
+
+ if (!accessor.empty())
+ {
+ resolved_block->addLine("// Generate ReadInterface API calls for " + accessor);
+ }
+ else
+ {
+ resolved_block->addLine("// Generate ReadInterface API calls for the top level object " + ExtractCppTypename(typenavigator));
+ }
+
+ if (!readElement.empty())
+ {
+ resolved_block->addLine("auto " + read_start_result_accessor + " = " + readElement + "; // of " + accessor);
+ }
+
+ if (typenavigator->getMaybe() != type::Maybe::eNone)
+ {
+ resolved_block->addLine("if (" + read_start_result_accessor + ".success)");
+ CppBlockPtr if_success = std::make_shared<CppBlock>();
+
+ switch (typenavigator->getMaybe())
+ {
+ case type::Maybe::eOptional:
+ {
+ auto s = FromAronTypeNaviagtorPtr(typenavigator);
+ if_success->addLine(accessor + " = " + s->getFullTypenameGenerator() + "();");
+ if_success->appendBlock(block_if_data);
+ break;
+ }
+ case type::Maybe::eRawPointer:
+ {
+ auto s = FromAronTypeNaviagtorPtr(typenavigator);
+ if_success->addLine(accessor + " = " + s->getFullTypenameGenerator() + "();");
+ if_success->appendBlock(block_if_data);
+ break;
+ }
+ case type::Maybe::eSharedPointer:
+ [[fallthrough]];
+ case type::Maybe::eUniquePointer:
+ {
+ auto s = FromAronTypeNaviagtorPtr(typenavigator);
+ if_success->addLine(accessor + " = " + s->getFullTypenameGenerator() + "();");
+ if_success->appendBlock(block_if_data);
+ break;
+ }
+ default:
+ {
+ throw error::MaybeNotValidException("Serializer", "ResolveMaybeReadBlock", "Unkown Maybe type (perhaps forgot to add in switch case?)", typenavigator->getMaybe(), typenavigator->getPath());
+ }
+ }
+ resolved_block->addBlock(if_success);
+ return resolved_block;
+ }
+ resolved_block->appendBlock(block_if_data);
+ return resolved_block;
+ }
+
+ CppBlockPtr Serializer::ResolveMaybeEqualsBlock(const std::string& accessor, const std::string& otherInstanceAccessor, const CppBlockPtr& block_if_data, const typenavigator::NavigatorPtr& typenavigator)
+ {
+ CppBlockPtr b = std::make_shared<CppBlock>();
+ if (!accessor.empty())
+ {
+ b->addLine("// Comparing " + accessor + " and " + otherInstanceAccessor);
+ }
+ else
+ {
+ b->addLine("// Comparing two objects of type " + ExtractCppTypename(typenavigator));
+ }
+
+ switch (typenavigator->getMaybe())
+ {
+ case type::Maybe::eNone:
+ {
+ b->appendBlock(block_if_data);
+ break;
+ }
+ case type::Maybe::eOptional:
+ {
+ b->addLine("if (" + accessor + ".has_value() and " + otherInstanceAccessor + ".has_value()) // both have a value set");
+ b->addBlock(block_if_data);
+ b->addLine("if ((" + accessor + ".has_value() and !" + otherInstanceAccessor + ".has_value()) or (!" + accessor + ".has_value() and " + otherInstanceAccessor + ".has_value())) // only one has a value set (XOR)");
+ b->addLine("\t return false;");
+ break;
+ }
+ case type::Maybe::eRawPointer:
+ {
+ b->addLine("if (" + accessor + " != NULL and " + otherInstanceAccessor + " != NULL) // both have a value set");
+ b->addBlock(block_if_data);
+ b->addLine("if ((" + accessor + " != NULL and " + otherInstanceAccessor + " == NULL) or (" + accessor + " == NULL and " + otherInstanceAccessor + " != NULL)) // only one has a value set (XOR)");
+ b->addLine("\t return false;");
+ break;
+ }
+ case type::Maybe::eSharedPointer:
+ [[fallthrough]];
+ case type::Maybe::eUniquePointer:
+ {
+ b->addLine("if (" + accessor + " != nullptr and " + otherInstanceAccessor + " != nullptr) // both have a value set");
+ b->addBlock(block_if_data);
+ b->addLine("if ((" + accessor + " != nullptr and " + otherInstanceAccessor + " == nullptr) or (" + accessor + " == nullptr and " + otherInstanceAccessor + " != nullptr)) // only one has a value set (XOR)");
+ b->addLine("\t return false;");
+ break;
+ }
+ default:
+ {
+ throw error::MaybeNotValidException("Serializer", "ResolveMaybeEqualsBlock", "Unkown Maybe type (perhaps forgot to add in switch case?)", typenavigator->getMaybe(), typenavigator->getPath());
+ }
+ }
+ return b;
+ }
+
+ CppBlockPtr Serializer::ResolveMaybeResetSoftBlock(const std::string& accessor, const CppBlockPtr& block_if_data, const typenavigator::NavigatorPtr& typenavigator)
+ {
+ CppBlockPtr b = std::make_shared<CppBlock>();
+
+ if (!accessor.empty())
+ {
+ b->addLine("// Resetting soft member " + accessor);
+ }
+ else
+ {
+ b->addLine("// Ressetting soft the type " + ExtractCppTypename(typenavigator));
+ }
+
+ switch (typenavigator->getMaybe())
+ {
+ case type::Maybe::eNone:
+ {
+ b->appendBlock(block_if_data);
+ break;
+ }
+ case type::Maybe::eOptional:
+ {
+ b->addLine("if (" + accessor + ".has_value())");
+ b->addBlock(block_if_data);
+ break;
+ }
+ case type::Maybe::eRawPointer:
+ {
+ b->addLine("if (" + accessor + " != NULL)");
+ b->addBlock(block_if_data);
+ break;
+ }
+ case type::Maybe::eSharedPointer:
+ [[fallthrough]];
+ case type::Maybe::eUniquePointer:
+ {
+ b->addLine("if (" + accessor + " != nullptr)");
+ b->addBlock(block_if_data);
+ break;
+ }
+ default:
+ {
+ throw error::MaybeNotValidException("Serializer", "ResolveMaybeResetSoftBlock", "Unkown Maybe type (perhaps forgot to add in switch case?)", typenavigator->getMaybe(), typenavigator->getPath());
+ }
+ }
+ return b;
+ }
+
+ CppBlockPtr Serializer::ResolveMaybeResetHardBlock(const std::string& accessor, const CppBlockPtr& block_if_data, const typenavigator::NavigatorPtr& typenavigator)
+ {
+ CppBlockPtr b = std::make_shared<CppBlock>();
+ if (!accessor.empty())
+ {
+ b->addLine("// Resetting hard member " + accessor);
+ }
+ else
+ {
+ b->addLine("// Ressetting hard the type " + ExtractCppTypename(typenavigator));
+ }
+
+ switch (typenavigator->getMaybe())
+ {
+ case type::Maybe::eNone:
+ {
+ b->appendBlock(block_if_data);
+ break;
+ }
+ case type::Maybe::eOptional:
+ [[fallthrough]];
+ case type::Maybe::eRawPointer:
+ [[fallthrough]];
+ case type::Maybe::eSharedPointer:
+ [[fallthrough]];
+ case type::Maybe::eUniquePointer:
+ {
+ b->addLine(accessor + " = {};");
+ break;
+ }
+ default:
+ {
+ throw error::MaybeNotValidException("Serializer", "ResolveMaybeResetSoftBlock", "Unkown Maybe type (perhaps forgot to add in switch case?)", typenavigator->getMaybe(), typenavigator->getPath());
+ }
+ }
+ return b;
}
std::string Serializer::EscapeAccessor(const std::string& accessor)
@@ -98,7 +355,7 @@ namespace armarx::aron::cppcodegenerator
std::string Serializer::ExtractCppTypename(const typenavigator::NavigatorPtr& n)
{
SerializerPtr cpp = Serializer::FromAronTypeNaviagtorPtr(n);
- return cpp->getFullCppTypename();
+ return cpp->getCoreCppTypename();
}
std::vector<std::string> Serializer::ExtractCppTypenames(const std::vector<typenavigator::NavigatorPtr>& n)
@@ -113,29 +370,16 @@ namespace armarx::aron::cppcodegenerator
SerializerPtr Serializer::FromAronTypeNaviagtorPtr(const typenavigator::NavigatorPtr& n)
{
+ ARMARX_CHECK_NOT_NULL(n);
return FACTORY->create(n, n->getPath());
}
- // constructors
- Serializer::Serializer(const std::string& cppName, const std::string& aronDataTypename, const std::string& aronTypeTypename, bool is_ptr) :
- is_ptr(is_ptr),
- cppTypename(cppName),
- aronDataTypename(aronDataTypename),
- aronTypeTypename(aronTypeTypename)
- {
-
- }
-
+ // public methods
std::string Serializer::getCoreCppTypename() const
{
return cppTypename;
}
- std::string Serializer::getFullCppTypename() const
- {
- return is_ptr ? "std::shared_ptr<" + cppTypename + ">" : cppTypename;
- }
-
std::string Serializer::getAronDataTypename() const
{
return aronDataTypename;
@@ -161,7 +405,7 @@ namespace armarx::aron::cppcodegenerator
CppCtorPtr c = CppCtorPtr(new CppCtor(name + "()"));
std::vector<std::pair<std::string, std::string>> initList = this->getCtorInitializers("");
CppBlockPtr b = CppBlockPtr(new CppBlock());
- b->addLine("initialize();");
+ b->addLine("resetHard();");
b->appendBlock(this->getCtorBlock(""));
c->addInitListEntries(initList);
c->setBlock(b);
@@ -169,50 +413,38 @@ namespace armarx::aron::cppcodegenerator
return c;
}
- CppMethodPtr Serializer::toResetMethod() const
+ CppMethodPtr Serializer::toResetSoftMethod() const
{
std::stringstream doc;
- doc << "@brief reset() - This method resets all member variables to default. \n";
+ doc << "@brief resetSoft() - This method resets all member variables with respect to the current parameterization. \n";
doc << "@return - nothing";
- CppMethodPtr m = CppMethodPtr(new CppMethod("virtual void reset() override", doc.str()));
- CppBlockPtr b = this->getResetBlock("");
+ CppMethodPtr m = CppMethodPtr(new CppMethod("virtual void resetSoft() override", doc.str()));
+ CppBlockPtr b = this->getResetSoftBlock("");
m->setBlock(b);
return m;
}
- CppMethodPtr Serializer::toInitializeMethod() const
+ CppMethodPtr Serializer::toResetHardMethod() const
{
std::stringstream doc;
- doc << "@brief initialize() - This method initializes member variables according to the XML type description. \n";
+ doc << "@brief resetHard() - This method resets member variables according to the XML type description. \n";
doc << "@return - nothing";
- CppMethodPtr m = CppMethodPtr(new CppMethod("virtual void initialize() override", doc.str()));
- CppBlockPtr b = this->getInitializeBlock("");
+ CppMethodPtr m = CppMethodPtr(new CppMethod("virtual void resetHard() override", doc.str()));
+ CppBlockPtr b = this->getResetHardBlock("");
m->setBlock(b);
return m;
}
- CppMethodPtr Serializer::toWriteInitialTypeMethod() const
+ CppMethodPtr Serializer::toWriteTypeMethod() const
{
std::stringstream doc;
doc << "@brief writeType() - This method returns a new type from the class structure using a type writer implementation. This function is static. \n";
doc << "@return - the result of the writer implementation";
- CppMethodPtr m = CppMethodPtr(new CppMethod("static void writeInitialType(armarx::aron::typeIO::WriterInterface& w)", doc.str()));
- CppBlockPtr b = this->getWriteInitialTypeBlock("");
- m->setBlock(b);
- return m;
- }
-
- CppMethodPtr Serializer::toWriteCurrentTypeMethod() const
- {
- std::stringstream doc;
- doc << "@brief writeType() - This method returns a new type from the current class structure using a type writer implementation. \n";
- doc << "@return - the result of the writer implementation";
-
- CppMethodPtr m = CppMethodPtr(new CppMethod("void writeCurrentType(armarx::aron::typeIO::WriterInterface& w) const override", doc.str()));
- CppBlockPtr b = this->getWriteCurrentTypeBlock("");
+ CppMethodPtr m = CppMethodPtr(new CppMethod("static void writeType(armarx::aron::typeIO::WriterInterface& w, armarx::aron::type::Maybe __aronMaybeType = armarx::aron::type::Maybe::eNone)", doc.str()));
+ CppBlockPtr b = this->getWriteTypeBlock("");
m->setBlock(b);
return m;
}
@@ -237,8 +469,10 @@ namespace armarx::aron::cppcodegenerator
doc << "@param r - The reader implementation\n";
doc << "@return - nothing";
- CppMethodPtr m = CppMethodPtr(new CppMethod("virtual void read(armarx::aron::dataIO::ReaderInterface& r) override", doc.str()));
- CppBlockPtr b = this->getReadBlock("");
+ CppMethodPtr m = CppMethodPtr(new CppMethod("virtual void read(armarx::aron::dataIO::ReaderInterface& r, bool skip_first_readStartDict = false) override", doc.str()));
+ CppBlockPtr b = std::make_shared<CppBlock>();
+ b->addLine("this->resetSoft();");
+ b->appendBlock(this->getReadBlock(""));
m->setBlock(b);
return m;
}
@@ -268,7 +502,7 @@ namespace armarx::aron::cppcodegenerator
return m;
}
- CppMethodPtr Serializer::toSpecializedInitialTypeWriterMethod(const std::string& returnname, const std::string& methodname, const std::string& writerName, const std::string& enforceConversion) const
+ CppMethodPtr Serializer::toSpecializedTypeWriterMethod(const std::string& returnname, const std::string& methodname, const std::string& writerName, const std::string& enforceConversion) const
{
std::stringstream doc;
doc << "@brief specializedTypeWrite() - This method returns a new type from the member data types using a writer implementation. \n";
@@ -276,36 +510,11 @@ namespace armarx::aron::cppcodegenerator
CppMethodPtr m = CppMethodPtr(new CppMethod("static " + returnname + " " + methodname + "()", doc.str()));
m->addLine(writerName + " writer;");
- m->addLine("writeInitialType(writer);");
- m->addLine("return " + enforceConversion + "(writer.getResult());");
- return m;
- }
-
- CppMethodPtr Serializer::toSpecializedCurrentTypeWriterMethod(const std::string& returnname, const std::string& methodname, const std::string& writerName, const std::string& enforceConversion) const
- {
- std::stringstream doc;
- doc << "@brief specializedTypeWrite() - This method returns a new type from the current member data types using a writer implementation. \n";
- doc << "@return - the result of the writer implementation";
-
- CppMethodPtr m = CppMethodPtr(new CppMethod(returnname + " " + methodname + "()", doc.str()));
- m->addLine(writerName + " writer;");
- m->addLine("this->writeCurrentType(writer);");
+ m->addLine("writeType(writer);");
m->addLine("return " + enforceConversion + "(writer.getResult());");
return m;
}
- CppMethodPtr Serializer::toSpecializedCurrentTypeReaderMethod(const std::string& argumentname, const std::string& methodname, const std::string& readerName, const std::string& enforceConversion) const
- {
- std::stringstream doc;
- doc << "@brief specializedTypeRead() - This method sets the structure of the 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(" + enforceConversion + "(input));");
- m->addLine("this->readType(reader);");
- return m;
- }
-
CppMethodPtr Serializer::toEqualsMethod() const
{
std::stringstream doc;
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/Serializer.h b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/Serializer.h
index 9d115fb427be210f023a79d54f713bfe07501bc1..ce4f11e9bc22d06d9d075f69d123127d50b9c70a 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/Serializer.h
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/Serializer.h
@@ -32,6 +32,7 @@
// Simox
#include <SimoxUtility/meta/type_name.h>
#include <SimoxUtility/algorithm/string.h>
+#include <SimoxUtility/algorithm/vector.hpp>
// ArmarX
#include <ArmarXCore/libraries/cppgen/CppBlock.h>
@@ -44,7 +45,7 @@
#include <RobotAPI/libraries/aron/core/Exception.h>
#include <RobotAPI/libraries/aron/core/navigator/type/Navigator.h>
-namespace armarx::aron::cppcodegenerator
+namespace armarx::aron::cppserializer
{
class SerializerFactory;
typedef std::shared_ptr<SerializerFactory> SerializerFactoryPtr;
@@ -59,11 +60,13 @@ namespace armarx::aron::cppcodegenerator
public:
// constructors
- Serializer(const std::string& cppName, const std::string& aronDataTypename, const std::string& aronTypeTypename, bool is_ptr = false);
+ Serializer() = default;
+ Serializer(const std::string& cppName, const std::string& aronDataTypename, const std::string& aronTypeTypename);
// public member methods
std::string getCoreCppTypename() const;
- std::string getFullCppTypename() const;
+ virtual std::string getFullCppTypename() const = 0;
+ virtual std::string getFullTypenameGenerator() const = 0;
std::string getAronDataTypename() const;
std::string getAronDataPtrTypename() const;
@@ -74,9 +77,7 @@ namespace armarx::aron::cppcodegenerator
CppMethodPtr toSpecializedDataWriterMethod(const std::string&, const std::string&, const std::string&, const std::string& enforceType = "") const;
CppMethodPtr toSpecializedDataReaderMethod(const std::string&, const std::string&, const std::string&, const std::string& enforceType = "") const;
- CppMethodPtr toSpecializedInitialTypeWriterMethod(const std::string&, const std::string&, const std::string&, const std::string& enforceType = "") const;
- CppMethodPtr toSpecializedCurrentTypeWriterMethod(const std::string&, const std::string&, const std::string&, const std::string& enforceType = "") const;
- CppMethodPtr toSpecializedCurrentTypeReaderMethod(const std::string&, const std::string&, const std::string&, const std::string& enforceType = "") const;
+ CppMethodPtr toSpecializedTypeWriterMethod(const std::string&, const std::string&, const std::string&, const std::string& enforceType = "") const;
// virtual override definitions
virtual std::vector<CppFieldPtr> getPublicVariableDeclarations(const std::string&) const = 0;
@@ -85,17 +86,14 @@ namespace armarx::aron::cppcodegenerator
virtual std::vector<std::pair<std::string, std::string>> getCtorInitializers(const std::string&) const = 0;
virtual CppBlockPtr getCtorBlock(const std::string&) const = 0;
- CppMethodPtr toResetMethod() const;
- virtual CppBlockPtr getResetBlock(const std::string&) const = 0;
+ CppMethodPtr toResetSoftMethod() const;
+ virtual CppBlockPtr getResetSoftBlock(const std::string&) const = 0;
- CppMethodPtr toWriteInitialTypeMethod() const;
- virtual CppBlockPtr getWriteInitialTypeBlock(const std::string&) const = 0;
+ CppMethodPtr toWriteTypeMethod() const;
+ virtual CppBlockPtr getWriteTypeBlock(const std::string&) const = 0;
- CppMethodPtr toWriteCurrentTypeMethod() const;
- virtual CppBlockPtr getWriteCurrentTypeBlock(const std::string&) const = 0;
-
- CppMethodPtr toInitializeMethod() const;
- virtual CppBlockPtr getInitializeBlock(const std::string&) const = 0;
+ CppMethodPtr toResetHardMethod() const;
+ virtual CppBlockPtr getResetHardBlock(const std::string&) const = 0;
CppMethodPtr toWriteMethod() const;
virtual CppBlockPtr getWriteBlock(const std::string&) const = 0;
@@ -107,8 +105,6 @@ namespace armarx::aron::cppcodegenerator
virtual CppBlockPtr getEqualsBlock(const std::string&, const std::string&) const = 0;
// static methods
- static std::string ResolveCppTypename(const std::string&);
- static std::vector<std::string> ResolveCppTypenames(const std::vector<std::string>&);
static std::string EscapeAccessor(const std::string&);
static std::string UnescapeAccessor(const std::string&);
@@ -117,20 +113,27 @@ namespace armarx::aron::cppcodegenerator
static SerializerPtr FromAronTypeNaviagtorPtr(const typenavigator::NavigatorPtr&);
+ static std::string ResolveMaybeAccessor(const std::string&, const typenavigator::NavigatorPtr&);
+ static CppBlockPtr ResolveMaybeWriteBlock(const std::string&, const CppBlockPtr&, const typenavigator::NavigatorPtr&);
+ static CppBlockPtr ResolveMaybeReadBlock(const std::string&, const std::string&, const CppBlockPtr&, const typenavigator::NavigatorPtr&);
+ static CppBlockPtr ResolveMaybeEqualsBlock(const std::string&, const std::string&, const CppBlockPtr&, const typenavigator::NavigatorPtr&);
+ static CppBlockPtr ResolveMaybeResetSoftBlock(const std::string&, const CppBlockPtr&, const typenavigator::NavigatorPtr&);
+ static CppBlockPtr ResolveMaybeResetHardBlock(const std::string&, const CppBlockPtr&, const typenavigator::NavigatorPtr&);
+
+ protected:
+ static const std::string READ_START_RETURN_TYPE_ACCESSOR;
+
private:
- // members
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;
- static const std::map<std::string, std::string> RESOLVE_TYPENAMES;
static const std::map<std::string, std::string> ESCAPE_ACCESSORS;
static const SerializerFactoryPtr FACTORY;
- bool is_ptr;
std::string cppTypename;
std::string aronDataTypename;
std::string aronTypeTypename;
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/SerializerFactory.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/SerializerFactory.cpp
index 15d698675592fa368cca5b212300d839c2142b8f..080176c5743b1924313493799227d97a1d4f7714 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/SerializerFactory.cpp
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/SerializerFactory.cpp
@@ -29,42 +29,24 @@
// ArmarX
#include <RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/AllSerializers.h>
-namespace armarx::aron::cppcodegenerator
+namespace armarx::aron::cppserializer
{
// Access method
SerializerPtr SerializerFactory::create(const typenavigator::NavigatorPtr& n, const Path& path) const
{
+#define RUN_ARON_MACRO(upperType, lowerType, capsType) \
+ {type::Descriptor::e##upperType, SerializerFactoryPtr(new upperType##SerializerFactory())},
static const std::map<type::Descriptor, SerializerFactoryPtr> Factories =
{
- {type::Descriptor::eObject, SerializerFactoryPtr(new ObjectSerializerFactory())},
- {type::Descriptor::eDict, SerializerFactoryPtr(new DictSerializerFactory())},
- {type::Descriptor::eList, SerializerFactoryPtr(new ListSerializerFactory())},
- {type::Descriptor::eTuple, SerializerFactoryPtr(new TupleSerializerFactory())},
- {type::Descriptor::ePair, SerializerFactoryPtr(new PairSerializerFactory())},
- {type::Descriptor::eEigenMatrix, SerializerFactoryPtr(new EigenMatrixSerializerFactory())},
- {type::Descriptor::eEigenQuaternion, SerializerFactoryPtr(new EigenQuaternionSerializerFactory())},
- {type::Descriptor::eIVTCByteImage, SerializerFactoryPtr(new IVTCByteImageSerializerFactory())},
- {type::Descriptor::eOpenCVMat, SerializerFactoryPtr(new OpenCVMatSerializerFactory())},
- {type::Descriptor::ePCLPointCloud, SerializerFactoryPtr(new PCLPointCloudSerializerFactory())},
- {type::Descriptor::ePosition, SerializerFactoryPtr(new PositionSerializerFactory())},
- {type::Descriptor::eOrientation, SerializerFactoryPtr(new OrientationSerializerFactory())},
- {type::Descriptor::ePose, SerializerFactoryPtr(new PoseSerializerFactory())},
- {type::Descriptor::eIntEnum, SerializerFactoryPtr(new IntEnumSerializerFactory())},
- {type::Descriptor::eInt, SerializerFactoryPtr(new IntSerializerFactory())},
- {type::Descriptor::eLong, SerializerFactoryPtr(new LongSerializerFactory())},
- {type::Descriptor::eFloat, SerializerFactoryPtr(new FloatSerializerFactory())},
- {type::Descriptor::eDouble, SerializerFactoryPtr(new DoubleSerializerFactory())},
- {type::Descriptor::eString, SerializerFactoryPtr(new StringSerializerFactory())},
- {type::Descriptor::eBool, SerializerFactoryPtr(new BoolSerializerFactory())},
- {type::Descriptor::eTime, SerializerFactoryPtr(new TimeSerializerFactory())}
+ HANDLE_ALL_ARON_TYPES
};
+#undef RUN_ARON_MACRO
//CheckIfPtrIsNull("NavigatorFactory", "create", path, n);
- auto res = n->getResult();
- auto factory_iterator = Factories.find(Resolver::GetDescriptor(res));
+ auto factory_iterator = Factories.find(n->getDescriptor());
if (factory_iterator == Factories.end())
{
- throw error::AronException("NavigatorFactory", "create", "Cannot find the desired factory for input (ice_id): " + res->ice_id() + ". Cannot create navigator", path);
+ throw error::AronException("NavigatorFactory", "create", "Cannot find the desired factory for input (ice_id): " + n->toAronPtr()->ice_id() + ". Cannot create navigator", path);
}
return factory_iterator->second->createSpecific(n, path);
}
@@ -76,11 +58,9 @@ namespace armarx::aron::cppcodegenerator
// Factories
#define RUN_ARON_MACRO(upperType, lowerType, capsType) \
- SerializerPtr upperType##SerializerFactory::createSpecific(const typenavigator::NavigatorPtr& n, const Path& path) const \
+ SerializerPtr upperType##SerializerFactory::createSpecific(const typenavigator::NavigatorPtr& n, const Path&) const \
{ \
- /*CheckIfInputIsNull("Aron"+std::string(#upperType)+"TypeCppSerializerFactory", "createSpecific[BeforeCast]", path, n); */\
- typenavigator::upperType##NavigatorPtr casted = typenavigator::upperType##Navigator::DynamicCast(n); \
- /*CheckIfInputIsNull("Aron"+std::string(#upperType)+"TypeCppSerializerFactory", "createSpecific[AfterCast]", path, casted); */\
+ typenavigator::upperType##NavigatorPtr casted = typenavigator::upperType##Navigator::DynamicCastAndCheck(n); \
return SerializerPtr(new serializer::upperType##Serializer(casted)); \
}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/SerializerFactory.h b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/SerializerFactory.h
index 07af1e5fefcd375e2dea87a1d66d1798e6d40816..5c24d5692f980651a721fe0ffd6cadc54aa327c9 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/SerializerFactory.h
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/SerializerFactory.h
@@ -35,7 +35,7 @@
#include <RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/AllSerializers.h>
#include <RobotAPI/libraries/aron/core/Descriptor.h>
-namespace armarx::aron::cppcodegenerator
+namespace armarx::aron::cppserializer
{
class SerializerFactory;
typedef std::shared_ptr<SerializerFactory> SerializerFactoryPtr;
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/container/Dict.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/container/Dict.cpp
index 7431b8ee225433eccd286322310c4feb47082ebe..10b3470db62c07bad810b95099bfab57568674b2 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/container/Dict.cpp
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/container/Dict.cpp
@@ -25,128 +25,96 @@
#include "Dict.h"
-namespace armarx::aron::cppcodegenerator::serializer
+namespace armarx::aron::cppserializer::serializer
{
// constructors
- DictSerializer::DictSerializer(const typenavigator::DictNavigatorPtr& n) :
- Serializer("std::map<std::string, " + FromAronTypeNaviagtorPtr(n->getAcceptedType())->getFullCppTypename() + ">", simox::meta::get_type_name(typeid(data::AronDict)), simox::meta::get_type_name(typeid(type::AronDict))),
- navigator(n)
+ DictSerializer::DictSerializer(const typenavigator::DictNavigatorPtr& e) :
+ detail::ContainerSerializerBase<typenavigator::DictNavigator, DictSerializer>("std::map<std::string, " + FromAronTypeNaviagtorPtr(e->getAcceptedType())->getFullCppTypename() + ">",
+ simox::meta::get_type_name(typeid(data::AronDict)),
+ simox::meta::get_type_name(typeid(type::AronDict)), e)
{
-
}
// virtual implementations
- std::vector<CppFieldPtr> DictSerializer::getPublicVariableDeclarations(const std::string& name) const
- {
- CppFieldPtr field = CppFieldPtr(new CppField(getFullCppTypename(), name));
- return {field};
- }
-
- std::vector<std::pair<std::string, std::string>> DictSerializer::getCtorInitializers(const std::string&) const
- {
- return {};
- }
-
- CppBlockPtr DictSerializer::getCtorBlock(const std::string&) const
- {
- CppBlockPtr b = CppBlockPtr(new CppBlock());
- return b;
- }
-
- CppBlockPtr DictSerializer::getResetBlock(const std::string& accessor) const
+ CppBlockPtr DictSerializer::getResetSoftBlock(const std::string& accessor) const
{
- CppBlockPtr b = CppBlockPtr(new CppBlock());
- b->addLine(accessor + ".clear();");
- return b;
- }
+ CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
+ std::string resolved_accessor = ResolveMaybeAccessor(accessor, typenavigator);
- CppBlockPtr DictSerializer::getInitializeBlock(const std::string& accessor) const
- {
- CppBlockPtr b = CppBlockPtr(new CppBlock());
- b->addLine(accessor + " = {};");
- return b;
+ block_if_data->addLine(accessor + nextEl() + "clear();");
+ return ResolveMaybeResetSoftBlock(accessor, block_if_data, typenavigator);
}
- CppBlockPtr DictSerializer::getWriteInitialTypeBlock(const std::string& accessor) const
+ CppBlockPtr DictSerializer::getResetHardBlock(const std::string& accessor) const
{
- CppBlockPtr b = CppBlockPtr(new CppBlock());
- b->addLine("w.writeStartDict();");
-
- std::string escaped_accessor = EscapeAccessor(accessor);
- std::string accessor_iterator = escaped_accessor + DICT_ITERATOR_ACCESSOR;
-
- auto type_s = FromAronTypeNaviagtorPtr(navigator->getAcceptedType());
- CppBlockPtr b2 = type_s->getWriteInitialTypeBlock(type_s->getFullCppTypename());
- b->appendBlock(b2);
- b->addLine("w.writeEndDict();");
- return b;
+ CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
+ block_if_data->addLine(accessor + " = " + this->getFullTypenameGenerator() + "();");
+ return ResolveMaybeResetHardBlock(accessor, block_if_data, typenavigator);
}
- CppBlockPtr DictSerializer::getWriteCurrentTypeBlock(const std::string& accessor) const
+ CppBlockPtr DictSerializer::getWriteTypeBlock(const std::string& accessor) const
{
CppBlockPtr b = CppBlockPtr(new CppBlock());
- b->addLine("w.writeStartDict();");
-
- std::string escaped_accessor = EscapeAccessor(accessor);
- std::string accessor_iterator = escaped_accessor + DICT_ITERATOR_ACCESSOR;
+ b->addLine("w.writeStartDict({" + MAYBE_TO_STRING(typenavigator->getMaybe()) + "}); // of " + accessor);
- auto type_s = FromAronTypeNaviagtorPtr(navigator->getAcceptedType());
- CppBlockPtr b2 = type_s->getWriteCurrentTypeBlock(type_s->getFullCppTypename());
+ auto type_s = FromAronTypeNaviagtorPtr(typenavigator->getAcceptedType());
+ CppBlockPtr b2 = type_s->getWriteTypeBlock(type_s->getFullCppTypename());
b->appendBlock(b2);
- b->addLine("w.writeEndDict();");
+ b->addLine("w.writeEndDict(); // of " + accessor);
return b;
}
CppBlockPtr DictSerializer::getWriteBlock(const std::string& accessor) const
{
- CppBlockPtr b = CppBlockPtr(new CppBlock());
- b->addLine("w.writeStartDict();");
+ CppBlockPtr block_if_data = std::make_shared<CppBlock>();
+ block_if_data->addLine("w.writeStartDict(); // of " + accessor);
std::string escaped_accessor = EscapeAccessor(accessor);
std::string accessor_iterator_key = escaped_accessor + DICT_KEY_ACCESSOR;
std::string accessor_iterator_val = escaped_accessor + DICT_VALUE_ACCESSOR;
- b->addLine("for (const auto& [" + accessor_iterator_key + ", " + accessor_iterator_val + "] : " + accessor + ") ");
+ std::string resolved_accessor = ResolveMaybeAccessor(accessor, typenavigator);
+
+ block_if_data->addLine("for (const auto& [" + accessor_iterator_key + ", " + accessor_iterator_val + "] : " + resolved_accessor + ") ");
- auto type_s = FromAronTypeNaviagtorPtr(navigator->getAcceptedType());
+ auto type_s = FromAronTypeNaviagtorPtr(typenavigator->getAcceptedType());
CppBlockPtr b2 = CppBlockPtr(new CppBlock());
b2->addLine("w.writeKey(" + accessor_iterator_key + ");");
- b2 = CppBlock::MergeBlocks(b2, type_s->getWriteBlock(accessor_iterator_val));
- b->addBlock(b2);
+ b2->appendBlock(type_s->getWriteBlock(accessor_iterator_val));
+ block_if_data->addBlock(b2);
+ block_if_data->addLine("w.writeEndDict(); // of " + accessor);
- b->addLine("w.writeEndDict();");
- return b;
+ return ResolveMaybeWriteBlock(accessor, block_if_data, typenavigator);
}
CppBlockPtr DictSerializer::getReadBlock(const std::string& accessor) const
{
- CppBlockPtr b = CppBlockPtr(new CppBlock());
- b->addLine(accessor + ".clear();");
- b->addLine("r.readStartDict();");
+ CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
std::string escaped_accessor = EscapeAccessor(accessor);
std::string accessor_iterator = escaped_accessor + DICT_ITERATOR_ACCESSOR;
std::string accessor_iterator_key = escaped_accessor + DICT_KEY_ACCESSOR;
- auto type_s = FromAronTypeNaviagtorPtr(navigator->getAcceptedType());
+ auto type_s = FromAronTypeNaviagtorPtr(typenavigator->getAcceptedType());
- b->addLine("while(!r.readEndDict())");
+ block_if_data->addLine("while(!r.readEndDict()) // of " + accessor);
CppBlockPtr b2 = CppBlockPtr(new CppBlock());
- b2->addLine("std::string " + accessor_iterator_key + " = r.readKey();");
+
+ b2->addLine("std::string " + accessor_iterator_key + " = r.readKey(); // in dict " + accessor);
b2->addLine(type_s->getFullCppTypename() + " " + accessor_iterator + ";");
- b2 = CppBlock::MergeBlocks(b2, type_s->getReadBlock(accessor_iterator));
- b2->addLine(accessor + "[" + accessor_iterator_key + "] = " + accessor_iterator + ";");
+ b2->appendBlock(type_s->getReadBlock(accessor_iterator));
+ b2->addLine(accessor + nextEl() + "insert({" + accessor_iterator_key + ", " + accessor_iterator + "});");
- b->addBlock(b2);
- return b;
+ block_if_data->addBlock(b2);
+ return ResolveMaybeReadBlock(accessor, "r.readStartDict()", block_if_data, typenavigator);
}
CppBlockPtr DictSerializer::getEqualsBlock(const std::string& accessor, const std::string& otherInstanceAccessor) const
{
- CppBlockPtr b = CppBlockPtr(new CppBlock());
- b->addLine("if (not (" + accessor + " == " + otherInstanceAccessor + "))");
- b->addLine("\t return false;");
- return b;
+ CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
+ block_if_data->addLine("if (not (" + accessor + " == " + otherInstanceAccessor + "))");
+ block_if_data->addLine("\t return false;");
+ return ResolveMaybeEqualsBlock(accessor, otherInstanceAccessor, block_if_data, typenavigator);
}
}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/container/Dict.h b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/container/Dict.h
index 9e2e459761cb8589877893609a04da40b5579030..9bd48144aeb8cdf578ac4d8c491343f539e0bcc8 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/container/Dict.h
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/container/Dict.h
@@ -28,19 +28,19 @@
#include <map>
// Base Class
-#include <RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/Serializer.h>
+#include "../detail/ContainerSerializerBase.h"
// ArmarX
-#include <RobotAPI/libraries/aron/core/navigator/type/container/Dict.h>
+#include "../../../../../navigator/type/container/Dict.h"
-namespace armarx::aron::cppcodegenerator::serializer
+namespace armarx::aron::cppserializer::serializer
{
class DictSerializer;
typedef std::shared_ptr<DictSerializer> DictSerializerPtr;
class DictSerializer :
- virtual public Serializer
+ public detail::ContainerSerializerBase<typenavigator::DictNavigator, DictSerializer>
{
public:
using PointerType = DictSerializerPtr;
@@ -51,13 +51,9 @@ namespace armarx::aron::cppcodegenerator::serializer
DictSerializer(const typenavigator::DictNavigatorPtr&);
// virtual implementations
- virtual std::vector<CppFieldPtr> getPublicVariableDeclarations(const std::string&) const override;
- virtual std::vector<std::pair<std::string, std::string>> getCtorInitializers(const std::string&) const override;
- virtual CppBlockPtr getCtorBlock(const std::string&) const override;
- virtual CppBlockPtr getWriteInitialTypeBlock(const std::string&) const override;
- virtual CppBlockPtr getWriteCurrentTypeBlock(const std::string&) const override;
- virtual CppBlockPtr getInitializeBlock(const std::string&) const override;
- virtual CppBlockPtr getResetBlock(const std::string&) const override;
+ virtual CppBlockPtr getWriteTypeBlock(const std::string&) const override;
+ virtual CppBlockPtr getResetHardBlock(const std::string&) const override;
+ virtual CppBlockPtr getResetSoftBlock(const std::string&) const override;
virtual CppBlockPtr getWriteBlock(const std::string&) const override;
virtual CppBlockPtr getReadBlock(const std::string&) const override;
virtual CppBlockPtr getEqualsBlock(const std::string&, const std::string&) const override;
@@ -67,7 +63,5 @@ namespace armarx::aron::cppcodegenerator::serializer
static constexpr const char* DICT_ITERATOR_ACCESSOR = "_iterator";
static constexpr const char* DICT_KEY_ACCESSOR = "_key";
static constexpr const char* DICT_VALUE_ACCESSOR = "_value";
-
- typenavigator::DictNavigatorPtr navigator;
};
}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/container/List.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/container/List.cpp
index 2e4f924eb68608d9662231d94cae4e3cb89aa2ef..b0fb56473b5f2bb435365c45d9a5040f104bba24 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/container/List.cpp
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/container/List.cpp
@@ -24,115 +24,86 @@
// Header
#include "List.h"
-namespace armarx::aron::cppcodegenerator::serializer
+namespace armarx::aron::cppserializer::serializer
{
// constructors
ListSerializer::ListSerializer(const typenavigator::ListNavigatorPtr& e) :
- Serializer("std::vector<" + FromAronTypeNaviagtorPtr(e->getAcceptedType())->getFullCppTypename() + ">", simox::meta::get_type_name(typeid(data::AronList)), simox::meta::get_type_name(typeid(type::AronList))),
- typenavigator(e)
+ detail::ContainerSerializerBase<typenavigator::ListNavigator, ListSerializer>("std::vector<" + FromAronTypeNaviagtorPtr(e->getAcceptedType())->getFullCppTypename() + ">",
+ simox::meta::get_type_name(typeid(data::AronList)),
+ simox::meta::get_type_name(typeid(type::AronList)), e)
{
-
- }
-
- std::vector<CppFieldPtr> ListSerializer::getPublicVariableDeclarations(const std::string& name) const
- {
- CppFieldPtr field = CppFieldPtr(new CppField(getFullCppTypename(), name));
- return {field};
}
- std::vector<std::pair<std::string, std::string>> ListSerializer::getCtorInitializers(const std::string&) const
+ CppBlockPtr ListSerializer::getResetSoftBlock(const std::string& accessor) const
{
- return {};
- }
+ CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
+ std::string resolved_accessor = ResolveMaybeAccessor(accessor, typenavigator);
- CppBlockPtr ListSerializer::getCtorBlock(const std::string&) const
- {
- CppBlockPtr b = CppBlockPtr(new CppBlock());
- return b;
+ block_if_data->addLine(accessor + nextEl() + "clear();");
+ return ResolveMaybeResetSoftBlock(accessor, block_if_data, typenavigator);
}
- CppBlockPtr ListSerializer::getResetBlock(const std::string& accessor) const
+ CppBlockPtr ListSerializer::getResetHardBlock(const std::string& accessor) const
{
- CppBlockPtr b = CppBlockPtr(new CppBlock());
- b->addLine(accessor + ".clear();");
- return b;
- }
-
- CppBlockPtr ListSerializer::getInitializeBlock(const std::string& accessor) const
- {
- CppBlockPtr b = CppBlockPtr(new CppBlock());
- b->addLine(accessor + " = {};");
- return b;
+ CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
+ block_if_data->addLine(accessor + " = " + this->getFullTypenameGenerator() + "();");
+ return ResolveMaybeResetHardBlock(accessor, block_if_data, typenavigator);
}
- CppBlockPtr ListSerializer::getWriteInitialTypeBlock(const std::string&) const
+ CppBlockPtr ListSerializer::getWriteTypeBlock(const std::string& accessor) const
{
CppBlockPtr b = CppBlockPtr(new CppBlock());
- b->addLine("w.writeStartList();");
- auto type_s = FromAronTypeNaviagtorPtr(typenavigator->getAcceptedType());
-
- CppBlockPtr b2 = type_s->getWriteInitialTypeBlock(type_s->getFullCppTypename());
- b->appendBlock(b2);
-
- b->addLine("w.writeEndList();");
- return b;
- }
- CppBlockPtr ListSerializer::getWriteCurrentTypeBlock(const std::string&) const
- {
- CppBlockPtr b = CppBlockPtr(new CppBlock());
- b->addLine("w.writeStartList();");
+ b->addLine("w.writeStartList({" + MAYBE_TO_STRING(typenavigator->getMaybe()) + "}); // of " + accessor);
auto type_s = FromAronTypeNaviagtorPtr(typenavigator->getAcceptedType());
- CppBlockPtr b2 = type_s->getWriteInitialTypeBlock(type_s->getFullCppTypename()); // TODO: think about having a better solution not ignoting the current type
+ CppBlockPtr b2 = type_s->getWriteTypeBlock(type_s->getFullCppTypename());
b->appendBlock(b2);
- b->addLine("w.writeEndList();");
+ b->addLine("w.writeEndList(); // of " + accessor);
return b;
}
CppBlockPtr ListSerializer::getWriteBlock(const std::string& accessor) const
{
- CppBlockPtr b = CppBlockPtr(new CppBlock());
- b->addLine("w.writeStartList();");
+ CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
+ block_if_data->addLine("w.writeStartList(); // of " + accessor);
std::string escaped_accessor = EscapeAccessor(accessor);
std::string accessor_iterator = escaped_accessor + LIST_ITERATOR_ACCESSOR;
auto type_s = FromAronTypeNaviagtorPtr(typenavigator->getAcceptedType());
- b->addLine("for(unsigned int " + accessor_iterator + " = 0; " + accessor_iterator + " < " + accessor + ".size(); ++" + accessor_iterator + ")");
- CppBlockPtr b2 = type_s->getWriteBlock(accessor + "[" + accessor_iterator + "]");
- b->addBlock(b2);
- b->addLine("w.writeEndList();");
- return b;
+ block_if_data->addLine("for(unsigned int " + accessor_iterator + " = 0; " + accessor_iterator + " < " + accessor + nextEl() + "size(); ++" + accessor_iterator + ")");
+ block_if_data->addBlock(type_s->getWriteBlock(accessor + nextEl() + "at(" + accessor_iterator + ")"));
+ block_if_data->addLine("w.writeEndList(); // of " + accessor);
+
+ return ResolveMaybeWriteBlock(accessor, block_if_data, typenavigator);
}
CppBlockPtr ListSerializer::getReadBlock(const std::string& accessor) const
{
- CppBlockPtr b = CppBlockPtr(new CppBlock());
- b->addLine(accessor + ".clear();");
- b->addLine("r.readStartList();");
+ CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
std::string escaped_accessor = EscapeAccessor(accessor);
std::string accessor_iterator = escaped_accessor + LIST_ITERATOR_ACCESSOR;
- b->addLine("while(!r.readEndList())");
+ block_if_data->addLine("while(!r.readEndList()) // of " + accessor);
CppBlockPtr b2 = CppBlockPtr(new CppBlock());
auto type_s = FromAronTypeNaviagtorPtr(typenavigator->getAcceptedType());
b2->addLine(type_s->getFullCppTypename() + " " + accessor_iterator + ";");
- b2 = CppBlock::MergeBlocks(b2, type_s->getReadBlock(accessor_iterator));
- b2->addLine(accessor + ".push_back(" + accessor_iterator + ");");
+ b2->appendBlock(type_s->getReadBlock(accessor_iterator));
+ b2->addLine(accessor + nextEl() + "push_back(" + accessor_iterator + ");");
- b->addBlock(b2);
- return b;
+ block_if_data->addBlock(b2);
+ return ResolveMaybeReadBlock(accessor, "r.readStartList()", block_if_data, typenavigator);
}
CppBlockPtr ListSerializer::getEqualsBlock(const std::string& accessor, const std::string& otherInstanceAccessor) const
{
- CppBlockPtr b = CppBlockPtr(new CppBlock());
- b->addLine("if (not (" + accessor + " == " + otherInstanceAccessor + "))");
- b->addLine("\t return false;");
- return b;
+ CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
+ block_if_data->addLine("if (not (" + accessor + " == " + otherInstanceAccessor + "))");
+ block_if_data->addLine("\t return false;");
+ return ResolveMaybeEqualsBlock(accessor, otherInstanceAccessor, block_if_data, typenavigator);
}
}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/container/List.h b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/container/List.h
index e35aaa2d8addcac9d6025e024c18c6f54b135589..6b8afaabeef27186acee30d0167be5f1632d1ad7 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/container/List.h
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/container/List.h
@@ -28,18 +28,18 @@
#include <vector>
// Base Class
-#include <RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/Serializer.h>
+#include "../detail/ContainerSerializerBase.h"
// ArmarX
-#include <RobotAPI/libraries/aron/core/navigator/type/container/List.h>
+#include "../../../../../navigator/type/container/List.h"
-namespace armarx::aron::cppcodegenerator::serializer
+namespace armarx::aron::cppserializer::serializer
{
class ListSerializer;
typedef std::shared_ptr<ListSerializer> ListSerializerPtr;
class ListSerializer :
- public Serializer
+ public detail::ContainerSerializerBase<typenavigator::ListNavigator, ListSerializer>
{
public:
using PointerType = ListSerializerPtr;
@@ -49,13 +49,9 @@ namespace armarx::aron::cppcodegenerator::serializer
ListSerializer(const typenavigator::ListNavigatorPtr&);
// virtual implementations
- virtual std::vector<CppFieldPtr> getPublicVariableDeclarations(const std::string&) const override;
- virtual std::vector<std::pair<std::string, std::string>> getCtorInitializers(const std::string&) const override;
- virtual CppBlockPtr getCtorBlock(const std::string&) const override;
- virtual CppBlockPtr getWriteInitialTypeBlock(const std::string&) const override;
- virtual CppBlockPtr getWriteCurrentTypeBlock(const std::string&) const override;
- virtual CppBlockPtr getInitializeBlock(const std::string&) const override;
- virtual CppBlockPtr getResetBlock(const std::string&) const override;
+ virtual CppBlockPtr getWriteTypeBlock(const std::string&) const override;
+ virtual CppBlockPtr getResetHardBlock(const std::string&) const override;
+ virtual CppBlockPtr getResetSoftBlock(const std::string&) const override;
virtual CppBlockPtr getWriteBlock(const std::string&) const override;
virtual CppBlockPtr getReadBlock(const std::string&) const override;
virtual CppBlockPtr getEqualsBlock(const std::string&, const std::string&) const override;
@@ -63,7 +59,5 @@ namespace armarx::aron::cppcodegenerator::serializer
private:
// members
static constexpr const char* LIST_ITERATOR_ACCESSOR = "_iterator";
-
- typenavigator::ListNavigatorPtr typenavigator;
};
}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/container/Object.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/container/Object.cpp
index 7dbfa3665199b9798232af977d5d53a63777716c..1ffbe7db264f0e27a4b74da7c9e5533193003af1 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/container/Object.cpp
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/container/Object.cpp
@@ -25,81 +25,60 @@
#include "Object.h"
-namespace armarx::aron::cppcodegenerator::serializer
+namespace armarx::aron::cppserializer::serializer
{
// constructors
ObjectSerializer::ObjectSerializer(const typenavigator::ObjectNavigatorPtr& e) :
- Serializer(e->getObjectName(), simox::meta::get_type_name(typeid(data::AronDict)), simox::meta::get_type_name(typeid(type::AronObject))),
- navigator(e)
+ detail::ContainerSerializerBase<typenavigator::ObjectNavigator, ObjectSerializer>(e->getObjectName(),
+ simox::meta::get_type_name(typeid(data::AronDict)),
+ simox::meta::get_type_name(typeid(type::AronObject)), e)
{
- //AddObjectTypeToAllObjectTypesList(ObjectNavigatorPtr(this));
}
- std::vector<CppFieldPtr> ObjectSerializer::getPublicVariableDeclarations(const std::string& name) const
+ CppBlockPtr ObjectSerializer::getResetSoftBlock(const std::string& accessor) const
{
- CppFieldPtr field = CppFieldPtr(new CppField(getFullCppTypename(), name));
- return {field};
- }
-
- std::vector<std::pair<std::string, std::string>> ObjectSerializer::getCtorInitializers(const std::string&) const
- {
- return {};
- }
-
- CppBlockPtr ObjectSerializer::getCtorBlock(const std::string&) const
- {
- CppBlockPtr b = CppBlockPtr(new CppBlock());
- return b;
- }
-
- CppBlockPtr ObjectSerializer::getResetBlock(const std::string& accessor) const
- {
- CppBlockPtr b = CppBlockPtr(new CppBlock());
- b->addLine(accessor + ".reset();");
- return b;
- }
+ CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
+ std::string resolved_accessor = ResolveMaybeAccessor(accessor, typenavigator);
- CppBlockPtr ObjectSerializer::getInitializeBlock(const std::string& accessor) const
- {
- CppBlockPtr b = CppBlockPtr(new CppBlock());
- b->addLine(accessor + ".initialize();");
- return b;
+ block_if_data->addLine(accessor + nextEl() + "resetSoft();");
+ return ResolveMaybeResetSoftBlock(accessor, block_if_data, typenavigator);
}
- CppBlockPtr ObjectSerializer::getWriteInitialTypeBlock(const std::string& accessor) const
+ CppBlockPtr ObjectSerializer::getResetHardBlock(const std::string& accessor) const
{
- CppBlockPtr b = CppBlockPtr(new CppBlock());
- b->addLine(accessor + "::writeInitialType(w);");
- return b;
+ CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
+ block_if_data->addLine(accessor + " = " + this->getFullTypenameGenerator() + "();");
+ return ResolveMaybeResetHardBlock(accessor, block_if_data, typenavigator);
}
- CppBlockPtr ObjectSerializer::getWriteCurrentTypeBlock(const std::string& accessor) const
+ CppBlockPtr ObjectSerializer::getWriteTypeBlock(const std::string& accessor) const
{
CppBlockPtr b = CppBlockPtr(new CppBlock());
- b->addLine(accessor + ".writeCurrentType(w);");
+ b->addLine(accessor + "::writeType(w, " + MAYBE_TO_STRING(typenavigator->getMaybe()) + "); // of " + accessor);
return b;
}
CppBlockPtr ObjectSerializer::getWriteBlock(const std::string& accessor) const
{
- CppBlockPtr b = CppBlockPtr(new CppBlock());
- b->addLine(accessor + ".write(w);");
- return b;
+ CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
+ block_if_data->addLine(accessor + nextEl() + "write(w); // of " + accessor);
+
+ return ResolveMaybeWriteBlock(accessor, block_if_data, typenavigator);
}
CppBlockPtr ObjectSerializer::getReadBlock(const std::string& accessor) const
{
- CppBlockPtr b = CppBlockPtr(new CppBlock());
- b->addLine(accessor + ".read(r);");
- return b;
+ CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
+ block_if_data->addLine(accessor + nextEl() + "read(r, true); // of " + accessor);
+ return ResolveMaybeReadBlock(accessor, "r.readStartDict()", block_if_data, typenavigator);
}
CppBlockPtr ObjectSerializer::getEqualsBlock(const std::string& accessor, const std::string& otherInstanceAccessor) const
{
- CppBlockPtr b = CppBlockPtr(new CppBlock());
- b->addLine("if (not (" + accessor + " == " + otherInstanceAccessor + "))");
- b->addLine("\t return false;");
- return b;
+ CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
+ block_if_data->addLine("if (not (" + accessor + " == " + otherInstanceAccessor + "))");
+ block_if_data->addLine("\t return false;");
+ return ResolveMaybeEqualsBlock(accessor, otherInstanceAccessor, block_if_data, typenavigator);
}
}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/container/Object.h b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/container/Object.h
index b879a8794e7b1f25d8d409581bcfb477d21344dd..d04a44557a19b5337f8678b788c68361a7b0ff0a 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/container/Object.h
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/container/Object.h
@@ -28,20 +28,20 @@
#include <map>
// Base Class
-#include <RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/Serializer.h>
+#include "../detail/ContainerSerializerBase.h"
// ArmarX
-#include <RobotAPI/libraries/aron/core/navigator/type/container/Object.h>
+#include "../../../../../navigator/type/container/Object.h"
-namespace armarx::aron::cppcodegenerator::serializer
+namespace armarx::aron::cppserializer::serializer
{
class ObjectSerializer;
typedef std::shared_ptr<ObjectSerializer> ObjectSerializerPtr;
class ObjectSerializer :
- virtual public Serializer
+ public detail::ContainerSerializerBase<typenavigator::ObjectNavigator, ObjectSerializer>
{
public:
using PointerType = ObjectSerializerPtr;
@@ -51,13 +51,9 @@ namespace armarx::aron::cppcodegenerator::serializer
ObjectSerializer(const typenavigator::ObjectNavigatorPtr&);
// virtual implementations
- virtual std::vector<CppFieldPtr> getPublicVariableDeclarations(const std::string&) const override;
- virtual std::vector<std::pair<std::string, std::string>> getCtorInitializers(const std::string&) const override;
- virtual CppBlockPtr getCtorBlock(const std::string&) const override;
- virtual CppBlockPtr getWriteInitialTypeBlock(const std::string&) const override;
- virtual CppBlockPtr getWriteCurrentTypeBlock(const std::string&) const override;
- virtual CppBlockPtr getInitializeBlock(const std::string&) const override;
- virtual CppBlockPtr getResetBlock(const std::string&) const override;
+ virtual CppBlockPtr getWriteTypeBlock(const std::string&) const override;
+ virtual CppBlockPtr getResetHardBlock(const std::string&) const override;
+ virtual CppBlockPtr getResetSoftBlock(const std::string&) const override;
virtual CppBlockPtr getWriteBlock(const std::string&) const override;
virtual CppBlockPtr getReadBlock(const std::string&) const override;
virtual CppBlockPtr getEqualsBlock(const std::string&, const std::string&) const override;
@@ -65,7 +61,5 @@ namespace armarx::aron::cppcodegenerator::serializer
private:
// members
static constexpr const char* EXTENDS_ITERATOR_ACCESSOR = "_extends";
-
- typenavigator::ObjectNavigatorPtr navigator;
};
}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/container/Pair.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/container/Pair.cpp
index 6981f8625b4d748d97b51188e96f54129cfd2033..6f78c29962573fb4c617392fa2b48398637e5bef 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/container/Pair.cpp
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/container/Pair.cpp
@@ -25,149 +25,100 @@
#include "Pair.h"
-namespace armarx::aron::cppcodegenerator::serializer
+namespace armarx::aron::cppserializer::serializer
{
PairSerializer::PairSerializer(const typenavigator::PairNavigatorPtr& e) :
- Serializer("std::pair<" + ExtractCppTypename(e->getFirstAcceptedType()) + ", " + ExtractCppTypename(e->getSecondAcceptedType()) + ">", simox::meta::get_type_name(typeid(data::AronList)), simox::meta::get_type_name(typeid(type::AronPair))),
- typenavigator(e)
+ detail::ContainerSerializerBase<typenavigator::PairNavigator, PairSerializer>("std::pair<" + ExtractCppTypename(e->getFirstAcceptedType()) + ", " + ExtractCppTypename(e->getSecondAcceptedType()) + ">",
+ simox::meta::get_type_name(typeid(data::AronList)),
+ simox::meta::get_type_name(typeid(type::AronPair)), e)
{
-
- }
-
- std::vector<CppFieldPtr> PairSerializer::getPublicVariableDeclarations(const std::string& name) const
- {
- CppFieldPtr field = CppFieldPtr(new CppField(getFullCppTypename(), name));
- return {field};
- }
-
- std::vector<std::pair<std::string, std::string>> PairSerializer::getCtorInitializers(const std::string&) const
- {
- return {};
}
- CppBlockPtr PairSerializer::getCtorBlock(const std::string&) const
+ CppBlockPtr PairSerializer::getResetHardBlock(const std::string& accessor) const
{
- CppBlockPtr b = CppBlockPtr(new CppBlock());
- return b;
+ CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
+ block_if_data->addLine(accessor + " = " + this->getFullTypenameGenerator() + "();");
+ return ResolveMaybeResetHardBlock(accessor, block_if_data, typenavigator);
}
- CppBlockPtr PairSerializer::getInitializeBlock(const std::string& accessor) const
+ CppBlockPtr PairSerializer::getResetSoftBlock(const std::string& accessor) const
{
- CppBlockPtr b = CppBlockPtr(new CppBlock());
+ CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
+ std::string resolved_accessor = ResolveMaybeAccessor(accessor, typenavigator);
auto child_s1 = FromAronTypeNaviagtorPtr(typenavigator->getFirstAcceptedType());
- CppBlockPtr b21 = child_s1->getInitializeBlock(accessor + ".first");
- b->appendBlock(b21);
+ CppBlockPtr b21 = child_s1->getResetSoftBlock(accessor + nextEl() + "first");
+ block_if_data->appendBlock(b21);
auto child_s2 = FromAronTypeNaviagtorPtr(typenavigator->getSecondAcceptedType());
- CppBlockPtr b22 = child_s2->getInitializeBlock(accessor + ".second");
- b->appendBlock(b22);
-
- return b;
+ CppBlockPtr b22 = child_s2->getResetSoftBlock(accessor + nextEl() + "second");
+ block_if_data->appendBlock(b22);
+ return ResolveMaybeResetSoftBlock(accessor, block_if_data, typenavigator);
}
- CppBlockPtr PairSerializer::getWriteInitialTypeBlock(const std::string& accessor) const
+ CppBlockPtr PairSerializer::getWriteTypeBlock(const std::string& accessor) const
{
CppBlockPtr b = CppBlockPtr(new CppBlock());
- b->addLine("w.writeStartPair();");
+ b->addLine("w.writeStartPair({" + MAYBE_TO_STRING(typenavigator->getMaybe()) + "}); // of " + accessor);
std::string escaped_accessor = EscapeAccessor(accessor);
auto child_s1 = FromAronTypeNaviagtorPtr(typenavigator->getFirstAcceptedType());
std::string accessor_iterator1 = escaped_accessor + PAIR_FIRST_ACCESSOR;
b->addLine("w.writeKey(\"0\");");
- CppBlockPtr b21 = child_s1->getWriteInitialTypeBlock(accessor_iterator1);
+ CppBlockPtr b21 = child_s1->getWriteTypeBlock(accessor_iterator1);
b->appendBlock(b21);
auto child_s2 = FromAronTypeNaviagtorPtr(typenavigator->getSecondAcceptedType());
std::string accessor_iterator2 = escaped_accessor + PAIR_SECOND_ACCESSOR;
b->addLine("w.writeKey(\"1\");");
- CppBlockPtr b22 = child_s2->getWriteInitialTypeBlock(accessor_iterator2);
- b->appendBlock(b22);
-
- b->addLine("w.writeEndPair();");
- return b;
- }
-
- CppBlockPtr PairSerializer::getWriteCurrentTypeBlock(const std::string& accessor) const
- {
- CppBlockPtr b = CppBlockPtr(new CppBlock());
- b->addLine("w.writeStartPair();");
-
- std::string escaped_accessor = EscapeAccessor(accessor);
-
- auto child_s1 = FromAronTypeNaviagtorPtr(typenavigator->getFirstAcceptedType());
- std::string accessor_iterator1 = escaped_accessor + PAIR_FIRST_ACCESSOR;
- b->addLine("w.writeKey(\"0\");");
- CppBlockPtr b21 = child_s1->getWriteCurrentTypeBlock(accessor_iterator1);
- b->appendBlock(b21);
-
- auto child_s2 = FromAronTypeNaviagtorPtr(typenavigator->getSecondAcceptedType());
- std::string accessor_iterator2 = escaped_accessor + PAIR_SECOND_ACCESSOR;
- b->addLine("w.writeKey(\"1\");");
- CppBlockPtr b22 = child_s2->getWriteCurrentTypeBlock(accessor_iterator2);
- b->appendBlock(b22);
-
- b->addLine("w.writeEndPair();");
- return b;
- }
-
- CppBlockPtr PairSerializer::getResetBlock(const std::string& accessor) const
- {
- CppBlockPtr b = CppBlockPtr(new CppBlock());
-
- auto child_s1 = FromAronTypeNaviagtorPtr(typenavigator->getFirstAcceptedType());
- CppBlockPtr b21 = child_s1->getResetBlock(accessor + ".first");
- b->appendBlock(b21);
-
- auto child_s2 = FromAronTypeNaviagtorPtr(typenavigator->getSecondAcceptedType());
- CppBlockPtr b22 = child_s2->getResetBlock(accessor + ".second");
+ CppBlockPtr b22 = child_s2->getWriteTypeBlock(accessor_iterator2);
b->appendBlock(b22);
+ b->addLine("w.writeEndPair(); // of " + accessor);
return b;
}
CppBlockPtr PairSerializer::getWriteBlock(const std::string& accessor) const
{
- CppBlockPtr b = CppBlockPtr(new CppBlock());
- b->addLine("w.writeStartList();");
+ CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
+ block_if_data->addLine("w.writeStartList(); // of " + accessor);
auto child_s1 = FromAronTypeNaviagtorPtr(typenavigator->getFirstAcceptedType());
- CppBlockPtr b21 = child_s1->getWriteBlock(accessor + ".first");
- b->appendBlock(b21);
+ CppBlockPtr b21 = child_s1->getWriteBlock(accessor + nextEl() + "first");
+ block_if_data->appendBlock(b21);
auto child_s2 = FromAronTypeNaviagtorPtr(typenavigator->getSecondAcceptedType());
- CppBlockPtr b22 = child_s2->getWriteBlock(accessor + ".second");
- b->appendBlock(b22);
+ CppBlockPtr b22 = child_s2->getWriteBlock(accessor + nextEl() + "second");
+ block_if_data->appendBlock(b22);
- b->addLine("w.writeEndList();");
- return b;
+ block_if_data->addLine("w.writeEndList(); // of " + accessor);
+
+ return ResolveMaybeWriteBlock(accessor, block_if_data, typenavigator);
}
CppBlockPtr PairSerializer::getReadBlock(const std::string& accessor) const
{
- CppBlockPtr b = CppBlockPtr(new CppBlock());
- b->addLine(accessor + " = {};");
- b->addLine("r.readStartList();");
+ CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
auto child_s1 = FromAronTypeNaviagtorPtr(typenavigator->getFirstAcceptedType());
- CppBlockPtr b21 = child_s1->getReadBlock(accessor + ".first");
- b->appendBlock(b21);
+ CppBlockPtr b21 = child_s1->getReadBlock(accessor + nextEl() + "first");
+ block_if_data->appendBlock(b21);
auto child_s2 = FromAronTypeNaviagtorPtr(typenavigator->getSecondAcceptedType());
- CppBlockPtr b22 = child_s2->getReadBlock(accessor + ".second");
- b->appendBlock(b22);
+ CppBlockPtr b22 = child_s2->getReadBlock(accessor + nextEl() + "second");
+ block_if_data->appendBlock(b22);
- b->addLine("r.readEndList();");
- return b;
+ block_if_data->addLine("r.readEndList(); // of " + accessor);
+ return ResolveMaybeReadBlock(accessor, "r.readStartList()", block_if_data, typenavigator);
}
CppBlockPtr PairSerializer::getEqualsBlock(const std::string& accessor, const std::string& otherInstanceAccessor) const
{
- CppBlockPtr b = CppBlockPtr(new CppBlock());
- b->addLine("if (not (" + accessor + " == " + otherInstanceAccessor + "))");
- b->addLine("\t return false;");
- return b;
+ CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
+ block_if_data->addLine("if (not (" + accessor + " == " + otherInstanceAccessor + "))");
+ block_if_data->addLine("\t return false;");
+ return ResolveMaybeEqualsBlock(accessor, otherInstanceAccessor, block_if_data, typenavigator);
}
}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/container/Pair.h b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/container/Pair.h
index f9441c02b5d4d82a866a32380c32794dc2df298e..a7ef2495bcd4939820808a1298dcda92d1c9e12c 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/container/Pair.h
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/container/Pair.h
@@ -28,19 +28,19 @@
#include <map>
// Base Class
-#include <RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/Serializer.h>
+#include "../detail/ContainerSerializerBase.h"
// ArmarX
-#include <RobotAPI/libraries/aron/core/navigator/type/container/Pair.h>
+#include "../../../../../navigator/type/container/Pair.h"
-namespace armarx::aron::cppcodegenerator::serializer
+namespace armarx::aron::cppserializer::serializer
{
class PairSerializer;
typedef std::shared_ptr<PairSerializer> PairSerializerPtr;
class PairSerializer :
- virtual public Serializer
+ public detail::ContainerSerializerBase<typenavigator::PairNavigator, PairSerializer>
{
public:
using PointerType = PairSerializerPtr;
@@ -50,13 +50,9 @@ namespace armarx::aron::cppcodegenerator::serializer
PairSerializer(const typenavigator::PairNavigatorPtr& e);
// virtual implementations
- virtual std::vector<CppFieldPtr> getPublicVariableDeclarations(const std::string&) const override;
- virtual std::vector<std::pair<std::string, std::string>> getCtorInitializers(const std::string&) const override;
- virtual CppBlockPtr getCtorBlock(const std::string&) const override;
- virtual CppBlockPtr getWriteInitialTypeBlock(const std::string&) const override;
- virtual CppBlockPtr getInitializeBlock(const std::string&) const override;
- virtual CppBlockPtr getWriteCurrentTypeBlock(const std::string&) const override;
- virtual CppBlockPtr getResetBlock(const std::string&) const override;
+ virtual CppBlockPtr getWriteTypeBlock(const std::string&) const override;
+ virtual CppBlockPtr getResetHardBlock(const std::string&) const override;
+ virtual CppBlockPtr getResetSoftBlock(const std::string&) const override;
virtual CppBlockPtr getWriteBlock(const std::string&) const override;
virtual CppBlockPtr getReadBlock(const std::string&) const override;
virtual CppBlockPtr getEqualsBlock(const std::string&, const std::string&) const override;
@@ -65,8 +61,5 @@ namespace armarx::aron::cppcodegenerator::serializer
// members
static constexpr const char* PAIR_FIRST_ACCESSOR = "_first";
static constexpr const char* PAIR_SECOND_ACCESSOR = "_second";
-
-
- typenavigator::PairNavigatorPtr typenavigator;
};
}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/container/Tuple.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/container/Tuple.cpp
index b83f2cd9e17c1e669749452fac0d827080b34a67..dab5b33a01a7bb3d12817645b60688ab930e8b2b 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/container/Tuple.cpp
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/container/Tuple.cpp
@@ -25,50 +25,41 @@
#include "Tuple.h"
-namespace armarx::aron::cppcodegenerator::serializer
+namespace armarx::aron::cppserializer::serializer
{
TupleSerializer::TupleSerializer(const typenavigator::TupleNavigatorPtr& e) :
- Serializer("std::tuple<" + simox::alg::join(ExtractCppTypenames(e->getAcceptedTypes()), ", ") + ">", simox::meta::get_type_name(typeid(data::AronList)), simox::meta::get_type_name(typeid(type::AronTuple))),
- typenavigator(e)
+ detail::ContainerSerializerBase<typenavigator::TupleNavigator, TupleSerializer>("std::tuple<" + simox::alg::join(ExtractCppTypenames(e->getAcceptedTypes()), ", ") + ">",
+ simox::meta::get_type_name(typeid(data::AronList)),
+ simox::meta::get_type_name(typeid(type::AronTuple)), e)
{
-
- }
-
- std::vector<CppFieldPtr> TupleSerializer::getPublicVariableDeclarations(const std::string& name) const
- {
- CppFieldPtr field = CppFieldPtr(new CppField(getFullCppTypename(), name));
- return {field};
- }
-
- std::vector<std::pair<std::string, std::string>> TupleSerializer::getCtorInitializers(const std::string&) const
- {
- return {};
}
- CppBlockPtr TupleSerializer::getCtorBlock(const std::string&) const
+ CppBlockPtr TupleSerializer::getResetHardBlock(const std::string& accessor) const
{
- CppBlockPtr b = CppBlockPtr(new CppBlock());
- return b;
+ CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
+ block_if_data->addLine(accessor + " = " + this->getFullTypenameGenerator() + "();");
+ return ResolveMaybeResetHardBlock(accessor, block_if_data, typenavigator);
}
- CppBlockPtr TupleSerializer::getInitializeBlock(const std::string& accessor) const
+ CppBlockPtr TupleSerializer::getResetSoftBlock(const std::string& accessor) const
{
- CppBlockPtr b = CppBlockPtr(new CppBlock());
+ CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
+ std::string resolved_accessor = ResolveMaybeAccessor(accessor, typenavigator);
unsigned int i = 0;
for (const auto& child : typenavigator->getAcceptedTypes())
{
auto child_s = FromAronTypeNaviagtorPtr(child);
- CppBlockPtr b2 = child_s->getInitializeBlock("std::get<" + std::to_string(i++) + ">(" + accessor + ")");
- b->appendBlock(b2);
+ CppBlockPtr b2 = child_s->getResetSoftBlock("std::get<" + std::to_string(i++) + ">(" + resolved_accessor + ")");
+ block_if_data->appendBlock(b2);
}
- return b;
+ return ResolveMaybeResetSoftBlock(accessor, block_if_data, typenavigator);
}
- CppBlockPtr TupleSerializer::getWriteInitialTypeBlock(const std::string& accessor) const
+ CppBlockPtr TupleSerializer::getWriteTypeBlock(const std::string& accessor) const
{
CppBlockPtr b = CppBlockPtr(new CppBlock());
- b->addLine("w.writeStartTuple();");
+ b->addLine("w.writeStartTuple({" + MAYBE_TO_STRING(typenavigator->getMaybe()) + "}); // of " + accessor);
std::string escaped_accessor = EscapeAccessor(accessor);
unsigned int i = 0;
@@ -77,85 +68,55 @@ namespace armarx::aron::cppcodegenerator::serializer
std::string accessor_iterator = escaped_accessor + TUPLE_ITERATOR_ACCESSOR + std::to_string(i++);
auto type_s = FromAronTypeNaviagtorPtr(type);
b->addLine("w.writeKey(\"" + std::to_string(i) + "\");");
- CppBlockPtr b2 = type_s->getWriteInitialTypeBlock(accessor_iterator);
+ CppBlockPtr b2 = type_s->getWriteTypeBlock(accessor_iterator);
b->appendBlock(b2);
}
- b->addLine("w.writeEndTuple();");
+ b->addLine("w.writeEndTuple(); // of " + accessor);
return b;
}
- CppBlockPtr TupleSerializer::getWriteCurrentTypeBlock(const std::string& accessor) const
+ CppBlockPtr TupleSerializer::getWriteBlock(const std::string& accessor) const
{
- CppBlockPtr b = CppBlockPtr(new CppBlock());
- b->addLine("w.writeStartTuple();");
-
- std::string escaped_accessor = EscapeAccessor(accessor);
- unsigned int i = 0;
- for (const auto& type : typenavigator->getAcceptedTypes())
- {
- std::string accessor_iterator = escaped_accessor + TUPLE_ITERATOR_ACCESSOR + std::to_string(i++);
- auto type_s = FromAronTypeNaviagtorPtr(type);
- b->addLine("w.writeKey(\"" + std::to_string(i) + "\");");
- CppBlockPtr b2 = type_s->getWriteCurrentTypeBlock(accessor_iterator);
- b->appendBlock(b2);
- }
- b->addLine("w.writeEndTuple();");
- return b;
- }
+ CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
+ block_if_data->addLine("w.writeStartList(); // of " + accessor);
- CppBlockPtr TupleSerializer::getResetBlock(const std::string& accessor) const
- {
- CppBlockPtr b = CppBlockPtr(new CppBlock());
+ std::string resolved_accessor = ResolveMaybeAccessor(accessor, typenavigator);
unsigned int i = 0;
for (const auto& type : typenavigator->getAcceptedTypes())
{
auto type_s = FromAronTypeNaviagtorPtr(type);
- CppBlockPtr b2 = type_s->getResetBlock("std::get<" + std::to_string(i++) + ">(" + accessor + ")");
- b->appendBlock(b2);
+ CppBlockPtr b2 = type_s->getWriteBlock("std::get<" + std::to_string(i++) + ">(" + resolved_accessor + ")");
+ block_if_data->appendBlock(b2);
}
- return b;
- }
-
- CppBlockPtr TupleSerializer::getWriteBlock(const std::string& accessor) const
- {
- CppBlockPtr b = CppBlockPtr(new CppBlock());
- b->addLine("w.writeStartList();");
+ block_if_data->addLine("w.writeEndList(); // of " + accessor);
- unsigned int i = 0;
- for (const auto& type : typenavigator->getAcceptedTypes())
- {
- auto type_s = FromAronTypeNaviagtorPtr(type);
- CppBlockPtr b2 = type_s->getWriteBlock("std::get<" + std::to_string(i++) + ">(" + accessor + ")");
- b->appendBlock(b2);
- }
- b->addLine("w.writeEndList();");
- return b;
+ return ResolveMaybeWriteBlock(accessor, block_if_data, typenavigator);
}
CppBlockPtr TupleSerializer::getReadBlock(const std::string& accessor) const
{
- CppBlockPtr b = CppBlockPtr(new CppBlock());
- b->addLine(accessor + " = {};");
- b->addLine("r.readStartList();");
+ CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
+
+ std::string resolved_accessor = ResolveMaybeAccessor(accessor, typenavigator);
unsigned int i = 0;
for (const auto& type : typenavigator->getAcceptedTypes())
{
auto type_s = FromAronTypeNaviagtorPtr(type);
- CppBlockPtr b2 = type_s->getReadBlock("std::get<" + std::to_string(i++) + ">(" + accessor + ")");
- b->appendBlock(b2);
+ CppBlockPtr b2 = type_s->getReadBlock("std::get<" + std::to_string(i++) + ">(" + resolved_accessor + ")");
+ block_if_data->appendBlock(b2);
}
- b->addLine("r.readEndList();");
- return b;
+ block_if_data->addLine("r.readEndList(); // of " + accessor);
+ return ResolveMaybeReadBlock(accessor, "r.readStartList()", block_if_data, typenavigator);
}
CppBlockPtr TupleSerializer::getEqualsBlock(const std::string& accessor, const std::string& otherInstanceAccessor) const
{
- CppBlockPtr b = CppBlockPtr(new CppBlock());
- b->addLine("if (not (" + accessor + " == " + otherInstanceAccessor + "))");
- b->addLine("\t return false;");
- return b;
+ CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
+ block_if_data->addLine("if (not (" + accessor + " == " + otherInstanceAccessor + "))");
+ block_if_data->addLine("\t return false;");
+ return ResolveMaybeEqualsBlock(accessor, otherInstanceAccessor, block_if_data, typenavigator);
}
}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/container/Tuple.h b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/container/Tuple.h
index 52bd376ec1e2485800500e486313f408eadaa7a9..d8ec0bf428b98cc8290600b7556fc503d4acf5ee 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/container/Tuple.h
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/container/Tuple.h
@@ -28,19 +28,19 @@
#include <map>
// Base Class
-#include <RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/Serializer.h>
+#include "../detail/ContainerSerializerBase.h"
// ArmarX
-#include <RobotAPI/libraries/aron/core/navigator/type/container/Tuple.h>
+#include "../../../../../navigator/type/container/Tuple.h"
-namespace armarx::aron::cppcodegenerator::serializer
+namespace armarx::aron::cppserializer::serializer
{
class TupleSerializer;
typedef std::shared_ptr<TupleSerializer> TupleSerializerPtr;
class TupleSerializer :
- virtual public Serializer
+ public detail::ContainerSerializerBase<typenavigator::TupleNavigator, TupleSerializer>
{
public:
using PointerType = TupleSerializerPtr;
@@ -50,13 +50,9 @@ namespace armarx::aron::cppcodegenerator::serializer
TupleSerializer(const typenavigator::TupleNavigatorPtr& e);
// virtual implementations
- virtual std::vector<CppFieldPtr> getPublicVariableDeclarations(const std::string&) const override;
- virtual std::vector<std::pair<std::string, std::string>> getCtorInitializers(const std::string&) const override;
- virtual CppBlockPtr getCtorBlock(const std::string&) const override;
- virtual CppBlockPtr getWriteInitialTypeBlock(const std::string&) const override;
- virtual CppBlockPtr getInitializeBlock(const std::string&) const override;
- virtual CppBlockPtr getWriteCurrentTypeBlock(const std::string&) const override;
- virtual CppBlockPtr getResetBlock(const std::string&) const override;
+ virtual CppBlockPtr getWriteTypeBlock(const std::string&) const override;
+ virtual CppBlockPtr getResetHardBlock(const std::string&) const override;
+ virtual CppBlockPtr getResetSoftBlock(const std::string&) const override;
virtual CppBlockPtr getWriteBlock(const std::string&) const override;
virtual CppBlockPtr getReadBlock(const std::string&) const override;
virtual CppBlockPtr getEqualsBlock(const std::string&, const std::string&) const override;
@@ -64,7 +60,5 @@ namespace armarx::aron::cppcodegenerator::serializer
private:
// members
static constexpr const char* TUPLE_ITERATOR_ACCESSOR = "_iterator";
-
- typenavigator::TupleNavigatorPtr typenavigator;
};
}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/detail/ContainerSerializerBase.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/detail/ContainerSerializerBase.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..d5dc077b9751440762c98533d41498cfe6f8c9c9
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/detail/ContainerSerializerBase.cpp
@@ -0,0 +1,25 @@
+/*
+ * 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 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
+ */
+
+// Header
+#include "ContainerSerializerBase.h"
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/detail/ContainerSerializerBase.h b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/detail/ContainerSerializerBase.h
new file mode 100644
index 0000000000000000000000000000000000000000..d2ca8f77efce232d34c15c7d9f7cd24513a46f53
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/detail/ContainerSerializerBase.h
@@ -0,0 +1,48 @@
+/*
+ * 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 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
+ */
+
+#pragma once
+
+// STD/STL
+#include <memory>
+#include <string>
+#include <unordered_map>
+
+// Base class
+#include "SerializerBase.h"
+
+// ArmarX
+
+namespace armarx::aron::cppserializer::detail
+{
+ template<typename TypenavigatorT, typename DerivedT>
+ class ContainerSerializerBase :
+ public SerializerBase<TypenavigatorT, DerivedT>
+ {
+ public:
+ ContainerSerializerBase(const std::string& cppName, const std::string& aronDataTypename, const std::string& aronTypeTypename, const typename TypenavigatorT::PointerType& t) :
+ SerializerBase<TypenavigatorT, DerivedT>(cppName, aronDataTypename, aronTypeTypename, t)
+ {
+ }
+ };
+}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/detail/NDArraySerializerBase.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/detail/NDArraySerializerBase.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..98ecea508e8055664d1a300e29e3aac42cea1010
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/detail/NDArraySerializerBase.cpp
@@ -0,0 +1,25 @@
+/*
+ * 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 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
+ */
+
+// Header
+#include "NDArraySerializerBase.h"
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/detail/NDArraySerializerBase.h b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/detail/NDArraySerializerBase.h
new file mode 100644
index 0000000000000000000000000000000000000000..67b6c0435617cc7b4fcbebfe6bcadd604850794a
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/detail/NDArraySerializerBase.h
@@ -0,0 +1,47 @@
+/*
+ * 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 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
+ */
+
+#pragma once
+
+// STD/STL
+#include <memory>
+#include <string>
+#include <unordered_map>
+
+// Base class
+#include "SerializerBase.h"
+
+// ArmarX
+
+namespace armarx::aron::cppserializer::detail
+{
+ template<typename TypenavigatorT, typename DerivedT>
+ class NDArraySerializerBase :
+ public SerializerBase<TypenavigatorT, DerivedT>
+ {
+ public:
+ NDArraySerializerBase(const std::string& cppName, const std::string& aronDataTypename, const std::string& aronTypeTypename, const typename TypenavigatorT::PointerType& t) :
+ SerializerBase<TypenavigatorT, DerivedT>(cppName, aronDataTypename, aronTypeTypename, t)
+ {}
+ };
+}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/detail/PrimitiveSerializerBase.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/detail/PrimitiveSerializerBase.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..1b8d9aeb079543fea479fb67e46a111910b0c4d3
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/detail/PrimitiveSerializerBase.cpp
@@ -0,0 +1,25 @@
+/*
+ * 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 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
+ */
+
+// Header
+#include "PrimitiveSerializerBase.h"
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/detail/PrimitiveSerializerBase.h b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/detail/PrimitiveSerializerBase.h
new file mode 100644
index 0000000000000000000000000000000000000000..171f4defb27cff9a51217805f71d76fb58ca0da5
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/detail/PrimitiveSerializerBase.h
@@ -0,0 +1,86 @@
+/*
+ * 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 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
+ */
+
+#pragma once
+
+// STD/STL
+#include <memory>
+#include <string>
+#include <unordered_map>
+
+// Base class
+#include "SerializerBase.h"
+
+// ArmarX
+
+namespace armarx::aron::cppserializer::detail
+{
+ template<typename TypenavigatorT, typename DerivedT>
+ class PrimitiveSerializerBase :
+ public SerializerBase<TypenavigatorT, DerivedT>
+ {
+ public:
+ PrimitiveSerializerBase(const std::string& cppName, const std::string& aronDataTypename, const std::string& aronTypeTypename, const typename TypenavigatorT::PointerType& t) :
+ SerializerBase<TypenavigatorT, DerivedT>(cppName, aronDataTypename, aronTypeTypename, t)
+ {}
+
+ virtual CppBlockPtr getResetHardBlock(const std::string& accessor) const
+ {
+ CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
+ block_if_data->addLine(accessor + " = " + this->getFullTypenameGenerator() + "();");
+ return this->ResolveMaybeResetHardBlock(accessor, block_if_data, this->typenavigator);
+ }
+
+ virtual CppBlockPtr getResetSoftBlock(const std::string& accessor) const
+ {
+ CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
+ block_if_data->addLine(accessor + " = " + this->getFullCppTypename() + "();");
+ return this->ResolveMaybeResetSoftBlock(accessor, block_if_data, this->typenavigator);
+ }
+
+ virtual CppBlockPtr getWriteBlock(const std::string& accessor) const override
+ {
+ CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
+ std::string resolved_accessor = this->ResolveMaybeAccessor(accessor, this->typenavigator);
+ block_if_data->addLine("w.writePrimitive(" + resolved_accessor + "); // of " + accessor);
+
+ return this->ResolveMaybeWriteBlock(accessor, block_if_data, this->typenavigator);
+ }
+
+ virtual CppBlockPtr getReadBlock(const std::string& accessor) const override
+ {
+ CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
+ std::string resolved_accessor = this->ResolveMaybeAccessor(accessor, this->typenavigator);
+ block_if_data->addLine("r.readEndPrimitive(" + resolved_accessor + "); // of " + accessor);
+ return this->ResolveMaybeReadBlock(accessor, "r.readStartPrimitive()", block_if_data, this->typenavigator);
+ }
+
+ virtual CppBlockPtr getEqualsBlock(const std::string& accessor, const std::string& otherInstanceAccessor) const override
+ {
+ CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
+ block_if_data->addLine("if (not (" + accessor + " == " + otherInstanceAccessor + "))");
+ block_if_data->addLine("\t return false;");
+ return this->ResolveMaybeEqualsBlock(accessor, otherInstanceAccessor, block_if_data, this->typenavigator);
+ }
+ };
+}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/detail/SerializerBase.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/detail/SerializerBase.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..9b592c636489e9b5031c5d54cf3bc517f8f0bdde
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/detail/SerializerBase.cpp
@@ -0,0 +1,25 @@
+/*
+ * 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 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
+ */
+
+// Header
+#include "SerializerBase.h"
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/detail/SerializerBase.h b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/detail/SerializerBase.h
new file mode 100644
index 0000000000000000000000000000000000000000..4b2dd5f8918bf0859b9e577d58dcd357a2fe7e37
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/detail/SerializerBase.h
@@ -0,0 +1,144 @@
+/*
+ * 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 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
+ */
+
+#pragma once
+
+// STD/STL
+#include <memory>
+#include <string>
+#include <unordered_map>
+
+// Base class
+#include "../Serializer.h"
+
+// ArmarX
+
+namespace armarx::aron::cppserializer::detail
+{
+ template<typename TypenavigatorT, typename DerivedT>
+ class SerializerBase :
+ public cppserializer::Serializer
+ {
+ public:
+ using PointerType = std::shared_ptr<DerivedT>;
+ using TypenavigatorType = TypenavigatorT;
+
+ public:
+ SerializerBase(const std::string& cppName, const std::string& aronDataTypename, const std::string& aronTypeTypename, const typename TypenavigatorType::PointerType& t) :
+ Serializer(cppName, aronDataTypename, aronTypeTypename),
+ typenavigator(t)
+ {
+ ARMARX_CHECK_NOT_NULL(typenavigator);
+ }
+
+ virtual std::string getFullCppTypename() const override
+ {
+ switch (typenavigator->getMaybe())
+ {
+ case type::Maybe::eNone:
+ return getCoreCppTypename();
+ case type::Maybe::eOptional:
+ return "std::optional<" + getCoreCppTypename() + ">";
+ case type::Maybe::eRawPointer:
+ return getCoreCppTypename() + "*";
+ case type::Maybe::eSharedPointer:
+ return "std::shared_ptr<" + getCoreCppTypename() + ">";
+ case type::Maybe::eUniquePointer:
+ return "std::unique_ptr<" + getCoreCppTypename() + ">";
+ default:
+ throw error::MaybeNotValidException("SerializerBase", "getFullCppTypename", "Received unknown maybe enum", typenavigator->getMaybe(), typenavigator->getPath());
+ }
+ }
+
+ virtual std::string getFullTypenameGenerator() const override
+ {
+ switch (typenavigator->getMaybe())
+ {
+ case type::Maybe::eNone:
+ return getCoreCppTypename();
+ case type::Maybe::eOptional:
+ return "std::make_optional<" + getCoreCppTypename() + ">";
+ case type::Maybe::eRawPointer:
+ return "new " + getCoreCppTypename();
+ case type::Maybe::eSharedPointer:
+ return "std::make_shared<" + getCoreCppTypename() + ">";
+ case type::Maybe::eUniquePointer:
+ return "std::make_unique<" + getCoreCppTypename() + ">";
+ default:
+ throw error::MaybeNotValidException("SerializerBase", "getFullTypenameGenerator", "Received unknown maybe enum", typenavigator->getMaybe(), typenavigator->getPath());
+ }
+ }
+
+ virtual std::vector<CppFieldPtr> getPublicVariableDeclarations(const std::string& name) const override
+ {
+ CppFieldPtr field = CppFieldPtr(new CppField(this->getFullCppTypename(), name));
+ return {field};
+ }
+
+ virtual std::vector<std::pair<std::string, std::string>> getCtorInitializers(const std::string&) const override
+ {
+ return {};
+ }
+
+ virtual CppBlockPtr getCtorBlock(const std::string&) const override
+ {
+ CppBlockPtr b = CppBlockPtr(new CppBlock());
+ return b;
+ }
+
+
+ protected:
+ std::string nextEl() const
+ {
+ switch (typenavigator->getMaybe())
+ {
+ case type::Maybe::eNone:
+ return ".";
+ case type::Maybe::eOptional:
+ case type::Maybe::eRawPointer:
+ case type::Maybe::eSharedPointer:
+ case type::Maybe::eUniquePointer:
+ return "->";
+ default:
+ throw error::MaybeNotValidException("SerializerBase", "nextEl", "Received unknown maybe enum", typenavigator->getMaybe(), typenavigator->getPath());
+ }
+ }
+
+ std::string toPointerAccessor(const std::string& accessor) const
+ {
+ switch (typenavigator->getMaybe())
+ {
+ case type::Maybe::eRawPointer:
+ return accessor;
+ case type::Maybe::eSharedPointer:
+ case type::Maybe::eUniquePointer:
+ return accessor + ".get()";
+ default:
+ throw error::MaybeNotValidException("SerializerBase", "toPointerAccessor", "Received invalid maybe enum (not a pointer?)", typenavigator->getMaybe(), typenavigator->getPath());
+ }
+ }
+
+ protected:
+ typename TypenavigatorT::PointerType typenavigator;
+ };
+}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/enum/IntEnum.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/enum/IntEnum.cpp
index a7ed2ec3339fcbd368b81b7afa3d7f0a873a8735..e412f52b59bb3b53397439c7068338131493d60d 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/enum/IntEnum.cpp
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/enum/IntEnum.cpp
@@ -24,71 +24,58 @@
// Header
#include "IntEnum.h"
-namespace armarx::aron::cppcodegenerator::serializer
+namespace armarx::aron::cppserializer::serializer
{
// constructors
- IntEnumSerializer::IntEnumSerializer(const typenavigator::IntEnumNavigatorPtr& n) :
- Serializer(n->getEnumName(), simox::meta::get_type_name(typeid(data::AronNDArray)), simox::meta::get_type_name(typeid(type::AronIntEnum))),
- navigator(n)
+ IntEnumSerializer::IntEnumSerializer(const typenavigator::IntEnumNavigatorPtr& e) :
+ detail::SerializerBase<typenavigator::IntEnumNavigator, IntEnumSerializer>(e->getEnumName(),
+ simox::meta::get_type_name(typeid(data::AronNDArray)),
+ simox::meta::get_type_name(typeid(type::AronIntEnum)), e)
{
}
- std::vector<CppFieldPtr> IntEnumSerializer::getPublicVariableDeclarations(const std::string& name) const
+ CppBlockPtr IntEnumSerializer::getResetSoftBlock(const std::string& accessor) const
{
- CppFieldPtr field = CppFieldPtr(new CppField(getFullCppTypename(), name));
- return {field};
- }
-
- std::vector<std::pair<std::string, std::string>> IntEnumSerializer::getCtorInitializers(const std::string&) const
- {
- return {};
- }
-
- CppBlockPtr IntEnumSerializer::getCtorBlock(const std::string&) const
- {
- CppBlockPtr b = CppBlockPtr(new CppBlock());
- return b;
- }
-
- CppBlockPtr IntEnumSerializer::getResetBlock(const std::string& accessor) const
- {
- CppBlockPtr b = CppBlockPtr(new CppBlock());
- b->addLine(accessor + ".reset();");
- return b;
- }
+ CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
+ std::string resolved_accessor = ResolveMaybeAccessor(accessor, typenavigator);
- CppBlockPtr IntEnumSerializer::getWriteInitialTypeBlock(const std::string& accessor) const
- {
- CppBlockPtr b = CppBlockPtr(new CppBlock());
- b->addLine(accessor + "::writeInitialType(w);");
- return b;
+ block_if_data->addLine(accessor + nextEl() + "resetSoft();");
+ return ResolveMaybeResetSoftBlock(accessor, block_if_data, typenavigator);
}
- CppBlockPtr IntEnumSerializer::getWriteCurrentTypeBlock(const std::string& accessor) const
+ CppBlockPtr IntEnumSerializer::getWriteTypeBlock(const std::string& accessor) const
{
CppBlockPtr b = CppBlockPtr(new CppBlock());
- b->addLine(accessor + ".writeCurrentType(w);");
+ b->addLine(accessor + "::writeType(w, " + MAYBE_TO_STRING(typenavigator->getMaybe()) + ");");
return b;
}
- CppBlockPtr IntEnumSerializer::getInitializeBlock(const std::string& accessor) const
+ CppBlockPtr IntEnumSerializer::getResetHardBlock(const std::string& accessor) const
{
CppBlockPtr b = CppBlockPtr(new CppBlock());
- b->addLine(accessor + ".initialize();");
+ if (typenavigator->getMaybe() != type::Maybe::eNone)
+ {
+ b->addLine(accessor + " = {};");
+ }
+ else
+ {
+ b->addLine(accessor + nextEl() + "resetHard();");
+ }
return b;
}
CppBlockPtr IntEnumSerializer::getWriteBlock(const std::string& accessor) const
{
- CppBlockPtr b = CppBlockPtr(new CppBlock());
- b->addLine(accessor + ".write(w);");
- return b;
+ CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
+ block_if_data->addLine(accessor + nextEl() + "write(w);");
+
+ return ResolveMaybeWriteBlock(accessor, block_if_data, typenavigator);
}
CppBlockPtr IntEnumSerializer::getReadBlock(const std::string& accessor) const
{
CppBlockPtr b = CppBlockPtr(new CppBlock());
- b->addLine(accessor + ".read(r);");
+ b->addLine(accessor + nextEl() + "read(r);");
return b;
}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/enum/IntEnum.h b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/enum/IntEnum.h
index abb82b441cb59b139ec531110523a7844216273d..af290bfe03f46cd5faa5cad4e218a6e3f5bd9e65 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/enum/IntEnum.h
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/enum/IntEnum.h
@@ -28,19 +28,19 @@
#include <map>
// Base Class
-#include <RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/Serializer.h>
+#include "../detail/SerializerBase.h"
// ArmarX
-#include <RobotAPI/libraries/aron/core/navigator/type/enum/IntEnum.h>
+#include "../../../../../navigator/type/enum/IntEnum.h"
-namespace armarx::aron::cppcodegenerator::serializer
-{
+namespace armarx::aron::cppserializer::serializer
+{
class IntEnumSerializer;
typedef std::shared_ptr<IntEnumSerializer> IntEnumSerializerPtr;
class IntEnumSerializer :
- virtual public Serializer
+ virtual public detail::SerializerBase<typenavigator::IntEnumNavigator, IntEnumSerializer>
{
public:
using PointerType = IntEnumSerializerPtr;
@@ -50,13 +50,9 @@ namespace armarx::aron::cppcodegenerator::serializer
IntEnumSerializer(const typenavigator::IntEnumNavigatorPtr&);
// virtual implementations
- virtual std::vector<CppFieldPtr> getPublicVariableDeclarations(const std::string&) const override;
- virtual std::vector<std::pair<std::string, std::string>> getCtorInitializers(const std::string&) const override;
- virtual CppBlockPtr getCtorBlock(const std::string&) const override;
- virtual CppBlockPtr getWriteInitialTypeBlock(const std::string&) const override;
- virtual CppBlockPtr getWriteCurrentTypeBlock(const std::string&) const override;
- virtual CppBlockPtr getInitializeBlock(const std::string&) const override;
- virtual CppBlockPtr getResetBlock(const std::string&) const override;
+ virtual CppBlockPtr getResetHardBlock(const std::string& accessor) const override;
+ virtual CppBlockPtr getResetSoftBlock(const std::string& accessor) const override;
+ virtual CppBlockPtr getWriteTypeBlock(const std::string&) const override;
virtual CppBlockPtr getWriteBlock(const std::string&) const override;
virtual CppBlockPtr getReadBlock(const std::string&) const override;
virtual CppBlockPtr getEqualsBlock(const std::string&, const std::string&) const override;
@@ -64,7 +60,5 @@ namespace armarx::aron::cppcodegenerator::serializer
private:
// Members
static const std::map<std::string, std::pair<std::string, int>> ACCEPTED_TYPES;
-
- typenavigator::IntEnumNavigatorPtr navigator;
};
}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/EigenMatrix.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/EigenMatrix.cpp
index 9bb5203eb76777c0c09a08667689c108ddb25c85..790d6157d4f28a33e983d0394a09448d818c6158 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/EigenMatrix.cpp
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/EigenMatrix.cpp
@@ -24,7 +24,7 @@
// Header
#include "EigenMatrix.h"
-namespace armarx::aron::cppcodegenerator::serializer
+namespace armarx::aron::cppserializer::serializer
{
const std::map<std::string, std::pair<std::string, int>> EigenMatrixSerializer::ACCEPTED_TYPES =
@@ -40,76 +40,54 @@ namespace armarx::aron::cppcodegenerator::serializer
// constructors
EigenMatrixSerializer::EigenMatrixSerializer(const typenavigator::EigenMatrixNavigatorPtr& n) :
- Serializer("Eigen::Matrix<" + ACCEPTED_TYPES.at(n->getTypename()).first + ", " + std::to_string(n->getRows()) + ", " + std::to_string(n->getCols()) + ">", simox::meta::get_type_name(typeid(data::AronNDArray)), simox::meta::get_type_name(typeid(type::AronEigenMatrix))),
- typenavigator(n)
+ detail::NDArraySerializerBase<typenavigator::EigenMatrixNavigator, EigenMatrixSerializer>("Eigen::Matrix<" + ACCEPTED_TYPES.at(n->getTypename()).first + ", " + std::to_string(n->getRows()) + ", " + std::to_string(n->getCols()) + ">",
+ simox::meta::get_type_name(typeid(data::AronNDArray)),
+ simox::meta::get_type_name(typeid(type::AronEigenMatrix)), n)
{
+ ARMARX_CHECK_NOT_NULL(typenavigator);
}
- std::vector<CppFieldPtr> EigenMatrixSerializer::getPublicVariableDeclarations(const std::string& name) const
+ CppBlockPtr EigenMatrixSerializer::getResetHardBlock(const std::string& accessor) const
{
- CppFieldPtr field = CppFieldPtr(new CppField(getFullCppTypename(), name));
- return {field};
+ CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
+ block_if_data->addLine(accessor + " = " + this->getFullTypenameGenerator() + "();");
+ return ResolveMaybeResetHardBlock(accessor, block_if_data, typenavigator);
}
- std::vector<std::pair<std::string, std::string>> EigenMatrixSerializer::getCtorInitializers(const std::string&) const
+ CppBlockPtr EigenMatrixSerializer::getResetSoftBlock(const std::string& accessor) const
{
- return {};
+ CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
+ block_if_data->addLine(accessor + " = " + this->getFullCppTypename() + "();");
+ return ResolveMaybeResetSoftBlock(accessor, block_if_data, typenavigator);
}
- CppBlockPtr EigenMatrixSerializer::getCtorBlock(const std::string&) const
+ CppBlockPtr EigenMatrixSerializer::getWriteTypeBlock(const std::string& accessor) const
{
CppBlockPtr b = CppBlockPtr(new CppBlock());
- return b;
- }
-
- CppBlockPtr EigenMatrixSerializer::getResetBlock(const std::string& accessor) const
- {
- CppBlockPtr b = CppBlockPtr(new CppBlock());
- b->addLine(accessor + " = " + getFullCppTypename() + "();");
- return b;
- }
-
- CppBlockPtr EigenMatrixSerializer::getWriteInitialTypeBlock(const std::string&) const
- {
- CppBlockPtr b = CppBlockPtr(new CppBlock());
- b->addLine("w.writeEigenMatrix({" + simox::alg::to_string<int>(typenavigator->getDimensions(), ", ") + "}, \"" + typenavigator->getTypename() + "\");");
- return b;
- }
-
- CppBlockPtr EigenMatrixSerializer::getWriteCurrentTypeBlock(const std::string& accessor) const
- {
- CppBlockPtr b = CppBlockPtr(new CppBlock());
- b->addLine("w.writeEigenMatrix({" + accessor + ".rows(), " + accessor + ".cols()}, \"" + typenavigator->getTypename() + "\");");
- return b;
- }
-
- CppBlockPtr EigenMatrixSerializer::getInitializeBlock(const std::string& accessor) const
- {
- CppBlockPtr b = CppBlockPtr(new CppBlock());
- b->addLine(accessor + " = " + getFullCppTypename() + "();");
+ b->addLine("w.writeEigenMatrix({(int) " + std::to_string(typenavigator->getRows()) + ", (int) " + std::to_string(typenavigator->getCols()) + ", \"" + typenavigator->getTypename() + "\", " + MAYBE_TO_STRING(typenavigator->getMaybe()) + "}); // of " + accessor);
return b;
}
CppBlockPtr EigenMatrixSerializer::getWriteBlock(const std::string& accessor) const
{
- CppBlockPtr b = CppBlockPtr(new CppBlock());
- b->addLine("w.writeNDArray({" + accessor + ".rows(), " + accessor + ".cols(), " + std::to_string(ACCEPTED_TYPES.at(typenavigator->getTypename()).second) + "}, \"" + typenavigator->getTypename() + "\", reinterpret_cast<const unsigned char*>(" + accessor + ".data()));");
- return b;
+ CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
+ block_if_data->addLine("w.writeNDArray({(int) " + accessor + nextEl() + "rows(), (int) " + accessor + nextEl() + "cols(), " + std::to_string(ACCEPTED_TYPES.at(typenavigator->getTypename()).second) + "}, \"" + typenavigator->getTypename() + "\", reinterpret_cast<const unsigned char*>(" + accessor + nextEl() + "data())); // of " + accessor);
+
+ return ResolveMaybeWriteBlock(accessor, block_if_data, typenavigator);
}
CppBlockPtr EigenMatrixSerializer::getReadBlock(const std::string& accessor) const
{
- CppBlockPtr b = CppBlockPtr(new CppBlock());
- b->addLine("r.readStartNDArray(); // We do not need the dims and type since a EigenMat can not change (templated)");
- b->addLine("r.readEndNDArray(reinterpret_cast<unsigned char*>(" + accessor + ".data()));");
- return b;
+ CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
+ block_if_data->addLine("r.readEndNDArray(reinterpret_cast<unsigned char*>(" + accessor + nextEl() + "data())); // of " + accessor);
+ return ResolveMaybeReadBlock(accessor, "r.readStartNDArray()", block_if_data, typenavigator);
}
CppBlockPtr EigenMatrixSerializer::getEqualsBlock(const std::string& accessor, const std::string& otherInstanceAccessor) const
{
- CppBlockPtr b = CppBlockPtr(new CppBlock());
- b->addLine("if (not (" + accessor + ".isApprox(" + otherInstanceAccessor + ")))");
- b->addLine("\t return false;");
- return b;
+ CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
+ block_if_data->addLine("if (not (" + accessor + nextEl() + "isApprox(" + ResolveMaybeAccessor(otherInstanceAccessor, typenavigator) + ")))");
+ block_if_data->addLine("\t return false;");
+ return ResolveMaybeEqualsBlock(accessor, otherInstanceAccessor, block_if_data, typenavigator);
}
}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/EigenMatrix.h b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/EigenMatrix.h
index bbdb75fdf7bb55b287b6853457053da6aa32f476..9c2ecd36e588238923f539ebe364af57c3681f42 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/EigenMatrix.h
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/EigenMatrix.h
@@ -28,35 +28,27 @@
#include <map>
// Base Class
-#include <RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/Serializer.h>
+#include "../detail/NDArraySerializerBase.h"
// ArmarX
-#include <RobotAPI/libraries/aron/core/navigator/type/ndarray/EigenMatrix.h>
+#include "../../../../../navigator/type/ndarray/EigenMatrix.h"
-namespace armarx::aron::cppcodegenerator::serializer
+namespace armarx::aron::cppserializer::serializer
{
-
class EigenMatrixSerializer;
typedef std::shared_ptr<EigenMatrixSerializer> EigenMatrixSerializerPtr;
class EigenMatrixSerializer :
- virtual public Serializer
+ virtual public detail::NDArraySerializerBase<typenavigator::EigenMatrixNavigator, EigenMatrixSerializer>
{
- public:
- using PointerType = EigenMatrixSerializerPtr;
-
public:
// constructors
EigenMatrixSerializer(const typenavigator::EigenMatrixNavigatorPtr&);
// virtual implementations
- virtual std::vector<CppFieldPtr> getPublicVariableDeclarations(const std::string&) const override;
- virtual std::vector<std::pair<std::string, std::string>> getCtorInitializers(const std::string&) const override;
- virtual CppBlockPtr getCtorBlock(const std::string&) const override;
- virtual CppBlockPtr getWriteInitialTypeBlock(const std::string&) const override;
- virtual CppBlockPtr getWriteCurrentTypeBlock(const std::string&) const override;
- virtual CppBlockPtr getInitializeBlock(const std::string&) const override;
- virtual CppBlockPtr getResetBlock(const std::string&) const override;
+ virtual CppBlockPtr getResetHardBlock(const std::string& accessor) const override;
+ virtual CppBlockPtr getResetSoftBlock(const std::string& accessor) const override;
+ virtual CppBlockPtr getWriteTypeBlock(const std::string&) const override;
virtual CppBlockPtr getWriteBlock(const std::string&) const override;
virtual CppBlockPtr getReadBlock(const std::string&) const override;
virtual CppBlockPtr getEqualsBlock(const std::string&, const std::string&) const override;
@@ -64,7 +56,5 @@ namespace armarx::aron::cppcodegenerator::serializer
private:
// Members
static const std::map<std::string, std::pair<std::string, int>> ACCEPTED_TYPES;
-
- typenavigator::EigenMatrixNavigatorPtr typenavigator;
};
}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/EigenQuaternion.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/EigenQuaternion.cpp
index 825765164e23e2358dc465171d667f6dbe117020..8e697ab6d64c9e425310b6927842d756ff96cae3 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/EigenQuaternion.cpp
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/EigenQuaternion.cpp
@@ -24,7 +24,7 @@
// Header
#include "EigenQuaternion.h"
-namespace armarx::aron::cppcodegenerator::serializer
+namespace armarx::aron::cppserializer::serializer
{
const std::map<std::string, std::pair<std::string, int>> EigenQuaternionSerializer::ACCEPTED_TYPES =
@@ -35,76 +35,54 @@ namespace armarx::aron::cppcodegenerator::serializer
// constructors
EigenQuaternionSerializer::EigenQuaternionSerializer(const typenavigator::EigenQuaternionNavigatorPtr& n) :
- Serializer("Eigen::Quaternion<" + ACCEPTED_TYPES.at(n->getTypename()).first + ">", simox::meta::get_type_name(typeid(data::AronNDArray)), simox::meta::get_type_name(typeid(type::AronEigenQuaternion))),
- typenavigator(n)
+ detail::NDArraySerializerBase<typenavigator::EigenQuaternionNavigator, EigenQuaternionSerializer>("Eigen::Quaternion<" + ACCEPTED_TYPES.at(n->getTypename()).first + ">",
+ simox::meta::get_type_name(typeid(data::AronNDArray)),
+ simox::meta::get_type_name(typeid(type::AronEigenQuaternion)), n)
{
+ ARMARX_CHECK_NOT_NULL(typenavigator);
}
- std::vector<CppFieldPtr> EigenQuaternionSerializer::getPublicVariableDeclarations(const std::string& name) const
+ CppBlockPtr EigenQuaternionSerializer::getResetHardBlock(const std::string& accessor) const
{
- CppFieldPtr field = CppFieldPtr(new CppField(getFullCppTypename(), name));
- return {field};
+ CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
+ block_if_data->addLine(accessor + " = " + this->getFullTypenameGenerator() + "();");
+ return ResolveMaybeResetHardBlock(accessor, block_if_data, typenavigator);
}
- std::vector<std::pair<std::string, std::string>> EigenQuaternionSerializer::getCtorInitializers(const std::string&) const
+ CppBlockPtr EigenQuaternionSerializer::getResetSoftBlock(const std::string& accessor) const
{
- return {};
+ CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
+ block_if_data->addLine(accessor + " = " + this->getFullCppTypename() + "();");
+ return ResolveMaybeResetSoftBlock(accessor, block_if_data, typenavigator);
}
- CppBlockPtr EigenQuaternionSerializer::getCtorBlock(const std::string&) const
+ CppBlockPtr EigenQuaternionSerializer::getWriteTypeBlock(const std::string& accessor) const
{
CppBlockPtr b = CppBlockPtr(new CppBlock());
- return b;
- }
-
- CppBlockPtr EigenQuaternionSerializer::getResetBlock(const std::string& accessor) const
- {
- CppBlockPtr b = CppBlockPtr(new CppBlock());
- b->addLine(accessor + " = " + getFullCppTypename() + "();");
- return b;
- }
-
- CppBlockPtr EigenQuaternionSerializer::getWriteInitialTypeBlock(const std::string&) const
- {
- CppBlockPtr b = CppBlockPtr(new CppBlock());
- b->addLine("w.writeEigenQuaternion(\"" + typenavigator->getTypename() + "\");");
- return b;
- }
-
- CppBlockPtr EigenQuaternionSerializer::getWriteCurrentTypeBlock(const std::string&) const
- {
- CppBlockPtr b = CppBlockPtr(new CppBlock());
- b->addLine("w.writeEigenQuaternion(\"" + typenavigator->getTypename() + "\");");
- return b;
- }
-
- CppBlockPtr EigenQuaternionSerializer::getInitializeBlock(const std::string& accessor) const
- {
- CppBlockPtr b = CppBlockPtr(new CppBlock());
- b->addLine(accessor + " = " + getFullCppTypename() + "();");
+ b->addLine("w.writeEigenQuaternion({\"" + typenavigator->getTypename() + "\", " + MAYBE_TO_STRING(typenavigator->getMaybe()) + "}); // of " + accessor);
return b;
}
CppBlockPtr EigenQuaternionSerializer::getWriteBlock(const std::string& accessor) const
{
- CppBlockPtr b = CppBlockPtr(new CppBlock());
- b->addLine("w.writeNDArray({" + simox::alg::to_string(typenavigator->getDimensions(), ", ") + ", " + std::to_string(ACCEPTED_TYPES.at(typenavigator->getTypename()).second) + "}, \"" + typenavigator->getTypename() + "\", reinterpret_cast<const unsigned char*>(" + accessor + ".coeffs().data()));");
- return b;
+ CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
+ block_if_data->addLine("w.writeNDArray({" + simox::alg::to_string(typenavigator->ACCEPTED_DIMENSION, ", ") + ", " + std::to_string(ACCEPTED_TYPES.at(typenavigator->getTypename()).second) + "}, \"" + typenavigator->getTypename() + "\", reinterpret_cast<const unsigned char*>(" + accessor + nextEl() + "coeffs().data())); // of " + accessor);
+
+ return ResolveMaybeWriteBlock(accessor, block_if_data, typenavigator);
}
CppBlockPtr EigenQuaternionSerializer::getReadBlock(const std::string& accessor) const
{
- CppBlockPtr b = CppBlockPtr(new CppBlock());
- b->addLine("r.readStartNDArray(); // We do not need the dims and type since a EigenMat can not change (templated)");
- b->addLine("r.readEndNDArray(reinterpret_cast<unsigned char*>(" + accessor + ".coeffs().data()));");
- return b;
+ CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
+ block_if_data->addLine("r.readEndNDArray(reinterpret_cast<unsigned char*>(" + accessor + nextEl() + "coeffs().data())); // of " + accessor);
+ return ResolveMaybeReadBlock(accessor, "r.readStartNDArray()", block_if_data, typenavigator);
}
CppBlockPtr EigenQuaternionSerializer::getEqualsBlock(const std::string& accessor, const std::string& otherInstanceAccessor) const
{
- CppBlockPtr b = CppBlockPtr(new CppBlock());
- b->addLine("if (not (" + accessor + ".isApprox(" + otherInstanceAccessor + ")))");
- b->addLine("\t return false;");
- return b;
+ CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
+ block_if_data->addLine("if (not (" + accessor + nextEl() + "isApprox(" + otherInstanceAccessor + ")))");
+ block_if_data->addLine("\t return false;");
+ return ResolveMaybeEqualsBlock(accessor, otherInstanceAccessor, block_if_data, typenavigator);
}
}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/EigenQuaternion.h b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/EigenQuaternion.h
index 9f306d7dd6604e43ba4e86b4e112c225c911e504..3d756220f32d541d7f01fbadc11d597028322e3f 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/EigenQuaternion.h
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/EigenQuaternion.h
@@ -28,19 +28,19 @@
#include <map>
// Base Class
-#include <RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/Serializer.h>
+#include "../detail/NDArraySerializerBase.h"
// ArmarX
-#include <RobotAPI/libraries/aron/core/navigator/type/ndarray/EigenQuaternion.h>
+#include "../../../../../navigator/type/ndarray/EigenQuaternion.h"
-namespace armarx::aron::cppcodegenerator::serializer
+namespace armarx::aron::cppserializer::serializer
{
class EigenQuaternionSerializer;
typedef std::shared_ptr<EigenQuaternionSerializer> EigenQuaternionSerializerPtr;
class EigenQuaternionSerializer :
- virtual public Serializer
+ virtual public detail::NDArraySerializerBase<typenavigator::EigenQuaternionNavigator, EigenQuaternionSerializer>
{
public:
using PointerType = EigenQuaternionSerializerPtr;
@@ -50,13 +50,9 @@ namespace armarx::aron::cppcodegenerator::serializer
EigenQuaternionSerializer(const typenavigator::EigenQuaternionNavigatorPtr&);
// virtual implementations
- virtual std::vector<CppFieldPtr> getPublicVariableDeclarations(const std::string&) const override;
- virtual std::vector<std::pair<std::string, std::string>> getCtorInitializers(const std::string&) const override;
- virtual CppBlockPtr getCtorBlock(const std::string&) const override;
- virtual CppBlockPtr getWriteInitialTypeBlock(const std::string&) const override;
- virtual CppBlockPtr getWriteCurrentTypeBlock(const std::string&) const override;
- virtual CppBlockPtr getInitializeBlock(const std::string&) const override;
- virtual CppBlockPtr getResetBlock(const std::string&) const override;
+ virtual CppBlockPtr getResetHardBlock(const std::string& accessor) const override;
+ virtual CppBlockPtr getResetSoftBlock(const std::string& accessor) const override;
+ virtual CppBlockPtr getWriteTypeBlock(const std::string&) const override;
virtual CppBlockPtr getWriteBlock(const std::string&) const override;
virtual CppBlockPtr getReadBlock(const std::string&) const override;
virtual CppBlockPtr getEqualsBlock(const std::string&, const std::string&) const override;
@@ -64,7 +60,5 @@ namespace armarx::aron::cppcodegenerator::serializer
private:
// Members
static const std::map<std::string, std::pair<std::string, int>> ACCEPTED_TYPES;
-
- typenavigator::EigenQuaternionNavigatorPtr typenavigator;
};
}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/IVTCByteImage.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/IVTCByteImage.cpp
index cdb9e79036d16756fb4e2f6f05c97bcf1d7d6b2e..e33934d359e63943bea3a194f0a739ae5adf471b 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/IVTCByteImage.cpp
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/IVTCByteImage.cpp
@@ -24,7 +24,7 @@
// Header
#include "IVTCByteImage.h"
-namespace armarx::aron::cppcodegenerator::serializer
+namespace armarx::aron::cppserializer::serializer
{
const std::map<std::string, std::pair<std::string, int>> IVTCByteImageSerializer::ACCEPTED_TYPES =
@@ -35,87 +35,66 @@ namespace armarx::aron::cppcodegenerator::serializer
};
// constructors
- IVTCByteImageSerializer::IVTCByteImageSerializer(const typenavigator::IVTCByteImageNavigatorPtr& e) :
- Serializer("CByteImage", simox::meta::get_type_name(typeid(data::AronNDArray)), simox::meta::get_type_name(typeid(type::AronIVTCByteImage)), true),
- typenavigator(e)
+ IVTCByteImageSerializer::IVTCByteImageSerializer(const typenavigator::IVTCByteImageNavigatorPtr& n) :
+ detail::NDArraySerializerBase<typenavigator::IVTCByteImageNavigator, IVTCByteImageSerializer>("CByteImage",
+ simox::meta::get_type_name(typeid(data::AronNDArray)),
+ simox::meta::get_type_name(typeid(type::AronIVTCByteImage)), n)
{
- // check if type exists
- ACCEPTED_TYPES.at(typenavigator->getTypename());
+ ARMARX_CHECK_NOT_NULL(typenavigator);
+ if (typenavigator->getMaybe() == type::Maybe::eNone or typenavigator->getMaybe() == type::Maybe::eOptional)
+ {
+ throw error::MaybeNotValidException("IVTCByteImageSerializer", "IVTCByteImageSerializer", "An IVTCByteImage must be a pointer!", typenavigator->getMaybe(), typenavigator->getPath());
+ }
}
- std::vector<CppFieldPtr> IVTCByteImageSerializer::getPublicVariableDeclarations(const std::string& name) const
+ CppBlockPtr IVTCByteImageSerializer::getResetHardBlock(const std::string& accessor) const
{
- CppFieldPtr field = CppFieldPtr(new CppField(getFullCppTypename(), name));
- return {field};
+ CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
+ block_if_data->addLine(accessor + " = " + this->getFullTypenameGenerator() + "();");
+ return ResolveMaybeResetHardBlock(accessor, block_if_data, typenavigator);
}
- std::vector<std::pair<std::string, std::string>> IVTCByteImageSerializer::getCtorInitializers(const std::string&) const
+ CppBlockPtr IVTCByteImageSerializer::getResetSoftBlock(const std::string& accessor) const
{
- return {};
+ CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
+ block_if_data->addLine(accessor + nextEl() + "Set(" + accessor + nextEl() + "width, " + accessor + nextEl() + "height, " + accessor + nextEl() + "type);");
+ return ResolveMaybeResetSoftBlock(accessor, block_if_data, typenavigator);
}
- CppBlockPtr IVTCByteImageSerializer::getCtorBlock(const std::string&) const
+ CppBlockPtr IVTCByteImageSerializer::getWriteTypeBlock(const std::string& accessor) const
{
CppBlockPtr b = CppBlockPtr(new CppBlock());
- return b;
- }
-
- CppBlockPtr IVTCByteImageSerializer::getResetBlock(const std::string& accessor) const
- {
- CppBlockPtr b = CppBlockPtr(new CppBlock());
- b->addLine(accessor + "->Set(" + accessor + "->width, " + accessor + "->height, " + accessor + "->type);");
- return b;
- }
-
- CppBlockPtr IVTCByteImageSerializer::getInitializeBlock(const std::string& accessor) const
- {
- CppBlockPtr b = CppBlockPtr(new CppBlock());
- b->addLine(accessor + " = std::make_shared<" + getCoreCppTypename() + ">(" + std::to_string(typenavigator->getWidth()) + ", " + std::to_string(typenavigator->getHeight()) + ", " + ACCEPTED_TYPES.at(typenavigator->getTypename()).first + ");");
- return b;
- }
-
- CppBlockPtr IVTCByteImageSerializer::getWriteInitialTypeBlock(const std::string&) const
- {
- CppBlockPtr b = CppBlockPtr(new CppBlock());
- b->addLine("w.writeIVTCByteImage(" + std::to_string(typenavigator->getWidth()) + ", " + std::to_string(typenavigator->getHeight()) + ", \"" + typenavigator->getTypename() + "\");");
- return b;
- }
-
- CppBlockPtr IVTCByteImageSerializer::getWriteCurrentTypeBlock(const std::string& accessor) const
- {
- CppBlockPtr b = CppBlockPtr(new CppBlock());
- b->addLine("w.writeIVTCByteImage(" + accessor + "->width, " + accessor + "->height, \"" + typenavigator->getTypename() + "\");");
+ b->addLine("w.writeIVTCByteImage({" + MAYBE_TO_STRING(typenavigator->getMaybe()) + "}); // of " + accessor);
return b;
}
CppBlockPtr IVTCByteImageSerializer::getWriteBlock(const std::string& accessor) const
{
- CppBlockPtr b = CppBlockPtr(new CppBlock());
- b->addLine("w.writeNDArray({" + accessor + "->width, " + accessor + "->height, " + accessor + "->bytesPerPixel}, std::to_string(" + accessor + "->type), reinterpret_cast<const unsigned char*>(" + accessor + "->pixels));");
- return b;
+ CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
+ block_if_data->addLine("w.writeNDArray({" + accessor + nextEl() + "width, " + accessor + nextEl() + "height, " + accessor + nextEl() + "bytesPerPixel}, std::to_string(" + accessor + nextEl() + "type), reinterpret_cast<const unsigned char*>(" + accessor + nextEl() + "pixels)); // of " + accessor);
+
+ return ResolveMaybeWriteBlock(accessor, block_if_data, typenavigator);
}
CppBlockPtr IVTCByteImageSerializer::getReadBlock(const std::string& accessor) const
{
- CppBlockPtr b = CppBlockPtr(new CppBlock());
+ CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
std::string escaped_accessor = EscapeAccessor(accessor);
- std::string dims_accessor = escaped_accessor + DIMENSION_ACCESSOR;
- std::string type_accessor = escaped_accessor + TYPE_ACCESSOR;
+ std::string read_start_result_accessor = escaped_accessor + READ_START_RETURN_TYPE_ACCESSOR;
- b->addLine("const auto [" + dims_accessor + ", " + type_accessor + "] = r.readStartNDArray();");
- b->addLine(accessor + "->Set(" + dims_accessor + "[0], " + dims_accessor + "[1], static_cast<CByteImage::ImageType>(std::stoi(" + type_accessor + ")));");
- b->addLine("r.readEndNDArray(reinterpret_cast<unsigned char*>(" + accessor + "->pixels));");
- return b;
+ block_if_data->addLine(accessor + nextEl() + "Set(" + read_start_result_accessor + ".dims[0], " + read_start_result_accessor + ".dims[1], static_cast<CByteImage::ImageType>(std::stoi(" + read_start_result_accessor + ".type)));");
+ block_if_data->addLine("r.readEndNDArray(reinterpret_cast<unsigned char*>(" + accessor + nextEl() + "pixels)); // of " + accessor);
+ return ResolveMaybeReadBlock(accessor, "r.readStartNDArray()", block_if_data, typenavigator);
}
CppBlockPtr IVTCByteImageSerializer::getEqualsBlock(const std::string& accessor, const std::string& otherInstanceAccessor) const
{
- CppBlockPtr b = CppBlockPtr(new CppBlock());
- b->addLine("if (not (" + accessor + "->IsCompatible(" + otherInstanceAccessor + ".get())))");
- b->addLine("\t return false;");
- b->addLine("if (not (memcmp(" + accessor + "->pixels, " + otherInstanceAccessor + "->pixels, " + accessor + "->width * " + accessor + "->height * " + accessor + "->bytesPerPixel) == 0))");
- b->addLine("\t return false;");
- return b;
+ CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
+ block_if_data->addLine("if (not (" + accessor + nextEl() + "IsCompatible(" + toPointerAccessor(otherInstanceAccessor) + ")))");
+ block_if_data->addLine("\t return false;");
+ block_if_data->addLine("if (not (memcmp(" + accessor + nextEl() + "pixels, " + otherInstanceAccessor + nextEl() + "pixels, " + accessor + nextEl() + "width * " + accessor + nextEl() + "height * " + accessor + nextEl() + "bytesPerPixel) == 0))");
+ block_if_data->addLine("\t return false;");
+ return ResolveMaybeEqualsBlock(accessor, otherInstanceAccessor, block_if_data, typenavigator);
}
}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/IVTCByteImage.h b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/IVTCByteImage.h
index ac91df514b87f3377f5f9738a613cd1dc6c29ca6..07e61842942c67a10bdef419596ae5580c352dfd 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/IVTCByteImage.h
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/IVTCByteImage.h
@@ -28,19 +28,19 @@
#include <map>
// Base Class
-#include <RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/Serializer.h>
+#include "../detail/NDArraySerializerBase.h"
// ArmarX
-#include <RobotAPI/libraries/aron/core/navigator/type/ndarray/IVTCByteImage.h>
+#include "../../../../../navigator/type/ndarray/IVTCByteImage.h"
-namespace armarx::aron::cppcodegenerator::serializer
+namespace armarx::aron::cppserializer::serializer
{
class IVTCByteImageSerializer;
typedef std::shared_ptr<IVTCByteImageSerializer> AronIVTCByteImageTypeCppSerializerPtr;
class IVTCByteImageSerializer :
- virtual public Serializer
+ virtual public detail::NDArraySerializerBase<typenavigator::IVTCByteImageNavigator, IVTCByteImageSerializer>
{
public:
using PointerType = AronIVTCByteImageTypeCppSerializerPtr;
@@ -50,13 +50,9 @@ namespace armarx::aron::cppcodegenerator::serializer
IVTCByteImageSerializer(const typenavigator::IVTCByteImageNavigatorPtr&);
// virtual implementations
- virtual std::vector<CppFieldPtr> getPublicVariableDeclarations(const std::string&) const override;
- virtual std::vector<std::pair<std::string, std::string>> getCtorInitializers(const std::string&) const override;
- virtual CppBlockPtr getCtorBlock(const std::string&) const override;
- virtual CppBlockPtr getWriteInitialTypeBlock(const std::string&) const override;
- virtual CppBlockPtr getInitializeBlock(const std::string&) const override;
- virtual CppBlockPtr getWriteCurrentTypeBlock(const std::string&) const override;
- virtual CppBlockPtr getResetBlock(const std::string&) const override;
+ virtual CppBlockPtr getResetHardBlock(const std::string& accessor) const override;
+ virtual CppBlockPtr getResetSoftBlock(const std::string& accessor) const override;
+ virtual CppBlockPtr getWriteTypeBlock(const std::string&) const override;
virtual CppBlockPtr getWriteBlock(const std::string&) const override;
virtual CppBlockPtr getReadBlock(const std::string&) const override;
virtual CppBlockPtr getEqualsBlock(const std::string&, const std::string&) const override;
@@ -64,9 +60,5 @@ namespace armarx::aron::cppcodegenerator::serializer
private:
// members
static const std::map<std::string, std::pair<std::string, int>> ACCEPTED_TYPES;
- static constexpr const char* DIMENSION_ACCESSOR = "_dimensions";
- static constexpr const char* TYPE_ACCESSOR = "_type";
-
- typenavigator::IVTCByteImageNavigatorPtr typenavigator;
};
}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/NDArray.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/NDArray.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..67eb03720ef2650e14be2f0b4e0713dca4e2a010
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/NDArray.cpp
@@ -0,0 +1,76 @@
+/*
+ * 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 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
+ */
+
+// Header
+#include "NDArray.h"
+
+namespace armarx::aron::cppserializer::serializer
+{
+ // constructors
+ NDArraySerializer::NDArraySerializer(const typenavigator::NDArrayNavigatorPtr& n) :
+ detail::NDArraySerializerBase<typenavigator::NDArrayNavigator, NDArraySerializer>("NDArray",
+ simox::meta::get_type_name(typeid(data::AronNDArray)),
+ simox::meta::get_type_name(typeid(type::AronNDArray)), n)
+ {
+ ARMARX_CHECK_NOT_NULL(typenavigator);
+ }
+
+ CppBlockPtr NDArraySerializer::getResetHardBlock(const std::string& accessor) const
+ {
+ CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
+ block_if_data->addLine(accessor + " = " + this->getFullTypenameGenerator() + "();");
+ return ResolveMaybeResetHardBlock(accessor, block_if_data, typenavigator);
+ }
+
+ CppBlockPtr NDArraySerializer::getResetSoftBlock(const std::string& accessor) const
+ {
+ CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
+ block_if_data->addLine(accessor + " = " + this->getFullCppTypename() + "();");
+ return ResolveMaybeResetSoftBlock(accessor, block_if_data, typenavigator);
+ }
+
+ CppBlockPtr NDArraySerializer::getWriteTypeBlock(const std::string&) const
+ {
+ CppBlockPtr b = CppBlockPtr(new CppBlock());
+ return b;
+ }
+
+ CppBlockPtr NDArraySerializer::getWriteBlock(const std::string& accessor) const
+ {
+ CppBlockPtr b = CppBlockPtr(new CppBlock());
+ return b;
+ }
+
+ CppBlockPtr NDArraySerializer::getReadBlock(const std::string& accessor) const
+ {
+ CppBlockPtr b = CppBlockPtr(new CppBlock());
+ return b;
+ }
+
+ CppBlockPtr NDArraySerializer::getEqualsBlock(const std::string& accessor, const std::string& otherInstanceAccessor) const
+ {
+ CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
+ return ResolveMaybeEqualsBlock(accessor, otherInstanceAccessor, block_if_data, typenavigator);
+ }
+}
+
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/NDArray.h b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/NDArray.h
new file mode 100644
index 0000000000000000000000000000000000000000..7785a7b0c30f6c3a34c6708f67b577d4cc27fd26
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/NDArray.h
@@ -0,0 +1,63 @@
+/*
+ * 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 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
+ */
+
+#pragma once
+
+// STD/STL
+#include <string>
+#include <map>
+
+// Base Class
+#include "../detail/NDArraySerializerBase.h"
+
+// ArmarX
+#include "../../../../../navigator/type/ndarray/NDArray.h"
+
+namespace armarx::aron::cppserializer::serializer
+{
+ class NDArraySerializer;
+ typedef std::shared_ptr<NDArraySerializer> AronNDArrayTypeCppSerializerPtr;
+
+ class NDArraySerializer :
+ virtual public detail::NDArraySerializerBase<typenavigator::NDArrayNavigator, NDArraySerializer>
+ {
+ public:
+ using PointerType = AronNDArrayTypeCppSerializerPtr;
+
+ public:
+ // constructors
+ NDArraySerializer(const typenavigator::NDArrayNavigatorPtr&);
+
+ // virtual implementations
+ virtual CppBlockPtr getResetHardBlock(const std::string& accessor) const override;
+ virtual CppBlockPtr getResetSoftBlock(const std::string& accessor) const override;
+ virtual CppBlockPtr getWriteTypeBlock(const std::string&) const override;
+ virtual CppBlockPtr getWriteBlock(const std::string&) const override;
+ virtual CppBlockPtr getReadBlock(const std::string&) const override;
+ virtual CppBlockPtr getEqualsBlock(const std::string&, const std::string&) const override;
+
+ private:
+ static constexpr const char* DIMENSION_ACCESSOR = "_dimensions";
+ static constexpr const char* TYPE_ACCESSOR = "_type";
+ };
+}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/OpenCVMat.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/OpenCVMat.cpp
index 3961a3884b7867599997cc2c1b695d8abeffec7b..da0e3c01389ef54f1d3afbd97f6767112db70a24 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/OpenCVMat.cpp
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/OpenCVMat.cpp
@@ -24,7 +24,7 @@
// Header
#include "OpenCVMat.h"
-namespace armarx::aron::cppcodegenerator::serializer
+namespace armarx::aron::cppserializer::serializer
{
const std::map<std::string, std::pair<std::string, int>> OpenCVMatSerializer::ACCEPTED_TYPES =
@@ -39,35 +39,15 @@ namespace armarx::aron::cppcodegenerator::serializer
};
// constructors
- OpenCVMatSerializer::OpenCVMatSerializer(const typenavigator::OpenCVMatNavigatorPtr& e) :
- Serializer("cv::Mat", simox::meta::get_type_name(typeid(data::AronNDArray)), simox::meta::get_type_name(typeid(type::AronOpenCVMat))),
- typenavigator(e)
+ OpenCVMatSerializer::OpenCVMatSerializer(const typenavigator::OpenCVMatNavigatorPtr& n) :
+ detail::NDArraySerializerBase<typenavigator::OpenCVMatNavigator, OpenCVMatSerializer>("cv::Mat",
+ simox::meta::get_type_name(typeid(data::AronNDArray)),
+ simox::meta::get_type_name(typeid(type::AronOpenCVMat)), n)
{
- // check if type exists
- if (ACCEPTED_TYPES.find(e->getTypename()) == ACCEPTED_TYPES.end())
- {
- throw error::StringNotValidException("OpenCVMatSerializer", "OpenCVMatSerializer", "Could not find a matching typename.", e->getTypename(), e->getPath());
- }
+ ARMARX_CHECK_NOT_NULL(typenavigator);
}
- std::vector<CppFieldPtr> OpenCVMatSerializer::getPublicVariableDeclarations(const std::string& name) const
- {
- CppFieldPtr field = CppFieldPtr(new CppField(getFullCppTypename(), name));
- return {field};
- }
-
- std::vector<std::pair<std::string, std::string>> OpenCVMatSerializer::getCtorInitializers(const std::string&) const
- {
- return {};
- }
-
- CppBlockPtr OpenCVMatSerializer::getCtorBlock(const std::string&) const
- {
- CppBlockPtr b = CppBlockPtr(new CppBlock());
- return b;
- }
-
- CppBlockPtr OpenCVMatSerializer::getResetBlock(const std::string& accessor) const
+ std::pair<CppBlockPtr, std::string> OpenCVMatSerializer::getDimensionsFromAccessor(const std::string& accessor) const
{
CppBlockPtr b = CppBlockPtr(new CppBlock());
@@ -76,82 +56,76 @@ namespace armarx::aron::cppcodegenerator::serializer
std::string accessor_iterator = escaped_accessor + ITERATOR_ACCESSOR;
b->addLine("std::vector<int> " + accessor_dimensions + ";");
- b->addLine("for (int " + accessor_iterator + " = 0; " + accessor_iterator + " < " + accessor + ".dims; ++" + accessor_iterator + ")");
+ b->addLine("for (int " + accessor_iterator + " = 0; " + accessor_iterator + " < " + accessor + nextEl() + "dims; ++" + accessor_iterator + ")");
CppBlockPtr b2 = CppBlockPtr(new CppBlock());
- b2->addLine(accessor_dimensions + ".push_back(" + accessor + ".size[" + accessor_iterator + "]);");
- b->appendBlock(b2);
- b->addLine(accessor + " = " + getFullCppTypename() + "(" + accessor_dimensions + ", " + accessor + ".type());");
- return b;
+ b2->addLine(accessor_dimensions + ".push_back(" + accessor + nextEl() + "size[" + accessor_iterator + "]);");
+ b->addBlock(b2);
+ return {b, accessor_dimensions};
}
- CppBlockPtr OpenCVMatSerializer::getInitializeBlock(const std::string& accessor) const
+ CppBlockPtr OpenCVMatSerializer::getResetHardBlock(const std::string& accessor) const
{
- CppBlockPtr b = CppBlockPtr(new CppBlock());
- b->addLine(accessor + " = " + getFullCppTypename() + "(std::vector<int>({" + simox::alg::to_string<int>(typenavigator->getDimensions(), ", ") + "}), " + ACCEPTED_TYPES.at(typenavigator->getTypename()).first + ");");
- return b;
+ CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
+ block_if_data->addLine(accessor + " = " + this->getFullTypenameGenerator() + "();");
+ return ResolveMaybeResetHardBlock(accessor, block_if_data, typenavigator);
}
- CppBlockPtr OpenCVMatSerializer::getWriteInitialTypeBlock(const std::string&) const
+ CppBlockPtr OpenCVMatSerializer::getResetSoftBlock(const std::string& accessor) const
{
- CppBlockPtr b = CppBlockPtr(new CppBlock());
- b->addLine("w.writeOpenCVMat({" + simox::alg::to_string<int>(typenavigator->getDimensions(), ", ") + "}, \"" + typenavigator->getTypename() + "\");");
- return b;
+ CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
+
+ auto [get_dim_block, accessor_dimensions] = getDimensionsFromAccessor(accessor);
+ block_if_data->appendBlock(get_dim_block);
+ block_if_data->addLine("if (!" + accessor_dimensions + ".empty())");
+ CppBlockPtr b2 = CppBlockPtr(new CppBlock());
+ b2->addLine(accessor + " = " + getFullCppTypename() + "(" + accessor_dimensions + ", " + accessor + nextEl() + "type());");
+ block_if_data->addBlock(b2);
+
+ return ResolveMaybeResetSoftBlock(accessor, block_if_data, typenavigator);
}
- CppBlockPtr OpenCVMatSerializer::getWriteCurrentTypeBlock(const std::string& accessor) const
+ CppBlockPtr OpenCVMatSerializer::getWriteTypeBlock(const std::string& accessor) const
{
CppBlockPtr b = CppBlockPtr(new CppBlock());
-
- std::string escaped_accessor = EscapeAccessor(accessor);
- std::string accessor_dimensions = escaped_accessor + DIMENSION_ACCESSOR;
- std::string accessor_iterator = escaped_accessor + ITERATOR_ACCESSOR;
-
- b->addLine("std::vector<int> " + accessor_dimensions + ";");
- b->addLine("for (int " + accessor_iterator + " = 0; " + accessor_iterator + " < " + accessor + ".dims; ++" + accessor_iterator + ")");
- CppBlockPtr b2 = CppBlockPtr(new CppBlock());
- b2->addLine(accessor_dimensions + ".push_back(" + accessor + ".size[" + accessor_iterator + "]);");
- b->appendBlock(b2);
- b->addLine("w.writeOpenCVMat(" + accessor_dimensions + ", std::to_string(" + accessor + ".type()));");
+ b->addLine("w.writeOpenCVMat({" + MAYBE_TO_STRING(typenavigator->getMaybe()) + "}); // of " + accessor);
return b;
}
CppBlockPtr OpenCVMatSerializer::getWriteBlock(const std::string& accessor) const
{
- CppBlockPtr b = CppBlockPtr(new CppBlock());
+ CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
std::string escaped_accessor = EscapeAccessor(accessor);
- std::string accessor_dimensions = escaped_accessor + DIMENSION_ACCESSOR;
- std::string accessor_iterator = escaped_accessor + ITERATOR_ACCESSOR;
- b->addLine("std::vector<int> " + accessor_dimensions + ";");
- b->addLine("for (int " + accessor_iterator + " = 0; " + accessor_iterator + " < " + accessor + ".dims; ++" + accessor_iterator + ")");
- CppBlockPtr b2 = CppBlockPtr(new CppBlock());
- b2->addLine(accessor_dimensions + ".push_back(" + accessor + ".size[" + accessor_iterator + "]);");
- b->appendBlock(b2);
- b->addLine(accessor_dimensions + ".push_back(" + accessor + ".elemSize());");
- b->addLine("w.writeNDArray(" + accessor_dimensions + ", std::to_string(" + accessor + ".type()), reinterpret_cast<const unsigned char*>(" + accessor + ".data));");
- return b;
+ auto [get_dim_block, accessor_dimensions] = getDimensionsFromAccessor(accessor);
+ block_if_data->appendBlock(get_dim_block);
+ block_if_data->addLine(accessor_dimensions + ".push_back(" + accessor + nextEl() + "elemSize());");
+ block_if_data->addLine("w.writeNDArray(" + accessor_dimensions + ", std::to_string(" + accessor + nextEl() + "type()), reinterpret_cast<const unsigned char*>(" + accessor + nextEl() + "data)); // of " + accessor);
+
+ return ResolveMaybeWriteBlock(accessor, block_if_data, typenavigator);
}
CppBlockPtr OpenCVMatSerializer::getReadBlock(const std::string& accessor) const
{
- CppBlockPtr b = CppBlockPtr(new CppBlock());
+ CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
std::string escaped_accessor = EscapeAccessor(accessor);
- std::string accessor_dimensions = escaped_accessor + DIMENSION_ACCESSOR;
- std::string accessor_type = escaped_accessor + TYPE_ACCESSOR;
+ std::string read_start_result_accessor = escaped_accessor + READ_START_RETURN_TYPE_ACCESSOR;
- b->addLine("const auto [" + accessor_dimensions + ", " + accessor_type + "] = r.readStartNDArray();");
- b->addLine(accessor + " = " + getFullCppTypename() + "(std::vector<int>({" + accessor_dimensions + ".begin(), std::prev(" + accessor_dimensions + ".end())}), std::stoi(" + accessor_type + "));");
- b->addLine("r.readEndNDArray(reinterpret_cast<unsigned char*>(" + accessor + ".data));");
- return b;
+ block_if_data->addLine("if (!" + read_start_result_accessor + ".dims.empty())");
+ CppBlockPtr b2 = CppBlockPtr(new CppBlock());
+ b2->addLine(accessor + " = " + getCoreCppTypename() + "(std::vector<int>({" + read_start_result_accessor + ".dims.begin(), std::prev(" + read_start_result_accessor + ".dims.end())}), std::stoi(" + read_start_result_accessor + ".type));");
+ b2->addLine("r.readEndNDArray(reinterpret_cast<unsigned char*>(" + accessor + nextEl() + "data)); // of " + accessor);
+ block_if_data->addBlock(b2);
+
+ return ResolveMaybeReadBlock(accessor, "r.readStartNDArray()", block_if_data, typenavigator);
}
CppBlockPtr OpenCVMatSerializer::getEqualsBlock(const std::string& accessor, const std::string& otherInstanceAccessor) const
{
- CppBlockPtr b = CppBlockPtr(new CppBlock());
- b->addLine("if (cv::countNonZero(" + accessor + " != " + otherInstanceAccessor + ") != 0)");
- b->addLine("\t return false;");
- return b;
+ CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
+ block_if_data->addLine("if (cv::countNonZero(" + accessor + " != " + otherInstanceAccessor + ") != 0)");
+ block_if_data->addLine("\t return false;");
+ return ResolveMaybeEqualsBlock(accessor, otherInstanceAccessor, block_if_data, typenavigator);
}
}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/OpenCVMat.h b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/OpenCVMat.h
index dff7118e2e4dad0cdc46267e4a927f7d53435eac..4ed162490b73187359eecd5546115a37c0d4b01f 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/OpenCVMat.h
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/OpenCVMat.h
@@ -28,19 +28,19 @@
#include <map>
// Base Class
-#include <RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/Serializer.h>
+#include "../detail/NDArraySerializerBase.h"
// ArmarX
-#include <RobotAPI/libraries/aron/core/navigator/type/ndarray/OpenCVMat.h>
+#include "../../../../../navigator/type/ndarray/OpenCVMat.h"
-namespace armarx::aron::cppcodegenerator::serializer
+namespace armarx::aron::cppserializer::serializer
{
class OpenCVMatSerializer;
typedef std::shared_ptr<OpenCVMatSerializer> AronOpenCVMatTypeCppSerializerPtr;
class OpenCVMatSerializer :
- virtual public Serializer
+ virtual public detail::NDArraySerializerBase<typenavigator::OpenCVMatNavigator, OpenCVMatSerializer>
{
public:
using PointerType = AronOpenCVMatTypeCppSerializerPtr;
@@ -50,24 +50,21 @@ namespace armarx::aron::cppcodegenerator::serializer
OpenCVMatSerializer(const typenavigator::OpenCVMatNavigatorPtr& n);
// virtual implementations
- virtual std::vector<CppFieldPtr> getPublicVariableDeclarations(const std::string&) const override;
- virtual std::vector<std::pair<std::string, std::string>> getCtorInitializers(const std::string&) const override;
- virtual CppBlockPtr getCtorBlock(const std::string&) const override;
- virtual CppBlockPtr getWriteInitialTypeBlock(const std::string&) const override;
- virtual CppBlockPtr getInitializeBlock(const std::string&) const override;
- virtual CppBlockPtr getWriteCurrentTypeBlock(const std::string&) const override;
- virtual CppBlockPtr getResetBlock(const std::string&) const override;
+ virtual CppBlockPtr getResetHardBlock(const std::string& accessor) const override;
+ virtual CppBlockPtr getResetSoftBlock(const std::string& accessor) const override;
+ virtual CppBlockPtr getWriteTypeBlock(const std::string&) const override;
virtual CppBlockPtr getWriteBlock(const std::string&) const override;
virtual CppBlockPtr getReadBlock(const std::string&) const override;
virtual CppBlockPtr getEqualsBlock(const std::string&, const std::string&) const override;
+ private:
+ std::pair<CppBlockPtr, std::string> getDimensionsFromAccessor(const std::string& accessor) const;
+
private:
// members
static const std::map<std::string, std::pair<std::string, int>> ACCEPTED_TYPES;
static constexpr const char* ITERATOR_ACCESSOR = "_iterator";
static constexpr const char* DIMENSION_ACCESSOR = "_dimensions";
- static constexpr const char* TYPE_ACCESSOR = "_type";
-
- typenavigator::OpenCVMatNavigatorPtr typenavigator;
+ static constexpr const char* NUM_DIMENSION_ACCESSOR = "_num_dimensions";
};
}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/Orientation.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/Orientation.cpp
index 42fe5d66dc70e3c5e769aaf65abc58e7ac7728c9..c10d1adfb26b3462639d2aed861d0f2d22be2fca 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/Orientation.cpp
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/Orientation.cpp
@@ -24,86 +24,59 @@
// Header
#include "Orientation.h"
-namespace armarx::aron::cppcodegenerator::serializer
+namespace armarx::aron::cppserializer::serializer
{
// constructors
OrientationSerializer::OrientationSerializer(const typenavigator::OrientationNavigatorPtr& n) :
- Serializer("Eigen::Quaternion<" + n->getTypename() + ">", simox::meta::get_type_name(typeid(data::AronNDArray)), simox::meta::get_type_name(typeid(type::AronOrientation))),
- typenavigator(n)
+ detail::NDArraySerializerBase<typenavigator::OrientationNavigator, OrientationSerializer>("Eigen::Quaternion<" + n->ACCEPTED_TYPE + ">",
+ simox::meta::get_type_name(typeid(data::AronNDArray)),
+ simox::meta::get_type_name(typeid(type::AronOrientation)), n)
{
+ ARMARX_CHECK_NOT_NULL(typenavigator);
}
- std::vector<CppFieldPtr> OrientationSerializer::getPublicVariableDeclarations(const std::string& name) const
+ CppBlockPtr OrientationSerializer::getResetHardBlock(const std::string& accessor) const
{
- CppFieldPtr field = CppFieldPtr(new CppField(getFullCppTypename(), name));
- return {field};
+ CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
+ block_if_data->addLine(accessor + " = " + this->getFullTypenameGenerator() + "();");
+ return ResolveMaybeResetHardBlock(accessor, block_if_data, typenavigator);
}
- std::vector<std::pair<std::string, std::string>> OrientationSerializer::getCtorInitializers(const std::string&) const
+ CppBlockPtr OrientationSerializer::getResetSoftBlock(const std::string& accessor) const
{
- return {};
+ CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
+ block_if_data->addLine(accessor + " = " + this->getFullCppTypename() + "();");
+ return ResolveMaybeResetSoftBlock(accessor, block_if_data, typenavigator);
}
- CppBlockPtr OrientationSerializer::getCtorBlock(const std::string&) const
+ CppBlockPtr OrientationSerializer::getWriteTypeBlock(const std::string& accessor) const
{
CppBlockPtr b = CppBlockPtr(new CppBlock());
- return b;
- }
-
- CppBlockPtr OrientationSerializer::getResetBlock(const std::string& accessor) const
- {
- CppBlockPtr b = CppBlockPtr(new CppBlock());
- b->addLine(accessor + " = " + getFullCppTypename() + "();");
- return b;
- }
-
- CppBlockPtr OrientationSerializer::getInitializeBlock(const std::string& accessor) const
- {
- CppBlockPtr b = CppBlockPtr(new CppBlock());
- b->addLine(accessor + " = " + getFullCppTypename() + "();");
- return b;
- }
-
- CppBlockPtr OrientationSerializer::getWriteInitialTypeBlock(const std::string&) const
- {
- CppBlockPtr b = CppBlockPtr(new CppBlock());
- b->addLine("w.writeOrientation();");
- return b;
- }
-
- CppBlockPtr OrientationSerializer::getWriteCurrentTypeBlock(const std::string&) const
- {
- CppBlockPtr b = CppBlockPtr(new CppBlock());
- b->addLine("w.writeOrientation();");
+ b->addLine("w.writeOrientation({" + MAYBE_TO_STRING(typenavigator->getMaybe()) + "}); // of " + accessor);
return b;
}
CppBlockPtr OrientationSerializer::getWriteBlock(const std::string& accessor) const
{
- CppBlockPtr b = CppBlockPtr(new CppBlock());
- b->addLine("w.writeNDArray({" + simox::alg::to_string(typenavigator->getDimensions(), ", ") + ", 4}, \"" + typenavigator->getTypename() + "\", reinterpret_cast<const unsigned char*>(" + accessor + ".coeffs().data()));");
- return b;
+ CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
+ block_if_data->addLine("w.writeNDArray({" + simox::alg::to_string(typenavigator->ACCEPTED_DIMENSION, ", ") + ", 4}, \"" + typenavigator->ACCEPTED_TYPE + "\", reinterpret_cast<const unsigned char*>(" + accessor + nextEl() + "coeffs().data())); // of " + accessor);
+
+ return ResolveMaybeWriteBlock(accessor, block_if_data, typenavigator);
}
CppBlockPtr OrientationSerializer::getReadBlock(const std::string& accessor) const
{
- CppBlockPtr b = CppBlockPtr(new CppBlock());
- std::string escaped_accessor = EscapeAccessor(accessor);
- std::string accessor_dimensions = escaped_accessor + DIMENSION_ACCESSOR;
- std::string accessor_type = escaped_accessor + TYPE_ACCESSOR;
-
- b->addLine("const auto [" + accessor_dimensions + ", " + accessor_type + "] = r.readStartNDArray();");
- b->addLine(accessor + " = " + getFullCppTypename() + "();");
- b->addLine("r.readEndNDArray(reinterpret_cast<unsigned char*>(" + accessor + ".coeffs().data()));");
- return b;
+ CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
+ block_if_data->addLine("r.readEndNDArray(reinterpret_cast<unsigned char*>(" + accessor + nextEl() + "coeffs().data())); // of " + accessor);
+ return ResolveMaybeReadBlock(accessor, "r.readStartNDArray()", block_if_data, typenavigator);
}
CppBlockPtr OrientationSerializer::getEqualsBlock(const std::string& accessor, const std::string& otherInstanceAccessor) const
{
- CppBlockPtr b = CppBlockPtr(new CppBlock());
- b->addLine("if (not (" + accessor + ".isApprox(" + otherInstanceAccessor + ")))");
- b->addLine("\t return false;");
- return b;
+ CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
+ block_if_data->addLine("if (not (" + accessor + nextEl() + "isApprox(" + otherInstanceAccessor + ")))");
+ block_if_data->addLine("\t return false;");
+ return ResolveMaybeEqualsBlock(accessor, otherInstanceAccessor, block_if_data, typenavigator);
}
}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/Orientation.h b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/Orientation.h
index 248a1a6da3f4a93da04e362640bdb0fa5feedd8e..df2349ca6ccb488131be4524eb17a63cc16874bd 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/Orientation.h
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/Orientation.h
@@ -28,18 +28,18 @@
#include <map>
// Base Class
-#include <RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/Serializer.h>
+#include "../detail/NDArraySerializerBase.h"
// ArmarX
-#include <RobotAPI/libraries/aron/core/navigator/type/ndarray/Orientation.h>
+#include "../../../../../navigator/type/ndarray/Orientation.h"
-namespace armarx::aron::cppcodegenerator::serializer
+namespace armarx::aron::cppserializer::serializer
{
class OrientationSerializer;
typedef std::shared_ptr<OrientationSerializer> AronOrientationTypeCppSerializerPtr;
class OrientationSerializer :
- virtual public Serializer
+ virtual public detail::NDArraySerializerBase<typenavigator::OrientationNavigator, OrientationSerializer>
{
public:
using PointerType = AronOrientationTypeCppSerializerPtr;
@@ -49,13 +49,9 @@ namespace armarx::aron::cppcodegenerator::serializer
OrientationSerializer(const typenavigator::OrientationNavigatorPtr&);
// virtual implementations
- virtual std::vector<CppFieldPtr> getPublicVariableDeclarations(const std::string&) const override;
- virtual std::vector<std::pair<std::string, std::string>> getCtorInitializers(const std::string&) const override;
- virtual CppBlockPtr getCtorBlock(const std::string&) const override;
- virtual CppBlockPtr getWriteInitialTypeBlock(const std::string&) const override;
- virtual CppBlockPtr getInitializeBlock(const std::string&) const override;
- virtual CppBlockPtr getWriteCurrentTypeBlock(const std::string&) const override;
- virtual CppBlockPtr getResetBlock(const std::string&) const override;
+ virtual CppBlockPtr getResetHardBlock(const std::string& accessor) const override;
+ virtual CppBlockPtr getResetSoftBlock(const std::string& accessor) const override;
+ virtual CppBlockPtr getWriteTypeBlock(const std::string&) const override;
virtual CppBlockPtr getWriteBlock(const std::string&) const override;
virtual CppBlockPtr getReadBlock(const std::string&) const override;
virtual CppBlockPtr getEqualsBlock(const std::string&, const std::string&) const override;
@@ -63,7 +59,5 @@ namespace armarx::aron::cppcodegenerator::serializer
private:
static constexpr const char* DIMENSION_ACCESSOR = "_dimensions";
static constexpr const char* TYPE_ACCESSOR = "_type";
-
- typenavigator::OrientationNavigatorPtr typenavigator;
};
}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/PCLPointCloud.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/PCLPointCloud.cpp
index cd14f9221dce3541d1f2f2e59720077a8caed805..8229554f6542ce0fb91509ed32837df20e076ca4 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/PCLPointCloud.cpp
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/PCLPointCloud.cpp
@@ -24,7 +24,7 @@
// Header
#include "PCLPointCloud.h"
-namespace armarx::aron::cppcodegenerator::serializer
+namespace armarx::aron::cppserializer::serializer
{
const std::map<std::string, std::pair<std::string, int>> PCLPointCloudSerializer::ACCEPTED_TYPES =
@@ -41,84 +41,61 @@ namespace armarx::aron::cppcodegenerator::serializer
// constructors
PCLPointCloudSerializer::PCLPointCloudSerializer(const typenavigator::PCLPointCloudNavigatorPtr& n) :
- Serializer("pcl::PointCloud<" + ACCEPTED_TYPES.at(n->getTypename()).first + ">", simox::meta::get_type_name(typeid(data::AronNDArray)), simox::meta::get_type_name(typeid(type::AronPCLPointCloud))),
- typenavigator(n)
+ detail::NDArraySerializerBase<typenavigator::PCLPointCloudNavigator, PCLPointCloudSerializer>("pcl::PointCloud<" + ACCEPTED_TYPES.at(n->getTypename()).first + ">",
+ simox::meta::get_type_name(typeid(data::AronNDArray)),
+ simox::meta::get_type_name(typeid(type::AronPCLPointCloud)), n)
{
+ ARMARX_CHECK_NOT_NULL(typenavigator);
}
- std::vector<CppFieldPtr> PCLPointCloudSerializer::getPublicVariableDeclarations(const std::string& name) const
+ CppBlockPtr PCLPointCloudSerializer::getResetHardBlock(const std::string& accessor) const
{
- CppFieldPtr field = CppFieldPtr(new CppField(getFullCppTypename(), name));
- return {field};
+ CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
+ block_if_data->addLine(accessor + " = " + this->getFullTypenameGenerator() + "();");
+ return ResolveMaybeResetHardBlock(accessor, block_if_data, typenavigator);
}
- std::vector<std::pair<std::string, std::string>> PCLPointCloudSerializer::getCtorInitializers(const std::string&) const
+ CppBlockPtr PCLPointCloudSerializer::getResetSoftBlock(const std::string& accessor) const
{
- return {};
+ CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
+ block_if_data->addLine(accessor + " = " + getFullCppTypename() + "(" + accessor + nextEl() + "width, " + accessor + nextEl() + "height);");
+ return ResolveMaybeResetSoftBlock(accessor, block_if_data, typenavigator);
}
- CppBlockPtr PCLPointCloudSerializer::getCtorBlock(const std::string&) const
+ CppBlockPtr PCLPointCloudSerializer::getWriteTypeBlock(const std::string& accessor) const
{
CppBlockPtr b = CppBlockPtr(new CppBlock());
- return b;
- }
-
- CppBlockPtr PCLPointCloudSerializer::getResetBlock(const std::string& accessor) const
- {
- CppBlockPtr b = CppBlockPtr(new CppBlock());
- b->addLine(accessor + " = " + getFullCppTypename() + "(" + accessor + ".width, " + accessor + ".height);");
- return b;
- }
-
- CppBlockPtr PCLPointCloudSerializer::getInitializeBlock(const std::string& accessor) const
- {
- CppBlockPtr b = CppBlockPtr(new CppBlock());
- b->addLine(accessor + " = " + getFullCppTypename() + "(" + std::to_string(typenavigator->getWidth()) + ", " + std::to_string(typenavigator->getHeight()) + ");");
- return b;
- }
-
- CppBlockPtr PCLPointCloudSerializer::getWriteInitialTypeBlock(const std::string&) const
- {
- CppBlockPtr b = CppBlockPtr(new CppBlock());
- b->addLine("w.writePCLPointCloud(" + std::to_string(typenavigator->getWidth()) + ", " + std::to_string(typenavigator->getHeight()) + ", \"" + typenavigator->getTypename() + "\");");
- return b;
- }
-
- CppBlockPtr PCLPointCloudSerializer::getWriteCurrentTypeBlock(const std::string& accessor) const
- {
- CppBlockPtr b = CppBlockPtr(new CppBlock());
- b->addLine("w.writePCLPointCloud(" + accessor + ".width, " + accessor + ".height, \"" + typenavigator->getTypename() + "\");");
+ b->addLine("w.writePCLPointCloud({\"" + typenavigator->getTypename() + "\", " + MAYBE_TO_STRING(typenavigator->getMaybe()) + "}); // of " + accessor);
return b;
}
CppBlockPtr PCLPointCloudSerializer::getWriteBlock(const std::string& accessor) const
{
- CppBlockPtr b = CppBlockPtr(new CppBlock());
- b->addLine("w.writeNDArray({" + accessor + ".width, " + accessor + ".height, " + std::to_string(ACCEPTED_TYPES.at(typenavigator->getTypename()).second) + "}, \"" + typenavigator->getTypename() + "\", reinterpret_cast<const unsigned char*>(" + accessor + ".points.data()));");
- return b;
+ CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
+ block_if_data->addLine("w.writeNDArray({" + accessor + nextEl() + "width, " + accessor + nextEl() + "height, " + std::to_string(ACCEPTED_TYPES.at(typenavigator->getTypename()).second) + "}, \"" + typenavigator->getTypename() + "\", reinterpret_cast<const unsigned char*>(" + accessor + nextEl() + "points.data())); // of " + accessor);
+
+ return ResolveMaybeWriteBlock(accessor, block_if_data, typenavigator);
}
CppBlockPtr PCLPointCloudSerializer::getReadBlock(const std::string& accessor) const
{
- CppBlockPtr b = CppBlockPtr(new CppBlock());
+ CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
std::string escaped_accessor = EscapeAccessor(accessor);
- std::string dims_accessor = escaped_accessor + DIMENSION_ACCESSOR;
- std::string type_accessor = escaped_accessor + TYPE_ACCESSOR;
+ std::string read_start_result_accessor = escaped_accessor + READ_START_RETURN_TYPE_ACCESSOR;
- b->addLine("const auto [" + dims_accessor + ", " + type_accessor + "] = r.readStartNDArray();");
- b->addLine(accessor + " = " + getFullCppTypename() + "(" + dims_accessor + "[0], " + dims_accessor + "[1]);");
- b->addLine("r.readEndNDArray(reinterpret_cast<unsigned char*>(" + accessor + ".points.data()));");
- return b;
+ block_if_data->addLine(accessor + " = " + getCoreCppTypename() + "(" + read_start_result_accessor + ".dims[0], " + read_start_result_accessor + ".dims[1]);");
+ block_if_data->addLine("r.readEndNDArray(reinterpret_cast<unsigned char*>(" + accessor + nextEl() + "points.data())); // of " + accessor);
+ return ResolveMaybeReadBlock(accessor, "r.readStartNDArray()", block_if_data, typenavigator);
}
CppBlockPtr PCLPointCloudSerializer::getEqualsBlock(const std::string& accessor, const std::string& otherInstanceAccessor) const
{
- CppBlockPtr b = CppBlockPtr(new CppBlock());
- b->addLine("if (" + accessor + ".width != " + otherInstanceAccessor + ".width || " + accessor + ".height != " + otherInstanceAccessor + ".height)");
- b->addLine("\t return false;");
+ CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
+ block_if_data->addLine("if (" + accessor + nextEl() + "width != " + otherInstanceAccessor + nextEl() + "width || " + accessor + nextEl() + "height != " + otherInstanceAccessor + nextEl() + "height)");
+ block_if_data->addLine("\t return false;");
//b->addLine("if (" + accessor + "->points != " + otherInstanceAccessor + "->points)");
//b->addLine("\t return false;");
- return b;
+ return ResolveMaybeEqualsBlock(accessor, otherInstanceAccessor, block_if_data, typenavigator);
}
}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/PCLPointCloud.h b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/PCLPointCloud.h
index 101ee2ee61047673fed94964f2d41da297d0c263..88d9b337b727e9f421a518e62ed7bc7df0f41284 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/PCLPointCloud.h
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/PCLPointCloud.h
@@ -28,18 +28,18 @@
#include <map>
// Base Class
-#include <RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/Serializer.h>
+#include "../detail/NDArraySerializerBase.h"
// ArmarX
-#include <RobotAPI/libraries/aron/core/navigator/type/ndarray/PCLPointCloud.h>
+#include "../../../../../navigator/type/ndarray/PCLPointCloud.h"
-namespace armarx::aron::cppcodegenerator::serializer
+namespace armarx::aron::cppserializer::serializer
{
class PCLPointCloudSerializer;
typedef std::shared_ptr<PCLPointCloudSerializer> AronPCLPointCloudTypeCppSerializerPtr;
class PCLPointCloudSerializer :
- virtual public Serializer
+ virtual public detail::NDArraySerializerBase<typenavigator::PCLPointCloudNavigator, PCLPointCloudSerializer>
{
public:
using PointerType = AronPCLPointCloudTypeCppSerializerPtr;
@@ -49,13 +49,9 @@ namespace armarx::aron::cppcodegenerator::serializer
PCLPointCloudSerializer(const typenavigator::PCLPointCloudNavigatorPtr&);
// virtual implementations
- virtual std::vector<CppFieldPtr> getPublicVariableDeclarations(const std::string&) const override;
- virtual std::vector<std::pair<std::string, std::string>> getCtorInitializers(const std::string&) const override;
- virtual CppBlockPtr getCtorBlock(const std::string&) const override;
- virtual CppBlockPtr getWriteInitialTypeBlock(const std::string&) const override;
- virtual CppBlockPtr getInitializeBlock(const std::string&) const override;
- virtual CppBlockPtr getWriteCurrentTypeBlock(const std::string&) const override;
- virtual CppBlockPtr getResetBlock(const std::string&) const override;
+ virtual CppBlockPtr getResetHardBlock(const std::string& accessor) const override;
+ virtual CppBlockPtr getResetSoftBlock(const std::string& accessor) const override;
+ virtual CppBlockPtr getWriteTypeBlock(const std::string&) const override;
virtual CppBlockPtr getWriteBlock(const std::string&) const override;
virtual CppBlockPtr getReadBlock(const std::string&) const override;
virtual CppBlockPtr getEqualsBlock(const std::string&, const std::string&) const override;
@@ -63,9 +59,5 @@ namespace armarx::aron::cppcodegenerator::serializer
private:
// members
static const std::map<std::string, std::pair<std::string, int>> ACCEPTED_TYPES;
- static constexpr const char* DIMENSION_ACCESSOR = "_dimensions";
- static constexpr const char* TYPE_ACCESSOR = "_type";
-
- typenavigator::PCLPointCloudNavigatorPtr typenavigator;
};
}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/Pose.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/Pose.cpp
index 98a1dbae29a52a5392bfbbd537be7deea856a94d..93e340f51c049d3cd1d6913ea74c32b1505f1e08 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/Pose.cpp
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/Pose.cpp
@@ -24,86 +24,59 @@
// Header
#include "Pose.h"
-namespace armarx::aron::cppcodegenerator::serializer
+namespace armarx::aron::cppserializer::serializer
{
// constructors
PoseSerializer::PoseSerializer(const typenavigator::PoseNavigatorPtr& n) :
- Serializer("Eigen::Matrix<" + n->getTypename() + ", " + simox::alg::to_string(n->getDimensions(), ", ") + ">", simox::meta::get_type_name(typeid(data::AronNDArray)), simox::meta::get_type_name(typeid(type::AronPose))),
- typenavigator(n)
+ detail::NDArraySerializerBase<typenavigator::PoseNavigator, PoseSerializer>("Eigen::Matrix<" + n->ACCEPTED_TYPE + ", " + simox::alg::to_string(n->ACCEPTED_DIMENSION, ", ") + ">",
+ simox::meta::get_type_name(typeid(data::AronNDArray)),
+ simox::meta::get_type_name(typeid(type::AronPose)), n)
{
+ ARMARX_CHECK_NOT_NULL(typenavigator);
}
- std::vector<CppFieldPtr> PoseSerializer::getPublicVariableDeclarations(const std::string& name) const
+ CppBlockPtr PoseSerializer::getResetHardBlock(const std::string& accessor) const
{
- CppFieldPtr field = CppFieldPtr(new CppField(getFullCppTypename(), name));
- return {field};
+ CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
+ block_if_data->addLine(accessor + " = " + this->getFullTypenameGenerator() + "();");
+ return ResolveMaybeResetHardBlock(accessor, block_if_data, typenavigator);
}
- std::vector<std::pair<std::string, std::string>> PoseSerializer::getCtorInitializers(const std::string&) const
+ CppBlockPtr PoseSerializer::getResetSoftBlock(const std::string& accessor) const
{
- return {};
+ CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
+ block_if_data->addLine(accessor + " = " + this->getFullCppTypename() + "();");
+ return ResolveMaybeResetSoftBlock(accessor, block_if_data, typenavigator);
}
- CppBlockPtr PoseSerializer::getCtorBlock(const std::string&) const
+ CppBlockPtr PoseSerializer::getWriteTypeBlock(const std::string& accessor) const
{
CppBlockPtr b = CppBlockPtr(new CppBlock());
- return b;
- }
-
- CppBlockPtr PoseSerializer::getResetBlock(const std::string& accessor) const
- {
- CppBlockPtr b = CppBlockPtr(new CppBlock());
- b->addLine(accessor + " = " + getFullCppTypename() + "();");
- return b;
- }
-
- CppBlockPtr PoseSerializer::getInitializeBlock(const std::string& accessor) const
- {
- CppBlockPtr b = CppBlockPtr(new CppBlock());
- b->addLine(accessor + " = " + getFullCppTypename() + "();");
- return b;
- }
-
- CppBlockPtr PoseSerializer::getWriteInitialTypeBlock(const std::string&) const
- {
- CppBlockPtr b = CppBlockPtr(new CppBlock());
- b->addLine("w.writePose();");
- return b;
- }
-
- CppBlockPtr PoseSerializer::getWriteCurrentTypeBlock(const std::string&) const
- {
- CppBlockPtr b = CppBlockPtr(new CppBlock());
- b->addLine("w.writePose();");
+ b->addLine("w.writePose({" + MAYBE_TO_STRING(typenavigator->getMaybe()) + "}); // of " + accessor);
return b;
}
CppBlockPtr PoseSerializer::getWriteBlock(const std::string& accessor) const
{
- CppBlockPtr b = CppBlockPtr(new CppBlock());
- b->addLine("w.writeNDArray({" + simox::alg::to_string(typenavigator->getDimensions(), ", ") + ", 4}, \"" + typenavigator->getTypename() + "\", reinterpret_cast<const unsigned char*>(" + accessor + ".data()));");
- return b;
+ CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
+ block_if_data->addLine("w.writeNDArray({" + simox::alg::to_string(typenavigator->ACCEPTED_DIMENSION, ", ") + ", 4}, \"" + typenavigator->ACCEPTED_TYPE + "\", reinterpret_cast<const unsigned char*>(" + accessor + nextEl() + "data())); // of " + accessor);
+
+ return ResolveMaybeWriteBlock(accessor, block_if_data, typenavigator);
}
CppBlockPtr PoseSerializer::getReadBlock(const std::string& accessor) const
{
- CppBlockPtr b = CppBlockPtr(new CppBlock());
- std::string escaped_accessor = EscapeAccessor(accessor);
- std::string accessor_dimensions = escaped_accessor + DIMENSION_ACCESSOR;
- std::string accessor_type = escaped_accessor + TYPE_ACCESSOR;
-
- b->addLine("const auto [" + accessor_dimensions + ", " + accessor_type + "] = r.readStartNDArray();");
- b->addLine(accessor + " = " + getFullCppTypename() + "();");
- b->addLine("r.readEndNDArray(reinterpret_cast<unsigned char*>(" + accessor + ".data()));");
- return b;
+ CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
+ block_if_data->addLine("r.readEndNDArray(reinterpret_cast<unsigned char*>(" + accessor + nextEl() + "data())); // of " + accessor);
+ return ResolveMaybeReadBlock(accessor, "r.readStartNDArray()", block_if_data, typenavigator);
}
CppBlockPtr PoseSerializer::getEqualsBlock(const std::string& accessor, const std::string& otherInstanceAccessor) const
{
- CppBlockPtr b = CppBlockPtr(new CppBlock());
- b->addLine("if (not (" + accessor + ".isApprox(" + otherInstanceAccessor + ")))");
- b->addLine("\t return false;");
- return b;
+ CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
+ block_if_data->addLine("if (not (" + accessor + nextEl() + "isApprox(" + otherInstanceAccessor + ")))");
+ block_if_data->addLine("\t return false;");
+ return ResolveMaybeEqualsBlock(accessor, otherInstanceAccessor, block_if_data, typenavigator);
}
}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/Pose.h b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/Pose.h
index 567b4f1ef970f2084c7b032074808438d3d27e5d..46e7b82a51ff696f2f1b853a2dc4b5deb2e29acd 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/Pose.h
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/Pose.h
@@ -28,18 +28,18 @@
#include <map>
// Base Class
-#include <RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/Serializer.h>
+#include "../detail/NDArraySerializerBase.h"
// ArmarX
-#include <RobotAPI/libraries/aron/core/navigator/type/ndarray/Pose.h>
+#include "../../../../../navigator/type/ndarray/Pose.h"
-namespace armarx::aron::cppcodegenerator::serializer
+namespace armarx::aron::cppserializer::serializer
{
class PoseSerializer;
typedef std::shared_ptr<PoseSerializer> AronPoseTypeCppSerializerPtr;
class PoseSerializer :
- virtual public Serializer
+ virtual public detail::NDArraySerializerBase<typenavigator::PoseNavigator, PoseSerializer>
{
public:
using PointerType = AronPoseTypeCppSerializerPtr;
@@ -49,13 +49,9 @@ namespace armarx::aron::cppcodegenerator::serializer
PoseSerializer(const typenavigator::PoseNavigatorPtr&);
// virtual implementations
- virtual std::vector<CppFieldPtr> getPublicVariableDeclarations(const std::string&) const override;
- virtual std::vector<std::pair<std::string, std::string>> getCtorInitializers(const std::string&) const override;
- virtual CppBlockPtr getCtorBlock(const std::string&) const override;
- virtual CppBlockPtr getWriteInitialTypeBlock(const std::string&) const override;
- virtual CppBlockPtr getInitializeBlock(const std::string&) const override;
- virtual CppBlockPtr getWriteCurrentTypeBlock(const std::string&) const override;
- virtual CppBlockPtr getResetBlock(const std::string&) const override;
+ virtual CppBlockPtr getResetHardBlock(const std::string& accessor) const override;
+ virtual CppBlockPtr getResetSoftBlock(const std::string& accessor) const override;
+ virtual CppBlockPtr getWriteTypeBlock(const std::string&) const override;
virtual CppBlockPtr getWriteBlock(const std::string&) const override;
virtual CppBlockPtr getReadBlock(const std::string&) const override;
virtual CppBlockPtr getEqualsBlock(const std::string&, const std::string&) const override;
@@ -63,7 +59,5 @@ namespace armarx::aron::cppcodegenerator::serializer
private:
static constexpr const char* DIMENSION_ACCESSOR = "_dimensions";
static constexpr const char* TYPE_ACCESSOR = "_type";
-
- typenavigator::PoseNavigatorPtr typenavigator;
};
}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/Position.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/Position.cpp
index a0f7a0d6d553d203e71dfec31d43c996c226b634..b4409c2f9f226995c118deb33a8cb298462997cb 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/Position.cpp
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/Position.cpp
@@ -24,86 +24,59 @@
// Header
#include "Position.h"
-namespace armarx::aron::cppcodegenerator::serializer
+namespace armarx::aron::cppserializer::serializer
{
// constructors
PositionSerializer::PositionSerializer(const typenavigator::PositionNavigatorPtr& n) :
- Serializer("Eigen::Matrix<" + n->getTypename() + ", " + simox::alg::to_string(n->getDimensions(), ", ") + ">", simox::meta::get_type_name(typeid(data::AronNDArray)), simox::meta::get_type_name(typeid(type::AronPosition))),
- typenavigator(n)
+ detail::NDArraySerializerBase<typenavigator::PositionNavigator, PositionSerializer>("Eigen::Matrix<" + n->ACCEPTED_TYPE + ", " + simox::alg::to_string(n->ACCEPTED_DIMENSION, ", ") + ">",
+ simox::meta::get_type_name(typeid(data::AronNDArray)),
+ simox::meta::get_type_name(typeid(type::AronPosition)), n)
{
+ ARMARX_CHECK_NOT_NULL(typenavigator);
}
- std::vector<CppFieldPtr> PositionSerializer::getPublicVariableDeclarations(const std::string& name) const
+ CppBlockPtr PositionSerializer::getResetHardBlock(const std::string& accessor) const
{
- CppFieldPtr field = CppFieldPtr(new CppField(getFullCppTypename(), name));
- return {field};
+ CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
+ block_if_data->addLine(accessor + " = " + this->getFullTypenameGenerator() + "();");
+ return ResolveMaybeResetHardBlock(accessor, block_if_data, typenavigator);
}
- std::vector<std::pair<std::string, std::string>> PositionSerializer::getCtorInitializers(const std::string&) const
+ CppBlockPtr PositionSerializer::getResetSoftBlock(const std::string& accessor) const
{
- return {};
+ CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
+ block_if_data->addLine(accessor + " = " + this->getFullCppTypename() + "();");
+ return ResolveMaybeResetSoftBlock(accessor, block_if_data, typenavigator);
}
- CppBlockPtr PositionSerializer::getCtorBlock(const std::string&) const
+ CppBlockPtr PositionSerializer::getWriteTypeBlock(const std::string& accessor) const
{
CppBlockPtr b = CppBlockPtr(new CppBlock());
- return b;
- }
-
- CppBlockPtr PositionSerializer::getResetBlock(const std::string& accessor) const
- {
- CppBlockPtr b = CppBlockPtr(new CppBlock());
- b->addLine(accessor + " = " + getFullCppTypename() + "();");
- return b;
- }
-
- CppBlockPtr PositionSerializer::getInitializeBlock(const std::string& accessor) const
- {
- CppBlockPtr b = CppBlockPtr(new CppBlock());
- b->addLine(accessor + " = " + getFullCppTypename() + "();");
- return b;
- }
-
- CppBlockPtr PositionSerializer::getWriteInitialTypeBlock(const std::string&) const
- {
- CppBlockPtr b = CppBlockPtr(new CppBlock());
- b->addLine("w.writePosition();");
- return b;
- }
-
- CppBlockPtr PositionSerializer::getWriteCurrentTypeBlock(const std::string&) const
- {
- CppBlockPtr b = CppBlockPtr(new CppBlock());
- b->addLine("w.writePosition();");
+ b->addLine("w.writePosition({" + MAYBE_TO_STRING(typenavigator->getMaybe()) + "}); // of " + accessor);
return b;
}
CppBlockPtr PositionSerializer::getWriteBlock(const std::string& accessor) const
{
- CppBlockPtr b = CppBlockPtr(new CppBlock());
- b->addLine("w.writeNDArray({" + simox::alg::to_string(typenavigator->getDimensions(), ", ") + ", 4}, \"" + typenavigator->getTypename() + "\", reinterpret_cast<const unsigned char*>(" + accessor + ".data()));");
- return b;
+ CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
+ block_if_data->addLine("w.writeNDArray({" + simox::alg::to_string(typenavigator->ACCEPTED_DIMENSION, ", ") + ", 4}, \"" + typenavigator->ACCEPTED_TYPE + "\", reinterpret_cast<const unsigned char*>(" + accessor + nextEl() + "data())); // of " + accessor);
+
+ return ResolveMaybeWriteBlock(accessor, block_if_data, typenavigator);
}
CppBlockPtr PositionSerializer::getReadBlock(const std::string& accessor) const
{
- CppBlockPtr b = CppBlockPtr(new CppBlock());
- std::string escaped_accessor = EscapeAccessor(accessor);
- std::string accessor_dimensions = escaped_accessor + DIMENSION_ACCESSOR;
- std::string accessor_type = escaped_accessor + TYPE_ACCESSOR;
-
- b->addLine("const auto [" + accessor_dimensions + ", " + accessor_type + "] = r.readStartNDArray();");
- b->addLine(accessor + " = " + getFullCppTypename() + "();");
- b->addLine("r.readEndNDArray(reinterpret_cast<unsigned char*>(" + accessor + ".data()));");
- return b;
+ CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
+ block_if_data->addLine("r.readEndNDArray(reinterpret_cast<unsigned char*>(" + accessor + nextEl() + "data())); // of " + accessor);
+ return ResolveMaybeReadBlock(accessor, "r.readStartNDArray()", block_if_data, typenavigator);
}
CppBlockPtr PositionSerializer::getEqualsBlock(const std::string& accessor, const std::string& otherInstanceAccessor) const
{
- CppBlockPtr b = CppBlockPtr(new CppBlock());
- b->addLine("if (not (" + accessor + ".isApprox(" + otherInstanceAccessor + ")))");
- b->addLine("\t return false;");
- return b;
+ CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
+ block_if_data->addLine("if (not (" + accessor + nextEl() + "isApprox(" + otherInstanceAccessor + ")))");
+ block_if_data->addLine("\t return false;");
+ return ResolveMaybeEqualsBlock(accessor, otherInstanceAccessor, block_if_data, typenavigator);
}
}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/Position.h b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/Position.h
index 94b9317e0921c8d1e9c1985eb0060d6427ddfeaa..1c2f17b9028670e8aa87b79aa30a50b55941baaf 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/Position.h
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/Position.h
@@ -28,18 +28,18 @@
#include <map>
// Base Class
-#include <RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/Serializer.h>
+#include "../detail/NDArraySerializerBase.h"
// ArmarX
-#include <RobotAPI/libraries/aron/core/navigator/type/ndarray/Position.h>
+#include "../../../../../navigator/type/ndarray/Position.h"
-namespace armarx::aron::cppcodegenerator::serializer
+namespace armarx::aron::cppserializer::serializer
{
class PositionSerializer;
typedef std::shared_ptr<PositionSerializer> AronPositionTypeCppSerializerPtr;
class PositionSerializer :
- virtual public Serializer
+ virtual public detail::NDArraySerializerBase<typenavigator::PositionNavigator, PositionSerializer>
{
public:
using PointerType = AronPositionTypeCppSerializerPtr;
@@ -49,13 +49,9 @@ namespace armarx::aron::cppcodegenerator::serializer
PositionSerializer(const typenavigator::PositionNavigatorPtr&);
// virtual implementations
- virtual std::vector<CppFieldPtr> getPublicVariableDeclarations(const std::string&) const override;
- virtual std::vector<std::pair<std::string, std::string>> getCtorInitializers(const std::string&) const override;
- virtual CppBlockPtr getCtorBlock(const std::string&) const override;
- virtual CppBlockPtr getWriteInitialTypeBlock(const std::string&) const override;
- virtual CppBlockPtr getInitializeBlock(const std::string&) const override;
- virtual CppBlockPtr getWriteCurrentTypeBlock(const std::string&) const override;
- virtual CppBlockPtr getResetBlock(const std::string&) const override;
+ virtual CppBlockPtr getResetHardBlock(const std::string& accessor) const override;
+ virtual CppBlockPtr getResetSoftBlock(const std::string& accessor) const override;
+ virtual CppBlockPtr getWriteTypeBlock(const std::string&) const override;
virtual CppBlockPtr getWriteBlock(const std::string&) const override;
virtual CppBlockPtr getReadBlock(const std::string&) const override;
virtual CppBlockPtr getEqualsBlock(const std::string&, const std::string&) const override;
@@ -63,7 +59,5 @@ namespace armarx::aron::cppcodegenerator::serializer
private:
static constexpr const char* DIMENSION_ACCESSOR = "_dimensions";
static constexpr const char* TYPE_ACCESSOR = "_type";
-
- typenavigator::PositionNavigatorPtr typenavigator;
};
}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/Bool.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/Bool.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c0bb65832b4bda3aaea920efd48fc19b98d752f3
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/Bool.cpp
@@ -0,0 +1,48 @@
+/*
+ * 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 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
+ */
+
+
+// STD/STL
+#include <string>
+#include <map>
+
+// Header
+#include "Bool.h"
+
+namespace armarx::aron::cppserializer::serializer
+{
+ /* constructors */
+ BoolSerializer::BoolSerializer(const typenavigator::BoolNavigatorPtr& e) :
+ detail::PrimitiveSerializerBase<typenavigator::BoolNavigator, BoolSerializer>("bool", simox::meta::get_type_name(typeid(data::AronBool)), simox::meta::get_type_name(typeid(type::AronBool)), e)
+ {
+ ARMARX_CHECK_NOT_NULL(typenavigator);
+ }
+
+ /* virtual implementations */
+ CppBlockPtr BoolSerializer::getWriteTypeBlock(const std::string& accessor) const
+ {
+ CppBlockPtr b = CppBlockPtr(new CppBlock());
+ b->addLine("w.writeBool({" + MAYBE_TO_STRING(typenavigator->getMaybe()) + "}); // of " + accessor);
+ return b;
+ }
+}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/Bool.h b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/Bool.h
new file mode 100644
index 0000000000000000000000000000000000000000..ba1dd70de0220840e9feda9d18304d631142ae06
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/Bool.h
@@ -0,0 +1,50 @@
+/*
+ * 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 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
+ */
+
+#pragma once
+
+// STD/STL
+#include <string>
+
+// Base Class
+#include "../detail/PrimitiveSerializerBase.h"
+
+// ArmarX
+#include "../../../../../navigator/type/primitive/Bool.h"
+
+namespace armarx::aron::cppserializer::serializer
+{
+ class BoolSerializer;
+ typedef std::shared_ptr<BoolSerializer> BoolSerializerPtr;
+
+ class BoolSerializer :
+ virtual public detail::PrimitiveSerializerBase<typenavigator::BoolNavigator, BoolSerializer>
+ {
+ public:
+ /* constructors */
+ BoolSerializer(const typenavigator::BoolNavigatorPtr& e);
+
+ /* virtual implementations */
+ virtual CppBlockPtr getWriteTypeBlock(const std::string& accessor) const override;
+ };
+}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/Double.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/Double.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..62a47a280f4bfea571384b30d9674fa08d8aa801
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/Double.cpp
@@ -0,0 +1,48 @@
+/*
+ * 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 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
+ */
+
+
+// STD/STL
+#include <string>
+#include <map>
+
+// Header
+#include "Double.h"
+
+namespace armarx::aron::cppserializer::serializer
+{
+ /* constructors */
+ DoubleSerializer::DoubleSerializer(const typenavigator::DoubleNavigatorPtr& e) :
+ detail::PrimitiveSerializerBase<typenavigator::DoubleNavigator, DoubleSerializer>("double", simox::meta::get_type_name(typeid(data::AronDouble)), simox::meta::get_type_name(typeid(type::AronDouble)), e)
+ {
+ ARMARX_CHECK_NOT_NULL(typenavigator);
+ }
+
+ /* virtual implementations */
+ CppBlockPtr DoubleSerializer::getWriteTypeBlock(const std::string& accessor) const
+ {
+ CppBlockPtr b = CppBlockPtr(new CppBlock());
+ b->addLine("w.writeDouble({" + MAYBE_TO_STRING(typenavigator->getMaybe()) + "}); // of " + accessor);
+ return b;
+ }
+}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/Double.h b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/Double.h
new file mode 100644
index 0000000000000000000000000000000000000000..6a965e2034d3924b842634d7dbb8219467a17294
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/Double.h
@@ -0,0 +1,50 @@
+/*
+ * 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 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
+ */
+
+#pragma once
+
+// STD/STL
+#include <string>
+
+// Base Class
+#include "../detail/PrimitiveSerializerBase.h"
+
+// ArmarX
+#include "../../../../../navigator/type/primitive/Double.h"
+
+namespace armarx::aron::cppserializer::serializer
+{
+ class DoubleSerializer;
+ typedef std::shared_ptr<DoubleSerializer> DoubleSerializerPtr;
+
+ class DoubleSerializer :
+ virtual public detail::PrimitiveSerializerBase<typenavigator::DoubleNavigator, DoubleSerializer>
+ {
+ public:
+ /* constructors */
+ DoubleSerializer(const typenavigator::DoubleNavigatorPtr& e);
+
+ /* virtual implementations */
+ virtual CppBlockPtr getWriteTypeBlock(const std::string& accessor) const override;
+ };
+}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/Float.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/Float.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..06cbd8c1a4304f095fc85887060f696335b05f55
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/Float.cpp
@@ -0,0 +1,48 @@
+/*
+ * 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 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
+ */
+
+
+// STD/STL
+#include <string>
+#include <map>
+
+// Header
+#include "Float.h"
+
+namespace armarx::aron::cppserializer::serializer
+{
+ /* constructors */
+ FloatSerializer::FloatSerializer(const typenavigator::FloatNavigatorPtr& e) :
+ detail::PrimitiveSerializerBase<typenavigator::FloatNavigator, FloatSerializer>("float", simox::meta::get_type_name(typeid(data::AronFloat)), simox::meta::get_type_name(typeid(type::AronFloat)), e)
+ {
+ ARMARX_CHECK_NOT_NULL(typenavigator);
+ }
+
+ /* virtual implementations */
+ CppBlockPtr FloatSerializer::getWriteTypeBlock(const std::string& accessor) const
+ {
+ CppBlockPtr b = CppBlockPtr(new CppBlock());
+ b->addLine("w.writeFloat({" + MAYBE_TO_STRING(typenavigator->getMaybe()) + "}); // of " + accessor);
+ return b;
+ }
+}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/Float.h b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/Float.h
new file mode 100644
index 0000000000000000000000000000000000000000..b2139ed2a368912adba187220a84fb6565b760ee
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/Float.h
@@ -0,0 +1,50 @@
+/*
+ * 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 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
+ */
+
+#pragma once
+
+// STD/STL
+#include <string>
+
+// Base Class
+#include "../detail/PrimitiveSerializerBase.h"
+
+// ArmarX
+#include "../../../../../navigator/type/primitive/Float.h"
+
+namespace armarx::aron::cppserializer::serializer
+{
+ class FloatSerializer;
+ typedef std::shared_ptr<FloatSerializer> FloatSerializerPtr;
+
+ class FloatSerializer :
+ virtual public detail::PrimitiveSerializerBase<typenavigator::FloatNavigator, FloatSerializer>
+ {
+ public:
+ /* constructors */
+ FloatSerializer(const typenavigator::FloatNavigatorPtr& e);
+
+ /* virtual implementations */
+ virtual CppBlockPtr getWriteTypeBlock(const std::string& accessor) const override;
+ };
+}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/Int.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/Int.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..7458faa2858bd545c77d9c7014556632bdc0c196
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/Int.cpp
@@ -0,0 +1,48 @@
+/*
+ * 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 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
+ */
+
+
+// STD/STL
+#include <string>
+#include <map>
+
+// Header
+#include "Int.h"
+
+namespace armarx::aron::cppserializer::serializer
+{
+ /* constructors */
+ IntSerializer::IntSerializer(const typenavigator::IntNavigatorPtr& e) :
+ detail::PrimitiveSerializerBase<typenavigator::IntNavigator, IntSerializer>("int", simox::meta::get_type_name(typeid(data::AronInt)), simox::meta::get_type_name(typeid(type::AronInt)), e)
+ {
+ ARMARX_CHECK_NOT_NULL(typenavigator);
+ }
+
+ /* virtual implementations */
+ CppBlockPtr IntSerializer::getWriteTypeBlock(const std::string& accessor) const
+ {
+ CppBlockPtr b = CppBlockPtr(new CppBlock());
+ b->addLine("w.writeInt({" + MAYBE_TO_STRING(typenavigator->getMaybe()) + "}); // of " + accessor);
+ return b;
+ }
+}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/Int.h b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/Int.h
new file mode 100644
index 0000000000000000000000000000000000000000..7bb6311bdd60de2374a11b37741070da3e8c737a
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/Int.h
@@ -0,0 +1,50 @@
+/*
+ * 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 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
+ */
+
+#pragma once
+
+// STD/STL
+#include <string>
+
+// Base Class
+#include "../detail/PrimitiveSerializerBase.h"
+
+// ArmarX
+#include "../../../../../navigator/type/primitive/Int.h"
+
+namespace armarx::aron::cppserializer::serializer
+{
+ class IntSerializer;
+ typedef std::shared_ptr<IntSerializer> IntSerializerPtr;
+
+ class IntSerializer :
+ virtual public detail::PrimitiveSerializerBase<typenavigator::IntNavigator, IntSerializer>
+ {
+ public:
+ /* constructors */
+ IntSerializer(const typenavigator::IntNavigatorPtr& e);
+
+ /* virtual implementations */
+ virtual CppBlockPtr getWriteTypeBlock(const std::string& accessor) const override;
+ };
+}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/Long.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/Long.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..754f4818ddd5f08dc224a62ebfbb95cf89e1673a
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/Long.cpp
@@ -0,0 +1,48 @@
+/*
+ * 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 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
+ */
+
+
+// STD/STL
+#include <string>
+#include <map>
+
+// Header
+#include "Long.h"
+
+namespace armarx::aron::cppserializer::serializer
+{
+ /* constructors */
+ LongSerializer::LongSerializer(const typenavigator::LongNavigatorPtr& e) :
+ detail::PrimitiveSerializerBase<typenavigator::LongNavigator, LongSerializer>("long", simox::meta::get_type_name(typeid(data::AronLong)), simox::meta::get_type_name(typeid(type::AronLong)), e)
+ {
+ ARMARX_CHECK_NOT_NULL(typenavigator);
+ }
+
+ /* virtual implementations */
+ CppBlockPtr LongSerializer::getWriteTypeBlock(const std::string& accessor) const
+ {
+ CppBlockPtr b = CppBlockPtr(new CppBlock());
+ b->addLine("w.writeLong({" + MAYBE_TO_STRING(typenavigator->getMaybe()) + "}); // of " + accessor);
+ return b;
+ }
+}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/Long.h b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/Long.h
new file mode 100644
index 0000000000000000000000000000000000000000..5d0d0948ee029b6d5718f9ee740f7ae2b0d99533
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/Long.h
@@ -0,0 +1,50 @@
+/*
+ * 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 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
+ */
+
+#pragma once
+
+// STD/STL
+#include <string>
+
+// Base Class
+#include "../detail/PrimitiveSerializerBase.h"
+
+// ArmarX
+#include "../../../../../navigator/type/primitive/Long.h"
+
+namespace armarx::aron::cppserializer::serializer
+{
+ class LongSerializer;
+ typedef std::shared_ptr<LongSerializer> LongSerializerPtr;
+
+ class LongSerializer :
+ virtual public detail::PrimitiveSerializerBase<typenavigator::LongNavigator, LongSerializer>
+ {
+ public:
+ /* constructors */
+ LongSerializer(const typenavigator::LongNavigatorPtr& e);
+
+ /* virtual implementations */
+ virtual CppBlockPtr getWriteTypeBlock(const std::string& accessor) const override;
+ };
+}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/Primitive.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/Primitive.cpp
deleted file mode 100644
index 28c002f1ccb06aa6e9484df1c15c40c2c9e55615..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/Primitive.cpp
+++ /dev/null
@@ -1,125 +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 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
- */
-
-
-// STD/STL
-#include <string>
-#include <map>
-
-// Header
-#include "Primitive.h"
-
-namespace armarx::aron::cppcodegenerator::serializer
-{
-
- namespace
- {
- std::string convertStdString(const std::string& s)
- {
- if (s == "string")
- {
- return "std::string";
- }
- return s;
- }
- }
-#define RUN_ARON_MACRO(upperType, lowerType, capsType, upperData, lowerData, capsData) \
- /* constructors */ \
- upperType##Serializer::upperType##Serializer(const typenavigator::upperType##NavigatorPtr& e) : \
- Serializer(convertStdString(#lowerData), simox::meta::get_type_name(typeid(data::Aron##upperData)), simox::meta::get_type_name(typeid(type::Aron##upperType))), \
- typenavigator(e) \
- { \
- \
- } \
- \
- /* virtual implementations */ \
- std::vector<CppFieldPtr> upperType##Serializer::getPublicVariableDeclarations(const std::string& name) const \
- { \
- CppFieldPtr field = CppFieldPtr(new CppField(getFullCppTypename(), name)); \
- return {field}; \
- } \
- \
- std::vector<std::pair<std::string, std::string>> upperType##Serializer::getCtorInitializers(const std::string&) const \
- { \
- return {}; \
- } \
- \
- CppBlockPtr upperType##Serializer::getCtorBlock(const std::string&) const \
- { \
- CppBlockPtr b = CppBlockPtr(new CppBlock()); \
- return b; \
- } \
- \
- CppBlockPtr upperType##Serializer::getResetBlock(const std::string& accessor) const \
- { \
- CppBlockPtr b = CppBlockPtr(new CppBlock()); \
- b->addLine(accessor + " = {};"); \
- return b; \
- } \
- \
- CppBlockPtr upperType##Serializer::getInitializeBlock(const std::string& accessor) const \
- { \
- CppBlockPtr b = CppBlockPtr(new CppBlock()); \
- b->addLine(accessor + " = {};"); \
- return b; \
- } \
- \
- CppBlockPtr upperType##Serializer::getWriteInitialTypeBlock(const std::string&) const \
- { \
- CppBlockPtr b = CppBlockPtr(new CppBlock()); \
- b->addLine("w.write" + std::string(#upperType) + "();"); \
- return b; \
- } \
- \
- CppBlockPtr upperType##Serializer::getWriteCurrentTypeBlock(const std::string&) const \
- { \
- CppBlockPtr b = CppBlockPtr(new CppBlock()); \
- b->addLine("w.write" + std::string(#upperType) + "();"); \
- return b; \
- } \
- \
- CppBlockPtr upperType##Serializer::getWriteBlock(const std::string& accessor) const \
- { \
- CppBlockPtr b = CppBlockPtr(new CppBlock()); \
- b->addLine("w.writePrimitive(" + accessor + ");"); \
- return b; \
- } \
- \
- CppBlockPtr upperType##Serializer::getReadBlock(const std::string& accessor) const \
- { \
- CppBlockPtr b = CppBlockPtr(new CppBlock()); \
- b->addLine("r.readPrimitive("+accessor+");"); \
- return b; \
- } \
- \
- CppBlockPtr upperType##Serializer::getEqualsBlock(const std::string& accessor, const std::string& otherInstanceAccessor) const \
- { \
- CppBlockPtr b = CppBlockPtr(new CppBlock()); \
- b->addLine("if (not (" + accessor + " == " + otherInstanceAccessor + "))"); \
- b->addLine("\t return false;"); \
- return b; \
- }
-
- HANDLE_PRIMITIVE_CORRESPONDING
-#undef RUN_ARON_MACRO
-}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/Primitive.h b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/Primitive.h
deleted file mode 100644
index 4356f6757d20c4ba68b603d49e98d51f9aa2a3e8..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/Primitive.h
+++ /dev/null
@@ -1,72 +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 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
- */
-
-#pragma once
-
-// STD/STL
-#include <string>
-
-// Base Class
-#include <RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/Serializer.h>
-
-// ArmarX
-#include <RobotAPI/libraries/aron/core/Concepts.h>
-#include <RobotAPI/libraries/aron/core/navigator/type/primitive/Primitive.h>
-
-namespace armarx::aron::cppcodegenerator::serializer
-{
-
-#define RUN_ARON_MACRO(upperType, lowerType, capsType) \
- class upperType##Serializer; \
- typedef std::shared_ptr<upperType##Serializer> upperType##SerializerPtr; \
- \
- class upperType##Serializer : \
- virtual public Serializer \
- { \
- public: \
- using PointerType = upperType##SerializerPtr; \
- \
- public: \
- /* constructors */ \
- upperType##Serializer(const typenavigator::upperType##NavigatorPtr& e); \
- \
- /* virtual implementations */ \
- virtual std::vector<CppFieldPtr> getPublicVariableDeclarations(const std::string& name) const override; \
- virtual std::vector<std::pair<std::string, std::string>> getCtorInitializers(const std::string&) const override; \
- virtual CppBlockPtr getCtorBlock(const std::string&) const override; \
- virtual CppBlockPtr getResetBlock(const std::string& accessor) const override; \
- virtual CppBlockPtr getInitializeBlock(const std::string& accessor) const override; \
- virtual CppBlockPtr getWriteCurrentTypeBlock(const std::string&) const override; \
- virtual CppBlockPtr getWriteInitialTypeBlock(const std::string& accessor) const override; \
- virtual CppBlockPtr getWriteBlock(const std::string& accessor) const override; \
- virtual CppBlockPtr getReadBlock(const std::string& accessor) const override; \
- CppBlockPtr getEqualsBlock(const std::string& accessor, const std::string& otherInstanceAccessor) const override; \
- \
- private: \
- /* members */ \
- typenavigator::upperType##NavigatorPtr typenavigator; \
- };
-
- HANDLE_PRIMITIVE_TYPES
-#undef RUN_ARON_MACRO
-}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/String.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/String.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..89e3603ee8afcb7ca786097d391c21e4478b3bb3
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/String.cpp
@@ -0,0 +1,48 @@
+/*
+ * 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 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
+ */
+
+
+// STD/STL
+#include <string>
+#include <map>
+
+// Header
+#include "String.h"
+
+namespace armarx::aron::cppserializer::serializer
+{
+ /* constructors */
+ StringSerializer::StringSerializer(const typenavigator::StringNavigatorPtr& e) :
+ detail::PrimitiveSerializerBase<typenavigator::StringNavigator, StringSerializer>("std::string", simox::meta::get_type_name(typeid(data::AronString)), simox::meta::get_type_name(typeid(type::AronString)), e)
+ {
+ ARMARX_CHECK_NOT_NULL(typenavigator);
+ }
+
+ /* virtual implementations */
+ CppBlockPtr StringSerializer::getWriteTypeBlock(const std::string& accessor) const
+ {
+ CppBlockPtr b = CppBlockPtr(new CppBlock());
+ b->addLine("w.writeString({" + MAYBE_TO_STRING(typenavigator->getMaybe()) + "}); // of " + accessor);
+ return b;
+ }
+}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/String.h b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/String.h
new file mode 100644
index 0000000000000000000000000000000000000000..d6bc43272efabf2b93a55bce3eec77e68274afd4
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/String.h
@@ -0,0 +1,50 @@
+/*
+ * 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 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
+ */
+
+#pragma once
+
+// STD/STL
+#include <string>
+
+// Base Class
+#include "../detail/PrimitiveSerializerBase.h"
+
+// ArmarX
+#include "../../../../../navigator/type/primitive/String.h"
+
+namespace armarx::aron::cppserializer::serializer
+{
+ class StringSerializer;
+ typedef std::shared_ptr<StringSerializer> StringSerializerPtr;
+
+ class StringSerializer :
+ virtual public detail::PrimitiveSerializerBase<typenavigator::StringNavigator, StringSerializer>
+ {
+ public:
+ /* constructors */
+ StringSerializer(const typenavigator::StringNavigatorPtr& e);
+
+ /* virtual implementations */
+ virtual CppBlockPtr getWriteTypeBlock(const std::string& accessor) const override;
+ };
+}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/Time.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/Time.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..e76d5e1ba97eea373fa1eb9af90ec1f3dc5838cf
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/Time.cpp
@@ -0,0 +1,69 @@
+/*
+ * 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 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
+ */
+
+
+// STD/STL
+#include <string>
+#include <map>
+
+// Header
+#include "Time.h"
+
+namespace armarx::aron::cppserializer::serializer
+{
+ /* constructors */
+ TimeSerializer::TimeSerializer(const typenavigator::TimeNavigatorPtr& e) :
+ detail::PrimitiveSerializerBase<typenavigator::TimeNavigator, TimeSerializer>("IceUtil::Time", simox::meta::get_type_name(typeid(data::AronLong)), simox::meta::get_type_name(typeid(type::AronTime)), e)
+ {
+ ARMARX_CHECK_NOT_NULL(typenavigator);
+ }
+
+ /* virtual implementations */
+ CppBlockPtr TimeSerializer::getWriteTypeBlock(const std::string& accessor) const
+ {
+ CppBlockPtr b = CppBlockPtr(new CppBlock());
+ b->addLine("w.writeTime({" + MAYBE_TO_STRING(typenavigator->getMaybe()) + "}); // of " + accessor);
+ return b;
+ }
+
+ CppBlockPtr TimeSerializer::getWriteBlock(const std::string& accessor) const
+ {
+ CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
+ std::string resolved_accessor = this->ResolveMaybeAccessor(accessor, this->typenavigator);
+ block_if_data->addLine("w.writePrimitive(" + resolved_accessor + nextEl() + "toMicroSeconds()); // of " + accessor);
+
+ return this->ResolveMaybeWriteBlock(accessor, block_if_data, this->typenavigator);
+ }
+
+ CppBlockPtr TimeSerializer::getReadBlock(const std::string& accessor) const
+ {
+ CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
+ std::string escaped_accessor = EscapeAccessor(accessor);
+ std::string resolved_accessor = ResolveMaybeAccessor(accessor, typenavigator);
+ block_if_data->addLine("long " + escaped_accessor + TIME_SETTER_ACCESSOR_SUFFIX + ";");
+ block_if_data->addLine("r.readEndPrimitive(" + escaped_accessor + TIME_SETTER_ACCESSOR_SUFFIX + "); // of " + accessor);
+ block_if_data->addLine(resolved_accessor + " = IceUtil::Time::microSeconds(" + escaped_accessor + TIME_SETTER_ACCESSOR_SUFFIX + ");");
+ return this->ResolveMaybeReadBlock(accessor, "r.readStartPrimitive()", block_if_data, typenavigator);
+ }
+
+}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/Time.h b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/Time.h
new file mode 100644
index 0000000000000000000000000000000000000000..ff06aff3e934bd23aa52d6b86bf3f45a403bf1ef
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/Time.h
@@ -0,0 +1,55 @@
+/*
+ * 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 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
+ */
+
+#pragma once
+
+// STD/STL
+#include <string>
+
+// Base Class
+#include "../detail/PrimitiveSerializerBase.h"
+
+// ArmarX
+#include "../../../../../navigator/type/primitive/Time.h"
+
+namespace armarx::aron::cppserializer::serializer
+{
+ class TimeSerializer;
+ typedef std::shared_ptr<TimeSerializer> TimeSerializerPtr;
+
+ class TimeSerializer :
+ virtual public detail::PrimitiveSerializerBase<typenavigator::TimeNavigator, TimeSerializer>
+ {
+ public:
+ /* constructors */
+ TimeSerializer(const typenavigator::TimeNavigatorPtr& e);
+
+ /* virtual implementations */
+ virtual CppBlockPtr getWriteTypeBlock(const std::string& accessor) const override;
+ virtual CppBlockPtr getWriteBlock(const std::string& accessor) const override;
+ virtual CppBlockPtr getReadBlock(const std::string& accessor) const override;
+
+ private:
+ static const constexpr char* TIME_SETTER_ACCESSOR_SUFFIX = "__time_setter";
+ };
+}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/toplevel/IntEnumClass.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/toplevel/IntEnumClass.cpp
index b038334fbb5dcc6ac39206c33174320f6b8e44d0..6998d81b564e532f5f174c8c07db8b4b79b1f703 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/toplevel/IntEnumClass.cpp
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/toplevel/IntEnumClass.cpp
@@ -24,14 +24,17 @@
// Header
#include "IntEnumClass.h"
-namespace armarx::aron::cppcodegenerator::serializer
+namespace armarx::aron::cppserializer::serializer
{
// constructors
IntEnumClassSerializer::IntEnumClassSerializer(const typenavigator::IntEnumNavigatorPtr& n) :
- Serializer(n->getEnumName(), simox::meta::get_type_name(typeid(data::AronNDArray)), simox::meta::get_type_name(typeid(type::AronIntEnum))),
- navigator(n),
- enumName("__ImplEnum")
+ detail::SerializerBase<typenavigator::IntEnumNavigator, IntEnumClassSerializer>(n->getEnumName(), simox::meta::get_type_name(typeid(data::AronNDArray)), simox::meta::get_type_name(typeid(type::AronIntEnum)), n)
{
+ ARMARX_CHECK_NOT_NULL(typenavigator);
+ if (typenavigator->getMaybe() != type::Maybe::eNone)
+ {
+ throw error::MaybeNotValidException("IntEnumClassSerializer", "IntEnumClassSerializer", "Somehow the maybe flag of a top level int enum declaration is set. This is not valid!", typenavigator->getMaybe(), typenavigator->getPath());
+ }
}
std::vector<CppFieldPtr> IntEnumClassSerializer::getPublicVariableDeclarations(const std::string&) const
@@ -46,10 +49,10 @@ namespace armarx::aron::cppcodegenerator::serializer
name_to_enum << "{" << std::endl;
enum_to_value << "{" << std::endl;
value_to_enum << "{" << std::endl;
- for (const auto& [key, value] : navigator->getAcceptedValues())
+ for (const auto& [key, value] : typenavigator->getAcceptedValueMap())
{
- std::string enumKeyWithNamespace = enumName + "::" + key;
- fields.push_back(std::make_shared<CppField>("const static " + enumName, key + " = " + enumKeyWithNamespace));
+ std::string enumKeyWithNamespace = std::string(IMPL_ENUM) + "::" + key;
+ fields.push_back(std::make_shared<CppField>("const static " + std::string(IMPL_ENUM), key + " = " + enumKeyWithNamespace));
enum_to_name << "\t\t{" << enumKeyWithNamespace << ", \"" << key << "\"}," << std::endl;
name_to_enum << "\t\t{\"" << key << "\", " << enumKeyWithNamespace << "}," << std::endl;
@@ -63,77 +66,59 @@ namespace armarx::aron::cppcodegenerator::serializer
enum_to_value << "\t}";
value_to_enum << "\t}";
- fields.push_back(std::make_shared<CppField>("const std::map<" + enumName + ", std::string>", "EnumToStringMap", enum_to_name.str()));
- fields.push_back(std::make_shared<CppField>("const std::map<std::string, " + enumName + ">", "StringToEnumMap", name_to_enum.str()));
- fields.push_back(std::make_shared<CppField>("const std::map<" + enumName + ", int>", "EnumToValueMap", enum_to_value.str()));
- fields.push_back(std::make_shared<CppField>("const std::map<int, " + enumName + ">", "ValueToEnumMap", value_to_enum.str()));
+ fields.push_back(std::make_shared<CppField>("const std::map<" + std::string(IMPL_ENUM) + ", std::string>", "EnumToStringMap", enum_to_name.str(), "Mapping enum values to readable strings"));
+ fields.push_back(std::make_shared<CppField>("const std::map<std::string, " + std::string(IMPL_ENUM) + ">", "StringToEnumMap", name_to_enum.str(), "Mapping readable strings to enum values"));
+ fields.push_back(std::make_shared<CppField>("const std::map<" + std::string(IMPL_ENUM) + ", int>", "EnumToValueMap", enum_to_value.str(), "Mapping enum values to a int value"));
+ fields.push_back(std::make_shared<CppField>("const std::map<int, " + std::string(IMPL_ENUM) + ">", "ValueToEnumMap", value_to_enum.str(), "Mapping int values to a enum"));
- fields.push_back(std::make_shared<CppField>(enumName, "value"));
+ fields.push_back(std::make_shared<CppField>(std::string(IMPL_ENUM), "value", "", "The current value of the enum object"));
return fields;
}
- std::vector<std::pair<std::string, std::string>> IntEnumClassSerializer::getCtorInitializers(const std::string&) const
+ CppBlockPtr IntEnumClassSerializer::getResetSoftBlock(const std::string& accessor) const
{
- return {};
+ CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
+ block_if_data->addLine("value = {};");
+ return ResolveMaybeResetSoftBlock(accessor, block_if_data, typenavigator);
}
- CppBlockPtr IntEnumClassSerializer::getCtorBlock(const std::string&) const
+ CppBlockPtr IntEnumClassSerializer::getResetHardBlock(const std::string& accessor) const
{
- CppBlockPtr b = CppBlockPtr(new CppBlock());
- return b;
+ CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
+ block_if_data->addLine("value = {};");
+ return ResolveMaybeResetSoftBlock(accessor, block_if_data, typenavigator);
}
- CppBlockPtr IntEnumClassSerializer::getResetBlock(const std::string&) const
+ CppBlockPtr IntEnumClassSerializer::getWriteTypeBlock(const std::string&) const
{
CppBlockPtr b = CppBlockPtr(new CppBlock());
- b->addLine("value = {};");
+ b->addLine("w.writeInt({__aronMaybeType}); // of top level enum " + getCoreCppTypename());
return b;
}
- CppBlockPtr IntEnumClassSerializer::getWriteInitialTypeBlock(const std::string&) const
+ CppBlockPtr IntEnumClassSerializer::getWriteBlock(const std::string& accessor) const
{
- CppBlockPtr b = CppBlockPtr(new CppBlock());
- b->addLine("w.writeInt();");
- return b;
+ CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
+ block_if_data->addLine("w.writePrimitive(EnumToValueMap.at(value)); // of top level enum " + getCoreCppTypename());
+ return ResolveMaybeWriteBlock(accessor, block_if_data, typenavigator);
}
- CppBlockPtr IntEnumClassSerializer::getWriteCurrentTypeBlock(const std::string&) const
+ CppBlockPtr IntEnumClassSerializer::getReadBlock(const std::string& accessor) const
{
- CppBlockPtr b = CppBlockPtr(new CppBlock());
- b->addLine("w.writeInt();");
- return b;
- }
-
- CppBlockPtr IntEnumClassSerializer::getInitializeBlock(const std::string&) const
- {
- CppBlockPtr b = CppBlockPtr(new CppBlock());
- b->addLine("value = {};");
- return b;
+ CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
+ block_if_data->addLine("int temporary;");
+ block_if_data->addLine("r.readEndPrimitive(temporary); // of top level enum " + getCoreCppTypename());
+ block_if_data->addLine("value = ValueToEnumMap.at(temporary);");
+ return ResolveMaybeReadBlock(accessor, "r.readStartPrimitive()", block_if_data, typenavigator);
}
- CppBlockPtr IntEnumClassSerializer::getWriteBlock(const std::string&) const
+ CppBlockPtr IntEnumClassSerializer::getEqualsBlock(const std::string& accessor, const std::string& otherInstanceAccessor) const
{
- CppBlockPtr b = CppBlockPtr(new CppBlock());
- b->addLine("w.writePrimitive(EnumToValueMap.at(value));");
- return b;
- }
-
- CppBlockPtr IntEnumClassSerializer::getReadBlock(const std::string&) const
- {
- CppBlockPtr b = CppBlockPtr(new CppBlock());
- b->addLine("int temporary;");
- b->addLine("r.readPrimitive(temporary);");
- b->addLine("value = ValueToEnumMap.at(temporary);");
- return b;
- }
-
- CppBlockPtr IntEnumClassSerializer::getEqualsBlock(const std::string&, const std::string& otherInstanceAccessor) const
- {
- CppBlockPtr b = CppBlockPtr(new CppBlock());
- b->addLine("if (not (value == " + otherInstanceAccessor + ".value))");
- b->addLine("\t return false;");
- return b;
+ CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
+ block_if_data->addLine("if (not (value == " + otherInstanceAccessor + ".value))");
+ block_if_data->addLine("\t return false;");
+ return ResolveMaybeEqualsBlock(accessor, otherInstanceAccessor, block_if_data, typenavigator);
}
@@ -147,7 +132,7 @@ namespace armarx::aron::cppcodegenerator::serializer
CppCtorPtr IntEnumClassSerializer::toInnerEnumCtor(const std::string& name) const
{
- CppCtorPtr c = std::make_shared<CppCtor>(name + "(const " + enumName + " e)");
+ CppCtorPtr c = std::make_shared<CppCtor>(name + "(const " + std::string(IMPL_ENUM) + " e)");
std::vector<std::pair<std::string, std::string>> initList = {{"value", "e"}};
c->addInitListEntries(initList);
return c;
@@ -155,8 +140,8 @@ namespace armarx::aron::cppcodegenerator::serializer
CppEnumPtr IntEnumClassSerializer::toInnerEnumDefinition() const
{
- CppEnumPtr e = std::make_shared<CppEnum>(enumName, "The internal enum definition of the enum of this autogenerated class.");
- for (const auto& [key, value] : navigator->getAcceptedValues())
+ CppEnumPtr e = std::make_shared<CppEnum>(std::string(IMPL_ENUM), "The internal enum definition of the enum of this autogenerated class.");
+ for (const auto& [key, value] : typenavigator->getAcceptedValueMap())
{
e->addField(std::make_shared<CppEnumField>(key));
}
@@ -185,6 +170,7 @@ namespace armarx::aron::cppcodegenerator::serializer
CppMethodPtr m = CppMethodPtr(new CppMethod(getFullCppTypename() + "& operator=(const " + getFullCppTypename() + "& c)", doc.str()));
CppBlockPtr b = std::make_shared<CppBlock>();
b->addLine("value = c.value;");
+ b->addLine("return *this;");
m->setBlock(b);
return m;
}
@@ -195,9 +181,10 @@ namespace armarx::aron::cppcodegenerator::serializer
doc << "@brief operator=() - Assignment operator for the internally defined enum \n";
doc << "@return - nothing";
- CppMethodPtr m = CppMethodPtr(new CppMethod(getFullCppTypename() + "& operator=(" + enumName + " v)", doc.str()));
+ CppMethodPtr m = CppMethodPtr(new CppMethod(getFullCppTypename() + "& operator=(" + std::string(IMPL_ENUM) + " v)", doc.str()));
CppBlockPtr b = std::make_shared<CppBlock>();
b->addLine("value = v;");
+ b->addLine("return *this;");
m->setBlock(b);
return m;
}
@@ -208,7 +195,7 @@ namespace armarx::aron::cppcodegenerator::serializer
doc << "@brief operator=() - Assignment operator for the internally defined enum \n";
doc << "@return - nothing";
- CppMethodPtr m = CppMethodPtr(new CppMethod("void operator=(int v)", doc.str()));
+ CppMethodPtr m = CppMethodPtr(new CppMethod(getFullCppTypename() + "& operator=(int v)", doc.str()));
CppBlockPtr b = std::make_shared<CppBlock>();
b->addLine("if (auto it = ValueToEnumMap.find(v); it == ValueToEnumMap.end())");
CppBlockPtr b2 = std::make_shared<CppBlock>();
@@ -218,6 +205,7 @@ namespace armarx::aron::cppcodegenerator::serializer
CppBlockPtr b3 = std::make_shared<CppBlock>();
b3->addLine("value = it->second;");
b->addBlock(b3);
+ b->addLine("return *this;");
m->setBlock(b);
return m;
}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/toplevel/IntEnumClass.h b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/toplevel/IntEnumClass.h
index cf5b46c25b102ce0d2e49d4e971a6c5935f50000..8bc3ce9e21e220fc34c710b7bb5a7da59521dd60 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/toplevel/IntEnumClass.h
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/toplevel/IntEnumClass.h
@@ -28,35 +28,29 @@
#include <map>
// Base Class
-#include <RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/Serializer.h>
+#include "../detail/SerializerBase.h"
// ArmarX
-#include <RobotAPI/libraries/aron/core/navigator/type/enum/IntEnum.h>
+#include "../../../../../navigator/type/enum/IntEnum.h"
-namespace armarx::aron::cppcodegenerator::serializer
+namespace armarx::aron::cppserializer::serializer
{
class IntEnumClassSerializer;
typedef std::shared_ptr<IntEnumClassSerializer> IntEnumClassSerializerPtr;
class IntEnumClassSerializer :
- virtual public Serializer
+ virtual public detail::SerializerBase<typenavigator::IntEnumNavigator, IntEnumClassSerializer>
{
- public:
- using PointerType = IntEnumClassSerializerPtr;
-
public:
// constructors
IntEnumClassSerializer(const typenavigator::IntEnumNavigatorPtr&);
// virtual implementations
std::vector<CppFieldPtr> getPublicVariableDeclarations(const std::string&) const;
- virtual std::vector<std::pair<std::string, std::string>> getCtorInitializers(const std::string&) const override;
- virtual CppBlockPtr getCtorBlock(const std::string&) const override;
- virtual CppBlockPtr getWriteInitialTypeBlock(const std::string&) const override;
- virtual CppBlockPtr getWriteCurrentTypeBlock(const std::string&) const override;
- virtual CppBlockPtr getInitializeBlock(const std::string&) const override;
- virtual CppBlockPtr getResetBlock(const std::string&) const override;
+ virtual CppBlockPtr getResetHardBlock(const std::string&) const override;
+ virtual CppBlockPtr getResetSoftBlock(const std::string&) const override;
+ virtual CppBlockPtr getWriteTypeBlock(const std::string&) const override;
virtual CppBlockPtr getWriteBlock(const std::string&) const override;
virtual CppBlockPtr getReadBlock(const std::string&) const override;
virtual CppBlockPtr getEqualsBlock(const std::string&, const std::string&) const override;
@@ -75,8 +69,6 @@ namespace armarx::aron::cppcodegenerator::serializer
private:
// Members
static const std::map<std::string, std::pair<std::string, int>> ACCEPTED_TYPES;
-
- typenavigator::IntEnumNavigatorPtr navigator;
- std::string enumName;
+ static constexpr const char* IMPL_ENUM = "__ImplEnum";
};
}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/toplevel/ObjectClass.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/toplevel/ObjectClass.cpp
index 25abfc6f9c848d3e78dbcb67e738ac80d2f8672b..e674c2445e24fa91af875bb81fbcd76e1d6d39d6 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/toplevel/ObjectClass.cpp
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/toplevel/ObjectClass.cpp
@@ -25,20 +25,23 @@
#include "ObjectClass.h"
-namespace armarx::aron::cppcodegenerator::serializer
+namespace armarx::aron::cppserializer::serializer
{
// constructors
ObjectClassSerializer::ObjectClassSerializer(const typenavigator::ObjectNavigatorPtr& e) :
- Serializer(e->getObjectName(), simox::meta::get_type_name(typeid(data::AronDict)), simox::meta::get_type_name(typeid(type::AronObject))),
- navigator(e)
+ detail::SerializerBase<typenavigator::ObjectNavigator, ObjectClassSerializer>(e->getObjectName(), simox::meta::get_type_name(typeid(data::AronDict)), simox::meta::get_type_name(typeid(type::AronObject)), e)
{
- //AddObjectTypeToAllObjectTypesList(ObjectNavigatorPtr(this));
+ ARMARX_CHECK_NOT_NULL(typenavigator);
+ if (typenavigator->getMaybe() != type::Maybe::eNone)
+ {
+ throw error::MaybeNotValidException("ObjectClassSerializer", "ObjectClassSerializer", "Somehow the maybe flag of a top level object declaration is set. This is not valid!", typenavigator->getMaybe(), typenavigator->getPath());
+ }
}
std::vector<CppFieldPtr> ObjectClassSerializer::getPublicVariableDeclarations(const std::string&) const
{
std::vector<CppFieldPtr> fields;
- for (const auto& [key, member] : navigator->getMemberTypes())
+ for (const auto& [key, member] : typenavigator->getMemberTypes())
{
auto member_s = FromAronTypeNaviagtorPtr(member);
std::vector<CppFieldPtr> member_fields = member_s->getPublicVariableDeclarations(key);
@@ -58,142 +61,127 @@ namespace armarx::aron::cppcodegenerator::serializer
return b;
}
- CppBlockPtr ObjectClassSerializer::getResetBlock(const std::string&) const
+ CppBlockPtr ObjectClassSerializer::getResetSoftBlock(const std::string& accessor) const
{
- CppBlockPtr b = CppBlockPtr(new CppBlock());
- if (navigator->getExtends() != nullptr)
+ CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
+ if (typenavigator->getExtends() != nullptr)
{
- const auto extends_s = FromAronTypeNaviagtorPtr(navigator->getExtends());
- b->addLine(extends_s->getFullCppTypename() + "::reset();");
+ const auto extends_s = FromAronTypeNaviagtorPtr(typenavigator->getExtends());
+ block_if_data->addLine(extends_s->getFullCppTypename() + "::resetSoft();");
}
- for (const auto& [key, child] : navigator->getMemberTypes())
+ for (const auto& [key, child] : typenavigator->getMemberTypes())
{
auto child_s = FromAronTypeNaviagtorPtr(child);
- CppBlockPtr b2 = child_s->getResetBlock(key);
- b->appendBlock(b2);
+ CppBlockPtr b2 = child_s->getResetSoftBlock(key);
+ block_if_data->appendBlock(b2);
}
- return b;
+ return ResolveMaybeResetSoftBlock(accessor, block_if_data, typenavigator);
}
- CppBlockPtr ObjectClassSerializer::getInitializeBlock(const std::string&) const
+ CppBlockPtr ObjectClassSerializer::getResetHardBlock(const std::string& accessor) const
{
- CppBlockPtr b = CppBlockPtr(new CppBlock());
- if (navigator->getExtends() != nullptr)
+ CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
+ if (typenavigator->getExtends() != nullptr)
{
- const auto extends_s = FromAronTypeNaviagtorPtr(navigator->getExtends());
- b->addLine(extends_s->getFullCppTypename() + "::initialize();");
+ const auto extends_s = FromAronTypeNaviagtorPtr(typenavigator->getExtends());
+ block_if_data->addLine(extends_s->getFullCppTypename() + "::resetHard();");
}
- for (const auto& [key, child] : navigator->getMemberTypes())
+ for (const auto& [key, child] : typenavigator->getMemberTypes())
{
const auto child_s = FromAronTypeNaviagtorPtr(child);
- CppBlockPtr b2 = child_s->getInitializeBlock(key);
- b->appendBlock(b2);
+ CppBlockPtr b2 = child_s->getResetHardBlock(key);
+ block_if_data->appendBlock(b2);
}
- return b;
+ return ResolveMaybeResetHardBlock(accessor, block_if_data, typenavigator);
}
- CppBlockPtr ObjectClassSerializer::getWriteInitialTypeBlock(const std::string&) const
+ CppBlockPtr ObjectClassSerializer::getWriteTypeBlock(const std::string&) const
{
CppBlockPtr b = CppBlockPtr(new CppBlock());
- if (navigator->getExtends() != nullptr)
+ if (typenavigator->getExtends() != nullptr)
{
- const auto extends_s = FromAronTypeNaviagtorPtr(navigator->getExtends());
- b->addLine(extends_s->getFullCppTypename() + "::writeInitialType(w);");
+ const auto extends_s = FromAronTypeNaviagtorPtr(typenavigator->getExtends());
+ b->addLine(extends_s->getFullCppTypename() + "::writeType(w);");
}
- b->addLine("w.writeStartObject(\"" + navigator->getObjectName() + "\");");
- for (const auto& [key, child] : navigator->getMemberTypes())
+ b->addLine("w.writeStartObject({\"" + typenavigator->getObjectName() + "\", __aronMaybeType}); // of top level object " + getCoreCppTypename());
+ for (const auto& [key, child] : typenavigator->getMemberTypes())
{
const auto child_s = FromAronTypeNaviagtorPtr(child);
b->addLine("w.writeKey(\"" + key + "\");");
- CppBlockPtr b2 = child_s->getWriteInitialTypeBlock(child_s->getFullCppTypename());
+ CppBlockPtr b2 = child_s->getWriteTypeBlock(child_s->getCoreCppTypename());
b->appendBlock(b2);
}
- b->addLine("w.writeEndObject();");
- return b;
- }
-
- CppBlockPtr ObjectClassSerializer::getWriteCurrentTypeBlock(const std::string&) const
- {
- CppBlockPtr b = CppBlockPtr(new CppBlock());
- if (navigator->getExtends() != nullptr)
- {
- const auto extends_s = FromAronTypeNaviagtorPtr(navigator->getExtends());
- b->addLine(extends_s->getFullCppTypename() + "::writeCurrentType(w);");
- }
- b->addLine("w.writeStartObject(\"" + navigator->getObjectName() + "\");");
- for (const auto& [key, child] : navigator->getMemberTypes())
- {
- const auto child_s = FromAronTypeNaviagtorPtr(child);
- b->addLine("w.writeKey(\"" + key + "\");");
- CppBlockPtr b2 = child_s->getWriteCurrentTypeBlock(key);
- b->appendBlock(b2);
- }
- b->addLine("w.writeEndObject();");
+ b->addLine("w.writeEndObject(); // of top level object " + getCoreCppTypename());
return b;
}
- CppBlockPtr ObjectClassSerializer::getWriteBlock(const std::string&) const
+ CppBlockPtr ObjectClassSerializer::getWriteBlock(const std::string& accessor) const
{
- CppBlockPtr b = CppBlockPtr(new CppBlock());
- if (navigator->getExtends() != nullptr)
+ CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
+ if (typenavigator->getExtends() != nullptr)
{
- const auto extends_s = FromAronTypeNaviagtorPtr(navigator->getExtends());
- b->addLine(extends_s->getFullCppTypename() + "::write(w, aron_type);");
+ const auto extends_s = FromAronTypeNaviagtorPtr(typenavigator->getExtends());
+ block_if_data->addLine(extends_s->getFullCppTypename() + "::write(w, aron_type);");
}
- b->addLine("w.writeStartDict();");
- for (const auto& [key, child] : navigator->getMemberTypes())
+ block_if_data->addLine("w.writeStartDict(); // of top level object " + getCoreCppTypename());
+ for (const auto& [key, child] : typenavigator->getMemberTypes())
{
const auto child_s = FromAronTypeNaviagtorPtr(child);
- b->addLine("w.writeKey(\"" + key + "\");");
- CppBlockPtr b2 = child_s->getWriteBlock(key);
- b->appendBlock(b2);
+ CppBlockPtr child_b = CppBlockPtr(new CppBlock());
+
+ child_b->addLine("w.writeKey(\"" + key + "\");");
+ child_b->appendBlock(child_s->getWriteBlock(key));
+ block_if_data->appendBlock(child_b);
}
- b->addLine("w.writeEndDict();");
- return b;
+ block_if_data->addLine("w.writeEndDict(); // of top level object " + getCoreCppTypename());
+ return ResolveMaybeWriteBlock(accessor, block_if_data, typenavigator);
}
- CppBlockPtr ObjectClassSerializer::getReadBlock(const std::string&) const
+ CppBlockPtr ObjectClassSerializer::getReadBlock(const std::string& accessor) const
{
- CppBlockPtr b = CppBlockPtr(new CppBlock());
- if (navigator->getExtends() != nullptr)
+ CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
+ if (typenavigator->getExtends() != nullptr)
{
- const auto extends_s = FromAronTypeNaviagtorPtr(navigator->getExtends());
- b->addLine(extends_s->getFullCppTypename() + "::read(r);");
+ const auto extends_s = FromAronTypeNaviagtorPtr(typenavigator->getExtends());
+ block_if_data->addLine(extends_s->getFullCppTypename() + "::read(r);");
}
- b->addLine("r.readStartDict();");
- for (const auto& [key, child] : navigator->getMemberTypes())
+ block_if_data->addLine("if(!skip_first_readStartDict)");
+ auto readStartDict = std::make_shared<CppBlock>();
+ readStartDict->addLine("r.readStartDict(); // of top level object " + getCoreCppTypename());
+ block_if_data->addBlock(readStartDict);
+
+ for (const auto& [key, child] : typenavigator->getMemberTypes())
{
const auto child_s = FromAronTypeNaviagtorPtr(child);
- b->addLine("r.loadMember(\"" + key + "\");");
- CppBlockPtr b2 = child_s->getReadBlock(key);
- b->appendBlock(b2);
+ block_if_data->addLine("r.loadMember(\"" + key + "\");");
+ block_if_data->appendBlock(child_s->getReadBlock(key));
}
- b->addLine("r.readEndDict();");
- return b;
+ block_if_data->addLine("r.readEndDict(); // of top level object " + getCoreCppTypename());
+ return ResolveMaybeReadBlock(accessor, "", block_if_data, typenavigator);
}
- CppBlockPtr ObjectClassSerializer::getEqualsBlock(const std::string&, const std::string& otherInstanceAccessor) const
+ CppBlockPtr ObjectClassSerializer::getEqualsBlock(const std::string& accessor, const std::string& otherInstanceAccessor) const
{
- CppBlockPtr b = CppBlockPtr(new CppBlock());
- if (navigator->getExtends() != nullptr)
+ CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
+ if (typenavigator->getExtends() != nullptr)
{
- const auto extends_s = FromAronTypeNaviagtorPtr(navigator->getExtends());
- b->addLine("if (not (" + extends_s->getFullCppTypename() + "::operator== (" + otherInstanceAccessor + ")))");
- b->addLine("\t return false;");
+ const auto extends_s = FromAronTypeNaviagtorPtr(typenavigator->getExtends());
+ block_if_data->addLine("if (not (" + extends_s->getFullCppTypename() + "::operator== (" + otherInstanceAccessor + ")))");
+ block_if_data->addLine("\t return false;");
}
- for (const auto& [key, child] : navigator->getMemberTypes())
+ for (const auto& [key, child] : typenavigator->getMemberTypes())
{
auto child_s = FromAronTypeNaviagtorPtr(child);
CppBlockPtr b2 = child_s->getEqualsBlock(key, otherInstanceAccessor + "." + key);
- b->appendBlock(b2);
+ block_if_data->appendBlock(b2);
}
- return b;
+ return ResolveMaybeEqualsBlock(accessor, otherInstanceAccessor, block_if_data, typenavigator);
}
}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/toplevel/ObjectClass.h b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/toplevel/ObjectClass.h
index 7b996cb7625ab2968d515cb0db05060a85e0ce60..03f8817a5c4e64dfbf15e9a54e8e0f6ae88f4ff3 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/toplevel/ObjectClass.h
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/toplevel/ObjectClass.h
@@ -28,24 +28,21 @@
#include <map>
// Base Class
-#include <RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/Serializer.h>
+#include "../detail/SerializerBase.h"
// ArmarX
-#include <RobotAPI/libraries/aron/core/navigator/type/container/Object.h>
+#include "../../../../../navigator/type/container/Object.h"
-namespace armarx::aron::cppcodegenerator::serializer
+namespace armarx::aron::cppserializer::serializer
{
class ObjectClassSerializer;
typedef std::shared_ptr<ObjectClassSerializer> ObjectClassSerializerPtr;
class ObjectClassSerializer :
- virtual public Serializer
+ virtual public detail::SerializerBase<typenavigator::ObjectNavigator, ObjectClassSerializer>
{
- public:
- using PointerType = ObjectClassSerializerPtr;
-
public:
// constructors
ObjectClassSerializer(const typenavigator::ObjectNavigatorPtr&);
@@ -54,10 +51,9 @@ namespace armarx::aron::cppcodegenerator::serializer
std::vector<CppFieldPtr> getPublicVariableDeclarations(const std::string&) const;
virtual std::vector<std::pair<std::string, std::string>> getCtorInitializers(const std::string&) const override;
virtual CppBlockPtr getCtorBlock(const std::string&) const override;
- virtual CppBlockPtr getWriteInitialTypeBlock(const std::string&) const override;
- virtual CppBlockPtr getWriteCurrentTypeBlock(const std::string&) const override;
- virtual CppBlockPtr getInitializeBlock(const std::string&) const override;
- virtual CppBlockPtr getResetBlock(const std::string&) const override;
+ virtual CppBlockPtr getWriteTypeBlock(const std::string&) const override;
+ virtual CppBlockPtr getResetHardBlock(const std::string&) const override;
+ virtual CppBlockPtr getResetSoftBlock(const std::string&) const override;
virtual CppBlockPtr getWriteBlock(const std::string&) const override;
virtual CppBlockPtr getReadBlock(const std::string&) const override;
virtual CppBlockPtr getEqualsBlock(const std::string&, const std::string&) const override;
@@ -65,7 +61,5 @@ namespace armarx::aron::cppcodegenerator::serializer
private:
// members
static constexpr const char* EXTENDS_ITERATOR_ACCESSOR = "_extends";
-
- typenavigator::ObjectNavigatorPtr navigator;
};
}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/typeReader/xml/Data.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/typeReader/xml/Data.cpp
index 8b137891791fe96927ad78e64b0aad7bded08bdc..a593bad8cf3d2b9b2a08af3adc10ae478f56cf87 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/typeReader/xml/Data.cpp
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/typeReader/xml/Data.cpp
@@ -1 +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 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
+ */
+#include "Data.h"
+
+#include <ArmarXCore/core/rapidxml/wrapper/RapidXmlReader.h>
+
+#include <SimoxUtility/algorithm/string/string_tools.h>
+
+namespace armarx::aron::xmltypereader
+{
+
+ void Data::EnforceAttribute(const RapidXmlReaderNode& node, const std::string& att)
+ {
+ if (!HasAttribute(node, att))
+ {
+ throw error::StringNotValidException("XMLReaderData", "EnforceAttribute", "A <" + node.name() + ">-tag does not have the correct attribute", att);
+ }
+ }
+
+ bool Data::HasAttribute(const RapidXmlReaderNode& node, const std::string& att)
+ {
+ return node.has_attribute(att.c_str());
+ }
+
+ std::string Data::GetAttribute(const RapidXmlReaderNode& node, const std::string& att)
+ {
+ EnforceAttribute(node, att);
+ return node.attribute_value(att.c_str());
+ }
+
+ std::string Data::GetAttributeWithDefault(const armarx::RapidXmlReaderNode& node, const std::string& att, const std::string def)
+ {
+ if (!(HasAttribute(node, att)))
+ {
+ return def;
+ }
+ return node.attribute_value(att.c_str());
+ }
+
+ bool Data::AttributeIsTrue(const armarx::RapidXmlReaderNode& node, const std::string& att)
+ {
+ if (HasAttribute(node, att))
+ {
+ std::string v = simox::alg::to_lower(node.attribute_value(att.c_str()));
+ if (v == "1" or v == "true" or v == "wahr" or v == "yes" or v == "ja" or v == "")
+ {
+ return true;
+ }
+ }
+ return false;
+ }
+
+ bool Data::HasTagName(const armarx::RapidXmlReaderNode& node, const std::string& name)
+ {
+ return (simox::alg::to_lower(name) == simox::alg::to_lower(node.name()));
+ }
+
+ void Data::EnforceTagName(const armarx::RapidXmlReaderNode& node, const std::string& name)
+ {
+ if (!(HasTagName(node, name)))
+ {
+ throw error::StringNotValidException("XMLReaderData", "EnforceTagName", "The node <" + node.name() + "> has the wrong tag", name);
+ }
+ }
+
+ std::string Data::GetTagName(const armarx::RapidXmlReaderNode& node)
+ {
+ return simox::alg::to_lower(node.name());
+ }
+
+ bool Data::CheckMinChildSize(const armarx::RapidXmlReaderNode& node, const size_t size)
+ {
+ std::vector<RapidXmlReaderNode> children = node.nodes();
+ return children.size() >= size;
+ }
+
+ bool Data::CheckMaxChildSize(const armarx::RapidXmlReaderNode& node, const size_t size)
+ {
+ std::vector<RapidXmlReaderNode> children = node.nodes();
+ return children.size() <= size;
+ }
+
+ bool Data::CheckExactChildSize(const armarx::RapidXmlReaderNode& node, const size_t size)
+ {
+ std::vector<RapidXmlReaderNode> children = node.nodes();
+ return children.size() == size;
+ }
+
+ void Data::EnforceChildSize(const armarx::RapidXmlReaderNode& node, const size_t size)
+ {
+ if (!Data::CheckExactChildSize(node, size))
+ {
+ throw error::SizeNotValidException("XMLReaderData", "EnforceChildSize", "The node <" + node.name() + "> has the wrong number of children", node.nodes().size(), size);
+ }
+ }
+
+}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/typeReader/xml/Data.h b/source/RobotAPI/libraries/aron/core/codegenerator/typeReader/xml/Data.h
index 2f152c9e4d1d2925d8a0c5848f9529458b666604..4950d51f77149b7f192766d79144bfdfb7fea3c6 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/typeReader/xml/Data.h
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/typeReader/xml/Data.h
@@ -28,15 +28,13 @@
#include <map>
// ArmarX
-#include <ArmarXCore/core/rapidxml/wrapper/RapidXmlReader.h>
-
#include <RobotAPI/libraries/aron/core/Concepts.h>
+#include <RobotAPI/libraries/aron/core/navigator/type/AllNavigators.h>
-#include <RobotAPI/libraries/aron/core/navigator/type/Navigator.h>
-#include <RobotAPI/libraries/aron/core/navigator/type/container/Object.h>
-#include <RobotAPI/libraries/aron/core/navigator/type/container/List.h>
-#include <RobotAPI/libraries/aron/core/navigator/type/primitive/Primitive.h>
-
+namespace armarx
+{
+ class RapidXmlReaderNode;
+}
namespace armarx::aron::xmltypereader
{
@@ -52,10 +50,12 @@ namespace armarx::aron::xmltypereader
static constexpr const char* CODE_INCLUDES_TAG = "codeincludes";
static constexpr const char* INCLUDES_TAG = "aronincludes";
static constexpr const char* GENERATE_TYPES_TAG = "generatetypes";
+ static constexpr const char* AUTO_CODE_INCLUDE = "autoinclude";
static constexpr const char* INCLUDE_TAG = "include";
static constexpr const char* GENERATE_OBJECT_TAG = "object";
static constexpr const char* GENERATE_INT_ENUM_TAG = "intenum";
+ static constexpr const char* GENERATE_NDARRAY_TAG = "ndarray";
// Attribute names
static constexpr const char* METHOD_ATTRIBUTE_NAME = "method";
@@ -74,6 +74,10 @@ namespace armarx::aron::xmltypereader
static constexpr const char* ROWS_ATTRIBUTE_NAME = "rows";
static constexpr const char* COLS_ATTRIBUTE_NAME = "cols";
static constexpr const char* DIMENSIONS_ATTRIBUTE_NAME = "dimensions";
+ static constexpr const char* OPTIONAL_NAME = "optional";
+ static constexpr const char* RAW_PTR_NAME = "raw_ptr";
+ static constexpr const char* SHARED_PTR_NAME = "shared_ptr";
+ static constexpr const char* UNIQUE_PTR_NAME = "unique_ptr";
// Second level tags. Only important if in specific top level tag
static constexpr const char* OBJECT_CHILD_TAG = "objectchild";
@@ -86,76 +90,28 @@ namespace armarx::aron::xmltypereader
HANDLE_ALL_ARON_TYPES
#undef RUN_ARON_MACRO
- static void EnforceAttribute(const RapidXmlReaderNode& node, const std::string& att)
- {
- if (!HasAttribute(node, att))
- {
- throw error::StringNotValidException("XMLReaderData", "EnforceAttribute", "A <" + node.name() + ">-tag does not have the correct attribute", att);
- }
- }
-
- static bool HasAttribute(const RapidXmlReaderNode& node, const std::string& att)
- {
- return node.has_attribute(att.c_str());
- }
-
- static std::string GetAttribute(const RapidXmlReaderNode& node, const std::string& att)
- {
- EnforceAttribute(node, att);
- return node.attribute_value(att.c_str());
- }
-
- static std::string GetAttributeWithDefault(const RapidXmlReaderNode& node, const std::string& att, const std::string def)
- {
- if (!(HasAttribute(node, att)))
- {
- return def;
- }
- return node.attribute_value(att.c_str());
- }
-
- static bool HasTagName(const RapidXmlReaderNode& node, const std::string& name)
- {
- return (simox::alg::to_lower(name) == simox::alg::to_lower(node.name()));
- }
-
- static void EnforceTagName(const RapidXmlReaderNode& node, const std::string& name)
- {
- if (!(HasTagName(node, name)))
- {
- throw error::StringNotValidException("XMLReaderData", "EnforceTagName", "The node <" + node.name() + "> has the wrong tag", name);
- }
- }
-
- static std::string GetTagName(const RapidXmlReaderNode& node)
- {
- return simox::alg::to_lower(node.name());
- }
-
- static bool CheckMinChildSize(const RapidXmlReaderNode& node, const size_t size)
- {
- std::vector<RapidXmlReaderNode> children = node.nodes();
- return children.size() >= size;
- }
-
- static bool CheckMaxChildSize(const RapidXmlReaderNode& node, const size_t size)
- {
- std::vector<RapidXmlReaderNode> children = node.nodes();
- return children.size() <= size;
- }
-
- static bool CheckExactChildSize(const RapidXmlReaderNode& node, const size_t size)
- {
- std::vector<RapidXmlReaderNode> children = node.nodes();
- return children.size() == size;
- }
-
- static void EnforceChildSize(const RapidXmlReaderNode& node, const size_t size)
- {
- if (!Data::CheckExactChildSize(node, size))
- {
- throw error::SizeNotValidException("XMLReaderData", "EnforceChildSize", "The node <" + node.name() + "> has the wrong number of children", node.nodes().size(), size);
- }
- }
+ static void EnforceAttribute(const RapidXmlReaderNode& node, const std::string& att);
+
+ static bool HasAttribute(const RapidXmlReaderNode& node, const std::string& att);
+
+ static std::string GetAttribute(const RapidXmlReaderNode& node, const std::string& att);
+
+ static std::string GetAttributeWithDefault(const RapidXmlReaderNode& node, const std::string& att, const std::string def);
+
+ static bool AttributeIsTrue(const RapidXmlReaderNode& node, const std::string& att);
+
+ static bool HasTagName(const RapidXmlReaderNode& node, const std::string& name);
+
+ static void EnforceTagName(const RapidXmlReaderNode& node, const std::string& name);
+
+ static std::string GetTagName(const RapidXmlReaderNode& node);
+
+ static bool CheckMinChildSize(const RapidXmlReaderNode& node, const size_t size);
+
+ static bool CheckMaxChildSize(const RapidXmlReaderNode& node, const size_t size);
+
+ static bool CheckExactChildSize(const RapidXmlReaderNode& node, const size_t size);
+
+ static void EnforceChildSize(const RapidXmlReaderNode& node, const size_t size);
};
}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/typeReader/xml/Reader.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/typeReader/xml/Reader.cpp
index cf56e483356695bf2b54a747e36c4d758e7f1a56..944f755f4ffdac48ae204903c0047a39c45e828f 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/typeReader/xml/Reader.cpp
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/typeReader/xml/Reader.cpp
@@ -129,7 +129,7 @@ namespace armarx::aron::xmltypereader
std::vector<RapidXmlReaderNode> includes = children[cpp_includes_index].nodes();
for (const auto& include : includes)
{
- this->codeIncludes.push_back(readCppInclude(include));
+ this->codeIncludes.push_back(readCodeInclude(include));
}
}
@@ -169,7 +169,7 @@ namespace armarx::aron::xmltypereader
}
}
- std::string Reader::readCppInclude(const RapidXmlReaderNode& node) const
+ std::string Reader::readCodeInclude(const RapidXmlReaderNode& node)
{
Data::EnforceTagName(node, Data::INCLUDE_TAG);
Data::EnforceAttribute(node, Data::INCLUDE_ATTRIBUTE_NAME);
@@ -177,16 +177,21 @@ namespace armarx::aron::xmltypereader
return include;
}
- std::string Reader::readAronInclude(const RapidXmlReaderNode& node) const
+ std::string Reader::readAronInclude(const RapidXmlReaderNode& node)
{
Data::EnforceTagName(node, Data::INCLUDE_TAG);
-
const std::string xmlinclude = Data::GetAttribute(node, Data::INCLUDE_ATTRIBUTE_NAME);
// parse parent xml file and add objects to alreday known
Reader anotherReader;
anotherReader.parseFile(xmlinclude);
+ if (Data::HasAttribute(node, Data::AUTO_CODE_INCLUDE))
+ {
+ std::string codeinclude = simox::alg::replace_last(xmlinclude, ".xml", ".aron.generated.h");
+ this->codeIncludes.push_back(codeinclude);
+ }
+
return xmlinclude;
}
@@ -194,14 +199,12 @@ namespace armarx::aron::xmltypereader
{
Data::EnforceTagName(node, Data::GENERATE_OBJECT_TAG);
return typenavigator::ObjectNavigator::DynamicCastAndCheck(factory.create(node, Path()));
-
}
typenavigator::IntEnumNavigatorPtr Reader::readGenerateIntEnum(const RapidXmlReaderNode& node) const
{
Data::EnforceTagName(node, Data::GENERATE_INT_ENUM_TAG);
return typenavigator::IntEnumNavigator::DynamicCastAndCheck(factory.create(node, Path()));
-
}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/typeReader/xml/Reader.h b/source/RobotAPI/libraries/aron/core/codegenerator/typeReader/xml/Reader.h
index e19083a9aa2718792c705ec41267a4e7f50d2cc8..387b2888dc23a44f3efebe832163a9888be249bc 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/typeReader/xml/Reader.h
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/typeReader/xml/Reader.h
@@ -56,8 +56,8 @@ namespace armarx::aron::xmltypereader
private:
void parse(const RapidXmlReaderPtr& node);
- std::string readCppInclude(const RapidXmlReaderNode& node) const;
- std::string readAronInclude(const RapidXmlReaderNode& node) const;
+ std::string readCodeInclude(const RapidXmlReaderNode& node);
+ std::string readAronInclude(const RapidXmlReaderNode& node);
typenavigator::ObjectNavigatorPtr readGenerateObject(const RapidXmlReaderNode& node) const;
typenavigator::IntEnumNavigatorPtr readGenerateIntEnum(const RapidXmlReaderNode& node) const;
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/typeReader/xml/ReaderFactory.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/typeReader/xml/ReaderFactory.cpp
index da9eff9f25c9ee9478d7c183ae4fde981e21a16f..6bc140e98a8d8ae555490e9c2b07b3360ac176cc 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/typeReader/xml/ReaderFactory.cpp
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/typeReader/xml/ReaderFactory.cpp
@@ -77,18 +77,41 @@ namespace armarx::aron::xmltypereader
const auto public_intenum_it = AllPublicIntEnums.find(name);
if (public_intenum_it != AllPublicIntEnums.end())
{
- return public_intenum_it->second->correspondingType;
+ // copy the navigator
+ return typenavigator::Navigator::FromAronType(public_intenum_it->second->correspondingType->toAronTypePtr());
}
const auto public_obj_it = AllPublicObjects.find(name);
if (public_obj_it != AllPublicObjects.end())
{
- return public_obj_it->second->correspondingType;
+ // copy the navigator
+ return typenavigator::Navigator::FromAronType(public_obj_it->second->correspondingType->toAronTypePtr());
}
throw error::StringNotValidException("XMLReaderFactory", "ResolveTypename", "Could not find a tag. You can only access already finished public objects.", name);
}
+ type::Maybe ReaderFactory::GetMaybe(const RapidXmlReaderNode& n)
+ {
+ if (Data::AttributeIsTrue(n, Data::OPTIONAL_NAME))
+ {
+ return type::Maybe::eOptional;
+ }
+ if (Data::AttributeIsTrue(n, Data::RAW_PTR_NAME))
+ {
+ return type::Maybe::eRawPointer;
+ }
+ if (Data::AttributeIsTrue(n, Data::SHARED_PTR_NAME))
+ {
+ return type::Maybe::eSharedPointer;
+ }
+ if (Data::AttributeIsTrue(n, Data::UNIQUE_PTR_NAME))
+ {
+ return type::Maybe::eUniquePointer;
+ }
+ return type::Maybe::eNone;
+ }
+
// Object type
typenavigator::NavigatorPtr ObjectReaderFactory::createSpecific(const RapidXmlReaderNode& node, const Path& path) const
{
@@ -115,13 +138,8 @@ namespace armarx::aron::xmltypereader
if (extends != "")
{
- auto it = AllPublicObjects.find(extends);
- if (it == AllPublicObjects.end())
- {
- throw error::StringNotValidException("AronObjectTypeXMLReaderFactory", "createSpecific", "An extends name could not be found. Perhaps you need to change the order of your types or you have to add another UseType", extends, path);
- }
- codegeneratorhelper::GenerateObjectInfoPtr parentObj = it->second;
- aronObjectType->setExtends(parentObj->correspondingType);
+ auto parentObj = typenavigator::ObjectNavigator::DynamicCastAndCheck(ResolveTypename(simox::alg::to_lower(extends)));
+ aronObjectType->setExtends(parentObj);
}
ObjectGenerationStack.push(newObject);
@@ -134,7 +152,11 @@ namespace armarx::aron::xmltypereader
const std::string key = objectChild.attribute_value(Data::KEY_ATTRIBUTE_NAME);
std::vector<RapidXmlReaderNode> children = objectChild.nodes();
+
+ auto maybe = GetMaybe(children[0]);
typenavigator::NavigatorPtr childNavigator = create(children[0], Path(path, key));
+
+ childNavigator->setMaybe(maybe);
aronObjectType->addMemberType(key, childNavigator);
}
ObjectGenerationStack.pop();
@@ -149,8 +171,10 @@ namespace armarx::aron::xmltypereader
{
// We are a public top level class. Add to AllPublicObjects
AllPublicObjects.insert(std::make_pair(simox::alg::to_lower(newObject->typeName), newObject));
+ aronObjectType = typenavigator::ObjectNavigator::DynamicCastAndCheck(ResolveTypename(simox::alg::to_lower(newObject->typeName))); // make a copy
}
+ ARMARX_CHECK_NOT_NULL(aronObjectType);
return aronObjectType;
}
@@ -163,7 +187,9 @@ namespace armarx::aron::xmltypereader
Data::EnforceChildSize(node, 1);
std::vector<RapidXmlReaderNode> listTypeNodeChildren = node.nodes();
const RapidXmlReaderNode typeNode = listTypeNodeChildren[0];
+ auto maybe = GetMaybe(typeNode);
typenavigator::NavigatorPtr type = create(typeNode, Path(path, "type"));
+ type->setMaybe(maybe);
list->setAcceptedType(type);
return list;
@@ -178,9 +204,11 @@ namespace armarx::aron::xmltypereader
Data::EnforceChildSize(node, 1);
std::vector<RapidXmlReaderNode> dictTypeNodeChildren = node.nodes();
const RapidXmlReaderNode typeNode = dictTypeNodeChildren[0];
+ auto maybe = GetMaybe(typeNode);
typenavigator::NavigatorPtr type = create(typeNode, Path(path, "type"));
- dict->setAcceptedType(type);
+ type->setMaybe(maybe);
+ dict->setAcceptedType(type);
return dict;
}
@@ -203,8 +231,11 @@ namespace armarx::aron::xmltypereader
std::vector<RapidXmlReaderNode> typeNodeChildren = tupleTypeDeclarationNode.nodes();
const RapidXmlReaderNode typeNode = typeNodeChildren[0];
+ auto maybe = GetMaybe(typeNode);
typenavigator::NavigatorPtr type = create(typeNode, Path(path, "<" + std::to_string(i++) + ">"));
+ type->setMaybe(maybe);
+
tuple->addAcceptedType(type);
}
return tuple;
@@ -227,33 +258,36 @@ namespace armarx::aron::xmltypereader
std::vector<RapidXmlReaderNode> type1NodeChildren = nodeChildren[0].nodes();
const RapidXmlReaderNode type1Node = type1NodeChildren[0];
+ auto maybe1 = GetMaybe(type1Node);
+
typenavigator::NavigatorPtr type1 = create(type1Node, Path(path, std::to_string(0)));
+ type1->setMaybe(maybe1);
pair->setFirstAcceptedType(type1);
Data::EnforceChildSize(nodeChildren[1], 1);
std::vector<RapidXmlReaderNode> type2NodeChildren = nodeChildren[1].nodes();
const RapidXmlReaderNode type2Node = type2NodeChildren[0];
+ auto maybe2 = GetMaybe(type2Node);
+
typenavigator::NavigatorPtr type2 = create(type2Node, Path(path, std::to_string(1)));
+ type2->setMaybe(maybe2);
pair->setSecondAcceptedType(type2);
return pair;
}
+ // Complex type (NDArray)
+ typenavigator::NavigatorPtr NDArrayReaderFactory::createSpecific(const RapidXmlReaderNode& node, const Path& path) const
+ {
+ return nullptr;
+ }
+
// Complex type (IVTCByteImage)
typenavigator::NavigatorPtr IVTCByteImageReaderFactory::createSpecific(const RapidXmlReaderNode& node, const Path& path) const
{
Data::EnforceTagName(node, Data::GENERATE_IVT_CBYTE_IMAGE_MEMBER_TAG);
- Data::EnforceAttribute(node, Data::TYPE_ATTRIBUTE_NAME);
-
- int width = std::stoi(Data::GetAttributeWithDefault(node, Data::WIDTH_ATTRIBUTE_NAME, "1"));
- int height = std::stoi(Data::GetAttributeWithDefault(node, Data::HEIGHT_ATTRIBUTE_NAME, "1"));
- std::string type = node.attribute_value(Data::TYPE_ATTRIBUTE_NAME);;
-
typenavigator::IVTCByteImageNavigatorPtr complex(new typenavigator::IVTCByteImageNavigator(path));
- complex->setWidth(width);
- complex->setHeight(height);
- complex->setTypename(type);
return complex;
}
@@ -263,8 +297,8 @@ namespace armarx::aron::xmltypereader
Data::EnforceTagName(node, Data::GENERATE_EIGEN_MATRIX_MEMBER_TAG);
Data::EnforceAttribute(node, Data::TYPE_ATTRIBUTE_NAME);
- int rows = std::stoi(Data::GetAttributeWithDefault(node, Data::ROWS_ATTRIBUTE_NAME, "1"));
- int cols = std::stoi(Data::GetAttributeWithDefault(node, Data::COLS_ATTRIBUTE_NAME, "1"));
+ const int rows = std::stoi(Data::GetAttributeWithDefault(node, Data::ROWS_ATTRIBUTE_NAME, "4"));
+ const int cols = std::stoi(Data::GetAttributeWithDefault(node, Data::COLS_ATTRIBUTE_NAME, "4"));
std::string type = node.attribute_value(Data::TYPE_ATTRIBUTE_NAME);
typenavigator::EigenMatrixNavigatorPtr complex(new typenavigator::EigenMatrixNavigator(path));
@@ -291,15 +325,8 @@ namespace armarx::aron::xmltypereader
typenavigator::NavigatorPtr OpenCVMatReaderFactory::createSpecific(const RapidXmlReaderNode& node, const Path& path) const
{
Data::EnforceTagName(node, Data::GENERATE_OPENCV_MAT_MEMBER_TAG);
- Data::EnforceAttribute(node, Data::TYPE_ATTRIBUTE_NAME);
-
- std::vector<std::string> dimensions_as_strings = simox::alg::split(Data::GetAttributeWithDefault(node, Data::DIMENSIONS_ATTRIBUTE_NAME, "1"), ",");
- std::vector<int> dimensions = simox::alg::to_vec<int>(dimensions_as_strings);
- std::string type = node.attribute_value(Data::TYPE_ATTRIBUTE_NAME);
typenavigator::OpenCVMatNavigatorPtr complex(new typenavigator::OpenCVMatNavigator(path));
- complex->setDimensions(dimensions);
- complex->setTypename(type);
return complex;
}
@@ -308,14 +335,9 @@ namespace armarx::aron::xmltypereader
{
Data::EnforceTagName(node, Data::GENERATE_PCL_POINTCLOUD_MEMBER_TAG);
Data::EnforceAttribute(node, Data::TYPE_ATTRIBUTE_NAME);
-
- int width = std::stoi(Data::GetAttributeWithDefault(node, Data::WIDTH_ATTRIBUTE_NAME, "1"));
- int height = std::stoi(Data::GetAttributeWithDefault(node, Data::HEIGHT_ATTRIBUTE_NAME, "1"));
std::string type = node.attribute_value(Data::TYPE_ATTRIBUTE_NAME);;
typenavigator::PCLPointCloudNavigatorPtr complex(new typenavigator::PCLPointCloudNavigator(path));
- complex->setWidth(width);
- complex->setHeight(height);
complex->setTypename(type);
return complex;
}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/typeReader/xml/ReaderFactory.h b/source/RobotAPI/libraries/aron/core/codegenerator/typeReader/xml/ReaderFactory.h
index 88e7ad01df9ec261b7bdc6226c1974b5375ff043..f94ed41ece1715f61cf0d207064d0e97fe5628b6 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/typeReader/xml/ReaderFactory.h
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/typeReader/xml/ReaderFactory.h
@@ -55,6 +55,7 @@ namespace armarx::aron::xmltypereader
protected:
static typenavigator::NavigatorPtr ResolveTypename(const std::string&);
+ static type::Maybe GetMaybe(const RapidXmlReaderNode&);
public:
static std::map<std::string, codegeneratorhelper::GenerateObjectInfoPtr> AllPublicObjects;
diff --git a/source/RobotAPI/libraries/aron/core/io/Data.h b/source/RobotAPI/libraries/aron/core/io/Data.h
index 73c693b53311f47232c1b581698dc7765495c8c4..6f9fe8ca878a6f7a8f697af1295fd6f0ccbe581b 100644
--- a/source/RobotAPI/libraries/aron/core/io/Data.h
+++ b/source/RobotAPI/libraries/aron/core/io/Data.h
@@ -26,7 +26,7 @@
// ArmarX
#include <ArmarXCore/core/exceptions/Exception.h>
-
+#include <RobotAPI/libraries/aron/core/Descriptor.h>
#include <RobotAPI/libraries/aron/core/Config.h>
@@ -39,8 +39,14 @@ namespace armarx::aron::io
Data() = delete;
public:
- // TODO: Remove copy from ReaderWriter
- static constexpr const char* READER_WRITER_NAME_SLUG = "__ARON_NAME";
+ static constexpr const char* READER_WRITER_MAYBE_SLUG = "__ARON_MAYBE";
+ static constexpr const char* READER_WRITER_TYPE_SLUG = "__ARON_TYPE";
+ static constexpr const char* READER_WRITER_VERSION_SLUG = "__ARON_VERSION";
+
+ static constexpr const char* READER_WRITER_VALUE_SLUG = "__ARON_VALUE";
+
+ static constexpr const char* READER_WRITER_OBJECT_NAME_SLUG = "__ARON_OBJECT_NAME";
+
static constexpr const char* READER_WRITER_DICT_ACCEPTED_TYPE_SLUG = "__ARON_DICT_ACCEPTED_TYPE";
static constexpr const char* READER_WRITER_LIST_ACCEPTED_TYPE_SLUG = "__ARON_LIST_ACCEPTED_TYPE";
@@ -50,13 +56,12 @@ namespace armarx::aron::io
static constexpr const char* READER_WRITER_NDARRAY_DATA_SLUG = "__ARON_NDARRAY_DATA";
static constexpr const char* READER_WRITER_INT_ENUM_NAME_SLUG = "__ARON_INT_ENUM_NAME";
- static constexpr const char* READER_WRITER_INT_ENUM_VALUE_SLUG = "__ARON_INT_ENUM_VALUE";
static constexpr const char* READER_WRITER_ENUM_KEY_SLUG = "__ARON_ENUM_KEY";
#define RUN_ARON_MACRO(upperType, lowerType, capsType) \
static constexpr const char* READER_WRITER_##capsType##_TYPENAME_SLUG = #capsType;
- HANDLE_PRIMITIVE_TYPES
+ HANDLE_ALL_ARON_TYPES
#undef RUN_ARON_MACRO
};
}
diff --git a/source/RobotAPI/libraries/aron/core/io/dataIO/Reader.cpp b/source/RobotAPI/libraries/aron/core/io/dataIO/Reader.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..2d6a9ff042aeb6e04e75aa8e8fa0de65ab68da38
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/io/dataIO/Reader.cpp
@@ -0,0 +1,27 @@
+/*
+* 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/>.
+*
+* @author Fabian Peller (fabian dot peller at kit dot edu)
+* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#include "Reader.h"
+
+namespace armarx::aron::dataIO
+{
+
+}
+
diff --git a/source/RobotAPI/libraries/aron/core/io/dataIO/Reader.h b/source/RobotAPI/libraries/aron/core/io/dataIO/Reader.h
index 574b9167d3f6b77e4b2a8434a5dfb90911cabd2c..e2769dd1c0f19152ec635c08ee7d580f223fab9f 100644
--- a/source/RobotAPI/libraries/aron/core/io/dataIO/Reader.h
+++ b/source/RobotAPI/libraries/aron/core/io/dataIO/Reader.h
@@ -24,6 +24,7 @@
#include <memory>
#include <string>
#include <vector>
+#include <optional>
// ArmarX
#include <RobotAPI/libraries/aron/core/Config.h>
@@ -37,33 +38,51 @@ namespace armarx::aron::dataIO
class ReaderInterface
{
public:
- virtual int readStartDict() = 0;
+ struct ReadStartDictReturnType
+ {
+ int elements;
+ bool success;
+ };
+ virtual ReadStartDictReturnType readStartDict() = 0;
virtual bool readEndDict() = 0;
- virtual int readStartList() = 0;
- virtual bool readEndList() = 0;
- virtual std::tuple<std::vector<int>, std::string> readStartNDArray() = 0;
- virtual bool readEndNDArray(unsigned char*) = 0;
+ struct ReadStartListReturnType
+ {
+ int elements;
+ bool success;
+ };
+ virtual ReadStartListReturnType readStartList() = 0;
+ virtual bool readEndList() = 0;
- virtual void readPrimitive(int&) = 0;
- virtual void readPrimitive(long&) = 0;
- virtual void readPrimitive(float&) = 0;
- virtual void readPrimitive(double&) = 0;
- virtual void readPrimitive(std::string&) = 0;
- virtual void readPrimitive(bool&) = 0;
+ struct ReadStartNDArrayReturnType
+ {
+ std::vector<int> dims;
+ std::string type;
+ bool success;
+ };
+ virtual ReadStartNDArrayReturnType readStartNDArray() = 0;
+ virtual void readEndNDArray(unsigned char*) = 0;
- template<class T>
- T readPrimitive()
+ struct ReadPrimitiveReturnType
{
- T p;
- readPrimitive(p);
- return p;
- }
+ bool success;
+ };
+ virtual ReadPrimitiveReturnType readStartPrimitive() = 0;
+ virtual void readEndPrimitive(int&) = 0;
+ virtual void readEndPrimitive(long&) = 0;
+ virtual void readEndPrimitive(float&) = 0;
+ virtual void readEndPrimitive(double&) = 0;
+ virtual void readEndPrimitive(std::string&) = 0;
+ virtual void readEndPrimitive(bool&) = 0;
virtual std::string readKey() = 0;
virtual void loadMember(const std::string&) = 0;
+ // This method is for converters or visitors. The code generation knows what to call (e.g. readStartNDArray) since it knows the type.
+ // But if the type is unknown (and null), we need a method to increase the cnt of a token to the next element
+ virtual void readNull() = 0;
+
// Helper functions
- virtual data::Descriptor getTypeOfNext(const type::Descriptor hint = type::Descriptor::eUnknown) const = 0;
+ virtual data::Descriptor getTypeOfNext() const = 0;
};
}
diff --git a/source/RobotAPI/libraries/aron/core/io/dataIO/Writer.cpp b/source/RobotAPI/libraries/aron/core/io/dataIO/Writer.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..2a5ff93011457741da3f3dfb89902fb8fa794d26
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/io/dataIO/Writer.cpp
@@ -0,0 +1,27 @@
+/*
+* 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/>.
+*
+* @author Fabian Peller (fabian dot peller at kit dot edu)
+* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#include "Writer.h"
+
+
+namespace armarx::aron::dataIO
+{
+
+}
diff --git a/source/RobotAPI/libraries/aron/core/io/dataIO/Writer.h b/source/RobotAPI/libraries/aron/core/io/dataIO/Writer.h
index 89aca64073cc652b2a72d3da62b7ce0a7c3e76d9..0cd1caec4d0b515a104eeb935f23df946e1b6db3 100644
--- a/source/RobotAPI/libraries/aron/core/io/dataIO/Writer.h
+++ b/source/RobotAPI/libraries/aron/core/io/dataIO/Writer.h
@@ -27,8 +27,7 @@
// ArmarX
#include <RobotAPI/interface/aron.h>
#include <RobotAPI/libraries/aron/core/Config.h>
-
-
+#include <RobotAPI/libraries/aron/core/Descriptor.h>
namespace armarx::aron::dataIO
{
@@ -52,6 +51,8 @@ namespace armarx::aron::dataIO
virtual void writePrimitive(const std::string&) = 0;
virtual void writePrimitive(bool) = 0;
+ virtual void writeNull() = 0;
+
virtual void writeKey(const std::string&) = 0;
};
}
diff --git a/source/RobotAPI/libraries/aron/core/io/dataIO/converter/Converter.cpp b/source/RobotAPI/libraries/aron/core/io/dataIO/converter/Converter.cpp
index a0feee781f33f1623d24948600a26370368db685..b8dbb60419279e56ca1a8c9cdb2d6db286a64550 100644
--- a/source/RobotAPI/libraries/aron/core/io/dataIO/converter/Converter.cpp
+++ b/source/RobotAPI/libraries/aron/core/io/dataIO/converter/Converter.cpp
@@ -35,12 +35,15 @@ namespace armarx::aron::dataIO
{
type::Descriptor t_desc = expectedStructure != nullptr ? expectedStructure->getDescriptor() : type::Descriptor::eUnknown;
//std::cout << "Expected Structure: " << type::DESCRIPTOR_TO_STRING(t_desc) << std::endl;
- data::Descriptor desc = reader.getTypeOfNext(t_desc);
+ data::Descriptor desc = reader.getTypeOfNext();
+
+ // TODO check expected type and read type
+
switch (desc)
{
case data::Descriptor::eDict:
{
- int elements = reader.readStartDict();
+ auto readStartDict = reader.readStartDict();
writer.writeStartDict();
typenavigator::NavigatorPtr childType = nullptr;
@@ -50,7 +53,7 @@ namespace armarx::aron::dataIO
childType = t->getAcceptedType();
}
- for (int i = 0; i < elements; ++i)
+ for (int i = 0; i < readStartDict.elements; ++i)
{
std::string key = reader.readKey();
writer.writeKey(key);
@@ -71,7 +74,7 @@ namespace armarx::aron::dataIO
}
case data::Descriptor::eList:
{
- int elements = reader.readStartList();
+ auto readStartList = reader.readStartList();
writer.writeStartList();
typenavigator::NavigatorPtr childType = nullptr;
@@ -81,7 +84,7 @@ namespace armarx::aron::dataIO
childType = t->getAcceptedType();
}
- for (int i = 0; i < elements; ++i)
+ for (int i = 0; i < readStartList.elements; ++i)
{
if (t_desc == type::Descriptor::eObject)
{
@@ -99,55 +102,67 @@ namespace armarx::aron::dataIO
case data::Descriptor::eNDArray:
{
- auto [dims, type] = reader.readStartNDArray();
- int elements = std::accumulate(std::begin(dims), std::end(dims), 1, std::multiplies<int>());
+ auto readStartNDArray = reader.readStartNDArray();
+ int elements = std::accumulate(std::begin(readStartNDArray.dims), std::end(readStartNDArray.dims), 1, std::multiplies<int>());
std::vector<unsigned char> data(elements);
reader.readEndNDArray(data.data());
- writer.writeNDArray(dims, type, data.data());
+ writer.writeNDArray(readStartNDArray.dims, readStartNDArray.type, data.data());
break;
}
case data::Descriptor::eInt:
{
- int val = reader.readPrimitive<int>();
+ int val;
+ reader.readEndPrimitive(val);
writer.writePrimitive(val);
break;
}
case data::Descriptor::eLong:
{
- long val = reader.readPrimitive<long>();
+ long val;
+ reader.readEndPrimitive(val);
writer.writePrimitive(val);
break;
}
case data::Descriptor::eFloat:
{
- float val = reader.readPrimitive<float>();
+ float val;
+ reader.readEndPrimitive(val);
writer.writePrimitive(val);
break;
}
case data::Descriptor::eDouble:
{
- double val = reader.readPrimitive<double>();
+ double val;
+ reader.readEndPrimitive(val);
writer.writePrimitive(val);
break;
}
case data::Descriptor::eString:
{
- std::string val = reader.readPrimitive<std::string>();
+ std::string val;
+ reader.readEndPrimitive(val);
writer.writePrimitive(val);
break;
}
case data::Descriptor::eBool:
{
- bool val = reader.readPrimitive<bool>();
+ bool val;
+ reader.readEndPrimitive(val);
writer.writePrimitive(val);
break;
}
- default:
+ case data::Descriptor::eUnknown:
{
- throw error::DescriptorNotValidException("Converter", "ReadAndConvert", "Data-Type could not be resolved", desc);
+ // it seems like we found an optional value
+ if (t_desc != type::Descriptor::eUnknown && expectedStructure->getMaybe() == type::Maybe::eNone)
+ {
+ throw error::DescriptorNotValidException("Converter", "ReadAndConvert", "The Descriptor is unknown but the expected type assumes a non-maybe type!", desc);
+ }
+ reader.readNull();
+ writer.writeNull();
}
}
}
diff --git a/source/RobotAPI/libraries/aron/core/io/dataIO/reader/ReaderToken.h b/source/RobotAPI/libraries/aron/core/io/dataIO/reader/ReaderToken.h
index b1c8e774e9b252bb582ba7a7dbf2c38c8f158122..d9f2a7bde5ec5bb45278b36e6fb9f73165de6732 100644
--- a/source/RobotAPI/libraries/aron/core/io/dataIO/reader/ReaderToken.h
+++ b/source/RobotAPI/libraries/aron/core/io/dataIO/reader/ReaderToken.h
@@ -24,6 +24,9 @@
#include <memory>
#include <string>
+// Simox
+#include <SimoxUtility/algorithm/string.h>
+
// ArmarX
#include <RobotAPI/libraries/aron/core/Config.h>
#include <RobotAPI/libraries/aron/core/Exception.h>
@@ -45,8 +48,9 @@ namespace armarx::aron::dataIO
// Interface
- virtual data::Descriptor getTypeOfNext(const type::Descriptor hint = type::Descriptor::eUnknown) const = 0;
+ virtual data::Descriptor getTypeOfNext() const = 0;
virtual ElementTypename getNextElement() const = 0;
+ virtual bool hasNextElement() const = 0;
// General implementation
ElementTypename getElement() const
@@ -85,6 +89,15 @@ namespace armarx::aron::dataIO
return this->currentKey;
}
+ bool hasKey(const std::string& k) const
+ {
+ if (descriptor != data::Descriptor::eDict)
+ {
+ throw error::DescriptorNotValidException("ReaderTokenInterface", "hasKey", "Checking whether a container has a key only makes sense in a dict.", descriptor);
+ }
+ return std::find(allMemberNames.begin(), allMemberNames.end(), k) != allMemberNames.end();
+ }
+
void setCurrentKey(const std::string& s)
{
currentKey = s;
@@ -92,7 +105,7 @@ namespace armarx::aron::dataIO
void assertType(data::Descriptor t) const
{
- if (getDescriptor() != t)
+ if (descriptor != t)
{
throw error::DescriptorNotValidException("dataIO::ReaderTokenInterface", "assertType", "The type was not equal.", t);
}
@@ -106,13 +119,22 @@ namespace armarx::aron::dataIO
}
}
- ElementTypename getNextElementAndIncreaseCnt()
+ void increaseCounter()
{
- ElementTypename next = getNextElement();
this->currentIndex++;
- return next;
}
+ unsigned int getCurrentIndex() const
+ {
+ return currentIndex;
+ }
+
+ std::string allMemberNamesToString() const
+ {
+ return simox::alg::to_string(allMemberNames, ", ");
+ }
+
+
protected:
// members
data::Descriptor descriptor = data::Descriptor::eUnknown;
diff --git a/source/RobotAPI/libraries/aron/core/io/dataIO/reader/navigator/NavigatorReader.cpp b/source/RobotAPI/libraries/aron/core/io/dataIO/reader/navigator/NavigatorReader.cpp
index 2dc7c3db842d4e7deb5ee1c5d94b4be5618f2e64..c431b5b49ea2547f262da4fd657be6aa6fed1fac 100644
--- a/source/RobotAPI/libraries/aron/core/io/dataIO/reader/navigator/NavigatorReader.cpp
+++ b/source/RobotAPI/libraries/aron/core/io/dataIO/reader/navigator/NavigatorReader.cpp
@@ -27,54 +27,52 @@
// ArmarX
#include <RobotAPI/libraries/aron/core/Exception.h>
-#include <RobotAPI/libraries/aron/core/navigator/data/container/List.h>
-#include <RobotAPI/libraries/aron/core/navigator/data/primitive/Primitive.h>
+#include <RobotAPI/libraries/aron/core/navigator/data/AllNavigators.h>
namespace armarx::aron::dataIO::reader
{
- NavigatorReader::NavigatorReader(const datanavigator::NavigatorPtr& n) :
- input(n)
+ NavigatorReader::NavigatorReader(const data::AronDictPtr& n) :
+ NavigatorReader(datanavigator::DictNavigator::DynamicCastAndCheck(datanavigator::Navigator::FromAronData(n)))
{
}
- NavigatorReader::NavigatorReader(const data::AronDataPtr& n) :
- input(datanavigator::Navigator::FromAronData(n))
+ NavigatorReader::NavigatorReader(const datanavigator::DictNavigatorPtr& n) :
+ input(n)
{
+ auto current_nav_casted = datanavigator::DictNavigator::DynamicCastAndCheck(input);
+ auto newToken = std::make_shared<NavigatorReaderToken>(current_nav_casted);
+ stack.push(newToken);
}
- datanavigator::NavigatorPtr NavigatorReader::getNextAndIncrease()
+ // Containers
+ ReaderInterface::ReadStartDictReturnType NavigatorReader::readStartDict()
{
- if (!readInitialStart)
+ if (!readFirstStartDict)
{
- readInitialStart = true;
- return input;
+ // we already added in constructor
+ readFirstStartDict = true;
+ auto lastToken = stack.top();
+ auto current_nav = lastToken->getElement();
+ int c = current_nav->childrenSize();
+ return {c, true};
}
+
auto lastToken = stack.top();
- return lastToken->getNextElementAndIncreaseCnt();
- }
- datanavigator::NavigatorPtr NavigatorReader::getNext()
- {
- if (!readInitialStart)
+ auto current_nav = lastToken->getNextElement();
+ if (!current_nav)
{
- readInitialStart = true;
- return input;
+ lastToken->increaseCounter();
+ return {0, false};
}
- auto lastToken = stack.top();
- return lastToken->getNextElement();
- }
- // Containers
- int NavigatorReader::readStartDict()
- {
- auto current_nav = getNextAndIncrease();
auto current_nav_casted = datanavigator::DictNavigator::DynamicCastAndCheck(current_nav);
int c = current_nav->childrenSize();
- auto newToken = std::make_shared<NavigatorReaderToken>(data::Descriptor::eDict, current_nav_casted);
+ auto newToken = std::make_shared<NavigatorReaderToken>(current_nav_casted);
stack.push(newToken);
- return c;
+ return {c, true};
}
bool NavigatorReader::readEndDict()
@@ -82,22 +80,39 @@ namespace armarx::aron::dataIO::reader
auto token = stack.top();
datanavigator::DictNavigator::DynamicCastAndCheck(token->getElement());
+ //std::cout << "We are at: " << token->getCurrentIndex() << "/" << token->getElementChildrenSize() << std::endl;
+
if (token->finishedElement())
{
stack.pop();
+
+ if (!stack.empty())
+ {
+ auto lastToken = stack.top();
+ lastToken->increaseCounter();
+ }
+
return true;
}
return false;
}
- int NavigatorReader::readStartList()
+ ReaderInterface::ReadStartListReturnType NavigatorReader::readStartList()
{
- auto current_nav = getNextAndIncrease();
+ auto lastToken = stack.top();
+
+ auto current_nav = lastToken->getNextElement();
+ if (!current_nav)
+ {
+ lastToken->increaseCounter();
+ return {0, false};
+ }
+
auto current_nav_casted = datanavigator::ListNavigator::DynamicCastAndCheck(current_nav);
int c = current_nav->childrenSize();
- auto newToken = std::make_shared<NavigatorReaderToken>(data::Descriptor::eList, current_nav_casted);
+ auto newToken = std::make_shared<NavigatorReaderToken>(current_nav_casted);
stack.push(newToken);
- return c;
+ return {c, true};
}
bool NavigatorReader::readEndList()
@@ -105,105 +120,157 @@ namespace armarx::aron::dataIO::reader
auto token = stack.top();
datanavigator::ListNavigator::DynamicCastAndCheck(token->getElement());
+ //std::cout << "We are at: " << token->getCurrentIndex() << "/" << token->getElementChildrenSize() << std::endl;
if (token->finishedElement())
{
stack.pop();
+
+ auto lastToken = stack.top();
+ lastToken->increaseCounter();
+
return true;
}
return false;
}
// Complex Data
- std::tuple<std::vector<int>, std::string> NavigatorReader::readStartNDArray()
+ ReaderInterface::ReadStartNDArrayReturnType NavigatorReader::readStartNDArray()
{
- datanavigator::NavigatorPtr nav = getNext();
- datanavigator::NDArrayNavigatorPtr casted = datanavigator::NDArrayNavigator::DynamicCastAndCheck(nav);
- return {casted->getDimensions(), casted->getType()};
+ auto lastToken = stack.top();
+
+ auto current_nav = lastToken->getNextElement();
+ if (!current_nav)
+ {
+ lastToken->increaseCounter();
+ return {{}, "", false};
+ }
+ datanavigator::NDArrayNavigatorPtr casted = datanavigator::NDArrayNavigator::DynamicCastAndCheck(current_nav);
+ return {casted->getDimensions(), casted->getType(), true};
}
- bool NavigatorReader::readEndNDArray(unsigned char* data)
+ void NavigatorReader::readEndNDArray(unsigned char* data)
{
if (data == NULL)
{
throw error::AronException("NavigatorReader", "readEndNDArray", "The corresponding data pointer of an complex aron ptr is NULL.");
}
- auto nav = getNextAndIncrease();
- auto casted = datanavigator::NDArrayNavigator::DynamicCastAndCheck(nav);
+
+ auto lastToken = stack.top();
+ auto current_nav = lastToken->getNextElement();
+ lastToken->increaseCounter();
+
+ auto casted = datanavigator::NDArrayNavigator::DynamicCastAndCheck(current_nav);
std::vector<int> dims = casted->getDimensions();
memcpy(data, casted->getData(), std::accumulate(std::begin(dims), std::end(dims), 1, std::multiplies<int>()));
- return true;
}
// Read primitives
- void NavigatorReader::readPrimitive(int& t)
+ ReaderInterface::ReadPrimitiveReturnType NavigatorReader::readStartPrimitive()
+ {
+ auto lastToken = stack.top();
+ auto current_nav = lastToken->getNextElement();
+ if (!current_nav)
+ {
+ lastToken->increaseCounter();
+ return {false};
+ }
+ return {true};
+ }
+
+ void NavigatorReader::readEndPrimitive(int& t)
{
- auto nav = getNextAndIncrease();
- auto casted = datanavigator::IntNavigator::DynamicCastAndCheck(nav);
+ auto lastToken = stack.top();
+ auto current_nav = lastToken->getNextElement();
+ lastToken->increaseCounter();
+
+ auto casted = datanavigator::IntNavigator::DynamicCastAndCheck(current_nav);
t = casted->getValue();
}
- void NavigatorReader::readPrimitive(long& t)
+ void NavigatorReader::readEndPrimitive(long& t)
{
- auto nav = getNextAndIncrease();
- auto casted = datanavigator::LongNavigator::DynamicCastAndCheck(nav);
+ auto lastToken = stack.top();
+ auto current_nav = lastToken->getNextElement();
+ lastToken->increaseCounter();
+
+ auto casted = datanavigator::LongNavigator::DynamicCastAndCheck(current_nav);
t = casted->getValue();
}
- void NavigatorReader::readPrimitive(float& t)
+ void NavigatorReader::readEndPrimitive(float& t)
{
- auto nav = getNextAndIncrease();
- auto casted = datanavigator::FloatNavigator::DynamicCastAndCheck(nav);
+ auto lastToken = stack.top();
+ auto current_nav = lastToken->getNextElement();
+ lastToken->increaseCounter();
+
+ auto casted = datanavigator::FloatNavigator::DynamicCastAndCheck(current_nav);
t = casted->getValue();
}
- void NavigatorReader::readPrimitive(double& t)
+ void NavigatorReader::readEndPrimitive(double& t)
{
- auto nav = getNextAndIncrease();
- auto casted = datanavigator::DoubleNavigator::DynamicCastAndCheck(nav);
+ auto lastToken = stack.top();
+ auto current_nav = lastToken->getNextElement();
+ lastToken->increaseCounter();
+
+ auto casted = datanavigator::DoubleNavigator::DynamicCastAndCheck(current_nav);
t = casted->getValue();
}
- void NavigatorReader::readPrimitive(std::string& t)
+ void NavigatorReader::readEndPrimitive(std::string& t)
{
- auto nav = getNextAndIncrease();
- auto casted = datanavigator::StringNavigator::DynamicCastAndCheck(nav);
+ auto lastToken = stack.top();
+ auto current_nav = lastToken->getNextElement();
+ lastToken->increaseCounter();
+
+ auto casted = datanavigator::StringNavigator::DynamicCastAndCheck(current_nav);
t = casted->getValue();
}
- void NavigatorReader::readPrimitive(bool& t)
+ void NavigatorReader::readEndPrimitive(bool& t)
{
- auto nav = getNextAndIncrease();
- auto casted = datanavigator::BoolNavigator::DynamicCastAndCheck(nav);
+ auto lastToken = stack.top();
+ auto current_nav = lastToken->getNextElement();
+ lastToken->increaseCounter();
+
+ auto casted = datanavigator::BoolNavigator::DynamicCastAndCheck(current_nav);
t = casted->getValue();
}
std::string NavigatorReader::readKey()
{
auto token = stack.top();
- return token->getCurrentKey();
+ auto s = token->getCurrentKey();
+ //std::cout << "Read key: " << s << std::endl;
+ return s;
}
void NavigatorReader::loadMember(const std::string& k)
{
+ //std::cout << "Loading member " << k << std::endl;
auto token = stack.top();
token->setCurrentKey(k);
}
+ void NavigatorReader::readNull()
+ {
+ auto token = stack.top();
+ auto current_nav = token->getNextElement();
+ if (!current_nav)
+ {
+ throw error::AronException("NavigatorReader", "readNull", "The value of a navigator is not null!");
+ }
+ token->increaseCounter();
+ }
+
// Helper functions
- data::Descriptor NavigatorReader::getTypeOfNext(const type::Descriptor hint) const
+ data::Descriptor NavigatorReader::getTypeOfNext() const
{
if (stack.empty())
{
- if (hint == type::Descriptor::eUnknown)
- {
- return data::Descriptor::eDict;
- }
- else
- {
- return Resolver::GetCorresponding(hint);
- }
+ return data::Descriptor::eDict;
}
auto token = stack.top();
- return token->getTypeOfNext(hint);
+ return token->getTypeOfNext();
}
}
diff --git a/source/RobotAPI/libraries/aron/core/io/dataIO/reader/navigator/NavigatorReader.h b/source/RobotAPI/libraries/aron/core/io/dataIO/reader/navigator/NavigatorReader.h
index db52ad1d85c2f10807d10f63bc44c7de717e6b97..26f7628de7747d030f9f7f38a4ca9d2c3fa5ac39 100644
--- a/source/RobotAPI/libraries/aron/core/io/dataIO/reader/navigator/NavigatorReader.h
+++ b/source/RobotAPI/libraries/aron/core/io/dataIO/reader/navigator/NavigatorReader.h
@@ -45,36 +45,36 @@ namespace armarx::aron::dataIO::reader
// constructors
NavigatorReader() = delete;
- NavigatorReader(const datanavigator::NavigatorPtr& n);
- NavigatorReader(const data::AronDataPtr& n);
+ NavigatorReader(const datanavigator::DictNavigatorPtr& n);
+ NavigatorReader(const data::AronDictPtr& n);
- virtual int readStartDict() override;
+ virtual ReadStartDictReturnType readStartDict() override;
virtual bool readEndDict() override;
- virtual int readStartList() override;
+
+ virtual ReadStartListReturnType readStartList() override;
virtual bool readEndList() override;
- virtual std::tuple<std::vector<int>, std::string> readStartNDArray() override;
- virtual bool readEndNDArray(unsigned char*) override;
+ virtual ReadStartNDArrayReturnType readStartNDArray() override;
+ virtual void readEndNDArray(unsigned char*) override;
- virtual void readPrimitive(int&) override;
- virtual void readPrimitive(long&) override;
- virtual void readPrimitive(float&) override;
- virtual void readPrimitive(double&) override;
- virtual void readPrimitive(std::string&) override;
- virtual void readPrimitive(bool&) override;
+ virtual ReadPrimitiveReturnType readStartPrimitive() override;
+ virtual void readEndPrimitive(int&) override;
+ virtual void readEndPrimitive(long&) override;
+ virtual void readEndPrimitive(float&) override;
+ virtual void readEndPrimitive(double&) override;
+ virtual void readEndPrimitive(std::string&) override;
+ virtual void readEndPrimitive(bool&) override;
virtual std::string readKey() override;
virtual void loadMember(const std::string&) override;
- virtual data::Descriptor getTypeOfNext(const type::Descriptor hint = type::Descriptor::eUnknown) const override;
+ virtual void readNull() override;
- private:
- datanavigator::NavigatorPtr getNext();
- datanavigator::NavigatorPtr getNextAndIncrease();
+ virtual data::Descriptor getTypeOfNext() const override;
private:
+ bool readFirstStartDict = false;
datanavigator::NavigatorPtr input;
- bool readInitialStart = false;
std::stack<NavigatorReaderTokenPtr> stack;
};
}
diff --git a/source/RobotAPI/libraries/aron/core/io/dataIO/reader/navigator/NavigatorReaderToken.h b/source/RobotAPI/libraries/aron/core/io/dataIO/reader/navigator/NavigatorReaderToken.h
index 4b85fe7725b3840c2e9c2b87a5d6cc9c48f9e63f..6941b5f4ec9c3e2566a7717e7675c673399a171d 100644
--- a/source/RobotAPI/libraries/aron/core/io/dataIO/reader/navigator/NavigatorReaderToken.h
+++ b/source/RobotAPI/libraries/aron/core/io/dataIO/reader/navigator/NavigatorReaderToken.h
@@ -46,9 +46,9 @@ namespace armarx::aron::dataIO::reader
public:
// constructors
NavigatorReaderToken() = delete;
- NavigatorReaderToken(const data::Descriptor desc, const datanavigator::NavigatorPtr& data)
+ NavigatorReaderToken(const datanavigator::NavigatorPtr& data)
{
- descriptor = desc;
+ descriptor = data->getDescriptor();
element = data;
switch (descriptor)
@@ -65,11 +65,11 @@ namespace armarx::aron::dataIO::reader
break;
}
default:
- throw error::DescriptorNotValidException("NavigatorReaderToken", "NavigatorReaderToken", "Received an invalid instance for a reader token.", desc);
+ throw error::DescriptorNotValidException("NavigatorReaderToken", "NavigatorReaderToken", "Received an invalid instance for a reader token.", descriptor);
}
}
- data::Descriptor getTypeOfNext(const type::Descriptor hint = type::Descriptor::eUnknown) const override
+ data::Descriptor getTypeOfNext() const override
{
const datanavigator::NavigatorPtr next = getNextElement();
return next->getDescriptor();
@@ -77,6 +77,11 @@ namespace armarx::aron::dataIO::reader
datanavigator::NavigatorPtr getNextElement() const override
{
+ if (!hasNextElement())
+ {
+ return nullptr;
+ }
+
switch (descriptor)
{
case data::Descriptor::eDict:
@@ -96,6 +101,27 @@ namespace armarx::aron::dataIO::reader
}
}
+ virtual bool hasNextElement() const override
+ {
+ switch (descriptor)
+ {
+ case data::Descriptor::eDict:
+ {
+ datanavigator::DictNavigatorPtr casted = datanavigator::DictNavigator::DynamicCast(element);
+ datanavigator::NavigatorPtr ret = casted->getElement(getCurrentKey());
+ return (bool) ret;
+ }
+ case data::Descriptor::eList:
+ {
+ datanavigator::ListNavigatorPtr casted = datanavigator::ListNavigator::DynamicCast(element);
+ datanavigator::NavigatorPtr ret = casted->getElement(currentIndex);
+ return (bool) ret;
+ }
+ default:
+ throw error::DescriptorNotValidException("NavigatorReaderToken", "hasData", "Could not resove a type of a navigator. Allowed are only container types due to performance", descriptor);
+ }
+ }
+
private:
};
diff --git a/source/RobotAPI/libraries/aron/core/io/dataIO/reader/nlohmannJSON/NlohmannJSONReader.cpp b/source/RobotAPI/libraries/aron/core/io/dataIO/reader/nlohmannJSON/NlohmannJSONReader.cpp
index c243c4e4beaf851090607423dd91e1d5ade4118a..5d614b7025a52239be1ecc9933202c5a5aa3d26f 100644
--- a/source/RobotAPI/libraries/aron/core/io/dataIO/reader/nlohmannJSON/NlohmannJSONReader.cpp
+++ b/source/RobotAPI/libraries/aron/core/io/dataIO/reader/nlohmannJSON/NlohmannJSONReader.cpp
@@ -27,52 +27,67 @@
// ArmarX
#include <RobotAPI/libraries/aron/core/Exception.h>
-#include <RobotAPI/libraries/aron/core/navigator/data/container/List.h>
-#include <RobotAPI/libraries/aron/core/navigator/data/primitive/Primitive.h>
+#include <RobotAPI/libraries/aron/core/navigator/data/AllNavigators.h>
namespace armarx::aron::dataIO::reader
{
- NlohmannJSONReader::NlohmannJSONReader(const nlohmann::json& n) :
- input(n)
- {
- }
-
NlohmannJSONReader::NlohmannJSONReader(const std::string& n) :
- input(nlohmann::json::parse(n))
+ NlohmannJSONReader(nlohmann::json::parse(n))
{
}
- nlohmann::json NlohmannJSONReader::getNextAndIncrease()
+ NlohmannJSONReader::NlohmannJSONReader(const nlohmann::json& n) :
+ input(n)
{
- if (!readInitialStart)
+ if (!n.is_object())
{
- readInitialStart = true;
- return input;
+ throw error::AronException("NlohmannJSONReader", "NlohmannJSONReader", "Allowed are only objects in reader");
}
- NlohmannJSONReaderTokenPtr lastToken = stack.top();
- return lastToken->getNextElementAndIncreaseCnt();
- }
- nlohmann::json NlohmannJSONReader::getNext()
- {
- if (!readInitialStart)
+ if (n.at(io::Data::READER_WRITER_TYPE_SLUG) != io::Data::READER_WRITER_DICT_TYPENAME_SLUG)
{
- readInitialStart = true;
- return input;
+ throw error::StringNotValidException("NlohmannJSONReader", "readStartList", "The json is not of dict type!", n[io::Data::READER_WRITER_TYPE_SLUG], io::Data::READER_WRITER_DICT_TYPENAME_SLUG);
}
- NlohmannJSONReaderTokenPtr lastToken = stack.top();
- return lastToken->getNextElement();
+
+ auto newToken = std::make_shared<NlohmannJSONReaderToken>(n);
+ stack.push(newToken);
}
- int NlohmannJSONReader::readStartDict()
+ ReaderInterface::ReadStartDictReturnType NlohmannJSONReader::readStartDict()
{
- nlohmann::json current_json = getNextAndIncrease();
- int c = current_json.size();
- auto newToken = std::make_shared<NlohmannJSONReaderToken>(data::Descriptor::eDict, current_json);
+ auto lastToken = stack.top();
+ if (!readFirstStartDict)
+ {
+ // we already added in constructor
+ readFirstStartDict = true;
+ auto current_json = lastToken->getElement();
+ int c = lastToken->getElementChildrenSize();
+ return {c, true};
+ }
+
+ auto current_json = lastToken->getNextElement();
+ if (current_json.is_null())
+ {
+ lastToken->increaseCounter(); // no need to call readEndDict anymore
+ return {0, false};
+ }
+
+ if (!current_json.is_object())
+ {
+ throw error::AronException("NlohmannJSONReader", "readStartDict", "The json is not an object. Every part of the data must be an object containing the type and the value.");
+ }
+
+ if (current_json.at(io::Data::READER_WRITER_TYPE_SLUG) != io::Data::READER_WRITER_DICT_TYPENAME_SLUG)
+ {
+ throw error::StringNotValidException("NlohmannJSONReader", "readStartList", "The json is not of dict type!", current_json.at(io::Data::READER_WRITER_TYPE_SLUG), io::Data::READER_WRITER_DICT_TYPENAME_SLUG);
+ }
+
+ auto newToken = std::make_shared<NlohmannJSONReaderToken>(current_json);
+ int c = newToken->getElementChildrenSize();
stack.push(newToken);
- return c;
+ return {c, true};
}
bool NlohmannJSONReader::readEndDict()
@@ -83,18 +98,43 @@ namespace armarx::aron::dataIO::reader
if (token->finishedElement())
{
stack.pop();
+
+ if (!stack.empty())
+ {
+ auto lastToken = stack.top();
+ lastToken->increaseCounter();
+ }
+
return true;
}
return false;
}
- int NlohmannJSONReader::readStartList()
+ ReaderInterface::ReadStartListReturnType NlohmannJSONReader::readStartList()
{
- nlohmann::json current_json = getNextAndIncrease();
- int c = current_json.size();
- auto newToken = std::make_shared<NlohmannJSONReaderToken>(data::Descriptor::eList, current_json);
+ auto lastToken = stack.top();
+
+ auto current_json = lastToken->getNextElement();
+ if (current_json.is_null())
+ {
+ lastToken->increaseCounter(); // no need to call readEndList anymore
+ return {0, false};
+ }
+
+ if (!current_json.is_object())
+ {
+ throw error::AronException("NlohmannJSONReader", "readStartList", "The json is not an object. Every part of the data must be an object containing the type and the value.");
+ }
+
+ if (current_json.at(io::Data::READER_WRITER_TYPE_SLUG) != io::Data::READER_WRITER_LIST_TYPENAME_SLUG)
+ {
+ throw error::StringNotValidException("NlohmannJSONReader", "readStartList", "The json is not of list type!", current_json.at(io::Data::READER_WRITER_TYPE_SLUG), io::Data::READER_WRITER_LIST_TYPENAME_SLUG);
+ }
+
+ auto newToken = std::make_shared<NlohmannJSONReaderToken>(current_json);
+ int c = newToken->getElementChildrenSize();
stack.push(newToken);
- return c;
+ return {c, true};
}
bool NlohmannJSONReader::readEndList()
@@ -105,68 +145,150 @@ namespace armarx::aron::dataIO::reader
if (token->finishedElement())
{
stack.pop();
+
+ auto lastToken = stack.top();
+ lastToken->increaseCounter();
+
return true;
}
return false;
}
// Complex Data
- std::tuple<std::vector<int>, std::string> NlohmannJSONReader::readStartNDArray()
+ ReaderInterface::ReadStartNDArrayReturnType NlohmannJSONReader::readStartNDArray()
{
- nlohmann::json j = getNext();
- std::vector<int> dims = j[io::Data::READER_WRITER_NDARRAY_DIMENSIONS_SLUG];
- std::string type = j[io::Data::READER_WRITER_NDARRAY_TYPE_SLUG];
- return {dims, type};
+ auto lastToken = stack.top();
+
+ auto current_json = lastToken->getNextElement();
+ if (current_json.is_null())
+ {
+ lastToken->increaseCounter();
+ return {{}, "", false};
+ }
+
+ if (current_json.at(io::Data::READER_WRITER_TYPE_SLUG) != io::Data::READER_WRITER_NDARRAY_TYPENAME_SLUG)
+ {
+ throw error::StringNotValidException("NlohmannJSONReader", "readStartNDArray", "The json is not of ndarray type!", current_json.at(io::Data::READER_WRITER_TYPE_SLUG), io::Data::READER_WRITER_NDARRAY_TYPENAME_SLUG);
+ }
+
+ std::vector<int> dims = current_json.at(io::Data::READER_WRITER_NDARRAY_DIMENSIONS_SLUG);
+ std::string type = current_json.at(io::Data::READER_WRITER_NDARRAY_TYPE_SLUG);
+ return {dims, type, true};
}
- bool NlohmannJSONReader::readEndNDArray(unsigned char* data)
+ void NlohmannJSONReader::readEndNDArray(unsigned char* data)
{
if (data == NULL)
{
throw error::AronException("NlohmannJSONReader", "readEndNDArray", "The corresponding data pointer of an complex aron ptr is NULL.");
}
- nlohmann::json j = getNextAndIncrease();
- std::vector<int> dims = j[io::Data::READER_WRITER_NDARRAY_DIMENSIONS_SLUG];
- std::vector<unsigned char> d = j[io::Data::READER_WRITER_NDARRAY_DATA_SLUG];
+
+ auto lastToken = stack.top();
+
+ auto current_json = lastToken->getNextElement();
+ std::vector<int> dims = current_json.at(io::Data::READER_WRITER_NDARRAY_DIMENSIONS_SLUG);
+ std::vector<unsigned char> d = current_json.at(io::Data::READER_WRITER_NDARRAY_DATA_SLUG);
memcpy(data, d.data(), std::accumulate(std::begin(dims), std::end(dims), 1, std::multiplies<int>()));
- return true;
+ lastToken->increaseCounter();
}
// Read primitives
- void NlohmannJSONReader::readPrimitive(int& t)
+ ReaderInterface::ReadPrimitiveReturnType NlohmannJSONReader::readStartPrimitive()
{
- nlohmann::json j = getNextAndIncrease();
- t = j;
+ auto lastToken = stack.top();
+
+ auto current_json = lastToken->getNextElement();
+ if (current_json.is_null())
+ {
+ lastToken->increaseCounter();
+ return {false};
+ }
+
+ return {true};
+ }
+
+ void NlohmannJSONReader::readEndPrimitive(int& t)
+ {
+ auto lastToken = stack.top();
+ auto j = lastToken->getNextElement();
+
+ if (j.at(io::Data::READER_WRITER_TYPE_SLUG) != io::Data::READER_WRITER_INT_TYPENAME_SLUG)
+ {
+ throw error::StringNotValidException("NlohmannJSONReader", "readEndPrimitive", "The json is not of int type!", j.at(io::Data::READER_WRITER_TYPE_SLUG), io::Data::READER_WRITER_INT_TYPENAME_SLUG);
+ }
+
+ lastToken->increaseCounter();
+ t = j.at(io::Data::READER_WRITER_VALUE_SLUG);
}
- void NlohmannJSONReader::readPrimitive(long& t)
+ void NlohmannJSONReader::readEndPrimitive(long& t)
{
- nlohmann::json j = getNextAndIncrease();
- t = j;
+ auto lastToken = stack.top();
+ auto j = lastToken->getNextElement();
+
+ if (j.at(io::Data::READER_WRITER_TYPE_SLUG) != io::Data::READER_WRITER_LONG_TYPENAME_SLUG)
+ {
+ throw error::StringNotValidException("NlohmannJSONReader", "readEndPrimitive", "The json is not of long type!", j.at(io::Data::READER_WRITER_TYPE_SLUG), io::Data::READER_WRITER_LONG_TYPENAME_SLUG);
+ }
+
+ lastToken->increaseCounter();
+ t = j.at(io::Data::READER_WRITER_VALUE_SLUG);
}
- void NlohmannJSONReader::readPrimitive(float& t)
+ void NlohmannJSONReader::readEndPrimitive(float& t)
{
- nlohmann::json j = getNextAndIncrease();
- t = j;
+ auto lastToken = stack.top();
+ auto j = lastToken->getNextElement();
+
+ if (j.at(io::Data::READER_WRITER_TYPE_SLUG) != io::Data::READER_WRITER_FLOAT_TYPENAME_SLUG)
+ {
+ throw error::StringNotValidException("NlohmannJSONReader", "readEndPrimitive", "The json is not of float type!", j.at(io::Data::READER_WRITER_TYPE_SLUG), io::Data::READER_WRITER_FLOAT_TYPENAME_SLUG);
+ }
+
+ lastToken->increaseCounter();
+ t = j.at(io::Data::READER_WRITER_VALUE_SLUG);
}
- void NlohmannJSONReader::readPrimitive(double& t)
+ void NlohmannJSONReader::readEndPrimitive(double& t)
{
- nlohmann::json j = getNextAndIncrease();
- t = j;
+ auto lastToken = stack.top();
+ auto j = lastToken->getNextElement();
+
+ if (j.at(io::Data::READER_WRITER_TYPE_SLUG) != io::Data::READER_WRITER_DOUBLE_TYPENAME_SLUG)
+ {
+ throw error::StringNotValidException("NlohmannJSONReader", "readEndPrimitive", "The json is not of double type!", j.at(io::Data::READER_WRITER_TYPE_SLUG), io::Data::READER_WRITER_DOUBLE_TYPENAME_SLUG);
+ }
+
+ lastToken->increaseCounter();
+ t = j.at(io::Data::READER_WRITER_VALUE_SLUG);
}
- void NlohmannJSONReader::readPrimitive(std::string& t)
+ void NlohmannJSONReader::readEndPrimitive(std::string& t)
{
- nlohmann::json j = getNextAndIncrease();
- t = j;
+ auto lastToken = stack.top();
+ auto j = lastToken->getNextElement();
+
+ if (j.at(io::Data::READER_WRITER_TYPE_SLUG) != io::Data::READER_WRITER_STRING_TYPENAME_SLUG)
+ {
+ throw error::StringNotValidException("NlohmannJSONReader", "readEndPrimitive", "The json is not of string type!", j.at(io::Data::READER_WRITER_TYPE_SLUG), io::Data::READER_WRITER_STRING_TYPENAME_SLUG);
+ }
+
+ lastToken->increaseCounter();
+ t = j.at(io::Data::READER_WRITER_VALUE_SLUG);
}
- void NlohmannJSONReader::readPrimitive(bool& t)
+ void NlohmannJSONReader::readEndPrimitive(bool& t)
{
- nlohmann::json j = getNextAndIncrease();
- t = j;
+ auto lastToken = stack.top();
+ auto j = lastToken->getNextElement();
+
+ if (j.at(io::Data::READER_WRITER_TYPE_SLUG) != io::Data::READER_WRITER_BOOL_TYPENAME_SLUG)
+ {
+ throw error::StringNotValidException("NlohmannJSONReader", "readEndPrimitive", "The json is not of bool type!", j.at(io::Data::READER_WRITER_TYPE_SLUG), io::Data::READER_WRITER_BOOL_TYPENAME_SLUG);
+ }
+
+ lastToken->increaseCounter();
+ t = j.at(io::Data::READER_WRITER_VALUE_SLUG);
}
std::string NlohmannJSONReader::readKey()
@@ -181,21 +303,25 @@ namespace armarx::aron::dataIO::reader
token->setCurrentKey(k);
}
+ void NlohmannJSONReader::readNull()
+ {
+ auto token = stack.top();
+ auto current_json = token->getNextElement();
+ if (!current_json.is_null())
+ {
+ throw error::AronException("NlohmannJSONReader", "readNull", "The value of a json is not null!");
+ }
+ token->increaseCounter();
+ }
+
// Helper functions
- data::Descriptor NlohmannJSONReader::getTypeOfNext(const type::Descriptor hint) const
+ data::Descriptor NlohmannJSONReader::getTypeOfNext() const
{
if (stack.empty())
{
- if (hint == type::Descriptor::eUnknown)
- {
- return data::Descriptor::eDict;
- }
- else
- {
- return Resolver::GetCorresponding(hint);
- }
+ return data::Descriptor::eDict;
}
auto token = stack.top();
- return token->getTypeOfNext(hint);
+ return token->getTypeOfNext();
}
}
diff --git a/source/RobotAPI/libraries/aron/core/io/dataIO/reader/nlohmannJSON/NlohmannJSONReader.h b/source/RobotAPI/libraries/aron/core/io/dataIO/reader/nlohmannJSON/NlohmannJSONReader.h
index 7749530308b806da5898881879cef6df26143358..c2cf0b3ef9ea8fdb2fc6efeecedfd27bd5ea852b 100644
--- a/source/RobotAPI/libraries/aron/core/io/dataIO/reader/nlohmannJSON/NlohmannJSONReader.h
+++ b/source/RobotAPI/libraries/aron/core/io/dataIO/reader/nlohmannJSON/NlohmannJSONReader.h
@@ -46,33 +46,33 @@ namespace armarx::aron::dataIO::reader
NlohmannJSONReader(const nlohmann::json& n);
NlohmannJSONReader(const std::string& n);
- virtual int readStartDict() override;
+ virtual ReadStartDictReturnType readStartDict() override;
virtual bool readEndDict() override;
- virtual int readStartList() override;
+
+ virtual ReadStartListReturnType readStartList() override;
virtual bool readEndList() override;
- virtual std::tuple<std::vector<int>, std::string> readStartNDArray() override;
- virtual bool readEndNDArray(unsigned char*) override;
+ virtual ReadStartNDArrayReturnType readStartNDArray() override;
+ virtual void readEndNDArray(unsigned char*) override;
- virtual void readPrimitive(int&) override;
- virtual void readPrimitive(long&) override;
- virtual void readPrimitive(float&) override;
- virtual void readPrimitive(double&) override;
- virtual void readPrimitive(std::string&) override;
- virtual void readPrimitive(bool&) override;
+ virtual ReadPrimitiveReturnType readStartPrimitive() override;
+ virtual void readEndPrimitive(int&) override;
+ virtual void readEndPrimitive(long&) override;
+ virtual void readEndPrimitive(float&) override;
+ virtual void readEndPrimitive(double&) override;
+ virtual void readEndPrimitive(std::string&) override;
+ virtual void readEndPrimitive(bool&) override;
virtual std::string readKey() override;
virtual void loadMember(const std::string&) override;
- virtual data::Descriptor getTypeOfNext(const type::Descriptor hint = type::Descriptor::eUnknown) const override;
+ virtual void readNull() override;
- private:
- nlohmann::json getNext();
- nlohmann::json getNextAndIncrease();
+ virtual data::Descriptor getTypeOfNext() const override;
private:
+ bool readFirstStartDict = false;
nlohmann::json input;
- bool readInitialStart = false;
std::stack<NlohmannJSONReaderTokenPtr> stack;
};
}
diff --git a/source/RobotAPI/libraries/aron/core/io/dataIO/reader/nlohmannJSON/NlohmannJSONReaderToken.h b/source/RobotAPI/libraries/aron/core/io/dataIO/reader/nlohmannJSON/NlohmannJSONReaderToken.h
index 04acd920b51789d2ce925f8aab48c268baf3fed7..0e273108d6944bbabad1e3a34d57e22177e5a138 100644
--- a/source/RobotAPI/libraries/aron/core/io/dataIO/reader/nlohmannJSON/NlohmannJSONReaderToken.h
+++ b/source/RobotAPI/libraries/aron/core/io/dataIO/reader/nlohmannJSON/NlohmannJSONReaderToken.h
@@ -50,9 +50,14 @@ namespace armarx::aron::dataIO::reader
// constructors
NlohmannJSONReaderToken() = delete;
- NlohmannJSONReaderToken(const data::Descriptor desc, const nlohmann::json& data)
+ NlohmannJSONReaderToken(const nlohmann::json& data)
{
- descriptor = desc;
+ if (!data.is_object())
+ {
+ throw error::AronException("NlohmannJSONReaderToken", "NlohmannJSONReaderToken", "Received an invalid json instance for a reader. Allowed are only objects.");
+ }
+
+ descriptor = data::STRING_TO_DESCRIPTOR(data.at(io::Data::READER_WRITER_TYPE_SLUG));
element = data;
switch (descriptor)
@@ -61,79 +66,78 @@ namespace armarx::aron::dataIO::reader
{
for (auto it = data.begin(); it != data.end(); ++it)
{
+ if (it.key() == io::Data::READER_WRITER_TYPE_SLUG || it.key() == io::Data::READER_WRITER_VERSION_SLUG)
+ {
+ continue;
+ }
allMemberNames.push_back(it.key());
}
- childrenSize = data.size();
+ childrenSize = allMemberNames.size();
break;
}
case data::Descriptor::eList:
{
- childrenSize = data.size();
+ childrenSize = data.at(io::Data::READER_WRITER_VALUE_SLUG).size();
break;
}
default:
- throw error::DescriptorNotValidException("NlohmannJSONReaderToken", "NlohmannJSONReaderToken", "Received an invalid instance for a reader token.", desc);
+ throw error::DescriptorNotValidException("NlohmannJSONReaderToken", "NlohmannJSONReaderToken", "Received an invalid instance for a reader token.", descriptor);
}
}
- data::Descriptor getTypeOfNext(const type::Descriptor hint) const override
+ data::Descriptor getTypeOfNext() const override
{
const nlohmann::json next = getNextElement();
- if (next.is_object())
- {
- // Check if specific NDArray key exists
- if (next.find(io::Data::READER_WRITER_NDARRAY_DATA_SLUG) != next.end())
- {
- return data::Descriptor::eNDArray;
- }
- return data::Descriptor::eDict;
- }
- if (next.is_array())
+ if (next.is_null())
{
- return data::Descriptor::eList;
+ return data::Descriptor::eUnknown;
}
- if (next.is_number_integer())
- {
- data::Descriptor d = Resolver::GetFirstIfRelated(
- Resolver::GetCorresponding(hint),
- data::Descriptor::eLong);
- return d;
- }
- if (next.is_number_float())
- {
- data::Descriptor d = Resolver::GetFirstIfRelated(
- Resolver::GetCorresponding(hint),
- data::Descriptor::eDouble);
- return d;
- }
- if (next.is_boolean())
+ std::string type_as_str = next.at(io::Data::READER_WRITER_TYPE_SLUG);
+ data::Descriptor desc = data::STRING_TO_DESCRIPTOR(type_as_str);
+ return desc;
+ }
+
+ nlohmann::json getNextElement() const override
+ {
+ if (!hasNextElement())
{
- return data::Descriptor::eBool;
+ return nlohmann::json();
}
- if (next.is_string())
+
+ switch (descriptor)
{
- return data::Descriptor::eString;
+ case data::Descriptor::eDict:
+ {
+ nlohmann::json ret = element.at(getCurrentKey());
+ return ret;
+ }
+ case data::Descriptor::eList:
+ {
+ nlohmann::json ret = element.at(io::Data::READER_WRITER_VALUE_SLUG).at(currentIndex);
+ return ret;
+ }
+ default:
+ throw error::DescriptorNotValidException("NlohmannJSONReaderToken", "getNextElement", "Could not resove a type of a json. Allowed are only container types due to performance", descriptor);
}
- throw error::AronException("NlohmannJSONReaderToken", "getTypeOfNextElement", "Could not determine the type of an nlohmann::json object. Could not convert to data::Descriptor enum.");
}
- nlohmann::json getNextElement() const override
+ virtual bool hasNextElement() const override
{
switch (descriptor)
{
case data::Descriptor::eDict:
{
- nlohmann::json ret = element[getCurrentKey()];
- return ret;
+ nlohmann::json ret = element.at(getCurrentKey());
+ return !ret.is_null();
}
case data::Descriptor::eList:
{
- nlohmann::json ret = element[currentIndex];
- return ret;
+ auto size = element.at(io::Data::READER_WRITER_VALUE_SLUG).size();
+ return size >= currentIndex;
}
default:
- throw error::DescriptorNotValidException("NlohmannJSONReaderToken", "getNextElement", "Could not resove a type of a navigator. Allowed are only container types due to performance", descriptor);
+ throw error::DescriptorNotValidException("NlohmannJSONReaderToken", "hasData", "Could not resove a type of a json. Allowed are only container types due to performance", descriptor);
}
}
diff --git a/source/RobotAPI/libraries/aron/core/io/dataIO/visitor/Visitor.cpp b/source/RobotAPI/libraries/aron/core/io/dataIO/visitor/Visitor.cpp
index b5be280a8cfe72963afa63b88a900b6885250c33..e6646349096ddc8f312a0a03b8affd4a0c767f43 100644
--- a/source/RobotAPI/libraries/aron/core/io/dataIO/visitor/Visitor.cpp
+++ b/source/RobotAPI/libraries/aron/core/io/dataIO/visitor/Visitor.cpp
@@ -35,11 +35,23 @@ namespace armarx::aron::dataIO
void Visitor::VisitAndSetup(WriterInterface& writer, const datanavigator::NavigatorPtr& aron)
{
- VisitAndSetup(writer, aron->getResult());
+ if (!aron)
+ {
+ writer.writeNull();
+ return;
+ }
+
+ VisitAndSetup(writer, aron->toAronPtr());
}
void Visitor::VisitAndSetup(WriterInterface& writer, const data::AronDataPtr& aron)
{
+ if (!aron)
+ {
+ writer.writeNull();
+ return;
+ }
+
data::Descriptor desc = Resolver::GetDescriptor(aron);
switch (desc)
{
@@ -49,6 +61,7 @@ namespace armarx::aron::dataIO
writer.writeStartDict();
for (const auto& [key, value] : casted->elements)
{
+ //std::cout << "Visit writer for " << key << std::endl;
writer.writeKey(key);
Visitor::VisitAndSetup(writer, value);
}
diff --git a/source/RobotAPI/libraries/aron/core/io/dataIO/writer/navigator/NavigatorWriter.cpp b/source/RobotAPI/libraries/aron/core/io/dataIO/writer/navigator/NavigatorWriter.cpp
index 15f57ecf56e00bf09720c4250e8972b9f79f546d..8777b5d0c9691d75a6dce7c780c4e3f6c5c6ce04 100644
--- a/source/RobotAPI/libraries/aron/core/io/dataIO/writer/navigator/NavigatorWriter.cpp
+++ b/source/RobotAPI/libraries/aron/core/io/dataIO/writer/navigator/NavigatorWriter.cpp
@@ -29,7 +29,6 @@
#include <RobotAPI/libraries/aron/core/navigator/data/AllNavigators.h>
-
namespace armarx::aron::dataIO::writer
{
@@ -53,7 +52,7 @@ namespace armarx::aron::dataIO::writer
{
Path path = generatePath();
auto data = std::make_shared<datanavigator::DictNavigator>(path);
- auto new_token = std::make_shared<NavigatorWriterToken>(data::Descriptor::eDict, data);
+ auto new_token = std::make_shared<NavigatorWriterToken>(data);
stack.push(new_token);
}
@@ -73,7 +72,7 @@ namespace armarx::aron::dataIO::writer
{
Path path = generatePath();
auto data = std::make_shared<datanavigator::ListNavigator>(path);
- auto new_token = std::make_shared<NavigatorWriterToken>(data::Descriptor::eList, data);
+ auto new_token = std::make_shared<NavigatorWriterToken>(data);
stack.push(new_token);
}
@@ -157,6 +156,13 @@ namespace armarx::aron::dataIO::writer
token->addElement(aron);
}
+ void NavigatorWriter::writeNull()
+ {
+ Path path = generatePath();
+ NavigatorWriterTokenPtr token = stack.top();
+ token->addElement(nullptr);
+ }
+
void NavigatorWriter::writeKey(const std::string& k)
{
auto token = stack.top();
diff --git a/source/RobotAPI/libraries/aron/core/io/dataIO/writer/navigator/NavigatorWriter.h b/source/RobotAPI/libraries/aron/core/io/dataIO/writer/navigator/NavigatorWriter.h
index 3467d630a3301e4ed962f4529057d4be4089a4b5..483883b85b9205adac25ac69fd738f0298895e87 100644
--- a/source/RobotAPI/libraries/aron/core/io/dataIO/writer/navigator/NavigatorWriter.h
+++ b/source/RobotAPI/libraries/aron/core/io/dataIO/writer/navigator/NavigatorWriter.h
@@ -30,6 +30,7 @@
// ArmarX
#include <RobotAPI/libraries/aron/core/Concepts.h>
#include <RobotAPI/libraries/aron/core/navigator/data/Navigator.h>
+#include <RobotAPI/libraries/aron/core/navigator/data/NavigatorFactory.h>
#include <RobotAPI/libraries/aron/core/io/dataIO/writer/navigator/NavigatorWriterToken.h>
namespace armarx::aron::dataIO::writer
@@ -57,6 +58,8 @@ namespace armarx::aron::dataIO::writer
virtual void writePrimitive(const std::string&) override;
virtual void writePrimitive(const bool) override;
+ virtual void writeNull() override;
+
virtual void writeKey(const std::string&) override;
datanavigator::NavigatorPtr getResult() const
diff --git a/source/RobotAPI/libraries/aron/core/io/dataIO/writer/navigator/NavigatorWriterToken.h b/source/RobotAPI/libraries/aron/core/io/dataIO/writer/navigator/NavigatorWriterToken.h
index 8f10dd89e20a4ad0d606a6b2054d152679a76bee..c7de6070ec9688fcd39a5bd6d2b027c82de70224 100644
--- a/source/RobotAPI/libraries/aron/core/io/dataIO/writer/navigator/NavigatorWriterToken.h
+++ b/source/RobotAPI/libraries/aron/core/io/dataIO/writer/navigator/NavigatorWriterToken.h
@@ -45,9 +45,11 @@ namespace armarx::aron::dataIO::writer
public:
// constructor
NavigatorWriterToken() = delete;
- NavigatorWriterToken(const data::Descriptor desc, const datanavigator::NavigatorPtr& d)
+ NavigatorWriterToken(const datanavigator::NavigatorPtr& d)
{
- descriptor = desc;
+ ARMARX_CHECK_NOT_NULL(d);
+
+ descriptor = d->getDescriptor();
element = d;
}
diff --git a/source/RobotAPI/libraries/aron/core/io/dataIO/writer/nlohmannJSON/NlohmannJSONWriter.cpp b/source/RobotAPI/libraries/aron/core/io/dataIO/writer/nlohmannJSON/NlohmannJSONWriter.cpp
index b768c5da072915d37406b2c0d7f3fb9bb5839dd9..b12b59774aa483a71dfbff040ad493abda841e79 100644
--- a/source/RobotAPI/libraries/aron/core/io/dataIO/writer/nlohmannJSON/NlohmannJSONWriter.cpp
+++ b/source/RobotAPI/libraries/aron/core/io/dataIO/writer/nlohmannJSON/NlohmannJSONWriter.cpp
@@ -32,7 +32,8 @@ namespace armarx::aron::dataIO::writer
void NlohmannJSONWriter::writeStartDict()
{
nlohmann::json data;
- auto new_token = std::make_shared<NlohmannJSONWriterToken>(data::Descriptor::eDict, data);
+ data[io::Data::READER_WRITER_TYPE_SLUG] = io::Data::READER_WRITER_DICT_TYPENAME_SLUG;
+ auto new_token = std::make_shared<NlohmannJSONWriterToken>(data);
stack.push(new_token);
}
@@ -51,7 +52,9 @@ namespace armarx::aron::dataIO::writer
void NlohmannJSONWriter::writeStartList()
{
nlohmann::json data;
- auto new_token = std::make_shared<NlohmannJSONWriterToken>(data::Descriptor::eList, data);
+ data[io::Data::READER_WRITER_TYPE_SLUG] = io::Data::READER_WRITER_LIST_TYPENAME_SLUG;
+ data[io::Data::READER_WRITER_VALUE_SLUG] = nlohmann::json::array();
+ auto new_token = std::make_shared<NlohmannJSONWriterToken>(data);
stack.push(new_token);
}
@@ -72,6 +75,7 @@ namespace armarx::aron::dataIO::writer
{
NlohmannJSONWriterTokenPtr token = stack.top();
nlohmann::json j;
+ j[io::Data::READER_WRITER_TYPE_SLUG] = io::Data::READER_WRITER_NDARRAY_TYPENAME_SLUG;
j[io::Data::READER_WRITER_NDARRAY_DIMENSIONS_SLUG] = dims;
j[io::Data::READER_WRITER_NDARRAY_TYPE_SLUG] = t;
int elements = std::accumulate(std::begin(dims), std::end(dims), 1, std::multiplies<int>());
@@ -85,45 +89,63 @@ namespace armarx::aron::dataIO::writer
void NlohmannJSONWriter::writePrimitive(const int x)
{
auto token = stack.top();
- nlohmann::json j(x);
+ nlohmann::json j;
+ j[io::Data::READER_WRITER_TYPE_SLUG] = io::Data::READER_WRITER_INT_TYPENAME_SLUG;
+ j[io::Data::READER_WRITER_VALUE_SLUG] = x;
token->addElement(j);
}
void NlohmannJSONWriter::writePrimitive(const long x)
{
auto token = stack.top();
- nlohmann::json j(x);
+ nlohmann::json j;
+ j[io::Data::READER_WRITER_TYPE_SLUG] = io::Data::READER_WRITER_LONG_TYPENAME_SLUG;
+ j[io::Data::READER_WRITER_VALUE_SLUG] = x;
token->addElement(j);
}
void NlohmannJSONWriter::writePrimitive(const float x)
{
auto token = stack.top();
- nlohmann::json j(x);
+ nlohmann::json j;
+ j[io::Data::READER_WRITER_TYPE_SLUG] = io::Data::READER_WRITER_FLOAT_TYPENAME_SLUG;
+ j[io::Data::READER_WRITER_VALUE_SLUG] = x;
token->addElement(j);
}
void NlohmannJSONWriter::writePrimitive(const double x)
{
auto token = stack.top();
- nlohmann::json j(x);
+ nlohmann::json j;
+ j[io::Data::READER_WRITER_TYPE_SLUG] = io::Data::READER_WRITER_DOUBLE_TYPENAME_SLUG;
+ j[io::Data::READER_WRITER_VALUE_SLUG] = x;
token->addElement(j);
}
void NlohmannJSONWriter::writePrimitive(const std::string& x)
{
auto token = stack.top();
- nlohmann::json j(x);
+ nlohmann::json j;
+ j[io::Data::READER_WRITER_TYPE_SLUG] = io::Data::READER_WRITER_STRING_TYPENAME_SLUG;
+ j[io::Data::READER_WRITER_VALUE_SLUG] = x;
token->addElement(j);
}
void NlohmannJSONWriter::writePrimitive(const bool x)
{
auto token = stack.top();
- nlohmann::json j(x);
+ nlohmann::json j;
+ j[io::Data::READER_WRITER_TYPE_SLUG] = io::Data::READER_WRITER_BOOL_TYPENAME_SLUG;
+ j[io::Data::READER_WRITER_VALUE_SLUG] = x;
token->addElement(j);
}
+ void NlohmannJSONWriter::writeNull()
+ {
+ auto token = stack.top();
+ token->addElement(nullptr);
+ }
+
void NlohmannJSONWriter::writeKey(const std::string& k)
{
auto token = stack.top();
diff --git a/source/RobotAPI/libraries/aron/core/io/dataIO/writer/nlohmannJSON/NlohmannJSONWriter.h b/source/RobotAPI/libraries/aron/core/io/dataIO/writer/nlohmannJSON/NlohmannJSONWriter.h
index f36fa55d53127a2c79a6d0b9629718a24cb2070c..44bf3c002101982f4e1f8af86e34838798bcd320 100644
--- a/source/RobotAPI/libraries/aron/core/io/dataIO/writer/nlohmannJSON/NlohmannJSONWriter.h
+++ b/source/RobotAPI/libraries/aron/core/io/dataIO/writer/nlohmannJSON/NlohmannJSONWriter.h
@@ -57,6 +57,8 @@ namespace armarx::aron::dataIO::writer
virtual void writePrimitive(const std::string&) override;
virtual void writePrimitive(const bool) override;
+ virtual void writeNull() override;
+
virtual void writeKey(const std::string&) override;
nlohmann::json getResult() const
diff --git a/source/RobotAPI/libraries/aron/core/io/dataIO/writer/nlohmannJSON/NlohmannJSONWriterToken.h b/source/RobotAPI/libraries/aron/core/io/dataIO/writer/nlohmannJSON/NlohmannJSONWriterToken.h
index 9f37b4993a86c7006cfe8ad98b40ea006c09ff53..ef8ded6b28d1b9ca65bfdf9a3470e39157030390 100644
--- a/source/RobotAPI/libraries/aron/core/io/dataIO/writer/nlohmannJSON/NlohmannJSONWriterToken.h
+++ b/source/RobotAPI/libraries/aron/core/io/dataIO/writer/nlohmannJSON/NlohmannJSONWriterToken.h
@@ -47,9 +47,9 @@ namespace armarx::aron::dataIO::writer
public:
// constructor
NlohmannJSONWriterToken() = delete;
- NlohmannJSONWriterToken(const data::Descriptor desc, const nlohmann::json& d)
+ NlohmannJSONWriterToken(const nlohmann::json& d)
{
- descriptor = desc;
+ descriptor = data::STRING_TO_DESCRIPTOR(d[io::Data::READER_WRITER_TYPE_SLUG]);
element = d;
}
@@ -66,7 +66,7 @@ namespace armarx::aron::dataIO::writer
}
case data::Descriptor::eList:
{
- element.push_back(n);
+ element[io::Data::READER_WRITER_VALUE_SLUG].push_back(n);
currentIndex++;
break;
}
diff --git a/source/RobotAPI/libraries/aron/core/io/typeIO/Reader.cpp b/source/RobotAPI/libraries/aron/core/io/typeIO/Reader.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..4fe8d311a52e250284f99917ca02d2afd6cfeaf8
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/io/typeIO/Reader.cpp
@@ -0,0 +1,21 @@
+/*
+* 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/>.
+*
+* @author Fabian Peller (fabian dot peller at kit dot edu)
+* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#include "Reader.h"
diff --git a/source/RobotAPI/libraries/aron/core/io/typeIO/Reader.h b/source/RobotAPI/libraries/aron/core/io/typeIO/Reader.h
index 738b4c351b068bbfc54b363cf68901dbb6035837..6a070543e33b69691f381da73283634bb2308d1a 100644
--- a/source/RobotAPI/libraries/aron/core/io/typeIO/Reader.h
+++ b/source/RobotAPI/libraries/aron/core/io/typeIO/Reader.h
@@ -38,33 +38,111 @@ namespace armarx::aron::typeIO
class ReaderInterface
{
public:
- virtual std::tuple<std::string, int> readStartObject() = 0;
+ struct ReadStartObjectReturnType
+ {
+ std::string name;
+ unsigned int elements;
+ type::Maybe maybe;
+ };
+ virtual ReadStartObjectReturnType readStartObject() = 0;
virtual bool readEndObject() = 0;
- virtual int readStartList() = 0;
+
+ struct ReadStartListReturnType
+ {
+ unsigned int elements;
+ type::Maybe maybe;
+ };
+ virtual ReadStartListReturnType readStartList() = 0;
virtual bool readEndList() = 0;
- virtual int readStartDict() = 0;
+
+ struct ReadStartDictReturnType
+ {
+ unsigned int elements;
+ type::Maybe maybe;
+ };
+ virtual ReadStartDictReturnType readStartDict() = 0;
virtual bool readEndDict() = 0;
- virtual int readStartTuple() = 0;
+
+ struct ReadStartTupleReturnType
+ {
+ unsigned int elements;
+ type::Maybe maybe;
+ };
+ virtual ReadStartTupleReturnType readStartTuple() = 0;
virtual bool readEndTuple() = 0;
- virtual int readStartPair() = 0;
+
+ struct ReadStartPairReturnType
+ {
+ unsigned int elements;
+ type::Maybe maybe;
+ };
+ virtual ReadStartPairReturnType readStartPair() = 0;
virtual bool readEndPair() = 0;
- virtual std::tuple<std::vector<int>, std::string> readEigenMatrix() = 0;
- virtual std::string readEigenQuaternion() = 0;
- virtual std::tuple<unsigned int, unsigned int, std::string> readIVTCByteImage() = 0;
- virtual std::tuple<std::vector<int>, std::string> readOpenCVMat() = 0;
- virtual std::tuple<unsigned int, unsigned int, std::string> readPCLPointCloud() = 0;
- virtual void readPosition() = 0;
- virtual void readOrientation() = 0;
- virtual void readPose() = 0;
-
- virtual void readInt() = 0;
- virtual void readLong() = 0;
- virtual void readFloat() = 0;
- virtual void readDouble() = 0;
- virtual void readString() = 0;
- virtual void readBool() = 0;
- virtual void readTime() = 0;
+ struct ReadEigenMatrixReturnType
+ {
+ unsigned int rows;
+ unsigned int cols;
+ std::string type;
+ type::Maybe maybe;
+ };
+ virtual ReadEigenMatrixReturnType readEigenMatrix() = 0;
+
+ struct ReadEigenQuaternionReturnType
+ {
+ std::string type;
+ type::Maybe maybe;
+ };
+ virtual ReadEigenQuaternionReturnType readEigenQuaternion() = 0;
+
+ struct ReadIVTCbyteImageReturnType
+ {
+ type::Maybe maybe;
+ };
+ virtual ReadIVTCbyteImageReturnType readIVTCByteImage() = 0;
+
+ struct ReadOpenCVMatReturnType
+ {
+ type::Maybe maybe;
+ };
+ virtual ReadOpenCVMatReturnType readOpenCVMat() = 0;
+
+ struct ReadPCLPointCloudReturnType
+ {
+ std::string type;
+ type::Maybe maybe;
+ };
+ virtual ReadPCLPointCloudReturnType readPCLPointCloud() = 0;
+
+ struct ReadPositionReturnType
+ {
+ type::Maybe maybe;
+ };
+ virtual ReadPositionReturnType readPosition() = 0;
+
+ struct ReadOrientationReturnType
+ {
+ type::Maybe maybe;
+ };
+ virtual ReadOrientationReturnType readOrientation() = 0;
+
+ struct ReadPoseReturnType
+ {
+ type::Maybe maybe;
+ };
+ virtual ReadPoseReturnType readPose() = 0;
+
+ struct ReadPrimitiveReturnType
+ {
+ type::Maybe maybe;
+ };
+ virtual ReadPrimitiveReturnType readInt() = 0;
+ virtual ReadPrimitiveReturnType readLong() = 0;
+ virtual ReadPrimitiveReturnType readFloat() = 0;
+ virtual ReadPrimitiveReturnType readDouble() = 0;
+ virtual ReadPrimitiveReturnType readString() = 0;
+ virtual ReadPrimitiveReturnType readBool() = 0;
+ virtual ReadPrimitiveReturnType readTime() = 0;
virtual std::string readKey() = 0;
virtual void loadMember(const std::string&) = 0;
diff --git a/source/RobotAPI/libraries/aron/core/io/typeIO/Writer.cpp b/source/RobotAPI/libraries/aron/core/io/typeIO/Writer.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..e78a27bd58bbba3fa6f64c28b7fa8d721b94c0fd
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/io/typeIO/Writer.cpp
@@ -0,0 +1,21 @@
+/*
+* 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/>.
+*
+* @author Fabian Peller (fabian dot peller at kit dot edu)
+* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#include "Writer.h"
diff --git a/source/RobotAPI/libraries/aron/core/io/typeIO/Writer.h b/source/RobotAPI/libraries/aron/core/io/typeIO/Writer.h
index 552b5fc1a9427357ee9b971115fc49dda89d3745..7dc762492105af180e30773518b3b8ef1eaae963 100644
--- a/source/RobotAPI/libraries/aron/core/io/typeIO/Writer.h
+++ b/source/RobotAPI/libraries/aron/core/io/typeIO/Writer.h
@@ -39,33 +39,106 @@ namespace armarx::aron::typeIO
class WriterInterface
{
public:
- virtual void writeStartObject(const std::string&) = 0;
+ struct WriteStartObjectInput
+ {
+ std::string name;
+ type::Maybe maybe;
+ };
+ virtual void writeStartObject(const WriteStartObjectInput&) = 0;
virtual void writeEndObject() = 0;
- virtual void writeStartList() = 0;
+
+ struct WriteStartListInput
+ {
+ type::Maybe maybe;
+ };
+ virtual void writeStartList(const WriteStartListInput&) = 0;
virtual void writeEndList() = 0;
- virtual void writeStartDict() = 0;
+
+ struct WriteStartDictInput
+ {
+ type::Maybe maybe;
+ };
+ virtual void writeStartDict(const WriteStartDictInput&) = 0;
virtual void writeEndDict() = 0;
- virtual void writeStartTuple() = 0;
+
+ struct WriteStartTupleInput
+ {
+ type::Maybe maybe;
+ };
+ virtual void writeStartTuple(const WriteStartTupleInput&) = 0;
virtual void writeEndTuple() = 0;
- virtual void writeStartPair() = 0;
+
+ struct WriteStartPairInput
+ {
+ type::Maybe maybe;
+ };
+ virtual void writeStartPair(const WriteStartPairInput&) = 0;
virtual void writeEndPair() = 0;
- virtual void writeEigenMatrix(const std::vector<int>&, const std::string&) = 0;
- virtual void writeEigenQuaternion(const std::string&) = 0;
- virtual void writeIVTCByteImage(unsigned int w, unsigned int h, const std::string&) = 0;
- virtual void writeOpenCVMat(const std::vector<int>&, const std::string&) = 0;
- virtual void writePCLPointCloud(unsigned int w, unsigned int h, const std::string&) = 0;
- virtual void writePosition() = 0;
- virtual void writeOrientation() = 0;
- virtual void writePose() = 0;
-
- virtual void writeInt() = 0;
- virtual void writeLong() = 0;
- virtual void writeFloat() = 0;
- virtual void writeDouble() = 0;
- virtual void writeString() = 0;
- virtual void writeBool() = 0;
- virtual void writeTime() = 0;
+ struct WriteEigenMatrixInput
+ {
+ unsigned int rows;
+ unsigned int cols;
+ std::string type;
+ type::Maybe maybe;
+ };
+ virtual void writeEigenMatrix(const WriteEigenMatrixInput&) = 0;
+
+ struct WriteEigenQuaternionInput
+ {
+ std::string type;
+ type::Maybe maybe;
+ };
+ virtual void writeEigenQuaternion(const WriteEigenQuaternionInput&) = 0;
+
+ struct WriteIVTCByteImageInput
+ {
+ type::Maybe maybe;
+ };
+ virtual void writeIVTCByteImage(const WriteIVTCByteImageInput&) = 0;
+
+ struct WriteOpenCVMatInput
+ {
+ type::Maybe maybe;
+ };
+ virtual void writeOpenCVMat(const WriteOpenCVMatInput&) = 0;
+
+ struct WritePCLPointCloudInput
+ {
+ std::string type;
+ type::Maybe maybe;
+ };
+ virtual void writePCLPointCloud(const WritePCLPointCloudInput&) = 0;
+
+ struct WritePositionInput
+ {
+ type::Maybe maybe;
+ };
+ virtual void writePosition(const WritePositionInput&) = 0;
+
+ struct WriteOrientationInput
+ {
+ type::Maybe maybe;
+ };
+ virtual void writeOrientation(const WriteOrientationInput&) = 0;
+
+ struct WritePoseInput
+ {
+ type::Maybe maybe;
+ };
+ virtual void writePose(const WritePoseInput&) = 0;
+
+ struct WritePrimitiveInput
+ {
+ type::Maybe maybe;
+ };
+ virtual void writeInt(const WritePrimitiveInput&) = 0;
+ virtual void writeLong(const WritePrimitiveInput&) = 0;
+ virtual void writeFloat(const WritePrimitiveInput&) = 0;
+ virtual void writeDouble(const WritePrimitiveInput&) = 0;
+ virtual void writeString(const WritePrimitiveInput&) = 0;
+ virtual void writeBool(const WritePrimitiveInput&) = 0;
+ virtual void writeTime(const WritePrimitiveInput&) = 0;
virtual void writeKey(const std::string&) = 0;
};
diff --git a/source/RobotAPI/libraries/aron/core/io/typeIO/converter/Converter.cpp b/source/RobotAPI/libraries/aron/core/io/typeIO/converter/Converter.cpp
index 48decff3a1763338b8d2de06c217bfa38b60cf86..f5df053b92c714a3285cc743cb5ebabd49421f87 100644
--- a/source/RobotAPI/libraries/aron/core/io/typeIO/converter/Converter.cpp
+++ b/source/RobotAPI/libraries/aron/core/io/typeIO/converter/Converter.cpp
@@ -34,10 +34,10 @@ namespace armarx::aron::typeIO
{
case type::Descriptor::eObject:
{
- const auto [name, elements] = reader.readStartObject();
- writer.writeStartObject(name);
+ const auto readStartObject = reader.readStartObject();
+ writer.writeStartObject({readStartObject.name, readStartObject.maybe});
- for (int i = 0; i < elements; ++i)
+ for (unsigned int i = 0; i < readStartObject.elements; ++i)
{
std::string key = reader.readKey();
writer.writeKey(key);
@@ -50,8 +50,8 @@ namespace armarx::aron::typeIO
}
case type::Descriptor::eDict:
{
- reader.readStartDict();
- writer.writeStartDict();
+ const auto readStartDict = reader.readStartDict();
+ writer.writeStartDict({readStartDict.maybe});
Converter::ReadAndConvert(reader, writer);
@@ -61,10 +61,10 @@ namespace armarx::aron::typeIO
}
case type::Descriptor::eTuple:
{
- int elements = reader.readStartTuple();
- writer.writeStartTuple();
+ const auto readStartTuple = reader.readStartTuple();
+ writer.writeStartTuple({readStartTuple.maybe});
- for (int i = 0; i < elements; ++i)
+ for (unsigned int i = 0; i < readStartTuple.elements; ++i)
{
Converter::ReadAndConvert(reader, writer);
}
@@ -75,8 +75,8 @@ namespace armarx::aron::typeIO
}
case type::Descriptor::eList:
{
- reader.readStartList();
- writer.writeStartList();
+ const auto readStartList = reader.readStartList();
+ writer.writeStartList({readStartList.maybe});
Converter::ReadAndConvert(reader, writer);
@@ -86,92 +86,92 @@ namespace armarx::aron::typeIO
}
case type::Descriptor::eEigenMatrix:
{
- auto x = reader.readEigenMatrix();
- writer.writeEigenMatrix(std::get<0>(x), std::get<1>(x));
+ const auto readEigenMatrix = reader.readEigenMatrix();
+ writer.writeEigenMatrix({readEigenMatrix.rows, readEigenMatrix.cols, readEigenMatrix.type, readEigenMatrix.maybe});
break;
}
case type::Descriptor::eEigenQuaternion:
{
- auto x = reader.readEigenQuaternion();
- writer.writeEigenQuaternion(x);
+ const auto readEigenQuaternion = reader.readEigenQuaternion();
+ writer.writeEigenQuaternion({readEigenQuaternion.type, readEigenQuaternion.maybe});
break;
}
case type::Descriptor::eIVTCByteImage:
{
- auto x = reader.readIVTCByteImage();
- writer.writeIVTCByteImage(std::get<0>(x), std::get<1>(x), std::get<2>(x));
+ const auto readIVTCByteImage = reader.readIVTCByteImage();
+ writer.writeIVTCByteImage({readIVTCByteImage.maybe});
break;
}
case type::Descriptor::eOpenCVMat:
{
- auto x = reader.readOpenCVMat();
- writer.writeOpenCVMat(std::get<0>(x), std::get<1>(x));
+ const auto readOpenCVMat = reader.readOpenCVMat();
+ writer.writeOpenCVMat({readOpenCVMat.maybe});
break;
}
case type::Descriptor::ePCLPointCloud:
{
- auto x = reader.readPCLPointCloud();
- writer.writePCLPointCloud(std::get<0>(x), std::get<1>(x), std::get<2>(x));
+ const auto readPCLPointCloud = reader.readPCLPointCloud();
+ writer.writePCLPointCloud({readPCLPointCloud.type, readPCLPointCloud.maybe});
break;
}
case type::Descriptor::ePosition:
{
- reader.readPosition();
- writer.writePosition();
+ const auto readPosition = reader.readPosition();
+ writer.writePosition({readPosition.maybe});
break;
}
case type::Descriptor::eOrientation:
{
- reader.readOrientation();
- writer.writeOrientation();
+ const auto readOrientation = reader.readOrientation();
+ writer.writeOrientation({readOrientation.maybe});
break;
}
case type::Descriptor::ePose:
{
- reader.readPose();
- writer.writePose();
+ const auto readPose = reader.readPose();
+ writer.writePose({readPose.maybe});
break;
}
case type::Descriptor::eInt:
{
- reader.readInt();
- writer.writeInt();
+ const auto readInt = reader.readInt();
+ writer.writeInt({readInt.maybe});
break;
}
case type::Descriptor::eLong:
{
- reader.readLong();
- writer.writeLong();
+ const auto readLong = reader.readLong();
+ writer.writeLong({readLong.maybe});
break;
}
case type::Descriptor::eFloat:
{
- reader.readFloat();
- writer.writeFloat();
+ const auto readFloat = reader.readFloat();
+ writer.writeFloat({readFloat.maybe});
break;
}
case type::Descriptor::eDouble:
{
- reader.readDouble();
- writer.writeDouble();
+ const auto readDouble = reader.readDouble();
+ writer.writeDouble({readDouble.maybe});
break;
}
case type::Descriptor::eString:
{
- reader.readString();
- writer.writeString();
+ const auto readString = reader.readString();
+ writer.writeString({readString.maybe});
break;
}
case type::Descriptor::eBool:
{
- reader.readBool();
- writer.writeBool();
+ const auto readBool = reader.readBool();
+ writer.writeBool({readBool.maybe});
break;
}
case type::Descriptor::eTime:
{
- reader.readTime();
- writer.writeTime();
+ const auto readTime = reader.readTime();
+ writer.writeTime({readTime.maybe});
break;
}
default:
diff --git a/source/RobotAPI/libraries/aron/core/io/typeIO/reader/navigator/NavigatorReader.cpp b/source/RobotAPI/libraries/aron/core/io/typeIO/reader/navigator/NavigatorReader.cpp
index 0effe2a60983b988abd7c3e8ae8191f69e30ee0d..d52367bce8274a85c195ee213b5a033971781c3c 100644
--- a/source/RobotAPI/libraries/aron/core/io/typeIO/reader/navigator/NavigatorReader.cpp
+++ b/source/RobotAPI/libraries/aron/core/io/typeIO/reader/navigator/NavigatorReader.cpp
@@ -27,9 +27,7 @@
// ArmarX
#include <RobotAPI/libraries/aron/core/Exception.h>
-#include <RobotAPI/libraries/aron/core/navigator/type/container/List.h>
-#include <RobotAPI/libraries/aron/core/navigator/type/primitive/Primitive.h>
-
+#include <RobotAPI/libraries/aron/core/navigator/type/AllNavigators.h>
namespace armarx::aron::typeIO::reader
@@ -55,13 +53,13 @@ namespace armarx::aron::typeIO::reader
return lastToken->getNextElementAndIncreaseCnt();
}
- std::tuple<std::string, int> NavigatorReader::readStartObject()
+ ReaderInterface::ReadStartObjectReturnType NavigatorReader::readStartObject()
{
auto current_nav = getNextAndIncrease();
auto current_nav_casted = typenavigator::ObjectNavigator::DynamicCastAndCheck(current_nav);
auto newToken = std::make_shared<NavigatorReaderToken>(type::Descriptor::eObject, current_nav_casted);
stack.push(newToken);
- return {newToken->getElementName(), newToken->getElementChildrenSize()};
+ return {newToken->getElementName(), newToken->getElementChildrenSize(), current_nav_casted->getMaybe()};
}
bool NavigatorReader::readEndObject()
@@ -77,13 +75,13 @@ namespace armarx::aron::typeIO::reader
return false;
}
- int NavigatorReader::readStartDict()
+ ReaderInterface::ReadStartDictReturnType NavigatorReader::readStartDict()
{
auto current_nav = getNextAndIncrease();
auto current_nav_casted = typenavigator::DictNavigator::DynamicCastAndCheck(current_nav);
auto newToken = std::make_shared<NavigatorReaderToken>(type::Descriptor::eDict, current_nav_casted);
stack.push(newToken);
- return newToken->getElementChildrenSize();
+ return {newToken->getElementChildrenSize(), current_nav_casted->getMaybe()};
}
bool NavigatorReader::readEndDict()
@@ -99,13 +97,13 @@ namespace armarx::aron::typeIO::reader
return false;
}
- int NavigatorReader::readStartList()
+ ReaderInterface::ReadStartListReturnType NavigatorReader::readStartList()
{
auto current_nav = getNextAndIncrease();
auto current_nav_casted = typenavigator::ListNavigator::DynamicCastAndCheck(current_nav);
auto newToken = std::make_shared<NavigatorReaderToken>(type::Descriptor::eList, current_nav_casted);
stack.push(newToken);
- return newToken->getElementChildrenSize();
+ return {newToken->getElementChildrenSize(), current_nav_casted->getMaybe()};
}
bool NavigatorReader::readEndList()
@@ -121,13 +119,13 @@ namespace armarx::aron::typeIO::reader
return false;
}
- int NavigatorReader::readStartTuple()
+ ReaderInterface::ReadStartTupleReturnType NavigatorReader::readStartTuple()
{
auto current_nav = getNextAndIncrease();
auto current_nav_casted = typenavigator::TupleNavigator::DynamicCastAndCheck(current_nav);
auto newToken = std::make_shared<NavigatorReaderToken>(type::Descriptor::eTuple, current_nav_casted);
stack.push(newToken);
- return newToken->getElementChildrenSize();
+ return {newToken->getElementChildrenSize(), current_nav_casted->getMaybe()};
}
bool NavigatorReader::readEndTuple()
@@ -143,13 +141,13 @@ namespace armarx::aron::typeIO::reader
return false;
}
- int NavigatorReader::readStartPair()
+ ReaderInterface::ReadStartPairReturnType NavigatorReader::readStartPair()
{
auto current_nav = getNextAndIncrease();
auto current_nav_casted = typenavigator::PairNavigator::DynamicCastAndCheck(current_nav);
auto newToken = std::make_shared<NavigatorReaderToken>(type::Descriptor::ePair, current_nav_casted);
stack.push(newToken);
- return newToken->getElementChildrenSize();
+ return {newToken->getElementChildrenSize(), current_nav_casted->getMaybe()};
}
bool NavigatorReader::readEndPair()
@@ -165,100 +163,110 @@ namespace armarx::aron::typeIO::reader
return false;
}
- std::tuple<std::vector<int>, std::string> NavigatorReader::readEigenMatrix()
+ ReaderInterface::ReadEigenMatrixReturnType NavigatorReader::readEigenMatrix()
{
auto nav = getNextAndIncrease();
auto casted = typenavigator::EigenMatrixNavigator::DynamicCastAndCheck(nav);
- return std::make_tuple(casted->getDimensions(), casted->getTypename());
+ return {casted->getRows(), casted->getCols(), casted->getTypename(), casted->getMaybe()};
}
- std::string NavigatorReader::readEigenQuaternion()
+ ReaderInterface::ReadEigenQuaternionReturnType NavigatorReader::readEigenQuaternion()
{
auto nav = getNextAndIncrease();
auto casted = typenavigator::EigenMatrixNavigator::DynamicCastAndCheck(nav);
- return casted->getTypename();
+ return {casted->getTypename(), casted->getMaybe()};
}
- std::tuple<unsigned int, unsigned int, std::string> NavigatorReader::readIVTCByteImage()
+ ReaderInterface::ReadIVTCbyteImageReturnType NavigatorReader::readIVTCByteImage()
{
auto nav = getNextAndIncrease();
auto casted = typenavigator::IVTCByteImageNavigator::DynamicCastAndCheck(nav);
- return make_tuple(casted->getWidth(), casted->getHeight(), casted->getTypename());
+ return {casted->getMaybe()};
}
- std::tuple<std::vector<int>, std::string> NavigatorReader::readOpenCVMat()
+ ReaderInterface::ReadOpenCVMatReturnType NavigatorReader::readOpenCVMat()
{
auto nav = getNextAndIncrease();
auto casted = typenavigator::OpenCVMatNavigator::DynamicCastAndCheck(nav);
- return make_tuple(casted->getDimensions(), casted->getTypename());
+ return {casted->getMaybe()};
}
- std::tuple<unsigned int, unsigned int, std::string> NavigatorReader::readPCLPointCloud()
+ ReaderInterface::ReadPCLPointCloudReturnType NavigatorReader::readPCLPointCloud()
{
auto nav = getNextAndIncrease();
auto casted = typenavigator::PCLPointCloudNavigator::DynamicCastAndCheck(nav);
- return make_tuple(casted->getWidth(), casted->getHeight(), casted->getTypename());
+ return {casted->getTypename(), casted->getMaybe()};
}
- void NavigatorReader::readPosition()
+ ReaderInterface::ReadPositionReturnType NavigatorReader::readPosition()
{
auto nav = getNextAndIncrease();
- typenavigator::PositionNavigator::DynamicCastAndCheck(nav);
+ auto casted = typenavigator::PositionNavigator::DynamicCastAndCheck(nav);
+ return {casted->getMaybe()};
}
- void NavigatorReader::readOrientation()
+ ReaderInterface::ReadOrientationReturnType NavigatorReader::readOrientation()
{
auto nav = getNextAndIncrease();
- typenavigator::OrientationNavigator::DynamicCastAndCheck(nav);
+ auto casted = typenavigator::OrientationNavigator::DynamicCastAndCheck(nav);
+ return {casted->getMaybe()};
}
- void NavigatorReader::readPose()
+ ReaderInterface::ReadPoseReturnType NavigatorReader::readPose()
{
auto nav = getNextAndIncrease();
- typenavigator::PoseNavigator::DynamicCastAndCheck(nav);
+ auto casted = typenavigator::PoseNavigator::DynamicCastAndCheck(nav);
+ return {casted->getMaybe()};
}
// Read primitives
- void NavigatorReader::readInt()
+ ReaderInterface::ReadPrimitiveReturnType NavigatorReader::readInt()
{
auto nav = getNextAndIncrease();
- typenavigator::IntNavigator::DynamicCastAndCheck(nav);
+ auto casted = typenavigator::IntNavigator::DynamicCastAndCheck(nav);
+ return {casted->getMaybe()};
}
- void NavigatorReader::readFloat()
+ ReaderInterface::ReadPrimitiveReturnType NavigatorReader::readFloat()
{
auto nav = getNextAndIncrease();
- typenavigator::FloatNavigator::DynamicCastAndCheck(nav);
+ auto casted = typenavigator::FloatNavigator::DynamicCastAndCheck(nav);
+ return {casted->getMaybe()};
}
- void NavigatorReader::readLong()
+ ReaderInterface::ReadPrimitiveReturnType NavigatorReader::readLong()
{
auto nav = getNextAndIncrease();
- typenavigator::LongNavigator::DynamicCastAndCheck(nav);
+ auto casted = typenavigator::LongNavigator::DynamicCastAndCheck(nav);
+ return {casted->getMaybe()};
}
- void NavigatorReader::readDouble()
+ ReaderInterface::ReadPrimitiveReturnType NavigatorReader::readDouble()
{
auto nav = getNextAndIncrease();
- typenavigator::DoubleNavigator::DynamicCastAndCheck(nav);
+ auto casted = typenavigator::DoubleNavigator::DynamicCastAndCheck(nav);
+ return {casted->getMaybe()};
}
- void NavigatorReader::readString()
+ ReaderInterface::ReadPrimitiveReturnType NavigatorReader::readString()
{
auto nav = getNextAndIncrease();
- typenavigator::StringNavigator::DynamicCastAndCheck(nav);
+ auto casted = typenavigator::StringNavigator::DynamicCastAndCheck(nav);
+ return {casted->getMaybe()};
}
- void NavigatorReader::readBool()
+ ReaderInterface::ReadPrimitiveReturnType NavigatorReader::readBool()
{
auto nav = getNextAndIncrease();
- typenavigator::BoolNavigator::DynamicCastAndCheck(nav);
+ auto casted = typenavigator::BoolNavigator::DynamicCastAndCheck(nav);
+ return {casted->getMaybe()};
}
- void NavigatorReader::readTime()
+ ReaderInterface::ReadPrimitiveReturnType NavigatorReader::readTime()
{
auto nav = getNextAndIncrease();
- typenavigator::TimeNavigator::DynamicCastAndCheck(nav);
+ auto casted = typenavigator::TimeNavigator::DynamicCastAndCheck(nav);
+ return {casted->getMaybe()};
}
std::string NavigatorReader::readKey()
diff --git a/source/RobotAPI/libraries/aron/core/io/typeIO/reader/navigator/NavigatorReader.h b/source/RobotAPI/libraries/aron/core/io/typeIO/reader/navigator/NavigatorReader.h
index 0b733ce7cb7bafc9965a6a08c649271c35079028..d85113eb9349b3f7ef65facb324a71f76a04419b 100644
--- a/source/RobotAPI/libraries/aron/core/io/typeIO/reader/navigator/NavigatorReader.h
+++ b/source/RobotAPI/libraries/aron/core/io/typeIO/reader/navigator/NavigatorReader.h
@@ -49,34 +49,33 @@ namespace armarx::aron::typeIO::reader
NavigatorReader(const typenavigator::NavigatorPtr& n);
NavigatorReader(const type::AronTypePtr& n);
-
- virtual std::tuple<std::string, int> readStartObject() override;
+ virtual ReadStartObjectReturnType readStartObject() override;
virtual bool readEndObject() override;
- virtual int readStartList() override;
+ virtual ReadStartListReturnType readStartList() override;
virtual bool readEndList() override;
- virtual int readStartDict() override;
+ virtual ReadStartDictReturnType readStartDict() override;
virtual bool readEndDict() override;
- virtual int readStartTuple() override;
+ virtual ReadStartTupleReturnType readStartTuple() override;
virtual bool readEndTuple() override;
- virtual int readStartPair() override;
+ virtual ReadStartPairReturnType readStartPair() override;
virtual bool readEndPair() override;
- virtual std::tuple<std::vector<int>, std::string> readEigenMatrix() override;
- virtual std::string readEigenQuaternion() override;
- virtual std::tuple<unsigned int, unsigned int, std::string> readIVTCByteImage() override;
- virtual std::tuple<std::vector<int>, std::string> readOpenCVMat() override;
- virtual std::tuple<unsigned int, unsigned int, std::string> readPCLPointCloud() override;
- virtual void readPosition() override;
- virtual void readOrientation() override;
- virtual void readPose() override;
-
- virtual void readInt() override;
- virtual void readLong() override;
- virtual void readFloat() override;
- virtual void readDouble() override;
- virtual void readString() override;
- virtual void readBool() override;
- virtual void readTime() override;
+ virtual ReadEigenMatrixReturnType readEigenMatrix() override;
+ virtual ReadEigenQuaternionReturnType readEigenQuaternion() override;
+ virtual ReadIVTCbyteImageReturnType readIVTCByteImage() override;
+ virtual ReadOpenCVMatReturnType readOpenCVMat() override;
+ virtual ReadPCLPointCloudReturnType readPCLPointCloud() override;
+ virtual ReadPositionReturnType readPosition() override;
+ virtual ReadOrientationReturnType readOrientation() override;
+ virtual ReadPoseReturnType readPose() override;
+
+ virtual ReadPrimitiveReturnType readInt() override;
+ virtual ReadPrimitiveReturnType readLong() override;
+ virtual ReadPrimitiveReturnType readFloat() override;
+ virtual ReadPrimitiveReturnType readDouble() override;
+ virtual ReadPrimitiveReturnType readString() override;
+ virtual ReadPrimitiveReturnType readBool() override;
+ virtual ReadPrimitiveReturnType readTime() override;
virtual std::string readKey() override;
virtual void loadMember(const std::string&) override;
diff --git a/source/RobotAPI/libraries/aron/core/io/typeIO/reader/nlohmannJSON/NlohmannJSONReader.cpp b/source/RobotAPI/libraries/aron/core/io/typeIO/reader/nlohmannJSON/NlohmannJSONReader.cpp
index 98d6953ade3f36ca2408dc379a3b325e8fd007de..dbb7ef1ceba56fbbc7e8a994b7414a3296eacb24 100644
--- a/source/RobotAPI/libraries/aron/core/io/typeIO/reader/nlohmannJSON/NlohmannJSONReader.cpp
+++ b/source/RobotAPI/libraries/aron/core/io/typeIO/reader/nlohmannJSON/NlohmannJSONReader.cpp
@@ -27,10 +27,7 @@
// ArmarX
#include <RobotAPI/libraries/aron/core/Exception.h>
-#include <RobotAPI/libraries/aron/core/navigator/type/container/List.h>
-#include <RobotAPI/libraries/aron/core/navigator/type/primitive/Primitive.h>
-
-
+#include <RobotAPI/libraries/aron/core/navigator/type/AllNavigators.h>
namespace armarx::aron::typeIO::reader
{
@@ -55,12 +52,13 @@ namespace armarx::aron::typeIO::reader
return lastToken->getNextElementAndIncreaseCnt();
}
- std::tuple<std::string, int> NlohmannJSONReader::readStartObject()
+ ReaderInterface::ReadStartObjectReturnType NlohmannJSONReader::readStartObject()
{
nlohmann::json current_json = getNextAndIncrease();
auto newToken = std::make_shared<NlohmannJSONReaderToken>(type::Descriptor::eObject, current_json);
stack.push(newToken);
- return {newToken->getElementName(), newToken->getElementChildrenSize()};
+ type::Maybe o = current_json[io::Data::READER_WRITER_MAYBE_SLUG];
+ return {newToken->getElementName(), newToken->getElementChildrenSize(), o};
}
bool NlohmannJSONReader::readEndObject()
@@ -76,12 +74,13 @@ namespace armarx::aron::typeIO::reader
return false;
}
- int NlohmannJSONReader::readStartDict()
+ ReaderInterface::ReadStartDictReturnType NlohmannJSONReader::readStartDict()
{
nlohmann::json current_json = getNextAndIncrease();
auto newToken = std::make_shared<NlohmannJSONReaderToken>(type::Descriptor::eDict, current_json);
stack.push(newToken);
- return newToken->getElementChildrenSize();
+ type::Maybe o = current_json[io::Data::READER_WRITER_MAYBE_SLUG];
+ return {newToken->getElementChildrenSize(), o};
}
bool NlohmannJSONReader::readEndDict()
@@ -97,12 +96,13 @@ namespace armarx::aron::typeIO::reader
return false;
}
- int NlohmannJSONReader::readStartList()
+ ReaderInterface::ReadStartListReturnType NlohmannJSONReader::readStartList()
{
nlohmann::json current_json = getNextAndIncrease();
auto newToken = std::make_shared<NlohmannJSONReaderToken>(type::Descriptor::eList, current_json);
stack.push(newToken);
- return newToken->getElementChildrenSize();
+ type::Maybe o = current_json[io::Data::READER_WRITER_MAYBE_SLUG];
+ return {newToken->getElementChildrenSize(), o};
}
bool NlohmannJSONReader::readEndList()
@@ -118,12 +118,13 @@ namespace armarx::aron::typeIO::reader
return false;
}
- int NlohmannJSONReader::readStartTuple()
+ ReaderInterface::ReadStartTupleReturnType NlohmannJSONReader::readStartTuple()
{
nlohmann::json current_json = getNextAndIncrease();
auto newToken = std::make_shared<NlohmannJSONReaderToken>(type::Descriptor::eTuple, current_json);
stack.push(newToken);
- return newToken->getElementChildrenSize();
+ type::Maybe o = current_json[io::Data::READER_WRITER_MAYBE_SLUG];
+ return {newToken->getElementChildrenSize(), o};
}
bool NlohmannJSONReader::readEndTuple()
@@ -139,12 +140,13 @@ namespace armarx::aron::typeIO::reader
return false;
}
- int NlohmannJSONReader::readStartPair()
+ ReaderInterface::ReadStartPairReturnType NlohmannJSONReader::readStartPair()
{
nlohmann::json current_json = getNextAndIncrease();
auto newToken = std::make_shared<NlohmannJSONReaderToken>(type::Descriptor::ePair, current_json);
stack.push(newToken);
- return newToken->getElementChildrenSize();
+ type::Maybe o = current_json[io::Data::READER_WRITER_MAYBE_SLUG];
+ return {newToken->getElementChildrenSize(), o};
}
bool NlohmannJSONReader::readEndPair()
@@ -160,97 +162,114 @@ namespace armarx::aron::typeIO::reader
return false;
}
- std::tuple<std::vector<int>, std::string> NlohmannJSONReader::readEigenMatrix()
+ ReaderInterface::ReadEigenMatrixReturnType NlohmannJSONReader::readEigenMatrix()
{
nlohmann::json j = getNextAndIncrease();
- std::vector<int> dims = j[io::Data::READER_WRITER_NDARRAY_DIMENSIONS_SLUG];
+ std::vector<unsigned int> dims = j[io::Data::READER_WRITER_NDARRAY_DIMENSIONS_SLUG];
std::string type = j[io::Data::READER_WRITER_NDARRAY_TYPE_SLUG];
- return std::make_tuple(dims, type);
+ type::Maybe o = j[io::Data::READER_WRITER_MAYBE_SLUG];
+ return {dims[0], dims[1], type, o};
}
- std::string NlohmannJSONReader::readEigenQuaternion()
+ ReaderInterface::ReadEigenQuaternionReturnType NlohmannJSONReader::readEigenQuaternion()
{
nlohmann::json j = getNextAndIncrease();
std::string type = j[io::Data::READER_WRITER_NDARRAY_TYPE_SLUG];
- return type;
+ type::Maybe o = j[io::Data::READER_WRITER_MAYBE_SLUG];
+ return {type, o};
}
- std::tuple<unsigned int, unsigned int, std::string> NlohmannJSONReader::readIVTCByteImage()
+ ReaderInterface::ReadIVTCbyteImageReturnType NlohmannJSONReader::readIVTCByteImage()
{
nlohmann::json j = getNextAndIncrease();
- std::vector<int> dims = j[io::Data::READER_WRITER_NDARRAY_DIMENSIONS_SLUG];
- std::string type = j[io::Data::READER_WRITER_NDARRAY_TYPE_SLUG];
- return std::make_tuple(dims[0], dims[1], type);
+ type::Maybe o = j[io::Data::READER_WRITER_MAYBE_SLUG];
+ return {o};
}
- std::tuple<std::vector<int>, std::string> NlohmannJSONReader::readOpenCVMat()
+ ReaderInterface::ReadOpenCVMatReturnType NlohmannJSONReader::readOpenCVMat()
{
nlohmann::json j = getNextAndIncrease();
- std::vector<int> dims = j[io::Data::READER_WRITER_NDARRAY_DIMENSIONS_SLUG];
- std::string type = j[io::Data::READER_WRITER_NDARRAY_TYPE_SLUG];
- return std::make_tuple(dims, type);
+ type::Maybe o = j[io::Data::READER_WRITER_MAYBE_SLUG];
+ return {o};
}
- std::tuple<unsigned int, unsigned int, std::string> NlohmannJSONReader::readPCLPointCloud()
+ ReaderInterface::ReadPCLPointCloudReturnType NlohmannJSONReader::readPCLPointCloud()
{
nlohmann::json j = getNextAndIncrease();
- std::vector<int> dims = j[io::Data::READER_WRITER_NDARRAY_DIMENSIONS_SLUG];
std::string type = j[io::Data::READER_WRITER_NDARRAY_TYPE_SLUG];
- return std::make_tuple(dims[0], dims[1], type);
+ type::Maybe o = j[io::Data::READER_WRITER_MAYBE_SLUG];
+ return {type, o};
}
- void NlohmannJSONReader::readPosition()
+ ReaderInterface::ReadPositionReturnType NlohmannJSONReader::readPosition()
{
- getNextAndIncrease();
- return;
+ nlohmann::json j = getNextAndIncrease();
+ type::Maybe o = j[io::Data::READER_WRITER_MAYBE_SLUG];
+ return {o};
}
- void NlohmannJSONReader::readOrientation()
+ ReaderInterface::ReadOrientationReturnType NlohmannJSONReader::readOrientation()
{
- getNextAndIncrease();
- return;
+ nlohmann::json j = getNextAndIncrease();
+ type::Maybe o = j[io::Data::READER_WRITER_MAYBE_SLUG];
+ return {o};
}
- void NlohmannJSONReader::readPose()
+ ReaderInterface::ReadPoseReturnType NlohmannJSONReader::readPose()
{
- getNextAndIncrease();
- return;
+ nlohmann::json j = getNextAndIncrease();
+ type::Maybe o = j[io::Data::READER_WRITER_MAYBE_SLUG];
+ return {o};
}
// Read primitives
- void NlohmannJSONReader::readInt()
+ ReaderInterface::ReadPrimitiveReturnType NlohmannJSONReader::readInt()
{
nlohmann::json j = getNextAndIncrease();
+ type::Maybe o = j[io::Data::READER_WRITER_MAYBE_SLUG];
+ return {o};
}
- void NlohmannJSONReader::readLong()
+ ReaderInterface::ReadPrimitiveReturnType NlohmannJSONReader::readLong()
{
nlohmann::json j = getNextAndIncrease();
+ type::Maybe o = j[io::Data::READER_WRITER_MAYBE_SLUG];
+ return {o};
}
- void NlohmannJSONReader::readFloat()
+ ReaderInterface::ReadPrimitiveReturnType NlohmannJSONReader::readFloat()
{
nlohmann::json j = getNextAndIncrease();
+ type::Maybe o = j[io::Data::READER_WRITER_MAYBE_SLUG];
+ return {o};
}
- void NlohmannJSONReader::readDouble()
+ ReaderInterface::ReadPrimitiveReturnType NlohmannJSONReader::readDouble()
{
nlohmann::json j = getNextAndIncrease();
+ type::Maybe o = j[io::Data::READER_WRITER_MAYBE_SLUG];
+ return {o};
}
- void NlohmannJSONReader::readString()
+ ReaderInterface::ReadPrimitiveReturnType NlohmannJSONReader::readString()
{
nlohmann::json j = getNextAndIncrease();
+ type::Maybe o = j[io::Data::READER_WRITER_MAYBE_SLUG];
+ return {o};
}
- void NlohmannJSONReader::readBool()
+ ReaderInterface::ReadPrimitiveReturnType NlohmannJSONReader::readBool()
{
nlohmann::json j = getNextAndIncrease();
+ type::Maybe o = j[io::Data::READER_WRITER_MAYBE_SLUG];
+ return {o};
}
- void NlohmannJSONReader::readTime()
+ ReaderInterface::ReadPrimitiveReturnType NlohmannJSONReader::readTime()
{
nlohmann::json j = getNextAndIncrease();
+ type::Maybe o = j[io::Data::READER_WRITER_MAYBE_SLUG];
+ return {o};
}
std::string NlohmannJSONReader::readKey()
diff --git a/source/RobotAPI/libraries/aron/core/io/typeIO/reader/nlohmannJSON/NlohmannJSONReader.h b/source/RobotAPI/libraries/aron/core/io/typeIO/reader/nlohmannJSON/NlohmannJSONReader.h
index 70efcc50ec54620533272f9493aa77c17701fa41..78caf17db08697fd0e2f47113dd7142df9a6de8b 100644
--- a/source/RobotAPI/libraries/aron/core/io/typeIO/reader/nlohmannJSON/NlohmannJSONReader.h
+++ b/source/RobotAPI/libraries/aron/core/io/typeIO/reader/nlohmannJSON/NlohmannJSONReader.h
@@ -47,33 +47,33 @@ namespace armarx::aron::typeIO::reader
NlohmannJSONReader(const nlohmann::json& n);
NlohmannJSONReader(const std::string& n);
- virtual std::tuple<std::string, int> readStartObject() override;
+ virtual ReadStartObjectReturnType readStartObject() override;
virtual bool readEndObject() override;
- virtual int readStartList() override;
+ virtual ReadStartListReturnType readStartList() override;
virtual bool readEndList() override;
- virtual int readStartDict() override;
+ virtual ReadStartDictReturnType readStartDict() override;
virtual bool readEndDict() override;
- virtual int readStartTuple() override;
+ virtual ReadStartTupleReturnType readStartTuple() override;
virtual bool readEndTuple() override;
- virtual int readStartPair() override;
+ virtual ReadStartPairReturnType readStartPair() override;
virtual bool readEndPair() override;
- virtual std::tuple<std::vector<int>, std::string> readEigenMatrix() override;
- virtual std::string readEigenQuaternion() override;
- virtual std::tuple<unsigned int, unsigned int, std::string> readIVTCByteImage() override;
- virtual std::tuple<std::vector<int>, std::string> readOpenCVMat() override;
- virtual std::tuple<unsigned int, unsigned int, std::string> readPCLPointCloud() override;
- virtual void readPosition() override;
- virtual void readOrientation() override;
- virtual void readPose() override;
+ virtual ReadEigenMatrixReturnType readEigenMatrix() override;
+ virtual ReadEigenQuaternionReturnType readEigenQuaternion() override;
+ virtual ReadIVTCbyteImageReturnType readIVTCByteImage() override;
+ virtual ReadOpenCVMatReturnType readOpenCVMat() override;
+ virtual ReadPCLPointCloudReturnType readPCLPointCloud() override;
+ virtual ReadPositionReturnType readPosition() override;
+ virtual ReadOrientationReturnType readOrientation() override;
+ virtual ReadPoseReturnType readPose() override;
- virtual void readInt() override;
- virtual void readLong() override;
- virtual void readFloat() override;
- virtual void readDouble() override;
- virtual void readString() override;
- virtual void readBool() override;
- virtual void readTime() override;
+ virtual ReadPrimitiveReturnType readInt() override;
+ virtual ReadPrimitiveReturnType readLong() override;
+ virtual ReadPrimitiveReturnType readFloat() override;
+ virtual ReadPrimitiveReturnType readDouble() override;
+ virtual ReadPrimitiveReturnType readString() override;
+ virtual ReadPrimitiveReturnType readBool() override;
+ virtual ReadPrimitiveReturnType readTime() override;
virtual std::string readKey() override;
virtual void loadMember(const std::string&) override;
diff --git a/source/RobotAPI/libraries/aron/core/io/typeIO/reader/nlohmannJSON/NlohmannJSONReaderToken.h b/source/RobotAPI/libraries/aron/core/io/typeIO/reader/nlohmannJSON/NlohmannJSONReaderToken.h
index 3d3503af2567199be5ebf627f45813f2b220c606..d95638c1a2c3d3b1f4c6ebf835a454f8831ea924 100644
--- a/source/RobotAPI/libraries/aron/core/io/typeIO/reader/nlohmannJSON/NlohmannJSONReaderToken.h
+++ b/source/RobotAPI/libraries/aron/core/io/typeIO/reader/nlohmannJSON/NlohmannJSONReaderToken.h
@@ -59,7 +59,7 @@ namespace armarx::aron::typeIO::reader
{
for (auto& [key, val] : type.get<nlohmann::json::object_t>())
{
- if (key == io::Data::READER_WRITER_NAME_SLUG)
+ if (key == io::Data::READER_WRITER_OBJECT_NAME_SLUG)
{
continue;
}
@@ -67,7 +67,7 @@ namespace armarx::aron::typeIO::reader
allMemberNames.push_back(key);
}
childrenSize = allMemberNames.size();
- elementName = element[io::Data::READER_WRITER_NAME_SLUG];
+ elementName = element[io::Data::READER_WRITER_OBJECT_NAME_SLUG];
break;
}
case type::Descriptor::eDict:
diff --git a/source/RobotAPI/libraries/aron/core/io/typeIO/visitor/Visitor.cpp b/source/RobotAPI/libraries/aron/core/io/typeIO/visitor/Visitor.cpp
index 1b0bb02c4ea191472b8a5502ade389fefcc30028..0e87ee0e42888a5fac73884aa82ef89fe6216cd4 100644
--- a/source/RobotAPI/libraries/aron/core/io/typeIO/visitor/Visitor.cpp
+++ b/source/RobotAPI/libraries/aron/core/io/typeIO/visitor/Visitor.cpp
@@ -22,6 +22,9 @@
#include <memory>
#include <numeric>
+// Simox
+#include <SimoxUtility/algorithm/vector.hpp>
+
// Header
#include "Visitor.h"
@@ -35,7 +38,7 @@ namespace armarx::aron::typeIO
void Visitor::VisitAndSetup(WriterInterface& writer, const typenavigator::NavigatorPtr& aron)
{
- VisitAndSetup(writer, aron->getResult());
+ VisitAndSetup(writer, aron->toAronPtr());
}
void Visitor::VisitAndSetup(WriterInterface& writer, const type::AronTypePtr& aron)
@@ -45,8 +48,8 @@ namespace armarx::aron::typeIO
{
case type::Descriptor::eObject:
{
- type::AronObjectPtr casted = type::AronObjectPtr::dynamicCast(aron);
- writer.writeStartObject(casted->objectName);
+ auto casted = type::AronObjectPtr::dynamicCast(aron);
+ writer.writeStartObject({casted->objectName, casted->maybe});
for (const auto& [key, value] : casted->elementTypes)
{
writer.writeKey(key);
@@ -57,16 +60,16 @@ namespace armarx::aron::typeIO
}
case type::Descriptor::eDict:
{
- type::AronDictPtr casted = type::AronDictPtr::dynamicCast(aron);
- writer.writeStartDict();
+ auto casted = type::AronDictPtr::dynamicCast(aron);
+ writer.writeStartDict({casted->maybe});
Visitor::VisitAndSetup(writer, casted->acceptedType);
writer.writeEndDict();
break;
}
case type::Descriptor::eTuple:
{
- type::AronTuplePtr casted = type::AronTuplePtr::dynamicCast(aron);
- writer.writeStartTuple();
+ auto casted = type::AronTuplePtr::dynamicCast(aron);
+ writer.writeStartTuple({casted->maybe});
for (const auto& value : casted->elementTypes)
{
Visitor::VisitAndSetup(writer, value);
@@ -76,102 +79,105 @@ namespace armarx::aron::typeIO
}
case type::Descriptor::eList:
{
- type::AronListPtr casted = type::AronListPtr::dynamicCast(aron);
- writer.writeStartList();
+ auto casted = type::AronListPtr::dynamicCast(aron);
+ writer.writeStartList({casted->maybe});
Visitor::VisitAndSetup(writer, casted->acceptedType);
writer.writeEndList();
break;
}
case type::Descriptor::eEigenMatrix:
{
- type::AronEigenMatrixPtr casted = type::AronEigenMatrixPtr::dynamicCast(aron);
- writer.writeEigenMatrix(casted->dimensions, casted->typeName);
+ auto casted = type::AronEigenMatrixPtr::dynamicCast(aron);
+ writer.writeEigenMatrix({(unsigned int) casted->rows, (unsigned int) casted->cols, casted->typeName, casted->maybe});
break;
}
case type::Descriptor::eEigenQuaternion:
{
- type::AronEigenQuaternionPtr casted = type::AronEigenQuaternionPtr::dynamicCast(aron);
- writer.writeEigenQuaternion(casted->typeName);
+ auto casted = type::AronEigenQuaternionPtr::dynamicCast(aron);
+ writer.writeEigenQuaternion({casted->typeName, casted->maybe});
break;
}
case type::Descriptor::eIVTCByteImage:
{
- type::AronIVTCByteImagePtr casted = type::AronIVTCByteImagePtr::dynamicCast(aron);
- writer.writeIVTCByteImage(casted->width, casted->height, casted->typeName);
+ auto casted = type::AronIVTCByteImagePtr::dynamicCast(aron);
+ writer.writeIVTCByteImage({casted->maybe});
break;
}
case type::Descriptor::eOpenCVMat:
{
- type::AronOpenCVMatPtr casted = type::AronOpenCVMatPtr::dynamicCast(aron);
- writer.writeOpenCVMat(casted->dimensions, casted->typeName);
+ auto casted = type::AronOpenCVMatPtr::dynamicCast(aron);
+ writer.writeOpenCVMat({casted->maybe});
break;
}
case type::Descriptor::ePCLPointCloud:
{
- type::AronPCLPointCloudPtr casted = type::AronPCLPointCloudPtr::dynamicCast(aron);
- writer.writePCLPointCloud(casted->width, casted->height, casted->typeName);
+ auto casted = type::AronPCLPointCloudPtr::dynamicCast(aron);
+ writer.writePCLPointCloud({casted->typeName, casted->maybe});
break;
}
case type::Descriptor::ePosition:
{
- writer.writePosition();
+ auto casted = type::AronPositionPtr::dynamicCast(aron);
+ writer.writePosition({casted->maybe});
break;
}
case type::Descriptor::eOrientation:
{
- writer.writeOrientation();
+ auto casted = type::AronOrientationPtr::dynamicCast(aron);
+ writer.writeOrientation({casted->maybe});
break;
}
case type::Descriptor::ePose:
{
- writer.writePose();
+ auto casted = type::AronPosePtr::dynamicCast(aron);
+ writer.writePose({casted->maybe});
break;
}
case type::Descriptor::eInt:
{
- type::AronIntPtr casted = type::AronIntPtr::dynamicCast(aron);
- writer.writeInt();
+ auto casted = type::AronIntPtr::dynamicCast(aron);
+ writer.writeInt({casted->maybe});
break;
}
case type::Descriptor::eLong:
{
- type::AronLongPtr casted = type::AronLongPtr::dynamicCast(aron);
- writer.writeLong();
+ auto casted = type::AronLongPtr::dynamicCast(aron);
+ writer.writeLong({casted->maybe});
break;
}
case type::Descriptor::eFloat:
{
- type::AronFloatPtr casted = type::AronFloatPtr::dynamicCast(aron);
- writer.writeFloat();
+ auto casted = type::AronFloatPtr::dynamicCast(aron);
+ writer.writeFloat({casted->maybe});
break;
}
case type::Descriptor::eDouble:
{
- type::AronDoublePtr casted = type::AronDoublePtr::dynamicCast(aron);
- writer.writeDouble();
+ auto casted = type::AronDoublePtr::dynamicCast(aron);
+ writer.writeDouble({casted->maybe});
break;
}
case type::Descriptor::eString:
{
- type::AronStringPtr casted = type::AronStringPtr::dynamicCast(aron);
- writer.writeString();
+ auto casted = type::AronStringPtr::dynamicCast(aron);
+ writer.writeString({casted->maybe});
break;
}
case type::Descriptor::eBool:
{
- type::AronBoolPtr casted = type::AronBoolPtr::dynamicCast(aron);
- writer.writeBool();
+ auto casted = type::AronBoolPtr::dynamicCast(aron);
+ writer.writeBool({casted->maybe});
break;
}
case type::Descriptor::eTime:
{
- type::AronTimePtr casted = type::AronTimePtr::dynamicCast(aron);
- writer.writeTime();
+ auto casted = type::AronTimePtr::dynamicCast(aron);
+ writer.writeTime({casted->maybe});
break;
}
default:
{
- throw error::DescriptorNotValidException("Visitor", "SetupWriterFromAronTypePtr", "Type-Type could not be resolved. The ice_id of the input was: " + aron->ice_id(), desc);
+ throw error::DescriptorNotValidException("Visitor", "VisitAndSetup", "Type could not be resolved. The ice_id of the input was: " + aron->ice_id(), desc);
}
}
}
diff --git a/source/RobotAPI/libraries/aron/core/io/typeIO/writer/navigator/NavigatorWriter.cpp b/source/RobotAPI/libraries/aron/core/io/typeIO/writer/navigator/NavigatorWriter.cpp
index 06e8c00764ed0c3c446c7fdb2b1448a780d085cb..9693eb6851131add3d419b8ecb55c15cc43518db 100644
--- a/source/RobotAPI/libraries/aron/core/io/typeIO/writer/navigator/NavigatorWriter.cpp
+++ b/source/RobotAPI/libraries/aron/core/io/typeIO/writer/navigator/NavigatorWriter.cpp
@@ -26,13 +26,8 @@
#include "NavigatorWriter.h"
// ArmarX
-#include <RobotAPI/libraries/aron/core/navigator/data/container/List.h>
-#include <RobotAPI/libraries/aron/core/navigator/data/primitive/Primitive.h>
-
-#include <RobotAPI/libraries/aron/core/navigator/type/container/List.h>
-#include <RobotAPI/libraries/aron/core/navigator/type/container/Object.h>
-#include <RobotAPI/libraries/aron/core/navigator/type/container/Tuple.h>
-#include <RobotAPI/libraries/aron/core/navigator/type/primitive/Primitive.h>
+#include <RobotAPI/libraries/aron/core/navigator/data/AllNavigators.h>
+#include <RobotAPI/libraries/aron/core/navigator/type/AllNavigators.h>
namespace armarx::aron::typeIO::writer
{
@@ -53,11 +48,12 @@ namespace armarx::aron::typeIO::writer
}
// interface
- void NavigatorWriter::writeStartObject(const std::string& n)
+ void NavigatorWriter::writeStartObject(const WriteStartObjectInput& o)
{
Path path = generatePath();
auto type = std::make_shared<typenavigator::ObjectNavigator>(path);
- type->setObjectName(n);
+ type->setObjectName(o.name);
+ type->setMaybe(o.maybe);
auto new_token = std::make_shared<NavigatorWriterToken>(type->getDescriptor(), type);
stack.push(new_token);
}
@@ -74,10 +70,11 @@ namespace armarx::aron::typeIO::writer
}
}
- void NavigatorWriter::writeStartDict()
+ void NavigatorWriter::writeStartDict(const WriteStartDictInput& o)
{
Path path = generatePath();
auto type = std::make_shared<typenavigator::DictNavigator>(path);
+ type->setMaybe(o.maybe);
auto new_token = std::make_shared<NavigatorWriterToken>(type->getDescriptor(), type);
stack.push(new_token);
}
@@ -94,10 +91,11 @@ namespace armarx::aron::typeIO::writer
}
}
- void NavigatorWriter::writeStartList()
+ void NavigatorWriter::writeStartList(const WriteStartListInput& o)
{
Path path = generatePath();
auto type = std::make_shared<typenavigator::ListNavigator>(path);
+ type->setMaybe(o.maybe);
auto new_token = std::make_shared<NavigatorWriterToken>(type->getDescriptor(), type);
stack.push(new_token);
}
@@ -114,10 +112,11 @@ namespace armarx::aron::typeIO::writer
}
}
- void NavigatorWriter::writeStartTuple()
+ void NavigatorWriter::writeStartTuple(const WriteStartTupleInput& o)
{
Path path = generatePath();
auto type = std::make_shared<typenavigator::TupleNavigator>(path);
+ type->setMaybe(o.maybe);
auto new_token = std::make_shared<NavigatorWriterToken>(type->getDescriptor(), type);
stack.push(new_token);
}
@@ -134,10 +133,11 @@ namespace armarx::aron::typeIO::writer
}
}
- void NavigatorWriter::writeStartPair()
+ void NavigatorWriter::writeStartPair(const WriteStartPairInput& o)
{
Path path = generatePath();
auto type = std::make_shared<typenavigator::PairNavigator>(path);
+ type->setMaybe(o.maybe);
auto new_token = std::make_shared<NavigatorWriterToken>(type->getDescriptor(), type);
stack.push(new_token);
}
@@ -154,135 +154,144 @@ namespace armarx::aron::typeIO::writer
}
}
- void NavigatorWriter::writeEigenMatrix(const std::vector<int>& dims, const std::string& t)
+ void NavigatorWriter::writeEigenMatrix(const WriteEigenMatrixInput& o)
{
Path path = generatePath();
NavigatorWriterTokenPtr token = stack.top();
- typenavigator::EigenMatrixNavigatorPtr aron(new typenavigator::EigenMatrixNavigator(path));
- aron->setTypename(t);
- aron->setDimensions(dims);
- token->addElement(aron);
+ auto type = std::make_shared<typenavigator::EigenMatrixNavigator>(path);
+ type->setRows(o.rows);
+ type->setCols(o.cols);
+ type->setTypename(o.type);
+ type->setMaybe(o.maybe);
+ token->addElement(type);
}
- void NavigatorWriter::writeEigenQuaternion(const std::string& t)
+ void NavigatorWriter::writeEigenQuaternion(const WriteEigenQuaternionInput& o)
{
Path path = generatePath();
NavigatorWriterTokenPtr token = stack.top();
- typenavigator::EigenQuaternionNavigatorPtr aron(new typenavigator::EigenQuaternionNavigator(path));
- aron->setTypename(t);
- token->addElement(aron);
+ auto type = std::make_shared<typenavigator::EigenQuaternionNavigator>(path);
+ type->setTypename(o.type);
+ type->setMaybe(o.maybe);
+ token->addElement(type);
}
- void NavigatorWriter::writeIVTCByteImage(unsigned int w, unsigned int h, const std::string& t)
+ void NavigatorWriter::writeIVTCByteImage(const WriteIVTCByteImageInput& o)
{
Path path = generatePath();
NavigatorWriterTokenPtr token = stack.top();
- typenavigator::IVTCByteImageNavigatorPtr aron(new typenavigator::IVTCByteImageNavigator(path));
- aron->setTypename(t);
- aron->setWidth(w);
- aron->setHeight(h);
- token->addElement(aron);
+ auto type = std::make_shared<typenavigator::IVTCByteImageNavigator>(path);
+ type->setMaybe(o.maybe);
+ token->addElement(type);
}
- void NavigatorWriter::writeOpenCVMat(const std::vector<int>& dims, const std::string& t)
+ void NavigatorWriter::writeOpenCVMat(const WriteOpenCVMatInput& o)
{
Path path = generatePath();
NavigatorWriterTokenPtr token = stack.top();
- typenavigator::OpenCVMatNavigatorPtr aron(new typenavigator::OpenCVMatNavigator(path));
- aron->setTypename(t);
- aron->setDimensions(dims);
- token->addElement(aron);
+ auto type = std::make_shared<typenavigator::OpenCVMatNavigator>(path);
+ type->setMaybe(o.maybe);
+ token->addElement(type);
}
- void NavigatorWriter::writePCLPointCloud(unsigned int w, unsigned int h, const std::string& t)
+ void NavigatorWriter::writePCLPointCloud(const WritePCLPointCloudInput& o)
{
Path path = generatePath();
NavigatorWriterTokenPtr token = stack.top();
- typenavigator::PCLPointCloudNavigatorPtr aron(new typenavigator::PCLPointCloudNavigator(path));
- aron->setTypename(t);
- aron->setWidth(w);
- aron->setHeight(h);
- token->addElement(aron);
+ auto type = std::make_shared<typenavigator::PCLPointCloudNavigator>(path);
+ type->setTypename(o.type);
+ type->setMaybe(o.maybe);
+ token->addElement(type);
}
- void NavigatorWriter::writePosition()
+ void NavigatorWriter::writePosition(const WritePositionInput& o)
{
Path path = generatePath();
NavigatorWriterTokenPtr token = stack.top();
- typenavigator::PositionNavigatorPtr aron(new typenavigator::PositionNavigator(path));
- token->addElement(aron);
+ auto type = std::make_shared<typenavigator::PositionNavigator>(path);
+ type->setMaybe(o.maybe);
+ token->addElement(type);
}
- void NavigatorWriter::writeOrientation()
+ void NavigatorWriter::writeOrientation(const WriteOrientationInput& o)
{
Path path = generatePath();
NavigatorWriterTokenPtr token = stack.top();
- typenavigator::OrientationNavigatorPtr aron(new typenavigator::OrientationNavigator(path));
- token->addElement(aron);
+ auto type = std::make_shared<typenavigator::OrientationNavigator>(path);
+ type->setMaybe(o.maybe);
+ token->addElement(type);
}
- void NavigatorWriter::writePose()
+ void NavigatorWriter::writePose(const WritePoseInput& o)
{
Path path = generatePath();
NavigatorWriterTokenPtr token = stack.top();
- typenavigator::PoseNavigatorPtr aron(new typenavigator::PoseNavigator(path));
- token->addElement(aron);
+ auto type = std::make_shared<typenavigator::PoseNavigator>(path);
+ type->setMaybe(o.maybe);
+ token->addElement(type);
}
- void NavigatorWriter::writeInt()
+ void NavigatorWriter::writeInt(const WritePrimitiveInput& o)
{
Path path = generatePath();
NavigatorWriterTokenPtr token = stack.top();
- typenavigator::IntNavigatorPtr aron(new typenavigator::IntNavigator(path));
- token->addElement(aron);
+ auto type = std::make_shared<typenavigator::IntNavigator>(path);
+ type->setMaybe(o.maybe);
+ token->addElement(type);
}
- void NavigatorWriter::writeLong()
+ void NavigatorWriter::writeLong(const WritePrimitiveInput& o)
{
Path path = generatePath();
NavigatorWriterTokenPtr token = stack.top();
- typenavigator::LongNavigatorPtr aron(new typenavigator::LongNavigator(path));
- token->addElement(aron);
+ auto type = std::make_shared<typenavigator::LongNavigator>(path);
+ type->setMaybe(o.maybe);
+ token->addElement(type);
}
- void NavigatorWriter::writeFloat()
+ void NavigatorWriter::writeFloat(const WritePrimitiveInput& o)
{
Path path = generatePath();
NavigatorWriterTokenPtr token = stack.top();
- typenavigator::FloatNavigatorPtr aron(new typenavigator::FloatNavigator(path));
- token->addElement(aron);
+ auto type = std::make_shared<typenavigator::FloatNavigator>(path);
+ type->setMaybe(o.maybe);
+ token->addElement(type);
}
- void NavigatorWriter::writeDouble()
+ void NavigatorWriter::writeDouble(const WritePrimitiveInput& o)
{
Path path = generatePath();
NavigatorWriterTokenPtr token = stack.top();
- typenavigator::DoubleNavigatorPtr aron(new typenavigator::DoubleNavigator(path));
- token->addElement(aron);
+ auto type = std::make_shared<typenavigator::DoubleNavigator>(path);
+ type->setMaybe(o.maybe);
+ token->addElement(type);
}
- void NavigatorWriter::writeString()
+ void NavigatorWriter::writeString(const WritePrimitiveInput& o)
{
Path path = generatePath();
NavigatorWriterTokenPtr token = stack.top();
- typenavigator::StringNavigatorPtr aron(new typenavigator::StringNavigator(path));
- token->addElement(aron);
+ auto type = std::make_shared<typenavigator::StringNavigator>(path);
+ type->setMaybe(o.maybe);
+ token->addElement(type);
}
- void NavigatorWriter::writeBool()
+ void NavigatorWriter::writeBool(const WritePrimitiveInput& o)
{
Path path = generatePath();
NavigatorWriterTokenPtr token = stack.top();
- typenavigator::BoolNavigatorPtr aron(new typenavigator::BoolNavigator(path));
- token->addElement(aron);
+ auto type = std::make_shared<typenavigator::BoolNavigator>(path);
+ type->setMaybe(o.maybe);
+ token->addElement(type);
}
- void NavigatorWriter::writeTime()
+ void NavigatorWriter::writeTime(const WritePrimitiveInput& o)
{
Path path = generatePath();
NavigatorWriterTokenPtr token = stack.top();
- typenavigator::TimeNavigatorPtr aron(new typenavigator::TimeNavigator(path));
- token->addElement(aron);
+ auto type = std::make_shared<typenavigator::TimeNavigator>(path);
+ type->setMaybe(o.maybe);
+ token->addElement(type);
}
void NavigatorWriter::writeKey(const std::string& k)
diff --git a/source/RobotAPI/libraries/aron/core/io/typeIO/writer/navigator/NavigatorWriter.h b/source/RobotAPI/libraries/aron/core/io/typeIO/writer/navigator/NavigatorWriter.h
index 00b7ed96e3789deb7f72a6f370b2a8fbb9371576..b15ab9e9b35fb140869017c2399f4e7345eda226 100644
--- a/source/RobotAPI/libraries/aron/core/io/typeIO/writer/navigator/NavigatorWriter.h
+++ b/source/RobotAPI/libraries/aron/core/io/typeIO/writer/navigator/NavigatorWriter.h
@@ -43,33 +43,33 @@ namespace armarx::aron::typeIO::writer
public:
NavigatorWriter() = default;
- virtual void writeStartObject(const std::string&) override;
+ virtual void writeStartObject(const WriteStartObjectInput&) override;
virtual void writeEndObject() override;
- virtual void writeStartList() override;
+ virtual void writeStartList(const WriteStartListInput&) override;
virtual void writeEndList() override;
- virtual void writeStartDict() override;
+ virtual void writeStartDict(const WriteStartDictInput&) override;
virtual void writeEndDict() override;
- virtual void writeStartTuple() override;
+ virtual void writeStartTuple(const WriteStartTupleInput&) override;
virtual void writeEndTuple() override;
- virtual void writeStartPair() override;
+ virtual void writeStartPair(const WriteStartPairInput&) override;
virtual void writeEndPair() override;
- virtual void writeEigenMatrix(const std::vector<int>&, const std::string&) override;
- virtual void writeEigenQuaternion(const std::string&) override;
- virtual void writeIVTCByteImage(unsigned int w, unsigned int h, const std::string&) override;
- virtual void writeOpenCVMat(const std::vector<int>&, const std::string&) override;
- virtual void writePCLPointCloud(unsigned int w, unsigned int h, const std::string&) override;
- virtual void writePosition() override;
- virtual void writeOrientation() override;
- virtual void writePose() override;
+ virtual void writeEigenMatrix(const WriteEigenMatrixInput&) override;
+ virtual void writeEigenQuaternion(const WriteEigenQuaternionInput&) override;
+ virtual void writeIVTCByteImage(const WriteIVTCByteImageInput&) override;
+ virtual void writeOpenCVMat(const WriteOpenCVMatInput&) override;
+ virtual void writePCLPointCloud(const WritePCLPointCloudInput&) override;
+ virtual void writePosition(const WritePositionInput&) override;
+ virtual void writeOrientation(const WriteOrientationInput&) override;
+ virtual void writePose(const WritePoseInput&) override;
- virtual void writeInt() override;
- virtual void writeLong() override;
- virtual void writeFloat() override;
- virtual void writeDouble() override;
- virtual void writeString() override;
- virtual void writeBool() override;
- virtual void writeTime() override;
+ virtual void writeInt(const WritePrimitiveInput&) override;
+ virtual void writeLong(const WritePrimitiveInput&) override;
+ virtual void writeFloat(const WritePrimitiveInput&) override;
+ virtual void writeDouble(const WritePrimitiveInput&) override;
+ virtual void writeString(const WritePrimitiveInput&) override;
+ virtual void writeBool(const WritePrimitiveInput&) override;
+ virtual void writeTime(const WritePrimitiveInput&) override;
virtual void writeKey(const std::string&) override;
diff --git a/source/RobotAPI/libraries/aron/core/io/typeIO/writer/nlohmannJSON/NlohmannJSONWriter.cpp b/source/RobotAPI/libraries/aron/core/io/typeIO/writer/nlohmannJSON/NlohmannJSONWriter.cpp
index 0f3e96192baf5c2c5d088e0c3a5a2fb4e354d68d..1ce3afe2394a3a20aa89948db49f97493d45865f 100644
--- a/source/RobotAPI/libraries/aron/core/io/typeIO/writer/nlohmannJSON/NlohmannJSONWriter.cpp
+++ b/source/RobotAPI/libraries/aron/core/io/typeIO/writer/nlohmannJSON/NlohmannJSONWriter.cpp
@@ -26,10 +26,11 @@
namespace armarx::aron::typeIO::writer
{
- void NlohmannJSONWriter::writeStartObject(const std::string& n)
+ void NlohmannJSONWriter::writeStartObject(const WriteStartObjectInput& o)
{
nlohmann::json data;
- data[io::Data::READER_WRITER_NAME_SLUG] = n;
+ data[io::Data::READER_WRITER_OBJECT_NAME_SLUG] = o.name;
+ data[io::Data::READER_WRITER_MAYBE_SLUG] = o.maybe;
auto new_token = std::make_shared<NlohmannJSONWriterToken>(type::Descriptor::eObject, data);
stack.push(new_token);
}
@@ -46,9 +47,10 @@ namespace armarx::aron::typeIO::writer
}
}
- void NlohmannJSONWriter::writeStartList()
+ void NlohmannJSONWriter::writeStartList(const WriteStartListInput& o)
{
nlohmann::json data;
+ data[io::Data::READER_WRITER_MAYBE_SLUG] = o.maybe;
auto new_token = std::make_shared<NlohmannJSONWriterToken>(type::Descriptor::eList, data);
stack.push(new_token);
}
@@ -65,9 +67,10 @@ namespace armarx::aron::typeIO::writer
}
}
- void NlohmannJSONWriter::writeStartDict()
+ void NlohmannJSONWriter::writeStartDict(const WriteStartDictInput& o)
{
nlohmann::json data;
+ data[io::Data::READER_WRITER_MAYBE_SLUG] = o.maybe;
auto new_token = std::make_shared<NlohmannJSONWriterToken>(type::Descriptor::eDict, data);
stack.push(new_token);
}
@@ -84,9 +87,10 @@ namespace armarx::aron::typeIO::writer
}
}
- void NlohmannJSONWriter::writeStartTuple()
+ void NlohmannJSONWriter::writeStartTuple(const WriteStartTupleInput& o)
{
nlohmann::json data;
+ data[io::Data::READER_WRITER_MAYBE_SLUG] = o.maybe;
auto new_token = std::make_shared<NlohmannJSONWriterToken>(type::Descriptor::eTuple, data);
stack.push(new_token);
}
@@ -103,9 +107,10 @@ namespace armarx::aron::typeIO::writer
}
}
- void NlohmannJSONWriter::writeStartPair()
+ void NlohmannJSONWriter::writeStartPair(const WriteStartPairInput& o)
{
nlohmann::json data;
+ data[io::Data::READER_WRITER_MAYBE_SLUG] = o.maybe;
auto new_token = std::make_shared<NlohmannJSONWriterToken>(type::Descriptor::ePair, data);
stack.push(new_token);
}
@@ -122,125 +127,142 @@ namespace armarx::aron::typeIO::writer
}
}
- void NlohmannJSONWriter::writeEigenMatrix(const std::vector<int>& dims, const std::string& t)
+ void NlohmannJSONWriter::writeEigenMatrix(const WriteEigenMatrixInput& o)
{
auto token = stack.top();
nlohmann::json j;
+ j[io::Data::READER_WRITER_MAYBE_SLUG] = o.maybe;
j[io::Data::READER_WRITER_NDARRAY_NAME_SLUG] = "EigenMatrix";
- j[io::Data::READER_WRITER_NDARRAY_DIMENSIONS_SLUG] = dims;
- j[io::Data::READER_WRITER_NDARRAY_TYPE_SLUG] = t;
+ j[io::Data::READER_WRITER_NDARRAY_DIMENSIONS_SLUG] = std::vector<unsigned int>({o.rows, o.cols});
+ j[io::Data::READER_WRITER_NDARRAY_TYPE_SLUG] = o.type;
token->addElement(j);
}
- void NlohmannJSONWriter::writeEigenQuaternion(const std::string& t)
+ void NlohmannJSONWriter::writeEigenQuaternion(const WriteEigenQuaternionInput& o)
{
auto token = stack.top();
nlohmann::json j;
+ j[io::Data::READER_WRITER_MAYBE_SLUG] = o.maybe;
j[io::Data::READER_WRITER_NDARRAY_NAME_SLUG] = "EigenQuaternion";
- j[io::Data::READER_WRITER_NDARRAY_TYPE_SLUG] = t;
+ j[io::Data::READER_WRITER_NDARRAY_TYPE_SLUG] = o.type;
token->addElement(j);
}
- void NlohmannJSONWriter::writeIVTCByteImage(unsigned int w, unsigned int h, const std::string& t)
+ void NlohmannJSONWriter::writeIVTCByteImage(const WriteIVTCByteImageInput& o)
{
auto token = stack.top();
nlohmann::json j;
+ j[io::Data::READER_WRITER_MAYBE_SLUG] = o.maybe;
j[io::Data::READER_WRITER_NDARRAY_NAME_SLUG] = "IVTCByteImage";
- j[io::Data::READER_WRITER_NDARRAY_DIMENSIONS_SLUG] = {w, h};
- j[io::Data::READER_WRITER_NDARRAY_TYPE_SLUG] = t;
token->addElement(j);
}
- void NlohmannJSONWriter::writeOpenCVMat(const std::vector<int>& dims, const std::string& t)
+ void NlohmannJSONWriter::writeOpenCVMat(const WriteOpenCVMatInput& o)
{
auto token = stack.top();
nlohmann::json j;
+ j[io::Data::READER_WRITER_MAYBE_SLUG] = o.maybe;
j[io::Data::READER_WRITER_NDARRAY_NAME_SLUG] = "OpenCVMat";
- j[io::Data::READER_WRITER_NDARRAY_DIMENSIONS_SLUG] = dims;
- j[io::Data::READER_WRITER_NDARRAY_TYPE_SLUG] = t;
token->addElement(j);
}
- void NlohmannJSONWriter::writePCLPointCloud(unsigned int w, unsigned int h, const std::string& t)
+ void NlohmannJSONWriter::writePCLPointCloud(const WritePCLPointCloudInput& o)
{
auto token = stack.top();
nlohmann::json j;
+ j[io::Data::READER_WRITER_MAYBE_SLUG] = o.maybe;
j[io::Data::READER_WRITER_NDARRAY_NAME_SLUG] = "PCLPointCloud";
- j[io::Data::READER_WRITER_NDARRAY_DIMENSIONS_SLUG] = {w, h};
- j[io::Data::READER_WRITER_NDARRAY_TYPE_SLUG] = t;
+ j[io::Data::READER_WRITER_NDARRAY_TYPE_SLUG] = o.type;
token->addElement(j);
}
- void NlohmannJSONWriter::writePosition()
+ void NlohmannJSONWriter::writePosition(const WritePositionInput& o)
{
auto token = stack.top();
nlohmann::json j;
+ j[io::Data::READER_WRITER_MAYBE_SLUG] = o.maybe;
j[io::Data::READER_WRITER_NDARRAY_NAME_SLUG] = "Position";
token->addElement(j);
}
- void NlohmannJSONWriter::writeOrientation()
+ void NlohmannJSONWriter::writeOrientation(const WriteOrientationInput& o)
{
auto token = stack.top();
nlohmann::json j;
+ j[io::Data::READER_WRITER_MAYBE_SLUG] = o.maybe;
j[io::Data::READER_WRITER_NDARRAY_NAME_SLUG] = "Orientation";
token->addElement(j);
}
- void NlohmannJSONWriter::writePose()
+ void NlohmannJSONWriter::writePose(const WritePoseInput& o)
{
auto token = stack.top();
nlohmann::json j;
+ j[io::Data::READER_WRITER_MAYBE_SLUG] = o.maybe;
j[io::Data::READER_WRITER_NDARRAY_NAME_SLUG] = "Pose";
token->addElement(j);
}
- void NlohmannJSONWriter::writeInt()
+ void NlohmannJSONWriter::writeInt(const WritePrimitiveInput& o)
{
auto token = stack.top();
- nlohmann::json j(io::Data::READER_WRITER_INT_TYPENAME_SLUG);
+ nlohmann::json j;
+ j[io::Data::READER_WRITER_TYPE_SLUG] = io::Data::READER_WRITER_INT_TYPENAME_SLUG;
+ j[io::Data::READER_WRITER_MAYBE_SLUG] = o.maybe;
token->addElement(j);
}
- void NlohmannJSONWriter::writeLong()
+ void NlohmannJSONWriter::writeLong(const WritePrimitiveInput& o)
{
auto token = stack.top();
- nlohmann::json j(io::Data::READER_WRITER_LONG_TYPENAME_SLUG);
+ nlohmann::json j;
+ j[io::Data::READER_WRITER_TYPE_SLUG] = io::Data::READER_WRITER_LONG_TYPENAME_SLUG;
+ j[io::Data::READER_WRITER_MAYBE_SLUG] = o.maybe;
token->addElement(j);
}
- void NlohmannJSONWriter::writeFloat()
+ void NlohmannJSONWriter::writeFloat(const WritePrimitiveInput& o)
{
auto token = stack.top();
- nlohmann::json j(io::Data::READER_WRITER_FLOAT_TYPENAME_SLUG);
+ nlohmann::json j;
+ j[io::Data::READER_WRITER_TYPE_SLUG] = io::Data::READER_WRITER_FLOAT_TYPENAME_SLUG;
+ j[io::Data::READER_WRITER_MAYBE_SLUG] = o.maybe;
token->addElement(j);
}
- void NlohmannJSONWriter::writeDouble()
+ void NlohmannJSONWriter::writeDouble(const WritePrimitiveInput& o)
{
auto token = stack.top();
- nlohmann::json j(io::Data::READER_WRITER_DOUBLE_TYPENAME_SLUG);
+ nlohmann::json j;
+ j[io::Data::READER_WRITER_TYPE_SLUG] = io::Data::READER_WRITER_DOUBLE_TYPENAME_SLUG;
+ j[io::Data::READER_WRITER_MAYBE_SLUG] = o.maybe;
token->addElement(j);
}
- void NlohmannJSONWriter::writeString()
+ void NlohmannJSONWriter::writeString(const WritePrimitiveInput& o)
{
auto token = stack.top();
- nlohmann::json j(io::Data::READER_WRITER_STRING_TYPENAME_SLUG);
+ nlohmann::json j;
+ j[io::Data::READER_WRITER_TYPE_SLUG] = io::Data::READER_WRITER_STRING_TYPENAME_SLUG;
+ j[io::Data::READER_WRITER_MAYBE_SLUG] = o.maybe;
token->addElement(j);
}
- void NlohmannJSONWriter::writeBool()
+ void NlohmannJSONWriter::writeBool(const WritePrimitiveInput& o)
{
auto token = stack.top();
- nlohmann::json j(io::Data::READER_WRITER_BOOL_TYPENAME_SLUG);
+ nlohmann::json j;
+ j[io::Data::READER_WRITER_TYPE_SLUG] = io::Data::READER_WRITER_BOOL_TYPENAME_SLUG;
+ j[io::Data::READER_WRITER_MAYBE_SLUG] = o.maybe;
token->addElement(j);
}
- void NlohmannJSONWriter::writeTime()
+ void NlohmannJSONWriter::writeTime(const WritePrimitiveInput& o)
{
auto token = stack.top();
- nlohmann::json j(io::Data::READER_WRITER_TIME_TYPENAME_SLUG);
+ nlohmann::json j;
+ j[io::Data::READER_WRITER_TYPE_SLUG] = io::Data::READER_WRITER_TIME_TYPENAME_SLUG;
+ j[io::Data::READER_WRITER_MAYBE_SLUG] = o.maybe;
token->addElement(j);
}
diff --git a/source/RobotAPI/libraries/aron/core/io/typeIO/writer/nlohmannJSON/NlohmannJSONWriter.h b/source/RobotAPI/libraries/aron/core/io/typeIO/writer/nlohmannJSON/NlohmannJSONWriter.h
index 7826f895e8e9fec9f3a98a12413ed1f39d12a9e9..3be0f4a1329aca74ad3c19710736a86f1c542582 100644
--- a/source/RobotAPI/libraries/aron/core/io/typeIO/writer/nlohmannJSON/NlohmannJSONWriter.h
+++ b/source/RobotAPI/libraries/aron/core/io/typeIO/writer/nlohmannJSON/NlohmannJSONWriter.h
@@ -40,33 +40,33 @@ namespace armarx::aron::typeIO::writer
public:
NlohmannJSONWriter() = default;
- virtual void writeStartObject(const std::string&) override;
+ virtual void writeStartObject(const WriteStartObjectInput&) override;
virtual void writeEndObject() override;
- virtual void writeStartList() override;
+ virtual void writeStartList(const WriteStartListInput&) override;
virtual void writeEndList() override;
- virtual void writeStartDict() override;
+ virtual void writeStartDict(const WriteStartDictInput&) override;
virtual void writeEndDict() override;
- virtual void writeStartTuple() override;
+ virtual void writeStartTuple(const WriteStartTupleInput&) override;
virtual void writeEndTuple() override;
- virtual void writeStartPair() override;
+ virtual void writeStartPair(const WriteStartPairInput&) override;
virtual void writeEndPair() override;
- virtual void writeEigenMatrix(const std::vector<int>&, const std::string&) override;
- virtual void writeEigenQuaternion(const std::string&) override;
- virtual void writeIVTCByteImage(unsigned int w, unsigned int h, const std::string&) override;
- virtual void writeOpenCVMat(const std::vector<int>&, const std::string&) override;
- virtual void writePCLPointCloud(unsigned int w, unsigned int h, const std::string&) override;
- virtual void writePosition() override;
- virtual void writeOrientation() override;
- virtual void writePose() override;
+ virtual void writeEigenMatrix(const WriteEigenMatrixInput&) override;
+ virtual void writeEigenQuaternion(const WriteEigenQuaternionInput&) override;
+ virtual void writeIVTCByteImage(const WriteIVTCByteImageInput&) override;
+ virtual void writeOpenCVMat(const WriteOpenCVMatInput&) override;
+ virtual void writePCLPointCloud(const WritePCLPointCloudInput&) override;
+ virtual void writePosition(const WritePositionInput&) override;
+ virtual void writeOrientation(const WriteOrientationInput&) override;
+ virtual void writePose(const WritePoseInput&) override;
- virtual void writeInt() override;
- virtual void writeLong() override;
- virtual void writeFloat() override;
- virtual void writeDouble() override;
- virtual void writeString() override;
- virtual void writeBool() override;
- virtual void writeTime() override;
+ virtual void writeInt(const WritePrimitiveInput&) override;
+ virtual void writeLong(const WritePrimitiveInput&) override;
+ virtual void writeFloat(const WritePrimitiveInput&) override;
+ virtual void writeDouble(const WritePrimitiveInput&) override;
+ virtual void writeString(const WritePrimitiveInput&) override;
+ virtual void writeBool(const WritePrimitiveInput&) override;
+ virtual void writeTime(const WritePrimitiveInput&) override;
virtual void writeKey(const std::string&) override;
diff --git a/source/RobotAPI/libraries/aron/core/io/typeIO/writer/nlohmannJSON/NlohmannJSONWriterToken.h b/source/RobotAPI/libraries/aron/core/io/typeIO/writer/nlohmannJSON/NlohmannJSONWriterToken.h
index ea4410885b98ece7ee08e8c0ffb1c7443253c5f6..bea71d05b68deca116a253bbf9891ef29f7393a3 100644
--- a/source/RobotAPI/libraries/aron/core/io/typeIO/writer/nlohmannJSON/NlohmannJSONWriterToken.h
+++ b/source/RobotAPI/libraries/aron/core/io/typeIO/writer/nlohmannJSON/NlohmannJSONWriterToken.h
@@ -92,7 +92,7 @@ namespace armarx::aron::typeIO::writer
{
throw error::DescriptorNotValidException("NlohmannJSONWriterToken", "setName", "Cant set the name of a non-object token.", desc);
}
- element[io::Data::READER_WRITER_NAME_SLUG] = n;
+ element[io::Data::READER_WRITER_OBJECT_NAME_SLUG] = n;
}
};
}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/Navigator.h b/source/RobotAPI/libraries/aron/core/navigator/Navigator.h
index 807e24579720bffd67de117529b4a1b7f713ba32..8077649e619f023f73c79e7dc636ec9acb4a5643 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/Navigator.h
+++ b/source/RobotAPI/libraries/aron/core/navigator/Navigator.h
@@ -48,7 +48,7 @@ namespace armarx::aron
{
public:
// constructors
- Navigator() = delete;
+ Navigator() = default;
Navigator(const Descriptor& descriptor, const Path& path) :
descriptor(descriptor),
path(path)
@@ -71,20 +71,11 @@ namespace armarx::aron
return path.toString();
}
- // virual definitions
- virtual typename Aron::PointerType getResult() const = 0;
+ // virtual definitions
+ virtual typename Aron::PointerType toAronPtr() const = 0;
virtual std::string getName() const = 0;
protected:
- static void CheckAronPtrForNull(const std::string& c, const std::string& m, const Path& p, const typename Aron::PointerType& data)
- {
- if (data.get() == nullptr)
- {
- throw error::AronException(c, m, "Could not cast an AronPtr. The Ptr was NULL.", p);
- }
- }
-
- private:
// members
const Descriptor descriptor;
const Path path;
diff --git a/source/RobotAPI/libraries/aron/core/navigator/NavigatorFactory.h b/source/RobotAPI/libraries/aron/core/navigator/NavigatorFactory.h
index c8d8ea651ab62129bf1268b9aa15ec0a22cba669..f66f163ee906f4e1c20b4a48e0882b1a3f2a6bc0 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/NavigatorFactory.h
+++ b/source/RobotAPI/libraries/aron/core/navigator/NavigatorFactory.h
@@ -38,7 +38,7 @@
namespace armarx::aron
{
- template <typename Input, typename Output> // requires ...
+ template <typename Input, typename Output, typename EnumT> // requires ...
class NavigatorFactory :
virtual public Factory<Input, Output>
{
diff --git a/source/RobotAPI/libraries/aron/core/navigator/data/AllNavigators.h b/source/RobotAPI/libraries/aron/core/navigator/data/AllNavigators.h
index 9ea0326951e3488d1cbf9870a2a50c475f6708f5..7e857a199fff69f5fe986161f2b524b80cbfef21 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/data/AllNavigators.h
+++ b/source/RobotAPI/libraries/aron/core/navigator/data/AllNavigators.h
@@ -3,4 +3,9 @@
#include "container/Dict.h"
#include "container/List.h"
#include "complex/NDArray.h"
-#include "primitive/Primitive.h"
+#include "primitive/Int.h"
+#include "primitive/Long.h"
+#include "primitive/Float.h"
+#include "primitive/Double.h"
+#include "primitive/String.h"
+#include "primitive/Bool.h"
diff --git a/source/RobotAPI/libraries/aron/core/navigator/data/Navigator.cpp b/source/RobotAPI/libraries/aron/core/navigator/data/Navigator.cpp
index 4372e4e63ae646ae1e6342c0d91add66c6a5a76d..ff4a18c89c6b8cc41c76ce29208006a3eb849e23 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/data/Navigator.cpp
+++ b/source/RobotAPI/libraries/aron/core/navigator/data/Navigator.cpp
@@ -25,20 +25,16 @@
#include "Navigator.h"
// ArmarX
-#include <RobotAPI/libraries/aron/core/Exception.h>
-#include <RobotAPI/libraries/aron/core/navigator/data/NavigatorFactory.h>
+#include "../../Exception.h"
+#include "NavigatorFactory.h"
+#include "container/Dict.h"
+
namespace armarx::aron::datanavigator
{
// static data members
const NavigatorFactoryPtr Navigator::FACTORY = NavigatorFactoryPtr(new NavigatorFactory());
- // constructors
- Navigator::Navigator(const data::Descriptor& descriptor, const Path& path) :
- aron::Navigator<data::Descriptor, data::AronData>::Navigator(descriptor, path)
- {
- }
-
// static methods
NavigatorPtr Navigator::FromAronData(const data::AronDataPtr& a, const Path& path)
{
@@ -59,27 +55,11 @@ namespace armarx::aron::datanavigator
std::vector<data::AronDataPtr> ret;
for (const auto& aron : a)
{
- ret.push_back(aron->getResult());
+ ret.push_back(aron->toAronPtr());
}
return ret;
}
- void Navigator::CheckDataNavigatorPtrForNull(const std::string& c, const std::string& m, const NavigatorPtr& data)
- {
- if (data.get() == nullptr)
- {
- throw error::AronException(c, m, "Could not make use of an NavigatorPtr. The Ptr was NULL");
- }
- }
-
- void Navigator::CheckDataNavigatorPtrForNull(const std::string& c, const std::string& m, const Path& p, const NavigatorPtr& data)
- {
- if (data.get() == nullptr)
- {
- throw error::AronException(c, m, "Could not make use of an NavigatorPtr. The Ptr was NULL", p);
- }
- }
-
NavigatorPtr Navigator::navigateAbsolute(const Path& path) const
{
throw error::AronException("Navigator", "navigateAbsolute", "Could not navigate through a non container navigator. The input path was: " + path.toString(), getPath());
@@ -91,23 +71,4 @@ namespace armarx::aron::datanavigator
}
- /*AronContainerDataNavigatorPtr AronContainerDataNavigator::DynamicCast(const NavigatorPtr& n)
- {
- AronContainerDataNavigatorPtr casted = std::dynamic_pointer_cast<AronContainerDataNavigator>(n);
- return casted;
- }
-
- AronContainerDataNavigatorPtr AronContainerDataNavigator::DynamicCastAndCheck(const NavigatorPtr& n)
- {
- CheckDataNavigatorPtrForNull("AronContainerDataNavigator", "DynamicCastAndCheck[Before]", n);
- AronContainerDataNavigatorPtr casted = AronContainerDataNavigator::DynamicCast(n);
- CheckDataNavigatorPtrForNull("AronContainerDataNavigator", "DynamicCastAndCheck[After]", n->getPath(), casted);
- return casted;
- }
-
- NavigatorPtr AronContainerDataNavigator::navigateRelative(const Path& path) const
- {
- Path absoluteFromHere = path.getWithoutPrefix(getPath());
- return navigateAbsolute(absoluteFromHere);
- }*/
}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/data/Navigator.h b/source/RobotAPI/libraries/aron/core/navigator/data/Navigator.h
index e703ba364db0607e0c2732bf56ca62ce72260267..5721a19d68e4a33e0df65130375eabc7ba5728ae 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/data/Navigator.h
+++ b/source/RobotAPI/libraries/aron/core/navigator/data/Navigator.h
@@ -50,32 +50,34 @@ namespace armarx::aron::datanavigator
public:
// constructors
- Navigator() = delete;
- Navigator(const data::Descriptor&, const Path& p = Path());
+ Navigator() = default;
// operators
- virtual bool equalsDataNavigator(const NavigatorPtr&) const = 0;
+ virtual bool operator==(const Navigator& other) const = 0;
+ virtual bool operator==(const NavigatorPtr& other) const = 0;
// static methods
static NavigatorPtr FromAronData(const data::AronDataPtr&, const Path& = Path());
static std::vector<NavigatorPtr> FromAronData(const std::vector<data::AronDataPtr>&, const Path& = Path());
static std::vector<data::AronDataPtr> ToAronData(const std::vector<NavigatorPtr>&);
- template<typename NavigatorType>
- static typename NavigatorType::PointerType DynamicCast(const NavigatorPtr& n)
+ template<typename NavigatorT>
+ static typename NavigatorT::PointerType DynamicCast(const NavigatorPtr& n)
{
- return NavigatorType::DynamicCast(n);
+ return NavigatorT::DynamicCast(n);
}
- template<typename NavigatorType>
- static typename NavigatorType::PointerType DynamicCastAndCheck(const NavigatorPtr& n)
+ template<typename NavigatorT>
+ static typename NavigatorT::PointerType DynamicCast(const Navigator& n)
{
- return NavigatorType::DynamicCastAndCheck(n);
+ return NavigatorT::DynamicCast(n);
}
- // virtual definitions of base class
- virtual data::AronDataPtr getResult() const override = 0;
- virtual std::string getName() const override = 0;
+ template<typename NavigatorT>
+ static typename NavigatorT::PointerType DynamicCastAndCheck(const NavigatorPtr& n)
+ {
+ return NavigatorT::DynamicCastAndCheck(n);
+ }
// virtual definitions
virtual std::vector<NavigatorPtr> getChildren() const = 0;
@@ -87,9 +89,11 @@ namespace armarx::aron::datanavigator
virtual NavigatorPtr navigateAbsolute(const Path& path) const;
virtual NavigatorPtr navigateRelative(const Path& path) const;
- protected:
- static void CheckDataNavigatorPtrForNull(const std::string&, const std::string&, const NavigatorPtr&);
- static void CheckDataNavigatorPtrForNull(const std::string&, const std::string&, const Path&, const NavigatorPtr&);
+ // public member functions
+ data::AronDataPtr toAronDataPtr() const
+ {
+ return toAronPtr();
+ }
private:
static const NavigatorFactoryPtr FACTORY;
diff --git a/source/RobotAPI/libraries/aron/core/navigator/data/NavigatorFactory.cpp b/source/RobotAPI/libraries/aron/core/navigator/data/NavigatorFactory.cpp
index 1adcdfff3dd6786b6042bec2b20591ad56ff3281..6a4a62854a06a6f9c4457464b2d4f8035a66f012 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/data/NavigatorFactory.cpp
+++ b/source/RobotAPI/libraries/aron/core/navigator/data/NavigatorFactory.cpp
@@ -43,11 +43,16 @@ namespace armarx::aron::datanavigator
};
#undef RUN_ARON_MACRO
- auto typeDescriptor = Resolver::GetDescriptor(aron);
- auto factory_iterator = Factories.find(typeDescriptor);
+ if (!aron)
+ {
+ return nullptr;
+ }
+
+ auto dataDescriptor = Resolver::GetDescriptor(aron);
+ auto factory_iterator = Factories.find(dataDescriptor);
if (factory_iterator == Factories.end())
{
- throw error::AronException("NavigatorFactory", "create", "Cannot find the desired factory for input: " + data::DESCRIPTOR_TO_STRING(typeDescriptor) + ". Cannot create navigator", path);
+ throw error::AronException("NavigatorFactory", "create", "Cannot find the desired factory for input: " + data::DESCRIPTOR_TO_STRING(dataDescriptor) + ". Cannot create navigator", path);
}
return factory_iterator->second->createSpecific(aron, path);
}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/data/NavigatorFactory.h b/source/RobotAPI/libraries/aron/core/navigator/data/NavigatorFactory.h
index 63d08c14524f7861e58d7294758e7582eaa2b434..5e92b40922ac3750aec89feb38d75ce522a966da 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/data/NavigatorFactory.h
+++ b/source/RobotAPI/libraries/aron/core/navigator/data/NavigatorFactory.h
@@ -40,7 +40,7 @@ namespace armarx::aron::datanavigator
typedef std::shared_ptr<NavigatorFactory> NavigatorFactoryPtr;
class NavigatorFactory :
- virtual public Factory<data::AronDataPtr, datanavigator::NavigatorPtr>
+ virtual public armarx::aron::NavigatorFactory<data::AronDataPtr, datanavigator::NavigatorPtr, data::Descriptor>
{
public:
NavigatorFactory() = default;
diff --git a/source/RobotAPI/libraries/aron/core/navigator/data/NavigatorFwd.h b/source/RobotAPI/libraries/aron/core/navigator/data/NavigatorFwd.h
new file mode 100644
index 0000000000000000000000000000000000000000..64ac3a8c95258d5c9585570935660252bbec58fb
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/navigator/data/NavigatorFwd.h
@@ -0,0 +1,41 @@
+/**
+ * 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/>.
+ *
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <memory>
+
+// TODO add forward declarations for those as well
+// #include "complex/NDArray.h"
+// #include "container/List.h"
+// #include "primitive/Bool.h"
+// #include "primitive/Double.h"
+// #include "primitive/Float.h"
+// #include "primitive/Int.h"
+// #include "primitive/Long.h"
+// #include "primitive/String.h"
+
+namespace armarx::aron::datanavigator
+{
+ class DictNavigator;
+ using DictNavigatorPtr = std::shared_ptr<DictNavigator>;
+
+} // namespace armarx::aron::datanavigator
diff --git a/source/RobotAPI/libraries/aron/core/navigator/data/complex/NDArray.cpp b/source/RobotAPI/libraries/aron/core/navigator/data/complex/NDArray.cpp
index 6806d9dd41abe290d246d7a66a036f39a499461a..eb1a486374c836d8156d8055e543379de9840a47 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/data/complex/NDArray.cpp
+++ b/source/RobotAPI/libraries/aron/core/navigator/data/complex/NDArray.cpp
@@ -29,6 +29,7 @@
// ArmarX
#include <RobotAPI/libraries/aron/core/navigator/data/NavigatorFactory.h>
+#include <RobotAPI/libraries/aron/core/navigator/type/ndarray/NDArray.h>
#include <RobotAPI/libraries/aron/core/navigator/type/ndarray/OpenCVMat.h>
#include <RobotAPI/libraries/aron/core/navigator/type/ndarray/EigenMatrix.h>
#include <RobotAPI/libraries/aron/core/navigator/type/ndarray/EigenQuaternion.h>
@@ -42,19 +43,15 @@ namespace armarx::aron::datanavigator
{
// constructors
NDArrayNavigator::NDArrayNavigator(const Path& path) :
- aron::Navigator<data::Descriptor, data::AronData>::Navigator(data::Descriptor::eNDArray, path),
- Navigator(data::Descriptor::eNDArray, path),
- aron(new data::AronNDArray())
+ aron::Navigator<data::Descriptor, data::AronData>::Navigator(data::Descriptor::eNDArray, path)
{
}
NDArrayNavigator::NDArrayNavigator(const data::AronNDArrayPtr& o, const Path& path) :
aron::Navigator<data::Descriptor, data::AronData>::Navigator(data::Descriptor::eNDArray, path),
- Navigator(data::Descriptor::eNDArray, path),
- aron(o)
+ detail::NavigatorBase<data::AronNDArray, NDArrayNavigator>(o)
{
- CheckAronPtrForNull("NDArrayNavigator", "NDArrayNavigator", getPath(), aron);
}
NDArrayNavigator::NDArrayNavigator(const std::vector<int>& dim, const std::string& t, const std::vector<unsigned char>& data, const Path& path) :
@@ -81,47 +78,16 @@ namespace armarx::aron::datanavigator
}
return true;
}
-
- bool NDArrayNavigator::equalsDataNavigator(const NavigatorPtr& other) const
- {
- if (other == nullptr)
- {
- return false;
- }
- NDArrayNavigatorPtr casted = DynamicCast(other);
- return equalsDeep(casted);
- }
-
- bool NDArrayNavigator::equalsDeep(const NDArrayNavigatorPtr& other) const
+ bool NDArrayNavigator::operator==(const NDArrayNavigatorPtr& other) const
{
- if (other == nullptr)
+ if (!other)
{
return false;
}
return *this == *other;
}
-
// static methods
- NDArrayNavigatorPtr NDArrayNavigator::DynamicCast(const NavigatorPtr& n)
- {
- NDArrayNavigatorPtr casted = std::dynamic_pointer_cast<NDArrayNavigator>(n);
- return casted;
- }
-
- NDArrayNavigator NDArrayNavigator::DynamicCast(Navigator& n)
- {
- return dynamic_cast<NDArrayNavigator&>(n);
- }
-
- NDArrayNavigatorPtr NDArrayNavigator::DynamicCastAndCheck(const NavigatorPtr& n)
- {
- CheckDataNavigatorPtrForNull("NDArrayNavigator", "DynamicCastAndCheck[Before]", n);
- NDArrayNavigatorPtr casted = NDArrayNavigator::DynamicCast(n);
- CheckDataNavigatorPtrForNull("NDArrayNavigator", "DynamicCastAndCheck[After]", n->getPath(), casted);
- return casted;
- }
-
NDArrayNavigatorPtr NDArrayNavigator::FromAronNDArrayPtr(const data::AronNDArrayPtr& aron)
{
return std::make_shared<NDArrayNavigator>(aron);
@@ -133,6 +99,14 @@ namespace armarx::aron::datanavigator
}
// public member functions
+ DictNavigatorPtr NDArrayNavigator::getAsDict() const
+ {
+ auto dict = std::make_shared<DictNavigator>();
+ auto copy_this = FromAronData(toAronDataPtr(), getPath());
+ dict->addElement("data", copy_this);
+ return dict;
+ }
+
unsigned char* NDArrayNavigator::getData() const
{
return aron->data.data();
@@ -180,11 +154,6 @@ namespace armarx::aron::datanavigator
}
// virtual implementations
- data::AronDataPtr NDArrayNavigator::getResult() const
- {
- return toAronNDArrayPtr();
- }
-
std::string NDArrayNavigator::getName() const
{
return "AronNDArray<" + simox::alg::to_string(aron->dimensions, ", ") + ", " + aron->type + ">";
@@ -192,10 +161,8 @@ namespace armarx::aron::datanavigator
typenavigator::NavigatorPtr NDArrayNavigator::recalculateType() const
{
- // We set all to openCVMat because it accepts n-dimensional types
+ // We set all to openCVMat because it accepts n-dimensional types TODO
typenavigator::OpenCVMatNavigatorPtr typenav = typenavigator::OpenCVMatNavigatorPtr(new typenavigator::OpenCVMatNavigator(getPath()));
- typenav->setDimensions(std::vector<int>(aron->dimensions.begin(), std::prev(aron->dimensions.end())));
- typenav->setTypename(aron->type);
return typenav;
}
@@ -208,19 +175,16 @@ namespace armarx::aron::datanavigator
switch (type->getDescriptor())
{
-#define RUN_ARON_MACRO(upperType, lowerType, capsType) \
-case type::Descriptor::e##upperType: \
-{ \
- typenavigator::upperType##NavigatorPtr casted = typenavigator::upperType##Navigator::DynamicCast(type); \
- if (std::vector<int>(aron->dimensions.begin(), std::prev(aron->dimensions.end())) != casted->getDimensions() || aron->type != casted->getTypename()) \
- { \
- return false; \
- } \
- break; \
-}
-
- HANDLE_NDARRAY_TYPES
-#undef RUN_ARON_MACRO
+ case type::Descriptor::eEigenMatrix:
+ {
+ auto casted = typenavigator::EigenMatrixNavigator::DynamicCast(type);
+ if ((unsigned int) aron->dimensions[0] != casted->getRows() || (unsigned int) aron->dimensions[1] != casted->getCols())
+ {
+ return false;
+ }
+ break;
+ }
+ // TODO:
default:
throw error::AronException("NDArrayNavigator", "fullfillsType", "Could not cast a type to an NDArray-Type", getPath());
}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/data/complex/NDArray.h b/source/RobotAPI/libraries/aron/core/navigator/data/complex/NDArray.h
index c6d12cc6b896a53a8b28cbc2e6c134fae58612f0..e41fbdb6518bc3e7f5f57999fc3fd3de2f14fa13 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/data/complex/NDArray.h
+++ b/source/RobotAPI/libraries/aron/core/navigator/data/complex/NDArray.h
@@ -31,10 +31,21 @@
#include <numeric>
#include <vector>
-// ArmarX
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+// Base class
+#include "../detail/NavigatorBase.h"
-#include <RobotAPI/libraries/aron/core/navigator/data/Navigator.h>
+// ArmarX
+#include "../container/Dict.h"
+
+#include "../../type/ndarray/EigenMatrix.h"
+#include "../../type/ndarray/EigenQuaternion.h"
+#include "../../type/ndarray/IVTCByteImage.h"
+#include "../../type/ndarray/NDArray.h"
+#include "../../type/ndarray/OpenCVMat.h"
+#include "../../type/ndarray/Orientation.h"
+#include "../../type/ndarray/PCLPointCloud.h"
+#include "../../type/ndarray/Pose.h"
+#include "../../type/ndarray/Position.h"
namespace armarx::aron::datanavigator
{
@@ -42,33 +53,28 @@ namespace armarx::aron::datanavigator
using NDArrayNavigatorPtr = std::shared_ptr<NDArrayNavigator>;
class NDArrayNavigator :
- virtual public Navigator
+ virtual public detail::NavigatorBase<data::AronNDArray, NDArrayNavigator>
{
public:
- using PointerType = NDArrayNavigatorPtr;
-
// constructors
NDArrayNavigator(const Path& path = Path());
NDArrayNavigator(const data::AronNDArrayPtr&, const Path& path = Path());
NDArrayNavigator(const std::vector<int>&, const std::string&, const std::vector<unsigned char>&, const Path& path = Path());
// operators
- bool operator==(const NDArrayNavigator&) const;
- virtual bool equalsDataNavigator(const NavigatorPtr&) const override;
- bool equalsDeep(const NDArrayNavigatorPtr&) const;
+ virtual bool operator==(const NDArrayNavigator&) const override;
+ bool operator==(const NDArrayNavigatorPtr&) const override;
// static methods
- static NDArrayNavigatorPtr DynamicCast(const NavigatorPtr& n);
- static NDArrayNavigator DynamicCast(Navigator& n);
- static NDArrayNavigatorPtr DynamicCastAndCheck(const NavigatorPtr& n);
-
- static NDArrayNavigatorPtr FromAronNDArrayPtr(const data::AronNDArrayPtr& aron);
- static data::AronNDArrayPtr ToAronNDArrayPtr(const NDArrayNavigatorPtr& navigator);
+ static PointerType FromAronNDArrayPtr(const data::AronNDArrayPtr& aron);
+ static data::AronNDArrayPtr ToAronNDArrayPtr(const PointerType& navigator);
/// Return dimensions in a readable string such as "(2, 3, 4)".
static std::string DimensionsToString(const std::vector<int>& dimensions);
// public member functions
+ DictNavigatorPtr getAsDict() const;
+
unsigned char* getData() const;
void setData(unsigned int, const unsigned char*);
@@ -82,15 +88,11 @@ namespace armarx::aron::datanavigator
data::AronNDArrayPtr toAronNDArrayPtr() const;
// virtual implementations
- virtual data::AronDataPtr getResult() const override;
virtual std::string getName() const override;
virtual std::vector<NavigatorPtr> getChildren() const override;
virtual size_t childrenSize() const override;
virtual typenavigator::NavigatorPtr recalculateType() const override;
virtual bool fullfillsType(const typenavigator::NavigatorPtr&) const override;
-
- private:
- data::AronNDArrayPtr aron;
};
}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/data/container/Dict.cpp b/source/RobotAPI/libraries/aron/core/navigator/data/container/Dict.cpp
index 67a2a6d816823611376158d2862c0b5918d6d693..db8b7a02f37b948c0030cac5090281b10f400aec 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/data/container/Dict.cpp
+++ b/source/RobotAPI/libraries/aron/core/navigator/data/container/Dict.cpp
@@ -29,25 +29,22 @@
#include <RobotAPI/libraries/aron/core/navigator/type/container/Dict.h>
#include <RobotAPI/libraries/aron/core/navigator/type/container/Object.h>
+#include <SimoxUtility/algorithm/string/string_conversion.h>
+
namespace armarx::aron::datanavigator
{
// constructors
DictNavigator::DictNavigator(const Path& path) :
- aron::Navigator<data::Descriptor, data::AronData>::Navigator(data::Descriptor::eDict, path),
- Navigator(data::Descriptor::eDict, path),
- aron(new data::AronDict())
+ aron::Navigator<data::Descriptor, data::AronData>::Navigator(data::Descriptor::eDict, path)
{
}
DictNavigator::DictNavigator(const data::AronDictPtr& o, const Path& path) :
aron::Navigator<data::Descriptor, data::AronData>::Navigator(data::Descriptor::eDict, path),
- Navigator(data::Descriptor::eDict, path),
- aron(o)
+ detail::NavigatorBase<data::AronDict, DictNavigator>(o)
{
- CheckAronPtrForNull("DictNavigator", "DictNavigator", getPath(), aron);
-
- for (const auto& [key, dataPtr] : aron->elements)
+ for (const auto& [key, dataPtr] : this->aron->elements)
{
childrenNavigators[key] = Navigator::FromAronData(dataPtr, Path(path, key));
}
@@ -76,27 +73,20 @@ namespace armarx::aron::datanavigator
{
return false;
}
- if (not(nav->equalsDataNavigator(other.getElement(key))))
+ if (!nav)
+ {
+ return !((bool) other.getElement(key));
+ }
+ if (not(*nav == *other.getElement(key)))
{
return false;
}
}
return true;
}
-
- bool DictNavigator::equalsDataNavigator(const NavigatorPtr& other) const
- {
- if (other == nullptr)
- {
- return false;
- }
- DictNavigatorPtr casted = DynamicCast(other);
- return equalsDeep(casted);
- }
-
- bool DictNavigator::equalsDeep(const DictNavigatorPtr& other) const
+ bool DictNavigator::operator==(const DictNavigatorPtr& other) const
{
- if (other == nullptr)
+ if (!other)
{
return false;
}
@@ -104,24 +94,6 @@ namespace armarx::aron::datanavigator
}
// static methods
- DictNavigatorPtr DictNavigator::DynamicCast(const NavigatorPtr& n)
- {
- return std::dynamic_pointer_cast<DictNavigator>(n);
- }
-
- DictNavigator DictNavigator::DynamicCast(Navigator& n)
- {
- return dynamic_cast<DictNavigator&>(n);
- }
-
- DictNavigatorPtr DictNavigator::DynamicCastAndCheck(const NavigatorPtr& n)
- {
- CheckDataNavigatorPtrForNull("DictNavigator", "DynamicCastAndCheck[Before]", n);
- DictNavigatorPtr casted = DictNavigator::DynamicCast(n);
- CheckDataNavigatorPtrForNull("DictNavigator", "DynamicCastAndCheck[After]", n->getPath(), casted);
- return casted;
- }
-
DictNavigatorPtr DictNavigator::FromAronDictPtr(const data::AronDictPtr& aron)
{
return std::make_shared<DictNavigator>(aron);
@@ -148,10 +120,23 @@ namespace armarx::aron::datanavigator
return ret;
}
+ std::string DictNavigator::getAllKeysAsString() const
+ {
+ return simox::alg::to_string(getAllKeys(), ", ");
+ }
+
void DictNavigator::addElement(const std::string& key, const NavigatorPtr& data)
{
- this->childrenNavigators[key] = data;
- this->aron->elements[key] = data->getResult();
+ if (data)
+ {
+ this->childrenNavigators[key] = data;
+ this->aron->elements[key] = data->toAronDataPtr();
+ }
+ else
+ {
+ this->childrenNavigators[key] = nullptr;
+ this->aron->elements[key] = nullptr;
+ }
}
bool DictNavigator::hasElement(const std::string& key) const
@@ -186,21 +171,22 @@ namespace armarx::aron::datanavigator
}
// virtual implementations
- data::AronDataPtr DictNavigator::getResult() const
- {
- return toAronDictPtr();
- }
-
std::string DictNavigator::getName() const
{
return "AronDict";
}
+ // TODO
typenavigator::NavigatorPtr DictNavigator::recalculateType() const
{
typenavigator::DictNavigatorPtr typenav = typenavigator::DictNavigatorPtr(new typenavigator::DictNavigator(getPath()));
for (const auto& [key, nav] : childrenNavigators)
{
+ if (!nav)
+ {
+ continue;
+ }
+
if (typenav->getAcceptedType() == nullptr)
{
typenav->setAcceptedType(nav->recalculateType());
@@ -230,6 +216,10 @@ namespace armarx::aron::datanavigator
typenavigator::ObjectNavigatorPtr objectTypeNav = typenavigator::ObjectNavigator::DynamicCast(type);
for (const auto& [key, nav] : childrenNavigators)
{
+ if (!nav && !objectTypeNav->getMemberType(key))
+ {
+ return false;
+ }
if (!nav->fullfillsType(objectTypeNav->getMemberType(key)))
{
return false;
@@ -242,6 +232,10 @@ namespace armarx::aron::datanavigator
typenavigator::DictNavigatorPtr dictTypeNav = typenavigator::DictNavigator::DynamicCast(type);
for (const auto& [_, nav] : childrenNavigators)
{
+ if (!nav && !dictTypeNav->getAcceptedType())
+ {
+ return false;
+ }
if (!nav->fullfillsType(dictTypeNav->getAcceptedType()))
{
return false;
@@ -288,6 +282,10 @@ namespace armarx::aron::datanavigator
else
{
Path next = path.withDetachedFirstElement();
+ if (!childrenNavigators.at(el))
+ {
+ throw error::AronException("DictNavigator", "navigateAbsolute", "Could not navigate into a NULL member. Seems like the member is optional and not set.", next);
+ }
return childrenNavigators.at(el)->navigateAbsolute(next);
}
}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/data/container/Dict.h b/source/RobotAPI/libraries/aron/core/navigator/data/container/Dict.h
index 98c07794a3a7cb7423ced2559ce91887bd2c2637..2ea343434d5d5c7b4ef77fce7b33525ceb2cd397 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/data/container/Dict.h
+++ b/source/RobotAPI/libraries/aron/core/navigator/data/container/Dict.h
@@ -27,9 +27,11 @@
#include <memory>
#include <map>
+// Base class
+#include "../detail/NavigatorBase.h"
+
// ArmarX
-#include <RobotAPI/libraries/aron/core/navigator/data/Navigator.h>
-#include <RobotAPI/libraries/aron/core/navigator/type/container/Dict.h>
+#include "../../type/container/Dict.h"
namespace armarx::aron::datanavigator
{
@@ -37,11 +39,8 @@ namespace armarx::aron::datanavigator
typedef std::shared_ptr<DictNavigator> DictNavigatorPtr;
class DictNavigator :
- virtual public Navigator
+ virtual public detail::NavigatorBase<data::AronDict, DictNavigator>
{
- public:
- using PointerType = DictNavigatorPtr;
-
public:
// constructors
DictNavigator(const Path& path = Path());
@@ -50,21 +49,16 @@ namespace armarx::aron::datanavigator
DictNavigator(const std::map<std::string, NavigatorPtr>&, const Path& path = Path());
// operators
- bool operator==(const DictNavigator&) const;
- virtual bool equalsDataNavigator(const NavigatorPtr&) const override;
- bool equalsDeep(const DictNavigatorPtr&) const;
-
- // static methods
- static DictNavigatorPtr DynamicCast(const NavigatorPtr& n);
- static DictNavigator DynamicCast(Navigator& n);
- static DictNavigatorPtr DynamicCastAndCheck(const NavigatorPtr& n);
+ virtual bool operator==(const DictNavigator&) const override;
+ bool operator==(const DictNavigatorPtr&) const override;
- static DictNavigatorPtr FromAronDictPtr(const data::AronDictPtr& aron);
- static data::AronDictPtr ToAronDictPtr(const DictNavigatorPtr& navigator);
+ static PointerType FromAronDictPtr(const data::AronDictPtr& aron);
+ static data::AronDictPtr ToAronDictPtr(const PointerType& navigator);
// public member functions
data::AronDictPtr toAronDictPtr() const;
std::vector<std::string> getAllKeys() const;
+ std::string getAllKeysAsString() const;
void addElement(const std::string& key, const NavigatorPtr&);
bool hasElement(const std::string&) const;
@@ -74,7 +68,6 @@ namespace armarx::aron::datanavigator
void clear();
// virtual implementations
- virtual data::AronDataPtr getResult() const override;
virtual std::string getName() const override;
virtual std::vector<NavigatorPtr> getChildren() const override;
virtual size_t childrenSize() const override;
@@ -88,6 +81,5 @@ namespace armarx::aron::datanavigator
private:
// members
std::map<std::string, NavigatorPtr> childrenNavigators;
- data::AronDictPtr aron;
};
}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/data/container/List.cpp b/source/RobotAPI/libraries/aron/core/navigator/data/container/List.cpp
index d2d5bee4af83060dad86c93498be761f42a84f80..44f5c61eca8325e2175edbd0245085005dbfdb50 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/data/container/List.cpp
+++ b/source/RobotAPI/libraries/aron/core/navigator/data/container/List.cpp
@@ -35,19 +35,14 @@ namespace armarx::aron::datanavigator
{
// constructors
ListNavigator::ListNavigator(const Path& path) :
- aron::Navigator<data::Descriptor, data::AronData>::Navigator(data::Descriptor::eList, path),
- Navigator(data::Descriptor::eList, path),
- aron(new data::AronList())
+ aron::Navigator<data::Descriptor, data::AronData>::Navigator(data::Descriptor::eList, path)
{
}
ListNavigator::ListNavigator(const data::AronListPtr& l, const Path& path) :
aron::Navigator<data::Descriptor, data::AronData>::Navigator(data::Descriptor::eList, path),
- Navigator(data::Descriptor::eList, path),
- aron(l)
+ detail::NavigatorBase<data::AronList, ListNavigator>(l)
{
- CheckAronPtrForNull("ListNavigator", "ListNavigator", getPath(), aron);
-
unsigned int i = 0;
for (const auto& dataPtr : l->elements)
{
@@ -75,32 +70,28 @@ namespace armarx::aron::datanavigator
unsigned int i = 0;
for (const auto& nav : childrenNavigators)
{
- if (not(other.hasElement(i)))
+ if (!other.hasElement(i))
{
return false;
}
- if (not(nav->equalsDataNavigator(other.getElement(i))))
+ if (!nav)
+ {
+ if (!other.getElement(i))
+ {
+ return false;
+ }
+ }
+ if (!(*nav == other.getElement(i)))
{
return false;
}
- i++;
+ ++i;
}
return true;
}
-
- bool ListNavigator::equalsDataNavigator(const NavigatorPtr& other) const
+ bool ListNavigator::operator==(const ListNavigatorPtr& other) const
{
- if (other == nullptr)
- {
- return false;
- }
- ListNavigatorPtr casted = DynamicCast(other);
- return equalsDeep(casted);
- }
-
- bool ListNavigator::equalsDeep(const ListNavigatorPtr& other) const
- {
- if (other == nullptr)
+ if (!other)
{
return false;
}
@@ -108,25 +99,6 @@ namespace armarx::aron::datanavigator
}
// static methods
- ListNavigatorPtr ListNavigator::DynamicCast(const NavigatorPtr& n)
- {
- ListNavigatorPtr casted = std::dynamic_pointer_cast<ListNavigator>(n);
- return casted;
- }
-
- ListNavigator ListNavigator::DynamicCast(Navigator& n)
- {
- return dynamic_cast<ListNavigator&>(n);
- }
-
- ListNavigatorPtr ListNavigator::DynamicCastAndCheck(const NavigatorPtr& n)
- {
- CheckDataNavigatorPtrForNull("ListNavigator", "DynamicCastAndCheck[Before]", n);
- ListNavigatorPtr casted = ListNavigator::DynamicCast(n);
- CheckDataNavigatorPtrForNull("ListNavigator", "DynamicCastAndCheck[After]", n->getPath(), casted);
- return casted;
- }
-
ListNavigatorPtr ListNavigator::FromAronListPtr(const data::AronListPtr& aron)
{
return std::make_shared<ListNavigator>(aron);
@@ -138,10 +110,26 @@ namespace armarx::aron::datanavigator
}
// public member functions
+ DictNavigatorPtr ListNavigator::getAsDict() const
+ {
+ auto dict = std::make_shared<DictNavigator>();
+ auto copy_this = FromAronData(toAronDataPtr(), getPath());
+ dict->addElement("data", copy_this);
+ return dict;
+ }
+
void ListNavigator::addElement(const NavigatorPtr& n)
{
- childrenNavigators.push_back(n);
- aron->elements.push_back(n->getResult());
+ if (n)
+ {
+ childrenNavigators.push_back(n);
+ aron->elements.push_back(n->toAronDataPtr());
+ }
+ else
+ {
+ childrenNavigators.push_back(nullptr);
+ aron->elements.push_back(nullptr);
+ }
}
bool ListNavigator::hasElement(unsigned int i) const
@@ -175,16 +163,12 @@ namespace armarx::aron::datanavigator
}
// virtual implementations
- data::AronDataPtr ListNavigator::getResult() const
- {
- return toAronListPtr();
- }
-
std::string ListNavigator::getName() const
{
return "AronList";
}
+ // TODO
typenavigator::NavigatorPtr ListNavigator::recalculateType() const
{
typenavigator::ListNavigatorPtr typenav = typenavigator::ListNavigatorPtr(new typenavigator::ListNavigator(getPath()));
@@ -221,6 +205,10 @@ namespace armarx::aron::datanavigator
typenavigator::ListNavigatorPtr listTypeNav = typenavigator::ListNavigator::DynamicCast(type);
for (const auto& nav : childrenNavigators)
{
+ if (!nav && !listTypeNav->getAcceptedType())
+ {
+ return false;
+ }
if (!nav->fullfillsType(listTypeNav->getAcceptedType()))
{
return false;
@@ -234,6 +222,10 @@ namespace armarx::aron::datanavigator
unsigned int i = 0;
for (const auto& nav : childrenNavigators)
{
+ if (!nav && !tupleTypeNav->getAcceptedType(i))
+ {
+ return false;
+ }
if (!nav->fullfillsType(tupleTypeNav->getAcceptedType(i)))
{
return false;
@@ -248,6 +240,14 @@ namespace armarx::aron::datanavigator
{
return false;
}
+ if (!childrenNavigators[0] && !pairTypeNav->getFirstAcceptedType())
+ {
+ return false;
+ }
+ if (!childrenNavigators[1] && !pairTypeNav->getSecondAcceptedType())
+ {
+ return false;
+ }
return childrenNavigators[0]->fullfillsType(pairTypeNav->getFirstAcceptedType()) && childrenNavigators[1]->fullfillsType(pairTypeNav->getSecondAcceptedType());
}
default:
@@ -284,6 +284,10 @@ namespace armarx::aron::datanavigator
else
{
Path next = path.withDetachedFirstElement();
+ if (!childrenNavigators.at(i))
+ {
+ throw error::AronException("ListNavigator", "navigateAbsolute", "Could not navigate into a NULL member. Seems like the member is optional and not set.", next);
+ }
return childrenNavigators.at(i)->navigateAbsolute(next);
}
}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/data/container/List.h b/source/RobotAPI/libraries/aron/core/navigator/data/container/List.h
index 63b2f03059d6fde007fbe84b690561b75e7d0d23..8df55ff032ba6a48b9651b011718f713484c26b2 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/data/container/List.h
+++ b/source/RobotAPI/libraries/aron/core/navigator/data/container/List.h
@@ -26,10 +26,12 @@
// STD/STL
#include <memory>
+// Base class
+#include "../detail/NavigatorBase.h"
+
// ArmarX
-#include <RobotAPI/interface/aron.h>
-#include <RobotAPI/libraries/aron/core/navigator/data/Navigator.h>
-#include <RobotAPI/libraries/aron/core/navigator/type/container/List.h>
+#include "Dict.h"
+#include "../../type/container/List.h"
namespace armarx::aron::datanavigator
{
@@ -37,11 +39,8 @@ namespace armarx::aron::datanavigator
typedef std::shared_ptr<ListNavigator> ListNavigatorPtr;
class ListNavigator :
- virtual public Navigator
+ virtual public detail::NavigatorBase<data::AronList, ListNavigator>
{
- public:
- using PointerType = ListNavigatorPtr;
-
public:
// constructors
ListNavigator(const Path& path = Path());
@@ -50,19 +49,16 @@ namespace armarx::aron::datanavigator
ListNavigator(const std::vector<NavigatorPtr>&, const Path& path = Path());
// operators
- bool operator==(const ListNavigator&) const;
- virtual bool equalsDataNavigator(const NavigatorPtr&) const override;
- bool equalsDeep(const ListNavigatorPtr&) const;
+ virtual bool operator==(const ListNavigator&) const override;
+ bool operator==(const ListNavigatorPtr&) const override;
// static methods
- static ListNavigatorPtr DynamicCast(const NavigatorPtr& n);
- static ListNavigator DynamicCast(Navigator& n);
- static ListNavigatorPtr DynamicCastAndCheck(const NavigatorPtr& n);
-
- static ListNavigatorPtr FromAronListPtr(const data::AronListPtr& aron);
- static data::AronListPtr ToAronListPtr(const ListNavigatorPtr& navigator);
+ static PointerType FromAronListPtr(const data::AronListPtr& aron);
+ static data::AronListPtr ToAronListPtr(const PointerType& navigator);
// public member functions
+ DictNavigatorPtr getAsDict() const;
+
data::AronListPtr toAronListPtr() const;
void addElement(const NavigatorPtr&);
@@ -73,7 +69,6 @@ namespace armarx::aron::datanavigator
void clear();
// virtual implementations
- virtual data::AronDataPtr getResult() const override;
virtual std::string getName() const override;
virtual std::vector<NavigatorPtr> getChildren() const override;
virtual size_t childrenSize() const override;
@@ -86,6 +81,5 @@ namespace armarx::aron::datanavigator
private:
std::vector<NavigatorPtr> childrenNavigators;
- data::AronListPtr aron;
};
}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/data/detail/NavigatorBase.cpp b/source/RobotAPI/libraries/aron/core/navigator/data/detail/NavigatorBase.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..a5e8c5aa05314a73d33e01c14d01a38976ba88a3
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/navigator/data/detail/NavigatorBase.cpp
@@ -0,0 +1,25 @@
+/*
+ * 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 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
+ */
+
+// Header
+#include "NavigatorBase.h"
diff --git a/source/RobotAPI/libraries/aron/core/navigator/data/detail/NavigatorBase.h b/source/RobotAPI/libraries/aron/core/navigator/data/detail/NavigatorBase.h
new file mode 100644
index 0000000000000000000000000000000000000000..0c455a3eec808c7b995cb5a611e81139aa7008cc
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/navigator/data/detail/NavigatorBase.h
@@ -0,0 +1,115 @@
+/*
+ * 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 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
+ */
+
+#pragma once
+
+// STD/STL
+#include <memory>
+#include <string>
+#include <unordered_map>
+
+// Base class
+#include "../Navigator.h"
+
+// ArmarX
+
+namespace armarx::aron::datanavigator::detail
+{
+ template<typename AronDataT, typename DerivedT>
+ class NavigatorBase :
+ virtual public datanavigator::Navigator
+ {
+ public:
+ using PointerType = std::shared_ptr<DerivedT>;
+ using AronDataType = AronDataT;
+
+ public:
+ NavigatorBase() :
+ aron(new AronDataType())
+ {
+ }
+
+ NavigatorBase(const typename AronDataType::PointerType& o) :
+ aron(o)
+ {
+ ARMARX_CHECK_NOT_NULL(aron);
+ }
+
+ // operators
+ operator typename AronDataType::PointerType()
+ {
+ return aron;
+ }
+
+ virtual bool operator==(const Navigator& other) const override
+ {
+ const auto& n = DerivedT::DynamicCast(other);
+ return *this == n;
+ }
+ virtual bool operator==(const NavigatorPtr& other) const override
+ {
+ if (!other)
+ {
+ return false;
+ }
+ return *this == *other;
+ }
+
+ virtual bool operator==(const DerivedT&) const = 0;
+ virtual bool operator==(const PointerType& other) const = 0;
+
+ // virtual implementations
+ virtual data::AronDataPtr toAronPtr() const override
+ {
+ ARMARX_CHECK_NOT_NULL(aron);
+ return aron;
+ }
+
+ // static methods
+ static PointerType DynamicCast(const NavigatorPtr& n)
+ {
+ return std::dynamic_pointer_cast<DerivedT>(n);
+ }
+
+ static DerivedT& DynamicCast(Navigator& n)
+ {
+ return dynamic_cast<DerivedT&>(n);
+ }
+
+ static const DerivedT& DynamicCast(const Navigator& n)
+ {
+ return dynamic_cast<const DerivedT&>(n);
+ }
+
+ static PointerType DynamicCastAndCheck(const NavigatorPtr& n)
+ {
+ ARMARX_CHECK_NOT_NULL(n);
+ auto casted = DerivedT::DynamicCast(n);
+ ARMARX_CHECK_NOT_NULL(casted);
+ return casted;
+ }
+
+ protected:
+ typename AronDataType::PointerType aron;
+ };
+}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/data/detail/PrimitiveNavigatorBase.cpp b/source/RobotAPI/libraries/aron/core/navigator/data/detail/PrimitiveNavigatorBase.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..5068d7dce5fce2a05fa8cd10684ff969de3353fe
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/navigator/data/detail/PrimitiveNavigatorBase.cpp
@@ -0,0 +1,25 @@
+/*
+ * 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 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
+ */
+
+// Header
+#include "PrimitiveNavigatorBase.h"
diff --git a/source/RobotAPI/libraries/aron/core/navigator/data/detail/PrimitiveNavigatorBase.h b/source/RobotAPI/libraries/aron/core/navigator/data/detail/PrimitiveNavigatorBase.h
new file mode 100644
index 0000000000000000000000000000000000000000..4e91d3235c4f4234d7a9e3db943571a3dfb438fb
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/navigator/data/detail/PrimitiveNavigatorBase.h
@@ -0,0 +1,101 @@
+/*
+ * 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 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
+ */
+
+#pragma once
+
+// STD/STL
+#include <memory>
+#include <string>
+#include <unordered_map>
+
+// Base class
+#include "NavigatorBase.h"
+
+// ArmarX
+#include "../container/Dict.h"
+
+namespace armarx::aron::datanavigator::detail
+{
+ template<typename AronDataT, typename ValueT, typename DerivedT>
+ class PrimitiveNavigatorBase :
+ virtual public NavigatorBase<AronDataT, DerivedT>
+ {
+ public:
+ using ValueType = ValueT;
+
+ public:
+ PrimitiveNavigatorBase() {}
+
+ PrimitiveNavigatorBase(const ValueT& v)
+ {
+ this->aron->value = v;
+ }
+
+ operator ValueT() const
+ {
+ return this->aron->value;
+ }
+
+ DerivedT& operator=(const ValueT& x)
+ {
+ this->aron->value = x;
+ return *this;
+ }
+
+ // Already implemented through thge constructor of a primitive navigator
+ /*bool operator==(const ValueT& x) const
+ {
+ return this->aron->value == x;
+ }*/
+
+ // virtual implementations
+ virtual std::vector<NavigatorPtr> getChildren() const override
+ {
+ return {};
+ }
+ virtual size_t childrenSize() const override
+ {
+ return 0;
+ }
+
+ // static methods
+
+ /* public member functions */
+ DictNavigatorPtr getAsDict() const
+ {
+ auto dict = std::make_shared<DictNavigator>();
+ auto copy_this = FromAronData(this->toAronDataPtr(), this->getPath());
+ dict->addElement("data", copy_this);
+ return dict;
+ }
+
+ void setValue(const ValueT& x)
+ {
+ this->aron->value = x;
+ }
+ ValueT getValue() const
+ {
+ return this->aron->value;
+ }
+ };
+}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/data/primitive/Bool.cpp b/source/RobotAPI/libraries/aron/core/navigator/data/primitive/Bool.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..091343a774a134a760ab9c579aefa2edc6304113
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/navigator/data/primitive/Bool.cpp
@@ -0,0 +1,97 @@
+/*
+ * 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 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
+ */
+
+#include "Bool.h"
+
+namespace armarx::aron::datanavigator
+{
+ /* constructors */
+ BoolNavigator::BoolNavigator(const data::AronBoolPtr& o, const Path& path) :
+ aron::Navigator<data::Descriptor, data::AronData>::Navigator(data::Descriptor::eBool, path),
+ detail::NavigatorBase<data::AronBool, BoolNavigator>(o)
+ {
+ }
+
+ BoolNavigator::BoolNavigator(const Path& path) :
+ aron::Navigator<data::Descriptor, data::AronData>::Navigator(data::Descriptor::eBool, path)
+ {
+ }
+
+ BoolNavigator::BoolNavigator(const bool d, const Path& path) :
+ BoolNavigator(data::AronBoolPtr(new data::AronBool(d)), path)
+ {
+ }
+
+ /* operators */
+ bool BoolNavigator::operator==(const BoolNavigator& other) const
+ {
+ const auto& otherAron = other.toAronBoolPtr();
+ if (aron->value != otherAron->value)
+ {
+ return false;
+ }
+ return true;
+ }
+ bool BoolNavigator::operator==(const BoolNavigatorPtr& other) const
+ {
+ if (!other)
+ {
+ return false;
+ }
+ return *this == *other;
+ }
+
+
+ /* static methods */
+ BoolNavigatorPtr BoolNavigator::FromAronBoolPtr(const data::AronBoolPtr& aron)
+ {
+ return std::make_shared<BoolNavigator>(aron);
+ }
+
+ data::AronBoolPtr BoolNavigator::ToAronBoolPtr(const BoolNavigatorPtr& navigator)
+ {
+ return navigator ? navigator->toAronBoolPtr() : nullptr;
+ }
+
+ /* public member functions */
+ data::AronBoolPtr BoolNavigator::toAronBoolPtr() const
+ {
+ return aron;
+ }
+
+ /* virtual implementations */
+ std::string BoolNavigator::getName() const
+ {
+ return "data::AronBool";
+ }
+
+ bool BoolNavigator::fullfillsType(const typenavigator::NavigatorPtr& type) const
+ {
+ return type->getDescriptor() == type::Descriptor::eBool;
+ }
+
+ typenavigator::NavigatorPtr BoolNavigator::recalculateType() const
+ {
+ return std::make_shared<typenavigator::BoolNavigator>(getPath());
+ }
+}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/data/primitive/Bool.h b/source/RobotAPI/libraries/aron/core/navigator/data/primitive/Bool.h
new file mode 100644
index 0000000000000000000000000000000000000000..b95f1a86e3b84d517807b4b0d25edd7438c6ed12
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/navigator/data/primitive/Bool.h
@@ -0,0 +1,67 @@
+/*
+ * 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 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
+ */
+
+#pragma once
+
+// STD/STL
+#include <memory>
+
+// Base class
+#include "../detail/PrimitiveNavigatorBase.h"
+
+// ArmarX
+#include "../../type/primitive/Bool.h"
+
+namespace armarx::aron::datanavigator
+{
+
+ class BoolNavigator;
+ typedef std::shared_ptr<BoolNavigator> BoolNavigatorPtr;
+
+ class BoolNavigator :
+ virtual public detail::PrimitiveNavigatorBase<data::AronBool, bool, BoolNavigator>
+ {
+ public:
+ /* constructors */
+ BoolNavigator(const Path& = Path());
+ BoolNavigator(const data::AronBoolPtr&, const Path& = Path());
+ BoolNavigator(const bool, const Path& = Path());
+
+ // operators
+ bool operator==(const BoolNavigator& other) const override;
+ bool operator==(const BoolNavigatorPtr&) const override;
+
+ // static methods
+ static BoolNavigatorPtr FromAronBoolPtr(const data::AronBoolPtr& aron);
+ static data::AronBoolPtr ToAronBoolPtr(const BoolNavigatorPtr& navigator);
+
+ // class methods
+ data::AronBoolPtr toAronBoolPtr() const;
+
+ /* virtual implementations */
+ virtual std::string getName() const override;
+
+ virtual typenavigator::NavigatorPtr recalculateType() const override;
+ virtual bool fullfillsType(const typenavigator::NavigatorPtr&) const override;
+ };
+}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/data/primitive/Double.cpp b/source/RobotAPI/libraries/aron/core/navigator/data/primitive/Double.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..352e7abb14aef60126cb82ff7781afd8f427f72c
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/navigator/data/primitive/Double.cpp
@@ -0,0 +1,102 @@
+/*
+ * 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 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
+ */
+
+// Header
+#include "Double.h"
+
+// ArmarX
+#include "Float.h"
+
+namespace armarx::aron::datanavigator
+{
+ /* constructors */
+ DoubleNavigator::DoubleNavigator(const data::AronDoublePtr& o, const Path& path) :
+ aron::Navigator<data::Descriptor, data::AronData>::Navigator(data::Descriptor::eDouble, path),
+ detail::NavigatorBase<data::AronDouble, DoubleNavigator>(o)
+ {
+ }
+
+ DoubleNavigator::DoubleNavigator(const Path& path) :
+ aron::Navigator<data::Descriptor, data::AronData>::Navigator(data::Descriptor::eDouble, path)
+ {
+ }
+
+ DoubleNavigator::DoubleNavigator(const double d, const Path& path) :
+ DoubleNavigator(data::AronDoublePtr(new data::AronDouble(d)), path)
+ {
+ }
+
+ /* operators */
+ bool DoubleNavigator::operator==(const DoubleNavigator& other) const
+ {
+ const auto& otherAron = other.toAronDoublePtr();
+ if (this->aron->value != otherAron->value)
+ {
+ return false;
+ }
+ return true;
+ }
+ bool DoubleNavigator::operator==(const DoubleNavigatorPtr& other) const
+ {
+ if (!other)
+ {
+ return false;
+ }
+ return *this == *other;
+ }
+
+ /* static methods */
+ DoubleNavigatorPtr DoubleNavigator::FromAronDoublePtr(const data::AronDoublePtr& aron)
+ {
+ return std::make_shared<DoubleNavigator>(aron);
+ }
+
+ data::AronDoublePtr DoubleNavigator::ToAronDoublePtr(const DoubleNavigatorPtr& navigator)
+ {
+ return navigator ? navigator->toAronDoublePtr() : nullptr;
+ }
+
+
+ /* public member functions */
+ data::AronDoublePtr DoubleNavigator::toAronDoublePtr() const
+ {
+ return aron;
+ }
+
+ /* virtual implementations */
+ std::string DoubleNavigator::getName() const
+ {
+ return "data::AronDouble";
+ }
+
+ bool DoubleNavigator::fullfillsType(const typenavigator::NavigatorPtr& type) const
+ {
+ const auto f = FloatNavigator();
+ return type->getDescriptor() == type::Descriptor::eDouble || f.fullfillsType(type);
+ }
+
+ typenavigator::NavigatorPtr DoubleNavigator::recalculateType() const
+ {
+ return std::make_shared<typenavigator::DoubleNavigator>(getPath());
+ }
+}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/data/primitive/Double.h b/source/RobotAPI/libraries/aron/core/navigator/data/primitive/Double.h
new file mode 100644
index 0000000000000000000000000000000000000000..4996bc46bd479fe4318c9bb6bcf8228c453c65fc
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/navigator/data/primitive/Double.h
@@ -0,0 +1,67 @@
+/*
+ * 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 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
+ */
+
+#pragma once
+
+// STD/STL
+#include <memory>
+
+// Base class
+#include "../detail/PrimitiveNavigatorBase.h"
+
+// ArmarX
+#include "../../type/primitive/Double.h"
+
+namespace armarx::aron::datanavigator
+{
+
+ class DoubleNavigator;
+ typedef std::shared_ptr<DoubleNavigator> DoubleNavigatorPtr;
+
+ class DoubleNavigator :
+ virtual public detail::PrimitiveNavigatorBase<data::AronDouble, double, DoubleNavigator>
+ {
+ public:
+ /* constructors */
+ DoubleNavigator(const Path& = Path());
+ DoubleNavigator(const data::AronDoublePtr&, const Path& = Path());
+ DoubleNavigator(const double, const Path& = Path());
+
+ /* operators */
+ bool operator==(const DoubleNavigator&) const override;
+ bool operator==(const DoubleNavigatorPtr&) const override;
+
+ /* static methods */
+ static DoubleNavigatorPtr FromAronDoublePtr(const data::AronDoublePtr& aron);
+ static data::AronDoublePtr ToAronDoublePtr(const DoubleNavigatorPtr& navigator);
+
+ /* public member functions */
+ data::AronDoublePtr toAronDoublePtr() const;
+
+ /* virtual implementations */
+ virtual std::string getName() const override;
+
+ virtual typenavigator::NavigatorPtr recalculateType() const override;
+ virtual bool fullfillsType(const typenavigator::NavigatorPtr&) const override;
+ };
+}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/data/primitive/Float.cpp b/source/RobotAPI/libraries/aron/core/navigator/data/primitive/Float.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..145e96f9d2baa2292354556eac3fc67a2c54a192
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/navigator/data/primitive/Float.cpp
@@ -0,0 +1,98 @@
+/*
+ * 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 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
+ */
+
+#include "Float.h"
+
+namespace armarx::aron::datanavigator
+{
+ /* constructors */
+ FloatNavigator::FloatNavigator(const data::AronFloatPtr& o, const Path& path) :
+ aron::Navigator<data::Descriptor, data::AronData>::Navigator(data::Descriptor::eFloat, path),
+ detail::NavigatorBase<data::AronFloat, FloatNavigator>(o)
+ {
+ }
+
+ FloatNavigator::FloatNavigator(const Path& path) :
+ aron::Navigator<data::Descriptor, data::AronData>::Navigator(data::Descriptor::eFloat, path)
+ {
+ }
+
+ FloatNavigator::FloatNavigator(const float d, const Path& path) :
+ FloatNavigator(data::AronFloatPtr(new data::AronFloat(d)), path)
+ {
+ }
+
+ /* operators */
+ bool FloatNavigator::operator==(const FloatNavigator& other) const
+ {
+ const auto& otherAron = other.toAronFloatPtr();
+ if (this->aron->value != otherAron->value)
+ {
+ return false;
+ }
+ return true;
+ }
+ bool FloatNavigator::operator==(const FloatNavigatorPtr& other) const
+ {
+ if (!other)
+ {
+ return false;
+ }
+ return *this == *other;
+ }
+
+
+ /* static methods */
+ FloatNavigatorPtr FloatNavigator::FromAronFloatPtr(const data::AronFloatPtr& aron)
+ {
+ return std::make_shared<FloatNavigator>(aron);
+ }
+
+ data::AronFloatPtr FloatNavigator::ToAronFloatPtr(const FloatNavigatorPtr& navigator)
+ {
+ return navigator ? navigator->toAronFloatPtr() : nullptr;
+ }
+
+ /* public member functions */
+ data::AronFloatPtr FloatNavigator::toAronFloatPtr() const
+ {
+ return aron;
+ }
+
+
+ /* virtual implementations */
+ std::string FloatNavigator::getName() const
+ {
+ return "data::AronFloat";
+ }
+
+ bool FloatNavigator::fullfillsType(const typenavigator::NavigatorPtr& type) const
+ {
+ return type->getDescriptor() == type::Descriptor::eFloat;
+ }
+
+ typenavigator::NavigatorPtr FloatNavigator::recalculateType() const
+ {
+ return std::make_shared<typenavigator::FloatNavigator>(getPath());
+ }
+}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/data/primitive/Float.h b/source/RobotAPI/libraries/aron/core/navigator/data/primitive/Float.h
new file mode 100644
index 0000000000000000000000000000000000000000..b060f82d9606c14baf63c539450fc2beed1a6a89
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/navigator/data/primitive/Float.h
@@ -0,0 +1,67 @@
+/*
+ * 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 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
+ */
+
+#pragma once
+
+// STD/STL
+#include <memory>
+
+// Base class
+#include "../detail/PrimitiveNavigatorBase.h"
+
+// ArmarX
+#include "../../type/primitive/Float.h"
+
+namespace armarx::aron::datanavigator
+{
+
+ class FloatNavigator;
+ typedef std::shared_ptr<FloatNavigator> FloatNavigatorPtr;
+
+ class FloatNavigator :
+ virtual public detail::PrimitiveNavigatorBase<data::AronFloat, float, FloatNavigator>
+ {
+ public:
+ /* constructors */
+ FloatNavigator(const Path& = Path());
+ FloatNavigator(const data::AronFloatPtr&, const Path& = Path());
+ FloatNavigator(const float, const Path& = Path());
+
+ /* operators */
+ bool operator==(const FloatNavigator&) const override;
+ bool operator==(const FloatNavigatorPtr&) const override;
+
+ /* static methods */
+ static FloatNavigatorPtr FromAronFloatPtr(const data::AronFloatPtr& aron);
+ static data::AronFloatPtr ToAronFloatPtr(const FloatNavigatorPtr& navigator);
+
+ /* public member functions */
+ data::AronFloatPtr toAronFloatPtr() const;
+
+ /* virtual implementations */
+ virtual std::string getName() const override;
+
+ virtual typenavigator::NavigatorPtr recalculateType() const override;
+ virtual bool fullfillsType(const typenavigator::NavigatorPtr&) const override;
+ };
+}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/data/primitive/Int.cpp b/source/RobotAPI/libraries/aron/core/navigator/data/primitive/Int.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..29c706c12774e2aef91d962a66260db4dc5d7d21
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/navigator/data/primitive/Int.cpp
@@ -0,0 +1,101 @@
+/*
+ * 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 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
+ */
+
+#include "Int.h"
+
+#include <RobotAPI/libraries/aron/core/navigator/data/NavigatorFactory.h>
+#include <RobotAPI/libraries/aron/core/navigator/type/primitive/Int.h>
+
+namespace armarx::aron::datanavigator
+{
+ /* constructors */
+ IntNavigator::IntNavigator(const data::AronIntPtr& o, const Path& path) :
+ aron::Navigator<data::Descriptor, data::AronData>::Navigator(data::Descriptor::eInt, path),
+ detail::NavigatorBase<data::AronInt, IntNavigator>(o)
+ {
+ }
+
+ IntNavigator::IntNavigator(const Path& path) :
+ aron::Navigator<data::Descriptor, data::AronData>::Navigator(data::Descriptor::eInt, path)
+ {
+ }
+
+ IntNavigator::IntNavigator(const int d, const Path& path) :
+ IntNavigator(data::AronIntPtr(new data::AronInt(d)), path)
+ {
+ }
+
+ /* operators */
+ bool IntNavigator::operator==(const IntNavigator& other) const
+ {
+ const auto& otherAron = other.toAronIntPtr();
+ if (this->aron->value != otherAron->value)
+ {
+ return false;
+ }
+ return true;
+ }
+ bool IntNavigator::operator==(const IntNavigatorPtr& other) const
+ {
+ if (!other)
+ {
+ return false;
+ }
+ return *this == *other;
+ }
+
+
+ /* static methods */
+ IntNavigatorPtr IntNavigator::FromAronIntPtr(const data::AronIntPtr& aron)
+ {
+ return std::make_shared<IntNavigator>(aron);
+ }
+
+ data::AronIntPtr IntNavigator::ToAronIntPtr(const IntNavigatorPtr& navigator)
+ {
+ return navigator ? navigator->toAronIntPtr() : nullptr;
+ }
+
+ /* public member functions */
+ data::AronIntPtr IntNavigator::toAronIntPtr() const
+ {
+ return aron;
+ }
+
+
+ /* virtual implementations */
+ std::string IntNavigator::getName() const
+ {
+ return "data::AronInt";
+ }
+
+ bool IntNavigator::fullfillsType(const typenavigator::NavigatorPtr& type) const
+ {
+ return type->getDescriptor() == type::Descriptor::eInt || type->getDescriptor() == type::Descriptor::eIntEnum;
+ }
+
+ typenavigator::NavigatorPtr IntNavigator::recalculateType() const
+ {
+ return std::make_shared<typenavigator::IntNavigator>(getPath());
+ }
+}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/data/primitive/Int.h b/source/RobotAPI/libraries/aron/core/navigator/data/primitive/Int.h
new file mode 100644
index 0000000000000000000000000000000000000000..317650e847695cde2700867757c0407b1fc7d8a0
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/navigator/data/primitive/Int.h
@@ -0,0 +1,67 @@
+/*
+ * 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 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
+ */
+
+#pragma once
+
+// STD/STL
+#include <memory>
+
+// Base class
+#include "../detail/PrimitiveNavigatorBase.h"
+
+// ArmarX
+#include "../../type/primitive/Int.h"
+
+namespace armarx::aron::datanavigator
+{
+
+ class IntNavigator;
+ typedef std::shared_ptr<IntNavigator> IntNavigatorPtr;
+
+ class IntNavigator :
+ virtual public detail::PrimitiveNavigatorBase<data::AronInt, int, IntNavigator>
+ {
+ public:
+ /* constructors */
+ IntNavigator(const Path& = Path());
+ IntNavigator(const data::AronIntPtr&, const Path& = Path());
+ IntNavigator(const int, const Path& = Path());
+
+ /* operators */
+ bool operator==(const IntNavigator&) const override;
+ bool operator==(const IntNavigatorPtr&) const override;
+
+ /* static methods */
+ static IntNavigatorPtr FromAronIntPtr(const data::AronIntPtr& aron);
+ static data::AronIntPtr ToAronIntPtr(const IntNavigatorPtr& navigator);
+
+ /* public member functions */
+ data::AronIntPtr toAronIntPtr() const;
+
+ /* virtual implementations */
+ virtual std::string getName() const override;
+
+ virtual typenavigator::NavigatorPtr recalculateType() const override;
+ virtual bool fullfillsType(const typenavigator::NavigatorPtr&) const override;
+ };
+}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/data/primitive/Long.cpp b/source/RobotAPI/libraries/aron/core/navigator/data/primitive/Long.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..73676a1d51c21f9f7f37b2d3b92eaf429f20afe2
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/navigator/data/primitive/Long.cpp
@@ -0,0 +1,103 @@
+/*
+ * 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 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
+ */
+
+// Header
+#include "Long.h"
+
+// ArmarX
+#include "Int.h"
+
+namespace armarx::aron::datanavigator
+{
+ /* constructors */
+ LongNavigator::LongNavigator(const data::AronLongPtr& o, const Path& path) :
+ aron::Navigator<data::Descriptor, data::AronData>::Navigator(data::Descriptor::eLong, path),
+ detail::NavigatorBase<data::AronLong, LongNavigator>(o)
+ {
+ }
+
+ LongNavigator::LongNavigator(const Path& path) :
+ aron::Navigator<data::Descriptor, data::AronData>::Navigator(data::Descriptor::eLong, path)
+ {
+ }
+
+ LongNavigator::LongNavigator(const long d, const Path& path) :
+ LongNavigator(data::AronLongPtr(new data::AronLong(d)), path)
+ {
+ }
+
+ /* operators */
+ bool LongNavigator::operator==(const LongNavigator& other) const
+ {
+ const auto& otherAron = other.toAronLongPtr();
+ if (this->aron->value != otherAron->value)
+ {
+ return false;
+ }
+ return true;
+ }
+ bool LongNavigator::operator==(const LongNavigatorPtr& other) const
+ {
+ if (!other)
+ {
+ return false;
+ }
+ return *this == *other;
+ }
+
+
+ /* static methods */
+ LongNavigatorPtr LongNavigator::FromAronLongPtr(const data::AronLongPtr& aron)
+ {
+ return std::make_shared<LongNavigator>(aron);
+ }
+
+ data::AronLongPtr LongNavigator::ToAronLongPtr(const LongNavigatorPtr& navigator)
+ {
+ return navigator ? navigator->toAronLongPtr() : nullptr;
+ }
+
+ /* public member functions */
+ data::AronLongPtr LongNavigator::toAronLongPtr() const
+ {
+ return aron;
+ }
+
+
+ /* virtual implementations */
+ std::string LongNavigator::getName() const
+ {
+ return "data::AronLong";
+ }
+
+ bool LongNavigator::fullfillsType(const typenavigator::NavigatorPtr& type) const
+ {
+ const auto i = IntNavigator();
+ return type->getDescriptor() == type::Descriptor::eLong || type->getDescriptor() == type::Descriptor::eTime || i.fullfillsType(type);
+ }
+
+ typenavigator::NavigatorPtr LongNavigator::recalculateType() const
+ {
+ return std::make_shared<typenavigator::LongNavigator>(getPath());
+ }
+}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/data/primitive/Long.h b/source/RobotAPI/libraries/aron/core/navigator/data/primitive/Long.h
new file mode 100644
index 0000000000000000000000000000000000000000..f9593ec4059b9676dd56df76196418c82465f17c
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/navigator/data/primitive/Long.h
@@ -0,0 +1,67 @@
+/*
+ * 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 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
+ */
+
+#pragma once
+
+// STD/STL
+#include <memory>
+
+// Base class
+#include "../detail/PrimitiveNavigatorBase.h"
+
+// ArmarX
+#include "../../type/primitive/Long.h"
+
+namespace armarx::aron::datanavigator
+{
+
+ class LongNavigator;
+ typedef std::shared_ptr<LongNavigator> LongNavigatorPtr;
+
+ class LongNavigator :
+ virtual public detail::PrimitiveNavigatorBase<data::AronLong, long, LongNavigator>
+ {
+ public:
+ /* constructors */
+ LongNavigator(const Path& = Path());
+ LongNavigator(const data::AronLongPtr&, const Path& = Path());
+ LongNavigator(const long, const Path& = Path());
+
+ /* operators */
+ bool operator==(const LongNavigator&) const override;
+ bool operator==(const LongNavigatorPtr&) const override;
+
+ /* static methods */
+ static LongNavigatorPtr FromAronLongPtr(const data::AronLongPtr& aron);
+ static data::AronLongPtr ToAronLongPtr(const LongNavigatorPtr& navigator);
+
+ /* public member functions */
+ data::AronLongPtr toAronLongPtr() const;
+
+ /* virtual implementations */
+ virtual std::string getName() const override;
+
+ virtual typenavigator::NavigatorPtr recalculateType() const override;
+ virtual bool fullfillsType(const typenavigator::NavigatorPtr&) const override;
+ };
+}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/data/primitive/Primitive.cpp b/source/RobotAPI/libraries/aron/core/navigator/data/primitive/Primitive.cpp
deleted file mode 100644
index 23e117fc3f23ed1639faa0b310ba9ddd0f997e24..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/navigator/data/primitive/Primitive.cpp
+++ /dev/null
@@ -1,165 +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 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
- */
-
-#include "Primitive.h"
-
-#include <RobotAPI/libraries/aron/core/navigator/data/NavigatorFactory.h>
-#include <RobotAPI/libraries/aron/core/navigator/type/primitive/Primitive.h>
-
-namespace armarx::aron::datanavigator
-{
-#define RUN_ARON_MACRO(upperType, lowerType, capsType) \
- /* constructors */ \
- upperType##Navigator::upperType##Navigator(const data::Aron##upperType##Ptr& o, const Path& path) : \
- aron::Navigator<data::Descriptor, data::AronData>::Navigator(data::Descriptor::e##upperType, path), \
- Navigator(data::Descriptor::e##upperType, path), \
- aron(o) \
- { \
- } \
- \
- upperType##Navigator::upperType##Navigator(const Path& path) : \
- aron::Navigator<data::Descriptor, data::AronData>::Navigator(data::Descriptor::e##upperType, path), \
- Navigator(data::Descriptor::e##upperType, path), \
- aron(new data::Aron##upperType()) \
- { \
- } \
- \
- upperType##Navigator::upperType##Navigator(const lowerType& d, const Path& path) : \
- upperType##Navigator(data::Aron##upperType##Ptr(new data::Aron##upperType(d)), path) \
- { \
- } \
- \
- /* operators */ \
- upperType##Navigator::operator lowerType() const \
- { \
- return aron->value; \
- } \
- \
- bool upperType##Navigator::operator==(const upperType##Navigator& other) const \
- { \
- const auto& otherAron = other.toAron##upperType##Ptr(); \
- if(aron->value != otherAron->value) \
- return false; \
- return true; \
- } \
- \
- bool upperType##Navigator::equalsDataNavigator(const NavigatorPtr& other) const \
- { \
- if(other == nullptr) \
- { \
- return false; \
- } \
- upperType##NavigatorPtr casted = DynamicCast(other); \
- return equalsDeep(casted); \
- } \
- \
- bool upperType##Navigator::equalsDeep(const upperType##NavigatorPtr& other) const \
- { \
- /* only for consistency. There is no "deep" comparison. */ \
- if (other == nullptr) \
- { \
- return false; \
- } \
- return *this == *other; \
- } \
- \
- /* static methods */ \
- upperType##NavigatorPtr upperType##Navigator::DynamicCast(const NavigatorPtr& n) \
- { \
- upperType##NavigatorPtr casted = std::dynamic_pointer_cast<upperType##Navigator>(n); \
- return casted; \
- } \
- \
- upperType##Navigator upperType##Navigator::DynamicCast(Navigator& n) \
- { \
- return dynamic_cast<upperType##Navigator&>(n); \
- } \
- \
- upperType##NavigatorPtr upperType##Navigator::DynamicCastAndCheck(const NavigatorPtr& n) \
- { \
- CheckDataNavigatorPtrForNull("Aron"+std::string(#upperType)+"DataNavigator", "DynamicCastAndCheck[Before]", n); \
- upperType##NavigatorPtr casted = upperType##Navigator::DynamicCast(n); \
- CheckDataNavigatorPtrForNull("Aron"+std::string(#upperType)+"DataNavigator", "DynamicCastAndCheck[After]", n->getPath(), casted); \
- return casted; \
- } \
- \
- upperType##NavigatorPtr upperType##Navigator::FromAron##upperType##Ptr(const data::Aron##upperType##Ptr& aron) \
- { \
- return std::make_shared<upperType##Navigator>(aron); \
- } \
- \
- data::Aron##upperType##Ptr upperType##Navigator::ToAron##upperType##Ptr(const upperType##NavigatorPtr& navigator) \
- { \
- return navigator ? navigator->toAron##upperType##Ptr() : nullptr; \
- } \
- \
- /* public member functions */ \
- void upperType##Navigator::setValue(const lowerType& x) \
- { \
- aron->value = x; \
- } \
- \
- lowerType upperType##Navigator::getValue() const \
- { \
- return aron->value; \
- } \
- \
- data::Aron##upperType##Ptr upperType##Navigator::toAron##upperType##Ptr() const \
- { \
- return aron; \
- } \
- \
- /* virtual implementations */ \
- data::AronDataPtr upperType##Navigator::getResult() const \
- { \
- return aron; \
- } \
- \
- std::string upperType##Navigator::getName() const \
- { \
- return "Aron" + std::string(#upperType); \
- } \
- \
- bool upperType##Navigator::fullfillsType(const typenavigator::NavigatorPtr& type) const \
- { \
- return Resolver::Correspond(type->getDescriptor(), getDescriptor()); \
- } \
- \
- typenavigator::NavigatorPtr upperType##Navigator::recalculateType() const \
- { \
- return typenavigator::upperType##NavigatorPtr(new typenavigator::upperType##Navigator(getPath())); \
- } \
- \
- std::vector<NavigatorPtr> upperType##Navigator::getChildren() const \
- { \
- return {}; \
- } \
- \
- size_t upperType##Navigator::childrenSize() const \
- { \
- return 0; \
- } \
-
- HANDLE_PRIMITIVE_DATA
-#undef RUN_ARON_MACRO
-}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/data/primitive/Primitive.h b/source/RobotAPI/libraries/aron/core/navigator/data/primitive/Primitive.h
deleted file mode 100644
index 2891a277fd1e2d13f116bce302957bcc6042f746..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/navigator/data/primitive/Primitive.h
+++ /dev/null
@@ -1,92 +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 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
- */
-
-#pragma once
-
-// STD/STL
-#include <memory>
-
-// ArmarX
-#include <RobotAPI/interface/aron.h>
-#include <RobotAPI/libraries/aron/core/navigator/data/Navigator.h>
-
-namespace armarx::aron::datanavigator
-{
-
-#define RUN_ARON_MACRO(upperType, lowerType, capsType) \
- class upperType##Navigator; \
- typedef std::shared_ptr<upperType##Navigator> upperType##NavigatorPtr;
-
- HANDLE_PRIMITIVE_DATA
-#undef RUN_ARON_MACRO
-
-
-#define RUN_ARON_MACRO(upperType, lowerType, capsType) \
- class upperType##Navigator : \
- virtual public Navigator \
- { \
- public: \
- using PointerType = upperType##NavigatorPtr; \
- \
- /* constructors */ \
- upperType##Navigator(const Path& = Path()); \
- upperType##Navigator(const data::Aron##upperType##Ptr&, const Path& = Path()); \
- upperType##Navigator(const lowerType&, const Path& = Path()); \
- \
- /* operators */ \
- operator lowerType() const; \
- bool operator==(const upperType##Navigator&) const; \
- virtual bool equalsDataNavigator(const NavigatorPtr&) const override; \
- bool equalsDeep(const upperType##NavigatorPtr&) const; \
- \
- /* static methods */ \
- static upperType##NavigatorPtr DynamicCast(const NavigatorPtr& n); \
- static upperType##Navigator DynamicCast(Navigator& n); \
- static upperType##NavigatorPtr DynamicCastAndCheck(const NavigatorPtr& n); \
- \
- static upperType##NavigatorPtr FromAron##upperType##Ptr(const data::Aron##upperType##Ptr& aron); \
- static data::Aron##upperType##Ptr ToAron##upperType##Ptr(const upperType##NavigatorPtr& navigator); \
- \
- /* public member functions */ \
- void setValue(const lowerType& x); \
- lowerType getValue() const; \
- \
- data::Aron##upperType##Ptr toAron##upperType##Ptr() const; \
- \
- /* virtual implementations */ \
- virtual data::AronDataPtr getResult() const override; \
- virtual std::string getName() const override; \
- virtual std::vector<NavigatorPtr> getChildren() const override; \
- virtual size_t childrenSize() const override; \
- \
- virtual typenavigator::NavigatorPtr recalculateType() const override; \
- virtual bool fullfillsType(const typenavigator::NavigatorPtr&) const override; \
- \
- private: \
- /* members */ \
- data::Aron##upperType##Ptr aron; \
- };
-
- HANDLE_PRIMITIVE_DATA
-#undef RUN_ARON_MACRO
-}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/data/primitive/String.cpp b/source/RobotAPI/libraries/aron/core/navigator/data/primitive/String.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..11477863e1062e53575bfa5482f5e767a57ad2c6
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/navigator/data/primitive/String.cpp
@@ -0,0 +1,98 @@
+/*
+ * 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 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
+ */
+
+#include "String.h"
+
+namespace armarx::aron::datanavigator
+{
+ /* constructors */
+ StringNavigator::StringNavigator(const data::AronStringPtr& o, const Path& path) :
+ aron::Navigator<data::Descriptor, data::AronData>::Navigator(data::Descriptor::eString, path),
+ detail::NavigatorBase<data::AronString, StringNavigator>(o)
+ {
+ }
+
+ StringNavigator::StringNavigator(const Path& path) :
+ aron::Navigator<data::Descriptor, data::AronData>::Navigator(data::Descriptor::eString, path)
+ {
+ }
+
+ StringNavigator::StringNavigator(const std::string& d, const Path& path) :
+ StringNavigator(data::AronStringPtr(new data::AronString(d)), path)
+ {
+ }
+
+ /* operators */
+ bool StringNavigator::operator==(const StringNavigator& other) const
+ {
+ const auto& otherAron = other.toAronStringPtr();
+ if (this->aron->value != otherAron->value)
+ {
+ return false;
+ }
+ return true;
+ }
+ bool StringNavigator::operator==(const StringNavigatorPtr& other) const
+ {
+ if (!other)
+ {
+ return false;
+ }
+ return *this == *other;
+ }
+
+
+ /* static methods */
+ StringNavigatorPtr StringNavigator::FromAronStringPtr(const data::AronStringPtr& aron)
+ {
+ return std::make_shared<StringNavigator>(aron);
+ }
+
+ data::AronStringPtr StringNavigator::ToAronStringPtr(const StringNavigatorPtr& navigator)
+ {
+ return navigator ? navigator->toAronStringPtr() : nullptr;
+ }
+
+ /* public member functions */
+ data::AronStringPtr StringNavigator::toAronStringPtr() const
+ {
+ return aron;
+ }
+
+
+ /* virtual implementations */
+ std::string StringNavigator::getName() const
+ {
+ return "data::AronString";
+ }
+
+ bool StringNavigator::fullfillsType(const typenavigator::NavigatorPtr& type) const
+ {
+ return type->getDescriptor() == type::Descriptor::eString;
+ }
+
+ typenavigator::NavigatorPtr StringNavigator::recalculateType() const
+ {
+ return std::make_shared<typenavigator::StringNavigator>(getPath());
+ }
+}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/data/primitive/String.h b/source/RobotAPI/libraries/aron/core/navigator/data/primitive/String.h
new file mode 100644
index 0000000000000000000000000000000000000000..915334a9d8794245cb3c44c755ac5d3e39a7ddb5
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/navigator/data/primitive/String.h
@@ -0,0 +1,67 @@
+/*
+ * 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 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
+ */
+
+#pragma once
+
+// STD/STL
+#include <memory>
+
+// Base class
+#include "../detail/PrimitiveNavigatorBase.h"
+
+// ArmarX
+#include "../../type/primitive/String.h"
+
+namespace armarx::aron::datanavigator
+{
+
+ class StringNavigator;
+ typedef std::shared_ptr<StringNavigator> StringNavigatorPtr;
+
+ class StringNavigator :
+ virtual public detail::PrimitiveNavigatorBase<data::AronString, std::string, StringNavigator>
+ {
+ public:
+ /* constructors */
+ StringNavigator(const Path& = Path());
+ StringNavigator(const data::AronStringPtr&, const Path& = Path());
+ StringNavigator(const std::string&, const Path& = Path());
+
+ /* operators */
+ bool operator==(const StringNavigator&) const override;
+ bool operator==(const StringNavigatorPtr&) const override;
+
+ /* static methods */
+ static StringNavigatorPtr FromAronStringPtr(const data::AronStringPtr& aron);
+ static data::AronStringPtr ToAronStringPtr(const StringNavigatorPtr& navigator);
+
+ /* public member functions */
+ data::AronStringPtr toAronStringPtr() const;
+
+ /* virtual implementations */
+ virtual std::string getName() const override;
+
+ virtual typenavigator::NavigatorPtr recalculateType() const override;
+ virtual bool fullfillsType(const typenavigator::NavigatorPtr&) const override;
+ };
+}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/AllNavigators.h b/source/RobotAPI/libraries/aron/core/navigator/type/AllNavigators.h
index d1b5b03a26dd0ac3784223887c43fd54ecddcd7b..51ea5a762037da7b7e4a3808c42d36a468d9c6b1 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/type/AllNavigators.h
+++ b/source/RobotAPI/libraries/aron/core/navigator/type/AllNavigators.h
@@ -5,6 +5,7 @@
#include "container/Object.h"
#include "container/Pair.h"
#include "container/Tuple.h"
+#include "ndarray/NDArray.h"
#include "ndarray/EigenMatrix.h"
#include "ndarray/EigenQuaternion.h"
#include "ndarray/IVTCByteImage.h"
@@ -15,4 +16,10 @@
#include "ndarray/Pose.h"
#include "ndarray/Position.h"
#include "enum/IntEnum.h"
-#include "primitive/Primitive.h"
+#include "primitive/Int.h"
+#include "primitive/Long.h"
+#include "primitive/Float.h"
+#include "primitive/Double.h"
+#include "primitive/String.h"
+#include "primitive/Bool.h"
+#include "primitive/Time.h"
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/Navigator.cpp b/source/RobotAPI/libraries/aron/core/navigator/type/Navigator.cpp
index 1e38841d2647cd986a521ed2e92166dae7f1929e..2e3eb1e69cb967cb833ef78bee55d6e2fbaf527b 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/type/Navigator.cpp
+++ b/source/RobotAPI/libraries/aron/core/navigator/type/Navigator.cpp
@@ -36,105 +36,10 @@ namespace armarx::aron::typenavigator
// static data members
const NavigatorFactoryPtr Navigator::FACTORY = NavigatorFactoryPtr(new NavigatorFactory());
- // constructors
- Navigator::Navigator(const type::Descriptor& d, const Path& path) :
- aron::Navigator<type::Descriptor, type::AronType>::Navigator(d, path)
- {
- }
-
NavigatorPtr Navigator::FromAronType(const type::AronTypePtr& a, const Path& path)
{
return FACTORY->create(a, path);
}
-
- void Navigator::CheckTypeNavigatorPtrForNull(const std::string& c, const std::string& m, const NavigatorPtr& data)
- {
- if (data.get() == nullptr)
- {
- throw error::AronException(c, m, "Could not cast an NavigatorPtr. The Ptr was NULL.");
- }
- }
-
- void Navigator::CheckTypeNavigatorPtrForNull(const std::string& c, const std::string& m, const Path& p, const NavigatorPtr& data)
- {
- if (data.get() == nullptr)
- {
- throw error::AronException(c, m, "Could not cast an NavigatorPtr. The Ptr was NULL.", p);
- }
- }
-
- /* AronSingleAcceptedTypeHavingTypeNavigator */
- /*NavigatorPtr AronSingleAcceptedTypeHavingTypeNavigator::getAcceptedType() const
- {
- return acceptedTypeNavigator;
- }
-
- size_t AronSingleAcceptedTypeHavingTypeNavigator::childrenSize() const
- {
- return 1;
- }*/
-
- /* AronMultipleListAcceptedTypeHavingTypeNavigator */
- /*std::vector<NavigatorPtr> AronMultipleListAcceptedTypeHavingTypeNavigator::getAcceptedTypes() const
- {
- return acceptedTypeNavigators;
- }
-
- NavigatorPtr AronMultipleListAcceptedTypeHavingTypeNavigator::getAcceptedType(unsigned int i) const
- {
- if (i >= acceptedTypeNavigators.size())
- {
- return nullptr;
- }
- return acceptedTypeNavigators[i];
- }
-
- size_t AronMultipleListAcceptedTypeHavingTypeNavigator::childrenSize() const
- {
- return acceptedTypeNavigators.size();
- }*/
-
- /* AronMultipleDictAcceptedTypeHavingTypeNavigator */
- /*std::map<std::string, NavigatorPtr> AronMultipleDictAcceptedTypeHavingTypeNavigator::getAcceptedTypes() const
- {
- return acceptedTypeNavigators;
- }
-
- NavigatorPtr AronMultipleDictAcceptedTypeHavingTypeNavigator::getAcceptedType(const std::string& s) const
- {
- if (acceptedTypeNavigators.find(s) == acceptedTypeNavigators.end())
- {
- return nullptr;
- }
- return acceptedTypeNavigators.at(s);
- }
-
- size_t AronMultipleDictAcceptedTypeHavingTypeNavigator::childrenSize() const
- {
- return acceptedTypeNavigators.size();
- }
-
- std::vector<std::string> AronMultipleDictAcceptedTypeHavingTypeNavigator::getAllKeys() const
- {
- std::vector<std::string> ret;
- for (const auto& [key, _] : acceptedTypeNavigators)
- {
- ret.push_back(key);
- }
- return ret;
- }*/
-
- /* AronComplexTypeNavigator */
- /*size_t AronComplexTypeNavigator::childrenSize() const
- {
- return 0;
- }*/
-
- /* AronPrimitiveTypeNavigator */
- /*size_t AronPrimitiveTypeNavigator::childrenSize() const
- {
- return 0;
- }*/
}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/Navigator.h b/source/RobotAPI/libraries/aron/core/navigator/type/Navigator.h
index 863cfac1ced3afc9ea8bc04730aac956c89786fb..1ca45844db2cedd4cba004e94a57dde857ac491f 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/type/Navigator.h
+++ b/source/RobotAPI/libraries/aron/core/navigator/type/Navigator.h
@@ -30,7 +30,7 @@
#include <string>
// Simox
-#include <SimoxUtility/algorithm/string.h>
+// #include <SimoxUtility/algorithm/string.h>
// ArmarX
#include <RobotAPI/libraries/aron/core/Exception.h>
@@ -53,33 +53,46 @@ namespace armarx::aron::typenavigator
public:
// constructors
- Navigator() = delete;
- Navigator(const type::Descriptor&, const Path&);
+ Navigator() = default;
- // virtual methods
- virtual std::vector<NavigatorPtr> getChildren() const = 0;
- virtual size_t childrenSize() const = 0;
- virtual type::AronTypePtr getResult() const override = 0;
- virtual std::string getName() const override = 0;
+ // operators
+ virtual bool operator==(const Navigator& other) const = 0;
+ virtual bool operator==(const NavigatorPtr& other) const = 0;
// static methods
static NavigatorPtr FromAronType(const type::AronTypePtr&, const Path& = Path());
+ static std::vector<NavigatorPtr> FromAronType(const std::vector<type::AronTypePtr>&, const Path& = Path());
+ static std::vector<type::AronTypePtr> FromAronType(const std::vector<NavigatorPtr>&);
+
+ template<typename NavigatorT>
+ static typename NavigatorT::PointerType DynamicCast(const NavigatorPtr& n)
+ {
+ return NavigatorT::DynamicCast(n);
+ }
- template<typename NavigatorType>
- static typename NavigatorType::PointerType DynamicCast(const NavigatorPtr& n)
+ template<typename NavigatorT>
+ static typename NavigatorT::PointerType DynamicCast(const Navigator& n)
{
- return NavigatorType::DynamicCast(n);
+ return NavigatorT::DynamicCast(n);
}
- template<typename NavigatorType>
- static typename NavigatorType::PointerType DynamicCastAndCheck(const NavigatorPtr& n)
+ template<typename NavigatorT>
+ static typename NavigatorT::PointerType DynamicCastAndCheck(const NavigatorPtr& n)
{
- return NavigatorType::DynamicCastAndCheck(n);
+ return NavigatorT::DynamicCastAndCheck(n);
}
- protected:
- static void CheckTypeNavigatorPtrForNull(const std::string&, const std::string&, const NavigatorPtr& data);
- static void CheckTypeNavigatorPtrForNull(const std::string&, const std::string&, const Path&, const NavigatorPtr& data);
+ // virtual methods
+ virtual std::vector<NavigatorPtr> getChildren() const = 0;
+ virtual size_t childrenSize() const = 0;
+ virtual void setMaybe(const type::Maybe m) = 0;
+ virtual type::Maybe getMaybe() const = 0;
+
+ // public member functions
+ type::AronTypePtr toAronTypePtr() const
+ {
+ return toAronPtr();
+ }
private:
static const NavigatorFactoryPtr FACTORY;
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/NavigatorFactory.h b/source/RobotAPI/libraries/aron/core/navigator/type/NavigatorFactory.h
index 7e443cc3c02e19c7237bf8dd4a0bfa59ae7a82b8..e470407db41466a817ce5d2bd8199c160819be24 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/type/NavigatorFactory.h
+++ b/source/RobotAPI/libraries/aron/core/navigator/type/NavigatorFactory.h
@@ -40,7 +40,7 @@ namespace armarx::aron::typenavigator
typedef std::shared_ptr<NavigatorFactory> NavigatorFactoryPtr;
class NavigatorFactory :
- virtual public armarx::aron::NavigatorFactory<type::AronTypePtr, typenavigator::NavigatorPtr>
+ virtual public armarx::aron::NavigatorFactory<type::AronTypePtr, typenavigator::NavigatorPtr, type::Descriptor>
{
public:
NavigatorFactory() = default;
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/NavigatorFwd.h b/source/RobotAPI/libraries/aron/core/navigator/type/NavigatorFwd.h
new file mode 100644
index 0000000000000000000000000000000000000000..b5a9da2b9d05df4a0744a1d6074abcfff32aaafd
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/navigator/type/NavigatorFwd.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/>.
+ *
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <memory>
+
+/// implemented
+// #include "container/List.h"
+// #include "container/Object.h"
+
+/// TODO add forward decl for those as well
+// #include "container/Pair.h"
+// #include "container/Tuple.h"
+// #include "enum/IntEnum.h"
+// #include "ndarray/EigenMatrix.h"
+// #include "ndarray/EigenQuaternion.h"
+// #include "ndarray/IVTCByteImage.h"
+// #include "ndarray/NDArray.h"
+// #include "ndarray/OpenCVMat.h"
+// #include "ndarray/Orientation.h"
+// #include "ndarray/PCLPointCloud.h"
+// #include "ndarray/Pose.h"
+// #include "ndarray/Position.h"
+// #include "primitive/Bool.h"
+// #include "primitive/Double.h"
+// #include "primitive/Float.h"
+// #include "primitive/Int.h"
+// #include "primitive/Long.h"
+// #include "primitive/String.h"
+// #include "primitive/Time.h"
+
+namespace armarx::aron::typenavigator
+{
+ class DictNavigator;
+ using DictNavigatorPtr = std::shared_ptr<DictNavigator>;
+
+ class ListNavigator;
+ using ListNavigatorPtr = std::shared_ptr<ListNavigator>;
+
+} // namespace armarx::aron::typenavigator
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/container/Dict.cpp b/source/RobotAPI/libraries/aron/core/navigator/type/container/Dict.cpp
index fd14e22ead4f1610d4f4faf4d065396f850496bc..0b2b15fe570bbf32238da76bc1608234856e6249 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/type/container/Dict.cpp
+++ b/source/RobotAPI/libraries/aron/core/navigator/type/container/Dict.cpp
@@ -29,20 +29,26 @@ namespace armarx::aron::typenavigator
{
// constructors
DictNavigator::DictNavigator(const Path& path) :
- aron::Navigator<type::Descriptor, type::AronType>::Navigator(type::Descriptor::eDict, path),
- Navigator(type::Descriptor::eDict, path),
- type(new type::AronDict())
+ aron::Navigator<type::Descriptor, type::AronType>::Navigator(type::Descriptor::eDict, path)
{
}
DictNavigator::DictNavigator(const type::AronDictPtr& o, const Path& path) :
aron::Navigator<type::Descriptor, type::AronType>::Navigator(type::Descriptor::eDict, path),
- Navigator(type::Descriptor::eDict, path),
- type(o)
+ detail::NavigatorBase<type::AronDict, DictNavigator>(o),
+ acceptedType(FromAronType(o->acceptedType))
+ {
+ }
+
+ // operators
+ bool DictNavigator::operator==(const DictNavigator& other) const
{
- CheckAronPtrForNull("DictNavigator", "DictNavigator", getPath(), o);
- acceptedType = FromAronType(o->acceptedType);
+ if (getMaybe() != other.getMaybe())
+ {
+ return false;
+ }
+ return getAcceptedType() == other.getAcceptedType();
}
NavigatorPtr DictNavigator::getAcceptedType() const
@@ -52,28 +58,13 @@ namespace armarx::aron::typenavigator
void DictNavigator::setAcceptedType(const NavigatorPtr& a)
{
- type->acceptedType = a->getResult();
+ this->aron->acceptedType = a->toAronTypePtr();
acceptedType = a;
}
type::AronDictPtr DictNavigator::toAronDictPtr() const
{
- CheckAronPtrForNull("DictNavigator", "getResult", getPath(), type->acceptedType);
- return type;
- }
-
- // static methods
- DictNavigatorPtr DictNavigator::DynamicCast(const NavigatorPtr& n)
- {
- return std::dynamic_pointer_cast<DictNavigator>(n);
- }
-
- DictNavigatorPtr DictNavigator::DynamicCastAndCheck(const NavigatorPtr& n)
- {
- CheckTypeNavigatorPtrForNull("DictNavigator", "DynamicCastAndCheck[Before]", n);
- DictNavigatorPtr casted = DictNavigator::DynamicCast(n);
- CheckTypeNavigatorPtrForNull("DictNavigator", "DynamicCastAndCheck[After]", n->getPath(), casted);
- return casted;
+ return this->aron;
}
// virtual implementations
@@ -87,12 +78,6 @@ namespace armarx::aron::typenavigator
return 1;
}
- type::AronTypePtr DictNavigator::getResult() const
- {
- CheckAronPtrForNull("DictNavigator", "getResult", getPath(), type->acceptedType);
- return type;
- }
-
std::string DictNavigator::getName() const
{
return "AronDictType<" + acceptedType->getName() + ">";
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/container/Dict.h b/source/RobotAPI/libraries/aron/core/navigator/type/container/Dict.h
index 08ba52beda2b7070032647ba035b69db75e26f84..7d021d8fedb1b1185d0288728bd61c91f025b502 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/type/container/Dict.h
+++ b/source/RobotAPI/libraries/aron/core/navigator/type/container/Dict.h
@@ -28,7 +28,7 @@
#include <map>
// Base Class
-#include <RobotAPI/libraries/aron/core/navigator/type/Navigator.h>
+#include "../detail/ContainerNavigatorBase.h"
namespace armarx::aron::typenavigator
{
@@ -36,35 +36,29 @@ namespace armarx::aron::typenavigator
typedef std::shared_ptr<DictNavigator> DictNavigatorPtr;
class DictNavigator :
- virtual public Navigator
+ virtual public detail::ContainerNavigatorBase<type::AronDict, DictNavigator>
{
- public:
- using PointerType = DictNavigatorPtr;
-
public:
// constructors
DictNavigator(const Path& path = Path());
DictNavigator(const type::AronDictPtr&, const Path& path = Path());
+ // operators
+ virtual bool operator==(const DictNavigator&) const override;
+
+ // public member functions
NavigatorPtr getAcceptedType() const;
void setAcceptedType(const NavigatorPtr&);
type::AronDictPtr toAronDictPtr() const;
- // static methods
- static DictNavigatorPtr DynamicCast(const NavigatorPtr& n);
- static DictNavigatorPtr DynamicCastAndCheck(const NavigatorPtr& n);
-
// virtual implementations
+ std::string getName() const override;
virtual std::vector<NavigatorPtr> getChildren() const override;
virtual size_t childrenSize() const override;
- virtual type::AronTypePtr getResult() const override;
- virtual std::string getName() const override;
private:
// members
NavigatorPtr acceptedType;
- type::AronDictPtr type;
-
};
}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/container/List.cpp b/source/RobotAPI/libraries/aron/core/navigator/type/container/List.cpp
index 3e87c4d5fbbc7879299e9da5f20d7a2086d87465..3a20cc76d750fd3811c1e9c9a46ac874ee61dad5 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/type/container/List.cpp
+++ b/source/RobotAPI/libraries/aron/core/navigator/type/container/List.cpp
@@ -28,19 +28,25 @@ namespace armarx::aron::typenavigator
{
// constructors
ListNavigator::ListNavigator(const Path& path) :
- aron::Navigator<type::Descriptor, type::AronType>::Navigator(type::Descriptor::eList, path),
- Navigator(type::Descriptor::eList, path),
- type(new type::AronList())
+ aron::Navigator<type::Descriptor, type::AronType>::Navigator(type::Descriptor::eList, path)
{
}
ListNavigator::ListNavigator(const type::AronListPtr& o, const Path& path) :
aron::Navigator<type::Descriptor, type::AronType>::Navigator(type::Descriptor::eList, path),
- Navigator(type::Descriptor::eList, path),
- type(o)
+ detail::NavigatorBase<type::AronList, ListNavigator>(o),
+ acceptedType(FromAronType(o->acceptedType))
+ {
+ }
+
+ // operators
+ bool ListNavigator::operator==(const ListNavigator& other) const
{
- CheckAronPtrForNull("ListNavigator", "ListNavigator", getPath(), o);
- acceptedType = FromAronType(o->acceptedType);
+ if (getMaybe() != other.getMaybe())
+ {
+ return false;
+ }
+ return getAcceptedType() == other.getAcceptedType();
}
// Member functions
@@ -51,29 +57,15 @@ namespace armarx::aron::typenavigator
void ListNavigator::setAcceptedType(const NavigatorPtr& a)
{
- CheckTypeNavigatorPtrForNull("ListNavigator", "setAcceptedType", a);
- type->acceptedType = a->getResult();
+ ARMARX_CHECK_NOT_NULL(a);
+ this->aron->acceptedType = a->toAronTypePtr();
acceptedType = a;
}
// static methods
- ListNavigatorPtr ListNavigator::DynamicCast(const NavigatorPtr& n)
- {
- return std::dynamic_pointer_cast<ListNavigator>(n);
- }
-
- ListNavigatorPtr ListNavigator::DynamicCastAndCheck(const NavigatorPtr& n)
- {
- CheckTypeNavigatorPtrForNull("ListNavigator", "DynamicCast[Before]", n);
- ListNavigatorPtr casted = ListNavigator::DynamicCast(n);
- CheckTypeNavigatorPtrForNull("ListNavigator", "DynamicCast[After]", n->getPath(), casted);
- return casted;
- }
-
type::AronListPtr ListNavigator::toAronListPtr() const
{
- CheckAronPtrForNull("ListNavigator", "getCastedResult", getPath(), type->acceptedType);
- return type;
+ return this->aron;
}
// virtual implementations
@@ -87,11 +79,6 @@ namespace armarx::aron::typenavigator
return 1;
}
- type::AronTypePtr ListNavigator::getResult() const
- {
- return toAronListPtr();
- }
-
std::string ListNavigator::getName() const
{
return "AronListType<" + acceptedType->getName() + ">";
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/container/List.h b/source/RobotAPI/libraries/aron/core/navigator/type/container/List.h
index 2dac8bebb527e77f3a9e6d039a08c72fa2486b53..d02ea8a6e4a0c7afb9cd7996651e66c48a22cad8 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/type/container/List.h
+++ b/source/RobotAPI/libraries/aron/core/navigator/type/container/List.h
@@ -28,7 +28,7 @@
#include <vector>
// Base Class
-#include <RobotAPI/libraries/aron/core/navigator/type/Navigator.h>
+#include "../detail/ContainerNavigatorBase.h"
namespace armarx::aron::typenavigator
{
@@ -36,34 +36,29 @@ namespace armarx::aron::typenavigator
typedef std::shared_ptr<ListNavigator> ListNavigatorPtr;
class ListNavigator :
- virtual public Navigator
+ virtual public detail::ContainerNavigatorBase<type::AronList, ListNavigator>
{
- public:
- using PointerType = ListNavigatorPtr;
-
public:
// constructors
ListNavigator(const Path& path = Path());
ListNavigator(const type::AronListPtr&, const Path& path = Path());
+ // operators
+ virtual bool operator==(const ListNavigator&) const override;
+
+ // public member functions
NavigatorPtr getAcceptedType() const;
void setAcceptedType(const NavigatorPtr&);
type::AronListPtr toAronListPtr() const;
- // static methods
- static ListNavigatorPtr DynamicCast(const NavigatorPtr& n);
- static ListNavigatorPtr DynamicCastAndCheck(const NavigatorPtr& n);
-
// virtual implementations
+ std::string getName() const override;
virtual std::vector<NavigatorPtr> getChildren() const override;
virtual size_t childrenSize() const override;
- virtual type::AronTypePtr getResult() const override;
- virtual std::string getName() const override;
private:
// members
NavigatorPtr acceptedType;
- type::AronListPtr type;
};
}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/container/Object.cpp b/source/RobotAPI/libraries/aron/core/navigator/type/container/Object.cpp
index bc030f335ae8d67ecd9160e949a8943d99196e16..410ecaedd54896288b2c6d399b3d3994040632c7 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/type/container/Object.cpp
+++ b/source/RobotAPI/libraries/aron/core/navigator/type/container/Object.cpp
@@ -24,8 +24,9 @@
// Header
#include "Object.h"
-// ArmarX
-#include "SimoxUtility/algorithm/get_map_keys_values.h"
+
+#include <SimoxUtility/algorithm/get_map_keys_values.h>
+#include <SimoxUtility/algorithm/string/string_conversion.h>
namespace armarx::aron::typenavigator
@@ -33,54 +34,44 @@ namespace armarx::aron::typenavigator
// constructors
ObjectNavigator::ObjectNavigator(const Path& path) :
- aron::Navigator<type::Descriptor, type::AronType>::Navigator(type::Descriptor::eObject, path),
- Navigator(type::Descriptor::eObject, path),
- type((new type::AronObject()))
+ aron::Navigator<type::Descriptor, type::AronType>::Navigator(type::Descriptor::eObject, path)
{
}
ObjectNavigator::ObjectNavigator(const std::string& name, const Path& path) :
- aron::Navigator<type::Descriptor, type::AronType>::Navigator(type::Descriptor::eObject, path),
- Navigator(type::Descriptor::eObject, path),
- type((new type::AronObject()))
+ aron::Navigator<type::Descriptor, type::AronType>::Navigator(type::Descriptor::eObject, path)
{
setObjectName(name);
}
ObjectNavigator::ObjectNavigator(const type::AronObjectPtr& o, const Path& path) :
aron::Navigator<type::Descriptor, type::AronType>::Navigator(type::Descriptor::eObject, path),
- Navigator(type::Descriptor::eObject, path),
- type(o)
+ detail::NavigatorBase<type::AronObject, ObjectNavigator>(o)
{
- CheckAronPtrForNull("ObjectNavigator", "ObjectNavigator", getPath(), o);
-
for (const auto& [key, t] : o->elementTypes)
{
memberTypes[key] = FromAronType(t);
}
}
- bool ObjectNavigator::checkObjectName(const std::string& s) const
+ // operators
+ bool ObjectNavigator::operator==(const ObjectNavigator& other) const
{
- if (s.empty())
+ if (getMaybe() != other.getMaybe())
{
- throw error::AronException("ObjectNavigator", "getResult", "The object name is empty.", getPath());
+ return false;
}
- return true;
- }
- // static methods
- ObjectNavigatorPtr ObjectNavigator::DynamicCast(const NavigatorPtr& n)
- {
- return std::dynamic_pointer_cast<ObjectNavigator>(n);
+ return getObjectName() == other.getObjectName();
}
- ObjectNavigatorPtr ObjectNavigator::DynamicCastAndCheck(const NavigatorPtr& n)
+ bool ObjectNavigator::checkObjectName(const std::string& s) const
{
- CheckTypeNavigatorPtrForNull("ObjectNavigator", "DynamicCast[Before]", n);
- ObjectNavigatorPtr casted = ObjectNavigator::DynamicCast(n);
- CheckTypeNavigatorPtrForNull("ObjectNavigator", "DynamicCast[After]", n->getPath(), casted);
- return casted;
+ if (s.empty())
+ {
+ throw error::AronException("ObjectNavigator", "getResult", "The object name is empty.", getPath());
+ }
+ return true;
}
// public member functions
@@ -104,28 +95,35 @@ namespace armarx::aron::typenavigator
{
throw error::AronException("ObjectNavigator", "addAcceptedType", "Cannot set an element with an empty key.", getPath());
}
- CheckTypeNavigatorPtrForNull("ObjectNavigator", "addAcceptedType", v);
- type->elementTypes[k] = v->getResult();
+
+ ARMARX_CHECK_NOT_NULL(v);
+
+ this->aron->elementTypes[k] = v->toAronTypePtr();
memberTypes[k] = v;
}
void ObjectNavigator::setObjectName(const std::string& n)
{
//path.setRootIdentifier(n);
- type->objectName = n;
+ this->aron->objectName = n;
}
void ObjectNavigator::setExtends(const ObjectNavigatorPtr& p)
{
- CheckTypeNavigatorPtrForNull("ObjectNavigator", "setExtends", p);
+ ARMARX_CHECK_NOT_NULL(p);
type::AronObjectPtr ex = p->toAronObjectPtr();
- CheckAronPtrForNull("ObjectNavigator", "setExtends", getPath(), ex);
+ ARMARX_CHECK_NOT_NULL(ex);
extends = p;
}
+ bool ObjectNavigator::hasMemberType(const std::string& k) const
+ {
+ return memberTypes.count(k) > 0;
+ }
+
std::string ObjectNavigator::getObjectName() const
{
- return type->objectName;
+ return this->aron->objectName;
}
ObjectNavigatorPtr ObjectNavigator::getExtends() const
@@ -145,14 +143,12 @@ namespace armarx::aron::typenavigator
type::AronObjectPtr ObjectNavigator::toAronObjectPtr() const
{
- checkObjectName(type->objectName);
-
// TODO: Shall we allow empty objects?
//if(acceptedTypeNavigators.empty())
//{
// throw exception::AronExceptionWithPathInfo("ObjectNavigator", "getResult", "No accepted types set", getPath());
//}
- return type;
+ return this->aron;
}
// virtual implementations
@@ -171,14 +167,9 @@ namespace armarx::aron::typenavigator
return memberTypes.size();
}
- type::AronTypePtr ObjectNavigator::getResult() const
- {
- return toAronObjectPtr();
- }
-
std::string ObjectNavigator::getName() const
{
- return "AronObjectType<" + type->objectName + ">";
+ return "AronObjectType<" + this->aron->objectName + ">";
}
}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/container/Object.h b/source/RobotAPI/libraries/aron/core/navigator/type/container/Object.h
index 47d5016284001fe61531544fc479051343b71cf1..f3f9255646e22b826909ecc460a920a0b6071f97 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/type/container/Object.h
+++ b/source/RobotAPI/libraries/aron/core/navigator/type/container/Object.h
@@ -28,7 +28,7 @@
#include <map>
// Base Class
-#include <RobotAPI/libraries/aron/core/navigator/type/Navigator.h>
+#include "../detail/ContainerNavigatorBase.h"
namespace armarx::aron::typenavigator
@@ -37,17 +37,17 @@ namespace armarx::aron::typenavigator
typedef std::shared_ptr<ObjectNavigator> ObjectNavigatorPtr;
class ObjectNavigator :
- virtual public Navigator
+ virtual public detail::ContainerNavigatorBase<type::AronObject, ObjectNavigator>
{
- public:
- using PointerType = ObjectNavigatorPtr;
-
public:
// constructors
ObjectNavigator(const Path& = Path());
ObjectNavigator(const std::string&, const Path& = Path());
ObjectNavigator(const type::AronObjectPtr&, const Path& = Path());
+ // operators
+ virtual bool operator==(const ObjectNavigator&) const override;
+
// public member functions
bool checkObjectName(const std::string&) const;
@@ -60,24 +60,20 @@ namespace armarx::aron::typenavigator
void setExtends(const ObjectNavigatorPtr&);
void addMemberType(const std::string&, const NavigatorPtr&);
+ bool hasMemberType(const std::string&) const;
+
std::vector<std::string> getAllKeys() const;
type::AronObjectPtr toAronObjectPtr() const;
- // static methods
- static ObjectNavigatorPtr DynamicCast(const NavigatorPtr& n);
- static ObjectNavigatorPtr DynamicCastAndCheck(const NavigatorPtr& n);
-
// virtual implementations
+ std::string getName() const override;
virtual std::vector<NavigatorPtr> getChildren() const override;
virtual size_t childrenSize() const override;
- virtual type::AronTypePtr getResult() const override;
- virtual std::string getName() const override;
private:
// members
ObjectNavigatorPtr extends;
std::map<std::string, NavigatorPtr> memberTypes;
- type::AronObjectPtr type;
};
}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/container/Pair.cpp b/source/RobotAPI/libraries/aron/core/navigator/type/container/Pair.cpp
index 60e8947d4f9876b61c42add47979921e33395567..f9a2f4bd4a2a2d47e298ee12f3ee0def45edd0ba 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/type/container/Pair.cpp
+++ b/source/RobotAPI/libraries/aron/core/navigator/type/container/Pair.cpp
@@ -29,22 +29,27 @@ namespace armarx::aron::typenavigator
{
// constructors
PairNavigator::PairNavigator(const Path& path) :
- aron::Navigator<type::Descriptor, type::AronType>::Navigator(type::Descriptor::ePair, path),
- Navigator(type::Descriptor::ePair, path),
- type(new type::AronPair())
+ aron::Navigator<type::Descriptor, type::AronType>::Navigator(type::Descriptor::ePair, path)
{
}
PairNavigator::PairNavigator(const type::AronPairPtr& o, const Path& path) :
aron::Navigator<type::Descriptor, type::AronType>::Navigator(type::Descriptor::ePair, path),
- Navigator(type::Descriptor::ePair, path),
- type(o)
+ detail::NavigatorBase<type::AronPair, PairNavigator>(o),
+ acceptedType1(FromAronType(o->acceptedType1)),
+ acceptedType2(FromAronType(o->acceptedType2))
{
- CheckAronPtrForNull("AronPairTypeNavigator", "AronPairTypeNavigator", getPath(), o);
+ }
- acceptedType1 = FromAronType(o->acceptedType1);
- acceptedType2 = FromAronType(o->acceptedType2);
+ // operators
+ bool PairNavigator::operator==(const PairNavigator& other) const
+ {
+ if (getMaybe() != other.getMaybe())
+ {
+ return false;
+ }
+ return getFirstAcceptedType() == other.getFirstAcceptedType() && getSecondAcceptedType() == other.getSecondAcceptedType();
}
// Member functions
@@ -65,6 +70,8 @@ namespace armarx::aron::typenavigator
void PairNavigator::addAcceptedType(const NavigatorPtr& n)
{
+ ARMARX_CHECK_NOT_NULL(n);
+
if (acceptedType1 == nullptr)
{
acceptedType1 = n;
@@ -80,33 +87,19 @@ namespace armarx::aron::typenavigator
void PairNavigator::setFirstAcceptedType(const NavigatorPtr& n)
{
+ ARMARX_CHECK_NOT_NULL(n);
acceptedType1 = n;
}
void PairNavigator::setSecondAcceptedType(const NavigatorPtr& n)
{
+ ARMARX_CHECK_NOT_NULL(n);
acceptedType2 = n;
}
type::AronPairPtr PairNavigator::toAronPairPtr() const
{
- CheckAronPtrForNull("PairNavigator", "getResult", getPath(), type->acceptedType1);
- CheckAronPtrForNull("PairNavigator", "getResult", getPath(), type->acceptedType2);
- return type;
- }
-
- // static methods
- PairNavigatorPtr PairNavigator::DynamicCast(const NavigatorPtr& n)
- {
- return std::dynamic_pointer_cast<PairNavigator>(n);
- }
-
- PairNavigatorPtr PairNavigator::DynamicCastAndCheck(const NavigatorPtr& n)
- {
- CheckTypeNavigatorPtrForNull("AronPairTypeNavigator", "DynamicCastAndCheck[Before]", n);
- PairNavigatorPtr casted = PairNavigator::DynamicCast(n);
- CheckTypeNavigatorPtrForNull("AronPairTypeNavigator", "DynamicCastAndCheck[After]", n->getPath(), casted);
- return casted;
+ return this->aron;
}
// virtual implementations
@@ -120,13 +113,6 @@ namespace armarx::aron::typenavigator
return 2;
}
- type::AronTypePtr PairNavigator::getResult() const
- {
- CheckAronPtrForNull("PairNavigator", "getResult", getPath(), type->acceptedType1);
- CheckAronPtrForNull("PairNavigator", "getResult", getPath(), type->acceptedType2);
- return type;
- }
-
std::string PairNavigator::getName() const
{
return "AronPairType<" + acceptedType1->getName() + ", " + acceptedType2->getName() + ">";
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/container/Pair.h b/source/RobotAPI/libraries/aron/core/navigator/type/container/Pair.h
index 999a6c85656aa3ee4b5adc353e3cdbc26d770ce5..b29a3ba35d6876bc4f34c36ea2daacd40eda4797 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/type/container/Pair.h
+++ b/source/RobotAPI/libraries/aron/core/navigator/type/container/Pair.h
@@ -28,7 +28,7 @@
#include <map>
// Base Class
-#include <RobotAPI/libraries/aron/core/navigator/type/Navigator.h>
+#include "../detail/ContainerNavigatorBase.h"
namespace armarx::aron::typenavigator
{
@@ -36,16 +36,17 @@ namespace armarx::aron::typenavigator
typedef std::shared_ptr<PairNavigator> PairNavigatorPtr;
class PairNavigator :
- virtual public Navigator
+ virtual public detail::ContainerNavigatorBase<type::AronPair, PairNavigator>
{
- public:
- using PointerType = PairNavigatorPtr;
-
public:
// constructors
PairNavigator(const Path& path = Path());
PairNavigator(const type::AronPairPtr&, const Path& path = Path());
+ // operators
+ virtual bool operator==(const PairNavigator&) const override;
+
+ // public member functions
std::pair<NavigatorPtr, NavigatorPtr> getAcceptedTypes() const;
NavigatorPtr getFirstAcceptedType() const;
NavigatorPtr getSecondAcceptedType() const;
@@ -55,21 +56,15 @@ namespace armarx::aron::typenavigator
type::AronPairPtr toAronPairPtr() const;
- // static methods
- static PairNavigatorPtr DynamicCast(const NavigatorPtr& n);
- static PairNavigatorPtr DynamicCastAndCheck(const NavigatorPtr& n);
-
// virtual implementations
+ std::string getName() const override;
virtual std::vector<NavigatorPtr> getChildren() const override;
virtual size_t childrenSize() const override;
- virtual type::AronTypePtr getResult() const override;
- virtual std::string getName() const override;
private:
// members
NavigatorPtr acceptedType1;
NavigatorPtr acceptedType2;
- type::AronPairPtr type;
};
}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/container/Tuple.cpp b/source/RobotAPI/libraries/aron/core/navigator/type/container/Tuple.cpp
index 14a94b890106d5daa688b4e120eff978b32199cc..bded16377622e363c07fd5007de86349d2248a11 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/type/container/Tuple.cpp
+++ b/source/RobotAPI/libraries/aron/core/navigator/type/container/Tuple.cpp
@@ -24,51 +24,52 @@
// Header
#include "Tuple.h"
+#include <SimoxUtility/algorithm/string/string_conversion.h>
namespace armarx::aron::typenavigator
{
// constructors
TupleNavigator::TupleNavigator(const Path& path) :
- aron::Navigator<type::Descriptor, type::AronType>::Navigator(type::Descriptor::eTuple, path),
- Navigator(type::Descriptor::eTuple, path),
- type(new type::AronTuple())
+ aron::Navigator<type::Descriptor, type::AronType>::Navigator(type::Descriptor::eTuple, path)
{
}
TupleNavigator::TupleNavigator(const type::AronTuplePtr& o, const Path& path) :
aron::Navigator<type::Descriptor, type::AronType>::Navigator(type::Descriptor::eTuple, path),
- Navigator(type::Descriptor::eTuple, path),
- type(o)
+ detail::NavigatorBase<type::AronTuple, TupleNavigator>(o)
{
- CheckAronPtrForNull("TupleNavigator", "TupleNavigator", getPath(), o);
-
- for (const auto& t : type->elementTypes)
+ for (const auto& t : this->aron->elementTypes)
{
acceptedTypes.push_back(FromAronType(t));
}
}
- type::AronTuplePtr TupleNavigator::toAronTuplePtr() const
+ // operators
+ bool TupleNavigator::operator==(const TupleNavigator& other) const
{
- if (acceptedTypes.empty())
+ if (getMaybe() != other.getMaybe())
{
- throw error::AronException("TupleNavigator", "getCastedResult", "No accepted types set", getPath());
+ return false;
}
- return type;
- }
- // static methods
- TupleNavigatorPtr TupleNavigator::DynamicCast(const NavigatorPtr& n)
- {
- return std::dynamic_pointer_cast<TupleNavigator>(n);
+ unsigned int i = 0;
+ for (const auto& nav : acceptedTypes)
+ {
+ if (nav != other.getAcceptedType(i++))
+ {
+ return false;
+ }
+ }
+ return true;
}
- TupleNavigatorPtr TupleNavigator::DynamicCastAndCheck(const NavigatorPtr& n)
+ type::AronTuplePtr TupleNavigator::toAronTuplePtr() const
{
- CheckTypeNavigatorPtrForNull("TupleNavigator", "DynamicCast[Before]", n);
- TupleNavigatorPtr casted = TupleNavigator::DynamicCast(n);
- CheckTypeNavigatorPtrForNull("TupleNavigator", "DynamicCast[After]", n->getPath(), casted);
- return casted;
+ if (acceptedTypes.empty())
+ {
+ throw error::AronException("TupleNavigator", "getCastedResult", "No accepted types set", getPath());
+ }
+ return this->aron;
}
// public member functions
@@ -84,8 +85,8 @@ namespace armarx::aron::typenavigator
void TupleNavigator::addAcceptedType(const NavigatorPtr& v)
{
- CheckTypeNavigatorPtrForNull("TupleNavigator", "addAcceptedType", getPath(), v);
- type->elementTypes.push_back(v->getResult());
+ ARMARX_CHECK_NOT_NULL(v);
+ this->aron->elementTypes.push_back(v->toAronTypePtr());
acceptedTypes.push_back(v);
}
@@ -100,11 +101,6 @@ namespace armarx::aron::typenavigator
return acceptedTypes.size();
}
- type::AronTypePtr TupleNavigator::getResult() const
- {
- return toAronTuplePtr();
- }
-
std::string TupleNavigator::getName() const
{
std::vector<std::string> names;
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/container/Tuple.h b/source/RobotAPI/libraries/aron/core/navigator/type/container/Tuple.h
index 83348835ca0535934989354a8d63e1dcdf5d83bf..60665e7d41d2e2120badc3de60b9e84067fbf215 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/type/container/Tuple.h
+++ b/source/RobotAPI/libraries/aron/core/navigator/type/container/Tuple.h
@@ -28,7 +28,7 @@
#include <map>
// Base Class
-#include <RobotAPI/libraries/aron/core/navigator/type/Navigator.h>
+#include "../detail/ContainerNavigatorBase.h"
namespace armarx::aron::typenavigator
{
@@ -36,34 +36,29 @@ namespace armarx::aron::typenavigator
typedef std::shared_ptr<TupleNavigator> TupleNavigatorPtr;
class TupleNavigator :
- virtual public Navigator
+ virtual public detail::ContainerNavigatorBase<type::AronTuple, TupleNavigator>
{
- public:
- using PointerType = TupleNavigatorPtr;
-
public:
// constructors
TupleNavigator(const Path& path = Path());
TupleNavigator(const type::AronTuplePtr&, const Path& path = Path());
+ // operators
+ virtual bool operator==(const TupleNavigator&) const override;
+
+ // public member functions
std::vector<NavigatorPtr> getAcceptedTypes() const;
NavigatorPtr getAcceptedType(unsigned int i) const;
void addAcceptedType(const NavigatorPtr&);
type::AronTuplePtr toAronTuplePtr() const;
- // static methods
- static TupleNavigatorPtr DynamicCast(const NavigatorPtr& n);
- static TupleNavigatorPtr DynamicCastAndCheck(const NavigatorPtr& n);
-
// virtual implementations
+ std::string getName() const override;
virtual std::vector<NavigatorPtr> getChildren() const override;
virtual size_t childrenSize() const override;
- virtual type::AronTypePtr getResult() const override;
- virtual std::string getName() const override;
private:
std::vector<NavigatorPtr> acceptedTypes;
- type::AronTuplePtr type;
};
}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/detail/ContainerNavigatorBase.cpp b/source/RobotAPI/libraries/aron/core/navigator/type/detail/ContainerNavigatorBase.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..5068d7dce5fce2a05fa8cd10684ff969de3353fe
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/navigator/type/detail/ContainerNavigatorBase.cpp
@@ -0,0 +1,25 @@
+/*
+ * 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 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
+ */
+
+// Header
+#include "PrimitiveNavigatorBase.h"
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/detail/ContainerNavigatorBase.h b/source/RobotAPI/libraries/aron/core/navigator/type/detail/ContainerNavigatorBase.h
new file mode 100644
index 0000000000000000000000000000000000000000..e1b60ff4005f5aa59a3d88c89508ddbe26d374f5
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/navigator/type/detail/ContainerNavigatorBase.h
@@ -0,0 +1,51 @@
+/*
+ * 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 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
+ */
+
+#pragma once
+
+// STD/STL
+#include <memory>
+#include <string>
+#include <unordered_map>
+
+// Base class
+#include "NavigatorBase.h"
+
+// ArmarX
+
+namespace armarx::aron::typenavigator::detail
+{
+ template<typename AronTypeT, typename DerivedT>
+ class ContainerNavigatorBase :
+ virtual public NavigatorBase<AronTypeT, DerivedT>
+ {
+ public:
+ ContainerNavigatorBase() {}
+
+ // virtual implementations
+
+ // static methods
+
+ /* public member functions */
+ };
+}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/detail/NDArrayNavigatorBase.cpp b/source/RobotAPI/libraries/aron/core/navigator/type/detail/NDArrayNavigatorBase.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..5068d7dce5fce2a05fa8cd10684ff969de3353fe
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/navigator/type/detail/NDArrayNavigatorBase.cpp
@@ -0,0 +1,25 @@
+/*
+ * 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 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
+ */
+
+// Header
+#include "PrimitiveNavigatorBase.h"
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/detail/NDArrayNavigatorBase.h b/source/RobotAPI/libraries/aron/core/navigator/type/detail/NDArrayNavigatorBase.h
new file mode 100644
index 0000000000000000000000000000000000000000..7beb2256a8671ff008bdef2d244f1e635fac17e5
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/navigator/type/detail/NDArrayNavigatorBase.h
@@ -0,0 +1,55 @@
+/*
+ * 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 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
+ */
+
+#pragma once
+
+// STD/STL
+#include <memory>
+#include <string>
+#include <unordered_map>
+
+// Base class
+#include "NavigatorBase.h"
+
+// ArmarX
+
+namespace armarx::aron::typenavigator::detail
+{
+ template<typename AronTypeT, typename DerivedT>
+ class NDArrayNavigatorBase :
+ virtual public NavigatorBase<AronTypeT, DerivedT>
+ {
+ public:
+ NDArrayNavigatorBase() {};
+
+ /* virtual implementations */
+ virtual std::vector<NavigatorPtr> getChildren() const override
+ {
+ return {};
+ }
+ virtual size_t childrenSize() const override
+ {
+ return 0;
+ }
+ };
+}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/detail/NavigatorBase.cpp b/source/RobotAPI/libraries/aron/core/navigator/type/detail/NavigatorBase.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..a5e8c5aa05314a73d33e01c14d01a38976ba88a3
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/navigator/type/detail/NavigatorBase.cpp
@@ -0,0 +1,25 @@
+/*
+ * 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 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
+ */
+
+// Header
+#include "NavigatorBase.h"
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/detail/NavigatorBase.h b/source/RobotAPI/libraries/aron/core/navigator/type/detail/NavigatorBase.h
new file mode 100644
index 0000000000000000000000000000000000000000..055b1d3b68b14956bc65281bfc38030773baf5c9
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/navigator/type/detail/NavigatorBase.h
@@ -0,0 +1,131 @@
+/*
+ * 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 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
+ */
+
+#pragma once
+
+// STD/STL
+#include <memory>
+#include <string>
+#include <unordered_map>
+
+// Base class
+#include "../Navigator.h"
+
+// ArmarX
+
+namespace armarx::aron::typenavigator::detail
+{
+ template<typename AronTypeT, typename DerivedT>
+ class NavigatorBase :
+ virtual public typenavigator::Navigator
+ {
+ public:
+ using PointerType = std::shared_ptr<DerivedT>;
+ using AronType = AronTypeT;
+
+ public:
+ NavigatorBase() :
+ aron(new AronTypeT())
+ {
+ }
+
+ NavigatorBase(const typename AronType::PointerType& o) :
+ aron(new AronTypeT(*o))
+ {
+ ARMARX_CHECK_NOT_NULL(aron);
+ }
+
+ // operators
+ operator typename AronType::PointerType()
+ {
+ return aron;
+ }
+
+ virtual bool operator==(const Navigator& other) const override
+ {
+ const auto& n = DerivedT::DynamicCast(other);
+ return *this == n;
+ }
+ virtual bool operator==(const NavigatorPtr& other) const override
+ {
+ if (!other)
+ {
+ return false;
+ }
+ return *this == *other;
+ }
+
+ virtual bool operator==(const DerivedT&) const = 0;
+ bool operator==(const PointerType& other) const
+ {
+ if (other)
+ {
+ return *this == *other;
+ }
+ return false;
+ }
+
+ // virtual implementations
+ virtual type::AronTypePtr toAronPtr() const override
+ {
+ ARMARX_CHECK_NOT_NULL(aron);
+ return aron;
+ }
+ void setMaybe(const type::Maybe m) override
+ {
+ aron->maybe = m;
+ }
+
+ type::Maybe getMaybe() const override
+ {
+ return aron->maybe;
+ }
+
+ // static methods
+ static PointerType DynamicCast(const NavigatorPtr& n)
+ {
+ return std::dynamic_pointer_cast<DerivedT>(n);
+ }
+
+ static DerivedT& DynamicCast(Navigator& n)
+ {
+ return dynamic_cast<DerivedT&>(n);
+ }
+
+ static const DerivedT& DynamicCast(const Navigator& n)
+ {
+ return dynamic_cast<const DerivedT&>(n);
+ }
+
+ static PointerType DynamicCastAndCheck(const NavigatorPtr& n)
+ {
+ ARMARX_CHECK_NOT_NULL(n);
+ auto casted = DerivedT::DynamicCast(n);
+ ARMARX_CHECK_NOT_NULL(casted);
+ return casted;
+ }
+
+ protected:
+ typename AronType::PointerType aron;
+ };
+}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/detail/PrimitiveNavigatorBase.cpp b/source/RobotAPI/libraries/aron/core/navigator/type/detail/PrimitiveNavigatorBase.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..5068d7dce5fce2a05fa8cd10684ff969de3353fe
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/navigator/type/detail/PrimitiveNavigatorBase.cpp
@@ -0,0 +1,25 @@
+/*
+ * 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 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
+ */
+
+// Header
+#include "PrimitiveNavigatorBase.h"
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/detail/PrimitiveNavigatorBase.h b/source/RobotAPI/libraries/aron/core/navigator/type/detail/PrimitiveNavigatorBase.h
new file mode 100644
index 0000000000000000000000000000000000000000..a73fbcbd1c0f3556f623ef759d3f2552f1376128
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/navigator/type/detail/PrimitiveNavigatorBase.h
@@ -0,0 +1,61 @@
+/*
+ * 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 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
+ */
+
+#pragma once
+
+// STD/STL
+#include <memory>
+#include <string>
+#include <unordered_map>
+
+// Base class
+#include "NavigatorBase.h"
+
+// ArmarX
+
+namespace armarx::aron::typenavigator::detail
+{
+ template<typename AronDataT, typename DerivedT>
+ class PrimitiveNavigatorBase :
+ virtual public NavigatorBase<AronDataT, DerivedT>
+ {
+ public:
+ PrimitiveNavigatorBase() {};
+
+ // operators
+ virtual bool operator==(const DerivedT& other) const override
+ {
+ return this->getMaybe() == other.getMaybe();
+ }
+
+ /* virtual implementations */
+ virtual std::vector<NavigatorPtr> getChildren() const override
+ {
+ return {};
+ }
+ virtual size_t childrenSize() const override
+ {
+ return 0;
+ }
+ };
+}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/enum/IntEnum.cpp b/source/RobotAPI/libraries/aron/core/navigator/type/enum/IntEnum.cpp
index 682a177ea6ab823e7a1ee7b0a2372eb95054d837..0e3f0594bb085b991fda4a66acf3877b5590a550 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/type/enum/IntEnum.cpp
+++ b/source/RobotAPI/libraries/aron/core/navigator/type/enum/IntEnum.cpp
@@ -28,28 +28,54 @@ namespace armarx::aron::typenavigator
{
// constructors
IntEnumNavigator::IntEnumNavigator(const Path& path) :
- aron::Navigator<type::Descriptor, type::AronType>::Navigator(type::Descriptor::eIntEnum, path),
- Navigator(type::Descriptor::eIntEnum, path),
- type(new type::AronIntEnum())
+ aron::Navigator<type::Descriptor, type::AronType>::Navigator(type::Descriptor::eIntEnum, path)
{
}
IntEnumNavigator::IntEnumNavigator(const type::AronIntEnumPtr& o, const Path& path) :
aron::Navigator<type::Descriptor, type::AronType>::Navigator(type::Descriptor::eIntEnum, path),
- Navigator(type::Descriptor::eIntEnum, path),
- type(o)
+ detail::NavigatorBase<type::AronIntEnum, IntEnumNavigator>(o)
{
- CheckAronPtrForNull("AronEnumTypeNavigator", "AronEnumTypeNavigator", getPath(), o);
}
- std::map<std::string, int> IntEnumNavigator::getAcceptedValues() const
+ // operators
+ bool IntEnumNavigator::operator==(const IntEnumNavigator& other) const
{
- return type->acceptedValues;
+ if (getMaybe() != other.getMaybe())
+ {
+ return false;
+ }
+ return getEnumName() == other.getEnumName();
+ }
+
+ std::map<std::string, int> IntEnumNavigator::getAcceptedValueMap() const
+ {
+ return this->aron->acceptedValues;
+ }
+
+ std::vector<std::string> IntEnumNavigator::getAcceptedValueNames() const
+ {
+ std::vector<std::string> names;
+ for (const auto& [k, _] : aron->acceptedValues)
+ {
+ names.push_back(k);
+ }
+ return names;
+ }
+
+ std::vector<int> IntEnumNavigator::getAcceptedValues() const
+ {
+ std::vector<int> vals;
+ for (const auto& [_, i] : aron->acceptedValues)
+ {
+ vals.push_back(i);
+ }
+ return vals;
}
std::string IntEnumNavigator::getValueName(int i) const
{
- for (const auto& [k, v] : type->acceptedValues)
+ for (const auto& [k, v] : this->aron->acceptedValues)
{
if (v == i)
{
@@ -61,62 +87,41 @@ namespace armarx::aron::typenavigator
int IntEnumNavigator::getValue(const std::string& s) const
{
- return type->acceptedValues[s];
+ return this->aron->acceptedValues[s];
}
std::string IntEnumNavigator::getEnumName() const
{
- return type->enumName;
+ return this->aron->enumName;
}
void IntEnumNavigator::setEnumName(const std::string& s)
{
- type->enumName = s;
+ this->aron->enumName = s;
}
void IntEnumNavigator::addAcceptedValue(const std::string& s, int i)
{
- type->acceptedValues[s] = i;
+ this->aron->acceptedValues[s] = i;
}
type::AronIntEnumPtr IntEnumNavigator::toAronIntEnumPtr() const
{
- return type;
+ return this->aron;
}
- // static methods
- IntEnumNavigatorPtr IntEnumNavigator::DynamicCast(const NavigatorPtr& n)
- {
- return std::dynamic_pointer_cast<IntEnumNavigator>(n);
- }
-
- IntEnumNavigatorPtr IntEnumNavigator::DynamicCastAndCheck(const NavigatorPtr& n)
+ // virtual implementations
+ std::string IntEnumNavigator::getName() const
{
- CheckTypeNavigatorPtrForNull("AronEnumTypeNavigator", "DynamicCast[Before]", n);
- IntEnumNavigatorPtr casted = IntEnumNavigator::DynamicCast(n);
- CheckTypeNavigatorPtrForNull("AronEnumTypeNavigator", "DynamicCast[After]", n->getPath(), casted);
- return casted;
+ return "AronIntEnumType";
}
-
- // virtual implementations
std::vector<NavigatorPtr> IntEnumNavigator::getChildren() const
{
return {};
}
-
size_t IntEnumNavigator::childrenSize() const
{
return 0;
}
-
- type::AronTypePtr IntEnumNavigator::getResult() const
- {
- return toAronIntEnumPtr();
- }
-
- std::string IntEnumNavigator::getName() const
- {
- return "AronIntEnumType";
- }
}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/enum/IntEnum.h b/source/RobotAPI/libraries/aron/core/navigator/type/enum/IntEnum.h
index 9556a9d48ee6efb8ab74e37deb6493bbc8ae3ce0..31ccdab0ddecf016b9eaf67a5af5941e65d6a272 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/type/enum/IntEnum.h
+++ b/source/RobotAPI/libraries/aron/core/navigator/type/enum/IntEnum.h
@@ -27,8 +27,8 @@
#include <string>
#include <map>
-// Base Class
-#include <RobotAPI/libraries/aron/core/navigator/type/Navigator.h>
+// Base class
+#include "../detail/NavigatorBase.h"
namespace armarx::aron::typenavigator
{
@@ -36,18 +36,21 @@ namespace armarx::aron::typenavigator
typedef std::shared_ptr<IntEnumNavigator> IntEnumNavigatorPtr;
class IntEnumNavigator :
- virtual public Navigator
+ virtual public detail::NavigatorBase<type::AronIntEnum, IntEnumNavigator>
{
- public:
- using PointerType = IntEnumNavigatorPtr;
-
public:
// constructors
- IntEnumNavigator() = delete;
- IntEnumNavigator(const Path& path);
- IntEnumNavigator(const type::AronIntEnumPtr&, const Path& path);
+ IntEnumNavigator(const Path& path = Path());
+ IntEnumNavigator(const type::AronIntEnumPtr&, const Path& path = Path());
+
+ // operators
+ virtual bool operator==(const IntEnumNavigator&) const override;
+
+ // public member functions
+ std::map<std::string, int> getAcceptedValueMap() const;
+ std::vector<std::string> getAcceptedValueNames() const;
+ std::vector<int> getAcceptedValues() const;
- std::map<std::string, int> getAcceptedValues() const;
std::string getValueName(int) const;
int getValue(const std::string&) const;
std::string getEnumName() const;
@@ -57,20 +60,9 @@ namespace armarx::aron::typenavigator
type::AronIntEnumPtr toAronIntEnumPtr() const;
- // static methods
- static IntEnumNavigatorPtr DynamicCast(const NavigatorPtr& n);
- static IntEnumNavigatorPtr DynamicCastAndCheck(const NavigatorPtr& n);
-
// virtual implementations
+ virtual std::string getName() const override;
virtual std::vector<NavigatorPtr> getChildren() const override;
virtual size_t childrenSize() const override;
- virtual type::AronTypePtr getResult() const override;
- virtual std::string getName() const override;
-
- public:
-
- private:
- // members
- type::AronIntEnumPtr type;
};
}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/EigenMatrix.cpp b/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/EigenMatrix.cpp
index cb76098c35a99a7a1fd8a3dbea5fd396a4c5f475..df5223b3c99df12055f6e9e7106b3f2cac1f53fd 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/EigenMatrix.cpp
+++ b/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/EigenMatrix.cpp
@@ -24,150 +24,100 @@
// Header
#include "EigenMatrix.h"
+// Simox
+#include <SimoxUtility/algorithm/vector.hpp>
+
namespace armarx::aron::typenavigator
{
// constructors
EigenMatrixNavigator::EigenMatrixNavigator(const Path& path) :
- aron::Navigator<type::Descriptor, type::AronType>::Navigator(type::Descriptor::eEigenMatrix, path),
- Navigator(type::Descriptor::eEigenMatrix, path),
- type(new type::AronEigenMatrix({1, 1}, ""))
+ aron::Navigator<type::Descriptor, type::AronType>::Navigator(type::Descriptor::eEigenMatrix, path)
{
}
EigenMatrixNavigator::EigenMatrixNavigator(const type::AronEigenMatrixPtr& o, const Path& path) :
aron::Navigator<type::Descriptor, type::AronType>::Navigator(type::Descriptor::eDict, path),
- Navigator(type::Descriptor::eEigenMatrix, path),
- type(o)
+ detail::NavigatorBase<type::AronEigenMatrix, EigenMatrixNavigator>(o)
{
- CheckAronPtrForNull("AronEigenMatrixNavigator", "AronEigenMatrixNavigator", getPath(), o);
- checkDimensions(type->dimensions);
- checkTypename(type->typeName);
}
- bool EigenMatrixNavigator::checkDimensions(const std::vector<int>& d) const
- {
- if (d.size() < ACCEPTED_DIMENSION_MIN_SIZE)
- {
- throw error::AronException("AronEigenMatrixNavigator", "checkDimensions", "The dimension size is wrong. Got size: " + std::to_string(d.size()), getPath());
- }
-
- if (std::any_of(d.begin(), d.end(), [](int i)
+ // operators
+ bool EigenMatrixNavigator::operator==(const EigenMatrixNavigator& other) const
{
- return i < -1 || i == 0;
- }))
+ if (getMaybe() != other.getMaybe())
{
- throw error::AronException("AronEigenMatrixNavigator", "checkDimensions", "The dimension size is wrong. At least one empty is < -1 or 0. Got as dimension: " + simox::alg::to_string(d, ", "), getPath());
+ return false;
}
- return true;
+ return getCols() == other.getCols() && getRows() == other.getRows() && getTypename() == other.getTypename();
}
- bool EigenMatrixNavigator::checkTypename(const std::string& s) const
- {
- if (s.empty())
- {
- throw error::AronException("AronEigenMatrixNavigator", "checkTypename", "The typename is empty.", getPath());
- }
-
- if (!std::any_of(ACCEPTED_TYPES.begin(), ACCEPTED_TYPES.end(), [s](const auto & el)
+ std::string EigenMatrixNavigator::checkTypename(const std::string& s) const
{
- return s == el.first || std::find(el.second.begin(), el.second.end(), s) != el.second.end();
- }))
+ for (const auto& [key, list] : ACCEPTED_TYPES)
{
- throw error::AronException("AronEigenMatrixNavigator", "checkTypename", "The typename is wrong. Got " + s, getPath());
+ if (s == key)
+ {
+ return key;
+ }
+
+ for (const auto& el : list)
+ {
+ if (s == el)
+ {
+ return key;
+ }
+ }
}
- return true;
+ throw error::StringNotValidException("EigenMatrixNavigator", "checkTypename", "The typename is wrong.", s, getPath());
}
unsigned int EigenMatrixNavigator::getRows() const
{
- return type->dimensions[0];
+ return this->aron->rows;
}
unsigned int EigenMatrixNavigator::getCols() const
{
- return type->dimensions[1];
+ return this->aron->cols;
}
- void EigenMatrixNavigator::setRows(const unsigned int& w)
+ void EigenMatrixNavigator::setRows(int w)
{
- if (w == 0)
+ if (w == 0 || w < -1)
{
- throw error::AronException("AronEigenMatrixNavigator", "setRows", "The rows cannot be 0", getPath());
+ throw error::AronException("AronEigenMatrixNavigator", "setRows", "The rows cannot be 0 or < -1", getPath());
}
- type->dimensions[0] = w;
+ this->aron->rows = w;
}
- void EigenMatrixNavigator::setCols(const unsigned int& h)
+ void EigenMatrixNavigator::setCols(int h)
{
- if (h == 0)
+ if (h == 0 || h < -1)
{
- throw error::AronException("AronEigenMatrixNavigator", "setCols", "The cols cannot be 0", getPath());
+ throw error::AronException("AronEigenMatrixNavigator", "setCols", "The cols cannot be 0 or < -1", getPath());
}
- type->dimensions[1] = h;
- }
-
- type::AronEigenMatrixPtr EigenMatrixNavigator::toAronEigenMatrixPtr() const
- {
- checkDimensions(type->dimensions);
- checkTypename(type->typeName);
- return type;
+ this->aron->cols = h;
}
std::string EigenMatrixNavigator::getTypename() const
{
- return type->typeName;
- }
-
- std::vector<int> EigenMatrixNavigator::getDimensions() const
- {
- return type->dimensions;
+ return this->aron->typeName;
}
void EigenMatrixNavigator::setTypename(const std::string& u)
{
- checkTypename(u);
- type->typeName = u;
+ this->aron->typeName = checkTypename(u);;
}
- void EigenMatrixNavigator::setDimensions(const std::vector<int>& d)
- {
- checkDimensions(d);
- type->dimensions = d;
- }
-
- // static methods
- EigenMatrixNavigatorPtr EigenMatrixNavigator::DynamicCast(const NavigatorPtr& n)
- {
- return std::dynamic_pointer_cast<EigenMatrixNavigator>(n);
- }
-
- EigenMatrixNavigatorPtr EigenMatrixNavigator::DynamicCastAndCheck(const NavigatorPtr& n)
+ type::AronEigenMatrixPtr EigenMatrixNavigator::toAronEigenMatrixPtr() const
{
- CheckTypeNavigatorPtrForNull("AronEigenMatrixNavigator", "DynamicCast[Before]", n);
- EigenMatrixNavigatorPtr casted = EigenMatrixNavigator::DynamicCast(n);
- CheckTypeNavigatorPtrForNull("AronEigenMatrixNavigator", "DynamicCast[After]", n->getPath(), casted);
- return casted;
+ return this->aron;
}
// virtual implementations
- std::vector<NavigatorPtr> EigenMatrixNavigator::getChildren() const
- {
- return {};
- }
-
- size_t EigenMatrixNavigator::childrenSize() const
- {
- return 0;
- }
-
- type::AronTypePtr EigenMatrixNavigator::getResult() const
- {
- return toAronEigenMatrixPtr();
- }
-
std::string EigenMatrixNavigator::getName() const
{
- return "AronEigenMatrix<" + simox::alg::to_string(type->dimensions, ", ") + ", " + type->typeName + ">";
+ return "AronEigenMatrix<" + std::to_string(this->aron->rows) + ", " + std::to_string(this->aron->cols) + ", " + this->aron->typeName + ">";
}
}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/EigenMatrix.h b/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/EigenMatrix.h
index 0fb7d9a71bd21a96b024ee3eb5cec9f231216fd6..4cff659c1b9d063e54f786fb27230ff7cbd8d689 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/EigenMatrix.h
+++ b/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/EigenMatrix.h
@@ -27,8 +27,8 @@
#include <string>
#include <map>
-// Base Class
-#include <RobotAPI/libraries/aron/core/navigator/type/Navigator.h>
+// Base class
+#include "../detail/NDArrayNavigatorBase.h"
namespace armarx::aron::typenavigator
{
@@ -36,40 +36,30 @@ namespace armarx::aron::typenavigator
typedef std::shared_ptr<EigenMatrixNavigator> EigenMatrixNavigatorPtr;
class EigenMatrixNavigator :
- virtual public Navigator
+ virtual public detail::NDArrayNavigatorBase<type::AronEigenMatrix, EigenMatrixNavigator>
{
- public:
- using PointerType = EigenMatrixNavigatorPtr;
-
public:
// constructors
- EigenMatrixNavigator() = delete;
- EigenMatrixNavigator(const Path& path);
- EigenMatrixNavigator(const type::AronEigenMatrixPtr&, const Path& path);
+ EigenMatrixNavigator(const Path& path = Path());
+ EigenMatrixNavigator(const type::AronEigenMatrixPtr&, const Path& path = Path());
+
+ // operators
+ virtual bool operator==(const EigenMatrixNavigator&) const override;
- bool checkDimensions(const std::vector<int>&) const;
- bool checkTypename(const std::string&) const;
+ // public member functions
+ std::string checkTypename(const std::string&) const;
unsigned int getRows() const;
unsigned int getCols() const;
std::string getTypename() const;
- std::vector<int> getDimensions() const;
- void setRows(const unsigned int&);
- void setCols(const unsigned int&);
+ void setRows(int);
+ void setCols(int);
void setTypename(const std::string&);
- void setDimensions(const std::vector<int>&);
type::AronEigenMatrixPtr toAronEigenMatrixPtr() const;
- // static methods
- static EigenMatrixNavigatorPtr DynamicCast(const NavigatorPtr& n);
- static EigenMatrixNavigatorPtr DynamicCastAndCheck(const NavigatorPtr& n);
-
// virtual implementations
- virtual std::vector<NavigatorPtr> getChildren() const override;
- virtual size_t childrenSize() const override;
- virtual type::AronTypePtr getResult() const override;
virtual std::string getName() const override;
public:
@@ -83,10 +73,5 @@ namespace armarx::aron::typenavigator
//{"complex float", {"std::complex<float>"}},
//{"complex double", {"std::complex<double>"}},
};
- const unsigned int ACCEPTED_DIMENSION_MIN_SIZE = 2;
-
- private:
- // members
- type::AronEigenMatrixPtr type;
};
}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/EigenQuaternion.cpp b/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/EigenQuaternion.cpp
index 7ec0c21dbccc35c0ef62517787782e15c18ff33c..fbea54cbfe068e63adff2a16ef2fdb9dd168959e 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/EigenQuaternion.cpp
+++ b/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/EigenQuaternion.cpp
@@ -24,96 +24,63 @@
// Header
#include "EigenQuaternion.h"
+#include <SimoxUtility/algorithm/string/string_conversion.h>
+
namespace armarx::aron::typenavigator
{
// constructors
EigenQuaternionNavigator::EigenQuaternionNavigator(const Path& path) :
- aron::Navigator<type::Descriptor, type::AronType>::Navigator(type::Descriptor::eEigenQuaternion, path),
- Navigator(type::Descriptor::eEigenQuaternion, path),
- type(new type::AronEigenQuaternion(""))
+ aron::Navigator<type::Descriptor, type::AronType>::Navigator(type::Descriptor::eEigenQuaternion, path)
{
}
EigenQuaternionNavigator::EigenQuaternionNavigator(const type::AronEigenQuaternionPtr& o, const Path& path) :
aron::Navigator<type::Descriptor, type::AronType>::Navigator(type::Descriptor::eEigenQuaternion, path),
- Navigator(type::Descriptor::eEigenQuaternion, path),
- type(o)
+ detail::NavigatorBase<type::AronEigenQuaternion, EigenQuaternionNavigator>(o)
{
- CheckAronPtrForNull("AronEigenQuaternionTypeNavigator", "AronEigenQuaternionTypeNavigator", getPath(), o);
}
- bool EigenQuaternionNavigator::checkTypename(const std::string& s) const
+ // operators
+ bool EigenQuaternionNavigator::operator==(const EigenQuaternionNavigator& other) const
{
- if (s.empty())
+ if (getMaybe() != other.getMaybe())
{
- throw error::AronException("AronEigenQuaternionTypeNavigator", "checkTypename", "The typename is empty.", getPath());
+ return false;
}
+ return getTypename() == other.getTypename();
+ }
- if (!std::any_of(ACCEPTED_TYPES.begin(), ACCEPTED_TYPES.end(), [s](const auto & el)
+ std::string EigenQuaternionNavigator::checkTypename(const std::string& s) const
{
- return s == el.first || std::find(el.second.begin(), el.second.end(), s) != el.second.end();
- }))
+ for (const auto& [k, t] : ACCEPTED_TYPES)
{
- throw error::AronException("AronEigenQuaternionTypeNavigator", "checkTypename", "The typename is wrong. Got " + s, getPath());
+ if (s == k || std::find(t.begin(), t.end(), s) != t.end())
+ {
+ return k;
+ }
}
- return true;
+ throw error::StringNotValidException("EigenQuaternionNavigator", "checkTypename", "The typename is wrong.", s, getPath());
}
type::AronEigenQuaternionPtr EigenQuaternionNavigator::toAronEigenQuaternionPtr() const
{
- checkTypename(type->typeName);
- return type;
+ return this->aron;
}
std::string EigenQuaternionNavigator::getTypename() const
{
- return type->typeName;
- }
-
- std::vector<int> EigenQuaternionNavigator::getDimensions() const
- {
- return ACCEPTED_DIMENSION;
+ return this->aron->typeName;
}
void EigenQuaternionNavigator::setTypename(const std::string& u)
{
- checkTypename(u);
- type->typeName = u;
- }
-
- // static methods
- EigenQuaternionNavigatorPtr EigenQuaternionNavigator::DynamicCast(const NavigatorPtr& n)
- {
- return std::dynamic_pointer_cast<EigenQuaternionNavigator>(n);
- }
-
- EigenQuaternionNavigatorPtr EigenQuaternionNavigator::DynamicCastAndCheck(const NavigatorPtr& n)
- {
- CheckTypeNavigatorPtrForNull("AronEigenQuaternionTypeNavigator", "DynamicCast[Before]", n);
- EigenQuaternionNavigatorPtr casted = EigenQuaternionNavigator::DynamicCast(n);
- CheckTypeNavigatorPtrForNull("AronEigenQuaternionTypeNavigator", "DynamicCast[After]", n->getPath(), casted);
- return casted;
+ this->aron->typeName = checkTypename(u);
}
// virtual implementations
- std::vector<NavigatorPtr> EigenQuaternionNavigator::getChildren() const
- {
- return {};
- }
-
- size_t EigenQuaternionNavigator::childrenSize() const
- {
- return 0;
- }
-
- type::AronTypePtr EigenQuaternionNavigator::getResult() const
- {
- return toAronEigenQuaternionPtr();
- }
-
std::string EigenQuaternionNavigator::getName() const
{
- return "AronEigenQuaternionType<" + simox::alg::to_string(ACCEPTED_DIMENSION, ", ") + ", " + type->typeName + ">";
+ return "AronEigenQuaternionType<" + simox::alg::to_string(ACCEPTED_DIMENSION, ", ") + ", " + this->aron->typeName + ">";
}
}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/EigenQuaternion.h b/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/EigenQuaternion.h
index 1f388fdf5e75b3e58a962025e55cd038ad2b9394..5112f7f21b2017f4c84982520983a49a9eafa20f 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/EigenQuaternion.h
+++ b/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/EigenQuaternion.h
@@ -27,8 +27,8 @@
#include <string>
#include <map>
-// Base Class
-#include <RobotAPI/libraries/aron/core/navigator/type/Navigator.h>
+// Base class
+#include "../detail/NDArrayNavigatorBase.h"
namespace armarx::aron::typenavigator
{
@@ -36,34 +36,26 @@ namespace armarx::aron::typenavigator
typedef std::shared_ptr<EigenQuaternionNavigator> EigenQuaternionNavigatorPtr;
class EigenQuaternionNavigator :
- virtual public Navigator
+ virtual public detail::NDArrayNavigatorBase<type::AronEigenQuaternion, EigenQuaternionNavigator>
{
- public:
- using PointerType = EigenQuaternionNavigatorPtr;
-
public:
// constructors
- EigenQuaternionNavigator() = delete;
- EigenQuaternionNavigator(const Path& path);
+ EigenQuaternionNavigator(const Path& path = Path());
EigenQuaternionNavigator(const type::AronEigenQuaternionPtr&, const Path& path);
- bool checkTypename(const std::string&) const;
+ // operators
+ virtual bool operator==(const EigenQuaternionNavigator&) const override;
+
+ // public member functions
+ std::string checkTypename(const std::string&) const;
std::string getTypename() const;
- std::vector<int> getDimensions() const;
void setTypename(const std::string&);
type::AronEigenQuaternionPtr toAronEigenQuaternionPtr() const;
- // static methods
- static EigenQuaternionNavigatorPtr DynamicCast(const NavigatorPtr& n);
- static EigenQuaternionNavigatorPtr DynamicCastAndCheck(const NavigatorPtr& n);
-
// virtual implementations
- virtual std::vector<NavigatorPtr> getChildren() const override;
- virtual size_t childrenSize() const override;
- virtual type::AronTypePtr getResult() const override;
virtual std::string getName() const override;
public:
@@ -73,9 +65,5 @@ namespace armarx::aron::typenavigator
{"double", {}}
};
const std::vector<int> ACCEPTED_DIMENSION = {1, 4};
-
- private:
- // members
- type::AronEigenQuaternionPtr type;
};
}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/IVTCByteImage.cpp b/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/IVTCByteImage.cpp
index 8d76320d402b315e5b06db15c6bbd85986da5da3..bc09aaf12feb336eddd479ccfebb3b000dd1980d 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/IVTCByteImage.cpp
+++ b/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/IVTCByteImage.cpp
@@ -28,122 +28,35 @@ namespace armarx::aron::typenavigator
{
// constructors
IVTCByteImageNavigator::IVTCByteImageNavigator(const Path& path) :
- aron::Navigator<type::Descriptor, type::AronType>::Navigator(type::Descriptor::eIVTCByteImage, path),
- Navigator(type::Descriptor::eIVTCByteImage, path),
- type(new type::AronIVTCByteImage(0, 0, ""))
+ aron::Navigator<type::Descriptor, type::AronType>::Navigator(type::Descriptor::eIVTCByteImage, path)
{
}
IVTCByteImageNavigator::IVTCByteImageNavigator(const type::AronIVTCByteImagePtr& o, const Path& path) :
aron::Navigator<type::Descriptor, type::AronType>::Navigator(type::Descriptor::eIVTCByteImage, path),
- Navigator(type::Descriptor::eIVTCByteImage, path),
- type(o)
+ detail::NavigatorBase<type::AronIVTCByteImage, IVTCByteImageNavigator>(o)
{
- CheckAronPtrForNull("AronIVTCByteImageTypeNavigator", "AronIVTCByteImageTypeNavigator", getPath(), o);
}
- std::string IVTCByteImageNavigator::checkTypename(const std::string& s) const
+ // operators
+ bool IVTCByteImageNavigator::operator==(const IVTCByteImageNavigator& other) const
{
- for (const auto& [key, list] : ACCEPTED_TYPES)
+ if (getMaybe() != other.getMaybe())
{
- if (s == key)
- {
- return key;
- }
-
- for (const auto& el : list)
- {
- if (s == el)
- {
- return key;
- }
- }
+ return false;
}
- throw error::AronException("AronIVTCByteImageTypeNavigator", "checkTypename", "The typename is empty.", getPath());
+ return true;
}
type::AronIVTCByteImagePtr IVTCByteImageNavigator::toAronIVTCByteImagePtr() const
{
- checkTypename(type->typeName);
- return type;
- }
-
- std::vector<int> IVTCByteImageNavigator::getDimensions() const
- {
- return {type->width, type->height};
- }
-
- unsigned int IVTCByteImageNavigator::getWidth() const
- {
- return type->width;
- }
-
- unsigned int IVTCByteImageNavigator::getHeight() const
- {
- return type->height;
- }
-
- std::string IVTCByteImageNavigator::getTypename() const
- {
- return type->typeName;
- }
-
- void IVTCByteImageNavigator::setWidth(const unsigned int& w)
- {
- if (w == 0)
- {
- throw error::AronException("AronIVTCByteImageTypeNavigator", "setWidth", "The width of an image cannot be 0", getPath());
- }
- type->width = w;
- }
-
- void IVTCByteImageNavigator::setHeight(const unsigned int& h)
- {
- if (h == 0)
- {
- throw error::AronException("AronIVTCByteImageTypeNavigator", "setHeight", "The height of an image cannot be 0", getPath());
- }
- type->height = h;
- }
-
- void IVTCByteImageNavigator::setTypename(const std::string& u)
- {
- type->typeName = checkTypename(u);
- }
-
- // static methods
- IVTCByteImageNavigatorPtr IVTCByteImageNavigator::DynamicCast(const NavigatorPtr& n)
- {
- return std::dynamic_pointer_cast<IVTCByteImageNavigator>(n);
- }
-
- IVTCByteImageNavigatorPtr IVTCByteImageNavigator::DynamicCastAndCheck(const NavigatorPtr& n)
- {
- CheckTypeNavigatorPtrForNull("AronIVTCByteImageTypeNavigator", "DynamicCast[Before]", n);
- IVTCByteImageNavigatorPtr casted = IVTCByteImageNavigator::DynamicCast(n);
- CheckTypeNavigatorPtrForNull("AronIVTCByteImageTypeNavigator", "DynamicCast[After]", n->getPath(), casted);
- return casted;
+ return this->aron;
}
// virtual implementations
- std::vector<NavigatorPtr> IVTCByteImageNavigator::getChildren() const
- {
- return {};
- }
-
- size_t IVTCByteImageNavigator::childrenSize() const
- {
- return 0;
- }
-
- type::AronTypePtr IVTCByteImageNavigator::getResult() const
- {
- return toAronIVTCByteImagePtr();
- }
-
std::string IVTCByteImageNavigator::getName() const
{
- return "AronIVTCByteImageType<" + simox::alg::to_string(getDimensions(), ", ") + ", " + type->typeName + ">";
+ return "AronIVTCByteImageType";
}
}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/IVTCByteImage.h b/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/IVTCByteImage.h
index aeaf5a3c7f8057cda95706a91637f11ea25fec3e..83927a86d1baf61b98d443086539fb32f638d2fe 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/IVTCByteImage.h
+++ b/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/IVTCByteImage.h
@@ -27,8 +27,8 @@
#include <string>
#include <map>
-// Base Class
-#include <RobotAPI/libraries/aron/core/navigator/type/Navigator.h>
+// Base class
+#include "../detail/NDArrayNavigatorBase.h"
namespace armarx::aron::typenavigator
{
@@ -36,38 +36,20 @@ namespace armarx::aron::typenavigator
typedef std::shared_ptr<IVTCByteImageNavigator> IVTCByteImageNavigatorPtr;
class IVTCByteImageNavigator :
- virtual public Navigator
+ virtual public detail::NDArrayNavigatorBase<type::AronIVTCByteImage, IVTCByteImageNavigator>
{
- public:
- using PointerType = IVTCByteImageNavigatorPtr;
-
public:
// constructors
- IVTCByteImageNavigator() = delete;
- IVTCByteImageNavigator(const Path& path);
+ IVTCByteImageNavigator(const Path& path = Path());
IVTCByteImageNavigator(const type::AronIVTCByteImagePtr&, const Path& path);
- std::string checkTypename(const std::string&) const;
-
- unsigned int getWidth() const;
- unsigned int getHeight() const;
- std::string getTypename() const;
- std::vector<int> getDimensions() const;
-
- void setWidth(const unsigned int&);
- void setHeight(const unsigned int&);
- void setTypename(const std::string&);
+ // operators
+ virtual bool operator==(const IVTCByteImageNavigator&) const override;
+ // public member functions
type::AronIVTCByteImagePtr toAronIVTCByteImagePtr() const;
- // static methods
- static IVTCByteImageNavigatorPtr DynamicCast(const NavigatorPtr& n);
- static IVTCByteImageNavigatorPtr DynamicCastAndCheck(const NavigatorPtr& n);
-
// virtual implementations
- virtual std::vector<NavigatorPtr> getChildren() const override;
- virtual size_t childrenSize() const override;
- virtual type::AronTypePtr getResult() const override;
virtual std::string getName() const override;
public:
@@ -77,9 +59,5 @@ namespace armarx::aron::typenavigator
{"RGB24", {"CByteImage::ImageType::eRGB24", "ImageType::eRGB24"}},
{"RGB24Split", {"CByteImage::ImageType::eRGB24Split", "ImageType::eRGB24Split"}}
};
-
- private:
- // members
- type::AronIVTCByteImagePtr type;
};
}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/NDArray.cpp b/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/NDArray.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..14b7552ccbfdadd718113f99d62c40707bda7929
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/NDArray.cpp
@@ -0,0 +1,97 @@
+/*
+ * 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 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
+ */
+
+// Header
+#include "NDArray.h"
+
+namespace armarx::aron::typenavigator
+{
+ // constructors
+ NDArrayNavigator::NDArrayNavigator(const Path& path) :
+ aron::Navigator<type::Descriptor, type::AronType>::Navigator(type::Descriptor::eNDArray, path)
+ {
+ }
+
+ NDArrayNavigator::NDArrayNavigator(const type::AronNDArrayPtr& o, const Path& path) :
+ aron::Navigator<type::Descriptor, type::AronType>::Navigator(type::Descriptor::eNDArray, path),
+ detail::NavigatorBase<type::AronNDArray, NDArrayNavigator>(o)
+ {
+ }
+
+ // operators
+ bool NDArrayNavigator::operator==(const NDArrayNavigator& other) const
+ {
+ if (getMaybe() != other.getMaybe())
+ {
+ return false;
+ }
+ return getDimensions() == other.getDimensions() && getElementSize() == other.getElementSize() && getTypename() == other.getTypename();
+ }
+
+ type::AronNDArrayPtr NDArrayNavigator::toAronNDArrayPtr() const
+ {
+ return this->aron;
+ }
+
+ std::vector<int> NDArrayNavigator::getDimensions() const
+ {
+ return this->aron->dimensions;
+ }
+
+ std::string NDArrayNavigator::getTypename() const
+ {
+ return this->aron->typeName;
+ }
+
+ unsigned int NDArrayNavigator::getElementSize() const
+ {
+ return 0; //type->elementSize;
+ }
+
+ void NDArrayNavigator::setTypename(const std::string& s)
+ {
+ this->aron->typeName = s;
+ }
+
+ void NDArrayNavigator::setDimensions(const std::vector<int>& v)
+ {
+ this->aron->dimensions = v;
+ }
+
+ void NDArrayNavigator::addDimension(int i)
+ {
+ this->aron->dimensions.push_back(i);
+ }
+
+ void NDArrayNavigator::setElementSize(unsigned int i)
+ {
+ //type->elementSize = i;
+ }
+
+ // virtual implementations
+ std::string NDArrayNavigator::getName() const
+ {
+ return "AronNDArrayType";
+ }
+}
+
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/NDArray.h b/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/NDArray.h
new file mode 100644
index 0000000000000000000000000000000000000000..b4deca5fe53786f98e8502ce9401985ee2f02eb6
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/NDArray.h
@@ -0,0 +1,64 @@
+/*
+ * 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 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
+ */
+
+#pragma once
+
+// STD/STL
+#include <string>
+#include <map>
+
+// Base class
+#include "../detail/NDArrayNavigatorBase.h"
+
+namespace armarx::aron::typenavigator
+{
+ class NDArrayNavigator;
+ typedef std::shared_ptr<NDArrayNavigator> NDArrayNavigatorPtr;
+
+ class NDArrayNavigator :
+ virtual public detail::NDArrayNavigatorBase<type::AronNDArray, NDArrayNavigator>
+ {
+ public:
+ // constructors
+ NDArrayNavigator(const Path& path = Path());
+ NDArrayNavigator(const type::AronNDArrayPtr&, const Path& path = Path());
+
+ // operators
+ virtual bool operator==(const NDArrayNavigator&) const override;
+
+ // public member functions
+ std::string getTypename() const;
+ std::vector<int> getDimensions() const;
+ unsigned int getElementSize() const;
+
+ void setTypename(const std::string&);
+ void setDimensions(const std::vector<int>&);
+ void addDimension(int);
+ void setElementSize(unsigned int);
+
+ type::AronNDArrayPtr toAronNDArrayPtr() const;
+
+ // virtual implementations
+ virtual std::string getName() const override;
+ };
+}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/OpenCVMat.cpp b/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/OpenCVMat.cpp
index e0d159cc3fc108c2bfb14d15fa6846e6c97d654f..2d41e10ab7c13af0fbfcba8d59919830ce37dd1a 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/OpenCVMat.cpp
+++ b/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/OpenCVMat.cpp
@@ -28,121 +28,35 @@ namespace armarx::aron::typenavigator
{
// constructors
OpenCVMatNavigator::OpenCVMatNavigator(const Path& path) :
- aron::Navigator<type::Descriptor, type::AronType>::Navigator(type::Descriptor::eOpenCVMat, path),
- Navigator(type::Descriptor::eOpenCVMat, path),
- type(new type::AronOpenCVMat({}, ""))
+ aron::Navigator<type::Descriptor, type::AronType>::Navigator(type::Descriptor::eOpenCVMat, path)
{
}
OpenCVMatNavigator::OpenCVMatNavigator(const type::AronOpenCVMatPtr& o, const Path& path) :
aron::Navigator<type::Descriptor, type::AronType>::Navigator(type::Descriptor::eOpenCVMat, path),
- Navigator(type::Descriptor::eOpenCVMat, path),
- type(o)
+ detail::NavigatorBase<type::AronOpenCVMat, OpenCVMatNavigator>(o)
{
- CheckAronPtrForNull("AronOpenCVMatTypeNavigator", "AronOpenCVMatTypeNavigator", getPath(), o);
- checkDimensions(type->dimensions);
- checkTypename(type->typeName);
}
- bool OpenCVMatNavigator::checkDimensions(const std::vector<int>& d) const
- {
- if (d.size() < ACCEPTED_DIMENSION_MIN_SIZE)
- {
- throw error::AronException("AronOpenCVMatTypeNavigator", "checkDimensions", "The dimension size is wrong. Got size: " + std::to_string(d.size()), getPath());
- }
-
- if (std::any_of(d.begin(), d.end(), [](int i)
+ // operators
+ bool OpenCVMatNavigator::operator==(const OpenCVMatNavigator& other) const
{
- return i < -1 || i == 0;
- }))
+ if (getMaybe() != other.getMaybe())
{
- throw error::AronException("AronOpenCVMatTypeNavigator", "checkDimensions", "The dimension size is wrong. At least one empty is < -1 or 0. Got as dimension: " + simox::alg::to_string(d, ", "), getPath());
+ return false;
}
return true;
}
- std::string OpenCVMatNavigator::checkTypename(const std::string& s) const
- {
- for (const auto& [key, list] : ACCEPTED_TYPES)
- {
- if (s == key)
- {
- return key;
- }
-
- for (const auto& el : list)
- {
- if (s == el)
- {
- return key;
- }
- }
- }
- throw error::AronException("OpenCVMatNavigator", "checkTypename", "The typename is empty.", getPath());
- }
-
type::AronOpenCVMatPtr OpenCVMatNavigator::toAronOpenCVMatPtr() const
{
- checkDimensions(type->dimensions);
- checkTypename(type->typeName);
- return type;
- }
-
- std::string OpenCVMatNavigator::getTypename() const
- {
- return type->typeName;
- }
-
- std::vector<int> OpenCVMatNavigator::getDimensions() const
- {
- return type->dimensions;
- }
-
- void OpenCVMatNavigator::setTypename(const std::string& u)
- {
- type->typeName = checkTypename(u);
- }
-
- void OpenCVMatNavigator::setDimensions(const std::vector<int>& d)
- {
- checkDimensions(d);
- type->dimensions = d;
- }
-
-
- // static methods
- OpenCVMatNavigatorPtr OpenCVMatNavigator::DynamicCast(const NavigatorPtr& n)
- {
- return std::dynamic_pointer_cast<OpenCVMatNavigator>(n);
- }
-
- OpenCVMatNavigatorPtr OpenCVMatNavigator::DynamicCastAndCheck(const NavigatorPtr& n)
- {
- CheckTypeNavigatorPtrForNull("AronOpenCVMatTypeNavigator", "DynamicCast[Before]", n);
- OpenCVMatNavigatorPtr casted = OpenCVMatNavigator::DynamicCast(n);
- CheckTypeNavigatorPtrForNull("AronOpenCVMatTypeNavigator", "DynamicCast[After]", n->getPath(), casted);
- return casted;
+ return this->aron;
}
// virtual implementations
- std::vector<NavigatorPtr> OpenCVMatNavigator::getChildren() const
- {
- return {};
- }
-
- size_t OpenCVMatNavigator::childrenSize() const
- {
- return 0;
- }
-
- type::AronTypePtr OpenCVMatNavigator::getResult() const
- {
- return toAronOpenCVMatPtr();
- }
-
std::string OpenCVMatNavigator::getName() const
{
- return "AronOpenCVMatType<" + simox::alg::to_string(type->dimensions, ", ") + ", " + type->typeName + ">";
+ return "AronOpenCVMatType";
}
}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/OpenCVMat.h b/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/OpenCVMat.h
index 4a1c0a88fb65404a0d0eab15ca54d0129af180d7..74e4e285540a66823409c8272b707d825e78ba40 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/OpenCVMat.h
+++ b/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/OpenCVMat.h
@@ -27,8 +27,8 @@
#include <string>
#include <map>
-// Base Class
-#include <RobotAPI/libraries/aron/core/navigator/type/Navigator.h>
+// Base class
+#include "../detail/NDArrayNavigatorBase.h"
namespace armarx::aron::typenavigator
{
@@ -36,36 +36,20 @@ namespace armarx::aron::typenavigator
typedef std::shared_ptr<OpenCVMatNavigator> OpenCVMatNavigatorPtr;
class OpenCVMatNavigator :
- virtual public Navigator
+ virtual public detail::NDArrayNavigatorBase<type::AronOpenCVMat, OpenCVMatNavigator>
{
- public:
- using PointerType = OpenCVMatNavigatorPtr;
-
public:
// constructors
- OpenCVMatNavigator() = delete;
- OpenCVMatNavigator(const Path& path);
+ OpenCVMatNavigator(const Path& path = Path());
OpenCVMatNavigator(const type::AronOpenCVMatPtr&, const Path& path);
- bool checkDimensions(const std::vector<int>&) const;
- std::string checkTypename(const std::string&) const;
-
- std::string getTypename() const;
- std::vector<int> getDimensions() const;
-
- void setDimensions(const std::vector<int>&);
- void setTypename(const std::string&);
+ // operators
+ virtual bool operator==(const OpenCVMatNavigator&) const override;
+ // public member functions
type::AronOpenCVMatPtr toAronOpenCVMatPtr() const;
- // static methods
- static OpenCVMatNavigatorPtr DynamicCast(const NavigatorPtr& n);
- static OpenCVMatNavigatorPtr DynamicCastAndCheck(const NavigatorPtr& n);
-
// virtual implementations
- virtual std::vector<NavigatorPtr> getChildren() const override;
- virtual size_t childrenSize() const override;
- virtual type::AronTypePtr getResult() const override;
virtual std::string getName() const override;
public:
@@ -79,10 +63,5 @@ namespace armarx::aron::typenavigator
{"CV_32F", {"32F"}},
{"CV_64F", {"64F"}}
};
- const unsigned int ACCEPTED_DIMENSION_MIN_SIZE = 2;
-
- private:
- // members
- type::AronOpenCVMatPtr type;
};
}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/Orientation.cpp b/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/Orientation.cpp
index 13be796db0958e73425151df684488dbcc49d86e..79cbe06d5560b27acd3a8e790b9bcbff9565b9cd 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/Orientation.cpp
+++ b/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/Orientation.cpp
@@ -28,65 +28,32 @@ namespace armarx::aron::typenavigator
{
// constructors
OrientationNavigator::OrientationNavigator(const Path& path) :
- aron::Navigator<type::Descriptor, type::AronType>::Navigator(type::Descriptor::eOrientation, path),
- Navigator(type::Descriptor::eOrientation, path),
- type(new type::AronOrientation())
+ aron::Navigator<type::Descriptor, type::AronType>::Navigator(type::Descriptor::eOrientation, path)
{
}
OrientationNavigator::OrientationNavigator(const type::AronOrientationPtr& o, const Path& path) :
aron::Navigator<type::Descriptor, type::AronType>::Navigator(type::Descriptor::eOrientation, path),
- Navigator(type::Descriptor::eOrientation, path),
- type(o)
- {
- CheckAronPtrForNull("AronOrientationTypeNavigator", "AronOrientationTypeNavigator", getPath(), o);
- }
-
- type::AronOrientationPtr OrientationNavigator::toAronOrientationPtr() const
- {
- return type;
- }
-
- std::vector<int> OrientationNavigator::getDimensions() const
- {
- return ACCEPTED_DIMENSION;
- }
-
- std::string OrientationNavigator::getTypename() const
+ detail::NavigatorBase<type::AronOrientation, OrientationNavigator>(o)
{
- return ACCEPTED_TYPE;
}
- // static methods
- OrientationNavigatorPtr OrientationNavigator::DynamicCast(const NavigatorPtr& n)
+ // operators
+ bool OrientationNavigator::operator==(const OrientationNavigator& other) const
{
- return std::dynamic_pointer_cast<OrientationNavigator>(n);
+ if (getMaybe() != other.getMaybe())
+ {
+ return false;
+ }
+ return true;
}
- OrientationNavigatorPtr OrientationNavigator::DynamicCastAndCheck(const NavigatorPtr& n)
+ type::AronOrientationPtr OrientationNavigator::toAronOrientationPtr() const
{
- CheckTypeNavigatorPtrForNull("AronOrientationTypeNavigator", "DynamicCast[Before]", n);
- OrientationNavigatorPtr casted = OrientationNavigator::DynamicCast(n);
- CheckTypeNavigatorPtrForNull("AronOrientationTypeNavigator", "DynamicCast[After]", n->getPath(), casted);
- return casted;
+ return this->aron;
}
// virtual implementations
- std::vector<NavigatorPtr> OrientationNavigator::getChildren() const
- {
- return {};
- }
-
- size_t OrientationNavigator::childrenSize() const
- {
- return 0;
- }
-
- type::AronTypePtr OrientationNavigator::getResult() const
- {
- return toAronOrientationPtr();
- }
-
std::string OrientationNavigator::getName() const
{
return "AronOrientationType";
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/Orientation.h b/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/Orientation.h
index 4c9c9929b7b466b14d54617be897d01562d2e39d..6b2df5228b77f93395fd2c653718c2c64637266a 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/Orientation.h
+++ b/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/Orientation.h
@@ -27,8 +27,8 @@
#include <string>
#include <map>
-// Base Class
-#include <RobotAPI/libraries/aron/core/navigator/type/Navigator.h>
+// Base class
+#include "../detail/NDArrayNavigatorBase.h"
namespace armarx::aron::typenavigator
{
@@ -36,41 +36,24 @@ namespace armarx::aron::typenavigator
typedef std::shared_ptr<OrientationNavigator> OrientationNavigatorPtr;
class OrientationNavigator :
- virtual public Navigator
+ virtual public detail::NDArrayNavigatorBase<type::AronOrientation, OrientationNavigator>
{
- public:
- using PointerType = OrientationNavigatorPtr;
-
public:
// constructors
- OrientationNavigator() = delete;
- OrientationNavigator(const Path& path);
+ OrientationNavigator(const Path& path = Path());
OrientationNavigator(const type::AronOrientationPtr&, const Path& path);
- bool checkDimensions(const std::vector<int>&) const;
- bool checkTypename(const std::string&) const;
-
- std::string getTypename() const;
- std::vector<int> getDimensions() const;
+ // operators
+ virtual bool operator==(const OrientationNavigator&) const override;
+ // public member functions
type::AronOrientationPtr toAronOrientationPtr() const;
- // static methods
- static OrientationNavigatorPtr DynamicCast(const NavigatorPtr& n);
- static OrientationNavigatorPtr DynamicCastAndCheck(const NavigatorPtr& n);
-
// virtual implementations
- virtual std::vector<NavigatorPtr> getChildren() const override;
- virtual size_t childrenSize() const override;
- virtual type::AronTypePtr getResult() const override;
virtual std::string getName() const override;
public:
const std::string ACCEPTED_TYPE = "float";
const std::vector<int> ACCEPTED_DIMENSION = {1, 4};
-
- private:
- // members
- type::AronOrientationPtr type;
};
}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/PCLPointCloud.cpp b/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/PCLPointCloud.cpp
index 2de0158a82ac9243202becc23a12272ebed0cd43..76d8543d0f2348341ab5972214bc86e4205554b7 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/PCLPointCloud.cpp
+++ b/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/PCLPointCloud.cpp
@@ -28,19 +28,25 @@ namespace armarx::aron::typenavigator
{
// constructors
PCLPointCloudNavigator::PCLPointCloudNavigator(const Path& path) :
- aron::Navigator<type::Descriptor, type::AronType>::Navigator(type::Descriptor::ePCLPointCloud, path),
- Navigator(type::Descriptor::ePCLPointCloud, path),
- type(new type::AronPCLPointCloud(0, 0, ""))
+ aron::Navigator<type::Descriptor, type::AronType>::Navigator(type::Descriptor::ePCLPointCloud, path)
{
}
PCLPointCloudNavigator::PCLPointCloudNavigator(const type::AronPCLPointCloudPtr& o, const Path& path) :
aron::Navigator<type::Descriptor, type::AronType>::Navigator(type::Descriptor::ePCLPointCloud, path),
- Navigator(type::Descriptor::ePCLPointCloud, path),
- type(o)
+ detail::NavigatorBase<type::AronPCLPointCloud, PCLPointCloudNavigator>(o)
+ {
+ checkTypename(this->aron->typeName);
+ }
+
+ // operators
+ bool PCLPointCloudNavigator::operator==(const PCLPointCloudNavigator& other) const
{
- CheckAronPtrForNull("AronPCLPointCloudTypeNavigator", "AronPCLPointCloudTypeNavigator", getPath(), o);
- checkTypename(type->typeName);
+ if (getMaybe() != other.getMaybe())
+ {
+ return false;
+ }
+ return getTypename() == other.getTypename();
}
std::string PCLPointCloudNavigator::checkTypename(const std::string& s) const
@@ -60,92 +66,28 @@ namespace armarx::aron::typenavigator
}
}
}
- throw error::AronException("PCLPointCloudNavigator", "checkTypename", "The typename is empty.", getPath());
+ throw error::StringNotValidException("PCLPointCloudNavigator", "checkTypename", "The typename is wrong.", s, getPath());
}
type::AronPCLPointCloudPtr PCLPointCloudNavigator::toAronPCLPointCloudPtr() const
{
- checkTypename(type->typeName);
- return type;
- }
-
- std::vector<int> PCLPointCloudNavigator::getDimensions() const
- {
- return {type->width, type->height};
- }
-
- unsigned int PCLPointCloudNavigator::getWidth() const
- {
- return type->width;
- }
-
- unsigned int PCLPointCloudNavigator::getHeight() const
- {
- return type->height;
+ return this->aron;
}
std::string PCLPointCloudNavigator::getTypename() const
{
- return type->typeName;
- }
-
- void PCLPointCloudNavigator::setWidth(const unsigned int& w)
- {
- if (w == 0)
- {
- throw error::AronException("AronPCLPointCloudTypeNavigator", "setWidth", "The width of aa pointcloud cannot be 0", getPath());
- }
- type->width = w;
- }
-
- void PCLPointCloudNavigator::setHeight(const unsigned int& h)
- {
- if (h == 0)
- {
- throw error::AronException("AronPCLPointCloudTypeNavigator", "setHeight", "The height of aa pointcloud cannot be 0", getPath());
- }
- type->height = h;
+ return this->aron->typeName;
}
void PCLPointCloudNavigator::setTypename(const std::string& u)
{
- type->typeName = checkTypename(u);
- }
-
-
- // static methods
- PCLPointCloudNavigatorPtr PCLPointCloudNavigator::DynamicCast(const NavigatorPtr& n)
- {
- return std::dynamic_pointer_cast<PCLPointCloudNavigator>(n);
- }
-
- PCLPointCloudNavigatorPtr PCLPointCloudNavigator::DynamicCastAndCheck(const NavigatorPtr& n)
- {
- CheckTypeNavigatorPtrForNull("AronPCLPointCloudTypeNavigator", "DynamicCast[Before]", n);
- PCLPointCloudNavigatorPtr casted = PCLPointCloudNavigator::DynamicCast(n);
- CheckTypeNavigatorPtrForNull("AronPCLPointCloudTypeNavigator", "DynamicCast[After]", n->getPath(), casted);
- return casted;
+ this->aron->typeName = checkTypename(u);
}
// virtual implementations
- std::vector<NavigatorPtr> PCLPointCloudNavigator::getChildren() const
- {
- return {};
- }
-
- size_t PCLPointCloudNavigator::childrenSize() const
- {
- return 0;
- }
-
- type::AronTypePtr PCLPointCloudNavigator::getResult() const
- {
- return toAronPCLPointCloudPtr();
- }
-
std::string PCLPointCloudNavigator::getName() const
{
- return "AronPCLPointCloudType<" + simox::alg::to_string(getDimensions(), ", ") + ", " + type->typeName + ">";
+ return "AronPCLPointCloudType<" + this->aron->typeName + ">";
}
}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/PCLPointCloud.h b/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/PCLPointCloud.h
index 2f62f1b579b5e84b3acc93aa7a72d70329492736..45cd8b8680af13101403820fe6d62447ed78b522 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/PCLPointCloud.h
+++ b/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/PCLPointCloud.h
@@ -27,8 +27,8 @@
#include <string>
#include <map>
-// Base Class
-#include <RobotAPI/libraries/aron/core/navigator/type/Navigator.h>
+// Base class
+#include "../detail/NDArrayNavigatorBase.h"
namespace armarx::aron::typenavigator
{
@@ -36,38 +36,26 @@ namespace armarx::aron::typenavigator
typedef std::shared_ptr<PCLPointCloudNavigator> PCLPointCloudNavigatorPtr;
class PCLPointCloudNavigator :
- virtual public Navigator
+ virtual public detail::NDArrayNavigatorBase<type::AronPCLPointCloud, PCLPointCloudNavigator>
{
- public:
- using PointerType = PCLPointCloudNavigatorPtr;
-
public:
// constructors
- PCLPointCloudNavigator() = delete;
- PCLPointCloudNavigator(const Path& path);
+ PCLPointCloudNavigator(const Path& path = Path());
PCLPointCloudNavigator(const type::AronPCLPointCloudPtr&, const Path& path);
+ // operators
+ virtual bool operator==(const PCLPointCloudNavigator&) const override;
+
+ // public member functions
std::string checkTypename(const std::string&) const;
- unsigned int getWidth() const;
- unsigned int getHeight() const;
std::string getTypename() const;
- std::vector<int> getDimensions() const;
- void setWidth(const unsigned int&);
- void setHeight(const unsigned int&);
void setTypename(const std::string&);
type::AronPCLPointCloudPtr toAronPCLPointCloudPtr() const;
- // static methods
- static PCLPointCloudNavigatorPtr DynamicCast(const NavigatorPtr& n);
- static PCLPointCloudNavigatorPtr DynamicCastAndCheck(const NavigatorPtr& n);
-
// virtual implementations
- virtual std::vector<NavigatorPtr> getChildren() const override;
- virtual size_t childrenSize() const override;
- virtual type::AronTypePtr getResult() const override;
virtual std::string getName() const override;
public:
@@ -83,9 +71,5 @@ namespace armarx::aron::typenavigator
};
const unsigned int ACCEPTED_DIMENSION_MIN_SIZE = 2;
const unsigned int ACCEPTED_DIMENSION_MAX_SIZE = 2;
-
- private:
- // members
- type::AronPCLPointCloudPtr type;
};
}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/Pose.cpp b/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/Pose.cpp
index 8713907ec36228dced593baf15dcef2f35d30cd8..4db19f217776ba7e2eb68852c04fd412b3d6090d 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/Pose.cpp
+++ b/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/Pose.cpp
@@ -28,65 +28,32 @@ namespace armarx::aron::typenavigator
{
// constructors
PoseNavigator::PoseNavigator(const Path& path) :
- aron::Navigator<type::Descriptor, type::AronType>::Navigator(type::Descriptor::ePose, path),
- Navigator(type::Descriptor::ePose, path),
- type(new type::AronPose())
+ aron::Navigator<type::Descriptor, type::AronType>::Navigator(type::Descriptor::ePose, path)
{
}
PoseNavigator::PoseNavigator(const type::AronPosePtr& o, const Path& path) :
aron::Navigator<type::Descriptor, type::AronType>::Navigator(type::Descriptor::ePose, path),
- Navigator(type::Descriptor::ePose, path),
- type(o)
- {
- CheckAronPtrForNull("AronPoseTypeNavigator", "AronPoseTypeNavigator", getPath(), o);
- }
-
- type::AronPosePtr PoseNavigator::toAronPosePtr() const
- {
- return type;
- }
-
- std::vector<int> PoseNavigator::getDimensions() const
- {
- return ACCEPTED_DIMENSION;
- }
-
- std::string PoseNavigator::getTypename() const
+ detail::NavigatorBase<type::AronPose, PoseNavigator>(o)
{
- return ACCEPTED_TYPE;
}
- // static methods
- PoseNavigatorPtr PoseNavigator::DynamicCast(const NavigatorPtr& n)
+ // operators
+ bool PoseNavigator::operator==(const PoseNavigator& other) const
{
- return std::dynamic_pointer_cast<PoseNavigator>(n);
+ if (getMaybe() != other.getMaybe())
+ {
+ return false;
+ }
+ return true;
}
- PoseNavigatorPtr PoseNavigator::DynamicCastAndCheck(const NavigatorPtr& n)
+ type::AronPosePtr PoseNavigator::toAronPosePtr() const
{
- CheckTypeNavigatorPtrForNull("AronPoseTypeNavigator", "DynamicCast[Before]", n);
- PoseNavigatorPtr casted = PoseNavigator::DynamicCast(n);
- CheckTypeNavigatorPtrForNull("AronPoseTypeNavigator", "DynamicCast[After]", n->getPath(), casted);
- return casted;
+ return this->aron;
}
// virtual implementations
- std::vector<NavigatorPtr> PoseNavigator::getChildren() const
- {
- return {};
- }
-
- size_t PoseNavigator::childrenSize() const
- {
- return 0;
- }
-
- type::AronTypePtr PoseNavigator::getResult() const
- {
- return toAronPosePtr();
- }
-
std::string PoseNavigator::getName() const
{
return "AronPoseType";
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/Pose.h b/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/Pose.h
index e99d0ea3bd3ac35bbbbfe1b8e493d5ae8eafb1bf..6277f027273049e98d6e1c8b18ac29c36ed5e23b 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/Pose.h
+++ b/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/Pose.h
@@ -27,8 +27,8 @@
#include <string>
#include <map>
-// Base Class
-#include <RobotAPI/libraries/aron/core/navigator/type/Navigator.h>
+// Base class
+#include "../detail/NDArrayNavigatorBase.h"
namespace armarx::aron::typenavigator
{
@@ -36,37 +36,24 @@ namespace armarx::aron::typenavigator
typedef std::shared_ptr<PoseNavigator> PoseNavigatorPtr;
class PoseNavigator :
- virtual public Navigator
+ virtual public detail::NDArrayNavigatorBase<type::AronPose, PoseNavigator>
{
- public:
- using PointerType = PoseNavigatorPtr;
-
public:
// constructors
PoseNavigator(const Path& path = Path());
PoseNavigator(const type::AronPosePtr&, const Path& path = Path());
- std::string getTypename() const;
- std::vector<int> getDimensions() const;
+ // operators
+ virtual bool operator==(const PoseNavigator&) const override;
+ // public member functions
type::AronPosePtr toAronPosePtr() const;
- // static methods
- static PoseNavigatorPtr DynamicCast(const NavigatorPtr& n);
- static PoseNavigatorPtr DynamicCastAndCheck(const NavigatorPtr& n);
-
// virtual implementations
- virtual std::vector<NavigatorPtr> getChildren() const override;
- virtual size_t childrenSize() const override;
- virtual type::AronTypePtr getResult() const override;
virtual std::string getName() const override;
public:
const std::string ACCEPTED_TYPE = "float";
const std::vector<int> ACCEPTED_DIMENSION = {4, 4};
-
- private:
- // members
- type::AronPosePtr type;
};
}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/Position.cpp b/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/Position.cpp
index 2172f431b17cbcad41704c1933e78f31758b4ca8..ac46a301a4fb8514a8c5a1d49c7ee524333ef3fd 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/Position.cpp
+++ b/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/Position.cpp
@@ -28,65 +28,32 @@ namespace armarx::aron::typenavigator
{
// constructors
PositionNavigator::PositionNavigator(const Path& path) :
- aron::Navigator<type::Descriptor, type::AronType>::Navigator(type::Descriptor::ePosition, path),
- Navigator(type::Descriptor::ePosition, path),
- type(new type::AronPosition())
+ aron::Navigator<type::Descriptor, type::AronType>::Navigator(type::Descriptor::ePosition, path)
{
}
PositionNavigator::PositionNavigator(const type::AronPositionPtr& o, const Path& path) :
aron::Navigator<type::Descriptor, type::AronType>::Navigator(type::Descriptor::ePosition, path),
- Navigator(type::Descriptor::ePosition, path),
- type(o)
- {
- CheckAronPtrForNull("AronPositionTypeNavigator", "AronPositionTypeNavigator", getPath(), o);
- }
-
- type::AronPositionPtr PositionNavigator::toAronPositionPtr() const
- {
- return type;
- }
-
- std::vector<int> PositionNavigator::getDimensions() const
- {
- return ACCEPTED_DIMENSION;
- }
-
- std::string PositionNavigator::getTypename() const
+ detail::NavigatorBase<type::AronPosition, PositionNavigator>(o)
{
- return ACCEPTED_TYPE;
}
- // static methods
- PositionNavigatorPtr PositionNavigator::DynamicCast(const NavigatorPtr& n)
+ // operators
+ bool PositionNavigator::operator==(const PositionNavigator& other) const
{
- return std::dynamic_pointer_cast<PositionNavigator>(n);
+ if (getMaybe() != other.getMaybe())
+ {
+ return false;
+ }
+ return true;
}
- PositionNavigatorPtr PositionNavigator::DynamicCastAndCheck(const NavigatorPtr& n)
+ type::AronPositionPtr PositionNavigator::toAronPositionPtr() const
{
- CheckTypeNavigatorPtrForNull("AronPositionTypeNavigator", "DynamicCast[Before]", n);
- PositionNavigatorPtr casted = PositionNavigator::DynamicCast(n);
- CheckTypeNavigatorPtrForNull("AronPositionTypeNavigator", "DynamicCast[After]", n->getPath(), casted);
- return casted;
+ return this->aron;
}
// virtual implementations
- std::vector<NavigatorPtr> PositionNavigator::getChildren() const
- {
- return {};
- }
-
- size_t PositionNavigator::childrenSize() const
- {
- return 0;
- }
-
- type::AronTypePtr PositionNavigator::getResult() const
- {
- return toAronPositionPtr();
- }
-
std::string PositionNavigator::getName() const
{
return "AronPositionType";
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/Position.h b/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/Position.h
index c6a1d0f731a4d068be041efebc79907d6a1f4a87..19bb1834b6e597631539d0da8027eef56f2fa8be 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/Position.h
+++ b/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/Position.h
@@ -27,8 +27,8 @@
#include <string>
#include <map>
-// Base Class
-#include <RobotAPI/libraries/aron/core/navigator/type/Navigator.h>
+// Base class
+#include "../detail/NDArrayNavigatorBase.h"
namespace armarx::aron::typenavigator
{
@@ -36,37 +36,24 @@ namespace armarx::aron::typenavigator
typedef std::shared_ptr<PositionNavigator> PositionNavigatorPtr;
class PositionNavigator :
- virtual public Navigator
+ virtual public detail::NDArrayNavigatorBase<type::AronPosition, PositionNavigator>
{
- public:
- using PointerType = PositionNavigatorPtr;
-
public:
// constructors
PositionNavigator(const Path& path = Path());
PositionNavigator(const type::AronPositionPtr&, const Path& path = Path());
- std::string getTypename() const;
- std::vector<int> getDimensions() const;
+ // operators
+ virtual bool operator==(const PositionNavigator&) const override;
+ // public member functions
type::AronPositionPtr toAronPositionPtr() const;
- // static methods
- static PositionNavigatorPtr DynamicCast(const NavigatorPtr&);
- static PositionNavigatorPtr DynamicCastAndCheck(const NavigatorPtr&);
-
// virtual implementations
- virtual std::vector<NavigatorPtr> getChildren() const override;
- virtual size_t childrenSize() const override;
- type::AronTypePtr getResult() const override;
std::string getName() const override;
public:
const std::string ACCEPTED_TYPE = "float";
const std::vector<int> ACCEPTED_DIMENSION = {3, 1};
-
- private:
- // members
- type::AronPositionPtr type;
};
}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/primitive/Bool.cpp b/source/RobotAPI/libraries/aron/core/navigator/type/primitive/Bool.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..ae187d241f1f19c79673a593b12fe9da8eea7918
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/navigator/type/primitive/Bool.cpp
@@ -0,0 +1,56 @@
+/*
+ * 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 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
+ */
+
+
+// STD/STL
+#include <string>
+#include <map>
+
+// Header
+#include "Bool.h"
+
+namespace armarx::aron::typenavigator
+{
+ /* constructors */
+ BoolNavigator::BoolNavigator(const Path& path) :
+ aron::Navigator<type::Descriptor, type::AronType>::Navigator(type::Descriptor::eBool, path)
+ {
+ }
+
+ BoolNavigator::BoolNavigator(const type::AronBoolPtr&o, const Path& path) :
+ aron::Navigator<type::Descriptor, type::AronType>::Navigator(type::Descriptor::eBool, path),
+ detail::NavigatorBase<type::AronBool, BoolNavigator>(o)
+ {
+ }
+
+ type::AronBoolPtr BoolNavigator::toAronBoolPtr() const
+ {
+ return this->aron;
+ }
+
+ /* virtual implementations */
+ std::string BoolNavigator::getName() const
+ {
+ return "type::AronBool";
+ }
+}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/primitive/Bool.h b/source/RobotAPI/libraries/aron/core/navigator/type/primitive/Bool.h
new file mode 100644
index 0000000000000000000000000000000000000000..d8cc1f53b1f1261ba46983b6d506df3d90111e63
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/navigator/type/primitive/Bool.h
@@ -0,0 +1,50 @@
+/*
+ * 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 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
+ */
+
+#pragma once
+
+// STD/STL
+#include <string>
+
+// Base class
+#include "../detail/PrimitiveNavigatorBase.h"
+
+namespace armarx::aron::typenavigator
+{
+ class BoolNavigator;
+ typedef std::shared_ptr<BoolNavigator> BoolNavigatorPtr;
+
+ class BoolNavigator :
+ virtual public detail::PrimitiveNavigatorBase<type::AronBool, BoolNavigator>
+ {
+ public:
+ /* constructors */
+ BoolNavigator(const Path& = Path());
+ BoolNavigator(const type::AronBoolPtr&, const Path& = Path());
+
+ type::AronBoolPtr toAronBoolPtr() const;
+
+ /* virtual implementations */
+ virtual std::string getName() const override;
+ };
+}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/primitive/Double.cpp b/source/RobotAPI/libraries/aron/core/navigator/type/primitive/Double.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..98fff8740e8c2afdd9781d7eb5fc89c0469cd82e
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/navigator/type/primitive/Double.cpp
@@ -0,0 +1,56 @@
+/*
+ * 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 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
+ */
+
+
+// STD/STL
+#include <string>
+#include <map>
+
+// Header
+#include "Double.h"
+
+namespace armarx::aron::typenavigator
+{
+ /* constructors */
+ DoubleNavigator::DoubleNavigator(const Path& path) :
+ aron::Navigator<type::Descriptor, type::AronType>::Navigator(type::Descriptor::eDouble, path)
+ {
+ }
+
+ DoubleNavigator::DoubleNavigator(const type::AronDoublePtr&o, const Path& path) :
+ aron::Navigator<type::Descriptor, type::AronType>::Navigator(type::Descriptor::eDouble, path),
+ detail::NavigatorBase<type::AronDouble, DoubleNavigator>(o)
+ {
+ }
+
+ type::AronDoublePtr DoubleNavigator::toAronDoublePtr() const
+ {
+ return this->aron;
+ }
+
+ /* virtual implementations */
+ std::string DoubleNavigator::getName() const
+ {
+ return "type::AronDouble";
+ }
+}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/primitive/Double.h b/source/RobotAPI/libraries/aron/core/navigator/type/primitive/Double.h
new file mode 100644
index 0000000000000000000000000000000000000000..9c19b29f73e092b7e7e4330e2215301be2849674
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/navigator/type/primitive/Double.h
@@ -0,0 +1,50 @@
+/*
+ * 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 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
+ */
+
+#pragma once
+
+// STD/STL
+#include <string>
+
+// Base class
+#include "../detail/PrimitiveNavigatorBase.h"
+
+namespace armarx::aron::typenavigator
+{
+ class DoubleNavigator;
+ typedef std::shared_ptr<DoubleNavigator> DoubleNavigatorPtr;
+
+ class DoubleNavigator :
+ virtual public detail::PrimitiveNavigatorBase<type::AronDouble, DoubleNavigator>
+ {
+ public:
+ /* constructors */
+ DoubleNavigator(const Path& = Path());
+ DoubleNavigator(const type::AronDoublePtr&, const Path& = Path());
+
+ type::AronDoublePtr toAronDoublePtr() const;
+
+ /* virtual implementations */
+ virtual std::string getName() const override;
+ };
+}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/primitive/Float.cpp b/source/RobotAPI/libraries/aron/core/navigator/type/primitive/Float.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..277ea6bf525bf8e4eb7da12d0ecf2da44e6e9618
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/navigator/type/primitive/Float.cpp
@@ -0,0 +1,56 @@
+/*
+ * 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 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
+ */
+
+
+// STD/STL
+#include <string>
+#include <map>
+
+// Header
+#include "Float.h"
+
+namespace armarx::aron::typenavigator
+{
+ /* constructors */
+ FloatNavigator::FloatNavigator(const Path& path) :
+ aron::Navigator<type::Descriptor, type::AronType>::Navigator(type::Descriptor::eFloat, path)
+ {
+ }
+
+ FloatNavigator::FloatNavigator(const type::AronFloatPtr&o, const Path& path) :
+ aron::Navigator<type::Descriptor, type::AronType>::Navigator(type::Descriptor::eFloat, path),
+ detail::NavigatorBase<type::AronFloat, FloatNavigator>(o)
+ {
+ }
+
+ type::AronFloatPtr FloatNavigator::toAronFloatPtr() const
+ {
+ return this->aron;
+ }
+
+ /* virtual implementations */
+ std::string FloatNavigator::getName() const
+ {
+ return "type::AronFloat";
+ }
+}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/primitive/Float.h b/source/RobotAPI/libraries/aron/core/navigator/type/primitive/Float.h
new file mode 100644
index 0000000000000000000000000000000000000000..6f583207a03cb0806c5ca7f15d84c051a047bc36
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/navigator/type/primitive/Float.h
@@ -0,0 +1,50 @@
+/*
+ * 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 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
+ */
+
+#pragma once
+
+// STD/STL
+#include <string>
+
+// Base class
+#include "../detail/PrimitiveNavigatorBase.h"
+
+namespace armarx::aron::typenavigator
+{
+ class FloatNavigator;
+ typedef std::shared_ptr<FloatNavigator> FloatNavigatorPtr;
+
+ class FloatNavigator :
+ virtual public detail::PrimitiveNavigatorBase<type::AronFloat, FloatNavigator>
+ {
+ public:
+ /* constructors */
+ FloatNavigator(const Path& = Path());
+ FloatNavigator(const type::AronFloatPtr&, const Path& = Path());
+
+ type::AronFloatPtr toAronFloatPtr() const;
+
+ /* virtual implementations */
+ virtual std::string getName() const override;
+ };
+}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/primitive/Int.cpp b/source/RobotAPI/libraries/aron/core/navigator/type/primitive/Int.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..89a78c5616a6d70f278ab9da56bf5df8f0b3d27f
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/navigator/type/primitive/Int.cpp
@@ -0,0 +1,56 @@
+/*
+ * 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 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
+ */
+
+
+// STD/STL
+#include <string>
+#include <map>
+
+// Header
+#include "Int.h"
+
+namespace armarx::aron::typenavigator
+{
+ /* constructors */
+ IntNavigator::IntNavigator(const Path& path) :
+ aron::Navigator<type::Descriptor, type::AronType>::Navigator(type::Descriptor::eInt, path)
+ {
+ }
+
+ IntNavigator::IntNavigator(const type::AronIntPtr&o, const Path& path) :
+ aron::Navigator<type::Descriptor, type::AronType>::Navigator(type::Descriptor::eInt, path),
+ detail::NavigatorBase<type::AronInt, IntNavigator>(o)
+ {
+ }
+
+ type::AronIntPtr IntNavigator::toAronIntPtr() const
+ {
+ return this->aron;
+ }
+
+ /* virtual implementations */
+ std::string IntNavigator::getName() const
+ {
+ return "type::AronInt";
+ }
+}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/primitive/Int.h b/source/RobotAPI/libraries/aron/core/navigator/type/primitive/Int.h
new file mode 100644
index 0000000000000000000000000000000000000000..552b51e1778e1114f187a283229f3fe738577fbc
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/navigator/type/primitive/Int.h
@@ -0,0 +1,50 @@
+/*
+ * 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 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
+ */
+
+#pragma once
+
+// STD/STL
+#include <string>
+
+// Base class
+#include "../detail/PrimitiveNavigatorBase.h"
+
+namespace armarx::aron::typenavigator
+{
+ class IntNavigator;
+ typedef std::shared_ptr<IntNavigator> IntNavigatorPtr;
+
+ class IntNavigator :
+ virtual public detail::PrimitiveNavigatorBase<type::AronInt, IntNavigator>
+ {
+ public:
+ /* constructors */
+ IntNavigator(const Path& = Path());
+ IntNavigator(const type::AronIntPtr&, const Path& = Path());
+
+ type::AronIntPtr toAronIntPtr() const;
+
+ /* virtual implementations */
+ virtual std::string getName() const override;
+ };
+}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/primitive/Long.cpp b/source/RobotAPI/libraries/aron/core/navigator/type/primitive/Long.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..0f00f590a83b77a070f90597be596298c826dacc
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/navigator/type/primitive/Long.cpp
@@ -0,0 +1,56 @@
+/*
+ * 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 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
+ */
+
+
+// STD/STL
+#include <string>
+#include <map>
+
+// Header
+#include "Long.h"
+
+namespace armarx::aron::typenavigator
+{
+ /* constructors */
+ LongNavigator::LongNavigator(const Path& path) :
+ aron::Navigator<type::Descriptor, type::AronType>::Navigator(type::Descriptor::eLong, path)
+ {
+ }
+
+ LongNavigator::LongNavigator(const type::AronLongPtr&o, const Path& path) :
+ aron::Navigator<type::Descriptor, type::AronType>::Navigator(type::Descriptor::eLong, path),
+ detail::NavigatorBase<type::AronLong, LongNavigator>(o)
+ {
+ }
+
+ type::AronLongPtr LongNavigator::toAronLongPtr() const
+ {
+ return this->aron;
+ }
+
+ /* virtual implementations */
+ std::string LongNavigator::getName() const
+ {
+ return "type::AronLong";
+ }
+}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/primitive/Long.h b/source/RobotAPI/libraries/aron/core/navigator/type/primitive/Long.h
new file mode 100644
index 0000000000000000000000000000000000000000..fb7b455587cee532d78c145990d50e5d3304dddf
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/navigator/type/primitive/Long.h
@@ -0,0 +1,50 @@
+/*
+ * 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 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
+ */
+
+#pragma once
+
+// STD/STL
+#include <string>
+
+// Base class
+#include "../detail/PrimitiveNavigatorBase.h"
+
+namespace armarx::aron::typenavigator
+{
+ class LongNavigator;
+ typedef std::shared_ptr<LongNavigator> LongNavigatorPtr;
+
+ class LongNavigator :
+ virtual public detail::PrimitiveNavigatorBase<type::AronLong, LongNavigator>
+ {
+ public:
+ /* constructors */
+ LongNavigator(const Path& = Path());
+ LongNavigator(const type::AronLongPtr&, const Path& = Path());
+
+ type::AronLongPtr toAronLongPtr() const;
+
+ /* virtual implementations */
+ virtual std::string getName() const override;
+ };
+}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/primitive/Primitive.cpp b/source/RobotAPI/libraries/aron/core/navigator/type/primitive/Primitive.cpp
deleted file mode 100644
index bebe319697f9809815cdaa7fad247c247604499b..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/navigator/type/primitive/Primitive.cpp
+++ /dev/null
@@ -1,96 +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 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
- */
-
-
-// STD/STL
-#include <string>
-#include <map>
-
-// Header
-#include "Primitive.h"
-
-// ArmarX
-#include <RobotAPI/libraries/aron/core/Concepts.h>
-
-namespace armarx::aron::typenavigator
-{
-#define RUN_ARON_MACRO(upperType, lowerType, capsType) \
- /* constructors */ \
- upperType##Navigator::upperType##Navigator(const Path& path) : \
- aron::Navigator<type::Descriptor, type::AronType>::Navigator(type::Descriptor::e##upperType, path), \
- Navigator(type::Descriptor::e##upperType, path), \
- type(new type::Aron##upperType()) \
- { \
- } \
- \
- upperType##Navigator::upperType##Navigator(const type::Aron##upperType##Ptr&o, const Path& path) : \
- aron::Navigator<type::Descriptor, type::AronType>::Navigator(type::Descriptor::e##upperType, path), \
- Navigator(type::Descriptor::e##upperType, path), \
- type(o) \
- { \
- CheckAronPtrForNull("typenavigator::" + std::string(#upperType) + "Navigator", std::string(#upperType) + "Navigator", getPath(), o); \
- } \
- \
- type::Aron##upperType##Ptr upperType##Navigator::toAron##upperType##Ptr() const \
- { \
- return type; \
- } \
- \
- /* static methods */ \
- upperType##NavigatorPtr upperType##Navigator::DynamicCastAndCheck(const NavigatorPtr& n) \
- {\
- CheckTypeNavigatorPtrForNull("typenavigator::" + std::string(#upperType) + "Navigator", "DynamicCast[Before]", n); \
- upperType##NavigatorPtr casted = upperType##Navigator::DynamicCast(n); \
- CheckTypeNavigatorPtrForNull("typenavigator::" + std::string(#upperType) + "Navigator", "DynamicCast[After]", n->getPath(), casted); \
- return casted; \
- }\
- \
- upperType##NavigatorPtr upperType##Navigator::DynamicCast(const NavigatorPtr& n) \
- {\
- return std::dynamic_pointer_cast<upperType##Navigator>(n); \
- }\
- \
- /* virtual implementations */ \
- std::vector<NavigatorPtr> upperType##Navigator::getChildren() const \
- { \
- return {}; \
- } \
- \
- size_t upperType##Navigator::childrenSize() const \
- { \
- return 0; \
- } \
- \
- type::AronTypePtr upperType##Navigator::getResult() const \
- {\
- return toAron##upperType##Ptr(); \
- }\
- \
- std::string upperType##Navigator::getName() const \
- {\
- return "type::Aron" + std::string(#upperType); \
- }
-
- HANDLE_PRIMITIVE_TYPES
-#undef RUN_ARON_MACRO
-}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/primitive/Primitive.h b/source/RobotAPI/libraries/aron/core/navigator/type/primitive/Primitive.h
deleted file mode 100644
index 9fe2fadf5f510f89be02ac3505351f61a63a8f23..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/navigator/type/primitive/Primitive.h
+++ /dev/null
@@ -1,70 +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 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
- */
-
-#pragma once
-
-// STD/STL
-#include <string>
-
-// Base Class
-#include <RobotAPI/libraries/aron/core/navigator/type/Navigator.h>
-
-// ArmarX
-#include <RobotAPI/libraries/aron/core/Concepts.h>
-
-namespace armarx::aron::typenavigator
-{
-
-#define RUN_ARON_MACRO(upperType, lowerType, capsType) \
- class upperType##Navigator; \
- typedef std::shared_ptr<upperType##Navigator> upperType##NavigatorPtr; \
- \
- class upperType##Navigator : \
- virtual public Navigator \
- { \
- public: \
- using PointerType = upperType##NavigatorPtr; \
- \
- /* constructors */ \
- upperType##Navigator(const Path& = Path()); \
- upperType##Navigator(const type::Aron##upperType##Ptr&, const Path& = Path()); \
- \
- type::Aron##upperType##Ptr toAron##upperType##Ptr() const; \
- \
- /* static methods */ \
- static upperType##NavigatorPtr DynamicCast(const NavigatorPtr&); \
- static upperType##NavigatorPtr DynamicCastAndCheck(const NavigatorPtr& n); \
- \
- /* virtual implementations */ \
- virtual std::vector<NavigatorPtr> getChildren() const override; \
- virtual size_t childrenSize() const override; \
- virtual type::AronTypePtr getResult() const override; \
- virtual std::string getName() const override; \
- \
- private: \
- type::Aron##upperType##Ptr type; \
- };
-
- HANDLE_PRIMITIVE_TYPES
-#undef RUN_ARON_MACRO
-}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/primitive/String.cpp b/source/RobotAPI/libraries/aron/core/navigator/type/primitive/String.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..1ad8135634298ca3de84ae41743d36a58018461d
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/navigator/type/primitive/String.cpp
@@ -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 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
+ */
+
+
+// STD/STL
+#include <string>
+#include <map>
+
+// Header
+#include "String.h"
+
+namespace armarx::aron::typenavigator
+{
+ /* constructors */
+ StringNavigator::StringNavigator(const Path& path) :
+ aron::Navigator<type::Descriptor, type::AronType>::Navigator(type::Descriptor::eString, path)
+ {
+ }
+
+ StringNavigator::StringNavigator(const type::AronStringPtr&o, const Path& path) :
+ aron::Navigator<type::Descriptor, type::AronType>::Navigator(type::Descriptor::eString, path),
+ detail::NavigatorBase<type::AronString, StringNavigator>(o)
+ {
+ }
+
+ /* public member functions */
+ type::AronStringPtr StringNavigator::toAronStringPtr() const
+ {
+ return this->aron;
+ }
+
+ /* virtual implementations */
+ std::string StringNavigator::getName() const
+ {
+ return "type::AronString";
+ }
+}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/primitive/String.h b/source/RobotAPI/libraries/aron/core/navigator/type/primitive/String.h
new file mode 100644
index 0000000000000000000000000000000000000000..b661da12f3fe5a4fe2f35404f94816ec9a372245
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/navigator/type/primitive/String.h
@@ -0,0 +1,50 @@
+/*
+ * 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 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
+ */
+
+#pragma once
+
+// STD/STL
+#include <string>
+
+// Base class
+#include "../detail/PrimitiveNavigatorBase.h"
+
+namespace armarx::aron::typenavigator
+{
+ class StringNavigator;
+ typedef std::shared_ptr<StringNavigator> StringNavigatorPtr;
+
+ class StringNavigator :
+ virtual public detail::PrimitiveNavigatorBase<type::AronString, StringNavigator>
+ {
+ public:
+ /* constructors */
+ StringNavigator(const Path& = Path());
+ StringNavigator(const type::AronStringPtr&, const Path& = Path());
+
+ type::AronStringPtr toAronStringPtr() const;
+
+ /* virtual implementations */
+ virtual std::string getName() const override;
+ };
+}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/primitive/Time.cpp b/source/RobotAPI/libraries/aron/core/navigator/type/primitive/Time.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..09c04bebdc871d2b10ed47840e406b010b525317
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/navigator/type/primitive/Time.cpp
@@ -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 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
+ */
+
+
+// STD/STL
+#include <string>
+#include <map>
+
+// Header
+#include "Time.h"
+
+namespace armarx::aron::typenavigator
+{
+ /* constructors */
+ TimeNavigator::TimeNavigator(const Path& path) :
+ aron::Navigator<type::Descriptor, type::AronType>::Navigator(type::Descriptor::eTime, path)
+ {
+ }
+
+ TimeNavigator::TimeNavigator(const type::AronTimePtr&o, const Path& path) :
+ aron::Navigator<type::Descriptor, type::AronType>::Navigator(type::Descriptor::eTime, path),
+ detail::NavigatorBase<type::AronTime, TimeNavigator>(o)
+ {
+ }
+
+ /* public member functions */
+ type::AronTimePtr TimeNavigator::toAronTimePtr() const
+ {
+ return this->aron;
+ }
+
+ /* virtual implementations */
+ std::string TimeNavigator::getName() const
+ {
+ return "type::AronTime";
+ }
+}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/primitive/Time.h b/source/RobotAPI/libraries/aron/core/navigator/type/primitive/Time.h
new file mode 100644
index 0000000000000000000000000000000000000000..7fd07408ee8db90c6591bf55c64804d511140ee5
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/navigator/type/primitive/Time.h
@@ -0,0 +1,50 @@
+/*
+ * 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 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
+ */
+
+#pragma once
+
+// STD/STL
+#include <string>
+
+// Base class
+#include "../detail/PrimitiveNavigatorBase.h"
+
+namespace armarx::aron::typenavigator
+{
+ class TimeNavigator;
+ typedef std::shared_ptr<TimeNavigator> TimeNavigatorPtr;
+
+ class TimeNavigator :
+ virtual public detail::PrimitiveNavigatorBase<type::AronTime, TimeNavigator>
+ {
+ public:
+ /* constructors */
+ TimeNavigator(const Path& = Path());
+ TimeNavigator(const type::AronTimePtr&, const Path& = Path());
+
+ type::AronTimePtr toAronTimePtr() const;
+
+ /* virtual implementations */
+ virtual std::string getName() const override;
+ };
+}
diff --git a/source/RobotAPI/libraries/aron/core/test/CMakeLists.txt b/source/RobotAPI/libraries/aron/core/test/CMakeLists.txt
index de44d5a1d28a8148139265d1a18a6667c1c4a697..239c92ecff42e75ea7be37b5e43c8d442983f678 100644
--- a/source/RobotAPI/libraries/aron/core/test/CMakeLists.txt
+++ b/source/RobotAPI/libraries/aron/core/test/CMakeLists.txt
@@ -1,44 +1,107 @@
-SET(TEST_NAME aronTest)
-
+find_package(Simox QUIET)
+armarx_build_if(Simox_FOUND "Simox not available")
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(IVT COMPONENTS ivt ivtopencv QUIET)
armarx_build_if(IVT_FOUND "IVT not available")
-if(IVT_FOUND)
- include_directories(${IVT_INCLUDE_DIRS})
-endif()
find_package(OpenCV QUIET)
armarx_build_if(OpenCV_FOUND "OpenCV not available")
-if(OpenCV_FOUND)
- include_directories(${OpenCV_INCLUDE_DIRS})
-endif()
find_package(PCL QUIET)
armarx_build_if(PCL_FOUND "PCL not available")
-if(PCL_FOUND)
- include_directories(${PCL_INCLUDE_DIRS})
-endif()
-# Libs required for the tests
-SET(LIBS ${LIBS} ArmarXCore aron ivt ivtopencv ${PCL_COMMON_LIBRARIES})
+######################
+# ARON OPERATOR TEST #
+######################
+armarx_add_test(
+ TEST_NAME
+ aronOperatorTest
+ TEST_FILE
+ aronOperatorTest.cpp
+ LIBS
+ Simox::SimoxUtility
+ ArmarXCore
+ RobotAPI::aron
+ INCLUDE_DIRECTORIES
+ ${Simox_INCLUDE_DIR}
+)
+
+######################
+# ARON CODE GEN TEST #
+######################
+armarx_add_test(
+ TEST_NAME
+ aronCodeGenerationTest
+ TEST_FILE
+ aronCodeGenerationTest.cpp
+ LIBS
+ Simox::SimoxUtility
+ ArmarXCore
+ RobotAPI::aron
+ ARON_FILES
+ # xmls/BaseClass.xml
+ # xmls/DerivedClassTest.xml
+ xmls/DictTest.xml
+ xmls/EigenMatrixTest.xml
+ xmls/EigenQuaternionTest.xml
+ xmls/EnumTest.xml
+ xmls/HumanPoseTest.xml
+ xmls/IVTCByteImageTest.xml
+ xmls/ListTest.xml
+ xmls/NaturalIKTest.xml
+ xmls/ObjectTest.xml
+ xmls/OpenCVMatTest.xml
+ xmls/OrientationTest.xml
+ xmls/PCLPointCloudTest.xml
+ xmls/PoseTest.xml
+ xmls/PositionTest.xml
+ xmls/PrimitiveTest.xml
+ xmls/OptionalTest.xml
+ INCLUDE_DIRECTORIES
+ ${Simox_INCLUDE_DIR}
+ ${Eigen3_INCLUDE_DIR}
+ ${IVT_INCLUDE_DIRS}
+ ${OpenCV_INCLUDE_DIRS}
+ ${PCL_INCLUDE_DIRS}
+)
+
+######################
+# ARON NAVIGATE TEST #
+######################
+armarx_add_test(
+ TEST_NAME
+ aronNavigateTest
+ TEST_FILE
+ aronNavigateTest.cpp
+ LIBS
+ Simox::SimoxUtility
+ ArmarXCore
+ RobotAPI::aron
+ ARON_FILES
+ xmls/NaturalIKTest.xml
+ INCLUDE_DIRECTORIES
+ ${Simox_INCLUDE_DIR}
+)
-armarx_add_test(${TEST_NAME} aronTest.cpp "${LIBS}")
-armarx_enable_aron_file_generation_for_target(
- TARGET_NAME
- ${TEST_NAME}
+########################
+# ARON RANDOMIZED TEST #
+########################
+armarx_add_test(
+ TEST_NAME
+ aronRandomizedTest
+ TEST_FILE
+ aronRandomizedTest.cpp
+ LIBS
+ Simox::SimoxUtility
+ ArmarXCore
+ RobotAPI::aron
+ ivt
+ ivtopencv
+ ${PCL_COMMON_LIBRARIES}
ARON_FILES
# xmls/BaseClass.xml
# xmls/DerivedClassTest.xml
@@ -57,5 +120,11 @@ armarx_enable_aron_file_generation_for_target(
xmls/PoseTest.xml
xmls/PositionTest.xml
xmls/PrimitiveTest.xml
- #ENABLE_DEBUG_INFO
+ xmls/OptionalTest.xml
+ INCLUDE_DIRECTORIES
+ ${Simox_INCLUDE_DIR}
+ ${Eigen3_INCLUDE_DIR}
+ ${IVT_INCLUDE_DIRS}
+ ${OpenCV_INCLUDE_DIRS}
+ ${PCL_INCLUDE_DIRS}
)
diff --git a/source/RobotAPI/libraries/aron/core/test/aronCodeGenerationTest.cpp b/source/RobotAPI/libraries/aron/core/test/aronCodeGenerationTest.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c1db4b859aa464794c3bba2e1025feb5436a5bba
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/test/aronCodeGenerationTest.cpp
@@ -0,0 +1,212 @@
+/*
+ * 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::aron
+ * @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
+ */
+
+#define BOOST_TEST_MODULE RobotAPI::ArmarXLibraries::aron
+
+#define ARMARX_BOOST_TEST
+
+// STD/STL
+#include <iostream>
+#include <cstdlib>
+#include <ctime>
+#include <numeric>
+
+// Boost
+#include <boost/algorithm/string.hpp>
+
+// Test
+#include <RobotAPI/Test.h>
+
+// ArmarX
+#include <ArmarXCore/libraries/cppgen/CppMethod.h>
+#include <ArmarXCore/libraries/cppgen/CppClass.h>
+#include <RobotAPI/libraries/aron/core/Exception.h>
+
+// Aron
+#include <RobotAPI/libraries/aron/core/Debug.h>
+#include <RobotAPI/libraries/aron/core/Randomizer.h>
+
+// Generated File
+#include <RobotAPI/libraries/aron/core/test/aron/ListTest.aron.generated.h>
+#include <RobotAPI/libraries/aron/core/test/aron/DictTest.aron.generated.h>
+#include <RobotAPI/libraries/aron/core/test/aron/PrimitiveTest.aron.generated.h>
+#include <RobotAPI/libraries/aron/core/test/aron/ObjectTest.aron.generated.h>
+#include <RobotAPI/libraries/aron/core/test/aron/IVTCByteImageTest.aron.generated.h>
+#include <RobotAPI/libraries/aron/core/test/aron/EigenMatrixTest.aron.generated.h>
+#include <RobotAPI/libraries/aron/core/test/aron/EigenQuaternionTest.aron.generated.h>
+#include <RobotAPI/libraries/aron/core/test/aron/OpenCVMatTest.aron.generated.h>
+#include <RobotAPI/libraries/aron/core/test/aron/PCLPointCloudTest.aron.generated.h>
+#include <RobotAPI/libraries/aron/core/test/aron/PositionTest.aron.generated.h>
+#include <RobotAPI/libraries/aron/core/test/aron/OrientationTest.aron.generated.h>
+#include <RobotAPI/libraries/aron/core/test/aron/PoseTest.aron.generated.h>
+#include <RobotAPI/libraries/aron/core/test/aron/EnumTest.aron.generated.h>
+#include <RobotAPI/libraries/aron/core/test/aron/OptionalTest.aron.generated.h>
+
+using namespace armarx;
+using namespace aron;
+
+BOOST_AUTO_TEST_CASE(AronCodeGenerationListTest)
+{
+ std::cout << "Running Code Gen List test" << std::endl;
+ ListTest p_tmp;
+ ListTest p = p_tmp; // test assignment
+ BOOST_CHECK_EQUAL(p_tmp == p, true);
+
+ BOOST_CHECK_EQUAL(typeid(p.intList) == typeid(std::vector<int>), true);
+ BOOST_CHECK_EQUAL(typeid(p.longList) == typeid(std::vector<long>), true);
+ BOOST_CHECK_EQUAL(typeid(p.floatList) == typeid(std::vector<float>), true);
+ BOOST_CHECK_EQUAL(typeid(p.doubleList) == typeid(std::vector<double>), true);
+ BOOST_CHECK_EQUAL(typeid(p.stringList) == typeid(std::vector<std::string>), true);
+ BOOST_CHECK_EQUAL(typeid(p.boolList) == typeid(std::vector<bool>), true);
+ //BOOST_CHECK_EQUAL(typeid(p.objectList) == typeid(std::vector<ListTest::ListClass>), true);
+
+ BOOST_CHECK_EQUAL(p.intList.size() == 0, true);
+ BOOST_CHECK_EQUAL(p.longList.size() == 0, true);
+ BOOST_CHECK_EQUAL(p.floatList.size() == 0, true);
+ BOOST_CHECK_EQUAL(p.doubleList.size() == 0, true);
+ BOOST_CHECK_EQUAL(p.stringList.size() == 0, true);
+ BOOST_CHECK_EQUAL(p.boolList.size() == 0, true);
+ BOOST_CHECK_EQUAL(p.objectList.size() == 0, true);
+}
+
+BOOST_AUTO_TEST_CASE(AronCodeGenerationDictTest)
+{
+ std::cout << "Running Code Gen Dict test" << std::endl;
+ DictTest p_tmp;
+ DictTest p = p_tmp; // test assignment
+ BOOST_CHECK_EQUAL(p_tmp == p, true);
+
+ BOOST_CHECK_EQUAL(p.dict.size() == 0, true);
+}
+
+BOOST_AUTO_TEST_CASE(AronCodeGenerationEigenMatrixTest)
+{
+ std::cout << "Running Code Gen EigenMatrix test" << std::endl;
+ EigenMatrixTest p_tmp;
+ EigenMatrixTest p = p_tmp; // test assignment
+ BOOST_CHECK_EQUAL(p_tmp == p, true);
+
+ BOOST_CHECK_EQUAL(p.the_short_eigen_matrix.rows() == 5, true);
+ BOOST_CHECK_EQUAL(p.the_short_eigen_matrix.cols() == 7, true);
+ BOOST_CHECK_EQUAL(p.the_int_eigen_matrix.rows() == 7, true);
+ BOOST_CHECK_EQUAL(p.the_int_eigen_matrix.cols() == 7, true);
+ BOOST_CHECK_EQUAL(p.the_long_eigen_matrix.rows() == 7, true);
+ BOOST_CHECK_EQUAL(p.the_long_eigen_matrix.cols() == 5, true);
+ BOOST_CHECK_EQUAL(p.the_float_eigen_matrix.rows() == 1, true);
+ BOOST_CHECK_EQUAL(p.the_float_eigen_matrix.cols() == 9, true);
+ BOOST_CHECK_EQUAL(p.the_double_eigen_matrix.rows() == 25, true);
+ BOOST_CHECK_EQUAL(p.the_double_eigen_matrix.cols() == 1, true);
+}
+
+BOOST_AUTO_TEST_CASE(AronCodeGenerationEigenQuaternionTest)
+{
+ std::cout << "Running Code Gen EigenQuaternion test" << std::endl;
+ EigenQuaternionTest p_tmp;
+ EigenQuaternionTest p = p_tmp; // test assignment
+ BOOST_CHECK_EQUAL(p_tmp == p, true);
+
+ //BOOST_CHECK_EQUAL(p.the_double_eigen_matrix.w() == 0, true);
+ //BOOST_CHECK_EQUAL(p.the_double_eigen_matrix.x() == 0, true);
+ //BOOST_CHECK_EQUAL(p.the_double_eigen_matrix.y() == 0, true);
+ //BOOST_CHECK_EQUAL(p.the_double_eigen_matrix.z() == 0, true);
+}
+
+BOOST_AUTO_TEST_CASE(AronCodeGenerationEigenPositionTest)
+{
+ std::cout << "Running Code Gen EigenPosition test" << std::endl;
+ PositionTest p_tmp;
+ PositionTest p = p_tmp; // test assignment
+ BOOST_CHECK_EQUAL(p_tmp == p, true);
+
+ BOOST_CHECK_EQUAL(p.position.rows() == 3, true);
+ BOOST_CHECK_EQUAL(p.position.cols() == 1, true);
+}
+
+BOOST_AUTO_TEST_CASE(AronCodeGenerationEigenPoseTest)
+{
+ std::cout << "Running Code Gen EigenPose test" << std::endl;
+ PoseTest p_tmp;
+ PoseTest p = p_tmp; // test assignment
+ BOOST_CHECK_EQUAL(p_tmp == p, true);
+
+ BOOST_CHECK_EQUAL(p.pose.rows() == 4, true);
+ BOOST_CHECK_EQUAL(p.pose.cols() == 4, true);
+}
+
+BOOST_AUTO_TEST_CASE(AronCodeGenerationIntEnumTest)
+{
+ std::cout << "Running Code Gen IntEnum test" << std::endl;
+ TheObjectThatUsesTheIntEnum p_tmp;
+ TheObjectThatUsesTheIntEnum p = p_tmp; // test assignment
+ BOOST_CHECK_EQUAL(p_tmp == p, true);
+
+ BOOST_CHECK_EQUAL(p.the_int_enum.value == TheIntEnum::INT_ENUM_VALUE_0, true);
+}
+
+BOOST_AUTO_TEST_CASE(AronCodeGenerationPrimitiveTest)
+{
+ std::cout << "Running Code Gen Primitive test" << std::endl;
+ PrimitiveTest p_tmp;
+ PrimitiveTest p = p_tmp; // test assignment
+ BOOST_CHECK_EQUAL(p_tmp == p, true);
+
+ BOOST_CHECK_EQUAL(p.the_bool == false, true);
+ BOOST_CHECK_EQUAL(p.the_double == 0.0, true);
+ BOOST_CHECK_EQUAL(p.the_float == 0.0, true);
+ BOOST_CHECK_EQUAL(p.the_int == 0, true);
+ BOOST_CHECK_EQUAL(p.the_long == 0, true);
+ BOOST_CHECK_EQUAL(p.the_string == "", true);
+ BOOST_CHECK_EQUAL(p.the_time == IceUtil::Time(), true);
+}
+
+BOOST_AUTO_TEST_CASE(AronCodeGenerationOptionalTest)
+{
+ std::cout << "Running Code Gen Primitive test" << std::endl;
+ OptionalTest p_tmp;
+ OptionalTest p = p_tmp; // test assignment
+ BOOST_CHECK_EQUAL(p_tmp == p, true);
+
+ BOOST_CHECK_EQUAL(typeid(p.some_dict) == typeid(std::optional<std::map<std::string, float>>), true);
+ BOOST_CHECK_EQUAL(typeid(p.some_dict_with_optional_type) == typeid(std::map<std::string, std::optional<float>>), true);
+ BOOST_CHECK_EQUAL(typeid(p.some_eigen_matrix) == typeid(std::optional<Eigen::Matrix<long, 25, 10>>), true);
+ BOOST_CHECK_EQUAL(typeid(p.some_float) == typeid(std::optional<float>), true);
+ BOOST_CHECK_EQUAL(typeid(p.some_list) == typeid(std::optional<std::vector<double>>), true);
+ BOOST_CHECK_EQUAL(typeid(p.some_list_with_optional_list) == typeid(std::optional<std::vector<std::optional<std::vector<std::optional<float>>>>>), true);
+ BOOST_CHECK_EQUAL(typeid(p.some_list_with_optional_type) == typeid(std::vector<std::optional<double>>), true);
+ BOOST_CHECK_EQUAL(typeid(p.some_obj) == typeid(std::optional<armarx::OptionalTestElement>), true);
+ BOOST_CHECK_EQUAL(typeid(p.some_string) == typeid(std::optional<std::string>), true);
+
+
+ auto aronType = p.toAronType();
+ BOOST_CHECK_EQUAL(aronType->getMemberType("some_float")->getMaybe() == aron::type::Maybe::eOptional, true);
+
+ BOOST_CHECK_EQUAL(p.some_float.has_value() == false, true);
+ auto aron = p.toAron();
+ BOOST_CHECK_EQUAL(aron->getElement("some_float") == nullptr, true);
+
+ p.some_float = 5.0f;
+ BOOST_CHECK_EQUAL(p.some_float.has_value() == true, true);
+ aron = p.toAron();
+ BOOST_CHECK_EQUAL(*aron->getElement("some_float") == std::make_shared<datanavigator::FloatNavigator>(5.0f), true);
+ BOOST_CHECK_EQUAL(*aron->getElement("some_float") == nullptr, false);
+
+}
diff --git a/source/RobotAPI/libraries/aron/core/test/aronDataWithoutCodeGeneration.h b/source/RobotAPI/libraries/aron/core/test/aronDataWithoutCodeGeneration.h
deleted file mode 100644
index 0e7e29371456eb156a786976eb838ab4444d5a9a..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/test/aronDataWithoutCodeGeneration.h
+++ /dev/null
@@ -1,335 +0,0 @@
-#pragma once
-
-#include <vector>
-
-#include <RobotAPI/libraries/aron/core/io/dataIO/writer/navigator/NavigatorWriter.h>
-#include <RobotAPI/libraries/aron/core/io/dataIO/reader/navigator/NavigatorReader.h>
-
-#include <RobotAPI/libraries/aron/core/io/dataIO/writer/navigator/NavigatorWriter.h>
-#include <RobotAPI/libraries/aron/core/io/dataIO/reader/navigator/NavigatorReader.h>
-
-#include <RobotAPI/libraries/aron/core/io/typeIO/writer/Writer.h>
-#include <RobotAPI/libraries/aron/core/io/typeIO/writer/navigator/NavigatorWriter.h>
-
-namespace armarx
-{
-
- class AronTrivialDataWithoutCodeGeneration
- {
- public:
- float f = 1;
- int i = 42;
-
- bool operator==(const AronTrivialDataWithoutCodeGeneration& x) const
- {
- return x.f == f && x.i == i;
- }
-
- void write(armarx::aron::io::AronDataWriter& w) const
- {
- w.writeStartDict();
- w.writeKey("f");
- w.writePrimitive(f);
- w.writeKey("i");
- w.writePrimitive(i);
- w.writeEndDict();
- }
-
- void writeType(armarx::aron::io::Writer& w) const
- {
- w.writeStartObjectType();
- w.writeObjectName("AronTrivialDataWithoutCodeGeneration");
- w.writeKey("f");
- w.writeFloatType();
- w.writeKey("i");
- w.writeIntType();
- w.writeEndObjectType();
- }
-
- void read(armarx::aron::io::AronDataReader& w)
- {
- f = 0;
- i = 0;
-
- w.readStartDict();
- w.readKey("f");
- w.readPrimitive(f);
- w.readKey("i");
- w.readPrimitive(i);
- w.readEndDict();
- }
-
- armarx::aron::datanavigator::DictNavigatorPtr toAron() const
- {
- armarx::aron::io::NavigatorWriter writer;
- this->write(writer);
- return armarx::aron::datanavigator::DictNavigator::DynamicCast(writer.getResult());
- }
-
- void fromAron(const armarx::aron::datanavigator::DictNavigatorPtr& input)
- {
- armarx::aron::io::NavigatorReader reader(input);
- //this->read(reader);
- }
-
- armarx::aron::typenavigator::ObjectNavigatorPtr toInitialAronType() const
- {
- armarx::aron::io::NavigatorWriter writer;
- this->writeType(writer);
- return armarx::aron::typenavigator::ObjectNavigator::DynamicCast(writer.getResult());
- }
- };
-
- class AronSimpleDataWithoutCodeGeneration
- {
- public:
- std::string str = "stringy";
- std::vector<int> vec_i = {37, 38, 39};
-
- bool operator==(const AronSimpleDataWithoutCodeGeneration& i) const
- {
- return i.str == str && i.vec_i == vec_i;
- }
-
- void write(armarx::aron::io::AronDataWriter& w) const
- {
- w.writeStartDict();
-
- w.writeKey("str");
- w.writePrimitive(str);
-
- w.writeKey("vec_i");
- w.writeStartList();
- for (const auto& e : vec_i)
- {
- w.writePrimitive(e);
- }
- w.writeEndList();
-
- w.writeEndDict();
- }
-
- void writeType(armarx::aron::io::Writer& w) const
- {
- w.writeStartObjectType();
- w.writeObjectName("AronSimpleDataWithoutCodeGeneration");
- w.writeKey("str");
- w.writeStringType();
- w.writeKey("vec_i");
- w.writeStartListType();
- w.writeIntType();
- w.writeEndListType();
- w.writeEndObjectType();
- }
-
- void read(armarx::aron::io::AronDataReader& w)
- {
- str = "";
- vec_i.clear();
-
- w.readStartDict();
-
- w.readKey("str");
- w.readPrimitive(str);
-
- w.readKey("vec_i");
- w.readStartList();
- while (!w.readEndList())
- {
- int e;
- w.readPrimitive(e);
- vec_i.push_back(e);
- }
- w.readEndDict();
- }
-
-
- armarx::aron::datanavigator::DictNavigatorPtr toAron() const
- {
- armarx::aron::io::NavigatorWriter writer;
- this->write(writer);
- return armarx::aron::datanavigator::DictNavigator::DynamicCast(writer.getResult());
- }
-
- void fromAron(const armarx::aron::datanavigator::DictNavigatorPtr& input)
- {
- //armarx::aron::io::NavigatorReader reader(input);
- //this->read(reader);
- }
-
- armarx::aron::typenavigator::ObjectNavigatorPtr toInitialAronType() const
- {
- armarx::aron::io::NavigatorWriter writer;
- this->writeType(writer);
- return armarx::aron::typenavigator::ObjectNavigator::DynamicCast(writer.getResult());
- }
- };
-
- class AronNestedDataWithoutCodeGeneration
- {
- public:
- AronTrivialDataWithoutCodeGeneration tri;
- std::vector<AronTrivialDataWithoutCodeGeneration> vec_tri{{}, {}, {}};
- AronSimpleDataWithoutCodeGeneration simp;
- std::vector<AronSimpleDataWithoutCodeGeneration> vec_simp{{}, {}};
-
- bool operator==(const AronNestedDataWithoutCodeGeneration& i) const
- {
- return i.tri == tri &&
- i.vec_tri == vec_tri &&
- simp == i.simp &&
- i.vec_simp == vec_simp;
- }
-
- void write(armarx::aron::io::AronDataWriter& w) const
- {
- w.writeStartDict();
-
- w.writeKey("tri");
- tri.write(w);
-
- w.writeKey("vec_tri");
- w.writeStartList();
- for (auto& e : vec_tri)
- {
- e.write(w);
- }
- w.writeEndList();
-
- w.writeKey("simp");
- simp.write(w);
- w.writeKey("vec_simp");
- w.writeStartList();
- for (auto& e : vec_simp)
- {
- e.write(w);
- }
- w.writeEndList();
- w.writeEndDict();
- }
-
- void writeType(armarx::aron::io::Writer& w) const
- {
- w.writeStartObjectType();
- w.writeObjectName("AronNestedDataWithoutCodeGeneration");
- w.writeKey("tri");
- tri.writeType(w);
-
- w.writeKey("vec_tri");
- w.writeStartListType();
- tri.writeType(w); // hack here
- w.writeEndListType();
-
- w.writeKey("simp");
- simp.writeType(w);
-
- w.writeKey("vec_simp");
- w.writeStartListType();
- simp.writeType(w); // hack here
- w.writeEndListType();
-
- w.writeEndObjectType();
- }
-
- void read(armarx::aron::io::AronDataReader& w)
- {
- // dont forget to reset!
- tri = AronTrivialDataWithoutCodeGeneration();
- vec_tri.clear();
- simp = AronSimpleDataWithoutCodeGeneration();
- vec_simp.clear();
-
- w.readStartDict();
-
- w.readKey("tri");
- tri.read(w);
-
- w.readKey("vec_tri");
- w.readStartList();
- while (!w.readEndList())
- {
- AronTrivialDataWithoutCodeGeneration e;
- e.read(w);
- vec_tri.push_back(e);
- }
-
- w.readKey("simp");
- simp.read(w);
-
- w.readKey("vec_simp");
- w.readStartList();
- while (!w.readEndList())
- {
- AronSimpleDataWithoutCodeGeneration e;
- e.read(w);
- vec_simp.push_back(e);
- }
- w.readEndDict();
- }
-
- armarx::aron::datanavigator::DictNavigatorPtr toAron() const
- {
- armarx::aron::io::NavigatorWriter writer;
- this->write(writer);
- return armarx::aron::datanavigator::DictNavigator::DynamicCast(writer.getResult());
- }
-
- void fromAron(const armarx::aron::datanavigator::DictNavigatorPtr& input)
- {
- armarx::aron::io::NavigatorReader reader(input);
- //this->read(reader);
- }
-
- armarx::aron::typenavigator::ObjectNavigatorPtr toInitialAronType() const
- {
- armarx::aron::io::NavigatorWriter writer;
- this->writeType(writer);
- return armarx::aron::typenavigator::ObjectNavigator::DynamicCast(writer.getResult());
- }
-
- };
-
- /*template<class T>
- void test(const auto& name)
- {
- std::cout << "############################\n" << name << '\n';
- T orig;
- T round;
- std::string orig_j;
- std::string round_j;
-
- {
- armarx::aron::io::AronDataJSONWriter jwrt_o;
- write(orig, jwrt_o);
- orig_j = jwrt_o.getResult();
- std::cout << "orig\n" << orig_j << '\n';
- }
-
- {
- armarx::aron::io::NavigatorWriter wrt;
- write(orig, wrt);
- armarx::aron::io::NavigatorReader rdr(wrt.getResult());
- read(round, rdr);
- }
-
- {
- armarx::aron::io::AronDataJSONWriter jwrt_r;
- write(round, jwrt_r);
- round_j = jwrt_r.getResult();
- std::cout << "round\n" << round_j << '\n';
- }
-
- std::cout << "equal? " << (orig_j == round_j) << '\n';
- }*/
-
- //#include <iostream>
- //int main()
- //{
- // test<trivial>("trivial");
- // test<simple >("simple");
- // test<nested >("nested");
- //
- // return 0;
- //}
-
-}
diff --git a/source/RobotAPI/libraries/aron/core/test/aronNavigateTest.cpp b/source/RobotAPI/libraries/aron/core/test/aronNavigateTest.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..233814ffd7c60a09fa9588093c7d1e2f726e282e
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/test/aronNavigateTest.cpp
@@ -0,0 +1,98 @@
+/*
+ * 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::aron
+ * @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
+ */
+
+#define BOOST_TEST_MODULE RobotAPI::ArmarXLibraries::aron
+
+#define ARMARX_BOOST_TEST
+
+// STD/STL
+#include <iostream>
+#include <cstdlib>
+#include <ctime>
+#include <numeric>
+
+// Boost
+#include <boost/algorithm/string.hpp>
+
+// Test
+#include <RobotAPI/Test.h>
+
+// ArmarX
+#include <ArmarXCore/libraries/cppgen/CppMethod.h>
+#include <ArmarXCore/libraries/cppgen/CppClass.h>
+#include <RobotAPI/libraries/aron/core/Exception.h>
+
+// Aron
+#include <RobotAPI/libraries/aron/core/Debug.h>
+#include <RobotAPI/libraries/aron/core/Randomizer.h>
+
+// Generated File
+#include <RobotAPI/libraries/aron/core/test/aron/NaturalIKTest.aron.generated.h>
+
+using namespace armarx;
+using namespace aron;
+
+BOOST_AUTO_TEST_CASE(AronNavigateTest)
+{
+ std::cout << "Running navigate test" << std::endl;
+ NaturalIKResult k;
+ NaturalIKResult k2;
+
+ // test Path
+ datanavigator::DictNavigatorPtr aron = k.toAron();
+ Path path = aron->getPath(); // should be empty since aron is top level object
+ Path memberReached(path, "reached");
+ Path memberJointValues(path, "jointValues");
+ Path indexJointValues0(memberJointValues, "0");
+ Path indexJointValues1(memberJointValues, "1");
+
+ BOOST_CHECK_EQUAL(path.toString(), "\\");
+ BOOST_CHECK_EQUAL(path.size(), 0);
+
+ BOOST_CHECK_EQUAL(memberReached.toString(), "\\->reached");
+ BOOST_CHECK_EQUAL(memberReached.size(), 1);
+
+ BOOST_CHECK_EQUAL(memberJointValues.toString(), "\\->jointValues");
+ BOOST_CHECK_EQUAL(memberJointValues.size(), 1);
+
+ BOOST_CHECK_EQUAL(indexJointValues0.toString(), "\\->jointValues->0");
+ BOOST_CHECK_EQUAL(indexJointValues0.size(), 2);
+
+ BOOST_CHECK_EQUAL(indexJointValues1.toString(), "\\->jointValues->1");
+ BOOST_CHECK_EQUAL(indexJointValues1.size(), 2);
+
+ datanavigator::BoolNavigatorPtr reached = datanavigator::BoolNavigator::DynamicCastAndCheck(aron->navigateAbsolute(memberReached));
+ datanavigator::ListNavigatorPtr jointValues = datanavigator::ListNavigator::DynamicCastAndCheck(aron->navigateAbsolute(memberJointValues));
+
+ if (jointValues->childrenSize() > 0)
+ {
+ datanavigator::FloatNavigatorPtr el = datanavigator::FloatNavigator::DynamicCastAndCheck(aron->navigateAbsolute(indexJointValues0));
+ }
+ if (jointValues->childrenSize() > 1)
+ {
+ datanavigator::FloatNavigatorPtr el = datanavigator::FloatNavigator::DynamicCastAndCheck(aron->navigateAbsolute(indexJointValues1));
+ }
+
+ Path diff = indexJointValues1.getWithoutPrefix(indexJointValues0);
+ BOOST_CHECK_EQUAL(diff.toString(), "\\->1");
+ BOOST_CHECK_EQUAL(diff.size(), 1);
+}
diff --git a/source/RobotAPI/libraries/aron/core/test/aronOperatorTest.cpp b/source/RobotAPI/libraries/aron/core/test/aronOperatorTest.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..539b6fb0449f1ba3e7258c502e28602a07a5b796
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/test/aronOperatorTest.cpp
@@ -0,0 +1,94 @@
+/*
+ * 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::aron
+ * @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
+ */
+
+#define BOOST_TEST_MODULE RobotAPI::ArmarXLibraries::aron
+
+#define ARMARX_BOOST_TEST
+
+// STD/STL
+#include <iostream>
+#include <cstdlib>
+#include <ctime>
+#include <numeric>
+
+// Boost
+#include <boost/algorithm/string.hpp>
+
+// Test
+#include <RobotAPI/Test.h>
+
+// ArmarX
+#include <ArmarXCore/libraries/cppgen/CppMethod.h>
+#include <ArmarXCore/libraries/cppgen/CppClass.h>
+#include <RobotAPI/libraries/aron/core/Exception.h>
+
+// Aron
+#include <RobotAPI/libraries/aron/core/Debug.h>
+#include <RobotAPI/libraries/aron/core/navigator/data/AllNavigators.h>
+
+using namespace armarx;
+using namespace aron;
+
+BOOST_AUTO_TEST_CASE(AronAssignmentTest)
+{
+ std::cout << "Aron assignment test" << std::endl;
+ float f = 5.0;
+ double d = 42.0;
+ int i = 666;
+ long l = 123;
+ std::string s = "Hello World";
+ bool b = true;
+
+ aron::datanavigator::FloatNavigator fn = f;
+ aron::datanavigator::DoubleNavigator dn = d;
+ aron::datanavigator::IntNavigator in = i;
+ aron::datanavigator::LongNavigator ln = l;
+ aron::datanavigator::StringNavigator sn = s;
+ aron::datanavigator::BoolNavigator bn = b;
+
+ BOOST_CHECK_EQUAL((float) fn == f, true);
+ BOOST_CHECK_EQUAL((double) dn == d, true);
+ BOOST_CHECK_EQUAL((int) in == i, true);
+ BOOST_CHECK_EQUAL((long) ln == l, true);
+ BOOST_CHECK_EQUAL((std::string) sn == s, true);
+ BOOST_CHECK_EQUAL((bool) bn == b, true);
+}
+
+BOOST_AUTO_TEST_CASE(AronEqualsTest)
+{
+ std::cout << "Aron equals test" << std::endl;
+ aron::datanavigator::FloatNavigator fn = 5.0;
+ aron::datanavigator::FloatNavigator fn_equals = 5.0;
+ aron::datanavigator::FloatNavigator fn_unequals = 6.0;
+
+ BOOST_CHECK_EQUAL(fn == fn_equals, true);
+ BOOST_CHECK_EQUAL(fn == fn_unequals, false);
+
+ auto fn_ptr_equals = std::make_shared<aron::datanavigator::FloatNavigator>(5.0);
+ auto fn_ptr_unequals = std::make_shared<aron::datanavigator::FloatNavigator>(42.0);
+
+ BOOST_CHECK_EQUAL(fn == fn_ptr_equals, true);
+ BOOST_CHECK_EQUAL(fn == fn_ptr_unequals, false);
+}
+
+// TODO more tests (fabian.peller)
+
diff --git a/source/RobotAPI/libraries/aron/core/test/aronRandomizedTest.cpp b/source/RobotAPI/libraries/aron/core/test/aronRandomizedTest.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..d579f3a6dab0e0a432b8a383f47be4496dcca137
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/test/aronRandomizedTest.cpp
@@ -0,0 +1,375 @@
+/*
+ * 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::aron
+ * @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
+ */
+
+#define BOOST_TEST_MODULE RobotAPI::ArmarXLibraries::aron
+
+#define ARMARX_BOOST_TEST
+
+// STD/STL
+#include <iostream>
+#include <cstdlib>
+#include <ctime>
+#include <numeric>
+
+// IVT
+#include <Image/ByteImage.h>
+
+// Eigen
+#include <Eigen/Core>
+
+// OpenCV
+#include <opencv2/core/core.hpp>
+
+// PCL
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+
+// Boost
+#include <boost/algorithm/string.hpp>
+
+// Test
+#include <RobotAPI/Test.h>
+
+// ArmarX
+#include <ArmarXCore/libraries/cppgen/CppMethod.h>
+#include <ArmarXCore/libraries/cppgen/CppClass.h>
+#include <RobotAPI/libraries/aron/core/Exception.h>
+
+// Aron
+#include <RobotAPI/libraries/aron/core/Debug.h>
+#include <RobotAPI/libraries/aron/core/Randomizer.h>
+
+#include <RobotAPI/libraries/aron/core/io/dataIO/reader/navigator/NavigatorReader.h>
+#include <RobotAPI/libraries/aron/core/io/dataIO/reader/nlohmannJSON/NlohmannJSONReader.h>
+
+#include <RobotAPI/libraries/aron/core/io/dataIO/writer/navigator/NavigatorWriter.h>
+#include <RobotAPI/libraries/aron/core/io/dataIO/writer/nlohmannJSON/NlohmannJSONWriter.h>
+
+#include <RobotAPI/libraries/aron/core/io/typeIO/reader/navigator/NavigatorReader.h>
+#include <RobotAPI/libraries/aron/core/io/typeIO/reader/nlohmannJSON/NlohmannJSONReader.h>
+
+#include <RobotAPI/libraries/aron/core/io/typeIO/writer/navigator/NavigatorWriter.h>
+#include <RobotAPI/libraries/aron/core/io/typeIO/writer/nlohmannJSON/NlohmannJSONWriter.h>
+
+// Files without code generation
+//#include "aronDataWithoutCodeGeneration.h"
+
+// Generated File
+#include <RobotAPI/libraries/aron/core/test/aron/ListTest.aron.generated.h>
+#include <RobotAPI/libraries/aron/core/test/aron/DictTest.aron.generated.h>
+#include <RobotAPI/libraries/aron/core/test/aron/PrimitiveTest.aron.generated.h>
+#include <RobotAPI/libraries/aron/core/test/aron/ObjectTest.aron.generated.h>
+#include <RobotAPI/libraries/aron/core/test/aron/IVTCByteImageTest.aron.generated.h>
+#include <RobotAPI/libraries/aron/core/test/aron/EigenMatrixTest.aron.generated.h>
+#include <RobotAPI/libraries/aron/core/test/aron/EigenQuaternionTest.aron.generated.h>
+#include <RobotAPI/libraries/aron/core/test/aron/OpenCVMatTest.aron.generated.h>
+#include <RobotAPI/libraries/aron/core/test/aron/PCLPointCloudTest.aron.generated.h>
+#include <RobotAPI/libraries/aron/core/test/aron/PositionTest.aron.generated.h>
+#include <RobotAPI/libraries/aron/core/test/aron/OrientationTest.aron.generated.h>
+#include <RobotAPI/libraries/aron/core/test/aron/PoseTest.aron.generated.h>
+#include <RobotAPI/libraries/aron/core/test/aron/EnumTest.aron.generated.h>
+#include <RobotAPI/libraries/aron/core/test/aron/OptionalTest.aron.generated.h>
+
+using namespace armarx;
+using namespace aron;
+
+template <typename T>
+void runTestWithInstances(T& k1, T& k2)
+{
+ // assumes not nullptrs as k1 and k2. If you have a maybe type then make sure that it is set properly
+
+ int echo_debug_lv = 1;
+
+ Randomizer r;
+
+ {
+ if (echo_debug_lv)
+ {
+ std::cout << "\t getting type 1" << std::endl;
+ }
+ typenavigator::ObjectNavigatorPtr k1_type_nav = k1.toAronType();
+ type::AronObjectPtr k1_type = k1_type_nav->toAronObjectPtr();
+ BOOST_CHECK_EQUAL((bool) k1_type, true);
+
+ armarx::aron::typeIO::writer::NlohmannJSONWriter json_writer_k1;
+ k1.writeType(json_writer_k1);
+ nlohmann::json k1_type_json = json_writer_k1.getResult();
+ BOOST_CHECK_EQUAL(k1_type_json.is_object(), true);
+
+ if (echo_debug_lv)
+ {
+ std::cout << "\t getting type 2" << std::endl;
+ }
+ typenavigator::ObjectNavigatorPtr k2_type_nav = k2.toAronType();
+ type::AronObjectPtr k2_type = k2_type_nav->toAronObjectPtr();
+ BOOST_CHECK_EQUAL((bool) k2_type, true);
+
+ armarx::aron::typeIO::writer::NlohmannJSONWriter json_writer_k2;
+ k2.writeType(json_writer_k2);
+ nlohmann::json k2_type_json = json_writer_k2.getResult();
+ BOOST_CHECK_EQUAL(k2_type_json.is_object(), true);
+ BOOST_CHECK_EQUAL(k1_type_json.dump(2), k2_type_json.dump(2));
+ }
+
+ {
+ typenavigator::ObjectNavigatorPtr k1_type_nav = k1.toAronType();
+ datanavigator::DictNavigatorPtr k1_aron_nav = k1.toAron();
+
+ if (echo_debug_lv)
+ {
+ std::cout << "\t initialize aron 1 randomly" << std::endl;
+ }
+ r.initializeRandomly(k1_aron_nav, k1_type_nav);
+
+ if (echo_debug_lv)
+ {
+ std::cout << "\t getting aron 1" << std::endl;
+ }
+ data::AronDictPtr k1_aron = k1_aron_nav->toAronDictPtr();
+ BOOST_CHECK_EQUAL((bool) k1_aron, true);
+
+ if (echo_debug_lv >= 2)
+ {
+ std::cout << "K1 Aron:" << std::endl;
+ std::cout << armarx::aron::Debug::AronDataPtrToString(k1_aron) << std::endl;
+ std::cout << "K2 Aron:" << std::endl;
+ std::cout << armarx::aron::Debug::AronDataPtrToString(k2.toAron()->toAronDictPtr()) << std::endl;
+ }
+
+ if (echo_debug_lv)
+ {
+ std::cout << "\t setting aron 2 from aron 1" << std::endl;
+ }
+ k2.fromAron(k1_aron);
+
+ if (echo_debug_lv)
+ {
+ std::cout << "\t getting aron 2" << std::endl;
+ }
+ datanavigator::DictNavigatorPtr k2_aron_nav = k2.toAron();
+ BOOST_CHECK_EQUAL((*k1_aron_nav == *k2_aron_nav), true);
+
+ data::AronDictPtr k2_aron = k2_aron_nav->toAronDictPtr();
+ BOOST_CHECK_EQUAL((bool) k2_aron, true);
+
+ if (echo_debug_lv)
+ {
+ std::cout << "\t setting aron 1 from aron 2 and check for equality" << std::endl;
+ }
+ k1.fromAron(k2_aron_nav);
+
+ if (echo_debug_lv >= 2)
+ {
+ std::cout << "K1 Aron:" << std::endl;
+ std::cout << armarx::aron::Debug::AronDataPtrToString(k1.toAron()->toAronDictPtr()) << std::endl;
+ std::cout << "K2 Aron:" << std::endl;
+ std::cout << armarx::aron::Debug::AronDataPtrToString(k2.toAron()->toAronDictPtr()) << std::endl;
+ }
+ BOOST_CHECK_EQUAL((k1 == k2), true);
+
+ datanavigator::DictNavigatorPtr k1_aron_nav_again = k1.toAron();
+ BOOST_CHECK_EQUAL((*k1_aron_nav == *k1_aron_nav_again), true);
+ }
+
+ {
+ typenavigator::ObjectNavigatorPtr k1_type_nav = k1.toAronType();
+ datanavigator::DictNavigatorPtr k1_aron_nav = k1.toAron();
+
+ r.initializeRandomly(k1_aron_nav, k1_type_nav);
+
+ data::AronDictPtr k1_aron = k1_aron_nav->toAronDictPtr();
+ k1.fromAron(k1_aron);
+
+ if (echo_debug_lv)
+ {
+ std::cout << "\t check JSON export of k and k2 for equality" << std::endl;
+ }
+ armarx::aron::dataIO::writer::NlohmannJSONWriter json_writer_for_k1;
+ armarx::aron::dataIO::writer::NlohmannJSONWriter json_writer_for_k2;
+
+ k1.write(json_writer_for_k1);
+ nlohmann::json k1_aron_json = json_writer_for_k1.getResult();
+ std::string k1_aron_json_str = k1_aron_json.dump(4);
+
+ armarx::aron::dataIO::writer::NlohmannJSONWriter direct_json_writer_for_k1;
+ armarx::aron::dataIO::Visitor::VisitAndSetup(direct_json_writer_for_k1, k1_aron);
+ nlohmann::json direct_k1_aron_json = direct_json_writer_for_k1.getResult();
+ std::string direct_k1_aron_json_str = direct_k1_aron_json.dump(4);
+
+ if (echo_debug_lv >= 2)
+ {
+ std::cout << "\t K1 as json: " << std::endl << k1_aron_json_str << std::endl;
+ std::cout << "\t K1 as direct json: " << std::endl << direct_k1_aron_json_str << std::endl;
+ }
+ BOOST_CHECK_EQUAL((k1_aron_json_str == direct_k1_aron_json_str), true);
+
+ armarx::aron::dataIO::reader::NlohmannJSONReader json_reader_for_k2(k1_aron_json);
+
+ k2.read(json_reader_for_k2);
+ k2.write(json_writer_for_k2);
+
+ nlohmann::json k2_aron_json = json_writer_for_k2.getResult();
+ std::string k2_aron_json_str = k2_aron_json.dump(4);
+
+ if (echo_debug_lv >= 2)
+ {
+ std::cout << "\t K1 as json: " << std::endl << k1_aron_json_str << std::endl;
+ std::cout << "\t K2 as json: " << std::endl << k2_aron_json_str << std::endl;
+ }
+ BOOST_CHECK_EQUAL((k1_aron_json_str == k2_aron_json_str), true);
+ }
+}
+
+BOOST_AUTO_TEST_CASE(AronListTest)
+{
+ std::cout << "Running List test" << std::endl;
+ ListTest l;
+ ListTest l2;
+ runTestWithInstances<ListTest>(l, l2);
+}
+
+BOOST_AUTO_TEST_CASE(AronDictTest)
+{
+ std::cout << "Running Dict test" << std::endl;
+ DictTest d;
+ DictTest d2;
+ runTestWithInstances<DictTest>(d, d2);
+}
+
+BOOST_AUTO_TEST_CASE(AronPrimitiveTest)
+{
+ std::cout << "Running Primitive test" << std::endl;
+ PrimitiveTest p;
+ PrimitiveTest p2;
+ runTestWithInstances<PrimitiveTest>(p, p2);
+}
+
+BOOST_AUTO_TEST_CASE(AronObjectTest)
+{
+ std::cout << "Running Object test" << std::endl;
+ ObjectTest1 o1;
+ ObjectTest1 o12;
+ runTestWithInstances<ObjectTest1>(o1, o12);
+
+ ObjectTest2 o2;
+ ObjectTest2 o22;
+ runTestWithInstances<ObjectTest2>(o2, o22);
+}
+
+BOOST_AUTO_TEST_CASE(AronImageTest)
+{
+ std::cout << "Running Image test" << std::endl;
+ IVTCByteImageTest ii;
+ IVTCByteImageTest ii2;
+
+ ii.the_grayscale_image = std::make_shared<CByteImage>();
+ ii2.the_grayscale_image = std::make_shared<CByteImage>();
+
+ ii.the_rgb_image = std::make_shared<CByteImage>();
+ ii2.the_rgb_image = std::make_shared<CByteImage>();
+
+ ii.the_grayscale_image->Set(5, 5, CByteImage::eGrayScale);
+ ii2.the_grayscale_image->Set(7, 7, CByteImage::eGrayScale);
+
+ ii.the_rgb_image->Set(5, 5, CByteImage::eRGB24);
+ ii2.the_rgb_image->Set(7, 7, CByteImage::eRGB24);
+
+ runTestWithInstances<IVTCByteImageTest>(ii, ii2);
+}
+
+BOOST_AUTO_TEST_CASE(AronEigenMatrixTest)
+{
+ // Eigen may cause problems with dimensions > 145
+ std::cout << "Running EigenMatrix test" << std::endl;
+ EigenMatrixTest em;
+ EigenMatrixTest em2;
+ runTestWithInstances<EigenMatrixTest>(em, em2);
+}
+
+BOOST_AUTO_TEST_CASE(AronOpenCVTest)
+{
+ std::cout << "Running OpenCVMat test" << std::endl;
+ OpenCVMatTest ocv;
+ OpenCVMatTest ocv2;
+
+ ocv.the_2d_opencv_matrix = cv::Mat(2, 2, CV_64F);
+ ocv.the_3d_opencv_matrix = cv::Mat(std::vector<int>({2, 2, 2}), CV_32F);
+ ocv.the_4d_opencv_matrix = cv::Mat(std::vector<int>({2, 2, 2, 2}), CV_16S);
+ ocv.the_5d_opencv_matrix = cv::Mat(std::vector<int>({2, 2, 2, 2, 2}), CV_8U);
+
+ runTestWithInstances<OpenCVMatTest>(ocv, ocv2);
+}
+
+BOOST_AUTO_TEST_CASE(AronPCLPointCloudTest)
+{
+ std::cout << "Running PCLPointCloud test" << std::endl;
+ PointCloudTest pc;
+ PointCloudTest pc2;
+
+ pc.the_xyzrgb_pcl_pointcloud = pcl::PointCloud<pcl::PointXYZRGB>(5, 5);
+ pc.the_xyzrgba_pcl_pointcloud = pcl::PointCloud<pcl::PointXYZRGBA>(7, 7);
+
+ pc2.the_xyzrgb_pcl_pointcloud = pcl::PointCloud<pcl::PointXYZRGB>(4, 4);
+ pc2.the_xyzrgba_pcl_pointcloud = pcl::PointCloud<pcl::PointXYZRGBA>(3, 3);
+
+ runTestWithInstances<PointCloudTest>(pc, pc2);
+}
+
+BOOST_AUTO_TEST_CASE(AronEigenQuaternionTest)
+{
+ std::cout << "Running EigenQuaternion test" << std::endl;
+ EigenQuaternionTest eq;
+ EigenQuaternionTest eq2;
+ runTestWithInstances<EigenQuaternionTest>(eq, eq2);
+}
+
+BOOST_AUTO_TEST_CASE(AronPositionTest)
+{
+ std::cout << "Running Position test" << std::endl;
+ PositionTest pc;
+ PositionTest pc2;
+ runTestWithInstances<PositionTest>(pc, pc2);
+}
+
+BOOST_AUTO_TEST_CASE(AronOrientationTest)
+{
+ std::cout << "Running Orientation test" << std::endl;
+ OrientationTest pc;
+ OrientationTest pc2;
+ runTestWithInstances<OrientationTest>(pc, pc2);
+}
+
+BOOST_AUTO_TEST_CASE(AronPoseTest)
+{
+ std::cout << "Running Pose test" << std::endl;
+ PoseTest pc;
+ PoseTest pc2;
+ runTestWithInstances<PoseTest>(pc, pc2);
+}
+
+BOOST_AUTO_TEST_CASE(AronOptionalTest)
+{
+ std::cout << "Running Optional test" << std::endl;
+ OptionalTest pc;
+ OptionalTest pc2;
+ runTestWithInstances<OptionalTest>(pc, pc2);
+}
diff --git a/source/RobotAPI/libraries/aron/core/test/aronTest.cpp b/source/RobotAPI/libraries/aron/core/test/aronTest.cpp
deleted file mode 100644
index e8fd551be59727494f1c92040d81a8bddf1fe8bd..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/test/aronTest.cpp
+++ /dev/null
@@ -1,352 +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::aron
- * @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
- */
-
-#define BOOST_TEST_MODULE RobotAPI::ArmarXLibraries::aron
-
-#define ARMARX_BOOST_TEST
-
-// STD/STL
-#include <iostream>
-#include <cstdlib>
-#include <ctime>
-#include <numeric>
-
-// IVT
-#include <Image/ByteImage.h>
-
-// Eigen
-#include <Eigen/Core>
-
-// OpenCV
-#include <opencv2/core/core.hpp>
-
-// PCL
-#include <pcl/point_cloud.h>
-#include <pcl/point_types.h>
-
-// Boost
-#include <boost/algorithm/string.hpp>
-
-// Test
-#include <RobotAPI/Test.h>
-
-// ArmarX
-#include <ArmarXCore/libraries/cppgen/CppMethod.h>
-#include <ArmarXCore/libraries/cppgen/CppClass.h>
-#include <RobotAPI/libraries/aron/core/Exception.h>
-
-// Aron
-#include <RobotAPI/libraries/aron/core/Debug.h>
-#include <RobotAPI/libraries/aron/core/Randomizer.h>
-
-#include <RobotAPI/libraries/aron/core/io/dataIO/reader/navigator/NavigatorReader.h>
-#include <RobotAPI/libraries/aron/core/io/dataIO/reader/nlohmannJSON/NlohmannJSONReader.h>
-
-#include <RobotAPI/libraries/aron/core/io/dataIO/writer/navigator/NavigatorWriter.h>
-#include <RobotAPI/libraries/aron/core/io/dataIO/writer/nlohmannJSON/NlohmannJSONWriter.h>
-
-// Files without code generation
-//#include "aronDataWithoutCodeGeneration.h"
-
-// Generated File
-#include <RobotAPI/libraries/aron/core/test/aron/NaturalIKTest.aron.generated.h>
-#include <RobotAPI/libraries/aron/core/test/aron/ListTest.aron.generated.h>
-#include <RobotAPI/libraries/aron/core/test/aron/DictTest.aron.generated.h>
-#include <RobotAPI/libraries/aron/core/test/aron/PrimitiveTest.aron.generated.h>
-#include <RobotAPI/libraries/aron/core/test/aron/ObjectTest.aron.generated.h>
-#include <RobotAPI/libraries/aron/core/test/aron/IVTCByteImageTest.aron.generated.h>
-#include <RobotAPI/libraries/aron/core/test/aron/EigenMatrixTest.aron.generated.h>
-#include <RobotAPI/libraries/aron/core/test/aron/EigenQuaternionTest.aron.generated.h>
-#include <RobotAPI/libraries/aron/core/test/aron/OpenCVMatTest.aron.generated.h>
-#include <RobotAPI/libraries/aron/core/test/aron/PCLPointCloudTest.aron.generated.h>
-#include <RobotAPI/libraries/aron/core/test/aron/PositionTest.aron.generated.h>
-#include <RobotAPI/libraries/aron/core/test/aron/OrientationTest.aron.generated.h>
-#include <RobotAPI/libraries/aron/core/test/aron/PoseTest.aron.generated.h>
-#include <RobotAPI/libraries/aron/core/test/aron/EnumTest.aron.generated.h>
-
-using namespace armarx;
-using namespace aron;
-
-template <typename T>
-void runTestWithInstances(T& k1, T& k2)
-{
- Randomizer r;
-
- std::cout << "\t getting type 1" << std::endl;
- typenavigator::ObjectNavigatorPtr k1_type_nav = k1.toInitialAronType();
- type::AronObjectPtr k1_type = k1_type_nav->toAronObjectPtr();
- BOOST_CHECK_NE(k1_type.get(), nullptr);
-
- std::cout << "\t getting type 2" << std::endl;
- typenavigator::ObjectNavigatorPtr k2_type_nav = k2.toInitialAronType();
- type::AronObjectPtr k2_type = k2_type_nav->toAronObjectPtr();
- BOOST_CHECK_NE(k2_type.get(), nullptr);
-
- std::cout << "\t getting aron 1" << std::endl;
- datanavigator::DictNavigatorPtr k1_aron_nav = k1.toAron();
- data::AronDictPtr k1_aron = k1_aron_nav->toAronDictPtr();
- BOOST_CHECK_NE(k1_aron.get(), nullptr);
-
- std::cout << "\t initialize aron 1 randomly" << std::endl;
- r.initializeRandomly(k1_aron_nav, k1_type_nav);
-
- //std::cout << "K Aron:" << std::endl;
- //std::cout << AronDebug::AronDataPtrToString(k_aron) << std::endl;
-
- std::cout << "\t setting aron 2 from aron 1" << std::endl;
- k2.fromAron(k1_aron);
-
- std::cout << "\t getting aron 2" << std::endl;
- datanavigator::DictNavigatorPtr k2_aron_nav = k2.toAron();
- data::AronDictPtr k2_aron = k2_aron_nav->toAronDictPtr();
- BOOST_CHECK_NE(k2_aron.get(), nullptr);
-
- //std::cout << "K2 Aron:" << std::endl;
- //std::cout << AronDebug::AronDataPtrToString(k2_aron) << std::endl;
-
- std::cout << "\t setting aron 1 from aron 2 and check for equality" << std::endl;
-
- k1.fromAron(k2_aron_nav);
-
- BOOST_CHECK_EQUAL((k1 == k2), true);
-
-
- //std::cout << "\t getting current type 1" << std::endl;
- //typenavigator::NavigatorPtr k1_current_type_nav = k1.toCurrentAronType();
- //type::AronTypePtr k1_current_type = k1_current_type_nav->getResult();
- //BOOST_CHECK_NE(k1_current_type.get(), nullptr);
-
- //std::cout << "\t getting type 2" << std::endl;
- //typenavigator::NavigatorPtr k2_current_type_nav = k2.toCurrentAronType();
- //type::AronTypePtr k2_current_type = k2_current_type_nav->getResult();
- //BOOST_CHECK_NE(k2_current_type.get(), nullptr);
-
- r.initializeRandomly(k1_aron_nav, k1_type_nav);
-
- k1.fromAron(k1_aron);
-
-
- std::cout << "\t check JSON export of k and k2 for equality" << std::endl;
- armarx::aron::dataIO::writer::NlohmannJSONWriter json_writer_for_k1;
- armarx::aron::dataIO::writer::NlohmannJSONWriter json_writer_for_k2;
-
- k1.write(json_writer_for_k1);
- nlohmann::json k1_aron_json = json_writer_for_k1.getResult();
- std::string k1_aron_json_str = k1_aron_json.dump(4);
-
- armarx::aron::dataIO::writer::NlohmannJSONWriter direct_json_writer_for_k1;
- armarx::aron::dataIO::Visitor::VisitAndSetup(direct_json_writer_for_k1, k1_aron);
- nlohmann::json direct_k1_aron_json = direct_json_writer_for_k1.getResult();
- std::string direct_k1_aron_json_str = direct_k1_aron_json.dump(4);
- BOOST_CHECK_EQUAL((k1_aron_json_str == direct_k1_aron_json_str), true);
-
- armarx::aron::dataIO::reader::NlohmannJSONReader json_reader_for_k2(k1_aron_json);
- k2.read(json_reader_for_k2);
-
- k2.write(json_writer_for_k2);
- nlohmann::json k2_aron_json = json_writer_for_k2.getResult();
- std::string k2_aron_json_str = k2_aron_json.dump(4);
-
- //std::cout << "\t K1 as json: " << std::endl << k1_aron_json_str << std::endl;
- //std::cout << "\t K2 as json: " << std::endl << k2_aron_json_str << std::endl;
- BOOST_CHECK_EQUAL((k1_aron_json_str == k2_aron_json_str), true);
-}
-
-
-BOOST_AUTO_TEST_CASE(AronNaturalIKTest)
-{
- std::cout << "Running NaturalIK test" << std::endl;
- NaturalIKResult k;
- NaturalIKResult k2;
- runTestWithInstances<NaturalIKResult>(k, k2);
-
- // also test Path
- datanavigator::DictNavigatorPtr aron = k.toAron();
- Path path = aron->getPath(); // should be empty since aron is top level object
- Path memberReached(path, "reached");
- Path memberJointValues(path, "jointValues");
- Path indexJointValues0(memberJointValues, "0");
- Path indexJointValues1(memberJointValues, "1");
-
- BOOST_CHECK_EQUAL(path.toString(), "\\");
- BOOST_CHECK_EQUAL(path.size(), 0);
-
- BOOST_CHECK_EQUAL(memberReached.toString(), "\\->reached");
- BOOST_CHECK_EQUAL(memberReached.size(), 1);
-
- BOOST_CHECK_EQUAL(memberJointValues.toString(), "\\->jointValues");
- BOOST_CHECK_EQUAL(memberJointValues.size(), 1);
-
- BOOST_CHECK_EQUAL(indexJointValues0.toString(), "\\->jointValues->0");
- BOOST_CHECK_EQUAL(indexJointValues0.size(), 2);
-
- BOOST_CHECK_EQUAL(indexJointValues1.toString(), "\\->jointValues->1");
- BOOST_CHECK_EQUAL(indexJointValues1.size(), 2);
-
- datanavigator::BoolNavigatorPtr reached = datanavigator::BoolNavigator::DynamicCastAndCheck(aron->navigateAbsolute(memberReached));
- datanavigator::ListNavigatorPtr jointValues = datanavigator::ListNavigator::DynamicCastAndCheck(aron->navigateAbsolute(memberJointValues));
-
- if (jointValues->childrenSize() > 0)
- {
- datanavigator::FloatNavigatorPtr el = datanavigator::FloatNavigator::DynamicCastAndCheck(aron->navigateAbsolute(indexJointValues0));
- }
- if (jointValues->childrenSize() > 1)
- {
- datanavigator::FloatNavigatorPtr el = datanavigator::FloatNavigator::DynamicCastAndCheck(aron->navigateAbsolute(indexJointValues1));
- }
-
- Path diff = indexJointValues1.getWithoutPrefix(indexJointValues0);
- BOOST_CHECK_EQUAL(diff.toString(), "\\->1");
- BOOST_CHECK_EQUAL(diff.size(), 1);
-}
-
-
-BOOST_AUTO_TEST_CASE(AronListTest)
-{
- std::cout << "Running List test" << std::endl;
- ListTest l;
- ListTest l2;
- runTestWithInstances<ListTest>(l, l2);
-}
-
-BOOST_AUTO_TEST_CASE(AronDictTest)
-{
- std::cout << "Running Dict test" << std::endl;
- DictTest d;
- DictTest d2;
- runTestWithInstances<DictTest>(d, d2);
-}
-
-BOOST_AUTO_TEST_CASE(AronPrimitiveTest)
-{
- std::cout << "Running Primitive test" << std::endl;
- PrimitiveTest p;
- PrimitiveTest p2;
- runTestWithInstances<PrimitiveTest>(p, p2);
-}
-
-BOOST_AUTO_TEST_CASE(AronObjectTest)
-{
- std::cout << "Running Object test" << std::endl;
- ObjectTest1 o1;
- ObjectTest1 o12;
- runTestWithInstances<ObjectTest1>(o1, o12);
-
- ObjectTest2 o2;
- ObjectTest2 o22;
- runTestWithInstances<ObjectTest2>(o2, o22);
-}
-
-BOOST_AUTO_TEST_CASE(AronImageTest)
-{
- std::cout << "Running Image test" << std::endl;
- IVTCByteImageTest i;
- IVTCByteImageTest i2;
- runTestWithInstances<IVTCByteImageTest>(i, i2);
-}
-
-/*BOOST_AUTO_TEST_CASE(AronImageChangeSizeTest)
-{
- std::cout << "Running Image test with changed sizes" << std::endl;
- IVTCByteImageTest ii;
- IVTCByteImageTest ii2;
- ii.the_ivt_image.Set(100, 100, ii.the_ivt_image.type);
- ii2.the_ivt_image.Set(10, 10, ii.the_ivt_image.type);
- runTestWithInstances<IVTCByteImageTest>(ii, ii2);
-}*/
-
-BOOST_AUTO_TEST_CASE(AronEigenMatrixTest)
-{
- std::cout << "Running EigenMatrix test" << std::endl;
- EigenMatrixTest em;
- EigenMatrixTest em2;
- runTestWithInstances<EigenMatrixTest>(em, em2);
-}
-
-BOOST_AUTO_TEST_CASE(AronOpenCVTest)
-{
- std::cout << "Running OpenCVMat test" << std::endl;
- OpenCVMatTest ocv;
- OpenCVMatTest ocv2;
- runTestWithInstances<OpenCVMatTest>(ocv, ocv2);
-}
-
-BOOST_AUTO_TEST_CASE(AronPCLPointCloudTest)
-{
- std::cout << "Running PCLPointCloud test" << std::endl;
- PointCloudTest pc;
- PointCloudTest pc2;
- runTestWithInstances<PointCloudTest>(pc, pc2);
-}
-
-/*
-BOOST_AUTO_TEST_CASE(AronTests)
-{
- initialize_random();
-
- std::cout << "Running (Raphael) Trivial test" << std::endl;
- AronTrivialDataWithoutCodeGeneration rt1;
- AronTrivialDataWithoutCodeGeneration rt2;
- runTestWithInstances<AronTrivialDataWithoutCodeGeneration>(rt1, rt2);
-
- std::cout << "Running (Raphael) Simple test" << std::endl;
- AronSimpleDataWithoutCodeGeneration rs1;
- AronSimpleDataWithoutCodeGeneration rs2;
- runTestWithInstances<AronSimpleDataWithoutCodeGeneration>(rs1, rs2);
-
- std::cout << "Running (Raphael) Nested test" << std::endl;
- AronNestedDataWithoutCodeGeneration rn1;
- AronNestedDataWithoutCodeGeneration rn2;
- runTestWithInstances<AronNestedDataWithoutCodeGeneration>(rn1, rn2);
-}*/
-
-BOOST_AUTO_TEST_CASE(AronEigenQuaternionTest)
-{
- std::cout << "Running EigenQuaternion test" << std::endl;
- EigenQuaternionTest eq;
- EigenQuaternionTest eq2;
- runTestWithInstances<EigenQuaternionTest>(eq, eq2);
-}
-
-BOOST_AUTO_TEST_CASE(AronPositionTest)
-{
- std::cout << "Running Position test" << std::endl;
- PositionTest pc;
- PositionTest pc2;
- runTestWithInstances<PositionTest>(pc, pc2);
-}
-
-BOOST_AUTO_TEST_CASE(AronOrientationTest)
-{
- std::cout << "Running Orientation test" << std::endl;
- OrientationTest pc;
- OrientationTest pc2;
- runTestWithInstances<OrientationTest>(pc, pc2);
-}
-
-BOOST_AUTO_TEST_CASE(AronPoseTest)
-{
- std::cout << "Running Pose test" << std::endl;
- PoseTest pc;
- PoseTest pc2;
- runTestWithInstances<PoseTest>(pc, pc2);
-}
diff --git a/source/RobotAPI/libraries/aron/core/test/xmls/EigenMatrixTest.xml b/source/RobotAPI/libraries/aron/core/test/xmls/EigenMatrixTest.xml
index b3f5f6447c6fc6f3d3a8851e00578156ad4560b3..59e994015fda4f1359f74c6cc6ac401cbb4f66fd 100644
--- a/source/RobotAPI/libraries/aron/core/test/xmls/EigenMatrixTest.xml
+++ b/source/RobotAPI/libraries/aron/core/test/xmls/EigenMatrixTest.xml
@@ -9,19 +9,19 @@
<GenerateTypes>
<Object name='armarx::EigenMatrixTest'>
<ObjectChild key='the_short_eigen_matrix'>
- <EigenMatrix rows="10" cols="25" type="short" />
+ <EigenMatrix rows="5" cols="7" type="short" />
</ObjectChild>
<ObjectChild key='the_int_eigen_matrix'>
- <EigenMatrix rows="25" cols="25" type="int" />
+ <EigenMatrix rows="7" cols="7" type="int" />
</ObjectChild>
<ObjectChild key='the_long_eigen_matrix'>
- <EigenMatrix rows="25" cols="10" type="long" />
+ <EigenMatrix rows="7" cols="5" type="long" />
</ObjectChild>
<ObjectChild key='the_float_eigen_matrix'>
- <EigenMatrix rows="1" cols="200" type="float" />
+ <EigenMatrix rows="1" cols="9" type="float" />
</ObjectChild>
<ObjectChild key='the_double_eigen_matrix'>
- <EigenMatrix rows="200" cols="1" type="double" />
+ <EigenMatrix rows="25" cols="1" type="double" />
</ObjectChild>
</Object>
</GenerateTypes>
diff --git a/source/RobotAPI/libraries/aron/core/test/xmls/IVTCByteImageTest.xml b/source/RobotAPI/libraries/aron/core/test/xmls/IVTCByteImageTest.xml
index 5abc5d5a15e3e3b17fc842e958a5216c862999a3..38ac4391e6a51647ac9f43529f9543303bdbced1 100644
--- a/source/RobotAPI/libraries/aron/core/test/xmls/IVTCByteImageTest.xml
+++ b/source/RobotAPI/libraries/aron/core/test/xmls/IVTCByteImageTest.xml
@@ -7,11 +7,11 @@
<Object name='armarx::IVTCByteImageTest'>
<ObjectChild key='the_grayscale_image'>
- <IVTCByteImage width="25" height="25" type="GrayScale" />
+ <IVTCByteImage width="25" height="25" type="GrayScale" shared_ptr="ja" />
</ObjectChild>
<ObjectChild key='the_rgb_image'>
- <IVTCByteImage width="1920" height="1080" type="RGB24" />
+ <IVTCByteImage width="1920" height="1080" type="RGB24" shared_ptr="ja" />
</ObjectChild>
</Object>
diff --git a/source/RobotAPI/libraries/aron/core/test/xmls/OptionalTest.xml b/source/RobotAPI/libraries/aron/core/test/xmls/OptionalTest.xml
new file mode 100644
index 0000000000000000000000000000000000000000..7b402cb38e4b904723c31ce7097ebc7623c296ba
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/test/xmls/OptionalTest.xml
@@ -0,0 +1,66 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+ <CodeIncludes>
+ <Include include="<Eigen/Core>" />
+ </CodeIncludes>
+ <GenerateTypes>
+ <Object name="armarx::OptionalTestElement">
+ <ObjectChild key="val">
+ <Float />
+ </ObjectChild>
+ </Object>
+ <Object name='armarx::OptionalTest'>
+
+ <ObjectChild key='some_float'>
+ <Float optional="1"/>
+ </ObjectChild>
+
+ <ObjectChild key='some_string'>
+ <String optional="true"/>
+ </ObjectChild>
+
+ <ObjectChild key='some_dict'>
+ <Dict optional="wahr">
+ <Float />
+ </Dict>
+ </ObjectChild>
+
+ <ObjectChild key='some_dict_with_optional_type'>
+ <Dict>
+ <Float optional="true" />
+ </Dict>
+ </ObjectChild>
+
+ <ObjectChild key='some_list'>
+ <List optional="ja">
+ <Double />
+ </List>
+ </ObjectChild>
+
+ <ObjectChild key='some_list_with_optional_type'>
+ <List>
+ <Double optional="true"/>
+ </List>
+ </ObjectChild>
+
+ <ObjectChild key='some_eigen_matrix'>
+ <EigenMatrix rows="25" cols="10" type="long" optional="ja"/>
+ </ObjectChild>
+
+ <ObjectChild key='some_obj'>
+ <armarx::OptionalTestElement optional="ja"/>
+ </ObjectChild>
+
+ <ObjectChild key='some_list_with_optional_list'>
+ <List optional="ja">
+ <List optional="ja">
+ <Float optional="ja" />
+ </List>
+ </List>
+ </ObjectChild>
+
+
+
+ </Object>
+ </GenerateTypes>
+</AronTypeDefinition>
diff --git a/source/RobotAPI/libraries/core/CartesianVelocityController.cpp b/source/RobotAPI/libraries/core/CartesianVelocityController.cpp
index f140f9512bcb990a82e50ed3eb271536817d4d88..9527835a6dd48107f5e11846ec882d30b056c54a 100644
--- a/source/RobotAPI/libraries/core/CartesianVelocityController.cpp
+++ b/source/RobotAPI/libraries/core/CartesianVelocityController.cpp
@@ -57,6 +57,11 @@ void CartesianVelocityController::calculateJacobis(VirtualRobot::IKSolver::Carte
_jacobiWithCosts(r, c) = jacobi(r, c) / _jointCosts(c);
}
}
+ auto svd = Eigen::JacobiSVD(_jacobiWithCosts, Eigen::ComputeThinU | Eigen::ComputeThinV);
+ auto sv = svd.singularValues();
+ double minSigVal = sv(sv.rows() - 1, sv.cols() - 1);
+ double damping = minSigVal < 1e-2 ? 1e-2 : 1e-8;
+ ik->setDampedSvdLambda(damping);
_inv = ik->computePseudoInverseJacobianMatrix(_jacobiWithCosts, ik->getJacobiRegularization(mode));
}
@@ -267,6 +272,11 @@ bool CartesianVelocityController::clampJacobiAtJointLimits(VirtualRobot::IKSolve
}
if (modifiedJacobi)
{
+ auto svd = Eigen::JacobiSVD(jacobi, Eigen::ComputeThinU | Eigen::ComputeThinV);
+ auto sv = svd.singularValues();
+ double minSigVal = sv(sv.rows() - 1, sv.cols() - 1);
+ double damping = minSigVal < 1e-2 ? 1e-2 : 1e-8;
+ ik->setDampedSvdLambda(damping);
inv = ik->computePseudoInverseJacobianMatrix(jacobi, ik->getJacobiRegularization(mode));
}
diff --git a/source/RobotAPI/libraries/core/PIDController.cpp b/source/RobotAPI/libraries/core/PIDController.cpp
index 0b2dd67089394ee5c219ec66d919eb4d07a4f927..d3b4eb51940bfdf8dbefb2daf7809c1e9d27c7a4 100644
--- a/source/RobotAPI/libraries/core/PIDController.cpp
+++ b/source/RobotAPI/libraries/core/PIDController.cpp
@@ -26,6 +26,7 @@
#include <ArmarXCore/core/time/TimeUtil.h>
#include <RobotAPI/libraries/core/math/MathUtils.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/observers/filters/rtfilters/RTFilterBase.h>
#include <ArmarXCore/observers/filters/rtfilters/ButterworthFilter.h>
#include <memory>
diff --git a/source/RobotAPI/libraries/core/PIDController.h b/source/RobotAPI/libraries/core/PIDController.h
index 4e17ef0d016a6c12400b4192e993f4e9b10993ae..9e183e77050ab09abf3a5e1b1daa309b087eccf5 100644
--- a/source/RobotAPI/libraries/core/PIDController.h
+++ b/source/RobotAPI/libraries/core/PIDController.h
@@ -27,7 +27,6 @@
#include "MultiDimPIDController.h"
#include <ArmarXCore/core/logging/Logging.h>
-#include <ArmarXCore/observers/filters/rtfilters/RTFilterBase.h>
#include <Eigen/Core>
@@ -36,6 +35,11 @@
namespace armarx
{
+ namespace rtfilters
+ {
+ class RTFilterBase;
+ }
+
class PIDController :
public Logging
{
@@ -77,8 +81,8 @@ namespace armarx
bool firstRun;
bool limitless;
bool threadSafe = true;
- rtfilters::RTFilterBasePtr differentialFilter;
- rtfilters::RTFilterBasePtr pdOutputFilter;
+ std::shared_ptr<rtfilters::RTFilterBase> differentialFilter;
+ std::shared_ptr<rtfilters::RTFilterBase> pdOutputFilter;
private:
using ScopedRecursiveLock = std::unique_lock<std::recursive_mutex>;
using ScopedRecursiveLockPtr = std::unique_ptr<ScopedRecursiveLock>;
diff --git a/source/RobotAPI/libraries/core/math/MathUtils.h b/source/RobotAPI/libraries/core/math/MathUtils.h
index 7a7fb5c8dd21363e246ad3a45bfbcdb304640f8b..4944430b100596e3336d6feeb2b45870b5258d8c 100644
--- a/source/RobotAPI/libraries/core/math/MathUtils.h
+++ b/source/RobotAPI/libraries/core/math/MathUtils.h
@@ -22,9 +22,9 @@
#pragma once
-#include <math.h>
-#include <Eigen/Eigen>
+#include <Eigen/Core>
#include <vector>
+#include <math.h>
namespace armarx::math
{
diff --git a/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.cpp b/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.cpp
index 91f15876447e836e6a7e5f883747ae613765a0e5..65db64e872112548b72048542cfdecb7835cd81f 100644
--- a/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.cpp
+++ b/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.cpp
@@ -90,7 +90,7 @@ namespace armarx
return _root;
}
- bool RemoteRobot::hasRobotNode(const std::string& robotNodeName)
+ bool RemoteRobot::hasRobotNode(const std::string& robotNodeName) const
{
if (_cachedNodes.find(name) == _cachedNodes.end())
{
@@ -103,7 +103,7 @@ namespace armarx
}
- bool RemoteRobot::hasRobotNode(RobotNodePtr robotNode)
+ bool RemoteRobot::hasRobotNode(RobotNodePtr robotNode) const
{
return this->hasRobotNode(robotNode->getName());
@@ -159,7 +159,7 @@ namespace armarx
}
}
- bool RemoteRobot::hasRobotNodeSet(const std::string& name)
+ bool RemoteRobot::hasRobotNodeSet(const std::string& name) const
{
return _robot->hasRobotNodeSet(name);
}
@@ -539,17 +539,17 @@ namespace armarx
// Private (unused methods)
- bool RemoteRobot::hasEndEffector(const std::string& endEffectorName)
+ bool RemoteRobot::hasEndEffector(const std::string& endEffectorName) const
{
return false;
}
- EndEffectorPtr RemoteRobot::getEndEffector(const std::string& endEffectorName)
+ EndEffectorPtr RemoteRobot::getEndEffector(const std::string& endEffectorName) const
{
return EndEffectorPtr();
}
- void RemoteRobot::getEndEffectors(std::vector<EndEffectorPtr>& storeEEF) {}
+ void RemoteRobot::getEndEffectors(std::vector<EndEffectorPtr>& storeEEF) const {}
void RemoteRobot::setRootNode(RobotNodePtr node) {}
void RemoteRobot::registerRobotNode(RobotNodePtr node) {}
void RemoteRobot::deregisterRobotNode(RobotNodePtr node) {}
diff --git a/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.h b/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.h
index b687f0e0fc8cc1eac7b967b10408ec8b63440f56..734db7407354682df6fa45c866acdb0e9c8c0a6f 100644
--- a/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.h
+++ b/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.h
@@ -145,13 +145,13 @@ namespace armarx
VirtualRobot::RobotNodePtr getRootNode() const override;
- bool hasRobotNode(const std::string& robotNodeName) override;
- bool hasRobotNode(VirtualRobot::RobotNodePtr) override;
+ bool hasRobotNode(const std::string& robotNodeName) const override;
+ bool hasRobotNode(VirtualRobot::RobotNodePtr) const override;
VirtualRobot::RobotNodePtr getRobotNode(const std::string& robotNodeName) const override;
void getRobotNodes(std::vector< VirtualRobot::RobotNodePtr >& storeNodes, bool clearVector = true) const override;
- bool hasRobotNodeSet(const std::string& name) override;
+ bool hasRobotNodeSet(const std::string& name) const override;
VirtualRobot::RobotNodeSetPtr getRobotNodeSet(const std::string& nodeSetName) const override;
void getRobotNodeSets(std::vector<VirtualRobot::RobotNodeSetPtr>& storeNodeSet) const override;
@@ -223,11 +223,11 @@ namespace armarx
protected:
/// Not implemented yet
- bool hasEndEffector(const std::string& endEffectorName) override;
+ bool hasEndEffector(const std::string& endEffectorName) const override;
/// Not implemented yet
- VirtualRobot::EndEffectorPtr getEndEffector(const std::string& endEffectorName) override;
+ VirtualRobot::EndEffectorPtr getEndEffector(const std::string& endEffectorName) const override;
/// Not implemented yet
- void getEndEffectors(std::vector<VirtualRobot::EndEffectorPtr>& storeEEF) override;
+ void getEndEffectors(std::vector<VirtualRobot::EndEffectorPtr>& storeEEF) const override;
/// Not implemented yet
void setRootNode(VirtualRobot::RobotNodePtr node) override;
diff --git a/source/RobotAPI/libraries/core/test/CartesianVelocityControllerTest.cpp b/source/RobotAPI/libraries/core/test/CartesianVelocityControllerTest.cpp
index ce26e1c141573e7033a5dd3272f63d5dad44578a..683ad38f8e45e9d49ffdf0b425bb3569eef4d499 100644
--- a/source/RobotAPI/libraries/core/test/CartesianVelocityControllerTest.cpp
+++ b/source/RobotAPI/libraries/core/test/CartesianVelocityControllerTest.cpp
@@ -130,8 +130,8 @@ BOOST_AUTO_TEST_CASE(testJointLimitAwareness)
rns->setJointValues(oldJV + jointVel2 * accuracy);
Eigen::VectorXf resultCartesianVel2 = ((h->_tcp->getPositionInRootFrame() - posBefore) / accuracy);
- Eigen::Vector3f diff = (resultCartesianVel - cartesianVel);
- Eigen::Vector3f diff2 = (resultCartesianVel2 - cartesianVel);
+ Eigen::Vector3f diff = (resultCartesianVel - cartesianVel.head<3>());
+ Eigen::Vector3f diff2 = (resultCartesianVel2 - cartesianVel.head<3>());
if (!((diff - diff2).norm() < 0.5 || diff2.norm() < diff.norm()))
{