From 82dd3d75bf0650b163aa0730b567bfecbe394ce4 Mon Sep 17 00:00:00 2001
From: Rainer Kartmann <rainer.kartmann@kit.edu>
Date: Tue, 3 Aug 2021 18:52:15 +0200
Subject: [PATCH] Revise segment::Segment(). (put code and includes into cpp
etc...)
---
.../armem/server/segment/Segment.cpp | 108 ++++++++++++
.../libraries/armem/server/segment/Segment.h | 157 ++++++++----------
.../MotionDatabase/MDBMotions/Segment.h | 2 +-
.../server/description/Segment.cpp | 2 -
.../server/localization/Segment.cpp | 40 +++--
.../server/localization/Segment.h | 4 +-
.../server/proprioception/Segment.cpp | 2 -
7 files changed, 202 insertions(+), 113 deletions(-)
diff --git a/source/RobotAPI/libraries/armem/server/segment/Segment.cpp b/source/RobotAPI/libraries/armem/server/segment/Segment.cpp
index ec25ce784..050e77d39 100644
--- a/source/RobotAPI/libraries/armem/server/segment/Segment.cpp
+++ b/source/RobotAPI/libraries/armem/server/segment/Segment.cpp
@@ -1 +1,109 @@
#include "Segment.h"
+
+#include <ArmarXCore/core/application/properties/PluginAll.h>
+#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
+
+#include <RobotAPI/libraries/aron/core/navigator/type/container/Object.h>
+#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
+
+
+namespace armarx::armem::server::segment
+{
+
+ wm::CoreSegmentBase::CoreSegmentBase(
+ armem::server::MemoryToIceAdapter& iceMemory,
+ const std::string& defaultCoreSegmentName,
+ aron::typenavigator::ObjectNavigatorPtr coreSegmentAronType,
+ int defaultMaxHistorySize) :
+ Base(iceMemory),
+ p({defaultCoreSegmentName, defaultMaxHistorySize}),
+ coreSegmentAronType(coreSegmentAronType)
+ {
+ Logging::setTag("armarx::armem::wm::Segment");
+ }
+
+
+ wm::CoreSegmentBase::~CoreSegmentBase()
+ {
+ }
+
+
+ void wm::CoreSegmentBase::defineProperties(
+ armarx::PropertyDefinitionsPtr defs,
+ const std::string& prefix)
+ {
+ ARMARX_CHECK_NOT_NULL(defs);
+
+ defs->optional(p.coreSegmentName, prefix + "CoreSegmentName", "Name of the '" + p.coreSegmentName + "' core segment.");
+ defs->optional(p.maxHistorySize, prefix + "MaxHistorySize", "Maximal size of '" + p.coreSegmentName + "' history (-1 for infinite).");
+ }
+
+
+ void wm::CoreSegmentBase::onInit()
+ {
+ ARMARX_CHECK_NOT_NULL(iceMemory.workingMemory);
+
+ segment = &iceMemory.workingMemory->addCoreSegment(p.coreSegmentName, coreSegmentAronType);
+ segment->setMaxHistorySize(p.maxHistorySize);
+ }
+
+
+ std::mutex& wm::CoreSegmentBase::mutex() const
+ {
+ ARMARX_CHECK_NOT_NULL(segment);
+ return segment->mutex();
+ }
+
+
+
+ wm::ProviderSegmentBase::ProviderSegmentBase(
+ armem::server::MemoryToIceAdapter& iceMemory,
+ const std::string& defaultProviderSegmentName,
+ aron::typenavigator::ObjectNavigatorPtr providerSegmentAronType,
+ const std::string& defaultCoreSegmentName,
+ aron::typenavigator::ObjectNavigatorPtr coreSegmentAronType,
+ int defaultMaxHistorySize) :
+
+ Base(iceMemory),
+ p({defaultCoreSegmentName, defaultProviderSegmentName, defaultMaxHistorySize}),
+ coreSegmentAronType(coreSegmentAronType),
+ providerSegmentAronType(providerSegmentAronType)
+ {
+ Logging::setTag("armarx::armem::wm::Segment");
+ }
+
+
+ wm::ProviderSegmentBase::~ProviderSegmentBase()
+ {
+ }
+
+
+ void wm::ProviderSegmentBase::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
+ {
+ ARMARX_CHECK_NOT_NULL(defs);
+
+ defs->optional(p.coreSegmentName, prefix + "CoreSegmentName", "Name of the '" + p.coreSegmentName + "' core segment.");
+ defs->optional(p.providerSegmentName, prefix + "ProviderSegmentName", "Name of the '" + p.providerSegmentName + "' provider segment.");
+ defs->optional(p.maxHistorySize, prefix + "MaxHistorySize", "Maximal size of '" + p.providerSegmentName + "' history (-1 for infinite).");
+ }
+
+
+ void wm::ProviderSegmentBase::onInit()
+ {
+ ARMARX_CHECK_NOT_NULL(iceMemory.workingMemory);
+
+ if (!iceMemory.workingMemory->hasCoreSegment(p.coreSegmentName))
+ {
+ coreSegment = &iceMemory.workingMemory->addCoreSegment(p.coreSegmentName, coreSegmentAronType);
+ coreSegment->setMaxHistorySize(p.maxHistorySize);
+ }
+ else
+ {
+ coreSegment = &iceMemory.workingMemory->getCoreSegment(p.coreSegmentName);
+ }
+
+ segment = &coreSegment->addProviderSegment(p.providerSegmentName, providerSegmentAronType);
+ segment->setMaxHistorySize(p.maxHistorySize);
+ }
+
+}
diff --git a/source/RobotAPI/libraries/armem/server/segment/Segment.h b/source/RobotAPI/libraries/armem/server/segment/Segment.h
index 6cd9761f8..e59b63fc3 100644
--- a/source/RobotAPI/libraries/armem/server/segment/Segment.h
+++ b/source/RobotAPI/libraries/armem/server/segment/Segment.h
@@ -6,12 +6,25 @@
// ArmarX
#include <ArmarXCore/core/logging/Logging.h>
-#include <ArmarXCore/core/application/properties/PluginAll.h>
-#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
-
-#include "../MemoryToIceAdapter.h"
+#include <ArmarXCore/core/application/properties/forward_declarations.h>
+namespace armarx::aron::typenavigator
+{
+ using ObjectNavigatorPtr = std::shared_ptr<class ObjectNavigator>;
+}
+namespace armarx::armem
+{
+ namespace server
+ {
+ class MemoryToIceAdapter;
+ }
+ namespace wm
+ {
+ class CoreSegment;
+ class ProviderSegment;
+ }
+}
namespace armarx::armem::server::segment
{
namespace detail
@@ -25,8 +38,9 @@ namespace armarx::armem::server::segment
class SegmentBase : public armarx::Logging
{
public:
+
SegmentBase() = delete;
- SegmentBase(armem::server::MemoryToIceAdapter& iceMemory) :
+ SegmentBase(MemoryToIceAdapter& iceMemory) :
iceMemory(iceMemory)
{
Logging::setTag("armarx::armem::Segment");
@@ -36,18 +50,20 @@ namespace armarx::armem::server::segment
virtual void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "") = 0;
virtual void onInit() = 0;
- virtual void onConnect() = 0;
+ // virtual void onConnect() = 0;
+
protected:
+
// Memory connection
- armem::server::MemoryToIceAdapter& iceMemory;
+ MemoryToIceAdapter& iceMemory;
SegmentType* segment;
- private:
};
}
+
namespace wm
{
/**
@@ -58,47 +74,26 @@ namespace armarx::armem::server::segment
using Base = detail::SegmentBase<armarx::armem::wm::CoreSegment>;
public:
- CoreSegmentBase(armem::server::MemoryToIceAdapter& iceMemory,
- const std::string& defaultCoreSegmentName = "",
- aron::typenavigator::ObjectNavigatorPtr coreSegmentAronType = nullptr,
- int defaultMaxHistorySize = -1):
- Base(iceMemory),
- p({defaultCoreSegmentName, defaultMaxHistorySize}),
- coreSegmentAronType(coreSegmentAronType)
- {
- Logging::setTag("armarx::armem::wm::Segment");
- }
- virtual ~CoreSegmentBase() = default;
+ CoreSegmentBase(
+ MemoryToIceAdapter& iceMemory,
+ const std::string& defaultCoreSegmentName = "",
+ aron::typenavigator::ObjectNavigatorPtr coreSegmentAronType = nullptr,
+ int defaultMaxHistorySize = -1);
- virtual void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "") override
- {
- ARMARX_CHECK_NOT_NULL(defs);
+ virtual ~CoreSegmentBase() override;
- defs->optional(p.coreSegmentName, prefix + "CoreSegmentName", "Name of the '" + p.coreSegmentName + "' core segment.");
- defs->optional(p.maxHistorySize, prefix + "MaxHistorySize", "Maximal size of '" + p.coreSegmentName + "' history (-1 for infinite).");
- }
- virtual void onInit() override
- {
- ARMARX_CHECK_NOT_NULL(iceMemory.workingMemory);
-
- segment = &iceMemory.workingMemory->addCoreSegment(p.coreSegmentName, coreSegmentAronType);
- segment->setMaxHistorySize(p.maxHistorySize);
- }
+ virtual void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "") override;
+ virtual void onInit() override;
+ // virtual void onConnect() override;
- virtual void onConnect() override
- {
- }
+ std::mutex& mutex() const;
- std::mutex& mutex() const
- {
- ARMARX_CHECK_NOT_NULL(segment);
- return segment->mutex();
- }
protected:
+
struct Properties
{
std::string coreSegmentName;
@@ -107,6 +102,7 @@ namespace armarx::armem::server::segment
Properties p;
aron::typenavigator::ObjectNavigatorPtr coreSegmentAronType;
+
};
@@ -118,55 +114,25 @@ namespace armarx::armem::server::segment
using Base = detail::SegmentBase<armarx::armem::wm::ProviderSegment>;
public:
- ProviderSegmentBase(armem::server::MemoryToIceAdapter& iceMemory,
- const std::string& defaultProviderSegmentName = "",
- aron::typenavigator::ObjectNavigatorPtr providerSegmentAronType = nullptr,
- const std::string& defaultCoreSegmentName = "",
- aron::typenavigator::ObjectNavigatorPtr coreSegmentAronType = nullptr,
- int defaultMaxHistorySize = -1):
- Base(iceMemory),
- p({defaultCoreSegmentName, defaultProviderSegmentName, defaultMaxHistorySize}),
- coreSegmentAronType(coreSegmentAronType),
- providerSegmentAronType(providerSegmentAronType)
- {
- Logging::setTag("armarx::armem::wm::Segment");
- }
- virtual ~ProviderSegmentBase() = default;
+ ProviderSegmentBase(
+ MemoryToIceAdapter& iceMemory,
+ const std::string& defaultProviderSegmentName = "",
+ aron::typenavigator::ObjectNavigatorPtr providerSegmentAronType = nullptr,
+ const std::string& defaultCoreSegmentName = "",
+ aron::typenavigator::ObjectNavigatorPtr coreSegmentAronType = nullptr,
+ int defaultMaxHistorySize = -1);
- virtual void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "") override
- {
- ARMARX_CHECK_NOT_NULL(defs);
+ virtual ~ProviderSegmentBase() override;
- defs->optional(p.coreSegmentName, prefix + "CoreSegmentName", "Name of the '" + p.coreSegmentName + "' core segment.");
- defs->optional(p.providerSegmentName, prefix + "ProviderSegmentName", "Name of the '" + p.providerSegmentName + "' provider segment.");
- defs->optional(p.maxHistorySize, prefix + "MaxHistorySize", "Maximal size of '" + p.providerSegmentName + "' history (-1 for infinite).");
- }
- virtual void onInit() override
- {
- ARMARX_CHECK_NOT_NULL(iceMemory.workingMemory);
-
- if (!iceMemory.workingMemory->hasCoreSegment(p.coreSegmentName))
- {
- coreSegment = &iceMemory.workingMemory->addCoreSegment(p.coreSegmentName, coreSegmentAronType);
- coreSegment->setMaxHistorySize(p.maxHistorySize);
- }
- else
- {
- coreSegment = &iceMemory.workingMemory->getCoreSegment(p.coreSegmentName);
- }
-
- segment = &coreSegment->addProviderSegment(p.providerSegmentName, providerSegmentAronType);
- segment->setMaxHistorySize(p.maxHistorySize);
- }
+ virtual void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "") override;
+ virtual void onInit() override;
+ // virtual void onConnect() override;
- virtual void onConnect() override
- {
-
- }
protected:
+
struct Properties
{
std::string coreSegmentName;
@@ -182,6 +148,7 @@ namespace armarx::armem::server::segment
};
+
/**
* @brief A base class for provider segments with an aron business object
*/
@@ -191,10 +158,13 @@ namespace armarx::armem::server::segment
using Base = CoreSegmentBase;
public:
- AronTypedCoreSegmentBase(armem::server::MemoryToIceAdapter& iceMemory,
- const std::string& defaultCoreSegmentName = "",
- int defaultMaxHistorySize = -1):
- Base(iceMemory, defaultCoreSegmentName, BusinessClassObject::toAronType(), defaultMaxHistorySize)
+
+ AronTypedCoreSegmentBase(
+ MemoryToIceAdapter& iceMemory,
+ const std::string& defaultCoreSegmentName = "",
+ int defaultMaxHistorySize = -1) :
+ Base(iceMemory, defaultCoreSegmentName, BusinessClassObject::toAronType(),
+ defaultMaxHistorySize)
{
}
@@ -202,6 +172,7 @@ namespace armarx::armem::server::segment
};
+
/**
* @brief A base class for provider segments with an aron business object
*/
@@ -211,15 +182,19 @@ namespace armarx::armem::server::segment
using Base = ProviderSegmentBase;
public:
- AronTypedProviderSegmentBase(armem::server::MemoryToIceAdapter& iceMemory,
- const std::string& defaultProviderSegmentName = "",
- const std::string& defaultCoreSegmentName = "",
- int defaultMaxHistorySize = -1):
- Base(iceMemory, defaultProviderSegmentName, BusinessClassObject::toAronType(), defaultCoreSegmentName, nullptr, defaultMaxHistorySize)
+
+ AronTypedProviderSegmentBase(
+ MemoryToIceAdapter& iceMemory,
+ const std::string& defaultProviderSegmentName = "",
+ const std::string& defaultCoreSegmentName = "",
+ int defaultMaxHistorySize = -1) :
+ Base(iceMemory, defaultProviderSegmentName, BusinessClassObject::toAronType(),
+ defaultCoreSegmentName, nullptr, defaultMaxHistorySize)
{
}
virtual ~AronTypedProviderSegmentBase() = default;
+
};
}
}
diff --git a/source/RobotAPI/libraries/armem_motions/server/MotionDatabase/MDBMotions/Segment.h b/source/RobotAPI/libraries/armem_motions/server/MotionDatabase/MDBMotions/Segment.h
index 66e55f410..f0dc4d726 100644
--- a/source/RobotAPI/libraries/armem_motions/server/MotionDatabase/MDBMotions/Segment.h
+++ b/source/RobotAPI/libraries/armem_motions/server/MotionDatabase/MDBMotions/Segment.h
@@ -22,7 +22,7 @@ namespace armarx::armem::server::motions::mdb
virtual void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "") override;
virtual void onInit() override;
- virtual void onConnect() override;
+ virtual void onConnect();
private:
int loadByMotionFinder(const std::string&);
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 bd1e9c4fc..fc4eef76e 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.cpp
@@ -41,8 +41,6 @@ namespace armarx::armem::server::robot_state::description
void Segment::onConnect(const RobotUnitInterfacePrx& robotUnitPrx)
{
- Base::onConnect();
-
robotUnit = robotUnitPrx;
// store the robot description linked to the robot unit in this segment
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 ce82f0afa..604795dd4 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.cpp
@@ -48,6 +48,10 @@ namespace armarx::armem::server::robot_state::localization
Segment::~Segment() = default;
+ void Segment::onConnect()
+ {
+ }
+
Segment::RobotFramePoseMap
Segment::getRobotFramePosesLocking(const armem::Time& timestamp) const
@@ -64,14 +68,17 @@ namespace armarx::armem::server::robot_state::localization
using common::robot_state::localization::TransformQuery;
RobotFramePoseMap frames;
-
- const auto knownRobots = simox::alg::get_keys(*segment);
- for (const auto& robotName : knownRobots)
+ for (const auto& [robotName, _] : *segment)
{
- TransformQuery query{.header{.parentFrame = GlobalFrame,
- .frame = "root", // TODO, FIXME
- .agent = robotName,
- .timestamp = timestamp}};
+ TransformQuery query
+ {
+ .header{
+ .parentFrame = GlobalFrame,
+ .frame = "root", // TODO, FIXME
+ .agent = robotName,
+ .timestamp = timestamp
+ }
+ };
const auto result = TransformHelper::lookupTransformChain(*segment, query);
if (not result)
@@ -105,16 +112,17 @@ namespace armarx::armem::server::robot_state::localization
using common::robot_state::localization::TransformQuery;
RobotPoseMap robotGlobalPoses;
-
- const std::vector<std::string> knownRobots = simox::alg::get_keys(*segment);
-
- for (const std::string& robotName : knownRobots)
+ for (const auto& [robotName, _] : *segment)
{
-
- TransformQuery query{.header{.parentFrame = GlobalFrame,
- .frame = "root", // TODO, FIXME
- .agent = robotName,
- .timestamp = timestamp}};
+ TransformQuery query
+ {
+ .header{
+ .parentFrame = GlobalFrame,
+ .frame = "root", // TODO, FIXME
+ .agent = robotName,
+ .timestamp = timestamp
+ }
+ };
const auto result = TransformHelper::lookupTransform(*segment, query);
if (not result)
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 53d112362..31fffb420 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.h
+++ b/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.h
@@ -41,7 +41,7 @@
namespace armarx::armem
{
- class EntityUpdate;
+ struct EntityUpdate;
} // namespace armarx::armem
namespace armarx::armem::server::robot_state::localization
@@ -61,6 +61,8 @@ namespace armarx::armem::server::robot_state::localization
Segment(server::MemoryToIceAdapter& iceMemory);
virtual ~Segment() override;
+ void onConnect();
+
RobotPoseMap getRobotGlobalPoses(const armem::Time& timestamp) const;
RobotPoseMap getRobotGlobalPosesLocking(const armem::Time& timestamp) const;
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 9f2fadcf5..e94fc83e2 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.cpp
@@ -40,8 +40,6 @@ namespace armarx::armem::server::robot_state::proprioception
void Segment::onConnect(RobotUnitInterfacePrx robotUnitPrx)
{
- Base::onConnect();
-
this->robotUnit = robotUnitPrx;
std::string providerSegmentName = "Robot";
--
GitLab