diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml
new file mode 100644
index 0000000000000000000000000000000000000000..73976608226406fb3172844aba6097810a89d584
--- /dev/null
+++ b/.gitlab-ci.yml
@@ -0,0 +1,103 @@
+
+stages:
+ - build-and-test
+
+
+.build-and-test:
+
+ cache:
+ # https://docs.gitlab.com/ee/ci/caching/#share-caches-across-jobs-in-different-branches
+ key: one-key-to-rule-them-all
+ paths:
+ - .apt
+ - .ccache
+
+ before_script:
+ # Apt cache configuration.
+ - rm -rf /var/cache/apt/archives || true
+ - rm -f /etc/apt/apt.conf.d/docker-clean # Remove docker-clean script to avoid cache deletion.
+ - mkdir .apt || true
+ - ln -s "$CI_PROJECT_DIR/.apt" /var/cache/apt/archives
+
+ # Update apt info.
+ - apt-get update
+
+ # Ccache configuration and introspection.
+ - apt-get install ccache --yes
+ - ccache --set-config=cache_dir="$CI_PROJECT_DIR/.ccache"
+ - ccache --max-size=20G
+ - ccache --show-stats
+
+ # Ice configuration to run in Docker.
+ - export ICE_DEFAULT_HOST="127.0.0.1"
+ - export ICE_RUN_AS_ROOT="true"
+
+ # Activate Axii.
+ - source /axii/scripts/install_axii.sh
+
+ script:
+ # Create workspace.
+ - axii workspace create ~/workspace workspace
+ - axii workspace activate workspace
+ - _axii_auto_env_refresh
+
+ # Use workspace configuration from project.
+ - cp "$CI_PROJECT_DIR/.gitlab/ci/armarx-workspace.json" "$ARMARX_WORKSPACE/armarx-workspace.json"
+ - cat "$ARMARX_WORKSPACE/armarx-workspace.json"
+
+ - axii workspace env
+ - _axii_auto_env_refresh
+
+ - echo "Workspace information:"
+ - axii workspace list-modules
+ - axii workspace list-modules --deps
+ - axii workspace info
+
+ - export PROJECT_MODULE="armarx/RobotAPI"
+ - export PROJECT_PATH_IN_WORKSPACE="$armarx__RobotAPI__PATH"
+
+ # Symlink project directory into Axii workspace.
+ - mkdir -p "$(dirname $PROJECT_PATH_IN_WORKSPACE)"
+ - ln -s "$CI_PROJECT_DIR" "$PROJECT_PATH_IN_WORKSPACE"
+
+ # Fix "CMake Error in CMakeLists.txt: Imported target "VirtualRobot" includes non-existent path "/usr/lib/include"
+ # (caused by at least dmp)
+ - mkdir -p /usr/lib/include
+
+ # Upgrade.
+ - axii workspace system --accept-apt-install
+ - axii workspace update --prefer-https
+ - axii workspace upgrade -m "$PROJECT_MODULE"
+
+ - ccache --show-stats
+
+ # Try starting Ice.
+ - armarx switch docker_test --ice-host 127.0.0.1 --ice-port 10000 --ice-default-host 127.0.0.1 --mongo-host 127.0.0.1 --mongo-port 10001
+ - armarx profile
+ - armarx status || true
+ #- armarx start
+ # armarx status
+ #- armarx stop
+
+ # Test.
+ # ToDo: Add and use `axii ws test -m "$PROJECT_MODULE"`
+ - cd "$PROJECT_PATH_IN_WORKSPACE/build"
+ - ctest --output-on-failure --output-junit "$CI_PROJECT_DIR/report.xml" .
+
+ artifacts:
+ reports:
+ junit: report.xml
+
+
+build-and-test-bionic:
+ stage: build-and-test
+ extends: .build-and-test
+
+ image: git.h2t.iar.kit.edu:5050/sw/armarx/meta/axii:latest-bionic
+
+
+build-and-test-jammy:
+ stage: build-and-test
+ extends: .build-and-test
+
+ image: git.h2t.iar.kit.edu:5050/sw/armarx/meta/axii:latest-jammy
diff --git a/.gitlab/ci/armarx-workspace.json b/.gitlab/ci/armarx-workspace.json
new file mode 100644
index 0000000000000000000000000000000000000000..ae5ce8a95c44efa6def8c0525c11708a114a1429
--- /dev/null
+++ b/.gitlab/ci/armarx-workspace.json
@@ -0,0 +1,17 @@
+{
+ "modules": {
+ "tools/ccache/default": {},
+ "armarx/RobotAPI": {}
+ },
+ "global": {
+ "prepare": {
+ "cmake": {
+ "definitions": {
+ "CMAKE_BUILD_TYPE": "RelWithDebInfo",
+ "CMAKE_C_COMPILER_LAUNCHER": "$CCACHE",
+ "CMAKE_CXX_COMPILER_LAUNCHER": "$CCACHE"
+ }
+ }
+ }
+ }
+}
diff --git a/scenarios/RobotHealthTest/RobotHealthTest.scx b/scenarios/RobotHealthTest/RobotHealthTest.scx
index c0dde0a3b95ecb6a6155a07f818b0343badd6e6d..68dad197bc70f86a5cdef0161121f48d0d312800 100644
--- a/scenarios/RobotHealthTest/RobotHealthTest.scx
+++ b/scenarios/RobotHealthTest/RobotHealthTest.scx
@@ -1,6 +1,6 @@
<?xml version="1.0" encoding="utf-8"?>
<scenario name="RobotHealthTest" creation="2018-11-30.11:42:03" globalConfigName="./config/global.cfg" package="RobotAPI" deploymentType="local" nodeName="NodeMain">
- <application name="RobotHealthApp" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
+ <application name="RobotHealthApp" instance="" package="RobotAPI" nodeName="" enabled="false" iceAutoRestart="false"/>
<application name="RobotHealthDummyApp" instance="HealthDummy1" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
<application name="RobotHealthDummyApp" instance="HealthDummy2" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
</scenario>
diff --git a/scenarios/RobotHealthTest/config/RobotHealthApp.cfg b/scenarios/RobotHealthTest/config/RobotHealthApp.cfg
index b91501c1a405672cec4ce8b06ea9bcf83f758c4b..ebffea693fd85455a4f16d02ab490a8cad798bc3 100644
--- a/scenarios/RobotHealthTest/config/RobotHealthApp.cfg
+++ b/scenarios/RobotHealthTest/config/RobotHealthApp.cfg
@@ -18,7 +18,7 @@
# 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)
+# ArmarX.CachePath: Path for cache files. If relative path AND env. variable ARMARX_CONFIG_DIR is set, the cache path will be made relative to ARMARX_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${ARMARX_WORKSPACE}/armarx_config)
# Attributes:
# - Default: mongo/.cache
# - Case sensitivity: yes
@@ -109,20 +109,12 @@
# ArmarX.RemoteHandlesDeletionTimeout = 3000
-# ArmarX.RobotHealth.AggregatedRobotHealthTopicName: Name of the AggregatedRobotHealthTopic
+# ArmarX.RobotHealth.DebugObserverTopicName: Name of the topic the DebugObserver listens on
# Attributes:
-# - Default: AggregatedRobotHealthTopic
+# - Default: DebugObserver
# - Case sensitivity: yes
# - Required: no
-# ArmarX.RobotHealth.AggregatedRobotHealthTopicName = AggregatedRobotHealthTopic
-
-
-# ArmarX.RobotHealth.EmergencyStopTopicName: The name of the topic over which changes of the emergencyStopState are sent.
-# Attributes:
-# - Default: EmergencyStop
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.RobotHealth.EmergencyStopTopicName = EmergencyStop
+# ArmarX.RobotHealth.DebugObserverTopicName = DebugObserver
# ArmarX.RobotHealth.EnableProfiling: enable profiler which is used for logging performance events
@@ -159,37 +151,36 @@
# ArmarX.RobotHealth.MinimumLoggingLevel = Undefined
-# ArmarX.RobotHealth.ObjectName: Name of IceGrid well-known object
+# ArmarX.RobotHealth.Name of the AggregatedRobotHealthTopic: Name of the `AggregatedRobotHealth` topic to publish data to.
# Attributes:
-# - Default: ""
+# - Default: AggregatedRobotHealthTopic
# - Case sensitivity: yes
# - Required: no
-# ArmarX.RobotHealth.ObjectName = ""
+# ArmarX.RobotHealth.Name of the AggregatedRobotHealthTopic = AggregatedRobotHealthTopic
-# ArmarX.RobotHealth.ReportErrorsWithSpeech:
+# ArmarX.RobotHealth.ObjectName: Name of IceGrid well-known object
# Attributes:
-# - Default: true
+# - Default: ""
# - Case sensitivity: yes
# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.RobotHealth.ReportErrorsWithSpeech = true
+# ArmarX.RobotHealth.ObjectName = ""
-# ArmarX.RobotHealth.RequiredComponents: Comma separated list of required components
+# ArmarX.RobotHealth.RequiredTags: Tags that should be requested.
# Attributes:
# - Default: ""
# - Case sensitivity: yes
# - Required: no
-# ArmarX.RobotHealth.RequiredComponents = ""
+# ArmarX.RobotHealth.RequiredTags = ""
-# ArmarX.RobotHealth.RobotHealthTopicName: Name of the RobotHealth topic
+# ArmarX.RobotHealth.RobotHealthTopic: Name of the RobotHealth topic
# Attributes:
# - Default: RobotHealthTopic
# - Case sensitivity: yes
# - Required: no
-# ArmarX.RobotHealth.RobotHealthTopicName = RobotHealthTopic
+# ArmarX.RobotHealth.RobotHealthTopic = RobotHealthTopic
# ArmarX.RobotHealth.RobotUnitName: No Description
@@ -208,20 +199,12 @@ ArmarX.RobotHealth.RobotUnitName = RobotUnitSimulation
ArmarX.RobotHealth.RobotUnitRequired = 0
-# ArmarX.RobotHealth.SpeechMinimumReportInterval: Time that has to pass between reported messages in seconds.
+# ArmarX.RobotHealth.The name of the topic over which changes of the emergencyStopState are sent.: Name of the `EmergencyStop` topic to publish data to.
# Attributes:
-# - Default: 60
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.RobotHealth.SpeechMinimumReportInterval = 60
-
-
-# ArmarX.RobotHealth.TextToSpeechTopicName: Name of the TextToSpeech topic
-# Attributes:
-# - Default: TextToSpeech
+# - Default: EmergencyStop
# - Case sensitivity: yes
# - Required: no
-# ArmarX.RobotHealth.TextToSpeechTopicName = TextToSpeech
+# ArmarX.RobotHealth.The name of the topic over which changes of the emergencyStopState are sent. = EmergencyStop
# ArmarX.SecondsStartupDelay: The startup will be delayed by this number of seconds (useful for debugging)
diff --git a/scenarios/RobotHealthTest/config/RobotHealthDummyApp.HealthDummy1.cfg b/scenarios/RobotHealthTest/config/RobotHealthDummyApp.HealthDummy1.cfg
index 5df0ff4cfd0fd82e8c0c7b9155be4e797fd14959..0f14e83d287e44accc961037ba069f3ea77dce46 100644
--- a/scenarios/RobotHealthTest/config/RobotHealthDummyApp.HealthDummy1.cfg
+++ b/scenarios/RobotHealthTest/config/RobotHealthDummyApp.HealthDummy1.cfg
@@ -18,7 +18,7 @@
# 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)
+# ArmarX.CachePath: Path for cache files. If relative path AND env. variable ARMARX_CONFIG_DIR is set, the cache path will be made relative to ARMARX_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${ARMARX_WORKSPACE}/armarx_config)
# Attributes:
# - Default: mongo/.cache
# - Case sensitivity: yes
diff --git a/scenarios/RobotHealthTest/config/RobotHealthDummyApp.HealthDummy2.cfg b/scenarios/RobotHealthTest/config/RobotHealthDummyApp.HealthDummy2.cfg
index 60d2ee3f291cbdb09a97eb89500b1fd7426e4a72..e157c2704c9191a48e3daccf14a545f125e72c51 100644
--- a/scenarios/RobotHealthTest/config/RobotHealthDummyApp.HealthDummy2.cfg
+++ b/scenarios/RobotHealthTest/config/RobotHealthDummyApp.HealthDummy2.cfg
@@ -18,7 +18,7 @@
# 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)
+# ArmarX.CachePath: Path for cache files. If relative path AND env. variable ARMARX_CONFIG_DIR is set, the cache path will be made relative to ARMARX_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${ARMARX_WORKSPACE}/armarx_config)
# Attributes:
# - Default: mongo/.cache
# - Case sensitivity: yes
@@ -148,7 +148,7 @@ ArmarX.RobotHealthDummy.ObjectName = HealthDummy2
# - Default: nanosleep
# - Case sensitivity: yes
# - Required: no
-# ArmarX.RobotHealthDummy.SleepMode = nanosleep
+ArmarX.RobotHealthDummy.SleepMode = std::this_thread::sleep_for
# ArmarX.SecondsStartupDelay: The startup will be delayed by this number of seconds (useful for debugging)
diff --git a/source/RobotAPI/components/ArViz/CMakeLists.txt b/source/RobotAPI/components/ArViz/CMakeLists.txt
index 28c1a21d9e549011313e6e0d36368cd5c7edee23..9638058676450ed6fcb7d8ff884cf7ebf8beb67e 100644
--- a/source/RobotAPI/components/ArViz/CMakeLists.txt
+++ b/source/RobotAPI/components/ArViz/CMakeLists.txt
@@ -142,7 +142,7 @@ set(HEADERS
ArVizStorage.h
)
-armarx_add_component_executable("${SOURCES}" "${HEADERS}")
+armarx_add_component_executable("${SOURCES};${HEADERS}" "${HEADERS}")
add_subdirectory(Example)
diff --git a/source/RobotAPI/components/ArViz/Coin/VisualizationRobot.cpp b/source/RobotAPI/components/ArViz/Coin/VisualizationRobot.cpp
index a01c4b37c08698a77fea6588d46963fee6e6c6df..4722e9c5ce8deaabfc33a917c9b0c744d1a781fd 100644
--- a/source/RobotAPI/components/ArViz/Coin/VisualizationRobot.cpp
+++ b/source/RobotAPI/components/ArViz/Coin/VisualizationRobot.cpp
@@ -22,7 +22,7 @@ namespace armarx::viz::coin
if (filename.empty())
{
ARMARX_INFO << deactivateSpam() << "No filename provided for robot.";
- return result;
+ return nullptr;
}
std::string fullFilename;
@@ -41,7 +41,7 @@ namespace armarx::viz::coin
{
ARMARX_INFO << deactivateSpam()
<< "Unable to find readable file for name: " << filename;
- return result;
+ return nullptr;
}
try
@@ -226,15 +226,21 @@ namespace armarx::viz::coin
// robot.setGlobalPose(pose, false);
// Check joint values for changes
- bool jointValuesChanged = false;
- for (auto& pair : element.jointValues)
+
+ for (const auto& pair : element.jointValues)
{
std::string const& nodeName = pair.first;
float newJointValue = pair.second;
VirtualRobot::RobotNodePtr robotNode = robot.getRobotNode(nodeName);
- float oldJointValue = robotNode->getJointValue();
- float diff = std::abs(newJointValue - oldJointValue);
- jointValuesChanged = diff > 0.001f;
+
+ if (robotNode == nullptr)
+ {
+ continue;
+ }
+
+ const float oldJointValue = robotNode->getJointValue();
+ const float diff = std::abs(newJointValue - oldJointValue);
+ const bool jointValuesChanged = diff > 0.001f;
if (jointValuesChanged)
{
// Only set the joint values if they changed
diff --git a/source/RobotAPI/components/RobotHealth/RobotHealthDummy.cpp b/source/RobotAPI/components/RobotHealth/RobotHealthDummy.cpp
index e028bc55648f02446b5980cc00124950d4564219..34894b6ea9c449d13f47af00d2c012a47f764f1d 100644
--- a/source/RobotAPI/components/RobotHealth/RobotHealthDummy.cpp
+++ b/source/RobotAPI/components/RobotHealth/RobotHealthDummy.cpp
@@ -80,7 +80,7 @@ namespace armarx
}
void
- RobotHealthDummy::busydwait(long microseconds)
+ RobotHealthDummy::busywait(long microseconds)
{
long start = TimeUtil::GetTime().toMicroSeconds();
auto end = start + microseconds;
@@ -95,51 +95,60 @@ namespace armarx
{
auto args = RobotHealthHeartbeatArgs();
args.identifier = getName();
+ armarx::core::time::toIce(args.maximumCycleTimeError, armarx::core::time::Duration::MilliSeconds(1000));
+ armarx::core::time::toIce(args.maximumCycleTimeWarning, armarx::core::time::Duration::MilliSeconds(500));
robotHealthTopicPrx->signUp(args);
ARMARX_INFO << "starting rinning task";
while (!dummyTask->isStopped())
{
- long beforeTopicCall = TimeUtil::GetTime().toMicroSeconds();
+ auto beforeTopicCall = armarx::core::time::DateTime::Now();
//ARMARX_INFO << "send heartbeat";
armarx::core::time::dto::DateTime now;
armarx::core::time::toIce(now, armarx::core::time::DateTime::Now());
robotHealthTopicPrx->heartbeat(getName(), now);
- long afterTopicCall = TimeUtil::GetTime().toMicroSeconds();
+ auto afterTopicCall = armarx::core::time::DateTime::Now();
+
if (sleepmode == "nanosleep")
{
- NanoSleep(10 * 1000 * 1000);
+ NanoSleep(10 * 1000 * 1000); // wait 10 milliseconds
}
else if (sleepmode == "sleepwait")
{
- sleepwait(10 * 1000);
+ sleepwait(10 * 1000); // wait 10 milliseconds
}
else if (sleepmode == "yieldwait")
{
- yieldwait(10 * 1000);
+ yieldwait(10 * 1000); // wait 10 milliseconds
}
else if (sleepmode == "busywait")
{
- busydwait(10 * 1000);
+ busywait(10 * 1000); // wait 10 milliseconds
+ }
+ else if (sleepmode == "std::this_thread::sleep_for")
+ {
+ std::this_thread::sleep_for(std::chrono::microseconds(10 * 1000));
}
else
{
throw LocalException("Unknown sleepmode.");
}
- long afterSleep = TimeUtil::GetTime().toMicroSeconds();
- long topicCallDelay = afterTopicCall - beforeTopicCall;
- long sleepDelay = afterSleep - afterTopicCall;
- if (sleepDelay > 20000)
+ auto afterSleep = armarx::core::time::DateTime::Now();
+ auto topicCallDelay = afterTopicCall - beforeTopicCall;
+ auto sleepDelay = afterSleep - afterTopicCall;
+ if (sleepDelay.toMicroSeconds() > 20000)
{
- ARMARX_IMPORTANT << sleepmode << ": " << sleepDelay << "us";
+ ARMARX_IMPORTANT << sleepmode << " took long: " << sleepDelay << "us";
}
- if (topicCallDelay > 1000)
+ if (topicCallDelay.toMicroSeconds() > 1000)
{
- ARMARX_IMPORTANT << "topic: " << topicCallDelay << "us";
+ ARMARX_IMPORTANT << "topic took long: " << topicCallDelay << "us";
}
}
+
+ robotHealthTopicPrx->unregister(args.identifier);
}
void
diff --git a/source/RobotAPI/components/RobotHealth/RobotHealthDummy.h b/source/RobotAPI/components/RobotHealth/RobotHealthDummy.h
index cfd8d369d0335768c2e85dd735f1d31d1e30e60d..a841720145b96f925c427443f3998475620509bb 100644
--- a/source/RobotAPI/components/RobotHealth/RobotHealthDummy.h
+++ b/source/RobotAPI/components/RobotHealth/RobotHealthDummy.h
@@ -78,7 +78,7 @@ namespace armarx
int NanoSleep(long nanosec);
void yieldwait(long microseconds);
- void busydwait(long microseconds);
+ void busywait(long microseconds);
void sleepwait(long microseconds);
protected:
diff --git a/source/RobotAPI/components/armem/client/ExampleMemoryClient/ExampleMemoryClient.cpp b/source/RobotAPI/components/armem/client/ExampleMemoryClient/ExampleMemoryClient.cpp
index e7fdc5eaa92b960a964d78deecce66dfd242227f..7fea318b5249b557e92cb1e207f3f04e0de70321 100644
--- a/source/RobotAPI/components/armem/client/ExampleMemoryClient/ExampleMemoryClient.cpp
+++ b/source/RobotAPI/components/armem/client/ExampleMemoryClient/ExampleMemoryClient.cpp
@@ -205,6 +205,8 @@ namespace armarx
return armem::MemoryID(result.segmentID);
}
+ // COMMIT
+
armem::MemoryID
ExampleMemoryClient::commitSingleSnapshot(const armem::MemoryID& entityID)
{
@@ -272,10 +274,12 @@ namespace armarx
}
}
+ // QUERY
+
void
ExampleMemoryClient::queryLatestSnapshot(const armem::MemoryID& entityID)
{
- ARMARX_IMPORTANT << "Querying latest snapshot: "
+ ARMARX_IMPORTANT << "Querying latest snapshot in entity: "
<< "\n- entityID: \t'" << entityID << "'";
armem::client::query::Builder builder;
@@ -295,6 +299,8 @@ namespace armarx
ARMARX_IMPORTANT << "Getting entity via ID";
armem::wm::Memory& memory = qResult.memory;
+ ARMARX_CHECK(memory.hasInstances());
+
ARMARX_CHECK_GREATER_EQUAL(memory.size(), 1);
const armem::wm::Entity* entity = memory.findEntity(entityID);
@@ -335,13 +341,23 @@ namespace armarx
if (qResult.success)
{
armem::wm::Memory memory = std::move(qResult.memory);
- const armem::wm::EntitySnapshot& entitySnapshot =
- memory.getEntity(snapshotID).getLatestSnapshot();
+ {
+ const armem::wm::EntitySnapshot& entitySnapshot = memory.getLatestSnapshot();
- ARMARX_INFO << "Result snapshot: "
- << "\n- time: \t" << entitySnapshot.time() << "\n- # instances: \t"
- << entitySnapshot.size();
+ ARMARX_INFO << "Result snapshot: "
+ << "\n- time: \t" << entitySnapshot.time()
+ << "\n- # instances: \t" << entitySnapshot.size();
+ }
+ {
+ const armem::wm::EntitySnapshot& entitySnapshot =
+ memory.getEntity(snapshotID).getLatestSnapshot();
+
+ ARMARX_INFO << "Result snapshot: "
+ << "\n- time: \t" << entitySnapshot.time()
+ << "\n- # instances: \t" << entitySnapshot.size();
+ }
}
+
else
{
ARMARX_ERROR << qResult.errorMessage;
diff --git a/source/RobotAPI/components/units/RobotUnit/Devices/RTThreadTimingsSensorDevice.h b/source/RobotAPI/components/units/RobotUnit/Devices/RTThreadTimingsSensorDevice.h
index d1df912e12771ae8c09befd2c6dd4832897fadad..5af57f010d19837ecb8af20260191c1faee3ff54 100644
--- a/source/RobotAPI/components/units/RobotUnit/Devices/RTThreadTimingsSensorDevice.h
+++ b/source/RobotAPI/components/units/RobotUnit/Devices/RTThreadTimingsSensorDevice.h
@@ -23,6 +23,7 @@
#pragma once
#include "../SensorValues/SensorValueRTThreadTimings.h"
+#include "../util/RtTiming.h"
#include "SensorDevice.h"
namespace armarx::RobotUnitModule
@@ -100,7 +101,7 @@ namespace armarx
void
rtMarkRtLoopStart() override
{
- const IceUtil::Time now = TimeUtil::GetTime(true);
+ const auto now = armarx::rtNow();
value.rtLoopRoundTripTime = now - rtLoopStart;
rtLoopStart = now;
}
@@ -108,13 +109,13 @@ namespace armarx
void
rtMarkRtLoopPreSleep() override
{
- value.rtLoopDuration = TimeUtil::GetTime(true) - rtLoopStart;
+ value.rtLoopDuration = armarx::rtNow() - rtLoopStart;
}
void
rtMarkRtBusSendReceiveStart() override
{
- const IceUtil::Time now = TimeUtil::GetTime(true);
+ const IceUtil::Time now = armarx::rtNow();
value.rtBusSendReceiveRoundTripTime = now - rtBusSendReceiveStart;
rtBusSendReceiveStart = now;
}
@@ -122,30 +123,34 @@ namespace armarx
void
rtMarkRtBusSendReceiveEnd() override
{
- value.rtBusSendReceiveDuration = TimeUtil::GetTime(true) - rtBusSendReceiveStart;
+ value.rtBusSendReceiveDuration = armarx::rtNow() - rtBusSendReceiveStart;
}
protected:
#define make_markRT_X_Start_End(name) \
virtual void rtMarkRt##name##Start() override \
{ \
- const IceUtil::Time now = TimeUtil::GetTime(true); \
+ const IceUtil::Time now = armarx::rtNow(); \
value.rt##name##RoundTripTime = now - rt##name##Start; \
rt##name##Start = now; \
} \
virtual void rtMarkRt##name##End() override \
{ \
- value.rt##name##Duration = TimeUtil::GetTime(true) - rt##name##Start; \
- }
-
- make_markRT_X_Start_End(SwitchControllerSetup) make_markRT_X_Start_End(RunNJointControllers)
- make_markRT_X_Start_End(HandleInvalidTargets)
- make_markRT_X_Start_End(RunJointControllers)
- make_markRT_X_Start_End(ReadSensorDeviceValues)
- make_markRT_X_Start_End(UpdateSensorAndControlBuffer)
- make_markRT_X_Start_End(ResetAllTargets)
+ value.rt##name##Duration = armarx::rtNow() - rt##name##Start; \
+ } \
+ static_assert(true, "force trailing semi-colon")
+
+ make_markRT_X_Start_End(SwitchControllerSetup);
+ make_markRT_X_Start_End(RunNJointControllers);
+ make_markRT_X_Start_End(HandleInvalidTargets);
+ make_markRT_X_Start_End(RunJointControllers);
+ make_markRT_X_Start_End(ReadSensorDeviceValues);
+ make_markRT_X_Start_End(UpdateSensorAndControlBuffer);
+ make_markRT_X_Start_End(ResetAllTargets);
#undef make_markRT_X_Start_End
- protected : IceUtil::Time rtSwitchControllerSetupStart;
+
+ protected:
+ IceUtil::Time rtSwitchControllerSetupStart;
IceUtil::Time rtRunNJointControllersStart;
IceUtil::Time rtHandleInvalidTargetsStart;
IceUtil::Time rtRunJointControllersStart;
diff --git a/source/RobotAPI/components/units/RobotUnit/util/RtTiming.h b/source/RobotAPI/components/units/RobotUnit/util/RtTiming.h
index 4e88ef1f3a8fae9e6baeeb453aa4711a40d77f30..243f1a119df65ab1f5a9f79a5f53d243fe8b102c 100644
--- a/source/RobotAPI/components/units/RobotUnit/util/RtTiming.h
+++ b/source/RobotAPI/components/units/RobotUnit/util/RtTiming.h
@@ -29,13 +29,13 @@
namespace armarx
{
+
inline IceUtil::Time
rtNow()
{
- return IceUtil::Time::now(IceUtil::Time::Monotonic);
- // using namespace std::chrono;
- // auto epoch = time_point_cast<microseconds>(high_resolution_clock::now()).time_since_epoch();
- // return IceUtil::Time::microSeconds(duration_cast<milliseconds>(epoch).count());
+ struct timespec ts;
+ clock_gettime(CLOCK_MONOTONIC, &ts);
+ return IceUtil::Time::microSeconds(ts.tv_sec * 1e6 + ts.tv_nsec / 1000);
}
} // namespace armarx
@@ -43,9 +43,8 @@ namespace armarx
//! \ingroup VirtualTime
//! Prints duration with comment in front of it, yet only once per second.
#define RT_TIMING_END_COMMENT(name, comment) \
- ARMARX_RT_LOGF_INFO("%s - duration: %.3f ms", \
- comment, \
- IceUtil::Time(armarx::rtNow() - name).toMilliSecondsDouble()) \
+ ARMARX_RT_LOGF_INFO( \
+ "%s - duration: %.3f ms", comment, (armarx::rtNow() - name).toMilliSecondsDouble()) \
.deactivateSpam(1);
//! \ingroup VirtualTime
//! Prints duration
@@ -53,7 +52,7 @@ namespace armarx
//! \ingroup VirtualTime
//! Prints duration with comment in front of it if it took longer than threshold
#define RT_TIMING_CEND_COMMENT(name, comment, thresholdMs) \
- if ((armarx::rtNow() - name).toMilliSecondsDouble() >= thresholdMs) \
+ if ((armarx::rtNow() - name).toMilliSeconds() >= thresholdMs) \
RT_TIMING_END_COMMENT(name, comment)
//! \ingroup VirtualTime
//! Prints duration if it took longer than thresholdMs
diff --git a/source/RobotAPI/gui-plugins/GuiHealthClient/GuiHealthClientWidgetController.cpp b/source/RobotAPI/gui-plugins/GuiHealthClient/GuiHealthClientWidgetController.cpp
index d976d77e6742b63152b172b7a55107929405302d..4868ebda8519f9d571c0a62a17ccbecd66e5a18d 100644
--- a/source/RobotAPI/gui-plugins/GuiHealthClient/GuiHealthClientWidgetController.cpp
+++ b/source/RobotAPI/gui-plugins/GuiHealthClient/GuiHealthClientWidgetController.cpp
@@ -28,6 +28,7 @@
#include <qrgb.h>
#include <qtablewidget.h>
#include <string>
+#include <iomanip>
#include <QLabel>
#include <QPushButton>
@@ -110,6 +111,24 @@ namespace armarx
robotHealthTopicPrx->heartbeat(getName(), now);
}
+
+ std::string to_string_rounded(float value, int decimals = 100)
+ {
+ std::stringstream ss;
+ ss << std::fixed << std::setprecision(3) << value;
+ return ss.str();
+ }
+
+
+ QTableWidgetItem* make_item(const std::string& text,
+ Qt::AlignmentFlag horAlignment = Qt::AlignLeft)
+ {
+ auto* item = new QTableWidgetItem(QString::fromStdString(text));
+ item->setTextAlignment(horAlignment | Qt::AlignVCenter);
+ return item;
+ }
+
+
void
GuiHealthClientWidgetController::updateSummaryTimerClb()
{
@@ -147,64 +166,63 @@ namespace armarx
const auto& entry = summaryVals.at(i);
std::string stateRepr;
- QColor color;
+ QColor stateColor;
switch(entry.state)
{
case HealthOK:
stateRepr = "yes";
- color.setRgb(0, 255, 0); // green
+ stateColor.setRgb(0, 255, 0); // green
break;
case HealthWarning:
stateRepr = "yes";
- color.setRgb(255, 165, 0); // orange
+ stateColor.setRgb(255, 165, 0); // orange
break;
case HealthError:
stateRepr = "no";
- color.setRgb(255, 0, 0); // red
+ stateColor.setRgb(255, 0, 0); // red
break;
}
const std::string hostname = entry.lastReferenceTimestamp.hostname;
- const std::string timeSinceLastArrivalRepr = std::to_string(entry.timeSinceLastArrival.microSeconds / 1000);
- const std::string timeToLastReferenceRepr = std::to_string(entry.timeSinceLastUpdateReference.microSeconds / 1000);
+ const std::string timeSinceLastArrivalRepr = to_string_rounded(entry.timeSinceLastArrival.microSeconds / 1000.0);
+ const std::string timeToLastReferenceRepr = to_string_rounded(entry.timeSinceLastUpdateReference.microSeconds / 1000.0);
const std::string tagsRepr = serializeList(entry.tags);
- const long syncErrorMilliSeconds = std::abs(entry.timeSinceLastArrival.microSeconds - entry.timeSinceLastUpdateReference.microSeconds) / 1000;
-
+ const float syncErrorMilliSeconds = std::abs(entry.timeSinceLastArrival.microSeconds - entry.timeSinceLastUpdateReference.microSeconds) / 1000.0;
tableWidget->setItem(i, 0, new QTableWidgetItem(QString::fromStdString(entry.identifier)));
-
- auto* requiredItem = new QTableWidgetItem(QString::fromStdString(entry.required ? "yes" : "no"));
- requiredItem->setTextAlignment(Qt::AlignHCenter | Qt::AlignVCenter);
- tableWidget->setItem(i, 1, requiredItem);
-
- auto* stateItem = new QTableWidgetItem(QString::fromStdString(stateRepr));
- stateItem->setTextAlignment(Qt::AlignHCenter | Qt::AlignVCenter);
- tableWidget->setItem(i, 2, stateItem);
- tableWidget->item(i, 2)->setBackgroundColor(color);
+ tableWidget->setItem(i, 1, make_item(entry.required ? "yes" : "no", Qt::AlignHCenter));
+
+ {
+ auto* item = make_item(stateRepr, Qt::AlignHCenter);
+ item->setBackgroundColor(stateColor);
+ tableWidget->setItem(i, 2, item);
+ }
tableWidget->setItem(i, 3, new QTableWidgetItem(QString::fromStdString(tagsRepr)));
-
- tableWidget->setItem(i, 4, new QTableWidgetItem(QString::fromStdString(timeSinceLastArrivalRepr)));
- tableWidget->setItem(i, 5, new QTableWidgetItem(QString::fromStdString(timeToLastReferenceRepr)));
-
- tableWidget->setItem(i, 6, new QTableWidgetItem(QString::fromStdString(std::to_string(syncErrorMilliSeconds))));
+ tableWidget->setItem(i, 4, make_item(timeSinceLastArrivalRepr, Qt::AlignRight));
+ tableWidget->setItem(i, 5, make_item(timeToLastReferenceRepr, Qt::AlignRight));
- if(syncErrorMilliSeconds > 20)
{
+ auto* item = make_item(to_string_rounded(syncErrorMilliSeconds), Qt::AlignRight);
+
QColor timeSyncColor;
- timeSyncColor.setRgb(255, 0, 0);
- tableWidget->item(i, 6)->setBackgroundColor(timeSyncColor);
- }else {
- QColor timeSyncColor;
- timeSyncColor.setRgb(0, 255, 0);
- tableWidget->item(i, 6)->setBackgroundColor(timeSyncColor);
+ if (syncErrorMilliSeconds > 20.)
+ {
+ timeSyncColor.setRgb(255, 0, 0);
+ }
+ else
+ {
+ timeSyncColor.setRgb(0, 255, 0);
+ }
+ item->setBackgroundColor(timeSyncColor);
+
+ tableWidget->setItem(i, 6, item);
}
-
- tableWidget->setItem(i, 7, new QTableWidgetItem(QString::fromStdString(std::to_string(entry.maximumCycleTimeWarning.microSeconds / 1000))));
- tableWidget->setItem(i, 8, new QTableWidgetItem(QString::fromStdString(std::to_string(entry.maximumCycleTimeError.microSeconds / 1000))));
+ tableWidget->setItem(i, 7, make_item(to_string_rounded(entry.maximumCycleTimeWarning.microSeconds / 1000.), Qt::AlignRight));
+ tableWidget->setItem(i, 8, make_item(to_string_rounded(entry.maximumCycleTimeError.microSeconds / 1000.), Qt::AlignRight));
tableWidget->setItem(i, 9, new QTableWidgetItem(QString::fromStdString(hostname)));
}
diff --git a/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp b/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp
index 5437d499db20e1c4a29a59efe98f2c1b92a341fa..fa8011d66e9fb45de290395eab222e9299888620 100644
--- a/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp
+++ b/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp
@@ -22,59 +22,59 @@
* GNU General Public License
*/
#include "KinematicUnitGuiPlugin.h"
-#include "KinematicUnitConfigDialog.h"
-#include <RobotAPI/gui-plugins/KinematicUnitPlugin/ui_KinematicUnitConfigDialog.h>
-#include <RobotAPI/interface/core/NameValueMap.h>
-#include <RobotAPI/interface/units/KinematicUnitInterface.h>
+#include <SimoxUtility/algorithm/string.h>
+#include <SimoxUtility/json.h>
+#include <VirtualRobot/XML/RobotIO.h>
#include "ArmarXCore/core/exceptions/local/ExpressionException.h"
#include "ArmarXCore/core/logging/Logging.h"
#include "ArmarXCore/core/services/tasks/RunningTask.h"
#include "ArmarXCore/core/time/Metronome.h"
#include "ArmarXCore/core/time/forward_declarations.h"
-#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
+#include <ArmarXCore/core/ArmarXManager.h>
+#include <ArmarXCore/core/ArmarXObjectScheduler.h>
#include <ArmarXCore/core/application/Application.h>
#include <ArmarXCore/core/system/ArmarXDataPath.h>
-#include <ArmarXCore/core/ArmarXObjectScheduler.h>
-#include <ArmarXCore/core/ArmarXManager.h>
-
-#include <ArmarXCore/util/json/JSONObject.h>
+#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
#include <ArmarXCore/core/util/algorithm.h>
+#include <ArmarXCore/util/json/JSONObject.h>
-#include <VirtualRobot/XML/RobotIO.h>
-#include <SimoxUtility/json.h>
-#include <SimoxUtility/algorithm/string.h>
+#include <RobotAPI/gui-plugins/KinematicUnitPlugin/ui_KinematicUnitConfigDialog.h>
+#include <RobotAPI/interface/core/NameValueMap.h>
+#include <RobotAPI/interface/units/KinematicUnitInterface.h>
+
+#include "KinematicUnitConfigDialog.h"
// Qt headers
-#include <Qt>
-#include <QtGlobal>
-#include <QSpinBox>
-#include <QSlider>
+#include <QCheckBox>
+#include <QClipboard>
+#include <QInputDialog>
#include <QPushButton>
+#include <QSlider>
+#include <QSpinBox>
#include <QStringList>
#include <QTableView>
-#include <QCheckBox>
#include <QTableWidget>
-#include <QClipboard>
-#include <QInputDialog>
+#include <Qt>
+#include <QtGlobal>
#include <qtimer.h>
-#include <Inventor/SoDB.h>
-#include <Inventor/Qt/SoQt.h>
#include <ArmarXCore/observers/filters/MedianFilter.h>
+#include <Inventor/Qt/SoQt.h>
+#include <Inventor/SoDB.h>
+
// System
+#include <cmath>
#include <cstddef>
#include <cstdio>
-#include <memory>
-#include <string>
-#include <optional>
#include <cstdlib>
-#include <iostream>
-#include <cmath>
-
#include <filesystem>
+#include <iostream>
+#include <memory>
+#include <optional>
+#include <string>
//#define KINEMATIC_UNIT_FILE_DEFAULT std::string("RobotAPI/robots/Armar3/ArmarIII.xml")
@@ -84,268 +84,277 @@
#define SLIDER_POS_DEG_MULTIPLIER 5
#define SLIDER_POS_RAD_MULTIPLIER 100
-
namespace armarx
{
-KinematicUnitGuiPlugin::KinematicUnitGuiPlugin()
-{
-
- qRegisterMetaType<DebugInfo>("DebugInfo");
+ KinematicUnitGuiPlugin::KinematicUnitGuiPlugin()
+ {
- addWidget<KinematicUnitWidgetController>();
-}
+ qRegisterMetaType<DebugInfo>("DebugInfo");
-KinematicUnitWidgetController::KinematicUnitWidgetController() :
- kinematicUnitNode(nullptr),
- enableValueValidator(true),
- historyTime(100000), // 1/10 s
- currentValueMax(5.0f)
-{
- rootVisu = NULL;
- debugLayerVisu = NULL;
+ addWidget<KinematicUnitWidgetController>();
+ }
- // init gui
- ui.setupUi(getWidget());
- getWidget()->setEnabled(false);
+ KinematicUnitWidgetController::KinematicUnitWidgetController() :
+ kinematicUnitNode(nullptr),
+ enableValueValidator(true),
+ historyTime(100000), // 1/10 s
+ currentValueMax(5.0f)
+ {
+ rootVisu = NULL;
+ debugLayerVisu = NULL;
- ui.tableJointList->setItemDelegateForColumn(eTabelColumnAngleProgressbar, &delegate);
+ // init gui
+ ui.setupUi(getWidget());
+ getWidget()->setEnabled(false);
- ui.radioButtonUnknown->setHidden(true);
-}
+ ui.tableJointList->setItemDelegateForColumn(eTabelColumnAngleProgressbar, &delegate);
-void KinematicUnitWidgetController::onInitComponent()
-{
- ARMARX_INFO << flush;
- verbose = true;
+ ui.radioButtonUnknown->setHidden(true);
+ }
+ void
+ KinematicUnitWidgetController::onInitComponent()
+ {
+ ARMARX_INFO << flush;
+ verbose = true;
- rootVisu = new SoSeparator;
- rootVisu->ref();
- robotVisu = new SoSeparator;
- robotVisu->ref();
- rootVisu->addChild(robotVisu);
- // create the debugdrawer component
- std::string debugDrawerComponentName = "KinemticUnitGUIDebugDrawer_" + getName();
- ARMARX_INFO << "Creating component " << debugDrawerComponentName;
- debugDrawer = Component::create<DebugDrawerComponent>(getIceProperties(), debugDrawerComponentName);
- showVisuLayers(false);
+ rootVisu = new SoSeparator;
+ rootVisu->ref();
+ robotVisu = new SoSeparator;
+ robotVisu->ref();
+ rootVisu->addChild(robotVisu);
- if (mutex3D)
- {
- //ARMARX_IMPORTANT << "mutex3d:" << mutex3D.get();
- debugDrawer->setMutex(mutex3D);
- }
- else
- {
- ARMARX_ERROR << " No 3d mutex available...";
- }
+ // create the debugdrawer component
+ std::string debugDrawerComponentName = "KinemticUnitGUIDebugDrawer_" + getName();
+ ARMARX_INFO << "Creating component " << debugDrawerComponentName;
+ debugDrawer =
+ Component::create<DebugDrawerComponent>(getIceProperties(), debugDrawerComponentName);
+ showVisuLayers(false);
- ArmarXManagerPtr m = getArmarXManager();
- m->addObject(debugDrawer, false);
+ if (mutex3D)
+ {
+ //ARMARX_IMPORTANT << "mutex3d:" << mutex3D.get();
+ debugDrawer->setMutex(mutex3D);
+ }
+ else
+ {
+ ARMARX_ERROR << " No 3d mutex available...";
+ }
+ ArmarXManagerPtr m = getArmarXManager();
+ m->addObject(debugDrawer, false);
- {
- std::unique_lock lock(*mutex3D);
- debugLayerVisu = new SoSeparator();
- debugLayerVisu->ref();
- debugLayerVisu->addChild(debugDrawer->getVisualization());
- rootVisu->addChild(debugLayerVisu);
- }
- connectSlots();
+ {
+ std::unique_lock lock(*mutex3D);
+ debugLayerVisu = new SoSeparator();
+ debugLayerVisu->ref();
+ debugLayerVisu->addChild(debugDrawer->getVisualization());
+ rootVisu->addChild(debugLayerVisu);
+ }
- usingProxy(kinematicUnitName);
-}
+ connectSlots();
-void KinematicUnitWidgetController::onConnectComponent()
-{
- // ARMARX_INFO << "Kinematic Unit Gui :: onConnectComponent()";
- jointCurrentHistory.clear();
- jointCurrentHistory.set_capacity(5);
+ usingProxy(kinematicUnitName);
+ }
- // jointAnglesUpdateFrequency = new filters::MedianFilter(100);
- kinematicUnitInterfacePrx = getProxy<KinematicUnitInterfacePrx>(kinematicUnitName);
+ void
+ KinematicUnitWidgetController::onConnectComponent()
+ {
+ // ARMARX_INFO << "Kinematic Unit Gui :: onConnectComponent()";
+ jointCurrentHistory.clear();
+ jointCurrentHistory.set_capacity(5);
- lastJointAngleUpdateTimestamp = Clock::Now();
- robotVisu->removeAllChildren();
+ // jointAnglesUpdateFrequency = new filters::MedianFilter(100);
+ kinematicUnitInterfacePrx = getProxy<KinematicUnitInterfacePrx>(kinematicUnitName);
- robot.reset();
+ lastJointAngleUpdateTimestamp = Clock::Now();
+ robotVisu->removeAllChildren();
- std::string rfile;
- Ice::StringSeq includePaths;
+ robot.reset();
- // Get robot filename
- try
- {
- Ice::StringSeq packages = kinematicUnitInterfacePrx->getArmarXPackages();
- packages.push_back(Application::GetProjectName());
- ARMARX_VERBOSE << "ArmarX packages " << packages;
+ std::string rfile;
+ Ice::StringSeq includePaths;
- for (const std::string& projectName : packages)
+ // Get robot filename
+ try
{
- if (projectName.empty())
+ Ice::StringSeq packages = kinematicUnitInterfacePrx->getArmarXPackages();
+ packages.push_back(Application::GetProjectName());
+ ARMARX_VERBOSE << "ArmarX packages " << packages;
+
+ for (const std::string& projectName : packages)
{
- continue;
+ if (projectName.empty())
+ {
+ continue;
+ }
+
+ CMakePackageFinder project(projectName);
+ auto pathsString = project.getDataDir();
+ ARMARX_VERBOSE << "Data paths of ArmarX package " << projectName << ": "
+ << pathsString;
+ Ice::StringSeq projectIncludePaths = Split(pathsString, ";,", true, true);
+ ARMARX_VERBOSE << "Result: Data paths of ArmarX package " << projectName << ": "
+ << projectIncludePaths;
+ includePaths.insert(
+ includePaths.end(), projectIncludePaths.begin(), projectIncludePaths.end());
}
- CMakePackageFinder project(projectName);
- auto pathsString = project.getDataDir();
- ARMARX_VERBOSE << "Data paths of ArmarX package " << projectName << ": " << pathsString;
- Ice::StringSeq projectIncludePaths = Split(pathsString, ";,", true, true);
- ARMARX_VERBOSE << "Result: Data paths of ArmarX package " << projectName << ": " << projectIncludePaths;
- includePaths.insert(includePaths.end(), projectIncludePaths.begin(), projectIncludePaths.end());
- }
+ rfile = kinematicUnitInterfacePrx->getRobotFilename();
+ ARMARX_VERBOSE << "Relative robot file " << rfile;
+ ArmarXDataPath::getAbsolutePath(rfile, rfile, includePaths);
+ ARMARX_VERBOSE << "Absolute robot file " << rfile;
- rfile = kinematicUnitInterfacePrx->getRobotFilename();
- ARMARX_VERBOSE << "Relative robot file " << rfile;
- ArmarXDataPath::getAbsolutePath(rfile, rfile, includePaths);
- ARMARX_VERBOSE << "Absolute robot file " << rfile;
-
- robotNodeSetName = kinematicUnitInterfacePrx->getRobotNodeSetName();
- }
- catch (...)
- {
- ARMARX_ERROR << "Unable to retrieve robot filename.";
- }
-
- try
- {
- ARMARX_INFO << "Loading robot from file " << rfile;
- robot = loadRobotFile(rfile);
- }
- catch (const std::exception& e)
- {
- ARMARX_ERROR << "Failed to init robot: " << e.what();
- }
- catch (...)
- {
- ARMARX_ERROR << "Failed to init robot";
- }
+ robotNodeSetName = kinematicUnitInterfacePrx->getRobotNodeSetName();
+ }
+ catch (...)
+ {
+ ARMARX_ERROR << "Unable to retrieve robot filename.";
+ }
- if (!robot || !robot->hasRobotNodeSet(robotNodeSetName))
- {
- getObjectScheduler()->terminate();
- if (getWidget()->parentWidget())
+ try
{
- getWidget()->parentWidget()->close();
+ ARMARX_INFO << "Loading robot from file " << rfile;
+ robot = loadRobotFile(rfile);
+ }
+ catch (const std::exception& e)
+ {
+ ARMARX_ERROR << "Failed to init robot: " << e.what();
+ }
+ catch (...)
+ {
+ ARMARX_ERROR << "Failed to init robot";
}
- return;
- }
- // Check robot name and disable setZero Button if necessary
- if (not simox::alg::starts_with(robot->getName(), "Armar3"))
- {
- ARMARX_VERBOSE << "Disable the SetZero button because the robot name is '" << robot->getName() << "'.";
- ui.pushButtonKinematicUnitPos1->setDisabled(true);
- }
+ if (!robot || !robot->hasRobotNodeSet(robotNodeSetName))
+ {
+ getObjectScheduler()->terminate();
+ if (getWidget()->parentWidget())
+ {
+ getWidget()->parentWidget()->close();
+ }
+ return;
+ }
- kinematicUnitFile = rfile;
- robotNodeSet = robot->getRobotNodeSet(robotNodeSetName);
+ // Check robot name and disable setZero Button if necessary
+ if (not simox::alg::starts_with(robot->getName(), "Armar3"))
+ {
+ ARMARX_VERBOSE << "Disable the SetZero button because the robot name is '"
+ << robot->getName() << "'.";
+ ui.pushButtonKinematicUnitPos1->setDisabled(true);
+ }
- kinematicUnitVisualization = getCoinVisualization(robot);
- kinematicUnitNode = kinematicUnitVisualization->getCoinVisualization();
- robotVisu->addChild(kinematicUnitNode);
+ kinematicUnitFile = rfile;
+ robotNodeSet = robot->getRobotNodeSet(robotNodeSetName);
- // Fetch the current joint angles.
- synchronizeRobotJointAngles();
+ kinematicUnitVisualization = getCoinVisualization(robot);
+ kinematicUnitNode = kinematicUnitVisualization->getCoinVisualization();
+ robotVisu->addChild(kinematicUnitNode);
- initGUIComboBox(robotNodeSet); // init the pull down menu (QT: ComboBox)
- initGUIJointListTable(robotNodeSet);
+ // Fetch the current joint angles.
+ synchronizeRobotJointAngles();
- const auto initialDebugInfo = kinematicUnitInterfacePrx->getDebugInfo();
+ initGUIComboBox(robotNodeSet); // init the pull down menu (QT: ComboBox)
+ initGUIJointListTable(robotNodeSet);
- initializeUi(initialDebugInfo);
+ const auto initialDebugInfo = kinematicUnitInterfacePrx->getDebugInfo();
- QMetaObject::invokeMethod(this, "resetSlider");
- enableMainWidgetAsync(true);
-
- updateTask = new RunningTask<KinematicUnitWidgetController>(this, &KinematicUnitWidgetController::runUpdate);
- updateTask->start();
-}
+ initializeUi(initialDebugInfo);
-void KinematicUnitWidgetController::runUpdate()
-{
- Metronome metronome(Frequency::Hertz(10));
+ QMetaObject::invokeMethod(this, "resetSlider");
+ enableMainWidgetAsync(true);
- while(kinematicUnitInterfacePrx)
- {
- fetchData();
- metronome.waitForNextTick();
+ updateTask = new RunningTask<KinematicUnitWidgetController>(
+ this, &KinematicUnitWidgetController::runUpdate);
+ updateTask->start();
}
- ARMARX_INFO << "Connection to kinemetic unit lost. Update task terminates.";
-}
-
-void KinematicUnitWidgetController::onDisconnectComponent()
-{
- kinematicUnitInterfacePrx = nullptr;
-
- if(updateTask)
+ void
+ KinematicUnitWidgetController::runUpdate()
{
- updateTask->stop();
- updateTask->join();
- updateTask = nullptr;
- }
+ Metronome metronome(Frequency::Hertz(10));
- // killTimer(updateTimerId);
- enableMainWidgetAsync(false);
+ while (kinematicUnitInterfacePrx)
+ {
+ fetchData();
+ metronome.waitForNextTick();
+ }
- {
- std::unique_lock lock(mutexNodeSet);
- robot.reset();
- robotNodeSet.reset();
- currentNode.reset();
+ ARMARX_INFO << "Connection to kinemetic unit lost. Update task terminates.";
}
+ void
+ KinematicUnitWidgetController::onDisconnectComponent()
{
- std::unique_lock lock(*mutex3D);
- robotVisu->removeAllChildren();
- debugLayerVisu->removeAllChildren();
- }
-
-}
+ kinematicUnitInterfacePrx = nullptr;
-void KinematicUnitWidgetController::onExitComponent()
-{
- kinematicUnitInterfacePrx = nullptr;
-
- if(updateTask)
- {
- updateTask->stop();
- updateTask->join();
- updateTask = nullptr;
- }
+ if (updateTask)
+ {
+ updateTask->stop();
+ updateTask->join();
+ updateTask = nullptr;
+ }
- enableMainWidgetAsync(false);
+ // killTimer(updateTimerId);
+ enableMainWidgetAsync(false);
- {
- std::unique_lock lock(*mutex3D);
+ {
+ std::unique_lock lock(mutexNodeSet);
+ robot.reset();
+ robotNodeSet.reset();
+ currentNode.reset();
+ }
- if (robotVisu)
{
+ std::unique_lock lock(*mutex3D);
robotVisu->removeAllChildren();
- robotVisu->unref();
- robotVisu = NULL;
+ debugLayerVisu->removeAllChildren();
}
+ }
+
+ void
+ KinematicUnitWidgetController::onExitComponent()
+ {
+ kinematicUnitInterfacePrx = nullptr;
- if (debugLayerVisu)
+ if (updateTask)
{
- debugLayerVisu->removeAllChildren();
- debugLayerVisu->unref();
- debugLayerVisu = NULL;
+ updateTask->stop();
+ updateTask->join();
+ updateTask = nullptr;
}
- if (rootVisu)
+ enableMainWidgetAsync(false);
+
{
- rootVisu->removeAllChildren();
- rootVisu->unref();
- rootVisu = NULL;
+ std::unique_lock lock(*mutex3D);
+
+ if (robotVisu)
+ {
+ robotVisu->removeAllChildren();
+ robotVisu->unref();
+ robotVisu = NULL;
+ }
+
+ if (debugLayerVisu)
+ {
+ debugLayerVisu->removeAllChildren();
+ debugLayerVisu->unref();
+ debugLayerVisu = NULL;
+ }
+
+ if (rootVisu)
+ {
+ rootVisu->removeAllChildren();
+ rootVisu->unref();
+ rootVisu = NULL;
+ }
}
- }
- /*
+ /*
if (debugDrawer && debugDrawer->getObjectScheduler())
{
ARMARX_INFO << "Removing DebugDrawer component...";
@@ -353,757 +362,848 @@ void KinematicUnitWidgetController::onExitComponent()
ARMARX_INFO << "Removing DebugDrawer component...done";
}
*/
-}
-
-QPointer<QDialog> KinematicUnitWidgetController::getConfigDialog(QWidget* parent)
-{
- if (!dialog)
- {
- dialog = new KinematicUnitConfigDialog(parent);
- dialog->setName(dialog->getDefaultName());
}
- return qobject_cast<KinematicUnitConfigDialog*>(dialog);
-}
-
-void KinematicUnitWidgetController::configured()
-{
- ARMARX_VERBOSE << "KinematicUnitWidget::configured()";
- kinematicUnitName = dialog->proxyFinder->getSelectedProxyName().toStdString();
- enableValueValidator = dialog->ui->checkBox->isChecked();
- viewerEnabled = dialog->ui->checkBox3DViewerEnabled->isChecked();
- historyTime = dialog->ui->spinBoxHistory->value() * 1000;
- currentValueMax = dialog->ui->doubleSpinBoxMaxMinCurrent->value();
-}
-
-void KinematicUnitWidgetController::loadSettings(QSettings* settings)
-{
- kinematicUnitName = settings->value("kinematicUnitName", KINEMATIC_UNIT_NAME_DEFAULT).toString().toStdString();
- enableValueValidator = settings->value("enableValueValidator", true).toBool();
- viewerEnabled = settings->value("viewerEnabled", true).toBool();
- historyTime = settings->value("historyTime", 100).toInt() * 1000;
- currentValueMax = settings->value("currentValueMax", 5.0).toFloat();
-}
-
-void KinematicUnitWidgetController::saveSettings(QSettings* settings)
-{
- settings->setValue("kinematicUnitName", QString::fromStdString(kinematicUnitName));
- settings->setValue("enableValueValidator", enableValueValidator);
- settings->setValue("viewerEnabled", viewerEnabled);
- assert(historyTime % 1000 == 0);
- settings->setValue("historyTime", static_cast<int>(historyTime / 1000));
- settings->setValue("currentValueMax", currentValueMax);
-}
-
-
-void KinematicUnitWidgetController::showVisuLayers(bool show)
-{
- if (debugDrawer)
+ QPointer<QDialog>
+ KinematicUnitWidgetController::getConfigDialog(QWidget* parent)
{
- if (show)
- {
- debugDrawer->enableAllLayers();
- }
- else
+ if (!dialog)
{
- debugDrawer->disableAllLayers();
+ dialog = new KinematicUnitConfigDialog(parent);
+ dialog->setName(dialog->getDefaultName());
}
+
+ return qobject_cast<KinematicUnitConfigDialog*>(dialog);
}
-}
-void KinematicUnitWidgetController::copyToClipboard()
-{
- NameValueMap values;
+ void
+ KinematicUnitWidgetController::configured()
{
- std::unique_lock lock(mutexNodeSet);
-
- ARMARX_CHECK_NOT_NULL(kinematicUnitInterfacePrx);
- const auto debugInfo = kinematicUnitInterfacePrx->getDebugInfo();
-
- const auto selectedControlMode = getSelectedControlMode();
-
- if (selectedControlMode == ePositionControl)
- {
- values = debugInfo.jointAngles;
- }
- else if (selectedControlMode == eVelocityControl)
- {
- values = debugInfo.jointVelocities;
- }
+ ARMARX_VERBOSE << "KinematicUnitWidget::configured()";
+ kinematicUnitName = dialog->proxyFinder->getSelectedProxyName().toStdString();
+ enableValueValidator = dialog->ui->checkBox->isChecked();
+ viewerEnabled = dialog->ui->checkBox3DViewerEnabled->isChecked();
+ historyTime = dialog->ui->spinBoxHistory->value() * 1000;
+ currentValueMax = dialog->ui->doubleSpinBoxMaxMinCurrent->value();
}
- JSONObjectPtr serializer = new JSONObject();
- for (auto& kv : values)
+ void
+ KinematicUnitWidgetController::loadSettings(QSettings* settings)
{
- serializer->setFloat(kv.first, kv.second);
+ kinematicUnitName = settings->value("kinematicUnitName", KINEMATIC_UNIT_NAME_DEFAULT)
+ .toString()
+ .toStdString();
+ enableValueValidator = settings->value("enableValueValidator", true).toBool();
+ viewerEnabled = settings->value("viewerEnabled", true).toBool();
+ historyTime = settings->value("historyTime", 100).toInt() * 1000;
+ currentValueMax = settings->value("currentValueMax", 5.0).toFloat();
}
- const QString json = QString::fromStdString(serializer->asString(true));
- QClipboard* clipboard = QApplication::clipboard();
- clipboard->setText(json);
- QApplication::processEvents();
-}
-
-
-void KinematicUnitWidgetController::updateGuiElements()
-{
- // modelUpdateCB();
-}
-void KinematicUnitWidgetController::updateKinematicUnitListInDialog()
-{
-}
-
-void KinematicUnitWidgetController::modelUpdateCB()
-{
-}
-
-SoNode* KinematicUnitWidgetController::getScene()
-{
- if (viewerEnabled)
+ void
+ KinematicUnitWidgetController::saveSettings(QSettings* settings)
{
- ARMARX_INFO << "Returning scene ";
- return rootVisu;
+ settings->setValue("kinematicUnitName", QString::fromStdString(kinematicUnitName));
+ settings->setValue("enableValueValidator", enableValueValidator);
+ settings->setValue("viewerEnabled", viewerEnabled);
+ assert(historyTime % 1000 == 0);
+ settings->setValue("historyTime", static_cast<int>(historyTime / 1000));
+ settings->setValue("currentValueMax", currentValueMax);
}
- else
+
+ void
+ KinematicUnitWidgetController::showVisuLayers(bool show)
{
- ARMARX_INFO << "viewer disabled - returning null scene";
- return NULL;
+ if (debugDrawer)
+ {
+ if (show)
+ {
+ debugDrawer->enableAllLayers();
+ }
+ else
+ {
+ debugDrawer->disableAllLayers();
+ }
+ }
}
-}
-
-void KinematicUnitWidgetController::connectSlots()
-{
- connect(ui.pushButtonKinematicUnitPos1, SIGNAL(clicked()), this, SLOT(kinematicUnitZeroPosition()));
- connect(ui.nodeListComboBox, SIGNAL(currentIndexChanged(int)), this, SLOT(selectJoint(int)));
- connect(ui.horizontalSliderKinematicUnitPos, SIGNAL(valueChanged(int)), this, SLOT(sliderValueChanged(int)));
-
- connect(ui.horizontalSliderKinematicUnitPos, SIGNAL(sliderReleased()), this, SLOT(resetSliderToZeroPosition()));
-
- connect(ui.radioButtonPositionControl, SIGNAL(clicked(bool)), this, SLOT(setControlModePosition()));
- connect(ui.radioButtonVelocityControl, SIGNAL(clicked(bool)), this, SLOT(setControlModeVelocity()));
- connect(ui.radioButtonTorqueControl, SIGNAL(clicked(bool)), this, SLOT(setControlModeTorque()));
- connect(ui.pushButtonFromJson, SIGNAL(clicked()), this, SLOT(on_pushButtonFromJson_clicked()));
-
- connect(ui.copyToClipboard, SIGNAL(clicked()), this, SLOT(copyToClipboard()));
- connect(ui.showDebugLayer, SIGNAL(toggled(bool)), this, SLOT(showVisuLayers(bool)), Qt::QueuedConnection);
-
- connect(this, SIGNAL(jointAnglesReported()), this, SLOT(updateJointAnglesTable()), Qt::QueuedConnection);
- connect(this, SIGNAL(jointVelocitiesReported()), this, SLOT(updateJointVelocitiesTable()), Qt::QueuedConnection);
- connect(this, SIGNAL(jointTorquesReported()), this, SLOT(updateJointTorquesTable()), Qt::QueuedConnection);
- connect(this, SIGNAL(jointCurrentsReported()), this, SLOT(updateJointCurrentsTable()), Qt::QueuedConnection);
- connect(this, SIGNAL(jointMotorTemperaturesReported()), this, SLOT(updateMotorTemperaturesTable()), Qt::QueuedConnection);
- connect(this, SIGNAL(jointControlModesReported()), this, SLOT(updateControlModesTable()), Qt::QueuedConnection);
- connect(this, SIGNAL(jointStatusesReported()), this, SLOT(updateJointStatusesTable()), Qt::QueuedConnection);
-
- connect(ui.tableJointList, SIGNAL(cellDoubleClicked(int, int)), this, SLOT(selectJointFromTableWidget(int, int)), Qt::QueuedConnection);
-
- connect(ui.checkBoxUseDegree, SIGNAL(clicked()), this, SLOT(resetSlider()), Qt::QueuedConnection);
- connect(ui.checkBoxUseDegree, SIGNAL(clicked()), this, SLOT(setControlModePosition()));
- connect(ui.checkBoxUseDegree, SIGNAL(clicked()), this, SLOT(setControlModeVelocity()));
- connect(ui.checkBoxUseDegree, SIGNAL(clicked()), this, SLOT(setControlModeTorque()));
-
- connect(this, SIGNAL(onDebugInfoReceived(const DebugInfo&)), this, SLOT(debugInfoReceived(const DebugInfo&)));
-}
-
-void KinematicUnitWidgetController::initializeUi(const DebugInfo& debugInfo)
-{
- //signal clicked is not emitted if you call setDown(), setChecked() or toggle().
-
- // there is no default control mode
- setControlModeRadioButtonGroup(ControlMode::eUnknown);
+ void
+ KinematicUnitWidgetController::copyToClipboard()
+ {
+ NameValueMap values;
+ {
+ std::unique_lock lock(mutexNodeSet);
- ui.widgetSliderFactor->setVisible(false);
+ ARMARX_CHECK_NOT_NULL(kinematicUnitInterfacePrx);
+ const auto debugInfo = kinematicUnitInterfacePrx->getDebugInfo();
- fetchData();
-}
+ const auto selectedControlMode = getSelectedControlMode();
+ if (selectedControlMode == ePositionControl)
+ {
+ values = debugInfo.jointAngles;
+ }
+ else if (selectedControlMode == eVelocityControl)
+ {
+ values = debugInfo.jointVelocities;
+ }
+ }
-void KinematicUnitWidgetController::kinematicUnitZeroVelocity()
-{
- if (!robotNodeSet)
- {
- return;
+ JSONObjectPtr serializer = new JSONObject();
+ for (auto& kv : values)
+ {
+ serializer->setFloat(kv.first, kv.second);
+ }
+ const QString json = QString::fromStdString(serializer->asString(true));
+ QClipboard* clipboard = QApplication::clipboard();
+ clipboard->setText(json);
+ QApplication::processEvents();
}
- std::unique_lock lock(mutexNodeSet);
- std::vector< VirtualRobot::RobotNodePtr > rn = robotNodeSet->getAllRobotNodes();
- NameValueMap vels;
- NameControlModeMap jointModes;
-
- for (unsigned int i = 0; i < rn.size(); i++)
+ void
+ KinematicUnitWidgetController::updateGuiElements()
{
- jointModes[rn[i]->getName()] = eVelocityControl;
- vels[rn[i]->getName()] = 0.0f;
+ // modelUpdateCB();
}
- try
- {
- kinematicUnitInterfacePrx->switchControlMode(jointModes);
- kinematicUnitInterfacePrx->setJointVelocities(vels);
- }
- catch (...)
+ void
+ KinematicUnitWidgetController::updateKinematicUnitListInDialog()
{
}
- const auto selectedControlMode = getSelectedControlMode();
- if (selectedControlMode == eVelocityControl)
+ void
+ KinematicUnitWidgetController::modelUpdateCB()
{
- ui.horizontalSliderKinematicUnitPos->setSliderPosition(SLIDER_ZERO_POSITION);
}
-}
-
-void KinematicUnitWidgetController::resetSlider()
-{
- const auto selectedControlMode = getSelectedControlMode();
-
- if (selectedControlMode == eVelocityControl || selectedControlMode == eTorqueControl)
- {
- resetSliderToZeroPosition();
- }
- else if (selectedControlMode == ePositionControl)
+ SoNode*
+ KinematicUnitWidgetController::getScene()
{
- if (currentNode)
+ if (viewerEnabled)
{
- const bool isDeg = ui.checkBoxUseDegree->isChecked();
- const bool isRot = currentNode->isRotationalJoint();
- const auto factor = isRot && ! isDeg ? SLIDER_POS_RAD_MULTIPLIER : SLIDER_POS_DEG_MULTIPLIER;
- float conversionFactor = isRot && isDeg ? 180.0 / M_PI : 1.0f;
- float pos = currentNode->getJointValue() * conversionFactor;
-
- ui.lcdNumberKinematicUnitJointValue->display((int)pos);
- ui.horizontalSliderKinematicUnitPos->setSliderPosition((int)(pos * factor));
+ ARMARX_INFO << "Returning scene ";
+ return rootVisu;
+ }
+ else
+ {
+ ARMARX_INFO << "viewer disabled - returning null scene";
+ return NULL;
}
}
-}
-
-void KinematicUnitWidgetController::resetSliderToZeroPosition()
-{
- const auto selectedControlMode = getSelectedControlMode();
+ void
+ KinematicUnitWidgetController::connectSlots()
+ {
+ connect(ui.pushButtonKinematicUnitPos1,
+ SIGNAL(clicked()),
+ this,
+ SLOT(kinematicUnitZeroPosition()));
+
+ connect(
+ ui.nodeListComboBox, SIGNAL(currentIndexChanged(int)), this, SLOT(selectJoint(int)));
+ connect(ui.horizontalSliderKinematicUnitPos,
+ SIGNAL(valueChanged(int)),
+ this,
+ SLOT(sliderValueChanged(int)));
+
+ connect(ui.horizontalSliderKinematicUnitPos,
+ SIGNAL(sliderReleased()),
+ this,
+ SLOT(resetSliderToZeroPosition()));
+
+ connect(ui.radioButtonPositionControl,
+ SIGNAL(clicked(bool)),
+ this,
+ SLOT(setControlModePosition()));
+ connect(ui.radioButtonVelocityControl,
+ SIGNAL(clicked(bool)),
+ this,
+ SLOT(setControlModeVelocity()));
+ connect(
+ ui.radioButtonTorqueControl, SIGNAL(clicked(bool)), this, SLOT(setControlModeTorque()));
+ connect(
+ ui.pushButtonFromJson, SIGNAL(clicked()), this, SLOT(on_pushButtonFromJson_clicked()));
+
+ connect(ui.copyToClipboard, SIGNAL(clicked()), this, SLOT(copyToClipboard()));
+ connect(ui.showDebugLayer,
+ SIGNAL(toggled(bool)),
+ this,
+ SLOT(showVisuLayers(bool)),
+ Qt::QueuedConnection);
+
+ connect(this,
+ SIGNAL(jointAnglesReported()),
+ this,
+ SLOT(updateJointAnglesTable()),
+ Qt::QueuedConnection);
+ connect(this,
+ SIGNAL(jointVelocitiesReported()),
+ this,
+ SLOT(updateJointVelocitiesTable()),
+ Qt::QueuedConnection);
+ connect(this,
+ SIGNAL(jointTorquesReported()),
+ this,
+ SLOT(updateJointTorquesTable()),
+ Qt::QueuedConnection);
+ connect(this,
+ SIGNAL(jointCurrentsReported()),
+ this,
+ SLOT(updateJointCurrentsTable()),
+ Qt::QueuedConnection);
+ connect(this,
+ SIGNAL(jointMotorTemperaturesReported()),
+ this,
+ SLOT(updateMotorTemperaturesTable()),
+ Qt::QueuedConnection);
+ connect(this,
+ SIGNAL(jointControlModesReported()),
+ this,
+ SLOT(updateControlModesTable()),
+ Qt::QueuedConnection);
+ connect(this,
+ SIGNAL(jointStatusesReported()),
+ this,
+ SLOT(updateJointStatusesTable()),
+ Qt::QueuedConnection);
+
+ connect(ui.tableJointList,
+ SIGNAL(cellDoubleClicked(int, int)),
+ this,
+ SLOT(selectJointFromTableWidget(int, int)),
+ Qt::QueuedConnection);
+
+ connect(ui.checkBoxUseDegree,
+ SIGNAL(clicked()),
+ this,
+ SLOT(resetSlider()),
+ Qt::QueuedConnection);
+ connect(ui.checkBoxUseDegree, SIGNAL(clicked()), this, SLOT(setControlModePosition()));
+ connect(ui.checkBoxUseDegree, SIGNAL(clicked()), this, SLOT(setControlModeVelocity()));
+ connect(ui.checkBoxUseDegree, SIGNAL(clicked()), this, SLOT(setControlModeTorque()));
+
+ connect(this,
+ SIGNAL(onDebugInfoReceived(const DebugInfo&)),
+ this,
+ SLOT(debugInfoReceived(const DebugInfo&)));
+ }
+
+ void
+ KinematicUnitWidgetController::initializeUi(const DebugInfo& debugInfo)
+ {
+ //signal clicked is not emitted if you call setDown(), setChecked() or toggle().
+
+ // there is no default control mode
+ setControlModeRadioButtonGroup(ControlMode::eUnknown);
+
+ ui.widgetSliderFactor->setVisible(false);
- if (selectedControlMode == eVelocityControl || selectedControlMode == eTorqueControl)
- {
- ui.horizontalSliderKinematicUnitPos->setSliderPosition(SLIDER_ZERO_POSITION);
- ui.lcdNumberKinematicUnitJointValue->display(SLIDER_ZERO_POSITION);
- }
-}
-
-void KinematicUnitWidgetController::setControlModeRadioButtonGroup(const ControlMode& controlMode)
-{
- ARMARX_VERBOSE << "Setting control mode of radio button group to " << controlMode;
-
- switch(controlMode)
- {
- case eDisabled:
- case eUnknown:
- case ePositionVelocityControl:
- ui.radioButtonUnknown->setChecked(true);
- break;
- case ePositionControl:
- ui.radioButtonPositionControl->setChecked(true);
- break;
- case eVelocityControl:
- ui.radioButtonVelocityControl->setChecked(true);
- break;
- case eTorqueControl:
- ui.radioButtonTorqueControl->setChecked(true);
- break;
+ fetchData();
}
-}
-
-void KinematicUnitWidgetController::setControlModePosition()
-{
- if (!ui.radioButtonPositionControl->isChecked())
+ void
+ KinematicUnitWidgetController::kinematicUnitZeroVelocity()
{
- return;
- }
- NameControlModeMap jointModes;
- // selectedControlMode = ePositionControl;
- ui.widgetSliderFactor->setVisible(false);
+ if (!robotNodeSet)
+ {
+ return;
+ }
- // FIXME currentNode should be passed to this function!
+ std::unique_lock lock(mutexNodeSet);
+ std::vector<VirtualRobot::RobotNodePtr> rn = robotNodeSet->getAllRobotNodes();
+ NameValueMap vels;
+ NameControlModeMap jointModes;
- if (currentNode)
- {
- QString unit = currentNode->isRotationalJoint()
- ? (ui.checkBoxUseDegree->isChecked() ? "deg" : "rad")
- : "mm";
- ui.labelUnit->setText(unit);
- const bool isDeg = ui.checkBoxUseDegree->isChecked();
- const bool isRot = currentNode->isRotationalJoint();
- const auto factor = isRot && ! isDeg ? SLIDER_POS_RAD_MULTIPLIER : SLIDER_POS_DEG_MULTIPLIER;
- float conversionFactor = isRot && isDeg ? 180.0 / M_PI : 1.0f;
- jointModes[currentNode->getName()] = ePositionControl;
+ for (unsigned int i = 0; i < rn.size(); i++)
+ {
+ jointModes[rn[i]->getName()] = eVelocityControl;
+ vels[rn[i]->getName()] = 0.0f;
+ }
- if (kinematicUnitInterfacePrx)
+ try
{
kinematicUnitInterfacePrx->switchControlMode(jointModes);
+ kinematicUnitInterfacePrx->setJointVelocities(vels);
}
-
- float lo = currentNode->getJointLimitLo() * conversionFactor;
- float hi = currentNode->getJointLimitHi() * conversionFactor;
-
- if (hi - lo <= 0.0f)
+ catch (...)
{
- return;
}
+ const auto selectedControlMode = getSelectedControlMode();
+ if (selectedControlMode == eVelocityControl)
{
- // currentNode->getJointValue() can we wrong after we re-connected to the robot unit.
- // E.g., it can be 0 although the torso joint was at -365 before the unit disconnected.
- // Therefore, we first have to fetch the actual joint values and use that one.
- // However, this should actually not be necessary, as the robot model should be updated
- // via the topics.
- synchronizeRobotJointAngles();
+ ui.horizontalSliderKinematicUnitPos->setSliderPosition(SLIDER_ZERO_POSITION);
}
+ }
- float pos = currentNode->getJointValue() * conversionFactor;
- ARMARX_INFO << "Setting position control for current node "
- << "(name '" << currentNode->getName() << "' with current value " << pos << ")";
+ void
+ KinematicUnitWidgetController::resetSlider()
+ {
+ const auto selectedControlMode = getSelectedControlMode();
- // Setting the slider position to pos will set the position to the slider tick closest to pos
- // This will initially send a position target with a small delta to the joint.
- ui.horizontalSliderKinematicUnitPos->blockSignals(true);
+ if (selectedControlMode == eVelocityControl || selectedControlMode == eTorqueControl)
+ {
+ resetSliderToZeroPosition();
+ }
+ else if (selectedControlMode == ePositionControl)
+ {
+ if (currentNode)
+ {
+ const bool isDeg = ui.checkBoxUseDegree->isChecked();
+ const bool isRot = currentNode->isRotationalJoint();
+ const auto factor =
+ isRot && !isDeg ? SLIDER_POS_RAD_MULTIPLIER : SLIDER_POS_DEG_MULTIPLIER;
+ float conversionFactor = isRot && isDeg ? 180.0 / M_PI : 1.0f;
+ float pos = currentNode->getJointValue() * conversionFactor;
+
+ ui.lcdNumberKinematicUnitJointValue->display((int)pos);
+ ui.horizontalSliderKinematicUnitPos->setSliderPosition((int)(pos * factor));
+ }
+ }
+ }
- ui.horizontalSliderKinematicUnitPos->setMaximum(hi * factor);
- ui.horizontalSliderKinematicUnitPos->setMinimum(lo * factor);
- ui.lcdNumberKinematicUnitJointValue->display(pos);
+ void
+ KinematicUnitWidgetController::resetSliderToZeroPosition()
+ {
+ const auto selectedControlMode = getSelectedControlMode();
- ui.horizontalSliderKinematicUnitPos->blockSignals(false);
- resetSlider();
+ if (selectedControlMode == eVelocityControl || selectedControlMode == eTorqueControl)
+ {
+ ui.horizontalSliderKinematicUnitPos->setSliderPosition(SLIDER_ZERO_POSITION);
+ ui.lcdNumberKinematicUnitJointValue->display(SLIDER_ZERO_POSITION);
+ }
}
-}
-void KinematicUnitWidgetController::setControlModeVelocity()
-{
- if (!ui.radioButtonVelocityControl->isChecked())
+ void
+ KinematicUnitWidgetController::setControlModeRadioButtonGroup(const ControlMode& controlMode)
{
- return;
+ ARMARX_VERBOSE << "Setting control mode of radio button group to " << controlMode;
+
+ switch (controlMode)
+ {
+ case eDisabled:
+ case eUnknown:
+ case ePositionVelocityControl:
+ ui.radioButtonUnknown->setChecked(true);
+ break;
+ case ePositionControl:
+ ui.radioButtonPositionControl->setChecked(true);
+ break;
+ case eVelocityControl:
+ ui.radioButtonVelocityControl->setChecked(true);
+ break;
+ case eTorqueControl:
+ ui.radioButtonTorqueControl->setChecked(true);
+ break;
+ }
}
- NameControlModeMap jointModes;
- NameValueMap jointVelocities;
- if (currentNode)
+ void
+ KinematicUnitWidgetController::setControlModePosition()
{
- jointModes[currentNode->getName()] = eVelocityControl;
-
- // set the velocity to zero to stop any previous controller (e.g. torque controller)
- jointVelocities[currentNode->getName()] = 0;
+ if (!ui.radioButtonPositionControl->isChecked())
+ {
+ return;
+ }
+ NameControlModeMap jointModes;
+ // selectedControlMode = ePositionControl;
+ ui.widgetSliderFactor->setVisible(false);
- const bool isDeg = ui.checkBoxUseDegree->isChecked();
- const bool isRot = currentNode->isRotationalJoint();
- QString unit = isRot ?
- (isDeg ? "deg/s" : "rad/(100*s)") :
- "mm/s";
- ui.labelUnit->setText(unit);
- ARMARX_INFO << "setting velocity control for current Node Name: " << currentNode->getName() << flush;
- float lo = isRot ? (isDeg ? -90 : -M_PI * 100) : -1000;
- float hi = isRot ? (isDeg ? +90 : +M_PI * 100) : 1000;
+ // FIXME currentNode should be passed to this function!
- try
+ if (currentNode)
{
+ QString unit = currentNode->isRotationalJoint()
+ ? (ui.checkBoxUseDegree->isChecked() ? "deg" : "rad")
+ : "mm";
+ ui.labelUnit->setText(unit);
+ const bool isDeg = ui.checkBoxUseDegree->isChecked();
+ const bool isRot = currentNode->isRotationalJoint();
+ const auto factor =
+ isRot && !isDeg ? SLIDER_POS_RAD_MULTIPLIER : SLIDER_POS_DEG_MULTIPLIER;
+ float conversionFactor = isRot && isDeg ? 180.0 / M_PI : 1.0f;
+ jointModes[currentNode->getName()] = ePositionControl;
+
if (kinematicUnitInterfacePrx)
{
kinematicUnitInterfacePrx->switchControlMode(jointModes);
- kinematicUnitInterfacePrx->setJointVelocities(jointVelocities);
}
- }
- catch (...)
- {
- }
-
- ui.widgetSliderFactor->setVisible(true);
+ float lo = currentNode->getJointLimitLo() * conversionFactor;
+ float hi = currentNode->getJointLimitHi() * conversionFactor;
- ui.horizontalSliderKinematicUnitPos->blockSignals(true);
- ui.horizontalSliderKinematicUnitPos->setMaximum(hi);
- ui.horizontalSliderKinematicUnitPos->setMinimum(lo);
- ui.horizontalSliderKinematicUnitPos->blockSignals(false);
- resetSlider();
- }
-}
-
-ControlMode KinematicUnitWidgetController::getSelectedControlMode() const
-{
- if(ui.radioButtonPositionControl->isChecked())
- {
- return ControlMode::ePositionControl;
- }
+ if (hi - lo <= 0.0f)
+ {
+ return;
+ }
- if (ui.radioButtonVelocityControl->isChecked())
- {
- return ControlMode::eVelocityControl;
- }
+ {
+ // currentNode->getJointValue() can we wrong after we re-connected to the robot unit.
+ // E.g., it can be 0 although the torso joint was at -365 before the unit disconnected.
+ // Therefore, we first have to fetch the actual joint values and use that one.
+ // However, this should actually not be necessary, as the robot model should be updated
+ // via the topics.
+ synchronizeRobotJointAngles();
+ }
- if (ui.radioButtonTorqueControl->isChecked())
- {
- return ControlMode::eTorqueControl;
- }
+ float pos = currentNode->getJointValue() * conversionFactor;
+ ARMARX_INFO << "Setting position control for current node "
+ << "(name '" << currentNode->getName() << "' with current value " << pos
+ << ")";
- // if no button is checked, then the joint is likely initialized but no controller has been loaded yet
- // (well, the no movement controller should be active)
- return ControlMode::eUnknown;
+ // Setting the slider position to pos will set the position to the slider tick closest to pos
+ // This will initially send a position target with a small delta to the joint.
+ ui.horizontalSliderKinematicUnitPos->blockSignals(true);
-
-}
+ ui.horizontalSliderKinematicUnitPos->setMaximum(hi * factor);
+ ui.horizontalSliderKinematicUnitPos->setMinimum(lo * factor);
+ ui.lcdNumberKinematicUnitJointValue->display(pos);
-void KinematicUnitWidgetController::setControlModeTorque()
-{
- if (!ui.radioButtonTorqueControl->isChecked())
- {
- return;
+ ui.horizontalSliderKinematicUnitPos->blockSignals(false);
+ resetSlider();
+ }
}
- NameControlModeMap jointModes;
- if (currentNode)
+ void
+ KinematicUnitWidgetController::setControlModeVelocity()
{
- jointModes[currentNode->getName()] = eTorqueControl;
- ui.labelUnit->setText("Ncm");
- ARMARX_INFO << "setting torque control for current Node Name: " << currentNode->getName() << flush;
+ if (!ui.radioButtonVelocityControl->isChecked())
+ {
+ return;
+ }
+ NameControlModeMap jointModes;
+ NameValueMap jointVelocities;
- if (kinematicUnitInterfacePrx)
+ if (currentNode)
{
+ jointModes[currentNode->getName()] = eVelocityControl;
+
+ // set the velocity to zero to stop any previous controller (e.g. torque controller)
+ jointVelocities[currentNode->getName()] = 0;
+
+ const bool isDeg = ui.checkBoxUseDegree->isChecked();
+ const bool isRot = currentNode->isRotationalJoint();
+ QString unit = isRot ? (isDeg ? "deg/s" : "rad/(100*s)") : "mm/s";
+ ui.labelUnit->setText(unit);
+ ARMARX_INFO << "setting velocity control for current Node Name: "
+ << currentNode->getName() << flush;
+ float lo = isRot ? (isDeg ? -90 : -M_PI * 100) : -1000;
+ float hi = isRot ? (isDeg ? +90 : +M_PI * 100) : 1000;
+
try
{
- kinematicUnitInterfacePrx->switchControlMode(jointModes);
+ if (kinematicUnitInterfacePrx)
+ {
+ kinematicUnitInterfacePrx->switchControlMode(jointModes);
+ kinematicUnitInterfacePrx->setJointVelocities(jointVelocities);
+ }
}
catch (...)
{
-
}
- }
-
- ui.horizontalSliderKinematicUnitPos->blockSignals(true);
- ui.horizontalSliderKinematicUnitPos->setMaximum(20000.0);
- ui.horizontalSliderKinematicUnitPos->setMinimum(-20000.0);
- ui.widgetSliderFactor->setVisible(true);
+ ui.widgetSliderFactor->setVisible(true);
- ui.horizontalSliderKinematicUnitPos->blockSignals(false);
- resetSlider();
+ ui.horizontalSliderKinematicUnitPos->blockSignals(true);
+ ui.horizontalSliderKinematicUnitPos->setMaximum(hi);
+ ui.horizontalSliderKinematicUnitPos->setMinimum(lo);
+ ui.horizontalSliderKinematicUnitPos->blockSignals(false);
+ resetSlider();
+ }
}
-}
-VirtualRobot::RobotPtr KinematicUnitWidgetController::loadRobotFile(std::string fileName)
-{
- VirtualRobot::RobotPtr robot;
-
- if (verbose)
+ ControlMode
+ KinematicUnitWidgetController::getSelectedControlMode() const
{
- ARMARX_INFO << "Loading KinematicUnit " << kinematicUnitName << " from " << kinematicUnitFile << " ..." << flush;
+ if (ui.radioButtonPositionControl->isChecked())
+ {
+ return ControlMode::ePositionControl;
+ }
+
+ if (ui.radioButtonVelocityControl->isChecked())
+ {
+ return ControlMode::eVelocityControl;
+ }
+
+ if (ui.radioButtonTorqueControl->isChecked())
+ {
+ return ControlMode::eTorqueControl;
+ }
+
+ // if no button is checked, then the joint is likely initialized but no controller has been loaded yet
+ // (well, the no movement controller should be active)
+ return ControlMode::eUnknown;
}
- if (!ArmarXDataPath::getAbsolutePath(fileName, fileName))
+ void
+ KinematicUnitWidgetController::setControlModeTorque()
{
- ARMARX_INFO << "Could not find Robot XML file with name " << fileName << flush;
- }
+ if (!ui.radioButtonTorqueControl->isChecked())
+ {
+ return;
+ }
+ NameControlModeMap jointModes;
- robot = VirtualRobot::RobotIO::loadRobot(fileName);
+ if (currentNode)
+ {
+ jointModes[currentNode->getName()] = eTorqueControl;
+ ui.labelUnit->setText("Ncm");
+ ARMARX_INFO << "setting torque control for current Node Name: "
+ << currentNode->getName() << flush;
- if (!robot)
- {
- ARMARX_INFO << "Could not find Robot XML file with name " << fileName << "(" << kinematicUnitName << ")" << flush;
- }
+ if (kinematicUnitInterfacePrx)
+ {
+ try
+ {
+ kinematicUnitInterfacePrx->switchControlMode(jointModes);
+ }
+ catch (...)
+ {
+ }
+ }
- return robot;
-}
+ ui.horizontalSliderKinematicUnitPos->blockSignals(true);
+ ui.horizontalSliderKinematicUnitPos->setMaximum(20000.0);
+ ui.horizontalSliderKinematicUnitPos->setMinimum(-20000.0);
-VirtualRobot::CoinVisualizationPtr KinematicUnitWidgetController::getCoinVisualization(VirtualRobot::RobotPtr robot)
-{
- VirtualRobot::CoinVisualizationPtr coinVisualization;
+ ui.widgetSliderFactor->setVisible(true);
+
+ ui.horizontalSliderKinematicUnitPos->blockSignals(false);
+ resetSlider();
+ }
+ }
- if (robot != NULL)
+ VirtualRobot::RobotPtr
+ KinematicUnitWidgetController::loadRobotFile(std::string fileName)
{
- ARMARX_VERBOSE << "getting coin visualization" << flush;
- coinVisualization = robot->getVisualization<VirtualRobot::CoinVisualization>();
+ VirtualRobot::RobotPtr robot;
- if (!coinVisualization || !coinVisualization->getCoinVisualization())
+ if (verbose)
{
- ARMARX_INFO << "could not get coin visualization" << flush;
+ ARMARX_INFO << "Loading KinematicUnit " << kinematicUnitName << " from "
+ << kinematicUnitFile << " ..." << flush;
}
- }
- return coinVisualization;
-}
+ if (!ArmarXDataPath::getAbsolutePath(fileName, fileName))
+ {
+ ARMARX_INFO << "Could not find Robot XML file with name " << fileName << flush;
+ }
-VirtualRobot::RobotNodeSetPtr KinematicUnitWidgetController::getRobotNodeSet(VirtualRobot::RobotPtr robot, std::string nodeSetName)
-{
- VirtualRobot::RobotNodeSetPtr nodeSetPtr;
+ robot = VirtualRobot::RobotIO::loadRobot(fileName);
+
+ if (!robot)
+ {
+ ARMARX_INFO << "Could not find Robot XML file with name " << fileName << "("
+ << kinematicUnitName << ")" << flush;
+ }
- if (robot)
+ return robot;
+ }
+
+ VirtualRobot::CoinVisualizationPtr
+ KinematicUnitWidgetController::getCoinVisualization(VirtualRobot::RobotPtr robot)
{
- nodeSetPtr = robot->getRobotNodeSet(nodeSetName);
+ VirtualRobot::CoinVisualizationPtr coinVisualization;
- if (!nodeSetPtr)
+ if (robot != NULL)
{
- ARMARX_INFO << "RobotNodeSet with name " << nodeSetName << " is not defined" << flush;
+ ARMARX_VERBOSE << "getting coin visualization" << flush;
+ coinVisualization = robot->getVisualization<VirtualRobot::CoinVisualization>();
+ if (!coinVisualization || !coinVisualization->getCoinVisualization())
+ {
+ ARMARX_INFO << "could not get coin visualization" << flush;
+ }
}
+
+ return coinVisualization;
}
- return nodeSetPtr;
-}
+ VirtualRobot::RobotNodeSetPtr
+ KinematicUnitWidgetController::getRobotNodeSet(VirtualRobot::RobotPtr robot,
+ std::string nodeSetName)
+ {
+ VirtualRobot::RobotNodeSetPtr nodeSetPtr;
+ if (robot)
+ {
+ nodeSetPtr = robot->getRobotNodeSet(nodeSetName);
-bool KinematicUnitWidgetController::initGUIComboBox(VirtualRobot::RobotNodeSetPtr robotNodeSet)
-{
- ui.nodeListComboBox->clear();
+ if (!nodeSetPtr)
+ {
+ ARMARX_INFO << "RobotNodeSet with name " << nodeSetName << " is not defined"
+ << flush;
+ }
+ }
- if (robotNodeSet)
+ return nodeSetPtr;
+ }
+
+ bool
+ KinematicUnitWidgetController::initGUIComboBox(VirtualRobot::RobotNodeSetPtr robotNodeSet)
{
- std::vector< VirtualRobot::RobotNodePtr > rn = robotNodeSet->getAllRobotNodes();
+ ui.nodeListComboBox->clear();
- for (unsigned int i = 0; i < rn.size(); i++)
+ if (robotNodeSet)
{
- // ARMARX_INFO << "adding item to joint combo box" << rn[i]->getName() << flush;
- QString name(rn[i]->getName().c_str());
- ui.nodeListComboBox->addItem(name);
+ std::vector<VirtualRobot::RobotNodePtr> rn = robotNodeSet->getAllRobotNodes();
+
+ for (unsigned int i = 0; i < rn.size(); i++)
+ {
+ // ARMARX_INFO << "adding item to joint combo box" << rn[i]->getName() << flush;
+ QString name(rn[i]->getName().c_str());
+ ui.nodeListComboBox->addItem(name);
+ }
+ ui.nodeListComboBox->setCurrentIndex(-1);
+ return true;
}
- ui.nodeListComboBox->setCurrentIndex(-1);
- return true;
+ return false;
}
- return false;
-}
-
-bool KinematicUnitWidgetController::initGUIJointListTable(VirtualRobot::RobotNodeSetPtr robotNodeSet)
-{
- uint numberOfColumns = 10;
+ bool
+ KinematicUnitWidgetController::initGUIJointListTable(VirtualRobot::RobotNodeSetPtr robotNodeSet)
+ {
+ uint numberOfColumns = 10;
- //dont use clear! It is not required here and somehow causes the tabel to have
- //numberOfColumns additional empty columns and rn.size() additional empty rows.
- //Somehow columncount (rowcount) stay at numberOfColumns (rn.size())
- //ui.tableJointList->clear();
+ //dont use clear! It is not required here and somehow causes the tabel to have
+ //numberOfColumns additional empty columns and rn.size() additional empty rows.
+ //Somehow columncount (rowcount) stay at numberOfColumns (rn.size())
+ //ui.tableJointList->clear();
- if (robotNodeSet)
- {
- std::vector< VirtualRobot::RobotNodePtr > rn = robotNodeSet->getAllRobotNodes();
-
- //set dimension of table
- //ui.tableJointList->setColumnWidth(0,110);
-
- //ui.tableJointList->horizontalHeader()->setResizeMode(QHeaderView::ResizeToContents);
- ui.tableJointList->setRowCount(rn.size());
- ui.tableJointList->setColumnCount(eTabelColumnCount);
-
-
- //ui.tableJointList->setSizePolicy(QSizePolicy::Expanding,QSizePolicy::Expanding);
-
- // set table header
- // if the order is changed dont forget to update the order in the enum JointTabelColumnIndex
- // in theheader file
- QStringList s;
- s << "Joint Name"
- << "Control Mode"
- << "Angle [deg]/Position [mm]"
- << "Velocity [deg/s]/[mm/s]"
- << "Torque [Nm] / PWM"
- << "Current [A]"
- << "Temperature [C]"
- << "Operation"
- << "Error"
- << "Enabled"
- << "Emergency Stop";
- ui.tableJointList->setHorizontalHeaderLabels(s);
- ARMARX_CHECK_EXPRESSION(ui.tableJointList->columnCount() == eTabelColumnCount) << "Current table size: " << ui.tableJointList->columnCount();
-
-
- // fill in joint names
- for (unsigned int i = 0; i < rn.size(); i++)
+ if (robotNodeSet)
{
- // ARMARX_INFO << "adding item to joint table" << rn[i]->getName() << flush;
- QString name(rn[i]->getName().c_str());
+ std::vector<VirtualRobot::RobotNodePtr> rn = robotNodeSet->getAllRobotNodes();
+
+ //set dimension of table
+ //ui.tableJointList->setColumnWidth(0,110);
+
+ //ui.tableJointList->horizontalHeader()->setResizeMode(QHeaderView::ResizeToContents);
+ ui.tableJointList->setRowCount(rn.size());
+ ui.tableJointList->setColumnCount(eTabelColumnCount);
+
+
+ //ui.tableJointList->setSizePolicy(QSizePolicy::Expanding,QSizePolicy::Expanding);
+
+ // set table header
+ // if the order is changed dont forget to update the order in the enum JointTabelColumnIndex
+ // in theheader file
+ QStringList s;
+ s << "Joint Name"
+ << "Control Mode"
+ << "Angle [deg]/Position [mm]"
+ << "Velocity [deg/s]/[mm/s]"
+ << "Torque [Nm] / PWM"
+ << "Current [A]"
+ << "Temperature [C]"
+ << "Operation"
+ << "Error"
+ << "Enabled"
+ << "Emergency Stop";
+ ui.tableJointList->setHorizontalHeaderLabels(s);
+ ARMARX_CHECK_EXPRESSION(ui.tableJointList->columnCount() == eTabelColumnCount)
+ << "Current table size: " << ui.tableJointList->columnCount();
+
+
+ // fill in joint names
+ for (unsigned int i = 0; i < rn.size(); i++)
+ {
+ // ARMARX_INFO << "adding item to joint table" << rn[i]->getName() << flush;
+ QString name(rn[i]->getName().c_str());
- QTableWidgetItem* newItem = new QTableWidgetItem(name);
- ui.tableJointList->setItem(i, eTabelColumnName, newItem);
- }
+ QTableWidgetItem* newItem = new QTableWidgetItem(name);
+ ui.tableJointList->setItem(i, eTabelColumnName, newItem);
+ }
- // init missing table fields with default values
- for (unsigned int i = 0; i < rn.size(); i++)
- {
- for (unsigned int j = 1; j < numberOfColumns; j++)
+ // init missing table fields with default values
+ for (unsigned int i = 0; i < rn.size(); i++)
{
- QString state = "--";
- QTableWidgetItem* newItem = new QTableWidgetItem(state);
- ui.tableJointList->setItem(i, j, newItem);
+ for (unsigned int j = 1; j < numberOfColumns; j++)
+ {
+ QString state = "--";
+ QTableWidgetItem* newItem = new QTableWidgetItem(state);
+ ui.tableJointList->setItem(i, j, newItem);
+ }
}
- }
- //hide columns Operation, Error, Enabled and Emergency Stop
- //they will be shown when changes occur
- ui.tableJointList->setColumnHidden(eTabelColumnTemperature, true);
- ui.tableJointList->setColumnHidden(eTabelColumnOperation, true);
- ui.tableJointList->setColumnHidden(eTabelColumnError, true);
- ui.tableJointList->setColumnHidden(eTabelColumnEnabled, true);
- ui.tableJointList->setColumnHidden(eTabelColumnEmergencyStop, true);
+ //hide columns Operation, Error, Enabled and Emergency Stop
+ //they will be shown when changes occur
+ ui.tableJointList->setColumnHidden(eTabelColumnTemperature, true);
+ ui.tableJointList->setColumnHidden(eTabelColumnOperation, true);
+ ui.tableJointList->setColumnHidden(eTabelColumnError, true);
+ ui.tableJointList->setColumnHidden(eTabelColumnEnabled, true);
+ ui.tableJointList->setColumnHidden(eTabelColumnEmergencyStop, true);
- return true;
+ return true;
+ }
+
+ return false;
}
- return false;
-}
+ void
+ KinematicUnitWidgetController::selectJoint(int i)
+ {
+ std::unique_lock lock(mutexNodeSet);
+ ARMARX_INFO << "Selected index: " << ui.nodeListComboBox->currentIndex();
-void KinematicUnitWidgetController::selectJoint(int i)
-{
- std::unique_lock lock(mutexNodeSet);
+ if (!robotNodeSet || i < 0 || i >= static_cast<int>(robotNodeSet->getSize()))
+ {
+ return;
+ }
- ARMARX_INFO << "Selected index: " << ui.nodeListComboBox->currentIndex();
+ currentNode = robotNodeSet->getAllRobotNodes()[i];
+ ARMARX_IMPORTANT << "Selected joint is `" << currentNode->getName() << "`.";
- if (!robotNodeSet || i < 0 || i >= static_cast<int>(robotNodeSet->getSize()))
- {
- return;
- }
+ const auto controlModes = kinematicUnitInterfacePrx->getControlModes();
+ if (controlModes.count(currentNode->getName()) == 0)
+ {
+ ARMARX_ERROR << "Could not retrieve control mode for joint `" << currentNode->getName()
+ << "` from kinematic unit!";
+ return;
+ }
- currentNode = robotNodeSet->getAllRobotNodes()[i];
- ARMARX_IMPORTANT << "Selected joint is `" << currentNode->getName() << "`.";
+ const auto controlMode = controlModes.at(currentNode->getName());
+ setControlModeRadioButtonGroup(controlMode);
- const auto controlModes = kinematicUnitInterfacePrx->getControlModes();
- if(controlModes.count(currentNode->getName()) == 0)
- {
- ARMARX_ERROR << "Could not retrieve control mode for joint `" << currentNode->getName() << "` from kinematic unit!";
- return;
+ if (controlMode == ePositionControl)
+ {
+ setControlModePosition();
+ }
+ else if (controlMode == eVelocityControl)
+ {
+ setControlModeVelocity();
+ ui.horizontalSliderKinematicUnitPos->setSliderPosition(SLIDER_ZERO_POSITION);
+ }
+ else if (controlMode == eTorqueControl)
+ {
+ setControlModeTorque();
+ ui.horizontalSliderKinematicUnitPos->setSliderPosition(SLIDER_ZERO_POSITION);
+ }
}
- const auto controlMode = controlModes.at(currentNode->getName());
- setControlModeRadioButtonGroup(controlMode);
-
- if (controlMode == ePositionControl)
+ void
+ KinematicUnitWidgetController::selectJointFromTableWidget(int row, int column)
{
- setControlModePosition();
- }
- else if (controlMode == eVelocityControl)
- {
- setControlModeVelocity();
- ui.horizontalSliderKinematicUnitPos->setSliderPosition(SLIDER_ZERO_POSITION);
- }
- else if (controlMode == eTorqueControl)
- {
- setControlModeTorque();
- ui.horizontalSliderKinematicUnitPos->setSliderPosition(SLIDER_ZERO_POSITION);
+ if (column == eTabelColumnName)
+ {
+ ui.nodeListComboBox->setCurrentIndex(row);
+ // selectJoint(row);
+ }
}
-}
-void KinematicUnitWidgetController::selectJointFromTableWidget(int row, int column)
-{
- if (column == eTabelColumnName)
+ void
+ KinematicUnitWidgetController::sliderValueChanged(int pos)
{
- ui.nodeListComboBox->setCurrentIndex(row);
- // selectJoint(row);
- }
-}
-
-void KinematicUnitWidgetController::sliderValueChanged(int pos)
-{
- std::unique_lock lock(mutexNodeSet);
+ std::unique_lock lock(mutexNodeSet);
- if (!currentNode)
- {
- return;
- }
+ if (!currentNode)
+ {
+ return;
+ }
- const float value = static_cast<float>(ui.horizontalSliderKinematicUnitPos->value());
+ const float value = static_cast<float>(ui.horizontalSliderKinematicUnitPos->value());
- const ControlMode currentControlMode = getSelectedControlMode();
+ const ControlMode currentControlMode = getSelectedControlMode();
- const bool isDeg = ui.checkBoxUseDegree->isChecked();
- const bool isRot = currentNode->isRotationalJoint();
+ const bool isDeg = ui.checkBoxUseDegree->isChecked();
+ const bool isRot = currentNode->isRotationalJoint();
- if (currentControlMode == ePositionControl)
- {
- const auto factor = isRot && ! isDeg ? SLIDER_POS_RAD_MULTIPLIER : SLIDER_POS_DEG_MULTIPLIER;
- float conversionFactor = isRot && isDeg ? 180.0 / M_PI : 1.0f;
+ if (currentControlMode == ePositionControl)
+ {
+ const auto factor =
+ isRot && !isDeg ? SLIDER_POS_RAD_MULTIPLIER : SLIDER_POS_DEG_MULTIPLIER;
+ float conversionFactor = isRot && isDeg ? 180.0 / M_PI : 1.0f;
- NameValueMap jointAngles;
+ NameValueMap jointAngles;
- jointAngles[currentNode->getName()] = value / conversionFactor / factor;
- ui.lcdNumberKinematicUnitJointValue->display(value / factor);
- if (kinematicUnitInterfacePrx)
- {
- try
- {
- kinematicUnitInterfacePrx->setJointAngles(jointAngles);
- }
- catch (...)
+ jointAngles[currentNode->getName()] = value / conversionFactor / factor;
+ ui.lcdNumberKinematicUnitJointValue->display(value / factor);
+ if (kinematicUnitInterfacePrx)
{
+ try
+ {
+ kinematicUnitInterfacePrx->setJointAngles(jointAngles);
+ }
+ catch (...)
+ {
+ }
}
}
- }
- else if (currentControlMode == eVelocityControl)
- {
- float conversionFactor = isRot ? (isDeg ? 180.0 / M_PI : 100.f) : 1.0f;
- NameValueMap jointVelocities;
- jointVelocities[currentNode->getName()] = value / conversionFactor * static_cast<float>(ui.doubleSpinBoxKinematicUnitPosFactor->value());
- ui.lcdNumberKinematicUnitJointValue->display(value);
-
- if (kinematicUnitInterfacePrx)
+ else if (currentControlMode == eVelocityControl)
{
- try
- {
- kinematicUnitInterfacePrx->setJointVelocities(jointVelocities);
- }
- catch (...)
- {
- }
- }
- }
- else if (currentControlMode == eTorqueControl)
- {
- NameValueMap jointTorques;
- float torqueTargetValue = value / 100.0f * static_cast<float>(ui.doubleSpinBoxKinematicUnitPosFactor->value());
- jointTorques[currentNode->getName()] = torqueTargetValue;
- ui.lcdNumberKinematicUnitJointValue->display(torqueTargetValue);
+ float conversionFactor = isRot ? (isDeg ? 180.0 / M_PI : 100.f) : 1.0f;
+ NameValueMap jointVelocities;
+ jointVelocities[currentNode->getName()] =
+ value / conversionFactor *
+ static_cast<float>(ui.doubleSpinBoxKinematicUnitPosFactor->value());
+ ui.lcdNumberKinematicUnitJointValue->display(value);
- if (kinematicUnitInterfacePrx)
- {
- try
+ if (kinematicUnitInterfacePrx)
{
- kinematicUnitInterfacePrx->setJointTorques(jointTorques);
+ try
+ {
+ kinematicUnitInterfacePrx->setJointVelocities(jointVelocities);
+ }
+ catch (...)
+ {
+ }
}
- catch (...)
+ }
+ else if (currentControlMode == eTorqueControl)
+ {
+ NameValueMap jointTorques;
+ float torqueTargetValue =
+ value / 100.0f *
+ static_cast<float>(ui.doubleSpinBoxKinematicUnitPosFactor->value());
+ jointTorques[currentNode->getName()] = torqueTargetValue;
+ ui.lcdNumberKinematicUnitJointValue->display(torqueTargetValue);
+
+ if (kinematicUnitInterfacePrx)
{
+ try
+ {
+ kinematicUnitInterfacePrx->setJointTorques(jointTorques);
+ }
+ catch (...)
+ {
+ }
}
}
+ else
+ {
+ ARMARX_INFO << "current ControlModes unknown" << flush;
+ }
}
- else
- {
- ARMARX_INFO << "current ControlModes unknown" << flush;
- }
-}
-
-
-
-void KinematicUnitWidgetController::updateControlModesTable(const NameControlModeMap& reportedJointControlModes)
-{
- if (!getWidget() || !robotNodeSet)
- {
- return;
- }
-
- std::unique_lock lock(mutexNodeSet);
- std::vector< VirtualRobot::RobotNodePtr > rn = robotNodeSet->getAllRobotNodes();
- for (unsigned int i = 0; i < rn.size(); i++)
+ void
+ KinematicUnitWidgetController::updateControlModesTable(
+ const NameControlModeMap& reportedJointControlModes)
{
- NameControlModeMap::const_iterator it;
- it = reportedJointControlModes.find(rn[i]->getName());
- QString state;
-
- if (it == reportedJointControlModes.end())
+ if (!getWidget() || !robotNodeSet)
{
- state = "unknown";
+ return;
}
- else
- {
- ControlMode currentMode = it->second;
+ std::unique_lock lock(mutexNodeSet);
+ std::vector<VirtualRobot::RobotNodePtr> rn = robotNodeSet->getAllRobotNodes();
+
+ for (unsigned int i = 0; i < rn.size(); i++)
+ {
+ NameControlModeMap::const_iterator it;
+ it = reportedJointControlModes.find(rn[i]->getName());
+ QString state;
- switch (currentMode)
+ if (it == reportedJointControlModes.end())
+ {
+ state = "unknown";
+ }
+ else
{
- /*case eNoMode:
+ ControlMode currentMode = it->second;
+
+
+ switch (currentMode)
+ {
+ /*case eNoMode:
state = "None";
break;
@@ -1111,531 +1211,563 @@ void KinematicUnitWidgetController::updateControlModesTable(const NameControlMod
state = "Unknown";
break;
*/
- case eDisabled:
- state = "Disabled";
- break;
+ case eDisabled:
+ state = "Disabled";
+ break;
- case eUnknown:
- state = "Unknown";
- break;
+ case eUnknown:
+ state = "Unknown";
+ break;
- case ePositionControl:
- state = "Position";
- break;
+ case ePositionControl:
+ state = "Position";
+ break;
- case eVelocityControl:
- state = "Velocity";
- break;
+ case eVelocityControl:
+ state = "Velocity";
+ break;
- case eTorqueControl:
- state = "Torque";
- break;
+ case eTorqueControl:
+ state = "Torque";
+ break;
- case ePositionVelocityControl:
- state = "Position + Velocity";
- break;
+ case ePositionVelocityControl:
+ state = "Position + Velocity";
+ break;
- default:
- //show the value of the mode so it can be implemented
- state = QString("<nyi Mode: %1>").arg(static_cast<int>(currentMode));
- break;
+ default:
+ //show the value of the mode so it can be implemented
+ state = QString("<nyi Mode: %1>").arg(static_cast<int>(currentMode));
+ break;
+ }
}
- }
- QTableWidgetItem* newItem = new QTableWidgetItem(state);
- ui.tableJointList->setItem(i, eTabelColumnControlMode, newItem);
+ QTableWidgetItem* newItem = new QTableWidgetItem(state);
+ ui.tableJointList->setItem(i, eTabelColumnControlMode, newItem);
+ }
}
-}
-void KinematicUnitWidgetController::updateJointStatusesTable(const NameStatusMap& reportedJointStatuses)
-{
- if (!getWidget() || !robotNodeSet || reportedJointStatuses.empty())
+ void
+ KinematicUnitWidgetController::updateJointStatusesTable(
+ const NameStatusMap& reportedJointStatuses)
{
- return;
- }
+ if (!getWidget() || !robotNodeSet || reportedJointStatuses.empty())
+ {
+ return;
+ }
- std::unique_lock lock(mutexNodeSet);
- std::vector< VirtualRobot::RobotNodePtr > rn = robotNodeSet->getAllRobotNodes();
+ std::unique_lock lock(mutexNodeSet);
+ std::vector<VirtualRobot::RobotNodePtr> rn = robotNodeSet->getAllRobotNodes();
- for (unsigned int i = 0; i < rn.size(); i++)
- {
- NameStatusMap::const_iterator it;
- it = reportedJointStatuses.find(rn[i]->getName());
- JointStatus currentStatus = it->second;
+ for (unsigned int i = 0; i < rn.size(); i++)
+ {
- QString state = translateStatus(currentStatus.operation);
- QTableWidgetItem* newItem = new QTableWidgetItem(state);
- ui.tableJointList->setItem(i, eTabelColumnOperation, newItem);
+ auto it = reportedJointStatuses.find(rn[i]->getName());
+ if (it == reportedJointStatuses.end())
+ {
+ ARMARX_WARNING << deactivateSpam(5) << "Joint Status for " << rn[i]->getName()
+ << " was not reported!";
+ continue;
+ }
+ JointStatus currentStatus = it->second;
- state = translateStatus(currentStatus.error);
- newItem = new QTableWidgetItem(state);
- ui.tableJointList->setItem(i, eTabelColumnError, newItem);
+ QString state = translateStatus(currentStatus.operation);
+ QTableWidgetItem* newItem = new QTableWidgetItem(state);
+ ui.tableJointList->setItem(i, eTabelColumnOperation, newItem);
- state = currentStatus.enabled ? "X" : "-";
- newItem = new QTableWidgetItem(state);
- ui.tableJointList->setItem(i, eTabelColumnEnabled, newItem);
+ state = translateStatus(currentStatus.error);
+ newItem = new QTableWidgetItem(state);
+ ui.tableJointList->setItem(i, eTabelColumnError, newItem);
- state = currentStatus.emergencyStop ? "X" : "-";
- newItem = new QTableWidgetItem(state);
- ui.tableJointList->setItem(i, eTabelColumnEmergencyStop, newItem);
- }
+ state = currentStatus.enabled ? "X" : "-";
+ newItem = new QTableWidgetItem(state);
+ ui.tableJointList->setItem(i, eTabelColumnEnabled, newItem);
- //show columns
- ui.tableJointList->setColumnHidden(eTabelColumnOperation, false);
- ui.tableJointList->setColumnHidden(eTabelColumnError, false);
- ui.tableJointList->setColumnHidden(eTabelColumnEnabled, false);
- ui.tableJointList->setColumnHidden(eTabelColumnEmergencyStop, false);
-}
+ state = currentStatus.emergencyStop ? "X" : "-";
+ newItem = new QTableWidgetItem(state);
+ ui.tableJointList->setItem(i, eTabelColumnEmergencyStop, newItem);
+ }
-QString KinematicUnitWidgetController::translateStatus(OperationStatus status)
-{
- switch (status)
+ //show columns
+ ui.tableJointList->setColumnHidden(eTabelColumnOperation, false);
+ ui.tableJointList->setColumnHidden(eTabelColumnError, false);
+ ui.tableJointList->setColumnHidden(eTabelColumnEnabled, false);
+ ui.tableJointList->setColumnHidden(eTabelColumnEmergencyStop, false);
+ }
+
+ QString
+ KinematicUnitWidgetController::translateStatus(OperationStatus status)
{
- case eOffline:
- return "Offline";
+ switch (status)
+ {
+ case eOffline:
+ return "Offline";
- case eOnline:
- return "Online";
+ case eOnline:
+ return "Online";
- case eInitialized:
- return "Initialized";
+ case eInitialized:
+ return "Initialized";
- default:
- return "?";
+ default:
+ return "?";
+ }
}
-}
-QString KinematicUnitWidgetController::translateStatus(ErrorStatus status)
-{
- switch (status)
+ QString
+ KinematicUnitWidgetController::translateStatus(ErrorStatus status)
{
- case eOk:
- return "Ok";
-
- case eWarning:
- return "Wr";
-
- case eError:
- return "Er";
+ switch (status)
+ {
+ case eOk:
+ return "Ok";
- default:
- return "?";
- }
-}
+ case eWarning:
+ return "Wr";
-void KinematicUnitWidgetController::updateJointAnglesTable(const NameValueMap& reportedJointAngles)
-{
- std::unique_lock lock(mutexNodeSet);
+ case eError:
+ return "Er";
- if (!robotNodeSet)
- {
- return;
+ default:
+ return "?";
+ }
}
- std::vector< VirtualRobot::RobotNodePtr > rn = robotNodeSet->getAllRobotNodes();
-
- for (unsigned int i = 0; i < rn.size(); i++)
+ void
+ KinematicUnitWidgetController::updateJointAnglesTable(const NameValueMap& reportedJointAngles)
{
- NameValueMap::const_iterator it;
- VirtualRobot::RobotNodePtr node = rn[i];
- it = reportedJointAngles.find(node->getName());
+ std::unique_lock lock(mutexNodeSet);
- if (it == reportedJointAngles.end())
+ if (!robotNodeSet)
{
- continue;
+ return;
}
+ std::vector<VirtualRobot::RobotNodePtr> rn = robotNodeSet->getAllRobotNodes();
- const float currentValue = it->second;
- QModelIndex index = ui.tableJointList->model()->index(i, eTabelColumnAngleProgressbar);
- float conversionFactor = ui.checkBoxUseDegree->isChecked() &&
- node->isRotationalJoint() ? 180.0 / M_PI : 1;
- ui.tableJointList->model()->setData(index, (int)(cutJitter(currentValue * conversionFactor) * 100) / 100.0f, eJointAngleRole);
- ui.tableJointList->model()->setData(index, node->getJointLimitHigh() * conversionFactor, eJointHiRole);
- ui.tableJointList->model()->setData(index, node->getJointLimitLow() * conversionFactor, eJointLoRole);
- }
-}
+ for (unsigned int i = 0; i < rn.size(); i++)
+ {
+ NameValueMap::const_iterator it;
+ VirtualRobot::RobotNodePtr node = rn[i];
+ it = reportedJointAngles.find(node->getName());
-void KinematicUnitWidgetController::updateJointVelocitiesTable(const NameValueMap& reportedJointVelocities)
-{
- if (!getWidget())
- {
- return;
- }
+ if (it == reportedJointAngles.end())
+ {
+ continue;
+ }
- std::unique_lock lock(mutexNodeSet);
- if (!robotNodeSet)
- {
- return;
+ const float currentValue = it->second;
+
+ QModelIndex index = ui.tableJointList->model()->index(i, eTabelColumnAngleProgressbar);
+ float conversionFactor =
+ ui.checkBoxUseDegree->isChecked() && node->isRotationalJoint() ? 180.0 / M_PI : 1;
+ ui.tableJointList->model()->setData(
+ index,
+ (int)(cutJitter(currentValue * conversionFactor) * 100) / 100.0f,
+ eJointAngleRole);
+ ui.tableJointList->model()->setData(
+ index, node->getJointLimitHigh() * conversionFactor, eJointHiRole);
+ ui.tableJointList->model()->setData(
+ index, node->getJointLimitLow() * conversionFactor, eJointLoRole);
+ }
}
- std::vector< VirtualRobot::RobotNodePtr > rn = robotNodeSet->getAllRobotNodes();
- QTableWidgetItem* newItem;
- for (unsigned int i = 0; i < rn.size(); i++)
+ void
+ KinematicUnitWidgetController::updateJointVelocitiesTable(
+ const NameValueMap& reportedJointVelocities)
{
- NameValueMap::const_iterator it;
- it = reportedJointVelocities.find(rn[i]->getName());
-
- if (it == reportedJointVelocities.end())
+ if (!getWidget())
{
- continue;
+ return;
}
- float currentValue = it->second;
- if (ui.checkBoxUseDegree->isChecked() && rn[i]->isRotationalJoint())
+ std::unique_lock lock(mutexNodeSet);
+ if (!robotNodeSet)
{
- currentValue *= 180.0 / M_PI;
+ return;
}
- const QString Text = QString::number(cutJitter(currentValue), 'g', 2);
- newItem = new QTableWidgetItem(Text);
- ui.tableJointList->setItem(i, eTabelColumnVelocity, newItem);
- }
-}
+ std::vector<VirtualRobot::RobotNodePtr> rn = robotNodeSet->getAllRobotNodes();
+ QTableWidgetItem* newItem;
-void KinematicUnitWidgetController::updateJointTorquesTable(const NameValueMap& reportedJointTorques)
-{
+ for (unsigned int i = 0; i < rn.size(); i++)
+ {
+ NameValueMap::const_iterator it;
+ it = reportedJointVelocities.find(rn[i]->getName());
+ if (it == reportedJointVelocities.end())
+ {
+ continue;
+ }
- std::unique_lock lock(mutexNodeSet);
- if (!getWidget() || !robotNodeSet)
- {
- return;
+ float currentValue = it->second;
+ if (ui.checkBoxUseDegree->isChecked() && rn[i]->isRotationalJoint())
+ {
+ currentValue *= 180.0 / M_PI;
+ }
+ const QString Text = QString::number(cutJitter(currentValue), 'g', 2);
+ newItem = new QTableWidgetItem(Text);
+ ui.tableJointList->setItem(i, eTabelColumnVelocity, newItem);
+ }
}
- std::vector< VirtualRobot::RobotNodePtr > rn = robotNodeSet->getAllRobotNodes();
- QTableWidgetItem* newItem;
- NameValueMap::const_iterator it;
- for (unsigned int i = 0; i < rn.size(); i++)
+ void
+ KinematicUnitWidgetController::updateJointTorquesTable(const NameValueMap& reportedJointTorques)
{
- it = reportedJointTorques.find(rn[i]->getName());
- if (it == reportedJointTorques.end())
+
+ std::unique_lock lock(mutexNodeSet);
+ if (!getWidget() || !robotNodeSet)
{
- continue;
+ return;
}
+ std::vector<VirtualRobot::RobotNodePtr> rn = robotNodeSet->getAllRobotNodes();
+ QTableWidgetItem* newItem;
+ NameValueMap::const_iterator it;
- const float currentValue = it->second;
- newItem = new QTableWidgetItem(QString::number(cutJitter(currentValue)));
- ui.tableJointList->setItem(i, eTabelColumnTorque, newItem);
- }
-}
-
-void KinematicUnitWidgetController::updateJointCurrentsTable(const NameValueMap& reportedJointCurrents, const NameStatusMap& reportedJointStatuses)
-{
+ for (unsigned int i = 0; i < rn.size(); i++)
+ {
+ it = reportedJointTorques.find(rn[i]->getName());
+ if (it == reportedJointTorques.end())
+ {
+ continue;
+ }
- std::unique_lock lock(mutexNodeSet);
- if (!getWidget() || !robotNodeSet || jointCurrentHistory.size() == 0)
- {
- return;
+ const float currentValue = it->second;
+ newItem = new QTableWidgetItem(QString::number(cutJitter(currentValue)));
+ ui.tableJointList->setItem(i, eTabelColumnTorque, newItem);
+ }
}
- std::vector< VirtualRobot::RobotNodePtr > rn = robotNodeSet->getAllRobotNodes();
- QTableWidgetItem* newItem;
- // FIXME history!
- // NameValueMap reportedJointCurrents = jointCurrentHistory.back().second;
- NameValueMap::const_iterator it;
-
- for (unsigned int i = 0; i < rn.size(); i++)
+ void
+ KinematicUnitWidgetController::updateJointCurrentsTable(
+ const NameValueMap& reportedJointCurrents,
+ const NameStatusMap& reportedJointStatuses)
{
- it = reportedJointCurrents.find(rn[i]->getName());
- if (it == reportedJointCurrents.end())
+
+ std::unique_lock lock(mutexNodeSet);
+ if (!getWidget() || !robotNodeSet || jointCurrentHistory.size() == 0)
{
- continue;
+ return;
}
+ std::vector<VirtualRobot::RobotNodePtr> rn = robotNodeSet->getAllRobotNodes();
+ QTableWidgetItem* newItem;
- const float currentValue = it->second;
- newItem = new QTableWidgetItem(QString::number(cutJitter(currentValue)));
- ui.tableJointList->setItem(i, eTabelColumnCurrent, newItem);
- }
+ // FIXME history!
+ // NameValueMap reportedJointCurrents = jointCurrentHistory.back().second;
+ NameValueMap::const_iterator it;
- highlightCriticalValues(reportedJointStatuses);
-}
+ for (unsigned int i = 0; i < rn.size(); i++)
+ {
+ it = reportedJointCurrents.find(rn[i]->getName());
-void KinematicUnitWidgetController::updateMotorTemperaturesTable(const NameValueMap& reportedJointTemperatures)
-{
+ if (it == reportedJointCurrents.end())
+ {
+ continue;
+ }
+ const float currentValue = it->second;
+ newItem = new QTableWidgetItem(QString::number(cutJitter(currentValue)));
+ ui.tableJointList->setItem(i, eTabelColumnCurrent, newItem);
+ }
- std::unique_lock lock(mutexNodeSet);
- if (!getWidget() || !robotNodeSet || reportedJointTemperatures.empty())
- {
- return;
+ highlightCriticalValues(reportedJointStatuses);
}
- std::vector< VirtualRobot::RobotNodePtr > rn = robotNodeSet->getAllRobotNodes();
- QTableWidgetItem* newItem;
- NameValueMap::const_iterator it;
- for (unsigned int i = 0; i < rn.size(); i++)
+ void
+ KinematicUnitWidgetController::updateMotorTemperaturesTable(
+ const NameValueMap& reportedJointTemperatures)
{
- it = reportedJointTemperatures.find(rn[i]->getName());
- if (it == reportedJointTemperatures.end())
+
+ std::unique_lock lock(mutexNodeSet);
+ if (!getWidget() || !robotNodeSet || reportedJointTemperatures.empty())
{
- continue;
+ return;
}
+ std::vector<VirtualRobot::RobotNodePtr> rn = robotNodeSet->getAllRobotNodes();
+ QTableWidgetItem* newItem;
+ NameValueMap::const_iterator it;
- const float currentValue = it->second;
- newItem = new QTableWidgetItem(QString::number(cutJitter(currentValue)));
- ui.tableJointList->setItem(i, eTabelColumnTemperature, newItem);
- }
- ui.tableJointList->setColumnHidden(eTabelColumnTemperature, false);
-
-}
+ for (unsigned int i = 0; i < rn.size(); i++)
+ {
+ it = reportedJointTemperatures.find(rn[i]->getName());
+ if (it == reportedJointTemperatures.end())
+ {
+ continue;
+ }
-void KinematicUnitWidgetController::updateModel(const NameValueMap& reportedJointAngles)
-{
- // ARMARX_INFO << "updateModel()" << flush;
- std::unique_lock lock(mutexNodeSet);
- if (!robotNodeSet)
- {
- return;
+ const float currentValue = it->second;
+ newItem = new QTableWidgetItem(QString::number(cutJitter(currentValue)));
+ ui.tableJointList->setItem(i, eTabelColumnTemperature, newItem);
+ }
+ ui.tableJointList->setColumnHidden(eTabelColumnTemperature, false);
}
- robot->setJointValues(reportedJointAngles);
-}
-std::optional<float> mean(const boost::circular_buffer<NameValueMap>& buffer, const std::string& key)
-{
- float sum = 0;
- std::size_t count = 0;
-
- for(const auto& element: buffer)
+ void
+ KinematicUnitWidgetController::updateModel(const NameValueMap& reportedJointAngles)
{
- if(element.count(key) > 0)
+ // ARMARX_INFO << "updateModel()" << flush;
+ std::unique_lock lock(mutexNodeSet);
+ if (!robotNodeSet)
{
- sum += element.at(key);
+ return;
}
+ robot->setJointValues(reportedJointAngles);
}
- if(count == 0)
+ std::optional<float>
+ mean(const boost::circular_buffer<NameValueMap>& buffer, const std::string& key)
{
- return std::nullopt;
- }
-
- return sum / static_cast<float>(count);
-}
-
-void KinematicUnitWidgetController::highlightCriticalValues(const NameStatusMap& reportedJointStatuses)
-{
- if (!enableValueValidator)
- {
- return;
- }
+ float sum = 0;
+ std::size_t count = 0;
- std::unique_lock lock(mutexNodeSet);
-
- std::vector< VirtualRobot::RobotNodePtr > rn = robotNodeSet->getAllRobotNodes();
+ for (const auto& element : buffer)
+ {
+ if (element.count(key) > 0)
+ {
+ sum += element.at(key);
+ }
+ }
- // get standard line colors
- static std::vector<QBrush> standardColors;
- if (standardColors.size() == 0)
- {
- for (unsigned int i = 0; i < rn.size(); i++)
+ if (count == 0)
{
- // all cells of a row have the same color
- standardColors.push_back(ui.tableJointList->item(i, eTabelColumnCurrent)->background());
+ return std::nullopt;
}
+
+ return sum / static_cast<float>(count);
}
- // check robot current value of nodes
- for (unsigned int i = 0; i < rn.size(); i++)
+ void
+ KinematicUnitWidgetController::highlightCriticalValues(
+ const NameStatusMap& reportedJointStatuses)
{
- const auto& jointName = rn[i]->getName();
-
- const auto currentSmoothValOpt = mean(jointCurrentHistory, jointName);
- if(not currentSmoothValOpt.has_value())
+ if (!enableValueValidator)
{
- continue;
+ return;
}
- const float smoothValue = std::fabs(currentSmoothValOpt.value());
+ std::unique_lock lock(mutexNodeSet);
+
+ std::vector<VirtualRobot::RobotNodePtr> rn = robotNodeSet->getAllRobotNodes();
- if(jointCurrentHistory.front().count(jointName) == 0)
+ // get standard line colors
+ static std::vector<QBrush> standardColors;
+ if (standardColors.size() == 0)
{
- continue;
+ for (unsigned int i = 0; i < rn.size(); i++)
+ {
+ // all cells of a row have the same color
+ standardColors.push_back(
+ ui.tableJointList->item(i, eTabelColumnCurrent)->background());
+ }
}
- const float startValue = jointCurrentHistory.front().at(jointName);
- const bool isStatic = (smoothValue == startValue);
+ // check robot current value of nodes
+ for (unsigned int i = 0; i < rn.size(); i++)
+ {
+ const auto& jointName = rn[i]->getName();
+
+ const auto currentSmoothValOpt = mean(jointCurrentHistory, jointName);
+ if (not currentSmoothValOpt.has_value())
+ {
+ continue;
+ }
+
+ const float smoothValue = std::fabs(currentSmoothValOpt.value());
+
+ if (jointCurrentHistory.front().count(jointName) == 0)
+ {
+ continue;
+ }
- NameStatusMap::const_iterator it;
- it = reportedJointStatuses.find(rn[i]->getName());
- JointStatus currentStatus = it->second;
+ const float startValue = jointCurrentHistory.front().at(jointName);
+ const bool isStatic = (smoothValue == startValue);
- if (isStatic)
- {
- if (currentStatus.operation != eOffline)
+ NameStatusMap::const_iterator it;
+ it = reportedJointStatuses.find(rn[i]->getName());
+ JointStatus currentStatus = it->second;
+
+ if (isStatic)
+ {
+ if (currentStatus.operation != eOffline)
+ {
+ // current value is zero, but joint is not offline
+ ui.tableJointList->item(i, eTabelColumnCurrent)->setBackground(Qt::yellow);
+ }
+ }
+ else if (std::abs(smoothValue) > currentValueMax)
+ {
+ // current value is too high
+ ui.tableJointList->item(i, eTabelColumnCurrent)->setBackground(Qt::red);
+ }
+ else
{
- // current value is zero, but joint is not offline
- ui.tableJointList->item(i, eTabelColumnCurrent)->setBackground(Qt::yellow);
+ // everything seems to work as expected
+ ui.tableJointList->item(i, eTabelColumnCurrent)->setBackground(standardColors[i]);
}
+ }
+ }
+
+ void
+ KinematicUnitWidgetController::setMutex3D(RecursiveMutexPtr const& mutex3D)
+ {
+ this->mutex3D = mutex3D;
+ if (debugDrawer)
+ {
+ debugDrawer->setMutex(mutex3D);
}
- else if (std::abs(smoothValue) > currentValueMax)
+ }
+
+ QPointer<QWidget>
+ KinematicUnitWidgetController::getCustomTitlebarWidget(QWidget* parent)
+ {
+ if (customToolbar)
{
- // current value is too high
- ui.tableJointList->item(i, eTabelColumnCurrent)->setBackground(Qt::red);
+ customToolbar->setParent(parent);
}
else
{
- // everything seems to work as expected
- ui.tableJointList->item(i, eTabelColumnCurrent)->setBackground(standardColors[i]);
+ customToolbar = new QToolBar(parent);
+ customToolbar->addAction("ZeroVelocity", this, SLOT(kinematicUnitZeroVelocity()));
}
+ return customToolbar.data();
}
-}
-
-void KinematicUnitWidgetController::setMutex3D(RecursiveMutexPtr const& mutex3D)
-{
- this->mutex3D = mutex3D;
- if (debugDrawer)
+ float
+ KinematicUnitWidgetController::cutJitter(float value)
{
- debugDrawer->setMutex(mutex3D);
+ return (abs(value) < static_cast<float>(ui.jitterThresholdSpinBox->value())) ? 0 : value;
}
-}
-QPointer<QWidget> KinematicUnitWidgetController::getCustomTitlebarWidget(QWidget* parent)
-{
- if (customToolbar)
+ void
+ KinematicUnitWidgetController::fetchData()
{
- customToolbar->setParent(parent);
- }
- else
- {
- customToolbar = new QToolBar(parent);
- customToolbar->addAction("ZeroVelocity", this, SLOT(kinematicUnitZeroVelocity()));
- }
- return customToolbar.data();
-}
+ ARMARX_DEBUG << "updateGui";
-float KinematicUnitWidgetController::cutJitter(float value)
-{
- return (abs(value) < static_cast<float>(ui.jitterThresholdSpinBox->value())) ? 0 : value;
-}
+ if (not kinematicUnitInterfacePrx)
+ {
+ ARMARX_WARNING << "KinematicUnit is not available!";
+ return;
+ }
-void KinematicUnitWidgetController::fetchData()
-{
- ARMARX_DEBUG << "updateGui";
+ const auto debugInfo = kinematicUnitInterfacePrx->getDebugInfo();
- if(not kinematicUnitInterfacePrx)
- {
- ARMARX_WARNING << "KinematicUnit is not available!";
- return;
+ emit onDebugInfoReceived(debugInfo);
}
- const auto debugInfo = kinematicUnitInterfacePrx->getDebugInfo();
-
- emit onDebugInfoReceived(debugInfo);
-}
-
-void KinematicUnitWidgetController::debugInfoReceived(const DebugInfo& debugInfo)
-{
- ARMARX_DEBUG << "debug info received";
-
- updateModel(debugInfo.jointAngles);
+ void
+ KinematicUnitWidgetController::debugInfoReceived(const DebugInfo& debugInfo)
+ {
+ ARMARX_DEBUG << "debug info received";
- updateJointAnglesTable(debugInfo.jointAngles);
- updateJointVelocitiesTable(debugInfo.jointVelocities);
- updateJointTorquesTable(debugInfo.jointTorques);
- updateJointCurrentsTable(debugInfo.jointCurrents, debugInfo.jointStatus);
- updateControlModesTable(debugInfo.jointModes);
- updateJointStatusesTable(debugInfo.jointStatus);
- updateMotorTemperaturesTable(debugInfo.jointMotorTemperatures);
+ updateModel(debugInfo.jointAngles);
-}
+ updateJointAnglesTable(debugInfo.jointAngles);
+ updateJointVelocitiesTable(debugInfo.jointVelocities);
+ updateJointTorquesTable(debugInfo.jointTorques);
+ updateJointCurrentsTable(debugInfo.jointCurrents, debugInfo.jointStatus);
+ updateControlModesTable(debugInfo.jointModes);
+ updateJointStatusesTable(debugInfo.jointStatus);
+ updateMotorTemperaturesTable(debugInfo.jointMotorTemperatures);
+ }
-void RangeValueDelegate::paint(QPainter* painter, const QStyleOptionViewItem& option, const QModelIndex& index) const
-{
- if (index.column() == KinematicUnitWidgetController::eTabelColumnAngleProgressbar)
+ void
+ RangeValueDelegate::paint(QPainter* painter,
+ const QStyleOptionViewItem& option,
+ const QModelIndex& index) const
{
- float jointValue = index.data(KinematicUnitWidgetController:: eJointAngleRole).toFloat();
- float loDeg = index.data(KinematicUnitWidgetController::eJointLoRole).toFloat();
- float hiDeg = index.data(KinematicUnitWidgetController::eJointHiRole).toFloat();
+ if (index.column() == KinematicUnitWidgetController::eTabelColumnAngleProgressbar)
+ {
+ float jointValue = index.data(KinematicUnitWidgetController::eJointAngleRole).toFloat();
+ float loDeg = index.data(KinematicUnitWidgetController::eJointLoRole).toFloat();
+ float hiDeg = index.data(KinematicUnitWidgetController::eJointHiRole).toFloat();
+
+ if (hiDeg - loDeg <= 0)
+ {
+ QStyledItemDelegate::paint(painter, option, index);
+ return;
+ }
- if (hiDeg - loDeg <= 0)
+ QStyleOptionProgressBar progressBarOption;
+ progressBarOption.rect = option.rect;
+ progressBarOption.minimum = loDeg;
+ progressBarOption.maximum = hiDeg;
+ progressBarOption.progress = jointValue;
+ progressBarOption.text = QString::number(jointValue);
+ progressBarOption.textVisible = true;
+ QPalette pal;
+ pal.setColor(QPalette::Background, Qt::red);
+ progressBarOption.palette = pal;
+ QApplication::style()->drawControl(QStyle::CE_ProgressBar, &progressBarOption, painter);
+ }
+ else
{
QStyledItemDelegate::paint(painter, option, index);
- return;
}
-
- QStyleOptionProgressBar progressBarOption;
- progressBarOption.rect = option.rect;
- progressBarOption.minimum = loDeg;
- progressBarOption.maximum = hiDeg;
- progressBarOption.progress = jointValue;
- progressBarOption.text = QString::number(jointValue);
- progressBarOption.textVisible = true;
- QPalette pal;
- pal.setColor(QPalette::Background, Qt::red);
- progressBarOption.palette = pal;
- QApplication::style()->drawControl(QStyle::CE_ProgressBar,
- &progressBarOption, painter);
-
}
- else
+
+ KinematicUnitWidgetController::~KinematicUnitWidgetController()
{
- QStyledItemDelegate::paint(painter, option, index);
- }
-}
+ kinematicUnitInterfacePrx = nullptr;
-KinematicUnitWidgetController::~KinematicUnitWidgetController()
-{
- kinematicUnitInterfacePrx = nullptr;
+ if (updateTask)
+ {
+ updateTask->stop();
+ updateTask->join();
+ updateTask = nullptr;
+ }
+ }
- if(updateTask)
+ void
+ KinematicUnitWidgetController::on_pushButtonFromJson_clicked()
{
- updateTask->stop();
- updateTask->join();
- updateTask = nullptr;
- }
-}
+ bool ok;
+ const auto text = QInputDialog::getMultiLineText(
+ __widget, tr("JSON Joint values"), tr("Json:"), "{\n}", &ok)
+ .toStdString();
-void KinematicUnitWidgetController::on_pushButtonFromJson_clicked()
-{
- bool ok;
- const auto text = QInputDialog::getMultiLineText(
- __widget,
- tr("JSON Joint values"),
- tr("Json:"), "{\n}", &ok).toStdString();
+ if (!ok || text.empty())
+ {
+ return;
+ }
- if (!ok || text.empty())
- {
- return;
- }
+ NameValueMap jointAngles;
+ try
+ {
+ jointAngles = simox::json::json2NameValueMap(text);
+ }
+ catch (...)
+ {
+ ARMARX_ERROR << "invalid json";
+ }
- NameValueMap jointAngles;
- try
- {
- jointAngles = simox::json::json2NameValueMap(text);
- }
- catch (...)
- {
- ARMARX_ERROR << "invalid json";
- }
+ NameControlModeMap jointModes;
+ for (const auto& [key, _] : jointAngles)
+ {
+ jointModes[key] = ePositionControl;
+ }
- NameControlModeMap jointModes;
- for (const auto& [key, _] : jointAngles)
- {
- jointModes[key] = ePositionControl;
+ try
+ {
+ kinematicUnitInterfacePrx->switchControlMode(jointModes);
+ kinematicUnitInterfacePrx->setJointAngles(jointAngles);
+ }
+ catch (...)
+ {
+ ARMARX_ERROR << "failed to switch mode or set angles";
+ }
}
- try
+ void
+ KinematicUnitWidgetController::synchronizeRobotJointAngles()
{
- kinematicUnitInterfacePrx->switchControlMode(jointModes);
- kinematicUnitInterfacePrx->setJointAngles(jointAngles);
+ const NameValueMap currentJointAngles = kinematicUnitInterfacePrx->getJointAngles();
+ robot->setJointValues(currentJointAngles);
}
- catch (...)
- {
- ARMARX_ERROR << "failed to switch mode or set angles";
- }
-}
-
-void KinematicUnitWidgetController::synchronizeRobotJointAngles()
-{
- const NameValueMap currentJointAngles = kinematicUnitInterfacePrx->getJointAngles();
- robot->setJointValues(currentJointAngles);
-}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectPoseClient.cpp b/source/RobotAPI/libraries/ArmarXObjects/ObjectPoseClient.cpp
index a85ae2c2c5531450d3f9c0440e31d8c1695884ba..e577eb166ebc77bbbb66dc4ecfe47b9728b2a2be 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectPoseClient.cpp
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectPoseClient.cpp
@@ -62,7 +62,8 @@ namespace armarx::objpose
std::optional<ObjectPose>
ObjectPoseClient::fetchObjectPose(const ObjectID& objectID) const
{
- const auto *object = findObjectPoseByID(fetchObjectPoses(), objectID);
+ const auto objectPoses = fetchObjectPoses();
+ const auto *object = findObjectPoseByID(objectPoses, objectID);
if(object != nullptr)
{
diff --git a/source/RobotAPI/libraries/ArmarXObjects/test/ArmarXObjectsTest.cpp b/source/RobotAPI/libraries/ArmarXObjects/test/ArmarXObjectsTest.cpp
index 433fc67ec7890cbb7fede64d2edda22533e46eb8..4a775da39159a8e0a8435f88679368b3cf39a0dc 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/test/ArmarXObjectsTest.cpp
+++ b/source/RobotAPI/libraries/ArmarXObjects/test/ArmarXObjectsTest.cpp
@@ -24,13 +24,15 @@
#define ARMARX_BOOST_TEST
-#include <RobotAPI/Test.h>
#include "../ArmarXObjects.h"
#include <iostream>
-#include <RobotAPI/libraries/ArmarXObjects/aron/ObjectPose.aron.generated.h>
+#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
+
+#include <RobotAPI/Test.h>
#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
+#include <RobotAPI/libraries/ArmarXObjects/aron/ObjectPose.aron.generated.h>
namespace fs = std::filesystem;
@@ -38,7 +40,6 @@ namespace fs = std::filesystem;
BOOST_AUTO_TEST_SUITE(arondto_ObjectPose_test)
-
BOOST_AUTO_TEST_CASE(test_ObjectType_copy_assignment)
{
BOOST_TEST_MESSAGE("Constructor");
@@ -76,17 +77,25 @@ BOOST_AUTO_TEST_CASE(test_ObjectPose_copy_assignment)
BOOST_AUTO_TEST_SUITE_END()
-
BOOST_AUTO_TEST_SUITE(ObjectFinderTest)
-
BOOST_AUTO_TEST_CASE(test_find)
{
using namespace armarx;
+ {
+ armarx::CMakePackageFinder packageFinder(ObjectFinder::DefaultObjectsPackageName);
+ if (not packageFinder.packageFound())
+ {
+ // Do not test further.
+ BOOST_CHECK(true);
+ return;
+ }
+ }
+
ObjectFinder finder;
- bool checkPaths = false;
+ const bool checkPaths = false;
std::vector<ObjectInfo> objects = finder.findAllObjects(checkPaths);
BOOST_CHECK_GT(objects.size(), 0);
@@ -106,5 +115,4 @@ BOOST_AUTO_TEST_CASE(test_find)
}
-
BOOST_AUTO_TEST_SUITE_END()
diff --git a/source/RobotAPI/libraries/GraspingUtility/GraspCandidateReader.cpp b/source/RobotAPI/libraries/GraspingUtility/GraspCandidateReader.cpp
index af59f5fa837a3d80e199350ee9da562bc5745703..4e14baacb6040f7bf97a4855d3da7e4d252bce03 100644
--- a/source/RobotAPI/libraries/GraspingUtility/GraspCandidateReader.cpp
+++ b/source/RobotAPI/libraries/GraspingUtility/GraspCandidateReader.cpp
@@ -1,25 +1,26 @@
#include "GraspCandidateReader.h"
+#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
+
#include <RobotAPI/libraries/GraspingUtility/aron/GraspCandidate.aron.generated.h>
#include <RobotAPI/libraries/GraspingUtility/aron_conversions.h>
#include <RobotAPI/libraries/armem/core/error/mns.h>
-#include <RobotAPI/libraries/armem/util/util.h>
#include <RobotAPI/libraries/armem/core/wm/visitor/FunctionalVisitor.h>
-#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
+#include <RobotAPI/libraries/armem/util/util.h>
namespace armarx::armem
{
- GraspCandidateReader::GraspCandidateReader(armem::client::MemoryNameSystem& memoryNameSystem)
- : memoryNameSystem(memoryNameSystem)
+ GraspCandidateReader::GraspCandidateReader(armem::client::MemoryNameSystem& memoryNameSystem) :
+ memoryNameSystem(memoryNameSystem)
{
}
-
- void GraspCandidateReader::connect(bool use)
+ void
+ GraspCandidateReader::connect(bool use)
{
// Wait for the memory to become available and add it as dependency.
- ARMARX_IMPORTANT << "GraspCandidateReader: Waiting for memory '"
- << properties.memoryName << "' ...";
+ ARMARX_IMPORTANT << "GraspCandidateReader: Waiting for memory '" << properties.memoryName
+ << "' ...";
try
{
memoryReader = use ? memoryNameSystem.useReader(properties.memoryName)
@@ -34,8 +35,8 @@ namespace armarx::armem
}
}
-
- armarx::grasping::GraspCandidate asGraspCandidate(const armem::wm::EntityInstance& instance)
+ armarx::grasping::GraspCandidate
+ asGraspCandidate(const armem::wm::EntityInstance& instance)
{
armarx::grasping::GraspCandidate candidate;
@@ -48,7 +49,8 @@ namespace armarx::armem
return candidate;
}
- armarx::grasping::BimanualGraspCandidate asBimanualGraspCandidate(const armem::wm::EntityInstance& instance)
+ armarx::grasping::BimanualGraspCandidate
+ asBimanualGraspCandidate(const armem::wm::EntityInstance& instance)
{
armarx::grasping::BimanualGraspCandidate candidate;
@@ -60,8 +62,8 @@ namespace armarx::armem
return candidate;
}
-
- grasping::GraspCandidatePtr GraspCandidateReader::queryGraspCandidateInstanceByID(const armem::MemoryID& id) const
+ grasping::GraspCandidatePtr
+ GraspCandidateReader::queryGraspCandidateInstanceByID(const armem::MemoryID& id) const
{
auto dict = queryGraspCandidateInstancesByID({id});
if (auto it = dict.find(id.str()); it != dict.end())
@@ -74,8 +76,8 @@ namespace armarx::armem
}
}
-
- grasping::GraspCandidateDict GraspCandidateReader::queryGraspCandidateInstancesByID(const std::vector<MemoryID>& ids) const
+ grasping::GraspCandidateDict
+ GraspCandidateReader::queryGraspCandidateInstancesByID(const std::vector<MemoryID>& ids) const
{
armem::client::query::Builder qb;
@@ -83,8 +85,7 @@ namespace armarx::armem
qb.multipleEntitySnapshots(ids);
- const armem::client::QueryResult qResult =
- memoryReader.query(qb.buildQueryInput());
+ const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
if (!qResult.success)
{
@@ -104,9 +105,8 @@ namespace armarx::armem
return candidates;
}
-
- grasping::BimanualGraspCandidatePtr GraspCandidateReader::queryBimanualGraspCandidateInstanceByID(
- const armem::MemoryID& id) const
+ grasping::BimanualGraspCandidatePtr
+ GraspCandidateReader::queryBimanualGraspCandidateInstanceByID(const armem::MemoryID& id) const
{
armem::client::query::Builder qb;
@@ -114,8 +114,7 @@ namespace armarx::armem
qb.singleEntitySnapshot(id.getEntitySnapshotID());
- const armem::client::QueryResult qResult =
- memoryReader.query(qb.buildQueryInput());
+ const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
if (!qResult.success)
{
@@ -127,11 +126,12 @@ namespace armarx::armem
armarx::grasping::BimanualGraspCandidatePtr candidate;
armem::wm::FunctionalVisitor visitor;
- visitor.instanceConstFn = [id, &candidate](armem::wm::EntityInstance const & instance)
+ visitor.instanceConstFn = [id, &candidate](armem::wm::EntityInstance const& instance)
{
if (instance.id() == id)
{
- candidate = new grasping::BimanualGraspCandidate(asBimanualGraspCandidate(instance));
+ candidate =
+ new grasping::BimanualGraspCandidate(asBimanualGraspCandidate(instance));
}
return true;
};
@@ -141,46 +141,52 @@ namespace armarx::armem
return candidate;
}
- grasping::GraspCandidateDict GraspCandidateReader::queryLatestGraspCandidateEntity(
- const std::string& provider, const std::string& entity) const
+ grasping::GraspCandidateDict
+ GraspCandidateReader::queryLatestGraspCandidateEntity(const std::string& provider,
+ const std::string& entity) const
{
armem::client::query::Builder qb;
ARMARX_DEBUG << "Query for memory name: " << properties.memoryName;
- qb
- .coreSegments().withName(properties.graspCandidateMemoryName)
- .providerSegments().withName(provider)
- .entities().withName(entity)
- .snapshots().latest();
+ qb.coreSegments()
+ .withName(properties.graspCandidateMemoryName)
+ .providerSegments()
+ .withName(provider)
+ .entities()
+ .withName(entity)
+ .snapshots()
+ .latest();
- const armem::client::QueryResult qResult =
- memoryReader.query(qb.buildQueryInput());
+ const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
return getGraspCandidatesFromResultSet(qResult);
}
- std::map<std::string, grasping::BimanualGraspCandidatePtr> GraspCandidateReader::queryLatestBimanualGraspCandidateEntity(
- const std::string& provider, const std::string& entity) const
+ std::map<std::string, grasping::BimanualGraspCandidatePtr>
+ GraspCandidateReader::queryLatestBimanualGraspCandidateEntity(const std::string& provider,
+ const std::string& entity) const
{
armem::client::query::Builder qb;
ARMARX_DEBUG << "Query for memory name: " << properties.memoryName;
- qb
- .coreSegments().withName(properties.bimanualGraspCandidateMemoryName)
- .providerSegments().withName(provider)
- .entities().withName(entity)
- .snapshots().latest();
+ qb.coreSegments()
+ .withName(properties.bimanualGraspCandidateMemoryName)
+ .providerSegments()
+ .withName(provider)
+ .entities()
+ .withName(entity)
+ .snapshots()
+ .latest();
- const armem::client::QueryResult qResult =
- memoryReader.query(qb.buildQueryInput());
+ const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
return getBimanualGraspCandidatesFromResultSet(qResult);
}
- grasping::GraspCandidateDict GraspCandidateReader::queryLatestGraspCandidates(
- const std::string& provider) const
+ grasping::GraspCandidateDict
+ GraspCandidateReader::queryLatestGraspCandidates(const std::string& provider) const
{
armem::client::query::Builder qb;
@@ -190,29 +196,34 @@ namespace armarx::armem
if (!provider.empty())
{
- qb
- .coreSegments().withName(properties.graspCandidateMemoryName)
- .providerSegments().withName(provider)
- .entities().all()
- .snapshots().latest();
+ qb.coreSegments()
+ .withName(properties.graspCandidateMemoryName)
+ .providerSegments()
+ .withName(provider)
+ .entities()
+ .all()
+ .snapshots()
+ .latest();
}
else
{
- qb
- .coreSegments().withName(properties.graspCandidateMemoryName)
- .providerSegments().all()
- .entities().all()
- .snapshots().latest();
+ qb.coreSegments()
+ .withName(properties.graspCandidateMemoryName)
+ .providerSegments()
+ .all()
+ .entities()
+ .all()
+ .snapshots()
+ .latest();
}
- const armem::client::QueryResult qResult =
- memoryReader.query(qb.buildQueryInput());
+ const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
return getGraspCandidatesFromResultSet(qResult);
}
- std::map<std::string, grasping::BimanualGraspCandidatePtr> GraspCandidateReader::queryLatestBimanualGraspCandidates(
- const std::string& provider) const
+ std::map<std::string, grasping::BimanualGraspCandidatePtr>
+ GraspCandidateReader::queryLatestBimanualGraspCandidates(const std::string& provider) const
{
armem::client::query::Builder qb;
@@ -221,42 +232,49 @@ namespace armarx::armem
if (!provider.empty())
{
- qb
- .coreSegments().withName(properties.bimanualGraspCandidateMemoryName)
- .providerSegments().withName(provider)
- .entities().all()
- .snapshots().latest();
+ qb.coreSegments()
+ .withName(properties.bimanualGraspCandidateMemoryName)
+ .providerSegments()
+ .withName(provider)
+ .entities()
+ .all()
+ .snapshots()
+ .latest();
}
else
{
- qb
- .coreSegments().withName(properties.bimanualGraspCandidateMemoryName)
- .providerSegments().all()
- .entities().all()
- .snapshots().latest();
+ qb.coreSegments()
+ .withName(properties.bimanualGraspCandidateMemoryName)
+ .providerSegments()
+ .all()
+ .entities()
+ .all()
+ .snapshots()
+ .latest();
}
- const armem::client::QueryResult qResult =
- memoryReader.query(qb.buildQueryInput());
+ const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
return getBimanualGraspCandidatesFromResultSet(qResult);
}
-
- void GraspCandidateReader::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
+ void
+ GraspCandidateReader::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
{
ARMARX_DEBUG << "GraspCandidateReader: registerPropertyDefinitions";
const std::string prefix = propertyPrefix;
- def->optional(properties.graspCandidateMemoryName, prefix + "GraspCandidateMemoryName",
+ def->optional(properties.graspCandidateMemoryName,
+ prefix + "GraspCandidateMemoryName",
"Name of the grasping memory core segment to use.");
def->optional(properties.memoryName, prefix + "MemoryName");
}
- grasping::GraspCandidateDict GraspCandidateReader::getGraspCandidatesFromResultSet(
- const armem::client::QueryResult& qResult) const
+ grasping::GraspCandidateDict
+ GraspCandidateReader::getGraspCandidatesFromResultSet(
+ const armem::client::QueryResult& qResult) const
{
if (!qResult.success)
{
@@ -267,9 +285,10 @@ namespace armarx::armem
std::map<std::string, armarx::grasping::GraspCandidatePtr> candidates;
armem::wm::FunctionalVisitor visitor;
- visitor.instanceConstFn = [&candidates](armem::wm::EntityInstance const & instance)
+ visitor.instanceConstFn = [&candidates](armem::wm::EntityInstance const& instance)
{
- candidates[instance.id().str()] = new grasping::GraspCandidate(asGraspCandidate(instance));
+ candidates[instance.id().str()] =
+ new grasping::GraspCandidate(asGraspCandidate(instance));
return true;
};
@@ -279,8 +298,9 @@ namespace armarx::armem
return candidates;
}
- std::map<std::string, grasping::BimanualGraspCandidatePtr> GraspCandidateReader::getBimanualGraspCandidatesFromResultSet(
- const armem::client::QueryResult& qResult) const
+ std::map<std::string, grasping::BimanualGraspCandidatePtr>
+ GraspCandidateReader::getBimanualGraspCandidatesFromResultSet(
+ const armem::client::QueryResult& qResult) const
{
if (!qResult.success)
{
@@ -291,9 +311,10 @@ namespace armarx::armem
std::map<std::string, armarx::grasping::BimanualGraspCandidatePtr> candidates;
armem::wm::FunctionalVisitor visitor;
- visitor.instanceConstFn = [&candidates](armem::wm::EntityInstance const & instance)
+ visitor.instanceConstFn = [&candidates](armem::wm::EntityInstance const& instance)
{
- candidates[instance.id().str()] = new grasping::BimanualGraspCandidate(asBimanualGraspCandidate(instance));
+ candidates[instance.id().str()] =
+ new grasping::BimanualGraspCandidate(asBimanualGraspCandidate(instance));
return true;
};
@@ -303,5 +324,41 @@ namespace armarx::armem
return candidates;
}
+ grasping::GraspCandidateDict
+ GraspCandidateReader::queryGraspCandidatesNewerThan(const std::string& provider,
+ const DateTime& timestamp) const
+ {
+ armem::client::query::Builder qb;
+
+ ARMARX_DEBUG << "Query for memory name: " << properties.memoryName;
+
+
+ if (!provider.empty())
+ {
+ qb.coreSegments()
+ .withName(properties.graspCandidateMemoryName)
+ .providerSegments()
+ .withName(provider)
+ .entities()
+ .all()
+ .snapshots()
+ .timeRange(timestamp, armarx::core::time::DateTime::Now());
+ }
+ else
+ {
+ qb.coreSegments()
+ .withName(properties.graspCandidateMemoryName)
+ .providerSegments()
+ .all()
+ .entities()
+ .all()
+ .snapshots()
+ .timeRange(timestamp, armarx::core::time::DateTime::Now());
+ }
+
+ const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
+ return getGraspCandidatesFromResultSet(qResult);
+ }
+
-}
+} // namespace armarx::armem
diff --git a/source/RobotAPI/libraries/GraspingUtility/GraspCandidateReader.h b/source/RobotAPI/libraries/GraspingUtility/GraspCandidateReader.h
index 5de5c25e335e5544170f18f838df2e5e23d213fa..384f5cdb0c1158b714ba8b3038e09f4707dd74bb 100644
--- a/source/RobotAPI/libraries/GraspingUtility/GraspCandidateReader.h
+++ b/source/RobotAPI/libraries/GraspingUtility/GraspCandidateReader.h
@@ -2,10 +2,9 @@
#include <vector>
-#include <RobotAPI/libraries/armem/client/Reader.h>
-#include <RobotAPI/libraries/armem/client.h>
#include <RobotAPI/interface/units/GraspCandidateProviderInterface.h>
-
+#include <RobotAPI/libraries/armem/client.h>
+#include <RobotAPI/libraries/armem/client/Reader.h>
namespace armarx::armem
{
@@ -17,35 +16,43 @@ namespace armarx::armem
void connect(bool use = true);
- grasping::GraspCandidatePtr queryGraspCandidateInstanceByID(armem::MemoryID const& id) const;
- grasping::GraspCandidateDict queryGraspCandidateInstancesByID(std::vector<armem::MemoryID> const& ids) const;
+ ::armarx::grasping::GraspCandidatePtr
+ queryGraspCandidateInstanceByID(armem::MemoryID const& id) const;
- grasping::BimanualGraspCandidatePtr queryBimanualGraspCandidateInstanceByID(armem::MemoryID const& id) const;
+ ::armarx::grasping::GraspCandidateDict
+ queryGraspCandidateInstancesByID(std::vector<armem::MemoryID> const& ids) const;
- grasping::GraspCandidateDict queryLatestGraspCandidateEntity(
- std::string const& provider, std::string const& entity) const;
+ ::armarx::grasping::BimanualGraspCandidatePtr
+ queryBimanualGraspCandidateInstanceByID(armem::MemoryID const& id) const;
- std::map<std::string, grasping::BimanualGraspCandidatePtr> queryLatestBimanualGraspCandidateEntity(
- std::string const& provider, std::string const& entity) const;
+ ::armarx::grasping::GraspCandidateDict
+ queryLatestGraspCandidateEntity(std::string const& provider,
+ std::string const& entity) const;
- grasping::GraspCandidateDict queryLatestGraspCandidates(
- std::string const& provider = "") const;
+ std::map<std::string, ::armarx::grasping::BimanualGraspCandidatePtr>
+ queryLatestBimanualGraspCandidateEntity(std::string const& provider,
+ std::string const& entity) const;
- std::map<std::string, grasping::BimanualGraspCandidatePtr> queryLatestBimanualGraspCandidates(
- std::string const& provider = "") const;
+ ::armarx::grasping::GraspCandidateDict
+ queryLatestGraspCandidates(std::string const& provider = "") const;
+ ::armarx::grasping::GraspCandidateDict queryGraspCandidatesNewerThan(
+ std::string const& provider = "",
+ const armarx::DateTime& timestamp = armarx::DateTime::Now()) const;
+
+ std::map<std::string, ::armarx::grasping::BimanualGraspCandidatePtr>
+ queryLatestBimanualGraspCandidates(std::string const& provider = "") const;
void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def);
private:
+ ::armarx::grasping::GraspCandidateDict
+ getGraspCandidatesFromResultSet(armem::client::QueryResult const& qResult) const;
- grasping::GraspCandidateDict getGraspCandidatesFromResultSet(
- armem::client::QueryResult const& qResult) const;
-
- std::map<std::string, grasping::BimanualGraspCandidatePtr> getBimanualGraspCandidatesFromResultSet(
- armem::client::QueryResult const& qResult) const;
+ std::map<std::string, ::armarx::grasping::BimanualGraspCandidatePtr>
+ getBimanualGraspCandidatesFromResultSet(armem::client::QueryResult const& qResult) const;
armem::client::Reader memoryReader;
@@ -57,11 +64,9 @@ namespace armarx::armem
std::string bimanualGraspCandidateMemoryName = "BimanualGraspCandidate";
} properties;
-
const std::string propertyPrefix = "mem.grasping.";
armem::client::MemoryNameSystem& memoryNameSystem;
-
};
-}
+} // namespace armarx::armem
diff --git a/source/RobotAPI/libraries/GraspingUtility/GraspTrajectory.cpp b/source/RobotAPI/libraries/GraspingUtility/GraspTrajectory.cpp
index c4b170313f7d9d71d09b055ffd497b9b658af5ac..045026b0eb62f1a1ce95210acc555045fd2e0c82 100644
--- a/source/RobotAPI/libraries/GraspingUtility/GraspTrajectory.cpp
+++ b/source/RobotAPI/libraries/GraspingUtility/GraspTrajectory.cpp
@@ -21,6 +21,10 @@
* GNU General Public License
*/
+#include "GraspTrajectory.h"
+
+#include <memory>
+
#include <VirtualRobot/math/Helpers.h>
#include <ArmarXCore/core/exceptions/Exception.h>
@@ -34,14 +38,23 @@
#include <RobotAPI/interface/units/GraspCandidateProviderInterface.h>
#include <RobotAPI/libraries/SimpleJsonLogger/SimpleJsonLogger.h>
-#include "GraspTrajectory.h"
-
namespace armarx
{
+ Eigen::VectorXf
+ mapValuesToVector(const armarx::NameValueMap& map)
+ {
+ ARMARX_TRACE;
+ Eigen::VectorXf vector(map.size());
+ std::transform(
+ map.begin(), map.end(), vector.data(), [](const auto& item) { return item.second; });
+ return vector;
+ }
+
void
GraspTrajectory::writeToFile(const std::string& filename)
{
+ ARMARX_TRACE;
RapidXmlWriter writer;
RapidXmlWriterNode root = writer.createRootNode("GraspTrajectory");
for (const KeypointPtr& keypoint : keypoints)
@@ -49,7 +62,12 @@ namespace armarx
SimpleJsonLoggerEntry e;
e.Add("dt", keypoint->dt);
e.AddAsArr("Pose", keypoint->tcpTarget);
- e.AddAsArr("HandValues", keypoint->handJointsTarget);
+ JsonObjectPtr const obj = std::make_shared<JsonObject>();
+ for (const auto& [name, value] : keypoint->handJointsTarget)
+ {
+ obj->add(name, JsonValue::Create(value));
+ }
+ e.obj->add("HandValues", obj);
root.append_string_node("Keypoint", e.obj->toJsonString(0, "", true));
}
writer.saveToFile(filename, true);
@@ -58,7 +76,8 @@ namespace armarx
GraspTrajectoryPtr
GraspTrajectory::ReadFromReader(const RapidXmlReaderPtr& reader)
{
- RapidXmlReaderNode root = reader->getRoot();
+ ARMARX_TRACE;
+ RapidXmlReaderNode const root = reader->getRoot();
GraspTrajectoryPtr traj;
for (const RapidXmlReaderNode& kpNode : root.nodes("Keypoint"))
@@ -79,16 +98,17 @@ namespace armarx
}
}
- Eigen::Vector3f handValues;
+ armarx::NameValueMap handValues;
std::vector<JPathNavigator> cells = nav.select("HandValues/*");
- for (int j = 0; j < 3; j++)
+ for (const auto& cell : cells)
{
- handValues(j) = cells.at(j).asFloat();
+ handValues[cell.getKey()] = cell.asFloat();
}
+
if (!traj)
{
- traj = GraspTrajectoryPtr(new GraspTrajectory(pose, handValues));
+ traj = std::make_shared<GraspTrajectory>(pose, handValues);
}
else
{
@@ -101,25 +121,32 @@ namespace armarx
GraspTrajectoryPtr
GraspTrajectory::ReadFromFile(const std::string& filename)
{
+ ARMARX_TRACE;
return ReadFromReader(RapidXmlReader::FromFile(filename));
}
GraspTrajectoryPtr
GraspTrajectory::ReadFromString(const std::string& xml)
{
+ ARMARX_TRACE;
return ReadFromReader(RapidXmlReader::FromXmlString(xml));
}
- GraspTrajectory::GraspTrajectory(const Eigen::Matrix4f &tcpStart,
- const Eigen::Vector3f &handJointsStart) {
+ GraspTrajectory::GraspTrajectory(const Eigen::Matrix4f& tcpStart,
+ const armarx::NameValueMap& handJointsStart)
+ {
+ ARMARX_TRACE;
KeypointPtr keypoint(new Keypoint(tcpStart, handJointsStart));
keypointMap[0] = keypoints.size();
keypoints.push_back(keypoint);
}
void
- GraspTrajectory::addKeypoint(const Eigen::Matrix4f &tcpTarget, const Eigen::Vector3f &handJointsTarget,
- float dt) {
+ GraspTrajectory::addKeypoint(const Eigen::Matrix4f& tcpTarget,
+ const armarx::NameValueMap& handJointsTarget,
+ float dt)
+ {
+ ARMARX_TRACE;
KeypointPtr prev = lastKeypoint();
KeypointPtr keypoint(new Keypoint(tcpTarget, handJointsTarget));
keypoint->updateVelocities(prev, dt);
@@ -129,13 +156,19 @@ namespace armarx
}
size_t
- GraspTrajectory::getKeypointCount() const {
+ GraspTrajectory::getKeypointCount() const
+ {
+ ARMARX_TRACE;
return keypoints.size();
}
void
- GraspTrajectory::insertKeypoint(size_t index, const Eigen::Matrix4f &tcpTarget,
- const Eigen::Vector3f &handJointsTarget, float dt) {
+ GraspTrajectory::insertKeypoint(size_t index,
+ const Eigen::Matrix4f& tcpTarget,
+ const armarx::NameValueMap& handJointsTarget,
+ float dt)
+ {
+ ARMARX_TRACE;
if (index <= 0 || index > keypoints.size())
{
throw LocalException("Index out of range" + std::to_string(index));
@@ -153,7 +186,9 @@ namespace armarx
}
void
- GraspTrajectory::removeKeypoint(size_t index) {
+ GraspTrajectory::removeKeypoint(size_t index)
+ {
+ ARMARX_TRACE;
if (index <= 0 || index >= keypoints.size())
{
throw LocalException("Index out of range" + std::to_string(index));
@@ -161,6 +196,7 @@ namespace armarx
keypoints.erase(keypoints.begin() + index);
if (index < keypoints.size())
{
+ ARMARX_TRACE;
KeypointPtr prev = keypoints.at(index - 1);
KeypointPtr next = keypoints.at(index);
next->updateVelocities(prev, next->dt);
@@ -169,8 +205,12 @@ namespace armarx
}
void
- GraspTrajectory::replaceKeypoint(size_t index, const Eigen::Matrix4f &tcpTarget,
- const Eigen::Vector3f &handJointsTarget, float dt) {
+ GraspTrajectory::replaceKeypoint(size_t index,
+ const Eigen::Matrix4f& tcpTarget,
+ const armarx::NameValueMap& handJointsTarget,
+ float dt)
+ {
+ ARMARX_TRACE;
if (index <= 0 || index >= keypoints.size())
{
throw LocalException("Index out of range" + std::to_string(index));
@@ -183,7 +223,9 @@ namespace armarx
}
void
- GraspTrajectory::setKeypointDt(size_t index, float dt) {
+ GraspTrajectory::setKeypointDt(size_t index, float dt)
+ {
+ ARMARX_TRACE;
if (index <= 0 || index >= keypoints.size())
{
throw LocalException("Index out of range" + std::to_string(index));
@@ -194,23 +236,31 @@ namespace armarx
updateKeypointMap();
}
- GraspTrajectory::KeypointPtr &
- GraspTrajectory::lastKeypoint() {
+ GraspTrajectory::KeypointPtr&
+ GraspTrajectory::lastKeypoint()
+ {
+ ARMARX_TRACE;
return keypoints.at(keypoints.size() - 1);
}
- GraspTrajectory::KeypointPtr &
- GraspTrajectory::getKeypoint(int i) {
+ GraspTrajectory::KeypointPtr&
+ GraspTrajectory::getKeypoint(int i)
+ {
+ ARMARX_TRACE;
return keypoints.at(i);
}
Eigen::Matrix4f
- GraspTrajectory::getStartPose() {
+ GraspTrajectory::getStartPose()
+ {
+ ARMARX_TRACE;
return getKeypoint(0)->getTargetPose();
}
void
- GraspTrajectory::getIndex(float t, int &i1, int &i2, float &f) {
+ GraspTrajectory::getIndex(float t, int& i1, int& i2, float& f)
+ {
+ ARMARX_TRACE;
if (t <= 0)
{
i1 = 0;
@@ -234,15 +284,20 @@ namespace armarx
}
Eigen::Vector3f
- GraspTrajectory::GetPosition(float t) {
+ GraspTrajectory::GetPosition(float t)
+ {
+ ARMARX_TRACE;
int i1, i2;
float f;
getIndex(t, i1, i2, f);
- return ::math::Helpers::Lerp(getKeypoint(i1)->getTargetPosition(), getKeypoint(i2)->getTargetPosition(), f);
+ return ::math::Helpers::Lerp(
+ getKeypoint(i1)->getTargetPosition(), getKeypoint(i2)->getTargetPosition(), f);
}
Eigen::Matrix3f
- GraspTrajectory::GetOrientation(float t) {
+ GraspTrajectory::GetOrientation(float t)
+ {
+ ARMARX_TRACE;
int i1, i2;
float f;
getIndex(t, i1, i2, f);
@@ -257,12 +312,16 @@ namespace armarx
}
Eigen::Matrix4f
- GraspTrajectory::GetPose(float t) {
+ GraspTrajectory::GetPose(float t)
+ {
+ ARMARX_TRACE;
return ::math::Helpers::CreatePose(GetPosition(t), GetOrientation(t));
}
std::vector<Eigen::Matrix4f>
- GraspTrajectory::getAllKeypointPoses() {
+ GraspTrajectory::getAllKeypointPoses()
+ {
+ ARMARX_TRACE;
std::vector<Eigen::Matrix4f> res;
for (const KeypointPtr& keypoint : keypoints)
{
@@ -272,7 +331,9 @@ namespace armarx
}
std::vector<Eigen::Vector3f>
- GraspTrajectory::getAllKeypointPositions() {
+ GraspTrajectory::getAllKeypointPositions()
+ {
+ ARMARX_TRACE;
std::vector<Eigen::Vector3f> res;
for (const KeypointPtr& keypoint : keypoints)
{
@@ -282,7 +343,9 @@ namespace armarx
}
std::vector<Eigen::Matrix3f>
- GraspTrajectory::getAllKeypointOrientations() {
+ GraspTrajectory::getAllKeypointOrientations()
+ {
+ ARMARX_TRACE;
std::vector<Eigen::Matrix3f> res;
for (const KeypointPtr& keypoint : keypoints)
{
@@ -291,16 +354,29 @@ namespace armarx
return res;
}
- Eigen::Vector3f
- GraspTrajectory::GetHandValues(float t) {
+ armarx::NameValueMap
+ GraspTrajectory::GetHandValues(float t)
+ {
+ ARMARX_TRACE;
int i1, i2;
float f;
getIndex(t, i1, i2, f);
- return ::math::Helpers::Lerp(getKeypoint(i1)->handJointsTarget, getKeypoint(i2)->handJointsTarget, f);
+ auto handTargetsMap = getKeypoint(i1)->handJointsTarget;
+ const auto handTargets1 = mapValuesToVector(handTargetsMap);
+ const auto handTargets2 = mapValuesToVector(getKeypoint(i2)->handJointsTarget);
+ const Eigen::VectorXf lerpTargets = handTargets1 * (1 - f) + handTargets2 * f;
+ auto* lerpTargetsIt = lerpTargets.data();
+ for (auto& [name, value] : handTargetsMap)
+ {
+ value = *(lerpTargetsIt++);
+ }
+ return handTargetsMap;
}
Eigen::Vector3f
- GraspTrajectory::GetPositionDerivative(float t) {
+ GraspTrajectory::GetPositionDerivative(float t)
+ {
+ ARMARX_TRACE;
int i1, i2;
float f;
getIndex(t, i1, i2, f);
@@ -308,7 +384,9 @@ namespace armarx
}
Eigen::Vector3f
- GraspTrajectory::GetOrientationDerivative(float t) {
+ GraspTrajectory::GetOrientationDerivative(float t)
+ {
+ ARMARX_TRACE;
int i1, i2;
float f;
getIndex(t, i1, i2, f);
@@ -316,7 +394,9 @@ namespace armarx
}
Eigen::Matrix<float, 6, 1>
- GraspTrajectory::GetTcpDerivative(float t) {
+ GraspTrajectory::GetTcpDerivative(float t)
+ {
+ ARMARX_TRACE;
Eigen::Matrix<float, 6, 1> ffVel;
ffVel.block<3, 1>(0, 0) = GetPositionDerivative(t);
ffVel.block<3, 1>(3, 0) = GetOrientationDerivative(t);
@@ -324,7 +404,9 @@ namespace armarx
}
Eigen::Vector3f
- GraspTrajectory::GetHandJointsDerivative(float t) {
+ GraspTrajectory::GetHandJointsDerivative(float t)
+ {
+ ARMARX_TRACE;
int i1, i2;
float f;
getIndex(t, i1, i2, f);
@@ -332,12 +414,16 @@ namespace armarx
}
float
- GraspTrajectory::getDuration() const {
+ GraspTrajectory::getDuration() const
+ {
+ ARMARX_TRACE;
return keypointMap.rbegin()->first;
}
GraspTrajectory::Length
- GraspTrajectory::calculateLength() const {
+ GraspTrajectory::calculateLength() const
+ {
+ ARMARX_TRACE;
Length l;
for (size_t i = 1; i < keypoints.size(); i++)
{
@@ -357,19 +443,31 @@ namespace armarx
}
GraspTrajectoryPtr
- GraspTrajectory::getTranslatedAndRotated(const Eigen::Vector3f &translation, const Eigen::Matrix3f &rotation) {
- GraspTrajectoryPtr traj(new GraspTrajectory(::math::Helpers::TranslateAndRotatePose(getKeypoint(0)->getTargetPose(), translation, rotation), getKeypoint(0)->handJointsTarget));
+ GraspTrajectory::getTranslatedAndRotated(const Eigen::Vector3f& translation,
+ const Eigen::Matrix3f& rotation)
+ {
+ ARMARX_TRACE;
+ GraspTrajectoryPtr traj(
+ new GraspTrajectory(::math::Helpers::TranslateAndRotatePose(
+ getKeypoint(0)->getTargetPose(), translation, rotation),
+ getKeypoint(0)->handJointsTarget));
for (size_t i = 1; i < keypoints.size(); i++)
{
KeypointPtr& kp = keypoints.at(i);
- traj->addKeypoint(::math::Helpers::TranslateAndRotatePose(kp->getTargetPose(), translation, rotation), kp->handJointsTarget, kp->dt);
+ traj->addKeypoint(
+ ::math::Helpers::TranslateAndRotatePose(kp->getTargetPose(), translation, rotation),
+ kp->handJointsTarget,
+ kp->dt);
}
return traj;
}
GraspTrajectoryPtr
- GraspTrajectory::getTransformed(const Eigen::Matrix4f &transform) {
- GraspTrajectoryPtr traj(new GraspTrajectory(transform * getStartPose(), getKeypoint(0)->handJointsTarget));
+ GraspTrajectory::getTransformed(const Eigen::Matrix4f& transform)
+ {
+ ARMARX_TRACE;
+ GraspTrajectoryPtr traj(
+ new GraspTrajectory(transform * getStartPose(), getKeypoint(0)->handJointsTarget));
for (size_t i = 1; i < keypoints.size(); i++)
{
KeypointPtr& kp = keypoints.at(i);
@@ -379,32 +477,44 @@ namespace armarx
}
GraspTrajectoryPtr
- GraspTrajectory::getClone() {
+ GraspTrajectory::getClone()
+ {
+ ARMARX_TRACE;
return getTransformed(Eigen::Matrix4f::Identity());
}
GraspTrajectoryPtr
- GraspTrajectory::getTransformedToGraspPose(const Eigen::Matrix4f &target, const Eigen::Vector3f &handForward) {
+ GraspTrajectory::getTransformedToGraspPose(const Eigen::Matrix4f& target,
+ const Eigen::Vector3f& handForward)
+ {
+ ARMARX_TRACE;
Eigen::Matrix4f startPose = getStartPose();
- Eigen::Vector3f targetHandForward = ::math::Helpers::TransformDirection(target, handForward);
- Eigen::Vector3f trajHandForward = ::math::Helpers::TransformDirection(startPose, handForward);
+ Eigen::Vector3f targetHandForward =
+ ::math::Helpers::TransformDirection(target, handForward);
+ Eigen::Vector3f trajHandForward =
+ ::math::Helpers::TransformDirection(startPose, handForward);
Eigen::Vector3f up(0, 0, 1);
float angle = ::math::Helpers::Angle(targetHandForward, trajHandForward, up);
Eigen::AngleAxisf aa(angle, up);
- Eigen::Matrix4f transform = ::math::Helpers::CreateTranslationRotationTranslationPose(-::math::Helpers::GetPosition(startPose), aa.toRotationMatrix(), ::math::Helpers::GetPosition(target));
+ Eigen::Matrix4f transform = ::math::Helpers::CreateTranslationRotationTranslationPose(
+ -::math::Helpers::GetPosition(startPose),
+ aa.toRotationMatrix(),
+ ::math::Helpers::GetPosition(target));
return getTransformed(transform);
}
GraspTrajectoryPtr
- GraspTrajectory::getTransformedToOtherHand() {
+ GraspTrajectory::getTransformedToOtherHand()
+ {
+ ARMARX_TRACE;
Eigen::Matrix3f flip_yz = Eigen::Matrix3f::Identity();
flip_yz(0, 0) *= -1.0;
Eigen::Matrix4f start_pose = getStartPose();
start_pose.block<3, 3>(0, 0) = flip_yz * start_pose.block<3, 3>(0, 0) * flip_yz;
GraspTrajectoryPtr output_trajectory(
- new GraspTrajectory(start_pose, getKeypoint(0)->handJointsTarget));
+ new GraspTrajectory(start_pose, getKeypoint(0)->handJointsTarget));
for (size_t i = 1; i < getKeypointCount(); i++)
{
GraspTrajectory::KeypointPtr& kp = getKeypoint(i);
@@ -416,13 +526,19 @@ namespace armarx
}
SimpleDiffIK::Reachability
- GraspTrajectory::calculateReachability(VirtualRobot::RobotNodeSetPtr rns, VirtualRobot::RobotNodePtr tcp,
- SimpleDiffIK::Parameters params) {
- return SimpleDiffIK::CalculateReachability(getAllKeypointPoses(), Eigen::VectorXf::Zero(rns->getSize()), rns, tcp, params);
+ GraspTrajectory::calculateReachability(VirtualRobot::RobotNodeSetPtr rns,
+ VirtualRobot::RobotNodePtr tcp,
+ SimpleDiffIK::Parameters params)
+ {
+ ARMARX_TRACE;
+ return SimpleDiffIK::CalculateReachability(
+ getAllKeypointPoses(), Eigen::VectorXf::Zero(rns->getSize()), rns, tcp, params);
}
void
- GraspTrajectory::updateKeypointMap() {
+ GraspTrajectory::updateKeypointMap()
+ {
+ ARMARX_TRACE;
keypointMap.clear();
float t = 0;
for (size_t i = 0; i < keypoints.size(); i++)
@@ -432,54 +548,72 @@ namespace armarx
}
}
-
-
void
- GraspTrajectory::Keypoint::updateVelocities(const GraspTrajectory::KeypointPtr &prev, float dt) {
+ GraspTrajectory::Keypoint::updateVelocities(const GraspTrajectory::KeypointPtr& prev,
+ float deltat)
+ {
+ ARMARX_TRACE;
Eigen::Vector3f pos0 = ::math::Helpers::GetPosition(prev->tcpTarget);
Eigen::Matrix3f ori0 = ::math::Helpers::GetOrientation(prev->tcpTarget);
- Eigen::Vector3f hnd0 = prev->handJointsTarget;
+ auto hnd0 = mapValuesToVector(prev->handJointsTarget);
Eigen::Vector3f pos1 = ::math::Helpers::GetPosition(tcpTarget);
Eigen::Matrix3f ori1 = ::math::Helpers::GetOrientation(tcpTarget);
- Eigen::Vector3f hnd1 = handJointsTarget;
+ auto hnd1 = mapValuesToVector(handJointsTarget);
Eigen::Vector3f dpos = pos1 - pos0;
Eigen::Vector3f dori = ::math::Helpers::GetRotationVector(ori0, ori1);
- Eigen::Vector3f dhnd = hnd1 - hnd0;
+ Eigen::VectorXf dhnd = hnd1 - hnd0;
- this->dt = dt;
- feedForwardPosVelocity = dpos / dt;
- feedForwardOriVelocity = dori / dt;
- feedForwardHandJointsVelocity = dhnd / dt;
+ this->dt = deltat;
+ feedForwardPosVelocity = dpos / deltat;
+ feedForwardOriVelocity = dori / deltat;
+ feedForwardHandJointsVelocity = dhnd / deltat;
}
Eigen::Vector3f
- GraspTrajectory::Keypoint::getTargetPosition() const {
+ GraspTrajectory::Keypoint::getTargetPosition() const
+ {
+ ARMARX_TRACE;
return ::math::Helpers::GetPosition(tcpTarget);
}
Eigen::Matrix3f
- GraspTrajectory::Keypoint::getTargetOrientation() const {
+ GraspTrajectory::Keypoint::getTargetOrientation() const
+ {
+ ARMARX_TRACE;
return ::math::Helpers::GetOrientation(tcpTarget);
}
Eigen::Matrix4f
- GraspTrajectory::Keypoint::getTargetPose() const {
+ GraspTrajectory::Keypoint::getTargetPose() const
+ {
+ ARMARX_TRACE;
return tcpTarget;
}
- GraspTrajectory::Keypoint::Keypoint(const Eigen::Matrix4f &tcpTarget, const Eigen::Vector3f &handJointsTarget,
- float dt, const Eigen::Vector3f &feedForwardPosVelocity,
- const Eigen::Vector3f &feedForwardOriVelocity,
- const Eigen::Vector3f &feedForwardHandJointsVelocity)
- : tcpTarget(tcpTarget), handJointsTarget(handJointsTarget), dt(dt),
- feedForwardPosVelocity(feedForwardPosVelocity), feedForwardOriVelocity(feedForwardOriVelocity),
- feedForwardHandJointsVelocity(feedForwardHandJointsVelocity)
- { }
-
- GraspTrajectory::Keypoint::Keypoint(const Eigen::Matrix4f &tcpTarget, const Eigen::Vector3f &handJointsTarget)
- : tcpTarget(tcpTarget), handJointsTarget(handJointsTarget), dt(0),
- feedForwardPosVelocity(0, 0, 0), feedForwardOriVelocity(0, 0, 0), feedForwardHandJointsVelocity(0, 0, 0)
- { }
-}
+ GraspTrajectory::Keypoint::Keypoint(const Eigen::Matrix4f& tcpTarget,
+ const armarx::NameValueMap& handJointsTarget,
+ float dt,
+ const Eigen::Vector3f& feedForwardPosVelocity,
+ const Eigen::Vector3f& feedForwardOriVelocity,
+ const Eigen::VectorXf& feedForwardHandJointsVelocity) :
+ tcpTarget(tcpTarget),
+ handJointsTarget(handJointsTarget),
+ dt(dt),
+ feedForwardPosVelocity(feedForwardPosVelocity),
+ feedForwardOriVelocity(feedForwardOriVelocity),
+ feedForwardHandJointsVelocity(feedForwardHandJointsVelocity)
+ {
+ }
+
+ GraspTrajectory::Keypoint::Keypoint(const Eigen::Matrix4f& tcpTarget,
+ const armarx::NameValueMap& handJointsTarget) :
+ tcpTarget(tcpTarget),
+ handJointsTarget(handJointsTarget),
+ dt(0),
+ feedForwardPosVelocity(0, 0, 0),
+ feedForwardOriVelocity(0, 0, 0)
+ {
+ }
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/GraspingUtility/GraspTrajectory.h b/source/RobotAPI/libraries/GraspingUtility/GraspTrajectory.h
index 18990ca21ae05f41fbe520c0da94374d4f2ae11e..697727b2274b6fee76a285fa392094043496f60e 100644
--- a/source/RobotAPI/libraries/GraspingUtility/GraspTrajectory.h
+++ b/source/RobotAPI/libraries/GraspingUtility/GraspTrajectory.h
@@ -23,12 +23,15 @@
#pragma once
-#include <Eigen/Core>
-#include <memory>
#include <map>
+#include <memory>
+
+#include <Eigen/Core>
+
#include <VirtualRobot/VirtualRobot.h>
-#include <RobotAPI/libraries/diffik/SimpleDiffIK.h>
+
#include <RobotAPI/interface/units/GraspCandidateProviderInterface.h>
+#include <RobotAPI/libraries/diffik/SimpleDiffIK.h>
namespace armarx
{
@@ -48,20 +51,23 @@ namespace armarx
{
public:
Eigen::Matrix4f tcpTarget;
- Eigen::Vector3f handJointsTarget;
+ armarx::NameValueMap handJointsTarget;
float dt;
Eigen::Vector3f feedForwardPosVelocity;
Eigen::Vector3f feedForwardOriVelocity;
- Eigen::Vector3f feedForwardHandJointsVelocity;
-
- Keypoint(const Eigen::Matrix4f& tcpTarget, const Eigen::Vector3f& handJointsTarget);
- Keypoint(const Eigen::Matrix4f& tcpTarget, const Eigen::Vector3f& handJointsTarget, float dt,
- const Eigen::Vector3f& feedForwardPosVelocity, const Eigen::Vector3f& feedForwardOriVelocity
- , const Eigen::Vector3f& feedForwardHandJointsVelocity);
+ Eigen::VectorXf feedForwardHandJointsVelocity;
+
+ Keypoint(const Eigen::Matrix4f& tcpTarget, const armarx::NameValueMap& handJointsTarget);
+ Keypoint(const Eigen::Matrix4f& tcpTarget,
+ const armarx::NameValueMap& handJointsTarget,
+ float dt,
+ const Eigen::Vector3f& feedForwardPosVelocity,
+ const Eigen::Vector3f& feedForwardOriVelocity,
+ const Eigen::VectorXf& feedForwardHandJointsVelocity);
Eigen::Vector3f getTargetPosition() const;
Eigen::Matrix3f getTargetOrientation() const;
Eigen::Matrix4f getTargetPose() const;
- void updateVelocities(const KeypointPtr& prev, float dt);
+ void updateVelocities(const KeypointPtr& prev, float deltat);
};
struct Length
@@ -73,17 +79,26 @@ namespace armarx
public:
GraspTrajectory() = default;
- GraspTrajectory(const Eigen::Matrix4f& tcpStart, const Eigen::Vector3f& handJointsStart);
+ GraspTrajectory(const Eigen::Matrix4f& tcpStart,
+ const armarx::NameValueMap& handJointsStart);
- void addKeypoint(const Eigen::Matrix4f& tcpTarget, const Eigen::Vector3f& handJointsTarget, float dt);
+ void addKeypoint(const Eigen::Matrix4f& tcpTarget,
+ const armarx::NameValueMap& handJointsTarget,
+ float dt);
size_t getKeypointCount() const;
- void insertKeypoint(size_t index, const Eigen::Matrix4f& tcpTarget, const Eigen::Vector3f& handJointsTarget, float dt);
+ void insertKeypoint(size_t index,
+ const Eigen::Matrix4f& tcpTarget,
+ const armarx::NameValueMap& handJointsTarget,
+ float dt);
void removeKeypoint(size_t index);
- void replaceKeypoint(size_t index, const Eigen::Matrix4f& tcpTarget, const Eigen::Vector3f& handJointsTarget, float dt);
+ void replaceKeypoint(size_t index,
+ const Eigen::Matrix4f& tcpTarget,
+ const armarx::NameValueMap& handJointsTarget,
+ float dt);
void setKeypointDt(size_t index, float dt);
@@ -103,7 +118,7 @@ namespace armarx
std::vector<Eigen::Vector3f> getAllKeypointPositions();
std::vector<Eigen::Matrix3f> getAllKeypointOrientations();
- Eigen::Vector3f GetHandValues(float t);
+ armarx::NameValueMap GetHandValues(float t);
Eigen::Vector3f GetPositionDerivative(float t);
@@ -118,18 +133,21 @@ namespace armarx
Length calculateLength() const;
- GraspTrajectoryPtr getTranslatedAndRotated(const Eigen::Vector3f& translation, const Eigen::Matrix3f& rotation);
+ GraspTrajectoryPtr getTranslatedAndRotated(const Eigen::Vector3f& translation,
+ const Eigen::Matrix3f& rotation);
GraspTrajectoryPtr getTransformed(const Eigen::Matrix4f& transform);
GraspTrajectoryPtr getTransformedToOtherHand();
GraspTrajectoryPtr getClone();
GraspTrajectoryPtr
- getTransformedToGraspPose(const Eigen::Matrix4f& target, const Eigen::Vector3f& handForward = Eigen::Vector3f::UnitZ());
-
- SimpleDiffIK::Reachability calculateReachability(VirtualRobot::RobotNodeSetPtr rns, VirtualRobot::RobotNodePtr tcp = VirtualRobot::RobotNodePtr(), SimpleDiffIK::Parameters params = SimpleDiffIK::Parameters());
-
+ getTransformedToGraspPose(const Eigen::Matrix4f& target,
+ const Eigen::Vector3f& handForward = Eigen::Vector3f::UnitZ());
+ SimpleDiffIK::Reachability
+ calculateReachability(VirtualRobot::RobotNodeSetPtr rns,
+ VirtualRobot::RobotNodePtr tcp = VirtualRobot::RobotNodePtr(),
+ SimpleDiffIK::Parameters params = SimpleDiffIK::Parameters());
void writeToFile(const std::string& filename);
@@ -139,13 +157,10 @@ namespace armarx
static GraspTrajectoryPtr ReadFromString(const std::string& xml);
private:
-
void updateKeypointMap();
private:
std::vector<KeypointPtr> keypoints;
std::map<float, size_t> keypointMap;
-
-
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/armem/client/Query.h b/source/RobotAPI/libraries/armem/client/Query.h
index bee999f073bc57cd53db0b9031cba9feee1cfbb7..8e14c5b5a7aa22ea260779c95bb63a24891cc29b 100644
--- a/source/RobotAPI/libraries/armem/client/Query.h
+++ b/source/RobotAPI/libraries/armem/client/Query.h
@@ -1,44 +1,60 @@
#pragma once
-// RobotAPI
#include <RobotAPI/interface/armem/query.h>
-
-#include <RobotAPI/libraries/armem/core/wm/ice_conversions.h>
#include <RobotAPI/libraries/armem/core/SuccessHeader.h>
#include <RobotAPI/libraries/armem/core/query/DataMode.h>
#include <RobotAPI/libraries/armem/core/query/QueryTarget.h>
+#include <RobotAPI/libraries/armem/core/wm/ice_conversions.h>
#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
-
namespace armarx::armem::client::query
{
// #include <RobotAPI/libraries/armem/client/query/Builder.h>
class Builder;
-}
+} // namespace armarx::armem::client::query
namespace armarx::armem::client
{
using QueryBuilder = query::Builder;
-
/**
- * @brief An update of an entity for a specific point in time.
+ * @brief A query for parts of a memory.
*/
struct QueryInput
{
+ /**
+ * @brief The queries.
+ */
armem::query::data::MemoryQuerySeq memoryQueries;
+ /**
+ * @brief Whether entity instance data (i.e., their payload) should be transferred.
+ */
armem::query::DataMode dataMode;
static QueryInput fromIce(const armem::query::data::Input& ice);
armem::query::data::Input toIce() const;
};
-
/**
- * @brief Result of an `EntityUpdate`.
+ * @brief Result of a `QueryInput`.
+ *
+ * If `success` is false, an error occurred during the query. In this case, `errorMessage` can
+ * contain more information.
+ *
+ * @note An empty result is valid, i.e. successful. In other words, an empty result is not a
+ * failure. To check whether any entity snapshot matched the query, use `memory.hasSnapshots()`
+ * or `memory.hasInstances()`.
+ *
+ * @see wm::Memory::hasInstances(), wm::Memory::hasSnapshots()
*/
struct QueryResult : public detail::SuccessHeader
{
+ /**
+ * @brief The slice of the memory that matched the query.
+ *
+ * To check whether the memory contains any snapshots or instances, use
+ * `memory.hasSnapshots()` or `memory.hasInstances()`.
+ */
wm::Memory memory;
@@ -48,11 +64,10 @@ namespace armarx::armem::client
friend std::ostream& operator<<(std::ostream& os, const QueryResult& rhs);
};
-
void toIce(armem::query::data::Input& ice, const QueryInput& input);
void fromIce(const armem::query::data::Input& ice, QueryInput& input);
void toIce(armem::query::data::Result& ice, const QueryResult& result);
void fromIce(const armem::query::data::Result& ice, QueryResult& result);
-}
+} // namespace armarx::armem::client
diff --git a/source/RobotAPI/libraries/armem/client/util/SimpleReaderBase.cpp b/source/RobotAPI/libraries/armem/client/util/SimpleReaderBase.cpp
index b0bbf2869134f92cdf15e45bde4dd264dda2d1b0..856bc217146eb651770fc192dd9ce8a03ebcdf3c 100644
--- a/source/RobotAPI/libraries/armem/client/util/SimpleReaderBase.cpp
+++ b/source/RobotAPI/libraries/armem/client/util/SimpleReaderBase.cpp
@@ -1,8 +1,7 @@
#include "SimpleReaderBase.h"
-#include <RobotAPI/libraries/armem/core/error.h>
#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
-
+#include <RobotAPI/libraries/armem/core/error.h>
namespace armarx::armem::client::util
{
@@ -12,9 +11,8 @@ namespace armarx::armem::client::util
{
}
-
- void SimpleReaderBase::registerPropertyDefinitions(
- armarx::PropertyDefinitionsPtr& def)
+ void
+ SimpleReaderBase::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
{
ARMARX_DEBUG << "Writer: registerPropertyDefinitions";
@@ -26,16 +24,17 @@ namespace armarx::armem::client::util
def->optional(props.coreSegmentName, prefix + "CoreSegment");
}
-
- void SimpleReaderBase::connect()
+ void
+ SimpleReaderBase::connect()
{
// Wait for the memory to become available and add it as dependency.
- ARMARX_IMPORTANT << "SimpleReaderBase: Waiting for memory '" << props.memoryName
- << "' ...";
+ ARMARX_IMPORTANT << "SimpleReaderBase: Waiting for memory '" << props.memoryName << "' ...";
try
{
- memoryReaderClient = memoryNameSystem.useReader(MemoryID().withMemoryName(props.memoryName));
- ARMARX_IMPORTANT << "SimpleReaderBase: Connected to memory '" << props.memoryName << "'";
+ memoryReaderClient =
+ memoryNameSystem.useReader(MemoryID().withMemoryName(props.memoryName));
+ ARMARX_IMPORTANT << "SimpleReaderBase: Connected to memory '" << props.memoryName
+ << "'";
}
catch (const armem::error::CouldNotResolveMemoryServer& e)
{
@@ -44,20 +43,20 @@ namespace armarx::armem::client::util
}
}
-
- std::mutex& SimpleReaderBase::memoryReaderMutex()
+ std::mutex&
+ SimpleReaderBase::memoryReaderMutex()
{
return memoryMutex;
}
-
- const armem::client::Reader& SimpleReaderBase::memoryReader() const
+ const armem::client::Reader&
+ SimpleReaderBase::memoryReader() const
{
return memoryReaderClient;
}
-
- const SimpleReaderBase::Properties& SimpleReaderBase::properties() const
+ const SimpleReaderBase::Properties&
+ SimpleReaderBase::properties() const
{
return props;
}
diff --git a/source/RobotAPI/libraries/armem/core/base/EntityBase.h b/source/RobotAPI/libraries/armem/core/base/EntityBase.h
index 7b6972a93bf7dcad9d804a03136869472b364082..1dbfeaebfa8eae9cfcf1cb54fc6017d6963b3ecf 100644
--- a/source/RobotAPI/libraries/armem/core/base/EntityBase.h
+++ b/source/RobotAPI/libraries/armem/core/base/EntityBase.h
@@ -17,7 +17,6 @@
#include "detail/lookup_mixins.h"
#include "detail/negative_index_semantics.h"
-
namespace armarx::armem::base
{
@@ -25,7 +24,7 @@ namespace armarx::armem::base
* @brief An entity over a period of time.
*
* An entity should be a physical thing or abstract concept existing
- * (and potentially evolving) over some time.
+ * (and potentially evolving) over time.
*
* Examples are:
* - objects (the green box)
@@ -42,16 +41,17 @@ namespace armarx::armem::base
*/
template <class _EntitySnapshotT, class _Derived>
class EntityBase :
- public detail::MemoryContainerBase<std::map<Time, _EntitySnapshotT>, _Derived>
- , public detail::ForEachEntityInstanceMixin<_Derived>
- , public detail::GetFindInstanceMixin<_Derived>
+ public detail::MemoryContainerBase<std::map<Time, _EntitySnapshotT>, _Derived>,
+ public detail::ForEachEntityInstanceMixin<_Derived>,
+ public detail::GetFindInstanceMixin<_Derived>,
+ public detail::GetLatestInstanceMixin<_Derived>,
+ public detail::GetLatestSnapshotMixin<_Derived>
{
using Base = detail::MemoryContainerBase<std::map<Time, _EntitySnapshotT>, _Derived>;
public:
-
- using typename Base::DerivedT;
using typename Base::ContainerT;
+ using typename Base::DerivedT;
using EntitySnapshotT = _EntitySnapshotT;
using EntityInstanceT = typename EntitySnapshotT::EntityInstanceT;
@@ -68,58 +68,69 @@ namespace armarx::armem::base
};
public:
-
EntityBase()
{
}
+
explicit EntityBase(const std::string& name, const MemoryID& parentID = {}) :
EntityBase(parentID.withEntityName(name))
{
}
- explicit EntityBase(const MemoryID& id) :
- Base(id)
+
+ explicit EntityBase(const MemoryID& id) : Base(id)
{
}
-
EntityBase(const EntityBase& other) = default;
EntityBase(EntityBase&& other) = default;
EntityBase& operator=(const EntityBase& other) = default;
EntityBase& operator=(EntityBase&& other) = default;
-
// READING
// Get key
- inline std::string& name()
+ inline std::string&
+ name()
{
return this->id().entityName;
}
- inline const std::string& name() const
+
+ inline const std::string&
+ name() const
{
return this->id().entityName;
}
+ /// Indicate whether the entity has any snapshots.
+ bool
+ hasSnapshots() const
+ {
+ return not this->empty();
+ }
- // Has child by key
- /// Indicates whether a history entry for the given time exists.
- bool hasSnapshot(const Time& time) const
+ // Has child with key
+ /// Indicate whether a snapshot at the given time exists.
+ bool
+ hasSnapshot(const Time& time) const
{
return this->findSnapshot(time) != nullptr;
}
- // Has child by MemoryID
- bool hasSnapshot(const MemoryID& snapshotID) const
+
+ // Has child with MemoryID
+ /// Indicate whether a snapshot with the given ID exists.
+ bool
+ hasSnapshot(const MemoryID& snapshotID) const
{
return this->findSnapshot(snapshotID) != nullptr;
}
-
// Find child via key
EntitySnapshotT*
findSnapshot(const Time& timestamp)
{
return detail::findChildByKey(timestamp, this->_container);
}
+
const EntitySnapshotT*
findSnapshot(const Time& timestamp) const
{
@@ -137,18 +148,21 @@ namespace armarx::armem::base
EntitySnapshotT&
getSnapshot(const Time& time)
{
- return detail::getChildByKey(time, this->_container, *this, [](const Time & time)
- {
- return toDateTimeMilliSeconds(time);
- });
+ return detail::getChildByKey(time,
+ this->_container,
+ *this,
+ [](const Time& time)
+ { return toDateTimeMilliSeconds(time); });
}
+
const EntitySnapshotT&
getSnapshot(const Time& time) const
{
- return detail::getChildByKey(time, this->_container, *this, [](const Time & time)
- {
- return toDateTimeMilliSeconds(time);
- });
+ return detail::getChildByKey(time,
+ this->_container,
+ *this,
+ [](const Time& time)
+ { return toDateTimeMilliSeconds(time); });
}
// Find child via MemoryID
@@ -158,6 +172,7 @@ namespace armarx::armem::base
detail::checkHasTimestamp(snapshotID);
return this->findSnapshot(snapshotID.timestamp);
}
+
const EntitySnapshotT*
findSnapshot(const MemoryID& snapshotID) const
{
@@ -172,12 +187,12 @@ namespace armarx::armem::base
detail::checkHasTimestamp(snapshotID);
return this->getSnapshot(snapshotID.timestamp);
}
+
const EntitySnapshotT&
getSnapshot(const MemoryID& snapshotID) const
{
detail::checkHasTimestamp(snapshotID);
return this->getSnapshot(snapshotID.timestamp);
-
}
// get/findInstance are provided by GetFindInstanceMixin
@@ -189,15 +204,18 @@ namespace armarx::armem::base
* @brief Get the latest timestamp.
* @throw `armem::error::EntityHistoryEmpty` If the history is empty.
*/
- Time getLatestTimestamp() const
+ Time
+ getLatestTimestamp() const
{
return this->getLatestSnapshot().time();
}
+
/**
* @brief Get the oldest timestamp.
* @throw `armem::error::EntityHistoryEmpty` If the history is empty.
*/
- Time getFirstTimestamp() const
+ Time
+ getFirstTimestamp() const
{
return this->getFirstSnapshot().time();
}
@@ -206,46 +224,30 @@ namespace armarx::armem::base
* @brief Return the snapshot with the most recent timestamp.
* @return The latest snapshot or nullptr if the entity is empty.
*/
- EntitySnapshotT* findLatestSnapshot()
- {
- return this->empty() ? nullptr : &this->_container.rbegin()->second;
- }
- const EntitySnapshotT* findLatestSnapshot() const
+ EntitySnapshotT*
+ findLatestSnapshot()
{
return this->empty() ? nullptr : &this->_container.rbegin()->second;
}
- /**
- * @brief Return the snapshot with the most recent timestamp.
- * @return The latest snapshot.
- * @throw `armem::error::EntityHistoryEmpty` If the history is empty.
- */
- EntitySnapshotT& getLatestSnapshot()
- {
- return const_cast<EntitySnapshotT&>(const_cast<const EntityBase*>(this)->getLatestSnapshot());
- }
- const EntitySnapshotT& getLatestSnapshot() const
+ const EntitySnapshotT*
+ findLatestSnapshot() const
{
- if (const EntitySnapshotT* snapshot = this->findLatestSnapshot())
- {
- return *snapshot;
- }
- else
- {
- throw armem::error::EntityHistoryEmpty(name(), "when getting the latest snapshot.");
- }
+ return this->empty() ? nullptr : &this->_container.rbegin()->second;
}
-
/**
* @brief Return the snapshot with the least recent timestamp.
* @return The first snapshot or nullptr if the entity is empty.
*/
- EntitySnapshotT* findFirstSnapshot()
+ EntitySnapshotT*
+ findFirstSnapshot()
{
return this->empty() ? nullptr : &this->_container.begin()->second;
}
- const EntitySnapshotT* findFirstSnapshot() const
+
+ const EntitySnapshotT*
+ findFirstSnapshot() const
{
return this->empty() ? nullptr : &this->_container.begin()->second;
}
@@ -255,11 +257,15 @@ namespace armarx::armem::base
* @return The first snapshot.
* @throw `armem::error::EntityHistoryEmpty` If the history is empty.
*/
- EntitySnapshotT& getFirstSnapshot()
+ EntitySnapshotT&
+ getFirstSnapshot()
{
- return const_cast<EntitySnapshotT&>(const_cast<const EntityBase*>(this)->getFirstSnapshot());
+ return const_cast<EntitySnapshotT&>(
+ const_cast<const EntityBase*>(this)->getFirstSnapshot());
}
- const EntitySnapshotT& getFirstSnapshot() const
+
+ const EntitySnapshotT&
+ getFirstSnapshot() const
{
if (const EntitySnapshotT* snapshot = this->findFirstSnapshot())
{
@@ -271,13 +277,13 @@ namespace armarx::armem::base
}
}
-
/**
* @brief Return the lastest snapshot before time.
* @param time The time.
* @return The latest snapshot < time or nullptr if none was found.
*/
- const EntitySnapshotT* findLatestSnapshotBefore(const Time& time) const
+ const EntitySnapshotT*
+ findLatestSnapshotBefore(const Time& time) const
{
if (this->empty())
{
@@ -309,7 +315,8 @@ namespace armarx::armem::base
* @param time The time.
* @return The latest snapshot <= time or nullptr if none was found.
*/
- const EntitySnapshotT* findLatestSnapshotBeforeOrAt(const Time& time) const
+ const EntitySnapshotT*
+ findLatestSnapshotBeforeOrAt(const Time& time) const
{
return findLatestSnapshotBefore(time + Duration::MicroSeconds(1));
}
@@ -319,7 +326,8 @@ namespace armarx::armem::base
* @param time The time.
* @return The first snapshot >= time or nullptr if none was found.
*/
- const EntitySnapshotT* findFirstSnapshotAfterOrAt(const Time& time) const
+ const EntitySnapshotT*
+ findFirstSnapshotAfterOrAt(const Time& time) const
{
// We want the leftmost element >= time.
// That's lower bound.
@@ -337,24 +345,28 @@ namespace armarx::armem::base
* @param time The time.
* @return The first snapshot >= time or nullptr if none was found.
*/
- const EntitySnapshotT* findFirstSnapshotAfter(const Time& time) const
+ const EntitySnapshotT*
+ findFirstSnapshotAfter(const Time& time) const
{
return findFirstSnapshotAfter(time - Duration::MicroSeconds(1));
}
-
- auto* findLatestInstance(int instanceIndex = 0)
+ auto*
+ findLatestInstance(int instanceIndex = 0)
{
auto* snapshot = this->findLatestSnapshot();
return snapshot ? snapshot->findInstance(instanceIndex) : nullptr;
}
- const auto* findLatestInstance(int instanceIndex = 0) const
+
+ const auto*
+ findLatestInstance(int instanceIndex = 0) const
{
auto* snapshot = this->findLatestSnapshot();
return snapshot ? snapshot->findInstance(instanceIndex) : nullptr;
}
-#if 0 // Do not offer this yet.
+
+#if 0 // Do not offer this yet.
auto* findLatestInstanceData(int instanceIndex = 0)
{
auto* instance = this->findLatestInstance(instanceIndex);
@@ -374,31 +386,38 @@ namespace armarx::armem::base
* @param func Function like: bool process(EntitySnapshotT& snapshot)
*/
template <class SnapshotFunctionT>
- bool forEachSnapshot(SnapshotFunctionT&& func)
+ bool
+ forEachSnapshot(SnapshotFunctionT&& func)
{
return this->forEachChild(func);
}
+
/**
* @param func Function like void process(const EntitySnapshotT& snapshot)
*/
template <class SnapshotFunctionT>
- bool forEachSnapshot(SnapshotFunctionT&& func) const
+ bool
+ forEachSnapshot(SnapshotFunctionT&& func) const
{
return this->forEachChild(func);
}
+
/**
* @param func Function like: bool process(EntitySnapshotT& snapshot)
*/
template <class SnapshotFunctionT>
- bool forEachSnapshotIn(const MemoryID& id, SnapshotFunctionT&& func)
+ bool
+ forEachSnapshotIn(const MemoryID& id, SnapshotFunctionT&& func)
{
return this->forEachChild(func);
}
+
/**
* @param func Function like void process(const EntitySnapshotT& snapshot)
*/
template <class SnapshotFunctionT>
- bool forEachSnapshotIn(const MemoryID& id, SnapshotFunctionT&& func) const
+ bool
+ forEachSnapshotIn(const MemoryID& id, SnapshotFunctionT&& func) const
{
if (id.hasTimestamp())
{
@@ -409,6 +428,7 @@ namespace armarx::armem::base
}
return this->forEachChild(func);
}
+
// forEachInstance() is provided by ForEachEntityInstanceMixin.
@@ -418,7 +438,8 @@ namespace armarx::armem::base
* @return The latest snapshots.
*/
template <class FunctionT>
- void forEachSnapshotBefore(const Time& time, FunctionT&& func) const
+ void
+ forEachSnapshotBefore(const Time& time, FunctionT&& func) const
{
for (const auto& [timestamp, snapshot] : this->_container)
{
@@ -439,12 +460,12 @@ namespace armarx::armem::base
* @return The latest snapshots.
*/
template <class FunctionT>
- void forEachSnapshotBeforeOrAt(const Time& time, FunctionT&& func) const
+ void
+ forEachSnapshotBeforeOrAt(const Time& time, FunctionT&& func) const
{
getSnapshotsBefore(time + Duration::MicroSeconds(1), func);
}
-
/**
* @brief Return all snapshots between, including, min and max.
* @param min The lowest time to include.
@@ -452,12 +473,15 @@ namespace armarx::armem::base
* @return The snapshots in [min, max].
*/
template <class FunctionT>
- void forEachSnapshotInTimeRange(const Time& min, const Time& max, FunctionT&& func) const
+ void
+ forEachSnapshotInTimeRange(const Time& min, const Time& max, FunctionT&& func) const
{
// Returns an iterator pointing to the first element that is not less than (i.e. greater or equal to) key.
- auto begin = min.toMicroSecondsSinceEpoch() > 0 ? this->_container.lower_bound(min) : this->_container.begin();
+ auto begin = min.toMicroSecondsSinceEpoch() > 0 ? this->_container.lower_bound(min)
+ : this->_container.begin();
// Returns an iterator pointing to the first element that is *greater than* key.
- auto end = max.toMicroSecondsSinceEpoch() > 0 ? this->_container.upper_bound(max) : this->_container.end();
+ auto end = max.toMicroSecondsSinceEpoch() > 0 ? this->_container.upper_bound(max)
+ : this->_container.end();
for (auto it = begin; it != end && it != this->_container.end(); ++it)
{
@@ -479,7 +503,8 @@ namespace armarx::armem::base
* @return The snapshots in [first, last].
*/
template <class FunctionT>
- void forEachSnapshotInIndexRange(long first, long last, FunctionT&& func) const
+ void
+ forEachSnapshotInIndexRange(long first, long last, FunctionT&& func) const
{
if (this->empty())
{
@@ -494,7 +519,7 @@ namespace armarx::armem::base
auto it = this->_container.begin();
std::advance(it, first_);
- size_t num = last_ - first_ + 1; // +1 to make last inclusive
+ size_t num = last_ - first_ + 1; // +1 to make last inclusive
for (size_t i = 0; i < num; ++i, ++it)
{
if (not call(func, it->second))
@@ -505,39 +530,44 @@ namespace armarx::armem::base
}
}
-
/**
* @param func Function like void process(EntityInstanceT& instance)>
*/
template <class InstanceFunctionT>
- bool forEachInstanceIn(const MemoryID& id, InstanceFunctionT&& func)
+ bool
+ forEachInstanceIn(const MemoryID& id, InstanceFunctionT&& func)
{
- return detail::forEachInstanceIn(
- id, func, *this,
- id.hasTimestamp(),
- id.hasTimestamp() ? this->findSnapshot(id.timestamp) : nullptr);
+ return detail::forEachInstanceIn(id,
+ func,
+ *this,
+ id.hasTimestamp(),
+ id.hasTimestamp() ? this->findSnapshot(id.timestamp)
+ : nullptr);
}
+
/**
* @param func Function like void process(EntityInstanceT& instance)>
*/
template <class InstanceFunctionT>
- bool forEachInstanceIn(const MemoryID& id, InstanceFunctionT&& func) const
+ bool
+ forEachInstanceIn(const MemoryID& id, InstanceFunctionT&& func) const
{
- return detail::forEachInstanceIn(
- id, func, *this,
- id.hasTimestamp(),
- id.hasTimestamp() ? this->findSnapshot(id.timestamp) : nullptr);
+ return detail::forEachInstanceIn(id,
+ func,
+ *this,
+ id.hasTimestamp(),
+ id.hasTimestamp() ? this->findSnapshot(id.timestamp)
+ : nullptr);
}
-
// Get child keys
/// @brief Get all timestamps in the history.
- std::vector<Time> getTimestamps() const
+ std::vector<Time>
+ getTimestamps() const
{
return simox::alg::get_keys(this->_container);
}
-
// MODIFICATION
/**
@@ -545,7 +575,8 @@ namespace armarx::armem::base
* @param update The update.
* @return The snapshot ID of the update.
*/
- UpdateResult update(const EntityUpdate& update)
+ UpdateResult
+ update(const EntityUpdate& update)
{
this->_checkContainerName(update.entityID.entityName, this->name());
UpdateResult ret;
@@ -572,59 +603,63 @@ namespace armarx::armem::base
return ret;
}
-
template <class OtherDerivedT>
- void append(const OtherDerivedT& other)
+ void
+ append(const OtherDerivedT& other)
{
- other.forEachSnapshot([this](const auto& snapshot)
- {
- auto it = this->_container.find(snapshot.time());
- if (it == this->_container.end())
+ other.forEachSnapshot(
+ [this](const auto& snapshot)
{
- EntitySnapshotT copy { snapshot };
- copy.id() = this->id().withTimestamp(snapshot.time()); // update id (e.g. memory name) if necessary
- this->_container.emplace(snapshot.time(), copy);
- }
- // else: snapshot already exists
- // We assume that a snapshot does not change, so ignore
- return true;
- });
+ auto it = this->_container.find(snapshot.time());
+ if (it == this->_container.end())
+ {
+ EntitySnapshotT copy{snapshot};
+ copy.id() = this->id().withTimestamp(
+ snapshot.time()); // update id (e.g. memory name) if necessary
+ this->_container.emplace(snapshot.time(), copy);
+ }
+ // else: snapshot already exists
+ // We assume that a snapshot does not change, so ignore
+ return true;
+ });
}
-
/// Add a snapshot at the given time.
- EntitySnapshotT& addSnapshot(const Time& timestamp)
+ EntitySnapshotT&
+ addSnapshot(const Time& timestamp)
{
return this->addSnapshot(timestamp, EntitySnapshotT(timestamp));
}
/// Copy and insert a snapshot
- EntitySnapshotT& addSnapshot(const EntitySnapshotT& snapshot)
+ EntitySnapshotT&
+ addSnapshot(const EntitySnapshotT& snapshot)
{
return this->addSnapshot(snapshot.time(), EntitySnapshotT(snapshot));
}
/// Move and insert a snapshot
- EntitySnapshotT& addSnapshot(EntitySnapshotT&& snapshot)
+ EntitySnapshotT&
+ addSnapshot(EntitySnapshotT&& snapshot)
{
- Time timestamp = snapshot.time(); // Copy before move.
+ Time timestamp = snapshot.time(); // Copy before move.
return this->addSnapshot(timestamp, std::move(snapshot));
}
/// Insert a snapshot in-place.
- template <class ...Args>
- EntitySnapshotT& addSnapshot(const Time& timestamp, Args... args)
+ template <class... Args>
+ EntitySnapshotT&
+ addSnapshot(const Time& timestamp, Args... args)
{
auto it = this->_container.emplace_hint(this->_container.end(), timestamp, args...);
it->second.id() = this->id().withTimestamp(timestamp);
return it->second;
}
-
-
// MISC
- bool equalsDeep(const DerivedT& other) const
+ bool
+ equalsDeep(const DerivedT& other) const
{
//std::cout << "Entity::equalsDeep" << std::endl;
if (this->size() != other.size())
@@ -645,15 +680,17 @@ namespace armarx::armem::base
return true;
}
- std::string getKeyString() const
+ std::string
+ getKeyString() const
{
return this->id().entityName;
}
- static std::string getLevelName()
+
+ static std::string
+ getLevelName()
{
return "entity";
}
-
};
-}
+} // namespace armarx::armem::base
diff --git a/source/RobotAPI/libraries/armem/core/base/EntityInstanceBase.h b/source/RobotAPI/libraries/armem/core/base/EntityInstanceBase.h
index 0bd03af1448d99536640c0b9f4a2f83036afc307..892c5db2ada839176eb42ff682e948d1d57b53a0 100644
--- a/source/RobotAPI/libraries/armem/core/base/EntityInstanceBase.h
+++ b/source/RobotAPI/libraries/armem/core/base/EntityInstanceBase.h
@@ -5,7 +5,6 @@
#include "detail/MemoryItem.h"
-
namespace armarx::armem::base
{
@@ -16,17 +15,19 @@ namespace armarx::armem::base
{
virtual ~NoData() = default;
- bool operator==(const NoData& other)
+ bool
+ operator==(const NoData& other)
{
return true;
}
- bool operator!=(const NoData& other)
+
+ bool
+ operator!=(const NoData& other)
{
return false;
}
};
-
/**
* @brief Metadata of an entity instance.
*/
@@ -62,7 +63,9 @@ namespace armarx::armem::base
void access() const;
bool operator==(const EntityInstanceMetadata& other) const;
- inline bool operator!=(const EntityInstanceMetadata& other) const
+
+ inline bool
+ operator!=(const EntityInstanceMetadata& other) const
{
return !(*this == other);
}
@@ -70,109 +73,123 @@ namespace armarx::armem::base
std::ostream& operator<<(std::ostream& os, const EntityInstanceMetadata& rhs);
-
-
/**
* @brief Data of a single entity instance.
*/
template <class _DataT = NoData, class _MetadataT = EntityInstanceMetadata>
- class EntityInstanceBase :
- public detail::MemoryItem
+ class EntityInstanceBase : public detail::MemoryItem
{
using Base = detail::MemoryItem;
public:
-
using MetadataT = _MetadataT;
using DataT = _DataT;
-
EntityInstanceBase()
{
}
+
explicit EntityInstanceBase(int index, const MemoryID& parentID = {}) :
EntityInstanceBase(parentID.withInstanceIndex(index))
{
}
- explicit EntityInstanceBase(const MemoryID& id) :
- Base(id)
+
+ explicit EntityInstanceBase(const MemoryID& id) : Base(id)
{
}
-
// Key
- inline int& index()
+ inline int&
+ index()
{
return id().instanceIndex;
}
- inline int index() const
+
+ inline int
+ index() const
{
return id().instanceIndex;
}
-
// Data
- MetadataT& metadata()
+ MetadataT&
+ metadata()
{
return _metadata;
}
- const MetadataT& metadata() const
+
+ const MetadataT&
+ metadata() const
{
return _metadata;
}
- const DataT& data() const
+ const DataT&
+ data() const
{
return _data;
}
- DataT& data()
+ DataT&
+ data()
{
return _data;
}
- void setData(DataT& d)
+ void
+ setData(const DataT& data)
{
- _data = d;
+ _data = data;
}
- void setMetadata(MetadataT& m)
+ void
+ setMetadata(const MetadataT& metadata)
{
- _metadata = m;
+ _metadata = metadata;
}
/**
* @brief Get the data converted to a generated Aron DTO class.
*/
template <class AronDtoT>
- AronDtoT dataAs() const
+ AronDtoT
+ dataAs() const
{
return AronDtoT::FromAron(_data);
}
+ template <class AronDtoT>
+ EntityInstanceBase<AronDtoT, MetadataT>
+ withDataAs() const
+ {
+ EntityInstanceBase<AronDtoT, MetadataT> instance{_id};
+ instance.data() = dataAs<AronDtoT>();
+ instance.metadata() = _metadata;
+ return instance;
+ }
// Misc
- static std::string getLevelName()
+ static std::string
+ getLevelName()
{
return "entity instance";
}
- std::string getKeyString() const
+ std::string
+ getKeyString() const
{
return std::to_string(index());
}
protected:
-
/// The metadata.
MetadataT _metadata;
/// The data. May be nullptr.
DataT _data;
-
};
-}
+} // namespace armarx::armem::base
diff --git a/source/RobotAPI/libraries/armem/core/base/EntitySnapshotBase.h b/source/RobotAPI/libraries/armem/core/base/EntitySnapshotBase.h
index b7048a6328990d6be2152c0559466cf1ff142b9e..2fb8bd3cc6ffcb217f7882e66fa2ed1e1b52d429 100644
--- a/source/RobotAPI/libraries/armem/core/base/EntitySnapshotBase.h
+++ b/source/RobotAPI/libraries/armem/core/base/EntitySnapshotBase.h
@@ -10,11 +10,11 @@
#include "detail/iteration_mixins.h"
#include "detail/lookup_mixins.h"
-
namespace armarx::armem::base::detail
{
void throwIfNotEqual(const Time& ownTime, const Time& updateTime);
}
+
namespace armarx::armem::base
{
/**
@@ -27,24 +27,23 @@ namespace armarx::armem::base
using Base = detail::MemoryContainerBase<std::vector<_EntityInstanceT>, _Derived>;
public:
-
- using typename Base::DerivedT;
using typename Base::ContainerT;
+ using typename Base::DerivedT;
using EntityInstanceT = _EntityInstanceT;
public:
-
EntitySnapshotBase()
{
}
+
explicit EntitySnapshotBase(Time time, const MemoryID& parentID = {}) :
EntitySnapshotBase(parentID.withTimestamp(time))
{
}
- explicit EntitySnapshotBase(const MemoryID& id) :
- Base(id)
+
+ explicit EntitySnapshotBase(const MemoryID& id) : Base(id)
{
}
@@ -53,43 +52,55 @@ namespace armarx::armem::base
EntitySnapshotBase& operator=(const EntitySnapshotBase& other) = default;
EntitySnapshotBase& operator=(EntitySnapshotBase&& other) = default;
-
// READING
// Get key
- inline Time& time()
+ inline Time&
+ time()
{
return this->id().timestamp;
}
- inline const Time& time() const
+
+ inline const Time&
+ time() const
{
return this->id().timestamp;
}
+ /// Indicate whether this snapshot has any instances.
+ bool
+ hasInstances() const
+ {
+ return not this->empty();
+ }
// Has child by key
- bool hasInstance(int index) const
+ bool
+ hasInstance(int index) const
{
return this->findInstance(index) != nullptr;
}
+
// Has child by ID
- bool hasInstance(const MemoryID& instanceID) const
+ bool
+ hasInstance(const MemoryID& instanceID) const
{
return this->findInstance(instanceID) != nullptr;
}
-
// Find child by key
- EntityInstanceT* findInstance(int index)
+ EntityInstanceT*
+ findInstance(int index)
{
- return const_cast<EntityInstanceT*>(const_cast<const EntitySnapshotBase*>(this)->findInstance(index));
+ return const_cast<EntityInstanceT*>(
+ const_cast<const EntitySnapshotBase*>(this)->findInstance(index));
}
- const EntityInstanceT* findInstance(int index) const
+
+ const EntityInstanceT*
+ findInstance(int index) const
{
const size_t si = static_cast<size_t>(index);
- return (index >= 0 && si < this->_container.size())
- ? &this->_container[si]
- : nullptr;
+ return (index >= 0 && si < this->_container.size()) ? &this->_container[si] : nullptr;
}
// Get child by key
@@ -102,8 +113,10 @@ namespace armarx::armem::base
EntityInstanceT&
getInstance(int index)
{
- return const_cast<EntityInstanceT&>(const_cast<const EntitySnapshotBase*>(this)->getInstance(index));
+ return const_cast<EntityInstanceT&>(
+ const_cast<const EntitySnapshotBase*>(this)->getInstance(index));
}
+
const EntityInstanceT&
getInstance(int index) const
{
@@ -113,7 +126,8 @@ namespace armarx::armem::base
}
else
{
- throw armem::error::MissingEntry::create<EntityInstanceT>(std::to_string(index), *this);
+ throw armem::error::MissingEntry::create<EntityInstanceT>(std::to_string(index),
+ *this);
}
}
@@ -124,6 +138,7 @@ namespace armarx::armem::base
detail::checkHasInstanceIndex(instanceID);
return this->findInstance(instanceID.instanceIndex);
}
+
const EntityInstanceT*
findInstance(const MemoryID& instanceID) const
{
@@ -145,6 +160,7 @@ namespace armarx::armem::base
detail::checkHasInstanceIndex(instanceID);
return this->getInstance(instanceID.instanceIndex);
}
+
const EntityInstanceT&
getInstance(const MemoryID& instanceID) const
{
@@ -152,22 +168,24 @@ namespace armarx::armem::base
return this->getInstance(instanceID.instanceIndex);
}
-
// ITERATION
/**
* @param func Function like void process(EntityInstanceT& instance)>
*/
template <class InstanceFunctionT>
- bool forEachInstance(InstanceFunctionT&& func)
+ bool
+ forEachInstance(InstanceFunctionT&& func)
{
return this->forEachChild(func);
}
+
/**
* @param func Function like void process (const EntityInstanceT& instance)
*/
template <class InstanceFunctionT>
- bool forEachInstance(InstanceFunctionT&& func) const
+ bool
+ forEachInstance(InstanceFunctionT&& func) const
{
return this->forEachChild(func);
}
@@ -176,7 +194,8 @@ namespace armarx::armem::base
* @param func Function like void process(EntityInstanceT& instance)>
*/
template <class InstanceFunctionT>
- bool forEachInstanceIn(const MemoryID& id, InstanceFunctionT&& func)
+ bool
+ forEachInstanceIn(const MemoryID& id, InstanceFunctionT&& func)
{
if (id.hasInstanceIndex())
{
@@ -188,11 +207,13 @@ namespace armarx::armem::base
return this->forEachInstance(func);
}
}
+
/**
* @param func Function like void process (const EntityInstanceT& instance)
*/
template <class InstanceFunctionT>
- bool forEachInstanceIn(const MemoryID& id, InstanceFunctionT&& func) const
+ bool
+ forEachInstanceIn(const MemoryID& id, InstanceFunctionT&& func) const
{
if (id.hasInstanceIndex())
{
@@ -205,9 +226,9 @@ namespace armarx::armem::base
}
}
-
// Get child keys
- std::vector<int> getInstanceIndices() const
+ std::vector<int>
+ getInstanceIndices() const
{
std::vector<int> indices;
indices.reserve(this->size());
@@ -218,10 +239,10 @@ namespace armarx::armem::base
return indices;
}
-
// MODIFICATION
- void update(const EntityUpdate& update)
+ void
+ update(const EntityUpdate& update)
{
detail::throwIfNotEqual(time(), update.referencedTime);
@@ -239,22 +260,30 @@ namespace armarx::armem::base
* @return The stored instance.
* @throw `armem::error::InvalidArgument` If the given index is invalid. Must be equal to container.size() or -1 (meaning push_back)
*/
- EntityInstanceT& addInstance(const EntityInstanceT& instance)
+ EntityInstanceT&
+ addInstance(const EntityInstanceT& instance)
{
return addInstance(EntityInstanceT(instance));
}
- EntityInstanceT& addInstance(EntityInstanceT&& instance)
+ EntityInstanceT&
+ addInstance(EntityInstanceT&& instance)
{
- if (instance.index() > 0 && static_cast<size_t>(instance.index()) < this->_container.size())
+ if (instance.index() > 0 &&
+ static_cast<size_t>(instance.index()) < this->_container.size())
{
- throw error::InvalidArgument(std::to_string(instance.index()), "EntitySnapshot::addInstance",
- "Cannot add an EntityInstance because its index already exists.");
+ throw error::InvalidArgument(
+ std::to_string(instance.index()),
+ "EntitySnapshot::addInstance",
+ "Cannot add an EntityInstance because its index already exists.");
}
- if (instance.index() > 0 && static_cast<size_t>(instance.index()) > this->_container.size())
+ if (instance.index() > 0 &&
+ static_cast<size_t>(instance.index()) > this->_container.size())
{
- throw error::InvalidArgument(std::to_string(instance.index()), "EntitySnapshot::addInstance",
- "Cannot add an EntityInstance because its index is too big.");
+ throw error::InvalidArgument(
+ std::to_string(instance.index()),
+ "EntitySnapshot::addInstance",
+ "Cannot add an EntityInstance because its index is too big.");
}
int index = static_cast<int>(this->_container.size());
@@ -263,7 +292,8 @@ namespace armarx::armem::base
return added;
}
- EntityInstanceT& addInstance()
+ EntityInstanceT&
+ addInstance()
{
int index = static_cast<int>(this->size());
EntityInstanceT& added = this->_container.emplace_back(EntityInstanceT());
@@ -271,10 +301,10 @@ namespace armarx::armem::base
return added;
}
-
// MISC
- bool equalsDeep(const DerivedT& other) const
+ bool
+ equalsDeep(const DerivedT& other) const
{
//std::cout << "EntitySnapshot::equalsDeep" << std::endl;
if (this->size() != other.size())
@@ -293,17 +323,17 @@ namespace armarx::armem::base
return true;
}
-
- std::string getKeyString() const
+ std::string
+ getKeyString() const
{
return toDateTimeMilliSeconds(this->time());
}
- static std::string getLevelName()
+ static std::string
+ getLevelName()
{
return "entity snapshot";
}
-
};
-}
+} // namespace armarx::armem::base
diff --git a/source/RobotAPI/libraries/armem/core/base/ProviderSegmentBase.h b/source/RobotAPI/libraries/armem/core/base/ProviderSegmentBase.h
index 6cd70391660d6f61fe073837052a9b06fb57da1c..ce5b8d7ea9a0ab258dabe04a3266f102ee9f4dbf 100644
--- a/source/RobotAPI/libraries/armem/core/base/ProviderSegmentBase.h
+++ b/source/RobotAPI/libraries/armem/core/base/ProviderSegmentBase.h
@@ -7,12 +7,11 @@
#include "EntityBase.h"
#include "detail/AronTyped.h"
-#include "detail/Predictive.h"
#include "detail/MemoryContainerBase.h"
+#include "detail/Predictive.h"
#include "detail/iteration_mixins.h"
#include "detail/lookup_mixins.h"
-
namespace armarx::armem::base
{
@@ -21,20 +20,19 @@ namespace armarx::armem::base
*/
template <class _EntityT, class _Derived>
class ProviderSegmentBase :
- public detail::MemoryContainerBase<std::map<std::string, _EntityT>, _Derived>
- , public detail::AronTyped
- , public detail::Predictive<_Derived>
- , public detail::ForEachEntityInstanceMixin<_Derived>
- , public detail::ForEachEntitySnapshotMixin<_Derived>
- , public detail::GetFindInstanceMixin<_Derived>
- , public detail::GetFindSnapshotMixin<_Derived>
+ public detail::MemoryContainerBase<std::map<std::string, _EntityT>, _Derived>,
+ public detail::AronTyped,
+ public detail::Predictive<_Derived>,
+ public detail::ForEachEntityInstanceMixin<_Derived>,
+ public detail::ForEachEntitySnapshotMixin<_Derived>,
+ public detail::GetFindInstanceMixin<_Derived>,
+ public detail::GetFindSnapshotMixin<_Derived>
{
using Base = detail::MemoryContainerBase<std::map<std::string, _EntityT>, _Derived>;
public:
-
- using typename Base::DerivedT;
using typename Base::ContainerT;
+ using typename Base::DerivedT;
using EntityT = _EntityT;
using EntitySnapshotT = typename EntityT::EntitySnapshotT;
@@ -50,39 +48,37 @@ namespace armarx::armem::base
std::vector<EntitySnapshotT> removedSnapshots;
UpdateResult() = default;
+
UpdateResult(const typename EntityT::UpdateResult& c) :
- entityUpdateType(c.entityUpdateType),
- id(c.id),
- removedSnapshots(c.removedSnapshots)
- {}
+ entityUpdateType(c.entityUpdateType), id(c.id), removedSnapshots(c.removedSnapshots)
+ {
+ }
};
public:
-
ProviderSegmentBase()
{
}
- explicit ProviderSegmentBase(
- const std::string& name,
- aron::type::ObjectPtr aronType = nullptr,
- const std::vector<PredictionEngine>& predictionEngines = {}) :
+ explicit ProviderSegmentBase(const std::string& name,
+ aron::type::ObjectPtr aronType = nullptr,
+ const std::vector<PredictionEngine>& predictionEngines = {}) :
ProviderSegmentBase(name, MemoryID(), aronType, predictionEngines)
{
}
- explicit ProviderSegmentBase(
- const std::string& name,
- const MemoryID parentID,
- aron::type::ObjectPtr aronType = nullptr,
- const std::vector<PredictionEngine>& predictionEngines = {}) :
+
+ explicit ProviderSegmentBase(const std::string& name,
+ const MemoryID parentID,
+ aron::type::ObjectPtr aronType = nullptr,
+ const std::vector<PredictionEngine>& predictionEngines = {}) :
ProviderSegmentBase(parentID.withProviderSegmentName(name), aronType, predictionEngines)
{
}
- explicit ProviderSegmentBase(
- const MemoryID id,
- aron::type::ObjectPtr aronType = nullptr,
- const std::vector<PredictionEngine>& predictionEngines = {}) :
+
+ explicit ProviderSegmentBase(const MemoryID id,
+ aron::type::ObjectPtr aronType = nullptr,
+ const std::vector<PredictionEngine>& predictionEngines = {}) :
Base(id), AronTyped(aronType), detail::Predictive<_Derived>(predictionEngines)
{
}
@@ -92,71 +88,86 @@ namespace armarx::armem::base
ProviderSegmentBase& operator=(const ProviderSegmentBase& other) = default;
ProviderSegmentBase& operator=(ProviderSegmentBase&& other) = default;
-
// READ ACCESS
// Get key
- inline std::string& name()
+ inline std::string&
+ name()
{
return this->id().providerSegmentName;
}
- inline const std::string& name() const
+
+ inline const std::string&
+ name() const
{
return this->id().providerSegmentName;
}
-
// Has child by key
- bool hasEntity(const std::string& name) const
+ bool
+ hasEntity(const std::string& name) const
{
return this->findEntity(name) != nullptr;
}
+
// Has child by ID
- bool hasEntity(const MemoryID& entityID) const
+ bool
+ hasEntity(const MemoryID& entityID) const
{
return this->findEntity(entityID) != nullptr;
}
-
// Find child by key
- EntityT* findEntity(const std::string& name)
+ EntityT*
+ findEntity(const std::string& name)
{
return detail::findChildByKey(name, this->_container);
}
- const EntityT* findEntity(const std::string& name) const
+
+ const EntityT*
+ findEntity(const std::string& name) const
{
return detail::findChildByKey(name, this->_container);
}
// Get child by key
- EntityT& getEntity(const std::string& name)
+ EntityT&
+ getEntity(const std::string& name)
{
return detail::getChildByKey(name, this->_container, *this);
}
- const EntityT& getEntity(const std::string& name) const
+
+ const EntityT&
+ getEntity(const std::string& name) const
{
return detail::getChildByKey(name, this->_container, *this);
}
// Find child by MemoryID
- EntityT* findEntity(const MemoryID& entityID)
+ EntityT*
+ findEntity(const MemoryID& entityID)
{
detail::checkHasEntityName(entityID);
return this->findEntity(entityID.entityName);
}
- const EntityT* findEntity(const MemoryID& entityID) const
+
+ const EntityT*
+ findEntity(const MemoryID& entityID) const
{
detail::checkHasEntityName(entityID);
return this->findEntity(entityID.entityName);
}
// Get child by MemoryID
- EntityT& getEntity(const MemoryID& entityID)
+ EntityT&
+ getEntity(const MemoryID& entityID)
{
detail::checkHasEntityName(entityID);
return this->getEntity(entityID.entityName);
}
- const EntityT& getEntity(const MemoryID& entityID) const
+
+ const EntityT&
+ getEntity(const MemoryID& entityID) const
{
detail::checkHasEntityName(entityID);
return this->getEntity(entityID.entityName);
@@ -174,15 +185,18 @@ namespace armarx::armem::base
* @param func Function like: bool process(EntityT& entity)
*/
template <class EntityFunctionT>
- bool forEachEntity(EntityFunctionT&& func)
+ bool
+ forEachEntity(EntityFunctionT&& func)
{
return this->forEachChild(func);
}
+
/**
* @param func Function like: bool process(const EntityT& entity)
*/
template <class EntityFunctionT>
- bool forEachEntity(EntityFunctionT&& func) const
+ bool
+ forEachEntity(EntityFunctionT&& func) const
{
return this->forEachChild(func);
}
@@ -194,33 +208,39 @@ namespace armarx::armem::base
* @param func Function like void process(EntityInstanceT& instance)>
*/
template <class InstanceFunctionT>
- bool forEachInstanceIn(const MemoryID& id, InstanceFunctionT&& func)
+ bool
+ forEachInstanceIn(const MemoryID& id, InstanceFunctionT&& func)
{
- return detail::forEachInstanceIn(
- id, func, *this,
- id.hasEntityName(),
- id.hasEntityName() ? this->findEntity(id.entityName) : nullptr);
+ return detail::forEachInstanceIn(id,
+ func,
+ *this,
+ id.hasEntityName(),
+ id.hasEntityName() ? this->findEntity(id.entityName)
+ : nullptr);
}
+
/**
* @param func Function like void process(EntityInstanceT& instance)>
*/
template <class InstanceFunctionT>
- bool forEachInstanceIn(const MemoryID& id, InstanceFunctionT&& func) const
+ bool
+ forEachInstanceIn(const MemoryID& id, InstanceFunctionT&& func) const
{
- return detail::forEachInstanceIn(
- id, func, *this,
- id.hasEntityName(),
- id.hasEntityName() ? this->findEntity(id.entityName) : nullptr);
+ return detail::forEachInstanceIn(id,
+ func,
+ *this,
+ id.hasEntityName(),
+ id.hasEntityName() ? this->findEntity(id.entityName)
+ : nullptr);
}
-
// Get child keys
- std::vector<std::string> getEntityNames() const
+ std::vector<std::string>
+ getEntityNames() const
{
return simox::alg::get_keys(this->_container);
}
-
// MODIFICATION
/**
@@ -228,7 +248,8 @@ namespace armarx::armem::base
*
* Missing entity entries are added before updating.
*/
- UpdateResult update(const EntityUpdate& update)
+ UpdateResult
+ update(const EntityUpdate& update)
{
this->_checkContainerName(update.entityID.providerSegmentName, this->name());
@@ -253,55 +274,61 @@ namespace armarx::armem::base
return ret;
}
-
template <class OtherDerivedT>
- void append(const OtherDerivedT& other)
+ void
+ append(const OtherDerivedT& other)
{
- other.forEachEntity([this](const auto& entity)
- {
- auto it = this->_container.find(entity.name());
- if (it == this->_container.end())
+ other.forEachEntity(
+ [this](const auto& entity)
{
- it = this->_container.emplace(entity.name(), this->id().withEntityName(entity.name())).first;
- }
- it->second.append(entity);
- return true;
- });
+ auto it = this->_container.find(entity.name());
+ if (it == this->_container.end())
+ {
+ it = this->_container
+ .emplace(entity.name(), this->id().withEntityName(entity.name()))
+ .first;
+ }
+ it->second.append(entity);
+ return true;
+ });
}
-
/// Add an empty entity with the given name.
- EntityT& addEntity(const std::string& name)
+ EntityT&
+ addEntity(const std::string& name)
{
return this->_derived().addEntity(name, name);
}
/// Copy and insert an entity.
- EntityT& addEntity(const EntityT& entity)
+ EntityT&
+ addEntity(const EntityT& entity)
{
return this->_derived().addEntity(entity.name(), EntityT(entity));
}
/// Move and insert an entity.
- EntityT& addEntity(EntityT&& entity)
+ EntityT&
+ addEntity(EntityT&& entity)
{
- const std::string name = entity.name(); // Copy before move.
+ const std::string name = entity.name(); // Copy before move.
return this->_derived().addEntity(name, std::move(entity));
}
/// Insert an entity in-place.
- template <class ...Args>
- EntityT& addEntity(const std::string& name, Args... args)
+ template <class... Args>
+ EntityT&
+ addEntity(const std::string& name, Args... args)
{
ChildT& child = this->template _addChild<ChildT>(name, args...);
child.id() = this->id().withEntityName(name);
return child;
}
-
// MISC
- bool equalsDeep(const DerivedT& other) const
+ bool
+ equalsDeep(const DerivedT& other) const
{
if (this->size() != other.size())
{
@@ -322,17 +349,17 @@ namespace armarx::armem::base
return true;
}
-
- static std::string getLevelName()
+ static std::string
+ getLevelName()
{
return "provider segment";
}
- std::string getKeyString() const
+ std::string
+ getKeyString() const
{
return this->name();
}
-
};
-}
+} // namespace armarx::armem::base
diff --git a/source/RobotAPI/libraries/armem/core/base/detail/iteration_mixins.h b/source/RobotAPI/libraries/armem/core/base/detail/iteration_mixins.h
index b686b1cc7b304e780579808a6bd3b3f4d1b50ff9..9f5190364d66b999fa9d15d64bf9c12454d05715 100644
--- a/source/RobotAPI/libraries/armem/core/base/detail/iteration_mixins.h
+++ b/source/RobotAPI/libraries/armem/core/base/detail/iteration_mixins.h
@@ -5,13 +5,14 @@
#include <RobotAPI/libraries/armem/core/MemoryID.h>
+#include "derived.h"
namespace
{
- template<typename F, typename Ret, typename A, typename... Rest>
+ template <typename F, typename Ret, typename A, typename... Rest>
A helper(Ret (F::*)(A, Rest...));
- template<typename F, typename Ret, typename A, typename... Rest>
+ template <typename F, typename Ret, typename A, typename... Rest>
A helper(Ret (F::*)(A, Rest...) const);
// volatile or lvalue/rvalue *this not required for lambdas (phew)
@@ -19,12 +20,12 @@ namespace
template <typename FuncT>
struct first_argument
{
- using type = decltype( helper(&FuncT::operator()) );
+ using type = decltype(helper(&FuncT::operator()));
};
template <typename FuncT>
using first_argument_t = typename first_argument<FuncT>::type;
-}
+} // namespace
namespace armarx::armem::base::detail
{
@@ -33,9 +34,10 @@ namespace armarx::armem::base::detail
// Handle functions with different return type.
template <class FunctionT, class ChildT>
- bool call(FunctionT&& func, ChildT&& child)
+ bool
+ call(FunctionT&& func, ChildT&& child)
{
- if constexpr(std::is_same_v<decltype(func(child)), bool>)
+ if constexpr (std::is_same_v<decltype(func(child)), bool>)
{
if (!func(child))
{
@@ -50,10 +52,10 @@ namespace armarx::armem::base::detail
}
}
-
// Single-valued containers.
template <class ContainerT, class FunctionT>
- bool forEachChildSingle(ContainerT& container, FunctionT&& func)
+ bool
+ forEachChildSingle(ContainerT& container, FunctionT&& func)
{
for (auto& child : container)
{
@@ -65,10 +67,10 @@ namespace armarx::armem::base::detail
return true;
}
-
// Pair-valued containers.
template <class ContainerT, class FunctionT>
- bool forEachChildPair(ContainerT& container, FunctionT&& func)
+ bool
+ forEachChildPair(ContainerT& container, FunctionT&& func)
{
for (auto& [_, child] : container)
{
@@ -80,22 +82,25 @@ namespace armarx::armem::base::detail
return true;
}
-
// see: https://en.cppreference.com/w/cpp/types/void_t
// primary template handles types that have no nested ::type member:
- template< class, class = void >
- struct has_mapped_type : std::false_type { };
+ template <class, class = void>
+ struct has_mapped_type : std::false_type
+ {
+ };
// specialization recognizes types that do have a nested ::type member:
- template< class T >
- struct has_mapped_type<T, std::void_t<typename T::mapped_type>> : std::true_type { };
-
+ template <class T>
+ struct has_mapped_type<T, std::void_t<typename T::mapped_type>> : std::true_type
+ {
+ };
template <class ContainerT, class FunctionT>
- bool forEachChild(ContainerT& container, FunctionT&& func)
+ bool
+ forEachChild(ContainerT& container, FunctionT&& func)
{
- if constexpr(has_mapped_type<ContainerT>::value)
+ if constexpr (has_mapped_type<ContainerT>::value)
{
return forEachChildPair(container, func);
}
@@ -105,20 +110,15 @@ namespace armarx::armem::base::detail
}
}
-
template <class FunctionT, class ParentT, class ChildT>
- bool forEachInstanceIn(
- const MemoryID& id,
- FunctionT&& func,
- ParentT& parent,
- bool single,
- ChildT* child
- )
+ bool
+ forEachInstanceIn(const MemoryID& id,
+ FunctionT&& func,
+ ParentT& parent,
+ bool single,
+ ChildT* child)
{
- auto childFn = [&id,&func](auto& child)
- {
- return child.forEachInstanceIn(id, func);
- };
+ auto childFn = [&id, &func](auto& child) { return child.forEachInstanceIn(id, func); };
if (single)
{
return child ? childFn(*child) : true;
@@ -129,8 +129,6 @@ namespace armarx::armem::base::detail
}
}
-
-
// We use auto instead of, e.g. DerivedT::EntitySnapshotT,
// as we cannot use the typedef before DerivedT was completely defined.
@@ -143,33 +141,29 @@ namespace armarx::armem::base::detail
* @param func Function like: bool process(EntityInstanceT& instance)>
*/
template <class InstanceFunctionT>
- bool forEachInstance(InstanceFunctionT&& func)
+ bool
+ forEachInstance(InstanceFunctionT&& func)
{
- return static_cast<DerivedT*>(this)->forEachSnapshot(
- [&func](auto & snapshot) -> bool
- {
- return snapshot.forEachInstance(func);
- });
+ return derived<DerivedT>(this).forEachSnapshot(
+ [&func](auto& snapshot) -> bool { return snapshot.forEachInstance(func); });
}
/**
* @param func Function like: bool process(const EntityInstanceT& instance)
*/
template <class InstanceFunctionT>
- bool forEachInstance(InstanceFunctionT&& func) const
+ bool
+ forEachInstance(InstanceFunctionT&& func) const
{
- return static_cast<const DerivedT*>(this)->forEachSnapshot(
- [&func](const auto & snapshot) -> bool
- {
- return snapshot.forEachInstance(func);
- });
+ return derived<DerivedT>(this).forEachSnapshot(
+ [&func](const auto& snapshot) -> bool { return snapshot.forEachInstance(func); });
}
/**
- * Call `func` on the data of each instances converted to Aron DTO class.
+ * @brief Call `func` on the data of each instance converted to Aron DTO class.
*
* @code
- * ().forEachEntityInstanceAs([](const my::arondto::CoolData& data)
+ * ().forEachEntityInstanceAs([](my::arondto::CoolData data)
* {
* ...
* });
@@ -177,21 +171,54 @@ namespace armarx::armem::base::detail
*
* The Aron DTO type is deduced from the passed function's first argument.
*
- * @param func Function like: `bool process(const my::arondto::CoolData& data)`
+ * @param func Function like: `bool process(my::arondto::CoolData data)`
*/
template <class AronDtoFunctionT>
- bool forEachInstanceAs(AronDtoFunctionT&& func) const
+ bool
+ forEachInstanceAs(AronDtoFunctionT&& func) const
{
- return static_cast<const DerivedT*>(this)->forEachInstance(
- [&func](const auto & instance) -> bool
- {
- using AronDtoT = typename std::remove_reference_t<first_argument_t<AronDtoFunctionT>>;
- return func(instance.template dataAs<AronDtoT>());
- });
+ using AronDtoT = typename std::remove_const_t<
+ std::remove_reference_t<first_argument_t<AronDtoFunctionT>>>;
+
+ return derived<DerivedT>(this).forEachInstance(
+ [&func](const auto& instance)
+ { return func(instance.template dataAs<AronDtoT>()); });
}
- };
+ /**
+ * @brief Call `func` on each instance with its data converted to Aron DTO class.
+ *
+ * @code
+ * ().forEachEntityInstanceWithDataAs(
+ * [](armem::wm::EntityInstanceBase<my::arondto::CoolData> instance)
+ * {
+ * const armarx::armem::MemoryID id = instance.id();
+ * const armarx::DateTime timestamp = instance.id().timestamp;
+ * const armarx::arondto::Duration& data = instance.data();
+ * ...
+ * });
+ * @endcode
+ *
+ * Compared to forEachInstanceAs(), this function allows accessing the full metadata of the
+ * instances in addition to the payload data converted to the ARON DTO.
+ *
+ * The Aron DTO type is deduced from the passed function's signature.
+ *
+ * @param func Function like:
+ * `bool process(armem::wm::EntityInstanceBase<my::arondto::CoolData> instance)`
+ */
+ template <class EntityInstanceBaseAronDtoFunctionT>
+ bool
+ forEachInstanceWithDataAs(EntityInstanceBaseAronDtoFunctionT&& func) const
+ {
+ using AronDtoT = typename std::remove_reference_t<
+ first_argument_t<EntityInstanceBaseAronDtoFunctionT>>::DataT;
+ return derived<DerivedT>(this).forEachInstance(
+ [&func](const auto& instance)
+ { return func(instance.template withDataAs<AronDtoT>()); });
+ }
+ };
template <class DerivedT>
struct ForEachEntitySnapshotMixin
@@ -200,30 +227,25 @@ namespace armarx::armem::base::detail
* @param func Function like: bool process(EntitySnapshotT& snapshot)>
*/
template <class SnapshotFunctionT>
- bool forEachSnapshot(SnapshotFunctionT&& func)
+ bool
+ forEachSnapshot(SnapshotFunctionT&& func)
{
- return static_cast<DerivedT*>(this)->forEachEntity(
- [&func](auto & entity) -> bool
- {
- return entity.forEachSnapshot(func);
- });
+ return derived<DerivedT>(this).forEachEntity([&func](auto& entity) -> bool
+ { return entity.forEachSnapshot(func); });
}
/**
* @param func Function like: bool process(const EntitySnapshotT& snapshot)
*/
template <class SnapshotFunctionT>
- bool forEachSnapshot(SnapshotFunctionT&& func) const
+ bool
+ forEachSnapshot(SnapshotFunctionT&& func) const
{
- return static_cast<const DerivedT*>(this)->forEachEntity(
- [&func](const auto & entity) -> bool
- {
- return entity.forEachSnapshot(func);
- });
+ return derived<DerivedT>(this).forEachEntity([&func](const auto& entity) -> bool
+ { return entity.forEachSnapshot(func); });
}
};
-
template <class DerivedT>
struct ForEachEntityMixin
{
@@ -231,30 +253,27 @@ namespace armarx::armem::base::detail
* @param func Function like: bool process(EntityT& entity)>
*/
template <class FunctionT>
- bool forEachEntity(FunctionT&& func)
+ bool
+ forEachEntity(FunctionT&& func)
{
- return static_cast<DerivedT*>(this)->forEachProviderSegment(
- [&func](auto & providerSegment) -> bool
- {
- return providerSegment.forEachEntity(func);
- });
+ return derived<DerivedT>(this).forEachProviderSegment(
+ [&func](auto& providerSegment) -> bool
+ { return providerSegment.forEachEntity(func); });
}
/**
* @param func Function like: bool process(const EntityT& entity)
*/
template <class FunctionT>
- bool forEachEntity(FunctionT&& func) const
+ bool
+ forEachEntity(FunctionT&& func) const
{
- return static_cast<const DerivedT*>(this)->forEachProviderSegment(
- [&func](const auto & providerSegment) -> bool
- {
- return providerSegment.forEachEntity(func);
- });
+ return derived<DerivedT>(this).forEachProviderSegment(
+ [&func](const auto& providerSegment) -> bool
+ { return providerSegment.forEachEntity(func); });
}
};
-
template <class DerivedT>
struct ForEachProviderSegmentMixin
{
@@ -262,27 +281,25 @@ namespace armarx::armem::base::detail
* @param func Function like: bool process(ProviderSegmentT& providerSegment)>
*/
template <class FunctionT>
- bool forEachProviderSegment(FunctionT&& func)
+ bool
+ forEachProviderSegment(FunctionT&& func)
{
- return static_cast<DerivedT*>(this)->forEachCoreSegment(
- [&func](auto & coreSegment) -> bool
- {
- return coreSegment.forEachProviderSegment(func);
- });
+ return derived<DerivedT>(this).forEachCoreSegment(
+ [&func](auto& coreSegment) -> bool
+ { return coreSegment.forEachProviderSegment(func); });
}
/**
* @param func Function like: bool process(const ProviderSegmentT& providerSegment)
*/
template <class FunctionT>
- bool forEachProviderSegment(FunctionT&& func) const
+ bool
+ forEachProviderSegment(FunctionT&& func) const
{
- return static_cast<const DerivedT*>(this)->forEachCoreSegment(
- [&func](const auto & coreSegment) -> bool
- {
- return coreSegment.forEachProviderSegment(func);
- });
+ return derived<DerivedT>(this).forEachCoreSegment(
+ [&func](const auto& coreSegment) -> bool
+ { return coreSegment.forEachProviderSegment(func); });
}
};
-}
+} // namespace armarx::armem::base::detail
diff --git a/source/RobotAPI/libraries/armem/core/base/detail/lookup_mixins.h b/source/RobotAPI/libraries/armem/core/base/detail/lookup_mixins.h
index ce63b1ee78d4dbc97fe99b9efddd783dc6e5e315..204b1644d15217b028592c2bebb94cb874993ccf 100644
--- a/source/RobotAPI/libraries/armem/core/base/detail/lookup_mixins.h
+++ b/source/RobotAPI/libraries/armem/core/base/detail/lookup_mixins.h
@@ -8,13 +8,25 @@
namespace armarx::armem::base::detail
{
+ /// Throw armem::error::InvalidMemoryID if the given ID has no instance index.
void checkHasInstanceIndex(const MemoryID& instanceID);
+ /// Throw armem::error::InvalidMemoryID if the given ID has no timestamp.
void checkHasTimestamp(const MemoryID& snapshotID);
+ /// Throw armem::error::InvalidMemoryID if the given ID has no entity name.
void checkHasEntityName(const MemoryID& entityID);
+ /// Throw armem::error::InvalidMemoryID if the given ID has provider segment name.
void checkHasProviderSegmentName(const MemoryID& providerSegmentID);
+ /// Throw armem::error::InvalidMemoryID if the given ID has core segment name.
void checkHasCoreSegmentName(const MemoryID& coreSegmentID);
+ /// Throw armem::error::InvalidMemoryID if the given ID has memory name.
void checkHasMemoryName(const MemoryID& memory);
+ /**
+ * @brief Find a child in a container by its key.
+ * @param key The child's key.
+ * @param container The child's container.
+ * @return A pointer to the child, or nullptr if it was not found.
+ */
template <class KeyT, class ContainerT>
auto*
findChildByKey(const KeyT& key, ContainerT&& container)
@@ -23,11 +35,20 @@ namespace armarx::armem::base::detail
return it != container.end() ? &it->second : nullptr;
}
+ /**
+ * @brief Retrieve a child in a container by its key.
+ * @param key The child's key.
+ * @param container The container.
+ * @param parent The container's owner. Used for the error message.
+ * @param keyStringFn A function which turns key into a string. Used for the error message.
+ * @return A reference to the child.
+ * @throw armem::error::ArMemError If the child was not found.
+ */
template <class KeyT, class ContainerT, class ParentT, class KeyStringFn>
auto&
getChildByKey(const KeyT& key,
ContainerT&& container,
- const ParentT& parent,
+ const ParentT& owner,
KeyStringFn&& keyStringFn)
{
if (auto* child = findChildByKey(key, container))
@@ -37,10 +58,18 @@ namespace armarx::armem::base::detail
else
{
throw armem::error::MissingEntry::create<typename ParentT::ChildT>(keyStringFn(key),
- parent);
+ owner);
}
}
+ /**
+ * @brief Retrieve a child in a container by its key.
+ * @param key The child's key.
+ * @param container The container.
+ * @param parent The container's owner. Used for the error message.
+ * @return A reference to the child.
+ * @throw armem::error::ArMemError If the child was not found.
+ */
template <class KeyT, class ContainerT, class ParentT>
auto&
getChildByKey(const KeyT& key, ContainerT&& container, const ParentT& parent)
@@ -51,8 +80,30 @@ namespace armarx::armem::base::detail
template <class DerivedT>
struct GetFindInstanceMixin
{
- // Relies on this->find/getSnapshot()
+ // Relies on this->find/get/forEachInstance()
+ /**
+ * @brief Indicate whether this container contains at least one entity instance.
+ * @return True if there is at least one entity instance.
+ */
+ bool
+ hasInstances() const
+ {
+ bool has = false;
+ derived<DerivedT>(this).forEachInstance(
+ [&has](const auto& snapshot)
+ {
+ has = true;
+ return false;
+ });
+ return has;
+ }
+
+ /**
+ * @brief Indicate whether this container has an instance with the given ID.
+ * @param instanceID The instance ID.
+ * @return ... WIP
+ */
bool
hasInstance(const MemoryID& instanceID) const
{
@@ -82,7 +133,7 @@ namespace armarx::armem::base::detail
* @brief Retrieve an entity instance.
* @param id The instance ID.
* @return The instance if it is found.
- * @throw `armem::error::ArMemError` if it is missing.
+ * @throw armem::error::ArMemError if it is missing.
*/
auto&
getInstance(const MemoryID& instanceID)
@@ -98,20 +149,118 @@ namespace armarx::armem::base::detail
};
template <class DerivedT>
- struct GetFindSnapshotMixin
+ struct GetLatestInstanceMixin
{
- // Relies on this->find/getEntity()
+ // Relies on findLatestInstance()
+
+ /**
+ * @brief Retrieve the latest entity instance.
+ * @param instanceIndex The instance's index in the latest snapshot.
+ * @return The latest entity instance.
+ * @throw armem::error::ArMemError If there is no entity instance.
+ */
+ auto&
+ getLatestInstance(int instanceIndex = 0)
+ {
+ auto* instance = derived<DerivedT>(this).findLatestInstance();
+ if (not instance)
+ {
+ throw armem::error::NoSuchEntries(
+ "entity instances", DerivedT::getLevelName(), derived<DerivedT>(this).id());
+ }
+ return *instance;
+ }
+
+ const auto&
+ getLatestInstance(int instanceIndex = 0) const
+ {
+ const auto* instance = derived<DerivedT>(this).findLatestInstance();
+ if (not instance)
+ {
+ throw armem::error::NoSuchEntries(
+ "entity instances", DerivedT::getLevelName(), derived<DerivedT>(this).id());
+ }
+ return *instance;
+ }
+ };
+
+ template <class DerivedT>
+ struct GetLatestSnapshotMixin
+ {
+ // Relies on findLatestSnapshot()
+
+ /**
+ * @brief Retrieve the latest entity snapshot.
+ * @param snapshotIndex The snapshot's index in the latest snapshot.
+ * @return The latest entity snapshot.
+ * @throw armem::error::ArMemError If there is no entity snapshot.
+ */
+ auto&
+ getLatestSnapshot(int snapshotIndex = 0)
+ {
+ auto* snapshot = derived<DerivedT>(this).findLatestSnapshot();
+ if (not snapshot)
+ {
+ throw armem::error::NoSuchEntries(
+ "entity snapshots", DerivedT::getLevelName(), derived<DerivedT>(this).id());
+ }
+ return *snapshot;
+ }
+
+ const auto&
+ getLatestSnapshot(int snapshotIndex = 0) const
+ {
+ const auto* snapshot = derived<DerivedT>(this).findLatestSnapshot();
+ if (not snapshot)
+ {
+ throw armem::error::NoSuchEntries(
+ "entity snapshots", DerivedT::getLevelName(), derived<DerivedT>(this).id());
+ }
+ return *snapshot;
+ }
+ };
+
+ template <class DerivedT>
+ struct GetFindSnapshotMixin :
+ public GetLatestInstanceMixin<DerivedT>,
+ public GetLatestSnapshotMixin<DerivedT>
+ {
+ // Relies on this->find/getEntity, forEachSnapshot()
+
+ /**
+ * @brief Indicate whether this container contains at least one entity snapshot.
+ * @return True if there is at least one entity snapshot.
+ */
+ bool
+ hasSnapshots() const
+ {
+ bool has = false;
+ derived<DerivedT>(this).forEachSnapshot(
+ [&has](const auto& snapshot)
+ {
+ has = true;
+ return false;
+ });
+ return has;
+ }
+ /**
+ * @brief Indicates whether a snapshot with the given ID exists.
+ * @param snapshotID The snapshot ID.
+ * @return True if the snapshot exists, false otherwise.
+ */
bool
hasSnapshot(const MemoryID& snapshotID) const
{
return derived<DerivedT>(this).findSnapshot(snapshotID) != nullptr;
}
+ // Snapshot by ID
+
/**
* @brief Find an entity snapshot.
* @param id The snapshot ID.
- * @return The snapshot or nullptr if it is missing.
+ * @return The snapshot, or nullptr if it is missing.
*/
auto*
findSnapshot(const MemoryID& snapshotID)
@@ -131,7 +280,7 @@ namespace armarx::armem::base::detail
* @brief Retrieve an entity snapshot.
* @param id The snapshot ID.
* @return The snapshot if it is found.
- * @throw `armem::error::ArMemError` if it is missing.
+ * @throw armem::error::ArMemError if it is missing.
*/
auto&
getSnapshot(const MemoryID& snapshotID)
@@ -145,8 +294,60 @@ namespace armarx::armem::base::detail
return derived<DerivedT>(this).getEntity(snapshotID).getSnapshot(snapshotID);
}
- // More elaborate cases
+ // Latest snapshot in container.
+
+ /**
+ * @brief Find the latest entity snapshot.
+ * @return A pointer to the latest instance, or nullptr if there is no snapshot.
+ */
+ const auto*
+ findLatestSnapshot() const
+ {
+ const typename DerivedT::EntitySnapshotT* latestSnapshot = nullptr;
+ derived<DerivedT>(this).forEachEntity(
+ [&latestSnapshot](const auto& entity)
+ {
+ const auto* snapshot = entity.findLatestSnapshot();
+ if (latestSnapshot == nullptr)
+ {
+ latestSnapshot = snapshot;
+ }
+ else if (snapshot and snapshot->time() > latestSnapshot->time())
+ {
+ latestSnapshot = snapshot;
+ }
+ });
+ return latestSnapshot;
+ }
+ auto*
+ findLatestSnapshot()
+ {
+ typename DerivedT::EntitySnapshotT* latestSnapshot = nullptr;
+ derived<DerivedT>(this).forEachEntity(
+ [&latestSnapshot](auto& entity)
+ {
+ auto* snapshot = entity.findLatestSnapshot();
+ if (latestSnapshot == nullptr)
+ {
+ latestSnapshot = snapshot;
+ }
+ else if (snapshot and snapshot->time() > latestSnapshot->time())
+ {
+ latestSnapshot = snapshot;
+ }
+ });
+ return latestSnapshot;
+ }
+
+ // Latest snapshot in entity
+
+ /**
+ * @brief Find the latest entity snapshot in the given entity.
+ * @param entityID The entity's ID.
+ * @return A pointer to the latest snapshot in the specified entity, or nullptr if the
+ * entity does not exist or it has no snapshot.
+ */
auto*
findLatestSnapshot(const MemoryID& entityID)
{
@@ -161,6 +362,34 @@ namespace armarx::armem::base::detail
return entity ? entity->findLatestSnapshot() : nullptr;
}
+ // Latest instance in container.
+
+ /**
+ * @brief Find the latest entity instance.
+ * @return A pointer to the latest instance, or nullptr if there is no instance.
+ */
+ const auto*
+ findLatestInstance(int instanceIndex = 0) const
+ {
+ auto* snapshot = derived<DerivedT>(this).findLatestSnapshot();
+ return snapshot ? snapshot->findInstance(instanceIndex) : nullptr;
+ }
+
+ auto*
+ findLatestInstance(int instanceIndex = 0)
+ {
+ auto* snapshot = derived<DerivedT>(this).findLatestSnapshot();
+ return snapshot ? snapshot->findInstance(instanceIndex) : nullptr;
+ }
+
+ // Latest instance in entity.
+
+ /**
+ * @brief Find the latest entity instance in the given entity.
+ * @param entityID The entity's ID.
+ * @return A pointer to the latest instance in the specified entity, or nullptr if the
+ * entity does not exist or it has no instance.
+ */
auto*
findLatestInstance(const MemoryID& entityID, int instanceIndex = 0)
{
@@ -210,7 +439,7 @@ namespace armarx::armem::base::detail
* @brief Retrieve an entity.
* @param id The entity ID.
* @return The entity if it is found.
- * @throw `armem::error::ArMemError` if it is missing.
+ * @throw armem::error::ArMemError if it is missing.
*/
auto&
getEntity(const MemoryID& entityID)
@@ -259,7 +488,7 @@ namespace armarx::armem::base::detail
* @brief Retrieve a provider segment.
* @param id The provider segment ID.
* @return The provider segment if it is found.
- * @throw `armem::error::ArMemError` if it is missing.
+ * @throw armem::error::ArMemError if it is missing.
*/
auto&
getProviderSegment(const MemoryID& providerSegmentID)
diff --git a/source/RobotAPI/libraries/armem/core/base/ice_conversions.h b/source/RobotAPI/libraries/armem/core/base/ice_conversions.h
index e808e01ace5165eb3fb38109cc850138e20061df..344073dc9361546d47e815638d054ea632ac6100 100644
--- a/source/RobotAPI/libraries/armem/core/base/ice_conversions.h
+++ b/source/RobotAPI/libraries/armem/core/base/ice_conversions.h
@@ -1,21 +1,19 @@
#pragma once
-#include "EntityInstanceBase.h"
-#include "EntitySnapshotBase.h"
-#include "EntityBase.h"
-#include "ProviderSegmentBase.h"
-#include "CoreSegmentBase.h"
-#include "MemoryBase.h"
-
-#include <RobotAPI/libraries/armem/core/ice_conversions.h>
-#include <RobotAPI/libraries/aron/core/type/variant/forward_declarations.h>
-
-#include <RobotAPI/interface/armem/memory.h>
-
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/ice_conversions/ice_conversions_templates.h>
#include <ArmarXCore/core/time/ice_conversions.h>
+#include <RobotAPI/interface/armem/memory.h>
+#include <RobotAPI/libraries/armem/core/ice_conversions.h>
+#include <RobotAPI/libraries/aron/core/type/variant/forward_declarations.h>
+
+#include "CoreSegmentBase.h"
+#include "EntityBase.h"
+#include "EntityInstanceBase.h"
+#include "EntitySnapshotBase.h"
+#include "MemoryBase.h"
+#include "ProviderSegmentBase.h"
namespace armarx::armem::base::detail
{
@@ -27,7 +25,8 @@ namespace armarx::armem::base::detail
void toIce(aron::type::dto::GenericTypePtr& ice, const aron::type::ObjectPtr& bo);
void fromIce(const aron::type::dto::GenericTypePtr& ice, aron::type::ObjectPtr& bo);
-}
+} // namespace armarx::armem::base::detail
+
namespace armarx::armem::base
{
void toIce(data::EntityInstanceMetadata& ice, const EntityInstanceMetadata& metadata);
@@ -36,9 +35,9 @@ namespace armarx::armem::base
void toIce(data::EntityInstanceMetadataPtr& ice, const EntityInstanceMetadata& metadata);
void fromIce(const data::EntityInstanceMetadataPtr& ice, EntityInstanceMetadata& metadata);
-
- template <class ...Args>
- void toIce(data::EntityInstance& ice, const EntityInstanceBase<Args...>& data)
+ template <class... Args>
+ void
+ toIce(data::EntityInstance& ice, const EntityInstanceBase<Args...>& data)
{
detail::toIceItem(ice, data);
@@ -46,8 +45,10 @@ namespace armarx::armem::base
detail::toIce(ice.data, data.data());
toIce(ice.metadata, data.metadata());
}
- template <class ...Args>
- void fromIce(const data::EntityInstance& ice, EntityInstanceBase<Args...>& data)
+
+ template <class... Args>
+ void
+ fromIce(const data::EntityInstance& ice, EntityInstanceBase<Args...>& data)
{
detail::fromIceItem(ice, data);
@@ -55,55 +56,61 @@ namespace armarx::armem::base
fromIce(ice.metadata, data.metadata());
}
- template <class ...Args>
- void toIce(data::EntitySnapshot& ice, const EntitySnapshotBase<Args...>& snapshot)
+ template <class... Args>
+ void
+ toIce(data::EntitySnapshot& ice, const EntitySnapshotBase<Args...>& snapshot)
{
detail::toIceItem(ice, snapshot);
ice.instances.clear();
- snapshot.forEachInstance([&ice](const auto & instance)
- {
- armarx::toIce(ice.instances.emplace_back(), instance);
- });
+ snapshot.forEachInstance([&ice](const auto& instance)
+ { armarx::toIce(ice.instances.emplace_back(), instance); });
}
- template <class ...Args>
- void fromIce(const data::EntitySnapshot& ice, EntitySnapshotBase<Args...>& snapshot)
+
+ template <class... Args>
+ void
+ fromIce(const data::EntitySnapshot& ice, EntitySnapshotBase<Args...>& snapshot)
{
detail::fromIceItem(ice, snapshot);
snapshot.clear();
for (const data::EntityInstancePtr& iceInstance : ice.instances)
{
- snapshot.addInstance(armarx::fromIce<typename EntitySnapshotBase<Args...>::EntityInstanceT>(iceInstance));
+ snapshot.addInstance(
+ armarx::fromIce<typename EntitySnapshotBase<Args...>::EntityInstanceT>(
+ iceInstance));
}
}
- template <class ...Args>
- void toIce(data::Entity& ice, const EntityBase<Args...>& entity)
+ template <class... Args>
+ void
+ toIce(data::Entity& ice, const EntityBase<Args...>& entity)
{
detail::toIceItem(ice, entity);
ice.history.clear();
- entity.forEachSnapshot([&ice](const auto & snapshot)
- {
- armarx::toIce(ice.history[armarx::toIce<dto::Time>(snapshot.time())], snapshot);
- });
+ entity.forEachSnapshot(
+ [&ice](const auto& snapshot)
+ { armarx::toIce(ice.history[armarx::toIce<dto::Time>(snapshot.time())], snapshot); });
}
- template <class ...Args>
- void fromIce(const data::Entity& ice, EntityBase<Args...>& entity)
+
+ template <class... Args>
+ void
+ fromIce(const data::Entity& ice, EntityBase<Args...>& entity)
{
detail::fromIceItem(ice, entity);
entity.clear();
for (const auto& [key, snapshot] : ice.history)
{
- entity.addSnapshot(armarx::fromIce<typename EntityBase<Args...>::EntitySnapshotT>(snapshot));
+ entity.addSnapshot(
+ armarx::fromIce<typename EntityBase<Args...>::EntitySnapshotT>(snapshot));
}
}
-
- template <class ...Args>
- void toIce(data::ProviderSegment& ice, const ProviderSegmentBase<Args...>& providerSegment)
+ template <class... Args>
+ void
+ toIce(data::ProviderSegment& ice, const ProviderSegmentBase<Args...>& providerSegment)
{
detail::toIceItem(ice, providerSegment);
@@ -111,13 +118,13 @@ namespace armarx::armem::base
ARMARX_CHECK(!providerSegment.aronType() || ice.aronType);
ice.entities.clear();
- providerSegment.forEachEntity([&ice](const auto & entity)
- {
- armarx::toIce(ice.entities[entity.name()], entity);
- });
+ providerSegment.forEachEntity([&ice](const auto& entity)
+ { armarx::toIce(ice.entities[entity.name()], entity); });
}
- template <class ...Args>
- void fromIce(const data::ProviderSegment& ice, ProviderSegmentBase<Args...>& providerSegment)
+
+ template <class... Args>
+ void
+ fromIce(const data::ProviderSegment& ice, ProviderSegmentBase<Args...>& providerSegment)
{
detail::fromIceItem(ice, providerSegment);
@@ -132,8 +139,9 @@ namespace armarx::armem::base
}
}
- template <class ...Args>
- void toIce(data::CoreSegment& ice, const CoreSegmentBase<Args...>& coreSegment)
+ template <class... Args>
+ void
+ toIce(data::CoreSegment& ice, const CoreSegmentBase<Args...>& coreSegment)
{
detail::toIceItem(ice, coreSegment);
@@ -141,13 +149,14 @@ namespace armarx::armem::base
ARMARX_CHECK(!coreSegment.aronType() || ice.aronType);
ice.providerSegments.clear();
- coreSegment.forEachProviderSegment([&ice](const auto & providerSegment)
- {
- armarx::toIce(ice.providerSegments[providerSegment.name()], providerSegment);
- });
+ coreSegment.forEachProviderSegment(
+ [&ice](const auto& providerSegment)
+ { armarx::toIce(ice.providerSegments[providerSegment.name()], providerSegment); });
}
- template <class ...Args>
- void fromIce(const data::CoreSegment& ice, CoreSegmentBase<Args...>& coreSegment)
+
+ template <class... Args>
+ void
+ fromIce(const data::CoreSegment& ice, CoreSegmentBase<Args...>& coreSegment)
{
detail::fromIceItem(ice, coreSegment);
@@ -159,23 +168,26 @@ namespace armarx::armem::base
for (const auto& [key, providerSegment] : ice.providerSegments)
{
coreSegment.addProviderSegment(
- armarx::fromIce<typename CoreSegmentBase<Args...>::ProviderSegmentT>(providerSegment));
+ armarx::fromIce<typename CoreSegmentBase<Args...>::ProviderSegmentT>(
+ providerSegment));
}
}
- template <class ...Args>
- void toIce(data::Memory& ice, const MemoryBase<Args...>& memory)
+ template <class... Args>
+ void
+ toIce(data::Memory& ice, const MemoryBase<Args...>& memory)
{
base::detail::toIceItem(ice, memory);
ice.coreSegments.clear();
- memory.forEachCoreSegment([&ice](const auto & coreSegment)
- {
- armarx::toIce(ice.coreSegments[coreSegment.name()], coreSegment);
- });
+ memory.forEachCoreSegment(
+ [&ice](const auto& coreSegment)
+ { armarx::toIce(ice.coreSegments[coreSegment.name()], coreSegment); });
}
- template <class ...Args>
- void fromIce(const data::Memory& ice, MemoryBase<Args...>& memory)
+
+ template <class... Args>
+ void
+ fromIce(const data::Memory& ice, MemoryBase<Args...>& memory)
{
base::detail::fromIceItem(ice, memory);
@@ -187,4 +199,4 @@ namespace armarx::armem::base
}
}
-}
+} // namespace armarx::armem::base
diff --git a/source/RobotAPI/libraries/armem/core/error/ArMemError.cpp b/source/RobotAPI/libraries/armem/core/error/ArMemError.cpp
index 87b687447b7277cbd3861d7c29e3df566e3e26b3..354b95712761f6a14d3fb6deb5baeff6b2a18aba 100644
--- a/source/RobotAPI/libraries/armem/core/error/ArMemError.cpp
+++ b/source/RobotAPI/libraries/armem/core/error/ArMemError.cpp
@@ -6,7 +6,6 @@
#include "../MemoryID.h"
-
namespace armarx::armem::error
{
@@ -15,7 +14,6 @@ namespace armarx::armem::error
{
}
-
InvalidArgument::InvalidArgument(const std::string& argument,
const std::string& function,
const std::string& message) :
@@ -41,7 +39,6 @@ namespace armarx::armem::error
return ss.str();
}
-
ContainerNameMismatch::ContainerNameMismatch(const std::string& gottenName,
const std::string& ownTerm,
const std::string& containerName) :
@@ -60,7 +57,6 @@ namespace armarx::armem::error
return ss.str();
}
-
ContainerEntryAlreadyExists::ContainerEntryAlreadyExists(const std::string& existingTerm,
const std::string& existingName,
const std::string& ownTerm,
@@ -81,7 +77,6 @@ namespace armarx::armem::error
return ss.str();
}
-
MissingEntry::MissingEntry(const std::string& missingTerm,
const std::string& missingName,
const std::string& containerTerm,
@@ -104,6 +99,24 @@ namespace armarx::armem::error
return ss.str();
}
+ NoSuchEntries::NoSuchEntries(const std::string& missingTerm,
+ const std::string& containerTerm,
+ const MemoryID& containerID,
+ const std::string& message) :
+ ArMemError(makeMsg(missingTerm, containerTerm, containerID, message))
+ {
+ }
+
+ std::string
+ NoSuchEntries::makeMsg(const std::string& missingTerm,
+ const std::string& containerTerm,
+ const MemoryID& containerID,
+ const std::string& message)
+ {
+ std::stringstream ss;
+ ss << "No " << missingTerm << " in " << containerTerm << " " << containerID << ".";
+ return ss.str();
+ }
MissingData::MissingData(const std::string& missingTerm,
const std::string& missingName,
@@ -125,7 +138,6 @@ namespace armarx::armem::error
return ss.str();
}
-
ParseIntegerError::ParseIntegerError(std::string string, std::string semanticName) :
ArMemError(makeMsg(string, semanticName))
{
@@ -139,7 +151,6 @@ namespace armarx::armem::error
return ss.str();
}
-
InvalidMemoryID::InvalidMemoryID(const MemoryID& id, const std::string& message) :
ArMemError(makeMsg(id, message))
{
@@ -153,7 +164,6 @@ namespace armarx::armem::error
return ss.str();
}
-
EntityHistoryEmpty::EntityHistoryEmpty(const std::string& entityName,
const std::string& message) :
ArMemError(makeMsg(entityName, message))
@@ -176,7 +186,6 @@ namespace armarx::armem::error
return ss.str();
}
-
UnknownQueryType::UnknownQueryType(const std::string& term, const std::string& typeName) :
ArMemError(makeMsg(term, typeName))
{
@@ -195,7 +204,8 @@ namespace armarx::armem::error
{
}
- std::string QueryFailed::makeMsg(const std::string& memory, const std::string& message)
+ std::string
+ QueryFailed::makeMsg(const std::string& memory, const std::string& message)
{
std::stringstream ss;
ss << "Query from memory " << memory << " failed with message: " << message;
@@ -207,7 +217,6 @@ namespace armarx::armem::error
{
}
-
std::string
IOError::makeMsg(const std::string& path, const std::string& message)
{
diff --git a/source/RobotAPI/libraries/armem/core/error/ArMemError.h b/source/RobotAPI/libraries/armem/core/error/ArMemError.h
index 6f5be02a243e9e953d56b67f5f1726be214b2745..69ab7d4ddff8e22791272c520386e2fdff3d6e6f 100644
--- a/source/RobotAPI/libraries/armem/core/error/ArMemError.h
+++ b/source/RobotAPI/libraries/armem/core/error/ArMemError.h
@@ -4,7 +4,6 @@
#include <SimoxUtility/meta/type_name.h>
-
namespace armarx::armem
{
class MemoryID;
@@ -19,85 +18,103 @@ namespace armarx::armem::error
class ArMemError : public std::runtime_error
{
public:
-
ArMemError(const std::string& msg);
-
};
-
/**
* @brief Indicates that an argument was invalid.
*/
class InvalidArgument : public ArMemError
{
public:
-
- InvalidArgument(const std::string& argument, const std::string& function,
+ InvalidArgument(const std::string& argument,
+ const std::string& function,
const std::string& message);
- static std::string makeMsg(const std::string& argument, const std::string& function,
+ static std::string makeMsg(const std::string& argument,
+ const std::string& function,
const std::string& message);
-
};
-
-
/**
* @brief Indicates that a name in a given ID does not match a container's own name.
*/
class ContainerNameMismatch : public ArMemError
{
public:
-
ContainerNameMismatch(const std::string& gottenName,
- const std::string& containerTerm, const std::string& containerName);
+ const std::string& containerTerm,
+ const std::string& containerName);
static std::string makeMsg(const std::string& gottenName,
- const std::string& containerTerm, const std::string& containerName);
-
+ const std::string& containerTerm,
+ const std::string& containerName);
};
-
/**
* @brief Indicates that a name in a given ID does not match a container's own name.
*/
class ContainerEntryAlreadyExists : public ArMemError
{
public:
-
- ContainerEntryAlreadyExists(const std::string& existingTerm, const std::string& existingName,
- const std::string& ownTerm, const std::string& ownName);
-
- static std::string makeMsg(const std::string& existingTerm, const std::string& existingName,
- const std::string& ownTerm, const std::string& ownName);
-
+ ContainerEntryAlreadyExists(const std::string& existingTerm,
+ const std::string& existingName,
+ const std::string& ownTerm,
+ const std::string& ownName);
+
+ static std::string makeMsg(const std::string& existingTerm,
+ const std::string& existingName,
+ const std::string& ownTerm,
+ const std::string& ownName);
};
-
/**
* @brief Indicates that a container did not have an entry under a given name.
*/
class MissingEntry : public ArMemError
{
public:
-
template <class MissingT, class ContainerT>
- static MissingEntry create(const std::string& missingKey, const ContainerT& container)
+ static MissingEntry
+ create(const std::string& missingKey, const ContainerT& container)
{
- return MissingEntry(MissingT::getLevelName(), missingKey,
- ContainerT::getLevelName(), container.getKeyString(), container.size());
+ return MissingEntry(MissingT::getLevelName(),
+ missingKey,
+ ContainerT::getLevelName(),
+ container.getKeyString(),
+ container.size());
}
-
- MissingEntry(const std::string& missingTerm, const std::string& missingName,
- const std::string& containerTerm, const std::string& containerName,
+ MissingEntry(const std::string& missingTerm,
+ const std::string& missingName,
+ 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& containerTerm, const std::string& containerName,
+ static std::string makeMsg(const std::string& missingTerm,
+ const std::string& missingName,
+ const std::string& containerTerm,
+ const std::string& containerName,
size_t size);
};
+ /**
+ * @brief Indicates that an operation requiring at least one element to exist
+ * failed because there were no such entries.
+ */
+ class NoSuchEntries : public ArMemError
+ {
+ public:
+ NoSuchEntries(const std::string& missingTerm,
+ const std::string& containerTerm,
+ const MemoryID& containerID,
+ const std::string& message = "");
+
+ static std::string makeMsg(const std::string& missingTerm,
+ const std::string& containerTerm,
+ const MemoryID& containerID,
+ const std::string& message = "");
+ };
/**
* @brief Indicates that a container did have an entry, but the entry's data was
@@ -106,14 +123,17 @@ namespace armarx::armem::error
class MissingData : public ArMemError
{
public:
- MissingData(const std::string& missingTerm, const std::string& missingName,
- const std::string& ownTerm, const std::string& ownName);
-
- static std::string makeMsg(const std::string& missingTerm, const std::string& missingName,
- const std::string& ownTerm, const std::string& ownName);
+ MissingData(const std::string& missingTerm,
+ const std::string& missingName,
+ const std::string& ownTerm,
+ const std::string& ownName);
+
+ static std::string makeMsg(const std::string& missingTerm,
+ const std::string& missingName,
+ const std::string& ownTerm,
+ const std::string& ownName);
};
-
/**
* @brief Indicates that a string could not be parsed as integer.
*/
@@ -125,52 +145,43 @@ namespace armarx::armem::error
static std::string makeMsg(std::string string, std::string semanticName);
};
-
-
/**
* @brief Indicates that a memory ID is invalid, e.g. does not contain necessary information.
*/
class InvalidMemoryID : public ArMemError
{
public:
-
InvalidMemoryID(const MemoryID& id, const std::string& message);
static std::string makeMsg(const MemoryID& id, const std::string& message);
-
};
-
/**
* @brief Indicates that an entity's history was queried, but is empty.
*/
class EntityHistoryEmpty : public ArMemError
{
public:
-
EntityHistoryEmpty(const std::string& entityName, const std::string& message = "");
static std::string makeMsg(const std::string& entityName, const std::string& message = "");
-
};
-
/**
* @brief Indicates that an entity's history was queried, but is empty.
*/
class UnknownQueryType : public ArMemError
{
public:
-
template <class QueryType>
UnknownQueryType(const std::string& term, const QueryType& query) :
UnknownQueryType(term, simox::meta::get_type_name(query))
{
}
+
UnknownQueryType(const std::string& term, const std::string& typeName);
static std::string makeMsg(const std::string& term, const std::string& typeName);
-
};
/**
@@ -179,28 +190,23 @@ namespace armarx::armem::error
class QueryFailed : public ArMemError
{
public:
-
QueryFailed(const std::string& memory, const std::string& message = "");
static std::string makeMsg(const std::string& memory, const std::string& message = "");
-
};
-
/**
* @brief Indicates that something went wrong when accessing the filesystem.
*/
class IOError : public ArMemError
{
public:
-
IOError(const std::string& path, const std::string& message = "");
static std::string makeMsg(const std::string& path, const std::string& message = "");
std::string path;
-
};
/**
diff --git a/source/RobotAPI/libraries/armem/core/wm/memory_definitions.cpp b/source/RobotAPI/libraries/armem/core/wm/memory_definitions.cpp
index bfc408ac99a98a5bf1826f37334e1c331196ed1b..1da7db6bc21c98a33f93c2c52f5f38da4a505a88 100644
--- a/source/RobotAPI/libraries/armem/core/wm/memory_definitions.cpp
+++ b/source/RobotAPI/libraries/armem/core/wm/memory_definitions.cpp
@@ -1,25 +1,23 @@
#include "memory_definitions.h"
-#include "error.h"
-
-#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-
#include <map>
#include <vector>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
+
+#include "error.h"
namespace armarx::armem::wm
{
- bool EntityInstance::equalsDeep(const EntityInstance& other) const
+ bool
+ EntityInstance::equalsDeep(const EntityInstance& other) const
{
if (_data and other.data())
{
- return id() == other.id()
- && _metadata == other.metadata()
- && *_data == *other.data();
+ return id() == other.id() && _metadata == other.metadata() && *_data == *other.data();
}
if (_data or other.data())
{
@@ -28,8 +26,8 @@ namespace armarx::armem::wm
return id() == other.id() && _metadata == other.metadata();
}
-
- void EntityInstance::update(const EntityUpdate& update)
+ void
+ EntityInstance::update(const EntityUpdate& update)
{
ARMARX_CHECK_FITS_SIZE(this->index(), update.instancesData.size());
@@ -42,4 +40,4 @@ namespace armarx::armem::wm
this->_metadata.arrivedTime = update.arrivedTime;
}
-}
+} // namespace armarx::armem::wm
diff --git a/source/RobotAPI/libraries/armem/core/wm/memory_definitions.h b/source/RobotAPI/libraries/armem/core/wm/memory_definitions.h
index 642cc50eda5e34a90e482c7bc5b26d48e3847081..dcc82878286cf3387e0381ca21312f76bff89bcb 100644
--- a/source/RobotAPI/libraries/armem/core/wm/memory_definitions.h
+++ b/source/RobotAPI/libraries/armem/core/wm/memory_definitions.h
@@ -1,35 +1,42 @@
#pragma once
-#include "detail/data_lookup_mixins.h"
-
+#include <RobotAPI/libraries/armem/core/base/CoreSegmentBase.h>
+#include <RobotAPI/libraries/armem/core/base/EntityBase.h>
#include <RobotAPI/libraries/armem/core/base/EntityInstanceBase.h>
#include <RobotAPI/libraries/armem/core/base/EntitySnapshotBase.h>
-#include <RobotAPI/libraries/armem/core/base/EntityBase.h>
-#include <RobotAPI/libraries/armem/core/base/ProviderSegmentBase.h>
-#include <RobotAPI/libraries/armem/core/base/CoreSegmentBase.h>
#include <RobotAPI/libraries/armem/core/base/MemoryBase.h>
-
+#include <RobotAPI/libraries/armem/core/base/ProviderSegmentBase.h>
#include <RobotAPI/libraries/aron/core/data/variant/forward_declarations.h>
+#include "detail/data_lookup_mixins.h"
namespace armarx::armem::wm
{
+ /**
+ * @brief Client-side working memory entity instance metadata.
+ */
using EntityInstanceMetadata = base::EntityInstanceMetadata;
+ /**
+ * @brief Client-side working memory entity instance data (payload).
+ */
using EntityInstanceData = armarx::aron::data::Dict;
+ /**
+ * @brief Pointer type of EntityInstanceData.
+ */
using EntityInstanceDataPtr = armarx::aron::data::DictPtr;
-
- /// @see base::EntityInstanceBase
+ /**
+ * @brief Client-side working entity instance.
+ * @see base::EntityInstanceBase
+ */
class EntityInstance :
public base::EntityInstanceBase<EntityInstanceDataPtr, EntityInstanceMetadata>
{
using Base = base::EntityInstanceBase<EntityInstanceDataPtr, EntityInstanceMetadata>;
public:
-
using Base::EntityInstanceBase;
-
/**
* @brief Fill `*this` with the update's values.
* @param update The update.
@@ -38,78 +45,99 @@ namespace armarx::armem::wm
void update(const EntityUpdate& update);
bool equalsDeep(const EntityInstance& other) const;
-
};
-
-
- /// @see base::EntitySnapshotBase
+ /**
+ * @brief Entity instance with a concrete ARON DTO type as data.
+ *
+ * This is the return type of EntityInstance::withDataAs<AronDtoT>().
+ *
+ * Usage example:
+ * @code
+ * #include <RobotAPI/libraries/aron/common/aron/time.aron.generated.h>
+ *
+ * armarx::arondto::Duration duration;
+ * duration.microSeconds = 1000;
+ *
+ * armarx::armem::wm::EntityInstance instance;
+ * instance.data() = duration.toAron();
+ *
+ * const armarx::armem::wm::EntityInstanceBase<armarx::arondto::Duration> cast =
+ * instance.withDataAs<armarx::arondto::Duration>();
+ *
+ * const armarx::arondto::Duration& durationOut = cast.data();
+ *
+ * assert(durationOut.microseconds == 1000);
+ * @endcode
+ */
+ template <class AronDtoT>
+ using EntityInstanceBase = base::EntityInstanceBase<AronDtoT, EntityInstanceMetadata>;
+
+ /**
+ * @brief Client-side working memory entity snapshot.
+ * @see base::EntitySnapshotBase
+ */
class EntitySnapshot :
- public base::EntitySnapshotBase<EntityInstance, EntitySnapshot>
- , public detail::FindInstanceDataMixinForSnapshot<EntitySnapshot>
+ public base::EntitySnapshotBase<EntityInstance, EntitySnapshot>,
+ public detail::FindInstanceDataMixinForSnapshot<EntitySnapshot>
+
{
public:
-
using base::EntitySnapshotBase<EntityInstance, EntitySnapshot>::EntitySnapshotBase;
-
};
-
- /// @see base::EntityBase
+ /**
+ * @brief Client-side working memory entity.
+ * @see base::EntityBase
+ */
class Entity :
- public base::EntityBase<EntitySnapshot, Entity>
- , public detail::FindInstanceDataMixinForEntity<Entity>
+ public base::EntityBase<EntitySnapshot, Entity>,
+ public detail::FindInstanceDataMixinForEntity<Entity>
{
public:
-
using base::EntityBase<EntitySnapshot, Entity>::EntityBase;
-
};
-
-
- /// @see base::ProviderSegmentBase
+ /**
+ * @brief Client-side working memory provider segment.
+ * @see base::ProviderSegmentBase
+ */
class ProviderSegment :
- public base::ProviderSegmentBase<Entity, ProviderSegment>
- , public detail::FindInstanceDataMixin<ProviderSegment>
+ public base::ProviderSegmentBase<Entity, ProviderSegment>,
+ public detail::FindInstanceDataMixin<ProviderSegment>
{
using Base = base::ProviderSegmentBase<Entity, ProviderSegment>;
public:
-
using Base::ProviderSegmentBase;
-
};
-
-
- /// @see base::CoreSegmentBase
+ /**
+ * @brief Client-side working memory core segment.
+ * @see base::CoreSegmentBase
+ */
class CoreSegment :
- public base::CoreSegmentBase<ProviderSegment, CoreSegment>
- , public detail::FindInstanceDataMixin<CoreSegment>
+ public base::CoreSegmentBase<ProviderSegment, CoreSegment>,
+ public detail::FindInstanceDataMixin<CoreSegment>
{
using Base = base::CoreSegmentBase<ProviderSegment, CoreSegment>;
public:
-
using Base::CoreSegmentBase;
-
};
-
-
- /// @see base::MemoryBase
+ /**
+ * @brief Client-side working memory.
+ * @see base::MemoryBase
+ */
class Memory :
- public base::MemoryBase<CoreSegment, Memory>
- , public detail::FindInstanceDataMixin<Memory>
+ public base::MemoryBase<CoreSegment, Memory>,
+ public detail::FindInstanceDataMixin<Memory>
{
using Base = base::MemoryBase<CoreSegment, Memory>;
public:
-
using Base::MemoryBase;
-
};
-}
-
+} // namespace armarx::armem::wm
diff --git a/source/RobotAPI/libraries/armem/test/ArMemEntityInstanceDataTest.cpp b/source/RobotAPI/libraries/armem/test/ArMemEntityInstanceDataTest.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..24193227415c833e91251a03208e830738ca1b31
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/test/ArMemEntityInstanceDataTest.cpp
@@ -0,0 +1,72 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::ArmarXObjects::armem
+ * @author Simon Ottenhaus ( simon dot ottenhaus at kit dot edu )
+ * @date 2020
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#define BOOST_TEST_MODULE RobotAPI::ArmarXLibraries::armem
+
+#define ARMARX_BOOST_TEST
+
+#include <iostream>
+#include <set>
+
+#include <RobotAPI/Test.h>
+#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
+#include <RobotAPI/libraries/aron/common/aron/Color.aron.generated.h>
+#include <RobotAPI/libraries/aron/common/aron/time.aron.generated.h>
+
+BOOST_AUTO_TEST_CASE(test_EntityInstance_withDataAs_Color)
+{
+ simox::arondto::Color color;
+ color.r = 0;
+ color.g = 100;
+ color.b = 200;
+ color.a = 255;
+
+ armarx::armem::wm::EntityInstance instance;
+ instance.data() = color.toAron();
+
+ const armarx::armem::wm::EntityInstanceBase<simox::arondto::Color> w =
+ instance.withDataAs<simox::arondto::Color>();
+ const simox::arondto::Color& colorOut = w.data();
+
+ BOOST_CHECK(colorOut == color);
+ BOOST_CHECK_EQUAL(colorOut.r, 0);
+ BOOST_CHECK_EQUAL(colorOut.g, 100);
+ BOOST_CHECK_EQUAL(colorOut.b, 200);
+ BOOST_CHECK_EQUAL(colorOut.a, 255);
+}
+
+BOOST_AUTO_TEST_CASE(test_EntityInstance_withDataAs_Time)
+{
+ armarx::arondto::Duration duration;
+ duration.microSeconds = 1000;
+
+ armarx::armem::wm::EntityInstance instance;
+ instance.data() = duration.toAron();
+
+ const armarx::armem::wm::EntityInstanceBase<armarx::arondto::Duration> w =
+ instance.withDataAs<armarx::arondto::Duration>();
+ const armarx::arondto::Duration& durationOut = w.data();
+
+ BOOST_CHECK(durationOut == duration);
+ BOOST_CHECK_EQUAL(durationOut.microSeconds, 1000);
+ assert(durationOut.microSeconds == 1000);
+}
diff --git a/source/RobotAPI/libraries/armem/test/ArMemForEachTest.cpp b/source/RobotAPI/libraries/armem/test/ArMemForEachTest.cpp
index bf97734a52d7d9db9198101307270c23d8b07383..53771403181a1afcba608d94979faa54db61404b 100644
--- a/source/RobotAPI/libraries/armem/test/ArMemForEachTest.cpp
+++ b/source/RobotAPI/libraries/armem/test/ArMemForEachTest.cpp
@@ -24,33 +24,30 @@
#define ARMARX_BOOST_TEST
-#include <RobotAPI/Test.h>
-#include <RobotAPI/libraries/armem/core/Commit.h>
-#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
-#include <RobotAPI/libraries/armem/core/operations.h>
-
#include <iostream>
#include <set>
+#include <RobotAPI/Test.h>
+#include <RobotAPI/libraries/armem/core/Commit.h>
+#include <RobotAPI/libraries/armem/core/operations.h>
+#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
+#include <RobotAPI/libraries/aron/common/aron/time.aron.generated.h>
namespace armem = armarx::armem;
-
template <class ValueT>
-static
-std::vector<ValueT> toVector(const std::set<ValueT>& set)
+static std::vector<ValueT>
+toVector(const std::set<ValueT>& set)
{
return {set.begin(), set.end()};
}
-
-
BOOST_AUTO_TEST_CASE(test_forEach)
{
using namespace armarx::armem;
const MemoryID mid("memory");
- const std::set<MemoryID> emptySet; // For comparison.
+ const std::set<MemoryID> emptySet; // For comparison.
wm::Memory memory(mid);
std::set<MemoryID> cids, pids, eids, sids, iids;
@@ -101,75 +98,79 @@ BOOST_AUTO_TEST_CASE(test_forEach)
BOOST_TEST_MESSAGE("Memory: \n" << armem::print(memory));
- memory.forEachInstance([&](const wm::EntityInstance & i) -> bool
- {
- BOOST_TEST_CONTEXT(wm::EntityInstance::getLevelName() << " " << i.id())
+ memory.forEachInstance(
+ [&](const wm::EntityInstance& i) -> bool
{
- BOOST_TEST_INFO(toVector(iids));
- BOOST_CHECK_EQUAL(iids.count(i.id()), 1);
- iids.erase(i.id());
- }
- return true;
- });
+ BOOST_TEST_CONTEXT(wm::EntityInstance::getLevelName() << " " << i.id())
+ {
+ BOOST_TEST_INFO(toVector(iids));
+ BOOST_CHECK_EQUAL(iids.count(i.id()), 1);
+ iids.erase(i.id());
+ }
+ return true;
+ });
BOOST_TEST_CONTEXT(toVector(iids))
{
BOOST_CHECK_EQUAL(iids.size(), 0);
}
- memory.forEachSnapshot([&](const wm::EntitySnapshot & s) -> bool
- {
- BOOST_TEST_CONTEXT(wm::EntitySnapshot::getLevelName() << " " << s.id())
+ memory.forEachSnapshot(
+ [&](const wm::EntitySnapshot& s) -> bool
{
- BOOST_TEST_INFO(toVector(sids));
- BOOST_CHECK_EQUAL(sids.count(s.id()), 1);
- sids.erase(s.id());
- }
- return true;
- });
+ BOOST_TEST_CONTEXT(wm::EntitySnapshot::getLevelName() << " " << s.id())
+ {
+ BOOST_TEST_INFO(toVector(sids));
+ BOOST_CHECK_EQUAL(sids.count(s.id()), 1);
+ sids.erase(s.id());
+ }
+ return true;
+ });
BOOST_TEST_INFO(toVector(sids));
BOOST_CHECK_EQUAL(sids.size(), 0);
- memory.forEachEntity([&](const wm::Entity & e) -> bool
- {
- BOOST_TEST_CONTEXT(wm::Entity::getLevelName() << " " << e.id())
+ memory.forEachEntity(
+ [&](const wm::Entity& e) -> bool
{
- BOOST_TEST_INFO(toVector(eids));
- BOOST_CHECK_EQUAL(eids.count(e.id()), 1);
- eids.erase(e.id());
- }
- return true;
- });
+ BOOST_TEST_CONTEXT(wm::Entity::getLevelName() << " " << e.id())
+ {
+ BOOST_TEST_INFO(toVector(eids));
+ BOOST_CHECK_EQUAL(eids.count(e.id()), 1);
+ eids.erase(e.id());
+ }
+ return true;
+ });
BOOST_TEST_INFO(toVector(eids));
BOOST_CHECK_EQUAL(eids.size(), 0);
- memory.forEachProviderSegment([&](const wm::ProviderSegment & p) // -> bool
- {
- BOOST_TEST_CONTEXT(wm::ProviderSegment::getLevelName() << " " << p.id())
+ memory.forEachProviderSegment(
+ [&](const wm::ProviderSegment& p) // -> bool
{
- BOOST_TEST_INFO(toVector(pids));
- BOOST_CHECK_EQUAL(pids.count(p.id()), 1);
- pids.erase(p.id());
- }
- return true;
- });
+ BOOST_TEST_CONTEXT(wm::ProviderSegment::getLevelName() << " " << p.id())
+ {
+ BOOST_TEST_INFO(toVector(pids));
+ BOOST_CHECK_EQUAL(pids.count(p.id()), 1);
+ pids.erase(p.id());
+ }
+ return true;
+ });
BOOST_TEST_INFO(toVector(pids));
BOOST_CHECK_EQUAL(pids.size(), 0);
- memory.forEachCoreSegment([&](const wm::CoreSegment & c) // -> bool
- {
- BOOST_TEST_CONTEXT(wm::CoreSegment::getLevelName() << " " << c.id())
+ memory.forEachCoreSegment(
+ [&](const wm::CoreSegment& c) // -> bool
{
- BOOST_TEST_INFO(toVector(cids));
- BOOST_CHECK_EQUAL(cids.count(c.id()), 1);
- cids.erase(c.id());
- }
- return true;
- });
+ BOOST_TEST_CONTEXT(wm::CoreSegment::getLevelName() << " " << c.id())
+ {
+ BOOST_TEST_INFO(toVector(cids));
+ BOOST_CHECK_EQUAL(cids.count(c.id()), 1);
+ cids.erase(c.id());
+ }
+ return true;
+ });
BOOST_TEST_INFO(toVector(cids));
BOOST_CHECK_EQUAL(cids.size(), 0);
}
-
BOOST_AUTO_TEST_CASE(test_forEach_non_bool_func)
{
// Check whether this compiles + runs without break.
@@ -179,15 +180,10 @@ BOOST_AUTO_TEST_CASE(test_forEach_non_bool_func)
entity.addSnapshot(armem::Time(armem::Duration::MicroSeconds(1500)));
int i = 0;
- entity.forEachSnapshot([&i](const armem::wm::EntitySnapshot&) -> void
- {
- ++i;
- });
+ entity.forEachSnapshot([&i](const armem::wm::EntitySnapshot&) -> void { ++i; });
BOOST_CHECK_EQUAL(i, 2);
}
-
-
BOOST_AUTO_TEST_CASE(test_forEachInstanceIn)
{
armem::wm::EntitySnapshot snapshot(armem::Time::Now());
@@ -195,10 +191,7 @@ BOOST_AUTO_TEST_CASE(test_forEachInstanceIn)
snapshot.addInstance();
int i = 0;
- auto count = [&i](const auto&) -> void
- {
- ++i;
- };
+ auto count = [&i](const auto&) -> void { ++i; };
{
auto test = [&](auto&& snapshot)
@@ -273,9 +266,7 @@ BOOST_AUTO_TEST_CASE(test_forEachInstanceIn)
armem::wm::ProviderSegment provSeg("provider");
provSeg.addEntity(entity);
- provSeg.addEntity(entity.name() + " 2")
- .addSnapshot(armem::Time::Now())
- .addInstance();
+ provSeg.addEntity(entity.name() + " 2").addSnapshot(armem::Time::Now()).addInstance();
{
auto test = [&](auto&& provSeg)
@@ -320,9 +311,9 @@ BOOST_AUTO_TEST_CASE(test_forEachInstanceIn)
armem::wm::CoreSegment coreSeg("core");
coreSeg.addProviderSegment(provSeg);
coreSeg.addProviderSegment("other prov segment")
- .addEntity("other entity")
- .addSnapshot(armem::Time::Now())
- .addInstance();
+ .addEntity("other entity")
+ .addSnapshot(armem::Time::Now())
+ .addInstance();
{
auto test = [&](auto&& coreSeg)
@@ -370,10 +361,10 @@ BOOST_AUTO_TEST_CASE(test_forEachInstanceIn)
armem::wm::Memory memory("memory");
memory.addCoreSegment(coreSeg);
memory.addCoreSegment("other memory")
- .addProviderSegment("other provider segment")
- .addEntity("other entity")
- .addSnapshot(armem::Time::Now())
- .addInstance();
+ .addProviderSegment("other provider segment")
+ .addEntity("other entity")
+ .addSnapshot(armem::Time::Now())
+ .addInstance();
{
auto test = [&](auto&& memory)
@@ -422,3 +413,212 @@ BOOST_AUTO_TEST_CASE(test_forEachInstanceIn)
test(const_cast<const armem::wm::Memory&>(memory));
}
}
+
+namespace forEachInstanceAs
+{
+ struct Fixture
+ {
+ armem::wm::Entity entity;
+ armem::wm::EntitySnapshot* snapshotA = nullptr;
+ armem::wm::EntitySnapshot* snapshotB = nullptr;
+
+ armem::wm::EntityInstance* instanceA0 = nullptr;
+ armem::wm::EntityInstance* instanceA1 = nullptr;
+ armem::wm::EntityInstance* instanceB0 = nullptr;
+ armem::wm::EntityInstance* instanceB1 = nullptr;
+
+ Fixture()
+ {
+ snapshotA = &entity.addSnapshot(armem::Time(armem::Duration::MicroSeconds(100)));
+ snapshotB = &entity.addSnapshot(armem::Time(armem::Duration::MicroSeconds(200)));
+
+ BOOST_REQUIRE_EQUAL(entity.size(), 2);
+ BOOST_REQUIRE_EQUAL(snapshotA->size(), 0);
+ BOOST_REQUIRE_EQUAL(snapshotB->size(), 0);
+
+ instanceA0 = &snapshotA->addInstance();
+ instanceA1 = &snapshotA->addInstance();
+ instanceB0 = &snapshotB->addInstance();
+ instanceB1 = &snapshotB->addInstance();
+
+ if (false)
+ {
+ BOOST_REQUIRE_EQUAL(snapshotA->findInstance(0), instanceA0);
+ BOOST_REQUIRE_EQUAL(snapshotA->findInstance(1), instanceA1);
+ BOOST_REQUIRE_EQUAL(snapshotB->findInstance(0), instanceB0);
+ BOOST_REQUIRE_EQUAL(snapshotB->findInstance(1), instanceB1);
+ }
+
+ int i = 0;
+
+ i = 0;
+ entity.forEachSnapshot(
+ [&i](const armem::wm::EntitySnapshot& snapshot)
+ {
+ BOOST_REQUIRE_EQUAL(snapshot.size(), 2);
+ ++i;
+ });
+ BOOST_REQUIRE_EQUAL(i, 2);
+
+ i = 0;
+ entity.forEachInstance(
+ [&i](armem::wm::EntityInstance& instance)
+ {
+ instance.setData(makeDuration(i * 1000).toAron());
+ BOOST_REQUIRE(instance.data());
+ ++i;
+ });
+ BOOST_REQUIRE_EQUAL(i, 4);
+
+ if (false)
+ {
+ instanceA0->setData(makeDuration(1000).toAron());
+ instanceA1->setData(makeDuration(2000).toAron());
+ instanceB0->setData(makeDuration(3000).toAron());
+ instanceB1->setData(makeDuration(4000).toAron());
+
+ BOOST_REQUIRE(instanceA0->data());
+ BOOST_REQUIRE(instanceA1->data());
+ BOOST_REQUIRE(instanceB0->data());
+ BOOST_REQUIRE(instanceB1->data());
+ BOOST_REQUIRE_NO_THROW(armarx::arondto::Duration::FromAron(instanceA0->data()));
+ BOOST_REQUIRE_NO_THROW(armarx::arondto::Duration::FromAron(instanceA1->data()));
+ BOOST_REQUIRE_NO_THROW(armarx::arondto::Duration::FromAron(instanceB0->data()));
+ BOOST_REQUIRE_NO_THROW(armarx::arondto::Duration::FromAron(instanceB1->data()));
+ }
+
+ i = 0;
+ entity.forEachInstance(
+ [&i](const armem::wm::EntityInstance& instance)
+ {
+ BOOST_REQUIRE(instance.data());
+ ++i;
+ return true;
+ });
+ BOOST_REQUIRE_EQUAL(i, 4);
+ }
+
+ static armarx::arondto::Duration
+ makeDuration(int microSeconds)
+ {
+ armarx::arondto::Duration duration;
+ duration.microSeconds = microSeconds;
+ return duration;
+ };
+ };
+} // namespace forEachInstanceAs
+
+
+BOOST_FIXTURE_TEST_SUITE(forEachInstanceAs, Fixture)
+
+BOOST_AUTO_TEST_CASE(test_forEachInstanceAs_const_ref_return_true)
+{
+ int i = 0;
+
+ i = 0;
+ entity.forEachInstanceAs(
+ [&i](const armarx::arondto::Duration& duration)
+ {
+ BOOST_CHECK_EQUAL(duration.microSeconds, i * 1000);
+ ++i;
+
+ return true;
+ });
+ BOOST_CHECK_EQUAL(i, 4);
+}
+
+BOOST_AUTO_TEST_CASE(test_forEachInstanceAs_const_ref)
+{
+ int i = 0;
+ entity.forEachInstanceAs(
+ [&i](const armarx::arondto::Duration& duration)
+ {
+ BOOST_CHECK_EQUAL(duration.microSeconds, i * 1000);
+ ++i;
+ });
+ BOOST_CHECK_EQUAL(i, 4);
+}
+
+BOOST_AUTO_TEST_CASE(test_forEachInstanceAs_value_return_true)
+{
+ int i = 0;
+ entity.forEachInstanceAs(
+ [&i](armarx::arondto::Duration duration)
+ {
+ BOOST_CHECK_EQUAL(duration.microSeconds, i * 1000);
+ ++i;
+
+ return true;
+ });
+ BOOST_CHECK_EQUAL(i, 4);
+}
+
+BOOST_AUTO_TEST_CASE(test_forEachInstanceAs_value)
+{
+ int i = 0;
+ entity.forEachInstanceAs(
+ [&i](armarx::arondto::Duration duration)
+ {
+ BOOST_CHECK_EQUAL(duration.microSeconds, i * 1000);
+ ++i;
+ });
+ BOOST_CHECK_EQUAL(i, 4);
+}
+
+BOOST_AUTO_TEST_CASE(test_forEachInstanceWithDataAs_const_ref_return_true)
+{
+ int i = 0;
+ entity.forEachInstanceWithDataAs(
+ [&i](const armem::wm::EntityInstanceBase<armarx::arondto::Duration>& instance)
+ {
+ const armarx::arondto::Duration& duration = instance.data();
+ BOOST_CHECK_EQUAL(duration.microSeconds, i * 1000);
+ ++i;
+
+ return true;
+ });
+ BOOST_CHECK_EQUAL(i, 4);
+}
+
+BOOST_AUTO_TEST_CASE(test_forEachInstanceWithDataAs_const_ref)
+{
+ int i = 0;
+ entity.forEachInstanceWithDataAs(
+ [&i](const armem::wm::EntityInstanceBase<armarx::arondto::Duration>& instance)
+ {
+ const armarx::arondto::Duration& duration = instance.data();
+ BOOST_CHECK_EQUAL(duration.microSeconds, i * 1000);
+ ++i;
+ });
+ BOOST_CHECK_EQUAL(i, 4);
+}
+
+BOOST_AUTO_TEST_CASE(test_forEachInstanceWithDataAs_value_return_true)
+{
+ int i = 0;
+ entity.forEachInstanceWithDataAs(
+ [&i](armem::wm::EntityInstanceBase<armarx::arondto::Duration> instance)
+ {
+ const armarx::arondto::Duration& duration = instance.data();
+ BOOST_CHECK_EQUAL(duration.microSeconds, i * 1000);
+ ++i;
+
+ return true;
+ });
+ BOOST_CHECK_EQUAL(i, 4);
+}
+
+BOOST_AUTO_TEST_CASE(test_forEachInstanceWithDataAs_value)
+{
+ int i = 0;
+ entity.forEachInstanceWithDataAs(
+ [&i](armem::wm::EntityInstanceBase<armarx::arondto::Duration> instance)
+ {
+ const armarx::arondto::Duration& duration = instance.data();
+ BOOST_CHECK_EQUAL(duration.microSeconds, i * 1000);
+ ++i;
+ });
+ BOOST_CHECK_EQUAL(i, 4);
+}
+
+BOOST_AUTO_TEST_SUITE_END()
diff --git a/source/RobotAPI/libraries/armem/test/ArMemGetFindTest.cpp b/source/RobotAPI/libraries/armem/test/ArMemGetFindTest.cpp
index f03b02052ef9956aab079dda96677acaf5aca48d..4c133b5ac32e58eaf573e3b26f334b148c6a17cb 100644
--- a/source/RobotAPI/libraries/armem/test/ArMemGetFindTest.cpp
+++ b/source/RobotAPI/libraries/armem/test/ArMemGetFindTest.cpp
@@ -14,7 +14,7 @@
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*
* @package RobotAPI::ArmarXObjects::armem
- * @author Simon Ottenhaus ( simon dot ottenhaus at kit dot edu )
+ * @author Rainer Kartmann ( raier dot kartmann at kit dot edu )
* @date 2020
* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
* GNU General Public License
@@ -24,29 +24,36 @@
#define ARMARX_BOOST_TEST
-#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
-#include <RobotAPI/libraries/armem/core/error.h>
-#include <RobotAPI/libraries/armem/core/operations.h>
+#include <iostream>
#include <RobotAPI/Test.h>
-
-#include <iostream>
+#include <RobotAPI/libraries/armem/core/error.h>
+#include <RobotAPI/libraries/armem/core/operations.h>
+#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
namespace armem = armarx::armem;
namespace wm = armarx::armem::wm;
namespace aron = armarx::aron;
-
namespace ArMemGetFindTest
{
struct Fixture
{
wm::EntitySnapshot snapshot;
+ wm::EntitySnapshot snapshotEmpty;
+
wm::Entity entity;
+ wm::Entity entityEmpty;
+
wm::ProviderSegment provSeg;
+ wm::ProviderSegment provSegEmpty;
+
wm::CoreSegment coreSeg;
+ wm::CoreSegment coreSegEmpty;
+
wm::Memory memory;
+ wm::Memory memoryEmpty;
armem::MemoryID instanceID;
armem::MemoryID snapshotID;
@@ -58,24 +65,38 @@ namespace ArMemGetFindTest
Fixture()
{
{
+ snapshotEmpty.time() = armem::Time(armem::Duration::MicroSeconds(500));
+
snapshot.time() = armem::Time(armem::Duration::MicroSeconds(1000));
snapshot.addInstance();
}
{
+ entityEmpty.name() = "entity empty";
+
entity.name() = "entity";
entity.addSnapshot(snapshot);
+ entity.addSnapshot(snapshotEmpty);
}
{
+ provSegEmpty.name() = "provider segment empty";
+
provSeg.name() = "provider segment";
provSeg.addEntity(entity);
+ provSeg.addEntity(entityEmpty);
}
{
+ coreSegEmpty.name() = "core segment empty";
+
coreSeg.name() = "core segment";
coreSeg.addProviderSegment(provSeg);
+ coreSeg.addProviderSegment(provSegEmpty);
}
{
+ memoryEmpty.name() = "memory empty";
+
memory.name() = "memory";
memory.addCoreSegment(coreSeg);
+ memory.addCoreSegment(coreSegEmpty);
}
memoryID = memory.id();
@@ -85,22 +106,18 @@ namespace ArMemGetFindTest
snapshotID = entityID.withTimestamp(snapshot.time());
instanceID = snapshotID.withInstanceIndex(0);
}
+
~Fixture()
{
}
-
template <class ParentT>
- void test_get_find_instance_by_id(ParentT&& parent)
- {
- _test_get_find_instance_by_id(const_cast<const ParentT&>(parent));
- _test_get_find_instance_by_id(const_cast<ParentT&>(parent));
- }
- template <class ParentT>
- void _test_get_find_instance_by_id(ParentT&& parent)
+ void
+ _test_get_find_instance_by_id(ParentT&& parent)
{
BOOST_TEST_CONTEXT("Parent: " << armem::print(parent))
{
+ BOOST_CHECK_EQUAL(parent.hasInstances(), true);
BOOST_CHECK_EQUAL(parent.hasInstance(snapshotID.withInstanceIndex(0)), true);
BOOST_CHECK_EQUAL(parent.hasInstance(snapshotID.withInstanceIndex(1)), false);
@@ -108,106 +125,240 @@ namespace ArMemGetFindTest
BOOST_CHECK_EQUAL(parent.findInstance(snapshotID.withInstanceIndex(1)), nullptr);
BOOST_CHECK_NO_THROW(parent.getInstance(snapshotID.withInstanceIndex(0)));
- BOOST_CHECK_THROW(parent.getInstance(snapshotID.withInstanceIndex(1)), armem::error::MissingEntry);
+ BOOST_CHECK_THROW(parent.getInstance(snapshotID.withInstanceIndex(1)),
+ armem::error::MissingEntry);
}
}
template <class ParentT>
- void test_get_find_snapshot_by_id(ParentT&& parent)
+ void
+ test_get_find_instance_by_id(ParentT&& parent)
{
- _test_get_find_snapshot_by_id(const_cast<const ParentT&>(parent));
- _test_get_find_snapshot_by_id(const_cast<ParentT&>(parent));
+ _test_get_find_instance_by_id(const_cast<const ParentT&>(parent));
+ _test_get_find_instance_by_id(const_cast<ParentT&>(parent));
}
+
template <class ParentT>
- void _test_get_find_snapshot_by_id(ParentT&& parent)
+ void
+ _test_get_find_latest_instance(ParentT&& parent)
{
BOOST_TEST_CONTEXT("Parent: " << armem::print(parent))
{
- BOOST_CHECK_EQUAL(parent.hasSnapshot(entityID.withTimestamp(armem::Time(armem::Duration::MicroSeconds(1000)))), true);
- BOOST_CHECK_EQUAL(parent.hasSnapshot(entityID.withTimestamp(armem::Time(armem::Duration::MicroSeconds(2000)))), false);
+ BOOST_CHECK_EQUAL(parent.hasInstances(), true);
+ BOOST_CHECK_NE(parent.findLatestInstance(), nullptr);
+ BOOST_CHECK_NO_THROW(parent.getLatestInstance());
+ }
+ }
- BOOST_CHECK_NE(parent.findSnapshot(entityID.withTimestamp(armem::Time(armem::Duration::MicroSeconds(1000)))), nullptr);
- BOOST_CHECK_EQUAL(parent.findSnapshot(entityID.withTimestamp(armem::Time(armem::Duration::MicroSeconds(2000)))), nullptr);
+ template <class ParentT>
+ void
+ test_get_find_latest_instance(ParentT&& parent)
+ {
+ _test_get_find_latest_instance(const_cast<const ParentT&>(parent));
+ _test_get_find_latest_instance(const_cast<ParentT&>(parent));
+ }
- BOOST_CHECK_NO_THROW(parent.getSnapshot(entityID.withTimestamp(armem::Time(armem::Duration::MicroSeconds(1000)))));
- BOOST_CHECK_THROW(parent.getSnapshot(entityID.withTimestamp(armem::Time(armem::Duration::MicroSeconds(2000)))), armem::error::MissingEntry);
+ template <class ParentT>
+ void
+ _test_get_find_latest_instance_when_empty(ParentT&& parent)
+ {
+ BOOST_TEST_CONTEXT("Parent: " << armem::print(parent))
+ {
+ BOOST_CHECK_EQUAL(parent.hasInstances(), false);
+ BOOST_CHECK_EQUAL(parent.findLatestInstance(), nullptr);
+ BOOST_CHECK_THROW(parent.getLatestInstance(), armem::error::NoSuchEntries);
}
}
+ template <class ParentT>
+ void
+ test_get_find_latest_instance_when_empty(ParentT&& parent)
+ {
+ _test_get_find_latest_instance_when_empty(const_cast<const ParentT&>(parent));
+ _test_get_find_latest_instance_when_empty(const_cast<ParentT&>(parent));
+ }
template <class ParentT>
- void test_get_find_entity_by_id(ParentT&& parent)
+ void
+ _test_get_find_snapshot_by_id(ParentT&& parent)
{
- _test_get_find_entity_by_id(const_cast<const ParentT&>(parent));
- _test_get_find_entity_by_id(const_cast<ParentT&>(parent));
+ BOOST_TEST_CONTEXT("Parent: " << armem::print(parent))
+ {
+ BOOST_CHECK_EQUAL(parent.hasSnapshots(), true);
+
+ BOOST_CHECK_EQUAL(parent.hasSnapshot(entityID.withTimestamp(
+ armem::Time(armem::Duration::MicroSeconds(1000)))),
+ true);
+ BOOST_CHECK_EQUAL(parent.hasSnapshot(entityID.withTimestamp(
+ armem::Time(armem::Duration::MicroSeconds(2000)))),
+ false);
+
+ BOOST_CHECK_NE(parent.findSnapshot(entityID.withTimestamp(
+ armem::Time(armem::Duration::MicroSeconds(1000)))),
+ nullptr);
+ BOOST_CHECK_EQUAL(parent.findSnapshot(entityID.withTimestamp(
+ armem::Time(armem::Duration::MicroSeconds(2000)))),
+ nullptr);
+
+ BOOST_CHECK_NO_THROW(parent.getSnapshot(
+ entityID.withTimestamp(armem::Time(armem::Duration::MicroSeconds(1000)))));
+ BOOST_CHECK_THROW(parent.getSnapshot(entityID.withTimestamp(
+ armem::Time(armem::Duration::MicroSeconds(2000)))),
+ armem::error::MissingEntry);
+ }
+ }
+
+ template <class ParentT>
+ void
+ test_get_find_snapshot_by_id(ParentT&& parent)
+ {
+ _test_get_find_snapshot_by_id(const_cast<const ParentT&>(parent));
+ _test_get_find_snapshot_by_id(const_cast<ParentT&>(parent));
}
+
template <class ParentT>
- void _test_get_find_entity_by_id(ParentT&& parent)
+ void
+ _test_get_find_entity_by_id(ParentT&& parent)
{
BOOST_TEST_CONTEXT("Parent: " << armem::print(parent))
{
BOOST_CHECK_EQUAL(parent.hasEntity(provSegID.withEntityName("entity")), true);
- BOOST_CHECK_EQUAL(parent.hasEntity(provSegID.withEntityName("other entity")), false);
+ BOOST_CHECK_EQUAL(parent.hasEntity(provSegID.withEntityName("other entity")),
+ false);
BOOST_CHECK_NE(parent.findEntity(provSegID.withEntityName("entity")), nullptr);
- BOOST_CHECK_EQUAL(parent.findEntity(provSegID.withEntityName("other entity")), nullptr);
+ BOOST_CHECK_EQUAL(parent.findEntity(provSegID.withEntityName("other entity")),
+ nullptr);
BOOST_CHECK_NO_THROW(parent.getEntity(provSegID.withEntityName("entity")));
- BOOST_CHECK_THROW(parent.getEntity(provSegID.withEntityName("other entity")), armem::error::MissingEntry);
+ BOOST_CHECK_THROW(parent.getEntity(provSegID.withEntityName("other entity")),
+ armem::error::MissingEntry);
}
}
template <class ParentT>
- void test_get_find_provider_segment_by_id(ParentT&& parent)
+ void
+ _test_get_find_latest_snapshot(ParentT&& parent)
{
- _test_get_find_provider_segment_by_id(const_cast<const ParentT&>(parent));
- _test_get_find_provider_segment_by_id(const_cast<ParentT&>(parent));
+ BOOST_TEST_CONTEXT("Parent: " << armem::print(parent))
+ {
+ BOOST_CHECK_EQUAL(parent.hasSnapshots(), true);
+ BOOST_CHECK_NE(parent.findLatestSnapshot(), nullptr);
+ BOOST_CHECK_NO_THROW(parent.getLatestSnapshot());
+ }
}
+
template <class ParentT>
- void _test_get_find_provider_segment_by_id(ParentT&& parent)
+ void
+ test_get_find_latest_snapshot(ParentT&& parent)
+ {
+ _test_get_find_latest_snapshot(const_cast<const ParentT&>(parent));
+ _test_get_find_latest_snapshot(const_cast<ParentT&>(parent));
+ }
+
+ template <class ParentT>
+ void
+ _test_get_find_latest_snapshot_when_empty(ParentT&& parent)
{
BOOST_TEST_CONTEXT("Parent: " << armem::print(parent))
{
- BOOST_CHECK_EQUAL(parent.hasProviderSegment(provSegID.withProviderSegmentName("provider segment")), true);
- BOOST_CHECK_EQUAL(parent.hasProviderSegment(provSegID.withProviderSegmentName("other provider segment")), false);
-
- BOOST_CHECK_NE(parent.findProviderSegment(provSegID.withProviderSegmentName("provider segment")), nullptr);
- BOOST_CHECK_EQUAL(parent.findProviderSegment(provSegID.withProviderSegmentName("other provider segment")), nullptr);
-
- BOOST_CHECK_NO_THROW(parent.getProviderSegment(provSegID.withProviderSegmentName("provider segment")));
- BOOST_CHECK_THROW(parent.getProviderSegment(provSegID.withProviderSegmentName("other provider segment")), armem::error::MissingEntry);
+ BOOST_CHECK_EQUAL(parent.hasSnapshots(), false);
+ BOOST_CHECK_EQUAL(parent.findLatestSnapshot(), nullptr);
+ BOOST_CHECK_THROW(parent.getLatestSnapshot(), armem::error::NoSuchEntries);
}
}
template <class ParentT>
- void test_get_find_core_segment_by_id(ParentT&& parent)
+ void
+ test_get_find_latest_snapshot_when_empty(ParentT&& parent)
{
- _test_get_find_core_segment_by_id(const_cast<const ParentT&>(parent));
- _test_get_find_core_segment_by_id(const_cast<ParentT&>(parent));
+ _test_get_find_latest_snapshot_when_empty(const_cast<const ParentT&>(parent));
+ _test_get_find_latest_snapshot_when_empty(const_cast<ParentT&>(parent));
}
+
template <class ParentT>
- void _test_get_find_core_segment_by_id(ParentT&& parent)
+ void
+ test_get_find_entity_by_id(ParentT&& parent)
+ {
+ _test_get_find_entity_by_id(const_cast<const ParentT&>(parent));
+ _test_get_find_entity_by_id(const_cast<ParentT&>(parent));
+ }
+
+ template <class ParentT>
+ void
+ _test_get_find_provider_segment_by_id(ParentT&& parent)
{
BOOST_TEST_CONTEXT("Parent: " << armem::print(parent))
{
- BOOST_CHECK_EQUAL(parent.hasCoreSegment(provSegID.withCoreSegmentName("core segment")), true);
- BOOST_CHECK_EQUAL(parent.hasCoreSegment(provSegID.withCoreSegmentName("other core segment")), false);
+ BOOST_CHECK_EQUAL(parent.hasProviderSegment(
+ provSegID.withProviderSegmentName("provider segment")),
+ true);
+ BOOST_CHECK_EQUAL(parent.hasProviderSegment(
+ provSegID.withProviderSegmentName("other provider segment")),
+ false);
+
+ BOOST_CHECK_NE(parent.findProviderSegment(
+ provSegID.withProviderSegmentName("provider segment")),
+ nullptr);
+ BOOST_CHECK_EQUAL(parent.findProviderSegment(
+ provSegID.withProviderSegmentName("other provider segment")),
+ nullptr);
+
+ BOOST_CHECK_NO_THROW(parent.getProviderSegment(
+ provSegID.withProviderSegmentName("provider segment")));
+ BOOST_CHECK_THROW(parent.getProviderSegment(
+ provSegID.withProviderSegmentName("other provider segment")),
+ armem::error::MissingEntry);
+ }
+ }
- BOOST_CHECK_NE(parent.findCoreSegment(provSegID.withCoreSegmentName("core segment")), nullptr);
- BOOST_CHECK_EQUAL(parent.findCoreSegment(provSegID.withCoreSegmentName("other core segment")), nullptr);
+ template <class ParentT>
+ void
+ test_get_find_provider_segment_by_id(ParentT&& parent)
+ {
+ _test_get_find_provider_segment_by_id(const_cast<const ParentT&>(parent));
+ _test_get_find_provider_segment_by_id(const_cast<ParentT&>(parent));
+ }
- BOOST_CHECK_NO_THROW(parent.getCoreSegment(provSegID.withCoreSegmentName("core segment")));
- BOOST_CHECK_THROW(parent.getCoreSegment(provSegID.withCoreSegmentName("other core segment")), armem::error::MissingEntry);
+ template <class ParentT>
+ void
+ _test_get_find_core_segment_by_id(ParentT&& parent)
+ {
+ BOOST_TEST_CONTEXT("Parent: " << armem::print(parent))
+ {
+ BOOST_CHECK_EQUAL(
+ parent.hasCoreSegment(provSegID.withCoreSegmentName("core segment")), true);
+ BOOST_CHECK_EQUAL(
+ parent.hasCoreSegment(provSegID.withCoreSegmentName("other core segment")),
+ false);
+
+ BOOST_CHECK_NE(
+ parent.findCoreSegment(provSegID.withCoreSegmentName("core segment")), nullptr);
+ BOOST_CHECK_EQUAL(
+ parent.findCoreSegment(provSegID.withCoreSegmentName("other core segment")),
+ nullptr);
+
+ BOOST_CHECK_NO_THROW(
+ parent.getCoreSegment(provSegID.withCoreSegmentName("core segment")));
+ BOOST_CHECK_THROW(
+ parent.getCoreSegment(provSegID.withCoreSegmentName("other core segment")),
+ armem::error::MissingEntry);
}
}
+
+ template <class ParentT>
+ void
+ test_get_find_core_segment_by_id(ParentT&& parent)
+ {
+ _test_get_find_core_segment_by_id(const_cast<const ParentT&>(parent));
+ _test_get_find_core_segment_by_id(const_cast<ParentT&>(parent));
+ }
};
-}
+} // namespace ArMemGetFindTest
BOOST_FIXTURE_TEST_SUITE(ArMemGetFindTest, Fixture)
-
-
BOOST_AUTO_TEST_CASE(test_snapshot_get_find_instance_by_key)
{
BOOST_CHECK_EQUAL(snapshot.hasInstance(0), true);
@@ -220,20 +371,20 @@ BOOST_AUTO_TEST_CASE(test_snapshot_get_find_instance_by_key)
BOOST_CHECK_THROW(snapshot.getInstance(1), armem::error::MissingEntry);
}
-
BOOST_AUTO_TEST_CASE(test_entity_get_find_snapshot_by_key)
{
BOOST_CHECK_EQUAL(entity.hasSnapshot(armem::Time(armem::Duration::MicroSeconds(1000))), true);
BOOST_CHECK_EQUAL(entity.hasSnapshot(armem::Time(armem::Duration::MicroSeconds(2000))), false);
BOOST_CHECK_NE(entity.findSnapshot(armem::Time(armem::Duration::MicroSeconds(1000))), nullptr);
- BOOST_CHECK_EQUAL(entity.findSnapshot(armem::Time(armem::Duration::MicroSeconds(2000))), nullptr);
+ BOOST_CHECK_EQUAL(entity.findSnapshot(armem::Time(armem::Duration::MicroSeconds(2000))),
+ nullptr);
BOOST_CHECK_NO_THROW(entity.getSnapshot(armem::Time(armem::Duration::MicroSeconds(1000))));
- BOOST_CHECK_THROW(entity.getSnapshot(armem::Time(armem::Duration::MicroSeconds(2000))), armem::error::MissingEntry);
+ BOOST_CHECK_THROW(entity.getSnapshot(armem::Time(armem::Duration::MicroSeconds(2000))),
+ armem::error::MissingEntry);
}
-
BOOST_AUTO_TEST_CASE(test_provider_segment_get_find_entity_by_key)
{
BOOST_CHECK_EQUAL(provSeg.hasEntity("entity"), true);
@@ -246,7 +397,6 @@ BOOST_AUTO_TEST_CASE(test_provider_segment_get_find_entity_by_key)
BOOST_CHECK_THROW(provSeg.getEntity("other entity"), armem::error::MissingEntry);
}
-
BOOST_AUTO_TEST_CASE(test_core_segment_get_find_provider_segment_by_key)
{
BOOST_CHECK_EQUAL(coreSeg.hasProviderSegment("provider segment"), true);
@@ -256,10 +406,10 @@ BOOST_AUTO_TEST_CASE(test_core_segment_get_find_provider_segment_by_key)
BOOST_CHECK_EQUAL(coreSeg.findProviderSegment("other provider segment"), nullptr);
BOOST_CHECK_NO_THROW(coreSeg.getProviderSegment("provider segment"));
- BOOST_CHECK_THROW(coreSeg.getProviderSegment("other provider segment"), armem::error::MissingEntry);
+ BOOST_CHECK_THROW(coreSeg.getProviderSegment("other provider segment"),
+ armem::error::MissingEntry);
}
-
BOOST_AUTO_TEST_CASE(test_memory_get_find_core_segment_by_key)
{
BOOST_CHECK_EQUAL(memory.hasCoreSegment("core segment"), true);
@@ -272,66 +422,119 @@ BOOST_AUTO_TEST_CASE(test_memory_get_find_core_segment_by_key)
BOOST_CHECK_THROW(memory.getCoreSegment("other core segment"), armem::error::MissingEntry);
}
-
-
BOOST_AUTO_TEST_CASE(test_snapshot_get_find_instance_by_id)
{
test_get_find_instance_by_id(snapshot);
}
+
BOOST_AUTO_TEST_CASE(test_entity_get_find_instance_by_id)
{
test_get_find_instance_by_id(entity);
}
+
BOOST_AUTO_TEST_CASE(test_provider_segment_get_find_instance_by_id)
{
test_get_find_instance_by_id(provSeg);
}
+
BOOST_AUTO_TEST_CASE(test_core_segment_get_find_instance_by_id)
{
test_get_find_instance_by_id(coreSeg);
}
+
BOOST_AUTO_TEST_CASE(test_memory_get_find_instance_by_id)
{
test_get_find_instance_by_id(memory);
}
+BOOST_AUTO_TEST_CASE(test_entity_get_find_latest_instance)
+{
+ test_get_find_latest_instance(entity);
+ test_get_find_latest_instance_when_empty(entityEmpty);
+}
+
+BOOST_AUTO_TEST_CASE(test_provider_segment_get_find_latest_instance)
+{
+ test_get_find_latest_instance(provSeg);
+ test_get_find_latest_instance_when_empty(provSegEmpty);
+}
+
+BOOST_AUTO_TEST_CASE(test_core_segment_get_find_latest_instance)
+{
+ test_get_find_latest_instance(coreSeg);
+ test_get_find_latest_instance_when_empty(coreSegEmpty);
+}
+
+BOOST_AUTO_TEST_CASE(test_memory_get_find_latest_instance)
+{
+ test_get_find_latest_instance(memory);
+ test_get_find_latest_instance_when_empty(memoryEmpty);
+}
BOOST_AUTO_TEST_CASE(test_entity_get_find_snapshot_by_id)
{
test_get_find_snapshot_by_id(entity);
}
+
BOOST_AUTO_TEST_CASE(test_provider_segment_get_find_snapshot_by_id)
{
test_get_find_snapshot_by_id(provSeg);
}
+
BOOST_AUTO_TEST_CASE(test_core_segment_get_find_snapshot_by_id)
{
test_get_find_snapshot_by_id(coreSeg);
}
+
BOOST_AUTO_TEST_CASE(test_memory_get_find_snapshot_by_id)
{
test_get_find_snapshot_by_id(memory);
}
+BOOST_AUTO_TEST_CASE(test_entity_get_find_latest_snapshot)
+{
+ test_get_find_latest_snapshot(entity);
+ test_get_find_latest_snapshot_when_empty(entityEmpty);
+}
+
+BOOST_AUTO_TEST_CASE(test_provider_segment_get_find_latest_snapshot)
+{
+ test_get_find_latest_snapshot(provSeg);
+ test_get_find_latest_snapshot_when_empty(provSegEmpty);
+}
+
+BOOST_AUTO_TEST_CASE(test_core_segment_get_find_latest_snapshot)
+{
+ test_get_find_latest_snapshot(coreSeg);
+ test_get_find_latest_snapshot_when_empty(coreSegEmpty);
+}
+
+BOOST_AUTO_TEST_CASE(test_memory_get_find_latest_snapshot)
+{
+ test_get_find_latest_snapshot(memory);
+ test_get_find_latest_snapshot_when_empty(memoryEmpty);
+}
BOOST_AUTO_TEST_CASE(test_provider_segment_get_find_entity_by_id)
{
test_get_find_entity_by_id(provSeg);
}
+
BOOST_AUTO_TEST_CASE(test_core_segment_get_find_entity_by_id)
{
test_get_find_entity_by_id(coreSeg);
}
+
BOOST_AUTO_TEST_CASE(test_memory_get_find_entity_by_id)
{
test_get_find_entity_by_id(memory);
}
-
BOOST_AUTO_TEST_CASE(test_core_segment_get_find_provider_segment_by_id)
{
test_get_find_provider_segment_by_id(coreSeg);
}
+
BOOST_AUTO_TEST_CASE(test_memory_get_find_provider_segment_by_id)
{
test_get_find_provider_segment_by_id(memory);
@@ -342,6 +545,4 @@ BOOST_AUTO_TEST_CASE(test_memory_get_find_core_segment_by_id)
test_get_find_core_segment_by_id(memory);
}
-
-
BOOST_AUTO_TEST_SUITE_END()
diff --git a/source/RobotAPI/libraries/armem/test/CMakeLists.txt b/source/RobotAPI/libraries/armem/test/CMakeLists.txt
index da77a43b82a6174b756ddf1705e8c29ea3acabb2..688f0009e90f7540708e8e02fde2f7336944a471 100644
--- a/source/RobotAPI/libraries/armem/test/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem/test/CMakeLists.txt
@@ -2,6 +2,7 @@
# Libs required for the tests
SET(LIBS ${LIBS} ArmarXCore ${LIB_NAME})
+armarx_add_test(ArMemEntityInstanceDataTest ArMemEntityInstanceDataTest.cpp "${LIBS}")
armarx_add_test(ArMemForEachTest ArMemForEachTest.cpp "${LIBS}")
armarx_add_test(ArMemGetFindTest ArMemGetFindTest.cpp "${LIBS}")
armarx_add_test(ArMemIceConversionsTest ArMemIceConversionsTest.cpp "${LIBS}")
diff --git a/source/RobotAPI/libraries/armem_grasping/client/KnownGraspCandidateReader.cpp b/source/RobotAPI/libraries/armem_grasping/client/KnownGraspCandidateReader.cpp
index 76112d64cfa0788fadfbcc5e86f923b8bf1d978b..efe2d01c13731c5c32ba876a3837b2e1816ea41a 100644
--- a/source/RobotAPI/libraries/armem_grasping/client/KnownGraspCandidateReader.cpp
+++ b/source/RobotAPI/libraries/armem_grasping/client/KnownGraspCandidateReader.cpp
@@ -67,7 +67,7 @@ namespace armarx::armem::grasping::known_grasps
{ instance = &i; });
if (instance == nullptr)
{
- ARMARX_WARNING << "No entity snapshots found";
+ ARMARX_VERBOSE << "No entity snapshots found";
return std::nullopt;
}
return armem::grasping::arondto::KnownGraspInfo::FromAron(instance->data());
@@ -88,7 +88,7 @@ namespace armarx::armem::grasping::known_grasps
const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
- ARMARX_INFO << "Lookup result in reader: " << qResult;
+ ARMARX_VERBOSE << "Lookup result in reader: " << qResult;
if (not qResult.success) /* c++20 [[unlikely]] */
{
diff --git a/source/RobotAPI/libraries/armem_grasping/server/KnownGraspProviderSegment.cpp b/source/RobotAPI/libraries/armem_grasping/server/KnownGraspProviderSegment.cpp
index 00bd50a3d4721765f2287482bd31f09540d480a3..a3a55ac0fafa59c5b0997dc08b4ac2065b616a15 100644
--- a/source/RobotAPI/libraries/armem_grasping/server/KnownGraspProviderSegment.cpp
+++ b/source/RobotAPI/libraries/armem_grasping/server/KnownGraspProviderSegment.cpp
@@ -1,4 +1,6 @@
#include "KnownGraspProviderSegment.h"
+#include <VirtualRobot/Grasping/GraspSet.h>
+#include <VirtualRobot/XML/ObjectIO.h>
#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
#include <ArmarXCore/core/rapidxml/wrapper/RapidXmlReader.h>
@@ -26,83 +28,57 @@ namespace armarx::armem::grasping::segment
if (std::filesystem::is_regular_file(graspFilePath))
{
- auto reader = RapidXmlReader::FromFile(graspFilePath);
- RapidXmlReaderNode root = reader->getRoot();
-
- std::string objectClassNameXML = root.attribute_value("name");
- ARMARX_CHECK_EQUAL(objectClassName, objectClassNameXML);
+ ARMARX_INFO << "loading " << graspFilePath;
+ try {
+ auto manipulationObject = VirtualRobot::ObjectIO::loadManipulationObject(graspFilePath);
+
+ if(manipulationObject == nullptr)
+ {
+ ARMARX_WARNING << "Invalid file content: " << graspFilePath;
+ return std::nullopt;
+ }
- arondto::KnownGraspInfo ret;
- ret.correspondingObject.memoryName = "Object";
- ret.correspondingObject.coreSegmentName = "Class";
- ret.correspondingObject.providerSegmentName = "PriorKnowledgeData";
- ret.correspondingObject.entityName = info.idStr();
- ret.xml.package = fileLocInfo.package;
- ret.xml.path = fileLocInfo.relativePath;
+ arondto::KnownGraspInfo ret;
+ ret.correspondingObject.memoryName = "Object";
+ ret.correspondingObject.coreSegmentName = "Class";
+ ret.correspondingObject.providerSegmentName = "PriorKnowledgeData";
+ ret.correspondingObject.entityName = info.idStr();
+ ret.xml.package = fileLocInfo.package;
+ ret.xml.path = fileLocInfo.relativePath;
- for (const auto& graspSetNode : root.nodes())
- {
- if (graspSetNode.name() != "GraspSet")
+ for (const VirtualRobot::GraspSetPtr& graspSet : manipulationObject->getAllGraspSets())
{
- continue;
- }
+ ARMARX_CHECK_NOT_NULL(graspSet);
- arondto::KnownGraspSet retGraspSet;
+ arondto::KnownGraspSet retGraspSet;
- retGraspSet.name = graspSetNode.attribute_value("name");
- retGraspSet.robot = graspSetNode.attribute_value("RobotType");
- retGraspSet.endeffector = graspSetNode.attribute_value("EndEffector");
+ retGraspSet.name = graspSet->getName();
+ retGraspSet.robot = graspSet->getRobotType();
+ retGraspSet.endeffector = graspSet->getEndEffector();
- for (const auto& graspNode : graspSetNode.nodes())
- {
- if (graspNode.name() != "Grasp")
+ for (const VirtualRobot::GraspPtr& grasp : graspSet->getGrasps())
{
- continue;
- }
+ ARMARX_CHECK_NOT_NULL(grasp);
+
+ arondto::KnownGrasp retGrasp;
+
+ retGrasp.name = grasp->getName();
+ retGrasp.quality = grasp->getQuality();
+ retGrasp.creator = grasp->getCreationMethod();
+ retGrasp.pose = grasp->getTransformation();
- arondto::KnownGrasp retGrasp;
-
- retGrasp.name = graspNode.attribute_value("name");
- retGrasp.quality = std::stof(graspNode.attribute_value("quality"));
- retGrasp.creator = graspNode.attribute_value("Creation");
-
- ARMARX_CHECK(graspNode.nodes().size() == 2 or graspNode.nodes().size() == 1);
- RapidXmlReaderNode transformNode = graspNode.nodes()[0];
-
- ARMARX_CHECK_EQUAL(transformNode.name(), "Transform");
- ARMARX_CHECK_EQUAL(transformNode.nodes().size(), 1);
- RapidXmlReaderNode matrixNode = transformNode.nodes()[0];
-
- ARMARX_CHECK_EQUAL(matrixNode.nodes().size(), 4);
- RapidXmlReaderNode row0 = matrixNode.nodes()[0];
- RapidXmlReaderNode row1 = matrixNode.nodes()[1];
- RapidXmlReaderNode row2 = matrixNode.nodes()[2];
- RapidXmlReaderNode row3 = matrixNode.nodes()[3];
-
- retGrasp.pose(0, 0) = std::stof(row0.attribute_value("c1"));
- retGrasp.pose(0, 1) = std::stof(row0.attribute_value("c2"));
- retGrasp.pose(0, 2) = std::stof(row0.attribute_value("c3"));
- retGrasp.pose(0, 3) = std::stof(row0.attribute_value("c4"));
- retGrasp.pose(1, 0) = std::stof(row1.attribute_value("c1"));
- retGrasp.pose(1, 1) = std::stof(row1.attribute_value("c2"));
- retGrasp.pose(1, 2) = std::stof(row1.attribute_value("c3"));
- retGrasp.pose(1, 3) = std::stof(row1.attribute_value("c4"));
- retGrasp.pose(2, 0) = std::stof(row2.attribute_value("c1"));
- retGrasp.pose(2, 1) = std::stof(row2.attribute_value("c2"));
- retGrasp.pose(2, 2) = std::stof(row2.attribute_value("c3"));
- retGrasp.pose(2, 3) = std::stof(row2.attribute_value("c4"));
- retGrasp.pose(3, 0) = std::stof(row3.attribute_value("c1"));
- retGrasp.pose(3, 1) = std::stof(row3.attribute_value("c2"));
- retGrasp.pose(3, 2) = std::stof(row3.attribute_value("c3"));
- retGrasp.pose(3, 3) = std::stof(row3.attribute_value("c4"));
-
- ARMARX_VERBOSE << "Found grasp '" << retGrasp.name << "' in set '" << retGraspSet.name << "' for obj '" << objectClassName << "' with pose \n" << retGrasp.pose;
-
- retGraspSet.grasps.push_back(retGrasp);
+ ARMARX_VERBOSE << "Found grasp '" << retGrasp.name << "' in set '" << retGraspSet.name << "' for obj '" << objectClassName << "' with pose \n" << retGrasp.pose;
+
+ retGraspSet.grasps.push_back(retGrasp);
+ }
+ ret.graspSets[retGraspSet.name] = retGraspSet;
}
- ret.graspSets[retGraspSet.name] = retGraspSet;
+ return ret;
+
+ } catch (...) {
+ ARMARX_WARNING << graspFilePath << " is not a manipulation object!";
+ return std::nullopt;
}
- return ret;
}
return std::nullopt;
}
diff --git a/source/RobotAPI/libraries/armem_gui/instance/display_visitors/TypedDataDisplayVisitor.cpp b/source/RobotAPI/libraries/armem_gui/instance/display_visitors/TypedDataDisplayVisitor.cpp
index e9bb7bc4441bb2246ca2d360dc747a3398b4fdfd..b4c59389a918789222beb3595a29ec49320775df 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/display_visitors/TypedDataDisplayVisitor.cpp
+++ b/source/RobotAPI/libraries/armem_gui/instance/display_visitors/TypedDataDisplayVisitor.cpp
@@ -90,6 +90,17 @@ namespace armarx::aron
value << DataDisplayVisitor::getValue(data);
}
+ void
+ TypedDataDisplayVisitor::visitIntEnum(const data::VariantPtr& data,
+ const type::VariantPtr& type)
+ {
+ type::IntEnumPtr enumType = type::IntEnum::DynamicCast(type);
+ data::IntPtr enumData = data::Int::DynamicCast(data);
+
+ std::string name = enumType->getValueName(enumData->getValue());
+ value << name;
+ }
+
/*void TypedDataDisplayVisitor::visitDateTime(const data::VariantPtr& data, const type::VariantPtr& type)
{
auto l = data::Long::DynamicCastAndCheck(data);
diff --git a/source/RobotAPI/libraries/armem_gui/instance/display_visitors/TypedDataDisplayVisitor.h b/source/RobotAPI/libraries/armem_gui/instance/display_visitors/TypedDataDisplayVisitor.h
index cdbcf69129ff9a3224c3f14d8f87a168d54238c4..c5a6fefc266f3bdda9dfde3942554334473f02ea 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/display_visitors/TypedDataDisplayVisitor.h
+++ b/source/RobotAPI/libraries/armem_gui/instance/display_visitors/TypedDataDisplayVisitor.h
@@ -37,6 +37,8 @@ namespace armarx::aron
void visitLong(const data::VariantPtr& data, const type::VariantPtr& type) override;
void visitString(const data::VariantPtr& data, const type::VariantPtr& type) override;
+ void visitIntEnum(const data::VariantPtr& data, const type::VariantPtr& type) override;
+
void visitMatrix(const data::VariantPtr& data, const type::VariantPtr& type) override;
void visitQuaternion(const data::VariantPtr& data, const type::VariantPtr& type) override;
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 8b2b4f1d70cc65a416c8aea58c3bd165b1bcdb3f..1447684d0e223660c00703f3ea0403fcf15c74d0 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/tree_builders/TypedDataTreeBuilder.cpp
+++ b/source/RobotAPI/libraries/armem_gui/instance/tree_builders/TypedDataTreeBuilder.cpp
@@ -2,18 +2,16 @@
#include <QTreeWidgetItem>
-#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/aron/MemoryID.aron.generated.h>
-
+#include <RobotAPI/libraries/armem/core/MemoryID.h>
+#include <RobotAPI/libraries/armem/core/aron_conversions.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>
+#include <RobotAPI/libraries/armem_gui/instance/display_visitors/TypedDataDisplayVisitor.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/aron/common/aron_conversions.h>
namespace armarx::armem::gui::instance
{
@@ -22,135 +20,138 @@ namespace armarx::armem::gui::instance
{
}
-
- void TypedDataTreeBuilder::updateTree(
- QTreeWidgetItem* parent,
- const aron::type::Dict& type,
- const aron::data::Dict& data)
+ void
+ TypedDataTreeBuilder::updateTree(QTreeWidgetItem* parent,
+ const aron::type::Dict& type,
+ const aron::data::Dict& data)
{
auto childType = type.getAcceptedType();
DictBuilder builder = getDictBuilder();
- builder.setUpdateItemFn([this, &childType, &data](const std::string & key, QTreeWidgetItem * item)
- {
- auto childData = data.getElement(key);
- if (childData)
+ builder.setUpdateItemFn(
+ [this, &childType, &data](const std::string& key, QTreeWidgetItem* item)
{
- this->updateDispatch(item, key, childType, childData);
- }
- return true;
- });
+ auto childData = data.getElement(key);
+ if (childData)
+ {
+ this->updateDispatch(item, key, childType, childData);
+ }
+ return true;
+ });
builder.updateTreeWithContainer(parent, data.getAllKeys());
}
- void TypedDataTreeBuilder::updateTree(
- QTreeWidgetItem* parent,
- const aron::type::AnyObject& type,
- const aron::data::Dict& data)
+ void
+ TypedDataTreeBuilder::updateTree(QTreeWidgetItem* parent,
+ const aron::type::AnyObject& type,
+ const aron::data::Dict& data)
{
DictBuilder builder = getDictBuilder();
- builder.setUpdateItemFn([this, &data](const std::string & key, QTreeWidgetItem * item)
- {
- auto childData = data.getElement(key);
- if (childData)
+ builder.setUpdateItemFn(
+ [this, &data](const std::string& key, QTreeWidgetItem* item)
{
- this->updateDispatch(item, key, nullptr, childData);
- }
- return true;
- });
+ auto childData = data.getElement(key);
+ if (childData)
+ {
+ this->updateDispatch(item, key, nullptr, childData);
+ }
+ return true;
+ });
builder.updateTreeWithContainer(parent, data.getAllKeys());
}
-
- void TypedDataTreeBuilder::updateTree(
- QTreeWidgetItem* parent,
- const aron::type::Object& type,
- const aron::data::Dict& data)
+ void
+ TypedDataTreeBuilder::updateTree(QTreeWidgetItem* parent,
+ const aron::type::Object& type,
+ const aron::data::Dict& data)
{
DictBuilder builder = getDictBuilder();
- builder.setMakeItemFn([this, &type](const std::string & key) -> QTreeWidgetItem*
- {
- if (type.hasMemberType(key)
- && type.getMemberType(key)->getFullName() == instance::rawMemoryIDTypeName)
+ builder.setMakeItemFn(
+ [this, &type](const std::string& key) -> QTreeWidgetItem*
{
- MemoryIDTreeWidgetItem* item = new MemoryIDTreeWidgetItem({QString::fromStdString(key)});
- item->addKeyChildren();
- return item;
- }
- else
+ if (type.hasMemberType(key) &&
+ type.getMemberType(key)->getFullName() == 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)
{
- return this->makeItem(key);
- }
- });
- builder.setUpdateItemFn([this, &type, &data](const std::string & key, QTreeWidgetItem * item)
- {
- auto childData = data.getElement(key);
-
- // We need this check here because getMemberType(key) throws
- // instead of returning nullptr if the type doesn't have the key.
- if (type.hasMemberType(key))
- {
- this->updateDispatch(item, key, type.getMemberType(key), childData);
- }
- else
- {
- this->updateDispatch(item, key, nullptr, childData);
- }
- return true;
- });
+ auto childData = data.getElement(key);
+
+ // We need this check here because getMemberType(key) throws
+ // instead of returning nullptr if the type doesn't have the key.
+ if (type.hasMemberType(key))
+ {
+ this->updateDispatch(item, key, type.getMemberType(key), childData);
+ }
+ else
+ {
+ this->updateDispatch(item, key, nullptr, childData);
+ }
+ return true;
+ });
builder.updateTreeWithContainer(parent, data.getAllKeys());
}
-
- void TypedDataTreeBuilder::updateTree(QTreeWidgetItem* parent,
- const aron::type::List& type,
- const aron::data::List& data)
+ void
+ TypedDataTreeBuilder::updateTree(QTreeWidgetItem* parent,
+ const aron::type::List& type,
+ const aron::data::List& data)
{
auto childType = type.getAcceptedType();
auto children = data.getChildren();
ListBuilder builder = getListBuilder();
- builder.setUpdateItemFn([this, &children, &childType](size_t key, QTreeWidgetItem * item)
- {
- if (auto childData = children.at(key))
+ builder.setUpdateItemFn(
+ [this, &children, &childType](size_t key, QTreeWidgetItem* item)
{
- this->updateDispatch(item, std::to_string(key), childType, childData);
- }
- return true;
- });
+ if (auto childData = children.at(key))
+ {
+ this->updateDispatch(item, std::to_string(key), childType, childData);
+ }
+ return true;
+ });
builder.updateTreeWithContainer(parent, getIndex(children.size()));
}
-
- void TypedDataTreeBuilder::updateTree(
- QTreeWidgetItem* parent,
- const aron::type::Pair& type,
- const aron::data::List& data)
+ void
+ TypedDataTreeBuilder::updateTree(QTreeWidgetItem* parent,
+ const aron::type::Pair& type,
+ const aron::data::List& data)
{
ARMARX_CHECK_EQUAL(data.childrenSize(), 2);
auto childTypes = type.getAcceptedTypes();
ListBuilder builder = getListBuilder();
- builder.setUpdateItemFn([this, &data, &childTypes](size_t i, QTreeWidgetItem * item)
- {
- auto childType = i == 0 ? childTypes.first : childTypes.second;
- auto childData = data.getElement(static_cast<unsigned int>(i));
+ builder.setUpdateItemFn(
+ [this, &data, &childTypes](size_t i, QTreeWidgetItem* item)
+ {
+ auto childType = i == 0 ? childTypes.first : childTypes.second;
+ auto childData = data.getElement(static_cast<unsigned int>(i));
- this->updateDispatch(item, std::to_string(i), childType, childData);
- return true;
- });
+ this->updateDispatch(item, std::to_string(i), childType, childData);
+ return true;
+ });
builder.updateTreeWithContainer(parent, getIndex(data.childrenSize()));
}
-
- void TypedDataTreeBuilder::updateTree(
- QTreeWidgetItem* parent,
- const aron::type::Tuple& type,
- const aron::data::List& data)
+ void
+ TypedDataTreeBuilder::updateTree(QTreeWidgetItem* parent,
+ const aron::type::Tuple& type,
+ const aron::data::List& data)
{
// Allows tuples where the data list is longer than the type tuple -
// is that desired behavior?
@@ -158,28 +159,28 @@ namespace armarx::armem::gui::instance
auto childTypes = type.getAcceptedTypes();
ListBuilder builder = getListBuilder();
- builder.setUpdateItemFn([this, &data, &childTypes](size_t i, QTreeWidgetItem * item)
- {
- auto childType = (i < childTypes.size()) ? childTypes.at(i) : nullptr;
- auto childData = data.getElement(static_cast<unsigned int>(i));
+ builder.setUpdateItemFn(
+ [this, &data, &childTypes](size_t i, QTreeWidgetItem* item)
+ {
+ auto childType = (i < childTypes.size()) ? childTypes.at(i) : nullptr;
+ auto childData = data.getElement(static_cast<unsigned int>(i));
- this->updateDispatch(item, std::to_string(i), childType, childData);
- return true;
- });
+ this->updateDispatch(item, std::to_string(i), childType, childData);
+ return true;
+ });
builder.updateTreeWithContainer(parent, getIndex(data.childrenSize()));
}
-
/*! Used so that elements in the data that don't appear in the type
* can still be shown in the GUI if type information is enabled
* (otherwise, they would be hidden).
*/
- void TypedDataTreeBuilder::updateDispatch(
- QTreeWidgetItem* item,
- const std::string& key,
- const aron::type::VariantPtr& type,
- const aron::data::VariantPtr& data)
+ void
+ TypedDataTreeBuilder::updateDispatch(QTreeWidgetItem* item,
+ const std::string& key,
+ const aron::type::VariantPtr& type,
+ const aron::data::VariantPtr& data)
{
if (type)
{
@@ -191,17 +192,27 @@ namespace armarx::armem::gui::instance
}
}
-
- void TypedDataTreeBuilder::update(
- QTreeWidgetItem* item,
- const std::string& key,
- const aron::type::VariantPtr& type,
- const aron::data::VariantPtr& data)
+ void
+ TypedDataTreeBuilder::update(QTreeWidgetItem* item,
+ const std::string& key,
+ const aron::type::VariantPtr& type,
+ const aron::data::VariantPtr& data)
{
using namespace aron;
- const std::string value = data ? aron::TypedDataDisplayVisitor::getValue(type, data) : "(none)";
- std::string typeName = instance::sanitizeTypeName(type->getFullName());
+ const std::string value =
+ data ? aron::TypedDataDisplayVisitor::getValue(type, data) : "(none)";
+
+ std::string typeName;
+ if (type::IntEnumPtr enumType = type::IntEnum::DynamicCast(type))
+ {
+ typeName = enumType->getEnumName();
+ }
+ else
+ {
+ typeName = type->getFullName();
+ }
+ typeName = instance::sanitizeTypeName(typeName);
switch (type->getMaybe())
{
case aron::type::Maybe::OPTIONAL:
@@ -213,7 +224,9 @@ namespace armarx::armem::gui::instance
setRowTexts(item, key, value, typeName);
- item->setData(columnKey, Qt::UserRole, data ? instance::serializePath(data->getPath()) : QStringList());
+ item->setData(columnKey,
+ Qt::UserRole,
+ data ? instance::serializePath(data->getPath()) : QStringList());
item->setData(columnType, Qt::UserRole, static_cast<int>(type->getDescriptor()));
if (typeName == sanitizedMemoryIDTypeName)
@@ -228,50 +241,57 @@ namespace armarx::armem::gui::instance
MemoryID id = aron::fromAron<MemoryID>(dto);
memoryIDItem->setInstanceID(id);
- return; // Done, no recursion.
+ return; // Done, no recursion.
}
}
// We pass empty containers if data is null so that subitems of the data are deleted.
auto emptyDict = aron::data::Dict(type->getPath());
auto emptyList = aron::data::List(type->getPath());
- if (const auto d = aron::data::Dict::DynamicCast(data); const auto t = type::Object::DynamicCast(type))
+ if (const auto d = aron::data::Dict::DynamicCast(data);
+ const auto t = type::Object::DynamicCast(type))
{
_updateTree(item, *t, d ? *d : emptyDict);
}
- else if (const auto d = aron::data::Dict::DynamicCast(data); const auto t = type::Dict::DynamicCast(type))
+ else if (const auto d = aron::data::Dict::DynamicCast(data);
+ const auto t = type::Dict::DynamicCast(type))
{
_updateTree(item, *t, d ? *d : emptyDict);
}
- else if (const auto d = aron::data::List::DynamicCast(data); const auto t = type::List::DynamicCast(type))
+ else if (const auto d = aron::data::List::DynamicCast(data);
+ const auto t = type::List::DynamicCast(type))
{
_updateTree(item, *t, d ? *d : emptyList);
}
- else if (const auto d = aron::data::List::DynamicCast(data); const auto t = type::Pair::DynamicCast(type))
+ else if (const auto d = aron::data::List::DynamicCast(data);
+ const auto t = type::Pair::DynamicCast(type))
{
_updateTree(item, *t, d ? *d : emptyList);
}
- else if (const auto d = aron::data::List::DynamicCast(data); const auto t = type::Tuple::DynamicCast(type))
+ else if (const auto d = aron::data::List::DynamicCast(data);
+ const auto t = type::Tuple::DynamicCast(type))
{
_updateTree(item, *t, d ? *d : emptyList);
}
- else if (const auto d = aron::data::Dict::DynamicCast(data); const auto t = type::AnyObject::DynamicCast(type))
+ else if (const auto d = aron::data::Dict::DynamicCast(data);
+ const auto t = type::AnyObject::DynamicCast(type))
{
_updateTree(item, *t, d ? *d : emptyDict);
}
}
-
- void TypedDataTreeBuilder::update(QTreeWidgetItem* item,
- const std::string& key,
- const aron::data::VariantPtr& data)
+ void
+ TypedDataTreeBuilder::update(QTreeWidgetItem* item,
+ const std::string& key,
+ const aron::data::VariantPtr& data)
{
if (data)
{
this->setRowTexts(item, key, data);
- item->setData(columnKey, Qt::UserRole,
- data ? instance::serializePath(data->getPath()) : QStringList());
+ item->setData(columnKey,
+ Qt::UserRole,
+ data ? instance::serializePath(data->getPath()) : QStringList());
if (auto cast = aron::data::Dict::DynamicCast(data))
{
@@ -290,11 +310,11 @@ namespace armarx::armem::gui::instance
}
}
-
template <class DataT, class TypeT>
- void TypedDataTreeBuilder::_updateTree(QTreeWidgetItem* item, TypeT& type, DataT& data)
+ void
+ TypedDataTreeBuilder::_updateTree(QTreeWidgetItem* item, TypeT& type, DataT& data)
{
updateTree(item, type, data);
}
-}
+} // namespace armarx::armem::gui::instance
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectWriter.cpp b/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectWriter.cpp
index 0b466d404fbc41d5eee48819418dd5b5cae35d1f..5e5b15f15d8135b7de89386c7ba95a7d65f29b9e 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectWriter.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectWriter.cpp
@@ -10,7 +10,6 @@
#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
-
namespace armarx::armem::articulated_object
{
armem::articulated_object::ArticulatedObject
@@ -37,13 +36,14 @@ namespace armarx::armem::articulated_object
return armem::articulated_object::ArticulatedObject{
.description = {.name = obj.getType(),
- .xml = PackagePath(armarx::ArmarXDataPath::getProject(
- {package}, fileRelPath),
- obj.getFilename())},
+ .xml = PackagePath(
+ armarx::ArmarXDataPath::getProject({package}, fileRelPath),
+ obj.getFilename())},
.instance = obj.getName(),
.config = {.timestamp = timestamp,
.globalPose = Eigen::Affine3f(obj.getRootNode()->getGlobalPose()),
- .jointMap = obj.getJointValues()},
+ .jointMap = obj.getJointValues(),
+ .proprioception = std::nullopt},
.timestamp = timestamp};
}
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 dd8a8473c2561c058c5754e59fb5997fdddec40e..ac01fb3b87a1dc4bc53b40982a38002a80cc0d23 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.cpp
@@ -262,7 +262,7 @@ namespace armarx::armem::articulated_object
robot::RobotState robotState{.timestamp = objectPose.timestamp,
.globalPose = Eigen::Affine3f(objectPose.objectPoseGlobal),
- .jointMap = objectPose.objectJointValues};
+ .jointMap = objectPose.objectJointValues, .proprioception=std::nullopt};
return robotState;
}
diff --git a/source/RobotAPI/libraries/armem_robot/aron/RobotState.xml b/source/RobotAPI/libraries/armem_robot/aron/RobotState.xml
index 4ef29cf75b7e9a86727a4ab62041b16714742eb7..38ea8a81548cbb2fe52c02627493309f6339772f 100644
--- a/source/RobotAPI/libraries/armem_robot/aron/RobotState.xml
+++ b/source/RobotAPI/libraries/armem_robot/aron/RobotState.xml
@@ -4,6 +4,9 @@
<CodeIncludes>
<SystemInclude include="<Eigen/Core>" />
</CodeIncludes>
+ <AronIncludes>
+ <PackagePath package="RobotAPI" path="libraries/armem_robot_state/aron/Proprioception.xml" />
+ </AronIncludes>
<GenerateTypes>
<Object name="armarx::armem::arondto::RobotState">
@@ -22,6 +25,9 @@
</Dict>
</ObjectChild>
+ <ObjectChild key='proprioception'>
+ <armarx::armem::arondto::Proprioception optional="true"/>
+ </ObjectChild>
</Object>
diff --git a/source/RobotAPI/libraries/armem_robot/client/interfaces.h b/source/RobotAPI/libraries/armem_robot/client/interfaces.h
index 4975e53e597f54b3f451c7075e368f00f692d343..64d68ed6a3549fa7b0e6fa5745a1cc5731158a38 100644
--- a/source/RobotAPI/libraries/armem_robot/client/interfaces.h
+++ b/source/RobotAPI/libraries/armem_robot/client/interfaces.h
@@ -12,10 +12,10 @@ namespace armarx::armem::robot
public:
virtual ~ReaderInterface() = default;
- virtual bool synchronize(Robot& obj, const armem::Time& timestamp) = 0;
+ virtual bool synchronize(Robot& obj, const armem::Time& timestamp) const = 0;
- virtual Robot get(const RobotDescription& description, const armem::Time& timestamp) = 0;
- virtual std::optional<Robot> get(const std::string& name, const armem::Time& timestamp) = 0;
+ virtual Robot get(const RobotDescription& description, const armem::Time& timestamp) const = 0;
+ virtual std::optional<Robot> get(const std::string& name, const armem::Time& timestamp) const = 0;
};
class WriterInterface
diff --git a/source/RobotAPI/libraries/armem_robot/types.h b/source/RobotAPI/libraries/armem_robot/types.h
index 2bc45c144b53e3900e8c7041c54d33ee0d205f56..9caf84f24574be547036e94fc0f07e50680d0fec 100644
--- a/source/RobotAPI/libraries/armem_robot/types.h
+++ b/source/RobotAPI/libraries/armem_robot/types.h
@@ -10,7 +10,7 @@
#include <ArmarXCore/core/time/DateTime.h>
#include <RobotAPI/libraries/ArmarXObjects/ObjectID.h>
-
+#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
namespace armarx::armem::robot
{
@@ -37,7 +37,9 @@ namespace armarx::armem::robot
struct ForceTorque
{
Eigen::Vector3f force;
+ Eigen::Vector3f gravityCompensatedForce;
Eigen::Vector3f torque;
+ Eigen::Vector3f gravityCompensatedTorque;
};
using ToFArray = Eigen::MatrixXf;
@@ -58,6 +60,8 @@ namespace armarx::armem::robot
Pose globalPose;
JointMap jointMap;
+
+ std::optional<armarx::armem::arondto::Proprioception> proprioception;
};
diff --git a/source/RobotAPI/libraries/armem_robot_state/CMakeLists.txt b/source/RobotAPI/libraries/armem_robot_state/CMakeLists.txt
index 8dda90015cd030197cd4af43fd2b9d534968fc19..c71d1d839e5511f73cb67b13d8c80c28def503d4 100644
--- a/source/RobotAPI/libraries/armem_robot_state/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem_robot_state/CMakeLists.txt
@@ -63,7 +63,6 @@ armarx_enable_aron_file_generation_for_target(
"${LIB_NAME}"
ARON_FILES
- aron/JointState.xml
aron/Proprioception.xml
aron/Exteroception.xml
aron/TransformHeader.xml
diff --git a/source/RobotAPI/libraries/armem_robot_state/aron/JointState.xml b/source/RobotAPI/libraries/armem_robot_state/aron/JointState.xml
deleted file mode 100644
index 5d18e8f78b6c99d58aab69ac2a154d5cb580bd63..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem_robot_state/aron/JointState.xml
+++ /dev/null
@@ -1,14 +0,0 @@
-<!--This class contains the data structure for ObjectPose -->
-<?xml version="1.0" encoding="UTF-8" ?>
-<AronTypeDefinition>
- <GenerateTypes>
- <Object name="armarx::armem::arondto::JointState">
- <ObjectChild key='name'>
- <String/>
- </ObjectChild>
- <ObjectChild key='position'>
- <float32 />
- </ObjectChild>
- </Object>
- </GenerateTypes>
-</AronTypeDefinition>
diff --git a/source/RobotAPI/libraries/armem_robot_state/aron_conversions.cpp b/source/RobotAPI/libraries/armem_robot_state/aron_conversions.cpp
index 3749c892d5e46e6ce0f7a848a86e8059ed70991c..78014ea76bf070f08f01212719da1c1be7483fa3 100644
--- a/source/RobotAPI/libraries/armem_robot_state/aron_conversions.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/aron_conversions.cpp
@@ -4,7 +4,6 @@
#include <ArmarXCore/core/logging/Logging.h>
-#include <RobotAPI/libraries/armem_robot_state/aron/JointState.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/aron/Exteroception.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/aron/Transform.aron.generated.h>
@@ -51,23 +50,6 @@ namespace armarx::armem
aron::fromAron(dto.timestamp, bo.timestamp);
}
- /* JointState */
-
- void
- fromAron(const arondto::JointState& dto, robot_state::JointState& bo)
- {
- aron::fromAron(dto.name, bo.name);
- aron::fromAron(dto.position, bo.position);
- }
-
- void
- toAron(arondto::JointState& dto, const robot_state::JointState& bo)
- {
- aron::toAron(dto.name, bo.name);
- aron::toAron(dto.position, bo.position);
- }
-
-
void
fromAron(const armarx::armem::prop::arondto::Platform& dto, robot::PlatformState& bo)
{
@@ -88,14 +70,18 @@ namespace armarx::armem
fromAron(const armarx::armem::prop::arondto::ForceTorque& dto, robot::ForceTorque& bo)
{
aron::fromAron(dto.force, bo.force);
+ aron::fromAron(dto.gravityCompensationForce, bo.gravityCompensatedForce);
aron::fromAron(dto.torque, bo.torque);
+ aron::fromAron(dto.gravityCompensationTorque, bo.gravityCompensatedTorque);
}
void
toAron(armarx::armem::prop::arondto::ForceTorque& dto, const robot::ForceTorque& bo)
{
aron::toAron(dto.force, bo.force);
+ aron::toAron(dto.gravityCompensationForce, bo.gravityCompensatedForce);
aron::toAron(dto.torque, bo.torque);
+ aron::toAron(dto.gravityCompensationTorque, bo.gravityCompensatedTorque);
}
diff --git a/source/RobotAPI/libraries/armem_robot_state/aron_conversions.h b/source/RobotAPI/libraries/armem_robot_state/aron_conversions.h
index f78efc200ee18205523ddf7ea5c60e8c6949bf9f..a3e821caac1379d7413160583822b792f8f7c453 100644
--- a/source/RobotAPI/libraries/armem_robot_state/aron_conversions.h
+++ b/source/RobotAPI/libraries/armem_robot_state/aron_conversions.h
@@ -61,9 +61,6 @@ namespace armarx::armem
void fromAron(const arondto::TransformHeader& dto, robot_state::TransformHeader& bo);
void toAron(arondto::TransformHeader& dto, const robot_state::TransformHeader& bo);
- void fromAron(const arondto::JointState& dto, robot_state::JointState& bo);
- void toAron(arondto::JointState& dto, const robot_state::JointState& bo);
-
void fromAron(const armarx::armem::prop::arondto::Platform& dto, robot::PlatformState& bo);
void toAron(armarx::armem::prop::arondto::Platform& dto, const robot::PlatformState& bo);
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 7699417369cb2dae5d6cc59b8a1d7fabcd0b9251..5fb5004c926f4997a9b3e252e8a882c92d0462ea 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp
@@ -23,7 +23,6 @@
#include <RobotAPI/libraries/armem_robot/robot_conversions.h>
#include <RobotAPI/libraries/armem_robot/types.h>
#include <RobotAPI/libraries/armem_robot_state/aron/Exteroception.aron.generated.h>
-#include <RobotAPI/libraries/armem_robot_state/aron/JointState.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/aron_conversions.h>
@@ -66,7 +65,7 @@ namespace armarx::armem::robot_state
}
std::optional<robot::Robot>
- RobotReader::get(const std::string& name, const armem::Time& timestamp)
+ RobotReader::get(const std::string& name, const armem::Time& timestamp) const
{
const auto description = queryDescription(name, timestamp);
@@ -80,7 +79,7 @@ namespace armarx::armem::robot_state
}
robot::Robot
- RobotReader::get(const robot::RobotDescription& description, const armem::Time& timestamp)
+ RobotReader::get(const robot::RobotDescription& description, const armem::Time& timestamp) const
{
robot::Robot robot{.description = description,
.instance = "", // TODO(fabian.reister):
@@ -106,7 +105,7 @@ namespace armarx::armem::robot_state
}
bool
- RobotReader::synchronize(robot::Robot& obj, const armem::Time& timestamp)
+ RobotReader::synchronize(robot::Robot& obj, const armem::Time& timestamp) const
{
const auto tsStartFunctionInvokation = armem::Time::Now();
@@ -136,7 +135,7 @@ namespace armarx::armem::robot_state
}
std::optional<robot::RobotDescription>
- RobotReader::queryDescription(const std::string& name, const armem::Time& timestamp)
+ RobotReader::queryDescription(const std::string& name, const armem::Time& timestamp) const
{
const auto sanitizedTimestamp = timestamp.isValid() ? timestamp : Clock::Now();
@@ -184,14 +183,17 @@ namespace armarx::armem::robot_state
std::optional<robot::RobotState>
RobotReader::queryState(const robot::RobotDescription& description,
- const armem::Time& timestamp)
+ const armem::Time& timestamp) const
{
- const auto jointMap = queryJointState(description, timestamp);
- if (not jointMap)
+ const auto proprioception = queryProprioception(description, timestamp);
+
+ if (not proprioception.has_value())
{
- ARMARX_VERBOSE << "Failed to query joint state for robot '" << description.name << "'.";
+ ARMARX_VERBOSE << "Failed to query proprioception for robot '" << description.name
+ << "'.";
return std::nullopt;
}
+ const auto jointMap = proprioception->joints.position;
const auto globalPose = queryGlobalPose(description, timestamp);
if (not globalPose)
@@ -200,8 +202,10 @@ namespace armarx::armem::robot_state
return std::nullopt;
}
- return robot::RobotState{
- .timestamp = timestamp, .globalPose = *globalPose, .jointMap = *jointMap};
+ return robot::RobotState{.timestamp = timestamp,
+ .globalPose = *globalPose,
+ .jointMap = jointMap,
+ .proprioception = proprioception};
}
std::optional<::armarx::armem::robot_state::Transform>
@@ -231,10 +235,9 @@ namespace armarx::armem::robot_state
// }
}
-
- std::optional<robot::RobotState::JointMap>
- RobotReader::queryJointState(const robot::RobotDescription& description,
- const armem::Time& timestamp) const // Why timestamp?!?!
+ std::optional<armarx::armem::arondto::Proprioception>
+ RobotReader::queryProprioception(const robot::RobotDescription& description,
+ const armem::Time& timestamp) const // Why timestamp?!?!
{
// TODO(fabian.reister): how to deal with multiple providers?
@@ -263,7 +266,7 @@ namespace armarx::armem::robot_state
return std::nullopt;
}
- return getRobotJointState(qResult.memory, description.name);
+ return getRobotProprioception(qResult.memory, description.name);
}
catch (...)
{
@@ -304,7 +307,6 @@ namespace armarx::armem::robot_state
return getRobotJointStates(qResult.memory, description.name);
}
-
std::optional<robot::PlatformState>
RobotReader::queryPlatformState(const robot::RobotDescription& description,
const armem::Time& timestamp) const
@@ -415,19 +417,19 @@ namespace armarx::armem::robot_state
}
}
- std::optional<robot::RobotState::JointMap>
- RobotReader::getRobotJointState(const armarx::armem::wm::Memory& memory,
- const std::string& name) const
+ std::optional<armarx::armem::arondto::Proprioception>
+ RobotReader::getRobotProprioception(const armarx::armem::wm::Memory& memory,
+ const std::string& name) const
{
// clang-format off
const armem::wm::CoreSegment& coreSegment = memory
.getCoreSegment(constants::proprioceptionCoreSegment);
// clang-format on
- std::optional<robot::RobotState::JointMap> jointMap;
+ std::optional<armarx::armem::arondto::Proprioception> proprioception;
coreSegment.forEachEntity(
- [&jointMap](const wm::Entity& entity)
+ [&proprioception](const wm::Entity& entity)
{
if (not entity.getLatestSnapshot().hasInstance(0))
{
@@ -436,25 +438,10 @@ namespace armarx::armem::robot_state
const auto& entityInstance = entity.getLatestSnapshot().getInstance(0);
- const auto proprioception =
- tryCast<::armarx::armem::arondto::Proprioception>(entityInstance);
- ARMARX_CHECK(proprioception.has_value());
-
- const armarx::armem::prop::arondto::Joints& joints = proprioception->joints;
-
- // const auto jointState = tryCast<::armarx::armem::arondto::JointState>(entityInstance);
- // if (not jointState)
- // {
- // ARMARX_WARNING << "Could not convert entity instance to 'JointState'";
- // return;
- // }
-
- jointMap = joints.position;
-
- // jointMap.emplace(jointState->name, jointState->position);
+ proprioception = tryCast<::armarx::armem::arondto::Proprioception>(entityInstance);
});
- return jointMap;
+ return proprioception;
}
RobotReader::JointTrajectory
@@ -497,7 +484,6 @@ namespace armarx::armem::robot_state
return jointTrajectory;
}
-
// force torque for left and right
std::optional<std::map<RobotReader::Hand, robot::ForceTorque>>
RobotReader::queryForceTorque(const robot::RobotDescription& description,
@@ -563,7 +549,6 @@ namespace armarx::armem::robot_state
return getForceTorques(qResult.memory, description.name);
}
-
std::optional<std::map<RobotReader::Hand, robot::ToFArray>>
RobotReader::queryToF(const robot::RobotDescription& description,
const armem::Time& timestamp) const
@@ -594,7 +579,6 @@ namespace armarx::armem::robot_state
return getToF(qResult.memory, description.name);
}
-
std::optional<robot::PlatformState>
RobotReader::getRobotPlatformState(const armarx::armem::wm::Memory& memory,
const std::string& name) const
@@ -755,7 +739,6 @@ namespace armarx::armem::robot_state
return tofs;
}
-
std::optional<robot::RobotDescription>
RobotReader::getRobotDescription(const armarx::armem::wm::Memory& memory,
const std::string& name) const
@@ -800,7 +783,7 @@ namespace armarx::armem::robot_state
}
std::vector<robot::RobotDescription>
- RobotReader::queryDescriptions(const armem::Time& timestamp)
+ RobotReader::queryDescriptions(const armem::Time& timestamp) const
{
const auto sanitizedTimestamp = timestamp.isValid() ? timestamp : Clock::Now();
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 ee1274fab20cadb62231bc7fdd685c408104e705..6f90a9b249286efb7d1db277ef3df492e0dd8319 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h
@@ -51,23 +51,23 @@ namespace armarx::armem::robot_state
virtual void registerPropertyDefinitions(::armarx::PropertyDefinitionsPtr& def);
- [[nodiscard]] bool synchronize(robot::Robot& obj, const armem::Time& timestamp) override;
+ [[nodiscard]] bool synchronize(robot::Robot& obj, const armem::Time& timestamp) const override;
std::optional<robot::Robot> get(const std::string& name,
- const armem::Time& timestamp) override;
+ const armem::Time& timestamp) const override;
robot::Robot get(const robot::RobotDescription& description,
- const armem::Time& timestamp) override;
+ const armem::Time& timestamp) const override;
std::optional<robot::RobotDescription> queryDescription(const std::string& name,
- const armem::Time& timestamp);
+ const armem::Time& timestamp) const;
- std::vector<robot::RobotDescription> queryDescriptions(const armem::Time& timestamp);
+ std::vector<robot::RobotDescription> queryDescriptions(const armem::Time& timestamp) const;
std::optional<robot::RobotState> queryState(const robot::RobotDescription& description,
- const armem::Time& timestamp);
+ const armem::Time& timestamp) const;
- std::optional<robot::RobotState::JointMap>
- queryJointState(const robot::RobotDescription& description,
+ std::optional<armarx::armem::arondto::Proprioception>
+ queryProprioception(const robot::RobotDescription& description,
const armem::Time& timestamp) const;
using JointTrajectory = std::map<armem::Time, robot::RobotState::JointMap>;
@@ -134,8 +134,8 @@ namespace armarx::armem::robot_state
std::vector<robot::RobotDescription>
getRobotDescriptions(const armarx::armem::wm::Memory& memory) const;
- std::optional<robot::RobotState::JointMap>
- getRobotJointState(const armarx::armem::wm::Memory& memory, const std::string& name) const;
+ std::optional<armarx::armem::arondto::Proprioception>
+ getRobotProprioception(const armarx::armem::wm::Memory& memory, const std::string& name) const;
JointTrajectory getRobotJointStates(const armarx::armem::wm::Memory& memory,
const std::string& name) const;
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotWriter.cpp b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotWriter.cpp
index cfa84fb3f50ad95db2f82b29114cd2606250a2c4..f971c88c3e41bf962500a1ac49e0b5ad7e5a39be 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotWriter.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotWriter.cpp
@@ -24,7 +24,6 @@
#include <RobotAPI/libraries/armem_robot/aron_conversions.h>
#include <RobotAPI/libraries/armem_robot/robot_conversions.h>
#include <RobotAPI/libraries/armem_robot/types.h>
-#include <RobotAPI/libraries/armem_robot_state/aron/JointState.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/aron/Transform.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/aron/TransformHeader.aron.generated.h>
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 76fa10a4c0186acecdb969ec69af75d97975708f..0e972f79138fbfc973dbab543206d94965227f03 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.cpp
@@ -32,7 +32,7 @@ namespace armarx::armem::robot_state
RobotReader::registerPropertyDefinitions(def);
}
- bool VirtualRobotReader::synchronizeRobot(VirtualRobot::Robot& robot, const armem::Time& timestamp)
+ bool VirtualRobotReader::synchronizeRobot(VirtualRobot::Robot& robot, const armem::Time& timestamp) const
{
// const static auto packages = armarx::CMakePackageFinder::FindAllArmarXSourcePackages();
// const auto package = armarx::ArmarXDataPath::getProject(packages, robot.getFilename());
@@ -117,5 +117,16 @@ namespace armarx::armem::robot_state
return nullptr;
}
+ std::optional<std::map<RobotReader::Hand, robot::ForceTorque>>
+ VirtualRobotReader::queryForceTorque(const std::string& name, const Time& timestamp)
+ {
+ const auto description = queryDescription(name, timestamp);
+ if (not description.has_value())
+ {
+ return std::nullopt;
+ }
+ return RobotReader::queryForceTorque(description.value(), timestamp);
+ }
+
} // 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 3de8fbe8b49c453a79a7cafbcf7904713f4d756f..7a2f39c17939d30b26ef75057e1c9d302b0f68a8 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h
@@ -47,7 +47,7 @@ namespace armarx::armem::robot_state
void registerPropertyDefinitions(::armarx::PropertyDefinitionsPtr& def) override;
[[nodiscard]] bool synchronizeRobot(VirtualRobot::Robot& robot,
- const armem::Time& timestamp);
+ const armem::Time& timestamp) const;
[[nodiscard]] VirtualRobot::RobotPtr
getRobot(const std::string& name,
@@ -68,6 +68,9 @@ namespace armarx::armem::robot_state
VirtualRobot::RobotIO::RobotDescription::eStructure,
bool blocking = true);
+ std::optional<std::map<RobotReader::Hand, robot::ForceTorque>>
+ queryForceTorque(const std::string& name, const armem::Time& timestamp);
+ using RobotReader::queryForceTorque;
private:
[[nodiscard]] VirtualRobot::RobotPtr
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotWriter.cpp b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotWriter.cpp
index 47519748cad8fe1c5a0ca37db35f215e0359197b..fc3aed1eea251e199077d92ccb615249b1777570 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotWriter.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotWriter.cpp
@@ -70,7 +70,7 @@ namespace armarx::armem::robot_state
const robot::RobotState robotState{.timestamp = timestamp,
.globalPose =
robot::RobotState::Pose(robot.getGlobalPose()),
- .jointMap = robot.getJointValues()};
+ .jointMap = robot.getJointValues(), .proprioception=std::nullopt};
return RobotWriter::storeState(robotState,
robot.getType(),
diff --git a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/ndarray/PointCloud.cpp b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/ndarray/PointCloud.cpp
index 66b3c77d68c4cdf598b4712819c618249e49cdb8..2c5a6d1d30bd297e35a4b5592a8bf7e32ef8abcb 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/ndarray/PointCloud.cpp
+++ b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/ndarray/PointCloud.cpp
@@ -115,8 +115,8 @@ namespace armarx::aron::codegenerator::cpp::generator
variantAccessor = ARON_VARIANT_RETURN_ACCESSOR + "_" + escaped_accessor;
block_if_data->addLine(
- variantAccessor + " = " + ARON_WRITER_ACCESSOR + ".writeNDArray({" + cppAccessor +
- nextEl() + "width, " + cppAccessor + nextEl() + "height, " +
+ variantAccessor + " = " + ARON_WRITER_ACCESSOR + ".writeNDArray({ static_cast<int>(" + cppAccessor +
+ nextEl() + "width), static_cast<int>(" + cppAccessor + nextEl() + "height), " +
std::to_string(std::get<1>(VoxelType2Cpp.at(type.getVoxelType()))) + "}, " + "\"" +
std::get<0>(VoxelType2Cpp.at(type.getVoxelType())) + "\", " +
"reinterpret_cast<const unsigned char*>(" + cppAccessor + nextEl() +
diff --git a/source/RobotAPI/libraries/aron/common/aron_conversions/framed.cpp b/source/RobotAPI/libraries/aron/common/aron_conversions/framed.cpp
index be011068dffcabda87b34bc3da51d6e8b432a2e1..27ab8bff523e82ccd52f02bede86a33b51e79225 100644
--- a/source/RobotAPI/libraries/aron/common/aron_conversions/framed.cpp
+++ b/source/RobotAPI/libraries/aron/common/aron_conversions/framed.cpp
@@ -67,6 +67,7 @@ namespace armarx::aron::framed
{
dto.header.frame = bo.frame;
dto.header.agent = bo.agent;
+ dto.pose = Eigen::Matrix4f::Identity();
Eigen::Vector3f vec;
vec.x() = bo.position->x;
vec.y() = bo.position->y;
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/detail/PrimitiveVariant.h b/source/RobotAPI/libraries/aron/core/data/variant/detail/PrimitiveVariant.h
index 69cabb5ba160f3abef84d331b971c4146d866c29..b5444266d3a2bfbc09e2cc151755eb90402e511a 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/detail/PrimitiveVariant.h
+++ b/source/RobotAPI/libraries/aron/core/data/variant/detail/PrimitiveVariant.h
@@ -45,7 +45,7 @@ namespace armarx::aron::data::detail
using ValueType = ValueT;
public:
- using Base::SpecializedVariantBase;
+ using typename Base::SpecializedVariantBase;
PrimitiveVariant(const ValueT& v,
const data::Descriptor descriptor,
diff --git a/source/RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.cpp b/source/RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.cpp
index c76da14105facaf03ff55742b22ac968fe1fd196..9009db639c7e6164474312630fb1de104f1a713d 100644
--- a/source/RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.cpp
+++ b/source/RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.cpp
@@ -98,7 +98,7 @@ namespace armarx::plugins
return;
}
- ARMARX_INFO << "Adding skill and set owning provider name" << skillName;
+ ARMARX_INFO << "Adding skill and set owning provider name `" << skillName << "`.";
auto s = skillImplementations.emplace(skillName, std::move(skill));
s.first->second.statusUpdate.skillId = skills::SkillID(parent().getName(), skillName);
}
diff --git a/source/RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.h b/source/RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.h
index 6b74dd1e2479068ca920dde2b8f2ec102b0d7646..ab6c8c4662ce303f532e94c6ebddc2d39dba11e3 100644
--- a/source/RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.h
+++ b/source/RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.h
@@ -46,16 +46,14 @@ namespace armarx::plugins
void addSkill(const skills::LambdaSkill::FunT&, const skills::SkillDescription&);
void addSkill(std::unique_ptr<skills::Skill>&&);
- template <typename T>
+ template <typename T, typename... Args>
T*
- addSkill()
+ addSkill(Args&&... args)
{
static_assert(std::is_base_of<skills::Skill, T>::value,
"T must be derived from skills::Skill!");
- static_assert(std::is_default_constructible<T>::value,
- "T must be default constructible!");
- auto skill = std::make_unique<T>();
+ auto skill = std::make_unique<T>(std::forward<Args>(args)...);
auto* skillPtr = skill.get();
addSkill(std::move(skill));
return static_cast<T*>(skillPtr);
@@ -111,11 +109,11 @@ namespace armarx
void addSkill(const skills::LambdaSkill::FunT&, const skills::SkillDescription&);
void addSkill(std::unique_ptr<skills::Skill>&&);
- template <typename T>
+ template <typename T, typename... Args>
T*
- addSkill()
+ addSkill(Args&&... args)
{
- return plugin->addSkill<T>();
+ return plugin->addSkill<T>(std::forward<Args>(args)...);
}
private: