From 5d2d2f303f7039c632bb51f9439c9e1b9e7f57e0 Mon Sep 17 00:00:00 2001
From: ARMAR-7a User <armar7a@h2t>
Date: Tue, 21 Nov 2023 09:37:12 +0100
Subject: [PATCH] changes from A7 pc1
---
.../RobotNameService/RobotNameService.cpp | 10 ++--
.../RobotNameService/RobotNameService.h | 12 ++---
.../RobotNameServiceInterface.ice | 42 +++++++++++------
.../libraries/armem_robot/aron/Robot.xml | 1 -
.../robot_name_service/client/Plugin.cpp | 10 ++--
.../robot_name_service/client/Plugin.h | 2 +-
.../robot_name_service/core/CMakeLists.txt | 10 ++++
.../robot_name_service/core/Robot.cpp | 46 +++++++++----------
.../libraries/robot_name_service/core/Robot.h | 26 +++++------
.../robot_name_service/core/aron/Robot.xml | 40 ++++++++++++++++
.../core/aron_conversions.cpp | 6 +++
.../core/aron_conversions.h | 14 ++++++
.../robot_name_service/server/CMakeLists.txt | 2 +
.../server/segments/RobotSegment.cpp | 6 +++
.../server/segments/RobotSegment.h | 17 +++++++
15 files changed, 174 insertions(+), 70 deletions(-)
create mode 100644 source/RobotAPI/libraries/robot_name_service/core/aron/Robot.xml
create mode 100644 source/RobotAPI/libraries/robot_name_service/core/aron_conversions.cpp
create mode 100644 source/RobotAPI/libraries/robot_name_service/core/aron_conversions.h
create mode 100644 source/RobotAPI/libraries/robot_name_service/server/segments/RobotSegment.cpp
create mode 100644 source/RobotAPI/libraries/robot_name_service/server/segments/RobotSegment.h
diff --git a/source/RobotAPI/components/RobotNameService/RobotNameService.cpp b/source/RobotAPI/components/RobotNameService/RobotNameService.cpp
index 9ab8e07d7..397a63891 100644
--- a/source/RobotAPI/components/RobotNameService/RobotNameService.cpp
+++ b/source/RobotAPI/components/RobotNameService/RobotNameService.cpp
@@ -54,8 +54,8 @@ namespace armarx
}
bool
- RobotNameService::registerRobot(const robot_name_service::dto::Robot& robot,
- const Ice::Current& current)
+ RobotNameService::registerRobotInfo(const robot_name_service::dto::RobotInfo& robot,
+ const Ice::Current& current)
{
std::scoped_lock l(robotsMutex);
ARMARX_INFO << "Register a new robot with name '" << robot.name << "' in RNS";
@@ -72,7 +72,7 @@ namespace armarx
}
void
- RobotNameService::unregisterRobot(const std::string& name, const Ice::Current& current)
+ RobotNameService::unregisterRobotInfo(const std::string& name, const Ice::Current& current)
{
std::scoped_lock l(robotsMutex);
@@ -82,8 +82,8 @@ namespace armarx
}
}
- IceUtil::Optional<robot_name_service::dto::Robot>
- RobotNameService::getRobot(const std::string& name, const Ice::Current& current)
+ IceUtil::Optional<robot_name_service::dto::RobotInfo>
+ RobotNameService::getRobotInfo(const std::string& name, const Ice::Current& current)
{
std::scoped_lock l(robotsMutex);
diff --git a/source/RobotAPI/components/RobotNameService/RobotNameService.h b/source/RobotAPI/components/RobotNameService/RobotNameService.h
index 7af787aef..6cc056c19 100644
--- a/source/RobotAPI/components/RobotNameService/RobotNameService.h
+++ b/source/RobotAPI/components/RobotNameService/RobotNameService.h
@@ -87,14 +87,14 @@ namespace armarx
// RobotNameServiceInterface interface
public:
- bool registerRobot(const robot_name_service::dto::Robot& robot,
- const Ice::Current& current) override;
- void unregisterRobot(const std::string& name, const Ice::Current& current) override;
- IceUtil::Optional<robot_name_service::dto::Robot>
- getRobot(const std::string& name, const Ice::Current& current) override;
+ bool registerRobotInfo(const robot_name_service::dto::RobotInfo& robot,
+ const Ice::Current& current) override;
+ void unregisterRobotInfo(const std::string& name, const Ice::Current& current) override;
+ IceUtil::Optional<robot_name_service::dto::RobotInfo>
+ getRobotInfo(const std::string& name, const Ice::Current& current) override;
private:
mutable std::mutex robotsMutex;
- std::map<std::string, robot_name_service::core::Robot> robots;
+ std::map<std::string, robot_name_service::core::RobotInfo> robots;
};
} // namespace armarx
diff --git a/source/RobotAPI/interface/robot_name_service/RobotNameServiceInterface.ice b/source/RobotAPI/interface/robot_name_service/RobotNameServiceInterface.ice
index 7891ed5db..5fb248cd5 100644
--- a/source/RobotAPI/interface/robot_name_service/RobotNameServiceInterface.ice
+++ b/source/RobotAPI/interface/robot_name_service/RobotNameServiceInterface.ice
@@ -27,10 +27,12 @@
#include <RobotAPI/interface/armem/mns.ice>
#include <RobotAPI/interface/skills/SkillManagerInterface.ice>
+#include <RobotAPI/interface/units/ForceTorqueUnit.ice>
#include <RobotAPI/interface/units/HandUnitInterface.ice>
#include <RobotAPI/interface/units/KinematicUnitInterface.ice>
#include <RobotAPI/interface/units/LocalizationUnitInterface.ice>
#include <RobotAPI/interface/units/PlatformUnitInterface.ice>
+#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.ice>
module armarx
{
@@ -39,24 +41,38 @@ module armarx
module dto
{
- struct Hand
+ module location
+ {
+ enum Side
+ {
+ LEFT,
+ RIGHT,
+ UNKNOWN
+ };
+ };
+
+ struct TCPInfo
{
string name;
string ft_name;
+ location::Side side;
HandUnitInterface* handUnitInterface;
};
- dictionary<string, Hand> Hands;
+ // maps tcp name to tcp
+ dictionary<string, TCPInfo> TCPInfos;
- struct Arm
+ struct ArmInfo
{
string kinematicChainName;
- Hand hand;
+ location::Side side;
+ TCPInfo tcpInfo;
};
- dictionary<string, Arm> Arms;
+ // maps kinematic chain name to arm
+ dictionary<string, ArmInfo> ArmInfos;
- struct Robot
+ struct RobotInfo
{
string name;
armarx::data::PackagePath xmlPackagePath;
@@ -66,12 +82,10 @@ module armarx
armarx::skills::manager::dti::SkillManagerInterface* skillManager;
// kinematic stuff. MAY BE EMPTY
- Arms arms;
+ ArmInfos armInfos;
- // units. MAY BE NULL
- armarx::KinematicUnitInterface* kinematicUnitInterface;
- armarx::PlatformUnitInterface* platformUnitInterface;
- armarx::LocalizationUnitInterface* localizationUnitInterface;
+ // RobotUnit
+ armarx::RobotUnitInterface* robotUnit;
// TODO: Feel free to extend!
};
@@ -81,9 +95,9 @@ module armarx
{
interface RobotNameServiceInterface
{
- bool registerRobot(dto::Robot robot);
- void unregisterRobot(string name);
- optional(1) dto::Robot getRobot(string name);
+ bool registerRobotInfo(dto::RobotInfo robot);
+ void unregisterRobotInfo(string name);
+ optional(1) dto::RobotInfo getRobotInfo(string name);
};
};
};
diff --git a/source/RobotAPI/libraries/armem_robot/aron/Robot.xml b/source/RobotAPI/libraries/armem_robot/aron/Robot.xml
index 7303d707d..b7192c366 100644
--- a/source/RobotAPI/libraries/armem_robot/aron/Robot.xml
+++ b/source/RobotAPI/libraries/armem_robot/aron/Robot.xml
@@ -1,4 +1,3 @@
-<!--This class contains the data structure for ObjectPose -->
<?xml version="1.0" encoding="UTF-8" ?>
<AronTypeDefinition>
<AronIncludes>
diff --git a/source/RobotAPI/libraries/robot_name_service/client/Plugin.cpp b/source/RobotAPI/libraries/robot_name_service/client/Plugin.cpp
index 89f973a6e..0f944891c 100644
--- a/source/RobotAPI/libraries/robot_name_service/client/Plugin.cpp
+++ b/source/RobotAPI/libraries/robot_name_service/client/Plugin.cpp
@@ -19,6 +19,8 @@ namespace armarx::plugins
{
using namespace armarx::robot_name_service::client::constants;
+ ARMARX_INFO << "Got as config: " << properties.configJson;
+
nlohmann::json config = nlohmann::json::parse(properties.configJson);
if (config.contains(CONFIG_ROBOT_NAME))
@@ -53,7 +55,7 @@ namespace armarx::plugins
{
for (const auto& [k, config_arm] : config[CONFIG_ARMS].items())
{
- auto& arm = robot.arms[k];
+ auto& arm = robot.armInfos[k];
if (config_arm.contains(CONFIG_ARM_KINEMATIC_CHAIN_NAME))
{
@@ -109,7 +111,7 @@ namespace armarx::plugins
{
for (const auto& [k, config_arm] : config[CONFIG_ARMS].items())
{
- auto& arm = robot.arms[k];
+ auto& arm = robot.armInfos[k];
if (config_arm.contains(CONFIG_ARM_HAND))
{
@@ -126,7 +128,7 @@ namespace armarx::plugins
}
// finally commit robot
- properties.rns->registerRobot(robot.toIce());
+ properties.rns->registerRobotInfo(robot.toIce());
}
void
@@ -139,7 +141,7 @@ namespace armarx::plugins
void
RobotNameServiceComponentPlugin::preOnDisconnectComponent()
{
- properties.rns->unregisterRobot(robot.name);
+ properties.rns->unregisterRobotInfo(robot.name);
}
} // namespace armarx::plugins
diff --git a/source/RobotAPI/libraries/robot_name_service/client/Plugin.h b/source/RobotAPI/libraries/robot_name_service/client/Plugin.h
index 813003f98..5d59cd16f 100644
--- a/source/RobotAPI/libraries/robot_name_service/client/Plugin.h
+++ b/source/RobotAPI/libraries/robot_name_service/client/Plugin.h
@@ -60,7 +60,7 @@ namespace armarx::plugins
armarx::robot_name_service::dti::RobotNameServiceInterfacePrx rns;
} properties;
- armarx::robot_name_service::core::Robot robot;
+ armarx::robot_name_service::core::RobotInfo robot;
friend class armarx::RobotNameServiceComponentPluginUser;
};
diff --git a/source/RobotAPI/libraries/robot_name_service/core/CMakeLists.txt b/source/RobotAPI/libraries/robot_name_service/core/CMakeLists.txt
index 6c702dd1e..19ac42cfe 100644
--- a/source/RobotAPI/libraries/robot_name_service/core/CMakeLists.txt
+++ b/source/RobotAPI/libraries/robot_name_service/core/CMakeLists.txt
@@ -17,8 +17,18 @@ armarx_add_library(
SOURCES
Robot.cpp
+ aron_conversions.cpp
HEADERS
Robot.h
+ aron_conversions.h
+)
+
+armarx_enable_aron_file_generation_for_target(
+ TARGET_NAME
+ "${LIB_NAME}"
+ ARON_FILES
+ aron/Robot.xml
)
add_library(RobotAPI::robot_name_service_core ALIAS robot_name_service_core)
+
diff --git a/source/RobotAPI/libraries/robot_name_service/core/Robot.cpp b/source/RobotAPI/libraries/robot_name_service/core/Robot.cpp
index bfa5fbfe5..11978184a 100644
--- a/source/RobotAPI/libraries/robot_name_service/core/Robot.cpp
+++ b/source/RobotAPI/libraries/robot_name_service/core/Robot.cpp
@@ -4,17 +4,17 @@ namespace armarx::robot_name_service::core
{
void
- Hand::fromIce(const dto::Hand& r)
+ TCPInfo::fromIce(const dto::TCPInfo& r)
{
name = r.name;
ft_name = r.ft_name;
handUnitInterface = r.handUnitInterface;
}
- dto::Hand
- Hand::toIce() const
+ dto::TCPInfo
+ TCPInfo::toIce() const
{
- dto::Hand r;
+ dto::TCPInfo r;
r.name = name;
r.ft_name = ft_name;
r.handUnitInterface = handUnitInterface;
@@ -22,57 +22,53 @@ namespace armarx::robot_name_service::core
}
void
- Arm::fromIce(const dto::Arm& r)
+ ArmInfo::fromIce(const dto::ArmInfo& r)
{
kinematicChainName = r.kinematicChainName;
- hand.fromIce(r.hand);
+ hand.fromIce(r.tcpInfo);
}
- dto::Arm
- Arm::toIce() const
+ dto::ArmInfo
+ ArmInfo::toIce() const
{
- dto::Arm r;
+ dto::ArmInfo r;
r.kinematicChainName = kinematicChainName;
- r.hand = hand.toIce();
+ r.tcpInfo = hand.toIce();
return r;
}
void
- Robot::fromIce(const dto::Robot& r)
+ RobotInfo::fromIce(const dto::RobotInfo& r)
{
name = r.name;
xmlPackagePath = r.xmlPackagePath;
memoryNameSystem = armarx::armem::client::MemoryNameSystem(r.memoryNameSystem);
skillManager = r.skillManager;
- arms.clear();
- for (const auto& [k, a] : r.arms)
+ armInfos.clear();
+ for (const auto& [k, a] : r.armInfos)
{
- arms[k].fromIce(a);
+ armInfos[k].fromIce(a);
}
- kinematicUnitInterface = r.kinematicUnitInterface;
- platformUnitInterface = r.platformUnitInterface;
- localizationUnitInterface = r.localizationUnitInterface;
+ robotUnit = r.robotUnit;
}
- dto::Robot
- Robot::toIce() const
+ dto::RobotInfo
+ RobotInfo::toIce() const
{
- dto::Robot r;
+ dto::RobotInfo r;
r.name = name;
r.xmlPackagePath = xmlPackagePath;
r.memoryNameSystem = memoryNameSystem.getMemoryNameSystem();
r.skillManager = skillManager;
- for (const auto& [k, a] : arms)
+ for (const auto& [k, a] : armInfos)
{
- r.arms[k] = a.toIce();
+ r.armInfos[k] = a.toIce();
}
- r.kinematicUnitInterface = kinematicUnitInterface;
- r.platformUnitInterface = platformUnitInterface;
- r.localizationUnitInterface = localizationUnitInterface;
+ r.robotUnit = robotUnit;
return r;
}
diff --git a/source/RobotAPI/libraries/robot_name_service/core/Robot.h b/source/RobotAPI/libraries/robot_name_service/core/Robot.h
index 243d94c1b..4dedb60a9 100644
--- a/source/RobotAPI/libraries/robot_name_service/core/Robot.h
+++ b/source/RobotAPI/libraries/robot_name_service/core/Robot.h
@@ -10,11 +10,11 @@
namespace armarx::robot_name_service::core
{
- class Hand
+ class TCPInfo
{
public:
- void fromIce(const armarx::robot_name_service::dto::Hand& r);
- armarx::robot_name_service::dto::Hand toIce() const;
+ void fromIce(const armarx::robot_name_service::dto::TCPInfo& r);
+ armarx::robot_name_service::dto::TCPInfo toIce() const;
public:
std::string name;
@@ -22,22 +22,22 @@ namespace armarx::robot_name_service::core
armarx::HandUnitInterfacePrx handUnitInterface;
};
- class Arm
+ class ArmInfo
{
public:
- void fromIce(const armarx::robot_name_service::dto::Arm& r);
- armarx::robot_name_service::dto::Arm toIce() const;
+ void fromIce(const armarx::robot_name_service::dto::ArmInfo& r);
+ armarx::robot_name_service::dto::ArmInfo toIce() const;
public:
std::string kinematicChainName;
- Hand hand;
+ TCPInfo hand;
};
- class Robot
+ class RobotInfo
{
public:
- void fromIce(const armarx::robot_name_service::dto::Robot& r);
- armarx::robot_name_service::dto::Robot toIce() const;
+ void fromIce(const armarx::robot_name_service::dto::RobotInfo& r);
+ armarx::robot_name_service::dto::RobotInfo toIce() const;
public:
// header
@@ -49,11 +49,9 @@ namespace armarx::robot_name_service::core
armarx::skills::manager::dti::SkillManagerInterfacePrx skillManager;
// kinematic stuff
- std::map<std::string, Arm> arms;
+ std::map<std::string, ArmInfo> armInfos;
// units
- armarx::KinematicUnitInterfacePrx kinematicUnitInterface;
- armarx::PlatformUnitInterfacePrx platformUnitInterface;
- armarx::LocalizationUnitInterfacePrx localizationUnitInterface;
+ armarx::RobotUnitInterfacePrx robotUnit;
};
} // namespace armarx::robot_name_service::core
diff --git a/source/RobotAPI/libraries/robot_name_service/core/aron/Robot.xml b/source/RobotAPI/libraries/robot_name_service/core/aron/Robot.xml
new file mode 100644
index 000000000..5b29a3d3c
--- /dev/null
+++ b/source/RobotAPI/libraries/robot_name_service/core/aron/Robot.xml
@@ -0,0 +1,40 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+ <GenerateTypes>
+
+ <Object name='armarx::robot_name_service::arondto::TCPInfo'>
+ <ObjectChild key='name'>
+ <string />
+ </ObjectChild>
+
+ </Object>
+
+ <Object name='armarx::robot_name_service::arondto::TCPInfo'>
+ <ObjectChild key='name'>
+ <string />
+ </ObjectChild>
+
+ <ObjectChild key='ft_name'>
+ <string />
+ </ObjectChild>
+
+ </Object>
+
+ <Object name='armarx::robot_name_service::arondto::ArmInfo'>
+ <ObjectChild key='kinematicChainName'>
+ <string />
+ </ObjectChild>
+
+ </Object>
+
+
+ <Object name='armarx::robot_name_service::arondto::RobotInfo'>
+ <ObjectChild key='name'>
+ <string />
+ </ObjectChild>
+
+ </Object>
+
+
+ </GenerateTypes>
+</AronTypeDefinition>
diff --git a/source/RobotAPI/libraries/robot_name_service/core/aron_conversions.cpp b/source/RobotAPI/libraries/robot_name_service/core/aron_conversions.cpp
new file mode 100644
index 000000000..57d536636
--- /dev/null
+++ b/source/RobotAPI/libraries/robot_name_service/core/aron_conversions.cpp
@@ -0,0 +1,6 @@
+#include "aron_conversions.h"
+
+namespace armarx::robot_name_service::core
+{
+
+} // namespace armarx::robot_name_service::core
diff --git a/source/RobotAPI/libraries/robot_name_service/core/aron_conversions.h b/source/RobotAPI/libraries/robot_name_service/core/aron_conversions.h
new file mode 100644
index 000000000..4019e276e
--- /dev/null
+++ b/source/RobotAPI/libraries/robot_name_service/core/aron_conversions.h
@@ -0,0 +1,14 @@
+#pragma once
+
+#include <map>
+#include <memory>
+#include <string>
+#include <vector>
+
+#include <RobotAPI/interface/robot_name_service/RobotNameServiceInterface.h>
+#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
+
+namespace armarx::robot_name_service::core
+{
+
+} // namespace armarx::robot_name_service::core
diff --git a/source/RobotAPI/libraries/robot_name_service/server/CMakeLists.txt b/source/RobotAPI/libraries/robot_name_service/server/CMakeLists.txt
index d0c268aa4..862934fff 100644
--- a/source/RobotAPI/libraries/robot_name_service/server/CMakeLists.txt
+++ b/source/RobotAPI/libraries/robot_name_service/server/CMakeLists.txt
@@ -18,8 +18,10 @@ armarx_add_library(
SOURCES
plugins.cpp
+ segments/RobotSegment.cpp
HEADERS
plugins.h
+ segments/RobotSegment.h
)
add_library(RobotAPI::robot_name_service_server ALIAS robot_name_service_server)
diff --git a/source/RobotAPI/libraries/robot_name_service/server/segments/RobotSegment.cpp b/source/RobotAPI/libraries/robot_name_service/server/segments/RobotSegment.cpp
new file mode 100644
index 000000000..9e67bbb25
--- /dev/null
+++ b/source/RobotAPI/libraries/robot_name_service/server/segments/RobotSegment.cpp
@@ -0,0 +1,6 @@
+#include "RobotSegment.h"
+
+namespace armarx::robot_name_service::server::segment
+{
+
+} // namespace armarx::robot_name_service::server::segment
diff --git a/source/RobotAPI/libraries/robot_name_service/server/segments/RobotSegment.h b/source/RobotAPI/libraries/robot_name_service/server/segments/RobotSegment.h
new file mode 100644
index 000000000..7e9d006f6
--- /dev/null
+++ b/source/RobotAPI/libraries/robot_name_service/server/segments/RobotSegment.h
@@ -0,0 +1,17 @@
+#pragma once
+
+#include <map>
+#include <memory>
+#include <string>
+#include <vector>
+
+#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
+#include <RobotAPI/libraries/armem/server/segment/SpecializedCoreSegment.h>
+
+namespace armarx::robot_name_service::server::segment
+{
+ class RobotInfoCoreSegment : public armarx::armem::server::segment::SpecializedCoreSegment
+ {
+ public:
+ };
+} // namespace armarx::robot_name_service::server::segment
--
GitLab