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/components/skills/SkillProviderExample/HelloWorld.cpp b/source/RobotAPI/components/skills/SkillProviderExample/HelloWorld.cpp
index d57f66c3a0e682d382dc91052c9a45b7acb639e7..edf918402a4c8ed1bbe510ab553d6dc568540dd5 100644
--- a/source/RobotAPI/components/skills/SkillProviderExample/HelloWorld.cpp
+++ b/source/RobotAPI/components/skills/SkillProviderExample/HelloWorld.cpp
@@ -20,6 +20,7 @@ namespace armarx::skills::provider
root_profile_params.some_float = 5;
root_profile_params.some_int = 42;
root_profile_params.some_text = "YOLO";
+ root_profile_params.some_optional_text = "OPTIONAL";
root_profile_params.some_list_of_matrices.push_back(Eigen::Matrix3f::Zero());
root_profile_params.some_matrix = Eigen::Matrix3f::Zero();
diff --git a/source/RobotAPI/components/skills/SkillProviderExample/aron/HelloWorldAcceptedType.xml b/source/RobotAPI/components/skills/SkillProviderExample/aron/HelloWorldAcceptedType.xml
index e90781ca62d7ef67751ed79b7556ab2c61f160a5..10c81ab542736b5a06bcd605f95353a619c3ad25 100644
--- a/source/RobotAPI/components/skills/SkillProviderExample/aron/HelloWorldAcceptedType.xml
+++ b/source/RobotAPI/components/skills/SkillProviderExample/aron/HelloWorldAcceptedType.xml
@@ -22,6 +22,9 @@
<ObjectChild key='some_text'>
<String />
</ObjectChild>
+ <ObjectChild key='some_optional_text'>
+ <String optional="True" />
+ </ObjectChild>
<ObjectChild key='some_list_of_matrices'>
<List>
diff --git a/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp b/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp
index 7c7cfae4d2fba28158d7bbe6cb2a3cb89fd5c195..f8a4b50ece99f95d8d4c618b377774e289ec4c03 100644
--- a/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp
+++ b/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp
@@ -651,7 +651,7 @@ namespace armarx
{
if (currentNode)
{
- if (currentNode->isRotationalJoint() or currentNode->isHemisphereJoint())
+ if (currentNode->isRotationalJoint() or currentNode->isHemisphereJoint() or currentNode->isFourBarJoint())
{
const bool isDeg = ui.checkBoxUseDegree->isChecked();
const auto factor =
@@ -728,7 +728,7 @@ namespace armarx
{
const QString unit = [&]() -> QString
{
- if (currentNode->isRotationalJoint() or currentNode->isHemisphereJoint())
+ if (currentNode->isRotationalJoint() or currentNode->isHemisphereJoint() or currentNode->isFourBarJoint())
{
if (ui.checkBoxUseDegree->isChecked())
{
@@ -751,7 +751,7 @@ namespace armarx
const auto [factor, conversionFactor] = [&]() -> std::pair<float, float>
{
- if (currentNode->isRotationalJoint() or currentNode->isHemisphereJoint())
+ if (currentNode->isRotationalJoint() or currentNode->isHemisphereJoint() or currentNode->isFourBarJoint())
{
const bool isDeg = ui.checkBoxUseDegree->isChecked();
if (isDeg)
@@ -841,7 +841,7 @@ namespace armarx
const QString unit = [&]() -> QString
{
- if (currentNode->isRotationalJoint() or currentNode->isHemisphereJoint())
+ if (currentNode->isRotationalJoint() or currentNode->isHemisphereJoint() or currentNode->isFourBarJoint())
{
if (ui.checkBoxUseDegree->isChecked())
{
@@ -865,7 +865,7 @@ namespace armarx
<< currentNode->getName() << flush;
const bool isDeg = ui.checkBoxUseDegree->isChecked();
- const bool isRot = currentNode->isRotationalJoint() or currentNode->isHemisphereJoint();
+ const bool isRot = currentNode->isRotationalJoint() or currentNode->isHemisphereJoint() or currentNode->isFourBarJoint();
const float lo = isRot ? (isDeg ? -90 : -M_PI * 100) : -1000;
const float hi = isRot ? (isDeg ? +90 : +M_PI * 100) : 1000;
@@ -1186,7 +1186,7 @@ namespace armarx
const ControlMode currentControlMode = getSelectedControlMode();
const bool isDeg = ui.checkBoxUseDegree->isChecked();
- const bool isRot = currentNode->isRotationalJoint() or currentNode->isHemisphereJoint();
+ const bool isRot = currentNode->isRotationalJoint() or currentNode->isHemisphereJoint() or currentNode->isFourBarJoint();
if (currentControlMode == ePositionControl)
{
@@ -1361,11 +1361,11 @@ namespace armarx
newItem = new QTableWidgetItem(state);
ui.tableJointList->setItem(i, eTabelColumnError, newItem);
- state = currentStatus.enabled ? "X" : "-";
+ state = currentStatus.enabled ? "yes" : "no";
newItem = new QTableWidgetItem(state);
ui.tableJointList->setItem(i, eTabelColumnEnabled, newItem);
- state = currentStatus.emergencyStop ? "X" : "-";
+ state = currentStatus.emergencyStop ? "yes" : "no";
newItem = new QTableWidgetItem(state);
ui.tableJointList->setItem(i, eTabelColumnEmergencyStop, newItem);
}
@@ -1443,7 +1443,7 @@ namespace armarx
QModelIndex index = ui.tableJointList->model()->index(i, eTabelColumnAngleProgressbar);
float conversionFactor =
ui.checkBoxUseDegree->isChecked() &&
- (node->isRotationalJoint() or node->isHemisphereJoint())
+ (node->isRotationalJoint() or node->isHemisphereJoint() or node->isFourBarJoint())
? 180.0 / M_PI
: 1;
ui.tableJointList->model()->setData(
@@ -1486,7 +1486,7 @@ namespace armarx
float currentValue = it->second;
if (ui.checkBoxUseDegree->isChecked() &&
- (rn[i]->isRotationalJoint() or rn[i]->isHemisphereJoint()))
+ (rn[i]->isRotationalJoint() or rn[i]->isHemisphereJoint() or rn[i]->isFourBarJoint()))
{
currentValue *= 180.0 / M_PI;
}
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/CMakeLists.txt b/source/RobotAPI/gui-plugins/SkillManagerPlugin/CMakeLists.txt
index a6d2ff63c1462557b5c55fc9538228dc46509ef1..113ccd8706d8f7c64cbd067f1002a5969b3f9507 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
@@ -34,13 +35,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 b96d3850e2c7181892e22030c600157963fb41af..83e522b9d6f895b6f492ba63d13529712a3f194d 100644
--- a/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidget.ui
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidget.ui
@@ -20,6 +20,9 @@
<string>SkillManagerMonitorWidget</string>
</property>
<layout class="QGridLayout" name="gridLayout_3">
+ <item row="3" column="2">
+ <widget class="QDoubleSpinBox" name="doubleSpinBoxUpdateFreq"/>
+ </item>
<item row="3" column="0">
<widget class="QCheckBox" name="checkBoxAutoUpdate">
<property name="text">
@@ -27,16 +30,6 @@
</property>
</widget>
</item>
- <item row="3" column="3">
- <widget class="QPushButton" name="pushButtonRefreshNow">
- <property name="text">
- <string>Refresh Now</string>
- </property>
- </widget>
- </item>
- <item row="3" column="2">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxUpdateFreq"/>
- </item>
<item row="3" column="1">
<widget class="QLabel" name="label">
<property name="text">
@@ -47,6 +40,13 @@
</property>
</widget>
</item>
+ <item row="3" column="3">
+ <widget class="QPushButton" name="pushButtonRefreshNow">
+ <property name="text">
+ <string>Refresh Now</string>
+ </property>
+ </widget>
+ </item>
<item row="4" column="0" colspan="4">
<widget class="QSplitter" name="splitter_2">
<property name="enabled">
@@ -190,72 +190,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>
- </widget>
- </item>
- <item row="3" column="0" colspan="4">
- <widget class="QTreeWidget" name="treeWidgetSkillDetails">
- <property name="contextMenuPolicy">
- <enum>Qt::CustomContextMenu</enum>
+ <property name="orientation">
+ <enum>Qt::Vertical</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>
- </widget>
- </item>
- <item row="0" column="1">
- <widget class="QPushButton" name="pushButtonCopy">
- <property name="text">
- <string>Copy args to clipboard</string>
- </property>
+ <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="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 191c160194207e561a1be7582c038361299a81ac..8b88718f93dc71bae29b16094d9701c0ae7d896a 100644
--- a/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidgetController.cpp
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidgetController.cpp
@@ -24,25 +24,22 @@
#include <string>
-#include <RobotAPI/libraries/skills/core/Skill.h>
-
-#include "aronTreeWidget/visitors/AronTreeWidgetConverter.h"
-#include "aronTreeWidget/visitors/AronTreeWidgetCreator.h"
-#include "aronTreeWidget/visitors/AronTreeWidgetModalCreator.h"
-
-// modals
-#include "aronTreeWidget/modal/text/AronTreeWidgetTextInputModalController.h"
-
-// debug
#include <QClipboard>
#include <QDoubleSpinBox>
+#include <QGridLayout>
+#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
{
QPointer<QDialog>
@@ -112,6 +109,12 @@ namespace armarx
widget.doubleSpinBoxUpdateFreq->setSingleStep(0.5);
widget.doubleSpinBoxUpdateFreq->setSuffix(" Hz");
+ skillDescriptionWidget = new SkillDescriptionWidget();
+ widget.skillDescription->parentWidget()->layout()->replaceWidget(widget.skillDescription,
+ skillDescriptionWidget);
+ widget.skillDescription = skillDescriptionWidget;
+
+
refreshSkillsResultTimer = new QTimer(this);
updateTimerFrequency();
refreshSkillsResultTimer->start();
@@ -502,7 +505,9 @@ namespace armarx
if (!current->parent())
{
- // no parent available. Perhaps provider clicked?
+ // no parent available. Perhaps provider clicked? Reset selected skill.
+ selectedSkill.skillId.providerId->providerName = "";
+ selectedSkill.skillId.skillName = "";
return;
}
@@ -532,28 +537,7 @@ namespace armarx
ARMARX_CHECK(skills.at(*selectedSkill.skillId.providerId).count(selectedSkill.skillId) > 0);
auto skillDesc = skills.at(*selectedSkill.skillId.providerId).at(selectedSkill.skillId);
- {
- auto it = new QTreeWidgetItem(widget.treeWidgetSkillDetails,
- {QString::fromStdString("Name"),
- QString::fromStdString(skillDesc.skillId.skillName)});
- widget.treeWidgetSkillDetails->addTopLevelItem(it);
- }
-
- {
- auto it = new QTreeWidgetItem(widget.treeWidgetSkillDetails,
- {QString::fromStdString("Description"),
- QString::fromStdString(skillDesc.description)});
- widget.treeWidgetSkillDetails->addTopLevelItem(it);
- }
-
- {
- auto it = new QTreeWidgetItem(
- widget.treeWidgetSkillDetails,
- {QString::fromStdString("Timeout"),
- QString::fromStdString(std::to_string(skillDesc.timeout.toMilliSeconds())) +
- " ms"});
- widget.treeWidgetSkillDetails->addTopLevelItem(it);
- }
+ skillDescriptionWidget->setSkillDescription(skillDesc);
// select root profile
widget.comboBoxProfiles->setCurrentIndex(0);
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidgetController.h b/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidgetController.h
index 1fd2e2f110fe8d34890f8d685c8faa5bc2031b70..9f251bf9e1e5890d9059836fa12017fca9264925 100644
--- a/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidgetController.h
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidgetController.h
@@ -46,6 +46,8 @@
namespace armarx
{
+ class SkillDescriptionWidget;
+
class SkillInfoTreeWidgetItem : public QTreeWidgetItem
{
public:
@@ -166,6 +168,7 @@ namespace armarx
// others
QTimer* refreshSkillsResultTimer;
+ SkillDescriptionWidget* skillDescriptionWidget = nullptr;
// connected flag
std::atomic_bool connected = false;
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetConverter.cpp b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetConverter.cpp
index 2e52ed4cf4854583641d1a83f85822096d8f5226..5c3db21fbe8604c9842cb834565274a4294e6233 100644
--- a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetConverter.cpp
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetConverter.cpp
@@ -90,6 +90,16 @@ namespace armarx
createdAron = createdAronDict;
QTreeWidgetItem* el = parentItem->child(index);
+ if (i->getMaybe() != armarx::aron::type::Maybe::NONE)
+ {
+ // its a maybetype. We have to check the state
+ if (el->checkState(1) == Qt::CheckState::Unchecked)
+ {
+ createdAron = nullptr;
+ return;
+ }
+ }
+
unsigned int x = 0;
for (const auto& [key, value] : i->getMemberTypes())
{
@@ -113,6 +123,16 @@ namespace armarx
createdAron = createdAronDict;
QTreeWidgetItem* el = parentItem->child(index);
+ if (i->getMaybe() != armarx::aron::type::Maybe::NONE)
+ {
+ // its a maybetype. We have to check the state
+ if (el->checkState(1) == Qt::CheckState::Unchecked)
+ {
+ createdAron = nullptr;
+ return;
+ }
+ }
+
for (int x = 0; x < el->childCount(); ++x)
{
auto it = el->child(x);
@@ -134,12 +154,23 @@ namespace armarx
ARMARX_TRACE;
auto createdAronList = std::make_shared<aron::data::List>(i->getPath());
createdAron = createdAronList;
- auto* elem = parentItem->child(index);
+ auto* el = parentItem->child(index);
+
+ if (i->getMaybe() != armarx::aron::type::Maybe::NONE)
+ {
+ // its a maybetype. We have to check the state
+ if (el->checkState(1) == Qt::CheckState::Unchecked)
+ {
+ createdAron = nullptr;
+ return;
+ }
+ }
+
auto childrenTypes = i->getChildren();
ARMARX_CHECK(childrenTypes.size() == 1);
- for (int j = 0; j < elem->childCount(); ++j)
+ for (int j = 0; j < el->childCount(); ++j)
{
- AronTreeWidgetConverterVisitor convVisitor(elem, j);
+ AronTreeWidgetConverterVisitor convVisitor(el, j);
aron::type::visit(convVisitor, childrenTypes[0]);
handleErrors(convVisitor);
@@ -156,11 +187,21 @@ namespace armarx
ARMARX_TRACE;
auto createdAronPair = std::make_shared<aron::data::List>(i->getPath());
createdAron = createdAronPair;
- auto* elem = parentItem->child(index);
+ auto* el = parentItem->child(index);
+
+ if (i->getMaybe() != armarx::aron::type::Maybe::NONE)
+ {
+ // its a maybetype. We have to check the state
+ if (el->checkState(1) == Qt::CheckState::Unchecked)
+ {
+ createdAron = nullptr;
+ return;
+ }
+ }
for (int j = 0; j < 2; ++j)
{
- AronTreeWidgetConverterVisitor convVisitor(elem, j);
+ AronTreeWidgetConverterVisitor convVisitor(el, j);
handleErrors(convVisitor);
if (convVisitor.createdAron && convVisitor.isConversionSuccessful())
{
@@ -177,6 +218,16 @@ namespace armarx
createdAron = createdAronList;
QTreeWidgetItem* el = parentItem->child(index);
+ if (i->getMaybe() != armarx::aron::type::Maybe::NONE)
+ {
+ // its a maybetype. We have to check the state
+ if (el->checkState(1) == Qt::CheckState::Unchecked)
+ {
+ createdAron = nullptr;
+ return;
+ }
+ }
+
for (int x = 0; x < el->childCount(); ++x)
{
auto* it = el->child(x);
@@ -242,11 +293,22 @@ namespace armarx
createdMatrix->setShape({i->getRows(), i->getCols(), dataSize});
int totalByteSize = i->getRows() * i->getCols() * dataSize;
createdAron = createdMatrix;
+ auto* el = parentItem->child(index);
+
+ if (i->getMaybe() != armarx::aron::type::Maybe::NONE)
+ {
+ // its a maybetype. We have to check the state
+ if (el->checkState(1) == Qt::CheckState::Unchecked)
+ {
+ createdAron = nullptr;
+ return;
+ }
+ }
- auto* currElem = parentItem->child(index);
- auto* rootWidget = currElem->treeWidget();
+
+ auto* rootWidget = el->treeWidget();
ARMARX_CHECK(rootWidget);
- auto* widget = rootWidget->itemWidget(currElem, 1);
+ auto* widget = rootWidget->itemWidget(el, 1);
auto* matrixWidget = EditMatrixWidget::DynamicCastAndCheck(widget);
handleErrors(matrixWidget->hasParseErrors());
@@ -280,8 +342,19 @@ namespace armarx
int dataSize = i->getElementType() == aron::type::quaternion::ElementType::FLOAT32 ? 4 : 8;
createdQuat->setShape({1, 4, dataSize});
createdQuat->setType(i->getFullName());
- auto* currTreeElem = parentItem->child(index);
- auto* itemWidget = currTreeElem->treeWidget()->itemWidget(currTreeElem, 1);
+ auto* el = parentItem->child(index);
+
+ if (i->getMaybe() != armarx::aron::type::Maybe::NONE)
+ {
+ // its a maybetype. We have to check the state
+ if (el->checkState(1) == Qt::CheckState::Unchecked)
+ {
+ createdAron = nullptr;
+ return;
+ }
+ }
+
+ auto* itemWidget = el->treeWidget()->itemWidget(el, 1);
auto* quatWidget = QuaternionWidget::DynamicCastAndCheck(itemWidget);
// error handling
@@ -320,6 +393,17 @@ namespace armarx
{
ARMARX_TRACE;
QTreeWidgetItem* el = parentItem->child(index);
+
+ if (i->getMaybe() != armarx::aron::type::Maybe::NONE)
+ {
+ // its a maybetype. We have to check the state
+ if (el->checkState(1) == Qt::CheckState::Unchecked)
+ {
+ createdAron = nullptr;
+ return;
+ }
+ }
+
auto* genericWidget = el->treeWidget()->itemWidget(el, 1);
auto* intEnumWidget = IntEnumWidget::DynamicCastAndCheck(genericWidget);
if (!intEnumWidget)
@@ -341,6 +425,16 @@ namespace armarx
createdAron = createdAronInt;
QTreeWidgetItem* el = parentItem->child(index);
+ if (i->getMaybe() != armarx::aron::type::Maybe::NONE)
+ {
+ // its a maybetype. We have to check the state
+ if (el->checkState(1) == Qt::CheckState::Unchecked)
+ {
+ createdAron = nullptr;
+ return;
+ }
+ }
+
std::string str = el->text(1).toStdString();
if (str.empty())
{
@@ -370,6 +464,16 @@ namespace armarx
createdAron = createdAronLong;
QTreeWidgetItem* el = parentItem->child(index);
+ if (i->getMaybe() != armarx::aron::type::Maybe::NONE)
+ {
+ // its a maybetype. We have to check the state
+ if (el->checkState(1) == Qt::CheckState::Unchecked)
+ {
+ createdAron = nullptr;
+ return;
+ }
+ }
+
std::string str = el->text(1).toStdString();
if (str.empty())
{
@@ -398,6 +502,16 @@ namespace armarx
createdAron = createdAronFloat;
QTreeWidgetItem* el = parentItem->child(index);
+ if (i->getMaybe() != armarx::aron::type::Maybe::NONE)
+ {
+ // its a maybetype. We have to check the state
+ if (el->checkState(1) == Qt::CheckState::Unchecked)
+ {
+ createdAron = nullptr;
+ return;
+ }
+ }
+
std::string str = el->text(1).toStdString();
if (str.empty())
{
@@ -425,6 +539,16 @@ namespace armarx
createdAron = createdAronDouble;
QTreeWidgetItem* el = parentItem->child(index);
+ if (i->getMaybe() != armarx::aron::type::Maybe::NONE)
+ {
+ // its a maybetype. We have to check the state
+ if (el->checkState(1) == Qt::CheckState::Unchecked)
+ {
+ createdAron = nullptr;
+ return;
+ }
+ }
+
std::string str = el->text(1).toStdString();
if (str.empty())
{
@@ -452,6 +576,16 @@ namespace armarx
createdAron = createdAronBool;
QTreeWidgetItem* el = parentItem->child(index);
+ if (i->getMaybe() != armarx::aron::type::Maybe::NONE)
+ {
+ // its a maybetype. We have to check the state
+ if (el->checkState(1) == Qt::CheckState::Unchecked)
+ {
+ createdAron = nullptr;
+ return;
+ }
+ }
+
std::string str = el->text(1).toStdString();
if (str.empty())
{
@@ -479,6 +613,16 @@ namespace armarx
createdAron = createdAronString;
QTreeWidgetItem* el = parentItem->child(index);
+ if (i->getMaybe() != armarx::aron::type::Maybe::NONE)
+ {
+ // its a maybetype. We have to check the state
+ if (el->checkState(1) == Qt::CheckState::Unchecked)
+ {
+ createdAron = nullptr;
+ return;
+ }
+ }
+
std::string str = el->text(1).toStdString();
createdAronString->setValue(str);
}
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetCreator.cpp b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetCreator.cpp
index 84cb58585e3b4e7f3a058db66f3b5f3a40f54aea..5bc5a40bbd407c104ce96905850e9d0beabecb48 100644
--- a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetCreator.cpp
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetCreator.cpp
@@ -101,6 +101,11 @@ namespace armarx
createdQWidgetItem =
new AronTreeWidgetItem(isDictChild, editableValue, QString::fromStdString(key), i);
+
+ if (i->getMaybe() != armarx::aron::type::Maybe::NONE)
+ {
+ createdQWidgetItem->setCheckState(1, Qt::CheckState::Unchecked);
+ }
createdQWidgetItem->setText(1, QString::fromStdString(defaul));
createdQWidgetItem->setText(2, QString::fromStdString(i->getShortName()));
createdQWidgetItem->setText(
@@ -129,6 +134,12 @@ namespace armarx
createdQWidgetItem =
new AronTreeWidgetItem(editableValue, false, QString::fromStdString(key), i);
+
+ if (i->getMaybe() != armarx::aron::type::Maybe::NONE)
+ {
+ createdQWidgetItem->setCheckState(1, Qt::CheckState::Unchecked);
+ }
+
parentOfCreatedObj->addChild(createdQWidgetItem);
for (const auto& [key, value] : i->getMemberTypes())
@@ -149,6 +160,7 @@ namespace armarx
// The DictType has only one member, its key-type. This also must be present
ARMARX_CHECK_EQUAL(i->getChildren().size(), 1);
}
+
void
AronTreeWidgetCreatorVisitor::visitAronVariant(const aron::type::PairPtr& pair)
{
@@ -167,6 +179,7 @@ namespace armarx
}
}
}
+
void
AronTreeWidgetCreatorVisitor::visitAronVariant(const aron::type::TuplePtr& tuple)
{
@@ -185,6 +198,7 @@ namespace armarx
}
}
}
+
void
AronTreeWidgetCreatorVisitor::visitAronVariant(const aron::type::ListPtr& i)
{
@@ -192,6 +206,7 @@ namespace armarx
auto txt = misc::generateNumElementsText(0);
createdQWidgetItem->setText(1, txt);
}
+
void
AronTreeWidgetCreatorVisitor::visitAronVariant(const aron::type::NDArrayPtr& i)
{
@@ -233,6 +248,7 @@ namespace armarx
i->getRows(), i->getCols(), i->getElementType(), createdQWidgetItem);
toplevelWidget->setItemWidget(createdQWidgetItem, 1, matWidget);
}
+
void
AronTreeWidgetCreatorVisitor::visitAronVariant(const aron::type::QuaternionPtr& i)
{
@@ -257,16 +273,19 @@ namespace armarx
new QuaternionWidget(i->getElementType(), createdQWidgetItem);
toplevelWidget->setItemWidget(createdQWidgetItem, 1, quatWidget);
}
+
void
AronTreeWidgetCreatorVisitor::visitAronVariant(const aron::type::ImagePtr& i)
{
insertNewTreeViewWidget(i, "");
}
+
void
AronTreeWidgetCreatorVisitor::visitAronVariant(const aron::type::PointCloudPtr& i)
{
insertNewTreeViewWidget(i, "");
}
+
void
AronTreeWidgetCreatorVisitor::visitAronVariant(const aron::type::IntEnumPtr& i)
{
@@ -278,36 +297,42 @@ namespace armarx
IntEnumWidget* widget = new IntEnumWidget(i, createdQWidgetItem);
createdQWidgetItem->treeWidget()->setItemWidget(createdQWidgetItem, 1, widget);
}
+
void
AronTreeWidgetCreatorVisitor::visitAronVariant(const aron::type::IntPtr& i)
{
editableValue = true;
insertNewTreeViewWidget(i, "0");
}
+
void
AronTreeWidgetCreatorVisitor::visitAronVariant(const aron::type::LongPtr& i)
{
editableValue = true;
insertNewTreeViewWidget(i, "0");
}
+
void
AronTreeWidgetCreatorVisitor::visitAronVariant(const aron::type::FloatPtr& i)
{
editableValue = true;
insertNewTreeViewWidget(i, "0.0");
}
+
void
AronTreeWidgetCreatorVisitor::visitAronVariant(const aron::type::DoublePtr& i)
{
editableValue = true;
insertNewTreeViewWidget(i, "0.0");
}
+
void
AronTreeWidgetCreatorVisitor::visitAronVariant(const aron::type::BoolPtr& i)
{
editableValue = true;
insertNewTreeViewWidget(i, "false");
}
+
void
AronTreeWidgetCreatorVisitor::visitAronVariant(const aron::type::StringPtr& i)
{
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetSetter.cpp b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetSetter.cpp
index cf719f8a2d1360d574fb8ebcac867378c3fd6f1b..cafc9b499b45a587efcc7a52ca531e028a8391df 100644
--- a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetSetter.cpp
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetSetter.cpp
@@ -31,7 +31,6 @@
#include "AronTreeWidgetContextMenu.h"
#include "AronTreeWidgetCreator.h"
-
template <typename T>
std::string
usString(T number, size_t precision = 3)
@@ -44,7 +43,6 @@ usString(T number, size_t precision = 3)
return ss.str();
}
-
//visitors
namespace armarx
{
@@ -101,7 +99,6 @@ namespace armarx
}
}
-
void
AronTreeWidgetSetterVisitor::visitAronVariant(const aron::data::DictPtr& i)
{
@@ -109,7 +106,7 @@ namespace armarx
if (i->getPath().size() == 0 ||
checkTreeWidgetItemForSimilarName(i->getPath().getLastElement()))
{
- QTreeWidgetItem* el = parentItem->child(index);
+ AronTreeWidgetItem* el = AronTreeWidgetItem::DynamicCast(parentItem->child(index));
auto* aronTreeWidget = AronTreeWidgetItem::DynamicCastAndCheck(el);
// allocate enough child items
adjustNumberOfChildren(aronTreeWidget, i->childrenSize());
@@ -123,7 +120,11 @@ namespace armarx
AronTreeWidgetSetterVisitor v(el, x++);
aron::data::visit(v, value);
}
- return;
+
+ if (el->aronType && el->aronType->getMaybe() != armarx::aron::type::Maybe::NONE)
+ {
+ el->setCheckState(1, Qt::CheckState::Checked);
+ }
}
}
@@ -132,7 +133,7 @@ namespace armarx
{
if (checkTreeWidgetItemForSimilarName(i->getPath().getLastElement()))
{
- QTreeWidgetItem* el = parentItem->child(index);
+ AronTreeWidgetItem* el = AronTreeWidgetItem::DynamicCast(parentItem->child(index));
auto* aronTreeWidget = AronTreeWidgetItem::DynamicCastAndCheck(el);
adjustNumberOfChildren(aronTreeWidget, i->childrenSize());
@@ -154,10 +155,14 @@ namespace armarx
auto currFont = aronTreeWidget->font(1);
currFont.setItalic(true);
aronTreeWidget->setFont(1, currFont);
+
+ if (el->aronType && el->aronType->getMaybe() != armarx::aron::type::Maybe::NONE)
+ {
+ el->setCheckState(1, Qt::CheckState::Checked);
+ }
}
}
-
void
visitMatrix(EditMatrixWidget* matrixWidget,
const std::shared_ptr<armarx::aron::type::Matrix>& matrixType,
@@ -215,6 +220,7 @@ namespace armarx
}
}
}
+
void
visitQuaternion(QuaternionWidget* quatWidget,
std::shared_ptr<armarx::aron::type::Quaternion>& quatType,
@@ -256,16 +262,16 @@ namespace armarx
AronTreeWidgetSetterVisitor::visitAronVariant(const aron::data::NDArrayPtr& arr)
{
// Matrices are handled as NDArray. Raw ndarrays cannot be created currently
- auto* castedEl = AronTreeWidgetItem::DynamicCast(parentItem->child(index));
- ARMARX_CHECK(castedEl);
+ auto* el = AronTreeWidgetItem::DynamicCast(parentItem->child(index));
+ ARMARX_CHECK(el);
- auto matrixCast = aron::type::Matrix::DynamicCast(castedEl->aronType);
- auto quaternionCast = aron::type::Quaternion::DynamicCast(castedEl->aronType);
+ auto matrixCast = aron::type::Matrix::DynamicCast(el->aronType);
+ auto quaternionCast = aron::type::Quaternion::DynamicCast(el->aronType);
- auto* rootWidget = castedEl->treeWidget();
+ auto* rootWidget = el->treeWidget();
ARMARX_CHECK(rootWidget);
- auto* matrixWidget = EditMatrixWidget::DynamicCast(rootWidget->itemWidget(castedEl, 1));
- auto* quaternionWidget = QuaternionWidget::DynamicCast(rootWidget->itemWidget(castedEl, 1));
+ auto* matrixWidget = EditMatrixWidget::DynamicCast(rootWidget->itemWidget(el, 1));
+ auto* quaternionWidget = QuaternionWidget::DynamicCast(rootWidget->itemWidget(el, 1));
if (matrixCast && matrixWidget)
{
@@ -280,6 +286,11 @@ namespace armarx
ARMARX_ERROR
<< "we do not support raw NDArrays. Ask Fabian Peller for more information.";
}
+
+ if (el->aronType && el->aronType->getMaybe() != armarx::aron::type::Maybe::NONE)
+ {
+ el->setCheckState(1, Qt::CheckState::Checked);
+ }
}
void
@@ -287,7 +298,7 @@ namespace armarx
{
if (checkTreeWidgetItemForSimilarName(i->getPath().getLastElement()))
{
- QTreeWidgetItem* el = parentItem->child(index);
+ AronTreeWidgetItem* el = AronTreeWidgetItem::DynamicCast(parentItem->child(index));
auto* enumWidget = IntEnumWidget::DynamicCast(el->treeWidget()->itemWidget(el, 1));
auto newText = QString::fromStdString(usString<int>(i->getValue()));
if (enumWidget)
@@ -300,6 +311,11 @@ namespace armarx
// Its just an int. -> do the QTreeWidgetItem call
el->setText(1, newText);
}
+
+ if (el->aronType && el->aronType->getMaybe() != armarx::aron::type::Maybe::NONE)
+ {
+ el->setCheckState(1, Qt::CheckState::Checked);
+ }
}
}
@@ -308,8 +324,13 @@ namespace armarx
{
if (checkTreeWidgetItemForSimilarName(i->getPath().getLastElement()))
{
- QTreeWidgetItem* el = parentItem->child(index);
+ AronTreeWidgetItem* el = AronTreeWidgetItem::DynamicCast(parentItem->child(index));
el->setText(1, QString::fromStdString(usString<long>(i->getValue())));
+
+ if (el->aronType && el->aronType->getMaybe() != armarx::aron::type::Maybe::NONE)
+ {
+ el->setCheckState(1, Qt::CheckState::Checked);
+ }
}
}
@@ -318,8 +339,13 @@ namespace armarx
{
if (checkTreeWidgetItemForSimilarName(i->getPath().getLastElement()))
{
- QTreeWidgetItem* el = parentItem->child(index);
+ AronTreeWidgetItem* el = AronTreeWidgetItem::DynamicCast(parentItem->child(index));
el->setText(1, QString::fromStdString(usString<float>(i->getValue())));
+
+ if (el->aronType && el->aronType->getMaybe() != armarx::aron::type::Maybe::NONE)
+ {
+ el->setCheckState(1, Qt::CheckState::Checked);
+ }
}
}
@@ -328,8 +354,13 @@ namespace armarx
{
if (checkTreeWidgetItemForSimilarName(i->getPath().getLastElement()))
{
- QTreeWidgetItem* el = parentItem->child(index);
+ AronTreeWidgetItem* el = AronTreeWidgetItem::DynamicCast(parentItem->child(index));
el->setText(1, QString::fromStdString(usString<double>(i->getValue())));
+
+ if (el->aronType && el->aronType->getMaybe() != armarx::aron::type::Maybe::NONE)
+ {
+ el->setCheckState(1, Qt::CheckState::Checked);
+ }
}
}
@@ -338,8 +369,13 @@ namespace armarx
{
if (checkTreeWidgetItemForSimilarName(i->getPath().getLastElement()))
{
- QTreeWidgetItem* el = parentItem->child(index);
+ AronTreeWidgetItem* el = AronTreeWidgetItem::DynamicCast(parentItem->child(index));
el->setText(1, QString::fromStdString(usString<bool>(i->getValue())));
+
+ if (el->aronType && el->aronType->getMaybe() != armarx::aron::type::Maybe::NONE)
+ {
+ el->setCheckState(1, Qt::CheckState::Checked);
+ }
}
}
@@ -348,8 +384,13 @@ namespace armarx
{
if (checkTreeWidgetItemForSimilarName(i->getPath().getLastElement()))
{
- QTreeWidgetItem* el = parentItem->child(index);
+ AronTreeWidgetItem* el = AronTreeWidgetItem::DynamicCast(parentItem->child(index));
el->setText(1, QString::fromStdString(i->getValue()));
+
+ if (el->aronType && el->aronType->getMaybe() != armarx::aron::type::Maybe::NONE)
+ {
+ el->setCheckState(1, Qt::CheckState::Checked);
+ }
}
}
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/CMakeLists.txt b/source/RobotAPI/libraries/armem/CMakeLists.txt
index 676c1fd0752baf0c2995e479dfad2cca4f3d9960..9030bd17597399abf79e3413ec71a789f3a113ca 100644
--- a/source/RobotAPI/libraries/armem/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem/CMakeLists.txt
@@ -57,12 +57,14 @@ set(LIB_FILES
client/MemoryNameSystem.cpp
client/Reader.cpp
client/Writer.cpp
+ client/ReadStream.cpp
client/plugins/ListeningPluginUser.cpp
client/plugins/ListeningPlugin.cpp
client/plugins/PluginUser.cpp
client/plugins/Plugin.cpp
+ client/util/SubscriptionHandle.cpp
client/util/MemoryListener.cpp
client/util/SimpleReaderBase.cpp
client/util/SimpleWriterBase.cpp
@@ -138,6 +140,7 @@ set(LIB_HEADERS
client/MemoryNameSystem.h
client/Reader.h
client/Writer.h
+ client/ReadStream.h
client/plugins.h
client/plugins/ListeningPluginUser.h
@@ -154,6 +157,7 @@ set(LIB_HEADERS
client/query/detail/NameSelectorOps.h
client/query/detail/SelectorOps.h
+ client/util/SubscriptionHandle.h
client/util/MemoryListener.h
client/util/SimpleReaderBase.h
client/util/SimpleWriterBase.h
diff --git a/source/RobotAPI/libraries/armem/client/ReadStream.cpp b/source/RobotAPI/libraries/armem/client/ReadStream.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..688357958419900abbea45f85b2c35cf4868f26f
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/client/ReadStream.cpp
@@ -0,0 +1,172 @@
+#include "ReadStream.h"
+
+#include <ArmarXCore/core/time/Clock.h>
+
+#include <RobotAPI/libraries/armem/client/query/Builder.h>
+
+namespace armarx::armem::client
+{
+ ReadStream::ReadStream() : metronome{armarx::core::time::Frequency::Hertz(10)}
+ {
+ }
+
+ ReadStream::ReadStream(const Reader& reader,
+ const MemoryID& queriedId,
+ const core::time::Frequency& maxPollFrequency) :
+ reader{reader}, queriedId{queriedId}, metronome{maxPollFrequency}
+ {
+ timeStart = armarx::core::time::Clock::Now();
+ }
+
+ std::optional<wm::EntitySnapshot>
+ ReadStream::pollBlocking(const SnapshotCallbackT& callback)
+ {
+ if (isPolling.exchange(true))
+ {
+ throw armarx::armem::error::ReadStreamAlreadyPolling(queriedId, __PRETTY_FUNCTION__);
+ }
+ pollingStoppedExternally = false;
+
+ auto result = _pollBlocking(callback);
+
+ isPolling = false;
+ return result;
+ }
+
+ void
+ ReadStream::pollAsync(const SnapshotCallbackT& callback)
+ {
+ if (isPolling.exchange(true))
+ {
+ throw armarx::armem::error::ReadStreamAlreadyPolling(queriedId, __PRETTY_FUNCTION__);
+ }
+ pollingStoppedExternally = false;
+
+ this->pollingThread = std::thread([&]() { this->_pollBlocking(callback); });
+ }
+
+ std::optional<wm::EntitySnapshot>
+ ReadStream::_pollBlocking(const SnapshotCallbackT& callback)
+ {
+ while (not pollingStoppedExternally)
+ {
+ auto snapshot = _pollOnce(callback);
+ if (snapshot.has_value())
+ {
+ return snapshot;
+ }
+ }
+ return std::nullopt;
+ }
+
+ void
+ ReadStream::stop()
+ {
+ pollingStoppedExternally = true;
+ if (pollingThread.joinable())
+ {
+ pollingThread.join();
+ isPolling = false;
+ }
+ }
+
+ std::optional<wm::EntitySnapshot>
+ ReadStream::pollOnce(const SnapshotCallbackT& callback)
+ {
+ if (isPolling.exchange(true))
+ {
+ throw armarx::armem::error::ReadStreamAlreadyPolling(queriedId, __PRETTY_FUNCTION__);
+ }
+
+ auto snapshot = _pollOnce(callback);
+
+ isPolling = false;
+ return snapshot;
+ }
+
+ std::optional<wm::EntitySnapshot>
+ ReadStream::_pollOnce(const SnapshotCallbackT& callback)
+ {
+ // Make sure to not busy wait. Also wait until probably data is available in first iteration.
+ metronome.waitForNextTick();
+
+ auto timeEnd = armarx::core::time::Clock::Now();
+ auto makeQuery = [this, &timeEnd](const MemoryID& id)
+ {
+ query::Builder qb;
+ query::CoreSegmentSelector& core =
+ id.hasCoreSegmentName() ? qb.coreSegments().withID(id) : qb.coreSegments().all();
+ query::ProviderSegmentSelector& prov = id.hasProviderSegmentName()
+ ? core.providerSegments().withID(id)
+ : core.providerSegments().all();
+ query::EntitySelector& entity =
+ id.hasEntityName() ? prov.entities().withID(id) : prov.entities().all();
+ entity.snapshots().timeRange(timeStart, timeEnd);
+
+ return qb.buildQueryInput();
+ };
+
+ auto query = makeQuery(queriedId);
+
+ auto result = reader.query(query);
+
+ if (result.success)
+ {
+ using EntitySnapshotReference =
+ std::reference_wrapper<armarx::armem::wm::EntitySnapshot>;
+ // Copy references of snapshots into vector to sort them.
+ std::vector<EntitySnapshotReference> snapshots;
+
+ result.memory.forEachSnapshot([&snapshots](armarx::armem::wm::EntitySnapshot& snapshot)
+ { snapshots.push_back(snapshot); });
+
+ // Sort correctly.
+ std::sort(snapshots.begin(),
+ snapshots.end(),
+ [](const EntitySnapshotReference& a, const EntitySnapshotReference& b)
+ { return a.get().id().timestamp < b.get().id().timestamp; });
+
+ // Determine the next start time.
+ DateTime nextStart;
+ if (snapshots.size() > 0)
+ {
+ // Because they are sorted, back() has the highest time stamp.
+ nextStart = snapshots.back().get().id().timestamp +
+ armarx::core::time::Duration::MicroSeconds(1);
+ }
+ else
+ {
+ nextStart = timeStart;
+ }
+
+ // Call the callback on all snapshots.
+ for (const auto& snapshot : snapshots)
+ {
+ // Assert times in correct interval.
+ ARMARX_CHECK_LESS_EQUAL(timeStart, snapshot.get().id().timestamp);
+ ARMARX_CHECK_GREATER_EQUAL(timeEnd, snapshot.get().id().timestamp);
+
+ const bool continue_ = callback(snapshot.get());
+ if (not continue_)
+ {
+ return snapshot;
+ }
+ }
+
+ timeStart = nextStart;
+ }
+ else
+ {
+ ARMARX_WARNING
+ << deactivateSpam()
+ << "Received an error in ReadStream when querying data from a "
+ "memory. The error was '"
+ << result.errorMessage
+ << "'. Continue with stream, perhaps the memory was not yet initialized.";
+ }
+
+ return std::nullopt;
+ }
+
+
+} // namespace armarx::armem::client
diff --git a/source/RobotAPI/libraries/armem/client/ReadStream.h b/source/RobotAPI/libraries/armem/client/ReadStream.h
new file mode 100644
index 0000000000000000000000000000000000000000..c86fb5b0675ddfc3fa3c4cd9abe49df5ab7a3016
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/client/ReadStream.h
@@ -0,0 +1,138 @@
+#pragma once
+
+#include <functional>
+#include <optional>
+#include <thread>
+
+#include <ArmarXCore/core/time/DateTime.h>
+#include <ArmarXCore/core/time/Frequency.h>
+#include <ArmarXCore/core/time/Metronome.h>
+
+#include <RobotAPI/libraries/armem/core/MemoryID.h>
+#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
+
+#include "Reader.h"
+
+namespace armarx::armem::client
+{
+ /**
+ * @brief A stream reading entity snapshots from the memory.
+ *
+ * After constructing a ReadStream, polling can be started in three ways:
+ *
+ * 1. Run a polling loop in this thread, blocking execution until terminated.
+ * See pollBlocking().
+ * 2. Run a polling loop in a new, separate thread, until it is stopped via stop().
+ * See pollAsync() and stop().
+ * 3. Perform a single query and process the result, embedded in your own loop or other control
+ * flow logic.
+ * See pollOnce().
+ */
+ class ReadStream
+ {
+ public:
+ /**
+ * @brief Callback called on each entity snapshot in the queried ID.
+ *
+ * If it returns false, the stream is stopped.
+ */
+ using SnapshotCallbackT = std::function<bool(const wm::EntitySnapshot&)>;
+
+ /**
+ * @brief Inizialize a ReadStream which does not represent a stream.
+ */
+ ReadStream();
+
+ /**
+ * @brief Initialize a read stream.
+ *
+ * @param reader
+ * The reader to perform the queries.
+ * @param queriedId
+ * The memory ID in which all snapshots should be processed by the stream.
+ * @param maxPollFrequency
+ * The maximum frequency with which queries are performed. The
+ * real frequency might be lower.
+ */
+ ReadStream(const Reader& reader,
+ const MemoryID& queriedId,
+ const armarx::core::time::Frequency& maxPollFrequency =
+ armarx::core::time::Frequency::Hertz(10));
+
+
+ /**
+ * @brief Poll in this thread as long as callback returns true.
+ *
+ * @param callback Function to call on each entity snapshot.
+ * @return The snapshot object that returns false.
+ * @throw armarx::armem::error::ReadStreamAlreadyPolling If the stream is already polling.
+ */
+ std::optional<wm::EntitySnapshot> pollBlocking(const SnapshotCallbackT& callback);
+
+ /**
+ * @brief Poll in a new thread as long as callback returns true.
+ *
+ * Note that callback will be called in a separate thread, so take care of synchronizing
+ * access to variables in the callback appropriately.
+ *
+ * Roughly equivalent to:
+ * @code
+ * std::thread thread([]() { stream.pollBlocking(); });
+ * @endcode
+ *
+ * @param callback Function to call on each entity snapshot.
+ * @throw armarx::armem::error::ReadStreamAlreadyPolling If the stream is already polling.
+ */
+ void pollAsync(const SnapshotCallbackT& callback);
+ /**
+ * @brief Stop a running polling loop.
+ *
+ * If a polling thread has been started by pollAsync() before, joins the thread.
+ */
+ void stop();
+
+ /**
+ * @brief Perform one query and call the callbacks on each snapshot.
+ *
+ * This allows you to define your own loop, for example:
+ *
+ * @code
+ * bool condition = true;
+ * while (condition)
+ * {
+ * auto snapshot = stream.pollOnce(callback);
+ *
+ * ...
+ *
+ * if (...)
+ * {
+ * condition = false;
+ * }
+ * }
+ * @endcode
+ *
+ * @param callback Function to call on each entity snapshot.
+ * @throw armarx::armem::error::ReadStreamAlreadyPolling If the stream is already polling.
+ */
+ std::optional<wm::EntitySnapshot> pollOnce(const SnapshotCallbackT& callback);
+
+
+ private:
+ std::optional<wm::EntitySnapshot> _pollBlocking(const SnapshotCallbackT& callback);
+ std::optional<wm::EntitySnapshot> _pollOnce(const SnapshotCallbackT& callback);
+
+
+ private:
+ Reader reader;
+ MemoryID queriedId;
+
+ armarx::core::time::Metronome metronome;
+ armarx::DateTime timeStart;
+
+ std::atomic_bool isPolling = false;
+ std::atomic_bool pollingStoppedExternally = false;
+
+ std::thread pollingThread;
+ };
+
+} // namespace armarx::armem::client
diff --git a/source/RobotAPI/libraries/armem/client/util/MemoryListener.cpp b/source/RobotAPI/libraries/armem/client/util/MemoryListener.cpp
index c8a7c6c683467194d6e2248edc0e5be57d3b547b..9aa23153cbfe67047579fdedab8753e35c260002 100644
--- a/source/RobotAPI/libraries/armem/client/util/MemoryListener.cpp
+++ b/source/RobotAPI/libraries/armem/client/util/MemoryListener.cpp
@@ -2,36 +2,33 @@
#include <sstream>
+#include <ArmarXCore/core/ManagedIceObject.h>
#include <ArmarXCore/core/exceptions/LocalException.h>
-#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/ice_conversions/ice_conversions_templates.h>
-#include <ArmarXCore/core/ManagedIceObject.h>
+#include <ArmarXCore/core/logging/Logging.h>
-#include <RobotAPI/libraries/armem/core/ice_conversions.h>
#include <RobotAPI/libraries/armem/core/error.h>
-
+#include <RobotAPI/libraries/armem/core/ice_conversions.h>
namespace armarx::armem::client::util
{
- std::string MemoryListener::MakeMemoryTopicName(const MemoryID& memoryID)
+ std::string
+ MemoryListener::MakeMemoryTopicName(const MemoryID& memoryID)
{
return "MemoryUpdates." + memoryID.memoryName;
}
-
- MemoryListener::MemoryListener(ManagedIceObject* component) :
- component(component)
+ MemoryListener::MemoryListener(ManagedIceObject* component) : component(component)
{
}
-
- void MemoryListener::setComponent(ManagedIceObject* component)
+ void
+ MemoryListener::setComponent(ManagedIceObject* component)
{
this->component = component;
}
-
void
MemoryListener::updated(const std::vector<data::MemoryID>& updatedSnapshotIDs) const
{
@@ -40,7 +37,6 @@ namespace armarx::armem::client::util
updated(bos);
}
-
void
MemoryListener::updated(const std::vector<MemoryID>& updatedSnapshotIDs) const
{
@@ -76,68 +72,104 @@ namespace armarx::armem::client::util
if (not matchingSnapshotIDs.empty())
{
ARMARX_DEBUG << "Calling " << subCallbacks.size() << " callbacks"
- << " subscribing " << subscription
- << " with " << matchingSnapshotIDs.size() << " snapshot IDs ...";
- for (auto& callback : subCallbacks)
+ << " subscribing " << subscription << " with "
+ << matchingSnapshotIDs.size() << " snapshot IDs ...";
+ for (auto& managedCallback : subCallbacks)
{
try
{
- callback(subscription, matchingSnapshotIDs);
+ managedCallback.callback(subscription, matchingSnapshotIDs);
}
catch (const armarx::LocalException& e)
{
error << "Calling callback subscribing " << subscription << " failed."
<< "\nCaught armarx::LocalException:"
- << "\n" << e.getReason()
- << "\n Stacktrace: \n" << e.generateBacktrace()
<< "\n"
- ;
+ << e.getReason() << "\n Stacktrace: \n"
+ << e.generateBacktrace() << "\n";
}
catch (const std::exception& e)
{
error << "Calling callback subscribing " << subscription << " failed."
<< "\nCaught armarx::Exception:"
- << "\n" << e.what()
<< "\n"
- ;
+ << e.what() << "\n";
}
catch (...)
{
error << "Calling callback subscribing " << subscription << " failed."
<< "\nCaught unknown exception."
- << "\n"
- ;
+ << "\n";
}
}
}
}
if (error.str().size() > 0)
{
- ARMARX_WARNING << "The following issues were encountered during MemoryListener::" << __FUNCTION__ << "(): \n\n"
+ ARMARX_WARNING << "The following issues were encountered during MemoryListener::"
+ << __FUNCTION__ << "(): \n\n"
<< error.str();
}
}
-
- void
- MemoryListener::subscribe(const MemoryID& id, Callback callback)
+ SubscriptionHandle
+ MemoryListener::subscribe(const MemoryID& memoryID, Callback callback)
{
- callbacks[id].push_back(callback);
- if (component)
+ ARMARX_CHECK_NOT_EMPTY(memoryID.memoryName)
+ << "The memoryName must be specified to subscribe";
+
+ if (component and memoryRefCount[memoryID.memoryName] == 0)
{
- component->usingTopic(MakeMemoryTopicName(id));
+ component->usingTopic(MakeMemoryTopicName(memoryID));
}
+
+ auto id = nextId++;
+ callbacks[memoryID].push_back({id, callback});
+
+ memoryRefCount[memoryID.memoryName]++;
+
+ return SubscriptionHandle(this, memoryID, id);
}
+ SubscriptionHandle
+ MemoryListener::subscribe(const MemoryID& subscriptionID, CallbackUpdatedOnly callback)
+ {
+ return subscribe(
+ subscriptionID,
+ [callback](const MemoryID&, const std::vector<MemoryID>& updatedSnapshotIDs)
+ { callback(updatedSnapshotIDs); });
+ }
void
- MemoryListener::subscribe(const MemoryID& subscriptionID, CallbackUpdatedOnly callback)
+ MemoryListener::unsubscribe(SubscriptionHandle& handle)
{
- subscribe(subscriptionID, [callback](const MemoryID&, const std::vector<MemoryID>& updatedSnapshotIDs)
+ if (not handle.valid)
+ {
+ return;
+ }
+ handle.valid = false;
+
+ // Remove ManagedCallback with ManagedCallback.id == handle.id from callbacks[handle.memoryID]
+ auto it = std::find_if(callbacks[handle.memoryID].begin(),
+ callbacks[handle.memoryID].end(),
+ [&handle](ManagedCallback& mCb) { return mCb.id == handle.id; });
+
+ std::iter_swap(it, callbacks[handle.memoryID].end() - 1);
+ callbacks[handle.memoryID].pop_back();
+
+ memoryRefCount[handle.memoryID.memoryName]--;
+
+ if (callbacks[handle.memoryID].empty())
{
- callback(updatedSnapshotIDs);
- });
+ callbacks.erase(handle.memoryID);
+
+ // unsubscribe from memory topic if no remainig callback needs it
+ if (component and memoryRefCount[handle.memoryID.memoryName] == 0)
+ {
+ component->unsubscribeFromTopic(MakeMemoryTopicName(handle.memoryID));
+ }
+ }
}
-}
+} // namespace armarx::armem::client::util
diff --git a/source/RobotAPI/libraries/armem/client/util/MemoryListener.h b/source/RobotAPI/libraries/armem/client/util/MemoryListener.h
index 521d9ce6981e9503d90515b0fc10c518eedaae5a..80d8eb3313c44a99abcc87d0048c2a24c2a137e3 100644
--- a/source/RobotAPI/libraries/armem/client/util/MemoryListener.h
+++ b/source/RobotAPI/libraries/armem/client/util/MemoryListener.h
@@ -1,6 +1,5 @@
#pragma once
-
// STD/STL
#include <functional>
#include <unordered_map>
@@ -10,7 +9,7 @@
#include <RobotAPI/interface/armem/client/MemoryListenerInterface.h>
#include <RobotAPI/libraries/armem/core/MemoryID.h>
-
+#include "SubscriptionHandle.h"
namespace armarx
{
@@ -19,34 +18,36 @@ namespace armarx
namespace armarx::armem::client::util
{
-
/**
* @brief Handles update signals from the memory system and distributes it
* to its subsribers.
*/
class MemoryListener
{
- public:
- using Callback = std::function<void(const MemoryID& subscriptionID, const std::vector<MemoryID>& updatedSnapshotIDs)>;
- using CallbackUpdatedOnly = std::function<void(const std::vector<MemoryID>& updatedSnapshotIDs)>;
+ public:
+ using Callback = std::function<void(const MemoryID& subscriptionID,
+ const std::vector<MemoryID>& updatedSnapshotIDs)>;
+ using CallbackUpdatedOnly =
+ std::function<void(const std::vector<MemoryID>& updatedSnapshotIDs)>;
template <class CalleeT>
- using MemberCallback = void(CalleeT::*)(const MemoryID& subscriptionID, const std::vector<MemoryID>& updatedSnapshotIDs);
+ using MemberCallback = void (CalleeT::*)(const MemoryID& subscriptionID,
+ const std::vector<MemoryID>& updatedSnapshotIDs);
template <class CalleeT>
- using MemberCallbackUpdatedOnly = void(CalleeT::*)(const std::vector<MemoryID>& updatedSnapshotIDs);
+ using MemberCallbackUpdatedOnly =
+ void (CalleeT::*)(const std::vector<MemoryID>& updatedSnapshotIDs);
static std::string MakeMemoryTopicName(const MemoryID& memoryID);
public:
-
MemoryListener(ManagedIceObject* component = nullptr);
void setComponent(ManagedIceObject* component);
- void subscribe(const MemoryID& subscriptionID, Callback Callback);
- void subscribe(const MemoryID& subscriptionID, CallbackUpdatedOnly Callback);
+ SubscriptionHandle subscribe(const MemoryID& subscriptionID, Callback Callback);
+ SubscriptionHandle subscribe(const MemoryID& subscriptionID, CallbackUpdatedOnly Callback);
/**
* Subscribe with a class member function:
@@ -55,28 +56,33 @@ namespace armarx::armem::client::util
* @endcode
*/
template <class CalleeT>
- void subscribe(const MemoryID& subscriptionID, CalleeT* callee, MemberCallback<CalleeT> callback)
+ SubscriptionHandle
+ subscribe(const MemoryID& subscriptionID, CalleeT* callee, MemberCallback<CalleeT> callback)
{
- auto cb = [callee, callback](const MemoryID & subscriptionID, const std::vector<MemoryID>& updatedSnapshotIDs)
- {
- (callee->*callback)(subscriptionID, updatedSnapshotIDs);
- };
- subscribe(subscriptionID, cb);
+ auto cb = [callee, callback](const MemoryID& subscriptionID,
+ const std::vector<MemoryID>& updatedSnapshotIDs)
+ { (callee->*callback)(subscriptionID, updatedSnapshotIDs); };
+ return subscribe(subscriptionID, cb);
}
template <class CalleeT>
- void subscribe(const MemoryID& subscriptionID, CalleeT* callee, MemberCallbackUpdatedOnly<CalleeT> callback)
+ SubscriptionHandle
+ subscribe(const MemoryID& subscriptionID,
+ CalleeT* callee,
+ MemberCallbackUpdatedOnly<CalleeT> callback)
{
- auto cb = [callee, callback](const MemoryID&, const std::vector<MemoryID>& updatedSnapshotIDs)
+ auto cb =
+ [callee, callback](const MemoryID&, const std::vector<MemoryID>& updatedSnapshotIDs)
{
- if(callee)
+ if (callee)
{
(callee->*callback)(updatedSnapshotIDs);
}
};
- subscribe(subscriptionID, cb);
+ return subscribe(subscriptionID, cb);
}
+ void unsubscribe(SubscriptionHandle& subscriptionHandle);
/// Function handling updates from the MemoryListener ice topic.
void updated(const std::vector<MemoryID>& updatedIDs) const;
@@ -84,13 +90,21 @@ namespace armarx::armem::client::util
protected:
+ long nextId = 0;
- std::unordered_map<MemoryID, std::vector<Callback>> callbacks;
+ struct ManagedCallback
+ {
+ long id = 0;
+ Callback callback;
+ };
- private:
+ std::unordered_map<MemoryID, std::vector<ManagedCallback>> callbacks;
- armarx::ManagedIceObject* component;
+ /// memoryName -> #callbacks needing memory topic
+ std::unordered_map<std::string, int> memoryRefCount;
+ private:
+ armarx::ManagedIceObject* component;
};
-}
+} // namespace armarx::armem::client::util
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/client/util/SubscriptionHandle.cpp b/source/RobotAPI/libraries/armem/client/util/SubscriptionHandle.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..114146c91699ff6a8a7fdf88bef61fdca734b892
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/client/util/SubscriptionHandle.cpp
@@ -0,0 +1,73 @@
+#include "SubscriptionHandle.h"
+
+#include "MemoryListener.h"
+
+namespace armarx::armem::client::util
+{
+ SubscriptionHandle::SubscriptionHandle(MemoryListener* memoryListener,
+ const MemoryID& memoryID,
+ long id) :
+ valid{true}, memoryListener{memoryListener}, memoryID(memoryID), id{id}
+ {
+ }
+
+ SubscriptionHandle::SubscriptionHandle() : valid{false}
+ {
+ }
+
+ SubscriptionHandle::SubscriptionHandle(SubscriptionHandle&& other) :
+ valid{other.valid},
+ memoryListener{other.memoryListener},
+ memoryID(std::move(other.memoryID)),
+ id{other.id}
+ {
+ other.valid = false;
+ }
+
+ SubscriptionHandle&
+ SubscriptionHandle::operator=(SubscriptionHandle other)
+ {
+ swap(*this, other);
+ return *this;
+ }
+
+ void
+ SubscriptionHandle::release()
+ {
+ memoryListener->unsubscribe(*this);
+ }
+
+ ScopedSubscriptionHandle::ScopedSubscriptionHandle()
+ {
+ }
+
+ ScopedSubscriptionHandle::ScopedSubscriptionHandle(SubscriptionHandle&& handle) :
+ handle(std::move(handle))
+ {
+ }
+
+ ScopedSubscriptionHandle&
+ ScopedSubscriptionHandle::operator=(SubscriptionHandle handle)
+ {
+ std::swap(this->handle, handle);
+ return *this;
+ }
+
+ ScopedSubscriptionHandle::~ScopedSubscriptionHandle()
+ {
+ handle.release();
+ }
+
+} // namespace armarx::armem::client::util
+
+namespace armarx::armem::client
+{
+ void
+ util::swap(util::SubscriptionHandle& first, util::SubscriptionHandle& second)
+ {
+ std::swap(first.valid, second.valid);
+ std::swap(first.memoryListener, second.memoryListener);
+ std::swap(first.memoryID, second.memoryID);
+ std::swap(first.id, second.id);
+ }
+} // namespace armarx::armem::client
diff --git a/source/RobotAPI/libraries/armem/client/util/SubscriptionHandle.h b/source/RobotAPI/libraries/armem/client/util/SubscriptionHandle.h
new file mode 100644
index 0000000000000000000000000000000000000000..6847ac9a7273cef86d91548b2e66befc8573c015
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/client/util/SubscriptionHandle.h
@@ -0,0 +1,62 @@
+#pragma once
+
+#include <RobotAPI/libraries/armem/core/MemoryID.h>
+
+namespace armarx::armem::client::util
+{
+
+ class MemoryListener;
+
+ class SubscriptionHandle
+ {
+ friend class MemoryListener;
+
+ public:
+ SubscriptionHandle();
+ SubscriptionHandle(SubscriptionHandle&& other);
+
+ /**
+ * @brief Assignment operator.
+ *
+ * @note Intentional call by value, since this leverages the move constructor. See
+ * https://stackoverflow.com/a/11540204 (section "Move assignment operators").
+ */
+ SubscriptionHandle& operator=(SubscriptionHandle other);
+
+ friend void swap(SubscriptionHandle& first, SubscriptionHandle& second);
+
+ void release();
+
+ private:
+ SubscriptionHandle(MemoryListener* memoryListener, const MemoryID& memoryID, long id);
+
+ private:
+ bool valid = false;
+ MemoryListener* memoryListener = nullptr;
+ MemoryID memoryID;
+ long id = 0;
+ };
+
+ class ScopedSubscriptionHandle
+ {
+ public:
+ ScopedSubscriptionHandle();
+ ScopedSubscriptionHandle(SubscriptionHandle&& handle);
+
+ /**
+ * @brief Assignment operator.
+ *
+ * @note Intentional call by value, since this leverages the move constructor. See
+ * https://stackoverflow.com/a/11540204 (section "Move assignment operators").
+ */
+ ScopedSubscriptionHandle& operator=(SubscriptionHandle handle);
+
+ ~ScopedSubscriptionHandle();
+
+ private:
+ SubscriptionHandle handle;
+ };
+
+ void swap(SubscriptionHandle& first, SubscriptionHandle& second);
+
+} // namespace armarx::armem::client::util
diff --git a/source/RobotAPI/libraries/armem/core/error/ArMemError.cpp b/source/RobotAPI/libraries/armem/core/error/ArMemError.cpp
index 354b95712761f6a14d3fb6deb5baeff6b2a18aba..7abe9891d8a1d4f599ae1e135d0dbe8e0d2a29dd 100644
--- a/source/RobotAPI/libraries/armem/core/error/ArMemError.cpp
+++ b/source/RobotAPI/libraries/armem/core/error/ArMemError.cpp
@@ -246,4 +246,19 @@ namespace armarx::armem::error
return sstream.str();
}
+ ReadStreamAlreadyPolling::ReadStreamAlreadyPolling(const MemoryID& queriedId,
+ const std::string& calledFunction) :
+ ArMemError(makeMsg(queriedId, calledFunction))
+ {
+ }
+
+ std::string
+ ReadStreamAlreadyPolling::makeMsg(const MemoryID& queriedId, const std::string& calledFunction)
+ {
+ std::stringstream ss;
+ ss << "The ReadStream for " << queriedId << " was already running when " << calledFunction
+ << " was called.";
+ return ss.str();
+ }
+
} // namespace armarx::armem::error
diff --git a/source/RobotAPI/libraries/armem/core/error/ArMemError.h b/source/RobotAPI/libraries/armem/core/error/ArMemError.h
index 69ab7d4ddff8e22791272c520386e2fdff3d6e6f..67435f8d3c4142d3453fc6f6626e4e48ebb33cad 100644
--- a/source/RobotAPI/libraries/armem/core/error/ArMemError.h
+++ b/source/RobotAPI/libraries/armem/core/error/ArMemError.h
@@ -220,4 +220,15 @@ namespace armarx::armem::error
static std::string makeMsg(const std::string& proxyName, const std::string& message = "");
};
+ /**
+ * @brief Indicates that a ReadStream is already polling when a polling method was called.
+ */
+ class ReadStreamAlreadyPolling : public ArMemError
+ {
+ public:
+ ReadStreamAlreadyPolling(const MemoryID& queriedId, const std::string& calledFunction);
+
+ static std::string makeMsg(const MemoryID& queriedId, const std::string& calledFunction);
+ };
+
} // namespace armarx::armem::error
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/aron/ObjectClass.xml b/source/RobotAPI/libraries/armem_objects/aron/ObjectClass.xml
index e2d695429d3c4193ad97df106821f5bb912a0028..019987f5054e844478066d377ad0bf1e948b6db7 100644
--- a/source/RobotAPI/libraries/armem_objects/aron/ObjectClass.xml
+++ b/source/RobotAPI/libraries/armem_objects/aron/ObjectClass.xml
@@ -13,6 +13,30 @@ Core segment type of Object/Class.
</AronIncludes>
<GenerateTypes>
+ <Object name="armarx::armem::arondto::Feature">
+ <ObjectChild key="angle">
+ <float32 />
+ </ObjectChild>
+
+ <ObjectChild key="scale">
+ <float32 />
+ </ObjectChild>
+
+ <ObjectChild key="point2d">
+ <vector2f />
+ </ObjectChild>
+
+ <ObjectChild key="point3d">
+ <vector3f />
+ </ObjectChild>
+
+ <ObjectChild key="feature">
+ <List>
+ <float32 />
+ </List>
+ </ObjectChild>
+ </Object>
+
<Object name="armarx::armem::arondto::ObjectClass">
<ObjectChild key="id">
@@ -62,6 +86,12 @@ Core segment type of Object/Class.
<armarx::arondto::ObjectNames />
</ObjectChild>
+ <ObjectChild key="ivtFeatures">
+ <List>
+ <armarx::armem::arondto::Feature />
+ </List>
+ </ObjectChild>
+
</Object>
</GenerateTypes>
diff --git a/source/RobotAPI/libraries/armem_objects/aron_conversions.cpp b/source/RobotAPI/libraries/armem_objects/aron_conversions.cpp
index 6b3f3c184edb32070f6d7f28c853f340bcd78d32..da32c2771a2149f58e5ba3a1a01db93ecbe71028 100644
--- a/source/RobotAPI/libraries/armem_objects/aron_conversions.cpp
+++ b/source/RobotAPI/libraries/armem_objects/aron_conversions.cpp
@@ -111,6 +111,71 @@ namespace armarx::armem
} // namespace armarx::armem
+namespace armarx::armem::clazz
+{
+
+ void
+ fromAron(const arondto::Feature& dto, Feature& bo)
+ {
+ bo.angle = dto.angle;
+ bo.scale = dto.scale;
+ bo.feature = dto.feature;
+ bo.point2d = dto.point2d;
+ bo.point3d = dto.point3d;
+ }
+
+ void
+ toAron(arondto::Feature& dto, const Feature& bo)
+ {
+ dto.angle = bo.angle;
+ dto.scale = bo.scale;
+ dto.feature = bo.feature;
+ dto.point2d = bo.point2d;
+ dto.point3d = bo.point3d;
+ }
+
+ void
+ fromAron(const arondto::ObjectClass& dto, ObjectClass& bo)
+ {
+ armarx::fromAron(dto.id, bo.id);
+ armarx::fromAron(dto.simoxXmlPath, bo.simoxXmlPath);
+ armarx::fromAron(dto.articulatedSimoxXmlPath, bo.articulatedSimoxXmlPath);
+ armarx::fromAron(dto.urdfPath, bo.urdfPath);
+ armarx::fromAron(dto.articulatedUrdfPath, bo.articulatedUrdfPath);
+ armarx::fromAron(dto.sdfPath, bo.sdfPath);
+ armarx::fromAron(dto.meshWrlPath, bo.meshWrlPath);
+ armarx::fromAron(dto.meshObjPath, bo.meshObjPath);
+ armarx::fromAron(dto.aabb, bo.aabb);
+ armarx::fromAron(dto.oobb, bo.oobb);
+ bo.ivtFeatures.clear();
+ for (const auto& i : dto.ivtFeatures)
+ {
+ fromAron(i, bo.ivtFeatures.emplace_back());
+ }
+ }
+
+ void
+ toAron(arondto::ObjectClass& dto, const ObjectClass& bo)
+ {
+ armarx::toAron(dto.id, bo.id);
+ armarx::toAron(dto.simoxXmlPath, bo.simoxXmlPath);
+ armarx::toAron(dto.articulatedSimoxXmlPath, bo.articulatedSimoxXmlPath);
+ armarx::toAron(dto.urdfPath, bo.urdfPath);
+ armarx::toAron(dto.articulatedUrdfPath, bo.articulatedUrdfPath);
+ armarx::toAron(dto.sdfPath, bo.sdfPath);
+ armarx::toAron(dto.meshWrlPath, bo.meshWrlPath);
+ armarx::toAron(dto.meshObjPath, bo.meshObjPath);
+ armarx::toAron(dto.aabb, bo.aabb);
+ armarx::toAron(dto.oobb, bo.oobb);
+ dto.ivtFeatures.clear();
+ for (const auto& i : bo.ivtFeatures)
+ {
+ toAron(dto.ivtFeatures.emplace_back(), i);
+ }
+ }
+
+} // namespace armarx::armem::clazz
+
armarx::armem::MemoryID
armarx::armem::obj::makeObjectInstanceMemoryID(const objpose::ObjectPose& objectPose)
{
diff --git a/source/RobotAPI/libraries/armem_objects/aron_conversions.h b/source/RobotAPI/libraries/armem_objects/aron_conversions.h
index e27ed16c4cede302e2eecf2c013440d240162550..b61164a5d6a7de07bd8ba3761117e263ac714949 100644
--- a/source/RobotAPI/libraries/armem_objects/aron_conversions.h
+++ b/source/RobotAPI/libraries/armem_objects/aron_conversions.h
@@ -1,11 +1,10 @@
#pragma once
#include <RobotAPI/libraries/ArmarXObjects/ObjectPose.h>
-
-#include <RobotAPI/libraries/armem_objects/aron/ObjectInstance.aron.generated.h>
#include <RobotAPI/libraries/armem_objects/aron/Attachment.aron.generated.h>
#include <RobotAPI/libraries/armem_objects/aron/Marker.aron.generated.h>
-
+#include <RobotAPI/libraries/armem_objects/aron/ObjectClass.aron.generated.h>
+#include <RobotAPI/libraries/armem_objects/aron/ObjectInstance.aron.generated.h>
#include <RobotAPI/libraries/armem_objects/types.h>
namespace armarx::armem
@@ -18,19 +17,32 @@ namespace armarx::armem
/* Attachments */
- void fromAron(const arondto::attachment::AgentDescription& dto, attachment::AgentDescription& bo);
+ void fromAron(const arondto::attachment::AgentDescription& dto,
+ attachment::AgentDescription& bo);
void toAron(arondto::attachment::AgentDescription& dto, const attachment::AgentDescription& bo);
- void fromAron(const arondto::attachment::ObjectAttachment& dto, attachment::ObjectAttachment& bo);
+ void fromAron(const arondto::attachment::ObjectAttachment& dto,
+ attachment::ObjectAttachment& bo);
void toAron(arondto::attachment::ObjectAttachment& dto, const attachment::ObjectAttachment& bo);
- void fromAron(const arondto::attachment::ArticulatedObjectAttachment& dto, attachment::ArticulatedObjectAttachment& bo);
- void toAron(arondto::attachment::ArticulatedObjectAttachment& dto, const attachment::ArticulatedObjectAttachment& bo);
+ void fromAron(const arondto::attachment::ArticulatedObjectAttachment& dto,
+ attachment::ArticulatedObjectAttachment& bo);
+ void toAron(arondto::attachment::ArticulatedObjectAttachment& dto,
+ const attachment::ArticulatedObjectAttachment& bo);
+
+ void fromAron(const arondto::Marker& dto, marker::Marker& bo);
+ void toAron(arondto::Marker& dto, const marker::Marker& bo);
+} // namespace armarx::armem
+
+namespace armarx::armem::clazz
+{
+ void fromAron(const armarx::armem::arondto::Feature& dto, Feature& bo);
+ void toAron(armarx::armem::arondto::Feature& dto, const Feature& bo);
- void fromAron(const arondto::Marker& dto, marker::Marker&bo);
- void toAron(arondto::Marker& dto, const marker::Marker&bo);
-} // namespace armarx::armem
+ void fromAron(const armarx::armem::arondto::ObjectClass& dto, ObjectClass& bo);
+ void toAron(armarx::armem::arondto::ObjectClass& dto, const ObjectClass& bo);
+} // namespace armarx::armem::clazz
#include <RobotAPI/libraries/armem/core/MemoryID.h>
@@ -38,4 +50,4 @@ namespace armarx::armem::obj
{
/// Make a Memory ID for the object instance snapshot representing this pose.
MemoryID makeObjectInstanceMemoryID(const objpose::ObjectPose& objectPose);
-}
+} // namespace armarx::armem::obj
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_objects/types.h b/source/RobotAPI/libraries/armem_objects/types.h
index 79fe9ac956f65869ceb03b35db8545155c4c963c..598c78ca9c6bc5f4fc007059352ee94fc28b2fec 100644
--- a/source/RobotAPI/libraries/armem_objects/types.h
+++ b/source/RobotAPI/libraries/armem_objects/types.h
@@ -23,6 +23,8 @@
#include <Eigen/Geometry>
+#include <SimoxUtility/shapes/AxisAlignedBoundingBox.h>
+
#include <RobotAPI/libraries/ArmarXObjects/ObjectPose.h>
#include <RobotAPI/libraries/armem/core/MemoryID.h>
#include <RobotAPI/libraries/armem/core/Time.h>
@@ -31,7 +33,6 @@
#include "aron_forward_declarations.h"
-
namespace armarx::armem
{
struct ObjectInstance
@@ -40,7 +41,36 @@ namespace armarx::armem
MemoryID classID;
MemoryID sourceID;
};
-}
+} // namespace armarx::armem
+
+namespace armarx::armem::clazz
+{
+ struct Feature
+ {
+ float angle;
+ float scale;
+ Eigen::Vector2f point2d;
+ Eigen::Vector3f point3d;
+ std::vector<float> feature;
+ };
+
+ struct ObjectClass
+ {
+ armarx::ObjectID id;
+ armarx::PackagePath simoxXmlPath;
+ armarx::PackagePath articulatedSimoxXmlPath;
+ armarx::PackagePath urdfPath;
+ armarx::PackagePath articulatedUrdfPath;
+ armarx::PackagePath sdfPath;
+ armarx::PackagePath articulatedSdfPath;
+ armarx::PackagePath meshWrlPath;
+ armarx::PackagePath meshObjPath;
+ simox::AxisAlignedBoundingBox aabb;
+ simox::OrientedBoxf oobb;
+ // TODO NAMES
+ std::vector<Feature> ivtFeatures;
+ };
+} // namespace armarx::armem::clazz
namespace armarx::armem::attachment
{
@@ -110,7 +140,6 @@ namespace armarx::armem::articulated_object
using ArticulatedObjects = armarx::armem::robot::Robots;
} // namespace armarx::armem::articulated_object
-
namespace armarx::armem::marker
{
class Marker
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/common/CMakeLists.txt b/source/RobotAPI/libraries/aron/common/CMakeLists.txt
index f8109b4a31f9aa10cfb3caae5b557e4a0c96a391..36cbeb1f837f96ea05b131a843654481b5195bb9 100644
--- a/source/RobotAPI/libraries/aron/common/CMakeLists.txt
+++ b/source/RobotAPI/libraries/aron/common/CMakeLists.txt
@@ -28,6 +28,7 @@ armarx_add_library(
aron_conversions/eigen.h
json_conversions/armarx.h
+ json_conversions/framed.h
rw/time.h
rw/eigen.h
@@ -47,6 +48,7 @@ armarx_add_library(
aron_conversions/eigen.cpp
json_conversions/armarx.cpp
+ json_conversions/framed.cpp
rw/time.cpp
rw/eigen.cpp
diff --git a/source/RobotAPI/libraries/aron/common/forward_declarations.h b/source/RobotAPI/libraries/aron/common/forward_declarations.h
index 626147c3fc35e827a7b905c8a8b0e76babf3f25e..d3c7df9f82d251032ed5f1e49180f74f30cf223f 100644
--- a/source/RobotAPI/libraries/aron/common/forward_declarations.h
+++ b/source/RobotAPI/libraries/aron/common/forward_declarations.h
@@ -1,13 +1,14 @@
#pragma once
-
namespace simox::arondto
{
class AxisAlignedBoundingBox;
class Color;
class OrientedBox;
-}
+} // namespace simox::arondto
+
namespace armarx::arondto
{
class Names;
-}
+ class FrameID;
+} // namespace armarx::arondto
diff --git a/source/RobotAPI/libraries/aron/common/json_conversions.h b/source/RobotAPI/libraries/aron/common/json_conversions.h
index a56bd44287eb16161f84e5d1ef0f5496903e2669..b29300e6cf827cfd0908521a3b9fa040b3b32a3d 100644
--- a/source/RobotAPI/libraries/aron/common/json_conversions.h
+++ b/source/RobotAPI/libraries/aron/common/json_conversions.h
@@ -1,3 +1,4 @@
#pragma once
#include "json_conversions/armarx.h"
+#include "json_conversions/framed.h"
diff --git a/source/RobotAPI/libraries/aron/common/json_conversions/framed.cpp b/source/RobotAPI/libraries/aron/common/json_conversions/framed.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..fa1cf6ae4237e505e00d8ac069824a0004a66e16
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/common/json_conversions/framed.cpp
@@ -0,0 +1,17 @@
+#include "framed.h"
+
+#include <RobotAPI/libraries/aron/common/aron/framed.aron.generated.h>
+
+void
+armarx::arondto::to_json(nlohmann::json& j, const FrameID& bo)
+{
+ j["agent"] = bo.agent;
+ j["frame"] = bo.frame;
+}
+
+void
+armarx::arondto::from_json(const nlohmann::json& j, FrameID& bo)
+{
+ j.at("agent").get_to(bo.agent);
+ j.at("frame").get_to(bo.frame);
+}
diff --git a/source/RobotAPI/libraries/aron/common/json_conversions/framed.h b/source/RobotAPI/libraries/aron/common/json_conversions/framed.h
new file mode 100644
index 0000000000000000000000000000000000000000..fb7314e83628ab4ddf951766139fedeee731a0e9
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/common/json_conversions/framed.h
@@ -0,0 +1,12 @@
+#pragma once
+
+#include <SimoxUtility/json/json.h>
+
+#include <RobotAPI/libraries/aron/common/forward_declarations.h>
+
+namespace armarx::arondto
+{
+ void to_json(nlohmann::json& j, const FrameID& bo);
+ void from_json(const nlohmann::json& j, FrameID& bo);
+
+} // namespace armarx::arondto
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/SkillStatusUpdate.h b/source/RobotAPI/libraries/skills/core/SkillStatusUpdate.h
index d517fc1ec8aa862d041a4d18fcbcc981f3c6f3f4..ace31510a8a362ed45289c4facdd51c97ba22fbb 100644
--- a/source/RobotAPI/libraries/skills/core/SkillStatusUpdate.h
+++ b/source/RobotAPI/libraries/skills/core/SkillStatusUpdate.h
@@ -86,6 +86,12 @@ namespace armarx
return true;
}
+ bool
+ hasBeenSucceeded() const
+ {
+ return status == TerminatedSkillStatus::Succeeded;
+ }
+
manager::dto::SkillStatusUpdate toManagerIce() const;
provider::dto::SkillStatusUpdate toProviderIce() const;
@@ -111,6 +117,12 @@ namespace armarx
status == ActiveOrTerminatedSkillStatus::Aborted;
}
+ bool
+ hasBeenSucceeded() const
+ {
+ return status == ActiveOrTerminatedSkillStatus::Succeeded;
+ }
+
manager::dto::SkillStatusUpdate toManagerIce() const;
provider::dto::SkillStatusUpdate toProviderIce() const;
@@ -195,6 +207,12 @@ namespace armarx
status == SkillStatus::Aborted;
}
+ bool
+ hasBeenSucceeded() const
+ {
+ return status == SkillStatus::Succeeded;
+ }
+
manager::dto::SkillStatusUpdate toManagerIce() const;
provider::dto::SkillStatusUpdate toProviderIce() const;
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/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