From 188f576d5934b3045ff78e264b5de65f4b96039d Mon Sep 17 00:00:00 2001
From: Fabian Peller <fabian.peller-konrad@kit.edu>
Date: Wed, 8 Nov 2023 15:58:51 +0100
Subject: [PATCH] added framework for robot definitions and global robot name
service
---
source/RobotAPI/components/CMakeLists.txt | 1 +
.../components/RobotDefinition/CMakeLists.txt | 26 ++++
.../RobotDefinition/RobotDefinition.cpp | 55 +++++++
.../RobotDefinition/RobotDefinition.h | 88 +++++++++++
.../RobotDefinition/test/CMakeLists.txt | 5 +
.../test/RobotNameServiceTest.cpp} | 22 ++-
.../RobotNameService/CMakeLists.txt | 30 +++-
.../RobotNameService/RobotNameService.cpp | 63 ++++++--
.../RobotNameService/RobotNameService.h | 44 +++---
source/RobotAPI/interface/CMakeLists.txt | 4 +-
.../RobotNameServiceInterface.ice | 90 +++++++++++
source/RobotAPI/libraries/CMakeLists.txt | 2 +
.../CMakeLists.txt | 0
.../robot_name_service/client/CMakeLists.txt | 27 ++++
.../robot_name_service/client/Plugin.cpp | 145 ++++++++++++++++++
.../robot_name_service/client/Plugin.h | 81 ++++++++++
.../robot_name_service/client/plugins.cpp | 6 +
.../robot_name_service/client/plugins.h | 8 +
.../robot_name_service/core/CMakeLists.txt | 24 +++
.../robot_name_service/core/Robot.cpp | 79 ++++++++++
.../libraries/robot_name_service/core/Robot.h | 59 +++++++
.../robot_name_service/server/CMakeLists.txt | 25 +++
.../robot_name_service/server/plugins.cpp | 6 +
.../robot_name_service/server/plugins.h | 6 +
24 files changed, 849 insertions(+), 47 deletions(-)
create mode 100644 source/RobotAPI/components/RobotDefinition/CMakeLists.txt
create mode 100644 source/RobotAPI/components/RobotDefinition/RobotDefinition.cpp
create mode 100644 source/RobotAPI/components/RobotDefinition/RobotDefinition.h
create mode 100644 source/RobotAPI/components/RobotDefinition/test/CMakeLists.txt
rename source/RobotAPI/{interface/components/RobotNameServiceInterface.ice => components/RobotDefinition/test/RobotNameServiceTest.cpp} (63%)
create mode 100644 source/RobotAPI/interface/robot_name_service/RobotNameServiceInterface.ice
rename source/RobotAPI/libraries/{robotapi => robot_name_service}/CMakeLists.txt (100%)
create mode 100644 source/RobotAPI/libraries/robot_name_service/client/CMakeLists.txt
create mode 100644 source/RobotAPI/libraries/robot_name_service/client/Plugin.cpp
create mode 100644 source/RobotAPI/libraries/robot_name_service/client/Plugin.h
create mode 100644 source/RobotAPI/libraries/robot_name_service/client/plugins.cpp
create mode 100644 source/RobotAPI/libraries/robot_name_service/client/plugins.h
create mode 100644 source/RobotAPI/libraries/robot_name_service/core/CMakeLists.txt
create mode 100644 source/RobotAPI/libraries/robot_name_service/core/Robot.cpp
create mode 100644 source/RobotAPI/libraries/robot_name_service/core/Robot.h
create mode 100644 source/RobotAPI/libraries/robot_name_service/server/CMakeLists.txt
create mode 100644 source/RobotAPI/libraries/robot_name_service/server/plugins.cpp
create mode 100644 source/RobotAPI/libraries/robot_name_service/server/plugins.h
diff --git a/source/RobotAPI/components/CMakeLists.txt b/source/RobotAPI/components/CMakeLists.txt
index 11908aeda..fcd09e915 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 000000000..58ce4d1f0
--- /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 000000000..6a2df4ccd
--- /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 000000000..ba4eb80b9
--- /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 000000000..154bbec83
--- /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 3478d4066..6e3e6a973 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 1205ad25b..c362b8dab 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 65f97877d..9ab8e07d7 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 92f594234..7af787aef 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/interface/CMakeLists.txt b/source/RobotAPI/interface/CMakeLists.txt
index 9e290598f..d8f626d9f 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 000000000..7891ed5db
--- /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 057440c94..4d481f861 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/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 000000000..c59cbcaa1
--- /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 000000000..89f973a6e
--- /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 000000000..813003f98
--- /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 000000000..51f19fb34
--- /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 000000000..a37a92b69
--- /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 000000000..6c702dd1e
--- /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 000000000..bfa5fbfe5
--- /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 000000000..243d94c1b
--- /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 000000000..d0c268aa4
--- /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 000000000..fc90afa38
--- /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 000000000..ca43f153e
--- /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
--
GitLab