diff --git a/scenarios/SkillProviderTest/config/ArVizStorage.cfg b/scenarios/SkillProviderTest/config/ArVizStorage.cfg
index 302ac28c37dd28de3e68fb4fe4c2174faa4ec3bf..a07db2940a4d3870521fe7d5f4faa74b95f8a788 100644
--- a/scenarios/SkillProviderTest/config/ArVizStorage.cfg
+++ b/scenarios/SkillProviderTest/config/ArVizStorage.cfg
@@ -68,6 +68,15 @@
# ArmarX.ArVizStorage.TopicName = ArVizTopic
+# 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
diff --git a/scenarios/SkillProviderTest/config/DebugObserver.cfg b/scenarios/SkillProviderTest/config/DebugObserver.cfg
index 8dc7ead26b3bd2f7678b3b3e7a1b00c01213225d..4692d6eb22961724c00da6ea0655e14b529dc0b3 100644
--- a/scenarios/SkillProviderTest/config/DebugObserver.cfg
+++ b/scenarios/SkillProviderTest/config/DebugObserver.cfg
@@ -18,6 +18,15 @@
# 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
diff --git a/scenarios/SkillProviderTest/config/MemoryNameSystem.cfg b/scenarios/SkillProviderTest/config/MemoryNameSystem.cfg
index b8bc70a66ca7f32a628886ad1bf13e373f9750d3..f3557b670ab65b2a6fbb924466be6539440df5a3 100644
--- a/scenarios/SkillProviderTest/config/MemoryNameSystem.cfg
+++ b/scenarios/SkillProviderTest/config/MemoryNameSystem.cfg
@@ -18,6 +18,15 @@
# 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
diff --git a/scenarios/SkillProviderTest/config/RemoteGuiProviderApp.cfg b/scenarios/SkillProviderTest/config/RemoteGuiProviderApp.cfg
index 4b6abea40d72afd7d313ee47a9b191f3b26de30d..1a338e3828f75121a596368c0dd067ad5e105487 100644
--- a/scenarios/SkillProviderTest/config/RemoteGuiProviderApp.cfg
+++ b/scenarios/SkillProviderTest/config/RemoteGuiProviderApp.cfg
@@ -18,6 +18,15 @@
# 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
diff --git a/scenarios/SkillProviderTest/config/SkillProviderExample.cfg b/scenarios/SkillProviderTest/config/SkillProviderExample.cfg
index 90ae11d1f4c91a4d1dc3e2b737fabe9475dd125d..fbc666bd2b68bcceaad2d9e882566fd9fb3c80a0 100644
--- a/scenarios/SkillProviderTest/config/SkillProviderExample.cfg
+++ b/scenarios/SkillProviderTest/config/SkillProviderExample.cfg
@@ -18,6 +18,15 @@
# 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
diff --git a/source/RobotAPI/components/CMakeLists.txt b/source/RobotAPI/components/CMakeLists.txt
index 11908aeda6ade0ff4b46f63d0590c88d007e51c9..fcd09e9158a7bb7b36ab3a27a1edb875aaa7b6e6 100644
--- a/source/RobotAPI/components/CMakeLists.txt
+++ b/source/RobotAPI/components/CMakeLists.txt
@@ -27,6 +27,7 @@ add_subdirectory(NaturalIKTest)
add_subdirectory(ObjectPoseClientExample)
add_subdirectory(ObjectPoseProviderExample)
add_subdirectory(RobotHealth)
+add_subdirectory(RobotDefinition)
add_subdirectory(RobotNameService)
add_subdirectory(RobotState)
add_subdirectory(RobotToArViz)
diff --git a/source/RobotAPI/components/RobotDefinition/CMakeLists.txt b/source/RobotAPI/components/RobotDefinition/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..58ce4d1f06bc828033afa68fee73f95dca48557d
--- /dev/null
+++ b/source/RobotAPI/components/RobotDefinition/CMakeLists.txt
@@ -0,0 +1,26 @@
+armarx_component_set_name("RobotDefinition")
+
+set(COMPONENT_LIBS
+ ArmarXCore
+ ArmarXCoreInterfaces # for DebugObserverInterface
+ ArmarXGuiComponentPlugins
+ RobotAPICore
+ RobotAPIInterfaces
+ RobotAPISkills
+ armem
+ robot_name_service_core
+ robot_name_service_client
+)
+
+set(SOURCES
+ RobotDefinition.cpp
+)
+
+set(HEADERS
+ RobotDefinition.h
+)
+
+armarx_add_component("${SOURCES}" "${HEADERS}")
+
+#generate the application
+armarx_generate_and_add_component_executable(COMPONENT_NAMESPACE "armarx")
diff --git a/source/RobotAPI/components/RobotDefinition/RobotDefinition.cpp b/source/RobotAPI/components/RobotDefinition/RobotDefinition.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..6a2df4ccd8d34174860acdb624b943ff232e98e5
--- /dev/null
+++ b/source/RobotAPI/components/RobotDefinition/RobotDefinition.cpp
@@ -0,0 +1,55 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::ArmarXObjects::RobotNameService
+ * @author [Author Name] ( [Author Email] )
+ * @date 2018
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "RobotDefinition.h"
+
+namespace armarx
+{
+ void
+ RobotDefinition::onInitComponent()
+ {
+ }
+
+ void
+ RobotDefinition::onConnectComponent()
+ {
+ }
+
+ void
+ RobotDefinition::onDisconnectComponent()
+ {
+ }
+
+ void
+ RobotDefinition::onExitComponent()
+ {
+ }
+
+ armarx::PropertyDefinitionsPtr
+ RobotDefinition::createPropertyDefinitions()
+ {
+ armarx::PropertyDefinitionsPtr defs =
+ new ComponentPropertyDefinitions(getConfigIdentifier());
+
+ return defs;
+ }
+} // namespace armarx
diff --git a/source/RobotAPI/components/RobotDefinition/RobotDefinition.h b/source/RobotAPI/components/RobotDefinition/RobotDefinition.h
new file mode 100644
index 0000000000000000000000000000000000000000..ba4eb80b97cc0a6ef049530c791f5e6a03c994a9
--- /dev/null
+++ b/source/RobotAPI/components/RobotDefinition/RobotDefinition.h
@@ -0,0 +1,88 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::ArmarXObjects::RobotNameService
+ * @author [Author Name] ( [Author Email] )
+ * @date 2018
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <map>
+#include <mutex>
+#include <string>
+
+#include <ArmarXCore/core/Component.h>
+
+#include <RobotAPI/interface/robot_name_service/RobotNameServiceInterface.h>
+#include <RobotAPI/libraries/robot_name_service/client/Plugin.h>
+#include <RobotAPI/libraries/robot_name_service/core/Robot.h>
+
+namespace armarx
+{
+ /**
+ * @defgroup Component-RobotNameService RobotNameService
+ * @ingroup RobotAPI-Components
+ * A description of the component RobotNameService.
+ *
+ * @class RobotNameService
+ * @ingroup Component-RobotNameService
+ * @brief Brief description of class RobotNameService.
+ *
+ * Detailed description of class RobotNameService.
+ */
+ class RobotDefinition :
+ virtual public armarx::Component,
+ virtual public armarx::RobotNameServiceComponentPluginUser
+ {
+ public:
+ /**
+ * @see armarx::ManagedIceObject::getDefaultName()
+ */
+ std::string
+ getDefaultName() const override
+ {
+ return "RobotDefinition";
+ }
+
+ protected:
+ /**
+ * @see armarx::ManagedIceObject::onInitComponent()
+ */
+ void onInitComponent() override;
+
+ /**
+ * @see armarx::ManagedIceObject::onConnectComponent()
+ */
+ void onConnectComponent() override;
+
+ /**
+ * @see armarx::ManagedIceObject::onDisconnectComponent()
+ */
+ void onDisconnectComponent() override;
+
+ /**
+ * @see armarx::ManagedIceObject::onExitComponent()
+ */
+ void onExitComponent() override;
+
+ /**
+ * @see PropertyUser::createPropertyDefinitions()
+ */
+ armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
+ };
+} // namespace armarx
diff --git a/source/RobotAPI/components/RobotDefinition/test/CMakeLists.txt b/source/RobotAPI/components/RobotDefinition/test/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..154bbec83350d9d49b761e5ad3845b9578612bf0
--- /dev/null
+++ b/source/RobotAPI/components/RobotDefinition/test/CMakeLists.txt
@@ -0,0 +1,5 @@
+
+# Libs required for the tests
+SET(LIBS ${LIBS} ArmarXCore RobotNameService)
+
+armarx_add_test(RobotNameServiceTest RobotNameServiceTest.cpp "${LIBS}")
\ No newline at end of file
diff --git a/source/RobotAPI/interface/components/RobotNameServiceInterface.ice b/source/RobotAPI/components/RobotDefinition/test/RobotNameServiceTest.cpp
similarity index 63%
rename from source/RobotAPI/interface/components/RobotNameServiceInterface.ice
rename to source/RobotAPI/components/RobotDefinition/test/RobotNameServiceTest.cpp
index 3478d40669895361a857ba972167bb85c131d0bc..6e3e6a973a04d105c2cf67478bab189ba7fbc2f4 100644
--- a/source/RobotAPI/interface/components/RobotNameServiceInterface.ice
+++ b/source/RobotAPI/components/RobotDefinition/test/RobotNameServiceTest.cpp
@@ -13,19 +13,25 @@
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*
- * @author Simon Ottenhaus ( simon dot ottenhaus at kit dot edu )
+ * @package RobotAPI::ArmarXObjects::RobotNameService
+ * @author [Author Name] ( [Author Email] )
* @date 2018
* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
* GNU General Public License
*/
-#include <RobotAPI/interface/observers/KinematicUnitObserverInterface.ice>
+#define BOOST_TEST_MODULE RobotAPI::ArmarXObjects::RobotNameService
-#pragma once
+#define ARMARX_BOOST_TEST
-module armarx
+#include <RobotAPI/Test.h>
+#include <RobotAPI/components/RobotNameService/RobotNameService.h>
+
+#include <iostream>
+
+BOOST_AUTO_TEST_CASE(testExample)
{
- interface RobotNameServiceInterface
- {
- };
-};
+ armarx::RobotNameService instance;
+
+ BOOST_CHECK_EQUAL(true, true);
+}
diff --git a/source/RobotAPI/components/RobotNameService/CMakeLists.txt b/source/RobotAPI/components/RobotNameService/CMakeLists.txt
index 1205ad25b0f143b5ca6af3719b44d4b3b6e07117..c362b8dab5b24f49a18c4a7d110e5dac956c8b2f 100644
--- a/source/RobotAPI/components/RobotNameService/CMakeLists.txt
+++ b/source/RobotAPI/components/RobotNameService/CMakeLists.txt
@@ -1,10 +1,26 @@
armarx_component_set_name("RobotNameService")
-set(COMPONENT_LIBS ArmarXCore)
-set(SOURCES RobotNameService.cpp)
-set(HEADERS RobotNameService.h)
-armarx_add_component("${SOURCES}" "${HEADERS}")
-# add unit tests
-add_subdirectory(test)
+set(COMPONENT_LIBS
+ ArmarXCore
+ ArmarXCoreInterfaces # for DebugObserverInterface
+ ArmarXGuiComponentPlugins
+ RobotAPICore
+ RobotAPIInterfaces
+ RobotAPISkills
+ armem
+ robot_name_service_core
+ robot_name_service_server
+)
+
+set(SOURCES
+ RobotNameService.cpp
+)
+
+set(HEADERS
+ RobotNameService.h
+)
+
+armarx_add_component("${SOURCES}" "${HEADERS}")
-armarx_generate_and_add_component_executable(APPLICATION_APP_SUFFIX)
+#generate the application
+armarx_generate_and_add_component_executable(COMPONENT_NAMESPACE "armarx")
diff --git a/source/RobotAPI/components/RobotNameService/RobotNameService.cpp b/source/RobotAPI/components/RobotNameService/RobotNameService.cpp
index 65f97877de7dff1b4313fba2afde4838913488ca..9ab8e07d7ca5234e40e167ae73201cae04105db5 100644
--- a/source/RobotAPI/components/RobotNameService/RobotNameService.cpp
+++ b/source/RobotAPI/components/RobotNameService/RobotNameService.cpp
@@ -22,35 +22,76 @@
#include "RobotNameService.h"
-
namespace armarx
{
- void RobotNameService::onInitComponent()
+ void
+ RobotNameService::onInitComponent()
{
-
}
-
- void RobotNameService::onConnectComponent()
+ void
+ RobotNameService::onConnectComponent()
{
+ }
+ void
+ RobotNameService::onDisconnectComponent()
+ {
}
+ void
+ RobotNameService::onExitComponent()
+ {
+ }
- void RobotNameService::onDisconnectComponent()
+ armarx::PropertyDefinitionsPtr
+ RobotNameService::createPropertyDefinitions()
{
+ armarx::PropertyDefinitionsPtr defs =
+ new ComponentPropertyDefinitions(getConfigIdentifier());
+ return defs;
}
+ bool
+ RobotNameService::registerRobot(const robot_name_service::dto::Robot& robot,
+ const Ice::Current& current)
+ {
+ std::scoped_lock l(robotsMutex);
+ ARMARX_INFO << "Register a new robot with name '" << robot.name << "' in RNS";
+
+ if (auto it = robots.find(robot.name); it != robots.end())
+ {
+ ARMARX_ERROR << "The robot with name '" << robot.name
+ << "' is already registered. Ignoring it.";
+ return false;
+ }
+
+ robots[robot.name].fromIce(robot);
+ return true;
+ }
- void RobotNameService::onExitComponent()
+ void
+ RobotNameService::unregisterRobot(const std::string& name, const Ice::Current& current)
{
+ std::scoped_lock l(robotsMutex);
+ if (auto it = robots.find(name); it != robots.end())
+ {
+ robots.erase(it);
+ }
}
- armarx::PropertyDefinitionsPtr RobotNameService::createPropertyDefinitions()
+ IceUtil::Optional<robot_name_service::dto::Robot>
+ RobotNameService::getRobot(const std::string& name, const Ice::Current& current)
{
- return armarx::PropertyDefinitionsPtr(new RobotNameServicePropertyDefinitions(
- getConfigIdentifier()));
+ std::scoped_lock l(robotsMutex);
+
+ if (auto it = robots.find(name); it == robots.end())
+ {
+ return {};
+ }
+
+ return robots[name].toIce();
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/RobotNameService/RobotNameService.h b/source/RobotAPI/components/RobotNameService/RobotNameService.h
index 92f5942342302f8fcb9db72830179552cc0188f8..7af787aef845a26c8ed47f7b9351c36cd6dce002 100644
--- a/source/RobotAPI/components/RobotNameService/RobotNameService.h
+++ b/source/RobotAPI/components/RobotNameService/RobotNameService.h
@@ -22,27 +22,17 @@
#pragma once
+#include <map>
+#include <mutex>
+#include <string>
#include <ArmarXCore/core/Component.h>
+#include <RobotAPI/interface/robot_name_service/RobotNameServiceInterface.h>
+#include <RobotAPI/libraries/robot_name_service/core/Robot.h>
+
namespace armarx
{
- /**
- * @class RobotNameServicePropertyDefinitions
- * @brief
- */
- class RobotNameServicePropertyDefinitions:
- public armarx::ComponentPropertyDefinitions
- {
- public:
- RobotNameServicePropertyDefinitions(std::string prefix):
- armarx::ComponentPropertyDefinitions(prefix)
- {
- //defineRequiredProperty<std::string>("PropertyName", "Description");
- //defineOptionalProperty<std::string>("PropertyName", "DefaultValue", "Description");
- }
- };
-
/**
* @defgroup Component-RobotNameService RobotNameService
* @ingroup RobotAPI-Components
@@ -55,13 +45,15 @@ namespace armarx
* Detailed description of class RobotNameService.
*/
class RobotNameService :
- virtual public armarx::Component
+ virtual public armarx::Component,
+ virtual public armarx::robot_name_service::dti::RobotNameServiceInterface
{
public:
/**
* @see armarx::ManagedIceObject::getDefaultName()
*/
- std::string getDefaultName() const override
+ std::string
+ getDefaultName() const override
{
return "RobotNameService";
}
@@ -91,6 +83,18 @@ namespace armarx
* @see PropertyUser::createPropertyDefinitions()
*/
armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
- };
-}
+
+ // 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;
+
+ private:
+ mutable std::mutex robotsMutex;
+ std::map<std::string, robot_name_service::core::Robot> robots;
+ };
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/CMakeLists.txt b/source/RobotAPI/gui-plugins/SkillManagerPlugin/CMakeLists.txt
index 1ee52e8455a17440fd7b4557d1bd24d2355dfde4..5df70b65810beeead4599151d25b15960a26e615 100644
--- a/source/RobotAPI/gui-plugins/SkillManagerPlugin/CMakeLists.txt
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/CMakeLists.txt
@@ -10,12 +10,13 @@ set(SOURCES
aronTreeWidget/visitors/AronTreeWidgetSetter.cpp
aronTreeWidget/visitors/AronTreeWidgetModalCreator.cpp
aronTreeWidget/visitors/AronTreeWidgetContextMenu.cpp
- aronTreeWidget/Data.cpp
aronTreeWidget/widgets/CustomWidget.cpp
aronTreeWidget/widgets/EditMatrixWidget.cpp
aronTreeWidget/widgets/IntEnumWidget.cpp
- aronTreeWidget/ListDictHelper.cpp
aronTreeWidget/widgets/QuaternionWidget.cpp
+ aronTreeWidget/widgets/SkillDescriptionWidget.cpp
+ aronTreeWidget/Data.cpp
+ aronTreeWidget/ListDictHelper.cpp
aronTreeWidget/AronTreeWidgetItem.cpp
aronTreeWidget/AronTreeWidgetController.cpp
aronTreeWidget/modal/text/AronTreeWidgetTextInputModalController.cpp
@@ -35,13 +36,14 @@ set(HEADERS
aronTreeWidget/visitors/AronTreeWidgetSetter.h
aronTreeWidget/visitors/AronTreeWidgetModalCreator.h
aronTreeWidget/visitors/AronTreeWidgetContextMenu.h
- aronTreeWidget/Data.h
aronTreeWidget/widgets/NDArrayHelper.h
aronTreeWidget/widgets/EditMatrixWidget.h
aronTreeWidget/widgets/CustomWidget.h
aronTreeWidget/widgets/IntEnumWidget.h
- aronTreeWidget/ListDictHelper.h
aronTreeWidget/widgets/QuaternionWidget.h
+ aronTreeWidget/widgets/SkillDescriptionWidget.h
+ aronTreeWidget/Data.h
+ aronTreeWidget/ListDictHelper.h
aronTreeWidget/AronTreeWidgetItem.h
aronTreeWidget/AronTreeWidgetController.h
aronTreeWidget/modal/AronTreeWidgetModal.h
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidget.ui b/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidget.ui
index abc07d96c9a9c56f0e2bcd0edef9a2540c75a5fc..1a1c52cd6593191210e807004bb7d6de3c5ae261 100644
--- a/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidget.ui
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidget.ui
@@ -149,72 +149,137 @@
<property name="title">
<string>Skill Details</string>
</property>
- <layout class="QGridLayout" name="gridLayout_2">
- <item row="8" column="3">
- <widget class="QPushButton" name="pushButtonExecuteSkill">
- <property name="text">
- <string>Request Execution</string>
+ <layout class="QVBoxLayout" name="verticalLayout">
+ <item>
+ <widget class="QSplitter" name="splitter_2">
+ <property name="enabled">
+ <bool>true</bool>
</property>
- </widget>
- </item>
- <item row="0" column="0">
- <widget class="QPushButton" name="pushButtonPaste">
- <property name="text">
- <string>Set args from clipboard</string>
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Preferred" vsizetype="Preferred">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
</property>
- </widget>
- </item>
- <item row="0" column="3">
- <widget class="QPushButton" name="pushButtonReset">
- <property name="text">
- <string>Reset args to profile</string>
+ <property name="orientation">
+ <enum>Qt::Vertical</enum>
</property>
- </widget>
- </item>
- <item row="3" column="0" colspan="4">
- <widget class="QTreeWidget" name="treeWidgetSkillDetails">
- <property name="contextMenuPolicy">
- <enum>Qt::CustomContextMenu</enum>
- </property>
- <property name="editTriggers">
- <set>QAbstractItemView::DoubleClicked|QAbstractItemView::EditKeyPressed</set>
+ <property name="childrenCollapsible">
+ <bool>false</bool>
</property>
- <column>
- <property name="text">
- <string>Key</string>
+ <widget class="QWidget" name="groupBoxSkillDetailsTop" native="true">
+ <layout class="QVBoxLayout" name="verticalLayout_2">
+ <property name="leftMargin">
+ <number>0</number>
</property>
- </column>
- <column>
- <property name="text">
- <string>Value</string>
+ <property name="topMargin">
+ <number>0</number>
</property>
- </column>
- <column>
- <property name="text">
- <string>Type</string>
+ <property name="rightMargin">
+ <number>0</number>
</property>
- </column>
- <column>
- <property name="text">
- <string>defaultValue (hidden in GUI)</string>
+ <property name="bottomMargin">
+ <number>0</number>
</property>
- </column>
+ <item>
+ <layout class="QGridLayout" name="gridLayout_5">
+ <item row="0" column="0">
+ <widget class="QPushButton" name="pushButtonPaste">
+ <property name="text">
+ <string>Set args from clipboard</string>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="3">
+ <widget class="QPushButton" name="pushButtonReset">
+ <property name="text">
+ <string>Reset args to profile</string>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="0" colspan="4">
+ <widget class="QComboBox" name="comboBoxProfiles">
+ <item>
+ <property name="text">
+ <string><No Profile selected. Using root></string>
+ </property>
+ </item>
+ </widget>
+ </item>
+ <item row="0" column="1">
+ <widget class="QPushButton" name="pushButtonCopy">
+ <property name="text">
+ <string>Copy args to clipboard</string>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="2">
+ <widget class="QLabel" name="label_2">
+ <property name="text">
+ <string/>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item>
+ <widget class="QWidget" name="skillDescription" native="true"/>
+ </item>
+ </layout>
</widget>
- </item>
- <item row="0" column="1">
- <widget class="QPushButton" name="pushButtonCopy">
- <property name="text">
- <string>Copy args to clipboard</string>
- </property>
- </widget>
- </item>
- <item row="1" column="1" colspan="3">
- <widget class="QComboBox" name="comboBoxProfiles">
- <item>
- <property name="text">
- <string><No Profile selected. Using root></string>
- </property>
- </item>
+ <widget class="QWidget" name="groupBoxSkillDetailsBottom" native="true">
+ <layout class="QVBoxLayout" name="verticalLayout_3">
+ <property name="leftMargin">
+ <number>0</number>
+ </property>
+ <property name="topMargin">
+ <number>0</number>
+ </property>
+ <property name="rightMargin">
+ <number>0</number>
+ </property>
+ <property name="bottomMargin">
+ <number>0</number>
+ </property>
+ <item>
+ <widget class="QTreeWidget" name="treeWidgetSkillDetails">
+ <property name="contextMenuPolicy">
+ <enum>Qt::CustomContextMenu</enum>
+ </property>
+ <property name="editTriggers">
+ <set>QAbstractItemView::DoubleClicked|QAbstractItemView::EditKeyPressed</set>
+ </property>
+ <column>
+ <property name="text">
+ <string>Key</string>
+ </property>
+ </column>
+ <column>
+ <property name="text">
+ <string>Value</string>
+ </property>
+ </column>
+ <column>
+ <property name="text">
+ <string>Type</string>
+ </property>
+ </column>
+ <column>
+ <property name="text">
+ <string>defaultValue (hidden in GUI)</string>
+ </property>
+ </column>
+ </widget>
+ </item>
+ <item>
+ <widget class="QPushButton" name="pushButtonExecuteSkill">
+ <property name="text">
+ <string>Request Execution</string>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </widget>
</widget>
</item>
</layout>
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidgetController.cpp b/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidgetController.cpp
index 13fd647d8be4948bc04a4ce4e4b05746e846f993..24064a8720bbf78111bf5f9244516d893e4910a0 100644
--- a/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidgetController.cpp
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidgetController.cpp
@@ -22,10 +22,11 @@
#include "SkillManagerMonitorWidgetController.h"
+#include <optional>
#include <regex>
#include <string>
-//#include <boost/algorithm/string.hpp>
+#include <SimoxUtility/algorithm/string.h>
#include <RobotAPI/libraries/skills/core/Skill.h>
@@ -34,21 +35,33 @@
#include "aronTreeWidget/visitors/AronTreeWidgetModalCreator.h"
// modals
+#include "aronTreeWidget/modal/text/AronTreeWidgetTextInputModalController.h"
+// debug
#include <QAction>
#include <QClipboard>
#include <QDoubleSpinBox>
+#include <QGridLayout>
#include <QMenu>
+#include <QTextBrowser>
#include <RobotAPI/libraries/aron/converter/json/NLohmannJSONConverter.h>
+#include <RobotAPI/libraries/skills/core/Skill.h>
#include <RobotAPI/libraries/skills/core/SkillExecutionRequest.h>
#include "aronTreeWidget/Data.h"
#include "aronTreeWidget/modal/text/AronTreeWidgetTextInputModalController.h"
+#include "aronTreeWidget/visitors/AronTreeWidgetConverter.h"
+#include "aronTreeWidget/visitors/AronTreeWidgetCreator.h"
+#include "aronTreeWidget/visitors/AronTreeWidgetModalCreator.h"
+#include "aronTreeWidget/widgets/SkillDescriptionWidget.h"
//configSk
namespace armarx
{
+ const skills::SkillID SkillManagerMonitorWidgetController::SelectedSkill::UNK_SKILL_ID =
+ skills::SkillID{.providerId = ::std::nullopt, .skillName = skills::SkillID::UNKNOWN};
+
QPointer<QDialog>
SkillManagerMonitorWidgetController::getConfigDialog(QWidget* parent)
{
@@ -146,7 +159,12 @@ namespace armarx
this->stopAllButton = new QPushButton("Stop all executions");
widget.stopAllLayout->addWidget(stopAllButton);
- this->currentSkillSearch = QString("");
+
+ skillDescriptionWidget = new SkillDescriptionWidget();
+ widget.skillDescription->parentWidget()->layout()->replaceWidget(widget.skillDescription,
+ skillDescriptionWidget);
+ widget.skillDescription = skillDescriptionWidget;
+
connect(this->stopAllButton,
&QPushButton::clicked,
@@ -271,8 +289,7 @@ namespace armarx
for (auto it = update.begin(); it != update.end();)
{
-
- if (boost::algorithm::to_lower_copy(skills::SkillID::FromIce(it->first).skillName)
+ if (simox::alg::to_lower(skills::SkillID::FromIce(it->first).skillName)
.find(this->currentSkillSearch.toLower().toStdString()))
{
update.erase(it++);
@@ -558,10 +575,10 @@ namespace armarx
char hostname[HOST_NAME_MAX];
gethostname(hostname, HOST_NAME_MAX);
- skills::SkillExecutionRequest req(selectedSkill.skillId,
- "Skills.Manager GUI (hostname: " + std::string(hostname) +
- ")",
- params);
+ skills::SkillExecutionRequest req{
+ .skillId = selectedSkill.skillId,
+ .executorName = "Skills.Manager GUI (hostname: " + std::string(hostname) + ")",
+ .parameters = params};
ARMARX_CHECK(selectedSkill.skillId.isFullySpecified()); // sanity check
ARMARX_IMPORTANT << "Executing skill from GUI: " << selectedSkill.skillId << ".";
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidgetController.h b/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidgetController.h
index df6f5b1efc658fa468419ccc5672b4d2a364650a..8aa3e4720fa87c0a20b6187a780c7ac62d77115f 100644
--- a/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidgetController.h
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidgetController.h
@@ -47,6 +47,8 @@
namespace armarx
{
+ class SkillDescriptionWidget;
+
class SkillInfoTreeWidgetItem : public QTreeWidgetItem
{
public:
@@ -163,18 +165,22 @@ namespace armarx
// User Input
struct SelectedSkill
{
+ static const skills::SkillID UNK_SKILL_ID;
+
skills::SkillID skillId;
skills::SkillExecutionID skillExecutionId;
// make default constructable
SelectedSkill() :
- skillId({}, skills::SkillID::UNKNOWN),
- skillExecutionId({{}, skills::SkillID::UNKNOWN},
- skills::SkillExecutionID::UNKNOWN,
- armarx::core::time::DateTime::Invalid())
+ skillId(UNK_SKILL_ID),
+ skillExecutionId{.skillId = UNK_SKILL_ID,
+ .executorName = skills::SkillExecutionID::UNKNOWN,
+ .executionStartedTime = armarx::core::time::DateTime::Invalid()}
{
}
- } selectedSkill;
+ }
+
+ selectedSkill;
void executeSkillWithParams(skills::SkillID skillId, aron::data::DictPtr params);
void matchSkillUpdateToSearch(std::map<skills::manager::dto::SkillID,
@@ -188,6 +194,7 @@ namespace armarx
// Helper to get the treeWidgetItem easily
QTreeWidgetItem* skillsArgumentsTreeWidgetItem = nullptr;
AronTreeWidgetControllerPtr aronTreeWidgetController = nullptr;
+ SkillDescriptionWidget* skillDescriptionWidget = nullptr;
// connected flag
std::atomic_bool connected = false;
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/widgets/SkillDescriptionWidget.cpp b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/widgets/SkillDescriptionWidget.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..60f5908c771cc6bfc5f3e5080445ec8321b613ac
--- /dev/null
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/widgets/SkillDescriptionWidget.cpp
@@ -0,0 +1,72 @@
+#include "SkillDescriptionWidget.h"
+
+#include <sstream>
+
+#include <QGridLayout>
+#include <QLabel>
+#include <QTextEdit>
+
+#include <ArmarXGui/libraries/ArmarXGuiBase/widgets/cpp-markdown/markdown.h> // ToDo: Move cpp-markdown to own Axii module.
+
+namespace armarx
+{
+
+ static std::string
+ markdownToHtml(const std::string& markdownText, size_t spacesPerTab = 2)
+ {
+ ::markdown::Document document(spacesPerTab);
+ document.read(markdownText);
+
+ std::stringstream html;
+ document.write(html);
+ return html.str();
+ }
+
+ SkillDescriptionWidget::SkillDescriptionWidget(QWidget* parent) : QWidget{parent}
+ {
+ QGridLayout* layout = new QGridLayout;
+ setLayout(layout);
+
+ layout->setMargin(0);
+
+ int row = 0;
+ int col = 0;
+
+ layout->addWidget(new QLabel("Name:"), row, col++);
+ name = new QLabel;
+ layout->addWidget(name, row, col++);
+
+ // Add some padding.
+ layout->addWidget(new QLabel(), row, col++);
+
+ {
+ QLabel* label = new QLabel("Timeout:");
+ label->setAlignment(Qt::AlignmentFlag::AlignRight);
+ layout->addWidget(label, row, col++);
+ }
+ timeout = new QLabel;
+ timeout->setAlignment(Qt::AlignmentFlag::AlignRight);
+ layout->addWidget(timeout, row, col++);
+
+ ++row;
+ int numCols = col;
+ col = 0;
+
+ description = new QTextEdit;
+ description->setReadOnly(true);
+ layout->addWidget(description, row, col, 1, numCols);
+ ++row;
+ }
+
+ void
+ SkillDescriptionWidget::setSkillDescription(const skills::SkillDescription& desc)
+ {
+ name->setText(QString::fromStdString(desc.skillId.skillName));
+ description->setHtml(QString::fromStdString(markdownToHtml(desc.description)));
+
+ std::stringstream timeoutStr;
+ timeoutStr << desc.timeout;
+ timeout->setText(QString::fromStdString(timeoutStr.str()));
+ }
+
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/widgets/SkillDescriptionWidget.h b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/widgets/SkillDescriptionWidget.h
new file mode 100644
index 0000000000000000000000000000000000000000..b58f06d0d6dc65893c2e7f65a4e768ef435d7201
--- /dev/null
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/widgets/SkillDescriptionWidget.h
@@ -0,0 +1,27 @@
+#pragma once
+
+#include <QWidget>
+
+#include <RobotAPI/libraries/skills/core/SkillDescription.h>
+
+class QLabel;
+class QTextEdit;
+
+namespace armarx
+{
+
+ class SkillDescriptionWidget : public QWidget
+ {
+ public:
+ SkillDescriptionWidget(QWidget* parent = nullptr);
+
+ void setSkillDescription(const skills::SkillDescription& desc);
+
+ private:
+ QLabel* name = nullptr;
+ QTextEdit* description = nullptr;
+ QLabel* timeout = nullptr;
+ };
+
+
+} // namespace armarx
diff --git a/source/RobotAPI/interface/CMakeLists.txt b/source/RobotAPI/interface/CMakeLists.txt
index 9e290598fee8047f853044e54ea3775b62ab49f7..d8f626d9fa4a752cd65eb7cbc95b3074e47c2638 100644
--- a/source/RobotAPI/interface/CMakeLists.txt
+++ b/source/RobotAPI/interface/CMakeLists.txt
@@ -92,7 +92,6 @@ set(SLICE_FILES
# Disabled for being unstable. To be replaced by skills/view_selection. Use GazeControl of ActiveVision instead.
# components/FrameTrackingInterface.ice
components/RobotHealthInterface.ice
- components/RobotNameServiceInterface.ice
components/TrajectoryPlayerInterface.ice
components/ViewSelectionInterface.ice
@@ -152,6 +151,9 @@ set(SLICE_FILES
skills/SkillProviderInterface.ice
mdb/MotionDatabase.ice
+
+ # RobotAPI
+ robot_name_service/RobotNameServiceInterface.ice
)
#core/RobotIK.ice
set(SLICE_FILES_ADDITIONAL_HEADERS
diff --git a/source/RobotAPI/interface/robot_name_service/RobotNameServiceInterface.ice b/source/RobotAPI/interface/robot_name_service/RobotNameServiceInterface.ice
new file mode 100644
index 0000000000000000000000000000000000000000..7891ed5dbfd4583eb592ebf8913d9c12ca3e9a4c
--- /dev/null
+++ b/source/RobotAPI/interface/robot_name_service/RobotNameServiceInterface.ice
@@ -0,0 +1,90 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2011-2016, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package
+ * @author
+ * @date
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+#pragma once
+
+#include <ArmarXCore/interface/core/PackagePath.ice>
+
+#include <RobotAPI/interface/armem/mns.ice>
+#include <RobotAPI/interface/skills/SkillManagerInterface.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>
+
+module armarx
+{
+ module robot_name_service
+ {
+ module dto
+ {
+
+ struct Hand
+ {
+ string name;
+ string ft_name;
+ HandUnitInterface* handUnitInterface;
+ };
+
+ dictionary<string, Hand> Hands;
+
+ struct Arm
+ {
+ string kinematicChainName;
+ Hand hand;
+ };
+
+ dictionary<string, Arm> Arms;
+
+ struct Robot
+ {
+ string name;
+ armarx::data::PackagePath xmlPackagePath;
+
+ // Memory and skills. MAY BE NULL
+ armarx::armem::mns::MemoryNameSystemInterface* memoryNameSystem;
+ armarx::skills::manager::dti::SkillManagerInterface* skillManager;
+
+ // kinematic stuff. MAY BE EMPTY
+ Arms arms;
+
+ // units. MAY BE NULL
+ armarx::KinematicUnitInterface* kinematicUnitInterface;
+ armarx::PlatformUnitInterface* platformUnitInterface;
+ armarx::LocalizationUnitInterface* localizationUnitInterface;
+
+ // TODO: Feel free to extend!
+ };
+ };
+
+ module dti
+ {
+ interface RobotNameServiceInterface
+ {
+ bool registerRobot(dto::Robot robot);
+ void unregisterRobot(string name);
+ optional(1) dto::Robot getRobot(string name);
+ };
+ };
+ };
+};
diff --git a/source/RobotAPI/libraries/CMakeLists.txt b/source/RobotAPI/libraries/CMakeLists.txt
index 057440c94c97f8428c191c2d9c7a83edc95cbf51..4d481f8619df7851446f95712d8dce766513c943 100644
--- a/source/RobotAPI/libraries/CMakeLists.txt
+++ b/source/RobotAPI/libraries/CMakeLists.txt
@@ -39,3 +39,5 @@ add_subdirectory(skills)
add_subdirectory(RobotUnitDataStreamingReceiver)
add_subdirectory(GraspingUtility)
add_subdirectory(obstacle_avoidance)
+
+add_subdirectory(robot_name_service)
diff --git a/source/RobotAPI/libraries/armem/client/util/SimpleReaderBase.cpp b/source/RobotAPI/libraries/armem/client/util/SimpleReaderBase.cpp
index e14ce8848fd1fe8242d681770563bf9cddefd7cd..650eae74f37e089c49a92b1592dff8e084a49be7 100644
--- a/source/RobotAPI/libraries/armem/client/util/SimpleReaderBase.cpp
+++ b/source/RobotAPI/libraries/armem/client/util/SimpleReaderBase.cpp
@@ -17,21 +17,25 @@ 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");
}
void
SimpleReaderBase::connect(armarx::armem::client::MemoryNameSystem& mns)
{
// Wait for the memory to become available and add it as dependency.
- ARMARX_IMPORTANT << "SimpleReaderBase: Waiting for memory '" << props.memoryName << "' ...";
+ ARMARX_IMPORTANT << "SimpleReaderBase: Waiting for memory '" << properties().memoryName
+ << "' ...";
try
{
- memoryReaderClient = mns.useReader(MemoryID().withMemoryName(props.memoryName));
- ARMARX_IMPORTANT << "SimpleReaderBase: Connected to memory '" << props.memoryName
+ memoryReaderClient = mns.useReader(MemoryID().withMemoryName(properties().memoryName));
+ ARMARX_IMPORTANT << "SimpleReaderBase: Connected to memory '" << properties().memoryName
<< "'";
}
catch (const armem::error::CouldNotResolveMemoryServer& e)
@@ -41,12 +45,6 @@ namespace armarx::armem::client::util
}
}
- std::mutex&
- SimpleReaderBase::memoryReaderMutex()
- {
- return memoryMutex;
- }
-
const armem::client::Reader&
SimpleReaderBase::memoryReader() const
{
@@ -56,7 +54,17 @@ namespace armarx::armem::client::util
const SimpleReaderBase::Properties&
SimpleReaderBase::properties() const
{
- return props;
+ if (not props.has_value())
+ {
+ const_cast<std::optional<Properties>&>(props) = defaultProperties();
+ }
+ return props.value();
+ }
+
+ void
+ SimpleReaderBase::setProperties(const Properties& p)
+ {
+ props = p;
}
} // namespace armarx::armem::client::util
diff --git a/source/RobotAPI/libraries/armem/client/util/SimpleReaderBase.h b/source/RobotAPI/libraries/armem/client/util/SimpleReaderBase.h
index 2b637193765215b81519c55397bcc51a7436041b..3e83956805c03705a90269a2dc565e14a6fdddf5 100644
--- a/source/RobotAPI/libraries/armem/client/util/SimpleReaderBase.h
+++ b/source/RobotAPI/libraries/armem/client/util/SimpleReaderBase.h
@@ -21,7 +21,7 @@
#pragma once
-#include <mutex>
+#include <optional>
#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
@@ -53,25 +53,18 @@ namespace armarx::armem::client::util
virtual void connect(armarx::armem::client::MemoryNameSystem& mns);
const Properties& properties() const;
-
- void
- setProperties(const Properties& p)
- {
- props = p;
- }
+ void setProperties(const Properties& p);
protected:
virtual std::string propertyPrefix() const = 0;
virtual Properties defaultProperties() const = 0;
- std::mutex& memoryReaderMutex();
const armem::client::Reader& memoryReader() const;
private:
- Properties props;
+ std::optional<Properties> props;
armem::client::Reader memoryReaderClient;
- std::mutex memoryMutex;
};
} // namespace armarx::armem::client::util
diff --git a/source/RobotAPI/libraries/armem_grasping/client/KnownGraspCandidateReader.h b/source/RobotAPI/libraries/armem_grasping/client/KnownGraspCandidateReader.h
index d0057afc8d23a30da6e5c6335143d58c638d0557..8d40f1077f46119e0ad7638c3cfc2ef157ed47db 100644
--- a/source/RobotAPI/libraries/armem_grasping/client/KnownGraspCandidateReader.h
+++ b/source/RobotAPI/libraries/armem_grasping/client/KnownGraspCandidateReader.h
@@ -56,7 +56,6 @@ namespace armarx::armem::grasping::known_grasps
const std::string propertyPrefix = "mem.grasping.knowngrasps.";
armem::client::Reader memoryReader;
- mutable std::mutex memoryWriterMutex;
};
diff --git a/source/RobotAPI/libraries/armem_objects/client/instance/ObjectWriter.h b/source/RobotAPI/libraries/armem_objects/client/instance/ObjectWriter.h
index d449502849b09de61e3abc65ab0364928502a083..da5744c594780644808d3f75b003cdb2d04d0465 100644
--- a/source/RobotAPI/libraries/armem_objects/client/instance/ObjectWriter.h
+++ b/source/RobotAPI/libraries/armem_objects/client/instance/ObjectWriter.h
@@ -57,7 +57,6 @@ namespace armarx::armem::obj::instance
const std::string propertyPrefix = "mem.obj.instance.";
armem::client::Writer memoryWriter;
- mutable std::mutex memoryWriterMutex;
};
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/CMakeLists.txt b/source/RobotAPI/libraries/armem_robot_state/server/CMakeLists.txt
index 3ad1d50684d89c246e6123a6ef5d2055779eab6f..54d5823876f19fb00a6e9e6ec57ecc89db0c3a13 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem_robot_state/server/CMakeLists.txt
@@ -20,10 +20,12 @@ armarx_add_library(
# This package
RobotAPICore
RobotAPIInterfaces
+ RobotStatechartHelpers
RobotAPI::armem_server
RobotAPI::armem_robot
RobotAPI::armem_robot_state
aroncommon
+ aroneigenconverter
# System / External
Eigen3::Eigen
@@ -38,6 +40,7 @@ armarx_add_library(
localization/Segment.h
proprioception/Segment.h
+ proprioception/SensorValues.h
proprioception/aron_conversions.h
proprioception/RobotStateWriter.h
proprioception/RobotUnitData.h
@@ -63,6 +66,7 @@ armarx_add_library(
localization/Segment.cpp
proprioception/Segment.cpp
+ proprioception/SensorValues.cpp
proprioception/aron_conversions.cpp
proprioception/RobotStateWriter.cpp
proprioception/RobotUnitData.cpp
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 41eaf6e2ec2f2bcead697ec6ca7e86868bca44c4..be19db6355d9c21e0b9a7e287e5889024a0c408c 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.cpp
@@ -8,6 +8,9 @@
#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>
#include <ArmarXCore/core/time/CycleUtil.h>
@@ -21,6 +24,7 @@
#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>
+#include <RobotAPI/libraries/armem_robot_state/server/proprioception/SensorValues.h>
#include "combine.h"
@@ -43,8 +47,12 @@ namespace armarx::armem::server::robot_state
void Visu::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
{
defs->optional(p.enabled, prefix + "enabled", "Enable or disable visualization of objects.");
- defs->optional(p.framesEnabled, prefix + "famesEnabled", "Enable or disable visualization of frames.");
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",
+ "Enable or disable visualization of force torque sensors.");
+ defs->optional(p.forceTorque.forceScale, prefix + "forceTorque.forceScale",
+ "Scaling of force arrows.");
}
@@ -191,11 +199,11 @@ namespace armarx::armem::server::robot_state
const auto frames = localizationSegment.getRobotFramePosesLocking(timestamp);
TIMING_END_STREAM(tRobotFramePoses, ARMARX_DEBUG);
- TIMING_START(tJointPositions);
- const auto jointPositions =
- proprioceptionSegment.getRobotJointPositionsLocking(
+ TIMING_START(tSensorValues);
+ const auto sensorValues =
+ proprioceptionSegment.getSensorValuesLocking(
timestamp, debugObserver ? &*debugObserver : nullptr);
- TIMING_END_STREAM(tJointPositions, ARMARX_DEBUG);
+ TIMING_END_STREAM(tSensorValues, ARMARX_DEBUG);
TIMING_END_STREAM(tVisuGetData, ARMARX_DEBUG);
@@ -211,10 +219,10 @@ namespace armarx::armem::server::robot_state
ARMARX_DEBUG << "Combining robot ..."
<< "\n- " << robotDescriptions.size() << " descriptions"
<< "\n- " << globalPoses.size() << " global poses"
- << "\n- " << jointPositions.size() << " joint positions";
+ << "\n- " << sensorValues.size() << " joint positions";
const robot::Robots robots =
- combine(robotDescriptions, globalPoses, jointPositions, timestamp);
+ combine(robotDescriptions, globalPoses, sensorValues, timestamp);
ARMARX_DEBUG << "Visualize " << robots.size() << " robots ...";
viz::Layer layer = arviz.layer("Robots");
@@ -237,6 +245,65 @@ namespace armarx::armem::server::robot_state
layers.push_back(layerFrames);
}
+ if (p.forceTorque.enabled)
+ {
+ viz::Layer layerFrames = arviz.layer("ForceTorque");
+ 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 proprioception::ForceTorqueValuesMap& forceTorques
+ = sensorValues.at(name).forceTorqueValuesMap;
+
+ for (const auto& [side, ft] : forceTorques)
+ {
+ 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.
+ }
+ }
+
+ }
+ layers.push_back(layerFrames);
+ }
+
// Commit layers.
@@ -255,7 +322,7 @@ namespace armarx::armem::server::robot_state
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 Joint Positions (ms)", tJointPositions.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());
}
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 99321b656be9977a23096e3b9bdd9ed4ba62a101..265f08e2a0c10153c1b3cc8c624fd214713d57a9 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.h
+++ b/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.h
@@ -33,6 +33,8 @@
#include <RobotAPI/libraries/armem_robot_state/server/forward_declarations.h>
+#include <RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.h>
+
namespace armarx::armem::server::robot_state
{
@@ -86,11 +88,23 @@ 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 Properties
{
bool enabled = true;
+ float frequencyHz = 25.f;
+
bool framesEnabled = false;
- float frequencyHz = 25;
+
+ struct ForceTorque
+ {
+ bool enabled = true;
+ float forceScale = 1.F;
+ };
+ ForceTorque forceTorque;
} p;
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/common/combine.cpp b/source/RobotAPI/libraries/armem_robot_state/server/common/combine.cpp
index abe3f0bbb024d971052aad031b366a3d54b15ffe..997253f25c8bce846e3dddfe8e14aa9f04414d78 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/common/combine.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/common/combine.cpp
@@ -2,6 +2,7 @@
#include <RobotAPI/libraries/armem_robot/types.h>
#include <RobotAPI/libraries/armem/core/Time.h>
+#include <RobotAPI/libraries/armem_robot_state/server/proprioception/SensorValues.h>
#include <ArmarXCore/core/logging/Logging.h>
@@ -15,7 +16,7 @@ namespace armarx::armem::server
robot_state::combine(
const description::RobotDescriptionMap& robotDescriptions,
const localization::RobotPoseMap& globalPoses,
- const proprioception::RobotJointPositionMap& jointPositions,
+ const proprioception::SensorValuesMap& sensorValues,
const armem::Time& timestamp)
{
std::stringstream logs;
@@ -42,9 +43,12 @@ namespace armarx::armem::server
{
logs << "\nNo global pose for robot '" << robotName << "'.";
}
- if (auto it = jointPositions.find(robotName); it != jointPositions.end())
+ if (auto it = sensorValues.find(robotName); it != sensorValues.end())
{
- robot.config.jointMap = it->second;
+ for (const auto& [name, values] : it->second.jointValueMap)
+ {
+ robot.config.jointMap.emplace(name, values.position);
+ }
}
else
{
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/common/combine.h b/source/RobotAPI/libraries/armem_robot_state/server/common/combine.h
index 811599b69d8fe853154520f5b72b4ca95eb9488f..b96ca240c552426651ed4871779ea747604706cf 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/common/combine.h
+++ b/source/RobotAPI/libraries/armem_robot_state/server/common/combine.h
@@ -32,7 +32,7 @@ namespace armarx::armem::server::robot_state
combine(
const description::RobotDescriptionMap& robotDescriptions,
const localization::RobotPoseMap& globalPoses,
- const proprioception::RobotJointPositionMap& jointPositions,
+ const proprioception::SensorValuesMap& sensorValues,
const armem::Time& timestamp);
} // namespace armarx::armem::server::robot_state
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/forward_declarations.h b/source/RobotAPI/libraries/armem_robot_state/server/forward_declarations.h
index e730e29f1d5c8e83f38701a2e26cfc7a55b23039..1b9c39543b21587f30a32feb2dfbbcac4fb5e257 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/forward_declarations.h
+++ b/source/RobotAPI/libraries/armem_robot_state/server/forward_declarations.h
@@ -74,7 +74,12 @@ namespace armarx::armem::server::robot_state::localization
namespace armarx::armem::server::robot_state::proprioception
{
- using RobotJointPositionMap = std::unordered_map<std::string, std::map<std::string, float>>;
+ struct JointValues;
+ struct ForceTorqueValues;
+ struct SensorValues;
+ using JointValuesMap = std::unordered_map<std::string, JointValues>;
+ using ForceTorqueValuesMap = std::unordered_map<std::string, ForceTorqueValues>;
+ using SensorValuesMap = std::unordered_map<std::string, SensorValues>;
class Segment;
}
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.cpp b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.cpp
index 99c330e7661d35c5b161c26e1d18b45af2d91ce9..d7136b1ef301a2cdc455fef4fa3f4dbcad5dd1d3 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.cpp
@@ -15,6 +15,9 @@
#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/memory_ids.h>
+#include "SensorValues.h"
+
+#include <RobotAPI/libraries/aron/converter/eigen/EigenConverter.h>
namespace armarx::armem::server::robot_state::proprioception
{
@@ -70,13 +73,13 @@ namespace armarx::armem::server::robot_state::proprioception
- RobotJointPositionMap Segment::getRobotJointPositionsLocking(
+ SensorValuesMap Segment::getSensorValuesLocking(
const armem::Time& timestamp,
DebugObserverHelper* debugObserver) const
{
return doLocked([this, ×tamp, &debugObserver]()
{
- return getRobotJointPositions(timestamp, debugObserver);
+ return getSensorValues(timestamp, debugObserver);
});
}
@@ -93,18 +96,18 @@ namespace armarx::armem::server::robot_state::proprioception
}
- RobotJointPositionMap
- Segment::getRobotJointPositions(
+ SensorValuesMap
+ Segment::getSensorValues(
const armem::Time& timestamp,
DebugObserverHelper* debugObserver) const
{
namespace adn = aron::data;
ARMARX_CHECK_NOT_NULL(segmentPtr);
- RobotJointPositionMap jointMap;
+ SensorValuesMap sensorValues;
int i = 0;
- Duration tFindData = Duration::MilliSeconds(0), tReadJointPositions = Duration::MilliSeconds(0);
+ Duration tFindData = Duration::MilliSeconds(0), tReadSensorValues = Duration::MilliSeconds(0);
TIMING_START(tProcessEntities)
segmentPtr->forEachEntity([&](const wm::Entity & entity)
{
@@ -128,12 +131,12 @@ namespace armarx::armem::server::robot_state::proprioception
}
if (data)
{
- TIMING_START(_tReadJointPositions)
+ TIMING_START(_tReadSensorValues)
- jointMap.emplace(entity.id().providerSegmentName, readJointPositions(*data));
+ sensorValues.emplace(entity.id().providerSegmentName, readSensorValues(*data));
- TIMING_END_COMMENT_STREAM(_tReadJointPositions, "tReadJointPositions " + std::to_string(i), ARMARX_DEBUG)
- tReadJointPositions += Duration::MicroSeconds(_tReadJointPositions.toMicroSeconds());
+ TIMING_END_COMMENT_STREAM(_tReadSensorValues, "tReadSensorValues " + std::to_string(i), ARMARX_DEBUG)
+ tReadSensorValues += Duration::MicroSeconds(_tReadSensorValues.toMicroSeconds());
}
++i;
});
@@ -143,10 +146,10 @@ namespace armarx::armem::server::robot_state::proprioception
{
debugObserver->setDebugObserverDatafield(dp + "t 1.1 Process Entities (ms)", tProcessEntities.toMilliSecondsDouble());
debugObserver->setDebugObserverDatafield(dp + "t 1.1.1 FindData (ms)", tFindData.toMilliSecondsDouble());
- debugObserver->setDebugObserverDatafield(dp + "t 1.1.2 ReadJointPositions (ms)", tReadJointPositions.toMilliSecondsDouble());
+ debugObserver->setDebugObserverDatafield(dp + "t 1.1.2 ReadSensorValues (ms)", tReadSensorValues.toMilliSecondsDouble());
}
- return jointMap;
+ return sensorValues;
}
@@ -277,25 +280,73 @@ namespace armarx::armem::server::robot_state::proprioception
}
- std::map<std::string, float>
- Segment::readJointPositions(const wm::EntityInstanceData& data)
+ SensorValues
+ Segment::readSensorValues(const wm::EntityInstanceData& data)
{
namespace adn = aron::data;
// Just get what we need without casting the whole data.
- std::map<std::string, float> jointPositions;
+ SensorValues sensorValues;
+ auto checkJVM = [&sensorValues](const std::string& name)
+ {
+ if (sensorValues.jointValueMap.find(name) == sensorValues.jointValueMap.end())
+ {
+ sensorValues.jointValueMap[name] = JointValues();
+ }
+ };
if (adn::DictPtr joints = getDictElement(data, "joints"))
{
- if (adn::DictPtr jointsPosition = getDictElement(*joints, "position"))
+ if (adn::DictPtr values = getDictElement(*joints, "position"))
+ {
+ for (const auto& [name, value] : values->getElements())
+ {
+ checkJVM(name);
+ sensorValues.jointValueMap[name].position
+ = adn::Float::DynamicCastAndCheck(value)->getValue();
+ }
+ }
+ if (adn::DictPtr values = getDictElement(*joints, "velocity"))
+ {
+ for (const auto& [name, value] : values->getElements())
+ {
+ checkJVM(name);
+ sensorValues.jointValueMap[name].velocity
+ = adn::Float::DynamicCastAndCheck(value)->getValue();
+ }
+ }
+ if (adn::DictPtr values = getDictElement(*joints, "torque"))
+ {
+ for (const auto& [name, value] : values->getElements())
+ {
+ checkJVM(name);
+ sensorValues.jointValueMap[name].velocity
+ = adn::Float::DynamicCastAndCheck(value)->getValue();
+ }
+ }
+ }
+ if (adn::DictPtr forceTorqueMap = getDictElement(data, "forceTorque"))
+ {
+ for (const auto& [name, value] : forceTorqueMap->getElements())
{
- for (const auto& [name, value] : jointsPosition->getElements())
+ if (adn::DictPtr forceTorqueValues = aron::data::Dict::DynamicCastAndCheck(value))
{
- const float jointPosition = adn::Float::DynamicCastAndCheck(value)->getValue();
- jointPositions[name] = jointPosition;
+ const Eigen::Vector3f torque
+ = armarx::aron::converter::AronEigenConverter::ConvertToVector3f(
+ adn::NDArray::DynamicCastAndCheck(forceTorqueValues->getElement("torque")));
+
+ const Eigen::Vector3f force
+ = armarx::aron::converter::AronEigenConverter::ConvertToVector3f(
+ adn::NDArray::DynamicCastAndCheck(forceTorqueValues->getElement("force")));
+
+ sensorValues.forceTorqueValuesMap[name] = ForceTorqueValues
+ {
+ .force = force,
+ .torque = torque
+ };
}
}
}
- return jointPositions;
+ return sensorValues;
}
} // namespace armarx::armem::server::robot_state::proprioception
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.h b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.h
index 8123aea55ea50a11d02b8bf1d50aee0fbe5b09dc..fbce59a996acb8b17ffb104095eadf570d02d535 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.h
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.h
@@ -60,9 +60,9 @@ namespace armarx::armem::server::robot_state::proprioception
void onConnect(RobotUnitInterfacePrx robotUnitPrx);
- RobotJointPositionMap getRobotJointPositions(
+ SensorValuesMap getSensorValues(
const armem::Time& timestamp, DebugObserverHelper* debugObserver = nullptr) const;
- RobotJointPositionMap getRobotJointPositionsLocking(
+ SensorValuesMap getSensorValuesLocking(
const armem::Time& timestamp, DebugObserverHelper* debugObserver = nullptr) const;
const armem::MemoryID& getRobotUnitProviderID() const;
@@ -73,8 +73,8 @@ namespace armarx::armem::server::robot_state::proprioception
private:
static
- std::map<std::string, float>
- readJointPositions(const wm::EntityInstanceData& data);
+ SensorValues
+ readSensorValues(const wm::EntityInstanceData& data);
private:
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/SensorValues.cpp b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/SensorValues.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..ee25d654eb5ee1e045d67ef5ea504ad4834bc8a5
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/SensorValues.cpp
@@ -0,0 +1,2 @@
+#include "SensorValues.h"
+
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/SensorValues.h b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/SensorValues.h
new file mode 100644
index 0000000000000000000000000000000000000000..2beb7608afb3a5710b761fce3651fe2dee81b73c
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/SensorValues.h
@@ -0,0 +1,50 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Andre Meixner ( andre dot meixner at kit dot edu )
+ * @date 2023
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <RobotAPI/libraries/armem_robot_state/server/forward_declarations.h>
+
+namespace armarx::armem::server::robot_state::proprioception
+{
+
+struct JointValues
+{
+ double position = 0.0;
+ double velocity = 0.0;
+ double torque = 0.0;
+ //double torqueTicks = 0.0;
+};
+
+struct ForceTorqueValues
+{
+ Eigen::Vector3f force = Eigen::Vector3f::Zero();
+ Eigen::Vector3f torque = Eigen::Vector3f::Zero();
+};
+
+struct SensorValues
+{
+ JointValuesMap jointValueMap;
+ ForceTorqueValuesMap forceTorqueValuesMap;
+};
+
+}
+
diff --git a/source/RobotAPI/libraries/aron/codegeneration/typereader/xml/Reader.cpp b/source/RobotAPI/libraries/aron/codegeneration/typereader/xml/Reader.cpp
index 7e9c71cbecf3c5dd9d8952135b61625a84ce1a27..1955a76581429fd9b327fce7d3f0d23f9f64e8b8 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/typereader/xml/Reader.cpp
+++ b/source/RobotAPI/libraries/aron/codegeneration/typereader/xml/Reader.cpp
@@ -21,25 +21,19 @@
* GNU General Public License
*/
-// STD/STL
-
-
-// Header
#include "Reader.h"
+
#include <sstream>
-// Simox
#include <SimoxUtility/algorithm/string/string_tools.h>
#include <SimoxUtility/algorithm/vector.hpp>
-// ArmarX
#include <ArmarXCore/core/rapidxml/wrapper/RapidXmlReader.h>
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
#include <RobotAPI/libraries/aron/codegeneration/typereader/xml/Data.h>
#include <RobotAPI/libraries/aron/core/type/variant/Factory.h>
-
namespace armarx::aron::typereader::xml
{
namespace fs = std::filesystem;
@@ -47,12 +41,14 @@ namespace armarx::aron::typereader::xml
namespace
{
/// Resolve a relative package path
- std::optional<fs::path> resolveRelativePackagePath(const fs::path& path, const std::vector<fs::path>& includePaths)
+ std::optional<fs::path>
+ resolveRelativePackagePath(const fs::path& filepath,
+ const std::vector<fs::path>& includePaths)
{
// new behavior: using provided include paths
for (const auto& includePath : includePaths)
{
- fs::path absPath = includePath / path;
+ fs::path absPath = includePath / filepath;
if (fs::is_regular_file(absPath))
{
// path is valid
@@ -61,27 +57,89 @@ namespace armarx::aron::typereader::xml
}
// legacy behavior: using cmake package finder paths
- const std::string package = *path.begin();
- armarx::CMakePackageFinder finder(package);
- if (finder.packageFound())
+
+ std::vector<std::string> packageNameCandidates;
+ {
+ auto it = filepath.begin();
+
+ std::string packageName = *it;
+ packageNameCandidates.push_back(packageName);
+
+ ++it;
+ if (it != filepath.end())
+ {
+ packageName += "_" + it->string();
+ packageNameCandidates.push_back(packageName);
+ }
+ }
+
+ std::vector<armarx::CMakePackageFinder> finders;
+ size_t foundFinderIndex = 0;
+ size_t numberOfFoundCandidates = 0;
+ for (const std::string& packageName : packageNameCandidates)
+ {
+ armarx::CMakePackageFinder& finder = finders.emplace_back(packageName);
+ if (finder.packageFound())
+ {
+ // TODO: In theory, we could also consider whether the requested file exists in
+ // the found package (see check below).
+ numberOfFoundCandidates++;
+ foundFinderIndex = finders.size() - 1;
+ }
+ }
+
+ if (numberOfFoundCandidates == 0)
+ {
+ std::stringstream msg;
+ msg << "No package matching the ARON include " << filepath
+ << " found. Tried CMake project names:";
+ for (const std::string& packageName : packageNameCandidates)
+ {
+ msg << " '" << packageName << "',";
+ }
+ throw error::AronException(__PRETTY_FUNCTION__, msg.str());
+ }
+ if (numberOfFoundCandidates > 1)
{
- for (const std::string& includePath : finder.getIncludePathList())
+ std::stringstream msg;
+ msg << "The ARON include " << filepath << " is ambiguous: The following "
+ << numberOfFoundCandidates
+ << " CMake projects matching the include were found:";
+ ARMARX_CHECK_EQUAL(packageNameCandidates.size(), finders.size());
+ for (size_t i = 0; i < packageNameCandidates.size(); ++i)
{
- fs::path absPath = includePath / path;
- if (fs::is_regular_file(absPath))
+ if (finders[i].packageFound())
{
- // path is valid
- return absPath;
+ msg << "\n- project '" << packageNameCandidates[i] << "' at '"
+ << finders[i].getPackageDir() << "'";
}
}
- return std::nullopt;
+ throw error::AronException(__PRETTY_FUNCTION__, msg.str());
+ }
+
+ ARMARX_CHECK_EQUAL(numberOfFoundCandidates, 1);
+ ARMARX_CHECK_LESS(foundFinderIndex, finders.size());
+
+ CMakePackageFinder& finder = finders.at(foundFinderIndex);
+ ARMARX_CHECK(finder.packageFound());
+
+ for (const std::string& includePath : finder.getIncludePathList())
+ {
+ fs::path absPath = includePath / filepath;
+ if (fs::is_regular_file(absPath))
+ {
+ // path is valid
+ return absPath;
+ }
}
return std::nullopt;
}
- }
+ } // namespace
- void Reader::parseFile(const std::string& _filename, const std::vector<std::filesystem::path>& includePaths)
+ void
+ Reader::parseFile(const std::string& _filename,
+ const std::vector<std::filesystem::path>& includePaths)
{
std::string filename = _filename;
@@ -93,7 +151,9 @@ namespace armarx::aron::typereader::xml
parseFile(std::filesystem::path(filename), includePaths);
}
- void Reader::parseFile(const std::filesystem::path& _file, const std::vector<std::filesystem::path>& includePaths)
+ void
+ Reader::parseFile(const std::filesystem::path& _file,
+ const std::vector<std::filesystem::path>& includePaths)
{
fs::path file = _file;
if (not std::filesystem::exists(file))
@@ -102,7 +162,10 @@ namespace armarx::aron::typereader::xml
auto p = resolveRelativePackagePath(file, includePaths);
if (not p)
{
- throw error::AronException(__PRETTY_FUNCTION__, "Could not find the file " + file.string() + ". Tried include paths: " + simox::alg::to_string(includePaths));
+ throw error::AronException(
+ __PRETTY_FUNCTION__,
+ "Could not find the file " + file.string() +
+ ". Tried include paths: " + simox::alg::to_string(includePaths));
}
file = *p;
}
@@ -112,7 +175,10 @@ namespace armarx::aron::typereader::xml
}
// private method reading nodes
- void Reader::parse(const RapidXmlReaderPtr& reader, const std::filesystem::path& filePath, const std::vector<std::filesystem::path>& includePaths)
+ void
+ Reader::parse(const RapidXmlReaderPtr& reader,
+ const std::filesystem::path& filePath,
+ const std::vector<std::filesystem::path>& includePaths)
{
RapidXmlReaderNode root = reader->getRoot();
@@ -132,7 +198,10 @@ namespace armarx::aron::typereader::xml
for (const auto& include : (*codeincludes).nodes())
{
auto i = readCodeInclude(include, filePath.parent_path(), includePaths);
- if (not i.empty()) this->systemIncludes.push_back(i);
+ if (not i.empty())
+ {
+ this->systemIncludes.push_back(i);
+ }
}
}
@@ -142,7 +211,10 @@ namespace armarx::aron::typereader::xml
for (const auto& include : (*aronincludes).nodes())
{
auto i = readAronInclude(include, filePath.parent_path(), includePaths);
- if (not i.empty()) this->aronIncludes.push_back(i);
+ if (not i.empty())
+ {
+ this->aronIncludes.push_back(i);
+ }
}
}
@@ -151,10 +223,14 @@ namespace armarx::aron::typereader::xml
{
for (const auto& include : (*includes).nodes())
{
- if (util::HasTagName(include, constantes::SYSTEM_INCLUDE_TAG)) // if its a system include tag then we know that it must be a code include
+ if (util::HasTagName(
+ include,
+ constantes::
+ SYSTEM_INCLUDE_TAG)) // if its a system include tag then we know that it must be a code include
{
auto i = readCodeInclude(include, filePath.parent_path(), includePaths);
- if (not i.empty()) this->systemIncludes.push_back(i);
+ if (not i.empty())
+ this->systemIncludes.push_back(i);
}
else
{
@@ -171,17 +247,20 @@ namespace armarx::aron::typereader::xml
what = util::GetAttribute(include, constantes::INCLUDE_ATTRIBUTE_NAME);
}
- if (not what.empty() && std::filesystem::path(what) != filePath) // did we found something?
+ if (not what.empty() &&
+ std::filesystem::path(what) != filePath) // did we found something?
{
if (simox::alg::ends_with(what, ARON_FILE_SUFFIX))
{
auto i = readAronInclude(include, filePath.parent_path(), includePaths);
- if (not i.empty()) this->aronIncludes.push_back(i);
+ if (not i.empty())
+ this->aronIncludes.push_back(i);
}
else // we believe that this is a code include since it is not an xml file
{
auto i = readCodeInclude(include, filePath.parent_path(), includePaths);
- if (not i.empty()) this->systemIncludes.push_back(i);
+ if (not i.empty())
+ this->systemIncludes.push_back(i);
}
}
}
@@ -195,13 +274,18 @@ namespace armarx::aron::typereader::xml
{
if (util::HasTagName(generateType, constantes::OBJECT_TAG))
{
- for (const auto& additionalInclude : getAdditionalIncludesFromReplacements(generateType, filePath.parent_path()))
+ for (const auto& additionalInclude : getAdditionalIncludesFromReplacements(
+ generateType, filePath.parent_path()))
{
- RapidXmlReaderPtr reader = RapidXmlReader::FromXmlString("<PackagePath package=\""+additionalInclude.first+"\" path=\""+additionalInclude.second+"\"/>");
+ RapidXmlReaderPtr reader = RapidXmlReader::FromXmlString(
+ "<PackagePath package=\"" + additionalInclude.first + "\" path=\"" +
+ additionalInclude.second + "\"/>");
auto node = reader->getRoot();
auto i = this->readAronInclude(node, filePath, includePaths);
- if (not i.empty() && std::find(this->aronIncludes.begin(), this->aronIncludes.end(), i) == this->aronIncludes.end())
+ if (not i.empty() &&
+ std::find(this->aronIncludes.begin(), this->aronIncludes.end(), i) ==
+ this->aronIncludes.end())
{
this->aronIncludes.push_back(i);
}
@@ -218,26 +302,37 @@ namespace armarx::aron::typereader::xml
if (util::HasTagName(generateType, constantes::OBJECT_TAG))
{
const auto nav = readGenerateObject(generateType);
- generateObjects.push_back(factory.allGeneratedPublicObjects.at(nav->getObjectName()));
+ generateObjects.push_back(
+ factory.allGeneratedPublicObjects.at(nav->getObjectName()));
continue;
}
if (util::HasTagName(generateType, constantes::INT_ENUM_TAG))
{
const auto nav = readGenerateIntEnum(generateType);
- generateIntEnums.push_back(factory.allGeneratedPublicIntEnums.at(nav->getEnumName()));
+ generateIntEnums.push_back(
+ factory.allGeneratedPublicIntEnums.at(nav->getEnumName()));
continue;
}
- throw error::ValueNotValidException("XMLReader", "parse", "Could not find a valid tag inside generatetypes", generateType.name());
+ throw error::ValueNotValidException(
+ "XMLReader",
+ "parse",
+ "Could not find a valid tag inside generatetypes",
+ generateType.name());
}
}
else
{
- throw error::AronException(__PRETTY_FUNCTION__, "No generate types found in aron xml '" + filePath.string() + "'.");
+ throw error::AronException(__PRETTY_FUNCTION__,
+ "No generate types found in aron xml '" + filePath.string() +
+ "'.");
}
}
- std::pair<std::string, std::string> Reader::readPackagePathInclude(const RapidXmlReaderNode& node, const std::filesystem::path&, const std::vector<std::filesystem::path>& includePaths)
+ std::pair<std::string, std::string>
+ Reader::readPackagePathInclude(const RapidXmlReaderNode& node,
+ const std::filesystem::path&,
+ const std::vector<std::filesystem::path>& includePaths)
{
util::EnforceTagName(node, constantes::PACKAGE_PATH_TAG);
std::string package = util::GetAttribute(node, constantes::PACKAGE_ATTRIBUTE_NAME);
@@ -250,15 +345,23 @@ namespace armarx::aron::typereader::xml
path = simox::alg::replace_all(path, ">", "");
const std::filesystem::path includepath(package + "/" + path);
- if (std::optional<fs::path> resolvedPackagePath = resolveRelativePackagePath(includepath, includePaths); resolvedPackagePath.has_value())
+ if (std::optional<fs::path> resolvedPackagePath =
+ resolveRelativePackagePath(includepath, includePaths);
+ resolvedPackagePath.has_value())
{
return {package + "/" + path, "<" + package + "/" + path + ">"};
}
- throw error::AronException(__PRETTY_FUNCTION__, "Could not find an file `" + includepath.string() + "`. Search paths are: " + simox::alg::to_string(includePaths));
+ throw error::AronException(
+ __PRETTY_FUNCTION__,
+ "Could not find an file `" + includepath.string() +
+ "`. Search paths are: " + simox::alg::to_string(includePaths));
}
- std::pair<std::string, std::string> Reader::readInclude(const RapidXmlReaderNode& node, const std::filesystem::path& filepath, const std::vector<std::filesystem::path>& includePaths)
+ std::pair<std::string, std::string>
+ Reader::readInclude(const RapidXmlReaderNode& node,
+ const std::filesystem::path& filepath,
+ const std::vector<std::filesystem::path>& includePaths)
{
util::EnforceTagName(node, constantes::INCLUDE_TAG);
std::string value = util::GetAttribute(node, constantes::INCLUDE_ATTRIBUTE_NAME);
@@ -270,7 +373,10 @@ namespace armarx::aron::typereader::xml
return {value, "<" + value + ">"};
}
- std::pair<std::string, std::string> Reader::readSystemInclude(const RapidXmlReaderNode& node, const std::filesystem::path&, const std::vector<std::filesystem::path>& includePaths)
+ std::pair<std::string, std::string>
+ Reader::readSystemInclude(const RapidXmlReaderNode& node,
+ const std::filesystem::path&,
+ const std::vector<std::filesystem::path>& includePaths)
{
util::EnforceTagName(node, constantes::SYSTEM_INCLUDE_TAG);
std::string value = util::GetAttribute(node, constantes::INCLUDE_ATTRIBUTE_NAME);
@@ -282,7 +388,10 @@ namespace armarx::aron::typereader::xml
return {"", "<" + value + ">"};
}
- std::string Reader::readCodeInclude(const RapidXmlReaderNode& node, const std::filesystem::path& filepath, const std::vector<std::filesystem::path>& includePaths)
+ std::string
+ Reader::readCodeInclude(const RapidXmlReaderNode& node,
+ const std::filesystem::path& filepath,
+ const std::vector<std::filesystem::path>& includePaths)
{
if (util::HasTagName(node, constantes::PACKAGE_PATH_TAG))
{
@@ -297,10 +406,14 @@ namespace armarx::aron::typereader::xml
return readSystemInclude(node, filepath, includePaths).second;
}
- throw error::ValueNotValidException(__PRETTY_FUNCTION__, "Could not parse a code include. The tag is wrong.", node.name());
+ throw error::ValueNotValidException(
+ __PRETTY_FUNCTION__, "Could not parse a code include. The tag is wrong.", node.name());
}
- std::string Reader::readAronInclude(const RapidXmlReaderNode& node, const std::filesystem::path& filepath, const std::vector<std::filesystem::path>& includePaths)
+ std::string
+ Reader::readAronInclude(const RapidXmlReaderNode& node,
+ const std::filesystem::path& filepath,
+ const std::vector<std::filesystem::path>& includePaths)
{
std::string include;
std::string include_unescaped;
@@ -320,7 +433,8 @@ namespace armarx::aron::typereader::xml
if (not include.empty()) // we found something
{
// autoinclude the code file (suffix will be replaced by correct language)
- std::string codeinclude = simox::alg::replace_last(include, ARON_FILE_SUFFIX, CODE_FILE_SUFFIX);
+ std::string codeinclude =
+ simox::alg::replace_last(include, ARON_FILE_SUFFIX, CODE_FILE_SUFFIX);
this->systemIncludes.push_back(codeinclude);
// parse parent xml file and add objects to alreday known
@@ -342,17 +456,22 @@ namespace armarx::aron::typereader::xml
return include;
}
- throw error::ValueNotValidException(__PRETTY_FUNCTION__, "Could not parse an aron include. The tag is wrong.", node.name());
+ throw error::ValueNotValidException(
+ __PRETTY_FUNCTION__, "Could not parse an aron include. The tag is wrong.", node.name());
}
- std::vector<std::pair<std::string, std::string>> Reader::getAdditionalIncludesFromReplacements(const RapidXmlReaderNode& node, const std::filesystem::path& filePath)
+ std::vector<std::pair<std::string, std::string>>
+ Reader::getAdditionalIncludesFromReplacements(const RapidXmlReaderNode& node,
+ const std::filesystem::path& filePath)
{
std::vector<std::pair<std::string, std::string>> ret;
for (const auto& repl : constantes::REPLACEMENTS)
{
auto replacement = repl.second;
- if (not replacement.additionalAronDTOXMLIncludePackagePath.first.empty() && not replacement.additionalAronDTOXMLIncludePackagePath.second.empty() && util::HasTagName(node, repl.first))
+ if (not replacement.additionalAronDTOXMLIncludePackagePath.first.empty() &&
+ not replacement.additionalAronDTOXMLIncludePackagePath.second.empty() &&
+ util::HasTagName(node, repl.first))
{
// we found a string that will be replaced so we might need to add a default include
ret.push_back(replacement.additionalAronDTOXMLIncludePackagePath);
@@ -374,15 +493,17 @@ namespace armarx::aron::typereader::xml
return ret;
}
- type::ObjectPtr Reader::readGenerateObject(const RapidXmlReaderNode& node)
+ type::ObjectPtr
+ Reader::readGenerateObject(const RapidXmlReaderNode& node)
{
util::EnforceTagName(node, constantes::OBJECT_TAG);
return type::Object::DynamicCastAndCheck(factory.create(node, Path()));
}
- type::IntEnumPtr Reader::readGenerateIntEnum(const RapidXmlReaderNode& node)
+ type::IntEnumPtr
+ Reader::readGenerateIntEnum(const RapidXmlReaderNode& node)
{
util::EnforceTagName(node, constantes::INT_ENUM_TAG);
return type::IntEnum::DynamicCastAndCheck(factory.create(node, Path()));
}
-}
+} // namespace armarx::aron::typereader::xml
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/container/Dict.cpp b/source/RobotAPI/libraries/aron/core/data/variant/container/Dict.cpp
index 1d85a7f800b038befc8dedd21561c133a1f13439..3e9f067c989d57087dc7311c73c8140cc7c1828a 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/container/Dict.cpp
+++ b/source/RobotAPI/libraries/aron/core/data/variant/container/Dict.cpp
@@ -261,9 +261,8 @@ namespace armarx::aron::data
const auto& p = data->getPath();
if (not p.hasDirectPrefix(this->getPath()))
{
- ARMARX_WARNING
- << "An element added to a dict does not have a correct path set. This "
- "may cause errors. Please use setElemetCopy() instead.";
+ ARMARX_DEBUG << "An element added to a dict does not have a correct path set. This "
+ "may cause errors. Please use setElemetCopy() instead.";
}
}
diff --git a/source/RobotAPI/libraries/aron_component_config/RemoteGuiVisitors.cpp b/source/RobotAPI/libraries/aron_component_config/RemoteGuiVisitors.cpp
index 89499311e289b0d50bdba2f29637e5b7dc2087d6..b76afea2912134e0ac4f8aed12beb7aceb454603 100644
--- a/source/RobotAPI/libraries/aron_component_config/RemoteGuiVisitors.cpp
+++ b/source/RobotAPI/libraries/aron_component_config/RemoteGuiVisitors.cpp
@@ -167,39 +167,62 @@ namespace armarx::aron::component_config
MakeConfigGuiVisitor::visitListOnEnter(DataInput& o, TypeInput& t)
{
in_list_ = true;
- auto group = RemoteGui::makeSimpleGridLayout(pathToName(o) + "_grid").cols(20);
- auto data = data::List::DynamicCastAndCheck(o);
auto type = type::List::DynamicCast(t)->getAcceptedType()->getDescriptor();
- if (std::find(implementedListDescriptors.begin(), implementedListDescriptors.end(), type) ==
+
+ if (std::find(implementedListDescriptors.begin(), implementedListDescriptors.end(), type) !=
implementedListDescriptors.end())
{
- return;
+ auto group = RemoteGui::makeSimpleGridLayout(pathToName(o) + "_grid").cols(20);
+ auto data = data::List::DynamicCastAndCheck(o);
+
+ for (const auto& el : data->getElements())
+ {
+ group.addChild(RemoteGui::makeLineEdit(pathToName(el))
+ .value(factories::VariantHelper::make(type)->to_string(el)),
+ 10);
+ group.addHSpacer(8);
+ group.addChild(RemoteGui::makeButton(pathToName(el) + "_button")
+ .label("-")
+ .toolTip("Remove List Element"),
+ 2);
+ }
+ group_hierarchy_.back()->addChild(
+ RemoteGui::makeGroupBox(pathToName(o) + "_grp")
+ .label(o->getPath().getLastElement())
+ .collapsed(true)
+ .addChild(group)
+ .addChild(RemoteGui::makeButton(pathToName(o) + "_add")
+ .label("+")
+ .toolTip("Add new list entry.")));
}
- for (const auto& el : data->getElements())
+ else
{
- group.addChild(RemoteGui::makeLineEdit(pathToName(el))
- .value(factories::VariantHelper::make(type)->to_string(el)),
- 10);
- group.addHSpacer(8);
- group.addChild(RemoteGui::makeButton(pathToName(el) + "_button")
- .label("-")
- .toolTip("Remove List Element"),
- 2);
+ std::string name = pathToName(o);
+ group_hierarchy_.emplace_back(std::make_shared<RemoteGui::detail::GroupBoxBuilder>(
+ RemoteGui::makeGroupBox(name)));
}
- group_hierarchy_.back()->addChild(
- RemoteGui::makeGroupBox(pathToName(o) + "_grp")
- .label(o->getPath().getLastElement())
- .collapsed(true)
- .addChild(group)
- .addChild(RemoteGui::makeButton(pathToName(o) + "_add")
- .label("+")
- .toolTip("Add new list entry.")));
}
void
- MakeConfigGuiVisitor::visitListOnExit(DataInput& /*unused*/, TypeInput& /*unused*/)
+ MakeConfigGuiVisitor::visitListOnExit(DataInput& /*unused*/, TypeInput& t)
{
in_list_ = false;
+
+ auto type = type::List::DynamicCast(t)->getAcceptedType()->getDescriptor();
+
+ if (std::find(implementedListDescriptors.begin(), implementedListDescriptors.end(), type) !=
+ implementedListDescriptors.end())
+ {
+ }
+ else
+ {
+ auto builder = *group_hierarchy_.back();
+ group_hierarchy_.pop_back();
+ if (not group_hierarchy_.empty())
+ {
+ group_hierarchy_.back()->addChild(builder);
+ }
+ }
}
void
@@ -232,7 +255,7 @@ namespace armarx::aron::component_config
group.addChild(RemoteGui::makeLabel(name + "_label").value(q->getPath().getLastElement()));
group.addHSpacer();
- if (cols == 4 && rows == 4)
+ if (cols == 4 and rows == 4)
{
// Poses
const auto& matrix = aron::data::converter::AronEigenConverter::ConvertToMatrix4f(data);
@@ -488,36 +511,55 @@ namespace armarx::aron::component_config
GetValueFromMapVisitor::visitListOnEnter(DataInput& o, TypeInput& t)
{
in_list_ = true;
- auto data = data::List::DynamicCastAndCheck(o);
+
auto type = type::List::DynamicCast(t)->getAcceptedType()->getDescriptor();
- if (std::find(implementedListDescriptors.begin(), implementedListDescriptors.end(), type) ==
+
+ if (std::find(implementedListDescriptors.begin(), implementedListDescriptors.end(), type) !=
implementedListDescriptors.end())
{
- return;
- }
- const auto& elements = data->getElements();
- for (const auto& [idx, el] : armarx::MakeIndexedContainer(elements))
- {
- if (proxy_->getButtonClicked(pathToName(el) + "_button"))
+ auto data = data::List::DynamicCastAndCheck(o);
+
+ const auto& elements = data->getElements();
+ for (const auto& [idx, el] : armarx::MakeIndexedContainer(elements))
{
- data->removeElement(idx);
+ if (proxy_->getButtonClicked(pathToName(el) + "_button"))
+ {
+ data->removeElement(idx);
+ tab_rebuild_required_ = true;
+ }
+ auto gui_value = proxy_->getValue<std::string>(pathToName(el)).get();
+ factories::VariantHelper::make(type)->set_value_from_string(el, gui_value);
+ }
+ if (proxy_->getButtonClicked(pathToName(o) + "_add"))
+ {
+ data->addElement(factories::VariantHelper::make(type)->from_string(
+ "", o->getPath().withIndex(data->childrenSize())));
tab_rebuild_required_ = true;
}
- auto gui_value = proxy_->getValue<std::string>(pathToName(el)).get();
- factories::VariantHelper::make(type)->set_value_from_string(el, gui_value);
}
- if (proxy_->getButtonClicked(pathToName(o) + "_add"))
+ else
{
- data->addElement(factories::VariantHelper::make(type)->from_string(
- "", o->getPath().withIndex(data->childrenSize())));
- tab_rebuild_required_ = true;
+ // TODO? Adapt to additional GroupBoxBuilder added by
+ // MakeConfigGuiVisitor::visitListOnEnter in this case?
}
}
void
- GetValueFromMapVisitor::visitListOnExit(DataInput&, TypeInput&)
+ GetValueFromMapVisitor::visitListOnExit(DataInput&, TypeInput& t)
{
in_list_ = false;
+
+ auto type = type::List::DynamicCast(t)->getAcceptedType()->getDescriptor();
+
+ if (std::find(implementedListDescriptors.begin(), implementedListDescriptors.end(), type) !=
+ implementedListDescriptors.end())
+ {
+ }
+ else
+ {
+ // TODO? Adapt to additional GroupBoxBuilder added by
+ // MakeConfigGuiVisitor::visitListOnEnter in this case?
+ }
}
bool
diff --git a/source/RobotAPI/libraries/robotapi/CMakeLists.txt b/source/RobotAPI/libraries/robot_name_service/CMakeLists.txt
similarity index 100%
rename from source/RobotAPI/libraries/robotapi/CMakeLists.txt
rename to source/RobotAPI/libraries/robot_name_service/CMakeLists.txt
diff --git a/source/RobotAPI/libraries/robot_name_service/client/CMakeLists.txt b/source/RobotAPI/libraries/robot_name_service/client/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..c59cbcaa176ff20936c9ef81a2c0f4f68b3a65d5
--- /dev/null
+++ b/source/RobotAPI/libraries/robot_name_service/client/CMakeLists.txt
@@ -0,0 +1,27 @@
+set(LIB_NAME robot_name_service_client)
+
+armarx_component_set_name("${LIB_NAME}")
+armarx_set_target("Library: ${LIB_NAME}")
+
+armarx_add_library(
+ LIBS
+ ArmarXCoreInterfaces
+ ArmarXCore
+ ArmarXCoreObservers
+
+ RobotAPI::Core
+ RobotAPI::skills::core
+ RobotAPIInterfaces
+ armem
+ robot_name_service_core
+
+
+ SOURCES
+ plugins.cpp
+ Plugin.cpp
+ HEADERS
+ plugins.h
+ Plugin.h
+)
+
+add_library(RobotAPI::robot_name_service_client ALIAS robot_name_service_client)
diff --git a/source/RobotAPI/libraries/robot_name_service/client/Plugin.cpp b/source/RobotAPI/libraries/robot_name_service/client/Plugin.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..89f973a6e3f4588096a977091cdcbbeb4742bc9a
--- /dev/null
+++ b/source/RobotAPI/libraries/robot_name_service/client/Plugin.cpp
@@ -0,0 +1,145 @@
+#include "Plugin.h"
+
+#include <ArmarXCore/core/Component.h>
+
+namespace armarx
+{
+
+ RobotNameServiceComponentPluginUser::RobotNameServiceComponentPluginUser()
+ {
+ addPlugin(plugin);
+ }
+} // namespace armarx
+
+namespace armarx::plugins
+{
+
+ void
+ RobotNameServiceComponentPlugin::postOnInitComponent()
+ {
+ using namespace armarx::robot_name_service::client::constants;
+
+ nlohmann::json config = nlohmann::json::parse(properties.configJson);
+
+ if (config.contains(CONFIG_ROBOT_NAME))
+ {
+ robot.name = config[CONFIG_ROBOT_NAME];
+ }
+
+ if (config.contains(CONFIG_XML_PACKAGE_PATH))
+ {
+ auto config_pp = config[CONFIG_XML_PACKAGE_PATH];
+ if (config_pp.contains(CONFIG_XML_PACKAGE_PATH_PACKAGE))
+ {
+ robot.xmlPackagePath.package = config_pp[CONFIG_XML_PACKAGE_PATH_PACKAGE];
+ }
+ if (config_pp.contains(CONFIG_XML_PACKAGE_PATH_PATH))
+ {
+ robot.xmlPackagePath.path = config_pp[CONFIG_XML_PACKAGE_PATH_PATH];
+ }
+ }
+ auto& p = parent<RobotNameServiceComponentPluginUser>();
+
+ if (config.contains(CONFIG_MNS_PRX))
+ {
+ p.usingProxy(config[CONFIG_MNS_PRX]);
+ }
+ if (config.contains(CONFIG_SKILLS_MANAGER_PRX))
+ {
+ p.usingProxy(config[CONFIG_SKILLS_MANAGER_PRX]);
+ }
+
+ if (config.contains(CONFIG_ARMS))
+ {
+ for (const auto& [k, config_arm] : config[CONFIG_ARMS].items())
+ {
+ auto& arm = robot.arms[k];
+
+ if (config_arm.contains(CONFIG_ARM_KINEMATIC_CHAIN_NAME))
+ {
+ arm.kinematicChainName = config_arm[CONFIG_ARM_KINEMATIC_CHAIN_NAME];
+ }
+
+ if (config_arm.contains(CONFIG_ARM_HAND))
+ {
+ auto config_hand = config_arm[CONFIG_ARM_HAND];
+
+ if (config_hand.contains(CONFIG_ARM_HAND_NAME))
+ {
+ arm.hand.name = config_hand[CONFIG_ARM_HAND_NAME];
+ }
+ if (config_hand.contains(CONFIG_ARM_HAND_FT_NAME))
+ {
+ arm.hand.ft_name = config_hand[CONFIG_ARM_HAND_FT_NAME];
+ }
+
+ if (config_hand.contains(CONFIG_ARM_HAND_PRX))
+ {
+ p.usingProxy(config_hand[CONFIG_ARM_HAND_PRX]);
+ }
+ }
+ }
+ }
+ }
+
+ void
+ RobotNameServiceComponentPlugin::preOnConnectComponent()
+ {
+ using namespace armarx::robot_name_service::client::constants;
+
+ nlohmann::json config = nlohmann::json::parse(properties.configJson);
+
+ auto& p = parent<RobotNameServiceComponentPluginUser>();
+
+ if (config.contains(CONFIG_MNS_PRX))
+ {
+ auto mns = p.getProxy<armarx::armem::mns::MemoryNameSystemInterfacePrx>(
+ config[CONFIG_MNS_PRX]);
+ robot.memoryNameSystem = armarx::armem::client::MemoryNameSystem(mns, &p);
+ }
+
+ if (config.contains(CONFIG_SKILLS_MANAGER_PRX))
+ {
+ auto skill_manager = p.getProxy<armarx::skills::manager::dti::SkillManagerInterfacePrx>(
+ config[CONFIG_SKILLS_MANAGER_PRX]);
+ robot.skillManager = skill_manager;
+ }
+
+ if (config.contains(CONFIG_ARMS))
+ {
+ for (const auto& [k, config_arm] : config[CONFIG_ARMS].items())
+ {
+ auto& arm = robot.arms[k];
+
+ if (config_arm.contains(CONFIG_ARM_HAND))
+ {
+ auto config_hand = config_arm[CONFIG_ARM_HAND];
+
+ if (config_hand.contains(CONFIG_ARM_HAND_PRX))
+ {
+ auto hand_prx = p.getProxy<armarx::HandUnitInterfacePrx>(
+ config_hand[CONFIG_ARM_HAND_PRX]);
+ arm.hand.handUnitInterface = hand_prx;
+ }
+ }
+ }
+ }
+
+ // finally commit robot
+ properties.rns->registerRobot(robot.toIce());
+ }
+
+ void
+ RobotNameServiceComponentPlugin::postCreatePropertyDefinitions(PropertyDefinitionsPtr& def)
+ {
+ def->optional(properties.configJson, "robotConfigJson");
+ def->component(properties.rns);
+ }
+
+ void
+ RobotNameServiceComponentPlugin::preOnDisconnectComponent()
+ {
+ properties.rns->unregisterRobot(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
new file mode 100644
index 0000000000000000000000000000000000000000..813003f98554196e32fa9cc2c27eb650cea5f914
--- /dev/null
+++ b/source/RobotAPI/libraries/robot_name_service/client/Plugin.h
@@ -0,0 +1,81 @@
+#pragma once
+#include <experimental/memory>
+#include <functional>
+#include <queue>
+#include <shared_mutex>
+#include <thread>
+#include <type_traits>
+
+#include <SimoxUtility/json/json.h>
+
+#include <ArmarXCore/core/ComponentPlugin.h>
+#include <ArmarXCore/core/ManagedIceObject.h>
+#include <ArmarXCore/core/services/tasks/RunningTask.h>
+
+#include <RobotAPI/interface/robot_name_service/RobotNameServiceInterface.h>
+#include <RobotAPI/libraries/robot_name_service/core/Robot.h>
+
+namespace armarx::robot_name_service::client::constants
+{
+ const auto CONFIG_ROBOT_NAME = "name";
+ const auto CONFIG_XML_PACKAGE_PATH = "xml_package_path";
+ const auto CONFIG_XML_PACKAGE_PATH_PACKAGE = "package";
+ const auto CONFIG_XML_PACKAGE_PATH_PATH = "path";
+ const auto CONFIG_MNS_PRX = "mns_prx";
+ const auto CONFIG_SKILLS_MANAGER_PRX = "manager_prx";
+ const auto CONFIG_ARMS = "arms";
+ const auto CONFIG_ARM_KINEMATIC_CHAIN_NAME = "kinematic_chain_name";
+ const auto CONFIG_ARM_HAND = "hand";
+ const auto CONFIG_ARM_HAND_NAME = "name";
+ const auto CONFIG_ARM_HAND_FT_NAME = "ft_name";
+ const auto CONFIG_ARM_HAND_PRX = "hand_prx";
+ // TODO
+} // namespace armarx::robot_name_service::client::constants
+
+namespace armarx
+{
+ class RobotNameServiceComponentPluginUser; // forward declaration
+}
+
+namespace armarx::plugins
+{
+ class RobotNameServiceComponentPlugin : public ComponentPlugin
+ {
+ public:
+ using ComponentPlugin::ComponentPlugin;
+
+ void postOnInitComponent() override;
+
+ void preOnConnectComponent() override;
+
+ void postCreatePropertyDefinitions(PropertyDefinitionsPtr& properties) override;
+
+ void preOnDisconnectComponent() override;
+
+ private:
+ struct Properties
+ {
+ std::string configJson = "{}";
+
+ armarx::robot_name_service::dti::RobotNameServiceInterfacePrx rns;
+ } properties;
+
+ armarx::robot_name_service::core::Robot robot;
+
+ friend class armarx::RobotNameServiceComponentPluginUser;
+ };
+} // namespace armarx::plugins
+
+namespace armarx
+
+{
+ class RobotNameServiceComponentPluginUser : virtual public ManagedIceObject
+ {
+ public:
+ RobotNameServiceComponentPluginUser();
+
+
+ private:
+ std::experimental::observer_ptr<plugins::RobotNameServiceComponentPlugin> plugin = nullptr;
+ };
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/robot_name_service/client/plugins.cpp b/source/RobotAPI/libraries/robot_name_service/client/plugins.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..51f19fb344c000a07436e20ac3881c38cee9d904
--- /dev/null
+++ b/source/RobotAPI/libraries/robot_name_service/client/plugins.cpp
@@ -0,0 +1,6 @@
+#include "plugins.h"
+
+namespace armarx::robot_name_service::client
+{
+
+} // namespace armarx::robot_name_service::client
diff --git a/source/RobotAPI/libraries/robot_name_service/client/plugins.h b/source/RobotAPI/libraries/robot_name_service/client/plugins.h
new file mode 100644
index 0000000000000000000000000000000000000000..a37a92b69011490ebf30f0f7c28fdccea5d28834
--- /dev/null
+++ b/source/RobotAPI/libraries/robot_name_service/client/plugins.h
@@ -0,0 +1,8 @@
+#pragma once
+
+#include "Plugin.h"
+
+namespace armarx::robot_name_service::client
+{
+
+} // namespace armarx::robot_name_service::client
diff --git a/source/RobotAPI/libraries/robot_name_service/core/CMakeLists.txt b/source/RobotAPI/libraries/robot_name_service/core/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..6c702dd1e210e92e3e14c4d02a4e6d870a731c44
--- /dev/null
+++ b/source/RobotAPI/libraries/robot_name_service/core/CMakeLists.txt
@@ -0,0 +1,24 @@
+set(LIB_NAME robot_name_service_core)
+
+armarx_component_set_name("${LIB_NAME}")
+armarx_set_target("Library: ${LIB_NAME}")
+
+armarx_add_library(
+ LIBS
+ ArmarXCoreInterfaces
+ ArmarXCore
+ ArmarXCoreObservers
+
+ RobotAPI::Core
+ RobotAPI::skills::core
+ RobotAPIInterfaces
+ armem
+
+
+ SOURCES
+ Robot.cpp
+ HEADERS
+ Robot.h
+)
+
+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
new file mode 100644
index 0000000000000000000000000000000000000000..bfa5fbfe5d529388f9b545d93adae68d5f853f57
--- /dev/null
+++ b/source/RobotAPI/libraries/robot_name_service/core/Robot.cpp
@@ -0,0 +1,79 @@
+#include "Robot.h"
+
+namespace armarx::robot_name_service::core
+{
+
+ void
+ Hand::fromIce(const dto::Hand& r)
+ {
+ name = r.name;
+ ft_name = r.ft_name;
+ handUnitInterface = r.handUnitInterface;
+ }
+
+ dto::Hand
+ Hand::toIce() const
+ {
+ dto::Hand r;
+ r.name = name;
+ r.ft_name = ft_name;
+ r.handUnitInterface = handUnitInterface;
+ return r;
+ }
+
+ void
+ Arm::fromIce(const dto::Arm& r)
+ {
+ kinematicChainName = r.kinematicChainName;
+ hand.fromIce(r.hand);
+ }
+
+ dto::Arm
+ Arm::toIce() const
+ {
+ dto::Arm r;
+ r.kinematicChainName = kinematicChainName;
+ r.hand = hand.toIce();
+ return r;
+ }
+
+ void
+ Robot::fromIce(const dto::Robot& 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)
+ {
+ arms[k].fromIce(a);
+ }
+
+ kinematicUnitInterface = r.kinematicUnitInterface;
+ platformUnitInterface = r.platformUnitInterface;
+ localizationUnitInterface = r.localizationUnitInterface;
+ }
+
+ dto::Robot
+ Robot::toIce() const
+ {
+ dto::Robot r;
+ r.name = name;
+ r.xmlPackagePath = xmlPackagePath;
+ r.memoryNameSystem = memoryNameSystem.getMemoryNameSystem();
+ r.skillManager = skillManager;
+
+ for (const auto& [k, a] : arms)
+ {
+ r.arms[k] = a.toIce();
+ }
+
+ r.kinematicUnitInterface = kinematicUnitInterface;
+ r.platformUnitInterface = platformUnitInterface;
+ r.localizationUnitInterface = localizationUnitInterface;
+ return r;
+ }
+
+} // namespace armarx::robot_name_service::core
diff --git a/source/RobotAPI/libraries/robot_name_service/core/Robot.h b/source/RobotAPI/libraries/robot_name_service/core/Robot.h
new file mode 100644
index 0000000000000000000000000000000000000000..243d94c1b2a7f72d8f050ee770d06611da5a83cf
--- /dev/null
+++ b/source/RobotAPI/libraries/robot_name_service/core/Robot.h
@@ -0,0 +1,59 @@
+#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
+{
+ class Hand
+ {
+ public:
+ void fromIce(const armarx::robot_name_service::dto::Hand& r);
+ armarx::robot_name_service::dto::Hand toIce() const;
+
+ public:
+ std::string name;
+ std::string ft_name;
+ armarx::HandUnitInterfacePrx handUnitInterface;
+ };
+
+ class Arm
+ {
+ public:
+ void fromIce(const armarx::robot_name_service::dto::Arm& r);
+ armarx::robot_name_service::dto::Arm toIce() const;
+
+ public:
+ std::string kinematicChainName;
+ Hand hand;
+ };
+
+ class Robot
+ {
+ public:
+ void fromIce(const armarx::robot_name_service::dto::Robot& r);
+ armarx::robot_name_service::dto::Robot toIce() const;
+
+ public:
+ // header
+ std::string name;
+ armarx::data::PackagePath xmlPackagePath;
+
+ // memory and skills
+ armarx::armem::client::MemoryNameSystem memoryNameSystem;
+ armarx::skills::manager::dti::SkillManagerInterfacePrx skillManager;
+
+ // kinematic stuff
+ std::map<std::string, Arm> arms;
+
+ // units
+ armarx::KinematicUnitInterfacePrx kinematicUnitInterface;
+ armarx::PlatformUnitInterfacePrx platformUnitInterface;
+ armarx::LocalizationUnitInterfacePrx localizationUnitInterface;
+ };
+} // 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
new file mode 100644
index 0000000000000000000000000000000000000000..d0c268aa439c501d38505d3fab455bf6b4a9b876
--- /dev/null
+++ b/source/RobotAPI/libraries/robot_name_service/server/CMakeLists.txt
@@ -0,0 +1,25 @@
+set(LIB_NAME robot_name_service_server)
+
+armarx_component_set_name("${LIB_NAME}")
+armarx_set_target("Library: ${LIB_NAME}")
+
+armarx_add_library(
+ LIBS
+ ArmarXCoreInterfaces
+ ArmarXCore
+ ArmarXCoreObservers
+
+ RobotAPI::Core
+ RobotAPI::skills::core
+ RobotAPIInterfaces
+ armem
+ robot_name_service_core
+
+
+ SOURCES
+ plugins.cpp
+ HEADERS
+ plugins.h
+)
+
+add_library(RobotAPI::robot_name_service_server ALIAS robot_name_service_server)
diff --git a/source/RobotAPI/libraries/robot_name_service/server/plugins.cpp b/source/RobotAPI/libraries/robot_name_service/server/plugins.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..fc90afa38d5542f6b1c156f359d72b912be3f4be
--- /dev/null
+++ b/source/RobotAPI/libraries/robot_name_service/server/plugins.cpp
@@ -0,0 +1,6 @@
+#include "plugins.h"
+
+namespace armarx::robot_name_service::server
+{
+
+} // namespace armarx::robot_name_service::server
diff --git a/source/RobotAPI/libraries/robot_name_service/server/plugins.h b/source/RobotAPI/libraries/robot_name_service/server/plugins.h
new file mode 100644
index 0000000000000000000000000000000000000000..ca43f153ee67d78b0b617370b93bec94e5cee3de
--- /dev/null
+++ b/source/RobotAPI/libraries/robot_name_service/server/plugins.h
@@ -0,0 +1,6 @@
+#pragma once
+
+namespace armarx::robot_name_service::server
+{
+
+} // namespace armarx::robot_name_service::server
diff --git a/source/RobotAPI/libraries/skills/core/Skill.cpp b/source/RobotAPI/libraries/skills/core/Skill.cpp
index 7d94de3e90815a156289948fae26463ce5f28247..0b7a32df224d925ced896414d7d47f540f04a0d5 100644
--- a/source/RobotAPI/libraries/skills/core/Skill.cpp
+++ b/source/RobotAPI/libraries/skills/core/Skill.cpp
@@ -245,7 +245,7 @@ namespace armarx
std::string("The skill '" + getSkillId().toString() + "' was asked to stop.");
message += abortedMessage.empty() ? "" : " Additional message: " + abortedMessage;
- throw error::SkillAbortedException(message);
+ throw error::SkillAbortedException(__PRETTY_FUNCTION__, message);
return;
}
@@ -256,7 +256,7 @@ namespace armarx
message += abortedMessage.empty() ? "" : " Additional message: " + abortedMessage;
ARMARX_WARNING << message;
- throw error::SkillFailedException(message);
+ throw error::SkillFailedException(__PRETTY_FUNCTION__, message);
}
}
diff --git a/source/RobotAPI/libraries/skills/core/SkillProxy.cpp b/source/RobotAPI/libraries/skills/core/SkillProxy.cpp
index f73c43dfd152c756f80e5ac3e83e901fe59e8ee1..ea4fc4d1a0192d5ad4819be4248df3ea52566164 100644
--- a/source/RobotAPI/libraries/skills/core/SkillProxy.cpp
+++ b/source/RobotAPI/libraries/skills/core/SkillProxy.cpp
@@ -12,9 +12,19 @@ namespace armarx
manager(manager)
{
ARMARX_CHECK_NOT_NULL(manager);
- skillDescription = SkillDescription::FromIce(
- manager->getSkillDescription(skillId.toManagerIce()).value());
- ARMARX_CHECK(skillDescription.skillId.isFullySpecified());
+ IceUtil::Optional<manager::dto::SkillDescription> description =
+ manager->getSkillDescription(skillId.toManagerIce());
+ if (description)
+ {
+ skillDescription = SkillDescription::FromIce(description.value());
+ ARMARX_CHECK(skillDescription.skillId.isFullySpecified());
+ }
+ else
+ {
+ std::stringstream reason;
+ reason << "No skill with ID " << skillId << " found";
+ throw error::SkillNotFoundException(__PRETTY_FUNCTION__, reason.str());
+ }
}
SkillProxy::SkillProxy(const manager::dti::SkillManagerInterfacePrx& manager,
diff --git a/source/RobotAPI/libraries/skills/core/error/Exception.h b/source/RobotAPI/libraries/skills/core/error/Exception.h
index 5e10baed1e9af3131226320397c11919d7a826e0..9e01205b27e74eddc69fa8ff7c28d2646eafc9e2 100644
--- a/source/RobotAPI/libraries/skills/core/error/Exception.h
+++ b/source/RobotAPI/libraries/skills/core/error/Exception.h
@@ -48,32 +48,38 @@ namespace armarx::skills::error
}
};
- class SkillAbortedException : public armarx::LocalException
+ /**
+ * @brief Indicates that a skill was not found, e.g., by the skill manager.
+ */
+ class SkillNotFoundException : public SkillException
{
public:
- SkillAbortedException() = delete;
+ SkillNotFoundException() = delete;
- SkillAbortedException(const std::string& reason) : LocalException(reason)
+ SkillNotFoundException(const std::string& prettymethod, const std::string& reason) :
+ SkillException(prettymethod, reason)
{
}
+ };
+
+ class SkillAbortedException : public SkillException
+ {
+ public:
+ SkillAbortedException() = delete;
SkillAbortedException(const std::string& prettymethod, const std::string& reason) :
- LocalException(prettymethod + ": " + reason + ".")
+ SkillException(prettymethod, reason)
{
}
};
- class SkillFailedException : public armarx::LocalException
+ class SkillFailedException : public SkillException
{
public:
SkillFailedException() = delete;
- SkillFailedException(const std::string& reason) : LocalException(reason)
- {
- }
-
SkillFailedException(const std::string& prettymethod, const std::string& reason) :
- LocalException(prettymethod + ": " + reason + ".")
+ SkillException(prettymethod, reason)
{
}
};
diff --git a/source/RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.cpp b/source/RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.cpp
index e4d3235a3366092cc2f2005dbad2a74d9b632223..a42d7808284d958d965cfba0dd70aa866a47cc17 100644
--- a/source/RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.cpp
+++ b/source/RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.cpp
@@ -190,9 +190,10 @@ namespace armarx::plugins
{
ARMARX_CHECK(executionRequest.skillId.isFullySpecified());
- skills::SkillExecutionID executionId(executionRequest.skillId,
- executionRequest.executorName,
- armarx::core::time::DateTime::Now());
+ skills::SkillExecutionID executionId{.skillId = executionRequest.skillId,
+ .executorName = executionRequest.executorName,
+ .executionStartedTime =
+ armarx::core::time::DateTime::Now()};
skills::SkillStatusUpdate ret{
{executionId, executionRequest.parameters, executionRequest.callbackInterface}};
diff --git a/source/RobotAPI/libraries/skills/provider/mixins/GraspReadingSkillMixin.h b/source/RobotAPI/libraries/skills/provider/mixins/GraspReadingSkillMixin.h
index d69f390dadcb7eb0daed57f8f7a82148c055fb52..abcfeb2d285a93254dfe1c029d7e7f1763709070 100644
--- a/source/RobotAPI/libraries/skills/provider/mixins/GraspReadingSkillMixin.h
+++ b/source/RobotAPI/libraries/skills/provider/mixins/GraspReadingSkillMixin.h
@@ -10,8 +10,10 @@ namespace armarx::skills::mixin
{
armem::grasping::known_grasps::Reader graspReader;
- GraspReadingSkillMixin(armem::client::MemoryNameSystem& mns) : graspReader(mns)
- {}
+ GraspReadingSkillMixin(const armem::grasping::known_grasps::Reader& r) : graspReader(r)
+ {
+
+ }
};
struct SpecificGraspReadingSkillMixin
@@ -19,7 +21,7 @@ namespace armarx::skills::mixin
std::string objectEntityId;
armem::grasping::known_grasps::Reader graspReader;
- SpecificGraspReadingSkillMixin(const std::string& n, armem::client::MemoryNameSystem& mns) : objectEntityId(n), graspReader(mns)
+ SpecificGraspReadingSkillMixin(const std::string& n, const armem::grasping::known_grasps::Reader& r) : objectEntityId(n), graspReader(r)
{}
};
}
diff --git a/source/RobotAPI/libraries/skills/provider/mixins/MemoryReadingSkillMixin.h b/source/RobotAPI/libraries/skills/provider/mixins/MemoryReadingSkillMixin.h
index 214fbac445e675d2bde67ae38372fa271a30b504..be453b9283d3db61b46df79fba50a3e2fb13be80 100644
--- a/source/RobotAPI/libraries/skills/provider/mixins/MemoryReadingSkillMixin.h
+++ b/source/RobotAPI/libraries/skills/provider/mixins/MemoryReadingSkillMixin.h
@@ -10,7 +10,8 @@ namespace armarx::skills::mixin
{
armem::client::Reader memoryReader;
- //MemoryReadingSkillMixin(armem::client::MemoryNameSystem& mns) : memoryReader(mns)
- //{}
+ MemoryReadingSkillMixin(const armem::client::Reader& r) : memoryReader(r)
+ {
+ }
};
-}
+} // namespace armarx::skills::mixin
diff --git a/source/RobotAPI/libraries/skills/provider/mixins/ObjectReadingSkillMixin.h b/source/RobotAPI/libraries/skills/provider/mixins/ObjectReadingSkillMixin.h
index ecf3d2a64ef0a5d3f01b5b9b23839b1db14cb921..84a7746a74177df50eff3f15c768885f77b6e446 100644
--- a/source/RobotAPI/libraries/skills/provider/mixins/ObjectReadingSkillMixin.h
+++ b/source/RobotAPI/libraries/skills/provider/mixins/ObjectReadingSkillMixin.h
@@ -10,8 +10,9 @@ namespace armarx::skills::mixin
{
armem::obj::instance::Reader objectReader;
- ObjectReadingSkillMixin(armem::client::MemoryNameSystem& mns) : objectReader(mns)
- {}
+ ObjectReadingSkillMixin(const armem::obj::instance::Reader& r) : objectReader(r)
+ {
+ }
};
struct SpecificObjectReadingSkillMixin
@@ -19,7 +20,10 @@ namespace armarx::skills::mixin
std::string objectEntityId;
armem::obj::instance::Reader objectReader;
- SpecificObjectReadingSkillMixin(const std::string& n, armem::client::MemoryNameSystem& mns) : objectEntityId(n), objectReader(mns)
- {}
+ SpecificObjectReadingSkillMixin(const std::string& n,
+ const armem::obj::instance::Reader& r) :
+ objectEntityId(n), objectReader(r)
+ {
+ }
};
-}
+} // namespace armarx::skills::mixin
diff --git a/source/RobotAPI/libraries/skills/provider/mixins/ObjectWritingSkillMixin.h b/source/RobotAPI/libraries/skills/provider/mixins/ObjectWritingSkillMixin.h
index edf8e6a2ef7360417f2e951ed36e0b24c62f4cef..2bf09e174f06717a79a00d503d325e9ed6be090d 100644
--- a/source/RobotAPI/libraries/skills/provider/mixins/ObjectWritingSkillMixin.h
+++ b/source/RobotAPI/libraries/skills/provider/mixins/ObjectWritingSkillMixin.h
@@ -10,7 +10,8 @@ namespace armarx::skills::mixin
{
armem::obj::instance::Writer objectWriter;
- ObjectWritingSkillMixin(armem::client::MemoryNameSystem& mns) : objectWriter(mns)
- {}
+ ObjectWritingSkillMixin(const armem::obj::instance::Writer& r) : objectWriter(r)
+ {
+ }
};
-}
+} // namespace armarx::skills::mixin
diff --git a/source/RobotAPI/libraries/skills/provider/mixins/RobotReadingSkillMixin.h b/source/RobotAPI/libraries/skills/provider/mixins/RobotReadingSkillMixin.h
index 631491ddcfb70a7fc1c6930317d30bbb3c969dca..9394445a59534482aee91a15a6e5e16429483564 100644
--- a/source/RobotAPI/libraries/skills/provider/mixins/RobotReadingSkillMixin.h
+++ b/source/RobotAPI/libraries/skills/provider/mixins/RobotReadingSkillMixin.h
@@ -10,9 +10,9 @@ namespace armarx::skills::mixin
{
armem::robot_state::VirtualRobotReader robotReader;
- RobotReadingSkillMixin(armem::client::MemoryNameSystem& mns) :
- robotReader(mns)
- {}
+ RobotReadingSkillMixin(const armem::robot_state::VirtualRobotReader& r) : robotReader(r)
+ {
+ }
};
struct SpecificRobotReadingSkillMixin
@@ -20,9 +20,10 @@ namespace armarx::skills::mixin
std::string robotName;
armem::robot_state::VirtualRobotReader robotReader;
- SpecificRobotReadingSkillMixin(const std::string& rn, armem::client::MemoryNameSystem& mns) :
- robotName(rn),
- robotReader(mns)
- {}
+ SpecificRobotReadingSkillMixin(const std::string& rn,
+ const armem::robot_state::VirtualRobotReader& r) :
+ robotName(rn), robotReader(r)
+ {
+ }
};
-}
+} // namespace armarx::skills::mixin