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 f84c223c292d85adbb7e7db190e471b7210e58d8..d6dcddd7d3fd568216a63657bfe6ae5ca1f10f9c 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.cpp
@@ -1,7 +1,8 @@
#include "Visu.h"
-#include <Eigen/src/Geometry/Transform.h>
#include <algorithm>
+#include <exception>
+#include <string>
#include <Eigen/Geometry>
@@ -17,8 +18,6 @@
#include <RobotAPI/components/ArViz/Client/Elements.h>
#include <RobotAPI/libraries/armem/core/Time.h>
#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
-#include <exception>
-#include <string>
#include "../description/Segment.h"
#include "../localization/Segment.h"
@@ -28,6 +27,13 @@
namespace armarx::armem::server::robot_state
{
+ Visu::Visu(const description::Segment& descriptionSegment, const proprioception::Segment& proprioceptionSegment, const localization::Segment& localizationSegment)
+ : descriptionSegment(descriptionSegment),
+ proprioceptionSegment(proprioceptionSegment),
+ localizationSegment(localizationSegment)
+ {}
+
+
void Visu::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
{
defs->optional(
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 5af7ee0ac9bb01fa394430d9a07ec70da4d25b50..95efae41963c63669e4e4bb11b202ba9ef948ec9 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.h
+++ b/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.h
@@ -63,11 +63,7 @@ namespace armarx::armem::server::robot_state
Visu(const description::Segment& descriptionSegment,
const proprioception::Segment& proprioceptionSegment,
- const localization::Segment& localizationSegment)
- : descriptionSegment(descriptionSegment),
- proprioceptionSegment(proprioceptionSegment),
- localizationSegment(localizationSegment)
- {}
+ const localization::Segment& localizationSegment);
void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "visu.");
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.cpp b/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.cpp
index 03488c3d2a66cfe33e60b9d4d6677ae81071e928..35a7ca578c0bac7ecbaa70fe5cf7f9ee8a8aff02 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.cpp
@@ -7,7 +7,6 @@
#include <ArmarXCore/core/application/properties/PluginAll.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
#include <ArmarXCore/core/time/TimeUtil.h>
@@ -33,35 +32,14 @@ namespace armarx::armem::server::robot_state::description
{
Segment::Segment(armem::server::MemoryToIceAdapter& memoryToIceAdapter) :
- iceMemory(memoryToIceAdapter)
+ SpecializedSegment(memoryToIceAdapter, arondto::RobotDescription::toAronType(),
+ "Description")
{
- Logging::setTag("DescriptionSegment");
}
Segment::~Segment() = default;
- void Segment::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
- {
- defs->optional(p.coreSegment,
- prefix + "seg.description.CoreSegment",
- "Name of the robot description core segment.");
- defs->optional(p.maxHistorySize,
- prefix + "seg.description.MaxHistorySize",
- "Maximal size of object poses history (-1 for infinite).");
- }
-
-
- void Segment::init()
- {
- ARMARX_CHECK_NOT_NULL(iceMemory.workingMemory);
-
- ARMARX_INFO << "Adding core segment '" << p.coreSegment << "'";
- coreSegment = &iceMemory.workingMemory->addCoreSegment(p.coreSegment, arondto::RobotDescription::toAronType());
- coreSegment->setMaxHistorySize(p.maxHistorySize);
- }
-
-
void Segment::connect(const RobotUnitInterfacePrx& robotUnitPrx)
{
robotUnit = robotUnitPrx;
@@ -71,13 +49,6 @@ namespace armarx::armem::server::robot_state::description
}
- std::mutex& Segment::mutex() const
- {
- ARMARX_CHECK_NOT_NULL(coreSegment);
- return coreSegment->mutex();
- }
-
-
void Segment::commitRobotDescription(const robot::RobotDescription& robotDescription)
{
const Time now = TimeUtil::GetTime();
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.h b/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.h
index 2a51250f769905ea7e71c555752ef6d916d7eb64..d020b5543e2c2f59ff593fc70c753640679b5a2b 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.h
+++ b/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.h
@@ -26,42 +26,27 @@
#include <mutex>
#include <unordered_map>
-#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
#include <RobotAPI/libraries/armem/core/MemoryID.h>
+#include <RobotAPI/libraries/armem/server/segment/SpecializedSegment.h>
#include <RobotAPI/libraries/armem_objects/types.h>
-namespace armarx::armem
-{
- namespace server
- {
- class MemoryToIceAdapter;
- }
-
- namespace wm
- {
- class CoreSegment;
- }
-} // namespace armarx::armem
-
namespace armarx::armem::server::robot_state::description
{
- class Segment : public armarx::Logging
+ class Segment : public segment::SpecializedSegment
{
public:
Segment(server::MemoryToIceAdapter& iceMemory);
- virtual ~Segment();
+ virtual ~Segment() override;
+
- void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "");
- void init();
void connect(const RobotUnitInterfacePrx& robotUnitPrx);
- std::mutex& mutex() const;
/// mapping "robot name" -> "robot description"
using RobotDescriptionMap = std::unordered_map<std::string, robot::RobotDescription>;
@@ -75,18 +60,8 @@ namespace armarx::armem::server::robot_state::description
void commitRobotDescription(const robot::RobotDescription& robotDescription);
void updateRobotDescription();
- server::MemoryToIceAdapter& iceMemory;
- wm::CoreSegment* coreSegment = nullptr;
-
RobotUnitInterfacePrx robotUnit;
- struct Properties
- {
- std::string coreSegment = "Description";
- int64_t maxHistorySize = -1;
- };
- Properties p;
-
};
} // namespace armarx::armem::server::robot_state::description
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.cpp b/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.cpp
index 75e2e9481025981a20f5764e6cf4cd2931a473ce..dd01313495ac922287f36549342feb2575c7d053 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.cpp
@@ -42,48 +42,20 @@ namespace armarx::armem::server::robot_state::localization
{
Segment::Segment(armem::server::MemoryToIceAdapter& memoryToIceAdapter) :
- iceMemory(memoryToIceAdapter)
+ SpecializedSegment(memoryToIceAdapter, arondto::Transform::toAronType(),
+ "Localization", 1024)
{
- Logging::setTag("LocalizationSegment");
}
Segment::~Segment() = default;
- void Segment::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
- {
- defs->optional(p.coreSegment,
- prefix + "seg.localization.CoreSegment",
- "Name of the object instance core segment.");
- defs->optional(p.maxHistorySize,
- prefix + "seg.localization.MaxHistorySize",
- "Maximal size of object poses history (-1 for infinite).");
- }
-
-
- void Segment::init()
- {
- ARMARX_CHECK_NOT_NULL(iceMemory.workingMemory);
-
- ARMARX_INFO << "Adding core segment '" << p.coreSegment << "'";
- coreSegment = &iceMemory.workingMemory->addCoreSegment(p.coreSegment, arondto::Transform::toAronType());
- coreSegment->setMaxHistorySize(p.maxHistorySize);
- }
-
-
void Segment::connect()
{
}
- std::mutex& Segment::mutex() const
- {
- ARMARX_CHECK_NOT_NULL(coreSegment);
- return coreSegment->mutex();
- }
-
-
Segment::RobotFramePoseMap
Segment::getRobotFramePosesLocking(const armem::Time& timestamp) const
{
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.h b/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.h
index d2f8eef82a2cb6b2a9530bc0d646cef5653e8f21..d573c37ec8c90fc25a4de80eed877bc32f825dcd 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.h
+++ b/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.h
@@ -23,13 +23,12 @@
#include <string>
#include <optional>
-#include <mutex>
#include <unordered_map>
-#include <ArmarXCore/core/logging/Logging.h>
-#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
+#include <Eigen/Geometry>
#include <RobotAPI/libraries/armem/core/MemoryID.h>
+#include <RobotAPI/libraries/armem/server/segment/SpecializedSegment.h>
#include <RobotAPI/libraries/armem_objects/types.h>
#include <RobotAPI/libraries/armem_robot_state/types.h>
@@ -37,21 +36,11 @@
namespace armarx::armem
{
class EntityUpdate;
- namespace server
- {
- class MemoryToIceAdapter;
- }
-
- namespace wm
- {
- class CoreSegment;
- }
} // namespace armarx::armem
-
namespace armarx::armem::server::robot_state::localization
{
- class Segment : public armarx::Logging
+ class Segment : public segment::SpecializedSegment
{
public:
@@ -62,13 +51,10 @@ namespace armarx::armem::server::robot_state::localization
public:
Segment(server::MemoryToIceAdapter& iceMemory);
- virtual ~Segment();
+ virtual ~Segment() override;
- void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "");
- void init();
- void connect();
- std::mutex& mutex() const;
+ void connect();
RobotPoseMap getRobotGlobalPoses(const armem::Time& timestamp) const;
RobotPoseMap getRobotGlobalPosesLocking(const armem::Time& timestamp) const;
@@ -85,18 +71,6 @@ namespace armarx::armem::server::robot_state::localization
EntityUpdate makeUpdate(const armarx::armem::robot_state::Transform& transform) const;
- private:
-
- server::MemoryToIceAdapter& iceMemory;
- wm::CoreSegment* coreSegment = nullptr;
-
- struct Properties
- {
- std::string coreSegment = "Localization";
- int64_t maxHistorySize = -1;
- };
- Properties p;
-
};
} // namespace armarx::armem::server::robot_state::localization
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 c84460fc71e43a4234365b65fb0e1649d22ffdcb..821f1019123e7eb814e8313e2a7d94a9091c73dd 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.cpp
@@ -31,31 +31,14 @@ namespace armarx::armem::server::robot_state::proprioception
{
Segment::Segment(armem::server::MemoryToIceAdapter& memoryToIceAdapter) :
- iceMemory(memoryToIceAdapter)
+ SpecializedSegment(memoryToIceAdapter, nullptr,
+ "Proprioception", 1024)
{
- Logging::setTag("ProprioceptionSegment");
}
Segment::~Segment() = default;
- void Segment::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
- {
- defs->optional(p.coreSegmentName, prefix + "seg.proprioception.CoreSegment", "Name of the proprioception core segment.");
- defs->optional(p.maxHistorySize, prefix + "seg.proprioception.MaxHistorySize", "Maximal size of proprioception entity histories (-1 for infinite).");
- }
-
-
- void Segment::init()
- {
- ARMARX_CHECK_NOT_NULL(iceMemory.workingMemory);
-
- ARMARX_INFO << "Adding core segment '" << p.coreSegmentName << "'";
- coreSegment = &iceMemory.workingMemory->addCoreSegment(p.coreSegmentName);
- coreSegment->setMaxHistorySize(p.maxHistorySize);
- }
-
-
void Segment::connect(RobotUnitInterfacePrx robotUnitPrx)
{
this->robotUnit = robotUnitPrx;
@@ -75,12 +58,6 @@ namespace armarx::armem::server::robot_state::proprioception
}
- std::mutex& Segment::mutex() const
- {
- ARMARX_CHECK_NOT_NULL(coreSegment);
- return coreSegment->mutex();
- }
-
Segment::RobotJointPositionMap Segment::getRobotJointPositionsLocking(
const armem::Time& timestamp,
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 f6c49f55e28226d90110f610c990f0b7208bdff7..87ef6a4e2b7937f3196a2ee0ce4610b22fd1e1cb 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.h
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.h
@@ -23,15 +23,13 @@
#include <string>
#include <optional>
-#include <mutex>
#include <unordered_map>
-#include <ArmarXCore/core/logging/Logging.h>
-#include <ArmarXCore/core/application/properties/forward_declarations.h>
-
#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
+
#include <RobotAPI/components/ArViz/Client/Client.h>
#include <RobotAPI/libraries/armem/core/MemoryID.h>
+#include <RobotAPI/libraries/armem/server/segment/SpecializedSegment.h>
#include <RobotAPI/libraries/armem_objects/types.h>
@@ -39,23 +37,9 @@ namespace armarx
{
class DebugObserverHelper;
}
-namespace armarx::armem
-{
- namespace server
- {
- class MemoryToIceAdapter;
- }
-
- namespace wm
- {
- class CoreSegment;
- }
-} // namespace armarx::armem
-
-
namespace armarx::armem::server::robot_state::proprioception
{
- class Segment : public armarx::Logging
+ class Segment : public segment::SpecializedSegment
{
public:
@@ -64,14 +48,10 @@ namespace armarx::armem::server::robot_state::proprioception
public:
Segment(server::MemoryToIceAdapter& iceMemory);
- virtual ~Segment();
+ virtual ~Segment() override;
- void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "");
- void init();
void connect(RobotUnitInterfacePrx robotUnitPrx);
- std::mutex& mutex() const;
-
RobotJointPositionMap getRobotJointPositions(
const armem::Time& timestamp, DebugObserverHelper* debugObserver = nullptr) const;
@@ -83,21 +63,12 @@ namespace armarx::armem::server::robot_state::proprioception
private:
- server::MemoryToIceAdapter& iceMemory;
- wm::CoreSegment* coreSegment = nullptr;
-
RobotUnitInterfacePrx robotUnit;
armem::MemoryID robotUnitProviderID;
- struct Properties
- {
- std::string coreSegmentName = "Proprioception";
- int64_t maxHistorySize = 1000;
- };
- Properties p;
-
// Debug Observer prefix
const std::string dp = "Proprioception::getRobotJointPositions() | ";
+
};
} // namespace armarx::armem::server::robot_state::proprioception