diff --git a/etc/doxygen/pages/HowTos.dox b/etc/doxygen/pages/HowTos.dox
index 73d1f6aa03ce74996e4fb74e9476b829dffb5f12..2fe48c81ed35c95d80515081fe3adf118d0fd89b 100644
--- a/etc/doxygen/pages/HowTos.dox
+++ b/etc/doxygen/pages/HowTos.dox
@@ -137,4 +137,17 @@ If a complete robot model (including 3d models) is needed, you can pass a filena
\endcode
This model can be synchronized in the same way as the first model.
+
+
+\section RobotAPI-HowTos-Memory-Plot-Values How to Plot Values from the Memory System
+
+You can use the component \ref Component-MemoryToDebugObserver to query data from memory servers
+and send them to the \ref Component-DebugObserver.
+These values can then be viewed using
+the \ref ArmarXGui-GuiPlugins-ObserverWidget
+or the \ref ArmarXGui-GuiPlugins-PlotterPlugin.
+
+To use this functionality in your own code, use the class
+armarx::armem::client::util::MemoryToDebugObserver.
+
*/
diff --git a/scenarios/memory_to_debug_observer/config/MemoryToDebugObserver.cfg b/scenarios/memory_to_debug_observer/config/MemoryToDebugObserver.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..a230049a091bc90361567f1e56dd87f98101412f
--- /dev/null
+++ b/scenarios/memory_to_debug_observer/config/MemoryToDebugObserver.cfg
@@ -0,0 +1,239 @@
+# ==================================================================
+# MemoryToDebugObserver properties
+# ==================================================================
+
+# ArmarX.AdditionalPackages: List of additional ArmarX packages which should be in the list of default packages. If you have custom packages, which should be found by the gui or other apps, specify them here. Comma separated List.
+# Attributes:
+# - Default: Default value not mapped.
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.AdditionalPackages = Default value not mapped.
+
+
+# ArmarX.ApplicationName: Application name
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ApplicationName = ""
+
+
+# ArmarX.AutodiscoverPackages: If enabled, will discover all ArmarX packages based on the environment variables. Otherwise, the `DefaultPackages` and `AdditionalPackages` properties are used.
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.AutodiscoverPackages = true
+
+
+# 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
+# - Required: no
+# ArmarX.CachePath = mongo/.cache
+
+
+# ArmarX.Config: Comma-separated list of configuration files
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.Config = ""
+
+
+# ArmarX.DataPath: Semicolon-separated search list for data files
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DataPath = ""
+
+
+# ArmarX.DefaultPackages: List of ArmarX packages which are accessible by default. Comma separated List. If you want to add your own packages and use all default ArmarX packages, use the property 'AdditionalPackages'.
+# Attributes:
+# - Default: Default value not mapped.
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DefaultPackages = Default value not mapped.
+
+
+# ArmarX.DependenciesConfig: Path to the (usually generated) config file containing all data paths of all dependent projects. This property usually does not need to be edited.
+# Attributes:
+# - Default: ./config/dependencies.cfg
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DependenciesConfig = ./config/dependencies.cfg
+
+
+# ArmarX.DisableLogging: Turn logging off in whole application
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.DisableLogging = false
+
+
+# ArmarX.EnableProfiling: Enable profiling of CPU load produced by this application
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.EnableProfiling = false
+
+
+# ArmarX.LoadLibraries: Libraries to load at start up of the application. Must be enabled by the Application with enableLibLoading(). Format: PackageName:LibraryName;... or /absolute/path/to/library;...
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.LoadLibraries = ""
+
+
+# ArmarX.LoggingGroup: The logging group is transmitted with every ArmarX log message over Ice in order to group the message in the GUI.
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.LoggingGroup = ""
+
+
+# ArmarX.MemoryToDebugObserver.DebugObserverTopicName: Name of the topic the DebugObserver listens on
+# Attributes:
+# - Default: DebugObserver
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.MemoryToDebugObserver.DebugObserverTopicName = DebugObserver
+
+
+# ArmarX.MemoryToDebugObserver.EnableProfiling: enable profiler which is used for logging performance events
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.MemoryToDebugObserver.EnableProfiling = false
+
+
+# ArmarX.MemoryToDebugObserver.MinimumLoggingLevel: Local logging level only for this component
+# Attributes:
+# - Default: Undefined
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
+# ArmarX.MemoryToDebugObserver.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.MemoryToDebugObserver.ObjectName: Name of IceGrid well-known object
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.MemoryToDebugObserver.ObjectName = ""
+
+
+# ArmarX.MemoryToDebugObserver.RemoteGuiName: Name of the remote gui provider
+# Attributes:
+# - Default: RemoteGuiProvider
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.MemoryToDebugObserver.RemoteGuiName = RemoteGuiProvider
+
+
+# ArmarX.MemoryToDebugObserver.mns.MemoryNameSystemEnabled: Whether to use (and depend on) the Memory Name System (MNS).
+# Set to false to use this memory as a stand-alone.
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.MemoryToDebugObserver.mns.MemoryNameSystemEnabled = true
+
+
+# ArmarX.MemoryToDebugObserver.mns.MemoryNameSystemName: Name of the Memory Name System (MNS) component.
+# Attributes:
+# - Default: MemoryNameSystem
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.MemoryToDebugObserver.mns.MemoryNameSystemName = MemoryNameSystem
+
+
+# ArmarX.MemoryToDebugObserver.p.memoryToDebugObserverJson: Configuration of MemoryToDebugObserver in JSON format.
+# Attributes:
+# - Default: {"plottedValues":[{"aronPath":["joints","position","Neck_1_Yaw"],"entityID":["RobotState","Proprioception","Armar7","Armar7"]},{"aronPath":["joints","velocity","Neck_1_Yaw"],"entityID":["RobotState","Proprioception","Armar7","Armar7"]},{"aronPath":["joints","position","Neck_2_Hemisphere_A"],"entityID":["RobotState","Proprioception","Armar7","Armar7"]},{"aronPath":["joints","velocity","Neck_2_Hemisphere_A"],"entityID":["RobotState","Proprioception","Armar7","Armar7"]},{"aronPath":["joints","position","Neck_3_Hemisphere_B"],"entityID":["RobotState","Proprioception","Armar7","Armar7"]},{"aronPath":["joints","velocity","Neck_3_Hemisphere_B"],"entityID":["RobotState","Proprioception","Armar7","Armar7"]}]}
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.MemoryToDebugObserver.p.memoryToDebugObserverJson = {"plottedValues":[{"aronPath":["joints","position","Neck_1_Yaw"],"entityID":["RobotState","Proprioception","Armar7","Armar7"]},{"aronPath":["joints","velocity","Neck_1_Yaw"],"entityID":["RobotState","Proprioception","Armar7","Armar7"]},{"aronPath":["joints","position","Neck_2_Hemisphere_A"],"entityID":["RobotState","Proprioception","Armar7","Armar7"]},{"aronPath":["joints","velocity","Neck_2_Hemisphere_A"],"entityID":["RobotState","Proprioception","Armar7","Armar7"]},{"aronPath":["joints","position","Neck_3_Hemisphere_B"],"entityID":["RobotState","Proprioception","Armar7","Armar7"]},{"aronPath":["joints","velocity","Neck_3_Hemisphere_B"],"entityID":["RobotState","Proprioception","Armar7","Armar7"]}]}
+
+
+# ArmarX.RedirectStdout: Redirect std::cout and std::cerr to ArmarXLog
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.RedirectStdout = true
+
+
+# ArmarX.RemoteHandlesDeletionTimeout: The timeout (in ms) before a remote handle deletes the managed object after the use count reached 0. This time can be used by a client to increment the count again (may be required when transmitting remote handles)
+# Attributes:
+# - Default: 3000
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RemoteHandlesDeletionTimeout = 3000
+
+
+# ArmarX.SecondsStartupDelay: The startup will be delayed by this number of seconds (useful for debugging)
+# Attributes:
+# - Default: 0
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.SecondsStartupDelay = 0
+
+
+# ArmarX.StartDebuggerOnCrash: If this application crashes (segmentation fault) qtcreator will attach to this process and start the debugger.
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.StartDebuggerOnCrash = false
+
+
+# ArmarX.ThreadPoolSize: Size of the ArmarX ThreadPool that is always running.
+# Attributes:
+# - Default: 1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ThreadPoolSize = 1
+
+
+# ArmarX.TopicSuffix: Suffix appended to all topic names for outgoing topics. This is mainly used to direct all topics to another name for TopicReplaying purposes.
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.TopicSuffix = ""
+
+
+# ArmarX.UseTimeServer: Enable using a global Timeserver (e.g. from ArmarXSimulator)
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.UseTimeServer = false
+
+
+# ArmarX.Verbosity: Global logging level for whole application
+# Attributes:
+# - Default: Info
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
+# ArmarX.Verbosity = Info
+
+
diff --git a/scenarios/memory_to_debug_observer/config/global.cfg b/scenarios/memory_to_debug_observer/config/global.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..973c65ddcf2132916ac3d1a2305dff27851fe5aa
--- /dev/null
+++ b/scenarios/memory_to_debug_observer/config/global.cfg
@@ -0,0 +1,4 @@
+# ==================================================================
+# Global Config from Scenario memory_to_debug_observer
+# ==================================================================
+
diff --git a/scenarios/memory_to_debug_observer/memory_to_debug_observer.scx b/scenarios/memory_to_debug_observer/memory_to_debug_observer.scx
new file mode 100644
index 0000000000000000000000000000000000000000..c3c1418d716246324848bc989aa5dbf06a9ce56c
--- /dev/null
+++ b/scenarios/memory_to_debug_observer/memory_to_debug_observer.scx
@@ -0,0 +1,5 @@
+<?xml version="1.0" encoding="utf-8"?>
+<scenario name="memory_to_debug_observer" creation="1970-01-01.01:00:00" globalConfigName="./config/global.cfg" package="RobotAPI" deploymentType="local" nodeName="NodeMain">
+ <application name="MemoryToDebugObserver" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
+</scenario>
+
diff --git a/source/RobotAPI/components/RobotNameService/RobotNameService.cpp b/source/RobotAPI/components/RobotNameService/RobotNameService.cpp
index 9ab8e07d7ca5234e40e167ae73201cae04105db5..397a638912d93ef767bcd23795e7e2e709b0f18a 100644
--- a/source/RobotAPI/components/RobotNameService/RobotNameService.cpp
+++ b/source/RobotAPI/components/RobotNameService/RobotNameService.cpp
@@ -54,8 +54,8 @@ namespace armarx
}
bool
- RobotNameService::registerRobot(const robot_name_service::dto::Robot& robot,
- const Ice::Current& current)
+ RobotNameService::registerRobotInfo(const robot_name_service::dto::RobotInfo& robot,
+ const Ice::Current& current)
{
std::scoped_lock l(robotsMutex);
ARMARX_INFO << "Register a new robot with name '" << robot.name << "' in RNS";
@@ -72,7 +72,7 @@ namespace armarx
}
void
- RobotNameService::unregisterRobot(const std::string& name, const Ice::Current& current)
+ RobotNameService::unregisterRobotInfo(const std::string& name, const Ice::Current& current)
{
std::scoped_lock l(robotsMutex);
@@ -82,8 +82,8 @@ namespace armarx
}
}
- IceUtil::Optional<robot_name_service::dto::Robot>
- RobotNameService::getRobot(const std::string& name, const Ice::Current& current)
+ IceUtil::Optional<robot_name_service::dto::RobotInfo>
+ RobotNameService::getRobotInfo(const std::string& name, const Ice::Current& current)
{
std::scoped_lock l(robotsMutex);
diff --git a/source/RobotAPI/components/RobotNameService/RobotNameService.h b/source/RobotAPI/components/RobotNameService/RobotNameService.h
index 7af787aef845a26c8ed47f7b9351c36cd6dce002..6cc056c194cdc73146551ae1da50e097b438c894 100644
--- a/source/RobotAPI/components/RobotNameService/RobotNameService.h
+++ b/source/RobotAPI/components/RobotNameService/RobotNameService.h
@@ -87,14 +87,14 @@ namespace armarx
// RobotNameServiceInterface interface
public:
- bool registerRobot(const robot_name_service::dto::Robot& robot,
- const Ice::Current& current) override;
- void unregisterRobot(const std::string& name, const Ice::Current& current) override;
- IceUtil::Optional<robot_name_service::dto::Robot>
- getRobot(const std::string& name, const Ice::Current& current) override;
+ bool registerRobotInfo(const robot_name_service::dto::RobotInfo& robot,
+ const Ice::Current& current) override;
+ void unregisterRobotInfo(const std::string& name, const Ice::Current& current) override;
+ IceUtil::Optional<robot_name_service::dto::RobotInfo>
+ getRobotInfo(const std::string& name, const Ice::Current& current) override;
private:
mutable std::mutex robotsMutex;
- std::map<std::string, robot_name_service::core::Robot> robots;
+ std::map<std::string, robot_name_service::core::RobotInfo> robots;
};
} // namespace armarx
diff --git a/source/RobotAPI/components/armem/client/CMakeLists.txt b/source/RobotAPI/components/armem/client/CMakeLists.txt
index 9e9e2da67d9f2b3c1ed686fcf71f15c0c7e6ad8c..284ce7a27afd70e99981da558c6c1e0c6be234c3 100644
--- a/source/RobotAPI/components/armem/client/CMakeLists.txt
+++ b/source/RobotAPI/components/armem/client/CMakeLists.txt
@@ -1,5 +1,6 @@
add_subdirectory(ExampleMemoryClient)
add_subdirectory(GraspProviderExample)
+add_subdirectory(MemoryToDebugObserver)
add_subdirectory(ObjectInstanceToIndex)
add_subdirectory(RobotStatePredictionClientExample)
add_subdirectory(SimpleVirtualRobot)
diff --git a/source/RobotAPI/components/armem/client/MemoryToDebugObserver/CMakeLists.txt b/source/RobotAPI/components/armem/client/MemoryToDebugObserver/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..797cb6b4789199a86085ccec3e47c367e9fae462
--- /dev/null
+++ b/source/RobotAPI/components/armem/client/MemoryToDebugObserver/CMakeLists.txt
@@ -0,0 +1,28 @@
+armarx_component_set_name(MemoryToDebugObserver)
+
+set(COMPONENT_LIBS
+ ArmarXCore
+ ArmarXCoreComponentPlugins
+ ArmarXGuiComponentPlugins
+ RobotAPICore RobotAPIInterfaces
+ armem
+)
+
+set(SOURCES
+ Component.cpp
+)
+
+set(HEADERS
+ Component.h
+)
+
+armarx_add_component("${SOURCES}" "${HEADERS}")
+
+armarx_generate_and_add_component_executable(
+ COMPONENT_INCLUDE
+ "${CMAKE_CURRENT_LIST_DIR}/Component.h"
+ COMPONENT_NAMESPACE
+ armarx::memory_to_debug_observer
+ COMPONENT_CLASS_NAME
+ Component
+)
diff --git a/source/RobotAPI/components/armem/client/MemoryToDebugObserver/Component.cpp b/source/RobotAPI/components/armem/client/MemoryToDebugObserver/Component.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..89782e6df70a51e35725ed198165f92d4d20029b
--- /dev/null
+++ b/source/RobotAPI/components/armem/client/MemoryToDebugObserver/Component.cpp
@@ -0,0 +1,158 @@
+/*
+ * 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::MemoryToDebugObserver
+ * @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
+ * @date 2023
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "Component.h"
+
+#include <SimoxUtility/json.h>
+
+#include <ArmarXCore/core/time/Frequency.h>
+#include <ArmarXCore/core/time/Metronome.h>
+
+namespace armarx::memory_to_debug_observer
+{
+
+ armarx::PropertyDefinitionsPtr
+ Component::createPropertyDefinitions()
+ {
+ armarx::PropertyDefinitionsPtr defs =
+ new ComponentPropertyDefinitions(getConfigIdentifier());
+
+ {
+ armem::MemoryID robotEntityId{"RobotState", "Proprioception", "Armar7", "Armar7"};
+
+ std::vector<std::string> jointNames{
+ "Neck_1_Yaw",
+ "Neck_2_Hemisphere_A",
+ "Neck_3_Hemisphere_B",
+ };
+
+ for (const std::string& jointName : jointNames)
+ {
+ std::vector<std::string> attributes{
+ "position",
+ "velocity",
+ };
+ for (const std::string& attribute : attributes)
+ {
+ properties.memoryToDebugObserver.plottedValues.push_back({
+ .entityID = robotEntityId,
+ .aronPath = {{"joints", attribute, jointName}},
+ });
+ }
+ }
+
+ simox::json::json j = properties.memoryToDebugObserver;
+ properties.memoryToDebugObserverJson = j.dump();
+ }
+ defs->optional(properties.memoryToDebugObserverJson,
+ "p.memoryToDebugObserverJson",
+ "Configuration of MemoryToDebugObserver in JSON format.");
+
+ defs->optional(properties.pollFrequencyHz, "p.pollFrequencyHz");
+
+ return defs;
+ }
+
+ Component::Component()
+ {
+ }
+
+ std::string
+ Component::getDefaultName() const
+ {
+ return "MemoryToDebugObserver";
+ }
+
+ void
+ Component::onInitComponent()
+ {
+ DebugObserverComponentPluginUser::setDebugObserverBatchModeEnabled(true);
+
+ {
+ simox::json::json j = simox::json::json::parse(properties.memoryToDebugObserverJson);
+ j.get_to(properties.memoryToDebugObserver);
+ }
+ }
+
+ void
+ Component::onConnectComponent()
+ {
+ createRemoteGuiTab();
+ RemoteGui_startRunningTask();
+
+ task = new RunningTask<Component>(this, &Component::runLoop);
+ task->start();
+ }
+
+ void
+ Component::onDisconnectComponent()
+ {
+ task->stop();
+ task = nullptr;
+ }
+
+ void
+ Component::onExitComponent()
+ {
+ }
+
+ void
+ Component::runLoop()
+ {
+ armem::client::util::MemoryToDebugObserver::Services services{
+ .memoryNameSystem = memoryNameSystem(),
+ .debugObserver = getDebugObserverComponentPlugin(),
+ };
+ armem::client::util::MemoryToDebugObserver memoryToDebugObserver{
+ properties.memoryToDebugObserver, services};
+
+ Frequency frequency = Frequency::Hertz(properties.pollFrequencyHz);
+ Metronome metronome(frequency);
+ while (task and not task->isStopped())
+ {
+ memoryToDebugObserver.pollOnce();
+ metronome.waitForNextTick();
+ }
+ }
+
+ void
+ Component::createRemoteGuiTab()
+ {
+ using namespace armarx::RemoteGui::Client;
+
+ tab.group = GroupBox();
+ tab.group.setLabel("Todo ...");
+
+ VBoxLayout root = {tab.group, VSpacer()};
+ RemoteGui_createTab(getName(), root, &tab);
+ }
+
+ void
+ Component::RemoteGui_update()
+ {
+ if (tab.rebuild.exchange(false))
+ {
+ createRemoteGuiTab();
+ }
+ }
+
+} // namespace armarx::memory_to_debug_observer
diff --git a/source/RobotAPI/components/armem/client/MemoryToDebugObserver/Component.h b/source/RobotAPI/components/armem/client/MemoryToDebugObserver/Component.h
new file mode 100644
index 0000000000000000000000000000000000000000..7ce514c96ec2a4554e04f81244a0c6e48a58f603
--- /dev/null
+++ b/source/RobotAPI/components/armem/client/MemoryToDebugObserver/Component.h
@@ -0,0 +1,105 @@
+/*
+ * 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::MemoryToDebugObserver
+ * @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
+ * @date 2023
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/libraries/ArmarXCoreComponentPlugins/DebugObserverComponentPlugin.h>
+#include <ArmarXCore/util/tasks.h>
+
+#include <ArmarXGui/libraries/ArmarXGuiComponentPlugins/LightweightRemoteGuiComponentPlugin.h>
+
+#include <RobotAPI/libraries/armem/client/plugins/PluginUser.h>
+#include <RobotAPI/libraries/armem/client/util/MemoryToDebugObserver.h>
+
+namespace armarx::memory_to_debug_observer
+{
+
+ /**
+ * @defgroup Component-MemoryToDebugObserver MemoryToDebugObserver
+ * @ingroup RobotAPI-Components
+ *
+ * Transfers data from the memory system to the \ref Component-DebugObserver "Debug Observer",
+ * allowing to visualize them in the \ref ArmarXGui-GuiPlugins-PlotterPlugin "Live Plotter".
+ *
+ * @see armarx::armem::client::util::MemoryToDebugObserver for the business logic
+ * implementation.
+ *
+ * @class Component
+ * @ingroup Component-MemoryToDebugObserver
+ * @brief Implementation of \ref Component-MemoryToDebugObserver.
+ *
+ * @see armarx::armem::client::util::MemoryToDebugObserver for the business logic
+ * implementation.
+ */
+ class Component :
+ virtual public armarx::Component,
+ virtual public armarx::armem::ClientPluginUser,
+ virtual public armarx::DebugObserverComponentPluginUser,
+ virtual public armarx::LightweightRemoteGuiComponentPluginUser
+ {
+ public:
+ Component();
+
+ /// @see armarx::ManagedIceObject::getDefaultName()
+ std::string getDefaultName() const override;
+
+ // LightweightRemoteGuiComponentPluginUser interface
+ public:
+ void createRemoteGuiTab();
+ void RemoteGui_update() override;
+
+
+ protected:
+ armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
+
+ void onInitComponent() override;
+ void onConnectComponent() override;
+ void onDisconnectComponent() override;
+ void onExitComponent() override;
+
+ void runLoop();
+
+
+ private:
+ struct Properties
+ {
+ armem::client::util::MemoryToDebugObserver::Properties memoryToDebugObserver;
+ std::string memoryToDebugObserverJson;
+
+ float pollFrequencyHz = 30;
+ };
+
+ Properties properties;
+
+ armarx::RunningTask<Component>::pointer_type task;
+
+ struct RemoteGuiTab : RemoteGui::Client::Tab
+ {
+ std::atomic_bool rebuild = false;
+
+ RemoteGui::Client::GroupBox group;
+ };
+
+ RemoteGuiTab tab;
+ };
+} // namespace armarx::memory_to_debug_observer
diff --git a/source/RobotAPI/components/skills/SkillProviderExample/HelloWorld.cpp b/source/RobotAPI/components/skills/SkillProviderExample/HelloWorld.cpp
index edf918402a4c8ed1bbe510ab553d6dc568540dd5..d3e66e838eeae19ae089f4af76d4d9d4f9cfc33b 100644
--- a/source/RobotAPI/components/skills/SkillProviderExample/HelloWorld.cpp
+++ b/source/RobotAPI/components/skills/SkillProviderExample/HelloWorld.cpp
@@ -29,7 +29,8 @@ namespace armarx::skills::provider
.rootProfileDefaults = root_profile_params.toAron(),
.timeout = armarx::core::time::Duration::MilliSeconds(1000),
.parametersType =
- armarx::skills::Example::HelloWorldAcceptedType::ToAronType()};
+ armarx::skills::Example::HelloWorldAcceptedType::ToAronType(),
+ .resultType = armarx::skills::Example::HelloWorldAcceptedType::ToAronType()};
}
Skill::MainResult
@@ -42,6 +43,6 @@ namespace armarx::skills::provider
.dump(2)
<< "\n"
<< "(executed at: " << IceUtil::Time::now() << ")";
- return {TerminatedSkillStatus::Succeeded, nullptr};
+ return {TerminatedSkillStatus::Succeeded, in.parameters.toAron()};
}
} // namespace armarx::skills::provider
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.cpp
index e0131a416855174e5571d22d9e44cfac50f76954..88cf11c532113f593f4dfa9056e4469fbc9a7992 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.cpp
@@ -853,7 +853,7 @@ namespace armarx::RobotUnitModule
}
SensorAndControl& sc = _module<ControlThreadDataBuffer>().rtGetSensorAndControlBuffer();
- sc.writeTimestamp = IceUtil::Time::now(); // this has to be in real time
+ sc.writeTimestamp = armarx::rtNow(); // this has to be in real time
sc.sensorValuesTimestamp = sensorValuesTimestamp;
sc.timeSinceLastIteration = timeSinceLastIteration;
ARMARX_CHECK_EQUAL(rtGetSensorDevices().size(), sc.sensors.size());
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControllerManagement.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControllerManagement.cpp
index 6f0001391b69aa246edc781fee744997f214f988..e53bb97efcc50b5571512f6149706c44c7501cea 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControllerManagement.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControllerManagement.cpp
@@ -95,8 +95,11 @@ namespace armarx::RobotUnitModule
std::stringstream ss;
ss << "Requested controller class '" << className
<< "' unknown! Known classes:" << NJointControllerRegistry::getKeys()
- << " (If this class exists in a different lib then load it via "
- "loadLibFromPath(path) or loadLibFromPackage(package, lib))";
+ << " (If this class exists in a different lib then load it in the property "
+ "definitions of the RT-unit. DO NOT load it via "
+ "loadLibFromPath(path) or loadLibFromPackage(package, lib)) (see "
+ "https://git.h2t.iar.kit.edu/sw/armarx-integration/robots/armar7/documentation/-/"
+ "issues/85)";
ARMARX_ERROR << ss.str();
throw InvalidArgumentException{ss.str()};
}
@@ -303,6 +306,9 @@ namespace armarx::RobotUnitModule
ControllerManagement::loadLibFromPath(const std::string& path, const Ice::Current&)
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ ARMARX_WARNING << "Do not use this function as it has implications on the RT thread (see "
+ "https://git.h2t.iar.kit.edu/sw/armarx-integration/robots/armar7/"
+ "documentation/-/issues/85)";
const bool result = getArmarXManager()->loadLibFromPath(path);
ARMARX_INFO << "loadLibFromPath('" << path << "') -> " << result;
return result;
@@ -314,6 +320,9 @@ namespace armarx::RobotUnitModule
const Ice::Current&)
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ ARMARX_WARNING << "Do not use this function as it has implications on the RT thread (see "
+ "https://git.h2t.iar.kit.edu/sw/armarx-integration/robots/armar7/"
+ "documentation/-/issues/85)";
const bool result = getArmarXManager()->loadLibFromPackage(package, lib);
ARMARX_INFO << "loadLibFromPackage('" << package << "', '" << lib << "') -> " << result;
return result;
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.cpp
index ee8522d21984cbf6936bd1b2c259e37850398d7f..b12127e4d8541fbf332b249988658288ea05ea29 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.cpp
@@ -50,14 +50,14 @@ namespace armarx::RobotUnitModule::details
void
start()
{
- t -= IceUtil::Time::now();
+ t -= armarx::rtNow();
++n;
}
void
stop()
{
- t += IceUtil::Time::now();
+ t += armarx::rtNow();
}
double
@@ -427,8 +427,8 @@ namespace armarx::RobotUnitModule
continue; //do not add to result and skipp during processing
}
auto& descrEntr = descr.entries[valData.fields.at(i).name];
-//*INDENT-OFF*
-// clang-format off
+ //*INDENT-OFF*
+ // clang-format off
#define make_case(Type, TName) \
(typeid(Type) == *valData.fields.at(i).type) \
{ \
@@ -494,7 +494,7 @@ namespace armarx::RobotUnitModule
ARMARX_TRACE;
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
std::lock_guard<std::mutex> guard{rtLoggingMutex};
-
+
if (rtDataStreamingEntry.count(receiver) == 0u)
{
ARMARX_INFO << "stopDataStreaming called for a nonexistent log";
@@ -562,11 +562,11 @@ namespace armarx::RobotUnitModule
ARMARX_TRACE;
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
std::lock_guard<std::mutex> guard{rtLoggingMutex};
- const auto now = IceUtil::Time::now();
+ const auto now = armarx::rtNow();
// entries are removed last
//remove backlog entries
- const auto start_time_remove_backlog_entries = IceUtil::Time::now();
+ const auto start_time_remove_backlog_entries = armarx::rtNow();
{
if (rtLoggingBacklogEnabled)
{
@@ -579,10 +579,9 @@ namespace armarx::RobotUnitModule
}
}
//log all
- const auto start_time_log_all = IceUtil::Time::now();
+ const auto start_time_log_all = armarx::rtNow();
details::DoLoggingDurations dlogdurs;
{
- ARMARX_TRACE;
if (!rtLoggingEntries.empty() || !rtDataStreamingEntry.empty())
{
ARMARX_DEBUG << deactivateSpam() << "Number of logs " << rtLoggingEntries.size()
@@ -617,7 +616,7 @@ namespace armarx::RobotUnitModule
ARMARX_DEBUG << ::deactivateSpam() << "the last " << backlog.size()
<< " iterations are stored";
//flush all files
- const auto start_time_flush_all_files = IceUtil::Time::now();
+ const auto start_time_flush_all_files = armarx::rtNow();
{
for (auto& pair : rtLoggingEntries)
{
@@ -626,7 +625,7 @@ namespace armarx::RobotUnitModule
}
//remove entries
- const auto start_time_remove_entries = IceUtil::Time::now();
+ const auto start_time_remove_entries = armarx::rtNow();
{
ARMARX_TRACE;
std::vector<std::string> toRemove;
@@ -646,7 +645,7 @@ namespace armarx::RobotUnitModule
}
}
//deal with data streaming
- const auto start_time_data_streaming = IceUtil::Time::now();
+ const auto start_time_data_streaming = armarx::rtNow();
{
ARMARX_TRACE;
std::vector<RobotUnitDataStreaming::ReceiverPrx> toRemove;
@@ -671,7 +670,7 @@ namespace armarx::RobotUnitModule
}
}
// clang-format off
- const auto end_time = IceUtil::Time::now();
+ const auto end_time = armarx::rtNow();
const auto time_total = (end_time - now).toMilliSecondsDouble();
ARMARX_DEBUG_S << deactivateSpam(1)
<< "rtlogging time required: " << time_total << "ms\n"
@@ -756,441 +755,448 @@ namespace armarx::RobotUnitModule
}
// Process devices.
- {// Sensors.
- {ARMARX_TRACE;
- durations.sens.start();
- //sensors
- for (std::size_t idxDev = 0; idxDev < data.sensors.size(); ++idxDev)
- {
- const SensorValueBase* val = data.sensors.at(idxDev);
- //dimensions of sensor value (e.g. vel, tor, f_x, f_y, ...)
- for (std::size_t idxField = 0; idxField < val->getNumberOfDataFields(); ++idxField)
+ { // Sensors.
{
- if (!rtLoggingEntries.empty())
+ ARMARX_TRACE;
+ durations.sens.start();
+ //sensors
+ for (std::size_t idxDev = 0; idxDev < data.sensors.size(); ++idxDev)
{
- durations.sens_csv.start();
- const auto str = val->getDataFieldAs<std::string>(idxField);
- for (auto& [_, entry] : rtLoggingEntries)
+ const SensorValueBase* val = data.sensors.at(idxDev);
+ //dimensions of sensor value (e.g. vel, tor, f_x, f_y, ...)
+ for (std::size_t idxField = 0; idxField < val->getNumberOfDataFields();
+ ++idxField)
{
- if (entry->loggedSensorDeviceValues.at(idxDev).at(idxField))
+ if (!rtLoggingEntries.empty())
{
- entry->stream << ";" << str;
+ durations.sens_csv.start();
+ const auto str = val->getDataFieldAs<std::string>(idxField);
+ for (auto& [_, entry] : rtLoggingEntries)
+ {
+ if (entry->loggedSensorDeviceValues.at(idxDev).at(idxField))
+ {
+ entry->stream << ";" << str;
+ }
+ }
+ durations.sens_csv.stop();
+ }
+ if (!rtDataStreamingEntry.empty())
+ {
+ durations.sens_stream.start();
+ for (auto& [_, rtStreamingEntry] : rtDataStreamingEntry)
+ {
+ durations.sens_stream_elem.start();
+ rtStreamingEntry.processSens(*val, idxDev, idxField);
+ durations.sens_stream_elem.stop();
+ }
+ durations.sens_stream.stop();
}
}
- durations.sens_csv.stop();
}
- if (!rtDataStreamingEntry.empty())
+ durations.sens.stop();
+ }
+
+ // Controller.
+ {
+ durations.ctrl.start();
+ ARMARX_TRACE;
+ //joint controllers
+ for (std::size_t idxDev = 0; idxDev < data.control.size(); ++idxDev)
{
- durations.sens_stream.start();
- for (auto& [_, rtStreamingEntry] : rtDataStreamingEntry)
+ const auto& vals = data.control.at(idxDev);
+ //control value (e.g. v_platform)
+ for (std::size_t idxVal = 0; idxVal < vals.size(); ++idxVal)
{
- durations.sens_stream_elem.start();
- rtStreamingEntry.processSens(*val, idxDev, idxField);
- durations.sens_stream_elem.stop();
+ const ControlTargetBase* val = vals.at(idxVal);
+ //dimensions of control value (e.g. v_platform_x, v_platform_y, v_platform_rotate)
+ for (std::size_t idxField = 0; idxField < val->getNumberOfDataFields();
+ ++idxField)
+ {
+ if (!rtLoggingEntries.empty())
+ {
+ durations.ctrl_csv.start();
+ std::string str;
+ val->getDataFieldAs(idxField, str); // expensive function
+ for (auto& [_, entry] : rtLoggingEntries)
+ {
+ if (entry->loggedControlDeviceValues.at(idxDev).at(idxVal).at(
+ idxField))
+ {
+ entry->stream << ";" << str;
+ }
+ }
+ durations.ctrl_csv.stop();
+ }
+ if (!rtDataStreamingEntry.empty())
+ {
+ durations.ctrl_stream.start();
+ for (auto& [_, rtStreamingEntry] : rtDataStreamingEntry)
+ {
+ durations.ctrl_stream_elem.start();
+ rtStreamingEntry.processCtrl(*val, idxDev, idxVal, idxField);
+ durations.ctrl_stream_elem.stop();
+ }
+ durations.ctrl_stream.stop();
+ }
+ }
}
- durations.sens_stream.stop();
}
+
+ durations.ctrl.stop();
}
- }
- durations.sens.stop();
- }
+ } // namespace armarx::RobotUnitModule
- // Controller.
- {
- durations.ctrl.start();
- ARMARX_TRACE;
- //joint controllers
- for (std::size_t idxDev = 0; idxDev < data.control.size(); ++idxDev)
+ //finish processing
{
- const auto& vals = data.control.at(idxDev);
- //control value (e.g. v_platform)
- for (std::size_t idxVal = 0; idxVal < vals.size(); ++idxVal)
+ //store data to backlog
{
- const ControlTargetBase* val = vals.at(idxVal);
- //dimensions of control value (e.g. v_platform_x, v_platform_y, v_platform_rotate)
- for (std::size_t idxField = 0; idxField < val->getNumberOfDataFields(); ++idxField)
+ if (rtLoggingBacklogEnabled)
{
- if (!rtLoggingEntries.empty())
+ durations.backlog.start();
+ ARMARX_TRACE;
+ if (data.writeTimestamp + rtLoggingBacklogRetentionTime >= now)
{
- durations.ctrl_csv.start();
- std::string str;
- val->getDataFieldAs(idxField, str); // expensive function
- for (auto& [_, entry] : rtLoggingEntries)
- {
- if (entry->loggedControlDeviceValues.at(idxDev).at(idxVal).at(idxField))
- {
- entry->stream << ";" << str;
- }
- }
- durations.ctrl_csv.stop();
+ backlog.emplace_back(data, true); //true for minimal copy
}
- if (!rtDataStreamingEntry.empty())
+ durations.backlog.stop();
+ }
+ }
+ //print + reset messages
+ {
+ durations.msg.start();
+ ARMARX_TRACE;
+ for (const ::armarx::detail::RtMessageLogEntryBase* ptr :
+ data.messages.getEntries())
+ {
+ if (!ptr)
{
- durations.ctrl_stream.start();
- for (auto& [_, rtStreamingEntry] : rtDataStreamingEntry)
- {
- durations.ctrl_stream_elem.start();
- rtStreamingEntry.processCtrl(*val, idxDev, idxVal, idxField);
- durations.ctrl_stream_elem.stop();
- }
- durations.ctrl_stream.stop();
+ break;
}
+ ptr->print(controlThreadId);
}
+ durations.msg.stop();
}
}
-
- durations.ctrl.stop();
}
-} // namespace armarx::RobotUnitModule
-//finish processing
-{
- //store data to backlog
+ bool
+ Logging::MatchName(const std::string& pattern, const std::string& name)
{
- if (rtLoggingBacklogEnabled)
+ ARMARX_TRACE;
+ if (pattern.empty())
{
- durations.backlog.start();
- ARMARX_TRACE;
- if (data.writeTimestamp + rtLoggingBacklogRetentionTime >= now)
- {
- backlog.emplace_back(data, true); //true for minimal copy
- }
- durations.backlog.stop();
+ return false;
}
- }
- //print + reset messages
- {
- durations.msg.start();
- ARMARX_TRACE;
- for (const ::armarx::detail::RtMessageLogEntryBase* ptr : data.messages.getEntries())
+ static const std::regex pattern_regex{R"(^\^?[- ._*a-zA-Z0-9]+\$?$)"};
+ if (!std::regex_match(pattern, pattern_regex))
{
- if (!ptr)
- {
- break;
- }
- ptr->print(controlThreadId);
+ throw InvalidArgumentException{"Pattern '" + pattern + "' is invalid"};
}
- durations.msg.stop();
+ static const std::regex reg_dot{"[.]"};
+ static const std::regex reg_star{"[*]"};
+ const std::string rpl1 = std::regex_replace(pattern, reg_dot, "\\.");
+ const std::string rpl2 = std::regex_replace(rpl1, reg_star, ".*");
+ const std::regex key_regex{rpl2};
+ return std::regex_search(name, key_regex);
}
-}
-}
-bool
-Logging::MatchName(const std::string& pattern, const std::string& name)
-{
- ARMARX_TRACE;
- if (pattern.empty())
- {
- return false;
- }
- static const std::regex pattern_regex{R"(^\^?[- ._*a-zA-Z0-9]+\$?$)"};
- if (!std::regex_match(pattern, pattern_regex))
- {
- throw InvalidArgumentException{"Pattern '" + pattern + "' is invalid"};
- }
- static const std::regex reg_dot{"[.]"};
- static const std::regex reg_star{"[*]"};
- const std::string rpl1 = std::regex_replace(pattern, reg_dot, "\\.");
- const std::string rpl2 = std::regex_replace(rpl1, reg_star, ".*");
- const std::regex key_regex{rpl2};
- return std::regex_search(name, key_regex);
-}
-
-void
-Logging::_postOnInitRobotUnit()
-{
- ARMARX_TRACE;
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- rtLoggingTimestepMs = getProperty<std::size_t>("RTLogging_PeriodMs");
- ARMARX_CHECK_LESS(0, rtLoggingTimestepMs) << "The property RTLoggingPeriodMs must not be 0";
-
- messageBufferSize = getProperty<std::size_t>("RTLogging_MessageBufferSize");
- messageBufferMaxSize = getProperty<std::size_t>("RTLogging_MaxMessageBufferSize");
- ARMARX_CHECK_LESS_EQUAL(messageBufferSize, messageBufferMaxSize);
-
- messageBufferNumberEntries = getProperty<std::size_t>("RTLogging_MessageNumber");
- messageBufferMaxNumberEntries = getProperty<std::size_t>("RTLogging_MaxMessageNumber");
- ARMARX_CHECK_LESS_EQUAL(messageBufferNumberEntries, messageBufferMaxNumberEntries);
-
- rtLoggingBacklogRetentionTime =
- IceUtil::Time::milliSeconds(getProperty<std::size_t>("RTLogging_KeepIterationsForMs"));
- rtLoggingBacklogMaxSize = getProperty<std::size_t>("RTLogging_MaxBacklogSize");
- rtLoggingBacklogEnabled = getProperty<bool>("RTLogging_EnableBacklog");
- getProperty(numberOfEntriesToLog, "RTLogging_LogLastNMessagesOnly");
- ARMARX_CHECK_GREATER(getControlThreadTargetPeriod().toMicroSeconds(), 0);
- ARMARX_IMPORTANT << "Initializing RTLogging with the following parameters: " << VAROUT(rtLoggingTimestepMs)
- << VAROUT(messageBufferSize) << VAROUT(messageBufferMaxSize)
- << VAROUT(messageBufferNumberEntries) << VAROUT(messageBufferMaxNumberEntries)
- << VAROUT(rtLoggingBacklogRetentionTime) << VAROUT(rtLoggingBacklogMaxSize)
- << VAROUT(rtLoggingBacklogEnabled) << VAROUT(numberOfEntriesToLog);
-}
-
-void
-Logging::_postFinishDeviceInitialization()
-{
- ARMARX_TRACE;
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- //init buffer
+ void
+ Logging::_postOnInitRobotUnit()
{
ARMARX_TRACE;
- std::size_t ctrlThreadPeriodUs =
- static_cast<std::size_t>(getControlThreadTargetPeriod().toMicroSeconds());
- std::size_t logThreadPeriodUs = rtLoggingTimestepMs * 1000;
- std::size_t nBuffers = (logThreadPeriodUs / ctrlThreadPeriodUs + 1) * 100;
-
- const auto bufferSize =
- _module<ControlThreadDataBuffer>().getControlThreadOutputBuffer().initialize(
- nBuffers,
- _module<Devices>().getControlDevices(),
- _module<Devices>().getSensorDevices(),
- messageBufferSize,
- messageBufferNumberEntries,
- messageBufferMaxSize,
- messageBufferMaxNumberEntries);
- ARMARX_INFO << "RTLogging activated. Using " << nBuffers << " buffers "
- << "(buffersize = " << bufferSize << " bytes)";
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ rtLoggingTimestepMs = getProperty<std::size_t>("RTLogging_PeriodMs");
+ ARMARX_CHECK_LESS(0, rtLoggingTimestepMs) << "The property RTLoggingPeriodMs must not be 0";
+
+ messageBufferSize = getProperty<std::size_t>("RTLogging_MessageBufferSize");
+ messageBufferMaxSize = getProperty<std::size_t>("RTLogging_MaxMessageBufferSize");
+ ARMARX_CHECK_LESS_EQUAL(messageBufferSize, messageBufferMaxSize);
+
+ messageBufferNumberEntries = getProperty<std::size_t>("RTLogging_MessageNumber");
+ messageBufferMaxNumberEntries = getProperty<std::size_t>("RTLogging_MaxMessageNumber");
+ ARMARX_CHECK_LESS_EQUAL(messageBufferNumberEntries, messageBufferMaxNumberEntries);
+
+ rtLoggingBacklogRetentionTime =
+ IceUtil::Time::milliSeconds(getProperty<std::size_t>("RTLogging_KeepIterationsForMs"));
+ rtLoggingBacklogMaxSize = getProperty<std::size_t>("RTLogging_MaxBacklogSize");
+ rtLoggingBacklogEnabled = getProperty<bool>("RTLogging_EnableBacklog");
+ getProperty(numberOfEntriesToLog, "RTLogging_LogLastNMessagesOnly");
+ ARMARX_CHECK_GREATER(getControlThreadTargetPeriod().toMicroSeconds(), 0);
+ ARMARX_IMPORTANT << "Initializing RTLogging with the following parameters: "
+ << VAROUT(rtLoggingTimestepMs) << VAROUT(messageBufferSize)
+ << VAROUT(messageBufferMaxSize) << VAROUT(messageBufferNumberEntries)
+ << VAROUT(messageBufferMaxNumberEntries)
+ << VAROUT(rtLoggingBacklogRetentionTime) << VAROUT(rtLoggingBacklogMaxSize)
+ << VAROUT(rtLoggingBacklogEnabled) << VAROUT(numberOfEntriesToLog);
}
- //init logging names + field types
+
+ void
+ Logging::_postFinishDeviceInitialization()
{
ARMARX_TRACE;
- const auto makeValueMetaData = [&](auto* val, const std::string& namePre)
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ //init buffer
+ {
+ ARMARX_TRACE;
+ std::size_t ctrlThreadPeriodUs =
+ static_cast<std::size_t>(getControlThreadTargetPeriod().toMicroSeconds());
+ std::size_t logThreadPeriodUs = rtLoggingTimestepMs * 1000;
+ std::size_t nBuffers = (logThreadPeriodUs / ctrlThreadPeriodUs + 1) * 100;
+
+ const auto bufferSize =
+ _module<ControlThreadDataBuffer>().getControlThreadOutputBuffer().initialize(
+ nBuffers,
+ _module<Devices>().getControlDevices(),
+ _module<Devices>().getSensorDevices(),
+ messageBufferSize,
+ messageBufferNumberEntries,
+ messageBufferMaxSize,
+ messageBufferMaxNumberEntries);
+ ARMARX_INFO << "RTLogging activated. Using " << nBuffers << " buffers "
+ << "(buffersize = " << bufferSize << " bytes)";
+ }
+ //init logging names + field types
{
- ValueMetaData data;
- const auto names = val->getDataFieldNames();
- data.fields.resize(names.size());
+ ARMARX_TRACE;
+ const auto makeValueMetaData = [&](auto* val, const std::string& namePre)
+ {
+ ValueMetaData data;
+ const auto names = val->getDataFieldNames();
+ data.fields.resize(names.size());
+
+ for (std::size_t fieldIdx = 0; fieldIdx < names.size(); ++fieldIdx)
+ {
+ std::string const& fieldName = names[fieldIdx];
+ data.fields.at(fieldIdx).name = namePre + '.' + fieldName;
+ data.fields.at(fieldIdx).type = &(val->getDataFieldType(fieldIdx));
+ }
+ return data;
+ };
- for (std::size_t fieldIdx = 0; fieldIdx < names.size(); ++fieldIdx)
+ //sensorDevices
+ controlDeviceValueMetaData.reserve(_module<Devices>().getControlDevices().size());
+ for (const auto& cd : _module<Devices>().getControlDevices().values())
{
- std::string const& fieldName = names[fieldIdx];
- data.fields.at(fieldIdx).name = namePre + '.' + fieldName;
- data.fields.at(fieldIdx).type = &(val->getDataFieldType(fieldIdx));
+ ARMARX_TRACE;
+ controlDeviceValueMetaData.emplace_back();
+ auto& dataForDev = controlDeviceValueMetaData.back();
+ dataForDev.reserve(cd->getJointControllers().size());
+ for (auto jointC : cd->getJointControllers())
+ {
+ dataForDev.emplace_back(makeValueMetaData(jointC->getControlTarget(),
+ "ctrl." + cd->getDeviceName() + "." +
+ jointC->getControlMode()));
+ }
}
- return data;
- };
-
- //sensorDevices
- controlDeviceValueMetaData.reserve(_module<Devices>().getControlDevices().size());
- for (const auto& cd : _module<Devices>().getControlDevices().values())
- {
- ARMARX_TRACE;
- controlDeviceValueMetaData.emplace_back();
- auto& dataForDev = controlDeviceValueMetaData.back();
- dataForDev.reserve(cd->getJointControllers().size());
- for (auto jointC : cd->getJointControllers())
+ //sensorDevices
+ sensorDeviceValueMetaData.reserve(_module<Devices>().getSensorDevices().size());
+ for (const auto& sd : _module<Devices>().getSensorDevices().values())
{
- dataForDev.emplace_back(makeValueMetaData(jointC->getControlTarget(),
- "ctrl." + cd->getDeviceName() + "." +
- jointC->getControlMode()));
+ ARMARX_TRACE;
+ sensorDeviceValueMetaData.emplace_back(
+ makeValueMetaData(sd->getSensorValue(), "sens." + sd->getDeviceName()));
}
}
- //sensorDevices
- sensorDeviceValueMetaData.reserve(_module<Devices>().getSensorDevices().size());
- for (const auto& sd : _module<Devices>().getSensorDevices().values())
+ //start logging thread is done in rtinit
+ //maybe add the default log
{
ARMARX_TRACE;
- sensorDeviceValueMetaData.emplace_back(
- makeValueMetaData(sd->getSensorValue(), "sens." + sd->getDeviceName()));
+ const auto loggingpath = getProperty<std::string>("RTLogging_DefaultLog").getValue();
+ if (!loggingpath.empty())
+ {
+ defaultLogHandle = startRtLogging(loggingpath, getLoggingNames());
+ }
}
}
- //start logging thread is done in rtinit
- //maybe add the default log
+
+ void
+ Logging::DataStreamingEntry::clearResult()
{
ARMARX_TRACE;
- const auto loggingpath = getProperty<std::string>("RTLogging_DefaultLog").getValue();
- if (!loggingpath.empty())
+ for (auto& e : result)
{
- defaultLogHandle = startRtLogging(loggingpath, getLoggingNames());
+ entryBuffer.emplace_back(std::move(e));
}
+ result.clear();
}
-}
-void
-Logging::DataStreamingEntry::clearResult()
-{
- ARMARX_TRACE;
- for (auto& e : result)
+ RobotUnitDataStreaming::TimeStep
+ Logging::DataStreamingEntry::allocateResultElement() const
{
- entryBuffer.emplace_back(std::move(e));
+ ARMARX_TRACE;
+ RobotUnitDataStreaming::TimeStep data;
+ data.bools.resize(numBools);
+ data.bytes.resize(numBytes);
+ data.shorts.resize(numShorts);
+ data.ints.resize(numInts);
+ data.longs.resize(numLongs);
+ data.floats.resize(numFloats);
+ data.doubles.resize(numDoubles);
+ return data;
}
- result.clear();
-}
-RobotUnitDataStreaming::TimeStep
-Logging::DataStreamingEntry::allocateResultElement() const
-{
- ARMARX_TRACE;
- RobotUnitDataStreaming::TimeStep data;
- data.bools.resize(numBools);
- data.bytes.resize(numBytes);
- data.shorts.resize(numShorts);
- data.ints.resize(numInts);
- data.longs.resize(numLongs);
- data.floats.resize(numFloats);
- data.doubles.resize(numDoubles);
- return data;
-}
-
-RobotUnitDataStreaming::TimeStep
-Logging::DataStreamingEntry::getResultElement()
-{
- ARMARX_TRACE;
- if (entryBuffer.empty())
+ RobotUnitDataStreaming::TimeStep
+ Logging::DataStreamingEntry::getResultElement()
{
- return allocateResultElement();
+ ARMARX_TRACE;
+ if (entryBuffer.empty())
+ {
+ return allocateResultElement();
+ }
+ auto e = std::move(entryBuffer.back());
+ entryBuffer.pop_back();
+ return e;
}
- auto e = std::move(entryBuffer.back());
- entryBuffer.pop_back();
- return e;
-}
-void
-Logging::DataStreamingEntry::processHeader(const ControlThreadOutputBuffer::Entry& e)
-{
- ARMARX_TRACE;
- if (stopStreaming)
+ void
+ Logging::DataStreamingEntry::processHeader(const ControlThreadOutputBuffer::Entry& e)
{
- return;
- }
+ ARMARX_TRACE;
+ if (stopStreaming)
+ {
+ return;
+ }
- auto& data = result.emplace_back(getResultElement());
+ auto& data = result.emplace_back(getResultElement());
- data.iterationId = e.iteration;
- data.timestampUSec = e.sensorValuesTimestamp.toMicroSeconds();
- data.timesSinceLastIterationUSec = e.timeSinceLastIteration.toMicroSeconds();
-}
+ data.iterationId = e.iteration;
+ data.timestampUSec = e.sensorValuesTimestamp.toMicroSeconds();
+ data.timesSinceLastIterationUSec = e.timeSinceLastIteration.toMicroSeconds();
+ }
-void
-WriteTo(const auto& dentr,
- const Logging::DataStreamingEntry::OutVal& out,
- const auto& val,
- std::size_t fidx,
- auto& data)
-{
- ARMARX_TRACE;
- using enum_t = Logging::DataStreamingEntry::ValueT;
- try
+ void
+ WriteTo(const auto& dentr,
+ const Logging::DataStreamingEntry::OutVal& out,
+ const auto& val,
+ std::size_t fidx,
+ auto& data)
{
ARMARX_TRACE;
- switch (out.value)
+ using enum_t = Logging::DataStreamingEntry::ValueT;
+ try
{
- case enum_t::Bool:
- bool b;
- val.getDataFieldAs(fidx, b);
- data.bools.at(out.idx) = b;
- return;
- case enum_t::Byte:
- val.getDataFieldAs(fidx, data.bytes.at(out.idx));
- return;
- case enum_t::Short:
- val.getDataFieldAs(fidx, data.shorts.at(out.idx));
- return;
- case enum_t::Int:
- val.getDataFieldAs(fidx, data.ints.at(out.idx));
- return;
- case enum_t::Long:
- val.getDataFieldAs(fidx, data.longs.at(out.idx));
- return;
- case enum_t::Float:
- val.getDataFieldAs(fidx, data.floats.at(out.idx));
- return;
- case enum_t::Double:
- val.getDataFieldAs(fidx, data.doubles.at(out.idx));
- return;
- case enum_t::Skipped:
- return;
+ ARMARX_TRACE;
+ switch (out.value)
+ {
+ case enum_t::Bool:
+ bool b;
+ val.getDataFieldAs(fidx, b);
+ data.bools.at(out.idx) = b;
+ return;
+ case enum_t::Byte:
+ val.getDataFieldAs(fidx, data.bytes.at(out.idx));
+ return;
+ case enum_t::Short:
+ val.getDataFieldAs(fidx, data.shorts.at(out.idx));
+ return;
+ case enum_t::Int:
+ val.getDataFieldAs(fidx, data.ints.at(out.idx));
+ return;
+ case enum_t::Long:
+ val.getDataFieldAs(fidx, data.longs.at(out.idx));
+ return;
+ case enum_t::Float:
+ val.getDataFieldAs(fidx, data.floats.at(out.idx));
+ return;
+ case enum_t::Double:
+ val.getDataFieldAs(fidx, data.doubles.at(out.idx));
+ return;
+ case enum_t::Skipped:
+ return;
+ }
+ }
+ catch (...)
+ {
+ ARMARX_ERROR << GetHandledExceptionString() << "\ntype " << static_cast<int>(out.value)
+ << "\n"
+ << VAROUT(data.bools.size()) << " " << VAROUT(dentr.numBools) << "\n"
+ << VAROUT(data.bytes.size()) << " " << VAROUT(dentr.numBytes) << "\n"
+ << VAROUT(data.shorts.size()) << " " << VAROUT(dentr.numShorts) << "\n"
+ << VAROUT(data.ints.size()) << " " << VAROUT(dentr.numInts) << "\n"
+ << VAROUT(data.longs.size()) << " " << VAROUT(dentr.numLongs) << "\n"
+ << VAROUT(data.floats.size()) << " " << VAROUT(dentr.numFloats) << "\n"
+ << VAROUT(data.doubles.size()) << " " << VAROUT(dentr.numDoubles);
+ throw;
}
}
- catch (...)
- {
- ARMARX_ERROR << GetHandledExceptionString() << "\ntype " << static_cast<int>(out.value)
- << "\n"
- << VAROUT(data.bools.size()) << " " << VAROUT(dentr.numBools) << "\n"
- << VAROUT(data.bytes.size()) << " " << VAROUT(dentr.numBytes) << "\n"
- << VAROUT(data.shorts.size()) << " " << VAROUT(dentr.numShorts) << "\n"
- << VAROUT(data.ints.size()) << " " << VAROUT(dentr.numInts) << "\n"
- << VAROUT(data.longs.size()) << " " << VAROUT(dentr.numLongs) << "\n"
- << VAROUT(data.floats.size()) << " " << VAROUT(dentr.numFloats) << "\n"
- << VAROUT(data.doubles.size()) << " " << VAROUT(dentr.numDoubles);
- throw;
- }
-}
-void
-Logging::DataStreamingEntry::processCtrl(const ControlTargetBase& val,
- std::size_t didx,
- std::size_t vidx,
- std::size_t fidx)
-{
- ARMARX_TRACE;
- if (stopStreaming)
+ void
+ Logging::DataStreamingEntry::processCtrl(const ControlTargetBase& val,
+ std::size_t didx,
+ std::size_t vidx,
+ std::size_t fidx)
{
- return;
+ ARMARX_TRACE;
+ if (stopStreaming)
+ {
+ return;
+ }
+ auto& data = result.back();
+ const OutVal& o = ctrlDevs.at(didx).at(vidx).at(fidx);
+ WriteTo(*this, o, val, fidx, data);
}
- auto& data = result.back();
- const OutVal& o = ctrlDevs.at(didx).at(vidx).at(fidx);
- WriteTo(*this, o, val, fidx, data);
-}
-
-void
-Logging::DataStreamingEntry::processSens(const SensorValueBase& val,
- std::size_t didx,
- std::size_t fidx)
-{
- ARMARX_TRACE;
- if (stopStreaming)
+
+ void
+ Logging::DataStreamingEntry::processSens(const SensorValueBase& val,
+ std::size_t didx,
+ std::size_t fidx)
{
- return;
+ ARMARX_TRACE;
+ if (stopStreaming)
+ {
+ return;
+ }
+ auto& data = result.back();
+ const OutVal& o = sensDevs.at(didx).at(fidx);
+ WriteTo(*this, o, val, fidx, data);
}
- auto& data = result.back();
- const OutVal& o = sensDevs.at(didx).at(fidx);
- WriteTo(*this, o, val, fidx, data);
-}
-void
-Logging::DataStreamingEntry::send(const RobotUnitDataStreaming::ReceiverPrx& r, std::uint64_t msgId)
-{
- ARMARX_TRACE;
- const auto start_send = IceUtil::Time::now();
- const auto num_timesteps = result.size();
- updateCalls.emplace_back(r->begin_update(result, static_cast<Ice::Long>(msgId)));
- const auto start_clear = IceUtil::Time::now();
- clearResult();
- const auto end = IceUtil::Time::now();
- ARMARX_DEBUG_S << "Logging::DataStreamingEntry::send"
- << "\n update " << (start_clear - start_send).toMilliSecondsDouble() << "ms ("
- << num_timesteps << " timesteps)"
- << "\n clear " << (end - start_clear).toMilliSecondsDouble() << "ms";
-
- //now execute all ready callbacks
- std::size_t i = 0;
- for (; i < updateCalls.size(); ++i)
+ void
+ Logging::DataStreamingEntry::send(const RobotUnitDataStreaming::ReceiverPrx& r,
+ std::uint64_t msgId)
{
- try
+ ARMARX_TRACE;
+ const auto start_send = armarx::rtNow();
+ const auto num_timesteps = result.size();
+ updateCalls.emplace_back(r->begin_update(result, static_cast<Ice::Long>(msgId)));
+ const auto start_clear = armarx::rtNow();
+ clearResult();
+ const auto end = armarx::rtNow();
+ ARMARX_DEBUG_S << "Logging::DataStreamingEntry::send"
+ << "\n update " << (start_clear - start_send).toMilliSecondsDouble()
+ << "ms (" << num_timesteps << " timesteps)"
+ << "\n clear " << (end - start_clear).toMilliSecondsDouble() << "ms";
+
+ //now execute all ready callbacks
+ std::size_t i = 0;
+ for (; i < updateCalls.size(); ++i)
{
- if (!updateCalls.at(i)->isCompleted())
+ try
{
- break;
+ if (!updateCalls.at(i)->isCompleted())
+ {
+ break;
+ }
+ r->end_update(updateCalls.at(i));
+ connectionFailures = 0;
}
- r->end_update(updateCalls.at(i));
- connectionFailures = 0;
- }
- catch (...)
- {
- ARMARX_TRACE;
- ++connectionFailures;
- if (connectionFailures > rtStreamMaxClientErrors)
+ catch (...)
{
- stopStreaming = true;
- ARMARX_WARNING_S << "DataStreaming Receiver was not reachable for "
- << connectionFailures << " iterations! Removing receiver";
- updateCalls.clear();
- break;
+ ARMARX_TRACE;
+ ++connectionFailures;
+ if (connectionFailures > rtStreamMaxClientErrors)
+ {
+ stopStreaming = true;
+ ARMARX_WARNING_S << "DataStreaming Receiver was not reachable for "
+ << connectionFailures << " iterations! Removing receiver";
+ updateCalls.clear();
+ break;
+ }
}
}
+ if (!updateCalls.empty())
+ {
+ updateCalls.erase(updateCalls.begin(), updateCalls.begin() + i);
+ }
}
- if (!updateCalls.empty())
- {
- updateCalls.erase(updateCalls.begin(), updateCalls.begin() + i);
- }
-}
} // namespace armarx::RobotUnitModule
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModulePublisher.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModulePublisher.cpp
index 5d51c4ca2474c8e46bd3be0140ae45ec9e877e55..4b9a12a21520e58e87f0038c808715be940b8584 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModulePublisher.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModulePublisher.cpp
@@ -255,7 +255,7 @@ namespace armarx::RobotUnitModule
{
ARMARX_TRACE;
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- const auto beg = TimeUtil::GetTime(true);
+ const auto beg = armarx::rtNow();
StringVariantBaseMap ctrlDevMap;
@@ -314,7 +314,7 @@ namespace armarx::RobotUnitModule
}
}
- const auto end = TimeUtil::GetTime(true);
+ const auto end = armarx::rtNow();
return new TimedVariant{TimestampVariant{end - beg}, lastControlThreadTimestamp};
}
@@ -476,6 +476,7 @@ namespace armarx::RobotUnitModule
getProperty<std::string>("DebugDrawerUpdatesTopicName").getValue();
debugObserverTopicName = getProperty<std::string>("DebugObserverTopicName").getValue();
+ observerEnablePublishing = getProperty<bool>("ObserverEnablePublishing").getValue();
observerPublishSensorValues = getProperty<bool>("ObserverPublishSensorValues").getValue();
observerPublishControlTargets =
getProperty<bool>("ObserverPublishControlTargets").getValue();
@@ -575,7 +576,10 @@ namespace armarx::RobotUnitModule
[&] { publish({}); }, publishPeriodMs, false, getName() + "_PublisherTask");
ARMARX_INFO << "starting publisher with timestep " << publishPeriodMs;
publisherTask->setDelayWarningTolerance(10 * publishPeriodMs);
- publisherTask->start();
+ if (observerEnablePublishing)
+ {
+ publisherTask->start();
+ }
}
void
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModulePublisher.h b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModulePublisher.h
index 9f061fd02cad06539fb919e32087912f72713f6e..ffa451eb40630cf7f54e9e4966a93575d6c2fb98 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModulePublisher.h
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModulePublisher.h
@@ -58,6 +58,10 @@ namespace armarx::RobotUnitModule
defineOptionalProperty<std::size_t>(
"PublishPeriodMs", 10, "Milliseconds between each publish");
+ defineOptionalProperty<bool>("ObserverEnablePublishing",
+ true,
+ "Whether the publishing thread is started or not",
+ PropertyDefinitionBase::eModifiable);
defineOptionalProperty<bool>("ObserverPublishSensorValues",
true,
"Whether sensor values are send to the observer",
@@ -271,6 +275,8 @@ namespace armarx::RobotUnitModule
/// @brief Whether \ref SensorValueBase "SensorValues" should be published to the observers
std::atomic_bool observerPublishSensorValues;
+ /// @brief Whether the publishing thread should be started or not
+ std::atomic_bool observerEnablePublishing;
/// @brief Whether \ref ControlTargetBase "ControlTargets" should be published to the observers
std::atomic_bool observerPublishControlTargets;
/// @brief Whether \ref Timing information should be published to the observers
diff --git a/source/RobotAPI/components/units/RobotUnit/util/RtTiming.h b/source/RobotAPI/components/units/RobotUnit/util/RtTiming.h
index 243f1a119df65ab1f5a9f79a5f53d243fe8b102c..7b9e6a630606ae15d3bdab1fc18d107d98a02fca 100644
--- a/source/RobotAPI/components/units/RobotUnit/util/RtTiming.h
+++ b/source/RobotAPI/components/units/RobotUnit/util/RtTiming.h
@@ -24,18 +24,25 @@
#pragma once
#include <chrono>
-//#include <ArmarXCore/core/time/TimeUtil.h>
#include "ControlThreadOutputBuffer.h"
namespace armarx
{
+ namespace rt_timing::constants
+ {
+ inline constexpr const std::int64_t seconds2MicroSeconds = 1e6;
+
+ static constexpr const std::int64_t nanoSeconds2MicroSeconds = 1000;
+ } // namespace rt_timing::constants
inline IceUtil::Time
- rtNow()
+ rtNow(clockid_t clockId = CLOCK_MONOTONIC_RAW)
{
+ using namespace rt_timing::constants;
struct timespec ts;
- clock_gettime(CLOCK_MONOTONIC, &ts);
- return IceUtil::Time::microSeconds(ts.tv_sec * 1e6 + ts.tv_nsec / 1000);
+ clock_gettime(clockId, &ts);
+ return IceUtil::Time::microSeconds(ts.tv_sec * seconds2MicroSeconds +
+ ts.tv_nsec / nanoSeconds2MicroSeconds);
}
} // namespace armarx
@@ -43,16 +50,15 @@ 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, (armarx::rtNow() - name).toMilliSecondsDouble()) \
- .deactivateSpam(1);
+ printf( \
+ "%s - duration: %.3f ms \n", comment, (armarx::rtNow() - name).toMilliSecondsDouble());
//! \ingroup VirtualTime
//! Prints duration
#define RT_TIMING_END(name) RT_TIMING_END_COMMENT(name, #name)
//! \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).toMilliSeconds() >= thresholdMs) \
+ if ((armarx::rtNow() - name).toMilliSecondsDouble() >= thresholdMs) \
RT_TIMING_END_COMMENT(name, comment)
//! \ingroup VirtualTime
//! Prints duration if it took longer than thresholdMs
diff --git a/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp b/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp
index f8a4b50ece99f95d8d4c618b377774e289ec4c03..9b56d6c370d6f24ea3dd98b07ed2bcb0b3ea7d2a 100644
--- a/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp
+++ b/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp
@@ -651,7 +651,8 @@ namespace armarx
{
if (currentNode)
{
- if (currentNode->isRotationalJoint() or currentNode->isHemisphereJoint() or currentNode->isFourBarJoint())
+ if (currentNode->isRotationalJoint() or currentNode->isHemisphereJoint() or
+ currentNode->isFourBarJoint())
{
const bool isDeg = ui.checkBoxUseDegree->isChecked();
const auto factor =
@@ -728,7 +729,8 @@ namespace armarx
{
const QString unit = [&]() -> QString
{
- if (currentNode->isRotationalJoint() or currentNode->isHemisphereJoint() or currentNode->isFourBarJoint())
+ if (currentNode->isRotationalJoint() or currentNode->isHemisphereJoint() or
+ currentNode->isFourBarJoint())
{
if (ui.checkBoxUseDegree->isChecked())
{
@@ -751,7 +753,8 @@ namespace armarx
const auto [factor, conversionFactor] = [&]() -> std::pair<float, float>
{
- if (currentNode->isRotationalJoint() or currentNode->isHemisphereJoint() or currentNode->isFourBarJoint())
+ if (currentNode->isRotationalJoint() or currentNode->isHemisphereJoint() or
+ currentNode->isFourBarJoint())
{
const bool isDeg = ui.checkBoxUseDegree->isChecked();
if (isDeg)
@@ -841,7 +844,8 @@ namespace armarx
const QString unit = [&]() -> QString
{
- if (currentNode->isRotationalJoint() or currentNode->isHemisphereJoint() or currentNode->isFourBarJoint())
+ if (currentNode->isRotationalJoint() or currentNode->isHemisphereJoint() or
+ currentNode->isFourBarJoint())
{
if (ui.checkBoxUseDegree->isChecked())
{
@@ -865,7 +869,8 @@ namespace armarx
<< currentNode->getName() << flush;
const bool isDeg = ui.checkBoxUseDegree->isChecked();
- const bool isRot = currentNode->isRotationalJoint() or currentNode->isHemisphereJoint() or currentNode->isFourBarJoint();
+ const bool isRot = currentNode->isRotationalJoint() or
+ currentNode->isHemisphereJoint() or currentNode->isFourBarJoint();
const float lo = isRot ? (isDeg ? -90 : -M_PI * 100) : -1000;
const float hi = isRot ? (isDeg ? +90 : +M_PI * 100) : 1000;
@@ -1186,7 +1191,8 @@ namespace armarx
const ControlMode currentControlMode = getSelectedControlMode();
const bool isDeg = ui.checkBoxUseDegree->isChecked();
- const bool isRot = currentNode->isRotationalJoint() or currentNode->isHemisphereJoint() or currentNode->isFourBarJoint();
+ const bool isRot = currentNode->isRotationalJoint() or currentNode->isHemisphereJoint() or
+ currentNode->isFourBarJoint();
if (currentControlMode == ePositionControl)
{
@@ -1347,7 +1353,7 @@ namespace armarx
auto it = reportedJointStatuses.find(rn[i]->getName());
if (it == reportedJointStatuses.end())
{
- ARMARX_WARNING << deactivateSpam(5) << "Joint Status for " << rn[i]->getName()
+ ARMARX_WARNING << deactivateSpam(5, rn[i]->getName()) << "Joint Status for " << rn[i]->getName()
<< " was not reported!";
continue;
}
@@ -1441,11 +1447,11 @@ namespace armarx
const float currentValue = it->second;
QModelIndex index = ui.tableJointList->model()->index(i, eTabelColumnAngleProgressbar);
- float conversionFactor =
- ui.checkBoxUseDegree->isChecked() &&
- (node->isRotationalJoint() or node->isHemisphereJoint() or node->isFourBarJoint())
- ? 180.0 / M_PI
- : 1;
+ float conversionFactor = ui.checkBoxUseDegree->isChecked() &&
+ (node->isRotationalJoint() or
+ node->isHemisphereJoint() or node->isFourBarJoint())
+ ? 180.0 / M_PI
+ : 1;
ui.tableJointList->model()->setData(
index,
(int)(cutJitter(currentValue * conversionFactor) * 100) / 100.0f,
@@ -1486,7 +1492,8 @@ namespace armarx
float currentValue = it->second;
if (ui.checkBoxUseDegree->isChecked() &&
- (rn[i]->isRotationalJoint() or rn[i]->isHemisphereJoint() or rn[i]->isFourBarJoint()))
+ (rn[i]->isRotationalJoint() or rn[i]->isHemisphereJoint() or
+ rn[i]->isFourBarJoint()))
{
currentValue *= 180.0 / M_PI;
}
diff --git a/source/RobotAPI/interface/robot_name_service/RobotNameServiceInterface.ice b/source/RobotAPI/interface/robot_name_service/RobotNameServiceInterface.ice
index 7891ed5dbfd4583eb592ebf8913d9c12ca3e9a4c..5fb248cd5f0824e78020c9279f72864b59688ead 100644
--- a/source/RobotAPI/interface/robot_name_service/RobotNameServiceInterface.ice
+++ b/source/RobotAPI/interface/robot_name_service/RobotNameServiceInterface.ice
@@ -27,10 +27,12 @@
#include <RobotAPI/interface/armem/mns.ice>
#include <RobotAPI/interface/skills/SkillManagerInterface.ice>
+#include <RobotAPI/interface/units/ForceTorqueUnit.ice>
#include <RobotAPI/interface/units/HandUnitInterface.ice>
#include <RobotAPI/interface/units/KinematicUnitInterface.ice>
#include <RobotAPI/interface/units/LocalizationUnitInterface.ice>
#include <RobotAPI/interface/units/PlatformUnitInterface.ice>
+#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.ice>
module armarx
{
@@ -39,24 +41,38 @@ module armarx
module dto
{
- struct Hand
+ module location
+ {
+ enum Side
+ {
+ LEFT,
+ RIGHT,
+ UNKNOWN
+ };
+ };
+
+ struct TCPInfo
{
string name;
string ft_name;
+ location::Side side;
HandUnitInterface* handUnitInterface;
};
- dictionary<string, Hand> Hands;
+ // maps tcp name to tcp
+ dictionary<string, TCPInfo> TCPInfos;
- struct Arm
+ struct ArmInfo
{
string kinematicChainName;
- Hand hand;
+ location::Side side;
+ TCPInfo tcpInfo;
};
- dictionary<string, Arm> Arms;
+ // maps kinematic chain name to arm
+ dictionary<string, ArmInfo> ArmInfos;
- struct Robot
+ struct RobotInfo
{
string name;
armarx::data::PackagePath xmlPackagePath;
@@ -66,12 +82,10 @@ module armarx
armarx::skills::manager::dti::SkillManagerInterface* skillManager;
// kinematic stuff. MAY BE EMPTY
- Arms arms;
+ ArmInfos armInfos;
- // units. MAY BE NULL
- armarx::KinematicUnitInterface* kinematicUnitInterface;
- armarx::PlatformUnitInterface* platformUnitInterface;
- armarx::LocalizationUnitInterface* localizationUnitInterface;
+ // RobotUnit
+ armarx::RobotUnitInterface* robotUnit;
// TODO: Feel free to extend!
};
@@ -81,9 +95,9 @@ module armarx
{
interface RobotNameServiceInterface
{
- bool registerRobot(dto::Robot robot);
- void unregisterRobot(string name);
- optional(1) dto::Robot getRobot(string name);
+ bool registerRobotInfo(dto::RobotInfo robot);
+ void unregisterRobotInfo(string name);
+ optional(1) dto::RobotInfo getRobotInfo(string name);
};
};
};
diff --git a/source/RobotAPI/interface/units/RobotUnit/RobotUnitInterface.ice b/source/RobotAPI/interface/units/RobotUnit/RobotUnitInterface.ice
index f59b430191bb535cf527d0d3a624849102385756..32bfba607c2147414e9e61bc8ddd8ea50e9be4d6 100644
--- a/source/RobotAPI/interface/units/RobotUnit/RobotUnitInterface.ice
+++ b/source/RobotAPI/interface/units/RobotUnit/RobotUnitInterface.ice
@@ -284,7 +284,9 @@ module armarx
void deactivateAndDeleteNJointControllers(Ice::StringSeq controllerInstanceNames)throws InvalidArgumentException, LogicError;
//loading libs
+ ["deprecate:loadLibFromPath(string path) has dangerous implications on the RT thread. Use the scenario config instead to load additional libraries. See https://git.h2t.iar.kit.edu/sw/armarx-integration/robots/armar7/documentation/-/issues/85"]
bool loadLibFromPath(string path);
+ ["deprecate:loadLibFromPackage(string package, string libname) has dangerous implications on the RT thread. Use the scenario config instead to load additional libraries. See https://git.h2t.iar.kit.edu/sw/armarx-integration/robots/armar7/documentation/-/issues/85"]
bool loadLibFromPackage(string package, string libname);
};
interface RobotUnitSelfCollisionCheckerInterface
diff --git a/source/RobotAPI/libraries/armem/CMakeLists.txt b/source/RobotAPI/libraries/armem/CMakeLists.txt
index 9030bd17597399abf79e3413ec71a789f3a113ca..670fef073807ec08cf5938f77a5876f099d89f07 100644
--- a/source/RobotAPI/libraries/armem/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem/CMakeLists.txt
@@ -7,6 +7,7 @@ armarx_set_target("Library: ${LIB_NAME}")
set(LIBS
ArmarXCoreInterfaces
ArmarXCore
+ DebugObserverHelper
RemoteGui
aron
aroncommon
@@ -64,8 +65,9 @@ set(LIB_FILES
client/plugins/PluginUser.cpp
client/plugins/Plugin.cpp
- client/util/SubscriptionHandle.cpp
client/util/MemoryListener.cpp
+ client/util/MemoryToDebugObserver.cpp
+ client/util/SubscriptionHandle.cpp
client/util/SimpleReaderBase.cpp
client/util/SimpleWriterBase.cpp
@@ -157,10 +159,11 @@ set(LIB_HEADERS
client/query/detail/NameSelectorOps.h
client/query/detail/SelectorOps.h
- client/util/SubscriptionHandle.h
+ client/util/MemoryToDebugObserver.h
client/util/MemoryListener.h
client/util/SimpleReaderBase.h
client/util/SimpleWriterBase.h
+ client/util/SubscriptionHandle.h
server.h
diff --git a/source/RobotAPI/libraries/armem/client/util/MemoryToDebugObserver.cpp b/source/RobotAPI/libraries/armem/client/util/MemoryToDebugObserver.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..89e2cf20cb7d5786458ed5453ece73749e4783bd
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/client/util/MemoryToDebugObserver.cpp
@@ -0,0 +1,241 @@
+/*
+ * 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::MemoryToDebugObserver
+ * @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
+ * @date 2023
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "MemoryToDebugObserver.h"
+
+#include <RobotAPI/libraries/armem/core/error/mns.h>
+#include <RobotAPI/libraries/armem/core/json_conversions.h>
+#include <RobotAPI/libraries/aron/core/data/variant/Variant.h>
+#include <RobotAPI/libraries/aron/core/data/variant/primitive/All.h>
+#include <RobotAPI/libraries/aron/core/data/visitor/variant/VariantVisitor.h>
+
+namespace armarx::armem::client::util
+{
+
+ MemoryToDebugObserver::MemoryToDebugObserver(const Properties& properties,
+ const Services& services) :
+ properties(properties), services(services)
+ {
+ services.debugObserver.setDebugObserverBatchModeEnabled(true);
+ }
+
+ class Visitor : public aron::data::ConstVariantVisitor
+ {
+ public:
+ Visitor(armarx::DebugObserverHelper& debugObserver) : debugObserver{debugObserver}
+ {
+ }
+
+ // void visitAronVariant(const data::DictPtr&) override;
+ // void visitAronVariant(const data::ListPtr&) override;
+ // void visitAronVariant(const data::NDArrayPtr&) override;
+ void
+ visitAronVariant(const aron::data::IntPtr& v) override
+ {
+ setDatafield(v->getValue());
+ }
+
+ void
+ visitAronVariant(const aron::data::LongPtr& v) override
+ {
+ setDatafield(v->getValue());
+ }
+
+ void
+ visitAronVariant(const aron::data::FloatPtr& v) override
+ {
+ setDatafield(v->getValue());
+ }
+
+ void
+ visitAronVariant(const aron::data::DoublePtr& v) override
+ {
+ setDatafield(v->getValue());
+ }
+
+ void
+ visitAronVariant(const aron::data::BoolPtr& v) override
+ {
+ setDatafield(v->getValue());
+ }
+
+ void
+ visitAronVariant(const aron::data::StringPtr& v) override
+ {
+ setDatafield(v->getValue());
+ }
+
+ template <class ValueT>
+ void
+ setDatafield(const ValueT& value)
+ {
+ debugObserver.setDebugObserverDatafield(channelName, datafieldName, value);
+ ++count;
+ }
+
+ armarx::DebugObserverHelper& debugObserver;
+ std::string channelName;
+ std::string datafieldName;
+
+ int count = 0;
+ };
+
+ void
+ MemoryToDebugObserver::pollOnce()
+ {
+ Visitor visitor(services.debugObserver);
+
+ std::stringstream log;
+
+ // Group by memory segment to reduce number of queries.
+ std::map<MemoryID, std::vector<const MemoryValueID*>> valuesPerProviderSegment;
+ for (const MemoryValueID& valueId : properties.plottedValues)
+ {
+ valuesPerProviderSegment[valueId.entityID.getProviderSegmentID()].push_back(&valueId);
+ }
+
+ for (const auto& [providerSegmentID, values] : valuesPerProviderSegment)
+ {
+ armem::client::Reader* reader = nullptr;
+ try
+ {
+ reader = getReader(providerSegmentID);
+ }
+ catch (armem::error::CouldNotResolveMemoryServer& e)
+ {
+ log << "\n" << e.what();
+ continue;
+ }
+ ARMARX_CHECK_NOT_NULL(reader);
+
+ const QueryResult result = reader->getLatestSnapshotsIn(providerSegmentID);
+ if (not result.success)
+ {
+ log << "Query to provider segment " << providerSegmentID
+ << " failed: " << result.errorMessage;
+ continue;
+ }
+
+ for (const MemoryValueID* valueId : values)
+ {
+ const wm::Entity* entity = result.memory.findEntity(valueId->entityID);
+ if (entity == nullptr)
+ {
+ log << "\nDid not find entity " << valueId->entityID << " in provider segment "
+ << providerSegmentID << ".";
+ continue;
+ }
+
+ const wm::EntityInstance& instance = entity->getLatestInstance();
+ aron::data::VariantPtr valueVariant =
+ instance.data()->navigateAbsolute(valueId->aronPath);
+ if (not valueVariant)
+ {
+ log << "\nDid not find " << valueId->aronPath.toString()
+ << " in entity instance " << instance.id() << ".";
+ continue;
+ }
+
+ visitor.channelName = makeChannelName(valueId->entityID);
+ visitor.datafieldName = makeDatafieldName(valueId->aronPath);
+
+ aron::data::visit(visitor, valueVariant);
+ }
+ }
+
+ services.debugObserver.sendDebugObserverBatch();
+
+ if (not log.str().empty())
+ {
+ ARMARX_INFO << deactivateSpam(60)
+ << "Encountered issues while sending memory values to the debug observer "
+ "for plotting: "
+ << log.str();
+ }
+ }
+
+ std::string
+ MemoryToDebugObserver::makeChannelName(const MemoryID& memoryID)
+ {
+ return simox::alg::replace_all(memoryID.str(), "/", ">");
+ }
+
+ std::string
+ MemoryToDebugObserver::makeDatafieldName(const aron::Path& path)
+ {
+ // The first element is always "ARON->", which we can discard for readability.
+ std::string str = path.toString();
+ str = simox::alg::remove_prefix(str, path.getRootIdentifier() + path.getDelimeter());
+ str = simox::alg::replace_all(str, path.getDelimeter(), ">");
+ return str;
+ }
+
+ Reader*
+ MemoryToDebugObserver::getReader(const MemoryID& memoryID)
+ {
+ auto it = memoryReaders.find(memoryID);
+ if (it != memoryReaders.end())
+ {
+ return &it->second;
+ }
+ else
+ {
+ armem::client::Reader reader = services.memoryNameSystem.getReader(memoryID);
+
+ auto [it, _] = memoryReaders.emplace(memoryID, reader);
+ return &it->second;
+ }
+ }
+
+} // namespace armarx::armem::client::util
+
+namespace armarx::armem::client
+{
+
+ void
+ util::to_json(simox::json::json& j, const MemoryValueID& id)
+ {
+ j["entityID"] = id.entityID.getItems();
+ j["aronPath"] = id.aronPath.getPath();
+ }
+
+ void
+ util::from_json(const simox::json::json& j, MemoryValueID& id)
+ {
+ id.entityID = MemoryID::fromItems(j.at("entityID").get<std::vector<std::string>>());
+ id.aronPath = {j.at("aronPath").get<std::vector<std::string>>()};
+ }
+
+ void
+ util::to_json(simox::json::json& j, const MemoryToDebugObserver::Properties& p)
+ {
+ j["plottedValues"] = p.plottedValues;
+ }
+
+ void
+ util::from_json(const simox::json::json& j, MemoryToDebugObserver::Properties& p)
+ {
+ j.at("plottedValues").get_to(p.plottedValues);
+ }
+
+
+} // namespace armarx::armem::client
diff --git a/source/RobotAPI/libraries/armem/client/util/MemoryToDebugObserver.h b/source/RobotAPI/libraries/armem/client/util/MemoryToDebugObserver.h
new file mode 100644
index 0000000000000000000000000000000000000000..f1628bc09025c99ec6b3d6c8382ad30c79973977
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/client/util/MemoryToDebugObserver.h
@@ -0,0 +1,101 @@
+/*
+ * 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::MemoryToDebugObserver
+ * @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
+ * @date 2023
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <SimoxUtility/json/json.h>
+
+#include <ArmarXCore/libraries/DebugObserverHelper/DebugObserverHelper.h>
+
+#include <RobotAPI/interface/armem/mns/MemoryNameSystemInterface.h>
+#include <RobotAPI/libraries/armem/client/Reader.h>
+#include <RobotAPI/libraries/armem/client/plugins/PluginUser.h>
+#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
+
+namespace armarx::armem::client::util
+{
+ /**
+ * @brief ID of an ARON value in the memory.
+ */
+ struct MemoryValueID
+ {
+ armem::MemoryID entityID;
+ aron::Path aronPath;
+ };
+
+ /**
+ * @brief Transfers data from memory servers to the DebugObserver.
+ *
+ * Transfers data from the memory system to the \ref Component-DebugObserver "Debug Observer",
+ * allowing to visualize them in the \ref ArmarXGui-GuiPlugins-PlotterPlugin "Live Plotter".
+ */
+ class MemoryToDebugObserver
+ {
+ public:
+ /**
+ * @brief Configuration.
+ *
+ * Can be converted to and from JSON.
+ */
+ struct Properties
+ {
+ std::vector<MemoryValueID> plottedValues;
+ };
+
+ /**
+ * @brief Required services.
+ */
+ struct Services
+ {
+ MemoryNameSystem& memoryNameSystem;
+ armarx::DebugObserverHelper& debugObserver;
+ };
+
+ /**
+ * @brief Constructor.
+ */
+ MemoryToDebugObserver(const Properties& properties, const Services& services);
+
+ /**
+ * @brief Query values from the memory and send them to the debug observer.
+ */
+ void pollOnce();
+
+ private:
+ static std::string makeChannelName(const armem::MemoryID& memoryID);
+ static std::string makeDatafieldName(const aron::Path& path);
+
+ armem::client::Reader* getReader(const armem::MemoryID& memoryID);
+
+ Properties properties;
+ Services services;
+
+ std::map<armem::MemoryID, armem::client::Reader> memoryReaders;
+ };
+
+ void to_json(simox::json::json& j, const MemoryValueID& id);
+ void from_json(const simox::json::json& j, MemoryValueID& id);
+
+ void to_json(simox::json::json& j, const MemoryToDebugObserver::Properties& p);
+ void from_json(const simox::json::json& j, MemoryToDebugObserver::Properties& p);
+
+} // namespace armarx::armem::client::util
diff --git a/source/RobotAPI/libraries/armem/client/util/SimpleWriterBase.cpp b/source/RobotAPI/libraries/armem/client/util/SimpleWriterBase.cpp
index 3d8907a348bad7051e3fc88239f3b816899acd5e..c46755e3d9a3c94f6e8634b15884604db71226b9 100644
--- a/source/RobotAPI/libraries/armem/client/util/SimpleWriterBase.cpp
+++ b/source/RobotAPI/libraries/armem/client/util/SimpleWriterBase.cpp
@@ -16,25 +16,28 @@ namespace armarx::armem::client::util
const std::string prefix = propertyPrefix();
- props = defaultProperties();
+ if (not props.has_value())
+ {
+ props = defaultProperties();
+ }
- def->optional(props.memoryName, prefix + "Memory");
- def->optional(props.coreSegmentName, prefix + "CoreSegment");
+ def->optional(props->memoryName, prefix + "Memory");
+ def->optional(props->coreSegmentName, prefix + "CoreSegment");
// TODO(fabian.reister): this might also be part of the subclass
// if the provider name has to be derived from e.g. the component name
- def->optional(props.providerName, prefix + "Provider", "Name of this provider");
+ def->optional(props->providerName, prefix + "Provider", "Name of this provider");
}
void
SimpleWriterBase::connect(armarx::armem::client::MemoryNameSystem& mns)
{
// Wait for the memory to become available and add it as dependency.
- ARMARX_IMPORTANT << "SimpleWriterBase: Waiting for memory '" << props.memoryName << "' ...";
+ ARMARX_IMPORTANT << "SimpleWriterBase: Waiting for memory '" << properties().memoryName << "' ...";
try
{
- memoryWriterClient = mns.useWriter(MemoryID().withMemoryName(props.memoryName));
- ARMARX_IMPORTANT << "SimpleWriterBase: Connected to memory '" << props.memoryName
+ memoryWriterClient = mns.useWriter(MemoryID().withMemoryName(properties().memoryName));
+ ARMARX_IMPORTANT << "SimpleWriterBase: Connected to memory '" << properties().memoryName
<< "'";
}
catch (const armem::error::CouldNotResolveMemoryServer& e)
@@ -53,7 +56,17 @@ namespace armarx::armem::client::util
const SimpleWriterBase::Properties&
SimpleWriterBase::properties() const
{
- return props;
+ if (not props.has_value())
+ {
+ const_cast<std::optional<Properties>&>(props) = defaultProperties();
+ }
+ return props.value();
+ }
+
+ void
+ SimpleWriterBase::setProperties(const Properties& p)
+ {
+ props = p;
}
} // namespace armarx::armem::client::util
diff --git a/source/RobotAPI/libraries/armem/client/util/SimpleWriterBase.h b/source/RobotAPI/libraries/armem/client/util/SimpleWriterBase.h
index 895376b0a2f3ff1f30fdd070c2e50c62b9aa2968..4950280b340f15c293fdd84d9c9fe25e7b19bf9d 100644
--- a/source/RobotAPI/libraries/armem/client/util/SimpleWriterBase.h
+++ b/source/RobotAPI/libraries/armem/client/util/SimpleWriterBase.h
@@ -54,6 +54,7 @@ namespace armarx::armem::client::util
};
const Properties& properties() const;
+ void setProperties(const Properties& p);
virtual std::string propertyPrefix() const = 0;
virtual Properties defaultProperties() const = 0;
@@ -62,7 +63,7 @@ namespace armarx::armem::client::util
private:
- Properties props;
+ std::optional<Properties> props;
armem::client::Writer memoryWriterClient;
};
diff --git a/source/RobotAPI/libraries/armem/core/MemoryID.cpp b/source/RobotAPI/libraries/armem/core/MemoryID.cpp
index c2a59ff9cb7e653f74153ced3eb73d00ed7572c4..cce838327d84b2191f603bf22d654d7c5bc55382 100644
--- a/source/RobotAPI/libraries/armem/core/MemoryID.cpp
+++ b/source/RobotAPI/libraries/armem/core/MemoryID.cpp
@@ -1,14 +1,13 @@
#include "MemoryID.h"
-#include "error/ArMemError.h"
-
-#include <SimoxUtility/algorithm/advanced.h>
-#include <SimoxUtility/algorithm/string/string_tools.h>
+#include <forward_list>
#include <boost/algorithm/string.hpp>
-#include <forward_list>
+#include <SimoxUtility/algorithm/advanced.h>
+#include <SimoxUtility/algorithm/string/string_tools.h>
+#include "error/ArMemError.h"
namespace armarx::armem
{
@@ -16,7 +15,6 @@ namespace armarx::armem
// This is constant, not a variable.
const std::string MemoryID::delimiter = "/";
-
MemoryID::MemoryID()
{
}
@@ -80,14 +78,12 @@ namespace armarx::armem
this->instanceIndex = instanceIndexFromStr(*it);
}
-
- MemoryID::MemoryID(
- const std::string& memoryName,
- const std::string& coreSegmentName,
- const std::string& providerSegmentName,
- const std::string& entityName,
- Time timestamp,
- int instanceIndex) :
+ MemoryID::MemoryID(const std::string& memoryName,
+ const std::string& coreSegmentName,
+ const std::string& providerSegmentName,
+ const std::string& entityName,
+ Time timestamp,
+ int instanceIndex) :
memoryName(memoryName),
coreSegmentName(coreSegmentName),
providerSegmentName(providerSegmentName),
@@ -97,13 +93,14 @@ namespace armarx::armem
{
}
-
- std::string MemoryID::str(bool escapeDelimiters) const
+ std::string
+ MemoryID::str(bool escapeDelimiters) const
{
return str(delimiter, escapeDelimiters);
}
- std::string MemoryID::str(const std::string& delimiter, bool escapeDelimiter) const
+ std::string
+ MemoryID::str(const std::string& delimiter, bool escapeDelimiter) const
{
std::vector<std::string> items = getAllItems(escapeDelimiter);
while (items.size() > 0 && items.back().empty())
@@ -113,7 +110,8 @@ namespace armarx::armem
return simox::alg::join(items, delimiter, false, false);
}
- std::string MemoryID::getLeafItem() const
+ std::string
+ MemoryID::getLeafItem() const
{
std::vector<std::string> items = getAllItems();
for (auto it = items.rbegin(); it != items.rend(); ++it)
@@ -126,7 +124,8 @@ namespace armarx::armem
return "";
}
- bool MemoryID::hasGap() const
+ bool
+ MemoryID::hasGap() const
{
bool emptyFound = false;
for (const std::string& item : getAllItems())
@@ -144,18 +143,59 @@ namespace armarx::armem
return false;
}
- bool MemoryID::isWellDefined() const
+ bool
+ MemoryID::isWellDefined() const
{
return !hasGap();
}
-
- MemoryID MemoryID::fromString(const std::string& string)
+ MemoryID
+ MemoryID::fromString(const std::string& string)
{
return MemoryID(string);
}
- std::vector<std::string> MemoryID::getItems(bool escapeDelimiters) const
+ MemoryID
+ MemoryID::fromItems(const std::vector<std::string>& items)
+ {
+ MemoryID id;
+
+ std::size_t i = 0;
+ if (i < items.size())
+ {
+ id.memoryName = items[i];
+ ++i;
+ }
+ if (i < items.size())
+ {
+ id.coreSegmentName = items[i];
+ ++i;
+ }
+ if (i < items.size())
+ {
+ id.providerSegmentName = items[i];
+ ++i;
+ }
+ if (i < items.size())
+ {
+ id.entityName = items[i];
+ ++i;
+ }
+ if (i < items.size())
+ {
+ id.timestamp = timestampFromStr(items[i]);
+ ++i;
+ }
+ if (i < items.size())
+ {
+ id.instanceIndex = instanceIndexFromStr(items[i]);
+ ++i;
+ }
+ return id;
+ }
+
+ std::vector<std::string>
+ MemoryID::getItems(bool escapeDelimiters) const
{
std::vector<std::string> items;
@@ -194,59 +234,69 @@ namespace armarx::armem
return items;
}
- std::vector<std::string> MemoryID::getAllItems(bool escapeDelimiters) const
+ std::vector<std::string>
+ MemoryID::getAllItems(bool escapeDelimiters) const
{
- return
- {
- escape(memoryName, escapeDelimiters), escape(coreSegmentName, escapeDelimiters),
- escape(providerSegmentName, escapeDelimiters), escape(entityName, escapeDelimiters),
- timestampStr(), instanceIndexStr()
+ return {
+ escape(memoryName, escapeDelimiters),
+ escape(coreSegmentName, escapeDelimiters),
+ escape(providerSegmentName, escapeDelimiters),
+ escape(entityName, escapeDelimiters),
+ timestampStr(),
+ instanceIndexStr(),
};
}
- MemoryID MemoryID::getMemoryID() const
+ MemoryID
+ MemoryID::getMemoryID() const
{
MemoryID id;
id.memoryName = memoryName;
return id;
}
- MemoryID MemoryID::getCoreSegmentID() const
+ MemoryID
+ MemoryID::getCoreSegmentID() const
{
MemoryID id = getMemoryID();
id.coreSegmentName = coreSegmentName;
return id;
}
- MemoryID MemoryID::getProviderSegmentID() const
+ MemoryID
+ MemoryID::getProviderSegmentID() const
{
MemoryID id = getCoreSegmentID();
id.providerSegmentName = providerSegmentName;
return id;
}
- MemoryID MemoryID::getEntityID() const
+ MemoryID
+ MemoryID::getEntityID() const
{
MemoryID id = getProviderSegmentID();
id.entityName = entityName;
return id;
}
- MemoryID MemoryID::getEntitySnapshotID() const
+ MemoryID
+ MemoryID::getEntitySnapshotID() const
{
MemoryID id = getEntityID();
id.timestamp = timestamp;
return id;
}
- MemoryID MemoryID::getEntityInstanceID() const
+ MemoryID
+ MemoryID::getEntityInstanceID() const
{
MemoryID id = getEntitySnapshotID();
id.instanceIndex = instanceIndex;
return id;
}
- MemoryID MemoryID::removeLeafItem() const
+ MemoryID
+ MemoryID::removeLeafItem() const
{
if (instanceIndex != -1)
{
@@ -268,118 +318,133 @@ namespace armarx::armem
{
return getMemoryID();
}
- return {}; // return empty if already empty. Client needs to check (Avoids using optional as additional include)
+ return {}; // return empty if already empty. Client needs to check (Avoids using optional as additional include)
}
- void MemoryID::setMemoryID(const MemoryID& id)
+ void
+ MemoryID::setMemoryID(const MemoryID& id)
{
memoryName = id.memoryName;
}
- void MemoryID::setCoreSegmentID(const MemoryID& id)
+ void
+ MemoryID::setCoreSegmentID(const MemoryID& id)
{
setMemoryID(id);
coreSegmentName = id.coreSegmentName;
}
- void MemoryID::setProviderSegmentID(const MemoryID& id)
+ void
+ MemoryID::setProviderSegmentID(const MemoryID& id)
{
setCoreSegmentID(id);
providerSegmentName = id.providerSegmentName;
}
- void MemoryID::setEntityID(const MemoryID& id)
+ void
+ MemoryID::setEntityID(const MemoryID& id)
{
setProviderSegmentID(id);
entityName = id.entityName;
}
- void MemoryID::setEntitySnapshotID(const MemoryID& id)
+ void
+ MemoryID::setEntitySnapshotID(const MemoryID& id)
{
setEntityID(id);
timestamp = id.timestamp;
}
- void MemoryID::setEntityInstanceID(const MemoryID& id)
+ void
+ MemoryID::setEntityInstanceID(const MemoryID& id)
{
setEntitySnapshotID(id);
instanceIndex = id.instanceIndex;
}
- MemoryID MemoryID::withMemoryName(const std::string& name) const
+ MemoryID
+ MemoryID::withMemoryName(const std::string& name) const
{
MemoryID id = *this;
id.memoryName = name;
return id;
}
- MemoryID MemoryID::withCoreSegmentName(const std::string& name) const
+ MemoryID
+ MemoryID::withCoreSegmentName(const std::string& name) const
{
MemoryID id = *this;
id.coreSegmentName = name;
return id;
}
- MemoryID MemoryID::withProviderSegmentName(const std::string& name) const
+ MemoryID
+ MemoryID::withProviderSegmentName(const std::string& name) const
{
MemoryID id = *this;
id.providerSegmentName = name;
return id;
}
- MemoryID MemoryID::withEntityName(const std::string& name) const
+ MemoryID
+ MemoryID::withEntityName(const std::string& name) const
{
MemoryID id = *this;
id.entityName = name;
return id;
}
- MemoryID MemoryID::withTimestamp(Time time) const
+ MemoryID
+ MemoryID::withTimestamp(Time time) const
{
MemoryID id = *this;
id.timestamp = time;
return id;
}
- MemoryID MemoryID::withInstanceIndex(int index) const
+ MemoryID
+ MemoryID::withInstanceIndex(int index) const
{
MemoryID id = *this;
id.instanceIndex = index;
return id;
}
-
- std::string MemoryID::timestampStr() const
+ std::string
+ MemoryID::timestampStr() const
{
return hasTimestamp() ? std::to_string(timestamp.toMicroSecondsSinceEpoch()) : "";
}
- std::string MemoryID::instanceIndexStr() const
+ std::string
+ MemoryID::instanceIndexStr() const
{
return hasInstanceIndex() ? std::to_string(instanceIndex) : "";
}
- Time MemoryID::timestampFromStr(const std::string& string)
+ Time
+ MemoryID::timestampFromStr(const std::string& string)
{
return Time(Duration::MicroSeconds(parseInteger(string, "timestamp")));
}
- int MemoryID::instanceIndexFromStr(const std::string& string)
+ int
+ MemoryID::instanceIndexFromStr(const std::string& string)
{
return int(parseInteger(string, "instance index"));
}
- bool MemoryID::operator==(const MemoryID& other) const
+ bool
+ MemoryID::operator==(const MemoryID& other) const
{
- return memoryName == other.memoryName
- and coreSegmentName == other.coreSegmentName
- and providerSegmentName == other.providerSegmentName
- and entityName == other.entityName
- and timestamp == other.timestamp
- and instanceIndex == other.instanceIndex;
+ return memoryName == other.memoryName and coreSegmentName == other.coreSegmentName and
+ providerSegmentName == other.providerSegmentName and
+ entityName == other.entityName and timestamp == other.timestamp and
+ instanceIndex == other.instanceIndex;
}
- bool MemoryID::operator<(const MemoryID& rhs) const
+ bool
+ MemoryID::operator<(const MemoryID& rhs) const
{
int c = memoryName.compare(rhs.memoryName);
if (c != 0)
@@ -413,7 +478,8 @@ namespace armarx::armem
return instanceIndex < rhs.instanceIndex;
}
- long long MemoryID::parseInteger(const std::string& string, const std::string& semanticName)
+ long long
+ MemoryID::parseInteger(const std::string& string, const std::string& semanticName)
{
if (string.empty())
{
@@ -433,12 +499,14 @@ namespace armarx::armem
}
}
- std::string MemoryID::escapeDelimiter(const std::string& name)
+ std::string
+ MemoryID::escapeDelimiter(const std::string& name)
{
return simox::alg::replace_all(name, delimiter, "\\" + delimiter);
}
- std::string MemoryID::escape(const std::string& name, bool escapeDelimiters)
+ std::string
+ MemoryID::escape(const std::string& name, bool escapeDelimiters)
{
if (escapeDelimiters)
{
@@ -450,24 +518,27 @@ namespace armarx::armem
}
}
- std::ostream& operator<<(std::ostream& os, const MemoryID id)
+ std::ostream&
+ operator<<(std::ostream& os, const MemoryID id)
{
return os << "'" << id.str() << "'";
}
-
- bool contains(const MemoryID& general, const MemoryID& specific)
+ bool
+ contains(const MemoryID& general, const MemoryID& specific)
{
if (!general.isWellDefined())
{
std::stringstream ss;
- ss << "\nGeneral ID " << general << " is not well-defined, which is required for `" << __FUNCTION__ << "()`.";
+ ss << "\nGeneral ID " << general << " is not well-defined, which is required for `"
+ << __FUNCTION__ << "()`.";
throw error::InvalidMemoryID(general, ss.str());
}
if (!specific.isWellDefined())
{
std::stringstream ss;
- ss << "\nSpecific ID " << specific << " is not well-defined, which is required for `" << __FUNCTION__ << "()`.";
+ ss << "\nSpecific ID " << specific << " is not well-defined, which is required for `"
+ << __FUNCTION__ << "()`.";
throw error::InvalidMemoryID(specific, ss.str());
}
@@ -528,4 +599,4 @@ namespace armarx::armem
return true;
}
-}
+} // namespace armarx::armem
diff --git a/source/RobotAPI/libraries/armem/core/MemoryID.h b/source/RobotAPI/libraries/armem/core/MemoryID.h
index 64850ddefa9d2e92cc924e33d765317a8895c067..d9dca2a2541bae3462a18ad1603a048f804fc551 100644
--- a/source/RobotAPI/libraries/armem/core/MemoryID.h
+++ b/source/RobotAPI/libraries/armem/core/MemoryID.h
@@ -1,12 +1,11 @@
#pragma once
-#include <functional> // for std::hash
+#include <functional> // for std::hash
#include <string>
#include <vector>
#include "Time.h"
-
namespace armarx::armem
{
@@ -48,7 +47,6 @@ namespace armarx::armem
class MemoryID
{
public:
-
std::string memoryName = "";
std::string coreSegmentName = "";
std::string providerSegmentName = "";
@@ -58,7 +56,6 @@ namespace armarx::armem
public:
-
/// Construct a default (empty) memory ID.
MemoryID();
/// (Re-)Construct a memory ID from a string representation as returned by `str()`.
@@ -72,6 +69,13 @@ namespace armarx::armem
int instanceIndex = -1);
+ /// Alias for constructor from string.
+ static MemoryID fromString(const std::string& string);
+
+ /// Constructor memory ID from items as returned by getItems().
+ static MemoryID fromItems(const std::vector<std::string>& items);
+
+
/**
* @brief Indicate whether this ID is well-defined.
*
@@ -93,43 +97,56 @@ namespace armarx::armem
*/
bool isWellDefined() const;
-
// Checks whether a specific level is specified.
- bool hasMemoryName() const
+ bool
+ hasMemoryName() const
{
return not memoryName.empty();
}
- bool hasCoreSegmentName() const
+
+ bool
+ hasCoreSegmentName() const
{
return not coreSegmentName.empty();
}
- bool hasProviderSegmentName() const
+
+ bool
+ hasProviderSegmentName() const
{
return not providerSegmentName.empty();
}
- bool hasEntityName() const
+
+ bool
+ hasEntityName() const
{
return not entityName.empty();
}
- bool hasTimestamp() const
+
+ bool
+ hasTimestamp() const
{
return timestamp.isValid();
}
- void clearTimestamp()
+
+ void
+ clearTimestamp()
{
timestamp = Time::Invalid();
}
- bool hasInstanceIndex() const
+
+ bool
+ hasInstanceIndex() const
{
return instanceIndex >= 0;
}
- void clearInstanceIndex()
+
+ void
+ clearInstanceIndex()
{
instanceIndex = -1;
}
-
// Slice getters: Get upper part of the ID.
MemoryID getMemoryID() const;
@@ -182,8 +199,6 @@ namespace armarx::armem
/// Get the instance index as string.
std::string instanceIndexStr() const;
- /// Alias for constructor from string.
- static MemoryID fromString(const std::string& string);
/// Reconstruct a timestamp from a string as returned by `timestampStr()`.
static Time timestampFromStr(const std::string& timestamp);
/// Reconstruct an instance index from a string as returned by `instanceIndexStr()`.
@@ -206,31 +221,38 @@ namespace armarx::armem
// Operators
- bool operator ==(const MemoryID& other) const;
- inline bool operator !=(const MemoryID& other) const
+ bool operator==(const MemoryID& other) const;
+
+ inline bool
+ operator!=(const MemoryID& other) const
{
- return not (*this == other);
+ return not(*this == other);
}
- bool operator< (const MemoryID& rhs) const;
- inline bool operator> (const MemoryID& rhs) const
+ bool operator<(const MemoryID& rhs) const;
+
+ inline bool
+ operator>(const MemoryID& rhs) const
{
return rhs < (*this);
}
- inline bool operator<=(const MemoryID& rhs) const
+
+ inline bool
+ operator<=(const MemoryID& rhs) const
{
- return not operator> (rhs);
+ return not operator>(rhs);
}
- inline bool operator>=(const MemoryID& rhs) const
+
+ inline bool
+ operator>=(const MemoryID& rhs) const
{
- return not operator< (rhs);
+ return not operator<(rhs);
}
friend std::ostream& operator<<(std::ostream& os, const MemoryID id);
private:
-
static long long parseInteger(const std::string& string, const std::string& semanticName);
static std::string escapeDelimiter(const std::string& name);
static std::string escape(const std::string& name, bool escapeDelimiters);
@@ -240,10 +262,8 @@ namespace armarx::armem
// Do not allow specifying the delimiter from outside.
std::string str(const std::string& delimiter, bool escapeDelimiter) const;
-
};
-
/**
* @brief Indicates whether `general` is "less specific" than, or equal to, `specific`,
* i.e. `general` "contains" `specific`.
@@ -266,8 +286,7 @@ namespace armarx::armem
*/
bool contains(const MemoryID& general, const MemoryID& specific);
-} // namespace armarx::armem
-
+} // namespace armarx::armem
namespace std
{
@@ -275,12 +294,12 @@ namespace std
template <>
struct hash<armarx::armem::MemoryID>
{
- std::size_t operator()(const armarx::armem::MemoryID& id) const
+ std::size_t
+ operator()(const armarx::armem::MemoryID& id) const
{
const std::string sid = id.str();
return std::hash<string>()(sid);
}
};
-}
-
+} // namespace std
diff --git a/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.cpp b/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.cpp
index 1dbb15e5cbcb3e1d5275fcfc9ea174a684c0f7fb..194785757e80ecbb9b77b06ec9224f500c85ec55 100644
--- a/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.cpp
+++ b/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.cpp
@@ -157,6 +157,50 @@ namespace armarx::armem::server
return this->_commit(commit, false);
}
+ data::CommitResult
+ MemoryToIceAdapter::commitLocking(const data::Commit& commitIce, Time timeArrived)
+ {
+ ARMARX_TRACE;
+ ARMARX_CHECK_NOT_NULL(workingMemory);
+ auto handleException = [](const std::string& what)
+ {
+ data::CommitResult result;
+ data::EntityUpdateResult& r = result.results.emplace_back();
+ r.success = false;
+ r.errorMessage = what;
+ return result;
+ };
+
+ armem::Commit commit;
+ try
+ {
+ ::armarx::armem::fromIce(commitIce, commit, timeArrived);
+ }
+ catch (const aron::error::AronNotValidException& e)
+ {
+ throw;
+ return handleException(e.what());
+ }
+ catch (const Ice::Exception& e)
+ {
+ throw;
+ return handleException(e.what());
+ }
+
+ armem::CommitResult result = this->commitLocking(commit);
+ data::CommitResult resultIce;
+ toIce(resultIce, result);
+
+ return resultIce;
+ }
+
+ data::CommitResult
+ MemoryToIceAdapter::commitLocking(const data::Commit& commitIce)
+ {
+ ARMARX_TRACE;
+ return commitLocking(commitIce, armem::Time::Now());
+ }
+
armem::CommitResult
MemoryToIceAdapter::commitLocking(const armem::Commit& commit)
{
diff --git a/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.h b/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.h
index a451054a2fb667e99c9472d847c86e3e2cb48dea..f6fdfe9eaf1a985fe3ceba8a17c2665111333b0e 100644
--- a/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.h
+++ b/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.h
@@ -37,6 +37,8 @@ namespace armarx::armem::server
data::CommitResult commit(const data::Commit& commitIce, Time timeArrived);
data::CommitResult commit(const data::Commit& commitIce);
armem::CommitResult commit(const armem::Commit& commit);
+ data::CommitResult commitLocking(const data::Commit& commitIce, Time timeArrived);
+ data::CommitResult commitLocking(const data::Commit& commitIce);
armem::CommitResult commitLocking(const armem::Commit& commit);
diff --git a/source/RobotAPI/libraries/armem/server/plugins/ReadWritePluginUser.cpp b/source/RobotAPI/libraries/armem/server/plugins/ReadWritePluginUser.cpp
index 0578214ecb8270828d6de3a9d7764c945c170311..1c039d569d1dc5007f72e37ca6a69e200ab07699 100644
--- a/source/RobotAPI/libraries/armem/server/plugins/ReadWritePluginUser.cpp
+++ b/source/RobotAPI/libraries/armem/server/plugins/ReadWritePluginUser.cpp
@@ -48,7 +48,7 @@ namespace armarx::armem::server::plugins
ReadWritePluginUser::commit(const data::Commit& commitIce, const Ice::Current&)
{
ARMARX_TRACE;
- return iceAdapter().commit(commitIce);
+ return iceAdapter().commitLocking(commitIce);
}
// READING
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/base/MemoryQueryProcessorBase.h b/source/RobotAPI/libraries/armem/server/query_proc/base/MemoryQueryProcessorBase.h
index ae8e1ddb8a28c0b6f2785e8d365a2c844cd200fe..89a7e2231d8bbbb00f8b45f829bea4a5eb939603 100644
--- a/source/RobotAPI/libraries/armem/server/query_proc/base/MemoryQueryProcessorBase.h
+++ b/source/RobotAPI/libraries/armem/server/query_proc/base/MemoryQueryProcessorBase.h
@@ -1,12 +1,11 @@
#pragma once
-#include "BaseQueryProcessorBase.h"
+#include <regex>
-#include <RobotAPI/libraries/armem/core/error.h>
#include <RobotAPI/interface/armem/query.h>
+#include <RobotAPI/libraries/armem/core/error.h>
-#include <regex>
-
+#include "BaseQueryProcessorBase.h"
namespace armarx::armem::server::query_proc::base
{
@@ -16,11 +15,10 @@ namespace armarx::armem::server::query_proc::base
public BaseQueryProcessorBase<_MemoryT, _ResultMemoryT, armem::query::data::MemoryQuery>
{
protected:
-
- using Base = BaseQueryProcessorBase<_MemoryT, _ResultMemoryT, armem::query::data::MemoryQuery>;
+ using Base =
+ BaseQueryProcessorBase<_MemoryT, _ResultMemoryT, armem::query::data::MemoryQuery>;
public:
-
using MemoryT = _MemoryT;
using CoreSegmentT = typename MemoryT::CoreSegmentT;
@@ -31,25 +29,26 @@ namespace armarx::armem::server::query_proc::base
public:
-
MemoryQueryProcessorBase()
{
}
- MemoryQueryProcessorBase(ChildProcessorT&& childProcessor) :
- childProcessor(childProcessor)
+
+ MemoryQueryProcessorBase(ChildProcessorT&& childProcessor) : childProcessor(childProcessor)
{
}
-
using Base::process;
- ResultMemoryT process(const armem::query::data::Input& input, const MemoryT& memory) const
+
+ ResultMemoryT
+ process(const armem::query::data::Input& input, const MemoryT& memory) const
{
return this->process(input.memoryQueries, memory);
}
- virtual void process(ResultMemoryT& result,
- const armem::query::data::MemoryQuery& query,
- const MemoryT& memory) const override
+ virtual void
+ process(ResultMemoryT& result,
+ const armem::query::data::MemoryQuery& query,
+ const MemoryT& memory) const override
{
if (auto q = dynamic_cast<const armem::query::data::memory::All*>(&query))
{
@@ -69,19 +68,19 @@ namespace armarx::armem::server::query_proc::base
}
}
- virtual void process(ResultMemoryT& result,
- const armem::query::data::memory::All& query,
- const MemoryT& memory) const
+ virtual void
+ process(ResultMemoryT& result,
+ const armem::query::data::memory::All& query,
+ const MemoryT& memory) const
{
memory.forEachCoreSegment([this, &result, &query](const CoreSegmentT& coreSegment)
- {
- this->_processResult(result, coreSegment, query);
- });
+ { this->_processResult(result, coreSegment, query); });
}
- virtual void process(ResultMemoryT& result,
- const armem::query::data::memory::Single& query,
- const MemoryT& memory) const
+ virtual void
+ process(ResultMemoryT& result,
+ const armem::query::data::memory::Single& query,
+ const MemoryT& memory) const
{
if (auto coreSegment = memory.findCoreSegment(query.coreSegmentName))
{
@@ -89,32 +88,35 @@ namespace armarx::armem::server::query_proc::base
}
}
- virtual void process(ResultMemoryT& result,
- const armem::query::data::memory::Regex& query,
- const MemoryT& memory) const
+ virtual void
+ process(ResultMemoryT& result,
+ const armem::query::data::memory::Regex& query,
+ const MemoryT& memory) const
{
const std::regex regex(query.coreSegmentNameRegex);
- memory.forEachCoreSegment([this, &result, &query, ®ex](const CoreSegmentT& coreSegment)
- {
- if (std::regex_search(coreSegment.name(), regex))
+ memory.forEachCoreSegment(
+ [this, &result, &query, ®ex](const CoreSegmentT& coreSegment)
{
- this->_processResult(result, coreSegment, query);
- }
- });
+ if (std::regex_search(coreSegment.name(), regex))
+ {
+ this->_processResult(result, coreSegment, query);
+ }
+ });
}
protected:
-
- virtual bool _processAllowed(const armem::query::data::MemoryQuery& query) const
+ virtual bool
+ _processAllowed(const armem::query::data::MemoryQuery& query) const
{
// always execute query. Override if you want to execute the quey only if a special condition is fulfilled (e.g. querytargets)
return true;
}
- void _processResult(ResultMemoryT& result,
- const CoreSegmentT& coreSegment,
- const armem::query::data::MemoryQuery& query) const
+ void
+ _processResult(ResultMemoryT& result,
+ const CoreSegmentT& coreSegment,
+ const armem::query::data::MemoryQuery& query) const
{
ResultCoreSegmentT* child = result.findCoreSegment(coreSegment.name());
if (child == nullptr)
@@ -126,8 +128,6 @@ namespace armarx::armem::server::query_proc::base
protected:
-
ChildProcessorT childProcessor;
-
};
-}
+} // namespace armarx::armem::server::query_proc::base
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/wm/wm.h b/source/RobotAPI/libraries/armem/server/query_proc/wm/wm.h
index 312a39896aa717fb8d17115dfe4c0c904a524181..4ecb033d49c1caff143808c36ac915f65497d78b 100644
--- a/source/RobotAPI/libraries/armem/server/query_proc/wm/wm.h
+++ b/source/RobotAPI/libraries/armem/server/query_proc/wm/wm.h
@@ -207,7 +207,6 @@ namespace armarx::armem::server::query_proc::wm_server
MemoryQueryProcessor(armem::query::DataMode dataMode = armem::query::DataMode::WithData);
using Base::process;
-
};
}
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 b4c59389a918789222beb3595a29ec49320775df..5e79a0a8d032429dacc6ef9ccc69e8fa0edde788 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/display_visitors/TypedDataDisplayVisitor.cpp
+++ b/source/RobotAPI/libraries/armem_gui/instance/display_visitors/TypedDataDisplayVisitor.cpp
@@ -97,6 +97,12 @@ namespace armarx::aron
type::IntEnumPtr enumType = type::IntEnum::DynamicCast(type);
data::IntPtr enumData = data::Int::DynamicCast(data);
+ if (enumType == nullptr or enumData == nullptr)
+ {
+ ARMARX_WARNING << "Enum Type or Data is NULL";
+ return;
+ }
+
std::string name = enumType->getValueName(enumData->getValue());
value << name;
}
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
index d6635696acd4f72d3c3342423233a459baf89af0..62dec875ea7ccb106b762edec6ae79e2341e27b0 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
@@ -226,107 +226,121 @@ namespace armarx::armem::server::obj::instance
}
}
- if (!discard)
+ if (discard)
{
- // Update the entity.
- stats.numUpdated++;
+ continue;
+ }
- VirtualRobot::RobotPtr robot =
- robots.get(provided.robotName, provided.providerName);
+ // Update the entity.
+ stats.numUpdated++;
- if (not robot)
- {
- ARMARX_INFO << deactivateSpam(1) << "Failed to retrieve robot `"
- << provided.robotName << "`.";
- }
+ VirtualRobot::RobotPtr robot = robots.get(provided.robotName, provided.providerName);
+ if (not robot)
+ {
+ ARMARX_INFO << deactivateSpam(1) << "Failed to retrieve robot `"
+ << provided.robotName << "`.";
+ }
- // robot may be null!
+ // robot may be null!
- // Update the robot to obtain correct local -> global transformation
- if (robot and robotSyncTimestamp != timestamp)
- {
- ARMARX_CHECK(robots.reader->synchronizeRobot(*robot, timestamp))
- << "Failed to synchronize robot to timestamp " << timestamp << ". This is "
- << (Clock::Now() - timestamp).toSecondsDouble() << " seconds in the past.";
+ bool synchronized = false;
- robotSyncTimestamp = timestamp;
+ // Update the robot to obtain correct local -> global transformation
+ if (robot and robotSyncTimestamp != timestamp)
+ {
+ synchronized = robots.reader->synchronizeRobot(*robot, timestamp);
+ if (not synchronized)
+ {
+ ARMARX_VERBOSE << "Failed to synchronize robot to timestamp " << timestamp
+ << " of provided object pose (" << provided.objectID
+ << "). This is " << (Clock::Now() - timestamp).toSecondsDouble()
+ << " seconds in the past.";
+ }
+ robotSyncTimestamp = timestamp;
- // Apply calibration offset after synchronizing the robot.
+ // Apply calibration offset after synchronizing the robot.
+ {
+ if (calibration.offset != 0 and robot->hasRobotNode(calibration.robotNode))
{
- if (calibration.offset != 0 and robot->hasRobotNode(calibration.robotNode))
- {
- VirtualRobot::RobotNodePtr robotNode =
- robot->getRobotNode(calibration.robotNode);
+ VirtualRobot::RobotNodePtr robotNode =
+ robot->getRobotNode(calibration.robotNode);
- float value = robotNode->getJointValue();
- float newValue = value + calibration.offset;
+ float value = robotNode->getJointValue();
+ float newValue = value + calibration.offset;
- /*
+ /*
* If newValue is out of the joint's limits, then the calibration would be ignored.
* In that case, we expand the joint value limits of the local robot model to allow
* for the calibrated value.
* As this is just for perception (and not for controlling the robot), this should be fine^TM.
*/
- VirtualRobot::RobotNode::JointLimits limits =
- robotNode->getJointLimits();
- bool limitsChanged = false;
- if (newValue < limits.low)
- {
- limits.low = newValue;
- limitsChanged = true;
- }
- if (newValue > limits.high)
- {
- limits.high = newValue;
- limitsChanged = true;
- }
- if (limitsChanged)
- {
- robotNode->setJointLimits(limits.low, limits.high);
- }
-
- robotNode->setJointValue(newValue);
+ VirtualRobot::RobotNode::JointLimits limits = robotNode->getJointLimits();
+ bool limitsChanged = false;
+ if (newValue < limits.low)
+ {
+ limits.low = newValue;
+ limitsChanged = true;
}
+ if (newValue > limits.high)
+ {
+ limits.high = newValue;
+ limitsChanged = true;
+ }
+ if (limitsChanged)
+ {
+ robotNode->setJointLimits(limits.low, limits.high);
+ }
+
+ robotNode->setJointValue(newValue);
}
}
+ }
- objpose::ObjectPose& newPose = newObjectPoses.emplace_back();
- if (provided.objectPoseFrame.empty())
- {
- objpose::data::ProvidedObjectPose copy = provided;
- copy.objectPoseFrame = armarx::GlobalFrame;
- newPose.fromProvidedPose(copy, robot); // robot == nullptr is OK.
- }
- else
- {
- newPose.fromProvidedPose(provided, robot); // robot == nullptr is OK.
- }
+ if (not synchronized)
+ {
+ /*
+ * We know nothing about the current robot configuration. So the behvaiour of the
+ * following code is the same as if we have no robot model at all.
+ */
+ robot = nullptr;
+ }
- if (previousPose && previousPose->attachment)
- {
- // Keep current attachment.
- ARMARX_CHECK(!p.discardSnapshotsWhileAttached);
- newPose.attachment = previousPose->attachment;
- }
+ objpose::ObjectPose& newPose = newObjectPoses.emplace_back();
+ if (provided.objectPoseFrame.empty())
+ {
+ objpose::data::ProvidedObjectPose copy = provided;
+ copy.objectPoseFrame = armarx::GlobalFrame;
+ newPose.fromProvidedPose(copy, robot); // robot == nullptr is OK.
+ }
+ else
+ {
+ newPose.fromProvidedPose(provided, robot); // robot == nullptr is OK.
+ }
- if (newPose.objectID.dataset().empty())
- {
- // Try to find the data set.
- const std::string dataset =
- classNameToDatasetCache.get(newPose.objectID.className());
- if (!dataset.empty())
- {
- newPose.objectID = {
- dataset, newPose.objectID.className(), newPose.objectID.instanceName()};
- }
- }
- if (!provided.localOOBB)
+ if (previousPose && previousPose->attachment)
+ {
+ // Keep current attachment.
+ ARMARX_CHECK(!p.discardSnapshotsWhileAttached);
+ newPose.attachment = previousPose->attachment;
+ }
+
+ if (newPose.objectID.dataset().empty())
+ {
+ // Try to find the data set.
+ const std::string dataset =
+ classNameToDatasetCache.get(newPose.objectID.className());
+ if (!dataset.empty())
{
- // Try to load oobb from disk.
- newPose.localOOBB = getObjectOOBB(newPose.objectID);
+ newPose.objectID = {
+ dataset, newPose.objectID.className(), newPose.objectID.instanceName()};
}
}
+ if (!provided.localOOBB)
+ {
+ // Try to load oobb from disk.
+ newPose.localOOBB = getObjectOOBB(newPose.objectID);
+ }
}
commitObjectPoses(newObjectPoses, providerName);
diff --git a/source/RobotAPI/libraries/armem_robot/aron/Robot.xml b/source/RobotAPI/libraries/armem_robot/aron/Robot.xml
index 7303d707d40e0f742a7faa2c9d1b8e4a9b561714..b7192c366e7ae685c176578b03a44d9f533f1e75 100644
--- a/source/RobotAPI/libraries/armem_robot/aron/Robot.xml
+++ b/source/RobotAPI/libraries/armem_robot/aron/Robot.xml
@@ -1,4 +1,3 @@
-<!--This class contains the data structure for ObjectPose -->
<?xml version="1.0" encoding="UTF-8" ?>
<AronTypeDefinition>
<AronIncludes>
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 a8280d7e93ee5513449571a8c13d8ee086c9843d..8f7c057e4dbd935d5574e6ee4914280611c7808a 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp
@@ -162,6 +162,7 @@ namespace armarx::armem::robot_state
try
{
+ std::scoped_lock l(memoryReaderMutex);
const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
ARMARX_DEBUG << "Lookup result in reader: " << qResult;
@@ -256,6 +257,7 @@ namespace armarx::armem::robot_state
try
{
+ std::scoped_lock l(memoryReaderMutex);
const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
ARMARX_DEBUG << "Lookup result in reader: " << qResult;
@@ -294,17 +296,27 @@ namespace armarx::armem::robot_state
.snapshots().timeRange(begin, end);
// clang-format on
- const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
+ try
+ {
+ std::scoped_lock l(memoryReaderMutex);
+ const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
+
+ ARMARX_DEBUG << "Lookup result in reader: " << qResult;
- ARMARX_DEBUG << "Lookup result in reader: " << qResult;
+ if (not qResult.success) /* c++20 [[unlikely]] */
+ {
+ ARMARX_VERBOSE << qResult.errorMessage;
+ return {};
+ }
- if (not qResult.success) /* c++20 [[unlikely]] */
+ return getRobotJointStates(qResult.memory, description.name);
+ }
+ catch (...)
{
- ARMARX_VERBOSE << qResult.errorMessage;
+ ARMARX_VERBOSE << deactivateSpam(1) << "Failed to query joint states. Reason: "
+ << GetHandledExceptionString();
return {};
}
-
- return getRobotJointStates(qResult.memory, description.name);
}
std::optional<robot::PlatformState>
@@ -326,17 +338,27 @@ namespace armarx::armem::robot_state
.snapshots().beforeOrAtTime(timestamp);
// clang-format on
- const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
+ try
+ {
+ std::scoped_lock l(memoryReaderMutex);
+ const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
- ARMARX_DEBUG << "Lookup result in reader: " << qResult;
+ ARMARX_DEBUG << "Lookup result in reader: " << qResult;
- if (not qResult.success) /* c++20 [[unlikely]] */
+ if (not qResult.success) /* c++20 [[unlikely]] */
+ {
+ ARMARX_VERBOSE << qResult.errorMessage;
+ return std::nullopt;
+ }
+
+ return getRobotPlatformState(qResult.memory, description.name);
+ }
+ catch (...)
{
- ARMARX_VERBOSE << qResult.errorMessage;
+ ARMARX_VERBOSE << deactivateSpam(1) << "Failed to query platform state. Reason: "
+ << GetHandledExceptionString();
return std::nullopt;
}
-
- return getRobotPlatformState(qResult.memory, description.name);
}
std::optional<robot::RobotState::Pose>
@@ -503,17 +525,27 @@ namespace armarx::armem::robot_state
.snapshots().beforeOrAtTime(timestamp);
// clang-format on
- const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
+ try
+ {
+ std::scoped_lock l(memoryReaderMutex);
+ const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
+
+ ARMARX_DEBUG << "Lookup result in reader: " << qResult;
- ARMARX_DEBUG << "Lookup result in reader: " << qResult;
+ if (not qResult.success) /* c++20 [[unlikely]] */
+ {
+ ARMARX_VERBOSE << qResult.errorMessage;
+ return std::nullopt;
+ }
- if (not qResult.success) /* c++20 [[unlikely]] */
+ return getForceTorque(qResult.memory, description.name);
+ }
+ catch (...)
{
- ARMARX_VERBOSE << qResult.errorMessage;
+ ARMARX_VERBOSE << deactivateSpam(1) << "Failed to query force torque. Reason: "
+ << GetHandledExceptionString();
return std::nullopt;
}
-
- return getForceTorque(qResult.memory, description.name);
}
// force torque for left and right
@@ -536,17 +568,27 @@ namespace armarx::armem::robot_state
.snapshots().timeRange(start, end);
// clang-format on
- const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
+ try
+ {
+ std::scoped_lock l(memoryReaderMutex);
+ const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
+
+ ARMARX_DEBUG << "Lookup result in reader: " << qResult;
- ARMARX_DEBUG << "Lookup result in reader: " << qResult;
+ if (not qResult.success) /* c++20 [[unlikely]] */
+ {
+ ARMARX_VERBOSE << qResult.errorMessage;
+ return std::nullopt;
+ }
- if (not qResult.success) /* c++20 [[unlikely]] */
+ return getForceTorques(qResult.memory, description.name);
+ }
+ catch (...)
{
- ARMARX_VERBOSE << qResult.errorMessage;
+ ARMARX_VERBOSE << deactivateSpam(1) << "Failed to query force torques. Reason: "
+ << GetHandledExceptionString();
return std::nullopt;
}
-
- return getForceTorques(qResult.memory, description.name);
}
std::optional<std::map<RobotReader::Hand, robot::ToFArray>>
@@ -566,17 +608,27 @@ namespace armarx::armem::robot_state
.snapshots().beforeOrAtTime(timestamp);
// clang-format on
- const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
+ try
+ {
+ std::scoped_lock l(memoryReaderMutex);
+ const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
- ARMARX_DEBUG << "Lookup result in reader: " << qResult;
+ ARMARX_DEBUG << "Lookup result in reader: " << qResult;
- if (not qResult.success) /* c++20 [[unlikely]] */
+ if (not qResult.success) /* c++20 [[unlikely]] */
+ {
+ ARMARX_VERBOSE << qResult.errorMessage;
+ return std::nullopt;
+ }
+
+ return getToF(qResult.memory, description.name);
+ }
+ catch (...)
{
- ARMARX_VERBOSE << qResult.errorMessage;
+ ARMARX_VERBOSE << deactivateSpam(1)
+ << "Failed to query ToF. Reason: " << GetHandledExceptionString();
return std::nullopt;
}
-
- return getToF(qResult.memory, description.name);
}
std::optional<robot::PlatformState>
@@ -652,7 +704,6 @@ namespace armarx::armem::robot_state
tryCast<::armarx::armem::arondto::Proprioception>(entityInstance);
ARMARX_CHECK(proprioception.has_value());
-
for (const auto& [handName, dtoFt] : proprioception->forceTorque)
{
robot::ForceTorque forceTorque;
@@ -809,6 +860,7 @@ namespace armarx::armem::robot_state
try
{
+ std::scoped_lock l(memoryReaderMutex);
const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
ARMARX_DEBUG << "Lookup result in reader: " << qResult;
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 dbf81e69a9e4cbc8e488df482634d3542487376c..8397ea9a8d8b8de46a7c2b1ec1167396d8383d06 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h
@@ -163,7 +163,7 @@ namespace armarx::armem::robot_state
const std::string propertyPrefix = "mem.robot_state.";
armem::client::Reader memoryReader;
- mutable std::mutex memoryWriterMutex;
+ mutable std::mutex memoryReaderMutex;
client::robot_state::localization::TransformReader transformReader;
};
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.cpp b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.cpp
index 51739021f56c5b87a47b930fa39cd51a8358607d..5a221a342be4e0803c1cb28e708352b10b7e98f6 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.cpp
@@ -59,6 +59,12 @@
namespace armarx::armem::client::robot_state::localization
{
+ TransformReader::TransformReader(const TransformReader& t) :
+ memoryReader(t.memoryReader), properties(t.properties), propertyPrefix(t.propertyPrefix)
+ {
+
+ }
+
TransformReader::~TransformReader() = default;
void
@@ -147,25 +153,41 @@ namespace armarx::armem::client::robot_state::localization
.snapshots().beforeOrAtTime(query.header.timestamp);
// clang-format on
- const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
- ARMARX_DEBUG << "Lookup result in reader: " << qResult;
-
- if (not qResult.success)
+ try
{
- return {.transform =
- {
- .header = query.header,
- },
- .status = TransformResult::Status::ErrorFrameNotAvailable,
- .errorMessage = "Error in tf lookup '" + query.header.parentFrame + " -> " +
- query.header.frame + "' : " + qResult.errorMessage};
+ std::scoped_lock l(memoryReaderMutex);
+ const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
+ ARMARX_DEBUG << "Lookup result in reader: " << qResult;
+
+ if (not qResult.success)
+ {
+ return {.transform =
+ {
+ .header = query.header,
+ },
+ .status = TransformResult::Status::ErrorFrameNotAvailable,
+ .errorMessage = "Error in tf lookup '" + query.header.parentFrame + " -> " +
+ query.header.frame + "' : " + qResult.errorMessage};
+ }
+
+ const auto& localizationCoreSegment =
+ qResult.memory.getCoreSegment(properties.localizationSegment);
+
+ using armarx::armem::common::robot_state::localization::TransformHelper;
+ return TransformHelper::lookupTransform(localizationCoreSegment, query);
+ }
+ catch (...)
+ {
+ ARMARX_VERBOSE << "Lookup Transform failure" << GetHandledExceptionString();
+ return TransformResult{.transform =
+ {
+ .header = query.header,
+ },
+ .status = TransformResult::Status::Error,
+ .errorMessage = "Error in tf lookup '" +
+ query.header.parentFrame + " -> " +
+ query.header.frame + "' : Memory exception."};
}
-
- const auto& localizationCoreSegment =
- qResult.memory.getCoreSegment(properties.localizationSegment);
-
- using armarx::armem::common::robot_state::localization::TransformHelper;
- return TransformHelper::lookupTransform(localizationCoreSegment, query);
}
} // namespace armarx::armem::client::robot_state::localization
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.h b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.h
index 4d9ee5983e783e41b7e24cbce982ca65e8ed7057..32b3c2fc12eb46c9c67baa362d92f19552ada51d 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.h
+++ b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.h
@@ -44,7 +44,7 @@ namespace armarx::armem::client::robot_state::localization
{
public:
TransformReader() = default;
- TransformReader(const TransformReader&) = default;
+ TransformReader(const TransformReader&);
~TransformReader() override;
@@ -60,6 +60,7 @@ namespace armarx::armem::client::robot_state::localization
private:
armem::client::Reader memoryReader;
+ mutable std::mutex memoryReaderMutex;
// Properties
struct Properties
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.cpp b/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.cpp
index be19db6355d9c21e0b9a7e287e5889024a0c408c..c5f3fe36a067f6969e2c09b238ac1ed473422c2b 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.cpp
@@ -9,7 +9,6 @@
#include <SimoxUtility/algorithm/get_map_keys_values.h>
#include <SimoxUtility/math/pose.h>
#include <SimoxUtility/math/rescale.h>
-
#include <VirtualRobot/XML/RobotIO.h>
#include <ArmarXCore/core/logging/Logging.h>
@@ -17,10 +16,8 @@
#include <ArmarXCore/core/time/TimeUtil.h>
#include <ArmarXCore/interface/core/PackagePath.h>
-#include <RobotAPI/libraries/armem/core/Time.h>
-
#include <RobotAPI/components/ArViz/Client/Elements.h>
-
+#include <RobotAPI/libraries/armem/core/Time.h>
#include <RobotAPI/libraries/armem_robot_state/server/description/Segment.h>
#include <RobotAPI/libraries/armem_robot_state/server/localization/Segment.h>
#include <RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.h>
@@ -28,40 +25,43 @@
#include "combine.h"
-
namespace armarx::armem::server::robot_state
{
- Visu::Visu(
- const description::Segment& descriptionSegment,
- const proprioception::Segment& proprioceptionSegment,
- const localization::Segment& localizationSegment)
- : descriptionSegment(descriptionSegment),
- proprioceptionSegment(proprioceptionSegment),
- localizationSegment(localizationSegment)
+ Visu::Visu(const description::Segment& descriptionSegment,
+ const proprioception::Segment& proprioceptionSegment,
+ const localization::Segment& localizationSegment) :
+ descriptionSegment(descriptionSegment),
+ proprioceptionSegment(proprioceptionSegment),
+ localizationSegment(localizationSegment)
{
Logging::setTag("Visu");
}
-
- void Visu::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
+ void
+ Visu::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
{
- defs->optional(p.enabled, prefix + "enabled", "Enable or disable visualization of objects.");
+ defs->optional(
+ p.enabled, prefix + "enabled", "Enable or disable visualization of objects.");
defs->optional(p.frequencyHz, prefix + "frequenzyHz", "Frequency of visualization.");
- defs->optional(p.framesEnabled, prefix + "framesEnabled", "Enable or disable visualization of frames.");
- defs->optional(p.forceTorque.enabled, prefix + "forceTorque.enabled",
+ defs->optional(p.framesEnabled,
+ prefix + "framesEnabled",
+ "Enable or disable visualization of frames.");
+ defs->optional(p.forceTorque.enabled,
+ prefix + "forceTorque.enabled",
"Enable or disable visualization of force torque sensors.");
- defs->optional(p.forceTorque.forceScale, prefix + "forceTorque.forceScale",
+ defs->optional(p.forceTorque.forceScale,
+ prefix + "forceTorque.forceScale",
"Scaling of force arrows.");
}
-
- void Visu::init()
+ void
+ Visu::init()
{
}
-
- void Visu::connect(const viz::Client& arviz, DebugObserverInterfacePrx debugObserver)
+ void
+ Visu::connect(const viz::Client& arviz, DebugObserverInterfacePrx debugObserver)
{
this->arviz = arviz;
if (debugObserver)
@@ -76,15 +76,12 @@ namespace armarx::armem::server::robot_state
updateTask->join();
updateTask = nullptr;
}
- updateTask = new SimpleRunningTask<>([this]()
- {
- this->visualizeRun();
- });
+ updateTask = new SimpleRunningTask<>([this]() { this->visualizeRun(); });
updateTask->start();
}
-
- void Visu::visualizeRobots(viz::Layer& layer, const robot::Robots& robots)
+ void
+ Visu::visualizeRobots(viz::Layer& layer, const robot::Robots& robots)
{
for (const robot::Robot& robot : robots)
{
@@ -103,8 +100,8 @@ namespace armarx::armem::server::robot_state
}
}
-
- void Visu::visualizeFrames(
+ void
+ Visu::visualizeFrames(
viz::Layer& layerFrames,
const std::unordered_map<std::string, std::vector<Eigen::Affine3f>>& frames)
{
@@ -120,12 +117,14 @@ namespace armarx::armem::server::robot_state
pose = to;
- layerFrames.add(viz::Arrow(robotName + std::to_string(++i)).fromTo(from.translation(), to.translation()));
+ layerFrames.add(viz::Arrow(robotName + std::to_string(++i))
+ .fromTo(from.translation(), to.translation()));
}
}
}
- void Visu::visualizeFramesIndividual(
+ void
+ Visu::visualizeFramesIndividual(
viz::Layer& layerFrames,
const std::unordered_map<std::string, std::vector<Eigen::Affine3f>>& frames)
{
@@ -137,14 +136,14 @@ namespace armarx::armem::server::robot_state
int i = 0;
for (const auto& frame : robotFrames)
{
- layerFrames.add(viz::Pose(robotName + FRAME_NAMES.at(i++)).pose(frame.matrix()).scale(3));
+ layerFrames.add(
+ viz::Pose(robotName + FRAME_NAMES.at(i++)).pose(frame.matrix()).scale(3));
}
}
}
-
-
- void Visu::visualizeRun()
+ void
+ Visu::visualizeRun()
{
CycleUtil cycle(static_cast<int>(1000 / p.frequencyHz));
while (updateTask and not updateTask->isStopped())
@@ -158,6 +157,11 @@ namespace armarx::armem::server::robot_state
{
visualizeOnce(timestamp);
}
+ catch (const std::out_of_range& e)
+ {
+ ARMARX_WARNING << "Caught std::out_of_range while visualizing robots: \n"
+ << e.what();
+ }
catch (const std::exception& e)
{
ARMARX_WARNING << "Caught exception while visualizing robots: \n" << e.what();
@@ -176,8 +180,8 @@ namespace armarx::armem::server::robot_state
}
}
-
- void Visu::visualizeOnce(const Time& timestamp)
+ void
+ Visu::visualizeOnce(const Time& timestamp)
{
TIMING_START(tVisuTotal);
@@ -200,9 +204,9 @@ namespace armarx::armem::server::robot_state
TIMING_END_STREAM(tRobotFramePoses, ARMARX_DEBUG);
TIMING_START(tSensorValues);
- const auto sensorValues =
- proprioceptionSegment.getSensorValuesLocking(
- timestamp, debugObserver ? &*debugObserver : nullptr);
+ const proprioception::SensorValuesMap sensorValues =
+ proprioceptionSegment.getSensorValuesLocking(timestamp,
+ debugObserver ? &*debugObserver : nullptr);
TIMING_END_STREAM(tSensorValues, ARMARX_DEBUG);
TIMING_END_STREAM(tVisuGetData, ARMARX_DEBUG);
@@ -225,83 +229,51 @@ namespace armarx::armem::server::robot_state
combine(robotDescriptions, globalPoses, sensorValues, timestamp);
ARMARX_DEBUG << "Visualize " << robots.size() << " robots ...";
- viz::Layer layer = arviz.layer("Robots");
- visualizeRobots(layer, robots);
- std::vector layers{layer};
+ std::vector<viz::Layer> layers;
+
+ {
+ viz::Layer& layer = layers.emplace_back(arviz.layer("Robots"));
+ visualizeRobots(layer, robots);
+ }
ARMARX_DEBUG << "Visualize frames ...";
- if(p.framesEnabled)
+ if (p.framesEnabled)
{
- viz::Layer layerFrames = arviz.layer("Frames");
- visualizeFrames(layerFrames, frames);
- layers.push_back(layerFrames);
+ viz::Layer& layer = layers.emplace_back(arviz.layer("Localization Frames"));
+ visualizeFrames(layer, frames);
}
TIMING_END_STREAM(tVisuBuildLayers, ARMARX_DEBUG);
{
- viz::Layer layerFrames = arviz.layer("FramesIndividual");
- visualizeFramesIndividual(layerFrames, frames);
- layers.push_back(layerFrames);
+ viz::Layer& layer = layers.emplace_back(arviz.layer("Localization Frames Individual"));
+ visualizeFramesIndividual(layer, frames);
}
if (p.forceTorque.enabled)
{
- viz::Layer layerFrames = arviz.layer("ForceTorque");
+ viz::Layer& layerForceTorque = layers.emplace_back(arviz.layer("Force Torque"));
+ std::stringstream log;
+
for (const robot::Robot& robot : robots)
{
- const std::string& name = robot.description.name;
- if (robotNameHelper.find(name) == robotNameHelper.end())
- {
- const std::filesystem::path robotPath = robot.description.xml.toSystemPath();
- robotNameHelper[name] = RobotNameHelper::Create(robotPath);
- robotModels[name]
- = VirtualRobot::RobotIO::loadRobot(robotPath, VirtualRobot::RobotIO::eStructure);
- }
-
- auto model = robotModels.at(name);
- model->setJointValues(robot.config.jointMap);
- model->setGlobalPose(robot.config.globalPose.matrix());
+ const std::string& robotName = robot.description.name;
- const proprioception::ForceTorqueValuesMap& forceTorques
- = sensorValues.at(name).forceTorqueValuesMap;
-
- for (const auto& [side, ft] : forceTorques)
+ auto itSensorValues = sensorValues.find(robotName);
+ if (itSensorValues == sensorValues.end())
{
- ARMARX_CHECK(side == RobotNameHelper::LocationLeft or side == RobotNameHelper::LocationRight) << side;
-
- const std::string forceTorqueSensorName =
- robotNameHelper.at(name)->getArm(side).getForceTorqueSensor();
-
- const Eigen::Matrix4f forceTorqueSensorPose
- = model->getSensor(forceTorqueSensorName)->getGlobalPose();
-
- const std::string xyz = "XYZ";
-
- const Eigen::Vector3f from = simox::math::position(forceTorqueSensorPose);
- for (int i = 0; i < 3; ++i)
- {
- simox::Color color = simox::Color((255 * Eigen::Matrix3i::Identity().col(i)).eval());
- color.a = 128;
-
- // Force arrows.
- const float length = p.forceTorque.forceScale * ft.force(i);
- const float width = std::min(
- simox::math::rescale(std::abs(length), 0.F, 100.F, 1.F, 10.F),
- 10.F);
-
- const Eigen::Vector3f to = from + length * simox::math::orientation(forceTorqueSensorPose).col(i);
-
- std::stringstream key;
- key << side << " Force " << xyz.at(i);
- layerFrames.add(viz::Arrow(key.str()).fromTo(from, to).color(color).width(width));
-
- // Torque circle arrows.
- }
+ log << "\nNo robot '" << robotName << "' in sensor values. Available are: ";
+ log << simox::alg::join(simox::alg::get_keys(sensorValues), ", ");
+ continue;
}
+ visualizeForceTorque(layerForceTorque, robot, itSensorValues->second);
+ }
+
+ if (not log.str().empty())
+ {
+ ARMARX_INFO << "While visualizing robot force torques: " << log.str();
}
- layers.push_back(layerFrames);
}
@@ -317,15 +289,99 @@ namespace armarx::armem::server::robot_state
if (debugObserver.has_value())
{
const std::string p = "Visu | ";
- debugObserver->setDebugObserverDatafield(p + "t Total (ms)", tVisuTotal.toMilliSecondsDouble());
- debugObserver->setDebugObserverDatafield(p + "t 1 Get Data (ms)", tVisuGetData.toMilliSecondsDouble());
- debugObserver->setDebugObserverDatafield(p + "t 1.1 Descriptions (ms)", tRobotDescriptions.toMilliSecondsDouble());
- debugObserver->setDebugObserverDatafield(p + "t 1.2 Global Poses (ms)", tGlobalPoses.toMilliSecondsDouble());
- debugObserver->setDebugObserverDatafield(p + "t 1.3 Frames (ms)", tRobotFramePoses.toMilliSecondsDouble());
- debugObserver->setDebugObserverDatafield(p + "t 1.4 Sensor Values (ms)", tSensorValues.toMilliSecondsDouble());
- debugObserver->setDebugObserverDatafield(p + "t 2 Build Layers (ms)", tVisuBuildLayers.toMilliSecondsDouble());
- debugObserver->setDebugObserverDatafield(p + "t 3 Commit (ms)", tVisuCommit.toMilliSecondsDouble());
+ debugObserver->setDebugObserverDatafield(p + "t Total (ms)",
+ tVisuTotal.toMilliSecondsDouble());
+ debugObserver->setDebugObserverDatafield(p + "t 1 Get Data (ms)",
+ tVisuGetData.toMilliSecondsDouble());
+ debugObserver->setDebugObserverDatafield(p + "t 1.1 Descriptions (ms)",
+ tRobotDescriptions.toMilliSecondsDouble());
+ debugObserver->setDebugObserverDatafield(p + "t 1.2 Global Poses (ms)",
+ tGlobalPoses.toMilliSecondsDouble());
+ debugObserver->setDebugObserverDatafield(p + "t 1.3 Frames (ms)",
+ tRobotFramePoses.toMilliSecondsDouble());
+ debugObserver->setDebugObserverDatafield(p + "t 1.4 Sensor Values (ms)",
+ tSensorValues.toMilliSecondsDouble());
+ debugObserver->setDebugObserverDatafield(p + "t 2 Build Layers (ms)",
+ tVisuBuildLayers.toMilliSecondsDouble());
+ debugObserver->setDebugObserverDatafield(p + "t 3 Commit (ms)",
+ tVisuCommit.toMilliSecondsDouble());
+ }
+ }
+
+ void
+ Visu::visualizeForceTorque(viz::Layer& layer,
+ const robot::Robot& robot,
+ const proprioception::SensorValues& sensorValues)
+ {
+ const std::string& robotName = robot.description.name;
+
+ RobotModel* robotModel = nullptr;
+ if (auto it = models.find(robotName); it != models.end())
+ {
+ robotModel = &it->second;
+ }
+ else
+ {
+ const std::filesystem::path robotPath = robot.description.xml.toSystemPath();
+ ARMARX_INFO << "Loading robot model for '" << robotName << "' from " << robotPath
+ << " for visualization.";
+ auto [it2, _] = models.emplace(robotName, robotPath);
+ robotModel = &it2->second;
}
+
+ robotModel->model->setJointValues(robot.config.jointMap);
+ robotModel->model->setGlobalPose(robot.config.globalPose.matrix());
+
+ const proprioception::ForceTorqueValuesMap& forceTorques =
+ sensorValues.forceTorqueValuesMap;
+
+ for (const auto& [side, ft] : forceTorques)
+ {
+ ARMARX_CHECK(side == RobotNameHelper::LocationLeft or
+ side == RobotNameHelper::LocationRight)
+ << side;
+
+ const std::string forceTorqueSensorName =
+ robotModel->nameHelper->getArm(side).getForceTorqueSensor();
+
+ const Eigen::Matrix4f forceTorqueSensorPose =
+ robotModel->model->getSensor(forceTorqueSensorName)->getGlobalPose();
+
+ const std::string xyz = "XYZ";
+
+ const Eigen::Vector3f from = simox::math::position(forceTorqueSensorPose);
+ for (int i = 0; i < 3; ++i)
+ {
+ simox::Color color =
+ simox::Color((255 * Eigen::Matrix3i::Identity().col(i)).eval());
+ color.a = 128;
+
+ // Force arrows.
+ const float length = p.forceTorque.forceScale * ft.force(i);
+ const float width =
+ std::min(simox::math::rescale(std::abs(length), 0.F, 100.F, 1.F, 10.F), 10.F);
+
+ const Eigen::Vector3f to =
+ from + length * simox::math::orientation(forceTorqueSensorPose).col(i);
+
+ std::stringstream key;
+ ARMARX_CHECK_FITS_SIZE(i, xyz.size());
+ key << side << " Force " << xyz.at(i);
+ layer.add(viz::Arrow(key.str()).fromTo(from, to).color(color).width(width));
+
+ // TODO: Torque circle arrows.
+ }
+ }
+ }
+
+ Visu::RobotModel::RobotModel(const std::filesystem::path& robotPath) :
+ model{
+ VirtualRobot::RobotIO::loadRobot(robotPath, VirtualRobot::RobotIO::eStructure),
+ },
+ nameHelper{
+ RobotNameHelper::Create(robotPath),
+ }
+ {
}
} // namespace armarx::armem::server::robot_state
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.h b/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.h
index 265f08e2a0c10153c1b3cc8c624fd214713d57a9..2afa9a27f3396939c625050a942925db79129d64 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.h
+++ b/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.h
@@ -28,14 +28,10 @@
#include <ArmarXCore/libraries/DebugObserverHelper/DebugObserverHelper.h>
#include <RobotAPI/components/ArViz/Client/Client.h>
-
+#include <RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.h>
#include <RobotAPI/libraries/armem_objects/types.h>
-
#include <RobotAPI/libraries/armem_robot_state/server/forward_declarations.h>
-#include <RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.h>
-
-
namespace armarx::armem::server::robot_state
{
@@ -46,41 +42,38 @@ namespace armarx::armem::server::robot_state
class Visu : public armarx::Logging
{
public:
-
Visu(const description::Segment& descriptionSegment,
const proprioception::Segment& proprioceptionSegment,
const localization::Segment& localizationSegment);
- void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "visu.");
+ void defineProperties(armarx::PropertyDefinitionsPtr defs,
+ const std::string& prefix = "visu.");
void init();
void connect(const viz::Client& arviz, DebugObserverInterfacePrx debugObserver = nullptr);
private:
-
void visualizeRun();
void visualizeOnce(const Time& timestamp);
- static
- void visualizeRobots(
- viz::Layer& layer,
- const robot::Robots& robots);
+ static void visualizeRobots(viz::Layer& layer, const robot::Robots& robots);
- static
- void visualizeFrames(
+ static void visualizeFrames(
viz::Layer& layerFrames,
const std::unordered_map<std::string, std::vector<Eigen::Affine3f>>& frames);
- static
- void visualizeFramesIndividual(
+ static void visualizeFramesIndividual(
viz::Layer& layerFrames,
const std::unordered_map<std::string, std::vector<Eigen::Affine3f>>& frames);
- private:
+ void visualizeForceTorque(viz::Layer& layer,
+ const robot::Robot& robot,
+ const proprioception::SensorValues& sensorValues);
+ private:
viz::Client arviz;
std::optional<DebugObserverHelper> debugObserver;
@@ -88,9 +81,15 @@ namespace armarx::armem::server::robot_state
const proprioception::Segment& proprioceptionSegment;
const localization::Segment& localizationSegment;
- std::map<std::string, RobotNameHelperPtr> robotNameHelper;
- std::map<std::string, VirtualRobot::RobotPtr> robotModels;
+ struct RobotModel
+ {
+ RobotModel(const std::filesystem::path& robotPath);
+
+ VirtualRobot::RobotPtr model;
+ RobotNameHelperPtr nameHelper;
+ };
+ std::map<std::string, RobotModel> models;
struct Properties
{
@@ -98,18 +97,17 @@ namespace armarx::armem::server::robot_state
float frequencyHz = 25.f;
bool framesEnabled = false;
-
+
struct ForceTorque
{
bool enabled = true;
float forceScale = 1.F;
};
+
ForceTorque forceTorque;
} p;
-
SimpleRunningTask<>::pointer_type updateTask;
-
};
-} // namespace armarx::armem::server::robot_state
+} // namespace armarx::armem::server::robot_state
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.cpp b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.cpp
index e1469a80d35feecd9cb12914a62b944cabb4ba81..5c4580c4c8c5d4540b0867365c657807d0502a7d 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.cpp
@@ -40,7 +40,6 @@
#include <RobotAPI/libraries/aron/core/data/variant/All.h>
#include <RobotAPI/libraries/core/FramedPose.h>
-
namespace armarx::armem::server::robot_state::proprioception
{
@@ -52,7 +51,6 @@ namespace armarx::armem::server::robot_state::proprioception
DebugObserverHelper(Logging::tag.tagName, debugObserverPlugin.getDebugObserver(), true);
}
-
static float
toDurationMs(std::chrono::high_resolution_clock::time_point start,
std::chrono::high_resolution_clock::time_point end)
@@ -61,7 +59,6 @@ namespace armarx::armem::server::robot_state::proprioception
return duration.count() / 1000.f;
}
-
void
RobotStateWriter::run(float pollFrequency,
Queue& dataBuffer,
@@ -70,10 +67,14 @@ namespace armarx::armem::server::robot_state::proprioception
{
while (isRunning())
{
- // if (debugObserver)
- // {
- // debugObserver->setDebugObserverDatafield("RobotStateWriter | Queue Size", dataBuffer.size());
- // }
+ if (debugObserver)
+ {
+ // This locks the queue, but I did not find an interface to lock the queue,
+ // get the size and wait_pull().
+ size_t dataBufferSize = dataBuffer.size();
+ debugObserver->setDebugObserverDatafield("RobotStateWriter | Queue Size",
+ static_cast<long>(dataBufferSize));
+ }
RobotUnitData robotUnitData;
if (const auto status = dataBuffer.wait_pull(robotUnitData);
@@ -88,10 +89,12 @@ namespace armarx::armem::server::robot_state::proprioception
// Commits lock the core segments.
// Proprioception + Exteroception
- armem::CommitResult resultProprioception = memory.commitLocking(update.proprioception);
+ armem::CommitResult resultProprioception =
+ memory.commitLocking(update.proprioception);
ARMARX_DEBUG << deactivateSpam(1) << VAROUT(update.exteroception.updates.size());
- armem::CommitResult resultExteroception = memory.commitLocking(update.exteroception);
+ armem::CommitResult resultExteroception =
+ memory.commitLocking(update.exteroception);
endProprioception = std::chrono::high_resolution_clock::now();
// Localization
@@ -105,13 +108,15 @@ namespace armarx::armem::server::robot_state::proprioception
if (not resultProprioception.allSuccess())
{
- ARMARX_WARNING << "Could not commit data to proprioception segment in memory. Error message: "
+ ARMARX_WARNING << "Could not commit data to proprioception segment in memory. "
+ "Error message: "
<< resultProprioception.allErrorMessages();
}
if (not resultExteroception.allSuccess())
{
- ARMARX_WARNING << "Could not commit data to exteroception segment in memory. Error message: "
+ ARMARX_WARNING << "Could not commit data to exteroception segment in memory. "
+ "Error message: "
<< resultExteroception.allErrorMessages();
}
@@ -158,21 +163,26 @@ namespace armarx::armem::server::robot_state::proprioception
// Send batch to memory
Update update;
- if(data.proprioception){
+ if (data.proprioception)
+ {
armem::EntityUpdate& up = update.proprioception.add();
up.entityID = properties.robotUnitProviderID.withEntityName(
properties.robotUnitProviderID.providerSegmentName);
- up.entityID.coreSegmentName =::armarx::armem::robot_state::constants::proprioceptionCoreSegment;
+ up.entityID.coreSegmentName =
+ ::armarx::armem::robot_state::constants::proprioceptionCoreSegment;
up.referencedTime = data.timestamp;
+ up.arrivedTime = data.timestampArrived;
up.instancesData = {data.proprioception};
}
// Exteroception
- if(data.exteroception){
+ if (data.exteroception)
+ {
armem::EntityUpdate& up = update.exteroception.add();
up.entityID = properties.robotUnitProviderID.withEntityName(
properties.robotUnitProviderID.providerSegmentName);
- up.entityID.coreSegmentName = ::armarx::armem::robot_state::constants::exteroceptionCoreSegment;
+ up.entityID.coreSegmentName =
+ ::armarx::armem::robot_state::constants::exteroceptionCoreSegment;
up.referencedTime = data.timestamp;
up.instancesData = {data.exteroception};
}
@@ -200,7 +210,6 @@ namespace armarx::armem::server::robot_state::proprioception
return update;
}
-
armem::robot_state::Transform
RobotStateWriter::getTransform(const aron::data::DictPtr& platformData,
const Time& timestamp) const
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitData.h b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitData.h
index 8d904f249029cf9fd5ce010657cb4324f3ba5551..2cd2307421175d4e1feed29be5096be99d56d355 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitData.h
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitData.h
@@ -13,6 +13,7 @@ namespace armarx::armem::server::robot_state::proprioception
struct RobotUnitData
{
Time timestamp;
+ Time timestampArrived;
aron::data::DictPtr proprioception;
aron::data::DictPtr exteroception;
};
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitReader.cpp b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitReader.cpp
index 34260f51e36951f7b9a0a68369cf8317e8a4dedc..ec7a56751f1ca8393f218941a27e4041190d1c96 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitReader.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitReader.cpp
@@ -56,10 +56,11 @@ namespace armarx::armem::server::robot_state::proprioception
if (std::optional<RobotUnitData> commit = fetchAndConvertLatestRobotUnitData())
{
- // will lock a mutex
- debugObserver->setDebugObserverDatafield("RobotUnitReader | t commitTimestamp [us]",
- commit->timestamp.toMicroSecondsSinceEpoch());
+ debugObserver->setDebugObserverDatafield(
+ "RobotUnitReader | t commitTimestamp [us]",
+ commit->timestamp.toMicroSecondsSinceEpoch());
+ // will lock a mutex
dataBuffer.push(std::move(commit.value()));
}
auto duration = std::chrono::duration_cast<std::chrono::microseconds>(
@@ -80,11 +81,14 @@ namespace armarx::armem::server::robot_state::proprioception
{
ARMARX_CHECK_NOT_NULL(converterProprioception);
+ RobotUnitData result;
std::optional<RobotUnitDataStreaming::TimeStep> data;
{
auto start = std::chrono::high_resolution_clock::now();
data = fetchLatestData();
+ result.timestampArrived = armarx::DateTime::Now();
+
auto duration = std::chrono::duration_cast<std::chrono::microseconds>(
std::chrono::high_resolution_clock::now() - start);
if (debugObserver)
@@ -101,7 +105,6 @@ namespace armarx::armem::server::robot_state::proprioception
ARMARX_DEBUG << "RobotUnitReader: Converting data current timestep to commit";
auto start = std::chrono::high_resolution_clock::now();
- RobotUnitData result;
if (converterProprioception != nullptr)
{
@@ -143,6 +146,9 @@ namespace armarx::armem::server::robot_state::proprioception
debugObserver->setDebugObserverDatafield(
"RobotUnitReader | RT Timestamp Since Last Iteration",
data.back().timesSinceLastIterationUSec);
+ debugObserver->setDebugObserverDatafield(
+ "RobotUnitReader | Timestamp Arrived in RSM",
+ armarx::DateTime::Now().toMicroSecondsSinceEpoch());
}
}
if (data.empty())
diff --git a/source/RobotAPI/libraries/aron/converter/ivt/IVTConverter.cpp b/source/RobotAPI/libraries/aron/converter/ivt/IVTConverter.cpp
index 715fe6526fc51e7078c97b769c85ec61bd437907..7d177ee2bfe68be685c514eaecbfe38dcf7b3dd4 100644
--- a/source/RobotAPI/libraries/aron/converter/ivt/IVTConverter.cpp
+++ b/source/RobotAPI/libraries/aron/converter/ivt/IVTConverter.cpp
@@ -39,13 +39,17 @@ namespace armarx::aron::data::converter
throw error::AronException(
__PRETTY_FUNCTION__, "The size of an NDArray does not match.", nav->getPath());
}
+
auto dims = nav->getShape();
auto ret = std::make_shared<CByteImage>();
+
ret->Set(dims[0], dims[1], static_cast<CByteImage::ImageType>(std::stoi(nav->getType())));
+
memcpy(reinterpret_cast<unsigned char*>(ret->pixels),
nav->getData(),
std::accumulate(std::begin(dims), std::end(dims), 1, std::multiplies<int>()));
+
return ret;
}
diff --git a/source/RobotAPI/libraries/robot_name_service/client/Plugin.cpp b/source/RobotAPI/libraries/robot_name_service/client/Plugin.cpp
index 89f973a6e3f4588096a977091cdcbbeb4742bc9a..0f944891c8bfa557ba7eeb72c6e4ca15b732466f 100644
--- a/source/RobotAPI/libraries/robot_name_service/client/Plugin.cpp
+++ b/source/RobotAPI/libraries/robot_name_service/client/Plugin.cpp
@@ -19,6 +19,8 @@ namespace armarx::plugins
{
using namespace armarx::robot_name_service::client::constants;
+ ARMARX_INFO << "Got as config: " << properties.configJson;
+
nlohmann::json config = nlohmann::json::parse(properties.configJson);
if (config.contains(CONFIG_ROBOT_NAME))
@@ -53,7 +55,7 @@ namespace armarx::plugins
{
for (const auto& [k, config_arm] : config[CONFIG_ARMS].items())
{
- auto& arm = robot.arms[k];
+ auto& arm = robot.armInfos[k];
if (config_arm.contains(CONFIG_ARM_KINEMATIC_CHAIN_NAME))
{
@@ -109,7 +111,7 @@ namespace armarx::plugins
{
for (const auto& [k, config_arm] : config[CONFIG_ARMS].items())
{
- auto& arm = robot.arms[k];
+ auto& arm = robot.armInfos[k];
if (config_arm.contains(CONFIG_ARM_HAND))
{
@@ -126,7 +128,7 @@ namespace armarx::plugins
}
// finally commit robot
- properties.rns->registerRobot(robot.toIce());
+ properties.rns->registerRobotInfo(robot.toIce());
}
void
@@ -139,7 +141,7 @@ namespace armarx::plugins
void
RobotNameServiceComponentPlugin::preOnDisconnectComponent()
{
- properties.rns->unregisterRobot(robot.name);
+ properties.rns->unregisterRobotInfo(robot.name);
}
} // namespace armarx::plugins
diff --git a/source/RobotAPI/libraries/robot_name_service/client/Plugin.h b/source/RobotAPI/libraries/robot_name_service/client/Plugin.h
index 813003f98554196e32fa9cc2c27eb650cea5f914..5d59cd16f3755ab02d566b4bef1a5955cfe2be92 100644
--- a/source/RobotAPI/libraries/robot_name_service/client/Plugin.h
+++ b/source/RobotAPI/libraries/robot_name_service/client/Plugin.h
@@ -60,7 +60,7 @@ namespace armarx::plugins
armarx::robot_name_service::dti::RobotNameServiceInterfacePrx rns;
} properties;
- armarx::robot_name_service::core::Robot robot;
+ armarx::robot_name_service::core::RobotInfo robot;
friend class armarx::RobotNameServiceComponentPluginUser;
};
diff --git a/source/RobotAPI/libraries/robot_name_service/core/CMakeLists.txt b/source/RobotAPI/libraries/robot_name_service/core/CMakeLists.txt
index 6c702dd1e210e92e3e14c4d02a4e6d870a731c44..19ac42cfedb1959c36c25d6961ba589ebd91ad31 100644
--- a/source/RobotAPI/libraries/robot_name_service/core/CMakeLists.txt
+++ b/source/RobotAPI/libraries/robot_name_service/core/CMakeLists.txt
@@ -17,8 +17,18 @@ armarx_add_library(
SOURCES
Robot.cpp
+ aron_conversions.cpp
HEADERS
Robot.h
+ aron_conversions.h
+)
+
+armarx_enable_aron_file_generation_for_target(
+ TARGET_NAME
+ "${LIB_NAME}"
+ ARON_FILES
+ aron/Robot.xml
)
add_library(RobotAPI::robot_name_service_core ALIAS robot_name_service_core)
+
diff --git a/source/RobotAPI/libraries/robot_name_service/core/Robot.cpp b/source/RobotAPI/libraries/robot_name_service/core/Robot.cpp
index bfa5fbfe5d529388f9b545d93adae68d5f853f57..11978184a40e334b4b3b29099debf83976df04e9 100644
--- a/source/RobotAPI/libraries/robot_name_service/core/Robot.cpp
+++ b/source/RobotAPI/libraries/robot_name_service/core/Robot.cpp
@@ -4,17 +4,17 @@ namespace armarx::robot_name_service::core
{
void
- Hand::fromIce(const dto::Hand& r)
+ TCPInfo::fromIce(const dto::TCPInfo& r)
{
name = r.name;
ft_name = r.ft_name;
handUnitInterface = r.handUnitInterface;
}
- dto::Hand
- Hand::toIce() const
+ dto::TCPInfo
+ TCPInfo::toIce() const
{
- dto::Hand r;
+ dto::TCPInfo r;
r.name = name;
r.ft_name = ft_name;
r.handUnitInterface = handUnitInterface;
@@ -22,57 +22,53 @@ namespace armarx::robot_name_service::core
}
void
- Arm::fromIce(const dto::Arm& r)
+ ArmInfo::fromIce(const dto::ArmInfo& r)
{
kinematicChainName = r.kinematicChainName;
- hand.fromIce(r.hand);
+ hand.fromIce(r.tcpInfo);
}
- dto::Arm
- Arm::toIce() const
+ dto::ArmInfo
+ ArmInfo::toIce() const
{
- dto::Arm r;
+ dto::ArmInfo r;
r.kinematicChainName = kinematicChainName;
- r.hand = hand.toIce();
+ r.tcpInfo = hand.toIce();
return r;
}
void
- Robot::fromIce(const dto::Robot& r)
+ RobotInfo::fromIce(const dto::RobotInfo& r)
{
name = r.name;
xmlPackagePath = r.xmlPackagePath;
memoryNameSystem = armarx::armem::client::MemoryNameSystem(r.memoryNameSystem);
skillManager = r.skillManager;
- arms.clear();
- for (const auto& [k, a] : r.arms)
+ armInfos.clear();
+ for (const auto& [k, a] : r.armInfos)
{
- arms[k].fromIce(a);
+ armInfos[k].fromIce(a);
}
- kinematicUnitInterface = r.kinematicUnitInterface;
- platformUnitInterface = r.platformUnitInterface;
- localizationUnitInterface = r.localizationUnitInterface;
+ robotUnit = r.robotUnit;
}
- dto::Robot
- Robot::toIce() const
+ dto::RobotInfo
+ RobotInfo::toIce() const
{
- dto::Robot r;
+ dto::RobotInfo r;
r.name = name;
r.xmlPackagePath = xmlPackagePath;
r.memoryNameSystem = memoryNameSystem.getMemoryNameSystem();
r.skillManager = skillManager;
- for (const auto& [k, a] : arms)
+ for (const auto& [k, a] : armInfos)
{
- r.arms[k] = a.toIce();
+ r.armInfos[k] = a.toIce();
}
- r.kinematicUnitInterface = kinematicUnitInterface;
- r.platformUnitInterface = platformUnitInterface;
- r.localizationUnitInterface = localizationUnitInterface;
+ r.robotUnit = robotUnit;
return r;
}
diff --git a/source/RobotAPI/libraries/robot_name_service/core/Robot.h b/source/RobotAPI/libraries/robot_name_service/core/Robot.h
index 243d94c1b2a7f72d8f050ee770d06611da5a83cf..4dedb60a99ccb82519bf810363457fb5bb723e52 100644
--- a/source/RobotAPI/libraries/robot_name_service/core/Robot.h
+++ b/source/RobotAPI/libraries/robot_name_service/core/Robot.h
@@ -10,11 +10,11 @@
namespace armarx::robot_name_service::core
{
- class Hand
+ class TCPInfo
{
public:
- void fromIce(const armarx::robot_name_service::dto::Hand& r);
- armarx::robot_name_service::dto::Hand toIce() const;
+ void fromIce(const armarx::robot_name_service::dto::TCPInfo& r);
+ armarx::robot_name_service::dto::TCPInfo toIce() const;
public:
std::string name;
@@ -22,22 +22,22 @@ namespace armarx::robot_name_service::core
armarx::HandUnitInterfacePrx handUnitInterface;
};
- class Arm
+ class ArmInfo
{
public:
- void fromIce(const armarx::robot_name_service::dto::Arm& r);
- armarx::robot_name_service::dto::Arm toIce() const;
+ void fromIce(const armarx::robot_name_service::dto::ArmInfo& r);
+ armarx::robot_name_service::dto::ArmInfo toIce() const;
public:
std::string kinematicChainName;
- Hand hand;
+ TCPInfo hand;
};
- class Robot
+ class RobotInfo
{
public:
- void fromIce(const armarx::robot_name_service::dto::Robot& r);
- armarx::robot_name_service::dto::Robot toIce() const;
+ void fromIce(const armarx::robot_name_service::dto::RobotInfo& r);
+ armarx::robot_name_service::dto::RobotInfo toIce() const;
public:
// header
@@ -49,11 +49,9 @@ namespace armarx::robot_name_service::core
armarx::skills::manager::dti::SkillManagerInterfacePrx skillManager;
// kinematic stuff
- std::map<std::string, Arm> arms;
+ std::map<std::string, ArmInfo> armInfos;
// units
- armarx::KinematicUnitInterfacePrx kinematicUnitInterface;
- armarx::PlatformUnitInterfacePrx platformUnitInterface;
- armarx::LocalizationUnitInterfacePrx localizationUnitInterface;
+ armarx::RobotUnitInterfacePrx robotUnit;
};
} // namespace armarx::robot_name_service::core
diff --git a/source/RobotAPI/libraries/robot_name_service/core/aron/Robot.xml b/source/RobotAPI/libraries/robot_name_service/core/aron/Robot.xml
new file mode 100644
index 0000000000000000000000000000000000000000..5b29a3d3c5bfb61b1ae6112a97972b2a4066aa58
--- /dev/null
+++ b/source/RobotAPI/libraries/robot_name_service/core/aron/Robot.xml
@@ -0,0 +1,40 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+ <GenerateTypes>
+
+ <Object name='armarx::robot_name_service::arondto::TCPInfo'>
+ <ObjectChild key='name'>
+ <string />
+ </ObjectChild>
+
+ </Object>
+
+ <Object name='armarx::robot_name_service::arondto::TCPInfo'>
+ <ObjectChild key='name'>
+ <string />
+ </ObjectChild>
+
+ <ObjectChild key='ft_name'>
+ <string />
+ </ObjectChild>
+
+ </Object>
+
+ <Object name='armarx::robot_name_service::arondto::ArmInfo'>
+ <ObjectChild key='kinematicChainName'>
+ <string />
+ </ObjectChild>
+
+ </Object>
+
+
+ <Object name='armarx::robot_name_service::arondto::RobotInfo'>
+ <ObjectChild key='name'>
+ <string />
+ </ObjectChild>
+
+ </Object>
+
+
+ </GenerateTypes>
+</AronTypeDefinition>
diff --git a/source/RobotAPI/libraries/robot_name_service/core/aron_conversions.cpp b/source/RobotAPI/libraries/robot_name_service/core/aron_conversions.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..57d53663682f641335c663b5483e626388d01d12
--- /dev/null
+++ b/source/RobotAPI/libraries/robot_name_service/core/aron_conversions.cpp
@@ -0,0 +1,6 @@
+#include "aron_conversions.h"
+
+namespace armarx::robot_name_service::core
+{
+
+} // namespace armarx::robot_name_service::core
diff --git a/source/RobotAPI/libraries/robot_name_service/core/aron_conversions.h b/source/RobotAPI/libraries/robot_name_service/core/aron_conversions.h
new file mode 100644
index 0000000000000000000000000000000000000000..4019e276eac94db6072c1338b74c3176d12734e8
--- /dev/null
+++ b/source/RobotAPI/libraries/robot_name_service/core/aron_conversions.h
@@ -0,0 +1,14 @@
+#pragma once
+
+#include <map>
+#include <memory>
+#include <string>
+#include <vector>
+
+#include <RobotAPI/interface/robot_name_service/RobotNameServiceInterface.h>
+#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
+
+namespace armarx::robot_name_service::core
+{
+
+} // namespace armarx::robot_name_service::core
diff --git a/source/RobotAPI/libraries/robot_name_service/server/CMakeLists.txt b/source/RobotAPI/libraries/robot_name_service/server/CMakeLists.txt
index d0c268aa439c501d38505d3fab455bf6b4a9b876..862934ffff91b608fdf651a6b57c4371b4749238 100644
--- a/source/RobotAPI/libraries/robot_name_service/server/CMakeLists.txt
+++ b/source/RobotAPI/libraries/robot_name_service/server/CMakeLists.txt
@@ -18,8 +18,10 @@ armarx_add_library(
SOURCES
plugins.cpp
+ segments/RobotSegment.cpp
HEADERS
plugins.h
+ segments/RobotSegment.h
)
add_library(RobotAPI::robot_name_service_server ALIAS robot_name_service_server)
diff --git a/source/RobotAPI/libraries/robot_name_service/server/segments/RobotSegment.cpp b/source/RobotAPI/libraries/robot_name_service/server/segments/RobotSegment.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..9e67bbb2526b9876818b342c0d55991ad7e473b6
--- /dev/null
+++ b/source/RobotAPI/libraries/robot_name_service/server/segments/RobotSegment.cpp
@@ -0,0 +1,6 @@
+#include "RobotSegment.h"
+
+namespace armarx::robot_name_service::server::segment
+{
+
+} // namespace armarx::robot_name_service::server::segment
diff --git a/source/RobotAPI/libraries/robot_name_service/server/segments/RobotSegment.h b/source/RobotAPI/libraries/robot_name_service/server/segments/RobotSegment.h
new file mode 100644
index 0000000000000000000000000000000000000000..7e9d006f6b7ca1d736af0486ed09daa1fe98f3c4
--- /dev/null
+++ b/source/RobotAPI/libraries/robot_name_service/server/segments/RobotSegment.h
@@ -0,0 +1,17 @@
+#pragma once
+
+#include <map>
+#include <memory>
+#include <string>
+#include <vector>
+
+#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
+#include <RobotAPI/libraries/armem/server/segment/SpecializedCoreSegment.h>
+
+namespace armarx::robot_name_service::server::segment
+{
+ class RobotInfoCoreSegment : public armarx::armem::server::segment::SpecializedCoreSegment
+ {
+ public:
+ };
+} // namespace armarx::robot_name_service::server::segment
diff --git a/source/RobotAPI/libraries/skills/core/Skill.cpp b/source/RobotAPI/libraries/skills/core/Skill.cpp
index 0b7a32df224d925ced896414d7d47f540f04a0d5..a01eb4908baed4e5f6dc18c76f5493053d4bb886 100644
--- a/source/RobotAPI/libraries/skills/core/Skill.cpp
+++ b/source/RobotAPI/libraries/skills/core/Skill.cpp
@@ -19,10 +19,22 @@ namespace armarx
}
std::optional<TerminatedSkillStatusUpdate>
- Skill::callSubskill(const SkillProxy& prx, const aron::data::DictPtr& params)
+ Skill::callSubskill(const SkillProxy& proxy)
{
- auto eid = callSubskillAsync(prx, params);
- auto ret = prx.join(eid);
+ return callSubskill(proxy, proxy.getRootProfileParameters());
+ }
+
+ std::optional<TerminatedSkillStatusUpdate>
+ Skill::callSubskill(const SkillProxy& proxy, const aron::data::DictPtr& parameters)
+ {
+ auto executionId = callSubskillAsync(proxy, parameters);
+ auto ret = proxy.join(executionId);
+
+ // While the sub skill was running, our skill might also have been aborted.
+ // In this case, the correct behavour would be aborting ourselves.
+ // The caller of callSubskill() can catch the thrown error::SkillAbortedException
+ // if necessary.
+ throwIfSkillShouldTerminate();
return ret;
}
@@ -37,6 +49,35 @@ namespace armarx
return eid;
}
+ std::optional<TerminatedSkillStatusUpdate>
+ Skill::callSubskill(const SkillID& skillId)
+ {
+ return callSubskill(SkillProxy(manager, skillId));
+ }
+
+ std::optional<TerminatedSkillStatusUpdate>
+ Skill::callSubskill(const SkillID& skillId, const aron::data::DictPtr& parameters)
+ {
+ return callSubskill(SkillProxy(manager, skillId), parameters);
+ }
+
+ std::optional<TerminatedSkillStatusUpdate>
+ Skill::callSubskill(const SkillID& skillId,
+ std::function<void(aron::data::DictPtr&)> parametersFunction)
+ {
+ SkillProxy proxy(manager, skillId);
+
+ aron::data::DictPtr parameters = proxy.getRootProfileParameters();
+ if (not parameters)
+ {
+ parameters = armarx::aron::make_dict();
+ }
+
+ parametersFunction(parameters);
+
+ return callSubskill(proxy, parameters);
+ }
+
void
Skill::updateParameters(const aron::data::DictPtr& d)
{
diff --git a/source/RobotAPI/libraries/skills/core/Skill.h b/source/RobotAPI/libraries/skills/core/Skill.h
index 5ec80e77d9956e8516ae84ffec16313bab0c9206..9fffd35a6f258e778200351ef387ba9d3b184480 100644
--- a/source/RobotAPI/libraries/skills/core/Skill.h
+++ b/source/RobotAPI/libraries/skills/core/Skill.h
@@ -1,15 +1,11 @@
#pragma once
-// std/stl
#include <functional>
#include <mutex>
#include <queue>
#include <thread>
-// base class
#include <ArmarXCore/core/logging/Logging.h>
-
-// ArmarX
#include <ArmarXCore/core/time/DateTime.h>
#include <ArmarXCore/core/time/Metronome.h>
@@ -27,6 +23,9 @@ namespace armarx
{
namespace skills
{
+ /**
+ * @brief Base class for skills.
+ */
class Skill : public armarx::Logging
{
public:
@@ -185,14 +184,136 @@ namespace armarx
void installConditionWithCallback(std::function<bool()>&& f,
std::function<void()>&& cb);
- /// call a subskill and block until the subskill terminates.
- /// If you call a subskill this way it will be stopped if the current skill stops.
+ // Calling subskills
+
+ /**
+ * @brief Call a subskill with default parameters and block until the subskill terminates.
+ *
+ * If you call a subskill this way it will be stopped if the current skill stops.
+ *
+ * @param proxy Skill proxy.
+ * @return Terminated skill status update.
+ * @throw armarx::skills::error::SkillAbortedException
+ * If the calling skill has been aborted.
+ * @throw armarx::skills::error::SkillFailedException
+ * If the calling skill's timeout was reached.
+ */
+ std::optional<TerminatedSkillStatusUpdate>
+ callSubskill(const skills::SkillProxy& proxy);
+
+ /**
+ * @brief Call a subskill with given parameters and block until the subskill terminates.
+ * @param proxy Skill proxy.
+ * @param parameters Parameters passed to the skill.
+ * @return Terminated skill status update.
+ * @throw armarx::skills::error::SkillAbortedException
+ * If the calling skill has been aborted.
+ * @throw armarx::skills::error::SkillFailedException
+ * If the calling skill's timeout was reached.
+ */
std::optional<TerminatedSkillStatusUpdate>
- callSubskill(const skills::SkillProxy& prx, const aron::data::DictPtr& = nullptr);
+ callSubskill(const skills::SkillProxy& proxy, const aron::data::DictPtr& parameters);
/// Similar to callSubskill but non-blocking
- skills::SkillExecutionID callSubskillAsync(const skills::SkillProxy& prx,
- const aron::data::DictPtr& = nullptr);
+ skills::SkillExecutionID callSubskillAsync(const skills::SkillProxy& proxy);
+
+ /// Similar to callSubskill but non-blocking
+ skills::SkillExecutionID callSubskillAsync(const skills::SkillProxy& proxy,
+ const aron::data::DictPtr& parameters);
+
+ /**
+ * @brief Call a subskill with the given ID and its default parameters.
+ * @param skillId The subskill's ID.
+ * @return The terminated skill status update.
+ * @throw armarx::skills::error::SkillAbortedException
+ * If the calling skill has been aborted.
+ * @throw armarx::skills::error::SkillFailedException
+ * If the calling skill's timeout was reached.
+ */
+ std::optional<TerminatedSkillStatusUpdate> callSubskill(const SkillID& skillId);
+
+ /**
+ * @brief Call a subskill with the given ID and parameters.
+ * @param skillId The subskill's ID.
+ * @param parameters The parameters.
+ * @return The terminated skill status update.
+ * @throw armarx::skills::error::SkillAbortedException
+ * If the calling skill has been aborted.
+ * @throw armarx::skills::error::SkillFailedException
+ * If the calling skill's timeout was reached.
+ */
+ std::optional<TerminatedSkillStatusUpdate>
+ callSubskill(const SkillID& skillId, const aron::data::DictPtr& parameters);
+
+ /**
+ * @brief Call a subskill with the given ID and parameters.
+ * @param skillId The subskill's ID.
+ * @param parameters The parameters.
+ * @return The terminated skill status update.
+ * @throw armarx::skills::error::SkillAbortedException
+ * If the calling skill has been aborted.
+ * @throw armarx::skills::error::SkillFailedException
+ * If the calling skill's timeout was reached.
+ */
+ template <class ParameterT>
+ std::optional<TerminatedSkillStatusUpdate>
+ callSubskill(const SkillID& skillId, const ParameterT& parameters)
+ {
+ return callSubskill(skillId, parameters.toAron());
+ }
+
+ /**
+ * @brief Call a subskill with parameters based on the default parameters.
+ *
+ * Creates the skill's default parameters, and calls `parametersFunction` on them.
+ * This allows the caller to modify the parameters before executing the skill.
+ *
+ * @param skillId The subskill's ID.
+ * @param parametersFunction Function which edits the parameters.
+ * @return The terminated skill status update.
+ * @throw armarx::skills::error::SkillAbortedException
+ * If the calling skill has been aborted.
+ * @throw armarx::skills::error::SkillFailedException
+ * If the calling skill's timeout was reached.
+ */
+ std::optional<TerminatedSkillStatusUpdate>
+ callSubskill(const SkillID& skillId,
+ std::function<void(aron::data::DictPtr& parameters)> parametersFunction);
+
+ /**
+ * @brief Call a subskill with parameters based on the default parameters.
+ *
+ * Creates the skill's default parameters, converts them to `ParameterT`,
+ * and calls `parametersFunction` on them.
+ * This allows the caller to modify the parameters as `ParameterT` before executing
+ * the skill.
+ *
+ * @param skillId The subskill's ID.
+ * @param parametersFunction Function which edits the parameters.
+ * @return The terminated skill status update.
+ * @throw armarx::skills::error::SkillAbortedException
+ * If the calling skill has been aborted.
+ * @throw armarx::skills::error::SkillFailedException
+ * If the calling skill's timeout was reached.
+ */
+ template <class ParameterT>
+ std::optional<TerminatedSkillStatusUpdate>
+ callSubskill(const SkillID& skillId,
+ std::function<void(ParameterT& parameters)> parametersFunction)
+ {
+ SkillProxy proxy(manager, skillId);
+
+ ParameterT parameters;
+ if (auto parametersAron = proxy.getRootProfileParameters())
+ {
+ parameters = ParameterT::FromAron(parametersAron);
+ }
+
+ parametersFunction(parameters);
+
+ return callSubskill(proxy, parameters.toAron());
+ }
+
public:
// running params
diff --git a/source/RobotAPI/libraries/skills/core/SkillID.cpp b/source/RobotAPI/libraries/skills/core/SkillID.cpp
index ece268aeeec9234a8dd4b2dcaa437d616988317e..2bf5cb5596a89a8daab37ef8e957a8c9fcc2b532 100644
--- a/source/RobotAPI/libraries/skills/core/SkillID.cpp
+++ b/source/RobotAPI/libraries/skills/core/SkillID.cpp
@@ -50,7 +50,7 @@ namespace armarx
manager::dto::SkillID
SkillID::toManagerIce() const
{
- ARMARX_CHECK(isFullySpecified());
+ ARMARX_CHECK(isFullySpecified()) << "Got: " << toString();
return {providerId->toManagerIce(), skillName};
}
diff --git a/source/RobotAPI/libraries/skills/core/SkillStatusUpdate.cpp b/source/RobotAPI/libraries/skills/core/SkillStatusUpdate.cpp
index 986aab1bc9be867aa918c14e4471ac9fe3e7cf3f..39220a32bc28269232f506d8368cb5394980b87b 100644
--- a/source/RobotAPI/libraries/skills/core/SkillStatusUpdate.cpp
+++ b/source/RobotAPI/libraries/skills/core/SkillStatusUpdate.cpp
@@ -315,6 +315,16 @@ namespace armarx
return ret;
}
+ template <class BoT, class DtoT>
+ static void
+ setResultFromIce(BoT& bo, const DtoT& dto)
+ {
+ if (dto.result)
+ {
+ bo.result = aron::make_dict(dto.result);
+ }
+ }
+
TerminatedSkillStatusUpdate
TerminatedSkillStatusUpdate::FromIce(const manager::dto::SkillStatusUpdate& update)
{
@@ -323,6 +333,7 @@ namespace armarx
.parameters = armarx::aron::data::Dict::FromAronDictDTO(update.parameters),
.callbackInterface = update.callbackInterface}};
skills::fromIce(update.status, ret.status);
+ setResultFromIce(ret, update);
return ret;
}
@@ -335,17 +346,20 @@ namespace armarx
.parameters = armarx::aron::data::Dict::FromAronDictDTO(update.parameters),
.callbackInterface = update.callbackInterface}};
skills::fromIce(update.status, ret.status);
+ setResultFromIce(ret, update);
return ret;
}
SkillStatusUpdate
SkillStatusUpdate::FromIce(const manager::dto::SkillStatusUpdate& update)
{
- SkillStatusUpdate ret{
- {.executionId = skills::SkillExecutionID::FromIce(update.executionId),
- .parameters = armarx::aron::data::Dict::FromAronDictDTO(update.parameters),
- .callbackInterface = update.callbackInterface}};
+ SkillStatusUpdate ret{{
+ .executionId = skills::SkillExecutionID::FromIce(update.executionId),
+ .parameters = armarx::aron::data::Dict::FromAronDictDTO(update.parameters),
+ .callbackInterface = update.callbackInterface,
+ }};
skills::fromIce(update.status, ret.status);
+ setResultFromIce(ret, update);
return ret;
}
@@ -353,11 +367,14 @@ namespace armarx
SkillStatusUpdate::FromIce(const provider::dto::SkillStatusUpdate& update,
const std::optional<skills::ProviderID>& providerId)
{
- SkillStatusUpdate ret{
- {.executionId = skills::SkillExecutionID::FromIce(update.executionId, providerId),
- .parameters = armarx::aron::data::Dict::FromAronDictDTO(update.parameters),
- .callbackInterface = update.callbackInterface}};
+ SkillStatusUpdate ret{{
+ .executionId = skills::SkillExecutionID::FromIce(update.executionId, providerId),
+ .parameters = armarx::aron::data::Dict::FromAronDictDTO(update.parameters),
+ .callbackInterface = update.callbackInterface,
+ .result = armarx::aron::data::Dict::FromAronDictDTO(update.result)
+ }};
skills::fromIce(update.status, ret.status);
+ setResultFromIce(ret, update);
return ret;
}
@@ -369,6 +386,7 @@ namespace armarx
.parameters = armarx::aron::data::Dict::FromAronDictDTO(update.parameters),
.callbackInterface = update.callbackInterface}};
skills::fromIce(update.status, ret.status);
+ setResultFromIce(ret, update);
return ret;
}
@@ -382,7 +400,9 @@ namespace armarx
.parameters = armarx::aron::data::Dict::FromAronDictDTO(update.parameters),
.callbackInterface = update.callbackInterface}};
skills::fromIce(update.status, ret.status);
+ setResultFromIce(ret, update);
return ret;
}
+
} // namespace skills
} // namespace armarx
diff --git a/source/RobotAPI/libraries/skills/manager/SkillManagerComponentPlugin.cpp b/source/RobotAPI/libraries/skills/manager/SkillManagerComponentPlugin.cpp
index 3999c956b3ec3d85be444e7b27d56c27fbc2d98e..14e4e64e8a0ba1af5d8a22a824bc76543cc91fe2 100644
--- a/source/RobotAPI/libraries/skills/manager/SkillManagerComponentPlugin.cpp
+++ b/source/RobotAPI/libraries/skills/manager/SkillManagerComponentPlugin.cpp
@@ -84,7 +84,8 @@ namespace armarx::plugins
skills::SkillStatusUpdate
SkillManagerComponentPlugin::executeSkill(const skills::SkillExecutionRequest& executionRequest)
{
- ARMARX_CHECK(executionRequest.skillId.isFullySpecified());
+ ARMARX_CHECK(executionRequest.skillId.isFullySpecified())
+ << "Got: " << executionRequest.skillId.toString();
std::unique_lock l(skillProviderMapMutex);
@@ -157,7 +158,8 @@ namespace armarx::plugins
SkillManagerComponentPlugin::executeSkillAsync(
const skills::SkillExecutionRequest& executionRequest)
{
- ARMARX_CHECK(executionRequest.skillId.isFullySpecified());
+ ARMARX_CHECK(executionRequest.skillId.isFullySpecified())
+ << "Got: " << executionRequest.skillId.toString();
std::unique_lock l(skillProviderMapMutex);
@@ -231,7 +233,8 @@ namespace armarx::plugins
SkillManagerComponentPlugin::updateSkillParameters(const skills::SkillExecutionID& executionId,
const aron::data::DictPtr& data)
{
- ARMARX_CHECK(executionId.skillId.isFullySpecified());
+ ARMARX_CHECK(executionId.skillId.isFullySpecified())
+ << "Got: " << executionId.skillId.toString();
std::unique_lock l(skillProviderMapMutex);
if (auto it = skillProviderMap.find(*executionId.skillId.providerId);
@@ -272,7 +275,8 @@ namespace armarx::plugins
bool
SkillManagerComponentPlugin::abortSkill(const skills::SkillExecutionID& executionId)
{
- ARMARX_CHECK(executionId.skillId.isFullySpecified());
+ ARMARX_CHECK(executionId.skillId.isFullySpecified())
+ << "Got: " << executionId.skillId.toString();
std::unique_lock l(skillProviderMapMutex);
if (auto it = skillProviderMap.find(*executionId.skillId.providerId);
@@ -312,7 +316,8 @@ namespace armarx::plugins
bool
SkillManagerComponentPlugin::abortSkillAsync(const skills::SkillExecutionID& executionId)
{
- ARMARX_CHECK(executionId.skillId.isFullySpecified());
+ ARMARX_CHECK(executionId.skillId.isFullySpecified())
+ << "Got: " << executionId.skillId.toString();
std::unique_lock l(skillProviderMapMutex);
if (auto it = skillProviderMap.find(*executionId.skillId.providerId);
@@ -350,7 +355,7 @@ namespace armarx::plugins
std::optional<skills::SkillDescription>
SkillManagerComponentPlugin::getSkillDescription(const skills::SkillID& skillId)
{
- ARMARX_CHECK(skillId.isFullySpecified());
+ ARMARX_CHECK(skillId.isFullySpecified()) << "Got: " << skillId.toString();
std::unique_lock l(skillProviderMapMutex);
if (auto it = skillProviderMap.find(*skillId.providerId); it != skillProviderMap.end())
@@ -453,7 +458,8 @@ namespace armarx::plugins
SkillManagerComponentPlugin::getSkillExecutionStatus(
const skills::SkillExecutionID& executionId)
{
- ARMARX_CHECK(executionId.skillId.isFullySpecified());
+ ARMARX_CHECK(executionId.skillId.isFullySpecified())
+ << "Got: " << executionId.skillId.toString();
std::unique_lock l(skillProviderMapMutex);
if (auto it = skillProviderMap.find(*executionId.skillId.providerId);
diff --git a/source/RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.cpp b/source/RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.cpp
index a42d7808284d958d965cfba0dd70aa866a47cc17..3d775f587834aa3a62fd42d1be353eb23a8fc0d4 100644
--- a/source/RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.cpp
+++ b/source/RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.cpp
@@ -121,7 +121,7 @@ namespace armarx::plugins
{
ARMARX_CHECK(execId.skillId.isSkillSpecified());
- const std::unique_lock l(skillExecutionsMutex);
+ const std::shared_lock l(skillExecutionsMutex);
if (skillExecutions.find(execId) == skillExecutions.end())
{
ARMARX_WARNING << "Skill execution for skill '" + execId.skillId.toString() +
@@ -137,7 +137,7 @@ namespace armarx::plugins
{
std::map<skills::SkillExecutionID, skills::SkillStatusUpdate> skillUpdates;
- const std::scoped_lock l(skillExecutionsMutex);
+ const std::shared_lock l(skillExecutionsMutex);
for (const auto& [key, impl] : skillExecutions)
{
const std::scoped_lock l2(impl.skillStatusesMutex);
@@ -151,7 +151,7 @@ namespace armarx::plugins
{
ARMARX_CHECK(skillId.isFullySpecified());
- const std::unique_lock l(skillFactoriesMutex);
+ const std::shared_lock l(skillFactoriesMutex);
if (skillFactories.find(skillId) == skillFactories.end())
{
std::stringstream ss;
@@ -175,7 +175,7 @@ namespace armarx::plugins
SkillProviderComponentPlugin::getSkillDescriptions() const
{
std::map<skills::SkillID, skills::SkillDescription> skillDesciptions;
- const std::unique_lock l(skillFactoriesMutex);
+ const std::shared_lock l(skillFactoriesMutex);
for (const auto& [key, fac] : skillFactories)
{
ARMARX_CHECK(key.isFullySpecified());
@@ -215,6 +215,7 @@ namespace armarx::plugins
executionRequest.parameters,
executionRequest.callbackInterface));
wrapper = &it.first->second;
+ // ATTENTION NOT DEFINED BEHAVIOR
}
// async start execution. But we wait for the execution to finish at the end of this method
@@ -269,6 +270,7 @@ namespace armarx::plugins
executionRequest.parameters,
executionRequest.callbackInterface));
wrapper = &it.first->second;
+ // ATTENTION NOT DEFINED BEHAVIOR
}
wrapper->execution = std::thread(
@@ -303,7 +305,7 @@ namespace armarx::plugins
{
ARMARX_CHECK(executionId.skillId.isFullySpecified());
- const std::scoped_lock l{skillExecutionsMutex};
+ std::shared_lock l{skillExecutionsMutex};
auto it = skillExecutions.find(executionId);
if (it == skillExecutions.end())
{
@@ -329,7 +331,7 @@ namespace armarx::plugins
{
ARMARX_CHECK(executionId.skillId.isFullySpecified());
- const std::unique_lock l(skillExecutionsMutex);
+ std::shared_lock l(skillExecutionsMutex);
auto it = skillExecutions.find(executionId);
if (it == skillExecutions.end())
{
@@ -363,7 +365,7 @@ namespace armarx::plugins
{
ARMARX_CHECK(executionId.skillId.isFullySpecified());
- const std::unique_lock l(skillExecutionsMutex);
+ std::shared_lock l(skillExecutionsMutex);
auto it = skillExecutions.find(executionId);
if (it == skillExecutions.end())
{
diff --git a/source/RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.h b/source/RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.h
index 812cc0a8814db843793038ebdd16a3a927c912cf..20ffe1fbdb75ee9f4b5831dbd406180f7acf22fc 100644
--- a/source/RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.h
+++ b/source/RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.h
@@ -104,10 +104,10 @@ namespace armarx::plugins
skills::manager::dti::SkillManagerInterfacePrx manager;
skills::provider::dti::SkillProviderInterfacePrx myPrx;
- mutable std::mutex skillFactoriesMutex;
+ mutable std::shared_mutex skillFactoriesMutex;
std::map<skills::SkillID, std::unique_ptr<skills::SkillBlueprint>> skillFactories;
- mutable std::mutex skillExecutionsMutex;
+ mutable std::shared_mutex skillExecutionsMutex;
std::map<skills::SkillExecutionID, skills::detail::SkillRuntime> skillExecutions;
friend class armarx::SkillProviderComponentPluginUser;
diff --git a/source/RobotAPI/libraries/skills/provider/detail/SkillImplementationWrapper.cpp b/source/RobotAPI/libraries/skills/provider/detail/SkillImplementationWrapper.cpp
index 55f9d80c01a78154aa95780e2cc6f549be94f843..25fe34efa5b5c26ffed19bdca3680555302326f6 100644
--- a/source/RobotAPI/libraries/skills/provider/detail/SkillImplementationWrapper.cpp
+++ b/source/RobotAPI/libraries/skills/provider/detail/SkillImplementationWrapper.cpp
@@ -340,7 +340,7 @@ namespace armarx
// Exit succeeded!
// All succeeded!
{
- updateStatus(SkillStatus::Succeeded);
+ updateStatus(SkillStatus::Succeeded, mainRet.data);
// return result of main method
std::unique_lock l(skillStatusesMutex);